Add physics engine integration with rapier3d

src/main.rs

@@ -5,6 +5,7 @@ mod gltf_loader;
 mod render;
 mod time;
 mod ui;
+mod physics;
 
 use anyhow::Result;
 use camera::Camera;
@@ -12,6 +13,8 @@ use ecs::{Transform};
 use glam::{Quat, Vec3, EulerRot};
 use glium::backend::glutin::SimpleWindowBuilder;
 use render::GliumRenderer;
+use rapier3d::prelude::*;
+use rapier3d::na::{UnitQuaternion, Quaternion};
 
 fn main() -> Result<()> {
     let event_loop = glium::winit::event_loop::EventLoop::builder()
@@ -20,7 +23,7 @@ fn main() -> Result<()> {
 
     let (window, display) = SimpleWindowBuilder::new()
         .with_title("fps")
-        .with_inner_size(1280, 720)
+        .with_inner_size(1920, 1080)
         .build(&event_loop);
 
     // Create ECS renderer which internally owns both the world and the renderer
@@ -30,6 +33,8 @@ fn main() -> Result<()> {
         ecs::ECSRenderer::new(renderer, world)
     };
 
+    let mut physics = physics::Physics::new();
+
     // Dear ImGui integration
     let mut gui = ui::Gui::new(&display, &window)?;
 
@@ -38,7 +43,7 @@ fn main() -> Result<()> {
     let object_ent = {
         let model_3d = gltf_loader::load_gltf("resources/models/tree.gltf", &display)?;
         ecsr.spawn_mesh(model_3d, Transform {
-            translation: Vec3::new(0.0, -2.5, -5.0),
+            translation: Vec3::new(0.0, 2.5, -5.0),
             rotation:    Quat::IDENTITY,
             scale:       Vec3::new(0.01, 0.01, 0.01),
         })
@@ -49,10 +54,34 @@ fn main() -> Result<()> {
         ecsr.spawn_mesh(model_3d, Transform {
             translation: Vec3::new(0.0, -1.5, 0.0),
             rotation:    Quat::IDENTITY,
-            scale:       Vec3::new(1.0, 1.0, 1.0),
+            scale:       Vec3::new(10.0, 10.0, 10.0),
         })
     };
 
+    // Add physics for ground
+    let ground_tr = *ecsr.world.get::<&Transform>(ground_ent).unwrap();
+    let (axis, angle) = ground_tr.rotation.to_axis_angle();
+    let rotation = vector![axis.x * angle, axis.y * angle, axis.z * angle];
+    let ground_pos = Isometry::new(
+        vector![ground_tr.translation.x, ground_tr.translation.y, ground_tr.translation.z],
+        rotation,
+    );
+    let ground_rb = RigidBodyBuilder::fixed().position(ground_pos).build();
+    let ground_collider = ColliderBuilder::cuboid(10.0, 0.1, 10.0).build();
+    physics.add_rigid_body(ground_ent, ground_rb, ground_collider);
+
+    // Add physics for object
+    let object_tr = *ecsr.world.get::<&Transform>(object_ent).unwrap();
+    let (axis, angle) = object_tr.rotation.to_axis_angle();
+    let rotation = vector![axis.x * angle, axis.y * angle, axis.z * angle];
+    let object_pos = Isometry::new(
+        vector![object_tr.translation.x, object_tr.translation.y, object_tr.translation.z],
+        rotation,
+    );
+    let object_rb = RigidBodyBuilder::dynamic().position(object_pos).build();
+    let object_collider = ColliderBuilder::cylinder(2.0, 0.2).build();
+    physics.add_rigid_body(object_ent, object_rb, object_collider);
+
 
     let camera_ent = {
         let (w, h): (u32, u32) = window.inner_size().into();
@@ -117,6 +146,7 @@ fn main() -> Result<()> {
                 },
                 Event::AboutToWait => {
                     time.tick();
+                    physics.step(time.delta_seconds(), &mut ecsr.world);
                     gui.prepare_frame(&window);
                     window.request_redraw();
                 }

src/physics.rs

@@ -0,0 +1,90 @@
+use std::collections::HashMap;
+use rapier3d::prelude::*;
+use hecs::World;
+use glam::{Vec3, Quat};
+use crate::ecs::Transform;
+
+pub struct Physics {
+    rigid_bodies: RigidBodySet,
+    colliders: ColliderSet,
+    integration_params: IntegrationParameters,
+    pipeline: PhysicsPipeline,
+    island_manager: IslandManager,
+    broad_phase: BroadPhaseMultiSap,
+    narrow_phase: NarrowPhase,
+    impulse_joints: ImpulseJointSet,
+    multibody_joints: MultibodyJointSet,
+    ccd_solver: CCDSolver,
+    query_pipeline: QueryPipeline,
+    gravity: Vector<f32>,
+    entity_to_rb: HashMap<hecs::Entity, RigidBodyHandle>,
+}
+
+impl Physics {
+    pub fn new() -> Self {
+        let rigid_bodies = RigidBodySet::new();
+        let colliders = ColliderSet::new();
+        let integration_params = IntegrationParameters::default();
+        let pipeline = PhysicsPipeline::new();
+        let island_manager = IslandManager::new();
+        let broad_phase = BroadPhaseMultiSap::new();
+        let narrow_phase = NarrowPhase::new();
+        let impulse_joints = ImpulseJointSet::new();
+        let multibody_joints = MultibodyJointSet::new();
+        let ccd_solver = CCDSolver::new();
+        let query_pipeline = QueryPipeline::new();
+        let gravity = vector![0.0, -9.81, 0.0];
+        Self {
+            rigid_bodies,
+            colliders,
+            integration_params,
+            pipeline,
+            island_manager,
+            broad_phase,
+            narrow_phase,
+            impulse_joints,
+            multibody_joints,
+            ccd_solver,
+            query_pipeline,
+            gravity,
+            entity_to_rb: HashMap::new(),
+        }
+    }
+
+    pub fn step(&mut self, dt: f32, world: &mut World) {
+        let physics_hooks = ();
+        let event_handler = ();
+        self.integration_params.dt = dt;
+        self.pipeline.step(
+            &self.gravity,
+            &self.integration_params,
+            &mut self.island_manager,
+            &mut self.broad_phase,
+            &mut self.narrow_phase,
+            &mut self.rigid_bodies,
+            &mut self.colliders,
+            &mut self.impulse_joints,
+            &mut self.multibody_joints,
+            &mut self.ccd_solver,
+            Some(&mut self.query_pipeline),
+            &physics_hooks,
+            &event_handler,
+        );
+
+        for (&ent, &handle) in &self.entity_to_rb {
+            if let Ok(mut tr) = world.get::<&mut Transform>(ent) {
+                let rb = self.rigid_bodies.get(handle).unwrap();
+                let pos = rb.position();
+                tr.translation = Vec3::new(pos.translation.x, pos.translation.y, pos.translation.z);
+                let q = pos.rotation;
+                tr.rotation = Quat::from_xyzw(q.i, q.j, q.k, q.w);
+            }
+        }
+    }
+
+    pub fn add_rigid_body(&mut self, ent: hecs::Entity, rb: RigidBody, collider: Collider) {
+        let rb_handle = self.rigid_bodies.insert(rb);
+        self.colliders.insert_with_parent(collider, rb_handle, &mut self.rigid_bodies);
+        self.entity_to_rb.insert(ent, rb_handle);
+    }
+}