/// <summary>
/// cast a convex against another convex object
/// </summary>
/// <param name="fromA"></param>
/// <param name="toA"></param>
/// <param name="fromB"></param>
/// <param name="toB"></param>
/// <param name="result"></param>
/// <returns></returns>
public bool CalcTimeOfImpact(Matrix fromA, Matrix toA, Matrix fromB, Matrix toB, CastResult result)
{
MinkowskiSumShape combined = new MinkowskiSumShape(_convexA, _convexB);
Matrix rayFromLocalA = MathHelper.InvertMatrix(fromA) * fromB;
Matrix rayToLocalA = MathHelper.InvertMatrix(toA) * toB;
Matrix transformA = fromA;
Matrix transformB = fromB;
transformA.Translation = new Vector3(0, 0, 0);
transformB.Translation = new Vector3(0, 0, 0);
combined.TransformA = transformA;
combined.TransformB = transformB;
float radius = 0.01f;
float lambda = 0;
Vector3 s = rayFromLocalA.Translation;
Vector3 r = rayToLocalA.Translation - rayFromLocalA.Translation;
Vector3 x = s;
Vector3 n = new Vector3();
Vector3 c = new Vector3();
bool hasResult = false;
float lastLambda = lambda;
IConvexPenetrationDepthSolver penSolver = null;
Matrix identityTransform = Matrix.Identity;
SphereShape raySphere = new SphereShape(0.0f);
raySphere.Margin = 0.0f;
Matrix sphereTransform = Matrix.Identity;
sphereTransform.Translation = rayFromLocalA.Translation;
result.DrawCoordSystem(sphereTransform);
{
PointCollector pointCollector = new PointCollector();
GjkPairDetector gjk = new GjkPairDetector(raySphere, combined, _simplexSolver, penSolver);
GjkPairDetector.ClosestPointInput input = new DiscreteCollisionDetectorInterface.ClosestPointInput();
input.TransformA = sphereTransform;
input.TransformB = identityTransform;
gjk.GetClosestPoints(input, pointCollector, null);
hasResult = pointCollector.HasResult;
c = pointCollector.PointInWorld;
n = pointCollector.NormalOnBInWorld;
}
if (hasResult)
{
float dist = (c - x).Length();
if (dist < radius)
{
lastLambda = 1.0f;
}
while (dist > radius)
{
n = x - c;
float dot = Vector3.Dot(n, r);
if (dot >= -(MathHelper.Epsilon * MathHelper.Epsilon))
{
return(false);
}
lambda = lambda - Vector3.Distance(n, n) / dot;
if (lambda <= lastLambda)
{
break;
}
lastLambda = lambda;
x = s + lambda * r;
sphereTransform.Translation = x;
result.DrawCoordSystem(sphereTransform);
PointCollector pointCollector = new PointCollector();
GjkPairDetector gjk = new GjkPairDetector(raySphere, combined, _simplexSolver, penSolver);
GjkPairDetector.ClosestPointInput input = new DiscreteCollisionDetectorInterface.ClosestPointInput();
input.TransformA = sphereTransform;
input.TransformB = identityTransform;
gjk.GetClosestPoints(input, pointCollector, null);
if (pointCollector.HasResult)
{
if (pointCollector.Distance < 0.0f)
{
result.Fraction = lastLambda;
result.Normal = n;
return(true);
}
c = pointCollector.PointInWorld;
dist = (c - x).Length();
}
else
{
return(false);
}
}
if (lastLambda < 1.0f)
{
result.Fraction = lastLambda;
result.Normal = n;
return(true);
}
}
return(false);
}
/// <summary>
/// SimsimplexConvexCast calculateTimeOfImpact calculates the time of impact+normal for the linear cast (sweep) between two moving objects.
/// Precondition is that objects should not penetration/overlap at the start from the interval. Overlap can be tested using GjkPairDetector.
/// </summary>
/// <param name="fromA"></param>
/// <param name="toA"></param>
/// <param name="fromB"></param>
/// <param name="toB"></param>
/// <param name="result"></param>
/// <returns></returns>
public bool CalcTimeOfImpact(Matrix fromA, Matrix toA, Matrix fromB, Matrix toB, CastResult result)
{
MinkowskiSumShape convex = new MinkowskiSumShape(_convexA, _convexB);
Matrix rayFromLocalA;
Matrix rayToLocalA;
rayFromLocalA = MathHelper.InvertMatrix(fromA) * fromB;
rayToLocalA = MathHelper.InvertMatrix(toA) * toB;
_simplexSolver.Reset();
convex.TransformB = rayFromLocalA;
float lambda = 0;
//todo: need to verify this:
//because of minkowski difference, we need the inverse direction
Vector3 s = -rayFromLocalA.Translation;
Vector3 r = -(rayToLocalA.Translation - rayFromLocalA.Translation);
Vector3 x = s;
Vector3 v;
Vector3 arbitraryPoint = convex.LocalGetSupportingVertex(r);
v = x - arbitraryPoint;
int maxIter = MaxIterations;
Vector3 n = new Vector3();
float lastLambda = lambda;
float dist2 = v.LengthSquared();
float epsilon = 0.0001f;
Vector3 w, p;
float VdotR;
while ((dist2 > epsilon) && (maxIter-- != 0))
{
p = convex.LocalGetSupportingVertex(v);
w = x - p;
float VdotW = Vector3.Dot(v, w);
if (VdotW > 0)
{
VdotR = Vector3.Dot(v, r);
if (VdotR >= -(MathHelper.Epsilon * MathHelper.Epsilon))
return false;
else
{
lambda = lambda - VdotW / VdotR;
x = s + lambda * r;
_simplexSolver.Reset();
//check next line
w = x - p;
lastLambda = lambda;
n = v;
}
}
_simplexSolver.AddVertex(w, x, p);
if (_simplexSolver.Closest(out v))
{
dist2 = v.LengthSquared();
}
else
{
dist2 = 0f;
}
}
result.Fraction = lambda;
result.Normal = n;
return true;
}
/// <summary>
/// cast a convex against another convex object
/// </summary>
/// <param name="fromA"></param>
/// <param name="toA"></param>
/// <param name="fromB"></param>
/// <param name="toB"></param>
/// <param name="result"></param>
/// <returns></returns>
public bool CalcTimeOfImpact(Matrix fromA, Matrix toA, Matrix fromB, Matrix toB, CastResult result)
{
MinkowskiSumShape combined = new MinkowskiSumShape(_convexA, _convexB);
Matrix rayFromLocalA = MathHelper.InvertMatrix(fromA) * fromB;
Matrix rayToLocalA = MathHelper.InvertMatrix(toA) * toB;
Matrix transformA = fromA;
Matrix transformB = fromB;
transformA.Translation = new Vector3(0, 0, 0);
transformB.Translation = new Vector3(0, 0, 0);
combined.TransformA = transformA;
combined.TransformB = transformB;
float radius = 0.01f;
float lambda = 0;
Vector3 s = rayFromLocalA.Translation;
Vector3 r = rayToLocalA.Translation - rayFromLocalA.Translation;
Vector3 x = s;
Vector3 n = new Vector3();
Vector3 c = new Vector3();
bool hasResult = false;
float lastLambda = lambda;
IConvexPenetrationDepthSolver penSolver = null;
Matrix identityTransform = Matrix.Identity;
SphereShape raySphere = new SphereShape(0.0f);
raySphere.Margin=0.0f;
Matrix sphereTransform = Matrix.Identity;
sphereTransform.Translation = rayFromLocalA.Translation;
result.DrawCoordSystem(sphereTransform);
{
PointCollector pointCollector = new PointCollector();
GjkPairDetector gjk = new GjkPairDetector(raySphere, combined, _simplexSolver, penSolver);
GjkPairDetector.ClosestPointInput input = new DiscreteCollisionDetectorInterface.ClosestPointInput();
input.TransformA = sphereTransform;
input.TransformB = identityTransform;
gjk.GetClosestPoints(input, pointCollector, null);
hasResult = pointCollector.HasResult;
c = pointCollector.PointInWorld;
n = pointCollector.NormalOnBInWorld;
}
if (hasResult)
{
float dist = (c - x).Length();
if (dist < radius)
{
lastLambda = 1.0f;
}
while (dist > radius)
{
n = x - c;
float dot = Vector3.Dot(n, r);
if (dot >= -(MathHelper.Epsilon * MathHelper.Epsilon)) return false;
lambda = lambda - Vector3.Distance(n, n) / dot;
if (lambda <= lastLambda) break;
lastLambda = lambda;
x = s + lambda * r;
sphereTransform.Translation = x;
result.DrawCoordSystem(sphereTransform);
PointCollector pointCollector = new PointCollector();
GjkPairDetector gjk = new GjkPairDetector(raySphere, combined, _simplexSolver, penSolver);
GjkPairDetector.ClosestPointInput input = new DiscreteCollisionDetectorInterface.ClosestPointInput();
input.TransformA = sphereTransform;
input.TransformB = identityTransform;
gjk.GetClosestPoints(input, pointCollector, null);
if (pointCollector.HasResult)
{
if (pointCollector.Distance < 0.0f)
{
result.Fraction = lastLambda;
result.Normal = n;
return true;
}
c = pointCollector.PointInWorld;
dist = (c - x).Length();
}
else
{
return false;
}
}
if (lastLambda < 1.0f)
{
result.Fraction = lastLambda;
result.Normal = n;
return true;
}
}
return false;
}
/// <summary>
/// SimsimplexConvexCast calculateTimeOfImpact calculates the time of impact+normal for the linear cast (sweep) between two moving objects.
/// Precondition is that objects should not penetration/overlap at the start from the interval. Overlap can be tested using GjkPairDetector.
/// </summary>
/// <param name="fromA"></param>
/// <param name="toA"></param>
/// <param name="fromB"></param>
/// <param name="toB"></param>
/// <param name="result"></param>
/// <returns></returns>
public bool CalcTimeOfImpact(Matrix fromA, Matrix toA, Matrix fromB, Matrix toB, CastResult result)
{
MinkowskiSumShape convex = new MinkowskiSumShape(_convexA, _convexB);
Matrix rayFromLocalA;
Matrix rayToLocalA;
rayFromLocalA = MathHelper.InvertMatrix(fromA) * fromB;
rayToLocalA = MathHelper.InvertMatrix(toA) * toB;
_simplexSolver.Reset();
convex.TransformB = rayFromLocalA;
float lambda = 0;
//todo: need to verify this:
//because of minkowski difference, we need the inverse direction
Vector3 s = -rayFromLocalA.Translation;
Vector3 r = -(rayToLocalA.Translation - rayFromLocalA.Translation);
Vector3 x = s;
Vector3 v;
Vector3 arbitraryPoint = convex.LocalGetSupportingVertex(r);
v = x - arbitraryPoint;
int maxIter = MaxIterations;
Vector3 n = new Vector3();
float lastLambda = lambda;
float dist2 = v.LengthSquared();
float epsilon = 0.0001f;
Vector3 w, p;
float VdotR;
while ((dist2 > epsilon) && (maxIter-- != 0))
{
p = convex.LocalGetSupportingVertex(v);
w = x - p;
float VdotW = Vector3.Dot(v, w);
if (VdotW > 0)
{
VdotR = Vector3.Dot(v, r);
if (VdotR >= -(MathHelper.Epsilon * MathHelper.Epsilon))
{
return(false);
}
else
{
lambda = lambda - VdotW / VdotR;
x = s + lambda * r;
_simplexSolver.Reset();
//check next line
w = x - p;
lastLambda = lambda;
n = v;
}
}
_simplexSolver.AddVertex(w, x, p);
if (_simplexSolver.Closest(out v))
{
dist2 = v.LengthSquared();
}
else
{
dist2 = 0f;
}
}
result.Fraction = lambda;
result.Normal = n;
return(true);
}
