|
private ReadCache ( |
||
| cache | ||
| shapeA | ||
| TransformA | Box2DX.Common.Transform | |
| shapeB | ||
| TransformB | Box2DX.Common.Transform | |
| return | void | |
static void Distance(out DistanceOutput output, ref SimplexCache cache, ref DistanceInput input, Shape shapeA, Shape shapeB)
{
output = new DistanceOutput();
Transform transformA = input.TransformA;
Transform transformB = input.TransformB;
// Initialize the simplex.
Simplex simplex = new Simplex();
#if ALLOWUNSAFE
fixed (SimplexCache* sPtr = &cache)
{
simplex.ReadCache(sPtr, shapeA, transformA, shapeB, transformB);
}
#else
simplex.ReadCache(cache, shapeA, transformA, shapeB, transformB);
#endif
// Get simplex vertices as an array.
#if ALLOWUNSAFE
SimplexVertex* vertices = &simplex._v1;
#else
SimplexVertex[] vertices = new SimplexVertex[] { simplex._v1, simplex._v2, simplex._v3 };
#endif
// These store the vertices of the last simplex so that we
// can check for duplicates and prevent cycling.
#if ALLOWUNSAFE
int* lastA = stackalloc int[4], lastB = stackalloc int[4];
#else
int[] lastA = new int[4];
int[] lastB = new int[4];
#endif // ALLOWUNSAFE
int lastCount;
// Main iteration loop.
int iter = 0;
const int k_maxIterationCount = 20;
while (iter < k_maxIterationCount)
{
// Copy simplex so we can identify duplicates.
lastCount = simplex._count;
int i;
for (i = 0; i < lastCount; ++i)
{
lastA[i] = vertices[i].indexA;
lastB[i] = vertices[i].indexB;
}
switch (simplex._count)
{
case 1:
break;
case 2:
simplex.Solve2();
break;
case 3:
simplex.Solve3();
break;
default:
#if DEBUG
Box2DXDebug.Assert(false);
#endif
break;
}
// If we have 3 points, then the origin is in the corresponding triangle.
if (simplex._count == 3)
{
break;
}
// Compute closest point.
Vector2 p = simplex.GetClosestPoint();
float distanceSqr = p.sqrMagnitude;
// Ensure the search direction is numerically fit.
if (distanceSqr < Common.Settings.FLT_EPSILON_SQUARED)
{
// 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.
#if ALLOWUNSAFE
SimplexVertex* vertex = vertices + simplex._count;
vertex->indexA = shapeA.GetSupport(transformA.InverseTransformDirection(p));
vertex->wA = transformA.TransformPoint(shapeA.GetVertex(vertex->indexA));
//Vec2 wBLocal;
vertex->indexB = shapeB.GetSupport(transformB.InverseTransformDirection(-p));
vertex->wB = transformB.TransformPoint(shapeB.GetVertex(vertex->indexB));
vertex->w = vertex->wB - vertex->wA;
#else
SimplexVertex vertex = vertices[simplex._count - 1];
vertex.indexA = shapeA.GetSupport(transformA.InverseTransformDirection(p));
vertex.wA = transformA.TransformPoint(shapeA.GetVertex(vertex.indexA));
//Vec2 wBLocal;
vertex.indexB = shapeB.GetSupport(transformB.InverseTransformDirection(-p));
vertex.wB = transformB.TransformPoint(shapeB.GetVertex(vertex.indexB));
vertex.w = vertex.wB - vertex.wA;
#endif // ALLOWUNSAFE
// Iteration count is equated to the number of support point calls.
++iter;
// Check for convergence.
#if ALLOWUNSAFE
float lowerBound = Vector2.Dot(p, vertex->w);
#else
float lowerBound = Vector2.Dot(p, vertex.w);
#endif
float upperBound = distanceSqr;
const float k_relativeTolSqr = 0.01f * 0.01f; // 1:100
if (upperBound - lowerBound <= k_relativeTolSqr * upperBound)
{
// Converged!
break;
}
// Check for duplicate support points.
bool duplicate = false;
for (i = 0; i < lastCount; ++i)
{
#if ALLOWUNSAFE
if (vertex->indexA == lastA[i] && vertex->indexB == lastB[i])
#else
if (vertex.indexA == lastA[i] && vertex.indexB == lastB[i])
#endif
{
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 ALLOWUNSAFE
fixed (DistanceOutput* doPtr = &output)
{
// Prepare output.
simplex.GetWitnessPoints(&doPtr->PointA, &doPtr->PointB);
doPtr->Distance = Vector2.Distance(doPtr->PointA, doPtr->PointB);
doPtr->Iterations = iter;
}
fixed (SimplexCache* sPtr = &cache)
{
// Cache the simplex.
simplex.WriteCache(sPtr);
}
#else
// Prepare output.
simplex.GetWitnessPoints(out output.PointA, out output.PointB);
output.Distance = Vector2.Distance(output.PointA, output.PointB);
output.Iterations = iter;
// Cache the simplex.
simplex.WriteCache(cache);
#endif
// Apply radii if requested.
if (input.UseRadii)
{
float rA = shapeA._radius;
float rB = shapeB._radius;
if (output.Distance > rA + rB && output.Distance > Common.Settings.FLT_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:
/// 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;
}
}
}
static void Distance(out DistanceOutput output, ref SimplexCache cache, ref DistanceInput input, Shape shapeA, Shape shapeB)
{
output = new DistanceOutput();
Transform transformA = input.TransformA;
Transform transformB = input.TransformB;
// Initialize the simplex.
Simplex simplex = new Simplex();
#if ALLOWUNSAFE
fixed(SimplexCache *sPtr = &cache)
{
simplex.ReadCache(sPtr, shapeA, transformA, shapeB, transformB);
}
#else
simplex.ReadCache(cache, shapeA, transformA, shapeB, transformB);
#endif
// Get simplex vertices as an array.
#if ALLOWUNSAFE
SimplexVertex *vertices = &simplex._v1;
#else
SimplexVertex[] vertices = new SimplexVertex[] { simplex._v1, simplex._v2, simplex._v3 };
#endif
// These store the vertices of the last simplex so that we
// can check for duplicates and prevent cycling.
#if ALLOWUNSAFE
int *lastA = stackalloc int[4], lastB = stackalloc int[4];
#else
int[] lastA = new int[4];
int[] lastB = new int[4];
#endif // ALLOWUNSAFE
int lastCount;
// Main iteration loop.
int iter = 0;
const int k_maxIterationCount = 20;
while (iter < k_maxIterationCount)
{
// Copy simplex so we can identify duplicates.
lastCount = simplex._count;
int i;
for (i = 0; i < lastCount; ++i)
{
lastA[i] = vertices[i].indexA;
lastB[i] = vertices[i].indexB;
}
switch (simplex._count)
{
case 1:
break;
case 2:
simplex.Solve2();
break;
case 3:
simplex.Solve3();
break;
default:
#if DEBUG
Box2DXDebug.Assert(false);
#endif
break;
}
// If we have 3 points, then the origin is in the corresponding triangle.
if (simplex._count == 3)
{
break;
}
// Compute closest point.
Vector2 p = simplex.GetClosestPoint();
float distanceSqr = p.sqrMagnitude;
// Ensure the search direction is numerically fit.
if (distanceSqr < Common.Settings.FLT_EPSILON_SQUARED)
{
// 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.
#if ALLOWUNSAFE
SimplexVertex *vertex = vertices + simplex._count;
vertex->indexA = shapeA.GetSupport(transformA.InverseTransformDirection(p));
vertex->wA = transformA.TransformPoint(shapeA.GetVertex(vertex->indexA));
//Vec2 wBLocal;
vertex->indexB = shapeB.GetSupport(transformB.InverseTransformDirection(-p));
vertex->wB = transformB.TransformPoint(shapeB.GetVertex(vertex->indexB));
vertex->w = vertex->wB - vertex->wA;
#else
SimplexVertex vertex = vertices[simplex._count - 1];
vertex.indexA = shapeA.GetSupport(transformA.InverseTransformDirection(p));
vertex.wA = transformA.TransformPoint(shapeA.GetVertex(vertex.indexA));
//Vec2 wBLocal;
vertex.indexB = shapeB.GetSupport(transformB.InverseTransformDirection(-p));
vertex.wB = transformB.TransformPoint(shapeB.GetVertex(vertex.indexB));
vertex.w = vertex.wB - vertex.wA;
#endif // ALLOWUNSAFE
// Iteration count is equated to the number of support point calls.
++iter;
// Check for convergence.
#if ALLOWUNSAFE
float lowerBound = Vector2.Dot(p, vertex->w);
#else
float lowerBound = Vector2.Dot(p, vertex.w);
#endif
float upperBound = distanceSqr;
const float k_relativeTolSqr = 0.01f * 0.01f; // 1:100
if (upperBound - lowerBound <= k_relativeTolSqr * upperBound)
{
// Converged!
break;
}
// Check for duplicate support points.
bool duplicate = false;
for (i = 0; i < lastCount; ++i)
{
#if ALLOWUNSAFE
if (vertex->indexA == lastA[i] && vertex->indexB == lastB[i])
#else
if (vertex.indexA == lastA[i] && vertex.indexB == lastB[i])
#endif
{
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 ALLOWUNSAFE
fixed(DistanceOutput *doPtr = &output)
{
// Prepare output.
simplex.GetWitnessPoints(&doPtr->PointA, &doPtr->PointB);
doPtr->Distance = Vector2.Distance(doPtr->PointA, doPtr->PointB);
doPtr->Iterations = iter;
}
fixed(SimplexCache *sPtr = &cache)
{
// Cache the simplex.
simplex.WriteCache(sPtr);
}
#else
// Prepare output.
simplex.GetWitnessPoints(out output.PointA, out output.PointB);
output.Distance = Vector2.Distance(output.PointA, output.PointB);
output.Iterations = iter;
// Cache the simplex.
simplex.WriteCache(cache);
#endif
// Apply radii if requested.
if (input.UseRadii)
{
float rA = shapeA._radius;
float rB = shapeB._radius;
if (output.Distance > rA + rB && output.Distance > Common.Settings.FLT_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:
/// 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;
}
}
}