///<summary>
/// Gets the local transform of B in the space of A.
///</summary>
///<param name="transformA">First transform.</param>
///<param name="transformB">Second transform.</param>
///<param name="localTransformB">Transform of B in the local space of A.</param>
public static void GetLocalTransform(ref RigidTransform transformA, ref RigidTransform transformB,
out RigidTransform localTransformB)
{
//Put B into A's space.
Quaternion conjugateOrientationA;
Quaternion.Conjugate(ref transformA.Orientation, out conjugateOrientationA);
Quaternion.Concatenate(ref transformB.Orientation, ref conjugateOrientationA, out localTransformB.Orientation);
Vector3.Subtract(ref transformB.Position, ref transformA.Position, out localTransformB.Position);
Vector3.Transform(ref localTransformB.Position, ref conjugateOrientationA, out localTransformB.Position);
}
///<summary>
/// Gets the extreme point of the shape in world space in a given direction.
///</summary>
///<param name="direction">Direction to find the extreme point in.</param>
/// <param name="shapeTransform">Transform to use for the shape.</param>
///<param name="extremePoint">Extreme point on the shape.</param>
public void GetExtremePointWithoutMargin(Vector3 direction, ref RigidTransform shapeTransform, out Vector3 extremePoint)
{
Quaternion conjugate;
Quaternion.Conjugate(ref shapeTransform.Orientation, out conjugate);
Vector3.Transform(ref direction, ref conjugate, out direction);
GetLocalExtremePointWithoutMargin(ref direction, out extremePoint);
Vector3.Transform(ref extremePoint, ref shapeTransform.Orientation, out extremePoint);
Vector3.Add(ref extremePoint, ref shapeTransform.Position, out extremePoint);
}
///<summary>
/// Gets the extreme point of the shape in world space in a given direction with margin expansion.
///</summary>
///<param name="direction">Direction to find the extreme point in.</param>
/// <param name="shapeTransform">Transform to use for the shape.</param>
///<param name="extremePoint">Extreme point on the shape.</param>
public void GetExtremePoint(Vector3 direction, ref RigidTransform shapeTransform, out Vector3 extremePoint)
{
GetExtremePointWithoutMargin(direction, ref shapeTransform, out extremePoint);
float directionLength = direction.LengthSquared();
if (directionLength > Toolbox.Epsilon)
{
Vector3.Multiply(ref direction, collisionMargin / (float)Math.Sqrt(directionLength), out direction);
Vector3.Add(ref extremePoint, ref direction, out extremePoint);
}
}
/// <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
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;
}
public override void UpdateCollision(float dt)
{
WasContaining = Containing;
WasTouching = Touching;
var 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();
}
public override void GetBoundingBox(ref RigidTransform shapeTransform, out BoundingBox boundingBox)
{
#if !WINDOWS
boundingBox = new BoundingBox();
#endif
Vector3 upExtreme = new Vector3(0, halfLength, 0);
Vector3 downExtreme = new Vector3(0, -halfLength, 0);
Vector3.Transform(ref upExtreme, ref shapeTransform.Orientation, out upExtreme);
Vector3.Transform(ref downExtreme, ref shapeTransform.Orientation, out downExtreme);
if (upExtreme.X > downExtreme.X)
{
boundingBox.Max.X = upExtreme.X;
boundingBox.Min.X = downExtreme.X;
}
else
{
boundingBox.Max.X = downExtreme.X;
boundingBox.Min.X = upExtreme.X;
}
if (upExtreme.Y > downExtreme.Y)
{
boundingBox.Max.Y = upExtreme.Y;
boundingBox.Min.Y = downExtreme.Y;
}
else
{
boundingBox.Max.Y = downExtreme.Y;
boundingBox.Min.Y = upExtreme.Y;
}
if (upExtreme.Z > downExtreme.Z)
{
boundingBox.Max.Z = upExtreme.Z;
boundingBox.Min.Z = downExtreme.Z;
}
else
{
boundingBox.Max.Z = downExtreme.Z;
boundingBox.Min.Z = upExtreme.Z;
}
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 extreme point of the minkowski difference of shapeA and shapeB in the local space of shapeA.
///</summary>
///<param name="shapeA">First shape.</param>
///<param name="shapeB">Second shape.</param>
///<param name="direction">Extreme point direction in local space.</param>
///<param name="localTransformB">Transform of shapeB in the local space of A.</param>
/// <param name="extremePointA">The extreme point on shapeA.</param>
///<param name="extremePoint">The extreme point in the local space of A.</param>
public static void GetLocalMinkowskiExtremePoint(ConvexShape shapeA, ConvexShape shapeB, ref Vector3 direction, ref RigidTransform localTransformB,
out Vector3 extremePointA, out Vector3 extremePoint)
{
//Extreme point of A-B along D = (extreme point of A along D) - (extreme point of B along -D)
shapeA.GetLocalExtremePointWithoutMargin(ref direction, out extremePointA);
Vector3 v;
Vector3.Negate(ref direction, out v);
Vector3 extremePointB;
shapeB.GetExtremePointWithoutMargin(v, ref localTransformB, out extremePointB);
ExpandMinkowskiSum(shapeA.collisionMargin, shapeB.collisionMargin, direction, ref extremePointA, ref extremePointB);
Vector3.Subtract(ref extremePointA, ref extremePointB, out extremePoint);
}
internal void Enable()
{
//Turn everything on.
lock (FlipLocker)
{
int initialCount = Math.Max(manager.entities.Count, 64);
backBuffer = new RigidTransform[initialCount];
states = new RigidTransform[initialCount];
for (int i = 0; i < manager.entities.Count; i++)
{
Entity entity = manager.entities[i];
backBuffer[i].Position = entity.position;
backBuffer[i].Orientation = entity.orientation;
}
Array.Copy(backBuffer, states, backBuffer.Length);
}
}
///<summary>
/// Tests if the pair is intersecting.
///</summary>
///<param name="shapeA">First shape of the pair.</param>
///<param name="shapeB">Second shape of the pair.</param>
///<param name="transformA">Transform to apply to the first shape.</param>
///<param name="transformB">Transform to apply to the second shape.</param>
///<param name="localSeparatingAxis">Warmstartable separating axis used by the method to quickly early-out if possible. Updated to the latest separating axis after each run.</param>
///<returns>Whether or not the objects were intersecting.</returns>
public static bool AreShapesIntersecting(ConvexShape shapeA, ConvexShape shapeB, ref RigidTransform transformA, ref RigidTransform transformB,
ref Vector3 localSeparatingAxis)
{
RigidTransform localtransformB;
MinkowskiToolbox.GetLocalTransform(ref transformA, ref transformB, out localtransformB);
//Warm start the simplex.
var simplex = new SimpleSimplex();
Vector3 extremePoint;
MinkowskiToolbox.GetLocalMinkowskiExtremePoint(shapeA, shapeB, ref localSeparatingAxis, ref localtransformB, out extremePoint);
simplex.AddNewSimplexPoint(ref extremePoint);
Vector3 closestPoint;
int count = 0;
while (count++ < MaximumGJKIterations)
{
if (simplex.GetPointClosestToOrigin(out closestPoint) || //Also reduces the simplex.
closestPoint.LengthSquared() <= simplex.GetErrorTolerance() * Toolbox.BigEpsilon)
{
//Intersecting, or so close to it that it will be difficult/expensive to figure out the separation.
return true;
}
//Use the closest point as a direction.
Vector3 direction;
Vector3.Negate(ref closestPoint, out direction);
MinkowskiToolbox.GetLocalMinkowskiExtremePoint(shapeA, shapeB, ref direction, ref localtransformB, out extremePoint);
//Since this is a boolean test, we don't need to refine the simplex if it becomes apparent that we cannot reach the origin.
//If the most extreme point at any given time does not go past the origin, then we can quit immediately.
float dot;
Vector3.Dot(ref extremePoint, ref closestPoint, out dot); //extreme point dotted against the direction pointing backwards towards the CSO.
if (dot > 0)
{
// If it's positive, that means that the direction pointing towards the origin produced an extreme point 'in front of' the origin, eliminating the possibility of any intersection.
localSeparatingAxis = direction;
return false;
}
simplex.AddNewSimplexPoint(ref extremePoint);
}
return false;
}
/// <summary>
/// Constructs a new demo.
/// </summary>
/// <param name="game">Game owning this demo.</param>
public MPRCastingDemo(DemosGame game)
: base(game)
{
bShape = new BoxShape(1, 0, 1);
//bShape.CollisionMargin = 0;
aShape = new ConeShape(1, .4f);
//aShape.CollisionMargin = 0;
a = new Entity(aShape);
b = new Entity(bShape);
CollisionRules.AddRule(a, b, CollisionRule.NoSolver);
NarrowPhaseHelper.CollisionManagers.Remove(new TypePair(typeof(ConvexCollidable<BoxShape>), typeof(ConvexCollidable<BoxShape>)));
Space.Add(a);
Space.Add(b);
a.Orientation = Quaternion.CreateFromAxisAngle(new Vector3(1, 0, 0), MathHelper.PiOver4);
b.Orientation = Quaternion.Identity;
aTransform = new RigidTransform(new Vector3(0, 0, 0), a.Orientation);
bTransform = new RigidTransform(new Vector3(0, 10, 0), b.Orientation);
game.Camera.Position = new Vector3(0, 5, 17);
}
/// <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.
Vector3 direction = new Vector3(o.M11, o.M21, o.M31);
Vector3 right;
GetLocalExtremePointWithoutMargin(ref direction, out right);
direction = new Vector3(o.M12, o.M22, o.M32);
Vector3 up;
GetLocalExtremePointWithoutMargin(ref direction, out up);
direction = new Vector3(o.M13, o.M23, o.M33);
Vector3 backward;
GetLocalExtremePointWithoutMargin(ref direction, out backward);
Matrix3X3.Transform(ref right, ref o, out right);
Matrix3X3.Transform(ref up, ref o, out up);
Matrix3X3.Transform(ref backward, ref o, out backward);
//These right/up/backward represent the extreme points in world space along the world space axes.
boundingBox.Max.X = shapeTransform.Position.X + collisionMargin + right.X;
boundingBox.Max.Y = shapeTransform.Position.Y + collisionMargin + up.Y;
boundingBox.Max.Z = shapeTransform.Position.Z + collisionMargin + backward.Z;
boundingBox.Min.X = shapeTransform.Position.X - collisionMargin - right.X;
boundingBox.Min.Y = shapeTransform.Position.Y - collisionMargin - up.Y;
boundingBox.Min.Z = shapeTransform.Position.Z - collisionMargin - backward.Z;
}
///<summary>
/// Transforms a rigid transform by another rigid transform.
///</summary>
///<param name="a">The first, "local" rigid transform.</param>
///<param name="b">The second, "world" rigid transform.</param>
///<param name="combined">Combined rigid transform.</param>
public static void Transform(ref RigidTransform a, ref RigidTransform b, out RigidTransform combined)
{
Vector3 intermediate;
Vector3.Transform(ref a.Position, ref b.Orientation, out intermediate);
Vector3.Add(ref intermediate, ref b.Position, out combined.Position);
Quaternion.Concatenate(ref a.Orientation, ref b.Orientation, out combined.Orientation);
}
/// <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;
Vector3 right = new Vector3(o.M11, o.M21, o.M31);
Vector3 up = new Vector3(o.M12, o.M22, o.M32);
Vector3 backward = new Vector3(o.M13, o.M23, o.M33);
Vector3.Dot(ref vertices.Elements[0], ref right, out maxX);
minX = maxX;
Vector3.Dot(ref vertices.Elements[0], ref up, out maxY);
minY = maxY;
Vector3.Dot(ref vertices.Elements[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.count; i++)
{
float dot;
Vector3.Dot(ref vertices.Elements[i], ref right, out dot);
if (dot < minX)
{
minX = dot;
minXIndex = i;
}
else if (dot > maxX)
{
maxX = dot;
maxXIndex = i;
}
Vector3.Dot(ref vertices.Elements[i], ref up, out dot);
if (dot < minY)
{
minY = dot;
minYIndex = i;
}
else if (dot > maxY)
{
maxY = dot;
maxYIndex = i;
}
Vector3.Dot(ref vertices.Elements[i], ref backward, out dot);
if (dot < minZ)
{
minZ = dot;
minZIndex = i;
}
else if (dot > maxZ)
{
maxZ = dot;
maxZIndex = i;
}
}
Vector3 minXpoint, maxXpoint, minYpoint, maxYpoint, minZpoint, maxZpoint;
Matrix3X3.Transform(ref vertices.Elements[minXIndex], ref o, out minXpoint);
Matrix3X3.Transform(ref vertices.Elements[maxXIndex], ref o, out maxXpoint);
Matrix3X3.Transform(ref vertices.Elements[minYIndex], ref o, out minYpoint);
Matrix3X3.Transform(ref vertices.Elements[maxYIndex], ref o, out maxYpoint);
Matrix3X3.Transform(ref vertices.Elements[minZIndex], ref o, out minZpoint);
Matrix3X3.Transform(ref vertices.Elements[maxZIndex], ref o, out maxZpoint);
boundingBox.Max.X = shapeTransform.Position.X + collisionMargin + maxXpoint.X;
boundingBox.Max.Y = shapeTransform.Position.Y + collisionMargin + maxYpoint.Y;
boundingBox.Max.Z = shapeTransform.Position.Z + collisionMargin + maxZpoint.Z;
boundingBox.Min.X = shapeTransform.Position.X - collisionMargin + minXpoint.X;
boundingBox.Min.Y = shapeTransform.Position.Y - collisionMargin + minYpoint.Y;
boundingBox.Min.Z = shapeTransform.Position.Z - collisionMargin + minZpoint.Z;
}
/// <summary>
/// Updates the collidable's world transform and bounding box.
/// This is a convenience method for external modification of the collidable's data.
/// </summary>
/// <param name="transform">Transform to use for the collidable.</param>
public void UpdateBoundingBoxForTransform(ref RigidTransform transform)
{
UpdateBoundingBoxForTransform(ref transform, 0);
}
/// <summary>
/// Updates the collidable's world transform and bounding box. The transform provided
/// will be offset by the collidable's LocalPosition to get the shape transform.
/// This is a convenience method for external modification of the collidable's data.
/// </summary>
/// <param name="transform">Transform to use for the collidable.</param>
/// <param name="dt">Duration of the simulation time step. Used to expand the
/// bounding box using the owning entity's velocity. If the collidable
/// does not have an owning entity, this must be zero.</param>
public void UpdateBoundingBoxForTransform(ref RigidTransform transform, float dt)
{
UpdateWorldTransform(ref transform.Position, ref transform.Orientation);
UpdateBoundingBoxInternal(dt);
}
public static void Validate(this RigidTransform r)
{
r.Position.Validate();
r.Orientation.Validate();
}
/// <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);
Vector3.Transform(ref ray.Direction, ref conjugate, out localRay.Direction);
Vector3.Subtract(ref ray.Position, ref transform.Position, out localRay.Position);
Vector3.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);
Vector3.Transform(ref hit.Normal, ref transform.Orientation, out hit.Normal);
return toReturn;
}
/// <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;
Vector3.Transform(ref vA, ref shapeTransform.Orientation, out a);
Vector3.Transform(ref vB, ref shapeTransform.Orientation, out b);
Vector3.Transform(ref vC, ref shapeTransform.Orientation, 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>
/// Transforms a position by a rigid transform's inverse.
///</summary>
///<param name="position">Position to transform.</param>
///<param name="transform">Transform to invert and apply.</param>
///<param name="result">Transformed position.</param>
public static void TransformByInverse(ref Vector3 position, ref RigidTransform transform, out Vector3 result)
{
Quaternion orientation;
Vector3 intermediate;
Vector3.Subtract(ref position, ref transform.Position, out intermediate);
Quaternion.Conjugate(ref transform.Orientation, out orientation);
Vector3.Transform(ref intermediate, ref orientation, out result);
}
/// <summary>
/// Gets the intersection between the sphere and the ray.
/// </summary>
/// <param name="ray">Ray to test against the sphere.</param>
/// <param name="transform">Transform applied to the convex for the test.</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 override bool RayTest(ref Ray ray, ref RigidTransform transform, float maximumLength, out RayHit hit)
{
return Toolbox.RayCastSphere(ref ray, ref transform.Position, collisionMargin, maximumLength, out hit);
//Vector3 normalizedDirection;
//float length = ray.Direction.Length();
//Vector3.Divide(ref ray.Direction, length, out normalizedDirection);
//maximumLength *= length;
//hit = new RayHit();
//Vector3 m;
//Vector3.Subtract(ref ray.Position, ref transform.Position, out m);
//float b = Vector3.Dot(m, normalizedDirection);
//float c = m.LengthSquared() - collisionMargin * collisionMargin;
//if (c > 0 && b > 0)
// return false;
//float discriminant = b * b - c;
//if (discriminant < 0)
// return false;
//hit.T = -b - (float)Math.Sqrt(discriminant);
//if (hit.T < 0)
// hit.T = 0;
//if (hit.T > maximumLength)
// return false;
//Vector3.Multiply(ref normalizedDirection, hit.T, out hit.Location);
//Vector3.Add(ref hit.Location, ref ray.Position, out hit.Location);
//Vector3.Subtract(ref hit.Location, ref transform.Position, out hit.Normal);
//hit.Normal.Normalize();
//return true;
}
///<summary>
/// Transforms a rigid transform by another rigid transform's inverse.
///</summary>
///<param name="a">The first rigid transform.</param>
///<param name="b">The second rigid transform, to be inverted.</param>
///<param name="combinedTransform">Combined rigid transform.</param>
public static void TransformByInverse(ref RigidTransform a, ref RigidTransform b, out RigidTransform combinedTransform)
{
Invert(ref b, out combinedTransform);
Transform(ref a, ref combinedTransform, out combinedTransform);
}
///<summary>
/// Transforms a position by a rigid transform.
///</summary>
///<param name="position">Position to transform.</param>
///<param name="transform">Transform to apply.</param>
///<param name="result">Transformed position.</param>
public static void Transform(ref Vector3 position, ref RigidTransform transform, out Vector3 result)
{
Vector3 intermediate;
Vector3.Transform(ref position, ref transform.Orientation, out intermediate);
Vector3.Add(ref intermediate, ref transform.Position, out result);
}
/// <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;
Toolbox.GetExpandedBoundingBox(ref castShape, ref startingTransform, ref sweep, out boundingBox);
var hitElements = Resources.GetCompoundChildList();
if (hierarchy.Tree.GetOverlaps(boundingBox, hitElements))
{
hit.T = float.MaxValue;
for (int i = 0; i < hitElements.count; i++)
{
var candidate = hitElements.Elements[i].CollisionInformation;
RayHit tempHit;
if (candidate.ConvexCast(castShape, ref startingTransform, ref sweep, out tempHit) && tempHit.T < hit.T)
{
hit = tempHit;
}
}
Resources.GiveBack(hitElements);
return hit.T != float.MaxValue;
}
Resources.GiveBack(hitElements);
return false;
}
/// <summary>
/// Inverts a rigid transform.
/// </summary>
/// <param name="transform">Transform to invert.</param>
/// <param name="inverse">Inverse of the transform.</param>
public static void Invert(ref RigidTransform transform, out RigidTransform inverse)
{
Quaternion.Conjugate(ref transform.Orientation, out inverse.Orientation);
Vector3.Transform(ref transform.Position, ref inverse.Orientation, out inverse.Position);
Vector3.Negate(ref inverse.Position, out inverse.Position);
}
public override bool ConvexCast(ConvexShape castShape, ref MathExtensions.RigidTransform startingTransform, ref Vector3 sweep, out RayHit hit)
{
hit = new RayHit();
BoundingBox boundingBox;
Toolbox.GetExpandedBoundingBox(ref castShape, ref startingTransform, ref sweep, out boundingBox);
var tri = Resources.GetTriangle();
var hitElements = Resources.GetIntList();
if (triangleMesh.Tree.GetOverlaps(boundingBox, hitElements))
{
hit.T = float.MaxValue;
for (int i = 0; i < hitElements.Count; i++)
{
triangleMesh.Data.GetTriangle(hitElements[i], out tri.vA, out tri.vB, 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;
Resources.GiveBack(tri);
Resources.GiveBack(hitElements);
return hit.T != float.MaxValue;
}
Resources.GiveBack(tri);
Resources.GiveBack(hitElements);
return false;
}
/// <summary>
/// Gets the normal of the triangle in world space.
/// </summary>
/// <param name="transform">World transform.</param>
/// <returns>Normal of the triangle in world space.</returns>
public Vector3 GetNormal(RigidTransform transform)
{
Vector3 normal = GetLocalNormal();
Vector3.Transform(ref normal, ref transform.Orientation, out normal);
return normal;
}
///<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 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);
////Sample the local directions from the orientation matrix, implicitly transposed.
//Vector3 right = new Vector3(o.M11 * 100000, o.M21 * 100000, o.M31 * 100000);
//Vector3 up = new Vector3(o.M12 * 100000, o.M22 * 100000, o.M32 * 100000);
//Vector3 backward = new Vector3(o.M13 * 100000, o.M23 * 100000, o.M33 * 100000);
//Vector3 left, down, forward;
//Vector3.Negate(ref right, out left);
//Vector3.Negate(ref up, out down);
//Vector3.Negate(ref backward, out forward);
//for (int i = 0; i < extents.count; i++)
//{
// extents.Elements[i].Clamp(ref right);
// extents.Elements[i].Clamp(ref left);
// extents.Elements[i].Clamp(ref up);
// extents.Elements[i].Clamp(ref down);
// extents.Elements[i].Clamp(ref backward);
// extents.Elements[i].Clamp(ref forward);
//}
//Matrix3X3.Transform(ref right, ref o, out right);
//Matrix3X3.Transform(ref left, ref o, out left);
//Matrix3X3.Transform(ref down, ref o, out down);
//Matrix3X3.Transform(ref up, ref o, out up);
//Matrix3X3.Transform(ref forward, ref o, out forward);
//Matrix3X3.Transform(ref backward, ref o, out backward);
//boundingBox.Max.X = shapeTransform.Position.X + right.X;
//boundingBox.Max.Y = shapeTransform.Position.Y + up.Y;
//boundingBox.Max.Z = shapeTransform.Position.Z + backward.Z;
//boundingBox.Min.X = shapeTransform.Position.X + left.X;
//boundingBox.Min.Y = shapeTransform.Position.Y + down.Y;
//boundingBox.Min.Z = shapeTransform.Position.Z + forward.Z;
Matrix3X3 o;
Matrix3X3.CreateFromQuaternion(ref shapeTransform.Orientation, out o);
GetBoundingBox(ref o, out boundingBox);
boundingBox.Max.X += shapeTransform.Position.X;
boundingBox.Max.Y += shapeTransform.Position.Y;
boundingBox.Max.Z += shapeTransform.Position.Z;
boundingBox.Min.X += shapeTransform.Position.X;
boundingBox.Min.Y += shapeTransform.Position.Y;
boundingBox.Min.Z += shapeTransform.Position.Z;
}
/// <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)
{
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;
AffineTransform transform = new AffineTransform();
transform.Translation = worldTransform.Position;
Matrix3X3.CreateFromQuaternion(ref worldTransform.Orientation, out transform.LinearTransform);
castShape.GetSweptLocalBoundingBox(ref startingTransform, ref transform, ref sweep, out boundingBox);
var tri = Resources.GetTriangle();
var hitElements = Resources.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();
triangleTransform.Orientation = Quaternion.Identity;
triangleTransform.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;
Resources.GiveBack(tri);
Resources.GiveBack(hitElements);
return hit.T != float.MaxValue;
}
Resources.GiveBack(tri);
Resources.GiveBack(hitElements);
return false;
}
/// <summary>
/// Gets the bounding box of the mesh transformed first into world space, and then into the local space of another affine transform.
/// </summary>
/// <param name="shapeTransform">Transform to use to put the shape into world space.</param>
/// <param name="spaceTransform">Used as the frame of reference to compute the bounding box.
/// In effect, the shape is transformed by the inverse of the space transform to compute its bounding box in local space.</param>
/// <param name="boundingBox">Bounding box in the local space.</param>
public void GetLocalBoundingBox(ref RigidTransform shapeTransform, ref AffineTransform spaceTransform, out BoundingBox boundingBox)
{
#if !WINDOWS
boundingBox = new BoundingBox();
#endif
//TODO: This method peforms quite a few sqrts because the collision margin can get scaled, and so cannot be applied as a final step.
//There should be a better way to do this.
//Additionally, this bounding box is not consistent in all cases with the post-add version. Adding the collision margin at the end can
//slightly overestimate the size of a margin expanded shape at the corners, which is fine (and actually important for the box-box special case).
//Move forward into convex's space, backwards into the new space's local space.
AffineTransform transform;
AffineTransform.Invert(ref spaceTransform, out transform);
AffineTransform.Multiply(ref shapeTransform, ref transform, out transform);
GetBoundingBox(ref transform.LinearTransform, out boundingBox);
boundingBox.Max.X += transform.Translation.X;
boundingBox.Max.Y += transform.Translation.Y;
boundingBox.Max.Z += transform.Translation.Z;
boundingBox.Min.X += transform.Translation.X;
boundingBox.Min.Y += transform.Translation.Y;
boundingBox.Min.Z += transform.Translation.Z;
}
/// <summary> /// Sweeps a convex shape against the entry. /// </summary> /// <param name="castShape">Swept shape.</param> /// <param name="startingTransform">Beginning location and orientation of the cast shape.</param> /// <param name="sweep">Sweep motion to apply to the cast shape.</param> /// <param name="hit">Hit data of the ray on the entry, if any.</param> /// <returns>Whether or not the ray hit the entry.</returns> public abstract bool ConvexCast(ConvexShape castShape, ref RigidTransform startingTransform, ref Vector3 sweep, out RayHit hit);
/// <summary>
/// Gets the bounding box of the mesh transformed first into world space, and then into the local space of another affine transform.
/// </summary>
/// <param name="shapeTransform">Transform to use to put the shape into world space.</param>
/// <param name="spaceTransform">Used as the frame of reference to compute the bounding box.
/// In effect, the shape is transformed by the inverse of the space transform to compute its bounding box in local space.</param>
/// <param name="sweep">World space sweep direction to transform and add to the bounding box.</param>
/// <param name="boundingBox">Bounding box in the local space.</param>
public void GetSweptLocalBoundingBox(ref RigidTransform shapeTransform, ref AffineTransform spaceTransform, ref Vector3 sweep, out BoundingBox boundingBox)
{
GetLocalBoundingBox(ref shapeTransform, ref spaceTransform, out boundingBox);
Vector3 expansion;
Matrix3X3.TransformTranspose(ref sweep, ref spaceTransform.LinearTransform, out expansion);
Toolbox.ExpandBoundingBox(ref boundingBox, ref expansion);
}
