tmp: add most recent progressexperiments

1 files changed, 116 insertions, 51 deletions

diff --git a/playground/src/main.rs b/playground/src/main.rs
index 8059417..3182b64 100644
--- a/playground/src/main.rs
+++ b/playground/src/main.rs
@@ -1,13 +1,16 @@
+use ::core::f64;
+
 use raylib::prelude::*;
 
 use physics::algebra::{Point, Scalar, Vector, N};
 use physics::force::{drag::Drag, gravity::Gravity, spring::Spring};
-use physics::nalgebra::Point as PointBase;
+use physics::nalgebra::{Point as PointBase, Rotation3, Unit};
 use physics::particle_system::{Particle, ParticleSystem};
 use physics::renderer::Camera;
 use physics::solver::Solver;
 
-const SCALE: Scalar = 10.0;
+const SCALE: Scalar = 25.0;
+const CAMERA_DISTANCE: Scalar = 20.0;
 fn screen_space_to_raylib(p: PointBase<Scalar, 2>, d: &RaylibDrawHandle) -> PointBase<i32, 2> {
     PointBase::<i32, 2>::new(
         ((d.get_screen_width() / 2) as Scalar + p.x * SCALE) as i32,
@@ -22,16 +25,14 @@ fn raylib_to_screen_space(p: PointBase<i32, 2>, d: &RaylibDrawHandle) -> PointBa
 }
 
 fn main() {
-    let dt = 0.0001;
+    let dt = 1e-5;
     let mut system = ParticleSystem {
         particles: vec![
-            Particle::new(Point::origin(), 4.0),
-            Particle::new(Point::new(-30.0, 0.0), 15.0),
-            Particle::new(Point::new(20.0, 0.0), 30.0),
-            Particle::new(Point::new(5.0, 20.0), 50.0),
-            Particle::new(Point::origin(), 25.0),
-            Particle::new(Point::new(50.0, 0.0), 10.0),
-            Particle::new(Point::new(-30.0, -30.0), 10.0),
+            Particle::new(Point::new(3.0, 0.0, 0.0), 1.0),
+            Particle::new(Point::new(0.0, 0.0, 0.0), 1.0),
+            Particle::new(Point::new(0.0, 4.0, 0.0), 1.0),
+            Particle::new(Point::new(-10.0, 00.0, 0.0), 1.0),
+            // Particle::new(Point::new(0.0, -100.0, -100.0), 1.0),
         ],
         constraints: vec![],
         forces: vec![
@@ -39,27 +40,43 @@ fn main() {
                 vector: Vector::y() * -9.8,
             }),
             Box::new(Drag { coefficient: 0.2 }),
-            Box::new(Spring {
-                particle_ids: [4, 2],
-                spring_constant: 0.75,
-                damping_constant: 0.3,
-                rest_length: 20.0,
-            }),
+            // Box::new(Spring {
+            //     particle_ids: [0, 3],
+            //     spring_constant: 1000.00,
+            //     damping_constant: 36.1,
+            //     rest_length: 13.0,
+            // }),
         ],
         t: 0.0,
     };
 
-    system.add_anchor_constraint(0, Point::origin());
-    system.add_beam_constraint([0, 2]);
+    system.add_beam_constraint([0, 1]);
     system.add_beam_constraint([1, 2]);
-    system.add_beam_constraint([1, 3]);
-    system.add_beam_constraint([2, 3]);
-    system.add_slider_constraint(5, Vector::x());
-    system.add_beam_constraint([5, 4]);
+    system.add_beam_constraint([2, 0]);
+
+    system.add_slider_constraint(3, Vector::x());
+    system.add_beam_constraint([3, 0]);
+
+    // system.add_beam_constraint([3, 4]);
+    // system.add_distsance_constraint(3, Line::new(Point::origin(), [Vector::x()]), 0.0);
+
+    // system.add_slider_constraint(5, Vector::x());
+    // system.add_anchor_constraint(2, Point::origin());
+    //
+    // system.add_beam_constraint([1, 2]);
+    // system.add_beam_constraint([1, 3]);
+    // system.add_beam_constraint([2, 3]);
+    // system.add_beam_constraint([5, 4]);
 
     let mut selected_particle_id = None;
     let mut watch_particle_id = None;
     let mouse_particle_id = system.particles.len() - 1;
+    let mut camera_right = Vector::x();
+
+    let trace_particle_id = 3;
+    let mut trace_particle_positions: Vec<PointBase<i32, 2>> = vec![];
+
+    let mut max_kinetic: Scalar = 0.0;
 
     let (mut rl, thread) = raylib::init()
         .size(640, 480)
@@ -68,33 +85,57 @@ fn main() {
         .build();
 
     while !rl.window_should_close() {
+        // sleep(Duration::from_millis(1));
+        // dbg!(&system.particles[3]);
+
         let mut d = rl.begin_drawing(&thread);
 
+        let kinetic = system.get_kinetic_energy();
+        max_kinetic = max_kinetic.max(kinetic);
+
         d.clear_background(Color::WHITE);
-        d.draw_text(
-            format!(
-                "Time: {:03.3}\n\nFPS: {}\n\nKinetic energy: {:03.0}",
+        let fps = d.get_fps();
+        let mut text = format!(
+                "Time: {:03.3}\n\nFPS: {}\n\nKinetic energy: {:03.0}\n\nMax kinetic energy: {:03.0}\n\n",
                 system.t,
-                d.get_fps(),
-                system.get_kinetic_energy()
-            )
-            .as_str(),
-            12,
-            12,
-            20,
-            Color::BLACK,
-        );
+                fps,
+                kinetic,
+                max_kinetic,
+            );
+
+        {
+            let (q, _) = system.collect_q();
+            for (id, constraint) in system.constraints.iter().enumerate() {
+                let c = constraint.c(&q);
+                text += &format!("Constraint {}: {:.05}\n\n", id, c);
+            }
+        }
+
+        d.draw_text(text.as_str(), 12, 12, 20, Color::BLACK);
+
+        if d.is_key_down(KeyboardKey::KEY_L) {
+            camera_right = Rotation3::from_axis_angle(
+                &Unit::new_normalize(Vector::y()),
+                (f64::consts::PI / 180.0 / 10.0) as Scalar,
+            ) * camera_right;
+        }
+        if d.is_key_down(KeyboardKey::KEY_H) {
+            camera_right = Rotation3::from_axis_angle(
+                &Unit::new_normalize(Vector::y()),
+                (-f64::consts::PI / 180.0 / 10.0) as Scalar,
+            ) * camera_right;
+        }
 
         let camera = Camera::new(
             match watch_particle_id {
                 Some(id) => {
                     let particle: &Particle = &system.particles[id];
                     particle.position
-                },
+                }
                 None => Point::origin(),
             },
             Vector::y(),
-            Vector::x(),
+            camera_right,
         );
 
         match selected_particle_id {
@@ -118,11 +159,17 @@ fn main() {
             None => (),
         }
 
-        for _ in 0..10 {
+        for _ in 0..500 as usize {
             system.apply_forces();
+            if d.is_key_down(KeyboardKey::KEY_Z) {
+                for particle in &mut system.particles {
+                    particle.apply_force(Vector::z() * 20.0);
+                }
+            }
             system.enforce_constraints();
             system.step(dt);
         }
+        // panic!("HALT");
 
         match selected_particle_id {
             Some(_) => {
@@ -138,7 +185,10 @@ fn main() {
         for (particle_id, particle) in system.particles.iter().enumerate() {
             let position =
                 screen_space_to_raylib(camera.world_to_screen_space(particle.position), &d);
-            let radius = particle.mass.powf(1.0 / (N as f64)) as f32 * SCALE as f32;
+            let radius_factor = (CAMERA_DISTANCE - particle.position.z) / CAMERA_DISTANCE;
+            let radius = particle.mass.powf(1.0 / (N as Scalar)) as f32
+                * SCALE as f32
+                * radius_factor as f32;
 
             if d.is_mouse_button_down(MouseButton::MOUSE_BUTTON_LEFT)
                 && selected_particle_id.is_none()
@@ -169,9 +219,24 @@ fn main() {
                 } else if watch_particle_id.is_some_and(|v| v == particle_id) {
                     Color::YELLOW
                 } else {
-                    Color::BLACK
+                    Color::new(
+                        0,
+                        0,
+                        (particle.position.z / CAMERA_DISTANCE * 255.0) as u8,
+                        255,
+                    )
                 },
             );
+
+            let force_end = screen_space_to_raylib(camera.world_to_screen_space(particle.position + particle.velocity), &d);
+            d.draw_line(position.x, position.y, force_end.x, force_end.y, Color::BLUE);
+
+            if particle_id == trace_particle_id {
+                trace_particle_positions.push(position);
+                if trace_particle_positions.len() > 500 {
+                    trace_particle_positions.remove(0);
+                }
+            }
         }
         for c in &system.constraints {
             let particle_ids = c.get_particles();
@@ -188,20 +253,20 @@ fn main() {
                 d.draw_line(a.x, a.y, b.x, b.y, Color::GRAY);
             }
         }
-
-        {
-            // Hard-coded spring
-            let particle_ids = [4, 2];
-            let a = screen_space_to_raylib(
-                camera.world_to_screen_space(system.particles[particle_ids[0]].position),
-                &d,
-            );
-            let b = screen_space_to_raylib(
-                camera.world_to_screen_space(system.particles[particle_ids[1]].position),
-                &d,
+        for (i, pos) in trace_particle_positions.iter().enumerate() {
+            d.draw_circle(
+                pos.x,
+                pos.y,
+                1.0,
+                Color::new(
+                    0,
+                    255,
+                    0,
+                    (i as f32 / trace_particle_positions.len() as f32 * 255.0) as u8,
+                ),
             );
-            d.draw_line(a.x, a.y, b.x, b.y, Color::GREEN);
         }
+
         {
             // Hard-coded spring
             match selected_particle_id {