Commit 030b72a8 by Tobias Leemann

### Implemented Iterative Collide Functor.

parent 3cd28eca
 //====================================================================================================================== // // 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 . // //! \file IterativeCollideFunctor.h //! \author Tobias Leemann //! \author Sebastian Eibl // //====================================================================================================================== #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 #include namespace walberla{ namespace pe{ namespace fcd { namespace iterative{ //function for all single rigid bodies. template inline bool generateContacts(GeomPrimitive *a, GeomPrimitive *b, Container& contacts_); //Planes template inline bool generateContacts(Plane *a, GeomPrimitive *b, Container& contacts_); template inline bool generateContacts(GeomPrimitive *a, Plane *b, Container& contacts_); template< typename Container> inline bool generateContacts(Plane *a, Plane *b, Container& contacts_); //Unions template inline bool generateContacts(Union *a, BodyB *b, Container& contacts_); template inline bool generateContacts(BodyA *a, Union *b, Container& contacts_); template inline bool generateContacts(Union *a, Union *b, Container& contacts_); //Union and Plane template inline bool generateContacts(Union *a, Plane *b, Container& contacts_); template inline bool generateContacts(Plane *a, Union *b, Container& contacts_); } /* Iterative Collide Functor for contact Generation with iterative collision detection. * Usage: fcd::GenericFCD testFCD; * testFCD.generateContacts(...); */ template 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 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 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 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 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 inline bool generateContacts(Union *a, BodyB *b, Container& contacts_){ IterativeSingleCollideFunctor func(b, contacts_); bool collision = false; for( auto it=a->begin(); it!=a->end(); ++it ) { collision |= SingleCast, bool>::execute(*it, func); } return collision; } template inline bool generateContacts(BodyA *a, Union *b, Container& contacts_){ return generateContacts(b, a, contacts_); } template inline bool generateContacts(Union *a, Union *b, Container& contacts_){ IterativeCollideFunctor 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, bool>::execute(*it1, *it2, func); } } return collision; } //Union and Plane (these calls are ambigous if not implemented seperatly) template inline bool generateContacts(Union *a, Plane *b, Container& contacts_){ IterativeSingleCollideFunctor func(b, contacts_); bool collision = false; for( auto it=a->begin(); it!=a->end(); ++it ) { collision |= SingleCast, bool>::execute(*it, func); } return collision; } template inline bool generateContacts(Plane *a, Union *b, Container& contacts_){ return generateContacts(b, a, contacts_); } } //namespace iterative } //fcd } //pe } //walberla
 ... ... @@ -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>, Ellipsoid> BodyTuple ; typedef boost::tuple>>>, 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 testFCD; fcd::GenericFCD 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(&pl, &sphere)); pcs.push_back(std::pair(&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(&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, &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 testFCD; fcd::GenericFCD 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> *un= new Union>(182, 182, Vec3(0,real_t(3.8),0), Vec3(0,0,0), Quat(), false, true, false); Union> *unsub = new Union>(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>>> *un = new Union>>>(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>*, Box*>(un, &box)); pcs.push_back(std::pair>>>*, 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, un)); pcs.push_back(std::pair>>>* >(&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(); } ... ...
Markdown is supported
0% or
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment