SHADE_Engine/src/Editor/EditorWindow/Inspector/SHEditorComponentView.hpp

@@ -286,7 +286,14 @@ namespace SHADE
       if(ImGui::CollapsingHeader("Debug Information", ImGuiTreeNodeFlags_DefaultOpen))//Dynamic or Kinematic only fields
       {
         SHEditorWidgets::DragVec3("Position", { "X", "Y", "Z" }, [component] {return component->GetPosition(); }, [](SHVec3 const& value) {}, false, "Position", 0.1f, "%.3f", 0.0f, 0.0f, ImGuiSliderFlags_ReadOnly);
-        //SHEditorWidgets::DragVec3("Rotation", { "X", "Y", "Z" }, [component] {return component->GetRotation(); }, [](SHVec3 const& value) {}, false, "Rotation", 0.1f, "%.3f", 0.0f, 0.0f, ImGuiSliderFlags_ReadOnly);
+        SHEditorWidgets::DragVec3("Rotation", { "X", "Y", "Z" }, [component]
+        {
+          // Convert it to degrees...
+          auto rot = component->GetRotation();
+          for (size_t i = 0; i < SHVec3::SIZE; ++i)
+            rot[i] = SHMath::RadiansToDegrees(rot[i]);
+          return rot;
+        }, [](SHVec3 const& value) {}, false, "Rotation", 0.1f, "%.3f", 0.0f, 0.0f, ImGuiSliderFlags_ReadOnly);
         if (rbType == SHRigidBodyComponent::Type::DYNAMIC || rbType == SHRigidBodyComponent::Type::KINEMATIC) //Dynamic or Kinematic only fields
         {
           SHEditorWidgets::DragVec3("Velocity", { "X", "Y", "Z" }, [component] {return component->GetLinearVelocity(); }, [](SHVec3 const& value) {}, false, "Linear Velocity", 0.1f, "%.3f", 0.0f, 0.0f, ImGuiSliderFlags_ReadOnly);

SHADE_Engine/src/Graphics/MiddleEnd/Interface/SHDebugDrawSystem.cpp

@@ -203,9 +203,9 @@ namespace SHADE
         drawCube(points, color, pos, size);
     }
 
-    void SHDebugDrawSystem::DrawSphere(const SHVec4& color, const SHVec3& pos, double radius)
+    void SHDebugDrawSystem::DrawSphere(const SHVec4& color, const SHVec3& pos, const SHVec3& rot, double radius)
     {
-        drawSphere(points, color, pos, radius);
+        drawSphere(points, color, pos, rot, radius);
     }
 
     /*---------------------------------------------------------------------------------*/
@@ -238,7 +238,7 @@ namespace SHADE
 
     void SHDebugDrawSystem::DrawPersistentSphere(const SHVec4& color, const SHVec3& pos, double radius)
     {
-        drawSphere(persistentPoints, color, pos, radius);
+        drawSphere(persistentPoints, color, pos, SHVec3::Zero, radius);
     }
 
     void SHDebugDrawSystem::ClearPersistentDraws()
@@ -315,7 +315,7 @@ namespace SHADE
         );
     }
 
-    void SHDebugDrawSystem::drawSphere(std::vector<PointVertex>& storage, const SHVec4& color, const SHVec3& pos, double radius)
+    void SHDebugDrawSystem::drawSphere(std::vector<PointVertex>& storage, const SHVec4& color, const SHVec3& pos, const SHVec3& rot, double radius)
     {
         //if (spherePoints.empty())
         {
@@ -324,7 +324,9 @@ namespace SHADE
             static const SHMeshData SPHERE = SHPrimitiveGenerator::Sphere();
             for (const auto& idx : SPHERE.Indices)
             {
-                spherePoints.emplace_back(SPHERE.VertexPositions[idx] * radius + pos);
+                SHVec3 SCALE { static_cast<float>(radius) };
+                const SHMatrix TRS = SHMatrix::Transform(pos, rot, { static_cast<float>(radius) });
+                spherePoints.emplace_back(SHVec3::Transform(SPHERE.VertexPositions[idx], TRS));
             }
         }
         drawLineSet(storage, color, spherePoints.begin(), spherePoints.end());

SHADE_Engine/src/Graphics/MiddleEnd/Interface/SHDebugDrawSystem.h

@@ -123,8 +123,9 @@ namespace SHADE
     /// </summary>
     /// <param name="color">Colour of the sphere.</param>
     /// <param name="pos">Position where the sphere wil be centered at.</param>
+    /// <param name="rot">Rotation of the sphere. </param>
     /// <param name="size">Size of the rendered sphere.</param>
-    void DrawSphere(const SHVec4& color, const SHVec3& pos, double radius);
+    void DrawSphere(const SHVec4& color, const SHVec3& pos, const SHVec3& rot, double radius);
 
     /*---------------------------------------------------------------------------------*/
     /* Persistent Draw Functions                                                       */
@@ -246,7 +247,7 @@ namespace SHADE
     template<typename IterType>
     void drawPoly(std::vector<PointVertex>& storage, const SHVec4& color, IterType pointListBegin, IterType pointListEnd);
     void drawCube(std::vector<PointVertex>& storage, const SHVec4& color, const SHVec3& pos, const SHVec3& size);
-    void drawSphere(std::vector<PointVertex>& storage, const SHVec4& color, const SHVec3& pos, double radius);
+    void drawSphere(std::vector<PointVertex>& storage, const SHVec4& color, const SHVec3& pos, const SHVec3& rot, double radius);
   };
 }
 

SHADE_Engine/src/Physics/Collision/CollisionShapes/SHCollisionShape.h

@@ -111,6 +111,12 @@ namespace SHADE
 
     void    SetCollisionTag     (SHCollisionTag* newCollisionTag)       noexcept;
 
+    /*---------------------------------------------------------------------------------*/
+    /* Member Functions                                                                */
+    /*---------------------------------------------------------------------------------*/
+
+    [[nodiscard]] virtual SHMatrix GetInertiaTensor (float mass) const noexcept = 0;
+
   protected:
     /*---------------------------------------------------------------------------------*/
     /* Data Members                                                                    */

SHADE_Engine/src/Physics/Collision/CollisionShapes/SHSphereCollisionShape.cpp

