1
  2
  3
  4
  5
  6
  7
  8
  9
 10
 11
 12
 13
 14
 15
 16
 17
 18
 19
 20
 21
 22
 23
 24
 25
 26
 27
 28
 29
 30
 31
 32
 33
 34
 35
 36
 37
 38
 39
 40
 41
 42
 43
 44
 45
 46
 47
 48
 49
 50
 51
 52
 53
 54
 55
 56
 57
 58
 59
 60
 61
 62
 63
 64
 65
 66
 67
 68
 69
 70
 71
 72
 73
 74
 75
 76
 77
 78
 79
 80
 81
 82
 83
 84
 85
 86
 87
 88
 89
 90
 91
 92
 93
 94
 95
 96
 97
 98
 99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
176
177
178
179
180
181
182
183
184
185
186
187
188
189
190
191
192
193
194
195
196
197
198
199
200
201
202
203
204
205
206
207
208
209
210
211
212
213
214
215
216
217
218
219
220
221
222
223
224
225
226
227
228
229
230
231
232
233
234
235
236
237
238
239
240
241
242
243
244
245
246
247
248
249
250
251
252
253
254
255
256
257
258
259
260
261
262
263
264
265
266
267
268
269
270
271
272
273
274
275
276
277
278
279
280
281
282
283
284
285
286
287
288
289
290
291
292
293
294
295
296
297
298
299
300
301
302
303
304
305
306
307
308
309
310
311
312
313
314
315
316
317
318
319
320
321
322
323
324
325
326
327
328
329
330
331
332
333
334
335
336
337
338
339
340
341
342
343
344
345
346
347
348
349
350
351
352
353
354
355
356
357
358
359
360
361
362
363
364
365
366
367
368
369
370
371
372
373
374
375
376
377
378
379
380
381
382
383
384
385
386
387
388
389
390
391
392
393
394
395
396
397
398
399
400
401
402
403
404
405
406
407
408
409
410
411
412
413
414
415
416
417
418
419
420
421
422
423
424
425
426
427
428
429
430
431
432
433
434
435
436
437
438
439
440
441
442
443
444
445
446
447
448
449
450
451
452
453
454
455
456
457
458
459
460
461
462
463
464
465
466
467
468
469
470
471
472
473
474
475
476
477
478
479
480
481
482
483
484
485
486
487
488
489
490
491
492
493
494
495
496
497
498
499
500
501
502
503
504
505
506
507
508
509
510
511
512
513
514
515
516
517
518
519
520
521
522
523
524
#![allow(missing_docs)] // For downcast.

use downcast_rs::Downcast;

use na::{self, DVectorSlice, DVectorSliceMut, RealField};
use ncollide::interpolation::{
    ConstantLinearVelocityRigidMotion, ConstantVelocityRigidMotion, RigidMotion,
};
use ncollide::shape::DeformationsType;

use crate::math::{Force, ForceType, Inertia, Isometry, Point, Vector, Velocity};

use crate::solver::{ForceDirection, IntegrationParameters};

pub enum BodyPartMotion<N: RealField> {
    RigidLinear(ConstantLinearVelocityRigidMotion<N>),
    RigidNonlinear(ConstantVelocityRigidMotion<N>),
    Static(Isometry<N>),
}

impl<N: RealField> BodyPartMotion<N> {
    pub(crate) fn is_static_or_linear(&self) -> bool {
        match self {
            BodyPartMotion::RigidLinear(_) | BodyPartMotion::Static(_) => true,
            _ => false,
        }
    }

    pub(crate) fn linvel(&self) -> Vector<N> {
        match self {
            BodyPartMotion::RigidLinear(m) => m.velocity,
            BodyPartMotion::RigidNonlinear(m) => m.linvel,
            BodyPartMotion::Static(_m) => Vector::zeros(),
        }
    }
}

