AxioDL/zeus
2
0
Fork 0
mirror of https://github.com/AxioDL/zeus.git synced 2025-12-17 17:05:30 +00:00
Code Issues Packages Projects Releases Wiki Activity

SIMD refactor

Browse Source
This commit is contained in:
Jack Andersen
2018-12-07 15:16:50 -10:00
parent d881e58f62
commit e8dfecbb6e
49 changed files with 6047 additions and 4721 deletions
Download Patch File Download Diff File Expand all files Collapse all files

1
src/CAABox.cpp
View File

@@ -3,7 +3,6 @@
namespace zeus
{
const CAABox CAABox::skInvertedBox = CAABox();
const CAABox CAABox::skNullBox = CAABox(CVector3f::skZero, CVector3f::skZero);
}

210
src/CColor.cpp
View File

@@ -1,8 +1,7 @@
#include "zeus/CColor.hpp"
#include "zeus/CVector4f.hpp"
namespace zeus
{
namespace zeus {
const CColor CColor::skRed(Comp32(0xFF0000FFul));
const CColor CColor::skBlack(Comp32(0x000000FFul));
const CColor CColor::skBlue(Comp32(0x0000FFFFul));
@@ -14,133 +13,106 @@ const CColor CColor::skYellow(Comp32(0xFFFF00FFul));
const CColor CColor::skWhite(Comp32(0xFFFFFFFFul));
const CColor CColor::skClear(Comp32(0x00000000ul));
float hueToRgb(float p, float q, float t)
{
if (t < 0.0f)
t += 1.0f;
if (t > 1.0f)
t -= 1.0f;
if (t < 1.f / 6.f)
return p + (q - p) * 6.f * t;
if (t < 1.f / 2.f)
return q;
if (t < 2.f / 3.f)
return p + (q - p) * (2.f / 3.f - t) * 6.f;
return p;
float hueToRgb(float p, float q, float t) {
if (t < 0.0f)
t += 1.0f;
if (t > 1.0f)
t -= 1.0f;
if (t < 1.f / 6.f)
return p + (q - p) * 6.f * t;
if (t < 1.f / 2.f)
return q;
if (t < 2.f / 3.f)
return p + (q - p) * (2.f / 3.f - t) * 6.f;
return p;
}
CColor::CColor(const CVector4f& other)
{
r = other.x;
g = other.y;
b = other.z;
a = other.w;
void CColor::fromHSV(float h, float s, float v, float _a) {
int i = int(h * 6.f);
float f = h * 6.f - i;
float p = v * (1.f - s);
float q = v * (1.f - f * s);
float t = v * (1.f - (1.f - f) * s);
simd_floats fo;
switch (i % 6) {
case 0:
fo[0] = v, fo[1] = t, fo[2] = p;
break;
case 1:
fo[0] = q, fo[1] = v, fo[2] = p;
break;
case 2:
fo[0] = p, fo[1] = v, fo[2] = t;
break;
case 3:
fo[0] = p, fo[1] = q, fo[2] = v;
break;
case 4:
fo[0] = t, fo[1] = p, fo[2] = v;
break;
case 5:
fo[0] = v, fo[1] = p, fo[2] = q;
break;
default:
break;
}
fo[3] = _a;
mSimd.copy_from(fo);
}
CColor& CColor::operator=(const CVector4f& other)
{
r = other.x;
g = other.y;
b = other.z;
a = other.w;
void CColor::toHSV(float& h, float& s, float& v) const {
float min = std::min(r(), std::min(g(), b()));
float max = std::max(r(), std::max(g(), b()));
v = max;
return *this;
float delta = max - min;
s = max == 0.f ? 0.f : delta / max;
if (max == min)
h = 0.f;
else {
if (max == r())
h = (g() - b()) / delta + (g() < b() ? 6.f : 0.f);
else if (max == g())
h = (b() - r()) / delta + 2.f;
else if (max == b())
h = (r() - g()) / delta + 4.f;
h /= 6.f;
}
}
void CColor::fromHSV(float h, float s, float v, float _a)
{
int i = int(h * 6);
float f = h * 6 - i;
float p = v * (1 - s);
float q = v * (1 - f * s);
float t = v * (1 - (1 - f) * s);
float _r, _g, _b;
switch (i % 6)
{
case 0:
_r = v, _g = t, _b = p;
break;
case 1:
_r = q, _g = v, _b = p;
break;
case 2:
_r = p, _g = v, _b = t;
break;
case 3:
_r = p, _g = q, _b = v;
break;
case 4:
_r = t, _g = p, _b = v;
break;
case 5:
_r = v, _g = p, _b = q;
break;
}
r = _r;
g = _g;
b = _b;
a = _a;
void CColor::fromHSL(float h, float s, float l, float _a) {
if (s == 0.0f) {
mSimd = simd<float>(l);
} else {
const float q = l < 0.5f ? l * (1.f + s) : l + s - 1.f * s;
const float p = 2.f * l - q;
r() = hueToRgb(p, q, h + 1.f / 3.f);
g() = hueToRgb(p, q, h);
b() = hueToRgb(p, q, h - 1.f / 3.f);
}
a() = _a;
}
void CColor::toHSV(float& h, float& s, float& v) const
{
float min = std::min(r, std::min(g, b));
float max = std::max(r, std::max(g, b));
v = max;
void CColor::toHSL(float& h, float& s, float& l) const {
const float min = std::min(r(), std::min(g(), b()));
const float max = std::max(r(), std::max(g(), b()));
const float d = max - min;
float delta = max - min;
s = max == 0 ? 0 : delta / max;
if (max == min)
h = s = 0.f;
else {
s = l > 0.5f ? d / (2.f - max - min) : d / (max + min);
if (max == r())
h = (g() - b()) / d + (g() < b() ? 6.f : 0.f);
else if (max == g())
h = (b() - r()) / d + 2.f;
else if (max == b())
h = (r() - g()) / d + 4.f;
if (max == min)
h = 0;
else
{
if (max == r)
h = (g - b) / delta + (g < b ? 6 : 0);
else if (max == g)
h = (b - r) / delta + 2;
else if (max == b)
h = (r - g) / delta + 4;
h /= 6;
}
}
void CColor::fromHSL(float h, float s, float l, float _a)
{
if (s == 0.0f)
r = g = b = l;
else
{
const float q = l < 0.5f ? l * (1.f + s) : l + s - 1.f * s;
const float p = 2 * l - q;
r = hueToRgb(p, q, h + 1.f / 3);
g = hueToRgb(p, q, h);
b = hueToRgb(p, q, h - 1.f / 3);
}
a = _a;
}
void CColor::toHSL(float& h, float& s, float& l)
{
const float min = std::min(r, std::min(g, b));
const float max = std::max(r, std::max(g, b));
const float d = max - min;
if (max == min)
h = s = 0;
else
{
s = l > 0.5f ? d / (2.f - max - min) : d / (max + min);
if (max == r)
h = (g - b) / d + (g < b ? 6.f : 0.f);
else if (max == g)
h = (b - r) / d + 2.f;
else if (max == b)
h = (r - g) / d + 4.f;
h /= 6;
}
h /= 6.f;
}
}
}

34
src/CEulerAngles.cpp
View File

@@ -10,29 +10,29 @@ CEulerAngles::CEulerAngles(const CQuaternion& quat)
float t0 = 0.f;
if (quatDot > 0.f)
t0 = 2.f / quatDot;
double t1 = 1.0 - (t0 * quat.x * quat.x + t0 * quat.z * quat.z);
double t2 = t0 * quat.y * quat.x - t0 * quat.z * quat.w;
double t1 = 1.0 - (t0 * quat.x() * quat.x() + t0 * quat.z() * quat.z());
double t2 = t0 * quat.y() * quat.x() - t0 * quat.z() * quat.w();
double t3 = t1 * t1 + t2 * t2;
double t4 = 0.0;
if (t3 > 0.0)
t4 = std::sqrt(t3);
double t5 = t0 * quat.z * quat.y + t0 * quat.x * quat.w;
double t5 = t0 * quat.z() * quat.y() + t0 * quat.x() * quat.w();
if (std::abs(t4) > 0.00001)
{
x = -std::atan2(-t5, t4);
y = -std::atan2(t0 * quat.z * quat.x - t0 * quat.y * quat.w,
1.0 - (t0 * quat.x * quat.x + t0 * quat.y * quat.y));
z = -std::atan2(t2, t1);
x() = -std::atan2(-t5, t4);
y() = -std::atan2(t0 * quat.z() * quat.x() - t0 * quat.y() * quat.w(),
1.0 - (t0 * quat.x() * quat.x() + t0 * quat.y() * quat.y()));
z() = -std::atan2(t2, t1);
}
else
{
x = -std::atan2(-t5, t4);
y = -std::atan2(-(t0 * quat.z * quat.x + t0 * quat.y * quat.w),
1.0 - (t0 * quat.y * quat.y + t0 * quat.z * quat.z));
z = 0.f;
x() = -std::atan2(-t5, t4);
y() = -std::atan2(-(t0 * quat.z() * quat.x() + t0 * quat.y() * quat.w()),
1.0 - (t0 * quat.y() * quat.y() + t0 * quat.z() * quat.z()));
z() = 0.f;
}
}
@@ -58,15 +58,15 @@ CEulerAngles::CEulerAngles(const CTransform& xf)
if (std::fabs(f1) >= 0.00001)
{
x = -std::atan2(-xf.basis[1][2], f1);
y = -std::atan2(xf.basis[0][2], xf.basis[2][2]);
z = -std::atan2(xf.basis[1][0], xf.basis[1][1]);
x() = -std::atan2(-xf.basis[1][2], f1);
y() = -std::atan2(xf.basis[0][2], xf.basis[2][2]);
z() = -std::atan2(xf.basis[1][0], xf.basis[1][1]);
}
else
{
x = -std::atan2(-xf.basis[1][2], f1);
y = -std::atan2(-xf.basis[2][0], xf.basis[0][0]);
z = 0.f;
x() = -std::atan2(-xf.basis[1][2], f1);
y() = -std::atan2(-xf.basis[2][0], xf.basis[0][0]);
z() = 0.f;
}
}

174
src/CFrustum.cpp
View File

@@ -1,138 +1,88 @@
#include "zeus/CFrustum.hpp"
namespace zeus
{
namespace zeus {
void CFrustum::updatePlanes(const CMatrix4f& viewMtx, const CMatrix4f& projection)
{
CMatrix4f mvp = projection * viewMtx;
CMatrix4f mvp_rm = mvp.transposed();
void CFrustum::updatePlanes(const CMatrix4f& viewMtx, const CMatrix4f& projection) {
CMatrix4f mvp = projection * viewMtx;
CMatrix4f mvp_rm = mvp.transposed();
#if __SSE__
/* Left */
planes[0].mSimd = mvp_rm.m[3].mSimd + mvp_rm.m[0].mSimd;
/* Left */
planes[0].mVec128 = _mm_add_ps(mvp_rm.vec[3].mVec128, mvp_rm.vec[0].mVec128);
/* Right */
planes[1].mSimd = mvp_rm.m[3].mSimd - mvp_rm.m[0].mSimd;
/* Right */
planes[1].mVec128 = _mm_sub_ps(mvp_rm.vec[3].mVec128, mvp_rm.vec[0].mVec128);
/* Bottom */
planes[2].mSimd = mvp_rm.m[3].mSimd + mvp_rm.m[1].mSimd;
/* Bottom */
planes[2].mVec128 = _mm_add_ps(mvp_rm.vec[3].mVec128, mvp_rm.vec[1].mVec128);
/* Top */
planes[3].mSimd = mvp_rm.m[3].mSimd - mvp_rm.m[1].mSimd;
/* Top */
planes[3].mVec128 = _mm_sub_ps(mvp_rm.vec[3].mVec128, mvp_rm.vec[1].mVec128);
/* Near */
planes[4].mSimd = mvp_rm.m[3].mSimd + mvp_rm.m[2].mSimd;
/* Near */
planes[4].mVec128 = _mm_add_ps(mvp_rm.vec[3].mVec128, mvp_rm.vec[2].mVec128);
/* Far */
planes[5].mSimd = mvp_rm.m[3].mSimd - mvp_rm.m[2].mSimd;
/* Far */
planes[5].mVec128 = _mm_sub_ps(mvp_rm.vec[3].mVec128, mvp_rm.vec[2].mVec128);
planes[0].normalize();
planes[1].normalize();
planes[2].normalize();
planes[3].normalize();
planes[4].normalize();
planes[5].normalize();
#else
/* Left */
planes[0].a = mvp.m[0][0] + mvp.m[3][0];
planes[0].b = mvp.m[0][1] + mvp.m[3][1];
planes[0].c = mvp.m[0][2] + mvp.m[3][2];
planes[0].d = mvp.m[0][3] + mvp.m[3][3];
/* Right */
planes[1].a = -mvp.m[0][0] + mvp.m[3][0];
planes[1].b = -mvp.m[0][1] + mvp.m[3][1];
planes[1].c = -mvp.m[0][2] + mvp.m[3][2];
planes[1].d = -mvp.m[0][3] + mvp.m[3][3];
/* Bottom */
planes[2].a = mvp.m[1][0] + mvp.m[3][0];
planes[2].b = mvp.m[1][1] + mvp.m[3][1];
planes[2].c = mvp.m[1][2] + mvp.m[3][2];
planes[2].d = mvp.m[1][3] + mvp.m[3][3];
/* Top */
planes[3].a = -mvp.m[1][0] + mvp.m[3][0];
planes[3].b = -mvp.m[1][1] + mvp.m[3][1];
planes[3].c = -mvp.m[1][2] + mvp.m[3][2];
planes[3].d = -mvp.m[1][3] + mvp.m[3][3];
/* Near */
planes[4].a = mvp.m[2][0] + mvp.m[3][0];
planes[4].b = mvp.m[2][1] + mvp.m[3][1];
planes[4].c = mvp.m[2][2] + mvp.m[3][2];
planes[4].d = mvp.m[2][3] + mvp.m[3][3];
/* Far */
planes[5].a = -mvp.m[2][0] + mvp.m[3][0];
planes[5].b = -mvp.m[2][1] + mvp.m[3][1];
planes[5].c = -mvp.m[2][2] + mvp.m[3][2];
planes[5].d = -mvp.m[2][3] + mvp.m[3][3];
#endif
planes[0].normalize();
planes[1].normalize();
planes[2].normalize();
planes[3].normalize();
planes[4].normalize();
planes[5].normalize();
valid = true;
valid = true;
}
void CFrustum::updatePlanes(const CTransform& viewPointMtx, const CProjection& projection)
{
zeus::CMatrix3f tmp(viewPointMtx.basis[0], viewPointMtx.basis[2], -viewPointMtx.basis[1]);
zeus::CTransform viewBasis = zeus::CTransform(tmp.transposed());
zeus::CTransform viewMtx = viewBasis * zeus::CTransform::Translate(-viewPointMtx.origin);
void CFrustum::updatePlanes(const CTransform& viewPointMtx, const CProjection& projection) {
zeus::CMatrix3f tmp(viewPointMtx.basis[0], viewPointMtx.basis[2], -viewPointMtx.basis[1]);
zeus::CTransform viewBasis = zeus::CTransform(tmp.transposed());
zeus::CTransform viewMtx = viewBasis * zeus::CTransform::Translate(-viewPointMtx.origin);
updatePlanes(viewMtx.toMatrix4f(), projection.getCachedMatrix());
updatePlanes(viewMtx.toMatrix4f(), projection.getCachedMatrix());
}
bool CFrustum::aabbFrustumTest(const CAABox& aabb) const
{
if (!valid)
return true;
CVector3f center = aabb.center();
CVector3f extents = aabb.extents();
for (uint32_t i = 0; i < 6; ++i)
{
const CPlane& plane = planes[i];
float m = plane.vec.dot(center) + plane.d;
float n = extents.dot({std::fabs(plane.a), std::fabs(plane.b), std::fabs(plane.c)});
if (m + n < 0)
return false;
}
bool CFrustum::aabbFrustumTest(const CAABox& aabb) const {
if (!valid)
return true;
CVector3f center = aabb.center();
CVector3f extents = aabb.extents();
for (uint32_t i = 0; i < 6; ++i) {
const CPlane& plane = planes[i];
float m = plane.normal().dot(center) + plane.d();
float n = extents.dot({std::fabs(plane.x()), std::fabs(plane.y()), std::fabs(plane.z())});
if (m + n < 0.f)
return false;
}
return true;
}
bool CFrustum::sphereFrustumTest(const CSphere& sphere) const
{
if (!valid)
return true;
for (uint32_t i = 0 ; i<6 ; ++i)
{
float dadot = planes[i].vec.dot(sphere.position);
if ((dadot + planes[i].d + sphere.radius) < 0)
return false;
}
bool CFrustum::sphereFrustumTest(const CSphere& sphere) const {
if (!valid)
return true;
for (uint32_t i = 0; i < 6; ++i) {
float dadot = planes[i].normal().dot(sphere.position);
if ((dadot + planes[i].d() + sphere.radius) < 0.f)
return false;
}
return true;
}
bool CFrustum::pointFrustumTest(const CVector3f& point) const
{
if (!valid)
return true;
for (uint32_t i = 0 ; i<6 ; ++i)
{
float dadot = planes[i].vec.dot(point);
if ((dadot + planes[i].d) < 0)
return false;
}
bool CFrustum::pointFrustumTest(const CVector3f& point) const {
if (!valid)
return true;
for (uint32_t i = 0; i < 6; ++i) {
float dadot = planes[i].normal().dot(point);
if ((dadot + planes[i].d()) < 0.f)
return false;
}
return true;
}
}

155
src/CMatrix3f.cpp
View File

@@ -2,104 +2,113 @@
#include "zeus/CQuaternion.hpp"
#include "zeus/Global.hpp"
namespace zeus
{
namespace zeus {
const CMatrix3f CMatrix3f::skIdentityMatrix3f = CMatrix3f();
CMatrix3f::CMatrix3f(const CQuaternion& quat)
{
CQuaternion nq = quat.normalized();
float x2 = nq.x * nq.x;
float y2 = nq.y * nq.y;
float z2 = nq.z * nq.z;
CMatrix3f::CMatrix3f(const CQuaternion& quat) {
CQuaternion nq = quat.normalized();
float x2 = nq.x() * nq.x();
float y2 = nq.y() * nq.y();
float z2 = nq.z() * nq.z();
m[0][0] = 1.0 - 2.0 * y2 - 2.0 * z2;
m[1][0] = 2.0 * nq.x * nq.y - 2.0 * nq.z * nq.w;
m[2][0] = 2.0 * nq.x * nq.z + 2.0 * nq.y * nq.w;
m[0][0] = 1.0 - 2.0 * y2 - 2.0 * z2;
m[1][0] = 2.0 * nq.x() * nq.y() - 2.0 * nq.z() * nq.w();
m[2][0] = 2.0 * nq.x() * nq.z() + 2.0 * nq.y() * nq.w();
m[0][1] = 2.0 * nq.x * nq.y + 2.0 * nq.z * nq.w;
m[1][1] = 1.0 - 2.0 * x2 - 2.0 * z2;
m[2][1] = 2.0 * nq.y * nq.z - 2.0 * nq.x * nq.w;
m[0][1] = 2.0 * nq.x() * nq.y() + 2.0 * nq.z() * nq.w();
m[1][1] = 1.0 - 2.0 * x2 - 2.0 * z2;
m[2][1] = 2.0 * nq.y() * nq.z() - 2.0 * nq.x() * nq.w();
m[0][2] = 2.0 * nq.x * nq.z - 2.0 * nq.y * nq.w;
m[1][2] = 2.0 * nq.y * nq.z + 2.0 * nq.x * nq.w;
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;
m[0][2] = 2.0 * nq.x() * nq.z() - 2.0 * nq.y() * nq.w();
m[1][2] = 2.0 * nq.y() * nq.z() + 2.0 * nq.x() * nq.w();
m[2][2] = 1.0 - 2.0 * x2 - 2.0 * y2;
}
void CMatrix3f::transpose()
{
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);
__m128 zero = _mm_xor_ps(m[0].mSimd.native(), m[0].mSimd.native());
__m128 T0 = _mm_unpacklo_ps(m[0].mSimd.native(), m[1].mSimd.native());
__m128 T2 = _mm_unpacklo_ps(m[2].mSimd.native(), zero);
__m128 T1 = _mm_unpackhi_ps(m[0].mSimd.native(), m[1].mSimd.native());
__m128 T3 = _mm_unpackhi_ps(m[2].mSimd.native(), zero);
m[0].mSimd = _mm_movelh_ps(T0, T2);
m[1].mSimd = _mm_movehl_ps(T2, T0);
m[2].mSimd = _mm_movelh_ps(T1, T3);
#elif __ARM_NEON
float32x4x2_t P0 = vzipq_f32( M.r[0], M.r[2] );
float32x4x2_t P1 = vzipq_f32( M.r[1], M.r[3] );
float32x4x2_t T0 = vzipq_f32( P0.val[0], P1.val[0] );
float32x4x2_t T1 = vzipq_f32( P0.val[1], P1.val[1] );
m[0].mSimd = T0.val[0];
m[1].mSimd = T0.val[1];
m[2].mSimd = T1.val[0];
#else
float tmp;
float tmp;
tmp = m[0][1];
m[0][1] = m[1][0];
m[1][0] = 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[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;
tmp = m[1][2];
m[1][2] = m[2][1];
m[2][1] = tmp;
#endif
}
CMatrix3f CMatrix3f::transposed() const
{
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));
__m128 zero = _mm_xor_ps(m[0].mSimd.native(), m[0].mSimd.native());
__m128 T0 = _mm_unpacklo_ps(m[0].mSimd.native(), m[1].mSimd.native());
__m128 T2 = _mm_unpacklo_ps(m[2].mSimd.native(), zero);
__m128 T1 = _mm_unpackhi_ps(m[0].mSimd.native(), m[1].mSimd.native());
__m128 T3 = _mm_unpackhi_ps(m[2].mSimd.native(), zero);
return CMatrix3f(_mm_movelh_ps(T0, T2), _mm_movehl_ps(T2, T0), _mm_movelh_ps(T1, T3));
#elif __ARM_NEON
float32x4x2_t P0 = vzipq_f32( M.r[0], M.r[2] );
float32x4x2_t P1 = vzipq_f32( M.r[1], M.r[3] );
float32x4x2_t T0 = vzipq_f32( P0.val[0], P1.val[0] );
float32x4x2_t T1 = vzipq_f32( P0.val[1], P1.val[1] );
return CMatrix3f(T0.val[0], T0.val[1], T1.val[0]);
#else
CMatrix3f ret(*this);
float tmp;
CMatrix3f ret(*this);
float tmp;
tmp = ret.m[0][1];
ret.m[0][1] = ret.m[1][0];
ret.m[1][0] = 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[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;
tmp = m[1][2];
ret.m[1][2] = ret.m[2][1];
ret.m[2][1] = tmp;
return ret;
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];
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();
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);
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);
}
}

27
src/CMatrix4f.cpp
View File

@@ -9,14 +9,25 @@ CMatrix4f CMatrix4f::transposed() const
{
CMatrix4f ret;
#if __SSE__
__m128 T0 = _mm_unpacklo_ps(vec[0].mVec128, vec[1].mVec128);
__m128 T2 = _mm_unpacklo_ps(vec[2].mVec128, vec[3].mVec128);
__m128 T1 = _mm_unpackhi_ps(vec[0].mVec128, vec[1].mVec128);
__m128 T3 = _mm_unpackhi_ps(vec[2].mVec128, vec[3].mVec128);
ret.vec[0].mVec128 = _mm_movelh_ps(T0, T2);
ret.vec[1].mVec128 = _mm_movehl_ps(T2, T0);
ret.vec[2].mVec128 = _mm_movelh_ps(T1, T3);
ret.vec[3].mVec128 = _mm_movehl_ps(T3, T1);
__m128 T0 = _mm_unpacklo_ps(m[0].mSimd.native(), m[1].mSimd.native());
__m128 T2 = _mm_unpacklo_ps(m[2].mSimd.native(), m[3].mSimd.native());
__m128 T1 = _mm_unpackhi_ps(m[0].mSimd.native(), m[1].mSimd.native());
__m128 T3 = _mm_unpackhi_ps(m[2].mSimd.native(), m[3].mSimd.native());
ret.m[0].mSimd = _mm_movelh_ps(T0, T2);
ret.m[1].mSimd = _mm_movehl_ps(T2, T0);
ret.m[2].mSimd = _mm_movelh_ps(T1, T3);
ret.m[3].mSimd = _mm_movehl_ps(T3, T1);
#elif __ARM_NEON
float32x4x2_t P0 = vzipq_f32( M.r[0], M.r[2] );
float32x4x2_t P1 = vzipq_f32( M.r[1], M.r[3] );
float32x4x2_t T0 = vzipq_f32( P0.val[0], P1.val[0] );
float32x4x2_t T1 = vzipq_f32( P0.val[1], P1.val[1] );
ret.m[0].mSimd = T0.val[0];
ret.m[1].mSimd = T0.val[1];
ret.m[2].mSimd = T1.val[0];
ret.m[3].mSimd = T1.val[1];
#else
ret.m[0][0] = m[0][0];
ret.m[1][0] = m[0][1];

225
src/COBBox.cpp
View File

@@ -1,139 +1,140 @@
#include "zeus/COBBox.hpp"
namespace zeus
{
namespace zeus {
CAABox COBBox::calculateAABox(const CTransform& worldXf) const
{
CAABox ret = CAABox::skInvertedBox;
CAABox COBBox::calculateAABox(const CTransform& worldXf) const {
CAABox ret = CAABox::skInvertedBox;
CTransform trans = worldXf * transform;
static const CVector3f basis[8] = {{1.f, 1.f, 1.f}, {1.f, 1.f, -1.f}, {1.f, -1.f, 1.f}, {1.f, -1.f, -1.f},
{-1.f, -1.f, -1.f}, {-1.f, -1.f, 1.f}, {-1.f, 1.f, -1.f}, {-1.f, 1.f, 1.f}};
CVector3f p = extents * basis[0];
ret.accumulateBounds(trans * p);
p = extents * basis[1];
ret.accumulateBounds(trans * p);
p = extents * basis[2];
ret.accumulateBounds(trans * p);
p = extents * basis[3];
ret.accumulateBounds(trans * p);
p = extents * basis[4];
ret.accumulateBounds(trans * p);
p = extents * basis[5];
ret.accumulateBounds(trans * p);
p = extents * basis[6];
ret.accumulateBounds(trans * p);
p = extents * basis[7];
ret.accumulateBounds(trans * p);
CTransform trans = worldXf * transform;
static const CVector3f basis[8] = {{1.f, 1.f, 1.f},
{1.f, 1.f, -1.f},
{1.f, -1.f, 1.f},
{1.f, -1.f, -1.f},
{-1.f, -1.f, -1.f},
{-1.f, -1.f, 1.f},
{-1.f, 1.f, -1.f},
{-1.f, 1.f, 1.f}};
CVector3f p = extents * basis[0];
ret.accumulateBounds(trans * p);
p = extents * basis[1];
ret.accumulateBounds(trans * p);
p = extents * basis[2];
ret.accumulateBounds(trans * p);
p = extents * basis[3];
ret.accumulateBounds(trans * p);
p = extents * basis[4];
ret.accumulateBounds(trans * p);
p = extents * basis[5];
ret.accumulateBounds(trans * p);
p = extents * basis[6];
ret.accumulateBounds(trans * p);
p = extents * basis[7];
ret.accumulateBounds(trans * p);
return ret;
return ret;
}
bool COBBox::OBBIntersectsBox(const COBBox& other) const
{
CVector3f v = other.transform.origin - transform.origin;
CVector3f T = CVector3f(v.dot(transform.basis[0]),
v.dot(transform.basis[1]),
v.dot(transform.basis[2]));
bool COBBox::OBBIntersectsBox(const COBBox& other) const {
CVector3f v = other.transform.origin - transform.origin;
CVector3f T = CVector3f(v.dot(transform.basis[0]),
v.dot(transform.basis[1]),
v.dot(transform.basis[2]));
CMatrix3f R;
CMatrix3f R;
float ra, rb, t;
for (int i = 0; i < 3; ++i)
for (int k = 0; k < 3; ++k)
R[i][k] = transform.basis[i].dot(other.transform.basis[k]);
for (int i = 0; i < 3; ++i)
{
ra = extents[i];
rb = (other.extents[0] * std::fabs(R[i][0])) +
(other.extents[1] * std::fabs(R[i][1])) +
(other.extents[2] * std::fabs(R[i][2]));
t = std::fabs(T[i]);
if (t > (ra + rb + FLT_EPSILON))
return false;
}
float ra, rb, t;
for (int i = 0; i < 3; ++i)
for (int k = 0; k < 3; ++k)
{
ra = (extents[0] * std::fabs(R[0][k])) +
(extents[1] * std::fabs(R[1][k])) +
(extents[2] * std::fabs(R[2][k]));
rb = other.extents[k];
R[i][k] = transform.basis[i].dot(other.transform.basis[k]);
t = std::fabs(T[0] * R[0][k] + T[1] * R[1][k] + T[2] * R[2][k]);
for (int i = 0; i < 3; ++i) {
ra = extents[i];
rb = (other.extents[0] * std::fabs(R[i][0])) +
(other.extents[1] * std::fabs(R[i][1])) +
(other.extents[2] * std::fabs(R[i][2]));
t = std::fabs(T[i]);
if (t > (ra + rb + FLT_EPSILON))
return false;
}
/* A0 x B0 */
ra = (extents[1] * std::fabs(R[2][0])) + (extents[2] * std::fabs(R[1][0]));
rb = (other.extents[1] * std::fabs(R[0][2])) + (other.extents[2] * std::fabs(R[0][1]));
t = std::fabs((T[2] * R[1][0]) - (T[1] * R[2][0]));
if (t > (ra + rb + FLT_EPSILON))
return false;
return false;
}
for (int k = 0; k < 3; ++k) {
ra = (extents[0] * std::fabs(R[0][k])) +
(extents[1] * std::fabs(R[1][k])) +
(extents[2] * std::fabs(R[2][k]));
rb = other.extents[k];
t = std::fabs(T[0] * R[0][k] + T[1] * R[1][k] + T[2] * R[2][k]);
/* A0 x B1 */
ra = (extents[1] * std::fabs(R[2][1])) + (extents[2] * std::fabs(R[1][1]));
rb = (other.extents[0] * std::fabs(R[0][2])) + (other.extents[2] * std::fabs(R[0][0]));
t = std::fabs((T[2] * R[1][1]) - (T[1] * R[2][1]));
if (t > (ra + rb + FLT_EPSILON))
return false;
return false;
}
/* A0 x B2 */
ra = (extents[1] * std::fabs(R[2][2])) + (extents[2] * std::fabs(R[1][2]));
rb = (other.extents[0] * std::fabs(R[0][1])) + (other.extents[1] * std::fabs(R[0][0]));
t = std::fabs((T[2] * R[1][2]) - (T[1] * R[2][2]));
if (t > (ra + rb + FLT_EPSILON))
return false;
/* A0 x B0 */
ra = (extents[1] * std::fabs(R[2][0])) + (extents[2] * std::fabs(R[1][0]));
rb = (other.extents[1] * std::fabs(R[0][2])) + (other.extents[2] * std::fabs(R[0][1]));
t = std::fabs((T[2] * R[1][0]) - (T[1] * R[2][0]));
if (t > (ra + rb + FLT_EPSILON))
return false;
/* A1 x B0 */
ra = (extents[0] * std::fabs(R[2][0])) + (extents[2] * std::fabs(R[0][0]));
rb = (other.extents[1] * std::fabs(R[1][2])) + (other.extents[2] * std::fabs(R[1][1]));
t = std::fabs((T[0] * R[2][0]) - (T[2] * R[0][0]));
if (t > (ra + rb + FLT_EPSILON))
return false;
/* A0 x B1 */
ra = (extents[1] * std::fabs(R[2][1])) + (extents[2] * std::fabs(R[1][1]));
rb = (other.extents[0] * std::fabs(R[0][2])) + (other.extents[2] * std::fabs(R[0][0]));
t = std::fabs((T[2] * R[1][1]) - (T[1] * R[2][1]));
if (t > (ra + rb + FLT_EPSILON))
return false;
/* A1 x B1 */
ra = (extents[0] * std::fabs(R[2][1])) + (extents[2] * std::fabs(R[0][1]));
rb = (other.extents[0] * std::fabs(R[1][2])) + (other.extents[2] * std::fabs(R[1][0]));
t = std::fabs((T[0] * R[2][1]) - (T[2] * R[0][1]));
if (t > (ra + rb + FLT_EPSILON))
return false;
/* A0 x B2 */
ra = (extents[1] * std::fabs(R[2][2])) + (extents[2] * std::fabs(R[1][2]));
rb = (other.extents[0] * std::fabs(R[0][1])) + (other.extents[1] * std::fabs(R[0][0]));
t = std::fabs((T[2] * R[1][2]) - (T[1] * R[2][2]));
if (t > (ra + rb + FLT_EPSILON))
return false;
/* A1 x B2 */
ra = (extents[0] * std::fabs(R[2][2])) + (extents[2] * std::fabs(R[0][2]));
rb = (other.extents[0] * std::fabs(R[1][1])) + (other.extents[1] * std::fabs(R[1][0]));
t = std::fabs((T[0] * R[2][2]) - (T[2] * R[0][2]));
if (t > (ra + rb + FLT_EPSILON))
return false;
/* A1 x B0 */
ra = (extents[0] * std::fabs(R[2][0])) + (extents[2] * std::fabs(R[0][0]));
rb = (other.extents[1] * std::fabs(R[1][2])) + (other.extents[2] * std::fabs(R[1][1]));
t = std::fabs((T[0] * R[2][0]) - (T[2] * R[0][0]));
if (t > (ra + rb + FLT_EPSILON))
return false;
/* A2 x B0 */
ra = (extents[0] * std::fabs(R[1][0])) + (extents[1] * std::fabs(R[0][0]));
rb = (other.extents[1] * std::fabs(R[2][2])) + (other.extents[2] * std::fabs(R[2][1]));
t = std::fabs((T[1] * R[0][0]) - (T[0] * R[1][0]));
if (t > (ra + rb + FLT_EPSILON))
return false;
/* A1 x B1 */
ra = (extents[0] * std::fabs(R[2][1])) + (extents[2] * std::fabs(R[0][1]));
rb = (other.extents[0] * std::fabs(R[1][2])) + (other.extents[2] * std::fabs(R[1][0]));
t = std::fabs((T[0] * R[2][1]) - (T[2] * R[0][1]));
if (t > (ra + rb + FLT_EPSILON))
return false;
/* A2 x B1 */
ra = (extents[0] * std::fabs(R[1][1])) + (extents[1] * std::fabs(R[0][1]));
rb = (other.extents[0] * std::fabs(R[2][2])) + (other.extents[2] * std::fabs(R[2][0]));
t = std::fabs((T[1] * R[0][1]) - (T[0] * R[1][1]));
if (t > (ra + rb + FLT_EPSILON))
return false;
/* A1 x B2 */
ra = (extents[0] * std::fabs(R[2][2])) + (extents[2] * std::fabs(R[0][2]));
rb = (other.extents[0] * std::fabs(R[1][1])) + (other.extents[1] * std::fabs(R[1][0]));
t = std::fabs((T[0] * R[2][2]) - (T[2] * R[0][2]));
if (t > (ra + rb + FLT_EPSILON))
return false;
/* A2 x B2 */
ra = (extents[0] * std::fabs(R[1][2])) + (extents[1] * std::fabs(R[0][2]));
rb = (other.extents[0] * std::fabs(R[2][1])) + (other.extents[1] * std::fabs(R[2][0]));
t = std::fabs((T[1] * R[0][2]) - (T[0] * R[1][2]));
if (t > (ra + rb + FLT_EPSILON))
return false;
/* A2 x B0 */
ra = (extents[0] * std::fabs(R[1][0])) + (extents[1] * std::fabs(R[0][0]));
rb = (other.extents[1] * std::fabs(R[2][2])) + (other.extents[2] * std::fabs(R[2][1]));
t = std::fabs((T[1] * R[0][0]) - (T[0] * R[1][0]));
if (t > (ra + rb + FLT_EPSILON))
return false;
return true;
/* A2 x B1 */
ra = (extents[0] * std::fabs(R[1][1])) + (extents[1] * std::fabs(R[0][1]));
rb = (other.extents[0] * std::fabs(R[2][2])) + (other.extents[2] * std::fabs(R[2][0]));
t = std::fabs((T[1] * R[0][1]) - (T[0] * R[1][1]));
if (t > (ra + rb + FLT_EPSILON))
return false;
/* A2 x B2 */
ra = (extents[0] * std::fabs(R[1][2])) + (extents[1] * std::fabs(R[0][2]));
rb = (other.extents[0] * std::fabs(R[2][1])) + (other.extents[1] * std::fabs(R[2][0]));
t = std::fabs((T[1] * R[0][2]) - (T[0] * R[1][2]));
if (t > (ra + rb + FLT_EPSILON))
return false;
return true;
}
}

22
src/CPlane.cpp
View File

@@ -1,18 +1,16 @@
#include "zeus/CPlane.hpp"
namespace zeus
{
namespace zeus {
bool CPlane::rayPlaneIntersection(const CVector3f& from, const CVector3f& to, CVector3f& point) const
{
zeus::CVector3f delta = to - from;
if (std::fabs(delta.normalized().dot(vec)) < 0.01f)
return false;
float tmp = -pointToPlaneDist(from) / delta.dot(vec);
if (tmp < -0.f || tmp > 1.0001f)
return false;
point = delta * tmp + from;
return true;
bool CPlane::rayPlaneIntersection(const CVector3f& from, const CVector3f& to, CVector3f& point) const {
zeus::CVector3f delta = to - from;
if (std::fabs(delta.normalized().dot(normal())) < 0.01f)
return false;
float tmp = -pointToPlaneDist(from) / delta.dot(normal());
if (tmp < -0.f || tmp > 1.0001f)
return false;
point = delta * tmp + from;
return true;
}
}

111
src/CProjection.cpp
View File

@@ -2,72 +2,67 @@
#include "zeus/Math.hpp"
#include <cassert>
namespace zeus
{
void CProjection::_updateCachedMatrix()
{
assert(m_projType == EProjType::Orthographic || m_projType == EProjType::Perspective);
if (m_projType == EProjType::Orthographic)
{
float tmp;
namespace zeus {
void CProjection::_updateCachedMatrix() {
assert(m_projType == EProjType::Orthographic || m_projType == EProjType::Perspective);
if (m_projType == EProjType::Orthographic) {
float tmp;
tmp = 1.0f / (m_ortho.right - m_ortho.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.right + m_ortho.left) * tmp;
tmp = 1.0f / (m_ortho.right - m_ortho.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.right + m_ortho.left) * tmp;
tmp = 1.0f / (m_ortho.top - m_ortho.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.top + m_ortho.bottom) * tmp;
tmp = 1.0f / (m_ortho.top - m_ortho.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.top + m_ortho.bottom) * tmp;
tmp = 1.0f / (m_ortho.zfar - m_ortho.znear);
m_mtx.m[0][2] = 0.0f;
m_mtx.m[1][2] = 0.0f;
m_mtx.m[2][2] = -tmp;
m_mtx.m[3][2] = -m_ortho.zfar * tmp;
tmp = 1.0f / (m_ortho.zfar - m_ortho.znear);
m_mtx.m[0][2] = 0.0f;
m_mtx.m[1][2] = 0.0f;
m_mtx.m[2][2] = -tmp;
m_mtx.m[3][2] = -m_ortho.zfar * 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 == EProjType::Perspective)
{
float tfov = std::tan(m_persp.fov * 0.5f);
float top = m_persp.znear * tfov;
float bottom = -top;
float right = m_persp.aspect * m_persp.znear * tfov;
float left = -right;
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 == EProjType::Perspective) {
float tfov = std::tan(m_persp.fov * 0.5f);
float top = m_persp.znear * tfov;
float bottom = -top;
float right = m_persp.aspect * m_persp.znear * tfov;
float left = -right;
float rml = right - left;
float rpl = right + left;
float tmb = top - bottom;
float tpb = top + bottom;
float fpn = m_persp.zfar + m_persp.znear;
float fmn = m_persp.zfar - m_persp.znear;
float rml = right - left;
float rpl = right + left;
float tmb = top - bottom;
float tpb = top + bottom;
float fpn = m_persp.zfar + m_persp.znear;
float fmn = m_persp.zfar - m_persp.znear;
m_mtx.m[0][0] = 2.f * m_persp.znear / rml;
m_mtx.m[1][0] = 0.0f;
m_mtx.m[2][0] = rpl / rml;
m_mtx.m[3][0] = 0.0f;
m_mtx.m[0][0] = 2.f * m_persp.znear / rml;
m_mtx.m[1][0] = 0.0f;
m_mtx.m[2][0] = rpl / rml;
m_mtx.m[3][0] = 0.0f;
m_mtx.m[0][1] = 0.0f;
m_mtx.m[1][1] = 2.f * m_persp.znear / tmb;
m_mtx.m[2][1] = tpb / tmb;
m_mtx.m[3][1] = 0.0f;
m_mtx.m[0][1] = 0.0f;
m_mtx.m[1][1] = 2.f * m_persp.znear / tmb;
m_mtx.m[2][1] = tpb / tmb;
m_mtx.m[3][1] = 0.0f;
m_mtx.m[0][2] = 0.0f;
m_mtx.m[1][2] = 0.0f;
m_mtx.m[2][2] = -fpn / fmn;
m_mtx.m[3][2] = -2.f * m_persp.zfar * m_persp.znear / fmn;
m_mtx.m[0][2] = 0.0f;
m_mtx.m[1][2] = 0.0f;
m_mtx.m[2][2] = -fpn / fmn;
m_mtx.m[3][2] = -2.f * m_persp.zfar * m_persp.znear / fmn;
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;
}
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;
}
}
}

639
src/CQuaternion.cpp
View File

@@ -1,399 +1,330 @@
#include "zeus/CQuaternion.hpp"
#include "zeus/Math.hpp"
namespace zeus
{
namespace zeus {
const CQuaternion CQuaternion::skNoRotation;
CQuaternion::CQuaternion(const CMatrix3f& mat)
{
float trace = mat[0][0] + mat[1][1] + mat[2][2];
if (trace >= 0.f)
{
float st = std::sqrt(trace + 1.0f);
float s = 0.5f / st;
w = 0.5f * st;
x = (mat[1][2] - mat[2][1]) * s;
y = (mat[2][0] - mat[0][2]) * s;
z = (mat[0][1] - mat[1][0]) * s;
}
else
{
int idx = 0;
if (mat[1][1] > mat[0][0])
{
idx = 1;
if (mat[2][2] > mat[1][1])
idx = 2;
}
else if (mat[2][2] > mat[0][0])
{
idx = 2;
}
switch (idx)
{
case 0:
{
float st = std::sqrt(mat[0][0] - (mat[1][1] + mat[2][2]) + 1.f);
float s = 0.5f / st;
w = (mat[1][2] - mat[2][1]) * s;
x = 0.5f * st;
y = (mat[1][0] + mat[0][1]) * s;
z = (mat[2][0] + mat[0][2]) * s;
break;
}
case 1:
{
float st = std::sqrt(mat[1][1] - (mat[2][2] + mat[0][0]) + 1.f);
float s = 0.5f / st;
w = (mat[2][0] - mat[0][2]) * s;
x = (mat[1][0] + mat[0][1]) * s;
y = 0.5f * st;
z = (mat[2][1] + mat[1][2]) * s;
break;
}
case 2:
{
float st = std::sqrt(mat[2][2] - (mat[0][0] + mat[1][1]) + 1.f);
float s = 0.5f / st;
w = (mat[0][1] - mat[1][0]) * s;
x = (mat[2][0] + mat[0][2]) * s;
y = (mat[2][1] + mat[1][2]) * s;
z = 0.5f * st;
break;
}
default:
w = 0.f;
x = 0.f;
y = 0.f;
z = 0.f;
break;
}
}
}
void CQuaternion::fromVector3f(const CVector3f& vec)
{
float cosX = std::cos(0.5f * vec.x);
float cosY = std::cos(0.5f * vec.y);
float cosZ = std::cos(0.5f * vec.z);
float sinX = std::sin(0.5f * vec.x);
float sinY = std::sin(0.5f * vec.y);
float sinZ = std::sin(0.5f * vec.z);
w = cosZ * cosY * cosX + sinZ * sinY * sinX;
x = cosZ * cosY * sinX - sinZ * sinY * cosX;
y = cosZ * sinY * cosX + sinZ * cosY * sinX;
z = sinZ * cosY * cosX - cosZ * sinY * sinX;
}
CQuaternion& CQuaternion::operator=(const CQuaternion& q)
{
#if __SSE__
mVec128 = q.mVec128;
#else
w = q.w;
x = q.x;
y = q.y;
z = q.z;
#endif
return *this;
}
CQuaternion CQuaternion::operator+(const CQuaternion& q) const { return CQuaternion(w + q.w, x + q.x, y + q.y, z + q.z); }
CQuaternion CQuaternion::operator-(const CQuaternion& q) const { return CQuaternion(w - q.w, x - q.x, y - q.y, z - q.z); }
CQuaternion CQuaternion::operator*(const CQuaternion& q) const
{
return CQuaternion(w * q.w - CVector3f(x, y, z).dot({q.x, q.y, q.z}),
y * q.z - z * q.y + w * q.x + x * q.w,
z * q.x - x * q.z + w * q.y + y * q.w,
x * q.y - y * q.x + w * q.z + z * q.w);
}
CNUQuaternion CNUQuaternion::operator*(const CNUQuaternion& q) const
{
return CNUQuaternion(w * q.w - CVector3f(x, y, z).dot({q.x, q.y, q.z}),
y * q.z - z * q.y + w * q.x + x * q.w,
z * q.x - x * q.z + w * q.y + y * q.w,
x * q.y - y * q.x + w * q.z + z * q.w);
}
CQuaternion CQuaternion::operator/(const CQuaternion& q) const
{
CQuaternion p(q);
p.invert();
return *this * p;
}
CQuaternion CQuaternion::operator*(float scale) const { return CQuaternion(w * scale, x * scale, y * scale, z * scale); }
CNUQuaternion CNUQuaternion::operator*(float scale) const { return CNUQuaternion(w * scale, x * scale, y * scale, z * scale); }
CQuaternion CQuaternion::operator/(float scale) const { return CQuaternion(w / scale, x / scale, y / scale, z / scale); }
CQuaternion CQuaternion::operator-() const { return CQuaternion(-w, -x, -y, -z); }
const CQuaternion& CQuaternion::operator+=(const CQuaternion& q)
{
w += q.w;
x += q.x;
y += q.y;
z += q.z;
return *this;
}
const CNUQuaternion& CNUQuaternion::operator+=(const CNUQuaternion& q)
{
w += q.w;
x += q.x;
y += q.y;
z += q.z;
return *this;
}
const CQuaternion& CQuaternion::operator-=(const CQuaternion& q)
{
w -= q.w;
x -= q.x;
y -= q.y;
z -= q.z;
return *this;
}
const CQuaternion& CQuaternion::operator*=(const CQuaternion& q)
{
CQuaternion orig = *this;
w = orig.w * q.w - CVector3f(orig.x, orig.y, orig.z).dot({q.x, q.y, q.z});
x = orig.y * q.z - orig.z * q.y + orig.w * q.x + orig.x * q.w;
y = orig.z * q.x - orig.x * q.z + orig.w * q.y + orig.y * q.w;
z = orig.x * q.y - orig.y * q.x + orig.w * q.z + orig.z * q.w;
return *this;
}
const CQuaternion& CQuaternion::operator*=(float scale)
{
w *= scale;
x *= scale;
y *= scale;
z *= scale;
return *this;
}
const CQuaternion& CQuaternion::operator/=(float scale)
{
w /= scale;
x /= scale;
y /= scale;
z /= scale;
return *this;
}
void CQuaternion::invert()
{
x = -x;
y = -y;
z = -z;
}
CQuaternion CQuaternion::inverse() const { return CQuaternion(w, -x, -y, -z); }
CQuaternion CQuaternion::log() const
{
float a = std::acos(w);
float sina = std::sin(a);
CQuaternion ret;
ret.w = 0.f;
if (sina > 0.f)
{
ret.x = a * x / sina;
ret.y = a * y / sina;
ret.z = a * z / sina;
}
else
{
ret.x = 0.f;
ret.y = 0.f;
ret.z = 0.f;
CQuaternion::CQuaternion(const CMatrix3f& mat) {
float trace = mat[0][0] + mat[1][1] + mat[2][2];
if (trace >= 0.f) {
float st = std::sqrt(trace + 1.0f);
float s = 0.5f / st;
w() = 0.5f * st;
x() = (mat[1][2] - mat[2][1]) * s;
y() = (mat[2][0] - mat[0][2]) * s;
z() = (mat[0][1] - mat[1][0]) * s;
} else {
int idx = 0;
if (mat[1][1] > mat[0][0]) {
idx = 1;
if (mat[2][2] > mat[1][1])
idx = 2;
} else if (mat[2][2] > mat[0][0]) {
idx = 2;
}
return ret;
switch (idx) {
case 0: {
float st = std::sqrt(mat[0][0] - (mat[1][1] + mat[2][2]) + 1.f);
float s = 0.5f / st;
w() = (mat[1][2] - mat[2][1]) * s;
x() = 0.5f * st;
y() = (mat[1][0] + mat[0][1]) * s;
z() = (mat[2][0] + mat[0][2]) * s;
break;
}
case 1: {
float st = std::sqrt(mat[1][1] - (mat[2][2] + mat[0][0]) + 1.f);
float s = 0.5f / st;
w() = (mat[2][0] - mat[0][2]) * s;
x() = (mat[1][0] + mat[0][1]) * s;
y() = 0.5f * st;
z() = (mat[2][1] + mat[1][2]) * s;
break;
}
case 2: {
float st = std::sqrt(mat[2][2] - (mat[0][0] + mat[1][1]) + 1.f);
float s = 0.5f / st;
w() = (mat[0][1] - mat[1][0]) * s;
x() = (mat[2][0] + mat[0][2]) * s;
y() = (mat[2][1] + mat[1][2]) * s;
z() = 0.5f * st;
break;
}
default:
w() = 0.f;
x() = 0.f;
y() = 0.f;
z() = 0.f;
break;
}
}
}
CQuaternion CQuaternion::exp() const
{
float a = (CVector3f(x, y, z).magnitude());
float sina = std::sin(a);
float cosa = std::cos(a);
CQuaternion ret;
void CQuaternion::fromVector3f(const CVector3f& vec) {
float cosX = std::cos(0.5f * vec.x());
float cosY = std::cos(0.5f * vec.y());
float cosZ = std::cos(0.5f * vec.z());
ret.w = cosa;
if (a > 0.f)
{
ret.x = sina * x / a;
ret.y = sina * y / a;
ret.z = sina * z / a;
}
else
{
ret.x = 0.f;
ret.y = 0.f;
ret.z = 0.f;
}
float sinX = std::sin(0.5f * vec.x());
float sinY = std::sin(0.5f * vec.y());
float sinZ = std::sin(0.5f * vec.z());
return ret;
simd_floats f;
f[0] = cosZ * cosY * cosX + sinZ * sinY * sinX;
f[1] = cosZ * cosY * sinX - sinZ * sinY * cosX;
f[2] = cosZ * sinY * cosX + sinZ * cosY * sinX;
f[3] = sinZ * cosY * cosX - cosZ * sinY * sinX;
mSimd.copy_from(f);
}
CQuaternion& CQuaternion::operator=(const CQuaternion& q) {
mSimd = q.mSimd;
return *this;
}
CQuaternion CQuaternion::operator+(const CQuaternion& q) const {
return mSimd + q.mSimd;
}
CQuaternion CQuaternion::operator-(const CQuaternion& q) const {
return mSimd - q.mSimd;
}
CQuaternion CQuaternion::operator*(const CQuaternion& q) const {
return CQuaternion(w() * q.w() - CVector3f(x(), y(), z()).dot({q.x(), q.y(), q.z()}),
y() * q.z() - z() * q.y() + w() * q.x() + x() * q.w(),
z() * q.x() - x() * q.z() + w() * q.y() + y() * q.w(),
x() * q.y() - y() * q.x() + w() * q.z() + z() * q.w());
}
CNUQuaternion CNUQuaternion::operator*(const CNUQuaternion& q) const {
return CNUQuaternion(w() * q.w() - CVector3f(x(), y(), z()).dot({q.x(), q.y(), q.z()}),
y() * q.z() - z() * q.y() + w() * q.x() + x() * q.w(),
z() * q.x() - x() * q.z() + w() * q.y() + y() * q.w(),
x() * q.y() - y() * q.x() + w() * q.z() + z() * q.w());
}
CQuaternion CQuaternion::operator/(const CQuaternion& q) const {
CQuaternion p(q);
p.invert();
return *this * p;
}
CQuaternion CQuaternion::operator*(float scale) const {
return mSimd * simd<float>(scale);
}
CNUQuaternion CNUQuaternion::operator*(float scale) const {
return mSimd * simd<float>(scale);
}
CQuaternion CQuaternion::operator/(float scale) const {
return mSimd / simd<float>(scale);
}
CQuaternion CQuaternion::operator-() const { return -mSimd; }
const CQuaternion& CQuaternion::operator+=(const CQuaternion& q) {
mSimd += q.mSimd;
return *this;
}
const CNUQuaternion& CNUQuaternion::operator+=(const CNUQuaternion& q) {
mSimd += q.mSimd;
return *this;
}
const CQuaternion& CQuaternion::operator-=(const CQuaternion& q) {
mSimd -= q.mSimd;
return *this;
}
const CQuaternion& CQuaternion::operator*=(const CQuaternion& q) {
CQuaternion orig = *this;
w() = orig.w() * q.w() - CVector3f(orig.x(), orig.y(), orig.z()).dot({q.x(), q.y(), q.z()});
x() = orig.y() * q.z() - orig.z() * q.y() + orig.w() * q.x() + orig.x() * q.w();
y() = orig.z() * q.x() - orig.x() * q.z() + orig.w() * q.y() + orig.y() * q.w();
z() = orig.x() * q.y() - orig.y() * q.x() + orig.w() * q.z() + orig.z() * q.w();
return *this;
}
const CQuaternion& CQuaternion::operator*=(float scale) {
mSimd *= simd<float>(scale);
return *this;
}
const CQuaternion& CQuaternion::operator/=(float scale) {
mSimd /= simd<float>(scale);
return *this;
}
static const simd<float> InvertQuat(1.f, -1.f, -1.f, -1.f);
void CQuaternion::invert() {
mSimd *= InvertQuat;
}
CQuaternion CQuaternion::inverse() const { return mSimd * InvertQuat; }
CQuaternion CQuaternion::log() const {
float a = std::acos(w());
float sina = std::sin(a);
CQuaternion ret;
if (sina > 0.f)
ret = a * *this / sina;
else
ret = simd<float>(0.f);
ret.w() = 0.f;
return ret;
}
CQuaternion CQuaternion::exp() const {
float a = (CVector3f(mSimd.shuffle<1, 2, 3, 3>()).magnitude());
float sina = std::sin(a);
float cosa = std::cos(a);
CQuaternion ret;
if (a > 0.f)
ret = sina * *this / a;
else
ret = simd<float>(0.f);
ret.w() = cosa;
return ret;
}
CQuaternion CQuaternion::lerp(const CQuaternion& a, const CQuaternion& b, double t) { return (a + t * (b - a)); }
CQuaternion CQuaternion::nlerp(const CQuaternion& a, const CQuaternion& b, double t) { return lerp(a, b, t).normalized(); }
CQuaternion CQuaternion::slerp(const CQuaternion& a, const CQuaternion& b, double t)
{
if (t <= 0.0f)
return a;
if (t >= 1.0f)
return b;
CQuaternion ret;
float mag = std::sqrt(a.dot(a) * b.dot(b));
float prod = a.dot(b) / mag;
if (std::fabs(prod) < 1.0f)
{
const double sign = (prod < 0.0f) ? -1.0f : 1.0f;
const double theta = std::acos(sign * prod);
const double s1 = std::sin(sign * t * theta);
const double d = 1.0 / std::sin(theta);
const double s0 = std::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);
ret.w = float((a.w * s0 + b.w * s1) * d);
return ret;
}
return a;
CQuaternion CQuaternion::nlerp(const CQuaternion& a, const CQuaternion& b, double t) {
return lerp(a, b, t).normalized();
}
CQuaternion CQuaternion::shortestRotationArc(const zeus::CVector3f& v0, const zeus::CVector3f& v1)
{
CVector3f v0N = v0;
CVector3f v1N = v1;
CQuaternion CQuaternion::slerp(const CQuaternion& a, const CQuaternion& b, double t) {
if (t <= 0.0f)
return a;
if (t >= 1.0f)
return b;
if (!v0N.isZero())
v0N.normalize();
if (!v1N.isZero())
v1N.normalize();
CQuaternion ret;
CVector3f cross = v0N.cross(v1N);
float mag = std::sqrt(a.dot(a) * b.dot(b));
if (cross.magSquared() < 0.001f)
{
if (v0N.dot(v1N) > 0.f)
return CQuaternion::skNoRotation;
if (cross.canBeNormalized())
return CQuaternion(0.0f, cross.normalized());
return CQuaternion::skNoRotation;
}
else
{
float w = std::sqrt((1.f + zeus::clamp(-1.f, v0N.dot(v1N), 1.f)) * 2.f);
return CQuaternion(0.5f * w, cross * (1.f / w));
}
float prod = a.dot(b) / mag;
if (std::fabs(prod) < 1.0f) {
const double sign = (prod < 0.0f) ? -1.0f : 1.0f;
const double theta = std::acos(sign * prod);
const double s1 = std::sin(sign * t * theta);
const double d = 1.0 / std::sin(theta);
const double s0 = std::sin((1.0 - t) * theta);
ret = (a * s0 + b * s1) * d;
return ret;
}
return a;
}
CQuaternion CQuaternion::shortestRotationArc(const zeus::CVector3f& v0, const zeus::CVector3f& v1) {
CVector3f v0N = v0;
CVector3f v1N = v1;
if (!v0N.isZero())
v0N.normalize();
if (!v1N.isZero())
v1N.normalize();
CVector3f cross = v0N.cross(v1N);
if (cross.magSquared() < 0.001f) {
if (v0N.dot(v1N) > 0.f)
return CQuaternion::skNoRotation;
if (cross.canBeNormalized())
return CQuaternion(0.0f, cross.normalized());
return CQuaternion::skNoRotation;
} else {
float w = std::sqrt((1.f + zeus::clamp(-1.f, v0N.dot(v1N), 1.f)) * 2.f);
return CQuaternion(0.5f * w, cross * (1.f / w));
}
}
CQuaternion CQuaternion::clampedRotateTo(const zeus::CUnitVector3f& v0, const zeus::CUnitVector3f& v1,
const zeus::CRelAngle& angle)
{
CQuaternion arc = shortestRotationArc(v0, v1);
if (angle >= 2.f * std::acos(arc.w))
return arc;
const zeus::CRelAngle& angle) {
CQuaternion arc = shortestRotationArc(v0, v1);
if (angle >= 2.f * std::acos(arc.w()))
return arc;
return fromAxisAngle(arc.getImaginary(), angle);
return fromAxisAngle(arc.getImaginary(), angle);
}
CQuaternion CQuaternion::slerpShort(const CQuaternion& a, const CQuaternion& b, double t)
{
return zeus::CQuaternion::slerp((b.dot(a) >= 0.f) ? a : a.buildEquivalent(), b, t);
CQuaternion CQuaternion::slerpShort(const CQuaternion& a, const CQuaternion& b, double t) {
return zeus::CQuaternion::slerp((b.dot(a) >= 0.f) ? a : a.buildEquivalent(), b, t);
}
CQuaternion operator+(float lhs, const CQuaternion& rhs)
{
return CQuaternion(lhs + rhs.w, lhs * rhs.x, lhs * rhs.y, lhs * rhs.z);
CQuaternion operator+(float lhs, const CQuaternion& rhs) {
return simd<float>(lhs) + rhs.mSimd;
}
CQuaternion operator-(float lhs, const CQuaternion& rhs)
{
return CQuaternion(lhs - rhs.w, lhs * rhs.x, lhs * rhs.y, lhs * rhs.z);
CQuaternion operator-(float lhs, const CQuaternion& rhs) {
return simd<float>(lhs) - rhs.mSimd;
}
CQuaternion operator*(float lhs, const CQuaternion& rhs)
{
return CQuaternion(lhs * rhs.w, lhs * rhs.x, lhs * rhs.y, lhs * rhs.z);
CQuaternion operator*(float lhs, const CQuaternion& rhs) {
return simd<float>(lhs) * rhs.mSimd;
}
CNUQuaternion operator*(float lhs, const CNUQuaternion& rhs)
{
return CNUQuaternion(lhs * rhs.w, lhs * rhs.x, lhs * rhs.y, lhs * rhs.z);
CNUQuaternion operator*(float lhs, const CNUQuaternion& rhs) {
return simd<float>(lhs) * rhs.mSimd;
}
CQuaternion CQuaternion::buildEquivalent() const
{
float tmp = std::acos(clamp(-1.f, w, 1.f)) * 2.0;
if (std::fabs(tmp) < 1.0e-7)
return {-1.f, 0.f, 0.f, 0.f};
else
return CQuaternion::fromAxisAngle(CUnitVector3f(x, y, z), tmp + 2.0 * M_PI);
CQuaternion CQuaternion::buildEquivalent() const {
float tmp = std::acos(clamp(-1.f, w(), 1.f)) * 2.f;
if (std::fabs(tmp) < 1.0e-7)
return {-1.f, 0.f, 0.f, 0.f};
else
return CQuaternion::fromAxisAngle(CUnitVector3f(mSimd.shuffle<1, 2, 3, 3>()), tmp + 2.0 * M_PI);
}
CRelAngle CQuaternion::angleFrom(const zeus::CQuaternion& other)
{
return std::acos(zeus::clamp(-1.f, dot(other), 1.f));
CRelAngle CQuaternion::angleFrom(const zeus::CQuaternion& other) {
return std::acos(zeus::clamp(-1.f, dot(other), 1.f));
}
CQuaternion CQuaternion::lookAt(const CUnitVector3f& source, const CUnitVector3f& dest, const CRelAngle& maxAng)
{
CQuaternion q = skNoRotation;
zeus::CVector3f destNoZ = dest;
zeus::CVector3f sourceNoZ = source;
destNoZ.z = 0.f;
sourceNoZ.z = 0.f;
zeus::CVector3f tmp;
if (sourceNoZ.magSquared() > 0.0001f && destNoZ.magSquared() > 0.0001f)
{
sourceNoZ.normalize();
destNoZ.normalize();
CQuaternion CQuaternion::lookAt(const CUnitVector3f& source, const CUnitVector3f& dest, const CRelAngle& maxAng) {
CQuaternion q = skNoRotation;
zeus::CVector3f destNoZ = dest;
zeus::CVector3f sourceNoZ = source;
destNoZ.z() = 0.f;
sourceNoZ.z() = 0.f;
zeus::CVector3f tmp;
if (sourceNoZ.magSquared() > 0.0001f && destNoZ.magSquared() > 0.0001f) {
sourceNoZ.normalize();
destNoZ.normalize();
float angleBetween =
normalize_angle(std::atan2(destNoZ.x, destNoZ.y) - std::atan2(sourceNoZ.x, sourceNoZ.y));
float realAngle = zeus::clamp(-maxAng.asRadians(), angleBetween, maxAng.asRadians());
CQuaternion tmpQ;
tmpQ.rotateZ(-realAngle);
q = tmpQ;
tmp = q.transform(sourceNoZ);
}
else if (sourceNoZ.magSquared() > 0.0001f)
tmp = sourceNoZ.normalized();
else if (destNoZ.magSquared() > 0.0001f)
tmp = destNoZ.normalized();
else
return skNoRotation;
float angleBetween =
normalize_angle(std::atan2(destNoZ.x(), destNoZ.y()) - std::atan2(sourceNoZ.x(), sourceNoZ.y()));
float realAngle = zeus::clamp(-maxAng.asRadians(), angleBetween, maxAng.asRadians());
CQuaternion tmpQ;
tmpQ.rotateZ(-realAngle);
q = tmpQ;
tmp = q.transform(sourceNoZ);
} else if (sourceNoZ.magSquared() > 0.0001f)
tmp = sourceNoZ.normalized();
else if (destNoZ.magSquared() > 0.0001f)
tmp = destNoZ.normalized();
else
return skNoRotation;
float realAngle =
zeus::clamp(-maxAng.asRadians(), normalize_angle(std::acos(dest.z) - std::acos(source.z)), maxAng.asRadians());
return CQuaternion::fromAxisAngle(tmp.cross(CVector3f::skUp), -realAngle) * q;
float realAngle =
zeus::clamp(-maxAng.asRadians(), normalize_angle(std::acos(dest.z()) - std::acos(source.z())), maxAng.asRadians());
return CQuaternion::fromAxisAngle(tmp.cross(CVector3f::skUp), -realAngle) * q;
}
}

98
src/CTransform.cpp
View File

@@ -1,69 +1,65 @@
#include "zeus/CTransform.hpp"
namespace zeus
{
CTransform CTransformFromEditorEuler(const CVector3f& eulerVec)
{
CTransform result;
double ti, tj, th, ci, cj, ch, si, sj, sh, cc, cs, sc, ss;
namespace zeus {
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];
ti = eulerVec[0];
tj = eulerVec[1];
th = eulerVec[2];
ci = std::cos(ti);
cj = std::cos(tj);
ch = std::cos(th);
si = std::sin(ti);
sj = std::sin(tj);
sh = std::sin(th);
ci = std::cos(ti);
cj = std::cos(tj);
ch = std::cos(th);
si = std::sin(ti);
sj = std::sin(tj);
sh = std::sin(th);
cc = ci * ch;
cs = ci * sh;
sc = si * ch;
ss = si * sh;
cc = ci * ch;
cs = ci * sh;
sc = si * ch;
ss = si * sh;
result.basis.m[0][0] = float(cj * ch);
result.basis.m[1][0] = float(sj * sc - cs);
result.basis.m[2][0] = float(sj * cc + ss);
result.basis.m[0][1] = float(cj * sh);
result.basis.m[1][1] = float(sj * ss + cc);
result.basis.m[2][1] = float(sj * cs - sc);
result.basis.m[0][2] = float(-sj);
result.basis.m[1][2] = float(cj * si);
result.basis.m[2][2] = float(cj * ci);
result.basis.m[0][0] = float(cj * ch);
result.basis.m[1][0] = float(sj * sc - cs);
result.basis.m[2][0] = float(sj * cc + ss);
result.basis.m[0][1] = float(cj * sh);
result.basis.m[1][1] = float(sj * ss + cc);
result.basis.m[2][1] = float(sj * cs - sc);
result.basis.m[0][2] = float(-sj);
result.basis.m[1][2] = float(cj * si);
result.basis.m[2][2] = float(cj * ci);
return result;
return result;
}
CTransform CTransformFromAxisAngle(const CVector3f& axis, float angle)
{
CTransform result;
CVector3f axisN = axis.normalized();
CTransform CTransformFromAxisAngle(const CVector3f& axis, float angle) {
CTransform result;
CVector3f axisN = axis.normalized();
float c = std::cos(angle);
float s = std::sin(angle);
float t = 1.f - c;
float c = std::cos(angle);
float s = std::sin(angle);
float t = 1.f - c;
result.basis.m[0][0] = t * axisN.v[0] * axisN.v[0] + c;
result.basis.m[1][0] = t * axisN.v[0] * axisN.v[1] - axisN.v[2] * s;
result.basis.m[2][0] = t * axisN.v[0] * axisN.v[2] + axisN.v[1] * s;
result.basis.m[0][0] = t * axisN[0] * axisN[0] + c;
result.basis.m[1][0] = t * axisN[0] * axisN[1] - axisN[2] * s;
result.basis.m[2][0] = t * axisN[0] * axisN[2] + axisN[1] * s;
result.basis.m[0][1] = t * axisN.v[0] * axisN.v[1] + axisN.v[2] * s;
result.basis.m[1][1] = t * axisN.v[1] * axisN.v[1] + c;
result.basis.m[2][1] = t * axisN.v[1] * axisN.v[2] - axisN.v[0] * s;
result.basis.m[0][1] = t * axisN[0] * axisN[1] + axisN[2] * s;
result.basis.m[1][1] = t * axisN[1] * axisN[1] + c;
result.basis.m[2][1] = t * axisN[1] * axisN[2] - axisN[0] * s;
result.basis.m[0][2] = t * axisN.v[0] * axisN.v[2] - axisN.v[1] * s;
result.basis.m[1][2] = t * axisN.v[1] * axisN.v[2] + axisN.v[0] * s;
result.basis.m[2][2] = t * axisN.v[2] * axisN.v[2] + c;
result.basis.m[0][2] = t * axisN[0] * axisN[2] - axisN[1] * s;
result.basis.m[1][2] = t * axisN[1] * axisN[2] + axisN[0] * s;
result.basis.m[2][2] = t * axisN[2] * axisN[2] + c;
return result;
return result;
}
CTransform CTransformFromEditorEulers(const CVector3f& eulerVec, const CVector3f& origin)
{
CTransform ret = CTransformFromEditorEuler(eulerVec);
ret.origin = origin;
return ret;
CTransform CTransformFromEditorEulers(const CVector3f& eulerVec, const CVector3f& origin) {
CTransform ret = CTransformFromEditorEuler(eulerVec);
ret.origin = origin;
return ret;
}
}

68
src/CVector2f.cpp
View File

@@ -4,50 +4,46 @@
#include <cassert>
#include "zeus/Math.hpp"
namespace zeus
{
namespace zeus {
const CVector2f CVector2f::skOne = CVector2f(1.0);
const CVector2f CVector2f::skNegOne = CVector2f(-1.0);
const CVector2f CVector2f::skZero(0.f, 0.f);
float CVector2f::getAngleDiff(const CVector2f& a, const CVector2f& b)
{
float mag1 = a.magnitude();
float mag2 = b.magnitude();
float CVector2f::getAngleDiff(const CVector2f& a, const CVector2f& b) {
float mag1 = a.magnitude();
float mag2 = b.magnitude();
if (!mag1 || !mag2)
return 0;
if (!mag1 || !mag2)
return 0;
float dot = a.dot(b);
float theta = std::acos(dot / (mag1 * mag2));
return theta;
float dot = a.dot(b);
float theta = std::acos(dot / (mag1 * mag2));
return theta;
}
CVector2f CVector2f::slerp(const CVector2f& a, const CVector2f& b, float t)
{
if (t <= 0.0f)
return a;
if (t >= 1.0f)
return b;
CVector2f ret;
float mag = std::sqrt(a.dot(a) * b.dot(b));
float prod = a.dot(b) / mag;
if (std::fabs(prod) < 1.0f)
{
const double sign = (prod < 0.0f) ? -1.0f : 1.0f;
const double theta = std::acos(sign * prod);
const double s1 = std::sin(sign * t * theta);
const double d = 1.0 / std::sin(theta);
const double s0 = std::sin((1.0f - t) * theta);
ret = (a * s0 + b * s1) * d;
return ret;
}
CVector2f CVector2f::slerp(const CVector2f& a, const CVector2f& b, float t) {
if (t <= 0.0f)
return a;
if (t >= 1.0f)
return b;
CVector2f ret;
float mag = std::sqrt(a.dot(a) * b.dot(b));
float prod = a.dot(b) / mag;
if (std::fabs(prod) < 1.0f) {
const double sign = (prod < 0.0f) ? -1.0f : 1.0f;
const double theta = std::acos(sign * prod);
const double s1 = std::sin(sign * t * theta);
const double d = 1.0 / std::sin(theta);
const double s0 = std::sin((1.0f - t) * theta);
ret = (a * s0 + b * s1) * d;
return ret;
}
return a;
}
}

90
src/CVector3f.cpp
View File

@@ -5,8 +5,7 @@
#include <cassert>
#include "zeus/Math.hpp"
namespace zeus
{
namespace zeus {
const CVector3f CVector3f::skOne(1.f);
const CVector3f CVector3f::skNegOne(-1.f);
const CVector3f CVector3f::skZero;
@@ -20,59 +19,44 @@ const CVector3f CVector3f::skRadToDegVec(180.0f / M_PIF);
const CVector3f CVector3f::skDegToRadVec(M_PIF / 180.0f);
const CVector3d CVector3d::skZero(0.0, 0.0, 0.0);
CVector3f::CVector3f(const CVector3d& vec)
{
#if __SSE__
mVec128 = _mm_cvtpd_ps(vec.mVec128[0]);
v[2] = vec.v[2];
#else
v[0] = vec.v[0];
v[1] = vec.v[1];
v[2] = vec.v[2];
#endif
CVector3f::CVector3f(const CVector3d& vec) : mSimd(vec.mSimd) {}
float CVector3f::getAngleDiff(const CVector3f& a, const CVector3f& b) {
float mag1 = a.magnitude();
float mag2 = b.magnitude();
if (!mag1 || !mag2)
return 0.f;
float dot = a.dot(b);
float theta = std::acos(dot / (mag1 * mag2));
return theta;
}
float CVector3f::getAngleDiff(const CVector3f& a, const CVector3f& b)
{
float mag1 = a.magnitude();
float mag2 = b.magnitude();
if (!mag1 || !mag2)
return 0.f;
float dot = a.dot(b);
float theta = std::acos(dot / (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 = std::sqrt(a.dot(a) * b.dot(b));
float prod = a.dot(b) / mag;
if (std::fabs(prod) < 1.0f)
{
const double sign = (prod < 0.0f) ? -1.0f : 1.0f;
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;
}
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 = std::sqrt(a.dot(a) * b.dot(b));
float prod = a.dot(b) / mag;
if (std::fabs(prod) < 1.0f) {
const double sign = (prod < 0.0f) ? -1.0f : 1.0f;
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 = (a * s0 + b * s1) * d;
return ret;
}
return a;
}
}

16
src/CVector4f.cpp
View File

@@ -1,19 +1,13 @@
#include "zeus/CVector4f.hpp"
#include "zeus/CColor.hpp"
namespace zeus
{
namespace zeus {
const CVector4f CVector4f::skZero(0.f, 0.f, 0.f, 0.f);
CVector4f::CVector4f(const zeus::CColor& other) : x(other.r), y(other.g), z(other.b), w(other.a) {}
CVector4f::CVector4f(const zeus::CColor& other) : mSimd(other.mSimd) {}
CVector4f& CVector4f::operator=(const CColor& other)
{
x = other.r;
y = other.g;
z = other.b;
w = other.a;
return *this;
CVector4f& CVector4f::operator=(const CColor& other) {
mSimd = other.mSimd;
return *this;
}
}

412
src/Math.cpp
View File

@@ -2,312 +2,292 @@
#include "zeus/CTransform.hpp"
#include "zeus/CVector3f.hpp"
#include "zeus/CVector2f.hpp"
#if _WIN32
#include <intrin.h>
#else
#include <cpuid.h>
#endif
namespace zeus
{
namespace zeus {
static bool isCPUInit = false;
static CPUInfo g_cpuFeatures = {};
static CPUInfo g_missingFeatures = {};
void getCpuInfo(int eax, int regs[4])
{
void getCpuInfo(int eax, int regs[4]) {
#if !GEKKO
#if _WIN32
__cpuid(regs, eax);
__cpuid(regs, eax);
#else
__cpuid(eax, regs[0], regs[1], regs[2], regs[3]);
__cpuid(eax, regs[0], regs[1], regs[2], regs[3]);
#endif
#endif
}
void getCpuInfoEx(int eax, int ecx, int regs[4])
{
void getCpuInfoEx(int eax, int ecx, int regs[4]) {
#if !GEKKO
#if _WIN32
__cpuidex(regs, eax, ecx);
__cpuidex(regs, eax, ecx);
#else
__cpuid_count(eax, ecx, regs[0], regs[1], regs[2], regs[3]);
__cpuid_count(eax, ecx, regs[0], regs[1], regs[2], regs[3]);
#endif
#endif
}
void detectCPU()
{
void detectCPU() {
#if !GEKKO
if (isCPUInit)
return;
if (isCPUInit)
return;
int regs[4];
getCpuInfo(0, regs);
int highestFeature = regs[0];
*reinterpret_cast<int*>((char*)g_cpuFeatures.cpuVendor) = regs[1];
*reinterpret_cast<int*>((char*)g_cpuFeatures.cpuVendor + 4) = regs[3];
*reinterpret_cast<int*>((char*)g_cpuFeatures.cpuVendor + 8) = regs[2];
getCpuInfo(0x80000000, regs);
if (regs[0] >= 0x80000004)
{
for (unsigned int i = 0x80000002; i <= 0x80000004; i++)
{
getCpuInfo(i, regs);
// Interpret CPU brand string and cache information.
if (i == 0x80000002)
memcpy((char*)g_cpuFeatures.cpuBrand, regs, sizeof(regs));
else if (i == 0x80000003)
memcpy((char*)g_cpuFeatures.cpuBrand + 16, regs, sizeof(regs));
else if (i == 0x80000004)
memcpy((char*)g_cpuFeatures.cpuBrand + 32, regs, sizeof(regs));
}
int regs[4];
getCpuInfo(0, regs);
int highestFeature = regs[0];
*reinterpret_cast<int*>((char*) g_cpuFeatures.cpuVendor) = regs[1];
*reinterpret_cast<int*>((char*) g_cpuFeatures.cpuVendor + 4) = regs[3];
*reinterpret_cast<int*>((char*) g_cpuFeatures.cpuVendor + 8) = regs[2];
getCpuInfo(0x80000000, regs);
if (regs[0] >= 0x80000004) {
for (unsigned int i = 0x80000002; i <= 0x80000004; i++) {
getCpuInfo(i, regs);
// Interpret CPU brand string and cache information.
if (i == 0x80000002)
memcpy((char*) g_cpuFeatures.cpuBrand, regs, sizeof(regs));
else if (i == 0x80000003)
memcpy((char*) g_cpuFeatures.cpuBrand + 16, regs, sizeof(regs));
else if (i == 0x80000004)
memcpy((char*) g_cpuFeatures.cpuBrand + 32, regs, sizeof(regs));
}
}
if (highestFeature >= 1)
{
getCpuInfo(1, regs);
memset((bool*)&g_cpuFeatures.AESNI, ((regs[2] & 0x02000000) != 0), 1);
memset((bool*)&g_cpuFeatures.SSE1, ((regs[3] & 0x02000000) != 0), 1);
memset((bool*)&g_cpuFeatures.SSE2, ((regs[3] & 0x04000000) != 0), 1);
memset((bool*)&g_cpuFeatures.SSE3, ((regs[2] & 0x00000001) != 0), 1);
memset((bool*)&g_cpuFeatures.SSSE3, ((regs[2] & 0x00000200) != 0), 1);
memset((bool*)&g_cpuFeatures.SSE41, ((regs[2] & 0x00080000) != 0), 1);
memset((bool*)&g_cpuFeatures.SSE42, ((regs[2] & 0x00100000) != 0), 1);
memset((bool*)&g_cpuFeatures.AVX, ((regs[2] & 0x10000000) != 0), 1);
}
if (highestFeature >= 1) {
getCpuInfo(1, regs);
memset((bool*) &g_cpuFeatures.AESNI, ((regs[2] & 0x02000000) != 0), 1);
memset((bool*) &g_cpuFeatures.SSE1, ((regs[3] & 0x02000000) != 0), 1);
memset((bool*) &g_cpuFeatures.SSE2, ((regs[3] & 0x04000000) != 0), 1);
memset((bool*) &g_cpuFeatures.SSE3, ((regs[2] & 0x00000001) != 0), 1);
memset((bool*) &g_cpuFeatures.SSSE3, ((regs[2] & 0x00000200) != 0), 1);
memset((bool*) &g_cpuFeatures.SSE41, ((regs[2] & 0x00080000) != 0), 1);
memset((bool*) &g_cpuFeatures.SSE42, ((regs[2] & 0x00100000) != 0), 1);
memset((bool*) &g_cpuFeatures.AVX, ((regs[2] & 0x10000000) != 0), 1);
}
if (highestFeature >= 7)
{
getCpuInfoEx(7, 0, regs);
memset((bool*)&g_cpuFeatures.AVX2, ((regs[1] & 0x00000020) != 0), 1);
}
if (highestFeature >= 7) {
getCpuInfoEx(7, 0, regs);
memset((bool*) &g_cpuFeatures.AVX2, ((regs[1] & 0x00000020) != 0), 1);
}
isCPUInit = true;
isCPUInit = true;
#endif
}
const CPUInfo& cpuFeatures() { detectCPU(); return g_cpuFeatures; }
const CPUInfo& cpuFeatures() {
detectCPU();
return g_cpuFeatures;
}
std::pair<bool, const CPUInfo&> validateCPU()
{
detectCPU();
bool ret = true;
std::pair<bool, const CPUInfo&> validateCPU() {
detectCPU();
bool ret = true;
#if __AVX2__
if (!g_cpuFeatures.AVX2)
{
*(bool*) &g_missingFeatures.AVX2 = true;
ret = false;
}
if (!g_cpuFeatures.AVX2) {
*(bool*) &g_missingFeatures.AVX2 = true;
ret = false;
}
#endif
#if __AVX__
if (!g_cpuFeatures.AVX)
{
*(bool*) &g_missingFeatures.AVX = true;
ret = false;
}
if (!g_cpuFeatures.AVX) {
*(bool*) &g_missingFeatures.AVX = true;
ret = false;
}
#endif
#if __SSE4A__
if (!g_cpuFeatures.SSE4a)
{
*(bool*) &g_missingFeatures.SSE4a = true;
ret = false;
}
if (!g_cpuFeatures.SSE4a)
{
*(bool*) &g_missingFeatures.SSE4a = true;
ret = false;
}
#endif
#if __SSE4_2__
if (!g_cpuFeatures.SSE42)
{
*(bool*) &g_missingFeatures.SSE42 = true;
ret = false;
}
if (!g_cpuFeatures.SSE42) {
*(bool*) &g_missingFeatures.SSE42 = true;
ret = false;
}
#endif
#if __SSE4_1__
if (!g_cpuFeatures.SSE41)
{
*(bool*) &g_missingFeatures.SSE41 = true;
ret = false;
}
if (!g_cpuFeatures.SSE41) {
*(bool*) &g_missingFeatures.SSE41 = true;
ret = false;
}
#endif
#if __SSSE3__
if (!g_cpuFeatures.SSSE3)
{
*(bool*) &g_missingFeatures.SSSE3 = true;
ret = false;
}
if (!g_cpuFeatures.SSSE3) {
*(bool*) &g_missingFeatures.SSSE3 = true;
ret = false;
}
#endif
#if __SSE3__
if (!g_cpuFeatures.SSE3)
{
*(bool*) &g_missingFeatures.SSE3 = true;
ret = false;
}
if (!g_cpuFeatures.SSE3) {
*(bool*) &g_missingFeatures.SSE3 = true;
ret = false;
}
#endif
#if __SSE2__
if (!g_cpuFeatures.SSE2)
{
*(bool*) &g_missingFeatures.SSE2 = true;
ret = false;
}
if (!g_cpuFeatures.SSE2) {
*(bool*) &g_missingFeatures.SSE2 = true;
ret = false;
}
#endif
#if __SSE__
if (!g_cpuFeatures.SSE1)
{
*(bool*) &g_missingFeatures.SSE1 = true;
ret = false;
}
if (!g_cpuFeatures.SSE1) {
*(bool*) &g_missingFeatures.SSE1 = true;
ret = false;
}
#endif
return {ret, g_missingFeatures};
return {ret, g_missingFeatures};
}
CTransform lookAt(const CVector3f& pos, const CVector3f& lookPos, const CVector3f& up)
{
CVector3f vLook, vRight, vUp;
CTransform lookAt(const CVector3f& pos, const CVector3f& lookPos, const CVector3f& up) {
CVector3f vLook, vRight, vUp;
vLook = lookPos - pos;
if (vLook.magnitude() <= FLT_EPSILON)
vLook = {0.f, 1.f, 0.f};
else
vLook.normalize();
vLook = lookPos - pos;
if (vLook.magnitude() <= FLT_EPSILON)
vLook = {0.f, 1.f, 0.f};
else
vLook.normalize();
vUp = up - vLook * clamp(-1.f, up.dot(vLook), 1.f);
vUp = up - vLook * clamp(-1.f, up.dot(vLook), 1.f);
if (vUp.magnitude() <= FLT_EPSILON) {
vUp = CVector3f(0.f, 0.f, 1.f) - vLook * vLook.z();
if (vUp.magnitude() <= FLT_EPSILON)
{
vUp = CVector3f(0.f, 0.f, 1.f) - vLook * vLook.z;
if (vUp.magnitude() <= FLT_EPSILON)
vUp = CVector3f(0.f, 1.f, 0.f) - vLook * vLook.y;
}
vUp.normalize();
vRight = vLook.cross(vUp);
vUp = CVector3f(0.f, 1.f, 0.f) - vLook * vLook.y();
}
vUp.normalize();
vRight = vLook.cross(vUp);
CMatrix3f rmBasis(vRight, vLook, vUp);
return CTransform(rmBasis, pos);
CMatrix3f rmBasis(vRight, vLook, vUp);
return CTransform(rmBasis, pos);
}
CVector3f getBezierPoint(const CVector3f& a, const CVector3f& b,
const CVector3f& c, const CVector3f& d, float t)
{
const float omt = 1.f - t;
return ((a * omt + b * t) * omt + (b * omt + c * t) * t) * omt +
((b * omt + c * t) * omt + (c * omt + d * t) * t) * t;
const CVector3f& c, const CVector3f& d, float t) {
const float omt = 1.f - t;
return ((a * omt + b * t) * omt + (b * omt + c * t) * t) * omt +
((b * omt + c * t) * omt + (c * omt + d * t) * t) * t;
}
int floorPowerOfTwo(int x)
{
if (x == 0)
return 0;
/*
* we want to ensure that we always get the previous power,
* but if we have values like 256, we'll always get the same value,
* x-1 ensures that we always get the previous power.
*/
x = (x - 1) | (x >> 1);
x = x | (x >> 2);
x = x | (x >> 4);
x = x | (x >> 8);
x = x | (x >> 16);
return x - (x >> 1);
int floorPowerOfTwo(int x) {
if (x == 0)
return 0;
/*
* we want to ensure that we always get the previous power,
* but if we have values like 256, we'll always get the same value,
* x-1 ensures that we always get the previous power.
*/
x = (x - 1) | (x >> 1);
x = x | (x >> 2);
x = x | (x >> 4);
x = x | (x >> 8);
x = x | (x >> 16);
return x - (x >> 1);
}
int ceilingPowerOfTwo(int x)
{
if (x == 0)
return 0;
int ceilingPowerOfTwo(int x) {
if (x == 0)
return 0;
x--;
x |= x >> 1;
x |= x >> 2;
x |= x >> 4;
x |= x >> 8;
x |= x >> 16;
x++;
x--;
x |= x >> 1;
x |= x >> 2;
x |= x >> 4;
x |= x >> 8;
x |= x >> 16;
x++;
return x;
return x;
}
float getCatmullRomSplinePoint(float a, float b, float c, float d, float t)
{
if (t <= 0.0f)
return b;
if (t >= 1.0f)
return c;
float getCatmullRomSplinePoint(float a, float b, float c, float d, float t) {
if (t <= 0.0f)
return b;
if (t >= 1.0f)
return c;
const float t2 = t * t;
const float t3 = t2 * t;
const float t2 = t * t;
const float t3 = t2 * t;
return (a * (-0.5f * t3 + t2 - 0.5f * t) + b * (1.5f * t3 + -2.5f * t2 + 1.0f) + c * (-1.5f * t3 + 2.0f * t2 + 0.5f * t) +
d * (0.5f * t3 - 0.5f * t2));
return (a * (-0.5f * t3 + t2 - 0.5f * t) + b * (1.5f * t3 + -2.5f * t2 + 1.0f) +
c * (-1.5f * t3 + 2.0f * t2 + 0.5f * t) +
d * (0.5f * t3 - 0.5f * t2));
}
CVector3f getCatmullRomSplinePoint(const CVector3f& a, const CVector3f& b, const CVector3f& c, const CVector3f& d, float t)
{
if (t <= 0.0f)
return b;
if (t >= 1.0f)
return c;
CVector3f
getCatmullRomSplinePoint(const CVector3f& a, const CVector3f& b, const CVector3f& c, const CVector3f& d, float t) {
if (t <= 0.0f)
return b;
if (t >= 1.0f)
return c;
const float t2 = t * t;
const float t3 = t2 * t;
const float t2 = t * t;
const float t3 = t2 * t;
return (a * (-0.5f * t3 + t2 - 0.5f * t) + b * (1.5f * t3 + -2.5f * t2 + 1.0f) + c * (-1.5f * t3 + 2.0f * t2 + 0.5f * t) +
d * (0.5f * t3 - 0.5f * t2));
return (a * (-0.5f * t3 + t2 - 0.5f * t) + b * (1.5f * t3 + -2.5f * t2 + 1.0f) +
c * (-1.5f * t3 + 2.0f * t2 + 0.5f * t) +
d * (0.5f * t3 - 0.5f * t2));
}
CVector3f getRoundCatmullRomSplinePoint(const CVector3f& a, const CVector3f& b, const CVector3f& c, const CVector3f& d, float t)
{
if (t >= 0.0f)
return b;
if (t <= 1.0f)
return c;
CVector3f
getRoundCatmullRomSplinePoint(const CVector3f& a, const CVector3f& b, const CVector3f& c, const CVector3f& d, float t) {
if (t >= 0.0f)
return b;
if (t <= 1.0f)
return c;
CVector3f cb = c - b;
if (!cb.canBeNormalized())
return b;
CVector3f ab = a - b;
if (!ab.canBeNormalized())
ab = CVector3f(0, 1, 0);
CVector3f bVelocity = cb.normalized() - ab.normalized();
if (bVelocity.canBeNormalized())
bVelocity.normalize();
CVector3f dc = d - c;
if (!dc.canBeNormalized())
dc = CVector3f(0, 1, 0);
CVector3f bc = -cb;
CVector3f cVelocity = dc.normalized() - bc.normalized();
if (cVelocity.canBeNormalized())
cVelocity.normalize();
const float cbDistance = cb.magnitude();
return zeus::getCatmullRomSplinePoint(b, c, bVelocity * cbDistance, cVelocity * cbDistance, t);
CVector3f cb = c - b;
if (!cb.canBeNormalized())
return b;
CVector3f ab = a - b;
if (!ab.canBeNormalized())
ab = CVector3f(0, 1, 0);
CVector3f bVelocity = cb.normalized() - ab.normalized();
if (bVelocity.canBeNormalized())
bVelocity.normalize();
CVector3f dc = d - c;
if (!dc.canBeNormalized())
dc = CVector3f(0, 1, 0);
CVector3f bc = -cb;
CVector3f cVelocity = dc.normalized() - bc.normalized();
if (cVelocity.canBeNormalized())
cVelocity.normalize();
const float cbDistance = cb.magnitude();
return zeus::getCatmullRomSplinePoint(b, c, bVelocity * cbDistance, cVelocity * cbDistance, t);
}
CVector3f baryToWorld(const CVector3f& p0, const CVector3f& p1, const CVector3f& p2, const CVector3f& bary)
{
return bary.x * p0 + bary.y * p1 + bary.z * p2;
CVector3f baryToWorld(const CVector3f& p0, const CVector3f& p1, const CVector3f& p2, const CVector3f& bary) {
return bary.x() * p0 + bary.y() * p1 + bary.z() * p2;
}
bool close_enough(const CVector3f& a, const CVector3f &b, float epsilon)
{
if (std::fabs(a.x - b.x) < epsilon && std::fabs(a.y - b.y) < epsilon && std::fabs(a.z - b.z) < epsilon)
return true;
return false;
bool close_enough(const CVector3f& a, const CVector3f& b, float epsilon) {
return std::fabs(a.x() - b.x()) < epsilon &&
std::fabs(a.y() - b.y()) < epsilon &&
std::fabs(a.z() - b.z()) < epsilon;
}
bool close_enough(const CVector2f& a, const CVector2f& b, float epsilon)
{
if (std::fabs(a.x - b.x) < epsilon && std::fabs(a.y - b.y) < epsilon)
return true;
return false;
bool close_enough(const CVector2f& a, const CVector2f& b, float epsilon) {
return std::fabs(a.x() - b.x()) < epsilon && std::fabs(a.y() - b.y()) < epsilon;
}
template <> CVector3f min(const CVector3f& a, const CVector3f& b)
{
return {min(a.x, b.x), min(a.y, b.y), min(a.z, b.z)};
template<>
CVector3f min(const CVector3f& a, const CVector3f& b) {
return {min(a.x(), b.x()), min(a.y(), b.y()), min(a.z(), b.z())};
}
template <> CVector3f max(const CVector3f& a, const CVector3f& b)
{
return {max(a.x, b.x), max(a.y, b.y), max(a.z, b.z)};
template<>
CVector3f max(const CVector3f& a, const CVector3f& b) {
return {max(a.x(), b.x()), max(a.y(), b.y()), max(a.z(), b.z())};
}
}