@@ -13,6 +13,9 @@
 // Primary Header
 #include "SHSphereCollisionShape.h"
 
+// Project Headers
+#include "Math/SHMatrix.h"
+
 namespace SHADE
 {
   /*-----------------------------------------------------------------------------------*/
@@ -179,5 +182,17 @@ namespace SHADE
     return SHSphere::Raycast(ray);
   }
 
+  SHMatrix SHSphereCollisionShape::GetInertiaTensor(float mass) const noexcept
+  {
+    static constexpr float TWO_OVER_FIVE = 2.0f / 5.0f;
+
+    const float DIAGONAL = TWO_OVER_FIVE * mass * (Radius * Radius);
+
+    SHMatrix result;
+    result.m[0][0] = result.m[1][1] = result.m[2][2] = DIAGONAL;
+    return result;
+  }
+
+
 
 } // namespace SHADE

SHADE_Engine/src/Physics/Collision/CollisionShapes/SHSphereCollisionShape.h

@@ -96,7 +96,7 @@ namespace SHADE
      * @return
      * True if the point is inside the sphere.
     */
-    bool            TestPoint (const SHVec3& point) const noexcept override;
+    [[nodiscard]] bool            TestPoint         (const SHVec3& point) const noexcept override;
 
     /**
      * @brief
@@ -107,9 +107,19 @@ namespace SHADE
      * An object holding the results of the raycast. <br/>
      * See the corresponding header for the contents of the object.
     */
-    SHRaycastResult Raycast   (const SHRay& ray)    const noexcept override;
+    [[nodiscard]] SHRaycastResult Raycast           (const SHRay& ray)    const noexcept override;
 
-    // TODO: Compute Moment of Inertia
+    /**
+     * @brief
+     * Computes the inertia tensor of the sphere.
+     * @param mass
+     * The mass of the sphere.
+     * @return
+     * The inertia tensor of the sphere.
+     */
+    [[nodiscard]] SHMatrix        GetInertiaTensor  (float mass) const noexcept override;
+
+    
 
   private:
     /*---------------------------------------------------------------------------------*/

SHADE_Engine/src/Physics/Dynamics/SHMotionState.cpp

@@ -80,6 +80,15 @@ namespace SHADE
     position      = newPosition;
   }
 
+  void SHMotionState::ForceOrientation(const SHQuaternion& newOrientation) noexcept
+  {
+    hasMoved      = true;
+
+    prevOrientation  = newOrientation;
+    orientation      = newOrientation;
+  }
+
+
   void SHMotionState::IntegratePosition(const SHVec3& velocity, float dt) noexcept
   {
     // Velocities are 0 when objects are static or sleeping. We do not want to integrate them here.
@@ -91,10 +100,32 @@ namespace SHADE
     position += velocity * dt;
   }
 
+  void SHMotionState::IntegrateOrientation(const SHVec3& velocity, float dt) noexcept
+  {
+    // Velocities are 0 when objects are static or sleeping. We do not want to integrate them here.
+    // This call should never reach here.
+
+    hasMoved = true;
+
+    prevOrientation = orientation;
+
+    SHQuaternion qv{ velocity.x * dt, velocity.y * dt, velocity.z * dt, 0.0f };
+    qv *= orientation;
+
+    orientation += qv * 0.5f;
+    orientation = SHQuaternion::Normalise(orientation);
+  }
+
+
   SHVec3 SHMotionState::InterpolatePositions(float factor) const noexcept
   {
     return SHVec3::ClampedLerp(prevPosition, position, factor);
   }
 
+  SHQuaternion SHMotionState::InterpolateOrientations(float factor) const noexcept
+  {
+    return SHQuaternion::ClampedSlerp(prevOrientation, orientation, factor);
+  }
+
 
 } // namespace SHADE

SHADE_Engine/src/Physics/Dynamics/SHMotionState.h

@@ -11,8 +11,10 @@
 #pragma once
 
 // Project Headers
+#include "Math/SHQuaternion.h"
 #include "Math/Vector/SHVec3.h"
 
+
 namespace SHADE
 {
   /*-------------------------------------------------------------------------------------*/
@@ -30,10 +32,13 @@ namespace SHADE
     /* Data Members                                                                      */
     /*-----------------------------------------------------------------------------------*/
 
-    bool    hasMoved;
+    bool          hasMoved;
 
-    SHVec3  position;
+    SHVec3        position;
-    SHVec3  prevPosition;
+    SHVec3        prevPosition;
+
+    SHQuaternion  orientation;
+    SHQuaternion  prevOrientation;
 
     /*-----------------------------------------------------------------------------------*/
     /* Constructors & Destructor                                                         */
@@ -65,17 +70,36 @@ namespace SHADE
      * @param newPosition
      * The new position to set.
     */
-    void ForcePosition          (const SHVec3& newPosition)         noexcept;
+    void        ForcePosition             (const SHVec3& newPosition)           noexcept;
 
     /**
      * @brief
-     * Integrates the positions using velocity with respect to time.
+     * Forcefully sets the orientation. Meant to be used when transform overrides the rigid body
+     * orientations
+     * @param newOrientation 
+     * The new orientation to set.
+    */
+    void        ForceOrientation          (const SHQuaternion& newOrientation)  noexcept;
+
+    /**
+     * @brief
+     * Integrates the positions using linear velocity with respect to time.
      * @param velocity
-     * The velocity to integrate.
+     * The linear velocity to integrate.
      * @param dt
      * The delta time to integrate with respect to.
     */
-    void IntegratePosition      (const SHVec3& velocity, float dt)  noexcept;
+    void        IntegratePosition         (const SHVec3& velocity, float dt)    noexcept;
+
+    /**
+     * @brief
+     * Integrates the orientation using angular velocity with respect to time.
+     * @param velocity
+     * The angular velocity to integrate.
+     * @param dt
+     * The delta time to integrate with respect to.
+    */
+    void        IntegrateOrientation      (const SHVec3& velocity, float dt)    noexcept;
 
     /**
      * @brief
@@ -85,7 +109,17 @@ namespace SHADE
      * @returns
      * The interpolated position meant for rendering.
     */
-    SHVec3 InterpolatePositions (float factor)                const noexcept;
+    SHVec3        InterpolatePositions    (float factor)                  const noexcept;
+
+    /**
+     * @brief
+     * Interpolates the orientation between the previous and the last using a given factor.
+     * @param factor
+     * The factor to interpolate by. Should be between 0 & 1.
+     * @returns
+     * The interpolated orientation meant for rendering.
+    */
+    SHQuaternion  InterpolateOrientations (float factor)                  const noexcept;
 
   };
 

