libsegl/quaternion.cpp

78 lines
1.5 KiB
C++
Raw Normal View History

2008-02-09 13:43:23 +01:00
#include "quaternion.h"
Quaternion::Quaternion() {
w = 1.0f;
x = y = z = 0.0f;
}
void Quaternion::createFromRotation(float deg, float _x, float _y, float _z) {
float tmpres = sin(deg2rad(deg) / 2.0f);
w = cos(deg2rad(deg) / 2.0f);
x = _x * tmpres;
y = _y * tmpres;
z = _z * tmpres;
}
Quaternion Quaternion::operator*(const Quaternion &q) {
Quaternion ret;
ret.w = w*q.w - x*q.x - y*q.y - z*q.z;
ret.x = w*q.x + x*q.w + y*q.z - z*q.y;
ret.y = w*q.y + y*q.w + z*q.x - x*q.z;
ret.z = w*q.z + z*q.w + x*q.y - y*q.x;
return ret;
}
void Quaternion::createGlMatrix(GLfloat *matrix) {
if(!matrix)
return;
matrix[0] = 1.0f - 2.0f * ( y*y + z*z);
matrix[1] = 2.0f * ( x*y + z*w);
matrix[2] = 2.0f * ( x*z - y*w);
matrix[3] = 0.0f;
matrix[4] = 2.0f * ( x*y - z*w);
matrix[5] = 1.0f - 2.0f * ( x*x + z*z);
matrix[6] = 2.0f * ( z*y + x*w);
matrix[7] = 0.0f;
matrix[8] = 2.0f * ( x*z + y*w);
matrix[9] = 2.0f * ( y*z - x*w);
matrix[10] = 1.0f - 2.0f * ( x*x + y*y);
matrix[11] = 0.0f;
matrix[12] = 0.0f;
matrix[13] = 0.0f;
matrix[14] = 0.0f;
matrix[15] = 1.0f;
}
Punkt3D Quaternion::getDirectionVector() {
GLfloat matrix[16];
createGlMatrix(matrix);
return getDirectionVector(matrix);
}
Punkt3D Quaternion::getDirectionVector(GLfloat *matrix) {
if(!matrix)
return Punkt3D();
Punkt3D tmp;
tmp.x = matrix[8];
tmp.y = matrix[9];
tmp.z = matrix[10];
return tmp;
}
void glMultMatrixf(Quaternion q) {
GLfloat matrix[16];
q.createGlMatrix(matrix);
glMultMatrixf(matrix);
}