// 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; }
/// <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) { float sigma = (float)Math.Sqrt((float)(direction.X * direction.X + direction.Z * direction.Z)); if (direction.Y > direction.Length() * sina) { result.X = 0.0f; result.Y = (2.0f / 3.0f) * height; result.Z = 0.0f; } else if (sigma > 0.0f) { result.X = radius * direction.X / sigma; result.Y = -(1.0f / 3.0f) * height; result.Z = radius * direction.Z / sigma; } else { result.X = 0.0f; result.Y = -(1.0f / 3.0f) * height; result.Z = 0.0f; } }