C# b2Vec2.Normalize方法代码示例

        public Prismatic()
        {
            b2Body ground = null;
            {
                b2BodyDef bd  = new b2BodyDef();
                ground = m_world.CreateBody(bd);

                b2EdgeShape shape = new b2EdgeShape();
                shape.Set(new b2Vec2(-40.0f, 0.0f), new b2Vec2(40.0f, 0.0f));
                ground.CreateFixture(shape, 0.0f);
            }

            {
                b2PolygonShape shape = new b2PolygonShape();
                shape.SetAsBox(2.0f, 0.5f);

                b2BodyDef bd  = new b2BodyDef();
                bd.type = b2BodyType.b2_dynamicBody;
                bd.position.Set(-10.0f, 10.0f);
                bd.angle = 0.5f * b2Settings.b2_pi;
                bd.allowSleep = false;
                b2Body body = m_world.CreateBody(bd);
                body.CreateFixture(shape, 5.0f);

                b2PrismaticJointDef pjd = new b2PrismaticJointDef();

                // Bouncy limit
                b2Vec2 axis = new b2Vec2(2.0f, 1.0f);
                axis.Normalize();
                pjd.Initialize(ground, body, new b2Vec2(0.0f, 0.0f), axis);

                // Non-bouncy limit
                //pjd.Initialize(ground, body, b2Vec2(-10.0f, 10.0f), b2Vec2(1.0f, 0.0f));

                pjd.motorSpeed = 10.0f;
                pjd.maxMotorForce = 10000.0f;
                pjd.enableMotor = true;
                pjd.lowerTranslation = 0.0f;
                pjd.upperTranslation = 20.0f;
                pjd.enableLimit = true;

                m_joint = (b2PrismaticJoint) m_world.CreateJoint(pjd);
            }
        }

		public void TouchesEnded(System.Collections.Generic.List<CCTouch> touches, CCEvent touchEvent)
		{
			//base.TouchesEnded(touches, touchEvent);

			foreach (var touch in touches)
			{
				var location = touch.LocationOnScreen; ; ; //??¿¿?¿?
				location.Y = Window.WindowSizeInPixels.Height - location.Y;
				if (slingShotNinjaInHand)
				{
					b2Vec2 direction = new b2Vec2(slingShotCenterPosition.X - location.X, slingShotCenterPosition.Y - location.Y);
					direction.Normalize();
					FireSlignshot(direction);
				}

				else if (continuePanningScreenOnFingerRelease)
				{

					if (panningTowardSling)
					{

						StartScreenPanToSling();

					}
					else
					{

						StartScreenPanToTargets();
					}

				}



			}
		}

            public b2PositionSolverManifold(ref b2ContactPositionConstraint pc, ref b2Transform xfA, ref b2Transform xfB, int index)
            {
                Debug.Assert(pc.pointCount > 0);

                switch (pc.type)
                {
                    case b2ManifoldType.e_circles:
                        {
                            b2Vec2 pointA = b2Math.b2Mul(xfA, pc.localPoint);
                            b2Vec2 pointB = b2Math.b2Mul(xfB, pc.localPoints[0]);
                            normal = pointB - pointA;
                            normal.Normalize();
                            point = 0.5f * (pointA + pointB);
                            separation = b2Math.b2Dot(pointB - pointA, normal) - pc.radiusA - pc.radiusB;
                        }
                        break;

                    case b2ManifoldType.e_faceA:
                        {
                            normal = b2Math.b2Mul(xfA.q, pc.localNormal);
                            b2Vec2 planePoint = b2Math.b2Mul(xfA, pc.localPoint);

                            b2Vec2 clipPoint = b2Math.b2Mul(xfB, pc.localPoints[index]);
                            separation = b2Math.b2Dot(clipPoint - planePoint, normal) - pc.radiusA - pc.radiusB;
                            point = clipPoint;
                        }
                        break;

                    case b2ManifoldType.e_faceB:
                        {
                            normal = b2Math.b2Mul(xfB.q, pc.localNormal);
                            b2Vec2 planePoint = b2Math.b2Mul(xfB, pc.localPoint);

                            b2Vec2 clipPoint = b2Math.b2Mul(xfA, pc.localPoints[index]);
                            separation = b2Math.b2Dot(clipPoint - planePoint, normal) - pc.radiusA - pc.radiusB;
                            point = clipPoint;

                            // Ensure normal points from A to B
                            normal = -normal;
                        }
                        break;
                }
            }

        public override void SolveVelocityConstraints(b2SolverData data)
        {
            b2Vec2 vA = m_bodyA.InternalVelocity.v;
            float wA = m_bodyA.InternalVelocity.w;
            b2Vec2 vB = m_bodyB.InternalVelocity.v;
            float wB = m_bodyB.InternalVelocity.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 = -m_angularMass * Cdot;

                float oldImpulse = m_angularImpulse;
                float maxImpulse = h * m_maxTorque;
                m_angularImpulse = b2Math.b2Clamp(m_angularImpulse + impulse, -maxImpulse, maxImpulse);
                impulse = m_angularImpulse - oldImpulse;

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

            // Solve linear friction
            {
                b2Vec2 Cdot = vB + b2Math.b2Cross(wB, ref m_rB) - vA - b2Math.b2Cross(wA, ref m_rA);

                b2Vec2 impulse = -b2Math.b2Mul(m_linearMass, Cdot);
                b2Vec2 oldImpulse = m_linearImpulse;
                m_linearImpulse += impulse;

                float maxImpulse = h * m_maxForce;

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

                impulse = m_linearImpulse - oldImpulse;

                vA -= mA * impulse;
                wA -= iA * b2Math.b2Cross(ref m_rA, ref impulse);

                vB += mB * impulse;
                wB += iB * b2Math.b2Cross(ref m_rB, ref impulse);
            }

            m_bodyA.InternalVelocity.v = vA;
            m_bodyA.InternalVelocity.w = wA;
            m_bodyB.InternalVelocity.v = vB;
            m_bodyB.InternalVelocity.w = wB;
        }

        // p = p1 + t * d
        // v = v1 + s * e
        // p1 + t * d = v1 + s * e
        // s * e - t * d = p1 - v1
        public override bool RayCast(out b2RayCastOutput output, b2RayCastInput input,
                                    b2Transform xf, int childIndex)
        {
            output = b2RayCastOutput.Zero;

            // Put the ray into the edge's frame of reference.
            b2Vec2 p1 = b2Math.b2MulT(xf.q, input.p1 - xf.p);
            b2Vec2 p2 = b2Math.b2MulT(xf.q, input.p2 - xf.p);
            b2Vec2 d = p2 - p1;

            b2Vec2 v1 = m_vertex1;
            b2Vec2 v2 = m_vertex2;
            b2Vec2 e = v2 - v1;
            b2Vec2 normal = new b2Vec2(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 = b2Math.b2Dot(normal, v1 - p1);
            float denominator = b2Math.b2Dot(normal, d);

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

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

            b2Vec2 q = p1 + t * d;

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

            float s = b2Math.b2Dot(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;
        }

        public b2PrismaticJoint(b2PrismaticJointDef def)
            : base(def)
        {
            m_localAnchorA = def.localAnchorA;
            m_localAnchorB = def.localAnchorB;
            m_localXAxisA = def.localAxisA;
            m_localXAxisA.Normalize();
            m_localYAxisA = m_localXAxisA.NegUnitCross(); //  b2Math.b2Cross(1.0f, m_localXAxisA);
            m_referenceAngle = def.referenceAngle;

            m_impulse.SetZero();
            m_motorMass = 0.0f;
            m_motorImpulse = 0.0f;

            m_lowerTranslation = def.lowerTranslation;
            m_upperTranslation = def.upperTranslation;
            m_maxMotorForce = def.maxMotorForce;
            m_motorSpeed = def.motorSpeed;
            m_enableLimit = def.enableLimit;
            m_enableMotor = def.enableMotor;
            m_limitState = b2LimitState.e_inactiveLimit;

            m_axis.SetZero();
            m_perp.SetZero();
        }

//.........这里部分代码省略.........
                b2Vec2 d = Q - P;
                float dd = b2Math.b2Dot(d, d);
                if (dd > radius * radius)
                {
                    return;
                }

                // Is there an edge connected to A?
                if (edgeA.HasVertex0)
                {
                    b2Vec2 A1 = edgeA.Vertex0;
                    b2Vec2 B1 = A;
                    b2Vec2 e1 = B1 - A1;
                    float u1 = b2Math.b2Dot(e1, B1 - Q);

                    // Is the circle in Region AB of the previous edge?
                    if (u1 > 0.0f)
                    {
                        return;
                    }
                }

                cf.indexA = 0;
                cf.typeA = b2ContactFeatureType.e_vertex;
                manifold.pointCount = 1;
                manifold.type = b2ManifoldType.e_circles;
                manifold.localNormal.SetZero();
                manifold.localPoint = P;
                manifold.points[0].id.key = 0;
                manifold.points[0].id.Set(cf);
                manifold.points[0].localPoint = circleB.Position;
                return;
            }

            // Region B
            if (u <= 0.0f)
            {
                b2Vec2 P = B;
                b2Vec2 d = Q - P;
                float dd = b2Math.b2Dot(d, d);
                if (dd > radius * radius)
                {
                    return;
                }

                // Is there an edge connected to B?
                if (edgeA.HasVertex3)
                {
                    b2Vec2 B2 = edgeA.Vertex3;
                    b2Vec2 A2 = B;
                    b2Vec2 e2 = B2 - A2;
                    float v2 = b2Math.b2Dot(e2, Q - A2);

                    // Is the circle in Region AB of the next edge?
                    if (v2 > 0.0f)
                    {
                        return;
                    }
                }

                cf.indexA = 1;
                cf.typeA = b2ContactFeatureType.e_vertex;
                manifold.pointCount = 1;
                manifold.type = b2ManifoldType.e_circles;
                manifold.localNormal.SetZero();
                manifold.localPoint = P;
                manifold.points[0].id.key = 0;
                manifold.points[0].id.Set(cf);
                manifold.points[0].localPoint = circleB.Position;
                return;
            }

            // Region AB
            float den = b2Math.b2Dot(e, e);
            System.Diagnostics.Debug.Assert(den > 0.0f);
            b2Vec2 xP = (1.0f / den) * (u * A + v * B);
            b2Vec2 xd = Q - xP;
            float xdd = b2Math.b2Dot(xd, xd);
            if (xdd > radius * radius)
            {
                return;
            }

            b2Vec2 n = new b2Vec2(-e.y, e.x);
            if (b2Math.b2Dot(n, Q - A) < 0.0f)
            {
                n.Set(-n.x, -n.y);
            }
            n.Normalize();

            cf.indexA = 0;
            cf.typeA = b2ContactFeatureType.e_face;
            manifold.pointCount = 1;
            manifold.type = b2ManifoldType.e_faceA;
            manifold.localNormal = n;
            manifold.localPoint = A;
            manifold.points[0].id.key = 0;
            manifold.points[0].id.Set(cf);
            manifold.points[0].localPoint = circleB.Position;
        }

        public b2Vec2[] points; //< world contact point (point of intersection)

        #endregion Fields

        #region Methods

        // Evaluate the manifold with supplied transforms. This assumes
        // modest motion from the original state. This does not change the
        // point count, impulses, etc. The radii must come from the shapes
        // that generated the manifold.
        public void Initialize(ref b2Manifold manifold,
                        b2Transform xfA, float radiusA,
                        b2Transform xfB, float radiusB)
        {
            points = new b2Vec2[b2Settings.b2_maxManifoldPoints];
            for (int p = 0; p < b2Settings.b2_maxManifoldPoints; p++)
                points[p] = b2Vec2.Zero;

            normal = b2Vec2.Zero;

            if (manifold.pointCount == 0)
            {
                return;
            }

            switch (manifold.type)
            {
            case b2ManifoldType.e_circles:
            {
                normal.Set(1.0f, 0.0f);
                b2Vec2 pointA = b2Math.b2Mul(xfA, manifold.localPoint);
                b2Vec2 pointB = b2Math.b2Mul(xfB, manifold.points[0].localPoint);
                if (b2Math.b2DistanceSquared(pointA, pointB) > b2Settings.b2_epsilonSqrd)
                {
                    normal = pointB - pointA;
                    normal.Normalize();
                }

                b2Vec2 cA = pointA + radiusA * normal;
                b2Vec2 cB = pointB - radiusB * normal;
                points[0] = 0.5f * (cA + cB);
            }
                break;

            case b2ManifoldType.e_faceA:
            {
                normal = b2Math.b2Mul(xfA.q, manifold.localNormal);
                b2Vec2 planePoint = b2Math.b2Mul(xfA, manifold.localPoint);

                for (int i = 0; i < manifold.pointCount; ++i)
                {
                    b2Vec2 clipPoint = b2Math.b2Mul(xfB, manifold.points[i].localPoint);
                    b2Vec2 cA = clipPoint + (radiusA - b2Math.b2Dot(clipPoint - planePoint, normal)) * normal;
                    b2Vec2 cB = clipPoint - radiusB * normal;
                    points[i] = 0.5f * (cA + cB);
                }
            }
                break;

            case b2ManifoldType.e_faceB:
            {
                normal = b2Math.b2Mul(xfB.q, manifold.localNormal);
                b2Vec2 planePoint = b2Math.b2Mul(xfB, manifold.localPoint);

                for (int i = 0; i < manifold.pointCount; ++i)
                {
                    b2Vec2 clipPoint = b2Math.b2Mul(xfA, manifold.points[i].localPoint);
                    b2Vec2 cB = clipPoint + (radiusB - b2Math.b2Dot(clipPoint - planePoint, normal)) * normal;
                    b2Vec2 cA = clipPoint - radiusA * normal;
                    points[i] = 0.5f * (cA + cB);
                }

                // Ensure normal points from A to B.
                normal = -normal;
            }
                break;
            }
        }

        // TODO_ERIN might not need to return the separation
        public float Initialize(ref b2SimplexCache cache,
		                        ref b2DistanceProxy proxyA, ref b2Sweep sweepA,
		                        ref b2DistanceProxy proxyB, ref b2Sweep sweepB,
		                        float t1)
        {
            m_proxyA = proxyA;
            m_proxyB = proxyB;
            int count = cache.count;
            Debug.Assert(0 < count && count < 3);

            m_sweepA = sweepA;
            m_sweepB = sweepB;

            b2Transform xfA = b2Transform.Default, xfB = b2Transform.Default;
            m_sweepA.GetTransform(ref xfA, t1);
            m_sweepB.GetTransform(ref xfB, t1);

            if (count == 1)
            {
                m_type = SeparationType.e_points;
                b2Vec2 localPointA = m_proxyA.GetVertex((int)cache.indexA[0]);
                b2Vec2 localPointB = m_proxyB.GetVertex((int)cache.indexB[0]);
                b2Vec2 pointA = b2Math.b2Mul(xfA, localPointA);
                b2Vec2 pointB = b2Math.b2Mul(xfB, localPointB);
                m_axis = pointB - pointA;
                float s = m_axis.Normalize();
                return s;
            }
            else if (cache.indexA[0] == cache.indexA[1])
            {
                // Two points on B and one on A.
                m_type = SeparationType.e_faceB;
                b2Vec2 localPointB1 = proxyB.GetVertex((int)cache.indexB[0]);
                b2Vec2 localPointB2 = proxyB.GetVertex((int)cache.indexB[1]);

                m_axis = b2Math.b2Cross(localPointB2 - localPointB1, 1.0f);
                m_axis.Normalize();
                b2Vec2 normal = b2Math.b2Mul(xfB.q, m_axis);

                m_localPoint = 0.5f * (localPointB1 + localPointB2);
                b2Vec2 pointB = b2Math.b2Mul(xfB, m_localPoint);

                b2Vec2 localPointA = proxyA.GetVertex((int)cache.indexA[0]);
                b2Vec2 pointA = b2Math.b2Mul(xfA, localPointA);

                float s = b2Math.b2Dot(pointA - pointB, normal);
                if (s < 0.0f)
                {
                    m_axis = -m_axis;
                    s = -s;
                }
                return s;
            }
            else
            {
                // Two points on A and one or two points on B.
                m_type = SeparationType.e_faceA;
                b2Vec2 localPointA1 = m_proxyA.GetVertex(cache.indexA[0]);
                b2Vec2 localPointA2 = m_proxyA.GetVertex(cache.indexA[1]);

                m_axis = b2Math.b2Cross(localPointA2 - localPointA1, 1.0f);
                m_axis.Normalize();
                b2Vec2 normal = b2Math.b2Mul(xfA.q, m_axis);

                m_localPoint = 0.5f * (localPointA1 + localPointA2);
                b2Vec2 pointA = b2Math.b2Mul(xfA, m_localPoint);

                b2Vec2 localPointB = m_proxyB.GetVertex(cache.indexB[0]);
                b2Vec2 pointB = b2Math.b2Mul(xfB, localPointB);

                float s = b2Math.b2Dot(pointB - pointA, normal);
                if (s < 0.0f)
                {
                    m_axis = -m_axis;
                    s = -s;
                }
                return s;
            }
        }

        public override void TouchesEnded(System.Collections.Generic.List<CCTouch> touches, CCEvent event_)
        {
            //base.TouchesEnded (touches, event_);
            foreach ( var touch in touches ) {
                var location = touch.LocationInView;
                location = CCDirector.SharedDirector.ConvertToGl(location);
                if (  slingShotNinjaInHand )
                {
                    b2Vec2 direction = new b2Vec2(slingShotCenterPosition.X - location.X, slingShotCenterPosition.Y - location.Y);
                    direction.Normalize();
                    FireSlignshot(direction);
                }

                else if (continuePanningScreenOnFingerRelease) {

                    if ( panningTowardSling) {

                        StartScreenPanToSling();

                    } else {

                        StartScreenPanToTargets();
                    }

                }

            }
        }