From 10d3f129b038bb129266b3974644623d0fc2040b Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?L=C3=A9o=20Lam?= Date: Thu, 27 Jan 2022 21:56:13 +0100 Subject: [PATCH] ksys/phys: Add RigidBody velocity calc and warping functions --- data/uking_functions.csv | 18 +- .../Physics/RigidBody/physRigidBody.cpp | 167 ++++++++++++++++-- .../Physics/RigidBody/physRigidBody.h | 72 +++++++- 3 files changed, 228 insertions(+), 29 deletions(-) diff --git a/data/uking_functions.csv b/data/uking_functions.csv index 7d04d4a3..117b9dec 100644 --- a/data/uking_functions.csv +++ b/data/uking_functions.csv @@ -83045,15 +83045,15 @@ Address,Quality,Size,Name 0x0000007100f91218,O,000076,_ZNK4ksys4phys9RigidBody18getAngularVelocityEv 0x0000007100f91264,O,000276,_ZNK4ksys4phys9RigidBody16getPointVelocityEPN4sead7Vector3IfEERKS4_ 0x0000007100f91378,O,000040,_ZNK4ksys4phys9RigidBody22getCenterOfMassInWorldEv -0x0000007100f913a0,U,000992,phys::RigidBody::x_45_moveToHomeMtxMaybe -0x0000007100f91780,U,000672,phys::RigidBody::x_46 -0x0000007100f91a20,U,000580,phys::RigidBody::x_47 -0x0000007100f91c64,U,000536,phys::RigidBody::x_48 -0x0000007100f91e7c,U,000656,phys::RigidBody::x_49 -0x0000007100f9210c,U,000968,phys::RigidBody::x_50 -0x0000007100f924d4,U,000644,phys::RigidBody::x_51 -0x0000007100f92758,U,000528,phys::RigidBody::x_52 -0x0000007100f92968,O,000524,_ZN4ksys4phys9RigidBody25computeVelocityForWarpingEPN4sead7Vector3IfEERKS4_b +0x0000007100f913a0,O,000992,_ZN4ksys4phys9RigidBody25changePositionAndRotationERKN4sead8Matrix34IfEEf +0x0000007100f91780,O,000672,_ZN4ksys4phys9RigidBody17computeVelocitiesEP10hkVector4fS3_RKS2_RK13hkQuaternionff +0x0000007100f91a20,O,000580,_ZN4ksys4phys9RigidBody14changePositionERKN4sead7Vector3IfEEbf +0x0000007100f91c64,O,000536,_ZN4ksys4phys9RigidBody14changeRotationERKN4sead4QuatIfEEf +0x0000007100f91e7c,O,000656,_ZN4ksys4phys9RigidBody14changeRotationERKN4sead8Matrix34IfEEf +0x0000007100f9210c,O,000968,_ZN4ksys4phys9RigidBody17computeVelocitiesEPN4sead7Vector3IfEES5_RKNS2_8Matrix34IfEE +0x0000007100f924d4,O,000644,_ZNK4ksys4phys9RigidBody22computeAngularVelocityEPN4sead7Vector3IfEERKNS2_8Matrix34IfEE +0x0000007100f92758,O,000528,_ZNK4ksys4phys9RigidBody22computeAngularVelocityEPN4sead7Vector3IfEERKNS2_4QuatIfEE +0x0000007100f92968,O,000524,_ZNK4ksys4phys9RigidBody21computeLinearVelocityEPN4sead7Vector3IfEERKS4_b 0x0000007100f92b74,O,000140,_ZN4ksys4phys9RigidBody17computeVelocitiesEP10hkVector4fS3_RKS2_RK13hkQuaternionf 0x0000007100f92c00,O,000128,_ZN4ksys4phys9RigidBody22setCenterOfMassInLocalERKN4sead7Vector3IfEE 0x0000007100f92c80,O,000016,_ZNK4ksys4phys9RigidBody22getCenterOfMassInLocalEPN4sead7Vector3IfEE 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); } -float RigidBody::getVelocityComputeTimeFactor() const { +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::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, + 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); - // 0x0000007100f91780 - void computeVelocities(hkVector4f* linear_velocity, hkVector4f* angular_velocity, - const hkVector4f& position, const hkQuaternionf& rotation, float factor); - float getVelocityComputeTimeFactor() const; + /// 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;