Michael Fabian 'Xaymar' Dirks
742 lines
16 KiB
C++
742 lines
16 KiB
C++
|
|
#ifndef GEOM_H
|
|
#define GEOM_H
|
|
|
|
#include <math.h>
|
|
|
|
class Vector;
|
|
class Line;
|
|
class Plane;
|
|
class Matrix;
|
|
class Transform;
|
|
|
|
static float PI = 3.14159265359f; //180 degrees
|
|
static float TWOPI = PI * 2.0f; //360 degrees
|
|
static float HALFPI = PI * .5f; //90 degrees
|
|
static float QUARTERPI = PI * .25f; //45 degrees
|
|
|
|
class Vector {
|
|
public:
|
|
float x, y, z;
|
|
|
|
Vector() : x(0), y(0), z(0) {}
|
|
Vector(float x, float y, float z) : x(x), y(y), z(z) {}
|
|
operator float*()
|
|
{
|
|
return &x;
|
|
}
|
|
operator const float*()
|
|
{
|
|
return &x;
|
|
}
|
|
float& operator[](int n)
|
|
{
|
|
return (&x)[n];
|
|
}
|
|
float operator[](int n) const
|
|
{
|
|
return (&x)[n];
|
|
}
|
|
Vector operator-() const
|
|
{
|
|
return Vector(-x, -y, -z);
|
|
}
|
|
Vector operator*(float scale) const
|
|
{
|
|
return Vector(x * scale, y * scale, z * scale);
|
|
}
|
|
Vector operator*(const Vector& q) const
|
|
{
|
|
return Vector(x * q.x, y * q.y, z * q.z);
|
|
}
|
|
Vector operator/(float scale) const
|
|
{
|
|
return Vector(x / scale, y / scale, z / scale);
|
|
}
|
|
Vector operator/(const Vector& q) const
|
|
{
|
|
return Vector(x / q.x, y / q.y, z / q.z);
|
|
}
|
|
Vector operator+(const Vector& q) const
|
|
{
|
|
return Vector(x + q.x, y + q.y, z + q.z);
|
|
}
|
|
Vector operator-(const Vector& q) const
|
|
{
|
|
return Vector(x - q.x, y - q.y, z - q.z);
|
|
}
|
|
Vector& operator*=(float scale)
|
|
{
|
|
x *= scale;
|
|
y *= scale;
|
|
z *= scale;
|
|
return *this;
|
|
}
|
|
Vector& operator*=(const Vector& q)
|
|
{
|
|
x *= q.x;
|
|
y *= q.y;
|
|
z *= q.z;
|
|
return *this;
|
|
}
|
|
Vector& operator/=(float scale)
|
|
{
|
|
x /= scale;
|
|
y /= scale;
|
|
z /= scale;
|
|
return *this;
|
|
}
|
|
Vector& operator/=(const Vector& q)
|
|
{
|
|
x /= q.x;
|
|
y /= q.y;
|
|
z /= q.z;
|
|
return *this;
|
|
}
|
|
Vector& operator+=(const Vector& q)
|
|
{
|
|
x += q.x;
|
|
y += q.y;
|
|
z += q.z;
|
|
return *this;
|
|
}
|
|
Vector& operator-=(const Vector& q)
|
|
{
|
|
x -= q.x;
|
|
y -= q.y;
|
|
z -= q.z;
|
|
return *this;
|
|
}
|
|
bool operator<(const Vector& q) const
|
|
{
|
|
if (fabs(x - q.x) > FLT_EPSILON)
|
|
return x < q.x ? true : false;
|
|
if (fabs(y - q.y) > FLT_EPSILON)
|
|
return y < q.y ? true : false;
|
|
return fabs(z - q.z) > FLT_EPSILON && z < q.z;
|
|
}
|
|
bool operator==(const Vector& q) const
|
|
{
|
|
return fabs(x - q.x) <= FLT_EPSILON && fabs(y - q.y) <= FLT_EPSILON && fabs(z - q.z) <= FLT_EPSILON;
|
|
}
|
|
bool operator!=(const Vector& q) const
|
|
{
|
|
return fabs(x - q.x) > FLT_EPSILON || fabs(y - q.y) > FLT_EPSILON || fabs(z - q.z) > FLT_EPSILON;
|
|
}
|
|
float dot(const Vector& q) const
|
|
{
|
|
return x * q.x + y * q.y + z * q.z;
|
|
}
|
|
Vector cross(const Vector& q) const
|
|
{
|
|
return Vector(y * q.z - z * q.y, z * q.x - x * q.z, x * q.y - y * q.x);
|
|
}
|
|
float length() const
|
|
{
|
|
return sqrtf(x * x + y * y + z * z);
|
|
}
|
|
float distance(const Vector& q) const
|
|
{
|
|
float dx = x - q.x, dy = y - q.y, dz = z - q.z;
|
|
return sqrtf(dx * dx + dy * dy + dz * dz);
|
|
}
|
|
Vector normalized() const
|
|
{
|
|
float l = length();
|
|
return Vector(x / l, y / l, z / l);
|
|
}
|
|
void normalize()
|
|
{
|
|
float l = length();
|
|
x /= l;
|
|
y /= l;
|
|
z /= l;
|
|
}
|
|
float yaw() const
|
|
{
|
|
return -atan2f(x, z);
|
|
}
|
|
float pitch() const
|
|
{
|
|
return -atan2f(y, sqrtf(x * x + z * z));
|
|
}
|
|
void clear()
|
|
{
|
|
x = y = z = 0;
|
|
}
|
|
};
|
|
|
|
class Line {
|
|
public:
|
|
Vector o, d;
|
|
Line() {}
|
|
Line(const Vector& o, const Vector& d) : o(o), d(d) {}
|
|
Line operator+(const Vector& q) const
|
|
{
|
|
return Line(o + q, d);
|
|
}
|
|
Line operator-(const Vector& q) const
|
|
{
|
|
return Line(o - q, d);
|
|
}
|
|
Vector operator*(float q) const
|
|
{
|
|
return o + d * q;
|
|
}
|
|
Vector nearest(const Vector& q) const
|
|
{
|
|
return o + d * (d.dot(q - o) / d.dot(d));
|
|
}
|
|
};
|
|
|
|
class Plane {
|
|
public:
|
|
Vector n;
|
|
float d;
|
|
|
|
Plane() : d(0) {}
|
|
//normal/offset form
|
|
Plane(const Vector& n, float d) : n(n), d(d) {}
|
|
//point/normal form
|
|
Plane(const Vector& p, const Vector& n) : n(n), d(-n.dot(p)) {}
|
|
//create plane from tri
|
|
Plane(const Vector& v0, const Vector& v1, const Vector& v2)
|
|
{
|
|
n = (v1 - v0).cross(v2 - v0).normalized();
|
|
d = -n.dot(v0);
|
|
}
|
|
Plane operator-() const
|
|
{
|
|
return Plane(-n, -d);
|
|
}
|
|
float t_intersect(const Line& q) const
|
|
{
|
|
return -distance(q.o) / n.dot(q.d);
|
|
}
|
|
Vector intersect(const Line& q) const
|
|
{
|
|
return q * t_intersect(q);
|
|
}
|
|
Line intersect(const Plane& q) const
|
|
{
|
|
Vector lv = n.cross(q.n).normalized();
|
|
return Line(q.intersect(Line(nearest(n * -d), n.cross(lv))), lv);
|
|
}
|
|
Vector nearest(const Vector& q) const
|
|
{
|
|
return q - n * distance(q);
|
|
}
|
|
void negate()
|
|
{
|
|
n = -n;
|
|
d = -d;
|
|
}
|
|
float distance(const Vector& q) const
|
|
{
|
|
return n.dot(q) + d;
|
|
}
|
|
};
|
|
|
|
struct Quat {
|
|
float w;
|
|
Vector v;
|
|
Quat() : w(1) {}
|
|
Quat(float w, const Vector& v) : w(w), v(v) {}
|
|
Quat operator-() const
|
|
{
|
|
return Quat(w, -v);
|
|
}
|
|
Quat operator+(const Quat& q) const
|
|
{
|
|
return Quat(w + q.w, v + q.v);
|
|
}
|
|
Quat operator-(const Quat& q) const
|
|
{
|
|
return Quat(w - q.w, v - q.v);
|
|
}
|
|
Quat operator*(const Quat& q) const
|
|
{
|
|
return Quat(w * q.w - v.dot(q.v), q.v.cross(v) + q.v * w + v * q.w);
|
|
}
|
|
Vector operator*(const Vector& q) const
|
|
{
|
|
return (*this * Quat(0, q) * -*this).v;
|
|
}
|
|
Quat operator*(float q) const
|
|
{
|
|
return Quat(w * q, v * q);
|
|
}
|
|
Quat operator/(float q) const
|
|
{
|
|
return Quat(w / q, v / q);
|
|
}
|
|
float dot(const Quat& q) const
|
|
{
|
|
return v.x * q.v.x + v.y * q.v.y + v.z * q.v.z + w * q.w;
|
|
}
|
|
float length() const
|
|
{
|
|
return sqrtf(w * w + v.x * v.x + v.y * v.y + v.z * v.z);
|
|
}
|
|
void normalize()
|
|
{
|
|
*this = *this / length();
|
|
}
|
|
Quat normalized() const
|
|
{
|
|
return *this / length();
|
|
}
|
|
Quat slerpTo(const Quat& q, float a) const
|
|
{
|
|
Quat t = q;
|
|
float d = dot(q), b = 1 - a;
|
|
if (d < 0) {
|
|
t.w = -t.w;
|
|
t.v = -t.v;
|
|
d = -d;
|
|
}
|
|
if (d < 1 - FLT_EPSILON) {
|
|
float om = acosf(d);
|
|
float si = sinf(om);
|
|
a = sinf(a * om) / si;
|
|
b = sinf(b * om) / si;
|
|
}
|
|
return *this * b + t * a;
|
|
}
|
|
Vector i() const
|
|
{
|
|
float xz = v.x * v.z, wy = w * v.y;
|
|
float xy = v.x * v.y, wz = w * v.z;
|
|
float yy = v.y * v.y, zz = v.z * v.z;
|
|
return Vector(1 - 2 * (yy + zz), 2 * (xy - wz), 2 * (xz + wy));
|
|
}
|
|
Vector j() const
|
|
{
|
|
float yz = v.y * v.z, wx = w * v.x;
|
|
float xy = v.x * v.y, wz = w * v.z;
|
|
float xx = v.x * v.x, zz = v.z * v.z;
|
|
return Vector(2 * (xy + wz), 1 - 2 * (xx + zz), 2 * (yz - wx));
|
|
}
|
|
Vector k() const
|
|
{
|
|
float xz = v.x * v.z, wy = w * v.y;
|
|
float yz = v.y * v.z, wx = w * v.x;
|
|
float xx = v.x * v.x, yy = v.y * v.y;
|
|
return Vector(2 * (xz - wy), 2 * (yz + wx), 1 - 2 * (xx + yy));
|
|
}
|
|
};
|
|
|
|
class Matrix {
|
|
static Matrix tmps[64];
|
|
static Matrix& alloc_tmp()
|
|
{
|
|
static int tmp = 0;
|
|
return tmps[tmp++ & 63];
|
|
}
|
|
friend class Transform;
|
|
|
|
public:
|
|
Vector i, j, k;
|
|
|
|
Matrix() : i(Vector(1, 0, 0)), j(Vector(0, 1, 0)), k(Vector(0, 0, 1)) {}
|
|
Matrix(const Vector& i, const Vector& j, const Vector& k) : i(i), j(j), k(k) {}
|
|
Matrix(const Quat& q)
|
|
{
|
|
float xx = q.v.x * q.v.x, yy = q.v.y * q.v.y, zz = q.v.z * q.v.z;
|
|
float xy = q.v.x * q.v.y, xz = q.v.x * q.v.z, yz = q.v.y * q.v.z;
|
|
float wx = q.w * q.v.x, wy = q.w * q.v.y, wz = q.w * q.v.z;
|
|
i = Vector(1 - 2 * (yy + zz), 2 * (xy - wz), 2 * (xz + wy)),
|
|
j = Vector(2 * (xy + wz), 1 - 2 * (xx + zz), 2 * (yz - wx)),
|
|
k = Vector(2 * (xz - wy), 2 * (yz + wx), 1 - 2 * (xx + yy));
|
|
}
|
|
Matrix(float angle, const Vector& axis)
|
|
{
|
|
const Vector& u = axis;
|
|
float c = cosf(angle), s = sinf(angle);
|
|
float x2 = axis.x * axis.x, y2 = axis.y * axis.y, z2 = axis.z * axis.z;
|
|
i = Vector(x2 + c * (1 - x2), u.x * u.y * (1 - c) - u.z * s, u.z * u.x * (1 - c) + u.y * s);
|
|
j = Vector(u.x * u.y * (1 - c) + u.z * s, y2 + c * (1 - y2), u.y * u.z * (1 - c) - u.x * s);
|
|
k = Vector(u.z * u.x * (1 - c) - u.y * s, u.y * u.z * (1 - c) + u.x * s, z2 + c * (1 - z2));
|
|
}
|
|
Vector& operator[](int n)
|
|
{
|
|
return (&i)[n];
|
|
}
|
|
const Vector& operator[](int n) const
|
|
{
|
|
return (&i)[n];
|
|
}
|
|
Matrix& operator~() const
|
|
{
|
|
Matrix& m = alloc_tmp();
|
|
m.i.x = i.x;
|
|
m.i.y = j.x;
|
|
m.i.z = k.x;
|
|
m.j.x = i.y;
|
|
m.j.y = j.y;
|
|
m.j.z = k.y;
|
|
m.k.x = i.z;
|
|
m.k.y = j.z;
|
|
m.k.z = k.z;
|
|
return m;
|
|
}
|
|
float determinant() const
|
|
{
|
|
return i.x * (j.y * k.z - j.z * k.y) - i.y * (j.x * k.z - j.z * k.x) + i.z * (j.x * k.y - j.y * k.x);
|
|
}
|
|
Matrix& operator-() const
|
|
{
|
|
Matrix& m = alloc_tmp();
|
|
float t = 1.0f / determinant();
|
|
m.i.x = t * (j.y * k.z - j.z * k.y);
|
|
m.i.y = -t * (i.y * k.z - i.z * k.y);
|
|
m.i.z = t * (i.y * j.z - i.z * j.y);
|
|
m.j.x = -t * (j.x * k.z - j.z * k.x);
|
|
m.j.y = t * (i.x * k.z - i.z * k.x);
|
|
m.j.z = -t * (i.x * j.z - i.z * j.x);
|
|
m.k.x = t * (j.x * k.y - j.y * k.x);
|
|
m.k.y = -t * (i.x * k.y - i.y * k.x);
|
|
m.k.z = t * (i.x * j.y - i.y * j.x);
|
|
return m;
|
|
}
|
|
Matrix& cofactor() const
|
|
{
|
|
Matrix& m = alloc_tmp();
|
|
m.i.x = (j.y * k.z - j.z * k.y);
|
|
m.i.y = -(j.x * k.z - j.z * k.x);
|
|
m.i.z = (j.x * k.y - j.y * k.x);
|
|
m.j.x = -(i.y * k.z - i.z * k.y);
|
|
m.j.y = (i.x * k.z - i.z * k.x);
|
|
m.j.z = -(i.x * k.y - i.y * k.x);
|
|
m.k.x = (i.y * j.z - i.z * j.y);
|
|
m.k.y = -(i.x * j.z - i.z * j.x);
|
|
m.k.z = (i.x * j.y - i.y * j.x);
|
|
return m;
|
|
}
|
|
bool operator==(const Matrix& q) const
|
|
{
|
|
return i == q.i && j == q.j && k == q.k;
|
|
}
|
|
bool operator!=(const Matrix& q) const
|
|
{
|
|
return i != q.i || j != q.j || k != q.k;
|
|
}
|
|
Vector operator*(const Vector& q) const
|
|
{
|
|
return Vector(i.x * q.x + j.x * q.y + k.x * q.z, i.y * q.x + j.y * q.y + k.y * q.z,
|
|
i.z * q.x + j.z * q.y + k.z * q.z);
|
|
}
|
|
Matrix& operator*(const Matrix& q) const
|
|
{
|
|
Matrix& m = alloc_tmp();
|
|
m.i.x = i.x * q.i.x + j.x * q.i.y + k.x * q.i.z;
|
|
m.i.y = i.y * q.i.x + j.y * q.i.y + k.y * q.i.z;
|
|
m.i.z = i.z * q.i.x + j.z * q.i.y + k.z * q.i.z;
|
|
m.j.x = i.x * q.j.x + j.x * q.j.y + k.x * q.j.z;
|
|
m.j.y = i.y * q.j.x + j.y * q.j.y + k.y * q.j.z;
|
|
m.j.z = i.z * q.j.x + j.z * q.j.y + k.z * q.j.z;
|
|
m.k.x = i.x * q.k.x + j.x * q.k.y + k.x * q.k.z;
|
|
m.k.y = i.y * q.k.x + j.y * q.k.y + k.y * q.k.z;
|
|
m.k.z = i.z * q.k.x + j.z * q.k.y + k.z * q.k.z;
|
|
return m;
|
|
}
|
|
void orthogonalize()
|
|
{
|
|
k.normalize();
|
|
i = j.cross(k).normalized();
|
|
j = k.cross(i);
|
|
}
|
|
Matrix& orthogonalized() const
|
|
{
|
|
Matrix& m = alloc_tmp();
|
|
m = *this;
|
|
m.orthogonalize();
|
|
return m;
|
|
}
|
|
};
|
|
|
|
class Box {
|
|
public:
|
|
Vector a, b;
|
|
Box() : a(Vector(INFINITY, INFINITY, INFINITY)), b(Vector(-INFINITY, -INFINITY, -INFINITY)) {}
|
|
Box(const Vector& q) : a(q), b(q) {}
|
|
Box(const Vector& a, const Vector& b) : a(a), b(b) {}
|
|
Box(const Line& l) : a(l.o), b(l.o)
|
|
{
|
|
update(l.o + l.d);
|
|
}
|
|
void clear()
|
|
{
|
|
a.x = a.y = a.z = INFINITY;
|
|
b.x = b.y = b.z = -INFINITY;
|
|
}
|
|
bool empty() const
|
|
{
|
|
return b.x < a.x || b.y < a.y || b.z < a.z;
|
|
}
|
|
Vector centre() const
|
|
{
|
|
return Vector((a.x + b.x) * .5f, (a.y + b.y) * .5f, (a.z + b.z) * .5f);
|
|
}
|
|
Vector corner(int n) const
|
|
{
|
|
return Vector(((n & 1) ? b : a).x, ((n & 2) ? b : a).y, ((n & 4) ? b : a).z);
|
|
}
|
|
void update(const Vector& q)
|
|
{
|
|
if (q.x < a.x)
|
|
a.x = q.x;
|
|
if (q.y < a.y)
|
|
a.y = q.y;
|
|
if (q.z < a.z)
|
|
a.z = q.z;
|
|
if (q.x > b.x)
|
|
b.x = q.x;
|
|
if (q.y > b.y)
|
|
b.y = q.y;
|
|
if (q.z > b.z)
|
|
b.z = q.z;
|
|
}
|
|
void update(const Box& q)
|
|
{
|
|
if (q.a.x < a.x)
|
|
a.x = q.a.x;
|
|
if (q.a.y < a.y)
|
|
a.y = q.a.y;
|
|
if (q.a.z < a.z)
|
|
a.z = q.a.z;
|
|
if (q.b.x > b.x)
|
|
b.x = q.b.x;
|
|
if (q.b.y > b.y)
|
|
b.y = q.b.y;
|
|
if (q.b.z > b.z)
|
|
b.z = q.b.z;
|
|
}
|
|
bool overlaps(const Box& q) const
|
|
{
|
|
return (b.x < q.b.x ? b.x : q.b.x) >= (a.x > q.a.x ? a.x : q.a.x)
|
|
&& (b.y < q.b.y ? b.y : q.b.y) >= (a.y > q.a.y ? a.y : q.a.y)
|
|
&& (b.z < q.b.z ? b.z : q.b.z) >= (a.z > q.a.z ? a.z : q.a.z);
|
|
}
|
|
void expand(float n)
|
|
{
|
|
a.x -= n;
|
|
a.y -= n;
|
|
a.z -= n;
|
|
b.x += n;
|
|
b.y += n;
|
|
b.z += n;
|
|
}
|
|
float width() const
|
|
{
|
|
return b.x - a.x;
|
|
}
|
|
float height() const
|
|
{
|
|
return b.y - a.y;
|
|
}
|
|
float depth() const
|
|
{
|
|
return b.z - a.z;
|
|
}
|
|
bool contains(const Vector& q)
|
|
{
|
|
return q.x >= a.x && q.x <= b.x && q.y >= a.y && q.y <= b.y && q.z >= a.z && q.z <= b.z;
|
|
}
|
|
};
|
|
|
|
class Transform {
|
|
static Transform tmps[64];
|
|
static Transform& alloc_tmp()
|
|
{
|
|
static int tmp = 0;
|
|
return tmps[tmp++ & 63];
|
|
}
|
|
|
|
public:
|
|
Matrix m;
|
|
Vector v;
|
|
|
|
Transform() {}
|
|
Transform(const Matrix& m) : m(m) {}
|
|
Transform(const Vector& v) : v(v) {}
|
|
Transform(const Matrix& m, const Vector& v) : m(m), v(v) {}
|
|
Transform& operator-() const
|
|
{
|
|
Transform& t = alloc_tmp();
|
|
t.m = -m;
|
|
t.v = t.m * -v;
|
|
return t;
|
|
}
|
|
Transform& operator~() const
|
|
{
|
|
Transform& t = alloc_tmp();
|
|
t.m = ~m;
|
|
t.v = t.m * -v;
|
|
return t;
|
|
}
|
|
Vector operator*(const Vector& q) const
|
|
{
|
|
return m * q + v;
|
|
}
|
|
Line operator*(const Line& q) const
|
|
{
|
|
Vector t = (*this) * q.o;
|
|
return Line(t, (*this) * (q.o + q.d) - t);
|
|
}
|
|
Box operator*(const Box& q) const
|
|
{
|
|
Box t((*this * q.corner(0)));
|
|
for (int k = 1; k < 8; ++k)
|
|
t.update(*this * q.corner(k));
|
|
return t;
|
|
}
|
|
Transform& operator*(const Transform& q) const
|
|
{
|
|
Transform& t = alloc_tmp();
|
|
t.m = m * q.m;
|
|
t.v = m * q.v + v;
|
|
return t;
|
|
}
|
|
bool operator==(const Transform& q) const
|
|
{
|
|
return m == q.m && v == q.v;
|
|
}
|
|
bool operator!=(const Transform& q) const
|
|
{
|
|
return !operator==(q);
|
|
}
|
|
};
|
|
|
|
inline float transformRadius(float r, const Matrix& t)
|
|
{
|
|
static const float sq_3 = sqrtf(1.0f / 3.0f);
|
|
return (t * Vector(sq_3, sq_3, sq_3)).length() * r;
|
|
}
|
|
|
|
inline Matrix pitchMatrix(float q)
|
|
{
|
|
return Matrix(Vector(1, 0, 0), Vector(0, cosf(q), sinf(q)), Vector(0, -sinf(q), cosf(q)));
|
|
}
|
|
|
|
inline Matrix yawMatrix(float q)
|
|
{
|
|
return Matrix(Vector(cosf(q), 0, sinf(q)), Vector(0, 1, 0), Vector(-sinf(q), 0, cosf(q)));
|
|
}
|
|
|
|
inline Matrix rollMatrix(float q)
|
|
{
|
|
return Matrix(Vector(cosf(q), sinf(q), 0), Vector(-sinf(q), cosf(q), 0), Vector(0, 0, 1));
|
|
}
|
|
|
|
inline float matrixPitch(const Matrix& m)
|
|
{
|
|
return m.k.pitch();
|
|
// return asinf( -m.k.y );
|
|
}
|
|
|
|
inline float matrixYaw(const Matrix& m)
|
|
{
|
|
return m.k.yaw();
|
|
//return atan2f( -m.k.x,m.k.z );
|
|
}
|
|
|
|
inline float matrixRoll(const Matrix& m)
|
|
{
|
|
return atan2f(m.i.y, m.j.y);
|
|
//Matrix t=pitchMatrix( -matrixPitch(m) )*yawMatrix( -matrixYaw(m) )*m;
|
|
//return atan2f( t.i.y,t.i.x );
|
|
}
|
|
|
|
inline Matrix scaleMatrix(float x, float y, float z)
|
|
{
|
|
return Matrix(Vector(x, 0, 0), Vector(0, y, 0), Vector(0, 0, z));
|
|
}
|
|
|
|
inline Matrix scaleMatrix(const Vector& scale)
|
|
{
|
|
return Matrix(Vector(scale.x, 0, 0), Vector(0, scale.y, 0), Vector(0, 0, scale.z));
|
|
}
|
|
|
|
inline Quat pitchQuat(float p)
|
|
{
|
|
return Quat(cosf(p / -2), Vector(sinf(p / -2), 0, 0));
|
|
}
|
|
|
|
inline Quat yawQuat(float y)
|
|
{
|
|
return Quat(cosf(y / 2), Vector(0, sinf(y / 2), 0));
|
|
}
|
|
|
|
inline Quat rollQuat(float r)
|
|
{
|
|
return Quat(cosf(r / -2), Vector(0, 0, sinf(r / -2)));
|
|
}
|
|
|
|
//inline Quat rotationQuat( float p,float y,float r ){
|
|
// return yawQuat(y)*pitchQuat(p)*rollQuat(r);
|
|
//}
|
|
|
|
Quat rotationQuat(float p, float y, float r);
|
|
|
|
inline Matrix rotationMatrix(float p, float y, float r)
|
|
{
|
|
return yawMatrix(y) * pitchMatrix(p) * rollMatrix(r);
|
|
}
|
|
|
|
inline Matrix rotationMatrix(const Vector& rot)
|
|
{
|
|
return yawMatrix(rot.y) * pitchMatrix(rot.x) * rollMatrix(rot.z);
|
|
}
|
|
|
|
inline float quatPitch(const Quat& q)
|
|
{
|
|
return q.k().pitch();
|
|
}
|
|
|
|
inline float quatYaw(const Quat& q)
|
|
{
|
|
return q.k().yaw();
|
|
}
|
|
|
|
inline float quatRoll(const Quat& q)
|
|
{
|
|
// Vector i=q.i(),j=q.j();
|
|
// return atan2f( i.y,j.y );
|
|
return matrixRoll(q);
|
|
}
|
|
|
|
inline Quat matrixQuat(const Matrix& p)
|
|
{
|
|
Matrix m = p;
|
|
m.orthogonalize();
|
|
float t = m.i.x + m.j.y + m.k.z, w, x, y, z;
|
|
if (t > FLT_EPSILON) {
|
|
t = sqrtf(t + 1) * 2;
|
|
x = (m.k.y - m.j.z) / t;
|
|
y = (m.i.z - m.k.x) / t;
|
|
z = (m.j.x - m.i.y) / t;
|
|
w = t / 4;
|
|
} else if (m.i.x > m.j.y && m.i.x > m.k.z) {
|
|
t = sqrtf(m.i.x - m.j.y - m.k.z + 1) * 2;
|
|
x = t / 4;
|
|
y = (m.j.x + m.i.y) / t;
|
|
z = (m.i.z + m.k.x) / t;
|
|
w = (m.k.y - m.j.z) / t;
|
|
} else if (m.j.y > m.k.z) {
|
|
t = sqrtf(m.j.y - m.k.z - m.i.x + 1) * 2;
|
|
x = (m.j.x + m.i.y) / t;
|
|
y = t / 4;
|
|
z = (m.k.y + m.j.z) / t;
|
|
w = (m.i.z - m.k.x) / t;
|
|
} else {
|
|
t = sqrtf(m.k.z - m.j.y - m.i.x + 1) * 2;
|
|
x = (m.i.z + m.k.x) / t;
|
|
y = (m.k.y + m.j.z) / t;
|
|
z = t / 4;
|
|
w = (m.j.x - m.i.y) / t;
|
|
}
|
|
return Quat(w, Vector(x, y, z));
|
|
}
|
|
|
|
#endif |