SHADE_Engine/src/Physics/Dynamics/SHPhysicsWorld.cpp

@@ -74,7 +74,10 @@ namespace SHADE
   void SHPhysicsWorld::Step(float dt)
   {
     for (auto* rigidBody : rigidBodies)
+    {
+      rigidBody->ComputeWorldData();
       integrateForces(*rigidBody, dt);
+    }
 
     for (auto* rigidBody : rigidBodies)
       integrateVelocities(*rigidBody, dt);
@@ -93,10 +96,14 @@ namespace SHADE
     const SHVec3 LINEAR_ACCELERATION        = rigidBody.accumulatedForce * rigidBody.invMass;
     const SHVec3 GRAVITATIONAL_ACCELERATION = rigidBody.IsGravityEnabled() ? settings.gravity * rigidBody.gravityScale : SHVec3::Zero;
 
-    rigidBody.linearVelocity += (LINEAR_ACCELERATION + GRAVITATIONAL_ACCELERATION) * dt;
+    rigidBody.linearVelocity  += (LINEAR_ACCELERATION + GRAVITATIONAL_ACCELERATION) * dt;
+
+    // Integrate torque into angular velocity
+    rigidBody.angularVelocity += rigidBody.worldInvInertia * (rigidBody.accumulatedTorque * dt);
 
     // Apply drag (exponentially applied)
-    rigidBody.linearVelocity *= 1.0f / (1.0f + dt * rigidBody.linearDrag);
+    rigidBody.linearVelocity  *= 1.0f / (1.0f + dt * rigidBody.linearDrag);
+    rigidBody.angularVelocity *= 1.0f / (1.0f + dt * rigidBody.angularDrag);
   }
 
   void SHPhysicsWorld::integrateVelocities(SHRigidBody& rigidBody, float dt) const noexcept
@@ -111,7 +118,13 @@ namespace SHADE
       , rigidBody.GetFreezePositionZ() ? 0.0f : rigidBody.linearVelocity.z
       };
 
-      // TODO: Enforce angular constraints
+      // Enforce angular constraints
+      rigidBody.angularVelocity = SHVec3
+      {
+        rigidBody.GetFreezeRotationX() ? 0.0f : rigidBody.angularVelocity.x
+      , rigidBody.GetFreezeRotationY() ? 0.0f : rigidBody.angularVelocity.y
+      , rigidBody.GetFreezeRotationZ() ? 0.0f : rigidBody.angularVelocity.z
+      };
     };
 
     // Always reset movement flag
@@ -129,13 +142,13 @@ namespace SHADE
       ENFORCE_CONSTRAINED_VELOCITIES(rigidBody);
 
       rigidBody.motionState.IntegratePosition(rigidBody.linearVelocity, dt);
-      // TODO: Integrate orientations
+      rigidBody.motionState.IntegrateOrientation(rigidBody.angularVelocity, dt);
 
       // Sync with collider transforms if a collider is present
       if (rigidBody.collider)
       {
         rigidBody.collider->SetPosition(rigidBody.motionState.position);
-        // TODO: Sync orientations
+        rigidBody.collider->SetOrientation(rigidBody.motionState.orientation);
 
         rigidBody.collider->RecomputeShapes();
       }

SHADE_Engine/src/Physics/Dynamics/SHRigidBody.cpp

@@ -27,9 +27,10 @@ namespace SHADE
   : entityID      { eid }
   , collider      { nullptr }
   , bodyType      { type }
+  , gravityScale  { 1.0f }
   , invMass       { type == Type::DYNAMIC ? 1.0f : 0.0f }
   , linearDrag    { 0.01f }
-  , gravityScale  { 1.0f }
+  , angularDrag   { 0.01f }
   , flags         { 0U }
   {
     // Set default flags
@@ -41,14 +42,19 @@ namespace SHADE
   }
 
   SHRigidBody::SHRigidBody(const SHRigidBody& rhs) noexcept
-  : entityID      { rhs.entityID }
+  : entityID        { rhs.entityID }
-  , collider      { nullptr }
+  , collider        { nullptr }
-  , bodyType      { rhs.bodyType }
+  , bodyType        { rhs.bodyType }
-  , invMass       { rhs.invMass }
+  , gravityScale    { rhs.gravityScale }
-  , linearDrag    { rhs.linearDrag }
+  , invMass         { rhs.invMass }
-  , gravityScale  { rhs.gravityScale }
+  , linearDrag      { rhs.linearDrag }
-  , flags         { rhs.flags }
+  , angularDrag     { rhs.angularDrag }
-  , motionState   { rhs.motionState }
+  , localInvInertia { rhs.localInvInertia }
+  , worldInvInertia { rhs.worldInvInertia }
+  , localCentroid   { rhs.localCentroid }
+  , worldCentroid   { rhs.worldCentroid }
+  , flags           { rhs.flags }
+  , motionState     { rhs.motionState }
   {
     // All other properties are defaulted to 0 to not carry over any potential errors / invalid values.
   }
@@ -57,9 +63,14 @@ namespace SHADE
   : entityID      { rhs.entityID }
   , collider      { nullptr }
   , bodyType      { rhs.bodyType }
+  , gravityScale  { rhs.gravityScale }
   , invMass       { rhs.invMass }
   , linearDrag    { rhs.linearDrag }
