diff --git a/CMakeLists.txt b/CMakeLists.txt index 4fc4ee41d0..3dd05d6810 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -38,6 +38,7 @@ topaz_add_library( src/tz/core/job.cpp src/tz/core/vector.cpp src/tz/core/matrix.cpp + src/tz/core/quaternion.cpp src/tz/core/trs.cpp src/tz/gpu/rhi_vulkan.cpp src/tz/os/impl_win32.cpp diff --git a/include/tz/core/quaternion.hpp b/include/tz/core/quaternion.hpp new file mode 100644 index 0000000000..3bacb8bea4 --- /dev/null +++ b/include/tz/core/quaternion.hpp @@ -0,0 +1,30 @@ +#ifndef TOPAZ_CORE_QUATERNION_HPP +#define TOPAZ_CORE_QUATERNION_HPP +#include "tz/core/vector.hpp" +#include "tz/core/matrix.hpp" + +namespace tz +{ + struct quat : public tz::v4f + { + static quat iden() + { + return {tz::v4f{0.0f, 0.0f, 0.0f, 1.0f}}; + } + quat() = default; + quat(tz::v4f vec); + static quat from_axis_angle(tz::v3f axis, float angle); + static quat from_euler_angles(tz::v3f euler_angles); + tz::m4f matrix() const; + quat inverse() const; + quat combine(const quat& rhs) const; + tz::v3f rotate(tz::v3f pos) const; + quat normalise() const; + quat slerp(const quat& rhs, float factor) const; + + quat& operator*=(const quat& rhs); + quat operator*(const quat& rhs) const{auto cpy = *this; return cpy *= rhs;} + }; +} + +#endif // TOPAZ_CORE_QUATERNION_HPP \ No newline at end of file diff --git a/include/tz/core/vector.hpp b/include/tz/core/vector.hpp index 8b3823aa11..0a9d964a46 100644 --- a/include/tz/core/vector.hpp +++ b/include/tz/core/vector.hpp @@ -2,6 +2,7 @@ #define TOPAZ_CORE_VECTOR_HPP #include #include +#include namespace tz { @@ -68,6 +69,10 @@ namespace tz vector operator*(const vector& rhs) const{auto cpy = *this; return cpy *= rhs;} vector operator/(const vector& rhs) const{auto cpy = *this; return cpy /= rhs;} + T length() const; + T dot(const vector& rhs) const; + vector cross(const vector& rhs) const requires(N == 3); + bool operator==(const vector& rhs) const = default; private: std::array arr; diff --git a/src/tz/core/quaternion.cpp b/src/tz/core/quaternion.cpp new file mode 100644 index 0000000000..79b5d37337 --- /dev/null +++ b/src/tz/core/quaternion.cpp @@ -0,0 +1,137 @@ +#include "tz/core/quaternion.hpp" +#include "tz/topaz.hpp" +#include + +namespace tz +{ + quat::quat(tz::v4f vec): tz::v4f(vec){} + + quat quat::from_axis_angle(tz::v3f axis, float angle) + { + return quat + {tz::v4f{ + axis[0] * std::sin(angle / 2.0f), + axis[1] * std::sin(angle / 2.0f), + axis[2] * std::sin(angle / 2.0f), + std::cos(angle / 2.0f) + }}.normalise(); + } + + quat quat::from_euler_angles(tz::v3f euler_angles) + { + tz::v3f vx{1.0f, 0.0f, 0.0f}, vy = {0.0f, 1.0f, 0.0f}, vz = {0.0f, 0.0f, 1.0f}; + auto qx = quat::from_axis_angle(vx, euler_angles[0]); + auto qy = quat::from_axis_angle(vy, euler_angles[1]); + auto qz = quat::from_axis_angle(vz, euler_angles[2]); + return ((qx*qy).normalise() * qz).normalise(); + } + + tz::m4f quat::matrix() const + { + tz::m4f rot = tz::m4f::iden(); + rot(0, 0) = 1.0f - 2.0f * (this->operator[](1) * this->operator[](1)) - (2 * this->operator[](2) * this->operator[](2)); + rot(1, 0) = (this->operator[](0) * this->operator[](1)) + (2 * this->operator[](2) * this->operator[](3)); + rot(2, 0) = (2 * this->operator[](0) * this->operator[](2)) - (2 * this->operator[](1) * this->operator[](3)); + rot(0, 1) = (2 * this->operator[](0) * this->operator[](1)) - (2 * this->operator[](2) * this->operator[](3)); + rot(1, 1) = 1.0f - 2.0f * (this->operator[](0) * this->operator[](0)) - (2 * this->operator[](2) * this->operator[](2)); + rot(2, 1) = (2 * this->operator[](1) * this->operator[](2)) + (2 * this->operator[](0) * this->operator[](3)); + rot(0, 2) = (2 * this->operator[](0) * this->operator[](2)) + (2 * this->operator[](1) * this->operator[](3)); + rot(1, 2) = (2 * this->operator[](1) * this->operator[](2)) - (2 * this->operator[](0) * this->operator[](3)); + rot(2, 2) = 1.0f - 2.0f * (this->operator[](0) * this->operator[](0)) - (2 * this->operator[](1) * this->operator[](1)); + + return rot; + } + + quat quat::inverse() const + { + auto cpy = *this; + float norm_sq = this->length() * this->length(); + cpy[0] = -cpy[0] * norm_sq; + cpy[1] = -cpy[1] * norm_sq; + cpy[2] = -cpy[2] * norm_sq; + cpy[3] = cpy[3] * norm_sq; + return *this; + } + + quat quat::combine(const quat& rhs) const + { + return rhs * (*this); + } + + tz::v3f quat::rotate(tz::v3f pos) const + { + tz::v3f me{(*this)[0], (*this)[1], (*this)[2]}; + tz::v3f uv = me.cross(pos); + tz::v3f uuv = me.cross(uv); + return pos + ((uv * this->operator[](3)) + uuv) * 2.0f; + } + + quat quat::normalise() const + { + auto cpy = *this; + float l = this->length(); + if(l == 0.0f) + { + // bad + return quat::iden(); + } + for(std::size_t i = 0; i < 4; i++) + { + cpy[i] /= l; + } + return cpy; + } + + quat quat::slerp(const quat& rhs, float factor) const + { + float cos_theta = this->dot(rhs); + + if (cos_theta < 0.0f) + { + // If the quaternions are in opposite directions, negate one to take the shortest path + quat neg_rhs = {{ -rhs[0], -rhs[1], -rhs[2], -rhs[3] }}; + return this->slerp(neg_rhs, factor); + } + + const float threshold = 0.9995f; + if (cos_theta > threshold) { + // Linear factorolation for small angles + quat result = + {{ + (*this)[0] + factor * (rhs[0] - (*this)[0]), + (*this)[1] + factor * (rhs[1] - (*this)[1]), + (*this)[2] + factor * (rhs[2] - (*this)[2]), + (*this)[3] + factor * (rhs[3] - (*this)[3]) + }}; + return result.normalise(); + } + + float angle = std::acos(cos_theta); + float sin_angle = std::sin(angle); + float t0 = std::sin((1.0f - factor) * angle) / sin_angle; + float t1 = std::sin(factor * angle) / sin_angle; + + quat result = + {{ + t0 * (*this)[0] + t1 * rhs[0], + t0 * (*this)[1] + t1 * rhs[1], + t0 * (*this)[2] + t1 * rhs[2], + t0 * (*this)[3] + t1 * rhs[3] + }}; + + return result.normalise(); + } + + quat& quat::operator*=(const quat& rhs) + { + quat& lhs = *this; + quat result; + + result[3] = lhs[3] * rhs[3] - lhs[0] * rhs[0] - lhs[1] * rhs[1] - lhs[2] * rhs[2]; + result[0] = lhs[3] * rhs[0] + lhs[0] * rhs[3] + lhs[1] * rhs[2] - lhs[2] * rhs[1]; + result[1] = lhs[3] * rhs[1] - lhs[0] * rhs[2] + lhs[1] * rhs[3] + lhs[2] * rhs[0]; + result[2] = lhs[3] * rhs[2] + lhs[0] * rhs[1] - lhs[1] * rhs[0] + lhs[2] * rhs[3]; + std::swap(result, lhs); + return *this; + } +} \ No newline at end of file diff --git a/src/tz/core/vector.cpp b/src/tz/core/vector.cpp index 94a1497c23..d1be103b42 100644 --- a/src/tz/core/vector.cpp +++ b/src/tz/core/vector.cpp @@ -1,4 +1,5 @@ #include "tz/core/vector.hpp" +#include namespace tz { @@ -58,6 +59,41 @@ namespace tz return *this; } + VECTOR_TEMPLATE_IMPL + T vector::length() const + { + T ret = T{0}; + for(std::size_t i = 0; i < N; i++) + { + ret += ((*this)[i] * (*this)[i]); + } + return std::sqrt(ret); + } + + VECTOR_TEMPLATE_IMPL + T vector::dot(const vector& rhs) const + { + T ret = T{0}; + for(std::size_t i = 0; i < N; i++) + { + ret += (*this)[i] * rhs[i]; + } + return ret; + } + + VECTOR_TEMPLATE_IMPL + vector vector::cross(const vector& rhs) const requires(N == 3) + { + vector ret; + //cx = aybz − azby + ret[0] = ((*this)[1] * rhs[2]) - ((*this)[2] * rhs[1]); + //cy = azbx − axbz + ret[1] = ((*this)[2] * rhs[0]) - ((*this)[0] * rhs[2]); + //cz = axby − aybx + ret[2] = ((*this)[0] * rhs[1]) - ((*this)[1] * rhs[0]); + return ret; + } + template struct vector; template struct vector; template struct vector;