public void Set(SimplexCache sc)
{
Array.Copy(sc.IndexA, 0, IndexA, 0, IndexA.Length);
Array.Copy(sc.IndexB, 0, IndexB, 0, IndexB.Length);
Metric = sc.Metric;
Count = sc.Count;
}
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;
}
}
public void WriteCache(ref SimplexCache cache)
{
cache.Metric = GetMetric();
cache.Count = (ushort)Count;
for (var i = 0; i < Count; ++i)
{
cache.IndexA[i] = (byte)Vertices[i].IndexA;
cache.IndexB[i] = (byte)Vertices[i].IndexB;
}
}
public void WriteCache(SimplexCache cache)
{
cache.Metric = Metric;
cache.Count = Count;
for (int i = 0; i < Count; ++i)
{
cache.IndexA[i] = (Vertices[i].IndexA);
cache.IndexB[i] = (Vertices[i].IndexB);
}
}
/// <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 override void Update(GameSettings settings, GameTime gameTime)
{
base.Update(settings, gameTime);
DistanceInput input = new DistanceInput();
input.ProxyA = new DistanceProxy(_polygonA, 0);
input.ProxyB = new DistanceProxy(_polygonB, 0);
input.TransformA = _transformA;
input.TransformB = _transformB;
input.UseRadii = true;
SimplexCache cache = new SimplexCache();
cache.Count = 0;
DistanceOutput output;
DistanceGJK.ComputeDistance(ref input, out output, out cache);
DrawString($"distance = {output.Distance}");
DrawString($"iterations = {output.Iterations}");
DebugView.BeginCustomDraw(ref GameInstance.Projection, ref GameInstance.View);
{
Color color = new Color(0.9f, 0.9f, 0.9f);
Vector2[] v = new Vector2[Settings.MaxPolygonVertices];
for (int i = 0; i < _polygonA.Vertices.Count; ++i)
{
v[i] = MathUtils.Mul(ref _transformA, _polygonA.Vertices[i]);
}
DebugView.DrawPolygon(v, _polygonA.Vertices.Count, color);
for (int i = 0; i < _polygonB.Vertices.Count; ++i)
{
v[i] = MathUtils.Mul(ref _transformB, _polygonB.Vertices[i]);
}
DebugView.DrawPolygon(v, _polygonB.Vertices.Count, color);
}
Vector2 x1 = output.PointA;
Vector2 x2 = output.PointB;
Color c1 = new Color(1.0f, 0.0f, 0.0f);
DebugView.DrawPoint(x1, 4.0f, c1);
Color c2 = new Color(1.0f, 1.0f, 0.0f);
DebugView.DrawPoint(x2, 4.0f, c2);
DebugView.EndCustomDraw();
}
public void Step(TestSettings settings)
{
base.Step(settings);
DistanceInput input = new DistanceInput();
input.proxyA.Set(m_polygonA, 0);
input.proxyB.Set(m_polygonB, 0);
input.transformA = m_transformA;
input.transformB = m_transformB;
input.useRadii = true;
SimplexCache cache = new SimplexCache();
cache.count = 0;
DistanceOutput output;
Utilities.Distance(out output, cache, input);
m_debugDraw.DrawString("distance = %g", output.distance);
m_debugDraw.DrawString("iterations = %d", output.iterations);
{
Color color = Color.FromArgb(225, 225, 225);
Vec2[] v = new Vec2[Settings._maxPolygonVertices];
for (int i = 0; i < m_polygonA.m_count; ++i)
{
v[i] = Utilities.Mul(m_transformA, m_polygonA.m_vertices[i]);
}
m_debugDraw.DrawPolygon(v, m_polygonA.m_count, color);
for (int i = 0; i < m_polygonB.m_count; ++i)
{
v[i] = Utilities.Mul(m_transformB, m_polygonB.m_vertices[i]);
}
m_debugDraw.DrawPolygon(v, m_polygonB.m_count, color);
}
Vec2 x1 = output.pointA;
Vec2 x2 = output.pointB;
Color c1 = Color.FromArgb(255, 0, 0);
m_debugDraw.DrawPoint(x1, 4.0f, c1);
Color c2 = Color.FromArgb(255, 255, 0);
m_debugDraw.DrawPoint(x2, 4.0f, c2);
}
public override void Step(Settings settings)
{
base.Step(settings);
DistanceInput input = new DistanceInput();
input.TransformA = _transformA;
input.TransformB = _transformB;
input.UseRadii = true;
SimplexCache cache = new SimplexCache();
cache.Count = 0;
DistanceOutput output;
Collision.Distance(out output, ref cache, ref input, _polygonA, _polygonB);
StringBuilder strBld = new StringBuilder();
strBld.AppendFormat("distance = {0}", new object[] { output.Distance });
OpenGLDebugDraw.DrawString(5, _textLine, strBld.ToString());
_textLine += 15;
strBld = new StringBuilder();
strBld.AppendFormat("iterations = {0}", new object[] { output.Iterations });
OpenGLDebugDraw.DrawString(5, _textLine, strBld.ToString());
_textLine += 15;
{
Color color = new Color(0.9f, 0.9f, 0.9f);
int i;
for (i = 0; i < _polygonA.VertexCount; ++i)
{
_dv[i] = Math.Mul(_transformA, _polygonA.Vertices[i]);
}
_debugDraw.DrawPolygon(_dv, _polygonA.VertexCount, color);
for (i = 0; i < _polygonB.VertexCount; ++i)
{
_dv[i] = Math.Mul(_transformB, _polygonB.Vertices[i]);
}
_debugDraw.DrawPolygon(_dv, _polygonB.VertexCount, color);
}
Vec2 x1 = output.PointA;
Vec2 x2 = output.PointB;
OpenGLDebugDraw.DrawPoint(x1, 4.0f, new Color(1, 0, 0));
OpenGLDebugDraw.DrawSegment(x1, x2, new Color(1, 1, 0));
OpenGLDebugDraw.DrawPoint(x2, 4.0f, new Color(1, 0, 0));
}
public override void Step(Framework.Settings settings)
{
base.Step(settings);
DistanceInput input = new DistanceInput();
input.proxyA.Set(_polygonA, 0);
input.proxyB.Set(_polygonB, 0);
input.transformA = _transformA;
input.transformB = _transformB;
input.useRadii = true;
SimplexCache cache = new SimplexCache();
cache.count = 0;
DistanceOutput output = new DistanceOutput();
Distance.ComputeDistance(out output, out cache, ref input);
_debugDraw.DrawString(50, _textLine, "distance = {0:n}", output.distance);
_textLine += 15;
_debugDraw.DrawString(50, _textLine, "iterations = {0:n}", output.iterations);
_textLine += 15;
{
Color color = new Color(0.9f, 0.9f, 0.9f);
FixedArray8 <Vector2> v = new FixedArray8 <Vector2>();
for (int i = 0; i < _polygonA._vertexCount; ++i)
{
v[i] = MathUtils.Multiply(ref _transformA, _polygonA._vertices[i]);
}
_debugDraw.DrawPolygon(ref v, _polygonA._vertexCount, color);
for (int i = 0; i < _polygonB._vertexCount; ++i)
{
v[i] = MathUtils.Multiply(ref _transformB, _polygonB._vertices[i]);
}
_debugDraw.DrawPolygon(ref v, _polygonB._vertexCount, color);
}
Vector2 x1 = output.pointA;
Vector2 x2 = output.pointB;
_debugDraw.DrawPoint(x1, 0.5f, new Color(1.0f, 0.0f, 0.0f));
_debugDraw.DrawPoint(x2, 0.5f, new Color(1.0f, 0.0f, 0.0f));
_debugDraw.DrawSegment(x1, x2, new Color(1.0f, 1.0f, 0.0f));
}
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;
}
}
public override void OnRender()
{
var input = new DistanceInput();
input.ProxyA.Set(_polygonA, 0);
input.ProxyB.Set(_polygonB, 0);
input.TransformA = _transformA;
input.TransformB = _transformB;
input.UseRadii = true;
var cache = new SimplexCache();
DistanceAlgorithm.Distance(out var output, ref cache, input);
DrawString($"distance = {output.Distance}");
DrawString($"iterations = {output.Iterations}");
{
var color = Color.FromArgb(230, 230, 230);
var v = new Vector2[Settings.MaxPolygonVertices];
for (var i = 0; i < _polygonA.Count; ++i)
{
v[i] = MathUtils.Mul(_transformA, _polygonA.Vertices[i]);
}
Drawer.DrawPolygon(v, _polygonA.Count, color);
for (var i = 0; i < _polygonB.Count; ++i)
{
v[i] = MathUtils.Mul(_transformB, _polygonB.Vertices[i]);
}
Drawer.DrawPolygon(v, _polygonB.Count, color);
}
var x1 = output.PointA;
var x2 = output.PointB;
var c1 = Color.FromArgb(255, 0, 0);
Drawer.DrawPoint(x1, 4.0f, c1);
var c2 = Color.FromArgb(255, 255, 0);
Drawer.DrawPoint(x2, 4.0f, c2);
}
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 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;
}
}
}
// 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);
}
/// <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 override void Step(Framework.Settings settings)
{
base.Step(settings);
DistanceInput input = new DistanceInput();
input.proxyA.Set(_polygonA, 0);
input.proxyB.Set(_polygonB, 0);
input.transformA = _transformA;
input.transformB = _transformB;
input.useRadii = true;
SimplexCache cache = new SimplexCache();
cache.count = 0;
DistanceOutput output = new DistanceOutput();
Distance.ComputeDistance( out output, out cache, ref input);
_debugDraw.DrawString(50, _textLine, "distance = {0:n}", output.distance);
_textLine += 15;
_debugDraw.DrawString(50, _textLine, "iterations = {0:n}", output.iterations);
_textLine += 15;
{
Color color = new Color(0.9f, 0.9f, 0.9f);
FixedArray8<Vector2> v = new FixedArray8<Vector2>();
for (int i = 0; i < _polygonA._vertexCount; ++i)
{
v[i] = MathUtils.Multiply(ref _transformA, _polygonA._vertices[i]);
}
_debugDraw.DrawPolygon(ref v, _polygonA._vertexCount, color);
for (int i = 0; i < _polygonB._vertexCount; ++i)
{
v[i] = MathUtils.Multiply(ref _transformB, _polygonB._vertices[i]);
}
_debugDraw.DrawPolygon(ref v, _polygonB._vertexCount, color);
}
Vector2 x1 = output.pointA;
Vector2 x2 = output.pointB;
_debugDraw.DrawPoint(x1, 0.5f, new Color(1.0f, 0.0f, 0.0f));
_debugDraw.DrawPoint(x2, 0.5f, new Color(1.0f, 0.0f, 0.0f));
_debugDraw.DrawSegment(x1, x2, new Color(1.0f, 1.0f, 0.0f));
}
// TODO_ERIN might not need to return the separation
public float Initialize(
ref SimplexCache cache,
DistanceProxy proxyA,
in Sweep sweepA,
public override void OnRender()
{
var transformA = new Transform {
Position = new Vector2(0.0f, 0.25f)
};
transformA.Rotation.SetIdentity();
var transformB = new Transform();
transformB.SetIdentity();
var input = new ShapeCastInput();
input.ProxyA.Set(_vAs, _countA, _radiusA);
input.ProxyB.Set(_vBs, _countB, _radiusB);
input.TransformA = transformA;
input.TransformB = transformB;
input.TranslationB.Set(8.0f, 0.0f);
var hit = DistanceAlgorithm.ShapeCast(out var output, input);
var transformB2 = new Transform
{
Rotation = transformB.Rotation, Position = transformB.Position + output.Lambda * input.TranslationB
};
var distanceInput = new DistanceInput
{
TransformA = transformA,
TransformB = transformB2,
UseRadii = false
};
distanceInput.ProxyA.Set(_vAs, _countA, _radiusA);
distanceInput.ProxyB.Set(_vBs, _countB, _radiusB);
var simplexCache = new SimplexCache();
DistanceAlgorithm.Distance(out var distanceOutput, ref simplexCache, distanceInput);
DrawString(
$"hit = {hit}, iters = {output.Iterations}, lambda = {output.Lambda}, distance = {distanceOutput.Distance}");
var vertices = new Vector2[Settings.MaxPolygonVertices];
for (var i = 0; i < _countA; ++i)
{
vertices[i] = MathUtils.Mul(transformA, _vAs[i]);
}
//g_debugDraw.DrawCircle(vertices[0], _radiusA, b2Color(0.9f, 0.9f, 0.9f));
Drawer.DrawPolygon(vertices, _countA, Color.FromArgb(230, 230, 230));
for (var i = 0; i < _countB; ++i)
{
vertices[i] = MathUtils.Mul(transformB, _vBs[i]);
}
//g_debugDraw.DrawCircle(vertices[0], _radiusB, b2Color(0.5f, 0.9f, 0.5f));
Drawer.DrawPolygon(vertices, _countB, Color.FromArgb(127, 230, 127));
for (var i = 0; i < _countB; ++i)
{
vertices[i] = MathUtils.Mul(transformB2, _vBs[i]);
}
//g_debugDraw.DrawCircle(vertices[0], _radiusB, b2Color(0.5f, 0.7f, 0.9f));
Drawer.DrawPolygon(vertices, _countB, Color.FromArgb(127, 129, 230));
if (hit)
{
var p1 = output.Point;
Drawer.DrawPoint(p1, 10.0f, Color.FromArgb(230, 77, 77));
var p2 = p1 + output.Normal;
Drawer.DrawSegment(p1, p2, Color.FromArgb(230, 77, 77));
}
}
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;
}
}
/// <inheritdoc />
protected override void PostStep()
{
if (Input.GetKeyDown(KeyCode.A))
{
_positionB.X -= 0.1f;
}
if (Input.GetKeyDown(KeyCode.D))
{
_positionB.X += 0.1f;
}
if (Input.GetKeyDown(KeyCode.S))
{
_positionB.Y -= 0.1f;
}
if (Input.GetKeyDown(KeyCode.W))
{
_positionB.Y += 0.1f;
}
if (Input.GetKeyDown(KeyCode.Q))
{
_angleB += 0.1f * Settings.Pi;
}
if (Input.GetKeyDown(KeyCode.E))
{
_angleB -= 0.1f * Settings.Pi;
}
_transformB.Set(_positionB, _angleB);
var input = new DistanceInput();
input.ProxyA.Set(_polygonA, 0);
input.ProxyB.Set(_polygonB, 0);
input.TransformA = _transformA;
input.TransformB = _transformB;
input.UseRadii = true;
var cache = SimplexCache.Create();
cache.Count = 0;
DistanceAlgorithm.Distance(out var output, ref cache, input);
DrawString($"distance = {output.Distance}");
DrawString($"iterations = {output.Iterations}");
{
var color = Color.FromArgb(230, 230, 230);
var v = new Vector2[Settings.MaxPolygonVertices];
for (var i = 0; i < _polygonA.Count; ++i)
{
v[i] = MathUtils.Mul(_transformA, _polygonA.Vertices[i]);
}
Drawer.DrawPolygon(v, _polygonA.Count, color);
for (var i = 0; i < _polygonB.Count; ++i)
{
v[i] = MathUtils.Mul(_transformB, _polygonB.Vertices[i]);
}
Drawer.DrawPolygon(v, _polygonB.Count, color);
}
var x1 = output.PointA;
var x2 = output.PointB;
var c1 = Color.FromArgb(255, 0, 0);
Drawer.DrawPoint(x1, 4.0f, c1);
var c2 = Color.FromArgb(255, 255, 0);
Drawer.DrawPoint(x2, 4.0f, c2);
}
public void Set(SimplexCache sc)
{
Array.Copy(sc.IndexA, 0, IndexA, 0, IndexA.Length);
Array.Copy(sc.IndexB, 0, IndexB, 0, IndexB.Length);
Metric = sc.Metric;
Count = sc.Count;
}
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>
/// 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 static void ComputeDistance(ref DistanceInput input, out DistanceOutput output, out SimplexCache cache)
{
cache = new SimplexCache();
if (Settings.EnableDiagnostics) //Velcro: We only gather diagnostics when enabled
{
++GJKCalls;
}
// Initialize the simplex.
Simplex simplex = new Simplex();
simplex.ReadCache(ref cache, ref input.ProxyA, ref input.TransformA, ref input.ProxyB, ref input.TransformB);
// These store the vertices of the last simplex so that we
// can check for duplicates and prevent cycling.
FixedArray3 <int> saveA = new FixedArray3 <int>();
FixedArray3 <int> saveB = new FixedArray3 <int>();
// Main iteration loop.
int iter = 0;
while (iter < Settings.MaxGJKIterations)
{
// Copy simplex so we can identify duplicates.
int saveCount = simplex.Count;
for (int i = 0; i < saveCount; ++i)
{
saveA[i] = simplex.V[i].IndexA;
saveB[i] = simplex.V[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;
}
// Get search direction.
Vector2 d = simplex.GetSearchDirection();
// Ensure the search direction is numerically fit.
if (d.LengthSquared() < MathConstants.Epsilon * MathConstants.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 = simplex.V[simplex.Count];
vertex.IndexA = input.ProxyA.GetSupport(MathUtils.MulT(input.TransformA.q, -d));
vertex.WA = MathUtils.Mul(ref input.TransformA, input.ProxyA.Vertices[vertex.IndexA]);
vertex.IndexB = input.ProxyB.GetSupport(MathUtils.MulT(input.TransformB.q, d));
vertex.WB = MathUtils.Mul(ref input.TransformB, input.ProxyB.Vertices[vertex.IndexB]);
vertex.W = vertex.WB - vertex.WA;
simplex.V[simplex.Count] = vertex;
// Iteration count is equated to the number of support point calls.
++iter;
if (Settings.EnableDiagnostics) //Velcro: We only gather diagnostics when enabled
{
++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;
}
if (Settings.EnableDiagnostics) //Velcro: We only gather diagnostics when enabled
{
GJKMaxIters = Math.Max(GJKMaxIters, iter);
}
// Prepare output.
simplex.GetWitnessPoints(out output.PointA, out output.PointB);
output.Distance = (output.PointA - output.PointB).Length();
output.Iterations = iter;
// Cache the simplex.
simplex.WriteCache(ref cache);
// Apply radii if requested.
if (input.UseRadii)
{
float rA = input.ProxyA.Radius;
float rB = input.ProxyB.Radius;
if (output.Distance > rA + rB && output.Distance > MathConstants.Epsilon)
{
// Shapes are still no overlapped.
// Move the witness points to the outer surface.
output.Distance -= rA + rB;
Vector2 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.
Vector2 p = 0.5f * (output.PointA + output.PointB);
output.PointA = p;
output.PointB = p;
output.Distance = 0.0f;
}
}
}
/// <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 distance(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)
int saveCount = 0;
simplex.getClosestPoint(closestPoint);
float distanceSqr1 = closestPoint.lengthSquared();
float distanceSqr2 = distanceSqr1;
// Main iteration loop
int iter = 0;
while (iter < GJK_MAX_ITERS)
{
// Copy simplex so we can identify duplicates.
saveCount = simplex.m_count;
for (int i = 0; i < saveCount; i++)
{
saveA[i] = vertices[i].indexA;
saveB[i] = vertices[i].indexB;
}
switch (simplex.m_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.m_count == 3)
{
break;
}
// Compute closest point.
simplex.getClosestPoint(closestPoint);
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.m_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_Renamed(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.m_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.m_radius;
float rB = proxyB.m_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_Renamed(output.pointB).subLocal(output.pointA);
normal.normalize();
temp.set_Renamed(normal).mulLocal(rA);
output.pointA.addLocal(temp);
temp.set_Renamed(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_Renamed(output.pointA);
output.distance = 0.0f;
}
}
}
public virtual void writeCache(SimplexCache cache)
{
cache.metric = Metric;
cache.count = m_count;
for (int i = 0; i < m_count; ++i)
{
cache.indexA[i] = (vertices[i].indexA);
cache.indexB[i] = (vertices[i].indexB);
}
}
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 void WriteCache(SimplexCache cache)
{
cache.Metric = Metric;
cache.Count = Count;
for (int i = 0; i < Count; ++i)
{
cache.IndexA[i] = (Vertices[i].IndexA);
cache.IndexB[i] = (Vertices[i].IndexB);
}
}
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.
}
public virtual void set_Renamed(SimplexCache sc)
{
Array.Copy(sc.indexA, 0, indexA, 0, indexA.Length);
Array.Copy(sc.indexB, 0, indexB, 0, indexB.Length);
metric = sc.metric;
count = sc.count;
}