-  , gravityScale  { rhs.gravityScale }
+  , angularDrag   { rhs.angularDrag }
+  , localInvInertia { rhs.localInvInertia }
+  , worldInvInertia { rhs.worldInvInertia }
+  , localCentroid   { rhs.localCentroid }
+  , worldCentroid   { rhs.worldCentroid }
   , flags         { rhs.flags }
   , motionState   { std::move(rhs.motionState) }
   {
@@ -77,17 +88,24 @@ namespace SHADE
       return *this;
 
     entityID      = rhs.entityID;
+
     // Deep copy the collider
-    *collider     = *rhs.collider;
+    *collider       = *rhs.collider;
-    bodyType      = rhs.bodyType;
+    bodyType        = rhs.bodyType;
-    invMass       = rhs.invMass;
+    gravityScale    = rhs.gravityScale;
-    linearDrag    = rhs.linearDrag;
+    invMass         = rhs.invMass;
-    gravityScale  = rhs.gravityScale;
+    linearDrag      = rhs.linearDrag;
-    flags         = rhs.flags;
+    angularDrag     = rhs.angularDrag;
-    motionState   = rhs.motionState;
+    localInvInertia = rhs.localInvInertia;
+    worldInvInertia = rhs.worldInvInertia;
+    localCentroid   = rhs.localCentroid;
+    worldCentroid   = rhs.worldCentroid;
+    flags           = rhs.flags;
+    motionState     = rhs.motionState;
 
     // All other properties are defaulted to 0 to not carry over any potential errors / invalid values.
     accumulatedForce  = SHVec3::Zero;
+    accumulatedTorque = SHVec3::Zero;
     linearVelocity    = SHVec3::Zero;
     angularVelocity   = SHVec3::Zero;
 
@@ -97,17 +115,24 @@ namespace SHADE
   SHRigidBody& SHRigidBody::operator=(SHRigidBody&& rhs) noexcept
   {
     entityID      = rhs.entityID;
+
     // Deep copy the collider
     *collider     = *rhs.collider;
     bodyType      = rhs.bodyType;
+    gravityScale  = rhs.gravityScale;
     invMass       = rhs.invMass;
     linearDrag    = rhs.linearDrag;
-    gravityScale  = rhs.gravityScale;
+    angularDrag   = rhs.angularDrag;
+    localInvInertia = rhs.localInvInertia;
+    worldInvInertia = rhs.worldInvInertia;
+    localCentroid   = rhs.localCentroid;
+    worldCentroid   = rhs.worldCentroid;
     flags         = rhs.flags;
     motionState   = std::move(rhs.motionState);
 
     // All other properties are defaulted to 0 to not carry over any potential errors / invalid values.
     accumulatedForce  = SHVec3::Zero;
+    accumulatedTorque = SHVec3::Zero;
     linearVelocity    = SHVec3::Zero;
     angularVelocity   = SHVec3::Zero;
 
@@ -123,6 +148,11 @@ namespace SHADE
     return bodyType;
   }
 
+  float SHRigidBody::GetGravityScale() const noexcept
+  {
+    return gravityScale;
+  }
+
   float SHRigidBody::GetMass() const noexcept
   {
     return 1.0f/ invMass;
@@ -133,16 +163,41 @@ namespace SHADE
     return linearDrag;
   }
 
-  float SHRigidBody::GetGravityScale() const noexcept
+  float SHRigidBody::GetAngularDrag() const noexcept
   {
-    return gravityScale;
+    return angularDrag;
   }
 
-  const SHVec3& SHRigidBody::GetAccumulatedForce() const noexcept
+  const SHMatrix& SHRigidBody::GetLocalInvInertia() const noexcept
+  {
+    return localInvInertia;
+  }
+
+  const SHMatrix& SHRigidBody::GetWorldInvInertia() const noexcept
+  {
+    return worldInvInertia;
+  }
+
+  const SHVec3& SHRigidBody::GetLocalCentroid() const noexcept
+  {
+    return localCentroid;
+  }
+
+  const SHVec3& SHRigidBody::GetWorldCentroid() const noexcept
+  {
+    return worldCentroid;
+  }
+
+  const SHVec3& SHRigidBody::GetForce() const noexcept
   {
     return accumulatedForce;
   }
 
+  const SHVec3& SHRigidBody::GetTorque() const noexcept
+  {
+    return accumulatedTorque;
+  }
+
   const SHVec3& SHRigidBody::GetLinearVelocity() const noexcept
   {
     return linearVelocity;
@@ -226,7 +281,6 @@ namespace SHADE
     return motionState;
   }
 
-
   /*-----------------------------------------------------------------------------------*/
   /* Setter Functions Definitions                                                      */
   /*-----------------------------------------------------------------------------------*/
@@ -245,6 +299,11 @@ namespace SHADE
     invMass     = newType == Type::DYNAMIC ? 1.0f : 0.0f;
   }
 
+  void SHRigidBody::SetGravityScale(float newGravityScale) noexcept
+  {
+    gravityScale = newGravityScale;
+  }
+
   void SHRigidBody::SetMass(float newMass) noexcept
   {
     if (bodyType != Type::DYNAMIC)
@@ -261,7 +320,7 @@ namespace SHADE
 
     invMass = 1.0f / newMass;
 
-    // TODO: Recompute inertia tensor
+    ComputeMassData();
   }
 
   void SHRigidBody::SetLinearDrag(float newLinearDrag) noexcept
@@ -281,16 +340,45 @@ namespace SHADE
     linearDrag = newLinearDrag;
   }
 
-  void SHRigidBody::SetGravityScale(float newGravityScale) noexcept
+  void SHRigidBody::SetAngularDrag(float newAngularDrag) noexcept
   {
-    gravityScale = newGravityScale;
+    if (bodyType == Type::STATIC)
+    {
+      SHLOG_WARNING("Cannot set angular drag of a Static Body {}", entityID)
+      return;
+    }
+
+    if (newAngularDrag < 0.0f)
+    {
+      SHLOG_WARNING("Cannot set drag below 0. Object {}'s angular drag will remain unchanged.", entityID)
+      return;
+    }
+
+    angularDrag = newAngularDrag;
   }
 
   void SHRigidBody::SetLinearVelocity(const SHVec3& newLinearVelocity) noexcept
   {
+    if (bodyType == Type::STATIC)
+    {
+      SHLOG_WARNING("Cannot set linear velocity of a Static Body {}", entityID)
+      return;
+    }
+
     linearVelocity = newLinearVelocity;
   }
 
