您最多选择25个主题 主题必须以字母或数字开头,可以包含连字符 (-),并且长度不得超过35个字符

192 行
4.3 KiB

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