rg3d_physics/
lib.rs

1extern crate rg3d_core;
2#[macro_use]
3extern crate bitflags;
4
5use rg3d_core::{
6    math::{
7        vec3::Vec3,
8        ray::Ray,
9    },
10    pool::{
11        Pool,
12        Handle,
13    },
14    visitor::{
15        Visit,
16        VisitResult,
17        Visitor,
18    },
19};
20use std::{
21    cmp::Ordering,
22    cell::RefCell
23};
24use crate::{
25    rigid_body::RigidBody,
26    static_geometry::StaticGeometry,
27    convex_shape::{
28        ConvexShape,
29        CircumRadius
30    }
31};
32use rg3d_core::pool::Ticket;
33
34pub mod gjk_epa;
35pub mod convex_shape;
36pub mod rigid_body;
37pub mod contact;
38pub mod static_geometry;
39
40pub enum HitKind {
41    Body(Handle<RigidBody>),
42    StaticTriangle {
43        static_geometry: Handle<StaticGeometry>,
44        triangle_index: usize,
45    },
46}
47
48pub struct RayCastOptions {
49    pub ignore_bodies: bool,
50    pub ignore_static_geometries: bool,
51    pub sort_results: bool,
52}
53
54impl Default for RayCastOptions {
55    fn default() -> Self {
56        Self {
57            ignore_bodies: false,
58            ignore_static_geometries: false,
59            sort_results: true,
60        }
61    }
62}
63
64pub struct RayCastResult {
65    pub kind: HitKind,
66    pub position: Vec3,
67    pub normal: Vec3,
68    pub sqr_distance: f32,
69}
70
71#[derive(Debug)]
72pub struct Physics {
73    bodies: Pool<RigidBody>,
74    static_geoms: Pool<StaticGeometry>,
75    query_buffer: RefCell<Vec<u32>>,
76    enabled: bool
77}
78
79impl Visit for Physics {
80    fn visit(&mut self, name: &str, visitor: &mut Visitor) -> VisitResult {
81        visitor.enter_region(name)?;
82
83        self.bodies.visit("Bodies", visitor)?;
84        self.static_geoms.visit("StaticGeoms", visitor)?;
85        let _ = self.enabled.visit("Enabled", visitor); // let _ for backward compatibility.
86
87        visitor.leave_region()
88    }
89}
90
91impl Default for Physics {
92    fn default() -> Self {
93        Self::new()
94    }
95}
96
97impl Clone for Physics {
98    fn clone(&self) -> Self {
99        Self {
100            bodies: self.bodies.clone(),
101            static_geoms: self.static_geoms.clone(),
102            query_buffer: Default::default(),
103            enabled: self.enabled
104        }
105    }
106}
107
108impl Physics {
109    pub fn new() -> Self {
110        Self {
111            bodies: Pool::new(),
112            static_geoms: Pool::new(),
113            query_buffer: Default::default(),
114            enabled: true
115        }
116    }
117
118    pub fn add_body(&mut self, body: RigidBody) -> Handle<RigidBody> {
119        self.bodies.spawn(body)
120    }
121
122    pub fn remove_body(&mut self, body_handle: Handle<RigidBody>) {
123        self.bodies.free(body_handle);
124    }
125
126    pub fn add_static_geometry(&mut self, static_geom: StaticGeometry) -> Handle<StaticGeometry> {
127        self.static_geoms.spawn(static_geom)
128    }
129
130    pub fn borrow_static_geometry(&self, static_geom: Handle<StaticGeometry>) -> &StaticGeometry {
131        &self.static_geoms[static_geom]
132    }
133
134    pub fn borrow_static_geometry_mut(&mut self, static_geom: Handle<StaticGeometry>) -> &mut StaticGeometry {
135        &mut self.static_geoms[static_geom]
136    }
137
138    pub fn is_static_geometry_handle_valid(&self, static_geom: Handle<StaticGeometry>) -> bool {
139        self.static_geoms.is_valid_handle(static_geom)
140    }
141
142    pub fn remove_static_geometry(&mut self, static_geom: Handle<StaticGeometry>) {
143        self.static_geoms.free(static_geom);
144    }
145
146    pub fn borrow_body(&self, handle: Handle<RigidBody>) -> &RigidBody {
147        self.bodies.borrow(handle)
148    }
149
150    pub fn borrow_body_mut(&mut self, handle: Handle<RigidBody>) -> &mut RigidBody {
151        self.bodies.borrow_mut(handle)
152    }
153
154    pub fn is_valid_body_handle(&self, handle: Handle<RigidBody>) -> bool {
155        self.bodies.is_valid_handle(handle)
156    }
157
158    pub fn take_reserve_body(&mut self, handle: Handle<RigidBody>) -> (Ticket<RigidBody>, RigidBody) {
159        self.bodies.take_reserve(handle)
160    }
161
162    pub fn put_body_back(&mut self, ticket: Ticket<RigidBody>, body: RigidBody) -> Handle<RigidBody> {
163        self.bodies.put_back(ticket, body)
164    }
165
166    pub fn forget_body_ticket(&mut self, ticket: Ticket<RigidBody>) {
167        self.bodies.forget_ticket(ticket)
168    }
169
170    pub fn take_reserve_static_geometry(&mut self, handle: Handle<StaticGeometry>) -> (Ticket<StaticGeometry>, StaticGeometry) {
171        self.static_geoms.take_reserve(handle)
172    }
173
174    pub fn put_static_geometry_back(&mut self, ticket: Ticket<StaticGeometry>, geom: StaticGeometry) -> Handle<StaticGeometry> {
175        self.static_geoms.put_back(ticket, geom)
176    }
177
178    pub fn forget_static_geometry_ticket(&mut self, ticket: Ticket<StaticGeometry>) {
179        self.static_geoms.forget_ticket(ticket)
180    }
181
182    pub fn set_enabled(&mut self, enabled: bool) {
183        self.enabled = enabled;
184    }
185
186    pub fn enabled(&self) -> bool {
187        self.enabled
188    }
189
190    pub fn step(&mut self, delta_time: f32) {
191        if !self.enabled {
192            return;
193        }
194
195        let dt2 = delta_time * delta_time;
196        let air_friction = 0.003;
197
198        // Take second mutable reference to bodies, this is safe because:
199        // 1. We won't modify collection while iterating over it.
200        // 2. Simultaneous access to a body won't happen because of
201        //    pointer equality check down below.
202        let other_bodies = unsafe { &mut *(&mut self.bodies as *mut Pool<RigidBody>) };
203
204        for (body_handle, body) in self.bodies.pair_iter_mut() {
205            if let Some(ref mut lifetime) = body.lifetime {
206                *lifetime -= delta_time;
207            }
208
209            body.acceleration += body.gravity;
210            body.verlet(dt2, air_friction);
211
212            body.contacts.clear();
213
214            for (other_body_handle, other_body) in other_bodies.pair_iter_mut() {
215                // Enforce borrowing rules at runtime.
216                if !std::ptr::eq(body, other_body) &&
217                    ((other_body.collision_group & body.collision_mask) != 0) &&
218                    ((body.collision_group & other_body.collision_mask) != 0) {
219                    body.solve_rigid_body_collision(body_handle,other_body, other_body_handle);
220                }
221            }
222
223            for (handle, static_geometry) in self.static_geoms.pair_iter() {
224                let mut query_buffer = self.query_buffer.borrow_mut();
225                static_geometry.octree.sphere_query(body.position, body.shape.circumradius(), &mut query_buffer);
226
227                for n in query_buffer.iter().map(|i| *i as usize) {
228                    let triangle = static_geometry.triangles.get(n).unwrap();
229                    body.solve_triangle_collision(&triangle, n, handle);
230                }
231            }
232        }
233
234        self.bodies.retain(|body| body.lifetime.is_none() || body.lifetime.unwrap() > 0.0);
235    }
236
237    pub fn ray_cast(&self, ray: &Ray, options: RayCastOptions, result: &mut Vec<RayCastResult>) -> bool {
238        result.clear();
239
240        // Check bodies
241        if !options.ignore_bodies {
242            for body_index in 0..self.bodies.get_capacity() {
243                let body = if let Some(body) = self.bodies.at(body_index) {
244                    body
245                } else {
246                    continue;
247                };
248
249                let body_handle = self.bodies.handle_from_index(body_index);
250
251                match &body.shape {
252                    ConvexShape::Dummy => {}
253                    ConvexShape::Box(box_shape) => {
254                        if let Some(points) = ray.box_intersection_points(&box_shape.get_min(), &box_shape.get_max()) {
255                            for point in points.iter() {
256                                result.push(RayCastResult {
257                                    kind: HitKind::Body(body_handle),
258                                    position: *point,
259                                    normal: *point - body.position, // TODO: Fix normal
260                                    sqr_distance: point.sqr_distance(&ray.origin),
261                                })
262                            }
263                        }
264                    }
265                    ConvexShape::Sphere(sphere_shape) => {
266                        if let Some(points) = ray.sphere_intersection_points(&body.position, sphere_shape.radius) {
267                            for point in points.iter() {
268                                result.push(RayCastResult {
269                                    kind: HitKind::Body(body_handle),
270                                    position: *point,
271                                    normal: *point - body.position,
272                                    sqr_distance: point.sqr_distance(&ray.origin),
273                                })
274                            }
275                        }
276                    }
277                    ConvexShape::Capsule(capsule_shape) => {
278                        let (pa, pb) = capsule_shape.get_cap_centers();
279                        let pa = pa + body.position;
280                        let pb = pb + body.position;
281
282                        if let Some(points) = ray.capsule_intersection(&pa, &pb, capsule_shape.get_radius()) {
283                            for point in points.iter() {
284                                result.push(RayCastResult {
285                                    kind: HitKind::Body(body_handle),
286                                    position: *point,
287                                    normal: *point - body.position,
288                                    sqr_distance: point.sqr_distance(&ray.origin),
289                                })
290                            }
291                        }
292                    }
293                    ConvexShape::Triangle(triangle_shape) => {
294                        if let Some(point) = ray.triangle_intersection(&triangle_shape.vertices) {
295                            result.push(RayCastResult {
296                                kind: HitKind::Body(body_handle),
297                                position: point,
298                                normal: triangle_shape.get_normal().unwrap(),
299                                sqr_distance: point.sqr_distance(&ray.origin),
300                            })
301                        }
302                    }
303                    ConvexShape::PointCloud(_point_cloud) => {
304                        // TODO: Implement this. This requires to build convex hull from point cloud first
305                        // i.e. by gift wrapping algorithm or some other more efficient algorithms -
306                        // https://dccg.upc.edu/people/vera/wp-content/uploads/2014/11/GA2014-ConvexHulls3D-Roger-Hernando.pdf
307                    }
308                }
309            }
310        }
311
312        // Check static geometries
313        if !options.ignore_static_geometries {
314            for (handle, geom) in self.static_geoms.pair_iter() {
315                let mut query_buffer = self.query_buffer.borrow_mut();
316                geom.octree.ray_query(ray, &mut query_buffer);
317
318                for triangle_index in query_buffer.iter().map(|i| *i as usize) {
319                    let triangle = geom.triangles.get(triangle_index).unwrap();
320                    if let Some(point) = ray.triangle_intersection(&triangle.points) {
321                        result.push(RayCastResult {
322                            kind: HitKind::StaticTriangle {
323                                static_geometry: handle,
324                                triangle_index,
325                            },
326                            position: point,
327                            normal: triangle.plane.normal,
328                            sqr_distance: point.sqr_distance(&ray.origin),
329                        })
330                    }
331                }
332            }
333        }
334
335        if options.sort_results {
336            result.sort_by(|a, b| {
337                if a.sqr_distance > b.sqr_distance {
338                    Ordering::Greater
339                } else if a.sqr_distance < b.sqr_distance {
340                    Ordering::Less
341                } else {
342                    Ordering::Equal
343                }
344            })
345        }
346
347        !result.is_empty()
348    }
349}
rg3d_physics/lib.rs

rg3d_physics/
lib.rs
