/// 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();
}
}
/// <summary>
/// Finds any robot collisions and adds them to the list of collisions for the current frame.
/// </summary>
/// <param name="pm"></param>
public void OnVisitPersistentManifold(PersistentManifold pm)
{
if (!mainState.Tracking)
{
return;
}
if (framesPassed == -1) // This is the first manifold visited of the frame
{
framesPassed = physicsWorld.frameCount - lastFrameCount;
}
if (passedContacts == null)
{
passedContacts = new List <ContactDescriptor> [framesPassed];
}
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;
}
int numContacts = pm.NumContacts;
for (int i = 0; i < framesPassed; i++)
{
ManifoldPoint mp = pm.GetContactPoint(i);
ContactDescriptor cd = new ContactDescriptor
{
AppliedImpulse = mp.AppliedImpulse,
Position = (mp.PositionWorldOnA + mp.PositionWorldOnB) * 0.5f,
RobotBody = robotBody
};
if (passedContacts[i] == null)
{
passedContacts[i] = new List <ContactDescriptor>();
}
passedContacts[i].Add(cd);
}
pm.ClearManifold();
}
