radar-gi/src/graphics/transforms/trackball.rs

use crate::components::{CodeType, Program, Snippet};
use crate::errors::Result;
use crate::graphics::AttaWithProgram;

use super::Transform;

use glow::HasContext;
use nalgebra::{Matrix4, Quaternion, Translation3, Unit, UnitQuaternion, Vector3};

#[derive(Debug, Clone)]
pub struct TrackballModel {
    rotation: UnitQuaternion<f32>,
    count: usize,
    model: Matrix4<f32>,
    renorm_count: usize,
    trackball_size: f32,
    x: f32,
    y: f32,
    theta: f32,
    phi: f32,
}

impl TrackballModel {
    pub fn new(theta: f32, phi: f32) -> Self {
        let mut trackball = Self {
            rotation: UnitQuaternion::identity(),
            count: 0,
            model: Matrix4::identity(),
            renorm_count: 97,
            trackball_size: 0.8,
            x: 0.0,
            y: 0.0,
            theta,
            phi,
        };
        trackball.set_orientation(theta, phi);
        trackball
    }

    fn drag_to(&mut self, x: f32, y: f32, dx: f32, dy: f32) {
        let q = self.rotate(x, y, dx, dy);
        self.rotation *= q;
        self.count += 1;
        if self.count > self.renorm_count {
            self.rotation = UnitQuaternion::new_normalize(*self.rotation.clone());
            self.count = 0;
        }
        self.model = self.rotation.to_homogeneous();
    }

    fn model(&self) -> &Matrix4<f32> {
        &self.model
    }

    fn theta(&self) -> f32 {
        self.theta
    }

    fn set_theta(&mut self, theta: f32) {
        self.set_orientation(theta % 360.0, self.phi % 360.0);
    }

    fn phi(&self) -> f32 {
        self.phi
    }

    fn set_phi(&mut self, phi: f32) {
        self.set_orientation(self.theta % 360.0, phi % 360.0);
    }

    fn get_orientation(&self) -> (f32, f32) {
        let q = self.rotation.quaternion();
        let ax = (2.0 * (q.w * q.i + q.j * q.k) / (1.0 - 2.0 * (q.i * q.i + q.j * q.j))).atan()
            * 180.0
            / std::f32::consts::PI;
        let az = (2.0 * (q.w * q.k + q.i * q.j) / (1.0 - 2.0 * (q.j * q.j + q.k * q.k))).atan()
            * 180.0
            / std::f32::consts::PI;
        (-az, ax)
    }

    fn set_orientation(&mut self, theta: f32, phi: f32) {
        self.theta = theta;
        self.phi = phi;

        let angle = self.theta * (std::f32::consts::PI / 180.0);
        let sine = (0.5 * angle).sin();
        let xrot =
            UnitQuaternion::from_quaternion(Quaternion::new((0.5 * angle).cos(), sine, 0.0, 0.0));

        let angle = self.phi * (std::f32::consts::PI / 180.0);
        let sine = (0.5 * angle).sin();
        let zrot =
            UnitQuaternion::from_quaternion(Quaternion::new((0.5 * angle).cos(), 0.0, 0.0, sine));

        self.rotation = xrot * zrot;
        self.model = self.rotation.to_homogeneous();
    }

    fn project(&self, r: f32, x: f32, y: f32) -> f32 {
        let d = (x * x + y * y).sqrt();
        if d < r * 0.70710678118654752440 {
            (r * r - d * d).sqrt()
        } else {
            let t = r / 1.41421356237309504880;
            t * t / d
        }
    }

    fn rotate(&self, x: f32, y: f32, dx: f32, dy: f32) -> UnitQuaternion<f32> {
        if dx == 0.0 && dy == 0.0 {
            return UnitQuaternion::identity();
        }

        let last = Vector3::new(x, y, self.project(self.trackball_size, x, y));
        let new = Vector3::new(
            x + dx,
            y + dy,
            self.project(self.trackball_size, x + dx, y + dy),
        );

        let a = new.cross(&last);
        let d = last - new;
        let t = d.norm() / (2.0 * self.trackball_size);
        let t = t.clamp(-1.0, 1.0);

        let phi = 2.0 * t.asin();
        UnitQuaternion::from_axis_angle(&Unit::new_normalize(a), phi)
    }
}

#[derive(Debug, Clone)]
pub struct Trackball {
    snippet: Snippet,
    model: TrackballModel,
}

impl Trackball {
    pub fn new() -> Result<Self> {
        let snippets = Snippet::new(
            "trackball",
            CodeType::<&'static str>::Path("transform/trackball.glsl".into()),
            false,
            None,
        )?;

        let model = TrackballModel::new(45.0, 45.0);

        Ok(Self {
            snippet: snippets,
            model,
        })
    }

    pub fn on_mouse_drag(&mut self, x: f32, y: f32, dx: f32, dy: f32) {
        self.model.drag_to(x, y, dx, dy);
    }

    pub fn model(&self) -> &Matrix4<f32> {
        self.model.model()
    }
}

impl Transform for Trackball {
    fn snippet(&self) -> &Snippet {
        &self.snippet
    }
}

impl AttaWithProgram for Trackball {
    fn attach_with_program(&self, gl: &glow::Context, program: &Program) -> Result<()> {
        unsafe {
            let l = program.get_uniform_location(gl, "trackball_model");
            gl.uniform_matrix_4_f32_slice(l.as_ref(), false, self.model.model().as_slice());
        }

        Ok(())
    }
}

mod test {
    use super::*;

    #[test]
    fn test_trackball() {
        // let trackball = Trackball::new().unwrap();
        // println!("{}", trackball.snippet);
    }
}