Line data Source code
1 : #pragma once
2 : #include <math3d/math3d.hpp>
3 : #include "rbc/AABB.hpp"
4 :
5 : namespace rbc
6 : {
7 : // ── Capsule ───────────────────────────────────────────────────────────────
8 : // A cylinder of radius `radius` capped with two hemispheres.
9 : // In local space the principal axis is Y.
10 : // Top hemisphere centre = (0, +half_height, 0)
11 : // Bottom hemisphere centre = (0, -half_height, 0)
12 : // Total height = 2*(half_height + radius).
13 : struct Capsule
14 : {
15 : m3d::scalar half_height; // distance from centre to each cap centre
16 : m3d::scalar radius;
17 :
18 : Capsule() : half_height(0.5), radius(0.25) {}
19 : Capsule(m3d::scalar half_height, m3d::scalar radius)
20 : : half_height(half_height), radius(radius) {}
21 :
22 : inline bool operator==(const Capsule &o) const
23 : { return half_height == o.half_height && radius == o.radius; }
24 : inline bool operator!=(const Capsule &o) const { return !(*this == o); }
25 : };
26 :
27 : // ── World-space endpoints (convenience, used by collision algorithms) ─────
28 : inline void capsule_endpoints(const Capsule &c, const m3d::tf &tf,
29 : m3d::vec3 &p1_out, m3d::vec3 &p2_out)
30 : {
31 : const m3d::vec3 axis = tf.rotate_vector(m3d::vec3(0, c.half_height, 0));
32 : p1_out = tf.pos + axis;
33 : p2_out = tf.pos - axis;
34 : }
35 :
36 : // ── Support function (local space, axis = local Y) ────────────────────────
37 : // Capsule = Minkowski sum of segment [(0,-h,0),(0,+h,0)] and sphere of radius r.
38 : // Support = endpoint in dir.y sign direction, then nudge by r·normalize(dir).
39 0 : inline m3d::vec3 support(const Capsule &c, const m3d::vec3 &dir)
40 : {
41 : const m3d::vec3 endpoint(0.0,
42 0 : (dir.y >= 0.0 ? c.half_height : -c.half_height),
43 0 : 0.0);
44 0 : const m3d::scalar len = m3d::length(dir);
45 0 : if (len < m3d::EPSILON) return endpoint;
46 0 : return endpoint + (dir / len) * c.radius;
47 : }
48 :
49 : inline m3d::scalar compute_volume(const Capsule &c)
50 : {
51 : // Cylinder + two hemispheres
52 : const m3d::scalar r = c.radius, h = c.half_height;
53 : return m3d::PI * r * r * (2.0 * h + (4.0 / 3.0) * r);
54 : }
55 :
56 : // Inertia tensor about the capsule's local axes (unit density).
57 : inline m3d::smat3 compute_inertia_tensor(const Capsule &c)
58 : {
59 : // References: Game Physics, Eberly; moment integrals for capsule.
60 : const m3d::scalar r = c.radius, h = c.half_height;
61 : const m3d::scalar r2 = r * r;
62 : const m3d::scalar h2 = h * h;
63 : const m3d::scalar mass = compute_volume(c); // unit density
64 :
65 : // Cylinder mass and hemisphere mass fractions
66 : const m3d::scalar m_cyl = m3d::PI * r2 * 2.0 * h;
67 : const m3d::scalar m_hemi = mass - m_cyl; // = (4/3)*pi*r³ total for both caps
68 :
69 : // About the principal axis (Y): Iyy
70 : const m3d::scalar Iyy = (m_cyl * r2 / 2.0) +
71 : (m_hemi * 2.0 * r2 / 5.0);
72 :
73 : // About a transverse axis (X or Z): Ixx
74 : // Cylinder contribution
75 : const m3d::scalar Ixx_cyl = m_cyl * (r2 / 4.0 + h2 / 3.0);
76 : // Hemisphere contribution (using parallel axis theorem, d = h + 3r/8 from cap centre)
77 : const m3d::scalar d_hemi = h + 3.0 * r / 8.0;
78 : const m3d::scalar Ixx_hemi = m_hemi * (2.0 * r2 / 5.0 + d_hemi * d_hemi);
79 : const m3d::scalar Ixx = Ixx_cyl + Ixx_hemi;
80 :
81 : return m3d::smat3(Ixx, Iyy, Ixx, 0.0, 0.0, 0.0);
82 : }
83 :
84 : // ── Tight AABB ────────────────────────────────────────────────────────────
85 : // World axis of the capsule, then union of two sphere-radius boxes at endpoints.
86 0 : inline AABB compute_aabb(const Capsule &c, const m3d::tf &tf)
87 : {
88 0 : const m3d::vec3 axis = tf.rotate_vector(m3d::vec3(0, c.half_height, 0));
89 0 : const m3d::vec3 p1 = tf.pos + axis;
90 0 : const m3d::vec3 p2 = tf.pos - axis;
91 0 : const m3d::vec3 r(c.radius, c.radius, c.radius);
92 : return {
93 0 : m3d::vec3(m3d::min(p1.x, p2.x), m3d::min(p1.y, p2.y), m3d::min(p1.z, p2.z)) - r,
94 0 : m3d::vec3(m3d::max(p1.x, p2.x), m3d::max(p1.y, p2.y), m3d::max(p1.z, p2.z)) + r
95 0 : };
96 : }
97 : } // namespace rbc
|
