├── .gitignore
├── .idea
    ├── .gitignore
    ├── vcs.xml
    ├── modules.xml
    └── rg3d-physics.iml
├── README.md
├── Cargo.toml
├── LICENSE.md
└── src
    ├── contact.rs
    ├── static_geometry.rs
    ├── rigid_body.rs
    ├── convex_shape.rs
    ├── lib.rs
    └── gjk_epa.rs


/.gitignore:
--------------------------------------------------------------------------------
1 | /target
2 | **/*.rs.bk
3 | Cargo.lock
4 | 


--------------------------------------------------------------------------------
/.idea/.gitignore:
--------------------------------------------------------------------------------
1 | 
2 | # Default ignored files
3 | /workspace.xml


--------------------------------------------------------------------------------
/.idea/vcs.xml:
--------------------------------------------------------------------------------
1 | <?xml version="1.0" encoding="UTF-8"?>
2 | <project version="4">
3 |   <component name="VcsDirectoryMappings">
4 |     <mapping directory="" vcs="Git" />
5 |   </component>
6 | </project>


--------------------------------------------------------------------------------
/README.md:
--------------------------------------------------------------------------------
1 | Contents of this repository was moved to [rg3d/rg3d-physics](https://github.com/mrDIMAS/rg3d/tree/master/rg3d-physics) and development will be continued there.
2 | 
3 | # rg3d-physics
4 | 
5 | Tiny physics library for rg3d-engine.
6 | 


--------------------------------------------------------------------------------
/.idea/modules.xml:
--------------------------------------------------------------------------------
1 | <?xml version="1.0" encoding="UTF-8"?>
2 | <project version="4">
3 |   <component name="ProjectModuleManager">
4 |     <modules>
5 |       <module fileurl="file://$PROJECT_DIR$/.idea/rg3d-physics.iml" filepath="$PROJECT_DIR$/.idea/rg3d-physics.iml" />
6 |     </modules>
7 |   </component>
8 | </project>


--------------------------------------------------------------------------------
/Cargo.toml:
--------------------------------------------------------------------------------
 1 | [package]
 2 | name = "rg3d-physics"
 3 | version = "0.8.0"
 4 | authors = ["Dmitry Stepanov <d1maxa@yandex.ru>"]
 5 | edition = "2018"
 6 | license = "MIT"
 7 | description = "Physics library for rg3d-engine."
 8 | keywords = ["physics", "game"]
 9 | include = ["/src/**/*", "/Cargo.toml", "/LICENSE", "/README.md"]
10 | readme = "README.md"
11 | 
12 | [dependencies]
13 | rg3d-core = { path = "../rg3d-core", version = "0.10.0" }
14 | bitflags = "1.2.1"
15 | 
16 | [features]
17 | enable_profiler = ["rg3d-core/enable_profiler"]


--------------------------------------------------------------------------------
/.idea/rg3d-physics.iml:
--------------------------------------------------------------------------------
 1 | <?xml version="1.0" encoding="UTF-8"?>
 2 | <module type="JAVA_MODULE" version="4">
 3 |   <component name="NewModuleRootManager" inherit-compiler-output="true">
 4 |     <exclude-output />
 5 |     <content url="file://$MODULE_DIR$">
 6 |       <sourceFolder url="file://$MODULE_DIR$/src" isTestSource="false" />
 7 |       <sourceFolder url="file://$MODULE_DIR$/../rg3d-physics\examples" isTestSource="false" />
 8 |       <sourceFolder url="file://$MODULE_DIR$/../rg3d-physics\tests" isTestSource="true" />
 9 |       <sourceFolder url="file://$MODULE_DIR$/../rg3d-physics\benches" isTestSource="true" />
10 |       <excludeFolder url="file://$MODULE_DIR$/target" />
11 |     </content>
12 |     <orderEntry type="inheritedJdk" />
13 |     <orderEntry type="sourceFolder" forTests="false" />
14 |   </component>
15 | </module>


--------------------------------------------------------------------------------
/LICENSE.md:
--------------------------------------------------------------------------------
 1 | 
 2 | The MIT License (MIT)
 3 | 
 4 | Copyright (c) 2019 
 5 | 
 6 | Permission is hereby granted, free of charge, to any person obtaining a copy
 7 | of this software and associated documentation files (the "Software"), to deal
 8 | in the Software without restriction, including without limitation the rights
 9 | to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
10 | copies of the Software, and to permit persons to whom the Software is
11 | furnished to do so, subject to the following conditions:
12 | 
13 | The above copyright notice and this permission notice shall be included in all
14 | copies or substantial portions of the Software.
15 | 
16 | THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
17 | IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
18 | FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
19 | AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
20 | LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
21 | OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
22 | SOFTWARE.
23 | 


--------------------------------------------------------------------------------
/src/contact.rs:
--------------------------------------------------------------------------------
 1 | use rg3d_core::{
 2 |     math::vec3::Vec3,
 3 |     pool::Handle,
 4 |     visitor::{Visit, VisitResult, Visitor}
 5 | };
 6 | use crate::{
 7 |     rigid_body::RigidBody,
 8 |     static_geometry::StaticGeometry
 9 | };
10 | 
11 | #[derive(Debug, Clone)]
12 | pub struct Contact {
13 |     pub body: Handle<RigidBody>,
14 |     pub position: Vec3,
15 |     pub normal: Vec3,
16 |     pub triangle_index: u32,
17 |     pub static_geom: Handle<StaticGeometry>
18 | }
19 | 
20 | impl Default for Contact {
21 |     fn default() -> Self {
22 |         Self {
23 |             body: Handle::NONE,
24 |             position: Vec3::ZERO,
25 |             normal: Vec3::new(0.0, 1.0, 0.0),
26 |             triangle_index: 0,
27 |             static_geom: Handle::NONE,
28 |         }
29 |     }
30 | }
31 | 
32 | impl Visit for Contact {
33 |     fn visit(&mut self, name: &str, visitor: &mut Visitor) -> VisitResult {
34 |         visitor.enter_region(name)?;
35 | 
36 |         self.body.visit("Body", visitor)?;
37 |         self.position.visit("Position", visitor)?;
38 |         self.normal.visit("Normal", visitor)?;
39 |         self.triangle_index.visit("TriangleIndex", visitor)?;
40 | 
41 |         visitor.leave_region()
42 |     }
43 | }


