C# JVector.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 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;
        }

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

//.........这里部分代码省略.........

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