Added implementation for raycasting into the collision space

SHADE_Engine/src/Math/Geometry/SHAABB.cpp

@@ -159,6 +159,8 @@ namespace SHADE
     {
       result.position = ray.position + ray.direction * result.distance;
       result.angle    = SHVec3::Angle(ray.position, result.position);
+
+      // TODO: Compute normal
     }
 
     return result;

SHADE_Engine/src/Math/Geometry/SHBox.cpp

@@ -111,6 +111,8 @@ namespace SHADE
     {
       result.position = ray.position + ray.direction * result.distance;
       result.angle    = SHVec3::Angle(ray.position, result.position);
+
+      // TODO: Compute Normal
     }
 
     return result;

SHADE_Engine/src/Math/Geometry/SHSphere.cpp

@@ -92,6 +92,8 @@ namespace SHADE
     {
       result.position = ray.position + ray.direction * result.distance;
       result.angle    = SHVec3::Angle(ray.position, result.position);
+
+      // TODO: Compute Normal
     }
 
     return result;

SHADE_Engine/src/Physics/Collision/Broadphase/SHDynamicAABBTree.cpp

@@ -249,9 +249,37 @@ namespace SHADE
   const std::vector<SHCollisionShapeID>& SHAABBTree::Query(const SHRay& ray, float distance) const noexcept
   {
     static std::vector<SHCollisionShapeID> potentialHits;
+    static std::stack<int32_t> nodeIndices;
 
     potentialHits.clear();
 
+    nodeIndices.push(root);
+    while (!nodeIndices.empty())
+    {
+      const int32_t INDEX = nodeIndices.top();
+      nodeIndices.pop();
+
+      if (INDEX == NULL_NODE)
+        continue;
+
+      const Node& NODE = nodes[INDEX];
+
+      const auto& RESULT = NODE.AABB.Raycast(ray);
+      if (!RESULT || RESULT.distance > distance)
+        continue;
+
+      if (isLeaf(INDEX))
+      {
+        potentialHits.emplace_back(NODE.id);
+      }
+      else
+      {
+        // Non-leaf nodes need to be traversed further
+        nodeIndices.push(NODE.left);
+        nodeIndices.push(NODE.right);
+      }
+    }
+
     return potentialHits;
   }
 

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

@@ -19,6 +19,7 @@
 #include "SHCollisionShapeID.h"
 #include "Math/Geometry/SHAABB.h"
 #include "Math/Transform/SHTransform.h"
+#include "Physics/Collision/SHPhysicsRaycastResult.h"
 
 namespace SHADE
 {
@@ -128,10 +129,10 @@ namespace SHADE
     /* Member Functions                                                                */
     /*---------------------------------------------------------------------------------*/
 
-    virtual void                    ComputeTransforms     ()            noexcept = 0;
+    virtual void                      ComputeTransforms     ()                  noexcept = 0;
-    [[nodiscard]] virtual SHMatrix  GetInertiaTensor      (float mass)  const noexcept = 0;
+    [[nodiscard]] virtual SHMatrix    GetInertiaTensor      (float mass)  const noexcept = 0;
-    [[nodiscard]] virtual SHMatrix  ComputeWorldTransform ()            const noexcept = 0;
+    [[nodiscard]] virtual SHMatrix    ComputeWorldTransform ()            const noexcept = 0;
-    [[nodiscard]] virtual SHAABB    ComputeAABB           ()            const noexcept = 0;
+    [[nodiscard]] virtual SHAABB      ComputeAABB           ()            const noexcept = 0;
 
   protected:
     /*---------------------------------------------------------------------------------*/

SHADE_Engine/src/Physics/Collision/SHCollisionSpace.cpp

@@ -160,6 +160,163 @@ namespace SHADE
       contactManager->Update();
   }
 
+  const SHCollisionSpace::RaycastHits& SHCollisionSpace::Raycast(const SHRay& ray, float distance, uint16_t layer) noexcept
+  {
+    raycastHits.clear();
+
+    const auto& POTENTIAL_HITS = broadphase.Query(ray, distance);
+    if (POTENTIAL_HITS.empty())
+      return raycastHits;
+
+    // Test potential hits individually
+    // Cull entities that are on different layers
+    for (auto& shapeID : POTENTIAL_HITS)
+    {
+      // Get shape
+      const EntityID EID = shapeID.GetEntityID();
+      const uint32_t IDX = shapeID.GetShapeIndex();
+
+      const auto* COLLIDER  = colliders.find(EID)->second;
+      const auto* SHAPE     = COLLIDER->GetCollisionShape(IDX);
+
+      // Cull by layer
+      const bool LAYER_MATCH = SHAPE->GetCollisionTag().GetMask() & layer;
+      if (!LAYER_MATCH)
+        continue;
+
+      // Well this is awkward...I honestly don't have the mental capacity to solve this oversight right now.
+      // Cast the underlying shape to raycast. Convex hulls do not have an inherited raycast function
+
+      SHRaycastResult result;
+      switch (SHAPE->GetType())
+      {
+        case SHCollisionShape::Type::SPHERE:
+        {
+          result = dynamic_cast<const SHSphereCollisionShape*>(SHAPE)->Raycast(ray);
+          break;
+        }
+        case SHCollisionShape::Type::BOX:
+        {
+          result = dynamic_cast<const SHBoxCollisionShape*>(SHAPE)->Raycast(ray);
+          break;
+        }
+        case SHCollisionShape::Type::CAPSULE:
+        {
+          // TODO
+          break;
+        }
+        default: break;
+      }
+
+      // If distance is greater than specified, skip this result
+      if (!result || result.distance > distance)
+        continue;
+
+      SHPhysicsRaycastResult physicsResult;
+      physicsResult.hit         = result.hit;
+      physicsResult.distance    = result.distance;
+      physicsResult.angle       = result.angle;
+      physicsResult.position    = result.position;
+      physicsResult.normal      = result.normal;
+      physicsResult.entityHit   = EID;
+      physicsResult.shapeIndex  = IDX;
+
+      raycastHits.emplace_back(physicsResult);
+    }
+
+    // Sort by distance
+    std::ranges::sort(raycastHits.begin(), raycastHits.end(), [](const SHPhysicsRaycastResult& lhs, const SHPhysicsRaycastResult& rhs)
+    {
+      return lhs.distance < rhs.distance;
+    });
+
+    return raycastHits;
+  }
+
+  const SHCollisionSpace::RaycastHits& SHCollisionSpace::Linecast(const SHVec3& start, const SHVec3& end, uint16_t layer) noexcept
+  {
+    // Create bounded ray and reuse the raycast function
+    const SHRay BOUNDED_RAY{ start, SHVec3::Normalise(end - start) };
+    const float DISTANCE = SHVec3::Distance(end, start);
+
+    return Raycast(BOUNDED_RAY, DISTANCE, layer);
+  }
+
+  const SHCollisionSpace::RaycastHits& SHCollisionSpace::ColliderRaycast(EntityID colliderEID, const SHRay& ray, float distance, uint16_t layer) noexcept
+  {
+    raycastHits.clear();
+
+    const auto& POTENTIAL_HITS = broadphase.Query(ray, distance);
+    if (POTENTIAL_HITS.empty())
+      return raycastHits;
+
+    // Test potential hits individually
+    // Cull entities that are on different layers
+    for (auto& shapeID : POTENTIAL_HITS)
+    {
+      // Get shape
+      const EntityID  EID       = shapeID.GetEntityID();
+      const auto*     COLLIDER  = colliders.find(EID)->second;
+      // Cull any shapes on the same entity
+      if (EID == colliderEID)
+        continue;
+
+      const uint32_t  IDX   = shapeID.GetShapeIndex();
+      const auto*     SHAPE = COLLIDER->GetCollisionShape(IDX);
+
+      // Cull by layer
+      const bool LAYER_MATCH = SHAPE->GetCollisionTag().GetMask() & layer;
+      if (!LAYER_MATCH)
+        continue;
+
+      // Well this is awkward...I honestly don't have the mental capacity to solve this oversight right now.
+      // Cast the underlying shape to raycast. Convex hulls do not have an inherited raycast function
+
+      SHRaycastResult result;
+      switch (SHAPE->GetType())
+      {
+        case SHCollisionShape::Type::SPHERE:
+        {
+          result = dynamic_cast<const SHSphereCollisionShape*>(SHAPE)->Raycast(ray);
+          break;
+        }
+        case SHCollisionShape::Type::BOX:
+        {
+          result = dynamic_cast<const SHBoxCollisionShape*>(SHAPE)->Raycast(ray);
+          break;
+        }
+        case SHCollisionShape::Type::CAPSULE:
+        {
+          // TODO
+          break;
+        }
+        default: break;
+      }
+
+      // If distance is greater than specified, skip this result
+      if (!result || result.distance > distance)
+        continue;
+
+      SHPhysicsRaycastResult physicsResult;
+      physicsResult.hit         = result.hit;
+      physicsResult.distance    = result.distance;
+      physicsResult.angle       = result.angle;
+      physicsResult.position    = result.position;
+      physicsResult.normal      = result.normal;
+      physicsResult.entityHit   = EID;
+      physicsResult.shapeIndex  = IDX;
+
+      raycastHits.emplace_back(physicsResult);
+    }
+
+    // Sort by distance
+    std::ranges::sort(raycastHits.begin(), raycastHits.end(), [](const SHPhysicsRaycastResult& lhs, const SHPhysicsRaycastResult& rhs)
+    {
+      return lhs.distance < rhs.distance;
+    });
+
+    return raycastHits;
+  }
 
   /*-----------------------------------------------------------------------------------*/
   /* Private Member Functions Definitions                                              */

