C# Vector3.Sub方法代码示例

 /// <summary>
 /// Builds a ball socket joint.
 /// </summary>
 /// <param name="connectionA">First connection in the pair.</param>
 /// <param name="connectionB">Second connection in the pair.</param>
 /// <param name="anchor">World space anchor location used to initialize the local anchors.</param>
 public IKBallSocketJoint(Bone connectionA, Bone connectionB, Vector3 anchor)
     : base(connectionA, connectionB)
 {
     Vector3 tmp;
     anchor.Sub(ref ConnectionA.Position, out tmp);
     SetOffsetA(ref tmp);
     anchor.Sub(ref ConnectionB.Position, out tmp);
     SetOffsetB(ref tmp);
 }

        /// <summary>
        /// Constructs a new constraint which restricts three degrees of linear freedom and one degree of angular freedom between two entities.
        /// </summary>
        /// <param name="connectionA">First entity of the constraint pair.</param>
        /// <param name="connectionB">Second entity of the constraint pair.</param>
        /// <param name="anchor">Point around which both entities rotate.</param>
        /// <param name="hingeAxis">Axis of allowed rotation in world space to be attached to connectionA.  Will be kept perpendicular with the twist axis.</param>
        public SwivelHingeJoint(Entity connectionA, Entity connectionB, ref Vector3 anchor, ref Vector3 hingeAxis)
        {
            if (connectionA == null)
                connectionA = TwoEntityConstraint.WorldEntity;
            if (connectionB == null)
                connectionB = TwoEntityConstraint.WorldEntity;
            BallSocketJoint = new BallSocketJoint(connectionA, connectionB, ref anchor);
			Vector3 tmp;
			BallSocketJoint.OffsetB.Invert( out tmp );
            AngularJoint = new SwivelHingeAngularJoint(connectionA, connectionB, ref hingeAxis, ref tmp );
            HingeLimit = new RevoluteLimit(connectionA, connectionB);
            HingeMotor = new RevoluteMotor(connectionA, connectionB, hingeAxis);
            TwistLimit = new TwistLimit(connectionA, connectionB, ref BallSocketJoint.worldOffsetA, ref tmp, 0, 0);
            TwistMotor = new TwistMotor(connectionA, connectionB, ref BallSocketJoint.worldOffsetA, ref tmp );
            HingeLimit.IsActive = false;
            HingeMotor.IsActive = false;
            TwistLimit.IsActive = false;
            TwistMotor.IsActive = false;

			//Ensure that the base and test direction is perpendicular to the free axis.
			Vector3 baseAxis; anchor.Sub( ref connectionA.position, out baseAxis );
            if (baseAxis.LengthSquared() < Toolbox.BigEpsilon) //anchor and connection a in same spot, so try the other way.
                connectionB.position.Sub( ref anchor, out baseAxis );
            baseAxis.AddScaled( ref hingeAxis, -Vector3.Dot( ref baseAxis, ref hingeAxis) , out baseAxis );
            if (baseAxis.LengthSquared() < Toolbox.BigEpsilon)
            {
                //However, if the free axis is totally aligned (like in an axis constraint), pick another reasonable direction.
                Vector3.Cross(ref hingeAxis, ref Vector3.Up, out baseAxis);
                if (baseAxis.LengthSquared() < Toolbox.BigEpsilon)
                {
                    Vector3.Cross(ref hingeAxis, ref Vector3.Right, out baseAxis);
                }
            }
            HingeLimit.Basis.SetWorldAxes(ref hingeAxis, ref baseAxis, ref connectionA.orientationMatrix);
            HingeMotor.Basis.SetWorldAxes( ref hingeAxis, ref baseAxis, ref connectionA.orientationMatrix);

			connectionB.position.Sub( ref anchor, out baseAxis );
            baseAxis.AddScaled( ref hingeAxis, -Vector3.Dot(ref baseAxis, ref hingeAxis), out baseAxis );
            if (baseAxis.LengthSquared() < Toolbox.BigEpsilon)
            {
                //However, if the free axis is totally aligned (like in an axis constraint), pick another reasonable direction.
                Vector3.Cross(ref hingeAxis, ref Vector3.Up, out baseAxis);
                if (baseAxis.LengthSquared() < Toolbox.BigEpsilon)
                {
                    Vector3.Cross(ref hingeAxis, ref Vector3.Right, out baseAxis);
                }
            }
            HingeLimit.TestAxis = baseAxis;
            HingeMotor.TestAxis = baseAxis;


            Add(BallSocketJoint);
            Add(AngularJoint);
            Add(HingeLimit);
            Add(HingeMotor);
            Add(TwistLimit);
            Add(TwistMotor);
        }

        public Camera(Vector3 pos, Vector3 lookAt, Vector2 frustrumSize)
        {
            this.pos = pos;

            var dir = lookAt.Sub(ref pos);
            frustrumFrontPlane.topLeft.x = -frustrumSize.x / 2;
            frustrumFrontPlane.topLeft.y = frustrumSize.y / 2;
            frustrumFrontPlane.bottomRight.x = frustrumSize.x / 2;
            frustrumFrontPlane.bottomRight.y = -frustrumSize.y / 2;

            this.frustrumFrontPlane = new Plane3(
                topLeft: new Vector3(-frustrumSize.x / 2, frustrumSize.y / 2, 0),
                topRight: new Vector3(frustrumSize.x / 2, frustrumSize.y / 2, 0),
                bottomLeft: new Vector3(-frustrumSize.x / 2, -frustrumSize.y / 2, 0),
                bottomRight: new Vector3(frustrumSize.x / 2, -frustrumSize.y / 2, 0));
        }

        public static double getAngle(Vector3 j0, Vector3 j1, Vector3 j2)
        {
            Vector3 v1; //declaration of the two vectors used
            Vector3 v2;

            v1 = j0.Sub(j1);
            v2 = j2.Sub(j1);

            v1.Normalize();
            v2.Normalize();

            double dotProduct = Vector3.Dot(v1, v2); 	//get dot product

            Vector3 crossProduct;						//get cross product vector and length of cross product
            crossProduct = Vector3.Cross(v1, v2);
            double crossProductLength = crossProduct.Length();

            double radianAngle = Math.Atan2(crossProductLength, dotProduct); //get radian angle between vectors
            double degreeAngle = radianAngle * (180 / Math.PI); //get angle in degrees
            double threedecimal = Math.Round(degreeAngle, 3);  //get degree angle in 3 decimal point accuracy

            return threedecimal;
        }

 /// <summary>
 /// Gets or sets the offset in world space from the center of mass of connection B to the anchor point.
 /// </summary>
 public void SetAnchorB(ref Vector3 value)
 {
     Vector3 tmp;
     value.Sub(ref ConnectionB.Position, out tmp);
     Quaternion.Transform(ref tmp, ref ConnectionB.ConjOrientation, out LocalAnchorB);
 }

 /// <summary>
 /// Sets the world space location of the line anchor attached to connection A.
 /// </summary>
 public void SetLineAnchor(ref Vector3 value)
 {
     Vector3 tmp;
     value.Sub(ref ConnectionA.Position, out tmp);
     Quaternion.Transform(ref tmp, ref ConnectionA.ConjOrientation, out LocalLineAnchor);
 }

        public void OrthoNormalise(Vector3 u, Vector3 v, Vector3 w)
        {
            // If the input vectors are v0, v1, and v2, then the Gram-Schmidt
            // orthonormalization produces vectors u0, u1, and u2 as follows,
            //
            //   u0 = v0/|v0|
            //   u1 = (v1-(u0*v1)u0)/|v1-(u0*v1)u0|
            //   u2 = (v2-(u0*v2)u0-(u1*v2)u1)/|v2-(u0*v2)u0-(u1*v2)u1|
            //
            // where |A| indicates length of vector A and A*B indicates dot
            // product of vectors A and B.

            // compute u0
            u.Normalise();

            // compute u1
            double dot0 = u.Dot(v);
            v = v.Sub(u.Mult(dot0));
            v.Normalise();

            // compute u2
            double dot1 = v.Dot(w);
            dot0 = u.Dot(w);
            w = w.Sub(u.Mult(dot0).Add(v.Mult(dot1)));
            w.Normalise();
        }

        private bool HitTest(Vector3 origin, Vector3 direction, float maxDistance, float stepSize, out Vector3 hitPos, out float iterations)
        {
            Vector3 step = direction.Mul(stepSize);
            hitPos = origin.Add(step);
            iterations = 0;

            while (hitPos.Sub(ref origin).Len() < maxDistance)
            {
                iterations = fractal.HitTest(hitPos);
                if (iterations >= threshold)
                {
                    return true;
                }

                hitPos.AddAssign(ref step);
            }

            return false;
        }

        public void MakePerspectiveProjection(Vector3 normal, Vector3 point, Vector3 eye)
        {
            //	 +-												 -+
            // M = | Dot(N,E-P)*I - E*N^T	-(Dot(N,E-P)*I - E*N^T)*E |
            //	 |		-N^t					  Dot(N,E)		 |
            //	 +-												 -+
            //
            // where E is the eye point, P is a point on the plane, and N is a
            // unit-length plane normal.

            double emp = normal.Dot(eye.Sub(point));

            entry[ 0] = emp - eye.tuple[0] * normal.tuple[0];
            entry[ 1] = -eye.tuple[0] * normal.tuple[1];
            entry[ 2] = -eye.tuple[0] * normal.tuple[2];
            entry[ 3] = -(entry[0] * eye.tuple[0] + entry[1] * eye.tuple[1] + entry[2] * eye.tuple[2]);
            entry[ 4] = -eye.tuple[1] * normal.tuple[0];
            entry[ 5] = emp - eye.tuple[1] * normal.tuple[1];
            entry[ 6] = -eye.tuple[1] * normal.tuple[2];
            entry[ 7] = -(entry[4] * eye.tuple[0] + entry[5] * eye.tuple[1] + entry[6] * eye.tuple[2]);
            entry[ 8] = -eye.tuple[2] * normal.tuple[0];
            entry[ 9] = -eye.tuple[2] * normal.tuple[1];
            entry[10] = emp - eye.tuple[2] * normal.tuple[2];
            entry[11] = -(entry[8] * eye.tuple[0] + entry[9] * eye.tuple[1] + entry[10] * eye.tuple[2]);
            entry[12] = -normal.tuple[0];
            entry[13] = -normal.tuple[1];
            entry[14] = -normal.tuple[2];
            entry[15] = normal.Dot(eye);
        }