+  void SHRigidBody::SetAngularVelocity(const SHVec3& newAngularVelocity) noexcept
+  {
+    if (bodyType == Type::STATIC)
+    {
+      SHLOG_WARNING("Cannot set angular velocity of a Static Body {}", entityID)
+      return;
+    }
+
+    angularVelocity = newAngularVelocity;
+  }
+
   void SHRigidBody::SetIsActive(bool isActive) noexcept
   {
     static constexpr unsigned int FLAG_POS  = 0;
@@ -304,7 +392,18 @@ namespace SHADE
     static constexpr unsigned int FLAG_POS  = 1;
     static constexpr uint16_t     VALUE     = 1U << FLAG_POS;
 
-    isSleeping ? flags |= VALUE : flags &= ~VALUE;
+    if (isSleeping)
+    {
+      flags |= VALUE;
+
+      ClearForces();
+      linearVelocity  = SHVec3::Zero;
+      angularVelocity = SHVec3::Zero;
+    }
+    else
+    {
+      flags &= ~VALUE;
+    }
   }
 
   void SHRigidBody::SetSleepingEnabled(bool enableSleeping) noexcept
@@ -411,6 +510,8 @@ namespace SHADE
       flags |= VALUE;
       // Reset angular velocity along X-axis
       angularVelocity.x = 0.0f;
+      // Set inertia tensor on the x-axis to 0
+      localInvInertia.m[0][0] = worldInvInertia.m[0][0] = 0.0f;
     }
     else
     {
@@ -428,6 +529,8 @@ namespace SHADE
       flags |= VALUE;
       // Reset angular velocity along Y-axis
       angularVelocity.y = 0.0f;
+      // Set inertia tensor on the y-axis to 0
+      localInvInertia.m[1][1] = worldInvInertia.m[1][1] = 0.0f;
     }
     else
     {
@@ -445,6 +548,8 @@ namespace SHADE
       flags |= VALUE;
       // Reset angular velocity along Z-axis
       angularVelocity.z = 0.0f;
+      // Set inertia tensor on the z-axis to 0
+      localInvInertia.m[2][2] = worldInvInertia.m[2][2] = 0.0f;
     }
     else
     {
@@ -461,21 +566,60 @@ namespace SHADE
     if (bodyType != Type::DYNAMIC)
       return;
 
-    accumulatedForce += force;
+    accumulatedForce  += force;
-    // Compute torque when force is offset
+    accumulatedTorque += SHVec3::Cross(pos, force);
   }
 
   void SHRigidBody::AddImpulse(const SHVec3& impulse, const SHVec3& pos) noexcept
   {
     if (bodyType != Type::DYNAMIC)
-    return;
+      return;
 
-    linearVelocity += impulse * invMass; 
+    linearVelocity  += impulse * invMass;
+    angularVelocity += worldInvInertia * SHVec3::Cross(pos, impulse);
+  }
+
+  void SHRigidBody::AddTorque(const SHVec3& torque) noexcept
+  {
+    if (bodyType != Type::DYNAMIC)
+      return;
+
+    accumulatedTorque += torque;
   }
 
   void SHRigidBody::ClearForces() noexcept
   {
-    accumulatedForce = SHVec3::Zero;
+    accumulatedForce  = SHVec3::Zero;
+    accumulatedTorque = SHVec3::Zero;
+  }
+
+  void SHRigidBody::ComputeWorldData() noexcept
+  {
+    const SHMatrix ROTATION = SHMatrix::Rotate(motionState.orientation);
+
+    // Compute world inertia
+    worldInvInertia = ROTATION * localInvInertia * SHMatrix::Transpose(ROTATION);
+
+    // Compute world centroid
+    worldCentroid = (ROTATION * localCentroid) + motionState.position;
+  }
+
+  void SHRigidBody::ComputeMassData() noexcept
+  {
+    // TODO: Compute total inertia and centroid from composited colliders using the Parallel Axis Theorem.
+    // TODO: If auto mass in enabled, compute total mass based from each collider.
+    // TODO: If auto mass disabled, compute inertia tensor for each shape using the ratio of its mass / total mass by comparing the volume / total volume.
+
+    if (collider && !collider->GetCollisionShapes().empty())
+    {
+      // HACK: For now, take only the first shape as we are testing with non-composited colliders. We are using the center as the centroid.
+      const auto* FIRST_SHAPE = collider->GetCollisionShape(0);
+      localInvInertia = SHMatrix::Inverse(FIRST_SHAPE->GetInertiaTensor(1.0f / invMass));
+    }
+    else
+    {
+      localInvInertia.m[0][0] = localInvInertia.m[1][1] = localInvInertia.m[2][2] = invMass;
+    }
   }
 
 } // namespace SHADE

SHADE_Engine/src/Physics/Dynamics/SHRigidBody.h

@@ -12,6 +12,7 @@
 
 // Project Headers
 #include "ECS_Base/Entity/SHEntity.h"
+#include "Math/SHMatrix.h"
 #include "Math/Vector/SHVec3.h"
 #include "SHMotionState.h"
 
@@ -73,12 +74,20 @@ namespace SHADE
 
     [[nodiscard]] Type                GetType             ()  const noexcept;
 
-    [[nodiscard]] float               GetMass             ()  const noexcept;
-    [[nodiscard]] float               GetLinearDrag       ()  const noexcept;
-
     [[nodiscard]] float               GetGravityScale     ()  const noexcept;
 
-    [[nodiscard]] const SHVec3&       GetAccumulatedForce ()  const noexcept;
+    [[nodiscard]] float               GetMass             ()  const noexcept;
+    [[nodiscard]] float               GetLinearDrag       ()  const noexcept;
+    [[nodiscard]] float               GetAngularDrag      ()  const noexcept;
+
+    [[nodiscard]] const SHMatrix&     GetLocalInvInertia  ()  const noexcept;
+    [[nodiscard]] const SHMatrix&     GetWorldInvInertia  ()  const noexcept;
+
+    [[nodiscard]] const SHVec3&       GetLocalCentroid    ()  const noexcept;
+    [[nodiscard]] const SHVec3&       GetWorldCentroid    ()  const noexcept;
+
+    [[nodiscard]] const SHVec3&       GetForce            ()  const noexcept;
+    [[nodiscard]] const SHVec3&       GetTorque           ()  const noexcept;
     [[nodiscard]] const SHVec3&       GetLinearVelocity   ()  const noexcept;
     [[nodiscard]] const SHVec3&       GetAngularVelocity  ()  const noexcept;
 
@@ -111,6 +120,8 @@ namespace SHADE
     */
     void SetType              (Type newType)                    noexcept;
 