SHADE_Engine/src/Physics/Collision/SHCollisionSpace.h

@@ -15,9 +15,10 @@
 
 // Project Headers
 #include "Broadphase/SHDynamicAABBTree.h"
-#include "Contacts/SHCollisionEvents.h"
 #include "Physics/Dynamics/SHContactManager.h"
 #include "SHCollider.h"
+#include "SHPhysicsRaycastResult.h"
+#include "CollisionTags/SHCollisionTags.h"
 
 namespace SHADE
 {
@@ -33,6 +34,12 @@ namespace SHADE
   class SH_API SHCollisionSpace
   {
   public:
+    /*---------------------------------------------------------------------------------*/
+    /* Type Definitions                                                                */
+    /*---------------------------------------------------------------------------------*/
+
+    using RaycastHits = std::vector<SHPhysicsRaycastResult>;
+
     /*---------------------------------------------------------------------------------*/
     /* Constructors & Destructor                                                       */
     /*---------------------------------------------------------------------------------*/
@@ -63,7 +70,7 @@ namespace SHADE
      * @param collider
      * A collider to add. Duplicates will be ignored.
     */
-    void AddCollider      (SHCollider*  collider)   noexcept;
+    void AddCollider (SHCollider*  collider)   noexcept;
 
     /**
      * @brief
@@ -72,21 +79,87 @@ namespace SHADE
      * @param collider
      * A collider to remove. If a reference to it doesn't exist, it will be ignored.
     */
-    void RemoveCollider   (SHCollider*  collider)   noexcept;
+    void RemoveCollider (SHCollider*  collider)   noexcept;
 
     /**
      * @brief
      * Invoke this method to update the broadphase of colliders that have been moved since
      * the last frame.
     */
-    void UpdateBroadphase ()                        noexcept;
+    void UpdateBroadphase () noexcept;
 
     /**
      * @brief 
      * Detects collisions between all colliders. Results are sent to the attached contact
      * manager for resolution.
     */
-    void DetectCollisions ()                        noexcept;
+    void DetectCollisions () noexcept;
+
+    /**
+     * @brief
+     * Casts a ray into the collision space.
+     * @param ray
+     * The ray to cast. The direction of the ray must be normalised.
+     * @param distance
+     * The distance to cast the ray. Defaults to infinity.
+     * @param layer
+     * The layer(s) the ray is casting on. Defaults to all layers.
+     * @return
+     * A container of all the objects the raycast hit, ordered by distance. <br/>
+     * The first object in the container is the first object hit etc.
+     */
+    [[nodiscard]] const RaycastHits& Raycast
+    (
+      const SHRay&  ray
+    , float         distance  = std::numeric_limits<float>::infinity()
+    , uint16_t      layer     = static_cast<uint16_t>(SHCollisionTag::Layer::ALL)
+    ) noexcept;
+
+    /**
+     * @brief
+     * Casts a bounded ray into the collision space.
+     * @param start
+     * The origin of the ray.
+     * @param end
+     * The end of the ray.
+     * @param layer
+     * The layer(s) the ray is casting on. Defaults to all layers.
+     * @return
+     * A container of all the objects the raycast hit, ordered by distance. <br/>
+     * The first object in the container is the first object hit etc.
+     */
+    [[nodiscard]] const RaycastHits& Linecast
+    (
+      const SHVec3& start
+    , const SHVec3& end
+    , uint16_t      layer   = static_cast<uint16_t>(SHCollisionTag::Layer::ALL)
+    ) noexcept;
+
+    /**
+     * @brief
+     * Casts a ray into the collision space from a collider's position. <br/>
+     * The collider and all it's shapes will be ignored.
+     * @param colliderEID
+     * The entityID of the collider to cast from.
+     * @param ray
+     * The ray to cast. <br/>
+     * The position of the ray is the position offset from the collider's position. <br/>
+     * The direction of the ray must be normalised.
+     * @param distance
+     * The distance to cast the ray. Defaults to infinity.
+     * @param layer
+     * The layer(s) the ray is casting on. Defaults to all layers.
+     * @return
+     * A container of all the objects the raycast hit, ordered by distance. <br/>
+     * The first object in the container is the first object hit etc.
+     */
+    [[nodiscard]] const RaycastHits& ColliderRaycast
+    (
+      EntityID      colliderEID 
+    , const SHRay&  ray
+    , float         distance  = std::numeric_limits<float>::infinity()
+    , uint16_t      layer     = static_cast<uint16_t>(SHCollisionTag::Layer::ALL)
+    ) noexcept;
 
   private:
     /*---------------------------------------------------------------------------------*/
@@ -111,20 +184,24 @@ namespace SHADE
     Colliders         colliders;
     NarrowphaseBatch  narrowphaseBatch;
 
+    RaycastHits       raycastHits;    // Reusable container for raycast results
+
     SHAABBTree        broadphase;
 
+    
+
     /*---------------------------------------------------------------------------------*/
     /* Member Functions                                                                */
     /*---------------------------------------------------------------------------------*/
 
     // Broadphase helpers
 
-    void broadphaseQuery    (SHRigidBody::Type rigidBodyType, SHCollider* collider)       noexcept;
+    void broadphaseQuery (SHRigidBody::Type rigidBodyType, SHCollider* collider) noexcept;
 
     // Narrowphase helpers
 
-    void collideTriggers    (const SHCollisionKey& key, NarrowphasePair& narrowphasePair) const noexcept;
+    void collideTriggers  (const SHCollisionKey& key, NarrowphasePair& narrowphasePair) const noexcept;
-    void collideManifolds   (const SHCollisionKey& key, NarrowphasePair& narrowphasePair) const noexcept;
+    void collideManifolds (const SHCollisionKey& key, NarrowphasePair& narrowphasePair) const noexcept;
 
   };