internal override void Refit(MeshBoundingBoxTreeData data)
{
data.GetBoundingBox(LeafIndex, out BoundingBox);
//Having an ever-so-slight margin allows the hierarchy use a volume metric even for degenerate shapes (consider a flat tessellated plane).
BoundingBox.Max.X += LeafMargin;
BoundingBox.Max.Y += LeafMargin;
BoundingBox.Max.Z += LeafMargin;
BoundingBox.Min.X -= LeafMargin;
BoundingBox.Min.Y -= LeafMargin;
BoundingBox.Min.Z -= LeafMargin;
}
protected override bool ConfigureLocalTriangle(int i, TriangleShape localTriangleShape, out TriangleIndices indices)
{
MeshBoundingBoxTreeData data = mesh.Shape.TriangleMesh.Data;
int triangleIndex = overlappedTriangles.Elements[i];
//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]
};
localTriangleShape.vA = data.vertices[indices.A];
localTriangleShape.vB = data.vertices[indices.B];
localTriangleShape.vC = data.vertices[indices.C];
return(true);
}
/// <summary>
/// Constructs a new tree.
/// </summary>
/// <param name="data">Data to use to construct the tree.</param>
public MeshBoundingBoxTree(MeshBoundingBoxTreeData data)
{
Data = data;
}
internal override void Refit(MeshBoundingBoxTreeData data)
{
ChildA.Refit(data);
ChildB.Refit(data);
BoundingBox.CreateMerged(ref ChildA.BoundingBox, ref ChildB.BoundingBox, out BoundingBox);
}
internal abstract void Refit(MeshBoundingBoxTreeData data);
///<summary>
/// Constructs a new triangle mesh.
///</summary>
///<param name="data">Data to use to construct the mesh.</param>
public TriangleMesh(MeshBoundingBoxTreeData data)
{
this.data = data;
tree = new MeshBoundingBoxTree(data);
}
///<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)
{
var overlap = BroadPhaseOverlap;
var meshMode = mobileMesh.entity == null ? PositionUpdateMode.Discrete : mobileMesh.entity.PositionUpdateMode;
var convexMode = convex.entity == null ? PositionUpdateMode.Discrete : convex.entity.PositionUpdateMode;
if (
(mobileMesh.IsActive || convex.IsActive) && //At least one has to be active.
(
(
convexMode == PositionUpdateMode.Continuous && //If both are continuous, only do the process for A.
meshMode == PositionUpdateMode.Continuous &&
overlap.entryA == requester
) ||
(
convexMode == PositionUpdateMode.Continuous ^ //If only one is continuous, then we must do it.
meshMode == 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 (convexMode == PositionUpdateMode.Discrete)
{
//Convex is static for the purposes of CCD.
Vector3.Negate(ref mobileMesh.entity.linearVelocity, out velocity);
}
else if (meshMode == PositionUpdateMode.Discrete)
{
//Mesh is static for the purposes of CCD.
velocity = convex.entity.linearVelocity;
}
else
{
//Both objects can move.
Vector3.Subtract(ref convex.entity.linearVelocity, 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 = 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 = 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;
}
}
}
PhysicsThreadResources.GiveBack(triangle);
}
}
}