int btPersistentManifold::addManifoldPoint(const btManifoldPoint& newPoint)
{
btAssert(validContactDistance(newPoint));
int insertIndex = getNumContacts();
if (insertIndex == MANIFOLD_CACHE_SIZE)
{
#if MANIFOLD_CACHE_SIZE >= 4
//sort cache so best points come first, based on area
insertIndex = sortCachedPoints(newPoint);
#else
insertIndex = 0;
#endif
clearUserCache(m_pointCache[insertIndex]);
} else
{
m_cachedPoints++;
}
if (insertIndex<0)
insertIndex=0;
btAssert(m_pointCache[insertIndex].m_userPersistentData==0);
m_pointCache[insertIndex] = newPoint;
return insertIndex;
}
void btPersistentManifold::refreshContactPoints(const btTransform& trA,const btTransform& trB)
{
int i;
#ifdef DEBUG_PERSISTENCY
printf("refreshContactPoints posA = (%f,%f,%f) posB = (%f,%f,%f)\n",
trA.getOrigin().getX(),
trA.getOrigin().getY(),
trA.getOrigin().getZ(),
trB.getOrigin().getX(),
trB.getOrigin().getY(),
trB.getOrigin().getZ());
#endif //DEBUG_PERSISTENCY
/// first refresh worldspace positions and distance
for (i=getNumContacts()-1;i>=0;i--)
{
btManifoldPoint &manifoldPoint = m_pointCache[i];
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
btScalar distance2d;
btVector3 projectedDifference,projectedPoint;
for (i=getNumContacts()-1;i>=0;i--)
{
btManifoldPoint &manifoldPoint = m_pointCache[i];
//contact becomes invalid when signed distance exceeds margin (projected on contactnormal direction)
if (!validContactDistance(manifoldPoint))
{
removeContactPoint(i);
} else
{
//contact also becomes invalid when relative movement orthogonal to normal exceeds margin
projectedPoint = manifoldPoint.m_positionWorldOnA - manifoldPoint.m_normalWorldOnB * manifoldPoint.m_distance1;
projectedDifference = manifoldPoint.m_positionWorldOnB - projectedPoint;
distance2d = projectedDifference.dot(projectedDifference);
if (distance2d > getContactBreakingThreshold()*getContactBreakingThreshold() )
{
removeContactPoint(i);
} else
{
//contact point processed callback
if (gContactProcessedCallback)
(*gContactProcessedCallback)(manifoldPoint,m_body0,m_body1);
}
}
}
#ifdef DEBUG_PERSISTENCY
DebugPersistency();
#endif //
}
void b3ContactCache::refreshContactPoints(const b3Transform& trA,const b3Transform& trB, struct b3Contact4Data& contacts)
{
int numContacts = b3Contact4Data_getNumPoints(&contacts);
int i;
/// first refresh worldspace positions and distance
for (i=numContacts-1;i>=0;i--)
{
b3Vector3 worldPosA = trA( contacts.m_localPosA[i]);
b3Vector3 worldPosB = trB( contacts.m_localPosB[i]);
contacts.m_worldPosB[i] = worldPosB;
float distance = (worldPosA - worldPosB).dot(contacts.m_worldNormalOnB);
contacts.m_worldPosB[i].w = distance;
}
/// then
b3Scalar distance2d;
b3Vector3 projectedDifference,projectedPoint;
for (i=numContacts-1;i>=0;i--)
{
b3Vector3 worldPosA = trA( contacts.m_localPosA[i]);
b3Vector3 worldPosB = trB( contacts.m_localPosB[i]);
b3Vector3&pt = contacts.m_worldPosB[i];
//contact becomes invalid when signed distance exceeds margin (projected on contactnormal direction)
if (!validContactDistance(pt))
{
removeContactPoint(contacts,i);
} else
{
//contact also becomes invalid when relative movement orthogonal to normal exceeds margin
projectedPoint = worldPosA - contacts.m_worldNormalOnB * contacts.m_worldPosB[i].w;
projectedDifference = contacts.m_worldPosB[i] - projectedPoint;
distance2d = projectedDifference.dot(projectedDifference);
if (distance2d > gContactBreakingThreshold*gContactBreakingThreshold )
{
removeContactPoint(contacts,i);
} else
{
////contact point processed callback
//if (gContactProcessedCallback)
// (*gContactProcessedCallback)(manifoldPoint,(void*)m_body0,(void*)m_body1);
}
}
}
}
