commit c8fa20ddbf8d30fad93f0ebdf3f89ec687e56183 Author: Phillip Stephens Date: Sun Apr 19 13:39:16 2015 -0700 Initial Commit diff --git a/CAxisAngle.hpp b/CAxisAngle.hpp new file mode 100644 index 0000000..53bae55 --- /dev/null +++ b/CAxisAngle.hpp @@ -0,0 +1,24 @@ +#ifndef CAXISANGLE_H +#define CAXISANGLE_H + +#include "Global.hpp" +#include "CVector3f.hpp" + +struct ZE_ALIGN(16) CAxisAngle +{ + ZE_DECLARE_ALIGNED_ALLOCATOR(); + + CAxisAngle() + : axis(CVector3f::skOne), + angle(0) + {} + CAxisAngle(const CVector3f& axis, float angle) + : axis(axis), + angle(angle) + {} + + CVector3f axis; + float angle; +}; + +#endif // CAXISANGLE_H diff --git a/CMatrix3f.cpp b/CMatrix3f.cpp new file mode 100644 index 0000000..0ac3150 --- /dev/null +++ b/CMatrix3f.cpp @@ -0,0 +1,139 @@ +#include "CMatrix3f.hpp" +#include "CQuaternion.hpp" +#include "Global.hpp" + +const CMatrix3f CMatrix3f::skIdentityMatrix3f = CMatrix3f(); + +CMatrix3f::CMatrix3f(const CQuaternion& quat) +{ + CQuaternion nq = quat.normalized(); + float x2 = nq.v[0] * nq.v[0]; + float y2 = nq.v[1] * nq.v[1]; + float z2 = nq.v[2] * nq.v[2]; + + m[0][0] = 1.0 - 2.0 * y2 - 2.0 * z2; + m[1][0] = 2.0 * nq.v[0] * nq.v[1] - 2.0 * nq.v[2] * nq.r; + m[2][0] = 2.0 * nq.v[0] * nq.v[2] + 2.0 * nq.v[1] * nq.r; + + m[0][1] = 2.0 * nq.v[0] * nq.v[1] + 2.0 * nq.v[2] * nq.r; + m[1][1] = 1.0 - 2.0 * x2 - 2.0 * z2; + m[2][1] = 2.0 * nq.v[1] * nq.v[2] - 2.0 * nq.v[0] * nq.r; + + m[0][2] = 2.0 * nq.v[0] * nq.v[2] - 2.0 * nq.v[1] * nq.r; + m[1][2] = 2.0 * nq.v[1] * nq.v[2] + 2.0 * nq.v[0] * nq.r; + m[2][2] = 1.0 - 2.0 * x2 - 2.0 * y2; + + m[0][3] = 0.0f; + m[1][3] = 0.0f; + m[2][3] = 0.0f; +} + +CMatrix3f operator*(const CMatrix3f& lhs, const CMatrix3f& rhs) +{ +#if __SSE__ + unsigned i; + TVectorUnion resVec[3]; + for (i=0 ; i<3 ; ++i) { + resVec[i].mVec128 = + _mm_add_ps(_mm_add_ps( + _mm_mul_ps(lhs[0].mVec128, ze_splat_ps(rhs[i].mVec128, 0)), + _mm_mul_ps(lhs[1].mVec128, ze_splat_ps(rhs[i].mVec128, 1))), + _mm_mul_ps(lhs[2].mVec128, ze_splat_ps(rhs[i].mVec128, 2))); + resVec[i].v[3] = 0.0; + } + return CMatrix3f(resVec[0].mVec128, resVec[1].mVec128, resVec[2].mVec128); +#else + return CMatrix3f(lhs[0][0] * rhs[0][0] + lhs[1][0] * rhs[0][1] + lhs[2][0] * rhs[0][2], + lhs[0][0] * rhs[1][0] + lhs[1][0] * rhs[1][1] + lhs[2][0] * rhs[1][2], + lhs[0][0] * rhs[2][0] + lhs[1][0] * rhs[2][1] + lhs[2][0] * rhs[2][2], + lhs[0][1] * rhs[0][0] + lhs[1][1] * rhs[0][1] + lhs[2][1] * rhs[0][2], + lhs[0][1] * rhs[1][0] + lhs[1][1] * rhs[1][1] + lhs[2][1] * rhs[1][2], + lhs[0][1] * rhs[2][0] + lhs[1][1] * rhs[2][1] + lhs[2][1] * rhs[2][2], + lhs[0][2] * rhs[0][0] + lhs[1][2] * rhs[0][1] + lhs[2][2] * rhs[0][2], + lhs[0][2] * rhs[1][0] + lhs[1][2] * rhs[1][1] + lhs[2][2] * rhs[1][2], + lhs[0][2] * rhs[2][0] + lhs[1][2] * rhs[2][1] + lhs[2][2] * rhs[2][2]); +#endif +} + +void CMatrix3f::transpose() +{ +#if __SSE__ + __m128 zero = _mm_xor_ps(vec[0].mVec128, vec[0].mVec128); + __m128 T0 = _mm_unpacklo_ps(vec[0].mVec128, vec[1].mVec128); + __m128 T2 = _mm_unpacklo_ps(vec[2].mVec128, zero); + __m128 T1 = _mm_unpackhi_ps(vec[0].mVec128, vec[1].mVec128); + __m128 T3 = _mm_unpackhi_ps(vec[2].mVec128, zero); + vec[0].mVec128 = _mm_movelh_ps(T0, T2); + vec[1].mVec128 = _mm_movehl_ps(T2, T0); + vec[2].mVec128 = _mm_movelh_ps(T1, T3); +#else + float tmp; + + tmp = m[0][1]; + m[0][1] = m[1][0]; + m[1][0] = tmp; + + tmp = m[0][2]; + m[0][2] = m[2][0]; + m[2][0] = tmp; + + tmp = m[1][2]; + m[1][2] = m[2][1]; + m[2][1] = tmp; +#endif +} + +CMatrix3f CMatrix3f::transposed() const +{ +#if __SSE__ + __m128 zero = _mm_xor_ps(vec[0].mVec128, vec[0].mVec128); + __m128 T0 = _mm_unpacklo_ps(vec[0].mVec128, vec[1].mVec128); + __m128 T2 = _mm_unpacklo_ps(vec[2].mVec128, zero); + __m128 T1 = _mm_unpackhi_ps(vec[0].mVec128, vec[1].mVec128); + __m128 T3 = _mm_unpackhi_ps(vec[2].mVec128, zero); + return CMatrix3f(_mm_movelh_ps(T0, T2), _mm_movehl_ps(T2, T0), _mm_movelh_ps(T1, T3)); +#else + CMatrix3f ret(*this); + float tmp; + + tmp = ret.m[0][1]; + ret.m[0][1] = ret.m[1][0]; + ret.m[1][0] = tmp; + + tmp = m[0][2]; + ret.m[0][2] = ret.m[2][0]; + ret.m[2][0] = tmp; + + tmp = m[1][2]; + ret.m[1][2] = ret.m[2][1]; + ret.m[2][1] = tmp; + + return ret; +#endif +} + +CMatrix3f CMatrix3f::inverted() const +{ + float det = + m[0][0] * m[1][1] * m[2][2] + + m[1][0] * m[2][1] * m[0][2] + + m[2][0] * m[0][1] * m[1][2] - + m[0][2] * m[1][1] * m[2][0] - + m[1][2] * m[2][1] * m[0][0] - + m[2][2] * m[0][1] * m[1][0]; + + if (det == 0.0) + return CMatrix3f(); + + det = 1.0f / det; + return CMatrix3f((m[1][1]*m[2][2] - m[1][2]*m[2][1]) * det, + -(m[1][0]*m[2][2] - m[1][2]*m[2][0]) * det, + (m[1][0]*m[2][1] - m[1][1]*m[2][0]) * det, + -(m[0][1]*m[2][2] - m[0][2]*m[2][1]) * det, + (m[0][0]*m[2][2] - m[0][2]*m[2][0]) * det, + -(m[0][0]*m[2][1] - m[0][1]*m[2][0]) * det, + (m[0][1]*m[1][2] - m[0][2]*m[1][1]) * det, + -(m[0][0]*m[1][2] - m[0][2]*m[1][0]) * det, + (m[0][0]*m[1][1] - m[0][1]*m[1][0]) * det); +} + diff --git a/CMatrix3f.hpp b/CMatrix3f.hpp new file mode 100644 index 0000000..e1d4e8d --- /dev/null +++ b/CMatrix3f.hpp @@ -0,0 +1,109 @@ +#ifndef CMATRIX3F_HPP +#define CMATRIX3F_HPP + +#include "Global.hpp" +#include "CVector3f.hpp" +#include + +/* Column-major matrix class */ + +class CQuaternion; +class ZE_ALIGN(16) CMatrix3f +{ +public: + ZE_DECLARE_ALIGNED_ALLOCATOR(); + + explicit CMatrix3f(bool zero = false) + { + memset(m, 0, sizeof(m)); + if (!zero) + { + m[0][0] = 1.0; + m[1][1] = 1.0; + m[2][2] = 1.0; + } + } + CMatrix3f(float m00, float m01, float m02, + float m10, float m11, float m12, + float m20, float m21, float m22) + { + m[0][0] = m00, m[1][0] = m01, m[2][0] = m02; + m[0][1] = m10, m[1][1] = m11, m[2][1] = m12; + m[0][2] = m20, m[1][2] = m21, m[2][2] = m22; + } + CMatrix3f(const CVector3f& scaleVec) + { + memset(m, 0, sizeof(m)); + m[0][0] = scaleVec[0]; + m[1][1] = scaleVec[1]; + m[2][2] = scaleVec[2]; + } + CMatrix3f(const CVector3f& u, const CVector3f& m, const CVector3f& w) + {vec[0] = u; vec[1] = m; vec[2] = w;} + CMatrix3f(const CMatrix3f& other) + {vec[0] = other.vec[0]; vec[1] = other.vec[1]; vec[2] = other.vec[2];} +#if __SSE__ + CMatrix3f(const __m128& u, const __m128& m, const __m128& w) + {vec[0].mVec128 = u; vec[1].mVec128 = m; vec[2].mVec128 = w;} +#endif + CMatrix3f(const CVector3f& axis, float angle); + CMatrix3f(const CQuaternion& quat); + + inline CMatrix3f& operator=(const CMatrix3f& other) + { + vec[0] = other.vec[0]; + vec[1] = other.vec[1]; + vec[2] = other.vec[2]; + return *this; + } + + inline CVector3f operator*(const CVector3f& other) const + { +#if __SSE__ + TVectorUnion res; + res.mVec128 = + _mm_add_ps(_mm_add_ps( + _mm_mul_ps(vec[0].mVec128, ze_splat_ps(other.mVec128, 0)), + _mm_mul_ps(vec[1].mVec128, ze_splat_ps(other.mVec128, 1))), + _mm_mul_ps(vec[2].mVec128, ze_splat_ps(other.mVec128, 2))); + return CVector3f(res.mVec128); +#else + return CVector3f(m[0][0] * other.v[0] + m[1][0] * other.v[1] + m[2][0] * other.v[2], + m[0][1] * other.v[0] + m[1][1] * other.v[1] + m[2][1] * other.v[2], + m[0][2] * other.v[0] + m[1][2] * other.v[1] + m[2][2] * other.v[2]); +#endif + } + + inline CVector3f& operator[](int i) + { + assert(0 <= i && i < 3); + return vec[i]; + } + + inline const CVector3f& operator[](int i) const + { + assert(0 <= i && i < 3); + return vec[i]; + } + + static const CMatrix3f skIdentityMatrix3f; + + void transpose(); + CMatrix3f transposed() const; + + inline void invert() {*this = inverted();} + CMatrix3f inverted() const; + + union + { + float m[3][4]; /* 4th row for union-alignment */ + struct + { + CVector3f vec[3]; + }; + }; +}; + +CMatrix3f operator*(const CMatrix3f& lhs, const CMatrix3f& rhs); + +#endif // CMATRIX3F_HPP diff --git a/CPlane.cpp b/CPlane.cpp new file mode 100644 index 0000000..e75806d --- /dev/null +++ b/CPlane.cpp @@ -0,0 +1,3 @@ +#include "CPlane.hpp" + + diff --git a/CPlane.hpp b/CPlane.hpp new file mode 100644 index 0000000..084669a --- /dev/null +++ b/CPlane.hpp @@ -0,0 +1,45 @@ +#ifndef CPLANE_HPP +#define CPLANE_HPP + +#include "Global.hpp" +#include "CVector3f.hpp" + +class ZE_ALIGN(16) CPlane +{ +public: + ZE_DECLARE_ALIGNED_ALLOCATOR(); + + CPlane() + { +#if __SSE__ + mVec128 = _mm_xor_ps(mVec128, mVec128); +#else + a = 0.0f; b = 0.0f; c = 0.0f; d = 0.0f; +#endif + } + CPlane(float a, float b, float c, float d) : a(a), b(b), c(c), d(d) {} + CPlane(const CVector3f& point, float displacement) + { +#if __SSE__ + mVec128 = point.mVec128; +#else + a = point[0]; b = point[1]; c = point[2]; +#endif + d = displacement; + } + +protected: + union + { + struct + { + float a, b, c, d; + }; + float p[4]; +#ifdef __SSE__ + __m128 mVec128; +#endif + }; +}; + +#endif // CPLANE_HPP diff --git a/CProjection.cpp b/CProjection.cpp new file mode 100644 index 0000000..cde161c --- /dev/null +++ b/CProjection.cpp @@ -0,0 +1,74 @@ +#include "CProjection.hpp" +#include + +const TMatrix4f kIdentityMtx4 = +{{ + {1.0, 0.0, 0.0, 0.0}, + {0.0, 1.0, 0.0, 0.0}, + {0.0, 0.0, 1.0, 0.0}, + {0.0, 0.0, 0.0, 1.0} +}}; + +void CProjection::_updateCachedMatrix() +{ + if (m_projType == PROJ_ORTHO) + { + float tmp; + + tmp = 1.0f / (m_ortho.m_right - m_ortho.m_left); + m_mtx.m[0][0] = 2.0f * tmp; + m_mtx.m[1][0] = 0.0f; + m_mtx.m[2][0] = 0.0f; + m_mtx.m[3][0] = -(m_ortho.m_right + m_ortho.m_left) * tmp; + + tmp = 1.0f / (m_ortho.m_top - m_ortho.m_bottom); + m_mtx.m[0][1] = 0.0f; + m_mtx.m[1][1] = 2.0f * tmp; + m_mtx.m[2][1] = 0.0f; + m_mtx.m[3][1] = -(m_ortho.m_top + m_ortho.m_bottom) * tmp; + + tmp = 1.0f / (m_ortho.m_far - m_ortho.m_near); + m_mtx.m[0][2] = 0.0f; + m_mtx.m[1][2] = 0.0f; + m_mtx.m[2][2] = -(1.0f) * tmp; + m_mtx.m[3][2] = -m_ortho.m_far * tmp; + + m_mtx.m[0][3] = 0.0f; + m_mtx.m[1][3] = 0.0f; + m_mtx.m[2][3] = 0.0f; + m_mtx.m[3][3] = 1.0f; + } + else if (m_projType == PROJ_PERSP) + { + float cot,tmp; + float t_fovy = tanf(m_persp.m_fov / 2.0); + + cot = 1.0f / t_fovy; + + m_mtx.m[0][0] = cot/m_persp.m_aspect; + m_mtx.m[1][0] = 0.0f; + m_mtx.m[2][0] = 0.0f; + m_mtx.m[3][0] = 0.0f; + + m_mtx.m[0][1] = 0.0f; + m_mtx.m[1][1] = cot; + m_mtx.m[2][1] = 0.0f; + m_mtx.m[3][1] = 0.0f; + + tmp = 1.0f / (m_persp.m_far - m_persp.m_near); + m_mtx.m[0][2] = 0.0f; + m_mtx.m[1][2] = 0.0f; + m_mtx.m[2][2] = -m_persp.m_far * tmp; + m_mtx.m[3][2] = -(m_persp.m_far * m_persp.m_near) * tmp; + + m_mtx.m[0][3] = 0.0f; + m_mtx.m[1][3] = 0.0f; + m_mtx.m[2][3] = -1.0f; + m_mtx.m[3][3] = 0.0f; + } + else + throw std::runtime_error("attempted to cache invalid projection type"); +} + + + diff --git a/CProjection.hpp b/CProjection.hpp new file mode 100644 index 0000000..6a59122 --- /dev/null +++ b/CProjection.hpp @@ -0,0 +1,142 @@ +#ifndef CPROJECTION_HPP +#define CPROJECTION_HPP + +#include "Global.hpp" +#include + +#define _USE_MATH_DEFINES 1 +#include + +typedef union +{ + float m[4][4]; +#if __SSE__ + __m128 mVec128[4]; +#endif +} TMatrix4f; +static inline void copyMatrix4f(TMatrix4f& dest, const TMatrix4f& src) +{ +#if __SSE__ + dest.mVec128[0] = src.mVec128[0]; + dest.mVec128[1] = src.mVec128[1]; + dest.mVec128[2] = src.mVec128[2]; + dest.mVec128[3] = src.mVec128[3]; +#else + dest.m[0][0] = src.m[0][0]; + dest.m[0][1] = src.m[0][1]; + dest.m[0][2] = src.m[0][2]; + dest.m[0][3] = src.m[0][3]; + dest.m[1][0] = src.m[1][0]; + dest.m[1][1] = src.m[1][1]; + dest.m[1][2] = src.m[1][2]; + dest.m[1][3] = src.m[1][3]; + dest.m[2][0] = src.m[2][0]; + dest.m[2][1] = src.m[2][1]; + dest.m[2][2] = src.m[2][2]; + dest.m[2][3] = src.m[2][3]; + dest.m[3][0] = src.m[3][0]; + dest.m[3][1] = src.m[3][1]; + dest.m[3][2] = src.m[3][2]; + dest.m[3][3] = src.m[3][3]; +#endif +} +extern const TMatrix4f kIdentityMtx4; + +enum EProjType +{ + PROJ_NONE = 0, + PROJ_ORTHO = 1, + PROJ_PERSP = 2 +}; +struct SProjOrtho +{ + float m_top, m_bottom, m_left, m_right, m_near, m_far; + SProjOrtho(float p_top=1.0f, float p_bottom=-1.0f, float p_left=-1.0f, float p_right=1.0f, + float p_near=1.0f, float p_far=-1.0f) : + m_top(p_top), m_bottom(p_bottom), m_left(p_left), m_right(p_right), m_near(p_near), m_far(p_far) {} +}; +struct SProjPersp +{ + float m_fov, m_aspect, m_near, m_far; + SProjPersp(float p_fov=55.0f * M_PI / 180.0f, float p_aspect=1.0f, float p_near=0.1f, float p_far=4096.f) : + m_fov(p_fov), m_aspect(p_aspect), m_near(p_near), m_far(p_far) {} +}; +extern const SProjOrtho kOrthoIdentity; + +class ZE_ALIGN(16) CProjection +{ + void _updateCachedMatrix(); +public: + ZE_DECLARE_ALIGNED_ALLOCATOR(); + + CProjection() + { + m_projType = PROJ_ORTHO; + m_ortho = SProjOrtho(); + copyMatrix4f(m_mtx, kIdentityMtx4); + } + CProjection(const CProjection& other) {*this = other;} + CProjection(const SProjOrtho& ortho) {setOrtho(ortho);} + CProjection(const SProjPersp& persp) {setPersp(persp);} + + inline CProjection& operator=(const CProjection& other) + { + if (this != &other) + { + m_projType = other.m_projType; + m_ortho = other.m_ortho; + copyMatrix4f(m_mtx, other.m_mtx); + } + return *this; + } + + inline void setOrtho(const SProjOrtho& ortho) + {m_projType = PROJ_ORTHO; m_ortho = ortho; _updateCachedMatrix();} + inline void setPersp(const SProjPersp& persp) + {m_projType = PROJ_PERSP; m_persp = persp; _updateCachedMatrix();} + + inline EProjType getType() const {return m_projType;} + inline const SProjOrtho& getOrtho() const + { + if (m_projType != PROJ_ORTHO) + throw std::runtime_error("attempted to access orthographic structure of non-ortho projection"); + return m_ortho; + } + inline const SProjPersp& getPersp() const + { + if (m_projType != PROJ_PERSP) + throw std::runtime_error("attempted to access perspective structure of non-persp projection"); + return m_persp; + } + + inline const TMatrix4f& getCachedMatrix() {return m_mtx;} + +protected: + + /* Projection type */ + enum EProjType m_projType; + + /* Projection intermediate */ + union + { +#ifdef _MSC_VER + struct + { + SProjOrtho m_ortho; + }; + struct + { + SProjPersp m_persp; + }; +#else + SProjOrtho m_ortho; + SProjPersp m_persp; +#endif + }; + + /* Cached projection matrix */ + TMatrix4f m_mtx; + +}; + +#endif // CMATRIX3F_HPP diff --git a/CQuaternion.cpp b/CQuaternion.cpp new file mode 100644 index 0000000..a8ba97a --- /dev/null +++ b/CQuaternion.cpp @@ -0,0 +1,286 @@ +#include "CQuaternion.hpp" +#include + +CQuaternion::CQuaternion() + : r(1.0f) +{ +} + +CQuaternion::CQuaternion(float r, float x, float y, float z) + : r(r), v(x, y, z) +{ +} + +CQuaternion::CQuaternion(float x, float y, float z) +{ + fromVector3f(CVector3f(x, y, z)); +} + +CQuaternion::CQuaternion(const CVector3f& vec) +{ + fromVector3f(vec); +} + +CQuaternion::CQuaternion(float r, const CVector3f& vec) + : r(r), v(vec) +{ +} + +CQuaternion::CQuaternion(Athena::io::IStreamReader& input) +{ + r = input.readFloat(); + v = CVector3f(input); +} + +CQuaternion::~CQuaternion() +{ +} + +void CQuaternion::fromVector3f(const CVector3f& vec) +{ + float cosX = cosf(0.5 * vec.x); + float cosY = cosf(0.5 * vec.y); + float cosZ = cosf(0.5 * vec.z); + + float sinX = sinf(0.5 * vec.x); + float sinY = sinf(0.5 * vec.y); + float sinZ = sinf(0.5 * vec.z); + + r = cosZ * cosY * cosX + sinZ * sinY * sinX; + v.x = cosZ * cosY * sinX - sinZ * sinY * cosX; + v.y = cosZ * sinY * cosX + sinZ * cosY * sinX; + v.z = sinZ * cosY * cosX - cosZ * sinY * sinX; +} + +CQuaternion& CQuaternion::operator=(const CQuaternion& q) +{ + r = q.r; + v = q.v; + return *this; +} + +CQuaternion CQuaternion::operator+(const CQuaternion& q) const +{ + return CQuaternion(r + q.r, v+q.v); +} + +CQuaternion CQuaternion::operator-(const CQuaternion& q) const +{ + return CQuaternion(r - q.r, v-q.v); +} + +CQuaternion CQuaternion::operator*(const CQuaternion& q) const +{ + return CQuaternion(r*q.r - v.dot(q.v), + v.y * q.v.z - v.z * q.v.y + r * q.v.x + v.x*q.r, + v.z * q.v.x - v.x * q.v.z + r * q.v.y + v.y*q.r, + v.x * q.v.y - v.y * q.v.x + r * q.v.z + v.z*q.r); +} + +CQuaternion CQuaternion::operator/(const CQuaternion& q) const +{ + CQuaternion p(q); + p.invert(); + return *this * p; +} + +CQuaternion CQuaternion::operator*(float scale) const +{ + return CQuaternion(r*scale, v*scale); +} + +CQuaternion CQuaternion::operator/(float scale) const +{ + return CQuaternion(r/scale, v/scale); +} + +CQuaternion CQuaternion::operator-() const +{ + return CQuaternion(-r, -v); +} + +const CQuaternion& CQuaternion::operator+=(const CQuaternion& q) +{ + r += q.r; + v += q.v; + return *this; +} + +const CQuaternion& CQuaternion::operator-=(const CQuaternion& q) +{ + r -= q.r; + v -= q.v; + return *this; +} + +const CQuaternion& CQuaternion::operator *=(const CQuaternion& q) +{ + r = r*q.r - v.dot(q.v); + v.x = v.y * q.v.z - v.z * q.v.y + r * q.v.x + v.x*q.r; + v.y = v.z * q.v.x - v.x * q.v.z + r * q.v.y + v.y*q.r; + v.z = v.x * q.v.y - v.y * q.v.x + r * q.v.z + v.z*q.r; + + return *this; +} + +const CQuaternion& CQuaternion::operator *=(float scale) +{ + r *= scale; + v *= scale; + return *this; +} + +const CQuaternion& CQuaternion::operator/=(float scale) +{ + r /= scale; + v /= scale; + return *this; +} + +float CQuaternion::length() const +{ + return sqrt(lengthSquared()); +} + +float CQuaternion::lengthSquared() const +{ + return (r*r + (v.dot(v))); +} + +void CQuaternion::normalize() +{ + *this /= length(); +} + +CQuaternion CQuaternion::normalized() const +{ + return *this/length(); +} + +void CQuaternion::invert() +{ + v = -v; +} + +CQuaternion CQuaternion::inverse() const +{ + return CQuaternion(r, -v); +} + +CAxisAngle CQuaternion::toAxisAngle() +{ + CAxisAngle ret; + ret.angle = acosf(r); + + float thetaInv = 1.0f/sinf(ret.angle); + + ret.axis.x = v.x * thetaInv; + ret.axis.y = v.y * thetaInv; + ret.axis.z = v.z * thetaInv; + + ret.angle *= 2; + + return ret; +} + +CQuaternion CQuaternion::log() const +{ + float a = acosf(r); + float sina = sinf(a); + CQuaternion ret; + + ret.r = 0; + + if (sina > 0) + { + ret.v.x = a * v.x / sina; + ret.v.y = a * v.y / sina; + ret.v.z = a * v.z / sina; + } + else + ret.v = CVector3f(0); + + return ret; +} + +CQuaternion CQuaternion::exp() const +{ + float a = (v.length()); + float sina = sinf(a); + float cosa = cos(a); + CQuaternion ret; + + ret.r = cosa; + if (a > 0) + { + ret.v.x = sina * v.x / a; + ret.v.y = sina * v.y / a; + ret.v.z = sina * v.z / a; + } + else + ret.v = CVector3f(0); + + return ret; +} + +float CQuaternion::dot(const CQuaternion& b) +{ + return v.x * b.v.x + v.y * b.v.y + v.z * b.v.z + r * b.r; +} + +CQuaternion CQuaternion::lerp(CQuaternion& a, CQuaternion& b, double t) +{ + return (a + t * (b - a)); +} + +CQuaternion CQuaternion::nlerp(CQuaternion& a, CQuaternion& b, double t) +{ + return lerp(a, b, t).normalized(); +} + +CQuaternion CQuaternion::slerp(CQuaternion& a, CQuaternion& b, double t) +{ + if (t <= 0.0f) + return a; + if (t >= 1.0f) + return b; + + CQuaternion ret; + + float mag = sqrtf(a.dot(a) * b.dot(b)); + + float prod = a.dot(b) / mag; + + if (fabsf(prod) < 1.0) + { + const double sign = (prod < 0.0) ? -1.0 : 1.0; + + const double theta = acos(sign * prod); + const double s1 = sin (sign * t * theta); + const double d = 1.0 / sin(theta); + const double s0 = sin((1.0 - t) * theta); + + ret.v.x = (float)(a.v.x * s0 + b.v.x * s1) * d; + ret.v.y = (float)(a.v.y * s0 + b.v.y * s1) * d; + ret.v.z = (float)(a.v.z * s0 + b.v.z * s1) * d; + ret.r = (float)(a.r * s0 + b.r * s1) * d; + + return ret; + } + return a; +} + +CQuaternion operator+(float lhs, const CQuaternion& rhs) +{ + return CQuaternion(lhs + rhs.r, lhs * rhs.v); +} + +CQuaternion operator-(float lhs, const CQuaternion& rhs) +{ + return CQuaternion(lhs - rhs.r, lhs * rhs.v); +} + +CQuaternion operator*(float lhs, const CQuaternion& rhs) +{ + return CQuaternion(lhs * rhs.r, lhs * rhs.v); +} diff --git a/CQuaternion.hpp b/CQuaternion.hpp new file mode 100644 index 0000000..ba5a19c --- /dev/null +++ b/CQuaternion.hpp @@ -0,0 +1,98 @@ +#ifndef CQUATERNION_HPP +#define CQUATERNION_HPP + +#include "Global.hpp" +#include "CAxisAngle.hpp" +#include "CVector3f.hpp" +#include +#include + +class ZE_ALIGN(16) CQuaternion +{ +public: + ZE_DECLARE_ALIGNED_ALLOCATOR(); + + CQuaternion(); + CQuaternion(float r, float x, float y, float z); + CQuaternion(float x, float y, float z); + CQuaternion(const CVector3f& vec); + CQuaternion(float r, const CVector3f& vec); + CQuaternion(Athena::io::IStreamReader& input); + ~CQuaternion(); + + 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 length() const; + float lengthSquared() const; + void normalize(); + CQuaternion normalized() const; + 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 inline CQuaternion fromAxisAngle(const CVector3f& axis, float angle) + { + return CQuaternion(cosf(angle/2), axis*sinf(angle/2)); + } + + CAxisAngle toAxisAngle(); + + static inline CVector3f rotate(const CQuaternion& rotation, const CVector3f& v) + { + CQuaternion q = rotation * v; + q *= rotation.inverse(); + + return q.v; + } + + CQuaternion log() const; + + CQuaternion exp() const; + + float dot(const CQuaternion& quat); + + static CQuaternion lerp(CQuaternion& a, CQuaternion& b, double t); + static CQuaternion slerp(CQuaternion& a, CQuaternion& b, double t); + static CQuaternion nlerp(CQuaternion& a, CQuaternion& b, double t); + + inline float roll() const + { + return atan2f(2.0 * (v.x * v.y + r * v.z), r * r + v.x * v.x - v.y * v.y - v.z * v.z); + } + + inline float pitch() const + { + return atan2f(2.0 * (v.y * v.z + r * v.x), r * r - v.x * v.x - v.y * v.y + v.z * v.z); + } + + inline float yaw() const + { + return asinf(-2.0 * (v.x * v.z - r * v.y)); + } + + float r; + CVector3f v; +}; + +CQuaternion operator+(float lhs, const CQuaternion& rhs); +CQuaternion operator-(float lhs, const CQuaternion& rhs); +CQuaternion operator*(float lhs, const CQuaternion& rhs); +#endif // CQUATERNION_HPP diff --git a/CTransform.cpp b/CTransform.cpp new file mode 100644 index 0000000..32b5395 --- /dev/null +++ b/CTransform.cpp @@ -0,0 +1,59 @@ +#include "CTransform.hpp" + +CTransform CTransformFromEditorEuler(const CVector3f& eulerVec) +{ + CTransform result; + double ti, tj, th, ci, cj, ch, si, sj, sh, cc, cs, sc, ss; + + ti = eulerVec[0]; + tj = eulerVec[1]; + th = eulerVec[2]; + + ci = cos(ti); + cj = cos(tj); + ch = cos(th); + si = sin(ti); + sj = sin(tj); + sh = sin(th); + + cc = ci * ch; + cs = ci * sh; + sc = si * ch; + ss = si * sh; + + result.m_basis.m[0][0] = (float)(cj * ch); + result.m_basis.m[1][0] = (float)(sj * sc - cs); + result.m_basis.m[2][0] = (float)(sj * cc + ss); + result.m_basis.m[0][1] = (float)(cj * sh); + result.m_basis.m[1][1] = (float)(sj * ss + cc); + result.m_basis.m[2][1] = (float)(sj * cs - sc); + result.m_basis.m[0][2] = (float)(-sj); + result.m_basis.m[1][2] = (float)(cj * si); + result.m_basis.m[2][2] = (float)(cj * ci); + + return result; +} + +CTransform CTransformFromAxisAngle(const CVector3f& axis, float angle) +{ + CTransform result; + CVector3f axisN = axis.normalized(); + + float c = cosf(angle); + float s = sinf(angle); + float t = 1 - c; + + result.m_basis.m[0][0] = t * axisN.v[0] * axisN.v[0] + c; + result.m_basis.m[1][0] = t * axisN.v[0] * axisN.v[1] - axisN.v[2] * s; + result.m_basis.m[2][0] = t * axisN.v[0] * axisN.v[2] + axisN.v[1] * s; + + result.m_basis.m[0][1] = t * axisN.v[0] * axisN.v[1] + axisN.v[2] * s; + result.m_basis.m[1][1] = t * axisN.v[1] * axisN.v[1] + c; + result.m_basis.m[2][1] = t * axisN.v[1] * axisN.v[2] - axisN.v[0] * s; + + result.m_basis.m[0][2] = t * axisN.v[0] * axisN.v[2] - axisN.v[1] * s; + result.m_basis.m[1][2] = t * axisN.v[1] * axisN.v[2] + axisN.v[0] * s; + result.m_basis.m[2][2] = t * axisN.v[2] * axisN.v[2] + c; + + return result; +} diff --git a/CTransform.hpp b/CTransform.hpp new file mode 100644 index 0000000..13987eb --- /dev/null +++ b/CTransform.hpp @@ -0,0 +1,56 @@ +#ifndef CTRANSFORM_HPP +#define CTRANSFORM_HPP + +#include "Global.hpp" +#include "CMatrix3f.hpp" +#include "CVector3f.hpp" +#include "CProjection.hpp" + +class ZE_ALIGN(16) CTransform +{ +public: + ZE_DECLARE_ALIGNED_ALLOCATOR(); + + CTransform() : m_basis(false) {} + CTransform(const CMatrix3f& basis, const CVector3f& offset=CVector3f::skZero) : + m_basis(basis), m_origin(offset) {} + + inline CTransform operator*(const CTransform& rhs) const + {return CTransform(m_basis * rhs.m_basis, m_origin + (m_basis * rhs.m_origin));} + + inline CTransform inverse() const + { + CMatrix3f inv = m_basis.inverted(); + return CTransform(inv, inv * -m_origin); + } + + inline CVector3f operator*(const CVector3f& other) const + {return m_origin + m_basis * other;} + + inline void toMatrix4f(TMatrix4f& mat) const + { +#if __SSE__ + mat.mVec128[0] = m_basis[0].mVec128; mat.m[0][3] = 0.0f; + mat.mVec128[1] = m_basis[1].mVec128; mat.m[1][3] = 0.0f; + mat.mVec128[2] = m_basis[2].mVec128; mat.m[2][3] = 0.0f; + mat.mVec128[3] = m_origin.mVec128; mat.m[3][3] = 1.0f; +#else + mat.m[0][0] = m_basis[0][0]; mat.m[0][1] = m_basis[0][1]; mat.m[0][2] = m_basis[0][2]; mat.m[0][3] = 0.0f; + mat.m[1][0] = m_basis[1][0]; mat.m[1][1] = m_basis[1][1]; mat.m[1][2] = m_basis[1][2]; mat.m[1][3] = 0.0f; + mat.m[2][0] = m_basis[2][0]; mat.m[2][1] = m_basis[2][1]; mat.m[2][2] = m_basis[2][2]; mat.m[2][3] = 0.0f; + mat.m[3][0] = m_origin[0]; mat.m[3][1] = m_origin[1]; mat.m[3][2] = m_origin[2]; mat.m[3][3] = 1.0f; +#endif + } + + CMatrix3f m_basis; + CVector3f m_origin; +}; + +static inline CTransform CTransformFromScaleVector(const CVector3f& scale) +{ + return CTransform(CMatrix3f(scale)); +} +CTransform CTransformFromEditorEuler(const CVector3f& eulerVec); +CTransform CTransformFromAxisAngle(const CVector3f& axis, float angle); + +#endif // CTRANSFORM_HPP diff --git a/CVector3d.cpp b/CVector3d.cpp new file mode 100644 index 0000000..516055c --- /dev/null +++ b/CVector3d.cpp @@ -0,0 +1,108 @@ +#include "CVector3d.hpp" +#include +#include + +CVector3d::CVector3d() +{ + memset(&v, 0, sizeof(v)); +} + +CVector3d::CVector3d(double x, double y, double z) + : x(x), + y(y), + z(z) +{ +} + +CVector3d::CVector3d(Athena::io::IStreamReader& input) +{ + x = input.readDouble(); + y = input.readDouble(); + z = input.readDouble(); +} + +CVector3d::~CVector3d() +{ +} + +bool CVector3d::operator ==(const CVector3d& rhs) +{ + return (x == rhs.x && y == rhs.y && z == rhs.z); +} + +CVector3d CVector3d::operator+(const CVector3d& rhs) +{ + return CVector3d(x + rhs.x, y + rhs.y, z + rhs.z); +} + +CVector3d CVector3d::operator-(const CVector3d& rhs) +{ + return CVector3d(x - rhs.x, y - rhs.y, z - rhs.z); +} + +CVector3d CVector3d::operator-() const +{ + return CVector3d(-x, -y, -z); +} + +CVector3d CVector3d::operator*(const CVector3d& rhs) +{ + return CVector3d(x * rhs.x, y * rhs.y, z * rhs.z); +} + +CVector3d CVector3d::operator/(const CVector3d& rhs) +{ + return CVector3d(x / rhs.x, y / rhs.y, z / rhs.z); +} + +CVector3d CVector3d::operator+(double val) +{ + return CVector3d(x + val, y + val, z + val); +} + +CVector3d CVector3d::operator-(double val) +{ + return CVector3d(x - val, y - val, z - val); +} + +CVector3d CVector3d::operator*(double val) +{ + return CVector3d(x * val, y * val, z * val); +} + +CVector3d CVector3d::operator/(double val) +{ + return CVector3d(x / val, y / val, z / val); +} + +CVector3d CVector3d::normalized() +{ + CVector3d ret; + double mag = magnitude(); + + ret.x = x/mag; + ret.y = y/mag; + ret.z = z/mag; + + return ret; +} + +CVector3d CVector3d::cross(const CVector3d& rhs) const +{ + return CVector3d(y * rhs.z - z * rhs.y, z * rhs.x - x * rhs.z, x * rhs.y - y * rhs.x); +} + +double CVector3d::dot(const CVector3d& rhs) const +{ + return (x * rhs.x) + (y * rhs.y) + (z * rhs.z); +} + +double CVector3d::magnitude() const +{ + return sqrt(x*x + y*y + z*z); +} + +CVector3f CVector3d::asVector3f() +{ + return CVector3f((float)x, (float)y, (float)z); +} diff --git a/CVector3d.hpp b/CVector3d.hpp new file mode 100644 index 0000000..e125e28 --- /dev/null +++ b/CVector3d.hpp @@ -0,0 +1,45 @@ +#ifndef CVECTOR3D_HPP +#define CVECTOR3D_HPP + +#include + +#include "CVector3f.hpp" + +class ZE_ALIGN(16) CVector3d +{ +public: + ZE_DECLARE_ALIGNED_ALLOCATOR(); + + CVector3d(); + CVector3d(const CVector3f& vec3); + CVector3d(double x, double y, double z); + CVector3d(Athena::io::IStreamReader& input); + ~CVector3d(); + + bool operator ==(const CVector3d& rhs); + CVector3d operator+(const CVector3d& rhs); + CVector3d operator-(const CVector3d& rhs); + CVector3d operator-() const; + CVector3d operator*(const CVector3d& rhs); + CVector3d operator/(const CVector3d& rhs); + CVector3d operator+(double val); + CVector3d operator-(double val); + CVector3d operator*(double val); + CVector3d operator/(double val); + CVector3d normalized(); + CVector3d cross(const CVector3d& rhs) const; + double dot(const CVector3d& rhs) const; + double magnitude() const; + + CVector3f asVector3f(); + union + { + struct + { + double x, y, z; + }; + __m128 v; + }; +}; + +#endif // CVECTOR3D_HPP diff --git a/CVector3f.cpp b/CVector3f.cpp new file mode 100644 index 0000000..42efdfd --- /dev/null +++ b/CVector3f.cpp @@ -0,0 +1,74 @@ +#include "CVector3f.hpp" +#include +#include +#include +#include "Math.hpp" + +const CVector3f CVector3f::skOne = CVector3f(1); +const CVector3f CVector3f::skZero; + +void CVector3f::normalize() +{ + float mag = length(); + assert(mag != 0.0); + + x /= mag; + y /= mag; + z /= mag; +} + +CVector3f CVector3f::normalized() const +{ + CVector3f ret; + float mag = length(); + assert(mag != 0.0); + + ret.x = x/mag; + ret.y = y/mag; + ret.z = z/mag; + + return ret; +} + +float CVector3f::getAngleDiff(const CVector3f& a, const CVector3f& b) +{ + float mag1 = a.length(); + float mag2 = b.length(); + + if (!mag1 || !mag2) + return 0; + + float theta = acosf(a.dot(b) / (mag1 * mag2)); + return theta; +} + +CVector3f CVector3f::slerp(const CVector3f& a, const CVector3f& b, float t) +{ + if (t <= 0.0f) + return a; + if (t >= 1.0f) + return b; + + CVector3f ret; + + float mag = sqrtf(a.dot(a) * b.dot(b)); + + float prod = a.dot(b) / mag; + + if (fabsf(prod) < 1.0) + { + const double sign = (prod < 0.0) ? -1.0 : 1.0; + + const double theta = acos(sign * prod); + const double s1 = sin (sign * t * theta); + const double d = 1.0 / sin(theta); + const double s0 = sin((1.0 - t) * theta); + + ret.x = (float)(a.x * s0 + b.x * s1) * d; + ret.y = (float)(a.y * s0 + b.y * s1) * d; + ret.z = (float)(a.z * s0 + b.z * s1) * d; + + return ret; + } + return a; +} diff --git a/CVector3f.hpp b/CVector3f.hpp new file mode 100644 index 0000000..975f233 --- /dev/null +++ b/CVector3f.hpp @@ -0,0 +1,283 @@ +#ifndef CVECTOR3F_HPP +#define CVECTOR3F_HPP + +#include "Global.hpp" +#include +#include + +typedef union +{ + float v[4]; +#if __SSE__ + __m128 mVec128; +#endif +} TVectorUnion; + +class ZE_ALIGN(16) CVector3f +{ +public: + ZE_DECLARE_ALIGNED_ALLOCATOR(); + + CVector3f() {zeroOut();} +#if __SSE__ + CVector3f(const __m128& mVec128) : mVec128(mVec128) {v[3] = 0.0f;} +#endif + CVector3f(float xyz) {splat(xyz);} + CVector3f(float x, float y, float z) {v[0] = x; v[1] = y; v[2] = z; v[3] = 0.0;} + CVector3f(Athena::io::IStreamReader& input) + { + x = input.readFloat(); + y = input.readFloat(); + z = input.readFloat(); + v[3] = 0.0f; + } + + inline bool operator ==(const CVector3f& rhs) const + {return (x == rhs.x && y == rhs.y && z == rhs.z);} + inline bool operator !=(const CVector3f& rhs) const + {return !(x == rhs.x && y == rhs.y && z == rhs.z);} + inline CVector3f operator+(const CVector3f& rhs) const + { +#if __SSE__ + return CVector3f(_mm_add_ps(mVec128, rhs.mVec128)); +#else + return CVector3f(x + rhs.x, y + rhs.y, z + rhs.z); +#endif + } + inline CVector3f operator-(const CVector3f& rhs) const + { +#if __SSE__ + return CVector3f(_mm_sub_ps(mVec128, rhs.mVec128)); +#else + return CVector3f(x - rhs.x, y - rhs.y, z - rhs.z); +#endif + } + inline CVector3f operator-() const + { +#if __SSE__ + return CVector3f(_mm_sub_ps(_mm_xor_ps(mVec128, mVec128), mVec128)); +#else + return CVector3f(-x, -y, -z); +#endif + } + inline CVector3f operator*(const CVector3f& rhs) const + { +#if __SSE__ + return CVector3f(_mm_mul_ps(mVec128, rhs.mVec128)); +#else + return CVector3f(x * rhs.x, y * rhs.y, z * rhs.z); +#endif + } + inline CVector3f operator/(const CVector3f& rhs) const + { +#if __SSE__ + return CVector3f(_mm_div_ps(mVec128, rhs.mVec128)); +#else + return CVector3f(x / rhs.x, y / rhs.y, z / rhs.z); +#endif + } + inline CVector3f operator+(float val) const + { +#if __SSE__ + TVectorUnion splat = {{val, val, val, 0.0}}; + return CVector3f(_mm_add_ps(mVec128, splat.mVec128)); +#else + return CVector3f(x + val, y + val, z + val); +#endif + } + inline CVector3f operator-(float val) const + { +#if __SSE__ + TVectorUnion splat = {{val, val, val, 0.0}}; + return CVector3f(_mm_sub_ps(mVec128, splat.mVec128)); +#else + return CVector3f(x - val, y - val, z - val); +#endif + } + inline CVector3f operator*(float val) const + { +#if __SSE__ + TVectorUnion splat = {{val, val, val, 0.0}}; + return CVector3f(_mm_mul_ps(mVec128, splat.mVec128)); +#else + return CVector3f(x * val, y * val, z * val); +#endif + } + inline CVector3f operator/(float val) const + { +#if __SSE__ + TVectorUnion splat = {{val, val, val, 0.0}}; + return CVector3f(_mm_div_ps(mVec128, splat.mVec128)); +#else + return CVector3f(x / val, y / val, z / val); +#endif + } + inline const CVector3f& operator +=(const CVector3f& rhs) + { +#if __SSE__ + mVec128 = _mm_add_ps(mVec128, rhs.mVec128); +#else + x += rhs.x; y += rhs.y; z += rhs.z; +#endif + return *this; + } + inline const CVector3f& operator -=(const CVector3f& rhs) + { +#if __SSE__ + mVec128 = _mm_sub_ps(mVec128, rhs.mVec128); +#else + x -= rhs.x; y -= rhs.y; z -= rhs.z; +#endif + return *this; + } + inline const CVector3f& operator *=(const CVector3f& rhs) + { +#if __SSE__ + mVec128 = _mm_mul_ps(mVec128, rhs.mVec128); +#else + x *= rhs.x; y *= rhs.y; z *= rhs.z; +#endif + return *this; + } + inline const CVector3f& operator /=(const CVector3f& rhs) + { +#if __SSE__ + mVec128 = _mm_div_ps(mVec128, rhs.mVec128); +#else + x /= rhs.x; y /= rhs.y; z /= rhs.z; +#endif + return *this; + } + void normalize(); + CVector3f normalized() const; + inline CVector3f cross(const CVector3f& rhs) const + { + return CVector3f(y * rhs.z - z * rhs.y, z * rhs.x - x * rhs.z, x * rhs.y - y * rhs.x); + } + inline float dot(const CVector3f& rhs) const + { +#if __SSE4_1__ + TVectorUnion result; + result.mVec128 = _mm_dp_ps(mVec128, rhs.mVec128, 0x71); + return result.v[0]; +#elif __SSE__ + TVectorUnion result; + result.mVec128 = _mm_mul_ps(mVec128, rhs.mVec128); + return result.v[0] + result.v[1] + result.v[2]; +#else + return (x * rhs.x) + (y * rhs.y) + (z * rhs.z); +#endif + } + inline float lengthSquared() const + { +#if __SSE4_1__ + TVectorUnion result; + result.mVec128 = _mm_dp_ps(mVec128, rhs.mVec128, 0x71); + return result.v[0]; +#elif __SSE__ + TVectorUnion result; + result.mVec128 = _mm_mul_ps(mVec128, mVec128); + return result.v[0] + result.v[1] + result.v[2]; +#else + return x*x + y*y + z*z; +#endif + } + inline float length() const + { + return sqrtf(lengthSquared()); + } + + inline void zeroOut() + { +#if __SSE__ + mVec128 = _mm_xor_ps(mVec128, mVec128); +#else + v[0] = 0.0; v[1] = 0.0; v[2] = 0.0; v[3] = 0.0; +#endif + } + + inline void splat(float xyz) + { +#if __SSE__ + TVectorUnion splat = {{xyz, xyz, xyz, 0.0f}}; + mVec128 = splat.mVec128; +#else + v[0] = xyz; v[1] = xyz; v[2] = xyz; v[3] = 0.0f; +#endif + } + + static float getAngleDiff(const CVector3f& a, const CVector3f& b); + + static inline CVector3f lerp(const CVector3f& a, const CVector3f& b, float t) + { + return (a + (b - a) * t); + } + static inline CVector3f nlerp(const CVector3f& a, const CVector3f& b, float t) + { + return lerp(a, b, t).normalized(); + } + static CVector3f slerp(const CVector3f& a, const CVector3f& b, float t); + + + inline float& operator[](size_t idx) {return (&x)[idx];} + inline const float& operator[](size_t idx) const {return (&x)[idx];} + + + union + { + struct + { + float x, y, z; + }; + float v[4]; +#if __SSE__ + __m128 mVec128; +#endif + }; + + static const CVector3f skOne; + static const CVector3f skZero; +}; + + +static inline CVector3f operator+(float lhs, const CVector3f& rhs) +{ +#if __SSE__ + TVectorUnion splat = {{lhs, lhs, lhs, 0.0}}; + return CVector3f(_mm_add_ps(splat.mVec128, rhs.mVec128)); +#else + return CVector3f(lhs + rhs.x, lhs + rhs.y, lhs + rhs.z); +#endif +} + +static inline CVector3f operator-(float lhs, const CVector3f& rhs) +{ +#if __SSE__ + TVectorUnion splat = {{lhs, lhs, lhs, 0.0}}; + return CVector3f(_mm_sub_ps(splat.mVec128, rhs.mVec128)); +#else + return CVector3f(lhs - rhs.x, lhs - rhs.y, lhs - rhs.z); +#endif +} + +static inline CVector3f operator*(float lhs, const CVector3f& rhs) +{ +#if __SSE__ + TVectorUnion splat = {{lhs, lhs, lhs, 0.0}}; + return CVector3f(_mm_mul_ps(splat.mVec128, rhs.mVec128)); +#else + return CVector3f(lhs * rhs.x, lhs * rhs.y, lhs * rhs.z); +#endif +} + +static inline CVector3f operator/(float lhs, const CVector3f& rhs) +{ +#if __SSE__ + TVectorUnion splat = {{lhs, lhs, lhs, 0.0}}; + return CVector3f(_mm_div_ps(splat.mVec128, rhs.mVec128)); +#else + return CVector3f(lhs / rhs.x, lhs / rhs.y, lhs / rhs.z); +#endif +} + +#endif // CVECTOR3F_HPP diff --git a/Global.hpp b/Global.hpp new file mode 100644 index 0000000..8308794 --- /dev/null +++ b/Global.hpp @@ -0,0 +1,51 @@ +#ifndef ZEUS_GLOBAL_HPP +#define ZEUS_GLOBAL_HPP + +#if _M_IX86_FP >= 1 || _M_X64 +# define __SSE__ 1 +#endif + +#if __SSE__ +# include +# ifdef _MSC_VER +# define ZE_ALIGN(x) __declspec(align(x)) +# else +# include +# define ZE_ALIGN(x) __attribute__((aligned(x))) +# endif +# define ZE_SHUFFLE(x,y,z,w) ((w)<<6 | (z)<<4 | (y)<<2 | (x)) +# define zeAlloc(sz, align) _mm_malloc(sz, align) +# define zeFree(ptr) _mm_free(ptr) +#else +# define ZE_ALIGN(x) +#endif + +#if __SSE__ +# define ZE_DECLARE_ALIGNED_ALLOCATOR() \ + inline void* operator new(size_t sizeInBytes) { return zeAlloc(sizeInBytes,16); } \ + inline void operator delete(void* ptr) { zeFree(ptr); } \ + inline void* operator new(size_t, void* ptr) { return ptr; } \ + inline void operator delete(void*, void*) { } \ + inline void* operator new[](size_t sizeInBytes) { return zeAlloc(sizeInBytes,16); } \ + inline void operator delete[](void* ptr) { zeFree(ptr); } \ + inline void* operator new[](size_t, void* ptr) { return ptr; } \ + inline void operator delete[](void*, void*) { } +#else +# define ZE_DECLARE_ALIGNED_ALLOCATOR() +#endif + +#if __SSE__ +# define ZE_SHUFFLE(x,y,z,w) ((w)<<6 | (z)<<4 | (y)<<2 | (x)) +# define ze_pshufd_ps( _a, _mask ) _mm_shuffle_ps((_a), (_a), (_mask) ) +# define ze_splat3_ps( _a, _i ) ze_pshufd_ps((_a), ZE_SHUFFLE(_i,_i,_i, 3) ) +# define ze_splat_ps( _a, _i ) ze_pshufd_ps((_a), ZE_SHUFFLE(_i,_i,_i,_i) ) +# if _WIN32 +# define zeCastiTo128f(a) (_mm_castsi128_ps(a)) +# else +# define zeCastiTo128f(a) ((__m128) (a)) +# endif +#endif + + +#endif //ZEUS_GLOBAL_HPP + diff --git a/Math.cpp b/Math.cpp new file mode 100644 index 0000000..15b3608 --- /dev/null +++ b/Math.cpp @@ -0,0 +1,21 @@ +#include "Math.hpp" + +const CVector3f Math::kUpVec(0.0, 0.0, 1.0); +const CVector3f Math::kRadToDegVec(180.0f / M_PI); +const CVector3f Math::kDegToRadVec(M_PI / 180.0f); + +CTransform Math::lookAt(const CVector3f& pos, const CVector3f& lookPos, const CVector3f& up) +{ + CVector3f vLook,vRight,vUp; + + vLook = pos - lookPos; + vLook.normalize(); + + vRight = up.cross(vLook); + vRight.normalize(); + + vUp = vLook.cross(vRight); + + CMatrix3f rmBasis(vRight, vUp, vLook); + return CTransform(rmBasis.transposed(), CVector3f(-pos.dot(vRight), -pos.dot(vUp), -pos.dot(vLook))); +} diff --git a/Math.hpp b/Math.hpp new file mode 100644 index 0000000..76c4d2e --- /dev/null +++ b/Math.hpp @@ -0,0 +1,34 @@ +#ifndef MATH_HPP +#define MATH_HPP + +#define _USE_MATH_DEFINES 1 +#include +#include "CVector3f.hpp" +#include "CTransform.hpp" + +/* Macros for min/max. */ +#ifndef MIN +#define MIN(a,b) (((a)<(b))?(a):(b)) +#endif /* MIN */ +#ifndef MAX +#define MAX(a,b) (((a)>(b))?(a):(b)) +#endif /* MAX */ + +namespace Math +{ + inline void clamp(float& val, float min, float max) {val = MAX(min, MIN(max, val));} + inline void clamp(int& val, int min, int max) {val = MAX(min, MIN(max, val));} + + inline float radToDeg(float rad) {return rad * 180.f / M_PI;} + inline float degToRad(float deg) {return deg * M_PI / 180;} + + extern const CVector3f kRadToDegVec; + extern const CVector3f kDegToRadVec; + inline CVector3f radToDeg(CVector3f rad) {return rad * kRadToDegVec;} + inline CVector3f degToRad(CVector3f deg) {return deg * kDegToRadVec;} + + extern const CVector3f kUpVec; + CTransform lookAt(const CVector3f& pos, const CVector3f& lookPos, const CVector3f& up=kUpVec); +} + +#endif // MATH_HPP diff --git a/MathLib.hpp b/MathLib.hpp new file mode 100644 index 0000000..c7500cb --- /dev/null +++ b/MathLib.hpp @@ -0,0 +1,15 @@ +#ifndef __MATHLIB_HPP +#define __MATHLIB_HPP + +#include "CAxisAngle.hpp" +#include "CMatrix3f.hpp" +#include "CProjection.hpp" +#include "CTransform.hpp" +#include "CQuaternion.hpp" +#include "CVector3d.hpp" +#include "CVector3f.hpp" +#include "CPlane.hpp" +#include "Global.hpp" +#include "Math.hpp" + +#endif // __MATHLIB_HPP diff --git a/MathLib.pri b/MathLib.pri new file mode 100644 index 0000000..eef3ef8 --- /dev/null +++ b/MathLib.pri @@ -0,0 +1,24 @@ +SOURCES += \ + $$PWD/CVector3f.cpp \ + $$PWD/CVector3d.cpp \ + $$PWD/Math.cpp \ + $$PWD/CQuaternion.cpp \ + $$PWD/CMatrix3f.cpp \ + $$PWD/CProjection.cpp \ + $$PWD/CPlane.cpp \ + $$PWD/CTransform.cpp + +HEADERS += \ + $$PWD/CVector3f.hpp \ + $$PWD/CVector3d.hpp \ + $$PWD/Math.hpp \ + $$PWD/CQuaternion.hpp \ + $$PWD/CMatrix3f.hpp \ + $$PWD/CProjection.hpp \ + $$PWD/CAxisAngle.hpp \ + $$PWD/CPlane.hpp \ + $$PWD/CTransform.hpp \ + $$PWD/Global.hpp \ + $$PWD/MathLib.hpp + +INCLUDEPATH += $$PWD diff --git a/MathLib.pro b/MathLib.pro new file mode 100644 index 0000000..80a775b --- /dev/null +++ b/MathLib.pro @@ -0,0 +1,14 @@ +TEMPLATE = app +CONFIG += console +CONFIG -= app_bundle +CONFIG -= qt + +gcc:QMAKE_CXXFLAGS += -msse -msse2 -msse3 +msvc:QMAKE_CXXFLAGS += /arch:SSE2 +win32:DEFINES -= WIN64 + +include (../Athena/AthenaCore.pri) +include (MathLib.pri) + +SOURCES += main.cpp + diff --git a/deployment.pri b/deployment.pri new file mode 100644 index 0000000..5f1749f --- /dev/null +++ b/deployment.pri @@ -0,0 +1,191 @@ +# This file was generated by an application wizard of Qt Creator. +# The code below handles deployment to Android and Maemo, aswell as copying +# of the application data to shadow build directories on desktop. +# It is recommended not to modify this file, since newer versions of Qt Creator +# may offer an updated version of it. + +defineTest(qtcAddDeployment) { +for(deploymentfolder, DEPLOYMENTFOLDERS) { + item = item$${deploymentfolder} + greaterThan(QT_MAJOR_VERSION, 4) { + itemsources = $${item}.files + } else { + itemsources = $${item}.sources + } + $$itemsources = $$eval($${deploymentfolder}.source) + itempath = $${item}.path + $$itempath= $$eval($${deploymentfolder}.target) + export($$itemsources) + export($$itempath) + DEPLOYMENT += $$item +} + +MAINPROFILEPWD = $$PWD + +android-no-sdk { + for(deploymentfolder, DEPLOYMENTFOLDERS) { + item = item$${deploymentfolder} + itemfiles = $${item}.files + $$itemfiles = $$eval($${deploymentfolder}.source) + itempath = $${item}.path + $$itempath = /data/user/qt/$$eval($${deploymentfolder}.target) + export($$itemfiles) + export($$itempath) + INSTALLS += $$item + } + + target.path = /data/user/qt + + export(target.path) + INSTALLS += target +} else:android { + for(deploymentfolder, DEPLOYMENTFOLDERS) { + item = item$${deploymentfolder} + itemfiles = $${item}.files + $$itemfiles = $$eval($${deploymentfolder}.source) + itempath = $${item}.path + $$itempath = /assets/$$eval($${deploymentfolder}.target) + export($$itemfiles) + export($$itempath) + INSTALLS += $$item + } + + x86 { + target.path = /libs/x86 + } else: armeabi-v7a { + target.path = /libs/armeabi-v7a + } else { + target.path = /libs/armeabi + } + + export(target.path) + INSTALLS += target +} else:win32 { + copyCommand = + for(deploymentfolder, DEPLOYMENTFOLDERS) { + source = $$MAINPROFILEPWD/$$eval($${deploymentfolder}.source) + source = $$replace(source, /, \\) + sourcePathSegments = $$split(source, \\) + target = $$OUT_PWD/$$eval($${deploymentfolder}.target)/$$last(sourcePathSegments) + target = $$replace(target, /, \\) + target ~= s,\\\\\\.?\\\\,\\, + !isEqual(source,$$target) { + !isEmpty(copyCommand):copyCommand += && + isEqual(QMAKE_DIR_SEP, \\) { + copyCommand += $(COPY_DIR) \"$$source\" \"$$target\" + } else { + source = $$replace(source, \\\\, /) + target = $$OUT_PWD/$$eval($${deploymentfolder}.target) + target = $$replace(target, \\\\, /) + copyCommand += test -d \"$$target\" || mkdir -p \"$$target\" && cp -r \"$$source\" \"$$target\" + } + } + } + !isEmpty(copyCommand) { + copyCommand = @echo Copying application data... && $$copyCommand + copydeploymentfolders.commands = $$copyCommand + first.depends = $(first) copydeploymentfolders + export(first.depends) + export(copydeploymentfolders.commands) + QMAKE_EXTRA_TARGETS += first copydeploymentfolders + } +} else:ios { + copyCommand = + for(deploymentfolder, DEPLOYMENTFOLDERS) { + source = $$MAINPROFILEPWD/$$eval($${deploymentfolder}.source) + source = $$replace(source, \\\\, /) + target = $CODESIGNING_FOLDER_PATH/$$eval($${deploymentfolder}.target) + target = $$replace(target, \\\\, /) + sourcePathSegments = $$split(source, /) + targetFullPath = $$target/$$last(sourcePathSegments) + targetFullPath ~= s,/\\.?/,/, + !isEqual(source,$$targetFullPath) { + !isEmpty(copyCommand):copyCommand += && + copyCommand += mkdir -p \"$$target\" + copyCommand += && cp -r \"$$source\" \"$$target\" + } + } + !isEmpty(copyCommand) { + copyCommand = echo Copying application data... && $$copyCommand + !isEmpty(QMAKE_POST_LINK): QMAKE_POST_LINK += ";" + QMAKE_POST_LINK += "$$copyCommand" + export(QMAKE_POST_LINK) + } +} else:unix { + maemo5 { + desktopfile.files = $${TARGET}.desktop + desktopfile.path = /usr/share/applications/hildon + icon.files = $${TARGET}64.png + icon.path = /usr/share/icons/hicolor/64x64/apps + } else:!isEmpty(MEEGO_VERSION_MAJOR) { + desktopfile.files = $${TARGET}_harmattan.desktop + desktopfile.path = /usr/share/applications + icon.files = $${TARGET}80.png + icon.path = /usr/share/icons/hicolor/80x80/apps + } else { # Assumed to be a Desktop Unix + copyCommand = + for(deploymentfolder, DEPLOYMENTFOLDERS) { + source = $$MAINPROFILEPWD/$$eval($${deploymentfolder}.source) + source = $$replace(source, \\\\, /) + macx { + target = $$OUT_PWD/$${TARGET}.app/Contents/Resources/$$eval($${deploymentfolder}.target) + } else { + target = $$OUT_PWD/$$eval($${deploymentfolder}.target) + } + target = $$replace(target, \\\\, /) + sourcePathSegments = $$split(source, /) + targetFullPath = $$target/$$last(sourcePathSegments) + targetFullPath ~= s,/\\.?/,/, + !isEqual(source,$$targetFullPath) { + !isEmpty(copyCommand):copyCommand += && + copyCommand += $(MKDIR) \"$$target\" + copyCommand += && $(COPY_DIR) \"$$source\" \"$$target\" + } + } + !isEmpty(copyCommand) { + copyCommand = @echo Copying application data... && $$copyCommand + copydeploymentfolders.commands = $$copyCommand + first.depends = $(first) copydeploymentfolders + export(first.depends) + export(copydeploymentfolders.commands) + QMAKE_EXTRA_TARGETS += first copydeploymentfolders + } + } + !isEmpty(target.path) { + installPrefix = $${target.path} + } else { + installPrefix = /opt/$${TARGET} + } + for(deploymentfolder, DEPLOYMENTFOLDERS) { + item = item$${deploymentfolder} + itemfiles = $${item}.files + $$itemfiles = $$eval($${deploymentfolder}.source) + itempath = $${item}.path + $$itempath = $${installPrefix}/$$eval($${deploymentfolder}.target) + export($$itemfiles) + export($$itempath) + INSTALLS += $$item + } + + !isEmpty(desktopfile.path) { + export(icon.files) + export(icon.path) + export(desktopfile.files) + export(desktopfile.path) + INSTALLS += icon desktopfile + } + + isEmpty(target.path) { + target.path = $${installPrefix}/bin + export(target.path) + } + INSTALLS += target +} + +export (ICON) +export (INSTALLS) +export (DEPLOYMENT) +export (LIBS) +export (QMAKE_EXTRA_TARGETS) +} + diff --git a/main.cpp b/main.cpp new file mode 100644 index 0000000..53ad7d2 --- /dev/null +++ b/main.cpp @@ -0,0 +1,37 @@ +#include +#include "CVector3d.hpp" +#include "CQuaternion.hpp" +#include "Math.hpp" +#include +#include +#include + +#if !__SSE__ +#error "SSE instruction set not enabled" +#endif + +class Test +{ + char t; + __m128 v; +}; + +int main() +{ + CQuaternion slerp1 = CQuaternion::fromAxisAngle(CVector3f(1.0, .5, 0.0), Math::degToRad(45.f)); +# if 1 + CQuaternion slerp2 = CQuaternion::fromAxisAngle(CVector3f(0.5, 256.0, 4096.0), Math::degToRad(90.f)); + CQuaternion slerpQuat = CQuaternion::slerp(slerp1, slerp2, 0.5); + + std::cout << Math::radToDeg(slerpQuat.r) << " " << slerpQuat.v.x << " " << slerpQuat.v.y << " " << slerpQuat.v.z << std::endl; + + CQuaternion quat = CQuaternion::fromAxisAngle(CVector3f(1.0, .5, 0.0), Math::degToRad(45.f)); + quat.invert(); + CAxisAngle axisAngle = quat.toAxisAngle(); + std::cout << axisAngle.axis.x << " " << axisAngle.axis.y << " " << axisAngle.axis.z << " " << Math::radToDeg(axisAngle.angle) << "d" << std::endl; + + std::cout << sizeof (Test) << std::endl; +#endif + return 0; +} +