transform_gizmo/
math.rs

1pub use emath::{Pos2, Rect, Vec2};
2pub use glam::{DMat3, DMat4, DQuat, DVec2, DVec3, DVec4, Mat4, Quat, Vec3, Vec4Swizzles};
3
4#[derive(Clone, Copy, Debug, PartialEq, PartialOrd)]
5pub struct Transform {
6    pub scale: mint::Vector3<f64>,
7    pub rotation: mint::Quaternion<f64>,
8    pub translation: mint::Vector3<f64>,
9}
10
11impl Default for Transform {
12    fn default() -> Self {
13        Self {
14            scale: DVec3::ONE.into(),
15            rotation: DQuat::IDENTITY.into(),
16            translation: DVec3::ZERO.into(),
17        }
18    }
19}
20
21impl Transform {
22    pub fn from_scale_rotation_translation(
23        scale: impl Into<mint::Vector3<f64>>,
24        rotation: impl Into<mint::Quaternion<f64>>,
25        translation: impl Into<mint::Vector3<f64>>,
26    ) -> Self {
27        Self {
28            scale: scale.into(),
29            rotation: rotation.into(),
30            translation: translation.into(),
31        }
32    }
33}
34
35/// Creates a matrix that represents rotation between two 3d vectors
36///
37/// Credit: <https://www.iquilezles.org/www/articles/noacos/noacos.htm>
38pub(crate) fn rotation_align(from: DVec3, to: DVec3) -> DMat3 {
39    let v = from.cross(to);
40    let c = from.dot(to);
41    let k = 1.0 / (1.0 + c);
42
43    DMat3::from_cols_array(&[
44        v.x * v.x * k + c,
45        v.x * v.y * k + v.z,
46        v.x * v.z * k - v.y,
47        v.y * v.x * k - v.z,
48        v.y * v.y * k + c,
49        v.y * v.z * k + v.x,
50        v.z * v.x * k + v.y,
51        v.z * v.y * k - v.x,
52        v.z * v.z * k + c,
53    ])
54}
55
56/// Finds points on two rays that are closest to each other.
57/// This can be used to determine the shortest distance between those two rays.
58///
59/// Credit: Practical Geometry Algorithms by Daniel Sunday: <http://geomalgorithms.com/code.html>
60pub(crate) fn ray_to_ray(a1: DVec3, adir: DVec3, b1: DVec3, bdir: DVec3) -> (f64, f64) {
61    let b = adir.dot(bdir);
62    let w = a1 - b1;
63    let d = adir.dot(w);
64    let e = bdir.dot(w);
65    let dot = 1.0 - b * b;
66    let ta;
67    let tb;
68
69    if dot < 1e-8 {
70        ta = 0.0;
71        tb = e;
72    } else {
73        ta = (b * e - d) / dot;
74        tb = (e - b * d) / dot;
75    }
76
77    (ta, tb)
78}
79
80/// Finds points on two segments that are closest to each other.
81/// This can be used to determine the shortest distance between those two segments.
82///
83/// Credit: Practical Geometry Algorithms by Daniel Sunday: <http://geomalgorithms.com/code.html>
84pub(crate) fn segment_to_segment(a1: DVec3, a2: DVec3, b1: DVec3, b2: DVec3) -> (f64, f64) {
85    let da = a2 - a1;
86    let db = b2 - b1;
87    let la = da.length_squared();
88    let lb = db.length_squared();
89    let dd = da.dot(db);
90    let d1 = a1 - b1;
91    let d = da.dot(d1);
92    let e = db.dot(d1);
93    let n = la * lb - dd * dd;
94
95    let mut sn;
96    let mut tn;
97    let mut sd = n;
98    let mut td = n;
99
100    if n < 1e-8 {
101        sn = 0.0;
102        sd = 1.0;
103        tn = e;
104        td = lb;
105    } else {
106        sn = dd * e - lb * d;
107        tn = la * e - dd * d;
108        if sn < 0.0 {
109            sn = 0.0;
110            tn = e;
111            td = lb;
112        } else if sn > sd {
113            sn = sd;
114            tn = e + dd;
115            td = lb;
116        }
117    }
118
119    if tn < 0.0 {
120        tn = 0.0;
121        if -d < 0.0 {
122            sn = 0.0;
123        } else if -d > la {
124            sn = sd;
125        } else {
126            sn = -d;
127            sd = la;
128        }
129    } else if tn > td {
130        tn = td;
131        if (-d + dd) < 0.0 {
132            sn = 0.0;
133        } else if (-d + dd) > la {
134            sn = sd;
135        } else {
136            sn = -d + dd;
137            sd = la;
138        }
139    }
140
141    let ta = if sn.abs() < 1e-8 { 0.0 } else { sn / sd };
142    let tb = if tn.abs() < 1e-8 { 0.0 } else { tn / td };
143
144    (ta, tb)
145}
146
147/// Finds the intersection point of a ray and a plane
148pub(crate) fn intersect_plane(
149    plane_normal: DVec3,
150    plane_origin: DVec3,
151    ray_origin: DVec3,
152    ray_dir: DVec3,
153    t: &mut f64,
154) -> bool {
155    let denom = plane_normal.dot(ray_dir);
156
157    if denom.abs() < 10e-8 {
158        false
159    } else {
160        *t = (plane_origin - ray_origin).dot(plane_normal) / denom;
161        *t >= 0.0
162    }
163}
164
165/// Finds the intersection point of a ray and a plane
166/// and distance from the intersection to the plane origin
167pub(crate) fn ray_to_plane_origin(
168    disc_normal: DVec3,
169    disc_origin: DVec3,
170    ray_origin: DVec3,
171    ray_dir: DVec3,
172) -> (f64, f64) {
173    let mut t = 0.0;
174    if intersect_plane(disc_normal, disc_origin, ray_origin, ray_dir, &mut t) {
175        let p = ray_origin + ray_dir * t;
176        let v = p - disc_origin;
177        let d2 = v.dot(v);
178        (t, f64::sqrt(d2))
179    } else {
180        (t, f64::MAX)
181    }
182}
183
184/// Rounds given value to the nearest interval
185pub(crate) fn round_to_interval(val: f64, interval: f64) -> f64 {
186    (val / interval).round() * interval
187}
188
189/// Calculates 2d screen coordinates from 3d world coordinates
190pub(crate) fn world_to_screen(viewport: Rect, mvp: DMat4, pos: DVec3) -> Option<Pos2> {
191    let mut pos = mvp * DVec4::from((pos, 1.0));
192
193    if pos.w < 1e-10 {
194        return None;
195    }
196
197    pos /= pos.w;
198    pos.y *= -1.0;
199
200    let center = viewport.center();
201
202    Some(Pos2::new(
203        (center.x as f64 + pos.x * viewport.width() as f64 / 2.0) as f32,
204        (center.y as f64 + pos.y * viewport.height() as f64 / 2.0) as f32,
205    ))
206}
207
208/// Calculates 3d world coordinates from 2d screen coordinates
209pub(crate) fn screen_to_world(viewport: Rect, mat: DMat4, pos: Pos2, z: f64) -> DVec3 {
210    let x = (((pos.x - viewport.min.x) / viewport.width()) * 2.0 - 1.0) as f64;
211    let y = (((pos.y - viewport.min.y) / viewport.height()) * 2.0 - 1.0) as f64;
212
213    let mut world_pos = mat * DVec4::new(x, -y, z, 1.0);
214
215    // w is zero when far plane is set to infinity
216    if world_pos.w.abs() < 1e-7 {
217        world_pos.w = 1e-7;
218    }
219
220    world_pos /= world_pos.w;
221
222    world_pos.xyz()
223}
transform_gizmo/math.rs

transform_gizmo/
math.rs
