From 38f6ddcbef0e3e41080066100de219eccbacc659 Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Vladim=C3=ADr=20Vondru=C5=A1?= <mosra@centrum.cz>
Date: Fri, 1 Feb 2013 23:45:58 +0100
Subject: [PATCH] Physics: function for detecting first collision with shape in
 the group.

---
 src/Physics/ObjectShapeGroup.cpp     | 13 +++++++++
 src/Physics/ObjectShapeGroup.h       |  9 +++++++
 src/Physics/Test/ObjectShapeTest.cpp | 40 +++++++++++++++++++++++++++-
 3 files changed, 61 insertions(+), 1 deletion(-)

diff --git a/src/Physics/ObjectShapeGroup.cpp b/src/Physics/ObjectShapeGroup.cpp
index 013bf4d78..f6d7cb30c 100644
--- a/src/Physics/ObjectShapeGroup.cpp
+++ b/src/Physics/ObjectShapeGroup.cpp
@@ -15,6 +15,7 @@
 
 #include "ObjectShapeGroup.h"
 
+#include "Physics/AbstractShape.h"
 #include "ObjectShape.h"
 
 namespace Magnum { namespace Physics {
@@ -32,6 +33,18 @@ template<std::uint8_t dimensions> void ObjectShapeGroup<dimensions>::setClean()
     dirty = false;
 }
 
+template<std::uint8_t dimensions> ObjectShape<dimensions>* ObjectShapeGroup<dimensions>::firstCollision(const ObjectShape<dimensions>* shape) {
+    /* Nothing to test with, done */
+    if(!shape->shape()) return nullptr;
+
+    setClean();
+    for(std::size_t i = 0; i != this->size(); ++i)
+        if((*this)[i] != shape && (*this)[i]->shape() && (*this)[i]->shape()->collides(shape->shape()))
+            return (*this)[i];
+
+    return nullptr;
+}
+
 template class ObjectShapeGroup<2>;
 template class ObjectShapeGroup<3>;
 
diff --git a/src/Physics/ObjectShapeGroup.h b/src/Physics/ObjectShapeGroup.h
index 0c864cbb7..46db09721 100644
--- a/src/Physics/ObjectShapeGroup.h
+++ b/src/Physics/ObjectShapeGroup.h
@@ -71,6 +71,15 @@ template<std::uint8_t dimensions> class MAGNUM_PHYSICS_EXPORT ObjectShapeGroup:
          */
         void setClean();
 
+        /**
+         * @brief First collision of given shape with other shapes in the group
+         *
+         * Returns first shape colliding with given one. If there aren't any
+         * collisions, returns `nullptr`. Calls setClean() before the
+         * operation.
+         */
+        ObjectShape<dimensions>* firstCollision(const ObjectShape<dimensions>* shape);
+
     private:
         bool dirty;
 };
diff --git a/src/Physics/Test/ObjectShapeTest.cpp b/src/Physics/Test/ObjectShapeTest.cpp
index 256e453d0..a6b8c17b5 100644
--- a/src/Physics/Test/ObjectShapeTest.cpp
+++ b/src/Physics/Test/ObjectShapeTest.cpp
@@ -18,6 +18,7 @@
 #include "Physics/ObjectShapeGroup.h"
 #include "Physics/ObjectShape.h"
 #include "Physics/Point.h"
+#include "Physics/Sphere.h"
 #include "SceneGraph/MatrixTransformation3D.h"
 #include "SceneGraph/Scene.h"
 
@@ -28,13 +29,15 @@ class ObjectShapeTest: public Corrade::TestSuite::Tester {
         ObjectShapeTest();
 
         void clean();
+        void firstCollision();
 };
 
 typedef SceneGraph::Scene<SceneGraph::MatrixTransformation3D<>> Scene3D;
 typedef SceneGraph::Object<SceneGraph::MatrixTransformation3D<>> Object3D;
 
 ObjectShapeTest::ObjectShapeTest() {
-    addTests(&ObjectShapeTest::clean);
+    addTests(&ObjectShapeTest::clean,
+             &ObjectShapeTest::firstCollision);
 }
 
 void ObjectShapeTest::clean() {
@@ -78,6 +81,41 @@ void ObjectShapeTest::clean() {
     CORRADE_VERIFY(b.isDirty());
 }
 
+void ObjectShapeTest::firstCollision() {
+    Scene3D scene;
+    ObjectShapeGroup3D group;
+
+    Object3D a(&scene);
+    ObjectShape3D* aShape = new ObjectShape3D(&a, &group);
+    aShape->setShape(Physics::Sphere3D({1.0f, -2.0f, 3.0f}, 1.5f));
+
+    Object3D b(&scene);
+    ObjectShape3D* bShape = new ObjectShape3D(&b, &group);
+    bShape->setShape(Physics::Point3D({3.0f, -2.0f, 3.0f}));
+
+    Object3D c(&scene);
+    ObjectShape3D* cShape = new ObjectShape3D(&c, &group);
+
+    /* No-op if the object has no shape */
+    CORRADE_VERIFY(group.isDirty());
+    CORRADE_VERIFY(!group.firstCollision(cShape));
+    CORRADE_VERIFY(group.isDirty());
+
+    /* No collisions initially */
+    CORRADE_VERIFY(!group.firstCollision(aShape));
+    CORRADE_VERIFY(!group.firstCollision(bShape));
+    CORRADE_VERIFY(!group.isDirty());
+
+    /* Move point into sphere */
+    b.translate(Vector3::xAxis(-1.0f));
+
+    /* Collision */
+    CORRADE_VERIFY(group.isDirty());
+    CORRADE_VERIFY(group.firstCollision(aShape) == bShape);
+    CORRADE_VERIFY(group.firstCollision(bShape) == aShape);
+    CORRADE_VERIFY(!group.isDirty());
+}
+
 }}}
 
 CORRADE_TEST_MAIN(Magnum::Physics::Test::ObjectShapeTest)