/// <summary>
/// Modifies a contribution using a transform, position, and weight.
/// </summary>
/// <param name="transform">Transform to use to modify the contribution.</param>
/// <param name="center">Center to use to modify the contribution.</param>
/// <param name="baseContribution">Original unmodified contribution.</param>
/// <param name="weight">Weight of the contribution.</param>
/// <param name="contribution">Transformed contribution.</param>
public static void TransformContribution(ref RigidTransform transform, ref Vector3 center, ref Matrix3x3 baseContribution, float weight, out Matrix3x3 contribution)
{
Matrix3x3 rotation;
Matrix3x3.CreateFromQuaternion(ref transform.Orientation, out rotation);
Matrix3x3 temp;
//Do angular transformed contribution first...
Matrix3x3.MultiplyTransposed(ref rotation, ref baseContribution, out temp);
Matrix3x3.Multiply(ref temp, ref rotation, out temp);
contribution = temp;
//Now add in the offset from the origin.
Vector3 offset;
Vector3.Subtract(ref transform.Position, ref center, out offset);
Matrix3x3 innerProduct;
Matrix3x3.CreateScale(offset.LengthSquared(), out innerProduct);
Matrix3x3 outerProduct;
Matrix3x3.CreateOuterProduct(ref offset, ref offset, out outerProduct);
Matrix3x3.Subtract(ref innerProduct, ref outerProduct, out temp);
Matrix3x3.Add(ref contribution, ref temp, out contribution);
Matrix3x3.Multiply(ref contribution, weight, out contribution);
}
void IPositionUpdateable.PreUpdatePosition(float dt)
{
Vector3 increment;
Vector3.Multiply(ref angularVelocity, dt * .5f, out increment);
var multiplier = new Quaternion(increment.X, increment.Y, increment.Z, 0);
Quaternion.Multiply(ref multiplier, ref orientation, out multiplier);
Quaternion.Add(ref orientation, ref multiplier, out orientation);
orientation.Normalize();
Matrix3x3.CreateFromQuaternion(ref orientation, out orientationMatrix);
//Only do the linear motion if this object doesn't obey CCD.
if (PositionUpdateMode == PositionUpdateMode.Discrete)
{
Vector3.Multiply(ref linearVelocity, dt, out increment);
Vector3.Add(ref position, ref increment, out position);
collisionInformation.UpdateWorldTransform(ref position, ref orientation);
//The position update is complete if this is a discretely updated object.
if (PositionUpdated != null)
PositionUpdated(this);
}
MathChecker.Validate(linearVelocity);
MathChecker.Validate(angularVelocity);
MathChecker.Validate(position);
MathChecker.Validate(orientation);
#if CONSERVE
MathChecker.Validate(angularMomentum);
#endif
}
/// <summary>
/// Gets the bounding box of the shape given a transform.
/// </summary>
/// <param name="shapeTransform">Transform to use.</param>
/// <param name="boundingBox">Bounding box of the transformed shape.</param>
public override void GetBoundingBox(ref RigidTransform shapeTransform, out BoundingBox boundingBox)
{
#if !WINDOWS
boundingBox = new BoundingBox();
#endif
Matrix3x3 o;
Matrix3x3.CreateFromQuaternion(ref shapeTransform.Orientation, out o);
//Sample the local directions from the orientation matrix, implicitly transposed.
//Notice only three directions are used. Due to box symmetry, 'left' is just -right.
var right = new Vector3(Math.Sign(o.M11) * halfWidth, Math.Sign(o.M21) * halfHeight, Math.Sign(o.M31) * halfLength);
var up = new Vector3(Math.Sign(o.M12) * halfWidth, Math.Sign(o.M22) * halfHeight, Math.Sign(o.M32) * halfLength);
var backward = new Vector3(Math.Sign(o.M13) * halfWidth, Math.Sign(o.M23) * halfHeight, Math.Sign(o.M33) * halfLength);
//Rather than transforming each axis independently (and doing three times as many operations as required), just get the 3 required values directly.
Vector3 offset;
TransformLocalExtremePoints(ref right, ref up, ref backward, ref o, out offset);
//The positive and negative vectors represent the X, Y and Z coordinates of the extreme points in world space along the world space axes.
Vector3.Add(ref shapeTransform.Position, ref offset, out boundingBox.Max);
Vector3.Subtract(ref shapeTransform.Position, ref offset, out boundingBox.Min);
}
///<summary>
/// Computes the bounding box of the transformed mesh shape.
///</summary>
///<param name="shapeTransform">Transform to apply to the shape during the bounding box calculation.</param>
///<param name="boundingBox">Bounding box containing the transformed mesh shape.</param>
public override void GetBoundingBox(ref RigidTransform shapeTransform, out BoundingBox boundingBox)
{
//TODO: Could use an approximate bounding volume. Would be cheaper at runtime and use less memory, though the box would be bigger.
Matrix3x3 o;
Matrix3x3.CreateFromQuaternion(ref shapeTransform.Orientation, out o);
GetBoundingBox(ref o, out boundingBox);
Vector3.Add(ref boundingBox.Max, ref shapeTransform.Position, out boundingBox.Max);
Vector3.Add(ref boundingBox.Min, ref shapeTransform.Position, out boundingBox.Min);
}
/// <summary>
/// Gets the bounding box of the shape given a transform.
/// </summary>
/// <param name="shapeTransform">Transform to use.</param>
/// <param name="boundingBox">Bounding box of the transformed shape.</param>
public override void GetBoundingBox(ref RigidTransform shapeTransform, out BoundingBox boundingBox)
{
#if !WINDOWS
boundingBox = new BoundingBox();
#endif
Matrix3x3 o;
Matrix3x3.CreateFromQuaternion(ref shapeTransform.Orientation, out o);
//Sample the local directions from the orientation matrix, implicitly transposed.
Vector3 right;
var direction = new Vector3(o.M11, o.M21, o.M31);
GetLocalExtremePointWithoutMargin(ref direction, out right);
Vector3 left;
direction = new Vector3(-o.M11, -o.M21, -o.M31);
GetLocalExtremePointWithoutMargin(ref direction, out left);
Vector3 up;
direction = new Vector3(o.M12, o.M22, o.M32);
GetLocalExtremePointWithoutMargin(ref direction, out up);
Vector3 down;
direction = new Vector3(-o.M12, -o.M22, -o.M32);
GetLocalExtremePointWithoutMargin(ref direction, out down);
Vector3 backward;
direction = new Vector3(o.M13, o.M23, o.M33);
GetLocalExtremePointWithoutMargin(ref direction, out backward);
Vector3 forward;
direction = new Vector3(-o.M13, -o.M23, -o.M33);
GetLocalExtremePointWithoutMargin(ref direction, out forward);
//Rather than transforming each axis independently (and doing three times as many operations as required), just get the 6 required values directly.
Vector3 positive, negative;
TransformLocalExtremePoints(ref right, ref up, ref backward, ref o, out positive);
TransformLocalExtremePoints(ref left, ref down, ref forward, ref o, out negative);
//The positive and negative vectors represent the X, Y and Z coordinates of the extreme points in world space along the world space axes.
boundingBox.Max.X = shapeTransform.Position.X + positive.X + collisionMargin;
boundingBox.Max.Y = shapeTransform.Position.Y + positive.Y + collisionMargin;
boundingBox.Max.Z = shapeTransform.Position.Z + positive.Z + collisionMargin;
boundingBox.Min.X = shapeTransform.Position.X + negative.X - collisionMargin;
boundingBox.Min.Y = shapeTransform.Position.Y + negative.Y - collisionMargin;
boundingBox.Min.Z = shapeTransform.Position.Z + negative.Z - collisionMargin;
}
///<summary>
/// Updates the cached simplex with the latest run's results.
///</summary>
///<param name="simplex">Simplex to update.</param>
public void UpdateCachedSimplex(ref CachedSimplex simplex)
{
simplex.LocalSimplexA = SimplexA;
switch (State)
{
case SimplexState.Point:
Vector3.Subtract(ref SimplexB.A, ref LocalTransformB.Position, out simplex.LocalSimplexB.A);
Quaternion conjugate;
Quaternion.Conjugate(ref LocalTransformB.Orientation, out conjugate);
Quaternion.Transform(ref simplex.LocalSimplexB.A, ref conjugate, out simplex.LocalSimplexB.A);
break;
case SimplexState.Segment:
Vector3.Subtract(ref SimplexB.A, ref LocalTransformB.Position, out simplex.LocalSimplexB.A);
Vector3.Subtract(ref SimplexB.B, ref LocalTransformB.Position, out simplex.LocalSimplexB.B);
Matrix3x3 transform;
Matrix3x3.CreateFromQuaternion(ref LocalTransformB.Orientation, out transform);
Matrix3x3.TransformTranspose(ref simplex.LocalSimplexB.A, ref transform, out simplex.LocalSimplexB.A);
Matrix3x3.TransformTranspose(ref simplex.LocalSimplexB.B, ref transform, out simplex.LocalSimplexB.B);
break;
case SimplexState.Triangle:
Vector3.Subtract(ref SimplexB.A, ref LocalTransformB.Position, out simplex.LocalSimplexB.A);
Vector3.Subtract(ref SimplexB.B, ref LocalTransformB.Position, out simplex.LocalSimplexB.B);
Vector3.Subtract(ref SimplexB.C, ref LocalTransformB.Position, out simplex.LocalSimplexB.C);
Matrix3x3.CreateFromQuaternion(ref LocalTransformB.Orientation, out transform);
Matrix3x3.TransformTranspose(ref simplex.LocalSimplexB.A, ref transform, out simplex.LocalSimplexB.A);
Matrix3x3.TransformTranspose(ref simplex.LocalSimplexB.B, ref transform, out simplex.LocalSimplexB.B);
Matrix3x3.TransformTranspose(ref simplex.LocalSimplexB.C, ref transform, out simplex.LocalSimplexB.C);
break;
case SimplexState.Tetrahedron:
Vector3.Subtract(ref SimplexB.A, ref LocalTransformB.Position, out simplex.LocalSimplexB.A);
Vector3.Subtract(ref SimplexB.B, ref LocalTransformB.Position, out simplex.LocalSimplexB.B);
Vector3.Subtract(ref SimplexB.C, ref LocalTransformB.Position, out simplex.LocalSimplexB.C);
Vector3.Subtract(ref SimplexB.D, ref LocalTransformB.Position, out simplex.LocalSimplexB.D);
Matrix3x3.CreateFromQuaternion(ref LocalTransformB.Orientation, out transform);
Matrix3x3.TransformTranspose(ref simplex.LocalSimplexB.A, ref transform, out simplex.LocalSimplexB.A);
Matrix3x3.TransformTranspose(ref simplex.LocalSimplexB.B, ref transform, out simplex.LocalSimplexB.B);
Matrix3x3.TransformTranspose(ref simplex.LocalSimplexB.C, ref transform, out simplex.LocalSimplexB.C);
Matrix3x3.TransformTranspose(ref simplex.LocalSimplexB.D, ref transform, out simplex.LocalSimplexB.D);
break;
}
simplex.State = State;
}
/// <summary>
/// Gets the intersection between the triangle and the ray.
/// </summary>
/// <param name="ray">Ray to test against the triangle.</param>
/// <param name="transform">Transform to apply to the triangle shape for the test.</param>
/// <param name="maximumLength">Maximum distance to travel in units of the direction vector's length.</param>
/// <param name="hit">Hit data of the ray cast, if any.</param>
/// <returns>Whether or not the ray hit the target.</returns>
public override bool RayTest(ref Ray ray, ref RigidTransform transform, float maximumLength, out RayHit hit)
{
Matrix3x3 orientation;
Matrix3x3.CreateFromQuaternion(ref transform.Orientation, out orientation);
Ray localRay;
Quaternion conjugate;
Quaternion.Conjugate(ref transform.Orientation, out conjugate);
Quaternion.Transform(ref ray.Direction, ref conjugate, out localRay.Direction);
Vector3.Subtract(ref ray.Position, ref transform.Position, out localRay.Position);
Quaternion.Transform(ref localRay.Position, ref conjugate, out localRay.Position);
bool toReturn = Toolbox.FindRayTriangleIntersection(ref localRay, maximumLength, sidedness, ref vA, ref vB, ref vC, out hit);
//Move the hit back into world space.
Vector3.Multiply(ref ray.Direction, hit.T, out hit.Location);
Vector3.Add(ref ray.Position, ref hit.Location, out hit.Location);
Quaternion.Transform(ref hit.Normal, ref transform.Orientation, out hit.Normal);
return(toReturn);
}
///<summary>
/// Tests a ray against the surface of the mesh. This does not take into account solidity.
///</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;
Matrix3x3 orientation;
Matrix3x3.CreateFromQuaternion(ref worldTransform.Orientation, out orientation);
Matrix3x3.TransformTranspose(ref ray.Direction, ref orientation, out localRay.Direction);
Vector3.Subtract(ref ray.Position, ref worldTransform.Position, out localRay.Position);
Matrix3x3.TransformTranspose(ref localRay.Position, ref orientation, 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.Transform(ref rayHit.Normal, ref orientation, out rayHit.Normal);
return(true);
}
rayHit = new RayHit();
return(false);
}
/// <summary>
/// Recalculates the bounding box of the fluid based on its depth, surface normal, and surface triangles.
/// </summary>
public void RecalculateBoundingBox()
{
var points = CommonResources.GetVectorList();
foreach (var tri in SurfaceTriangles)
{
points.Add(tri[0]);
points.Add(tri[1]);
points.Add(tri[2]);
points.Add(tri[0] - upVector * MaxDepth);
points.Add(tri[1] - upVector * MaxDepth);
points.Add(tri[2] - upVector * MaxDepth);
}
boundingBox = BoundingBox.CreateFromPoints(points);
CommonResources.GiveBack(points);
//Compute the transforms used to pull objects into fluid local space.
Quaternion.GetQuaternionBetweenNormalizedVectors(ref Toolbox.UpVector, ref upVector, out surfaceTransform.Orientation);
Matrix3x3.CreateFromQuaternion(ref surfaceTransform.Orientation, out toSurfaceRotationMatrix);
surfaceTransform.Position = surfaceTriangles[0][0];
}
/// <summary>
/// Gets the bounding box of the shape given a transform.
/// </summary>
/// <param name="shapeTransform">Transform to use.</param>
/// <param name="boundingBox">Bounding box of the transformed shape.</param>
public override void GetBoundingBox(ref RigidTransform shapeTransform, out BoundingBox boundingBox)
{
Vector3 a, b, c;
Matrix3x3 o;
Matrix3x3.CreateFromQuaternion(ref shapeTransform.Orientation, out o);
Matrix3x3.Transform(ref vA, ref o, out a);
Matrix3x3.Transform(ref vB, ref o, out b);
Matrix3x3.Transform(ref vC, ref o, out c);
Vector3.Min(ref a, ref b, out boundingBox.Min);
Vector3.Min(ref c, ref boundingBox.Min, out boundingBox.Min);
Vector3.Max(ref a, ref b, out boundingBox.Max);
Vector3.Max(ref c, ref boundingBox.Max, out boundingBox.Max);
boundingBox.Min.X += shapeTransform.Position.X - collisionMargin;
boundingBox.Min.Y += shapeTransform.Position.Y - collisionMargin;
boundingBox.Min.Z += shapeTransform.Position.Z - collisionMargin;
boundingBox.Max.X += shapeTransform.Position.X + collisionMargin;
boundingBox.Max.Y += shapeTransform.Position.Y + collisionMargin;
boundingBox.Max.Z += shapeTransform.Position.Z + collisionMargin;
}
/// <summary>
/// Gets the bounding box of the shape given a transform.
/// </summary>
/// <param name="shapeTransform">Transform to use.</param>
/// <param name="boundingBox">Bounding box of the transformed shape.</param>
public override void GetBoundingBox(ref RigidTransform shapeTransform, out BoundingBox boundingBox)
{
#if !WINDOWS
boundingBox = new BoundingBox();
#endif
Matrix3x3 o;
Matrix3x3.CreateFromQuaternion(ref shapeTransform.Orientation, out o);
float minX, maxX;
float minY, maxY;
float minZ, maxZ;
var right = new Vector3(o.M11, o.M21, o.M31);
var up = new Vector3(o.M12, o.M22, o.M32);
var backward = new Vector3(o.M13, o.M23, o.M33);
Vector3.Dot(ref vertices[0], ref right, out maxX);
minX = maxX;
Vector3.Dot(ref vertices[0], ref up, out maxY);
minY = maxY;
Vector3.Dot(ref vertices[0], ref backward, out maxZ);
minZ = maxZ;
int minXIndex = 0;
int maxXIndex = 0;
int minYIndex = 0;
int maxYIndex = 0;
int minZIndex = 0;
int maxZIndex = 0;
for (int i = 1; i < vertices.Length; ++i)
{
float dot;
Vector3.Dot(ref vertices[i], ref right, out dot);
if (dot < minX)
{
minX = dot;
minXIndex = i;
}
else if (dot > maxX)
{
maxX = dot;
maxXIndex = i;
}
Vector3.Dot(ref vertices[i], ref up, out dot);
if (dot < minY)
{
minY = dot;
minYIndex = i;
}
else if (dot > maxY)
{
maxY = dot;
maxYIndex = i;
}
Vector3.Dot(ref vertices[i], ref backward, out dot);
if (dot < minZ)
{
minZ = dot;
minZIndex = i;
}
else if (dot > maxZ)
{
maxZ = dot;
maxZIndex = i;
}
}
//Rather than transforming each axis independently (and doing three times as many operations as required), just get the 6 required values directly.
Vector3 positive, negative;
TransformLocalExtremePoints(ref vertices[maxXIndex], ref vertices[maxYIndex], ref vertices[maxZIndex], ref o, out positive);
TransformLocalExtremePoints(ref vertices[minXIndex], ref vertices[minYIndex], ref vertices[minZIndex], ref o, out negative);
//The positive and negative vectors represent the X, Y and Z coordinates of the extreme points in world space along the world space axes.
boundingBox.Max.X = shapeTransform.Position.X + positive.X + collisionMargin;
boundingBox.Max.Y = shapeTransform.Position.Y + positive.Y + collisionMargin;
boundingBox.Max.Z = shapeTransform.Position.Z + positive.Z + collisionMargin;
boundingBox.Min.X = shapeTransform.Position.X + negative.X - collisionMargin;
boundingBox.Min.Y = shapeTransform.Position.Y + negative.Y - collisionMargin;
boundingBox.Min.Z = shapeTransform.Position.Z + negative.Z - collisionMargin;
}
protected override void ProcessCandidates(ref QuickList <ContactData> candidates)
{
if (candidates.Count == 0 && parentContactCount == 0 && Mesh.Shape.solidity == MobileMeshSolidity.Solid)
{
//If there's no new contacts on the mesh and it's supposed to be a solid,
//then we must check the convex for containment within the shell.
//We already know that it's not on the shell, meaning that the shape is either
//far enough away outside the shell that there's no contact (and we're done),
//or it's far enough inside the shell that the triangles cannot create contacts.
//To find out which it is, raycast against the shell.
Matrix3x3 orientation;
Matrix3x3.CreateFromQuaternion(ref mesh.worldTransform.Orientation, out orientation);
Ray ray;
Vector3.Subtract(ref convex.worldTransform.Position, ref mesh.worldTransform.Position, out ray.Position);
Matrix3x3.TransformTranspose(ref ray.Position, ref orientation, out ray.Position);
//Cast from the current position back to the previous position.
Vector3.Subtract(ref lastValidConvexPosition, ref ray.Position, out ray.Direction);
float rayDirectionLength = ray.Direction.LengthSquared();
if (rayDirectionLength < Toolbox.Epsilon)
{
//The object may not have moved enough to normalize properly. If so, choose something arbitrary.
//Try the direction from the center of the object to the convex's position.
ray.Direction = ray.Position;
rayDirectionLength = ray.Direction.LengthSquared();
if (rayDirectionLength < Toolbox.Epsilon)
{
//This is unlikely; just pick something completely arbitrary then.
ray.Direction = Vector3.Up;
rayDirectionLength = 1;
}
}
Vector3.Divide(ref ray.Direction, (float)Math.Sqrt(rayDirectionLength), out ray.Direction);
RayHit hit;
if (mesh.Shape.IsLocalRayOriginInMesh(ref ray, out hit))
{
ContactData newContact = new ContactData {
Id = 2
};
//Give it a special id so that we know that it came from the inside.
Matrix3x3.Transform(ref ray.Position, ref orientation, out newContact.Position);
Vector3.Add(ref newContact.Position, ref mesh.worldTransform.Position, out newContact.Position);
newContact.Normal = hit.Normal;
newContact.Normal.Normalize();
float factor;
Vector3.Dot(ref ray.Direction, ref newContact.Normal, out factor);
newContact.PenetrationDepth = -factor * hit.T + convex.Shape.MinimumRadius;
Matrix3x3.Transform(ref newContact.Normal, ref orientation, out newContact.Normal);
newContact.Validate();
//Do not yet create a new contact. Check to see if an 'inner contact' with id == 2 already exists.
bool addContact = true;
for (int i = 0; i < contacts.Count; i++)
{
if (contacts.Elements[i].Id == 2)
{
contacts.Elements[i].Position = newContact.Position;
contacts.Elements[i].Normal = newContact.Normal;
contacts.Elements[i].PenetrationDepth = newContact.PenetrationDepth;
supplementData.Elements[i].BasePenetrationDepth = newContact.PenetrationDepth;
supplementData.Elements[i].LocalOffsetA = new Vector3();
supplementData.Elements[i].LocalOffsetB = ray.Position; //convex local position in mesh.
addContact = false;
break;
}
}
if (addContact && contacts.Count == 0)
{
Add(ref newContact);
}
previousDepth = newContact.PenetrationDepth;
}
else
{
//It's possible that we had a false negative. The previous frame may have been in deep intersection, and this frame just failed to come to the same conclusion.
//If we set the target location to the current location, the object will never escape the mesh. Instead, only do that if two frames agree that we are no longer colliding.
if (previousDepth > 0)
{
//We're not touching the mesh.
lastValidConvexPosition = ray.Position;
}
previousDepth = 0;
}
}
}
///<summary>
/// Constructs a new pair simplex.
///</summary>
///<param name="cachedSimplex">Cached simplex to use to warmstart the simplex.</param>
///<param name="localTransformB">Transform of shape B in the local space of A.</param>
public PairSimplex(ref CachedSimplex cachedSimplex, ref RigidTransform localTransformB)
{
//NOTE:
//USING A CACHED SIMPLEX INVALIDATES ASSUMPTIONS THAT ALLOW SIMPLEX CASES TO BE IGNORED!
//To get those assumptions back, either DO NOT USE CACHED SIMPLEXES, or
//VERIFY THE SIMPLEXES.
//-A point requires no verification.
//-A segment needs verification that the origin is in front of A in the direction of B.
//-A triangle needs verification that the origin is within the edge planes and in the direction of C.
//-A tetrahedron needs verification that the origin is within the edge planes of triangle ABC and is in the direction of D.
//This simplex implementation will not ignore any cases, so we can warm start safely with one problem.
//Due to relative movement, the simplex may become degenerate. Edges could become points, etc.
//Some protections are built into the simplex cases, but keep an eye out for issues.
//Most dangerous degeneracy seen so far is tetrahedron. It fails to find any points on opposing sides due to numerical problems and returns intersection.
previousDistanceToClosest = float.MaxValue;
errorTolerance = 0;
LocalTransformB = localTransformB;
//Transform the SimplexB into the working space of the simplex and compute the working space simplex.
State = cachedSimplex.State;
SimplexA = cachedSimplex.LocalSimplexA;
SimplexB = new ContributingShapeSimplex();
U = 0;
V = 0;
W = 0;
switch (State)
{
case SimplexState.Point:
Quaternion.Transform(ref cachedSimplex.LocalSimplexB.A, ref LocalTransformB.Orientation, out SimplexB.A);
Vector3.Add(ref SimplexB.A, ref LocalTransformB.Position, out SimplexB.A);
Vector3.Subtract(ref SimplexA.A, ref SimplexB.A, out A);
B = new Vector3();
C = new Vector3();
D = new Vector3();
break;
case SimplexState.Segment:
Matrix3x3 transform;
Matrix3x3.CreateFromQuaternion(ref localTransformB.Orientation, out transform);
Matrix3x3.Transform(ref cachedSimplex.LocalSimplexB.A, ref transform, out SimplexB.A);
Matrix3x3.Transform(ref cachedSimplex.LocalSimplexB.B, ref transform, out SimplexB.B);
Vector3.Add(ref SimplexB.A, ref LocalTransformB.Position, out SimplexB.A);
Vector3.Add(ref SimplexB.B, ref LocalTransformB.Position, out SimplexB.B);
Vector3.Subtract(ref SimplexA.A, ref SimplexB.A, out A);
Vector3.Subtract(ref SimplexA.B, ref SimplexB.B, out B);
C = new Vector3();
D = new Vector3();
////Test for degeneracy.
//float edgeLengthAB;
//Vector3.DistanceSquared(ref A, ref B, out edgeLengthAB);
//if (edgeLengthAB < Toolbox.Epsilon)
// State = SimplexState.Point;
break;
case SimplexState.Triangle:
Matrix3x3.CreateFromQuaternion(ref localTransformB.Orientation, out transform);
Matrix3x3.Transform(ref cachedSimplex.LocalSimplexB.A, ref transform, out SimplexB.A);
Matrix3x3.Transform(ref cachedSimplex.LocalSimplexB.B, ref transform, out SimplexB.B);
Matrix3x3.Transform(ref cachedSimplex.LocalSimplexB.C, ref transform, out SimplexB.C);
Vector3.Add(ref SimplexB.A, ref LocalTransformB.Position, out SimplexB.A);
Vector3.Add(ref SimplexB.B, ref LocalTransformB.Position, out SimplexB.B);
Vector3.Add(ref SimplexB.C, ref LocalTransformB.Position, out SimplexB.C);
Vector3.Subtract(ref SimplexA.A, ref SimplexB.A, out A);
Vector3.Subtract(ref SimplexA.B, ref SimplexB.B, out B);
Vector3.Subtract(ref SimplexA.C, ref SimplexB.C, out C);
D = new Vector3();
////Test for degeneracy.
//Vector3 AB, AC;
//Vector3.Subtract(ref B, ref A, out AB);
//Vector3.Subtract(ref C, ref A, out AC);
//Vector3 cross;
//Vector3.Cross(ref AB, ref AC, out cross);
////If the area is small compared to a tolerance (adjusted by the partial perimeter), it's degenerate.
//if (cross.LengthSquared() < Toolbox.BigEpsilon * (AB.LengthSquared() + AC.LengthSquared()))
// State = SimplexState.Point;
break;
case SimplexState.Tetrahedron:
Matrix3x3.CreateFromQuaternion(ref localTransformB.Orientation, out transform);
Matrix3x3.Transform(ref cachedSimplex.LocalSimplexB.A, ref transform, out SimplexB.A);
Matrix3x3.Transform(ref cachedSimplex.LocalSimplexB.B, ref transform, out SimplexB.B);
Matrix3x3.Transform(ref cachedSimplex.LocalSimplexB.C, ref transform, out SimplexB.C);
Matrix3x3.Transform(ref cachedSimplex.LocalSimplexB.D, ref transform, out SimplexB.D);
Vector3.Add(ref SimplexB.A, ref LocalTransformB.Position, out SimplexB.A);
Vector3.Add(ref SimplexB.B, ref LocalTransformB.Position, out SimplexB.B);
Vector3.Add(ref SimplexB.C, ref LocalTransformB.Position, out SimplexB.C);
Vector3.Add(ref SimplexB.D, ref LocalTransformB.Position, out SimplexB.D);
Vector3.Subtract(ref SimplexA.A, ref SimplexB.A, out A);
Vector3.Subtract(ref SimplexA.B, ref SimplexB.B, out B);
Vector3.Subtract(ref SimplexA.C, ref SimplexB.C, out C);
Vector3.Subtract(ref SimplexA.D, ref SimplexB.D, out D);
////Test for degeneracy.
//Vector3 AD;
//Vector3.Subtract(ref B, ref A, out AB);
//Vector3.Subtract(ref C, ref A, out AC);
//Vector3.Subtract(ref D, ref A, out AD);
//Vector3.Cross(ref AB, ref AC, out cross);
//float volume;
//Vector3.Dot(ref cross, ref AD, out volume);
////Volume is small compared to partial 'perimeter.'
//if (volume < Toolbox.BigEpsilon * (AB.LengthSquared() + AC.LengthSquared() + AD.LengthSquared()))
// State = SimplexState.Point;
break;
default:
A = new Vector3();
B = new Vector3();
C = new Vector3();
D = new Vector3();
break;
}
}
public override void Update(float dt)
{
//First, refresh all existing contacts. This is an incremental manifold.
ContactRefresher.ContactRefresh(contacts, supplementData, ref convex.worldTransform, ref triangle.worldTransform, contactIndicesToRemove);
RemoveQueuedContacts();
//Compute the local triangle vertices.
//TODO: this could be quicker and cleaner.
localTriangleShape.collisionMargin = triangle.Shape.collisionMargin;
localTriangleShape.sidedness = triangle.Shape.sidedness;
Matrix3x3 orientation;
Matrix3x3.CreateFromQuaternion(ref triangle.worldTransform.Orientation, out orientation);
Matrix3x3.Transform(ref triangle.Shape.vA, ref orientation, out localTriangleShape.vA);
Matrix3x3.Transform(ref triangle.Shape.vB, ref orientation, out localTriangleShape.vB);
Matrix3x3.Transform(ref triangle.Shape.vC, ref orientation, out localTriangleShape.vC);
Vector3.Add(ref localTriangleShape.vA, ref triangle.worldTransform.Position, out localTriangleShape.vA);
Vector3.Add(ref localTriangleShape.vB, ref triangle.worldTransform.Position, out localTriangleShape.vB);
Vector3.Add(ref localTriangleShape.vC, ref triangle.worldTransform.Position, out localTriangleShape.vC);
Vector3.Subtract(ref localTriangleShape.vA, ref convex.worldTransform.Position, out localTriangleShape.vA);
Vector3.Subtract(ref localTriangleShape.vB, ref convex.worldTransform.Position, out localTriangleShape.vB);
Vector3.Subtract(ref localTriangleShape.vC, ref convex.worldTransform.Position, out localTriangleShape.vC);
Matrix3x3.CreateFromQuaternion(ref convex.worldTransform.Orientation, out orientation);
Matrix3x3.TransformTranspose(ref localTriangleShape.vA, ref orientation, out localTriangleShape.vA);
Matrix3x3.TransformTranspose(ref localTriangleShape.vB, ref orientation, out localTriangleShape.vB);
Matrix3x3.TransformTranspose(ref localTriangleShape.vC, ref orientation, out localTriangleShape.vC);
//Now, generate a contact between the two shapes.
ContactData contact;
TinyStructList <ContactData> contactList;
if (pairTester.GenerateContactCandidates(localTriangleShape, out contactList))
{
for (int i = 0; i < contactList.Count; i++)
{
contactList.Get(i, out contact);
//Put the contact into world space.
Matrix3x3.Transform(ref contact.Position, ref orientation, out contact.Position);
Vector3.Add(ref contact.Position, ref convex.worldTransform.Position, out contact.Position);
Matrix3x3.Transform(ref contact.Normal, ref orientation, out contact.Normal);
//Check if the contact is unique before proceeding.
if (IsContactUnique(ref contact))
{
//Check if adding the new contact would overflow the manifold.
if (contacts.Count == 4)
{
//Adding that contact would overflow the manifold. Reduce to the best subset.
bool addCandidate;
ContactReducer.ReduceContacts(contacts, ref contact, contactIndicesToRemove, out addCandidate);
RemoveQueuedContacts();
if (addCandidate)
{
Add(ref contact);
}
}
else
{
//Won't overflow the manifold, so just toss it in PROVIDED that it isn't too close to something else.
Add(ref contact);
}
}
}
}
else
{
//Clear out the contacts, it's separated.
for (int i = contacts.Count - 1; i >= 0; i--)
{
Remove(i);
}
}
}
/// <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>
/// 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();
}
/// <summary>
/// Tests a ray against the entry.
/// </summary>
/// <param name="ray">Ray to test.</param>
/// <param name="maximumLength">Maximum length, in units of the ray's direction's length, to test.</param>
/// <param name="rayHit">Hit location of the ray on the entry, if any.</param>
/// <returns>Whether or not the ray hit the entry.</returns>
public override bool RayCast(Ray ray, float maximumLength, out RayHit rayHit)
{
//Put the ray into local space.
Ray localRay;
Matrix3x3 orientation;
Matrix3x3.CreateFromQuaternion(ref worldTransform.Orientation, out orientation);
Matrix3x3.TransformTranspose(ref ray.Direction, ref orientation, out localRay.Direction);
Vector3.Subtract(ref ray.Position, ref worldTransform.Position, out localRay.Position);
Matrix3x3.TransformTranspose(ref localRay.Position, ref orientation, out localRay.Position);
if (Shape.solidity == MobileMeshSolidity.Solid)
{
//Find all hits. Use the count to determine the ray started inside or outside.
//If it starts inside and we're in 'solid' mode, then return the ray start.
//The raycast must be of infinite length at first. This allows it to determine
//if it is inside or outside.
if (Shape.IsLocalRayOriginInMesh(ref localRay, out rayHit))
{
//It was inside!
rayHit = new RayHit()
{
Location = ray.Position, Normal = Vector3.Zero, T = 0
};
return(true);
}
else
{
if (rayHit.T < maximumLength)
{
//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.Transform(ref rayHit.Normal, ref orientation, out rayHit.Normal);
}
else
{
//The hit was too far away, or there was no hit (in which case T would be float.MaxValue).
return(false);
}
return(true);
}
}
else
{
//Just do a normal raycast since the object isn't solid.
TriangleSidedness sidedness;
switch (Shape.solidity)
{
case MobileMeshSolidity.Clockwise:
sidedness = TriangleSidedness.Clockwise;
break;
case MobileMeshSolidity.Counterclockwise:
sidedness = TriangleSidedness.Counterclockwise;
break;
default:
sidedness = TriangleSidedness.DoubleSided;
break;
}
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.Transform(ref rayHit.Normal, ref orientation, out rayHit.Normal);
return(true);
}
}
rayHit = new RayHit();
return(false);
}
///<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);
}
}
}
