2018-10-06 20:39:40 -07:00
|
|
|
#pragma once
|
2015-04-19 13:39:16 -07:00
|
|
|
|
|
|
|
#include "Global.hpp"
|
|
|
|
#include "CAxisAngle.hpp"
|
2016-03-04 15:03:26 -08:00
|
|
|
#include "zeus/CVector3f.hpp"
|
|
|
|
#include "zeus/CVector4f.hpp"
|
2016-04-07 20:35:16 -07:00
|
|
|
#include "zeus/CMatrix3f.hpp"
|
2016-03-04 15:03:26 -08:00
|
|
|
#include "zeus/Math.hpp"
|
2016-09-01 12:38:16 -07:00
|
|
|
#include "zeus/CRelAngle.hpp"
|
2016-09-13 22:10:36 -07:00
|
|
|
#include "zeus/CTransform.hpp"
|
2018-12-07 17:16:50 -08:00
|
|
|
|
2015-11-01 04:04:29 -08:00
|
|
|
#if ZE_ATHENA_TYPES
|
2018-12-07 17:16:50 -08:00
|
|
|
|
2016-03-04 15:03:26 -08:00
|
|
|
#include <athena/IStreamReader.hpp>
|
2018-12-07 17:16:50 -08:00
|
|
|
|
2015-11-01 04:04:29 -08:00
|
|
|
#endif
|
2015-04-19 13:39:16 -07:00
|
|
|
|
2018-12-07 17:16:50 -08:00
|
|
|
namespace zeus {
|
2016-09-16 15:20:12 -07:00
|
|
|
|
2018-12-07 17:16:50 -08:00
|
|
|
static float normalize_angle(float angle) {
|
|
|
|
if (angle > M_PIF)
|
|
|
|
angle -= 2.f * M_PIF;
|
|
|
|
else if (angle < -M_PIF)
|
|
|
|
angle += 2.f * M_PIF;
|
2016-09-16 15:20:12 -07:00
|
|
|
|
2018-12-07 17:16:50 -08:00
|
|
|
return angle;
|
2016-09-16 15:20:12 -07:00
|
|
|
}
|
|
|
|
|
2017-11-11 21:13:05 -08:00
|
|
|
class CNUQuaternion;
|
|
|
|
|
|
|
|
/** Unit quaternion, used for all quaternion arithmetic */
|
2018-12-07 17:16:50 -08:00
|
|
|
class CQuaternion {
|
2015-04-19 13:39:16 -07:00
|
|
|
public:
|
2018-12-07 17:16:50 -08:00
|
|
|
CQuaternion() : mSimd(1.f, 0.f, 0.f, 0.f) {}
|
|
|
|
|
|
|
|
CQuaternion(float wi, float xi, float yi, float zi) : mSimd(wi, xi, yi, zi) {}
|
|
|
|
|
|
|
|
CQuaternion(float xi, float yi, float zi) { fromVector3f(CVector3f(xi, yi, zi)); }
|
|
|
|
|
2018-12-07 21:23:50 -08:00
|
|
|
CQuaternion(float wi, const CVector3f& vec) : mSimd(vec.mSimd.shuffle<0, 0, 1, 2>()) { mSimd[0] = wi; }
|
2018-12-07 17:16:50 -08:00
|
|
|
|
|
|
|
template <typename T>
|
|
|
|
CQuaternion(const simd<T>& s) : mSimd(s) {}
|
2016-04-06 20:39:24 -07:00
|
|
|
|
2015-11-01 04:04:29 -08:00
|
|
|
#if ZE_ATHENA_TYPES
|
2016-01-17 14:40:48 -08:00
|
|
|
|
2018-12-07 17:16:50 -08:00
|
|
|
void readBig(athena::io::IStreamReader& input) {
|
|
|
|
simd_floats f;
|
|
|
|
f[0] = input.readFloatBig();
|
|
|
|
f[1] = input.readFloatBig();
|
|
|
|
f[2] = input.readFloatBig();
|
|
|
|
f[3] = input.readFloatBig();
|
|
|
|
mSimd.copy_from(f);
|
|
|
|
}
|
|
|
|
|
|
|
|
CQuaternion(const atVec4f& vec) : mSimd(vec.simd) {}
|
|
|
|
|
2018-12-07 21:23:50 -08:00
|
|
|
operator atVec4f&() { return *reinterpret_cast<atVec4f*>(this); }
|
2018-12-07 17:16:50 -08:00
|
|
|
|
2018-12-07 21:23:50 -08:00
|
|
|
operator const atVec4f&() const { return *reinterpret_cast<const atVec4f*>(this); }
|
2017-06-08 13:01:41 -07:00
|
|
|
|
|
|
|
#endif
|
2015-04-19 13:39:16 -07:00
|
|
|
|
2018-12-07 17:16:50 -08:00
|
|
|
CQuaternion(const CMatrix3f& mat);
|
|
|
|
|
|
|
|
CQuaternion(const CVector3f& vec) { fromVector3f(vec); }
|
|
|
|
|
|
|
|
CQuaternion(const CVector4f& vec) : mSimd(vec.mSimd) {}
|
|
|
|
|
|
|
|
CQuaternion(const CVector3f& vecA, const CVector3f& vecB) {
|
|
|
|
CVector3f vecAN = vecA.normalized();
|
|
|
|
CVector3f vecBN = vecB.normalized();
|
|
|
|
CVector3f w = vecAN.cross(vecBN);
|
|
|
|
*this = CQuaternion(1.f + vecAN.dot(vecBN), w).normalized();
|
|
|
|
}
|
|
|
|
|
|
|
|
void fromVector3f(const CVector3f& vec);
|
|
|
|
|
|
|
|
CQuaternion& operator=(const CQuaternion& q);
|
|
|
|
|
|
|
|
CQuaternion operator+(const CQuaternion& q) const;
|
|
|
|
|
|
|
|
CQuaternion operator-(const CQuaternion& q) const;
|
|
|
|
|
|
|
|
CQuaternion operator*(const CQuaternion& q) const;
|
|
|
|
|
|
|
|
CQuaternion operator/(const CQuaternion& q) const;
|
|
|
|
|
|
|
|
CQuaternion operator*(float scale) const;
|
|
|
|
|
|
|
|
CQuaternion operator/(float scale) const;
|
|
|
|
|
|
|
|
CQuaternion operator-() const;
|
|
|
|
|
|
|
|
const CQuaternion& operator+=(const CQuaternion& q);
|
|
|
|
|
|
|
|
const CQuaternion& operator-=(const CQuaternion& q);
|
|
|
|
|
|
|
|
const CQuaternion& operator*=(const CQuaternion& q);
|
|
|
|
|
|
|
|
const CQuaternion& operator*=(float scale);
|
|
|
|
|
|
|
|
const CQuaternion& operator/=(float scale);
|
|
|
|
|
|
|
|
float magnitude() const { return std::sqrt(magSquared()); }
|
|
|
|
|
|
|
|
float magSquared() const { return mSimd.dot4(mSimd); }
|
|
|
|
|
|
|
|
void normalize() { *this /= magnitude(); }
|
|
|
|
|
|
|
|
CQuaternion normalized() const { return *this / magnitude(); }
|
|
|
|
|
|
|
|
void invert();
|
|
|
|
|
|
|
|
CQuaternion inverse() const;
|
|
|
|
|
|
|
|
/**
|
|
|
|
* @brief Set the rotation using axis angle notation
|
|
|
|
* @param axis The axis to rotate around
|
|
|
|
* @param angle The magnitude of the rotation in radians
|
|
|
|
* @return
|
|
|
|
*/
|
|
|
|
static CQuaternion fromAxisAngle(const CUnitVector3f& axis, const CRelAngle& angle) {
|
|
|
|
return CQuaternion(std::cos(angle / 2.f), axis * std::sin(angle / 2.f));
|
|
|
|
}
|
|
|
|
|
|
|
|
void rotateX(const CRelAngle& angle) { *this *= fromAxisAngle({1.0f, 0.0f, 0.0f}, angle); }
|
|
|
|
|
|
|
|
void rotateY(const CRelAngle& angle) { *this *= fromAxisAngle({0.0f, 1.0f, 0.0f}, angle); }
|
2016-04-06 20:39:24 -07:00
|
|
|
|
2018-12-07 17:16:50 -08:00
|
|
|
void rotateZ(const CRelAngle& angle) { *this *= fromAxisAngle({0.0f, 0.0f, 1.0f}, angle); }
|
2016-04-06 20:39:24 -07:00
|
|
|
|
2018-12-07 17:16:50 -08:00
|
|
|
static CVector3f rotate(const CQuaternion& rotation, const CAxisAngle& v) {
|
|
|
|
CQuaternion q = rotation * v;
|
|
|
|
q *= rotation.inverse();
|
2016-04-06 20:39:24 -07:00
|
|
|
|
2018-12-07 17:16:50 -08:00
|
|
|
return {q.mSimd.shuffle<1, 2, 3, 3>()};
|
|
|
|
}
|
2016-04-07 19:39:36 -07:00
|
|
|
|
2018-12-07 17:16:50 -08:00
|
|
|
static CQuaternion lookAt(const CUnitVector3f& source, const CUnitVector3f& dest, const CRelAngle& maxAng);
|
2017-03-19 22:06:53 -07:00
|
|
|
|
2018-12-07 17:16:50 -08:00
|
|
|
CVector3f transform(const CVector3f& v) const {
|
|
|
|
CQuaternion r(0.f, v);
|
|
|
|
return (*this * r * inverse()).getImaginary();
|
|
|
|
}
|
2017-07-09 21:55:12 -07:00
|
|
|
|
2018-12-07 17:16:50 -08:00
|
|
|
CQuaternion log() const;
|
2017-10-28 23:21:23 -07:00
|
|
|
|
2018-12-07 17:16:50 -08:00
|
|
|
CQuaternion exp() const;
|
2015-04-19 13:39:16 -07:00
|
|
|
|
2018-12-07 17:16:50 -08:00
|
|
|
CTransform toTransform() const { return CTransform(CMatrix3f(*this)); }
|
2016-04-15 20:23:54 -07:00
|
|
|
|
2018-12-07 17:16:50 -08:00
|
|
|
CTransform toTransform(const zeus::CVector3f& origin) const { return CTransform(CMatrix3f(*this), origin); }
|
2017-11-11 21:13:05 -08:00
|
|
|
|
2018-12-07 21:23:50 -08:00
|
|
|
float dot(const CQuaternion& rhs) const { return mSimd.dot4(rhs.mSimd); }
|
2018-12-07 17:16:50 -08:00
|
|
|
|
|
|
|
static CQuaternion lerp(const CQuaternion& a, const CQuaternion& b, double t);
|
|
|
|
|
|
|
|
static CQuaternion slerp(const CQuaternion& a, const CQuaternion& b, double t);
|
|
|
|
|
|
|
|
static CQuaternion slerpShort(const CQuaternion& a, const CQuaternion& b, double t);
|
|
|
|
|
|
|
|
static CQuaternion nlerp(const CQuaternion& a, const CQuaternion& b, double t);
|
|
|
|
|
|
|
|
static CQuaternion shortestRotationArc(const zeus::CVector3f& v0, const zeus::CVector3f& v1);
|
|
|
|
|
|
|
|
static CQuaternion clampedRotateTo(const zeus::CUnitVector3f& v0, const zeus::CUnitVector3f& v1,
|
|
|
|
const zeus::CRelAngle& angle);
|
|
|
|
|
|
|
|
float roll() const {
|
|
|
|
simd_floats f(mSimd);
|
2018-12-07 21:23:50 -08:00
|
|
|
return std::asin(-2.f * (f[1] * f[3] - f[0] * f[2]));
|
2018-12-07 17:16:50 -08:00
|
|
|
}
|
|
|
|
|
|
|
|
float pitch() const {
|
|
|
|
simd_floats f(mSimd);
|
|
|
|
return std::atan2(2.f * (f[2] * f[3] + f[0] * f[1]), f[0] * f[0] - f[1] * f[1] - f[2] * f[2] + f[3] * f[3]);
|
|
|
|
}
|
|
|
|
|
|
|
|
float yaw() const {
|
|
|
|
simd_floats f(mSimd);
|
2018-12-07 21:23:50 -08:00
|
|
|
return std::atan2(2.f * (f[1] * f[2] + f[0] * f[3]), f[0] * f[0] + f[1] * f[1] - f[2] * f[2] - f[3] * f[3]);
|
2018-12-07 17:16:50 -08:00
|
|
|
}
|
|
|
|
|
|
|
|
CQuaternion buildEquivalent() const;
|
|
|
|
|
|
|
|
zeus::CVector3f getImaginary() const { return mSimd.shuffle<1, 2, 3, 3>(); }
|
|
|
|
|
|
|
|
void setImaginary(const zeus::CVector3f& i) {
|
|
|
|
x() = i.x();
|
|
|
|
y() = i.y();
|
|
|
|
z() = i.z();
|
|
|
|
}
|
|
|
|
|
|
|
|
CRelAngle angleFrom(const zeus::CQuaternion& other);
|
|
|
|
|
|
|
|
simd<float>::reference operator[](size_t idx) {
|
|
|
|
assert(idx < 4);
|
|
|
|
return mSimd[idx];
|
|
|
|
}
|
|
|
|
|
|
|
|
float operator[](size_t idx) const {
|
|
|
|
assert(idx < 4);
|
|
|
|
return mSimd[idx];
|
|
|
|
}
|
|
|
|
|
|
|
|
float w() const { return mSimd[0]; }
|
|
|
|
float x() const { return mSimd[1]; }
|
|
|
|
float y() const { return mSimd[2]; }
|
|
|
|
float z() const { return mSimd[3]; }
|
|
|
|
|
|
|
|
simd<float>::reference w() { return mSimd[0]; }
|
|
|
|
simd<float>::reference x() { return mSimd[1]; }
|
|
|
|
simd<float>::reference y() { return mSimd[2]; }
|
|
|
|
simd<float>::reference z() { return mSimd[3]; }
|
|
|
|
|
|
|
|
simd<float> mSimd;
|
|
|
|
|
|
|
|
static const CQuaternion skNoRotation;
|
|
|
|
|
|
|
|
static CQuaternion fromNUQuaternion(const CNUQuaternion& q);
|
2015-04-19 13:39:16 -07:00
|
|
|
};
|
|
|
|
|
2017-11-11 21:13:05 -08:00
|
|
|
/** Non-unit quaternion, no guarantee that it's normalized.
|
|
|
|
* Converting to CQuaternion will perform normalize operation.
|
|
|
|
*/
|
2018-12-07 17:16:50 -08:00
|
|
|
class CNUQuaternion {
|
2016-08-30 17:32:28 -07:00
|
|
|
public:
|
2018-12-07 17:16:50 -08:00
|
|
|
CNUQuaternion() : mSimd(1.f, 0.f, 0.f, 0.f) {}
|
|
|
|
|
|
|
|
CNUQuaternion(float wi, float xi, float yi, float zi) : mSimd(wi, xi, yi, zi) {}
|
|
|
|
|
2018-12-07 21:23:50 -08:00
|
|
|
CNUQuaternion(float win, const zeus::CVector3f& vec) : mSimd(vec.mSimd.shuffle<0, 0, 1, 2>()) { w() = win; }
|
2018-12-07 17:16:50 -08:00
|
|
|
|
|
|
|
CNUQuaternion(const CQuaternion& other) : mSimd(other.mSimd) {}
|
|
|
|
|
|
|
|
CNUQuaternion(const CMatrix3f& mtx) : CNUQuaternion(CQuaternion(mtx)) {}
|
|
|
|
|
|
|
|
CNUQuaternion(const simd<float>& s) : mSimd(s) {}
|
|
|
|
|
|
|
|
static CNUQuaternion fromAxisAngle(const CUnitVector3f& axis, const CRelAngle& angle) {
|
|
|
|
return CNUQuaternion(CQuaternion::fromAxisAngle(axis, angle));
|
|
|
|
}
|
|
|
|
|
|
|
|
float magnitude() const { return std::sqrt(magSquared()); }
|
|
|
|
|
|
|
|
float magSquared() const { return mSimd.dot4(mSimd); }
|
|
|
|
|
|
|
|
void normalize() {
|
|
|
|
float magDiv = 1.f / magnitude();
|
|
|
|
mSimd *= magDiv;
|
|
|
|
}
|
|
|
|
|
|
|
|
CNUQuaternion normalized() const {
|
|
|
|
float magDiv = 1.f / magnitude();
|
|
|
|
return mSimd * simd<float>(magDiv);
|
|
|
|
}
|
|
|
|
|
|
|
|
CNUQuaternion operator*(const CNUQuaternion& q) const;
|
|
|
|
|
|
|
|
CNUQuaternion operator*(float f) const;
|
|
|
|
|
|
|
|
const CNUQuaternion& operator+=(const CNUQuaternion& q);
|
|
|
|
|
|
|
|
zeus::simd<float>::reference operator[](size_t idx) {
|
|
|
|
assert(idx < 4);
|
|
|
|
return mSimd[idx];
|
|
|
|
}
|
|
|
|
|
|
|
|
float operator[](size_t idx) const {
|
|
|
|
assert(idx < 4);
|
|
|
|
return mSimd[idx];
|
|
|
|
}
|
|
|
|
|
|
|
|
float w() const { return mSimd[0]; }
|
|
|
|
float x() const { return mSimd[1]; }
|
|
|
|
float y() const { return mSimd[2]; }
|
|
|
|
float z() const { return mSimd[3]; }
|
|
|
|
|
|
|
|
simd<float>::reference w() { return mSimd[0]; }
|
|
|
|
simd<float>::reference x() { return mSimd[1]; }
|
|
|
|
simd<float>::reference y() { return mSimd[2]; }
|
|
|
|
simd<float>::reference z() { return mSimd[3]; }
|
|
|
|
|
|
|
|
simd<float> mSimd;
|
2016-08-30 17:32:28 -07:00
|
|
|
};
|
|
|
|
|
2018-12-07 17:16:50 -08:00
|
|
|
inline CQuaternion CQuaternion::fromNUQuaternion(const CNUQuaternion& q) {
|
|
|
|
auto norm = q.normalized();
|
|
|
|
return norm.mSimd;
|
2017-11-11 21:13:05 -08:00
|
|
|
}
|
|
|
|
|
2015-04-19 13:39:16 -07:00
|
|
|
CQuaternion operator+(float lhs, const CQuaternion& rhs);
|
2018-12-07 17:16:50 -08:00
|
|
|
|
2015-04-19 13:39:16 -07:00
|
|
|
CQuaternion operator-(float lhs, const CQuaternion& rhs);
|
2018-12-07 17:16:50 -08:00
|
|
|
|
2015-04-19 13:39:16 -07:00
|
|
|
CQuaternion operator*(float lhs, const CQuaternion& rhs);
|
2018-12-07 17:16:50 -08:00
|
|
|
|
2017-11-11 21:13:05 -08:00
|
|
|
CNUQuaternion operator*(float lhs, const CNUQuaternion& rhs);
|
2018-12-07 21:23:50 -08:00
|
|
|
} // namespace zeus
|