public void Update()
{
collisionWorld.PerformDiscreteCollisionDetection();
int numManifolds = collisionWorld.Dispatcher.NumManifolds;
for (int i = 0; i < numManifolds; i++)
{
PersistentManifold contactManifold = collisionWorld.Dispatcher.GetManifoldByIndexInternal(i);
CollisionObject obA = contactManifold.Body0;
CollisionObject obB = contactManifold.Body1;
int misilId = obB.UserIndex;
int objetoId = obA.UserIndex;
if (objetoId == ID_XWING && EsMisilEnemigo(misilId) &&
!Xwing.ESTADO_BARREL.BARRELROLL.Equals(VariablesGlobales.xwing.getEstadoBarrel()) &&
!listaIdMisilesQueColisionaronConXwing.Contains(misilId))
{
XwingCollision(contactManifold, misilId);
}
if (EsTorreta(objetoId) && EsMisilXWing(misilId) &&
!(listaColisionesTorretaMisil.Contains(new Colision(objetoId, misilId))))
{
TorretaCollision(contactManifold, misilId, objetoId);
}
if (EsObstaculo(objetoId) && EsMisilXWing(misilId) &&
!(listaColisionesObstaculoMisil.Contains(new Colision(objetoId, misilId))))
{
ObstaculoCollision(contactManifold, misilId, objetoId);
}
if (objetoId == ID_XWING && (misilId == ID_PARED_OBSTACULO || EsTorreta(misilId)))
{
VariablesGlobales.xwing.ChocarPared();
}
collisionWorld.Dispatcher.ClearManifold(contactManifold);
}
}
private void OnContactAdded(ManifoldPoint cp, CollisionObjectWrapper colObj0Wrap, int partId0, int index0,
CollisionObjectWrapper colObj1Wrap, int partId1, int index1)
{
//Debug.WriteLine("OnContactAdded");
int numManifolds = BtWorld.Dispatcher.NumManifolds;
for (int i = 0; i < numManifolds; i++)
{
PersistentManifold contactManifold = BtWorld.Dispatcher.GetManifoldByIndexInternal(i);
int numContacts = contactManifold.NumContacts;
if (numContacts > 0)
{
cp.UserPersistentData = 1;
CollisionObject obA = (CollisionObject)contactManifold.Body0;
CollisionObject obB = (CollisionObject)contactManifold.Body1;
RigidBody btRigidBodyA = (RigidBody)obA;
RigidBody btRigidBodyB = (RigidBody)obB;
RigidBodyImp rigidBodyA = new RigidBodyImp();
RigidBodyImp rigidBodyB = new RigidBodyImp();
rigidBodyA._rbi = btRigidBodyA;
rigidBodyB._rbi = btRigidBodyB;
rigidBodyA.OnCollision(rigidBodyB);
}
}
}
public void OnPhysicsStep(CollisionWorld world)
{
Dispatcher dispatcher = world.Dispatcher;
int numManifolds = dispatcher.NumManifolds;
for (int i = 0; i < numManifolds; i++)
{
PersistentManifold contactManifold = dispatcher.GetManifoldByIndexInternal(i);
CollisionObject a = contactManifold.Body0;
CollisionObject b = contactManifold.Body1;
if (a is CollisionObject && a.UserObject is BCollisionObject && ((BCollisionObject)a.UserObject).collisionCallbackEventHandlers.Count > 0)
{
foreach (BICollisionCallbackEventHandler collisionCallbackHandler in ((BCollisionObject)a.UserObject).collisionCallbackEventHandlers)
{
collisionCallbackHandler.OnVisitPersistentManifold(contactManifold);
}
}
if (b is CollisionObject && b.UserObject is BCollisionObject && ((BCollisionObject)b.UserObject).collisionCallbackEventHandlers.Count > 0)
{
foreach (BICollisionCallbackEventHandler collisionCallbackHandler in ((BCollisionObject)b.UserObject).collisionCallbackEventHandlers)
{
collisionCallbackHandler.OnVisitPersistentManifold(contactManifold);
}
}
}
foreach (BCollisionObject.BICollisionCallbackEventHandler coeh in collisionCallbackListeners)
{
if (coeh != null)
{
coeh.OnFinishedVisitingManifolds();
}
}
}
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);
}
public void Evaluate(int SpreadMax)
{
if (this.FWorld.IsConnected)
{
int contcnt = this.FWorld[0].Dispatcher.NumManifolds;
this.FBody1.SliceCount = contcnt;
this.FBody2.SliceCount = contcnt;
this.FContactPoints.SliceCount = contcnt;
for (int i = 0; i < contcnt; i++)
{
PersistentManifold pm = this.FWorld[0].Dispatcher.GetManifoldByIndexInternal(i);
RigidBody b1 = RigidBody.Upcast((CollisionObject)pm.Body0);
RigidBody b2 = RigidBody.Upcast((CollisionObject)pm.Body1);
this.FBody1[i] = b1;
this.FBody2[i] = b2;
this.FContactPoints[i].SliceCount = pm.NumContacts;
for (int j = 0; j < pm.NumContacts; j++)
{
this.FContactPoints[i][j] = pm.GetContactPoint(j);
}
}
}
else
{
FBody1.SliceCount = 0;
FBody2.SliceCount = 0;
}
}
public static void TickCallback(DynamicsWorld world, float timeStep)
{
int numManifolds = colWorld.Dispatcher.NumManifolds;
for (int i = 0; i < numManifolds; i++)
{
PersistentManifold contactManifold = colWorld.Dispatcher.GetManifoldByIndexInternal(i);
// CollisionObject obA = contactManifold.Body0 as CollisionObject;
// CollisionObject obB = contactManifold.Body1 as CollisionObject;
var entity1 = (contactManifold.Body0 as RigidBody).UserObject as Entity;
var entity2 = (contactManifold.Body1 as RigidBody).UserObject as Entity;
int numContacts = contactManifold.NumContacts;
for (int j = 0; j < numContacts; j++)
{
ManifoldPoint pt = contactManifold.GetContactPoint(j);
if (pt.Distance < 1.0f)
{
if (HardCollisionAction != null)
{
if (entity1.CollisionType == CollisionTypeEnum.Soft &&
entity2.CollisionType == CollisionTypeEnum.Soft)
{
SoftCollisionAction();
}
else
{
HardCollisionAction();
}
}
return;
}
}
}
}
//Check Collisions
private void OnPhysicsStep()
{
numManifolds = dispatcher.NumManifolds;
for (int i = 0; i < numManifolds; i++)
{
contactManifold = dispatcher.GetManifoldByIndexInternal(i);
a = contactManifold.Body0;
b = contactManifold.Body1;
if (a is CollisionObject && a.UserObject is BCollisionObject && ((BCollisionObject)a.UserObject).collisionCallbackEventHandler != null)
{
((BCollisionObject)a.UserObject).collisionCallbackEventHandler.OnVisitPersistentManifold(contactManifold);
}
if (b is CollisionObject && b.UserObject is BCollisionObject && ((BCollisionObject)b.UserObject).collisionCallbackEventHandler != null)
{
((BCollisionObject)b.UserObject).collisionCallbackEventHandler.OnVisitPersistentManifold(contactManifold);
}
}
foreach (BCollisionObject.BICollisionCallbackEventHandler coeh in collisionCallbackListeners)
{
if (coeh != null)
{
coeh.OnFinishedVisitingManifolds();
}
}
contactManifold = null;
a = b = null;
}
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 BoxBoxCollisionAlgorithm(PersistentManifold mf,CollisionAlgorithmConstructionInfo ci,CollisionObject body0,CollisionObject body1) : base(ci)
{
if (m_manifoldPtr == null && m_dispatcher.NeedsCollision(body0,body1))
{
m_manifoldPtr = m_dispatcher.GetNewManifold(body0,body1);
m_ownManifold = true;
}
}
/// <summary>
/// Called for each manifold iterated over.
/// </summary>
/// <param name="pm"></param>
public override void OnVisitPersistentManifold(PersistentManifold pm)
{
for (int i = 0; i < callbacks.Count; i++)
{
callbacks[i].OnVisitPersistentManifold(pm);
}
base.OnVisitPersistentManifold(pm);
}
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 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;
}
static SphereSphereCollisionAlgorithm AllocFromPool(PersistentManifold mf,CollisionAlgorithmConstructionInfo ci,CollisionObject body0,CollisionObject body1)
{
SphereSphereCollisionAlgorithm result;
if (ObjPool.Count > 0)
result = ObjPool.Dequeue();
else
result = new SphereSphereCollisionAlgorithm();
result.Constructor(mf, ci, body0, body1);
return result;
}
/// Clears all data instanced by this algorithm
/// if any (like persistent contact manifolds)
public override void Clear(object o)
{
int numManifolds = bt_collision_world.GetDispatcher().GetNumManifolds();
for (int i = 0; i < numManifolds; i++)
{
PersistentManifold contactManifold = bt_collision_world.GetDispatcher().GetManifoldByIndexInternal(i);
contactManifold.ClearManifold();
}
}
static ConvexPlaneCollisionAlgorithm AllocFromPool(PersistentManifold mf, CollisionAlgorithmConstructionInfo ci, CollisionObject col0, CollisionObject col1, bool isSwapped, int numPerturbationIterations, int minimumPointsPerturbationThreshold)
{
ConvexPlaneCollisionAlgorithm result;
if (ObjPool.Count > 0)
result = ObjPool.Dequeue();
else
result = new ConvexPlaneCollisionAlgorithm();
result.Constructor(mf, ci, col0, col1, isSwapped, numPerturbationIterations, minimumPointsPerturbationThreshold);
return result;
}
public CompoundLeafCallback (CollisionObject compoundObj,CollisionObject otherObj,IDispatcher dispatcher,DispatcherInfo dispatchInfo,ManifoldResult resultOut,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;
}
static SphereBoxCollisionAlgorithm AllocFromPool(PersistentManifold mf,CollisionAlgorithmConstructionInfo ci,CollisionObject col0,CollisionObject col1, bool isSwapped)
{
SphereBoxCollisionAlgorithm result;
if (ObjPool.Count > 0)
result = ObjPool.Dequeue();
else
result = new SphereBoxCollisionAlgorithm();
result.Constructor(mf, ci, col0, col1,isSwapped);
return result;
}
static CollisionAlgorithm AllocFromPool(PersistentManifold mf, CollisionAlgorithmConstructionInfo ci, CollisionObject body0, CollisionObject body1, ISimplexSolver simplexSolver, IConvexPenetrationDepthSolver pdSolver, int numPerturbationIterations, int minimumPointsPerturbationThreshold)
{
ConvexConvexAlgorithm result;
if (ObjPool.Count > 0)
result = ObjPool.Dequeue();
else
result = new ConvexConvexAlgorithm();
result.Constructor(mf, ci, body0, body1, simplexSolver, pdSolver, numPerturbationIterations, minimumPointsPerturbationThreshold);
return result;
}
public virtual void releaseManifold(PersistentManifold manifold)
{
clearManifold(manifold);
int findIndex = manifold.m_index1a;
PersistentManifold temp = m_manifoldsPtr[m_manifoldsPtr.Count - 1];
m_manifoldsPtr[m_manifoldsPtr.Count - 1] = m_manifoldsPtr[findIndex];
m_manifoldsPtr[findIndex] = temp;
m_manifoldsPtr[findIndex].m_index1a = findIndex;
m_manifoldsPtr.RemoveAt(m_manifoldsPtr.Count - 1);
}
public SphereBoxCollisionAlgorithm(PersistentManifold mf,CollisionAlgorithmConstructionInfo ci,CollisionObject col0,CollisionObject col1, bool isSwapped) : base(ci,col0,col1)
{
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 override void Cleanup()
{
if (m_ownManifold)
{
if (m_manifoldPtr != null)
{
m_dispatcher.ReleaseManifold(m_manifoldPtr);
}
m_ownManifold = false;
}
m_manifoldPtr = null;
}
public void Constructor(PersistentManifold mf, CollisionAlgorithmConstructionInfo ci, CollisionObject col0, CollisionObject col1)
{
base.Constructor(ci);
m_ownManifold = false;
m_manifoldPtr = mf;
if (m_manifoldPtr == null)
{
m_manifoldPtr = m_dispatcher.getNewManifold(col0, col1);
m_ownManifold = true;
}
}
public ManifoldResult(CollisionObject body0, CollisionObject body1)
{
m_manifoldPtr = null;
m_body0 = body0;
m_body1 = body1;
m_rootTransA = body0.WorldTransform;
m_rootTransB = body1.WorldTransform;
m_partId0 = -1;
m_partId1 = -1;
m_index0 = -1;
m_index1 = -1;
}
/// <summary>
/// Finds any robot collisions and adds them to the list of contact points.
/// </summary>
/// <param name="pm"></param>
public void OnVisitPersistentManifold(PersistentManifold pm)
{
if (!mainState.Tracking)
{
return;
}
if (framesPassed == -1)
{
framesPassed = physicsWorld.frameCount - lastFrameCount;
for (int i = 0; i < framesPassed; i++)
{
ContactPoints.Add(new List <ContactDescriptor>());
}
}
BRigidBody obA = pm.Body0.UserObject as BRigidBody;
BRigidBody obB = pm.Body1.UserObject as BRigidBody;
BRigidBody robotBody = obA != null && obA.gameObject.name.StartsWith("node") ? obA : obB != null && obB.gameObject.name.StartsWith("node") ? obB : null;
if (robotBody == null)
{
return;
}
if (pm.NumContacts < 1)
{
return;
}
int numContacts = pm.NumContacts;
for (int i = 0; i < Math.Min(framesPassed, numContacts); i++)
{
ManifoldPoint mp = pm.GetContactPoint(i);
ContactDescriptor cd = new ContactDescriptor
{
AppliedImpulse = mp.AppliedImpulse,
Position = (mp.PositionWorldOnA + mp.PositionWorldOnB) * 0.5f,
RobotBody = robotBody
};
if (ContactPoints[i] != null)
{
ContactPoints[i].Add(cd);
}
}
}
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;
}
public override void OnVisitPersistentManifold(PersistentManifold pm)
{
if (myCollisionObject == null)
{
return;
}
CollisionObject other;
if (pm.NumContacts > 0)
{
if (pm.Body0 == myCollisionObject)
{
other = pm.Body1;
}
else
{
other = pm.Body0;
}
PersistentManifoldList pml;
if (!otherObjs2ManifoldMap.TryGetValue(other, out pml))
{
//todo get PersistentManifoldList from object pool
//this is first contact with this other object
//might have multiple new contacts with same object stored in separate persistent manifolds
//don't add two different lists to new contacts
bool foundExisting = false;
for (int i = 0; i < newContacts.Count; i++)
{
if (newContacts[i].manifolds[0].Body0 == other ||
newContacts[i].manifolds[0].Body1 == other)
{
foundExisting = true;
newContacts[i].manifolds.Add(pm);
}
}
if (!foundExisting)
{
pml = new PersistentManifoldList();
newContacts.Add(pml);
pml.manifolds.Add(pm);
//don't add to otherObjs2ManifoldMap here. It messes up onStay do it after all pm's have been visited.
}
}
else
{
pml.manifolds.Add(pm);
}
}
}
private void ObstaculoCollision(PersistentManifold contactManifold, int misilId, int objetoId)
{
int numContacts = contactManifold.NumContacts;
for (int j = 0; j < numContacts; j++)
{
ManifoldPoint pt = contactManifold.GetContactPoint(j);
double ptdist = pt.Distance;
if (Math.Abs(ptdist) < epsilonContact)
{
listaColisionesObstaculoMisil.Add(new Colision(objetoId, misilId));
listaObstaculos[objetoId].Destruir();
}
}
}
private void TorretaCollision(PersistentManifold contactManifold, int misilId, int objetoId)
{
int numContacts = contactManifold.NumContacts;
for (int j = 0; j < numContacts; j++)
{
ManifoldPoint pt = contactManifold.GetContactPoint(j);
double ptdist = pt.Distance;
Console.WriteLine(ptdist);
if (Math.Abs(ptdist) < 6)
{
listaColisionesTorretaMisil.Add(new Colision(objetoId, misilId));
listaTorretas[objetoId].DisminuirVida();
}
}
}
protected void Constructor(PersistentManifold mf, CollisionAlgorithmConstructionInfo ci, CollisionObject col0, CollisionObject col1, bool isSwapped)
{
base.Constructor(ci);
m_ownManifold = false;
m_manifoldPtr=mf;
m_isSwapped=isSwapped;
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;
}
}
private void XwingCollision(PersistentManifold contactManifold, int misilId)
{
int numContacts = contactManifold.NumContacts;
for (int j = 0; j < numContacts && !listaIdMisilesQueColisionaronConXwing.Contains(misilId); j++)
{
ManifoldPoint pt = contactManifold.GetContactPoint(j);
double ptdist = pt.Distance;
if (Math.Abs(ptdist) < epsilonContact)
{
listaIdMisilesQueColisionaronConXwing.Add(misilId);
collisionWorld.RemoveCollisionObject(listaMisilesEnemigo[misilId]);
listaMisilesEnemigo.Remove(misilId);
VariablesGlobales.RestarVida();
}
}
}
public ConvexPlaneCollisionAlgorithm(PersistentManifold mf, CollisionAlgorithmConstructionInfo ci, CollisionObject col0,CollisionObject col1,bool isSwapped,int numPerturbationIterations,int minimumPointsPerturbationThreshold) : base(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;
}
}
void OnContactDestroyed(object userPersistantData)
{
int numManifolds = BtWorld.Dispatcher.NumManifolds;
for (int i = 0; i < numManifolds; i++)
{
PersistentManifold contactManifold = BtWorld.Dispatcher.GetManifoldByIndexInternal(i);
int numContacts = contactManifold.NumContacts;
if (numContacts > 0)
{
CollisionObject obA = (CollisionObject)contactManifold.Body0;
CollisionObject obB = (CollisionObject)contactManifold.Body1;
obA.CollisionFlags = CollisionFlags.CustomMaterialCallback;
obB.CollisionFlags = CollisionFlags.CustomMaterialCallback;
}
}
// Debug.WriteLine("OnContactDestroyed");
}
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();
}
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 OnVisitPersistentManifold(PersistentManifold pm)
{
CollisionObject other;
if (pm.Body0 == myCollisionObject)
{
other = pm.Body1;
}
else
{
other = pm.Body0;
}
PersistentManifoldList pml;
if (!otherObjs2ManifoldMap.TryGetValue(other, out pml))
{
//todo get from object pool
pml = new PersistentManifoldList();
newContacts.Add(pml);
}
pml.manifolds.Add(pm);
}
public override void Update(TimeSpan deltaTime)
{
float second = (float)deltaTime.TotalSeconds;
world.StepSimulation(second);
// on collision events
int numManifolds = world.Dispatcher.NumManifolds;
for (int i = 0; i < numManifolds; i++)
{
PersistentManifold manifold = world.Dispatcher.GetManifoldByIndexInternal(i);
CollisionProxy colA = (CollisionProxy)manifold.Body0.UserObject;
CollisionProxy colB = (CollisionProxy)manifold.Body1.UserObject;
// check if both are not null
if (colA != null && colB != null)
{
// execute colA -> colB event
try
{
colA.ExecuteCollision(colA, colB);
}
catch (Exception e)
{
LogError(string.Format("{0} {1}", e.Message, e.StackTrace));
}
// execute colB -> colA event
try
{
colB.ExecuteCollision(colB, colA);
}
catch (Exception e)
{
LogError(string.Format("{0} {1}", e.Message, e.StackTrace));
}
}
}
}
static void TestManifoldPoints()
{
int numManifolds = world.Dispatcher.NumManifolds;
for (int i = 0; i < numManifolds; i++)
{
PersistentManifold contactManifold = world.Dispatcher.GetManifoldByIndexInternal(i);
CollisionObject obA = contactManifold.Body0 as CollisionObject;
CollisionObject obB = contactManifold.Body1 as CollisionObject;
int numContacts = contactManifold.NumContacts;
for (int j = 0; j < numContacts; j++)
{
ManifoldPoint pt = contactManifold.GetContactPoint(j);
if (pt.Distance < 0.0f)
{
Vector3 ptA = pt.PositionWorldOnA;
Vector3 ptB = pt.PositionWorldOnB;
Vector3 normalOnB = pt.NormalWorldOnB;
}
}
}
}
public virtual void Update(float elapsedTime)
{
World.StepSimulation(elapsedTime);
for (int i = 0; i < World.Dispatcher.NumManifolds; ++i)
{
PersistentManifold manifold = World.Dispatcher.GetManifoldByIndexInternal(i);
RigidBody A = manifold.Body0 as RigidBody;
RigidBody B = manifold.Body1 as RigidBody;
if (A.UserObject != null && A.UserObject.Equals("Ground"))
{
World.RemoveRigidBody(B);
}
else
if (B.UserObject != null && B.UserObject.Equals("Ground"))
{
World.RemoveRigidBody(A);
}
}
// Console.WriteLine(elapsedTime);
}
// 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;
}
}
}
}
public static int GetIslandId(PersistentManifold lhs)
{
int islandId;
CollisionObject rcolObj0 = (CollisionObject)(lhs.GetBody0());
CollisionObject rcolObj1 = (CollisionObject)(lhs.GetBody1());
islandId = rcolObj0.GetIslandTag()>=0?rcolObj0.GetIslandTag():rcolObj1.GetIslandTag();
return islandId;
}
private static int PersistentManifoldSortPredicate(PersistentManifold lhs, PersistentManifold rhs)
{
int rIslandIdA, lIslandIdB;
rIslandIdA = GetIslandId(rhs);
lIslandIdB = GetIslandId(lhs);
//return lIslandId0 < rIslandId0;
if (lIslandIdB < rIslandIdA)
return -1;
//else if (lIslandIdB > rIslandIdA)
// return 1;
return 1;
}
public void SetPersistentManifold(PersistentManifold manifoldPtr)
{
m_manifoldPtr = manifoldPtr;
}
private void UpdateTrackers()
{
int numSteps = physicsWorld.frameCount - lastFrameCount;
if (tracking && numSteps > 0)
{
foreach (Tracker t in Trackers)
{
t.AddState(numSteps);
}
for (int i = numSteps; i > 0; i--)
{
List <ContactDescriptor> frameContacts = null;
int numManifolds = physicsWorld.world.Dispatcher.NumManifolds;
for (int j = 0; j < numManifolds; j++)
{
PersistentManifold contactManifold = physicsWorld.world.Dispatcher.GetManifoldByIndexInternal(j);
BRigidBody obA = (BRigidBody)contactManifold.Body0.UserObject;
BRigidBody obB = (BRigidBody)contactManifold.Body1.UserObject;
if (!obA.gameObject.name.StartsWith("node") && !obB.gameObject.name.StartsWith("node"))
{
continue;
}
ManifoldPoint mp = null;
int numContacts = contactManifold.NumContacts;
for (int k = 0; k < numContacts; k++)
{
mp = contactManifold.GetContactPoint(k);
if (mp.LifeTime == i)
{
break;
}
}
if (mp == null)
{
continue;
}
if (frameContacts == null)
{
frameContacts = new List <ContactDescriptor>();
}
frameContacts.Add(new ContactDescriptor
{
AppliedImpulse = mp.AppliedImpulse,
Position = (mp.PositionWorldOnA + mp.PositionWorldOnB) * 0.5f,
RobotBody = obA.name.StartsWith("node") ? obA : obB
});
}
contactPoints.Add(frameContacts);
}
}
lastFrameCount += numSteps;
}
protected void ConvertContact(PersistentManifold manifold, ContactSolverInfo infoGlobal)
{
CollisionObject colObj0 = null, colObj1 = null;
colObj0 = manifold.GetBody0() as CollisionObject;
colObj1 = manifold.GetBody1() as CollisionObject;
RigidBody solverBodyA = RigidBody.Upcast(colObj0);
RigidBody solverBodyB = 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 (cp.GetDistance() <= manifold.GetContactProcessingThreshold())
{
// Vector3 pos1 = cp.getPositionWorldOnA();
// Vector3 pos2 = cp.getPositionWorldOnB();
Vector3 rel_pos1 = Vector3.Zero;
Vector3 rel_pos2 = Vector3.Zero;
//;
float relaxation = 1f;
float rel_vel = 0f;
Vector3 vel = Vector3.Zero;
int frictionIndex = m_tmpSolverContactConstraintPool.Count;
SolverConstraint solverConstraint = new SolverConstraint();
//m_tmpSolverContactConstraintPool.Add(solverConstraint);
RigidBody rb0 = RigidBody.Upcast(colObj0);
RigidBody rb1 = RigidBody.Upcast(colObj1);
if (BulletGlobals.g_streamWriter != null && rb0 != null && debugSolver)
{
MathUtil.PrintContactPoint(BulletGlobals.g_streamWriter, cp);
}
solverConstraint.m_solverBodyA = rb0 != null ? rb0 : GetFixedBody();
solverConstraint.m_solverBodyB = rb1 != null ? rb1 : GetFixedBody();
solverConstraint.m_originalContactPoint = cp;
SetupContactConstraint(ref solverConstraint, colObj0, colObj1, cp, infoGlobal, ref vel, ref rel_vel, ref relaxation, ref rel_pos1, ref rel_pos2);
if (BulletGlobals.g_streamWriter != null && debugSolver)
{
TypedConstraint.PrintSolverConstraint(BulletGlobals.g_streamWriter, solverConstraint, 99);
}
/////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)System.Math.Sqrt(lat_rel_vel);
if (TestSolverMode(infoGlobal.m_solverMode, SolverMode.SOLVER_USE_2_FRICTION_DIRECTIONS))
{
cp.m_lateralFrictionDir2 = Vector3.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
TransformUtil.PlaneSpace1(ref cp.m_normalWorldOnB, ref cp.m_lateralFrictionDir1, ref 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);
m_tmpSolverContactConstraintPool.Add(solverConstraint);
}
}
}
public void OnVisitPersistentManifold(PersistentManifold pm)
{
// Not implemented
}
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);
}
public CollisionAlgorithmConstructionInfo(IDispatcher dispatcher)
{
m_dispatcher1 = dispatcher;
m_manifold = null;
}
public void SetContactManifold(PersistentManifold contactManifold)
{
m_contactManifold = contactManifold;
}
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 ContactConstraint(PersistentManifold contactManifold, RigidBody rbA, RigidBody rbB) : base(TypedConstraintType.CONTACT_CONSTRAINT_TYPE,rbA,rbB)
{
m_contactManifold = contactManifold;
}
public virtual void clearManifold(PersistentManifold manifold)
{
manifold.clearManifold();
}
private static int getIslandId(PersistentManifold lhs)
{
int islandId;
CollisionObject rcolObj0 = (CollisionObject)(lhs.Body0);
CollisionObject rcolObj1 = (CollisionObject)(lhs.Body1);
islandId = rcolObj0.IslandTag >= 0 ? rcolObj0.IslandTag : rcolObj1.IslandTag;
return islandId;
}
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 = (ConvexShape)(body0.GetCollisionShape());
ConvexShape min1 = (ConvexShape)(body1.GetCollisionShape());
Vector3 normalOnB = Vector3.Up;
Vector3 pointOnBWorld = Vector3.Zero;
#if !BT_DISABLE_CAPSULE_CAPSULE_COLLIDER
if ((min0.ShapeType == BroadphaseNativeTypes.CAPSULE_SHAPE_PROXYTYPE) && (min1.ShapeType == BroadphaseNativeTypes.CAPSULE_SHAPE_PROXYTYPE))
{
CapsuleShape capsuleA = (CapsuleShape) min0;
CapsuleShape capsuleB = (CapsuleShape) min1;
Vector3 localScalingA = capsuleA.GetLocalScaling();
Vector3 localScalingB = capsuleB.GetLocalScaling();
float threshold = m_manifoldPtr.GetContactBreakingThreshold();
float dist = CapsuleCapsuleDistance(ref normalOnB,ref 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 = new ClosestPointInput();
GjkPairDetector gjkPairDetector = new GjkPairDetector(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.Margin + min1.Margin + 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();
gjkPairDetector.GetClosestPoints(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)
{
Vector3 v0 = Vector3.Zero, v1 = Vector3.Zero;
Vector3 sepNormalWorldSpace = gjkPairDetector.GetCachedSeparatingAxis();
sepNormalWorldSpace.Normalize();
TransformUtil.PlaneSpace1(ref sepNormalWorldSpace, ref v0, ref 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;
}
Matrix unPerturbedTransform = Matrix.Identity;
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)
{
Quaternion perturbeRot = Quaternion.CreateFromAxisAngle(v0, perturbeAngle);
float iterationAngle = i * (MathUtil.SIMD_2_PI / (float)m_numPerturbationIterations);
Quaternion rotq = Quaternion.CreateFromAxisAngle(sepNormalWorldSpace, iterationAngle);
if (perturbeA)
{
//input.m_transformA.setBasis( btMatrix3x3(rotq.inverse()*perturbeRot*rotq)*body0.getWorldTransform().getBasis());
Quaternion temp = MathUtil.QuaternionMultiply(MathUtil.QuaternionInverse(ref rotq),MathUtil.QuaternionMultiply(perturbeRot,rotq));
input.m_transformA = MathUtil.BulletMatrixMultiplyBasis(Matrix.CreateFromQuaternion(temp),body0.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.setBasis( btMatrix3x3(rotq.inverse()*perturbeRot*rotq)*body1.getWorldTransform().getBasis());
Quaternion temp = MathUtil.QuaternionMultiply(MathUtil.QuaternionInverse(ref rotq),MathUtil.QuaternionMultiply(perturbeRot,rotq));
input.m_transformB = MathUtil.BulletMatrixMultiplyBasis(Matrix.CreateFromQuaternion(temp),body1.GetWorldTransform());
#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(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 SphereSphereCollisionAlgorithm(PersistentManifold mf,CollisionAlgorithmConstructionInfo ci,CollisionObject body0,CollisionObject body1) : base(ci,body0,body1)
{
}
public virtual float SolveGroup(List <CollisionObject> bodies, List <PersistentManifold> manifolds, int numManifolds, List <TypedConstraint> constraints, ContactSolverInfo infoGlobal, IDebugDraw debugDrawer)
{
if ((_solverMode & SolverMode.CacheFriendly) != SolverMode.None)
{
return(SolveGroupCacheFriendly(bodies, manifolds, numManifolds, constraints, infoGlobal, debugDrawer));
}
ContactSolverInfo info = infoGlobal;
int totalPoints = 0;
int numiter = infoGlobal.IterationsCount;
for (int j = 0; j < manifolds.Count; j++)
{
PersistentManifold manifold = manifolds[j];
PrepareConstraints(manifold, info);
for (int p = 0; p < manifolds[j].ContactsCount; p++)
{
_order[totalPoints].ManifoldIndex = j;
_order[totalPoints].PointIndex = p;
totalPoints++;
}
}
for (int j = 0; j < constraints.Count; j++)
{
constraints[j].BuildJacobian();
}
//should traverse the contacts random order...
int iteration;
for (iteration = 0; iteration < numiter; iteration++)
{
int j;
if ((_solverMode & SolverMode.RandomizeOrder) != SolverMode.None)
{
if ((iteration & 7) == 0)
{
for (j = 0; j < totalPoints; ++j)
{
OrderIndex tmp = _order[j];
int swapi = RandInt2(j + 1);
_order[j] = _order[swapi];
_order[swapi] = tmp;
}
}
}
for (j = 0; j < constraints.Count; j++)
{
constraints[j].SolveConstraint(info.TimeStep);
}
for (j = 0; j < totalPoints; j++)
{
PersistentManifold manifold = manifolds[_order[j].ManifoldIndex];
Solve((RigidBody)manifold.BodyA, (RigidBody)manifold.BodyB,
manifold.GetContactPoint(_order[j].PointIndex), info, iteration, debugDrawer);
}
for (j = 0; j < totalPoints; j++)
{
PersistentManifold manifold = manifolds[_order[j].ManifoldIndex];
SolveFriction((RigidBody)manifold.BodyA,
(RigidBody)manifold.BodyB, manifold.GetContactPoint(_order[j].PointIndex), info, iteration, debugDrawer);
}
}
return(0);
}
public override void processCollision(CollisionObject body0, CollisionObject body1, DispatcherInfo dispatchInfo, ref ManifoldResult resultOut)
{
if (m_manifoldPtr == null)
{
//swapped?
m_manifoldPtr = m_dispatcher.getNewManifold(body0, body1);
m_ownManifold = true;
}
resultOut.PersistentManifold = m_manifoldPtr;
//comment-out next line to test multi-contact generation
//resultOut->getPersistentManifold()->clearManifold();
ConvexShape min0 = (ConvexShape)(body0.CollisionShape);
ConvexShape min1 = (ConvexShape)(body1.CollisionShape);
btVector3 normalOnB;
btVector3 pointOnBWorld;
if ((min0.ShapeType == BroadphaseNativeTypes.CAPSULE_SHAPE_PROXYTYPE) && (min1.ShapeType == BroadphaseNativeTypes.CAPSULE_SHAPE_PROXYTYPE))
{
CapsuleShape capsuleA = (CapsuleShape)min0;
CapsuleShape capsuleB = (CapsuleShape)min1;
btVector3 localScalingA = capsuleA.LocalScaling;
btVector3 localScalingB = capsuleB.LocalScaling;
float threshold = m_manifoldPtr.ContactBreakingThreshold;
float dist = capsuleCapsuleDistance(out normalOnB, out pointOnBWorld, capsuleA.HalfHeight, capsuleA.Radius,
capsuleB.HalfHeight, capsuleB.Radius, capsuleA.UpAxis, capsuleB.UpAxis,
body0.WorldTransform, body1.WorldTransform, threshold);
if (dist < threshold)
{
Debug.Assert(normalOnB.Length2 >= (BulletGlobal.SIMD_EPSILON * BulletGlobal.SIMD_EPSILON));
resultOut.addContactPoint(ref normalOnB, ref pointOnBWorld, dist);
}
resultOut.refreshContactPoints();
return;
}
#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;
GjkPairDetector gjkPairDetector = new GjkPairDetector(min0, min1, m_simplexSolver, m_pdSolver);
//TODO: if (dispatchInfo.m_useContinuous)
gjkPairDetector.MinkowskiA = min0;
gjkPairDetector.MinkowskiB = min1;
#if USE_SEPDISTANCE_UTIL2
if (dispatchInfo.m_useConvexConservativeDistanceUtil)
{
input.m_maximumDistanceSquared = BT_LARGE_FLOAT;
} else
#endif //USE_SEPDISTANCE_UTIL2
{
input.m_maximumDistanceSquared = min0.Margin + min1.Margin + m_manifoldPtr.ContactBreakingThreshold;
input.m_maximumDistanceSquared *= input.m_maximumDistanceSquared;
}
//input.m_stackAlloc = dispatchInfo.m_stackAllocator;
input.m_transformA = body0.WorldTransform;
input.m_transformB = body1.WorldTransform;
gjkPairDetector.getClosestPoints(ref input, ref resultOut, dispatchInfo.m_debugDraw);
#if USE_SEPDISTANCE_UTIL2
btScalar sepDist = 0.f;
if (dispatchInfo.m_useConvexConservativeDistanceUtil)
{
sepDist = gjkPairDetector.getCachedSeparatingDistance();
if (sepDist>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.PersistentManifold.NumContacts < m_minimumPointsPerturbationThreshold)
{
int i;
btVector3 v0, v1;
btVector3 sepNormalWorldSpace;
//sepNormalWorldSpace = gjkPairDetector.getCachedSeparatingAxis().normalized();
gjkPairDetector.getCachedSeparatingAxis().normalized(out sepNormalWorldSpace);
btVector3.PlaneSpace1(ref sepNormalWorldSpace, out v0, out v1);
bool perturbeA = true;
float angleLimit = 0.125f * BulletGlobal.SIMD_PI;
float perturbeAngle;
float radiusA = min0.getAngularMotionDisc();
float radiusB = min1.getAngularMotionDisc();
if (radiusA < radiusB)
{
perturbeAngle = PersistentManifold.gContactBreakingThreshold / radiusA;
perturbeA = true;
}
else
{
perturbeAngle = PersistentManifold.gContactBreakingThreshold / radiusB;
perturbeA = false;
}
if (perturbeAngle > angleLimit)
perturbeAngle = angleLimit;
btTransform unPerturbedTransform;
if (perturbeA)
{
unPerturbedTransform = input.m_transformA;
}
else
{
unPerturbedTransform = input.m_transformB;
}
for (i = 0; i < m_numPerturbationIterations; i++)
{
if (v0.Length2 > BulletGlobal.SIMD_EPSILON)
{
btQuaternion perturbeRot = new btQuaternion(v0, perturbeAngle);
float iterationAngle = i * (BulletGlobal.SIMD_2_PI / m_numPerturbationIterations);
btQuaternion rotq = new btQuaternion(sepNormalWorldSpace, iterationAngle);
if (perturbeA)
{
#region input.m_transformA.Basis = new btMatrix3x3(rotq.inverse() * perturbeRot * rotq) * body0.WorldTransform.Basis;
{
btMatrix3x3 temp = new btMatrix3x3(rotq.inverse() * perturbeRot * rotq);
btMatrix3x3.Multiply(ref temp, ref body0.WorldTransform.Basis, out input.m_transformA.Basis);
}
#endregion
input.m_transformB = body1.WorldTransform;
#if DEBUG
dispatchInfo.m_debugDraw.drawTransform(ref input.m_transformA, 10.0f);
#endif //DEBUG_CONTACTS
}
else
{
input.m_transformA = body0.WorldTransform;
#region input.m_transformB.Basis = new btMatrix3x3(rotq.inverse() * perturbeRot * rotq) * body1.WorldTransform.Basis;
{
btMatrix3x3 temp = new btMatrix3x3(rotq.inverse() * perturbeRot * rotq);
btMatrix3x3.Multiply(ref temp, ref body1.WorldTransform.Basis, out input.m_transformB.Basis);
}
#endregion
#if DEBUG
dispatchInfo.m_debugDraw.drawTransform(ref input.m_transformB, 10.0f);
#endif
}
PerturbedContactResult perturbedResultOut = new PerturbedContactResult(input.m_transformA, input.m_transformB, unPerturbedTransform, perturbeA, dispatchInfo.m_debugDraw);
gjkPairDetector.getClosestPoints(ref input, ref perturbedResultOut, ref resultOut, dispatchInfo.m_debugDraw);
}
}
}
#if USE_SEPDISTANCE_UTIL2
if (dispatchInfo.m_useConvexConservativeDistanceUtil && (sepDist>SIMD_EPSILON))
{
m_sepDistance.initSeparatingDistance(gjkPairDetector.getCachedSeparatingAxis(),sepDist,body0->getWorldTransform(),body1->getWorldTransform());
}
#endif //USE_SEPDISTANCE_UTIL2
}
if (m_ownManifold)
{
resultOut.refreshContactPoints();
}
}
public abstract void OnVisitPersistentManifold(PersistentManifold pm);
public void OnVisitPersistentManifold(PersistentManifold pm)
{
lastManifolds.Add(pm);
}
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;
}
protected void PrepareConstraints(PersistentManifold manifold, ContactSolverInfo info)
{
RigidBody body0 = manifold.BodyA as RigidBody;
RigidBody body1 = manifold.BodyB as RigidBody;
//only necessary to refresh the manifold once (first iteration). The integration is done outside the loop
{
manifold.RefreshContactPoints(body0.CenterOfMassTransform, body1.CenterOfMassTransform);
int numpoints = manifold.ContactsCount;
_totalContactPoints += numpoints;
Vector3 color = new Vector3(0, 1, 0);
for (int i = 0; i < numpoints; i++)
{
ManifoldPoint cp = manifold.GetContactPoint(i);
if (cp.Distance <= 0)
{
Vector3 pos1 = cp.PositionWorldOnA;
Vector3 pos2 = cp.PositionWorldOnB;
Vector3 rel_pos1 = pos1 - body0.CenterOfMassPosition;
Vector3 rel_pos2 = pos2 - body1.CenterOfMassPosition;
//this jacobian entry is re-used for all iterations
JacobianEntry jac = new JacobianEntry(MatrixOperations.Transpose(body0.CenterOfMassTransform),
MatrixOperations.Transpose(body1.CenterOfMassTransform),
rel_pos1, rel_pos2, cp.NormalWorldOnB, body0.InvInertiaDiagLocal, body0.InverseMass,
body1.InvInertiaDiagLocal, body1.InverseMass);
float jacDiagAB = jac.Diagonal;
ConstraintPersistentData cpd = cp.UserPersistentData as ConstraintPersistentData;
if (cpd != null)
{
//might be invalid
cpd.PersistentLifeTime++;
if (cpd.PersistentLifeTime != cp.LifeTime)
{
//printf("Invalid: cpd->m_persistentLifeTime = %i cp.getLifeTime() = %i\n",cpd->m_persistentLifeTime,cp.getLifeTime());
cpd = new ConstraintPersistentData();
cpd.PersistentLifeTime = cp.LifeTime;
}
}
else
{
cpd = new ConstraintPersistentData();
_totalCpd++;
//printf("totalCpd = %i Created Ptr %x\n",totalCpd,cpd);
cp.UserPersistentData = cpd;
cpd.PersistentLifeTime = cp.LifeTime;
//printf("CREATED: %x . cpd->m_persistentLifeTime = %i cp.getLifeTime() = %i\n",cpd,cpd->m_persistentLifeTime,cp.getLifeTime());
}
if (cpd == null)
throw new BulletException();
cpd.JacDiagABInv = 1f / jacDiagAB;
//Dependent on Rigidbody A and B types, fetch the contact/friction response func
//perhaps do a similar thing for friction/restutution combiner funcs...
cpd.FrictionSolverFunc = _frictionDispatch[(int)body0.FrictionSolverType, (int)body1.FrictionSolverType];
cpd.ContactSolverFunc = _contactDispatch[(int)body0.ContactSolverType, (int)body1.ContactSolverType];
Vector3 vel1 = body0.GetVelocityInLocalPoint(rel_pos1);
Vector3 vel2 = body1.GetVelocityInLocalPoint(rel_pos2);
Vector3 vel = vel1 - vel2;
float rel_vel;
rel_vel = Vector3.Dot(cp.NormalWorldOnB, vel);
float combinedRestitution = cp.CombinedRestitution;
cpd.Penetration = cp.Distance;
cpd.Friction = cp.CombinedFriction;
cpd.Restitution = RestitutionCurve(rel_vel, combinedRestitution);
if (cpd.Restitution < 0f)
{
cpd.Restitution = 0.0f;
};
//restitution and penetration work in same direction so
//rel_vel
float penVel = -cpd.Penetration / info.TimeStep;
if (cpd.Restitution > penVel)
{
cpd.Penetration = 0;
}
float relaxation = info.Damping;
if ((_solverMode & SolverMode.UseWarmstarting) != 0)
{
cpd.AppliedImpulse *= relaxation;
}
else
{
cpd.AppliedImpulse = 0f;
}
//for friction
cpd.PreviousAppliedImpulse = cpd.AppliedImpulse;
//re-calculate friction direction every frame, todo: check if this is really needed
Vector3 fwta = cpd.FrictionWorldTangentialA;
Vector3 fwtb = cpd.FrictionWorldTangentialB;
MathHelper.PlaneSpace1(cp.NormalWorldOnB, ref fwta, ref fwtb);
cpd.FrictionWorldTangentialA = fwta;
cpd.FrictionWorldTangentialB = fwtb;
cpd.AccumulatedTangentImpulseA = 0;
cpd.AccumulatedTangentImpulseB = 0;
float denom0 = body0.ComputeImpulseDenominator(pos1, cpd.FrictionWorldTangentialA);
float denom1 = body1.ComputeImpulseDenominator(pos2, cpd.FrictionWorldTangentialA);
float denom = relaxation / (denom0 + denom1);
cpd.JacDiagABInvTangentA = denom;
denom0 = body0.ComputeImpulseDenominator(pos1, cpd.FrictionWorldTangentialB);
denom1 = body1.ComputeImpulseDenominator(pos2, cpd.FrictionWorldTangentialB);
denom = relaxation / (denom0 + denom1);
cpd.JacDiagABInvTangentB = denom;
Vector3 totalImpulse = cp.NormalWorldOnB * cpd.AppliedImpulse;
{
Vector3 torqueAxis0 = Vector3.Cross(rel_pos1, cp.NormalWorldOnB);
cpd.AngularComponentA = Vector3.TransformNormal(torqueAxis0, body0.InvInertiaTensorWorld);
Vector3 torqueAxis1 = Vector3.Cross(rel_pos2, cp.NormalWorldOnB);
cpd.AngularComponentB = Vector3.TransformNormal(torqueAxis1, body1.InvInertiaTensorWorld);
}
{
Vector3 ftorqueAxis0 = Vector3.Cross(rel_pos1, cpd.FrictionWorldTangentialA);
cpd.FrictionAngularComponent0A = Vector3.TransformNormal(ftorqueAxis0, body0.InvInertiaTensorWorld);
}
{
Vector3 ftorqueAxis1 = Vector3.Cross(rel_pos1, cpd.FrictionWorldTangentialB);
cpd.FrictionAngularComponent1A = Vector3.TransformNormal(ftorqueAxis1, body0.InvInertiaTensorWorld);
}
{
Vector3 ftorqueAxis0 = Vector3.Cross(rel_pos2, cpd.FrictionWorldTangentialA);
cpd.FrictionAngularComponent0B = Vector3.TransformNormal(ftorqueAxis0, body1.InvInertiaTensorWorld);
}
{
Vector3 ftorqueAxis1 = Vector3.Cross(rel_pos2, cpd.FrictionWorldTangentialB);
cpd.FrictionAngularComponent1B = Vector3.TransformNormal(ftorqueAxis1, body1.InvInertiaTensorWorld);
}
//apply previous frames impulse on both bodies
body0.ApplyImpulse(totalImpulse, rel_pos1);
body1.ApplyImpulse(-totalImpulse, rel_pos2);
}
}
}
}
