semantic-scene 0.1.1

//! Geometry primitives used by [`SemanticScene`](crate::SemanticScene).

use std::{f32::consts::FRAC_1_SQRT_2, fmt};

/// Two-dimensional vector.
#[derive(Debug, Clone, Copy, PartialEq)]
pub struct Vec2(pub f32, pub f32);

/// Quaternion rotation requested while loading a scene descriptor.
///
/// Values are stored as `[x, y, z, w]`, matching Habitat-Sim's Python vector
/// convention for loader rotations.
#[derive(Debug, Clone, Copy, PartialEq)]
pub struct Rotation3 {
    xyzw: [f32; 4],
}

/// Three-dimensional vector.
#[derive(Debug, Clone, Copy, PartialEq)]
pub struct Vec3(pub f32, pub f32, pub f32);

impl Vec3 {
    /// Returns the cross product of two vectors.
    #[must_use]
    pub fn cross(self, other: Self) -> Self {
        Self(
            self.1.mul_add(other.2, -(self.2 * other.1)),
            self.2.mul_add(other.0, -(self.0 * other.2)),
            self.0.mul_add(other.1, -(self.1 * other.0)),
        )
    }

    fn add(self, other: Self) -> Self {
        Self(self.0 + other.0, self.1 + other.1, self.2 + other.2)
    }

    fn scale(self, factor: f32) -> Self {
        Self(self.0 * factor, self.1 * factor, self.2 * factor)
    }

    const fn abs(self) -> Self {
        Self(self.0.abs(), self.1.abs(), self.2.abs())
    }
}

impl Rotation3 {
    /// Identity rotation.
    pub const IDENTITY: Self = Self::from_xyzw([0.0, 0.0, 0.0, 1.0]);

    /// Habitat-Sim's default rotation for MP3D semantic scenes.
    pub const HABITAT_MP3D: Self = Self::from_xyzw([-FRAC_1_SQRT_2, 0.0, 0.0, FRAC_1_SQRT_2]);

    /// Builds a rotation from a quaternion represented as `[x, y, z, w]`.
    #[must_use]
    pub const fn from_xyzw(xyzw: [f32; 4]) -> Self {
        Self { xyzw }
    }

    /// Returns the quaternion represented as `[x, y, z, w]`.
    #[must_use]
    pub const fn xyzw(self) -> [f32; 4] {
        self.xyzw
    }

    /// Returns whether this is the identity rotation.
    #[must_use]
    pub fn is_identity(self) -> bool {
        self == Self::IDENTITY
    }

    /// Rotates a vector.
    #[must_use]
    pub fn transform_vector(self, vector: Vec3) -> Vec3 {
        if self.is_identity() {
            return vector;
        }

        let [quat_x, quat_y, quat_z, quat_w] = self.xyzw;
        let quaternion_vector = Vec3(quat_x, quat_y, quat_z);
        let uv = quaternion_vector.cross(vector);
        let uuv = quaternion_vector.cross(uv);
        vector.add(uv.scale(2.0 * quat_w)).add(uuv.scale(2.0))
    }
}

impl Default for Rotation3 {
    fn default() -> Self {
        Self::HABITAT_MP3D
    }
}

impl fmt::Display for Vec2 {
    fn fmt(&self, formatter: &mut fmt::Formatter<'_>) -> fmt::Result {
        write!(formatter, "({:.2}, {:.2})", self.0, self.1)
    }
}

impl fmt::Display for Vec3 {
    fn fmt(&self, formatter: &mut fmt::Formatter<'_>) -> fmt::Result {
        write!(formatter, "({:.2}, {:.2}, {:.2})", self.0, self.1, self.2)
    }
}

/// Axis-aligned bounding box.
#[derive(Debug, Clone, Copy, PartialEq)]
pub struct Aabb {
    /// Minimum corner of the box.
    pub min: Vec3,
    /// Maximum corner of the box.
    pub max: Vec3,
}

