/// <summary> /// Creates an inverse translation Matrix /// </summary> /// <param name="translation"></param> /// <param name="scale"></param> /// <param name="orientation"></param> /// <returns></returns> public static Matrix4 ComposeInverse(Vector3 translation, Vector3 scale, Quaternion orientation) { // Invert the parameters Vector3 invTranslate = -translation; Vector3 invScale = new Vector3(1f / scale.x, 1f / scale.y, 1f / scale.z); Quaternion invRot = orientation.Inverse(); // Because we're inverting, order is translation, rotation, scale // So make translation relative to scale & rotation invTranslate *= invScale; // scale invTranslate = invRot * invTranslate; // rotate // Next, make a 3x3 rotation matrix and apply inverse scale Matrix3 rot3x3, scale3x3; rot3x3 = invRot.ToRotationMatrix(); scale3x3 = Matrix3.Zero; scale3x3.m00 = invScale.x; scale3x3.m11 = invScale.y; scale3x3.m22 = invScale.z; // Set up final matrix with scale, rotation and translation Matrix4 result = scale3x3 * rot3x3; result.Translation = invTranslate; return(result); }
/// <summary> /// Creates a translation Matrix /// </summary> /// <param name="position"></param> /// <param name="scale"></param> /// <param name="orientation"></param> /// <returns></returns> 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); }
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); }
/// <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; wheelRvec = rotQt.ToRotationMatrix() * wheelRvec; wheelLvec = rotQt.ToRotationMatrix() * wheelLvec; wdRR = wheelRvec + wdCarF; wdRL = wheelLvec + wdCarF; } // フロント軸位置とクルマの向きを元に、車輪の位置を求める // フロント車軸位置に(クルマの向き*carHeight)で、リア車軸位置 // フロント車軸位置にクルマの向き+90度の-+方向にフロント車輪がある // リア車軸位置にクルマの向き+90度の-+方向にリア車輪がある }
/// <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; // 1回転分の分解能 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; } // 移動量(長さ) / ホイール1回転の長さ * 1回転のパルス数 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; }