Add Geometries

src/Math/Geometry.h

@@ -30,14 +30,24 @@
 
 #include "Math/Dense.h"
 #include "Math/Transform.h"
+#include <cmath>
 
 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;
+
+};
 
 }