forked from bartvdbraak/blender
added more debug text, enabled the bullet penalty solver, instead of ode solver by default, added a better demo.
This commit is contained in:
parent
5afdfc6ac1
commit
5ebc7c8bda
21
extern/bullet/Bullet/Bullet3_vc8.vcproj
vendored
21
extern/bullet/Bullet/Bullet3_vc8.vcproj
vendored
@ -4,7 +4,6 @@
|
||||
Version="8.00"
|
||||
Name="Bullet3ContinuousCollision"
|
||||
ProjectGUID="{FFD3C64A-30E2-4BC7-BC8F-51818C320400}"
|
||||
SignManifests="true"
|
||||
>
|
||||
<Platforms>
|
||||
<Platform
|
||||
@ -277,14 +276,6 @@
|
||||
RelativePath=".\NarrowPhaseCollision\ManifoldPoint.h"
|
||||
>
|
||||
</File>
|
||||
<File
|
||||
RelativePath=".\NarrowPhaseCollision\ManifoldPointCollector.cpp"
|
||||
>
|
||||
</File>
|
||||
<File
|
||||
RelativePath=".\NarrowPhaseCollision\ManifoldPointCollector.h"
|
||||
>
|
||||
</File>
|
||||
<File
|
||||
RelativePath=".\NarrowPhaseCollision\MinkowskiPenetrationDepthSolver.cpp"
|
||||
>
|
||||
@ -437,6 +428,14 @@
|
||||
RelativePath=".\CollisionShapes\TriangleCallback.h"
|
||||
>
|
||||
</File>
|
||||
<File
|
||||
RelativePath=".\CollisionShapes\TriangleMesh.cpp"
|
||||
>
|
||||
</File>
|
||||
<File
|
||||
RelativePath=".\CollisionShapes\TriangleMesh.h"
|
||||
>
|
||||
</File>
|
||||
<File
|
||||
RelativePath=".\CollisionShapes\TriangleMeshShape.cpp"
|
||||
>
|
||||
@ -509,6 +508,10 @@
|
||||
RelativePath="..\LinearMath\GEN_random.h"
|
||||
>
|
||||
</File>
|
||||
<File
|
||||
RelativePath="..\LinearMath\IDebugDraw.h"
|
||||
>
|
||||
</File>
|
||||
<File
|
||||
RelativePath="..\LinearMath\SimdMatrix3x3.h"
|
||||
>
|
||||
|
@ -29,10 +29,6 @@ void BoxShape::GetAabb(const SimdTransform& t,SimdVector3& aabbMin,SimdVector3&
|
||||
abs_b[2].dot(halfExtents));
|
||||
extent += SimdVector3(GetMargin(),GetMargin(),GetMargin());
|
||||
|
||||
|
||||
//todo: this is a quick fix, we need to enlarge the aabb dependent on several criteria
|
||||
extent += SimdVector3(.2f,.2f,.2f);
|
||||
|
||||
aabbMin = center - extent;
|
||||
aabbMax = center + extent;
|
||||
|
||||
|
@ -19,7 +19,8 @@ class ManifoldPoint
|
||||
m_localPointB( pointB ),
|
||||
m_normalWorldOnB( normal ),
|
||||
m_distance1( distance )
|
||||
,m_appliedImpulse(0.f)
|
||||
,m_appliedImpulse(0.f),
|
||||
m_lifeTime(0)
|
||||
{}
|
||||
|
||||
SimdVector3 m_localPointA;
|
||||
@ -31,11 +32,16 @@ class ManifoldPoint
|
||||
float m_distance1;
|
||||
/// total applied impulse during most recent frame
|
||||
float m_appliedImpulse;
|
||||
int m_lifeTime;//lifetime of the contactpoint in frames
|
||||
|
||||
float GetDistance() const
|
||||
{
|
||||
return m_distance1;
|
||||
}
|
||||
int GetLifeTime() const
|
||||
{
|
||||
return m_lifeTime;
|
||||
}
|
||||
|
||||
SimdVector3 GetPositionWorldOnA() {
|
||||
return m_positionWorldOnA;
|
||||
|
@ -8,15 +8,21 @@
|
||||
struct MyResult : public DiscreteCollisionDetectorInterface::Result
|
||||
{
|
||||
|
||||
MyResult():m_hasResult(false)
|
||||
{
|
||||
}
|
||||
|
||||
SimdVector3 m_normalOnBInWorld;
|
||||
SimdVector3 m_pointInWorld;
|
||||
float m_depth;
|
||||
bool m_hasResult;
|
||||
|
||||
void AddContactPoint(const SimdVector3& normalOnBInWorld,const SimdVector3& pointInWorld,float depth)
|
||||
{
|
||||
m_normalOnBInWorld = normalOnBInWorld;
|
||||
m_pointInWorld = pointInWorld;
|
||||
m_depth = depth;
|
||||
m_hasResult = true;
|
||||
}
|
||||
};
|
||||
|
||||
@ -37,6 +43,7 @@ bool MinkowskiPenetrationDepthSolver::CalcPenDepth(SimplexSolverInterface& simpl
|
||||
SimdVector3 minVertex;
|
||||
SimdVector3 minA,minB;
|
||||
|
||||
//not so good, lots of directions overlap, better to use gauss map
|
||||
for (int i=-N;i<N;i++)
|
||||
{
|
||||
for (int j = -N;j<N;j++)
|
||||
@ -122,11 +129,10 @@ bool MinkowskiPenetrationDepthSolver::CalcPenDepth(SimplexSolverInterface& simpl
|
||||
MyResult res;
|
||||
gjkdet.GetClosestPoints(input,res);
|
||||
|
||||
SimdVector3 halfV = v*0.5f;
|
||||
|
||||
//approximate pa and pb
|
||||
pa = res.m_pointInWorld - halfV;
|
||||
pb = res.m_pointInWorld +halfV;
|
||||
|
||||
return true;
|
||||
if (res.m_hasResult)
|
||||
{
|
||||
pa = res.m_pointInWorld - res.m_normalOnBInWorld*res.m_depth;
|
||||
pb = res.m_pointInWorld;
|
||||
}
|
||||
return res.m_hasResult;
|
||||
}
|
||||
|
@ -14,7 +14,7 @@
|
||||
#include "SimdTransform.h"
|
||||
#include <assert.h>
|
||||
|
||||
float gContactBreakingTreshold = 0.002f;
|
||||
float gContactBreakingTreshold = 0.02f;
|
||||
|
||||
PersistentManifold::PersistentManifold()
|
||||
:m_body0(0),
|
||||
@ -128,6 +128,7 @@ void PersistentManifold::RefreshContactPoints(const SimdTransform& trA,const Sim
|
||||
manifoldPoint.m_positionWorldOnA = trA( manifoldPoint.m_localPointA );
|
||||
manifoldPoint.m_positionWorldOnB = trB( manifoldPoint.m_localPointB );
|
||||
manifoldPoint.m_distance1 = (manifoldPoint.m_positionWorldOnA - manifoldPoint.m_positionWorldOnB).dot(manifoldPoint.m_normalWorldOnB);
|
||||
manifoldPoint.m_lifeTime++;
|
||||
}
|
||||
|
||||
/// then
|
||||
|
@ -101,7 +101,7 @@ public:
|
||||
|
||||
bool ValidContactDistance(const ManifoldPoint& pt) const
|
||||
{
|
||||
return pt.m_distance1 < GetManifoldMargin();
|
||||
return pt.m_distance1 <= GetManifoldMargin();
|
||||
}
|
||||
/// calculated new worldspace coordinates and depth, and reject points that exceed the collision margin
|
||||
void RefreshContactPoints( const SimdTransform& trA,const SimdTransform& trB);
|
||||
|
@ -36,8 +36,12 @@
|
||||
|
||||
///Solid3EpaPenetrationDepth is not shipped by default, the license doesn't allow commercial, closed source. contact if you want the file
|
||||
///It improves the penetration depth handling dramatically
|
||||
//#define USE_EPA
|
||||
#ifdef USE_EPA
|
||||
#include "NarrowPhaseCollision/Solid3EpaPenetrationDepth.h"
|
||||
bool gUseEpa = true;
|
||||
#else
|
||||
bool gUseEpa = false;
|
||||
#endif// USE_EPA
|
||||
|
||||
#ifdef WIN32
|
||||
@ -57,7 +61,6 @@ bool gForceBoxBox = false;//false;//true;
|
||||
bool gBoxBoxUseGjk = true;//true;//false;
|
||||
bool gDisableConvexCollision = false;
|
||||
|
||||
bool gUseEpa = false;
|
||||
|
||||
|
||||
ConvexConvexAlgorithm::ConvexConvexAlgorithm(PersistentManifold* mf,const CollisionAlgorithmConstructionInfo& ci,BroadphaseProxy* proxy0,BroadphaseProxy* proxy1)
|
||||
@ -144,7 +147,10 @@ void ConvexConvexAlgorithm::CheckPenetrationDepthSolver()
|
||||
//not distributed, see top of this file
|
||||
#ifdef USE_EPA
|
||||
m_gjkPairDetector.SetPenetrationDepthSolver(new Solid3EpaPenetrationDepth);
|
||||
#else
|
||||
m_gjkPairDetector.SetPenetrationDepthSolver(new MinkowskiPenetrationDepthSolver);
|
||||
#endif
|
||||
|
||||
} else
|
||||
{
|
||||
m_gjkPairDetector.SetPenetrationDepthSolver(new MinkowskiPenetrationDepthSolver);
|
||||
|
@ -7,7 +7,6 @@
|
||||
#include "CollisionDispatch/EmptyCollisionAlgorithm.h"
|
||||
#include "CollisionDispatch/ConvexConcaveCollisionAlgorithm.h"
|
||||
|
||||
#define MAX_RIGIDBODIES 128
|
||||
|
||||
|
||||
void ToiContactDispatcher::FindUnions()
|
||||
|
@ -17,23 +17,20 @@
|
||||
|
||||
#define ASSERT2 assert
|
||||
|
||||
|
||||
//some values to find stable penalty method tresholds
|
||||
float MAX_FRICTION = 100.f;
|
||||
static SimdScalar ContactThreshold = -10.0f;
|
||||
|
||||
float useGlobalSettingContacts = false;//true;
|
||||
|
||||
SimdScalar contactDamping = 0.2f;
|
||||
SimdScalar contactTau = .02f;//0.02f;//*0.02f;
|
||||
|
||||
|
||||
|
||||
SimdScalar restitutionCurve(SimdScalar rel_vel, SimdScalar restitution)
|
||||
{
|
||||
return 0.f;
|
||||
// return restitution * GEN_min(1.0f, rel_vel / ContactThreshold);
|
||||
}
|
||||
|
||||
float MAX_FRICTION = 100.f;
|
||||
|
||||
SimdScalar calculateCombinedFriction(RigidBody& body0,RigidBody& body1)
|
||||
{
|
||||
@ -46,54 +43,11 @@ SimdScalar calculateCombinedFriction(RigidBody& body0,RigidBody& body1)
|
||||
|
||||
}
|
||||
|
||||
void applyFrictionInContactPointOld(RigidBody& body1, const SimdVector3& pos1,
|
||||
RigidBody& body2, const SimdVector3& pos2,
|
||||
const SimdVector3& normal,float normalImpulse,
|
||||
const ContactSolverInfo& solverInfo)
|
||||
{
|
||||
|
||||
|
||||
if (normalImpulse>0.f)
|
||||
{
|
||||
SimdVector3 rel_pos1 = pos1 - body1.getCenterOfMassPosition();
|
||||
SimdVector3 rel_pos2 = pos2 - body2.getCenterOfMassPosition();
|
||||
|
||||
SimdVector3 vel1 = body1.getVelocityInLocalPoint(rel_pos1);
|
||||
SimdVector3 vel2 = body2.getVelocityInLocalPoint(rel_pos2);
|
||||
SimdVector3 vel = vel1 - vel2;
|
||||
|
||||
SimdScalar rel_vel = normal.dot(vel);
|
||||
|
||||
float combinedFriction = calculateCombinedFriction(body1,body2);
|
||||
|
||||
#define PER_CONTACT_FRICTION
|
||||
#ifdef PER_CONTACT_FRICTION
|
||||
SimdVector3 lat_vel = vel - normal * rel_vel;
|
||||
SimdScalar lat_rel_vel = lat_vel.length();
|
||||
|
||||
if (lat_rel_vel > SIMD_EPSILON)
|
||||
{
|
||||
lat_vel /= lat_rel_vel;
|
||||
SimdVector3 temp1 = body1.getInvInertiaTensorWorld() * rel_pos1.cross(lat_vel);
|
||||
SimdVector3 temp2 = body2.getInvInertiaTensorWorld() * rel_pos2.cross(lat_vel);
|
||||
SimdScalar frictionMaxImpulse = lat_rel_vel /
|
||||
(body1.getInvMass() + body2.getInvMass() + lat_vel.dot(temp1.cross(rel_pos1) + temp2.cross(rel_pos2)));
|
||||
SimdScalar frictionImpulse = (normalImpulse) * combinedFriction;
|
||||
GEN_set_min(frictionImpulse,frictionMaxImpulse );
|
||||
|
||||
body1.applyImpulse(lat_vel * -frictionImpulse, rel_pos1);
|
||||
body2.applyImpulse(lat_vel * frictionImpulse, rel_pos2);
|
||||
|
||||
}
|
||||
#endif
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
//bilateral constraint between two dynamic objects
|
||||
void resolveSingleBilateral(RigidBody& body1, const SimdVector3& pos1,
|
||||
RigidBody& body2, const SimdVector3& pos2,
|
||||
SimdScalar depth, const SimdVector3& normal,SimdScalar& impulse ,float timeStep)
|
||||
SimdScalar distance, const SimdVector3& normal,SimdScalar& impulse ,float timeStep)
|
||||
{
|
||||
float normalLenSqr = normal.length2();
|
||||
ASSERT2(fabs(normalLenSqr) < 1.1f);
|
||||
@ -110,10 +64,7 @@ void resolveSingleBilateral(RigidBody& body1, const SimdVector3& pos1,
|
||||
SimdVector3 vel2 = body2.getVelocityInLocalPoint(rel_pos2);
|
||||
SimdVector3 vel = vel1 - vel2;
|
||||
|
||||
SimdScalar rel_vel;
|
||||
|
||||
/*
|
||||
|
||||
JacobianEntry jac(body1.getCenterOfMassTransform().getBasis().transpose(),
|
||||
body2.getCenterOfMassTransform().getBasis().transpose(),
|
||||
rel_pos1,rel_pos2,normal,body1.getInvInertiaDiagLocal(),body1.getInvMass(),
|
||||
@ -130,100 +81,22 @@ void resolveSingleBilateral(RigidBody& body1, const SimdVector3& pos1,
|
||||
float a;
|
||||
a=jacDiagABInv;
|
||||
|
||||
*/
|
||||
|
||||
rel_vel = normal.dot(vel);
|
||||
|
||||
|
||||
|
||||
|
||||
/*int color = 255+255*256;
|
||||
|
||||
DrawRasterizerLine(pos1,pos1+normal,color);
|
||||
*/
|
||||
|
||||
|
||||
|
||||
#ifdef ONLY_USE_LINEAR_MASS
|
||||
SimdScalar massTerm = 1.f / (body1.getInvMass() + body2.getInvMass());
|
||||
|
||||
impulse = - contactDamping * rel_vel * massTerm;//jacDiagABInv;
|
||||
|
||||
//SimdScalar velocityImpulse = -contactDamping * rel_vel * jacDiagABInv;
|
||||
//impulse = velocityImpulse;
|
||||
|
||||
impulse = - contactDamping * rel_vel * massTerm;
|
||||
#else
|
||||
SimdScalar velocityImpulse = -contactDamping * rel_vel * jacDiagABInv;
|
||||
impulse = velocityImpulse;
|
||||
#endif
|
||||
}
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
//velocity + friction
|
||||
//response between two dynamic objects with friction
|
||||
float resolveSingleCollision(RigidBody& body1, const SimdVector3& pos1,
|
||||
RigidBody& body2, const SimdVector3& pos2,
|
||||
SimdScalar depth, const SimdVector3& normal,
|
||||
const ContactSolverInfo& solverInfo
|
||||
)
|
||||
{
|
||||
|
||||
float normalLenSqr = normal.length2();
|
||||
ASSERT2(fabs(normalLenSqr) < 1.1f);
|
||||
if (normalLenSqr > 1.1f)
|
||||
return 0.f;
|
||||
|
||||
SimdVector3 rel_pos1 = pos1 - body1.getCenterOfMassPosition();
|
||||
SimdVector3 rel_pos2 = pos2 - body2.getCenterOfMassPosition();
|
||||
//this jacobian entry could be re-used for all iterations
|
||||
|
||||
SimdVector3 vel1 = body1.getVelocityInLocalPoint(rel_pos1);
|
||||
SimdVector3 vel2 = body2.getVelocityInLocalPoint(rel_pos2);
|
||||
SimdVector3 vel = vel1 - vel2;
|
||||
SimdScalar rel_vel;
|
||||
rel_vel = normal.dot(vel);
|
||||
|
||||
// if (rel_vel< 0.f)//-SIMD_EPSILON)
|
||||
// {
|
||||
float combinedRestitution = body1.getRestitution() * body2.getRestitution();
|
||||
|
||||
SimdScalar rest = restitutionCurve(rel_vel, combinedRestitution);
|
||||
|
||||
SimdScalar timeCorrection = 360.f*solverInfo.m_timeStep;
|
||||
float damping = solverInfo.m_damping ;
|
||||
float tau = solverInfo.m_tau;
|
||||
|
||||
if (useGlobalSettingContacts)
|
||||
{
|
||||
damping = contactDamping;
|
||||
tau = contactTau;
|
||||
}
|
||||
|
||||
if (depth < 0.f)
|
||||
return 0.f;//bdepth = 0.f;
|
||||
|
||||
SimdScalar penetrationImpulse = (depth*tau*timeCorrection);// * massTerm;//jacDiagABInv
|
||||
|
||||
SimdScalar velocityImpulse = -(1.0f + rest) * damping * rel_vel;
|
||||
|
||||
SimdScalar impulse = penetrationImpulse + velocityImpulse;
|
||||
SimdVector3 temp1 = body1.getInvInertiaTensorWorld() * rel_pos1.cross(normal);
|
||||
SimdVector3 temp2 = body2.getInvInertiaTensorWorld() * rel_pos2.cross(normal);
|
||||
impulse /=
|
||||
(body1.getInvMass() + body2.getInvMass() + normal.dot(temp1.cross(rel_pos1) + temp2.cross(rel_pos2)));
|
||||
|
||||
if (impulse > 0.f)
|
||||
{
|
||||
|
||||
body1.applyImpulse(normal*(impulse), rel_pos1);
|
||||
body2.applyImpulse(-normal*(impulse), rel_pos2);
|
||||
} else
|
||||
{
|
||||
impulse = 0.f;
|
||||
}
|
||||
|
||||
return impulse;//velocityImpulse;//impulse;
|
||||
|
||||
}
|
||||
|
||||
|
||||
|
||||
//velocity + friction
|
||||
//response between two dynamic objects with friction
|
||||
float resolveSingleCollisionWithFriction(
|
||||
@ -232,7 +105,7 @@ float resolveSingleCollisionWithFriction(
|
||||
const SimdVector3& pos1,
|
||||
RigidBody& body2,
|
||||
const SimdVector3& pos2,
|
||||
SimdScalar depth,
|
||||
SimdScalar distance,
|
||||
const SimdVector3& normal,
|
||||
|
||||
const ContactSolverInfo& solverInfo
|
||||
@ -283,22 +156,17 @@ SimdScalar rel_vel;
|
||||
damping = contactDamping;
|
||||
tau = contactTau;
|
||||
}
|
||||
SimdScalar penetrationImpulse = (depth* tau *timeCorrection) * jacDiagABInv;
|
||||
SimdScalar penetrationImpulse = (-distance* tau *timeCorrection) * jacDiagABInv;
|
||||
|
||||
if (penetrationImpulse < 0.f)
|
||||
penetrationImpulse = 0.f;
|
||||
|
||||
|
||||
|
||||
SimdScalar velocityImpulse = -(1.0f + rest) * damping * rel_vel * jacDiagABInv;
|
||||
|
||||
SimdScalar friction_impulse = 0.f;
|
||||
|
||||
if (velocityImpulse <= 0.f)
|
||||
velocityImpulse = 0.f;
|
||||
SimdScalar totalimpulse = penetrationImpulse+velocityImpulse;
|
||||
|
||||
// SimdScalar impulse = penetrationImpulse + velocityImpulse;
|
||||
//if (impulse > 0.f)
|
||||
if (totalimpulse > 0.f)
|
||||
{
|
||||
// SimdVector3 impulse_vector = normal * impulse;
|
||||
body1.applyImpulse(normal*(velocityImpulse+penetrationImpulse), rel_pos1);
|
||||
|
@ -19,30 +19,18 @@ class RigidBody;
|
||||
struct ContactSolverInfo;
|
||||
|
||||
///bilateral constraint between two dynamic objects
|
||||
///positive distance = separation, negative distance = penetration
|
||||
void resolveSingleBilateral(RigidBody& body1, const SimdVector3& pos1,
|
||||
RigidBody& body2, const SimdVector3& pos2,
|
||||
SimdScalar depth, const SimdVector3& normal,SimdScalar& impulse ,float timeStep);
|
||||
SimdScalar distance, const SimdVector3& normal,SimdScalar& impulse ,float timeStep);
|
||||
|
||||
|
||||
//contact constraint resolution:
|
||||
//calculate and apply impulse to satisfy non-penetration and non-negative relative velocity constraint
|
||||
float resolveSingleCollision(RigidBody& body1, const SimdVector3& pos1,
|
||||
RigidBody& body2, const SimdVector3& pos2,
|
||||
SimdScalar depth, const SimdVector3& normal,
|
||||
const ContactSolverInfo& info);
|
||||
|
||||
|
||||
/// apply friction force to simulate friction in a contact point related to the normal impulse
|
||||
void applyFrictionInContactPointOld(RigidBody& body1, const SimdVector3& pos1,
|
||||
RigidBody& body2, const SimdVector3& pos2,
|
||||
const SimdVector3& normal,float normalImpulse,
|
||||
const ContactSolverInfo& info);
|
||||
|
||||
//contact constraint resolution:
|
||||
//calculate and apply impulse to satisfy non-penetration and non-negative relative velocity constraint
|
||||
///contact constraint resolution:
|
||||
///calculate and apply impulse to satisfy non-penetration and non-negative relative velocity constraint
|
||||
///positive distance = separation, negative distance = penetration
|
||||
float resolveSingleCollisionWithFriction(RigidBody& body1, const SimdVector3& pos1,
|
||||
RigidBody& body2, const SimdVector3& pos2,
|
||||
SimdScalar depth, const SimdVector3& normal,
|
||||
SimdScalar distance, const SimdVector3& normal,
|
||||
const ContactSolverInfo& info);
|
||||
|
||||
#endif //CONTACT_CONSTRAINT_H
|
||||
|
@ -45,7 +45,7 @@ class BU_Joint;
|
||||
|
||||
OdeConstraintSolver::OdeConstraintSolver():
|
||||
m_cfm(1e-5f),
|
||||
m_erp(0.2f)
|
||||
m_erp(0.3f)
|
||||
{
|
||||
}
|
||||
|
||||
@ -62,10 +62,10 @@ float OdeConstraintSolver::SolveGroup(PersistentManifold** manifoldPtr, int numM
|
||||
m_CurJoint = 0;
|
||||
|
||||
|
||||
RigidBody* bodies [128];
|
||||
RigidBody* bodies [MAX_RIGIDBODIES];
|
||||
|
||||
int numBodies = 0;
|
||||
BU_Joint* joints [128*5];
|
||||
BU_Joint* joints [MAX_RIGIDBODIES*4];
|
||||
int numJoints = 0;
|
||||
|
||||
for (int j=0;j<numManifolds;j++)
|
||||
@ -158,27 +158,6 @@ int OdeConstraintSolver::ConvertBody(RigidBody* body,RigidBody** bodies,int& num
|
||||
body->m_I[2+2*4] = 1.f/body->getInvInertiaDiagLocal()[2];
|
||||
|
||||
|
||||
/*
|
||||
|
||||
SimdMatrix3x3 invI;
|
||||
invI.setIdentity();
|
||||
invI[0][0] = body->getInvInertiaDiagLocal()[0];
|
||||
invI[1][1] = body->getInvInertiaDiagLocal()[1];
|
||||
invI[2][2] = body->getInvInertiaDiagLocal()[2];
|
||||
SimdMatrix3x3 inertia = invI.inverse();
|
||||
|
||||
for (i=0;i<3;i++)
|
||||
{
|
||||
for (j=0;j<3;j++)
|
||||
{
|
||||
body->m_I[i+4*j] = inertia[i][j];
|
||||
}
|
||||
}
|
||||
*/
|
||||
// body->m_I[3+0*4] = 0.f;
|
||||
// body->m_I[3+1*4] = 0.f;
|
||||
// body->m_I[3+2*4] = 0.f;
|
||||
// body->m_I[3+3*4] = 0.f;
|
||||
|
||||
|
||||
dQuaternion q;
|
||||
@ -241,13 +220,19 @@ void OdeConstraintSolver::ConvertConstraint(PersistentManifold* manifold,BU_Join
|
||||
|
||||
if (debugDrawer)
|
||||
{
|
||||
debugDrawer->DrawLine(manifold->GetContactPoint(i).m_positionWorldOnA,manifold->GetContactPoint(i).m_positionWorldOnB,color);
|
||||
debugDrawer->DrawLine(manifold->GetContactPoint(i).m_positionWorldOnA,manifold->GetContactPoint(i).m_positionWorldOnB,color);
|
||||
const ManifoldPoint& cp = manifold->GetContactPoint(i);
|
||||
|
||||
debugDrawer->DrawContactPoint(
|
||||
cp.m_positionWorldOnB,
|
||||
cp.m_normalWorldOnB,
|
||||
cp.GetDistance(),
|
||||
cp.GetLifeTime(),
|
||||
color);
|
||||
|
||||
}
|
||||
assert (m_CurJoint < MAX_JOINTS_1);
|
||||
|
||||
if (manifold->GetContactPoint(i).GetDistance() < 0.0f)
|
||||
// if (manifold->GetContactPoint(i).GetDistance() < 0.0f)
|
||||
{
|
||||
ContactJoint* cont = new (&gJointArray[m_CurJoint++]) ContactJoint( manifold ,i, swapBodies,body0,body1);
|
||||
|
||||
|
@ -42,7 +42,11 @@ float SimpleConstraintSolver::SolveGroup(PersistentManifold** manifoldPtr, int n
|
||||
{
|
||||
for (int j=0;j<numManifolds;j++)
|
||||
{
|
||||
Solve(manifoldPtr[j],info,i,debugDrawer);
|
||||
int k=j;
|
||||
if (i % 2)
|
||||
k = numManifolds-1-j;
|
||||
|
||||
Solve(manifoldPtr[k],info,i,debugDrawer);
|
||||
}
|
||||
}
|
||||
return 0.f;
|
||||
@ -86,35 +90,29 @@ float SimpleConstraintSolver::Solve(PersistentManifold* manifoldPtr, const Conta
|
||||
|
||||
ManifoldPoint& cp = manifoldPtr->GetContactPoint(j);
|
||||
|
||||
if (debugDrawer)
|
||||
debugDrawer->DrawLine(cp.m_positionWorldOnA,cp.m_positionWorldOnB,color);
|
||||
|
||||
if (iter == 0)
|
||||
{
|
||||
if (debugDrawer)
|
||||
debugDrawer->DrawContactPoint(cp.m_positionWorldOnB,cp.m_normalWorldOnB,cp.GetDistance(),cp.GetLifeTime(),color);
|
||||
}
|
||||
|
||||
{
|
||||
|
||||
|
||||
float dist = invNumIterFl * cp.GetDistance() * penetrationResolveFactor / info.m_timeStep;// / timeStep;//penetrationResolveFactor*cp.m_solveDistance /timeStep;//cp.GetDistance();
|
||||
float actualDist = cp.GetDistance();
|
||||
#define MAXPENETRATIONPERFRAME -0.2f
|
||||
float dist = actualDist< MAXPENETRATIONPERFRAME? MAXPENETRATIONPERFRAME:actualDist;
|
||||
|
||||
|
||||
float impulse = 0.f;
|
||||
|
||||
if (doApplyImpulse)
|
||||
{
|
||||
impulse = resolveSingleCollision(*body0,
|
||||
float impulse = resolveSingleCollisionWithFriction(
|
||||
*body0,
|
||||
cp.GetPositionWorldOnA(),
|
||||
*body1,
|
||||
cp.GetPositionWorldOnB(),
|
||||
-dist,
|
||||
dist,
|
||||
cp.m_normalWorldOnB,
|
||||
info);
|
||||
|
||||
if (useImpulseFriction)
|
||||
{
|
||||
applyFrictionInContactPointOld(
|
||||
*body0,cp.GetPositionWorldOnA(),*body1,cp.GetPositionWorldOnB(),
|
||||
cp.m_normalWorldOnB,impulse,info) ;
|
||||
}
|
||||
}
|
||||
|
||||
if (iter == 0)
|
||||
{
|
||||
cp.m_appliedImpulse = impulse;
|
||||
|
@ -206,7 +206,7 @@ void dSetValue1 (SimdScalar *a, int n, SimdScalar value)
|
||||
// during the solution. depending on the situation, this can help a lot
|
||||
// or hardly at all, but it doesn't seem to hurt.
|
||||
|
||||
//#define RANDOMLY_REORDER_CONSTRAINTS 1
|
||||
#define RANDOMLY_REORDER_CONSTRAINTS 1
|
||||
|
||||
|
||||
|
||||
@ -764,7 +764,7 @@ void SolveInternal1 (float global_cfm,
|
||||
|
||||
// scale CFM
|
||||
for (i=0; i<m; i++)
|
||||
cfm[i] =0.f;//*= stepsize1;
|
||||
cfm[i] *= stepsize1;
|
||||
|
||||
// load lambda from the value saved on the previous iteration
|
||||
dRealAllocaArray (lambda,m);
|
||||
|
@ -124,13 +124,20 @@ void ContactJoint::GetInfo2(Info2 *info)
|
||||
SimdScalar k = info->fps * info->erp;
|
||||
|
||||
float depth = -point.GetDistance();
|
||||
if (depth < 0.f)
|
||||
depth = 0.f;
|
||||
// if (depth < 0.f)
|
||||
// depth = 0.f;
|
||||
|
||||
info->c[0] = k * depth;
|
||||
info->cfm[0] = 0.f;
|
||||
info->cfm[1] = 0.f;
|
||||
info->cfm[2] = 0.f;
|
||||
float maxvel = .2f;
|
||||
|
||||
// if (info->c[0] > maxvel)
|
||||
// info->c[0] = maxvel;
|
||||
|
||||
|
||||
//can override it, not necessary
|
||||
// info->cfm[0] = 0.f;
|
||||
// info->cfm[1] = 0.f;
|
||||
// info->cfm[2] = 0.f;
|
||||
|
||||
|
||||
|
||||
@ -158,7 +165,7 @@ void ContactJoint::GetInfo2(Info2 *info)
|
||||
c2[2] = ccc2[2];
|
||||
|
||||
|
||||
float friction = 30.f;//0.01f;//FRICTION_CONSTANT*m_body0->getFriction() * m_body1->getFriction();
|
||||
float friction = 10.1f;//FRICTION_CONSTANT*m_body0->getFriction() * m_body1->getFriction();
|
||||
|
||||
// first friction direction
|
||||
if (m_numRows >= 2)
|
||||
@ -198,7 +205,7 @@ void ContactJoint::GetInfo2(Info2 *info)
|
||||
// info->cfm[1] = j->contact.surface.slip1;
|
||||
} else
|
||||
{
|
||||
info->cfm[1] = 0.f;
|
||||
//info->cfm[1] = 0.f;
|
||||
}
|
||||
}
|
||||
|
||||
@ -214,6 +221,7 @@ void ContactJoint::GetInfo2(Info2 *info)
|
||||
info->J2l[s2+2] = -t2[2];
|
||||
dCROSS (info->J2a+s2,= -,c2,t2);
|
||||
}
|
||||
|
||||
// set right hand side
|
||||
if (0){//j->contact.surface.mode & dContactMotion2) {
|
||||
//info->c[2] = j->contact.surface.motion2;
|
||||
|
@ -33,6 +33,11 @@ RigidBody::RigidBody( const MassProps& massProps,SimdScalar linearDamping,SimdSc
|
||||
|
||||
}
|
||||
|
||||
void RigidBody::activate()
|
||||
{
|
||||
SetActivationState(1);
|
||||
m_deactivationTime = 0.f;
|
||||
}
|
||||
void RigidBody::setLinearVelocity(const SimdVector3& lin_vel)
|
||||
{
|
||||
|
||||
|
@ -12,6 +12,8 @@ typedef SimdScalar dMatrix3[4*3];
|
||||
extern float gLinearAirDamping;
|
||||
extern bool gUseEpa;
|
||||
|
||||
#define MAX_RIGIDBODIES 1024
|
||||
|
||||
|
||||
/// RigidBody class for RigidBody Dynamics
|
||||
///
|
||||
@ -142,6 +144,7 @@ public:
|
||||
{
|
||||
return m_friction;
|
||||
}
|
||||
void activate();
|
||||
|
||||
private:
|
||||
SimdTransform m_worldTransform;
|
||||
|
@ -7,8 +7,12 @@
|
||||
|
||||
class BP_Proxy;
|
||||
|
||||
///todo: fill all the empty CcdPhysicsController methods, hook them up to the RigidBody class
|
||||
|
||||
//'temporarily' global variables
|
||||
float gDeactivationTime = 2.f;
|
||||
bool gDisableDeactivation = false;
|
||||
|
||||
float gLinearSleepingTreshold = 0.8f;
|
||||
float gAngularSleepingTreshold = 1.0f;
|
||||
|
||||
@ -121,12 +125,26 @@ void CcdPhysicsController::RelativeRotate(const float drot[9],bool local)
|
||||
}
|
||||
void CcdPhysicsController::getOrientation(float &quatImag0,float &quatImag1,float &quatImag2,float &quatReal)
|
||||
{
|
||||
|
||||
}
|
||||
void CcdPhysicsController::setOrientation(float quatImag0,float quatImag1,float quatImag2,float quatReal)
|
||||
{
|
||||
m_body->activate();
|
||||
|
||||
SimdTransform xform = m_body->getCenterOfMassTransform();
|
||||
xform.setRotation(SimdQuaternion(quatImag0,quatImag1,quatImag2,quatReal));
|
||||
m_body->setCenterOfMassTransform(xform);
|
||||
|
||||
}
|
||||
|
||||
void CcdPhysicsController::setPosition(float posX,float posY,float posZ)
|
||||
{
|
||||
m_body->activate();
|
||||
|
||||
SimdTransform xform = m_body->getCenterOfMassTransform();
|
||||
xform.setOrigin(SimdVector3(posX,posY,posZ));
|
||||
m_body->setCenterOfMassTransform(xform);
|
||||
|
||||
}
|
||||
void CcdPhysicsController::resolveCombinedVelocities(float linvelX,float linvelY,float linvelZ,float angVelX,float angVelY,float angVelZ)
|
||||
{
|
||||
@ -167,9 +185,7 @@ void CcdPhysicsController::applyImpulse(float attachX,float attachY,float attac
|
||||
SimdVector3 impulse(impulseX,impulseY,impulseZ);
|
||||
SimdVector3 pos(attachX,attachY,attachZ);
|
||||
|
||||
//it might be sleeping... wake up !
|
||||
m_body->SetActivationState(1);
|
||||
m_body->m_deactivationTime = 0.f;
|
||||
m_body->activate();
|
||||
|
||||
m_body->applyImpulse(impulse,pos);
|
||||
|
||||
@ -226,7 +242,7 @@ bool CcdPhysicsController::wantsSleeping()
|
||||
{
|
||||
|
||||
//disable deactivation
|
||||
if (gDeactivationTime == 0.f)
|
||||
if (gDisableDeactivation || (gDeactivationTime == 0.f))
|
||||
return false;
|
||||
//2 == ISLAND_SLEEPING, 3 == WANTS_DEACTIVATION
|
||||
if ( (m_body->GetActivationState() == 2) || (m_body->GetActivationState() == 3))
|
||||
|
@ -13,6 +13,7 @@ class CollisionShape;
|
||||
extern float gDeactivationTime;
|
||||
extern float gLinearSleepingTreshold;
|
||||
extern float gAngularSleepingTreshold;
|
||||
extern bool gDisableDeactivation;
|
||||
|
||||
|
||||
struct CcdConstructionInfo
|
||||
|
@ -306,6 +306,13 @@ bool CcdPhysicsEnvironment::proceedDeltaTime(double curTime,float timeStep)
|
||||
if (timeStep == 0.f)
|
||||
return true;
|
||||
|
||||
if (m_debugDrawer)
|
||||
{
|
||||
gDisableDeactivation = (m_debugDrawer->GetDebugMode() & IDebugDraw::DBG_NoDeactivation);
|
||||
}
|
||||
|
||||
|
||||
|
||||
//clamp hardcoded for now
|
||||
if (timeStep > 0.02)
|
||||
timeStep = 0.02;
|
||||
@ -435,17 +442,17 @@ bool CcdPhysicsEnvironment::proceedDeltaTime(double curTime,float timeStep)
|
||||
shapeinterface->GetAabb(body->getCenterOfMassTransform(),
|
||||
minAabb,maxAabb);
|
||||
|
||||
SimdVector3 manifoldExtraExtents(gContactBreakingTreshold,gContactBreakingTreshold,gContactBreakingTreshold);
|
||||
minAabb -= manifoldExtraExtents;
|
||||
maxAabb += manifoldExtraExtents;
|
||||
|
||||
BroadphaseProxy* bp = (BroadphaseProxy*) ctrl->m_broadphaseHandle;
|
||||
if (bp)
|
||||
{
|
||||
|
||||
#ifdef WIN32
|
||||
SimdVector3 color (1,0,0);
|
||||
SimdVector3 color (1,1,0);
|
||||
|
||||
|
||||
|
||||
|
||||
if (m_debugDrawer)
|
||||
{
|
||||
//draw aabb
|
||||
@ -453,7 +460,7 @@ bool CcdPhysicsEnvironment::proceedDeltaTime(double curTime,float timeStep)
|
||||
{
|
||||
case ISLAND_SLEEPING:
|
||||
{
|
||||
color.setValue(0,1,0);
|
||||
color.setValue(1,1,1);
|
||||
break;
|
||||
}
|
||||
case WANTS_DEACTIVATION:
|
||||
@ -465,11 +472,13 @@ bool CcdPhysicsEnvironment::proceedDeltaTime(double curTime,float timeStep)
|
||||
{
|
||||
break;
|
||||
}
|
||||
|
||||
|
||||
};
|
||||
|
||||
DrawAabb(m_debugDrawer,minAabb,maxAabb,color);
|
||||
if (m_debugDrawer->GetDebugMode() & IDebugDraw::DBG_DrawAabb)
|
||||
{
|
||||
DrawAabb(m_debugDrawer,minAabb,maxAabb,color);
|
||||
}
|
||||
}
|
||||
#endif
|
||||
|
||||
|
@ -5,6 +5,7 @@
|
||||
Name="CcdPhysics"
|
||||
ProjectGUID="{E3D57F36-36AC-4E9A-B0E0-2AE4510FCBFE}"
|
||||
Keyword="Win32Proj"
|
||||
SignManifests="true"
|
||||
>
|
||||
<Platforms>
|
||||
<Platform
|
||||
@ -41,12 +42,12 @@
|
||||
Optimization="0"
|
||||
AdditionalIncludeDirectories="..\Common;..\..\..\LinearMath;..\..\..\Bullet;..\..\..\BulletDynamics"
|
||||
PreprocessorDefinitions="WIN32;_DEBUG;_LIB"
|
||||
MinimalRebuild="TRUE"
|
||||
MinimalRebuild="true"
|
||||
BasicRuntimeChecks="3"
|
||||
RuntimeLibrary="1"
|
||||
UsePrecompiledHeader="0"
|
||||
WarningLevel="3"
|
||||
Detect64BitPortabilityProblems="TRUE"
|
||||
Detect64BitPortabilityProblems="true"
|
||||
DebugInformationFormat="4"
|
||||
/>
|
||||
<Tool
|
||||
@ -72,7 +73,7 @@
|
||||
Name="VCBscMakeTool"
|
||||
/>
|
||||
<Tool
|
||||
Name="VCAppVerifierTool"
|
||||
Name="VCFxCopTool"
|
||||
/>
|
||||
<Tool
|
||||
Name="VCPostBuildEventTool"
|
||||
@ -107,7 +108,7 @@
|
||||
RuntimeLibrary="0"
|
||||
UsePrecompiledHeader="0"
|
||||
WarningLevel="3"
|
||||
Detect64BitPortabilityProblems="TRUE"
|
||||
Detect64BitPortabilityProblems="true"
|
||||
DebugInformationFormat="3"
|
||||
/>
|
||||
<Tool
|
||||
@ -133,7 +134,7 @@
|
||||
Name="VCBscMakeTool"
|
||||
/>
|
||||
<Tool
|
||||
Name="VCAppVerifierTool"
|
||||
Name="VCFxCopTool"
|
||||
/>
|
||||
<Tool
|
||||
Name="VCPostBuildEventTool"
|
||||
@ -182,4 +183,6 @@
|
||||
>
|
||||
</File>
|
||||
</Files>
|
||||
<Globals>
|
||||
</Globals>
|
||||
</VisualStudioProject>
|
||||
|
20
extern/bullet/LinearMath/IDebugDraw.h
vendored
20
extern/bullet/LinearMath/IDebugDraw.h
vendored
@ -30,12 +30,30 @@ DEALINGS IN THE SOFTWARE.
|
||||
|
||||
#include "SimdVector3.h"
|
||||
|
||||
|
||||
class IDebugDraw
|
||||
{
|
||||
public:
|
||||
public:
|
||||
|
||||
enum DebugDrawModes
|
||||
{
|
||||
DBG_NoDebug=0,
|
||||
DBG_DrawAabb=1,
|
||||
DBG_DrawText=2,
|
||||
DBG_DrawFeaturesText=4,
|
||||
DBG_DrawContactPoints=8,
|
||||
DBG_NoDeactivation=16,
|
||||
DBG_MAX_DEBUG_DRAW_MODE
|
||||
};
|
||||
|
||||
virtual void DrawLine(const SimdVector3& from,const SimdVector3& to,const SimdVector3& color)=0;
|
||||
|
||||
virtual void DrawContactPoint(const SimdVector3& PointOnB,const SimdVector3& normalOnB,float distance,int lifeTime,const SimdVector3& color)=0;
|
||||
|
||||
virtual void SetDebugMode(int debugMode) =0;
|
||||
|
||||
virtual int GetDebugMode() const = 0;
|
||||
|
||||
|
||||
};
|
||||
|
||||
|
Loading…
Reference in New Issue
Block a user