/// <summary> /// SupportMapping. Finds the point in the shape furthest away from the given direction. /// Imagine a plane with a normal in the search direction. Now move the plane along the normal /// until the plane does not intersect the shape. The last intersection point is the result. /// </summary> /// <param name="direction">The direction.</param> /// <param name="result">The result.</param> public override void SupportMapping(ref FPVector direction, out FPVector result) { result = direction; result.Normalize(); FPVector.Multiply(ref result, radius, out result); }
/// <summary> /// SupportMapping. Finds the point in the shape furthest away from the given direction. /// Imagine a plane with a normal in the search direction. Now move the plane along the normal /// until the plane does not intersect the shape. The last intersection point is the result. /// </summary> /// <param name="direction">The direction.</param> /// <param name="result">The result.</param> public override void SupportMapping(ref FPVector direction, out FPVector result) { FPVector expandVector; FPVector.Normalize(ref direction, out expandVector); FPVector.Multiply(ref expandVector, sphericalExpansion, out expandVector); int minIndex = 0; FP min = FPVector.Dot(ref points[0], ref direction); FP dot = FPVector.Dot(ref points[1], ref direction); if (dot > min) { min = dot; minIndex = 1; } dot = FPVector.Dot(ref points[2], ref direction); if (dot > min) { min = dot; minIndex = 2; } FPVector.Add(ref points[minIndex], ref expandVector, out result); }
public void SupportMapping(ref FPVector direction, out FPVector result) { FP min = FPVector.Dot(ref owner.points[indices.I0].position, ref direction); FP dot = FPVector.Dot(ref owner.points[indices.I1].position, ref direction); FPVector minVertex = owner.points[indices.I0].position; if (dot > min) { min = dot; minVertex = owner.points[indices.I1].position; } dot = FPVector.Dot(ref owner.points[indices.I2].position, ref direction); if (dot > min) { min = dot; minVertex = owner.points[indices.I2].position; } FPVector exp; FPVector.Normalize(ref direction, out exp); exp *= owner.triangleExpansion; result = minVertex + exp; }
/// <summary> /// SupportMapping. Finds the point in the shape furthest away from the given direction. /// Imagine a plane with a normal in the search direction. Now move the plane along the normal /// until the plane does not intersect the shape. The last intersection point is the result. /// </summary> /// <param name="direction">The direction.</param> /// <param name="result">The result.</param> public override void SupportMapping(ref FPVector direction, out FPVector result) { FPVector exp; FPVector.Normalize(ref direction, out exp); exp *= sphericalExpansion; FP min = FPVector.Dot(ref vecs[0], ref direction); int minIndex = 0; FP dot = FPVector.Dot(ref vecs[1], ref direction); if (dot > min) { min = dot; minIndex = 1; } dot = FPVector.Dot(ref vecs[2], ref direction); if (dot > min) { min = dot; minIndex = 2; } result = vecs[minIndex] + exp; }
/// <summary> /// Initializes a contact. /// </summary> /// <param name="body1">The first body.</param> /// <param name="body2">The second body.</param> /// <param name="point1">The collision point in worldspace</param> /// <param name="point2">The collision point in worldspace</param> /// <param name="n">The normal pointing to body2.</param> /// <param name="penetration">The estimated penetration depth.</param> public void Initialize(RigidBody body1, RigidBody body2, ref FPVector point1, ref FPVector point2, ref FPVector n, FP penetration, bool newContact, ContactSettings settings) { this.body1 = body1; this.body2 = body2; this.normal = n; normal.Normalize(); this.p1 = point1; this.p2 = point2; this.newContact = newContact; FPVector.Subtract(ref p1, ref body1.position, out relativePos1); FPVector.Subtract(ref p2, ref body2.position, out relativePos2); FPVector.Transform(ref relativePos1, ref body1.invOrientation, out realRelPos1); FPVector.Transform(ref relativePos2, ref body2.invOrientation, out realRelPos2); this.initialPen = penetration; this.penetration = penetration; body1IsMassPoint = body1.isParticle; body2IsMassPoint = body2.isParticle; // Material Properties if (newContact) { treatBody1AsStatic = body1.isStatic; treatBody2AsStatic = body2.isStatic; accumulatedNormalImpulse = FP.Zero; accumulatedTangentImpulse = FP.Zero; lostSpeculativeBounce = FP.Zero; switch (settings.MaterialCoefficientMixing) { case ContactSettings.MaterialCoefficientMixingType.TakeMaximum: staticFriction = FPMath.Max(body1.staticFriction, body2.staticFriction); dynamicFriction = FPMath.Max(body1.staticFriction, body2.staticFriction); restitution = FPMath.Max(body1.restitution, body2.restitution); break; case ContactSettings.MaterialCoefficientMixingType.TakeMinimum: staticFriction = FPMath.Min(body1.staticFriction, body2.staticFriction); dynamicFriction = FPMath.Min(body1.staticFriction, body2.staticFriction); restitution = FPMath.Min(body1.restitution, body2.restitution); break; case ContactSettings.MaterialCoefficientMixingType.UseAverage: staticFriction = (body1.staticFriction + body2.staticFriction) * FP.Half; dynamicFriction = (body1.staticFriction + body2.staticFriction) * FP.Half; restitution = (body1.restitution + body2.restitution) * FP.Half; break; } } this.settings = settings; }
/// <summary> /// Called once before iteration starts. /// </summary> /// <param name="timestep">The simulation timestep</param> public override void PrepareForIteration(FP timestep) { FPVector.Transform(ref localAnchor1, ref body1.orientation, out r1); FPVector.Transform(ref localAnchor2, ref body2.orientation, out r2); FPVector p1, p2, dp; FPVector.Add(ref body1.position, ref r1, out p1); FPVector.Add(ref body2.position, ref r2, out p2); FPVector.Subtract(ref p2, ref p1, out dp); FPVector l = FPVector.Transform(lineNormal, body1.orientation); l.Normalize(); FPVector t = (p1 - p2) % l; if (t.sqrMagnitude != FP.Zero) { t.Normalize(); } t = t % l; jacobian[0] = t; // linearVel Body1 jacobian[1] = (r1 + p2 - p1) % t; // angularVel Body1 jacobian[2] = -FP.One * t; // linearVel Body2 jacobian[3] = -FP.One * r2 % t; // angularVel Body2 effectiveMass = body1.inverseMass + body2.inverseMass + FPVector.Transform(jacobian[1], body1.invInertiaWorld) * jacobian[1] + FPVector.Transform(jacobian[3], body2.invInertiaWorld) * jacobian[3]; softnessOverDt = softness / timestep; effectiveMass += softnessOverDt; if (effectiveMass != 0) { effectiveMass = FP.One / effectiveMass; } bias = -(l % (p2 - p1)).magnitude * biasFactor * (FP.One / timestep); if (!body1.isStatic) { body1.linearVelocity += body1.inverseMass * accumulatedImpulse * jacobian[0]; body1.angularVelocity += FPVector.Transform(accumulatedImpulse * jacobian[1], body1.invInertiaWorld); } if (!body2.isStatic) { body2.linearVelocity += body2.inverseMass * accumulatedImpulse * jacobian[2]; body2.angularVelocity += FPVector.Transform(accumulatedImpulse * jacobian[3], body2.invInertiaWorld); } }
/// <summary> /// Constraints a point on a body to be fixed on a line /// which is fixed on another body. /// </summary> /// <param name="body1"></param> /// <param name="body2"></param> /// <param name="lineStartPointBody1"></param> /// <param name="lineDirection"></param> /// <param name="pointBody2"></param> public PointOnLine(RigidBody body1, RigidBody body2, FPVector lineStartPointBody1, FPVector pointBody2) : base(body1, body2) { FPVector.Subtract(ref lineStartPointBody1, ref body1.position, out localAnchor1); FPVector.Subtract(ref pointBody2, ref body2.position, out localAnchor2); FPVector.Transform(ref localAnchor1, ref body1.invOrientation, out localAnchor1); FPVector.Transform(ref localAnchor2, ref body2.invOrientation, out localAnchor2); lineNormal = FPVector.Normalize(lineStartPointBody1 - pointBody2); }
/// <summary> /// Called once before iteration starts. /// </summary> /// <param name="timestep">The 5simulation timestep</param> public override void PrepareForIteration(FP timestep) { FPVector dp; FPVector.Subtract(ref body2.position, ref body1.position, out dp); FP deltaLength = dp.magnitude - distance; if (behavior == DistanceBehavior.LimitMaximumDistance && deltaLength <= FP.Zero) { skipConstraint = true; } else if (behavior == DistanceBehavior.LimitMinimumDistance && deltaLength >= FP.Zero) { skipConstraint = true; } else { skipConstraint = false; FPVector n = dp; if (n.sqrMagnitude != FP.Zero) { n.Normalize(); } jacobian[0] = -FP.One * n; //jacobian[1] = -FP.One * (r1 % n); jacobian[1] = FP.One * n; //jacobian[3] = (r2 % n); effectiveMass = body1.inverseMass + body2.inverseMass; softnessOverDt = softness / timestep; effectiveMass += softnessOverDt; effectiveMass = FP.One / effectiveMass; bias = deltaLength * biasFactor * (FP.One / timestep); if (!body1.isStatic) { body1.linearVelocity += body1.inverseMass * accumulatedImpulse * jacobian[0]; } if (!body2.isStatic) { body2.linearVelocity += body2.inverseMass * accumulatedImpulse * jacobian[1]; } } }
/// <summary> /// /// </summary> /// <param name="rayOrigin"></param> /// <param name="rayDelta"></param> /// <returns></returns> public override int Prepare(ref FPVector rayOrigin, ref FPVector rayDelta) { potentialTriangles.Clear(); #region Expand Spherical FPVector expDelta; FPVector.Normalize(ref rayDelta, out expDelta); expDelta = rayDelta + expDelta * sphericalExpansion; #endregion octree.GetTrianglesIntersectingRay(potentialTriangles, rayOrigin, expDelta); return(potentialTriangles.Count); }
/// <summary> /// Called once before iteration starts. /// </summary> /// <param name="timestep">The 5simulation timestep</param> public override void PrepareForIteration(FP timestep) { FPVector.Transform(ref localAnchor1, ref body1.orientation, out r1); FPVector.Transform(ref localAnchor2, ref body2.orientation, out r2); FPVector p1, p2, dp; FPVector.Add(ref body1.position, ref r1, out p1); FPVector.Add(ref body2.position, ref r2, out p2); FPVector.Subtract(ref p2, ref p1, out dp); FP deltaLength = dp.magnitude; FPVector n = p2 - p1; if (n.sqrMagnitude != FP.Zero) { n.Normalize(); } jacobian[0] = -FP.One * n; jacobian[1] = -FP.One * (r1 % n); jacobian[2] = FP.One * n; jacobian[3] = (r2 % n); effectiveMass = body1.inverseMass + body2.inverseMass + FPVector.Transform(jacobian[1], body1.invInertiaWorld) * jacobian[1] + FPVector.Transform(jacobian[3], body2.invInertiaWorld) * jacobian[3]; softnessOverDt = softness / timestep; effectiveMass += softnessOverDt; effectiveMass = FP.One / effectiveMass; bias = deltaLength * biasFactor * (FP.One / timestep); if (!body1.isStatic) { body1.linearVelocity += body1.inverseMass * accumulatedImpulse * jacobian[0]; body1.angularVelocity += FPVector.Transform(accumulatedImpulse * jacobian[1], body1.invInertiaWorld); } if (!body2.isStatic) { body2.linearVelocity += body2.inverseMass * accumulatedImpulse * jacobian[2]; body2.angularVelocity += FPVector.Transform(accumulatedImpulse * jacobian[3], body2.invInertiaWorld); } }
/// <summary> /// /// </summary> /// <param name="rayOrigin"></param> /// <param name="rayDelta"></param> /// <returns></returns> public override int Prepare(ref FPVector rayOrigin, ref FPVector rayDelta) { TSBBox box = TSBBox.SmallBox; #region RayEnd + Expand Spherical FPVector rayEnd; FPVector.Normalize(ref rayDelta, out rayEnd); rayEnd = rayOrigin + rayDelta + rayEnd * sphericalExpansion; #endregion box.AddPoint(ref rayOrigin); box.AddPoint(ref rayEnd); return(this.Prepare(ref box)); }
public static void LookAt(FPVector forward, FPVector upwards, out FPMatrix result) { FPVector zaxis = forward; zaxis.Normalize(); FPVector xaxis = FPVector.Cross(upwards, zaxis); xaxis.Normalize(); FPVector yaxis = FPVector.Cross(zaxis, xaxis); result.M11 = xaxis.x; result.M21 = yaxis.x; result.M31 = zaxis.x; result.M12 = xaxis.y; result.M22 = yaxis.y; result.M32 = zaxis.y; result.M13 = xaxis.z; result.M23 = yaxis.z; result.M33 = zaxis.z; }
/// <summary> /// SupportMapping. Finds the point in the shape furthest away from the given direction. /// Imagine a plane with a normal in the search direction. Now move the plane along the normal /// until the plane does not intersect the shape. The last intersection point is the result. /// </summary> /// <param name="direction">The direction.</param> /// <param name="result">The result.</param> public override void SupportMapping(ref FPVector direction, out FPVector result) { FP r = FP.Sqrt(direction.x * direction.x + direction.z * direction.z); if (FP.Abs(direction.y) > FP.Zero) { FPVector dir; FPVector.Normalize(ref direction, out dir); FPVector.Multiply(ref dir, radius, out result); result.y += FP.Sign(direction.y) * FP.Half * length; } else if (r > FP.Zero) { result.x = direction.x / r * radius; result.y = FP.Zero; result.z = direction.z / r * radius; } else { result.x = FP.Zero; result.y = FP.Zero; result.z = FP.Zero; } }
/// <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> /// Hull making. /// </summary> /// <remarks>Based/Completely from http://www.xbdev.net/physics/MinkowskiDifference/index.php /// I don't (100%) see why this should always work. /// </remarks> /// <param name="triangleList"></param> /// <param name="generationThreshold"></param> public virtual void MakeHull(ref List <FPVector> triangleList, int generationThreshold) { FP distanceThreshold = FP.Zero; if (generationThreshold < 0) { generationThreshold = 4; } Stack <ClipTriangle> activeTriList = new Stack <ClipTriangle>(); FPVector[] v = new FPVector[] // 6 Array { new FPVector(-1, 0, 0), new FPVector(1, 0, 0), new FPVector(0, -1, 0), new FPVector(0, 1, 0), new FPVector(0, 0, -1), new FPVector(0, 0, 1), }; int[,] kTriangleVerts = new int[8, 3] // 8 x 3 Array { { 5, 1, 3 }, { 4, 3, 1 }, { 3, 4, 0 }, { 0, 5, 3 }, { 5, 2, 1 }, { 4, 1, 2 }, { 2, 0, 4 }, { 0, 2, 5 } }; for (int i = 0; i < 8; i++) { ClipTriangle tri = new ClipTriangle(); tri.n1 = v[kTriangleVerts[i, 0]]; tri.n2 = v[kTriangleVerts[i, 1]]; tri.n3 = v[kTriangleVerts[i, 2]]; tri.generation = 0; activeTriList.Push(tri); } // surfaceTriList while (activeTriList.Count > 0) { ClipTriangle tri = activeTriList.Pop(); FPVector p1; SupportMapping(ref tri.n1, out p1); FPVector p2; SupportMapping(ref tri.n2, out p2); FPVector p3; SupportMapping(ref tri.n3, out p3); FP d1 = (p2 - p1).sqrMagnitude; FP d2 = (p3 - p2).sqrMagnitude; FP d3 = (p1 - p3).sqrMagnitude; if (FPMath.Max(FPMath.Max(d1, d2), d3) > distanceThreshold && tri.generation < generationThreshold) { ClipTriangle tri1 = new ClipTriangle(); ClipTriangle tri2 = new ClipTriangle(); ClipTriangle tri3 = new ClipTriangle(); ClipTriangle tri4 = new ClipTriangle(); tri1.generation = tri.generation + 1; tri2.generation = tri.generation + 1; tri3.generation = tri.generation + 1; tri4.generation = tri.generation + 1; tri1.n1 = tri.n1; tri2.n2 = tri.n2; tri3.n3 = tri.n3; FPVector n = FP.Half * (tri.n1 + tri.n2); n.Normalize(); tri1.n2 = n; tri2.n1 = n; tri4.n3 = n; n = FP.Half * (tri.n2 + tri.n3); n.Normalize(); tri2.n3 = n; tri3.n2 = n; tri4.n1 = n; n = FP.Half * (tri.n3 + tri.n1); n.Normalize(); tri1.n3 = n; tri3.n1 = n; tri4.n2 = n; activeTriList.Push(tri1); activeTriList.Push(tri2); activeTriList.Push(tri3); activeTriList.Push(tri4); } else { if (((p3 - p1) % (p2 - p1)).sqrMagnitude > FPMath.Epsilon) { triangleList.Add(p1); triangleList.Add(p2); triangleList.Add(p3); } } } }
// 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); }