diff options
author | Erwin Coumans <blender@erwincoumans.com> | 2006-05-20 00:20:46 +0400 |
---|---|---|
committer | Erwin Coumans <blender@erwincoumans.com> | 2006-05-20 00:20:46 +0400 |
commit | bf38cf6f80697502f5b8423803910d23fa7728db (patch) | |
tree | 6ce1e03136ceec90988d2f1fe301805fe54da6da /extern | |
parent | 069e83be536b9cd3cb9d36f6b6152744c396dda0 (diff) |
Bullet: added per-contact point user-cache, + callback. This allows Collision Detection to be used more generically, and still have persistent info stored, like contact constraints.
Bullet: added support for restitution
Diffstat (limited to 'extern')
17 files changed, 323 insertions, 95 deletions
diff --git a/extern/bullet/Bullet/BroadphaseCollision/Dispatcher.h b/extern/bullet/Bullet/BroadphaseCollision/Dispatcher.h index 579731569f8..7f8351bcd93 100644 --- a/extern/bullet/Bullet/BroadphaseCollision/Dispatcher.h +++ b/extern/bullet/Bullet/BroadphaseCollision/Dispatcher.h @@ -76,6 +76,8 @@ public: virtual void ReleaseManifold(PersistentManifold* manifold)=0; + virtual void ClearManifold(PersistentManifold* manifold)=0; + virtual bool NeedsCollision(BroadphaseProxy& proxy0,BroadphaseProxy& proxy1) = 0; virtual ManifoldResult* GetNewManifoldResult(CollisionObject* obj0,CollisionObject* obj1,PersistentManifold* manifold) =0; diff --git a/extern/bullet/Bullet/CollisionDispatch/CollisionDispatcher.cpp b/extern/bullet/Bullet/CollisionDispatch/CollisionDispatcher.cpp index 5fe5157b420..0c746781758 100644 --- a/extern/bullet/Bullet/CollisionDispatch/CollisionDispatcher.cpp +++ b/extern/bullet/Bullet/CollisionDispatch/CollisionDispatcher.cpp @@ -25,6 +25,8 @@ subject to the following restrictions: #include "CollisionDispatch/CollisionObject.h" #include <algorithm> +int gNumManifold = 0; + void CollisionDispatcher::FindUnions() { if (m_useIslands) @@ -76,6 +78,8 @@ CollisionDispatcher::CollisionDispatcher (): PersistentManifold* CollisionDispatcher::GetNewManifold(void* b0,void* b1) { + gNumManifold++; + //printf("GetNewManifoldResult: gNumManifold %d\n",gNumManifold); CollisionObject* body0 = (CollisionObject*)b0; CollisionObject* body1 = (CollisionObject*)b1; @@ -86,10 +90,20 @@ PersistentManifold* CollisionDispatcher::GetNewManifold(void* b0,void* b1) return manifold; } +void CollisionDispatcher::ClearManifold(PersistentManifold* manifold) +{ + manifold->ClearManifold(); +} + void CollisionDispatcher::ReleaseManifold(PersistentManifold* manifold) { - manifold->ClearManifold(); + + gNumManifold--; + + //printf("ReleaseManifold: gNumManifold %d\n",gNumManifold); + + ClearManifold(manifold); std::vector<PersistentManifold*>::iterator i = std::find(m_manifoldsPtr.begin(), m_manifoldsPtr.end(), manifold); @@ -272,6 +286,8 @@ bool CollisionDispatcher::NeedsCollision(BroadphaseProxy& proxy0,BroadphaseProxy ///allows the user to get contact point callbacks ManifoldResult* CollisionDispatcher::GetNewManifoldResult(CollisionObject* obj0,CollisionObject* obj1,PersistentManifold* manifold) { + + //in-place, this prevents parallel dispatching, but just adding a list would fix that. ManifoldResult* manifoldResult = new (&m_defaultManifoldResult) ManifoldResult(obj0,obj1,manifold); return manifoldResult; diff --git a/extern/bullet/Bullet/CollisionDispatch/CollisionDispatcher.h b/extern/bullet/Bullet/CollisionDispatch/CollisionDispatcher.h index e3f3c09e76d..3896c19e161 100644 --- a/extern/bullet/Bullet/CollisionDispatch/CollisionDispatcher.h +++ b/extern/bullet/Bullet/CollisionDispatch/CollisionDispatcher.h @@ -113,6 +113,9 @@ public: ///allows the user to get contact point callbacks virtual void ReleaseManifoldResult(ManifoldResult*); + + virtual void ClearManifold(PersistentManifold* manifold); + CollisionAlgorithm* FindAlgorithm(BroadphaseProxy& proxy0,BroadphaseProxy& proxy1) { diff --git a/extern/bullet/Bullet/CollisionDispatch/CollisionWorld.cpp b/extern/bullet/Bullet/CollisionDispatch/CollisionWorld.cpp index 4775db1b1c1..1c057412962 100644 --- a/extern/bullet/Bullet/CollisionDispatch/CollisionWorld.cpp +++ b/extern/bullet/Bullet/CollisionDispatch/CollisionWorld.cpp @@ -27,6 +27,31 @@ subject to the following restrictions: #include <algorithm> +CollisionWorld::~CollisionWorld() +{ + //clean up remaining objects + std::vector<CollisionObject*>::iterator i; + + int index = 0; + for (i=m_collisionObjects.begin(); + !(i==m_collisionObjects.end()); i++) + + { + CollisionObject* collisionObject= (*i); + + BroadphaseProxy* bp = collisionObject->m_broadphaseHandle; + if (bp) + { + // + // only clear the cached algorithms + // + GetBroadphase()->CleanProxyFromPairs(bp); + GetBroadphase()->DestroyProxy(bp); + } + } + +} + void CollisionWorld::UpdateActivationState() { m_dispatcher->InitUnionFind(m_collisionObjects.size()); diff --git a/extern/bullet/Bullet/CollisionDispatch/CollisionWorld.h b/extern/bullet/Bullet/CollisionDispatch/CollisionWorld.h index 1d1249ec88e..611ee9616fc 100644 --- a/extern/bullet/Bullet/CollisionDispatch/CollisionWorld.h +++ b/extern/bullet/Bullet/CollisionDispatch/CollisionWorld.h @@ -46,7 +46,7 @@ public: { } - virtual ~CollisionWorld() {} + virtual ~CollisionWorld(); virtual void UpdateActivationState(); diff --git a/extern/bullet/Bullet/CollisionDispatch/ConvexConcaveCollisionAlgorithm.cpp b/extern/bullet/Bullet/CollisionDispatch/ConvexConcaveCollisionAlgorithm.cpp index 7032a863a54..a72432ef952 100644 --- a/extern/bullet/Bullet/CollisionDispatch/ConvexConcaveCollisionAlgorithm.cpp +++ b/extern/bullet/Bullet/CollisionDispatch/ConvexConcaveCollisionAlgorithm.cpp @@ -62,8 +62,7 @@ BoxTriangleCallback::~BoxTriangleCallback() void BoxTriangleCallback::ClearCache() { - - m_manifoldPtr->ClearManifold(); + m_dispatcher->ClearManifold(m_manifoldPtr); }; @@ -161,7 +160,7 @@ void ConvexConcaveCollisionAlgorithm::ProcessCollision (BroadphaseProxy* ,Broadp m_boxTriangleCallback.SetTimeStepAndCounters(collisionMarginTriangle,dispatchInfo); #ifdef USE_BOX_TRIANGLE - m_boxTriangleCallback.m_manifoldPtr->ClearManifold(); + m_dispatcher->ClearManifold(m_boxTriangleCallback.m_manifoldPtr); #endif m_boxTriangleCallback.m_manifoldPtr->SetBodies(m_convex.m_clientObject,m_concave.m_clientObject); diff --git a/extern/bullet/Bullet/CollisionDispatch/ConvexConvexAlgorithm.cpp b/extern/bullet/Bullet/CollisionDispatch/ConvexConvexAlgorithm.cpp index 6963cf23548..c3ca2ef6be9 100644 --- a/extern/bullet/Bullet/CollisionDispatch/ConvexConvexAlgorithm.cpp +++ b/extern/bullet/Bullet/CollisionDispatch/ConvexConvexAlgorithm.cpp @@ -282,7 +282,7 @@ void ConvexConvexAlgorithm ::ProcessCollision (BroadphaseProxy* ,BroadphaseProxy Transform trB = GetTransformFromSimdTransform(col1->m_worldTransform); //either use persistent manifold or clear it every time - m_manifoldPtr->ClearManifold(); + m_dispatcher->ClearManifold(m_manifoldPtr); ManifoldResult* resultOut = m_dispatcher->GetNewManifoldResult(col0,col1,m_manifoldPtr); ManifoldResultCollector hullContactCollector(*resultOut); @@ -339,6 +339,7 @@ bool disableCcd = false; float ConvexConvexAlgorithm::CalculateTimeOfImpact(BroadphaseProxy* proxy0,BroadphaseProxy* proxy1,const DispatcherInfo& dispatchInfo) { + CheckPenetrationDepthSolver(); //An adhoc way of testing the Continuous Collision Detection algorithms diff --git a/extern/bullet/Bullet/CollisionDispatch/ManifoldResult.cpp b/extern/bullet/Bullet/CollisionDispatch/ManifoldResult.cpp index ccd9a4b2cbc..15b6512a538 100644 --- a/extern/bullet/Bullet/CollisionDispatch/ManifoldResult.cpp +++ b/extern/bullet/Bullet/CollisionDispatch/ManifoldResult.cpp @@ -25,6 +25,7 @@ ManifoldResult::ManifoldResult(CollisionObject* body0,CollisionObject* body1,Per { } + void ManifoldResult::AddContactPoint(const SimdVector3& normalOnBInWorld,const SimdVector3& pointInWorld,float depth) { if (depth > m_manifoldPtr->GetContactBreakingTreshold()) @@ -42,9 +43,13 @@ void ManifoldResult::AddContactPoint(const SimdVector3& normalOnBInWorld,const S int insertIndex = m_manifoldPtr->GetCacheEntry(newPt); if (insertIndex >= 0) { - const ManifoldPoint& oldPoint = m_manifoldPtr->GetContactPoint(insertIndex); - newPt.CopyPersistentInformation(oldPoint); - m_manifoldPtr->ReplaceContactPoint(newPt,insertIndex); + +// This is not needed, just use the old info! +// const ManifoldPoint& oldPoint = m_manifoldPtr->GetContactPoint(insertIndex); +// newPt.CopyPersistentInformation(oldPoint); +// m_manifoldPtr->ReplaceContactPoint(newPt,insertIndex); + + } else { m_manifoldPtr->AddManifoldPoint(newPt); diff --git a/extern/bullet/Bullet/CollisionShapes/ConvexTriangleCallback.cpp b/extern/bullet/Bullet/CollisionShapes/ConvexTriangleCallback.cpp index 9ed5f240b9f..a34add4c04f 100644 --- a/extern/bullet/Bullet/CollisionShapes/ConvexTriangleCallback.cpp +++ b/extern/bullet/Bullet/CollisionShapes/ConvexTriangleCallback.cpp @@ -41,11 +41,6 @@ ConvexTriangleCallback::~ConvexTriangleCallback() } -void ConvexTriangleCallback::ClearCache() -{ - m_manifoldPtr->ClearManifold(); -}; - void ConvexTriangleCallback::ProcessTriangle(SimdVector3* triangle) diff --git a/extern/bullet/Bullet/CollisionShapes/ConvexTriangleCallback.h b/extern/bullet/Bullet/CollisionShapes/ConvexTriangleCallback.h index 17be96fc27e..9160d1f3226 100644 --- a/extern/bullet/Bullet/CollisionShapes/ConvexTriangleCallback.h +++ b/extern/bullet/Bullet/CollisionShapes/ConvexTriangleCallback.h @@ -47,7 +47,6 @@ int m_triangleCount; virtual void ProcessTriangle(SimdVector3* triangle); - void ClearCache(); inline const SimdVector3& GetAabbMin() const { diff --git a/extern/bullet/Bullet/NarrowPhaseCollision/ManifoldPoint.h b/extern/bullet/Bullet/NarrowPhaseCollision/ManifoldPoint.h index 000dc4c1057..35ebc4a40c6 100644 --- a/extern/bullet/Bullet/NarrowPhaseCollision/ManifoldPoint.h +++ b/extern/bullet/Bullet/NarrowPhaseCollision/ManifoldPoint.h @@ -20,12 +20,16 @@ subject to the following restrictions: #include "SimdTransformUtil.h" + + + /// ManifoldContactPoint collects and maintains persistent contactpoints. /// used to improve stability and performance of rigidbody dynamics response. class ManifoldPoint { public: ManifoldPoint() + :m_userPersistentData(0) { } @@ -36,51 +40,26 @@ class ManifoldPoint m_localPointB( pointB ), m_normalWorldOnB( normal ), m_distance1( distance ), - m_appliedImpulse(0.f), - m_prevAppliedImpulse(0.f), - m_accumulatedTangentImpulse0(0.f), - m_accumulatedTangentImpulse1(0.f), - m_jacDiagABInv(0.f), + m_userPersistentData(0), m_lifeTime(0) { - SimdPlaneSpace1(m_normalWorldOnB,m_frictionWorldTangential0,m_frictionWorldTangential1); + } + + SimdVector3 m_localPointA; SimdVector3 m_localPointB; SimdVector3 m_positionWorldOnB; ///m_positionWorldOnA is redundant information, see GetPositionWorldOnA(), but for clarity SimdVector3 m_positionWorldOnA; SimdVector3 m_normalWorldOnB; - - SimdVector3 m_frictionWorldTangential0; - SimdVector3 m_frictionWorldTangential1; - - - - + float m_distance1; - /// total applied impulse during most recent frame - float m_appliedImpulse; - float m_prevAppliedImpulse; - float m_accumulatedTangentImpulse0; - float m_accumulatedTangentImpulse1; - - float m_jacDiagABInv; - float m_jacDiagABInvTangent0; - float m_jacDiagABInvTangent1; - - void CopyPersistentInformation(const ManifoldPoint& otherPoint) - { - m_appliedImpulse = otherPoint.m_appliedImpulse; - m_accumulatedTangentImpulse0 = otherPoint.m_accumulatedTangentImpulse0; - m_accumulatedTangentImpulse1 = otherPoint.m_accumulatedTangentImpulse1; - m_prevAppliedImpulse = otherPoint.m_prevAppliedImpulse; - m_lifeTime = otherPoint.m_lifeTime; - - } + + void* m_userPersistentData; int m_lifeTime;//lifetime of the contactpoint in frames diff --git a/extern/bullet/Bullet/NarrowPhaseCollision/PersistentManifold.cpp b/extern/bullet/Bullet/NarrowPhaseCollision/PersistentManifold.cpp index 7aa225cdce2..df72b9a0417 100644 --- a/extern/bullet/Bullet/NarrowPhaseCollision/PersistentManifold.cpp +++ b/extern/bullet/Bullet/NarrowPhaseCollision/PersistentManifold.cpp @@ -18,7 +18,9 @@ subject to the following restrictions: #include "SimdTransform.h" #include <assert.h> -float gContactBreakingTreshold = 0.02f; +float gContactBreakingTreshold = 0.02f; +ContactDestroyedCallback gContactCallback = 0; + PersistentManifold::PersistentManifold() :m_body0(0), @@ -31,9 +33,61 @@ m_index1(0) void PersistentManifold::ClearManifold() { + int i; + for (i=0;i<m_cachedPoints;i++) + { + ClearUserCache(m_pointCache[i]); + } m_cachedPoints = 0; } +#ifdef DEBUG_PERSISTENCY +#include <stdio.h> +void PersistentManifold::DebugPersistency() +{ + int i; + printf("DebugPersistency : numPoints %d\n",m_cachedPoints); + for (i=0;i<m_cachedPoints;i++) + { + printf("m_pointCache[%d].m_userPersistentData = %x\n",i,m_pointCache[i].m_userPersistentData); + } +} +#endif //DEBUG_PERSISTENCY + +void PersistentManifold::ClearUserCache(ManifoldPoint& pt) +{ + + void* oldPtr = pt.m_userPersistentData; + if (oldPtr) + { +#ifdef DEBUG_PERSISTENCY + int i; + int occurance = 0; + for (i=0;i<m_cachedPoints;i++) + { + if (m_pointCache[i].m_userPersistentData == oldPtr) + { + occurance++; + if (occurance>1) + printf("error in ClearUserCache\n"); + } + } + assert(occurance<=0); +#endif //DEBUG_PERSISTENCY + + if (pt.m_userPersistentData && gContactCallback) + { + (*gContactCallback)(pt.m_userPersistentData); + pt.m_userPersistentData = 0; + } + +#ifdef DEBUG_PERSISTENCY + DebugPersistency(); +#endif + } + + +} @@ -114,9 +168,12 @@ void PersistentManifold::AddManifoldPoint(const ManifoldPoint& newPoint) insertIndex = 0; #endif + } else { m_cachedPoints++; + + } ReplaceContactPoint(newPoint,insertIndex); } @@ -163,6 +220,9 @@ void PersistentManifold::RefreshContactPoints(const SimdTransform& trA,const Sim } } } +#ifdef DEBUG_PERSISTENCY + DebugPersistency(); +#endif // } diff --git a/extern/bullet/Bullet/NarrowPhaseCollision/PersistentManifold.h b/extern/bullet/Bullet/NarrowPhaseCollision/PersistentManifold.h index 235f604a2f8..98bef621f60 100644 --- a/extern/bullet/Bullet/NarrowPhaseCollision/PersistentManifold.h +++ b/extern/bullet/Bullet/NarrowPhaseCollision/PersistentManifold.h @@ -26,6 +26,12 @@ struct CollisionResult; ///contact breaking and merging treshold extern float gContactBreakingTreshold; +typedef bool (*ContactDestroyedCallback)(void* userPersistentData); + +extern ContactDestroyedCallback gContactCallback; + + + #define MANIFOLD_CACHE_SIZE 4 ///PersistentManifold maintains contact points, and reduces them to 4 @@ -69,8 +75,12 @@ public: m_body1 = body1; } - + void ClearUserCache(ManifoldPoint& pt); +#ifdef DEBUG_PERSISTENCY + void DebugPersistency(); +#endif // + inline int GetNumContacts() const { return m_cachedPoints;} inline const ManifoldPoint& GetContactPoint(int index) const @@ -94,12 +104,20 @@ public: void RemoveContactPoint (int index) { - m_pointCache[index] = m_pointCache[GetNumContacts() - 1]; + ClearUserCache(m_pointCache[index]); + + int lastUsedIndex = GetNumContacts() - 1; + m_pointCache[index] = m_pointCache[lastUsedIndex]; + //get rid of duplicated userPersistentData pointer + m_pointCache[lastUsedIndex].m_userPersistentData = 0; m_cachedPoints--; } void ReplaceContactPoint(const ManifoldPoint& newPoint,int insertIndex) { assert(ValidContactDistance(newPoint)); + + ClearUserCache(m_pointCache[insertIndex]); + m_pointCache[insertIndex] = newPoint; } diff --git a/extern/bullet/BulletDynamics/ConstraintSolver/ContactConstraint.cpp b/extern/bullet/BulletDynamics/ConstraintSolver/ContactConstraint.cpp index f2b0a905491..e55c18e4716 100644 --- a/extern/bullet/BulletDynamics/ConstraintSolver/ContactConstraint.cpp +++ b/extern/bullet/BulletDynamics/ConstraintSolver/ContactConstraint.cpp @@ -25,17 +25,12 @@ subject to the following restrictions: #define ASSERT2 assert //some values to find stable tresholds -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); -} SimdScalar calculateCombinedFriction(RigidBody& body0,RigidBody& body1) @@ -118,8 +113,7 @@ float resolveSingleCollision( const SimdVector3& pos1 = contactPoint.GetPositionWorldOnA(); const SimdVector3& pos2 = contactPoint.GetPositionWorldOnB(); - SimdScalar distance = contactPoint.GetDistance(); - + // printf("distance=%f\n",distance); @@ -136,7 +130,7 @@ float resolveSingleCollision( float combinedRestitution = body1.getRestitution() * body2.getRestitution(); - SimdScalar restitution = restitutionCurve(rel_vel, combinedRestitution); + SimdScalar Kfps = 1.f / solverInfo.m_timeStep ; @@ -153,24 +147,30 @@ float resolveSingleCollision( //printf("dist=%f\n",distance); + ConstraintPersistentData* cpd = (ConstraintPersistentData*) contactPoint.m_userPersistentData; + assert(cpd); + + SimdScalar distance = cpd->m_penetration;//contactPoint.GetDistance(); + //distance = 0.f; SimdScalar positionalError = Kcor *-distance; //jacDiagABInv; - SimdScalar velocityError = -(1.0f + restitution) * rel_vel;// * damping; + SimdScalar velocityError = cpd->m_restitution - rel_vel;// * damping; - SimdScalar penetrationImpulse = positionalError * contactPoint.m_jacDiagABInv; + + SimdScalar penetrationImpulse = positionalError * cpd->m_jacDiagABInv; - SimdScalar velocityImpulse = velocityError * contactPoint.m_jacDiagABInv; + SimdScalar velocityImpulse = velocityError * cpd->m_jacDiagABInv; SimdScalar normalImpulse = penetrationImpulse+velocityImpulse; // See Erin Catto's GDC 2006 paper: Clamp the accumulated impulse - float oldNormalImpulse = contactPoint.m_appliedImpulse; + float oldNormalImpulse = cpd->m_appliedImpulse; float sum = oldNormalImpulse + normalImpulse; - contactPoint.m_appliedImpulse = 0.f > sum ? 0.f: sum; + cpd->m_appliedImpulse = 0.f > sum ? 0.f: sum; - normalImpulse = contactPoint.m_appliedImpulse - oldNormalImpulse; + normalImpulse = cpd->m_appliedImpulse - oldNormalImpulse; body1.applyImpulse(normal*(normalImpulse), rel_pos1); body2.applyImpulse(-normal*(normalImpulse), rel_pos2); @@ -194,8 +194,11 @@ float resolveSingleFriction( SimdVector3 rel_pos1 = pos1 - body1.getCenterOfMassPosition(); SimdVector3 rel_pos2 = pos2 - body2.getCenterOfMassPosition(); float combinedFriction = calculateCombinedFriction(body1,body2); - - SimdScalar limit = contactPoint.m_appliedImpulse * combinedFriction; + + ConstraintPersistentData* cpd = (ConstraintPersistentData*) contactPoint.m_userPersistentData; + assert(cpd); + + SimdScalar limit = cpd->m_appliedImpulse * combinedFriction; //if (contactPoint.m_appliedImpulse>0.f) //friction { @@ -208,17 +211,17 @@ float resolveSingleFriction( SimdVector3 vel2 = body2.getVelocityInLocalPoint(rel_pos2); SimdVector3 vel = vel1 - vel2; - SimdScalar vrel = contactPoint.m_frictionWorldTangential0.dot(vel); + SimdScalar vrel = cpd->m_frictionWorldTangential0.dot(vel); // calculate j that moves us to zero relative velocity - SimdScalar j = -vrel * contactPoint.m_jacDiagABInvTangent0; - float total = contactPoint.m_accumulatedTangentImpulse0 + j; + SimdScalar j = -vrel * cpd->m_jacDiagABInvTangent0; + float total = cpd->m_accumulatedTangentImpulse0 + j; GEN_set_min(total, limit); GEN_set_max(total, -limit); - j = total - contactPoint.m_accumulatedTangentImpulse0; - contactPoint.m_accumulatedTangentImpulse0 = total; - body1.applyImpulse(j * contactPoint.m_frictionWorldTangential0, rel_pos1); - body2.applyImpulse(j * -contactPoint.m_frictionWorldTangential0, rel_pos2); + j = total - cpd->m_accumulatedTangentImpulse0; + cpd->m_accumulatedTangentImpulse0 = total; + body1.applyImpulse(j * cpd->m_frictionWorldTangential0, rel_pos1); + body2.applyImpulse(j * -cpd->m_frictionWorldTangential0, rel_pos2); } @@ -228,18 +231,18 @@ float resolveSingleFriction( SimdVector3 vel2 = body2.getVelocityInLocalPoint(rel_pos2); SimdVector3 vel = vel1 - vel2; - SimdScalar vrel = contactPoint.m_frictionWorldTangential1.dot(vel); + SimdScalar vrel = cpd->m_frictionWorldTangential1.dot(vel); // calculate j that moves us to zero relative velocity - SimdScalar j = -vrel * contactPoint.m_jacDiagABInvTangent1; - float total = contactPoint.m_accumulatedTangentImpulse1 + j; + SimdScalar j = -vrel * cpd->m_jacDiagABInvTangent1; + float total = cpd->m_accumulatedTangentImpulse1 + j; GEN_set_min(total, limit); GEN_set_max(total, -limit); - j = total - contactPoint.m_accumulatedTangentImpulse1; - contactPoint.m_accumulatedTangentImpulse1 = total; - body1.applyImpulse(j * contactPoint.m_frictionWorldTangential1, rel_pos1); - body2.applyImpulse(j * -contactPoint.m_frictionWorldTangential1, rel_pos2); + j = total - cpd->m_accumulatedTangentImpulse1; + cpd->m_accumulatedTangentImpulse1 = total; + body1.applyImpulse(j * cpd->m_frictionWorldTangential1, rel_pos1); + body2.applyImpulse(j * -cpd->m_frictionWorldTangential1, rel_pos2); } } - return contactPoint.m_appliedImpulse; + return cpd->m_appliedImpulse; } diff --git a/extern/bullet/BulletDynamics/ConstraintSolver/ContactConstraint.h b/extern/bullet/BulletDynamics/ConstraintSolver/ContactConstraint.h index 8de9f8e30c4..847f2881f41 100644 --- a/extern/bullet/BulletDynamics/ConstraintSolver/ContactConstraint.h +++ b/extern/bullet/BulletDynamics/ConstraintSolver/ContactConstraint.h @@ -24,6 +24,39 @@ class RigidBody; struct ContactSolverInfo; class ManifoldPoint; +struct ConstraintPersistentData +{ + inline ConstraintPersistentData() + :m_appliedImpulse(0.f), + m_prevAppliedImpulse(0.f), + m_accumulatedTangentImpulse0(0.f), + m_accumulatedTangentImpulse1(0.f), + m_jacDiagABInv(0.f), + m_persistentLifeTime(0), + m_restitution(0.f), + m_penetration(0.f) + { + } + + + /// total applied impulse during most recent frame + float m_appliedImpulse; + float m_prevAppliedImpulse; + float m_accumulatedTangentImpulse0; + float m_accumulatedTangentImpulse1; + + float m_jacDiagABInv; + float m_jacDiagABInvTangent0; + float m_jacDiagABInvTangent1; + int m_persistentLifeTime; + float m_restitution; + float m_penetration; + SimdVector3 m_frictionWorldTangential0; + SimdVector3 m_frictionWorldTangential1; + + +}; + ///bilateral constraint between two dynamic objects ///positive distance = separation, negative distance = penetration void resolveSingleBilateral(RigidBody& body1, const SimdVector3& pos1, diff --git a/extern/bullet/BulletDynamics/ConstraintSolver/SimpleConstraintSolver.cpp b/extern/bullet/BulletDynamics/ConstraintSolver/SimpleConstraintSolver.cpp index be1025bb994..9f7d360f7eb 100644 --- a/extern/bullet/BulletDynamics/ConstraintSolver/SimpleConstraintSolver.cpp +++ b/extern/bullet/BulletDynamics/ConstraintSolver/SimpleConstraintSolver.cpp @@ -30,9 +30,31 @@ subject to the following restrictions: #include "quickprof.h" #endif //USE_PROFILE +int totalCpd = 0; + + + +bool MyContactDestroyedCallback(void* userPersistentData) +{ + assert (userPersistentData); + ConstraintPersistentData* cpd = (ConstraintPersistentData*)userPersistentData; + delete cpd; + totalCpd--; + //printf("totalCpd = %i. DELETED Ptr %x\n",totalCpd,userPersistentData); + return true; +} + + +SimpleConstraintSolver::SimpleConstraintSolver() +{ + gContactCallback = &MyContactDestroyedCallback; +} + + /// SimpleConstraintSolver Sequentially applies impulses float SimpleConstraintSolver::SolveGroup(PersistentManifold** manifoldPtr, int numManifolds,const ContactSolverInfo& infoGlobal,IDebugDraw* debugDrawer) { + ContactSolverInfo info = infoGlobal; int numiter = infoGlobal.m_numIterations; @@ -82,6 +104,12 @@ float SimpleConstraintSolver::SolveGroup(PersistentManifold** manifoldPtr, int n float penetrationResolveFactor = 0.9f; +SimdScalar restitutionCurve(SimdScalar rel_vel, SimdScalar restitution) +{ + SimdScalar rest = restitution * -rel_vel; + return rest; +} + float SimpleConstraintSolver::Solve(PersistentManifold* manifoldPtr, const ContactSolverInfo& info,int iter,IDebugDraw* debugDrawer) @@ -111,48 +139,108 @@ float SimpleConstraintSolver::Solve(PersistentManifold* manifoldPtr, const Conta SimdVector3 rel_pos1 = pos1 - body0->getCenterOfMassPosition(); SimdVector3 rel_pos2 = pos2 - body1->getCenterOfMassPosition(); + //this jacobian entry is re-used for all iterations JacobianEntry jac(body0->getCenterOfMassTransform().getBasis().transpose(), body1->getCenterOfMassTransform().getBasis().transpose(), rel_pos1,rel_pos2,cp.m_normalWorldOnB,body0->getInvInertiaDiagLocal(),body0->getInvMass(), body1->getInvInertiaDiagLocal(),body1->getInvMass()); + SimdScalar jacDiagAB = jac.getDiagonal(); + + ConstraintPersistentData* cpd = (ConstraintPersistentData*) cp.m_userPersistentData; + if (cpd) + { + //might be invalid + cpd->m_persistentLifeTime++; + if (cpd->m_persistentLifeTime != cp.GetLifeTime()) + { + //printf("Invalid: cpd->m_persistentLifeTime = %i cp.GetLifeTime() = %i\n",cpd->m_persistentLifeTime,cp.GetLifeTime()); + new (cpd) ConstraintPersistentData; + cpd->m_persistentLifeTime = cp.GetLifeTime(); + + } else + { + //printf("Persistent: cpd->m_persistentLifeTime = %i cp.GetLifeTime() = %i\n",cpd->m_persistentLifeTime,cp.GetLifeTime()); + + } + } else + { + + cpd = new ConstraintPersistentData(); + totalCpd ++; + //printf("totalCpd = %i Created Ptr %x\n",totalCpd,cpd); + cp.m_userPersistentData = cpd; + cpd->m_persistentLifeTime = cp.GetLifeTime(); + //printf("CREATED: %x . cpd->m_persistentLifeTime = %i cp.GetLifeTime() = %i\n",cpd,cpd->m_persistentLifeTime,cp.GetLifeTime()); + + } + assert(cpd); + + cpd->m_jacDiagABInv = 1.f / jacDiagAB; + + + SimdVector3 vel1 = body0->getVelocityInLocalPoint(rel_pos1); + SimdVector3 vel2 = body1->getVelocityInLocalPoint(rel_pos2); + SimdVector3 vel = vel1 - vel2; + SimdScalar rel_vel; + rel_vel = cp.m_normalWorldOnB.dot(vel); - cp.m_jacDiagABInv = 1.f / jacDiagAB; + float combinedRestitution = body0->getRestitution() * body1->getRestitution(); + + cpd->m_penetration = cp.GetDistance(); + cpd->m_restitution = restitutionCurve(rel_vel, combinedRestitution); + if (cpd->m_restitution <= 0.) //0.f) + { + cpd->m_restitution = 0.0f; + + }; + + //restitution and penetration work in same direction so + //rel_vel + + SimdScalar penVel = -cpd->m_penetration/info.m_timeStep; + + if (cpd->m_restitution >= penVel) + { + cpd->m_penetration = 0.f; + } float relaxation = info.m_damping; - cp.m_appliedImpulse *= relaxation; + cpd->m_appliedImpulse *= relaxation; //for friction - cp.m_prevAppliedImpulse = cp.m_appliedImpulse; + cpd->m_prevAppliedImpulse = cpd->m_appliedImpulse; //re-calculate friction direction every frame, todo: check if this is really needed - SimdPlaneSpace1(cp.m_normalWorldOnB,cp.m_frictionWorldTangential0,cp.m_frictionWorldTangential1); + SimdPlaneSpace1(cp.m_normalWorldOnB,cpd->m_frictionWorldTangential0,cpd->m_frictionWorldTangential1); + + #define NO_FRICTION_WARMSTART 1 #ifdef NO_FRICTION_WARMSTART - cp.m_accumulatedTangentImpulse0 = 0.f; - cp.m_accumulatedTangentImpulse1 = 0.f; + cpd->m_accumulatedTangentImpulse0 = 0.f; + cpd->m_accumulatedTangentImpulse1 = 0.f; #endif //NO_FRICTION_WARMSTART - float denom0 = body0->ComputeImpulseDenominator(pos1,cp.m_frictionWorldTangential0); - float denom1 = body1->ComputeImpulseDenominator(pos2,cp.m_frictionWorldTangential0); + float denom0 = body0->ComputeImpulseDenominator(pos1,cpd->m_frictionWorldTangential0); + float denom1 = body1->ComputeImpulseDenominator(pos2,cpd->m_frictionWorldTangential0); float denom = relaxation/(denom0+denom1); - cp.m_jacDiagABInvTangent0 = denom; + cpd->m_jacDiagABInvTangent0 = denom; - denom0 = body0->ComputeImpulseDenominator(pos1,cp.m_frictionWorldTangential1); - denom1 = body1->ComputeImpulseDenominator(pos2,cp.m_frictionWorldTangential1); + denom0 = body0->ComputeImpulseDenominator(pos1,cpd->m_frictionWorldTangential1); + denom1 = body1->ComputeImpulseDenominator(pos2,cpd->m_frictionWorldTangential1); denom = relaxation/(denom0+denom1); - cp.m_jacDiagABInvTangent1 = denom; + cpd->m_jacDiagABInvTangent1 = denom; SimdVector3 totalImpulse = #ifndef NO_FRICTION_WARMSTART cp.m_frictionWorldTangential0*cp.m_accumulatedTangentImpulse0+ cp.m_frictionWorldTangential1*cp.m_accumulatedTangentImpulse1+ #endif //NO_FRICTION_WARMSTART - cp.m_normalWorldOnB*cp.m_appliedImpulse; + cp.m_normalWorldOnB*cpd->m_appliedImpulse; //apply previous frames impulse on both bodies body0->applyImpulse(totalImpulse, rel_pos1); diff --git a/extern/bullet/BulletDynamics/ConstraintSolver/SimpleConstraintSolver.h b/extern/bullet/BulletDynamics/ConstraintSolver/SimpleConstraintSolver.h index e58f8020a7a..99b77623b4b 100644 --- a/extern/bullet/BulletDynamics/ConstraintSolver/SimpleConstraintSolver.h +++ b/extern/bullet/BulletDynamics/ConstraintSolver/SimpleConstraintSolver.h @@ -31,6 +31,8 @@ class SimpleConstraintSolver : public ConstraintSolver public: + SimpleConstraintSolver(); + virtual ~SimpleConstraintSolver() {} virtual float SolveGroup(PersistentManifold** manifold,int numManifolds,const ContactSolverInfo& info, IDebugDraw* debugDrawer=0); |