Something wrong with capsule

src/dawn/physics/3d/Ray3D.cpp

@@ -11,7 +11,8 @@ bool_t Dawn::raytestSphere(
   struct Ray3D ray,
   struct PhysicsSphere sphere,
   glm::vec3 *hit,
-  glm::vec3 *normal
+  glm::vec3 *normal,
+  float_t *distance
 ) {
   assertNotNull(hit);
   assertNotNull(normal);
@@ -22,10 +23,10 @@ bool_t Dawn::raytestSphere(
     sphere.center, sphere.radius,
     h, n
   );
-
+  if(!result) return result;
   *hit = h;
   *normal = n;
-
+  *distance = glm::distance(ray.origin, h);
   return result;
 }
 
@@ -209,49 +210,46 @@ bool_t Dawn::raytestCapsule(
   glm::vec3 *normal,
   float_t *distance
 ) {
-  // Calculate the axis and length of the capsule
+  // Calculate the axis of the capsule
-  glm::vec3 axis = glm::normalize(glm::vec3(0.0f, capsule.height, 0.0f));
+  glm::vec3 capsuleAxis = glm::normalize(ray.direction);
-  float length = capsule.height - 2 * capsule.radius;
+  glm::vec3 capsuleP0 = capsule.origin;
+  glm::vec3 capsuleP1 = capsule.origin + capsule.height * capsuleAxis;
 
-  // Calculate the closest point on the capsule axis to the ray origin
+  // Calculate the sphere centers and radii of the capsule end-caps
-  glm::vec3 originToCenter = -capsule.origin;
+  glm::vec3 sphereP0 = capsule.origin;
-  float proj = glm::dot(originToCenter, axis);
+  glm::vec3 sphereP1 = capsule.origin + capsule.height * capsuleAxis;
-  glm::vec3 closestPointOnAxis = capsule.origin + axis * proj;
+  float_t sphereR = capsule.radius;
 
-  // Calculate the distance between the closest point on the axis and the ray origin
+  // Calculate the closest points on the capsule axis and the ray
-  float distToClosestPoint = glm::length(closestPointOnAxis - capsule.origin);
+  glm::vec3 closestPointRay, closestPointAxis;
-
+  if(glm::distance(ray.origin, capsuleP0) < glm::distance(ray.origin, capsuleP1)) {
-  // If the distance is greater than the capsule radius, the ray misses the capsule
+    closestPointAxis = glm::clamp(glm::dot(ray.origin - capsuleP0, capsuleAxis), 0.0f, capsule.height) * capsuleAxis + capsuleP0;
-  if (distToClosestPoint > capsule.radius) {
-      return false;
-  }
-
-  // Calculate the distance between the closest point on the axis and the capsule end points
-  float distToCapsuleEnds = glm::length(originToCenter - axis * proj) - length * 0.5f;
-
-  // Calculate the distance along the ray to the point of intersection with the capsule
-  float t1 = glm::dot(-capsule.origin, ray.direction);
-  float t2 = glm::dot(closestPointOnAxis - capsule.origin, ray.direction);
-  float t3 = glm::sqrt(capsule.radius * capsule.radius - distToClosestPoint * distToClosestPoint);
-  float t4 = glm::sqrt(distToCapsuleEnds * distToCapsuleEnds + t3 * t3);
-  float tEnter = t1 + (t2 - t4);
-  float tExit = t1 + (t2 + t4);
-
-  // If the intersection point is behind the ray origin or beyond the ray endpoint, the ray misses the capsule
-  if (tEnter < 0 || tEnter > tExit) {
-      return false;
-  }
-
-  // Set the output parameters
-  *distance = tEnter;
-
-  // Calculate the hit point and normal
-  *point = capsule.origin + ray.direction * tEnter;
-  if (tEnter < 0 || tEnter > tExit) {
-    *normal = glm::normalize(*point);
   } else {
-    *normal = glm::normalize(*point - closestPointOnAxis);
+    closestPointAxis = glm::clamp(glm::dot(ray.origin - capsuleP1, -capsuleAxis), 0.0f, capsule.height) * -capsuleAxis + capsuleP1;
   }
 
+  closestPointRay = glm::clamp(
+    glm::dot(closestPointAxis - ray.origin, ray.direction),
+    0.0f, glm::length(ray.direction)
+  ) * ray.direction + ray.origin;
+
+  // Calculate the distance between the closest points on the ray and the axis
+  glm::vec3 temp = (closestPointRay - closestPointAxis);
+  float_t distanceSquared = glm::dot(temp, temp);
+
+  // Check if the ray intersects the end-caps of the capsule
+  if(
+    raytestSphere(ray, { .center = sphereP0, .radius = sphereR }, point, normal, distance) ||
+    raytestSphere(ray, { .center = sphereP1, .radius = sphereR }, point, normal, distance)
+  ) {
+    *normal = glm::normalize(*point - sphereP0);
+    return true;
+  }
+
+  // Check if the ray intersects the cylinder part of the capsule
+  if(distanceSquared > sphereR * sphereR) return false;
+  *distance = glm::distance(ray.origin, closestPointRay);
+  *point = closestPointRay;
+  *normal = glm::normalize(*point - closestPointAxis);
   return true;
 }

src/dawn/physics/3d/Ray3D.hpp

@@ -21,7 +21,8 @@ namespace Dawn {
     struct Ray3D ray,
     struct PhysicsSphere sphere,
     glm::vec3 *hit,
-    glm::vec3 *normal
+    glm::vec3 *normal,
+    float_t *distance
   );
 
   bool_t raytestTriangle(

src/dawn/scene/Scene.cpp

@@ -28,7 +28,7 @@ void Scene::update() {
   this->itemsNotInitialized.clear();
 
   #if DAWN_DEBUG_BUILD
-    this->debugGrid();
+    // this->debugGrid();
     this->debugOrigin();
     this->debugHitboxes();
   #endif

src/dawnrose/scene/components/PlayerController.cpp

@@ -6,6 +6,8 @@
 #include "PlayerController.hpp"
 #include "game/DawnGame.hpp"
 
+#include "scene/components/physics/3d/CapsuleCollider.hpp"
+
 using namespace Dawn;
 
 PlayerController::PlayerController(SceneItem *item) : SceneItemComponent(item) {
@@ -34,5 +36,34 @@ void PlayerController::onStart() {
 
     // Move / Update
     transform->setLocalPosition(transform->getLocalPosition() + (velocity * delta));
+
+
+    // tEST
+    auto collider = item->getComponent<CapsuleCollider>();
+    Collider3DRayResult result;
+
+    struct Ray3D ray;
+    ray.origin = glm::vec3(0, 0.5f, 1);
+    ray.direction = glm::vec3(10, 0, 0);
+
+    struct Color color = COLOR_MAGENTA;
+
+    if(collider->raycast(&result, ray)) {
+      // std::cout << "Collide!" << std::endl;
+      color = COLOR_WHITE;
+
+      getScene()->debugRay({
+        .start = result.point,
+        .direction = (result.normal * 100.0f),
+        .color = COLOR_GREEN
+      });
+    }
+
+    getScene()->debugRay({
+      .start = ray.origin,
+      .direction = ray.direction,
+      .color = color
+    });
+
   }, getScene()->eventSceneUpdate);
 }

src/dawnrose/scenes/HelloWorldScene.hpp

@@ -38,9 +38,8 @@ namespace Dawn {
         wallHitbox->min = -(wallHitbox->max = glm::vec3(1, 1, 5));
 
         camera = Camera::create(this);
-        // camera->transform->lookAt(glm::vec3(0, 0, 8), glm::vec3(0, 0, 0));
-        // auto gameCamera = camera->item->addComponent<GameCamera>();
         camera->transform->lookAt(glm::vec3(4, 4, 4), glm::vec3(0, 0, 0));
+        // auto gameCamera = camera->item->addComponent<GameCamera>();
         // gameCamera->player = player;
       }