SHADE_Engine/src/Physics/Dynamics/SHMotionState.cpp

@@ -109,10 +109,7 @@ namespace SHADE
 
     prevOrientation = orientation;
 
-    SHQuaternion qv{ velocity.x * dt, velocity.y * dt, velocity.z * dt, 0.0f };
+    orientation += orientation * SHQuaternion{ velocity.x, velocity.y, velocity.z, 0.0f } * dt * 0.5f;
-    qv *= orientation;
-
-    orientation += qv * 0.5f;
     orientation = SHQuaternion::Normalise(orientation);
   }
 

SHADE_Engine/src/Physics/Dynamics/SHPhysicsWorld.cpp

@@ -142,7 +142,7 @@ namespace SHADE
     rigidBody.linearVelocity  += (LINEAR_ACCELERATION + GRAVITATIONAL_ACCELERATION) * dt;
 
     // Integrate torque into angular velocity
-    rigidBody.angularVelocity += rigidBody.worldInvInertia * (rigidBody.accumulatedTorque * dt);
+    rigidBody.angularVelocity += rigidBody.worldInvInertia * rigidBody.accumulatedTorque * dt;
 
     // Apply drag (exponentially applied)
     rigidBody.linearVelocity  *= 1.0f / (1.0f + dt * rigidBody.linearDrag);

SHADE_Engine/src/Physics/Dynamics/SHRigidBody.cpp

@@ -637,16 +637,17 @@ namespace SHADE
 
   void SHRigidBody::ComputeWorldData() noexcept
   {
+    const SHMatrix R  = SHMatrix::Rotate(motionState.orientation);
+    const SHMatrix RT = SHMatrix::Transpose(R);
+
+    // Compute world centroid
+    worldCentroid = (R * localCentroid) + motionState.position;
+
     if (bodyType == Type::STATIC)
       return;
 
-    const SHMatrix ROTATION = SHMatrix::Rotate(motionState.orientation);
-
     // Compute world inertia
-    worldInvInertia = SHMatrix::Transpose(ROTATION) * localInvInertia * ROTATION;
+    worldInvInertia = R * (localInvInertia * RT);
-
-    // Compute world centroid
-    worldCentroid = (ROTATION * localCentroid) + motionState.position;
   }
 
   void SHRigidBody::ComputeMassData() noexcept

SHADE_Engine/src/Physics/System/Routines/SHPhysicsPostUpdateRoutine.cpp

@@ -19,7 +19,6 @@
 #include "Scene/SHSceneManager.h"
 #include "Scripting/SHScriptEngine.h"
 
-
 namespace SHADE
 {
   /*-----------------------------------------------------------------------------------*/
@@ -48,50 +47,55 @@ namespace SHADE
 
     // Interpolate transforms for rendering.
     // Only rigid bodies can move due to physics, so we run through the rigid body component dense set.
-    const auto& RIGIDBODY_DENSE = SHComponentManager::GetDense<SHRigidBodyComponent>();
+    if (physicsSystem->worldUpdated)
-    for (auto& rigidBodyComponent : RIGIDBODY_DENSE)
     {
-      const EntityID EID = rigidBodyComponent.GetEID();
+      const auto& RIGIDBODY_DENSE = SHComponentManager::GetDense<SHRigidBodyComponent>();
-
+      for (auto& rigidBodyComponent : RIGIDBODY_DENSE)
-      // Skip missing transforms
-      auto* transformComponent = SHComponentManager::GetComponent_s<SHTransformComponent>(EID);
-      if (!transformComponent)
-        continue;
-
-      // Skip invalid bodies (Should not occur)
-      if (!rigidBodyComponent.rigidBody)
-        continue;
-
-      // Skip inactive bodies
-      const bool IS_ACTIVE = SHSceneManager::CheckNodeAndComponentsActive<SHRigidBodyComponent>(EID);
-      if (!IS_ACTIVE)
-        continue;
-
-      const SHMotionState& MOTION_STATE = rigidBodyComponent.rigidBody->GetMotionState();
-
-      // Skip objects that have not moved
-      if (!MOTION_STATE)
-        continue;
-
-      if (rigidBodyComponent.IsInterpolating())
       {
-        const SHVec3 RENDER_POSITION = MOTION_STATE.InterpolatePositions(FACTOR);
+        const EntityID EID = rigidBodyComponent.GetEID();
-        const SHQuaternion RENDER_ORIENTATION = MOTION_STATE.InterpolateOrientations(FACTOR);
 
-        transformComponent->SetWorldPosition(RENDER_POSITION);
+        // Skip missing transforms
-        transformComponent->SetWorldOrientation(RENDER_ORIENTATION);
+        auto* transformComponent = SHComponentManager::GetComponent_s<SHTransformComponent>(EID);
-      }
+        if (!transformComponent)
-      else
+          continue;
-      {
-        transformComponent->SetWorldPosition(MOTION_STATE.position);
-        transformComponent->SetWorldOrientation(MOTION_STATE.orientation);
-      }
 
-      /*
+        // Skip invalid bodies (Should not occur)
-       * TODO: Test if the scene graph transforms abides by setting world position. Collisions will ignore the scene graph hierarchy.
+        if (!rigidBodyComponent.rigidBody)
-       */
+          continue;
+
+        // Skip inactive bodies
+        const bool IS_ACTIVE = SHSceneManager::CheckNodeAndComponentsActive<SHRigidBodyComponent>(EID);
+        if (!IS_ACTIVE)
+          continue;
+
+        const SHMotionState& MOTION_STATE = rigidBodyComponent.rigidBody->GetMotionState();
+
+        // Skip objects that have not moved
+        if (!MOTION_STATE)
+          continue;
+
+        if (rigidBodyComponent.IsInterpolating())
+        {
+          const SHVec3 RENDER_POSITION = MOTION_STATE.InterpolatePositions(FACTOR);
+          const SHQuaternion RENDER_ORIENTATION = MOTION_STATE.InterpolateOrientations(FACTOR);
+
+          transformComponent->SetWorldPosition(RENDER_POSITION);
+          transformComponent->SetWorldOrientation(RENDER_ORIENTATION);
+        }
+        else
+        {
+          transformComponent->SetWorldPosition(MOTION_STATE.position);
+          transformComponent->SetWorldOrientation(MOTION_STATE.orientation);
+        }
+
+        /*
+         * TODO: Test if the scene graph transforms abides by setting world position. Collisions will ignore the scene graph hierarchy.
+         */
+      }
     }
 
+    
+
     // Collision & Trigger messages
     if (scriptingSystem != nullptr)
       scriptingSystem->ExecuteCollisionFunctions();