rg3d_physics/
rigid_body.rs

1use rg3d_core::{
2    math::vec3::Vec3,
3    visitor::{Visit, VisitResult, Visitor},
4    pool::Handle
5};
6use crate::{
7    contact::Contact,
8    convex_shape::{ConvexShape, TriangleShape},
9    gjk_epa,
10    static_geometry::{
11        StaticTriangle,
12        StaticGeometry
13    }
14};
15
16bitflags! {
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)]
25pub 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
41impl Default for RigidBody {
42    fn default() -> Self {
43        Self::new(ConvexShape::Dummy)
44    }
45}
46
47impl 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
79impl 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
99impl 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}
rg3d_physics/rigid_body.rs

rg3d_physics/
rigid_body.rs