impl Aabb {
    /// Returns whether the point lies inside or on the box boundary.
    #[must_use]
    pub fn contains(self, point: Vec3) -> bool {
        point.0 >= self.min.0
            && point.0 <= self.max.0
            && point.1 >= self.min.1
            && point.1 <= self.max.1
            && point.2 >= self.min.2
            && point.2 <= self.max.2
    }

    /// Returns the center of the box.
    #[must_use]
    pub const fn center(self) -> Vec3 {
        Vec3(
            self.min.0.midpoint(self.max.0),
            self.min.1.midpoint(self.max.1),
            self.min.2.midpoint(self.max.2),
        )
    }

    /// Returns the size of the box along each axis.
    #[must_use]
    pub fn size(self) -> Vec3 {
        Vec3(
            self.max.0 - self.min.0,
            self.max.1 - self.min.1,
            self.max.2 - self.min.2,
        )
    }

    /// Returns this box rotated with Habitat-Sim's AABB algorithm.
    #[must_use]
    pub fn rotated(self, rotation: Rotation3) -> Self {
        if rotation.is_identity() {
            return self;
        }

        let center = rotation.transform_vector(self.center());
        let half_sizes = rotation.transform_vector(self.size()).abs().scale(0.5);
        Self {
            min: Vec3(
                center.0 - half_sizes.0,
                center.1 - half_sizes.1,
                center.2 - half_sizes.2,
            ),
            max: Vec3(
                center.0 + half_sizes.0,
                center.1 + half_sizes.1,
                center.2 + half_sizes.2,
            ),
        }
    }
}

impl fmt::Display for Aabb {
    fn fmt(&self, formatter: &mut fmt::Formatter<'_>) -> fmt::Result {
        write!(formatter, "min={}, max={}", self.min, self.max)
    }
}

/// Oriented bounding box.
#[derive(Debug, Clone, Copy, PartialEq)]
pub struct Obb {
    /// Box center.
    pub center: Vec3,
    /// First local box axis.
    pub axis0: Vec3,
    /// Second local box axis.
    pub axis1: Vec3,
    /// Third local box axis.
    pub axis2: Vec3,
    /// Half of the box dimensions along each local axis.
    pub half_extents: Vec3,
}

impl Obb {
    /// Returns the axis-aligned bounding box enclosing this oriented box.
    #[must_use]
    pub fn to_aabb(self) -> Aabb {
        let ex = extent(self.axis0.0, self.axis1.0, self.axis2.0, self.half_extents);
        let ey = extent(self.axis0.1, self.axis1.1, self.axis2.1, self.half_extents);
        let ez = extent(self.axis0.2, self.axis1.2, self.axis2.2, self.half_extents);

        Aabb {
            min: Vec3(self.center.0 - ex, self.center.1 - ey, self.center.2 - ez),
            max: Vec3(self.center.0 + ex, self.center.1 + ey, self.center.2 + ez),
        }
    }

    /// Returns this box rotated by rotating its center and local axes.
    #[must_use]
    pub fn rotated(self, rotation: Rotation3) -> Self {
        if rotation.is_identity() {
            return self;
        }

        let axis0 = rotation.transform_vector(self.axis0);
        let axis1 = rotation.transform_vector(self.axis1);
        Self {
            center: rotation.transform_vector(self.center),
            axis0,
            axis1,
            axis2: axis0.cross(axis1),
            half_extents: self.half_extents,
        }
    }
}

impl fmt::Display for Obb {
    fn fmt(&self, formatter: &mut fmt::Formatter<'_>) -> fmt::Result {
        write!(
            formatter,
            "center={}, half_extents={}",
            self.center, self.half_extents
        )
    }
}

fn extent(axis0: f32, axis1: f32, axis2: f32, half_extents: Vec3) -> f32 {
    axis2.abs().mul_add(
        half_extents.2,
        axis0
            .abs()
            .mul_add(half_extents.0, axis1.abs() * half_extents.1),
    )
}