From 030b72a8f3aad25e3755105d76276f706e7d6935 Mon Sep 17 00:00:00 2001
From: Tobias Leemann <tobias.leemann@fau.de>
Date: Fri, 24 Nov 2017 20:05:03 +0100
Subject: [PATCH] Implemented Iterative Collide Functor.
---
src/pe/fcd/IterativeCollideFunctor.h | 218 +++++++++++++++++++++++++++
tests/pe/CollisionTobiasGJK.cpp | 112 +++++++++++---
2 files changed, 305 insertions(+), 25 deletions(-)
create mode 100644 src/pe/fcd/IterativeCollideFunctor.h
diff --git a/src/pe/fcd/IterativeCollideFunctor.h b/src/pe/fcd/IterativeCollideFunctor.h
new file mode 100644
index 000000000..afdccd914
--- /dev/null
+++ b/src/pe/fcd/IterativeCollideFunctor.h
@@ -0,0 +1,218 @@
+//======================================================================================================================
+//
+// This file is part of waLBerla. waLBerla is free software: you can
+// redistribute it and/or modify it under the terms of the GNU General Public
+// License as published by the Free Software Foundation, either version 3 of
+// the License, or (at your option) any later version.
+//
+// waLBerla is distributed in the hope that it will be useful, but WITHOUT
+// ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
+// FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License
+// for more details.
+//
+// You should have received a copy of the GNU General Public License along
+// with waLBerla (see COPYING.txt). If not, see <http://www.gnu.org/licenses/>.
+//
+//! \file IterativeCollideFunctor.h
+//! \author Tobias Leemann
+//! \author Sebastian Eibl <sebastian.eibl@fau.de>
+//
+//======================================================================================================================
+
+#pragma once
+
+
+#include "pe/Types.h"
+#include "pe/collision/EPA.h"
+#include "pe/collision/GJK.h"
+#include "pe/rigidbody/Plane.h"
+#include "pe/rigidbody/Union.h"
+#include <pe/Thresholds.h>
+#include <boost/tuple/tuple.hpp>
+
+namespace walberla{
+namespace pe{
+namespace fcd {
+namespace iterative{
+
+ //function for all single rigid bodies.
+ template<typename Container>
+ inline bool generateContacts(GeomPrimitive *a, GeomPrimitive *b, Container& contacts_);
+
+ //Planes
+ template<typename Container>
+ inline bool generateContacts(Plane *a, GeomPrimitive *b, Container& contacts_);
+
+ template<typename Container>
+ inline bool generateContacts(GeomPrimitive *a, Plane *b, Container& contacts_);
+
+ template< typename Container>
+ inline bool generateContacts(Plane *a, Plane *b, Container& contacts_);
+
+ //Unions
+ template<typename BodyTupleA, typename BodyB, typename Container>
+ inline bool generateContacts(Union<BodyTupleA> *a, BodyB *b, Container& contacts_);
+
+ template<typename BodyA, typename BodyTupleB, typename Container>
+ inline bool generateContacts(BodyA *a, Union<BodyTupleB> *b, Container& contacts_);
+
+ template<typename BodyTupleA, typename BodyTupleB, typename Container>
+ inline bool generateContacts(Union<BodyTupleA> *a, Union<BodyTupleB> *b, Container& contacts_);
+
+ //Union and Plane
+ template<typename BodyTupleA, typename Container>
+ inline bool generateContacts(Union<BodyTupleA> *a, Plane *b, Container& contacts_);
+
+ template<typename BodyTupleB, typename Container>
+ inline bool generateContacts(Plane *a, Union<BodyTupleB> *b, Container& contacts_);
+}
+
+/* Iterative Collide Functor for contact Generation with iterative collision detection.
+ * Usage: fcd::GenericFCD<BodyTuple, fcd::IterativeCollideFunctor> testFCD;
+ * testFCD.generateContacts(...);
+ */
+template <typename Container>
+struct IterativeCollideFunctor
+{
+ Container& contacts_;
+
+ IterativeCollideFunctor(Container& contacts) : contacts_(contacts) {}
+
+ template< typename BodyType1, typename BodyType2 >
+ bool operator()( BodyType1* bd1, BodyType2* bd2) {
+ using namespace iterative;
+ return generateContacts(bd1, bd2, contacts_);
+ }
+};
+
+template <typename BodyType1, typename Container>
+struct IterativeSingleCollideFunctor
+{
+ BodyType1* bd1_;
+ Container& contacts_;
+
+ IterativeSingleCollideFunctor(BodyType1* bd1, Container& contacts) : bd1_(bd1), contacts_(contacts) {}
+
+ template< typename BodyType2 >
+ bool operator()( BodyType2* bd2) {
+ using namespace iterative;
+ return generateContacts( bd1_, bd2, contacts_);
+ }
+};
+
+
+namespace iterative{
+
+ //function for all single rigid bodies.
+ template<typename Container>
+ inline bool generateContacts(GeomPrimitive *a, GeomPrimitive *b, Container& contacts_){
+ Vec3 normal;
+ Vec3 contactPoint;
+ real_t penetrationDepth;
+
+ real_t margin = real_t(1e-4);
+ GJK gjk;
+ if(gjk.doGJKmargin(*a, *b, margin)){
+ //2. If collision is possible perform EPA.
+ EPA epa;
+ if(epa.doEPAmargin(*a, *b, gjk, normal, contactPoint, penetrationDepth, margin)){
+ contacts_.push_back( Contact(a, b, contactPoint, normal, penetrationDepth) );
+ return true;
+ }else{
+ return false;
+ }
+
+ }else{
+ return false;
+ }
+ }
+
+ //Planes
+ template<typename Container>
+ inline bool generateContacts(Plane *a, GeomPrimitive *b, Container& contacts_){
+ Vec3 normal;
+ Vec3 contactPoint;
+ real_t penetrationDepth;
+
+ Vec3 support_dir = -a->getNormal();
+ // We now have a direction facing to the "wall".
+ // Compute support point of body b in this direction. This will be the furthest point overlapping.
+ Vec3 contactp = b->support(support_dir);
+ real_t pdepth = contactp * a->getNormal() - a->getDisplacement();
+ if(pdepth < contactThreshold){ //We have a collision
+ normal = support_dir;
+ penetrationDepth = pdepth;
+ contactPoint = contactp + real_t(0.5) * penetrationDepth * normal;
+ contacts_.push_back( Contact(a, b, contactPoint, normal, penetrationDepth) );
+ return true;
+ }else{ //No collision
+ return false;
+ }
+ }
+
+ template<typename Container>
+ inline bool generateContacts(GeomPrimitive *a, Plane *b, Container& contacts_){
+ return generateContacts(b, a, contacts_);
+ }
+
+ //Planes cannot collide with each other
+ template< typename Container>
+ inline bool generateContacts(Plane*, Plane*, Container&){
+ return false;
+ }
+
+ //Unions
+ template<typename BodyTupleA, typename BodyB, typename Container>
+ inline bool generateContacts(Union<BodyTupleA> *a, BodyB *b, Container& contacts_){
+ IterativeSingleCollideFunctor<BodyB, Container> func(b, contacts_);
+ bool collision = false;
+ for( auto it=a->begin(); it!=a->end(); ++it )
+ {
+ collision |= SingleCast<BodyTupleA, IterativeSingleCollideFunctor<BodyB, Container>, bool>::execute(*it, func);
+ }
+ return collision;
+ }
+
+ template<typename BodyA, typename BodyTupleB, typename Container>
+ inline bool generateContacts(BodyA *a, Union<BodyTupleB> *b, Container& contacts_){
+ return generateContacts(b, a, contacts_);
+ }
+
+ template<typename BodyTupleA, typename BodyTupleB, typename Container>
+ inline bool generateContacts(Union<BodyTupleA> *a, Union<BodyTupleB> *b, Container& contacts_){
+ IterativeCollideFunctor<Container> func(contacts_);
+ bool collision = false;
+ for( auto it1=a->begin(); it1!=a->end(); ++it1 )
+ {
+ for( auto it2=b->begin(); it2!=b->end(); ++it2 )
+ {
+ collision |= DoubleCast<BodyTupleA, BodyTupleB, IterativeCollideFunctor<Container>, bool>::execute(*it1, *it2, func);
+ }
+ }
+ return collision;
+ }
+
+ //Union and Plane (these calls are ambigous if not implemented seperatly)
+ template<typename BodyTupleA, typename Container>
+ inline bool generateContacts(Union<BodyTupleA> *a, Plane *b, Container& contacts_){
+ IterativeSingleCollideFunctor<Plane, Container> func(b, contacts_);
+ bool collision = false;
+ for( auto it=a->begin(); it!=a->end(); ++it )
+ {
+ collision |= SingleCast<BodyTupleA, IterativeSingleCollideFunctor<Plane, Container>, bool>::execute(*it, func);
+ }
+ return collision;
+ }
+
+ template<typename BodyTupleB, typename Container>
+ inline bool generateContacts(Plane *a, Union<BodyTupleB> *b, Container& contacts_){
+ return generateContacts(b, a, contacts_);
+ }
+
+
+} //namespace iterative
+
+
+} //fcd
+} //pe
+} //walberla
diff --git a/tests/pe/CollisionTobiasGJK.cpp b/tests/pe/CollisionTobiasGJK.cpp
index 3ce9c22fc..af02c7f7a 100644
--- a/tests/pe/CollisionTobiasGJK.cpp
+++ b/tests/pe/CollisionTobiasGJK.cpp
@@ -22,8 +22,10 @@
#include "pe/contact/Contact.h"
#include "pe/fcd/IFCD.h"
+#include "pe/fcd/GenericFCD.h"
#include "pe/fcd/IterativeFCD.h"
#include "pe/fcd/AnalyticCollisionDetection.h"
+#include "pe/fcd/IterativeCollideFunctor.h"
#include "pe/Materials.h"
#include "pe/rigidbody/Box.h"
@@ -47,7 +49,7 @@
using namespace walberla;
using namespace walberla::pe;
-typedef boost::tuple<Box, Capsule, Plane, Sphere, Union<boost::tuple<Sphere>>, Ellipsoid> BodyTuple ;
+typedef boost::tuple<Box, Capsule, Plane, Sphere, Union<boost::tuple<Sphere, Union<boost::tuple<Sphere>>>>, Ellipsoid> BodyTuple ;
bool gjkEPAcollideHybrid(GeomPrimitive &geom1, GeomPrimitive &geom2, Vec3& normal, Vec3& contactPoint, real_t& penetrationDepth)
{
@@ -286,24 +288,46 @@ void PlaneTest()
{
WALBERLA_LOG_INFO("PLANE AND INTERFACE TEST");
MaterialID iron = Material::find("iron");
- fcd::IterativeFCD<BodyTuple> testFCD;
+ fcd::GenericFCD<BodyTuple, fcd::IterativeCollideFunctor> testFCD;
Plane pl(1, 1, Vec3(0, 1, 0), Vec3(0, 1, 0), real_t(1.0), iron );
Sphere sphere(2, 2, Vec3(0, real_t(1.9), 0), Vec3(0,0,0), Quat(), 1, iron, false, true, false);
+ Sphere sphere2(3, 3, Vec3(0, real_t(0.1), 0), Vec3(0,0,0), Quat(), 1, iron, false, true, false);
PossibleContacts pcs;
- pcs.push_back(std::pair<Plane*, Sphere*>(&pl, &sphere));
+ pcs.push_back(std::pair<Sphere*, Sphere*>(&sphere, &sphere2));
Contacts& container = testFCD.generateContacts(pcs);
-
WALBERLA_CHECK(container.size() == 1);
Contact &c = container.back();
-
+ //
WALBERLA_LOG_DEVEL( c.getDistance() << " " << c.getNormal() << " " << c.getPosition() );
- checkContact( c, Contact(&pl, &sphere, Vec3(0, real_t(0.95), 0), Vec3(0, -1, 0), real_t(-0.1)), Vec3(0,0,0));
+ if(c.getBody1()->getID() == 2) {
+ checkContact( c, Contact(&sphere, &sphere2, Vec3(0, real_t(1), 0), Vec3(0, 1, 0), real_t(-0.2)), Vec3(0,0,0));
+ } else if (c.getBody1()->getID() == 3) {
+ checkContact( c, Contact(&sphere2, &sphere, Vec3(0, real_t(1), 0), Vec3(0, -1, 0), real_t(-0.2)), Vec3(0,0,0));
+ } else {
+ WALBERLA_ABORT("Unknown ID!");
+ }
+ pcs.clear();
+
+ pcs.push_back(std::pair<Plane*, Sphere*>(&pl, &sphere));
+ container = testFCD.generateContacts(pcs);
+ WALBERLA_CHECK(container.size() == 1);
+ c = container.back();
+ //
+ WALBERLA_LOG_DEVEL( c.getDistance() << " " << c.getNormal() << " " << c.getPosition() );
+ if(c.getBody1()->getID() == 1) {
+ checkContact( c, Contact(&pl, &sphere, Vec3(0, real_t(0.95), 0), Vec3(0, -1, 0), real_t(-0.1)), Vec3(0,0,0));
+ } else if (c.getBody1()->getID() == 2) {
+ checkContact( c, Contact(&sphere, &pl, Vec3(0, real_t(0.95), 0), Vec3(0, 1, 0), real_t(-0.1)), Vec3(0,0,0));
+ } else {
+ WALBERLA_ABORT("Unknown ID!");
+ }
pcs.clear();
+
pcs.push_back(std::pair<Sphere*, Plane*>(&sphere, &pl));
container = testFCD.generateContacts(pcs);
@@ -311,7 +335,13 @@ void PlaneTest()
c = container.back();
WALBERLA_LOG_DEVEL( c.getDistance() << " " << c.getNormal() << " " << c.getPosition() );
- checkContact( c, Contact(&sphere, &pl, Vec3(0, real_t(0.95), 0), Vec3(0, 1, 0), real_t(-0.1)), Vec3(0,0,0));
+ if(c.getBody1()->getID() == 1) {
+ checkContact( c, Contact(&pl, &sphere, Vec3(0, real_t(0.95), 0), Vec3(0, -1, 0), real_t(-0.1)), Vec3(0,0,0));
+ } else if (c.getBody1()->getID() == 2) {
+ checkContact( c, Contact(&sphere, &pl, Vec3(0, real_t(0.95), 0), Vec3(0, 1, 0), real_t(-0.1)), Vec3(0,0,0));
+ } else {
+ WALBERLA_ABORT("Unknown ID!");
+ }
}
/** Test the GJK-EPA implementation for a collision
@@ -319,48 +349,80 @@ void PlaneTest()
void UnionTest(){
WALBERLA_LOG_INFO("UNION AND INTERFACE TEST");
MaterialID iron = Material::find("iron");
- fcd::IterativeFCD<BodyTuple> testFCD;
+ fcd::GenericFCD<BodyTuple, fcd::IterativeCollideFunctor> testFCD;
- //A union of two spheres is dropped on a box.
+ //A recursive union of three spheres is dropped on a box.
Box box(179, 179, Vec3(0,0,0), Vec3(0,0,0), Quat(), Vec3(real_t(10),real_t(2), real_t(10)), iron, false, true, false);
- Union<boost::tuple<Sphere>> *un= new Union<boost::tuple<Sphere>>(182, 182, Vec3(0,real_t(3.8),0), Vec3(0,0,0), Quat(), false, true, false);
+ Union<boost::tuple<Sphere>> *unsub = new Union<boost::tuple<Sphere>>(192, 192, Vec3(0,real_t(3.8),0), Vec3(0,0,0), Quat(), false, true, false);
Sphere sp1( 180, 180, Vec3(-3,real_t(3.8),0), Vec3(0,0,0), Quat(), real_t(3.0) , iron, false, true, false );
Sphere sp2( 181, 181, Vec3(3,real_t(3.8),0), Vec3(0,0,0), Quat(), real_t(3.0), iron, false, true, false );
- un->add(&sp1);
- un->add(&sp2);
+ Sphere sp3( 182, 182, Vec3(0,real_t(6),0), Vec3(0,0,0), Quat(), real_t(3.0), iron, false, true, false );
+ unsub->add(&sp1);
+ unsub->add(&sp2);
+
+ //Create another union, and add sub union
+ Union<boost::tuple<Sphere, Union<boost::tuple<Sphere>>>> *un = new Union<boost::tuple<Sphere, Union<boost::tuple<Sphere>>>>(193, 193, Vec3(0, 0, 0), Vec3(0,0,0), Quat(), false, true, false);
+ un->add(&sp3);
+ un->add(unsub);
+
PossibleContacts pcs;
- pcs.push_back(std::pair<Union<boost::tuple<Sphere>>*, Box*>(un, &box));
+ pcs.push_back(std::pair<Union<boost::tuple<Sphere,Union<boost::tuple<Sphere>>>>*, Box*>(un, &box));
Contacts& container = testFCD.generateContacts(pcs);
-
WALBERLA_CHECK(container.size() == 2);
Contact &c = container.back();
+ WALBERLA_LOG_DEVEL( c.getDistance() << " " << c.getNormal() << " " << c.getPosition() );
+ if(c.getBody1()->getID() == 181) {
+ checkContact( c, Contact(&sp2, &box, Vec3(real_t(3), real_t(0.9), 0), Vec3(0, 1, 0), real_t(-0.2)), Vec3(0,0,0));
+ } else if (c.getBody1()->getID() == 179) {
+ checkContact( c, Contact(&box, &sp2, Vec3(real_t(3), real_t(0.9), 0), Vec3(0, -1, 0), real_t(-0.2)), Vec3(0,0,0));
+ } else {
+ WALBERLA_ABORT("Unknown ID!");
+ }
+ container.pop_back();
- checkContact( c, Contact(&sp2, &box, Vec3(real_t(3), real_t(0.9), 0), Vec3(0, 1, 0), real_t(-0.2)), Vec3(0,0,0));
- container.pop_back();
- Contact &c2 = container.back();
- checkContact( c2, Contact(&sp1, &box, Vec3(real_t(-3), real_t(0.9), 0), Vec3(0, 1, 0), real_t(-0.2)), Vec3(0,0,0));
+ c = container.back();
+ WALBERLA_LOG_DEVEL( c.getDistance() << " " << c.getNormal() << " " << c.getPosition() );
+ if(c.getBody1()->getID() == 180) {
+ checkContact( c, Contact(&sp1, &box, Vec3(real_t(-3), real_t(0.9), 0), Vec3(0, 1, 0), real_t(-0.2)), Vec3(0,0,0));
+ } else if (c.getBody1()->getID() == 179) {
+ checkContact( c, Contact(&box, &sp1, Vec3(real_t(-3), real_t(0.9), 0), Vec3(0, -1, 0), real_t(-0.2)), Vec3(0,0,0));
+ } else {
+ WALBERLA_ABORT("Unknown ID!");
+ }
pcs.clear();
//Vice Versa
- pcs.push_back(std::pair<Box*, Union<boost::tuple<Sphere>>* >(&box, un));
+ pcs.push_back(std::pair<Box*, Union<boost::tuple<Sphere, Union<boost::tuple<Sphere>>>>* >(&box, un));
container = testFCD.generateContacts(pcs);
-
WALBERLA_CHECK(container.size() == 2);
- c = container.back();
-
- checkContact( c, Contact(&box, &sp2, Vec3(real_t(3), real_t(0.9), 0), Vec3(0, -1, 0), real_t(-0.2)), Vec3(0,0,0));
+ c = container.back();
+ WALBERLA_LOG_DEVEL( c.getDistance() << " " << c.getNormal() << " " << c.getPosition() );
+ if(c.getBody1()->getID() == 181) {
+ checkContact( c, Contact(&sp2, &box, Vec3(real_t(3), real_t(0.9), 0), Vec3(0, 1, 0), real_t(-0.2)), Vec3(0,0,0));
+ } else if (c.getBody1()->getID() == 179) {
+ checkContact( c, Contact(&box, &sp2, Vec3(real_t(3), real_t(0.9), 0), Vec3(0, -1, 0), real_t(-0.2)), Vec3(0,0,0));
+ } else {
+ WALBERLA_ABORT("Unknown ID!");
+ }
container.pop_back();
- c2 = container.back();
- checkContact( c2, Contact(&box, &sp1, Vec3(real_t(-3), real_t(0.9), 0), Vec3(0, -1, 0), real_t(-0.2)), Vec3(0,0,0));
+ c = container.back();
+ WALBERLA_LOG_DEVEL( c.getDistance() << " " << c.getNormal() << " " << c.getPosition() );
+ if(c.getBody1()->getID() == 180) {
+ checkContact( c, Contact(&sp1, &box, Vec3(real_t(-3), real_t(0.9), 0), Vec3(0, 1, 0), real_t(-0.2)), Vec3(0,0,0));
+ } else if (c.getBody1()->getID() == 179) {
+ checkContact( c, Contact(&box, &sp1, Vec3(real_t(-3), real_t(0.9), 0), Vec3(0, -1, 0), real_t(-0.2)), Vec3(0,0,0));
+ } else {
+ WALBERLA_ABORT("Unknown ID!");
+ }
pcs.clear();
}
--
GitLab