Line data    Source code

       1              : #pragma once
       2              : #include <math3d/math3d.hpp>
       3              : #include "rbc/AABB.hpp"
       4              : 
       5              : namespace rbc
       6              : {
       7              :     // ── Cone ──────────────────────────────────────────────────────────────────
       8              :     // Right circular cone. In local space:
       9              :     //   Apex   = (0, +half_height, 0)
      10              :     //   Base centre = (0, -half_height, 0), radius = base_radius
      11              :     // Total height = 2*half_height.
      12              :     struct Cone
      13              :     {
      14              :         m3d::scalar half_height;
      15              :         m3d::scalar base_radius;
      16              : 
      17              :         Cone() : half_height(0.5), base_radius(0.5) {}
      18              :         Cone(m3d::scalar half_height, m3d::scalar base_radius)
      19              :             : half_height(half_height), base_radius(base_radius) {}
      20              : 
      21              :         inline bool operator==(const Cone &o) const
      22              :         {
      23              :             return half_height == o.half_height && base_radius == o.base_radius;
      24              :         }
      25              :         inline bool operator!=(const Cone &o) const { return !(*this == o); }
      26              :     };
      27              : 
      28              :     // ── Support function (local space) ────────────────────────────────────────
      29              :     // Candidates: apex (0, h, 0) or base-circle edge point.
      30              :     // We compare d·apex vs d·(base_point), pick the larger.
      31              :     //   apex_dot       = d.y · h
      32              :     //   base_edge_dot  = d_radial · base_radius − d.y · h
      33              :     // where d_radial = sqrt(d.x² + d.z²).
      34            0 :     inline m3d::vec3 support(const Cone &c, const m3d::vec3 &dir)
      35              :     {
      36            0 :         const m3d::scalar d_radial = m3d::sqrt(dir.x * dir.x + dir.z * dir.z);
      37              : 
      38            0 :         if (d_radial < m3d::EPSILON)
      39              :         {
      40              :             // Direction is purely axial
      41            0 :             return (dir.y >= 0.0)
      42            0 :                        ? m3d::vec3(0.0, c.half_height, 0.0)             // apex
      43            0 :                        : m3d::vec3(c.base_radius, -c.half_height, 0.0); // base edge (arbitrary angle)
      44              :         }
      45              : 
      46            0 :         const m3d::scalar apex_dot = dir.y * c.half_height;
      47            0 :         const m3d::scalar base_edge_dot = d_radial * c.base_radius - dir.y * c.half_height;
      48              : 
      49            0 :         if (apex_dot >= base_edge_dot)
      50            0 :             return m3d::vec3(0.0, c.half_height, 0.0); // apex wins
      51              : 
      52              :         // Base-circle edge point in the radial direction of dir
      53            0 :         const m3d::scalar scale = c.base_radius / d_radial;
      54            0 :         return m3d::vec3(dir.x * scale, -c.half_height, dir.z * scale);
      55              :     }
      56              : 
      57              :     inline m3d::scalar compute_volume(const Cone &c)
      58              :     {
      59              :         return (1.0 / 3.0) * m3d::PI * c.base_radius * c.base_radius * (2.0 * c.half_height);
      60              :     }
      61              : 
      62              :     // Inertia tensor about the cone's local axes (unit density).
      63              :     // For a solid cone of height H=2h, base radius R, mass m:
      64              :     //   Iyy (axial)     = (3/10) · m · R²
      65              :     //   Ixx (transverse) = (3/20) · m · (R² + H²/4) shifted to centroid
      66              :     // The centroid is at 3H/8 from the base = h/4 from the centre (positive Y side).
      67              :     inline m3d::smat3 compute_inertia_tensor(const Cone &c)
      68              :     {
      69              :         const m3d::scalar mass = compute_volume(c);
      70              :         const m3d::scalar R2 = c.base_radius * c.base_radius;
      71              :         const m3d::scalar H = 2.0 * c.half_height;
      72              :         const m3d::scalar Iyy = (3.0 / 10.0) * mass * R2;
      73              :         // Transverse about apex, then shift to centroid (h/4 from mid-point)
      74              :         const m3d::scalar Ixx_apex = (3.0 / 5.0) * mass * (R2 / 4.0 + H * H);
      75              :         const m3d::scalar d_centroid = 3.0 * H / 8.0 - c.half_height; // offset from local origin
      76              :         const m3d::scalar Ixx = Ixx_apex - mass * d_centroid * d_centroid;
      77              :         return m3d::smat3(Ixx, Iyy, Ixx, 0.0, 0.0, 0.0);
      78              :     }
      79              : 
      80              :     // ── Tight AABB via 6-direction support ────────────────────────────────────
      81              :     // More accurate than a bounding sphere, still O(1).
      82            0 :     inline AABB compute_aabb(const Cone &c, const m3d::tf &tf)
      83              :     {
      84              :         const m3d::vec3 world_axes[6] = {
      85            0 :             {1, 0, 0}, {-1, 0, 0}, {0, 1, 0}, {0, -1, 0}, {0, 0, 1}, {0, 0, -1}};
      86              : 
      87            0 :         m3d::vec3 mn = tf.pos, mx = tf.pos;
      88            0 :         for (const auto &axis : world_axes)
      89              :         {
      90            0 :             const m3d::vec3 local_dir = tf.inverse_rotate_vector(axis);
      91            0 :             const m3d::vec3 world_sup = tf.pos + tf.rotate_vector(support(c, local_dir));
      92            0 :             mn = m3d::vec3(m3d::min(mn.x, world_sup.x), m3d::min(mn.y, world_sup.y), m3d::min(mn.z, world_sup.z));
      93            0 :             mx = m3d::vec3(m3d::max(mx.x, world_sup.x), m3d::max(mx.y, world_sup.y), m3d::max(mx.z, world_sup.z));
      94              :         }
      95            0 :         return {mn, mx};
      96              :     }
      97              : } // namespace rbc