diff --git a/Engine/lib/JoltPhysics-5.5.0.zip b/Engine/lib/JoltPhysics-5.5.0.zip new file mode 100644 index 000000000..3f4dc05b6 Binary files /dev/null and b/Engine/lib/JoltPhysics-5.5.0.zip differ diff --git a/Engine/source/CMakeLists.txt b/Engine/source/CMakeLists.txt index eec01e35b..872188c44 100644 --- a/Engine/source/CMakeLists.txt +++ b/Engine/source/CMakeLists.txt @@ -21,6 +21,7 @@ find_package(SndFile CONFIG REQUIRED) find_package(TinyXML2 CONFIG REQUIRED) find_package(assimp CONFIG REQUIRED) find_package(OpenAL CONFIG REQUIRED) +find_package(Jolt CONFIG REQUIRED) # ------------------------------------------------------------------ # Collect imported targets into a single variable @@ -41,6 +42,7 @@ set(TORQUE_LINK_THIRDPARTY ${TORQUE_LINK_THIRDPARTY} tinyxml2::tinyxml2 assimp::assimp OpenAL::OpenAL + Jolt::Jolt ) if (MSVC) @@ -105,7 +107,7 @@ torqueAddSourceDirectories("platform/nativeDialogs") torqueAddSourceDirectories( "T3D" "T3D/AI" "T3D/assets" "T3D/decal" "T3D/examples" "T3D/fps" "T3D/fx" "T3D/gameBase" "T3D/gameBase/std" "T3D/lighting" - "T3D/physics" + "T3D/physics" "T3D/physics/jolt" # "T3D/components" "T3D/sceneComponent" "T3D/systems" "T3D/gameOBjects" "T3D/sfx" "T3D/turret" "T3D/vehicles") diff --git a/Engine/source/T3D/physics/jolt/joltBody.cpp b/Engine/source/T3D/physics/jolt/joltBody.cpp new file mode 100644 index 000000000..e5953b380 --- /dev/null +++ b/Engine/source/T3D/physics/jolt/joltBody.cpp @@ -0,0 +1,363 @@ +#include "platform/platform.h" +#include "T3D/physics/jolt/joltBody.h" + +#include "math/mBox.h" +#include "console/console.h" +#include "scene/sceneObject.h" + +JoltBody::JoltBody() + :mBody(NULL), + mWorld(NULL), + mMass(0.0f), + mCenterOfMass(NULL), + mInvCenterOfMass(NULL), + mIsDynamic(false), + mIsEnabled(false) + +{ +} + +JoltBody::~JoltBody() +{ + SAFE_DELETE(mCenterOfMass); + SAFE_DELETE(mInvCenterOfMass); + + mColShape = NULL; + setSimulationEnabled(false); + JPH::BodyInterface& bi = mWorld->getPhysicsSystem()->GetBodyInterface(); + + bi.DestroyBody(mBody->GetID()); + mBody->SetUserData(NULL); + mBody = NULL; +} + +PhysicsWorld* JoltBody::getWorld() +{ + return mWorld; +} + +PhysicsCollision* JoltBody::getColShape() +{ + return mColShape; +} + +bool JoltBody::init(PhysicsCollision* shape, F32 mass, U32 bodyFlags, SceneObject* obj, PhysicsWorld* world) +{ + if (!shape || !world) + return false; + + mWorld = dynamic_cast(world); + if (!mWorld) + return false; + + mColShape = dynamic_cast(shape); + if (!mColShape) + return false; + + mIsDynamic = false; + + const JPH::Shape* joltShape = mColShape->getJoltShape(); + + // Determine motion type + JPH::EMotionType motionType = JPH::EMotionType::Static; + JPH::ObjectLayer layer = Layers::NON_MOVING; + + if (bodyFlags & BF_KINEMATIC) + { + motionType = JPH::EMotionType::Kinematic; + layer = Layers::MOVING; + } + else if (mass > 0.0f) + { + motionType = JPH::EMotionType::Dynamic; + layer = Layers::MOVING; + } + + MatrixF objXfm = obj->getTransform(); + + QuatF angPos(objXfm); + Point3F pos; + objXfm.getColumn(3, &pos); + + JPH::BodyCreationSettings settings( + joltShape, + joltCast(pos), // initial position + joltCast(angPos), // rotation + motionType, + layer + ); + + // Mass override if dynamic + if (motionType == JPH::EMotionType::Dynamic) + { + settings.mOverrideMassProperties = JPH::EOverrideMassProperties::CalculateInertia; + settings.mMassPropertiesOverride.mMass = mass; + settings.mMaxLinearVelocity = 1000.0f; // stops an assert, this is torques upper limit, jolts is 500.0f by default. + mIsDynamic = true; + } + + JPH::BodyInterface& bi = mWorld->getPhysicsSystem()->GetBodyInterface(); + JPH::Body* body = bi.CreateBody(settings); + bi.AddBody(body->GetID(), JPH::EActivation::Activate); + + if (bodyFlags & BF_TRIGGER) + { + body->SetIsSensor(true); + } + + mBody = body; + + getUserData().setObject(obj); + getUserData().setBody(this); + mBody->SetUserData(reinterpret_cast(&mUserData)); + + const JPH::Mat44 trans = mBody->GetCenterOfMassTransform(); + MatrixF comMat; + QuatF qang = joltCast(trans.GetQuaternion()); + qang.setMatrix(&comMat); + comMat.setPosition(joltCast(trans.GetTranslation())); + + mCenterOfMass = new MatrixF(comMat); + mInvCenterOfMass = new MatrixF(*mCenterOfMass); + mInvCenterOfMass->inverse(); + + return true; +} + +void JoltBody::setTransform(const MatrixF& xfm) +{ + if (!isValid()) + return; + + Point3F pos = xfm.getPosition(); + QuatF q(xfm); + JPH::BodyInterface& bi = mWorld->getPhysicsSystem()->GetBodyInterface(); + + bi.SetPositionAndRotation( + mBody->GetID(), + joltCast(pos), + joltCast(q), + mIsEnabled ? JPH::EActivation::Activate : JPH::EActivation::DontActivate + ); + + if (isDynamic()) + { + mBody->ResetMotion();// resets accumulated torque and forces, also velocity but do this explicitly + mBody->SetLinearVelocity(JPH::Vec3::sZero()); + mBody->SetAngularVelocity(JPH::Vec3::sZero()); + } +} + +void JoltBody::applyCorrection(const MatrixF& xfm) +{ + setTransform(xfm); +} + +MatrixF& JoltBody::getTransform(MatrixF* outMatrix) +{ + const JPH::Mat44 trans = mBody->GetWorldTransform(); + *outMatrix = joltCast(trans); + + return *outMatrix; +} + +void JoltBody::applyImpulse(const Point3F& origin, const Point3F& force) +{ + mBody->AddImpulse(joltCast(force), joltCast(origin)); +} + +void JoltBody::applyTorque(const Point3F& torque) +{ + mBody->AddTorque(joltCast(torque)); +} + +void JoltBody::applyForce(const Point3F& force) +{ + mBody->AddForce(joltCast(force)); +} + +void JoltBody::moveKinematicTo(const MatrixF& xfm) +{ + Point3F pos = xfm.getPosition(); + QuatF angPos(xfm); + mWorld->getPhysicsSystem()->GetBodyInterface().MoveKinematic( + mBody->GetID(), + joltCast(pos), + joltCast(angPos), + TickSec + ); +} + +bool JoltBody::isValid() +{ + return mBody != nullptr && mWorld != NULL; +} + +Box3F JoltBody::getWorldBounds() +{ + JPH::AABox box = mBody->GetWorldSpaceBounds(); + + return Box3F( + Point3F(joltCast(box.mMin)), + Point3F(joltCast(box.mMax)) + ); +} + +void JoltBody::setSimulationEnabled(bool enabled) +{ + if (!isValid()) + return; + + JPH::BodyInterface& bi = mWorld->getPhysicsSystem()->GetBodyInterface(); + + if (enabled) + { + if (bi.IsAdded(mBody->GetID())) + { + bi.ActivateBody(mBody->GetID()); + } + else + { + bi.AddBody(mBody->GetID(), JPH::EActivation::Activate); + } + } + else + { + bi.DeactivateBody(mBody->GetID()); + bi.RemoveBody(mBody->GetID()); + } + + mIsEnabled = enabled; +} + +void JoltBody::setSleepThreshold(F32 linear, F32 angular) +{ + if (!mBody->IsDynamic()) + return; + + JPH::MotionProperties* mp = mBody->GetMotionProperties(); + + /* mp->SetLinearSleepThreshold(linear); + mp->SetAngularSleepThreshold(angular);*/ +} + +void JoltBody::setDamping(F32 linear, F32 angular) +{ + if (!mBody->IsDynamic()) + return; + + JPH::MotionProperties* mp = mBody->GetMotionProperties(); + + mp->SetLinearDamping(linear); + mp->SetAngularDamping(angular); +} + +void JoltBody::setSleeping(bool sleeping) +{ + if (!isValid()) + return; + + if (!mIsEnabled) + return; + + JPH::BodyInterface& bi = mWorld->getPhysicsSystem()->GetBodyInterface(); + + if (sleeping) + bi.DeactivateBody(mBody->GetID()); + else + bi.ActivateBody(mBody->GetID()); +} + +void JoltBody::setMaterial(F32 restitution, F32 friction, F32 staticFriction) +{ + mBody->SetRestitution(restitution); + mBody->SetFriction(friction); +} + +void JoltBody::getState(PhysicsState* outState) +{ + if (!outState) + return; + + const JPH::RMat44 worldXfm = mBody->GetWorldTransform(); + + outState->position = joltCast(worldXfm.GetTranslation()); + outState->orientation = joltCast(mBody->GetRotation()); + outState->linVelocity = getLinVelocity(); + outState->angVelocity = getAngVelocity(); + + if (mBody->IsDynamic()) + { + const JPH::MotionProperties* mp = mBody->GetMotionProperties(); + + // Linear momentum = m * v + F32 mass = mp->GetInverseMass() > 0.0f ? 1.0f / mp->GetInverseMass() : 0.0f; + outState->momentum = outState->linVelocity * mass; + + // Angular momentum + JPH::Mat44 I_world = mBody->GetInverseInertia(); // 3x3 part + JPH::Vec3 angMomentum = I_world.Multiply3x3(mBody->GetAngularVelocity()); + outState->angularMomentum = joltCast(angMomentum); + } + else + { + outState->momentum.set(0, 0, 0); + outState->angularMomentum.set(0, 0, 0); + } + + // Sleeping? + outState->sleeping = !mBody->IsActive(); +} + +Point3F JoltBody::getCMassPosition() const +{ + JPH::RVec3 p = mBody->GetCenterOfMassPosition(); + return Point3F(joltCast(p)); +} + +Point3F JoltBody::getLinVelocity() const +{ + return joltCast(mBody->GetLinearVelocity()); +} + +Point3F JoltBody::getAngVelocity() const +{ + return joltCast(mBody->GetAngularVelocity()); +} + +void JoltBody::setLinVelocity(const Point3F& vel) +{ + if (!isValid()) + return; + + JPH::BodyInterface& bi = mWorld->getPhysicsSystem()->GetBodyInterface(); + + if (!bi.IsAdded(mBody->GetID())) + return; + + mBody->SetLinearVelocity(joltCast(vel)); +} + +void JoltBody::setAngVelocity(const Point3F& vel) +{ + if (!isValid()) + return; + + JPH::BodyInterface& bi = mWorld->getPhysicsSystem()->GetBodyInterface(); + + if (!bi.IsAdded(mBody->GetID())) + return; + + mBody->SetAngularVelocity(joltCast(vel)); +} + +void JoltBody::findContact(SceneObject** contactObject, + VectorF* contactNormal, + Vector* outOverlapObjects) const +{ + if (contactObject) + *contactObject = nullptr; + + if (contactNormal) + *contactNormal = VectorF::Zero; +} diff --git a/Engine/source/T3D/physics/jolt/joltBody.h b/Engine/source/T3D/physics/jolt/joltBody.h new file mode 100644 index 000000000..062da0ca3 --- /dev/null +++ b/Engine/source/T3D/physics/jolt/joltBody.h @@ -0,0 +1,94 @@ +#ifndef _JOLTBODY_H_ +#define _JOLTBODY_H_ + +#ifndef _T3D_PHYSICS_PHYSICSBODY_H_ +#include "T3D/physics/physicsBody.h" +#endif +#ifndef _REFBASE_H_ +#include "core/util/refBase.h" +#endif +#ifndef _MMATRIX_H_ +#include "math/mMatrix.h" +#endif + +#ifndef _T3D_JOLT_PLUGIN_H_ +#include "T3D/physics/jolt/joltPlugin.h" +#endif + +#ifndef _JOLTWORLD_H_ +#include "T3D/physics/jolt/joltWorld.h" +#endif + +#ifndef _JOLTCOLLISION_H_ +#include "T3D/physics/jolt/joltCollision.h" +#endif + + +class JoltBody : public PhysicsBody +{ +protected: + JPH::Body* mBody; + + JoltWorld* mWorld; + JoltCollisionRef mColShape; + + /// + F32 mMass; + + /// + bool mIsDynamic; + + /// Is the body participating in the physics simulation. + bool mIsEnabled; + + /// The center of mass offset used if the graphical + /// transform is not at the mass center. + MatrixF* mCenterOfMass; + + /// The inverse center of mass offset. + MatrixF* mInvCenterOfMass; + +public: + JoltBody(); + virtual ~JoltBody(); + + // PhysicsObject + virtual PhysicsWorld* getWorld(); + virtual void setTransform(const MatrixF& xfm); + virtual MatrixF& getTransform(MatrixF* outMatrix); + virtual Box3F getWorldBounds(); + virtual void setSimulationEnabled(bool enabled); + virtual bool isSimulationEnabled() { return mIsEnabled; } + + // PhysicsBody + virtual bool init(PhysicsCollision* shape, + F32 mass, + U32 bodyFlags, + SceneObject* obj, + PhysicsWorld* world); + bool isDynamic() const override { return mIsDynamic; } + PhysicsCollision* getColShape() override; + void setSleepThreshold(F32 linear, F32 angular) override; + void setDamping(F32 linear, F32 angular) override; + void getState(PhysicsState* outState) override; + F32 getMass() const override { return mMass; } + Point3F getCMassPosition() const override; + void setLinVelocity(const Point3F& vel) override; + void setAngVelocity(const Point3F& vel) override; + Point3F getLinVelocity() const override; + Point3F getAngVelocity() const override; + void setSleeping(bool sleeping) override; + void setMaterial(F32 restitution, + F32 friction, + F32 staticFriction) override; + void applyCorrection(const MatrixF& xfm) override; + void applyImpulse(const Point3F& origin, const Point3F& force) override; + void applyTorque(const Point3F& torque) override; + void applyForce(const Point3F& force) override; + void findContact(SceneObject** contactObject, VectorF* contactNormal, Vector* outOverlapObjects) const override; + void moveKinematicTo(const MatrixF& xfm) override; + + bool isValid() override; +}; + +#endif // !_JOLTBODY_H_ diff --git a/Engine/source/T3D/physics/jolt/joltCollision.cpp b/Engine/source/T3D/physics/jolt/joltCollision.cpp new file mode 100644 index 000000000..5b5e41d4d --- /dev/null +++ b/Engine/source/T3D/physics/jolt/joltCollision.cpp @@ -0,0 +1,353 @@ +#include "T3D/physics/jolt/joltCollision.h" +#include "terrain/terrFile.h" + +// Save and undefine the macro if it exists +#ifdef Offset + #pragma push_macro("Offset") + #undef Offset +#endif + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#ifdef Offset + // Restore the original macro after includes + #pragma pop_macro("Offset") +#endif + +JoltCollision::JoltCollision() +{ + VECTOR_SET_ASSOCIATION(mChildren); +} + +JoltCollision::~JoltCollision() +{ +} + +void JoltCollision::addPlane(const PlaneF& plane) +{ + JPH::Plane joltPlane({ plane.x, plane.y, plane.z }, plane.d); + JPH::PlaneShapeSettings planeSettings(joltPlane); + auto result = planeSettings.Create(); + if (result.HasError()) + { + Con::errorf("Jolt Error: %s", result.GetError().c_str()); + return; + } + + ChildShapeEntry entry; + entry.shape = result.Get(); + entry.localXfm = JPH::Mat44::sIdentity(); // Plane at origin by default + entry.localPos = JPH::Vec3::sZero(); + entry.localRot = JPH::Quat::sIdentity(); + mChildren.push_back(entry); + + rebuildCompound(); +} + +void JoltCollision::addBox(const Point3F& halfWidth, const MatrixF& localXfm) +{ + JPH::BoxShapeSettings boxSettings(JPH::Vec3(halfWidth.x, halfWidth.y, halfWidth.z)); + + auto result = boxSettings.Create(); + if (result.HasError()) + { + Con::errorf("Jolt Error: %s", result.GetError().c_str()); + return; + } + + auto baseShape = result.Get(); + + // Convert engine matrix to Jolt position + rotation + JPH::Vec3 localPos; + JPH::Quat localRot; + toJolt(localXfm, localPos, localRot); + + // Wrap the base shape with RotatedTranslatedShape + auto rtsSettings = new JPH::RotatedTranslatedShapeSettings(localPos, localRot, baseShape); + auto rtsShape = rtsSettings->Create().Get(); + + // Store in child entry + ChildShapeEntry entry; + entry.shape = rtsShape; + entry.localPos = localPos; + entry.localRot = localRot; + entry.localXfm = joltCast(localXfm); + mChildren.push_back(entry); + + rebuildCompound(); +} + +void JoltCollision::addSphere(F32 radius, const MatrixF& localXfm) +{ + JPH::SphereShapeSettings settings(radius); + auto result = settings.Create(); + if (result.HasError()) + { + Con::errorf("Jolt Error: %s", result.GetError().c_str()); + return; + } + + auto baseShape = result.Get(); + + JPH::Vec3 localPos; + JPH::Quat localRot; + toJolt(localXfm, localPos, localRot); + + auto rtsSettings = new JPH::RotatedTranslatedShapeSettings(localPos, localRot, baseShape); + auto rtsShape = rtsSettings->Create().Get(); + + ChildShapeEntry entry; + entry.shape = rtsShape; + entry.localPos = localPos; + entry.localRot = localRot; + entry.localXfm = joltCast(localXfm); + mChildren.push_back(entry); + + rebuildCompound(); +} + +void JoltCollision::addCapsule(F32 radius, F32 height, const MatrixF& localXfm) +{ + JPH::CapsuleShapeSettings settings(radius, height); + auto result = settings.Create(); + if (result.HasError()) + { + Con::errorf("Jolt Error: %s", result.GetError().c_str()); + return; + } + + auto baseShape = result.Get(); + + JPH::Vec3 localPos; + JPH::Quat localRot; + toJolt(localXfm, localPos, localRot); + + auto rtsSettings = new JPH::RotatedTranslatedShapeSettings(localPos, localRot, baseShape); + auto rtsShape = rtsSettings->Create().Get(); + + ChildShapeEntry entry; + entry.shape = rtsShape; + entry.localPos = localPos; + entry.localRot = localRot; + entry.localXfm = joltCast(localXfm); + mChildren.push_back(entry); + + rebuildCompound(); +} + +bool JoltCollision::addConvex(const Point3F* points, U32 count, const MatrixF& localXfm) +{ + if (count == 0) + return false; + + std::vector verts; + verts.reserve(count); + for (U32 i = 0; i < count; ++i) + verts.emplace_back(points[i].x, points[i].y, points[i].z); + + JPH::ConvexHullShapeSettings settings(verts.data(), verts.size()); + auto result = settings.Create(); + if (result.HasError()) + { + Con::errorf("Jolt Error: %s", result.GetError().c_str()); + return false; + } + + auto baseShape = result.Get(); + + JPH::Vec3 localPos; + JPH::Quat localRot; + toJolt(localXfm, localPos, localRot); + + auto rtsSettings = new JPH::RotatedTranslatedShapeSettings(localPos, localRot, baseShape); + auto rtsShape = rtsSettings->Create().Get(); + + ChildShapeEntry entry; + entry.shape = rtsShape; + entry.localPos = localPos; + entry.localRot = localRot; + entry.localXfm = joltCast(localXfm); + mChildren.push_back(entry); + + rebuildCompound(); + return true; +} + +bool JoltCollision::addTriangleMesh(const Point3F* vert, U32 vertCount, const U32* index, U32 triCount, const MatrixF& localXfm) +{ + if (!vert || !index || vertCount == 0 || triCount == 0) + return false; + + // Build the TriangleList + JPH::TriangleList triangles; + triangles.reserve(triCount); + + for (U32 i = 0; i < triCount; ++i) + { + const Point3F& v0 = vert[index[i * 3 + 0]]; + const Point3F& v1 = vert[index[i * 3 + 1]]; + const Point3F& v2 = vert[index[i * 3 + 2]]; + + triangles.push_back( + JPH::Triangle( + JPH::Float3(v0.x, v0.y, v0.z), + JPH::Float3(v2.x, v2.y, v2.z), + JPH::Float3(v1.x, v1.y, v1.z), + 0, // material index + i // user data = original triangle index + ) + ); + } + + // Create the MeshShape + JPH::MeshShapeSettings settings(triangles); + auto result = settings.Create(); + if (result.HasError()) + { + Con::errorf("Jolt Error: %s", result.GetError().c_str()); + return false; + } + + auto baseShape = result.Get(); + + JPH::Vec3 localPos; + JPH::Quat localRot; + toJolt(localXfm, localPos, localRot); + + auto rtsSettings = new JPH::RotatedTranslatedShapeSettings(localPos, localRot, baseShape); + auto rtsShape = rtsSettings->Create().Get(); + + ChildShapeEntry entry; + entry.shape = rtsShape; + entry.localPos = localPos; + entry.localRot = localRot; + entry.localXfm = joltCast(localXfm); + mChildren.push_back(entry); + + rebuildCompound(); + return true; +} + +bool JoltCollision::addHeightfield( + const U16* heightData, + const bool* holes, + U32 blockSize, + F32 metersPerSample, + const MatrixF& localXfm) // We won’t use localXfm directly +{ + if (!heightData || blockSize == 0) + return false; + + const F32 heightScale = 0.03125f; + const F32 minHeight = 0; + const F32 maxHeight = 65535 * heightScale; + + const U32 sampleCount = blockSize * blockSize; + std::vector samples(sampleCount); + + for (U32 x = 0; x < blockSize; ++x) + { + for (U32 y = 0; y < blockSize; ++y) + { + // Terrain storage (row-major) + U32 srcIdx = y * blockSize + x; + + // Flip Z direction for Jolt + U32 dstIdx = (blockSize - 1 - y) * blockSize + x; + + float h = fixedToFloat(heightData[srcIdx]); + + if (holes && holes[srcIdx]) + h = JPH::HeightFieldShapeConstants::cNoCollisionValue; + + samples[dstIdx] = h; + } + } + + float terrainSize = blockSize * metersPerSample; + float verticalAdjust = -heightScale * 0.5f; + JPH::Vec3 joltOffset( + 0.0, + verticalAdjust, + -terrainSize +); + + JPH::Vec3 joltScale(metersPerSample,1.0f, metersPerSample); + + JPH::HeightFieldShapeSettings settings(samples.data(), joltOffset, joltScale, blockSize); + settings.mBlockSize = metersPerSample; + + auto result = settings.Create(); + if (result.HasError()) + { + Con::errorf("Jolt Error: %s", result.GetError().c_str()); + return false; + } + + auto baseShape = result.Get(); + + JPH::Vec3 localPos; + JPH::Quat localRot; + toJolt(localXfm, localPos, localRot); + JPH::Quat rotFix = JPH::Quat::sRotation(JPH::Vec3::sAxisX(), JPH::DegreesToRadians(90.0f)); + localRot = rotFix * localRot; + + auto rtsSettings = new JPH::RotatedTranslatedShapeSettings(localPos, localRot, baseShape); + auto rtsShape = rtsSettings->Create().Get(); + + ChildShapeEntry entry; + entry.shape = rtsShape; + entry.localPos = localPos; + entry.localRot = localRot; + mChildren.push_back(entry); + + rebuildCompound(); + + return true; +} + +void JoltCollision::rebuildCompound() +{ + if (mChildren.empty()) + return; + + JPH::CompoundShapeSettings* compoundSettings = nullptr; + + if (mChildren.size() > 1) + { + compoundSettings = new JPH::StaticCompoundShapeSettings(); + } + + for (auto& colShape : mChildren) + { + if (compoundSettings) + { + compoundSettings->AddShape( + colShape.localPos, + colShape.localRot, + colShape.shape + ); + } + else + { + mCompundShape = colShape.shape; + } + } + + if (compoundSettings) + { + mCompundShape = compoundSettings->Create().Get(); + delete compoundSettings; + } + +} diff --git a/Engine/source/T3D/physics/jolt/joltCollision.h b/Engine/source/T3D/physics/jolt/joltCollision.h new file mode 100644 index 000000000..4ddfe30d4 --- /dev/null +++ b/Engine/source/T3D/physics/jolt/joltCollision.h @@ -0,0 +1,62 @@ +#ifndef _JOLTCOLLISION_H_ +#define _JOLTCOLLISION_H_ + +#ifndef _T3D_PHYSICS_PHYSICSCOLLISION_H_ +#include "T3D/physics/physicsCollision.h" +#endif + +#ifndef _T3D_JOLT_PLUGIN_H_ +#include "T3D/physics/jolt/joltPlugin.h" +#endif + +#ifndef _MMATRIX_H_ +#include "math/mMatrix.h" +#endif + +#ifndef _TVECTOR_H_ +#include "core/util/tVector.h" +#endif + +struct ChildShapeEntry +{ + JPH::Ref shape; + JPH::Mat44 localXfm; + JPH::Vec3 localPos; + JPH::Quat localRot; +}; + +class JoltCollision : public PhysicsCollision +{ +private: + JPH::Ref mCompundShape; + Vector mChildren; + +public: + JoltCollision(); + virtual ~JoltCollision(); + + // Add basic primitives + void addPlane(const PlaneF& plane) override; + + void addBox(const Point3F& halfWidth, const MatrixF& localXfm) override; + void addSphere(F32 radius, const MatrixF& localXfm) override; + void addCapsule(F32 radius, F32 height, const MatrixF& localXfm) override; + bool addConvex(const Point3F* points, U32 count, const MatrixF& localXfm) override; + bool addTriangleMesh(const Point3F* vert, U32 vertCount, + const U32* index, U32 triCount, + const MatrixF& localXfm) override; + bool addHeightfield(const U16* heights, const bool* holes, U32 blockSize, F32 metersPerSample, const MatrixF& localXfm) override; + + void rebuildCompound(); + + JPH::Ref getJoltShape() const { return mCompundShape; } +}; + +/// A strong pointer to a reference counted JoltCollision. +typedef StrongRefPtr JoltCollisionRef; + + +/// A weak pointer to a reference counted JoltCollision. +typedef WeakRefPtr JoltCollisionWeakRef; + +#endif // !_JOLTCOLLISION_H_ diff --git a/Engine/source/T3D/physics/jolt/joltPlayer.cpp b/Engine/source/T3D/physics/jolt/joltPlayer.cpp new file mode 100644 index 000000000..fe46c5363 --- /dev/null +++ b/Engine/source/T3D/physics/jolt/joltPlayer.cpp @@ -0,0 +1,540 @@ +#include "platform/platform.h" +#include "T3D/physics/jolt/joltPlayer.h" +#include "collision/collision.h" +#include "scene/sceneObject.h" +#include "T3D/player.h" + +// we use capsule for player? +#ifdef Offset +#pragma push_macro("Offset") +#undef Offset +#endif + +#include +#include +#include +#include +#include + +#ifdef Offset + // Restore the original macro after includes +#pragma pop_macro("Offset") +#endif + +JoltPlayer::JoltPlayer() + : mCharacter(NULL), + mObject(NULL), + mWorld(NULL), + mMaxSlopeCos(0.0f), + mStepHeight(0.0f), + mOriginOffset(0.0f), + mAllowSliding(false), + mCollisionEnabled(true), + mIsEnabled(false) +{ + mDesiredVelocity = JPH::Vec3::sZero(); +} + +JoltPlayer::~JoltPlayer() +{ + mCharacter->SetUserData(NULL); + mCharacter = NULL; + setSimulationEnabled(false); +} + +void JoltPlayer::setTransform(const MatrixF& xfm) +{ + if (!mCharacter) + return; + + JPH::Mat44 mat = joltCast(xfm); + + mCharacter->SetPosition(mat.GetTranslation()); + mCharacter->SetRotation(mat.GetQuaternion()); +} + +MatrixF& JoltPlayer::getTransform(MatrixF* outMatrix) +{ + const JPH::Mat44 trans = mCharacter->GetWorldTransform(); + *outMatrix = joltCast(trans); + return *outMatrix; +} + +Box3F JoltPlayer::getWorldBounds() +{ + JPH::AABox box = mCharacter->GetShape()->GetWorldSpaceBounds(mCharacter->GetCenterOfMassTransform(), JPH::Vec3::sOne()); + return Box3F( + Point3F(joltCast(box.mMin)), + Point3F(joltCast(box.mMax))); +} + +void JoltPlayer::setSimulationEnabled(bool enabled) +{ + if (!mCharacter || !mWorld) + return; + + mIsEnabled = enabled; +} + +void JoltPlayer::init(const char* type, const Point3F& size, F32 runSurfaceCos, F32 stepHeight, SceneObject* obj, PhysicsWorld* world) +{ + if (!obj || !world) + return; + + mObject = obj; + + mWorld = dynamic_cast(world); + if (!mWorld) + return; + + mStepHeight = stepHeight; + mMaxSlopeCos = runSurfaceCos; + + F32 radius = mMax(size.x, size.y) * 0.5f; + F32 height = size.z;// +0.5f; + height = mMax(0.01f, height); + + mOriginOffset = (0.5f * height);// +radius; + JPH::Quat rotFix = JPH::Quat::sRotation(JPH::Vec3::sAxisX(), JPH::DegreesToRadians(90.0f)); // zup + JPH::Ref shape = JPH::RotatedTranslatedShapeSettings(JPH::Vec3(0, 0, mOriginOffset), rotFix, new JPH::CapsuleShape(0.5f * height, radius)).Create().Get(); + JPH::Ref inner_shape = JPH::RotatedTranslatedShapeSettings(JPH::Vec3(0, 0, mOriginOffset), rotFix, new JPH::CapsuleShape(0.5f * 0.9f * height, 0.9f * radius)).Create().Get(); + + + JPH::Ref settings = new JPH::CharacterVirtualSettings(); + Con::printf("JoltPlayer::init - maxSlopeAngle: %f degrees", mRadToDeg(mAcos(runSurfaceCos))); + settings->mMaxSlopeAngle = mAcos(runSurfaceCos); + settings->mMaxStrength = 100.0f; + settings->mShape = shape; + settings->mMass = obj->getMass(); + settings->mBackFaceMode = JPH::EBackFaceMode::IgnoreBackFaces; + settings->mCharacterPadding = 0.05f; + settings->mPenetrationRecoverySpeed = 1.0f; + settings->mMaxNumHits = 64; + settings->mPredictiveContactDistance = 0.05f; + settings->mSupportingVolume = JPH::Plane(JPH::Vec3::sAxisZ(), -(radius)); + settings->mEnhancedInternalEdgeRemoval = true; + settings->mInnerBodyShape = inner_shape; + settings->mInnerBodyLayer = Layers::CHARACTER; + + MatrixF objXfm = obj->getTransform(); + QuatF angPos(objXfm); + Point3F pos; + objXfm.getColumn(3, &pos); + + mCharacter = new JPH::CharacterVirtual( + settings, + joltCast(pos), + joltCast(angPos), + 0, + mWorld->getPhysicsSystem() + ); + + getUserData().setObject(obj); + //getUserData().setBody(this); + + mCharacter->SetUserData(reinterpret_cast(&mUserData)); + mCharacter->SetUp(JPH::Vec3::sAxisZ()); + mCharacter->SetListener(this); + + setSimulationEnabled(true); +} + +class CharacterBodyFilter : public JPH::BodyFilter +{ +public: + JPH::BodyID mCharacterBodyID; + + CharacterBodyFilter(JPH::BodyID id) + : mCharacterBodyID(id) { + } + + virtual bool ShouldCollide(const JPH::BodyID& inBodyID) const override + { + return inBodyID != mCharacterBodyID; + } +}; + +Point3F JoltPlayer::move(const VectorF& displacement, CollisionList& outCol) +{ + if (!mCharacter || !mWorld) + return Point3F::Zero; + + if (!mIsEnabled) + return joltCast(mCharacter->GetPosition()); + + const F32 dt = TickSec; + JPH::Vec3 velocity = joltCast(displacement) / dt; + + const auto& system = mWorld->getPhysicsSystem(); + const JPH::Vec3 up = mCharacter->GetUp(); + + mCharacter->UpdateGroundVelocity(); + + const JPH::CharacterVirtual::EGroundState groundState = mCharacter->GetGroundState(); + const bool isOnGround = groundState == JPH::CharacterVirtual::EGroundState::OnGround; + + // Temporary - log what displacement.z actually is each frame + /*Con::printf("groundState=%d disp.z=%.4f vel.z=%.4f", + (int)groundState, + displacement.z, + displacement.z / TickSec);*/ + + if (isOnGround && !mCharacter->IsSlopeTooSteep(mCharacter->GetGroundNormal())) + { + JPH::Vec3 horizontalVel(velocity.GetX(), velocity.GetY(), 0.0f); + mAllowSliding = horizontalVel.Length() > 0.01f; + + if (velocity.GetZ() < 0.05f) + velocity.SetZ(0.00f); + + JPH::Vec3 groundVel = mCharacter->GetGroundVelocity(); + if (groundVel.LengthSq() > 0.001f) + velocity += groundVel; + } + else + { + mAllowSliding = true; + } + + mCharacter->SetLinearVelocity(velocity); + + //---------------------------------------- + // Substep (bullet was doing a max of 10 iterations) + //---------------------------------------- + const U32 iterations = 3; + const F32 subDt = dt / iterations; + + JPH::CharacterVirtual::ExtendedUpdateSettings settings; + + for (int i = 0; i < iterations; ++i) + { + bool isLast = (i == iterations - 1); + + settings.mWalkStairsStepUp = JPH::Vec3(0, 0, mStepHeight); + settings.mStickToFloorStepDown = JPH::Vec3(0, 0, -mStepHeight); + settings.mWalkStairsCosAngleForwardContact = mAcos(mMaxSlopeCos); + + + mCharacter->ExtendedUpdate( + subDt, + system->GetGravity(), + settings, + system->GetDefaultBroadPhaseLayerFilter(Layers::MOVING), + system->GetDefaultLayerFilter(Layers::MOVING), + {}, + {}, + *mWorld->getTempAllocator() + ); + } + + //---------------------------------------- + // Fill collision list from active contacts + //---------------------------------------- + const JPH::CharacterVirtual::ContactList& contacts = mCharacter->GetActiveContacts(); + + for (const JPH::CharacterVirtual::Contact& c : contacts) + { + if (!c.mHadCollision) + continue; + + if (outCol.getCount() >= CollisionList::MaxCollisions) + break; + + JPH::BodyLockRead lock(system->GetBodyLockInterface(), c.mBodyB); + if (!lock.Succeeded()) + continue; + + const JPH::Body& body = lock.GetBody(); + PhysicsUserData* ud = reinterpret_cast(body.GetUserData()); + + Collision& col = outCol.increment(); + col.point = joltCast(c.mPosition); + col.normal = joltCast(c.mContactNormal); + col.object = ud ? ud->getObject() : nullptr; + col.material = nullptr; // material lookup not available yet + col.distance = c.mDistance; + col.face = 0; + col.faceDot = mDot(col.normal, VectorF(0.f, 0.f, 1.f)); + } + + // If on ground, ensure the ground body is represented even if it + // wasn't caught in the contact list (CharacterVirtual separates these) + if (mCharacter->GetGroundState() == JPH::CharacterVirtual::EGroundState::OnGround + && outCol.getCount() < CollisionList::MaxCollisions) + { + JPH::BodyLockRead lock(system->GetBodyLockInterface(), mCharacter->GetGroundBodyID()); + if (lock.Succeeded()) + { + const JPH::Body& body = lock.GetBody(); + PhysicsUserData* ud = reinterpret_cast(body.GetUserData()); + + // Avoid duplicating if already added via active contacts + SceneObject* groundObj = ud ? ud->getObject() : nullptr; + bool alreadyPresent = false; + for (S32 i = 0; i < outCol.getCount(); ++i) + { + if (outCol[i].object == groundObj) + { + alreadyPresent = true; + break; + } + } + + if (!alreadyPresent) + { + Collision& col = outCol.increment(); + col.point = joltCast(mCharacter->GetGroundPosition()); + col.normal = joltCast(mCharacter->GetGroundNormal()); + col.object = groundObj; + col.material = nullptr; + col.distance = 0.f; + col.faceDot = mDot(col.normal, VectorF(0.f, 0.f, 1.f)); + } + } + } + + //---------------------------------------- + // Final position + //---------------------------------------- + Point3F finalPos = joltCast(mCharacter->GetPosition()); + return finalPos; +} + +void JoltPlayer::findContact(SceneObject** contactObject, VectorF* contactNormal, Vector* outOverlapObjects) const +{ + if (!mCharacter || !mIsEnabled) + return; + + const bool isOnGround = mCharacter->GetGroundState() == JPH::CharacterVirtual::EGroundState::OnGround; + F32 bestDot = -1.0f; + const JPH::Vec3 up = mCharacter->GetUp(); + + for (const JPH::CharacterVirtual::Contact& c : mCharacter->GetActiveContacts()) + { + if (c.mHadCollision) + { + JPH::BodyLockRead lock(mWorld->getPhysicsSystem()->GetBodyLockInterface(), c.mBodyB); + if (!lock.Succeeded()) + continue; + + const JPH::Body& body = lock.GetBody(); + PhysicsUserData* ud = (PhysicsUserData*)body.GetUserData(); + if (!ud) + continue; + + VectorF normal = joltCast(c.mContactNormal); + F32 dot = mDot(normal, VectorF(0, 0, 1)); + if (dot > bestDot) + { + bestDot = dot; + *contactObject = ud->getObject(); + *contactNormal = normal; + } + SceneObject* obj = ud->getObject(); + if (obj && !outOverlapObjects->contains(obj)) + outOverlapObjects->push_back(obj); + } + } + + if (isOnGround) + { + JPH::BodyLockRead lock(mWorld->getPhysicsSystem()->GetBodyLockInterface(), mCharacter->GetGroundBodyID()); + if (!lock.Succeeded()) + return; + + const JPH::Body& body = lock.GetBody(); + PhysicsUserData* ud = (PhysicsUserData*)body.GetUserData(); + if (!ud) + return; + + *contactObject = ud->getObject(); + *contactNormal = joltCast(mCharacter->GetGroundNormal()); + } +} + + +bool JoltPlayer::testSpacials(const Point3F& nPos, const Point3F& nSize) const +{ + return true; + + //if (!mCharacter || !mWorld) + // return false; + + //const auto& system = mWorld->getPhysicsSystem(); + + //JPH::BodyFilter body_filter; + //JPH::ShapeFilter shape_filter; + + //F32 radius = getMax(nSize.x, nSize.y) * 0.5f; + //F32 height = nSize.z - (radius * 2.0f); + + //F32 halfHeight = 0.5f * height; + //JPH::Vec3 shapeTranslation(0, 0, halfHeight + radius); + + //JPH::Quat rotFix = JPH::Quat::sRotation(JPH::Vec3::sAxisX(), JPH::DegreesToRadians(90.0f)); // zup + //JPH::Ref shape = JPH::RotatedTranslatedShapeSettings(shapeTranslation, rotFix, new JPH::CapsuleShape(halfHeight, radius)).Create().Get(); + + //const float maxPenetration = 0.05f; // tweak tolerance + + //return mCharacter->SetShape( + // shape, + // maxPenetration, + // system->GetDefaultBroadPhaseLayerFilter(Layers::MOVING), + // system->GetDefaultLayerFilter(Layers::MOVING), + // body_filter, + // shape_filter, + // *mWorld->getTempAllocator()); + +} + +void JoltPlayer::setSpacials(const Point3F& nPos, const Point3F& nSize) +{ + if (!mCharacter || !mWorld) + return; + + const auto& system = mWorld->getPhysicsSystem(); + + JPH::BodyFilter body_filter; + JPH::ShapeFilter shape_filter; + + F32 radius = mMax(nSize.x, nSize.y) * 0.5f; + F32 height = nSize.z;// +0.5f; + height = mMax(0.01f, height); + + mOriginOffset = (0.5f * height);// +radius; + JPH::Quat rotFix = JPH::Quat::sRotation(JPH::Vec3::sAxisX(), JPH::DegreesToRadians(90.0f)); // zup + JPH::Ref shape = JPH::RotatedTranslatedShapeSettings(JPH::Vec3(0, 0, mOriginOffset), rotFix, new JPH::CapsuleShape(0.5f * height, radius)).Create().Get(); + JPH::Ref inner_shape = JPH::RotatedTranslatedShapeSettings(JPH::Vec3(0, 0, mOriginOffset), rotFix, new JPH::CapsuleShape(0.5f * 0.9f * height, 0.9f * radius)).Create().Get(); + + const float maxPenetration = 0.05f; // tweak tolerance + + if (mCharacter->SetShape( + shape, + maxPenetration, + system->GetDefaultBroadPhaseLayerFilter(Layers::MOVING), + system->GetDefaultLayerFilter(Layers::MOVING), + body_filter, + shape_filter, + *mWorld->getTempAllocator()) + ) + { + mCharacter->SetInnerBodyShape(inner_shape); + } + + //mCharacter->SetPosition(joltCast(nPos)); +} + +void JoltPlayer::enableCollision() +{ + if (!mCharacter || !mWorld || mCollisionEnabled) + return; + + mCollisionEnabled = true; + + JPH::BodyID innerBodyID = mCharacter->GetInnerBodyID(); + if (innerBodyID.IsInvalid()) + return; + + mWorld->getPhysicsSystem()->GetBodyInterface().AddBody( + innerBodyID, + JPH::EActivation::Activate + ); +} + +void JoltPlayer::disableCollision() +{ + if (!mCharacter || !mWorld || !mCollisionEnabled) + return; + + mCollisionEnabled = false; + + JPH::BodyID innerBodyID = mCharacter->GetInnerBodyID(); + if (innerBodyID.IsInvalid()) + return; + + mWorld->getPhysicsSystem()->GetBodyInterface().RemoveBody(innerBodyID); +} + +void JoltPlayer::OnContactCommon(const JPH::CharacterVirtual* inCharacter, const JPH::BodyID& inBodyID2, const JPH::SubShapeID& inSubShapeID2, JPH::RVec3Arg inContactPosition, JPH::Vec3Arg inContactNormal, JPH::CharacterContactSettings& ioSettings) +{ + if (inCharacter == mCharacter) + { + if (ioSettings.mCanPushCharacter && mWorld->getPhysicsSystem()->GetBodyInterface().GetMotionType(inBodyID2) != JPH::EMotionType::Static) + { + mAllowSliding = true; + } + } +} + +void JoltPlayer::OnCharacterContactCommon(const JPH::CharacterVirtual* inCharacter, const JPH::CharacterVirtual* inOtherCharacter, const JPH::SubShapeID& inSubShapeID2, JPH::RVec3Arg inContactPosition, JPH::Vec3Arg inContactNormal, JPH::CharacterContactSettings& ioSettings) +{ + if (inCharacter == mCharacter && ioSettings.mCanPushCharacter) + mAllowSliding = true; +} + +//------------------------------------------------------------------ +// Character Contact Listener Callbacks +//------------------------------------------------------------------ + +void JoltPlayer::OnAdjustBodyVelocity(const JPH::CharacterVirtual* inCharacter, const JPH::Body& inBody2, JPH::Vec3& ioLinearVelocity, JPH::Vec3& ioAngularVelocity) +{ +} + +void JoltPlayer::OnContactAdded(const JPH::CharacterVirtual* inCharacter, const JPH::BodyID& inBodyID2, const JPH::SubShapeID& inSubShapeID2, JPH::RVec3Arg inContactPosition, JPH::Vec3Arg inContactNormal, JPH::CharacterContactSettings& ioSettings) +{ + if (inCharacter == mCharacter) + { + JPH::CharacterVirtual::ContactKey c(inBodyID2, inSubShapeID2); + if (std::find(mActiveContacts.begin(), mActiveContacts.end(), c) != mActiveContacts.end()) + AssertFatal(false, "JoltPlayer::OnContactAdded - Received a contact that should have been persisted"); + + mActiveContacts.push_back(c); + } +} + +void JoltPlayer::OnContactPersisted(const JPH::CharacterVirtual* inCharacter, const JPH::BodyID& inBodyID2, const JPH::SubShapeID& inSubShapeID2, JPH::RVec3Arg inContactPosition, JPH::Vec3Arg inContactNormal, JPH::CharacterContactSettings& ioSettings) +{ + if (inCharacter == mCharacter) + { + JPH::CharacterVirtual::ContactKey c(inBodyID2, inSubShapeID2); + if (std::find(mActiveContacts.begin(), mActiveContacts.end(), c) == mActiveContacts.end()) + AssertFatal(false, "JoltPlayer::OnContactPersisted - Received a contact that should have been added instead"); + } +} + +void JoltPlayer::OnContactRemoved(const JPH::CharacterVirtual* inCharacter, const JPH::BodyID& inBodyID2, const JPH::SubShapeID& inSubShapeID2) +{ + if (inCharacter == mCharacter) + { + JPH::CharacterVirtual::ContactKey c(inBodyID2, inSubShapeID2); + ContactSet::iterator it = std::find(mActiveContacts.begin(), mActiveContacts.end(), c); + if (it == mActiveContacts.end()) + AssertFatal(false, "JoltPlayer::OnContactRemoved - Attempted to remove a contact that was not added"); + + mActiveContacts.erase(it); + } +} + +void JoltPlayer::OnCharacterContactAdded(const JPH::CharacterVirtual* inCharacter, const JPH::CharacterVirtual* inOtherCharacter, const JPH::SubShapeID& inSubShapeID2, JPH::RVec3Arg inContactPosition, JPH::Vec3Arg inContactNormal, JPH::CharacterContactSettings& ioSettings) +{ +} + +void JoltPlayer::OnCharacterContactPersisted(const JPH::CharacterVirtual* inCharacter, const JPH::CharacterVirtual* inOtherCharacter, const JPH::SubShapeID& inSubShapeID2, JPH::RVec3Arg inContactPosition, JPH::Vec3Arg inContactNormal, JPH::CharacterContactSettings& ioSettings) +{ +} + +void JoltPlayer::OnCharacterContactRemoved(const JPH::CharacterVirtual* inCharacter, const JPH::CharacterID& inOtherCharacterID, const JPH::SubShapeID& inSubShapeID2) +{ +} + +void JoltPlayer::OnContactSolve(const JPH::CharacterVirtual* inCharacter, const JPH::BodyID& inBodyID2, const JPH::SubShapeID& inSubShapeID2, JPH::RVec3Arg inContactPosition, JPH::Vec3Arg inContactNormal, JPH::Vec3Arg inContactVelocity, const JPH::PhysicsMaterial* inContactMaterial, JPH::Vec3Arg inCharacterVelocity, JPH::Vec3& ioNewCharacterVelocity) +{ + if (inCharacter != mCharacter) + return; + + // When not sliding and contact surface isn't moving and slope is walkable, + // zero out velocity to prevent micro-jitter from solver residuals + if (!mAllowSliding && inContactVelocity.IsNearZero() && !inCharacter->IsSlopeTooSteep(inContactNormal)) + ioNewCharacterVelocity = JPH::Vec3::sZero(); +} + diff --git a/Engine/source/T3D/physics/jolt/joltPlayer.h b/Engine/source/T3D/physics/jolt/joltPlayer.h new file mode 100644 index 000000000..b95450ea8 --- /dev/null +++ b/Engine/source/T3D/physics/jolt/joltPlayer.h @@ -0,0 +1,116 @@ +#ifndef _JOLTPLAYER_H_ +#define _JOLTPLAYER_H_ + +#ifndef _T3D_PHYSICS_PHYSICSPLAYER_H_ +#include "T3D/physics/physicsPlayer.h" +#endif +#ifndef _T3D_JOLT_PLUGIN_H_ +#include "T3D/physics/jolt/joltPlugin.h" +#endif +#ifndef _JOLTWORLD_H_ +#include "T3D/physics/jolt/joltWorld.h" +#endif + + + +#ifdef Offset +#pragma push_macro("Offset") +#undef Offset +#endif + +#include + +#ifdef Offset +// Restore the original macro after includes +#pragma pop_macro("Offset") +#endif + +class JoltPlayer : public PhysicsPlayer, public JPH::CharacterContactListener +{ +protected: + JoltWorld* mWorld; + SceneObject* mObject; + JPH::Ref mCharacter; + JPH::Vec3 mDesiredVelocity; + F32 mMaxSlopeCos; + F32 mStepHeight; + F32 mOriginOffset; + /// Is the body participating in the physics simulation. + bool mIsEnabled; + bool mAllowSliding; + bool mCollisionEnabled; + + using ContactSet = JPH::Array; + ContactSet mActiveContacts; +public: + JoltPlayer(); + virtual ~JoltPlayer(); + + // PhysicsObject + PhysicsWorld* getWorld() override { return mWorld; } + void setTransform(const MatrixF& xfm) override; + MatrixF& getTransform(MatrixF* outMatrix) override; + Box3F getWorldBounds() override; + void setSimulationEnabled(bool enabled) override; + bool isSimulationEnabled() override { return mIsEnabled; } + + // Physics Player. + void init(const char* type, + const Point3F& size, + F32 runSurfaceCos, + F32 stepHeight, + SceneObject* obj, + PhysicsWorld* world) override; + + void preUpdate(F32 dt); + + Point3F move(const VectorF& displacement, CollisionList& outCol) override; + + bool _recoverFromPenetration(JPH::RVec3& inOutPos); + bool _sweep(JPH::RVec3& inOutPos, const JPH::Vec3& disp, CollisionList* outCol); + void _stepForward(JPH::RVec3& inOutPos, const JPH::Vec3& displacement, CollisionList* outCol); + + void findContact(SceneObject** contactObject, VectorF* contactNormal, Vector* outOverlapObjects) const override; + bool testSpacials(const Point3F& nPos, const Point3F& nSize) const override; + void setSpacials(const Point3F& nPos, const Point3F& nSize) override; + void enableCollision() override; + void disableCollision() override; + + JPH::Ref getCharacter() { return mCharacter; } + +protected: + // Common function to be called when contacts are added/persisted + void OnContactCommon(const JPH::CharacterVirtual* inCharacter, const JPH::BodyID& inBodyID2, const JPH::SubShapeID& inSubShapeID2, JPH::RVec3Arg inContactPosition, JPH::Vec3Arg inContactNormal, JPH::CharacterContactSettings& ioSettings); + void OnCharacterContactCommon(const JPH::CharacterVirtual* inCharacter, const JPH::CharacterVirtual* inOtherCharacter, const JPH::SubShapeID& inSubShapeID2, JPH::RVec3Arg inContactPosition, JPH::Vec3Arg inContactNormal, JPH::CharacterContactSettings& ioSettings); + + +public: // contact listener + /// Callback to adjust the velocity of a body as seen by the character. Can be adjusted to e.g. implement a conveyor belt or an inertial dampener system of a sci-fi space ship. + virtual void OnAdjustBodyVelocity(const JPH::CharacterVirtual* inCharacter, const JPH::Body& inBody2, JPH::Vec3& ioLinearVelocity, JPH::Vec3& ioAngularVelocity) override; + + // Called whenever the character collides with a body. + virtual void OnContactAdded(const JPH::CharacterVirtual* inCharacter, const JPH::BodyID& inBodyID2, const JPH::SubShapeID& inSubShapeID2, JPH::RVec3Arg inContactPosition, JPH::Vec3Arg inContactNormal, JPH::CharacterContactSettings& ioSettings) override; + + // Called whenever the character persists colliding with a body. + virtual void OnContactPersisted(const JPH::CharacterVirtual* inCharacter, const JPH::BodyID& inBodyID2, const JPH::SubShapeID& inSubShapeID2, JPH::RVec3Arg inContactPosition, JPH::Vec3Arg inContactNormal, JPH::CharacterContactSettings& ioSettings) override; + + // Called whenever the character loses contact with a body. + virtual void OnContactRemoved(const JPH::CharacterVirtual* inCharacter, const JPH::BodyID& inBodyID2, const JPH::SubShapeID& inSubShapeID2) override; + + // Called whenever the character collides with a virtual character. + virtual void OnCharacterContactAdded(const JPH::CharacterVirtual* inCharacter, const JPH::CharacterVirtual* inOtherCharacter, const JPH::SubShapeID& inSubShapeID2, JPH::RVec3Arg inContactPosition, JPH::Vec3Arg inContactNormal, JPH::CharacterContactSettings& ioSettings) override; + + // Called whenever the character persists colliding with a virtual character. + virtual void OnCharacterContactPersisted(const JPH::CharacterVirtual* inCharacter, const JPH::CharacterVirtual* inOtherCharacter, const JPH::SubShapeID& inSubShapeID2, JPH::RVec3Arg inContactPosition, JPH::Vec3Arg inContactNormal, JPH::CharacterContactSettings& ioSettings) override; + + // Called whenever the character loses contact with a virtual character. + virtual void OnCharacterContactRemoved(const JPH::CharacterVirtual* inCharacter, const JPH::CharacterID& inOtherCharacterID, const JPH::SubShapeID& inSubShapeID2) override; + + // Called whenever the character movement is solved and a constraint is hit. Allows the listener to override the resulting character velocity (e.g. by preventing sliding along certain surfaces). + virtual void OnContactSolve(const JPH::CharacterVirtual* inCharacter, const JPH::BodyID& inBodyID2, const JPH::SubShapeID& inSubShapeID2, JPH::RVec3Arg inContactPosition, JPH::Vec3Arg inContactNormal, JPH::Vec3Arg inContactVelocity, const JPH::PhysicsMaterial* inContactMaterial, JPH::Vec3Arg inCharacterVelocity, JPH::Vec3& ioNewCharacterVelocity) override; + + +}; + + +#endif diff --git a/Engine/source/T3D/physics/jolt/joltPlugin.cpp b/Engine/source/T3D/physics/jolt/joltPlugin.cpp new file mode 100644 index 000000000..9398594c6 --- /dev/null +++ b/Engine/source/T3D/physics/jolt/joltPlugin.cpp @@ -0,0 +1,303 @@ + +#include "platform/platform.h" +#include "T3D/physics/jolt/joltPlugin.h" + +#include "T3D/physics/jolt/joltWorld.h" +#include "T3D/physics/jolt/joltCollision.h" +#include "T3D/physics/jolt/joltBody.h" +#include "T3D/physics/jolt/joltPlayer.h" +#include "T3D/gameBase/gameProcess.h" +#include "core/util/tNamedFactory.h" +#include "gfx/sim/debugDraw.h" + +AFTER_MODULE_INIT(Sim) +{ + NamedFactory::add("JoltPhysics", &JoltPlugin::create); +} + +JoltPlugin::JoltPlugin() +{ + JPH::RegisterDefaultAllocator(); + + JPH::Factory::sInstance = new JPH::Factory(); + JPH::RegisterTypes(); + +#ifdef JPH_DEBUG_RENDERER + JPH::DebugRenderer::sInstance = new JoltDebugRenderer(); +#endif // TORQUE_TOOLS +} + +JoltPlugin::~JoltPlugin() +{ + delete JPH::Factory::sInstance; +#ifdef JPH_DEBUG_RENDERER + delete JPH::DebugRenderer::sInstance; + JPH::DebugRenderer::sInstance = nullptr; +#endif +} + +void JoltPlugin::reset() +{ + // First delete all the cleanup objects. + if (getPhysicsCleanup()) + getPhysicsCleanup()->deleteAllObjects(); + + getPhysicsResetSignal().trigger(PhysicsResetEvent_Restore); + + // Now let each world reset itself. + Map::Iterator iter = mPhysicsWorldLookup.begin(); + for (; iter != mPhysicsWorldLookup.end(); iter++) + iter->value->reset(); +} + +PhysicsPlugin* JoltPlugin::create() +{ + return new JoltPlugin(); +} + + +void JoltPlugin::destroyPlugin() +{ + // Cleanup any worlds that are still kicking. + Map::Iterator iter = mPhysicsWorldLookup.begin(); + for (; iter != mPhysicsWorldLookup.end(); iter++) + { + iter->value->destroyWorld(); + delete iter->value; + } + mPhysicsWorldLookup.clear(); + + delete this; +} + +PhysicsCollision* JoltPlugin::createCollision() +{ + return new JoltCollision(); +} + +PhysicsBody* JoltPlugin::createBody() +{ + return new JoltBody(); +} + +PhysicsPlayer* JoltPlugin::createPlayer() +{ + return new JoltPlayer(); +} + +bool JoltPlugin::isSimulationEnabled() const +{ + bool ret = false; + JoltWorld* world = static_cast(getWorld(smClientWorldName)); + if (world) + { + ret = world->getEnabled(); + return ret; + } + + world = static_cast(getWorld(smServerWorldName)); + if (world) + { + ret = world->getEnabled(); + return ret; + } + + return ret; +} + +void JoltPlugin::enableSimulation(const String& worldName, bool enable) +{ + JoltWorld* world = static_cast(getWorld(worldName)); + if (world) + world->setEnabled(enable); +} + + +void JoltPlugin::setTimeScale(const F32 timeScale) +{ + // Grab both the client and + // server worlds and set their time + // scales to the passed value. + JoltWorld* world = static_cast(getWorld(smClientWorldName)); + if (world) + world->setEditorTimeScale(timeScale); + + world = static_cast(getWorld(smServerWorldName)); + if (world) + world->setEditorTimeScale(timeScale); +} + + +const F32 JoltPlugin::getTimeScale() const +{ + // Grab both the client and + // server worlds and call + // setEnabled( true ) on them. + JoltWorld* world = static_cast(getWorld(smClientWorldName)); + if (!world) + { + world = static_cast(getWorld(smServerWorldName)); + if (!world) + return 0.0f; + } + + return world->getEditorTimeScale(); +} + +bool JoltPlugin::createWorld(const String& worldName) +{ + Map::Iterator iter = mPhysicsWorldLookup.find(worldName); + PhysicsWorld* world = NULL; + + iter != mPhysicsWorldLookup.end() ? world = (*iter).value : world = NULL; + + if (world) + { + Con::errorf("BtPlugin::createWorld - %s world already exists!", worldName.c_str()); + return false; + } + + world = new JoltWorld(); + + if (worldName.equal(smClientWorldName, String::NoCase)) + world->initWorld(false, ClientProcessList::get()); + else + world->initWorld(true, ServerProcessList::get()); + + mPhysicsWorldLookup.insert(worldName, world); + + return world != NULL; +} + +void JoltPlugin::destroyWorld(const String& worldName) +{ + Map::Iterator iter = mPhysicsWorldLookup.find(worldName); + if (iter == mPhysicsWorldLookup.end()) + return; + + PhysicsWorld* world = (*iter).value; + world->destroyWorld(); + + + mPhysicsWorldLookup.erase(iter); +} + +PhysicsWorld* JoltPlugin::getWorld(const String& worldName) const +{ + if (mPhysicsWorldLookup.isEmpty()) + return NULL; + + Map::ConstIterator iter = mPhysicsWorldLookup.find(worldName); + + return iter != mPhysicsWorldLookup.end() ? (*iter).value : NULL; +} + +PhysicsWorld* JoltPlugin::getWorld() const +{ + if (mPhysicsWorldLookup.size() == 0) + return NULL; + + Map::ConstIterator iter = mPhysicsWorldLookup.begin(); + return iter->value; +} + +U32 JoltPlugin::getWorldCount() const +{ + return mPhysicsWorldLookup.size(); +} + +#ifdef JPH_DEBUG_RENDERER + +JoltDebugRenderer::JoltDebugRenderer() +{ + JPH::DebugRenderer::Initialize(); +} + +void JoltDebugRenderer::DrawLine(JPH::RVec3Arg inFrom, JPH::RVec3Arg inTo, JPH::ColorArg inColor) +{ + DebugDrawer::get()->drawLine(joltCast(inFrom), joltCast(inTo), fromJolt(inColor)); +} + +void JoltDebugRenderer::DrawTriangle(JPH::RVec3Arg inV1, JPH::RVec3Arg inV2, JPH::RVec3Arg inV3, JPH::ColorArg inColor, ECastShadow inCastShadow) +{ + DebugDrawer::get()->drawTri(joltCast(inV1), joltCast(inV2), joltCast(inV3), fromJolt(inColor)); +} + +JPH::DebugRenderer::Batch JoltDebugRenderer::CreateTriangleBatch(const Triangle* inTriangles, int inTriangleCount) +{ + BatchImpl* batch = new BatchImpl; + + if (inTriangles && inTriangleCount) + batch->mTriangles.assign(inTriangles, inTriangles + inTriangleCount); + + return batch; +} + +JPH::DebugRenderer::Batch JoltDebugRenderer::CreateTriangleBatch(const Vertex* inVertices, int inVertexCount, const JPH::uint32* inIndices, int inIndexCount) +{ + BatchImpl* batch = new BatchImpl; + if (inVertices == nullptr || inVertexCount == 0 || inIndices == nullptr || inIndexCount == 0) + return batch; + + // Convert indexed triangle list to triangle list + batch->mTriangles.resize(inIndexCount / 3); + for (size_t t = 0; t < batch->mTriangles.size(); ++t) + { + JPH::DebugRenderer::Triangle& triangle = batch->mTriangles[t]; + triangle.mV[0] = inVertices[inIndices[t * 3 + 0]]; + triangle.mV[1] = inVertices[inIndices[t * 3 + 1]]; + triangle.mV[2] = inVertices[inIndices[t * 3 + 2]]; + } + + return batch; +} + +void JoltDebugRenderer::DrawGeometry(JPH::RMat44Arg inModelMatrix, const JPH::AABox& inWorldSpaceBounds, float inLODScaleSq, JPH::ColorArg inModelColor, const GeometryRef& inGeometry, ECullMode inCullMode, ECastShadow inCastShadow, EDrawMode inDrawMode) +{ + const auto& lod = inGeometry->GetLOD( + mFrameState.cameraPos, + inWorldSpaceBounds, + inLODScaleSq + ); + + const BatchImpl* batch = static_cast(lod.mTriangleBatch.GetPtr()); + + if (!batch) + return; + + const U32 triCount = batch->mTriangles.size(); + if (triCount == 0) + return; + + if (!mFrameState.frustum.getBounds().isOverlapped(fromJolt(inWorldSpaceBounds))) + return; + + // Start one batched primitive build + GFX->setupGenericShaders(); + GFXStateBlockDesc desc; + desc.setZReadWrite(false); + GFX->setStateBlockByDesc(desc); + PrimBuild::begin(GFXLineList, triCount * 6); + for (const auto& tri : batch->mTriangles) + { + JPH::Vec3 v0 = inModelMatrix * JPH::Vec3(tri.mV[0].mPosition); + JPH::Vec3 v1 = inModelMatrix * JPH::Vec3(tri.mV[1].mPosition); + JPH::Vec3 v2 = inModelMatrix * JPH::Vec3(tri.mV[2].mPosition); + JPH::Color color = inModelColor * tri.mV[0].mColor; + PrimBuild::color(fromJolt(color)); + // Edge 1 + PrimBuild::vertex3f(v0.GetX(), v0.GetY(), v0.GetZ()); + PrimBuild::vertex3f(v1.GetX(), v1.GetY(), v1.GetZ()); + + // Edge 2 + PrimBuild::vertex3f(v1.GetX(), v1.GetY(), v1.GetZ()); + PrimBuild::vertex3f(v2.GetX(), v2.GetY(), v2.GetZ()); + + // Edge 3 + PrimBuild::vertex3f(v2.GetX(), v2.GetY(), v2.GetZ()); + PrimBuild::vertex3f(v0.GetX(), v0.GetY(), v0.GetZ()); + } + + PrimBuild::end(true); +} +#endif diff --git a/Engine/source/T3D/physics/jolt/joltPlugin.h b/Engine/source/T3D/physics/jolt/joltPlugin.h new file mode 100644 index 000000000..01006c81f --- /dev/null +++ b/Engine/source/T3D/physics/jolt/joltPlugin.h @@ -0,0 +1,211 @@ +#ifndef _T3D_JOLT_PLUGIN_H_ +#define _T3D_JOLT_PLUGIN_H_ + +#ifndef _T3D_PHYSICS_PHYSICSPLUGIN_H_ +#include "T3D/physics/physicsPlugin.h" +#endif + +#ifndef _SCENERENDERSTATE_H_ +#include "scene/sceneRenderState.h" +#endif + +#if defined(TORQUE_TOOLS) && defined(TORQUE_OS_WIN) +#ifndef JPH_DEBUG_RENDERER +#define JPH_DEBUG_RENDERER +#endif +#endif + +// Save and undefine the macro if it exists +#ifdef Offset + #pragma push_macro("Offset") + #undef Offset +#endif + +#include +#include +#include +#include +#include +#include +#include +#ifdef JPH_DEBUG_RENDERER +#include +#endif +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#ifdef Offset + // Restore the original macro after includes + #pragma pop_macro("Offset") +#endif + +inline Point3F joltCast(const JPH::Vec3& vec) +{ + return Point3F(vec.GetX(), vec.GetY(), vec.GetZ()); +} + +inline JPH::Vec3 joltCast(const Point3F& point) +{ + return JPH::Vec3(point.x, point.y, point.z); +} + +inline QuatF joltCast(const JPH::Quat& quat) +{ + /// The Torque quat has the opposite winding order. + return QuatF(-quat.GetX(), -quat.GetY(), -quat.GetZ(), quat.GetW()); +} + +inline JPH::Quat joltCast(const QuatF& quat) +{ + /// The Torque quat has the opposite winding order. + return JPH::Quat(-quat.x, -quat.y, -quat.z, quat.w); +} + +inline JPH::RMat44 joltCast(const MatrixF& m) +{ + QuatF ang(m); + return JPH::Mat44::sRotationTranslation(joltCast(ang), joltCast(m.getPosition())); +} + +inline MatrixF joltCast(const JPH::Mat44& xfm) +{ + MatrixF out; + + QuatF ang = joltCast(xfm.GetQuaternion()); + ang.setMatrix(&out); + out.setPosition(joltCast(xfm.GetTranslation())); + + return out; +} + +inline void toJolt(const MatrixF& mat, JPH::RVec3& pos, JPH::Quat& rot) +{ + Point3F p; + p = mat.getPosition(); + + pos = joltCast(p); + + QuatF q(mat); + + rot = joltCast(q); +} + +inline JPH::AABox toJolt(const Box3F& box) +{ + return JPH::AABox( + joltCast(box.minExtents), + joltCast(box.maxExtents) + ); +} + +inline Box3F fromJolt(const JPH::AABox& box) +{ + return Box3F( + joltCast(box.mMin), + joltCast(box.mMax) + ); +} + +#ifndef _COLOR_H_ +#include "core/color.h" +#endif + +inline JPH::Color toJolt(const ColorI& c) +{ + return JPH::Color(c.red, c.green, c.blue, c.alpha); +} + + + +inline ColorI fromJolt(const JPH::Color& c) +{ + return ColorI(c.r, c.g, c.b, c.a); +} + +#ifdef JPH_DEBUG_RENDERER +struct DebugRenderFrameState +{ + Frustum frustum; + JPH::Vec3 cameraPos; +}; + +class JoltDebugRenderer final : public JPH::DebugRenderer +{ +public: + JPH_OVERRIDE_NEW_DELETE + explicit JoltDebugRenderer(); + virtual void DrawLine(JPH::RVec3Arg inFrom, JPH::RVec3Arg inTo, JPH::ColorArg inColor) override; + virtual void DrawTriangle(JPH::RVec3Arg inV1, JPH::RVec3Arg inV2, JPH::RVec3Arg inV3, JPH::ColorArg inColor, ECastShadow inCastShadow) override; + virtual JPH::DebugRenderer::Batch CreateTriangleBatch(const Triangle* inTriangles, int inTriangleCount) override; + virtual JPH::DebugRenderer::Batch CreateTriangleBatch(const Vertex* inVertices, int inVertexCount, const JPH::uint32* inIndices, int inIndexCount) override; + virtual void DrawGeometry(JPH::RMat44Arg inModelMatrix, const JPH::AABox& inWorldSpaceBounds, float inLODScaleSq, JPH::ColorArg inModelColor, const GeometryRef& inGeometry, ECullMode inCullMode, ECastShadow inCastShadow, EDrawMode inDrawMode) override; + virtual void DrawText3D(JPH::RVec3Arg inPosition, const JPH::string_view& inString, JPH::ColorArg inColor, float inHeight) override {}; + + + void SetFrameState(const DebugRenderFrameState& state) + { + mFrameState = state; + } + + /// Implementation specific batch object + class BatchImpl : public JPH::RefTargetVirtual + { + public: + JPH_OVERRIDE_NEW_DELETE + + virtual void AddRef() override + { + ++mRefCount; + } + virtual void Release() override + { + if (--mRefCount == 0) + delete this; + } + + JPH::Array mTriangles; + + private: + JPH::atomic mRefCount = 0; + }; + +private: + DebugRenderFrameState mFrameState; +}; +#endif + +class JoltPlugin : public PhysicsPlugin +{ +public: + JoltPlugin(); + ~JoltPlugin(); + + /// Create function for factory. + static PhysicsPlugin* create(); + + // PhysicsPlugin + virtual void destroyPlugin(); + virtual void reset(); + virtual PhysicsCollision* createCollision(); + virtual PhysicsBody* createBody(); + virtual PhysicsPlayer* createPlayer(); + virtual bool isSimulationEnabled() const; + virtual void enableSimulation(const String& worldName, bool enable); + virtual void setTimeScale(const F32 timeScale); + virtual const F32 getTimeScale() const; + virtual bool createWorld(const String& worldName); + virtual void destroyWorld(const String& worldName); + virtual PhysicsWorld* getWorld(const String& worldName) const; + virtual PhysicsWorld* getWorld() const; + virtual U32 getWorldCount() const; +}; + +#endif // !_T3D_JOLT_PLUGIN_H_ diff --git a/Engine/source/T3D/physics/jolt/joltWorld.cpp b/Engine/source/T3D/physics/jolt/joltWorld.cpp new file mode 100644 index 000000000..e9adb1335 --- /dev/null +++ b/Engine/source/T3D/physics/jolt/joltWorld.cpp @@ -0,0 +1,377 @@ + +#include "platform/platform.h" +#include "T3D/physics/jolt/joltWorld.h" +#include "T3D/physics/jolt/joltBody.h" +#include "T3D/physics/jolt/joltPlayer.h" + +#include "platform/profiler.h" +#include "sim/netConnection.h" +#include "console/console.h" +#include "console/consoleTypes.h" +#include "scene/sceneRenderState.h" +#include "T3D/gameBase/gameProcess.h" + +#include "T3D/physics/physicsUserData.h" + +// Save and undefine the macro if it exists +#ifdef Offset + #pragma push_macro("Offset") + #undef Offset +#endif + +#include +#include +#include +#include +#include +#include + +#ifdef Offset + // Restore the original macro after includes + #pragma pop_macro("Offset") +#endif + +JoltWorld::JoltWorld() + :mIsEnabled(false), + mIsSimulating(false), + mTickCount(0), + mProcessList(NULL), + mEditorTimeScale(1.0f) +{ + mTempAllocator = nullptr; + mJobSystem = nullptr; +} + + +JoltWorld::~JoltWorld() +{ +} + +bool JoltWorld::initWorld(bool isServer, ProcessList* processList) +{ + const U32 cMaxBodies = 65536; + const U32 cNumBodyMutexes = 1024; + const U32 cMaxBodyPairs = 65536; + const U32 cMaxContactConstraints = 10240; + + mTempAllocator = new JPH::TempAllocatorImpl(10 * 1024 * 1024); + + // thread pool of 2 threads? + mJobSystem = new JPH::JobSystemThreadPool( + JPH::cMaxPhysicsJobs, + JPH::cMaxPhysicsBarriers, + -1 + ); + + mPhysicsSystem.Init( + cMaxBodies, + cNumBodyMutexes, + cMaxBodyPairs, + cMaxContactConstraints, + mBroadPhaseLayerInterface, + mObjectVsBroadPhaseLayerFilter, + mObjectLayerPairFilter + ); + + mIsEnabled = true; + + mPhysicsSystem.SetGravity(joltCast(mGravity)); + + mProcessList = processList; + mProcessList->preTickSignal().notify(this, &JoltWorld::getPhysicsResults); + mProcessList->postTickSignal().notify(this, &JoltWorld::tickPhysics, 1000.0f); + + return true; +} + +void JoltWorld::destroyWorldInternal() +{ + // Stop ticking + mIsEnabled = false; + + // Remove all bodies + JPH::BodyInterface& bi = mPhysicsSystem.GetBodyInterface(); + JPH::BodyIDVector bodies; + mPhysicsSystem.GetBodies(bodies); + + for (JPH::BodyID id : bodies) + { + if (bi.IsAdded(id)) + bi.RemoveBody(id); + } + + // Remove tick listeners + if (mProcessList) + { + mProcessList->preTickSignal().remove(this, &JoltWorld::getPhysicsResults); + mProcessList->postTickSignal().remove(this, &JoltWorld::tickPhysics); + mProcessList = nullptr; + } + + // Delete job system and temp allocator + delete mJobSystem; + mJobSystem = nullptr; + + delete mTempAllocator; + mTempAllocator = nullptr; + + delete this; +} + +void JoltWorld::destroyWorld() +{ + mDestroyPending.store(true, std::memory_order_release); +} + +bool JoltWorld::castRay(const Point3F& startPnt, const Point3F& endPnt, RayInfo* ri, const Point3F& impulse) +{ + JPH::Vec3 startV(startPnt.x, startPnt.y, startPnt.z); + JPH::Vec3 dir = JPH::Vec3(endPnt.x - startPnt.x, endPnt.y - startPnt.y, endPnt.z - startPnt.z); + + F32 rayLength = dir.Length(); + + if (rayLength <= 0.0001f) + return false; + + JPH::RRayCast ray(startV, dir); + + JPH::RayCastResult result; + + if (!mPhysicsSystem.GetNarrowPhaseQuery().CastRay(ray, result)) + return false; + + JPH::BodyLockRead lock( + mPhysicsSystem.GetBodyLockInterface(), + result.mBodyID + ); + + if (!lock.Succeeded()) + return false; + + dir /= rayLength; + + const JPH::Body& body = lock.GetBody(); + + JPH::RVec3 hitPos = ray.GetPointOnRay(result.mFraction); + + JPH::Vec3 normal = body.GetWorldSpaceSurfaceNormal( result.mSubShapeID2, hitPos ); + + if (!body.GetUserData()) + return false; + + if (ri) + { + ri->point.set(joltCast(hitPos)); + ri->normal.set(joltCast(normal)); + + ri->distance = result.mFraction * rayLength; + // SceneObject mapping + PhysicsUserData* userData = PhysicsUserData::cast((void*)body.GetUserData()); + ri->object = userData->getObject(); + ri->material = NULL; + ri->face = 0; + ri->faceDot = 0; + } + + return true; +} + +PhysicsBody* JoltWorld::castRay(const Point3F& start, const Point3F& end, U32 bodyTypes) +{ + return nullptr; +} + +void JoltWorld::explosion(const Point3F& pos, F32 radius, F32 forceMagnitude) +{ + JPH::Vec3 center(pos.x, pos.y, pos.z); + float radiusSq = radius * radius; + + // Broadphase query: get all bodies in sphere + JPH::SphereShape sphere(radius); + + JPH::CollideShapeSettings settings; + settings.mMaxSeparationDistance = 0.0f; + + JPH::RMat44 transform = JPH::RMat44::sTranslation(center); + + struct ExplosionCollector : public JPH::CollideShapeCollector + { + std::vector bodies; + + void AddHit(const JPH::CollideShapeResult& result) override + { + bodies.push_back(result.mBodyID2); + } + }; + + ExplosionCollector collector; + + mPhysicsSystem.GetNarrowPhaseQuery().CollideShape( + &sphere, + JPH::Vec3::sOne(), + transform, + settings, + JPH::Vec3::sZero(), + collector + ); + + auto& bodyInterface = mPhysicsSystem.GetBodyInterface(); + + for (const JPH::BodyID& bodyID : collector.bodies) + { + if (!bodyInterface.IsAdded(bodyID)) + continue; + + // Wake the body FIRST + bodyInterface.ActivateBody(bodyID); + + // Lock body safely + JPH::BodyLockWrite lock( + mPhysicsSystem.GetBodyLockInterface(), + bodyID + ); + + if (!lock.Succeeded()) + continue; + + JPH::Body& body = lock.GetBody(); + + // Ignore static/kinematic bodies + if (!body.IsDynamic()) + continue; + + JPH::Vec3 bodyPos = body.GetCenterOfMassPosition(); + JPH::Vec3 offset = bodyPos - center; + + float distSq = offset.LengthSq(); + if (distSq > radiusSq || distSq <= 0.0001f) + continue; + + float dist = sqrtf(distSq); + + // Normalize direction + JPH::Vec3 dir = offset / dist; + + // Falloff (linear) + float falloff = 1.0f - (dist / radius); + + // Final impulse + JPH::Vec3 impulse = dir * (forceMagnitude * falloff); + + // Apply impulse at center of mass + body.AddImpulse(impulse); + } +} + +void JoltWorld::onDebugDraw(const SceneRenderState* state) +{ +#ifdef JPH_DEBUG_RENDERER + if (!state->isDiffusePass()) return; + + if (!mIsEnabled) + return; + + auto* renderer = static_cast(JPH::DebugRenderer::sInstance); + if (!renderer) + return; + + DebugRenderFrameState frame; + frame.frustum = state->getCullingFrustum(); + frame.cameraPos = joltCast(state->getCameraPosition()); + + renderer->SetFrameState(frame); + + JPH::BodyManager::DrawSettings settings; + + settings.mDrawShape = true; + settings.mDrawWorldTransform = false; + settings.mDrawBoundingBox = true; + settings.mDrawVelocity = false; + settings.mDrawCenterOfMassTransform = true; + settings.mDrawShapeColor = JPH::BodyManager::EShapeColor::SleepColor; + + mPhysicsSystem.DrawBodies(settings, renderer); +#endif +} + +void JoltWorld::tickPhysics(U32 elapsedMs) +{ + // Perform deferred destruction + if (mDestroyPending.exchange(false)) + { + destroyWorldInternal(); + return; + } + + if (!mIsEnabled) + return; + + if (mResetPending.exchange(false)) + performReset(); + + const F32 dt = (F32)elapsedMs * 0.001f; + + PROFILE_SCOPE(JoltWorld_TickPhysics); + + mPhysicsSystem.Update( + dt * mEditorTimeScale, + smPhysicsMaxSubSteps, // collision steps + mTempAllocator, + mJobSystem + ); + + mIsSimulating = true; +} + +void JoltWorld::getPhysicsResults() +{ + if (!mIsSimulating) + return; + + PROFILE_SCOPE(JoltWorld_getPhysicsResults); + + mIsSimulating = false; + mTickCount++; +} + +void JoltWorld::performReset() +{ + JPH::BodyInterface& bodyInterface = mPhysicsSystem.GetBodyInterface(); + + JPH::BodyIDVector bodies; + mPhysicsSystem.GetBodies(bodies); + + for (JPH::BodyID id : bodies) + { + if (!bodyInterface.IsAdded(id)) + continue; + + bodyInterface.ActivateBody(id); + + bodyInterface.SetLinearAndAngularVelocity( + id, + JPH::Vec3::sZero(), + JPH::Vec3::sZero() + ); + } + + mPhysicsSystem.OptimizeBroadPhase(); +} + +void JoltWorld::reset() +{ + if (!mIsEnabled) + return; + + // we operate with a threading environment o not reset right away. + mResetPending.store(true, std::memory_order_release); + +} + +void JoltWorld::setEnabled(bool enabled) +{ + mIsEnabled = enabled; + + if (!mIsEnabled) + getPhysicsResults(); +} diff --git a/Engine/source/T3D/physics/jolt/joltWorld.h b/Engine/source/T3D/physics/jolt/joltWorld.h new file mode 100644 index 000000000..28d5718f6 --- /dev/null +++ b/Engine/source/T3D/physics/jolt/joltWorld.h @@ -0,0 +1,190 @@ +#pragma once + +#ifndef _JOLTWORLD_H_ +#define _JOLTWORLD_H_ + +#ifndef _T3D_PHYSICS_PHYSICSWORLD_H_ +#include "T3D/physics/physicsWorld.h" +#endif + +#ifndef _T3D_JOLT_PLUGIN_H_ +#include "T3D/physics/jolt/joltPlugin.h" +#endif + + +class ProcessList; +class PhysicsBody; +class JoltPlayer; + +namespace Layers +{ + static constexpr JPH::ObjectLayer NON_MOVING = 0; + static constexpr JPH::ObjectLayer MOVING = 1; + static constexpr JPH::ObjectLayer CHARACTER = 2; + static constexpr JPH::ObjectLayer DEBRIS = 3; + static constexpr JPH::ObjectLayer NUM_LAYERS = 4; +} + +namespace BroadPhaseLayers +{ + static constexpr JPH::BroadPhaseLayer NON_MOVING(0); + static constexpr JPH::BroadPhaseLayer MOVING(1); + + static constexpr U32 NUM_LAYERS = 2; +} + +class BPLayerInterfaceImpl final : public JPH::BroadPhaseLayerInterface +{ +public: + + BPLayerInterfaceImpl() + { + mObjectToBroadPhase[Layers::NON_MOVING] = BroadPhaseLayers::NON_MOVING; + mObjectToBroadPhase[Layers::MOVING] = BroadPhaseLayers::MOVING; + mObjectToBroadPhase[Layers::CHARACTER] = BroadPhaseLayers::MOVING; + mObjectToBroadPhase[Layers::DEBRIS] = BroadPhaseLayers::MOVING; + } + + virtual U32 GetNumBroadPhaseLayers() const override + { + return BroadPhaseLayers::NUM_LAYERS; + } + + virtual JPH::BroadPhaseLayer GetBroadPhaseLayer(JPH::ObjectLayer layer) const override + { + return mObjectToBroadPhase[layer]; + } + +#if defined(JPH_EXTERNAL_PROFILE) || defined(JPH_PROFILE_ENABLED) + + virtual const char* GetBroadPhaseLayerName(JPH::BroadPhaseLayer layer) const override + { + switch ((JPH::BroadPhaseLayer::Type)layer) + { + case 0: return "NON_MOVING"; + case 1: return "MOVING"; + default: return "UNKNOWN"; + } + } + +#endif + +private: + + JPH::BroadPhaseLayer mObjectToBroadPhase[Layers::NUM_LAYERS]; +}; + +class ObjectLayerPairFilterImpl : public JPH::ObjectLayerPairFilter +{ +public: + + virtual bool ShouldCollide( + JPH::ObjectLayer layer1, + JPH::ObjectLayer layer2) const override + { + switch (layer1) + { + case Layers::NON_MOVING: + return layer2 == Layers::MOVING + || layer2 == Layers::CHARACTER + || layer2 == Layers::DEBRIS; + + case Layers::MOVING: + return true; + + case Layers::CHARACTER: + return layer2 == Layers::NON_MOVING + || layer2 == Layers::MOVING + || layer2 == Layers::CHARACTER; + + case Layers::DEBRIS: + return layer2 == Layers::NON_MOVING; // Only Statics + + default: + return false; + } + } +}; + +class ObjectVsBroadPhaseLayerFilterImpl : public JPH::ObjectVsBroadPhaseLayerFilter +{ +public: + + virtual bool ShouldCollide( + JPH::ObjectLayer layer1, + JPH::BroadPhaseLayer layer2) const override + { + switch (layer1) + { + case Layers::NON_MOVING: + return layer2 == BroadPhaseLayers::MOVING; + + case Layers::MOVING: + case Layers::CHARACTER: + return true; + + case Layers::DEBRIS: + return layer2 == BroadPhaseLayers::NON_MOVING; + + default: + return false; + } + } +}; + +class JoltWorld : public PhysicsWorld +{ +protected: + bool mIsEnabled; + + U32 mTickCount; + bool mIsSimulating; + + std::atomic mResetPending{ false }; + std::atomic mDestroyPending{ false }; + + F32 mEditorTimeScale; + JPH::PhysicsSystem mPhysicsSystem; + JPH::TempAllocatorImpl* mTempAllocator; + JPH::JobSystemThreadPool* mJobSystem; + BPLayerInterfaceImpl mBroadPhaseLayerInterface; + ObjectVsBroadPhaseLayerFilterImpl mObjectVsBroadPhaseLayerFilter; + ObjectLayerPairFilterImpl mObjectLayerPairFilter; + ProcessList* mProcessList; +public: + JoltWorld(); + virtual ~JoltWorld(); + + // PhysicWorld + virtual bool initWorld(bool isServer, ProcessList* processList); + void destroyWorldInternal(); + virtual void destroyWorld(); + virtual bool castRay(const Point3F& startPnt, const Point3F& endPnt, RayInfo* ri, const Point3F& impulse); + virtual PhysicsBody* castRay(const Point3F& start, const Point3F& end, U32 bodyTypes); + virtual void explosion(const Point3F& pos, F32 radius, F32 forceMagnitude); + virtual void onDebugDraw(const SceneRenderState* state); + virtual void reset(); + virtual bool isEnabled() const { return mIsEnabled; } + + void tickPhysics(U32 elapsedMs); + void getPhysicsResults(); + bool isWritable() const { return !mIsSimulating; } + + void performReset(); + + void setEnabled(bool enabled); + bool getEnabled() const { return mIsEnabled; } + + void setEditorTimeScale(F32 timeScale) { mEditorTimeScale = timeScale; } + const F32 getEditorTimeScale() const { return mEditorTimeScale; } + JPH::PhysicsSystem* getPhysicsSystem() { return &mPhysicsSystem; } + + const JPH::BroadPhaseLayerInterface& getBroadPhaseLayerInterface() const { return mBroadPhaseLayerInterface; }; + const JPH::ObjectLayerPairFilter& getObjectLayerPairFilter() const { return mObjectLayerPairFilter; } + const JPH::ObjectVsBroadPhaseLayerFilter& getObjectVsBroadPhaseLayerFilter() const { return mObjectVsBroadPhaseLayerFilter; } + JPH::TempAllocatorImpl* getTempAllocator() { return mTempAllocator; } + ProcessList* getProcessList() { return mProcessList; } + +}; + +#endif // !_JOLTWORLD_H_ diff --git a/Engine/source/environment/meshRoad.cpp b/Engine/source/environment/meshRoad.cpp index ea09c4352..624ca9398 100644 --- a/Engine/source/environment/meshRoad.cpp +++ b/Engine/source/environment/meshRoad.cpp @@ -2587,11 +2587,15 @@ void MeshRoad::_generateSegments() polylist.triangulate(); PhysicsCollision *colShape = PHYSICSMGR->createCollision(); - colShape->addTriangleMesh( polylist.mVertexList.address(), + if (!colShape->addTriangleMesh(polylist.mVertexList.address(), polylist.mVertexList.size(), polylist.mIndexList.address(), polylist.mIndexList.size() / 3, - MatrixF::Identity ); + getWorldTransform())) + { + // failed to create mesh, get out GET OUT NOWWWW + return; + } PhysicsWorld *world = PHYSICSMGR->getWorld( isServerObject() ? "server" : "client" ); mPhysicsRep = PHYSICSMGR->createBody(); diff --git a/Tools/CMake/vcpkg/ports/joltphysics/portfile.cmake b/Tools/CMake/vcpkg/ports/joltphysics/portfile.cmake new file mode 100644 index 000000000..4a1c0c1d0 --- /dev/null +++ b/Tools/CMake/vcpkg/ports/joltphysics/portfile.cmake @@ -0,0 +1,44 @@ +vcpkg_check_linkage(ONLY_STATIC_LIBRARY) + +set(LOCAL_ARCHIVE "$ENV{VCPKG_LIB_SOURCE_ROOT}/JoltPhysics-5.5.0.zip") + +vcpkg_extract_source_archive( + SOURCE_PATH + ARCHIVE "${LOCAL_ARCHIVE}" +) + +string(COMPARE EQUAL "${VCPKG_CRT_LINKAGE}" "static" USE_STATIC_CRT) + +vcpkg_check_features(OUT_FEATURE_OPTIONS FEATURE_OPTIONS + FEATURES + debugrenderer DEBUG_RENDERER_IN_DEBUG_AND_RELEASE + profiler PROFILER_IN_DEBUG_AND_RELEASE + rtti CPP_RTTI_ENABLED +) + +vcpkg_cmake_configure( + SOURCE_PATH "${SOURCE_PATH}/Build" + OPTIONS + -DTARGET_UNIT_TESTS=OFF + -DTARGET_HELLO_WORLD=OFF + -DTARGET_PERFORMANCE_TEST=OFF + -DTARGET_SAMPLES=OFF + -DTARGET_VIEWER=OFF + -DCROSS_PLATFORM_DETERMINISTIC=OFF + -DINTERPROCEDURAL_OPTIMIZATION=OFF + -DUSE_STATIC_MSVC_RUNTIME_LIBRARY=${USE_STATIC_CRT} + -DENABLE_ALL_WARNINGS=OFF + -DOVERRIDE_CXX_FLAGS=OFF + ${FEATURE_OPTIONS} + OPTIONS_RELEASE + -DGENERATE_DEBUG_SYMBOLS=OFF +) + +vcpkg_cmake_install() +vcpkg_copy_pdbs() +vcpkg_fixup_pkgconfig() + +file(REMOVE_RECURSE "${CURRENT_PACKAGES_DIR}/debug/include") +vcpkg_cmake_config_fixup(PACKAGE_NAME Jolt CONFIG_PATH "lib/cmake/Jolt") + +vcpkg_install_copyright(FILE_LIST "${SOURCE_PATH}/LICENSE") diff --git a/Tools/CMake/vcpkg/ports/joltphysics/vcpkg.json b/Tools/CMake/vcpkg/ports/joltphysics/vcpkg.json new file mode 100644 index 000000000..ad8c01f2f --- /dev/null +++ b/Tools/CMake/vcpkg/ports/joltphysics/vcpkg.json @@ -0,0 +1,28 @@ +{ + "name": "joltphysics", + "version": "5.5.0", + "description": "A multi core friendly rigid body physics and collision detection library suitable for games and VR applications", + "homepage": "https://github.com/jrouwe/JoltPhysics", + "license": "MIT", + "dependencies": [ + { + "name": "vcpkg-cmake", + "host": true + }, + { + "name": "vcpkg-cmake-config", + "host": true + } + ], + "features": { + "debugrenderer": { + "description": "Enable debug renderer in Debug and Release builds" + }, + "profiler": { + "description": "Enable the profiler in Debug and Release builds" + }, + "rtti": { + "description": "Enable C++ RTTI" + } + } +} diff --git a/vcpkg.json b/vcpkg.json index 3bc3bc20c..2d314b1e1 100644 --- a/vcpkg.json +++ b/vcpkg.json @@ -14,6 +14,16 @@ "assimp", "sdl2", "openal-soft", + { + "name": "joltphysics", + "default-features": false + }, + { + "name": "joltphysics", + "default-features": false, + "features": ["debugrenderer"], + "platform": "windows" + }, { "name": "libsndfile", "default-features": false,