190 lines
4.0 KiB
C++
190 lines
4.0 KiB
C++
// ==============================================================
|
|
// This file is part of Glest Shared Library (www.glest.org)
|
|
//
|
|
// Copyright (C) 2001-2008 Martiño Figueroa
|
|
//
|
|
// You can redistribute this code and/or modify it under
|
|
// the terms of the GNU General Public License as published
|
|
// by the Free Software Foundation; either version 2 of the
|
|
// License, or (at your option) any later version
|
|
// ==============================================================
|
|
|
|
#include "math_wrapper.h"
|
|
#include "quaternion.h"
|
|
|
|
#include "leak_dumper.h"
|
|
|
|
namespace Shared{ namespace Graphics{
|
|
|
|
// =====================================================
|
|
// class AxisAngle
|
|
// =====================================================
|
|
|
|
AxisAngle::AxisAngle(const Vec3f &axis, float angle){
|
|
this->axis= axis;
|
|
this->angle= angle;
|
|
}
|
|
|
|
// =====================================================
|
|
// class EulerAngles
|
|
// =====================================================
|
|
|
|
EulerAngles::EulerAngles(float x, float y, float z){
|
|
this->x= x;
|
|
this->y= y;
|
|
this->z= z;
|
|
}
|
|
|
|
// =====================================================
|
|
// class Quaternion
|
|
// =====================================================
|
|
|
|
Quaternion::Quaternion(){
|
|
setMultIdentity();
|
|
}
|
|
|
|
Quaternion::Quaternion(float w, const Vec3f &v){
|
|
this->w= w;
|
|
this->v= v;
|
|
}
|
|
|
|
Quaternion::Quaternion(const EulerAngles &eulerAngles){
|
|
setEuler(eulerAngles);
|
|
}
|
|
|
|
//Quaternion::Quaternion(const AxisAngle &axisAngle){
|
|
// setAxisAngle(axisAngle);
|
|
//}
|
|
|
|
void Quaternion::setMultIdentity(){
|
|
w= 1.0f;
|
|
v= Vec3f(0.0f);
|
|
}
|
|
|
|
void Quaternion::setAddIdentity(){
|
|
w= 0.0f;
|
|
v= Vec3f(0.0f);
|
|
}
|
|
|
|
void Quaternion::setEuler(const EulerAngles &eulerAngles){
|
|
Quaternion qx, qy, qz, qr;
|
|
|
|
qx.w= std::cos(eulerAngles.x/2.0f);
|
|
qx.v= Vec3f(std::sin(eulerAngles.x/2.0f), 0.0f, 0.0f);
|
|
|
|
qy.w= std::cos(eulerAngles.y/2.0f);
|
|
qy.v= Vec3f(0.0f, std::sin(eulerAngles.y/2.0f), 0.0f);
|
|
|
|
qz.w= std::cos(eulerAngles.z/2.0f);
|
|
qz.v= Vec3f(0.0f, 0.0f, std::sin(eulerAngles.z/2.0f));
|
|
|
|
qr= qx*qy*qz;
|
|
|
|
w= qr.w;
|
|
v= qr.v;
|
|
}
|
|
|
|
Quaternion Quaternion::conjugate(){
|
|
return Quaternion(w, -v);
|
|
}
|
|
|
|
Quaternion Quaternion::operator + (const Quaternion &q) const{
|
|
return Quaternion(w +q.w, v+q.v);
|
|
}
|
|
Quaternion Quaternion::operator * (const Quaternion &q) const{
|
|
return Quaternion(
|
|
w*q.w - v.x*q.v.x - v.y*q.v.y - v.z*q.v.z,
|
|
Vec3f(
|
|
w*q.v.x + v.x*q.w + v.y*q.v.z - v.z*q.v.y,
|
|
w*q.v.y + v.y*q.w + v.z*q.v.x - v.x*q.v.z,
|
|
w*q.v.z + v.z*q.w + v.x*q.v.y - v.y*q.v.x));
|
|
}
|
|
|
|
void Quaternion::operator += (const Quaternion &q){
|
|
*this= *this + q;
|
|
}
|
|
|
|
void Quaternion::operator *= (const Quaternion &q){
|
|
*this= *this * q;
|
|
}
|
|
|
|
Quaternion Quaternion::lerp(float t, const Quaternion &q) const{
|
|
return Quaternion(
|
|
w * (1.0f-t) + q.w * t,
|
|
v * (1.0f-t) + q.v * t);
|
|
}
|
|
|
|
Matrix3f Quaternion::toMatrix3() const{
|
|
Matrix3f rm;
|
|
|
|
//row1
|
|
rm[0]= 1.0f - 2*v.y*v.y - 2*v.z*v.z;
|
|
rm[3]= 2*v.x*v.y - 2*w*v.z;
|
|
rm[6]= 2*v.x*v.z + 2*w*v.y;
|
|
|
|
//row2
|
|
rm[1]= 2*v.x*v.y + 2*w*v.z;
|
|
rm[4]= 1.0f - 2*v.x*v.x - 2*v.z*v.z;
|
|
rm[7]= 2*v.y*v.z - 2*w*v.x;
|
|
|
|
//row3
|
|
rm[2]= 2*v.x*v.z - 2*w*v.y;
|
|
rm[5]= 2*v.y*v.z + 2*w*v.x;
|
|
rm[8]= 1.0f - 2*v.x*v.x - 2*v.y*v.y;
|
|
|
|
return rm;
|
|
}
|
|
|
|
Matrix4f Quaternion::toMatrix4() const{
|
|
Matrix4f rm;
|
|
|
|
//row1
|
|
rm[0]= 1.0f - 2*v.y*v.y - 2*v.z*v.z;
|
|
rm[4]= 2*v.x*v.y - 2*w*v.z;
|
|
rm[8]= 2*v.x*v.z + 2*w*v.y;
|
|
rm[12]= 0.0f;
|
|
|
|
//row2
|
|
rm[1]= 2*v.x*v.y + 2*w*v.z;
|
|
rm[5]= 1.0f - 2*v.x*v.x - 2*v.z*v.z;
|
|
rm[9]= 2*v.y*v.z - 2*w*v.x;
|
|
rm[13]= 0.0f;
|
|
|
|
//row3
|
|
rm[2]= 2*v.x*v.z - 2*w*v.y;
|
|
rm[6]= 2*v.y*v.z + 2*w*v.x;
|
|
rm[10]= 1.0f - 2*v.x*v.x - 2*v.y*v.y;
|
|
rm[14]= 0.0f;
|
|
|
|
//row4
|
|
rm[3]= 0.0f;
|
|
rm[7]= 0.0f;
|
|
rm[11]= 0.0f;
|
|
rm[15]= 1.0f;
|
|
|
|
return rm;
|
|
}
|
|
|
|
Vec3f Quaternion::getLocalXAxis() const{
|
|
return Vec3f(
|
|
1.0f - 2*v.y*v.y - 2*v.z*v.z,
|
|
2*v.x*v.y + 2*w*v.z,
|
|
2*v.x*v.z - 2*w*v.y);
|
|
}
|
|
|
|
Vec3f Quaternion::getLocalYAxis() const{
|
|
return Vec3f(
|
|
2*v.x*v.y - 2*w*v.z,
|
|
1.0f - 2*v.x*v.x - 2*v.z*v.z,
|
|
2*v.y*v.z + 2*w*v.x);
|
|
}
|
|
|
|
Vec3f Quaternion::getLocalZAxis() const{
|
|
return Vec3f(
|
|
2*v.x*v.z + 2*w*v.y,
|
|
2*v.y*v.z - 2*w*v.x,
|
|
1.0f - 2*v.x*v.x - 2*v.y*v.y);
|
|
}
|
|
|
|
}}//end namespace
|