--------------------------------------------------------------------------------
/src/static_geometry.rs:
--------------------------------------------------------------------------------
  1 | use rg3d_core::{
  2 |     math::{
  3 |         vec3::Vec3,
  4 |         plane::Plane
  5 |     },
  6 |     visitor::{Visit, VisitResult, Visitor, VisitError},
  7 |     octree::Octree
  8 | };
  9 | 
 10 | #[derive(Default, Clone, Debug)]
 11 | pub struct StaticGeometry {
 12 |     pub(in crate) triangles: Vec<StaticTriangle>,
 13 |     pub(in crate) octree: Octree,
 14 |     save_triangles: bool
 15 | }
 16 | 
 17 | impl StaticGeometry {
 18 |     pub const OCTREE_THRESHOLD: usize = 64;
 19 | 
 20 |     pub fn new(triangles: Vec<StaticTriangle>, save_triangles: bool) -> Self {
 21 |         let raw_triangles: Vec<[Vec3; 3]> = triangles.iter().map(|st| st.points).collect();
 22 | 
 23 |         Self {
 24 |             octree: Octree::new(&raw_triangles, Self::OCTREE_THRESHOLD),
 25 |             triangles,
 26 |             save_triangles
 27 |         }
 28 |     }
 29 | 
 30 |     pub fn set_save_triangles(&mut self, save_triangles: bool) {
 31 |         self.save_triangles = save_triangles;
 32 |     }
 33 | 
 34 |     pub fn save_triangles(&self) -> bool {
 35 |         self.save_triangles
 36 |     }
 37 | }
 38 | 
 39 | impl Visit for StaticGeometry {
 40 |     fn visit(&mut self, name: &str, visitor: &mut Visitor) -> VisitResult {
 41 |         visitor.enter_region(name)?;
 42 | 
 43 |         if !visitor.is_reading() {
 44 |             if self.save_triangles {
 45 |                 self.triangles.visit("Triangles", visitor)?;
 46 |             } else {
 47 |                 let mut empty: Vec<StaticTriangle> = Vec::new();
 48 |                 empty.visit("Triangles", visitor)?;
 49 |             }
 50 |         } else {
 51 |             self.triangles.visit("Triangles", visitor)?;
 52 |         }
 53 | 
 54 |         if visitor.is_reading() {
 55 |             let raw_triangles: Vec<[Vec3; 3]> = self.triangles.iter().map(|st| st.points).collect();
 56 |             self.octree = Octree::new(&raw_triangles, Self::OCTREE_THRESHOLD);
 57 |         }
 58 | 
 59 |         let _ = self.save_triangles.visit("SaveTriangles", visitor);
 60 | 
 61 |         visitor.leave_region()
 62 |     }
 63 | }
 64 | 
 65 | #[derive(Clone, Debug)]
 66 | pub struct StaticTriangle {
 67 |     pub points: [Vec3; 3],
 68 |     pub ca: Vec3,
 69 |     pub ba: Vec3,
 70 |     pub ca_dot_ca: f32,
 71 |     pub ca_dot_ba: f32,
 72 |     pub ba_dot_ba: f32,
 73 |     pub inv_denom: f32,
 74 |     pub plane: Plane,
 75 | }
 76 | 
 77 | impl Default for StaticTriangle {
 78 |     fn default() -> Self {
 79 |         Self {
 80 |             points: Default::default(),
 81 |             ca: Default::default(),
 82 |             ba: Default::default(),
 83 |             ca_dot_ca: 0.0,
 84 |             ca_dot_ba: 0.0,
 85 |             ba_dot_ba: 0.0,
 86 |             inv_denom: 0.0,
 87 |             plane: Default::default(),
 88 |         }
 89 |     }
 90 | }
 91 | 
 92 | impl Visit for StaticTriangle {
 93 |     fn visit(&mut self, name: &str, visitor: &mut Visitor) -> VisitResult {
 94 |         visitor.enter_region(name)?;
 95 | 
 96 |         let mut a = self.points[0];
 97 |         a.visit("A", visitor)?;
 98 | 
 99 |         let mut b = self.points[1];
100 |         b.visit("B", visitor)?;
101 | 
102 |         let mut c = self.points[2];
103 |         c.visit("C", visitor)?;
104 | 
105 |         *self = match Self::from_points(&a, &b, &c) {
106 |             None => return Err(VisitError::User(String::from("invalid triangle"))),
107 |             Some(triangle) => triangle,
108 |         };
109 | 
110 |         visitor.leave_region()
111 |     }
112 | }
113 | 
114 | impl StaticTriangle {
115 |     ///
116 |     /// Creates static triangle from tree points and precomputes some data
117 |     /// to speedup collision detection in runtime. This function may fail
118 |     /// if degenerated triangle was passed into.
119 |     ///
120 |     pub fn from_points(a: &Vec3, b: &Vec3, c: &Vec3) -> Option<StaticTriangle> {
121 |         let ca = *c - *a;
122 |         let ba = *b - *a;
123 |         let ca_dot_ca = ca.dot(&ca);
124 |         let ca_dot_ba = ca.dot(&ba);
125 |         let ba_dot_ba = ba.dot(&ba);
126 |         if let Ok(plane) = Plane::from_normal_and_point(&ba.cross(&ca), a) {
127 |             return Some(StaticTriangle {
128 |                 points: [*a, *b, *c],
129 |                 ba,
130 |                 ca: *c - *a,
131 |                 ca_dot_ca,
132 |                 ca_dot_ba,
133 |                 ba_dot_ba,
134 |                 inv_denom: 1.0 / (ca_dot_ca * ba_dot_ba - ca_dot_ba * ca_dot_ba),
135 |                 plane,
136 |             });
137 |         }
138 | 
139 |         None
140 |     }
141 | 
142 |     /// Checks if point lies inside or at edge of triangle. Uses a lot of precomputed data.
143 |     pub fn contains_point(&self, p: Vec3) -> bool {
144 |         let vp = p - self.points[0];
145 |         let dot02 = self.ca.dot(&vp);
146 |         let dot12 = self.ba.dot(&vp);
147 |         let u = (self.ba_dot_ba * dot02 - self.ca_dot_ba * dot12) * self.inv_denom;
148 |         let v = (self.ca_dot_ca * dot12 - self.ca_dot_ba * dot02) * self.inv_denom;
149 |         u >= 0.0 && v >= 0.0 && u + v < 1.0
150 |     }
151 | }


