slam-viewer 0.1.2

Simple wgpu based SLAM map viewer.
Documentation 
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110

//! Many of this code is from [kiss3d](https://github.com/sebcrozet/kiss3d)
//! https://github.com/sebcrozet/kiss3d/blob/master/src/camera/first_person.rs

use super::coord::CoordSystemRh;

use nalgebra::{Isometry3, Matrix4, Perspective3, Point3, Vector2, Vector3};
use num::Float;
use slam_cv::Number;

#[derive(Debug)]
pub struct Camera<N>
where
    N: Number,
{
    pub eye: Point3<N>,
    pub yaw: N,
    pub pitch: N,

    pub fovy: N,
    pub znear: N,
    pub zfar: N,

    pub coord_system: CoordSystemRh<N>,
}

impl<N> Camera<N>
where
    N: Number,
{
    /// Changes the orientation and position of the camera to look at the specified point.
    pub fn look_at(&mut self, eye: Option<Point3<N>>, at: Point3<N>) {
        let eye = eye.or_else(|| Some(self.eye)).unwrap();
        let dist = (eye - at).norm();

        let view_eye = self.coord_system.rotation_to_y_up * eye;
        let view_at = self.coord_system.rotation_to_y_up * at;
        let pitch = Float::acos((view_at.y - view_eye.y) / dist);
        let yaw = Float::atan2(view_at.z - view_eye.z, view_at.x - view_eye.x);

        self.eye = eye;
        self.yaw = yaw;
        self.pitch = pitch;
    }

    pub fn compute_view_proj(&self, aspect: N) -> Matrix4<N> {
        let proj = Perspective3::new(aspect, self.fovy, self.znear, self.zfar);

        let view = self.view_transform().to_homogeneous();

        proj.as_matrix() * view
    }

    pub fn handle_left_button_displacement(&mut self, dpos: Vector2<N>) {
        self.yaw += dpos.x;
        self.pitch += dpos.y;

        self.update_restrictions();
    }

    pub fn handle_right_button_displacement(&mut self, dpos: Vector2<N>) {
        let at = self.at();
        let dir = (at - self.eye).normalize();
        let tangent = self.coord_system.up_axis.cross(&dir).normalize();
        let bitangent = dir.cross(&tangent);

        self.eye += tangent * dpos.x + bitangent * dpos.y;
    }

    pub fn handle_scroll(&mut self, yoff: N) {
        let front = self.observer_frame() * Vector3::z();

        self.eye += front * yoff;
    }

    pub fn handle_keyboard(&mut self, dpos: Vector2<N>) {
        self.eye += Vector3::new(dpos.x, dpos.y, N::zero());
    }

    /// The point the camera is looking at.
    fn at(&self) -> Point3<N> {
        let view_eye = self.coord_system.rotation_to_y_up * self.eye;
        let ax = view_eye.x + Float::cos(self.yaw) * Float::sin(self.pitch);
        let ay = view_eye.y + Float::cos(self.pitch);
        let az = view_eye.z + Float::sin(self.yaw) * Float::sin(self.pitch);
        self.coord_system.rotation_to_y_up.inverse() * Point3::new(ax, ay, az)
    }

    /// The camera view transformation (i-e transformation without projection).
    fn view_transform(&self) -> Isometry3<N> {
        Isometry3::look_at_rh(&self.eye, &self.at(), &self.coord_system.up_axis)
    }

    /// The camera observer local frame.
    fn observer_frame(&self) -> Isometry3<N> {
        Isometry3::face_towards(&self.eye, &self.at(), &self.coord_system.up_axis)
    }

    fn update_restrictions(&mut self) {
        let thr = N::from(0.01).unwrap();

        if self.pitch <= thr {
            self.pitch = thr;
        }

        let pi = N::from(std::f32::consts::PI).unwrap();
        if self.pitch > pi - thr {
            self.pitch = pi - thr;
        }
    }
}