public override void GetMeshData(List <VertexPositionNormalTexture> vertices, List <ushort> indices)
{
var tempVertices = new VertexPositionNormalTexture[DisplayedObject.Shape.TriangleMesh.Data.Vertices.Length];
for (int i = 0; i < DisplayedObject.Shape.TriangleMesh.Data.Vertices.Length; i++)
{
tempVertices[i] = new VertexPositionNormalTexture(
MathConverter.Convert(AffineTransform.Transform(DisplayedObject.Shape.TriangleMesh.Data.Vertices[i], DisplayedObject.WorldTransform)),
Microsoft.Xna.Framework.Vector3.Zero,
Microsoft.Xna.Framework.Vector2.Zero);
}
for (int i = 0; i < DisplayedObject.Shape.TriangleMesh.Data.Indices.Length; i++)
{
indices.Add((ushort)DisplayedObject.Shape.TriangleMesh.Data.Indices[i]);
}
for (int i = 0; i < indices.Count; i += 3)
{
int a = indices[i];
int b = indices[i + 1];
int c = indices[i + 2];
Microsoft.Xna.Framework.Vector3 normal = Microsoft.Xna.Framework.Vector3.Normalize(Microsoft.Xna.Framework.Vector3.Cross(
tempVertices[c].Position - tempVertices[a].Position,
tempVertices[b].Position - tempVertices[a].Position));
tempVertices[a].Normal += normal;
tempVertices[b].Normal += normal;
tempVertices[c].Normal += normal;
}
for (int i = 0; i < tempVertices.Length; i++)
{
tempVertices[i].Normal.Normalize();
vertices.Add(tempVertices[i]);
}
}
/// <summary>
/// Gets the world space position of a vertex in the terrain at the given indices.
/// </summary>
///<param name="columnIndex">Index in the first dimension.</param>
///<param name="rowIndex">Index in the second dimension.</param>
/// <param name="transform">Transform to apply to the vertex.</param>
/// <param name="position">Transformed position of the vertex at the given indices.</param>
public void GetPosition(int columnIndex, int rowIndex, ref AffineTransform transform, out Vector3 position)
{
if (columnIndex <= 0)
{
columnIndex = 0;
}
else if (columnIndex >= heights.GetLength(0))
{
columnIndex = heights.GetLength(0) - 1;
}
if (rowIndex <= 0)
{
rowIndex = 0;
}
else if (rowIndex >= heights.GetLength(1))
{
rowIndex = heights.GetLength(1) - 1;
}
#if !WINDOWS
position = new Vector3();
#endif
position.X = columnIndex;
position.Y = heights[columnIndex, rowIndex];
position.Z = rowIndex;
AffineTransform.Transform(ref position, ref transform, out position);
}
/// <summary>
/// Casts a convex shape against the collidable.
/// </summary>
/// <param name="castShape">Shape to cast.</param>
/// <param name="startingTransform">Initial transform of the shape.</param>
/// <param name="sweep">Sweep to apply to the shape.</param>
/// <param name="hit">Hit data, if any.</param>
/// <returns>Whether or not the cast hit anything.</returns>
public override bool ConvexCast(ConvexShape castShape, ref RigidTransform startingTransform, ref Vector3 sweep, out RayHit hit)
{
hit = new RayHit();
BoundingBox boundingBox;
castShape.GetSweptLocalBoundingBox(ref startingTransform, ref worldTransform, ref sweep, out boundingBox);
var tri = PhysicsThreadResources.GetTriangle();
var hitElements = CommonResources.GetIntList();
if (this.Shape.TriangleMesh.Tree.GetOverlaps(boundingBox, hitElements))
{
hit.T = float.MaxValue;
for (int i = 0; i < hitElements.Count; i++)
{
Shape.TriangleMesh.Data.GetTriangle(hitElements[i], out tri.vA, out tri.vB, out tri.vC);
AffineTransform.Transform(ref tri.vA, ref worldTransform, out tri.vA);
AffineTransform.Transform(ref tri.vB, ref worldTransform, out tri.vB);
AffineTransform.Transform(ref tri.vC, ref worldTransform, out tri.vC);
Vector3 center;
Vector3.Add(ref tri.vA, ref tri.vB, out center);
Vector3.Add(ref center, ref tri.vC, out center);
Vector3.Multiply(ref center, 1f / 3f, out center);
Vector3.Subtract(ref tri.vA, ref center, out tri.vA);
Vector3.Subtract(ref tri.vB, ref center, out tri.vB);
Vector3.Subtract(ref tri.vC, ref center, out tri.vC);
tri.MaximumRadius = tri.vA.LengthSquared();
float radius = tri.vB.LengthSquared();
if (tri.MaximumRadius < radius)
{
tri.MaximumRadius = radius;
}
radius = tri.vC.LengthSquared();
if (tri.MaximumRadius < radius)
{
tri.MaximumRadius = radius;
}
tri.MaximumRadius = (float)Math.Sqrt(tri.MaximumRadius);
tri.collisionMargin = 0;
var triangleTransform = new RigidTransform {
Orientation = Quaternion.Identity, Position = center
};
RayHit tempHit;
if (MPRToolbox.Sweep(castShape, tri, ref sweep, ref Toolbox.ZeroVector, ref startingTransform, ref triangleTransform, out tempHit) && tempHit.T < hit.T)
{
hit = tempHit;
}
}
tri.MaximumRadius = 0;
PhysicsThreadResources.GiveBack(tri);
CommonResources.GiveBack(hitElements);
return(hit.T != float.MaxValue);
}
PhysicsThreadResources.GiveBack(tri);
CommonResources.GiveBack(hitElements);
return(false);
}
///<summary>
/// Tests a ray against the instance.
///</summary>
///<param name="ray">Ray to test.</param>
///<param name="maximumLength">Maximum length of the ray to test; in units of the ray's direction's length.</param>
///<param name="sidedness">Sidedness to use during the ray cast. This does not have to be the same as the mesh's sidedness.</param>
///<param name="rayHit">The hit location of the ray on the mesh, if any.</param>
///<returns>Whether or not the ray hit the mesh.</returns>
public bool RayCast(Ray ray, float maximumLength, TriangleSidedness sidedness, out RayHit rayHit)
{
//Put the ray into local space.
Ray localRay;
AffineTransform inverse;
AffineTransform.Invert(ref worldTransform, out inverse);
Matrix3x3.Transform(ref ray.Direction, ref inverse.LinearTransform, out localRay.Direction);
AffineTransform.Transform(ref ray.Position, ref inverse, out localRay.Position);
if (Shape.TriangleMesh.RayCast(localRay, maximumLength, sidedness, out rayHit))
{
//Transform the hit into world space.
Vector3.Multiply(ref ray.Direction, rayHit.T, out rayHit.Location);
Vector3.Add(ref rayHit.Location, ref ray.Position, out rayHit.Location);
Matrix3x3.TransformTranspose(ref rayHit.Normal, ref inverse.LinearTransform, out rayHit.Normal);
return(true);
}
rayHit = new RayHit();
return(false);
}
/// <summary>
/// Casts a convex shape against the collidable.
/// </summary>
/// <param name="castShape">Shape to cast.</param>
/// <param name="startingTransform">Initial transform of the shape.</param>
/// <param name="sweep">Sweep to apply to the shape.</param>
/// <param name="hit">Hit data, if any.</param>
/// <returns>Whether or not the cast hit anything.</returns>
public override bool ConvexCast(ConvexShape castShape, ref RigidTransform startingTransform, ref Vector3 sweep, out RayHit hit)
{
if (Shape.solidity == MobileMeshSolidity.Solid)
{
//If the convex cast is inside the mesh and the mesh is solid, it should return t = 0.
var ray = new Ray()
{
Position = startingTransform.Position, Direction = Toolbox.UpVector
};
if (Shape.IsLocalRayOriginInMesh(ref ray, out hit))
{
hit = new RayHit()
{
Location = startingTransform.Position, Normal = new Vector3(), T = 0
};
return(true);
}
}
hit = new RayHit();
BoundingBox boundingBox;
var transform = new AffineTransform {
Translation = worldTransform.Position
};
Matrix3x3.CreateFromQuaternion(ref worldTransform.Orientation, out transform.LinearTransform);
castShape.GetSweptLocalBoundingBox(ref startingTransform, ref transform, ref sweep, out boundingBox);
var tri = PhysicsThreadResources.GetTriangle();
var hitElements = CommonResources.GetIntList();
if (this.Shape.TriangleMesh.Tree.GetOverlaps(boundingBox, hitElements))
{
hit.T = float.MaxValue;
for (int i = 0; i < hitElements.Count; i++)
{
Shape.TriangleMesh.Data.GetTriangle(hitElements[i], out tri.vA, out tri.vB, out tri.vC);
AffineTransform.Transform(ref tri.vA, ref transform, out tri.vA);
AffineTransform.Transform(ref tri.vB, ref transform, out tri.vB);
AffineTransform.Transform(ref tri.vC, ref transform, out tri.vC);
Vector3 center;
Vector3.Add(ref tri.vA, ref tri.vB, out center);
Vector3.Add(ref center, ref tri.vC, out center);
Vector3.Multiply(ref center, 1f / 3f, out center);
Vector3.Subtract(ref tri.vA, ref center, out tri.vA);
Vector3.Subtract(ref tri.vB, ref center, out tri.vB);
Vector3.Subtract(ref tri.vC, ref center, out tri.vC);
tri.MaximumRadius = tri.vA.LengthSquared();
float radius = tri.vB.LengthSquared();
if (tri.MaximumRadius < radius)
{
tri.MaximumRadius = radius;
}
radius = tri.vC.LengthSquared();
if (tri.MaximumRadius < radius)
{
tri.MaximumRadius = radius;
}
tri.MaximumRadius = (float)Math.Sqrt(tri.MaximumRadius);
tri.collisionMargin = 0;
var triangleTransform = new RigidTransform {
Orientation = Quaternion.Identity, Position = center
};
RayHit tempHit;
if (MPRToolbox.Sweep(castShape, tri, ref sweep, ref Toolbox.ZeroVector, ref startingTransform, ref triangleTransform, out tempHit) && tempHit.T < hit.T)
{
hit = tempHit;
}
}
tri.MaximumRadius = 0;
PhysicsThreadResources.GiveBack(tri);
CommonResources.GiveBack(hitElements);
return(hit.T != float.MaxValue);
}
PhysicsThreadResources.GiveBack(tri);
CommonResources.GiveBack(hitElements);
return(false);
}
///<summary>
/// Tests a ray against the terrain shape.
///</summary>
///<param name="ray">Ray to test against the shape.</param>
///<param name="maximumLength">Maximum length of the ray in units of the ray direction's length.</param>
///<param name="transform">Transform to apply to the terrain shape during the test.</param>
///<param name="sidedness">Sidedness of the triangles to use when raycasting.</param>
///<param name="hit">Hit data of the ray cast, if any.</param>
///<returns>Whether or not the ray hit the transformed terrain shape.</returns>
public bool RayCast(ref Ray ray, float maximumLength, ref AffineTransform transform, TriangleSidedness sidedness, out RayHit hit)
{
hit = new RayHit();
//Put the ray into local space.
Ray localRay;
AffineTransform inverse;
AffineTransform.Invert(ref transform, out inverse);
Matrix3x3.Transform(ref ray.Direction, ref inverse.LinearTransform, out localRay.Direction);
AffineTransform.Transform(ref ray.Position, ref inverse, out localRay.Position);
//Use rasterizey traversal.
//The origin is at 0,0,0 and the map goes +X, +Y, +Z.
//if it's before the origin and facing away, or outside the max and facing out, early out.
float maxX = heights.GetLength(0) - 1;
float maxZ = heights.GetLength(1) - 1;
Vector3 progressingOrigin = localRay.Position;
float distance = 0;
//Check the outside cases first.
if (progressingOrigin.X < 0)
{
if (localRay.Direction.X > 0)
{
//Off the left side.
float timeToMinX = -progressingOrigin.X / localRay.Direction.X;
distance += timeToMinX;
Vector3 increment;
Vector3.Multiply(ref localRay.Direction, timeToMinX, out increment);
Vector3.Add(ref increment, ref progressingOrigin, out progressingOrigin);
}
else
{
return(false); //Outside and pointing away from the terrain.
}
}
else if (progressingOrigin.X > maxX)
{
if (localRay.Direction.X < 0)
{
//Off the left side.
float timeToMinX = -(progressingOrigin.X - maxX) / localRay.Direction.X;
distance += timeToMinX;
Vector3 increment;
Vector3.Multiply(ref localRay.Direction, timeToMinX, out increment);
Vector3.Add(ref increment, ref progressingOrigin, out progressingOrigin);
}
else
{
return(false); //Outside and pointing away from the terrain.
}
}
if (progressingOrigin.Z < 0)
{
if (localRay.Direction.Z > 0)
{
float timeToMinZ = -progressingOrigin.Z / localRay.Direction.Z;
distance += timeToMinZ;
Vector3 increment;
Vector3.Multiply(ref localRay.Direction, timeToMinZ, out increment);
Vector3.Add(ref increment, ref progressingOrigin, out progressingOrigin);
}
else
{
return(false);
}
}
else if (progressingOrigin.Z > maxZ)
{
if (localRay.Direction.Z < 0)
{
float timeToMinZ = -(progressingOrigin.Z - maxZ) / localRay.Direction.Z;
distance += timeToMinZ;
Vector3 increment;
Vector3.Multiply(ref localRay.Direction, timeToMinZ, out increment);
Vector3.Add(ref increment, ref progressingOrigin, out progressingOrigin);
}
else
{
return(false);
}
}
if (distance > maximumLength)
{
return(false);
}
//By now, we should be entering the main body of the terrain.
int xCell = (int)progressingOrigin.X;
int zCell = (int)progressingOrigin.Z;
//If it's hitting the border and going in, then correct the index
//so that it will initially target a valid quad.
//Without this, a quad beyond the border would be tried and failed.
if (xCell == heights.GetLength(0) - 1 && localRay.Direction.X < 0)
{
xCell = heights.GetLength(0) - 2;
}
if (zCell == heights.GetLength(1) - 1 && localRay.Direction.Z < 0)
{
zCell = heights.GetLength(1) - 2;
}
while (true)
{
//Check for a miss.
if (xCell < 0 ||
zCell < 0 ||
xCell >= heights.GetLength(0) - 1 ||
zCell >= heights.GetLength(1) - 1)
{
return(false);
}
//Test the triangles of this cell.
Vector3 v1, v2, v3, v4;
// v3 v4
// v1 v2
GetLocalPosition(xCell, zCell, out v1);
GetLocalPosition(xCell + 1, zCell, out v2);
GetLocalPosition(xCell, zCell + 1, out v3);
GetLocalPosition(xCell + 1, zCell + 1, out v4);
RayHit hit1, hit2;
bool didHit1;
bool didHit2;
//Don't bother doing ray intersection tests if the ray can't intersect it.
float highest = v1.Y;
float lowest = v1.Y;
if (v2.Y > highest)
{
highest = v2.Y;
}
else if (v2.Y < lowest)
{
lowest = v2.Y;
}
if (v3.Y > highest)
{
highest = v3.Y;
}
else if (v3.Y < lowest)
{
lowest = v3.Y;
}
if (v4.Y > highest)
{
highest = v4.Y;
}
else if (v4.Y < lowest)
{
lowest = v4.Y;
}
if (!(progressingOrigin.Y > highest && localRay.Direction.Y > 0 ||
progressingOrigin.Y < lowest && localRay.Direction.Y < 0))
{
if (quadTriangleOrganization == QuadTriangleOrganization.BottomLeftUpperRight)
{
//Always perform the raycast as if Y+ in local space is the way the triangles are facing.
didHit1 = Toolbox.FindRayTriangleIntersection(ref localRay, maximumLength, sidedness, ref v1, ref v2, ref v3, out hit1);
didHit2 = Toolbox.FindRayTriangleIntersection(ref localRay, maximumLength, sidedness, ref v2, ref v4, ref v3, out hit2);
}
else //if (quadTriangleOrganization == CollisionShapes.QuadTriangleOrganization.BottomRightUpperLeft)
{
didHit1 = Toolbox.FindRayTriangleIntersection(ref localRay, maximumLength, sidedness, ref v1, ref v2, ref v4, out hit1);
didHit2 = Toolbox.FindRayTriangleIntersection(ref localRay, maximumLength, sidedness, ref v1, ref v4, ref v3, out hit2);
}
if (didHit1 && didHit2)
{
if (hit1.T < hit2.T)
{
Vector3.Multiply(ref ray.Direction, hit1.T, out hit.Location);
Vector3.Add(ref hit.Location, ref ray.Position, out hit.Location);
Matrix3x3.TransformTranspose(ref hit1.Normal, ref inverse.LinearTransform, out hit.Normal);
hit.T = hit1.T;
return(true);
}
Vector3.Multiply(ref ray.Direction, hit2.T, out hit.Location);
Vector3.Add(ref hit.Location, ref ray.Position, out hit.Location);
Matrix3x3.TransformTranspose(ref hit2.Normal, ref inverse.LinearTransform, out hit.Normal);
hit.T = hit2.T;
return(true);
}
else if (didHit1)
{
Vector3.Multiply(ref ray.Direction, hit1.T, out hit.Location);
Vector3.Add(ref hit.Location, ref ray.Position, out hit.Location);
Matrix3x3.TransformTranspose(ref hit1.Normal, ref inverse.LinearTransform, out hit.Normal);
hit.T = hit1.T;
return(true);
}
else if (didHit2)
{
Vector3.Multiply(ref ray.Direction, hit2.T, out hit.Location);
Vector3.Add(ref hit.Location, ref ray.Position, out hit.Location);
Matrix3x3.TransformTranspose(ref hit2.Normal, ref inverse.LinearTransform, out hit.Normal);
hit.T = hit2.T;
return(true);
}
}
//Move to the next cell.
float timeToX;
if (localRay.Direction.X < 0)
{
timeToX = -(progressingOrigin.X - xCell) / localRay.Direction.X;
}
else if (localRay.Direction.X > 0)
{
timeToX = (xCell + 1 - progressingOrigin.X) / localRay.Direction.X;
}
else
{
timeToX = float.MaxValue;
}
float timeToZ;
if (localRay.Direction.Z < 0)
{
timeToZ = -(progressingOrigin.Z - zCell) / localRay.Direction.Z;
}
else if (localRay.Direction.Z > 0)
{
timeToZ = (zCell + 1 - progressingOrigin.Z) / localRay.Direction.Z;
}
else
{
timeToZ = float.MaxValue;
}
//Move to the next cell.
if (timeToX < timeToZ)
{
if (localRay.Direction.X < 0)
{
xCell--;
}
else
{
xCell++;
}
distance += timeToX;
if (distance > maximumLength)
{
return(false);
}
Vector3 increment;
Vector3.Multiply(ref localRay.Direction, timeToX, out increment);
Vector3.Add(ref increment, ref progressingOrigin, out progressingOrigin);
}
else
{
if (localRay.Direction.Z < 0)
{
zCell--;
}
else
{
zCell++;
}
distance += timeToZ;
if (distance > maximumLength)
{
return(false);
}
Vector3 increment;
Vector3.Multiply(ref localRay.Direction, timeToZ, out increment);
Vector3.Add(ref increment, ref progressingOrigin, out progressingOrigin);
}
}
}
///<summary>
/// Updates the manifold.
///</summary>
///<param name="dt">Timestep duration.</param>
public override void Update(float dt)
{
//First, refresh all existing contacts. This is an incremental manifold.
var transform = MeshTransform;
ContactRefresher.ContactRefresh(contacts, supplementData, ref convex.worldTransform, ref transform, contactIndicesToRemove);
RemoveQueuedContacts();
CleanUpOverlappingTriangles();
//Get all the overlapped triangle indices.
//Note that the collection of triangles is left up to the child implementation.
//We're basically treating the child class like an indexable collection.
//A little gross to have it organized this way instead of an explicit collection to separate the logic up. Would be nice to improve someday!
int triangleCount = FindOverlappingTriangles(dt);
//Just use 32 elements for all the lists and sets in this system.
const int bufferPoolSizePower = 5;
BoundarySets boundarySets;
if (UseImprovedBoundaryHandling)
{
boundarySets = new BoundarySets(bufferPoolSizePower);
}
else
{
boundarySets = new BoundarySets();
}
var candidatesToAdd = new QuickList <ContactData>(BufferPools <ContactData> .Thread, bufferPoolSizePower);
//A single triangle shape will be reused for all operations. It's pulled from a thread local pool to avoid holding a TriangleShape around for every single contact manifold or pair tester.
var localTriangleShape = PhysicsThreadResources.GetTriangle();
//Precompute the transform to take triangles from their native local space to the convex's local space.
RigidTransform inverseConvexWorldTransform;
RigidTransform.Invert(ref convex.worldTransform, out inverseConvexWorldTransform);
AffineTransform convexInverseWorldTransform;
AffineTransform.CreateFromRigidTransform(ref inverseConvexWorldTransform, out convexInverseWorldTransform);
AffineTransform fromMeshLocalToConvexLocal;
PrecomputeTriangleTransform(ref convexInverseWorldTransform, out fromMeshLocalToConvexLocal);
//Grab the convex's local space bounding box up front. This will be used for a secondary pruning step.
BoundingBox convexLocalBoundingBox;
convex.Shape.GetBoundingBox(ref Toolbox.RigidIdentity, out convexLocalBoundingBox);
Matrix3x3 orientation;
Matrix3x3.CreateFromQuaternion(ref convex.worldTransform.Orientation, out orientation);
var guaranteedContacts = 0;
for (int i = 0; i < triangleCount; i++)
{
//Initialize the local triangle.
TriangleIndices indices;
if (ConfigureLocalTriangle(i, localTriangleShape, out indices))
{
//Put the triangle into the local space of the convex.
AffineTransform.Transform(ref localTriangleShape.vA, ref fromMeshLocalToConvexLocal, out localTriangleShape.vA);
AffineTransform.Transform(ref localTriangleShape.vB, ref fromMeshLocalToConvexLocal, out localTriangleShape.vB);
AffineTransform.Transform(ref localTriangleShape.vC, ref fromMeshLocalToConvexLocal, out localTriangleShape.vC);
//Do one last AABB test between the convex and triangle in the convex's local space.
//This can prune out a lot of triangles when dealing with larger objects, and it's pretty cheap to do.
BoundingBox triangleBoundingBox;
Toolbox.GetTriangleBoundingBox(ref localTriangleShape.vA, ref localTriangleShape.vB, ref localTriangleShape.vC, out triangleBoundingBox);
bool intersecting;
triangleBoundingBox.Intersects(ref convexLocalBoundingBox, out intersecting);
if (!intersecting)
{
continue;
}
//Find a pairtester for the triangle.
TrianglePairTester pairTester;
if (!activePairTesters.TryGetValue(indices, out pairTester))
{
pairTester = GetTester();
pairTester.Initialize(convex.Shape);
activePairTesters.Add(indices, pairTester);
}
pairTester.Updated = true;
//Now, generate a contact between the two shapes.
TinyStructList <ContactData> contactList;
if (pairTester.GenerateContactCandidates(localTriangleShape, out contactList))
{
for (int j = 0; j < contactList.Count; j++)
{
ContactData contact;
contactList.Get(j, out contact);
if (UseImprovedBoundaryHandling)
{
if (AnalyzeCandidate(ref indices, localTriangleShape, pairTester, ref contact, ref boundarySets))
{
//This is let through if there's a face contact. Face contacts cannot be blocked.
guaranteedContacts++;
AddLocalContact(ref contact, ref orientation, ref candidatesToAdd);
}
}
else
{
AddLocalContact(ref contact, ref orientation, ref candidatesToAdd);
}
}
}
//Get the voronoi region from the contact candidate generation. Possibly just recalculate, since most of the systems don't calculate it.
//Depending on which voronoi region it is in (Switch on enumeration), identify the indices composing that region. For face contacts, don't bother- just add it if unique.
//For AB, AC, or BC, add an Edge to the blockedEdgeRegions set with the corresponding indices.
//For A, B, or C, add the index of the vertex to the blockedVertexRegions set.
//If the edge/vertex is already present in the set, then DO NOT add the contact.
//When adding a contact, add ALL other voronoi regions to the blocked sets.
}
}
if (UseImprovedBoundaryHandling)
{
//If there were no face contacts that absolutely must be included, we may get into a very rare situation
//where absolutely no contacts get created. For example, a sphere falling directly on top of a vertex in a flat terrain.
//It will generally get locked out of usage by belonging only to restricted regions (numerical issues make it visible by both edges and vertices).
//In some cases, the contacts will be ignored instead of corrected (e.g. spheres).
//To prevent objects from just falling through the ground in such a situation, force-correct the contacts regardless of the pair tester's desires.
//Sure, it might not be necessary under normal circumstances, but it's a better option than having no contacts.
//TODO: There is another option: Changing restricted regions so that a vertex only restricts the other two vertices and the far edge,
//and an edge only restricts the far vertex and other two edges. This introduces an occasional bump though...
//It's possible, in very specific instances, for an object to wedge itself between two adjacent triangles.
//For this state to continue beyond a brief instant generally requires the object be orientation locked and slender.
//However, some characters fit this description, so it can't be ignored!
//Conceptually, this issue can occur at either a vertex junction or a shared edge (usually on extremely flat surfaces only).
//However, an object stuck between multiple triangles is not in a stable state. In the edge case, the object gets shoved to one side
//as one contact 'wins' the solver war. That's not enough to escape, unfortunately.
//The vertex case, on the other hand, is degenerate and decays into an edge case rapidly thanks to this lack of stability.
//So, we don't have to explicitly handle the somewhat more annoying and computationally expensive vertex unstucking case, because the edge case handles both! :)
//This isn't a completely free operation, but it's guarded behind pretty rare conditions.
//Essentially, we will check to see if there's just edge contacts fighting against each other.
//If they are, then we will correct any stuck-contributing normals to the triangle normal.
if (boundarySets.VertexContacts.Count == 0 && guaranteedContacts == 0 && boundarySets.EdgeContacts.Count > 1)
{
//There are only edge contacts, check to see if:
//all normals are coplanar, and
//at least one normal faces against the other normals (meaning it's probably stuck, as opposed to just colliding on a corner).
bool allNormalsInSamePlane = true;
bool atLeastOneNormalAgainst = false;
var firstNormal = boundarySets.EdgeContacts.Elements[0].ContactData.Normal;
boundarySets.EdgeContacts.Elements[0].CorrectedNormal.Normalize();
float dot;
Vector3.Dot(ref firstNormal, ref boundarySets.EdgeContacts.Elements[0].CorrectedNormal, out dot);
if (Math.Abs(dot) > .01f)
{
//Go ahead and test the first contact separately, since we're using its contact normal to determine coplanarity.
allNormalsInSamePlane = false;
}
else
{
//TODO: Note that we're only checking the new edge contacts, not the existing contacts.
//It's possible that some existing contacts could interfere and cause issues, but for the sake of simplicity and due to rarity
//we'll ignore that possibility for now.
for (int i = 1; i < boundarySets.EdgeContacts.Count; i++)
{
Vector3.Dot(ref boundarySets.EdgeContacts.Elements[i].ContactData.Normal, ref firstNormal, out dot);
if (dot < 0)
{
atLeastOneNormalAgainst = true;
}
//Check to see if the normal is outside the plane.
Vector3.Dot(ref boundarySets.EdgeContacts.Elements[i].ContactData.Normal, ref boundarySets.EdgeContacts.Elements[0].CorrectedNormal, out dot);
if (Math.Abs(dot) > .01f)
{
//We are not stuck!
allNormalsInSamePlane = false;
break;
}
}
}
if (allNormalsInSamePlane && atLeastOneNormalAgainst)
{
//Uh oh! all the normals are parallel... The object is probably in a weird situation.
//Let's correct the normals!
//Already normalized the first contact above.
//We don't need to perform the perpendicularity test here- we did that before! We know it's perpendicular already.
boundarySets.EdgeContacts.Elements[0].ContactData.Normal = boundarySets.EdgeContacts.Elements[0].CorrectedNormal;
boundarySets.EdgeContacts.Elements[0].ShouldCorrect = true;
for (int i = 1; i < boundarySets.EdgeContacts.Count; i++)
{
//Must normalize the corrected normal before using it.
boundarySets.EdgeContacts.Elements[i].CorrectedNormal.Normalize();
Vector3.Dot(ref boundarySets.EdgeContacts.Elements[i].CorrectedNormal, ref boundarySets.EdgeContacts.Elements[i].ContactData.Normal, out dot);
if (dot < .01)
{
//Only bother doing the correction if the normal appears to be pointing nearly horizontally- implying that it's a contributor to the stuckness!
//If it's blocked, the next section will use the corrected normal- if it's not blocked, the next section will use the direct normal.
//Make them the same thing :)
boundarySets.EdgeContacts.Elements[i].ContactData.Normal = boundarySets.EdgeContacts.Elements[i].CorrectedNormal;
boundarySets.EdgeContacts.Elements[i].ShouldCorrect = true;
//Note that the penetration depth is NOT corrected. The contact's depth no longer represents the true depth.
//However, we only need to have some penetration depth to get the object to escape the rut.
//Furthermore, the depth computed from the horizontal opposing contacts is known to be less than the depth in the perpendicular direction.
//If the current depth was NOT less than the true depth along the corrected normal, then the collision detection system
//would have picked a different depth, as it finds a reasonable approximation of the minimum penetration!
//As a consequence, this contact will not be active beyond the object's destuckification, because its contact depth will be negative (or very close to it).
}
}
}
}
for (int i = 0; i < boundarySets.EdgeContacts.Count; i++)
{
//Only correct if it's allowed AND it's blocked.
//If it's not blocked, the contact being created is necessary!
//The normal generated by the triangle-convex tester is already known not to
//violate the triangle sidedness.
if (!boundarySets.BlockedEdgeRegions.Contains(boundarySets.EdgeContacts.Elements[i].Edge))
{
//If it's not blocked, use the contact as-is without correcting it.
AddLocalContact(ref boundarySets.EdgeContacts.Elements[i].ContactData, ref orientation, ref candidatesToAdd);
}
else if (boundarySets.EdgeContacts.Elements[i].ShouldCorrect || guaranteedContacts == 0)
{
//If it is blocked, we can still make use of the contact. But first, we need to change the contact normal to ensure that
//it will not interfere (and cause a bump or something).
float dot;
boundarySets.EdgeContacts.Elements[i].CorrectedNormal.Normalize();
Vector3.Dot(ref boundarySets.EdgeContacts.Elements[i].CorrectedNormal, ref boundarySets.EdgeContacts.Elements[i].ContactData.Normal, out dot);
boundarySets.EdgeContacts.Elements[i].ContactData.Normal = boundarySets.EdgeContacts.Elements[i].CorrectedNormal;
boundarySets.EdgeContacts.Elements[i].ContactData.PenetrationDepth *= MathHelper.Max(0, dot); //Never cause a negative penetration depth.
AddLocalContact(ref boundarySets.EdgeContacts.Elements[i].ContactData, ref orientation, ref candidatesToAdd);
}
//If it's blocked AND it doesn't allow correction, ignore its existence.
}
for (int i = 0; i < boundarySets.VertexContacts.Count; i++)
{
if (!boundarySets.BlockedVertexRegions.Contains(boundarySets.VertexContacts.Elements[i].Vertex))
{
//If it's not blocked, use the contact as-is without correcting it.
AddLocalContact(ref boundarySets.VertexContacts.Elements[i].ContactData, ref orientation, ref candidatesToAdd);
}
else if (boundarySets.VertexContacts.Elements[i].ShouldCorrect || guaranteedContacts == 0)
{
//If it is blocked, we can still make use of the contact. But first, we need to change the contact normal to ensure that
//it will not interfere (and cause a bump or something).
float dot;
boundarySets.VertexContacts.Elements[i].CorrectedNormal.Normalize();
Vector3.Dot(ref boundarySets.VertexContacts.Elements[i].CorrectedNormal, ref boundarySets.VertexContacts.Elements[i].ContactData.Normal, out dot);
boundarySets.VertexContacts.Elements[i].ContactData.Normal = boundarySets.VertexContacts.Elements[i].CorrectedNormal;
boundarySets.VertexContacts.Elements[i].ContactData.PenetrationDepth *= MathHelper.Max(0, dot); //Never cause a negative penetration depth.
AddLocalContact(ref boundarySets.VertexContacts.Elements[i].ContactData, ref orientation, ref candidatesToAdd);
}
//If it's blocked AND it doesn't allow correction, ignore its existence.
}
boundarySets.Dispose();
}
//Remove stale pair testers.
for (int i = activePairTesters.Count - 1; i >= 0; --i)
{
var tester = activePairTesters.Values[i];
if (!tester.Updated)
{
tester.CleanUp();
GiveBackTester(tester);
activePairTesters.FastRemove(activePairTesters.Keys[i]);
}
else
{
tester.Updated = false;
}
}
//Some child types will want to do some extra post processing on the manifold.
ProcessCandidates(ref candidatesToAdd);
//Check if adding the new contacts would overflow the manifold.
if (contacts.Count + candidatesToAdd.Count > 4)
{
//Adding all the contacts would overflow the manifold. Reduce to the best subset.
var reducedCandidates = new QuickList <ContactData>(BufferPools <ContactData> .Thread, bufferPoolSizePower);
ContactReducer.ReduceContacts(contacts, ref candidatesToAdd, contactIndicesToRemove, ref reducedCandidates);
RemoveQueuedContacts();
for (int i = reducedCandidates.Count - 1; i >= 0; i--)
{
Add(ref reducedCandidates.Elements[i]);
reducedCandidates.RemoveAt(i);
}
reducedCandidates.Dispose();
}
else if (candidatesToAdd.Count > 0)
{
//Won't overflow the manifold, so just toss it in PROVIDED that it isn't too close to something else.
for (int i = 0; i < candidatesToAdd.Count; i++)
{
Add(ref candidatesToAdd.Elements[i]);
}
}
PhysicsThreadResources.GiveBack(localTriangleShape);
candidatesToAdd.Dispose();
}
private void UpdateSurfaceVertices()
{
hullVertices.Clear();
if (Volume > 0)
{
ConvexHullHelper.GetConvexHull(triangleMesh.Data.vertices, hullVertices);
var transformableData = triangleMesh.Data as TransformableMeshData;
if (transformableData != null)
{
var transform = transformableData.worldTransform;
for (int i = 0; i < hullVertices.Count; i++)
{
AffineTransform.Transform(ref hullVertices.Elements[i], ref transform, out hullVertices.Elements[i]);
}
}
}
else
{
hullVertices.Clear();
//A mobile mesh is allowed to have zero volume, so long as it isn't solid.
//In this case, compute the bounding box of all points.
BoundingBox box = new BoundingBox();
for (int i = 0; i < triangleMesh.Data.vertices.Length; i++)
{
Vector3 v;
triangleMesh.Data.GetVertexPosition(i, out v);
if (v.X > box.Max.X)
{
box.Max.X = v.X;
}
if (v.X < box.Min.X)
{
box.Min.X = v.X;
}
if (v.Y > box.Max.Y)
{
box.Max.Y = v.Y;
}
if (v.Y < box.Min.Y)
{
box.Min.Y = v.Y;
}
if (v.Z > box.Max.Z)
{
box.Max.Z = v.Z;
}
if (v.Z < box.Min.Z)
{
box.Min.Z = v.Z;
}
}
//Add the corners. This will overestimate the size of the surface a bit.
hullVertices.Add(box.Min);
hullVertices.Add(box.Max);
hullVertices.Add(new Vector3(box.Min.X, box.Min.Y, box.Max.Z));
hullVertices.Add(new Vector3(box.Min.X, box.Max.Y, box.Min.Z));
hullVertices.Add(new Vector3(box.Max.X, box.Min.Y, box.Min.Z));
hullVertices.Add(new Vector3(box.Min.X, box.Max.Y, box.Max.Z));
hullVertices.Add(new Vector3(box.Max.X, box.Max.Y, box.Min.Z));
hullVertices.Add(new Vector3(box.Max.X, box.Min.Y, box.Max.Z));
}
}
///<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);
}
}
}