tmp: add most recent progressexperiments

12 files changed, 228 insertions, 19 deletions

diff --git a/physics/Cargo.lock b/physics/Cargo.lock
index 98fe5af..9f69cd7 100644
--- a/physics/Cargo.lock
+++ b/physics/Cargo.lock
@@ -24,6 +24,37 @@ source = "registry+https://github.com/rust-lang/crates.io-index"
 checksum = "8b37c88a63ffd85d15b406896cc343916d7cf57838a847b3a6f2ca5d39a5695a"
 
 [[package]]
+name = "crossbeam-deque"
+version = "0.8.6"
+source = "registry+https://github.com/rust-lang/crates.io-index"
+checksum = "9dd111b7b7f7d55b72c0a6ae361660ee5853c9af73f70c3c2ef6858b950e2e51"
+dependencies = [
+ "crossbeam-epoch",
+ "crossbeam-utils",
+]
+
+[[package]]
+name = "crossbeam-epoch"
+version = "0.9.18"
+source = "registry+https://github.com/rust-lang/crates.io-index"
+checksum = "5b82ac4a3c2ca9c3460964f020e1402edd5753411d7737aa39c3714ad1b5420e"
+dependencies = [
+ "crossbeam-utils",
+]
+
+[[package]]
+name = "crossbeam-utils"
+version = "0.8.21"
+source = "registry+https://github.com/rust-lang/crates.io-index"
+checksum = "d0a5c400df2834b80a4c3327b3aad3a4c4cd4de0629063962b03235697506a28"
+
+[[package]]
+name = "either"
+version = "1.13.0"
+source = "registry+https://github.com/rust-lang/crates.io-index"
+checksum = "60b1af1c220855b6ceac025d3f6ecdd2b7c4894bfe9cd9bda4fbb4bc7c0d4cf0"
+
+[[package]]
 name = "matrixmultiply"
 version = "0.3.9"
 source = "registry+https://github.com/rust-lang/crates.io-index"
@@ -119,6 +150,7 @@ name = "physics"
 version = "0.1.0"
 dependencies = [
  "nalgebra",
+ "rayon",
 ]
 
 [[package]]
@@ -146,6 +178,26 @@ source = "registry+https://github.com/rust-lang/crates.io-index"
 checksum = "60a357793950651c4ed0f3f52338f53b2f809f32d83a07f72909fa13e4c6c1e3"
 
 [[package]]
+name = "rayon"
+version = "1.10.0"
+source = "registry+https://github.com/rust-lang/crates.io-index"
+checksum = "b418a60154510ca1a002a752ca9714984e21e4241e804d32555251faf8b78ffa"
+dependencies = [
+ "either",
+ "rayon-core",
+]
+
+[[package]]
+name = "rayon-core"
+version = "1.12.1"
+source = "registry+https://github.com/rust-lang/crates.io-index"
+checksum = "1465873a3dfdaa8ae7cb14b4383657caab0b3e8a0aa9ae8e04b044854c8dfce2"
+dependencies = [
+ "crossbeam-deque",
+ "crossbeam-utils",
+]
+
+[[package]]
 name = "safe_arch"
 version = "0.7.2"
 source = "registry+https://github.com/rust-lang/crates.io-index"
diff --git a/physics/Cargo.toml b/physics/Cargo.toml
index e5db63e..08749b8 100644
--- a/physics/Cargo.toml
+++ b/physics/Cargo.toml
@@ -5,3 +5,4 @@ edition = "2021"
 
 [dependencies]
 nalgebra = "0.33.2"
