From c8fa20ddbf8d30fad93f0ebdf3f89ec687e56183 Mon Sep 17 00:00:00 2001 From: Phillip Stephens Date: Sun, 19 Apr 2015 13:39:16 -0700 Subject: [PATCH] Initial Commit --- CAxisAngle.hpp | 24 ++++ CMatrix3f.cpp | 139 +++++++++++++++++++++++ CMatrix3f.hpp | 109 ++++++++++++++++++ CPlane.cpp | 3 + CPlane.hpp | 45 ++++++++ CProjection.cpp | 74 +++++++++++++ CProjection.hpp | 142 ++++++++++++++++++++++++ CQuaternion.cpp | 286 ++++++++++++++++++++++++++++++++++++++++++++++++ CQuaternion.hpp | 98 +++++++++++++++++ CTransform.cpp | 59 ++++++++++ CTransform.hpp | 56 ++++++++++ CVector3d.cpp | 108 ++++++++++++++++++ CVector3d.hpp | 45 ++++++++ CVector3f.cpp | 74 +++++++++++++ CVector3f.hpp | 283 +++++++++++++++++++++++++++++++++++++++++++++++ Global.hpp | 51 +++++++++ Math.cpp | 21 ++++ Math.hpp | 34 ++++++ MathLib.hpp | 15 +++ MathLib.pri | 24 ++++ MathLib.pro | 14 +++ deployment.pri | 191 ++++++++++++++++++++++++++++++++ main.cpp | 37 +++++++ 23 files changed, 1932 insertions(+) create mode 100644 CAxisAngle.hpp create mode 100644 CMatrix3f.cpp create mode 100644 CMatrix3f.hpp create mode 100644 CPlane.cpp create mode 100644 CPlane.hpp create mode 100644 CProjection.cpp create mode 100644 CProjection.hpp create mode 100644 CQuaternion.cpp create mode 100644 CQuaternion.hpp create mode 100644 CTransform.cpp create mode 100644 CTransform.hpp create mode 100644 CVector3d.cpp create mode 100644 CVector3d.hpp create mode 100644 CVector3f.cpp create mode 100644 CVector3f.hpp create mode 100644 Global.hpp create mode 100644 Math.cpp create mode 100644 Math.hpp create mode 100644 MathLib.hpp create mode 100644 MathLib.pri create mode 100644 MathLib.pro create mode 100644 deployment.pri create mode 100644 main.cpp 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; +} +