123456789101112131415161718192021222324252627282930313233343536373839404142434445464748495051525354555657585960616263646566676869707172737475767778798081828384858687888990919293949596979899100101102103104105106107108109110111112113114115116117118119120121122123124125126127128129130131132133134135136137138139140141142143144145146147148149150151152153154155156157158159160161162163164165166167168169170171172173174175176177178179180181182183184185186187188189190191 |
- /* 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
|