public override void AddContactPoint(Vector3 normalOnBInWorld, Vector3 pointInWorld, float depth) { if (_manifold == null) { throw new BulletException("Manifold Pointer is null."); } //order in manifold needs to match if (depth > PersistentManifold.ContactBreakingThreshold) { return; } bool isSwapped = _manifold.BodyA != _bodyA; Vector3 pointA = pointInWorld + normalOnBInWorld * depth; Vector3 localA; Vector3 localB; if (isSwapped) { localA = MathHelper.InvXForm(_rootTransB, pointA); localB = MathHelper.InvXForm(_rootTransA, pointInWorld); } else { localA = MathHelper.InvXForm(_rootTransA, pointA); localB = MathHelper.InvXForm(_rootTransB, pointInWorld); } ManifoldPoint newPt = new ManifoldPoint(localA, localB, normalOnBInWorld, depth); int insertIndex = _manifold.GetCacheEntry(newPt); newPt.CombinedFriction = CalculateCombinedFriction(_bodyA, _bodyB); newPt.CombinedRestitution = CalculateCombinedRestitution(_bodyA, _bodyB); //User can override friction and/or restitution if (_contactAddedCallback != null && //and if either of the two bodies requires custom material ((_bodyA.CollisionFlags & CollisionOptions.CustomMaterialCallback) != 0 || (_bodyB.CollisionFlags & CollisionOptions.CustomMaterialCallback) != 0)) { //experimental feature info, for per-triangle material etc. CollisionObject obj0 = isSwapped ? _bodyB : _bodyA; CollisionObject obj1 = isSwapped ? _bodyA : _bodyB; _contactAddedCallback(newPt, obj0, _partIdA, _indexA, obj1, _partIdB, _indexB); } if (insertIndex >= 0) { _manifold.ReplaceContactPoint(newPt, insertIndex); } else { _manifold.AddManifoldPoint(newPt); } }
// 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; float maxPenetration = pt.Distance; for (int i = 0; i < 4; i++) { if (_pointCache[i].Distance < maxPenetration) { maxPenetrationIndex = i; maxPenetration = _pointCache[i].Distance; } } float res0 = 0, res1 = 0, res2 = 0, res3 = 0; if (maxPenetrationIndex != 0) { Vector3 a0 = pt.LocalPointA - _pointCache[1].LocalPointA; Vector3 b0 = _pointCache[3].LocalPointA - _pointCache[2].LocalPointA; Vector3 cross = Vector3.Cross(a0, b0); res0 = cross.LengthSquared(); } if (maxPenetrationIndex != 1) { Vector3 a1 = pt.LocalPointA - _pointCache[0].LocalPointA; Vector3 b1 = _pointCache[3].LocalPointA - _pointCache[2].LocalPointA; Vector3 cross = Vector3.Cross(a1, b1); res1 = cross.LengthSquared(); } if (maxPenetrationIndex != 2) { Vector3 a2 = pt.LocalPointA - _pointCache[0].LocalPointA; Vector3 b2 = _pointCache[3].LocalPointA - _pointCache[1].LocalPointA; Vector3 cross = Vector3.Cross(a2, b2); res2 = cross.LengthSquared(); } if (maxPenetrationIndex != 3) { Vector3 a3 = pt.LocalPointA - _pointCache[0].LocalPointA; Vector3 b3 = _pointCache[2].LocalPointA - _pointCache[1].LocalPointA; Vector3 cross = Vector3.Cross(a3, b3); res3 = cross.LengthSquared(); } Vector4 maxvec = new Vector4(res0, res1, res2, res3); int biggestarea = MathHelper.ClosestAxis(maxvec); return(biggestarea); }
private void ClearUserCache(ManifoldPoint pt) { if (pt != null) { object oldPtr = pt.UserPersistentData; if (oldPtr != null) { if (pt.UserPersistentData != null && _contactDestroyedCallback != null) { _contactDestroyedCallback(pt.UserPersistentData); pt.UserPersistentData = null; } } } }
public int GetCacheEntry(ManifoldPoint newPoint) { float shortestDist = ContactBreakingThreshold * ContactBreakingThreshold; int size = ContactsCount; int nearestPoint = -1; for (int i = 0; i < size; i++) { ManifoldPoint mp = _pointCache[i]; Vector3 diffA = mp.LocalPointA - newPoint.LocalPointA; float distToManiPoint = Vector3.Dot(diffA, diffA); if (distToManiPoint < shortestDist) { shortestDist = distToManiPoint; nearestPoint = i; } } return nearestPoint; }
public int GetCacheEntry(ManifoldPoint newPoint) { float shortestDist = ContactBreakingThreshold * ContactBreakingThreshold; int size = ContactsCount; int nearestPoint = -1; for (int i = 0; i < size; i++) { ManifoldPoint mp = _pointCache[i]; Vector3 diffA = mp.LocalPointA - newPoint.LocalPointA; float distToManiPoint = Vector3.Dot(diffA, diffA); if (distToManiPoint < shortestDist) { shortestDist = distToManiPoint; nearestPoint = i; } } return(nearestPoint); }
public void AddManifoldPoint(ManifoldPoint newPoint) { if (!ValidContactDistance(newPoint)) { throw new BulletException(); } int insertIndex = ContactsCount; if (insertIndex == 4) { //sort cache so best points come first, based on area insertIndex = SortCachedPoints(newPoint); } else { _cachedPoints++; } ReplaceContactPoint(newPoint, insertIndex); }
public void ReplaceContactPoint(ManifoldPoint newPoint, int insertIndex) { BulletDebug.Assert(ValidContactDistance(newPoint)); if (_pointCache[insertIndex] != null) { int lifeTime = _pointCache[insertIndex].LifeTime; BulletDebug.Assert(lifeTime >= 0); object cache = _pointCache[insertIndex].UserPersistentData; _pointCache[insertIndex] = newPoint; _pointCache[insertIndex].UserPersistentData = cache; _pointCache[insertIndex].LifeTime = lifeTime; } else { _pointCache[insertIndex] = newPoint; } //ClearUserCache(_pointCache[insertIndex]); //_pointCache[insertIndex] = newPoint; }
// calculated new worldspace coordinates and depth, and reject points that exceed the collision margin public void RefreshContactPoints(Matrix trA, Matrix trB) { // first refresh worldspace positions and distance for (int i = ContactsCount - 1; i >= 0; i--) { ManifoldPoint manifoldPoint = _pointCache[i]; manifoldPoint.PositionWorldOnA = MathHelper.MatrixToVector(trA, manifoldPoint.LocalPointA); manifoldPoint.PositionWorldOnB = MathHelper.MatrixToVector(trB, manifoldPoint.LocalPointB); manifoldPoint.Distance = Vector3.Dot(manifoldPoint.PositionWorldOnA - manifoldPoint.PositionWorldOnB, manifoldPoint.NormalWorldOnB); manifoldPoint.LifeTime++; } // then float distance2d; Vector3 projectedDifference, projectedPoint; for (int i = ContactsCount - 1; i >= 0; i--) { ManifoldPoint manifoldPoint = _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.PositionWorldOnA - manifoldPoint.NormalWorldOnB * manifoldPoint.Distance; projectedDifference = manifoldPoint.PositionWorldOnB - projectedPoint; distance2d = Vector3.Dot(projectedDifference, projectedDifference); if (distance2d > ContactBreakingThreshold * ContactBreakingThreshold) { RemoveContactPoint(i); } } } }
public override void AddContactPoint(Vector3 normalOnBInWorld, Vector3 pointInWorld, float depth) { if (_manifold == null) throw new BulletException("Manifold Pointer is null."); //order in manifold needs to match if (depth > PersistentManifold.ContactBreakingThreshold) return; bool isSwapped = _manifold.BodyA != _bodyA; Vector3 pointA = pointInWorld + normalOnBInWorld * depth; Vector3 localA; Vector3 localB; if (isSwapped) { localA = MathHelper.InvXForm(_rootTransB, pointA); localB = MathHelper.InvXForm(_rootTransA, pointInWorld); } else { localA = MathHelper.InvXForm(_rootTransA, pointA); localB = MathHelper.InvXForm(_rootTransB, pointInWorld); } ManifoldPoint newPt = new ManifoldPoint(localA, localB, normalOnBInWorld, depth); int insertIndex = _manifold.GetCacheEntry(newPt); newPt.CombinedFriction = CalculateCombinedFriction(_bodyA, _bodyB); newPt.CombinedRestitution = CalculateCombinedRestitution(_bodyA, _bodyB); //User can override friction and/or restitution if (_contactAddedCallback != null && //and if either of the two bodies requires custom material ((_bodyA.CollisionFlags & CollisionOptions.CustomMaterialCallback) != 0 || (_bodyB.CollisionFlags & CollisionOptions.CustomMaterialCallback) != 0)) { //experimental feature info, for per-triangle material etc. CollisionObject obj0 = isSwapped ? _bodyB : _bodyA; CollisionObject obj1 = isSwapped ? _bodyA : _bodyB; _contactAddedCallback(newPt, obj0, _partIdA, _indexA, obj1, _partIdB, _indexB); } if (insertIndex >= 0) { _manifold.ReplaceContactPoint(newPt, insertIndex); } else { _manifold.AddManifoldPoint(newPt); } }
private int FindContactPoint(ManifoldPoint unUsed, int numUnused, ManifoldPoint pt) { return(0); }
public bool ValidContactDistance(ManifoldPoint pt) { return pt.Distance <= ContactBreakingThreshold; }
public bool ValidContactDistance(ManifoldPoint pt) { return(pt.Distance <= ContactBreakingThreshold); }
public void AddManifoldPoint(ManifoldPoint newPoint) { if (!ValidContactDistance(newPoint)) throw new BulletException(); int insertIndex = ContactsCount; if (insertIndex == 4) { //sort cache so best points come first, based on area insertIndex = SortCachedPoints(newPoint); } else { _cachedPoints++; } ReplaceContactPoint(newPoint, insertIndex); }
// 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; float maxPenetration = pt.Distance; for (int i = 0; i < 4; i++) { if (_pointCache[i].Distance < maxPenetration) { maxPenetrationIndex = i; maxPenetration = _pointCache[i].Distance; } } float res0 = 0, res1 = 0, res2 = 0, res3 = 0; if (maxPenetrationIndex != 0) { Vector3 a0 = pt.LocalPointA - _pointCache[1].LocalPointA; Vector3 b0 = _pointCache[3].LocalPointA - _pointCache[2].LocalPointA; Vector3 cross = Vector3.Cross(a0, b0); res0 = cross.LengthSquared(); } if (maxPenetrationIndex != 1) { Vector3 a1 = pt.LocalPointA - _pointCache[0].LocalPointA; Vector3 b1 = _pointCache[3].LocalPointA - _pointCache[2].LocalPointA; Vector3 cross = Vector3.Cross(a1, b1); res1 = cross.LengthSquared(); } if (maxPenetrationIndex != 2) { Vector3 a2 = pt.LocalPointA - _pointCache[0].LocalPointA; Vector3 b2 = _pointCache[3].LocalPointA - _pointCache[1].LocalPointA; Vector3 cross = Vector3.Cross(a2, b2); res2 = cross.LengthSquared(); } if (maxPenetrationIndex != 3) { Vector3 a3 = pt.LocalPointA - _pointCache[0].LocalPointA; Vector3 b3 = _pointCache[2].LocalPointA - _pointCache[1].LocalPointA; Vector3 cross = Vector3.Cross(a3, b3); res3 = cross.LengthSquared(); } Vector4 maxvec = new Vector4(res0, res1, res2, res3); int biggestarea = MathHelper.ClosestAxis(maxvec); return biggestarea; }
private int FindContactPoint(ManifoldPoint unUsed, int numUnused, ManifoldPoint pt) { return 0; }