impl<N: RealField> RigidMotion<N> for BodyPartMotion<N> {
    fn position_at_time(&self, t: N) -> Isometry<N> {
        match self {
            BodyPartMotion::RigidLinear(m) => m.position_at_time(t),
            BodyPartMotion::RigidNonlinear(m) => m.position_at_time(t),
            BodyPartMotion::Static(m) => m.position_at_time(t),
        }
    }
}

/// The status of a body.
#[derive(Copy, Clone, PartialEq, Eq, Hash, Debug)]
pub enum BodyStatus {
    /// The body is disabled and ignored by the physics engine.
    Disabled,
    /// The body is static and thus cannot move.
    Static,
    /// The body is dynamic and thus can move and is subject to forces.
    Dynamic,
    /// The body is kinematic so its velocity is controlled by the user and it is not affected by forces and constraints.
    Kinematic,
}

/// The activation status of a body.
///
/// This controls whether a body is sleeping or not.
#[derive(Copy, Clone, Debug)]
pub struct ActivationStatus<N: RealField> {
    threshold: Option<N>,
    energy: N,
}

impl<N: RealField> ActivationStatus<N> {
    /// The default amount of energy bellow which a body can be put to sleep by nphysics.
    pub fn default_threshold() -> N {
        na::convert(0.01f64)
    }

    /// Create a new activation status initialised with the default activation threshold and is active.
    pub fn new_active() -> Self {
        ActivationStatus {
            threshold: Some(Self::default_threshold()),
            energy: Self::default_threshold() * na::convert(4.0),
        }
    }

    /// Create a new activation status initialised with the default activation threshold and is inactive.
    pub fn new_inactive() -> Self {
        ActivationStatus {
            threshold: Some(Self::default_threshold()),
            energy: N::zero(),
        }
    }

    /// Retuns `true` if the body is not asleep.
    #[inline]
    pub fn is_active(&self) -> bool {
        !self.energy.is_zero()
    }

    /// The threshold bellow which the body can be put to sleep.
    ///
    /// A value of `None` indicates that the body cannot sleep.
    #[inline]
    pub fn deactivation_threshold(&self) -> Option<N> {
        self.threshold
    }

    /// Set the threshold bellow which the body can be put to sleep.
    ///
    /// A value of `None` prevents the body from sleeping.
    #[inline]
    pub fn set_deactivation_threshold(&mut self, threshold: Option<N>) {
        self.threshold = threshold
    }

    /// The current energy averaged through several frames.
    #[inline]
    pub fn energy(&self) -> N {
        self.energy
    }

    /// Sets the current average energy of the body.
    #[inline]
    pub fn set_energy(&mut self, energy: N) {
        self.energy = energy
    }
}

/// Trait implemented by all bodies supported by nphysics.
pub trait Body<N: RealField>: Downcast + Send + Sync {
    /// Returns `true` if this body is the ground.
    fn is_ground(&self) -> bool {
        false
    }

    /// Update whether this body needs to be waken up after a user-interaction.
    fn update_activation_status(&mut self) {
        if self.update_status().body_needs_wake_up() {
            self.activate()
        }
    }

    fn advance(&mut self, _time_ratio: N) {}

    fn validate_advancement(&mut self) {}

    fn clamp_advancement(&mut self) {}

    fn part_motion(&self, _part_id: usize, _time_origin: N) -> Option<BodyPartMotion<N>> {
        None
    }

    fn step_started(&mut self) {}

    /// Updates the kinematics, e.g., positions and jacobians, of this body.
    fn update_kinematics(&mut self);

    /// Update the dynamics property of this body.
    fn update_dynamics(&mut self, dt: N);

    /// Update the acceleration of this body given the forces it is subject to and the gravity.
    fn update_acceleration(&mut self, gravity: &Vector<N>, parameters: &IntegrationParameters<N>);

    /// Reset the timestep-specific dynamic information of this body.
    fn clear_forces(&mut self);

    /// Clear all the update flags of this body.
    fn clear_update_flags(&mut self);

    /// The flags tracking what modifications were applied to a body.
    fn update_status(&self) -> BodyUpdateStatus;

    /// Applies a generalized displacement to this body.
    fn apply_displacement(&mut self, disp: &[N]);

