private void DetectCallback(object obj)
{
BroadphasePair pair = obj as BroadphasePair;
base.Detect(pair.Entity1, pair.Entity2);
BroadphasePair.Pool.GiveBack(pair);
}
public void NearCallback(BroadphasePair collisionPair, CollisionDispatcher dispatcher, DispatcherInfo dispatchInfo)
{
CollisionObject colObj0 = (CollisionObject)collisionPair.m_pProxy0.GetClientObject();
CollisionObject colObj1 = (CollisionObject)collisionPair.m_pProxy1.GetClientObject();
if (dispatcher.NeedsCollision(colObj0,colObj1))
{
//dispatcher will keep algorithms persistent in the collision pair
if (collisionPair.m_algorithm == null)
{
collisionPair.m_algorithm = dispatcher.FindAlgorithm(colObj0,colObj1,null);
}
if (collisionPair.m_algorithm != null)
{
ManifoldResult contactPointResult = new ManifoldResult(colObj0,colObj1);
if (dispatchInfo.GetDispatchFunc() == DispatchFunc.DISPATCH_DISCRETE)
{
//discrete collision detection query
collisionPair.m_algorithm.ProcessCollision(colObj0,colObj1,dispatchInfo,contactPointResult);
}
else
{
//continuous collision detection query, time of impact (toi)
float toi = collisionPair.m_algorithm.CalculateTimeOfImpact(colObj0,colObj1,dispatchInfo,contactPointResult);
if (dispatchInfo.GetTimeOfImpact() > toi)
{
dispatchInfo.SetTimeOfImpact(toi);
}
}
}
}
}
public Object RemoveOverlappingPair(BroadphaseProxy proxy0, BroadphaseProxy proxy1, IDispatcher dispatcher)
{
if (!HasDeferredRemoval())
{
BroadphasePair findPair = new BroadphasePair(proxy0,proxy1);
int findIndex = m_overlappingPairArray.IndexOf(findPair);
if (findIndex >= 0 && findIndex < m_overlappingPairArray.Count)
{
OverlappingPairCacheGlobals.gOverlappingPairs--;
BroadphasePair pair = m_overlappingPairArray[findIndex];
Object userData = pair.m_internalInfo1;
CleanOverlappingPair(pair,dispatcher);
if (m_ghostPairCallback != null)
{
m_ghostPairCallback.RemoveOverlappingPair(proxy0, proxy1,dispatcher);
}
//BroadphasePair temp = m_overlappingPairArray[findIndex];
//m_overlappingPairArray[findIndex] = m_overlappingPairArray[m_overlappingPairArray.Count-1];
//m_overlappingPairArray[m_overlappingPairArray.Count-1] = temp;
m_overlappingPairArray.RemoveAt(m_overlappingPairArray.Count - 1);
return userData;
}
}
return 0;
}
static void DispatcherNearCallback(BroadphasePair collisionPair, CollisionDispatcher dispatcher,
DispatcherInfo dispatchInfo)
{
//AddToDisposeQueue(dispatchInfo.DebugDraw);
TestManifoldPoints();
//Console.WriteLine("DispatcherNearCallback");
}
public override bool handleCollision(BroadphasePair collisionPair, CollisionDispatcher dispatcher)
{
CollisionObject colObj0 = collisionPair.pProxy0.clientObject;
CollisionObject colObj1 = collisionPair.pProxy1.clientObject;
if (colObj0.isStaticOrKinematicObject() && colObj1.isStaticOrKinematicObject())
{
collisionPair.manifold.clearManifold();
return(false);
}
PersistentManifold manifold = collisionPair.manifold;
int oldContacts = manifold.getContactPointsNum();
manifold.refreshContactPoints(colObj0.getWorldTransform(), colObj1.getWorldTransform());
/*if (oldContacts == manifold.getContactPointsNum() && oldContacts > 0)
* return true;*/
manifold.clearManifold();
CollisionAlgorithm algorithm = dispatcher.findAlgorithm(colObj0, colObj1);
// discrete collision detection query
algorithm(colObj0, colObj1, dispatcher.getDispatchInfo(), manifold);
return(manifold.getContactPointsNum() > 0);
}
/// <summary>
/// Tells the collisionsystem to check all bodies for collisions. Hook into the
/// <see cref="CollisionSystem.PassedBroadphase"/>
/// and <see cref="CollisionSystem.CollisionDetected"/> events to get the results.
/// </summary>
/// <param name="multiThreaded">If true internal multithreading is used.</param>
#region public override void Detect(bool multiThreaded)
public override void Detect(bool multiThreaded)
{
int count = bodyList.Count;
if (multiThreaded)
{
for (int i = 0; i < count; i++)
{
for (int e = i + 1; e < count; e++)
{
if (!this.CheckBothStaticOrInactive(bodyList[i], bodyList[e]) && this.CheckBoundingBoxes(bodyList[i], bodyList[e]))
{
if (RaisePassedBroadphase(bodyList[i], bodyList[e]))
{
BroadphasePair pair = BroadphasePair.Pool.GetNew();
if (swapOrder)
{
pair.Entity1 = bodyList[i]; pair.Entity2 = bodyList[e];
}
else
{
pair.Entity2 = bodyList[e]; pair.Entity1 = bodyList[i];
}
swapOrder = !swapOrder;
threadManager.AddTask(detectCallback, pair);
}
}
}
}
threadManager.Execute();
}
else
{
for (int i = 0; i < count; i++)
{
for (int e = i + 1; e < count; e++)
{
if (!this.CheckBothStaticOrInactive(bodyList[i], bodyList[e]) && this.CheckBoundingBoxes(bodyList[i], bodyList[e]))
{
if (RaisePassedBroadphase(bodyList[i], bodyList[e]))
{
if (swapOrder)
{
Detect(bodyList[i], bodyList[e]);
}
else
{
Detect(bodyList[e], bodyList[i]);
}
swapOrder = !swapOrder;
}
}
}
}
}
}
public virtual bool ProcessOverlap(BroadphasePair pair)
{
if (pair != null && ((pair.m_pProxy0 == m_cleanProxy) ||
(pair.m_pProxy1 == m_cleanProxy)))
{
m_pairCache.CleanOverlappingPair(pair,m_dispatcher);
}
return false;
}
public bool RecoverFromPenetration(CollisionWorld collisionWorld)
{
bool penetration = false;
collisionWorld.GetDispatcher().DispatchAllCollisionPairs(m_ghostObject.GetOverlappingPairCache(), collisionWorld.GetDispatchInfo(), collisionWorld.GetDispatcher());
m_currentPosition = m_ghostObject.GetWorldTransform()._origin;
float maxPen = 0f;
for (int i = 0; i < m_ghostObject.GetOverlappingPairCache().GetNumOverlappingPairs(); i++)
{
m_manifoldArray.Clear();
BroadphasePair collisionPair = m_ghostObject.GetOverlappingPairCache().GetOverlappingPairArray()[i];
if (collisionPair.m_algorithm != null)
{
collisionPair.m_algorithm.GetAllContactManifolds(m_manifoldArray);
}
for (int j = 0; j < m_manifoldArray.Count; j++)
{
PersistentManifold manifold = m_manifoldArray[j];
float directionSign = manifold.GetBody0() == m_ghostObject ? -1f : 1f;
for (int p = 0; p < manifold.GetNumContacts(); p++)
{
ManifoldPoint pt = manifold.GetContactPoint(p);
float dist = pt.GetDistance();
if (dist < 0.0)
{
if (dist < maxPen)
{
maxPen = dist;
m_touchingNormal = pt.m_normalWorldOnB * directionSign; //??
}
m_currentPosition += pt.m_normalWorldOnB * directionSign * dist * 0.2f;
penetration = true;
}
else
{
//printf("touching %f\n", dist);
}
}
//manifold->clearManifold();
}
}
IndexedMatrix newTrans = m_ghostObject.GetWorldTransform();
newTrans._origin = m_currentPosition;
m_ghostObject.SetWorldTransform(ref newTrans);
// printf("m_touchingNormal = %f,%f,%f\n",m_touchingNormal[0],m_touchingNormal[1],m_touchingNormal[2]);
return(penetration);
}
public void CleanOverlappingPair(BroadphasePair pair, IDispatcher dispatcher)
{
if (pair.m_algorithm != null)
{
{
pair.m_algorithm.Cleanup();
dispatcher.FreeCollisionAlgorithm(pair.m_algorithm);
pair.m_algorithm = null;
OverlappingPairCacheGlobals.gRemovePairs--;
}
}
}
private void AddToActiveMultithreaded(IBroadphaseEntity body, bool addToList)
{
float xmin = body.BoundingBox.Min.X;
int n = active.Count;
bool thisInactive = body.IsStaticOrInactive;
JBBox acBox, bodyBox;
for (int i = 0; i != n;)
{
IBroadphaseEntity ac = active[i];
acBox = ac.BoundingBox;
if (acBox.Max.X < xmin)
{
n--;
active.RemoveAt(i);
}
else
{
bodyBox = body.BoundingBox;
if (!(thisInactive && ac.IsStaticOrInactive) &&
(((bodyBox.Max.Z >= acBox.Min.Z) && (bodyBox.Min.Z <= acBox.Max.Z)) &&
((bodyBox.Max.Y >= acBox.Min.Y) && (bodyBox.Min.Y <= acBox.Max.Y))))
{
if (base.RaisePassedBroadphase(ac, body))
{
BroadphasePair pair = BroadphasePair.Pool.GetNew();
if (swapOrder)
{
pair.Entity1 = body; pair.Entity2 = ac;
}
else
{
pair.Entity2 = body; pair.Entity1 = ac;
}
swapOrder = !swapOrder;
threadManager.AddTask(detectCallback, pair);
}
}
i++;
}
}
active.Add(body);
}
public static void NearCallback(BroadphasePair collisionPair, CollisionDispatcher dispatcher, DispatcherInfo dispatcherInfo)
{
var entity1 = (collisionPair.Proxy0.ClientObject as RigidBody).UserObject as Entity;
var entity2 = (collisionPair.Proxy1.ClientObject as RigidBody).UserObject as Entity;
if (HardCollisionAction != null)
{
if (entity1.CollisionType == CollisionTypeEnum.Hard)
{
HardCollisionAction();
}
}
CollisionDispatcher.DefaultNearCallback(collisionPair, dispatcher, dispatcherInfo);
}
public void GhostObjectPairsTest()
{
var world = _context.DiscreteDynamicsWorld;
world.StepSimulation(1.0f / 60.0f);
AlignedManifoldArray manifoldArray = new AlignedManifoldArray();
AlignedBroadphasePairArray pairArray = _ghostObject.OverlappingPairCache.OverlappingPairArray;
foreach (BroadphasePair pair in pairArray)
{
//unless we manually perform collision detection on this pair, the contacts are in the dynamics world paircache:
BroadphasePair collisionPair = world.PairCache.FindPair(pair.Proxy0, pair.Proxy1);
if (collisionPair == null)
{
continue;
}
manifoldArray.Clear();
if (collisionPair.Algorithm != null)
{
collisionPair.Algorithm.GetAllContactManifolds(manifoldArray);
}
for (int j = 0; j < manifoldArray.Count; j++)
{
PersistentManifold manifold = manifoldArray[j];
float directionSign = manifold.Body0 == _ghostObject ? -1.0f : 1.0f;
for (int p = 0; p < manifold.NumContacts; p++)
{
ManifoldPoint pt = manifold.GetContactPoint(p);
if (pt.Distance < 0.0f)
{
Vector3 ptA = pt.PositionWorldOnA;
Vector3 ptB = pt.PositionWorldOnB;
Vector3 normalOnB = pt.NormalWorldOnB;
}
}
}
}
manifoldArray.Dispose();
}
public override bool NeedsCollision(BroadphaseProxy proxy0)
{
//don't collide with itself
if (proxy0.m_clientObject == m_me)
{
return(false);
}
///don't do CCD when the collision filters are not matching
if (!base.NeedsCollision(proxy0))
{
return(false);
}
CollisionObject otherObj = proxy0.m_clientObject as CollisionObject;
//call needsResponse, see http://code.google.com/p/bullet/issues/detail?id=179
if (m_dispatcher.NeedsResponse(m_me, otherObj))
{
#if false
///don't do CCD when there are already contact points (touching contact/penetration)
PersistentManifoldArray manifoldArray = new PersistentManifoldArray();
BroadphasePair collisionPair = m_pairCache.FindPair(m_me.GetBroadphaseHandle(), proxy0);
if (collisionPair != null)
{
if (collisionPair.m_algorithm != null)
{
collisionPair.m_algorithm.GetAllContactManifolds(manifoldArray);
int length = manifoldArray.Count;
for (int i = 0; i < length; ++i)
{
if (manifoldArray[i].GetNumContacts() > 0)
{
return(false);
}
}
}
}
#endif
return(true);
}
return(false);
}
static void TestGhostObjectPairs(PairCachingGhostObject ghostObject)
{
AlignedManifoldArray manifoldArray = new AlignedManifoldArray();
AlignedBroadphasePairArray pairArray = ghostObject.OverlappingPairCache.OverlappingPairArray;
int numPairs = pairArray.Count;
for (int i = 0; i < numPairs; i++)
{
manifoldArray.Clear();
BroadphasePair pair = pairArray[i];
//unless we manually perform collision detection on this pair, the contacts are in the dynamics world paircache:
BroadphasePair collisionPair = world.PairCache.FindPair(pair.Proxy0, pair.Proxy1);
if (collisionPair == null)
{
continue;
}
if (collisionPair.Algorithm != null)
{
collisionPair.Algorithm.GetAllContactManifolds(manifoldArray);
}
for (int j = 0; j < manifoldArray.Count; j++)
{
PersistentManifold manifold = manifoldArray[j];
float directionSign = manifold.Body0 == ghostObject ? -1.0f : 1.0f;
for (int p = 0; p < manifold.NumContacts; p++)
{
ManifoldPoint pt = manifold.GetContactPoint(p);
if (pt.Distance < 0.0f)
{
Vector3 ptA = pt.PositionWorldOnA;
Vector3 ptB = pt.PositionWorldOnB;
Vector3 normalOnB = pt.NormalWorldOnB;
/// work here
}
}
}
}
}
public override void ReportProximities(ChProximityContainer mproximitycontainer)
{
mproximitycontainer.BeginAddProximities();
/*
* int numManifolds = bt_collision_world->getDispatcher()->getNumManifolds();
* for (int i = 0; i < numManifolds; i++) {
* btPersistentManifold* contactManifold = bt_collision_world->getDispatcher()->getManifoldByIndexInternal(i);
* btCollisionObject* obA = static_cast<btCollisionObject*>(contactManifold->getBody0());
* btCollisionObject* obB = static_cast<btCollisionObject*>(contactManifold->getBody1());
* contactManifold->refreshContactPoints(obA->getWorldTransform(), obB->getWorldTransform());
*
* ChCollisionModel* modelA = (ChCollisionModel*)obA->getUserPointer();
* ChCollisionModel* modelB = (ChCollisionModel*)obB->getUserPointer();
*
* // Add to proximity container
* mproximitycontainer->AddProximity(modelA, modelB);
* }
*/
int numPairs = bt_collision_world.GetBroadphase().GetOverlappingPairCache().GetNumOverlappingPairs();
for (int i = 0; i < numPairs; i++)
{
BroadphasePair mp = bt_collision_world.GetBroadphase().GetOverlappingPairCache().GetOverlappingPairArray()[i];
CollisionObject obA = (CollisionObject)(mp.m_pProxy0.GetClientObject());
CollisionObject obB = (CollisionObject)(mp.m_pProxy1.GetClientObject());
ChCollisionModel modelA = (ChCollisionModel)obA.GetUserPointer();
ChCollisionModel modelB = (ChCollisionModel)obB.GetUserPointer();
// Add to proximity container
mproximitycontainer.AddProximity(modelA, modelB);
}
mproximitycontainer.EndAddProximities();
}
public void CleanOverlappingPair(BroadphasePair pair, IDispatcher dispatcher)
{
if (pair.m_algorithm != null)
{
dispatcher.FreeCollisionAlgorithm(pair.m_algorithm);
pair.m_algorithm = null;
}
}
private BroadphasePair InternalAddPair(BroadphaseProxy proxy0, BroadphaseProxy proxy1)
{
if (proxy0.m_uniqueId > proxy1.m_uniqueId)
{
BroadphaseProxy temp = proxy0;
proxy0 = proxy1;
proxy1 = temp;
}
int proxyId1 = proxy0.GetUid();
int proxyId2 = proxy1.GetUid();
int hash = (int)(GetHash((uint)proxyId1, (uint)proxyId2) & (m_overlappingPairArray.Capacity - 1)); // New hash value with new mask
BroadphasePair pair = InternalFindPair(proxy0, proxy1, hash);
if (pair != null)
{
return pair;
}
else
{
/*for(int i=0;i<m_overlappingPairArray.size();++i)
{
if( (m_overlappingPairArray[i].m_pProxy0==proxy0)&&
(m_overlappingPairArray[i].m_pProxy1==proxy1))
{
printf("Adding duplicated %u<>%u\r\n",proxyId1,proxyId2);
internalFindPair(proxy0, proxy1, hash);
}
}*/
int count = m_overlappingPairArray.Count;
int oldCapacity = m_overlappingPairArray.Capacity;
// MAN - 2.76 - uses expand noninitializing....??
//void* mem = &m_overlappingPairArray.expand();
//this is where we add an actual pair, so also call the 'ghost'
if (m_ghostPairCallback != null)
{
m_ghostPairCallback.AddOverlappingPair(proxy0, proxy1);
}
pair = new BroadphasePair(proxy0, proxy1);
m_overlappingPairArray.Add(pair);
int newCapacity = m_overlappingPairArray.Capacity;
if (oldCapacity < newCapacity)
{
GrowTables();
//hash with new capacity
hash = (int)(GetHash((uint)(proxyId1), (uint)(proxyId2)) & (m_overlappingPairArray.Capacity - 1));
}
m_next[count] = m_hashTable[hash];
m_hashTable[hash] = count;
return pair;
}
}
public bool processOverlap(BroadphasePair pair)
{
return ((pair.m_pProxy0 == m_obsoleteProxy) ||
(pair.m_pProxy1 == m_obsoleteProxy));
}
public void NearCallback(BroadphasePair collisionPair, CollisionDispatcher dispatcher, DispatcherInfo dispatchInfo)
{
CollisionObject colObj0 = collisionPair.m_pProxy0.GetClientObject() as CollisionObject;
CollisionObject colObj1 = collisionPair.m_pProxy1.GetClientObject() as CollisionObject;
if (dispatcher.NeedsCollision(colObj0, colObj1))
{
//dispatcher will keep algorithms persistent in the collision pair
if (collisionPair.m_algorithm == null)
{
collisionPair.m_algorithm = dispatcher.FindAlgorithm(colObj0, colObj1, null);
}
if (collisionPair.m_algorithm != null)
{
ManifoldResult contactPointResult = dispatcher.GetNewManifoldResult(colObj0, colObj1);
if (dispatchInfo.GetDispatchFunc() == DispatchFunc.DISPATCH_DISCRETE)
{
//discrete collision detection query
collisionPair.m_algorithm.ProcessCollision(colObj0, colObj1, dispatchInfo, contactPointResult);
}
else
{
//continuous collision detection query, time of impact (toi)
float toi = collisionPair.m_algorithm.CalculateTimeOfImpact(colObj0, colObj1, dispatchInfo, contactPointResult);
if (dispatchInfo.GetTimeOfImpact() > toi)
{
dispatchInfo.SetTimeOfImpact(toi);
}
}
#if DEBUG
if (BulletGlobals.g_streamWriter != null && BulletGlobals.debugDispatcher)
{
BulletGlobals.g_streamWriter.WriteLine("NearCallback[{0}][{1}][{2}]", contactPointResult.GetBody0Internal().GetUserPointer(), contactPointResult.GetBody1Internal().GetUserPointer(),contactPointResult.GetPersistentManifold().GetNumContacts());
}
#endif
dispatcher.FreeManifoldResult(contactPointResult);
}
}
}
public virtual bool ProcessOverlap(BroadphasePair pair)
{
return (pair != null && ((pair.m_pProxy0 == m_obsoleteProxy) ||
(pair.m_pProxy1 == m_obsoleteProxy)));
}
public virtual void CalculateOverlappingPairs(IDispatcher dispatcher)
{
if(BulletGlobals.g_streamWriter != null && BulletGlobals.debugBroadphase)
{
BulletGlobals.g_streamWriter.WriteLine("simple calculateOverlappingPairs");
}
//first check for new overlapping pairs
if (m_numHandles > 0)
{
int new_largest_index = -1;
for (int i = 0; i <= m_LastHandleIndex; i++)
{
SimpleBroadphaseProxy proxy0 = m_pHandles[i];
if (proxy0.GetClientObject() == null)
{
continue;
}
new_largest_index = i;
for (int j = i + 1; j <= m_LastHandleIndex; j++)
{
SimpleBroadphaseProxy proxy1 = m_pHandles[j];
//btAssert(proxy0 != proxy1);
if (proxy1.GetClientObject() == null)
{
continue;
}
if (AabbOverlap(proxy0, proxy1))
{
if (m_pairCache.FindPair(proxy0, proxy1) == null)
{
m_pairCache.AddOverlappingPair(proxy0, proxy1);
}
}
else
{
if (!m_pairCache.HasDeferredRemoval())
{
if (m_pairCache.FindPair(proxy0, proxy1) != null)
{
m_pairCache.RemoveOverlappingPair(proxy0, proxy1, dispatcher);
}
}
}
}
}
m_LastHandleIndex = new_largest_index;
if (m_ownsPairCache && m_pairCache.HasDeferredRemoval())
{
IList<BroadphasePair> overlappingPairArray = m_pairCache.GetOverlappingPairArray();
//perform a sort, to find duplicates and to sort 'invalid' pairs to the end
((List<BroadphasePair>)overlappingPairArray).Sort();
//overlappingPairArray.Capacity = (overlappingPairArray.Count - m_invalidPair);
m_invalidPair = 0;
BroadphasePair previousPair = new BroadphasePair();
for (int i = 0; i < overlappingPairArray.Count; i++)
{
BroadphasePair pair = overlappingPairArray[i];
bool isDuplicate = (pair == previousPair);
previousPair = pair;
bool needsRemoval = false;
if (!isDuplicate)
{
bool hasOverlap = TestAabbOverlap(pair.m_pProxy0, pair.m_pProxy1);
if (hasOverlap)
{
needsRemoval = false;//callback->processOverlap(pair);
}
else
{
needsRemoval = true;
}
}
else
{
//remove duplicate
needsRemoval = true;
//should have no algorithm
//btAssert(!pair.m_algorithm);
}
if (needsRemoval)
{
m_pairCache.CleanOverlappingPair(pair, dispatcher);
// m_overlappingPairArray.swap(i,m_overlappingPairArray.size()-1);
// m_overlappingPairArray.pop_back();
pair.m_pProxy0 = null;
pair.m_pProxy1 = null;
m_invalidPair++;
BulletGlobals.gOverlappingPairs--;
}
}
///if you don't like to skip the invalid pairs in the array, execute following code:
#if CLEAN_INVALID_PAIRS
//perform a sort, to sort 'invalid' pairs to the end
//overlappingPairArray.quickSort(new BroadphasePairSortPredicate());
((List<BroadphasePair>)overlappingPairArray).Sort();
//overlappingPairArray.Capacity = overlappingPairArray.Count - m_invalidPair;
m_invalidPair = 0;
#endif//CLEAN_INVALID_PAIRS
}
}
}
public void PerformDeferredRemoval(IDispatcher dispatcher)
{
if (m_paircache.HasDeferredRemoval())
{
ObjectArray<BroadphasePair> overlappingPairArray = m_paircache.GetOverlappingPairArray();
//perform a sort, to find duplicates and to sort 'invalid' pairs to the end
overlappingPairArray.QuickSort(new BroadphasePairQuickSort());
int invalidPair = 0;
int i;
BroadphasePair previousPair = new BroadphasePair();
for (i = 0; i < overlappingPairArray.Count; i++)
{
BroadphasePair pair = overlappingPairArray[i];
bool isDuplicate = (pair == previousPair);
previousPair = pair;
bool needsRemoval = false;
if (!isDuplicate)
{
//important to perform AABB check that is consistent with the broadphase
DbvtProxy pa = pair.m_pProxy0 as DbvtProxy;
DbvtProxy pb = pair.m_pProxy1 as DbvtProxy;
bool hasOverlap = DbvtAabbMm.Intersect(ref pa.leaf.volume, ref pb.leaf.volume);
if (hasOverlap)
{
needsRemoval = false;
}
else
{
needsRemoval = true;
}
}
else
{
//remove duplicate
needsRemoval = true;
//should have no algorithm
Debug.Assert(pair.m_algorithm != null);
}
if (needsRemoval)
{
m_paircache.CleanOverlappingPair(pair, dispatcher);
pair.m_pProxy0 = null;
pair.m_pProxy1 = null;
invalidPair++;
}
}
if (invalidPair > 0)
{
if (invalidPair < overlappingPairArray.Count)
{
int ibreak = 0;
}
//perform a sort, to sort 'invalid' pairs to the end
overlappingPairArray.QuickSort(new BroadphasePairQuickSort());
//overlappingPairArray.resize(overlappingPairArray.size() - invalidPair);
overlappingPairArray.Truncate(invalidPair);
}
}
}
public BroadphasePair FindPair(BroadphaseProxy proxy0, BroadphaseProxy proxy1)
{
if (!NeedsBroadphaseCollision(proxy0, proxy1))
{
return null;
}
BroadphasePair tmpPair = new BroadphasePair(proxy0, proxy1);
int index = m_overlappingPairArray.IndexOf(tmpPair);
if (index != -1)
{
return m_overlappingPairArray[index];
}
return null;
}
bool equalsPair(BroadphasePair pair, int proxyId1, int proxyId2)
{
return pair.m_pProxy0.UID == proxyId1 && pair.m_pProxy1.UID == proxyId2;
}
public void cleanOverlappingPair(BroadphasePair pair, IDispatcher dispatcher)
{
if (pair.m_algorithm != null)
{
{
/*pair.m_algorithm->~btCollisionAlgorithm();
dispatcher->freeCollisionAlgorithm(pair.m_algorithm);
pair.m_algorithm=0;*/
dispatcher.freeCollisionAlgorithm(pair.m_algorithm);
pair.m_algorithm = null;
}
}
}
void FixedUpdate()
{
CollisionWorld collisionWorld = BPhysicsWorld.Get().world;
collisionWorld.Dispatcher.DispatchAllCollisionPairs(m_ghostObject.OverlappingPairCache, collisionWorld.DispatchInfo, collisionWorld.Dispatcher);
//m_currentPosition = m_ghostObject.WorldTransform.Origin;
//float maxPen = 0f;
objsCurrentlyInContactWith.Clear();
for (int i = 0; i < m_ghostObject.OverlappingPairCache.NumOverlappingPairs; i++)
{
manifoldArray.Clear();
BroadphasePair collisionPair = m_ghostObject.OverlappingPairCache.OverlappingPairArray[i];
CollisionObject obj0 = collisionPair.Proxy0.ClientObject as CollisionObject;
CollisionObject obj1 = collisionPair.Proxy1.ClientObject as CollisionObject;
if ((obj0 != null && !obj0.HasContactResponse) || (obj1 != null && !obj1.HasContactResponse))
{
continue;
}
if (collisionPair.Algorithm != null)
{
collisionPair.Algorithm.GetAllContactManifolds(manifoldArray);
}
CollisionObject otherObj = null;
if (manifoldArray.Count > 0)
{
PersistentManifold pm = manifoldArray[0];
if (pm.Body0 == m_collisionObject)
{
otherObj = pm.Body1;
}
else
{
otherObj = pm.Body0;
}
}
else
{
continue;
}
objsCurrentlyInContactWith.Add(otherObj);
if (!objsIWasInContactWithLastFrame.Contains(otherObj))
{
BOnTriggerEnter(otherObj, manifoldArray);
}
else
{
BOnTriggerStay(otherObj, manifoldArray);
}
}
objsIWasInContactWithLastFrame.ExceptWith(objsCurrentlyInContactWith);
foreach (CollisionObject co in objsIWasInContactWithLastFrame)
{
BOnTriggerExit(co);
}
//swap the hashsets so objsIWasInContactWithLastFrame now contains the list of objs.
HashSet <CollisionObject> temp = objsIWasInContactWithLastFrame;
objsIWasInContactWithLastFrame = objsCurrentlyInContactWith;
objsCurrentlyInContactWith = temp;
}
public abstract bool handleCollision(BroadphasePair collisionPair, CollisionDispatcher dispatcher);
public virtual bool ProcessOverlap(BroadphasePair pair)
{
//(void)pair;
//btAssert(0);
return false;
}
private bool EqualsPair(BroadphasePair pair, int proxyId1, int proxyId2)
{
return pair.m_pProxy0.m_uniqueId == proxyId1 && pair.m_pProxy1.m_uniqueId == proxyId2;
}
public bool processOverlap(BroadphasePair pair)
{
if ((pair.m_pProxy0 == m_cleanProxy) ||
(pair.m_pProxy1 == m_cleanProxy))
{
m_pairCache.cleanOverlappingPair(pair, m_dispatcher);
}
return false;
}
public BroadphasePair AddOverlappingPair(BroadphaseProxy proxy0, BroadphaseProxy proxy1)
{
//don't add overlap with own
Debug.Assert(proxy0 != proxy1);
if (!NeedsBroadphaseCollision(proxy0, proxy1))
{
return null;
}
BroadphasePair pair = new BroadphasePair(proxy0, proxy1);
m_overlappingPairArray.Add(pair);
OverlappingPairCacheGlobals.gOverlappingPairs++;
OverlappingPairCacheGlobals.gAddedPairs++;
if (m_ghostPairCallback != null)
{
m_ghostPairCallback.AddOverlappingPair(proxy0, proxy1);
}
return pair;
}
/// <summary>
/// Tells the collisionsystem to check all bodies for collisions. Hook into the
/// <see cref="CollisionSystem.PassedBroadphase"/>
/// and <see cref="CollisionSystem.CollisionDetected"/> events to get the results.
/// </summary>
/// <param name="multiThreaded">If true internal multithreading is used.</param>
public override void Detect(bool multiThreaded)
{
if (addCounter > AddedObjectsBruteForceIsUsed)
{
fullOverlaps.Clear();
DirtySortAxis(axis1);
DirtySortAxis(axis2);
DirtySortAxis(axis3);
}
else
{
if (multiThreaded)
{
threadManager.AddTask(sortCallback, axis1);
threadManager.AddTask(sortCallback, axis2);
threadManager.AddTask(sortCallback, axis3);
threadManager.Execute();
}
else
{
sortCallback(axis1);
sortCallback(axis2);
sortCallback(axis3);
}
}
addCounter = 0;
foreach (OverlapPair key in fullOverlaps)
{
if (this.CheckBothStaticOrInactive(key.Entity1, key.Entity2))
{
continue;
}
if (base.RaisePassedBroadphase(key.Entity1, key.Entity2))
{
if (multiThreaded)
{
BroadphasePair pair = BroadphasePair.Pool.GetNew();
if (swapOrder)
{
pair.Entity1 = key.Entity1; pair.Entity2 = key.Entity2;
}
else
{
pair.Entity2 = key.Entity2; pair.Entity1 = key.Entity1;
}
threadManager.AddTask(detectCallback, pair);
}
else
{
if (swapOrder)
{
Detect(key.Entity1, key.Entity2);
}
else
{
Detect(key.Entity2, key.Entity1);
}
}
swapOrder = !swapOrder;
}
}
//Only execute if multithreading.
if (multiThreaded)
{
threadManager.Execute();
}
}
public virtual void CleanOverlappingPair(BroadphasePair pair, IDispatcher disaptcher)
{
}
// Portable static method: prerequisite call: m_dynamicsWorld.getBroadphase().getOverlappingPairCache().setInternalGhostPairCallback(new btGhostPairCallback());
public static void GetCollidingObjectsInsidePairCachingGhostObject(DiscreteDynamicsWorld m_dynamicsWorld, PairCachingGhostObject m_pairCachingGhostObject, ObjectArray <CollisionObject> collisionArrayOut)
{
bool addOnlyObjectsWithNegativeDistance = true; // With "false" things don't change much, and the code is a bit faster and cleaner...
collisionArrayOut.Resize(0);
if (m_pairCachingGhostObject == null || m_dynamicsWorld == null)
{
return;
}
//#define USE_PLAIN_COLLISION_WORLD // We dispatch all collision pairs of the ghost object every step (slow)
#if USE_PLAIN_COLLISION_WORLD
//======================================================================================================
// I thought this line was no longer needed, but it seems to be necessary (and I believe it's an expensive call):
m_dynamicsWorld.getDispatcher().dispatchAllCollisionPairs(m_pairCachingGhostObject.getOverlappingPairCache(), m_dynamicsWorld.getDispatchInfo(), m_dynamicsWorld.getDispatcher());
// Maybe the call can be automatically triggered by some other Bullet call (I'm almost sure I could comment it out in another demo I made long ago...)
// So by now the general rule is: in real projects, simply comment it out and see if it works!
//======================================================================================================
// UPDATE: in dynamic worlds, the line above can be commented out and the broadphase pair can be retrieved through the call to findPair(...) below.
// In collision worlds probably the above line is needed only if collision detection for all the bodies hasn't been made... This is something
// I'm still not sure of... the general rule is to try to comment out the line above and try to use findPair(...) and see if it works whenever possible....
//======================================================================================================
#endif //USE_PLAIN_COLLISION_WORLD
ObjectArray <BroadphasePair> collisionPairs = m_pairCachingGhostObject.GetOverlappingPairCache().GetOverlappingPairArray();
int numObjects = collisionPairs.Count;
PersistentManifoldArray m_manifoldArray = new PersistentManifoldArray();
bool added;
for (int i = 0; i < numObjects; i++)
{
m_manifoldArray.Resize(0);
#if USE_PLAIN_COLLISION_WORLD
const btBroadphasePair& collisionPair = collisionPairs[i];
if (collisionPair.m_algorithm)
{
collisionPair.m_algorithm.getAllContactManifolds(m_manifoldArray);
}
else // THIS SHOULD NEVER HAPPEN, AND IF IT DOES, PLEASE RE-ENABLE the "call" a few lines above...
{
printf("No collisionPair.m_algorithm - probably m_dynamicsWorld.getDispatcher().dispatchAllCollisionPairs(...) must be missing.\n");
}
#else // USE_PLAIN_COLLISION_WORLD
BroadphasePair cPair = collisionPairs[i];
//unless we manually perform collision detection on this pair, the contacts are in the dynamics world paircache:
BroadphasePair collisionPair = m_dynamicsWorld.GetPairCache().FindPair(cPair.m_pProxy0, cPair.m_pProxy1);
if (collisionPair == null)
{
continue;
}
if (collisionPair.m_algorithm != null)
{
collisionPair.m_algorithm.GetAllContactManifolds(m_manifoldArray);
}
else
{ // THIS SHOULD NEVER HAPPEN, AND IF IT DOES, PLEASE RE-ENABLE the "call" a few lines above...
//printf("No collisionPair.m_algorithm - probably m_dynamicsWorld.getDispatcher().dispatchAllCollisionPairs(...) must be missing.\n");
}
#endif //USE_PLAIN_COLLISION_WORLD
added = false;
for (int j = 0; j < m_manifoldArray.Count; j++)
{
PersistentManifold manifold = m_manifoldArray[j];
// Here we are in the narrowphase, but can happen that manifold.getNumContacts()==0:
if (addOnlyObjectsWithNegativeDistance)
{
for (int p = 0, numContacts = manifold.GetNumContacts(); p < numContacts; p++)
{
ManifoldPoint pt = manifold.GetContactPoint(p);
if (pt.GetDistance() < 0.0)
{
// How can I be sure that the colObjs are all distinct ? I use the "added" flag.
collisionArrayOut.Add((CollisionObject)(manifold.GetBody0() == m_pairCachingGhostObject ? manifold.GetBody1() : manifold.GetBody0()));
added = true;
break;
}
}
if (added)
{
break;
}
}
else if (manifold.GetNumContacts() > 0)
{
collisionArrayOut.Add((CollisionObject)(manifold.GetBody0() == m_pairCachingGhostObject ? manifold.GetBody1() : manifold.GetBody0()));
break;
}
}
}
}
protected virtual bool ProcessOverlap(ref BroadphasePair pair)
{
SimpleBroadphaseProxy proxy0 = (SimpleBroadphaseProxy)(pair.m_pProxy0);
SimpleBroadphaseProxy proxy1 = (SimpleBroadphaseProxy)(pair.m_pProxy1);
return ((m_targetProxy == proxy0 || m_targetProxy == proxy1));
}
public virtual bool ProcessOverlap(BroadphasePair pair)
{
m_dispatcher.GetNearCallback().NearCallback(pair, m_dispatcher, m_dispatchInfo);
return false;
}
public virtual void CalculateOverlappingPairs(IDispatcher dispatcher)
{
if (m_pairCache.HasDeferredRemoval())
{
IList<BroadphasePair> overlappingPairArray = m_pairCache.GetOverlappingPairArray();
//perform a sort, to find duplicates and to sort 'invalid' pairs to the end
((List<BroadphasePair>)overlappingPairArray).Sort();
//overlappingPairArray.resize(overlappingPairArray.size() - m_invalidPair);
m_invalidPair = 0;
BroadphasePair previousPair = new BroadphasePair();
for (int i=0;i<overlappingPairArray.Count;i++)
{
BroadphasePair pair = overlappingPairArray[i];
bool isDuplicate = pair.Equals(previousPair);
// MAN - not sure if this should be a deep copy or not...
previousPair = pair;
bool needsRemoval = false;
if (!isDuplicate)
{
///important to use an AABB test that is consistent with the broadphase
bool hasOverlap = TestAabbOverlap(pair.m_pProxy0,pair.m_pProxy1);
if (hasOverlap)
{
needsRemoval = false;//callback.processOverlap(pair);
} else
{
needsRemoval = true;
}
} else
{
//remove duplicate
needsRemoval = true;
//should have no algorithm
Debug.Assert(pair.m_algorithm == null);
}
if (needsRemoval)
{
m_pairCache.CleanOverlappingPair(pair,dispatcher);
// m_overlappingPairArray.swap(i,m_overlappingPairArray.size()-1);
// m_overlappingPairArray.pop_back();
pair.m_pProxy0 = null;
pair.m_pProxy1 = null;
m_invalidPair++;
OverlappingPairCacheGlobals.gOverlappingPairs--;
}
}
///if you don't like to skip the invalid pairs in the array, execute following code:
#if CLEAN_INVALID_PAIRS
//perform a sort, to sort 'invalid' pairs to the end
((List<BroadphasePair>)overlappingPairArray).Sort();
//overlappingPairArray.Capacity = (overlappingPairArray.Count - m_invalidPair);
m_invalidPair = 0;
#endif//CLEAN_INVALID_PAIRS
//printf("overlappingPairArray.size()=%d\n",overlappingPairArray.size());
}
}
public virtual bool ProcessOverlap(BroadphasePair pair)
{
return (!SimpleBroadphase.AabbOverlap((SimpleBroadphaseProxy)(pair.m_pProxy0), (SimpleBroadphaseProxy)(pair.m_pProxy1)));
}
public abstract bool processOverlap(BroadphasePair pair);
///calculateOverlappingPairs is optional: incremental algorithms (sweep and prune) might do it during the set aabb
public void CalculateOverlappingPairs(IDispatcher dispatcher)
{
if (!m_stopUpdating && GetOverlappingPairCache().HasDeferredRemoval())
{
IList<BroadphasePair> overlappingPairArray = GetOverlappingPairCache().GetOverlappingPairArray();
((List<BroadphasePair>)overlappingPairArray).Sort();
m_invalidPair = 0;
int i;
BroadphasePair previousPair = new BroadphasePair();
previousPair.m_pProxy0 = null;
previousPair.m_pProxy1 = null;
previousPair.m_algorithm = null;
for (i=0;i<overlappingPairArray.Count;i++)
{
BroadphasePair pair = overlappingPairArray[i];
MultiSapProxy aProxy0 = pair.m_pProxy0 != null ? (MultiSapProxy)pair.m_pProxy0.m_multiSapParentProxy : null;
MultiSapProxy aProxy1 = pair.m_pProxy1 != null ? (MultiSapProxy)pair.m_pProxy1.m_multiSapParentProxy : null;
MultiSapProxy bProxy0 = previousPair.m_pProxy0 != null ? (MultiSapProxy)previousPair.m_pProxy0.m_multiSapParentProxy : null;
MultiSapProxy bProxy1 = previousPair.m_pProxy1 != null ? (MultiSapProxy)previousPair.m_pProxy1.m_multiSapParentProxy : null;
bool isDuplicate = (aProxy0 == bProxy0) && (aProxy1 == bProxy1);
previousPair = pair;
bool needsRemoval = false;
if (!isDuplicate)
{
bool hasOverlap = TestAabbOverlap(pair.m_pProxy0,pair.m_pProxy1);
if (hasOverlap)
{
needsRemoval = false;//callback->processOverlap(pair);
} else
{
needsRemoval = true;
}
} else
{
//remove duplicate
needsRemoval = true;
//should have no algorithm
Debug.Assert(pair.m_algorithm == null);
}
if (needsRemoval)
{
GetOverlappingPairCache().CleanOverlappingPair(pair,dispatcher);
// m_overlappingPairArray.swap(i,m_overlappingPairArray.size()-1);
// m_overlappingPairArray.pop_back();
pair.m_pProxy0 = null;
pair.m_pProxy1 = null;
m_invalidPair++;
BulletGlobals.gOverlappingPairs--;
}
}
///if you don't like to skip the invalid pairs in the array, execute following code:
//#define CLEAN_INVALID_PAIRS 1
//#ifdef CLEAN_INVALID_PAIRS
// //perform a sort, to sort 'invalid' pairs to the end
// //overlappingPairArray.heapSort(btMultiSapBroadphasePairSortPredicate());
// overlappingPairArray.quickSort(btMultiSapBroadphasePairSortPredicate());
// overlappingPairArray.resize(overlappingPairArray.size() - m_invalidPair);
// m_invalidPair = 0;
//#endif//CLEAN_INVALID_PAIRS
//printf("overlappingPairArray.size()=%d\n",overlappingPairArray.size());
}
}
bool RecoverFromPenetration(CollisionWorld collisionWorld)
{
Vector3 minAabb, maxAabb;
collisionShape.GetAabb(collisionObject.WorldTransform, out minAabb, out maxAabb);
collisionWorld.Broadphase.SetAabbRef(collisionObject.BroadphaseHandle,
ref minAabb,
ref maxAabb,
collisionWorld.Dispatcher);
bool penetration = false;
collisionWorld.Dispatcher.DispatchAllCollisionPairs(collisionObjectCastToPairCache.OverlappingPairCache, collisionWorld.DispatchInfo, collisionWorld.Dispatcher);
currentPosition = collisionObject.WorldTransform.Origin;
float maxPen = 0f;
for (int i = 0; i < collisionObjectCastToPairCache.OverlappingPairCache.NumOverlappingPairs; i++)
{
manifoldArray.Clear();
BroadphasePair collisionPair = collisionObjectCastToPairCache.OverlappingPairCache.OverlappingPairArray[i];
CollisionObject obj0 = collisionPair.Proxy0.ClientObject as CollisionObject;
CollisionObject obj1 = collisionPair.Proxy1.ClientObject as CollisionObject;
if ((obj0 != null && !obj0.HasContactResponse) || (obj1 != null && !obj1.HasContactResponse))
{
continue;
}
if (collisionPair.Algorithm != null)
{
collisionPair.Algorithm.GetAllContactManifolds(manifoldArray);
}
for (int j = 0; j < manifoldArray.Count; j++)
{
PersistentManifold manifold = manifoldArray[j];
float directionSign = manifold.Body0 == collisionObject ? -1f : 1f;
for (int p = 0; p < manifold.NumContacts; p++)
{
ManifoldPoint pt = manifold.GetContactPoint(p);
float dist = pt.Distance;
if (dist < 0.0f)
{
if (dist < maxPen)
{
maxPen = dist;
}
currentPosition += pt.NormalWorldOnB * directionSign * dist * 0.2f;
penetration = true;
}
}
}
}
Matrix newTrans = collisionObject.WorldTransform;
newTrans.Origin = currentPosition;
collisionObject.WorldTransform = newTrans;
return(penetration);
}
//by default, Bullet will use this near callback
static void DefaultNearCallback(BroadphasePair collisionPair, CollisionDispatcher dispatcher, DispatcherInfo dispatchInfo)
{
CollisionObject colObj0 = (CollisionObject)collisionPair.m_pProxy0.m_clientObject;
CollisionObject colObj1 = (CollisionObject)collisionPair.m_pProxy1.m_clientObject;
if (dispatcher.needsCollision(colObj0,colObj1))
{
//dispatcher will keep algorithms persistent in the collision pair
if (collisionPair.m_algorithm==null)
{
collisionPair.m_algorithm = dispatcher.findAlgorithm(colObj0,colObj1);
}
if (collisionPair.m_algorithm!=null)
{
ManifoldResult contactPointResult=new ManifoldResult(colObj0,colObj1);
if (dispatchInfo.m_dispatchFunc == DispatchFunc.DISPATCH_DISCRETE)
{
//discrete collision detection query
collisionPair.m_algorithm.processCollision(colObj0,colObj1,dispatchInfo,ref contactPointResult);
} else
{
//continuous collision detection query, time of impact (toi)
float toi = collisionPair.m_algorithm.calculateTimeOfImpact(colObj0,colObj1,dispatchInfo,ref contactPointResult);
if (dispatchInfo.m_timeOfImpact > toi)
dispatchInfo.m_timeOfImpact = toi;
}
}
}
}
