godotengine/godot
1
0
Fork 0
mirror of https://github.com/godotengine/godot.git synced 2025-11-16 14:00:40 +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

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