    /// The status of this body.
    fn status(&self) -> BodyStatus;

    /// Set the status of this body.
    fn set_status(&mut self, status: BodyStatus);

    /// Information regarding activation and deactivation (sleeping) of this body.
    fn activation_status(&self) -> &ActivationStatus<N>;

    /// Sets the energy bellow which this body is put to sleep.
    ///
    /// If set to `None` the body will never sleep.
    fn set_deactivation_threshold(&mut self, threshold: Option<N>);

    /// The number of degrees of freedom of this body.
    fn ndofs(&self) -> usize;

    /// The generalized accelerations at each degree of freedom of this body.
    fn generalized_acceleration(&self) -> DVectorSlice<N>;

    /// The generalized velocities of this body.
    fn generalized_velocity(&self) -> DVectorSlice<N>;

    /// The companion ID of this body.
    fn companion_id(&self) -> usize;

    /// Set the companion ID of this body (may be reinitialized by nphysics).
    fn set_companion_id(&mut self, id: usize);

    /// The mutable generalized velocities of this body.
    fn generalized_velocity_mut(&mut self) -> DVectorSliceMut<N>;

    /// Integrate the position of this body.
    fn integrate(&mut self, parameters: &IntegrationParameters<N>);

    /// Force the activation of this body with the given level of energy.
    fn activate_with_energy(&mut self, energy: N);

    /// Put this body to sleep.
    fn deactivate(&mut self);

    /// A reference to the specified body part.
    fn part(&self, i: usize) -> Option<&dyn BodyPart<N>>;

    /// If this is a deformable body, returns its deformed positions.
    fn deformed_positions(&self) -> Option<(DeformationsType, &[N])>;

    /// If this is a deformable body, returns a mutable reference to its deformed positions.
    fn deformed_positions_mut(&mut self) -> Option<(DeformationsType, &mut [N])>;

    /// Fills all the jacobians (and the jacobians multiplied by the inverse augmented mass matrix) for a
    /// constraint applying a force at the point `center` (relative to the body part's center of mass) and
    /// the direction `dir`.
    ///
    /// If the force is a torque, it is applied at the center of mass of the body part.
    fn fill_constraint_geometry(
        &self,
        part: &dyn BodyPart<N>,
        ndofs: usize, // FIXME: keep this parameter?
        center: &Point<N>,
        dir: &ForceDirection<N>,
        j_id: usize,
        wj_id: usize,
        jacobians: &mut [N],
        inv_r: &mut N,
        ext_vels: Option<&DVectorSlice<N>>,
        out_vel: Option<&mut N>,
    );

    /// Transform the given point expressed in material coordinates to world-space.
    fn world_point_at_material_point(&self, part: &dyn BodyPart<N>, point: &Point<N>) -> Point<N>;

    /// Transform the given point expressed in material coordinates to world-space.
    fn position_at_material_point(&self, part: &dyn BodyPart<N>, point: &Point<N>) -> Isometry<N>;

    /// Transform the given point expressed in material coordinates to world-space.
    fn material_point_at_world_point(&self, part: &dyn BodyPart<N>, point: &Point<N>) -> Point<N>;

    /// Returns `true` if this bodies contains internal constraints that need to be solved.
    fn has_active_internal_constraints(&mut self) -> bool;

    /// Initializes the internal velocity constraints of a body.
    fn setup_internal_velocity_constraints(
        &mut self,
        ext_vels: &DVectorSlice<N>,
        parameters: &IntegrationParameters<N>,
    );

    /// For warmstarting the solver, modifies the delta velocity applied by the internal constraints of this body.
    fn warmstart_internal_velocity_constraints(&mut self, dvels: &mut DVectorSliceMut<N>);

    /// Execute one step for the iterative resolution of this body's internal velocity constraints.
    fn step_solve_internal_velocity_constraints(&mut self, dvels: &mut DVectorSliceMut<N>);

