|
//#pragma comment(linker, "/SUBSYSTEM:WINDOWS /ENTRY:mainCRTStartup")
#include <GL/freeglut/freeglut.h>
#include <stdio.h>
#include <math.h>
#define WIDTH 320
#define HEIGHT 240
#define PAI 3.14159
struct Vector3f{
float x;
float y;
float z;
}vec3d;
//クォータニオン構造体
struct Quaternion{
float w;
float x;
float y;
float z;
}qua;
//マトリクス構造体
struct MATRIX {
union {
struct {
float _11, _12, _13, _14;
float _21, _22, _23, _24;
float _31, _32, _33, _34;
float _41, _42, _43, _44;
};
float mat_4x4[4][4];
float mat_16[16];
};
MATRIX(){//単位行列に初期化
for(int i=0;i<16;i++){
this->mat_16[i]=0;
}
this->_11=this->_22=this->_33=this->_44=1;
}
void PRINT(char* text){
printf("%s\n%f,%f,%f,%f\n%f,%f,%f,%f\n%f,%f,%f,%f\n%f,%f,%f,%f\n\n",
text,
this->_11,this->_21,this->_31,this->_41,
this->_12,this->_22,this->_32,this->_42,
this->_13,this->_23,this->_33,this->_43,
this->_14,this->_24,this->_34,this->_44);
}
};
//任意軸回転をクォータニオンにする
Quaternion RotateToQuaternion(Vector3f vAxis,float Angle)
{
Quaternion q;
float radian = (float)(Angle * (PAI / 180.0) / 2.0);
float s = sin(radian);
q.w=cos(radian);
q.x=vAxis.x*s;
q.y=vAxis.y*s;
q.z=vAxis.z*s;
return q;
}
//クォータニオンを回転行列にする
MATRIX QuaternionToMatrix(Quaternion q){
MATRIX ret;
float sx = q.x * q.x;
float sy = q.y * q.y;
float sz = q.z * q.z;
float cx = q.y * q.z;
float cy = q.x * q.z;
float cz = q.x * q.y;
float wx = q.w * q.x;
float wy = q.w * q.y;
float wz = q.w * q.z;
ret._11= 1.0f - 2.0f * (sy + sz);
ret._12= 2.0f * (cz + wz);
ret._13= 2.0f * (cy - wy);
ret._21= 2.0f * (cz - wz);
ret._22= 1.0f - 2.0f * (sx + sz);
ret._23= 2.0f * (cx + wx);
ret._31= 2.0f * (cy + wy);
ret._32= 2.0f * (cx - wx);
ret._33= 1.0f - 2.0f * (sx + sy);
ret._41= 0.0f;
ret._42= 0.0f;
ret._43= 0.0f;
return ret;
}
//回転行列をクォータニオンにする
Quaternion MatrixToQuaternion(MATRIX mat){
Quaternion q;
float s;
float tr = mat._11 + mat._22 + mat._33 + 1.0f;
if (tr >= 1.0f) {
s = 0.5f / sqrt(tr);
q.w= 0.25f / s;
q.x= (mat._23 - mat._32) * s;
q.y= (mat._31 - mat._13) * s;
q.z= (mat._12 - mat._21) * s;
return q;
}else{
float max;
if(mat._22 > mat._33){
max = mat._22;
}else{
max = mat._33;
}
if (max < mat._11) {
s = sqrt(mat._11 - (mat._22 + mat._33) + 1.0f);
float x = s * 0.5f;
s = 0.5f / s;
q.x= x;
q.y= (mat._12 + mat._21) * s;
q.z= (mat._31 + mat._13) * s;
q.w= (mat._23 - mat._32) * s;
return q;
}else if (max == mat._22) {
s = sqrt(mat._22 - (mat._33 + mat._11) + 1.0f);
float y = s * 0.5f;
s = 0.5f / s;
q.x= (mat._12 + mat._21) * s;
q.y= y;
q.z= (mat._23 + mat._32) * s;
q.w= (mat._31 - mat._13) * s;
return q;
}else{
s = sqrt(mat._33 - (mat._11 + mat._22) + 1.0f);
float z = s * 0.5f;
s = 0.5f / s;
q.x= (mat._31 + mat._13) * s;
q.y= (mat._23 + mat._32) * s;
q.z= z;
q.w= (mat._12 - mat._21) * s;
return q;
}
}
}
MATRIX model;
void display(void)
{
glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT);
glViewport(0, 0, WIDTH, HEIGHT);
glMatrixMode(GL_PROJECTION);
glLoadIdentity();
//視野角,アスペクト比(ウィンドウの幅/高さ),描画する範囲(最も近い距離,最も遠い距離)
gluPerspective(30.0, (double)WIDTH / (double)HEIGHT, 1.0, 1000.0);
glMatrixMode(GL_MODELVIEW);
glLoadIdentity();
glRotatef(36.8f,1.0f,0.0f,0.0f);
glGetFloatv(GL_MODELVIEW_MATRIX,&model.mat_16[0]);
model.PRINT("glRotatefと同じ結果になるか?");
Quaternion q2=MatrixToQuaternion(model);
printf("回転行列をクォータニオンに変換\nw:%f\nx:%f\ny:%f\nz:%f\n\n", q2.w, q2.x, q2.y,
q2.z);
getchar();
glutSwapBuffers();
}
void idle(void)
{
glutPostRedisplay();
}
void Init(){
glClearColor(0.3f, 0.3f, 0.3f, 1.0f);
glEnable(GL_DEPTH_TEST);
glEnable(GL_LIGHTING);
glEnable(GL_LIGHT0);
vec3d.x=1.0f;
vec3d.y=0.0f;
vec3d.z=0.0f;
Quaternion q1=RotateToQuaternion(vec3d,36.8f);
printf("任意軸のベクトルと回転をクォータニオンに変換\nw:%f\nx:%f\ny:%f\nz:%f\n\n", q1.w, q1.x, q1.y,
q1.z);
MATRIX mat=QuaternionToMatrix(q1);
mat.PRINT("クォータニオンを回転行列に変換");
}
int main(int argc, char *argv[])
{
glutInitWindowPosition(100, 100);
glutInitWindowSize(WIDTH, HEIGHT);
glutInit(&argc, argv);
glutInitDisplayMode(GLUT_RGBA | GLUT_DOUBLE);
glutCreateWindow("クォータニオンの相互変換");
glutDisplayFunc(display);
glutIdleFunc(idle);
Init();
glutMainLoop();
return 0;
}
|