--------------------------------------------------------------------------------
/src/rigid_body.rs:
--------------------------------------------------------------------------------
  1 | use rg3d_core::{
  2 |     math::vec3::Vec3,
  3 |     visitor::{Visit, VisitResult, Visitor},
  4 |     pool::Handle
  5 | };
  6 | use crate::{
  7 |     contact::Contact,
  8 |     convex_shape::{ConvexShape, TriangleShape},
  9 |     gjk_epa,
 10 |     static_geometry::{
 11 |         StaticTriangle,
 12 |         StaticGeometry
 13 |     }
 14 | };
 15 | 
 16 | bitflags! {
 17 |     pub struct CollisionFlags: u8 {
 18 |         const NONE = 0;
 19 |         /// Collision response will be disabled but body still will gather contact information.
 20 |         const DISABLE_COLLISION_RESPONSE = 1;
 21 |     }
 22 | }
 23 | 
 24 | #[derive(Debug)]
 25 | pub struct RigidBody {
 26 |     pub(in crate) position: Vec3,
 27 |     pub(in crate) shape: ConvexShape,
 28 |     pub(in crate) last_position: Vec3,
 29 |     pub(in crate) acceleration: Vec3,
 30 |     pub(in crate) contacts: Vec<Contact>,
 31 |     pub(in crate) friction: Vec3,
 32 |     pub(in crate) gravity: Vec3,
 33 |     pub(in crate) speed_limit: f32,
 34 |     pub(in crate) lifetime: Option<f32>,
 35 |     pub user_flags: u64,
 36 |     pub collision_group: u64,
 37 |     pub collision_mask: u64,
 38 |     pub collision_flags: CollisionFlags,
 39 | }
 40 | 
 41 | impl Default for RigidBody {
 42 |     fn default() -> Self {
 43 |         Self::new(ConvexShape::Dummy)
 44 |     }
 45 | }
 46 | 
 47 | impl Visit for RigidBody {
 48 |     fn visit(&mut self, name: &str, visitor: &mut Visitor) -> VisitResult {
 49 |         visitor.enter_region(name)?;
 50 | 
 51 |         let mut id = self.shape.id();
 52 |         id.visit("ShapeKind", visitor)?;
 53 |         if visitor.is_reading() {
 54 |             self.shape = ConvexShape::new(id)?;
 55 |         }
 56 |         self.shape.visit("Shape", visitor)?;
 57 | 
 58 |         self.position.visit("Position", visitor)?;
 59 |         self.last_position.visit("LastPosition", visitor)?;
 60 |         self.acceleration.visit("Acceleration", visitor)?;
 61 |         self.contacts.visit("Contacts", visitor)?;
 62 |         self.friction.visit("Friction", visitor)?;
 63 |         self.gravity.visit("Gravity", visitor)?;
 64 |         self.speed_limit.visit("SpeedLimit", visitor)?;
 65 |         self.user_flags.visit("UserFlags", visitor)?;
 66 |         self.collision_group.visit("CollisionGroup", visitor)?;
 67 |         self.collision_mask.visit("CollisionMask", visitor)?;
 68 | 
 69 |         let mut collision_flags = self.collision_flags.bits;
 70 |         collision_flags.visit("CollisionFlags", visitor)?;
 71 |         if visitor.is_reading() {
 72 |             self.collision_flags = CollisionFlags::from_bits(collision_flags).unwrap();
 73 |         }
 74 | 
 75 |         visitor.leave_region()
 76 |     }
 77 | }
 78 | 
 79 | impl Clone for RigidBody {
 80 |     fn clone(&self) -> Self {
 81 |         Self {
 82 |             position: self.position,
 83 |             last_position: self.last_position,
 84 |             acceleration: self.acceleration,
 85 |             contacts: Vec::new(),
 86 |             friction: self.friction,
 87 |             gravity: self.gravity,
 88 |             shape: self.shape.clone(),
 89 |             speed_limit: self.speed_limit,
 90 |             lifetime: self.lifetime,
 91 |             user_flags: self.user_flags,
 92 |             collision_group: self.collision_group,
 93 |             collision_mask: self.collision_mask,
 94 |             collision_flags: CollisionFlags::NONE
 95 |         }
 96 |     }
 97 | }
 98 | 
 99 | impl RigidBody {
100 |     pub fn new(shape: ConvexShape) -> Self {
101 |         Self {
102 |             position: Vec3::ZERO,
103 |             last_position: Vec3::ZERO,
104 |             acceleration: Vec3::ZERO,
105 |             friction: Vec3::new(0.2, 0.2, 0.2),
106 |             gravity: Vec3::new(0.0, -9.81, 0.0),
107 |             shape,
108 |             contacts: Vec::new(),
109 |             speed_limit: 0.75,
110 |             lifetime: None,
111 |             user_flags: 0,
112 |             collision_group: 1,
113 |             collision_mask: std::u64::MAX,
114 |             collision_flags: CollisionFlags::NONE
115 |         }
116 |     }
117 | 
118 |     #[inline]
119 |     pub fn get_position(&self) -> Vec3 {
120 |         self.position
121 |     }
122 | 
123 |     #[inline]
124 |     pub fn set_position(&mut self, p: Vec3) -> &mut Self {
125 |         self.position = p;
126 |         self.last_position = p;
127 |         self
128 |     }
129 | 
130 |     #[inline]
131 |     pub fn move_by(&mut self, v: Vec3) -> &mut Self {
132 |         self.position += v;
133 |         self
134 |     }
135 | 
136 |     pub fn offset_by(&mut self, v: Vec3) -> &mut Self {
137 |         self.position += v;
138 |         self.last_position = self.position;
139 |         self
140 |     }
141 | 
142 |     #[inline]
143 |     pub fn set_shape(&mut self, shape: ConvexShape) -> &mut Self {
144 |         self.shape = shape;
145 |         self
146 |     }
147 | 
148 |     #[inline]
149 |     pub fn get_shape(&self) -> &ConvexShape {
150 |         &self.shape
151 |     }
152 | 
153 |     #[inline]
154 |     pub fn get_shape_mut(&mut self) -> &mut ConvexShape {
155 |         &mut self.shape
156 |     }
157 | 
158 |     #[inline]
159 |     pub fn set_friction(&mut self, friction: Vec3) -> &mut Self {
160 |         self.friction.x = friction.x.max(0.0).min(1.0);
161 |         self.friction.y = friction.y.max(0.0).min(1.0);
162 |         self.friction.z = friction.z.max(0.0).min(1.0);
163 |         self
164 |     }
165 | 
166 |     #[inline]
167 |     pub fn get_friction(&self) -> Vec3 {
168 |         self.friction
169 |     }
170 | 
171 |     #[inline]
172 |     pub fn set_x_velocity(&mut self, x: f32) -> &mut Self {
173 |         self.last_position.x = self.position.x - x;
174 |         self
175 |     }
176 | 
177 |     #[inline]
178 |     pub fn set_y_velocity(&mut self, y: f32) -> &mut Self {
179 |         self.last_position.y = self.position.y - y;
180 |         self
181 |     }
182 | 
183 |     #[inline]
184 |     pub fn set_z_velocity(&mut self, z: f32) -> &mut Self {
185 |         self.last_position.z = self.position.z - z;
186 |         self
187 |     }
188 | 
189 |     #[inline]
190 |     pub fn set_velocity(&mut self, v: Vec3)  -> &mut Self {
191 |         self.last_position = self.position - v;
192 |         self
193 |     }
194 | 
195 |     #[inline]
196 |     pub fn get_velocity(&self) -> Vec3 {
197 |         self.position - self.last_position
198 |     }
199 | 
200 |     #[inline]
201 |     pub fn get_contacts(&self) -> &[Contact] {
202 |         self.contacts.as_slice()
203 |     }
204 | 
205 |     #[inline]
206 |     pub fn set_gravity(&mut self, gravity: Vec3) -> &mut Self {
207 |         self.gravity = gravity;
208 |         self
209 |     }
210 | 
211 |     #[inline]
212 |     pub fn get_gravity(&self) -> Vec3 {
213 |         self.gravity
214 |     }
215 | 
216 |     #[inline]
217 |     pub fn set_lifetime(&mut self, time_seconds: f32) -> &mut Self {
218 |         self.lifetime = Some(time_seconds);
219 |         self
220 |     }
221 | 
222 |     #[inline]
223 |     pub fn get_lifetime(&self) -> Option<f32> {
224 |         self.lifetime
225 |     }
226 | 
227 |     pub fn verlet(&mut self, sqr_delta_time: f32, air_friction: f32) {
228 |         let friction =
229 |             if !self.collision_flags.contains(CollisionFlags::DISABLE_COLLISION_RESPONSE) && !self.contacts.is_empty() {
230 |                 self.friction
231 |             } else {
232 |                 Vec3::new(air_friction, air_friction, air_friction)
233 |             };
234 | 
235 |         let last_position = self.position;
236 | 
237 |         // Verlet integration
238 |         self.position = Vec3 {
239 |             x: (2.0 - friction.x) * self.position.x - (1.0 - friction.x) * self.last_position.x + self.acceleration.x * sqr_delta_time,
240 |             y: (2.0 - friction.y) * self.position.y - (1.0 - friction.y) * self.last_position.y + self.acceleration.y * sqr_delta_time,
241 |             z: (2.0 - friction.z) * self.position.z - (1.0 - friction.z) * self.last_position.z + self.acceleration.z * sqr_delta_time,
242 |         };
243 | 
244 |         self.last_position = last_position;
245 | 
246 |         self.acceleration = Vec3::ZERO;
247 | 
248 |         let velocity = self.last_position - self.position;
249 |         let sqr_speed = velocity.sqr_len();
250 |         if sqr_speed > self.speed_limit * self.speed_limit {
251 |             if let Some(direction) = velocity.normalized() {
252 |                 self.last_position = self.position - direction.scale(self.speed_limit);
253 |             }
254 |         }
255 |     }
256 | 
257 |     pub fn solve_triangle_collision(&mut self, triangle: &StaticTriangle, triangle_index: usize, static_geom: Handle<StaticGeometry>) {
258 |         let triangle_shape = ConvexShape::Triangle(TriangleShape {
259 |             vertices: triangle.points
260 |         });
261 | 
262 |         if let Some(simplex) = gjk_epa::gjk_is_intersects(&self.shape, self.position, &triangle_shape, Vec3::ZERO) {
263 |             if let Some(penetration_info) = gjk_epa::epa_get_penetration_info(simplex, &self.shape, self.position, &triangle_shape, Vec3::ZERO) {
264 |                 self.position -= penetration_info.penetration_vector;
265 | 
266 |                 self.contacts.push(Contact {
267 |                     static_geom,
268 |                     body: Handle::NONE,
269 |                     position: penetration_info.contact_point,
270 |                     normal: (-penetration_info.penetration_vector).normalized().unwrap_or(Vec3::ZERO),
271 |                     triangle_index: triangle_index as u32,
272 |                 })
273 |             }
274 |         }
275 |     }
276 | 
277 |     pub fn solve_rigid_body_collision(&mut self, self_handle: Handle<RigidBody>, other: &mut Self, other_handle: Handle<RigidBody>) {
278 |         if let Some(simplex) = gjk_epa::gjk_is_intersects(&self.shape, self.position, &other.shape, other.position) {
279 |             if let Some(penetration_info) = gjk_epa::epa_get_penetration_info(simplex, &self.shape, self.position, &other.shape, other.position) {
280 |                 let half_push = penetration_info.penetration_vector.scale(0.5);
281 |                 let response_disabled =
282 |                     self.collision_flags.contains(CollisionFlags::DISABLE_COLLISION_RESPONSE) ||
283 |                     other.collision_flags.contains(CollisionFlags::DISABLE_COLLISION_RESPONSE);
284 | 
285 |                 if !response_disabled {
286 |                     self.position -= half_push;
287 |                 }
288 |                 self.contacts.push(Contact {
289 |                     body: other_handle,
290 |                     position: penetration_info.contact_point,
291 |                     // TODO: WRONG NORMAL
292 |                     normal: (-penetration_info.penetration_vector).normalized().unwrap_or(Vec3::UP),
293 |                     triangle_index: 0,
294 |                     static_geom: Default::default()
295 |                 });
296 |                 if !response_disabled {
297 |                     other.position += half_push;
298 |                 }
299 |                 other.contacts.push(Contact {
300 |                     body: self_handle,
301 |                     position: penetration_info.contact_point,
302 |                     // TODO: WRONG NORMAL
303 |                     normal: (penetration_info.penetration_vector).normalized().unwrap_or(Vec3::UP),
304 |                     triangle_index: 0,
305 |                     static_geom: Default::default()
306 |                 })
307 |             }
308 |         }
309 |     }
310 | }


