/// <summary> /// Normalizes the given vector. /// </summary> /// <param name="value">The vector which should be normalized.</param> /// <returns>A normalized vector.</returns> #region public static JVector Normalize(JVector value) public static JVector Normalize(JVector value) { JVector result; JVector.Normalize(ref value, out result); return(result); }
public static bool ClosestPoints(ISupportMappable support1, ISupportMappable support2, ref JMatrix orientation1, ref JMatrix orientation2, ref JVector position1, ref JVector position2, out JVector p1, out JVector p2, out JVector normal) { VoronoiSimplexSolver simplexSolver = simplexSolverPool.GetNew(); simplexSolver.Reset(); p1 = p2 = JVector.Zero; JVector r = position1 - position2; JVector w, v; JVector supVertexA; JVector rn, vn; rn = JVector.Negate(r); SupportMapTransformed(support1, ref orientation1, ref position1, ref rn, out supVertexA); JVector supVertexB; SupportMapTransformed(support2, ref orientation2, ref position2, ref r, out supVertexB); v = supVertexA - supVertexB; normal = JVector.Zero; int iter = 0; float distSq = v.LengthSquared(); float epsilon = 0.00001f; while ((distSq > epsilon) && (iter++ < MaxIterations)) { IterationsTaken = iter; vn = JVector.Negate(v); SupportMapTransformed(support1, ref orientation1, ref position1, ref vn, out supVertexA); SupportMapTransformed(support2, ref orientation2, ref position2, ref v, out supVertexB); w = supVertexA - supVertexB; if (!simplexSolver.InSimplex(w)) simplexSolver.AddVertex(w, supVertexA, supVertexB); if (simplexSolver.Closest(out v)) { distSq = v.LengthSquared(); normal = v; } else distSq = 0.0f; } simplexSolver.ComputePoints(out p1, out p2); if (normal.LengthSquared() > JMath.Epsilon * JMath.Epsilon) normal.Normalize(); simplexSolverPool.GiveBack(simplexSolver); return true; }
internal static JVector Clamp(JVector v, float len) { return((JVector.Dot(v, v) > len * len) ? JVector.Multiply(JVector.Normalize(v), len) : v); }
/// <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 JVector point1, ref JVector point2, ref JVector n, float 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; JVector.Subtract(ref p1, ref body1.position, out relativePos1); JVector.Subtract(ref p2, ref body2.position, out relativePos2); JMatrix o1 = JMatrix.CreateRotationZ(body1.invOrientation); JMatrix o2 = JMatrix.CreateRotationZ(body2.invOrientation); JVector.Transform(ref relativePos1, ref o1, out realRelPos1); JVector.Transform(ref relativePos2, ref o2, 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 = 0.0f; accumulatedTangentImpulse = 0.0f; lostSpeculativeBounce = 0.0f; switch (settings.MaterialCoefficientMixing) { case ContactSettings.MaterialCoefficientMixingType.TakeMaximum: staticFriction = JMath.Max(body1.material.staticFriction, body2.material.staticFriction); dynamicFriction = JMath.Max(body1.material.kineticFriction, body2.material.kineticFriction); restitution = JMath.Max(body1.material.restitution, body2.material.restitution); break; case ContactSettings.MaterialCoefficientMixingType.TakeMinimum: staticFriction = JMath.Min(body1.material.staticFriction, body2.material.staticFriction); dynamicFriction = JMath.Min(body1.material.kineticFriction, body2.material.kineticFriction); restitution = JMath.Min(body1.material.restitution, body2.material.restitution); break; case ContactSettings.MaterialCoefficientMixingType.UseAverage: staticFriction = (body1.material.staticFriction + body2.material.staticFriction) / 2.0f; dynamicFriction = (body1.material.kineticFriction + body2.material.kineticFriction) / 2.0f; restitution = (body1.material.restitution + body2.material.restitution) / 2.0f; break; } } this.settings = settings; }
/// <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; }
/// <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; } }
/// <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 JVector direction, out JVector result) { result = direction; result.Normalize(); JVector.Multiply(ref result, radius, out result); }