+    void SetGravityScale      (float newGravityScale)           noexcept;
+
     /**
      * @brief
      * Mass is only modifiable for Dynamic bodies.
@@ -127,9 +138,16 @@ namespace SHADE
     */
     void  SetLinearDrag       (float newLinearDrag)             noexcept;
 
+    /**
+     * @brief
+     * Drag is only modifiable for non-Static bodies.
+     * @param newAngularDrag
+     * The new drag to set. Values below 0 will be ignored.
+    */
+    void  SetAngularDrag      (float newAngularDrag)            noexcept;
 
-    void  SetGravityScale     (float newGravityScale)           noexcept;
     void  SetLinearVelocity   (const SHVec3& newLinearVelocity) noexcept;
+    void  SetAngularVelocity  (const SHVec3& newAngularVelocity)noexcept;
 
     // Flags
 
@@ -151,29 +169,55 @@ namespace SHADE
 
     /**
      * @brief
-     * Adds a force to the body with an offset from it's center of mass.
+     * Adds a force to the body with an offset from it's center of mass. <br/>
+     * Non-dynamic bodies will be ignored.
      * @param force
      * The force to add to the body.
      * @param pos
      * The position from the center of mass to offset the force. Defaults to zero.
     */
-    void AddForce     (const SHVec3& force, const SHVec3& pos = SHVec3::Zero)   noexcept;
+    void AddForce         (const SHVec3& force, const SHVec3& pos = SHVec3::Zero) noexcept;
 
     /**
      * @brief
-     * Adds an impulse to the body with an offset from it's center of mass.
+     * Adds an impulse to the body with an offset from it's center of mass. <br/>
+     * Non-dynamic bodies will be ignored.
      * @param impulse
      * The impulse to add to the body.
      * @param pos
      * The position from the center of mass to offset the impulse. Defaults to zero.
     */
-    void AddImpulse   (const SHVec3& impulse, const SHVec3& pos = SHVec3::Zero) noexcept;
+    void AddImpulse       (const SHVec3& impulse, const SHVec3& pos = SHVec3::Zero) noexcept;
+
+    /**
+     * @brief
+     * Adds torque to rotate the body about it's centroid. <br/>
+     * Non-dynamic bodies will be ignored.
+     * @param torque
+     * The torque to add to the body.
+    */
+    void AddTorque        (const SHVec3& torque) noexcept;
 
     /**
      * @brief
      * Removes all the forces from the body.
     */
-    void ClearForces  () noexcept;
+    void ClearForces      () noexcept;
+
+    /**
+     * @brief
+     * Computes the centroid and invInertia in world space.
+    */
+    void ComputeWorldData () noexcept;
+
+    /**
+     * @brief
+     * Computes the centroid and inertia of the object. <br/>
+     * If auto-mass is enabled, computes the mass. <br/>
+     * If auto-mass is disabled, the inertia is computed based on the ratio each shape's volume over the total volume.
+     * 
+    */
+    void ComputeMassData  () noexcept;
 
   private:
     /*-----------------------------------------------------------------------------------*/
@@ -186,12 +230,20 @@ namespace SHADE
 
     Type          bodyType;
 
+    float         gravityScale;
+
     float         invMass;
     float         linearDrag;
+    float         angularDrag;
 
-    float         gravityScale;
+    SHMatrix      localInvInertia;
+    SHMatrix      worldInvInertia;
+
+    SHVec3        localCentroid;
+    SHVec3        worldCentroid;
       
     SHVec3        accumulatedForce;
+    SHVec3        accumulatedTorque;
 
     SHVec3        linearVelocity;
     SHVec3        angularVelocity;

SHADE_Engine/src/Physics/Interface/SHRigidBodyComponent.cpp

@@ -36,18 +36,12 @@ namespace SHADE
 
   bool SHRigidBodyComponent::IsGravityEnabled() const noexcept
   {
-    if (rigidBody)
+    return rigidBody ? rigidBody->IsGravityEnabled() : false;
-      return rigidBody->IsGravityEnabled();
-
-    return false;
   }
 
   bool SHRigidBodyComponent::IsAllowedToSleep() const noexcept
   {
-    if (rigidBody)
+    return rigidBody ? rigidBody->IsSleepingEnabled() : false;
-      return rigidBody->IsSleepingEnabled();
-
-    return false;
   }
 
   bool SHRigidBodyComponent::IsInterpolating() const noexcept
