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));
}
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 ComputeDistance(out DistanceOutput output, out SimplexCache cache, DistanceInput input)
{
cache = new SimplexCache();
/*
* if (Settings.EnableDiagnostics) //FPE: We only gather diagnostics when enabled
++GJKCalls;
*/
// Initialize the simplex.
Simplex simplex = new Simplex();
simplex.ReadCache(ref cache, input.ProxyA, ref input.TransformA, input.ProxyB, ref input.TransformB);
// These store the vertices of the last simplex so that we
// can check for duplicates and prevent cycling.
var saveA = new int[3];
var saveB = new int[3];
//float distanceSqr1 = Settings.MaxFloat;
// Main iteration loop.
int iter = 0;
while (iter < MaxGJKIterations)
{
// Copy simplex so we can identify duplicates.
int saveCount = simplex.Count;
for (var 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:
throw new ArgumentOutOfRangeException();
}
// If we have 3 points, then the origin is in the corresponding triangle.
if (simplex.Count == 3)
{
break;
}
//FPE: This code was not used anyway.
// Compute closest point.
//Vector2 p = simplex.GetClosestPoint();
//float distanceSqr2 = p.LengthSquared();
// Ensure progress
//if (distanceSqr2 >= distanceSqr1)
//{
//break;
//}
//distanceSqr1 = distanceSqr2;
// Get search direction.
Vector2 d = simplex.GetSearchDirection();
// Ensure the search direction is numerically fit.
if (d.LengthSquared < 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.
SimplexVertex vertex = simplex.V[simplex.Count];
vertex.IndexA = input.ProxyA.GetSupport(Transform.MulT(input.TransformA.Quaternion2D, -d));
vertex.WA = Transform.Mul(input.TransformA, input.ProxyA.Vertices[vertex.IndexA]);
vertex.IndexB = input.ProxyB.GetSupport(Transform.MulT(input.TransformB.Quaternion2D, d));
vertex.WB = Transform.Mul(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) //FPE: 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;
}
// 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 > float.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 = normal.Normalized;
output.PointA += normal * rA;
output.PointB -= normal * rB;
}
else
{
// Shapes are overlapped when radii are considered.
// Move the witness points to the middle.
Vector2 p = (output.PointA + output.PointB) * 0.5f;
output.PointA = p;
output.PointB = p;
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));
}
