physx 0.18.0

// Author: Tom Olsson <tom.olsson@embark-studios.com>
// Copyright © 2019, Embark Studios, all rights reserved.
// Created: 17 April 2019

use std::marker::PhantomData;

use crate::{
    actor::{Actor, ActorType},
    articulation_link::ArticulationLink,
    math::{PxTransform, PxVec3},
    rigid_actor::RigidActor,
    rigid_dynamic::RigidDynamic,
    traits::Class,
};

#[rustfmt::skip]
use physx_sys::{
    PxRigidBody,
    PxRigidBody_addForce_mut,
    PxRigidBody_addTorque_mut,
    PxRigidBody_clearForce_mut,
    PxRigidBody_clearTorque_mut,
    PxRigidBody_getAngularDamping,
    PxRigidBody_getAngularVelocity,
    PxRigidBody_getCMassLocalPose,
    PxRigidBody_getInvMass,
    PxRigidBody_getLinearDamping,
    PxRigidBody_getLinearVelocity,
    PxRigidBody_getMass,
    PxRigidBody_getMassSpaceInertiaTensor,
    PxRigidBody_getMassSpaceInvInertiaTensor,
    PxRigidBody_getMaxAngularVelocity,
    PxRigidBody_getMaxContactImpulse,
    PxRigidBody_getMaxDepenetrationVelocity,
    PxRigidBody_getMaxLinearVelocity,
    PxRigidBody_getMinCCDAdvanceCoefficient,
    PxRigidBody_getRigidBodyFlags,
    PxRigidBody_setAngularDamping_mut,
    PxRigidBody_setCMassLocalPose_mut,
    PxRigidBody_setForceAndTorque_mut,
    PxRigidBody_setLinearDamping_mut,
    PxRigidBody_setMassSpaceInertiaTensor_mut,
    PxRigidBody_setMass_mut,
    PxRigidBody_setMaxAngularVelocity_mut,
    PxRigidBody_setMaxContactImpulse_mut,
    PxRigidBody_setMaxDepenetrationVelocity_mut,
    PxRigidBody_setMaxLinearVelocity_mut,
    PxRigidBody_setMinCCDAdvanceCoefficient_mut,
    PxRigidBody_setRigidBodyFlag_mut,
    PxRigidBody_setRigidBodyFlags_mut,
};

pub use physx_sys::{
    PxForceMode as ForceMode, PxRigidBodyFlag as RigidBodyFlag, PxRigidBodyFlags as RigidBodyFlags,
};

impl<T> RigidBody for T where T: Class<PxRigidBody> + RigidActor {}

pub trait RigidBody: Class<PxRigidBody> + RigidActor {
    /// Set the mass of this body
    fn set_mass(&mut self, mass: f32) {
        unsafe {
            PxRigidBody_setMass_mut(self.as_mut_ptr(), mass);
        };
    }

    /// Get the angular velocity.
    #[inline]
    fn get_angular_velocity(&self) -> PxVec3 {
        unsafe { PxRigidBody_getAngularVelocity(self.as_ptr()).into() }
    }

    /// Get the linear velocity.
    #[inline]
    fn get_linear_velocity(&self) -> PxVec3 {
        unsafe { PxRigidBody_getLinearVelocity(self.as_ptr()).into() }
    }

    /// Get the (linear, angular) velocities.
    #[inline]
    fn get_velocities(&self) -> (PxVec3, PxVec3) {
        (self.get_linear_velocity(), self.get_angular_velocity())
    }

    /// Set the pose of the position and orientation of the center of mass.  This does not move tha body,
    /// Setting the center of mass too far away may cause instability.
    fn set_c_mass_local_pose(&mut self, pose: &PxTransform) {
        unsafe { PxRigidBody_setCMassLocalPose_mut(self.as_mut_ptr(), pose.as_ptr()) }
    }

    /// Get the pose of the center of mass.
    fn get_c_mass_local_pose(&self) -> PxTransform {
        unsafe { PxRigidBody_getCMassLocalPose(self.as_ptr()).into() }
    }

    /// Get the mass of the body.
    fn get_mass(&self) -> f32 {
        unsafe { PxRigidBody_getMass(self.as_ptr()) }
    }

    /// Get the inverse of the mass of this body.
    fn get_inv_mass(&self) -> f32 {
        unsafe { PxRigidBody_getInvMass(self.as_ptr()) }
    }

    /// Sets the inertia tensor using mass space coordinates.  Non-diagonal space inertia tensors
    /// must be diagonalized, only the diagonal is passed in.  The tensor elements must be non-negative.
    /// Values of 0 represent infinite inertia along that axis, and are not permitted for `PxArticulationLink`s.
    fn set_mass_space_inertia_tensor(&mut self, m: &PxVec3) {
        unsafe { PxRigidBody_setMassSpaceInertiaTensor_mut(self.as_mut_ptr(), m.as_ptr()) }
    }

    /// Gets the diagonal of the inertia tensor relative to the body's mass coordinate frame.
    /// A value of 0 represents infinite inertia along that axis.
    fn get_mass_space_inertia_tensor(&self) -> PxVec3 {
        unsafe { PxRigidBody_getMassSpaceInertiaTensor(self.as_ptr()).into() }
    }