--------------------------------------------------------------------------------
/src/convex_shape.rs:
--------------------------------------------------------------------------------
  1 | use rg3d_core::{
  2 |     define_is_as,
  3 |     math::{
  4 |         vec3::Vec3,
  5 |         self,
  6 |         aabb::AxisAlignedBoundingBox
  7 |     },
  8 |     visitor::{Visit, VisitResult, Visitor, VisitError},
  9 | };
 10 | 
 11 | #[derive(Clone, Debug)]
 12 | pub struct SphereShape {
 13 |     pub radius: f32
 14 | }
 15 | 
 16 | pub trait CircumRadius {
 17 |     fn circumradius(&self) -> f32;
 18 | }
 19 | 
 20 | impl Visit for SphereShape {
 21 |     fn visit(&mut self, name: &str, visitor: &mut Visitor) -> VisitResult {
 22 |         visitor.enter_region(name)?;
 23 | 
 24 |         self.radius.visit("Radius", visitor)?;
 25 | 
 26 |         visitor.leave_region()
 27 |     }
 28 | }
 29 | 
 30 | impl CircumRadius for SphereShape {
 31 |     fn circumradius(&self) -> f32 {
 32 |         self.radius
 33 |     }
 34 | }
 35 | 
 36 | impl Default for SphereShape {
 37 |     fn default() -> Self {
 38 |         Self {
 39 |             radius: 0.5,
 40 |         }
 41 |     }
 42 | }
 43 | 
 44 | impl SphereShape {
 45 |     pub fn new(radius: f32) -> Self {
 46 |         Self {
 47 |             radius
 48 |         }
 49 |     }
 50 | 
 51 |     pub fn set_radius(&mut self, radius: f32) {
 52 |         self.radius = radius;
 53 |     }
 54 | 
 55 |     pub fn get_radius(&self) -> f32 {
 56 |         self.radius
 57 |     }
 58 | 
 59 |     pub fn get_farthest_point(&self, direction: Vec3) -> Vec3 {
 60 |         let norm_dir = direction.normalized().unwrap_or_else(|| Vec3::new(1.0, 0.0, 0.0));
 61 |         norm_dir.scale(self.radius)
 62 |     }
 63 | }
 64 | 
 65 | #[derive(Clone, Debug)]
 66 | pub struct TriangleShape {
 67 |     pub vertices: [Vec3; 3]
 68 | }
 69 | 
 70 | impl CircumRadius for TriangleShape {
 71 |     fn circumradius(&self) -> f32 {
 72 |         AxisAlignedBoundingBox::from_points(&self.vertices).half_extents().max_value()
 73 |     }
 74 | }
 75 | 
 76 | impl Visit for TriangleShape {
 77 |     fn visit(&mut self, name: &str, visitor: &mut Visitor) -> VisitResult {
 78 |         visitor.enter_region(name)?;
 79 | 
 80 |         self.vertices[0].visit("A", visitor)?;
 81 |         self.vertices[1].visit("B", visitor)?;
 82 |         self.vertices[2].visit("C", visitor)?;
 83 | 
 84 |         visitor.leave_region()
 85 |     }
 86 | }
 87 | 
 88 | impl Default for TriangleShape {
 89 |     fn default() -> Self {
 90 |         Self {
 91 |             vertices: [
 92 |                 Vec3::new(0.0, 0.0, 0.0),
 93 |                 Vec3::new(1.0, 0.0, 0.0),
 94 |                 Vec3::new(0.5, 1.0, 0.0)]
 95 |         }
 96 |     }
 97 | }
 98 | 
 99 | impl TriangleShape {
100 |     pub fn new(vertices: [Vec3; 3]) -> Self {
101 |         Self {
102 |             vertices
103 |         }
104 |     }
105 | 
106 |     pub fn get_normal(&self) -> Option<Vec3> {
107 |         (self.vertices[2] - self.vertices[0]).cross(&(self.vertices[1] - self.vertices[0])).normalized()
108 |     }
109 | 
110 |     pub fn get_farthest_point(&self, direction: Vec3) -> Vec3 {
111 |         math::get_farthest_point(&self.vertices, direction)
112 |     }
113 | }
114 | 
115 | #[derive(Clone, Debug)]
116 | pub struct BoxShape {
117 |     half_extents: Vec3,
118 | }
119 | 
120 | impl CircumRadius for BoxShape {
121 |     fn circumradius(&self) -> f32 {
122 |         self.half_extents.max_value()
123 |     }
124 | }
125 | 
126 | impl Visit for BoxShape {
127 |     fn visit(&mut self, name: &str, visitor: &mut Visitor) -> VisitResult {
128 |         visitor.enter_region(name)?;
129 | 
130 |         self.half_extents.visit("HalfExtents", visitor)?;
131 | 
132 |         visitor.leave_region()
133 |     }
134 | }
135 | 
136 | impl Default for BoxShape {
137 |     fn default() -> Self {
138 |         Self {
139 |             half_extents: Vec3::new(0.5, 0.5, 0.5)
140 |         }
141 |     }
142 | }
143 | 
144 | impl BoxShape {
145 |     pub fn new(half_extents: Vec3) -> Self {
146 |         Self {
147 |             half_extents
148 |         }
149 |     }
150 | 
151 |     pub fn get_farthest_point(&self, direction: Vec3) -> Vec3 {
152 |         Vec3 {
153 |             x: if direction.x >= 0.0 { self.half_extents.x } else { -self.half_extents.x },
154 |             y: if direction.y >= 0.0 { self.half_extents.y } else { -self.half_extents.y },
155 |             z: if direction.z >= 0.0 { self.half_extents.z } else { -self.half_extents.z },
156 |         }
157 |     }
158 | 
159 |     pub fn get_min(&self) -> Vec3 {
160 |         Vec3 {
161 |             x: -self.half_extents.x,
162 |             y: -self.half_extents.y,
163 |             z: -self.half_extents.z,
164 |         }
165 |     }
166 | 
167 |     pub fn get_max(&self) -> Vec3 {
168 |         Vec3 {
169 |             x: self.half_extents.x,
170 |             y: self.half_extents.y,
171 |             z: self.half_extents.z,
172 |         }
173 |     }
174 | }
175 | 
176 | #[derive(Clone, Debug)]
177 | pub struct PointCloudShape {
178 |     points: Vec<Vec3>
179 | }
180 | 
181 | impl CircumRadius for PointCloudShape {
182 |     fn circumradius(&self) -> f32 {
183 |         // TODO: Unoptimal, value should be cached.
184 |         AxisAlignedBoundingBox::from_points(&self.points).half_extents().max_value()
185 |     }
186 | }
187 | 
188 | impl Visit for PointCloudShape {
189 |     fn visit(&mut self, name: &str, visitor: &mut Visitor) -> VisitResult {
190 |         visitor.enter_region(name)?;
191 | 
192 |         self.points.visit("Points", visitor)?;
193 | 
194 |         visitor.leave_region()
195 |     }
196 | }
197 | 
198 | impl Default for PointCloudShape {
199 |     fn default() -> Self {
200 |         Self {
201 |             points: Vec::new(),
202 |         }
203 |     }
204 | }
205 | 
206 | impl PointCloudShape {
207 |     pub fn new(points: Vec<Vec3>) -> Self {
208 |         Self {
209 |             points
210 |         }
211 |     }
212 | 
213 |     pub fn get_farthest_point(&self, direction: Vec3) -> Vec3 {
214 |         math::get_farthest_point(&self.points, direction)
215 |     }
216 | }
217 | 
218 | #[derive(Copy, Clone, Debug)]
219 | pub enum Axis {
220 |     X = 0,
221 |     Y = 1,
222 |     Z = 2,
223 | }
224 | 
225 | impl Visit for Axis {
226 |     fn visit(&mut self, name: &str, visitor: &mut Visitor) -> VisitResult {
227 |         let mut id = *self as u8;
228 |         id.visit(name, visitor)?;
229 |         if visitor.is_reading() {
230 |             *self = match id {
231 |                 0 => Self::X,
232 |                 1 => Self::Y,
233 |                 2 => Self::Z,
234 |                 _ => return Err(VisitError::User("Invalid axis".to_owned()))
235 |             }
236 |         }
237 |         Ok(())
238 |     }
239 | }
240 | 
241 | #[derive(Clone, Debug)]
242 | pub struct CapsuleShape {
243 |     axis: Axis,
244 |     radius: f32,
245 |     height: f32,
246 | }
247 | 
248 | impl CircumRadius for CapsuleShape {
249 |     fn circumradius(&self) -> f32 {
250 |         self.radius.max(self.height)
251 |     }
252 | }
253 | 
254 | impl Default for CapsuleShape {
255 |     fn default() -> Self {
256 |         Self {
257 |             axis: Axis::X,
258 |             radius: 0.0,
259 |             height: 0.0,
260 |         }
261 |     }
262 | }
263 | 
264 | impl Visit for CapsuleShape {
265 |     fn visit(&mut self, name: &str, visitor: &mut Visitor) -> VisitResult {
266 |         visitor.enter_region(name)?;
267 | 
268 |         self.radius.visit("Radius", visitor)?;
269 |         self.axis.visit("Axis", visitor)?;
270 |         self.height.visit("Height", visitor)?;
271 | 
272 |         visitor.leave_region()
273 |     }
274 | }
275 | 
276 | impl CapsuleShape {
277 |     pub fn new(radius: f32, height: f32, axis: Axis) -> Self {
278 |         Self {
279 |             axis,
280 |             radius,
281 |             height
282 |         }
283 |     }
284 | 
285 |     pub fn set_radius(&mut self, radius: f32) {
286 |         self.radius = radius.abs()
287 |     }
288 | 
289 |     pub fn get_radius(&self) -> f32 {
290 |         self.radius
291 |     }
292 | 
293 |     pub fn set_height(&mut self, height: f32) {
294 |         self.height = height.abs()
295 |     }
296 | 
297 |     pub fn get_height(&self) -> f32 {
298 |         self.height
299 |     }
300 | 
301 |     pub fn set_axis(&mut self, axis: Axis) {
302 |         self.axis = axis;
303 |     }
304 | 
305 |     pub fn get_axis(&self) -> Axis {
306 |         self.axis
307 |     }
308 | 
309 |     pub fn get_cap_centers(&self) -> (Vec3, Vec3) {
310 |         let half_height = self.height * 0.5;
311 | 
312 |         match self.axis {
313 |             Axis::X => {
314 |                 (Vec3::new(half_height, 0.0, 0.0),
315 |                  Vec3::new(-half_height, 0.0, 0.0))
316 |             }
317 |             Axis::Y => {
318 |                 (Vec3::new(0.0, half_height, 0.0),
319 |                  Vec3::new(0.0, -half_height, 0.0))
320 |             }
321 |             Axis::Z => {
322 |                 (Vec3::new(0.0, 0.0, half_height),
323 |                  Vec3::new(0.0, 0.0, -half_height))
324 |             }
325 |         }
326 |     }
327 | 
328 |     pub fn get_farthest_point(&self, direction: Vec3) -> Vec3 {
329 |         let norm_dir = direction.normalized().unwrap_or_else(|| Vec3::new(1.0, 0.0, 0.0));
330 |         let half_height = self.height * 0.5;
331 | 
332 |         let positive_cap_position = match self.axis {
333 |             Axis::X => Vec3::new(half_height, 0.0, 0.0),
334 |             Axis::Y => Vec3::new(0.0, half_height, 0.0),
335 |             Axis::Z => Vec3::new(0.0, 0.0, half_height),
336 |         };
337 | 
338 |         let mut max = -std::f32::MAX;
339 |         let mut farthest = Vec3::ZERO;
340 |         for cap_center in [positive_cap_position, -positive_cap_position].iter() {
341 |             let vertex = *cap_center + norm_dir.scale(self.radius);
342 |             let dot = norm_dir.dot(&vertex);
343 |             if dot > max {
344 |                 max = dot;
345 |                 farthest = vertex;
346 |             }
347 |         }
348 | 
349 |         farthest
350 |     }
351 | }
352 | 
353 | #[derive(Clone, Debug)]
354 | pub enum ConvexShape {
355 |     Dummy,
356 |     Box(BoxShape),
357 |     Sphere(SphereShape),
358 |     Capsule(CapsuleShape),
359 |     Triangle(TriangleShape),
360 |     PointCloud(PointCloudShape),
361 | }
362 | 
363 | impl CircumRadius for ConvexShape {
364 |     fn circumradius(&self) -> f32 {
365 |         match self {
366 |             Self::Dummy => 0.0,
367 |             Self::Box(box_shape) => box_shape.circumradius(),
368 |             Self::Sphere(sphere) => sphere.circumradius(),
369 |             Self::Capsule(capsule) => capsule.circumradius(),
370 |             Self::Triangle(triangle) => triangle.circumradius(),
371 |             Self::PointCloud(point_cloud) => point_cloud.circumradius(),
372 |         }
373 |     }
374 | }
375 | 
376 | impl ConvexShape {
377 |     pub fn get_farthest_point(&self, position: Vec3, direction: Vec3) -> Vec3 {
378 |         position + match self {
379 |             Self::Dummy => Vec3::ZERO,
380 |             Self::Box(box_shape) => box_shape.get_farthest_point(direction),
381 |             Self::Sphere(sphere) => sphere.get_farthest_point(direction),
382 |             Self::Capsule(capsule) => capsule.get_farthest_point(direction),
383 |             Self::Triangle(triangle) => triangle.get_farthest_point(direction),
384 |             Self::PointCloud(point_cloud) => point_cloud.get_farthest_point(direction),
385 |         }
386 |     }
387 | 
388 |     pub fn id(&self) -> i32 {
389 |         match self {
390 |             Self::Dummy => 0,
391 |             Self::Box(_) => 1,
392 |             Self::Sphere(_) => 2,
393 |             Self::Capsule(_) => 3,
394 |             Self::Triangle(_) => 4,
395 |             Self::PointCloud(_) => 5,
396 |         }
397 |     }
398 | 
399 |     pub fn new(id: i32) -> Result<Self, String> {
400 |         match id {
401 |             0 => Ok(Self::Dummy),
402 |             1 => Ok(Self::Box(Default::default())),
403 |             2 => Ok(Self::Sphere(Default::default())),
404 |             3 => Ok(Self::Capsule(Default::default())),
405 |             4 => Ok(Self::Triangle(Default::default())),
406 |             5 => Ok(Self::PointCloud(Default::default())),
407 |             _ => Err("Invalid shape id!".to_owned())
408 |         }
409 |     }
410 | 
411 |     define_is_as!(ConvexShape : Box -> ref BoxShape => fn is_box, fn as_box, fn as_box_mut);
412 |     define_is_as!(ConvexShape : Capsule -> ref CapsuleShape => fn is_capsule, fn as_capsule, fn as_capsule_mut);
413 |     define_is_as!(ConvexShape : Sphere -> ref SphereShape => fn is_sphere, fn as_sphere, fn as_sphere_mut);
414 |     define_is_as!(ConvexShape : Triangle -> ref TriangleShape => fn is_triangle, fn as_triangle, fn as_triangle_mut);
415 |     define_is_as!(ConvexShape : PointCloud -> ref PointCloudShape => fn is_point_cloud, fn as_point_cloud, fn as_point_cloud_mut);
416 | }
417 | 
418 | impl Visit for ConvexShape {
419 |     fn visit(&mut self, name: &str, visitor: &mut Visitor) -> VisitResult {
420 |         match self {
421 |             Self::Dummy => Ok(()),
422 |             Self::Box(box_shape) => box_shape.visit(name, visitor),
423 |             Self::Sphere(sphere) => sphere.visit(name, visitor),
424 |             Self::Capsule(capsule) => capsule.visit(name, visitor),
425 |             Self::Triangle(triangle) => triangle.visit(name, visitor),
426 |             Self::PointCloud(point_cloud) => point_cloud.visit(name, visitor),
427 |         }
428 |     }
429 | }
430 | 
431 | 
432 | 


