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;
}
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 bool ValidContactDistance(ManifoldPoint pt)
{
return(pt.Distance <= ContactBreakingThreshold);
}
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;
}
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; }