296 lines
7.7 KiB
C++
296 lines
7.7 KiB
C++
#ifndef _SIMPLEMATHGL_H_
|
|
#define _SIMPLEMATHGL_H_
|
|
|
|
#include "SimpleMath.h"
|
|
#include <cmath>
|
|
|
|
namespace SimpleMath {
|
|
|
|
typedef SimpleMath::Fixed::Matrix<float, 3, 1> Vector3f;
|
|
typedef SimpleMath::Fixed::Matrix<float, 3, 3> Matrix33f;
|
|
|
|
typedef SimpleMath::Fixed::Matrix<float, 4, 1> Vector4f;
|
|
typedef SimpleMath::Fixed::Matrix<float, 4, 4> Matrix44f;
|
|
|
|
namespace GL {
|
|
|
|
inline Matrix33f RotateMat33 (float rot_deg, float x, float y, float z) {
|
|
float c = cosf (rot_deg * M_PI / 180.f);
|
|
float s = sinf (rot_deg * M_PI / 180.f);
|
|
return Matrix33f (
|
|
x * x * (1.0f - c) + c,
|
|
y * x * (1.0f - c) + z * s,
|
|
x * z * (1.0f - c) - y * s,
|
|
|
|
x * y * (1.0f - c) - z * s,
|
|
y * y * (1.0f - c) + c,
|
|
y * z * (1.0f - c) + x * s,
|
|
|
|
x * z * (1.0f - c) + y * s,
|
|
y * z * (1.0f - c) - x * s,
|
|
z * z * (1.0f - c) + c
|
|
|
|
);
|
|
}
|
|
|
|
|
|
inline Matrix44f RotateMat44 (float rot_deg, float x, float y, float z) {
|
|
float c = cosf (rot_deg * M_PI / 180.f);
|
|
float s = sinf (rot_deg * M_PI / 180.f);
|
|
return Matrix44f (
|
|
x * x * (1.0f - c) + c,
|
|
y * x * (1.0f - c) + z * s,
|
|
x * z * (1.0f - c) - y * s,
|
|
0.f,
|
|
|
|
x * y * (1.0f - c) - z * s,
|
|
y * y * (1.0f - c) + c,
|
|
y * z * (1.0f - c) + x * s,
|
|
0.f,
|
|
|
|
x * z * (1.0f - c) + y * s,
|
|
y * z * (1.0f - c) - x * s,
|
|
z * z * (1.0f - c) + c,
|
|
0.f,
|
|
|
|
0.f, 0.f, 0.f, 1.f
|
|
);
|
|
}
|
|
|
|
inline Matrix44f TranslateMat44 (float x, float y, float z) {
|
|
return Matrix44f (
|
|
1.f, 0.f, 0.f, 0.f,
|
|
0.f, 1.f, 0.f, 0.f,
|
|
0.f, 0.f, 1.f, 0.f,
|
|
x, y, z, 1.f
|
|
);
|
|
}
|
|
|
|
inline Matrix44f ScaleMat44 (float x, float y, float z) {
|
|
return Matrix44f (
|
|
x, 0.f, 0.f, 0.f,
|
|
0.f, y, 0.f, 0.f,
|
|
0.f, 0.f, z, 0.f,
|
|
0.f, 0.f, 0.f, 1.f
|
|
);
|
|
}
|
|
|
|
/** Quaternion
|
|
*
|
|
* order: x,y,z,w
|
|
*/
|
|
class Quaternion : public Vector4f {
|
|
public:
|
|
Quaternion () :
|
|
Vector4f (0.f, 0.f, 0.f, 1.f)
|
|
{}
|
|
Quaternion (const Vector4f vec4) :
|
|
Vector4f (vec4)
|
|
{}
|
|
Quaternion (float x, float y, float z, float w):
|
|
Vector4f (x, y, z, w)
|
|
{}
|
|
/** This function is equivalent to multiplicate their corresponding rotation matrices */
|
|
Quaternion operator* (const Quaternion &q) const {
|
|
return Quaternion (
|
|
(*this)[3] * q[0] + (*this)[0] * q[3] + (*this)[1] * q[2] - (*this)[2] * q[1],
|
|
(*this)[3] * q[1] + (*this)[1] * q[3] + (*this)[2] * q[0] - (*this)[0] * q[2],
|
|
(*this)[3] * q[2] + (*this)[2] * q[3] + (*this)[0] * q[1] - (*this)[1] * q[0],
|
|
(*this)[3] * q[3] - (*this)[0] * q[0] - (*this)[1] * q[1] - (*this)[2] * q[2]
|
|
);
|
|
}
|
|
Quaternion& operator*=(const Quaternion &q) {
|
|
set (
|
|
(*this)[3] * q[0] + (*this)[0] * q[3] + (*this)[1] * q[2] - (*this)[2] * q[1],
|
|
(*this)[3] * q[1] + (*this)[1] * q[3] + (*this)[2] * q[0] - (*this)[0] * q[2],
|
|
(*this)[3] * q[2] + (*this)[2] * q[3] + (*this)[0] * q[1] - (*this)[1] * q[0],
|
|
(*this)[3] * q[3] - (*this)[0] * q[0] - (*this)[1] * q[1] - (*this)[2] * q[2]
|
|
);
|
|
return *this;
|
|
}
|
|
|
|
static Quaternion fromGLRotate (float angle, float x, float y, float z) {
|
|
float st = sinf (angle * M_PI / 360.f);
|
|
return Quaternion (
|
|
st * x,
|
|
st * y,
|
|
st * z,
|
|
cosf (angle * M_PI / 360.f)
|
|
);
|
|
}
|
|
|
|
Quaternion normalize() {
|
|
return Vector4f::normalize();
|
|
}
|
|
|
|
Quaternion slerp (float alpha, const Quaternion &quat) const {
|
|
// check whether one of the two has 0 length
|
|
float s = sqrt (squaredNorm() * quat.squaredNorm());
|
|
|
|
// division by 0.f is unhealthy!
|
|
assert (s != 0.f);
|
|
|
|
float angle = acos (dot(quat) / s);
|
|
if (angle == 0.f || std::isnan(angle)) {
|
|
return *this;
|
|
}
|
|
assert(!std::isnan(angle));
|
|
|
|
float d = 1.f / sinf (angle);
|
|
float p0 = sinf ((1.f - alpha) * angle);
|
|
float p1 = sinf (alpha * angle);
|
|
|
|
if (dot (quat) < 0.f) {
|
|
return Quaternion( ((*this) * p0 - quat * p1) * d);
|
|
}
|
|
return Quaternion( ((*this) * p0 + quat * p1) * d);
|
|
}
|
|
|
|
Matrix44f toGLMatrix() const {
|
|
float x = (*this)[0];
|
|
float y = (*this)[1];
|
|
float z = (*this)[2];
|
|
float w = (*this)[3];
|
|
return Matrix44f (
|
|
1 - 2*y*y - 2*z*z,
|
|
2*x*y + 2*w*z,
|
|
2*x*z - 2*w*y,
|
|
0.f,
|
|
|
|
2*x*y - 2*w*z,
|
|
1 - 2*x*x - 2*z*z,
|
|
2*y*z + 2*w*x,
|
|
0.f,
|
|
|
|
2*x*z + 2*w*y,
|
|
2*y*z - 2*w*x,
|
|
1 - 2*x*x - 2*y*y,
|
|
0.f,
|
|
|
|
0.f,
|
|
0.f,
|
|
0.f,
|
|
1.f);
|
|
}
|
|
|
|
static Quaternion fromGLMatrix(const Matrix44f &mat) {
|
|
float w = sqrt (1.f + mat(0,0) + mat(1,1) + mat(2,2)) * 0.5f;
|
|
return Quaternion (
|
|
-(mat(2,1) - mat(1,2)) / (w * 4.f),
|
|
-(mat(0,2) - mat(2,0)) / (w * 4.f),
|
|
-(mat(1,0) - mat(0,1)) / (w * 4.f),
|
|
w);
|
|
}
|
|
|
|
static Quaternion fromMatrix (const Matrix33f &mat) {
|
|
float w = sqrt (1.f + mat(0,0) + mat(1,1) + mat(2,2)) * 0.5f;
|
|
return Quaternion (
|
|
(mat(2,1) - mat(1,2)) / (w * 4.f),
|
|
(mat(0,2) - mat(2,0)) / (w * 4.f),
|
|
(mat(1,0) - mat(0,1)) / (w * 4.f),
|
|
w);
|
|
}
|
|
|
|
static Quaternion fromAxisAngle (const Vector3f &axis, double angle_rad) {
|
|
double d = axis.norm();
|
|
double s2 = std::sin (angle_rad * 0.5) / d;
|
|
return Quaternion (
|
|
axis[0] * s2,
|
|
axis[1] * s2,
|
|
axis[2] * s2,
|
|
std::cos(angle_rad * 0.5)
|
|
);
|
|
}
|
|
|
|
static Quaternion fromEulerZYX (const Vector3f &zyx_angles) {
|
|
return Quaternion::fromAxisAngle (Vector3f (0., 0., 1.), zyx_angles[0])
|
|
* Quaternion::fromAxisAngle (Vector3f (0., 1., 0.), zyx_angles[1])
|
|
* Quaternion::fromAxisAngle (Vector3f (1., 0., 0.), zyx_angles[2]);
|
|
}
|
|
|
|
static Quaternion fromEulerYXZ (const Vector3f &yxz_angles) {
|
|
return Quaternion::fromAxisAngle (Vector3f (0., 1., 0.), yxz_angles[0])
|
|
* Quaternion::fromAxisAngle (Vector3f (1., 0., 0.), yxz_angles[1])
|
|
* Quaternion::fromAxisAngle (Vector3f (0., 0., 1.), yxz_angles[2]);
|
|
}
|
|
|
|
static Quaternion fromEulerXYZ (const Vector3f &xyz_angles) {
|
|
return Quaternion::fromAxisAngle (Vector3f (0., 0., 01.), xyz_angles[2])
|
|
* Quaternion::fromAxisAngle (Vector3f (0., 1., 0.), xyz_angles[1])
|
|
* Quaternion::fromAxisAngle (Vector3f (1., 0., 0.), xyz_angles[0]);
|
|
}
|
|
|
|
Vector3f toEulerZYX () const {
|
|
return Vector3f (
|
|
atan2 (-2.f * (*this)[0] * (*this)[1] + 2.f * (*this)[3] * (*this)[2],
|
|
(*this)[0] * (*this)[0] + (*this)[3] * (*this)[3]
|
|
-(*this)[2] * (*this)[2] - (*this)[1] * (*this)[1]),
|
|
asin (2.f * (*this)[0] * (*this)[2] + 2.f * (*this)[3] * (*this)[1]),
|
|
atan2 (-2.f * (*this)[1] * (*this)[2] + 2.f * (*this)[3] * (*this)[0],
|
|
(*this)[2] * (*this)[2] - (*this)[1] * (*this)[1]
|
|
-(*this)[0] * (*this)[0] + (*this)[3] * (*this)[3]
|
|
)
|
|
);
|
|
}
|
|
|
|
Vector3f toEulerYXZ() const {
|
|
return Vector3f (
|
|
atan2 (-2.f * (*this)[0] * (*this)[2] + 2.f * (*this)[3] * (*this)[1],
|
|
(*this)[2] * (*this)[2] - (*this)[1] * (*this)[1]
|
|
-(*this)[0] * (*this)[0] + (*this)[3] * (*this)[3]),
|
|
asin (2.f * (*this)[1] * (*this)[2] + 2.f * (*this)[3] * (*this)[0]),
|
|
atan2 (-2.f * (*this)[0] * (*this)[1] + 2.f * (*this)[3] * (*this)[2],
|
|
(*this)[1] * (*this)[1] - (*this)[2] * (*this)[2]
|
|
+(*this)[3] * (*this)[3] - (*this)[0] * (*this)[0]
|
|
)
|
|
);
|
|
}
|
|
|
|
Matrix33f toMatrix() const {
|
|
float x = (*this)[0];
|
|
float y = (*this)[1];
|
|
float z = (*this)[2];
|
|
float w = (*this)[3];
|
|
return Matrix33f (
|
|
1 - 2*y*y - 2*z*z,
|
|
2*x*y - 2*w*z,
|
|
2*x*z + 2*w*y,
|
|
|
|
2*x*y + 2*w*z,
|
|
1 - 2*x*x - 2*z*z,
|
|
2*y*z - 2*w*x,
|
|
|
|
2*x*z - 2*w*y,
|
|
2*y*z + 2*w*x,
|
|
1 - 2*x*x - 2*y*y
|
|
);
|
|
}
|
|
|
|
Quaternion conjugate() const {
|
|
return Quaternion (
|
|
-(*this)[0],
|
|
-(*this)[1],
|
|
-(*this)[2],
|
|
(*this)[3]);
|
|
}
|
|
|
|
Vector3f rotate (const Vector3f &vec) const {
|
|
Vector3f vn (vec);
|
|
Quaternion vec_quat (vn[0], vn[1], vn[2], 0.f), res_quat;
|
|
|
|
res_quat = (*this) * vec_quat;
|
|
res_quat = res_quat * conjugate();
|
|
|
|
return Vector3f (res_quat[0], res_quat[1], res_quat[2]);
|
|
}
|
|
};
|
|
|
|
// namespace GL
|
|
}
|
|
|
|
// namespace SimpleMath
|
|
}
|
|
|
|
/* _SIMPLEMATHGL_H_ */
|
|
#endif
|