コード:
typedef struct {
float Rot;
VECTOR Axis;
}Quaternion_t;
typedef struct {
int model;
VECTOR position;
Quaternion_t quaternion;
} my_t;
my_t my;
Quaternion_t CreateRotationQuaternion(float rot,VECTOR vector){
Quaternion_t quaternion;
quaternion.Axis=vector;
quaternion.Rot=rot;
return quaternion;
}
Quaternion_t SynthesisQuaternion(Quaternion_t q_main,Quaternion_t q_z,Quaternion_t q_x,Quaternion_t q_y){
//それぞれのQを作成
float rotaq_z=cos(q_z.Rot/2);
VECTOR vectq_z=VGet(q_z.Axis.x*sin(q_z.Rot/2),q_z.Axis.y*sin(q_z.Rot/2),q_z.Axis.z*sin(q_z.Rot/2));
float rotaq_x=cos(q_x.Rot/2);
VECTOR vectq_x=VGet(q_x.Axis.x*sin(q_x.Rot/2),q_x.Axis.y*sin(q_x.Rot/2),q_x.Axis.x*sin(q_x.Rot/2));
float rotaq_y=cos(q_y.Rot/2);
VECTOR vectq_y=VGet(q_y.Axis.y*sin(q_y.Rot/2),q_y.Axis.y*sin(q_y.Rot/2),q_y.Axis.y*sin(q_y.Rot/2));
//それぞれの共役Rを作成
float rotar_z=cos(q_z.Rot/2);
VECTOR vectr_z=VGet(-q_z.Axis.x*sin(q_z.Rot/2),-q_z.Axis.y*sin(q_z.Rot/2),-q_z.Axis.z*sin(q_z.Rot/2));
float rotar_x=cos(q_x.Rot/2);
VECTOR vectr_x=VGet(-q_x.Axis.x*sin(q_x.Rot/2),-q_x.Axis.y*sin(q_x.Rot/2),-q_x.Axis.x*sin(q_x.Rot/2));
float rotar_y=cos(q_y.Rot/2);
VECTOR vectr_y=VGet(-q_y.Axis.y*sin(q_y.Rot/2),-q_y.Axis.y*sin(q_y.Rot/2),-q_y.Axis.y*sin(q_y.Rot/2));
//Qを統一する
float rotaq_t=rotaq_z*rotaq_x-VDot(vectq_z,vectq_x);
VECTOR vectq_t=VAdd(VAdd(VGet(rotaq_z*vectq_x.x,rotaq_z*vectq_x.y,rotaq_z*vectq_x.z),VGet(rotaq_x*vectq_z.x,rotaq_x*vectq_z.y,rotaq_x*vectq_z.z)),VCross(vectq_z,vectq_x));
float rotaqall=rotaq_t*rotaq_y-VDot(vectq_t,vectq_y);
VECTOR vectqall=VAdd(VAdd(VGet(rotaq_t*vectq_y.x,rotaq_t*vectq_y.y,rotaq_t*vectq_y.z),VGet(rotaq_y*vectq_t.x,rotaq_y*vectq_t.y,rotaq_y*vectq_t.z)),VCross(vectq_t,vectq_y));
//まずはR共役四元数を統一する
float rotar_t=rotar_z*rotar_x-VDot(vectr_z,vectr_x);
VECTOR vectr_t=VAdd(VAdd(VGet(rotar_z*vectr_x.x,rotar_z*vectr_x.y,rotar_z*vectr_x.z),VGet(rotar_x*vectr_z.x,rotar_x*vectr_z.y,rotar_x*vectr_z.z)),VCross(vectr_z,vectr_x));
float rotarall=rotar_t*rotar_y-VDot(vectr_t,vectr_y);
VECTOR vectrall=VAdd(VAdd(VGet(rotar_t*vectr_y.x,rotar_t*vectr_y.y,rotar_t*vectr_y.z),VGet(rotar_y*vectr_t.x,rotar_y*vectr_t.y,rotar_y*vectr_t.z)),VCross(vectr_t,vectr_y));
//RとPの掛け算をしてCクォータニオンを合成
float rotac=rotarall*q_main.Rot-VDot(vectrall,q_main.Axis);
VECTOR vectc=VAdd(VAdd(VGet(rotarall*q_main.Axis.x,rotarall*q_main.Axis.y,rotarall*q_main.Axis.z),VGet(q_main.Rot*vectrall.x,q_main.Rot*vectrall.y,q_main.Rot*vectrall.z)),VCross(vectrall,q_main.Axis));
//CクォータニオンとQクォータニオンを合成してベクトルを返す
Quaternion_t result;
result.Rot = rotac*rotaqall-VDot(vectc,vectqall);
result.Axis = VAdd(VAdd(VGet(rotac*vectqall.x,rotac*vectqall.y,rotac*vectqall.z),VGet(rotaqall*vectc.x,rotaqall*vectc.y,rotaqall*vectc.z)),VCross(vectc,vectqall));;
return result;
}
Quaternion_t QuaternionMult(Quaternion_t Q,Quaternion_t R){
/*
Q = (q; V);
R = (r; W);
QR = (qr - V ・ W; qW + rV + V × W);
*/
Quaternion_t Return;
Return.Rot = Q.Rot*R.Rot - VDot(Q.Axis,R.Axis);
Return.Axis = VAdd(VAdd(VScale(R.Axis,Q.Rot),VScale(Q.Axis,R.Rot)),VCross(Q.Axis,R.Axis));
return Return;
}
void func(){
MATRIX mat = MGetIdent();
float roll =0;
float pitch =0;
float yaw =0;
if(1==CheckHitKey(KEY_INPUT_LEFT)){
roll+=DX_PI_F/180;
}
if(1==CheckHitKey(KEY_INPUT_RIGHT)){
roll-=DX_PI_F/180;
}
if(1==CheckHitKey(KEY_INPUT_DOWN)){
pitch+=DX_PI_F/180;
}
if(1==CheckHitKey(KEY_INPUT_UP)){
pitch-=DX_PI_F/180;
}
if(1==CheckHitKey(KEY_INPUT_A)){
yaw+=DX_PI_F/180;
}
if(1==CheckHitKey(KEY_INPUT_D)){
yaw-=DX_PI_F/180;
}
//ロール
VECTOR zAxis = VGet(mat.m[2][0], mat.m[2][1], mat.m[2][2]);
Quaternion_t QuaZ = CreateRotationQuaternion(roll,zAxis);
//ピッチ
VECTOR xAxis = VGet(mat.m[0][0], mat.m[0][1], mat.m[0][2]);
Quaternion_t QuaX = CreateRotationQuaternion(pitch,xAxis);
//ヨー
VECTOR yAxis = VGet(mat.m[1][0], mat.m[1][1], mat.m[1][2]);
Quaternion_t QuaY = CreateRotationQuaternion(yaw,yAxis);
// 機体の回転
Quaternion_t AllQuaternion = SynthesisQuaternion(my.quaternion,QuaZ,QuaX,QuaY);
MATRIX matRot = MGetRotAxis(AllQuaternion.Axis,AllQuaternion.Rot);
mat = MMult( mat, matRot );
// 機体の移動
MATRIX matTrans = MGetTranslate(my.position);
mat = MMult( mat, matTrans );
// 機体のマトリックス設定
MV1SetMatrix( my.model, mat );
///////////////////////////////////
// カメラを機体後方に移動
MATRIX matCam = MGetIdent();//単位行列
matCam = MMult(matCam,MGetTranslate(VGet(0.0f,0.0f,-1000.0f))); //機体のZ後方に。
// カメラの回転
matCam = MMult( matCam, matRot );
// カメラの平行移動
matTrans = MGetTranslate(my.position);
matCam = MMult( matCam, matTrans );
// カメラの処理
MATRIX imatCam = MInverse(matCam); //逆行列。
SetCameraViewMatrix(imatCam);
}
int WINAPI WinMain( HINSTANCE, HINSTANCE, LPSTR, int )
{
ChangeWindowMode( TRUE );
SetGraphMode(1280,720,32);
DxLib_Init();
SetDrawScreen( DX_SCREEN_BACK );
my.model = MV1LoadModel( "test.mqo" );
int stage = MV1LoadModel("NightSea.mv1");
MV1SetUseZBuffer(stage,false);
SetCameraNearFar( 100.0f, 1000000.0f ) ;
//初期の単位ベクトル角度?
my.quaternion.Axis = VNorm(VGet(0,0,1));
//回転は無し
my.quaternion.Rot = 0.0;
my.position = VGet( 0.0f, 0.0f, 600.0f );
while( ScreenFlip() == 0 && ProcessMessage() == 0 && ClearDrawScreen() == 0 ) {
func();
//SetCameraPositionAndTarget_UpVecY(VGet(0,0,0),my.position);
DrawFormatString(200,200,1231,"(%f,%f,%f)",my.quaternion.Axis.x,my.quaternion.Axis.y,my.quaternion.Axis.z);
MV1DrawModel(stage);
MV1DrawModel( my.model );
}
DxLib_End();
return 0;
}