C# Vector3.Cross方法代码示例

        public static Vector3 operator *(Quaternion xform, Vector3 vec)
        {
            /* From nVidia SDK */
            Vector3 uv, uuv;
            Vector3 qvec = new Vector3(xform.X, xform.Y, xform.Z);

            uv = qvec.Cross(vec);
            uuv = qvec.Cross(uv);
            uv *= (2.0f * xform.W);
            uuv *= 2.0f;

            return vec + uv + uuv;
        }

        public static void CreateBillboard(ref Vector3 objectPosition, ref Vector3 cameraPosition,
            ref Vector3 cameraUpVector, Vector3 cameraForwardVector, out Matrix4 result)
        {
            Vector3 look = cameraPosition - objectPosition;
            look = look.Normalize();

            Vector3 right = cameraUpVector.Cross(look).Normalize();
            Vector3 up = look.Cross(right).Normalize();
            //Matrix4 mat = Matrix4.LookAt(cameraPosition, cameraForwardVector, cameraUpVector);
            //Vector3 right = new Vector3(mat.M11, mat.M21, mat.M31);
            //right = right.Normalize();

            //Vector3 up = new Vector3(mat.M12, mat.M22, mat.M32);
            //up = up.Normalize();

            result.M11 = right.X;
            result.M12 = right.Y;
            result.M13 = right.Z;
            result.M14 = 0;
            result.M21 = up.X;
            result.M22 = up.Y;
            result.M23 = up.Z;
            result.M24 = 0;
            result.M31 = look.X;
            result.M32 = look.Y;
            result.M33 = look.Z;
            result.M34 = 0;
            result.M41 = objectPosition.X;
            result.M42 = objectPosition.Y;
            result.M43 = objectPosition.Z;
            result.M44 = 1;
        }

        public static void Test_Vector3_Generic_Cross_Product_Against_Unity3D(float a, float b, float c, float d, float e, float f)
        {
            //arrange
            Vector3<float> genericVec3One = new Vector3<float>(a, b, c);
            Vector3<float> genericVec3Two = new Vector3<float>(d, e, f);

            UnityEngine.Vector3 unityVec3One = new UnityEngine.Vector3(a, b, c);
            UnityEngine.Vector3 unityVec3Two = new UnityEngine.Vector3(d, e, f);

            //act
            Vector3<float> genericVec3Cross = genericVec3One.Cross(genericVec3Two);
            UnityEngine.Vector3 unityVec3Cross = UnityEngine.Vector3.Cross(unityVec3One, unityVec3Two);

            Vector3<float> manualCross = new Vector3<float>(genericVec3One.y * genericVec3Two.z - genericVec3One.z * genericVec3Two.y,
                genericVec3One.z * genericVec3Two.x - genericVec3One.x * genericVec3Two.z, genericVec3One.x * genericVec3Two.y - genericVec3One.y * genericVec3Two.x);

            double termOne = Operator<double>.Multiply((double)genericVec3One.y, (double)genericVec3Two.z);
            double termTwo = Operator<double>.Multiply((double)genericVec3One.z, (double)genericVec3Two.y);

            double termThree = Operator<double>.Multiply((double)genericVec3One.z, (double)genericVec3Two.x);
            double termFour = Operator<double>.Multiply((double)genericVec3One.x, (double)genericVec3Two.z);

            double termFive = Operator<double>.Multiply((double)genericVec3One.x, (double)genericVec3Two.y);
            double termSix = Operator<double>.Multiply((double)genericVec3One.y, (double)genericVec3Two.x);

            double newX = Operator<double>.Subtract((double)termOne, (double)termTwo);
            double newY = Operator<double>.Subtract((double)termThree, (double)termFour);
            double newZ = Operator<double>.Subtract((double)termFive, (double)termSix);

            Vector3<float> operatorGenericCross = new Vector3<float>((float)newX, (float)newY, (float)newZ);

            //assert
            for (int i = 0; i < 3; i++) //we have to accept a higher delta of error since cross product has so much float math.
            {
                Assert.AreEqual(operatorGenericCross[i], manualCross[i]);
                Assert.AreEqual(manualCross[i], unityVec3Cross[i]);
                Assert.AreEqual(unityVec3Cross[i], genericVec3Cross[i], UnityEngine.Vector3.kEpsilon * 5, "Index: {0} failed. Failed to compute cross product.", i);
            }
        }

        public bool RayIntersect(Vector3 ray, Vector3 rayStart)
        {
            if (Math.Abs(Normal.Normal.Dot(ray)) < 0.02) return false;
            if (-Normal.Distance(rayStart) <= 0) return false;

            var T = rayStart - Vertices[0];
            var edge = Vertices[1] - Vertices[0];
            for (var i = 2; i < Vertices.Count; i++)
            {
                var prev = edge;
                edge = Vertices[i] - Vertices[0];
                var P = ray.Cross(edge);
                var Q = T.Cross(prev);
                var t = P.Dot(prev);
                var u = P.Dot(T) / t;
                var v = Q.Dot(ray) / t;
                t = 1.0f - u - v;
                if (u.IsBetween(0.0f, 1.0f) && v.IsBetween(0.0f, 1.0f) && t.IsBetween(0.0f, 1.0f))
                    return true;
            }

            return false;
        }

            public void Rotate( ref Vector3 _Axis, double _Angle, out Vector3 _Out )
            {
                double	cos_ang = Math.Cos( _Angle );
                double	sin_ang = Math.Sin( _Angle );

                _Out.x = x * cos_ang;
                _Out.y = y * cos_ang;
                _Out.z = z * cos_ang;

                double	temp = Dot( ref _Axis );
                        temp *= 1.0-cos_ang;

                _Out.x += _Axis.x * temp;
                _Out.y += _Axis.y * temp;
                _Out.z += _Axis.z * temp;

                _Axis.Cross( ref this, out TempCross );

                _Out.x += TempCross.x * sin_ang;
                _Out.y += TempCross.y * sin_ang;
                _Out.z += TempCross.z * sin_ang;
            }

        public bool RayIntersect(Vector3 rayDir, Vector3 dot, ref float lambda)
        {
            var u = Plane.Normal.Dot(rayDir);
            if(Math.Abs(u) < 0.001)
            {
                return false; // plane is parallel to the ray - no intersection
            }
            lambda = -Plane.Distance(dot) / u;

            var vp = 0; // current polygon index
            var T = dot - Vertices[0].Position;

            var E2 = Vertices[1].Position - Vertices[0].Position;

            for (var i = 0; i < Vertices.Count - 2; i++, vp++)
            {
                var E1 = E2; // PREV
                E2 = Vertices[vp + 2].Position - Vertices[0].Position; // NEXT

                var P = rayDir.Cross(E2);
                var Q = T.Cross(E1);

                var tt = P.Dot(E1);
                u = P.Dot(T);
                u /= tt;
                var v = Q.Dot(rayDir);
                v /= tt;
                tt = 1.0f - u - v;
                if(u.IsBetween(0, 1) && v.IsBetween(0, 1) && tt.IsBetween(0, 1))
                {
                    return true;
                }
            }

            return false;
        }

        public void SetCameraDef(Vector3 p, Vector3 l, Vector3 u)
        {
            if (pl[0] == null) { ConstructPlanes(); }
            Vector3 nc, fc, X, Y, Z;

            // compute the Z axis of camera
            // this axis points in the opposite direction from
            // the looking direction
            Z = p - l;
            Z.Normalize();

            // X axis of camera with given "up" vector and Z axis

            //X = u * Z;
            X = u.Cross(Z);
            X.Normalize();

            // the real "up" vector is the cross product of Z and X
            //Y = Z * X;
            Y = Z.Cross(X);

            // compute the centers of the near and far planes
            nc = p - Z * NearD;
            fc = p - Z * FarD;

            // compute the 4 corners of the frustum on the near plane
            ntl = nc + Y * nh - X * nw;
            ntr = nc + Y * nh + X * nw;
            nbl = nc - Y * nh - X * nw;
            nbr = nc - Y * nh + X * nw;

            // compute the 4 corners of the frustum on the far plane
            ftl = fc + Y * fh - X * fw;
            ftr = fc + Y * fh + X * fw;
            fbl = fc - Y * fh - X * fw;
            fbr = fc - Y * fh + X * fw;

            // compute the six planes
            // the function set3Points assumes that the points
            // are given in counter clockwise order
            pl[(int)FrustumPlanes.TOP].Set3Points(ntr, ntl, ftl);
            pl[(int)FrustumPlanes.BOTTOM].Set3Points(nbl, nbr, fbr);
            pl[(int)FrustumPlanes.LEFT].Set3Points(ntl, nbl, fbl);
            pl[(int)FrustumPlanes.RIGHT].Set3Points(nbr, ntr, fbr);
            pl[(int)FrustumPlanes.NEARP].Set3Points(ntl, ntr, nbr);
            pl[(int)FrustumPlanes.FARP].Set3Points(ftr, ftl, fbl);

            //Now let's set what is needed for the frustum sphere
            Center = p + (-Z * (((FarD - NearD) * 0.5f) + NearD));

            //And some more stuff for a frustum cone
            ViewAxis = -Z;
            ViewPosition = p;
        }

        public override void Move()
        {
            if (m_gridFinder != null)
            {
                //m_logger.debugLog("updating grid finder", "Move()");
                m_gridFinder.Update();

                // if grid finder picks a new entity, have to set OrbitEntity to null first or altitude will be incorrect
                if (m_gridFinder.Grid == null || OrbitEntity != m_gridFinder.Grid)
                {
                    m_logger.debugLog("no grid found");
                    OrbitEntity = null;
                    m_mover.StopMove();
                    return;
                }
                if (OrbitEntity == null)
                {
                    m_logger.debugLog("found grid: " + m_gridFinder.Grid.Entity.DisplayName);
                    OrbitEntity = m_gridFinder.Grid.Entity;
                    CalcFakeOrbitSpeedForce();
                }
            }

            Vector3D targetCentre = OrbitEntity.GetCentre() + m_targetPositionOffset;

            m_faceDirection = Vector3.Reject(targetCentre - m_navBlock.WorldPosition, m_orbitAxis);
            float alt = m_faceDirection.Normalize();
            Vector3 orbitDirection = m_faceDirection.Cross(m_orbitAxis);
            Vector3D destination = targetCentre - m_faceDirection * Altitude;
            float speed = alt > Altitude ?
                Math.Max(1f, OrbitSpeed - alt + Altitude) :
                OrbitSpeed;

            m_mover.CalcMove(m_navBlock, destination, OrbitEntity.GetLinearVelocity() + orbitDirection * speed);
        }

 public void Assign(Vector3 v1, Vector3 v2, Vector3 pos)
 {
     Normal = v1.Cross(v2);
     Normal = Normal.SafeNormalize();
     Dot = Normal.Dot(pos);
 }

		public static Matrix3x3 GetRotationMatrix(Vector3 a, Vector3 b)
		{
			a = a.Normalize ();
			b = b.Normalize ();

			if (a != b && 
				a.Length () > 0 &&
				b.Length () > 0) 
			{
				Vector3 c = a.Cross (b);
				return new Matrix3x3 (
					a.x, b.x, c.x,
					a.y, b.y, c.y,
					a.z, b.z, c.z);

			} 
				
			return new Matrix3x3 ();
		}

        public void cross() {
            var a = new Vector3(3, -3, 1);
            var b = new Vector3(4, 9, 2);

            Assert.Equal(
                new Vector3(-15, -2, 39),
                a.Cross(b)
            );
            Assert.Equal(
                new Vector3(15, 2, -39),
                b.Cross(a)
            );
        }

		private static JacobianContact[] addFriction(
			SimulationObject objectA,
			SimulationObject objectB,
			SimulationParameters simulationParameters,
			int indexA,
			int indexB,
			Vector3 normal,
			Vector3 relativeVelocity,
			Vector3 ra,
			Vector3 rb,
			List<StartImpulseProperties> startImpulseProperties)
		{
			JacobianContact[] friction = new JacobianContact[2];

			var tx = new Vector3 ();
			var ty = new Vector3 ();

			GeometryUtilities.ComputeBasis(
				normal,
				ref tx,
				ref ty);

			double constraintLimit = 0.0;

            Vector3 tangentialVelocity = relativeVelocity -
                                         (normal.Dot(relativeVelocity)) *
                                         normal;

            #region Get start friction direction

            if (Vector3.Length (tangentialVelocity) >
				simulationParameters.ShiftToStaticFrictionTolerance) 
				constraintLimit = 0.5 * (objectA.DynamicFrictionCoeff + objectB.DynamicFrictionCoeff);
			else 
				constraintLimit = 0.5 * (objectA.StaticFrictionCoeff + objectB.StaticFrictionCoeff);
			
			#endregion

			#region Tangential Direction 1

			var linearComponentA = tx;
			var linearComponentB = -1.0 * linearComponentA;

			var angularComponentA = ra.Cross (linearComponentA);
			var angularComponentB = -1.0 * rb.Cross (linearComponentA);

			friction [0] = JacobianCommon.GetDOF (
				indexA,
				indexB,
				linearComponentA,
				linearComponentB,
				angularComponentA,
				angularComponentB,
				objectA,
				objectB,
				0.0,
				0.0,
				simulationParameters.FrictionCFM,
				constraintLimit,
				ConstraintType.Friction,
                	null,
				startImpulseProperties[1]);

			#endregion

			#region Tangential Direction 2

			linearComponentA = ty;
			linearComponentB = -1.0 * linearComponentA;

			angularComponentA = ra.Cross (linearComponentA);
			angularComponentB = -1.0 * rb.Cross (linearComponentA);

			friction [1] = JacobianCommon.GetDOF (
				indexA,
				indexB,
				linearComponentA,
				linearComponentB,
				angularComponentA,
				angularComponentB,
				objectA,
				objectB,
				0.0,
				0.0,
				simulationParameters.FrictionCFM,
				constraintLimit,
				ConstraintType.Friction,
                null,
				startImpulseProperties[2]);

			#endregion

			return friction;
		}