--------------------------------------------------------------------------------
/src/lib.rs:
--------------------------------------------------------------------------------
  1 | extern crate rg3d_core;
  2 | #[macro_use]
  3 | extern crate bitflags;
  4 | 
  5 | use 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 | };
 20 | use std::{
 21 |     cmp::Ordering,
 22 |     cell::RefCell
 23 | };
 24 | use crate::{
 25 |     rigid_body::RigidBody,
 26 |     static_geometry::StaticGeometry,
 27 |     convex_shape::{
 28 |         ConvexShape,
 29 |         CircumRadius
 30 |     }
 31 | };
 32 | use rg3d_core::pool::Ticket;
 33 | 
 34 | pub mod gjk_epa;
 35 | pub mod convex_shape;
 36 | pub mod rigid_body;
 37 | pub mod contact;
 38 | pub mod static_geometry;
 39 | 
 40 | pub enum HitKind {
 41 |     Body(Handle<RigidBody>),
 42 |     StaticTriangle {
 43 |         static_geometry: Handle<StaticGeometry>,
 44 |         triangle_index: usize,
 45 |     },
 46 | }
 47 | 
 48 | pub struct RayCastOptions {
 49 |     pub ignore_bodies: bool,
 50 |     pub ignore_static_geometries: bool,
 51 |     pub sort_results: bool,
 52 | }
 53 | 
 54 | impl 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 | 
 64 | pub struct RayCastResult {
 65 |     pub kind: HitKind,
 66 |     pub position: Vec3,
 67 |     pub normal: Vec3,
 68 |     pub sqr_distance: f32,
 69 | }
 70 | 
 71 | #[derive(Debug)]
 72 | pub struct Physics {
 73 |     bodies: Pool<RigidBody>,
 74 |     static_geoms: Pool<StaticGeometry>,
 75 |     query_buffer: RefCell<Vec<u32>>,
 76 |     enabled: bool
 77 | }
 78 | 
 79 | impl 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 | 
 91 | impl Default for Physics {
 92 |     fn default() -> Self {
 93 |         Self::new()
 94 |     }
 95 | }
 96 | 
 97 | impl 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 | 
