public void ReplaceContactPoint(ManifoldPoint newPoint, int insertIndex)
{
Debug.Assert(ValidContactDistance(newPoint));
#if MAINTAIN_PERSISTENCY
int lifeTime = m_pointCache[insertIndex].GetLifeTime();
float appliedImpulse = m_pointCache[insertIndex].m_appliedImpulse;
float appliedLateralImpulse1 = m_pointCache[insertIndex].m_appliedImpulseLateral1;
float appliedLateralImpulse2 = m_pointCache[insertIndex].m_appliedImpulseLateral2;
Debug.Assert(lifeTime >= 0);
Object cache = m_pointCache[insertIndex].GetUserPersistentData();
BulletGlobals.ManifoldPointPool.Free(m_pointCache[insertIndex]);
m_pointCache[insertIndex] = newPoint;
m_pointCache[insertIndex].SetUserPersistentData(cache);
m_pointCache[insertIndex].SetAppliedImpulse(appliedImpulse);
m_pointCache[insertIndex].SetAppliedImpulseLateral1(appliedLateralImpulse1);
m_pointCache[insertIndex].SetAppliedImpulseLateral2(appliedLateralImpulse2);
m_pointCache[insertIndex].m_constraintRow[0].m_accumImpulse = appliedImpulse;
m_pointCache[insertIndex].m_constraintRow[1].m_accumImpulse = appliedLateralImpulse1;
m_pointCache[insertIndex].m_constraintRow[2].m_accumImpulse = appliedLateralImpulse2;
m_pointCache[insertIndex].SetLifeTime(lifeTime);
#else
ClearUserCache(ref m_pointCache[insertIndex]);
m_pointCache[insertIndex] = newPoint;
#endif
}
public int GetCacheEntry(ManifoldPoint newPoint)
{
//float shortestDist = GetContactBreakingThreshold() * GetContactBreakingThreshold();
float shortestDist = GetContactBreakingThreshold();
shortestDist *= shortestDist;
int size = GetNumContacts();
int nearestPoint = -1;
for (int i = 0; i < size; i++)
{
IndexedVector3 diffA = m_pointCache[i].GetLocalPointA() - newPoint.GetLocalPointA();
float distToManiPoint = diffA.LengthSquared();
if (distToManiPoint < shortestDist)
{
shortestDist = distToManiPoint;
nearestPoint = i;
}
}
#if DEBUG
if (BulletGlobals.g_streamWriter != null && BulletGlobals.debugPersistentManifold)
{
BulletGlobals.g_streamWriter.WriteLine("getCacheEntry [{0}]", nearestPoint);
}
#endif
return(nearestPoint);
}
public void ClearUserCache(ref ManifoldPoint pt)
{
Object oldPtr = pt.m_userPersistentData;
if (oldPtr != null)
{
#if DEBUG
if (BulletGlobals.g_streamWriter != null && BulletGlobals.debugPersistentManifold)
{
int occurance = 0;
for (int i = 0; i < m_cachedPoints; i++)
{
if (m_pointCache[i].m_userPersistentData == oldPtr)
{
occurance++;
if (occurance > 1)
{
BulletGlobals.g_streamWriter.WriteLine("error in clearUserCache\n");
}
}
}
//Debug.Assert(occurance<=0);
}
#endif
if (pt.m_userPersistentData != null && gContactDestroyedCallback != null)
{
gContactDestroyedCallback.Callback(pt.m_userPersistentData);
pt.m_userPersistentData = null;
}
DebugPersistency();
}
BulletGlobals.ManifoldPointPool.Free(pt);
pt = null;
}
/// calculated new worldspace coordinates and depth, and reject points that exceed the collision margin
public void RefreshContactPoints(ref IndexedMatrix trA, ref IndexedMatrix trB)
{
#if DEBUG
if (BulletGlobals.g_streamWriter != null && BulletGlobals.debugPersistentManifold)
{
MathUtil.PrintVector3(BulletGlobals.g_streamWriter, "refreshContactPoints trA", trA._origin);
MathUtil.PrintVector3(BulletGlobals.g_streamWriter, "refreshContactPoints trB", trB._origin);
}
#endif
/// first refresh worldspace positions and distance
int numContacts = GetNumContacts() - 1;
for (int i = numContacts; i >= 0; i--)
{
ManifoldPoint manifoldPoint = m_pointCache[i];
IndexedVector3 pointA = trA * manifoldPoint.GetLocalPointA();
IndexedVector3 pointB = trB * manifoldPoint.GetLocalPointB();
manifoldPoint.SetPositionWorldOnA(ref pointA);
manifoldPoint.SetPositionWorldOnB(ref pointB);
manifoldPoint.SetDistance(IndexedVector3.Dot((manifoldPoint.GetPositionWorldOnA() - manifoldPoint.GetPositionWorldOnB()), manifoldPoint.GetNormalWorldOnB()));
manifoldPoint.SetLifeTime(manifoldPoint.GetLifeTime() + 1);
m_pointCache[i] = manifoldPoint;
}
/// then
float distance2d;
IndexedVector3 projectedDifference, projectedPoint;
for (int i = numContacts; i >= 0; i--)
{
ManifoldPoint 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.GetPositionWorldOnA() - manifoldPoint.GetNormalWorldOnB() * manifoldPoint.GetDistance();
projectedDifference = manifoldPoint.GetPositionWorldOnB() - projectedPoint;
distance2d = IndexedVector3.Dot(projectedDifference, projectedDifference);
if (distance2d > GetContactBreakingThreshold() * GetContactBreakingThreshold())
{
RemoveContactPoint(i);
}
else
{
//contact point processed callback
if (gContactProcessedCallback != null)
{
gContactProcessedCallback.Callback(manifoldPoint, m_body0, m_body1);
}
}
}
}
DebugPersistency();
}
public int AddManifoldPoint(ManifoldPoint newPoint)
{
Debug.Assert(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;
}
ClearUserCache(ref m_pointCache[insertIndex]);
}
else
{
m_cachedPoints++;
}
if (insertIndex < 0)
{
insertIndex = 0;
}
//Debug.Assert(m_pointCache[insertIndex].GetUserPersistentData() == null);
m_pointCache[insertIndex] = newPoint;
#if DEBUG
if (BulletGlobals.g_streamWriter != null && BulletGlobals.debugPersistentManifold)
{
BulletGlobals.g_streamWriter.WriteLine("addManifoldPoint[{0}][{1}]", insertIndex, m_cachedPoints);
MathUtil.PrintContactPoint(BulletGlobals.g_streamWriter, newPoint);
}
#endif
return(insertIndex);
}
public virtual void AddContactPoint(ref IndexedVector3 normalOnBInWorld, ref IndexedVector3 pointInWorld, float depth)
{
Debug.Assert(m_manifoldPtr != null);
//order in manifold needs to match
#if DEBUG
if (BulletGlobals.g_streamWriter != null && BulletGlobals.debugManifoldResult)
{
BulletGlobals.g_streamWriter.WriteLine("AddContactPoint depth[{0}]", depth);
MathUtil.PrintVector3(BulletGlobals.g_streamWriter, "normalOnBInWorld", normalOnBInWorld);
MathUtil.PrintVector3(BulletGlobals.g_streamWriter, "pointInWorld", pointInWorld);
}
#endif
//if (depth > m_manifoldPtr.GetContactProcessingThreshold())
if (depth > m_manifoldPtr.GetContactBreakingThreshold())
{
return;
}
bool isSwapped = m_manifoldPtr.GetBody0() != m_body0;
IndexedVector3 pointA = pointInWorld + normalOnBInWorld * depth;
IndexedVector3 localA;
IndexedVector3 localB;
if (isSwapped)
{
MathUtil.InverseTransform(ref m_rootTransB, ref pointA, out localA);
MathUtil.InverseTransform(ref m_rootTransA, ref pointInWorld, out localB);
}
else
{
MathUtil.InverseTransform(ref m_rootTransA, ref pointA, out localA);
MathUtil.InverseTransform(ref m_rootTransB, ref pointInWorld, out localB);
}
ManifoldPoint newPt = BulletGlobals.ManifoldPointPool.Get();
newPt.Initialise(ref localA, ref localB, ref normalOnBInWorld, depth);
newPt.SetPositionWorldOnA(ref pointA);
newPt.SetPositionWorldOnB(ref pointInWorld);
int insertIndex = m_manifoldPtr.GetCacheEntry(newPt);
newPt.SetCombinedFriction(CalculateCombinedFriction(m_body0, m_body1));
newPt.SetCombinedRestitution(CalculateCombinedRestitution(m_body0, m_body1));
//BP mod, store contact triangles.
if (isSwapped)
{
newPt.m_partId0 = m_partId1;
newPt.m_partId1 = m_partId0;
newPt.m_index0 = m_index1;
newPt.m_index1 = m_index0;
}
else
{
newPt.m_partId0 = m_partId0;
newPt.m_partId1 = m_partId1;
newPt.m_index0 = m_index0;
newPt.m_index1 = m_index1;
}
#if DEBUG
if (BulletGlobals.g_streamWriter != null && BulletGlobals.debugManifoldResult)
{
MathUtil.PrintContactPoint(BulletGlobals.g_streamWriter, newPt);
}
#endif
//printf("depth=%f\n",depth);
///@todo, check this for any side effects
if (insertIndex >= 0)
{
//const btManifoldPoint& oldPoint = m_manifoldPtr->getContactPoint(insertIndex);
m_manifoldPtr.ReplaceContactPoint(newPt, insertIndex);
}
else
{
insertIndex = m_manifoldPtr.AddManifoldPoint(newPt);
}
//User can override friction and/or restitution
if (BulletGlobals.gContactAddedCallback != null &&
//and if either of the two bodies requires custom material
((m_body0.GetCollisionFlags() & CollisionFlags.CF_CUSTOM_MATERIAL_CALLBACK) != 0 ||
(m_body1.GetCollisionFlags() & CollisionFlags.CF_CUSTOM_MATERIAL_CALLBACK) != 0))
{
//experimental feature info, for per-triangle material etc.
CollisionObject obj0 = isSwapped ? m_body1 : m_body0;
CollisionObject obj1 = isSwapped ? m_body0 : m_body1;
//gContactAddedCallback.callback(m_manifoldPtr.getContactPoint(insertIndex),obj0,m_partId0,m_index0,obj1,m_partId1,m_index1);
ManifoldPoint point = m_manifoldPtr.GetContactPoint(insertIndex);
BulletGlobals.gContactAddedCallback.Callback(ref point, obj0, newPt.m_partId0, newPt.m_index0, obj1, newPt.m_partId1, newPt.m_index1);
}
}
/// sort cached points so most isolated points come first
private int SortCachedPoints(ManifoldPoint pt)
{
//calculate 4 possible cases areas, and take biggest area
//also need to keep 'deepest'
int maxPenetrationIndex = -1;
#if KEEP_DEEPEST_POINT
float maxPenetration = pt.GetDistance();
for (int i = 0; i < m_pointCache.Length; i++)
{
if (m_pointCache[i].GetDistance() < maxPenetration)
{
maxPenetrationIndex = i;
maxPenetration = m_pointCache[i].GetDistance();
}
}
#endif //KEEP_DEEPEST_POINT
float res0 = 0f, res1 = 0f, res2 = 0f, res3 = 0f;
if (maxPenetrationIndex != 0)
{
IndexedVector3 a0 = pt.GetLocalPointA() - m_pointCache[1].GetLocalPointA();
IndexedVector3 b0 = m_pointCache[3].GetLocalPointA() - m_pointCache[2].GetLocalPointA();
IndexedVector3 cross = IndexedVector3.Cross(a0, b0);
res0 = cross.LengthSquared();
}
if (maxPenetrationIndex != 1)
{
IndexedVector3 a1 = pt.GetLocalPointA() - m_pointCache[0].GetLocalPointA();
IndexedVector3 b1 = m_pointCache[3].GetLocalPointA() - m_pointCache[2].GetLocalPointA();
IndexedVector3 cross = IndexedVector3.Cross(a1, b1);
res1 = cross.LengthSquared();
}
if (maxPenetrationIndex != 2)
{
IndexedVector3 a2 = pt.GetLocalPointA() - m_pointCache[0].GetLocalPointA();
IndexedVector3 b2 = m_pointCache[3].GetLocalPointA() - m_pointCache[1].GetLocalPointA();
IndexedVector3 cross = IndexedVector3.Cross(a2, b2);
res2 = cross.LengthSquared();
}
if (maxPenetrationIndex != 3)
{
IndexedVector3 a3 = pt.GetLocalPointA() - m_pointCache[0].GetLocalPointA();
IndexedVector3 b3 = m_pointCache[2].GetLocalPointA() - m_pointCache[1].GetLocalPointA();
IndexedVector3 cross = IndexedVector3.Cross(a3, b3);
res3 = cross.LengthSquared();
}
IndexedVector4 maxvec = new IndexedVector4(res0, res1, res2, res3);
int biggestarea = MathUtil.ClosestAxis(ref maxvec);
#if DEBUG
if (BulletGlobals.g_streamWriter != null && BulletGlobals.debugPersistentManifold)
{
BulletGlobals.g_streamWriter.WriteLine("sortCachedPoints [{0}]", biggestarea);
}
#endif
return(biggestarea);
}
public bool ValidContactDistance(ManifoldPoint pt)
{
return(pt.m_distance1 <= GetContactBreakingThreshold());
}
public int FindContactPoint(ref ManifoldPoint unUsed, int numUnused, ref ManifoldPoint pt)
{
return(0);
}
