クォータニオンで指定した自機の回転から、追従するカメラの回転を指定したい

フォーラム(掲示板)ルール
フォーラム(掲示板)ルールはこちら　　※コードを貼り付ける場合は [code] と [/code] で囲って下さい。詳しくはこちら
閉鎖
珈琲

Re: クォータニオンで指定した自機の回転から、追従するカメラの回転を指定したい

#31

投稿記事 by 珈琲 » 12年前

作成してみましたが・・・

操作を全く受け付けません・・・


うーん・・・


今更ですが、こんなに計算式が多いとは思えませんし・・・

コード: 

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))); //機体のＺ後方に。
 
        //  カメラの回転
        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;
}
全然わかりません・・・

かといって、カメラをビュー行列で指定する必要がある以上、
行列で回転を管理するのは避けられませんし、
ジンバルロック対策も捨てられません

ISLeさんがおっしゃった
ひねり・ねじれ成分を加味すると、クォータニオンだけで飛行体の回転を表現するのは元々無理だということですよね

GRAMさんが書かれた
> そもそも機体姿勢の決定には最低6変数（座標および回転角度）が必要なのに対し、
> 最初のコードは変数が12個ありますから。（座標及び各軸方向つまり回転行列そのもの）
> もちろんクオータニオンのほうが変数の総量が少ないというのはありますが。（こっちなら合計7個）

ということは7+3成分で、10成分ということですよね
あんまり変わらなかったりするんですかね・・・
ページトップ
閉鎖

“C言語何でも質問掲示板” へ戻る