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