108 | impl 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 | }
350 | 


--------------------------------------------------------------------------------
/src/gjk_epa.rs:
--------------------------------------------------------------------------------
  1 | /// Gilbert-Johnson-Keerthi (GJK) intersection test +  Expanding Polytope
  2 | /// Algorithm (EPA) implementations.
  3 | ///
  4 | /// "Implementing GJK", Casey Muratori:
  5 | /// https://www.youtube.com/watch?v=Qupqu1xe7Io
  6 | ///
  7 | /// "GJK + Expanding Polytope Algorithm - Implementation and Visualization"
  8 | /// https://www.youtube.com/watch?v=6rgiPrzqt9w
  9 | ///
 10 | /// Some ideas about contact point generation were taken from here
 11 | /// https://www.gamedev.net/forums/topic/598678-contact-points-with-epa/
 12 | 
 13 | use rg3d_core::{
 14 |     math::vec3::Vec3,
 15 |     math,
 16 | };
 17 | use crate::convex_shape::ConvexShape;
 18 | 
 19 | pub const GJK_MAX_ITERATIONS: usize = 64;
 20 | pub const EPA_TOLERANCE: f32 = 0.0001;
 21 | pub const EPA_MAX_ITERATIONS: usize = 64;
 22 | pub const EPA_MAX_LOOSE_EDGES: usize = 32;
 23 | pub const EPA_MAX_FACES: usize = 64;
 24 | 
 25 | /// Vertex in space of Minkowski sum
 26 | #[derive(Copy, Clone)]
 27 | pub struct MinkowskiVertex {
 28 |     /// World space position of vertex of shape A. This position will be used
 29 |     /// to compute contact point in world space.
 30 |     shape_a_world_space: Vec3,
 31 | 
 32 |     /// Minkowski difference between point in shape A and point in shape B.
 33 |     /// This position will be used to do all simplex and polytope operations.
 34 |     /// https://en.wikipedia.org/wiki/Minkowski_addition
 35 |     minkowski_dif: Vec3,
 36 | }
 37 | 
 38 | impl Default for MinkowskiVertex {
 39 |     fn default() -> Self {
 40 |         Self {
 41 |             shape_a_world_space: Default::default(),
 42 |             minkowski_dif: Default::default(),
 43 |         }
 44 |     }
 45 | }
 46 | 
 47 | #[derive(Copy, Clone)]
 48 | struct PolytopeTriangle {
 49 |     vertices: [MinkowskiVertex; 3],
 50 |     normal: Vec3,
 51 | }
 52 | 
 53 | impl Default for PolytopeTriangle {
 54 |     fn default() -> Self {
 55 |         Self {
 56 |             vertices: Default::default(),
 57 |             normal: Default::default(),
 58 |         }
 59 |     }
 60 | }
 61 | 
 62 | #[derive(Copy, Clone)]
 63 | struct PolytopeEdge {
 64 |     begin: MinkowskiVertex,
 65 |     end: MinkowskiVertex,
 66 | }
 67 | 
 68 | impl Default for PolytopeEdge {
 69 |     fn default() -> Self {
 70 |         Self {
 71 |             begin: Default::default(),
 72 |             end: Default::default(),
 73 |         }
 74 |     }
 75 | }
 76 | 
 77 | impl PolytopeEdge {
 78 |     /// Returns true if edges are equal in combination like this:
 79 |     /// ab = ba
 80 |     fn eq_ccw(&self, other: &PolytopeEdge) -> bool {
 81 |         self.begin.minkowski_dif == other.end.minkowski_dif &&
 82 |             self.end.minkowski_dif == other.begin.minkowski_dif
 83 |     }
 84 | }
 85 | 
 86 | pub struct Simplex {
 87 |     /// Vertices of simplex.
 88 |     /// Important: a is most recently added point (closest to origin)!
 89 |     a: MinkowskiVertex,
 90 |     b: MinkowskiVertex,
 91 |     c: MinkowskiVertex,
 92 |     d: MinkowskiVertex,
 93 |     /// Rank of simplex
 94 |     /// 1 - point
 95 |     /// 2 - line
 96 |     /// 3 - triangle
 97 |     /// 4 - tetrahedron
 98 |     rank: usize,
 99 | }
