C# Vec2.Normalize方法代码示例

		public void Set(Vec2 v1, Vec2 v2)
		{
			_v1 = v1;
			_v2 = v2;

			_direction = _v2 - _v1;
			_length = _direction.Normalize();
			_normal = Vec2.Cross(_direction, 1.0f);

			_cornerDir1 = _normal;
			_cornerDir2 = -1.0f * _normal;
		}

		public LineJoint()
		{
			Body ground = null;
			{
				PolygonDef sd = new PolygonDef();
				sd.SetAsBox(50.0f, 10.0f);

				BodyDef bd = new BodyDef();
				bd.Position.Set(0.0f, -10.0f);
				ground = _world.CreateBody(bd);
				ground.CreateShape(sd);
			}

			{
				PolygonDef sd = new PolygonDef();
				sd.SetAsBox(0.5f, 2.0f);
				sd.Density = 1.0f;


				BodyDef bd = new BodyDef();
				bd.Position.Set(0.0f, 7.0f);
				Body body = _world.CreateBody(bd);
				body.CreateShape(sd);
				body.SetMassFromShapes();

				LineJointDef jd = new LineJointDef();
				Vec2 axis = new Vec2(2.0f, 1.0f);
				axis.Normalize();
				jd.Initialize(ground, body, new Vec2(0.0f, 8.5f), axis);
				jd.motorSpeed = 0.0f;
				jd.maxMotorForce = 100.0f;
				jd.enableMotor = true;
				jd.lowerTranslation = -4.0f;
				jd.upperTranslation = 4.0f;
				jd.enableLimit = true;
				_world.CreateJoint(jd);
			}
		}

        /// 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(Manifold manifold, Transform xfA, float radiusA, Transform xfB, float radiusB)
        {
            if (manifold.PointCount == 0)
            {
                return;
            }

            switch (manifold.Type)
            {
                case Manifold.ManifoldType.Circles:
                    {
                        Vec2 pointA = Math.Mul(xfA, manifold.LocalPoint);
                        Vec2 pointB = Math.Mul(xfB, manifold.Points[0].LocalPoint);
                        Vec2 normal = new Vec2(1.0f, 0.0f);
                        if (Vec2.DistanceSquared(pointA, pointB) > Settings.FLT_EPSILON * Settings.FLT_EPSILON)
                        {
                            normal = pointB - pointA;
                            normal.Normalize();
                        }

                        Normal = normal;

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

                case Manifold.ManifoldType.FaceA:
                    {
                        Vec2 normal = Math.Mul(xfA.R, manifold.LocalPlaneNormal);
                        Vec2 planePoint = Math.Mul(xfA, manifold.LocalPoint);

                        // Ensure normal points from A to B.
                        Normal = normal;

                        for (int i = 0; i < manifold.PointCount; ++i)
                        {
                            Vec2 clipPoint = Math.Mul(xfB, manifold.Points[i].LocalPoint);
                            Vec2 cA = clipPoint + (radiusA - Vec2.Dot(clipPoint - planePoint, normal)) * normal;
                            Vec2 cB = clipPoint - radiusB * normal;
                            Points[i] = 0.5f * (cA + cB);
                        }
                    }
                    break;

                case Manifold.ManifoldType.FaceB:
                    {
                        Vec2 normal = Math.Mul(xfB.R, manifold.LocalPlaneNormal);
                        Vec2 planePoint = Math.Mul(xfB, manifold.LocalPoint);

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

                        for (int i = 0; i < manifold.PointCount; ++i)
                        {
                            Vec2 clipPoint = Math.Mul(xfA, manifold.Points[i].LocalPoint);
                            Vec2 cA = clipPoint - radiusA * normal;
                            Vec2 cB = clipPoint + (radiusB - Vec2.Dot(clipPoint - planePoint, normal)) * normal;
                            Points[i] = 0.5f * (cA + cB);
                        }
                    }
                    break;
            }
        }

        // Collision Detection in Interactive 3D Environments by Gino van den Bergen
        // From Section 3.1.2
        // x = s + a * r
        // norm(x) = radius
        public override SegmentCollide TestSegment(XForm transform, out float lambda, out Vec2 normal, Segment segment, float maxLambda)
        {
            lambda = 0f;
            normal = Vec2.Zero;

            Vec2 position = transform.Position + Common.Math.Mul(transform.R, _localPosition);
            Vec2 s = segment.P1 - position;
            float b = Vec2.Dot(s, s) - _radius * _radius;

            // Does the segment start inside the circle?
            if (b < 0.0f)
            {
                lambda = 0f;
                return SegmentCollide.StartInsideCollide;
            }

            // Solve quadratic equation.
            Vec2 r = segment.P2 - segment.P1;
            float c = Vec2.Dot(s, r);
            float rr = Vec2.Dot(r, r);
            float sigma = c * c - rr * b;

            // Check for negative discriminant and short segment.
            if (sigma < 0.0f || rr < Common.Settings.FLT_EPSILON)
            {
                return SegmentCollide.MissCollide;
            }

            // Find the point of intersection of the line with the circle.
            float a = -(c + Common.Math.Sqrt(sigma));

            // Is the intersection point on the segment?
            if (0.0f <= a && a <= maxLambda * rr)
            {
                a /= rr;
                lambda = a;
                normal = s + a * r;
                normal.Normalize();
                return SegmentCollide.HitCollide;
            }

            return SegmentCollide.MissCollide;
        }

 public override SegmentCollide TestSegment(XForm transform, out float lambda, out Vec2 normal, Segment segment, float maxLambda)
 {
     lambda = 0f;
     normal = Vec2.Zero;
     Vec2 v = transform.Position + Box2DX.Common.Math.Mul(transform.R, this._localPosition);
     Vec2 vec = segment.P1 - v;
     float num = Vec2.Dot(vec, vec) - this._radius * this._radius;
     SegmentCollide result;
     if (num < 0f)
     {
         lambda = 0f;
         result = SegmentCollide.StartInsideCollide;
     }
     else
     {
         Vec2 vec2 = segment.P2 - segment.P1;
         float num2 = Vec2.Dot(vec, vec2);
         float num3 = Vec2.Dot(vec2, vec2);
         float num4 = num2 * num2 - num3 * num;
         if (num4 < 0f || num3 < Settings.FLT_EPSILON)
         {
             result = SegmentCollide.MissCollide;
         }
         else
         {
             float num5 = -(num2 + Box2DX.Common.Math.Sqrt(num4));
             if (0f <= num5 && num5 <= maxLambda * num3)
             {
                 num5 /= num3;
                 lambda = num5;
                 normal = vec + num5 * vec2;
                 normal.Normalize();
                 result = SegmentCollide.HitCollide;
             }
             else
             {
                 result = SegmentCollide.MissCollide;
             }
         }
     }
     return result;
 }

			internal void Initialize(ContactConstraint cc)
			{
				Box2DXDebug.Assert(cc.PointCount > 0);

				switch (cc.Type)
				{
					case ManifoldType.Circles:
						{
							Vec2 pointA = cc.BodyA.GetWorldPoint(cc.LocalPoint);
							Vec2 pointB = cc.BodyB.GetWorldPoint(cc.Points[0].LocalPoint);
							if (Vec2.DistanceSquared(pointA, pointB) > Settings.FLT_EPSILON_SQUARED)
							{
								Normal = pointB - pointA;
								Normal.Normalize();
							}
							else
							{
								Normal.Set(1.0f, 0.0f);
							}

							Points[0] = 0.5f * (pointA + pointB);
							Separations[0] = Vec2.Dot(pointB - pointA, Normal) - cc.Radius;
						}
						break;

					case ManifoldType.FaceA:
						{
							Normal = cc.BodyA.GetWorldVector(cc.LocalPlaneNormal);
							Vec2 planePoint = cc.BodyA.GetWorldPoint(cc.LocalPoint);

							for (int i = 0; i < cc.PointCount; ++i)
							{
								Vec2 clipPoint = cc.BodyB.GetWorldPoint(cc.Points[i].LocalPoint);
								Separations[i] = Vec2.Dot(clipPoint - planePoint, Normal) - cc.Radius;
								Points[i] = clipPoint;
							}
						}
						break;

					case ManifoldType.FaceB:
						{
							Normal = cc.BodyB.GetWorldVector(cc.LocalPlaneNormal);
							Vec2 planePoint = cc.BodyB.GetWorldPoint(cc.LocalPoint);

							for (int i = 0; i < cc.PointCount; ++i)
							{
								Vec2 clipPoint = cc.BodyA.GetWorldPoint(cc.Points[i].LocalPoint);
								Separations[i] = Vec2.Dot(clipPoint - planePoint, Normal) - cc.Radius;
								Points[i] = clipPoint;
							}

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

		public override void Step(Settings settings)
		{
			base.Step(settings);

			float segmentLength = 30.0f;

			Segment segment;
			Vec2 laserStart = new Vec2(5.0f - 0.1f, 0.0f);
			Vec2 laserDir = new Vec2(segmentLength, 0.0f);
			segment.P1 = laserBody.GetWorldPoint(laserStart);
			segment.P2 = laserBody.GetWorldVector(laserDir);
			segment.P2 += segment.P1;

			for (int rebounds = 0; rebounds < 10; rebounds++)
			{

				float lambda = 1;
				Vec2 normal;
				Shape shape = _world.RaycastOne(segment, out lambda, out normal, false, null);

				Color laserColor = new Color(255, 0, 0);

				if (shape != null)
				{
					_debugDraw.DrawSegment(segment.P1, (1 - lambda) * segment.P1 + lambda * segment.P2, laserColor);
				}
				else
				{
					_debugDraw.DrawSegment(segment.P1, segment.P2, laserColor);
					break;
				}
				//Bounce
				segmentLength *= (1 - lambda);
				if (segmentLength <= Box2DX.Common.Settings.FLT_EPSILON)
					break;
				laserStart = (1 - lambda) * segment.P1 + lambda * segment.P2;
				laserDir = segment.P2 - segment.P1;
				laserDir.Normalize();
				laserDir = laserDir - 2 * Vec2.Dot(laserDir, normal) * normal;
				segment.P1 = laserStart - 0.1f * laserDir;
				segment.P2 = laserStart + segmentLength * laserDir;
			}
		}