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
-  glm::vec3 axis = glm::normalize(glm::vec3(0.0f, capsule.height, 0.0f));
-  float length = capsule.height - 2 * capsule.radius;
+  // Calculate the axis of the capsule
+  glm::vec3 capsuleAxis = glm::normalize(ray.direction);
+  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
-  glm::vec3 originToCenter = -capsule.origin;
-  float proj = glm::dot(originToCenter, axis);
-  glm::vec3 closestPointOnAxis = capsule.origin + axis * proj;
+  // Calculate the sphere centers and radii of the capsule end-caps
+  glm::vec3 sphereP0 = capsule.origin;
+  glm::vec3 sphereP1 = capsule.origin + capsule.height * capsuleAxis;
+  float_t sphereR = capsule.radius;
 
-  // Calculate the distance between the closest point on the axis and the ray origin
-  float distToClosestPoint = glm::length(closestPointOnAxis - capsule.origin);
-
-  // If the distance is greater than the capsule radius, the ray misses the capsule
-  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);
+  // Calculate the closest points on the capsule axis and the ray
+  glm::vec3 closestPointRay, closestPointAxis;
+  if(glm::distance(ray.origin, capsuleP0) < glm::distance(ray.origin, capsuleP1)) {
+    closestPointAxis = glm::clamp(glm::dot(ray.origin - capsuleP0, capsuleAxis), 0.0f, capsule.height) * capsuleAxis + capsuleP0;
   } 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