ksys/phys: Add RigidBody velocity calc and warping functions

2 files changed, 219 insertions, 20 deletions

diff --git a/src/KingSystem/Physics/RigidBody/physRigidBody.cpp b/src/KingSystem/Physics/RigidBody/physRigidBody.cpp
index 3db6c834..b4daedec 100644
--- a/src/KingSystem/Physics/RigidBody/physRigidBody.cpp
+++ b/src/KingSystem/Physics/RigidBody/physRigidBody.cpp
@@ -1094,11 +1094,109 @@ void RigidBody::getPointVelocity(sead::Vector3f* velocity, const sead::Vector3f&
     velocity->add(getLinearVelocity());
 }
 
-void RigidBody::computeVelocityForWarping(sead::Vector3f* linear_velocity,
-                                          const sead::Vector3f& target_position,
-                                          bool take_angular_velocity_into_account) {
-    const float factor = getVelocityComputeTimeFactor();
-    const auto hk_target_pos = toHkVec4(target_position);
+void RigidBody::changePositionAndRotation(const sead::Matrix34f& transform, float epsilon) {
+    const float inv_delta_time = getInvDeltaTime();
+
+    sead::Quatf rotation;
+    transform.toQuat(rotation);
+    rotation.normalize();
+
+    sead::Vector3f position;
+    transform.getTranslation(position);
+
+    const auto hk_position = toHkVec4(position);
+    const auto hk_rotation = toHkQuat(rotation);
+
+    hkVector4f linear_velocity, angular_velocity;
+    computeLinearVelocity(&linear_velocity, hk_position, hk_rotation, inv_delta_time);
+    computeAngularVelocity(&angular_velocity, hk_rotation, inv_delta_time);
+
+    mMotionAccessor->setLinearVelocity(linear_velocity, epsilon);
+    mMotionAccessor->setAngularVelocity(angular_velocity, epsilon);
+}
+
+void RigidBody::changePosition(const sead::Vector3f& target_position, bool keep_angular_velocity,
+                               float epsilon) {
+    hkVector4f velocity;
+    computeLinearVelocity(&velocity, target_position, keep_angular_velocity);
+
+    mMotionAccessor->setLinearVelocity(velocity, epsilon);
+
+    if (!keep_angular_velocity)
+        mMotionAccessor->setAngularVelocity(sead::Vector3f::zero, epsilon);
+}
+
+void RigidBody::changeRotation(const sead::Quatf& target_rotation, float epsilon) {
+    hkVector4f velocity;
+    computeAngularVelocity(&velocity, target_rotation);
+    mMotionAccessor->setAngularVelocity(velocity, epsilon);
+}
+
+void RigidBody::changeRotation(const sead::Matrix34f& rotation_matrix, float epsilon) {
+    hkVector4f velocity;
+    sead::Quatf rotation;
+    rotation_matrix.toQuat(rotation);
+    rotation.normalize();
+
+    computeAngularVelocity(&velocity, rotation);
+    mMotionAccessor->setAngularVelocity(velocity, epsilon);
+}
+
+void RigidBody::computeAngularVelocity(hkVector4f* velocity, const hkQuaternionf& target_rotation,
+                                       float inv_delta_time) const {
+    hkQuaternionf hk_current_rot;
+    mMotionAccessor->getRotation(&hk_current_rot);
+
+    hkQuaternionf rotation;
+    rotation.setMulInverse(target_rotation, hk_current_rot);
+    rotation.normalize();
+
+    if (rotation.hasValidAxis()) {
+        rotation.getAxis(*velocity);
+        velocity->mul(inv_delta_time * rotation.getAngleSr());
+    } else {
+        velocity->setZero();
+    }
+}
+
+void RigidBody::computeAngularVelocity(hkVector4f* velocity,
+                                       const sead::Quatf& target_rotation) const {
+    const float inv_delta_time = getInvDeltaTime();
+    const auto hk_target_rot = toHkQuat(target_rotation);
+    computeAngularVelocity(velocity, hk_target_rot, inv_delta_time);
+}
+
+void RigidBody::computeAngularVelocity(sead::Vector3f* velocity,
+                                       const sead::Quatf& target_rotation) const {
+    hkVector4f result;
+    computeAngularVelocity(&result, target_rotation);
+    storeToVec3(velocity, result);
+}
+
+void RigidBody::computeAngularVelocity(sead::Vector3f* velocity,
+                                       const sead::Matrix34f& rotation_matrix) const {
+    sead::Quatf rotation;
+    rotation_matrix.toQuat(rotation);
+    rotation.normalize();
+    computeAngularVelocity(velocity, rotation);
+}
+
+void RigidBody::computeLinearVelocity(hkVector4f* velocity, const hkVector4f& target_position,
+                                      const hkQuaternionf& rotation, float inv_delta_time) const {
+    const auto center_local = toHkVec4(getCenterOfMassInLocal());
+
+    hkVector4f new_center_world;
+    new_center_world._setRotatedDir(rotation, center_local);
+    new_center_world.add(target_position);
+
+    velocity->setSub(new_center_world, toHkVec4(getCenterOfMassInWorld()));
+    velocity->mul(inv_delta_time);
+}
+
+KSYS_ALWAYS_INLINE void RigidBody::computeLinearVelocity(hkVector4f* velocity,
+                                                         const hkVector4f& target_position,
+                                                         bool take_angular_velocity_into_account,
+                                                         float inv_delta_time) const {
     auto hk_current_pos = toHkVec4(getPosition());
 
     if (take_angular_velocity_into_account) {
@@ -1109,24 +1207,69 @@ void RigidBody::computeVelocityForWarping(sead::Vector3f* linear_velocity,
 
             hkVector4f correction;
             correction.setCross(toHkVec4(getAngularVelocity()), rel_pos);
-            correction.mul(1.0f / factor);
+            correction.mul(1.0f / inv_delta_time);
             hk_current_pos.add(correction);
         }
     }
 
+    velocity->setSub(target_position, hk_current_pos);
+    velocity->mul(inv_delta_time);
+}
+
+KSYS_ALWAYS_INLINE void
+RigidBody::computeLinearVelocity(hkVector4f* velocity, const sead::Vector3f& target_position,
+                                 bool take_angular_velocity_into_account) const {
+    const float inv_delta_time = getInvDeltaTime();
+    const auto hk_target_pos = toHkVec4(target_position);
+    computeLinearVelocity(velocity, hk_target_pos, take_angular_velocity_into_account,
+                          inv_delta_time);
+}
+
+void RigidBody::computeLinearVelocity(sead::Vector3f* velocity,
+                                      const sead::Vector3f& target_position,
+                                      bool take_angular_velocity_into_account) const {
     hkVector4f result;
-    result.setSub(hk_target_pos, hk_current_pos);
-    result.mul(factor);
-    storeToVec3(linear_velocity, result);
+    computeLinearVelocity(&result, target_position, take_angular_velocity_into_account);
+    storeToVec3(velocity, result);
+}
+
+void RigidBody::computeVelocities(hkVector4f* linear_velocity, hkVector4f* angular_velocity,
+                                  const hkVector4f& position, const hkQuaternionf& rotation,
+                                  float inv_delta_time) {
+    computeLinearVelocity(linear_velocity, position, rotation, inv_delta_time);
+    computeAngularVelocity(angular_velocity, rotation, inv_delta_time);
 }
 
 void RigidBody::computeVelocities(hkVector4f* linear_velocity, hkVector4f* angular_velocity,
                                   const hkVector4f& position, const hkQuaternionf& rotation) {
-    const float factor = getVelocityComputeTimeFactor();
-    computeVelocities(linear_velocity, angular_velocity, position, rotation, factor);
+    const float inv_delta_time = getInvDeltaTime();
+    computeVelocities(linear_velocity, angular_velocity, position, rotation, inv_delta_time);
+}
+
+void RigidBody::computeVelocities(sead::Vector3f* linear_velocity, sead::Vector3f* angular_velocity,
+                                  const sead::Matrix34f& transform) {
+    const float inv_delta_time = getInvDeltaTime();
+
+    sead::Quatf rotation;
+    transform.toQuat(rotation);
+    rotation.normalize();
+
+    sead::Vector3f position;
+    transform.getTranslation(position);
+
+    const auto hk_position = toHkVec4(position);
+    const auto hk_rotation = toHkQuat(rotation);
+
+    hkVector4f hk_linear_velocity;
+    hkVector4f hk_angular_velocity;
+    computeLinearVelocity(&hk_linear_velocity, hk_position, hk_rotation, inv_delta_time);
+    computeAngularVelocity(&hk_angular_velocity, hk_rotation, inv_delta_time);
+
+    storeToVec3(linear_velocity, hk_linear_velocity);
+    storeToVec3(angular_velocity, hk_angular_velocity);
 }
 
-float RigidBody::getVelocityComputeTimeFactor() const {
+float RigidBody::getInvDeltaTime() const {
     const float time_factor = getTimeFactor();
     return time_factor == 0 ? 0 : (1.f / (time_factor * System::instance()->get64()));
 }
diff --git a/src/KingSystem/Physics/RigidBody/physRigidBody.h b/src/KingSystem/Physics/RigidBody/physRigidBody.h
index de8101e8..7cdd1514 100644
--- a/src/KingSystem/Physics/RigidBody/physRigidBody.h
+++ b/src/KingSystem/Physics/RigidBody/physRigidBody.h
@@ -285,6 +285,8 @@ public:
     void updateMotionTypeRelatedFlags();
     void triggerScheduledMotionTypeChange();
 
+    // region Velocity
+
     bool setLinearVelocity(const sead::Vector3f& velocity, float epsilon = sead::Mathf::epsilon());
     void getLinearVelocity(sead::Vector3f* velocity) const;
     sead::Vector3f getLinearVelocity() const;
@@ -295,16 +297,70 @@ public:
 
     void getPointVelocity(sead::Vector3f* velocity, const sead::Vector3f& point) const;
 
-    /// Compute the linear velocity that would be necessary to instantly warp to the target.
-    void computeVelocityForWarping(sead::Vector3f* linear_velocity,
-                                   const sead::Vector3f& target_position,
-                                   bool take_angular_velocity_into_account);
+    /// Move to the specified position and rotation by changing the linear and angular velocities.
+    ///
+    /// This is less expensive and less error prone than setTransform which may trigger a costly
+    /// broadphase update and result in interpenetration.
+    void changePositionAndRotation(const sead::Matrix34f& transform,
+                                   float epsilon = sead::Mathf::epsilon());
+
+    /// Modify the body's position by changing the linear velocity.
+    /// This is preferable to setting the position directly (see changePositionAndRotation for an
+    /// explanation).
+    void changePosition(const sead::Vector3f& target_position, bool keep_angular_velocity,
+                        float epsilon = sead::Mathf::epsilon());
+
+    /// Modify the body's rotation by changing the angular velocity.
+    /// This is preferable to setting the rotation directly (see changePositionAndRotation for an
+    /// explanation).
+    void changeRotation(const sead::Quatf& target_rotation, float epsilon = sead::Mathf::epsilon());
+    /// Modify the body's rotation by changing the angular velocity.
+    /// This is preferable to setting the rotation directly (see changePositionAndRotation for an
+    /// explanation).
+    void changeRotation(const sead::Matrix34f& rotation_matrix,
+                        float epsilon = sead::Mathf::epsilon());
+
+    /// Compute the angular velocity that would be necessary to instantly reach the target rotation.
+    void computeAngularVelocity(hkVector4f* velocity, const hkQuaternionf& target_rotation,
+                                float inv_delta_time) const;
+    /// Compute the angular velocity that would be necessary to instantly reach the target rotation.
+    void computeAngularVelocity(hkVector4f* velocity, const sead::Quatf& target_rotation) const;
+    /// Compute the angular velocity that would be necessary to instantly reach the target rotation.
+    void computeAngularVelocity(sead::Vector3f* velocity, const sead::Quatf& target_rotation) const;
+    /// Compute the angular velocity that would be necessary to instantly reach the target rotation.
+    void computeAngularVelocity(sead::Vector3f* velocity,
+                                const sead::Matrix34f& rotation_matrix) const;
+
+    /// Compute the linear velocity that would be necessary to instantly reach the target position.
+    void computeLinearVelocity(hkVector4f* velocity, const hkVector4f& target_position,
+                               const hkQuaternionf& rotation, float inv_delta_time) const;
+    /// Compute the linear velocity that would be necessary to instantly reach the target position.
+    void computeLinearVelocity(hkVector4f* velocity, const hkVector4f& target_position,
+                               bool take_angular_velocity_into_account, float inv_delta_time) const;
+    /// Compute the linear velocity that would be necessary to instantly reach the target position.
+    void computeLinearVelocity(hkVector4f* velocity, const sead::Vector3f& target_position,
+                               bool take_angular_velocity_into_account) const;
+    /// Compute the linear velocity that would be necessary to instantly reach the target position.
+    void computeLinearVelocity(sead::Vector3f* velocity, const sead::Vector3f& target_position,
+                               bool take_angular_velocity_into_account) const;
+
+    /// Compute the linear and angular velocities that would be necessary to instantly reach the
+    /// target position and rotation.
     void computeVelocities(hkVector4f* linear_velocity, hkVector4f* angular_velocity,
-                           const hkVector4f& position, const hkQuaternionf& rotation);
-    // 0x0000007100f91780
+                           const hkVector4f& position, const hkQuaternionf& rotation,
+                           float inv_delta_time);
+    /// Compute the linear and angular velocities that would be necessary to instantly reach the
+    /// target position and rotation.
     void computeVelocities(hkVector4f* linear_velocity, hkVector4f* angular_velocity,
-                           const hkVector4f& position, const hkQuaternionf& rotation, float factor);
-    float getVelocityComputeTimeFactor() const;
+                           const hkVector4f& position, const hkQuaternionf& rotation);
+    /// Compute the linear and angular velocities that would be necessary to instantly reach the
+    /// target position and rotation.
+    void computeVelocities(sead::Vector3f* linear_velocity, sead::Vector3f* angular_velocity,
+                           const sead::Matrix34f& transform);
+
+    float getInvDeltaTime() const;
+
+    // endregion
 
     void setCenterOfMassInLocal(const sead::Vector3f& center);
     void getCenterOfMassInLocal(sead::Vector3f* center) const;