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 maxIter = 15;
float distSq = v.LengthSquared();
float epsilon = 0.00001f;
while ((distSq > epsilon) && (maxIter-- != 0))
{
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;
}
public static void Absolute(ref JMatrix matrix,out JMatrix result)
{
result.M11 = Math.Abs(matrix.M11);
result.M12 = Math.Abs(matrix.M12);
result.M13 = Math.Abs(matrix.M13);
result.M21 = Math.Abs(matrix.M21);
result.M22 = Math.Abs(matrix.M22);
result.M23 = Math.Abs(matrix.M23);
result.M31 = Math.Abs(matrix.M31);
result.M32 = Math.Abs(matrix.M32);
result.M33 = Math.Abs(matrix.M33);
}
/// <summary>
/// Gets the axis aligned bounding box of the orientated shape. This includes
/// the whole shape.
/// </summary>
/// <param name="orientation">The orientation of the shape.</param>
/// <param name="box">The axis aligned bounding box of the shape.</param>
public override void GetBoundingBox(ref JMatrix orientation, out JBBox box)
{
JBBox helpBox = JBBox.LargeBox;
int length = this.Prepare(ref helpBox);
box = JBBox.SmallBox;
for (int i = 0; i < length; i++)
{
this.SetCurrentShape(i);
base.GetBoundingBox(ref orientation, out helpBox);
JBBox.CreateMerged(ref box, ref helpBox, out box);
}
}
/// <summary>
/// Creates a quaternion from a matrix.
/// </summary>
/// <param name="matrix">A matrix representing an orientation.</param>
/// <param name="result">JQuaternion representing an orientation.</param>
public static void CreateFromMatrix(ref JMatrix matrix, out JQuaternion result)
{
float num8 = (matrix.M11 + matrix.M22) + matrix.M33;
if (num8 > 0f)
{
float num = (float)Math.Sqrt((double)(num8 + 1f));
result.W = num * 0.5f;
num = 0.5f / num;
result.X = (matrix.M23 - matrix.M32) * num;
result.Y = (matrix.M31 - matrix.M13) * num;
result.Z = (matrix.M12 - matrix.M21) * num;
}
else if ((matrix.M11 >= matrix.M22) && (matrix.M11 >= matrix.M33))
{
float num7 = (float)Math.Sqrt((double)(((1f + matrix.M11) - matrix.M22) - matrix.M33));
float num4 = 0.5f / num7;
result.X = 0.5f * num7;
result.Y = (matrix.M12 + matrix.M21) * num4;
result.Z = (matrix.M13 + matrix.M31) * num4;
result.W = (matrix.M23 - matrix.M32) * num4;
}
else if (matrix.M22 > matrix.M33)
{
float num6 = (float)Math.Sqrt((double)(((1f + matrix.M22) - matrix.M11) - matrix.M33));
float num3 = 0.5f / num6;
result.X = (matrix.M21 + matrix.M12) * num3;
result.Y = 0.5f * num6;
result.Z = (matrix.M32 + matrix.M23) * num3;
result.W = (matrix.M31 - matrix.M13) * num3;
}
else
{
float num5 = (float)Math.Sqrt((double)(((1f + matrix.M33) - matrix.M11) - matrix.M22));
float num2 = 0.5f / num5;
result.X = (matrix.M31 + matrix.M13) * num2;
result.Y = (matrix.M32 + matrix.M23) * num2;
result.Z = 0.5f * num5;
result.W = (matrix.M12 - matrix.M21) * num2;
}
}
public static JQuaternion CreateFromMatrix(JMatrix matrix)
{
JQuaternion result;
JQuaternion.CreateFromMatrix(ref matrix, out result);
return result;
}
/// <summary>
/// Multiply a matrix by a scalefactor.
/// </summary>
/// <param name="matrix1">The matrix.</param>
/// <param name="scaleFactor">The scale factor.</param>
/// <param name="result">A JMatrix multiplied by the scale factor.</param>
public static void Multiply(ref JMatrix matrix1, float scaleFactor, out JMatrix result)
{
float num = scaleFactor;
result.M11 = matrix1.M11 * num;
result.M12 = matrix1.M12 * num;
result.M13 = matrix1.M13 * num;
result.M21 = matrix1.M21 * num;
result.M22 = matrix1.M22 * num;
result.M23 = matrix1.M23 * num;
result.M31 = matrix1.M31 * num;
result.M32 = matrix1.M32 * num;
result.M33 = matrix1.M33 * num;
}
/// <summary>
/// Transforms the bounding box into the space given by orientation and position.
/// </summary>
/// <param name="position"></param>
/// <param name="orientation"></param>
/// <param name="result"></param>
internal void InverseTransform(ref JVector position, ref JMatrix orientation)
{
JVector.Subtract(ref Max, ref position, out Max);
JVector.Subtract(ref Min, ref position, out Min);
JVector center;
JVector.Add(ref Max, ref Min, out center);
center.X *= 0.5f; center.Y *= 0.5f; center.Z *= 0.5f;
JVector halfExtents;
JVector.Subtract(ref Max, ref Min, out halfExtents);
halfExtents.X *= 0.5f; halfExtents.Y *= 0.5f; halfExtents.Z *= 0.5f;
JVector.TransposedTransform(ref center, ref orientation, out center);
JMatrix abs; JMath.Absolute(ref orientation, out abs);
JVector.TransposedTransform(ref halfExtents, ref abs, out halfExtents);
JVector.Add(ref center, ref halfExtents, out Max);
JVector.Subtract(ref center, ref halfExtents, out Min);
}
/// <summary>
/// Creates the transposed matrix.
/// </summary>
/// <param name="matrix">The matrix which should be transposed.</param>
/// <param name="result">The transposed JMatrix.</param>
public static void Transpose(ref JMatrix matrix, out JMatrix result)
{
result.M11 = matrix.M11;
result.M12 = matrix.M21;
result.M13 = matrix.M31;
result.M21 = matrix.M12;
result.M22 = matrix.M22;
result.M23 = matrix.M32;
result.M31 = matrix.M13;
result.M32 = matrix.M23;
result.M33 = matrix.M33;
}
private static void SupportMapTransformed(ISupportMappable support, ref JMatrix orientation, ref JVector position, ref JVector direction, out JVector result)
{
//JVector.Transform(ref direction, ref invOrientation, out result);
//support.SupportMapping(ref result, out result);
//JVector.Transform(ref result, ref orientation, out result);
//JVector.Add(ref result, ref position, out result);
result.X = ((direction.X * orientation.M11) + (direction.Y * orientation.M12)) + (direction.Z * orientation.M13);
result.Y = ((direction.X * orientation.M21) + (direction.Y * orientation.M22)) + (direction.Z * orientation.M23);
result.Z = ((direction.X * orientation.M31) + (direction.Y * orientation.M32)) + (direction.Z * orientation.M33);
support.SupportMapping(ref result, out result);
float x = ((result.X * orientation.M11) + (result.Y * orientation.M21)) + (result.Z * orientation.M31);
float y = ((result.X * orientation.M12) + (result.Y * orientation.M22)) + (result.Z * orientation.M32);
float z = ((result.X * orientation.M13) + (result.Y * orientation.M23)) + (result.Z * orientation.M33);
result.X = position.X + x;
result.Y = position.Y + y;
result.Z = position.Z + z;
}
/// <summary>
/// Checks if given point is within 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="point">The point to check.</param>
/// <returns>Returns true if the point is within the shape, otherwise false.</returns>
public static bool Pointcast(ISupportMappable support, ref JMatrix orientation,ref JVector position,ref JVector point)
{
JVector arbitraryPoint;
SupportMapTransformed(support, ref orientation, ref position, ref point, out arbitraryPoint);
JVector.Subtract(ref point, ref arbitraryPoint, out arbitraryPoint);
JVector r; support.SupportCenter(out r);
JVector.Transform(ref r, ref orientation, out r);
JVector.Add(ref position, ref r, out r);
JVector.Subtract(ref point, ref r, out r);
JVector x = point;
JVector w, p;
float VdotR;
JVector v; JVector.Subtract(ref x, ref arbitraryPoint, out v);
float dist = v.LengthSquared();
float epsilon = 0.0001f;
int maxIter = MaxIterations;
VoronoiSimplexSolver simplexSolver = simplexSolverPool.GetNew();
simplexSolver.Reset();
while ((dist > 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 * JMath.Epsilon)) { simplexSolverPool.GiveBack(simplexSolver); return false; }
else simplexSolver.Reset();
}
if (!simplexSolver.InSimplex(w)) simplexSolver.AddVertex(w, x, p);
if (simplexSolver.Closest(out v)) dist = v.LengthSquared();
else dist = 0.0f;
}
simplexSolverPool.GiveBack(simplexSolver);
return true;
}
/// <summary>
/// Uses the supportMapping to calculate the bounding box. Should be overidden
/// to make this faster.
/// </summary>
/// <param name="orientation">The orientation of the shape.</param>
/// <param name="box">The resulting axis aligned bounding box.</param>
public virtual void GetBoundingBox(ref JMatrix orientation, out JBBox box)
{
// I don't think that this can be done faster.
// 6 is the minimum number of SupportMap calls.
JVector vec = JVector.Zero;
vec.Set(orientation.M11, orientation.M21, orientation.M31);
SupportMapping(ref vec, out vec);
box.Max.X = orientation.M11 * vec.X + orientation.M21 * vec.Y + orientation.M31 * vec.Z;
vec.Set(orientation.M12, orientation.M22, orientation.M32);
SupportMapping(ref vec, out vec);
box.Max.Y = orientation.M12 * vec.X + orientation.M22 * vec.Y + orientation.M32 * vec.Z;
vec.Set(orientation.M13, orientation.M23, orientation.M33);
SupportMapping(ref vec, out vec);
box.Max.Z = orientation.M13 * vec.X + orientation.M23 * vec.Y + orientation.M33 * vec.Z;
vec.Set(-orientation.M11, -orientation.M21, -orientation.M31);
SupportMapping(ref vec, out vec);
box.Min.X = orientation.M11 * vec.X + orientation.M21 * vec.Y + orientation.M31 * vec.Z;
vec.Set(-orientation.M12, -orientation.M22, -orientation.M32);
SupportMapping(ref vec, out vec);
box.Min.Y = orientation.M12 * vec.X + orientation.M22 * vec.Y + orientation.M32 * vec.Z;
vec.Set(-orientation.M13, -orientation.M23, -orientation.M33);
SupportMapping(ref vec, out vec);
box.Min.Z = orientation.M13 * vec.X + orientation.M23 * vec.Y + orientation.M33 * vec.Z;
}
public static float CalculateMassInertia(Shape shape, out JVector centerOfMass,
out JMatrix inertia)
{
float mass = 0.0f;
centerOfMass = JVector.Zero; inertia = JMatrix.Zero;
if (shape is Multishape) throw new ArgumentException("Can't calculate inertia of multishapes.", "shape");
// build a triangle hull around the shape
List<JVector> hullTriangles = new List<JVector>();
shape.MakeHull(ref hullTriangles, 3);
// create inertia of tetrahedron with vertices at
// (0,0,0) (1,0,0) (0,1,0) (0,0,1)
float a = 1.0f / 60.0f, b = 1.0f / 120.0f;
JMatrix C = new JMatrix(a, b, b, b, a, b, b, b, a);
for (int i = 0; i < hullTriangles.Count; i += 3)
{
JVector column0 = hullTriangles[i + 0];
JVector column1 = hullTriangles[i + 1];
JVector column2 = hullTriangles[i + 2];
JMatrix A = new JMatrix(column0.X, column1.X, column2.X,
column0.Y, column1.Y, column2.Y,
column0.Z, column1.Z, column2.Z);
float detA = A.Determinant();
// now transform this canonical tetrahedron to the target tetrahedron
// inertia by a linear transformation A
JMatrix tetrahedronInertia = JMatrix.Multiply(A * C * JMatrix.Transpose(A), detA);
JVector tetrahedronCOM = (1.0f / 4.0f) * (hullTriangles[i + 0] + hullTriangles[i + 1] + hullTriangles[i + 2]);
float tetrahedronMass = (1.0f / 6.0f) * detA;
inertia += tetrahedronInertia;
centerOfMass += tetrahedronMass * tetrahedronCOM;
mass += tetrahedronMass;
}
inertia = JMatrix.Multiply(JMatrix.Identity, inertia.Trace()) - inertia;
centerOfMass = centerOfMass * (1.0f / mass);
float x = centerOfMass.X;
float y = centerOfMass.Y;
float z = centerOfMass.Z;
// now translate the inertia by the center of mass
JMatrix t = new JMatrix(
-mass * (y * y + z * z), mass * x * y, mass * x * z,
mass * y * x, -mass * (z * z + x * x), mass * y * z,
mass * z * x, mass * z * y, -mass * (x * x + y * y));
JMatrix.Add(ref inertia, ref t, out inertia);
return mass;
}
/// <summary>
/// Checks two shapes for collisions.
/// </summary>
/// <param name="support1">The SupportMappable implementation of the first shape to test.</param>
/// <param name="support2">The SupportMappable implementation of the seconds shape to test.</param>
/// <param name="orientation1">The orientation of the first shape.</param>
/// <param name="orientation2">The orientation of the second shape.</param>
/// <param name="position1">The position of the first shape.</param>
/// <param name="position2">The position of the second shape</param>
/// <param name="point">The pointin world coordinates, where collision occur.</param>
/// <param name="normal">The normal pointing from body2 to body1.</param>
/// <param name="penetration">Estimated penetration depth of the collision.</param>
/// <returns>Returns true if there is a collision, false otherwise.</returns>
public static bool Detect(ISupportMappable support1, ISupportMappable support2, ref JMatrix orientation1,
ref JMatrix orientation2, ref JVector position1, ref JVector position2,
out JVector point, out JVector normal, out float penetration)
{
// Used variables
JVector temp1, temp2;
JVector v01, v02, v0;
JVector v11, v12, v1;
JVector v21, v22, v2;
JVector v31, v32, v3;
JVector v41, v42, v4;
JVector mn;
// Initialization of the output
point = normal = JVector.Zero;
penetration = 0.0f;
//JVector right = JVector.Right;
// Get the center of shape1 in world coordinates -> v01
support1.SupportCenter(out v01);
JVector.Transform(ref v01, ref orientation1, out v01);
JVector.Add(ref position1, ref v01, out v01);
// Get the center of shape2 in world coordinates -> v02
support2.SupportCenter(out v02);
JVector.Transform(ref v02, ref orientation2, out v02);
JVector.Add(ref position2, ref v02, out v02);
// v0 is the center of the minkowski difference
JVector.Subtract(ref v02, ref v01, out v0);
// Avoid case where centers overlap -- any direction is fine in this case
if (v0.IsNearlyZero()) v0 = new JVector(0.00001f, 0, 0);
// v1 = support in direction of origin
mn = v0;
JVector.Negate(ref v0, out normal);
SupportMapTransformed(support1, ref orientation1, ref position1, ref mn, out v11);
SupportMapTransformed(support2, ref orientation2, ref position2, ref normal, out v12);
JVector.Subtract(ref v12, ref v11, out v1);
if (JVector.Dot(ref v1, ref normal) <= 0.0f) return false;
// v2 = support perpendicular to v1,v0
JVector.Cross(ref v1, ref v0, out normal);
if (normal.IsNearlyZero())
{
JVector.Subtract(ref v1, ref v0, out normal);
normal.Normalize();
point = v11;
JVector.Add(ref point, ref v12, out point);
JVector.Multiply(ref point, 0.5f, out point);
JVector.Subtract(ref v12, ref v11, out temp1);
penetration = JVector.Dot(ref temp1, ref normal);
//point = v11;
//point2 = v12;
return true;
}
JVector.Negate(ref normal, out mn);
SupportMapTransformed(support1, ref orientation1, ref position1, ref mn, out v21);
SupportMapTransformed(support2, ref orientation2, ref position2, ref normal, out v22);
JVector.Subtract(ref v22, ref v21, out v2);
if (JVector.Dot(ref v2, ref normal) <= 0.0f) return false;
// Determine whether origin is on + or - side of plane (v1,v0,v2)
JVector.Subtract(ref v1, ref v0, out temp1);
JVector.Subtract(ref v2, ref v0, out temp2);
JVector.Cross(ref temp1, ref temp2, out normal);
float dist = JVector.Dot(ref normal, ref v0);
// If the origin is on the - side of the plane, reverse the direction of the plane
if (dist > 0.0f)
{
JVector.Swap(ref v1, ref v2);
JVector.Swap(ref v11, ref v21);
JVector.Swap(ref v12, ref v22);
JVector.Negate(ref normal, out normal);
}
int phase2 = 0;
int phase1 = 0;
bool hit = false;
// Phase One: Identify a portal
while (true)
{
if (phase1 > MaximumIterations) return false;
phase1++;
// Obtain the support point in a direction perpendicular to the existing plane
// Note: This point is guaranteed to lie off the plane
JVector.Negate(ref normal, out mn);
SupportMapTransformed(support1, ref orientation1, ref position1, ref mn, out v31);
SupportMapTransformed(support2, ref orientation2, ref position2, ref normal, out v32);
JVector.Subtract(ref v32, ref v31, out v3);
if (JVector.Dot(ref v3, ref normal) <= 0.0f)
{
return false;
}
// If origin is outside (v1,v0,v3), then eliminate v2 and loop
JVector.Cross(ref v1, ref v3, out temp1);
if (JVector.Dot(ref temp1, ref v0) < 0.0f)
{
v2 = v3;
v21 = v31;
v22 = v32;
JVector.Subtract(ref v1, ref v0, out temp1);
JVector.Subtract(ref v3, ref v0, out temp2);
JVector.Cross(ref temp1, ref temp2, out normal);
continue;
}
// If origin is outside (v3,v0,v2), then eliminate v1 and loop
JVector.Cross(ref v3, ref v2, out temp1);
if (JVector.Dot(ref temp1, ref v0) < 0.0f)
{
v1 = v3;
v11 = v31;
v12 = v32;
JVector.Subtract(ref v3, ref v0, out temp1);
JVector.Subtract(ref v2, ref v0, out temp2);
JVector.Cross(ref temp1, ref temp2, out normal);
continue;
}
// Phase Two: Refine the portal
// We are now inside of a wedge...
while (true)
{
phase2++;
// Compute normal of the wedge face
JVector.Subtract(ref v2, ref v1, out temp1);
JVector.Subtract(ref v3, ref v1, out temp2);
JVector.Cross(ref temp1, ref temp2, out normal);
// Can this happen??? Can it be handled more cleanly?
if (normal.IsNearlyZero()) return true;
normal.Normalize();
// Compute distance from origin to wedge face
float d = JVector.Dot(ref normal, ref v1);
// If the origin is inside the wedge, we have a hit
if (d >= 0 && !hit)
{
// HIT!!!
hit = true;
}
// Find the support point in the direction of the wedge face
JVector.Negate(ref normal, out mn);
SupportMapTransformed(support1, ref orientation1, ref position1, ref mn, out v41);
SupportMapTransformed(support2, ref orientation2, ref position2, ref normal, out v42);
JVector.Subtract(ref v42, ref v41, out v4);
JVector.Subtract(ref v4, ref v3, out temp1);
float delta = JVector.Dot(ref temp1, ref normal);
penetration = JVector.Dot(ref v4, ref normal);
// If the boundary is thin enough or the origin is outside the support plane for the newly discovered vertex, then we can terminate
if (delta <= CollideEpsilon || penetration <= 0.0f || phase2 > MaximumIterations)
{
if (hit)
{
JVector.Cross(ref v1, ref v2, out temp1);
float b0 = JVector.Dot(ref temp1, ref v3);
JVector.Cross(ref v3, ref v2, out temp1);
float b1 = JVector.Dot(ref temp1, ref v0);
JVector.Cross(ref v0, ref v1, out temp1);
float b2 = JVector.Dot(ref temp1, ref v3);
JVector.Cross(ref v2, ref v1, out temp1);
float b3 = JVector.Dot(ref temp1, ref v0);
float sum = b0 + b1 + b2 + b3;
if (sum <= 0)
{
b0 = 0;
JVector.Cross(ref v2, ref v3, out temp1);
b1 = JVector.Dot(ref temp1, ref normal);
JVector.Cross(ref v3, ref v1, out temp1);
b2 = JVector.Dot(ref temp1, ref normal);
JVector.Cross(ref v1, ref v2, out temp1);
b3 = JVector.Dot(ref temp1, ref normal);
sum = b1 + b2 + b3;
}
float inv = 1.0f / sum;
JVector.Multiply(ref v01, b0, out point);
JVector.Multiply(ref v11, b1, out temp1);
JVector.Add(ref point, ref temp1, out point);
JVector.Multiply(ref v21, b2, out temp1);
JVector.Add(ref point, ref temp1, out point);
JVector.Multiply(ref v31, b3, out temp1);
JVector.Add(ref point, ref temp1, out point);
JVector.Multiply(ref v02, b0, out temp2);
JVector.Add(ref temp2, ref point, out point);
JVector.Multiply(ref v12, b1, out temp1);
JVector.Add(ref point, ref temp1, out point);
JVector.Multiply(ref v22, b2, out temp1);
JVector.Add(ref point, ref temp1, out point);
JVector.Multiply(ref v32, b3, out temp1);
JVector.Add(ref point, ref temp1, out point);
JVector.Multiply(ref point, inv * 0.5f, out point);
}
// Compute the barycentric coordinates of the origin
return hit;
}
////// Compute the tetrahedron dividing face (v4,v0,v1)
//JVector.Cross(ref v4, ref v1, out temp1);
//float d1 = JVector.Dot(ref temp1, ref v0);
////// Compute the tetrahedron dividing face (v4,v0,v2)
//JVector.Cross(ref v4, ref v2, out temp1);
//float d2 = JVector.Dot(ref temp1, ref v0);
// Compute the tetrahedron dividing face (v4,v0,v3)
JVector.Cross(ref v4, ref v0, out temp1);
float dot = JVector.Dot(ref temp1, ref v1);
if (dot >= 0.0f)
{
dot = JVector.Dot(ref temp1, ref v2);
if (dot >= 0.0f)
{
// Inside d1 & inside d2 ==> eliminate v1
v1 = v4;
v11 = v41;
v12 = v42;
}
else
{
// Inside d1 & outside d2 ==> eliminate v3
v3 = v4;
v31 = v41;
v32 = v42;
}
}
else
{
dot = JVector.Dot(ref temp1, ref v3);
if (dot >= 0.0f)
{
// Outside d1 & inside d3 ==> eliminate v2
v2 = v4;
v21 = v41;
v22 = v42;
}
else
{
// Outside d1 & outside d3 ==> eliminate v1
v1 = v4;
v11 = v41;
v12 = v42;
}
}
}
}
}
private static void SupportMapTransformed(ISupportMappable support,
ref JMatrix orientation, ref JVector position, ref JVector direction, out JVector result)
{
// THIS IS *THE* HIGH FREQUENCY CODE OF THE COLLLISION PART OF THE ENGINE
result.X = ((direction.X * orientation.M11) + (direction.Y * orientation.M12)) + (direction.Z * orientation.M13);
result.Y = ((direction.X * orientation.M21) + (direction.Y * orientation.M22)) + (direction.Z * orientation.M23);
result.Z = ((direction.X * orientation.M31) + (direction.Y * orientation.M32)) + (direction.Z * orientation.M33);
support.SupportMapping(ref result, out result);
float x = ((result.X * orientation.M11) + (result.Y * orientation.M21)) + (result.Z * orientation.M31);
float y = ((result.X * orientation.M12) + (result.Y * orientation.M22)) + (result.Z * orientation.M32);
float z = ((result.X * orientation.M13) + (result.Y * orientation.M23)) + (result.Z * orientation.M33);
result.X = position.X + x;
result.Y = position.Y + y;
result.Z = position.Z + z;
}
/// <summary>
/// Transforms a vector by the transposed of the given Matrix.
/// </summary>
/// <param name="position">The vector to transform.</param>
/// <param name="matrix">The transform matrix.</param>
/// <param name="result">The transformed vector.</param>
public static void TransposedTransform(ref JVector position, ref JMatrix matrix, out JVector result)
{
float num0 = ((position.X * matrix.M11) + (position.Y * matrix.M12)) + (position.Z * matrix.M31);
float num1 = ((position.X * matrix.M21) + (position.Y * matrix.M22)) + (position.Z * matrix.M23);
float num2 = ((position.X * matrix.M31) + (position.Y * matrix.M32)) + (position.Z * matrix.M33);
result.X = num0;
result.Y = num1;
result.Z = num2;
}
public static JVector Transform(JVector position, JMatrix matrix)
{
JVector result;
JVector.Transform(ref position, ref matrix, out result);
return result;
}
static JMatrix()
{
Zero = new JMatrix();
Identity = new JMatrix();
Identity.M11 = 1.0f;
Identity.M22 = 1.0f;
Identity.M33 = 1.0f;
InternalIdentity = Identity;
}
/// <summary>
/// Gets the axis aligned bounding box of the orientated shape. This includes
/// the whole shape.
/// </summary>
/// <param name="orientation">The orientation of the shape.</param>
/// <param name="box">The axis aligned bounding box of the shape.</param>
public override void GetBoundingBox(ref JMatrix orientation, out JBBox box)
{
box = boundings;
#region Expand Spherical
box.Min.X -= sphericalExpansion;
box.Min.Y -= sphericalExpansion;
box.Min.Z -= sphericalExpansion;
box.Max.X += sphericalExpansion;
box.Max.Y += sphericalExpansion;
box.Max.Z += sphericalExpansion;
#endregion
box.Transform(ref orientation);
}
public void Rotate(JMatrix orientation, JVector center)
{
for (int i = 0; i < points.Count; i++)
{
points[i].position = JVector.Transform(points[i].position - center, orientation);
}
}
/// <summary>
/// Constraints two bodies to always have the same relative
/// orientation to each other. Combine the AngleConstraint with a PointOnLine
/// Constraint to get a prismatic joint.
/// </summary>
public FixedAngle(RigidBody body1, RigidBody body2)
: base(body1, body2)
{
initialOrientation1 = body1.orientation;
initialOrientation2 = body2.orientation;
//orientationDifference = body1.orientation * body2.invOrientation;
//orientationDifference = JMatrix.Transpose(orientationDifference);
}
// 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;
}
/// <summary>
/// Creates a new instance of the TransformedShape struct.
/// </summary>
/// <param name="shape">The shape.</param>
/// <param name="orientation">The orientation this shape should have.</param>
/// <param name="position">The position this shape should have.</param>
public TransformedShape(Shape shape, JMatrix orientation, JVector position)
{
this.position = position;
this.orientation = orientation;
JMatrix.Transpose(ref orientation, out invOrientation);
this.shape = shape;
this.boundingBox = new JBBox();
UpdateBoundingBox();
}
/// <summary>
/// Gets the axis aligned bounding box of the orientated shape.
/// </summary>
/// <param name="orientation">The orientation of the shape.</param>
/// <param name="box">The axis aligned bounding box of the shape.</param>
public override void GetBoundingBox(ref JMatrix orientation, out JBBox box)
{
JMatrix abs; JMath.Absolute(ref orientation, out abs);
JVector temp;
JVector.Transform(ref halfSize, ref abs, out temp);
box.Max = temp;
JVector.Negate(ref temp, out box.Min);
}
public static JMatrix Transpose(JMatrix matrix)
{
JMatrix result;
JMatrix.Transpose(ref matrix, out result);
return result;
}
/// <summary>
/// Gets the axis aligned bounding box of the orientated shape. (Inlcuding all
/// 'sub' shapes)
/// </summary>
/// <param name="orientation">The orientation of the shape.</param>
/// <param name="box">The axis aligned bounding box of the shape.</param>
public override void GetBoundingBox(ref JMatrix orientation, out JBBox box)
{
box.Min = mInternalBBox.Min;
box.Max = mInternalBBox.Max;
JVector localHalfExtents = 0.5f * (box.Max - box.Min);
JVector localCenter = 0.5f * (box.Max + box.Min);
JVector center;
JVector.Transform(ref localCenter, ref orientation, out center);
JMatrix abs; JMath.Absolute(ref orientation, out abs);
JVector temp;
JVector.Transform(ref localHalfExtents, ref abs, out temp);
box.Max = center + temp;
box.Min = center - temp;
}
/// <summary>
/// Calculates the bounding box of the sphere.
/// </summary>
/// <param name="orientation">The orientation of the shape.</param>
/// <param name="box">The resulting axis aligned bounding box.</param>
public override void GetBoundingBox(ref JMatrix orientation, out JBBox box)
{
box.Min.X = -radius;
box.Min.Y = -radius;
box.Min.Z = -radius;
box.Max.X = radius;
box.Max.Y = radius;
box.Max.Z = radius;
}
public void Transform(ref JMatrix orientation)
{
JVector halfExtents = 0.5f * (Max - Min);
JVector center = 0.5f * (Max + Min);
JVector.Transform(ref center, ref orientation, out center);
JMatrix abs; JMath.Absolute(ref orientation, out abs);
JVector.Transform(ref halfExtents, ref abs, out halfExtents);
Max = center + halfExtents;
Min = center - halfExtents;
}
/// <summary>
/// Called once before iteration starts.
/// </summary>
/// <param name="timestep">The 5simulation timestep</param>
public override void PrepareForIteration(float timestep)
{
effectiveMass = body1.invInertiaWorld;
softnessOverDt = softness / timestep;
effectiveMass.M11 += softnessOverDt;
effectiveMass.M22 += softnessOverDt;
effectiveMass.M33 += softnessOverDt;
JMatrix.Inverse(ref effectiveMass, out effectiveMass);
JMatrix q = JMatrix.Transpose(orientation) * body1.orientation;
JVector axis;
float x = q.M32 - q.M23;
float y = q.M13 - q.M31;
float z = q.M21 - q.M12;
float r = JMath.Sqrt(x * x + y * y + z * z);
float t = q.M11 + q.M22 + q.M33;
float angle = (float)Math.Atan2(r, t - 1);
axis = new JVector(x, y, z) * angle;
if (r != 0.0f) axis = axis * (1.0f / r);
bias = axis * biasFactor * (-1.0f / timestep);
// Apply previous frame solution as initial guess for satisfying the constraint.
if (!body1.IsStatic) body1.angularVelocity += JVector.Transform(accumulatedImpulse, body1.invInertiaWorld);
}
/// <summary>
/// Constraints two bodies to always have the same relative
/// orientation to each other. Combine the AngleConstraint with a PointOnLine
/// Constraint to get a prismatic joint.
/// </summary>
public FixedAngle(RigidBody body1)
: base(body1, null)
{
orientation = body1.orientation;
}
public static JMatrix Multiply(JMatrix matrix1, float scaleFactor)
{
JMatrix result;
JMatrix.Multiply(ref matrix1, scaleFactor, out result);
return result;
}
