semantic_scene/
math.rs

1//! Geometry primitives used by [`SemanticScene`](crate::SemanticScene).
2
3use std::{f32::consts::FRAC_1_SQRT_2, fmt};
4
5/// Two-dimensional vector.
6#[derive(Debug, Clone, Copy, PartialEq)]
7pub struct Vec2(pub f32, pub f32);
8
9/// Quaternion rotation requested while loading a scene descriptor.
10///
11/// Values are stored as `[x, y, z, w]`, matching Habitat-Sim's Python vector
12/// convention for loader rotations.
13#[derive(Debug, Clone, Copy, PartialEq)]
14pub struct Rotation3 {
15    xyzw: [f32; 4],
16}
17
18/// Three-dimensional vector.
19#[derive(Debug, Clone, Copy, PartialEq)]
20pub struct Vec3(pub f32, pub f32, pub f32);
21
22impl Vec3 {
23    /// Returns the cross product of two vectors.
24    #[must_use]
25    pub fn cross(self, other: Self) -> Self {
26        Self(
27            self.1.mul_add(other.2, -(self.2 * other.1)),
28            self.2.mul_add(other.0, -(self.0 * other.2)),
29            self.0.mul_add(other.1, -(self.1 * other.0)),
30        )
31    }
32
33    fn add(self, other: Self) -> Self {
34        Self(self.0 + other.0, self.1 + other.1, self.2 + other.2)
35    }
36
37    fn scale(self, factor: f32) -> Self {
38        Self(self.0 * factor, self.1 * factor, self.2 * factor)
39    }
40
41    const fn abs(self) -> Self {
42        Self(self.0.abs(), self.1.abs(), self.2.abs())
43    }
44}
45
46impl Rotation3 {
47    /// Identity rotation.
48    pub const IDENTITY: Self = Self::from_xyzw([0.0, 0.0, 0.0, 1.0]);
49
50    /// Habitat-Sim's default rotation for MP3D semantic scenes.
51    pub const HABITAT_MP3D: Self = Self::from_xyzw([-FRAC_1_SQRT_2, 0.0, 0.0, FRAC_1_SQRT_2]);
52
53    /// Builds a rotation from a quaternion represented as `[x, y, z, w]`.
54    #[must_use]
55    pub const fn from_xyzw(xyzw: [f32; 4]) -> Self {
56        Self { xyzw }
57    }
58
59    /// Returns the quaternion represented as `[x, y, z, w]`.
60    #[must_use]
61    pub const fn xyzw(self) -> [f32; 4] {
62        self.xyzw
63    }
64
65    /// Returns whether this is the identity rotation.
66    #[must_use]
67    pub fn is_identity(self) -> bool {
68        self == Self::IDENTITY
69    }
70
71    /// Rotates a vector.
72    #[must_use]
73    pub fn transform_vector(self, vector: Vec3) -> Vec3 {
74        if self.is_identity() {
75            return vector;
76        }
77
78        let [quat_x, quat_y, quat_z, quat_w] = self.xyzw;
79        let quaternion_vector = Vec3(quat_x, quat_y, quat_z);
80        let uv = quaternion_vector.cross(vector);
81        let uuv = quaternion_vector.cross(uv);
82        vector.add(uv.scale(2.0 * quat_w)).add(uuv.scale(2.0))
83    }
84}
85
86impl Default for Rotation3 {
87    fn default() -> Self {
88        Self::HABITAT_MP3D
89    }
90}
91
92impl fmt::Display for Vec2 {
93    fn fmt(&self, formatter: &mut fmt::Formatter<'_>) -> fmt::Result {
94        write!(formatter, "({:.2}, {:.2})", self.0, self.1)
95    }
96}
97
98impl fmt::Display for Vec3 {
99    fn fmt(&self, formatter: &mut fmt::Formatter<'_>) -> fmt::Result {
100        write!(formatter, "({:.2}, {:.2}, {:.2})", self.0, self.1, self.2)
101    }
102}
103
104/// Axis-aligned bounding box.
105#[derive(Debug, Clone, Copy, PartialEq)]
106pub struct Aabb {
107    /// Minimum corner of the box.
108    pub min: Vec3,
109    /// Maximum corner of the box.
110    pub max: Vec3,
111}
112
113impl Aabb {
114    /// Returns whether the point lies inside or on the box boundary.
115    #[must_use]
116    pub fn contains(self, point: Vec3) -> bool {
117        point.0 >= self.min.0
118            && point.0 <= self.max.0
119            && point.1 >= self.min.1
120            && point.1 <= self.max.1
121            && point.2 >= self.min.2
122            && point.2 <= self.max.2
123    }
124
125    /// Returns the center of the box.
126    #[must_use]
127    pub const fn center(self) -> Vec3 {
128        Vec3(
129            self.min.0.midpoint(self.max.0),
130            self.min.1.midpoint(self.max.1),
131            self.min.2.midpoint(self.max.2),
132        )
133    }
134
135    /// Returns the size of the box along each axis.
136    #[must_use]
137    pub fn size(self) -> Vec3 {
138        Vec3(
139            self.max.0 - self.min.0,
140            self.max.1 - self.min.1,
141            self.max.2 - self.min.2,
142        )
143    }
144
145    /// Returns this box rotated with Habitat-Sim's AABB algorithm.
146    #[must_use]
147    pub fn rotated(self, rotation: Rotation3) -> Self {
148        if rotation.is_identity() {
149            return self;
150        }
151
152        let center = rotation.transform_vector(self.center());
153        let half_sizes = rotation.transform_vector(self.size()).abs().scale(0.5);
154        Self {
155            min: Vec3(
156                center.0 - half_sizes.0,
157                center.1 - half_sizes.1,
158                center.2 - half_sizes.2,
159            ),
160            max: Vec3(
161                center.0 + half_sizes.0,
162                center.1 + half_sizes.1,
163                center.2 + half_sizes.2,
164            ),
165        }
166    }
167}
168
169impl fmt::Display for Aabb {
170    fn fmt(&self, formatter: &mut fmt::Formatter<'_>) -> fmt::Result {
171        write!(formatter, "min={}, max={}", self.min, self.max)
172    }
173}
174
175/// Oriented bounding box.
176#[derive(Debug, Clone, Copy, PartialEq)]
177pub struct Obb {
178    /// Box center.
179    pub center: Vec3,
180    /// First local box axis.
181    pub axis0: Vec3,
182    /// Second local box axis.
183    pub axis1: Vec3,
184    /// Third local box axis.
185    pub axis2: Vec3,
186    /// Half of the box dimensions along each local axis.
187    pub half_extents: Vec3,
188}
189
190impl Obb {
191    /// Returns the axis-aligned bounding box enclosing this oriented box.
192    #[must_use]
193    pub fn to_aabb(self) -> Aabb {
194        let ex = extent(self.axis0.0, self.axis1.0, self.axis2.0, self.half_extents);
195        let ey = extent(self.axis0.1, self.axis1.1, self.axis2.1, self.half_extents);
196        let ez = extent(self.axis0.2, self.axis1.2, self.axis2.2, self.half_extents);
197
198        Aabb {
199            min: Vec3(self.center.0 - ex, self.center.1 - ey, self.center.2 - ez),
200            max: Vec3(self.center.0 + ex, self.center.1 + ey, self.center.2 + ez),
201        }
202    }
203
204    /// Returns this box rotated by rotating its center and local axes.
205    #[must_use]
206    pub fn rotated(self, rotation: Rotation3) -> Self {
207        if rotation.is_identity() {
208            return self;
209        }
210
211        let axis0 = rotation.transform_vector(self.axis0);
212        let axis1 = rotation.transform_vector(self.axis1);
213        Self {
214            center: rotation.transform_vector(self.center),
215            axis0,
216            axis1,
217            axis2: axis0.cross(axis1),
218            half_extents: self.half_extents,
219        }
220    }
221}
222
223impl fmt::Display for Obb {
224    fn fmt(&self, formatter: &mut fmt::Formatter<'_>) -> fmt::Result {
225        write!(
226            formatter,
227            "center={}, half_extents={}",
228            self.center, self.half_extents
229        )
230    }
231}
232
233fn extent(axis0: f32, axis1: f32, axis2: f32, half_extents: Vec3) -> f32 {
234    axis2.abs().mul_add(
235        half_extents.2,
236        axis0
237            .abs()
238            .mul_add(half_extents.0, axis1.abs() * half_extents.1),
239    )
240}
semantic_scene/math.rs

semantic_scene/
math.rs