    /// Execute one step for the iterative resolution of this body's internal position constraints.
    fn step_solve_internal_position_constraints(&mut self, parameters: &IntegrationParameters<N>);

    /// Add the given inertia to the local inertia of this body part.
    fn add_local_inertia_and_com(
        &mut self,
        _part_index: usize,
        _com: Point<N>,
        _inertia: Inertia<N>,
    ) {
    } // FIXME: don't auto-impl.

    /// The number of degrees of freedom (DOF) of this body, taking its status into account.
    ///
    /// In particular, this returns 0 for any body with a status different than `BodyStatus::Dynamic`.
    #[inline]
    fn status_dependent_ndofs(&self) -> usize {
        if self.is_dynamic() {
            self.ndofs()
        } else {
            0
        }
    }

    /// The velocity of the specified body part, taking this body status into account.
    ///
    /// This will return a zero velocity for any body with a status different than `BodyStatus::Dynamic`.
    #[inline]
    fn status_dependent_body_part_velocity(&self, part: &dyn BodyPart<N>) -> Velocity<N> {
        if self.is_dynamic() {
            part.velocity()
        } else {
            Velocity::zero()
        }
    }

    /// Check if this body is active.
    #[inline]
    fn is_active(&self) -> bool {
        match self.status() {
            BodyStatus::Dynamic => self.activation_status().is_active(),
            BodyStatus::Kinematic => true,
            BodyStatus::Static => false,
            BodyStatus::Disabled => false,
        }
    }

    /// Whether or not the status of this body is dynamic.
    #[inline]
    fn is_dynamic(&self) -> bool {
        self.status() == BodyStatus::Dynamic
    }

    /// Whether or not the status of this body is kinematic.
    #[inline]
    fn is_kinematic(&self) -> bool {
        self.status() == BodyStatus::Kinematic
    }

    /// Whether or not the status of this body is static.
    #[inline]
    fn is_static(&self) -> bool {
        self.status() == BodyStatus::Static
    }

    /// Force the activation of this body.
    #[inline]
    fn activate(&mut self) {
        if let Some(threshold) = self.activation_status().deactivation_threshold() {
            self.activate_with_energy(threshold * na::convert(2.0));
        }
    }

    /// Whether this body is affected by gravity.
    fn gravity_enabled(&self) -> bool;

    /// Enable or disable gravity for this body.
    fn enable_gravity(&mut self, enabled: bool);

    /// Gets the velocity of the given point of this body.
    fn velocity_at_point(&self, part_id: usize, point: &Point<N>) -> Velocity<N>;

    /*
     * Application of forces/impulses.
     */
    /// Apply a force at the center of mass of a part of this body.
    fn apply_force(
        &mut self,
        part_id: usize,
        force: &Force<N>,
        force_type: ForceType,
        auto_wake_up: bool,
    );

    /// Apply a local force at the center of mass of a part of this body.
    fn apply_local_force(
        &mut self,
        part_id: usize,
        force: &Force<N>,
        force_type: ForceType,
        auto_wake_up: bool,
    );

    /// Apply a force at a given point of a part of this body.
    fn apply_force_at_point(
        &mut self,
        part_id: usize,
        force: &Vector<N>,
        point: &Point<N>,
        force_type: ForceType,
        auto_wake_up: bool,
    );

    /// Apply a local force at a given point of a part of this body.
    fn apply_local_force_at_point(
        &mut self,
        part_id: usize,
        force: &Vector<N>,
        point: &Point<N>,
        force_type: ForceType,
        auto_wake_up: bool,
    );

    /// Apply a force at a given local point of a part of this body.
    fn apply_force_at_local_point(
        &mut self,
        part_id: usize,
        force: &Vector<N>,
        point: &Point<N>,
        force_type: ForceType,
        auto_wake_up: bool,
    );

    /// Apply a local force at a given local point of a part of this body.
    fn apply_local_force_at_local_point(
        &mut self,
        part_id: usize,
        force: &Vector<N>,
        point: &Point<N>,
        force_type: ForceType,
        auto_wake_up: bool,
    );
}

