Line data    Source code

       1              : #pragma once
       2              : #include <math3d/math3d.hpp>
       3              : #include "rbc/AABB.hpp"
       4              : 
       5              : namespace rbc
       6              : {
       7              :     // ── Ellipsoid ─────────────────────────────────────────────────────────────
       8              :     // Axis-aligned ellipsoid in local space; rotation handled by m3d::tf.
       9              :     struct Ellipsoid
      10              :     {
      11              :         m3d::vec3 half_extents; // semi-axes (a, b, c) along local X, Y, Z
      12              : 
      13              :         Ellipsoid() : half_extents(0.5, 0.5, 0.5) {}
      14              :         explicit Ellipsoid(const m3d::vec3 &half_extents) : half_extents(half_extents) {}
      15              : 
      16              :         inline bool operator==(const Ellipsoid &o) const { return half_extents == o.half_extents; }
      17              :         inline bool operator!=(const Ellipsoid &o) const { return !(*this == o); }
      18              :     };
      19              : 
      20              :     // ── Support function ──────────────────────────────────────────────────────
      21              :     // For x²/a² + y²/b² + z²/c² ≤ 1, maximise d·x via Lagrange multipliers:
      22              :     //   p_k = a_k² · d_k  /  sqrt( a_x²·d_x² + a_y²·d_y² + a_z²·d_z² )
      23            0 :     inline m3d::vec3 support(const Ellipsoid &e, const m3d::vec3 &dir)
      24              :     {
      25            0 :         const m3d::vec3 &a = e.half_extents;
      26            0 :         const m3d::vec3 scaled(a.x * a.x * dir.x,
      27            0 :                                a.y * a.y * dir.y,
      28            0 :                                a.z * a.z * dir.z);
      29            0 :         const m3d::scalar denom = m3d::sqrt(a.x * a.x * dir.x * dir.x +
      30            0 :                                             a.y * a.y * dir.y * dir.y +
      31            0 :                                             a.z * a.z * dir.z * dir.z);
      32            0 :         if (denom < m3d::EPSILON)
      33            0 :             return m3d::vec3(a.x, 0.0, 0.0);
      34            0 :         return scaled / denom;
      35              :     }
      36              : 
      37              :     inline m3d::scalar compute_volume(const Ellipsoid &e)
      38              :     {
      39              :         return (4.0 / 3.0) * m3d::PI * e.half_extents.x * e.half_extents.y * e.half_extents.z;
      40              :     }
      41              : 
      42              :     // Ixx = (2/5)·m·(b² + c²), cyclic.
      43              :     inline m3d::smat3 compute_inertia_tensor(const Ellipsoid &e)
      44              :     {
      45              :         const m3d::scalar mass = compute_volume(e);
      46              :         const m3d::scalar ax2 = e.half_extents.x * e.half_extents.x;
      47              :         const m3d::scalar ay2 = e.half_extents.y * e.half_extents.y;
      48              :         const m3d::scalar az2 = e.half_extents.z * e.half_extents.z;
      49              :         return m3d::smat3(
      50              :             (2.0 / 5.0) * mass * (ay2 + az2),
      51              :             (2.0 / 5.0) * mass * (ax2 + az2),
      52              :             (2.0 / 5.0) * mass * (ax2 + ay2),
      53              :             0.0, 0.0, 0.0);
      54              :     }
      55              : 
      56              :     // ── Tight AABB for a rotated ellipsoid ────────────────────────────────────
      57              :     // World-space half-extent along axis i:
      58              :     //   extent_i = sqrt( R[0][i]²·ax² + R[1][i]²·ay² + R[2][i]²·az² )
      59              :     // (Same derivation as Box but with sqrt of sum-of-squares instead of sum of |.|.)
      60            0 :     inline AABB compute_aabb(const Ellipsoid &e, const m3d::tf &tf)
      61              :     {
      62            0 :         const m3d::mat3 R = m3d::mat3_cast(tf.rot);
      63            0 :         const m3d::vec3 &a = e.half_extents;
      64              :         const m3d::vec3 extent(
      65            0 :             m3d::sqrt(R[0][0] * R[0][0] * a.x * a.x + R[1][0] * R[1][0] * a.y * a.y + R[2][0] * R[2][0] * a.z * a.z),
      66            0 :             m3d::sqrt(R[0][1] * R[0][1] * a.x * a.x + R[1][1] * R[1][1] * a.y * a.y + R[2][1] * R[2][1] * a.z * a.z),
      67            0 :             m3d::sqrt(R[0][2] * R[0][2] * a.x * a.x + R[1][2] * R[1][2] * a.y * a.y + R[2][2] * R[2][2] * a.z * a.z));
      68            0 :         return {tf.pos - extent, tf.pos + extent};
      69              :     }
      70              : } // namespace rbc