C# Quaternion.ToRotationMatrix方法代码示例

		/// <summary>
		/// Creates a translation Matrix
		/// </summary>
		public static Matrix4 Compose( Vector3 translation, Vector3 scale, Quaternion orientation )
		{
			// Ordering:
			//    1. Scale
			//    2. Rotate
			//    3. Translate

			Matrix3 rot3x3, scale3x3;
			rot3x3 = orientation.ToRotationMatrix();
			scale3x3 = Matrix3.Zero;
			scale3x3.m00 = scale.x;
			scale3x3.m11 = scale.y;
			scale3x3.m22 = scale.z;

			// Set up final matrix with scale, rotation and translation
			Matrix4 result = rot3x3 * scale3x3;
			result.Translation = translation;

			return result;
		}

		/// <summary>
		/// Internal method for building a Matrix4 from orientation / scale / position.
		/// </summary>
		/// <remarks>
		///	Transform is performed in the order scale, rotate, translation, i.e. translation is independent
		///	of orientation axes, scale does not affect size of translation, rotation and scaling are always
		///	centered on the origin.
		///	</remarks>
		protected void MakeTransform( Vector3 position, Vector3 scale, Quaternion orientation, ref Matrix4 destMatrix )
		{
			// Ordering:
			//    1. Scale
			//    2. Rotate
			//    3. Translate

			// Parent scaling is already applied to derived position
			// Own scale is applied before rotation
			Matrix3 rot3x3;
			Matrix3 scale3x3;
			rot3x3 = orientation.ToRotationMatrix();
			scale3x3 = Matrix3.Zero;
			scale3x3.m00 = scale.x;
			scale3x3.m11 = scale.y;
			scale3x3.m22 = scale.z;

			destMatrix = rot3x3 * scale3x3;
			destMatrix.Translation = position;
		}

        /// <summary>
        /// タイヤ位置計算
        /// </summary>
        /// <param name="timeTick"></param>
        public void calcTirePos(long timeTick)
        {
            long difMS = timeTick; //DateTime.Now.Millisecond - oldMS;
            //double moveRad = (wdCarAng + carHandleAng) * Math.PI / 180.0;

            double moveLength = ((double)((4 * 1000 * 1000) / 60 / 60) / 1000.0);      // 単位時間内の移動量 時速4Km計算

            // 時間辺りの移動量を求める
            moveLength = moveLength * -carAccVal * (double)difMS;

            oldMS = DateTime.Now.Millisecond;

            {
                Vector3 moveVec = new Vector3();
                Quaternion rotQt = new Quaternion();

                rotQt.RollInDegrees = wdCarAng + carHandleAng;
                moveVec.y = moveLength;
                moveVec = rotQt.ToRotationMatrix() * moveVec;

                // ハンドリングの影響を加えて、クルマの向きを求める
            #if true
                {
                    Vector3 carVec = new Vector3();
                    Vector3 movedcarVec = new Vector3();

                    // 車体のベクトル
                    Quaternion rotRQt = new Quaternion();
                    rotRQt.RollInDegrees = wdCarAng;
                    carVec.y = carHeight;
                    carVec = rotRQt.ToRotationMatrix() * carVec;

                    movedcarVec = carVec + moveVec;
                    double addRad = VecToRad(movedcarVec, carVec);
                    wdCarAng += addRad * 180.0 / Math.PI;
                }
            #else
            wdCarAng += carHandleAng;
            #endif
            }

            // クルマの向きに対する移動を求める
            {
                Quaternion rotQt = new Quaternion();
                Vector3 moveVec = new Vector3();

                rotQt.RollInDegrees = wdCarAng;
                moveVec.y = moveLength;
                moveVec = rotQt.ToRotationMatrix() * moveVec;

                // フロント中心軸を移動量分加算
                wdCarF += moveVec;

                // mkpへ反映
                {
                    mkp.X += moveVec.x;
                    mkp.Y += moveVec.y;
                    mkp.Theta = wdCarAng;
                }
                //Debug.WriteLine(wdCarF);

                // 差分ように更新前の値を保存
                wdRLOld = new Vector3(wdRL.x, wdRL.y, wdRL.z);
                wdRROld = new Vector3(wdRR.x, wdRR.y, wdRR.z);

                // 各車輪の位置座標計算
                Vector3 shaftVec = new Vector3();
                Vector3 wheelFRvec = new Vector3();
                Vector3 wheelFLvec = new Vector3();
                Vector3 wheelRvec = new Vector3();
                Vector3 wheelLvec = new Vector3();

                // 前輪位置 算出
                wheelFRvec.x = carWidthHf;
                wheelFLvec.x = -carWidthHf;

                wheelFRvec = rotQt.ToRotationMatrix() * wheelFRvec;
                wheelFLvec = rotQt.ToRotationMatrix() * wheelFLvec;

                wdFR = wheelFRvec + wdCarF;
                wdFL = wheelFLvec + wdCarF;

                // バック側　中心位置
                shaftVec.y = carHeight;
                shaftVec = rotQt.ToRotationMatrix() * shaftVec;
                shaftVec += wdCarF;

                wdCarR = shaftVec;

                // 後輪位置算出
                wheelRvec.y = carHeight;
                wheelRvec.x = carWidthHf;

                wheelLvec.y = carHeight;
                wheelLvec.x = -carWidthHf;

//.........这里部分代码省略.........

        /// <summary>
        /// クルマ初期化
        /// </summary>
        public void CarInit(double posx, double posy, double ang)
        {
            carHandleAng = 0.0;
            carAccVal = 0.0;

            wdCarAng = ang;

            wdCarF = new Vector3(posx, posy, 0.0);
            //wdCarR = new Vector3(posx, posy+carHeight, 0.0);
            {
                Vector3 carVec = new Vector3();

                // 車体のベクトル
                Quaternion rotRQt = new Quaternion();
                rotRQt.RollInDegrees = wdCarAng;
                carVec.y = carHeight;
                carVec = rotRQt.ToRotationMatrix() * carVec;

                wdCarR = new Vector3(carVec.x + posx, carVec.y + posy, carVec.z);
            }

            wdFL = new Vector3();
            wdFR = new Vector3();
            wdRL = new Vector3();
            wdRR = new Vector3();

            oldMS = DateTime.Now.Millisecond;

            calcTirePos(0);

            wdRLOld = new Vector3(wdRL.x, wdRL.y, wdRL.z);
            wdRROld = new Vector3(wdRR.x, wdRR.y, wdRR.z);

            wheelPulseR = 0.0;
            wheelPulseL = 0.0;
        }

        /// <summary>
        /// ホイールの移動量から
        /// ロータリーエンコーダ　回転パルス値を計算
        /// </summary>
        public void CalcWheelPosToREPulse()
        {
            const double WheelSize = 175;//172;    // ホイール直径
            const double OneRotValue = 240;   // １回転分の分解能

            Vector3 wheelLmov, wheelRmov;

            Real signL, signR;
            // 移動量と移動方向(+,-)を求める
            {
                Quaternion rotQt = new Quaternion();
                Vector3 moveVec = new Vector3();

                rotQt.RollInDegrees = wdCarAng;
                moveVec.y = 1.0;
                moveVec = rotQt.ToRotationMatrix() * moveVec;

                // 移動差分から、移動量を求める
                wheelLmov = new Vector3(wdRL.x - wdRLOld.x,
                                                 wdRL.y - wdRLOld.y,
                                                 wdRL.z - wdRLOld.z);

                wheelRmov = new Vector3(wdRR.x - wdRROld.x,
                                                 wdRR.y - wdRROld.y,
                                                 wdRR.z - wdRROld.z);

                if (moveVec.Dot(wheelLmov) > 0.0) signL = -1.0;
                else signL = 1.0;
                if (moveVec.Dot(wheelRmov) > 0.0) signR = -1.0;
                else signR = 1.0;
            }

            // 移動量(長さ) / ホイール１回転の長さ * １回転のパルス数
            wheelPulseL += (wheelLmov.Length / (WheelSize * Math.PI) * OneRotValue) * signL;
            wheelPulseR += (wheelRmov.Length / (WheelSize * Math.PI) * OneRotValue) * signR;
        }

		public static Matrix4 MakeViewMatrix( Vector3 position, Quaternion orientation, Matrix4 reflectMatrix )
		{
			Matrix4 viewMatrix;

			// View matrix is:
			//
			//  [ Lx  Uy  Dz  Tx  ]
			//  [ Lx  Uy  Dz  Ty  ]
			//  [ Lx  Uy  Dz  Tz  ]
			//  [ 0   0   0   1   ]
			//
			// Where T = -(Transposed(Rot) * Pos)

			// This is most efficiently done using 3x3 Matrices
			Matrix3 rot;
			rot = orientation.ToRotationMatrix();

			// Make the translation relative to new axes
			Matrix3 rotT = rot.Transpose();
			Vector3 trans = -rotT*position;

			// Make final matrix
			viewMatrix = Matrix4.Identity;
			viewMatrix = rotT; // fills upper 3x3
			viewMatrix[ 0, 3 ] = trans.x;
			viewMatrix[ 1, 3 ] = trans.y;
			viewMatrix[ 2, 3 ] = trans.z;

			// Deal with reflections
			if ( reflectMatrix != Matrix4.zeroMatrix )
			{
				viewMatrix = viewMatrix*( reflectMatrix );
			}

			return viewMatrix;
		}