/// <summary> /// Initializes a new instance of the WorldLineConstraint. /// </summary> /// <param name="body">The body of the constraint.</param> /// <param name="localAnchor">The anchor point on the body in local (body) /// coordinates.</param> /// <param name="lineDirection">The axis defining the line in world space.</param>/param> public PointOnLine(RigidBody body, JVector localAnchor, JVector lineDirection) : base(body, null) { if (lineDirection.LengthSquared() == 0.0f) throw new ArgumentException("Line direction can't be zero", "lineDirection"); localAnchor1 = localAnchor; this.anchor = body.position + JVector.Transform(localAnchor, body.orientation); this.lineNormal = lineDirection; this.lineNormal.Normalize(); }
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; }
// 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; }