@@ -57,131 +51,93 @@ namespace SHADE
 
   bool SHRigidBodyComponent::IsSleeping() const noexcept
   {
-    if (rigidBody)
+    return rigidBody ? rigidBody->IsSleeping() : false;
-      return rigidBody->IsSleeping();
-
-    return false;
   }
 
   bool SHRigidBodyComponent::GetAutoMass() const noexcept
   {
-    if (rigidBody)
+    return rigidBody ? rigidBody->IsAutoMassEnabled() : false;
-      return rigidBody->IsAutoMassEnabled();
-
-    return false;
   }
 
   bool SHRigidBodyComponent::GetFreezePositionX() const noexcept
   {
-    if (rigidBody)
+    return rigidBody ? rigidBody->GetFreezePositionX() : false;
-      return rigidBody->GetFreezePositionX();
-
-    return false;
   }
 
   bool SHRigidBodyComponent::GetFreezePositionY() const noexcept
   {
-    if (rigidBody)
+    return rigidBody ? rigidBody->GetFreezePositionY() : false;
-      return rigidBody->GetFreezePositionY();
-
-    return false;
   }
 
   bool SHRigidBodyComponent::GetFreezePositionZ() const noexcept
   {
-    if (rigidBody)
+    return rigidBody ? rigidBody->GetFreezePositionZ() : false;
-      return rigidBody->GetFreezePositionZ();
-
-    return false;
   }
 
   bool SHRigidBodyComponent::GetFreezeRotationX() const noexcept
   {
-    if (rigidBody)
+    return rigidBody ? rigidBody->GetFreezeRotationX() : false;
-      return rigidBody->GetFreezeRotationX();
-
-    return false;
   }
 
   bool SHRigidBodyComponent::GetFreezeRotationY() const noexcept
   {
-    if (rigidBody)
+    return rigidBody ? rigidBody->GetFreezeRotationY() : false;
-      return rigidBody->GetFreezeRotationY();
-
-    return false;
   }
 
   bool SHRigidBodyComponent::GetFreezeRotationZ() const noexcept
   {
-    if (rigidBody)
+    return rigidBody ? rigidBody->GetFreezeRotationZ() : false;
-      return rigidBody->GetFreezeRotationZ();
-
-    return false;
   }
 
   float SHRigidBodyComponent::GetGravityScale() const noexcept
   {
-    if (rigidBody)
+    return rigidBody ? rigidBody->GetGravityScale() : 0.0f;
-      return rigidBody->GetGravityScale();
-
-    return 0.0f;
   }
 
   float SHRigidBodyComponent::GetMass() const noexcept
   {
-    if (rigidBody)
+    return rigidBody ? rigidBody->GetMass() : -1.0f;
-      return rigidBody->GetMass();
-
-    return -1.0f;
   }
 
   float SHRigidBodyComponent::GetDrag() const noexcept
   {
-    if (rigidBody)
+    return rigidBody ? rigidBody->GetLinearDrag() : -1.0f;
-      return rigidBody->GetLinearDrag();
-
-    return -1.0f;
   }
 
   float SHRigidBodyComponent::GetAngularDrag() const noexcept
   {
-    return 0.0f;
+    return rigidBody ? rigidBody->GetAngularDrag() : -1.0f;
   }
 
   SHVec3 SHRigidBodyComponent::GetForce() const noexcept
   {
-    if (rigidBody)
+    return rigidBody ? rigidBody->GetForce() : SHVec3::Zero;
-      return rigidBody->GetAccumulatedForce();
-
-    return SHVec3::Zero;
   }
 
   SHVec3 SHRigidBodyComponent::GetTorque() const noexcept
   {
-    return SHVec3::Zero;
+    return rigidBody ? rigidBody->GetTorque() : SHVec3::Zero;
   }
 
   SHVec3 SHRigidBodyComponent::GetLinearVelocity() const noexcept
   {
-    if (rigidBody)
+    return rigidBody ? rigidBody->GetLinearVelocity() : SHVec3::Zero;
-      return rigidBody->GetLinearVelocity();
-
-    return SHVec3::Zero;
   }
 
   SHVec3 SHRigidBodyComponent::GetAngularVelocity() const noexcept
   {
-    return SHVec3::Zero;
+    return rigidBody ? rigidBody->GetAngularVelocity() : SHVec3::Zero;
   }
 
   SHVec3 SHRigidBodyComponent::GetPosition() const noexcept
   {
-    if (rigidBody)
+    return rigidBody ? rigidBody->GetMotionState().position : SHVec3::Zero;
-      return rigidBody->GetMotionState().position;
-
-    return SHVec3::Zero;
   }
 
+  SHVec3 SHRigidBodyComponent::GetRotation() const noexcept
+  {
+    return rigidBody ? rigidBody->GetMotionState().orientation.ToEuler() : SHVec3::Zero;
+  }
 
   /*-----------------------------------------------------------------------------------*/
   /* Setter Functions Definitions                                                      */
@@ -282,7 +238,8 @@ namespace SHADE
 
   void SHRigidBodyComponent::SetAngularDrag(float newAngularDrag) noexcept
   {
-
+    if (rigidBody)
+      rigidBody->SetAngularDrag(newAngularDrag);
   }
 
   void SHRigidBodyComponent::SetLinearVelocity(const SHVec3& newLinearVelocity) noexcept
@@ -293,7 +250,8 @@ namespace SHADE
 
   void SHRigidBodyComponent::SetAngularVelocity(const SHVec3& newAngularVelocity) noexcept
   {
-
+    if (rigidBody)
+      rigidBody->SetAngularVelocity(newAngularVelocity);
   }
 
   /*-----------------------------------------------------------------------------------*/
@@ -308,38 +266,67 @@ namespace SHADE
 
   void SHRigidBodyComponent::AddForceAtLocalPos(const SHVec3& force, const SHVec3& localPos) const noexcept
   {
-    
+    if (rigidBody)
+      rigidBody->AddForce(force, localPos);
   }
 
   void SHRigidBodyComponent::AddForceAtWorldPos(const SHVec3& force, const SHVec3& worldPos) const noexcept
   {
     if (rigidBody)
-      rigidBody->AddForce(force, worldPos);
+    {
+      // Convert world pos into local space of the body
+      const SHVec3 LOCAL_POS = worldPos - rigidBody->GetWorldCentroid();
+      rigidBody->AddForce(force, LOCAL_POS);
+    }
   }
 
   void SHRigidBodyComponent::AddRelativeForce(const SHVec3& relativeForce) const noexcept
   {
-    
+    if (rigidBody)
+    {
+      // Rotate force into world space
+      const SHVec3 FORCE  = SHVec3::Rotate(relativeForce, rigidBody->GetMotionState().orientation);
+      rigidBody->AddForce(FORCE);
+    }
   }
 
   void SHRigidBodyComponent::AddRelativeForceAtLocalPos(const SHVec3& relativeForce, const SHVec3& localPos) const noexcept
   {
-    
+    if (rigidBody)
+    {
+      // Rotate force into world space
+      const SHVec3 FORCE  = SHVec3::Rotate(relativeForce, rigidBody->GetMotionState().orientation);
+      rigidBody->AddForce(FORCE, localPos);
+    }
   }
 
   void SHRigidBodyComponent::AddRelativeForceAtWorldPos(const SHVec3& relativeForce, const SHVec3& worldPos) const noexcept
   {
-    
+    if (rigidBody)
+    {
+      // Rotate force into world space
+      const SHVec3 FORCE  = SHVec3::Rotate(relativeForce, rigidBody->GetMotionState().orientation);
+      // Convert world pos into local space of the body
+      const SHVec3 LOCAL_POS = worldPos - rigidBody->GetWorldCentroid();
+
+      rigidBody->AddForce(FORCE, LOCAL_POS);
+    }
   }
 
   void SHRigidBodyComponent::AddTorque(const SHVec3& torque) const noexcept
   {
-    
+    if (rigidBody)
+      rigidBody->AddTorque(torque);
   }
 
   void SHRigidBodyComponent::AddRelativeTorque(const SHVec3& relativeTorque) const noexcept
   {
-    
+    if (rigidBody)
+    {
+      // Rotate force into world space
+      const SHVec3 TORQUE  = SHVec3::Rotate(relativeTorque, rigidBody->GetMotionState().orientation);
+      rigidBody->AddTorque(TORQUE);
+    }
   }
 
   void SHRigidBodyComponent::ClearForces() const noexcept
