From 5ebc7c8bdabb756c1d1775e02b5cbaf6958bec53 Mon Sep 17 00:00:00 2001 From: Erwin Coumans Date: Fri, 12 Aug 2005 13:42:00 +0000 Subject: added more debug text, enabled the bullet penalty solver, instead of ode solver by default, added a better demo. --- extern/bullet/Bullet/Bullet3_vc8.vcproj | 21 +-- extern/bullet/Bullet/CollisionShapes/BoxShape.cpp | 4 - .../Bullet/NarrowPhaseCollision/ManifoldPoint.h | 8 +- .../MinkowskiPenetrationDepthSolver.cpp | 20 ++- .../NarrowPhaseCollision/PersistentManifold.cpp | 3 +- .../NarrowPhaseCollision/PersistentManifold.h | 2 +- .../CollisionDispatch/ConvexConvexAlgorithm.cpp | 8 +- .../CollisionDispatch/ToiContactDispatcher.cpp | 1 - .../ConstraintSolver/ContactConstraint.cpp | 162 ++------------------- .../ConstraintSolver/ContactConstraint.h | 24 +-- .../ConstraintSolver/OdeConstraintSolver.cpp | 39 ++--- .../ConstraintSolver/SimpleConstraintSolver.cpp | 36 +++-- .../BulletDynamics/ConstraintSolver/SorLcp.cpp | 4 +- .../BulletDynamics/Dynamics/ContactJoint.cpp | 22 ++- .../bullet/BulletDynamics/Dynamics/RigidBody.cpp | 5 + extern/bullet/BulletDynamics/Dynamics/RigidBody.h | 3 + .../CcdPhysics/CcdPhysicsController.cpp | 24 ++- .../CcdPhysics/CcdPhysicsController.h | 1 + .../CcdPhysics/CcdPhysicsEnvironment.cpp | 23 ++- .../CcdPhysics/CcdPhysics_vc8.vcproj | 13 +- extern/bullet/LinearMath/IDebugDraw.h | 20 ++- 21 files changed, 181 insertions(+), 262 deletions(-) (limited to 'extern') diff --git a/extern/bullet/Bullet/Bullet3_vc8.vcproj b/extern/bullet/Bullet/Bullet3_vc8.vcproj index c2f3e70180f..b9046f451e5 100644 --- a/extern/bullet/Bullet/Bullet3_vc8.vcproj +++ b/extern/bullet/Bullet/Bullet3_vc8.vcproj @@ -4,7 +4,6 @@ Version="8.00" Name="Bullet3ContinuousCollision" ProjectGUID="{FFD3C64A-30E2-4BC7-BC8F-51818C320400}" - SignManifests="true" > - - - - @@ -437,6 +428,14 @@ RelativePath=".\CollisionShapes\TriangleCallback.h" > + + + + @@ -509,6 +508,10 @@ RelativePath="..\LinearMath\GEN_random.h" > + + diff --git a/extern/bullet/Bullet/CollisionShapes/BoxShape.cpp b/extern/bullet/Bullet/CollisionShapes/BoxShape.cpp index 727af3ae499..6cbab4ff03a 100644 --- a/extern/bullet/Bullet/CollisionShapes/BoxShape.cpp +++ b/extern/bullet/Bullet/CollisionShapes/BoxShape.cpp @@ -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; diff --git a/extern/bullet/Bullet/NarrowPhaseCollision/ManifoldPoint.h b/extern/bullet/Bullet/NarrowPhaseCollision/ManifoldPoint.h index 5376d1f51aa..11b573c98c0 100644 --- a/extern/bullet/Bullet/NarrowPhaseCollision/ManifoldPoint.h +++ b/extern/bullet/Bullet/NarrowPhaseCollision/ManifoldPoint.h @@ -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; diff --git a/extern/bullet/Bullet/NarrowPhaseCollision/MinkowskiPenetrationDepthSolver.cpp b/extern/bullet/Bullet/NarrowPhaseCollision/MinkowskiPenetrationDepthSolver.cpp index f8679addd18..1926501b84b 100644 --- a/extern/bullet/Bullet/NarrowPhaseCollision/MinkowskiPenetrationDepthSolver.cpp +++ b/extern/bullet/Bullet/NarrowPhaseCollision/MinkowskiPenetrationDepthSolver.cpp @@ -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 -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 diff --git a/extern/bullet/Bullet/NarrowPhaseCollision/PersistentManifold.h b/extern/bullet/Bullet/NarrowPhaseCollision/PersistentManifold.h index ca02aa9c9a4..2d91a08602b 100644 --- a/extern/bullet/Bullet/NarrowPhaseCollision/PersistentManifold.h +++ b/extern/bullet/Bullet/NarrowPhaseCollision/PersistentManifold.h @@ -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); diff --git a/extern/bullet/BulletDynamics/CollisionDispatch/ConvexConvexAlgorithm.cpp b/extern/bullet/BulletDynamics/CollisionDispatch/ConvexConvexAlgorithm.cpp index 9450eb2e845..c29df9f3807 100644 --- a/extern/bullet/BulletDynamics/CollisionDispatch/ConvexConvexAlgorithm.cpp +++ b/extern/bullet/BulletDynamics/CollisionDispatch/ConvexConvexAlgorithm.cpp @@ -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); diff --git a/extern/bullet/BulletDynamics/CollisionDispatch/ToiContactDispatcher.cpp b/extern/bullet/BulletDynamics/CollisionDispatch/ToiContactDispatcher.cpp index 64db510e97e..3845f307a60 100644 --- a/extern/bullet/BulletDynamics/CollisionDispatch/ToiContactDispatcher.cpp +++ b/extern/bullet/BulletDynamics/CollisionDispatch/ToiContactDispatcher.cpp @@ -7,7 +7,6 @@ #include "CollisionDispatch/EmptyCollisionAlgorithm.h" #include "CollisionDispatch/ConvexConcaveCollisionAlgorithm.h" -#define MAX_RIGIDBODIES 128 void ToiContactDispatcher::FindUnions() diff --git a/extern/bullet/BulletDynamics/ConstraintSolver/ContactConstraint.cpp b/extern/bullet/BulletDynamics/ConstraintSolver/ContactConstraint.cpp index 10bb4af0a7d..8345e23517f 100644 --- a/extern/bullet/BulletDynamics/ConstraintSolver/ContactConstraint.cpp +++ b/extern/bullet/BulletDynamics/ConstraintSolver/ContactConstraint.cpp @@ -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); -*/ + rel_vel = normal.dot(vel); - SimdScalar massTerm = 1.f / (body1.getInvMass() + body2.getInvMass()); - - impulse = - contactDamping * rel_vel * massTerm;//jacDiagABInv; - - //SimdScalar velocityImpulse = -contactDamping * rel_vel * jacDiagABInv; - //impulse = velocityImpulse; +#ifdef ONLY_USE_LINEAR_MASS + SimdScalar massTerm = 1.f / (body1.getInvMass() + body2.getInvMass()); + 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); diff --git a/extern/bullet/BulletDynamics/ConstraintSolver/ContactConstraint.h b/extern/bullet/BulletDynamics/ConstraintSolver/ContactConstraint.h index 0f835546783..1a1e4b3f150 100644 --- a/extern/bullet/BulletDynamics/ConstraintSolver/ContactConstraint.h +++ b/extern/bullet/BulletDynamics/ConstraintSolver/ContactConstraint.h @@ -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 diff --git a/extern/bullet/BulletDynamics/ConstraintSolver/OdeConstraintSolver.cpp b/extern/bullet/BulletDynamics/ConstraintSolver/OdeConstraintSolver.cpp index 0399fdb8a37..b834eb9a417 100644 --- a/extern/bullet/BulletDynamics/ConstraintSolver/OdeConstraintSolver.cpp +++ b/extern/bullet/BulletDynamics/ConstraintSolver/OdeConstraintSolver.cpp @@ -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;jm_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); diff --git a/extern/bullet/BulletDynamics/ConstraintSolver/SimpleConstraintSolver.cpp b/extern/bullet/BulletDynamics/ConstraintSolver/SimpleConstraintSolver.cpp index 501b0a763e2..43b5dc32281 100644 --- a/extern/bullet/BulletDynamics/ConstraintSolver/SimpleConstraintSolver.cpp +++ b/extern/bullet/BulletDynamics/ConstraintSolver/SimpleConstraintSolver.cpp @@ -42,7 +42,11 @@ float SimpleConstraintSolver::SolveGroup(PersistentManifold** manifoldPtr, int n { for (int j=0;jGetContactPoint(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; diff --git a/extern/bullet/BulletDynamics/ConstraintSolver/SorLcp.cpp b/extern/bullet/BulletDynamics/ConstraintSolver/SorLcp.cpp index 4ceb000d111..aecf137a904 100644 --- a/extern/bullet/BulletDynamics/ConstraintSolver/SorLcp.cpp +++ b/extern/bullet/BulletDynamics/ConstraintSolver/SorLcp.cpp @@ -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; ifps * 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; diff --git a/extern/bullet/BulletDynamics/Dynamics/RigidBody.cpp b/extern/bullet/BulletDynamics/Dynamics/RigidBody.cpp index c0927a96915..8c82397988d 100644 --- a/extern/bullet/BulletDynamics/Dynamics/RigidBody.cpp +++ b/extern/bullet/BulletDynamics/Dynamics/RigidBody.cpp @@ -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) { diff --git a/extern/bullet/BulletDynamics/Dynamics/RigidBody.h b/extern/bullet/BulletDynamics/Dynamics/RigidBody.h index 4ff987f6eaa..9b213635616 100644 --- a/extern/bullet/BulletDynamics/Dynamics/RigidBody.h +++ b/extern/bullet/BulletDynamics/Dynamics/RigidBody.h @@ -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; diff --git a/extern/bullet/Extras/PhysicsInterface/CcdPhysics/CcdPhysicsController.cpp b/extern/bullet/Extras/PhysicsInterface/CcdPhysics/CcdPhysicsController.cpp index c16a31ee18d..03c9cab53bf 100644 --- a/extern/bullet/Extras/PhysicsInterface/CcdPhysics/CcdPhysicsController.cpp +++ b/extern/bullet/Extras/PhysicsInterface/CcdPhysics/CcdPhysicsController.cpp @@ -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)) diff --git a/extern/bullet/Extras/PhysicsInterface/CcdPhysics/CcdPhysicsController.h b/extern/bullet/Extras/PhysicsInterface/CcdPhysics/CcdPhysicsController.h index 8c87940721a..728a439866d 100644 --- a/extern/bullet/Extras/PhysicsInterface/CcdPhysics/CcdPhysicsController.h +++ b/extern/bullet/Extras/PhysicsInterface/CcdPhysics/CcdPhysicsController.h @@ -13,6 +13,7 @@ class CollisionShape; extern float gDeactivationTime; extern float gLinearSleepingTreshold; extern float gAngularSleepingTreshold; +extern bool gDisableDeactivation; struct CcdConstructionInfo diff --git a/extern/bullet/Extras/PhysicsInterface/CcdPhysics/CcdPhysicsEnvironment.cpp b/extern/bullet/Extras/PhysicsInterface/CcdPhysics/CcdPhysicsEnvironment.cpp index fa3253003fc..5c117301ffb 100644 --- a/extern/bullet/Extras/PhysicsInterface/CcdPhysics/CcdPhysicsEnvironment.cpp +++ b/extern/bullet/Extras/PhysicsInterface/CcdPhysics/CcdPhysicsEnvironment.cpp @@ -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 diff --git a/extern/bullet/Extras/PhysicsInterface/CcdPhysics/CcdPhysics_vc8.vcproj b/extern/bullet/Extras/PhysicsInterface/CcdPhysics/CcdPhysics_vc8.vcproj index 9204aff8adb..eb5a602e781 100644 --- a/extern/bullet/Extras/PhysicsInterface/CcdPhysics/CcdPhysics_vc8.vcproj +++ b/extern/bullet/Extras/PhysicsInterface/CcdPhysics/CcdPhysics_vc8.vcproj @@ -5,6 +5,7 @@ Name="CcdPhysics" ProjectGUID="{E3D57F36-36AC-4E9A-B0E0-2AE4510FCBFE}" Keyword="Win32Proj" + SignManifests="true" > + + diff --git a/extern/bullet/LinearMath/IDebugDraw.h b/extern/bullet/LinearMath/IDebugDraw.h index b5cb24f74e5..2f0b33a6129 100644 --- a/extern/bullet/LinearMath/IDebugDraw.h +++ b/extern/bullet/LinearMath/IDebugDraw.h @@ -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; + }; -- cgit v1.2.3