///<summary>
/// Constructs a new compound shape entry using the volume of the shape as a weight.
///</summary>
///<param name="shape">Shape to use.</param>
///<param name="position">Local position of the shape.</param>
///<param name="weight">Weight of the entry. This defines how much the entry contributes to its owner
/// for the purposes of center of mass and inertia computation.</param>
public CompoundShapeEntry(EntityShape shape, Vector3 position, float weight)
{
position.Validate();
LocalTransform = new RigidTransform(position);
Shape = shape;
Weight = weight;
}
///<summary>
/// Constructs a new compound shape entry using the volume of the shape as a weight.
///</summary>
///<param name="shape">Shape to use.</param>
///<param name="orientation">Local orientation of the shape.</param>
///<param name="weight">Weight of the entry. This defines how much the entry contributes to its owner
/// for the purposes of center of rotation computation.</param>
public CompoundShapeEntry(EntityShape shape, Quaternion orientation, float weight)
{
orientation.Validate();
LocalTransform = new RigidTransform(orientation);
Shape = shape;
Weight = weight;
}
///<summary>
/// Constructs a new compound shape entry using the volume of the shape as a weight.
///</summary>
///<param name="shape">Shape to use.</param>
///<param name="localTransform">Local transform of the shape.</param>
///<param name="weight">Weight of the entry. This defines how much the entry contributes to its owner
/// for the purposes of center of rotation computation.</param>
public CompoundShapeEntry(EntityShape shape, RigidTransform localTransform, float weight)
{
localTransform.Validate();
LocalTransform = localTransform;
Shape = shape;
Weight = weight;
}
/// <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);
}
public override bool ConvexCast(ConvexShape castShape, ref RigidTransform startingTransform, ref Vector3 sweep, Func<BroadPhaseEntry, bool> filter, out RayHit hit)
{
Vector3 swp = sweep;
double len = swp.Length();
swp /= len;
return ConvexCast(castShape, ref startingTransform, ref swp, len, MaterialSolidity.FULLSOLID, out hit);
}
public override SolverUpdateable GetBaseJoint()
{
RigidTransform rt1 = new RigidTransform(Ent1.Body.Position, Ent1.Body.Orientation);
RigidTransform rt2 = new RigidTransform(Ent2.Body.Position, Ent2.Body.Orientation);
RigidTransform.MultiplyByInverse(ref rt1, ref rt2, out Relative);
return new WeldJoint(Ent1.Body, Ent2.Body);
}
public bool HassSolidEntity(Location min, Location max)
{
// TODO: Better alg!
BoundingBox bb = new BoundingBox(min.ToBVector(), max.ToBVector());
List<BroadPhaseEntry> entries = new List<BroadPhaseEntry>();
PhysicsWorld.BroadPhase.QueryAccelerator.GetEntries(bb, entries);
if (entries.Count == 0)
{
return false;
}
Location center = (max + min) * 0.5;
Location rel = max - min;
BoxShape box = new BoxShape((double)rel.X, (double)rel.Y, (double)rel.Z);
RigidTransform start = new RigidTransform(center.ToBVector(), Quaternion.Identity);
Vector3 sweep = new Vector3(0, 0, 0.01f);
RayHit rh;
foreach (BroadPhaseEntry entry in entries)
{
if (entry is EntityCollidable && Collision.ShouldCollide(entry) &&
entry.CollisionRules.Group != CollisionUtil.Player &&
entry.ConvexCast(box, ref start, ref sweep, out rh))
{
return true;
}
}
return false;
}
///<summary>
/// Casts a fat (sphere expanded) ray against the shape. If the raycast appears to be stuck in the shape, the cast will be attempted
/// with a smaller ray (scaled by the MotionSettings.CoreShapeScaling each time).
///</summary>
///<param name="ray">Ray to test against the shape.</param>
///<param name="radius">Radius of the ray.</param>
///<param name="target">Shape to test against.</param>
///<param name="shapeTransform">Transform to apply to the shape for the test.</param>
///<param name="maximumLength">Maximum length of the ray in units of the ray direction's length.</param>
///<param name="hit">Hit data of the sphere cast, if any.</param>
///<returns>Whether or not the sphere cast hit the shape.</returns>
public static bool CCDSphereCast(Ray ray, float radius, ConvexShape target, ref RigidTransform shapeTransform, float maximumLength,
out RayHit hit)
{
int iterations = 0;
while (true)
{
if (GJKToolbox.SphereCast(ray, radius, target, ref shapeTransform, maximumLength, out hit) &&
hit.T > 0)
{
//The ray cast isn't embedded in the shape, and it's less than maximum length away!
return(true);
}
if (hit.T > maximumLength || hit.T < 0)
{
return(false); //Failure showed it was too far, or behind.
}
radius *= MotionSettings.CoreShapeScaling;
iterations++;
if (iterations > 3) //Limit could be configurable.
{
//It's iterated too much, let's just do a last ditch attempt using a raycast and hope that can help.
return(GJKToolbox.RayCast(ray, target, ref shapeTransform, maximumLength, out hit) && hit.T > 0);
}
}
}
/// <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="filter">Test to apply to the entry. If it returns true, the entry is processed, otherwise the entry is ignored. If a collidable hierarchy is present
/// in the entry, this filter will be passed into inner ray casts.</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, Func <BroadPhaseEntry, bool> filter, out RayHit hit)
{
RayCastResult result;
bool toReturn = Shape.ConvexCast(castShape, ref startingTransform, ref sweep, filter, out result);
hit = result.HitData;
return(toReturn);
}
/// <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 System.Numerics.Vector3 sweep, out RayHit hit)
{
RayCastResult result;
bool toReturn = Shape.ConvexCast(castShape, ref startingTransform, ref sweep, out result);
hit = result.HitData;
return(toReturn);
}
void ApplyLiquidForcesTo(Entity e, double dt)
{
if (e.Mass <= 0)
{
return;
}
RigidTransform ert = new RigidTransform(e.Position, e.Orientation);
BoundingBox entbb;
e.CollisionInformation.Shape.GetBoundingBox(ref ert, out entbb);
Location min = new Location(entbb.Min);
Location max = new Location(entbb.Max);
min = min.GetBlockLocation();
max = max.GetUpperBlockBorder();
for (int x = (int)min.X; x < max.X; x++)
{
for (int y = (int)min.Y; y < max.Y; y++)
{
for (int z = (int)min.Z; z < max.Z; z++)
{
Location c = new Location(x, y, z);
Material mat = (Material)TheRegion.GetBlockInternal_NoLoad(c).BlockMaterial;
if (mat.GetSolidity() != MaterialSolidity.LIQUID)
{
continue;
}
// TODO: Account for block shape?
double vol = e.CollisionInformation.Shape.Volume;
double dens = (e.Mass / vol);
double WaterDens = 5; // TODO: Read from material. // TODO: Sanity of values.
double modifier = (double)(WaterDens / dens);
double submod = 0.125f;
// TODO: Tracing accuracy!
Vector3 impulse = -(TheRegion.PhysicsWorld.ForceUpdater.Gravity + TheRegion.GravityNormal.ToBVector() * 0.4f) * e.Mass * dt * modifier * submod;
// TODO: Don't apply smaller logic this if scale is big!
for (double x2 = 0.25f; x2 < 1; x2 += 0.5f)
{
for (double y2 = 0.25f; y2 < 1; y2 += 0.5f)
{
for (double z2 = 0.25f; z2 < 1; z2 += 0.5f)
{
Location lc = c + new Location(x2, y2, z2);
RayHit rh;
if (e.CollisionInformation.RayCast(new Ray(lc.ToBVector(), new Vector3(0, 0, 1)), 0.01f, out rh)) // TODO: Efficiency!
{
Vector3 center = lc.ToBVector();
e.ApplyImpulse(ref center, ref impulse);
e.ModifyLinearDamping(mat.GetSpeedMod());
e.ModifyAngularDamping(mat.GetSpeedMod());
}
}
}
}
}
}
}
}
/// <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);
}
///<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 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);
}
public bool ConvexCast(ConvexShape castShape, ref RigidTransform startingTransform, ref Vector3 sweepnorm, double slen, MaterialSolidity solidness, out RayHit hit)
{
RigidTransform rt;
RigidTransform.MultiplyByInverse(ref startingTransform, ref worldTransform, out rt);
Vector3 swp = Quaternion.Transform(sweepnorm, Quaternion.Inverse(worldTransform.Orientation));
RayHit rh;
bool h = ChunkShape.ConvexCast(castShape, ref rt, ref swp, slen, solidness, out rh);
RigidTransform.Transform(ref rh.Location, ref worldTransform, out hit.Location);
hit.Normal = rh.Normal;
hit.T = rh.T;
return h;
}
/// <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;
}
///<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);
}
}
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
var upExtreme = new Vector3(0, halfLength, 0);
var 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>
/// Refreshes the contact manifold, removing any out of date contacts
/// and updating others.
/// </summary>
public static void ContactRefresh(RawList<Contact> contacts, RawValueList<ContactSupplementData> supplementData, ref RigidTransform transformA, ref RigidTransform transformB, RawList<int> toRemove)
{
//TODO: Could also refresh normals with some trickery.
//Would also need to refresh depth using new normals, and would require some extra information.
for (int k = 0; k < contacts.Count; k++)
{
contacts.Elements[k].Validate();
ContactSupplementData data = supplementData.Elements[k];
System.Numerics.Vector3 newPosA, newPosB;
RigidTransform.Transform(ref data.LocalOffsetA, ref transformA, out newPosA);
RigidTransform.Transform(ref data.LocalOffsetB, ref transformB, out newPosB);
//ab - (ab*n)*n
//Compute the horizontal offset.
System.Numerics.Vector3 ab;
Vector3Ex.Subtract(ref newPosB, ref newPosA, out ab);
float dot;
Vector3Ex.Dot(ref ab, ref contacts.Elements[k].Normal, out dot);
System.Numerics.Vector3 temp;
Vector3Ex.Multiply(ref contacts.Elements[k].Normal, dot, out temp);
Vector3Ex.Subtract(ref ab, ref temp, out temp);
dot = temp.LengthSquared();
if (dot > CollisionDetectionSettings.ContactInvalidationLengthSquared)
{
toRemove.Add(k);
}
else
{
//Depth refresh:
//Find deviation ((Ra-Rb)*N) and add to base depth.
Vector3Ex.Dot(ref ab, ref contacts.Elements[k].Normal, out dot);
contacts.Elements[k].PenetrationDepth = data.BasePenetrationDepth - dot;
if (contacts.Elements[k].PenetrationDepth < -CollisionDetectionSettings.maximumContactDistance)
toRemove.Add(k);
else
{
//Refresh position and ra/rb.
System.Numerics.Vector3 newPos;
Vector3Ex.Add(ref newPosB, ref newPosA, out newPos);
Vector3Ex.Multiply(ref newPos, .5f, out newPos);
contacts.Elements[k].Position = newPos;
//This is an interesting idea, but has very little effect one way or the other.
//data.BasePenetrationDepth = contacts.Elements[k].PenetrationDepth;
//RigidTransform.TransformByInverse(ref newPos, ref transformA, out data.LocalOffsetA);
//RigidTransform.TransformByInverse(ref newPos, ref transformB, out data.LocalOffsetB);
}
contacts.Elements[k].Validate();
}
}
}
///<summary>
/// Gets the closest points between the shapes.
///</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="cachedSimplex">Simplex from a previous updated used to warmstart the current attempt. Updated after each run.</param>
///<param name="closestPointA">Closest point on the first shape to the second shape.</param>
///<param name="closestPointB">Closest point on the second shape to the first shape.</param>
///<returns>Whether or not the objects were intersecting. If they are intersecting, then the closest points cannot be identified.</returns>
public static bool GetClosestPoints(ConvexShape shapeA, ConvexShape shapeB, ref RigidTransform transformA, ref RigidTransform transformB,
ref CachedSimplex cachedSimplex, out System.Numerics.Vector3 closestPointA, out System.Numerics.Vector3 closestPointB)
{
RigidTransform localtransformB;
MinkowskiToolbox.GetLocalTransform(ref transformA, ref transformB, out localtransformB);
bool toReturn = GetClosestPoints(shapeA, shapeB, ref localtransformB, ref cachedSimplex, out closestPointA, out closestPointB);
RigidTransform.Transform(ref closestPointA, ref transformA, out closestPointA);
RigidTransform.Transform(ref closestPointB, ref transformA, out closestPointB);
return(toReturn);
}
///<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="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 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 extremePoint);
Vector3 v;
Vector3 negativeN;
Vector3.Negate(ref direction, out negativeN);
shapeB.GetExtremePointWithoutMargin(negativeN, ref localTransformB, out v);
Vector3.Subtract(ref extremePoint, ref v, out extremePoint);
ExpandMinkowskiSum(shapeA.collisionMargin, shapeB.collisionMargin, ref direction, out v);
Vector3.Add(ref extremePoint, ref v, 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
Matrix3x3 o;
Matrix3x3.CreateFromQuaternion(ref shapeTransform.Orientation, out o);
//Sample the local directions from the orientation matrix, implicitly transposed.
Vector3 right;
var direction = new Vector3(o.M11, o.M21, o.M31);
GetLocalExtremePointWithoutMargin(ref direction, out right);
Vector3 left;
direction = new Vector3(-o.M11, -o.M21, -o.M31);
GetLocalExtremePointWithoutMargin(ref direction, out left);
Vector3 up;
direction = new Vector3(o.M12, o.M22, o.M32);
GetLocalExtremePointWithoutMargin(ref direction, out up);
Vector3 down;
direction = new Vector3(-o.M12, -o.M22, -o.M32);
GetLocalExtremePointWithoutMargin(ref direction, out down);
Vector3 backward;
direction = new Vector3(o.M13, o.M23, o.M33);
GetLocalExtremePointWithoutMargin(ref direction, out backward);
Vector3 forward;
direction = new Vector3(-o.M13, -o.M23, -o.M33);
GetLocalExtremePointWithoutMargin(ref direction, out forward);
//Rather than transforming each axis independently (and doing three times as many operations as required), just get the 6 required values directly.
Vector3 positive, negative;
TransformLocalExtremePoints(ref right, ref up, ref backward, ref o, out positive);
TransformLocalExtremePoints(ref left, ref down, ref forward, ref o, out negative);
//The positive and negative vectors represent the X, Y and Z coordinates of the extreme points in world space along the world space axes.
boundingBox.Max.X = shapeTransform.Position.X + positive.X + collisionMargin;
boundingBox.Max.Y = shapeTransform.Position.Y + positive.Y + collisionMargin;
boundingBox.Max.Z = shapeTransform.Position.Z + positive.Z + collisionMargin;
boundingBox.Min.X = shapeTransform.Position.X + negative.X - collisionMargin;
boundingBox.Min.Y = shapeTransform.Position.Y + negative.Y - collisionMargin;
boundingBox.Min.Z = shapeTransform.Position.Z + negative.Z - collisionMargin;
}
public MobileChunkShape(Vector3i csize, BlockInternal[] blocks, out Vector3 center)
{
Matrix3x3 boxMat = new BoxShape(csize.X, csize.Y, csize.Z).VolumeDistribution;
ChunkSize = csize;
Blocks = blocks;
double weightInv = 1f / blocks.Length;
center = new Vector3(csize.X / 2f, csize.Y / 2f, csize.Z / 2f);
// TODO: More accurately get center of weight based on which blocks are solid or not!?
Matrix3x3 volumeDistribution = new Matrix3x3();
RigidTransform transform = new RigidTransform(center);
Matrix3x3 contribution;
CompoundShape.TransformContribution(ref transform, ref center, ref boxMat, blocks.Length, out contribution);
Matrix3x3.Add(ref volumeDistribution, ref contribution, out volumeDistribution);
Matrix3x3.Multiply(ref volumeDistribution, weightInv, out volumeDistribution);
UpdateEntityShapeVolume(new EntityShapeVolumeDescription() { Volume = csize.X * csize.Y * csize.Z, VolumeDistribution = volumeDistribution });
Center = center;
}
public bool SpecialCaseConvexTrace(ConvexShape shape, Location start, Location dir, double len, MaterialSolidity considerSolid, Func<BroadPhaseEntry, bool> filter, out RayCastResult rayHit)
{
RigidTransform rt = new RigidTransform(start.ToBVector(), BEPUutilities.Quaternion.Identity);
BEPUutilities.Vector3 sweep = (dir * len).ToBVector();
RayCastResult best = new RayCastResult(new RayHit() { T = len }, null);
bool hA = false;
if (considerSolid.HasFlag(MaterialSolidity.FULLSOLID))
{
RayCastResult rcr;
if (PhysicsWorld.ConvexCast(shape, ref rt, ref sweep, filter, out rcr))
{
best = rcr;
hA = true;
}
}
sweep = dir.ToBVector();
AABB box = new AABB();
box.Min = start;
box.Max = start;
box.Include(start + dir * len);
foreach (KeyValuePair<Vector3i, Chunk> chunk in LoadedChunks)
{
if (chunk.Value == null || chunk.Value.FCO == null)
{
continue;
}
if (!box.Intersects(new AABB() { Min = chunk.Value.WorldPosition.ToLocation() * Chunk.CHUNK_SIZE,
Max = chunk.Value.WorldPosition.ToLocation() * Chunk.CHUNK_SIZE + new Location(Chunk.CHUNK_SIZE, Chunk.CHUNK_SIZE, Chunk.CHUNK_SIZE) }))
{
continue;
}
RayHit temp;
if (chunk.Value.FCO.ConvexCast(shape, ref rt, ref sweep, len, considerSolid, out temp))
{
hA = true;
if (temp.T < best.HitData.T)
{
best.HitData = temp;
best.HitObject = chunk.Value.FCO;
}
}
}
rayHit = best;
return hA;
}
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>
/// Gets the closest points between the shapes.
///</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="closestPointA">Closest point on the first shape to the second shape.</param>
///<param name="closestPointB">Closest point on the second shape to the first shape.</param>
///<returns>Whether or not the objects were intersecting. If they are intersecting, then the closest points cannot be identified.</returns>
public static bool GetClosestPoints(ConvexShape shapeA, ConvexShape shapeB, ref RigidTransform transformA, ref RigidTransform transformB,
out System.Numerics.Vector3 closestPointA, out System.Numerics.Vector3 closestPointB)
{
//The cached simplex stores locations that are local to the shapes. A fairly decent initial state is between the centroids of the objects.
//In local space, the centroids are at the origins.
RigidTransform localtransformB;
MinkowskiToolbox.GetLocalTransform(ref transformA, ref transformB, out localtransformB);
var simplex = new CachedSimplex {
State = SimplexState.Point
};
// new CachedSimplex(shapeA, shapeB, ref localtransformB);
bool toReturn = GetClosestPoints(shapeA, shapeB, ref localtransformB, ref simplex, out closestPointA, out closestPointB);
RigidTransform.Transform(ref closestPointA, ref transformA, out closestPointA);
RigidTransform.Transform(ref closestPointB, ref transformA, out closestPointB);
return(toReturn);
}
/// <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(-10, -10, -10), a.Orientation);
bTransform = new RigidTransform(new Vector3(10, 10, 10), b.Orientation);
game.Camera.Position = new Vector3(0, 5, 17);
}
/// <summary>
/// Returns the list of ConvexCollidable's and Entities inside or touching the specified sphere.
/// Result does not include static geometry and non-entity physical objects.
/// </summary>
/// <param name="world"></param>
/// <param name="origin"></param>
/// <param name="radius"></param>
/// <returns></returns>
public List <Entity> WeaponOverlap(Vector3 origin, float radius, Entity entToSkip)
{
BU.BoundingSphere sphere = new BU.BoundingSphere(MathConverter.Convert(origin), radius);
SphereShape sphereShape = new SphereShape(radius);
BU.Vector3 zeroSweep = BU.Vector3.Zero;
BU.RigidTransform rigidXForm = new BU.RigidTransform(MathConverter.Convert(origin));
var candidates = PhysicsResources.GetBroadPhaseEntryList();
PhysSpace.BroadPhase.QueryAccelerator.BroadPhase.QueryAccelerator.GetEntries(sphere, candidates);
var result = new List <Entity>();
foreach (var candidate in candidates)
{
BU.RayHit rayHit;
bool r = candidate.ConvexCast(sphereShape, ref rigidXForm, ref zeroSweep, out rayHit);
if (r)
{
var collidable = candidate as ConvexCollidable;
var entity = collidable == null ? null : collidable.Entity.Tag as Entity;
if (collidable == null)
{
continue;
}
if (entity == null)
{
continue;
}
result.Add(entity);
}
}
result.RemoveAll(e => e == entToSkip);
return(result);
}
/// <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>
/// 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);
}
private static bool GetClosestPoints(ConvexShape shapeA, ConvexShape shapeB, ref RigidTransform localTransformB,
ref CachedSimplex cachedSimplex, out System.Numerics.Vector3 localClosestPointA, out System.Numerics.Vector3 localClosestPointB)
{
var simplex = new PairSimplex(ref cachedSimplex, ref localTransformB);
System.Numerics.Vector3 closestPoint;
int count = 0;
while (true)
{
if (simplex.GetPointClosestToOrigin(out closestPoint) || //Also reduces the simplex and computes barycentric coordinates if necessary.
closestPoint.LengthSquared() <= Toolbox.Epsilon * simplex.errorTolerance)
{
//Intersecting.
localClosestPointA = Toolbox.ZeroVector;
localClosestPointB = Toolbox.ZeroVector;
simplex.UpdateCachedSimplex(ref cachedSimplex);
return(true);
}
if (++count > MaximumGJKIterations)
{
break; //Must break BEFORE a new vertex is added if we're over the iteration limit. This guarantees final simplex is not a tetrahedron.
}
if (simplex.GetNewSimplexPoint(shapeA, shapeB, count, ref closestPoint))
{
//No progress towards origin, not intersecting.
break;
}
}
//Compute closest points from the contributing simplexes and barycentric coordinates
simplex.GetClosestPoints(out localClosestPointA, out localClosestPointB);
//simplex.VerifyContributions();
//if (Vector3Ex.Distance(localClosestPointA - localClosestPointB, closestPoint) > .00001f)
// Debug.WriteLine("break.");
simplex.UpdateCachedSimplex(ref cachedSimplex);
return(false);
}
public override void Update(double dt)
{
NeedsHop = false;
Entity e = Entity;
Vector3 vel = e.LinearVelocity * dt;
RigidTransform start = new RigidTransform(e.Position + new Vector3(0, 0, 0.05f), e.Orientation);
RayCastResult rcr;
if (e.Space.ConvexCast((ConvexShape)e.CollisionInformation.Shape, ref start, ref vel, IgnoreThis, out rcr))
{
vel += new Vector3(0, 0, HopHeight);
if (!e.Space.ConvexCast((ConvexShape)e.CollisionInformation.Shape, ref start, ref vel, IgnoreThis, out rcr))
{
start.Position += vel;
vel = new Vector3(0, 0, -(HopHeight + 0.05f)); // TODO: Track gravity normals and all that stuff
if (e.Space.ConvexCast((ConvexShape)e.CollisionInformation.Shape, ref start, ref vel, IgnoreThis, out rcr))
{
NeedsHop = true;
Hop = -vel * (1f - rcr.HitData.T / (HopHeight + 0.05f));
}
}
}
}
/// <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="result">Hit data, if any.</param>
/// <returns>Whether or not the cast hit anything.</returns>
public bool ConvexCast(ConvexShapes.ConvexShape castShape, ref RigidTransform startingTransform, ref System.Numerics.Vector3 sweep, out RayCastResult result)
{
var outputOverlappedElements = PhysicsResources.GetCollidableList();
BoundingBox boundingBox;
castShape.GetSweptBoundingBox(ref startingTransform, ref sweep, out boundingBox);
CollidableTree.GetOverlaps(boundingBox, outputOverlappedElements);
result = new RayCastResult();
result.HitData.T = float.MaxValue;
for (int i = 0; i < outputOverlappedElements.Count; ++i)
{
RayHit hit;
if (outputOverlappedElements.Elements[i].ConvexCast(castShape, ref startingTransform, ref sweep, out hit))
{
if (hit.T < result.HitData.T)
{
result.HitData = hit;
result.HitObject = outputOverlappedElements.Elements[i];
}
}
}
PhysicsResources.GiveBack(outputOverlappedElements);
return result.HitData.T < float.MaxValue;
}
/// <summary>
/// Gets the bounding box of the shape given a transform.
/// </summary>
/// <param name="shapeTransform">Transform to use.</param>
/// <param name="boundingBox">Bounding box of the transformed shape.</param>
public override void GetBoundingBox(ref RigidTransform shapeTransform, out BoundingBox boundingBox)
{
#if !WINDOWS
boundingBox = new BoundingBox();
#endif
Matrix3x3 o;
Matrix3x3.CreateFromQuaternion(ref shapeTransform.Orientation, out o);
//Sample the local directions from the orientation matrix, implicitly transposed.
//Notice only three directions are used. Due to box symmetry, 'left' is just -right.
var 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>
/// 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);
}
///<summary>
/// Casts a fat (sphere expanded) ray against the shape. If the raycast appears to be stuck in the shape, the cast will be attempted
/// with a smaller ray (scaled by the MotionSettings.CoreShapeScaling each time).
///</summary>
///<param name="ray">Ray to test against the shape.</param>
///<param name="radius">Radius of the ray.</param>
///<param name="target">Shape to test against.</param>
///<param name="shapeTransform">Transform to apply to the shape for the test.</param>
///<param name="maximumLength">Maximum length of the ray in units of the ray direction's length.</param>
///<param name="hit">Hit data of the sphere cast, if any.</param>
///<returns>Whether or not the sphere cast hit the shape.</returns>
public static bool CCDSphereCast( ref Ray ray, float radius, ConvexShape target, ref RigidTransform shapeTransform, float maximumLength,
out RayHit hit )
{
int iterations = 0;
while( true )
{
if( GJKToolbox.SphereCast( ray, radius, target, ref shapeTransform, maximumLength, out hit ) &&
hit.T > 0 )
{
//The ray cast isn't embedded in the shape, and it's less than maximum length away!
return true;
}
if( hit.T > maximumLength || hit.T < 0 )
return false; //Failure showed it was too far, or behind.
radius *= MotionSettings.CoreShapeScaling;
iterations++;
if( iterations > 3 ) //Limit could be configurable.
{
//It's iterated too much, let's just do a last ditch attempt using a raycast and hope that can help.
return GJKToolbox.RayCast( ray, target, ref shapeTransform, maximumLength, out hit ) && hit.T > 0;
}
}
}
///<summary>
/// Casts a fat (sphere expanded) ray against the shape.
///</summary>
///<param name="ray">Ray to test against the shape.</param>
///<param name="radius">Radius of the ray.</param>
///<param name="shape">Shape to test against.</param>
///<param name="shapeTransform">Transform to apply to the shape for the test.</param>
///<param name="maximumLength">Maximum length of the ray in units of the ray direction's length.</param>
///<param name="hit">Hit data of the sphere cast, if any.</param>
///<returns>Whether or not the sphere cast hit the shape.</returns>
public static bool SphereCast(Ray ray, float radius, ConvexShape shape, ref RigidTransform shapeTransform, float maximumLength,
out RayHit hit)
{
//Transform the ray into the object's local space.
Vector3.Subtract(ref ray.Position, ref shapeTransform.Position, out ray.Position);
Quaternion conjugate;
Quaternion.Conjugate(ref shapeTransform.Orientation, out conjugate);
Quaternion.Transform(ref ray.Position, ref conjugate, out ray.Position);
Quaternion.Transform(ref ray.Direction, ref conjugate, out ray.Direction);
Vector3 w, p;
hit.T = 0;
hit.Location = ray.Position;
hit.Normal = Toolbox.ZeroVector;
Vector3 v = hit.Location;
RaySimplex simplex = new RaySimplex();
float vw, vdir;
int count = 0;
//This epsilon has a significant impact on performance and accuracy. Changing it to use BigEpsilon instead increases speed by around 30-40% usually, but jigging is more evident.
while (v.LengthSquared() >= Toolbox.Epsilon * simplex.GetErrorTolerance(ref ray.Position))
{
if (++count > MaximumGJKIterations)
{
//It's taken too long to find a hit. Numerical problems are probable; quit.
hit = new RayHit();
return false;
}
shape.GetLocalExtremePointWithoutMargin(ref v, out p);
Vector3 contribution;
MinkowskiToolbox.ExpandMinkowskiSum(shape.collisionMargin, radius, ref v, out contribution);
Vector3.Add(ref p, ref contribution, out p);
Vector3.Subtract(ref hit.Location, ref p, out w);
Vector3.Dot(ref v, ref w, out vw);
if (vw > 0)
{
Vector3.Dot(ref v, ref ray.Direction, out vdir);
hit.T = hit.T - vw / vdir;
if (vdir >= 0)
{
//We would have to back up!
return false;
}
if (hit.T > maximumLength)
{
//If we've gone beyond where the ray can reach, there's obviously no hit.
return false;
}
//Shift the ray up.
Vector3.Multiply(ref ray.Direction, hit.T, out hit.Location);
Vector3.Add(ref hit.Location, ref ray.Position, out hit.Location);
hit.Normal = v;
}
RaySimplex shiftedSimplex;
simplex.AddNewSimplexPoint(ref p, ref hit.Location, out shiftedSimplex);
shiftedSimplex.GetPointClosestToOrigin(ref simplex, out v);
}
//Transform the hit data into world space.
Quaternion.Transform(ref hit.Normal, ref shapeTransform.Orientation, out hit.Normal);
Quaternion.Transform(ref hit.Location, ref shapeTransform.Orientation, out hit.Location);
Vector3.Add(ref hit.Location, ref shapeTransform.Position, out hit.Location);
return true;
}
///<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>
///<returns>Whether or not the shapes are intersecting.</returns>
public static bool AreShapesIntersecting(ConvexShape shapeA, ConvexShape shapeB, ref RigidTransform transformA, ref RigidTransform transformB)
{
//Zero isn't a very good guess! But it's a cheap guess.
Vector3 separatingAxis = Toolbox.ZeroVector;
return AreShapesIntersecting(shapeA, shapeB, ref transformA, ref transformB, ref separatingAxis);
}
///<summary>
/// Sweeps two shapes against another.
///</summary>
///<param name="shapeA">First shape being swept.</param>
///<param name="shapeB">Second shape being swept.</param>
///<param name="sweepA">Sweep vector for the first shape.</param>
///<param name="sweepB">Sweep vector for the second shape.</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="hit">Hit data of the sweep test, if any.</param>
///<returns>Whether or not the swept shapes hit each other..</returns>
public static bool ConvexCast(ConvexShape shapeA, ConvexShape shapeB, ref Vector3 sweepA, ref Vector3 sweepB, ref RigidTransform transformA, ref RigidTransform transformB,
out RayHit hit)
{
//Put the velocity into shapeA's local space.
Vector3 velocityWorld;
Vector3.Subtract(ref sweepB, ref sweepA, out velocityWorld);
Quaternion conjugateOrientationA;
Quaternion.Conjugate(ref transformA.Orientation, out conjugateOrientationA);
Vector3 rayDirection;
Quaternion.Transform(ref velocityWorld, ref conjugateOrientationA, out rayDirection);
//Transform b into a's local space.
RigidTransform localTransformB;
Quaternion.Concatenate(ref transformB.Orientation, ref conjugateOrientationA, out localTransformB.Orientation);
Vector3.Subtract(ref transformB.Position, ref transformA.Position, out localTransformB.Position);
Quaternion.Transform(ref localTransformB.Position, ref conjugateOrientationA, out localTransformB.Position);
Vector3 w, p;
hit.T = 0;
hit.Location = Vector3.Zero; //The ray starts at the origin.
hit.Normal = Toolbox.ZeroVector;
Vector3 v = hit.Location;
RaySimplex simplex = new RaySimplex();
float vw, vdir;
int count = 0;
do
{
if (++count > MaximumGJKIterations)
{
//It's taken too long to find a hit. Numerical problems are probable; quit.
hit = new RayHit();
return false;
}
MinkowskiToolbox.GetLocalMinkowskiExtremePoint(shapeA, shapeB, ref v, ref localTransformB, out p);
Vector3.Subtract(ref hit.Location, ref p, out w);
Vector3.Dot(ref v, ref w, out vw);
if (vw > 0)
{
Vector3.Dot(ref v, ref rayDirection, out vdir);
if (vdir >= 0)
{
hit = new RayHit();
return false;
}
hit.T = hit.T - vw / vdir;
if (hit.T > 1)
{
//If we've gone beyond where the ray can reach, there's obviously no hit.
hit = new RayHit();
return false;
}
//Shift the ray up.
Vector3.Multiply(ref rayDirection, hit.T, out hit.Location);
//The ray origin is the origin! Don't need to add any ray position.
hit.Normal = v;
}
RaySimplex shiftedSimplex;
simplex.AddNewSimplexPoint(ref p, ref hit.Location, out shiftedSimplex);
shiftedSimplex.GetPointClosestToOrigin(ref simplex, out v);
//Could measure the progress of the ray. If it's too little, could early out.
//Not used by default since it's biased towards precision over performance.
} while (v.LengthSquared() >= Toolbox.Epsilon * simplex.GetErrorTolerance(ref Toolbox.ZeroVector));
//This epsilon has a significant impact on performance and accuracy. Changing it to use BigEpsilon instead increases speed by around 30-40% usually, but jigging is more evident.
//Transform the hit data into world space.
Quaternion.Transform(ref hit.Normal, ref transformA.Orientation, out hit.Normal);
Vector3.Multiply(ref velocityWorld, hit.T, out hit.Location);
Vector3.Add(ref hit.Location, ref transformA.Position, out hit.Location);
return true;
}
//TODO: Consider changing the termination epsilons on these casts. Epsilon * Modifier is okay, but there might be better options.
///<summary>
/// Tests a ray against a convex shape.
///</summary>
///<param name="ray">Ray to test against the shape.</param>
///<param name="shape">Shape to test.</param>
///<param name="shapeTransform">Transform to apply to the shape for the test.</param>
///<param name="maximumLength">Maximum length of the ray in units of the ray direction's length.</param>
///<param name="hit">Hit data of the ray cast, if any.</param>
///<returns>Whether or not the ray hit the shape.</returns>
public static bool RayCast(Ray ray, ConvexShape shape, ref RigidTransform shapeTransform, float maximumLength,
out RayHit hit)
{
//Transform the ray into the object's local space.
Vector3Ex.Subtract(ref ray.Position, ref shapeTransform.Position, out ray.Position);
System.Numerics.Quaternion conjugate;
QuaternionEx.Conjugate(ref shapeTransform.Orientation, out conjugate);
QuaternionEx.Transform(ref ray.Position, ref conjugate, out ray.Position);
QuaternionEx.Transform(ref ray.Direction, ref conjugate, out ray.Direction);
System.Numerics.Vector3 extremePointToRayOrigin, extremePoint;
hit.T = 0;
hit.Location = ray.Position;
hit.Normal = Toolbox.ZeroVector;
System.Numerics.Vector3 closestOffset = hit.Location;
RaySimplex simplex = new RaySimplex();
float vw, closestPointDotDirection;
int count = 0;
//This epsilon has a significant impact on performance and accuracy. Changing it to use BigEpsilon instead increases speed by around 30-40% usually, but jigging is more evident.
while (closestOffset.LengthSquared() >= Toolbox.Epsilon * simplex.GetErrorTolerance(ref ray.Position))
{
if (++count > MaximumGJKIterations)
{
//It's taken too long to find a hit. Numerical problems are probable; quit.
hit = new RayHit();
return(false);
}
shape.GetLocalExtremePoint(closestOffset, out extremePoint);
Vector3Ex.Subtract(ref hit.Location, ref extremePoint, out extremePointToRayOrigin);
Vector3Ex.Dot(ref closestOffset, ref extremePointToRayOrigin, out vw);
//If the closest offset and the extreme point->ray origin direction point the same way,
//then we might be able to conservatively advance the point towards the surface.
if (vw > 0)
{
Vector3Ex.Dot(ref closestOffset, ref ray.Direction, out closestPointDotDirection);
if (closestPointDotDirection >= 0)
{
hit = new RayHit();
return(false);
}
hit.T = hit.T - vw / closestPointDotDirection;
if (hit.T > maximumLength)
{
//If we've gone beyond where the ray can reach, there's obviously no hit.
hit = new RayHit();
return(false);
}
//Shift the ray up.
Vector3Ex.Multiply(ref ray.Direction, hit.T, out hit.Location);
Vector3Ex.Add(ref hit.Location, ref ray.Position, out hit.Location);
hit.Normal = closestOffset;
}
RaySimplex shiftedSimplex;
simplex.AddNewSimplexPoint(ref extremePoint, ref hit.Location, out shiftedSimplex);
//Compute the offset from the simplex surface to the origin.
shiftedSimplex.GetPointClosestToOrigin(ref simplex, out closestOffset);
}
//Transform the hit data into world space.
QuaternionEx.Transform(ref hit.Normal, ref shapeTransform.Orientation, out hit.Normal);
QuaternionEx.Transform(ref hit.Location, ref shapeTransform.Orientation, out hit.Location);
Vector3Ex.Add(ref hit.Location, ref shapeTransform.Position, out hit.Location);
return(true);
}
public static void Validate(this RigidTransform r)
{
r.Position.Validate();
r.Orientation.Validate();
}
///<summary>
/// Sweeps a shape against another shape using a given sweep vector.
///</summary>
///<param name="sweptShape">Shape to sweep.</param>
///<param name="target">Shape being swept against.</param>
///<param name="sweep">Sweep vector for the sweptShape.</param>
///<param name="startingSweptTransform">Starting transform of the sweptShape.</param>
///<param name="targetTransform">Transform to apply to the target shape.</param>
///<param name="hit">Hit data of the sweep test, if any.</param>
///<returns>Whether or not the swept shape hit the other shape.</returns>
public static bool ConvexCast(ConvexShape sweptShape, ConvexShape target, ref System.Numerics.Vector3 sweep, ref RigidTransform startingSweptTransform, ref RigidTransform targetTransform,
out RayHit hit)
{
return(ConvexCast(sweptShape, target, ref sweep, ref Toolbox.ZeroVector, ref startingSweptTransform, ref targetTransform, out hit));
}
///<summary>
/// Sweeps two shapes against another.
///</summary>
///<param name="shapeA">First shape being swept.</param>
///<param name="shapeB">Second shape being swept.</param>
///<param name="sweepA">Sweep vector for the first shape.</param>
///<param name="sweepB">Sweep vector for the second shape.</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="hit">Hit data of the sweep test, if any.</param>
///<returns>Whether or not the swept shapes hit each other..</returns>
public static bool ConvexCast(ConvexShape shapeA, ConvexShape shapeB, ref System.Numerics.Vector3 sweepA, ref System.Numerics.Vector3 sweepB, ref RigidTransform transformA, ref RigidTransform transformB,
out RayHit hit)
{
//Put the velocity into shapeA's local space.
System.Numerics.Vector3 velocityWorld;
Vector3Ex.Subtract(ref sweepB, ref sweepA, out velocityWorld);
System.Numerics.Quaternion conjugateOrientationA;
QuaternionEx.Conjugate(ref transformA.Orientation, out conjugateOrientationA);
System.Numerics.Vector3 rayDirection;
QuaternionEx.Transform(ref velocityWorld, ref conjugateOrientationA, out rayDirection);
//Transform b into a's local space.
RigidTransform localTransformB;
QuaternionEx.Concatenate(ref transformB.Orientation, ref conjugateOrientationA, out localTransformB.Orientation);
Vector3Ex.Subtract(ref transformB.Position, ref transformA.Position, out localTransformB.Position);
QuaternionEx.Transform(ref localTransformB.Position, ref conjugateOrientationA, out localTransformB.Position);
System.Numerics.Vector3 w, p;
hit.T = 0;
hit.Location = System.Numerics.Vector3.Zero; //The ray starts at the origin.
hit.Normal = Toolbox.ZeroVector;
System.Numerics.Vector3 v = hit.Location;
RaySimplex simplex = new RaySimplex();
float vw, vdir;
int count = 0;
do
{
if (++count > MaximumGJKIterations)
{
//It's taken too long to find a hit. Numerical problems are probable; quit.
hit = new RayHit();
return(false);
}
MinkowskiToolbox.GetLocalMinkowskiExtremePoint(shapeA, shapeB, ref v, ref localTransformB, out p);
Vector3Ex.Subtract(ref hit.Location, ref p, out w);
Vector3Ex.Dot(ref v, ref w, out vw);
if (vw > 0)
{
Vector3Ex.Dot(ref v, ref rayDirection, out vdir);
if (vdir >= 0)
{
hit = new RayHit();
return(false);
}
hit.T = hit.T - vw / vdir;
if (hit.T > 1)
{
//If we've gone beyond where the ray can reach, there's obviously no hit.
hit = new RayHit();
return(false);
}
//Shift the ray up.
Vector3Ex.Multiply(ref rayDirection, hit.T, out hit.Location);
//The ray origin is the origin! Don't need to add any ray position.
hit.Normal = v;
}
RaySimplex shiftedSimplex;
simplex.AddNewSimplexPoint(ref p, ref hit.Location, out shiftedSimplex);
shiftedSimplex.GetPointClosestToOrigin(ref simplex, out v);
//Could measure the progress of the ray. If it's too little, could early out.
//Not used by default since it's biased towards precision over performance.
} while (v.LengthSquared() >= Toolbox.Epsilon * simplex.GetErrorTolerance(ref Toolbox.ZeroVector));
//This epsilon has a significant impact on performance and accuracy. Changing it to use BigEpsilon instead increases speed by around 30-40% usually, but jigging is more evident.
//Transform the hit data into world space.
QuaternionEx.Transform(ref hit.Normal, ref transformA.Orientation, out hit.Normal);
Vector3Ex.Multiply(ref velocityWorld, hit.T, out hit.Location);
Vector3Ex.Add(ref hit.Location, ref transformA.Position, out hit.Location);
return(true);
}
///<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>
///<returns>Whether or not the shapes are intersecting.</returns>
public static bool AreShapesIntersecting(ConvexShape shapeA, ConvexShape shapeB, ref RigidTransform transformA, ref RigidTransform transformB)
{
//Zero isn't a very good guess! But it's a cheap guess.
System.Numerics.Vector3 separatingAxis = Toolbox.ZeroVector;
return(AreShapesIntersecting(shapeA, shapeB, ref transformA, ref transformB, ref separatingAxis));
}
///<summary>
/// Casts a fat (sphere expanded) ray against the shape.
///</summary>
///<param name="ray">Ray to test against the shape.</param>
///<param name="radius">Radius of the ray.</param>
///<param name="shape">Shape to test against.</param>
///<param name="shapeTransform">Transform to apply to the shape for the test.</param>
///<param name="maximumLength">Maximum length of the ray in units of the ray direction's length.</param>
///<param name="hit">Hit data of the sphere cast, if any.</param>
///<returns>Whether or not the sphere cast hit the shape.</returns>
public static bool SphereCast(Ray ray, float radius, ConvexShape shape, ref RigidTransform shapeTransform, float maximumLength,
out RayHit hit)
{
//Transform the ray into the object's local space.
Vector3Ex.Subtract(ref ray.Position, ref shapeTransform.Position, out ray.Position);
System.Numerics.Quaternion conjugate;
QuaternionEx.Conjugate(ref shapeTransform.Orientation, out conjugate);
QuaternionEx.Transform(ref ray.Position, ref conjugate, out ray.Position);
QuaternionEx.Transform(ref ray.Direction, ref conjugate, out ray.Direction);
System.Numerics.Vector3 w, p;
hit.T = 0;
hit.Location = ray.Position;
hit.Normal = Toolbox.ZeroVector;
System.Numerics.Vector3 v = hit.Location;
RaySimplex simplex = new RaySimplex();
float vw, vdir;
int count = 0;
//This epsilon has a significant impact on performance and accuracy. Changing it to use BigEpsilon instead increases speed by around 30-40% usually, but jigging is more evident.
while (v.LengthSquared() >= Toolbox.Epsilon * simplex.GetErrorTolerance(ref ray.Position))
{
if (++count > MaximumGJKIterations)
{
//It's taken too long to find a hit. Numerical problems are probable; quit.
hit = new RayHit();
return(false);
}
shape.GetLocalExtremePointWithoutMargin(ref v, out p);
System.Numerics.Vector3 contribution;
MinkowskiToolbox.ExpandMinkowskiSum(shape.collisionMargin, radius, ref v, out contribution);
Vector3Ex.Add(ref p, ref contribution, out p);
Vector3Ex.Subtract(ref hit.Location, ref p, out w);
Vector3Ex.Dot(ref v, ref w, out vw);
if (vw > 0)
{
Vector3Ex.Dot(ref v, ref ray.Direction, out vdir);
hit.T = hit.T - vw / vdir;
if (vdir >= 0)
{
//We would have to back up!
return(false);
}
if (hit.T > maximumLength)
{
//If we've gone beyond where the ray can reach, there's obviously no hit.
return(false);
}
//Shift the ray up.
Vector3Ex.Multiply(ref ray.Direction, hit.T, out hit.Location);
Vector3Ex.Add(ref hit.Location, ref ray.Position, out hit.Location);
hit.Normal = v;
}
RaySimplex shiftedSimplex;
simplex.AddNewSimplexPoint(ref p, ref hit.Location, out shiftedSimplex);
shiftedSimplex.GetPointClosestToOrigin(ref simplex, out v);
}
//Transform the hit data into world space.
QuaternionEx.Transform(ref hit.Normal, ref shapeTransform.Orientation, out hit.Normal);
QuaternionEx.Transform(ref hit.Location, ref shapeTransform.Orientation, out hit.Location);
Vector3Ex.Add(ref hit.Location, ref shapeTransform.Position, out hit.Location);
return(true);
}
///<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 System.Numerics.Vector3 localSeparatingAxis)
{
RigidTransform localtransformB;
MinkowskiToolbox.GetLocalTransform(ref transformA, ref transformB, out localtransformB);
//Warm start the simplex.
var simplex = new SimpleSimplex();
System.Numerics.Vector3 extremePoint;
MinkowskiToolbox.GetLocalMinkowskiExtremePoint(shapeA, shapeB, ref localSeparatingAxis, ref localtransformB, out extremePoint);
simplex.AddNewSimplexPoint(ref extremePoint);
System.Numerics.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.
System.Numerics.Vector3 direction;
Vector3Ex.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;
Vector3Ex.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);
}