    /// Gets the diagonal of the inverse inertia tensor relative to the body's mass coordinate frame.
    /// A value of 0 represents infinite inertia along that axis.
    fn get_mass_space_inv_inertia_tensor(&self) -> PxVec3 {
        unsafe { PxRigidBody_getMassSpaceInvInertiaTensor(self.as_ptr()).into() }
    }

    /// Sets the linear damping.  Zero represents no damping.  Values clamped to [0.0 .. ].
    fn set_linear_damping(&mut self, mut lin_damp: f32) {
        if lin_damp.is_sign_negative() {
            lin_damp = 0.0
        };
        unsafe { PxRigidBody_setLinearDamping_mut(self.as_mut_ptr(), lin_damp) }
    }

    /// Gets the linear damping.
    fn get_linear_damping(&self) -> f32 {
        unsafe { PxRigidBody_getLinearDamping(self.as_ptr()) }
    }

    /// Sets the angular damping.  Zero represents no damping.  Values clamped to (0.0, .. ).
    fn set_angular_damping(&mut self, mut ang_damp: f32) {
        if ang_damp.is_sign_negative() {
            ang_damp = 0.0
        };
        unsafe { PxRigidBody_setAngularDamping_mut(self.as_mut_ptr(), ang_damp) }
    }

    /// Get the angular damping.
    fn get_angular_damping(&self) -> f32 {
        unsafe { PxRigidBody_getAngularDamping(self.as_ptr()) }
    }

    /// Set the maximum angular velocity.  Very rapid rotation can cause simulation errors, setting this value
    /// will clamp velocty *before* the solver is ran (so velocty may briefly exceed this value).
    fn set_max_angular_velocity(&mut self, max_ang_vel: f32) {
        unsafe { PxRigidBody_setMaxAngularVelocity_mut(self.as_mut_ptr(), max_ang_vel) }
    }

    /// Get the maximum angular velocity.
    fn get_max_angular_velocity(&self) -> f32 {
        unsafe { PxRigidBody_getMaxAngularVelocity(self.as_ptr()) }
    }

    /// Set the maximum linear velocity.  Very rapid movement can cause simulation errors, setting this value
    /// will clamp velocty *before* the solver is ran (so velocty may briefly exceed this value).
    fn set_max_linear_velocity(&mut self, max_lin_vel: f32) {
        unsafe { PxRigidBody_setMaxLinearVelocity_mut(self.as_mut_ptr(), max_lin_vel) }
    }

    /// Get the maximum linear velocity.
    fn get_max_linear_velocity(&self) -> f32 {
        unsafe { PxRigidBody_getMaxLinearVelocity(self.as_ptr()) }
    }

    /// Apply a force to the actor.  This will not cause torque.  ForceMode determines how this force is applied.
    /// `ForceMode::Acceleration` and `ForceMode::VelocityChange` directly effect the acceleration and velocity change
    /// accumulators.  `ForceMode::Force` and `ForceMode::Impulse` are multiplied by inverse mass first.
    fn add_force(&mut self, force: &PxVec3, mode: ForceMode, autowake: bool) {
        unsafe { PxRigidBody_addForce_mut(self.as_mut_ptr(), force.as_ptr(), mode, autowake) }
    }

    /// Apply a torque to the actor.  ForceMode determines how this force is applied.  `ForceMode::Acceleration`
    /// and `ForceMode::VelocityChange` directly effect the angular acceleration and angular velocity change
    /// accumulators.  `ForceMode::Force` and `ForceMode::Impulse` are multiplied by inverse mass first.
    fn add_torque(&mut self, torque: &PxVec3, mode: ForceMode, autowake: bool) {
        unsafe { PxRigidBody_addTorque_mut(self.as_mut_ptr(), torque.as_ptr(), mode, autowake) }
    }

    /// Clear the accumulated acceleration or velocity change.  `ForceMode::Acceleration` and `ForceMode::Force`
    /// clear the same accumulator, as do `ForceMode::VelocityChange` and `ForceMode::Impulse`.
    fn clear_force(&mut self, mode: ForceMode) {
        unsafe { PxRigidBody_clearForce_mut(self.as_mut_ptr(), mode) }
    }

    /// Clear the accumulated angular acceleration or angular velocity change.  `ForceMode::Acceleration`
    /// and `ForceMode::Force` clear the same accumulator, as do `ForceMode::VelocityChange` and `ForceMode::Impulse`.
    fn clear_torque(&mut self, mode: ForceMode) {
        unsafe { PxRigidBody_clearTorque_mut(self.as_mut_ptr(), mode) }
    }

    /// Set the force and torque accumulators.
    fn set_force_and_torque(&mut self, force: &PxVec3, torque: &PxVec3, mode: ForceMode) {
        unsafe {
            PxRigidBody_setForceAndTorque_mut(
                self.as_mut_ptr(),
                force.as_ptr(),
                torque.as_ptr(),
                mode,
            )
        }
    }

