Merge pull request #679 from danhedron/feat/vehicle_volumequery

Use collision detection for AI vehicle behaviour

rwengine/CMakeLists.txt

@@ -49,6 +49,8 @@ set(RWENGINE_SOURCES
 
     src/dynamics/CollisionInstance.cpp
     src/dynamics/CollisionInstance.hpp
+    src/dynamics/HitTest.cpp
+    src/dynamics/HitTest.hpp
     src/dynamics/RaycastCallbacks.hpp
 
     src/engine/Animator.cpp

rwengine/src/ai/CharacterController.cpp

@@ -19,6 +19,7 @@
 #include <LinearMath/btScalar.h>
 
 #include <rw/debug.hpp>
+#include <dynamics/HitTest.hpp>
 
 #include "data/WeaponData.hpp"
 #include "engine/Animator.hpp"
@@ -193,91 +194,25 @@ void CharacterController::steerTo(const glm::vec3 &target) {
     vehicle->setSteeringAngle(steeringAngle, true);  
 }
 
-// @todo replace this by raytest/raycast logic
-bool CharacterController::checkForObstacles()
-{
+bool CharacterController::checkForObstacles() {
     // We can't drive without a vehicle
-    VehicleObject* vehicle = character->getCurrentVehicle();
+    VehicleObject *vehicle = character->getCurrentVehicle();
     if (vehicle == nullptr) {
         return false;
     }
 
-    // The minimal distance we test for objects
-    static constexpr float minColDist = 20.f;
+    HitTest test{*vehicle->engine->dynamicsWorld};
 
-    // Try to stop before pedestrians
-    for (const auto &obj : character->engine->pedestrianPool.objects) {
-        // Verify that the character isn't the driver and is walking
-        if (obj.second.get() != character &&
-            static_cast<CharacterObject *>(obj.second.get())->getCurrentVehicle() ==
-                nullptr) {
-            // Only check characters that are near our vehicle
-            if (glm::distance(vehicle->getPosition(),
-                              obj.second->getPosition()) <= minColDist) {
-                // Check if the character is in front of us and in our way
-                if (vehicle->isInFront(obj.second->getPosition()) > -3.f &&
-                    vehicle->isInFront(obj.second->getPosition()) < 10.f &&
-                    glm::abs(vehicle->isOnSide(obj.second->getPosition())) <
-                        3.f) {
-                    return true;
-                }
-            }
-        }
-    }
+    auto[center, halfSize] = vehicle->obstacleCheckVolume();
+    const auto rotation = vehicle->getRotation();
+    center = vehicle->getPosition() + rotation * center;
+    auto results = test.boxTest(center, halfSize, vehicle->getRotation());
 
-    // Brake when a car is in front of us and change lanes when possible
-    for (const auto &obj : character->engine->vehiclePool.objects) {
-        // Verify that the vehicle isn't our vehicle
-        if (obj.second.get() != vehicle) {
-            // Only check vehicles that are near our vehicle
-            if (glm::distance(vehicle->getPosition(),
-                              obj.second->getPosition()) <= minColDist) {
-                // Check if the vehicle is in front of us and in our way
-                if (vehicle->isInFront(obj.second->getPosition()) > 0.f &&
-                    vehicle->isInFront(obj.second->getPosition()) < 10.f &&
-                    glm::abs(vehicle->isOnSide(obj.second->getPosition())) <
-                        2.5f) {
-                    // Check if the road has more than one lane
-                    // @todo we don't know the direction of the road, so for
-                    // now, choose the bigger value
-                    int maxLanes =
-                        targetNode->rightLanes > targetNode->leftLanes
-                            ? targetNode->rightLanes
-                            : targetNode->leftLanes;
-                    if (maxLanes > 1) {
-                        // Change the lane, firstly check if there is an
-                        // occupant
-                        if (static_cast<VehicleObject *>(obj.second.get())
-                                ->getDriver() != nullptr) {
-                            // @todo for now we don't know the lane where the
-                            // player is currently driving so just slow down, in
-                            // the future calculate the lane
-                            if (static_cast<VehicleObject *>(obj.second.get())
-                                    ->getDriver()
-                                    ->isPlayer()) {
-                                return true;
-                            } else {
-                                int avoidLane =
-                                    static_cast<VehicleObject *>(obj.second.get())
-                                        ->getDriver()
-                                        ->controller->getLane();
-
-                                // @todo for now just two lanes
-                                if (avoidLane == 1)
-                                    character->controller->setLane(2);
-                                else
-                                    character->controller->setLane(1);
-                            }
-                        }
-                    } else {
-                        return true;
-                    }
-                }
-            }
-        }
-    }
-
-    return false;
+    return any_of(results.begin(), results.end(),
+                  [&](const auto &hit) {
+                      return !(hit.object == vehicle ||
+                               hit.body->isStaticObject());
+                  });
 }
 
 bool Activities::DriveTo::update(CharacterObject *character,
@@ -384,12 +319,16 @@ bool Activities::DriveTo::update(CharacterObject *character,
     }
 
     // Check whether a pedestrian or vehicle is in our way
-    if (controller->checkForObstacles() == true) {
+    if (controller->checkForObstacles()) {
         currentSpeed = 0.f;
     }
 
+    if (std::fabs(currentSpeed) < 0.1f) {
+        vehicle->setHandbraking(true);
+        vehicle->setThrottle(0.f);
+    }
     // Is the vehicle slower than it should be
-    if (vehicle->getVelocity() < currentSpeed) {
+    else if (vehicle->getVelocity() < currentSpeed) {
         vehicle->setHandbraking(false);
 
         // The vehicle is driving backwards, accelerate

rwengine/src/dynamics/HitTest.cpp

@@ -0,0 +1,64 @@
+#include "HitTest.hpp"
+#include <objects/GameObject.hpp>
+
+#ifdef _MSC_VER
+#pragma warning(disable : 4305 5033)
+#endif
+#include <btBulletDynamicsCommon.h>
+#include <BulletCollision/CollisionDispatch/btGhostObject.h>
+
+#ifdef _MSC_VER
+#pragma warning(default : 4305 5033)
+#endif
+
+namespace {
+
+HitTest::TestResult HitTestWorld(btDiscreteDynamicsWorld& world, btPairCachingGhostObject& tester)
+{
+    world.addCollisionObject(&tester);
+
+    HitTest::TestResult result;
+    result.reserve(static_cast<unsigned long>(tester.getNumOverlappingObjects()));
+
+    for (auto i = 0; i < tester.getNumOverlappingObjects(); ++i)
+    {
+        auto overlapping = tester.getOverlappingObject(i);
+
+        HitTest::Hit hit{};
+        hit.body = overlapping;
+
+        if (auto object = static_cast<GameObject*>(overlapping->getUserPointer()))
+        {
+            hit.object = object;
+        }
+
+        result.push_back(hit);
+    }
+
+    world.removeCollisionObject(&tester);
+
+    return result;
+}
+
+HitTest::TestResult HitTestWithShape(btDiscreteDynamicsWorld& world, btCollisionShape& shape,
+        const glm::vec3& center, const glm::quat& rotation) {
+    btPairCachingGhostObject ghost{};
+    btTransform xform{};
+    xform.setOrigin({center.x, center.y, center.z});
+    xform.setRotation({rotation.x, rotation.y, rotation.z, rotation.w});
+    ghost.setWorldTransform(xform);
+    ghost.setCollisionShape(&shape);
+    return HitTestWorld(world, ghost);
+}
+
+} // namespace
+
+HitTest::TestResult HitTest::sphereTest(const glm::vec3& center, float radius) {
+    btSphereShape sphere {radius};
+    return HitTestWithShape(_world, sphere, center, {});
+}
+
+HitTest::TestResult HitTest::boxTest(const glm::vec3 &center, const glm::vec3 &size, const glm::quat& rotation) {
+    btBoxShape box {{size.x, size.y, size.z}};
+    return HitTestWithShape(_world, box, center, rotation);
+}

rwengine/src/dynamics/HitTest.hpp

@@ -0,0 +1,39 @@
+#ifndef _RWENGINE_HITTEST_HPP_
+#define _RWENGINE_HITTEST_HPP_
+
+#include <glm/glm.hpp>
+#include <glm/gtx/quaternion.hpp>
+
+#include <vector>
+#include <memory>
+
+class btDiscreteDynamicsWorld;
+class btCollisionObject;
+class GameObject;
+
+/**
+ * Utility for performing collision tests against the world.
+ */
+class HitTest {
+public:
+    struct Hit {
+        btCollisionObject* body;
+        GameObject* object;
+    };
+    using TestResult = std::vector<Hit>;
+
+    explicit HitTest(btDiscreteDynamicsWorld& world)
+        : _world(world)
+    {}
+
+    ~HitTest() = default;
+
+    TestResult sphereTest(const glm::vec3& center, float radius);
+    TestResult boxTest(const glm::vec3& center, const glm::vec3& size, const glm::quat& rotation = {1.f, 0.f, 0.f, 0.f});
+
+private:
+    btDiscreteDynamicsWorld& _world;
+};
+
+
+#endif

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,17 +776,49 @@ 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()) {
-            const glm::vec3 pos1 = v->getPosition();
-            const glm::vec3 pos2 = v->controller->targetNode->position;
+        if (auto vehicle = v->getCurrentVehicle(); vehicle)
+        {
+            if (v->controller->targetNode) {
+                debug.drawLine(v->getPosition(), v->controller->targetNode->position, color);
+            }
 
-            btVector3 position1(pos1.x, pos1.y, pos1.z);
-            btVector3 position2(pos2.x, pos2.y, pos2.z);
+            auto [center, halfSize] = vehicle->obstacleCheckVolume();
+            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);
+        }
     }
 
     debug.flush(renderer);

tests/CMakeLists.txt

@@ -11,6 +11,7 @@ set(TESTS
     GameData
     GameWorld
     Garage
+    HitTest
     Input
     Items
     Lifetime

tests/test_HitTest.cpp

@@ -0,0 +1,91 @@
+#include <boost/test/unit_test.hpp>
+#include "test_Globals.hpp"
+#include <dynamics/HitTest.hpp>
+#include <engine/GameWorld.hpp>
+#include <dynamics/CollisionInstance.hpp>
+#ifdef _MSC_VER
+#pragma warning(disable : 4305 5033)
+#endif
+#include <btBulletDynamicsCommon.h>
+#include <BulletCollision/CollisionDispatch/btGhostObject.h>
+#ifdef _MSC_VER
+#pragma warning(default : 4305 5033)
+#endif
+
+namespace {
+
+struct HitTestFixture {
+    btDefaultCollisionConfiguration collisionConfig;
+    btCollisionDispatcher collisionDispatcher;
+    btDbvtBroadphase broadphase;
+    btGhostPairCallback overlappingPairCallback;
+    btSequentialImpulseConstraintSolver solver;
+    btDiscreteDynamicsWorld dynamicsWorld;
+    HitTest hitTest;
+
+    HitTestFixture()
+    : collisionDispatcher{&collisionConfig}
+    , dynamicsWorld{&collisionDispatcher, &broadphase, &solver, &collisionConfig}
+    , hitTest{dynamicsWorld}
+    {
+        broadphase.getOverlappingPairCache()->setInternalGhostPairCallback(
+                &overlappingPairCallback);
+    }
+};
+
+struct WithSphere : public HitTestFixture {
+    btSphereShape shape {0.5f};
+    std::unique_ptr<btRigidBody> target;
+    GameObject* object {reinterpret_cast<GameObject*>(0xDEADBEEF)};
+
+    WithSphere()
+    {
+        btDefaultMotionState ms;
+        btRigidBody::btRigidBodyConstructionInfo info {1.f, &ms, &shape};
+        target = std::make_unique<btRigidBody>(info);
+        target->setUserPointer(object);
+        dynamicsWorld.addRigidBody(target.get());
+    }
+
+    ~WithSphere() {
+        dynamicsWorld.removeRigidBody(target.get());
+    }
+};
+}
+
+BOOST_AUTO_TEST_SUITE(HitTestTests)
+
+BOOST_FIXTURE_TEST_CASE(test_creation, HitTestFixture) {
+    HitTest test {dynamicsWorld};
+}
+
+BOOST_FIXTURE_TEST_CASE(sphereTest_returns_result, WithSphere) {
+    const auto result = hitTest.sphereTest({0.f, 0.f, 0.f}, 1.f);
+    BOOST_CHECK_EQUAL(result.size(), 1);
+}
+
+BOOST_FIXTURE_TEST_CASE(boxTest_returns_result, WithSphere) {
+    auto sphereBoundingBoxEdge = glm::vec3{ 0.f, 0.f, shape.getRadius() };
+    const auto result = hitTest.boxTest(sphereBoundingBoxEdge, {0.01f, 0.01f, 0.01f});
+    BOOST_CHECK_EQUAL(result.size(), 1);
+}
+
+BOOST_FIXTURE_TEST_CASE(non_overlapping_test_returns_nothing, WithSphere) {
+    auto sphereBoundingBoxEdge = glm::vec3{ shape.getRadius() };
+    const auto result = hitTest.boxTest(sphereBoundingBoxEdge * 2.f, {0.01f, 0.01f, 0.01f});
+    BOOST_CHECK(result.empty());
+}
+
+BOOST_FIXTURE_TEST_CASE(result_contains_body, WithSphere) {
+    const auto result = hitTest.sphereTest({0.f, 0.f, 0.f}, 1.f);
+    BOOST_ASSERT(result.size() == 1);
+    BOOST_CHECK_EQUAL(result[0].body, target.get());
+}
+
+BOOST_FIXTURE_TEST_CASE(test_result_contains_object, WithSphere) {
+    const auto result = hitTest.sphereTest({0.f, 0.f, 0.f}, 1.f);
+    BOOST_ASSERT(result.size() == 1);
+    BOOST_CHECK_EQUAL(result[0].object, object);
+}
+
+BOOST_AUTO_TEST_SUITE_END()