+rayon = "1.10.0"
diff --git a/physics/README.md b/physics/README.md
index 59d5c16..bf14ff7 100644
--- a/physics/README.md
+++ b/physics/README.md
@@ -2,3 +2,53 @@
 ## Constrained dynamics
  - [A lot of pictures to get general idea and familiarize, but not very understandable](https://danielchappuis.ch/download/ConstraintsDerivationRigidBody3D.pdf)
  - [More text and math but very well written and understandable](https://graphics.pixar.com/pbm2001/pdf/notesf.pdf)
+
+
+ ## TODO:
+ - Camera perspective rendering:
+     - Subspace intersection
+     - Instead of orthogonal projection, intersect camera plane with the ray (S2 x S1 interesection)
+     - Scale radius:
+         - Take a right-most point (particle.pos + camera.right * true_radius)
+         - Project this point
+         - projected(radius) = projected(particle.pos) - projected(right_most_point)
+     - More generic `fn draw_sphere(radius, world_pos)`
+
+ - Implement vector drawing for debug:
+    - Line + small dot at the end
+    - Display particle forces
+    - Display particle velocities
+    - Display jacobians for each constraint (render constraint jacobian at each related particle)
+    - (?) Toggleable gradient for each constraint:
+        - Artificially take C(q) with some grid
+        - Grid should be on a plane, need an ability to control this plane as well
+
+ - (?) Refactor: subspace distance constraint:
+    - beam = distance to point
+    - slider = distance to a line
+    - But constraint has to act on 1 particle and 1 subspace, I will need dynamic subspaces (from point)
+    - Hold this for now
+
+
+### Better scene design
+What defines a scene:
+ - List of particles - points with mass that are affected by gravity
+ - List of global forces (gravity, drag, etc.)
+ - List of other renderable objects: beams, springs, lines, planes (each object may depend on some particle position):
+    - Each object may depend on particle position or some static point (line passing through a particle, spring between particle and static point)
+    - Each objects knows how to compute its subspace (point/line/plane)
+    - Beams automatically add constraints to particles
+    - Springs automatically register extra forces between particles
+    - Collision planes setup necessary routines
+
+ - List of additional constraints between particles and subspaces (e.g particle tied to a line)
+
+ <!-- - List of subspaces (special points, lines, planes etc.) - used as connectors for constraints -->
+ <!-- - (?) List of dynamic subspaces based on particle positions (i.e a plane that passes through a particle) -->
+ - Camera spec (lens etc.), position and orientation - defines how the scene is projected on the picture plane
+
+What the we should see on the screen:
+ - Particles (rendered as spheres with some small radius)
+ - Subspaces - points, lines, planes(?)
+ - Some constraints (beams represented as lines)
+ - Some forces (like springs)
diff --git a/physics/plots/plot_errors.py b/physics/plots/plot_errors.py
new file mode 100644
index 0000000..2632489
--- /dev/null
+++ b/physics/plots/plot_errors.py
@@ -0,0 +1,25 @@
+import sys
+import matplotlib.pyplot as plt
+import numpy as np
+
+# Read the errors from the file
+
+fig = plt.figure(figsize=(10, 6))
+ax = fig.add_subplot()
+
+for filename in sys.argv[1:]:
+    print(filename)
+    try:
+        data = np.loadtxt(filename)
+        ax.plot(range(0, len(data[0])), data[0], data[1], label=f"{filename}")
+    except Exception as e:
+        print(f"Error loading {filename}: {e}")
+    continue
+
+
+ax.set_xlabel("Time")
+ax.set_ylabel("Y")
+# ax.set_zlabel("Z")
+ax.legend()
+ax.grid(True)
+plt.show()
diff --git a/physics/src/algebra/distance_field.rs b/physics/src/algebra/distance_field.rs
new file mode 100644
index 0000000..3b511f1
--- /dev/null
+++ b/physics/src/algebra/distance_field.rs
@@ -0,0 +1,11 @@
+use super::{Point, Scalar};
+
+pub trait DistanceField {
+    fn distance(&self, point: Point) -> Scalar;
+}
+
+impl DistanceField for Point {
+    fn distance(&self, point: Point) -> Scalar {
+        (self - point).norm()
+    }
+}
diff --git a/physics/src/algebra/mod.rs b/physics/src/algebra/mod.rs
index 47efba8..116d44d 100644
--- a/physics/src/algebra/mod.rs
+++ b/physics/src/algebra/mod.rs
@@ -1,9 +1,10 @@
 use nalgebra::{Point as PointBase, SVector};
 
-pub const N: usize = 2;
+pub const N: usize = 3;
 pub type Scalar = f64;
 
 pub type Vector = SVector<Scalar, N>;
 pub type Point = PointBase<Scalar, N>;
 
 pub mod subspace;
+pub mod distance_field;
diff --git a/physics/src/algebra/subspace.rs b/physics/src/algebra/subspace.rs
index 9fb944e..3d36b52 100644
--- a/physics/src/algebra/subspace.rs
+++ b/physics/src/algebra/subspace.rs
@@ -1,6 +1,6 @@
 use nalgebra::SMatrix;
 
-use super::{Scalar, N, Point, Vector};
+use super::{distance_field::DistanceField, Point, Scalar, Vector, N};
 
 
 type ProjectionMatrix = SMatrix<Scalar, N, N>;
@@ -36,7 +36,10 @@ impl<const DIM: usize> Subspace<DIM> {
         self.projection_matrix * (point - self.point.coords) + self.point.coords
     }
 
-    pub fn distance(&self, point: Point) -> Scalar {
+}
+
+impl<const DIM: usize> DistanceField for Subspace<DIM> {
+    fn distance(&self, point: Point) -> Scalar {
         let projected = self.project_point(point);
         (projected - point).norm()
     }
@@ -44,13 +47,13 @@ impl<const DIM: usize> Subspace<DIM> {
 
 #[cfg(test)]
 mod tests {
-    use crate::algebra::{subspace::Line, Point, Vector};
+    use crate::algebra::{subspace::Line, Point, Vector, distance_field::DistanceField};
 
     #[test]
     fn test_projection_onto_line() {
-        let line = Line::new(Point::new(1.0, 0.0), [Vector::new(3.0, 1.0)]);
-        let point = Point::new(3.0, 4.0);
-
+        let line = Line::new(Point::new(1.0, 0.0, 0.0), [Vector::new(3.0, 1.0, 0.0)]);
+        let point = Point::new(3.0, 4.0, 0.0);
+        
         let matrix = line.projection_matrix;
         {
             let diff = matrix * matrix - matrix;
@@ -63,8 +66,21 @@ mod tests {
 
         {
             let projected = line.project_point(point);
-            let diff = projected - Point::new(4.0, 1.0);
+            let diff = projected - Point::new(4.0, 1.0, 0.0);
             assert!(diff.norm() < 0.001, "Point projected incorrectly");
         }
     }
+
+    #[test]
+    fn test_distance() {
+        let origin = Point::new(0.0, 0.0, 0.0);
+        let line = Line::new(origin, [Vector::x()]);
+        
+        let dq = 0.001;
+        let dx = line.distance(origin + Vector::x() * dq);
+        let dy = line.distance(origin + Vector::y() * dq);
+        let dz = line.distance(origin + Vector::z() * dq);
+        assert_eq!(dx, 0.0);
+        assert_eq!(dy, dz);
+    }
 }
diff --git a/physics/src/constraint/distance.rs b/physics/src/constraint/distance.rs
new file mode 100644
index 0000000..abdbdeb
--- /dev/null
+++ b/physics/src/constraint/distance.rs
@@ -0,0 +1,50 @@
+use nalgebra::{DVector, RowDVector};
+
+use crate::algebra::distance_field::DistanceField;
+use crate::algebra::subspace::{Line, Subspace};
+use crate::algebra::{Point, Scalar, N};
+use crate::particle_system::ParticleSystem;
+
+use super::Constraint;
+
+/// Keep a particle at certain distance within distance field
+pub struct DistanceConstraint<D: DistanceField> {
+    particle_id: usize,
+    distance_field: D,
+    distance: Scalar,
+}
+
+impl ParticleSystem {
+    pub fn add_distance_constraint<const DIM: usize>(&mut self, particle_id: usize, subspace: Subspace<DIM>, distance: Scalar) {
+        let particle = &self.particles[particle_id];
+
+        self.constraints.push(Box::new(DistanceConstraint {
+            particle_id,
+            distance,
+            distance_field: subspace,
+        }));
+    }
+}
+
+impl<D: DistanceField> Constraint for DistanceConstraint<D> {
+    fn get_particles(&self) -> Vec<usize> {
+        vec![self.particle_id]
+    }
+
+    fn c(&self, q: &DVector<Scalar>) -> Scalar {
+        let position = q.fixed_rows::<N>(self.particle_id * N);
+
+        self.distance_field.distance(Point::from_coordinates(position.into())) - self.distance
+    }
+
+    fn jacobian(&self, q: &DVector<Scalar>) -> RowDVector<Scalar> {
+        let point = Point::from_coordinates(q.fixed_rows::<N>(self.particle_id * N).into());
+        let normal = self.distance_field.project_point(point) - point;
+
+        let mut result = RowDVector::zeros(q.len());
+        for i in 0..N {
+            result[self.particle_id * N + i] = normal[i];
+        }
+        result
+    }
+}
diff --git a/physics/src/constraint/mod.rs b/physics/src/constraint/mod.rs
index ed9e603..3f76d33 100644
--- a/physics/src/constraint/mod.rs
+++ b/physics/src/constraint/mod.rs
@@ -4,6 +4,7 @@ use crate::algebra::{Scalar, N};
 use crate::particle_system::ParticleSystem;
 pub mod beam;
 pub mod slider;
+// pub mod distance;
 
 pub const SPRING_CONSTANT: Scalar = 16.0;
 pub const DAMPING_CONSTANT: Scalar = 4.0;
diff --git a/physics/src/constraint/slider.rs b/physics/src/constraint/slider.rs
index be8e261..a50a840 100644
--- a/physics/src/constraint/slider.rs
+++ b/physics/src/constraint/slider.rs
@@ -1,7 +1,7 @@
 use nalgebra::{DVector, RowDVector};
 
 use crate::{
-    algebra::{subspace::Line, Point, Scalar, Vector, N},
+    algebra::{distance_field::DistanceField, subspace::Line, Point, Scalar, Vector, N},
     particle_system::ParticleSystem,
 };
 
diff --git a/physics/src/renderer/mod.rs b/physics/src/renderer/mod.rs
index 63894b3..72b922d 100644
--- a/physics/src/renderer/mod.rs
+++ b/physics/src/renderer/mod.rs
@@ -45,21 +45,23 @@ impl Camera {
 
 #[cfg(test)]
 mod tests {
-    use crate::algebra::{Point, Vector};
+    use nalgebra::Point2;
+
+    use crate::algebra::{Point, Scalar, Vector};
 
     use super::Camera;
 
     #[test]
     fn test_projection() {
         let camera = Camera::new(
-            Point::new(1.0, 0.0),
-            Vector::new(1.0, 2.0),
-            Vector::new(2.0, -1.0),
+            Point::new(1.0, 0.0, 0.0),
+            Vector::new(1.0, 2.0, 0.0),
+            Vector::new(2.0, -1.0, 0.0),
         );
 
-        let point = Point::new(3.0, 1.0);
+        let point = Point::new(3.0, 1.0, 0.0);
 
-        let diff = camera.world_to_screen_space * point - Point::new(1.0, 1.0);
+        let diff = camera.world_to_screen_space * point - Point2::<Scalar>::new(1.0, 1.0);
         assert!(
             diff.norm() < 0.001,
             "Camera translated point into screen_space incorrectly"
diff --git a/physics/src/solver/mod.rs b/physics/src/solver/mod.rs
index 527bb06..50fe9a7 100644
--- a/physics/src/solver/mod.rs
+++ b/physics/src/solver/mod.rs
@@ -84,7 +84,7 @@ mod tests {
     #[test]
     fn test_collect_phase_space() {
         let system = ParticleSystem {
-            particles: vec![Particle::new(Point::new(2.0, 3.0), 1.0)],
+            particles: vec![Particle::new(Point::new(2.0, 3.0, 0.0), 1.0)],
             constraints: vec![],
             forces: vec![],
             t: 0.0,
@@ -100,12 +100,12 @@ mod tests {
     #[test]
     fn test_scatter_phase_space() {
         let mut phase_space = PhaseSpace::new(2);
-        phase_space.set_particle(1, Point::new(5.0, 7.0), Vector::x());
+        phase_space.set_particle(1, Point::new(5.0, 7.0, 0.0), Vector::x());
 
         let mut system = ParticleSystem {
             particles: vec![
-                Particle::new(Point::new(0.0, 0.0), 1.0),
-                Particle::new(Point::new(0.0, 0.0), 1.0),
+                Particle::new(Point::new(0.0, 0.0, 0.0), 1.0),
+                Particle::new(Point::new(0.0, 0.0, 0.0), 1.0),
             ],
             constraints: vec![],
             forces: vec![],