Logic and Debug Vis for vehicle path checking

rwengine/src/objects/VehicleObject.cpp

@@ -1083,3 +1083,15 @@ float VehicleObject::isOnSide(const glm::vec3& point) {
     return distance;
 }
 
+std::tuple<glm::vec3, glm::vec3> VehicleObject::obstacleCheckVolume() const {
+    const auto& dim = info->handling.dimensions;
+    const auto kMaxDistance = 20.f;
+    const auto velocity = getVelocity() / info->handling.maxVelocity;
+    const auto lookDistance = glm::clamp(kMaxDistance * velocity, 0.f, kMaxDistance);
+    const glm::vec3 areaSize{dim.x * 0.6f, 1.0f + lookDistance, 1.0f};
+    return {
+            {0.f, dim.y * 0.5f + areaSize.y, 0.f},
+            areaSize,
+    };
+}
+

rwengine/src/objects/VehicleObject.hpp

@@ -246,6 +246,11 @@ public:
 
     void grantOccupantRewards(CharacterObject* character);
 
+    /**
+     * @return The position, and size of the area that must be free for the vehicle to continue.
+     */
+    std::tuple<glm::vec3, glm::vec3> obstacleCheckVolume() const;
+
 private:
     void setupModel();
     void registerPart(ModelFrame* mf);

rwengine/src/render/DebugDraw.hpp

@@ -30,6 +30,12 @@ public:
 
     void drawLine(const btVector3 &from, const btVector3 &to,
                   const btVector3 &color) override;
+    void drawLine(const glm::vec3 &from, const glm::vec3 &to,
+                  const glm::vec3 &color) {
+        drawLine(btVector3{from.x, from.y, from.z},
+                 btVector3{to.x, to.y, to.z},
+                 btVector3{color.r, color.g, color.b});
+    }
     void drawContactPoint(const btVector3 &pointOnB, const btVector3 &normalOnB,
                           btScalar distance, int lifeTime,
                           const btVector3 &color) override;

rwgame/RWGame.cpp

@@ -24,6 +24,7 @@
 #include <functional>
 #include <iomanip>
 #include <iostream>
+#include <algorithm>
 
 namespace {
 static constexpr std::array<
@@ -775,16 +776,48 @@ void RWGame::renderDebugPaths(float time) {
     for (auto& p : world->pedestrianPool.objects) {
         auto v = static_cast<CharacterObject*>(p.second.get());
 
-        static const btVector3 color(1.f, 1.f, 0.f);
+        static const glm::vec3 color(1.f, 1.f, 0.f);
 
-        if (v->controller->targetNode && v->getCurrentVehicle()) {
+        if (auto vehicle = v->getCurrentVehicle(); vehicle)
-            const glm::vec3 pos1 = v->getPosition();
+        {
-            const glm::vec3 pos2 = v->controller->targetNode->position;
+            if (v->controller->targetNode) {
+                debug.drawLine(v->getPosition(), v->controller->targetNode->position, color);
+            }
 
-            btVector3 position1(pos1.x, pos1.y, pos1.z);
+            auto [center, halfSize] = vehicle->obstacleCheckVolume();
-            btVector3 position2(pos2.x, pos2.y, pos2.z);
+            std::array<glm::vec3, 8> corners { {
+                    glm::vec3{- halfSize.x, - halfSize.y, - halfSize.z},
+                    glm::vec3{+ halfSize.x, - halfSize.y, - halfSize.z},
+                    glm::vec3{- halfSize.x, - halfSize.y, + halfSize.z},
+                    glm::vec3{+ halfSize.x, - halfSize.y, + halfSize.z},
+                    glm::vec3{- halfSize.x, + halfSize.y, - halfSize.z},
+                    glm::vec3{+ halfSize.x, + halfSize.y, - halfSize.z},
+                    glm::vec3{- halfSize.x, + halfSize.y, + halfSize.z},
+                    glm::vec3{+ halfSize.x, + halfSize.y, + halfSize.z},
+                }
+            };
+            const auto iRotation = (vehicle->getRotation());
+            const auto rCenter = iRotation * center;
+            std::transform(corners.begin(), corners.end(), corners.begin(),
+                           [&](const auto& p) -> glm::vec3 {
+                               return vehicle->getPosition() + rCenter + iRotation * p;
+                           });
 
-            debug.drawLine(position1, position2, color);
+            static const glm::vec3 color2(1.f, 0.f, 0.f);
+            debug.drawLine(corners[0], corners[1], color2);
+            debug.drawLine(corners[0], corners[2], color2);
+            debug.drawLine(corners[3], corners[1], color2);
+            debug.drawLine(corners[3], corners[2], color2);
+
+            debug.drawLine(corners[0], corners[4], color2);
+            debug.drawLine(corners[1], corners[5], color2);
+            debug.drawLine(corners[2], corners[6], color2);
+            debug.drawLine(corners[3], corners[7], color2);
+
+            debug.drawLine(corners[4], corners[5], color2);
+            debug.drawLine(corners[4], corners[6], color2);
+            debug.drawLine(corners[7], corners[5], color2);
+            debug.drawLine(corners[7], corners[6], color2);
         }
     }