public void Execute(ref ModifiableContactHeader contactHeader, ref ModifiableContactPoint contactPoint)
{
bool isBodyA = (contactHeader.EntityA == SurfaceEntity);
bool isBodyB = (contactHeader.EntityB == SurfaceEntity);
if (isBodyA || isBodyB)
{
if (contactPoint.Index == 0)
{
// if we have a mesh surface we can get the surface normal from the plane of the polygon
var rbIdx = CollisionWorld.GetRigidBodyIndex(SurfaceEntity);
var body = CollisionWorld.Bodies[rbIdx];
if (body.Collider.Value.CollisionType == CollisionType.Composite)
{
unsafe
{
body.Collider.Value.GetLeaf(isBodyA ? contactHeader.ColliderKeyA : contactHeader.ColliderKeyB, out ChildCollider leafCollider);
if (leafCollider.Collider->Type == ColliderType.Triangle || leafCollider.Collider->Type == ColliderType.Quad)
{
PolygonCollider *polygonCollider = (PolygonCollider *)leafCollider.Collider;
// Potential optimization: If TransformFromChild has no rotation just use body.WorldFromBody.rot
// This is likely if you only have a MeshCollider with no hierarchy.
quaternion rotation = math.mul(body.WorldFromBody.rot, leafCollider.TransformFromChild.rot);
float3 surfaceNormal = math.rotate(rotation, polygonCollider->Planes[0].Normal);
distanceScale = math.dot(surfaceNormal, contactHeader.Normal);
contactHeader.Normal = surfaceNormal;
}
}
}
}
contactPoint.Distance *= distanceScale;
}
}
public void Execute(ref ModifiableContactHeader contactHeader, ref ModifiableContactPoint contactPoint)
{
bool isBodyA = (contactHeader.EntityA == SurfaceEntity);
bool isBodyB = (contactHeader.EntityB == SurfaceEntity);
if (isBodyA || isBodyB)
{
if (contactPoint.Index == 0)
{
var surfaceNormal = SurfaceNormal;
// if we have a mesh surface we can get the surface normal from the plane of the polygon
var rbIdx = CollisionWorld.GetRigidBodyIndex(SurfaceEntity);
var body = CollisionWorld.Bodies[rbIdx];
if (body.Collider.Value.Type == ColliderType.Mesh)
{
unsafe
{
var meshColliderPtr = (MeshCollider *)body.Collider.GetUnsafePtr();
meshColliderPtr->GetLeaf(isBodyA ? contactHeader.ColliderKeyA : contactHeader.ColliderKeyB, out var leafCollider);
var polygonCollider = (PolygonCollider *)leafCollider.Collider;
surfaceNormal = math.rotate(body.WorldFromBody.rot, polygonCollider->Planes[0].Normal);
}
}
var newNormal = surfaceNormal;
distanceScale = math.dot(newNormal, contactHeader.Normal);
contactHeader.Normal = newNormal;
}
contactPoint.Distance *= distanceScale;
}
}