| public ToRotationMatrix ( ) : Axiom.Math.Matrix3 | ||
| return | Axiom.Math.Matrix3 | |
/// <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; // １回転分の分解能
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;
}