Add a spline-following camera transform

jdahlstrom · jdahlstrom · commit 366efd8c46a2 · 2025-01-02T01:35:31.000+02:00
diff --git a/core/src/math/mat.rs b/core/src/math/mat.rs
@@ -42,7 +42,7 @@ pub trait Compose<Inner: LinearMap>: LinearMap<Source = Inner::Dest> {
 
 /// A change of basis in real vector space of dimension `DIM`.
 #[derive(Copy, Clone, Default, Eq, PartialEq)]
-pub struct RealToReal<const DIM: usize, SrcBasis = (), DstBasis = ()>(
+pub struct RealToReal<const DIM: usize, SrcBasis = (), DstBasis = SrcBasis>(
     Pd<(SrcBasis, DstBasis)>,
 );
 
@@ -555,6 +555,12 @@ pub fn orient_z(new_z: Vec3, x: Vec3) -> Mat4x4<RealToReal<3>> {
     orient(new_z.cross(&x).normalize(), new_z)
 }
 
+pub fn orient_z_y(new_z: Vec3, y: Vec3) -> Mat4x4<RealToReal<3>> {
+    let new_x = y.cross(&new_z);
+    let new_y = new_z.cross(&new_x);
+    Mat4x4::from_basis(new_x, new_y, new_z)
+}
+
 /// Constructs a change-of-basis matrix given y and z basis vectors.
 ///
 /// The third basis vector is the cross product of `new_y` and `new_z`.
@@ -565,7 +571,7 @@ pub fn orient_z(new_z: Vec3, x: Vec3) -> Mat4x4<RealToReal<3>> {
 /// If `new_y` is approximately parallel to `new_z` and the basis would
 /// be degenerate.
 #[cfg(feature = "fp")]
-fn orient(new_y: Vec3, new_z: Vec3) -> Mat4x4<RealToReal<3>> {
+pub fn orient(new_y: Vec3, new_z: Vec3) -> Mat4x4<RealToReal<3>> {
     let new_x = new_y.cross(&new_z);
     assert!(
         !new_x.len_sqr().approx_eq(&0.0),
diff --git a/core/src/math/spline.rs b/core/src/math/spline.rs
@@ -112,6 +112,20 @@ where
         co2.mul(t).add(&co1).mul(t).add(&co0).mul(3.0)
     }
 
+    /// Returns the acceleration vector of `self`
+    pub fn acceleration(&self, t: f32) -> T::Diff {
+        let [p0, p1, p2, p3] = &self.0;
+        let t = t.clamp(0.0, 1.0);
+
+        //   6 (3 (p1 - p2) + (p3 - p0)) * t
+        // + 6 ((p0 - p1 + p2 - p1)
+
+        let co1: T::Diff = p1.sub(p2).mul(3.0).add(&p3.sub(p0)).mul(3.0);
+        let co0: T::Diff = p0.sub(p1).add(&p2.sub(p1));
+
+        co1.mul(t).add(&co0).mul(6.0)
+    }
+
     /// Returns the coefficients used to evaluate the spline.
     ///
     /// These are constant as long as the control points do not change,
@@ -224,6 +238,11 @@ where
         CubicBezier(seg).tangent(t)
     }
 
+    pub fn acceleration(&self, t: f32) -> T::Diff {
+        let (t, seg) = self.segment(t);
+        CubicBezier(seg).acceleration(t)
+    }
+
     fn segment(&self, t: f32) -> (f32, [T; 4]) {
         let segs = ((self.0.len() - 1) / 3) as f32;
         // TODO use floor and make the code cleaner
diff --git a/core/src/render/cam.rs b/core/src/render/cam.rs
@@ -4,11 +4,12 @@ use core::ops::Range;
 
 use crate::geom::{Tri, Vertex};
 use crate::math::{
-    mat::RealToReal, orthographic, perspective, pt2, viewport, Lerp, Mat4x4,
-    Point3, SphericalVec, Vary,
+    mat::RealToReal, orient_y, orthographic, perspective, pt2, scale3,
+    viewport, ApproxEq, BezierSpline, Lerp, Mat4x4, Point3, SphericalVec, Vary,
 };
 use crate::util::{rect::Rect, Dims};
 
+use crate::math::mat::orient_z_y;
 #[cfg(feature = "fp")]
 use crate::math::{
     orient_z, pt3, rotate_x, rotate_y, spherical, translate, turns, Angle, Vec3,
@@ -69,16 +70,21 @@ pub struct Orbit {
     pub dir: SphericalVec,
 }
 
+pub struct Spline {
+    pub spline: BezierSpline<Point3<World>>,
+    pub t: f32,
+}
+
 //
 // Inherent impls
 //
 
 impl Camera<()> {
     /// Creates a camera with the given resolution.
-    pub fn new(dims: Dims) -> Self {
+    pub fn new(dims @ (w, h): Dims) -> Self {
         Self {
             dims,
-            viewport: viewport(pt2(0, 0)..pt2(dims.0, dims.1)),
+            viewport: viewport(pt2(0, 0)..pt2(w, h)),
             ..Default::default()
         }
     }
@@ -297,6 +303,18 @@ impl Transform for Orbit {
     }
 }
 
+impl Transform for Spline {
+    fn world_to_view(&self) -> Mat4x4<WorldToView> {
+        let pos = self.spline.eval(self.t);
+        let fwd = self.spline.tangent(self.t).normalize();
+
+        // Inverse of the view-to-world: (OT)^-1 = T^-1 O^-1
+        translate(-pos.to_vec().to())
+            .then(&orient_z_y(fwd.to(), Vec3::Y).transpose())
+            .to()
+    }
+}
+
 impl Transform for Mat4x4<WorldToView> {
     fn world_to_view(&self) -> Mat4x4<WorldToView> {
         *self
diff --git a/demos/src/bin/bezier.rs b/demos/src/bin/bezier.rs
@@ -41,7 +41,7 @@ fn main() {
     let vel = VectorsOnUnitDisk;
 
     let mut pos_vels: Vec<(Point2, Vec2)> =
-        (pos, vel).samples(rng).take(32).collect();
+        (pos, vel).samples(rng).take(8).collect();
 
     win.run(|Frame { dt, buf, .. }| {
         let rays: Vec<Ray<_, _>> = pos_vels
@@ -53,6 +53,19 @@ fn main() {
         // Stop once error is less than one pixel
         let apx = b.approximate(|err| err.len_sqr() < 1.0);
 
+        for t in 0.0.vary_to(1.0, 10) {
+            let o = b.eval(t);
+            let d = b.tangent(t) * 0.005;
+            let n = b.acceleration(t) * 0.0001;
+
+            for pt in line([o, o + 50.0 * d]) {
+                buf.color_buf[pt] = 0xFF_00_FF;
+            }
+            for pt in line([o, o + 50.0 * n]) {
+                buf.color_buf[pt] = 0xFF_FF_00;
+            }
+        }
+
         for seg in apx.windows(2) {
             for pt in line([seg[0], seg[1]]) {
                 // The curve can't go out of bounds if the control points don't
diff --git a/demos/src/bin/crates.rs b/demos/src/bin/crates.rs
@@ -1,14 +1,17 @@
 use core::ops::ControlFlow::*;
+use re::geom::Ray;
 
 use re::prelude::*;
 
 use re::math::color::gray;
+use re::math::mat::{orient, orient_z_y, RealToReal};
+use re::render::cam::Spline;
 use re::render::{
     cam::FirstPerson, shader::Shader, Batch, Camera, ModelToProj,
 };
 
 use re_front::sdl2::Window;
-use re_geom::solids::Box;
+use re_geom::solids::{Box, Cone};
 
 fn main() {
     let mut win = Window::builder()
@@ -32,55 +35,101 @@ fn main() {
         },
     );
 
+    let spline: BezierSpline<Point3<World>> = BezierSpline::from_rays([
+        Ray(pt3(-12.0, 0.0, 0.0), Vec3::Z.to() * 50.0),
+        Ray(pt3(0.0, -6.0, 10.0), Vec3::X.to() * 50.0),
+        Ray(pt3(12.0, -16.0, 0.0), -Vec3::Z.to() * 50.0),
+        Ray(pt3(10.0, -3.0, -10.0), -Vec3::X.to() * 50.0),
+        Ray(pt3(-12.0, 0.0, 0.0), Vec3::Z.to() * 50.0),
+    ]);
+
     let (w, h) = win.dims;
     let mut cam = Camera::new(win.dims)
-        .transform(FirstPerson::default())
-        .viewport((10..w - 10, 10..h - 10))
+        /*.transform(FirstPerson {
+            pos: pt3(-20.0, -2.0, 0.0),
+            heading: Default::default(),
+        })*/
+        .transform(Spline { spline: spline.clone(), t: 0.0 })
         .perspective(1.0, 0.1..1000.0);
 
     let floor = floor();
     let crat = Box::cube(2.0).build();
 
     win.run(|frame| {
-        // Camera
-
-        let mut cam_vel = Vec3::zero();
-
         let ep = &frame.win.ev_pump;
 
-        for key in ep.keyboard_state().pressed_scancodes() {
-            use sdl2::keyboard::Scancode as Sc;
-            match key {
-                Sc::W => cam_vel[2] += 4.0,
-                Sc::S => cam_vel[2] -= 2.0,
-                Sc::D => cam_vel[0] += 3.0,
-                Sc::A => cam_vel[0] -= 3.0,
-                _ => {}
-            }
-        }
-
-        let ms = ep.relative_mouse_state();
-        let d_az = turns(ms.x() as f32) * -0.001;
-        let d_alt = turns(ms.y() as f32) * 0.001;
-
-        cam.transform.rotate(d_az, d_alt);
-        cam.transform
-            .translate(cam_vel.mul(frame.dt.as_secs_f32()));
+        // eprint!(
+        //     "\r pos={:.2?} sdir={:.2?} cdir={:.2?}",
+        //     cam.transform.pos,
+        //     cam.transform.heading,
+        //     cam.transform.heading.to_cart::<()>()
+        // );
 
-        let flip = scale3(1.0, -1.0, -1.0).to();
+        // Camera
+        // let mut cam_vel = Vec3::zero();
+        //
+        // for key in ep.keyboard_state().pressed_scancodes() {
+        //     use sdl2::keyboard::Scancode as Sc;
+        //     match key {
+        //         Sc::W => cam_vel[2] += 4.0,
+        //         Sc::S => cam_vel[2] -= 2.0,
+        //         Sc::D => cam_vel[0] += 3.0,
+        //         Sc::A => cam_vel[0] -= 3.0,
+        //         _ => {}
+        //     }
+        // }
+        //
+        // let ms = ep.relative_mouse_state();
+        //
+        // let d_az = turns(ms.x() as f32) * -0.001;
+        //
+        // let d_alt = turns(ms.y() as f32) * 0.001;
+        //
+        // cam.transform.rotate(d_az, d_alt);
+        // cam.transform
+        //     .translate(cam_vel.mul(frame.dt.as_secs_f32()));
+
+        cam.transform.t = (frame.t.as_secs_f32() * 0.1) % 1.0;
 
         // Render
 
-        let world_to_project = flip.then(&cam.world_to_project());
+        let world_to_project = &cam.world_to_project();
 
         let batch = Batch::new()
             .viewport(cam.viewport)
             .context(&frame.ctx);
 
+        let arrow = Cone {
+            sectors: 20,
+            segments: 1,
+            capped: true,
+            base_radius: 0.4,
+            apex_radius: 0.0,
+        }
+        .build();
+
+        let t = (frame.t.as_secs_f32() * 0.1 + 0.1) % 1.0;
+        let pos = spline.eval(t);
+
+        let fwd = spline.tangent(t).normalize();
+
+        let arrow_tf = orient_y(fwd.to(), Vec3::Y)
+            .then(&translate(pos.to_vec().to()))
+            .to()
+            .then(&world_to_project);
+
+        batch
+            .clone()
+            .mesh(&arrow)
+            .uniform(&arrow_tf)
+            .shader(crate_shader)
+            .target(&mut frame.buf)
+            .render();
+
         batch
             .clone()
             .mesh(&floor)
-            .uniform(&world_to_project)
+            .uniform(&world_to_project.to())
             .shader(floor_shader)
             .target(&mut frame.buf)
             .render();
@@ -113,19 +162,19 @@ fn main() {
 fn floor() -> Mesh<Color3f> {
     let mut bld = Mesh::builder();
 
-    let size = 50;
-    for j in -size..=size {
-        for i in -size..=size {
+    let (min, max) = (-40, 40);
+    for j in min..=max {
+        for i in min..=max {
             let even_odd = ((i & 1) ^ (j & 1)) == 1;
 
-            let pos = pt3(i as f32, -1.0, j as f32);
+            let pos = pt3(i as f32, 1.0, j as f32);
             let col = if even_odd { gray(0.2) } else { gray(0.9) };
             bld.push_vert(pos, col);
 
-            if j > -size && i > -size {
-                let w = size * 2 + 1;
-                let j = size + j;
-                let i = size + i;
+            if j > min && i > min {
+                let w = (max - min) + 1;
+                let j = -min + j;
+                let i = -min + i;
                 let [a, b, c, d] = [
                     w * (j - 1) + (i - 1),
                     w * (j - 1) + i,
@@ -135,11 +184,11 @@ fn floor() -> Mesh<Color3f> {
                 .map(|i| i as usize);
 
                 if even_odd {
-                    bld.push_face(a, c, d);
-                    bld.push_face(a, d, b);
+                    bld.push_face(a, d, c);
+                    bld.push_face(a, b, d);
                 } else {
-                    bld.push_face(b, c, d);
-                    bld.push_face(b, a, c)
+                    bld.push_face(b, d, c);
+                    bld.push_face(b, c, a)
                 }
             }
         }
