/// <summary>
/// Computes the intersection, if any, between a ray and the objects in the character's bounding box.
/// </summary>
/// <param name="ray">Ray to test.</param>
/// <param name="length">Length of the ray to use in units of the ray's length.</param>
/// <param name="earliestHit">Earliest intersection location and information.</param>
/// <param name="hitObject">Collidable intersected by the ray, if any.</param>
/// <returns>Whether or not the ray hit anything.</returns>
public bool RayCast(Ray ray, float length, out RayHit earliestHit, out Collidable hitObject)
{
earliestHit = new RayHit();
earliestHit.T = float.MaxValue;
hitObject = null;
foreach (var collidable in characterBody.CollisionInformation.OverlappedCollidables)
{
//Check to see if the collidable is hit by the ray.
float t;
if (ray.Intersects(ref collidable.boundingBox, out t) && t < length)
{
//Is it an earlier hit than the current earliest?
RayHit hit;
if (collidable.RayCast(ray, length, SupportRayFilter, out hit) && hit.T < earliestHit.T)
{
earliestHit = hit;
hitObject = collidable;
}
}
}
if (earliestHit.T == float.MaxValue)
return false;
return true;
}
///<summary>
/// Tests a ray against the triangle mesh.
///</summary>
///<param name="ray">Ray to test against the mesh.</param>
///<param name="hitCount">Number of hits between the ray and the mesh.</param>
///<returns>Whether or not the ray hit the mesh.</returns>
public bool RayCast(Ray ray, out int hitCount)
{
var rayHits = CommonResources.GetRayHitList();
bool toReturn = RayCast(ray, rayHits);
hitCount = rayHits.Count;
CommonResources.GiveBack(rayHits);
return toReturn;
}
/// <summary>
/// Finds all intersections between the ray and broad phase entries.
/// </summary>
/// <param name="ray">Ray to test against the structure.</param>
/// <param name="maximumLength">Maximum length of the ray in units of the ray's direction's length.</param>
/// <param name="entries">Entries which have bounding boxes that overlap the ray.</param>
public bool RayCast(Ray ray, float maximumLength, IList<BroadPhaseEntry> entries)
{
if (hierarchy.root != null)
{
hierarchy.root.GetOverlaps(ref ray, maximumLength, entries);
return entries.Count > 0;
}
return false;
}
/// <summary>
///
/// </summary>
/// <param name="from"></param>
/// <param name="to"></param>
/// <param name="normal"></param>
/// <param name="pos"></param>
/// <returns></returns>
public bool RayCastAgainstAll( Vector3 from, Vector3 to, out Vector3 normal, out Vector3 pos, out Entity hitEntity, Entity skipEntity = null )
{
hitEntity = null;
var dir = to - from;
var dist = dir.Length();
var ndir = dir.Normalized();
Ray ray = new Ray( from, ndir );
normal = Vector3.Zero;
pos = to;
Func<BroadPhaseEntry, bool> filterFunc = delegate(BroadPhaseEntry bpe)
{
if (skipEntity==null) return true;
ConvexCollidable cc = bpe as ConvexCollidable;
if (cc==null) return true;
Entity ent = cc.Entity.Tag as Entity;
if (ent==null) return true;
if (ent==skipEntity) return false;
return true;
};
var rcr = new RayCastResult();
var bRay = MathConverter.Convert( ray );
bool result = physSpace.RayCast( bRay, dist, filterFunc, out rcr );
if (!result) {
return false;
}
var convex = rcr.HitObject as ConvexCollidable;
normal = MathConverter.Convert( rcr.HitData.Normal ).Normalized();
pos = MathConverter.Convert( rcr.HitData.Location );
hitEntity = (convex == null) ? null : convex.Entity.Tag as Entity;
return true;
}
/// <summary>
/// Tests a ray against the collidable.
/// </summary>
/// <param name="ray">Ray to test.</param>
/// <param name="maximumLength">Maximum length, in units of the ray's direction's length, to test.</param>
/// <param name="result">Hit data, if any.</param>
/// <returns>Whether or not the ray hit the entry.</returns>
public bool RayCast(Ray ray, float maximumLength, out RayCastResult result)
{
var outputOverlappedElements = PhysicsResources.GetCollidableList();
CollidableTree.GetOverlaps(ray, maximumLength, outputOverlappedElements);
result = new RayCastResult();
result.HitData.T = float.MaxValue;
for (int i = 0; i < outputOverlappedElements.Count; ++i)
{
RayHit hit;
if (outputOverlappedElements.Elements[i].RayCast(ray, maximumLength, 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>
/// Finds the intersection between the given ray and the given plane.
/// </summary>
/// <param name="ray">Ray to test against the plane.</param>
/// <param name="p">Plane for comparison.</param>
/// <param name="t">Interval along line to intersection (A + t * AB).</param>
/// <param name="q">Intersection point.</param>
/// <returns>Whether or not the line intersects the plane. If false, the line is parallel to the plane's surface.</returns>
public static bool GetRayPlaneIntersection(ref Ray ray, ref Plane p, out float t, out Vector3 q)
{
float denominator;
Vector3.Dot(ref p.Normal, ref ray.Direction, out denominator);
if (denominator < Epsilon && denominator > -Epsilon)
{
//Surface of plane and line are parallel (or very close to it).
q = new Vector3();
t = float.MaxValue;
return false;
}
float numerator;
Vector3.Dot(ref p.Normal, ref ray.Position, out numerator);
t = (p.D - numerator) / denominator;
//Compute the intersection position.
Vector3.Multiply(ref ray.Direction, t, out q);
Vector3.Add(ref ray.Position, ref q, out q);
return t >= 0;
}
/// <summary>
/// Determines the intersection between a ray and a triangle.
/// </summary>
/// <param name="ray">Ray to test.</param>
/// <param name="maximumLength">Maximum length to travel in units of the direction's length.</param>
/// <param name="sidedness">Sidedness of the triangle to test.</param>
/// <param name="a">First vertex of the triangle.</param>
/// <param name="b">Second vertex of the triangle.</param>
/// <param name="c">Third vertex of the triangle.</param>
/// <param name="hit">Hit data of the ray, if any</param>
/// <returns>Whether or not the ray and triangle intersect.</returns>
public static bool FindRayTriangleIntersection(ref Ray ray, float maximumLength, TriangleSidedness sidedness, ref Vector3 a, ref Vector3 b, ref Vector3 c, out RayHit hit)
{
hit = new RayHit();
Vector3 ab, ac;
Vector3.Subtract(ref b, ref a, out ab);
Vector3.Subtract(ref c, ref a, out ac);
Vector3.Cross(ref ab, ref ac, out hit.Normal);
if (hit.Normal.LengthSquared() < Epsilon)
return false; //Degenerate triangle!
float d;
Vector3.Dot(ref ray.Direction, ref hit.Normal, out d);
d = -d;
switch (sidedness)
{
case TriangleSidedness.DoubleSided:
if (d <= 0) //Pointing the wrong way. Flip the normal.
{
Vector3.Negate(ref hit.Normal, out hit.Normal);
d = -d;
}
break;
case TriangleSidedness.Clockwise:
if (d <= 0) //Pointing the wrong way. Can't hit.
return false;
break;
case TriangleSidedness.Counterclockwise:
if (d >= 0) //Pointing the wrong way. Can't hit.
return false;
Vector3.Negate(ref hit.Normal, out hit.Normal);
d = -d;
break;
}
Vector3 ap;
Vector3.Subtract(ref ray.Position, ref a, out ap);
Vector3.Dot(ref ap, ref hit.Normal, out hit.T);
hit.T /= d;
if (hit.T < 0 || hit.T > maximumLength)
return false;//Hit is behind origin, or too far away.
Vector3.Multiply(ref ray.Direction, hit.T, out hit.Location);
Vector3.Add(ref ray.Position, ref hit.Location, out hit.Location);
// Compute barycentric coordinates
Vector3.Subtract(ref hit.Location, ref a, out ap);
float ABdotAB, ABdotAC, ABdotAP;
float ACdotAC, ACdotAP;
Vector3.Dot(ref ab, ref ab, out ABdotAB);
Vector3.Dot(ref ab, ref ac, out ABdotAC);
Vector3.Dot(ref ab, ref ap, out ABdotAP);
Vector3.Dot(ref ac, ref ac, out ACdotAC);
Vector3.Dot(ref ac, ref ap, out ACdotAP);
float denom = 1 / (ABdotAB * ACdotAC - ABdotAC * ABdotAC);
float u = (ACdotAC * ABdotAP - ABdotAC * ACdotAP) * denom;
float v = (ABdotAB * ACdotAP - ABdotAC * ABdotAP) * denom;
return (u >= -Toolbox.BigEpsilon) && (v >= -Toolbox.BigEpsilon) && (u + v <= 1 + Toolbox.BigEpsilon);
}
///<summary>
/// Tests a ray against the triangle mesh.
///</summary>
///<param name="ray">Ray to test against the mesh.</param>
/// <param name="maximumLength">Maximum length of the ray in units of the ray direction's length.</param>
/// <param name="sidedness">Sidedness to apply to the mesh for the ray cast.</param>
///<param name="rayHit">Hit data for the ray, if any.</param>
///<returns>Whether or not the ray hit the mesh.</returns>
public bool RayCast(Ray ray, float maximumLength, TriangleSidedness sidedness, out RayHit rayHit)
{
var rayHits = CommonResources.GetRayHitList();
bool toReturn = RayCast(ray, maximumLength, sidedness, rayHits);
if (toReturn)
{
rayHit = rayHits[0];
for (int i = 1; i < rayHits.Count; i++)
{
RayHit hit = rayHits[i];
if (hit.T < rayHit.T)
rayHit = hit;
}
}
else
rayHit = new RayHit();
CommonResources.GiveBack(rayHits);
return toReturn;
}
/// <summary>
/// Determines if a ray intersects any object in the character's bounding box.
/// </summary>
/// <param name="ray">Ray to test.</param>
/// <param name="length">Length of the ray to use in units of the ray's length.</param>
/// <returns>Whether or not the ray hit anything.</returns>
public bool RayCastHitAnything(ref Ray ray, float length)
{
foreach (var collidable in characterBody.CollisionInformation.OverlappedCollidables)
{
//Check to see if the collidable is hit by the ray.
float t;
if (ray.Intersects(ref collidable.boundingBox, out t) && t < length)
{
RayHit hit;
if (collidable.RayCast(ray, length, SupportRayFilter, out hit))
{
return true;
}
}
}
return false;
}
bool TryDownCast(ref Ray ray, float length, out bool hasTraction, out SupportRayData supportRayData)
{
RayHit earliestHit;
Collidable earliestHitObject;
supportRayData = new SupportRayData();
hasTraction = false;
if (QueryManager.RayCast(ray, length, out earliestHit, out earliestHitObject))
{
float lengthSquared = earliestHit.Normal.LengthSquared();
if (lengthSquared < Toolbox.Epsilon)
{
//Don't try to continue if the support ray is stuck in something.
return false;
}
Vector3Ex.Divide(ref earliestHit.Normal, (float)Math.Sqrt(lengthSquared), out earliestHit.Normal);
//A collidable was hit! It's a support, but does it provide traction?
earliestHit.Normal.Normalize();
float dot;
Vector3Ex.Dot(ref ray.Direction, ref earliestHit.Normal, out dot);
if (dot < 0)
{
//Calibrate the normal so it always faces the same direction relative to the body.
Vector3Ex.Negate(ref earliestHit.Normal, out earliestHit.Normal);
dot = -dot;
}
//This down cast is only used for finding supports and traction, not for finding side contacts.
//If the detected normal is too steep, toss it out.
if (dot > ContactCategorizer.TractionThreshold)
{
//It has traction!
hasTraction = true;
supportRayData = new SupportRayData { HitData = earliestHit, HitObject = earliestHitObject, HasTraction = true };
}
else if (dot > ContactCategorizer.SupportThreshold)
supportRayData = new SupportRayData { HitData = earliestHit, HitObject = earliestHitObject };
else
return false; //Too steep! Toss it out.
return true;
}
return false;
}
/// <summary>
/// Tests a ray against the entry.
/// </summary>
/// <param name="ray">Ray to test.</param>
/// <param name="maximumLength">Maximum length, in units of the ray's direction's length, to test.</param>
/// <param name="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="rayHit">Hit location of the ray on the entry, if any.</param>
/// <returns>Whether or not the ray hit the entry.</returns>
public virtual bool RayCast(Ray ray, float maximumLength, Func<BroadPhaseEntry, bool> filter, out RayHit rayHit)
{
if (filter(this))
return RayCast(ray, maximumLength, out rayHit);
rayHit = new RayHit();
return false;
}
/// <summary>
/// Tests a ray against the entry.
/// </summary>
/// <param name="ray">Ray to test.</param>
/// <param name="maximumLength">Maximum length, in units of the ray's direction's length, to test.</param>
/// <param name="rayHit">Hit location of the ray on the entry, if any.</param>
/// <returns>Whether or not the ray hit the entry.</returns>
public override bool RayCast(Ray ray, float maximumLength, out RayHit rayHit)
{
//Put the ray into local space.
Ray localRay;
Matrix3x3 orientation;
Matrix3x3.CreateFromQuaternion(ref worldTransform.Orientation, out orientation);
Matrix3x3.TransformTranspose(ref ray.Direction, ref orientation, out localRay.Direction);
Vector3.Subtract(ref ray.Position, ref worldTransform.Position, out localRay.Position);
Matrix3x3.TransformTranspose(ref localRay.Position, ref orientation, out localRay.Position);
if (Shape.solidity == MobileMeshSolidity.Solid)
{
//Find all hits. Use the count to determine the ray started inside or outside.
//If it starts inside and we're in 'solid' mode, then return the ray start.
//The raycast must be of infinite length at first. This allows it to determine
//if it is inside or outside.
if (Shape.IsLocalRayOriginInMesh(ref localRay, out rayHit))
{
//It was inside!
rayHit = new RayHit() { Location = ray.Position, Normal = Vector3.Zero, T = 0 };
return true;
}
else
{
if (rayHit.T < maximumLength)
{
//Transform the hit into world space.
Vector3.Multiply(ref ray.Direction, rayHit.T, out rayHit.Location);
Vector3.Add(ref rayHit.Location, ref ray.Position, out rayHit.Location);
Matrix3x3.Transform(ref rayHit.Normal, ref orientation, out rayHit.Normal);
}
else
{
//The hit was too far away, or there was no hit (in which case T would be float.MaxValue).
return false;
}
return true;
}
}
else
{
//Just do a normal raycast since the object isn't solid.
TriangleSidedness sidedness;
switch (Shape.solidity)
{
case MobileMeshSolidity.Clockwise:
sidedness = TriangleSidedness.Clockwise;
break;
case MobileMeshSolidity.Counterclockwise:
sidedness = TriangleSidedness.Counterclockwise;
break;
default:
sidedness = TriangleSidedness.DoubleSided;
break;
}
if (Shape.TriangleMesh.RayCast(localRay, maximumLength, sidedness, out rayHit))
{
//Transform the hit into world space.
Vector3.Multiply(ref ray.Direction, rayHit.T, out rayHit.Location);
Vector3.Add(ref rayHit.Location, ref ray.Position, out rayHit.Location);
Matrix3x3.Transform(ref rayHit.Normal, ref orientation, out rayHit.Normal);
return true;
}
}
rayHit = new RayHit();
return false;
}
/// <summary>
/// Gets the intersection between the convex shape and the ray.
/// </summary>
/// <param name="ray">Ray to test.</param>
/// <param name="transform">Transform of the convex shape.</param>
/// <param name="maximumLength">Maximum distance to travel in units of the ray direction's length.</param>
/// <param name="hit">Ray hit data, if any.</param>
/// <returns>Whether or not the ray hit the target.</returns>
public override bool RayTest(ref Ray ray, ref RigidTransform transform, float maximumLength, out RayHit hit)
{
//Put the ray into local space.
Quaternion conjugate;
Quaternion.Conjugate(ref transform.Orientation, out conjugate);
Ray localRay;
Vector3.Subtract(ref ray.Position, ref transform.Position, out localRay.Position);
Quaternion.Transform(ref localRay.Position, ref conjugate, out localRay.Position);
Quaternion.Transform(ref ray.Direction, ref conjugate, out localRay.Direction);
//Check for containment.
if (localRay.Position.Y >= -halfHeight && localRay.Position.Y <= halfHeight && localRay.Position.X * localRay.Position.X + localRay.Position.Z * localRay.Position.Z <= radius * radius)
{
//It's inside!
hit.T = 0;
hit.Location = localRay.Position;
hit.Normal = new Vector3(hit.Location.X, 0, hit.Location.Z);
float normalLengthSquared = hit.Normal.LengthSquared();
if (normalLengthSquared > 1e-9f)
Vector3.Divide(ref hit.Normal, (float)Math.Sqrt(normalLengthSquared), out hit.Normal);
else
hit.Normal = new Vector3();
//Pull the hit into world space.
Quaternion.Transform(ref hit.Normal, ref transform.Orientation, out hit.Normal);
RigidTransform.Transform(ref hit.Location, ref transform, out hit.Location);
return true;
}
//Project the ray direction onto the plane where the cylinder is a circle.
//The projected ray is then tested against the circle to compute the time of impact.
//That time of impact is used to compute the 3d hit location.
Vector2 planeDirection = new Vector2(localRay.Direction.X, localRay.Direction.Z);
float planeDirectionLengthSquared = planeDirection.LengthSquared();
if (planeDirectionLengthSquared < Toolbox.Epsilon)
{
//The ray is nearly parallel with the axis.
//Skip the cylinder-sides test. We're either inside the cylinder and won't hit the sides, or we're outside
//and won't hit the sides.
if (localRay.Position.Y > halfHeight)
goto upperTest;
if (localRay.Position.Y < -halfHeight)
goto lowerTest;
hit = new RayHit();
return false;
}
Vector2 planeOrigin = new Vector2(localRay.Position.X, localRay.Position.Z);
float dot;
Vector2.Dot(ref planeDirection, ref planeOrigin, out dot);
float closestToCenterT = -dot / planeDirectionLengthSquared;
Vector2 closestPoint;
Vector2.Multiply(ref planeDirection, closestToCenterT, out closestPoint);
Vector2.Add(ref planeOrigin, ref closestPoint, out closestPoint);
//How close does the ray come to the circle?
float squaredDistance = closestPoint.LengthSquared();
if (squaredDistance > radius * radius)
{
//It's too far! The ray cannot possibly hit the capsule.
hit = new RayHit();
return false;
}
//With the squared distance, compute the distance backward along the ray from the closest point on the ray to the axis.
float backwardsDistance = radius * (float)Math.Sqrt(1 - squaredDistance / (radius * radius));
float tOffset = backwardsDistance / (float)Math.Sqrt(planeDirectionLengthSquared);
hit.T = closestToCenterT - tOffset;
//Compute the impact point on the infinite cylinder in 3d local space.
Vector3.Multiply(ref localRay.Direction, hit.T, out hit.Location);
Vector3.Add(ref hit.Location, ref localRay.Position, out hit.Location);
//Is it intersecting the cylindrical portion of the capsule?
if (hit.Location.Y <= halfHeight && hit.Location.Y >= -halfHeight && hit.T < maximumLength)
{
//Yup!
hit.Normal = new Vector3(hit.Location.X, 0, hit.Location.Z);
float normalLengthSquared = hit.Normal.LengthSquared();
if (normalLengthSquared > 1e-9f)
Vector3.Divide(ref hit.Normal, (float)Math.Sqrt(normalLengthSquared), out hit.Normal);
else
hit.Normal = new Vector3();
//Pull the hit into world space.
Quaternion.Transform(ref hit.Normal, ref transform.Orientation, out hit.Normal);
RigidTransform.Transform(ref hit.Location, ref transform, out hit.Location);
return true;
}
if (hit.Location.Y < halfHeight)
goto lowerTest;
upperTest:
//Nope! It may be intersecting the ends of the cylinder though.
//We're above the cylinder, so cast a ray against the upper cap.
if (localRay.Direction.Y > -1e-9)
{
//Can't hit the upper cap if the ray isn't pointing down.
hit = new RayHit();
return false;
}
float t = (halfHeight - localRay.Position.Y) / localRay.Direction.Y;
Vector3 planeIntersection;
Vector3.Multiply(ref localRay.Direction, t, out planeIntersection);
Vector3.Add(ref localRay.Position, ref planeIntersection, out planeIntersection);
if(planeIntersection.X * planeIntersection.X + planeIntersection.Z * planeIntersection.Z < radius * radius + 1e-9 && t < maximumLength)
{
//Pull the hit into world space.
Quaternion.Transform(ref Toolbox.UpVector, ref transform.Orientation, out hit.Normal);
RigidTransform.Transform(ref planeIntersection, ref transform, out hit.Location);
hit.T = t;
return true;
}
//No intersection! We can't be hitting the other sphere, so it's over!
hit = new RayHit();
return false;
lowerTest:
//Is it intersecting the bottom cap?
if (localRay.Direction.Y < 1e-9)
{
//Can't hit the bottom cap if the ray isn't pointing up.
hit = new RayHit();
return false;
}
t = (-halfHeight - localRay.Position.Y) / localRay.Direction.Y;
Vector3.Multiply(ref localRay.Direction, t, out planeIntersection);
Vector3.Add(ref localRay.Position, ref planeIntersection, out planeIntersection);
if (planeIntersection.X * planeIntersection.X + planeIntersection.Z * planeIntersection.Z < radius * radius + 1e-9 && t < maximumLength)
{
//Pull the hit into world space.
Quaternion.Transform(ref Toolbox.DownVector, ref transform.Orientation, out hit.Normal);
RigidTransform.Transform(ref planeIntersection, ref transform, out hit.Location);
hit.T = t;
return true;
}
//No intersection! We can't be hitting the other sphere, so it's over!
hit = new RayHit();
return false;
}
///<summary>
/// Tests a ray against the triangle mesh.
///</summary>
///<param name="ray">Ray to test against the mesh.</param>
/// <param name="sidedness">Sidedness to apply to the mesh for the ray cast.</param>
///<param name="rayHit">Hit data for the ray, if any.</param>
///<returns>Whether or not the ray hit the mesh.</returns>
public bool RayCast(Ray ray, TriangleSidedness sidedness, out RayHit rayHit)
{
return RayCast(ray, float.MaxValue, sidedness, out rayHit);
}
///<summary>
/// Tests a ray against the triangle mesh.
///</summary>
///<param name="ray">Ray to test against the mesh.</param>
/// <param name="maximumLength">Maximum length of the ray in units of the ray direction's length.</param>
/// <param name="sidedness">Sidedness to apply to the mesh for the ray cast.</param>
///<param name="hits">Hit data for the ray, if any.</param>
///<returns>Whether or not the ray hit the mesh.</returns>
public bool RayCast(Ray ray, float maximumLength, TriangleSidedness sidedness, IList<RayHit> hits)
{
var hitElements = CommonResources.GetIntList();
tree.GetOverlaps(ray, maximumLength, hitElements);
for (int i = 0; i < hitElements.Count; i++)
{
System.Numerics.Vector3 v1, v2, v3;
data.GetTriangle(hitElements[i], out v1, out v2, out v3);
RayHit hit;
if (Toolbox.FindRayTriangleIntersection(ref ray, maximumLength, sidedness, ref v1, ref v2, ref v3, out hit))
{
hits.Add(hit);
}
}
CommonResources.GiveBack(hitElements);
return hits.Count > 0;
}
///<summary>
/// Tests a ray against the triangle mesh.
///</summary>
///<param name="ray">Ray to test against the mesh.</param>
/// <param name="maximumLength">Maximum length of the ray in units of the ray direction's length.</param>
///<param name="hits">Hit data for the ray, if any.</param>
///<returns>Whether or not the ray hit the mesh.</returns>
public bool RayCast(Ray ray, float maximumLength, IList<RayHit> hits)
{
return RayCast(ray, maximumLength, TriangleSidedness.DoubleSided, hits);
}
/// <summary>
/// Determines the intersection between a ray and a triangle.
/// </summary>
/// <param name="ray">Ray to test.</param>
/// <param name="maximumLength">Maximum length to travel in units of the direction's length.</param>
/// <param name="a">First vertex of the triangle.</param>
/// <param name="b">Second vertex of the triangle.</param>
/// <param name="c">Third vertex of the triangle.</param>
/// <param name="hitClockwise">True if the the triangle was hit on the clockwise face, false otherwise.</param>
/// <param name="hit">Hit data of the ray, if any</param>
/// <returns>Whether or not the ray and triangle intersect.</returns>
public static bool FindRayTriangleIntersection(ref Ray ray, float maximumLength, ref Vector3 a, ref Vector3 b, ref Vector3 c, out bool hitClockwise, out RayHit hit)
{
hitClockwise = false;
hit = new RayHit();
Vector3 ab, ac;
Vector3.Subtract(ref b, ref a, out ab);
Vector3.Subtract(ref c, ref a, out ac);
Vector3.Cross(ref ab, ref ac, out hit.Normal);
if (hit.Normal.LengthSquared() < Epsilon)
return false; //Degenerate triangle!
float d;
Vector3.Dot(ref ray.Direction, ref hit.Normal, out d);
d = -d;
hitClockwise = d >= 0;
Vector3 ap;
Vector3.Subtract(ref ray.Position, ref a, out ap);
Vector3.Dot(ref ap, ref hit.Normal, out hit.T);
hit.T /= d;
if (hit.T < 0 || hit.T > maximumLength)
return false;//Hit is behind origin, or too far away.
Vector3.Multiply(ref ray.Direction, hit.T, out hit.Location);
Vector3.Add(ref ray.Position, ref hit.Location, out hit.Location);
// Compute barycentric coordinates
Vector3.Subtract(ref hit.Location, ref a, out ap);
float ABdotAB, ABdotAC, ABdotAP;
float ACdotAC, ACdotAP;
Vector3.Dot(ref ab, ref ab, out ABdotAB);
Vector3.Dot(ref ab, ref ac, out ABdotAC);
Vector3.Dot(ref ab, ref ap, out ABdotAP);
Vector3.Dot(ref ac, ref ac, out ACdotAC);
Vector3.Dot(ref ac, ref ap, out ACdotAP);
float denom = 1 / (ABdotAB * ACdotAC - ABdotAC * ABdotAC);
float u = (ACdotAC * ABdotAP - ABdotAC * ACdotAP) * denom;
float v = (ABdotAB * ACdotAP - ABdotAC * ABdotAP) * denom;
return (u >= -Toolbox.BigEpsilon) && (v >= -Toolbox.BigEpsilon) && (u + v <= 1 + Toolbox.BigEpsilon);
}
///<summary>
/// Tests a ray against the surface of the mesh. This does not take into account solidity.
///</summary>
///<param name="ray">Ray to test.</param>
///<param name="maximumLength">Maximum length of the ray to test; in units of the ray's direction's length.</param>
///<param name="sidedness">Sidedness to use during the ray cast. This does not have to be the same as the mesh's sidedness.</param>
///<param name="rayHit">The hit location of the ray on the mesh, if any.</param>
///<returns>Whether or not the ray hit the mesh.</returns>
public bool RayCast(Ray ray, float maximumLength, TriangleSidedness sidedness, out RayHit rayHit)
{
//Put the ray into local space.
Ray localRay;
Matrix3x3 orientation;
Matrix3x3.CreateFromQuaternion(ref worldTransform.Orientation, out orientation);
Matrix3x3.TransformTranspose(ref ray.Direction, ref orientation, out localRay.Direction);
Vector3.Subtract(ref ray.Position, ref worldTransform.Position, out localRay.Position);
Matrix3x3.TransformTranspose(ref localRay.Position, ref orientation, out localRay.Position);
if (Shape.TriangleMesh.RayCast(localRay, maximumLength, sidedness, out rayHit))
{
//Transform the hit into world space.
Vector3.Multiply(ref ray.Direction, rayHit.T, out rayHit.Location);
Vector3.Add(ref rayHit.Location, ref ray.Position, out rayHit.Location);
Matrix3x3.Transform(ref rayHit.Normal, ref orientation, out rayHit.Normal);
return true;
}
rayHit = new RayHit();
return false;
}
/// <summary> /// Tests a ray against the entry. /// </summary> /// <param name="ray">Ray to test.</param> /// <param name="maximumLength">Maximum length, in units of the ray's direction's length, to test.</param> /// <param name="rayHit">Hit location of the ray on the entry, if any.</param> /// <returns>Whether or not the ray hit the entry.</returns> public abstract bool RayCast(Ray ray, float maximumLength, out RayHit rayHit);
/// <summary>
/// Casts a convex shape against the collidable.
/// </summary>
/// <param name="castShape">Shape to cast.</param>
/// <param name="startingTransform">Initial transform of the shape.</param>
/// <param name="sweep">Sweep to apply to the shape.</param>
/// <param name="hit">Hit data, if any.</param>
/// <returns>Whether or not the cast hit anything.</returns>
public override bool ConvexCast(ConvexShape castShape, ref RigidTransform startingTransform, ref Vector3 sweep, out RayHit hit)
{
if (Shape.solidity == MobileMeshSolidity.Solid)
{
//If the convex cast is inside the mesh and the mesh is solid, it should return t = 0.
var ray = new Ray() { Position = startingTransform.Position, Direction = Toolbox.UpVector };
if (Shape.IsLocalRayOriginInMesh(ref ray, out hit))
{
hit = new RayHit() { Location = startingTransform.Position, Normal = new Vector3(), T = 0 };
return true;
}
}
hit = new RayHit();
BoundingBox boundingBox;
var transform = new AffineTransform {Translation = worldTransform.Position};
Matrix3x3.CreateFromQuaternion(ref worldTransform.Orientation, out transform.LinearTransform);
castShape.GetSweptLocalBoundingBox(ref startingTransform, ref transform, ref sweep, out boundingBox);
var tri = PhysicsResources.GetTriangle();
var hitElements = CommonResources.GetIntList();
if (this.Shape.TriangleMesh.Tree.GetOverlaps(boundingBox, hitElements))
{
hit.T = float.MaxValue;
for (int i = 0; i < hitElements.Count; i++)
{
Shape.TriangleMesh.Data.GetTriangle(hitElements[i], out tri.vA, out tri.vB, out tri.vC);
AffineTransform.Transform(ref tri.vA, ref transform, out tri.vA);
AffineTransform.Transform(ref tri.vB, ref transform, out tri.vB);
AffineTransform.Transform(ref tri.vC, ref transform, out tri.vC);
Vector3 center;
Vector3.Add(ref tri.vA, ref tri.vB, out center);
Vector3.Add(ref center, ref tri.vC, out center);
Vector3.Multiply(ref center, 1f / 3f, out center);
Vector3.Subtract(ref tri.vA, ref center, out tri.vA);
Vector3.Subtract(ref tri.vB, ref center, out tri.vB);
Vector3.Subtract(ref tri.vC, ref center, out tri.vC);
tri.MaximumRadius = tri.vA.LengthSquared();
float radius = tri.vB.LengthSquared();
if (tri.MaximumRadius < radius)
tri.MaximumRadius = radius;
radius = tri.vC.LengthSquared();
if (tri.MaximumRadius < radius)
tri.MaximumRadius = radius;
tri.MaximumRadius = (float)Math.Sqrt(tri.MaximumRadius);
tri.collisionMargin = 0;
var triangleTransform = new RigidTransform {Orientation = Quaternion.Identity, Position = center};
RayHit tempHit;
if (MPRToolbox.Sweep(castShape, tri, ref sweep, ref Toolbox.ZeroVector, ref startingTransform, ref triangleTransform, out tempHit) && tempHit.T < hit.T)
{
hit = tempHit;
}
}
tri.MaximumRadius = 0;
PhysicsResources.GiveBack(tri);
CommonResources.GiveBack(hitElements);
return hit.T != float.MaxValue;
}
PhysicsResources.GiveBack(tri);
CommonResources.GiveBack(hitElements);
return false;
}
/// <summary>
/// Updates the collection of supporting contacts.
/// </summary>
public void UpdateSupports(ref System.Numerics.Vector3 movementDirection)
{
bool hadTraction = HasTraction;
//Reset traction/support.
HasTraction = false;
HasSupport = false;
System.Numerics.Vector3 downDirection = characterBody.orientationMatrix.Down;
System.Numerics.Vector3 bodyPosition = characterBody.position;
//Compute the character's radius, minus a little margin. We want the rays to originate safely within the character's body.
//Assume vertical rotational invariance. Spheres, cylinders, and capsules don't have varying horizontal radii.
System.Numerics.Vector3 extremePoint;
var convexShape = characterBody.CollisionInformation.Shape as ConvexShape;
Debug.Assert(convexShape != null, "Character bodies must be convex.");
//Find the lowest point on the collision shape.
convexShape.GetLocalExtremePointWithoutMargin(ref Toolbox.DownVector, out extremePoint);
BottomDistance = -extremePoint.Y + convexShape.collisionMargin;
convexShape.GetLocalExtremePointWithoutMargin(ref Toolbox.RightVector, out extremePoint);
float rayCastInnerRadius = Math.Max((extremePoint.X + convexShape.collisionMargin) * 0.8f, extremePoint.X);
//Vertically, the rays will start at the same height as the character's center.
//While they could be started lower on a cylinder, that wouldn't always work for a sphere or capsule: the origin might end up outside of the shape!
tractionContacts.Clear();
supportContacts.Clear();
sideContacts.Clear();
headContacts.Clear();
foreach (var pair in characterBody.CollisionInformation.Pairs)
{
//Don't stand on things that aren't really colliding fully.
if (pair.CollisionRule != CollisionRule.Normal)
continue;
ContactCategorizer.CategorizeContacts(pair, characterBody.CollisionInformation, ref downDirection, ref tractionContacts, ref supportContacts, ref sideContacts, ref headContacts);
}
HasSupport = supportContacts.Count > 0;
HasTraction = tractionContacts.Count > 0;
//Only perform ray casts if the character has fully left the surface, and only if the previous frame had traction.
//(If ray casts are allowed when support contacts still exist, the door is opened for climbing surfaces which should not be climbable.
//Consider a steep slope. If the character runs at it, the character will likely be wedged off of the ground, making it lose traction while still having a support contact with the slope.
//If ray tests are allowed when support contacts exist, the character will maintain traction despite climbing the wall.
//The VerticalMotionConstraint can stop the character from climbing in many cases, but it's nice not to have to rely on it.
//Disallowing ray tests when supports exist does have a cost, though. For example, consider rounded steps.
//If the character walks off a step such that it is still in contact with the step but is far enough down that the slope is too steep for traction,
//the ray test won't recover traction. This situation just isn't very common.)
if (!HasSupport && hadTraction)
{
float supportRayLength = maximumAssistedDownStepHeight + BottomDistance;
SupportRayData = null;
//If the contacts aren't available to support the character, raycast down to find the ground.
if (!HasTraction)
{
//TODO: could also require that the character has a nonzero movement direction in order to use a ray cast. Questionable- would complicate the behavior on edges.
Ray ray = new Ray(bodyPosition, downDirection);
bool hasTraction;
SupportRayData data;
if (TryDownCast(ref ray, supportRayLength, out hasTraction, out data))
{
SupportRayData = data;
HasTraction = data.HasTraction;
HasSupport = true;
}
}
//If contacts and the center ray cast failed, try a ray offset in the movement direction.
bool tryingToMove = movementDirection.LengthSquared() > 0;
if (!HasTraction && tryingToMove)
{
Ray ray = new Ray(
characterBody.Position +
movementDirection * rayCastInnerRadius, downDirection);
//Have to test to make sure the ray doesn't get obstructed. This could happen if the character is deeply embedded in a wall; we wouldn't want it detecting things inside the wall as a support!
Ray obstructionRay;
obstructionRay.Position = characterBody.Position;
obstructionRay.Direction = ray.Position - obstructionRay.Position;
if (!QueryManager.RayCastHitAnything(obstructionRay, 1))
{
//The origin isn't obstructed, so now ray cast down.
bool hasTraction;
SupportRayData data;
if (TryDownCast(ref ray, supportRayLength, out hasTraction, out data))
{
if (SupportRayData == null || data.HitData.T < SupportRayData.Value.HitData.T)
{
//Only replace the previous support ray if we now have traction or we didn't have a support ray at all before,
//or this hit is a better (sooner) hit.
if (hasTraction)
{
SupportRayData = data;
HasTraction = true;
}
else if (SupportRayData == null)
SupportRayData = data;
HasSupport = true;
}
}
}
}
//If contacts, center ray, AND forward ray failed to find traction, try a side ray created from down x forward.
if (!HasTraction && tryingToMove)
{
//Compute the horizontal offset direction. Down direction and the movement direction are normalized and perpendicular, so the result is too.
System.Numerics.Vector3 horizontalOffset;
Vector3Ex.Cross(ref movementDirection, ref downDirection, out horizontalOffset);
Vector3Ex.Multiply(ref horizontalOffset, rayCastInnerRadius, out horizontalOffset);
Ray ray = new Ray(bodyPosition + horizontalOffset, downDirection);
//Have to test to make sure the ray doesn't get obstructed. This could happen if the character is deeply embedded in a wall; we wouldn't want it detecting things inside the wall as a support!
Ray obstructionRay;
obstructionRay.Position = bodyPosition;
obstructionRay.Direction = ray.Position - obstructionRay.Position;
if (!QueryManager.RayCastHitAnything(obstructionRay, 1))
{
//The origin isn't obstructed, so now ray cast down.
bool hasTraction;
SupportRayData data;
if (TryDownCast(ref ray, supportRayLength, out hasTraction, out data))
{
if (SupportRayData == null || data.HitData.T < SupportRayData.Value.HitData.T)
{
//Only replace the previous support ray if we now have traction or we didn't have a support ray at all before,
//or this hit is a better (sooner) hit.
if (hasTraction)
{
SupportRayData = data;
HasTraction = true;
}
else if (SupportRayData == null)
SupportRayData = data;
HasSupport = true;
}
}
}
}
//If contacts, center ray, forward ray, AND the first side ray failed to find traction, try a side ray created from forward x down.
if (!HasTraction && tryingToMove)
{
//Compute the horizontal offset direction. Down direction and the movement direction are normalized and perpendicular, so the result is too.
System.Numerics.Vector3 horizontalOffset;
Vector3Ex.Cross(ref downDirection, ref movementDirection, out horizontalOffset);
Vector3Ex.Multiply(ref horizontalOffset, rayCastInnerRadius, out horizontalOffset);
Ray ray = new Ray(bodyPosition + horizontalOffset, downDirection);
//Have to test to make sure the ray doesn't get obstructed. This could happen if the character is deeply embedded in a wall; we wouldn't want it detecting things inside the wall as a support!
Ray obstructionRay;
obstructionRay.Position = bodyPosition;
obstructionRay.Direction = ray.Position - obstructionRay.Position;
if (!QueryManager.RayCastHitAnything(obstructionRay, 1))
{
//The origin isn't obstructed, so now ray cast down.
bool hasTraction;
SupportRayData data;
if (TryDownCast(ref ray, supportRayLength, out hasTraction, out data))
{
if (SupportRayData == null || data.HitData.T < SupportRayData.Value.HitData.T)
{
//Only replace the previous support ray if we now have traction or we didn't have a support ray at all before,
//or this hit is a better (sooner) hit.
if (hasTraction)
{
SupportRayData = data;
HasTraction = true;
}
else if (SupportRayData == null)
SupportRayData = data;
HasSupport = true;
}
}
}
}
}
UpdateSupportData(ref downDirection);
UpdateVerticalSupportData(ref downDirection, ref movementDirection);
}
//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.
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 extremePointToRayOrigin, extremePoint;
hit.T = 0;
hit.Location = ray.Position;
hit.Normal = Toolbox.ZeroVector;
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);
Vector3.Subtract(ref hit.Location, ref extremePoint, out extremePointToRayOrigin);
Vector3.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)
{
Vector3.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.
Vector3.Multiply(ref ray.Direction, hit.T, out hit.Location);
Vector3.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.
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>
/// Gets the intersection between the box and the ray.
/// </summary>
/// <param name="ray">Ray to test against the box.</param>
/// <param name="transform">Transform of the shape.</param>
/// <param name="maximumLength">Maximum distance to travel in units of the direction vector's length.</param>
/// <param name="hit">Hit data for the raycast, 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)
{
hit = new RayHit();
System.Numerics.Quaternion conjugate;
QuaternionEx.Conjugate(ref transform.Orientation, out conjugate);
System.Numerics.Vector3 localOrigin;
Vector3Ex.Subtract(ref ray.Position, ref transform.Position, out localOrigin);
QuaternionEx.Transform(ref localOrigin, ref conjugate, out localOrigin);
System.Numerics.Vector3 localDirection;
QuaternionEx.Transform(ref ray.Direction, ref conjugate, out localDirection);
System.Numerics.Vector3 normal = Toolbox.ZeroVector;
float temp, tmin = 0, tmax = maximumLength;
if (Math.Abs(localDirection.X) < Toolbox.Epsilon && (localOrigin.X < -halfWidth || localOrigin.X > halfWidth))
return false;
float inverseDirection = 1 / localDirection.X;
float t1 = (-halfWidth - localOrigin.X) * inverseDirection;
float t2 = (halfWidth - localOrigin.X) * inverseDirection;
var tempNormal = new System.Numerics.Vector3(-1, 0, 0);
if (t1 > t2)
{
temp = t1;
t1 = t2;
t2 = temp;
tempNormal *= -1;
}
temp = tmin;
tmin = Math.Max(tmin, t1);
if (temp != tmin)
normal = tempNormal;
tmax = Math.Min(tmax, t2);
if (tmin > tmax)
return false;
if (Math.Abs(localDirection.Y) < Toolbox.Epsilon && (localOrigin.Y < -halfHeight || localOrigin.Y > halfHeight))
return false;
inverseDirection = 1 / localDirection.Y;
t1 = (-halfHeight - localOrigin.Y) * inverseDirection;
t2 = (halfHeight - localOrigin.Y) * inverseDirection;
tempNormal = new System.Numerics.Vector3(0, -1, 0);
if (t1 > t2)
{
temp = t1;
t1 = t2;
t2 = temp;
tempNormal *= -1;
}
temp = tmin;
tmin = Math.Max(tmin, t1);
if (temp != tmin)
normal = tempNormal;
tmax = Math.Min(tmax, t2);
if (tmin > tmax)
return false;
if (Math.Abs(localDirection.Z) < Toolbox.Epsilon && (localOrigin.Z < -halfLength || localOrigin.Z > halfLength))
return false;
inverseDirection = 1 / localDirection.Z;
t1 = (-halfLength - localOrigin.Z) * inverseDirection;
t2 = (halfLength - localOrigin.Z) * inverseDirection;
tempNormal = new System.Numerics.Vector3(0, 0, -1);
if (t1 > t2)
{
temp = t1;
t1 = t2;
t2 = temp;
tempNormal *= -1;
}
temp = tmin;
tmin = Math.Max(tmin, t1);
if (temp != tmin)
normal = tempNormal;
tmax = Math.Min(tmax, t2);
if (tmin > tmax)
return false;
hit.T = tmin;
Vector3Ex.Multiply(ref ray.Direction, tmin, out hit.Location);
Vector3Ex.Add(ref hit.Location, ref ray.Position, out hit.Location);
QuaternionEx.Transform(ref normal, ref transform.Orientation, out normal);
hit.Normal = normal;
return true;
}
///<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>
/// Tests a ray against the entry.
/// </summary>
/// <param name="ray">Ray to test.</param>
/// <param name="maximumLength">Maximum length, in units of the ray's direction's length, to test.</param>
/// <param name="rayHit">Hit location of the ray on the entry, if any.</param>
/// <returns>Whether or not the ray hit the entry.</returns>
public override bool RayCast(Ray ray, float maximumLength, out RayHit rayHit)
{
return RayCast(ray, maximumLength, sidedness, out rayHit);
}
///<summary>
/// Tests a ray against the triangle mesh.
///</summary>
///<param name="ray">Ray to test against the mesh.</param>
/// <param name="maximumLength">Maximum length of the ray in units of the ray direction's length.</param>
///<param name="rayHit">Hit data for the ray, if any.</param>
///<returns>Whether or not the ray hit the mesh.</returns>
public bool RayCast(Ray ray, float maximumLength, out RayHit rayHit)
{
return RayCast(ray, maximumLength, TriangleSidedness.DoubleSided, out rayHit);
}
///<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 a ray against the instance.
///</summary>
///<param name="ray">Ray to test.</param>
///<param name="maximumLength">Maximum length of the ray to test; in units of the ray's direction's length.</param>
///<param name="sidedness">Sidedness to use during the ray cast. This does not have to be the same as the mesh's sidedness.</param>
///<param name="rayHit">The hit location of the ray on the mesh, if any.</param>
///<returns>Whether or not the ray hit the mesh.</returns>
public bool RayCast(Ray ray, float maximumLength, TriangleSidedness sidedness, out RayHit rayHit)
{
//Put the ray into local space.
Ray localRay;
AffineTransform inverse;
AffineTransform.Invert(ref worldTransform, out inverse);
Matrix3x3.Transform(ref ray.Direction, ref inverse.LinearTransform, out localRay.Direction);
AffineTransform.Transform(ref ray.Position, ref inverse, out localRay.Position);
if (Shape.TriangleMesh.RayCast(localRay, maximumLength, sidedness, out rayHit))
{
//Transform the hit into world space.
Vector3.Multiply(ref ray.Direction, rayHit.T, out rayHit.Location);
Vector3.Add(ref rayHit.Location, ref ray.Position, out rayHit.Location);
Matrix3x3.TransformTranspose(ref rayHit.Normal, ref inverse.LinearTransform, out rayHit.Normal);
return true;
}
rayHit = new RayHit();
return false;
}
/// <summary>
/// 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>
/// Tests a ray against a sphere.
///</summary>
///<param name="ray">Ray to test.</param>
///<param name="spherePosition">Position of the sphere.</param>
///<param name="radius">Radius of the sphere.</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, if any.</param>
///<returns>Whether or not the ray hits the sphere.</returns>
public static bool RayCastSphere(ref Ray ray, ref Vector3 spherePosition, float radius, float maximumLength, out RayHit 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 spherePosition, out m);
float b = Vector3.Dot(m, normalizedDirection);
float c = m.LengthSquared() - radius * radius;
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;
hit.T /= length;
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 spherePosition, out hit.Normal);
hit.Normal.Normalize();
return true;
}
