public static FPMatrix ToTSMatrix(this Quaternion rot) { return(FPMatrix.CreateFromQuaternion(rot.ToFPQuaternion())); }
public static Quaternion ToQuaternion(this FPMatrix fMatrix) { return(FPQuaternion.CreateFromMatrix(fMatrix).ToQuaternion()); }
/// <summary> /// Calculates the inertia of the shape relative to the center of mass. /// </summary> /// <param name="shape"></param> /// <param name="centerOfMass"></param> /// <param name="inertia">Returns the inertia relative to the center of mass, not to the origin</param> /// <returns></returns> #region public static FP CalculateMassInertia(Shape shape, out JVector centerOfMass, out JMatrix inertia) public static FP CalculateMassInertia(Shape shape, out FPVector centerOfMass, out FPMatrix inertia) { FP mass = FP.Zero; centerOfMass = FPVector.zero; inertia = FPMatrix.Zero; if (shape is Multishape) { throw new ArgumentException("Can't calculate inertia of multishapes.", "shape"); } // build a triangle hull around the shape List <FPVector> hullTriangles = new List <FPVector>(); shape.MakeHull(ref hullTriangles, 3); // create inertia of tetrahedron with vertices at // (0,0,0) (1,0,0) (0,1,0) (0,0,1) FP a = FP.One / (60 * FP.One), b = FP.One / (120 * FP.One); FPMatrix C = new FPMatrix(a, b, b, b, a, b, b, b, a); for (int i = 0; i < hullTriangles.Count; i += 3) { FPVector column0 = hullTriangles[i + 0]; FPVector column1 = hullTriangles[i + 1]; FPVector column2 = hullTriangles[i + 2]; FPMatrix A = new FPMatrix(column0.x, column1.x, column2.x, column0.y, column1.y, column2.y, column0.z, column1.z, column2.z); FP detA = A.Determinant(); // now transform this canonical tetrahedron to the target tetrahedron // inertia by a linear transformation A FPMatrix tetrahedronInertia = FPMatrix.Multiply(A * C * FPMatrix.Transpose(A), detA); FPVector tetrahedronCOM = (FP.One / (4 * FP.One)) * (hullTriangles[i + 0] + hullTriangles[i + 1] + hullTriangles[i + 2]); FP tetrahedronMass = (FP.One / (6 * FP.One)) * detA; inertia += tetrahedronInertia; centerOfMass += tetrahedronMass * tetrahedronCOM; mass += tetrahedronMass; } inertia = FPMatrix.Multiply(FPMatrix.Identity, inertia.Trace()) - inertia; centerOfMass = centerOfMass * (FP.One / mass); FP x = centerOfMass.x; FP y = centerOfMass.y; FP z = centerOfMass.z; // now translate the inertia by the center of mass FPMatrix t = new FPMatrix( -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)); FPMatrix.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 FPMatrix orientation1, ref FPMatrix orientation2, ref FPVector position1, ref FPVector position2, out FPVector point, out FPVector normal, out FP penetration) { // Used variables FPVector temp1, temp2; FPVector v01, v02, v0; FPVector v11, v12, v1; FPVector v21, v22, v2; FPVector v31, v32, v3; FPVector v41 = FPVector.zero, v42 = FPVector.zero, v4 = FPVector.zero; FPVector mn; // Initialization of the output point = normal = FPVector.zero; penetration = FP.Zero; //JVector right = JVector.Right; // Get the center of shape1 in world coordinates -> v01 support1.SupportCenter(out v01); FPVector.Transform(ref v01, ref orientation1, out v01); FPVector.Add(ref position1, ref v01, out v01); // Get the center of shape2 in world coordinates -> v02 support2.SupportCenter(out v02); FPVector.Transform(ref v02, ref orientation2, out v02); FPVector.Add(ref position2, ref v02, out v02); // v0 is the center of the minkowski difference FPVector.Subtract(ref v02, ref v01, out v0); // Avoid case where centers overlap -- any direction is fine in this case if (v0.IsNearlyZero()) { v0 = new FPVector(FP.EN4, 0, 0); } // v1 = support in direction of origin mn = v0; FPVector.Negate(ref v0, out normal); //UnityEngine.Debug.Log("normal: " + normal); SupportMapTransformed(support1, ref orientation1, ref position1, ref mn, out v11); SupportMapTransformed(support2, ref orientation2, ref position2, ref normal, out v12); FPVector.Subtract(ref v12, ref v11, out v1); if (FPVector.Dot(ref v1, ref normal) <= FP.Zero) { return(false); } // v2 = support perpendicular to v1,v0 FPVector.Cross(ref v1, ref v0, out normal); if (normal.IsNearlyZero()) { FPVector.Subtract(ref v1, ref v0, out normal); //UnityEngine.Debug.Log("normal: " + normal); normal.Normalize(); point = v11; FPVector.Add(ref point, ref v12, out point); FPVector.Multiply(ref point, FP.Half, out point); FPVector.Subtract(ref v12, ref v11, out temp1); penetration = FPVector.Dot(ref temp1, ref normal); //point = v11; //point2 = v12; return(true); } FPVector.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); FPVector.Subtract(ref v22, ref v21, out v2); if (FPVector.Dot(ref v2, ref normal) <= FP.Zero) { return(false); } // Determine whether origin is on + or - side of plane (v1,v0,v2) FPVector.Subtract(ref v1, ref v0, out temp1); FPVector.Subtract(ref v2, ref v0, out temp2); FPVector.Cross(ref temp1, ref temp2, out normal); FP dist = FPVector.Dot(ref normal, ref v0); // If the origin is on the - side of the plane, reverse the direction of the plane if (dist > FP.Zero) { FPVector.Swap(ref v1, ref v2); FPVector.Swap(ref v11, ref v21); FPVector.Swap(ref v12, ref v22); FPVector.Negate(ref normal, out normal); //UnityEngine.Debug.Log("normal: " + 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 FPVector.Negate(ref normal, out mn); //UnityEngine.Debug.Log("mn: " + mn); SupportMapTransformed(support1, ref orientation1, ref position1, ref mn, out v31); SupportMapTransformed(support2, ref orientation2, ref position2, ref normal, out v32); FPVector.Subtract(ref v32, ref v31, out v3); if (FPVector.Dot(ref v3, ref normal) <= FP.Zero) { return(false); } // If origin is outside (v1,v0,v3), then eliminate v2 and loop FPVector.Cross(ref v1, ref v3, out temp1); if (FPVector.Dot(ref temp1, ref v0) < FP.Zero) { v2 = v3; v21 = v31; v22 = v32; FPVector.Subtract(ref v1, ref v0, out temp1); FPVector.Subtract(ref v3, ref v0, out temp2); FPVector.Cross(ref temp1, ref temp2, out normal); // UnityEngine.Debug.Log("normal: " + normal); continue; } // If origin is outside (v3,v0,v2), then eliminate v1 and loop FPVector.Cross(ref v3, ref v2, out temp1); if (FPVector.Dot(ref temp1, ref v0) < FP.Zero) { v1 = v3; v11 = v31; v12 = v32; FPVector.Subtract(ref v3, ref v0, out temp1); FPVector.Subtract(ref v2, ref v0, out temp2); FPVector.Cross(ref temp1, ref temp2, out normal); //UnityEngine.Debug.Log("normal: " + normal); continue; } // Phase Two: Refine the portal // We are now inside of a wedge... while (true) { phase2++; /* * UnityEngine.Debug.LogError(" ::Start STATE"); * UnityEngine.Debug.Log(temp1 + " " + temp2); * UnityEngine.Debug.Log( v01 + " " + v02 + " "+ v0); * UnityEngine.Debug.Log( v11+" "+ v12 +" "+ v1); * UnityEngine.Debug.Log( v21 +" "+ v22 +" "+ v2); * UnityEngine.Debug.Log( v31 +" "+ v32 +" "+ v3); * UnityEngine.Debug.Log( v41 +" "+ v42 +" "+ v4); * UnityEngine.Debug.Log( mn); * * UnityEngine.Debug.LogError(" ::END STATE"); */ // Compute normal of the wedge face FPVector.Subtract(ref v2, ref v1, out temp1); FPVector.Subtract(ref v3, ref v1, out temp2); FPVector.Cross(ref temp1, ref temp2, out normal); // Beginer // UnityEngine.Debug.Log("normal: " + normal); // Can this happen??? Can it be handled more cleanly? if (normal.IsNearlyZero()) { return(true); } normal.Normalize(); //UnityEngine.Debug.Log("normal: " + normal); // Compute distance from origin to wedge face FP d = FPVector.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 FPVector.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); FPVector.Subtract(ref v42, ref v41, out v4); FPVector.Subtract(ref v4, ref v3, out temp1); FP delta = FPVector.Dot(ref temp1, ref normal); penetration = FPVector.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 <= FP.Zero || phase2 > MaximumIterations) { if (hit) { FPVector.Cross(ref v1, ref v2, out temp1); FP b0 = FPVector.Dot(ref temp1, ref v3); FPVector.Cross(ref v3, ref v2, out temp1); FP b1 = FPVector.Dot(ref temp1, ref v0); FPVector.Cross(ref v0, ref v1, out temp1); FP b2 = FPVector.Dot(ref temp1, ref v3); FPVector.Cross(ref v2, ref v1, out temp1); FP b3 = FPVector.Dot(ref temp1, ref v0); FP sum = b0 + b1 + b2 + b3; if (sum <= 0) { b0 = 0; FPVector.Cross(ref v2, ref v3, out temp1); b1 = FPVector.Dot(ref temp1, ref normal); FPVector.Cross(ref v3, ref v1, out temp1); b2 = FPVector.Dot(ref temp1, ref normal); FPVector.Cross(ref v1, ref v2, out temp1); b3 = FPVector.Dot(ref temp1, ref normal); sum = b1 + b2 + b3; } FP inv = FP.One / sum; FPVector.Multiply(ref v01, b0, out point); FPVector.Multiply(ref v11, b1, out temp1); FPVector.Add(ref point, ref temp1, out point); FPVector.Multiply(ref v21, b2, out temp1); FPVector.Add(ref point, ref temp1, out point); FPVector.Multiply(ref v31, b3, out temp1); FPVector.Add(ref point, ref temp1, out point); FPVector.Multiply(ref v02, b0, out temp2); FPVector.Add(ref temp2, ref point, out point); FPVector.Multiply(ref v12, b1, out temp1); FPVector.Add(ref point, ref temp1, out point); FPVector.Multiply(ref v22, b2, out temp1); FPVector.Add(ref point, ref temp1, out point); FPVector.Multiply(ref v32, b3, out temp1); FPVector.Add(ref point, ref temp1, out point); FPVector.Multiply(ref point, inv * FP.Half, 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); //FP d1 = JVector.Dot(ref temp1, ref v0); //// Compute the tetrahedron dividing face (v4,v0,v2) //JVector.Cross(ref v4, ref v2, out temp1); //FP d2 = JVector.Dot(ref temp1, ref v0); // Compute the tetrahedron dividing face (v4,v0,v3) //UnityEngine.Debug.LogError("v4:" + v4 + " v0:" + v0); FPVector.Cross(ref v4, ref v0, out temp1); //UnityEngine.Debug.LogError("temp1:"+ temp1); //Ender // UnityEngine.Debug.Log("normal: " + normal); FP dot = FPVector.Dot(ref temp1, ref v1); if (dot >= FP.Zero) { // UnityEngine.Debug.Log("dot >= 0 temp1:" + temp1 + " v2:" + v2 ); dot = FPVector.Dot(ref temp1, ref v2); if (dot >= FP.Zero) { // UnityEngine.Debug.Log("dot >= 0 v1->v4"); // Inside d1 & inside d2 ==> eliminate v1 v1 = v4; v11 = v41; v12 = v42; } else { // UnityEngine.Debug.Log("dot < v3->v4"); // Inside d1 & outside d2 ==> eliminate v3 v3 = v4; v31 = v41; v32 = v42; } } else { // UnityEngine.Debug.Log("dot < 0 temp1:" + temp1 + " v3:" + v3 ); dot = FPVector.Dot(ref temp1, ref v3); if (dot >= FP.Zero) { // UnityEngine.Debug.Log("dot >= 0 v2 => v4"); // Outside d1 & inside d3 ==> eliminate v2 v2 = v4; v21 = v41; v22 = v42; } else { // UnityEngine.Debug.Log("dot < 0 v1 => v4"); // Outside d1 & outside d3 ==> eliminate v1 v1 = v4; v11 = v41; v12 = v42; } } } } }
/// <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 FPMatrix orientation, ref FPVector position, ref FPVector point) { FPVector arbitraryPoint; SupportMapTransformed(support, ref orientation, ref position, ref point, out arbitraryPoint); FPVector.Subtract(ref point, ref arbitraryPoint, out arbitraryPoint); FPVector r; support.SupportCenter(out r); FPVector.Transform(ref r, ref orientation, out r); FPVector.Add(ref position, ref r, out r); FPVector.Subtract(ref point, ref r, out r); FPVector x = point; FPVector w, p; FP VdotR; FPVector v; FPVector.Subtract(ref x, ref arbitraryPoint, out v); FP dist = v.sqrMagnitude; FP epsilon = CollideEpsilon; int maxIter = MaxIterations; VoronoiSimplexSolver simplexSolver = simplexSolverPool.GetNew(); simplexSolver.Reset(); while ((dist > epsilon) && (maxIter-- != 0)) { SupportMapTransformed(support, ref orientation, ref position, ref v, out p); FPVector.Subtract(ref x, ref p, out w); FP VdotW = FPVector.Dot(ref v, ref w); if (VdotW > FP.Zero) { VdotR = FPVector.Dot(ref v, ref r); if (VdotR >= -(FPMath.Epsilon * FPMath.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.sqrMagnitude; } else { dist = FP.Zero; } } simplexSolverPool.GiveBack(simplexSolver); return(true); }
// public static bool TimeOfImpact(ISupportMappable support1, ISupportMappable support2, ref JMatrix orientation1, //ref JMatrix orientation2, ref JVector position1, ref JVector position2, ref JVector sweptA, ref JVector sweptB, //out JVector p1, out JVector p2, out JVector normal) // { // VoronoiSimplexSolver simplexSolver = simplexSolverPool.GetNew(); // simplexSolver.Reset(); // FP lambda = FP.Zero; // p1 = p2 = JVector.Zero; // JVector x1 = position1; // JVector x2 = position2; // JVector r = sweptA - sweptB; // JVector w, v; // JVector supVertexA; // JVector rn = JVector.Negate(r); // SupportMapTransformed(support1, ref orientation1, ref x1, ref rn, out supVertexA); // JVector supVertexB; // SupportMapTransformed(support2, ref orientation2, ref x2, ref r, out supVertexB); // v = supVertexA - supVertexB; // bool hasResult = false; // normal = JVector.Zero; // FP lastLambda = lambda; // int maxIter = MaxIterations; // FP distSq = v.LengthSquared(); // FP epsilon = FP.EN5; // FP VdotR; // while ((distSq > epsilon) && (maxIter-- != 0)) // { // JVector vn = JVector.Negate(v); // SupportMapTransformed(support1, ref orientation1, ref x1, ref vn, out supVertexA); // SupportMapTransformed(support2, ref orientation2, ref x2, ref v, out supVertexB); // w = supVertexA - supVertexB; // FP VdotW = JVector.Dot(ref v, ref w); // if (VdotW > FP.Zero) // { // VdotR = JVector.Dot(ref v, ref r); // if (VdotR >= -JMath.Epsilon) // { // simplexSolverPool.GiveBack(simplexSolver); // return false; // } // else // { // lambda = lambda - VdotW / VdotR; // x1 = position1 + lambda * sweptA; // x2 = position2 + lambda * sweptB; // w = supVertexA - supVertexB; // normal = v; // hasResult = true; // } // } // if (!simplexSolver.InSimplex(w)) simplexSolver.AddVertex(w, supVertexA, supVertexB); // if (simplexSolver.Closest(out v)) // { // distSq = v.LengthSquared(); // normal = v; // hasResult = true; // } // else distSq = FP.Zero; // } // simplexSolver.ComputePoints(out p1, out p2); // if (normal.LengthSquared() > JMath.Epsilon * JMath.Epsilon) // normal.Normalize(); // p1 = p1 - lambda * sweptA; // p2 = p2 - lambda * sweptB; // simplexSolverPool.GiveBack(simplexSolver); // return true; // } #endregion // 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 FPMatrix orientation, ref FPMatrix invOrientation, ref FPVector position, ref FPVector origin, ref FPVector direction, out FP fraction, out FPVector normal) { VoronoiSimplexSolver simplexSolver = simplexSolverPool.GetNew(); simplexSolver.Reset(); normal = FPVector.zero; fraction = FP.MaxValue; FP lambda = FP.Zero; FPVector r = direction; FPVector x = origin; FPVector w, p, v; FPVector arbitraryPoint; SupportMapTransformed(support, ref orientation, ref position, ref r, out arbitraryPoint); FPVector.Subtract(ref x, ref arbitraryPoint, out v); int maxIter = MaxIterations; FP distSq = v.sqrMagnitude; FP epsilon = FP.EN6; FP VdotR; while ((distSq > epsilon) && (maxIter-- != 0)) { SupportMapTransformed(support, ref orientation, ref position, ref v, out p); FPVector.Subtract(ref x, ref p, out w); FP VdotW = FPVector.Dot(ref v, ref w); if (VdotW > FP.Zero) { VdotR = FPVector.Dot(ref v, ref r); if (VdotR >= -FPMath.Epsilon) { simplexSolverPool.GiveBack(simplexSolver); return(false); } else { lambda = lambda - VdotW / VdotR; FPVector.Multiply(ref r, lambda, out x); FPVector.Add(ref origin, ref x, out x); FPVector.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.sqrMagnitude; } else { distSq = FP.Zero; } } #region Retrieving hitPoint // Giving back the fraction like this *should* work // but is inaccurate against large objects: // fraction = lambda; FPVector p1, p2; simplexSolver.ComputePoints(out p1, out p2); p2 = p2 - origin; fraction = p2.magnitude / direction.magnitude; #endregion if (normal.sqrMagnitude > FPMath.Epsilon * FPMath.Epsilon) { normal.Normalize(); } simplexSolverPool.GiveBack(simplexSolver); return(true); }
public static bool ClosestPoints(ISupportMappable support1, ISupportMappable support2, ref FPMatrix orientation1, ref FPMatrix orientation2, ref FPVector position1, ref FPVector position2, out FPVector p1, out FPVector p2, out FPVector normal) { VoronoiSimplexSolver simplexSolver = simplexSolverPool.GetNew(); simplexSolver.Reset(); p1 = p2 = FPVector.zero; FPVector r = position1 - position2; FPVector w, v; FPVector supVertexA; FPVector rn, vn; rn = FPVector.Negate(r); SupportMapTransformed(support1, ref orientation1, ref position1, ref rn, out supVertexA); FPVector supVertexB; SupportMapTransformed(support2, ref orientation2, ref position2, ref r, out supVertexB); v = supVertexA - supVertexB; normal = FPVector.zero; int maxIter = MaxIterations; FP distSq = v.sqrMagnitude; FP epsilon = CollideEpsilon; while ((distSq > epsilon) && (maxIter-- != 0)) { vn = FPVector.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.sqrMagnitude; normal = v; } else { distSq = FP.Zero; } } simplexSolver.ComputePoints(out p1, out p2); if (normal.sqrMagnitude > FPMath.Epsilon * FPMath.Epsilon) { normal.Normalize(); } simplexSolverPool.GiveBack(simplexSolver); return(true); }
/// <summary> /// Adds two matrices. /// </summary> /// <param name="value1">The first matrix.</param> /// <param name="value2">The second matrix.</param> /// <returns>The sum of both values.</returns> #region public static JMatrix operator +(JMatrix value1, JMatrix value2) public static FPMatrix operator +(FPMatrix value1, FPMatrix value2) { FPMatrix result; FPMatrix.Add(ref value1, ref value2, out result); return(result); }
/// <summary> /// Multiplies two matrices. /// </summary> /// <param name="value1">The first matrix.</param> /// <param name="value2">The second matrix.</param> /// <returns>The product of both values.</returns> #region public static JMatrix operator *(JMatrix value1,JMatrix value2) public static FPMatrix operator *(FPMatrix value1, FPMatrix value2) { FPMatrix result; FPMatrix.Multiply(ref value1, ref value2, out result); return(result); }