public virtual void Initialize(PersistentManifold mf, CollisionAlgorithmConstructionInfo ci, CollisionObject body0, CollisionObject body1, bool swapped)
{
base.Initialize(ci, body0, body1);
m_ownManifold = false;
m_manifoldPtr = mf;
m_swapped = swapped;
}
public virtual PersistentManifold GetNewManifold(CollisionObject b0, CollisionObject b1)
{
gNumManifold++;
CollisionObject body0 = b0;
CollisionObject body1 = b1;
//optional relative contact breaking threshold, turned on by default (use setDispatcherFlags to switch off feature for improved performance)
float contactBreakingThreshold = ((m_dispatcherFlags & DispatcherFlags.CD_USE_RELATIVE_CONTACT_BREAKING_THRESHOLD) > 0) ?
Math.Min(body0.GetCollisionShape().GetContactBreakingThreshold(BulletGlobals.gContactBreakingThreshold), body1.GetCollisionShape().GetContactBreakingThreshold(BulletGlobals.gContactBreakingThreshold))
: BulletGlobals.gContactBreakingThreshold;
float contactProcessingThreshold = Math.Min(body0.GetContactProcessingThreshold(), body1.GetContactProcessingThreshold());
// nothing in our pool so create a new one and return it.
// need a way to flush the pool ideally
PersistentManifold manifold = BulletGlobals.PersistentManifoldPool.Get();
manifold.Initialise(body0, body1, 0, contactBreakingThreshold, contactProcessingThreshold);
manifold.m_index1a = m_manifoldsPtr.Count;
m_manifoldsPtr.Add(manifold);
#if DEBUG
if (BulletGlobals.g_streamWriter != null && BulletGlobals.debugDispatcher)
{
BulletGlobals.g_streamWriter.WriteLine("GetNewManifold[{0}][{1}]", manifold.m_index1a, m_manifoldsPtr.Count);
}
#endif
return(manifold);
}
public SphereTriangleCollisionAlgorithm() { } // for pool
public SphereTriangleCollisionAlgorithm(PersistentManifold mf, CollisionAlgorithmConstructionInfo ci, CollisionObject body0, CollisionObject body1, bool swapped)
: base(ci, body0, body1)
{
m_ownManifold = false;
m_manifoldPtr = mf;
m_swapped = swapped;
}
public virtual void Initialize(PersistentManifold mf, CollisionAlgorithmConstructionInfo ci, CollisionObject body0, CollisionObject body1, bool swapped)
{
base.Initialize(ci, body0, body1);
m_ownManifold = false;
m_manifoldPtr = mf;
m_swapped = swapped;
}
} // for pool
public SphereTriangleCollisionAlgorithm(PersistentManifold mf, CollisionAlgorithmConstructionInfo ci, CollisionObject body0, CollisionObject body1, bool swapped)
: base(ci, body0, body1)
{
m_ownManifold = false;
m_manifoldPtr = mf;
m_swapped = swapped;
}
public virtual void ReleaseManifold(PersistentManifold manifold)
{
gNumManifold--;
ClearManifold(manifold);
int findIndex = manifold.m_index1a;
//m_manifoldsPtr.Remove(manifold);
m_manifoldsPtr.RemoveAtQuick(findIndex);
m_manifoldsPtr[findIndex].m_index1a = findIndex;
//PersistentManifold swapTemp = m_manifoldsPtr[findIndex];
//m_manifoldsPtr[findIndex] = m_manifoldsPtr[m_manifoldsPtr.Count - 1];
//m_manifoldsPtr[m_manifoldsPtr.Count - 1] = swapTemp;
//m_manifoldsPtr[findIndex].m_index1a = findIndex;
//m_manifoldsPtr.RemoveAt(m_manifoldsPtr.Count - 1);
#if DEBUG
if (BulletGlobals.g_streamWriter != null && BulletGlobals.debugDispatcher)
{
BulletGlobals.g_streamWriter.WriteLine("ReleaseManifold[{0}][{1}]", manifold.m_index1a, m_manifoldsPtr.Count);
}
#endif
// and return it to free list.
BulletGlobals.PersistentManifoldPool.Free(manifold);
}
public void Initialize(IDispatcher dispatcher, CollisionObject body0, CollisionObject body1, bool isSwapped)
{
m_dispatcher = dispatcher;
m_convexBody = isSwapped ? body1 : body0;
m_triBody = isSwapped ? body0 : body1;
m_manifoldPtr = m_dispatcher.GetNewManifold(m_convexBody, m_triBody);
ClearCache();
}
public ConvexTriangleCallback(IDispatcher dispatcher, CollisionObject body0, CollisionObject body1, bool isSwapped)
{
m_dispatcher = dispatcher;
m_convexBody = isSwapped ? body1 : body0;
m_triBody = isSwapped ? body0 : body1;
m_manifoldPtr = m_dispatcher.GetNewManifold(m_convexBody, m_triBody);
ClearCache();
}
protected void DestroyContactManifolds()
{
if (m_manifoldPtr == null)
{
return;
}
m_dispatcher.ReleaseManifold(m_manifoldPtr);
m_manifoldPtr = null;
}
public Box2dBox2dCollisionAlgorithm(PersistentManifold mf,CollisionAlgorithmConstructionInfo ci,CollisionObject body0,CollisionObject body1) :
base(ci,body0,body1)
{
m_ownManifold = false;
m_manifoldPtr = mf;
if (m_manifoldPtr == null && m_dispatcher.NeedsCollision(body0, body1))
{
m_manifoldPtr = m_dispatcher.GetNewManifold(body0, body1);
m_ownManifold = true;
}
}
public void Initialize(CollisionObject compoundObj, CollisionObject otherObj, IDispatcher dispatcher, DispatcherInfo dispatchInfo, ManifoldResult resultOut, CompoundCollisionAlgorithm parent, IList <CollisionAlgorithm> childCollisionAlgorithms, PersistentManifold sharedManifold)
{
m_compoundColObj = compoundObj;
m_otherObj = otherObj;
m_dispatcher = dispatcher;
m_dispatchInfo = dispatchInfo;
m_resultOut = resultOut;
m_childCollisionAlgorithms = childCollisionAlgorithms;
m_sharedManifold = sharedManifold;
m_parent = parent;
}
public Convex2dConvex2dAlgorithm(PersistentManifold mf, CollisionAlgorithmConstructionInfo ci, CollisionObject body0, CollisionObject body1, ISimplexSolverInterface simplexSolver, IConvexPenetrationDepthSolver pdSolver, int numPerturbationIterations, int minimumPointsPerturbationThreshold)
: base(ci, body0, body1)
{
m_simplexSolver = simplexSolver;
m_pdSolver = pdSolver;
m_ownManifold = false;
m_manifoldPtr = mf;
m_lowLevelOfDetail = false;
m_numPerturbationIterations = numPerturbationIterations;
m_minimumPointsPerturbationThreshold = minimumPointsPerturbationThreshold;
}
public void Initialize(PersistentManifold mf, CollisionAlgorithmConstructionInfo ci, CollisionObject body0, CollisionObject body1)
{
base.Initialize(ci);
m_ownManifold = false;
m_manifoldPtr = mf;
if (m_manifoldPtr == null && m_dispatcher.NeedsCollision(body0, body1))
{
m_manifoldPtr = m_dispatcher.GetNewManifold(body0, body1);
m_ownManifold = true;
}
}
public Convex2dConvex2dAlgorithm(PersistentManifold mf, CollisionAlgorithmConstructionInfo ci, CollisionObject body0, CollisionObject body1, ISimplexSolverInterface simplexSolver, IConvexPenetrationDepthSolver pdSolver, int numPerturbationIterations, int minimumPointsPerturbationThreshold)
: base(ci, body0, body1)
{
m_simplexSolver = simplexSolver;
m_pdSolver = pdSolver;
m_ownManifold = false;
m_manifoldPtr = mf;
m_lowLevelOfDetail = false;
m_numPerturbationIterations = numPerturbationIterations;
m_minimumPointsPerturbationThreshold = minimumPointsPerturbationThreshold;
}
public virtual void Initialize(PersistentManifold mf, CollisionAlgorithmConstructionInfo ci, CollisionObject colObj0, CollisionObject colObj1)
{
base.Initialize(ci, colObj0, colObj1);
m_ownManifold = false;
m_manifoldPtr = mf;
if (m_manifoldPtr == null)
{
m_manifoldPtr = m_dispatcher.GetNewManifold(colObj0, colObj1);
m_ownManifold = true;
}
}
public int CompareTo(object obj)
{
if (obj is PersistentManifold)
{
PersistentManifold otherManifold = (PersistentManifold)obj;
return(getIslandId(this) - getIslandId(otherManifold));
}
else
{
throw new ArgumentException("Object is not a PersistentManifold");
}
}
public virtual void Initialize(PersistentManifold mf, CollisionAlgorithmConstructionInfo ci, CollisionObject colObj0, CollisionObject colObj1)
{
base.Initialize(ci, colObj0, colObj1);
m_ownManifold = false;
m_manifoldPtr = mf;
if (m_manifoldPtr == null)
{
m_manifoldPtr = m_dispatcher.GetNewManifold(colObj0, colObj1);
m_ownManifold = true;
}
}
public override void Cleanup()
{
if (m_ownManifold)
{
if (m_manifoldPtr != null)
{
m_dispatcher.ReleaseManifold(m_manifoldPtr);
}
m_ownManifold = false;
}
m_manifoldPtr = null;
BulletGlobals.ConvexConvexAlgorithmPool.Free(this);
}
} // for pool
public SphereSphereCollisionAlgorithm(PersistentManifold mf, CollisionAlgorithmConstructionInfo ci, CollisionObject body0, CollisionObject body1)
: base(ci, body0, body1)
{
m_ownManifold = false;
m_manifoldPtr = mf;
// for some reason theres no check in 2.76 or 2.78 for a needs collision, just a check on pointer being null.
if (m_manifoldPtr == null)
{
m_manifoldPtr = m_dispatcher.GetNewManifold(body0, body1);
m_ownManifold = true;
}
}
public static int GetIslandId(PersistentManifold lhs)
{
CollisionObject rcolObj0 = lhs.GetBody0() as CollisionObject;
int islandId = rcolObj0.GetIslandTag();
if (islandId >= 0)
{
return(islandId);
}
CollisionObject rcolObj1 = lhs.GetBody1() as CollisionObject;
return(rcolObj1.GetIslandTag());
}
public SphereSphereCollisionAlgorithm() { } // for pool
public SphereSphereCollisionAlgorithm(PersistentManifold mf, CollisionAlgorithmConstructionInfo ci, CollisionObject body0, CollisionObject body1)
: base(ci, body0, body1)
{
m_ownManifold = false;
m_manifoldPtr = mf;
// for some reason theres no check in 2.76 or 2.78 for a needs collision, just a check on pointer being null.
if (m_manifoldPtr == null)
{
m_manifoldPtr = m_dispatcher.GetNewManifold(body0, body1);
m_ownManifold = true;
}
}
public void Initialize(PersistentManifold mf, CollisionAlgorithmConstructionInfo ci, CollisionObject body0, CollisionObject body1, ISimplexSolverInterface simplexSolver, IConvexPenetrationDepthSolver pdSolver, int numPerturbationIterations, int minimumPointsPerturbationThreshold)
{
base.Initialize(ci, body0, body1);
m_simplexSolver = simplexSolver;
m_pdSolver = pdSolver;
m_ownManifold = false;
m_manifoldPtr = mf;
m_lowLevelOfDetail = false;
#if USE_SEPDISTANCE_UTIL2
m_sepDistance((static_cast <btConvexShape *>(body0.getCollisionShape())).getAngularMotionDisc(),
(static_cast <btConvexShape *>(body1.getCollisionShape())).getAngularMotionDisc()),
#endif
m_numPerturbationIterations = numPerturbationIterations;
m_minimumPointsPerturbationThreshold = minimumPointsPerturbationThreshold;
}
public SphereBoxCollisionAlgorithm() { } // for pool
public SphereBoxCollisionAlgorithm(PersistentManifold mf, CollisionAlgorithmConstructionInfo ci, CollisionObject col0, CollisionObject col1, bool isSwapped)
: base(ci, col0, col1)
{
m_isSwapped = isSwapped;
m_ownManifold = false;
m_manifoldPtr = mf;
CollisionObject sphereObj = m_isSwapped ? col1 : col0;
CollisionObject boxObj = m_isSwapped ? col0 : col1;
if (m_manifoldPtr == null && m_dispatcher.NeedsCollision(sphereObj, boxObj))
{
m_manifoldPtr = m_dispatcher.GetNewManifold(sphereObj, boxObj);
m_ownManifold = true;
}
}
public ConvexConvexAlgorithm() { } // for pool
public ConvexConvexAlgorithm(PersistentManifold mf, CollisionAlgorithmConstructionInfo ci, CollisionObject body0, CollisionObject body1, ISimplexSolverInterface simplexSolver, IConvexPenetrationDepthSolver pdSolver, int numPerturbationIterations, int minimumPointsPerturbationThreshold)
: base(ci, body0, body1)
{
m_simplexSolver = simplexSolver;
m_pdSolver = pdSolver;
m_ownManifold = false;
m_manifoldPtr = mf;
m_lowLevelOfDetail = false;
#if USE_SEPDISTANCE_UTIL2
m_sepDistance ((static_cast<btConvexShape*>(body0.getCollisionShape())).getAngularMotionDisc(),
(static_cast<btConvexShape*>(body1.getCollisionShape())).getAngularMotionDisc()),
#endif
m_numPerturbationIterations = numPerturbationIterations;
m_minimumPointsPerturbationThreshold = minimumPointsPerturbationThreshold;
}
} // for pool
public SphereBoxCollisionAlgorithm(PersistentManifold mf, CollisionAlgorithmConstructionInfo ci, CollisionObject col0, CollisionObject col1, bool isSwapped)
: base(ci, col0, col1)
{
m_isSwapped = isSwapped;
m_ownManifold = false;
m_manifoldPtr = mf;
CollisionObject sphereObj = m_isSwapped ? col1 : col0;
CollisionObject boxObj = m_isSwapped ? col0 : col1;
if (m_manifoldPtr == null && m_dispatcher.NeedsCollision(sphereObj, boxObj))
{
m_manifoldPtr = m_dispatcher.GetNewManifold(sphereObj, boxObj);
m_ownManifold = true;
}
}
public void Initialize(PersistentManifold mf, CollisionAlgorithmConstructionInfo ci, CollisionObject col0, CollisionObject col1, bool isSwapped, int numPerturbationIterations, int minimumPointsPerturbationThreshold)
{
base.Initialize(ci);
m_manifoldPtr = mf;
m_ownManifold = false;
m_isSwapped = isSwapped;
m_numPerturbationIterations = numPerturbationIterations;
m_minimumPointsPerturbationThreshold = minimumPointsPerturbationThreshold;
CollisionObject convexObj = m_isSwapped ? col1 : col0;
CollisionObject planeObj = m_isSwapped ? col0 : col1;
if (m_manifoldPtr == null && m_dispatcher.NeedsCollision(convexObj, planeObj))
{
m_manifoldPtr = m_dispatcher.GetNewManifold(convexObj, planeObj);
m_ownManifold = true;
}
}
public void Initialize(PersistentManifold mf, CollisionAlgorithmConstructionInfo ci, CollisionObject col0, CollisionObject col1, bool isSwapped, int numPerturbationIterations, int minimumPointsPerturbationThreshold)
{
base.Initialize(ci);
m_manifoldPtr = mf;
m_ownManifold = false;
m_isSwapped = isSwapped;
m_numPerturbationIterations = numPerturbationIterations;
m_minimumPointsPerturbationThreshold = minimumPointsPerturbationThreshold;
CollisionObject convexObj = m_isSwapped ? col1 : col0;
CollisionObject planeObj = m_isSwapped ? col0 : col1;
if (m_manifoldPtr == null && m_dispatcher.NeedsCollision(convexObj, planeObj))
{
m_manifoldPtr = m_dispatcher.GetNewManifold(convexObj, planeObj);
m_ownManifold = true;
}
}
public void SetContactManifold(PersistentManifold contactManifold)
{
m_contactManifold = contactManifold;
}
public void SetPersistentManifold(PersistentManifold manifoldPtr)
{
m_manifoldPtr = manifoldPtr;
}
public CollisionAlgorithm FindAlgorithm(CollisionObject body0, CollisionObject body1, PersistentManifold sharedManifold)
{
CollisionAlgorithmConstructionInfo ci = new CollisionAlgorithmConstructionInfo(this, -1);
ci.SetManifold(sharedManifold);
int index1 = (int)body0.GetCollisionShape().GetShapeType();
int index2 = (int)body1.GetCollisionShape().GetShapeType();
return m_doubleDispatch[index1, index2].CreateCollisionAlgorithm(ci, body0, body1);
}
public void BuildIslands(IDispatcher dispatcher, CollisionWorld collisionWorld)
{
BulletGlobals.StartProfile("islandUnionFindAndQuickSort");
ObjectArray <CollisionObject> collisionObjects = collisionWorld.GetCollisionObjectArray();
m_islandmanifold.Clear();
//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
GetUnionFind().sortIslands();
int numElem = GetUnionFind().GetNumElements();
int endIslandIndex = 1;
//update the sleeping state for bodies, if all are sleeping
for (int startIslandIndex = 0; startIslandIndex < numElem; startIslandIndex = endIslandIndex)
{
int islandId = GetUnionFind().GetElement(startIslandIndex).m_id;
if (BulletGlobals.g_streamWriter != null && BulletGlobals.debugIslands)
{
BulletGlobals.g_streamWriter.WriteLine(String.Format("buildIslands start[{0}] end[{1}] id[{2}]", startIslandIndex, endIslandIndex, islandId));
}
for (endIslandIndex = startIslandIndex + 1; (endIslandIndex < numElem) && (GetUnionFind().GetElement(endIslandIndex).m_id == islandId); endIslandIndex++)
{
}
//int numSleeping = 0;
bool allSleeping = true;
for (int idx = startIslandIndex; idx < endIslandIndex; idx++)
{
int i = GetUnionFind().GetElement(idx).m_sz;
CollisionObject colObj0 = collisionObjects[i];
if ((colObj0.GetIslandTag() != islandId) && (colObj0.GetIslandTag() != -1))
{
// printf("error in island management\n");
}
Debug.Assert((colObj0.GetIslandTag() == islandId) || (colObj0.GetIslandTag() == -1));
if (colObj0.GetIslandTag() == islandId)
{
if (colObj0.GetActivationState() == ActivationState.ACTIVE_TAG)
{
allSleeping = false;
}
if (colObj0.GetActivationState() == ActivationState.DISABLE_DEACTIVATION)
{
allSleeping = false;
}
}
}
if (allSleeping)
{
for (int idx = startIslandIndex; idx < endIslandIndex; idx++)
{
int i = GetUnionFind().GetElement(idx).m_sz;
CollisionObject colObj0 = collisionObjects[i];
int islandTag = colObj0.GetIslandTag();
if (islandTag != islandId && islandTag != -1)
{
// printf("error in island management\n");
}
Debug.Assert((islandTag == islandId) || (islandTag == -1));
if (islandTag == islandId)
{
colObj0.SetActivationState(ActivationState.ISLAND_SLEEPING);
}
}
}
else
{
for (int idx = startIslandIndex; idx < endIslandIndex; idx++)
{
int i = GetUnionFind().GetElement(idx).m_sz;
CollisionObject colObj0 = collisionObjects[i];
int islandTag = colObj0.GetIslandTag();
if (islandTag != islandId && islandTag != -1)
{
// printf("error in island management\n");
}
Debug.Assert((islandTag == islandId) || (islandTag == -1));
if (islandTag == islandId)
{
if (colObj0.GetActivationState() == ActivationState.ISLAND_SLEEPING)
{
colObj0.SetActivationState(ActivationState.WANTS_DEACTIVATION);
colObj0.SetDeactivationTime(0f);
}
}
}
}
}
int maxNumManifolds = dispatcher.GetNumManifolds();
//#definef SPLIT_ISLANDS 1
//#ifdef SPLIT_ISLANDS
//#endif //SPLIT_ISLANDS
for (int i = 0; i < maxNumManifolds; i++)
{
PersistentManifold manifold = dispatcher.GetManifoldByIndexInternal(i);
CollisionObject colObj0 = manifold.GetBody0() as CollisionObject;
CollisionObject colObj1 = manifold.GetBody1() as CollisionObject;
///@todo: check sleeping conditions!
if (((colObj0 != null) && colObj0.GetActivationState() != ActivationState.ISLAND_SLEEPING) ||
((colObj1 != null) && colObj1.GetActivationState() != ActivationState.ISLAND_SLEEPING))
{
//kinematic objects don't merge islands, but wake up all connected objects
if (colObj0.IsKinematicObject() && colObj0.GetActivationState() != ActivationState.ISLAND_SLEEPING)
{
if (colObj0.HasContactResponse())
{
colObj1.Activate();
}
}
if (colObj1.IsKinematicObject() && colObj1.GetActivationState() != ActivationState.ISLAND_SLEEPING)
{
if (colObj1.HasContactResponse())
{
colObj0.Activate();
}
}
if (m_splitIslands)
{
//filtering for response
if (dispatcher.NeedsResponse(colObj0, colObj1))
{
m_islandmanifold.Add(manifold);
}
}
}
}
BulletGlobals.StopProfile();
}
public void BuildAndProcessIslands(IDispatcher dispatcher, CollisionWorld collisionWorld, IIslandCallback callback)
{
ObjectArray <CollisionObject> collisionObjects = collisionWorld.GetCollisionObjectArray();
BuildIslands(dispatcher, collisionWorld);
int endIslandIndex = 1;
int startIslandIndex;
int numElem = GetUnionFind().GetNumElements();
BulletGlobals.StartProfile("processIslands");
if (!m_splitIslands)
{
PersistentManifoldArray manifolds = dispatcher.GetInternalManifoldPointer();
int maxNumManifolds = dispatcher.GetNumManifolds();
callback.ProcessIsland(collisionObjects, collisionObjects.Count, manifolds, 0, maxNumManifolds, -1);
}
else
{
// Sort manifolds, based on islands
// Sort the vector using predicate and std::sort
//std::sort(islandmanifold.begin(), islandmanifold.end(), btPersistentManifoldSortPredicate);
int numManifolds = m_islandmanifold.Count;
//we should do radix sort, it it much faster (O(n) instead of O (n log2(n))
m_islandmanifold.QuickSort(m_sortPredicate);
//now process all active islands (sets of manifolds for now)
int startManifoldIndex = 0;
int endManifoldIndex = 1;
//traverse the simulation islands, and call the solver, unless all objects are sleeping/deactivated
for (startIslandIndex = 0; startIslandIndex < numElem; startIslandIndex = endIslandIndex)
{
int islandId = GetUnionFind().GetElement(startIslandIndex).m_id;
if (BulletGlobals.g_streamWriter != null && BulletGlobals.debugIslands)
{
BulletGlobals.g_streamWriter.WriteLine(String.Format("buildAndProcessIslands start[{0}] end[{1}] id[{2}]", startIslandIndex, endIslandIndex, islandId));
}
bool islandSleeping = true;
for (endIslandIndex = startIslandIndex; (endIslandIndex < numElem) && (GetUnionFind().GetElement(endIslandIndex).m_id == islandId); endIslandIndex++)
{
int i = GetUnionFind().GetElement(endIslandIndex).m_sz;
CollisionObject colObj0 = collisionObjects[i];
m_islandBodies.Add(colObj0);
if (colObj0.IsActive())
{
islandSleeping = false;
}
}
//find the accompanying contact manifold for this islandId
int numIslandManifolds = 0;
PersistentManifold startManifold = null;
if (startManifoldIndex < numManifolds)
{
int curIslandId = GetIslandId(m_islandmanifold[startManifoldIndex]);
if (BulletGlobals.g_streamWriter != null && BulletGlobals.debugIslands)
{
BulletGlobals.g_streamWriter.WriteLine("curIsland[{0}] startManifold[{1}].", curIslandId, startManifoldIndex);
}
if (curIslandId == islandId)
{
startManifold = m_islandmanifold[startManifoldIndex];
for (endManifoldIndex = startManifoldIndex + 1; (endManifoldIndex < numManifolds) && (islandId == GetIslandId(m_islandmanifold[endManifoldIndex])); endManifoldIndex++)
{
if (BulletGlobals.g_streamWriter != null && BulletGlobals.debugIslands)
{
BulletGlobals.g_streamWriter.WriteLine("endManifoldIndex[{0}] islandId[{1}] getIsland[{2}].", endManifoldIndex, startManifoldIndex, GetIslandId(m_islandmanifold[endManifoldIndex]));
}
}
/// Process the actual simulation, only if not sleeping/deactivated
numIslandManifolds = endManifoldIndex - startManifoldIndex;
}
}
if (!islandSleeping)
{
callback.ProcessIsland(m_islandBodies, m_islandBodies.Count, m_islandmanifold, startManifoldIndex, numIslandManifolds, islandId);
// printf("Island callback of size:%d bodies, %d manifolds\n",islandBodies.size(),numIslandManifolds);
}
else
{
if (BulletGlobals.g_streamWriter != null && BulletGlobals.debugIslands)
{
BulletGlobals.g_streamWriter.WriteLine("islandSleeping.");
}
}
if (numIslandManifolds != 0)
{
startManifoldIndex = endManifoldIndex;
}
m_islandBodies.Clear();
}
} // else if(!splitIslands)
BulletGlobals.StopProfile();
}
protected void ConvertContact(PersistentManifold manifold, ContactSolverInfo infoGlobal)
{
CollisionObject colObj0 = null, colObj1 = null;
colObj0 = manifold.GetBody0() as CollisionObject;
colObj1 = manifold.GetBody1() as CollisionObject;
RigidBody solverBodyA = colObj0 as RigidBody;//RigidBody.Upcast(colObj0);
RigidBody solverBodyB = colObj1 as RigidBody;//RigidBody.Upcast(colObj1);
///avoid collision response between two static objects
if ((solverBodyA == null || solverBodyA.GetInvMass() == 0f) && (solverBodyB == null || solverBodyB.GetInvMass() == 0f))
{
return;
}
for (int j = 0; j < manifold.GetNumContacts(); j++)
{
ManifoldPoint cp = manifold.GetContactPoint(j);
#if DEBUG
if (BulletGlobals.g_streamWriter != null && BulletGlobals.debugSolver && false)
{
String nameA = solverBodyA != null ? (String)solverBodyA.GetUserPointer() : "Null";
String nameB = solverBodyA != null ? (String)solverBodyB.GetUserPointer() : "Null";
BulletGlobals.g_streamWriter.WriteLine("ConvertContact [{0}][{1}][{2}][{3}][{4}]", j, nameA,nameB, cp.GetDistance() ,manifold.GetContactProcessingThreshold());
MathUtil.PrintContactPoint(BulletGlobals.g_streamWriter, cp);
}
#endif
if (cp.GetDistance() <= manifold.GetContactProcessingThreshold())
{
// IndexedVector3 pos1 = cp.getPositionWorldOnA();
// IndexedVector3 pos2 = cp.getPositionWorldOnB();
IndexedVector3 rel_pos1;
IndexedVector3 rel_pos2;
//;
float relaxation = 1f;
float rel_vel = 0f;
IndexedVector3 vel = IndexedVector3.Zero;
int frictionIndex = m_tmpSolverContactConstraintPool.Count;
// will create if needed.
SolverConstraint solverConstraint = m_tmpSolverContactConstraintPool[m_tmpSolverContactConstraintPool.Count];
solverConstraint.Reset();
RigidBody rb0 = solverBodyA;//RigidBody.Upcast(colObj0);
RigidBody rb1 = solverBodyB;//RigidBody.Upcast(colObj1);
solverConstraint.m_solverBodyA = rb0 != null ? rb0 : GetFixedBody();
solverConstraint.m_solverBodyB = rb1 != null ? rb1 : GetFixedBody();
solverConstraint.m_originalContactPoint = cp;
#if DEBUG
if (BulletGlobals.g_streamWriter != null && rb0 != null && BulletGlobals.debugSolver)
{
BulletGlobals.g_streamWriter.WriteLine("ConvertContact [{0}][{1}]", (String)solverConstraint.m_solverBodyA.GetUserPointer(), (String)solverConstraint.m_solverBodyB.GetUserPointer());
MathUtil.PrintContactPoint(BulletGlobals.g_streamWriter, cp);
}
#endif
SetupContactConstraint(ref solverConstraint, colObj0, colObj1, cp, infoGlobal, ref vel, ref rel_vel, ref relaxation, out rel_pos1, out rel_pos2);
#if DEBUG
if (BulletGlobals.g_streamWriter != null && BulletGlobals.debugSolver)
{
TypedConstraint.PrintSolverConstraint(BulletGlobals.g_streamWriter, solverConstraint, 99);
}
#endif
/////setup the friction constraints
solverConstraint.m_frictionIndex = m_tmpSolverContactFrictionConstraintPool.Count;
if (!(TestSolverMode(infoGlobal.m_solverMode, SolverMode.SOLVER_ENABLE_FRICTION_DIRECTION_CACHING)) || !cp.GetLateralFrictionInitialized())
{
cp.m_lateralFrictionDir1 = vel - cp.m_normalWorldOnB * rel_vel;
float lat_rel_vel = cp.m_lateralFrictionDir1.LengthSquared();
if (!TestSolverMode(infoGlobal.m_solverMode, SolverMode.SOLVER_DISABLE_VELOCITY_DEPENDENT_FRICTION_DIRECTION) && lat_rel_vel > MathUtil.SIMD_EPSILON)
{
cp.m_lateralFrictionDir1 /= (float)Math.Sqrt(lat_rel_vel);
if (TestSolverMode(infoGlobal.m_solverMode, SolverMode.SOLVER_USE_2_FRICTION_DIRECTIONS))
{
cp.m_lateralFrictionDir2 = IndexedVector3.Cross(cp.m_lateralFrictionDir1, cp.m_normalWorldOnB);
cp.m_lateralFrictionDir2.Normalize();//??
ApplyAnisotropicFriction(colObj0, ref cp.m_lateralFrictionDir2);
ApplyAnisotropicFriction(colObj1, ref cp.m_lateralFrictionDir2);
AddFrictionConstraint(ref cp.m_lateralFrictionDir2, solverBodyA, solverBodyB, frictionIndex, cp, ref rel_pos1, ref rel_pos2, colObj0, colObj1, relaxation, 0f, 0f);
}
ApplyAnisotropicFriction(colObj0, ref cp.m_lateralFrictionDir1);
ApplyAnisotropicFriction(colObj1, ref cp.m_lateralFrictionDir1);
AddFrictionConstraint(ref cp.m_lateralFrictionDir1, solverBodyA, solverBodyB, frictionIndex, cp, ref rel_pos1, ref rel_pos2, colObj0, colObj1, relaxation, 0f, 0f);
cp.m_lateralFrictionInitialized = true;
}
else
{
//re-calculate friction direction every frame, todo: check if this is really needed
IndexedVector3 temp = cp.m_normalWorldOnB;
TransformUtil.PlaneSpace1(ref temp, out cp.m_lateralFrictionDir1, out cp.m_lateralFrictionDir2);
if (TestSolverMode(infoGlobal.m_solverMode, SolverMode.SOLVER_USE_2_FRICTION_DIRECTIONS))
{
ApplyAnisotropicFriction(colObj0, ref cp.m_lateralFrictionDir2);
ApplyAnisotropicFriction(colObj1, ref cp.m_lateralFrictionDir2);
AddFrictionConstraint(ref cp.m_lateralFrictionDir2, solverBodyA, solverBodyB, frictionIndex, cp, ref rel_pos1, ref rel_pos2, colObj0, colObj1, relaxation, 0f, 0f);
}
ApplyAnisotropicFriction(colObj0, ref cp.m_lateralFrictionDir1);
ApplyAnisotropicFriction(colObj1, ref cp.m_lateralFrictionDir1);
AddFrictionConstraint(ref cp.m_lateralFrictionDir1, solverBodyA, solverBodyB, frictionIndex, cp, ref rel_pos1, ref rel_pos2, colObj0, colObj1, relaxation, 0f, 0f);
cp.m_lateralFrictionInitialized = true;
}
}
else
{
AddFrictionConstraint(ref cp.m_lateralFrictionDir1, solverBodyA, solverBodyB, frictionIndex, cp, ref rel_pos1, ref rel_pos2, colObj0, colObj1, relaxation, cp.m_contactMotion1, cp.m_contactCFM1);
if (TestSolverMode(infoGlobal.m_solverMode, SolverMode.SOLVER_USE_2_FRICTION_DIRECTIONS))
{
AddFrictionConstraint(ref cp.m_lateralFrictionDir2, solverBodyA, solverBodyB, frictionIndex, cp, ref rel_pos1, ref rel_pos2, colObj0, colObj1, relaxation, cp.m_contactMotion2, cp.m_contactCFM2);
}
}
SetFrictionConstraintImpulse(ref solverConstraint, rb0, rb1, cp, infoGlobal);
}
}
}
public override void ProcessCollision(CollisionObject body0, CollisionObject body1, DispatcherInfo dispatchInfo, ManifoldResult resultOut)
{
if (m_manifoldPtr == null)
{
//swapped?
m_manifoldPtr = m_dispatcher.GetNewManifold(body0, body1);
m_ownManifold = true;
}
//resultOut = new ManifoldResult();
resultOut.SetPersistentManifold(m_manifoldPtr);
//comment-out next line to test multi-contact generation
//resultOut.GetPersistentManifold().ClearManifold();
ConvexShape min0 = body0.GetCollisionShape() as ConvexShape;
ConvexShape min1 = body1.GetCollisionShape() as ConvexShape;
IndexedVector3 normalOnB;
IndexedVector3 pointOnBWorld;
#if !BT_DISABLE_CAPSULE_CAPSULE_COLLIDER
if ((min0.GetShapeType() == BroadphaseNativeTypes.CAPSULE_SHAPE_PROXYTYPE) && (min1.GetShapeType() == BroadphaseNativeTypes.CAPSULE_SHAPE_PROXYTYPE))
{
CapsuleShape capsuleA = min0 as CapsuleShape;
CapsuleShape capsuleB = min1 as CapsuleShape;
//IndexedVector3 localScalingA = capsuleA.GetLocalScaling();
//IndexedVector3 localScalingB = capsuleB.GetLocalScaling();
float threshold = m_manifoldPtr.GetContactBreakingThreshold();
float dist = CapsuleCapsuleDistance(out normalOnB, out pointOnBWorld, capsuleA.GetHalfHeight(), capsuleA.GetRadius(),
capsuleB.GetHalfHeight(), capsuleB.GetRadius(), capsuleA.GetUpAxis(), capsuleB.GetUpAxis(),
body0.GetWorldTransform(), body1.GetWorldTransform(), threshold);
if (dist < threshold)
{
Debug.Assert(normalOnB.LengthSquared() >= (MathUtil.SIMD_EPSILON * MathUtil.SIMD_EPSILON));
resultOut.AddContactPoint(ref normalOnB, ref pointOnBWorld, dist);
}
resultOut.RefreshContactPoints();
return;
}
#endif //BT_DISABLE_CAPSULE_CAPSULE_COLLIDER
#if USE_SEPDISTANCE_UTIL2
if (dispatchInfo.m_useConvexConservativeDistanceUtil)
{
m_sepDistance.updateSeparatingDistance(body0.getWorldTransform(), body1.getWorldTransform());
}
if (!dispatchInfo.m_useConvexConservativeDistanceUtil || m_sepDistance.getConservativeSeparatingDistance() <= 0.f)
#endif //USE_SEPDISTANCE_UTIL2
{
ClosestPointInput input = ClosestPointInput.Default();
using (GjkPairDetector gjkPairDetector = BulletGlobals.GjkPairDetectorPool.Get())
{
gjkPairDetector.Initialize(min0, min1, m_simplexSolver, m_pdSolver);
//TODO: if (dispatchInfo.m_useContinuous)
gjkPairDetector.SetMinkowskiA(min0);
gjkPairDetector.SetMinkowskiB(min1);
#if USE_SEPDISTANCE_UTIL2
if (dispatchInfo.m_useConvexConservativeDistanceUtil)
{
input.m_maximumDistanceSquared = float.MaxValue;
}
else
#endif //USE_SEPDISTANCE_UTIL2
{
input.m_maximumDistanceSquared = min0.GetMargin() + min1.GetMargin() + m_manifoldPtr.GetContactBreakingThreshold();
input.m_maximumDistanceSquared *= input.m_maximumDistanceSquared;
}
//input.m_stackAlloc = dispatchInfo.m_stackAllocator;
input.m_transformA = body0.GetWorldTransform();
input.m_transformB = body1.GetWorldTransform();
if (min0.IsPolyhedral() && min1.IsPolyhedral())
{
DummyResult dummy = new DummyResult();
PolyhedralConvexShape polyhedronA = min0 as PolyhedralConvexShape;
PolyhedralConvexShape polyhedronB = min1 as PolyhedralConvexShape;
if (polyhedronA.GetConvexPolyhedron() != null && polyhedronB.GetConvexPolyhedron() != null)
{
float threshold = m_manifoldPtr.GetContactBreakingThreshold();
float minDist = float.MinValue;
IndexedVector3 sepNormalWorldSpace = new IndexedVector3(0, 1, 0);
bool foundSepAxis = true;
if (dispatchInfo.m_enableSatConvex)
{
foundSepAxis = PolyhedralContactClipping.FindSeparatingAxis(
polyhedronA.GetConvexPolyhedron(), polyhedronB.GetConvexPolyhedron(),
body0.GetWorldTransform(),
body1.GetWorldTransform(),
out sepNormalWorldSpace);
}
else
{
#if ZERO_MARGIN
gjkPairDetector.SetIgnoreMargin(true);
gjkPairDetector.GetClosestPoints(input, resultOut, dispatchInfo.m_debugDraw);
#else
gjkPairDetector.GetClosestPoints(ref input, dummy, dispatchInfo.m_debugDraw);
#endif
float l2 = gjkPairDetector.GetCachedSeparatingAxis().LengthSquared();
if (l2 > MathUtil.SIMD_EPSILON)
{
sepNormalWorldSpace = gjkPairDetector.GetCachedSeparatingAxis() * (1.0f / l2);
//minDist = -1e30f;//gjkPairDetector.getCachedSeparatingDistance();
minDist = gjkPairDetector.GetCachedSeparatingDistance() - min0.GetMargin() - min1.GetMargin();
#if ZERO_MARGIN
foundSepAxis = true; //gjkPairDetector.getCachedSeparatingDistance()<0.f;
#else
foundSepAxis = gjkPairDetector.GetCachedSeparatingDistance() < (min0.GetMargin() + min1.GetMargin());
#endif
}
}
if (foundSepAxis)
{
// printf("sepNormalWorldSpace=%f,%f,%f\n",sepNormalWorldSpace.getX(),sepNormalWorldSpace.getY(),sepNormalWorldSpace.getZ());
PolyhedralContactClipping.ClipHullAgainstHull(sepNormalWorldSpace, polyhedronA.GetConvexPolyhedron(), polyhedronB.GetConvexPolyhedron(),
body0.GetWorldTransform(),
body1.GetWorldTransform(), minDist - threshold, threshold, resultOut);
}
if (m_ownManifold)
{
resultOut.RefreshContactPoints();
}
return;
}
else
{
//we can also deal with convex versus triangle (without connectivity data)
if (polyhedronA.GetConvexPolyhedron() != null && polyhedronB.GetShapeType() == BroadphaseNativeTypes.TRIANGLE_SHAPE_PROXYTYPE)
{
m_vertices.Clear();
TriangleShape tri = polyhedronB as TriangleShape;
m_vertices.Add(body1.GetWorldTransform() * tri.m_vertices1[0]);
m_vertices.Add(body1.GetWorldTransform() * tri.m_vertices1[1]);
m_vertices.Add(body1.GetWorldTransform() * tri.m_vertices1[2]);
float threshold = m_manifoldPtr.GetContactBreakingThreshold();
IndexedVector3 sepNormalWorldSpace = new IndexedVector3(0, 1, 0);;
float minDist = float.MinValue;
float maxDist = threshold;
bool foundSepAxis = false;
if (false)
{
polyhedronB.InitializePolyhedralFeatures();
foundSepAxis = PolyhedralContactClipping.FindSeparatingAxis(
polyhedronA.GetConvexPolyhedron(), polyhedronB.GetConvexPolyhedron(),
body0.GetWorldTransform(),
body1.GetWorldTransform(),
out sepNormalWorldSpace);
// printf("sepNormalWorldSpace=%f,%f,%f\n",sepNormalWorldSpace.getX(),sepNormalWorldSpace.getY(),sepNormalWorldSpace.getZ());
}
else
{
#if ZERO_MARGIN
gjkPairDetector.SetIgnoreMargin(true);
gjkPairDetector.GetClosestPoints(input, resultOut, dispatchInfo.m_debugDraw);
#else
gjkPairDetector.GetClosestPoints(ref input, dummy, dispatchInfo.m_debugDraw);
#endif//ZERO_MARGIN
float l2 = gjkPairDetector.GetCachedSeparatingAxis().LengthSquared();
if (l2 > MathUtil.SIMD_EPSILON)
{
sepNormalWorldSpace = gjkPairDetector.GetCachedSeparatingAxis() * (1.0f / l2);
//minDist = gjkPairDetector.getCachedSeparatingDistance();
//maxDist = threshold;
minDist = gjkPairDetector.GetCachedSeparatingDistance() - min0.GetMargin() - min1.GetMargin();
foundSepAxis = true;
}
}
if (foundSepAxis)
{
PolyhedralContactClipping.ClipFaceAgainstHull(sepNormalWorldSpace, polyhedronA.GetConvexPolyhedron(),
body0.GetWorldTransform(), m_vertices, minDist - threshold, maxDist, resultOut);
}
if (m_ownManifold)
{
resultOut.RefreshContactPoints();
}
return;
}
}
}
gjkPairDetector.GetClosestPoints(ref input, resultOut, dispatchInfo.getDebugDraw(), false);
#if USE_SEPDISTANCE_UTIL2
float sepDist = 0.f;
if (dispatchInfo.m_useConvexConservativeDistanceUtil)
{
sepDist = gjkPairDetector.getCachedSeparatingDistance();
if (sepDist > MathUtil.SIMD_EPSILON)
{
sepDist += dispatchInfo.m_convexConservativeDistanceThreshold;
//now perturbe directions to get multiple contact points
}
}
#endif //USE_SEPDISTANCE_UTIL2
//now perform 'm_numPerturbationIterations' collision queries with the perturbated collision objects
//perform perturbation when more then 'm_minimumPointsPerturbationThreshold' points
if (m_numPerturbationIterations > 0 && resultOut.GetPersistentManifold().GetNumContacts() < m_minimumPointsPerturbationThreshold)
{
IndexedVector3 v0, v1;
IndexedVector3 sepNormalWorldSpace = gjkPairDetector.GetCachedSeparatingAxis();
sepNormalWorldSpace.Normalize();
TransformUtil.PlaneSpace1(ref sepNormalWorldSpace, out v0, out v1);
bool perturbeA = true;
const float angleLimit = 0.125f * MathUtil.SIMD_PI;
float perturbeAngle;
float radiusA = min0.GetAngularMotionDisc();
float radiusB = min1.GetAngularMotionDisc();
if (radiusA < radiusB)
{
perturbeAngle = BulletGlobals.gContactBreakingThreshold / radiusA;
perturbeA = true;
}
else
{
perturbeAngle = BulletGlobals.gContactBreakingThreshold / radiusB;
perturbeA = false;
}
if (perturbeAngle > angleLimit)
{
perturbeAngle = angleLimit;
}
IndexedMatrix unPerturbedTransform;
if (perturbeA)
{
unPerturbedTransform = input.m_transformA;
}
else
{
unPerturbedTransform = input.m_transformB;
}
for (int i = 0; i < m_numPerturbationIterations; i++)
{
if (v0.LengthSquared() > MathUtil.SIMD_EPSILON)
{
IndexedQuaternion perturbeRot = new IndexedQuaternion(v0, perturbeAngle);
float iterationAngle = i * (MathUtil.SIMD_2_PI / (float)m_numPerturbationIterations);
IndexedQuaternion rotq = new IndexedQuaternion(sepNormalWorldSpace, iterationAngle);
if (perturbeA)
{
input.m_transformA._basis = (new IndexedBasisMatrix(MathUtil.QuaternionInverse(rotq) * perturbeRot * rotq) * body0.GetWorldTransform()._basis);
input.m_transformB = body1.GetWorldTransform();
input.m_transformB = body1.GetWorldTransform();
#if DEBUG_CONTACTS
dispatchInfo.m_debugDraw.DrawTransform(ref input.m_transformA, 10.0f);
#endif //DEBUG_CONTACTS
}
else
{
input.m_transformA = body0.GetWorldTransform();
input.m_transformB._basis = (new IndexedBasisMatrix(MathUtil.QuaternionInverse(rotq) * perturbeRot * rotq) * body1.GetWorldTransform()._basis);
#if DEBUG_CONTACTS
dispatchInfo.m_debugDraw.DrawTransform(ref input.m_transformB, 10.0f);
#endif
}
PerturbedContactResult perturbedResultOut = new PerturbedContactResult(resultOut, ref input.m_transformA, ref input.m_transformB, ref unPerturbedTransform, perturbeA, dispatchInfo.getDebugDraw());
gjkPairDetector.GetClosestPoints(ref input, perturbedResultOut, dispatchInfo.getDebugDraw(), false);
}
}
}
#if USE_SEPDISTANCE_UTIL2
if (dispatchInfo.m_useConvexConservativeDistanceUtil && (sepDist > MathUtil.SIMD_EPSILON))
{
m_sepDistance.initSeparatingDistance(gjkPairDetector.getCachedSeparatingAxis(), sepDist, body0.getWorldTransform(), body1.getWorldTransform());
}
#endif //USE_SEPDISTANCE_UTIL2
}
}
if (m_ownManifold)
{
resultOut.RefreshContactPoints();
}
}
} // for pool
//! Creates a new contact point
protected PersistentManifold NewContactManifold(CollisionObject body0, CollisionObject body1)
{
m_manifoldPtr = m_dispatcher.GetNewManifold(body0, body1);
return(m_manifoldPtr);
}
//public int GetDispatcherId()
//{
// return 0;
//}
public void SetManifold(PersistentManifold manifold)
{
m_manifold = manifold;
}
protected void DestroyContactManifolds()
{
if (m_manifoldPtr == null)
{
return;
}
m_dispatcher.ReleaseManifold(m_manifoldPtr);
m_manifoldPtr = null;
}
public CollisionAlgorithm FindAlgorithm(CollisionObject body0, CollisionObject body1, PersistentManifold sharedManifold)
{
CollisionAlgorithmConstructionInfo ci = new CollisionAlgorithmConstructionInfo(this, -1);
ci.SetManifold(sharedManifold);
int index1 = (int)body0.GetCollisionShape().GetShapeType();
int index2 = (int)body1.GetCollisionShape().GetShapeType();
return(m_doubleDispatch[index1, index2].CreateCollisionAlgorithm(ci, body0, body1));
}
public void Initialize(CollisionObject compoundObj, CollisionObject otherObj, IDispatcher dispatcher, DispatcherInfo dispatchInfo, ManifoldResult resultOut, CompoundCollisionAlgorithm parent, IList<CollisionAlgorithm> childCollisionAlgorithms, PersistentManifold sharedManifold)
{
m_compoundColObj = compoundObj;
m_otherObj = otherObj;
m_dispatcher = dispatcher;
m_dispatchInfo = dispatchInfo;
m_resultOut = resultOut;
m_childCollisionAlgorithms = childCollisionAlgorithms;
m_sharedManifold = sharedManifold;
m_parent = parent;
}
private static int getIslandId(PersistentManifold lhs)
{
CollisionObject rcolObj0 = lhs.GetBody0() as CollisionObject;
int islandId = rcolObj0.GetIslandTag();
if (islandId >= 0)
return rcolObj0.GetIslandTag();
CollisionObject rcolObj1 = lhs.GetBody1() as CollisionObject;
return rcolObj1.GetIslandTag();
}
public virtual void ReleaseManifold(PersistentManifold manifold)
{
gNumManifold--;
ClearManifold(manifold);
int findIndex = manifold.m_index1a;
//m_manifoldsPtr.Remove(manifold);
m_manifoldsPtr.RemoveAtQuick(findIndex);
m_manifoldsPtr[findIndex].m_index1a = findIndex;
//PersistentManifold swapTemp = m_manifoldsPtr[findIndex];
//m_manifoldsPtr[findIndex] = m_manifoldsPtr[m_manifoldsPtr.Count - 1];
//m_manifoldsPtr[m_manifoldsPtr.Count - 1] = swapTemp;
//m_manifoldsPtr[findIndex].m_index1a = findIndex;
//m_manifoldsPtr.RemoveAt(m_manifoldsPtr.Count - 1);
#if DEBUG
if (BulletGlobals.g_streamWriter != null && BulletGlobals.debugDispatcher)
{
BulletGlobals.g_streamWriter.WriteLine("ReleaseManifold[{0}][{1}]", manifold.m_index1a, m_manifoldsPtr.Count);
}
#endif
// and return it to free list.
BulletGlobals.PersistentManifoldPool.Free(manifold);
}
public virtual void ClearManifold(PersistentManifold manifold)
{
manifold.ClearManifold();
}
public ContactConstraint(PersistentManifold contactManifold, RigidBody rbA, RigidBody rbB)
: base(TypedConstraintType.CONTACT_CONSTRAINT_TYPE, rbA, rbB)
{
m_contactManifold = contactManifold;
}
public void SetPersistentManifold(PersistentManifold manifoldPtr)
{
m_manifoldPtr = manifoldPtr;
}
public ConvexTriangleCallback(IDispatcher dispatcher, CollisionObject body0, CollisionObject body1, bool isSwapped)
{
m_dispatcher = dispatcher;
m_convexBody = isSwapped ? body1 : body0;
m_triBody = isSwapped ? body0 : body1;
m_manifoldPtr = m_dispatcher.GetNewManifold(m_convexBody, m_triBody);
ClearCache();
}
public GImpactCollisionAlgorithm() { } // for pool
//! Creates a new contact point
protected PersistentManifold NewContactManifold(CollisionObject body0, CollisionObject body1)
{
m_manifoldPtr = m_dispatcher.GetNewManifold(body0, body1);
return m_manifoldPtr;
}
public void Initialize(IDispatcher dispatcher, CollisionObject body0, CollisionObject body1, bool isSwapped)
{
m_dispatcher = dispatcher;
m_convexBody = isSwapped ? body1 : body0;
m_triBody = isSwapped ? body0 : body1;
m_manifoldPtr = m_dispatcher.GetNewManifold(m_convexBody, m_triBody);
ClearCache();
}
public override void ProcessCollision(CollisionObject body0, CollisionObject body1, DispatcherInfo dispatchInfo, ManifoldResult resultOut)
{
if (m_manifoldPtr == null)
{
//swapped?
m_manifoldPtr = m_dispatcher.GetNewManifold(body0, body1);
m_ownManifold = true;
}
//resultOut = new ManifoldResult();
resultOut.SetPersistentManifold(m_manifoldPtr);
//comment-out next line to test multi-contact generation
//resultOut.GetPersistentManifold().ClearManifold();
ConvexShape min0 = body0.GetCollisionShape() as ConvexShape;
ConvexShape min1 = body1.GetCollisionShape() as ConvexShape;
IndexedVector3 normalOnB;
IndexedVector3 pointOnBWorld;
#if !BT_DISABLE_CAPSULE_CAPSULE_COLLIDER
if ((min0.GetShapeType() == BroadphaseNativeTypes.CAPSULE_SHAPE_PROXYTYPE) && (min1.GetShapeType() == BroadphaseNativeTypes.CAPSULE_SHAPE_PROXYTYPE))
{
CapsuleShape capsuleA = min0 as CapsuleShape;
CapsuleShape capsuleB = min1 as CapsuleShape;
//IndexedVector3 localScalingA = capsuleA.GetLocalScaling();
//IndexedVector3 localScalingB = capsuleB.GetLocalScaling();
float threshold = m_manifoldPtr.GetContactBreakingThreshold();
float dist = CapsuleCapsuleDistance(out normalOnB, out pointOnBWorld, capsuleA.GetHalfHeight(), capsuleA.GetRadius(),
capsuleB.GetHalfHeight(), capsuleB.GetRadius(), capsuleA.GetUpAxis(), capsuleB.GetUpAxis(),
body0.GetWorldTransform(), body1.GetWorldTransform(), threshold);
if (dist < threshold)
{
Debug.Assert(normalOnB.LengthSquared() >= (MathUtil.SIMD_EPSILON * MathUtil.SIMD_EPSILON));
resultOut.AddContactPoint(ref normalOnB, ref pointOnBWorld, dist);
}
resultOut.RefreshContactPoints();
return;
}
#endif //BT_DISABLE_CAPSULE_CAPSULE_COLLIDER
#if USE_SEPDISTANCE_UTIL2
if (dispatchInfo.m_useConvexConservativeDistanceUtil)
{
m_sepDistance.updateSeparatingDistance(body0.getWorldTransform(),body1.getWorldTransform());
}
if (!dispatchInfo.m_useConvexConservativeDistanceUtil || m_sepDistance.getConservativeSeparatingDistance()<=0.f)
#endif //USE_SEPDISTANCE_UTIL2
{
ClosestPointInput input = ClosestPointInput.Default();
using (GjkPairDetector gjkPairDetector = BulletGlobals.GjkPairDetectorPool.Get())
{
gjkPairDetector.Initialize(min0, min1, m_simplexSolver, m_pdSolver);
//TODO: if (dispatchInfo.m_useContinuous)
gjkPairDetector.SetMinkowskiA(min0);
gjkPairDetector.SetMinkowskiB(min1);
#if USE_SEPDISTANCE_UTIL2
if (dispatchInfo.m_useConvexConservativeDistanceUtil)
{
input.m_maximumDistanceSquared = float.MaxValue;
}
else
#endif //USE_SEPDISTANCE_UTIL2
{
input.m_maximumDistanceSquared = min0.GetMargin() + min1.GetMargin() + m_manifoldPtr.GetContactBreakingThreshold();
input.m_maximumDistanceSquared *= input.m_maximumDistanceSquared;
}
//input.m_stackAlloc = dispatchInfo.m_stackAllocator;
input.m_transformA = body0.GetWorldTransform();
input.m_transformB = body1.GetWorldTransform();
if (min0.IsPolyhedral() && min1.IsPolyhedral())
{
DummyResult dummy = new DummyResult();
PolyhedralConvexShape polyhedronA = min0 as PolyhedralConvexShape;
PolyhedralConvexShape polyhedronB = min1 as PolyhedralConvexShape;
if (polyhedronA.GetConvexPolyhedron() != null && polyhedronB.GetConvexPolyhedron() != null)
{
float threshold = m_manifoldPtr.GetContactBreakingThreshold();
float minDist = float.MinValue;
IndexedVector3 sepNormalWorldSpace = new IndexedVector3(0, 1, 0);
bool foundSepAxis = true;
if (dispatchInfo.m_enableSatConvex)
{
foundSepAxis = PolyhedralContactClipping.FindSeparatingAxis(
polyhedronA.GetConvexPolyhedron(), polyhedronB.GetConvexPolyhedron(),
body0.GetWorldTransform(),
body1.GetWorldTransform(),
out sepNormalWorldSpace);
}
else
{
#if ZERO_MARGIN
gjkPairDetector.SetIgnoreMargin(true);
gjkPairDetector.GetClosestPoints(input,resultOut,dispatchInfo.m_debugDraw);
#else
gjkPairDetector.GetClosestPoints(ref input, dummy, dispatchInfo.m_debugDraw);
#endif
float l2 = gjkPairDetector.GetCachedSeparatingAxis().LengthSquared();
if (l2 > MathUtil.SIMD_EPSILON)
{
sepNormalWorldSpace = gjkPairDetector.GetCachedSeparatingAxis() * (1.0f / l2);
//minDist = -1e30f;//gjkPairDetector.getCachedSeparatingDistance();
minDist = gjkPairDetector.GetCachedSeparatingDistance() - min0.GetMargin() - min1.GetMargin();
#if ZERO_MARGIN
foundSepAxis = true;//gjkPairDetector.getCachedSeparatingDistance()<0.f;
#else
foundSepAxis = gjkPairDetector.GetCachedSeparatingDistance() < (min0.GetMargin() + min1.GetMargin());
#endif
}
}
if (foundSepAxis)
{
// printf("sepNormalWorldSpace=%f,%f,%f\n",sepNormalWorldSpace.getX(),sepNormalWorldSpace.getY(),sepNormalWorldSpace.getZ());
PolyhedralContactClipping.ClipHullAgainstHull(sepNormalWorldSpace, polyhedronA.GetConvexPolyhedron(), polyhedronB.GetConvexPolyhedron(),
body0.GetWorldTransform(),
body1.GetWorldTransform(), minDist - threshold, threshold, resultOut);
}
if (m_ownManifold)
{
resultOut.RefreshContactPoints();
}
return;
}
else
{
//we can also deal with convex versus triangle (without connectivity data)
if (polyhedronA.GetConvexPolyhedron() != null && polyhedronB.GetShapeType() == BroadphaseNativeTypes.TRIANGLE_SHAPE_PROXYTYPE)
{
m_vertices.Clear();
TriangleShape tri = polyhedronB as TriangleShape;
m_vertices.Add(body1.GetWorldTransform() * tri.m_vertices1[0]);
m_vertices.Add(body1.GetWorldTransform() * tri.m_vertices1[1]);
m_vertices.Add(body1.GetWorldTransform() * tri.m_vertices1[2]);
float threshold = m_manifoldPtr.GetContactBreakingThreshold();
IndexedVector3 sepNormalWorldSpace = new IndexedVector3(0, 1, 0); ;
float minDist = float.MinValue;
float maxDist = threshold;
bool foundSepAxis = false;
if (false)
{
polyhedronB.InitializePolyhedralFeatures();
foundSepAxis = PolyhedralContactClipping.FindSeparatingAxis(
polyhedronA.GetConvexPolyhedron(), polyhedronB.GetConvexPolyhedron(),
body0.GetWorldTransform(),
body1.GetWorldTransform(),
out sepNormalWorldSpace);
// printf("sepNormalWorldSpace=%f,%f,%f\n",sepNormalWorldSpace.getX(),sepNormalWorldSpace.getY(),sepNormalWorldSpace.getZ());
}
else
{
#if ZERO_MARGIN
gjkPairDetector.SetIgnoreMargin(true);
gjkPairDetector.GetClosestPoints(input,resultOut,dispatchInfo.m_debugDraw);
#else
gjkPairDetector.GetClosestPoints(ref input, dummy, dispatchInfo.m_debugDraw);
#endif//ZERO_MARGIN
float l2 = gjkPairDetector.GetCachedSeparatingAxis().LengthSquared();
if (l2 > MathUtil.SIMD_EPSILON)
{
sepNormalWorldSpace = gjkPairDetector.GetCachedSeparatingAxis() * (1.0f / l2);
//minDist = gjkPairDetector.getCachedSeparatingDistance();
//maxDist = threshold;
minDist = gjkPairDetector.GetCachedSeparatingDistance() - min0.GetMargin() - min1.GetMargin();
foundSepAxis = true;
}
}
if (foundSepAxis)
{
PolyhedralContactClipping.ClipFaceAgainstHull(sepNormalWorldSpace, polyhedronA.GetConvexPolyhedron(),
body0.GetWorldTransform(), m_vertices, minDist - threshold, maxDist, resultOut);
}
if (m_ownManifold)
{
resultOut.RefreshContactPoints();
}
return;
}
}
}
gjkPairDetector.GetClosestPoints(ref input, resultOut, dispatchInfo.getDebugDraw(), false);
#if USE_SEPDISTANCE_UTIL2
float sepDist = 0.f;
if (dispatchInfo.m_useConvexConservativeDistanceUtil)
{
sepDist = gjkPairDetector.getCachedSeparatingDistance();
if (sepDist>MathUtil.SIMD_EPSILON)
{
sepDist += dispatchInfo.m_convexConservativeDistanceThreshold;
//now perturbe directions to get multiple contact points
}
}
#endif //USE_SEPDISTANCE_UTIL2
//now perform 'm_numPerturbationIterations' collision queries with the perturbated collision objects
//perform perturbation when more then 'm_minimumPointsPerturbationThreshold' points
if (m_numPerturbationIterations > 0 && resultOut.GetPersistentManifold().GetNumContacts() < m_minimumPointsPerturbationThreshold)
{
IndexedVector3 v0, v1;
IndexedVector3 sepNormalWorldSpace = gjkPairDetector.GetCachedSeparatingAxis();
sepNormalWorldSpace.Normalize();
TransformUtil.PlaneSpace1(ref sepNormalWorldSpace, out v0, out v1);
bool perturbeA = true;
const float angleLimit = 0.125f * MathUtil.SIMD_PI;
float perturbeAngle;
float radiusA = min0.GetAngularMotionDisc();
float radiusB = min1.GetAngularMotionDisc();
if (radiusA < radiusB)
{
perturbeAngle = BulletGlobals.gContactBreakingThreshold / radiusA;
perturbeA = true;
}
else
{
perturbeAngle = BulletGlobals.gContactBreakingThreshold / radiusB;
perturbeA = false;
}
if (perturbeAngle > angleLimit)
{
perturbeAngle = angleLimit;
}
IndexedMatrix unPerturbedTransform;
if (perturbeA)
{
unPerturbedTransform = input.m_transformA;
}
else
{
unPerturbedTransform = input.m_transformB;
}
for (int i = 0; i < m_numPerturbationIterations; i++)
{
if (v0.LengthSquared() > MathUtil.SIMD_EPSILON)
{
IndexedQuaternion perturbeRot = new IndexedQuaternion(v0, perturbeAngle);
float iterationAngle = i * (MathUtil.SIMD_2_PI / (float)m_numPerturbationIterations);
IndexedQuaternion rotq = new IndexedQuaternion(sepNormalWorldSpace, iterationAngle);
if (perturbeA)
{
input.m_transformA._basis = (new IndexedBasisMatrix(MathUtil.QuaternionInverse(rotq) * perturbeRot * rotq) * body0.GetWorldTransform()._basis);
input.m_transformB = body1.GetWorldTransform();
input.m_transformB = body1.GetWorldTransform();
#if DEBUG_CONTACTS
dispatchInfo.m_debugDraw.DrawTransform(ref input.m_transformA, 10.0f);
#endif //DEBUG_CONTACTS
}
else
{
input.m_transformA = body0.GetWorldTransform();
input.m_transformB._basis = (new IndexedBasisMatrix(MathUtil.QuaternionInverse(rotq) * perturbeRot * rotq) * body1.GetWorldTransform()._basis);
#if DEBUG_CONTACTS
dispatchInfo.m_debugDraw.DrawTransform(ref input.m_transformB, 10.0f);
#endif
}
PerturbedContactResult perturbedResultOut = new PerturbedContactResult(resultOut, ref input.m_transformA, ref input.m_transformB, ref unPerturbedTransform, perturbeA, dispatchInfo.getDebugDraw());
gjkPairDetector.GetClosestPoints(ref input, perturbedResultOut, dispatchInfo.getDebugDraw(), false);
}
}
}
#if USE_SEPDISTANCE_UTIL2
if (dispatchInfo.m_useConvexConservativeDistanceUtil && (sepDist > MathUtil.SIMD_EPSILON))
{
m_sepDistance.initSeparatingDistance(gjkPairDetector.getCachedSeparatingAxis(),sepDist,body0.getWorldTransform(),body1.getWorldTransform());
}
#endif //USE_SEPDISTANCE_UTIL2
}
}
if (m_ownManifold)
{
resultOut.RefreshContactPoints();
}
}
public virtual void ClearManifold(PersistentManifold manifold)
{
manifold.ClearManifold();
}
public override void Cleanup()
{
if (m_ownManifold)
{
if (m_manifoldPtr != null)
{
m_dispatcher.ReleaseManifold(m_manifoldPtr);
}
m_ownManifold = false;
}
m_manifoldPtr = null;
BulletGlobals.ConvexConvexAlgorithmPool.Free(this);
}
//public int GetDispatcherId()
//{
// return 0;
//}
public void SetManifold(PersistentManifold manifold)
{
m_manifold = manifold;
}
