lgeo/gjk/
algorithm.rs

1// uses
2use lmaths::*;
3use smallvec::*;
4use super::*;
5use crate::Contact;
6
7#[allow(dead_code)]
8const MAX_ITERATIONS: u32 = 100;
9#[allow(dead_code)]
10const GJK_DISTANCE_TOLERANCE: f64 = 0.000_001;
11#[allow(dead_code)]
12const GJK_CONTINUOUS_TOLERANCE: f64 = 0.000_001;
13
14#[allow(dead_code)]
15pub struct GJK {
16    distance_tolerance: f64,
17    continuous_tolerance: f64,
18    max_iterations: u32,
19}
20
21#[allow(dead_code)]
22impl GJK {
23    /// Initialize new GJK settings, using the default one
24    pub fn new() -> Self {
25        Self {
26            distance_tolerance: GJK_DISTANCE_TOLERANCE,
27            continuous_tolerance: GJK_CONTINUOUS_TOLERANCE,
28            max_iterations: MAX_ITERATIONS,
29        }
30    }
31
32    /// Check if the shapes intersect and return the final Simplex if they do, or 'None' if they don't
33    pub fn intersect(&self, left: &dyn crate::Shape, right: &dyn crate::Shape) -> Option<Simplex>
34    {
35        let left_pos = left.position();
36        let right_pos = right.position();
37        let mut d = right_pos - left_pos;
38
39        if d == Vector2::ZERO {
40            d = Vector2::ONE;
41        }
42
43        let a = SupportPoint::from_minkowski(left, right, d);
44        if a.v.dot(d) <= 0.0 {
45            return None;
46        }
47
48        let mut simplex = Simplex::new(smallvec![]);
49        simplex.push(a);
50        d = -d;
51        for _ in 0..self.max_iterations {
52            let a = SupportPoint::from_minkowski(left, right, d);
53            if a.v.dot(d) <= 0.0 {
54                return None;
55            } else {
56                simplex.push(a);
57                if simplex.reduce_to_closest_feature(&mut d)
58                {
59                    return Some(simplex);
60                }
61            }
62        }
63
64        None
65    }
66
67    pub fn intersection_time_of_impact(&self, left: &dyn crate::Shape, left_velocity:Vector2, right: &dyn crate::Shape, right_velocity:Vector2, )
68        -> Option<Contact> {
69
70        let ray = right_velocity - left_velocity;
71
72        // initialize time of impact
73        let mut lambda = 0.0;
74        let mut normal = Vector2::ZERO;
75        let mut ray_origin = Vector2::ZERO;
76
77        // build simplex and get an initial support point to bootstrap the algorithm
78        let mut simplex = Simplex::new(smallvec![]);
79        let p = SupportPoint::from_minkowski(left, right, -ray);
80        // we only need the actual support point for this
81        let mut v = p.v;
82
83        // if the squared magnitude is small enough, we have a hit and can stop
84        while v.sqr_length() > self.continuous_tolerance {
85            // get a new support point
86            let p = SupportPoint::from_minkowski(left, right, -v);
87
88            let vp = v.dot(p.v);
89            let vr = v.dot(ray);
90            // check if we have a hit point along the ray further than the current clipped ray
91            if vp > lambda * vr {
92                // if the hit point is in the positive ray direction, we clip the ray, clear
93                // the simplex and start over from a new ray origin
94                if vr > 0.0 {
95                    lambda = vp / vr;
96                    // if the clipped hit point is beyond the end of the ray,
97                    // we can never have a hit
98                    if lambda > 1.0 {
99                        return None;
100                    }
101                    ray_origin = ray * lambda;
102                    simplex.clear();
103                    normal = -v;
104                } else {
105                    // if the hitpoint is behind the ray origin, we can never have a hit
106                    return None;
107                }
108            }
109            // we construct the simplex around the current ray origin (if we can)
110            simplex.push(p - ray_origin);
111            v = simplex.get_closest_point_to_origin();
112        }
113        if v.sqr_length() <= self.continuous_tolerance {
114            let transform = right.position() + right_velocity * lambda;
115            let mut contact = Contact::new_populate(
116                -normal.normalized(), // our convention is normal points from B towards A
117                v.length(),       // will always be very close to zero
118                transform + ray_origin,
119            );
120            contact.time_of_impact = lambda;
121            Some(contact)
122        } else {
123            None
124        }
125    }
126}
lgeo/gjk/algorithm.rs

lgeo/gjk/
algorithm.rs