    /// Set a rigid body flag.
    fn set_rigid_body_flag(&mut self, flag: RigidBodyFlag, value: bool) {
        unsafe { PxRigidBody_setRigidBodyFlag_mut(self.as_mut_ptr(), flag, value) }
    }

    /// Set all the rigid body flags.
    fn set_rigid_body_flags(&mut self, flags: RigidBodyFlags) {
        unsafe { PxRigidBody_setRigidBodyFlags_mut(self.as_mut_ptr(), flags) }
    }

    /// Get the rigid body flags.
    fn get_rigid_body_flags(&self) -> RigidBodyFlags {
        unsafe { PxRigidBody_getRigidBodyFlags(self.as_ptr()) }
    }

    /// Set the CCD advance coefficient, clamped to range [0.0 ..= 1.0].  Default is 0.15.  Values near 1.0
    /// ensures that some will be advanced, but objects may slowly drift through eachother.  Values near 0.0
    /// will never drift through eachother, but may "jam" ie. not advance through time during a given CCD pass.
    fn set_min_ccd_advance_coefficient(&mut self, mut advance_coefficient: f32) {
        if advance_coefficient.is_sign_negative() {
            advance_coefficient = 0.0
        } else if advance_coefficient > 1.0 {
            advance_coefficient = 1.0
        };
        unsafe {
            PxRigidBody_setMinCCDAdvanceCoefficient_mut(self.as_mut_ptr(), advance_coefficient)
        }
    }

    /// Get the CCD advance coefficient.
    fn get_min_ccd_advance_coefficient(&self) -> f32 {
        unsafe { PxRigidBody_getMinCCDAdvanceCoefficient(self.as_ptr()) }
    }

    /// sET the maximum allowed depenetration velocity
    fn set_max_depenetration_velocity(&mut self, bias_clamp: f32) {
        unsafe { PxRigidBody_setMaxDepenetrationVelocity_mut(self.as_mut_ptr(), bias_clamp) }
    }

    /// Get the maximum allowed depenetration velocity
    fn get_max_depenetration_velocity(&self) -> f32 {
        unsafe { PxRigidBody_getMaxDepenetrationVelocity(self.as_ptr()) }
    }

    /// Set the max number of contact impulses this body may experience
    fn set_max_contact_impulse(&mut self, max_impulse: f32) {
        unsafe { PxRigidBody_setMaxContactImpulse_mut(self.as_mut_ptr(), max_impulse) }
    }

    /// Get the max number of contact impulses this body may experience
    fn get_max_contact_impulse(&self) -> f32 {
        unsafe { PxRigidBody_getMaxContactImpulse(self.as_ptr()) }
    }
}

/// A wrapper for RigidBody, parametrized by the ArticulationLink and
/// RigidDynamic that may be it's most derived type.  Use `cast_map`
/// or `as_*` to cast to it's most derived type if specialized functionality is needed.
#[repr(transparent)]
pub struct RigidBodyMap<L, D>
where
    L: ArticulationLink,
    D: RigidDynamic,
{
    obj: PxRigidBody,
    phantom_user_data: PhantomData<(L, D)>,
}

unsafe impl<P, L, D> Class<P> for RigidBodyMap<L, D>
where
    PxRigidBody: Class<P>,
    L: ArticulationLink,
    D: RigidDynamic,
{
    fn as_ptr(&self) -> *const P {
        self.obj.as_ptr()
    }

    fn as_mut_ptr(&mut self) -> *mut P {
        self.obj.as_mut_ptr()
    }
}

impl<L, D> RigidBodyMap<L, D>
where
    L: ArticulationLink,
    D: RigidDynamic,
{
    /// Cast to the most-derived type as determined by `get_type`, which returns the ActorType.
    /// Because this does not use `get_concrete_type`, this method does not have the safety
    /// concerns of the other `cast_map`s, and a `try_cast_map` method is unneded.
    pub fn cast_map<'a, Ret, RDFn, ALFn>(
        &'a mut self,
        mut rigid_dynamic_fn: RDFn,
        mut articulation_link_fn: ALFn,
    ) -> Ret
    where
        RDFn: FnMut(&'a mut D) -> Ret,
        ALFn: FnMut(&'a mut L) -> Ret,
    {
        match self.get_type() {
            ActorType::RigidDynamic => {
                rigid_dynamic_fn(unsafe { &mut *(self as *mut _ as *mut D) })
            }
            ActorType::ArticulationLink => {
                articulation_link_fn(unsafe { &mut *(self as *mut _ as *mut L) })
            }
            ActorType::RigidStatic => unreachable!("RigidStatic is not a RigidBody"),
        }
    }

    /// Tries to cast to RigidDynamic.
    pub fn as_rigid_dynamic(&mut self) -> Option<&mut D> {
        match self.get_type() {
            ActorType::RigidDynamic => unsafe { Some(&mut *(self as *mut _ as *mut D)) },
            _ => None,
        }
    }

    /// Tries to cast to ArticulationLink.
    pub fn as_articulation_link(&mut self) -> Option<&mut L> {
        match self.get_type() {
            ActorType::ArticulationLink => unsafe { Some(&mut *(self as *mut _ as *mut L)) },
            _ => None,
        }
    }
}