godotengine/godot
1
0
Fork 0
mirror of https://github.com/godotengine/godot.git synced 2025-11-17 14:11:06 +00:00
Code Releases Wiki Activity

Merge pull request #105748 from mihe/jolt/body-pointer

Browse Source 
Remove no-op locking in Jolt Physics module
This commit is contained in:
Thaddeus Crews
2025-05-05 11:24:10 -05:00
parent 0d7b1ba868 2b88477efc
commit e939aefe2f
18 changed files with 253 additions and 926 deletions
Download Patch File Download Diff File Expand all files Collapse all files

13
modules/jolt_physics/joints/jolt_cone_twist_joint_3d.cpp
View File

@@ -348,21 +348,12 @@ void JoltConeTwistJoint3D::rebuild() {
destroy();
JoltSpace3D *space = get_space();
if (space == nullptr) {
return;
}
const JPH::BodyID body_ids[2] = {
body_a != nullptr ? body_a->get_jolt_id() : JPH::BodyID(),
body_b != nullptr ? body_b->get_jolt_id() : JPH::BodyID()
};
const JoltWritableBodies3D jolt_bodies = space->write_bodies(body_ids, 2);
JPH::Body *jolt_body_a = static_cast<JPH::Body *>(jolt_bodies[0]);
JPH::Body *jolt_body_b = static_cast<JPH::Body *>(jolt_bodies[1]);
JPH::Body *jolt_body_a = body_a != nullptr ? body_a->get_jolt_body() : nullptr;
JPH::Body *jolt_body_b = body_b != nullptr ? body_b->get_jolt_body() : nullptr;
ERR_FAIL_COND(jolt_body_a == nullptr && jolt_body_b == nullptr);
Transform3D shifted_ref_a;

12
modules/jolt_physics/joints/jolt_generic_6dof_joint_3d.cpp
View File

@@ -656,16 +656,8 @@ void JoltGeneric6DOFJoint3D::rebuild() {
return;
}
const JPH::BodyID body_ids[2] = {
body_a != nullptr ? body_a->get_jolt_id() : JPH::BodyID(),
body_b != nullptr ? body_b->get_jolt_id() : JPH::BodyID()
};
const JoltWritableBodies3D jolt_bodies = space->write_bodies(body_ids, 2);
JPH::Body *jolt_body_a = static_cast<JPH::Body *>(jolt_bodies[0]);
JPH::Body *jolt_body_b = static_cast<JPH::Body *>(jolt_bodies[1]);
JPH::Body *jolt_body_a = body_a != nullptr ? body_a->get_jolt_body() : nullptr;
JPH::Body *jolt_body_b = body_b != nullptr ? body_b->get_jolt_body() : nullptr;
ERR_FAIL_COND(jolt_body_a == nullptr && jolt_body_b == nullptr);
Transform3D shifted_ref_a;

13
modules/jolt_physics/joints/jolt_hinge_joint_3d.cpp
View File

@@ -381,21 +381,12 @@ void JoltHingeJoint3D::rebuild() {
destroy();
JoltSpace3D *space = get_space();
if (space == nullptr) {
return;
}
const JPH::BodyID body_ids[2] = {
body_a != nullptr ? body_a->get_jolt_id() : JPH::BodyID(),
body_b != nullptr ? body_b->get_jolt_id() : JPH::BodyID()
};
const JoltWritableBodies3D jolt_bodies = space->write_bodies(body_ids, 2);
JPH::Body *jolt_body_a = static_cast<JPH::Body *>(jolt_bodies[0]);
JPH::Body *jolt_body_b = static_cast<JPH::Body *>(jolt_bodies[1]);
JPH::Body *jolt_body_a = body_a != nullptr ? body_a->get_jolt_body() : nullptr;
JPH::Body *jolt_body_b = body_b != nullptr ? body_b->get_jolt_body() : nullptr;
ERR_FAIL_COND(jolt_body_a == nullptr && jolt_body_b == nullptr);
float ref_shift = 0.0f;

13
modules/jolt_physics/joints/jolt_pin_joint_3d.cpp
View File

@@ -138,21 +138,12 @@ void JoltPinJoint3D::rebuild() {
destroy();
JoltSpace3D *space = get_space();
if (space == nullptr) {
return;
}
const JPH::BodyID body_ids[2] = {
body_a != nullptr ? body_a->get_jolt_id() : JPH::BodyID(),
body_b != nullptr ? body_b->get_jolt_id() : JPH::BodyID()
};
const JoltWritableBodies3D jolt_bodies = space->write_bodies(body_ids, 2);
JPH::Body *jolt_body_a = static_cast<JPH::Body *>(jolt_bodies[0]);
JPH::Body *jolt_body_b = static_cast<JPH::Body *>(jolt_bodies[1]);
JPH::Body *jolt_body_a = body_a != nullptr ? body_a->get_jolt_body() : nullptr;
JPH::Body *jolt_body_b = body_b != nullptr ? body_b->get_jolt_body() : nullptr;
ERR_FAIL_COND(jolt_body_a == nullptr && jolt_body_b == nullptr);
Transform3D shifted_ref_a;

13
modules/jolt_physics/joints/jolt_slider_joint_3d.cpp
View File

@@ -494,21 +494,12 @@ void JoltSliderJoint3D::rebuild() {
destroy();
JoltSpace3D *space = get_space();
if (space == nullptr) {
return;
}
const JPH::BodyID body_ids[2] = {
body_a != nullptr ? body_a->get_jolt_id() : JPH::BodyID(),
body_b != nullptr ? body_b->get_jolt_id() : JPH::BodyID()
};
const JoltWritableBodies3D jolt_bodies = space->write_bodies(body_ids, 2);
JPH::Body *jolt_body_a = static_cast<JPH::Body *>(jolt_bodies[0]);
JPH::Body *jolt_body_b = static_cast<JPH::Body *>(jolt_bodies[1]);
JPH::Body *jolt_body_a = body_a != nullptr ? body_a->get_jolt_body() : nullptr;
JPH::Body *jolt_body_b = body_b != nullptr ? body_b->get_jolt_body() : nullptr;
ERR_FAIL_COND(jolt_body_a == nullptr && jolt_body_b == nullptr);
float ref_shift = 0.0f;

34
modules/jolt_physics/objects/jolt_area_3d.cpp
View File

@@ -83,12 +83,12 @@ void JoltArea3D::_add_to_space() {
jolt_settings->SetShape(jolt_shape);
const JPH::BodyID new_jolt_id = space->add_rigid_body(*this, *jolt_settings);
if (new_jolt_id.IsInvalid()) {
JPH::Body *new_jolt_body = space->add_rigid_body(*this, *jolt_settings);
if (new_jolt_body == nullptr) {
return;
}
jolt_id = new_jolt_id;
jolt_body = new_jolt_body;
delete jolt_settings;
jolt_settings = nullptr;
@@ -107,8 +107,7 @@ void JoltArea3D::_dequeue_call_queries() {
}
void JoltArea3D::_add_shape_pair(Overlap &p_overlap, const JPH::BodyID &p_body_id, const JPH::SubShapeID &p_other_shape_id, const JPH::SubShapeID &p_self_shape_id) {
const JoltReadableBody3D other_jolt_body = space->read_body(p_body_id);
const JoltShapedObject3D *other_object = other_jolt_body.as_shaped();
const JoltShapedObject3D *other_object = space->try_get_shaped(p_body_id);
ERR_FAIL_NULL(other_object);
p_overlap.rid = other_object->get_rid();
@@ -187,25 +186,15 @@ void JoltArea3D::_report_event(const Callable &p_callback, PhysicsServer3D::Area
}
void JoltArea3D::_notify_body_entered(const JPH::BodyID &p_body_id) {
const JoltReadableBody3D jolt_body = space->read_body(p_body_id);
JoltBody3D *body = jolt_body.as_body();
if (unlikely(body == nullptr)) {
return;
if (JoltBody3D *other_body = space->try_get_body(p_body_id)) {
other_body->add_area(this);
}
body->add_area(this);
}
void JoltArea3D::_notify_body_exited(const JPH::BodyID &p_body_id) {
const JoltReadableBody3D jolt_body = space->read_body(p_body_id);
JoltBody3D *body = jolt_body.as_body();
if (unlikely(body == nullptr)) {
return;
if (JoltBody3D *other_body = space->try_get_body(p_body_id)) {
other_body->remove_area(this);
}
body->remove_area(this);
}
void JoltArea3D::_force_bodies_entered() {
@@ -291,10 +280,7 @@ void JoltArea3D::_update_group_filter() {
return;
}
const JoltWritableBody3D body = space->write_body(jolt_id);
ERR_FAIL_COND(body.is_invalid());
body->GetCollisionGroup().SetGroupFilter(JoltGroupFilter::instance);
jolt_body->GetCollisionGroup().SetGroupFilter(JoltGroupFilter::instance);
}
void JoltArea3D::_update_default_gravity() {
@@ -384,7 +370,7 @@ void JoltArea3D::set_transform(Transform3D p_transform) {
jolt_settings->mPosition = to_jolt_r(p_transform.origin);
jolt_settings->mRotation = to_jolt(p_transform.basis);
} else {
space->get_body_iface().SetPositionAndRotation(jolt_id, to_jolt_r(p_transform.origin), to_jolt(p_transform.basis), JPH::EActivation::DontActivate);
space->get_body_iface().SetPositionAndRotation(jolt_body->GetID(), to_jolt_r(p_transform.origin), to_jolt(p_transform.basis), JPH::EActivation::DontActivate);
}
}

279
modules/jolt_physics/objects/jolt_body_3d.cpp
View File

@@ -144,12 +144,12 @@ void JoltBody3D::_add_to_space() {
jolt_settings->SetShape(jolt_shape);
const JPH::BodyID new_jolt_id = space->add_rigid_body(*this, *jolt_settings, sleep_initially);
if (new_jolt_id.IsInvalid()) {
JPH::Body *new_jolt_body = space->add_rigid_body(*this, *jolt_settings, sleep_initially);
if (new_jolt_body == nullptr) {
return;
}
jolt_id = new_jolt_id;
jolt_body = new_jolt_body;
delete jolt_settings;
jolt_settings = nullptr;
@@ -295,14 +295,9 @@ JPH::MassProperties JoltBody3D::_calculate_mass_properties() const {
}
void JoltBody3D::_update_mass_properties() {
if (!in_space()) {
return;
if (in_space()) {
jolt_body->GetMotionPropertiesUnchecked()->SetMassProperties(_calculate_allowed_dofs(), _calculate_mass_properties());
}
const JoltWritableBody3D body = space->write_body(jolt_id);
ERR_FAIL_COND(body.is_invalid());
body->GetMotionPropertiesUnchecked()->SetMassProperties(_calculate_allowed_dofs(), _calculate_mass_properties());
}
void JoltBody3D::_update_gravity(JPH::Body &p_jolt_body) {
@@ -407,10 +402,7 @@ void JoltBody3D::_update_possible_kinematic_contacts() {
if (!in_space()) {
jolt_settings->mCollideKinematicVsNonDynamic = value;
} else {
const JoltWritableBody3D body = space->write_body(jolt_id);
ERR_FAIL_COND(body.is_invalid());
body->SetCollideKinematicVsNonDynamic(value);
jolt_body->SetCollideKinematicVsNonDynamic(value);
}
}
@@ -419,13 +411,9 @@ void JoltBody3D::_update_sleep_allowed() {
if (!in_space()) {
jolt_settings->mAllowSleeping = value;
return;
} else {
jolt_body->SetAllowSleeping(value);
}
const JoltWritableBody3D body = space->write_body(jolt_id);
ERR_FAIL_COND(body.is_invalid());
body->SetAllowSleeping(value);
}
void JoltBody3D::_destroy_joint_constraints() {
@@ -435,8 +423,10 @@ void JoltBody3D::_destroy_joint_constraints() {
}
void JoltBody3D::_exit_all_areas() {
ERR_FAIL_COND(!in_space());
for (JoltArea3D *area : areas) {
area->body_exited(jolt_id, false);
area->body_exited(jolt_body->GetID(), false);
}
areas.clear();
@@ -447,13 +437,9 @@ void JoltBody3D::_update_group_filter() {
if (!in_space()) {
jolt_settings->mCollisionGroup.SetGroupFilter(group_filter);
return;
} else {
jolt_body->GetCollisionGroup().SetGroupFilter(group_filter);
}
const JoltWritableBody3D body = space->write_body(jolt_id);
ERR_FAIL_COND(body.is_invalid());
body->GetCollisionGroup().SetGroupFilter(group_filter);
}
void JoltBody3D::_mode_changed() {
@@ -559,7 +545,7 @@ void JoltBody3D::set_transform(Transform3D p_transform) {
} else if (is_kinematic()) {
kinematic_transform = p_transform;
} else {
space->get_body_iface().SetPositionAndRotation(jolt_id, to_jolt_r(p_transform.origin), to_jolt(p_transform.basis), JPH::EActivation::DontActivate);
space->get_body_iface().SetPositionAndRotation(jolt_body->GetID(), to_jolt_r(p_transform.origin), to_jolt(p_transform.basis), JPH::EActivation::DontActivate);
}
_transform_changed();
@@ -694,29 +680,20 @@ void JoltBody3D::set_custom_integrator(bool p_enabled) {
custom_integrator = p_enabled;
if (!in_space()) {
_motion_changed();
return;
if (in_space()) {
jolt_body->ResetForce();
jolt_body->ResetTorque();
}
const JoltWritableBody3D body = space->write_body(jolt_id);
ERR_FAIL_COND(body.is_invalid());
body->ResetForce();
body->ResetTorque();
_motion_changed();
}
bool JoltBody3D::is_sleeping() const {
if (!in_space()) {
return sleep_initially;
} else {
return !jolt_body->IsActive();
}
const JoltReadableBody3D body = space->read_body(jolt_id);
ERR_FAIL_COND_V(body.is_invalid(), false);
return !body->IsActive();
}
bool JoltBody3D::is_sleep_actually_allowed() const {
@@ -726,15 +703,12 @@ bool JoltBody3D::is_sleep_actually_allowed() const {
void JoltBody3D::set_is_sleeping(bool p_enabled) {
if (!in_space()) {
sleep_initially = p_enabled;
return;
}
JPH::BodyInterface &body_iface = space->get_body_iface();
if (p_enabled) {
body_iface.DeactivateBody(jolt_id);
} else {
body_iface.ActivateBody(jolt_id);
if (p_enabled) {
space->get_body_iface().DeactivateBody(jolt_body->GetID());
} else {
space->get_body_iface().ActivateBody(jolt_body->GetID());
}
}
}
@@ -749,63 +723,45 @@ void JoltBody3D::set_is_sleep_allowed(bool p_enabled) {
}
Basis JoltBody3D::get_principal_inertia_axes() const {
ERR_FAIL_NULL_V_MSG(space, Basis(), vformat("Failed to retrieve principal inertia axes of '%s'. Doing so without a physics space is not supported when using Jolt Physics. If this relates to a node, try adding the node to a scene tree first.", to_string()));
ERR_FAIL_COND_V_MSG(!in_space(), Basis(), vformat("Failed to retrieve principal inertia axes of '%s'. Doing so without a physics space is not supported when using Jolt Physics. If this relates to a node, try adding the node to a scene tree first.", to_string()));
if (unlikely(is_static() || is_kinematic())) {
return Basis();
}
const JoltReadableBody3D body = space->read_body(jolt_id);
ERR_FAIL_COND_V(body.is_invalid(), Basis());
return to_godot(body->GetRotation() * body->GetMotionProperties()->GetInertiaRotation());
return to_godot(jolt_body->GetRotation() * jolt_body->GetMotionPropertiesUnchecked()->GetInertiaRotation());
}
Vector3 JoltBody3D::get_inverse_inertia() const {
ERR_FAIL_NULL_V_MSG(space, Vector3(), vformat("Failed to retrieve inverse inertia of '%s'. Doing so without a physics space is not supported when using Jolt Physics. If this relates to a node, try adding the node to a scene tree first.", to_string()));
ERR_FAIL_COND_V_MSG(!in_space(), Vector3(), vformat("Failed to retrieve inverse inertia of '%s'. Doing so without a physics space is not supported when using Jolt Physics. If this relates to a node, try adding the node to a scene tree first.", to_string()));
if (unlikely(is_static() || is_kinematic())) {
return Vector3();
}
const JoltReadableBody3D body = space->read_body(jolt_id);
ERR_FAIL_COND_V(body.is_invalid(), Vector3());
const JPH::MotionProperties &motion_properties = *body->GetMotionPropertiesUnchecked();
return to_godot(motion_properties.GetLocalSpaceInverseInertia().GetDiagonal3());
return to_godot(jolt_body->GetMotionPropertiesUnchecked()->GetLocalSpaceInverseInertia().GetDiagonal3());
}
Basis JoltBody3D::get_inverse_inertia_tensor() const {
ERR_FAIL_NULL_V_MSG(space, Basis(), vformat("Failed to retrieve inverse inertia tensor of '%s'. Doing so without a physics space is not supported when using Jolt Physics. If this relates to a node, try adding the node to a scene tree first.", to_string()));
ERR_FAIL_COND_V_MSG(!in_space(), Basis(), vformat("Failed to retrieve inverse inertia tensor of '%s'. Doing so without a physics space is not supported when using Jolt Physics. If this relates to a node, try adding the node to a scene tree first.", to_string()));
if (unlikely(is_static() || is_kinematic())) {
return Basis();
}
const JoltReadableBody3D body = space->read_body(jolt_id);
ERR_FAIL_COND_V(body.is_invalid(), Basis());
return to_godot(body->GetInverseInertia()).basis;
return to_godot(jolt_body->GetInverseInertia()).basis;
}
void JoltBody3D::set_linear_velocity(const Vector3 &p_velocity) {
if (is_static() || is_kinematic()) {
linear_surface_velocity = p_velocity;
_motion_changed();
return;
}
} else {
if (!in_space()) {
jolt_settings->mLinearVelocity = to_jolt(p_velocity);
_motion_changed();
return;
} else {
jolt_body->GetMotionPropertiesUnchecked()->SetLinearVelocityClamped(to_jolt(p_velocity));
}
}
const JoltWritableBody3D body = space->write_body(jolt_id);
ERR_FAIL_COND(body.is_invalid());
body->GetMotionPropertiesUnchecked()->SetLinearVelocityClamped(to_jolt(p_velocity));
_motion_changed();
}
@@ -813,20 +769,13 @@ void JoltBody3D::set_linear_velocity(const Vector3 &p_velocity) {
void JoltBody3D::set_angular_velocity(const Vector3 &p_velocity) {
if (is_static() || is_kinematic()) {
angular_surface_velocity = p_velocity;
_motion_changed();
return;
}
} else {
if (!in_space()) {
jolt_settings->mAngularVelocity = to_jolt(p_velocity);
_motion_changed();
return;
} else {
jolt_body->GetMotionPropertiesUnchecked()->SetAngularVelocityClamped(to_jolt(p_velocity));
}
}
const JoltWritableBody3D body = space->write_body(jolt_id);
ERR_FAIL_COND(body.is_invalid());
body->GetMotionPropertiesUnchecked()->SetAngularVelocityClamped(to_jolt(p_velocity));
_motion_changed();
}
@@ -840,9 +789,6 @@ void JoltBody3D::set_axis_velocity(const Vector3 &p_axis_velocity) {
linear_velocity += p_axis_velocity;
jolt_settings->mLinearVelocity = to_jolt(linear_velocity);
} else {
const JoltWritableBody3D body = space->write_body(jolt_id);
ERR_FAIL_COND(body.is_invalid());
Vector3 linear_velocity = get_linear_velocity();
linear_velocity -= axis * axis.dot(linear_velocity);
linear_velocity += p_axis_velocity;
@@ -857,14 +803,10 @@ Vector3 JoltBody3D::get_velocity_at_position(const Vector3 &p_position) const {
return Vector3();
}
const JoltReadableBody3D body = space->read_body(jolt_id);
ERR_FAIL_COND_V(body.is_invalid(), Vector3());
const JPH::MotionProperties &motion_properties = *body->GetMotionPropertiesUnchecked();
const JPH::MotionProperties &motion_properties = *jolt_body->GetMotionPropertiesUnchecked();
const Vector3 total_linear_velocity = to_godot(motion_properties.GetLinearVelocity()) + linear_surface_velocity;
const Vector3 total_angular_velocity = to_godot(motion_properties.GetAngularVelocity()) + angular_surface_velocity;
const Vector3 com_to_pos = p_position - to_godot(body->GetCenterOfMassPosition());
const Vector3 com_to_pos = p_position - to_godot(jolt_body->GetCenterOfMassPosition());
return total_linear_velocity + total_angular_velocity.cross(com_to_pos);
}
@@ -894,14 +836,10 @@ void JoltBody3D::set_max_contacts_reported(int p_count) {
if (!in_space()) {
jolt_settings->mUseManifoldReduction = use_manifold_reduction;
_contact_reporting_changed();
return;
} else {
space->get_body_iface().SetUseManifoldReduction(jolt_body->GetID(), use_manifold_reduction);
}
JPH::BodyInterface &body_iface = space->get_body_iface();
body_iface.SetUseManifoldReduction(jolt_id, use_manifold_reduction);
_contact_reporting_changed();
}
@@ -963,115 +901,73 @@ void JoltBody3D::reset_mass_properties() {
}
void JoltBody3D::apply_force(const Vector3 &p_force, const Vector3 &p_position) {
ERR_FAIL_NULL_MSG(space, vformat("Failed to apply force to '%s'. Doing so without a physics space is not supported when using Jolt Physics. If this relates to a node, try adding the node to a scene tree first.", to_string()));
ERR_FAIL_COND_MSG(!in_space(), vformat("Failed to apply force to '%s'. Doing so without a physics space is not supported when using Jolt Physics. If this relates to a node, try adding the node to a scene tree first.", to_string()));
if (unlikely(!is_rigid())) {
if (unlikely(!is_rigid()) || custom_integrator || p_force == Vector3()) {
return;
}
if (custom_integrator || p_force == Vector3()) {
return;
}
const JoltWritableBody3D body = space->write_body(jolt_id);
ERR_FAIL_COND(body.is_invalid());
body->AddForce(to_jolt(p_force), body->GetPosition() + to_jolt(p_position));
jolt_body->AddForce(to_jolt(p_force), jolt_body->GetPosition() + to_jolt(p_position));
_motion_changed();
}
void JoltBody3D::apply_central_force(const Vector3 &p_force) {
ERR_FAIL_NULL_MSG(space, vformat("Failed to apply central force to '%s'. Doing so without a physics space is not supported when using Jolt Physics. If this relates to a node, try adding the node to a scene tree first.", to_string()));
ERR_FAIL_COND_MSG(!in_space(), vformat("Failed to apply central force to '%s'. Doing so without a physics space is not supported when using Jolt Physics. If this relates to a node, try adding the node to a scene tree first.", to_string()));
if (unlikely(!is_rigid())) {
if (unlikely(!is_rigid()) || custom_integrator || p_force == Vector3()) {
return;
}
if (custom_integrator || p_force == Vector3()) {
return;
}
const JoltWritableBody3D body = space->write_body(jolt_id);
ERR_FAIL_COND(body.is_invalid());
body->AddForce(to_jolt(p_force));
jolt_body->AddForce(to_jolt(p_force));
_motion_changed();
}
void JoltBody3D::apply_impulse(const Vector3 &p_impulse, const Vector3 &p_position) {
ERR_FAIL_NULL_MSG(space, vformat("Failed to apply impulse to '%s'. Doing so without a physics space is not supported when using Jolt Physics. If this relates to a node, try adding the node to a scene tree first.", to_string()));
ERR_FAIL_COND_MSG(!in_space(), vformat("Failed to apply impulse to '%s'. Doing so without a physics space is not supported when using Jolt Physics. If this relates to a node, try adding the node to a scene tree first.", to_string()));
if (unlikely(!is_rigid())) {
if (unlikely(!is_rigid()) || p_impulse == Vector3()) {
return;
}
if (p_impulse == Vector3()) {
return;
}
const JoltWritableBody3D body = space->write_body(jolt_id);
ERR_FAIL_COND(body.is_invalid());
body->AddImpulse(to_jolt(p_impulse), body->GetPosition() + to_jolt(p_position));
jolt_body->AddImpulse(to_jolt(p_impulse), jolt_body->GetPosition() + to_jolt(p_position));
_motion_changed();
}
void JoltBody3D::apply_central_impulse(const Vector3 &p_impulse) {
ERR_FAIL_NULL_MSG(space, vformat("Failed to apply central impulse to '%s'. Doing so without a physics space is not supported when using Jolt Physics. If this relates to a node, try adding the node to a scene tree first.", to_string()));
ERR_FAIL_COND_MSG(!in_space(), vformat("Failed to apply central impulse to '%s'. Doing so without a physics space is not supported when using Jolt Physics. If this relates to a node, try adding the node to a scene tree first.", to_string()));
if (unlikely(!is_rigid())) {
if (unlikely(!is_rigid()) || p_impulse == Vector3()) {
return;
}
if (p_impulse == Vector3()) {
return;
}
const JoltWritableBody3D body = space->write_body(jolt_id);
ERR_FAIL_COND(body.is_invalid());
body->AddImpulse(to_jolt(p_impulse));
jolt_body->AddImpulse(to_jolt(p_impulse));
_motion_changed();
}
void JoltBody3D::apply_torque(const Vector3 &p_torque) {
ERR_FAIL_NULL_MSG(space, vformat("Failed to apply torque to '%s'. Doing so without a physics space is not supported when using Jolt Physics. If this relates to a node, try adding the node to a scene tree first.", to_string()));
ERR_FAIL_COND_MSG(!in_space(), vformat("Failed to apply torque to '%s'. Doing so without a physics space is not supported when using Jolt Physics. If this relates to a node, try adding the node to a scene tree first.", to_string()));
if (unlikely(!is_rigid())) {
if (unlikely(!is_rigid()) || custom_integrator || p_torque == Vector3()) {
return;
}
if (custom_integrator || p_torque == Vector3()) {
return;
}
const JoltWritableBody3D body = space->write_body(jolt_id);
ERR_FAIL_COND(body.is_invalid());
body->AddTorque(to_jolt(p_torque));
jolt_body->AddTorque(to_jolt(p_torque));
_motion_changed();
}
void JoltBody3D::apply_torque_impulse(const Vector3 &p_impulse) {
ERR_FAIL_NULL_MSG(space, vformat("Failed to apply torque impulse to '%s'. Doing so without a physics space is not supported when using Jolt Physics. If this relates to a node, try adding the node to a scene tree first.", to_string()));
ERR_FAIL_COND_MSG(!in_space(), vformat("Failed to apply torque impulse to '%s'. Doing so without a physics space is not supported when using Jolt Physics. If this relates to a node, try adding the node to a scene tree first.", to_string()));
if (unlikely(!is_rigid())) {
if (unlikely(!is_rigid()) || p_impulse == Vector3()) {
return;
}
if (p_impulse == Vector3()) {
return;
}
const JoltWritableBody3D body = space->write_body(jolt_id);
ERR_FAIL_COND(body.is_invalid());
body->AddAngularImpulse(to_jolt(p_impulse));
jolt_body->AddAngularImpulse(to_jolt(p_impulse));
_motion_changed();
}
@@ -1091,9 +987,6 @@ void JoltBody3D::add_constant_force(const Vector3 &p_force, const Vector3 &p_pos
return;
}
const JoltWritableBody3D body = space->write_body(jolt_id);
ERR_FAIL_COND(body.is_invalid());
constant_force += p_force;
constant_torque += (p_position - get_center_of_mass_relative()).cross(p_force);
@@ -1260,22 +1153,19 @@ void JoltBody3D::set_mode(PhysicsServer3D::BodyMode p_mode) {
const JPH::EMotionType motion_type = _get_motion_type();
const JoltWritableBody3D body = space->write_body(jolt_id);
ERR_FAIL_COND(body.is_invalid());
if (motion_type == JPH::EMotionType::Static) {
put_to_sleep();
}
body->SetMotionType(motion_type);
jolt_body->SetMotionType(motion_type);
if (motion_type != JPH::EMotionType::Static) {
wake_up();
}
if (motion_type == JPH::EMotionType::Kinematic) {
body->SetLinearVelocity(JPH::Vec3::sZero());
body->SetAngularVelocity(JPH::Vec3::sZero());
jolt_body->SetLinearVelocity(JPH::Vec3::sZero());
jolt_body->SetAngularVelocity(JPH::Vec3::sZero());
}
linear_surface_velocity = Vector3();
@@ -1287,11 +1177,9 @@ void JoltBody3D::set_mode(PhysicsServer3D::BodyMode p_mode) {
bool JoltBody3D::is_ccd_enabled() const {
if (!in_space()) {
return jolt_settings->mMotionQuality == JPH::EMotionQuality::LinearCast;
} else {
return space->get_body_iface().GetMotionQuality(jolt_body->GetID()) == JPH::EMotionQuality::LinearCast;
}
const JPH::BodyInterface &body_iface = space->get_body_iface();
return body_iface.GetMotionQuality(jolt_id) == JPH::EMotionQuality::LinearCast;
}
void JoltBody3D::set_ccd_enabled(bool p_enabled) {
@@ -1299,12 +1187,9 @@ void JoltBody3D::set_ccd_enabled(bool p_enabled) {
if (!in_space()) {
jolt_settings->mMotionQuality = motion_quality;
return;
} else {
space->get_body_iface().SetMotionQuality(jolt_body->GetID(), motion_quality);
}
JPH::BodyInterface &body_iface = space->get_body_iface();
body_iface.SetMotionQuality(jolt_id, motion_quality);
}
void JoltBody3D::set_mass(float p_mass) {
@@ -1324,47 +1209,33 @@ void JoltBody3D::set_inertia(const Vector3 &p_inertia) {
float JoltBody3D::get_bounce() const {
if (!in_space()) {
return jolt_settings->mRestitution;
} else {
return jolt_body->GetRestitution();
}
const JoltReadableBody3D body = space->read_body(jolt_id);
ERR_FAIL_COND_V(body.is_invalid(), 0.0f);
return body->GetRestitution();
}
void JoltBody3D::set_bounce(float p_bounce) {
if (!in_space()) {
jolt_settings->mRestitution = p_bounce;
return;
} else {
jolt_body->SetRestitution(p_bounce);
}
const JoltWritableBody3D body = space->write_body(jolt_id);
ERR_FAIL_COND(body.is_invalid());
body->SetRestitution(p_bounce);
}
float JoltBody3D::get_friction() const {
if (!in_space()) {
return jolt_settings->mFriction;
} else {
return jolt_body->GetFriction();
}
const JoltReadableBody3D body = space->read_body(jolt_id);
ERR_FAIL_COND_V(body.is_invalid(), 0.0f);
return body->GetFriction();
}
void JoltBody3D::set_friction(float p_friction) {
if (!in_space()) {
jolt_settings->mFriction = p_friction;
return;
} else {
jolt_body->SetFriction(p_friction);
}
const JoltWritableBody3D body = space->write_body(jolt_id);
ERR_FAIL_COND(body.is_invalid());
body->SetFriction(p_friction);
}
void JoltBody3D::set_gravity_scale(float p_scale) {

9
modules/jolt_physics/objects/jolt_object_3d.cpp
View File

@@ -37,13 +37,12 @@
#include "jolt_group_filter.h"
void JoltObject3D::_remove_from_space() {
if (unlikely(jolt_id.IsInvalid())) {
if (!in_space()) {
return;
}
space->remove_body(jolt_id);
jolt_id = JPH::BodyID();
space->remove_body(jolt_body->GetID());
jolt_body = nullptr;
}
void JoltObject3D::_reset_space() {
@@ -60,7 +59,7 @@ void JoltObject3D::_update_object_layer() {
return;
}
space->get_body_iface().SetObjectLayer(jolt_id, _get_object_layer());
space->get_body_iface().SetObjectLayer(jolt_body->GetID(), _get_object_layer());
}
void JoltObject3D::_collision_layer_changed() {

7
modules/jolt_physics/objects/jolt_object_3d.h
View File

@@ -66,7 +66,7 @@ protected:
RID rid;
ObjectID instance_id;
JoltSpace3D *space = nullptr;
JPH::BodyID jolt_id;
JPH::Body *jolt_body = nullptr;
uint32_t collision_layer = 1;
uint32_t collision_mask = 1;
@@ -121,11 +121,12 @@ public:
void set_instance_id(ObjectID p_id) { instance_id = p_id; }
Object *get_instance() const;
JPH::BodyID get_jolt_id() const { return jolt_id; }
JPH::Body *get_jolt_body() const { return jolt_body; }
JPH::BodyID get_jolt_id() const { return jolt_body->GetID(); }
JoltSpace3D *get_space() const { return space; }
void set_space(JoltSpace3D *p_space);
bool in_space() const { return space != nullptr && !jolt_id.IsInvalid(); }
bool in_space() const { return space != nullptr && jolt_body != nullptr; }
uint32_t get_collision_layer() const { return collision_layer; }
void set_collision_layer(uint32_t p_layer);

55
modules/jolt_physics/objects/jolt_shaped_object_3d.cpp
View File

@@ -178,11 +178,8 @@ void JoltShapedObject3D::_space_changing() {
previous_jolt_shape = nullptr;
if (space != nullptr) {
const JoltWritableBody3D body = space->write_body(jolt_id);
ERR_FAIL_COND(body.is_invalid());
jolt_settings = new JPH::BodyCreationSettings(body->GetBodyCreationSettings());
if (in_space()) {
jolt_settings = new JPH::BodyCreationSettings(jolt_body->GetBodyCreationSettings());
}
}
@@ -208,12 +205,9 @@ JoltShapedObject3D::~JoltShapedObject3D() {
Transform3D JoltShapedObject3D::get_transform_unscaled() const {
if (!in_space()) {
return Transform3D(to_godot(jolt_settings->mRotation), to_godot(jolt_settings->mPosition));
} else {
return Transform3D(to_godot(jolt_body->GetRotation()), to_godot(jolt_body->GetPosition()));
}
const JoltReadableBody3D body = space->read_body(jolt_id);
ERR_FAIL_COND_V(body.is_invalid(), Transform3D());
return Transform3D(to_godot(body->GetRotation()), to_godot(body->GetPosition()));
}
Transform3D JoltShapedObject3D::get_transform_scaled() const {
@@ -223,32 +217,22 @@ Transform3D JoltShapedObject3D::get_transform_scaled() const {
Basis JoltShapedObject3D::get_basis() const {
if (!in_space()) {
return to_godot(jolt_settings->mRotation);
} else {
return to_godot(jolt_body->GetRotation());
}
const JoltReadableBody3D body = space->read_body(jolt_id);
ERR_FAIL_COND_V(body.is_invalid(), Basis());
return to_godot(body->GetRotation());
}
Vector3 JoltShapedObject3D::get_position() const {
if (!in_space()) {
return to_godot(jolt_settings->mPosition);
} else {
return to_godot(jolt_body->GetPosition());
}
const JoltReadableBody3D body = space->read_body(jolt_id);
ERR_FAIL_COND_V(body.is_invalid(), Vector3());
return to_godot(body->GetPosition());
}
Vector3 JoltShapedObject3D::get_center_of_mass() const {
ERR_FAIL_NULL_V_MSG(space, Vector3(), vformat("Failed to retrieve center-of-mass of '%s'. Doing so without a physics space is not supported when using Jolt Physics. If this relates to a node, try adding the node to a scene tree first.", to_string()));
const JoltReadableBody3D body = space->read_body(jolt_id);
ERR_FAIL_COND_V(body.is_invalid(), Vector3());
return to_godot(body->GetCenterOfMassPosition());
ERR_FAIL_COND_V_MSG(!in_space(), Vector3(), vformat("Failed to retrieve center-of-mass of '%s'. Doing so without a physics space is not supported when using Jolt Physics. If this relates to a node, try adding the node to a scene tree first.", to_string()));
return to_godot(jolt_body->GetCenterOfMassPosition());
}
Vector3 JoltShapedObject3D::get_center_of_mass_relative() const {
@@ -264,23 +248,17 @@ Vector3 JoltShapedObject3D::get_center_of_mass_local() const {
Vector3 JoltShapedObject3D::get_linear_velocity() const {
if (!in_space()) {
return to_godot(jolt_settings->mLinearVelocity);
} else {
return to_godot(jolt_body->GetLinearVelocity());
}
const JoltReadableBody3D body = space->read_body(jolt_id);
ERR_FAIL_COND_V(body.is_invalid(), Vector3());
return to_godot(body->GetLinearVelocity());
}
Vector3 JoltShapedObject3D::get_angular_velocity() const {
if (!in_space()) {
return to_godot(jolt_settings->mAngularVelocity);
} else {
return to_godot(jolt_body->GetAngularVelocity());
}
const JoltReadableBody3D body = space->read_body(jolt_id);
ERR_FAIL_COND_V(body.is_invalid(), Vector3());
return to_godot(body->GetAngularVelocity());
}
AABB JoltShapedObject3D::get_aabb() const {
@@ -321,9 +299,6 @@ void JoltShapedObject3D::commit_shapes(bool p_optimize_compound) {
return;
}
const JoltWritableBody3D body = space->write_body(jolt_id);
ERR_FAIL_COND(body.is_invalid());
JPH::ShapeRefC new_shape = build_shapes(p_optimize_compound);
if (new_shape == jolt_shape) {
return;
@@ -332,7 +307,7 @@ void JoltShapedObject3D::commit_shapes(bool p_optimize_compound) {
previous_jolt_shape = jolt_shape;
jolt_shape = new_shape;
space->get_body_iface().SetShape(jolt_id, jolt_shape, false, JPH::EActivation::DontActivate);
space->get_body_iface().SetShape(jolt_body->GetID(), jolt_shape, false, JPH::EActivation::DontActivate);
_enqueue_shapes_changed();

111
modules/jolt_physics/objects/jolt_soft_body_3d.cpp
View File

@@ -78,15 +78,12 @@ JPH::ObjectLayer JoltSoftBody3D::_get_object_layer() const {
void JoltSoftBody3D::_space_changing() {
JoltObject3D::_space_changing();
_deref_shared_data();
if (space != nullptr && !jolt_id.IsInvalid()) {
const JoltReadableBody3D body = space->read_body(jolt_id);
ERR_FAIL_COND(body.is_invalid());
jolt_settings = new JPH::SoftBodyCreationSettings(body->GetSoftBodyCreationSettings());
if (in_space()) {
jolt_settings = new JPH::SoftBodyCreationSettings(jolt_body->GetSoftBodyCreationSettings());
jolt_settings->mSettings = nullptr;
}
_deref_shared_data();
}
void JoltSoftBody3D::_space_changed() {
@@ -117,12 +114,12 @@ void JoltSoftBody3D::_add_to_space() {
jolt_settings->mCollisionGroup = JPH::CollisionGroup(nullptr, group_id, sub_group_id);
jolt_settings->mMaxLinearVelocity = JoltProjectSettings::max_linear_velocity;
const JPH::BodyID new_jolt_id = space->add_soft_body(*this, *jolt_settings);
if (new_jolt_id.IsInvalid()) {
JPH::Body *new_jolt_body = space->add_soft_body(*this, *jolt_settings);
if (new_jolt_body == nullptr) {
return;
}
jolt_id = new_jolt_id;
jolt_body = new_jolt_body;
delete jolt_settings;
jolt_settings = nullptr;
@@ -237,11 +234,7 @@ void JoltSoftBody3D::_update_mass() {
return;
}
JoltWritableBody3D body = space->write_body(jolt_id);
ERR_FAIL_COND(body.is_invalid());
JPH::SoftBodyMotionProperties &motion_properties = static_cast<JPH::SoftBodyMotionProperties &>(*body->GetMotionPropertiesUnchecked());
JPH::SoftBodyMotionProperties &motion_properties = static_cast<JPH::SoftBodyMotionProperties &>(*jolt_body->GetMotionPropertiesUnchecked());
JPH::Array<JPH::SoftBodyVertex> &physics_vertices = motion_properties.GetVertices();
const float inverse_vertex_mass = mass == 0.0f ? 1.0f : (float)physics_vertices.size() / mass;
@@ -259,11 +252,7 @@ void JoltSoftBody3D::_update_pressure() {
return;
}
JoltWritableBody3D body = space->write_body(jolt_id);
ERR_FAIL_COND(body.is_invalid());
JPH::SoftBodyMotionProperties &motion_properties = static_cast<JPH::SoftBodyMotionProperties &>(*body->GetMotionPropertiesUnchecked());
JPH::SoftBodyMotionProperties &motion_properties = static_cast<JPH::SoftBodyMotionProperties &>(*jolt_body->GetMotionPropertiesUnchecked());
motion_properties.SetPressure(pressure);
}
@@ -273,11 +262,7 @@ void JoltSoftBody3D::_update_damping() {
return;
}
JoltWritableBody3D body = space->write_body(jolt_id);
ERR_FAIL_COND(body.is_invalid());
JPH::SoftBodyMotionProperties &motion_properties = static_cast<JPH::SoftBodyMotionProperties &>(*body->GetMotionPropertiesUnchecked());
JPH::SoftBodyMotionProperties &motion_properties = static_cast<JPH::SoftBodyMotionProperties &>(*jolt_body->GetMotionPropertiesUnchecked());
motion_properties.SetLinearDamping(linear_damping);
}
@@ -287,11 +272,7 @@ void JoltSoftBody3D::_update_simulation_precision() {
return;
}
JoltWritableBody3D body = space->write_body(jolt_id);
ERR_FAIL_COND(body.is_invalid());
JPH::SoftBodyMotionProperties &motion_properties = static_cast<JPH::SoftBodyMotionProperties &>(*body->GetMotionPropertiesUnchecked());
JPH::SoftBodyMotionProperties &motion_properties = static_cast<JPH::SoftBodyMotionProperties &>(*jolt_body->GetMotionPropertiesUnchecked());
motion_properties.SetNumIterations((JPH::uint32)simulation_precision);
}
@@ -300,13 +281,9 @@ void JoltSoftBody3D::_update_group_filter() {
if (!in_space()) {
jolt_settings->mCollisionGroup.SetGroupFilter(group_filter);
return;
} else {
jolt_body->GetCollisionGroup().SetGroupFilter(group_filter);
}
const JoltWritableBody3D body = space->write_body(jolt_id);
ERR_FAIL_COND(body.is_invalid());
body->GetCollisionGroup().SetGroupFilter(group_filter);
}
void JoltSoftBody3D::_try_rebuild() {
@@ -407,21 +384,16 @@ void JoltSoftBody3D::set_mesh(const RID &p_mesh) {
}
_deref_shared_data();
mesh = p_mesh;
_mesh_changed();
}
bool JoltSoftBody3D::is_sleeping() const {
if (!in_space()) {
return false;
} else {
return !jolt_body->IsActive();
}
const JoltReadableBody3D body = space->read_body(jolt_id);
ERR_FAIL_COND_V(body.is_invalid(), false);
return !body->IsActive();
}
void JoltSoftBody3D::set_is_sleeping(bool p_enabled) {
@@ -432,32 +404,26 @@ void JoltSoftBody3D::set_is_sleeping(bool p_enabled) {
JPH::BodyInterface &body_iface = space->get_body_iface();
if (p_enabled) {
body_iface.DeactivateBody(jolt_id);
body_iface.DeactivateBody(jolt_body->GetID());
} else {
body_iface.ActivateBody(jolt_id);
body_iface.ActivateBody(jolt_body->GetID());
}
}
bool JoltSoftBody3D::is_sleep_allowed() const {
if (!in_space()) {
return true;
return jolt_settings->mAllowSleeping;
} else {
return jolt_body->GetAllowSleeping();
}
const JoltReadableBody3D body = space->read_body(jolt_id);
ERR_FAIL_COND_V(body.is_invalid(), false);
return body->GetAllowSleeping();
}
void JoltSoftBody3D::set_is_sleep_allowed(bool p_enabled) {
if (!in_space()) {
return;
jolt_settings->mAllowSleeping = p_enabled;
} else {
jolt_body->SetAllowSleeping(p_enabled);
}
const JoltWritableBody3D body = space->write_body(jolt_id);
ERR_FAIL_COND(body.is_invalid());
body->SetAllowSleeping(p_enabled);
}
void JoltSoftBody3D::set_simulation_precision(int p_precision) {
@@ -571,9 +537,6 @@ Transform3D JoltSoftBody3D::get_transform() const {
void JoltSoftBody3D::set_transform(const Transform3D &p_transform) {
ERR_FAIL_COND_MSG(!in_space(), vformat("Failed to set transform for '%s'. Doing so without a physics space is not supported when using Jolt Physics. If this relates to a node, try adding the node to a scene tree first.", to_string()));
JoltWritableBody3D body = space->write_body(jolt_id);
ERR_FAIL_COND(body.is_invalid());
// For whatever reason this has to be interpreted as a relative global-space transform rather than an absolute one,
// because `SoftBody3D` will immediately upon entering the scene tree set itself to be top-level and also set its
// transform to be identity, while still expecting to stay in its original position.
@@ -581,7 +544,7 @@ void JoltSoftBody3D::set_transform(const Transform3D &p_transform) {
// We also discard any scaling, since we have no way of scaling the actual edge lengths.
const JPH::Mat44 relative_transform = to_jolt(p_transform.orthonormalized());
JPH::SoftBodyMotionProperties &motion_properties = static_cast<JPH::SoftBodyMotionProperties &>(*body->GetMotionPropertiesUnchecked());
JPH::SoftBodyMotionProperties &motion_properties = static_cast<JPH::SoftBodyMotionProperties &>(*jolt_body->GetMotionPropertiesUnchecked());
JPH::Array<JPH::SoftBodyVertex> &physics_vertices = motion_properties.GetVertices();
for (JPH::SoftBodyVertex &vertex : physics_vertices) {
@@ -592,11 +555,7 @@ void JoltSoftBody3D::set_transform(const Transform3D &p_transform) {
AABB JoltSoftBody3D::get_bounds() const {
ERR_FAIL_COND_V_MSG(!in_space(), AABB(), vformat("Failed to retrieve world bounds of '%s'. Doing so without a physics space is not supported when using Jolt Physics. If this relates to a node, try adding the node to a scene tree first.", to_string()));
const JoltReadableBody3D body = space->read_body(jolt_id);
ERR_FAIL_COND_V(body.is_invalid(), AABB());
return to_godot(body->GetWorldSpaceBounds());
return to_godot(jolt_body->GetWorldSpaceBounds());
}
void JoltSoftBody3D::update_rendering_server(PhysicsServer3DRenderingServerHandler *p_rendering_server_handler) {
@@ -605,10 +564,7 @@ void JoltSoftBody3D::update_rendering_server(PhysicsServer3DRenderingServerHandl
return;
}
const JoltReadableBody3D body = space->read_body(jolt_id);
ERR_FAIL_COND(body.is_invalid());
const JPH::SoftBodyMotionProperties &motion_properties = static_cast<const JPH::SoftBodyMotionProperties &>(*body->GetMotionPropertiesUnchecked());
const JPH::SoftBodyMotionProperties &motion_properties = static_cast<const JPH::SoftBodyMotionProperties &>(*jolt_body->GetMotionPropertiesUnchecked());
typedef JPH::SoftBodyMotionProperties::Vertex SoftBodyVertex;
typedef JPH::SoftBodyMotionProperties::Face SoftBodyFace;
@@ -660,14 +616,11 @@ Vector3 JoltSoftBody3D::get_vertex_position(int p_index) {
ERR_FAIL_INDEX_V(p_index, (int)shared->mesh_to_physics.size(), Vector3());
const size_t physics_index = (size_t)shared->mesh_to_physics[p_index];
const JoltReadableBody3D body = space->read_body(jolt_id);
ERR_FAIL_COND_V(body.is_invalid(), Vector3());
const JPH::SoftBodyMotionProperties &motion_properties = static_cast<const JPH::SoftBodyMotionProperties &>(*body->GetMotionPropertiesUnchecked());
const JPH::SoftBodyMotionProperties &motion_properties = static_cast<const JPH::SoftBodyMotionProperties &>(*jolt_body->GetMotionPropertiesUnchecked());
const JPH::Array<JPH::SoftBodyVertex> &physics_vertices = motion_properties.GetVertices();
const JPH::SoftBodyVertex &physics_vertex = physics_vertices[physics_index];
return to_godot(body->GetCenterOfMassPosition() + physics_vertex.mPosition);
return to_godot(jolt_body->GetCenterOfMassPosition() + physics_vertex.mPosition);
}
void JoltSoftBody3D::set_vertex_position(int p_index, const Vector3 &p_position) {
@@ -682,15 +635,11 @@ void JoltSoftBody3D::set_vertex_position(int p_index, const Vector3 &p_position)
return;
}
JoltWritableBody3D body = space->write_body(jolt_id);
ERR_FAIL_COND(body.is_invalid());
JPH::SoftBodyMotionProperties &motion_properties = static_cast<JPH::SoftBodyMotionProperties &>(*body->GetMotionPropertiesUnchecked());
JPH::SoftBodyMotionProperties &motion_properties = static_cast<JPH::SoftBodyMotionProperties &>(*jolt_body->GetMotionPropertiesUnchecked());
JPH::Array<JPH::SoftBodyVertex> &physics_vertices = motion_properties.GetVertices();
JPH::SoftBodyVertex &physics_vertex = physics_vertices[physics_index];
const JPH::RVec3 center_of_mass = body->GetCenterOfMassPosition();
const JPH::RVec3 center_of_mass = jolt_body->GetCenterOfMassPosition();
const JPH::Vec3 local_position = JPH::Vec3(to_jolt_r(p_position) - center_of_mass);
const JPH::Vec3 displacement = local_position - physics_vertex.mPosition;
const JPH::Vec3 velocity = displacement / last_step;

196
modules/jolt_physics/spaces/jolt_body_accessor_3d.cpp
View File

@@ -1,196 +0,0 @@
/**************************************************************************/
/* jolt_body_accessor_3d.cpp */
/**************************************************************************/
/* This file is part of: */
/* GODOT ENGINE */
/* https://godotengine.org */
/**************************************************************************/
/* Copyright (c) 2014-present Godot Engine contributors (see AUTHORS.md). */
/* Copyright (c) 2007-2014 Juan Linietsky, Ariel Manzur. */
/* */
/* Permission is hereby granted, free of charge, to any person obtaining */
/* a copy of this software and associated documentation files (the */
/* "Software"), to deal in the Software without restriction, including */
/* without limitation the rights to use, copy, modify, merge, publish, */
/* distribute, sublicense, and/or sell copies of the Software, and to */
/* permit persons to whom the Software is furnished to do so, subject to */
/* the following conditions: */
/* */
/* The above copyright notice and this permission notice shall be */
/* included in all copies or substantial portions of the Software. */
/* */
/* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, */
/* EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF */
/* MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. */
/* IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY */
/* CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, */
/* TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE */
/* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. */
/**************************************************************************/
#include "jolt_body_accessor_3d.h"
#include "jolt_space_3d.h"
namespace {
template <class... TTypes>
struct VariantVisitors : TTypes... {
using TTypes::operator()...;
};
template <class... TTypes>
VariantVisitors(TTypes...) -> VariantVisitors<TTypes...>;
} // namespace
JoltBodyAccessor3D::JoltBodyAccessor3D(const JoltSpace3D *p_space) :
space(p_space) {
}
JoltBodyAccessor3D::~JoltBodyAccessor3D() = default;
void JoltBodyAccessor3D::acquire(const JPH::BodyID *p_ids, int p_id_count) {
ERR_FAIL_NULL(space);
lock_iface = &space->get_lock_iface();
ids = BodyIDSpan(p_ids, p_id_count);
_acquire_internal(p_ids, p_id_count);
}
void JoltBodyAccessor3D::acquire(const JPH::BodyID &p_id) {
ERR_FAIL_NULL(space);
lock_iface = &space->get_lock_iface();
ids = p_id;
_acquire_internal(&p_id, 1);
}
void JoltBodyAccessor3D::acquire_active() {
const JPH::PhysicsSystem &physics_system = space->get_physics_system();
acquire(physics_system.GetActiveBodiesUnsafe(JPH::EBodyType::RigidBody), (int)physics_system.GetNumActiveBodies(JPH::EBodyType::RigidBody));
}
void JoltBodyAccessor3D::acquire_all() {
ERR_FAIL_NULL(space);
lock_iface = &space->get_lock_iface();
JPH::BodyIDVector *vector = std::get_if<JPH::BodyIDVector>(&ids);
if (vector == nullptr) {
ids = JPH::BodyIDVector();
vector = std::get_if<JPH::BodyIDVector>(&ids);
}
space->get_physics_system().GetBodies(*vector);
_acquire_internal(vector->data(), (int)vector->size());
}
void JoltBodyAccessor3D::release() {
_release_internal();
lock_iface = nullptr;
}
const JPH::BodyID *JoltBodyAccessor3D::get_ids() const {
ERR_FAIL_COND_V(not_acquired(), nullptr);
return std::visit(
VariantVisitors{
[](const JPH::BodyID &p_id) { return &p_id; },
[](const JPH::BodyIDVector &p_vector) { return p_vector.data(); },
[](const BodyIDSpan &p_span) { return p_span.ptr; } },
ids);
}
int JoltBodyAccessor3D::get_count() const {
ERR_FAIL_COND_V(not_acquired(), 0);
return std::visit(
VariantVisitors{
[](const JPH::BodyID &p_id) { return 1; },
[](const JPH::BodyIDVector &p_vector) { return (int)p_vector.size(); },
[](const BodyIDSpan &p_span) { return p_span.count; } },
ids);
}
const JPH::BodyID &JoltBodyAccessor3D::get_at(int p_index) const {
CRASH_BAD_INDEX(p_index, get_count());
return get_ids()[p_index];
}
void JoltBodyReader3D::_acquire_internal(const JPH::BodyID *p_ids, int p_id_count) {
mutex_mask = lock_iface->GetMutexMask(p_ids, p_id_count);
lock_iface->LockRead(mutex_mask);
}
void JoltBodyReader3D::_release_internal() {
ERR_FAIL_COND(not_acquired());
lock_iface->UnlockRead(mutex_mask);
}
JoltBodyReader3D::JoltBodyReader3D(const JoltSpace3D *p_space) :
JoltBodyAccessor3D(p_space) {
}
const JPH::Body *JoltBodyReader3D::try_get(const JPH::BodyID &p_id) const {
if (unlikely(p_id.IsInvalid())) {
return nullptr;
}
ERR_FAIL_COND_V(not_acquired(), nullptr);
return lock_iface->TryGetBody(p_id);
}
const JPH::Body *JoltBodyReader3D::try_get(int p_index) const {
const int count = get_count();
if (unlikely(p_index < 0 || p_index >= count)) {
return nullptr;
}
return try_get(get_at(p_index));
}
const JPH::Body *JoltBodyReader3D::try_get() const {
return try_get(0);
}
void JoltBodyWriter3D::_acquire_internal(const JPH::BodyID *p_ids, int p_id_count) {
mutex_mask = lock_iface->GetMutexMask(p_ids, p_id_count);
lock_iface->LockWrite(mutex_mask);
}
void JoltBodyWriter3D::_release_internal() {
ERR_FAIL_COND(not_acquired());
lock_iface->UnlockWrite(mutex_mask);
}
JoltBodyWriter3D::JoltBodyWriter3D(const JoltSpace3D *p_space) :
JoltBodyAccessor3D(p_space) {
}
JPH::Body *JoltBodyWriter3D::try_get(const JPH::BodyID &p_id) const {
if (unlikely(p_id.IsInvalid())) {
return nullptr;
}
ERR_FAIL_COND_V(not_acquired(), nullptr);
return lock_iface->TryGetBody(p_id);
}
JPH::Body *JoltBodyWriter3D::try_get(int p_index) const {
const int count = get_count();
if (unlikely(p_index < 0 || p_index >= count)) {
return nullptr;
}
return try_get(get_at(p_index));
}
JPH::Body *JoltBodyWriter3D::try_get() const {
return try_get(0);
}

211
modules/jolt_physics/spaces/jolt_body_accessor_3d.h
View File

@@ -1,211 +0,0 @@
/**************************************************************************/
/* jolt_body_accessor_3d.h */
/**************************************************************************/
/* This file is part of: */
/* GODOT ENGINE */
/* https://godotengine.org */
/**************************************************************************/
/* Copyright (c) 2014-present Godot Engine contributors (see AUTHORS.md). */
/* Copyright (c) 2007-2014 Juan Linietsky, Ariel Manzur. */
/* */
/* Permission is hereby granted, free of charge, to any person obtaining */
/* a copy of this software and associated documentation files (the */
/* "Software"), to deal in the Software without restriction, including */
/* without limitation the rights to use, copy, modify, merge, publish, */
/* distribute, sublicense, and/or sell copies of the Software, and to */
/* permit persons to whom the Software is furnished to do so, subject to */
/* the following conditions: */
/* */
/* The above copyright notice and this permission notice shall be */
/* included in all copies or substantial portions of the Software. */
/* */
/* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, */
/* EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF */
/* MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. */
/* IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY */
/* CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, */
/* TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE */
/* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. */
/**************************************************************************/
#pragma once
#include "../objects/jolt_object_3d.h"
#include "Jolt/Jolt.h"
#include "Jolt/Physics/Body/BodyLockInterface.h"
#include <variant>
class JoltArea3D;
class JoltBody3D;
class JoltShapedObject3D;
class JoltSpace3D;
class JoltBodyAccessor3D {
protected:
struct BodyIDSpan {
BodyIDSpan(const JPH::BodyID *p_ptr, int p_count) :
ptr(p_ptr), count(p_count) {}
const JPH::BodyID *ptr;
int count;
};
virtual void _acquire_internal(const JPH::BodyID *p_ids, int p_id_count) = 0;
virtual void _release_internal() = 0;
const JoltSpace3D *space = nullptr;
const JPH::BodyLockInterface *lock_iface = nullptr;
std::variant<JPH::BodyID, JPH::BodyIDVector, BodyIDSpan> ids;
public:
explicit JoltBodyAccessor3D(const JoltSpace3D *p_space);
virtual ~JoltBodyAccessor3D() = 0;
void acquire(const JPH::BodyID *p_ids, int p_id_count);
void acquire(const JPH::BodyID &p_id);
void acquire_active();
void acquire_all();
void release();
bool is_acquired() const { return lock_iface != nullptr; }
bool not_acquired() const { return lock_iface == nullptr; }
const JoltSpace3D &get_space() const { return *space; }
const JPH::BodyID *get_ids() const;
int get_count() const;
const JPH::BodyID &get_at(int p_index) const;
};
class JoltBodyReader3D final : public JoltBodyAccessor3D {
virtual void _acquire_internal(const JPH::BodyID *p_ids, int p_id_count) override;
virtual void _release_internal() override;
JPH::BodyLockInterface::MutexMask mutex_mask = 0;
public:
explicit JoltBodyReader3D(const JoltSpace3D *p_space);
const JPH::Body *try_get(const JPH::BodyID &p_id) const;
const JPH::Body *try_get(int p_index) const;
const JPH::Body *try_get() const;
};
class JoltBodyWriter3D final : public JoltBodyAccessor3D {
virtual void _acquire_internal(const JPH::BodyID *p_ids, int p_id_count) override;
virtual void _release_internal() override;
JPH::BodyLockInterface::MutexMask mutex_mask = 0;
public:
explicit JoltBodyWriter3D(const JoltSpace3D *p_space);
JPH::Body *try_get(const JPH::BodyID &p_id) const;
JPH::Body *try_get(int p_index) const;
JPH::Body *try_get() const;
};
template <typename TBodyAccessor>
class JoltScopedBodyAccessor3D {
TBodyAccessor inner;
public:
JoltScopedBodyAccessor3D(const JoltSpace3D &p_space, const JPH::BodyID *p_ids, int p_id_count) :
inner(&p_space) { inner.acquire(p_ids, p_id_count); }
JoltScopedBodyAccessor3D(const JoltSpace3D &p_space, const JPH::BodyID &p_id) :
inner(&p_space) { inner.acquire(p_id); }
JoltScopedBodyAccessor3D(const JoltScopedBodyAccessor3D &p_other) = delete;
JoltScopedBodyAccessor3D(JoltScopedBodyAccessor3D &&p_other) = default;
~JoltScopedBodyAccessor3D() { inner.release(); }
const JoltSpace3D &get_space() const { return inner.get_space(); }
int get_count() const { return inner.get_count(); }
const JPH::BodyID &get_at(int p_index) const { return inner.get_at(p_index); }
JoltScopedBodyAccessor3D &operator=(const JoltScopedBodyAccessor3D &p_other) = delete;
JoltScopedBodyAccessor3D &operator=(JoltScopedBodyAccessor3D &&p_other) = default;
decltype(auto) try_get(const JPH::BodyID &p_id) const { return inner.try_get(p_id); }
decltype(auto) try_get(int p_index) const { return inner.try_get(p_index); }
decltype(auto) try_get() const { return inner.try_get(); }
};
template <typename TAccessor, typename TBody>
class JoltAccessibleBody3D {
TAccessor accessor;
TBody *body = nullptr;
public:
JoltAccessibleBody3D(const JoltSpace3D &p_space, const JPH::BodyID &p_id) :
accessor(p_space, p_id), body(accessor.try_get()) {}
bool is_valid() const { return body != nullptr; }
bool is_invalid() const { return body == nullptr; }
JoltObject3D *as_object() const {
if (body != nullptr) {
return reinterpret_cast<JoltObject3D *>(body->GetUserData());
} else {
return nullptr;
}
}
JoltShapedObject3D *as_shaped() const {
if (JoltObject3D *object = as_object(); object != nullptr && object->is_shaped()) {
return reinterpret_cast<JoltShapedObject3D *>(body->GetUserData());
} else {
return nullptr;
}
}
JoltBody3D *as_body() const {
if (JoltObject3D *object = as_object(); object != nullptr && object->is_body()) {
return reinterpret_cast<JoltBody3D *>(body->GetUserData());
} else {
return nullptr;
}
}
JoltArea3D *as_area() const {
if (JoltObject3D *object = as_object(); object != nullptr && object->is_area()) {
return reinterpret_cast<JoltArea3D *>(body->GetUserData());
} else {
return nullptr;
}
}
TBody *operator->() const { return body; }
TBody &operator*() const { return *body; }
explicit operator TBody *() const { return body; }
};
template <typename TAccessor, typename TBody>
class JoltAccessibleBodies3D {
TAccessor accessor;
public:
JoltAccessibleBodies3D(const JoltSpace3D &p_space, const JPH::BodyID *p_ids, int p_id_count) :
accessor(p_space, p_ids, p_id_count) {}
JoltAccessibleBody3D<TAccessor, TBody> operator[](int p_index) const {
const JPH::BodyID &body_id = p_index < accessor.get_count() ? accessor.get_at(p_index) : JPH::BodyID();
return JoltAccessibleBody3D<TAccessor, TBody>(accessor.get_space(), body_id);
}
};
typedef JoltScopedBodyAccessor3D<JoltBodyReader3D> JoltScopedBodyReader3D;
typedef JoltScopedBodyAccessor3D<JoltBodyWriter3D> JoltScopedBodyWriter3D;
typedef JoltAccessibleBody3D<JoltScopedBodyReader3D, const JPH::Body> JoltReadableBody3D;
typedef JoltAccessibleBody3D<JoltScopedBodyWriter3D, JPH::Body> JoltWritableBody3D;
typedef JoltAccessibleBodies3D<JoltScopedBodyReader3D, const JPH::Body> JoltReadableBodies3D;
typedef JoltAccessibleBodies3D<JoltScopedBodyWriter3D, JPH::Body> JoltWritableBodies3D;

51
modules/jolt_physics/spaces/jolt_contact_listener_3d.cpp
View File

@@ -262,7 +262,6 @@ bool JoltContactListener3D::_try_evaluate_area_overlap(const JPH::Body &p_body1,
};
const JPH::SubShapeIDPair shape_pair1(p_body1.GetID(), p_manifold.mSubShapeID1, p_body2.GetID(), p_manifold.mSubShapeID2);
const JPH::SubShapeIDPair shape_pair2(p_body2.GetID(), p_manifold.mSubShapeID2, p_body1.GetID(), p_manifold.mSubShapeID1);
const JoltObject3D *object1 = reinterpret_cast<JoltObject3D *>(p_body1.GetUserData());
@@ -405,13 +404,10 @@ void JoltContactListener3D::_flush_contacts() {
const JPH::SubShapeIDPair &shape_pair = E.key;
Manifold &manifold = E.value;
const JPH::BodyID body_ids[2] = { shape_pair.GetBody1ID(), shape_pair.GetBody2ID() };
const JoltReadableBodies3D jolt_bodies = space->read_bodies(body_ids, 2);
JoltBody3D *body1 = jolt_bodies[0].as_body();
JoltBody3D *body1 = space->try_get_body(shape_pair.GetBody1ID());
ERR_FAIL_NULL(body1);
JoltBody3D *body2 = jolt_bodies[1].as_body();
JoltBody3D *body2 = space->try_get_body(shape_pair.GetBody2ID());
ERR_FAIL_NULL(body2);
const int shape_index1 = body1->find_shape_index(shape_pair.GetSubShapeID1());
@@ -435,21 +431,18 @@ void JoltContactListener3D::_flush_area_enters() {
const JPH::BodyID &body_id1 = shape_pair.GetBody1ID();
const JPH::BodyID &body_id2 = shape_pair.GetBody2ID();
const JPH::SubShapeID &sub_shape_id1 = shape_pair.GetSubShapeID1();
const JPH::SubShapeID &sub_shape_id2 = shape_pair.GetSubShapeID2();
JoltObject3D *object1 = space->try_get_object(body_id1);
JoltObject3D *object2 = space->try_get_object(body_id2);
const JPH::BodyID body_ids[2] = { body_id1, body_id2 };
const JoltReadableBodies3D jolt_bodies = space->read_bodies(body_ids, 2);
const JoltReadableBody3D jolt_body1 = jolt_bodies[0];
const JoltReadableBody3D jolt_body2 = jolt_bodies[1];
if (jolt_body1.is_invalid() || jolt_body2.is_invalid()) {
if (object1 == nullptr || object2 == nullptr) {
continue;
}
JoltArea3D *area1 = jolt_body1.as_area();
JoltArea3D *area2 = jolt_body2.as_area();
JoltArea3D *area1 = object1->as_area();
JoltArea3D *area2 = object2->as_area();
const JPH::SubShapeID &sub_shape_id1 = shape_pair.GetSubShapeID1();
const JPH::SubShapeID &sub_shape_id2 = shape_pair.GetSubShapeID2();
if (area1 != nullptr && area2 != nullptr) {
area1->area_shape_entered(body_id2, sub_shape_id2, sub_shape_id1);
@@ -466,8 +459,7 @@ void JoltContactListener3D::_flush_area_enters() {
void JoltContactListener3D::_flush_area_shifts() {
for (const JPH::SubShapeIDPair &shape_pair : area_overlaps) {
auto is_shifted = [&](const JPH::BodyID &p_body_id, const JPH::SubShapeID &p_sub_shape_id) {
const JoltReadableBody3D jolt_body = space->read_body(p_body_id);
const JoltShapedObject3D *object = jolt_body.as_shaped();
const JoltShapedObject3D *object = space->try_get_shaped(p_body_id);
ERR_FAIL_NULL_V(object, false);
if (object->get_previous_jolt_shape() == nullptr) {
@@ -495,21 +487,18 @@ void JoltContactListener3D::_flush_area_exits() {
const JPH::BodyID &body_id1 = shape_pair.GetBody1ID();
const JPH::BodyID &body_id2 = shape_pair.GetBody2ID();
JoltObject3D *object1 = space->try_get_object(body_id1);
JoltObject3D *object2 = space->try_get_object(body_id2);
JoltArea3D *area1 = object1 != nullptr ? object1->as_area() : nullptr;
JoltArea3D *area2 = object2 != nullptr ? object2->as_area() : nullptr;
const JoltBody3D *body1 = object1 != nullptr ? object1->as_body() : nullptr;
const JoltBody3D *body2 = object2 != nullptr ? object2->as_body() : nullptr;
const JPH::SubShapeID &sub_shape_id1 = shape_pair.GetSubShapeID1();
const JPH::SubShapeID &sub_shape_id2 = shape_pair.GetSubShapeID2();
const JPH::BodyID body_ids[2] = { body_id1, body_id2 };
const JoltReadableBodies3D jolt_bodies = space->read_bodies(body_ids, 2);
const JoltReadableBody3D jolt_body1 = jolt_bodies[0];
const JoltReadableBody3D jolt_body2 = jolt_bodies[1];
JoltArea3D *area1 = jolt_body1.as_area();
JoltArea3D *area2 = jolt_body2.as_area();
const JoltBody3D *body1 = jolt_body1.as_body();
const JoltBody3D *body2 = jolt_body2.as_body();
if (area1 != nullptr && area2 != nullptr) {
area1->area_shape_exited(body_id2, sub_shape_id2, sub_shape_id1);
} else if (area1 != nullptr && body2 != nullptr) {

3
modules/jolt_physics/spaces/jolt_motion_filter_3d.cpp
View File

@@ -90,8 +90,7 @@ bool JoltMotionFilter3D::ShouldCollideLocked(const JPH::Body &p_jolt_body) const
return false;
}
const JoltReadableBody3D jolt_body_self = space.read_body(body_self);
return jolt_body_self->GetCollisionGroup().CanCollide(p_jolt_body.GetCollisionGroup());
return body_self.get_jolt_body()->GetCollisionGroup().CanCollide(p_jolt_body.GetCollisionGroup());
}
bool JoltMotionFilter3D::ShouldCollide(const JPH::Shape *p_jolt_shape, const JPH::SubShapeID &p_jolt_shape_id) const {

55
modules/jolt_physics/spaces/jolt_physics_direct_space_state_3d.cpp
View File

@@ -121,7 +121,7 @@ bool JoltPhysicsDirectSpaceState3D::_cast_motion_impl(const JPH::Shape &p_jolt_s
continue;
}
const JoltReadableBody3D other_jolt_body = space->read_body(other_jolt_id);
const JPH::Body *other_jolt_body = space->try_get_jolt_body(other_jolt_id);
if (!p_body_filter.ShouldCollideLocked(*other_jolt_body)) {
continue;
}
@@ -202,9 +202,7 @@ bool JoltPhysicsDirectSpaceState3D::_body_motion_recover(const JoltBody3D &p_bod
for (int j = 0; j < hit_count; j++) {
const JPH::CollideShapeResult &hit = collector.get_hit(j);
const JoltReadableBody3D other_jolt_body = space->read_body(hit.mBodyID2);
const JoltBody3D *other_body = other_jolt_body.as_body();
const JoltBody3D *other_body = space->try_get_body(hit.mBodyID2);
ERR_CONTINUE(other_body == nullptr);
combined_priority += other_body->get_collision_priority();
@@ -234,8 +232,7 @@ bool JoltPhysicsDirectSpaceState3D::_body_motion_recover(const JoltBody3D &p_bod
continue;
}
const JoltReadableBody3D other_jolt_body = space->read_body(hit.mBodyID2);
const JoltBody3D *other_body = other_jolt_body.as_body();
const JoltBody3D *other_body = space->try_get_body(hit.mBodyID2);
ERR_CONTINUE(other_body == nullptr);
const float recovery_distance = penetration_depth * JoltProjectSettings::motion_query_recovery_amount;
@@ -356,8 +353,7 @@ bool JoltPhysicsDirectSpaceState3D::_body_motion_collide(const JoltBody3D &p_bod
contact_points2.push_back(hit.mContactPointOn2);
}
const JoltReadableBody3D collider_jolt_body = space->read_body(hit.mBodyID2);
const JoltShapedObject3D *collider = collider_jolt_body.as_shaped();
const JoltShapedObject3D *collider = space->try_get_shaped(hit.mBodyID2);
ERR_FAIL_NULL_V(collider, false);
const int local_shape = p_body.find_shape_index(hit.mSubShapeID1);
@@ -495,8 +491,7 @@ bool JoltPhysicsDirectSpaceState3D::intersect_ray(const RayParameters &p_paramet
const JPH::BodyID &body_id = hit.mBodyID;
const JPH::SubShapeID &sub_shape_id = hit.mSubShapeID2;
const JoltReadableBody3D body = space->read_body(body_id);
const JoltObject3D *object = body.as_object();
const JoltObject3D *object = space->try_get_object(body_id);
ERR_FAIL_NULL_V(object, false);
const JPH::RVec3 position = ray.GetPointOnRay(hit.mFraction);
@@ -504,7 +499,7 @@ bool JoltPhysicsDirectSpaceState3D::intersect_ray(const RayParameters &p_paramet
JPH::Vec3 normal = JPH::Vec3::sZero();
if (!p_parameters.hit_from_inside || hit.mFraction > 0.0f) {
normal = body->GetWorldSpaceSurfaceNormal(sub_shape_id, position);
normal = object->get_jolt_body()->GetWorldSpaceSurfaceNormal(sub_shape_id, position);
// If we got a back-face normal we need to flip it.
if (normal.Dot(vector) > 0) {
@@ -523,7 +518,7 @@ bool JoltPhysicsDirectSpaceState3D::intersect_ray(const RayParameters &p_paramet
const int shape_index = shaped_object->find_shape_index(sub_shape_id);
ERR_FAIL_COND_V(shape_index == -1, false);
r_result.shape = shape_index;
r_result.face_index = _try_get_face_index(*body, sub_shape_id);
r_result.face_index = _try_get_face_index(*object->get_jolt_body(), sub_shape_id);
}
return true;
@@ -546,9 +541,7 @@ int JoltPhysicsDirectSpaceState3D::intersect_point(const PointParameters &p_para
for (int i = 0; i < hit_count; ++i) {
const JPH::CollidePointResult &hit = collector.get_hit(i);
const JoltReadableBody3D body = space->read_body(hit.mBodyID);
const JoltObject3D *object = body.as_object();
const JoltObject3D *object = space->try_get_object(hit.mBodyID);
ERR_FAIL_NULL_V(object, 0);
ShapeResult &result = *r_results++;
@@ -605,9 +598,7 @@ int JoltPhysicsDirectSpaceState3D::intersect_shape(const ShapeParameters &p_para
for (int i = 0; i < hit_count; ++i) {
const JPH::CollideShapeResult &hit = collector.get_hit(i);
const JoltReadableBody3D body = space->read_body(hit.mBodyID2);
const JoltObject3D *object = body.as_object();
const JoltObject3D *object = space->try_get_object(hit.mBodyID2);
ERR_FAIL_NULL_V(object, 0);
ShapeResult &result = *r_results++;
@@ -769,8 +760,7 @@ bool JoltPhysicsDirectSpaceState3D::rest_info(const ShapeParameters &p_parameter
}
const JPH::CollideShapeResult &hit = collector.get_hit();
const JoltReadableBody3D body = space->read_body(hit.mBodyID2);
const JoltObject3D *object = body.as_object();
const JoltObject3D *object = space->try_get_object(hit.mBodyID2);
ERR_FAIL_NULL_V(object, false);
r_info->shape = 0;
@@ -807,11 +797,9 @@ Vector3 JoltPhysicsDirectSpaceState3D::get_closest_point_to_object_volume(RID p_
ERR_FAIL_NULL_V(object, Vector3());
ERR_FAIL_COND_V(object->get_space() != space, Vector3());
const JoltReadableBody3D body = space->read_body(*object);
const JPH::TransformedShape root_shape = body->GetTransformedShape();
JoltQueryCollectorAll<JPH::TransformedShapeCollector, 32> collector;
root_shape.CollectTransformedShapes(body->GetWorldSpaceBounds(), collector);
const JPH::TransformedShape root_shape = object->get_jolt_body()->GetTransformedShape();
root_shape.CollectTransformedShapes(object->get_jolt_body()->GetWorldSpaceBounds(), collector);
const JPH::RVec3 point = to_jolt_r(p_point);
@@ -835,17 +823,14 @@ Vector3 JoltPhysicsDirectSpaceState3D::get_closest_point_to_object_volume(RID p_
JPH::ConvexShape::SupportBuffer shape_support_buffer;
const JPH::ConvexShape::Support *shape_support = shape_convex.GetSupportFunction(JPH::ConvexShape::ESupportMode::IncludeConvexRadius, shape_support_buffer, shape_transformed.GetShapeScale());
const JPH::Quat &shape_rotation = shape_transformed.mShapeRotation;
const JPH::RVec3 &shape_pos_com = shape_transformed.mShapePositionCOM;
const JPH::RMat44 shape_3x3 = JPH::RMat44::sRotation(shape_rotation);
const JPH::Vec3 shape_com_local = shape.GetCenterOfMass();
const JPH::Vec3 shape_com = shape_3x3.Multiply3x3(shape_com_local);
const JPH::RVec3 shape_pos = shape_pos_com - JPH::RVec3(shape_com);
const JPH::RMat44 shape_4x4 = shape_3x3.PostTranslated(shape_pos);
const JPH::RMat44 shape_4x4_inv = shape_4x4.InversedRotationTranslation();
const JPH::RMat44 shape_rotation = JPH::RMat44::sRotation(shape_transformed.mShapeRotation);
const JPH::Vec3 shape_com = shape_rotation.Multiply3x3(shape.GetCenterOfMass());
const JPH::RVec3 shape_pos = shape_transformed.mShapePositionCOM - JPH::RVec3(shape_com);
const JPH::RMat44 shape_xform = shape_rotation.PostTranslated(shape_pos);
const JPH::RMat44 shape_xform_inv = shape_xform.InversedRotationTranslation();
JPH::PointConvexSupport point_support;
point_support.mPoint = JPH::Vec3(shape_4x4_inv * point);
point_support.mPoint = JPH::Vec3(shape_xform_inv * point);
JPH::Vec3 separating_axis = JPH::Vec3::sAxisX();
JPH::Vec3 point_on_a = JPH::Vec3::sZero();
@@ -861,7 +846,7 @@ Vector3 JoltPhysicsDirectSpaceState3D::get_closest_point_to_object_volume(RID p_
if (distance_sq < closest_distance_sq) {
closest_distance_sq = distance_sq;
closest_point = shape_4x4 * point_on_a;
closest_point = shape_xform * point_on_a;
found_point = true;
}
}
@@ -869,7 +854,7 @@ Vector3 JoltPhysicsDirectSpaceState3D::get_closest_point_to_object_volume(RID p_
if (found_point) {
return to_godot(closest_point);
} else {
return to_godot(body->GetPosition());
return to_godot(object->get_jolt_body()->GetPosition());
}
}

75
modules/jolt_physics/spaces/jolt_space_3d.cpp
View File

@@ -298,28 +298,53 @@ void JoltSpace3D::map_from_object_layer(JPH::ObjectLayer p_object_layer, JPH::Br
layers->from_object_layer(p_object_layer, r_broad_phase_layer, r_collision_layer, r_collision_mask);
}
JoltReadableBody3D JoltSpace3D::read_body(const JPH::BodyID &p_body_id) const {
return JoltReadableBody3D(*this, p_body_id);
JPH::Body *JoltSpace3D::try_get_jolt_body(const JPH::BodyID &p_body_id) const {
return get_lock_iface().TryGetBody(p_body_id);
}
JoltReadableBody3D JoltSpace3D::read_body(const JoltObject3D &p_object) const {
return read_body(p_object.get_jolt_id());
JoltObject3D *JoltSpace3D::try_get_object(const JPH::BodyID &p_body_id) const {
const JPH::Body *jolt_body = try_get_jolt_body(p_body_id);
if (unlikely(jolt_body == nullptr)) {
return nullptr;
}
return reinterpret_cast<JoltObject3D *>(jolt_body->GetUserData());
}
JoltWritableBody3D JoltSpace3D::write_body(const JPH::BodyID &p_body_id) const {
return JoltWritableBody3D(*this, p_body_id);
JoltShapedObject3D *JoltSpace3D::try_get_shaped(const JPH::BodyID &p_body_id) const {
JoltObject3D *object = try_get_object(p_body_id);
if (unlikely(object == nullptr)) {
return nullptr;
}
return object->as_shaped();
}
JoltWritableBody3D JoltSpace3D::write_body(const JoltObject3D &p_object) const {
return write_body(p_object.get_jolt_id());
JoltBody3D *JoltSpace3D::try_get_body(const JPH::BodyID &p_body_id) const {
JoltObject3D *object = try_get_object(p_body_id);
if (unlikely(object == nullptr)) {
return nullptr;
}
return object->as_body();
}
JoltReadableBodies3D JoltSpace3D::read_bodies(const JPH::BodyID *p_body_ids, int p_body_count) const {
return JoltReadableBodies3D(*this, p_body_ids, p_body_count);
JoltArea3D *JoltSpace3D::try_get_area(const JPH::BodyID &p_body_id) const {
JoltObject3D *object = try_get_object(p_body_id);
if (unlikely(object == nullptr)) {
return nullptr;
}
return object->as_area();
}
JoltWritableBodies3D JoltSpace3D::write_bodies(const JPH::BodyID *p_body_ids, int p_body_count) const {
return JoltWritableBodies3D(*this, p_body_ids, p_body_count);
JoltSoftBody3D *JoltSpace3D::try_get_soft_body(const JPH::BodyID &p_body_id) const {
JoltObject3D *object = try_get_object(p_body_id);
if (unlikely(object == nullptr)) {
return nullptr;
}
return object->as_soft_body();
}
JoltPhysicsDirectSpaceState3D *JoltSpace3D::get_direct_state() {
@@ -346,38 +371,40 @@ void JoltSpace3D::set_default_area(JoltArea3D *p_area) {
}
}
JPH::BodyID JoltSpace3D::add_rigid_body(const JoltObject3D &p_object, const JPH::BodyCreationSettings &p_settings, bool p_sleeping) {
const JPH::BodyID body_id = get_body_iface().CreateAndAddBody(p_settings, p_sleeping ? JPH::EActivation::DontActivate : JPH::EActivation::Activate);
if (unlikely(body_id.IsInvalid())) {
JPH::Body *JoltSpace3D::add_rigid_body(const JoltObject3D &p_object, const JPH::BodyCreationSettings &p_settings, bool p_sleeping) {
JPH::BodyInterface &body_iface = get_body_iface();
JPH::Body *jolt_body = body_iface.CreateBody(p_settings);
if (unlikely(jolt_body == nullptr)) {
ERR_PRINT_ONCE(vformat("Failed to create underlying Jolt Physics body for '%s'. "
"Consider increasing maximum number of bodies in project settings. "
"Maximum number of bodies is currently set to %d.",
p_object.to_string(), JoltProjectSettings::max_bodies));
return JPH::BodyID();
return nullptr;
}
body_iface.AddBody(jolt_body->GetID(), p_sleeping ? JPH::EActivation::DontActivate : JPH::EActivation::Activate);
bodies_added_since_optimizing += 1;
return body_id;
return jolt_body;
}
JPH::BodyID JoltSpace3D::add_soft_body(const JoltObject3D &p_object, const JPH::SoftBodyCreationSettings &p_settings, bool p_sleeping) {
const JPH::BodyID body_id = get_body_iface().CreateAndAddSoftBody(p_settings, p_sleeping ? JPH::EActivation::DontActivate : JPH::EActivation::Activate);
if (unlikely(body_id.IsInvalid())) {
JPH::Body *JoltSpace3D::add_soft_body(const JoltObject3D &p_object, const JPH::SoftBodyCreationSettings &p_settings, bool p_sleeping) {
JPH::BodyInterface &body_iface = get_body_iface();
JPH::Body *jolt_body = body_iface.CreateSoftBody(p_settings);
if (unlikely(jolt_body == nullptr)) {
ERR_PRINT_ONCE(vformat("Failed to create underlying Jolt Physics body for '%s'. "
"Consider increasing maximum number of bodies in project settings. "
"Maximum number of bodies is currently set to %d.",
p_object.to_string(), JoltProjectSettings::max_bodies));
return JPH::BodyID();
return nullptr;
}
body_iface.AddBody(jolt_body->GetID(), p_sleeping ? JPH::EActivation::DontActivate : JPH::EActivation::Activate);
bodies_added_since_optimizing += 1;
return body_id;
return jolt_body;
}
void JoltSpace3D::remove_body(const JPH::BodyID &p_body_id) {

21
modules/jolt_physics/spaces/jolt_space_3d.h
View File

@@ -30,8 +30,6 @@
#pragma once
#include "jolt_body_accessor_3d.h"
#include "servers/physics_server_3d.h"
#include "Jolt/Jolt.h"
@@ -52,6 +50,7 @@ class JoltLayers;
class JoltObject3D;
class JoltPhysicsDirectSpaceState3D;
class JoltShapedObject3D;
class JoltSoftBody3D;
class JoltSpace3D {
SelfList<JoltBody3D>::List body_call_queries_list;
@@ -112,14 +111,12 @@ public:
JPH::ObjectLayer map_to_object_layer(JPH::BroadPhaseLayer p_broad_phase_layer, uint32_t p_collision_layer, uint32_t p_collision_mask);
void map_from_object_layer(JPH::ObjectLayer p_object_layer, JPH::BroadPhaseLayer &r_broad_phase_layer, uint32_t &r_collision_layer, uint32_t &r_collision_mask) const;
JoltReadableBody3D read_body(const JPH::BodyID &p_body_id) const;
JoltReadableBody3D read_body(const JoltObject3D &p_object) const;
JoltWritableBody3D write_body(const JPH::BodyID &p_body_id) const;
JoltWritableBody3D write_body(const JoltObject3D &p_object) const;
JoltReadableBodies3D read_bodies(const JPH::BodyID *p_body_ids, int p_body_count) const;
JoltWritableBodies3D write_bodies(const JPH::BodyID *p_body_ids, int p_body_count) const;
JPH::Body *try_get_jolt_body(const JPH::BodyID &p_body_id) const;
JoltObject3D *try_get_object(const JPH::BodyID &p_body_id) const;
JoltShapedObject3D *try_get_shaped(const JPH::BodyID &p_body_id) const;
JoltBody3D *try_get_body(const JPH::BodyID &p_body_id) const;
JoltArea3D *try_get_area(const JPH::BodyID &p_body_id) const;
JoltSoftBody3D *try_get_soft_body(const JPH::BodyID &p_body_id) const;
JoltPhysicsDirectSpaceState3D *get_direct_state();
@@ -128,8 +125,8 @@ public:
float get_last_step() const { return last_step; }
JPH::BodyID add_rigid_body(const JoltObject3D &p_object, const JPH::BodyCreationSettings &p_settings, bool p_sleeping = false);
JPH::BodyID add_soft_body(const JoltObject3D &p_object, const JPH::SoftBodyCreationSettings &p_settings, bool p_sleeping = false);
JPH::Body *add_rigid_body(const JoltObject3D &p_object, const JPH::BodyCreationSettings &p_settings, bool p_sleeping = false);
JPH::Body *add_soft_body(const JoltObject3D &p_object, const JPH::SoftBodyCreationSettings &p_settings, bool p_sleeping = false);
void remove_body(const JPH::BodyID &p_body_id);