diff --git a/Extras/AlternativeCollisionAlgorithms/SphereSphereCollisionAlgorithm.cpp b/Extras/AlternativeCollisionAlgorithms/SphereSphereCollisionAlgorithm.cpp index 5191e69ea..52beffaef 100644 --- a/Extras/AlternativeCollisionAlgorithms/SphereSphereCollisionAlgorithm.cpp +++ b/Extras/AlternativeCollisionAlgorithms/SphereSphereCollisionAlgorithm.cpp @@ -41,34 +41,36 @@ SphereSphereCollisionAlgorithm::~SphereSphereCollisionAlgorithm() void SphereSphereCollisionAlgorithm::ProcessCollision (BroadphaseProxy* proxy0,BroadphaseProxy* proxy1,const DispatcherInfo& dispatchInfo) { - if (!m_manifoldPtr) return; CollisionObject* col0 = static_cast(proxy0->m_clientObject); CollisionObject* col1 = static_cast(proxy1->m_clientObject); - SphereShape* sphere0 = (SphereShape*)col0->m_collisionShape; SphereShape* sphere1 = (SphereShape*)col1->m_collisionShape; + SimdVector3 diff = col0->m_worldTransform.getOrigin()- col1->m_worldTransform.getOrigin(); + float len = diff.length(); SimdScalar radius0 = sphere0->GetRadius(); SimdScalar radius1 = sphere1->GetRadius(); - SimdVector3 diff = col0->m_worldTransform.getOrigin()- - col1->m_worldTransform.getOrigin(); - - float len = diff.length(); + ///iff distance positive, don't generate a new contact if ( len > (radius0+radius1)) return; + ///distance (negative means penetration) SimdScalar dist = len - (radius0+radius1); SimdVector3 normalOnSurfaceB = diff / len; + ///point on A (worldspace) SimdVector3 pos0 = col0->m_worldTransform.getOrigin() - radius0 * normalOnSurfaceB; + ///point on B (worldspace) SimdVector3 pos1 = col1->m_worldTransform.getOrigin() + radius1* normalOnSurfaceB; + /// report a contact. internally this will be kept persistent, and contact reduction is done ManifoldResult* resultOut = m_dispatcher->GetNewManifoldResult(col0,col1,m_manifoldPtr); resultOut->AddContactPoint(normalOnSurfaceB,pos1,dist); + m_dispatcher->ReleaseManifoldResult(resultOut); }