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; } }