C# FVector2.Normalize方法代码示例

        /// <summary>
        /// Triangulates a polygon using simple ear-clipping algorithm. Returns
        /// size of Triangle array unless the polygon can't be triangulated.
        /// This should only happen if the polygon self-intersects,
        /// though it will not _always_ return null for a bad polygon - it is the
        /// caller's responsibility to check for self-intersection, and if it
        /// doesn't, it should at least check that the return value is non-null
        /// before using. You're warned!
        ///
        /// Triangles may be degenerate, especially if you have identical points
        /// in the input to the algorithm.  Check this before you use them.
        ///
        /// This is totally unoptimized, so for large polygons it should not be part
        /// of the simulation loop.
        ///
        /// Warning: Only works on simple polygons.
        /// </summary>
        /// <returns></returns>
        public static List<Triangle> TriangulatePolygon(Vertices vertices)
        {
            List<Triangle> results = new List<Triangle>();
            if (vertices.Count < 3)
                return new List<Triangle>();

            //Recurse and split on pinch points
            Vertices pA, pB;
            Vertices pin = new Vertices(vertices);
            if (ResolvePinchPoint(pin, out pA, out pB))
            {
                List<Triangle> mergeA = TriangulatePolygon(pA);
                List<Triangle> mergeB = TriangulatePolygon(pB);

                if (mergeA.Count == -1 || mergeB.Count == -1)
                    throw new Exception("Can't triangulate your polygon.");

                for (int i = 0; i < mergeA.Count; ++i)
                {
                    results.Add(new Triangle(mergeA[i]));
                }
                for (int i = 0; i < mergeB.Count; ++i)
                {
                    results.Add(new Triangle(mergeB[i]));
                }

                return results;
            }

            Triangle[] buffer = new Triangle[vertices.Count - 2];
            int bufferSize = 0;
            float[] xrem = new float[vertices.Count];
            float[] yrem = new float[vertices.Count];
            for (int i = 0; i < vertices.Count; ++i)
            {
                xrem[i] = vertices[i].X;
                yrem[i] = vertices[i].Y;
            }

            int vNum = vertices.Count;

            while (vNum > 3)
            {
                // Find an ear
                int earIndex = -1;
                float earMaxMinCross = -10.0f;
                for (int i = 0; i < vNum; ++i)
                {
                    if (IsEar(i, xrem, yrem, vNum))
                    {
                        int lower = Remainder(i - 1, vNum);
                        int upper = Remainder(i + 1, vNum);
                        FVector2 d1 = new FVector2(xrem[upper] - xrem[i], yrem[upper] - yrem[i]);
                        FVector2 d2 = new FVector2(xrem[i] - xrem[lower], yrem[i] - yrem[lower]);
                        FVector2 d3 = new FVector2(xrem[lower] - xrem[upper], yrem[lower] - yrem[upper]);

                        d1.Normalize();
                        d2.Normalize();
                        d3.Normalize();
                        float cross12;
                        MathUtils.Cross(ref d1, ref d2, out cross12);
                        cross12 = Math.Abs(cross12);

                        float cross23;
                        MathUtils.Cross(ref d2, ref d3, out cross23);
                        cross23 = Math.Abs(cross23);

                        float cross31;
                        MathUtils.Cross(ref d3, ref d1, out cross31);
                        cross31 = Math.Abs(cross31);

                        //Find the maximum minimum angle
                        float minCross = Math.Min(cross12, Math.Min(cross23, cross31));
                        if (minCross > earMaxMinCross)
                        {
                            earIndex = i;
                            earMaxMinCross = minCross;
                        }
                    }
                }

                // If we still haven't found an ear, we're screwed.
//.........这里部分代码省略.........

        internal override void SolveVelocityConstraints(ref SolverData data)
        {
            FVector2 vA = data.velocities[m_indexA].v;
            float wA = data.velocities[m_indexA].w;
            FVector2 vB = data.velocities[m_indexB].v;
            float wB = data.velocities[m_indexB].w;

            float mA = m_invMassA, mB = m_invMassB;
            float iA = m_invIA, iB = m_invIB;

            float h = data.step.dt;

            // Solve angular friction
            {
                float Cdot = wB - wA;
                float impulse = -_angularMass * Cdot;

                float oldImpulse = _angularImpulse;
                float maxImpulse = h * MaxTorque;
                _angularImpulse = MathUtils.Clamp(_angularImpulse + impulse, -maxImpulse, maxImpulse);
                impulse = _angularImpulse - oldImpulse;

                wA -= iA * impulse;
                wB += iB * impulse;
            }

            // Solve linear friction
            {
                FVector2 Cdot = vB + MathUtils.Cross(wB, m_rB) - vA - MathUtils.Cross(wA, m_rA);

                FVector2 impulse = -MathUtils.Mul(ref _linearMass, Cdot);
                FVector2 oldImpulse = _linearImpulse;
                _linearImpulse += impulse;

                float maxImpulse = h * MaxForce;

                if (_linearImpulse.LengthSquared() > maxImpulse * maxImpulse)
                {
                    _linearImpulse.Normalize();
                    _linearImpulse *= maxImpulse;
                }

                impulse = _linearImpulse - oldImpulse;

                vA -= mA * impulse;
                wA -= iA * MathUtils.Cross(m_rA, impulse);

                vB += mB * impulse;
                wB += iB * MathUtils.Cross(m_rB, impulse);
            }

            data.velocities[m_indexA].v = vA;
            data.velocities[m_indexA].w = wA;
            data.velocities[m_indexB].v = vB;
            data.velocities[m_indexB].w = wB;
        }

        public static void Set(ref SimplexCache cache,
                               DistanceProxy proxyA, ref Sweep sweepA,
                               DistanceProxy proxyB, ref Sweep sweepB,
                               float t1)
        {
            _localPoint = FVector2.Zero;
            _proxyA = proxyA;
            _proxyB = proxyB;
            int count = cache.Count;
            Debug.Assert(0 < count && count < 3);

            _sweepA = sweepA;
            _sweepB = sweepB;

            Transform xfA, xfB;
            _sweepA.GetTransform(out xfA, t1);
            _sweepB.GetTransform(out xfB, t1);

            if (count == 1)
            {
                _type = SeparationFunctionType.Points;
                FVector2 localPointA = _proxyA.Vertices[cache.IndexA[0]];
                FVector2 localPointB = _proxyB.Vertices[cache.IndexB[0]];
                FVector2 pointA = MathUtils.Mul(ref xfA, localPointA);
                FVector2 pointB = MathUtils.Mul(ref xfB, localPointB);
                _axis = pointB - pointA;
                _axis.Normalize();
                return;
            }
            else if (cache.IndexA[0] == cache.IndexA[1])
            {
                // Two points on B and one on A.
                _type = SeparationFunctionType.FaceB;
                FVector2 localPointB1 = proxyB.Vertices[cache.IndexB[0]];
                FVector2 localPointB2 = proxyB.Vertices[cache.IndexB[1]];

                FVector2 a = localPointB2 - localPointB1;
                _axis = new FVector2(a.Y, -a.X);
                _axis.Normalize();
                FVector2 normal = MathUtils.Mul(ref xfB.q, _axis);

                _localPoint = 0.5f * (localPointB1 + localPointB2);
                FVector2 pointB = MathUtils.Mul(ref xfB, _localPoint);

                FVector2 localPointA = proxyA.Vertices[cache.IndexA[0]];
                FVector2 pointA = MathUtils.Mul(ref xfA, localPointA);

                float s = FVector2.Dot(pointA - pointB, normal);
                if (s < 0.0f)
                {
                    _axis = -_axis;
                    s = -s;
                }
                return;
            }
            else
            {
                // Two points on A and one or two points on B.
                _type = SeparationFunctionType.FaceA;
                FVector2 localPointA1 = _proxyA.Vertices[cache.IndexA[0]];
                FVector2 localPointA2 = _proxyA.Vertices[cache.IndexA[1]];

                FVector2 a = localPointA2 - localPointA1;
                _axis = new FVector2(a.Y, -a.X);
                _axis.Normalize();
                FVector2 normal = MathUtils.Mul(ref xfA.q, _axis);

                _localPoint = 0.5f * (localPointA1 + localPointA2);
                FVector2 pointA = MathUtils.Mul(ref xfA, _localPoint);

                FVector2 localPointB = _proxyB.Vertices[cache.IndexB[0]];
                FVector2 pointB = MathUtils.Mul(ref xfB, localPointB);

                float s = FVector2.Dot(pointB - pointA, normal);
                if (s < 0.0f)
                {
                    _axis = -_axis;
                    s = -s;
                }
                return;
            }
        }

            public static void Initialize(ContactPositionConstraint pc, Transform xfA, Transform xfB, int index, out FVector2 normal, out FVector2 point, out float separation)
            {
                Debug.Assert(pc.pointCount > 0);

                switch (pc.type)
                {
                    case ManifoldType.Circles:
                        {
                            FVector2 pointA = MathUtils.Mul(ref xfA, pc.localPoint);
                            FVector2 pointB = MathUtils.Mul(ref xfB, pc.localPoints[0]);
                            normal = pointB - pointA;
                            normal.Normalize();
                            point = 0.5f * (pointA + pointB);
                            separation = FVector2.Dot(pointB - pointA, normal) - pc.radiusA - pc.radiusB;
                        }
                        break;

                    case ManifoldType.FaceA:
                        {
                            normal = MathUtils.Mul(xfA.q, pc.localNormal);
                            FVector2 planePoint = MathUtils.Mul(ref xfA, pc.localPoint);

                            FVector2 clipPoint = MathUtils.Mul(ref xfB, pc.localPoints[index]);
                            separation = FVector2.Dot(clipPoint - planePoint, normal) - pc.radiusA - pc.radiusB;
                            point = clipPoint;
                        }
                        break;

                    case ManifoldType.FaceB:
                        {
                            normal = MathUtils.Mul(xfB.q, pc.localNormal);
                            FVector2 planePoint = MathUtils.Mul(ref xfB, pc.localPoint);

                            FVector2 clipPoint = MathUtils.Mul(ref xfA, pc.localPoints[index]);
                            separation = FVector2.Dot(clipPoint - planePoint, normal) - pc.radiusA - pc.radiusB;
                            point = clipPoint;

                            // Ensure normal points from A to B
                            normal = -normal;
                        }
                        break;
                    default:
                        normal = FVector2.Zero;
                        point = FVector2.Zero;
                        separation = 0;
                        break;

                }
            }

        /// <summary>
        /// This requires defining a line of
        /// motion using an axis and an anchor point. The definition uses local
        /// anchor points and a local axis so that the initial configuration
        /// can violate the constraint slightly. The joint translation is zero
        /// when the local anchor points coincide in world space. Using local
        /// anchors and a local axis helps when saving and loading a game.
        /// </summary>
        /// <param name="bodyA">The first body.</param>
        /// <param name="bodyB">The second body.</param>
        /// <param name="localAnchorA">The first body anchor.</param>
        /// <param name="localAnchorB">The second body anchor.</param>
        /// <param name="axis">The axis.</param>
        public PrismaticJoint(Body bodyA, Body bodyB, FVector2 localAnchorA, FVector2 localAnchorB, FVector2 axis)
            : base(bodyA, bodyB)
        {
            JointType = JointType.Prismatic;

            LocalAnchorA = localAnchorA;
            LocalAnchorB = localAnchorB;

            _localXAxisA = BodyA.GetLocalVector(axis);
            _localXAxisA.Normalize();
            _localYAxisA = MathUtils.Cross(1.0f, _localXAxisA);
            m_referenceAngle = BodyB.Rotation - BodyA.Rotation;

            _limitState = LimitState.Inactive;
        }

        /// <summary>
        /// Cast a ray against a child shape.
        /// </summary>
        /// <param name="output">The ray-cast results.</param>
        /// <param name="input">The ray-cast input parameters.</param>
        /// <param name="transform">The transform to be applied to the shape.</param>
        /// <param name="childIndex">The child shape index.</param>
        /// <returns>True if the ray-cast hits the shape</returns>
        public override bool RayCast(out RayCastOutput output, ref RayCastInput input,
            ref Transform transform, int childIndex)
        {
            // p = p1 + t * d
            // v = v1 + s * e
            // p1 + t * d = v1 + s * e
            // s * e - t * d = p1 - v1

            output = new RayCastOutput();

            // Put the ray into the edge's frame of reference.
            FVector2 p1 = MathUtils.MulT(transform.q, input.Point1 - transform.p);
            FVector2 p2 = MathUtils.MulT(transform.q, input.Point2 - transform.p);
            FVector2 d = p2 - p1;

            FVector2 v1 = _vertex1;
            FVector2 v2 = _vertex2;
            FVector2 e = v2 - v1;
            FVector2 normal = new FVector2(e.Y, -e.X);
            normal.Normalize();

            // q = p1 + t * d
            // dot(normal, q - v1) = 0
            // dot(normal, p1 - v1) + t * dot(normal, d) = 0
            float numerator = FVector2.Dot(normal, v1 - p1);
            float denominator = FVector2.Dot(normal, d);

            if (denominator == 0.0f)
            {
                return false;
            }

            float t = numerator / denominator;
            if (t < 0.0f || input.MaxFraction < t)
            {
                return false;
            }

            FVector2 q = p1 + t * d;

            // q = v1 + s * r
            // s = dot(q - v1, r) / dot(r, r)
            FVector2 r = v2 - v1;
            float rr = FVector2.Dot(r, r);
            if (rr == 0.0f)
            {
                return false;
            }

            float s = FVector2.Dot(q - v1, r) / rr;
            if (s < 0.0f || 1.0f < s)
            {
                return false;
            }

            output.Fraction = t;
            if (numerator > 0.0f)
            {
                output.Normal = -normal;
            }
            else
            {
                output.Normal = normal;
            }
            return true;
        }

        /// <summary>
        /// Copy vertices. This assumes the vertices define a convex polygon.
        /// It is assumed that the exterior is the the right of each edge.
        /// </summary>
        /// @warning the points may be re-ordered, even if they form a convex polygon
        /// @warning collinear points are handled but not removed. Collinear points
        /// may lead to poor stacking behavior.
        /// <param name="input">The vertices.</param>
        public void Set(Vertices input)
        {
            Debug.Assert(input.Count >= 3 && input.Count <= Settings.MaxPolygonVertices);

            //TODO: Uncomment and remove the other line
            //Vertices = GiftWrap.GetConvexHull(input);
            Vertices = new Vertices(input);
            Normals = new Vertices(Vertices.Count);

            // Compute normals. Ensure the edges have non-zero length.
            for (int i = 0; i < Vertices.Count; ++i)
            {
                int i1 = i;
                int i2 = i + 1 < Vertices.Count ? i + 1 : 0;
                FVector2 edge = Vertices[i2] - Vertices[i1];
                Debug.Assert(edge.LengthSquared() > Settings.Epsilon * Settings.Epsilon);

                FVector2 temp = new FVector2(edge.Y, -edge.X);
                temp.Normalize();
                Normals.Add(temp);
            }

            // Compute the polygon mass data
            ComputeProperties();
        }