Skip to content

Commit

Permalink
rework the bodyId fusion
Browse files Browse the repository at this point in the history
  • Loading branch information
@Lamakaio
Lamakaio committed Jun 16, 2024
1 parent 523a707 commit 816b7a7
Show file tree
Hide file tree
Showing 4 changed files with 70 additions and 53 deletions.
41 changes: 27 additions & 14 deletions src/ospjolt/activescene/joltinteg.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -66,30 +66,41 @@ JPH_SUPPRESS_WARNING_POP
#include <iostream>
#include <cstdarg>
#include <thread>
#include <logging.h>
#include <osp/util/logging.h>
#include <concepts>
#include <typeindex>

#include "osp/core/strong_id.h"

namespace ospjolt
{
using namespace JPH;

using JoltBodyPtr_t = std::unique_ptr< Body >;

static constexpr osp::Vector3 Vec3JoltToMagnum(Vec3 in)
using BodyId = osp::StrongId<uint32, struct DummyForBodyId>;

inline JPH::BodyID BToJolt(const BodyId& bodyId)
{
return JPH::BodyID(bodyId.value, 0);
}

inline osp::Vector3 Vec3JoltToMagnum(Vec3 in)
{
return Vector3(in.GetX(), in.GetY(), in.GetZ());
return osp::Vector3(in.GetX(), in.GetY(), in.GetZ());
}

static constexpr Vec3 Vec3MagnumToJolt(osp::Vector3 in)
inline Vec3 Vec3MagnumToJolt(osp::Vector3 in)
{
return Vec3(in.x(), in.y(), in.z());
}

static constexpr Quat QuatMagnumToJolt(osp::Quaternion in)
inline Quat QuatMagnumToJolt(osp::Quaternion in)
{
return Quat(in.vector().x(), in.vector().y(), in.vector().z(), in.scalar());
}

static constexpr osp::Quaternion QuatJoltToMagnum(Quat in)
inline osp::Quaternion QuatJoltToMagnum(Quat in)
{
return osp::Quaternion(osp::Vector3(in.GetX(), in.GetY(), in.GetZ()), in.GetW());
}
Expand Down Expand Up @@ -246,7 +257,7 @@ struct ACtxJoltWorld
struct ForceFactorFunc
{
using UserData_t = std::array<void*, 6u>;
using Func_t = void (*)(BodyID bodyId, ACtxJoltWorld const&, UserData_t, osp::Vector3&, osp::Vector3&) noexcept;
using Func_t = void (*)(BodyId bodyId, ACtxJoltWorld const&, UserData_t, osp::Vector3&, osp::Vector3&) noexcept;

Func_t m_func{nullptr};
UserData_t m_userData{nullptr};
Expand Down Expand Up @@ -282,6 +293,8 @@ struct ACtxJoltWorld
std::call_once(ACtxJoltWorld::initFlag, ACtxJoltWorld::initJoltGlobalInternal);
}



TempAllocatorImpl m_temp_allocator;
ObjectLayerPairFilterImpl m_objectLayerFilter;
BPLayerInterfaceImpl m_bPLInterface;
Expand All @@ -292,12 +305,12 @@ struct ACtxJoltWorld

std::unique_ptr<PhysicsStepListenerImpl> m_listener;

lgrn::IdRegistryStl<BodyID> m_bodyIds;
osp::KeyedVec<BodyID, ForceFactors_t> m_bodyFactors;
lgrn::IdSetStl<BodyID> m_bodyDirty;
lgrn::IdRegistryStl<BodyId> m_bodyIds;
osp::IdMap_t<BodyId, ForceFactors_t> m_bodyFactors;
lgrn::IdSetStl<BodyId> m_bodyDirty;

osp::KeyedVec<BodyID, osp::active::ActiveEnt> m_bodyToEnt;
osp::IdMap_t<osp::active::ActiveEnt, BodyID> m_entToBody;
osp::IdMap_t<BodyId, osp::active::ActiveEnt> m_bodyToEnt;
osp::IdMap_t<osp::active::ActiveEnt, BodyId> m_entToBody;

std::vector<ForceFactorFunc> m_factors;
ShapeStorage_t m_shapes;
Expand Down Expand Up @@ -327,10 +340,10 @@ struct ACtxJoltWorld
}