/// Trait implemented by each part of a body supported by nphysics.
pub trait BodyPart<N: RealField>: Downcast + Send + Sync {
    /// Returns `true` if this body part is the ground.
    fn is_ground(&self) -> bool {
        false
    }

    /// The center of mass of this body part.
    fn center_of_mass(&self) -> Point<N>;

    /// The local center of mass of this body part.
    fn local_center_of_mass(&self) -> Point<N>;

    /// The position of this body part wrt. the ground.
    fn position(&self) -> Isometry<N>;

    /// If CCD is enabled, this is the last position known to be tunnelling-free.
    fn safe_position(&self) -> Isometry<N> {
        self.position()
    }

    /// The velocity of this body part.
    fn velocity(&self) -> Velocity<N>;

    /// The world-space inertia of this body part.
    fn inertia(&self) -> Inertia<N>;

    /// The local-space inertia of this body part.
    fn local_inertia(&self) -> Inertia<N>;
}

impl_downcast!(Body<N> where N: RealField);
impl_downcast!(BodyPart<N> where N: RealField);

bitflags! {
    #[derive(Default)]
    struct BodyUpdateStatusFlags: u8 {
        const POSITION_CHANGED = 0b00000001;
        const VELOCITY_CHANGED = 0b00000010;
        const LOCAL_INERTIA_CHANGED = 0b000100;
        const LOCAL_COM_CHANGED = 0b001000;
        const DAMPING_CHANGED = 0b010000;
        const STATUS_CHANGED = 0b100000;
    }
}

macro_rules! bitflags_accessors(
    ($($get_name: ident, $set_name: ident, $variant: ident)*) => {$(
        #[inline]
        pub fn $get_name(&self) -> bool {
            self.0.contains(BodyUpdateStatusFlags::$variant)
        }

        #[inline]
        pub fn $set_name(&mut self, changed: bool) {
            self.0.set(BodyUpdateStatusFlags::$variant, changed)
        }
    )*}
);

#[derive(Copy, Clone, Debug, Hash, PartialEq, Eq)]
pub struct BodyUpdateStatus(BodyUpdateStatusFlags);
impl BodyUpdateStatus {
    #[inline]
    pub fn all() -> Self {
        BodyUpdateStatus(BodyUpdateStatusFlags::all())
    }

    #[inline]
    pub fn empty() -> Self {
        BodyUpdateStatus(BodyUpdateStatusFlags::default())
    }

    bitflags_accessors!(
        position_changed, set_position_changed, POSITION_CHANGED
        velocity_changed, set_velocity_changed, VELOCITY_CHANGED
        local_inertia_changed, set_local_inertia_changed, LOCAL_INERTIA_CHANGED
        local_com_changed, set_local_com_changed, LOCAL_COM_CHANGED
        damping_changed, set_damping_changed, DAMPING_CHANGED
        status_changed, set_status_changed, STATUS_CHANGED
    );

    #[inline]
    pub fn inertia_needs_update(&self) -> bool {
        self.0.intersects(
            BodyUpdateStatusFlags::POSITION_CHANGED
                | BodyUpdateStatusFlags::VELOCITY_CHANGED
                | BodyUpdateStatusFlags::LOCAL_INERTIA_CHANGED
                | BodyUpdateStatusFlags::LOCAL_COM_CHANGED
                | BodyUpdateStatusFlags::DAMPING_CHANGED
                | BodyUpdateStatusFlags::STATUS_CHANGED,
        )
    }

    #[inline]
    pub fn colliders_need_update(&self) -> bool {
        self.position_changed()
    }

    #[inline]
    pub fn body_needs_wake_up(&self) -> bool {
        self.0.intersects(
            BodyUpdateStatusFlags::POSITION_CHANGED
                | BodyUpdateStatusFlags::VELOCITY_CHANGED
                | BodyUpdateStatusFlags::STATUS_CHANGED,
        )
    }

    #[inline]
    pub fn clear(&mut self) {
        self.0 = BodyUpdateStatusFlags::empty()
    }
}