100 | 
101 | impl Default for Simplex {
102 |     fn default() -> Self {
103 |         Self {
104 |             a: Default::default(),
105 |             b: Default::default(),
106 |             c: Default::default(),
107 |             d: Default::default(),
108 |             rank: 0,
109 |         }
110 |     }
111 | }
112 | 
113 | impl Simplex {
114 |     fn update_triangle(&mut self) -> Vec3 {
115 |         let ca = self.c.minkowski_dif - self.a.minkowski_dif;
116 |         let ba = self.b.minkowski_dif - self.a.minkowski_dif;
117 | 
118 |         // Direction to origin
119 |         let ao = -self.a.minkowski_dif;
120 | 
121 |         self.rank = 2;
122 | 
123 |         let triangle_normal = ba.cross(&ca);
124 | 
125 |         if ba.cross(&triangle_normal).is_same_direction_as(&ao) {
126 |             // Closest to edge AB
127 |             self.c = self.a;
128 |             return ba.cross(&ao).cross(&ba);
129 |         }
130 | 
131 |         if triangle_normal.cross(&ca).is_same_direction_as(&ao) {
132 |             // Closest to edge AC
133 |             self.b = self.a;
134 |             return ca.cross(&ao).cross(&ca);
135 |         }
136 | 
137 |         self.rank = 3;
138 | 
139 |         if triangle_normal.is_same_direction_as(&ao) {
140 |             // Above triangle
141 |             self.d = self.c;
142 |             self.c = self.b;
143 |             self.b = self.a;
144 |             return triangle_normal;
145 |         }
146 | 
147 |         // Below triangle
148 |         self.d = self.b;
149 |         self.b = self.a;
150 | 
151 |         -triangle_normal
152 |     }
153 | 
154 |     fn update_tetrahedron(&mut self) -> Result<(), Vec3> {
155 |         // Point a is tip of pyramid, BCD is the base (counterclockwise winding order)
156 | 
157 |         // Direction to origin
158 |         let ao = -self.a.minkowski_dif;
159 | 
160 |         // Plane-test origin with 3 faces. This is very inaccurate approach and
161 |         // it would be better to add additional checks for each face of tetrahedron
162 |         // to select search direction more precisely. In this case we assume that
163 |         //we always will produce triangles as final simplex.
164 |         self.rank = 3;
165 | 
166 |         let ba = self.b.minkowski_dif - self.a.minkowski_dif;
167 |         let ca = self.c.minkowski_dif - self.a.minkowski_dif;
168 | 
169 |         let abc_normal = ba.cross(&ca);
170 |         if abc_normal.is_same_direction_as(&ao) {
171 |             // In front of ABC
172 |             self.d = self.c;
173 |             self.c = self.b;
174 |             self.b = self.a;
175 |             return Err(abc_normal);
176 |         }
177 | 
178 |         let da = self.d.minkowski_dif - self.a.minkowski_dif;
179 |         let acd_normal = ca.cross(&da);
180 |         if acd_normal.is_same_direction_as(&ao) {
181 |             // In front of ACD
182 |             self.b = self.a;
183 |             return Err(acd_normal);
184 |         }
185 | 
186 |         let adb_normal = da.cross(&ba);
187 |         if adb_normal.is_same_direction_as(&ao) {
188 |             // In front of ADB
189 |             self.c = self.d;
190 |             self.d = self.b;
191 |             self.b = self.a;
192 |             return Err(adb_normal);
193 |         }
194 | 
195 |         // Otherwise origin is inside tetrahedron
196 |         Ok(())
197 |     }
198 | }
199 | 
200 | fn de_gjk_support(shape1: &ConvexShape, shape1_position: Vec3, shape2: &ConvexShape, shape2_position: Vec3, dir: &Vec3) -> MinkowskiVertex
201 | {
202 |     let shape_a_world_space = shape1.get_farthest_point(shape1_position, *dir);
203 |     let b = shape2.get_farthest_point(shape2_position, -*dir);
204 | 
205 |     MinkowskiVertex {
206 |         shape_a_world_space,
207 |         minkowski_dif: shape_a_world_space - b,
208 |     }
209 | }
210 | 
211 | pub fn gjk_is_intersects(shape1: &ConvexShape, shape1_position: Vec3, shape2: &ConvexShape, shape2_position: Vec3) -> Option<Simplex> {
212 |     // This is good enough heuristic to choose initial search direction
213 |     let mut search_dir = shape1_position - shape2_position;
214 | 
215 |     if search_dir.sqr_len() == 0.0 {
216 |         search_dir.x = 1.0;
217 |     }
218 | 
219 |     // Get initial point for simplex
220 |     let mut simplex = Simplex::default();
221 |     simplex.c = de_gjk_support(shape1, shape1_position, shape2, shape2_position, &search_dir);
222 |     search_dir = -simplex.c.minkowski_dif; // Search in direction of origin
223 | 
224 |     // Get second point for a line segment simplex
225 |     simplex.b = de_gjk_support(shape1, shape1_position, shape2, shape2_position, &search_dir);
226 | 
227 |     if !simplex.b.minkowski_dif.is_same_direction_as(&search_dir) {
228 |         return None;
229 |     }
230 | 
231 |     let cb = simplex.c.minkowski_dif - simplex.b.minkowski_dif;
232 | 
233 |     // Search perpendicular to line segment towards origin
234 |     search_dir = cb.cross(&(-simplex.b.minkowski_dif)).cross(&cb);
235 | 
236 |     // Origin is on this line segment - fix search direction.
237 |     if search_dir.sqr_len() == 0.0 {
238 |         // Perpendicular with x-axis
239 |         search_dir = cb.cross(&Vec3::new(1.0, 0.0, 0.0));
240 |         if search_dir.sqr_len() == 0.0 {
241 |             // Perpendicular with z-axis
242 |             search_dir = cb.cross(&Vec3::new(0.0, 0.0, -1.0));
243 |         }
244 |     }
245 | 
246 |     simplex.rank = 2;
247 |     for _ in 0..GJK_MAX_ITERATIONS {
248 |         simplex.a = de_gjk_support(shape1, shape1_position, shape2, shape2_position, &search_dir);
249 | 
250 |         if !simplex.a.minkowski_dif.is_same_direction_as(&search_dir) {
251 |             return None;
252 |         }
253 | 
254 |         simplex.rank += 1;
255 |         if simplex.rank == 3 {
256 |             search_dir = simplex.update_triangle();
257 |         } else {
258 |             match simplex.update_tetrahedron() {
259 |                 Ok(_) => return Some(simplex),
260 |                 Err(dir) => search_dir = dir,
261 |             }
262 |         }
263 |     }
264 | 
265 |     // No convergence - no intersection
266 |     None
267 | }
268 | 
269 | fn epa_compute_contact_point(closest_triangle: PolytopeTriangle) -> Vec3 {
270 |     // Project origin onto triangle's plane
271 |     let proj = closest_triangle.normal.scale(
272 |         closest_triangle.vertices[0].minkowski_dif.dot(&closest_triangle.normal));
273 | 
274 |     // Find barycentric coordinates of the projection in Minkowski difference space
275 |     let (u, v, w) = math::get_barycentric_coords(
276 |         &proj, &closest_triangle.vertices[0].minkowski_dif,
277 |         &closest_triangle.vertices[1].minkowski_dif,
278 |         &closest_triangle.vertices[2].minkowski_dif);
279 | 
280 |     // Use barycentric coordinates to get projection in world space and sum all
281 |     // vectors to get world space contact point
282 |     closest_triangle.vertices[0].shape_a_world_space.scale(u) +
283 |         closest_triangle.vertices[1].shape_a_world_space.scale(v) +
284 |         closest_triangle.vertices[2].shape_a_world_space.scale(w)
285 | }
286 | 
287 | pub struct PenetrationInfo {
288 |     pub penetration_vector: Vec3,
289 |     pub contact_point: Vec3,
290 | }
291 | 
292 | pub fn epa_get_penetration_info(simplex: Simplex, shape1: &ConvexShape, shape1_position: Vec3,
293 |                                 shape2: &ConvexShape, shape2_position: Vec3) -> Option<PenetrationInfo> {
294 |     let mut triangles = [PolytopeTriangle::default(); EPA_MAX_FACES];
295 | 
296 |     // Reconstruct polytope from tetrahedron simplex points.
297 | 
298 |     // ABC
299 |     let ba = simplex.b.minkowski_dif - simplex.a.minkowski_dif;
300 |     let ca = simplex.c.minkowski_dif - simplex.a.minkowski_dif;
301 | 
302 |     let abc_normal = ba.cross(&ca);
303 |     if let Some(abc_normal) = abc_normal.normalized() {
304 |         triangles[0] = PolytopeTriangle {
305 |             vertices: [simplex.a, simplex.b, simplex.c],
306 |             normal: abc_normal,
307 |         };
308 |     } else {
309 |         return None;
310 |     }
311 | 
312 |     // ACD
313 |     let da = simplex.d.minkowski_dif - simplex.a.minkowski_dif;
314 | 
315 |     let acd_normal = ca.cross(&da);
316 |     if let Some(acd_normal) = acd_normal.normalized() {
317 |         triangles[1] = PolytopeTriangle {
318 |             vertices: [simplex.a, simplex.c, simplex.d],
319 |             normal: acd_normal,
320 |         };
321 |     } else {
322 |         return None;
323 |     }
324 | 
325 |     // ADB
326 |     let adb_normal = da.cross(&ba);
327 |     if let Some(adb_normal) = adb_normal.normalized() {
328 |         triangles[2] = PolytopeTriangle {
329 |             vertices: [simplex.a, simplex.d, simplex.b],
330 |             normal: adb_normal,
331 |         };
332 |     } else {
333 |         return None;
334 |     }
335 | 
336 |     // BDC
337 |     let db = simplex.d.minkowski_dif - simplex.b.minkowski_dif;
338 |     let cb = simplex.c.minkowski_dif - simplex.b.minkowski_dif;
339 | 
340 |     let bdc_normal = db.cross(&cb);
341 |     if let Some(bdc_normal) = bdc_normal.normalized() {
342 |         triangles[3] = PolytopeTriangle {
343 |             vertices: [simplex.b, simplex.d, simplex.c],
344 |             normal: bdc_normal,
345 |         };
346 |     } else {
347 |         return None;
348 |     }
349 | 
350 |     let mut triangle_count = 4;
351 |     let mut closest_triangle_index = 0;
352 | 
353 |     for _ in 0..EPA_MAX_ITERATIONS {
354 |         // Find triangle that is closest to origin
355 |         let mut min_dist = triangles[0].vertices[0].minkowski_dif.dot(&triangles[0].normal);
356 |         closest_triangle_index = 0;
357 |         for (i, triangle) in triangles.iter().enumerate().take(triangle_count).skip(1) {
358 |             let dist = triangle.vertices[0].minkowski_dif.dot(&triangle.normal);
359 |             if dist < min_dist {
360 |                 min_dist = dist;
361 |                 closest_triangle_index = i;
362 |             }
363 |         }
364 | 
365 |         // Search normal to triangle that's closest to origin
366 |         let closest_triangle = triangles[closest_triangle_index];
367 |         let search_dir = closest_triangle.normal;
368 |         let new_point = de_gjk_support(shape1, shape1_position, shape2, shape2_position, &search_dir);
369 | 
370 |         let distance_to_origin = new_point.minkowski_dif.dot(&search_dir);
371 |         if distance_to_origin - min_dist < EPA_TOLERANCE {
372 |             return Some(PenetrationInfo {
373 |                 penetration_vector: closest_triangle.normal.scale(distance_to_origin),
374 |                 contact_point: epa_compute_contact_point(closest_triangle),
375 |             });
376 |         }
377 | 
378 |         // Loose edges after we remove triangle must give us list of edges we have
379 |         // to stitch with new point to keep polytope convex.
380 |         let mut loose_edge_count = 0;
381 |         let mut loose_edges = [PolytopeEdge::default(); EPA_MAX_LOOSE_EDGES];
382 | 
383 |         // Find all triangles that are facing new point and remove them
384 |         let mut i = 0;
385 |         while i < triangle_count {
386 |             let triangle = triangles[i];
387 | 
388 |             // If triangle i faces new point, remove it. Also search for adjacent edges of it
389 |             // and remove them too to maintain loose edge list in correct state (see below).
390 |             let to_new_point = new_point.minkowski_dif - triangle.vertices[0].minkowski_dif;
391 | 
392 |             if triangle.normal.dot(&to_new_point) > 0.0 {
393 |                 for j in 0..3 {
394 |                     let current_edge = PolytopeEdge {
395 |                         begin: triangle.vertices[j],
396 |                         end: triangle.vertices[(j + 1) % 3],
397 |                     };
398 | 
399 |                     let mut already_in_list = false;
400 |                     // Check if current edge is already in list
401 |                     let last_loose_edge = loose_edge_count;
402 |                     for k in 0..last_loose_edge {
403 |                         if loose_edges[k].eq_ccw(&current_edge) {
404 |                             // If we found that current edge is same as other loose edge
405 |                             // but in reverse order, then we need to replace the loose
406 |                             // edge by the last loose edge. Lets see at this drawing and
407 |                             // follow it step-by-step. This is pyramid with tip below A
408 |                             // and bottom BCDE divided into 4 triangles by point A. All
409 |                             // triangles given in CCW order.
410 |                             //
411 |                             //       B
412 |                             //      /|\
413 |                             //     / | \
414 |                             //    /  |  \
415 |                             //   /   |   \
416 |                             //  /    |    \
417 |                             // E-----A-----C
418 |                             //  \    |    /
419 |                             //   \   |   /
420 |                             //    \  |  /
421 |                             //     \ | /
422 |                             //      \|/
423 |                             //       D
424 |                             //
425 |                             // We found that triangles we want to remove are ACB (1), ADC (2),
426 |                             // AED (3), and ABE (4). Lets start removing them from triangle ACB.
427 |                             // Also we have to keep list of loose edges for futher linking with
428 |                             // new point.
429 |                             //
430 |                             // 1. AC, CB, BA - just edges of ACB triangle in CCW order.
431 |                             //
432 |                             // 2. AC, CB, BA, AD, DC, (CA) - we see that CA already in list
433 |                             //                               but in reverse order. Do not add it
434 |                             //                               but move DC onto AC position.
435 |                             //    DC, CB, BA, AD - loose edge list for 2nd triangle
436 |                             //
437 |                             // 3. DC, CB, BA, AD, AE, ED, (DA) - again we already have DA in list as AD
438 |                             //                                   move ED to AD position.
439 |                             //    DC, CB, BA, ED, AE
440 |                             //
441 |                             // 4. DC, CB, BA, ED, AE, (AB) - same AB already here as BA, move AE to BA.
442 |                             //    continue adding rest of edges
443 |                             //    DC, CB, AE, ED, BE, (EA) - EA already here as AE, move BE to AE
444 |                             //
445 |                             //    DC, CB, BE, ED - final list of loose edges which gives us
446 |                             //
447 |                             //       B
448 |                             //      / \
449 |                             //     /   \
450 |                             //    /     \
451 |                             //   /       \
452 |                             //  /         \
453 |                             // E           C
454 |                             //  \         /
455 |                             //   \       /
456 |                             //    \     /
457 |                             //     \   /
458 |                             //      \ /
459 |                             //       D
460 |                             //
461 |                             // Viola! We now have contour which we have to patch using new point.
462 |                             loose_edge_count -= 1;
463 |                             loose_edges.swap(k, loose_edge_count);
464 | 
465 |                             already_in_list = true;
466 |                             break;
467 |                         }
468 |                     }
469 | 
470 |                     if !already_in_list {
471 |                         // Add current edge to list
472 |                         if loose_edge_count >= EPA_MAX_LOOSE_EDGES {
473 |                             break;
474 |                         }
475 |                         loose_edges[loose_edge_count] = current_edge;
476 |                         loose_edge_count += 1;
477 |                     }
478 |                 }
479 | 
480 |                 // Replace current triangle with last in list and discard last, so we will continue
481 |                 // processing last triangle and then next to removed. This will effectively reduce
482 |                 // amount of triangles in polytope.
483 |                 triangle_count -= 1;
484 |                 triangles.swap(i, triangle_count);
485 |             } else {
486 |                 i += 1;
487 |             }
488 |         }
489 | 
490 |         // Reconstruct polytope with new point added
491 |         for loose_edge in loose_edges[0..loose_edge_count].iter() {
492 |             if triangle_count >= EPA_MAX_FACES {
493 |                 break;
494 |             }
495 |             let new_triangle = triangles.get_mut(triangle_count).unwrap();
496 |             new_triangle.vertices = [loose_edge.begin, loose_edge.end, new_point];
497 | 
498 |             let edge_vector = loose_edge.begin.minkowski_dif - loose_edge.end.minkowski_dif;
499 |             let begin_to_point = loose_edge.begin.minkowski_dif - new_point.minkowski_dif;
500 | 
501 |             new_triangle.normal = edge_vector.cross(&begin_to_point).normalized().unwrap_or(Vec3::UP);
502 | 
503 |             // Check for wrong normal to maintain CCW winding
504 |             let bias = 2.0 * std::f32::EPSILON;
505 |             if new_triangle.vertices[0].minkowski_dif.dot(&new_triangle.normal) + bias < 0.0 {
506 |                 // Swap vertices to make CCW winding and flip normal.
507 |                 new_triangle.vertices.swap(0, 1);
508 |                 new_triangle.normal = -new_triangle.normal;
509 |             }
510 |             triangle_count += 1;
511 |         }
512 |     }
513 | 
514 |     // Return most recent closest point - this is still valid result but less accurate than
515 |     // if we would have total convergence.
516 |     let closest_triangle = triangles[closest_triangle_index];
517 |     Some(PenetrationInfo {
518 |         penetration_vector: closest_triangle.normal.scale(closest_triangle.vertices[0].minkowski_dif.dot(&closest_triangle.normal)),
519 |         contact_point: epa_compute_contact_point(closest_triangle),
520 |     })
521 | }


--------------------------------------------------------------------------------