First public release of Rapier.v0.1.0

1 files changed, 234 insertions, 0 deletions

diff --git a/src/geometry/cuboid.rs b/src/geometry/cuboid.rs
new file mode 100644
index 0000000..89e1a1e
--- /dev/null
+++ b/src/geometry/cuboid.rs
@@ -0,0 +1,234 @@
+#[cfg(feature = "dim3")]
+use crate::geometry::PolyhedronFace;
+use crate::geometry::{Cuboid, CuboidFeature, CuboidFeatureFace};
+use crate::math::{Point, Vector};
+use crate::utils::WSign;
+
+pub fn local_support_point(cube: &Cuboid, local_dir: Vector<f32>) -> Point<f32> {
+    local_dir.copy_sign_to(cube.half_extents).into()
+}
+
+// #[cfg(feature = "dim2")]
+// pub fn polygon_ref(
+//     cuboid: Cuboid,
+//     out_vertices: &mut [Point<f32>; 4],
+//     out_normals: &mut [Vector<f32>; 4],
+// ) -> PolygonRef {
+//     *out_vertices = [
+//         Point::new(cuboid.half_extents.x, -cuboid.half_extents.y),
+//         Point::new(cuboid.half_extents.x, cuboid.half_extents.y),
+//         Point::new(-cuboid.half_extents.x, cuboid.half_extents.y),
+//         Point::new(-cuboid.half_extents.x, -cuboid.half_extents.y),
+//     ];
+//     *out_normals = [Vector::x(), Vector::y(), -Vector::x(), -Vector::y()];
+//
+//     PolygonRef {
+//         vertices: &out_vertices[..],
+//         normals: &out_normals[..],
+//     }
+// }
+
+#[cfg(feature = "dim2")]
+pub fn vertex_feature_id(vertex: Point<f32>) -> u8 {
+    ((vertex.x.to_bits() >> 31) & 0b001 | (vertex.y.to_bits() >> 30) & 0b010) as u8
+}
+
+// #[cfg(feature = "dim3")]
+// pub fn vertex_feature_id(vertex: Point<f32>) -> u8 {
+//     ((vertex.x.to_bits() >> 31) & 0b001
+//         | (vertex.y.to_bits() >> 30) & 0b010
+//         | (vertex.z.to_bits() >> 29) & 0b100) as u8
+// }
+
+#[cfg(feature = "dim3")]
+pub fn polyhedron_support_face(cube: &Cuboid, local_dir: Vector<f32>) -> PolyhedronFace {
+    support_face(cube, local_dir).into()
+}
+
+#[cfg(feature = "dim2")]
+pub(crate) fn support_feature(cube: &Cuboid, local_dir: Vector<f32>) -> CuboidFeature {
+    // In 2D, it is best for stability to always return a face.
+    // It won't have any notable impact on performances anyway.
+    CuboidFeature::Face(support_face(cube, local_dir))
+
+    /*
+    let amax = local_dir.amax();
+
+    const MAX_DOT_THRESHOLD: f32 = 0.98480775301; // 10 degrees.
+
+    if amax > MAX_DOT_THRESHOLD {
+        // Support face.
+        CuboidFeature::Face(cube.support_face(local_dir))
+    } else {
+        // Support vertex
+        CuboidFeature::Vertex(cube.support_vertex(local_dir))
+    }
+    */
+}
+
+#[cfg(feature = "dim3")]
+pub(crate) fn support_feature(cube: &Cuboid, local_dir: Vector<f32>) -> CuboidFeature {
+    CuboidFeature::Face(support_face(cube, local_dir))
+    /*
+    const MAX_DOT_THRESHOLD: f32 = crate::utils::COS_10_DEGREES;
+    const MIN_DOT_THRESHOLD: f32 = 1.0 - MAX_DOT_THRESHOLD;
+
+    let amax = local_dir.amax();
+    let amin = local_dir.amin();
+
+    if amax > MAX_DOT_THRESHOLD {
+        // Support face.
+        CuboidFeature::Face(support_face(cube, local_dir))
+    } else if amin < MIN_DOT_THRESHOLD {
+        // Support edge.
+        CuboidFeature::Edge(support_edge(cube, local_dir))
+    } else {
+        // Support vertex.
+        CuboidFeature::Vertex(support_vertex(cube, local_dir))
+    }
+    */
+}
+
+// #[cfg(feature = "dim3")]
+// pub(crate) fn support_vertex(cube: &Cuboid, local_dir: Vector<f32>) -> CuboidFeatureVertex {
+//     let vertex = local_support_point(cube, local_dir);
+//     let vid = vertex_feature_id(vertex);
+//
+//     CuboidFeatureVertex { vertex, vid }
+// }
+
+// #[cfg(feature = "dim3")]
+// pub(crate) fn support_edge(cube: &Cuboid, local_dir: Vector<f32>) -> CuboidFeatureEdge {
+//     let he = cube.half_extents;
+//     let i = local_dir.iamin();
+//     let j = (i + 1) % 3;
+//     let k = (i + 2) % 3;
+//     let mut a = Point::origin();
+//     a[i] = he[i];
+//     a[j] = local_dir[j].copy_sign_to(he[j]);
+//     a[k] = local_dir[k].copy_sign_to(he[k]);
+//
+//     let mut b = a;
+//     b[i] = -he[i];
+//
+//     let vid1 = vertex_feature_id(a);
+//     let vid2 = vertex_feature_id(b);
+//     let eid = (vid1.max(vid2) << 3) | vid1.min(vid2) | 0b11_000_000;
+//
+//     CuboidFeatureEdge {
+//         vertices: [a, b],
+//         vids: [vid1, vid2],
+//         eid,
+//     }
+// }
+
+#[cfg(feature = "dim2")]
+pub fn support_face(cube: &Cuboid, local_dir: Vector<f32>) -> CuboidFeatureFace {
+    let he = cube.half_extents;
+    let i = local_dir.iamin();
+    let j = (i + 1) % 2;
+    let mut a = Point::origin();
+    a[i] = he[i];
+    a[j] = local_dir[j].copy_sign_to(he[j]);
+
+    let mut b = a;
+    b[i] = -he[i];
+
+    let vid1 = vertex_feature_id(a);
+    let vid2 = vertex_feature_id(b);
+    let fid = (vid1.max(vid2) << 2) | vid1.min(vid2) | 0b11_00_00;
+
+    CuboidFeatureFace {
+        vertices: [a, b],
+        vids: [vid1, vid2],
+        fid,
+    }
+}
+
+#[cfg(feature = "dim3")]
+pub(crate) fn support_face(cube: &Cuboid, local_dir: Vector<f32>) -> CuboidFeatureFace {
+    // NOTE: can we use the orthonormal basis of local_dir
+    // to make this AoSoA friendly?
+    let he = cube.half_extents;
+    let iamax = local_dir.iamax();
+    let sign = local_dir[iamax].copy_sign_to(1.0);
+
+    let vertices = match iamax {
+        0 => [
+            Point::new(he.x * sign, he.y, he.z),
+            Point::new(he.x * sign, -he.y, he.z),
+            Point::new(he.x * sign, -he.y, -he.z),
+            Point::new(he.x * sign, he.y, -he.z),
+        ],
+        1 => [
+            Point::new(he.x, he.y * sign, he.z),
+            Point::new(-he.x, he.y * sign, he.z),
+            Point::new(-he.x, he.y * sign, -he.z),
+            Point::new(he.x, he.y * sign, -he.z),
+        ],
+        2 => [
+            Point::new(he.x, he.y, he.z * sign),
+            Point::new(he.x, -he.y, he.z * sign),
+            Point::new(-he.x, -he.y, he.z * sign),
+            Point::new(-he.x, he.y, he.z * sign),
+        ],
+        _ => unreachable!(),
+    };
+
+    pub fn vid(i: u8) -> u8 {
+        // Each vertex has an even feature id.
+        i * 2
+    }
+
+    let sign_index = ((sign as i8 + 1) / 2) as usize;
+    // The vertex id as numbered depending on the sign of the vertex
+    // component. A + sign means the corresponding bit is 0 while a -
+    // sign means the corresponding bit is 1.
+    // For exampl the vertex [2.0, -1.0, -3.0] has the id 0b011
+    let vids = match iamax {
+        0 => [
+            [vid(0b000), vid(0b010), vid(0b011), vid(0b001)],
+            [vid(0b100), vid(0b110), vid(0b111), vid(0b101)],
+        ][sign_index],
+        1 => [
+            [vid(0b000), vid(0b100), vid(0b101), vid(0b001)],
+            [vid(0b010), vid(0b110), vid(0b111), vid(0b011)],
+        ][sign_index],
+        2 => [
+            [vid(0b000), vid(0b010), vid(0b110), vid(0b100)],
+            [vid(0b001), vid(0b011), vid(0b111), vid(0b101)],
+        ][sign_index],
+        _ => unreachable!(),
+    };
+
+    // The feature ids of edges is obtained from the vertex ids
+    // of their endpoints.
+    // Assuming vid1 > vid2, we do:   (vid1 << 3) | vid2 | 0b11000000
+    //
+    let eids = match iamax {
+        0 => [
+            [0b11_010_000, 0b11_011_010, 0b11_011_001, 0b11_001_000],
+            [0b11_110_100, 0b11_111_110, 0b11_111_101, 0b11_101_100],
+        ][sign_index],
+        1 => [
+            [0b11_100_000, 0b11_101_100, 0b11_101_001, 0b11_001_000],
+            [0b11_110_010, 0b11_111_110, 0b11_111_011, 0b11_011_010],
+        ][sign_index],
+        2 => [
+            [0b11_010_000, 0b11_110_010, 0b11_110_100, 0b11_100_000],
+            [0b11_011_001, 0b11_111_011, 0b11_111_101, 0b11_101_001],
+        ][sign_index],
+        _ => unreachable!(),
+    };
+
+    // The face with normals [x, y, z] are numbered [10, 11, 12].
+    // The face with negated normals are numbered [13, 14, 15].
+    let fid = iamax + sign_index * 3 + 10;
+
+    CuboidFeatureFace {
+        vertices,
+        vids,
+        eids,
+        fid: fid as u8,
+    }
+}