protected override bool ConfigureTriangle(int i, out TriangleIndices indices)
{
MeshBoundingBoxTreeData data = mesh.Shape.TriangleMesh.Data;
int triangleIndex = overlappedTriangles.Elements[i];
data.GetTriangle(triangleIndex, out localTriangleShape.vA, out localTriangleShape.vB, out localTriangleShape.vC);
AffineTransform.Transform(ref localTriangleShape.vA, ref mesh.worldTransform, out localTriangleShape.vA);
AffineTransform.Transform(ref localTriangleShape.vB, ref mesh.worldTransform, out localTriangleShape.vB);
AffineTransform.Transform(ref localTriangleShape.vC, ref mesh.worldTransform, out localTriangleShape.vC);
//In instanced meshes, the bounding box we found in local space could collect more triangles than strictly necessary.
//By doing a second pass, we should be able to prune out quite a few of them.
BoundingBox triangleAABB;
Toolbox.GetTriangleBoundingBox(ref localTriangleShape.vA, ref localTriangleShape.vB, ref localTriangleShape.vC, out triangleAABB);
bool toReturn;
triangleAABB.Intersects(ref convex.boundingBox, out toReturn);
if (!toReturn)
{
indices = new TriangleIndices();
return(false);
}
localTriangleShape.sidedness = mesh.sidedness;
localTriangleShape.collisionMargin = 0;
indices = new TriangleIndices()
{
A = data.indices[triangleIndex],
B = data.indices[triangleIndex + 1],
C = data.indices[triangleIndex + 2]
};
return(true);
}
/// <summary>
/// Creates a detector volume.
/// </summary>
/// <param name="triangleMesh">Arbitrary closed triangle mesh representing the volume.</param>
/// <param name="queryAccelerator">System used to find nearby objects.</param>
public DetectorVolume(MeshBoundingBoxTreeData triangleMesh, IQueryAccelerator queryAccelerator)
{
TriangleMesh = new TriangleMesh(triangleMesh);
QueryAccelerator = queryAccelerator;
collisionRules = new CollisionRules()
{
group = new CollisionGroup()
};
}
protected override bool ConfigureTriangle(int i, out TriangleIndices indices)
{
MeshBoundingBoxTreeData data = mesh.Shape.TriangleMesh.Data;
int triangleIndex = overlappedTriangles.Elements[i];
data.GetTriangle(triangleIndex, out localTriangleShape.vA, out localTriangleShape.vB, out localTriangleShape.vC);
AffineTransform transform;
AffineTransform.CreateFromRigidTransform(ref mesh.worldTransform, out transform);
AffineTransform.Transform(ref localTriangleShape.vA, ref transform, out localTriangleShape.vA);
AffineTransform.Transform(ref localTriangleShape.vB, ref transform, out localTriangleShape.vB);
AffineTransform.Transform(ref localTriangleShape.vC, ref transform, out localTriangleShape.vC);
//In instanced meshes, the bounding box we found in local space could collect more triangles than strictly necessary.
//By doing a second pass, we should be able to prune out quite a few of them.
BoundingBox triangleAABB;
Toolbox.GetTriangleBoundingBox(ref localTriangleShape.vA, ref localTriangleShape.vB, ref localTriangleShape.vC, out triangleAABB);
bool toReturn;
triangleAABB.Intersects(ref convex.boundingBox, out toReturn);
if (!toReturn)
{
indices = new TriangleIndices();
return(false);
}
TriangleSidedness sidedness;
switch (mesh.Shape.solidity)
{
case MobileMeshSolidity.Clockwise:
sidedness = TriangleSidedness.Clockwise;
break;
case MobileMeshSolidity.Counterclockwise:
sidedness = TriangleSidedness.Counterclockwise;
break;
case MobileMeshSolidity.DoubleSided:
sidedness = TriangleSidedness.DoubleSided;
break;
default:
sidedness = mesh.Shape.solidSidedness;
break;
}
localTriangleShape.sidedness = sidedness;
localTriangleShape.collisionMargin = 0;
indices = new TriangleIndices()
{
A = data.uindices[triangleIndex],
B = data.uindices[triangleIndex + 1],
C = data.uindices[triangleIndex + 2]
};
return(true);
}
protected override bool ConfigureLocalTriangle(int i, TriangleShape localTriangleShape,
out TriangleIndices indices)
{
MeshBoundingBoxTreeData data = mesh.Shape.TriangleMesh.Data;
int triangleIndex = overlappedTriangles.Elements[i];
TriangleSidedness sidedness;
//TODO: Note superhack; don't do this in v2.
if (IsQuery)
{
sidedness = TriangleSidedness.DoubleSided;
}
else
{
switch (mesh.Shape.solidity)
{
case MobileMeshSolidity.Clockwise:
sidedness = TriangleSidedness.Clockwise;
break;
case MobileMeshSolidity.Counterclockwise:
sidedness = TriangleSidedness.Counterclockwise;
break;
case MobileMeshSolidity.DoubleSided:
sidedness = TriangleSidedness.DoubleSided;
break;
default:
sidedness = mesh.Shape.SidednessWhenSolid;
break;
}
}
localTriangleShape.sidedness = sidedness;
localTriangleShape.collisionMargin = 0;
indices = new TriangleIndices
{
A = data.indices[triangleIndex],
B = data.indices[triangleIndex + 1],
C = data.indices[triangleIndex + 2]
};
localTriangleShape.vA = data.vertices[indices.A];
localTriangleShape.vB = data.vertices[indices.B];
localTriangleShape.vC = data.vertices[indices.C];
return(true);
}
protected override bool ConfigureLocalTriangle(int i, TriangleShape localTriangleShape, out TriangleIndices indices)
{
MeshBoundingBoxTreeData data = mesh.Shape.TriangleMesh.Data;
int triangleIndex = overlappedTriangles.Elements[i];
localTriangleShape.sidedness = mesh.sidedness;
localTriangleShape.collisionMargin = 0;
indices = new TriangleIndices
{
A = data.indices[triangleIndex],
B = data.indices[triangleIndex + 1],
C = data.indices[triangleIndex + 2]
};
localTriangleShape.vA = data.vertices[indices.A];
localTriangleShape.vB = data.vertices[indices.B];
localTriangleShape.vC = data.vertices[indices.C];
return(true);
}
protected override bool ConfigureLocalTriangle(int i, TriangleShape localTriangleShape,
out TriangleIndices indices)
{
int triangleIndex = overlappedTriangles.Elements[i];
MeshBoundingBoxTreeData data = mesh.Mesh.Data;
localTriangleShape.vA = data.vertices[data.indices[triangleIndex]];
localTriangleShape.vB = data.vertices[data.indices[triangleIndex + 1]];
localTriangleShape.vC = data.vertices[data.indices[triangleIndex + 2]];
//TODO: Note the IsQuery hack to avoid missing contacts. Avoid doing this in v2.
localTriangleShape.sidedness = IsQuery ? TriangleSidedness.DoubleSided : mesh.sidedness;
localTriangleShape.collisionMargin = 0;
indices = new TriangleIndices
{
A = data.indices[triangleIndex],
B = data.indices[triangleIndex + 1],
C = data.indices[triangleIndex + 2]
};
return(true);
}
///<summary>
/// Updates the time of impact for the pair.
///</summary>
///<param name="requester">Collidable requesting the update.</param>
///<param name="dt">Timestep duration.</param>
public override void UpdateTimeOfImpact(Collidable requester, float dt)
{
//Notice that we don't test for convex entity null explicitly. The convex.IsActive property does that for us.
if (convex.IsActive && convex.entity.PositionUpdateMode == PositionUpdateMode.Continuous)
{
//TODO: This system could be made more robust by using a similar region-based rejection of edges.
//CCD events are awfully rare under normal circumstances, so this isn't usually an issue.
//Only perform the test if the minimum radii are small enough relative to the size of the velocity.
Vector3 velocity;
Vector3.Multiply(ref convex.entity.linearVelocity, dt, out velocity);
float velocitySquared = velocity.LengthSquared();
var minimumRadius = convex.Shape.MinimumRadius * MotionSettings.CoreShapeScaling;
timeOfImpact = 1;
if (minimumRadius * minimumRadius < velocitySquared)
{
var triangle = PhysicsThreadResources.GetTriangle();
triangle.collisionMargin = 0;
//Spherecast against all triangles to find the earliest time.
for (int i = 0; i < MeshManifold.overlappedTriangles.Count; i++)
{
MeshBoundingBoxTreeData data = instancedMesh.Shape.TriangleMesh.Data;
int triangleIndex = MeshManifold.overlappedTriangles.Elements[i];
data.GetTriangle(triangleIndex, out triangle.vA, out triangle.vB, out triangle.vC);
AffineTransform.Transform(ref triangle.vA, ref instancedMesh.worldTransform, out triangle.vA);
AffineTransform.Transform(ref triangle.vB, ref instancedMesh.worldTransform, out triangle.vB);
AffineTransform.Transform(ref triangle.vC, ref instancedMesh.worldTransform, out triangle.vC);
//Put the triangle into 'localish' space of the convex.
Vector3.Subtract(ref triangle.vA, ref convex.worldTransform.Position, out triangle.vA);
Vector3.Subtract(ref triangle.vB, ref convex.worldTransform.Position, out triangle.vB);
Vector3.Subtract(ref triangle.vC, ref convex.worldTransform.Position, out triangle.vC);
RayHit rayHit;
if (GJKToolbox.CCDSphereCast(new Ray(Toolbox.ZeroVector, velocity), minimumRadius, triangle, ref Toolbox.RigidIdentity, timeOfImpact, out rayHit) &&
rayHit.T > Toolbox.BigEpsilon)
{
if (instancedMesh.sidedness != TriangleSidedness.DoubleSided)
{
Vector3 AB, AC;
Vector3.Subtract(ref triangle.vB, ref triangle.vA, out AB);
Vector3.Subtract(ref triangle.vC, ref triangle.vA, out AC);
Vector3 normal;
Vector3.Cross(ref AB, ref AC, out normal);
float dot;
Vector3.Dot(ref normal, ref rayHit.Normal, out dot);
//Only perform sweep if the object is in danger of hitting the object.
//Triangles can be one sided, so check the impact normal against the triangle normal.
if (instancedMesh.sidedness == TriangleSidedness.Counterclockwise && dot < 0 ||
instancedMesh.sidedness == TriangleSidedness.Clockwise && dot > 0)
{
timeOfImpact = rayHit.T;
}
}
else
{
timeOfImpact = rayHit.T;
}
}
}
PhysicsThreadResources.GiveBack(triangle);
}
}
}
///<summary>
/// Updates the time of impact for the pair.
///</summary>
///<param name="requester">Collidable requesting the update.</param>
///<param name="dt">Timestep duration.</param>
public override void UpdateTimeOfImpact(Collidable requester, float dt)
{
//TODO: This conditional early outing stuff could be pulled up into a common system, along with most of the pair handler.
var overlap = BroadPhaseOverlap;
var convexMode = convex.entity == null ? PositionUpdateMode.Discrete : convex.entity.PositionUpdateMode;
if (
(mobileMesh.IsActive || (convex.entity == null ? false : convex.entity.activityInformation.IsActive)) && //At least one has to be active.
(
(
convexMode == PositionUpdateMode.Continuous && //If both are continuous, only do the process for A.
mobileMesh.entity.PositionUpdateMode == PositionUpdateMode.Continuous &&
overlap.entryA == requester
) ||
(
convexMode == PositionUpdateMode.Continuous ^ //If only one is continuous, then we must do it.
mobileMesh.entity.PositionUpdateMode == PositionUpdateMode.Continuous
)
)
)
{
//TODO: This system could be made more robust by using a similar region-based rejection of edges.
//CCD events are awfully rare under normal circumstances, so this isn't usually an issue.
//Only perform the test if the minimum radii are small enough relative to the size of the velocity.
Vector3 velocity;
if (convex.entity != null)
{
Vector3.Subtract(ref convex.entity.linearVelocity, ref mobileMesh.entity.linearVelocity, out velocity);
}
else
{
Vector3.Negate(ref mobileMesh.entity.linearVelocity, out velocity);
}
Vector3.Multiply(ref velocity, dt, out velocity);
float velocitySquared = velocity.LengthSquared();
var minimumRadius = convex.Shape.minimumRadius * MotionSettings.CoreShapeScaling;
timeOfImpact = 1;
if (minimumRadius * minimumRadius < velocitySquared)
{
TriangleSidedness sidedness = mobileMesh.Shape.Sidedness;
Matrix3X3 orientation;
Matrix3X3.CreateFromQuaternion(ref mobileMesh.worldTransform.Orientation, out orientation);
var triangle = Resources.GetTriangle();
triangle.collisionMargin = 0;
//Spherecast against all triangles to find the earliest time.
for (int i = 0; i < MeshManifold.overlappedTriangles.count; i++)
{
MeshBoundingBoxTreeData data = mobileMesh.Shape.TriangleMesh.Data;
int triangleIndex = MeshManifold.overlappedTriangles.Elements[i];
data.GetTriangle(triangleIndex, out triangle.vA, out triangle.vB, out triangle.vC);
Matrix3X3.Transform(ref triangle.vA, ref orientation, out triangle.vA);
Matrix3X3.Transform(ref triangle.vB, ref orientation, out triangle.vB);
Matrix3X3.Transform(ref triangle.vC, ref orientation, out triangle.vC);
Vector3.Add(ref triangle.vA, ref mobileMesh.worldTransform.Position, out triangle.vA);
Vector3.Add(ref triangle.vB, ref mobileMesh.worldTransform.Position, out triangle.vB);
Vector3.Add(ref triangle.vC, ref mobileMesh.worldTransform.Position, out triangle.vC);
//Put the triangle into 'localish' space of the convex.
Vector3.Subtract(ref triangle.vA, ref convex.worldTransform.Position, out triangle.vA);
Vector3.Subtract(ref triangle.vB, ref convex.worldTransform.Position, out triangle.vB);
Vector3.Subtract(ref triangle.vC, ref convex.worldTransform.Position, out triangle.vC);
RayHit rayHit;
if (GJKToolbox.CCDSphereCast(new Ray(Toolbox.ZeroVector, velocity), minimumRadius, triangle, ref Toolbox.RigidIdentity, timeOfImpact, out rayHit) &&
rayHit.T > Toolbox.BigEpsilon)
{
if (sidedness != TriangleSidedness.DoubleSided)
{
Vector3 AB, AC;
Vector3.Subtract(ref triangle.vB, ref triangle.vA, out AB);
Vector3.Subtract(ref triangle.vC, ref triangle.vA, out AC);
Vector3 normal;
Vector3.Cross(ref AB, ref AC, out normal);
float dot;
Vector3.Dot(ref normal, ref rayHit.Normal, out dot);
//Only perform sweep if the object is in danger of hitting the object.
//Triangles can be one sided, so check the impact normal against the triangle normal.
if (sidedness == TriangleSidedness.Counterclockwise && dot < 0 ||
sidedness == TriangleSidedness.Clockwise && dot > 0)
{
timeOfImpact = rayHit.T;
}
}
else
{
timeOfImpact = rayHit.T;
}
}
}
Resources.GiveBack(triangle);
}
}
}
public override void UpdateCollision(float dt)
{
WasContaining = Containing;
WasTouching = Touching;
mobileTriangle.collisionMargin = mesh.Shape.MeshCollisionMargin;
//Scan the pairs in sequence, updating the state as we go.
//Touching can be set to true by a single touching subpair.
Touching = false;
//Containing can be set to false by a single noncontaining or nontouching subpair.
Containing = true;
MeshBoundingBoxTreeData meshData = mesh.Shape.TriangleMesh.Data;
RigidTransform mobileTriangleTransform, detectorTriangleTransform;
mobileTriangleTransform.Orientation = Quaternion.Identity;
detectorTriangleTransform.Orientation = Quaternion.Identity;
for (int i = 0; i < meshData.Indices.Length; i += 3)
{
//Grab a triangle associated with the mobile mesh.
meshData.GetTriangle(i, out mobileTriangle.vA, out mobileTriangle.vB, out mobileTriangle.vC);
RigidTransform.Transform(ref mobileTriangle.vA, ref mesh.worldTransform, out mobileTriangle.vA);
RigidTransform.Transform(ref mobileTriangle.vB, ref mesh.worldTransform, out mobileTriangle.vB);
RigidTransform.Transform(ref mobileTriangle.vC, ref mesh.worldTransform, out mobileTriangle.vC);
Vector3.Add(ref mobileTriangle.vA, ref mobileTriangle.vB, out mobileTriangleTransform.Position);
Vector3.Add(ref mobileTriangle.vC, ref mobileTriangleTransform.Position,
out mobileTriangleTransform.Position);
Vector3.Multiply(ref mobileTriangleTransform.Position, 1 / 3f, out mobileTriangleTransform.Position);
Vector3.Subtract(ref mobileTriangle.vA, ref mobileTriangleTransform.Position, out mobileTriangle.vA);
Vector3.Subtract(ref mobileTriangle.vB, ref mobileTriangleTransform.Position, out mobileTriangle.vB);
Vector3.Subtract(ref mobileTriangle.vC, ref mobileTriangleTransform.Position, out mobileTriangle.vC);
//Go through all the detector volume triangles which are near the mobile mesh triangle.
bool triangleTouching, triangleContaining;
BoundingBox mobileBoundingBox;
mobileTriangle.GetBoundingBox(ref mobileTriangleTransform, out mobileBoundingBox);
DetectorVolume.TriangleMesh.Tree.GetOverlaps(mobileBoundingBox, overlaps);
for (int j = 0; j < overlaps.Count; j++)
{
DetectorVolume.TriangleMesh.Data.GetTriangle(overlaps.Elements[j], out detectorTriangle.vA,
out detectorTriangle.vB, out detectorTriangle.vC);
Vector3.Add(ref detectorTriangle.vA, ref detectorTriangle.vB,
out detectorTriangleTransform.Position);
Vector3.Add(ref detectorTriangle.vC, ref detectorTriangleTransform.Position,
out detectorTriangleTransform.Position);
Vector3.Multiply(ref detectorTriangleTransform.Position, 1 / 3f,
out detectorTriangleTransform.Position);
Vector3.Subtract(ref detectorTriangle.vA, ref detectorTriangleTransform.Position,
out detectorTriangle.vA);
Vector3.Subtract(ref detectorTriangle.vB, ref detectorTriangleTransform.Position,
out detectorTriangle.vB);
Vector3.Subtract(ref detectorTriangle.vC, ref detectorTriangleTransform.Position,
out detectorTriangle.vC);
//If this triangle collides with the convex, we can stop immediately since we know we're touching and not containing.)))
//[MPR is used here in lieu of GJK because the MPR implementation tends to finish quicker than GJK when objects are overlapping. The GJK implementation does better on separated objects.]
if (MPRToolbox.AreShapesOverlapping(detectorTriangle, mobileTriangle, ref detectorTriangleTransform,
ref mobileTriangleTransform))
{
triangleTouching = true;
//The convex can't be fully contained if it's still touching the surface.
triangleContaining = false;
overlaps.Clear();
goto finishTriangleTest;
}
}
overlaps.Clear();
//If we get here, then there was no shell intersection.
//If the convex's center point is contained by the mesh, then the convex is fully contained.
//This test is only needed if containment hasn't yet been outlawed or a touching state hasn't been established.
if ((!Touching || Containing) &&
DetectorVolume.IsPointContained(ref mobileTriangleTransform.Position, overlaps))
{
triangleTouching = true;
triangleContaining = true;
goto finishTriangleTest;
}
//If we get here, then there was no surface intersection and the convex's center is not contained- the volume and convex are separate!
triangleTouching = false;
triangleContaining = false;
finishTriangleTest:
//Analyze the results of the triangle test.
if (triangleTouching)
{
Touching = true; //If one child is touching, then we are touching too.
}
else
{
Containing = false; //If one child isn't touching, then we aren't containing.
}
if (!triangleContaining) //If one child isn't containing, then we aren't containing.
{
Containing = false;
}
if (!Containing && Touching)
//If it's touching but not containing, no further pairs will change the state.
//Containment has been invalidated by something that either didn't touch or wasn't contained.
//Touching has been ensured by at least one object touching.
{
break;
}
}
//There is a possibility that the MobileMesh is solid and fully contains the DetectorVolume.
//In this case, we should be Touching, but currently we are not.
if (mesh.Shape.solidity == MobileMeshSolidity.Solid && !Containing && !Touching)
{
//To determine if the detector volume is fully contained, check if one of the detector mesh's vertices
//are in the mobile mesh.
//This *could* fail if the mobile mesh is actually multiple pieces, but that's not a common or really supported case for solids.
Vector3 vertex;
DetectorVolume.TriangleMesh.Data.GetVertexPosition(0, out vertex);
Ray ray;
ray.Direction = Vector3.Up;
RayHit hit;
RigidTransform.TransformByInverse(ref vertex, ref mesh.worldTransform, out ray.Position);
if (mesh.Shape.IsLocalRayOriginInMesh(ref ray, out hit))
{
Touching = true;
}
}
NotifyDetectorVolumeOfChanges();
}
/// <summary>
/// Creates a detector volume.
/// </summary>
/// <param name="triangleMesh">Arbitrary closed triangle mesh representing the volume.</param>
/// <param name="queryAccelerator">System used to find nearby objects.</param>
public DetectorVolume(MeshBoundingBoxTreeData triangleMesh, IQueryAccelerator queryAccelerator)
{
TriangleMesh = new TriangleMesh(triangleMesh);
QueryAccelerator = queryAccelerator;
}
