rs_math3d/
primitives.rs

1// Copyright 2020-Present (c) Raja Lehtihet & Wael El Oraiby
2//
3// Redistribution and use in source and binary forms, with or without
4// modification, are permitted provided that the following conditions are met:
5//
6// 1. Redistributions of source code must retain the above copyright notice,
7// this list of conditions and the following disclaimer.
8//
9// 2. Redistributions in binary form must reproduce the above copyright notice,
10// this list of conditions and the following disclaimer in the documentation
11// and/or other materials provided with the distribution.
12//
13// 3. Neither the name of the copyright holder nor the names of its contributors
14// may be used to endorse or promote products derived from this software without
15// specific prior written permission.
16//
17// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
18// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
19// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
20// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
21// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
22// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
23// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
24// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
25// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
26// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
27// POSSIBILITY OF SUCH DAMAGE.
28
29use crate::matrix::*;
30use crate::scalar::*;
31use crate::vector::*;
32
33////////////////////////////////////////////////////////////////////////////////
34/// Rect
35////////////////////////////////////////////////////////////////////////////////
36#[repr(C)]
37#[derive(Debug, Clone, Copy, Default)]
38pub struct Dimension<T: Scalar> {
39    pub width: T,
40    pub height: T,
41}
42
43impl<T: Scalar> Dimension<T> {
44    pub fn new(width: T, height: T) -> Self {
45        Self { width, height }
46    }
47}
48
49////////////////////////////////////////////////////////////////////////////////
50/// Rect
51////////////////////////////////////////////////////////////////////////////////
52#[repr(C)]
53#[derive(Debug, Clone, Copy, Default)]
54pub struct Rect<T: Scalar> {
55    pub x: T,
56    pub y: T,
57    pub width: T,
58    pub height: T,
59}
60
61impl<T: Scalar> Rect<T> {
62    pub fn new(x: T, y: T, width: T, height: T) -> Self {
63        Self {
64            x,
65            y,
66            width,
67            height,
68        }
69    }
70    pub fn from(min_vec: &Vector2<T>, max_vec: &Vector2<T>) -> Self {
71        let min_x = T::min(min_vec.x, max_vec.x);
72        let min_y = T::min(min_vec.y, max_vec.y);
73        let max_x = T::max(min_vec.x, max_vec.x);
74        let max_y = T::max(min_vec.y, max_vec.y);
75        Self {
76            x: min_x,
77            y: min_y,
78            width: max_x - min_x,
79            height: max_y - min_y,
80        }
81    }
82
83    pub fn min(&self) -> Vector2<T> {
84        Vector2::new(self.x, self.y)
85    }
86    pub fn max(&self) -> Vector2<T> {
87        Vector2::new(self.x + self.width, self.y + self.height)
88    }
89    pub fn intersect(&self, other: &Self) -> Option<Self> {
90        let smx = self.max();
91        let smn = self.min();
92        let omx = other.max();
93        let omn = other.min();
94
95        if smx.x < omn.x || smx.y < omn.y || smn.x > omx.x || smn.y > omx.y {
96            return None;
97        }
98
99        let min_vec = Vector2::max(&self.min(), &other.min());
100        let max_vec = Vector2::min(&self.max(), &other.max());
101        Some(Self::from(&min_vec, &max_vec))
102    }
103
104    pub fn contains(&self, p: &Vector2<T>) -> bool {
105        p.x >= self.x && p.y >= self.y && p.x <= self.x + self.width && p.y <= self.y + self.height
106    }
107}
108
109////////////////////////////////////////////////////////////////////////////////
110/// Box3
111////////////////////////////////////////////////////////////////////////////////
112#[repr(C)]
113#[derive(Debug, Clone, Copy, Default)]
114pub struct Box3<T: Scalar> {
115    pub min: Vector3<T>,
116    pub max: Vector3<T>,
117}
118
119impl<T: Scalar> Box3<T> {
120    pub fn new(v0: &Vector3<T>, v1: &Vector3<T>) -> Self {
121        Self {
122            min: Vector3::min(v0, v1),
123            max: Vector3::max(v0, v1),
124        }
125    }
126    pub fn center(&self) -> Vector3<T> {
127        (self.max + self.min) * T::half()
128    }
129    pub fn extent(&self) -> Vector3<T> {
130        self.max - self.center()
131    }
132    pub fn overlap(&self, other: &Self) -> bool {
133        if self.max.x < other.min.x {
134            return false;
135        }
136        if self.max.y < other.min.y {
137            return false;
138        }
139        if self.max.z < other.min.z {
140            return false;
141        }
142
143        if self.min.x > other.max.x {
144            return false;
145        }
146        if self.min.x > other.max.x {
147            return false;
148        }
149        if self.min.x > other.max.x {
150            return false;
151        }
152        return true;
153    }
154
155    pub fn add(&self, p: &Vector3<T>) -> Self {
156        Self {
157            min: Vector3::min(&p, &self.min),
158            max: Vector3::max(&p, &self.max),
159        }
160    }
161
162    pub fn subdivide(&self) -> [Self; 8] {
163        let cube_table: [Vector3<i32>; 8] = [
164            Vector3::new(0, 1, 0),
165            Vector3::new(1, 1, 0),
166            Vector3::new(1, 1, 1),
167            Vector3::new(0, 1, 1),
168            Vector3::new(0, 0, 0),
169            Vector3::new(1, 0, 0),
170            Vector3::new(1, 0, 1),
171            Vector3::new(0, 0, 1),
172        ];
173
174        let ps: [Vector3<T>; 2] = [self.min, self.max];
175        let mut vs = [Vector3::zero(); 8];
176        for i in 0..8 {
177            vs[i] = Vector3::new(
178                ps[cube_table[i].x as usize].x,
179                ps[cube_table[i].y as usize].y,
180                ps[cube_table[i].z as usize].z,
181            );
182        }
183
184        let c = self.center();
185        let mut out = [Box3 {
186            min: Vector3::zero(),
187            max: Vector3::zero(),
188        }; 8];
189        for i in 0..8 {
190            out[i] = Self::new(&Vector3::min(&c, &vs[i]), &Vector3::max(&c, &vs[i]));
191        }
192        out
193    }
194}
195
196////////////////////////////////////////////////////////////////////////////////
197/// Line
198////////////////////////////////////////////////////////////////////////////////
199#[repr(C)]
200#[derive(Debug, Clone, Copy, Default)]
201pub struct Line<T: Scalar, V: Vector<T>> {
202    pub p: V, // point on the line
203    pub d: V, // direction of the line
204    t: core::marker::PhantomData<T>,
205}
206
207impl<T: Scalar, V: Vector<T>> Line<T, V> {
208    pub fn new(p: &V, d: &V) -> Self {
209        Self {
210            p: *p,
211            d: *d,
212            t: core::marker::PhantomData,
213        }
214    }
215    pub fn from_start_end(s: &V, e: &V) -> Self {
216        Self {
217            p: *s,
218            d: *e - *s,
219            t: core::marker::PhantomData,
220        }
221    }
222    pub fn closest_point_on_line(&self, p: &V) -> (T, V) {
223        let p_dir = *p - self.p;
224
225        let d_sp = V::dot(&self.d, &p_dir);
226        let d_ss = V::dot(&self.d, &self.d);
227
228        let t = d_sp / d_ss;
229
230        (t, self.p + self.d * t)
231    }
232}
233
234impl<T: FloatScalar, V: FloatVector<T>> Line<T, V> {
235    pub fn normalize(&self) -> Self {
236        Self {
237            p: self.p,
238            d: FloatVector::normalize(&self.d),
239            t: core::marker::PhantomData,
240        }
241    }
242}
243
244pub fn shortest_segment3d_between_lines3d<T: FloatScalar>(
245    line0: &Line<T, Vector3<T>>,
246    line1: &Line<T, Vector3<T>>,
247    epsilon: T,
248) -> Option<Segment<T, Vector3<T>>> {
249    let s0 = line0.p;
250    let s1 = line1.p;
251
252    let d1 = line1.d;
253    let d0 = line0.d;
254
255    let normal = Vector3::normalize(&Vector3::cross(&d1, &d0));
256    let n0 = Vector3::normalize(&Vector3::cross(&normal, &d0));
257    let n1 = Vector3::normalize(&Vector3::cross(&normal, &d1));
258
259    let plane0 = Plane::new(&n0, &s0);
260    let plane1 = Plane::new(&n1, &s1);
261
262    let p1 = plane0.intersect_line(line1, epsilon);
263    let p0 = plane1.intersect_line(line0, epsilon);
264
265    match (p0, p1) {
266        (Some((_, s)), Some((_, e))) => Some(Segment::new(&s, &e)),
267        _ => None,
268    }
269}
270
271////////////////////////////////////////////////////////////////////////////////
272/// Segment
273////////////////////////////////////////////////////////////////////////////////
274#[repr(C)]
275#[derive(Clone, Copy, Default)]
276pub struct Segment<T: Scalar, V: Vector<T>> {
277    pub s: V, // start
278    pub e: V, // end
279    t: core::marker::PhantomData<T>,
280}
281
282impl<T: Scalar, V: Vector<T>> Segment<T, V> {
283    pub fn new(s: &V, e: &V) -> Self {
284        Self {
285            s: *s,
286            e: *e,
287            t: core::marker::PhantomData,
288        }
289    }
290    pub fn closest_point_on_segment(&self, p: &V) -> (T, V) {
291        let dir = self.e - self.s;
292        let p_dir = *p - self.s;
293
294        let d_sp = V::dot(&dir, &p_dir);
295        let d_ss = V::dot(&dir, &dir);
296
297        if d_sp < T::zero() {
298            return (T::zero(), self.s);
299        } else if d_sp > d_ss {
300            return (T::one(), self.e);
301        }
302
303        let t = d_sp / d_ss;
304
305        (t, self.s + dir * t)
306    }
307}
308
309impl<T: FloatScalar, V: FloatVector<T>> Segment<T, V> {
310    pub fn distance(&self, p: &V) -> T {
311        let (_, p_on_seg) = self.closest_point_on_segment(p);
312        V::length(&(p_on_seg - *p))
313    }
314}
315
316////////////////////////////////////////////////////////////////////////////////
317/// Ray
318////////////////////////////////////////////////////////////////////////////////
319#[repr(C)]
320#[derive(Clone, Copy, Default)]
321pub struct Ray<T: Scalar, V: Vector<T>> {
322    pub start: V,
323    pub direction: V,
324    t: core::marker::PhantomData<T>,
325}
326
327impl<T: FloatScalar, V: FloatVector<T>> Ray<T, V> {
328    pub fn new(start: &V, direction: &V) -> Self {
329        Self {
330            start: *start,
331            direction: V::normalize(direction),
332            t: core::marker::PhantomData,
333        }
334    }
335}
336
337impl<T: Scalar> Ray<T, Vector3<T>> {
338    pub fn intersect_plane(&self, p: &Plane<T>) -> Option<Vector3<T>> {
339        let n = p.normal();
340        let t: T = -(p.d + Vector3::dot(&n, &self.start)) / Vector3::dot(&n, &self.direction);
341        if t < T::zero() {
342            None
343        } else {
344            Some(self.direction * t + self.start)
345        }
346    }
347}
348
349////////////////////////////////////////////////////////////////////////////////
350/// Sphere3
351////////////////////////////////////////////////////////////////////////////////
352#[repr(C)]
353#[derive(Clone, Copy, Default)]
354pub struct Sphere3<T: FloatScalar> {
355    pub center: Vector3<T>,
356    pub radius: T,
357}
358
359impl<T: FloatScalar> Sphere3<T> {
360    pub fn new(center: Vector3<T>, radius: T) -> Self {
361        Self {
362            center: center,
363            radius: radius,
364        }
365    }
366}
367
368////////////////////////////////////////////////////////////////////////////////
369/// Triangle
370////////////////////////////////////////////////////////////////////////////////
371#[repr(C)]
372#[derive(Clone, Copy, Default)]
373pub struct Tri3<T: FloatScalar> {
374    vertices: [Vector3<T>; 3],
375}
376
377impl<T: FloatScalar> Tri3<T> {
378    pub fn new(vertices: [Vector3<T>; 3]) -> Self {
379        Self { vertices: vertices }
380    }
381    pub fn vertices(&self) -> &[Vector3<T>; 3] {
382        &self.vertices
383    }
384
385    pub fn barycentric_coordinates(&self, pt: &Vector3<T>) -> Vector3<T> {
386        let v0 = self.vertices[0];
387        let v1 = self.vertices[1];
388        let v2 = self.vertices[2];
389        let vv1 = v1 - v0;
390        let vv2 = v2 - v0;
391        let vvc = Vector3::cross(&vv1, &vv2);
392        let m = Matrix3::new(
393            vv1.x, vv1.y, vv1.z, vv2.x, vv2.y, vv2.z, vvc.x, vvc.y, vvc.z,
394        );
395        let lambda = m.inverse() * *pt;
396        Vector3::new(T::one() - lambda.x - lambda.y, lambda.x, lambda.y)
397    }
398}
399
400////////////////////////////////////////////////////////////////////////////////
401/// Plane
402////////////////////////////////////////////////////////////////////////////////
403#[repr(C)]
404#[derive(Clone, Copy, Default)]
405pub struct Plane<T: Scalar> {
406    a: T,
407    b: T,
408    c: T,
409    d: T,
410}
411
412impl<T: Scalar> Plane<T> {
413    pub fn normal(&self) -> Vector3<T> {
414        Vector3::new(self.a, self.b, self.c)
415    }
416    pub fn constant(&self) -> T {
417        self.d
418    }
419
420    pub fn intersect_ray(&self, r: &Ray<T, Vector3<T>>) -> Option<Vector3<T>> {
421        r.intersect_plane(self)
422    }
423
424    pub fn intersect_line(
425        &self,
426        line: &Line<T, Vector3<T>>,
427        epsilon: T,
428    ) -> Option<(T, Vector3<T>)> {
429        let s = line.p;
430        let dir = line.d;
431        let n = self.normal();
432
433        let denom = Vector3::dot(&n, &dir);
434        if denom.tabs() < epsilon {
435            None
436        } else {
437            let t = -(self.constant() + Vector3::dot(&n, &s)) / denom;
438            Some((t, dir * t + s))
439        }
440    }
441}
442
443impl<T: FloatScalar> Plane<T> {
444    pub fn new(n: &Vector3<T>, p: &Vector3<T>) -> Self {
445        let norm = Vector3::normalize(n);
446        let d = Vector3::dot(&norm, p);
447        Self {
448            a: norm.x,
449            b: norm.y,
450            c: norm.z,
451            d: -d,
452        }
453    }
454
455    pub fn from_tri(v0: &Vector3<T>, v1: &Vector3<T>, v2: &Vector3<T>) -> Self {
456        let n = tri_normal(v0, v1, v2);
457        Self::new(&n, v0)
458    }
459    pub fn from_quad(v0: &Vector3<T>, v1: &Vector3<T>, v2: &Vector3<T>, v3: &Vector3<T>) -> Self {
460        let n = quad_normal(v0, v1, v2, v3);
461        let c = (*v0 + *v1 + *v2 + *v3) * T::quarter();
462        Self::new(&n, &c)
463    }
464}
465
466////////////////////////////////////////////////////////////////////////////////
467/// Parametric Plane
468////////////////////////////////////////////////////////////////////////////////
469#[repr(C)]
470#[derive(Clone, Copy, Default)]
471pub struct ParametricPlane<T: Scalar> {
472    pub center: Vector3<T>,
473    pub x_axis: Vector3<T>,
474    pub y_axis: Vector3<T>,
475}
476
477impl<T: FloatScalar> ParametricPlane<T> {
478    pub fn new(center: &Vector3<T>, x_axis: &Vector3<T>, y_axis: &Vector3<T>) -> Self {
479        Self {
480            center: *center,
481            x_axis: *x_axis,
482            y_axis: *y_axis,
483        }
484    }
485
486    pub fn plane(&self) -> Plane<T> {
487        Plane::new(&self.normal(), &self.center)
488    }
489
490    pub fn normal(&self) -> Vector3<T> {
491        Vector3::cross(&self.x_axis, &self.y_axis).normalize()
492    }
493
494    pub fn intersect_ray(&self, r: &Ray<T, Vector3<T>>) -> Option<Vector3<T>> {
495        r.intersect_plane(&self.plane())
496    }
497
498    pub fn intersect_line(
499        &self,
500        line: &Line<T, Vector3<T>>,
501        epsilon: T,
502    ) -> Option<(T, Vector3<T>)> {
503        self.plane().intersect_line(line, epsilon)
504    }
505
506    pub fn project(&self, v: &Vector3<T>) -> Vector2<T> {
507        let p = *v - self.center;
508        let x_coord = dot(&p, &self.x_axis) / dot(&self.x_axis, &self.x_axis);
509        let y_coord = dot(&p, &self.y_axis) / dot(&self.y_axis, &self.y_axis);
510        Vector2::new(x_coord, y_coord)
511    }
512}
513
514////////////////////////////////////////////////////////////////////////////////
515/// Tri
516////////////////////////////////////////////////////////////////////////////////
517pub fn tri_normal<T: FloatScalar>(v0: &Vector3<T>, v1: &Vector3<T>, v2: &Vector3<T>) -> Vector3<T> {
518    let v10 = *v1 - *v0;
519    let v20 = *v2 - *v0;
520    Vector3::normalize(&Vector3::cross(&v10, &v20))
521}
522
523////////////////////////////////////////////////////////////////////////////////
524/// Tri
525////////////////////////////////////////////////////////////////////////////////
526pub fn quad_normal<T: FloatScalar>(
527    v0: &Vector3<T>,
528    v1: &Vector3<T>,
529    v2: &Vector3<T>,
530    v3: &Vector3<T>,
531) -> Vector3<T> {
532    let v20 = *v2 - *v0;
533    let v31 = *v3 - *v1;
534    Vector3::normalize(&Vector3::cross(&v20, &v31))
535}
536
537#[cfg(test)]
538mod tests {
539    use super::*;
540    #[test]
541    pub fn test_barycentric() {
542        let v0 = Vector3::new(0.0, 0.0, 0.0);
543        let v1 = Vector3::new(0.0, 1.0, 0.0);
544        let v2 = Vector3::new(0.0, 0.0, 1.0);
545
546        let tri = Tri3::new([v0, v1, v2]);
547        let pp0 = tri.barycentric_coordinates(&v0);
548        assert!(f32::abs(pp0.x - 1.0) < 0.01);
549        assert!(f32::abs(pp0.y) < 0.01);
550        assert!(f32::abs(pp0.z) < 0.01);
551
552        let pp1 = tri.barycentric_coordinates(&v1);
553        assert!(f32::abs(pp1.x) < 0.01);
554        assert!(f32::abs(pp1.y - 1.0) < 0.01);
555        assert!(f32::abs(pp1.z) < 0.01);
556
557        let pp2 = tri.barycentric_coordinates(&v2);
558        assert!(f32::abs(pp2.x) < 0.01);
559        assert!(f32::abs(pp2.y) < 0.01);
560        assert!(f32::abs(pp2.z - 1.0) < 0.01);
561    }
562}
rs_math3d/primitives.rs

rs_math3d/
primitives.rs
