public static void ReadCache(
out Simplex simplex,
SimplexCache cache,
WorldTransform xfA,
DistanceProxy proxyA,
WorldTransform xfB,
DistanceProxy proxyB)
{
System.Diagnostics.Debug.Assert(cache.Count <= 3);
// Copy data from cache.
simplex = new Simplex {
Count = cache.Count
};
for (var i = 0; i < simplex.Count; ++i)
{
SimplexVertex v;
v.IndexA = cache.IndexA[i];
v.IndexB = cache.IndexB[i];
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.0f;
simplex.Vertices[i] = v;
}
// Compute the new simplex metric, if it is substantially different than
// old metric then flush the simplex.
if (simplex.Count > 1)
{
var metric1 = cache.Metric;
var metric2 = simplex.GetMetric();
if (metric2 < 0.5f * metric1 || 2.0f * metric1 < metric2 || metric2 < float.Epsilon)
{
// Reset the simplex, triggers computation below.
simplex.Count = 0;
}
}
// If the cache is empty or invalid ...
if (simplex.Count == 0)
{
SimplexVertex v;
v.IndexA = 0;
v.IndexB = 0;
v.VertexA = xfA.ToGlobal(proxyA.Vertices[0]);
v.VertexB = xfB.ToGlobal(proxyB.Vertices[0]);
// ReSharper disable RedundantCast Necessary for FarPhysics.
v.VertexDelta = (LocalPoint)(v.VertexB - v.VertexA);
// ReSharper restore RedundantCast
v.Alpha = 1.0f;
simplex.Vertices.Item1 = v;
simplex.Count = 1;
}
}
/// <summary>Tests whether two shapes overlap, using their distance proxies.</summary>
/// <param name="proxyA">The proxy for the first shape.</param>
/// <param name="proxyB">The proxy for the second shape.</param>
/// <param name="transformA">The transform of the first shape.</param>
/// <param name="transformB">The transform of the second shape.</param>
/// <returns></returns>
internal static bool TestOverlap(
DistanceProxy proxyA,
DistanceProxy proxyB,
WorldTransform transformA,
WorldTransform transformB)
{
var cache = new SimplexCache {
Count = 0
};
return(Distance(ref cache, proxyA, transformA, proxyB, transformB, true) < 10 * Settings.Epsilon);
}
public static float Evaluate(int indexA, int indexB, float t, DistanceProxy proxyA, ref Sweep sweepA,
DistanceProxy proxyB, ref Sweep sweepB, ref Vector2 axis, ref Vector2 localPoint,
SeparationFunctionType type)
{
Transform xfA, xfB;
sweepA.GetTransform(out xfA, t);
sweepB.GetTransform(out xfB, t);
switch (type)
{
case SeparationFunctionType.Points:
{
var localPointA = proxyA.Vertices[indexA];
var localPointB = proxyB.Vertices[indexB];
var pointA = MathUtils.Mul(ref xfA, localPointA);
var pointB = MathUtils.Mul(ref xfB, localPointB);
var separation = Vector2.Dot(pointB - pointA, axis);
return(separation);
}
case SeparationFunctionType.FaceA:
{
var normal = MathUtils.Mul(ref xfA.q, axis);
var pointA = MathUtils.Mul(ref xfA, localPoint);
var localPointB = proxyB.Vertices[indexB];
var pointB = MathUtils.Mul(ref xfB, localPointB);
var separation = Vector2.Dot(pointB - pointA, normal);
return(separation);
}
case SeparationFunctionType.FaceB:
{
var normal = MathUtils.Mul(ref xfB.q, axis);
var pointB = MathUtils.Mul(ref xfB, localPoint);
var localPointA = proxyA.Vertices[indexA];
var pointA = MathUtils.Mul(ref xfA, localPointA);
var separation = Vector2.Dot(pointA - pointB, normal);
return(separation);
}
default:
Debug.Assert(false);
return(0.0f);
}
}
/// <summary>
/// Reads the cache using the specified cache
/// </summary>
/// <param name="cache">The cache</param>
/// <param name="proxyA">The proxy</param>
/// <param name="transformA">The transform</param>
/// <param name="proxyB">The proxy</param>
/// <param name="transformB">The transform</param>
internal void ReadCache(ref SimplexCache cache, ref DistanceProxy proxyA, ref Transform transformA,
ref DistanceProxy proxyB, ref Transform transformB)
{
Debug.Assert(cache.Count <= 3);
// Copy data from cache.
Count = cache.Count;
for (int i = 0; i < Count; ++i)
{
SimplexVertex v = V[i];
v.IndexA = cache.IndexA[i];
v.IndexB = cache.IndexB[i];
Vector2 wALocal = proxyA.Vertices[v.IndexA];
Vector2 wBLocal = proxyB.Vertices[v.IndexB];
v.Wa = MathUtils.Mul(ref transformA, wALocal);
v.Wb = MathUtils.Mul(ref transformB, wBLocal);
v.W = v.Wb - v.Wa;
v.A = 0.0f;
V[i] = v;
}
// Compute the new simplex metric, if it is substantially different than
// old metric then flush the simplex.
if (Count > 1)
{
float metric1 = cache.Metric;
float metric2 = GetMetric();
if (metric2 < 0.5f * metric1 || 2.0f * metric1 < metric2 || metric2 < MathConstants.Epsilon)
{
// Reset the simplex.
Count = 0;
}
}
// If the cache is empty or invalid ...
if (Count == 0)
{
SimplexVertex v = V[0];
v.IndexA = 0;
v.IndexB = 0;
Vector2 wALocal = proxyA.Vertices[0];
Vector2 wBLocal = proxyB.Vertices[0];
v.Wa = MathUtils.Mul(ref transformA, wALocal);
v.Wb = MathUtils.Mul(ref transformB, wBLocal);
v.W = v.Wb - v.Wa;
v.A = 1.0f;
V[0] = v;
Count = 1;
}
}
public void ReadCache(SimplexCache cache,
DistanceProxy shapeA, ref Transform transformA,
DistanceProxy shapeB, ref Transform transformB)
{
Box2DXDebug.Assert(0 <= cache.Count && cache.Count <= 3);
// Copy data from cache.
Count = cache.Count;
for (int i = 0; i < Count; ++i)
{
SimplexVertex v = Vertices[i];
v.IndexA = cache.IndexA[i];
v.IndexB = cache.IndexB[i];
Vec2 wALocal = shapeA.GetVertex(v.IndexA);
Vec2 wBLocal = shapeB.GetVertex(v.IndexB);
v.WA = Math.Mul(transformA, wALocal);
v.WB = Math.Mul(transformB, wBLocal);
v.W = v.WB - v.WA;
v.A = 0.0f;
}
// Compute the new simplex metric, if it is substantially different than
// old metric then flush the simplex.
if (Count > 1)
{
float metric1 = cache.Metric;
float metric2 = GetMetric();
if (metric2 < 0.5f * metric1 || 2.0f * metric1 < metric2 || metric2 < Settings.FLT_EPSILON)
{
// Reset the simplex.
Count = 0;
}
}
// If the cache is empty or invalid ...
if (Count == 0)
{
SimplexVertex v = Vertices[0];
v.IndexA = 0;
v.IndexB = 0;
Vec2 wALocal = shapeA.GetVertex(0);
Vec2 wBLocal = shapeB.GetVertex(0);
v.WA = Math.Mul(transformA, wALocal);
v.WB = Math.Mul(transformB, wBLocal);
v.W = v.WB - v.WA;
Count = 1;
}
}
internal static unsafe bool PointDistance <T>(PointDistanceInput input, Collider *collider, ref T collector) where T : struct, ICollector <DistanceHit>
{
if (!CollisionFilter.IsCollisionEnabled(input.Filter, collider->Filter))
{
return(false);
}
// Ensure the query context is initialized.
input.QueryContext.EnsureIsInitialized();
var proxySource = new DistanceProxy(1, &input.Position, 0f);
DistanceProxy proxyTarget;
switch (collider->ColliderType)
{
case ColliderType.Box:
case ColliderType.Polygon:
case ColliderType.Capsule:
case ColliderType.Circle:
{
var convexCollider = (ConvexCollider *)collider;
proxyTarget = new DistanceProxy(ref convexCollider->m_ConvexHull);
break;
}
case ColliderType.Compound:
return(PointDistanceCompound(input, (PhysicsCompoundCollider *)collider, ref collector));
default:
SafetyChecks.ThrowNotImplementedException();
return(default);
}
var hit = ColliderDistance(PhysicsTransform.Identity, ref proxySource, ref proxyTarget);
if (hit.Distance < collector.MaxFraction)
{
hit.PhysicsBodyIndex = input.QueryContext.PhysicsBodyIndex;
hit.ColliderKey = input.QueryContext.ColliderKey;
hit.Entity = input.QueryContext.Entity;
hit.PointA = PhysicsMath.mul(input.QueryContext.LocalToWorldTransform, hit.PointA);
hit.PointB = PhysicsMath.mul(input.QueryContext.LocalToWorldTransform, hit.PointB);
return(collector.AddHit(hit));
}
return(false);
}
public void ReadCache(SimplexCache cache, DistanceProxy proxyA, Transform transformA, DistanceProxy proxyB, Transform transformB)
{
Debug.Assert(cache.Count <= 3);
// Copy data from cache.
Count = cache.Count;
for (int i = 0; i < Count; ++i)
{
SimplexVertex v = Vertices[i];
v.IndexA = cache.IndexA[i];
v.IndexB = cache.IndexB[i];
Vec2 wALocal = proxyA.GetVertex(v.IndexA);
Vec2 wBLocal = proxyB.GetVertex(v.IndexB);
Transform.MulToOutUnsafe(transformA, wALocal, v.WA);
Transform.MulToOutUnsafe(transformB, wBLocal, v.WB);
v.W.Set(v.WB).SubLocal(v.WA);
v.A = 0.0f;
}
// Compute the new simplex metric, if it is substantially different than
// old metric then flush the simplex.
if (Count > 1)
{
float metric1 = cache.Metric;
float metric2 = Metric;
if (metric2 < 0.5f * metric1 || 2.0f * metric1 < metric2 || metric2 < Settings.EPSILON)
{
// Reset the simplex.
Count = 0;
}
}
// If the cache is empty or invalid ...
if (Count == 0)
{
SimplexVertex v = Vertices[0];
v.IndexA = 0;
v.IndexB = 0;
Vec2 wALocal = proxyA.GetVertex(0);
Vec2 wBLocal = proxyB.GetVertex(0);
Transform.MulToOutUnsafe(transformA, wALocal, v.WA);
Transform.MulToOutUnsafe(transformB, wBLocal, v.WB);
v.W.Set(v.WB).SubLocal(v.WA);
Count = 1;
}
}
static bool CastConvex(ref ColliderCastInput input, ref DistanceProxy proxyTarget, out ColliderCastHit hit)
{
DistanceProxy proxySource;
var inputColliderBlob = input.Collider;
switch (inputColliderBlob.Value.ColliderType)
{
case ColliderType.Box:
case ColliderType.Polygon:
case ColliderType.Capsule:
case ColliderType.Circle:
{
ref var convexHull = ref inputColliderBlob.GetColliderRef <ConvexCollider>().m_ConvexHull;
proxySource = new DistanceProxy(ref convexHull);
break;
}
void ReadCache(SimplexCache cache, DistanceProxy proxyA, XForm transformA, DistanceProxy proxyB, XForm transformB)
{
// Copy data from cache.
m_count = cache.count;
SimplexVertex[] vertices = m_v1;
for (int i = 0; i < m_count; ++i)
{
SimplexVertex v = vertices[i];
v.indexA = cache.indexA[i];
v.indexB = cache.indexB[i];
Vec2 wALocal = proxyA.GetVertex(v.indexA);
Vec2 wBLocal = proxyB.GetVertex(v.indexB);
v.wA = MathB2.Mul(transformA, wALocal);
v.wB = MathB2.Mul(transformB, wBLocal);
v.w = v.wB - v.wA;
v.a = 0.0f;
}
// Compute the new simplex metric, if it is substantially different than
// old metric then flush the simplex.
if (m_count > 1)
{
float metric1 = cache.metric;
float metric2 = GetMetric();
if (metric2 < 0.5f * metric1 || 2.0f * metric1 < metric2 || metric2 < Settings.FLT_EPSILON)
{
// Reset the simplex.
m_count = 0;
}
}
// If the cache is empty or invalid ...
if (m_count == 0)
{
SimplexVertex v = vertices[0];
v.indexA = 0;
v.indexB = 0;
Vec2 wALocal = proxyA.GetVertex(0);
Vec2 wBLocal = proxyB.GetVertex(0);
v.wA = MathB2.Mul(transformA, wALocal);
v.wB = MathB2.Mul(transformB, wBLocal);
v.w = v.wB - v.wA;
v.a = 1.0f;
m_count = 1;
}
}
private float distanceWithPhysicObj(PhysicObj obj)
{
if (obj == null)
{
return(-1);
}
Fixture proxyAfix = owner.getBoundsFixture();
Fixture proxyBfix = obj.getBoundsFixture();
if (proxyAfix == null || proxyBfix == null)
{
return(-1);
}
DistanceProxy proxyA = new DistanceProxy();
proxyA.Set(proxyAfix.Shape, 0);
DistanceProxy proxyB = new DistanceProxy();
proxyB.Set(proxyBfix.Shape, 1);
DistanceInput distInput = new DistanceInput();
distInput.ProxyA = proxyA;
distInput.ProxyB = proxyB;
Transform transformA;
owner.body.GetTransform(out transformA);
Transform transformB;
obj.body.GetTransform(out transformB);
distInput.TransformA = transformA;
distInput.TransformB = transformB;
DistanceOutput distout = new DistanceOutput();
SimplexCache simplexCache = new SimplexCache();
Distance.ComputeDistance(out distout, out simplexCache, distInput);
return(distout.Distance);
}
public virtual void readCache(SimplexCache cache, DistanceProxy proxyA, Transform transformA, DistanceProxy proxyB, Transform transformB)
{
Debug.Assert(cache.count <= 3);
// Copy data from cache.
m_count = cache.count;
for (int i = 0; i < m_count; ++i)
{
SimplexVertex v = vertices[i];
v.indexA = cache.indexA[i];
v.indexB = cache.indexB[i];
Vec2 wALocal = proxyA.getVertex(v.indexA);
Vec2 wBLocal = proxyB.getVertex(v.indexB);
Transform.mulToOutUnsafe(transformA, wALocal, v.wA);
Transform.mulToOutUnsafe(transformB, wBLocal, v.wB);
v.w.set_Renamed(v.wB).subLocal(v.wA);
v.a = 0.0f;
}
// Compute the new simplex metric, if it is substantially different than
// old metric then flush the simplex.
if (m_count > 1)
{
float metric1 = cache.metric;
float metric2 = Metric;
if (metric2 < 0.5f * metric1 || 2.0f * metric1 < metric2 || metric2 < Settings.EPSILON)
{
// Reset the simplex.
m_count = 0;
}
}
// If the cache is empty or invalid ...
if (m_count == 0)
{
SimplexVertex v = vertices[0];
v.indexA = 0;
v.indexB = 0;
Vec2 wALocal = proxyA.getVertex(0);
Vec2 wBLocal = proxyB.getVertex(0);
Transform.mulToOutUnsafe(transformA, wALocal, v.wA);
Transform.mulToOutUnsafe(transformB, wBLocal, v.wB);
v.w.set_Renamed(v.wB).subLocal(v.wA);
m_count = 1;
}
}
public void Initialize(SimplexCache cache,
DistanceProxy proxyA, Transform transformA,
DistanceProxy proxyB, Transform transformB)
{
_proxyA = proxyA;
_proxyB = proxyB;
int count = cache.Count;
Box2DXDebug.Assert(0 < count && count < 3);
if (count == 1)
{
_type = Type.Points;
Vec2 localPointA = _proxyA.GetVertex(cache.IndexA[0]);
Vec2 localPointB = _proxyB.GetVertex(cache.IndexB[0]);
Vec2 pointA = Math.Mul(transformA, localPointA);
Vec2 pointB = Math.Mul(transformB, localPointB);
_axis = pointB - pointA;
_axis.Normalize();
}
else if (cache.IndexB[0] == cache.IndexB[1])
{
// Two points on A and one on B
_type = Type.FaceA;
Vec2 localPointA1 = _proxyA.GetVertex(cache.IndexA[0]);
Vec2 localPointA2 = _proxyA.GetVertex(cache.IndexA[1]);
Vec2 localPointB = _proxyB.GetVertex(cache.IndexB[0]);
_localPoint = 0.5f*(localPointA1 + localPointA2);
_axis = Vec2.Cross(localPointA2 - localPointA1, 1.0f);
_axis.Normalize();
Vec2 normal = Math.Mul(transformA.R, _axis);
Vec2 pointA = Math.Mul(transformA, _localPoint);
Vec2 pointB = Math.Mul(transformB, localPointB);
float s = Vec2.Dot(pointB - pointA, normal);
if (s < 0.0f)
{
_axis = -_axis;
}
}
else if (cache.IndexA[0] == cache.IndexA[1])
{
// Two points on B and one on A.
_type = Type.FaceB;
Vec2 localPointA = proxyA.GetVertex(cache.IndexA[0]);
Vec2 localPointB1 = proxyB.GetVertex(cache.IndexB[0]);
Vec2 localPointB2 = proxyB.GetVertex(cache.IndexB[1]);
_localPoint = 0.5f*(localPointB1 + localPointB2);
_axis = Vec2.Cross(localPointB2 - localPointB1, 1.0f);
_axis.Normalize();
Vec2 normal = Math.Mul(transformB.R, _axis);
Vec2 pointB = Math.Mul(transformB, _localPoint);
Vec2 pointA = Math.Mul(transformA, localPointA);
float s = Vec2.Dot(pointA - pointB, normal);
if (s < 0.0f)
{
_axis = -_axis;
}
}
else
{
// Two points on B and two points on A.
// The faces are parallel.
Vec2 localPointA1 = _proxyA.GetVertex(cache.IndexA[0]);
Vec2 localPointA2 = _proxyA.GetVertex(cache.IndexA[1]);
Vec2 localPointB1 = _proxyB.GetVertex(cache.IndexB[0]);
Vec2 localPointB2 = _proxyB.GetVertex(cache.IndexB[1]);
Vec2 pA = Math.Mul(transformA, localPointA1);
Vec2 dA = Math.Mul(transformA.R, localPointA2 - localPointA1);
Vec2 pB = Math.Mul(transformB, localPointB1);
Vec2 dB = Math.Mul(transformB.R, localPointB2 - localPointB1);
float a = Vec2.Dot(dA, dA);
float e = Vec2.Dot(dB, dB);
Vec2 r = pA - pB;
float c = Vec2.Dot(dA, r);
float f = Vec2.Dot(dB, r);
float b = Vec2.Dot(dA, dB);
float denom = a*e - b*b;
float s = 0.0f;
if (denom != 0.0f)
{
s = Math.Clamp((b*f - c*e)/denom, 0.0f, 1.0f);
}
float t = (b*s + f)/e;
if (t < 0.0f)
{
t = 0.0f;
s = Math.Clamp(-c/a, 0.0f, 1.0f);
}
else if (t > 1.0f)
{
t = 1.0f;
s = Math.Clamp((b - c)/a, 0.0f, 1.0f);
}
Vec2 localPointA = localPointA1 + s*(localPointA2 - localPointA1);
Vec2 localPointB = localPointB1 + t*(localPointB2 - localPointB1);
if (s == 0.0f || s == 1.0f)
{
_type = Type.FaceB;
_axis = Vec2.Cross(localPointB2 - localPointB1, 1.0f);
_axis.Normalize();
_localPoint = localPointB;
Vec2 normal = Math.Mul(transformB.R, _axis);
Vec2 pointA = Math.Mul(transformA, localPointA);
Vec2 pointB = Math.Mul(transformB, localPointB);
float sgn = Vec2.Dot(pointA - pointB, normal);
if (sgn < 0.0f)
{
_axis = -_axis;
}
}
else
{
_type = Type.FaceA;
_axis = Vec2.Cross(localPointA2 - localPointA1, 1.0f);
_axis.Normalize();
_localPoint = localPointA;
Vec2 normal = Math.Mul(transformA.R, _axis);
Vec2 pointA = Math.Mul(transformA, localPointA);
Vec2 pointB = Math.Mul(transformB, localPointB);
float sgn = Vec2.Dot(pointB - pointA, normal);
if (sgn < 0.0f)
{
_axis = -_axis;
}
}
}
}
/// <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;
}
}
}
/// <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);
}
public void Initialize(SimplexCache cache,
DistanceProxy proxyA, Transform transformA,
DistanceProxy proxyB, Transform transformB)
{
_proxyA = proxyA;
_proxyB = proxyB;
int count = cache.Count;
Box2DXDebug.Assert(0 < count && count < 3);
if (count == 1)
{
_type = Type.Points;
Vec2 localPointA = _proxyA.GetVertex(cache.IndexA[0]);
Vec2 localPointB = _proxyB.GetVertex(cache.IndexB[0]);
Vec2 pointA = Math.Mul(transformA, localPointA);
Vec2 pointB = Math.Mul(transformB, localPointB);
_axis = pointB - pointA;
_axis.Normalize();
}
else if (cache.IndexB[0] == cache.IndexB[1])
{
// Two points on A and one on B
_type = Type.FaceA;
Vec2 localPointA1 = _proxyA.GetVertex(cache.IndexA[0]);
Vec2 localPointA2 = _proxyA.GetVertex(cache.IndexA[1]);
Vec2 localPointB = _proxyB.GetVertex(cache.IndexB[0]);
_localPoint = 0.5f * (localPointA1 + localPointA2);
_axis = Vec2.Cross(localPointA2 - localPointA1, 1.0f);
_axis.Normalize();
Vec2 normal = Math.Mul(transformA.R, _axis);
Vec2 pointA = Math.Mul(transformA, _localPoint);
Vec2 pointB = Math.Mul(transformB, localPointB);
float s = Vec2.Dot(pointB - pointA, normal);
if (s < 0.0f)
{
_axis = -_axis;
}
}
else if (cache.IndexA[0] == cache.IndexA[1])
{
// Two points on B and one on A.
_type = Type.FaceB;
Vec2 localPointA = proxyA.GetVertex(cache.IndexA[0]);
Vec2 localPointB1 = proxyB.GetVertex(cache.IndexB[0]);
Vec2 localPointB2 = proxyB.GetVertex(cache.IndexB[1]);
_localPoint = 0.5f * (localPointB1 + localPointB2);
_axis = Vec2.Cross(localPointB2 - localPointB1, 1.0f);
_axis.Normalize();
Vec2 normal = Math.Mul(transformB.R, _axis);
Vec2 pointB = Math.Mul(transformB, _localPoint);
Vec2 pointA = Math.Mul(transformA, localPointA);
float s = Vec2.Dot(pointA - pointB, normal);
if (s < 0.0f)
{
_axis = -_axis;
}
}
else
{
// Two points on B and two points on A.
// The faces are parallel.
Vec2 localPointA1 = _proxyA.GetVertex(cache.IndexA[0]);
Vec2 localPointA2 = _proxyA.GetVertex(cache.IndexA[1]);
Vec2 localPointB1 = _proxyB.GetVertex(cache.IndexB[0]);
Vec2 localPointB2 = _proxyB.GetVertex(cache.IndexB[1]);
Vec2 pA = Math.Mul(transformA, localPointA1);
Vec2 dA = Math.Mul(transformA.R, localPointA2 - localPointA1);
Vec2 pB = Math.Mul(transformB, localPointB1);
Vec2 dB = Math.Mul(transformB.R, localPointB2 - localPointB1);
float a = Vec2.Dot(dA, dA);
float e = Vec2.Dot(dB, dB);
Vec2 r = pA - pB;
float c = Vec2.Dot(dA, r);
float f = Vec2.Dot(dB, r);
float b = Vec2.Dot(dA, dB);
float denom = a * e - b * b;
float s = 0.0f;
if (denom != 0.0f)
{
s = Math.Clamp((b * f - c * e) / denom, 0.0f, 1.0f);
}
float t = (b * s + f) / e;
if (t < 0.0f)
{
t = 0.0f;
s = Math.Clamp(-c / a, 0.0f, 1.0f);
}
else if (t > 1.0f)
{
t = 1.0f;
s = Math.Clamp((b - c) / a, 0.0f, 1.0f);
}
Vec2 localPointA = localPointA1 + s * (localPointA2 - localPointA1);
Vec2 localPointB = localPointB1 + t * (localPointB2 - localPointB1);
if (s == 0.0f || s == 1.0f)
{
_type = Type.FaceB;
_axis = Vec2.Cross(localPointB2 - localPointB1, 1.0f);
_axis.Normalize();
_localPoint = localPointB;
Vec2 normal = Math.Mul(transformB.R, _axis);
Vec2 pointA = Math.Mul(transformA, localPointA);
Vec2 pointB = Math.Mul(transformB, localPointB);
float sgn = Vec2.Dot(pointA - pointB, normal);
if (sgn < 0.0f)
{
_axis = -_axis;
}
}
else
{
_type = Type.FaceA;
_axis = Vec2.Cross(localPointA2 - localPointA1, 1.0f);
_axis.Normalize();
_localPoint = localPointA;
Vec2 normal = Math.Mul(transformA.R, _axis);
Vec2 pointA = Math.Mul(transformA, localPointA);
Vec2 pointB = Math.Mul(transformB, localPointB);
float sgn = Vec2.Dot(pointB - pointA, normal);
if (sgn < 0.0f)
{
_axis = -_axis;
}
}
}
}
public static void Initialize(ref SimplexCache cache, DistanceProxy proxyA, ref Sweep sweepA, DistanceProxy proxyB, ref Sweep sweepB, float t1, out Vector2 axis, out Vector2 localPoint, out SeparationFunctionType type)
{
int count = cache.Count;
Debug.Assert(0 < count && count < 3);
sweepA.GetTransform(out Transform xfA, t1);
sweepB.GetTransform(out Transform xfB, t1);
if (count == 1)
{
localPoint = Vector2.Zero;
type = SeparationFunctionType.Points;
Vector2 localPointA = proxyA._vertices[cache.IndexA[0]];
Vector2 localPointB = proxyB._vertices[cache.IndexB[0]];
Vector2 pointA = MathUtils.Mul(ref xfA, localPointA);
Vector2 pointB = MathUtils.Mul(ref xfB, localPointB);
axis = pointB - pointA;
axis.Normalize();
}
else if (cache.IndexA[0] == cache.IndexA[1])
{
// Two points on B and one on A.
type = SeparationFunctionType.FaceB;
Vector2 localPointB1 = proxyB._vertices[cache.IndexB[0]];
Vector2 localPointB2 = proxyB._vertices[cache.IndexB[1]];
Vector2 a = localPointB2 - localPointB1;
axis = new Vector2(a.Y, -a.X);
axis.Normalize();
Vector2 normal = MathUtils.Mul(ref xfB.q, axis);
localPoint = 0.5f * (localPointB1 + localPointB2);
Vector2 pointB = MathUtils.Mul(ref xfB, localPoint);
Vector2 localPointA = proxyA._vertices[cache.IndexA[0]];
Vector2 pointA = MathUtils.Mul(ref xfA, localPointA);
float s = Vector2.Dot(pointA - pointB, normal);
if (s < 0.0f)
{
axis = -axis;
}
}
else
{
// Two points on A and one or two points on B.
type = SeparationFunctionType.FaceA;
Vector2 localPointA1 = proxyA._vertices[cache.IndexA[0]];
Vector2 localPointA2 = proxyA._vertices[cache.IndexA[1]];
Vector2 a = localPointA2 - localPointA1;
axis = new Vector2(a.Y, -a.X);
axis.Normalize();
Vector2 normal = MathUtils.Mul(ref xfA.q, axis);
localPoint = 0.5f * (localPointA1 + localPointA2);
Vector2 pointA = MathUtils.Mul(ref xfA, localPoint);
Vector2 localPointB = proxyB._vertices[cache.IndexB[0]];
Vector2 pointB = MathUtils.Mul(ref xfB, localPointB);
float s = Vector2.Dot(pointB - pointA, normal);
if (s < 0.0f)
{
axis = -axis;
}
}
//Velcro note: the returned value that used to be here has been removed, as it was not used.
}
// CCD via the local separating axis method. This seeks progression
// by computing the largest time at which separation is maintained.
public static bool TimeOfImpact(
DistanceProxy proxyA,
DistanceProxy proxyB,
Sweep sweepA,
Sweep sweepB,
float tMax,
out float t)
{
// Large rotations can make the root finder fail, so we normalize the
// sweep angles.
sweepA.Normalize();
sweepB.Normalize();
var totalRadius = proxyA.Radius + proxyB.Radius;
var target = System.Math.Max(Settings.LinearSlop, totalRadius - 3.0f * Settings.LinearSlop);
const float tolerance = 0.25f * Settings.LinearSlop;
System.Diagnostics.Debug.Assert(target > tolerance);
// Prepare input for distance query.
var cache = new SimplexCache {
Count = 0
};
// The outer loop progressively attempts to compute new separating axes.
// This loop terminates when an axis is repeated (no progress is made).
var t1 = 0.0f;
for (var iter = 0; iter < 20; ++iter)
{
WorldTransform xfA, xfB;
sweepA.GetTransform(out xfA, t1);
sweepB.GetTransform(out xfB, t1);
// Get the distance between shapes. We can also use the results
// to get a separating axis.
var distance = Distance(ref cache, proxyA, xfA, proxyB, xfB);
// If the shapes are overlapped, we give up on continuous collision.
if (distance <= 0.0f)
{
// Failure!
t = 0.0f;
return(false);
}
if (distance < target + tolerance)
{
// Victory!
t = t1;
return(true);
}
// Initialize the separating axis.
SeparationFunction fcn;
SeparationFunction.Initialize(out fcn, cache, proxyA, proxyB, sweepA, sweepB, t1);
// Compute the TOI on the separating axis. We do this by successively
// resolving the deepest point. This loop is bounded by the number of vertices.
var t2 = tMax;
for (var pushBackIter = 0; pushBackIter < Settings.MaxPolygonVertices; ++pushBackIter)
{
// Find the deepest point at t2. Store the witness point indices.
int indexA, indexB;
var s2 = fcn.FindMinSeparation(out indexA, out indexB, t2);
// Is the final configuration separated?
if (s2 > target + tolerance)
{
// Victory!
t = tMax;
return(false);
}
// Has the separation reached tolerance?
if (s2 > target - tolerance)
{
// Advance the sweeps
t1 = t2;
break;
}
// Compute the initial separation of the witness points.
var s1 = fcn.Evaluate(indexA, indexB, t1);
// Check for initial overlap. This might happen if the root finder
// runs out of iterations.
if (s1 < target - tolerance)
{
t = t1;
return(false);
}
// Check for touching
if (s1 <= target + tolerance)
{
// Victory! t1 should hold the TOI (could be 0.0).
t = t1;
return(true);
}
// Compute 1D root of: f(x) - target = 0
float a1 = t1, a2 = t2;
for (var rootIterCount = 0; rootIterCount < 50; ++rootIterCount)
{
// Use a mix of the secant rule and bisection.
float u;
if ((rootIterCount & 1) != 0)
{
// Secant rule to improve convergence.
u = a1 + (target - s1) * (a2 - a1) / (s2 - s1);
}
else
{
// Bisection to guarantee progress.
u = 0.5f * (a1 + a2);
}
var s = fcn.Evaluate(indexA, indexB, u);
if (System.Math.Abs(s - target) < tolerance)
{
// t2 holds a tentative value for t1
t2 = u;
break;
}
// Ensure we continue to bracket the root.
if (s > target)
{
a1 = u;
s1 = s;
}
else
{
a2 = u;
s2 = s;
}
}
}
}
// Root finder got stuck. Semi-victory.
t = t1;
return(false);
}
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
});
}
public void ReadCache(SimplexCache cache, DistanceProxy proxyA, Transform transformA, DistanceProxy proxyB, Transform transformB)
{
Debug.Assert(cache.Count <= 3);
// Copy data from cache.
Count = cache.Count;
for (int i = 0; i < Count; ++i)
{
SimplexVertex v = Vertices[i];
v.IndexA = cache.IndexA[i];
v.IndexB = cache.IndexB[i];
Vec2 wALocal = proxyA.GetVertex(v.IndexA);
Vec2 wBLocal = proxyB.GetVertex(v.IndexB);
Transform.MulToOutUnsafe(transformA, wALocal, v.WA);
Transform.MulToOutUnsafe(transformB, wBLocal, v.WB);
v.W.Set(v.WB).SubLocal(v.WA);
v.A = 0.0f;
}
// Compute the new simplex metric, if it is substantially different than
// old metric then flush the simplex.
if (Count > 1)
{
float metric1 = cache.Metric;
float metric2 = Metric;
if (metric2 < 0.5f * metric1 || 2.0f * metric1 < metric2 || metric2 < Settings.EPSILON)
{
// Reset the simplex.
Count = 0;
}
}
// If the cache is empty or invalid ...
if (Count == 0)
{
SimplexVertex v = Vertices[0];
v.IndexA = 0;
v.IndexB = 0;
Vec2 wALocal = proxyA.GetVertex(0);
Vec2 wBLocal = proxyB.GetVertex(0);
Transform.MulToOutUnsafe(transformA, wALocal, v.WA);
Transform.MulToOutUnsafe(transformB, wBLocal, v.WB);
v.W.Set(v.WB).SubLocal(v.WA);
Count = 1;
}
}
/// <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;
}
}
}
public void ReadCache(SimplexCache cache,
DistanceProxy shapeA, ref Transform transformA,
DistanceProxy shapeB, ref Transform transformB)
{
Box2DXDebug.Assert(0 <= cache.Count && cache.Count <= 3);
// Copy data from cache.
Count = cache.Count;
for (int i = 0; i < Count; ++i)
{
SimplexVertex v = Vertices[i];
v.IndexA = cache.IndexA[i];
v.IndexB = cache.IndexB[i];
Vec2 wALocal = shapeA.GetVertex(v.IndexA);
Vec2 wBLocal = shapeB.GetVertex(v.IndexB);
v.WA = Math.Mul(transformA, wALocal);
v.WB = Math.Mul(transformB, wBLocal);
v.W = v.WB - v.WA;
v.A = 0.0f;
}
// Compute the new simplex metric, if it is substantially different than
// old metric then flush the simplex.
if (Count > 1)
{
float metric1 = cache.Metric;
float metric2 = GetMetric();
if (metric2 < 0.5f * metric1 || 2.0f * metric1 < metric2 || metric2 < Settings.FLT_EPSILON)
{
// Reset the simplex.
Count = 0;
}
}
// If the cache is empty or invalid ...
if (Count == 0)
{
SimplexVertex v = Vertices[0];
v.IndexA = 0;
v.IndexB = 0;
Vec2 wALocal = shapeA.GetVertex(0);
Vec2 wBLocal = shapeB.GetVertex(0);
v.WA = Math.Mul(transformA, wALocal);
v.WB = Math.Mul(transformB, wBLocal);
v.W = v.WB - v.WA;
Count = 1;
}
}
// TODO_ERIN might not need to return the separation
public float Initialize(
ref SimplexCache cache,
DistanceProxy proxyA,
in Sweep sweepA,
public static void Initialize(
out SeparationFunction f,
SimplexCache cache,
DistanceProxy proxyA,
DistanceProxy proxyB,
Sweep sweepA,
Sweep sweepB,
float t1)
{
System.Diagnostics.Debug.Assert(0 < cache.Count && cache.Count < 3);
f._proxyA = proxyA;
f._proxyB = proxyB;
f._sweepA = sweepA;
f._sweepB = sweepB;
WorldTransform xfA, xfB;
f._sweepA.GetTransform(out xfA, t1);
f._sweepB.GetTransform(out xfB, t1);
if (cache.Count == 1)
{
f._type = Type.Points;
var localPointA = f._proxyA.Vertices[cache.IndexA.Item1];
var localPointB = f._proxyB.Vertices[cache.IndexB.Item1];
var pointA = xfA.ToGlobal(localPointA);
var pointB = xfB.ToGlobal(localPointB);
// ReSharper disable RedundantCast Necessary for FarPhysics.
f._axis = (Vector2)(pointB - pointA);
// ReSharper restore RedundantCast
f._axis.Normalize();
f._localPoint = LocalPoint.Zero;
}
else if (cache.IndexA.Item1 == cache.IndexA.Item2)
{
// Two points on B and one on A.
f._type = Type.FaceB;
var localPointB1 = proxyB.Vertices[cache.IndexB.Item1];
var localPointB2 = proxyB.Vertices[cache.IndexB.Item2];
f._axis = Vector2Util.Cross(localPointB2 - localPointB1, 1.0f);
f._axis.Normalize();
var normal = xfB.Rotation * f._axis;
f._localPoint = 0.5f * (localPointB1 + localPointB2);
var pointB = xfB.ToGlobal(f._localPoint);
var localPointA = proxyA.Vertices[cache.IndexA[0]];
var pointA = xfA.ToGlobal(localPointA);
// ReSharper disable RedundantCast Necessary for FarPhysics.
var s = Vector2Util.Dot((Vector2)(pointA - pointB), normal);
// ReSharper restore RedundantCast
if (s < 0.0f)
{
f._axis = -f._axis;
}
}
else
{
// Two points on A and one or two points on B.
f._type = Type.FaceA;
var localPointA1 = f._proxyA.Vertices[cache.IndexA.Item1];
var localPointA2 = f._proxyA.Vertices[cache.IndexA.Item2];
f._axis = Vector2Util.Cross(localPointA2 - localPointA1, 1.0f);
f._axis.Normalize();
var normal = xfA.Rotation * f._axis;
f._localPoint = 0.5f * (localPointA1 + localPointA2);
var pointA = xfA.ToGlobal(f._localPoint);
var localPointB = f._proxyB.Vertices[cache.IndexB[0]];
var pointB = xfB.ToGlobal(localPointB);
// ReSharper disable RedundantCast Necessary for FarPhysics.
var s = Vector2Util.Dot((Vector2)(pointB - pointA), normal);
// ReSharper restore RedundantCast
if (s < 0.0f)
{
f._axis = -f._axis;
}
}
}
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);
}
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>
/// CCD via the secant method.
/// Compute the time when two shapes begin to touch or touch at a closer distance.
/// TOI considers the shape radii. It attempts to have the radii overlap by the tolerance.
/// Iterations terminate with the overlap is within 0.5 * tolerance. The tolerance should be
/// smaller than sum of the shape radii.
/// @warning the sweeps must have the same time interval.
/// fraction=0 means the shapes begin touching/overlapped, and fraction=1 means the shapes don't touch.
/// </summary>
/// <param name="input">The input.</param>
/// <param name="shapeA">The shape A.</param>
/// <param name="shapeB">The shape B.</param>
/// <returns>
/// fraction between [0,1] in which the shapes first touch.
/// </returns>
public static float TimeOfImpact(TOIInput input)
{
++ToiCalls;
DistanceProxy proxyA = input.ProxyA;
DistanceProxy proxyB = input.ProxyB;
Sweep sweepA = input.SweepA;
Sweep sweepB = input.SweepB;
Box2DXDebug.Assert(sweepA.T0 == sweepB.T0);
Box2DXDebug.Assert(1.0f - sweepA.T0 > Settings.FLT_EPSILON);
float radius = proxyA._radius + proxyB._radius;
float tolerance = input.Tolerance;
float alpha = 0.0f;
const int k_maxIterations = 1000; // TODO_ERIN b2Settings
int iter = 0;
float target = 0.0f;
// Prepare input for distance query.
SimplexCache cache = new SimplexCache();
cache.Count = 0;
DistanceInput distanceInput = new DistanceInput();
distanceInput.proxyA = input.ProxyA;
distanceInput.proxyB = input.ProxyB;
distanceInput.UseRadii = false;
for (;;)
{
Transform xfA, xfB;
sweepA.GetTransform(out xfA, alpha);
sweepB.GetTransform(out xfB, alpha);
// Get the distance between shapes.
distanceInput.TransformA = xfA;
distanceInput.TransformB = xfB;
DistanceOutput distanceOutput;
Distance(out distanceOutput, cache, distanceInput);
if (distanceOutput.Distance <= 0.0f)
{
alpha = 1.0f;
break;
}
SeparationFunction fcn = new SeparationFunction();
fcn.Initialize(cache, proxyA, xfA, proxyB, xfB);
float separation = fcn.Evaluate(xfA, xfB);
if (separation <= 0.0f)
{
alpha = 1.0f;
break;
}
if (iter == 0)
{
// Compute a reasonable target distance to give some breathing room
// for conservative advancement. We take advantage of the shape radii
// to create additional clearance.
if (separation > radius)
{
target = Math.Max(radius - tolerance, 0.75f * radius);
}
else
{
target = Math.Max(separation - tolerance, 0.02f * radius);
}
}
if (separation - target < 0.5f * tolerance)
{
if (iter == 0)
{
alpha = 1.0f;
break;
}
break;
}
#if false
// Dump the curve seen by the root finder
{
const int N = 100;
float dx = 1.0f / N;
float xs[N + 1];