JOLT Physics update

adds jolt physics to torque3d

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

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<JoltWorld*>(world);
+   if (!mWorld)
+      return false;
+
+   mColShape = dynamic_cast<JoltCollision*>(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<U64>(&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<SceneObject*>* outOverlapObjects) const
+{
+   if (contactObject)
+      *contactObject = nullptr;
+
+   if (contactNormal)
+      *contactNormal = VectorF::Zero;
+}

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<SceneObject*>* outOverlapObjects) const override;
+   void moveKinematicTo(const MatrixF& xfm) override;
+
+   bool isValid() override;
+};
+
+#endif // !_JOLTBODY_H_

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 <Jolt/Physics/Collision/Shape/BoxShape.h>
+#include <Jolt/Physics/Collision/Shape/SphereShape.h>
+#include <Jolt/Physics/Collision/Shape/CapsuleShape.h>
+#include <Jolt/Physics/Collision/Shape/ConvexHullShape.h>
+#include <Jolt/Physics/Collision/Shape/TriangleShape.h>
+#include <Jolt/Physics/Collision/Shape/HeightFieldShape.h>
+#include <Jolt/Physics/Collision/Shape/PlaneShape.h>
+#include <Jolt/Physics/Collision/Shape/CompoundShape.h>
+#include <Jolt/Physics/Collision/Shape/MeshShape.h>
+#include <Jolt/Physics/Collision/Shape/StaticCompoundShape.h>
+#include <Jolt/Physics/Collision/Shape/RotatedTranslatedShape.h>
+
+#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<JPH::Vec3> 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<float> 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;
+   }
+
+}

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<JPH::Shape> shape;
+   JPH::Mat44 localXfm;
+   JPH::Vec3 localPos;
+   JPH::Quat localRot;
+};
+
+class JoltCollision : public PhysicsCollision
+{
+private:
+   JPH::Ref<JPH::Shape> mCompundShape;
+   Vector<ChildShapeEntry> 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<JPH::Shape> getJoltShape() const { return mCompundShape; }
+};
+
+/// A strong pointer to a reference counted JoltCollision.
+typedef StrongRefPtr<JoltCollision> JoltCollisionRef;
+
+
+/// A weak pointer to a reference counted JoltCollision.
+typedef WeakRefPtr<JoltCollision> JoltCollisionWeakRef;
+
+#endif // !_JOLTCOLLISION_H_

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 <Jolt/Physics/Collision/ShapeCast.h>
+#include <Jolt/Physics/Collision/Shape/CapsuleShape.h>
+#include <Jolt/Physics/Collision/Shape/RotatedTranslatedShape.h>
+#include <Jolt/Physics/Collision/CollideShape.h>
+#include <Jolt/Physics/Collision/CollisionCollectorImpl.h>
+
+#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<JoltWorld*>(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<JPH::Shape> shape = JPH::RotatedTranslatedShapeSettings(JPH::Vec3(0, 0, mOriginOffset), rotFix, new JPH::CapsuleShape(0.5f * height, radius)).Create().Get();
+   JPH::Ref<JPH::Shape> 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<JPH::CharacterVirtualSettings> 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<U64>(&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<PhysicsUserData*>(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<PhysicsUserData*>(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<SceneObject*>* 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<JPH::Shape> 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<JPH::Shape> shape = JPH::RotatedTranslatedShapeSettings(JPH::Vec3(0, 0, mOriginOffset), rotFix, new JPH::CapsuleShape(0.5f * height, radius)).Create().Get();
+   JPH::Ref<JPH::Shape> 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();
+}
+

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 <Jolt/Physics/Character/CharacterVirtual.h>
+
+#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<JPH::CharacterVirtual> 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<JPH::CharacterVirtual::ContactKey>;
+   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<SceneObject*>* 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<JPH::CharacterVirtual> 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

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<PhysicsPlugin>::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<StringNoCase, PhysicsWorld*>::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<StringNoCase, PhysicsWorld*>::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<JoltWorld*>(getWorld(smClientWorldName));
+   if (world)
+   {
+      ret = world->getEnabled();
+      return ret;
+   }
+
+   world = static_cast<JoltWorld*>(getWorld(smServerWorldName));
+   if (world)
+   {
+      ret = world->getEnabled();
+      return ret;
+   }
+
+   return ret;
+}
+
+void JoltPlugin::enableSimulation(const String& worldName, bool enable)
+{
+   JoltWorld* world = static_cast<JoltWorld*>(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<JoltWorld*>(getWorld(smClientWorldName));
+   if (world)
+      world->setEditorTimeScale(timeScale);
+
+   world = static_cast<JoltWorld*>(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<JoltWorld*>(getWorld(smClientWorldName));
+   if (!world)
+   {
+      world = static_cast<JoltWorld*>(getWorld(smServerWorldName));
+      if (!world)
+         return 0.0f;
+   }
+
+   return world->getEditorTimeScale();
+}
+
+bool JoltPlugin::createWorld(const String& worldName)
+{
+   Map<StringNoCase, PhysicsWorld*>::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<StringNoCase, PhysicsWorld*>::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<StringNoCase, PhysicsWorld*>::ConstIterator iter = mPhysicsWorldLookup.find(worldName);
+
+   return iter != mPhysicsWorldLookup.end() ? (*iter).value : NULL;
+}
+
+PhysicsWorld* JoltPlugin::getWorld() const
+{
+   if (mPhysicsWorldLookup.size() == 0)
+      return NULL;
+
+   Map<StringNoCase, PhysicsWorld*>::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<const BatchImpl*>(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

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 <Jolt/Jolt.h>
+#include <Jolt/Physics/PhysicsSystem.h>
+#include <Jolt/RegisterTypes.h>
+#include <Jolt/Core/TempAllocator.h>
+#include <Jolt/Core/StreamWrapper.h>
+#include <Jolt/Physics/Collision/RayCast.h>
+#include <Jolt/Physics/Collision/CastResult.h>
+#ifdef JPH_DEBUG_RENDERER
+#include <Jolt/Renderer/DebugRenderer.h>
+#endif
+#include <Jolt/Core/JobSystemThreadPool.h>
+#include <Jolt/Physics/PhysicsSettings.h>
+#include <Jolt/Physics/StateRecorderImpl.h>
+#include <Jolt/Physics/Collision/Shape/BoxShape.h>
+#include <Jolt/Physics/Collision/Shape/CylinderShape.h>
+#include <Jolt/Physics/Collision/Shape/SphereShape.h>
+#include <Jolt/Physics/Collision/Shape/MeshShape.h>
+#include <Jolt/Physics/Body/BodyCreationSettings.h>
+#include <Jolt/Physics/Constraints/SixDOFConstraint.h>
+#include <Jolt/Physics/Body/BodyActivationListener.h>
+
+#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<JPH::DebugRenderer::Triangle> mTriangles;
+
+   private:
+      JPH::atomic<U32> 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_

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 <Jolt/Physics/Collision/RayCast.h>
+#include <Jolt/Physics/Collision/CastResult.h>
+#include <Jolt/Physics/Body/BodyLock.h>
+#include <Jolt/Physics/Body/Body.h>
+#include <Jolt/Physics/Collision/NarrowPhaseQuery.h>
+#include <Jolt/Physics/Collision/CollideShape.h>
+
+#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<JPH::BodyID> 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<JoltDebugRenderer*>(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();
+}

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<bool> mResetPending{ false };
+   std::atomic<bool> 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_

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