Commit 030b72a8 authored by Tobias Leemann's avatar 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 <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
......@@ -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();
}
......
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