add an example for intersection plane (#249)

crates/parry3d/Cargo.toml

@@ -88,6 +88,7 @@ obj = { version = "0.10.2", optional = true }
 oorandom = "11"
 ptree = "0.4.0"
 rand = { version = "0.8" }
+macroquad = "0.4"
 
 [package.metadata.docs.rs]
 rustdoc-args = ["-Zunstable-options", "--generate-link-to-definition"]

crates/parry3d/examples/plane_intersection.rs

@@ -0,0 +1,156 @@
+use macroquad::models::Vertex;
+use macroquad::prelude::*;
+use nalgebra::{Point3, UnitVector3, Vector3};
+use parry3d::math::Real;
+use parry3d::query::IntersectResult;
+use parry3d::shape::{Cuboid, TriMesh};
+
+#[macroquad::main("parry3d::query::PlaneIntersection")]
+async fn main() {
+    let trimesh = Cuboid::new(Vector3::repeat(1.0)).to_trimesh();
+
+    let camera_pos = Vec3::new(-1.5f32, 2.5f32, -3f32);
+
+    let mesh = mquad_mesh_from_points(&trimesh, camera_pos);
+    let trimesh = TriMesh::new(trimesh.0, trimesh.1);
+
+    for _ in 1.. {
+        clear_background(BLACK);
+
+        let elapsed_time = get_time();
+
+        // Animated rotation for the intersection plane.
+        let bias = -1.2 * (elapsed_time as f32 / 3f32).sin();
+        let rotation = Quat::from_axis_angle(Vec3::Z, (elapsed_time as f32 * 40f32).to_radians());
+        let up_plane_vector = rotation * Vec3::Y;
+
+        // Get the intersection polyline.
+        let intersection_result = trimesh.intersection_with_local_plane(
+            &UnitVector3::new_normalize(Vector3::new(
+                up_plane_vector.x,
+                up_plane_vector.y,
+                up_plane_vector.z,
+            )),
+            bias,
+            0.0005,
+        );
+
+        // Initialize 3D camera.
+        set_camera(&Camera3D {
+            position: camera_pos,
+            up: Vec3::new(0f32, 1f32, 0f32),
+            target: Vec3::new(0.5f32, 0f32, 0.5f32),
+            ..Default::default()
+        });
+
+        // Draw involved shapes.
+        let plane_center = up_plane_vector * bias;
+        draw_line_3d(plane_center, plane_center + up_plane_vector, GREEN);
+        draw_mesh(&mesh);
+        draw_grid_ex(10, 0.333, BLUE, RED, plane_center, rotation);
+
+        /*
+         *
+         * Render the intersection.
+         *
+         */
+        match intersection_result {
+            IntersectResult::Intersect(points) => {
+                draw_polyline(
+                    points
+                        .segments()
+                        .map(|s| (mquad_from_na(s.a), mquad_from_na(s.b)))
+                        .collect(),
+                    Color::new(0f32, 1f32, 0f32, 1f32),
+                );
+                set_default_camera();
+                draw_text("Intersection found!");
+            }
+            IntersectResult::Negative => {
+                set_default_camera();
+                draw_text("No intersection found, the shape is below the plane.");
+            }
+            IntersectResult::Positive => {
+                set_default_camera();
+                draw_text("No intersection found, the shape is above the plane.");
+            }
+        }
+        next_frame().await
+    }
+}
+
+fn mquad_mesh_from_points(trimesh: &(Vec<Point3<Real>>, Vec<[u32; 3]>), camera_pos: Vec3) -> Mesh {
+    let (points, indices) = trimesh;
+    // Transform the parry mesh into a mquad Mesh
+    let (mquad_points, mquad_indices) = (
+        points
+            .iter()
+            .map(|p| Vertex {
+                position: mquad_from_na(*p),
+                uv: Vec2::new(p.x, p.y),
+                color: DARKGRAY,
+            })
+            .collect(),
+        indices.iter().flatten().map(|v| *v as u16).collect(),
+    );
+
+    // Macroquad doesn´t support adding normals to vertices, so we'll bake a color into these vertices.
+    // See https://github.com/not-fl3/macroquad/issues/321.
+
+    // Compute the normal of each vertex, making them unique
+    let vertices: Vec<Vertex> = mquad_compute_normals(&mquad_points, &mquad_indices, camera_pos);
+    // Regenerate the index for each vertex.
+    let indices: Vec<u16> = (0..vertices.len() * 3)
+        .into_iter()
+        .map(|i| i as u16)
+        .collect();
+    let mesh = Mesh {
+        vertices,
+        indices,
+        texture: None,
+    };
+    mesh
+}
+
+fn mquad_compute_normals(points: &Vec<Vertex>, indices: &Vec<u16>, cam_pos: Vec3) -> Vec<Vertex> {
+    let mut vertices: Vec<Vertex> = Vec::<Vertex>::new();
+    for indices in indices.chunks(3) {
+        let v0 = &points[indices[0] as usize];
+        let v1 = &points[indices[1] as usize];
+        let v2 = &points[indices[2] as usize];
+        let normal = (v0.position - v2.position)
+            .cross(v1.position - v2.position)
+            .normalize();
+        let brightness_mod = 0.2 + (0.8 / 2.) * (normal.dot(cam_pos) + 1.);
+
+        for &i in indices.iter() {
+            let mut color = points[i as usize].color;
+            color.r *= brightness_mod;
+            color.g *= brightness_mod;
+            color.b *= brightness_mod;
+
+            vertices.push(Vertex {
+                position: points[i as usize].position,
+                uv: Vec2::ZERO,
+                color: color,
+            });
+        }
+    }
+    vertices
+}
+
+fn draw_polyline(polygon: Vec<(Vec3, Vec3)>, color: Color) {
+    for i in 0..polygon.len() {
+        let a = polygon[i].0;
+        let b = polygon[i].1;
+        draw_line_3d(a, b, color);
+    }
+}
+
+fn mquad_from_na(a: Point3<Real>) -> Vec3 {
+    Vec3::new(a.x, a.y, a.z)
+}
+
+fn draw_text(text: &str) {
+    macroquad::text::draw_text(text, 10.0, 48.0 + 18.0, 30.0, WHITE);
+}