public CompoundCollisionAlgorithm(
CollisionAlgorithmConstructionInfo collisionAlgorithmConstructionInfo,
CollisionObject bodyA,
CollisionObject bodyB, bool isSwapped)
: base(collisionAlgorithmConstructionInfo)
{
//Begin
_isSwapped = isSwapped;
CollisionObject collisionObject = isSwapped ? bodyB : bodyA;
CollisionObject otherObject = isSwapped ? bodyA : bodyB;
BulletDebug.Assert(collisionObject.CollisionShape.IsCompound);
CompoundShape compoundShape = collisionObject.CollisionShape as CompoundShape;
int childrenNumber = compoundShape.ChildShapeCount;
int index = 0;
_childCollisionAlgorithms = new List <CollisionAlgorithm>(childrenNumber);
for (index = 0; index < childrenNumber; index++)
{
CollisionShape childShape = compoundShape.GetChildShape(index);
CollisionShape orgShape = collisionObject.CollisionShape;
collisionObject.CollisionShape = childShape;
_childCollisionAlgorithms[index] = collisionAlgorithmConstructionInfo.Dispatcher.FindAlgorithm(collisionObject, otherObject);
collisionObject.CollisionShape = orgShape;
}
}
public override void ProcessCollision(
CollisionObject bodyA,
CollisionObject bodyB,
DispatcherInfo dispatchInfo, ManifoldResult resultOut)
{
//Begin
CollisionObject collisionObject = _isSwapped ? bodyB : bodyB;
CollisionObject otherObject = _isSwapped ? bodyA : bodyB;
//Debug.Assert(collisionObject.getCollisionShape().isCompound());
BulletDebug.Assert(collisionObject.CollisionShape.IsCompound);
CompoundShape compoundShape = (CompoundShape)collisionObject.CollisionShape;
int childrenNumber = _childCollisionAlgorithms.Count;
for (int i = 0; i < childrenNumber; i++)
{
CompoundShape childShape = compoundShape.GetChildShape(i) as CompoundShape;
Matrix orgTransform = collisionObject.WorldTransform;
CollisionShape orgShape = collisionObject.CollisionShape;
Matrix childTransform = compoundShape.GetChildTransform(i);
Matrix newChildWorld = orgTransform * childTransform;
collisionObject.WorldTransform = newChildWorld;
collisionObject.CollisionShape = childShape;
_childCollisionAlgorithms[i].ProcessCollision(collisionObject, otherObject, dispatchInfo, resultOut);
collisionObject.CollisionShape = orgShape;
collisionObject.WorldTransform = orgTransform;
}
}
public AxisSweep3(Vector3 worldAabbMin, Vector3 worldAabbMax, int maxHandles)
: base()
{
BulletDebug.Assert(maxHandles > 1 && maxHandles < 32767);
// init bounds
_worldAabbMin = worldAabbMin;
_worldAabbMax = worldAabbMax;
Vector3 aabbSize = _worldAabbMax - _worldAabbMin;
_quantize = new Vector3(65535.0f, 65535.0f, 65535.0f) / aabbSize;
// allocate handles buffer and put all handles on free list
_handles = new Handle[maxHandles];
for (int i = 0; i < maxHandles; i++)
{
_handles[i] = new Handle();
}
_maxHandles = maxHandles;
_numHandles = 0;
// handle 0 is reserved as the null index, and is also used as the sentinel
_firstFreeHandle = 1;
{
for (int i = _firstFreeHandle; i < maxHandles; i++)
{
_handles[i].NextFree = (ushort)(i + 1);
}
_handles[maxHandles - 1].NextFree = 0;
}
{
// allocate edge buffers
for (int i = 0; i < 3; i++)
{
_edges[i] = new Edge[maxHandles * 2];
for (int j = 0; j < maxHandles * 2; j++)
{
_edges[i][j] = new Edge();
}
}
}
//removed overlap management
// make boundary sentinels
_handles[0].ClientData = 0;
for (int axis = 0; axis < 3; axis++)
{
_handles[0].MinEdges[axis] = 0;
_handles[0].MaxEdges[axis] = 1;
_edges[axis][0].Position = 0;
_edges[axis][0].Handle = 0;
_edges[axis][1].Position = 0xffff;
_edges[axis][1].Handle = 0;
}
}
public void RemoveVertex(int index)
{
BulletDebug.Assert(_numVertices > 0);
_numVertices--;
_simplexVectorW[index] = _simplexVectorW[_numVertices];
_simplexPointsP[index] = _simplexPointsP[_numVertices];
_simplexPointsQ[index] = _simplexPointsQ[_numVertices];
}
public override void ProcessAllOverlappingPairs(IOverlapCallback callback)
{
OverlappingPairs.Sort(new Comparison <BroadphasePair>(BroadphasePair.ComparisonSort));
if (_invalidPair != 0)
{
OverlappingPairs.RemoveRange(OverlappingPairs.Count - _invalidPair, _invalidPair);
}
_invalidPair = 0;
BroadphasePair previousPair = new BroadphasePair();
previousPair.ProxyA = null;
previousPair.ProxyB = null;
previousPair.CollisionAlgorithm = null;
List <BroadphasePair> removal = new List <BroadphasePair>();
for (int i = 0; i < OverlappingPairs.Count; i++)
{
bool isDuplicate = (OverlappingPairs[i] == previousPair);
previousPair = OverlappingPairs[i];
bool needsRemoval;
if (!isDuplicate)
{
bool hasOverlap = TestOverlap(previousPair.ProxyA, previousPair.ProxyB);
if (hasOverlap)
{
needsRemoval = callback.ProcessOverlap(ref previousPair);
}
else
{
needsRemoval = true;
}
}
else
{
needsRemoval = true;
BulletDebug.Assert(previousPair.CollisionAlgorithm == null);
}
if (needsRemoval)
{
removal.Add(previousPair);
}
}
for (int i = 0; i < removal.Count; i++)
{
BroadphasePair pair = removal[i];
CleanOverlappingPair(ref pair);
pair.ProxyA = null;
pair.ProxyB = null;
_invalidPair++;
OverlappingPairCount--;
}
}
private void FreeHandle(ushort handle)
{
BulletDebug.Assert(handle > 0 && handle < _maxHandles);
GetHandle(handle).NextFree = _firstFreeHandle;
_firstFreeHandle = handle;
_numHandles--;
}
public override BroadphaseProxy CreateProxy(Vector3 min, Vector3 max, BroadphaseNativeTypes shapeType, object userData, BroadphaseProxy.CollisionFilterGroups collisionFilterGroup, BroadphaseProxy.CollisionFilterGroups collisionFilterMask)
{
if (_proxies.Count >= _maxProxies)
{
BulletDebug.Assert(false);
return(null); //should never happen, but don't let the game crash ;-)
}
BulletDebug.Assert(min.X <= max.X && min.Y <= max.Y && min.Z <= max.Z);
SimpleBroadphaseProxy proxy = new SimpleBroadphaseProxy(min, max, shapeType, userData, collisionFilterGroup, collisionFilterMask);
_proxies.Add(proxy);
return(proxy);
}
public static void SetRotation(ref Matrix m, Quaternion q)
{
float d = q.LengthSquared();
BulletDebug.Assert(d != 0);
float s = 2f / d;
float xs = q.X * s, ys = q.Y * s, zs = q.Z * s;
float wx = q.W * xs, wy = q.W * ys, wz = q.W * zs;
float xx = q.X * xs, xy = q.X * ys, xz = q.X * zs;
float yy = q.Y * ys, yz = q.Y * zs, zz = q.Z * zs;
m = new Matrix(1 - (yy + zz), xy - wz, xz + wy, 0,
xy + wz, 1 - (xx + zz), yz - wx, 0,
xz - wy, yz + wx, 1 - (xx + yy), 0,
m.M41, m.M42, m.M43, 1);
}
public void AddOverlappingPair(BroadphaseProxy proxyA, BroadphaseProxy proxyB)
{
//don't add overlap with own
bool test = proxyA != proxyB;
BulletDebug.Assert(proxyA != proxyB);
if (!NeedsBroadphaseCollision(proxyA, proxyB))
{
return;
}
BroadphasePair pair = new BroadphasePair(proxyA, proxyB);
_overlappingPairs.Add(pair);
_overlappingPairCount++;
}
public override float CalculateTimeOfImpact(CollisionObject bodyA, CollisionObject bodyB, DispatcherInfo dispatchInfo, ManifoldResult resultOut)
{
CollisionObject collisionObject = _isSwapped ? bodyB : bodyA;
CollisionObject otherObject = _isSwapped ? bodyA : bodyB;
BulletDebug.Assert(collisionObject.CollisionShape.IsCompound);
CompoundShape compoundShape = (CompoundShape)collisionObject.CollisionShape;
float hitFraction = 1.0f;
for (int i = 0; i < _childCollisionAlgorithms.Count; i++)
{
CollisionShape childShape = compoundShape.GetChildShape(i);
Matrix orgTransform = collisionObject.WorldTransform;
CollisionShape orgShape = collisionObject.CollisionShape;
Matrix childTransform = compoundShape.GetChildTransform(i);
Matrix newChildWorld = orgTransform * childTransform;
collisionObject.WorldTransform = newChildWorld;
collisionObject.CollisionShape = childShape;
float frac = _childCollisionAlgorithms[i].CalculateTimeOfImpact(
collisionObject, otherObject, dispatchInfo, resultOut
);
if (frac < hitFraction)
{
hitFraction = frac;
}
collisionObject.CollisionShape = orgShape;
collisionObject.WorldTransform = orgTransform;
}
return(hitFraction);
}
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 override void CalculateLocalInertia(float mass, out Vector3 inertia)
{
inertia = new Vector3();
//moving concave objects not supported
BulletDebug.Assert(false);
}
public override void CalculateLocalInertia(float mass, out Vector3 inertia)
{
inertia = new Vector3();
BulletDebug.Assert(false);
}
public virtual void GetPreferredPenetrationDirection(int index, out Vector3 penetrationVector)
{
penetrationVector = new Vector3();
BulletDebug.Assert(false);
}
public override void GetClosestPoints(DiscreteCollisionDetectorInterface.ClosestPointInput input, DiscreteCollisionDetectorInterface.Result output, IDebugDraw debugDraw)
{
float distance = 0;
Vector3 normalInB = new Vector3();
Vector3 pointOnA = new Vector3(), pointOnB = new Vector3();
Matrix localTransA = input.TransformA;
Matrix localTransB = input.TransformB;
Vector3 positionOffset = (localTransA.Translation + localTransB.Translation) * 0.5f;
localTransA.Translation -= positionOffset;
localTransB.Translation -= positionOffset;
float marginA = _minkowskiA.Margin;
float marginB = _minkowskiB.Margin;
_numGjkChecks++;
if (_ignoreMargin)
{
marginA = 0;
marginB = 0;
}
_currentIteration = 0;
int gjkMaxIter = 1000;
_cachedSeparatingAxis = new Vector3(0, 1, 0);
bool isValid = false;
bool checkSimplex = false;
bool checkPenetration = true;
_degenerateSimplex = 0;
_lastUsedMethod = -1;
{
float squaredDistance = MathHelper.Infinity;
float delta = 0;
float margin = marginA + marginB;
_simplexSolver.Reset();
while (true)
{
Matrix transABasis = input.TransformA;
transABasis.Translation = Vector3.Zero;
Matrix transBBasis = input.TransformB;
transBBasis.Translation = Vector3.Zero;
Vector3 seperatingAxisInA = Vector3.TransformNormal(-_cachedSeparatingAxis, transABasis);
Vector3 seperatingAxisInB = Vector3.TransformNormal(_cachedSeparatingAxis, transBBasis);
Vector3 pInA = _minkowskiA.LocalGetSupportingVertexWithoutMargin(seperatingAxisInA);
Vector3 qInB = _minkowskiB.LocalGetSupportingVertexWithoutMargin(seperatingAxisInB);
Vector3 pWorld = MathHelper.MatrixToVector(localTransA, pInA);
Vector3 qWorld = MathHelper.MatrixToVector(localTransB, qInB);
Vector3 w = pWorld - qWorld;
delta = Vector3.Dot(_cachedSeparatingAxis, w);
if ((delta > 0.0) && (delta * delta > squaredDistance * input.MaximumDistanceSquared))
{
checkPenetration = false;
break;
}
if (_simplexSolver.InSimplex(w))
{
_degenerateSimplex = 1;
checkSimplex = true;
break;
}
float f0 = squaredDistance - delta;
float f1 = squaredDistance * RelativeError2;
if (f0 <= f1)
{
if (f0 <= 0.0f)
{
_degenerateSimplex = 2;
}
checkSimplex = true;
break;
}
_simplexSolver.AddVertex(w, pWorld, qWorld);
if (!_simplexSolver.Closest(out _cachedSeparatingAxis))
{
_degenerateSimplex = 3;
checkSimplex = true;
break;
}
float previouseSquaredDistance = squaredDistance;
squaredDistance = _cachedSeparatingAxis.LengthSquared();
if (previouseSquaredDistance - squaredDistance <= MathHelper.Epsilon * previouseSquaredDistance)
{
_simplexSolver.BackupClosest(out _cachedSeparatingAxis);
checkSimplex = true;
break;
}
if (_currentIteration++ > gjkMaxIter)
{
#if DEBUG
Console.WriteLine("GjkPairDetector maxIter exceeded: {0}", _currentIteration);
Console.WriteLine("sepAxis=({0},{1},{2}), squaredDistance = {3}, shapeTypeA={4}, shapeTypeB={5}",
_cachedSeparatingAxis.X,
_cachedSeparatingAxis.Y,
_cachedSeparatingAxis.Z,
squaredDistance,
_minkowskiA.ShapeType,
_minkowskiB.ShapeType
);
#endif
break;
}
bool check = (!_simplexSolver.FullSimplex);
if (!check)
{
_simplexSolver.BackupClosest(out _cachedSeparatingAxis);
break;
}
}
if (checkSimplex)
{
_simplexSolver.ComputePoints(out pointOnA, out pointOnB);
normalInB = pointOnA - pointOnB;
float lenSqr = _cachedSeparatingAxis.LengthSquared();
if (lenSqr < 0.0001f)
{
_degenerateSimplex = 5;
}
if (lenSqr > MathHelper.Epsilon * MathHelper.Epsilon)
{
float rlen = 1.0f / (float)Math.Sqrt((float)lenSqr);
normalInB *= rlen;
float s = (float)Math.Sqrt((float)squaredDistance);
BulletDebug.Assert(s > 0);
pointOnA -= _cachedSeparatingAxis * (marginA / s);
pointOnB += _cachedSeparatingAxis * (marginB / s);
distance = ((1 / rlen) - margin);
isValid = true;
_lastUsedMethod = 1;
}
else
{
_lastUsedMethod = 2;
}
}
bool catchDegeneratePenetrationCase =
(_catchDegeneracies != 0 && _penetrationDepthSolver != null && _degenerateSimplex != 0 && ((distance + margin) < 0.01f));
if (checkPenetration && (!isValid || catchDegeneratePenetrationCase))
{
#warning Check this
if (_penetrationDepthSolver != null)
{
Vector3 tmpPointOnA, tmpPointOnB;
_numDeepPenetrationChecks++;
bool isValid2 = _penetrationDepthSolver.CalculatePenetrationDepth(
_simplexSolver, _minkowskiA, _minkowskiB, localTransA, localTransB,
_cachedSeparatingAxis, out tmpPointOnA, out tmpPointOnB,
debugDraw
);
if (isValid2)
{
Vector3 tmpNormalInB = tmpPointOnB - tmpPointOnA;
float lengSqr = tmpNormalInB.LengthSquared();
if (lengSqr > (MathHelper.Epsilon * MathHelper.Epsilon))
{
tmpNormalInB /= (float)Math.Sqrt((float)lengSqr);
float distance2 = -(tmpPointOnA - tmpPointOnB).Length();
if (!isValid || (distance2 < distance))
{
distance = distance2;
pointOnA = tmpPointOnA;
pointOnB = tmpPointOnB;
normalInB = tmpNormalInB;
isValid = true;
_lastUsedMethod = 3;
}
else
{
}
}
else
{
_lastUsedMethod = 4;
}
}
else
{
_lastUsedMethod = 5;
}
}
}
if (isValid)
{
output.AddContactPoint(normalInB, pointOnB + positionOffset, distance);
}
}
}
public override void GetEdge(int i, out Vector3 pa, out Vector3 pb)
{
pa = new Vector3();
pb = new Vector3();
BulletDebug.Assert(false);
}
public override bool IsInside(Vector3 pt, float tolerance)
{
BulletDebug.Assert(false);
return(false);
}
public override void GetPlane(out Vector3 planeNormal, out Vector3 planeSupport, int i)
{
planeNormal = new Vector3();
planeSupport = new Vector3();
BulletDebug.Assert(false);
}
public override void GetVertex(int i, out Vector3 vtx)
{
vtx = new Vector3();
BulletDebug.Assert(false);
}
public virtual Vector3 LocalGetSupportingVertexWithoutMargin(Vector3 vec)
{
BulletDebug.Assert(false);
return(LocalGetSupportingVertex(vec));
}
public void BuildAndProcessIslands(IDispatcher dispatcher, List <CollisionObject> collisionObjects, IIslandCallback callback)
{
//we are going to sort the unionfind array, and store the element id in the size
//afterwards, we clean unionfind, to make sure no-one uses it anymore
UnionFind.SortIslands();
int numElem = UnionFind.ElementCount;
int endIslandIndex = 1;
int startIslandIndex;
//update the sleeping state for bodies, if all are sleeping
for (startIslandIndex = 0; startIslandIndex < numElem; startIslandIndex = endIslandIndex)
{
int islandId = UnionFind[startIslandIndex].ID;
for (endIslandIndex = startIslandIndex + 1; (endIslandIndex < numElem) && (UnionFind[endIslandIndex].ID == islandId); endIslandIndex++)
{
}
//int numSleeping = 0;
bool allSleeping = true;
int idx;
for (idx = startIslandIndex; idx < endIslandIndex; idx++)
{
int i = UnionFind[idx].Size;
CollisionObject colObjA = collisionObjects[i];
if ((colObjA.IslandTag != islandId) && (colObjA.IslandTag != -1))
{
Console.WriteLine("error in island management");
}
BulletDebug.Assert((colObjA.IslandTag == islandId) || (colObjA.IslandTag == -1));
if (colObjA.IslandTag == islandId)
{
if (colObjA.ActivationState == ActivationState.Active)
{
allSleeping = false;
}
if (colObjA.ActivationState == ActivationState.DisableDeactivation)
{
allSleeping = false;
}
}
}
if (allSleeping)
{
for (idx = startIslandIndex; idx < endIslandIndex; idx++)
{
int i = UnionFind[idx].Size;
CollisionObject colObjA = collisionObjects[i];
if ((colObjA.IslandTag != islandId) && (colObjA.IslandTag != -1))
{
Console.WriteLine("error in island management");
}
BulletDebug.Assert((colObjA.IslandTag == islandId) || (colObjA.IslandTag == -1));
if (colObjA.IslandTag == islandId)
{
colObjA.ActivationState = ActivationState.IslandSleeping;
}
}
}
else
{
for (idx = startIslandIndex; idx < endIslandIndex; idx++)
{
int i = UnionFind[idx].Size;
CollisionObject colObjA = collisionObjects[i];
if ((colObjA.IslandTag != islandId) && (colObjA.IslandTag != -1))
{
Console.WriteLine("error in island management");
}
BulletDebug.Assert((colObjA.IslandTag == islandId) || (colObjA.IslandTag == -1));
if (colObjA.IslandTag == islandId)
{
if (colObjA.ActivationState == ActivationState.IslandSleeping)
{
colObjA.ActivationState = ActivationState.WantsDeactivation;
}
}
}
}
}
//int maxNumManifolds = dispatcher.ManifoldCount;
List <PersistentManifold> islandmanifold = new List <PersistentManifold>(dispatcher.ManifoldCount);
for (int i = 0; i < dispatcher.ManifoldCount; i++)
{
PersistentManifold manifold = dispatcher.GetManifoldByIndex(i);
CollisionObject colObjA = manifold.BodyA as CollisionObject;
CollisionObject colObjB = manifold.BodyB as CollisionObject;
//todo: check sleeping conditions!
if (((colObjA != null) && colObjA.ActivationState != ActivationState.IslandSleeping) ||
((colObjB != null) && colObjB.ActivationState != ActivationState.IslandSleeping))
{
//kinematic objects don't merge islands, but wake up all connected objects
if (colObjA.IsStaticOrKinematicObject && colObjA.ActivationState != ActivationState.IslandSleeping)
{
colObjB.Activate();
}
if (colObjB.IsStaticOrKinematicObject && colObjB.ActivationState != ActivationState.IslandSleeping)
{
colObjA.Activate();
}
//filtering for response
if (dispatcher.NeedsResponse(colObjA, colObjB))
{
islandmanifold.Add(manifold);
}
}
}
int numManifolds = islandmanifold.Count;
// Sort manifolds, based on islands
// Sort the vector using predicate and std::sort
islandmanifold.Sort(new Comparison <PersistentManifold>(PersistentManifoldSortPredicate));
//now process all active islands (sets of manifolds for now)
int startManifoldIndex = 0;
int endManifoldIndex = 1;
List <CollisionObject> islandBodies = new List <CollisionObject>();
for (startIslandIndex = 0; startIslandIndex < numElem; startIslandIndex = endIslandIndex)
{
int islandId = UnionFind[startIslandIndex].ID;
bool islandSleeping = false;
for (endIslandIndex = startIslandIndex; (endIslandIndex < numElem) && (UnionFind[endIslandIndex].ID == islandId); endIslandIndex++)
{
int i = UnionFind[endIslandIndex].Size;
CollisionObject colObjA = collisionObjects[i];
islandBodies.Add(colObjA);
if (!colObjA.IsActive)
{
islandSleeping = true;
}
}
//find the accompanying contact manifold for this islandId
int numIslandManifolds = 0;
List <PersistentManifold> startManifold = new List <PersistentManifold>(numIslandManifolds);
if (startManifoldIndex < numManifolds)
{
int curIslandID = GetIslandId(islandmanifold[startManifoldIndex]);
if (curIslandID == islandId)
{
for (int k = startManifoldIndex; k < islandmanifold.Count; k++)
{
startManifold.Add(islandmanifold[k]);
}
for (endManifoldIndex = startManifoldIndex + 1; (endManifoldIndex < numManifolds) && (islandId == GetIslandId(islandmanifold[endManifoldIndex])); endManifoldIndex++)
{
}
// Process the actual simulation, only if not sleeping/deactivated
numIslandManifolds = endManifoldIndex - startManifoldIndex;
}
}
if (!islandSleeping)
{
callback.ProcessIsland(islandBodies, startManifold, numIslandManifolds, islandId);
}
if (numIslandManifolds != 0)
{
startManifoldIndex = endManifoldIndex;
}
islandBodies.Clear();
}
}
