Line data    Source code

       1              : #pragma once
       2              : #include <math3d/math3d.hpp>
       3              : #include "rbc/AABB.hpp"
       4              : 
       5              : namespace rbc
       6              : {
       7              :     struct Box
       8              :     {
       9              :         m3d::vec3 half_extents;
      10              :         // Constructors
      11              :         Box() : half_extents(0.5, 0.5, 0.5) {}
      12           43 :         Box(const m3d::vec3 &half_extents) : half_extents(half_extents) {}
      13              : 
      14              :         inline bool operator==(const Box &other) const
      15              :         {
      16              :             return half_extents == other.half_extents;
      17              :         }
      18              : 
      19              :         inline bool operator!=(const Box &other) const
      20              :         {
      21              :             return !(*this == other);
      22              :         }
      23              :     };
      24              : 
      25          170 :     inline m3d::vec3 support(const Box &box, const m3d::vec3 &dir)
      26              :     {
      27              :         // The furthest point of a box maps to its corners based on the sign of the direction
      28              :         return m3d::vec3(
      29          277 :             (dir.x > 0) ? box.half_extents.x : -box.half_extents.x,
      30          278 :             (dir.y > 0) ? box.half_extents.y : -box.half_extents.y,
      31          170 :             (dir.z > 0) ? box.half_extents.z : -box.half_extents.z);
      32              :     }
      33              : 
      34              :     inline m3d::scalar compute_volume(const Box &box)
      35              :     {
      36              :         return 8.0 * box.half_extents.x * box.half_extents.y * box.half_extents.z; // Volume of the box
      37              :     }
      38              : 
      39              :     inline m3d::smat3 compute_inertia_tensor(const Box &box)
      40              :     {
      41              :         m3d::scalar x2 = 4.0 * box.half_extents.x * box.half_extents.x;
      42              :         m3d::scalar y2 = 4.0 * box.half_extents.y * box.half_extents.y;
      43              :         m3d::scalar z2 = 4.0 * box.half_extents.z * box.half_extents.z;
      44              :         m3d::scalar mass = compute_volume(box); // Assuming unit density for simplicity (How we should ahndle this? Should we add a density parameter to the box struct?)
      45              :         return m3d::smat3(
      46              :             (1.0 / 12.0) * mass * (y2 + z2), // Ixx
      47              :             (1.0 / 12.0) * mass * (x2 + z2), // Iyy
      48              :             (1.0 / 12.0) * mass * (x2 + y2), // Izz
      49              :             0,                               // Ixy
      50              :             0,                               // Ixz
      51              :             0                                // Iyz
      52              :         );
      53              :     }
      54              : 
      55              :     // Project the OBB onto world axes by summing |R_ij| * half_extents_j
      56              :     // for each world axis i. This is the standard tight AABB for a rotated box.
      57              :     //
      58              :     // NOTE: Assumes m3d::mat3_cast(quat) returns a column-major mat3 where
      59              :     //       mat[col][row] gives the element, and tf.rot is an m3d::quat.
      60              :     //       Adjust the indexing if your math3d uses a different convention.
      61          504 :     inline AABB compute_aabb(const Box &b, const m3d::tf &tf)
      62              :     {
      63          504 :         const m3d::mat3 R = m3d::mat3_cast(tf.rot);
      64          504 :         const m3d::vec3 &h = b.half_extents;
      65              : 
      66              :         // World-space half-extents of the AABB that wraps the rotated box.
      67              :         // extent_i = |R_i0|*hx + |R_i1|*hy + |R_i2|*hz  (row i of R dotted with |h|)
      68              :         const m3d::vec3 extent(
      69          504 :             m3d::abs(R[0][0]) * h.x + m3d::abs(R[1][0]) * h.y + m3d::abs(R[2][0]) * h.z,
      70          504 :             m3d::abs(R[0][1]) * h.x + m3d::abs(R[1][1]) * h.y + m3d::abs(R[2][1]) * h.z,
      71         1512 :             m3d::abs(R[0][2]) * h.x + m3d::abs(R[1][2]) * h.y + m3d::abs(R[2][2]) * h.z);
      72              : 
      73          504 :         return {tf.pos - extent, tf.pos + extent};
      74              :     }
      75              : }