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); }
/// 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(); }
/// 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); }