falso
/
botw
mirror of https://github.com/zeldaret/botw.git
1
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

ksys/phys: Add RigidBody velocity calc and warping functions

This commit is contained in:
Léo Lam 2022-01-27 21:56:13 +01:00
parent 82286fa560
commit 10d3f129b0
No known key found for this signature in database
GPG Key ID: 0DF30F9081000741
3 changed files with 228 additions and 29 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

18
data/uking_functions.csv
View File

@ -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

Can't render this file because it is too large.

167
src/KingSystem/Physics/RigidBody/physRigidBody.cpp
View File

@ -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()));
}

72
src/KingSystem/Physics/RigidBody/physRigidBody.h
View File

@ -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;