C# JVector.Length方法代码示例

        /// <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;
            }
        }

        // 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;
        }

        public TraceResult Trace(JVector origin, JVector direction, TraceOptions options)
        {
            if (direction.Length() < 0.1f)
                return null;

            RaycastCallback callback = (b, n, f) =>
            {
                if ((b.IsStatic == true) && (options.HasFlag(TraceOptions.IgnoreStatic)))
                    return false;
                if ((b.IsStatic == false) && (options.HasFlag(TraceOptions.IgnoreDynamic)))
                    return false;
                return true;
            };
            RigidBody body;
            JVector normal;
            float friction;

            if (this.CollisionSystem.Raycast(
                origin,
                direction,
                callback,
                out body,
                out normal,
                out friction))
            {
                return new TraceResult(body, friction, origin + friction * direction, normal);
            }
            return null;
        }