public Entity CreateDebugEntity(PhysicsComponent component, bool alwaysAddOffset = false)
{
if (component?.ColliderShape == null) return null;
if (component.DebugEntity != null) return null;
var debugEntity = new Entity();
var skinnedElement = component as PhysicsSkinnedComponentBase;
if (skinnedElement != null && skinnedElement.BoneIndex != -1)
{
Vector3 scale, pos;
Quaternion rot;
skinnedElement.BoneWorldMatrixOut.Decompose(out scale, out rot, out pos);
debugEntity.Transform.Position = pos;
debugEntity.Transform.Rotation = rot;
}
else
{
Vector3 scale, pos;
Quaternion rot;
component.Entity.Transform.WorldMatrix.Decompose(out scale, out rot, out pos);
debugEntity.Transform.Position = pos;
debugEntity.Transform.Rotation = rot;
}
var rigidBody = component as RigidbodyComponent;
//don't add offset for non bone dynamic and kinematic as it is added already in the updates
var colliderEntity = CreateChildEntity(component, component.ColliderShape, alwaysAddOffset || rigidBody == null);
if (colliderEntity != null) debugEntity.AddChild(colliderEntity);
return debugEntity;
}
public Collision(PhysicsComponent colliderA, PhysicsComponent colliderB)
{
ColliderA = colliderA;
ColliderB = colliderB;
NewContactChannel = new Channel<ContactPoint> { Preference = ChannelPreference.PreferSender };
ContactUpdateChannel = new Channel<ContactPoint> { Preference = ChannelPreference.PreferSender };
ContactEndedChannel = new Channel<ContactPoint> { Preference = ChannelPreference.PreferSender };
}
public Entity CreateDebugEntity(PhysicsComponent component)
{
if (component?.ColliderShape == null) return null;
if (component.DebugEntity != null) return null;
var debugEntity = new Entity();
var colliderEntity = CreateChildEntity(component, component.ColliderShape, true);
if (colliderEntity == null) return null;
debugEntity.AddChild(colliderEntity);
if (component.CanScaleShape)
{
debugEntity.Transform.Scale = component.ColliderShape.Scaling;
}
var skinnedElement = component as PhysicsSkinnedComponentBase;
if (skinnedElement != null && skinnedElement.BoneIndex != -1)
{
Vector3 scale, pos;
Quaternion rot;
skinnedElement.BoneWorldMatrixOut.Decompose(out scale, out rot, out pos);
debugEntity.Transform.Position = pos;
debugEntity.Transform.Rotation = rot;
}
else
{
Vector3 scale, pos;
Quaternion rot;
component.Entity.Transform.WorldMatrix.Decompose(out scale, out rot, out pos);
debugEntity.Transform.Position = pos;
debugEntity.Transform.Rotation = rot;
}
return debugEntity;
}
internal void RemoveCollider(PhysicsComponent component)
{
collisionWorld.RemoveCollisionObject(component.NativeCollisionObject);
}
internal void AddCollider(PhysicsComponent component, CollisionFilterGroupFlags group, CollisionFilterGroupFlags mask)
{
collisionWorld.AddCollisionObject(component.NativeCollisionObject, (BulletSharp.CollisionFilterGroups)group, (BulletSharp.CollisionFilterGroups)mask);
}
internal bool InternalEquals(PhysicsComponent a, PhysicsComponent b)
{
return (ColliderA == a && ColliderB == b) || (ColliderB == a && ColliderA == b);
}
private Entity CreateChildEntity(PhysicsComponent component, ColliderShape shape, bool addOffset = false)
{
if (shape == null)
return null;
switch (shape.Type)
{
case ColliderShapeTypes.StaticPlane:
{
//Hmm TODO maybe can draw an infinite plane??
return null;
}
case ColliderShapeTypes.Compound:
{
var entity = new Entity();
//We got to recurse
var compound = (CompoundColliderShape)shape;
for (var i = 0; i < compound.Count; i++)
{
var subShape = compound[i];
var subEntity = CreateChildEntity(component, subShape, true);
subEntity.Transform.UseTRS = false;
entity.AddChild(subEntity);
}
entity.Transform.LocalMatrix = Matrix.Identity;
entity.Transform.UseTRS = false;
return entity;
}
default:
{
var mat = triggerMaterial;
if (component is RigidbodyComponent)
{
mat = ((RigidbodyComponent)component).IsKinematic ? kinematicMaterial : dynamicMaterial;
}
else if (component is CharacterComponent)
{
mat = characterMaterial;
}
else if (component is StaticColliderComponent)
{
mat = staticMaterial;
}
var entity = new Entity
{
new ModelComponent
{
Model = new Model
{
mat,
new Mesh
{
Draw = shape.CreateDebugPrimitive(graphicsDevice)
}
}
}
};
var offset = addOffset ? Matrix.RotationQuaternion(shape.LocalRotation)*Matrix.Translation(shape.LocalOffset) : Matrix.Identity;
if (shape.Type == ColliderShapeTypes.ConvexHull)
{
var hullDesc = (ConvexHullColliderShape)shape;
entity.Transform.LocalMatrix = shape.DebugPrimitiveMatrix * Matrix.Scaling(hullDesc.Scaling) * offset;
}
else
{
entity.Transform.LocalMatrix = shape.DebugPrimitiveMatrix * offset;
}
entity.Transform.UseTRS = false;
return entity;
}
}
}
private Entity CreateChildEntity(PhysicsComponent component, ColliderShape shape, bool addOffset)
{
if (shape == null)
return null;
switch (shape.Type)
{
case ColliderShapeTypes.Compound:
{
var entity = new Entity();
//We got to recurse
var compound = (CompoundColliderShape)shape;
for (var i = 0; i < compound.Count; i++)
{
var subShape = compound[i];
var subEntity = CreateChildEntity(component, subShape, true); //always add offsets to compounds
if (subEntity != null)
{
entity.AddChild(subEntity);
}
}
entity.Transform.LocalMatrix = Matrix.Identity;
entity.Transform.UseTRS = false;
compound.DebugEntity = entity;
return entity;
}
case ColliderShapeTypes.Box:
case ColliderShapeTypes.Capsule:
case ColliderShapeTypes.ConvexHull:
case ColliderShapeTypes.Cylinder:
case ColliderShapeTypes.Sphere:
case ColliderShapeTypes.Cone:
{
var mat = triggerMaterial;
var rigidbodyComponent = component as RigidbodyComponent;
if (rigidbodyComponent != null)
{
mat = rigidbodyComponent.IsKinematic ? kinematicMaterial : dynamicMaterial;
mat = rigidbodyComponent.IsTrigger ? triggerMaterial : mat;
}
else if (component is CharacterComponent)
{
mat = characterMaterial;
}
else if (component is StaticColliderComponent)
{
var staticCollider = (StaticColliderComponent)component;
mat = staticCollider.IsTrigger ? triggerMaterial : staticMaterial;
}
MeshDraw draw;
var type = shape.GetType();
if (type == typeof(CapsuleColliderShape) || type == typeof(ConvexHullColliderShape))
{
if (!debugMeshCache2.TryGetValue(shape, out draw))
{
draw = shape.CreateDebugPrimitive(graphicsDevice);
debugMeshCache2[shape] = draw;
}
}
else
{
if (!debugMeshCache.TryGetValue(shape.GetType(), out draw))
{
draw = shape.CreateDebugPrimitive(graphicsDevice);
debugMeshCache[shape.GetType()] = draw;
}
}
var entity = new Entity
{
new ModelComponent
{
Model = new Model
{
mat,
new Mesh
{
Draw = draw
}
}
}
};
var offset = addOffset ? Matrix.RotationQuaternion(shape.LocalRotation) * Matrix.Translation(shape.LocalOffset) : Matrix.Identity;
entity.Transform.LocalMatrix = shape.DebugPrimitiveMatrix * offset * Matrix.Scaling(shape.Scaling);
entity.Transform.UseTRS = false;
shape.DebugEntity = entity;
return entity;
}
default:
return null;
}
}
unsafe internal void ContactTest(PhysicsComponent component)
{
foreach (var collision in component.Collisions)
{
skippedCollisions.Add(collision);
}
IntPtr buffer;
int bufferSize;
collisionWorld.GetCollisions(component.NativeCollisionObject, out buffer, out bufferSize);
var contacts = (ContactData*) buffer;
for (var i = 0; i < bufferSize; i++)
{
var contact = contacts[i];
var obj0 = BulletSharp.CollisionObject.GetManaged((IntPtr)contact.ColliderA);
var obj1 = BulletSharp.CollisionObject.GetManaged((IntPtr)contact.ColliderB);
var component0 = (PhysicsComponent)obj0.UserObject;
var component1 = (PhysicsComponent)obj1.UserObject;
if (((int)component0.CanCollideWith & (int)component1.CollisionGroup) != 0 || ((int)component1.CanCollideWith & (int)component0.CollisionGroup) != 0)
{
var skip = false;
foreach (var collision in component0.Collisions)
{
if ((collision.ColliderA == component0 && collision.ColliderB == component1) || (collision.ColliderA == component1 && collision.ColliderB == component0))
{
var oldContact = collision.Contacts[0];
oldContact.Distance = contact.Distance;
oldContact.Normal = new Vector3(contact.NormalX, contact.NormalY, contact.NormalZ);
oldContact.PositionOnA = new Vector3(contact.PositionOnAx, contact.PositionOnAy, contact.PositionOnAz);
oldContact.PositionOnB = new Vector3(contact.PositionOnBx, contact.PositionOnBy, contact.PositionOnBz);
updatedContactsCache.Add(oldContact);
skippedCollisions.Remove(collision);
skip = true;
break;
}
}
if (skip)
{
continue;
}
var newCollision = new Collision
{
Contacts = new TrackingCollection<ContactPoint>(),
ColliderA = component0,
ColliderB = component1
};
//todo this has to change
var newContact = new ContactPoint
{
Collision = newCollision,
Distance = contact.Distance,
LifeTime = 0,
Normal = new Vector3(contact.NormalX, contact.NormalY, contact.NormalZ),
PositionOnA = new Vector3(contact.PositionOnAx, contact.PositionOnAy, contact.PositionOnAz),
PositionOnB = new Vector3(contact.PositionOnBx, contact.PositionOnBy, contact.PositionOnBz)
};
newCollision.Contacts.Add(newContact);
component0.Collisions.Add(newCollision);
component1.Collisions.Add(newCollision);
newCollisionsCache.Add(newCollision);
newContactsFastCache.Add(newContact);
newCollisions.Add(newCollision);
}
}
}
internal void CleanContacts(PhysicsComponent component)
{
previousToRemove.Clear(true);
foreach (var previousFrameContact in previousFrameContacts)
{
var obj0 = BulletSharp.CollisionObject.GetManaged(previousFrameContact.ColliderA);
var obj1 = BulletSharp.CollisionObject.GetManaged(previousFrameContact.ColliderB);
var component0 = (PhysicsComponent)obj0.UserObject;
var component1 = (PhysicsComponent)obj1.UserObject;
if (component == component0 || component == component1)
{
previousToRemove.Add(previousFrameContact);
ContactRemoval(previousFrameContact, component0, component1);
}
}
foreach (var contactPoint in previousToRemove)
{
previousFrameContacts.Remove(contactPoint);
}
}
internal unsafe void ContactTest(PhysicsComponent component)
{
IntPtr buffer;
int bufferSize;
collisionWorld.GetCollisions(component.NativeCollisionObject, out buffer, out bufferSize);
var contacts = (ContactPoint*) buffer;
for (var i = 0; i < bufferSize; i++)
{
var contact = contacts[i];
currentFrameContacts.Add(contact);
}
}
private void ContactRemoval(ContactPoint contact, PhysicsComponent component0, PhysicsComponent component1)
{
Collision existingPair = null;
foreach (var x in component0.Collisions)
{
if (x.InternalEquals(component0, component1))
{
existingPair = x;
break;
}
}
if (existingPair == null)
{
#if DEBUG
//should not happen?
throw new Exception("Pair not present.");
#else
return;
#endif
}
if (existingPair.Contacts.Contains(contact))
{
existingPair.Contacts.Remove(contact);
removedContactsCache.Add(contact);
contactToCollision.Remove(contact);
if (existingPair.Contacts.Count == 0)
{
component0.Collisions.Remove(existingPair);
component1.Collisions.Remove(existingPair);
removedCollisionsCache.Add(existingPair);
}
}
else
{
#if DEBUG
//should not happen?
throw new Exception("Contact not in pair.");
#endif
}
}
