/// <summary>
/// Gets the intersection between the convex shape and the ray.
/// </summary>
/// <param name="ray">Ray to test.</param>
/// <param name="transform">Transform of the convex shape.</param>
/// <param name="maximumLength">Maximum distance to travel in units of the ray direction's length.</param>
/// <param name="hit">Ray hit data, if any.</param>
/// <returns>Whether or not the ray hit the target.</returns>
public virtual bool RayTest(ref Ray ray, ref RigidTransform transform, float maximumLength, out RayHit hit)
{
//RayHit newHit;
//bool newBool = GJKToolbox.RayCast(ray, this, ref transform, maximumLength, out newHit);
//RayHit oldHit;
//bool oldBool = OldGJKVerifier.RayCastGJK(ray.Position, ray.Direction, maximumLength, this, transform, out oldHit.Location, out oldHit.Normal, out oldHit.T);
//bool mprBool = MPRToolbox.RayCast(ray, maximumLength, this, ref transform, out hit);
////if (newBool != oldBool || ((newBool && oldBool) && Vector3.DistanceSquared(newHit.Location, hit.Location) > .01f))
//// Debug.WriteLine("break.");
//return mprBool;
//if (GJKToolbox.RayCast(ray, this, ref transform, maximumLength, out hit))
//{
// //GJK toolbox doesn't normalize the hit normal; it's unnecessary for some other systems so it just saves on time.
// //It would be nice if ray tests always normalized it though.
// float length = hit.Normal.LengthSquared();
// if (length > Toolbox.Epsilon)
// Vector3.Divide(ref hit.Normal, (float) Math.Sqrt(length), out hit.Normal);
// else
// hit.Normal = new Vector3();
// return true;
//}
//return false;
return(MPRToolbox.RayCast(ray, maximumLength, this, ref transform, out hit));
}
/// <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(CollisionShapes.ConvexShapes.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);
}
public override void UpdateCollision(float dt)
{
WasContaining = Containing;
WasTouching = Touching;
RigidTransform transform = new RigidTransform {
Orientation = Quaternion.Identity
};
DetectorVolume.TriangleMesh.Tree.GetOverlaps(convex.boundingBox, overlaps);
for (int i = 0; i < overlaps.Count; i++)
{
DetectorVolume.TriangleMesh.Data.GetTriangle(overlaps.Elements[i], out triangle.vA, out triangle.vB,
out triangle.vC);
Vector3.Add(ref triangle.vA, ref triangle.vB, out transform.Position);
Vector3.Add(ref triangle.vC, ref transform.Position, out transform.Position);
Vector3.Multiply(ref transform.Position, 1 / 3f, out transform.Position);
Vector3.Subtract(ref triangle.vA, ref transform.Position, out triangle.vA);
Vector3.Subtract(ref triangle.vB, ref transform.Position, out triangle.vB);
Vector3.Subtract(ref triangle.vC, ref transform.Position, out triangle.vC);
//If this triangle collides with the convex, we can stop immediately since we know we're touching and not containing.)))
//[MPR is used here in lieu of GJK because the MPR implementation tends to finish quicker when objects are overlapping than GJK. The GJK implementation does better on separated objects.]
if (MPRToolbox.AreShapesOverlapping(convex.Shape, triangle, ref convex.worldTransform, ref transform))
{
Touching = true;
//The convex can't be fully contained if it's still touching the surface.
Containing = false;
overlaps.Clear();
goto events;
}
}
overlaps.Clear();
//If we get here, then there was no shell intersection.
//If the convex's center point is contained by the mesh, then the convex is fully contained.
//If this is a child pair, the CheckContainment flag may be set to false. This is because the parent has
//already determined that it is not contained (another child performed the check and found that it was not contained)
//and that it is already touching somehow (either by intersection or by containment).
//so further containment tests are unnecessary.
if (CheckContainment && DetectorVolume.IsPointContained(ref convex.worldTransform.Position, overlaps))
{
Touching = true;
Containing = true;
goto events;
}
//If we get here, then there was no surface intersection and the convex's center is not contained- the volume and convex are separate!
Touching = false;
Containing = false;
events:
NotifyDetectorVolumeOfChanges();
}
/// <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(CollisionShapes.ConvexShapes.ConvexShape castShape, ref RigidTransform startingTransform, ref Vector3f sweep, out RayHit hit)
{
hit = new RayHit();
BoundingBox localSpaceBoundingBox;
castShape.GetSweptLocalBoundingBox(ref startingTransform, ref worldTransform, ref sweep, out localSpaceBoundingBox);
var tri = PhysicsThreadResources.GetTriangle();
var hitElements = new QuickList <int>(BufferPools <int> .Thread);
if (Shape.GetOverlaps(localSpaceBoundingBox, ref hitElements))
{
hit.T = float.MaxValue;
for (int i = 0; i < hitElements.Count; i++)
{
Shape.GetTriangle(hitElements.Elements[i], ref worldTransform, out tri.vA, out tri.vB, out tri.vC);
Vector3f center;
Vector3f.Add(ref tri.vA, ref tri.vB, out center);
Vector3f.Add(ref center, ref tri.vC, out center);
Vector3f.Multiply(ref center, 1f / 3f, out center);
Vector3f.Subtract(ref tri.vA, ref center, out tri.vA);
Vector3f.Subtract(ref tri.vB, ref center, out tri.vB);
Vector3f.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);
hitElements.Dispose();
return(hit.T != float.MaxValue);
}
PhysicsThreadResources.GiveBack(tri);
hitElements.Dispose();
return(false);
}
public override void Update(float dt)
{
if (Game.KeyboardInput.IsKeyDown(Keys.NumPad6))
{
aTransform.Position += Vector3.Right * dt;
}
if (Game.KeyboardInput.IsKeyDown(Keys.NumPad4))
{
aTransform.Position += Vector3.Left * dt;
}
if (Game.KeyboardInput.IsKeyDown(Keys.NumPad1))
{
aTransform.Position += Vector3.Up * dt;
}
if (Game.KeyboardInput.IsKeyDown(Keys.NumPad0))
{
aTransform.Position += Vector3.Down * dt;
}
if (Game.KeyboardInput.IsKeyDown(Keys.NumPad8))
{
aTransform.Position += Vector3.Forward * dt;
}
if (Game.KeyboardInput.IsKeyDown(Keys.NumPad5))
{
aTransform.Position += Vector3.Backward * dt;
}
Vector3 sweepA = new Vector3(0, 10, 0);
Vector3 sweepB = new Vector3(0, -10, 0);
if (Game.KeyboardInput.IsKeyDown(Keys.P))
{
Debug.WriteLine("Breka.");
}
if (hit = MPRToolbox.Sweep(aShape, bShape, ref sweepA, ref sweepB, ref aTransform, ref bTransform, out hitData))
//if (hit = OldGJKVerifier.ConvexCast(a.CollisionInformation.Shape, b.CollisionInformation.Shape, ref sweepA, ref sweepB, ref aTransform, ref bTransform, out hitData))
{
a.Position = aTransform.Position + sweepA * hitData.T;
b.Position = bTransform.Position + sweepB * hitData.T;
}
else
{
a.Position = aTransform.Position;
b.Position = bTransform.Position;
}
base.Update(dt);
}
private bool DoDeepContact(out ContactData contact)
{
if (previousState == CollisionState.Separated
) //If it was shallow before, then its closest points will be used to find the normal.
{
//It's overlapping! Find the relative velocity at the point relative to the two objects. The point is still in local space!
//The above takes into account angular velocity, but linear velocity alone is a lot more stable and does the job just fine.
if (collidableA.entity != null && collidableB.entity != null)
{
Vector3.Subtract(ref collidableA.entity.linearVelocity, ref collidableB.entity.linearVelocity,
out localDirection);
}
else
{
localDirection = localSeparatingAxis;
}
if (localDirection.LengthSquared() < Toolbox.Epsilon)
{
localDirection = Vector3.Up;
}
}
if (MPRToolbox.GetContact(collidableA.Shape, collidableB.Shape, ref collidableA.worldTransform,
ref collidableB.worldTransform, ref localDirection, out contact))
{
if (contact.PenetrationDepth < collidableA.Shape.collisionMargin + collidableB.Shape.collisionMargin)
{
state = CollisionState.ShallowContact;
}
return(true);
}
//This is rare, but could happen.
state = CollisionState.Separated;
return(false);
}
/// <summary>
/// Gets the intersection between the convex shape and the ray.
/// </summary>
/// <param name="ray">Ray to test.</param>
/// <param name="transform">Transform of the convex shape.</param>
/// <param name="maximumLength">Maximum distance to travel in units of the ray direction's length.</param>
/// <param name="hit">Ray hit data, if any.</param>
/// <returns>Whether or not the ray hit the target.</returns>
public virtual bool RayTest(ref Ray ray, ref RigidTransform transform, float maximumLength, out RayHit hit)
{
return(MPRToolbox.RayCast(ray, maximumLength, this, ref transform, out hit));
}
private bool DoDeepContact(TriangleShape triangle, out TinyStructList <ContactData> contactList)
{
//Find the origin to triangle center offset.
Vector3 center;
Vector3.Add(ref triangle.vA, ref triangle.vB, out center);
Vector3.Add(ref center, ref triangle.vC, out center);
Vector3.Multiply(ref center, 1f / 3f, out center);
ContactData contact;
contactList = new TinyStructList <ContactData>();
if (MPRToolbox.AreLocalShapesOverlapping(convex, triangle, ref center, ref Toolbox.RigidIdentity))
{
float dot;
Vector3 triangleNormal, ab, ac;
Vector3.Subtract(ref triangle.vB, ref triangle.vA, out ab);
Vector3.Subtract(ref triangle.vC, ref triangle.vA, out ac);
Vector3.Cross(ref ab, ref ac, out triangleNormal);
float lengthSquared = triangleNormal.LengthSquared();
if (lengthSquared < Toolbox.Epsilon * .01f)
{
//Degenerate triangle! That's no good.
//Just use the direction pointing from A to B, "B" being the triangle. That direction is center - origin, or just center.
MPRToolbox.LocalSurfaceCast(convex, triangle, ref Toolbox.RigidIdentity, ref center,
out contact.PenetrationDepth, out contact.Normal, out contact.Position);
}
else
{
//Normalize the normal.
Vector3.Divide(ref triangleNormal, (float)Math.Sqrt(lengthSquared), out triangleNormal);
//TODO: This tests all three edge axes with a full MPR raycast. That's not really necessary; the correct edge normal should be discoverable, resulting in a single MPR raycast.
//Find the edge directions that will be tested with MPR.
Vector3 AO, BO, CO;
Vector3 AB, BC, CA;
Vector3.Subtract(ref center, ref triangle.vA, out AO);
Vector3.Subtract(ref center, ref triangle.vB, out BO);
Vector3.Subtract(ref center, ref triangle.vC, out CO);
Vector3.Subtract(ref triangle.vB, ref triangle.vA, out AB);
Vector3.Subtract(ref triangle.vC, ref triangle.vB, out BC);
Vector3.Subtract(ref triangle.vA, ref triangle.vC, out CA);
//We don't have to worry about degenerate triangles here because we've already handled that possibility above.
Vector3 ABnormal, BCnormal, CAnormal;
//Project the center onto the edge to find the direction from the center to the edge AB.
Vector3.Dot(ref AO, ref AB, out dot);
Vector3.Multiply(ref AB, dot / AB.LengthSquared(), out ABnormal);
Vector3.Subtract(ref AO, ref ABnormal, out ABnormal);
ABnormal.Normalize();
//Project the center onto the edge to find the direction from the center to the edge BC.
Vector3.Dot(ref BO, ref BC, out dot);
Vector3.Multiply(ref BC, dot / BC.LengthSquared(), out BCnormal);
Vector3.Subtract(ref BO, ref BCnormal, out BCnormal);
BCnormal.Normalize();
//Project the center onto the edge to find the direction from the center to the edge BC.
Vector3.Dot(ref CO, ref CA, out dot);
Vector3.Multiply(ref CA, dot / CA.LengthSquared(), out CAnormal);
Vector3.Subtract(ref CO, ref CAnormal, out CAnormal);
CAnormal.Normalize();
MPRToolbox.LocalSurfaceCast(convex, triangle, ref Toolbox.RigidIdentity, ref ABnormal,
out contact.PenetrationDepth, out contact.Normal);
//Check to see if the normal is facing in the proper direction, considering that this may not be a two-sided triangle.
Vector3.Dot(ref triangleNormal, ref contact.Normal, out dot);
if (triangle.sidedness == TriangleSidedness.Clockwise && dot > 0 ||
triangle.sidedness == TriangleSidedness.Counterclockwise && dot < 0)
{
//Normal was facing the wrong way.
//Instead of ignoring it entirely, correct the direction to as close as it can get by removing any component parallel to the triangle normal.
Vector3 previousNormal = contact.Normal;
Vector3.Dot(ref contact.Normal, ref triangleNormal, out dot);
Vector3 p;
Vector3.Multiply(ref contact.Normal, dot, out p);
Vector3.Subtract(ref contact.Normal, ref p, out contact.Normal);
float length = contact.Normal.LengthSquared();
if (length > Toolbox.Epsilon)
{
//Renormalize the corrected normal.
Vector3.Divide(ref contact.Normal, (float)Math.Sqrt(length), out contact.Normal);
Vector3.Dot(ref contact.Normal, ref previousNormal, out dot);
contact.PenetrationDepth *= dot;
}
else
{
contact.PenetrationDepth = float.MaxValue;
contact.Normal = new Vector3();
}
}
Vector3 candidateNormal;
float candidateDepth;
MPRToolbox.LocalSurfaceCast(convex, triangle, ref Toolbox.RigidIdentity, ref BCnormal,
out candidateDepth, out candidateNormal);
//Check to see if the normal is facing in the proper direction, considering that this may not be a two-sided triangle.
Vector3.Dot(ref triangleNormal, ref candidateNormal, out dot);
if (triangle.sidedness == TriangleSidedness.Clockwise && dot > 0 ||
triangle.sidedness == TriangleSidedness.Counterclockwise && dot < 0)
{
//Normal was facing the wrong way.
//Instead of ignoring it entirely, correct the direction to as close as it can get by removing any component parallel to the triangle normal.
Vector3 previousNormal = candidateNormal;
Vector3.Dot(ref candidateNormal, ref triangleNormal, out dot);
Vector3 p;
Vector3.Multiply(ref candidateNormal, dot, out p);
Vector3.Subtract(ref candidateNormal, ref p, out candidateNormal);
float length = candidateNormal.LengthSquared();
if (length > Toolbox.Epsilon)
{
//Renormalize the corrected normal.
Vector3.Divide(ref candidateNormal, (float)Math.Sqrt(length), out candidateNormal);
Vector3.Dot(ref candidateNormal, ref previousNormal, out dot);
candidateDepth *= dot;
}
else
{
contact.PenetrationDepth = float.MaxValue;
contact.Normal = new Vector3();
}
}
if (candidateDepth < contact.PenetrationDepth)
{
contact.Normal = candidateNormal;
contact.PenetrationDepth = candidateDepth;
}
MPRToolbox.LocalSurfaceCast(convex, triangle, ref Toolbox.RigidIdentity, ref CAnormal,
out candidateDepth, out candidateNormal);
//Check to see if the normal is facing in the proper direction, considering that this may not be a two-sided triangle.
Vector3.Dot(ref triangleNormal, ref candidateNormal, out dot);
if (triangle.sidedness == TriangleSidedness.Clockwise && dot > 0 ||
triangle.sidedness == TriangleSidedness.Counterclockwise && dot < 0)
{
//Normal was facing the wrong way.
//Instead of ignoring it entirely, correct the direction to as close as it can get by removing any component parallel to the triangle normal.
Vector3 previousNormal = candidateNormal;
Vector3.Dot(ref candidateNormal, ref triangleNormal, out dot);
Vector3 p;
Vector3.Multiply(ref candidateNormal, dot, out p);
Vector3.Subtract(ref candidateNormal, ref p, out candidateNormal);
float length = candidateNormal.LengthSquared();
if (length > Toolbox.Epsilon)
{
//Renormalize the corrected normal.
Vector3.Divide(ref candidateNormal, (float)Math.Sqrt(length), out candidateNormal);
Vector3.Dot(ref candidateNormal, ref previousNormal, out dot);
candidateDepth *= dot;
}
else
{
contact.PenetrationDepth = float.MaxValue;
contact.Normal = new Vector3();
}
}
if (candidateDepth < contact.PenetrationDepth)
{
contact.Normal = candidateNormal;
contact.PenetrationDepth = candidateDepth;
}
//Try the depth along the positive triangle normal.
//If it's clockwise, this direction is unnecessary (the resulting normal would be invalidated by the onesidedness of the triangle).
if (triangle.sidedness != TriangleSidedness.Clockwise)
{
MPRToolbox.LocalSurfaceCast(convex, triangle, ref Toolbox.RigidIdentity, ref triangleNormal,
out candidateDepth, out candidateNormal);
if (candidateDepth < contact.PenetrationDepth)
{
contact.Normal = candidateNormal;
contact.PenetrationDepth = candidateDepth;
}
}
//Try the depth along the negative triangle normal.
//If it's counterclockwise, this direction is unnecessary (the resulting normal would be invalidated by the onesidedness of the triangle).
if (triangle.sidedness != TriangleSidedness.Counterclockwise)
{
Vector3.Negate(ref triangleNormal, out triangleNormal);
MPRToolbox.LocalSurfaceCast(convex, triangle, ref Toolbox.RigidIdentity, ref triangleNormal,
out candidateDepth, out candidateNormal);
if (candidateDepth < contact.PenetrationDepth)
{
contact.Normal = candidateNormal;
contact.PenetrationDepth = candidateDepth;
}
}
}
MPRToolbox.RefinePenetration(convex, triangle, ref Toolbox.RigidIdentity, contact.PenetrationDepth,
ref contact.Normal, out contact.PenetrationDepth, out contact.Normal, out contact.Position);
//It's possible for the normal to still face the 'wrong' direction according to one sided triangles.
if (triangle.sidedness != TriangleSidedness.DoubleSided)
{
Vector3.Dot(ref triangleNormal, ref contact.Normal, out dot);
if (dot < 0)
//Skip the add process.
{
goto InnerSphere;
}
}
contact.Id = -1;
if (contact.PenetrationDepth < convex.collisionMargin + triangle.collisionMargin)
{
state = CollisionState
.ExternalNear; //If it's emerged from the deep contact, we can go back to using the preferred GJK method.
}
contactList.Add(ref contact);
}
InnerSphere:
if (TryInnerSphereContact(triangle, out contact))
{
contactList.Add(ref contact);
}
if (contactList.Count > 0)
{
return(true);
}
state = CollisionState.ExternalSeparated;
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>
/// Constructs a new demo.
/// </summary>
/// <param name="game">Game owning this demo.</param>
public MPRTestDemo(DemosGame game)
: base(game)
{
var shapeA = new BoxShape(1, 1, 1);
shapeA.CollisionMargin = 0;
var shapeB = new BoxShape(1, 1, 1);
shapeB.CollisionMargin = 0;
var transformA = new RigidTransform(new Vector3(0, 0, 0));
var transformB = new RigidTransform(new Vector3(.5m, .5m, 0));
Vector3 overlap;
bool overlapped = MPRToolbox.GetLocalOverlapPosition(shapeA, shapeB, ref transformB, out overlap);
Vector3 normal;
Fix64 depth;
Vector3 direction = new Vector3(0, -1, 0);
MPRToolbox.LocalSurfaceCast(shapeA, shapeB, ref transformB, ref direction, out depth, out normal);
ContactData contactData;
//bool overlappedOld = MPRToolboxOld.AreObjectsColliding(shapeA, shapeB, ref transformA, ref transformB, out contactData);
//Random rand = new Random(0);
//for (int i = 0; i < 10000000; i++)
//{
// transformA = new RigidTransform(new Vector3((Fix64)rand.NextDouble() * 10 - 5, (Fix64)rand.NextDouble() * 10 - 5, (Fix64)rand.NextDouble() * 10 - 5),
// Quaternion.CreateFromYawPitchRoll((Fix64)rand.NextDouble() * 1000, (Fix64)rand.NextDouble() * 1000, (Fix64)rand.NextDouble() * 1000));
// transformB = new RigidTransform(new Vector3((Fix64)rand.NextDouble() * 10 - 5, (Fix64)rand.NextDouble() * 10 - 5, (Fix64)rand.NextDouble() * 10 - 5),
// Quaternion.CreateFromYawPitchRoll((Fix64)rand.NextDouble() * 1000, (Fix64)rand.NextDouble() * 1000, (Fix64)rand.NextDouble() * 1000));
// overlapped = MPRTesting.GetOverlapPosition(shapeA, shapeB, ref transformA, ref transformB, out overlap);
// overlappedOld = MPRToolbox.AreObjectsColliding(shapeA, shapeB, ref transformA, ref transformB, out contactData);
// if (overlapped && !overlappedOld &&
// (!MPRToolbox.IsPointInsideShape(ref overlap, shapeA, ref transformA) ||
// !MPRToolbox.IsPointInsideShape(ref overlap, shapeB, ref transformB)))
// Debug.WriteLine("Break.");
// if (overlappedOld && !overlapped &&
// (!MPRToolbox.IsPointInsideShape(ref contactData.Position, shapeA, ref transformA) ||
// !MPRToolbox.IsPointInsideShape(ref contactData.Position, shapeB, ref transformB)))
// Debug.WriteLine("Break.");
// if (overlapped && overlappedOld &&
// (!MPRToolbox.IsPointInsideShape(ref overlap, shapeA, ref transformA) ||
// !MPRToolbox.IsPointInsideShape(ref overlap, shapeB, ref transformB) ||
// !MPRToolbox.IsPointInsideShape(ref contactData.Position, shapeA, ref transformA) ||
// !MPRToolbox.IsPointInsideShape(ref contactData.Position, shapeB, ref transformB)))
// Debug.WriteLine("Break.");
//}
//Do these tests with rotationally immobile objects.
CollisionDetectionSettings.DefaultMargin = 0;
groundWidth = 10;
groundHeight = .1m;
groundLength = 10;
//a = new Box(new Vector3(0, -5, 0), groundWidth, groundHeight, groundLength, 1);
//a = new TransformableEntity(new Vector3(0,0,0), new TriangleShape(new Vector3(-5, -5, -5), new Vector3(5, -5, -5), new Vector3(-5, -5, 5)), Matrix3x3.Identity);
a = new Triangle(new Vector3(0, -5, 0), new Vector3(5, -5, 0), new Vector3(5, -5, 5), 1);
Space.Add(a);
Space.ForceUpdater.Gravity = new Vector3();
boxWidth = .25m;
boxHeight = .05m;
boxLength = 1;
b = new TransformableEntity(new Vector3(0, 2, 0), new BoxShape(boxWidth, boxHeight, boxLength), Matrix3x3.Identity, 1);
//b = new Cone(new Vector3(0, 2, 0), .2m, .1m, 1);
//b = new Capsule(new Vector3(0, 2, 0), 1, .5m, 1);
//b = new Capsule(new Vector3(0, 2, 0), 1, .5m, 1);
b.LocalInertiaTensorInverse = new Matrix3x3();
CollisionRules.AddRule(b, a, CollisionRule.NoSolver);
b.ActivityInformation.IsAlwaysActive = true;
Space.Add(b);
//Space.Add(new TransformableEntity(new Vector3(0, 4, 0), new BoxShape(1, 1, 1), Matrix3x3.Identity, 1));
//Space.Add( new TransformableEntity(new Vector3(0, 6, 0), new BoxShape(1, 1, 1), Matrix3x3.Identity, 1));
//Vector3[] vertices = new Vector3[] { new Vector3(0, -5, 0), new Vector3(5, -5, 0), new Vector3(5, -5, 5), new Vector3(0, -60, 5) };
//int[] indices = new int[] { 0, 1, 2 , 0, 2, 3 };
//StaticMesh mesh = new StaticMesh(vertices, indices);
//Space.Add(mesh);
//mesh.ImproveBoundaryBehavior = true;
//mesh.Sidedness = TriangleSidedness.Counterclockwise;
//game.ModelDrawer.Add(mesh);
//mesh.CollisionRules.Personal = CollisionRule.NoSolver;
}
/// <summary>
/// Constructs a new demo.
/// </summary>
/// <param name="game">Game owning this demo.</param>
public BooleanConvexTestDemo(DemosGame game)
: base(game)
{
var random = new Random();
int numberOfConfigurations = 1000;
int numberOfTestsPerConfiguration = 10000;
float size = 2;
var aPositionBounds = new BoundingBox(new Vector3(-size, -size, -size), new Vector3(size, size, size));
var bPositionBounds = new BoundingBox(new Vector3(-size, -size, -size), new Vector3(size, size, size));
size = 1;
var aShapeBounds = new BoundingBox(new Vector3(-size, -size, -size), new Vector3(size, size, size));
var bShapeBounds = new BoundingBox(new Vector3(-size, -size, -size), new Vector3(size, size, size));
int pointsInA = 10;
int pointsInB = 10;
RawList <Vector3> points = new RawList <Vector3>();
long accumulatedMPR = 0;
long accumulatedGJK = 0;
long accumulatedGJKSeparatingAxis = 0;
for (int i = 0; i < numberOfConfigurations; i++)
{
//Create two convex hull shapes.
for (int j = 0; j < pointsInA; j++)
{
Vector3 point;
GetRandomPointInBoundingBox(random, ref aShapeBounds, out point);
points.Add(point);
}
var a = new ConvexHullShape(points);
points.Clear();
for (int j = 0; j < pointsInB; j++)
{
Vector3 point;
GetRandomPointInBoundingBox(random, ref bShapeBounds, out point);
points.Add(point);
}
var b = new ConvexHullShape(points);
points.Clear();
//Generate some random tranforms for the shapes.
RigidTransform aTransform;
var axis = Vector3.Normalize(new Vector3((float)((random.NextDouble() - .5f) * 2), (float)((random.NextDouble() - .5f) * 2), (float)((random.NextDouble() - .5f) * 2)));
var angle = (float)random.NextDouble() * MathHelper.TwoPi;
Quaternion.CreateFromAxisAngle(ref axis, angle, out aTransform.Orientation);
GetRandomPointInBoundingBox(random, ref aPositionBounds, out aTransform.Position);
RigidTransform bTransform;
axis = Vector3.Normalize(new Vector3((float)((random.NextDouble() - .5f) * 2), (float)((random.NextDouble() - .5f) * 2), (float)((random.NextDouble() - .5f) * 2)));
angle = (float)random.NextDouble() * MathHelper.TwoPi;
Quaternion.CreateFromAxisAngle(ref axis, angle, out bTransform.Orientation);
GetRandomPointInBoundingBox(random, ref bPositionBounds, out bTransform.Position);
//Perform MPR tests.
//Warm up the cache a bit.
MPRToolbox.AreShapesOverlapping(a, b, ref aTransform, ref bTransform);
long start = Stopwatch.GetTimestamp();
for (int j = 0; j < numberOfTestsPerConfiguration; j++)
{
if (MPRToolbox.AreShapesOverlapping(a, b, ref aTransform, ref bTransform))
{
overlapsMPR++;
}
}
long end = Stopwatch.GetTimestamp();
accumulatedMPR += end - start;
//Perform GJK tests.
//Warm up the cache a bit.
GJKToolbox.AreShapesIntersecting(a, b, ref aTransform, ref bTransform);
start = Stopwatch.GetTimestamp();
for (int j = 0; j < numberOfTestsPerConfiguration; j++)
{
if (GJKToolbox.AreShapesIntersecting(a, b, ref aTransform, ref bTransform))
{
overlapsGJK++;
}
}
end = Stopwatch.GetTimestamp();
accumulatedGJK += end - start;
//Perform GJK Separating Axis tests.
//Warm up the cache a bit.
Vector3 localSeparatingAxis = Vector3.Up;
GJKToolbox.AreShapesIntersecting(a, b, ref aTransform, ref bTransform, ref localSeparatingAxis);
start = Stopwatch.GetTimestamp();
for (int j = 0; j < numberOfTestsPerConfiguration; j++)
{
if (GJKToolbox.AreShapesIntersecting(a, b, ref aTransform, ref bTransform, ref localSeparatingAxis))
{
overlapsGJKSeparatingAxis++;
}
}
end = Stopwatch.GetTimestamp();
accumulatedGJKSeparatingAxis += end - start;
}
//Compute the actual time per test.
long denominator = Stopwatch.Frequency * numberOfConfigurations * numberOfTestsPerConfiguration;
timeMPR = (double)accumulatedMPR / denominator;
timeGJK = (double)accumulatedGJK / denominator;
timeGJKSeparatingAxis = (double)accumulatedGJKSeparatingAxis / denominator;
}
public static void Test()
{
var f0 = BuildHull();
f0.CollisionMargin = 0;
//Generate spheres all around the central froxel in such a way that we know that they're not colliding.
var froxelSphereSurface = new BoundingBox(new Vector3(-1.51f, -1.51f, -1.51f), new Vector3(1.51f, 1.51f, 1.51f));
int testIterations = 1000;
int innerIterations = 1000;
Random random = new Random(5);
long sphereFroxelSeparatedTicks = 0;
SphereShape sphere = new SphereShape(1);
for (int i = 0; i < testIterations; ++i)
{
var ray = GetRandomRay(ref froxelSphereSurface, random);
float t;
ray.Intersects(ref froxelSphereSurface, out t);
var sphereTransform = new RigidTransform {
Position = ray.Position + ray.Direction * t, Orientation = Quaternion.Identity
};
var start = Stopwatch.GetTimestamp();
for (int j = 0; j < innerIterations; ++j)
{
if (MPRToolbox.AreLocalShapesOverlapping(f0, sphere, ref sphereTransform))
{
Trace.Fail("By construction there can be no intersection!");
}
}
var end = Stopwatch.GetTimestamp();
sphereFroxelSeparatedTicks += (end - start);
}
Console.WriteLine($"Sphere-froxel separated: {(1e6 * sphereFroxelSeparatedTicks) / (testIterations * innerIterations * Stopwatch.Frequency)}");
//Do the same kind of test, but now with intersection.
froxelSphereSurface = new BoundingBox(new Vector3(-0.5f, -0.5f, -0.5f), new Vector3(0.5f, 0.5f, 0.5f));
long sphereFroxelIntersectingTicks = 0;
for (int i = 0; i < testIterations; ++i)
{
var ray = GetRandomRay(ref froxelSphereSurface, random);
float t;
ray.Intersects(ref froxelSphereSurface, out t);
var sphereTransform = new RigidTransform {
Position = ray.Position + ray.Direction * (t - 0.99f), Orientation = Quaternion.Identity
};
var start = Stopwatch.GetTimestamp();
for (int j = 0; j < innerIterations; ++j)
{
if (!MPRToolbox.AreLocalShapesOverlapping(f0, sphere, ref sphereTransform))
{
Trace.Fail("By construction there can be no separation!");
}
}
var end = Stopwatch.GetTimestamp();
sphereFroxelIntersectingTicks += (end - start);
}
Console.WriteLine($"Sphere-froxel intersecting: {(1e6 * sphereFroxelIntersectingTicks) / (testIterations * innerIterations * Stopwatch.Frequency)}");
//Create a surface for the rays to hit such that every query froxel will be just outside of the central froxel.
var froxelFroxelSurface = new BoundingBox(new Vector3(-1.01f, -1.01f, -1.01f), new Vector3(1.01f, 1.01f, 1.01f));
var queryHull = BuildHull();
queryHull.CollisionMargin = 0;
long froxelFroxelSeparatedTicks = 0;
for (int i = 0; i < testIterations; ++i)
{
var ray = GetRandomRay(ref froxelFroxelSurface, random);
float t;
ray.Intersects(ref froxelFroxelSurface, out t);
var queryTransform = new RigidTransform(ray.Position + ray.Direction * t);
var start = Stopwatch.GetTimestamp();
for (int j = 0; j < innerIterations; ++j)
{
if (MPRToolbox.AreLocalShapesOverlapping(f0, queryHull, ref queryTransform))
{
Trace.Fail("By construction there can be no intersection!");
}
}
var end = Stopwatch.GetTimestamp();
froxelFroxelSeparatedTicks += (end - start);
}
Console.WriteLine($"Froxel-froxel separated: {(1e6 * froxelFroxelSeparatedTicks) / (testIterations * innerIterations * Stopwatch.Frequency)}");
//Same thing as above, but now with slight intersection.
froxelFroxelSurface = new BoundingBox(new Vector3(-.99f, -.99f, -.99f), new Vector3(0.99f, 0.99f, 0.99f));
long froxelFroxelIntersectingTicks = 0;
for (int i = 0; i < testIterations; ++i)
{
var ray = GetRandomRay(ref froxelFroxelSurface, random);
float t;
ray.Intersects(ref froxelFroxelSurface, out t);
var queryTransform = new RigidTransform(ray.Position + ray.Direction * t);
var start = Stopwatch.GetTimestamp();
for (int j = 0; j < innerIterations; ++j)
{
if (!MPRToolbox.AreLocalShapesOverlapping(f0, queryHull, ref queryTransform))
{
Trace.Fail("By construction there can be no separation!");
}
}
var end = Stopwatch.GetTimestamp();
froxelFroxelIntersectingTicks += (end - start);
}
Console.WriteLine($"Froxel-froxel intersecting: {(1e6 * froxelFroxelIntersectingTicks) / (testIterations * innerIterations * Stopwatch.Frequency)}");
}
private bool TryInnerSphereContact(out ContactData contact)
{
Vector3 closestPoint;
Toolbox.GetClosestPointOnTriangleToPoint(ref triangle.vA, ref triangle.vB, ref triangle.vC, ref Toolbox.ZeroVector, out closestPoint);
float length = closestPoint.LengthSquared();
float minimumRadius = convex.minimumRadius * (MotionSettings.CoreShapeScaling + .01f);
if (length < minimumRadius * minimumRadius)
{
Vector3 triangleNormal, ab, ac;
Vector3.Subtract(ref triangle.vB, ref triangle.vA, out ab);
Vector3.Subtract(ref triangle.vC, ref triangle.vA, out ac);
Vector3.Cross(ref ab, ref ac, out triangleNormal);
float dot;
Vector3.Dot(ref closestPoint, ref triangleNormal, out dot);
if ((triangle.sidedness == TriangleSidedness.Clockwise && dot > 0) || (triangle.sidedness == TriangleSidedness.Counterclockwise && dot < 0))
{
//Normal was facing the wrong way.
contact = new ContactData();
return(false);
}
length = (float)Math.Sqrt(length);
contact.Position = closestPoint;
if (length > Toolbox.Epsilon) //Watch out for NaN's!
{
Vector3.Divide(ref closestPoint, length, out contact.Normal);
}
else
{
//The direction is undefined. Use the triangle's normal.
//One sided triangles can only face in the appropriate direction.
float normalLength = triangleNormal.LengthSquared();
if (triangleNormal.LengthSquared() > Toolbox.Epsilon)
{
Vector3.Divide(ref triangleNormal, (float)Math.Sqrt(normalLength), out triangleNormal);
if (triangle.sidedness == TriangleSidedness.Clockwise)
{
contact.Normal = triangleNormal;
}
else
{
Vector3.Negate(ref triangleNormal, out contact.Normal);
}
}
else
{
//Degenerate triangle!
contact = new ContactData();
return(false);
}
}
//Compute the actual depth of the contact.
MPRToolbox.LocalSurfaceCast(convex, triangle, ref Toolbox.RigidIdentity, ref contact.Normal, out contact.PenetrationDepth, out triangleNormal); //Trash the 'corrected' normal. We want to use the spherical normal.
contact.Id = -1;
return(true);
}
contact = new ContactData();
return(false);
}
public override bool ConvexCast(ConvexShape castShape, ref MathExtensions.RigidTransform startingTransform, ref Vector3 sweep, out RayHit hit)
{
return(MPRToolbox.Sweep(castShape, Shape, ref sweep, ref Toolbox.ZeroVector, ref startingTransform, ref worldTransform, out hit));
}
private bool DoDeepContact(out TinyStructList <ContactData> contactList)
{
//Find the origin to triangle center offset.
Vector3 center;
Vector3.Add(ref triangle.vA, ref triangle.vB, out center);
Vector3.Add(ref center, ref triangle.vC, out center);
Vector3.Multiply(ref center, 1f / 3f, out center);
ContactData contact;
contactList = new TinyStructList <ContactData>();
if (MPRToolbox.AreLocalShapesOverlapping(convex, triangle, ref center, ref Toolbox.RigidIdentity))
{
float dot;
Vector3 triangleNormal, ab, ac;
Vector3.Subtract(ref triangle.vB, ref triangle.vA, out ab);
Vector3.Subtract(ref triangle.vC, ref triangle.vA, out ac);
Vector3.Cross(ref ab, ref ac, out triangleNormal);
float lengthSquared = triangleNormal.LengthSquared();
if (lengthSquared < Toolbox.Epsilon * .01f)
{
//Degenerate triangle! That's no good.
//Just use the direction pointing from A to B, "B" being the triangle. That direction is center - origin, or just center.
MPRToolbox.LocalSurfaceCast(convex, triangle, ref Toolbox.RigidIdentity, ref center, out contact.PenetrationDepth, out contact.Normal, out contact.Position);
}
else
{
//Normalize the normal.
Vector3.Divide(ref triangleNormal, (float)Math.Sqrt(lengthSquared), out triangleNormal);
////The first direction to check is one of the triangle's edge normals. Choose the one that is most aligned with the offset from A to B.
////Project the direction onto the triangle plane.
//Vector3.Dot(ref triangleNormal, ref center, out dot);
//Vector3 trianglePlaneDirection;
//Vector3.Multiply(ref triangleNormal, dot, out trianglePlaneDirection);
//Vector3.Subtract(ref trianglePlaneDirection, ref center, out trianglePlaneDirection);
////To find out which edge to use, compute which region the direction is in.
////This is done by constructing three planes which segment the triangle into three sub-triangles.
////These planes are defined by A, origin, center; B, origin, center; C, origin, center.
////The plane tests against the direction can be reordered to:
////(center x direction) * A
////(center x direction) * B
////(center x direction) * C
//Vector3 OxD;
//Vector3.Cross(ref trianglePlaneDirection, ref center, out OxD);
//Vector3 p;
//float dotA, dotB, dotC;
//Vector3.Dot(ref triangle.vA, ref OxD, out dotA);
//Vector3.Dot(ref triangle.vB, ref OxD, out dotB);
//Vector3.Dot(ref triangle.vC, ref OxD, out dotC);
//if (dotA >= 0 && dotB <= 0)
//{
// //Direction is in the AB edge zone.
// //Compute the edge normal using AB x (AO x AB).
// Vector3 AB, AO;
// Vector3.Subtract(ref triangle.vB, ref triangle.vA, out AB);
// Vector3.Subtract(ref center, ref triangle.vA, out AO);
// Vector3.Cross(ref AO, ref AB, out p);
// Vector3.Cross(ref AB, ref p, out trianglePlaneDirection);
//}
//else if (dotB >= 0 && dotC <= 0)
//{
// //Direction is in the BC edge zone.
// //Compute the edge normal using BC x (BO x BC).
// Vector3 BC, BO;
// Vector3.Subtract(ref triangle.vC, ref triangle.vB, out BC);
// Vector3.Subtract(ref center, ref triangle.vB, out BO);
// Vector3.Cross(ref BO, ref BC, out p);
// Vector3.Cross(ref BC, ref p, out trianglePlaneDirection);
//}
//else // dotC > 0 && dotA < 0
//{
// //Direction is in the CA edge zone.
// //Compute the edge normal using CA x (CO x CA).
// Vector3 CA, CO;
// Vector3.Subtract(ref triangle.vA, ref triangle.vC, out CA);
// Vector3.Subtract(ref center, ref triangle.vC, out CO);
// Vector3.Cross(ref CO, ref CA, out p);
// Vector3.Cross(ref CA, ref p, out trianglePlaneDirection);
//}
//dot = trianglePlaneDirection.LengthSquared();
//if (dot > Toolbox.Epsilon)
//{
// Vector3.Divide(ref trianglePlaneDirection, (float)Math.Sqrt(dot), out trianglePlaneDirection);
// MPRTesting.LocalSurfaceCast(convex, triangle, ref Toolbox.RigidIdentity, ref trianglePlaneDirection, out contact.PenetrationDepth, out contact.Normal);
// //Check to see if the normal is facing in the proper direction, considering that this may not be a two-sided triangle.
// Vector3.Dot(ref triangleNormal, ref contact.Normal, out dot);
// if ((triangle.sidedness == TriangleSidedness.Clockwise && dot > 0) || (triangle.sidedness == TriangleSidedness.Counterclockwise && dot < 0))
// {
// //Normal was facing the wrong way.
// //Instead of ignoring it entirely, correct the direction to as close as it can get by removing any component parallel to the triangle normal.
// Vector3 previousNormal = contact.Normal;
// Vector3.Dot(ref contact.Normal, ref triangleNormal, out dot);
// Vector3.Multiply(ref contact.Normal, dot, out p);
// Vector3.Subtract(ref contact.Normal, ref p, out contact.Normal);
// float length = contact.Normal.LengthSquared();
// if (length > Toolbox.Epsilon)
// {
// //Renormalize the corrected normal.
// Vector3.Divide(ref contact.Normal, (float)Math.Sqrt(length), out contact.Normal);
// Vector3.Dot(ref contact.Normal, ref previousNormal, out dot);
// contact.PenetrationDepth *= dot;
// }
// else
// {
// contact.PenetrationDepth = float.MaxValue;
// contact.Normal = new Vector3();
// }
// }
//}
//else
//{
// contact.PenetrationDepth = float.MaxValue;
// contact.Normal = new Vector3();
//}
//TODO: This tests all three edge axes with a full MPR raycast. That's not really necessary; the correct edge normal should be discoverable, resulting in a single MPR raycast.
//Find the edge directions that will be tested with MPR.
Vector3 AO, BO, CO;
Vector3 AB, BC, CA;
Vector3.Subtract(ref center, ref triangle.vA, out AO);
Vector3.Subtract(ref center, ref triangle.vB, out BO);
Vector3.Subtract(ref center, ref triangle.vC, out CO);
Vector3.Subtract(ref triangle.vB, ref triangle.vA, out AB);
Vector3.Subtract(ref triangle.vC, ref triangle.vB, out BC);
Vector3.Subtract(ref triangle.vA, ref triangle.vC, out CA);
//We don't have to worry about degenerate triangles here because we've already handled that possibility above.
Vector3 ABnormal, BCnormal, CAnormal;
//Project the center onto the edge to find the direction from the center to the edge AB.
Vector3.Dot(ref AO, ref AB, out dot);
Vector3.Multiply(ref AB, dot / AB.LengthSquared(), out ABnormal);
Vector3.Subtract(ref AO, ref ABnormal, out ABnormal);
ABnormal.Normalize();
//Project the center onto the edge to find the direction from the center to the edge BC.
Vector3.Dot(ref BO, ref BC, out dot);
Vector3.Multiply(ref BC, dot / BC.LengthSquared(), out BCnormal);
Vector3.Subtract(ref BO, ref BCnormal, out BCnormal);
BCnormal.Normalize();
//Project the center onto the edge to find the direction from the center to the edge BC.
Vector3.Dot(ref CO, ref CA, out dot);
Vector3.Multiply(ref CA, dot / CA.LengthSquared(), out CAnormal);
Vector3.Subtract(ref CO, ref CAnormal, out CAnormal);
CAnormal.Normalize();
MPRToolbox.LocalSurfaceCast(convex, triangle, ref Toolbox.RigidIdentity, ref ABnormal, out contact.PenetrationDepth, out contact.Normal);
//Check to see if the normal is facing in the proper direction, considering that this may not be a two-sided triangle.
Vector3.Dot(ref triangleNormal, ref contact.Normal, out dot);
if ((triangle.sidedness == TriangleSidedness.Clockwise && dot > 0) || (triangle.sidedness == TriangleSidedness.Counterclockwise && dot < 0))
{
//Normal was facing the wrong way.
//Instead of ignoring it entirely, correct the direction to as close as it can get by removing any component parallel to the triangle normal.
Vector3 previousNormal = contact.Normal;
Vector3.Dot(ref contact.Normal, ref triangleNormal, out dot);
Vector3 p;
Vector3.Multiply(ref contact.Normal, dot, out p);
Vector3.Subtract(ref contact.Normal, ref p, out contact.Normal);
float length = contact.Normal.LengthSquared();
if (length > Toolbox.Epsilon)
{
//Renormalize the corrected normal.
Vector3.Divide(ref contact.Normal, (float)Math.Sqrt(length), out contact.Normal);
Vector3.Dot(ref contact.Normal, ref previousNormal, out dot);
contact.PenetrationDepth *= dot;
}
else
{
contact.PenetrationDepth = float.MaxValue;
contact.Normal = new Vector3();
}
}
Vector3 candidateNormal;
float candidateDepth;
MPRToolbox.LocalSurfaceCast(convex, triangle, ref Toolbox.RigidIdentity, ref BCnormal, out candidateDepth, out candidateNormal);
//Check to see if the normal is facing in the proper direction, considering that this may not be a two-sided triangle.
Vector3.Dot(ref triangleNormal, ref candidateNormal, out dot);
if ((triangle.sidedness == TriangleSidedness.Clockwise && dot > 0) || (triangle.sidedness == TriangleSidedness.Counterclockwise && dot < 0))
{
//Normal was facing the wrong way.
//Instead of ignoring it entirely, correct the direction to as close as it can get by removing any component parallel to the triangle normal.
Vector3 previousNormal = candidateNormal;
Vector3.Dot(ref candidateNormal, ref triangleNormal, out dot);
Vector3 p;
Vector3.Multiply(ref candidateNormal, dot, out p);
Vector3.Subtract(ref candidateNormal, ref p, out candidateNormal);
float length = candidateNormal.LengthSquared();
if (length > Toolbox.Epsilon)
{
//Renormalize the corrected normal.
Vector3.Divide(ref candidateNormal, (float)Math.Sqrt(length), out candidateNormal);
Vector3.Dot(ref candidateNormal, ref previousNormal, out dot);
candidateDepth *= dot;
}
else
{
contact.PenetrationDepth = float.MaxValue;
contact.Normal = new Vector3();
}
}
if (candidateDepth < contact.PenetrationDepth)
{
contact.Normal = candidateNormal;
contact.PenetrationDepth = candidateDepth;
}
MPRToolbox.LocalSurfaceCast(convex, triangle, ref Toolbox.RigidIdentity, ref CAnormal, out candidateDepth, out candidateNormal);
//Check to see if the normal is facing in the proper direction, considering that this may not be a two-sided triangle.
Vector3.Dot(ref triangleNormal, ref candidateNormal, out dot);
if ((triangle.sidedness == TriangleSidedness.Clockwise && dot > 0) || (triangle.sidedness == TriangleSidedness.Counterclockwise && dot < 0))
{
//Normal was facing the wrong way.
//Instead of ignoring it entirely, correct the direction to as close as it can get by removing any component parallel to the triangle normal.
Vector3 previousNormal = candidateNormal;
Vector3.Dot(ref candidateNormal, ref triangleNormal, out dot);
Vector3 p;
Vector3.Multiply(ref candidateNormal, dot, out p);
Vector3.Subtract(ref candidateNormal, ref p, out candidateNormal);
float length = candidateNormal.LengthSquared();
if (length > Toolbox.Epsilon)
{
//Renormalize the corrected normal.
Vector3.Divide(ref candidateNormal, (float)Math.Sqrt(length), out candidateNormal);
Vector3.Dot(ref candidateNormal, ref previousNormal, out dot);
candidateDepth *= dot;
}
else
{
contact.PenetrationDepth = float.MaxValue;
contact.Normal = new Vector3();
}
}
if (candidateDepth < contact.PenetrationDepth)
{
contact.Normal = candidateNormal;
contact.PenetrationDepth = candidateDepth;
}
//Try the depth along the positive triangle normal.
//If it's clockwise, this direction is unnecessary (the resulting normal would be invalidated by the onesidedness of the triangle).
if (triangle.sidedness != TriangleSidedness.Clockwise)
{
MPRToolbox.LocalSurfaceCast(convex, triangle, ref Toolbox.RigidIdentity, ref triangleNormal, out candidateDepth, out candidateNormal);
if (candidateDepth < contact.PenetrationDepth)
{
contact.Normal = candidateNormal;
contact.PenetrationDepth = candidateDepth;
}
}
//Try the depth along the negative triangle normal.
//If it's counterclockwise, this direction is unnecessary (the resulting normal would be invalidated by the onesidedness of the triangle).
if (triangle.sidedness != TriangleSidedness.Counterclockwise)
{
Vector3.Negate(ref triangleNormal, out triangleNormal);
MPRToolbox.LocalSurfaceCast(convex, triangle, ref Toolbox.RigidIdentity, ref triangleNormal, out candidateDepth, out candidateNormal);
if (candidateDepth < contact.PenetrationDepth)
{
contact.Normal = candidateNormal;
contact.PenetrationDepth = candidateDepth;
}
}
}
MPRToolbox.RefinePenetration(convex, triangle, ref Toolbox.RigidIdentity, contact.PenetrationDepth, ref contact.Normal, out contact.PenetrationDepth, out contact.Normal, out contact.Position);
//It's possible for the normal to still face the 'wrong' direction according to one sided triangles.
if (triangle.sidedness != TriangleSidedness.DoubleSided)
{
Vector3.Dot(ref triangleNormal, ref contact.Normal, out dot);
if (dot < 0)
{
return(false);
}
}
////The local casting can optionally continue. Eventually, it will converge to the local minimum.
//int optimizingCount = 0;
//while (true)
//{
// MPRTesting.LocalSurfaceCast(convex, triangle, ref Toolbox.RigidIdentity, ref contact.Normal, out candidateDepth, out candidateNormal);
// if (contact.PenetrationDepth - candidateDepth <= Toolbox.BigEpsilon ||
// ++optimizingCount < 4)
// {
// //If we've reached the end due to convergence, the normal will be extremely close to correct (if not 100% correct).
// //The candidateDepth computed is the previous contact normal's depth.
// //The reason why the previous normal is kept is that the last raycast computed the depth for that normal, not the new normal.
// contact.PenetrationDepth = candidateDepth;
// break;
// }
// contact.PenetrationDepth = candidateDepth;
// contact.Normal = candidateNormal;
//}
//Correct the penetration depth.
//MPRTesting.LocalSurfaceCast(convex, triangle, ref Toolbox.RigidIdentity, ref contact.Normal, out contact.PenetrationDepth, out center); //Center is just a trash variable now.
contact.Id = -1;
if (contact.PenetrationDepth < convex.collisionMargin + triangle.collisionMargin)
{
state = CollisionState.ExternalNear; //If it's emerged from the deep contact, we can go back to using the preferred GJK method.
}
contactList.Add(ref contact);
}
if (TryInnerSphereContact(out contact))
{
contactList.Add(ref contact);
}
if (contactList.count > 0)
{
return(true);
}
state = CollisionState.ExternalSeparated;
return(false);
}
public override void Update(Fix64 dt)
{
if (Game.KeyboardInput.IsKeyDown(Keys.Left))
{
rayCastDirection = Matrix3x3.Transform(rayCastDirection, Matrix3x3.CreateFromAxisAngle(Vector3.Forward, .01m));
}
if (Game.KeyboardInput.IsKeyDown(Keys.Right))
{
rayCastDirection = Matrix3x3.Transform(rayCastDirection, Matrix3x3.CreateFromAxisAngle(Vector3.Forward, -.01m));
}
if (Game.KeyboardInput.IsKeyDown(Keys.Down))
{
rayCastDirection = Matrix3x3.Transform(rayCastDirection, Matrix3x3.CreateFromAxisAngle(Vector3.Right, .01m));
}
if (Game.KeyboardInput.IsKeyDown(Keys.Up))
{
rayCastDirection = Matrix3x3.Transform(rayCastDirection, Matrix3x3.CreateFromAxisAngle(Vector3.Right, -.01m));
}
if (Game.KeyboardInput.IsKeyDown(Keys.P))
{
Debug.WriteLine("Break.");
}
base.Update(dt);
RigidTransform localTransformB;
RigidTransform aTransform = a.CollisionInformation.WorldTransform, bTransform = b.CollisionInformation.WorldTransform;
MinkowskiToolbox.GetLocalTransform(ref aTransform, ref bTransform, out localTransformB);
Vector3 position;
if (MPRToolbox.GetLocalOverlapPosition((a.CollisionInformation.Shape as ConvexShape), (b.CollisionInformation.Shape as ConvexShape), ref localTransformB, out position))
{
//Vector3 rayCastDirection = new Vector3(1,0,0);// (Vector3.Normalize(localDirection) + Vector3.Normalize(collidableB.worldTransform.Position - collidableA.worldTransform.Position)) / 2;
Fix64 previousT;
Vector3 previousNormal;
Fix64 t;
Vector3 normal;
rayCastDirection = localTransformB.Position;
MPRToolbox.LocalSurfaceCast((a.CollisionInformation.Shape as ConvexShape), (b.CollisionInformation.Shape as ConvexShape), ref localTransformB, ref rayCastDirection, out previousT, out previousNormal);
//Vector3 secondDirection = Vector3.Cross(rayCastDirection, Vector3.Up);
//MPRTesting.LocalSurfaceCast((a.CollisionInformation.Shape as ConvexShape), (b.CollisionInformation.Shape as ConvexShape), ref localTransformB, ref secondDirection, out t, out normal);
//if (t < previousT)
//{
// previousNormal = normal;
// previousT = t;
//}
//Vector3 thirdDirection = Vector3.Cross(secondDirection, rayCastDirection);
//MPRTesting.LocalSurfaceCast((a.CollisionInformation.Shape as ConvexShape), (b.CollisionInformation.Shape as ConvexShape), ref localTransformB, ref thirdDirection, out t, out normal);
//if (t < previousT)
//{
// previousNormal = normal;
// previousT = t;
//}
//Vector3 fourthDirection = -secondDirection;
//MPRTesting.LocalSurfaceCast((a.CollisionInformation.Shape as ConvexShape), (b.CollisionInformation.Shape as ConvexShape), ref localTransformB, ref fourthDirection, out t, out normal);
//if (t < previousT)
//{
// previousNormal = normal;
// previousT = t;
//}
//Vector3 fifthDirection = -thirdDirection;
//MPRTesting.LocalSurfaceCast((a.CollisionInformation.Shape as ConvexShape), (b.CollisionInformation.Shape as ConvexShape), ref localTransformB, ref fifthDirection, out t, out normal);
//if (t < previousT)
//{
// previousNormal = normal;
// previousT = t;
//}
//Correct the penetration depth.
MPRToolbox.LocalSurfaceCast((a.CollisionInformation.Shape as ConvexShape), (b.CollisionInformation.Shape as ConvexShape), ref localTransformB, ref previousNormal, out t, out normal);
contactDepth = t;
contactNormal = previousNormal;
////Converge to local minimum.
//while (true)
//{
// MPRTesting.LocalSurfaceCast((a.CollisionInformation.Shape as ConvexShape), (b.CollisionInformation.Shape as ConvexShape), ref localTransformB, ref previousNormal, out t, out normal);
// if (previousT - t <= Toolbox.BigEpsilon)
// break;
// previousT = t;
// previousNormal = normal;
//}
}
#region Box Box minkowski sum
////Construct explicit minkowski sum.
//Vector3[] aLines = new Vector3[8];
//aLines[0] = new Vector3(-boxWidth / 2, -boxHeight / 2, -boxLength / 2);
//aLines[1] = new Vector3(-boxWidth / 2, -boxHeight / 2, boxLength / 2);
//aLines[2] = new Vector3(-boxWidth / 2, boxHeight / 2, -boxLength / 2);
//aLines[3] = new Vector3(-boxWidth / 2, boxHeight / 2, boxLength / 2);
//aLines[4] = new Vector3(boxWidth / 2, -boxHeight / 2, -boxLength / 2);
//aLines[5] = new Vector3(boxWidth / 2, -boxHeight / 2, boxLength / 2);
//aLines[6] = new Vector3(boxWidth / 2, boxHeight / 2, -boxLength / 2);
//aLines[7] = new Vector3(boxWidth / 2, boxHeight / 2, boxLength / 2);
//Vector3[] bLines = new Vector3[8];
//bLines[0] = new Vector3(-groundWidth / 2, -groundHeight / 2, -groundLength / 2);
//bLines[1] = new Vector3(-groundWidth / 2, -groundHeight / 2, groundLength / 2);
//bLines[2] = new Vector3(-groundWidth / 2, groundHeight / 2, -groundLength / 2);
//bLines[3] = new Vector3(-groundWidth / 2, groundHeight / 2, groundLength / 2);
//bLines[4] = new Vector3(groundWidth / 2, -groundHeight / 2, -groundLength / 2);
//bLines[5] = new Vector3(groundWidth / 2, -groundHeight / 2, groundLength / 2);
//bLines[6] = new Vector3(groundWidth / 2, groundHeight / 2, -groundLength / 2);
//bLines[7] = new Vector3(groundWidth / 2, groundHeight / 2, groundLength / 2);
//for (int i = 0; i < 8; i++)
// aLines[i] = Vector3.Transform(aLines[i], localTransformB.Matrix);
//List<Vector3> vertices = new List<Vector3>();
//for (int i = 0; i < 8; i++)
//{
// for (int j = 0; j < 8; j++)
// {
// if (b.CollisionInformation.Pairs.Count > 0)
// {
// if (b.CollisionInformation.Pairs[0].BroadPhaseOverlap.EntryA == b.CollisionInformation)
// vertices.Add(aLines[i] - bLines[j]);
// else
// vertices.Add(bLines[i] - aLines[j]);
// }
// else
// {
// vertices.Add(bLines[i] - aLines[j]);
// }
// }
//}
//var indices = new List<int>();
//Toolbox.GetConvexHull(vertices, indices);
#endregion
#region Arbitrary minkowski sum
var vertices = new List <Vector3>();
Vector3 max;
var direction = new Vector3();
int NumSamples = 16;
Fix64 angleChange = MathHelper.TwoPi / NumSamples;
for (int i = 1; i < NumSamples / 2 - 1; i++)
{
Fix64 phi = MathHelper.PiOver2 - i * angleChange;
var sinPhi = Fix64.Sin(phi);
var cosPhi = Fix64.Cos(phi);
for (int j = 0; j < NumSamples; j++)
{
Fix64 theta = j * angleChange;
direction.X = Fix64.Cos(theta) * cosPhi;
direction.Y = sinPhi;
direction.Z = Fix64.Sin(theta) * cosPhi;
MinkowskiToolbox.GetLocalMinkowskiExtremePoint(a.CollisionInformation.Shape as ConvexShape, b.CollisionInformation.Shape as ConvexShape, ref direction, ref localTransformB, out max);
vertices.Add(max);
}
}
MinkowskiToolbox.GetLocalMinkowskiExtremePoint(a.CollisionInformation.Shape as ConvexShape, b.CollisionInformation.Shape as ConvexShape, ref Toolbox.UpVector, ref localTransformB, out max);
vertices.Add(max);
MinkowskiToolbox.GetLocalMinkowskiExtremePoint(a.CollisionInformation.Shape as ConvexShape, b.CollisionInformation.Shape as ConvexShape, ref Toolbox.DownVector, ref localTransformB, out max);
vertices.Add(max);
var indices = new List <int>();
ConvexHullHelper.GetConvexHull(vertices, indices);
#endregion
minkowskiLines.Clear();
for (int i = 0; i < indices.Count; i += 3)
{
minkowskiLines.Add(new VertexPositionColor(MathConverter.Convert(vertices[indices[i]]), Microsoft.Xna.Framework.Color.Blue));
minkowskiLines.Add(new VertexPositionColor(MathConverter.Convert(vertices[indices[i + 1]]), Microsoft.Xna.Framework.Color.Blue));
minkowskiLines.Add(new VertexPositionColor(MathConverter.Convert(vertices[indices[i + 1]]), Microsoft.Xna.Framework.Color.Blue));
minkowskiLines.Add(new VertexPositionColor(MathConverter.Convert(vertices[indices[i + 2]]), Microsoft.Xna.Framework.Color.Blue));
minkowskiLines.Add(new VertexPositionColor(MathConverter.Convert(vertices[indices[i + 2]]), Microsoft.Xna.Framework.Color.Blue));
minkowskiLines.Add(new VertexPositionColor(MathConverter.Convert(vertices[indices[i]]), Microsoft.Xna.Framework.Color.Blue));
}
}
private bool DoDeepContact(out ContactData contact)
{
#region Informed search
if (previousState == CollisionState.Separated) //If it was shallow before, then its closest points will be used to find the normal.
{
//It's overlapping! Find the relative velocity at the point relative to the two objects. The point is still in local space!
//Vector3 velocityA;
//Vector3.Cross(ref contact.Position, ref collidableA.entity.angularVelocity, out velocityA);
//Vector3.Add(ref velocityA, ref collidableA.entity.linearVelocity, out velocityA);
//Vector3 velocityB;
//Vector3.Subtract(ref contact.Position, ref localTransformB.Position, out velocityB);
//Vector3.Cross(ref velocityB, ref collidableB.entity.angularVelocity, out velocityB);
//Vector3.Add(ref velocityB, ref collidableB.entity.linearVelocity, out velocityB);
////The velocity is negated because the direction so point backwards along the velocity.
//Vector3.Subtract(ref velocityA, ref velocityB, out localDirection);
//The above takes into account angular velocity, but linear velocity alone is a lot more stable and does the job just fine.
if (collidableA.entity != null && collidableB.entity != null)
{
Vector3.Subtract(ref collidableA.entity.linearVelocity, ref collidableB.entity.linearVelocity, out localDirection);
}
else
{
localDirection = localSeparatingAxis;
}
if (localDirection.LengthSquared() < Toolbox.Epsilon)
{
localDirection = Vector3.Up;
}
}
if (MPRToolbox.GetContact(collidableA.Shape, collidableB.Shape, ref collidableA.worldTransform, ref collidableB.worldTransform, ref localDirection, out contact))
{
if (contact.PenetrationDepth < collidableA.Shape.collisionMargin + collidableB.Shape.collisionMargin)
{
state = CollisionState.ShallowContact;
}
return(true);
}
//This is rare, but could happen.
state = CollisionState.Separated;
return(false);
//if (MPRTesting.GetLocalOverlapPosition(collidableA.Shape, collidableB.Shape, ref localTransformB, out contact.Position))
//{
// //First, try to use the heuristically found direction. This comes from either the GJK shallow contact separating axis or from the relative velocity.
// Vector3 rayCastDirection;
// float lengthSquared = localDirection.LengthSquared();
// if (lengthSquared > Toolbox.Epsilon)
// {
// Vector3.Divide(ref localDirection, (float)Math.Sqrt(lengthSquared), out rayCastDirection);// (Vector3.Normalize(localDirection) + Vector3.Normalize(collidableB.worldTransform.Position - collidableA.worldTransform.Position)) / 2;
// MPRTesting.LocalSurfaceCast(collidableA.Shape, collidableB.Shape, ref localTransformB, ref rayCastDirection, out contact.PenetrationDepth, out contact.Normal);
// }
// else
// {
// contact.PenetrationDepth = float.MaxValue;
// contact.Normal = Toolbox.UpVector;
// }
// //Try the offset between the origins as a second option. Sometimes this is a better choice than the relative velocity.
// //TODO: Could use the position-finding MPR iteration to find the A-B direction hit by continuing even after the origin has been found (optimization).
// Vector3 normalCandidate;
// float depthCandidate;
// lengthSquared = localTransformB.Position.LengthSquared();
// if (lengthSquared > Toolbox.Epsilon)
// {
// Vector3.Divide(ref localTransformB.Position, (float)Math.Sqrt(lengthSquared), out rayCastDirection);
// MPRTesting.LocalSurfaceCast(collidableA.Shape, collidableB.Shape, ref localTransformB, ref rayCastDirection, out depthCandidate, out normalCandidate);
// if (depthCandidate < contact.PenetrationDepth)
// {
// contact.Normal = normalCandidate;
// }
// }
// //Correct the penetration depth.
// MPRTesting.LocalSurfaceCast(collidableA.Shape, collidableB.Shape, ref localTransformB, ref contact.Normal, out contact.PenetrationDepth, out rayCastDirection);
// ////The local casting can optionally continue. Eventually, it will converge to the local minimum.
// //while (true)
// //{
// // MPRTesting.LocalSurfaceCast(collidableA.Shape, collidableB.Shape, ref localTransformB, ref contact.Normal, out depthCandidate, out normalCandidate);
// // if (contact.PenetrationDepth - depthCandidate <= Toolbox.BigEpsilon)
// // break;
// // contact.PenetrationDepth = depthCandidate;
// // contact.Normal = normalCandidate;
// //}
// contact.Id = -1;
// //we're still in local space! transform it all back.
// Matrix3X3 orientation;
// Matrix3X3.CreateFromQuaternion(ref collidableA.worldTransform.Orientation, out orientation);
// Matrix3X3.Transform(ref contact.Normal, ref orientation, out contact.Normal);
// //Vector3.Negate(ref contact.Normal, out contact.Normal);
// Matrix3X3.Transform(ref contact.Position, ref orientation, out contact.Position);
// Vector3.Add(ref contact.Position, ref collidableA.worldTransform.Position, out contact.Position);
// if (contact.PenetrationDepth < collidableA.Shape.collisionMargin + collidableB.Shape.collisionMargin)
// state = CollisionState.ShallowContact;
// return true;
//}
////This is rare, but could happen.
//state = CollisionState.Separated;
//contact = new ContactData();
//return false;
#endregion
#region Testing
//RigidTransform localTransformB;
//MinkowskiToolbox.GetLocalTransform(ref collidableA.worldTransform, ref collidableB.worldTransform, out localTransformB);
//contact.Id = -1;
//if (MPRTesting.GetLocalOverlapPosition(collidableA.Shape, collidableB.Shape, ref localTransformB, out contact.Position))
//{
// Vector3 rayCastDirection = localTransformB.Position;
// MPRTesting.LocalSurfaceCast(collidableA.Shape, collidableB.Shape, ref localTransformB, ref rayCastDirection, out contact.PenetrationDepth, out contact.Normal);
// MPRTesting.LocalSurfaceCast(collidableA.Shape, collidableB.Shape, ref localTransformB, ref contact.Normal, out contact.PenetrationDepth, out rayCastDirection);
// RigidTransform.Transform(ref contact.Position, ref collidableA.worldTransform, out contact.Position);
// Vector3.Transform(ref contact.Normal, ref collidableA.worldTransform.Orientation, out contact.Normal);
// return true;
//}
//contact.Normal = new Vector3();
//contact.PenetrationDepth = 0;
//return false;
#endregion
#region v0.15.2 and before
//if (MPRToolbox.AreObjectsColliding(collidableA.Shape, collidableB.Shape, ref collidableA.worldTransform, ref collidableB.worldTransform, out contact))
//{
// if (contact.PenetrationDepth < collidableA.Shape.collisionMargin + collidableB.Shape.collisionMargin)
// state = CollisionState.ShallowContact; //If it's emerged from the deep contact, we can go back to using the preferred GJK method.
// return true;
//}
////This is rare, but could happen.
//state = CollisionState.Separated;
//return false;
#endregion
}
public override void UpdateCollision(float dt)
{
WasContaining = Containing;
WasTouching = Touching;
mobileTriangle.collisionMargin = mesh.Shape.MeshCollisionMargin;
//Scan the pairs in sequence, updating the state as we go.
//Touching can be set to true by a single touching subpair.
Touching = false;
//Containing can be set to false by a single noncontaining or nontouching subpair.
Containing = true;
var meshData = mesh.Shape.TriangleMesh.Data;
RigidTransform mobileTriangleTransform, detectorTriangleTransform;
mobileTriangleTransform.Orientation = Quaternion.Identity;
detectorTriangleTransform.Orientation = Quaternion.Identity;
for (int i = 0; i < meshData.Indices.Length; i += 3)
{
//Grab a triangle associated with the mobile mesh.
meshData.GetTriangle(i, out mobileTriangle.vA, out mobileTriangle.vB, out mobileTriangle.vC);
RigidTransform.Transform(ref mobileTriangle.vA, ref mesh.worldTransform, out mobileTriangle.vA);
RigidTransform.Transform(ref mobileTriangle.vB, ref mesh.worldTransform, out mobileTriangle.vB);
RigidTransform.Transform(ref mobileTriangle.vC, ref mesh.worldTransform, out mobileTriangle.vC);
Vector3f.Add(ref mobileTriangle.vA, ref mobileTriangle.vB, out mobileTriangleTransform.Position);
Vector3f.Add(ref mobileTriangle.vC, ref mobileTriangleTransform.Position, out mobileTriangleTransform.Position);
Vector3f.Multiply(ref mobileTriangleTransform.Position, 1 / 3f, out mobileTriangleTransform.Position);
Vector3f.Subtract(ref mobileTriangle.vA, ref mobileTriangleTransform.Position, out mobileTriangle.vA);
Vector3f.Subtract(ref mobileTriangle.vB, ref mobileTriangleTransform.Position, out mobileTriangle.vB);
Vector3f.Subtract(ref mobileTriangle.vC, ref mobileTriangleTransform.Position, out mobileTriangle.vC);
//Go through all the detector volume triangles which are near the mobile mesh triangle.
bool triangleTouching, triangleContaining;
BoundingBox mobileBoundingBox;
mobileTriangle.GetBoundingBox(ref mobileTriangleTransform, out mobileBoundingBox);
DetectorVolume.TriangleMesh.Tree.GetOverlaps(mobileBoundingBox, overlaps);
for (int j = 0; j < overlaps.Count; j++)
{
DetectorVolume.TriangleMesh.Data.GetTriangle(overlaps.Elements[j], out detectorTriangle.vA, out detectorTriangle.vB, out detectorTriangle.vC);
Vector3f.Add(ref detectorTriangle.vA, ref detectorTriangle.vB, out detectorTriangleTransform.Position);
Vector3f.Add(ref detectorTriangle.vC, ref detectorTriangleTransform.Position, out detectorTriangleTransform.Position);
Vector3f.Multiply(ref detectorTriangleTransform.Position, 1 / 3f, out detectorTriangleTransform.Position);
Vector3f.Subtract(ref detectorTriangle.vA, ref detectorTriangleTransform.Position, out detectorTriangle.vA);
Vector3f.Subtract(ref detectorTriangle.vB, ref detectorTriangleTransform.Position, out detectorTriangle.vB);
Vector3f.Subtract(ref detectorTriangle.vC, ref detectorTriangleTransform.Position, out detectorTriangle.vC);
//If this triangle collides with the convex, we can stop immediately since we know we're touching and not containing.)))
//[MPR is used here in lieu of GJK because the MPR implementation tends to finish quicker than GJK when objects are overlapping. The GJK implementation does better on separated objects.]
if (MPRToolbox.AreShapesOverlapping(detectorTriangle, mobileTriangle, ref detectorTriangleTransform, ref mobileTriangleTransform))
{
triangleTouching = true;
//The convex can't be fully contained if it's still touching the surface.
triangleContaining = false;
overlaps.Clear();
goto finishTriangleTest;
}
}
overlaps.Clear();
//If we get here, then there was no shell intersection.
//If the convex's center point is contained by the mesh, then the convex is fully contained.
//This test is only needed if containment hasn't yet been outlawed or a touching state hasn't been established.
if ((!Touching || Containing) && DetectorVolume.IsPointContained(ref mobileTriangleTransform.Position, overlaps))
{
triangleTouching = true;
triangleContaining = true;
goto finishTriangleTest;
}
//If we get here, then there was no surface intersection and the convex's center is not contained- the volume and convex are separate!
triangleTouching = false;
triangleContaining = false;
finishTriangleTest:
//Analyze the results of the triangle test.
if (triangleTouching)
{
Touching = true; //If one child is touching, then we are touching too.
}
else
{
Containing = false; //If one child isn't touching, then we aren't containing.
}
if (!triangleContaining) //If one child isn't containing, then we aren't containing.
{
Containing = false;
}
if (!Containing && Touching)
{
//If it's touching but not containing, no further pairs will change the state.
//Containment has been invalidated by something that either didn't touch or wasn't contained.
//Touching has been ensured by at least one object touching.
break;
}
}
//There is a possibility that the MobileMesh is solid and fully contains the DetectorVolume.
//In this case, we should be Touching, but currently we are not.
if (mesh.Shape.solidity == MobileMeshSolidity.Solid && !Containing && !Touching)
{
//To determine if the detector volume is fully contained, check if one of the detector mesh's vertices
//are in the mobile mesh.
//This *could* fail if the mobile mesh is actually multiple pieces, but that's not a common or really supported case for solids.
Vector3f vertex;
DetectorVolume.TriangleMesh.Data.GetVertexPosition(0, out vertex);
Ray ray;
ray.Direction = VectorHelper.Up;
RayHit hit;
RigidTransform.TransformByInverse(ref vertex, ref mesh.worldTransform, out ray.Position);
if (mesh.Shape.IsLocalRayOriginInMesh(ref ray, out hit))
{
Touching = true;
}
}
NotifyDetectorVolumeOfChanges();
}
public override bool ConvexCast(ConvexShape castShape, ref RigidTransform startingTransform, ref System.Numerics.Vector3 sweep, out RayHit hit)
{
return(MPRToolbox.Sweep(castShape, Shape, ref sweep, ref Toolbox.ZeroVector, ref startingTransform, ref worldTransform, out hit));
}
