// see: btSubSimplexConvexCast.cpp
/// <summary>
/// Checks if a ray definied through it's origin and direction collides
/// with a shape.
/// </summary>
/// <param name="support">The supportmap implementation representing the shape.</param>
/// <param name="orientation">The orientation of the shape.</param>
/// <param name="invOrientation">The inverse orientation of the shape.</param>
/// <param name="position">The position of the shape.</param>
/// <param name="origin">The origin of the ray.</param>
/// <param name="direction">The direction of the ray.</param>
/// <param name="fraction">The fraction which gives information where at the
/// ray the collision occured. The hitPoint is calculated by: origin+friction*direction.</param>
/// <param name="normal">The normal from the ray collision.</param>
/// <returns>Returns true if the ray hit the shape, false otherwise.</returns>
public static bool Raycast(ISupportMappable support, ref JMatrix orientation, ref JMatrix invOrientation,
ref JVector position, ref JVector origin, ref JVector direction, out float fraction, out JVector normal)
{
VoronoiSimplexSolver simplexSolver = simplexSolverPool.GetNew();
simplexSolver.Reset();
normal = JVector.Zero;
fraction = float.MaxValue;
float lambda = 0.0f;
JVector r = direction;
JVector x = origin;
JVector w, p, v;
JVector arbitraryPoint;
SupportMapTransformed(support, ref orientation, ref position, ref r, out arbitraryPoint);
JVector.Subtract(ref x, ref arbitraryPoint, out v);
int maxIter = MaxIterations;
float distSq = v.LengthSquared();
float epsilon = 0.000001f;
float VdotR;
while ((distSq > epsilon) && (maxIter-- != 0))
{
SupportMapTransformed(support, ref orientation, ref position, ref v, out p);
JVector.Subtract(ref x, ref p, out w);
float VdotW = JVector.Dot(ref v, ref w);
if (VdotW > 0.0f)
{
VdotR = JVector.Dot(ref v, ref r);
if (VdotR >= -JMath.Epsilon)
{
simplexSolverPool.GiveBack(simplexSolver);
return(false);
}
else
{
lambda = lambda - VdotW / VdotR;
JVector.Multiply(ref r, lambda, out x);
JVector.Add(ref origin, ref x, out x);
JVector.Subtract(ref x, ref p, out w);
normal = v;
}
}
if (!simplexSolver.InSimplex(w))
{
simplexSolver.AddVertex(w, x, p);
}
if (simplexSolver.Closest(out v))
{
distSq = v.LengthSquared();
}
else
{
distSq = 0.0f;
}
}
#region Retrieving hitPoint
// Giving back the fraction like this *should* work
// but is inaccurate against large objects:
// fraction = lambda;
JVector p1, p2;
simplexSolver.ComputePoints(out p1, out p2);
p2 = p2 - origin;
fraction = p2.Length() / direction.Length();
#endregion
if (normal.LengthSquared() > JMath.Epsilon * JMath.Epsilon)
{
normal.Normalize();
}
simplexSolverPool.GiveBack(simplexSolver);
return(true);
}
// see: btSubSimplexConvexCast.cpp
/// <summary>
/// Checks if a ray definied through it's origin and direction collides
/// with a shape.
/// </summary>
/// <param name="support">The supportmap implementation representing the shape.</param>
/// <param name="orientation">The orientation of the shape.</param>
/// <param name="invOrientation">The inverse orientation of the shape.</param>
/// <param name="position">The position of the shape.</param>
/// <param name="origin">The origin of the ray.</param>
/// <param name="direction">The direction of the ray.</param>
/// <param name="fraction">The fraction which gives information where at the
/// ray the collision occured. The hitPoint is calculated by: origin+friction*direction.</param>
/// <param name="normal">The normal from the ray collision.</param>
/// <returns>Returns true if the ray hit the shape, false otherwise.</returns>
public static bool Raycast(ISupportMappable support, ref MatrixD orientation, ref MatrixD invOrientation,
ref Vector3D position, ref Vector3D origin, ref Vector3D direction, out double fraction, out Vector3D normal)
{
VoronoiSimplexSolver simplexSolver = simplexSolverPool.GetNew();
simplexSolver.Reset();
normal = Vector3D.Zero;
fraction = double.MaxValue;
double lambda = 0.0f;
Vector3D r = direction;
Vector3D x = origin;
Vector3D w, p, v;
Vector3D arbitraryPoint;
SupportMapTransformed(support, ref orientation, ref position, ref r, out arbitraryPoint);
Vector3D.Subtract(ref x, ref arbitraryPoint, out v);
int maxIter = MaxIterations;
double distSq = v.LengthSquared();
double epsilon = 0.000001f;
double VdotR;
while ((distSq > epsilon) && (maxIter-- != 0))
{
SupportMapTransformed(support, ref orientation, ref position, ref v, out p);
Vector3D.Subtract(ref x, ref p, out w);
double VdotW = Vector3D.Dot(v, w);
if (VdotW > 0.0f)
{
VdotR = Vector3D.Dot(v, r);
if (VdotR >= -MathHelper.EPSILON)
{
simplexSolverPool.GiveBack(simplexSolver);
return(false);
}
else
{
lambda = lambda - VdotW / VdotR;
Vector3D.Multiply(ref r, lambda, out x);
Vector3D.Add(ref origin, ref x, out x);
Vector3D.Subtract(ref x, ref p, out w);
normal = v;
}
}
if (!simplexSolver.InSimplex(w))
{
simplexSolver.AddVertex(w, x, p);
}
if (simplexSolver.Closest(out v))
{
distSq = v.LengthSquared();
}
else
{
distSq = 0.0f;
}
}
#region Retrieving hitPoint
// Giving back the fraction like this *should* work
// but is inaccurate against large objects:
// fraction = lambda;
Vector3D p1, p2;
simplexSolver.ComputePoints(out p1, out p2);
p2 = p2 - origin;
fraction = p2.Length() / direction.Length();
#endregion
if (normal.LengthSquared() > MathHelper.EPSILON * MathHelper.EPSILON)
{
normal.Normalize();
}
simplexSolverPool.GiveBack(simplexSolver);
return(true);
}
public static bool ClosestPoints(ISupportMappable support1, ISupportMappable support2, ref Matrix3 orientation1,
ref Matrix3 orientation2, ref Vector3 position1, ref Vector3 position2,
out Vector3 p1, out Vector3 p2, out Vector3 normal)
{
VoronoiSimplexSolver simplexSolver = simplexSolverPool.GetNew();
simplexSolver.Reset();
p1 = p2 = Vector3.Zero;
Vector3 r = position1 - position2;
Vector3 w, v;
Vector3 supVertexA;
Vector3 rn, vn;
rn = r * -1;
SupportMapTransformed(support1, ref orientation1, ref position1, ref rn, out supVertexA);
Vector3 supVertexB;
SupportMapTransformed(support2, ref orientation2, ref position2, ref r, out supVertexB);
v = supVertexA - supVertexB;
normal = Vector3.Zero;
int maxIter = 15;
float distSq = v.LengthSquared;
float epsilon = 0.00001f;
while ((distSq > epsilon) && (maxIter-- != 0))
{
vn = v * -1;
SupportMapTransformed(support1, ref orientation1, ref position1, ref vn, out supVertexA);
SupportMapTransformed(support2, ref orientation2, ref position2, ref v, out supVertexB);
w = supVertexA - supVertexB;
if (!simplexSolver.InSimplex(w))
{
simplexSolver.AddVertex(w, supVertexA, supVertexB);
}
if (simplexSolver.Closest(out v))
{
distSq = v.LengthSquared;
normal = v;
}
else
{
distSq = 0.0f;
}
}
simplexSolver.ComputePoints(out p1, out p2);
if (normal.LengthSquared > JMath.Epsilon * JMath.Epsilon)
{
normal.Normalize();
}
simplexSolverPool.GiveBack(simplexSolver);
return(true);
}
public static bool ClosestPoints(ISupportMappable support1, ISupportMappable support2, ref MatrixD orientation1,
ref MatrixD orientation2, ref Vector3D position1, ref Vector3D position2,
out Vector3D p1, out Vector3D p2, out Vector3D normal)
{
VoronoiSimplexSolver simplexSolver = simplexSolverPool.GetNew();
simplexSolver.Reset();
p1 = p2 = Vector3D.Zero;
Vector3D r = position1 - position2;
Vector3D w, v;
Vector3D supVertexA;
Vector3D rn, vn;
rn = Vector3D.Negate(r);
SupportMapTransformed(support1, ref orientation1, ref position1, ref rn, out supVertexA);
Vector3D supVertexB;
SupportMapTransformed(support2, ref orientation2, ref position2, ref r, out supVertexB);
v = supVertexA - supVertexB;
normal = Vector3D.Zero;
int maxIter = 15;
double distSq = v.LengthSquared();
double epsilon = 0.00001f;
while ((distSq > epsilon) && (maxIter-- != 0))
{
vn = Vector3D.Negate(v);
SupportMapTransformed(support1, ref orientation1, ref position1, ref vn, out supVertexA);
SupportMapTransformed(support2, ref orientation2, ref position2, ref v, out supVertexB);
w = supVertexA - supVertexB;
if (!simplexSolver.InSimplex(w))
{
simplexSolver.AddVertex(w, supVertexA, supVertexB);
}
if (simplexSolver.Closest(out v))
{
distSq = v.LengthSquared();
normal = v;
}
else
{
distSq = 0.0f;
}
}
simplexSolver.ComputePoints(out p1, out p2);
if (normal.LengthSquared() > MathHelper.EPSILON * MathHelper.EPSILON)
{
normal.Normalize();
}
simplexSolverPool.GiveBack(simplexSolver);
return(true);
}
public static bool ClosestPoints(ISupportMappable support1, ISupportMappable support2, ref JMatrix orientation1,
ref JMatrix orientation2, ref JVector position1, ref JVector position2,
out JVector p1, out JVector p2, out JVector normal)
{
VoronoiSimplexSolver simplexSolver = simplexSolverPool.GetNew();
simplexSolver.Reset();
p1 = p2 = JVector.Zero;
JVector r = position1 - position2;
JVector w, v;
JVector supVertexA;
JVector rn, vn;
rn = JVector.Negate(r);
SupportMapTransformed(support1, ref orientation1, ref position1, ref rn, out supVertexA);
JVector supVertexB;
SupportMapTransformed(support2, ref orientation2, ref position2, ref r, out supVertexB);
v = supVertexA - supVertexB;
normal = JVector.Zero;
int iter = 0;
float distSq = v.LengthSquared();
float epsilon = 0.00001f;
while ((distSq > epsilon) && (iter++ < MaxIterations))
{
IterationsTaken = iter;
vn = JVector.Negate(v);
SupportMapTransformed(support1, ref orientation1, ref position1, ref vn, out supVertexA);
SupportMapTransformed(support2, ref orientation2, ref position2, ref v, out supVertexB);
w = supVertexA - supVertexB;
if (!simplexSolver.InSimplex(w))
{
simplexSolver.AddVertex(w, supVertexA, supVertexB);
}
if (simplexSolver.Closest(out v))
{
distSq = v.LengthSquared();
normal = v;
}
else
{
distSq = 0.0f;
}
}
simplexSolver.ComputePoints(out p1, out p2);
if (normal.LengthSquared() > JMath.Epsilon * JMath.Epsilon)
{
normal.Normalize();
}
simplexSolverPool.GiveBack(simplexSolver);
return(true);
}
