192 lines
4.3 KiB
C++
192 lines
4.3 KiB
C++
/* libsegl - Sebas Extended GL Library
|
|
* Collection of Opengl/3D-Math helpers
|
|
*
|
|
* Copyright (c) 2008 by Sebastian Lohff, seba@seba-geek.de
|
|
* http://www.seba-geek.de
|
|
*
|
|
* This library is free software; you can redistribute it and/or
|
|
* modify it under the terms of the GNU Library General Public
|
|
* License as published by the Free Software Foundation; either
|
|
* version 2 of the License, or (at your option) any later version.
|
|
*
|
|
* This library is distributed in the hope that it will be useful,
|
|
* but WITHOUT ANY WARRANTY; without even the implied warranty of
|
|
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
|
|
* Library General Public License for more details.
|
|
*
|
|
* You should have received a copy of the GNU Library General Public
|
|
* License along with this library; if not, write to the
|
|
* Free Software Foundation, Inc., 51 Franklin St, Fifth Floor,
|
|
* Boston, MA 02110-1301, USA.
|
|
*/
|
|
|
|
#include "quaternion.h"
|
|
|
|
namespace segl {
|
|
|
|
Quaternion::Quaternion() {
|
|
w = 1.0f,
|
|
x = y = z = 0.0f;
|
|
}
|
|
|
|
Quaternion::Quaternion(float _x, float _y, float _z, float _w) {
|
|
x = _x;
|
|
y = _y;
|
|
z = _z;
|
|
w = _w;
|
|
}
|
|
|
|
Quaternion::Quaternion(float deg, Punkt3D p) {
|
|
set(deg, p);
|
|
}
|
|
|
|
Quaternion::Quaternion(float _x, float _y, float _z) {
|
|
set(_x, _y, _z);
|
|
}
|
|
|
|
void Quaternion::set(float rotx, float roty, float rotz) {
|
|
rotx = deg2rad(rotx);
|
|
roty = deg2rad(roty);
|
|
rotz = deg2rad(rotz);
|
|
|
|
float sinx = sin(rotx);
|
|
float siny = sin(roty);
|
|
float sinz = sin(rotz);
|
|
float cosx = cos(rotx);
|
|
float cosy = cos(roty);
|
|
float cosz = cos(rotz);
|
|
|
|
x = sinz * cosx * cosy - cosz * sinx * siny;
|
|
y = cosz * sinx * cosy + sinz * cosx * siny;
|
|
z = cosz * cosx * siny - sinz * sinx * cosy;
|
|
w = cosz * cosx * cosy - sinz * sinx * siny;
|
|
|
|
normalize();
|
|
}
|
|
|
|
void Quaternion::set(float deg, Punkt3D p) {
|
|
float angle = deg2rad(0.5f*deg);
|
|
float sinangle = sin(angle);
|
|
|
|
x = p.x * sinangle;
|
|
y = p.y * sinangle;
|
|
z = p.z * sinangle;
|
|
w = cos(angle);
|
|
|
|
normalize();
|
|
}
|
|
|
|
void Quaternion::normalize() {
|
|
float len = x*x+y*y+z*z+w*w;
|
|
if(std::fabs(len-1.0f)>0.00001f) {
|
|
len = sqrt(len);
|
|
x /= len;
|
|
y /= len;
|
|
z /= len;
|
|
w /= len;
|
|
}
|
|
}
|
|
|
|
Quaternion Quaternion::getConjugate() {
|
|
return Quaternion(-x, -y, -z, w);
|
|
}
|
|
|
|
// Rotationsmatrix Quaternion::getRotMat() {
|
|
//
|
|
// }
|
|
|
|
Punkt3D Quaternion::rotP3D(const Punkt3D &p) {
|
|
Quaternion q(p.x, p.y, p.z, 0.0f);
|
|
|
|
Quaternion erg = *this * q * this->getConjugate();
|
|
|
|
return Punkt3D(erg.x, erg.y, erg.z);
|
|
}
|
|
|
|
Quaternion Quaternion::operator*(const Quaternion &q) {
|
|
return Quaternion(w*q.x - x*q.w + y*q.z - z*q.y,
|
|
w*q.y - y*q.w + z*q.x - x*q.z,
|
|
w*q.z - z*q.w + x*q.y - y*q.x,
|
|
w*q.w - x*q.x - y*q.y - z*q.z);
|
|
}
|
|
|
|
|
|
|
|
// 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);
|
|
// }
|
|
|
|
} // namespace segl
|