/// <summary> /// Inverses the direction of a vector. /// </summary> /// <param name="value">The vector to inverse.</param> /// <returns>The negated vector.</returns> public static JVector Negate(JVector value) { JVector result; JVector.Negate(ref value, out result); return(result); }
//object locker = new object(); private void CollisionDetected(RigidBody body1, RigidBody body2, JVector point1, JVector point2, JVector normal, float penetration) { Arbiter arbiter; arbiterMap.LookUpArbiter(body1, body2, out arbiter); if (arbiter == null) { arbiter = Arbiter.Pool.GetNew(); arbiter.body1 = body1; arbiter.body2 = body2; arbiterMap.Add(new ArbiterKey(body1, body2), arbiter); addedArbiterQueue.Enqueue(arbiter); events.RaiseBodiesBeginCollide(body1, body2); } Contact contact; if (arbiter.body1 == body1) { JVector.Negate(ref normal, out normal); contact = arbiter.AddContact(point1, point2, normal, penetration, contactSettings); } else { contact = arbiter.AddContact(point2, point1, normal, penetration, contactSettings); } if (contact != null) events.RaiseContactCreated(contact); }
/// <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); }
/// <summary> /// Discrete Circle vs Circle test. Very fast. Generates contact info. /// NOTE: check distance for collisions. If negative then a collision has occurred. /// This is done to remove all branches from this test and leave it to the user to decide when to branch. /// </summary> public static void CircleCircleTest(JVector centerA, float radiusA, JVector centerB, float radiusB, out JVector pointA, out JVector pointB, out JVector normal, out float distance) { // ||A-B|| - (r1+r2) < 0 float d = JVector.DistanceSquared(centerA, centerB); float r = (radiusA + radiusB); r *= r; distance = d - r; normal = (centerA - centerB) / d; normal.Normalize(); // calculate closest 2 points pointA = JVector.Negate(normal) * radiusA + centerA; pointB = normal * radiusB + centerB; }
public static bool OriginInTriangle(JVector a, JVector b, JVector c) { float pab = JVector.Cross(JVector.Negate(a), b - a); float pbc = JVector.Cross(JVector.Negate(b), c - b); if (!SameSign(pab, pbc)) { return(false); } float pca = JVector.Cross(JVector.Negate(c), a - c); if (!SameSign(pab, pca)) { return(false); } return(true); }
private void FindSupportPoints(RigidBody body1, RigidBody body2, Shape shape1, Shape shape2, ref JVector point, ref JVector normal, out JVector point1, out JVector point2) { JVector mn; JVector.Negate(ref normal, out mn); JVector sA; SupportMapping(body1, shape1, ref mn, out sA); JVector sB; SupportMapping(body2, shape2, ref normal, out sB); JVector.Subtract(ref sA, ref point, out sA); JVector.Subtract(ref sB, ref point, out sB); float dot1 = JVector.Dot(ref sA, ref normal); float dot2 = JVector.Dot(ref sB, ref normal); JVector.Multiply(ref normal, dot1, out sA); JVector.Multiply(ref normal, dot2, out sB); JVector.Add(ref point, ref sA, out point1); JVector.Add(ref point, ref sB, out point2); }
internal static bool CircleCapsuleTest(JVector centerA, float radiusA, JVector centerB, JVector axis, float length, float radiusB, out JVector pointA, out JVector pointB, out JVector normal, out float distance) { // get capsule endpoints var p0 = centerB - axis * (length * 0.5f); var p1 = centerB + axis * (length * 0.5f); // get vector from endpoint to circle var D = centerA - p0; // project vector onto axis and clamp var d = JVector.Dot(D, axis); d = JMath.Clamp(d, 0, length); // get point on axis var R = p0 + axis * d; // distance var b = Math.Abs((centerA - R).Length()); normal = (centerA - R) / b; // calculate closest 2 points var RH = JVector.Normalize(centerA - R); pointA = JVector.Negate(RH) * radiusA + centerA; pointB = RH * radiusB + R; normal.Negate(); distance = b - (radiusA + radiusB); // if (b < radiusA + radiusB) { return(true); } return(false); }
/// <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; } } } } }
public override bool Raycast(RigidBody body, JVector rayOrigin, JVector rayDirection, out JVector normal, out float fraction) { fraction = float.MaxValue; normal = JVector.Zero; if (!body.BoundingBox.RayIntersect(rayOrigin, rayDirection)) { return(false); } if (body.Shape is Multishape multishape) { multishape = multishape.RequestWorkingClone(); bool multiShapeCollides = false; JVector.Subtract(rayOrigin, body.position, out var transformedOrigin); JVector.Transform(transformedOrigin, body.invOrientation, out transformedOrigin); JVector.Transform(rayDirection, body.invOrientation, out var transformedDirection); int msLength = multishape.Prepare(transformedOrigin, transformedDirection); for (int i = 0; i < msLength; i++) { multishape.SetCurrentShape(i); if (GJKCollide.Raycast( multishape, body.orientation, body.position, rayOrigin, rayDirection, out float tempFraction, out var tempNormal) && tempFraction < fraction) { if (useTerrainNormal && multishape is TerrainShape terrainShape) { terrainShape.CollisionNormal(out tempNormal); JVector.Transform(tempNormal, body.orientation, out tempNormal); tempNormal = JVector.Negate(tempNormal); } else if (useTriangleMeshNormal && multishape is TriangleMeshShape triangleMeshShape) { triangleMeshShape.CollisionNormal(out tempNormal); JVector.Transform(tempNormal, body.orientation, out tempNormal); tempNormal = JVector.Negate(tempNormal); } normal = tempNormal; fraction = tempFraction; multiShapeCollides = true; } } multishape.ReturnWorkingClone(); return(multiShapeCollides); } else { return(GJKCollide.Raycast( body.Shape, body.orientation, body.position, rayOrigin, rayDirection, out fraction, out normal)); } }
/* * private ResourcePool<List<int>> potentialTriangleLists = new ResourcePool<List<int>>(); * * private void DetectSoftSoft(SoftBody body1, SoftBody body2) * { * List<int> my = potentialTriangleLists.GetNew(); * List<int> other = potentialTriangleLists.GetNew(); * * body1.dynamicTree.Query(other, my, body2.dynamicTree); * * for (int i = 0; i < other.Count; i++) * { * SoftBody.Triangle myTriangle = body1.dynamicTree.GetUserData(my[i]); * SoftBody.Triangle otherTriangle = body2.dynamicTree.GetUserData(other[i]); * * JVector point, normal; * float penetration; * bool result; * * result = XenoCollide.Detect(myTriangle, otherTriangle, ref JMatrix.InternalIdentity, ref JMatrix.InternalIdentity, * ref JVector.InternalZero, ref JVector.InternalZero, out point, out normal, out penetration); * * if (result) * { * int minIndexMy = FindNearestTrianglePoint(body1, my[i], ref point); * int minIndexOther = FindNearestTrianglePoint(body2, other[i], ref point); * * if (this.RaisePassedNarrowphase(body1.points[minIndexMy], body2.points[minIndexOther], * ref point, ref normal, penetration)) * { * RaiseCollisionDetected(body1.points[minIndexMy], * body2.points[minIndexOther], ref point, ref point, ref normal, penetration); * } * } * } * * my.Clear(); other.Clear(); * potentialTriangleLists.GiveBack(my); * potentialTriangleLists.GiveBack(other); * } */ private void DetectRigidRigid(RigidBody body1, RigidBody body2) { // we don't support multi shapes yet! bool b1IsMulti = false; // (body1.Shape is Multishape); bool b2IsMulti = false; // (body2.Shape is Multishape); bool speculative = speculativeContacts || (body1.EnableSpeculativeContacts || body2.EnableSpeculativeContacts); JVector point = JVector.Zero; JVector normal = JVector.Zero; float penetration = 0; if (!b1IsMulti && !b2IsMulti) { JVector point1 = JVector.Zero; JVector point2 = JVector.Zero; JMatrix OA = JMatrix.CreateRotationZ(body1.orientation); JMatrix OB = JMatrix.CreateRotationZ(body2.orientation); JVector zero = JVector.Zero; float t = 0.0f; if (body1.Shape.type == ShapeType.Box && body2.Shape.type == ShapeType.Box) { var A = body1.Shape as BoxShape; var B = body2.Shape as BoxShape; A.UpdateAxes(body1.orientation); B.UpdateAxes(body2.orientation); JVector[] CA = new JVector[2], CB = new JVector[2]; int Cnum = 0; if (Collision.BoxBoxTestContact(ref A, ref body1.position, ref OA, ref B, ref body2.position, ref OB, out normal, out t, out CA, out CB, out Cnum)) { normal.Negate(); RaiseCollisionDetected(body1, body2, ref CA[0], ref CB[0], ref normal, -t); } } else if (body1.Shape.type == ShapeType.Circle && body2.Shape.type == ShapeType.Circle) { var A = body1.Shape as CircleShape; var B = body2.Shape as CircleShape; Collision.CircleCircleTest(body1.position, A.Radius, body2.position, B.Radius, out point1, out point2, out normal, out penetration); if (penetration < 0) { RaiseCollisionDetected(body1, body2, ref point1, ref point2, ref normal, -penetration); } } // all shapes (GJK) else if (speculative) { //JVector hit1, hit2; //if (GJKCollide.ClosestPoints(body1.Shape, body2.Shape, ref OA, ref OB, // ref body1.position, ref body2.position, out hit1, out hit2, out normal)) //{ // JVector delta = hit2 - hit1; // if (delta.LengthSquared() < (body1.sweptDirection - body2.sweptDirection).LengthSquared()) // { // //normal.Negate(); // penetration = delta * normal; // if (penetration < 0.0f) // { // RaiseCollisionDetected(body1, body2, ref hit1, ref hit2, ref normal, penetration); // } // } //} } } else if (b1IsMulti && b2IsMulti) { /* * Multishape ms1 = (body1.Shape as Multishape); * Multishape ms2 = (body2.Shape as Multishape); * * ms1 = ms1.RequestWorkingClone(); * ms2 = ms2.RequestWorkingClone(); * * JBBox transformedBoundingBox = body2.boundingBox; * transformedBoundingBox.InverseTransform(ref body1.position, ref body1.orientation); * * int ms1Length = ms1.Prepare(ref transformedBoundingBox); * * transformedBoundingBox = body1.boundingBox; * transformedBoundingBox.InverseTransform(ref body2.position, ref body2.orientation); * * int ms2Length = ms2.Prepare(ref transformedBoundingBox); * * if (ms1Length == 0 || ms2Length == 0) * { * ms1.ReturnWorkingClone(); * ms2.ReturnWorkingClone(); * return; * } * * for (int i = 0; i < ms1Length; i++) * { * ms1.SetCurrentShape(i); * * for (int e = 0; e < ms2Length; e++) * { * ms2.SetCurrentShape(e); * * if (XenoCollide.Detect(ms1, ms2, ref body1.orientation, * ref body2.orientation, ref body1.position, ref body2.position, * out point, out normal, out penetration)) * { * if (this.RaisePassedNarrowphase(body1, body2, ref point, ref normal, penetration)) * { * JVector point1, point2; * FindSupportPoints(body1, body2, ms1, ms2, ref point, ref normal, out point1, out point2); * * RaiseCollisionDetected(body1, body2, ref point1, ref point2, ref normal, penetration); * } * } * } * } * * ms1.ReturnWorkingClone(); * ms2.ReturnWorkingClone(); * */ } else { /* * RigidBody b1, b2; * * if (body2.Shape is Multishape) { b1 = body2; b2 = body1; } * else { b2 = body2; b1 = body1; } * * Multishape ms = (b1.Shape as Multishape); * * ms = ms.RequestWorkingClone(); * * JBBox transformedBoundingBox = b2.boundingBox; * transformedBoundingBox.InverseTransform(ref b1.position, ref b1.orientation); * * int msLength = ms.Prepare(ref transformedBoundingBox); * * if (msLength == 0) * { * ms.ReturnWorkingClone(); * return; * } * * for (int i = 0; i < msLength; i++) * { * ms.SetCurrentShape(i); * * if (XenoCollide.Detect(ms, b2.Shape, ref b1.orientation, * ref b2.orientation, ref b1.position, ref b2.position, * out point, out normal, out penetration)) * { * if (this.RaisePassedNarrowphase(b1, b2, ref point, ref normal, penetration)) * { * JVector point1, point2; * FindSupportPoints(b1, b2, ms, b2.Shape, ref point, ref normal, out point1, out point2); * * if (useTerrainNormal && ms is TerrainShape) * { * (ms as TerrainShape).CollisionNormal(out normal); * JVector.Transform(ref normal, ref b1.orientation, out normal); * } * else if (useTriangleMeshNormal && ms is TriangleMeshShape) * { * (ms as TriangleMeshShape).CollisionNormal(out normal); * JVector.Transform(ref normal, ref b1.orientation, out normal); * } * * RaiseCollisionDetected(b1, b2, ref point1, ref point2, ref normal, penetration); * } * } * } * * ms.ReturnWorkingClone(); * */ } }
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); }
/// <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 v01, v02, v0; JVector v11, v12, v1; JVector v21, v22, v2; JVector v31, v32, v3; JVector mn; // Initialization of the output point = normal = JVector.Zero; penetration = 0.0f; // 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); } // 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 normal = OutsidePortal(v1, v0); 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); //LD.Draw(Conversion.ToXNAVector2(v1), Conversion.ToXNAVector2(v0), Color.Blue); //LD.Draw(Conversion.ToXNAVector2(v2), Conversion.ToXNAVector2(v0), Color.Blue); if (JVector.Dot(ref v2, ref normal) <= 0.0f) { return(false); } // phase two: portal refinement int maxIterations = 0; while (true) { // find normal direction if (!IntersectPortal(v0, v2, v1)) { normal = InsidePortal(v2, v1); } else { // origin ray crosses the portal normal = OutsidePortal(v2, v1); } // obtain the next support point 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); //LD.Draw(Conversion.ToXNAVector2(v3), Conversion.ToXNAVector2(v0), Color.Green); if (JVector.Dot(v3, normal) <= 0) { JVector ab = v3 - v2; float t = -(JVector.Dot(v2, ab)) / (JVector.Dot(ab, ab)); normal = (v2 + (t * ab)); return(false); } // Portal lies on the outside edge of the Minkowski Hull. // Return contact information if (JVector.Dot((v3 - v2), normal) <= CollideEpsilon || ++maxIterations > MaximumIterations) { JVector ab = v2 - v1; float t = JVector.Dot(JVector.Negate(v1), ab); if (t <= 0.0f) { t = 0.0f; normal = v1; } else { float denom = JVector.Dot(ab, ab); if (t >= denom) { normal = v2; t = 1.0f; } else { t /= denom; normal = v1 + t * ab; } } float s = 1 - t; point = s * v11 + t * v21; var point2 = s * v12 + t * v22; // this causes a sq root = bad! penetration = normal.Length(); normal.Normalize(); return(true); } // if origin is inside (v1, v0, v3), refine portal if (OriginInTriangle(v0, v1, v3)) { v2 = v3; v21 = v31; v22 = v32; continue; } // if origin is inside (v3, v0, v2), refine portal else if (OriginInTriangle(v0, v2, v3)) { v1 = v3; v11 = v31; v12 = v32; continue; } return(false); } }