mgf/
compound.rs

1// Copyright 2017 Matthew Plant. This file is part of MGF.
2//
3// MGF is free software: you can redistribute it and/or modify
4// it under the terms of the GNU Lesser General Public License as published by
5// the Free Software Foundation, either version 3 of the License, or
6// (at your option) any later version.
7//
8// MGF is distributed in the hope that it will be useful,
9// but WITHOUT ANY WARRANTY; without even the implied warranty of
10// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
11// GNU Lesser General Public License for more details.
12//
13// You should have received a copy of the GNU Lesser General Public License
14// along with MGF. If not, see <http://www.gnu.org/licenses/>.
15
16use std::f32;
17use std::vec::Vec;
18use std::ops::{Add, AddAssign, Sub, SubAssign};
19use cgmath::prelude::*;
20use cgmath::{EuclideanSpace, Rotation, Rotation3, Vector3, Point3, Quaternion, One, Zero};
21
22use smallvec::SmallVec;
23
24use crate::bvh::*;
25use crate::bounds::*;
26use crate::collision::*;
27use crate::geom::*;
28
29/// A component is a generic volume that can either be a Sphere or Capsule at
30/// runtime. Anything that can collide with a Sphere and a Capsule can collide
31/// with a component. 
32#[derive(Copy, Clone, Debug)]
33pub enum Component {
34    Sphere(Sphere),
35    Capsule(Capsule),
36    // More shapes to come...
37}
38
39impl Component {
40    /// Create a ComponentConstructor and return the position and rotation of
41    /// the Component.
42    pub fn deconstruct(&self) -> (Point3<f32>, Quaternion<f32>, ComponentConstructor)  {
43        match self {
44            &Component::Sphere(Sphere{ r, c }) =>
45                (c, Quaternion::one(), ComponentConstructor::Sphere{ r }),
46            &Component::Capsule(Capsule{ r, a, d }) => {
47                let h = d.magnitude();
48                let rot = Quaternion::from_arc(Vector3::new(0.0, 1.0, 0.0) * h, d, None);
49                (a + d * 0.5, rot, ComponentConstructor::Capsule{ r, half_h: h * 0.5 })
50            },
51        }
52    }
53}
54
55impl Volumetric for Component {
56    fn rotate<R: Rotation3<f32>>(self, r: R) -> Self {
57        match self {
58            Component::Sphere(s) => Component::Sphere(s.rotate(r)),
59            Component::Capsule(c) => Component::Capsule(c.rotate(r)),
60        }
61    }
62}
63
64impl From<Sphere> for Component {
65    fn from(s: Sphere) -> Self {
66        Component::Sphere(s)
67    }
68}
69
70impl From<Capsule> for Component {
71    fn from(c: Capsule) -> Self {
72        Component::Capsule(c)
73    }
74}
75
76impl Add<Vector3<f32>> for Component {
77    type Output = Self;
78
79    fn add(self, v: Vector3<f32>) -> Self {
80        match self {
81            Component::Sphere(s) => Component::Sphere(s + v),
82            Component::Capsule(c) => Component::Capsule(c + v),
83        }
84    }
85}
86        
87impl Sub<Vector3<f32>> for Component {
88    type Output = Self;
89
90    fn sub(self, v: Vector3<f32>) -> Self {
91        match self {
92            Component::Sphere(s) => Component::Sphere(s - v),
93            Component::Capsule(c) => Component::Capsule(c - v),
94        }
95    }
96}
97
98impl AddAssign<Vector3<f32>> for Component {
99    fn add_assign(&mut self, v: Vector3<f32>) {
100        match self {
101            &mut Component::Sphere(ref mut s) => *s += v,
102            &mut Component::Capsule(ref mut c) => *c += v,
103        }
104    }
105}
106
107impl SubAssign<Vector3<f32>> for Component {
108    fn sub_assign(&mut self, v: Vector3<f32>) {
109        match self {
110            &mut Component::Sphere(ref mut s) => *s -= v,
111            &mut Component::Capsule(ref mut c) => *c -= v,
112        }
113    }
114}
115
116impl Shape for Component {
117    fn center(&self) -> Point3<f32> {
118        match self {
119            &Component::Sphere(s) => s.center(),
120            &Component::Capsule(c) => c.center(),
121        }
122    }
123
124    fn closest_point(&self, to: Point3<f32>) -> Point3<f32> {
125        match self {
126            &Component::Sphere(s) => s.closest_point(to),
127            &Component::Capsule(c) => c.closest_point(to),
128        }
129    }
130}
131
132impl BoundedBy<AABB> for Component {
133    fn bounds(&self) -> AABB {
134        match self {
135            &Component::Sphere(s) => s.bounds(),
136            &Component::Capsule(c) => c.bounds(),
137        }
138    }
139}
140
141impl BoundedBy<Sphere> for Component {
142    fn bounds(&self) -> Sphere {
143       match self {
144            &Component::Sphere(s) => s.bounds(),
145            &Component::Capsule(c) => c.bounds(),
146        }
147    }
148}
149
150impl<P: Particle> Intersects<Component> for P {
151    fn intersection(&self, rhs: &Component) -> Option<Intersection> {
152        match rhs {
153            &Component::Sphere(ref s) => self.intersection(s),
154            &Component::Capsule(ref c) => self.intersection(c),
155        }
156    }
157}
158
159macro_rules! impl_component_collision {
160    (
161        $recv:ty
162    ) => {
163        impl Contacts<Moving<Component>> for $recv {
164            fn contacts<F: FnMut(Contact)>(&self, rhs: &Moving<Component>, callback: F) -> bool {
165                match rhs.0 {
166                    Component::Sphere(s) => self.contacts(&Moving::sweep(s, rhs.1), callback),
167                    Component::Capsule(c) => self.contacts(&Moving::sweep(c, rhs.1), callback),
168                }
169            }
170        }
171    };
172}
173
174impl_component_collision!{ Plane }
175impl_component_collision!{ Triangle }
176impl_component_collision!{ Rectangle }
177impl_component_collision!{ Sphere }
178impl_component_collision!{ Capsule }
179
180impl<RHS> Contacts<RHS> for Moving<Component>
181where
182    RHS: Contacts<Moving<Sphere>> + Contacts<Moving<Capsule>>
183{
184    fn contacts<F: FnMut(Contact)>(&self, rhs: &RHS, mut callback: F) -> bool {
185        match self.0 {
186            Component::Sphere(s) => rhs.contacts(&Moving::sweep(s, self.1), |c|callback(-c)),
187            Component::Capsule(c) => rhs.contacts(&Moving::sweep(c, self.1), |c|callback(-c)),
188        }
189    }
190}
191
192impl LocalContacts<Moving<Component>> for Moving<Component> {
193    fn local_contacts<F: FnMut(LocalContact)>(&self, rhs: &Moving<Component>, mut callback: F) -> bool {
194        self.contacts(
195            rhs,
196            | c | {
197                callback(
198                    LocalContact {
199                        local_a: c.a + -(self.0.center() + self.1 * c.t).to_vec(),
200                        local_b: c.b + -(rhs.0.center() + rhs.1 * c.t).to_vec(),
201                        global: c,
202                    }
203                );
204            }
205        )
206    }
207}
208
209/// A description of a Component minus rotation and position.
210#[derive(Copy, Clone, Debug)]
211pub enum ComponentConstructor {
212    Sphere{ r: f32 },
213    Capsule{ r: f32, half_h: f32 },
214    // More shapes to come...
215}
216
217impl ComponentConstructor {
218    /// Create a component from a component constructor.
219    pub fn construct<R: Rotation3<f32>>(&self, p: Point3<f32>, rot: R) -> Component {
220        match self {
221            &ComponentConstructor::Sphere{ r } => Component::Sphere(Sphere{ r, c: p }),
222            &ComponentConstructor::Capsule{ r, half_h } => {
223                let d = rot.rotate_vector(Vector3::new(0.0, 1.0, 0.0) * half_h); 
224                Component::Capsule(Capsule{ r: r, a: p + -d, d: d * 2.0 })
225            },
226        }
227    }
228}
229
230/// An aggregate structure of Spheres and Capsules. Has a position and rotation.
231#[derive(Clone)]
232pub struct Compound {
233    /// The displacement of the object.
234    pub disp: Vector3<f32>,
235    /// The rotation of the object. Assumed to be normalized.
236    pub rot: Quaternion<f32>,
237    /// Indices of the geometries composing the compound in the BVH.
238    /// One-to-one with the constructing vector.
239    pub shapes: SmallVec<[usize; 1]>,
240    /// BVH storing the components to improve collision efficiency.
241    pub bvh: BVH<AABB, Component>,
242}
243
244impl Compound {
245    pub fn new(components: Vec<Component>) -> Self {
246        let mut bvh: BVH<AABB, Component> = BVH::new();
247        let mut shapes: SmallVec<[usize; 1]> = SmallVec::with_capacity(components.len());
248        for component in components.iter() {
249            shapes.push(bvh.insert(component, *component));
250        }
251        Compound {
252            disp: Vector3::zero(),
253            rot: Quaternion::one(),
254            shapes,
255            bvh,
256        }
257    }
258}
259
260impl AddAssign<Vector3<f32>> for Compound {
261    fn add_assign(&mut self, v: Vector3<f32>) {
262        self.disp += v
263    }
264}
265
266impl SubAssign<Vector3<f32>> for Compound {
267    fn sub_assign(&mut self, v: Vector3<f32>) {
268        self.disp -= v
269    }
270}
271
272impl BoundedBy<AABB> for Compound {
273    fn bounds(&self) -> AABB {
274        self.bvh[self.bvh.root()].rotate(self.rot) + self.disp
275    }
276}
277
278impl BoundedBy<Sphere> for Compound {
279    fn bounds(&self) -> Sphere {
280        let s: Sphere = self.bvh[self.bvh.root()].bounds();
281        s + self.disp
282    }
283}
284
285impl Shape for Compound {
286    /// The point returned by a compound shape is the displacement of the
287    /// object and not the center of mass. It is impossible for Compound
288    /// to calculate the center of mass given it has no information regarding
289    /// the mass of individual components.
290    fn center(&self) -> Point3<f32> {
291        Point3::from_vec(self.disp)
292    }
293
294    fn closest_point(&self, to: Point3<f32>) -> Point3<f32> {
295        let mut best_p = Point3::new(0.0, 0.0, 0.0);
296        let mut best_dist: f32 = f32::INFINITY;
297        for shape in self.shapes.iter() {
298            let new_p = self.bvh.get_leaf(*shape).closest_point(to);
299            let new_dist = (to - new_p).magnitude2();
300            if new_dist < best_dist {
301                best_p = new_p;
302                best_dist = new_dist;
303            }
304        }
305        best_p
306    }
307}
308
309impl<P: Particle> Intersects<Compound> for P {
310    fn intersection(&self, rhs: &Compound) -> Option<Intersection> {
311        let conj_rot = rhs.rot.conjugate();
312        let p = conj_rot.rotate_point(self.pos() + -rhs.disp) + rhs.disp;
313        let d = conj_rot.rotate_vector(self.dir());
314        let r = Ray{ p, d };
315        let mut result: Option<Intersection> = None;
316        rhs.bvh.raytrace(&r, |&comp, inter| {
317            if inter.t > P::DT {
318                return;
319            }
320            let shape = comp.rotate(rhs.rot) + rhs.disp;
321            if let Some(inter) = self.intersection(&shape) {
322                if let Some(res) = result {
323                    if inter.t > res.t {
324                        return;
325                    }
326                }
327                result = Some(inter)
328            }
329        });
330        result
331    }
332}
333
334impl<RHS> Contacts<RHS> for Compound
335where
336    RHS: Contacts<Component> + BoundedBy<AABB>
337{
338    fn contacts<F: FnMut(Contact)>(&self, rhs: &RHS, mut callback: F) -> bool {
339        // We assume that self.rot is normalized.
340        let conj_rot = self.rot.conjugate();
341        let mut rhs_bounds = rhs.bounds().rotate(conj_rot);
342        let rhs_center = rhs_bounds.center();
343        let bounds_disp = conj_rot.rotate_point(rhs_center + -self.disp) + self.disp;
344        rhs_bounds.set_pos(bounds_disp);
345        let mut collided = false;
346        self.bvh.query(&rhs_bounds, |&comp| {
347            let shape = comp.rotate_about(self.rot, Point3::new(0.0, 0.0, 0.0)) + self.disp;
348            rhs.contacts(&shape, |c| { collided = true; callback(-c) });
349        });
350        collided
351    }
352}
353
354#[cfg(test)]
355mod tests {
356    mod compound {
357        use cgmath::InnerSpace;
358        use crate::compound::*;
359        use crate::collision::Contacts;
360
361        #[test]
362        fn test_compound() {
363            let components = vec![
364                Component::Sphere(Sphere{ c: Point3::new(-5.0, 0.0, 0.0), r: 1.0 }),
365                Component::Sphere(Sphere{ c: Point3::new(5.0, 0.0, 0.0), r: 1.0 }),
366            ];
367            let mut compound = Compound::new(components);
368            let test_sphere = Moving::sweep(Sphere{ c: Point3::new(0.0, 8.0, 0.0), r: 1.0 },
369                                            Vector3::new(0.0, -1.5, 0.0));
370            assert!(!compound.contacts(&test_sphere, |c: Contact| { panic!("c = {:?}", c); }));
371            // rotate compounds
372            compound.rot = Quaternion::from_arc(Vector3::new(1.0, 0.0, 0.0),
373                                                Vector3::new(0.0, 1.0, 0.0),
374                                                None).normalize();
375            let contact: Contact = compound.last_contact(&test_sphere).unwrap();
376            assert_relative_eq!(contact.t, 0.6666663, epsilon = COLLISION_EPSILON);
377            assert_relative_eq!(contact.a, Point3::new(0.0, 6.0, 0.0), epsilon = COLLISION_EPSILON);
378
379            let static_rect = Rect {
380                c: Point3::new(0.0, -2.0, 0.0),
381                u: [ Vector3::new(1.0, 0.0, 0.0), Vector3::new(0.0, 0.0, 1.0) ],
382                e: [ 6.0, 6.0 ],
383            };
384
385            compound.rot = Quaternion::one();
386
387            let _contact: Contact = compound.last_contact(&Moving::sweep(static_rect, Vector3::new(0.0, 3.0, 0.0))).unwrap();
388        }
389    }
390}
mgf/compound.rs

mgf/
compound.rs