@@ -348,11 +335,6 @@ namespace SHADE
       rigidBody->ClearForces();
   }
 
-  void SHRigidBodyComponent::ClearTorque() const noexcept
-  {
-    
-  }
-
   /*-----------------------------------------------------------------------------------*/
   /* Public Function Member Definitions                                                */
   /*-----------------------------------------------------------------------------------*/

SHADE_Engine/src/Physics/Interface/SHRigidBodyComponent.h

@@ -93,6 +93,7 @@ namespace SHADE
     [[nodiscard]] SHVec3              GetAngularVelocity  ()  const noexcept;
 
     [[nodiscard]] SHVec3              GetPosition         ()  const noexcept;
+    [[nodiscard]] SHVec3              GetRotation         ()  const noexcept;
 
     /*---------------------------------------------------------------------------------*/
     /* Setter Functions                                                                */
@@ -138,7 +139,6 @@ namespace SHADE
     void  AddRelativeTorque           (const SHVec3& relativeTorque)                        const noexcept;
 
     void  ClearForces                 () const noexcept;
-    void  ClearTorque                 () const noexcept;
 
   private:
     /*---------------------------------------------------------------------------------*/

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

@@ -62,15 +62,23 @@ namespace SHADE
       if (!MOTION_STATE)
         continue;
 
-      SHVec3 renderPosition = rigidBodyComponent.IsInterpolating() ? MOTION_STATE.InterpolatePositions(FACTOR) : MOTION_STATE.position;
+      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.
        */
-
-
-      transformComponent->SetWorldPosition(renderPosition);
-      // TODO: SetOrientation
     }
 
     // Collision & Trigger messages

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

@@ -54,7 +54,7 @@ namespace SHADE
         SHMotionState& motionState = physicsObject.rigidBody->GetMotionState();
 
         motionState.ForcePosition(TRANSFORM_COMPONENT->GetWorldPosition());
-        // TODO: Force Orientation
+        motionState.ForceOrientation(TRANSFORM_COMPONENT->GetWorldOrientation());
       }
 
       if (physicsObject.collider)

SHADE_Engine/src/Physics/System/SHPhysicsDebugDrawSystem.cpp

@@ -120,7 +120,10 @@ namespace SHADE
         case SHCollisionShape::Type::SPHERE:
         {
           const SHSphereCollisionShape* SPHERE = dynamic_cast<const SHSphereCollisionShape*>(SHAPE);
-          drawSphere(debugDrawSystem, *SPHERE);
+
+          // Compute rotation of sphere
+          const SHVec3 ROTATION = collider.GetOrientation().ToEuler() + SPHERE->GetRotationOffset();
+          drawSphere(debugDrawSystem, *SPHERE, ROTATION);
 
           break;
         }
@@ -137,10 +140,10 @@ namespace SHADE
     }
   }
 
-  void SHPhysicsDebugDrawSystem::drawSphere(SHDebugDrawSystem* debugDrawSystem, const SHSphereCollisionShape& sphere) noexcept
+  void SHPhysicsDebugDrawSystem::drawSphere(SHDebugDrawSystem* debugDrawSystem, const SHSphereCollisionShape& sphere, const SHVec3& rotation) noexcept
   {
     const SHColour& DRAW_COLOUR = DEBUG_DRAW_COLOURS[SHUtilities::ConvertEnum(sphere.IsTrigger() ? Colours::TRIGGER : Colours::COLLIDER)];
-    debugDrawSystem->DrawSphere(DRAW_COLOUR, sphere.GetCenter(), sphere.GetWorldRadius());
+    debugDrawSystem->DrawSphere(DRAW_COLOUR, sphere.GetCenter(), rotation, sphere.GetWorldRadius());
   }
 
   void SHPhysicsDebugDrawSystem::drawContact(SHDebugDrawSystem* debugDrawSystem, const DebugDrawInfo::Contact& contactInfo) noexcept

SHADE_Engine/src/Physics/System/SHPhysicsDebugDrawSystem.h

@@ -138,8 +138,8 @@ namespace SHADE
 
     SHEventHandle onColliderDraw(SHEventPtr onColliderDrawEvent);
 
-    static void   drawCollider  (SHDebugDrawSystem* debugDrawSystem, const SHCollider& collider)                noexcept;
+    static void   drawCollider  (SHDebugDrawSystem* debugDrawSystem, const SHCollider& collider)                                    noexcept;
-    static void   drawSphere    (SHDebugDrawSystem* debugDrawSystem, const SHSphereCollisionShape& sphere)      noexcept;
+    static void   drawSphere    (SHDebugDrawSystem* debugDrawSystem, const SHSphereCollisionShape& sphere, const SHVec3& rotation)  noexcept;
 
     static void   drawContact   (SHDebugDrawSystem* debugDrawSystem, const DebugDrawInfo::Contact& contactInfo) noexcept;
     static void   drawRaycast   (SHDebugDrawSystem* debugDrawSystem, const DebugDrawInfo::Raycast& raycastInfo) noexcept;

SHADE_Engine/src/Tools/SHDebugDraw.cpp

@@ -68,7 +68,7 @@ namespace SHADE
 
   void SHDebugDraw::Sphere(const SHVec4& color, const SHVec3& pos, double radius)
   {
-    dbgDrawSys->DrawSphere(color, pos, radius);
+    dbgDrawSys->DrawSphere(color, pos, SHVec3::Zero, radius);
   }
 
   void SHDebugDraw::PersistentLine(const SHVec4& color, const SHVec3& startPt, const SHVec3& endPt)

SHADE_Managed/src/Components/RigidBody.cxx

@@ -217,10 +217,4 @@ namespace SHADE
     {
         return Convert::ToCLI(GetNativeComponent()->GetTorque());
     }
-
-    void RigidBody::ClearTorque()
-    {
-        GetNativeComponent()->ClearTorque();
-    }
-
 }

SHADE_Managed/src/Components/RigidBody.hxx

@@ -155,7 +155,6 @@ namespace SHADE
         void AddRelativeTorque(Vector3 relativeForce);
 
         Vector3 GetTorque();
-        void ClearTorque();
     };
   
 }