public float FindMinSeparation(out int indexA, out int indexB, float t)
{
WorldTransform xfA, xfB;
_sweepA.GetTransform(out xfA, t);
_sweepB.GetTransform(out xfB, t);
switch (_type)
{
case Type.Points:
{
var axisA = -xfA.Rotation * _axis;
var axisB = -xfB.Rotation * -_axis;
indexA = _proxyA.GetSupport(axisA);
indexB = _proxyB.GetSupport(axisB);
var localPointA = _proxyA.Vertices[indexA];
var localPointB = _proxyB.Vertices[indexB];
var pointA = xfA.ToGlobal(localPointA);
var pointB = xfB.ToGlobal(localPointB);
// ReSharper disable RedundantCast Necessary for FarPhysics.
return(Vector2Util.Dot((Vector2)(pointB - pointA), _axis));
// ReSharper restore RedundantCast
}
case Type.FaceA:
{
var normal = xfA.Rotation * _axis;
var pointA = xfA.ToGlobal(_localPoint);
var axisB = -xfB.Rotation * -normal;
indexA = -1;
indexB = _proxyB.GetSupport(axisB);
var localPointB = _proxyB.Vertices[indexB];
var pointB = xfB.ToGlobal(localPointB);
// ReSharper disable RedundantCast Necessary for FarPhysics.
return(Vector2Util.Dot((Vector2)(pointB - pointA), normal));
// ReSharper restore RedundantCast
}
case Type.FaceB:
{
var normal = xfB.Rotation * _axis;
var pointB = xfB.ToGlobal(_localPoint);
var axisA = -xfA.Rotation * -normal;
indexB = -1;
indexA = _proxyA.GetSupport(axisA);
var localPointA = _proxyA.Vertices[indexA];
var pointA = xfA.ToGlobal(localPointA);
// ReSharper disable RedundantCast Necessary for FarPhysics.
return(Vector2Util.Dot((Vector2)(pointA - pointB), normal));
// ReSharper restore RedundantCast
}
default:
throw new ArgumentOutOfRangeException();
}
}
public static float FindMinSeparation(out int indexA, out int indexB, float t, DistanceProxy proxyA, ref Sweep sweepA, DistanceProxy proxyB, ref Sweep sweepB, ref Vector2 axis, ref Vector2 localPoint, SeparationFunctionType type)
{
sweepA.GetTransform(out Transform xfA, t);
sweepB.GetTransform(out Transform xfB, t);
switch (type)
{
case SeparationFunctionType.Points:
{
Vector2 axisA = MathUtils.MulT(ref xfA.q, axis);
Vector2 axisB = MathUtils.MulT(ref xfB.q, -axis);
indexA = proxyA.GetSupport(axisA);
indexB = proxyB.GetSupport(axisB);
Vector2 localPointA = proxyA._vertices[indexA];
Vector2 localPointB = proxyB._vertices[indexB];
Vector2 pointA = MathUtils.Mul(ref xfA, localPointA);
Vector2 pointB = MathUtils.Mul(ref xfB, localPointB);
float separation = Vector2.Dot(pointB - pointA, axis);
return(separation);
}
case SeparationFunctionType.FaceA:
{
Vector2 normal = MathUtils.Mul(ref xfA.q, axis);
Vector2 pointA = MathUtils.Mul(ref xfA, localPoint);
Vector2 axisB = MathUtils.MulT(ref xfB.q, -normal);
indexA = -1;
indexB = proxyB.GetSupport(axisB);
Vector2 localPointB = proxyB._vertices[indexB];
Vector2 pointB = MathUtils.Mul(ref xfB, localPointB);
float separation = Vector2.Dot(pointB - pointA, normal);
return(separation);
}
case SeparationFunctionType.FaceB:
{
Vector2 normal = MathUtils.Mul(ref xfB.q, axis);
Vector2 pointB = MathUtils.Mul(ref xfB, localPoint);
Vector2 axisA = MathUtils.MulT(ref xfA.q, -normal);
indexB = -1;
indexA = proxyA.GetSupport(axisA);
Vector2 localPointA = proxyA._vertices[indexA];
Vector2 pointA = MathUtils.Mul(ref xfA, localPointA);
float separation = Vector2.Dot(pointA - pointB, normal);
return(separation);
}
default:
Debug.Assert(false);
indexA = -1;
indexB = -1;
return(0.0f);
}
}
/// <summary>
/// Computes the distance between two shapes represented by the specified proxies, positioned as defined by the
/// specified transforms. The cache is read-write and will be updated for future calls.
/// </summary>
private static float Distance(
ref SimplexCache cache,
DistanceProxy proxyA,
WorldTransform xfA,
DistanceProxy proxyB,
WorldTransform xfB,
bool useRadii = false)
{
// Initialize the simplex.
Simplex simplex;
Simplex.ReadCache(out simplex, cache, xfA, proxyA, xfB, proxyB);
// These store the vertices of the last simplex so that we
// can check for duplicates and prevent cycling.
var saveA = new FixedArray3 <int>();
var saveB = new FixedArray3 <int>();
// Main iteration loop.
for (var i = 0; i < 20; ++i)
{
// Copy simplex so we can identify duplicates.
var saveCount = simplex.Count;
for (var j = 0; j < saveCount; ++j)
{
saveA[j] = simplex.Vertices[j].IndexA;
saveB[j] = simplex.Vertices[j].IndexB;
}
switch (simplex.Count)
{
case 1:
break;
case 2:
simplex.Solve2();
break;
case 3:
simplex.Solve3();
break;
default:
throw new ArgumentOutOfRangeException();
}
// If we have 3 points, then the origin is in the corresponding triangle.
if (simplex.Count == 3)
{
break;
}
// Get search direction.
var d = simplex.GetSearchDirection();
// Ensure the search direction is numerically fit.
if (d.LengthSquared() < Settings.Epsilon * Settings.Epsilon)
{
// The origin is probably contained by a line segment
// or triangle. Thus the shapes are overlapped.
// We can't return zero here even though there may be overlap.
// In case the simplex is a point, segment, or triangle it is difficult
// to determine if the origin is contained in the CSO or very close to it.
break;
}
// Compute a tentative new simplex vertex using support points.
SimplexVertex v;
v.IndexA = proxyA.GetSupport(-xfA.Rotation * -d);
v.IndexB = proxyB.GetSupport(-xfB.Rotation * d);
v.VertexA = xfA.ToGlobal(proxyA.Vertices[v.IndexA]);
v.VertexB = xfB.ToGlobal(proxyB.Vertices[v.IndexB]);
// ReSharper disable RedundantCast Necessary for FarPhysics.
v.VertexDelta = (LocalPoint)(v.VertexB - v.VertexA);
// ReSharper restore RedundantCast
v.Alpha = 0;
simplex.Vertices[simplex.Count] = v;
// Check for duplicate support points. This is the main termination criteria.
var duplicate = false;
for (var j = 0; j < saveCount; ++j)
{
if (v.IndexA == saveA[j] && v.IndexB == saveB[j])
{
duplicate = true;
break;
}
}
// If we found a duplicate support point we must exit to avoid cycling.
if (duplicate)
{
break;
}
// New vertex is ok and needed.
++simplex.Count;
}
// Cache the simplex.
simplex.WriteCache(ref cache);
// Prepare output.
var distance = simplex.GetWitnessPointDistance();
// Apply radii if requested.
if (useRadii)
{
var rA = proxyA.Radius;
var rB = proxyB.Radius;
if (distance > rA + rB && distance > Settings.Epsilon)
{
// Shapes are still not overlapped.
return(distance - (rA + rB));
}
// Shapes are overlapped when radii are considered.
return(0.0f);
}
return(distance);
}
static unsafe bool CastCollider(
Ray ray, ref float2x2 rotation,
ref DistanceProxy proxySource, ref DistanceProxy proxyTarget,
out ColliderCastHit hit)
{
hit = default;
var transformSource = new PhysicsTransform
{
Translation = ray.Origin,
Rotation = rotation
};
// Check we're not initially overlapped.
if ((proxySource.VertexCount < 3 || proxyTarget.VertexCount < 3) &&
OverlapQueries.OverlapConvexConvex(ref transformSource, ref proxySource, ref proxyTarget))
{
return(false);
}
// B = Source
// A = Target
var radiusSource = proxySource.ConvexRadius;
var radiusTarget = proxyTarget.ConvexRadius;
var totalRadius = radiusSource + radiusTarget;
var invRotation = math.inverse(rotation);
var sweepDirection = ray.Displacement;
var normal = float2.zero;
var lambda = 0.0f;
// Initialize the simplex.
var simplex = new Simplex();
simplex.Count = 0;
var vertices = &simplex.Vertex1;
// Get a support point in the inverse direction.
var indexTarget = proxyTarget.GetSupport(-sweepDirection);
var supportTarget = proxyTarget.Vertices[indexTarget];
var indexSource = proxySource.GetSupport(PhysicsMath.mul(invRotation, sweepDirection));
var supportSource = PhysicsMath.mul(transformSource, proxySource.Vertices[indexSource]);
var v = supportTarget - supportSource;
// Sigma is the target distance between polygons
var sigma = math.max(PhysicsSettings.Constants.MinimumConvexRadius, totalRadius - PhysicsSettings.Constants.MinimumConvexRadius);
const float tolerance = PhysicsSettings.Constants.LinearSlop * 0.5f;
var iteration = 0;
while (
iteration++ < PhysicsSettings.Constants.MaxGJKInterations &&
math.abs(math.length(v) - sigma) > tolerance
)
{
if (simplex.Count >= 3)
{
SafetyChecks.ThrowInvalidOperationException("ColliderCast Simplex must have less than 3 vertex.");
}
// Support in direction -supportV (Target - Source)
indexTarget = proxyTarget.GetSupport(-v);
supportTarget = proxyTarget.Vertices[indexTarget];
indexSource = proxySource.GetSupport(PhysicsMath.mul(invRotation, v));
supportSource = PhysicsMath.mul(transformSource, proxySource.Vertices[indexSource]);
var p = supportTarget - supportSource;
v = math.normalizesafe(v);
// Intersect ray with plane.
var vp = math.dot(v, p);
var vr = math.dot(v, sweepDirection);
if (vp - sigma > lambda * vr)
{
if (vr <= 0.0f)
{
return(false);
}
lambda = (vp - sigma) / vr;
if (lambda > 1.0f)
{
return(false);
}
normal = -v;
simplex.Count = 0;
}
// Reverse simplex since it works with B - A.
// Shift by lambda * r because we want the closest point to the current clip point.
// Note that the support point p is not shifted because we want the plane equation
// to be formed in un-shifted space.
var vertex = vertices + simplex.Count;
vertex->IndexA = indexSource;
vertex->SupportA = supportSource + lambda * sweepDirection;
vertex->IndexB = indexTarget;
vertex->SupportB = supportTarget;
vertex->W = vertex->SupportB - vertex->SupportA;
vertex->A = 1.0f;
simplex.Count += 1;
switch (simplex.Count)
{
case 1:
break;
case 2:
simplex.Solve2();
break;
case 3:
simplex.Solve3();
break;
default:
SafetyChecks.ThrowInvalidOperationException("Simplex has invalid count.");
return(default);
}
// If we have 3 points, then the origin is in the corresponding triangle.
if (simplex.Count == 3)
{
// Overlap.
return(false);
}
// Get search direction.
v = simplex.GetClosestPoint();
}
// Ensure we don't process an empty simplex.
if (simplex.Count == 0)
{
return(false);
}
// Prepare result.
var pointSource = float2.zero;
var pointTarget = float2.zero;
simplex.GetWitnessPoints(ref pointSource, ref pointTarget);
normal = math.normalizesafe(-v);
hit = new ColliderCastHit
{
Position = pointTarget + (normal * radiusTarget),
SurfaceNormal = normal,
Fraction = lambda
};
return(true);
}
internal static unsafe DistanceHit ColliderDistance(PhysicsTransform transformA, ref DistanceProxy proxyA, ref DistanceProxy proxyB)
{
var simplex = new Simplex();
simplex.Reset(transformA, proxyA, proxyB);
var inverseRotationA = math.transpose(transformA.Rotation);
var vertices = &simplex.Vertex1;
Simplex.VertexIndexTriple saveA;
Simplex.VertexIndexTriple saveB;
var iteration = 0;
while (iteration < PhysicsSettings.Constants.MaxGJKInterations)
{
// Copy simplex so we can identify duplicates.
var saveCount = simplex.Count;
for (var i = 0; i < saveCount; ++i)
{
saveA.Index[i] = vertices[i].IndexA;
saveB.Index[i] = vertices[i].IndexB;
}
switch (saveCount)
{
case 1:
break;
case 2:
simplex.Solve2();
break;
case 3:
simplex.Solve3();
break;
default:
SafetyChecks.ThrowInvalidOperationException("Simplex has invalid count.");
return(default);
}
// If we have 3 points, then the origin is in the corresponding triangle.
if (simplex.Count == 3)
{
break;
}
// Get search direction.
var direction = simplex.GetSearchDirection();
// Ensure the search direction is numerically fit.
if (math.lengthsq(direction) < float.Epsilon * float.Epsilon)
{
// The origin is probably contained by a line segment
// or triangle. Thus the shapes are overlapped.
// We can't return zero here even though there may be overlap.
// In case the simplex is a point, segment, or triangle it is difficult
// to determine if the origin is contained in the CSO or very close to it.
break;
}
// Compute a tentative new simplex vertex using support points.
var vertex = vertices + simplex.Count;
vertex->IndexA = proxyA.GetSupport(PhysicsMath.mul(inverseRotationA, -direction));
vertex->SupportA = PhysicsMath.mul(transformA, proxyA.Vertices[vertex->IndexA]);
vertex->IndexB = proxyB.GetSupport(direction);
vertex->SupportB = proxyB.Vertices[vertex->IndexB];
vertex->W = vertex->SupportB - vertex->SupportA;
// Iteration count is equated to the number of support point calls.
++iteration;
// Check for duplicate support points. This is the main termination criteria.
var duplicate = false;
for (var i = 0; i < saveCount; ++i)
{
if (vertex->IndexA == saveA.Index[i] && vertex->IndexB == saveB.Index[i])
{
duplicate = true;
break;
}
}
// If we found a duplicate support point we must exit to avoid cycling.
if (duplicate)
{
break;
}
// New vertex is okay and needed.
simplex.Count++;
}
// Prepare result.
var pointA = float2.zero;
var pointB = float2.zero;
simplex.GetWitnessPoints(ref pointA, ref pointB);
var distance = math.distance(pointA, pointB);
var radiusA = proxyA.ConvexRadius;
var radiusB = proxyB.ConvexRadius;
if (distance > (radiusA + radiusB) && distance > float.Epsilon)
{
// Shapes not overlapped.
// Move the witness points to the outer surface.
distance -= radiusA + radiusB;
var normal = math.normalize(pointB - pointA);
pointA += radiusA * normal;
pointB -= radiusB * normal;
}
else
{
// Shapes are overlapped.
// Move the witness points to the middle.
pointA = pointB = 0.5f * (pointA + pointB);
distance = 0f;
}
return(new DistanceHit
{
PointA = pointA,
PointB = pointB,
Fraction = distance
});
}
/// <summary>
/// Compute the closest points between two shapes. Supports any combination of: CircleShape and
/// PolygonShape. The simplex cache is input/output. On the first call set SimplexCache.count to
/// zero.
/// </summary>
/// <param name="output"></param>
/// <param name="cache"></param>
/// <param name="input"></param>
public void GetDistance(DistanceOutput output, SimplexCache cache, DistanceInput input)
{
GJK_CALLS++;
DistanceProxy proxyA = input.ProxyA;
DistanceProxy proxyB = input.ProxyB;
Transform transformA = input.TransformA;
Transform transformB = input.TransformB;
// Initialize the simplex.
simplex.ReadCache(cache, proxyA, transformA, proxyB, transformB);
// Get simplex vertices as an array.
SimplexVertex[] vertices = simplex.Vertices;
// These store the vertices of the last simplex so that we
// can check for duplicates and prevent cycling.
// (pooled above)
simplex.GetClosestPoint(closestPoint);
float distanceSqr1 = closestPoint.LengthSquared();
// Main iteration loop
int iter = 0;
while (iter < GJK_MAX_ITERS)
{
// Copy simplex so we can identify duplicates.
int saveCount = simplex.Count;
for (int i = 0; i < saveCount; i++)
{
saveA[i] = vertices[i].IndexA;
saveB[i] = vertices[i].IndexB;
}
switch (simplex.Count)
{
case 1:
break;
case 2:
simplex.Solve2();
break;
case 3:
simplex.Solve3();
break;
default:
Debug.Assert(false);
break;
}
// If we have 3 points, then the origin is in the corresponding triangle.
if (simplex.Count == 3)
{
break;
}
// Compute closest point.
simplex.GetClosestPoint(closestPoint);
float distanceSqr2 = closestPoint.LengthSquared();
// ensure progress
if (distanceSqr2 >= distanceSqr1)
{
// break;
}
distanceSqr1 = distanceSqr2;
// get search direction;
simplex.GetSearchDirection(d);
// Ensure the search direction is numerically fit.
if (d.LengthSquared() < Settings.EPSILON * Settings.EPSILON)
{
// The origin is probably contained by a line segment
// or triangle. Thus the shapes are overlapped.
// We can't return zero here even though there may be overlap.
// In case the simplex is a point, segment, or triangle it is difficult
// to determine if the origin is contained in the CSO or very close to it.
break;
}
/*
* SimplexVertex* vertex = vertices + simplex.m_count; vertex.indexA =
* proxyA.GetSupport(MulT(transformA.R, -d)); vertex.wA = Mul(transformA,
* proxyA.GetVertex(vertex.indexA)); Vec2 wBLocal; vertex.indexB =
* proxyB.GetSupport(MulT(transformB.R, d)); vertex.wB = Mul(transformB,
* proxyB.GetVertex(vertex.indexB)); vertex.w = vertex.wB - vertex.wA;
*/
// Compute a tentative new simplex vertex using support points.
SimplexVertex vertex = vertices[simplex.Count];
Rot.MulTransUnsafe(transformA.Q, d.NegateLocal(), temp);
vertex.IndexA = proxyA.GetSupport(temp);
Transform.MulToOutUnsafe(transformA, proxyA.GetVertex(vertex.IndexA), vertex.WA);
// Vec2 wBLocal;
Rot.MulTransUnsafe(transformB.Q, d.NegateLocal(), temp);
vertex.IndexB = proxyB.GetSupport(temp);
Transform.MulToOutUnsafe(transformB, proxyB.GetVertex(vertex.IndexB), vertex.WB);
vertex.W.Set(vertex.WB).SubLocal(vertex.WA);
// Iteration count is equated to the number of support point calls.
++iter;
++GJK_ITERS;
// Check for duplicate support points. This is the main termination criteria.
bool duplicate = false;
for (int i = 0; i < saveCount; ++i)
{
if (vertex.IndexA == saveA[i] && vertex.IndexB == saveB[i])
{
duplicate = true;
break;
}
}
// If we found a duplicate support point we must exit to avoid cycling.
if (duplicate)
{
break;
}
// New vertex is ok and needed.
++simplex.Count;
}
GJK_MAX_ITERS = MathUtils.Max(GJK_MAX_ITERS, iter);
// Prepare output.
simplex.GetWitnessPoints(output.PointA, output.PointB);
output.Distance = MathUtils.Distance(output.PointA, output.PointB);
output.Iterations = iter;
// Cache the simplex.
simplex.WriteCache(cache);
// Apply radii if requested.
if (input.UseRadii)
{
float rA = proxyA.Radius;
float rB = proxyB.Radius;
if (output.Distance > rA + rB && output.Distance > Settings.EPSILON)
{
// Shapes are still no overlapped.
// Move the witness points to the outer surface.
output.Distance -= (rA + rB);
normal.Set(output.PointB).SubLocal(output.PointA);
normal.Normalize();
temp.Set(normal).MulLocal(rA);
output.PointA.AddLocal(temp);
temp.Set(normal).MulLocal(rB);
output.PointB.SubLocal(temp);
}
else
{
// Shapes are overlapped when radii are considered.
// Move the witness points to the middle.
// Vec2 p = 0.5f * (output.pointA + output.pointB);
output.PointA.AddLocal(output.PointB).MulLocal(.5f);
output.PointB.Set(output.PointA);
output.Distance = 0.0f;
}
}
}
/// <summary>
/// Compute the closest points between two shapes. Supports any combination of:
/// b2CircleShape, b2PolygonShape, b2EdgeShape. The simplex cache is input/output.
/// On the first call set b2SimplexCache.count to zero.
/// </summary>
public static void Distance(out DistanceOutput output,
SimplexCache cache,
DistanceInput input)
{
++GjkCalls;
DistanceProxy proxyA = input.proxyA;
DistanceProxy proxyB = input.proxyB;
Transform transformA = input.TransformA;
Transform transformB = input.TransformB;
// Initialize the simplex.
Simplex simplex = new Simplex();
simplex.ReadCache(cache, proxyA, ref transformA, proxyB, ref transformB);
// Get simplex vertices as an array.
SimplexVertex[] vertices = simplex.Vertices;
const int k_maxIters = 20;
// These store the vertices of the last simplex so that we
// can check for duplicates and prevent cycling.
int[] saveA = new int[3], saveB = new int[3];
int saveCount = 0;
Vec2 closestPoint = simplex.GetClosestPoint();
float distanceSqr1 = closestPoint.LengthSquared();
float distanceSqr2 = distanceSqr1;
// Main iteration loop.
int iter = 0;
while (iter < k_maxIters)
{
// Copy simplex so we can identify duplicates.
saveCount = simplex.Count;
for (int i = 0; i < saveCount; ++i)
{
saveA[i] = vertices[i].IndexA;
saveB[i] = vertices[i].IndexB;
}
switch (simplex.Count)
{
case 1:
break;
case 2:
simplex.Solve2();
break;
case 3:
simplex.Solve3();
break;
default:
Box2DXDebug.Assert(false);
break;
}
// If we have 3 points, then the origin is in the corresponding triangle.
if (simplex.Count == 3)
{
break;
}
// Compute closest point.
Vec2 p = simplex.GetClosestPoint();
float distanceSqr = p.LengthSquared();
// Ensure progress
if (distanceSqr2 >= distanceSqr1)
{
//break;
}
distanceSqr1 = distanceSqr2;
// Get search direction.
Vec2 d = simplex.GetSearchDirection();
// Ensure the search direction is numerically fit.
if (d.LengthSquared() < Settings.FLT_EPSILON * Settings.FLT_EPSILON)
{
// The origin is probably contained by a line segment
// or triangle. Thus the shapes are overlapped.
// We can't return zero here even though there may be overlap.
// In case the simplex is a point, segment, or triangle it is difficult
// to determine if the origin is contained in the CSO or very close to it.
break;
}
// Compute a tentative new simplex vertex using support points.
SimplexVertex vertex = vertices[simplex.Count];
vertex.IndexA = proxyA.GetSupport(Math.MulT(transformA.R, -d));
vertex.WA = Math.Mul(transformA, proxyA.GetVertex(vertex.IndexA));
vertex.IndexB = proxyB.GetSupport(Math.MulT(transformB.R, d));
vertex.WB = Math.Mul(transformB, proxyB.GetVertex(vertex.IndexB));
vertex.W = vertex.WB - vertex.WA;
// Iteration count is equated to the number of support point calls.
++iter;
++GjkIters;
// Check for duplicate support points. This is the main termination criteria.
bool duplicate = false;
for (int i = 0; i < saveCount; ++i)
{
if (vertex.IndexA == saveA[i] && vertex.IndexB == saveB[i])
{
duplicate = true;
break;
}
}
// If we found a duplicate support point we must exit to avoid cycling.
if (duplicate)
{
break;
}
// New vertex is ok and needed.
++simplex.Count;
}
GjkMaxIters = Math.Max(GjkMaxIters, iter);
// Prepare output.
simplex.GetWitnessPoints(out output.PointA, out output.PointB);
output.Distance = Vec2.Distance(output.PointA, output.PointB);
output.Iterations = iter;
// Cache the simplex.
simplex.WriteCache(cache);
// Apply radii if requested.
if (input.UseRadii)
{
float rA = proxyA._radius;
float rB = proxyB._radius;
if (output.Distance > rA + rB && output.Distance > Settings.FLT_EPSILON)
{
// Shapes are still no overlapped.
// Move the witness points to the outer surface.
output.Distance -= rA + rB;
Vec2 normal = output.PointB - output.PointA;
normal.Normalize();
output.PointA += rA * normal;
output.PointB -= rB * normal;
}
else
{
// Shapes are overlapped when radii are considered.
// Move the witness points to the middle.
Vec2 p = 0.5f * (output.PointA + output.PointB);
output.PointA = p;
output.PointB = p;
output.Distance = 0.0f;
}
}
}