static std::once_flag initFlag;
inline static std::once_flag initFlag;

};


} //namespace ospjolt

}
40 changes: 21 additions & 19 deletions src/ospjolt/activescene/joltinteg_fn.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -50,8 +50,6 @@ using osp::Vector3;
void SysJolt::resize_body_data(ACtxJoltWorld& rCtxWorld)
{
std::size_t const capacity = rCtxWorld.m_bodyIds.capacity();
rCtxWorld.m_bodyToEnt .resize(capacity);
rCtxWorld.m_bodyFactors .resize(capacity);
}


Expand Down Expand Up @@ -94,7 +92,7 @@ void SysJolt::update_world(
// Apply changed velocities
for (auto const& [ent, vel] : std::exchange(rCtxPhys.m_setVelocity, {}))
{
BodyID const bodyId = rCtxWorld.m_entToBody.at(ent);
JPH::BodyID const bodyId = BToJolt(rCtxWorld.m_entToBody.at(ent));

bodyInterface.SetLinearVelocity(bodyId, Vec3MagnumToJolt(vel));
}
Expand All @@ -108,10 +106,14 @@ void SysJolt::update_world(
void SysJolt::remove_components(ACtxJoltWorld& rCtxWorld, ActiveEnt ent) noexcept
{
auto itBodyId = rCtxWorld.m_entToBody.find(ent);
BodyInterface &bodyInterface = rCtxWorld.m_pPhysicsSystem->GetBodyInterface();

if (itBodyId != rCtxWorld.m_entToBody.end())
{
BodyID const bodyId = itBodyId->second;
BodyId const bodyId = itBodyId->second;
JPH::BodyID joltBodyId = BToJolt(bodyId);
bodyInterface.RemoveBody(joltBodyId);
bodyInterface.DestroyBody(joltBodyId);
rCtxWorld.m_bodyIds.remove(bodyId);
rCtxWorld.m_bodyToEnt[bodyId] = lgrn::id_null<ActiveEnt>();
rCtxWorld.m_entToBody.erase(itBodyId);
Expand All @@ -130,14 +132,12 @@ Ref<Shape> SysJolt::create_primitive(ACtxJoltWorld &rCtxWorld, osp::EShape shape
return BoxShapeSettings(scale).Create().Get();
case EShape::Cylinder:
//cylinder needs to be internally rotated 90° to match with graphics
{
CylinderShapeSettings cylinderSettings(scale.GetY(), 2.0f * scale.GetX());
return RotatedTranslatedShapeSettings(
Vec3Arg::sZero(),
Quat::sRotation(Vec3::sAxisX(), JPH_PI/2),
&cylinderSettings
).Create().Get();
}
return RotatedTranslatedShapeSettings(
Vec3Arg::sZero(),
Quat::sRotation(Vec3::sAxisX(), JPH_PI/2),
new CylinderShapeSettings(scale.GetY(), 2.0f * scale.GetX())
).Create().Get();

default:
return SphereShapeSettings(1.0f * scale.GetX()).Create().Get();
}
Expand All @@ -158,11 +158,11 @@ void SysJolt::scale_shape(Ref<Shape> rShape, Vec3Arg scale) {
}

float SysJolt::get_inverse_mass_no_lock(PhysicsSystem &physicsSystem,
BodyID bodyId) {
BodyId bodyId) {
const BodyLockInterfaceNoLock &lockInterface =
physicsSystem.GetBodyLockInterfaceNoLock();
{
JPH::BodyLockRead lock(lockInterface, bodyId);
JPH::BodyLockRead lock(lockInterface, BToJolt(bodyId));
if (lock.Succeeded()) // body_id may no longer be valid
{
const JPH::Body &body = lock.GetBody();
Expand Down Expand Up @@ -224,17 +224,19 @@ void PhysicsStepListenerImpl::OnStep(float inDeltaTime, PhysicsSystem &rJoltWorl
{
//no lock as all bodies are already locked
BodyInterface &bodyInterface = rJoltWorld.GetBodyInterfaceNoLock();
for (BodyID bodyId : m_context->m_bodyIds)
for (BodyId bodyId : m_context->m_bodyIds)
{
if (bodyInterface.GetMotionType(bodyId) != EMotionType::Dynamic)
JPH::BodyID joltBodyId = BToJolt(bodyId);
if (bodyInterface.GetMotionType(joltBodyId) != EMotionType::Dynamic)
{
continue;
}

//Transform jolt -> osp


ActiveEnt const ent = m_context->m_bodyToEnt[bodyId];
Mat44 worldTranform = bodyInterface.GetWorldTransform(bodyId);
Mat44 worldTranform = bodyInterface.GetWorldTransform(joltBodyId);

worldTranform.StoreFloat4x4((Float4*)m_context->m_pTransform->get(ent).m_transform.data());

Expand All @@ -248,8 +250,8 @@ void PhysicsStepListenerImpl::OnStep(float inDeltaTime, PhysicsSystem &rJoltWorl
ACtxJoltWorld::ForceFactorFunc const& factor = m_context->m_factors[factorIdx];
factor.m_func(bodyId, *m_context, factor.m_userData, force, torque);
}
Vec3 vel = bodyInterface.GetLinearVelocity(bodyId);
Vec3 vel = bodyInterface.GetLinearVelocity(joltBodyId);

bodyInterface.AddForceAndTorque(bodyId, Vec3MagnumToJolt(force), Vec3MagnumToJolt(torque));
bodyInterface.AddForceAndTorque(joltBodyId, Vec3MagnumToJolt(force), Vec3MagnumToJolt(torque));
}
}
2 changes: 1 addition & 1 deletion src/ospjolt/activescene/joltinteg_fn.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -93,7 +93,7 @@ class SysJolt
static void scale_shape(Ref<Shape> rJoltShap, Vec3Arg scale);

//Get the inverse mass of a jolt body
static float get_inverse_mass_no_lock(PhysicsSystem& physicsSystem, BodyID bodyId);
static float get_inverse_mass_no_lock(PhysicsSystem& physicsSystem, BodyId bodyId);

private:

Expand Down
40 changes: 21 additions & 19 deletions src/testapp/sessions/jolt.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -147,7 +147,7 @@ osp::Session setup_jolt_force_accel(

ACtxJoltWorld::ForceFactorFunc const factor
{
.m_func = [] (BodyID const bodyId, ACtxJoltWorld const& rJolt, UserData_t data, Vector3& rForce, Vector3& rTorque) noexcept
.m_func = [] (BodyId const bodyId, ACtxJoltWorld const& rJolt, UserData_t data, Vector3& rForce, Vector3& rTorque) noexcept
{
PhysicsSystem *pJoltWorld = rJolt.m_pPhysicsSystem.get();

Expand Down Expand Up @@ -211,7 +211,7 @@ Session setup_phys_shapes_jolt(

Ref<Shape> pShape = SysJolt::create_primitive(rJolt, spawn.m_shape, Vec3MagnumToJolt(spawn.m_size));

BodyID const bodyId = rJolt.m_bodyIds.create();
BodyId const bodyId = rJolt.m_bodyIds.create();
SysJolt::resize_body_data(rJolt);

BodyCreationSettings bodyCreation(pShape,
Expand All @@ -234,10 +234,12 @@ Session setup_phys_shapes_jolt(
bodyCreation.mMotionType = EMotionType::Static;
bodyCreation.mObjectLayer = Layers::MOVING;
}
//TODO helper function ?
PhysicsSystem *pJoltWorld = rJolt.m_pPhysicsSystem.get();
BodyInterface &bodyInterface = pJoltWorld->GetBodyInterface();
bodyInterface.CreateBodyWithID(bodyId, bodyCreation);
bodyInterface.AddBody(bodyId, EActivation::Activate);
JPH::BodyID joltBodyId = BToJolt(bodyId);
bodyInterface.CreateBodyWithID(joltBodyId, bodyCreation);
bodyInterface.AddBody(joltBodyId, EActivation::Activate);

rJolt.m_bodyToEnt[bodyId] = root;
rJolt.m_bodyFactors[bodyId] = joltFactors;
Expand Down Expand Up @@ -452,10 +454,9 @@ Session setup_vehicle_spawn_jolt(
Ref<Shape> compoundShape = compound.Create().Get();
BodyCreationSettings bodyCreation(compoundShape, Vec3Arg::sZero(), Quat::sZero(), EMotionType::Dynamic, Layers::MOVING);

BodyID const bodyId = rJolt.m_bodyIds.create();
BodyId const bodyId = rJolt.m_bodyIds.create();
SysJolt::resize_body_data(rJolt);


rJolt.m_bodyToEnt[bodyId] = weldEnt;
rJolt.m_bodyFactors[bodyId] = {1}; // TODO: temporary
rJolt.m_entToBody.emplace(weldEnt, bodyId);
Expand Down Expand Up @@ -491,9 +492,9 @@ Session setup_vehicle_spawn_jolt(

PhysicsSystem *pJoltWorld = rJolt.m_pPhysicsSystem.get();
BodyInterface &bodyInterface = pJoltWorld->GetBodyInterface();

bodyInterface.CreateBodyWithID(bodyId, bodyCreation);
bodyInterface.AddBody(bodyId, EActivation::Activate);
JPH::BodyID joltBodyId = BToJolt(bodyId);
bodyInterface.CreateBodyWithID(joltBodyId, bodyCreation);
bodyInterface.AddBody(joltBodyId, EActivation::Activate);

rPhys.m_setVelocity.emplace_back(weldEnt, toInit.velocity);
});
Expand All @@ -519,7 +520,8 @@ struct BodyRocket
struct ACtxRocketsJolt
{
// map each bodyId to a {machine, offset}
lgrn::IntArrayMultiMap<BodyID, BodyRocket> m_bodyRockets;
//TODO: make an IdMultiMap or something.
lgrn::IntArrayMultiMap<BodyId::entity_type, BodyRocket> m_bodyRockets;

};

Expand All @@ -539,11 +541,11 @@ static void assign_rockets(
using adera::ports_magicrocket::gc_multiplierIn;

ActiveEnt const weldEnt = rScnParts.weldToActive[weld];
BodyID const body = rJolt.m_entToBody.at(weldEnt);
BodyId const body = rJolt.m_entToBody.at(weldEnt);

if (rRocketsJolt.m_bodyRockets.contains(body))
if (rRocketsJolt.m_bodyRockets.contains(body.value))
{
rRocketsJolt.m_bodyRockets.erase(body);
rRocketsJolt.m_bodyRockets.erase(body.value);
}

for (PartId const part : rScnParts.weldToParts[weld])
Expand Down Expand Up @@ -617,18 +619,18 @@ static void assign_rockets(

rBodyFactors[0] |= rJoltFactors[0];

rRocketsJolt.m_bodyRockets.emplace(body, rTemp.begin(), rTemp.end());
rRocketsJolt.m_bodyRockets.emplace(body.value, rTemp.begin(), rTemp.end());
rTemp.clear();
}

// ACtxjoltWorld::ForceFactorFunc::Func_t
static void rocket_thrust_force(BodyID const bodyId, ACtxJoltWorld const& rJolt, ACtxJoltWorld::ForceFactorFunc::UserData_t data, Vector3& rForce, Vector3& rTorque) noexcept
static void rocket_thrust_force(BodyId const bodyId, ACtxJoltWorld const& rJolt, ACtxJoltWorld::ForceFactorFunc::UserData_t data, Vector3& rForce, Vector3& rTorque) noexcept
{
auto const& rRocketsJolt = *reinterpret_cast<ACtxRocketsJolt const*> (data[0]);
auto const& rMachines = *reinterpret_cast<Machines const*> (data[1]);
auto const& rSigValFloat = *reinterpret_cast<SignalValues_t<float> const*> (data[2]);

auto &rBodyRockets = rRocketsJolt.m_bodyRockets[bodyId];
auto &rBodyRockets = rRocketsJolt.m_bodyRockets[bodyId.value];

PhysicsSystem *pJoltWorld = rJolt.m_pPhysicsSystem.get();
//no lock as all bodies are locked in callbacks
Expand All @@ -639,9 +641,9 @@ static void rocket_thrust_force(BodyID const bodyId, ACtxJoltWorld const& rJolt,
return;
}

Quaternion const rot = QuatJoltToMagnum(bodyInterface.GetRotation(bodyId));

RVec3 joltCOM = bodyInterface.GetCenterOfMassPosition(bodyId) - bodyInterface.GetPosition(bodyId);
JPH::BodyID joltBodyId = BToJolt(bodyId);
Quaternion const rot = QuatJoltToMagnum(bodyInterface.GetRotation(joltBodyId));
RVec3 joltCOM = bodyInterface.GetCenterOfMassPosition(joltBodyId) - bodyInterface.GetPosition(joltBodyId);
Vector3 com = Vec3JoltToMagnum(joltCOM);

for (BodyRocket const& bodyRocket : rBodyRockets)
Expand Down

0 comments on commit 816b7a7

Please sign in to comment.