Commit aeeaac92 authored by Sebastian Eibl's avatar Sebastian Eibl

Merge branch 'improvedgjkepa' into 'master'

Improved iterative collision detection with GJK-EPA

See merge request walberla/walberla!62
parents e1acd858 077b3708
Pipeline #6028 passed with stages
in 326 minutes and 56 seconds
......@@ -65,6 +65,7 @@ class Box;
class Capsule;
class Contact;
class Cylinder;
class Ellipsoid;
class CylindricalBoundary;
class FixedJoint;
class ForceGenerator;
......@@ -111,6 +112,10 @@ typedef CylindricalBoundary CylindricalBoundaryType; //!< Type of
typedef CylindricalBoundary* CylindricalBoundaryID; //!< Handle for a cylindrical boundary primitive.
typedef const CylindricalBoundary* ConstCylindricalBoundaryID; //!< Handle for a constant cylindrical boundary primitive.
typedef Ellipsoid EllipsoidType; //!< Type of the ellipsoid geometric primitive.
typedef Ellipsoid* EllipsoidID; //!< Handle for a ellipsoid primitive.
typedef const Ellipsoid* ConstEllipsoidID; //!< Handle for a constant ellipsoid primitive.
typedef Plane PlaneType; //!< Type of the plane geometric primitive.
typedef Plane* PlaneID; //!< Handle for a plane primitive.
typedef const Plane* ConstPlaneID; //!< Handle for a constant plane primitive.
......
......@@ -44,6 +44,7 @@
#include "pe/rigidbody/PlaneFactory.h"
#include "pe/rigidbody/SphereFactory.h"
#include "pe/rigidbody/UnionFactory.h"
#include "pe/rigidbody/EllipsoidFactory.h"
#include "pe/synchronization/SyncNextNeighbors.h"
#include "pe/synchronization/SyncShadowOwners.h"
......
This diff is collapsed.
This diff is collapsed.
This diff is collapsed.
//======================================================================================================================
//
// 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 GJK.h
//! \author Tobias Scharpff
//! \author Tobias Leemann
//
//======================================================================================================================
#pragma once
//*************************************************************************************************
// Includes
//*************************************************************************************************
#include <vector>
#include <pe/rigidbody/GeomPrimitive.h>
#include <pe/Thresholds.h>
#include <core/Abort.h>
#include <core/math/Limits.h>
#include <core/math/Vector3.h>
namespace walberla {
namespace pe {
namespace fcd {
//=================================================================================================
//
// CLASS DEFINITION
//
//=================================================================================================
//*************************************************************************************************
/*!\brief Impelementation of the Gilbert-Johnson-Keerthi Algorithm.
*/
class GJK
{
public:
//**Constructor*********************************************************************************
/*! \name Constructor */
//@{
explicit inline GJK();
//@}
//**********************************************************************************************
//**Query functions*****************************************************************************
/*! \name Query functions */
//@{
real_t doGJK( GeomPrimitive &geom1, GeomPrimitive &geom2, Vec3& normal, Vec3& contactPoint );
bool doGJKmargin( GeomPrimitive &geom1, GeomPrimitive &geom2, const real_t margin = contactThreshold);
//@}
//**********************************************************************************************
//**Get functions*******************************************************************************
/*! \name Get functions */
//@{
inline const std::vector<Vec3>& getSimplex() const;
inline size_t getSimplexSize() const;
inline const std::vector<Vec3>& getSupportA() const;
inline const std::vector<Vec3>& getSupportB() const;
//@}
//**********************************************************************************************
private:
//**Utility functions***************************************************************************
/*! \name Utility functions */
//@{
bool simplex2(Vec3& d);
bool simplex3(Vec3& d);
bool simplex4(Vec3& d);
inline bool sameDirection ( const Vec3& vec1, const Vec3& vec2 ) const;
inline bool zeroLengthVector( const Vec3& vec ) const;
real_t calcDistance ( Vec3& normal, Vec3& contactPoint );
inline const Vec3 putSupport(const GeomPrimitive &geom1, const GeomPrimitive &geom2, const Vec3& dir, const real_t margin,
std::vector<Vec3> &simplex, std::vector<Vec3> &supportA, std::vector<Vec3> &supportB, size_t index);
//@}
//**********************************************************************************************
//**Member variables****************************************************************************
/*! \name Member variables */
//@{
std::vector<Vec3> simplex_; //<! Container to hold the simplex.
std::vector<Vec3> supportA_; //<! Container to hold the support points generated in triangle mesh mA
std::vector<Vec3> supportB_; //<! Container to hold the support points generated in triangle mesh mB
unsigned char numPoints_; //<! Current number of points in the simplex.
Vec3 d_; //<! The next search direction.
//@}
//**********************************************************************************************
};
//*************************************************************************************************
//=================================================================================================
//
// CONSTRUCTOR
//
//=================================================================================================
//*************************************************************************************************
inline GJK::GJK() : simplex_(4), supportA_(4), supportB_(4), numPoints_(0)
{
d_ = Vec3(real_t(0.0),real_t(0.6),real_t(0.8)); // just start with any vector of length 1
}
//*************************************************************************************************
//=================================================================================================
//
// GET FUNCTIONS
//
//=================================================================================================
//*************************************************************************************************
inline const std::vector<Vec3>& GJK::getSimplex() const
{
return simplex_;
}
//*************************************************************************************************
//*************************************************************************************************
inline size_t GJK::getSimplexSize() const
{
return numPoints_;
}
//*************************************************************************************************
//*************************************************************************************************
inline const std::vector<Vec3>& GJK::getSupportA() const
{
return supportA_;
}
//*************************************************************************************************
//*************************************************************************************************
inline const std::vector<Vec3>& GJK::getSupportB() const
{
return supportB_;
}
//*************************************************************************************************
//=================================================================================================
//
// UTILITY FUNCTIONS
//
//=================================================================================================
//*************************************************************************************************
/*!\brief Checks if two vectors roughly point in the same directionTODO
*/
inline bool GJK::sameDirection(const Vec3& vec1, const Vec3& vec2) const
{
return vec1 * vec2 > real_t(0.0);
}
//*************************************************************************************************
//*************************************************************************************************
/* Checks if the length of a vector is zero or as close to zero that it can not be distinguished form zero
*/
inline bool GJK::zeroLengthVector(const Vec3& vec) const
{
return vec.sqrLength() < math::Limits<real_t>::fpuAccuracy();
}
//*************************************************************************************************
//*************************************************************************************************
/*!\brief Calucate a support point of a body extended by threshold.
* \param geom The body.
* \param dir The support point direction.
* \param threshold Extension of the Body.
*/
inline const Vec3 GJK::putSupport(const GeomPrimitive &geom1, const GeomPrimitive &geom2, const Vec3& dir, const real_t margin,
std::vector<Vec3> &simplex, std::vector<Vec3> &supportA, std::vector<Vec3> &supportB, size_t index){
supportA[index] = geom1.support(dir);
supportB[index] = geom2.support(-dir);
Vec3 supp = supportA[index]- supportB[index] + (real_t(2.0) * dir * margin);
simplex[index] = supp;
return supp;
}
//*************************************************************************************************
} // namespace fcd
} // namespace pe
} // namespace walberla
......@@ -36,7 +36,7 @@ extern "C" {
namespace walberla {
namespace pe {
Vec3 convertVec3(const ccd_vec3_t& vec) { return Vec3(vec.v[0], vec.v[1], vec.v[2]); }
Vec3 convertVec3(const ccd_vec3_t& vec) { return Vec3(real_c(vec.v[0]), real_c(vec.v[1]), real_c(vec.v[2])); }
ccd_vec3_t convertVec3(const Vec3& vec) { ccd_vec3_t ret; ret.v[0] = vec[0]; ret.v[1] = vec[1]; ret.v[2] = vec[2]; return ret; }
void support(const void *obj, const ccd_vec3_t *dir, ccd_vec3_t *vec)
......@@ -65,7 +65,9 @@ bool collideGJK( ConstGeomID bd1,
ccd.epa_tolerance = epaTol;
ccd_vec3_t dir, pos;
int intersect = ccdGJKPenetration(reinterpret_cast<const void*> (bd1), reinterpret_cast<const void*> (bd2), &ccd, &penetrationDepth, &dir, &pos);
ccd_real_t penetrationDepthCCD;
int intersect = ccdGJKPenetration(reinterpret_cast<const void*> (bd1), reinterpret_cast<const void*> (bd2), &ccd, &penetrationDepthCCD, &dir, &pos);
penetrationDepth = real_c(penetrationDepthCCD);
contactPoint = convertVec3(pos);
contactNormal = -convertVec3(dir);
penetrationDepth *= -1;
......
......@@ -33,6 +33,7 @@
#include "pe/communication/rigidbody/Capsule.h"
#include "pe/communication/rigidbody/Sphere.h"
#include "pe/communication/rigidbody/Union.h"
#include "pe/communication/rigidbody/Ellipsoid.h"
#include "pe/utility/BodyCast.h"
#include "core/Abort.h"
......
//======================================================================================================================
//
// 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 Ellipsoid.cpp
//! \author Tobias Preclik
//! \author Sebastian Eibl <sebastian.eibl@fau.de>
//! \brief Marshalling of objects for data transmission or storage.
//
//======================================================================================================================
#include "Ellipsoid.h"
namespace walberla {
namespace pe {
namespace communication {
//*************************************************************************************************
/*!\brief Marshalling a Ellipsoid primitive.
*
* \param buffer The buffer to be filled.
* \param obj The object to be marshalled.
* \return void
*/
void marshal( mpi::SendBuffer& buffer, const Ellipsoid& obj ) {
marshal( buffer, static_cast<const GeomPrimitive&>( obj ) );
buffer << obj.getSemiAxes();
}
//*************************************************************************************************
//*************************************************************************************************
/*!\brief Unmarshalling a Ellipsoid primitive.
*
* \param buffer The buffer from where to read.
* \param objparam The object to be reconstructed.
* \param hasSuperBody False if body is not part of a union. Passed on to rigid body unmarshalling.
* \return void
*/
void unmarshal( mpi::RecvBuffer& buffer, EllipsoidParameters& objparam ) {
unmarshal( buffer, static_cast<GeomPrimitiveParameters&>( objparam ) );
buffer >> objparam.semiAxes_;
}
//*************************************************************************************************
} // namespace communication
} // namespace pe
} // namespace walberla
//======================================================================================================================
//
// 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 Ellipsoid.h
//! \author Tobias Preclik
//! \author Sebastian Eibl <sebastian.eibl@fau.de>
//! \brief Marshalling of objects for data transmission or storage.
//
//======================================================================================================================
#pragma once
//*************************************************************************************************
// Includes
//*************************************************************************************************
#include "pe/communication/Instantiate.h"
#include "pe/communication/Marshalling.h"
#include "pe/rigidbody/Ellipsoid.h"
namespace walberla {
namespace pe {
namespace communication {
struct EllipsoidParameters : public GeomPrimitiveParameters {
Vec3 semiAxes_;
};
//*************************************************************************************************
/*!\brief Marshalling a Ellipsoid primitive.
*
* \param buffer The buffer to be filled.
* \param obj The object to be marshalled.
* \return void
*/
void marshal( mpi::SendBuffer& buffer, const Ellipsoid& obj );
//*************************************************************************************************
//*************************************************************************************************
/*!\brief Unmarshalling a Ellipsoid primitive.
*
* \param buffer The buffer from where to read.
* \param objparam The object to be reconstructed.
* \param hasSuperBody False if body is not part of a union. Passed on to rigid body unmarshalling.
* \return void
*/
void unmarshal( mpi::RecvBuffer& buffer, EllipsoidParameters& objparam );
//*************************************************************************************************
inline EllipsoidID instantiate( mpi::RecvBuffer& buffer, const math::AABB& domain, const math::AABB& block, EllipsoidID& newBody )
{
EllipsoidParameters subobjparam;
unmarshal( buffer, subobjparam );
correctBodyPosition(domain, block.center(), subobjparam.gpos_);
newBody = new Ellipsoid( subobjparam.sid_, subobjparam.uid_, subobjparam.gpos_, subobjparam.rpos_, subobjparam.q_, subobjparam.semiAxes_, subobjparam.material_, false, subobjparam.communicating_, subobjparam.infiniteMass_ );
newBody->setLinearVel( subobjparam.v_ );
newBody->setAngularVel( subobjparam.w_ );
newBody->MPITrait.setOwner( subobjparam.mpiTrait_.owner_ );
return newBody;
}
} // namespace communication
} // namespace pe
} // namespace walberla
//======================================================================================================================
//
// 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 GJKEPACollideFunctor.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 gjkepa{
//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 (GJK and EPA algorithms).
* Usage: fcd::GenericFCD<BodyTuple, fcd::GJKEPACollideFunctor> testFCD;
* testFCD.generateContacts(...);
*/
template <typename Container>
struct GJKEPACollideFunctor
{
Container& contacts_;
GJKEPACollideFunctor(Container& contacts) : contacts_(contacts) {}
template< typename BodyType1, typename BodyType2 >
bool operator()( BodyType1* bd1, BodyType2* bd2) {
using namespace gjkepa;
return generateContacts(bd1, bd2, contacts_);
}
};
template <typename BodyType1, typename Container>
struct GJKEPASingleCollideFunctor
{
BodyType1* bd1_;
Container& contacts_;
GJKEPASingleCollideFunctor(BodyType1* bd1, Container& contacts) : bd1_(bd1), contacts_(contacts) {}
template< typename BodyType2 >
bool operator()( BodyType2* bd2) {
using namespace gjkepa;
return generateContacts( bd1_, bd2, contacts_);
}
};
namespace gjkepa{
//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_){
GJKEPASingleCollideFunctor<BodyB, Container> func(b, contacts_);
bool collision = false;
for( auto it=a->begin(); it!=a->end(); ++it )
{
collision |= SingleCast<BodyTupleA, GJKEPASingleCollideFunctor<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_){
GJKEPACollideFunctor<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, GJKEPACollideFunctor<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_){
GJKEPASingleCollideFunctor<Plane, Container> func(b, contacts_);
bool collision = false;
for( auto it=a->begin(); it!=a->end(); ++it )
{
collision |= SingleCast<BodyTupleA, GJKEPASingleCollideFunctor<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 gjkepa
} //fcd
} //pe
} //walberla
......@@ -83,6 +83,7 @@ public:
/*!\name Get functions */
//@{
inline const Vec3& getLengths() const;
virtual inline real_t getVolume() const;
//@}
//**********************************************************************************************
......@@ -306,7 +307,16 @@ inline const Vec3& Box::getLengths() const
}
//*************************************************************************************************
//*************************************************************************************************
/*!\brief Returns the volume of the box.
*
* \return The volume of the box.
*/
inline real_t Box::getVolume() const
{
return Box::calcVolume(getLengths());
}
//*************************************************************************************************
//=================================================================================================
......
This diff is collapsed.
This diff is collapsed.
//======================================================================================================================
//
// 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 EllipsoidFactory.cpp
//! \author Tobias Leemann <tobias.leemann@fau.de>
//! \author Sebastian Eibl <sebastian.eibl@fau.de>
//
//======================================================================================================================
#include "EllipsoidFactory.h"
#include "pe/Materials.h"
#include "pe/rigidbody/BodyStorage.h"
#include "pe/rigidbody/Ellipsoid.h"
namespace walberla {
namespace pe {
EllipsoidID createEllipsoid( BodyStorage& globalStorage, BlockStorage& blocks, BlockDataID storageID,
id_t uid, const Vec3& gpos, const Vec3& semiAxes,
MaterialID material,
bool global, bool communicating, bool infiniteMass )
{
WALBERLA_ASSERT_UNEQUAL( Ellipsoid::getStaticTypeID(), std::numeric_limits<id_t>::max(), "Ellipsoid TypeID not initalized!");
// Checking the semiAxes
if( semiAxes[0] <= real_c(0) || semiAxes[1] <= real_c(0) || semiAxes[2] <= real_c(0) )
throw std::invalid_argument( "Invalid Ellipsoid semi-axes" );
EllipsoidID ellipsoid = NULL;
if (global)
{
const id_t sid = UniqueID<RigidBody>::createGlobal();
WALBERLA_ASSERT_EQUAL(communicating, false);
WALBERLA_ASSERT_EQUAL(infiniteMass, true);
ellipsoid = new Ellipsoid(sid, uid, gpos, Vec3(0,0,0), Quat(), semiAxes, material, global, false, true);
globalStorage.add(ellipsoid);
} else
{
for (auto it = blocks.begin(); it != blocks.end(); ++it){
IBlock* block = (&(*it));
if (block->getAABB().contains(gpos))
{
const id_t sid( UniqueID<RigidBody>::create() );
Storage* bs = block->getData<Storage>(storageID);
ellipsoid = new Ellipsoid(sid, uid, gpos, Vec3(0,0,0), Quat(), semiAxes, material, global,