diff --git a/src/Math/Geometry.h b/src/Math/Geometry.h index 3d1ff70..532ef88 100644 --- a/src/Math/Geometry.h +++ b/src/Math/Geometry.h @@ -30,14 +30,24 @@ #include "Math/Dense.h" #include "Math/Transform.h" +#include namespace uLib { class Geometry : public AffineTransform { public: + virtual Vector3f ToLinear(const Vector3f& curved_space) const { + return curved_space; + } + + virtual Vector3f FromLinear(const Vector3f& cartesian_space) const { + return cartesian_space; + } + inline Vector4f GetWorldPoint(const Vector4f v) const { - return this->GetWorldMatrix() * v; + Vector3f lin = ToLinear(Vector3f(v.x(), v.y(), v.z())); + return this->GetWorldMatrix() * Vector4f(lin.x(), lin.y(), lin.z(), v.w()); } inline Vector4f GetWorldPoint(const float x, const float y, const float z) { @@ -45,7 +55,9 @@ public: } inline Vector4f GetLocalPoint(const Vector4f v) const { - return this->GetWorldMatrix().inverse() * v; + Vector4f loc_lin = this->GetWorldMatrix().inverse() * v; + Vector3f curv = FromLinear(Vector3f(loc_lin.x(), loc_lin.y(), loc_lin.z())); + return Vector4f(curv.x(), curv.y(), curv.z(), loc_lin.w()); } inline Vector4f GetLocalPoint(const float x, const float y, const float z) { @@ -53,6 +65,73 @@ public: } }; +class CylindricalGeometry : public Geometry { +public: + CylindricalGeometry() {} + + Vector3f ToLinear(const Vector3f& cylindrical) const { + return Vector3f(cylindrical.x() * std::cos(cylindrical.y()), + cylindrical.x() * std::sin(cylindrical.y()), + cylindrical.z()); + } + + Vector3f FromLinear(const Vector3f& linear) const { + float r = std::sqrt(linear.x() * linear.x() + linear.y() * linear.y()); + float phi = std::atan2(linear.y(), linear.x()); + return Vector3f(r, phi, linear.z()); + } + +}; + +class SphericalGeometry : public Geometry { +public: + SphericalGeometry() {} + + Vector3f ToLinear(const Vector3f& spherical) const { + float r = spherical.x(); + float theta = spherical.y(); + float phi = spherical.z(); + return Vector3f(r * std::sin(theta) * std::cos(phi), + r * std::sin(theta) * std::sin(phi), + r * std::cos(theta)); + } + + Vector3f FromLinear(const Vector3f& linear) const { + float r = linear.norm(); + float theta = (r == 0.0f) ? 0.0f : std::acos(linear.z() / r); + float phi = std::atan2(linear.y(), linear.x()); + return Vector3f(r, theta, phi); + } + +}; + +class ToroidalGeometry : public Geometry { +public: + ToroidalGeometry(float Rtor) : m_Rtor(Rtor) {} + + Vector3f ToLinear(const Vector3f& toroidal) const { + float r = toroidal.x(); + float theta = toroidal.y(); + float phi = toroidal.z(); + return Vector3f((m_Rtor + r * std::cos(theta)) * std::cos(phi), + (m_Rtor + r * std::cos(theta)) * std::sin(phi), + r * std::sin(theta)); + } + + Vector3f FromLinear(const Vector3f& linear) const { + float phi = std::atan2(linear.y(), linear.x()); + float r_xy = std::sqrt(linear.x() * linear.x() + linear.y() * linear.y()); + float delta_r = r_xy - m_Rtor; + float z = linear.z(); + float r = std::sqrt(delta_r * delta_r + z * z); + float theta = std::atan2(z, delta_r); + return Vector3f(r, theta, phi); + } + +private: + float m_Rtor; + +}; } diff --git a/src/Math/testing/ContainerBoxTest.cpp b/src/Math/testing/ContainerBoxTest.cpp index 61539d5..b6deca5 100644 --- a/src/Math/testing/ContainerBoxTest.cpp +++ b/src/Math/testing/ContainerBoxTest.cpp @@ -164,13 +164,12 @@ int main() { // test parent-child relationship ContainerBox Parent; - Parent.SetOrigin(Vector3f(1,1,1)); - Parent.SetSize(Vector3f(2,2,2)); + Parent.SetPosition(Vector3f(1,1,1)); + Parent.Scale(Vector3f(2,2,2)); ContainerBox Child; Child.SetParent(&Parent); - Child.SetOrigin(Vector3f(1,1,1)); - Child.SetSize(Vector3f(2,2,2)); + Child.SetPosition(Vector3f(0,0,0)); HPoint3f pt = Child.GetLocalPoint(HPoint3f(0,0,0)); HPoint3f wp = Child.GetWorldPoint(pt); @@ -178,16 +177,18 @@ int main() pt = HPoint3f(1,1,1); wp = Child.GetWorldPoint(pt); - TEST0( Vector4f0(wp - HPoint3f(2,2,2)) ); + TEST0( Vector4f0(wp - HPoint3f(3,3,3)) ); - pt = HPoint3f(1,2,3); + Parent.Rotate(Vector3f(M_PI,0,0)); + pt = HPoint3f(1,1,1); wp = Child.GetWorldPoint(pt); - std::cout << "Child.GetWorldPoint(HPoint3f(1,2,3)): " << wp << std::endl; - TEST0( Vector4f0(wp - HPoint3f(2,3,4)) ); + TEST0( Vector4f0(wp - HPoint3f(3,-1,-1)) ); - pt = HPoint3f(1,2,3); + Child.Rotate(Vector3f(M_PI,0,0)); + pt = HPoint3f(1,1,1); wp = Child.GetWorldPoint(pt); - TEST0( Vector4f0(wp - HPoint3f(2,3,4)) ); + TEST0( Vector4f0(wp - HPoint3f(3,3,3)) ); + } diff --git a/src/Math/testing/GeometryTest.cpp b/src/Math/testing/GeometryTest.cpp index 0e6788f..075e750 100644 --- a/src/Math/testing/GeometryTest.cpp +++ b/src/Math/testing/GeometryTest.cpp @@ -92,6 +92,36 @@ int main() + // CYLINDRICAL GEOMETRY TESTING + { + CylindricalGeometry cyl; + Vector3f cyl_pt(5.0f, M_PI_2, 3.0f); + Vector3f lin = cyl.ToLinear(cyl_pt); + TEST0( Vector4f0(lin.homogeneous() - HPoint3f(0.0f, 5.0f, 3.0f)) ); + Vector3f recovered = cyl.FromLinear(lin); + TEST0( Vector4f0(recovered.homogeneous() - cyl_pt.homogeneous()) ); + } + + // SPHERICAL GEOMETRY TESTING + { + SphericalGeometry sph; + Vector3f sph_pt(5.0f, M_PI_2, M_PI); + Vector3f lin = sph.ToLinear(sph_pt); + TEST0( Vector4f0(lin.homogeneous() - HPoint3f(-5.0f, 0.0f, 0.0f)) ); + Vector3f recovered = sph.FromLinear(Vector3f(-5.0f, 0.0f, 0.0f)); + TEST0( Vector4f0(recovered.homogeneous() - sph_pt.homogeneous()) ); + } + + // TOROIDAL GEOMETRY TESTING + { + ToroidalGeometry tor(10.0f); + Vector3f tor_pt(1.0f, M_PI_2, M_PI); + Vector3f lin = tor.ToLinear(tor_pt); + TEST0( Vector4f0(lin.homogeneous() - HPoint3f(-10.0f, 0.0f, 1.0f)) ); + Vector3f recovered = tor.FromLinear(Vector3f(-10.0f, 0.0f, 1.0f)); + TEST0( Vector4f0(recovered.homogeneous() - tor_pt.homogeneous()) ); + } + END_TESTING; }