/// <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;
int numContacts = pm.NumContacts;
if (robotBody == null || numContacts < 1)
{
return;
}
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);
}
}
}
/// <summary>
/// Generates contact information from the given byte buffer.
/// </summary>
/// <param name="buffer"></param>
/// <returns></returns>
private static List <List <KeyValuePair <ContactDescriptor, int> > > ReadContactDescriptors(byte[] buffer)
{
IFormatter formatter = new BinaryFormatter();
List <List <KeyValuePair <ContactDescriptor, int> > > contacts = new List <List <KeyValuePair <ContactDescriptor, int> > >();
BinaryReader reader = new BinaryReader(new MemoryStream(buffer));
int numContactLists = reader.ReadInt32();
for (int i = 0; i < numContactLists; i++)
{
int contactCount = reader.ReadInt32();
if (contactCount > 0)
{
List <KeyValuePair <ContactDescriptor, int> > currentContacts = new List <KeyValuePair <ContactDescriptor, int> >();
for (int j = 0; j < contactCount; j++)
{
ContactDescriptor contact = new ContactDescriptor
{
AppliedImpulse = reader.ReadSingle(),
Position = (BulletSharp.Math.Vector3)formatter.Deserialize(reader.BaseStream)
};
currentContacts.Add(new KeyValuePair <ContactDescriptor, int>(contact, reader.ReadInt32()));
}
contacts.Add(currentContacts);
}
else
{
contacts.Add(null);
}
}
return(contacts);
}
