diff --git a/src/core/ptrvector/Algorithm.h b/src/core/ptrvector/Algorithm.h
deleted file mode 100644
index c00063d702e55c804b1b02ceadf44845a1ff1069..0000000000000000000000000000000000000000
--- a/src/core/ptrvector/Algorithm.h
+++ /dev/null
@@ -1,95 +0,0 @@
-//======================================================================================================================
-//
-//  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 Algorithm.h
-//! \ingroup core
-//! \author Klaus Iglberger
-//! \author Sebastian Eibl <sebastian.eibl@fau.de>
-//
-//======================================================================================================================
-
-#pragma once
-
-
-//*************************************************************************************************
-// Includes
-//*************************************************************************************************
-
-#include <boost/type_traits/is_base_of.hpp>
-
-namespace walberla {
-
-//=================================================================================================
-//
-//  POLYMORPHIC COUNT
-//
-//=================================================================================================
-
-//*************************************************************************************************
-/*!\brief Counts the pointer to objects with dynamic type \a D.
- *
- * \param first Iterator to the first pointer of the pointer range.
- * \param last Iterator to the pointer one past the last pointer of the pointer range.
- * \return The number of objects with dynamic type \a D.
- *
- * This function traverses the range \f$ [first,last) \f$ of pointers to objects with static
- * type \a S and counts all polymorphic pointers to objects of dynamic type \a D. Note that
- * in case \a D is not a type derived from \a S, a compile time error is created!
- */
-template< typename D    // Dynamic type of the objects
-        , typename S >  // Static type of the objects
-inline size_t polymorphicCount( S *const * first, S *const * last )
-{
-   static_assert(boost::is_base_of<S, D>::value && !boost::is_base_of<D, S>::value, "D has to be strictly derived from S");
-
-   size_t count( 0 );
-   for( S *const * it=first; it!=last; ++it )
-      if( dynamic_cast<D*>( *it ) ) ++count;
-   return count;
-}
-//*************************************************************************************************
-
-
-
-
-//=================================================================================================
-//
-//  POLYMORPHIC FIND
-//
-//=================================================================================================
-
-//*************************************************************************************************
-/*!\brief Finds the next pointer to an object with dynamic type \a D.
- *
- * \param first Iterator to the first pointer of the pointer range.
- * \param last Iterator to the pointer one past the last pointer of the pointer range.
- * \return The next pointer to an object with dynamic type \a D.
- *
- * This function traverses the range \f$ [first,last) \f$ of pointers to objects with static
- * type \a S until it finds the next polymorphic pointer to an object of dynamic type \a D.
- * Note that in case \a D is not a type derived from \a S, a compile time error is created!
- */
-template< typename D    // Dynamic type of the objects
-        , typename S >  // Static type of the objects
-inline S *const * polymorphicFind( S *const * first, S *const * last )
-{
-   static_assert(boost::is_base_of<S, D>::value && !boost::is_base_of<D, S>::value, "D has to be strictly derived from S");
-
-   while( first != last && !dynamic_cast<D*>( *first ) ) ++first;
-   return first;
-}
-//*************************************************************************************************
-
-} // namespace
diff --git a/src/core/ptrvector/Null.h b/src/core/ptrvector/Null.h
deleted file mode 100644
index 0f08813a0244eacfe1f82e9f9249308e924c932c..0000000000000000000000000000000000000000
--- a/src/core/ptrvector/Null.h
+++ /dev/null
@@ -1,289 +0,0 @@
-//======================================================================================================================
-//
-//  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 Null.h
-//! \ingroup core
-//! \author Klaus Iglberger
-//! \author Sebastian Eibl <sebastian.eibl@fau.de>
-//
-//======================================================================================================================
-
-#pragma once
-
-namespace walberla {
-
-//=================================================================================================
-//
-//  CLASS DEFINITION
-//
-//=================================================================================================
-
-//*************************************************************************************************
-/*!\brief Safe C++ NULL pointer implementation.
- * \ingroup util
- *
- * This implementation offers a remedy for the use of the NULL pointer in C++. For this, the
- * NULL macro is replaced by an instance of the Null class, which can only be assigned and
- * compared with pointers and pointers-to-member. Therefore the use of NULL regains the type
- * safety it lost in C++ due to the strict C++ type system.\n
- * The NULL pointer is used exactly as before:
-
-   \code
-   int* pi = NULL;
-   if( pi == NULL ) {...}
-   \endcode
- */
-class Null
-{
-public:
-   //**Constructor*********************************************************************************
-   /*!\name Constructor */
-   //@{
-   inline Null();
-   //@}
-   //**********************************************************************************************
-
-   //**Destructor**********************************************************************************
-   // No explicitly declared destructor.
-   //**********************************************************************************************
-
-   //**Conversion operators************************************************************************
-   /*!\name Conversion operators */
-   //@{
-   template< typename T >
-   inline operator T*() const;
-
-   template< typename T, typename C >
-   inline operator T C::*() const;
-   //@}
-   //**********************************************************************************************
-
-   //**Utility functions***************************************************************************
-   /*!\name Utility functions */
-   //@{
-   template< typename T >
-   inline bool equal( const T* rhs ) const;
-
-   template< typename T, typename C >
-   inline bool equal( const T C::* rhs ) const;
-   //@}
-   //**********************************************************************************************
-
-private:
-   //**Forbidden operations************************************************************************
-   /*!\name Forbidden operations */
-   //@{
-   Null( const Null& n );             //!< Copy constructor (private & undefined)
-   Null& operator=( const Null& n );  //!< Copy assignment operator (private & undefined)
-   void* operator&() const;           //!< Address operator (private & undefined)
-   //@}
-   //**********************************************************************************************
-};
-//*************************************************************************************************
-
-
-
-
-//=================================================================================================
-//
-//  CONSTRUCTOR
-//
-//=================================================================================================
-
-//*************************************************************************************************
-/*!\brief The default constructor of the Null class.
- */
-inline Null::Null()
-{}
-//*************************************************************************************************
-
-
-
-
-//=================================================================================================
-//
-//  CONVERSION OPERATORS
-//
-//=================================================================================================
-
-//*************************************************************************************************
-/*!\brief Conversion operator to a pointer.
- *
- * This conversion operator offers a type safe conversion of zero to a pointer of any kind.
- */
-template< typename T >
-inline Null::operator T*() const
-{
-   return 0;
-}
-//*************************************************************************************************
-
-
-//*************************************************************************************************
-/*!\brief Conversion operator to a pointer-to-member.
- *
- * This conversion operator offers the type safe conversion of zero to a pointer-to-member of
- * any kind.
- */
-template< typename T, typename C >
-inline Null::operator T C::*() const
-{
-   return 0;
-}
-//*************************************************************************************************
-
-
-
-
-//=================================================================================================
-//
-//  UTILITY FUNCTIONS
-//
-//=================================================================================================
-
-//*************************************************************************************************
-/*!\brief Comparison between Null and a pointer.
- *
- * The function offers a type safe comparison between zero and an arbitrary pointer.
- */
-template< typename T >
-inline bool Null::equal( const T* rhs ) const
-{
-   return rhs == 0;
-}
-//*************************************************************************************************
-
-
-//*************************************************************************************************
-/*!\brief Comparison between Null and a pointer-to-member.
- *
- * The function offers a type safe comparison between zero and an arbitrary pointer-to-member.
- */
-template< typename T, typename C >
-inline bool Null::equal( const T C::* rhs ) const
-{
-   return rhs == 0;
-}
-//*************************************************************************************************
-
-
-
-
-//=================================================================================================
-//
-//  GLOBAL OPERATORS
-//
-//=================================================================================================
-
-//*************************************************************************************************
-/*!\name Null operators */
-//@{
-template< typename T >
-inline bool operator==( const Null& lhs, const T& rhs );
-
-template< typename T >
-inline bool operator==( const T& lhs, const Null& rhs );
-
-template< typename T >
-inline bool operator!=( const Null& lhs, const T& rhs );
-
-template< typename T >
-inline bool operator!=( const T& lhs, const Null& rhs );
-//@}
-//*************************************************************************************************
-
-
-//*************************************************************************************************
-/*!\brief Equality comparison between Null and a pointer or pointer-to-member.
- *
- * This operator takes a reference to an object of type T instead of a pointer of a pointer-
- * to-member to avoid the ambiguity with the built-in pointer comparison operators. However,
- * only pointers and pointers-to-member can be compared to Null.
- */
-template< typename T >
-inline bool operator==( const Null& lhs, const T& rhs )
-{
-   return lhs.equal( rhs );
-}
-//*************************************************************************************************
-
-
-//*************************************************************************************************
-/*!\brief Equality comparison between a pointer or pointer-to-member and Null.
- *
- * This operator takes a reference to an object of type T instead of a pointer of a pointer-
- * to-member to avoid the ambiguity with the built-in pointer comparison operators. However,
- * only pointers and pointers-to-member can be compared to Null.
- */
-template< typename T >
-inline bool operator==( const T& lhs, const Null& rhs )
-{
-   return rhs.equal( lhs );
-}
-//*************************************************************************************************
-
-
-//*************************************************************************************************
-/*!\brief Inequality comparison between Null and a pointer or pointer-to-member.
- *
- * This operator takes a reference to an object of type T instead of a pointer of a pointer-
- * to-member to avoid the ambiguity with the built-in pointer comparison operators. However,
- * only pointers and pointers-to-member can be compared to Null.
- */
-template< typename T >
-inline bool operator!=( const Null& lhs, const T& rhs )
-{
-   return !lhs.equal( rhs );
-}
-//*************************************************************************************************
-
-
-//*************************************************************************************************
-/*!\brief Inequality comparison between a pointer or pointer-to-member and Null.
- *
- * This operator takes a reference to an object of type T instead of a pointer of a pointer-
- * to-member to avoid the ambiguity with the built-in pointer comparison operators. However,
- * only pointers and pointers-to-member can be compared to Null.
- */
-template< typename T >
-inline bool operator!=( const T& lhs, const Null& rhs )
-{
-   return !rhs.equal( lhs );
-}
-//*************************************************************************************************
-
-} // namespace
-
-
-
-
-////=================================================================================================
-////
-////  NULL DEFINITION
-////
-////=================================================================================================
-
-//#ifdef NULL
-//#  undef NULL
-//#endif
-
-////*************************************************************************************************
-///*!\brief Global NULL pointer.
-// * \ingroup util
-// *
-// * This instance of the Null class replaces the NULL macro to ensure a type-safe NULL pointer.
-// */
-//const walberla::Null NULL;
-////*************************************************************************************************
diff --git a/src/core/ptrvector/PtrIterator.h b/src/core/ptrvector/PtrIterator.h
deleted file mode 100644
index a6ecef7f3b6194ead74e78f2b52e030dd3c9b704..0000000000000000000000000000000000000000
--- a/src/core/ptrvector/PtrIterator.h
+++ /dev/null
@@ -1,548 +0,0 @@
-//======================================================================================================================
-//
-//  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 PtrIterator.h
-//! \ingroup core
-//! \author Klaus Iglberger
-//! \author Sebastian Eibl <sebastian.eibl@fau.de>
-//
-//======================================================================================================================
-
-#pragma once
-
-
-//*************************************************************************************************
-// Includes
-//*************************************************************************************************
-
-#include <iterator>
-#include <core/ptrvector/Null.h>
-
-namespace walberla {
-
-//=================================================================================================
-//
-//  CLASS DEFINITION
-//
-//=================================================================================================
-
-//*************************************************************************************************
-/*!\brief Implementation of an iterator for pointer vectors.
- * \ingroup util
- *
- * The PtrIterator class follows the example of the random-access iterator classes of the STL.
- * However, the focus of this iterator implementation is the use with (polymorphic) pointers.
- * Since the physics engine makes heavy use of polymorphic pointers, this implementation eases
- * the use of iterators over a range of pointers and improves the semantics on these pointers.\n
- *
- * In contrast to the STL iterators, the PtrIterator class slightly changes the meaning of the
- * access operators. Consider the following example:
-
-   \code
-   // Definition of class A
-   class A
-   {
-    public:
-      A( int i=0 ):i_(i) {}
-
-      void set( int i )       { i_ = i; }
-      int  get()        const { return i_; }
-
-    private:
-      int i_;
-   };
-
-   // Definition of a pointer vector for class A
-   typedef PtrVector<A>  AVector;
-
-   AVector vector;
-   AVector::Iterator it = vector.begin();
-
-   // The subscript operator returns a handle to the underlying object
-   A* a1 = it[0];
-
-   // The dereference operator returns a handle to the underlying object
-   A* a2 = *it;
-
-   // The member access operator offers direct access to the underlying object
-   it->set( 2 );
-   \endcode
-
- * The constant iterators (iterator over constant objects) prohibit the access to non-const
- * member functions. Therefore the following operation results in a compile-time error:
-
-   \code
-   AVector vector;
-   AVector::ConstIterator it = vector.begin();
-
-   it->set( 2 );  // Compile-time error!
-   \endcode
- */
-template< typename Type >
-class PtrIterator
-{
-public:
-   //**Type definitions****************************************************************************
-   // pe naming convention
-   typedef std::random_access_iterator_tag  IteratorCategory;   //!< The iterator category.
-   typedef Type*                            ValueType;          //!< Type of the underlying pointers.
-   typedef Type* const                      PointerType;        //!< Pointer return type.
-   typedef ValueType const&                 ReferenceType;      //!< Reference return type.
-   typedef ValueType const*                 IteratorType;       //!< Type of the internal pointer.
-   typedef std::ptrdiff_t                   DifferenceType;     //!< Difference between two iterators.
-
-   // STL iterator requirements
-   typedef IteratorCategory                 iterator_category;  //!< The iterator category.
-   typedef ValueType                        value_type;         //!< Type of the underlying pointers.
-   typedef PointerType                      pointer;            //!< Pointer return type.
-   typedef ReferenceType                    reference;          //!< Reference return type.
-   typedef DifferenceType                   difference_type;    //!< Difference between two iterators.
-   //**********************************************************************************************
-
-   //**Constructors********************************************************************************
-   /*!\name Constructors */
-   //@{
-            inline PtrIterator();
-   explicit inline PtrIterator( const IteratorType& it );
-
-   template< typename Other >
-   inline PtrIterator( const PtrIterator<Other>& it );
-
-   // No explicitly declared copy constructor.
-   //@}
-   //**********************************************************************************************
-
-   //**Destructor**********************************************************************************
-   // No explicitly declared destructor.
-   //**********************************************************************************************
-
-   //**Copy assignment operator********************************************************************
-   // No explicitly declared copy assignment operator.
-   //**********************************************************************************************
-
-   //**Operators***********************************************************************************
-   /*!\name Operators */
-   //@{
-   inline PtrIterator&   operator++();
-   inline PtrIterator    operator++( int );
-   inline PtrIterator&   operator--();
-   inline PtrIterator    operator--( int );
-   inline PtrIterator&   operator+=( DifferenceType n );
-   inline PtrIterator    operator+ ( DifferenceType n )      const;
-   inline PtrIterator&   operator-=( DifferenceType n );
-   inline PtrIterator    operator- ( DifferenceType n )      const;
-   inline DifferenceType operator- ( const PtrIterator& it ) const;
-   //@}
-   //**********************************************************************************************
-
-   //**Access operators****************************************************************************
-   /*!\name Access operators */
-   //@{
-   inline PointerType& operator[]( DifferenceType n ) const;
-   inline PointerType& operator*()                    const;
-   inline PointerType& operator->()                   const;
-   //@}
-   //**********************************************************************************************
-
-   //**Utility functions***************************************************************************
-   /*!\name Utility functions */
-   //@{
-   inline const IteratorType& base() const;
-   //@}
-   //**********************************************************************************************
-
-private:
-   //**Member variables****************************************************************************
-   /*!\name Member variables */
-   //@{
-   IteratorType it_;  //!< Pointer to the current memory location.
-   //@}
-   //**********************************************************************************************
-};
-//*************************************************************************************************
-
-
-
-
-//=================================================================================================
-//
-//  CONSTRUCTORS
-//
-//=================================================================================================
-
-//*************************************************************************************************
-/*!\brief Default constructor for PtrIterator.
- */
-template< typename Type >
-inline PtrIterator<Type>::PtrIterator()
-   : it_(NULL)  // Pointer to the current memory location
-{}
-//*************************************************************************************************
-
-
-//*************************************************************************************************
-/*!\brief Standard constructor for PtrIterator.
- *
- * \param it The value of the iterator.
- */
-template< typename Type >
-inline PtrIterator<Type>::PtrIterator( const IteratorType& it )
-   : it_(it)  // Pointer to the current memory location
-{}
-//*************************************************************************************************
-
-
-//*************************************************************************************************
-/*!\brief Conversion constructor from different PtrIterator instances.
- *
- * \param it The foreign PtrIterator instance to be copied.
- */
-template< typename Type >
-template< typename Other >
-inline PtrIterator<Type>::PtrIterator( const PtrIterator<Other>& it )
-   : it_( it.base() )  // Pointer to the current memory location
-{}
-//*************************************************************************************************
-
-
-
-
-//=================================================================================================
-//
-//  OPERATORS
-//
-//=================================================================================================
-
-//*************************************************************************************************
-/*!\brief Pre-increment operator.
- *
- * \return Reference to the incremented pointer iterator.
- */
-template< typename Type >
-inline PtrIterator<Type>& PtrIterator<Type>::operator++()
-{
-   ++it_;
-   return *this;
-}
-//*************************************************************************************************
-
-
-//*************************************************************************************************
-/*!\brief Post-increment operator.
- *
- * \return The incremented pointer iterator.
- */
-template< typename Type >
-inline PtrIterator<Type> PtrIterator<Type>::operator++( int )
-{
-   PtrIterator tmp( *this );
-   ++it_;
-   return tmp;
-}
-//*************************************************************************************************
-
-
-//*************************************************************************************************
-/*!\brief Pre-decrement operator.
- *
- * \return Reference to the decremented pointer iterator.
- */
-template< typename Type >
-inline PtrIterator<Type>& PtrIterator<Type>::operator--()
-{
-   --it_;
-   return *this;
-}
-//*************************************************************************************************
-
-
-//*************************************************************************************************
-/*!\brief Post-decrement operator.
- *
- * \return The decremented pointer iterator.
- */
-template< typename Type >
-inline PtrIterator<Type> PtrIterator<Type>::operator--( int )
-{
-   PtrIterator tmp( *this );
-   --it_;
-   return tmp;
-}
-//*************************************************************************************************
-
-
-//*************************************************************************************************
-/*!\brief Shifting the iterator by \a n elements to the higher elements.
- *
- * \param n The number of elements.
- * \return Reference to the shifted pointer iterator.
- */
-template< typename Type >
-inline PtrIterator<Type>& PtrIterator<Type>::operator+=( DifferenceType n )
-{
-   it_ += n;
-   return *this;
-}
-//*************************************************************************************************
-
-
-//*************************************************************************************************
-/*!\brief Shifting the iterator by \a n elements to the higher elements.
- *
- * \param n The number of elements.
- * \return The shifted pointer iterator.
- */
-template< typename Type >
-inline PtrIterator<Type> PtrIterator<Type>::operator+( DifferenceType n ) const
-{
-   return PtrIterator( it_ + n );
-}
-//*************************************************************************************************
-
-
-//*************************************************************************************************
-/*!\brief Shifting the iterator by \a n elements to the lower elements.
- *
- * \param n The number of elements.
- * \return Reference to the shifted pointer iterator.
- */
-template< typename Type >
-inline PtrIterator<Type>& PtrIterator<Type>::operator-=( DifferenceType n )
-{
-   it_ -= n;
-   return *this;
-}
-//*************************************************************************************************
-
-
-//*************************************************************************************************
-/*!\brief Shifting the iterator by \a n elements to the lower elements.
- *
- * \param n The number of elements.
- * \return The shifted pointer iterator.
- */
-template< typename Type >
-inline PtrIterator<Type> PtrIterator<Type>::operator-( DifferenceType n ) const
-{
-   return PtrIterator( it_ - n );
-}
-//*************************************************************************************************
-
-
-//*************************************************************************************************
-/*!\brief Calculating the number of elements between two pointer iterators.
- *
- * \param it The right hand side iterator.
- * \return The number of elements between the two pointer iterators.
- */
-template< typename Type >
-inline typename PtrIterator<Type>::DifferenceType PtrIterator<Type>::operator-( const PtrIterator& it ) const
-{
-   return it_ - it.it_;
-}
-//*************************************************************************************************
-
-
-
-
-//=================================================================================================
-//
-//  ACCESS OPERATORS
-//
-//=================================================================================================
-
-//*************************************************************************************************
-/*!\brief Subscript operator for the direct element access.
- *
- * \param index Access index. Accesses the element \a index elements away from the current iterator position.
- * \return Handle to the accessed element.
- */
-template< typename Type >
-inline typename PtrIterator<Type>::PointerType& PtrIterator<Type>::operator[]( DifferenceType index ) const
-{
-   return it_[index];
-}
-//*************************************************************************************************
-
-
-//*************************************************************************************************
-/*!\brief Returns a handle to the element at the current iterator position.
- *
- * \return Handle to the element at the current iterator position.
- */
-template< typename Type >
-inline typename PtrIterator<Type>::PointerType& PtrIterator<Type>::operator*() const
-{
-   return *it_;
-}
-//*************************************************************************************************
-
-
-//*************************************************************************************************
-/*!\brief Direct access to the element at the current iterator position.
- *
- * \return Reference to the element at the current iterator position.
- */
-template< typename Type >
-inline typename PtrIterator<Type>::PointerType& PtrIterator<Type>::operator->() const
-{
-   return *it_;
-}
-//*************************************************************************************************
-
-
-
-
-//=================================================================================================
-//
-//  UTILITY FUNCTIONS
-//
-//=================================================================================================
-
-//*************************************************************************************************
-/*!\brief Access to the underlying member of the pointer iterator.
- *
- * \return Pointer to the current memory location.
- */
-template< typename Type >
-inline const typename PtrIterator<Type>::IteratorType& PtrIterator<Type>::base() const
-{
-   return it_;
-}
-//*************************************************************************************************
-
-
-
-
-//=================================================================================================
-//
-//  GLOBAL OPERATORS
-//
-//=================================================================================================
-
-//*************************************************************************************************
-/*!\name PtrIterator operators */
-//@{
-template< typename TypeL, typename TypeR >
-inline bool operator==( const PtrIterator<TypeL>& lhs, const PtrIterator<TypeR>& rhs );
-
-template< typename TypeL, typename TypeR >
-inline bool operator!=( const PtrIterator<TypeL>& lhs, const PtrIterator<TypeR>& rhs );
-
-template< typename TypeL, typename TypeR >
-inline bool operator<( const PtrIterator<TypeL>& lhs, const PtrIterator<TypeR>& rhs );
-
-template< typename TypeL, typename TypeR >
-inline bool operator>( const PtrIterator<TypeL>& lhs, const PtrIterator<TypeR>& rhs );
-
-template< typename TypeL, typename TypeR >
-inline bool operator<=( const PtrIterator<TypeL>& lhs, const PtrIterator<TypeR>& rhs );
-
-template< typename TypeL, typename TypeR >
-inline bool operator>=( const PtrIterator<TypeL>& lhs, const PtrIterator<TypeR>& rhs );
-//@}
-//*************************************************************************************************
-
-
-//*************************************************************************************************
-/*!\brief Equality comparison between two PtrIterator objects.
- *
- * \param lhs The left-hand side pointer iterator.
- * \param rhs The right-hand side pointer iterator.
- * \return \a true if the iterators point to the same element, \a false if not.
- */
-template< typename TypeL, typename TypeR >
-inline bool operator==( const PtrIterator<TypeL>& lhs, const PtrIterator<TypeR>& rhs )
-{
-   return lhs.base() == rhs.base();
-}
-//*************************************************************************************************
-
-
-//*************************************************************************************************
-/*!\brief Inequality comparison between two PtrIterator objects.
- *
- * \param lhs The left-hand side pointer iterator.
- * \param rhs The right-hand side pointer iterator.
- * \return \a true if the iterators don't point to the same element, \a false if they do.
- */
-template< typename TypeL, typename TypeR >
-inline bool operator!=( const PtrIterator<TypeL>& lhs, const PtrIterator<TypeR>& rhs )
-{
-   return lhs.base() != rhs.base();
-}
-//*************************************************************************************************
-
-
-//*************************************************************************************************
-/*!\brief Less-than comparison between two PtrIterator objects.
- *
- * \param lhs The left-hand side pointer iterator.
- * \param rhs The right-hand side pointer iterator.
- * \return \a true if the left-hand side iterator points to a lower element, \a false if not.
- */
-template< typename TypeL, typename TypeR >
-inline bool operator<( const PtrIterator<TypeL>& lhs, const PtrIterator<TypeR>& rhs )
-{
-   return lhs.base() < rhs.base();
-}
-//*************************************************************************************************
-
-
-//*************************************************************************************************
-/*!\brief Greater-than comparison between two PtrIterator objects.
- *
- * \param lhs The left-hand side pointer iterator.
- * \param rhs The right-hand side pointer iterator.
- * \return \a true if the left-hand side iterator points to a higher element, \a false if not.
- */
-template< typename TypeL, typename TypeR >
-inline bool operator>( const PtrIterator<TypeL>& lhs, const PtrIterator<TypeR>& rhs )
-{
-   return lhs.base() > rhs.base();
-}
-//*************************************************************************************************
-
-
-//*************************************************************************************************
-/*!\brief Less-or-equal-than comparison between two PtrIterator objects.
- *
- * \param lhs The left-hand side pointer iterator.
- * \param rhs The right-hand side pointer iterator.
- * \return \a true if the left-hand side iterator points to a lower or the same element, \a false if not.
- */
-template< typename TypeL, typename TypeR >
-inline bool operator<=( const PtrIterator<TypeL>& lhs, const PtrIterator<TypeR>& rhs )
-{
-   return lhs.base() <= rhs.base();
-}
-//*************************************************************************************************
-
-
-//*************************************************************************************************
-/*!\brief Greater-or-equal-than comparison between two PtrIterator objects.
- *
- * \param lhs The left-hand side pointer iterator.
- * \param rhs The right-hand side pointer iterator.
- * \return \a true if the left-hand side iterator points to a higher or the same element, \a false if not.
- */
-template< typename TypeL, typename TypeR >
-inline bool operator>=( const PtrIterator<TypeL>& lhs, const PtrIterator<TypeR>& rhs )
-{
-   return lhs.base() >= rhs.base();
-}
-//*************************************************************************************************
-
-} // namespace
diff --git a/src/core/ptrvector/PtrVector.h b/src/core/ptrvector/PtrVector.h
deleted file mode 100644
index 2e62290407295ed080e6b619f7242e709ee952fe..0000000000000000000000000000000000000000
--- a/src/core/ptrvector/PtrVector.h
+++ /dev/null
@@ -1,2604 +0,0 @@
-//======================================================================================================================
-//
-//  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 PtrVector.h
-//! \ingroup core
-//! \author Klaus Iglberger
-//! \author Sebastian Eibl <sebastian.eibl@fau.de>
-//
-//======================================================================================================================
-
-#pragma once
-
-
-//*************************************************************************************************
-// Includes
-//*************************************************************************************************
-
-#include <algorithm>
-#include <stdexcept>
-#include <core/debug/Debug.h>
-#include <core/ptrvector/Algorithm.h>
-#include <core/ptrvector/Null.h>
-#include <core/ptrvector/policies/PtrDelete.h>
-#include <core/ptrvector/policies/OptimalGrowth.h>
-#include <core/ptrvector/PtrIterator.h>
-#include <core/Template.h>
-
-#include <boost/type_traits/is_base_of.hpp>
-#include <boost/type_traits/is_convertible.hpp>
-
-namespace walberla {
-
-//=================================================================================================
-//
-//  CLASS DEFINITION
-//
-//=================================================================================================
-
-//*************************************************************************************************
-/*!\brief Implementation of a vector for (polymorphic) pointers.
- * \ingroup util
- *
- * \section basics Basic usage
- *
- * The \a std::vector is one of the standard libraries most useful tools. It is the standard
- * solution for a dynamically allocated, automatically growing, and memory managed array. It
- * provides fast random access to its elements, since a vector guarantees that the elements
- * lie adjacent in memory and manages the dynamically allocated memory according to the RAII
- * idiom.\n
- * Yet there are some situations, where users of \a std::vector experience several drawbacks,
- * especially when \a std::vector is used in combination with pointers. For instance, a
- * \a const_iterator over a range of pointers will not allow the stored pointers to change,
- * but the objects behind the pointers remain changeable. The following example illustrates
- * that it is possible to change the values of \a double values through an iterator-to-const:
-
-   \code
-   typedef std::vector<double*>  Doubles;
-
-   Doubles doubles;  // Creating a vector for pointers to double values
-
-   // Filling the vector with pointers to double values. All values are initialized with 1.
-   for( size_t i=0; i<10; ++i )
-      doubles.push_back( new double( 1.0 ) );
-
-   // Accessing the first rigid body
-   Doubles::const_iterator first = doubles.begin();
-   **first = 2.0;  // Changes the double value through an iterator-to-const
-   \endcode
-
- * The basic reason for this behavior is that \a std::vector is unaware of the fact that it
- * stores pointers instead of objects and therefore the pointer are considered constant, not
- * the objects behind the pointer.\n
- * Another drawback of \a std::vector is the fact that during destruction of a vector object
- * the dynamically allocated bodies are not deleted. Again, \a std::vector is unaware of the
- * special property of pointers and therefore does not apply any kind of deletion policy. It
- * basically calls the default destructor for pointers, which in turn does nothing and
- * especially does not destroy the attached objects.\n
- * A different approach is taken by the Boost \a ptr_vector. A \a ptr_vector is perfectly
- * aware of the fact that is stores pointers to dynamically objects (and in consequence may
- * only be used with pointers to dynamically allocated objects) and takes full responsibility
- * for these resources. However, in order to accomplish this task, \a ptr_vector completely
- * abstracts from the fact that it stores pointers and provides a view as if it would contain
- * objects instead of pointers. However, unfortunately this strict memory management creates
- * problems in the context of \b walberla, where vectors to pointers are used both internally
- * (including proper resource management) and outside by the user (without any resource
- * management).\n
- * \b walberla provides a special vector container for pointers, which is
- * a cross of the functionalities of the \a std::vector and \a ptr_vector. The \b walberla PtrVector
- * is not a RAII class in the classical sense (as for instance the Boost \a ptr_vector) since
- * it does not strictly encapsule the resource management. As in the case of \a std::vector,
- * it still is the responsibility of a user of PtrVector to manage the resources accordingly.
- * However, it perfectly fits the requirements of \b walberla: PtrVector can be used internally
- * to store pointers to dynamically allocated objects and resources within RAII classes, and
- * outside of \b walberla by a user as storage for handles to resources that are managed elsewhere.
- * In contrast to the \a boost::ptr_vector, the PtrVector provides full access to the contained
- * pointers, but its iterators work similar to the \a ptr_vector
- * iterator and only provide access to the objects behind the pointers, creating the illusion
- * that objects are stored instead of pointers:
-
-   \code
-   typedef walberla::PtrVector<double>  Doubles;
-   Doubles doubles;  // Creating an empty PtrVector for pointers to double values
-
-   doubles.pushBack( new double(1.0) ); // A new pointer-to-double is added to the vector
-
-   double_vector::iterator first = doubles.begin();
-   *first = 2.0;  // No indirection needed
-
-   Doubles::ConstIterator second( first+1 );
-   *second = 3.0;  // Compile time error! It is not possible to change double
-                   // values via an iterator-to-const
-   \endcode
-
- * Notice the differences in the usage of the iterator in contrast to the \a std::vector and
- * \a boost::ptr_vector. In contrast to them the functions of PtrVector follow the naming
- * convention of the \b walberla physics engine (i.e. pushBack instead of push_back). In addition,
- * the underlying iterator adds an additional dereference to all access operators, which eases
- * the access to the underlying objects:
-
-   \code
-   // STL style:
-   **first = 2.0;
-
-   // walberla style:
-   *first = 2.0;
-   \endcode
-
- * A noteworthy difference between the STL vector and the pointer vector is the used template
- * argument: instead of the pointer type, the \b walberla pointer vector is only provided with the
- * type of the underlying objects:
-
-   \code
-   // STL style:
-   std::vector<double*> vector;
-
-   // walberla style:
-   walberla::PtrVector<double> vector;
-   \endcode
-
- * Additionally, the \b walberla pointer vector offers some limited possibilities to configure the
- * memory management and the growth of the internal storage, and implements special features
- * for polymorphic pointers, as for instance a convenient way to iterate over a subset of
- * polymorphic objects contained in the pointer vector.\n\n
- *
- *
- * \section polymorphic Polymorphic pointers
- *
- * For polymorphic pointers, the PtrVector class additionally offers two special iterators to
- * iterate over all objects of a specific type: the CastIterator and ConstCastIterator.
-
-   \code
-   // Definition of class A and the derived type B
-   class A { ... };
-   class B : public A { ... };
-
-   // Definition of function f for non-const pointer vectors
-   void f( PtrVector<A>& vector )
-   {
-      PtrVector<A>::CastIterator<B> begin = vector.begin<B>();
-      PtrVector<A>::CastIterator<B> end   = vector.end<B>();
-
-      // Loop over all objects of type B contained in the vector
-      for( ; begin!=end; ++begin )
-         ...
-   }
-
-   // Definition of function f for const pointer vectors
-   void f( const PtrVector<A>& vector )
-   {
-      PtrVector<A>::ConstCastIterator<B> begin = vector.begin<B>();
-      PtrVector<A>::ConstCastIterator<B> end   = vector.end<B>();
-
-      // Loop over all objects of type B contained in the vector
-      for( ; begin!=end; ++begin )
-   }
-   \endcode
-
- * In the example, the cast iterators are used to iterate over all objects of type \a B within
- * the pointer vector, where \a B must be a type derived from \a A. The attempt to use these
- * iterators for types that are not derived from \a A results in a compile time error. Note that
- * the usage of the cast iterators is computationally more expensive than the use of the standard
- * iterators. Therefore these iterators should not be used unless a down-cast is really necessary,
- * e.g. in order to access a type specific function.\n\n
- *
- *
- * \section container Using a pointer vector within other container classes
- *
- * If a pointer vector is used within an other container and is used to store polymorphic pointers,
- * you might face the problem of not being able to create type definitions for the cast iterators.
- * Whereas it is possible to create typedefs for the standard iterators, it is unfortunately not
- * possible (yet) to create type definitions for template classes. In order to create a new return
- * type within the container, the following approach could be taken:
-
-   \code
-   template< typename A >
-   class Container
-   {
-    public:
-      template< typename C >
-      struct CastIterator : public PtrVector<A>::CastIterator<C>
-      {
-         CastIterator( const PtrVector<A>::CastIterator<C>& it )
-            : PtrVector<A>::CastIterator<C>( it )  // Initializing the base class
-         {}
-      };
-
-      template< typename C >
-      CastIterator<C> begin();
-
-      template< typename C >
-      CastIterator<C> end();
-
-    private:
-      PtrVector<A> vector_;
-   };
-   \endcode
-
- * Instead of a typedef within the Container class, a new class CastIterator is derived from the
- * PtrVector::CastIterator class. This approach acts similar as the typedef as a user can now
- * use the Container as follows:
-
-   \code
-   class A { ... };
-   class B : public A { ... };
-
-   Container<A>::CastIterator<B> begin;
-   \endcode
-
- * This provides the same abstraction from the internal implementation as the desired typedef. The
- * same approach could be taken for a ConstCastIterator definition. However, to keep the code
- * footprint small you should consider using the original type instead.\n\n
- *
- *
- * \section adaptions Adapting a pointer vector
- *
- * The growth and deletion behavior of the PtrVector class can be adapted to any specific task. The
- * second template argument of the PtrVector specifies the growth rate. The following growth rates
- * can be selected:
- *
- *  - ConstantGrowth
- *  - LinearGrowth
- *  - OptimalGrowth (the default behavior)
- *
- * The third template argument of the PtrVector specifies the deletion behavior for the case that
- * the pointer vector is destroyed. Note that the deletion behavior has only limited effect on
- * the memory management of the contained resources. For instance, copying a PtrVector always
- * results in a shallow copy, i.e., the contained resources are not copied/cloned. Therefore the
- * deletion policy should be considered a convenience functionality in the context of a resource
- * managing class. The following policies can be selected:
- *
- *  - NoDelete : No deletion of the contained pointers (the default behavior).
- *  - PtrDelete : Applies \a delete to all contained pointers.
- *  - ArrayDelete : Applies \a delete[] to all contained pointers.\n\n
- */
-template< typename T                    // Type
-        , typename D = PtrDelete        // Deletion policy
-        , typename G = OptimalGrowth >  // Growth policy
-class PtrVector
-{
-private:
-   //**Friend declarations*************************************************************************
-   /*! \cond internal */
-   template< typename T2, typename D2, typename G2 > friend class PtrVector;
-   /*! \endcond */
-   //**********************************************************************************************
-
-public:
-   //**Type definitions****************************************************************************
-   typedef T*                    ValueType;           //!< Type of the underlying values.
-   typedef T*                    PointerType;         //!< Pointer to a non-const object.
-   typedef const T*              ConstPointerType;    //!< Pointer to a const object.
-   typedef T*&                   ReferenceType;       //!< Reference to a non-const object.
-   typedef T*const&              ConstReferenceType;  //!< Reference to a const object.
-   typedef size_t                SizeType;            //!< Size type of the pointer vector.
-   typedef PtrIterator<T>        Iterator;            //!< Iterator over non-const objects.
-   typedef PtrIterator<const T>  ConstIterator;       //!< Iterator over const objects.
-   typedef D                     DeletionPolicy;      //!< Type of the deletion policy.
-   typedef G                     GrowthPolicy;        //!< Type of the growth policy.
-
-   // STL iterator requirements
-   typedef ValueType             value_type;          //!< Type of the underlying values.
-   typedef PointerType           pointer;             //!< Pointer to a non-const object.
-   typedef ConstPointerType      const_pointer;       //!< Pointer to a const object.
-   typedef ReferenceType         reference;           //!< Reference to a non-const object.
-   typedef ConstReferenceType    const_reference;     //!< Reference to a const object.
-   typedef SizeType              size_type;           //!< Size type of the pointer vector.
-   //**********************************************************************************************
-
-   //**Forward declarations for nested classes*****************************************************
-   template< typename C > class CastIterator;
-   template< typename C > class ConstCastIterator;
-   //**********************************************************************************************
-
-   //**Constructors********************************************************************************
-   /*!\name Constructors */
-   //@{
-   explicit inline PtrVector( SizeType initCapacity = 0 );
-            inline PtrVector( const PtrVector& pv );
-
-   template< typename T2, typename D2, typename G2 >
-            inline PtrVector( const PtrVector<T2,D2,G2>& pv );
-   //@}
-   //**********************************************************************************************
-
-   //**Destructor**********************************************************************************
-   /*!\name Destructor */
-   //@{
-   inline ~PtrVector();
-   //@}
-   //**********************************************************************************************
-
-   //**Assignment operators************************************************************************
-   /*!\name Assignment operators */
-   //@{
-   PtrVector& operator=( const PtrVector& pv );
-
-   template< typename T2, typename D2, typename G2 >
-   PtrVector& operator=( const PtrVector<T2,D2,G2>& pv );
-   //@}
-   //**********************************************************************************************
-
-   //**Get functions*******************************************************************************
-   /*!\name Get functions */
-   //@{
-                          inline SizeType maxSize()  const;
-                          inline SizeType size()     const;
-   template< typename C > inline SizeType size()     const;
-                          inline SizeType capacity() const;
-                          inline bool     isEmpty()  const;
-   //@}
-   //**********************************************************************************************
-
-   //**Access functions****************************************************************************
-   /*!\name Access functions */
-   //@{
-   inline ReferenceType      operator[]( SizeType index );
-   inline ConstReferenceType operator[]( SizeType index ) const;
-   inline ReferenceType      front();
-   inline ConstReferenceType front() const;
-   inline ReferenceType      back();
-   inline ConstReferenceType back()  const;
-   //@}
-   //**********************************************************************************************
-
-   //**Iterator functions**************************************************************************
-   /*!\name Iterator functions */
-   //@{
-                          inline Iterator             begin();
-                          inline ConstIterator        begin() const;
-   template< typename C > inline CastIterator<C>      begin();
-   template< typename C > inline ConstCastIterator<C> begin() const;
-
-                          inline Iterator             end();
-                          inline ConstIterator        end()   const;
-   template< typename C > inline CastIterator<C>      end();
-   template< typename C > inline ConstCastIterator<C> end()   const;
-   //@}
-   //**********************************************************************************************
-
-   //**Element functions***************************************************************************
-   /*!\name Element functions */
-   //@{
-   inline void     pushBack   ( PointerType p );
-   inline void     popBack    ();
-   inline void     releaseBack();
-
-   template< typename IteratorType >
-   inline void     assign( IteratorType first, IteratorType last );
-
-   inline Iterator insert( Iterator pos, PointerType p );
-
-   template< typename IteratorType >
-   inline void     insert( Iterator pos, IteratorType first, IteratorType last );
-
-   /*! \cond internal */
-   template< typename IteratorType >
-   inline void     insert( Iterator pos, IteratorType* first, IteratorType* last );
-   /*! \endcond */
-
-                          inline Iterator        erase  ( Iterator pos );
-   template< typename C > inline CastIterator<C> erase  ( CastIterator<C> pos );
-                          inline Iterator        release( Iterator pos );
-   template< typename C > inline CastIterator<C> release( CastIterator<C> pos );
-                          inline void            clear  ();
-   //@}
-   //**********************************************************************************************
-
-   //**Utility functions***************************************************************************
-   /*!\name Utility functions */
-   //@{
-          void reserve( SizeType newCapacity );
-   inline void swap( PtrVector& pv ) /* throw() */;
-   //@}
-   //**********************************************************************************************
-
-private:
-   //**Helper functions****************************************************************************
-   /*!\name Helper functions */
-   //@{
-   inline size_t calcCapacity ( size_t minCapacity ) const;
-   inline void   deleteElement( PointerType ptr )    const;
-   //@}
-   //**********************************************************************************************
-
-   //**Insertion helper functions******************************************************************
-   /*!\name Insertion helper functions */
-   //@{
-          void insert( T**const pos, PointerType p );
-
-   /*! \cond internal */
-   template< typename IteratorType >
-   inline void insert( Iterator pos, IteratorType first, IteratorType last, std::input_iterator_tag );
-
-   template< typename IteratorType >
-   inline void insert( Iterator pos, IteratorType first, IteratorType last, std::random_access_iterator_tag );
-   /*! \endcond */
-
-   template< typename IteratorType >
-          void insert( T** pos, IteratorType first, IteratorType last, SizeType n );
-   //@}
-   //**********************************************************************************************
-
-   //**Member variables****************************************************************************
-   /*!\name Member variables */
-   //@{
-   SizeType size_;       //!< The current size of the pointer vector.
-   SizeType capacity_;   //!< The capacity of the pointer vector.
-   PointerType* begin_;  //!< Pointer to the first element of the pointer vector.
-   PointerType* end_;    //!< Pointer to the last element of the pointer vector.
-   //@}
-   //**********************************************************************************************
-
-public:
-   //**CastIterator/ConstCastIterator comparison operators*****************************************
-   // The following comparison operators cannot be defined as namespace or member functions
-   // but have to be injected into the surrounding scope via the Barton-Nackman trick since
-   // the template arguments of nested templates cannot be deduced (C++ standard 14.8.2.4/4).
-   /*!\name CastIterator/ConstCastIterator comparison operators */
-   //@{
-
-   //**********************************************************************************************
-   /*!\brief Equality comparison between two CastIterator objects.
-   //
-   // \param lhs The left hand side cast iterator.
-   // \param rhs The right hand side cast iterator.
-   // \return \a true if the iterators point to the same element, \a false if not.
-   */
-   template< typename L, typename R >
-   friend inline bool operator==( const CastIterator<L>& lhs, const CastIterator<R>& rhs )
-   {
-      return lhs.base() == rhs.base();
-   }
-   //**********************************************************************************************
-
-   //**********************************************************************************************
-   /*!\brief Equality comparison between a CastIterator and a ConstCastIterator.
-   //
-   // \param lhs The left hand side cast iterator.
-   // \param rhs The right hand side constant cast iterator.
-   // \return \a true if the iterators point to the same element, \a false if not.
-   */
-   template< typename L, typename R >
-   friend inline bool operator==( const CastIterator<L>& lhs, const ConstCastIterator<R>& rhs )
-   {
-      return lhs.base() == rhs.base();
-   }
-   //**********************************************************************************************
-
-   //**********************************************************************************************
-   /*!\brief Equality comparison between a ConstCastIterator and a CastIterator.
-   //
-   // \param lhs The left hand side constant cast iterator.
-   // \param rhs The right hand side cast iterator.
-   // \return \a true if the iterators point to the same element, \a false if not.
-   */
-   template< typename L, typename R >
-   friend inline bool operator==( const ConstCastIterator<L>& lhs, const CastIterator<R>& rhs )
-   {
-      return lhs.base() == rhs.base();
-   }
-   //**********************************************************************************************
-
-   //**********************************************************************************************
-   /*!\brief Equality comparison between two ConstCastIterator objects.
-   //
-   // \param lhs The left hand side constant cast iterator.
-   // \param rhs The right hand side constant cast iterator.
-   // \return \a true if the iterators point to the same element, \a false if not.
-   */
-   template< typename L, typename R >
-   friend inline bool operator==( const ConstCastIterator<L>& lhs, const ConstCastIterator<R>& rhs )
-   {
-      return lhs.base() == rhs.base();
-   }
-   //**********************************************************************************************
-
-   //**********************************************************************************************
-   /*!\brief Inequality comparison between two CastIterator objects.
-   //
-   // \param lhs The left hand side cast iterator.
-   // \param rhs The right hand side cast iterator.
-   // \return \a true if the iterators don't point to the same element, \a false if they do.
-   */
-   template< typename L, typename R >
-   friend inline bool operator!=( const CastIterator<L>& lhs, const CastIterator<R>& rhs )
-   {
-      return lhs.base() != rhs.base();
-   }
-   //**********************************************************************************************
-
-   //**********************************************************************************************
-   /*!\brief Inequality comparison between a CastIterator and a ConstCastIterator.
-   //
-   // \param lhs The left hand side cast iterator.
-   // \param rhs The right hand side constant cast iterator.
-   // \return \a true if the iterators don't point to the same element, \a false if they do.
-   */
-   template< typename L, typename R >
-   friend inline bool operator!=( const CastIterator<L>& lhs, const ConstCastIterator<R>& rhs )
-   {
-      return lhs.base() != rhs.base();
-   }
-   //**********************************************************************************************
-
-   //**********************************************************************************************
-   /*!\brief Inequality comparison between a ConstCastIterator and a CastIterator.
-   //
-   // \param lhs The left hand side constant cast iterator.
-   // \param rhs The right hand side cast iterator.
-   // \return \a true if the iterators don't point to the same element, \a false if they do.
-   */
-   template< typename L, typename R >
-   friend inline bool operator!=( const ConstCastIterator<L>& lhs, const CastIterator<R>& rhs )
-   {
-      return lhs.base() != rhs.base();
-   }
-   //**********************************************************************************************
-
-   //**********************************************************************************************
-   /*!\brief Inequality comparison between two ConstCastIterator objects.
-   //
-   // \param lhs The left hand side constant cast iterator.
-   // \param rhs The right hand side constant cast iterator.
-   // \return \a true if the iterators don't point to the same element, \a false if they do.
-   */
-   template< typename L, typename R >
-   friend inline bool operator!=( const ConstCastIterator<L>& lhs, const ConstCastIterator<R>& rhs )
-   {
-      return lhs.base() != rhs.base();
-   }
-   //**********************************************************************************************
-
-   //@}
-   //**********************************************************************************************
-};
-//*************************************************************************************************
-
-
-
-
-//=================================================================================================
-//
-//  CONSTRUCTORS
-//
-//=================================================================================================
-
-//*************************************************************************************************
-/*!\brief Standard constructor for PtrVector.
- *
- * \param initCapacity The initial capacity of the pointer vector.
- *
- * The default initial capacity of the pointer vector is specified by the selected growth policy.
- */
-template< typename T    // Type
-        , typename D    // Deletion policy
-        , typename G >  // Growth policy
-inline PtrVector<T,D,G>::PtrVector( SizeType initCapacity )
-   : size_( 0 )                               // Current size of the pointer vector
-   , capacity_( initCapacity )                // Capacity of the pointer vector
-   , begin_( new PointerType[initCapacity] )  // Pointer to the first element
-   , end_( begin_ )                           // Pointer to the last element
-{}
-//*************************************************************************************************
-
-
-//*************************************************************************************************
-/*!\brief Copy constructor for PtrVector.
- *
- * \param pv The pointer vector to be copied.
- */
-template< typename T    // Type
-        , typename D    // Deletion policy
-        , typename G >  // Growth policy
-inline PtrVector<T,D,G>::PtrVector( const PtrVector& pv )
-   : size_( pv.size_ )                     // Current size of the pointer vector
-   , capacity_( pv.size_ )                 // Capacity of the pointer vector
-   , begin_( new PointerType[capacity_] )  // Pointer to the first element
-   , end_( begin_+size_ )                  // Pointer to the last element
-{
-   for( SizeType i=0; i<size_; ++i )
-      begin_[i] = pv.begin_[i];
-}
-//*************************************************************************************************
-
-
-//*************************************************************************************************
-/*!\brief Conversion constructor from different PtrVector instances.
- *
- * \param pv The pointer vector to be copied.
- */
-template< typename T     // Type of the pointer vector
-        , typename D     // Deletion policy of the pointer vector
-        , typename G >   // Growth policy of the pointer vector
-template< typename T2    // Type of the foreign pointer vector
-        , typename D2    // Deletion policy of the foreign pointer vector
-        , typename G2 >  // Growth policy of the foreign pointer vector
-inline PtrVector<T,D,G>::PtrVector( const PtrVector<T2,D2,G2>& pv )
-   : size_( pv.size_ )                     // Current size of the pointer vector
-   , capacity_( pv.size_ )                 // Capacity of the pointer vector
-   , begin_( new PointerType[capacity_] )  // Pointer to the first element
-   , end_( begin_+size_ )                  // Pointer to the last element
-{
-   for( SizeType i=0; i<size_; ++i )
-      begin_[i] = pv.begin_[i];
-}
-//*************************************************************************************************
-
-
-
-
-//=================================================================================================
-//
-//  DESTRUCTOR
-//
-//=================================================================================================
-
-//*************************************************************************************************
-/*!\brief Destructor for PtrVector.
- *
- * In the destructor, the selected deletion policy is applied to all elements of the pointer
- * vector.
- */
-template< typename T    // Type
-        , typename D    // Deletion policy
-        , typename G >  // Growth policy
-inline PtrVector<T,D,G>::~PtrVector()
-{
-   for( PointerType* it=begin_; it!=end_; ++it )
-      deleteElement( *it );
-   delete [] begin_;
-}
-//*************************************************************************************************
-
-
-
-
-//=================================================================================================
-//
-//  ASSIGNMENT OPERATORS
-//
-//=================================================================================================
-
-//*************************************************************************************************
-/*!\brief Copy assignment operator for PtrVector.
- *
- * \param pv The pointer vector to be copied.
- * \return Reference to the assigned pointer vector.
- */
-template< typename T    // Type
-        , typename D    // Deletion policy
-        , typename G >  // Growth policy
-PtrVector<T,D,G>& PtrVector<T,D,G>::operator=( const PtrVector& pv )
-{
-   if( &pv == this ) return *this;
-
-   if( pv.size_ > capacity_ ) {
-      PointerType* newBegin( new PointerType[pv.size_] );
-      end_ = std::copy( pv.begin_, pv.end_, newBegin );
-      std::swap( begin_, newBegin );
-      delete [] newBegin;
-
-      size_ = pv.size_;
-      capacity_ = pv.size_;
-   }
-   else {
-      end_ = std::copy( pv.begin_, pv.end_, begin_ );
-      size_ = pv.size_;
-   }
-
-   return *this;
-}
-//*************************************************************************************************
-
-
-//*************************************************************************************************
-/*!\brief Assignment operator for different PtrVector instances.
- *
- * \param pv The pointer vector to be copied.
- * \return Reference to the assigned pointer vector.
- */
-template< typename T     // Type of the pointer vector
-        , typename D     // Deletion policy of the pointer vector
-        , typename G >   // Growth policy of the pointer vector
-template< typename T2    // Type of the foreign pointer vector
-        , typename D2    // Deletion policy of the foreign pointer vector
-        , typename G2 >  // Growth policy of the foreign pointer vector
-PtrVector<T,D,G>& PtrVector<T,D,G>::operator=( const PtrVector<T2,D2,G2>& pv )
-{
-   if( pv.size_ > capacity_ ) {
-      PointerType* newBegin( new PointerType[pv.size_] );
-      end_ = std::copy( pv.begin_, pv.end_, newBegin );
-      std::swap( begin_, newBegin );
-      delete [] newBegin;
-
-      size_ = pv.size_;
-      capacity_ = pv.size_;
-   }
-   else {
-      end_ = std::copy( pv.begin_, pv.end_, begin_ );
-      size_ = pv.size_;
-   }
-
-   return *this;
-}
-//*************************************************************************************************
-
-
-
-
-//=================================================================================================
-//
-//  GET FUNCTIONS
-//
-//=================================================================================================
-
-//*************************************************************************************************
-/*!\brief Returns the maximum possible size of a pointer vector.
- *
- * \return The maximum possible size.
- */
-template< typename T    // Type
-        , typename D    // Deletion policy
-        , typename G >  // Growth policy
-inline typename PtrVector<T,D,G>::SizeType PtrVector<T,D,G>::maxSize() const
-{
-   return SizeType(-1) / sizeof(PointerType);
-}
-//*************************************************************************************************
-
-
-//*************************************************************************************************
-/*!\brief Returns the current size of the pointer vector.
- *
- * \return The current size.
- */
-template< typename T    // Type
-        , typename D    // Deletion policy
-        , typename G >  // Growth policy
-inline typename PtrVector<T,D,G>::SizeType PtrVector<T,D,G>::size() const
-{
-   return size_;
-}
-//*************************************************************************************************
-
-
-//*************************************************************************************************
-/*!\brief Returns the total number of objects of type \a C contained in the pointer vector.
- *
- * \return The total number of objects of type \a C.
- *
- * This function calculates the total number of objects of type \a C within the pointer vector,
- * where \a C is a type derived from the type \a T of objects contained in the pointer vector.
- * The attempt to use this function for types that are not derived from \a T results in a
- * compile time error.
-
-   \code
-   // Definition of class A and the derived type B
-   class A { ... };
-   class B : public A { ... };
-
-   // Definition of a pointer vector for class A
-   typedef PtrVector<A> AVector;
-   AVector vector;
-
-   AVector::SizeType total = vector.size();     // Calculating the total number of pointers
-   AVector::SizeType numB  = vector.size<B>();  // Calculating the total number of B objects
-   \endcode
-
- * \b Note: The total number of objects of type \a C is not cached inside the pointer vector
- * but is calculated each time the function is called. Using the templated version of size()
- * to calculate the total number objects of type \a C is therefore more expensive than using
- * the non-template version of size() to get the total number of pointers in the vector!
- */
-template< typename T    // Type
-        , typename D    // Deletion policy
-        , typename G >  // Growth policy
-template< typename C >  // Cast type
-inline typename PtrVector<T,D,G>::SizeType PtrVector<T,D,G>::size() const
-{
-   // The polymorphicCount() function returns the number of objects with dynamic type 'C'
-   // contained in the range [begin,end). An equivalent code might look like this:
-   //
-   // SizeType count( 0 );
-   // for( PointerType* it=begin_; it!=end_; ++it )
-   //    if( dynamic_cast<C*>( *it ) ) ++count;
-   // return count;
-   //
-   // However, the specialization of polymorphicCount() for special type combinations is
-   // much more efficient (and easier) than the specialization of this function!
-   return polymorphicCount<C>( begin_, end_ );
-}
-//*************************************************************************************************
-
-
-//*************************************************************************************************
-/*!\brief Returns the capacity of the pointer vector.
- *
- * \return The capacity.
- */
-template< typename T    // Type
-        , typename D    // Deletion policy
-        , typename G >  // Growth policy
-inline typename PtrVector<T,D,G>::SizeType PtrVector<T,D,G>::capacity() const
-{
-   return capacity_;
-}
-//*************************************************************************************************
-
-
-//*************************************************************************************************
-/*!\brief Returns \a true if the pointer vector has no elements.
- *
- * \return \a true if the pointer vector is empty, \a false if it is not.
- */
-template< typename T    // Type
-        , typename D    // Deletion policy
-        , typename G >  // Growth policy
-inline bool PtrVector<T,D,G>::isEmpty() const
-{
-   return size_ == 0;
-}
-//*************************************************************************************************
-
-
-
-
-//=================================================================================================
-//
-//  ACCESS FUNCTIONS
-//
-//=================================================================================================
-
-//*************************************************************************************************
-/*!\brief Subscript operator for the direct access to the pointer vector elements.
- *
- * \param index Access index. The index has to be in the range \f$[0..size-1]\f$.
- * \return Handle to the accessed element.
- *
- * \b Note: No runtime check is performed to insure the validity of the access index.
- */
-template< typename T    // Type
-        , typename D    // Deletion policy
-        , typename G >  // Growth policy
-inline typename PtrVector<T,D,G>::ReferenceType PtrVector<T,D,G>::operator[]( SizeType index )
-{
-   return *(begin_+index);
-}
-//*************************************************************************************************
-
-
-//*************************************************************************************************
-/*!\brief Subscript operator for the direct access to the pointer vector elements.
- *
- * \param index Access index. The index has to be in the range \f$[0..size-1]\f$.
- * \return Handle to the accessed element.
- *
- * \b Note: No runtime check is performed to insure the validity of the access index.
- */
-template< typename T    // Type
-        , typename D    // Deletion policy
-        , typename G >  // Growth policy
-inline typename PtrVector<T,D,G>::ConstReferenceType PtrVector<T,D,G>::operator[]( SizeType index ) const
-{
-   return *(begin_+index);
-}
-//*************************************************************************************************
-
-
-//*************************************************************************************************
-/*!\brief Returns a reference to the first element of the pointer vector.
- *
- * \return Handle to the first element.
- *
- * \b Note: No runtime check is performed if the first element exists!
- */
-template< typename T    // Type
-        , typename D    // Deletion policy
-        , typename G >  // Growth policy
-inline typename PtrVector<T,D,G>::ReferenceType PtrVector<T,D,G>::front()
-{
-   WALBERLA_ASSERT( size_ > 0, "Pointer vector is empty, invalid access to the front element" );
-   return *begin_;
-}
-//*************************************************************************************************
-
-
-//*************************************************************************************************
-/*!\brief Returns a reference to the first element of the pointer vector.
- *
- * \return Handle to the first element.
- *
- * \b Note: No runtime check is performed if the first element exists!
- */
-template< typename T    // Type
-        , typename D    // Deletion policy
-        , typename G >  // Growth policy
-inline typename PtrVector<T,D,G>::ConstReferenceType PtrVector<T,D,G>::front() const
-{
-   WALBERLA_ASSERT( size_ > 0, "Pointer vector is empty, invalid access to the front element" );
-   return *begin_;
-}
-//*************************************************************************************************
-
-
-//*************************************************************************************************
-/*!\brief Returns a reference to the last element of the pointer vector.
- *
- * \return Handle to the last element.
- *
- * \b Note: No runtime check is performed if the last element exists!
- */
-template< typename T    // Type
-        , typename D    // Deletion policy
-        , typename G >  // Growth policy
-inline typename PtrVector<T,D,G>::ReferenceType PtrVector<T,D,G>::back()
-{
-   WALBERLA_ASSERT( size_ > 0, "Pointer vector is empty, invalid access to the back element" );
-   return *(end_-1);
-}
-//*************************************************************************************************
-
-
-//*************************************************************************************************
-/*!\brief Returns a reference to the last element of the pointer vector.
- *
- * \return Handle to the last element.
- *
- * \b Note: No runtime check is performed if the last element exists!
- */
-template< typename T    // Type
-        , typename D    // Deletion policy
-        , typename G >  // Growth policy
-inline typename PtrVector<T,D,G>::ConstReferenceType PtrVector<T,D,G>::back() const
-{
-   WALBERLA_ASSERT( size_ > 0, "Pointer vector is empty, invalid access to the back element" );
-   return *(end_-1);
-}
-//*************************************************************************************************
-
-
-
-
-//=================================================================================================
-//
-//  ITERATOR FUNCTIONS
-//
-//=================================================================================================
-
-//*************************************************************************************************
-/*!\brief Returns an iterator to the beginning of the pointer vector.
- *
- * \return Iterator to the beginning of the pointer vector.
- */
-template< typename T    // Type
-        , typename D    // Deletion policy
-        , typename G >  // Growth policy
-inline typename PtrVector<T,D,G>::Iterator PtrVector<T,D,G>::begin()
-{
-   return Iterator( begin_ );
-}
-//*************************************************************************************************
-
-
-//*************************************************************************************************
-/*!\brief Returns an iterator to the beginning of the pointer vector.
- *
- * \return Iterator to the beginning of the pointer vector.
- */
-template< typename T    // Type
-        , typename D    // Deletion policy
-        , typename G >  // Growth policy
-inline typename PtrVector<T,D,G>::ConstIterator PtrVector<T,D,G>::begin() const
-{
-   return ConstIterator( begin_ );
-}
-//*************************************************************************************************
-
-
-//*************************************************************************************************
-/*!\brief Returns an iterator to the first element of type \a C within the pointer vector.
- *
- * \return Iterator to the first element of type \a C.
- *
- * This function returns an iterator to the first element of type \a C within in the pointer
- * vector, where \a C is a type derived from the type \a T of objects contained in the pointer
- * vector. In case there is no element of type \a C contained in the vector, an iterator just
- * past the last element of the pointer vector is returned. In combination with the according
- * end function (see example), this iterator allows to iterate over all objects of type \a C
- * in the range of the pointer vector. The attempt to use this function for types that are not
- * derived from \a T results in a compile time error.
-
-   \code
-   // Definition of class A and the derived type B
-   class A { ... };
-   class B : public A { ... };
-
-   // Definition of function f for non-const pointer vectors
-   void f( PtrVector<A>& vector )
-   {
-      PtrVector<A>::CastIterator<B> begin = vector.begin<B>();
-      PtrVector<A>::CastIterator<B> end   = vector.end<B>();
-
-      // Loop over all objects of type B contained in the vector
-      for( ; begin!=end; ++begin )
-         ...
-   }
-
-   // Definition of function f for const pointer vectors
-   void f( const PtrVector<A>& vector )
-   {
-      PtrVector<A>::ConstCastIterator<B> begin = vector.begin<B>();
-      PtrVector<A>::ConstCastIterator<B> end   = vector.end<B>();
-
-      // Loop over all objects of type B contained in the vector
-      for( ; begin!=end; ++begin )
-   }
-   \endcode
-
- * \b Note: Using the templated versions of begin() and end() to traverse all elements of type
- * \a C in the element range of the pointer vector is more expensive than using the non-template
- * versions to traverse the entire range of elements. Use this function only if you require a
- * type-specific member of type \a C.
- */
-template< typename T    // Type
-        , typename D    // Deletion policy
-        , typename G >  // Growth policy
-template< typename C >  // Cast type
-inline typename PtrVector<T,D,G>::WALBERLA_TEMPLATE CastIterator<C> PtrVector<T,D,G>::begin()
-{
-   return CastIterator<C>( begin_, end_ );
-}
-//*************************************************************************************************
-
-
-//*************************************************************************************************
-/*!\brief Returns an iterator to the first element of type \a C within the pointer vector.
- *
- * \return Iterator to the first element of type \a C.
- *
- * This function returns an iterator to the first element of type \a C within in the pointer
- * vector, where \a C is a type derived from the type \a T of objects contained in the pointer
- * vector. In case there is no element of type \a C contained in the vector, an iterator just
- * past the last element of the pointer vector is returned. In combination with the according
- * end function (see example), this iterator allows to iterate over all objects of type \a C
- * in the range of the pointer vector. The attempt to use this function for types that are not
- * derived from \a T results in a compile time error.
-
-   \code
-   // Definition of class A and the derived type B
-   class A { ... };
-   class B : public A { ... };
-
-   // Definition of function f for non-const pointer vectors
-   void f( PtrVector<A>& vector )
-   {
-      PtrVector<A>::CastIterator<B> begin = vector.begin<B>();
-      PtrVector<A>::CastIterator<B> end   = vector.end<B>();
-
-      // Loop over all objects of type B contained in the vector
-      for( ; begin!=end; ++begin )
-         ...
-   }
-
-   // Definition of function f for const pointer vectors
-   void f( const PtrVector<A>& vector )
-   {
-      PtrVector<A>::ConstCastIterator<B> begin = vector.begin<B>();
-      PtrVector<A>::ConstCastIterator<B> end   = vector.end<B>();
-
-      // Loop over all objects of type B contained in the vector
-      for( ; begin!=end; ++begin )
-   }
-   \endcode
-
- * \b Note: Using the templated versions of begin() and end() to traverse all elements of type
- * \a C in the element range of the pointer vector is more expensive than using the non-template
- * version to traverse the entire range of elements. Use this function only if you require a
- * type-specific member of type \a C.
- */
-template< typename T    // Type
-        , typename D    // Deletion policy
-        , typename G >  // Growth policy
-template< typename C >  // Cast type
-inline typename PtrVector<T,D,G>::WALBERLA_TEMPLATE ConstCastIterator<C> PtrVector<T,D,G>::begin() const
-{
-   return ConstCastIterator<C>( begin_, end_ );
-}
-//*************************************************************************************************
-
-
-//*************************************************************************************************
-/*!\brief Returns an iterator just past the last element of the pointer vector.
- *
- * \return Iterator just past the last element of the pointer vector.
- */
-template< typename T    // Type
-        , typename D    // Deletion policy
-        , typename G >  // Growth policy
-inline typename PtrVector<T,D,G>::Iterator PtrVector<T,D,G>::end()
-{
-   return Iterator( end_ );
-}
-//*************************************************************************************************
-
-
-//*************************************************************************************************
-/*!\brief Returns an iterator just past the last element of the pointer vector.
- *
- * \return Iterator just past the last element of the pointer vector.
- */
-template< typename T    // Type
-        , typename D    // Deletion policy
-        , typename G >  // Growth policy
-inline typename PtrVector<T,D,G>::ConstIterator PtrVector<T,D,G>::end() const
-{
-   return ConstIterator( end_ );
-}
-//*************************************************************************************************
-
-
-//*************************************************************************************************
-/*!\brief Returns an iterator just past the last element of the pointer vector.
- *
- * \return Iterator just past the last element of the pointer vector.
- *
- * This function returns an iterator just past the last element of the pointer vector. In
- * combination with the according begin function (see example), this iterator allows to iterate
- * over all objects of type \a C in the range of the pointer vector. The attempt to use this
- * function for types that are not derived from \a T results in a compile time error.
-
-   \code
-   // Definition of class A and the derived type B
-   class A { ... };
-   class B : public A { ... };
-
-   // Definition of function f for non-const pointer vectors
-   void f( PtrVector<A>& vector )
-   {
-      PtrVector<A>::CastIterator<B> begin = vector.begin<B>();
-      PtrVector<A>::CastIterator<B> end   = vector.end<B>();
-
-      // Loop over all objects of type B contained in the vector
-      for( ; begin!=end; ++begin )
-         ...
-   }
-
-   // Definition of function f for const pointer vectors
-   void f( const PtrVector<A>& vector )
-   {
-      PtrVector<A>::ConstCastIterator<B> begin = vector.begin<B>();
-      PtrVector<A>::ConstCastIterator<B> end   = vector.end<B>();
-
-      // Loop over all objects of type B contained in the vector
-      for( ; begin!=end; ++begin )
-   }
-   \endcode
-
- * \b Note: Using the templated versions of begin() and end() to traverse all elements of type
- * \a C in the element range of the pointer vector is more expensive than using the non-template
- * versions to traverse the entire range of elements. Use this function only if you require a
- * type-specific member of type \a C.
- */
-template< typename T    // Type
-        , typename D    // Deletion policy
-        , typename G >  // Growth policy
-template< typename C >  // Cast type
-inline typename PtrVector<T,D,G>::WALBERLA_TEMPLATE CastIterator<C> PtrVector<T,D,G>::end()
-{
-   return CastIterator<C>( end_, end_ );
-}
-//*************************************************************************************************
-
-
-//*************************************************************************************************
-/*!\brief Returns an iterator just past the last element of the pointer vector.
- *
- * \return Iterator just past the last element of the pointer vector.
- *
- * This function returns an iterator just past the last element of the pointer vector. In
- * combination with the according begin function (see example), this iterator allows to iterate
- * over all objects of type \a C in the range of the pointer vector. The attempt to use this
- * function for types that are not derived from \a T results in a compile time error.
-
-   \code
-   // Definition of class A and the derived type B
-   class A { ... };
-   class B : public A { ... };
-
-   // Definition of function f for non-const pointer vectors
-   void f( PtrVector<A>& vector )
-   {
-      PtrVector<A>::CastIterator<B> begin = vector.begin<B>();
-      PtrVector<A>::CastIterator<B> end   = vector.end<B>();
-
-      // Loop over all objects of type B contained in the vector
-      for( ; begin!=end; ++begin )
-         ...
-   }
-
-   // Definition of function f for const pointer vectors
-   void f( const PtrVector<A>& vector )
-   {
-      PtrVector<A>::ConstCastIterator<B> begin = vector.begin<B>();
-      PtrVector<A>::ConstCastIterator<B> end   = vector.end<B>();
-
-      // Loop over all objects of type B contained in the vector
-      for( ; begin!=end; ++begin )
-   }
-   \endcode
-
- * \b Note: Using the templated versions of begin() and end() to traverse all elements of type
- * \a C in the element range of the pointer vector is more expensive than using the non-template
- * version to traverse the entire range of elements. Use this function only if you require a
- * type-specific member of type \a C.
- */
-template< typename T    // Type
-        , typename D    // Deletion policy
-        , typename G >  // Growth policy
-template< typename C >  // Cast type
-inline typename PtrVector<T,D,G>::WALBERLA_TEMPLATE ConstCastIterator<C> PtrVector<T,D,G>::end() const
-{
-   return ConstCastIterator<C>( end_, end_ );
-}
-//*************************************************************************************************
-
-
-
-
-//=================================================================================================
-//
-//  ELEMENT FUNCTIONS
-//
-//=================================================================================================
-
-//*************************************************************************************************
-/*!\brief Adding an element to the end of the pointer vector.
- *
- * \param p The pointer to be added to the end of the pointer vector.
- * \return void
- * \exception std::length_error Maximum pointer vector length exceeded.
- *
- * The pushBack function runs in constant time.
- */
-template< typename T    // Type
-        , typename D    // Deletion policy
-        , typename G >  // Growth policy
-inline void PtrVector<T,D,G>::pushBack( PointerType p )
-{
-   if( size_ != capacity_ ) {
-      *end_ = p;
-      ++end_;
-      ++size_;
-   }
-   else {
-      insert( end_, p );
-   }
-}
-//*************************************************************************************************
-
-
-//*************************************************************************************************
-/*!\brief Removing an element from the end of the pointer vector.
- *
- * \return void
- *
- * This function removes the element at the end of the pointer vector, i.e. the element
- * is deleted according to the deletion policy and removed from the vector. Note that in
- * case the deletion policy is NoDelete, this function is identical to the releaseBack()
- * function.
- */
-template< typename T    // Type
-        , typename D    // Deletion policy
-        , typename G >  // Growth policy
-inline void PtrVector<T,D,G>::popBack()
-{
-   deleteElement( *--end_ );
-   --size_;
-}
-//*************************************************************************************************
-
-
-//*************************************************************************************************
-/*!\brief Releasing the element at the end of the pointer vector.
- *
- * \return void
- *
- * This function releases the element at the end of the pointer vector, i.e. the element is
- * removed without applying the deletion policy. Therefore the responsibility to delete the
- * element is passed to the function caller. Note that in case the deletion policy is NoDelete,
- * this function is identical to the popBack() function.
- */
-template< typename T    // Type
-        , typename D    // Deletion policy
-        , typename G >  // Growth policy
-inline void PtrVector<T,D,G>::releaseBack()
-{
-   --end_;
-   --size_;
-}
-//*************************************************************************************************
-
-
-//*************************************************************************************************
-/*!\brief Assigning a range of elements to the pointer vector.
- *
- * \param first Iterator to the first element of the element range.
- * \param last Iterator to the element one past the last element of the element range.
- * \return void
- * \exception std::length_error Maximum pointer vector length exceeded.
- *
- * This functions assigns the elements in the range \f$ [first,last) \f$ to the pointer vector.
- * All elements previously contained in the pointer vector are removed. The assign function runs
- * in linear time.
- */
-template< typename T    // Type
-        , typename D    // Deletion policy
-        , typename G >  // Growth policy
-template< typename IteratorType >
-inline void PtrVector<T,D,G>::assign( IteratorType first, IteratorType last )
-{
-   clear();
-   insert( end(), first, last );
-}
-//*************************************************************************************************
-
-
-//*************************************************************************************************
-/*!\brief Inserting an element into the pointer vector.
- *
- * \param pos The position before which the element is inserted.
- * \param p The pointer to be inserted into the pointer vector.
- * \return Iterator to the inserted element.
- * \exception std::length_error Maximum pointer vector length exceeded.
- *
- * The insert function runs in linear time. Note however that inserting elements into a pointer
- * vector can be a relatively time-intensive operation.
- */
-template< typename T    // Type
-        , typename D    // Deletion policy
-        , typename G >  // Growth policy
-inline typename PtrVector<T,D,G>::Iterator PtrVector<T,D,G>::insert( Iterator pos, PointerType p )
-{
-   T** const base = const_cast<T**>( pos.base() );
-   const auto diff( base - begin_ );
-
-   if( size_ != capacity_ && base == end_ ) {
-      *end_ = p;
-      ++end_;
-      ++size_;
-   }
-   else {
-      insert( base, p );
-   }
-
-   return Iterator( begin_+diff );
-}
-//*************************************************************************************************
-
-
-//*************************************************************************************************
-/*!\brief Inserting a range of elements into the pointer vector.
- *
- * \param pos The position before which the elements are inserted.
- * \param first Iterator to the first element of the element range.
- * \param last Iterator to the element one past the last element of the element range.
- * \return void
- * \exception std::length_error Maximum pointer vector length exceeded.
- *
- * This functions inserts the elements in the range \f$ [first,last) \f$ into the pointer vector.
- * The insert function runs in linear time. Note however that inserting elements into a pointer
- * vector can be a relatively time-intensive operation.
- */
-template< typename T    // Type
-        , typename D    // Deletion policy
-        , typename G >  // Growth policy
-template< typename IteratorType >
-inline void PtrVector<T,D,G>::insert( Iterator pos, IteratorType first, IteratorType last )
-{
-   insert( pos, first, last, typename IteratorType::iterator_category() );
-}
-//*************************************************************************************************
-
-
-//*************************************************************************************************
-/*! \cond internal */
-/*!\brief Inserting a range of elements into the pointer vector.
- *
- * \param pos The position before which the elements are inserted.
- * \param first Pointer to the first element of the element range.
- * \param last Pointer to the element one past the last element of the element range.
- * \return void
- * \exception std::length_error Maximum pointer vector length exceeded.
- *
- * This functions inserts the elements in the range \f$ [first,last) \f$ into the pointer vector.
- * The insert function runs in linear time. Note however that inserting elements into a pointer
- * vector can be a relatively time-intensive operation.
- */
-template< typename T    // Type
-        , typename D    // Deletion policy
-        , typename G >  // Growth policy
-template< typename IteratorType >
-inline void PtrVector<T,D,G>::insert( Iterator pos, IteratorType* first, IteratorType* last )
-{
-   insert( pos, first, last, std::random_access_iterator_tag() );
-}
-/*! \endcond */
-//*************************************************************************************************
-
-
-//*************************************************************************************************
-/*!\brief Removing an element from the pointer vector.
- *
- * \param pos The position of the element to be removed.
- * \return Iterator to the element after the erased element.
- *
- * This function erases an element from the pointer vector, i.e. the element is deleted
- * according to the deletion policy of the pointer vector and removed from the vector.
- * Note that in case the deletion policy is NoDelete, this function is identical to the
- * release() function.
- */
-template< typename T    // Type
-        , typename D    // Deletion policy
-        , typename G >  // Growth policy
-inline typename PtrVector<T,D,G>::Iterator PtrVector<T,D,G>::erase( Iterator pos )
-{
-   T** const base = const_cast<T**>( pos.base() );
-   deleteElement( *base );
-   std::copy( base+1, end_, base );
-
-   --size_;
-   --end_;
-
-   return pos;
-}
-//*************************************************************************************************
-
-
-//*************************************************************************************************
-/*!\brief Removing an element from the pointer vector.
- *
- * \param pos The position of the element to be removed.
- * \return Iterator to the element after the erased element.
- *
- * This function erases an element from the pointer vector, i.e. the element is deleted
- * according to the deletion policy of the pointer vector and removed from the vector.
- * Note that in case the deletion policy is NoDelete, this function is identical to the
- * release() function.
- *
- * Note: The cast iterator \a pos is not invalidated but every other cast iterators including those
- * returned by end<C>() are.
- */
-template< typename T    // Type
-        , typename D    // Deletion policy
-        , typename G >  // Growth policy
-template< typename C >  // Cast type
-inline typename PtrVector<T,D,G>::WALBERLA_TEMPLATE CastIterator<C>
-   PtrVector<T,D,G>::erase( CastIterator<C> pos )
-{
-   T** const base = const_cast<T**>( pos.base() );
-   deleteElement( *base );
-   std::copy( base+1, end_, base );
-
-   --size_;
-   --end_;
-
-   return CastIterator<C>( base, end_ );
-}
-//*************************************************************************************************
-
-
-//*************************************************************************************************
-/*!\brief Releasing an element from the pointer vector.
- *
- * \param pos The position of the element to be released.
- * \return Iterator to the element after the released element.
- *
- * This function releases an element from the pointer vector, i.e. the element is removed
- * without applying the deletion policy. Therefore the responsibility to delete the element
- * is passed to the function caller.
- */
-template< typename T    // Type
-        , typename D    // Deletion policy
-        , typename G >  // Growth policy
-inline typename PtrVector<T,D,G>::Iterator PtrVector<T,D,G>::release( Iterator pos )
-{
-   T** const base = const_cast<T**>( pos.base() );
-   std::copy( base+1, end_, base );
-
-   --size_;
-   --end_;
-
-   return pos;
-}
-//*************************************************************************************************
-
-
-//*************************************************************************************************
-/*!\brief Releasing an element from the pointer vector.
- *
- * \param pos The position of the element to be released.
- * \return Iterator to the element after the released element.
- *
- * This function releases an element from the pointer vector, i.e. the element is removed
- * without applying the deletion policy. Therefore the responsibility to delete the element
- * is passed to the function caller.
- */
-template< typename T    // Type
-        , typename D    // Deletion policy
-        , typename G >  // Growth policy
-template< typename C >  // Cast type
-inline typename PtrVector<T,D,G>::WALBERLA_TEMPLATE CastIterator<C>
-   PtrVector<T,D,G>::release( CastIterator<C> pos )
-{
-   T** const base = const_cast<T**>( pos.base() );
-   std::copy( base+1, end_, base );
-
-   --size_;
-   --end_;
-
-   return CastIterator<C>( base, end_ );
-}
-//*************************************************************************************************
-
-
-//*************************************************************************************************
-/*!\brief Removing all elements from the pointer vector.
- *
- * \return void
- */
-template< typename T    // Type
-        , typename D    // Deletion policy
-        , typename G >  // Growth policy
-inline void PtrVector<T,D,G>::clear()
-{
-   for( PointerType* it=begin_; it!=end_; ++it )
-      deleteElement( *it );
-
-   end_  = begin_;
-   size_ = 0;
-}
-//*************************************************************************************************
-
-
-
-
-//=================================================================================================
-//
-//  UTILITY FUNCTIONS
-//
-//=================================================================================================
-
-//*************************************************************************************************
-/*!\brief Setting the minimum capacity of the pointer vector.
- *
- * \param newCapacity The new minimum capacity of the pointer vector.
- * \return void
- */
-template< typename T    // Type
-        , typename D    // Deletion policy
-        , typename G >  // Growth policy
-void PtrVector<T,D,G>::reserve( SizeType newCapacity )
-{
-   if( newCapacity > capacity_ )
-   {
-      // Calculating the new capacity
-      newCapacity = calcCapacity( newCapacity );
-
-      // Allocating a new array
-      PointerType* tmp = new PointerType[newCapacity];
-
-      // Replacing the old array
-      std::copy( begin_, end_, tmp );
-      std::swap( tmp, begin_ );
-      capacity_ = newCapacity;
-      end_ = begin_ + size_;
-      delete [] tmp;
-   }
-}
-//*************************************************************************************************
-
-
-//*************************************************************************************************
-/*!\brief Swapping the contents of two pointer vectors.
- *
- * \param pv The pointer vector to be swapped.
- * \return void
- * \exception no-throw guarantee.
- */
-template< typename T    // Type
-        , typename D    // Deletion policy
-        , typename G >  // Growth policy
-inline void PtrVector<T,D,G>::swap( PtrVector& pv ) /* throw() */
-{
-   // By using the 'std::swap' function to swap all member variables,
-   // the function can give the nothrow guarantee.
-   std::swap( size_, pv.size_ );
-   std::swap( capacity_, pv.capacity_ );
-   std::swap( begin_, pv.begin_ );
-   std::swap( end_, pv.end_ );
-}
-//*************************************************************************************************
-
-
-
-
-//=================================================================================================
-//
-//  HELPER FUNCTIONS
-//
-//=================================================================================================
-
-//*************************************************************************************************
-/*!\brief Calculating the new capacity of the vector based on its growth policy.
- *
- * \param minCapacity The minimum necessary capacity.
- * \return The new capacity.
- */
-template< typename T    // Type
-        , typename D    // Deletion policy
-        , typename G >  // Growth policy
-inline size_t PtrVector<T,D,G>::calcCapacity( size_t minCapacity ) const
-{
-   WALBERLA_ASSERT( minCapacity > capacity_, "Invalid new vector capacity" );
-   const size_t newCapacity( GrowthPolicy()( capacity_, minCapacity ) );
-   WALBERLA_ASSERT( newCapacity > capacity_, "Invalid new vector capacity" );
-   return newCapacity;
-}
-//*************************************************************************************************
-
-
-//*************************************************************************************************
-/*!\brief Deleting an element of the pointer vector according to the deletion policy.
- *
- * \param ptr The element to be deleted.
- * \return void
- */
-template< typename T    // Type
-        , typename D    // Deletion policy
-        , typename G >  // Growth policy
-inline void PtrVector<T,D,G>::deleteElement( PointerType ptr ) const
-{
-   DeletionPolicy()( ptr );
-}
-//*************************************************************************************************
-
-
-
-
-//=================================================================================================
-//
-//  INSERTION HELPER FUNCTIONS
-//
-//=================================================================================================
-
-//*************************************************************************************************
-/*!\brief Inserting an element into the pointer vector.
- *
- * \param pos The position before which the element is inserted.
- * \param p The pointer to be inserted into the pointer vector.
- * \return void
- * \exception std::length_error Maximum pointer vector length exceeded.
- */
-template< typename T    // Type
-        , typename D    // Deletion policy
-        , typename G >  // Growth policy
-void PtrVector<T,D,G>::insert( T**const pos, PointerType p )
-{
-   if( size_ != capacity_ ) {
-      std::copy_backward( pos, end_, end_+1 );
-      *pos = p;
-      ++end_;
-      ++size_;
-   }
-   else if( size_ == maxSize() ) {
-      throw std::length_error( "Maximum pointer vector length exceeded!" );
-   }
-   else {
-      SizeType newCapacity( calcCapacity( capacity_+1 ) );
-      if( newCapacity > maxSize() || newCapacity < capacity_ ) newCapacity = maxSize();
-
-      PointerType* newBegin = new PointerType[newCapacity];
-      PointerType* newEnd = std::copy( begin_, pos, newBegin );
-      *newEnd = p;
-      ++newEnd;
-      end_ = std::copy( pos, end_, newEnd );
-
-      std::swap( newBegin, begin_ );
-      delete [] newBegin;
-      capacity_ = newCapacity;
-      ++size_;
-   }
-}
-//*************************************************************************************************
-
-
-//*************************************************************************************************
-/*! \cond internal */
-/*!\brief Inserting a range of elements into the pointer vector.
- *
- * \param pos The position before which the elements are inserted.
- * \param first Iterator to the first element of the element range.
- * \param last Iterator to the element one past the last element of the element range.
- * \return void
- * \exception std::length_error Maximum pointer vector length exceeded.
- *
- * This functions inserts the elements in the range \f$ [first,last) \f$ into the pointer vector.
- * The iterators are treated as input iterators.
- */
-template< typename T    // Type
-        , typename D    // Deletion policy
-        , typename G >  // Growth policy
-template< typename IteratorType >
-inline void PtrVector<T,D,G>::insert( Iterator pos, IteratorType first, IteratorType last,
-                                      std::input_iterator_tag )
-{
-   for( ; first!=last; ++first ) {
-      pos = insert( pos, *first );
-      ++pos;
-   }
-}
-/*! \endcond */
-//*************************************************************************************************
-
-
-//*************************************************************************************************
-/*! \cond internal */
-/*!\brief Inserting a range of elements into the pointer vector.
- *
- * \param pos The position before which the elements are inserted.
- * \param first Iterator to the first element of the element range.
- * \param last Iterator to the element one past the last element of the element range.
- * \return void
- * \exception std::length_error Maximum pointer vector length exceeded.
- *
- * This functions inserts the elements in the range \f$ [first,last) \f$ into the pointer vector.
- * The iterators are treated as random access iterators.
- */
-template< typename T    // Type
-        , typename D    // Deletion policy
-        , typename G >  // Growth policy
-template< typename IteratorType >
-inline void PtrVector<T,D,G>::insert( Iterator pos, IteratorType first, IteratorType last,
-                                      std::random_access_iterator_tag )
-{
-   T** const base = const_cast<T**>( pos.base() );
-   const SizeType diff( last - first );
-
-   if( size_+diff <= capacity_ && base == end_ ) {
-      for( ; first!=last; ++first, ++end_ ) {
-         *end_ = *first;
-      }
-      size_ += diff;
-   }
-   else {
-      insert( base, first, last, diff );
-   }
-}
-/*! \endcond */
-//*************************************************************************************************
-
-
-//*************************************************************************************************
-/*!\brief Inserting a range of elements into the pointer vector.
- *
- * \param pos The position before which the elements are inserted.
- * \param first Iterator to the first element of the element range.
- * \param last Iterator to the element one past the last element of the element range.
- * \param n The number of elements to be inserted.
- * \return void
- * \exception std::length_error Maximum pointer vector length exceeded.
- */
-template< typename T    // Type
-        , typename D    // Deletion policy
-        , typename G >  // Growth policy
-template< typename IteratorType >
-void PtrVector<T,D,G>::insert( T** pos, IteratorType first, IteratorType last, SizeType n )
-{
-   const SizeType newSize( size_ + n );
-
-   if( newSize <= capacity_ ) {
-      std::copy_backward( pos, end_, end_+n );
-      for( ; first!=last; ++first, ++pos ) {
-         *pos = *first;
-      }
-      end_ += n;
-      size_ = newSize;
-   }
-   else if( newSize > maxSize() || newSize < size_ ) {
-      throw std::length_error( "Maximum pointer vector length exceeded!" );
-   }
-   else {
-      PointerType* newBegin = new PointerType[newSize];
-      PointerType* newEnd = std::copy( begin_, pos, newBegin );
-
-      for( ; first!=last; ++first, ++newEnd ) {
-         *newEnd = *first;
-      }
-
-      end_ = std::copy( pos, end_, newEnd );
-
-      std::swap( newBegin, begin_ );
-      delete [] newBegin;
-      capacity_ = newSize;
-      size_ = newSize;
-   }
-}
-//*************************************************************************************************
-
-
-
-
-//=================================================================================================
-//
-//  GLOBAL OPERATORS
-//
-//=================================================================================================
-
-//*************************************************************************************************
-/*!\name PtrVector operators */
-//@{
-template< typename T, typename D, typename G >
-inline bool operator==( const PtrVector<T,D,G>& lhs, const PtrVector<T,D,G>& rhs );
-
-template< typename T, typename D, typename G >
-inline bool operator!=( const PtrVector<T,D,G>& lhs, const PtrVector<T,D,G>& rhs );
-
-template< typename T, typename D, typename G >
-inline void swap( PtrVector<T,D,G>& a, PtrVector<T,D,G>& b ) /* throw() */;
-//@}
-//*************************************************************************************************
-
-
-//*************************************************************************************************
-/*!\brief Equality comparison between two pointer vectors.
- *
- * \param lhs The left hand side pointer vector.
- * \param rhs The right hand side pointer vector.
- * \return \a true if the two pointer vectors are equal, \a false if they are not.
- */
-template< typename T    // Type
-        , typename D    // Deletion policy
-        , typename G >  // Growth policy
-inline bool operator==( const PtrVector<T,D,G>& lhs, const PtrVector<T,D,G>& rhs )
-{
-   return lhs.size() == rhs.size() && std::equal( lhs.begin(), lhs.end(), rhs.begin() );
-}
-//*************************************************************************************************
-
-
-//*************************************************************************************************
-/*!\brief Inequality comparison between two pointer vectors.
- *
- * \param lhs The left hand side pointer vector.
- * \param rhs The right hand side pointer vector.
- * \return \a true if the two pointer vectors are inequal, \a false if they are not.
- */
-template< typename T    // Type
-        , typename D    // Deletion policy
-        , typename G >  // Growth policy
-inline bool operator!=( const PtrVector<T,D,G>& lhs, const PtrVector<T,D,G>& rhs )
-{
-   return lhs.size() != rhs.size() || !std::equal( lhs.begin(), lhs.end(), rhs.begin() );
-}
-//*************************************************************************************************
-
-
-//*************************************************************************************************
-/*!\brief Swapping the contents of two pointer vectors.
- *
- * \param a The first pointer vector to be swapped.
- * \param b The second pointer vector to be swapped.
- * \return void
- * \exception no-throw guarantee.
- */
-template< typename T    // Type
-        , typename D    // Deletion policy
-        , typename G >  // Growth policy
-inline void swap( PtrVector<T,D,G>& a, PtrVector<T,D,G>& b ) /* throw() */
-{
-   a.swap( b );
-}
-//*************************************************************************************************
-
-
-
-
-
-
-
-
-//=================================================================================================
-//
-//  NESTED CLASS PTRVECTOR::CASTITERATOR
-//
-//=================================================================================================
-
-//*************************************************************************************************
-/*!\brief Dynamic cast iterator for polymorphic pointer vectors.
- * \ingroup util
- *
- * The CastIterator class is part of the PtrVector class and represent a forward iterator
- * over all elements of type \a C contained in a range of elements of type \a T, where \a C
- * is a type derived from \a T.
-
-   \code
-   class A { ... };
-   class B : public class A { ... };
-
-   PtrVector<A>::CastIterator<B> begin;
-   PtrVector<A>::CastIterator<B> end;
-
-   // Loop over all elements of type B within the range [begin..end)
-   for( ; begin!=end; ++begin )
-      ...
-   \endcode
-
- * \b Note: Using a CastIterator is computationally more expensive than using a standard
- * iterator over all elements contained in the vector.
- */
-template< typename T    // Type
-        , typename D    // Deletion policy
-        , typename G >  // Growth policy
-template< typename C >  // Cast type
-class PtrVector<T,D,G>::CastIterator
-{
-public:
-   //**Type definitions****************************************************************************
-   // pe naming convention
-   typedef std::forward_iterator_tag  IteratorCategory;   //!< The iterator category.
-   typedef C*                         ValueType;          //!< Type of the underlying pointers.
-   typedef C*                         PointerType;        //!< Pointer return type.
-   typedef C* const&                  ReferenceType;      //!< Reference return type.
-   typedef ptrdiff_t                  DifferenceType;     //!< Difference between two iterators.
-   typedef T* const*                  IteratorType;       //!< Type of the internal pointer.
-
-   // STL iterator requirements
-   typedef IteratorCategory           iterator_category;  //!< The iterator category.
-   typedef ValueType                  value_type;         //!< Type of the underlying pointers.
-   typedef PointerType                pointer;            //!< Pointer return type.
-   typedef ReferenceType              reference;          //!< Reference return type.
-   typedef DifferenceType             difference_type;    //!< Difference between two iterators.
-   //**********************************************************************************************
-
-   //**Constructors********************************************************************************
-   /*!\name Constructors */
-   //@{
-   inline CastIterator();
-   inline CastIterator( IteratorType begin, IteratorType end );
-
-   template< typename Other >
-   inline CastIterator( const CastIterator<Other>& it );
-
-   // No explicitly declared copy constructor.
-   //@}
-   //**********************************************************************************************
-
-   //**Destructor**********************************************************************************
-   // No explicitly declared destructor.
-   //**********************************************************************************************
-
-   //**Copy assignment operator********************************************************************
-   // No explicitly declared copy assignment operator.
-   //**********************************************************************************************
-
-   //**Operators***********************************************************************************
-   /*!\name Operators */
-   //@{
-   inline CastIterator& operator++();
-   inline CastIterator  operator++( int );
-   //@}
-   //**********************************************************************************************
-
-   //**Access operators****************************************************************************
-   /*!\name Access operators */
-   //@{
-   inline PointerType operator*()  const;
-   inline PointerType operator->() const;
-   //@}
-   //**********************************************************************************************
-
-   //**Utility functions***************************************************************************
-   /*!\name Utility functions */
-   //@{
-   inline const IteratorType& base() const;
-   inline const IteratorType& stop() const;
-   //@}
-   //**********************************************************************************************
-
-private:
-   //**Member variables****************************************************************************
-   /*!\name Member variables */
-   //@{
-   IteratorType cur_;  //!< Pointer to the current memory location.
-   IteratorType end_;  //!< Pointer to the element one past the last element in the element range.
-   //@}
-   //**********************************************************************************************
-};
-//*************************************************************************************************
-
-
-
-
-//=================================================================================================
-//
-//  CONSTRUCTOR
-//
-//=================================================================================================
-
-//*************************************************************************************************
-/*!\brief Default constructor for CastIterator.
- */
-template< typename T    // Type
-        , typename D    // Deletion policy
-        , typename G >  // Growth policy
-template< typename C >  // Cast type
-inline PtrVector<T,D,G>::CastIterator<C>::CastIterator()
-   : cur_(NULL)  // Pointer to the current memory location
-   , end_(NULL)  // Pointer to the element one past the last element in the element range
-{
-   static_assert(boost::is_base_of<T, C>::value && !boost::is_base_of<C, T>::value, "C has to be strictly derived from T");
-}
-//*************************************************************************************************
-
-
-//*************************************************************************************************
-/*!\brief Standard constructor for CastIterator.
- *
- * \param begin The beginning of the element range.
- * \param end The end of the element range.
- */
-template< typename T    // Type
-        , typename D    // Deletion policy
-        , typename G >  // Growth policy
-template< typename C >  // Cast type
-inline PtrVector<T,D,G>::CastIterator<C>::CastIterator( IteratorType begin, IteratorType end )
-   : cur_(begin)  // Pointer to the current memory location
-   , end_(end)    // Pointer to the element one past the last element in the element range
-{
-   static_assert(boost::is_base_of<T, C>::value && !boost::is_base_of<C, T>::value, "C has to be strictly derived from T");
-   // The polymorphicFind() function finds the next pointer to an object with dynamic type 'C'
-   // contained in the range [cur_,end). An equivalent code might look like this:
-   //
-   // while( cur_ != end_ && !dynamic_cast<C*>( *cur_ ) ) ++cur_;
-   //
-   // However, the specialization of polymorphicFind() for special type combinations is much
-   // more efficient (and way easier!) than the specialization of this function!
-   cur_ = polymorphicFind<C>( cur_, end_ );
-}
-//*************************************************************************************************
-
-
-//*************************************************************************************************
-/*!\brief Conversion constructor from different CastIterator instances.
- *
- * \param it The foreign CastIterator instance to be copied.
- */
-template< typename T        // Type
-        , typename D        // Deletion policy
-        , typename G >      // Growth policy
-template< typename C >      // Cast type
-template< typename Other >  // The foreign cast iterator type
-inline PtrVector<T,D,G>::CastIterator<C>::CastIterator( const CastIterator<Other>& it )
-   : cur_( it.base() )  // Pointer to the current memory location
-   , end_( it.stop() )  // Pointer to the element one past the last element in the element range
-{
-   static_assert(boost::is_base_of<C, T>::value && !boost::is_base_of<C, T>::value, "C has to be strictly derived from T");
-   static_assert(boost::is_convertible<Other*, C*>::value,  "Other must be convertible to C" );
-}
-//*************************************************************************************************
-
-
-
-
-//=================================================================================================
-//
-//  OPERATORS
-//
-//=================================================================================================
-
-//*************************************************************************************************
-/*!\brief Pre-increment operator.
- *
- * \return Reference to the incremented cast iterator.
- */
-template< typename T    // Type
-        , typename D    // Deletion policy
-        , typename G >  // Growth policy
-template< typename C >  // Cast type
-inline typename PtrVector<T,D,G>::WALBERLA_TEMPLATE CastIterator<C>&
-   PtrVector<T,D,G>::CastIterator<C>::operator++()
-{
-   // The polymorphicFind() function finds the next pointer to an object with dynamic type 'C'
-   // contained in the range [cur_+1,end). An equivalent code might look like this:
-   //
-   // while( ++cur_ != end_ && !dynamic_cast<C*>( *cur_ ) ) {}
-   //
-   // However, the specialization of polymorphicFind() for special type combinations is much
-   // more efficient (and way easier!) than the specialization of this function!
-   cur_ = polymorphicFind<C>( ++cur_, end_ );
-
-   return *this;
-}
-//*************************************************************************************************
-
-
-//*************************************************************************************************
-/*!\brief Post-increment operator.
- *
- * \return The incremented cast iterator.
- */
-template< typename T    // Type
-        , typename D    // Deletion policy
-        , typename G >  // Growth policy
-template< typename C >  // Cast type
-inline typename PtrVector<T,D,G>::WALBERLA_TEMPLATE CastIterator<C>
-   PtrVector<T,D,G>::CastIterator<C>::operator++( int )
-{
-   CastIterator tmp( *this );
-
-   // The polymorphicFind() function finds the next pointer to an object with dynamic type 'C'
-   // contained in the range [cur_+1,end). An equivalent code might look like this:
-   //
-   // while( ++cur_ != end_ && !dynamic_cast<C*>( *cur_ ) ) {}
-   //
-   // However, the specialization of polymorphicFind() for special type combinations is much
-   // more efficient (and way easier!) than the specialization of this function!
-   cur_ = polymorphicFind<C>( ++cur_, end_ );
-
-   return tmp;
-}
-//*************************************************************************************************
-
-
-
-
-//=================================================================================================
-//
-//  ACCESS OPERATORS
-//
-//=================================================================================================
-
-//*************************************************************************************************
-/*!\brief Returns a handle to the element at the current iterator position.
- *
- * \return Handle to the element at the current iterator position.
- */
-template< typename T    // Type
-        , typename D    // Deletion policy
-        , typename G >  // Growth policy
-template< typename C >  // Cast type
-inline typename PtrVector<T,D,G>::WALBERLA_TEMPLATE CastIterator<C>::PointerType
-   PtrVector<T,D,G>::CastIterator<C>::operator*() const
-{
-   return static_cast<C*>( *cur_ );
-}
-//*************************************************************************************************
-
-
-//*************************************************************************************************
-/*!\brief Direct access to the element at the current iterator position.
- *
- * \return Reference to the element at the current iterator position.
- */
-template< typename T    // Type
-        , typename D    // Deletion policy
-        , typename G >  // Growth policy
-template< typename C >  // Cast type
-inline typename PtrVector<T,D,G>::WALBERLA_TEMPLATE CastIterator<C>::PointerType
-   PtrVector<T,D,G>::CastIterator<C>::operator->() const
-{
-   return static_cast<C*>( *cur_ );
-}
-//*************************************************************************************************
-
-
-
-
-//=================================================================================================
-//
-//  UTILITY FUNCTIONS
-//
-//=================================================================================================
-
-//*************************************************************************************************
-/*!\brief Direct access to the current memory location of the cast iterator.
- *
- * \return Pointer to the current memory location.
- */
-template< typename T    // Type
-        , typename D    // Deletion policy
-        , typename G >  // Growth policy
-template< typename C >  // Cast type
-inline const typename PtrVector<T,D,G>::WALBERLA_TEMPLATE CastIterator<C>::IteratorType&
-   PtrVector<T,D,G>::CastIterator<C>::base() const
-{
-   return cur_;
-}
-//*************************************************************************************************
-
-
-//*************************************************************************************************
-/*!\brief Direct access to the final memory location of the cast iterator.
- *
- * \return Pointer to the final memory location.
- */
-template< typename T    // Type
-        , typename D    // Deletion policy
-        , typename G >  // Growth policy
-template< typename C >  // Cast type
-inline const typename PtrVector<T,D,G>::WALBERLA_TEMPLATE CastIterator<C>::IteratorType&
-   PtrVector<T,D,G>::CastIterator<C>::stop() const
-{
-   return end_;
-}
-//*************************************************************************************************
-
-
-
-
-
-
-
-
-//=================================================================================================
-//
-//  NESTED CLASS PTRVECTOR::CONSTCASTITERATOR
-//
-//=================================================================================================
-
-//*************************************************************************************************
-/*!\brief Dynamic cast iterator for polymorphic pointer vectors.
- * \ingroup util
- *
- * The ConstCastIterator class is part of the PtrVector class and represent a forward iterator
- * over all elements of type \a C contained in a range of elements of type \a T, where \a C
- * is a type derived from \a T. The ConstCastIterator is the counterpart of CastIterator for
- * constant vectors.
-
-   \code
-   class A { ... };
-   class B : public class A { ... };
-
-   PtrVector<A>::ConstCastIterator<B> begin;
-   PtrVector<A>::ConstCastIterator<B> end;
-
-   // Loop over all elements of type B within the range [begin..end)
-   for( ; begin!=end; ++begin )
-      ...
-   \endcode
-
- * \b Note: Using a ConstCastIterator is computationally more expensive than using a standard
- * iterator over all elements contained in the vector.
- */
-template< typename T    // Type
-        , typename D    // Deletion policy
-        , typename G >  // Growth policy
-template< typename C >  // Cast type
-class PtrVector<T,D,G>::ConstCastIterator
-{
-public:
-   //**Type definitions****************************************************************************
-   // pe naming convention
-   typedef std::forward_iterator_tag  IteratorCategory;   //!< The iterator category.
-   typedef const C*                   ValueType;          //!< Type of the underlying pointers.
-   typedef const C*                   PointerType;        //!< Pointer return type.
-   typedef const C* const&            ReferenceType;      //!< Reference return type.
-   typedef ptrdiff_t                  DifferenceType;     //!< Difference between two iterators.
-   typedef const T* const*            IteratorType;       //!< Type of the internal pointer.
-
-   // STL iterator requirements
-   typedef IteratorCategory           iterator_category;  //!< The iterator category.
-   typedef ValueType                  value_type;         //!< Type of the underlying pointers.
-   typedef PointerType                pointer;            //!< Pointer return type.
-   typedef ReferenceType              reference;          //!< Reference return type.
-   typedef DifferenceType             difference_type;    //!< Difference between two iterators.
-   //**********************************************************************************************
-
-   //**Constructors********************************************************************************
-   /*!\name Constructors */
-   //@{
-   inline ConstCastIterator();
-   inline ConstCastIterator( IteratorType begin, IteratorType end );
-
-   template< typename Other >
-   inline ConstCastIterator( const ConstCastIterator<Other>& it );
-
-   template< typename Other >
-   inline ConstCastIterator( const typename PtrVector<T,D,G>::WALBERLA_TEMPLATE CastIterator<Other>& it );
-
-   // No explicitly declared copy constructor.
-   //@}
-   //**********************************************************************************************
-
-   //**Destructor**********************************************************************************
-   // No explicitly declared destructor.
-   //**********************************************************************************************
-
-   //**Copy assignment operator********************************************************************
-   // No explicitly declared copy assignment operator.
-   //**********************************************************************************************
-
-   //**Operators***********************************************************************************
-   /*!\name Operators */
-   //@{
-   inline ConstCastIterator& operator++();
-   inline ConstCastIterator  operator++( int );
-   //@}
-   //**********************************************************************************************
-
-   //**Access operators****************************************************************************
-   /*!\name Access operators */
-   //@{
-   inline PointerType operator*()  const;
-   inline PointerType operator->() const;
-   //@}
-   //**********************************************************************************************
-
-   //**Utility functions***************************************************************************
-   /*!\name Utility functions */
-   //@{
-   inline const IteratorType& base() const;
-   inline const IteratorType& stop() const;
-   //@}
-   //**********************************************************************************************
-
-private:
-   //**Member variables****************************************************************************
-   /*!\name Member variables */
-   //@{
-   IteratorType cur_;  //!< Pointer to the current memory location.
-   IteratorType end_;  //!< Pointer to the element one past the last element in the element range.
-   //@}
-   //**********************************************************************************************
-};
-//*************************************************************************************************
-
-
-
-
-//=================================================================================================
-//
-//  CONSTRUCTOR
-//
-//=================================================================================================
-
-//*************************************************************************************************
-/*!\brief Default constructor for ConstCastIterator.
- */
-template< typename T    // Type
-        , typename D    // Deletion policy
-        , typename G >  // Growth policy
-template< typename C >  // Cast type
-inline PtrVector<T,D,G>::ConstCastIterator<C>::ConstCastIterator()
-   : cur_(NULL)  // Pointer to the current memory location
-   , end_(NULL)  // Pointer to the element one past the last element in the element range
-{
-   static_assert(boost::is_base_of<C, T>::value && !boost::is_base_of<C, T>::value, "C has to be strictly derived from T");
-}
-//*************************************************************************************************
-
-
-//*************************************************************************************************
-/*!\brief Standard constructor for ConstCastIterator.
- *
- * \param begin The beginning of the element range.
- * \param end The end of the element range.
- */
-template< typename T    // Type
-        , typename D    // Deletion policy
-        , typename G >  // Growth policy
-template< typename C >  // Cast type
-inline PtrVector<T,D,G>::ConstCastIterator<C>::ConstCastIterator( IteratorType begin, IteratorType end )
-   : cur_(begin)  // Pointer to the current memory location
-   , end_(end)    // Pointer to the element one past the last element in the element range
-{
-   // The polymorphicFind() function finds the next pointer to an object with dynamic type 'C'
-   // contained in the range [cur_,end). An equivalent code might look like this:
-   //
-   // while( cur_ != end_ && !dynamic_cast<C*>( *cur_ ) ) ++cur_;
-   //
-   // However, the specialization of polymorphicFind() for special type combinations is much
-   // more efficient (and way easier!) than the specialization of this function!
-   cur_ = polymorphicFind<C>( cur_, end_ );
-}
-//*************************************************************************************************
-
-
-//*************************************************************************************************
-/*!\brief Conversion constructor from different ConstCastIterator instances.
- *
- * \param it The foreign ConstCastIterator instance to be copied.
- */
-template< typename T        // Type
-        , typename D        // Deletion policy
-        , typename G >      // Growth policy
-template< typename C >      // Cast type
-template< typename Other >  // The foreign constant cast iterator type
-inline PtrVector<T,D,G>::ConstCastIterator<C>::ConstCastIterator( const ConstCastIterator<Other>& it )
-   : cur_( it.base() )  // Pointer to the current memory location
-   , end_( it.stop() )  // Pointer to the element one past the last element in the element range
-{
-    static_assert(boost::is_base_of<C, T>::value && !boost::is_base_of<C, T>::value, "C has to be strictly derived from T");
-    static_assert(boost::is_convertible<Other*, C*>::value,  "Other must be convertible to C" );
-}
-//*************************************************************************************************
-
-
-//*************************************************************************************************
-/*!\brief Conversion constructor from CastIterator instances.
- *
- * \param it The foreign CastIterator instance to be copied.
- */
-template< typename T        // Type
-        , typename D        // Deletion policy
-        , typename G >      // Growth policy
-template< typename C >      // Cast type
-template< typename Other >  // The foreign cast iterator type
-inline PtrVector<T,D,G>::ConstCastIterator<C>::ConstCastIterator( const typename PtrVector<T,D,G>::WALBERLA_TEMPLATE CastIterator<Other>& it )
-   : cur_( it.base() )  // Pointer to the current memory location
-   , end_( it.stop() )  // Pointer to the element one past the last element in the element range
-{
-    static_assert(boost::is_base_of<C, T>::value && !boost::is_base_of<C, T>::value, "C has to be strictly derived from T");
-    static_assert(boost::is_convertible<Other*, C*>::value,  "Other must be convertible to C" );
-}
-//*************************************************************************************************
-
-
-
-
-//=================================================================================================
-//
-//  OPERATORS
-//
-//=================================================================================================
-
-//*************************************************************************************************
-/*!\brief Pre-increment operator.
- *
- * \return Reference to the incremented cast iterator.
- */
-template< typename T    // Type
-        , typename D    // Deletion policy
-        , typename G >  // Growth policy
-template< typename C >  // Cast type
-inline typename PtrVector<T,D,G>::WALBERLA_TEMPLATE ConstCastIterator<C>&
-   PtrVector<T,D,G>::ConstCastIterator<C>::operator++()
-{
-   // The polymorphicFind() function finds the next pointer to an object with dynamic type 'C'
-   // contained in the range [cur_+1,end). An equivalent code might look like this:
-   //
-   // while( ++cur_ != end_ && !dynamic_cast<const C*>( *cur_ ) ) {}
-   //
-   // However, the specialization of polymorphicFind() for special type combinations is much
-   // more efficient (and way easier!) than the specialization of this function!
-   cur_ = polymorphicFind<const C>( ++cur_, end_ );
-
-   return *this;
-}
-//*************************************************************************************************
-
-
-//*************************************************************************************************
-/*!\brief Post-increment operator.
- *
- * \return The incremented cast iterator.
- */
-template< typename T    // Type
-        , typename D    // Deletion policy
-        , typename G >  // Growth policy
-template< typename C >  // Cast type
-inline typename PtrVector<T,D,G>::WALBERLA_TEMPLATE ConstCastIterator<C>
-   PtrVector<T,D,G>::ConstCastIterator<C>::operator++( int )
-{
-   ConstCastIterator tmp( *this );
-
-   // The polymorphicFind() function finds the next pointer to an object with dynamic type 'C'
-   // contained in the range [cur_+1,end). An equivalent code might look like this:
-   //
-   // while( ++cur_ != end_ && !dynamic_cast<const C*>( *cur_ ) ) {}
-   //
-   // However, the specialization of polymorphicFind() for special type combinations is much
-   // more efficient (and way easier!) than the specialization of this function!
-   cur_ = polymorphicFind<const C>( ++cur_, end_ );
-
-   return tmp;
-}
-//*************************************************************************************************
-
-
-
-
-//=================================================================================================
-//
-//  ACCESS OPERATORS
-//
-//=================================================================================================
-
-//*************************************************************************************************
-/*!\brief Returns a handle to the element at the current iterator position.
- *
- * \return Handle to the element at the current iterator position.
- */
-template< typename T    // Type
-        , typename D    // Deletion policy
-        , typename G >  // Growth policy
-template< typename C >  // Cast type
-inline typename PtrVector<T,D,G>::WALBERLA_TEMPLATE ConstCastIterator<C>::PointerType
-   PtrVector<T,D,G>::ConstCastIterator<C>::operator*() const
-{
-   return static_cast<const C*>( *cur_ );
-}
-//*************************************************************************************************
-
-
-//*************************************************************************************************
-/*!\brief Direct access to the element at the current iterator position.
- *
- * \return Reference to the element at the current iterator position.
- */
-template< typename T    // Type
-        , typename D    // Deletion policy
-        , typename G >  // Growth policy
-template< typename C >  // Cast type
-inline typename PtrVector<T,D,G>::WALBERLA_TEMPLATE ConstCastIterator<C>::PointerType
-   PtrVector<T,D,G>::ConstCastIterator<C>::operator->() const
-{
-   return static_cast<const C*>( *cur_ );
-}
-//*************************************************************************************************
-
-
-
-
-//=================================================================================================
-//
-//  UTILITY FUNCTIONS
-//
-//=================================================================================================
-
-//*************************************************************************************************
-/*!\brief Direct access to the current memory location of the constant cast iterator.
- *
- * \return Pointer to the current memory location.
- */
-template< typename T    // Type
-        , typename D    // Deletion policy
-        , typename G >  // Growth policy
-template< typename C >  // Cast type
-inline const typename PtrVector<T,D,G>::WALBERLA_TEMPLATE ConstCastIterator<C>::IteratorType&
-   PtrVector<T,D,G>::ConstCastIterator<C>::base() const
-{
-   return cur_;
-}
-//*************************************************************************************************
-
-
-//*************************************************************************************************
-/*!\brief Direct access to the final memory location of the constant cast iterator.
- *
- * \return Pointer to the final memory location.
- */
-template< typename T    // Type
-        , typename D    // Deletion policy
-        , typename G >  // Growth policy
-template< typename C >  // Cast type
-inline const typename PtrVector<T,D,G>::WALBERLA_TEMPLATE ConstCastIterator<C>::IteratorType&
-   PtrVector<T,D,G>::ConstCastIterator<C>::stop() const
-{
-   return end_;
-}
-//*************************************************************************************************
-}
diff --git a/src/core/ptrvector/policies/ArrayDelete.h b/src/core/ptrvector/policies/ArrayDelete.h
deleted file mode 100644
index bf6b605abc8d82a6612b9943aa596ba4fbb2bae6..0000000000000000000000000000000000000000
--- a/src/core/ptrvector/policies/ArrayDelete.h
+++ /dev/null
@@ -1,88 +0,0 @@
-//======================================================================================================================
-//
-//  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 ArrayDelete.h
-//! \ingroup core
-//! \author Klaus Iglberger
-//! \author Sebastian Eibl <sebastian.eibl@fau.de>
-//
-//======================================================================================================================
-
-#pragma once
-
-
-//*************************************************************************************************
-// Includes
-//*************************************************************************************************
-
-#include <boost/checked_delete.hpp>
-
-namespace walberla {
-
-//=================================================================================================
-//
-//  CLASS DEFINITION
-//
-//=================================================================================================
-
-//*************************************************************************************************
-/*!\brief Array-delete policy class.
- * \ingroup util
- *
- * The ArrayDelete policy functor class applies an array delete operation to the given argument.
- * Note that the array delete operation is NOT permitted for inclomplete types (i.e. declared
- * but undefined data types). The attempt to apply an ArrayDelete functor to a pointer to an
- * array of objects of incomplete type results in a compile time error!
- */
-struct ArrayDelete
-{
-   //**Utility functions***************************************************************************
-   /*!\name Utility functions */
-   //@{
-   template< typename Type >
-   inline void operator()( Type ptr ) const;
-   //@}
-   //**********************************************************************************************
-};
-//*************************************************************************************************
-
-
-
-
-//=================================================================================================
-//
-//  UTILITY FUNCTIONS
-//
-//=================================================================================================
-
-//*************************************************************************************************
-/*!\brief Implementation of the array-delete policy.
- *
- * \param ptr The pointer to the array to be deleted.
- * \return void
- *
- * This function applies an array delete operation to the given argument. Note that the array
- * delete operation is NOT permitted for inclomplete types (i.e. declared but undefined data
- * types). The attempt to use this function for a pointer to an array of objects of incomplete
- * type results in a compile time error!
- */
-template< typename Type >
-inline void ArrayDelete::operator()( Type ptr ) const
-{
-   boost::checked_array_delete( ptr );
-}
-//*************************************************************************************************
-
-} // namespace
diff --git a/src/core/ptrvector/policies/ConstantGrowth.h b/src/core/ptrvector/policies/ConstantGrowth.h
deleted file mode 100644
index 2d20250ca78ac865eac5a1d362c35258f28ddaf2..0000000000000000000000000000000000000000
--- a/src/core/ptrvector/policies/ConstantGrowth.h
+++ /dev/null
@@ -1,94 +0,0 @@
-//======================================================================================================================
-//
-//  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 ConstantGrowth.h
-//! \ingroup core
-//! \author Klaus Iglberger
-//! \author Sebastian Eibl <sebastian.eibl@fau.de>
-//
-//======================================================================================================================
-
-#pragma once
-
-#include <core/DataTypes.h>
-
-//*************************************************************************************************
-// Includes
-//*************************************************************************************************
-
-namespace walberla {
-
-//=================================================================================================
-//
-//  CLASS DEFINITION
-//
-//=================================================================================================
-
-//*************************************************************************************************
-/*!\brief Constant growth policy class.
- * \ingroup util
- *
- * The ConstantGrowth policy class implements a constant growth strategy. It can be customized
- * for any purpose: the \a Growth template argument specifies the constant increase of the given
- * size.
- */
-template< size_t Growth >
-struct ConstantGrowth
-{
-   //**Utility functions***************************************************************************
-   /*!\name Utility functions */
-   //@{
-   inline size_t operator()( size_t oldSize, size_t minSize ) const;
-   //@}
-   //**********************************************************************************************
-};
-//*************************************************************************************************
-
-
-//*************************************************************************************************
-/*! \cond internal */
-/*!\brief Specialization of the constant growth policy for 0 growth.
- * \ingroup util
- */
-template<>
-struct ConstantGrowth<0>;
-/*! \endcond */
-//*************************************************************************************************
-
-
-
-
-//=================================================================================================
-//
-//  UTILITY FUNCTIONS
-//
-//=================================================================================================
-
-//*************************************************************************************************
-/*!\brief Returns a new size depending on the given old size and the required minimum size.
- *
- * \param old The old size.
- * \param minimum The required minimum size.
- * \return The new size (at least the required minimum size).
- */
-template< size_t Growth >
-inline size_t ConstantGrowth<Growth>::operator()( size_t old, size_t minimum ) const
-{
-   const size_t needed( math::max<size_t>( old+Growth, minimum ) );
-   return ( ( needed )?( 4 * ( (needed-1)/4+1 ) ):( 0 ) );
-}
-//*************************************************************************************************
-
-} // namespace
diff --git a/src/core/ptrvector/policies/LinearGrowth.h b/src/core/ptrvector/policies/LinearGrowth.h
deleted file mode 100644
index 314b79380525dda287747d986be18a28a15010c4..0000000000000000000000000000000000000000
--- a/src/core/ptrvector/policies/LinearGrowth.h
+++ /dev/null
@@ -1,106 +0,0 @@
-//======================================================================================================================
-//
-//  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 LinearGrowth.h
-//! \ingroup core
-//! \author Klaus Iglberger
-//! \author Sebastian Eibl <sebastian.eibl@fau.de>
-//
-//======================================================================================================================
-
-#pragma once
-
-
-//*************************************************************************************************
-// Includes
-//*************************************************************************************************
-
-#include <core/math/Utility.h>
-#include <core/DataTypes.h>
-
-namespace walberla {
-
-//=================================================================================================
-//
-//  CLASS DEFINITION
-//
-//=================================================================================================
-
-//*************************************************************************************************
-/*!\brief Linear growth policy class.
- * \ingroup util
- *
- * The LinearGrowth policy class implements a linear growth strategy. It can be customized for
- * any purpose: the \a Growth template argument specifies the factor of the size growth.
- */
-template< size_t Growth >
-struct LinearGrowth
-{
-   //**Utility functions***************************************************************************
-   /*!\name Utility functions */
-   //@{
-   inline size_t operator()( size_t oldSize, size_t minSize ) const;
-   //@}
-   //**********************************************************************************************
-};
-//*************************************************************************************************
-
-
-//*************************************************************************************************
-/*! \cond internal */
-/*!\brief Specialization of the linear growth policy for 0 growth.
- * \ingroup util
- */
-template<>
-struct LinearGrowth<0>;
-/*! \endcond */
-//*************************************************************************************************
-
-
-//*************************************************************************************************
-/*! \cond internal */
-/*!\brief Specialization of the linear growth policy for 1 growth.
- * \ingroup util
- */
-template<>
-struct LinearGrowth<1>;
-/*! \endcond */
-//*************************************************************************************************
-
-
-
-
-//=================================================================================================
-//
-//  UTILITY FUNCTIONS
-//
-//=================================================================================================
-
-//*************************************************************************************************
-/*!\brief Returns a new size depending on the given old size and the required minimum size.
- *
- * \param old The old size.
- * \param minimum The required minimum size.
- * \return The new size (at least the required minimum size).
- */
-template< size_t Growth >
-inline size_t LinearGrowth<Growth>::operator()( size_t old, size_t minimum ) const
-{
-   const size_t needed( math::max<size_t>( old*Growth, minimum ) );
-   return ( ( needed )?( 4 * ( (needed-1)/4+1 ) ):( 0 ) );
-}
-//*************************************************************************************************
-
-} // namespace
diff --git a/src/core/ptrvector/policies/NoDelete.h b/src/core/ptrvector/policies/NoDelete.h
deleted file mode 100644
index 14b24aae3c1b48891a0b39ed02be7366b7d6a978..0000000000000000000000000000000000000000
--- a/src/core/ptrvector/policies/NoDelete.h
+++ /dev/null
@@ -1,69 +0,0 @@
-//======================================================================================================================
-//
-//  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 NoDelete.h
-//! \ingroup core
-//! \author Klaus Iglberger
-//! \author Sebastian Eibl <sebastian.eibl@fau.de>
-//
-//======================================================================================================================
-
-#pragma once
-
-namespace walberla {
-
-//=================================================================================================
-//
-//  CLASS DEFINITION
-//
-//=================================================================================================
-
-//*************************************************************************************************
-/*!\brief No-delete policy class.
- * \ingroup util
- */
-struct NoDelete
-{
-   //**Utility functions***************************************************************************
-   /*!\name Utility functions */
-   //@{
-   template< typename Type >
-   inline void operator()( const Type& ptr ) const;
-   //@}
-   //**********************************************************************************************
-};
-//*************************************************************************************************
-
-
-
-
-//=================================================================================================
-//
-//  UTILITY FUNCTIONS
-//
-//=================================================================================================
-
-//*************************************************************************************************
-/*!\brief Implementation of the no-delete policy.
- *
- * \param ptr The pointer to delete.
- * \return void
- */
-template< typename Type >
-inline void NoDelete::operator()( const Type& /*ptr*/ ) const
-{}
-//*************************************************************************************************
-
-} // namespace
diff --git a/src/core/ptrvector/policies/OptimalGrowth.h b/src/core/ptrvector/policies/OptimalGrowth.h
deleted file mode 100644
index 59abed396b4ac6dccb2a35077f49f085e465c3c7..0000000000000000000000000000000000000000
--- a/src/core/ptrvector/policies/OptimalGrowth.h
+++ /dev/null
@@ -1,85 +0,0 @@
-//======================================================================================================================
-//
-//  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 OptimalGrowth.h
-//! \ingroup core
-//! \author Klaus Iglberger
-//! \author Sebastian Eibl <sebastian.eibl@fau.de>
-//
-//======================================================================================================================
-
-#pragma once
-
-
-//*************************************************************************************************
-// Includes
-//*************************************************************************************************
-
-#include <core/math/Utility.h>
-#include <core/DataTypes.h>
-
-namespace walberla {
-
-//=================================================================================================
-//
-//  CLASS DEFINITION
-//
-//=================================================================================================
-
-//*************************************************************************************************
-/*!\brief Optimal growth policy class.
- * \ingroup util
- *
- * The OptimalGrowth policy class implements the optimal growth strategy suggested by Andrew
- * Koenig for the std::vector class (see Andrew Koenig's column in the September 1998 issue of
- * JOOP (Journal of Object-Oriented Programming), or the Dr. Dobb's article 'C++ Made Easier:
- * How Vectors Grow', 2001). It applies an exponential growth strategy using a factor of 1.5
- * and additionally ensures that the sizes returns are always multiples of four.
- */
-struct OptimalGrowth
-{
-   //**Utility functions***************************************************************************
-   /*!\name Utility functions */
-   //@{
-   inline size_t operator()( size_t oldSize, size_t minSize ) const;
-   //@}
-   //**********************************************************************************************
-};
-//*************************************************************************************************
-
-
-
-
-//=================================================================================================
-//
-//  UTILITY FUNCTIONS
-//
-//=================================================================================================
-
-//*************************************************************************************************
-/*!\brief Returns a new size depending on the given old size and the required minimum size.
- *
- * \param old The old size.
- * \param minimum The required minimum size.
- * \return The new size (at least the required minimum size).
- */
-inline size_t OptimalGrowth::operator()( size_t old, size_t minimum ) const
-{
-   const size_t needed( math::max( static_cast<size_t>( real_c(old)*1.5 ), minimum ) );
-   return ( ( needed )?( 4 * ( (needed-1)/4 + 1 ) ):( 0 ) );
-}
-//*************************************************************************************************
-
-} // namespace
diff --git a/src/core/ptrvector/policies/PtrDelete.h b/src/core/ptrvector/policies/PtrDelete.h
deleted file mode 100644
index 1940275e2ece96848af8982873160b8bb41c7cd8..0000000000000000000000000000000000000000
--- a/src/core/ptrvector/policies/PtrDelete.h
+++ /dev/null
@@ -1,88 +0,0 @@
-//======================================================================================================================
-//
-//  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 PtrDelete.h
-//! \ingroup core
-//! \author Klaus Iglberger
-//! \author Sebastian Eibl <sebastian.eibl@fau.de>
-//
-//======================================================================================================================
-
-#pragma once
-
-
-//*************************************************************************************************
-// Includes
-//*************************************************************************************************
-
-#include <boost/checked_delete.hpp>
-
-namespace walberla {
-
-//=================================================================================================
-//
-//  CLASS DEFINITION
-//
-//=================================================================================================
-
-//*************************************************************************************************
-/*!\brief Pointer-delete policy class.
- * \ingroup util
- *
- * The PtrDelete policy functor class applies a delete operation to the given argument. Note that
- * the delete operation is NOT permitted for inclomplete types (i.e. declared but undefined data
- * types). The attempt to apply a PtrDelete functor to a pointer to an object of incomplete type
- * results in a compile time error!
- */
-struct PtrDelete
-{
-   //**Utility functions***************************************************************************
-   /*!\name Utility functions */
-   //@{
-   template< typename Type >
-   inline void operator()( Type ptr ) const;
-   //@}
-   //**********************************************************************************************
-};
-//*************************************************************************************************
-
-
-
-
-//=================================================================================================
-//
-//  UTILITY FUNCTIONS
-//
-//=================================================================================================
-
-//*************************************************************************************************
-/*!\brief Implementation of the pointer-delete policy.
- *
- * \param ptr The pointer to delete.
- * \return void
- *
- * This function applies a standard delete operation to the given argument. Note that the delete
- * operation is NOT permitted for inclomplete types (i.e. declared but undefined data types). The
- * attempt to use this function for a pointer to an object of incomplete type results in a compile
- * time error!
- */
-template< typename Type >
-inline void PtrDelete::operator()( Type ptr ) const
-{
-   boost::checked_delete( ptr );
-}
-//*************************************************************************************************
-
-} // namespace
diff --git a/src/mesh/pe/Types.h b/src/mesh/pe/Types.h
index 842ba1132befa7e72d0d09f7c50a4889406d693b..6d94fad9c463f5ee0ff1bd645bf8d7e16ec18834 100644
--- a/src/mesh/pe/Types.h
+++ b/src/mesh/pe/Types.h
@@ -19,6 +19,8 @@
 //
 //======================================================================================================================
 
+#include <memory>
+
 namespace walberla {
 namespace mesh {
 namespace pe {
@@ -28,6 +30,7 @@ class ConvexPolyhedron;
 typedef ConvexPolyhedron        ConvexPolyhedronType;    //!< Type of the convex polyhedron geometric primitive.
 typedef ConvexPolyhedron*       ConvexPolyhedronID;      //!< Handle for a convexpolyhedron primitive.
 typedef const ConvexPolyhedron* ConstConvexPolyhedronID; //!< Handle for a constant convex polyhedron primitive.
+using   ConvexPolyhedronPtr   = std::unique_ptr<ConvexPolyhedron>;
 
 } // namespace pe
 } // namespace mesh
diff --git a/src/mesh/pe/communication/ConvexPolyhedron.h b/src/mesh/pe/communication/ConvexPolyhedron.h
index ef9bbb21cf1b6f4f8883a867395dbee0b8f5080f..df14bb1762f9755225f004d2730a76be617ed236 100644
--- a/src/mesh/pe/communication/ConvexPolyhedron.h
+++ b/src/mesh/pe/communication/ConvexPolyhedron.h
@@ -69,16 +69,17 @@ namespace pe {
 namespace communication {
 
 template<>
-inline mesh::pe::ConvexPolyhedronID instantiate( mpi::RecvBuffer& buffer, const math::AABB& domain, const math::AABB& block, mesh::pe::ConvexPolyhedronID& newBody )
+inline mesh::pe::ConvexPolyhedronPtr instantiate( mpi::RecvBuffer& buffer, const math::AABB& domain, const math::AABB& block, mesh::pe::ConvexPolyhedronID& newBody )
 {
    mesh::pe::ConvexPolyhedronParameters subobjparam;
    unmarshal( buffer, subobjparam );
    correctBodyPosition(domain, block.center(), subobjparam.gpos_);
-   newBody = new mesh::pe::ConvexPolyhedron( subobjparam.sid_, subobjparam.uid_, subobjparam.gpos_, subobjparam.rpos_, subobjparam.q_, subobjparam.mesh_, subobjparam.material_, false, subobjparam.communicating_, subobjparam.infiniteMass_ );
-   newBody->setLinearVel( subobjparam.v_ );
-   newBody->setAngularVel( subobjparam.w_ );
-   newBody->MPITrait.setOwner( subobjparam.mpiTrait_.owner_ );
-   return newBody;
+   auto cp = std::make_unique<mesh::pe::ConvexPolyhedron>( subobjparam.sid_, subobjparam.uid_, subobjparam.gpos_, subobjparam.rpos_, subobjparam.q_, subobjparam.mesh_, subobjparam.material_, false, subobjparam.communicating_, subobjparam.infiniteMass_ );
+   cp->setLinearVel( subobjparam.v_ );
+   cp->setAngularVel( subobjparam.w_ );
+   cp->MPITrait.setOwner( subobjparam.mpiTrait_.owner_ );
+   newBody = static_cast<mesh::pe::ConvexPolyhedronID>(cp.get());
+   return cp;
 }
 
 }  // namespace communication
diff --git a/src/mesh/pe/rigid_body/ConvexPolyhedronFactory.cpp b/src/mesh/pe/rigid_body/ConvexPolyhedronFactory.cpp
index 9919e2f6ab04481ede579ed7b4239b8d5dd0e3df..63c268ae6031a333d50e2720d8dacf8adeb97b93 100644
--- a/src/mesh/pe/rigid_body/ConvexPolyhedronFactory.cpp
+++ b/src/mesh/pe/rigid_body/ConvexPolyhedronFactory.cpp
@@ -65,7 +65,7 @@ ConvexPolyhedronID createConvexPolyhedron( BodyStorage& globalStorage, BlockStor
 {
    WALBERLA_ASSERT_UNEQUAL( ConvexPolyhedron::getStaticTypeID(), std::numeric_limits<id_t>::max(), "ConvexPolyhedron TypeID not initalized!");
 
-   ConvexPolyhedronID poly = NULL;
+   ConvexPolyhedronID poly = nullptr;
 
    Vec3 centroid = toWalberla( computeCentroid( mesh ) );
    translate( mesh, -centroid );
@@ -78,25 +78,24 @@ ConvexPolyhedronID createConvexPolyhedron( BodyStorage& globalStorage, BlockStor
       WALBERLA_CHECK_EQUAL(communicating, false, "Global bodies can not be communicating!" );
       WALBERLA_CHECK_EQUAL(infiniteMass, true, "Global bodies must have infinite mass!" );
 
-      poly = new ConvexPolyhedron(sid, uid, gpos, Vec3(0,0,0), Quat(), mesh, material, global, false, true);
-      globalStorage.add(poly);
+      auto cp = std::make_unique<ConvexPolyhedron>(sid, uid, gpos, Vec3(0,0,0), Quat(), mesh, material, global, false, true);
+      poly = static_cast<ConvexPolyhedronID>(&globalStorage.add(std::move(cp)));
    } else
    {
-      for (auto it = blocks.begin(); it != blocks.end(); ++it){
-         IBlock* block = (&(*it));
-         if (block->getAABB().contains(gpos))
+      for (auto& block : blocks){
+         if (block.getAABB().contains(gpos))
          {
             const id_t sid( UniqueID<RigidBody>::create() );
 
-            Storage* bs = block->getData<Storage>(storageID);
-            poly = new ConvexPolyhedron(sid, uid, gpos, Vec3(0,0,0), Quat(), mesh, material, global, communicating, infiniteMass);
-            poly->MPITrait.setOwner(Owner(MPIManager::instance()->rank(), block->getId().getID()));
-            (*bs)[0].add(poly);
+            BodyStorage& bs = (*block.getData<Storage>(storageID))[0];
+            auto cp = std::make_unique<ConvexPolyhedron>(sid, uid, gpos, Vec3(0,0,0), Quat(), mesh, material, global, communicating, infiniteMass);
+            cp->MPITrait.setOwner(Owner(MPIManager::instance()->rank(), block.getId().getID()));
+            poly = static_cast<ConvexPolyhedronID>(&bs.add( std::move(cp) ) );
          }
       }
    }
 
-   if (poly != NULL)
+   if (poly != nullptr)
    {
       // Logging the successful creation of the box
       WALBERLA_LOG_DETAIL(
diff --git a/src/mesh/pe/vtk/PeVTKMeshWriter.h b/src/mesh/pe/vtk/PeVTKMeshWriter.h
index 55721ae20d67cdad08ed468ab8fe7378a4818e47..72685c362bbd486418b9edd1002db50347f1c712 100644
--- a/src/mesh/pe/vtk/PeVTKMeshWriter.h
+++ b/src/mesh/pe/vtk/PeVTKMeshWriter.h
@@ -186,16 +186,16 @@ protected:
          const walberla::pe::BodyStorage & bodyStorage = (*storage)[0];
          for(auto bodyIt = bodyStorage.begin(); bodyIt != bodyStorage.end(); ++bodyIt)
          {
-            if (!bodyFilter_(**bodyIt)) continue;
+            if (!bodyFilter_(*bodyIt)) continue;
             newVertices.clear();
             newFaces.clear();
-            tesselation_( **bodyIt, *mesh_, newVertices, newFaces );
+            tesselation_( *bodyIt, *mesh_, newVertices, newFaces );
 
             for( const auto & fh: newFaces )
-               faceBodyPointer_[fh] = *bodyIt;
+               faceBodyPointer_[fh] = bodyIt.getBodyID();
 
             for( const auto & vh: newVertices )
-               vertexBodyPointer_[vh] = *bodyIt;
+               vertexBodyPointer_[vh] = bodyIt.getBodyID();
 
          }
       }
diff --git a/src/pe/CMakeLists.txt b/src/pe/CMakeLists.txt
index 1dd7edd80c06535b2df4798075355d75fa46cd90..dadbb9186bda2e10b45607705aa606da413a0ce0 100644
--- a/src/pe/CMakeLists.txt
+++ b/src/pe/CMakeLists.txt
@@ -5,27 +5,4 @@
 #
 ###################################################################################################
 
-if(WALBERLA_DOUBLE_ACCURACY)
-    set(CCD_DOUBLE ON)
-    set(CCD_SINGLE OFF)
-else()
-    set(CCD_DOUBLE OFF)
-    set(CCD_SINGLE ON)
-endif()
-
-configure_file(extern/libccd/ccd/config.h.cmake.in extern/libccd/ccd/config.h)
-
-include_directories(extern/libccd)
-include_directories("${CMAKE_CURRENT_BINARY_DIR}/extern/libccd")
-
 waLBerla_add_module( DEPENDS core blockforest domain_decomposition geometry stencil vtk  )
-
-if( WALBERLA_CXX_COMPILER_IS_MSVC )
-   SET_SOURCE_FILES_PROPERTIES( extern/libccd/ccd.c
-                                extern/libccd/mpr.c
-                                extern/libccd/polytope.c
-                                extern/libccd/support.c
-                                extern/libccd/vec3.c
-                                PROPERTIES LANGUAGE CXX )
-endif()
-
diff --git a/src/pe/Types.h b/src/pe/Types.h
index 764bf4bb70610b43ec54215b60c85a95219ae25f..c71ee80907fa5506206c58de904e21d2bebc4550 100644
--- a/src/pe/Types.h
+++ b/src/pe/Types.h
@@ -27,6 +27,7 @@
 
 #include <boost/array.hpp>
 
+#include <memory>
 #include <vector>
 
 namespace walberla{
@@ -57,8 +58,6 @@ using math::AABB;
 //
 //=================================================================================================
 
-class Attachable;
-class BallJoint;
 class BodyManager;
 class BodyStorage;
 class Box;
@@ -67,21 +66,11 @@ class Contact;
 class Cylinder;
 class Ellipsoid;
 class CylindricalBoundary;
-class FixedJoint;
-class ForceGenerator;
 class GeomPrimitive;
-class Gravity;
-class HingeJoint;
-class Joint;
-class Link;
 class Material;
 class MPISystem;
-class Node;
 class Plane;
-class Process;
 class RigidBody;
-class Section;
-class SliderJoint;
 class Sphere;
 class Spring;
 class Squirmer;
@@ -92,6 +81,7 @@ class Union;
 typedef RigidBody             BodyType;            //!< Type of the rigid bodies.
 typedef RigidBody*            BodyID;              //!< Handle for a rigid body.
 typedef const RigidBody*      ConstBodyID;         //!< Handle for a constant rigid body.
+using   BodyPtr             = std::unique_ptr<RigidBody>;
 
 typedef GeomPrimitive*  GeomID;
 typedef const GeomPrimitive*  ConstGeomID;
@@ -99,80 +89,52 @@ typedef const GeomPrimitive*  ConstGeomID;
 typedef Box                   BoxType;             //!< Type of the box geometric primitive.
 typedef Box*                  BoxID;               //!< Handle for a box primitive.
 typedef const Box*            ConstBoxID;          //!< Handle for a constant box primitive.
+using   BoxPtr              = std::unique_ptr<Box>;
 
 typedef Capsule               CapsuleType;         //!< Type of the capsule geometric primitive.
 typedef Capsule*              CapsuleID;           //!< Handle for a capsule primitive.
 typedef const Capsule*        ConstCapsuleID;      //!< Handle for a constant capsule primitive.
+using   CapsulePtr          = std::unique_ptr<Capsule>;
 
 typedef Cylinder              CylinderType;        //!< Type of the cylinder geometric primitive.
 typedef Cylinder*             CylinderID;          //!< Handle for a cylinder primitive.
 typedef const Cylinder*       ConstCylinderID;     //!< Handle for a constant cylinder primitive.
+using   CylinderPtr         = std::unique_ptr<Cylinder>;
 
 typedef CylindricalBoundary        CylindricalBoundaryType;        //!< Type of the cylindrical boundary geometric primitive.
 typedef CylindricalBoundary*       CylindricalBoundaryID;          //!< Handle for a cylindrical boundary primitive.
 typedef const CylindricalBoundary* ConstCylindricalBoundaryID;     //!< Handle for a constant cylindrical boundary primitive.
+using   CylindricalBoundaryPtr   = std::unique_ptr<CylindricalBoundary>;
 
 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.
+using   EllipsoidPtr        = std::unique_ptr<Ellipsoid>;
 
 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.
+using   PlanePtr            = std::unique_ptr<Plane>;
 
 typedef Sphere                SphereType;          //!< Type of the sphere geometric primitive.
 typedef Sphere*               SphereID;            //!< Handle for a sphere primitive.
 typedef const Sphere*         ConstSphereID;       //!< Handle for a constant sphere primitive.
+using   SpherePtr           = std::unique_ptr<Sphere>;
 
 typedef Squirmer              SquirmerType;        //!< Type of the squirmer geometric primitive.
 typedef Squirmer*             SquirmerID;          //!< Handle for a squirmer primitive.
 typedef const Squirmer*       ConstSquirmerID;     //!< Handle for a constant squirmer primitive.
+using   SquirmerPtr         = std::unique_ptr<Squirmer>;
 
 typedef TriangleMesh          MeshType;             //!< Type of the triangle mesh geometric primitive.
 typedef TriangleMesh*         MeshID;               //!< Handle for a triangle mesh primitive.
 typedef const TriangleMesh*   ConstMeshID;          //!< Handle for a constant triangle mesh primitive.
-
-typedef Attachable            AttachableType;      //!< Type of the attachables.
-typedef Attachable*           AttachableID;        //!< Handle for an attachable.
-typedef const Attachable*     ConstAttachableID;   //!< Handle for a constant attachable.
-
-typedef Gravity               GravityType;         //!< Type of the gravity force generators.
-typedef Gravity*              GravityID;           //!< Handle for a gravity force generator.
-typedef const Gravity*        ConstGravityID;      //!< Handle for a constant gravity force generator.
-
-typedef Spring                SpringType;          //!< Type of the spring force generators.
-typedef Spring*               SpringID;            //!< Handle for a spring force generator.
-typedef const Spring*         ConstSpringID;       //!< Handle for a constant spring force generator.
+using   TriangleMeshPtr     = std::unique_ptr<TriangleMesh>;
 
 typedef Contact               ContactType;         //!< Type of the contacts.
 typedef Contact*              ContactID;           //!< Handle for a contact.
 typedef const Contact*        ConstContactID;      //!< Handle for a constant contact.
 
-typedef Joint                 JointType;           //!< Type of the joints.
-typedef Joint*                JointID;             //!< Handle for a joint.
-typedef const Joint*          ConstJointID;        //!< Handle for a constant joint.
-
-typedef SliderJoint           SliderJointType;     //!< Type of the slider joint.
-typedef SliderJoint*          SldierJointID;       //!< Handle for a slider joint.
-typedef const SliderJoint*    ConstSliderJointID;  //!< Handle for a constant slider joint.
-
-typedef HingeJoint            HingeJointType;      //!< Type of the hinge joint.
-typedef HingeJoint*           HingeJointID;        //!< Handle for a hinge joint.
-typedef const HingeJoint*     ConstHingeJointID;   //!< Handle for a constant hinge joint.
-
-typedef BallJoint             BallJointType;       //!< Type of the ball joint.
-typedef BallJoint*            BallJointID;         //!< Handle for a ball joint.
-typedef const BallJoint*      ConstBallJointID;    //!< Handle for a constant ball joint.
-
-typedef Node                  NodeType;
-typedef Node*                 NodeID;       //!< Handle to a BodyTrait instance.
-typedef const Node*           ConstNodeID;  //!< Handle to a constant BodyTrait instance.
-
-typedef Process               ProcessType;         //!< Type of the remote processes.
-typedef Process*              ProcessID;           //!< Handle for a remote process.
-typedef const Process*        ConstProcessID;      //!< Handle for a constant remote process.
-
-
 typedef BodyManager*          ManagerID;           //!< Handle for a BodyManager.
 typedef const BodyManager*    ConstManagerID;      //!< Handle for a constant BodyManager.
 
diff --git a/src/pe/attachable/Attachable.cpp b/src/pe/attachable/Attachable.cpp
deleted file mode 100644
index 76056d6866479db8625431b72e49b7c87bcc06f8..0000000000000000000000000000000000000000
--- a/src/pe/attachable/Attachable.cpp
+++ /dev/null
@@ -1,143 +0,0 @@
-//======================================================================================================================
-//
-//  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 Attachable.cpp
-//! \author Klaus Iglberger
-//! \author Sebastian Eibl <sebastian.eibl@fau.de>
-//! \brief Source file for the Attachable class
-//
-//======================================================================================================================
-
-
-//*************************************************************************************************
-// Includes
-//*************************************************************************************************
-
-#include <pe/attachable/Attachable.h>
-#include <pe/attachable/AttachableStorage.h>
-#include <core/DataTypes.h>
-
-namespace walberla {
-namespace pe {
-
-//=================================================================================================
-//
-//  CONSTRUCTOR
-//
-//=================================================================================================
-
-//*************************************************************************************************
-/*!\brief Constructor of the Attachable class.
- *
- * \param type The type of the rigid body.
- * \param sid The unique system-specific ID of the attachable.
- */
-Attachable::Attachable( AttachableType type, id_t sid )
-   : type_(type)
-   , sid_(sid)
-   , bodies_()
-{}
-//*************************************************************************************************
-
-
-
-
-//=================================================================================================
-//
-//  DESTRUCTOR
-//
-//=================================================================================================
-
-//*************************************************************************************************
-/*!\brief Destructor for the Attachable class.
- */
-Attachable::~Attachable()
-{
-}
-//*************************************************************************************************
-
-
-
-
-//=================================================================================================
-//
-//  MPI FUNCTIONS
-//
-//=================================================================================================
-
-//*************************************************************************************************
-/*!\fn bool Attachable::isRemote() const
- * \brief Returns whether the attachable is remote or not.
- *
- * \return \a true in case the attachable is remote, \a false if not.
- *
- * This function returns whether the attachable is remote or not. In case the attachable is
- * attached to at least a single local rigid body the function returns \a false. Otherwise
- * it returns \a true.
- */
-//*************************************************************************************************
-
-
-
-
-//=================================================================================================
-//
-//  ATTACHABLE SETUP FUNCTIONS
-//
-//=================================================================================================
-
-//*************************************************************************************************
-/*!\brief Detaches the given attachable.
- *
- * \param attachable The detachable to be detached.
- */
-WALBERLA_PUBLIC void detach( AttachableID attachable )
-{
-   ///TDOD REVIEW
-//   typedef AttachableStorage AS;
-
-   // WARNING: Using friend relationship to get non-constant reference of attachable storage.
-//   AS& attachablestorage( theCollisionSystem()->attachablestorage_ );
-
-   // Removing the attachable from the attachable storage
-//   const AS::Iterator pos( attachablestorage.find( attachable ) );
-//   attachablestorage.remove( pos );
-
-   delete attachable;
-}
-//*************************************************************************************************
-
-//*************************************************************************************************
-/*!\brief Global output operator for AttachableType.
- *
- * \param os Reference to the output stream.
- * \param type The AttachableType to be put into the stream.
- * \return Reference to the output stream.
- */
-std::ostream& operator<<( std::ostream& os, Attachable::AttachableType type )
-{
-   switch( type )
-   {
-      case Attachable::gravityType: os << "gravity force generator"; break;
-      case Attachable::springType:  os << "spring force generator";  break;
-      default: WALBERLA_ASSERT( false, "Unknown attachable type" ); break;
-   }
-
-   return os;
-}
-//*************************************************************************************************
-
-} // namespace pe
-}  // namespace walberla
diff --git a/src/pe/attachable/Attachable.h b/src/pe/attachable/Attachable.h
deleted file mode 100644
index 9c7b2b368ad9d0377cd3284be39132122cfeb752..0000000000000000000000000000000000000000
--- a/src/pe/attachable/Attachable.h
+++ /dev/null
@@ -1,264 +0,0 @@
-//======================================================================================================================
-//
-//  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 Attachable.h
-//! \author Klaus Iglberger
-//! \author Sebastian Eibl <sebastian.eibl@fau.de>
-//! \brief Header file for the Attachable class
-//
-//======================================================================================================================
-
-#pragma once
-
-
-//*************************************************************************************************
-// Includes
-//*************************************************************************************************
-
-#include <core/NonCopyable.h>
-#include <pe/Types.h>
-#include <core/ptrvector/policies/NoDelete.h>
-#include <core/ptrvector/PtrVector.h>
-#include <pe/Types.h>
-
-#include "core/UniqueID.h"
-
-namespace walberla {
-namespace pe {
-
-//=================================================================================================
-//
-//  CLASS DEFINITION
-//
-//=================================================================================================
-
-//*************************************************************************************************
-/*!\brief Attachable interface class.
- *
- * The Attachable class is the base class for the Attachable concept of the physics engine.
- * Classes deriving from this interface class (which are simply called Attachables) can be
- * attached to rigid bodies and are notified in case the rigid body is destroyed. This
- * concept is for example used for all force generators (see the ForceGenerator class
- * description).
- */
-class Attachable : private NonCopyable
-{
-public:
-   //*************************************************************************************************
-   //! Type codes of the attachables.
-   enum AttachableType {
-      gravityType  = 1,  //!< Code for the Gravity force generator.
-      springType   = 2   //!< Code for the Spring force generator.
-   };
-   //*************************************************************************************************
-protected:
-   //**Type definitions****************************************************************************
-   typedef PtrVector<RigidBody,NoDelete>  Bodies;  //!< Vector for attached rigid bodies.
-   //**********************************************************************************************
-
-   //**Constructor*********************************************************************************
-   /*!\name Constructor */
-   //@{
-   explicit Attachable( AttachableType type, id_t sid );
-   //@}
-   //**********************************************************************************************
-
-   //**Destructor**********************************************************************************
-   /*!\name Destructor */
-   //@{
-   virtual ~Attachable();
-   //@}
-   //**********************************************************************************************
-
-public:
-
-   //**Type definitions****************************************************************************
-   typedef Bodies::Iterator       Iterator;       //!< Iterator over the currently attached bodies.
-   typedef Bodies::ConstIterator  ConstIterator;  //!< ConstIterator over the currently attached bodies.
-   //**********************************************************************************************
-
-   //**MPI functions*******************************************************************************
-   /*!\name MPI functions */
-   //@{
-   virtual bool isRemote() const = 0;
-   //@}
-   //**********************************************************************************************
-
-   //**Get functions*******************************************************************************
-   /*!\name Get functions */
-   //@{
-   inline AttachableType getType()     const;
-   inline id_t           getSystemID() const;
-   //@}
-   //**********************************************************************************************
-
-   //**Utility functions***************************************************************************
-   /*!\name Utility functions */
-   //@{
-   inline Iterator      begin();
-   inline ConstIterator begin() const;
-   inline Iterator      end  ();
-   inline ConstIterator end  () const;
-   inline size_t        size () const;
-   //@}
-   //**********************************************************************************************
-
-protected:
-   //**Member variables****************************************************************************
-   /*!\name Member variables */
-   //@{
-   const AttachableType type_;  //!< The type code of the attachable.
-   id_t sid_;                   //!< The unique system-specific attachable ID.
-   Bodies bodies_;              //!< Vector of attached rigid bodies.
-   //@}
-   //**********************************************************************************************
-
-private:
-   //**Attachable setup functions******************************************************************
-   /*! \cond internal */
-   friend void detach( AttachableID attachable );
-   /*! \endcond */
-   //**********************************************************************************************
-};
-//*************************************************************************************************
-
-
-
-
-//=================================================================================================
-//
-//  ATTACHABLE SETUP FUNCTIONS
-//
-//=================================================================================================
-
-//*************************************************************************************************
-/*!\name Attachable setup functions */
-//@{
-void detach( AttachableID attachable );
-//@}
-//*************************************************************************************************
-
-//=================================================================================================
-//
-//  GET FUNCTIONS
-//
-//=================================================================================================
-
-//*************************************************************************************************
-/*!\brief Returns the type of the attachable.
- *
- * \return The type of the attachable.
- */
-inline Attachable::AttachableType Attachable::getType() const
-{
-   return type_;
-}
-//*************************************************************************************************
-
-
-//*************************************************************************************************
-/*!\brief Returns the unique system-specific ID of the attachable.
- *
- * \return The system-specific ID.
- */
-inline id_t Attachable::getSystemID() const
-{
-   return sid_;
-}
-//*************************************************************************************************
-
-
-
-
-//=================================================================================================
-//
-//  UTILITY FUNCTIONS
-//
-//=================================================================================================
-
-//*************************************************************************************************
-/*!\brief Returns an iterator to the first attached rigid body.
- *
- * \return Iterator to the first attached rigid body.
- */
-inline Attachable::Iterator Attachable::begin()
-{
-   return bodies_.begin();
-}
-//*************************************************************************************************
-
-
-//*************************************************************************************************
-/*!\brief Returns an iterator to the first attached rigid body.
- *
- * \return Iterator to the first attached rigid body.
- */
-inline Attachable::ConstIterator Attachable::begin() const
-{
-   return bodies_.begin();
-}
-//*************************************************************************************************
-
-
-//*************************************************************************************************
-/*!\brief Returns an iterator just past the last attached rigid body.
- *
- * \return Iterator just past the last attached rigid body.
- */
-inline Attachable::Iterator Attachable::end()
-{
-   return bodies_.end();
-}
-//*************************************************************************************************
-
-
-//*************************************************************************************************
-/*!\brief Returns an iterator just past the last attached rigid body.
- *
- * \return Iterator just past the last attached rigid body.
- */
-inline Attachable::ConstIterator Attachable::end() const
-{
-   return bodies_.end();
-}
-//*************************************************************************************************
-
-
-//*************************************************************************************************
-/*!\brief Returns the number of attached rigid bodies.
- *
- * \return The number of attached rigid bodies
- */
-inline size_t Attachable::size() const
-{
-   return bodies_.size();
-}
-//*************************************************************************************************
-
-//=================================================================================================
-//
-//  ATTACHABLE TYPE UTILITY FUNCTIONS
-//
-//=================================================================================================
-
-//*************************************************************************************************
-/*!\name Attachable type utility functions */
-//@{
-std::ostream& operator<<( std::ostream& os, Attachable::AttachableType type );
-//@}
-//*************************************************************************************************
-
-} // namespace pe
-}  // namespace walberla
diff --git a/src/pe/attachable/AttachableCast.h b/src/pe/attachable/AttachableCast.h
deleted file mode 100644
index bace6db7f1aa632f16c9d70de8f8ede1f8ffcd49..0000000000000000000000000000000000000000
--- a/src/pe/attachable/AttachableCast.h
+++ /dev/null
@@ -1,93 +0,0 @@
-//======================================================================================================================
-//
-//  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 AttachableCast.h
-//! \author Klaus Iglberger
-//! \author Sebastian Eibl <sebastian.eibl@fau.de>
-//
-//======================================================================================================================
-
-#pragma once
-
-#include <boost/type_traits/is_base_of.hpp>
-
-namespace walberla {
-namespace pe {
-
-//=================================================================================================
-//
-//  ATTACHABLE CAST OPERATORS
-//
-//=================================================================================================
-
-//*************************************************************************************************
-/*!\name Attachable cast operators */
-//@{
-template< typename To, typename From > inline To* static_attachable_cast( From* attachable );
-template< typename To, typename From > inline To* dynamic_attachable_cast( From* attachable );
-//@}
-//*************************************************************************************************
-
-
-//*************************************************************************************************
-/*!\brief Static cast for attachables.
- *
- * \param attachable The attachable to be cast.
- * \return The casted attachable.
- *
- * The static_attachable_cast function is used exactly as the built-in static_cast operator
- * but for attachables.
-
-   \code
-   SphereID     sphere     = createSphere ( 1, 0.0, 0.0, 0.0, 1.0, iron );
-   AttachableID attachable = attachGravity( sphere, 0.0, 0.0, -9.81 );
-   GravityID    gravity    = static_attachable_cast<Gravity>( attachable );
-   \endcode
- */
-template< typename To, typename From >
-inline To* static_attachable_cast( From* attachable )
-{
-   static_assert(boost::is_base_of<Attachable, From>::value, "From has to be derived from Attachable!");
-   static_assert(boost::is_base_of<Attachable, To>::value, "To has to be derived from Attachable!");
-   return static_cast<To*>( attachable );
-}
-//*************************************************************************************************
-
-
-//*************************************************************************************************
-/*!\brief Dynamic cast for attachables.
- *
- * \param attachable The attachable to be cast.
- * \return The casted attachable.
- *
- * The dynamic_attachable_cast function is used exactly as the built-in dynamic_cast operator
- * but for attachables.
-
-   \code
-   AttachableID attachable;
-   GravityID gravity = dynamic_attachable_cast<Gravity>( attachable );
-   \endcode
- */
-template< typename To, typename From >
-inline To* dynamic_attachable_cast( From* attachable )
-{
-   static_assert(boost::is_base_of<Attachable, From>::value, "From has to be derived from Attachable!");
-   static_assert(boost::is_base_of<Attachable, To>::value, "To has to be derived from Attachable!");
-   return dynamic_cast<To*>( attachable );
-}
-//*************************************************************************************************
-
-} // namespace pe
-}
diff --git a/src/pe/attachable/AttachableStorage.h b/src/pe/attachable/AttachableStorage.h
deleted file mode 100644
index 009c3b087bef8d6556e071306846d210b548dd90..0000000000000000000000000000000000000000
--- a/src/pe/attachable/AttachableStorage.h
+++ /dev/null
@@ -1,356 +0,0 @@
-//======================================================================================================================
-//
-//  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 AttachableStorage.h
-//! \author Klaus Iglberger
-//! \author Sebastian Eibl <sebastian.eibl@fau.de>
-//
-//======================================================================================================================
-
-#pragma once
-
-
-//*************************************************************************************************
-// Includes
-//*************************************************************************************************
-
-#include <algorithm>
-#include <functional>
-#include <pe/attachable/Attachable.h>
-#include <pe/Types.h>
-#include <core/ptrvector/policies/NoDelete.h>
-#include <core/ptrvector/PtrVector.h>
-#include <core/DataTypes.h>
-#include <core/debug/Debug.h>
-
-namespace walberla {
-namespace pe {
-
-//=================================================================================================
-//
-//  CLASS DEFINITION
-//
-//=================================================================================================
-
-//*************************************************************************************************
-/*!\brief Attachable storage of the rigid body simulation world.
- *
- * The AttachableStorage class stores all currently existing attachables in the simulation world
- * (see class World).
- */
-class AttachableStorage
-{
-private:
-   //**Type definitions****************************************************************************
-   //! Container for the attachables contained in the simulation world.
-   typedef PtrVector<Attachable,NoDelete>  Attachables;
-   //**********************************************************************************************
-
-public:
-   //**Type definitions****************************************************************************
-   typedef Attachables::SizeType       SizeType;       //!< Size type of the attachable storage.
-   typedef Attachables::Iterator       Iterator;       //!< Iterator over non-const attachables.
-   typedef Attachables::ConstIterator  ConstIterator;  //!< Iterator over constant attachables.
-   //**********************************************************************************************
-
-   //**Constructors********************************************************************************
-   /*!\name Constructors */
-   //@{
-   explicit inline AttachableStorage( SizeType initCapacity = 100 );
-   //@}
-   //**********************************************************************************************
-
-   //**Utility functions***************************************************************************
-   /*!\name Utility functions */
-   //@{
-   inline bool          isEmpty() const;
-   inline SizeType      size   () const;
-   inline Iterator      begin  ();
-   inline ConstIterator begin  () const;
-   inline Iterator      end    ();
-   inline ConstIterator end    () const;
-   inline Iterator      find   ( id_t sid );
-   inline ConstIterator find   ( id_t sid ) const;
-   inline Iterator      find   ( ConstAttachableID attachable );
-   inline ConstIterator find   ( ConstAttachableID attachable ) const;
-   //@}
-   //**********************************************************************************************
-
-   //**Add/Remove functions************************************************************************
-   /*!\name Add/Remove functions */
-   //@{
-   inline void     add   ( AttachableID attachable );
-   inline Iterator remove( Iterator pos );
-   //@}
-   //**********************************************************************************************
-
-private:
-   //**Member variables****************************************************************************
-   /*!\name Member variables */
-   //@{
-   Attachables attachables_;  //!< The currently existing attachables.
-   //@}
-   //**********************************************************************************************
-
-   //**Private struct Compare**********************************************************************
-   /*! \cond internal */
-   /*!\brief Helper class for the find() function. */
-   struct Compare : public std::binary_function<AttachableID,id_t,bool>
-   {
-      //**Binary function call operator************************************************************
-      /*!\name Binary function call operator */
-      //@{
-      inline bool operator()( ConstAttachableID attachable, id_t sid ) const {
-         return attachable->getSystemID() < sid;
-      }
-      inline bool operator()( id_t sid, ConstAttachableID attachable ) const {
-         return sid < attachable->getSystemID();
-      }
-      inline bool operator()( ConstAttachableID attachable1, ConstAttachableID attachable2 ) const {
-         return attachable1->getSystemID() < attachable2->getSystemID();
-      }
-      //@}
-      //*******************************************************************************************
-   };
-   /*! \endcond */
-   //**********************************************************************************************
-
-   //**Friend declarations*************************************************************************
-   /*! \cond internal */
-   friend class Attachable;
-   /*! \endcond */
-   //**********************************************************************************************
-};
-//*************************************************************************************************
-
-
-
-
-//=================================================================================================
-//
-//  CONSTRUCTORS
-//
-//=================================================================================================
-
-//*************************************************************************************************
-/*!\brief Standard constructor for the AttachableStorage.
- *
- * \param initCapacity The initial capacity of the attachable storage.
- */
-inline AttachableStorage::AttachableStorage( SizeType initCapacity )
-   : attachables_( initCapacity )
-{}
-//*************************************************************************************************
-
-
-
-
-//=================================================================================================
-//
-//  UTILITY FUNCTIONS
-//
-//=================================================================================================
-
-//*************************************************************************************************
-/*!\brief Returns \a true if the attachable storage contains no attachables.
- *
- * \return \a true if the attachable storage is empty, \a false if it is not.
- */
-inline bool AttachableStorage::isEmpty() const
-{
-   return attachables_.isEmpty();
-}
-//*************************************************************************************************
-
-
-//*************************************************************************************************
-/*!\brief Returns the number of attachables contained in the attachable storage.
- *
- * \return The number of attachables.
- */
-inline AttachableStorage::SizeType AttachableStorage::size() const
-{
-   return attachables_.size();
-}
-//*************************************************************************************************
-
-
-//*************************************************************************************************
-/*!\brief Returns an iterator to the first contained attachable.
- *
- * \return Iterator to the first contained attachable.
- */
-inline AttachableStorage::Iterator AttachableStorage::begin()
-{
-   return attachables_.begin();
-}
-//*************************************************************************************************
-
-
-//*************************************************************************************************
-/*!\brief Returns a constant iterator to the first contained attachable.
- *
- * \return Constant iterator to the first contained attachable.
- */
-inline AttachableStorage::ConstIterator AttachableStorage::begin() const
-{
-   return attachables_.begin();
-}
-//*************************************************************************************************
-
-
-//*************************************************************************************************
-/*!\brief Returns an iterator just past the last contained attachable.
- *
- * \return Iterator just past the last contained attachable.
- */
-inline AttachableStorage::Iterator AttachableStorage::end()
-{
-   return attachables_.end();
-}
-//*************************************************************************************************
-
-
-//*************************************************************************************************
-/*!\brief Returns a constant iterator just past the last contained attachable.
- *
- * \return Constant iterator just past the last contained attachable.
- */
-inline AttachableStorage::ConstIterator AttachableStorage::end() const
-{
-   return attachables_.end();
-}
-//*************************************************************************************************
-
-
-//*************************************************************************************************
-/*!\brief Finding an attachable with a certain unique system-specific ID.
- *
- * \param sid The unique system-specific ID for the search.
- * \return Iterator to the attachable with system-specific ID \a sid or an iterator just past the end.
- *
- * This function finds the attachable with the system-specific ID \a sid. In case the attachable
- * is found, the function returns an iterator to the attachable. Otherwise, the function returns
- * an iterator just past the end of last attachable contained in the attachable storage.
- */
-inline AttachableStorage::Iterator AttachableStorage::find( id_t sid )
-{
-   Iterator pos = std::lower_bound( begin(), end(), sid, Compare() );
-   if( pos != end() && pos->getSystemID() == sid )
-      return pos;
-   else
-      return end();
-}
-//*************************************************************************************************
-
-
-//*************************************************************************************************
-/*!\brief Finding an attachable with a certain unique system-specific ID.
- *
- * \param sid The unique system-specific ID for the search.
- * \return Constant iterator to the attachable with system-specific ID \a sid or a constant iterator just past the end.
- *
- * This function finds the attachable with the system-specific ID \a sid. In case the attachable
- * is found, the function returns a constant iterator to the attachable. Otherwise, the function
- * returns a constant iterator just past the end of last attachable contained in the attachable
- * storage.
- */
-inline AttachableStorage::ConstIterator AttachableStorage::find( id_t sid ) const
-{
-   ConstIterator pos = std::lower_bound( begin(), end(), sid, Compare() );
-   if( pos != end() && pos->getSystemID() == sid )
-      return pos;
-   else
-      return end();
-}
-//*************************************************************************************************
-
-
-//*************************************************************************************************
-/*!\brief Finding a specific attachable in the attachable storage.
- *
- * \param attachable The given attachable for the search.
- * \return Iterator to the given attachable or an iterator just past the end.
- *
- * This function finds the attachable in the attachable storage. In case the attachable is found,
- * the function returns an iterator to the attachable. Otherwise, the function returns an iterator
- * just past the end of last attachable contained in the attachable storage.
- */
-inline AttachableStorage::Iterator AttachableStorage::find( ConstAttachableID attachable )
-{
-   return find( attachable->getSystemID() );
-}
-//*************************************************************************************************
-
-
-//*************************************************************************************************
-/*!\brief Finding a specific attachable in the attachable storage.
- *
- * \param attachable The given attachable for the search.
- * \return Iterator to the given attachable or an iterator just past the end.
- *
- * This function finds the attachable in the attachable storage. In case the attachable is found,
- * the function returns an iterator to the attachable. Otherwise, the function returns an iterator
- * just past the end of last attachable contained in the attachable storage.
- */
-inline AttachableStorage::ConstIterator AttachableStorage::find( ConstAttachableID attachable ) const
-{
-   return find( attachable->getSystemID() );
-}
-//*************************************************************************************************
-
-
-
-
-//=================================================================================================
-//
-//  ADD/REMOVE FUNCTIONS
-//
-//=================================================================================================
-
-//*************************************************************************************************
-/*!\brief Adding an attachable to the attachable storage.
- *
- * \param attachable The new attachable to be added to the attachable storage.
- * \return void
- *
- * This function adds an attachable to the attachable storage.
- */
-inline void AttachableStorage::add( AttachableID attachable )
-{
-   Iterator pos = std::lower_bound( begin(), end(), attachable->getSystemID(), Compare() );
-   attachables_.insert( pos, attachable );
-}
-//*************************************************************************************************
-
-
-//*************************************************************************************************
-/*!\brief Removing an attachable from the attachable storage.
- *
- * \param pos The position of the attachable to be removed.
- * \return Iterator to the attachable after the erased attachable.
- *
- * This function removes an attachable from the attachable storage.
- */
-inline AttachableStorage::Iterator AttachableStorage::remove( Iterator pos )
-{
-   WALBERLA_ASSERT( pos != end(), "Attachable is not contained in the attachable storage" );
-
-   return attachables_.erase( pos );
-}
-//*************************************************************************************************
-
-} // namespace pe
-}  // namespace walberla
diff --git a/src/pe/ccd/HashGrids.cpp b/src/pe/ccd/HashGrids.cpp
index 31d4c96cc8a84206298eab9ea2738a978aa3e5e3..ce6b8c993353b82f5bbd6cb9926184884eff97c8 100644
--- a/src/pe/ccd/HashGrids.cpp
+++ b/src/pe/ccd/HashGrids.cpp
@@ -756,15 +756,15 @@ void HashGrids::reloadBodies()
 {
    clear();
 
-   for (auto bodyIt = bodystorage_.begin(); bodyIt != bodystorage_.end(); ++bodyIt)
+   for (auto& body : bodystorage_)
    {
-      add( *bodyIt );
+      add( &body );
    }
    if( &bodystorage_ != &bodystorageShadowCopies_ )
    {
-      for (auto bodyIt = bodystorageShadowCopies_.begin(); bodyIt != bodystorageShadowCopies_.end(); ++bodyIt)
+      for (auto& body : bodystorageShadowCopies_)
       {
-         add( *bodyIt );
+         add( &body );
       }
    }
 }
@@ -864,46 +864,42 @@ void HashGrids::update(WcTimingTree* tt)
       //                        suitably sized cells (=> "addGrid()").
 
       {
-         const auto end( bodystorage_.end() );
-         for( auto bIt = bodystorage_.begin(); bIt != end; ++bIt )
+         for( auto& body : bodystorage_ )
          {
-            BodyID body = *bIt;
-            HashGrid* grid = static_cast<HashGrid*>( body->getGrid() );
+            HashGrid* grid = static_cast<HashGrid*>( body.getGrid() );
 
             if( grid != NULL )
             {
-               real_t size     = body->getAABBSize();
+               real_t size     = body.getAABBSize();
                real_t cellSpan = grid->getCellSpan();
 
                if( size >= cellSpan || size < ( cellSpan / hierarchyFactor ) ) {
-                  grid->remove( body );
-                  addGrid( body );
+                  grid->remove( &body );
+                  addGrid( &body );
                }
                else {
-                  grid->update( body );
+                  grid->update( &body );
                }
             }
          }
       }
 
       if( &bodystorage_ != &bodystorageShadowCopies_ ) {
-         const auto end( bodystorageShadowCopies_.end() );
-         for( auto bIt = bodystorageShadowCopies_.begin(); bIt != end; ++bIt )
+         for( auto& body : bodystorageShadowCopies_ )
          {
-            BodyID body = *bIt;
-            HashGrid* grid = static_cast<HashGrid*>( body->getGrid() );
+            HashGrid* grid = static_cast<HashGrid*>( body.getGrid() );
 
             if( grid != NULL )
             {
-               real_t size     = body->getAABBSize();
+               real_t size     = body.getAABBSize();
                real_t cellSpan = grid->getCellSpan();
 
                if( size >= cellSpan || size < ( cellSpan / hierarchyFactor ) ) {
-                  grid->remove( body );
-                  addGrid( body );
+                  grid->remove( &body );
+                  addGrid( &body );
                }
                else {
-                  grid->update( body );
+                  grid->update( &body );
                }
             }
          }
@@ -958,7 +954,7 @@ PossibleContacts& HashGrids::generatePossibleContacts( WcTimingTree* tt )
             }
             // Test all bodies stored in 'grid' against all bodies stored in 'globalStorage_'.
             for( auto bIt = globalStorage_.begin(); bIt < globalStorage_.end(); ++bIt ) {
-               collide( *a, *bIt, contacts_ );
+               collide( *a, &(*bIt), contacts_ );
             }
          }
       }
@@ -974,7 +970,7 @@ PossibleContacts& HashGrids::generatePossibleContacts( WcTimingTree* tt )
 
       // Pairwise test (=> contact generation) for all bodies that are stored in 'nonGridBodies_' with global bodies.
       for( auto bIt = globalStorage_.begin(); bIt < globalStorage_.end(); ++bIt ) {
-         collide( *aIt, *bIt, contacts_ );
+         collide( *aIt, &(*bIt), contacts_ );
       }
    }
    if (tt != NULL) tt->stop("Detection");
diff --git a/src/pe/ccd/SimpleCCD.cpp b/src/pe/ccd/SimpleCCD.cpp
index dee87d16b92bd0b26120cee2c63d61d7eac93636..2fcb2b9e6506767a21ae77c7d0903e10ac0c0b0e 100644
--- a/src/pe/ccd/SimpleCCD.cpp
+++ b/src/pe/ccd/SimpleCCD.cpp
@@ -70,12 +70,12 @@ PossibleContacts& SimpleCCD::generatePossibleContacts( WcTimingTree* tt ){
 
       for (auto it2 = globalStorage_.begin(); it2 != globalStorage_.end(); ++it2)
       {
-         if (!((*it1)->hasInfiniteMass() && (*it2)->hasInfiniteMass()))
+         if (!((*it1)->hasInfiniteMass() && it2->hasInfiniteMass()))
          {
-            if ( (*it1)->getSystemID() > (*it2)->getSystemID() )
-               contacts_.push_back(std::make_pair(*it2, *it1));
+            if ( (*it1)->getSystemID() > it2->getSystemID() )
+               contacts_.push_back(std::make_pair(it2.getBodyID(), *it1));
             else
-               contacts_.push_back(std::make_pair(*it1, *it2));
+               contacts_.push_back(std::make_pair(*it1, it2.getBodyID()));
          }
       }
    }
diff --git a/src/pe/collision/GJKEPAHelper.cpp b/src/pe/collision/GJKEPAHelper.cpp
deleted file mode 100644
index d8316cd6db099983dc7b0772f6d506e9ac23cd3a..0000000000000000000000000000000000000000
--- a/src/pe/collision/GJKEPAHelper.cpp
+++ /dev/null
@@ -1,79 +0,0 @@
-//======================================================================================================================
-//
-//  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 GJKHelper.cpp
-//! \author Sebastian Eibl <sebastian.eibl@fau.de>
-//
-//======================================================================================================================
-
-//*************************************************************************************************
-// Includes
-//*************************************************************************************************
-
-#include "GJKEPAHelper.h"
-
-#include "pe/rigidbody/GeomPrimitive.h"
-
-extern "C" {
-   #include "pe/extern/libccd/ccd/ccd.h"
-   #include "pe/extern/libccd/ccd/quat.h"
-}
-
-#include "core/logging/Logging.h"
-
-namespace walberla {
-namespace pe {
-
-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)
-{
-    ConstGeomID bd = reinterpret_cast<ConstGeomID> (obj);
-    Vec3 d = convertVec3(*dir);
-    Vec3 sup = bd->support( d );
-    *vec = convertVec3(sup);
-}
-
-bool collideGJK( ConstGeomID bd1,
-                 ConstGeomID bd2,
-                 Vec3& contactPoint,
-                 Vec3& contactNormal,
-                 real_t& penetrationDepth,
-                 const unsigned long numIterations,
-                 const real_t epaTol )
-{
-    ccd_t ccd;
-    CCD_INIT(&ccd); // initialize ccd_t struct
-
-    // set up ccd_t struct
-    ccd.support1       = support;       // support function for first object
-    ccd.support2       = support;       // support function for second object
-    ccd.max_iterations = numIterations; // maximal number of iterations
-    ccd.epa_tolerance  = epaTol;
-
-    ccd_vec3_t 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;
-
-    return (intersect == 0);
-}
-
-} // namespace pe
-} // namespace walberla
diff --git a/src/pe/collision/GJKEPAHelper.h b/src/pe/collision/GJKEPAHelper.h
deleted file mode 100644
index b1d73b1a19584a53449034529163c9247a5f230c..0000000000000000000000000000000000000000
--- a/src/pe/collision/GJKEPAHelper.h
+++ /dev/null
@@ -1,87 +0,0 @@
-//======================================================================================================================
-//
-//  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 GJKHelper.h
-//! \author Sebastian Eibl <sebastian.eibl@fau.de>
-//
-//======================================================================================================================
-
-#pragma once
-
-//*************************************************************************************************
-// Includes
-//*************************************************************************************************
-
-#include "pe/contact/Contact.h"
-#include "pe/Types.h"
-
-#include "core/math/Vector3.h"
-
-namespace walberla {
-namespace pe {
-
-static unsigned long maxGJKIterations = 100ul;
-static real_t epaTolerance = real_t(0.000001);
-
-inline
-void setMaxGJKIterations(const unsigned long& iter)
-{
-   maxGJKIterations = iter;
-}
-inline
-unsigned long getMaxGJKIterations()
-{
-   return maxGJKIterations;
-}
-
-inline
-void setEPATolerance(const real_t& tol)
-{
-   epaTolerance = tol;
-}
-inline
-real_t getEPATolerance()
-{
-   return epaTolerance;
-}
-
-bool collideGJK( ConstGeomID bd1,
-                 ConstGeomID bd2,
-                 Vec3& contactPoint,
-                 Vec3& contactNormal,
-                 real_t& penetrationDepth,
-                 const unsigned long numIterations = maxGJKIterations,
-                 const real_t epaTol = epaTolerance );
-
-template <typename Container>
-bool collideGJK( GeomID bd1,
-                 GeomID bd2,
-                 Container& container,
-                 const unsigned long numIterations = maxGJKIterations,
-                 const real_t epaTol = epaTolerance )
-{
-   real_t penetrationDepth;
-   Vec3   contactPoint;
-   Vec3   contactNormal;
-   bool retVal = collideGJK(bd1, bd2, contactPoint, contactNormal, penetrationDepth, numIterations, epaTol);
-   if (retVal)
-   {
-      container.push_back( Contact(bd1, bd2, contactPoint, contactNormal, penetrationDepth) );
-   }
-   return retVal;
-}
-
-} // namespace pe
-} // namespace walberla
diff --git a/src/pe/communication/DynamicMarshalling.h b/src/pe/communication/DynamicMarshalling.h
index e2d2c4a6ceb2b4e35d1aad180c9b4700514bc03f..032708bff7ac44d5ece117697ea337097f6a1a66 100644
--- a/src/pe/communication/DynamicMarshalling.h
+++ b/src/pe/communication/DynamicMarshalling.h
@@ -91,7 +91,7 @@ private:
          , block_(block) {}
 
       template< typename BodyType >
-      BodyID operator()( BodyType* bd) { return instantiate( buffer_, domain_, block_, bd ); }
+      BodyPtr operator()( BodyType* bd) { return instantiate( buffer_, domain_, block_, bd ); }
    };
 
 public:
@@ -105,10 +105,10 @@ public:
     * The rigid body is casted dynamically to its original type and then marshalled. For recognition
     * an identifying tag is prepended.
     */
-   static BodyID execute(mpi::RecvBuffer& buffer, const id_t typeID, const math::AABB& domain, const math::AABB& block)
+   static BodyPtr execute(mpi::RecvBuffer& buffer, const id_t typeID, const math::AABB& domain, const math::AABB& block)
    {
       UnmarshalFunctor func(buffer, domain, block);
-      return SingleCast<BodyTypeTuple, UnmarshalFunctor, BodyID>::execute (typeID, func);
+      return SingleCast<BodyTypeTuple, UnmarshalFunctor, BodyPtr>::execute (typeID, func);
    }
 };
 
diff --git a/src/pe/communication/Instantiate.h b/src/pe/communication/Instantiate.h
index 6096c6552d3e21e63ec29a99efbefcfc0cfb20c3..f10a27dddab2efb0a4a6b9ae375110fce368dd29 100644
--- a/src/pe/communication/Instantiate.h
+++ b/src/pe/communication/Instantiate.h
@@ -55,7 +55,7 @@ void correctBodyPosition(const math::AABB& domain, const Vec3& center, Vec3& pos
 }
 
 template < class BodyT >
-BodyT* instantiate( mpi::RecvBuffer& /*buffer*/, const math::AABB& /*domain*/, const math::AABB& /*block*/, BodyT*& /*newBody*/ )
+std::unique_ptr<BodyT> instantiate( mpi::RecvBuffer& /*buffer*/, const math::AABB& /*domain*/, const math::AABB& /*block*/, BodyT*& /*newBody*/ )
 {
    WALBERLA_ABORT( "Body instantiation not implemented! (" << demangle(typeid(BodyT).name()) << ")" );
 }
diff --git a/src/pe/communication/ParseMessage.h b/src/pe/communication/ParseMessage.h
index 5735beb152fb332ae5d007ad56115d339b6f5194..deea7db9689387c9adcd3f8dc4edccca02a8488a 100644
--- a/src/pe/communication/ParseMessage.h
+++ b/src/pe/communication/ParseMessage.h
@@ -64,17 +64,18 @@ void parseMessage(Owner sender, mpi::RecvBuffer& rb, const domain_decomposition:
 
             WALBERLA_LOG_DETAIL( "Received " << objparam.geomType_ << " copy notification from neighboring process with rank " << sender );
 
-            BodyID obj = UnmarshalDynamically<BodyTypeTuple>::execute(rb, objparam.geomType_, blockStorage.getDomain(), block.getAABB());
+            BodyPtr obj = UnmarshalDynamically<BodyTypeTuple>::execute(rb, objparam.geomType_, blockStorage.getDomain(), block.getAABB());
             obj->setRemote( true );
+            obj->MPITrait.setBlockState( sender.blockID_ );
+
             if (shadowStorage.find( obj->getSystemID() ) == shadowStorage.end())
             {
-               WALBERLA_LOG_DETAIL( "Adding new shadow copy with id " << obj->getSystemID() << " to domain " << block.getAABB() << ".\n" << obj);
-               shadowStorage.add( obj );
+               WALBERLA_LOG_DETAIL( "Adding new shadow copy with id " << obj->getSystemID() << " to domain " << block.getAABB() << ".\n" << *obj);
+               shadowStorage.add( std::move(obj) );
             } else
             {
                WALBERLA_LOG_DETAIL( "Shadow copy with id " << obj->getSystemID() << " already existend.");
             }
-            obj->MPITrait.setBlockState( sender.blockID_ );
 
             WALBERLA_LOG_DETAIL( "Processed " << objparam.geomType_ << " copy notification."  );
 
@@ -88,7 +89,7 @@ void parseMessage(Owner sender, mpi::RecvBuffer& rb, const domain_decomposition:
 
             auto bodyIt = shadowStorage.find( objparam.sid_ );
             WALBERLA_ASSERT_UNEQUAL( bodyIt, shadowStorage.end() );
-            BodyID b( *bodyIt );
+            BodyID b( bodyIt.getBodyID() );
 
             WALBERLA_ASSERT( b->MPITrait.getOwner().blockID_ == sender.blockID_, "Update notifications must be sent by owner.\n" << b->MPITrait.getOwner().blockID_ << " != "<< sender.blockID_ );
             WALBERLA_ASSERT( b->isRemote(), "Update notification must only concern shadow copies." );
@@ -112,10 +113,9 @@ void parseMessage(Owner sender, mpi::RecvBuffer& rb, const domain_decomposition:
 
             auto bodyIt = shadowStorage.find( objparam.sid_ );
             if ( bodyIt == shadowStorage.end() ) WALBERLA_ABORT( "Object with id: " << objparam.sid_ << " not found in shadowStorage! Cannot transfer ownership! \nlocal domain: " << block.getAABB() );
-            BodyID b( *bodyIt );
+            BodyID b( bodyIt.getBodyID() );
 
-            shadowStorage.release( b );
-            localStorage.add( b );
+            localStorage.add( shadowStorage.release( b ) );
 
             WALBERLA_ASSERT( sender.blockID_ == b->MPITrait.getOwner().blockID_, "Migration notifications must be sent by previous owner.\n" << b->MPITrait.getOwner().blockID_ << " != "<< sender.blockID_ );
             WALBERLA_ASSERT( b->isRemote(), "Bodies in migration notifications must be available as shadow copies in local process." );
@@ -143,7 +143,7 @@ void parseMessage(Owner sender, mpi::RecvBuffer& rb, const domain_decomposition:
 
             auto bodyIt = shadowStorage.find( objparam.sid_ );
             WALBERLA_ASSERT_UNEQUAL( bodyIt, shadowStorage.end() );
-            BodyID b( *bodyIt );
+            BodyID b( bodyIt.getBodyID() );
 
             WALBERLA_ASSERT( sender.blockID_ == b->MPITrait.getOwner().blockID_, "Remote migration notifications must be sent by previous owner.\n" << sender.blockID_ << " != " << b->MPITrait.getOwner().blockID_  );
             WALBERLA_ASSERT( b->isRemote(), "Bodies in remote migration notifications must be available as shadow copies in local process." );
@@ -164,7 +164,7 @@ void parseMessage(Owner sender, mpi::RecvBuffer& rb, const domain_decomposition:
             // Remove shadow copy as prompted.
             auto bodyIt = shadowStorage.find( objparam.sid_ );
             WALBERLA_ASSERT_UNEQUAL( bodyIt, shadowStorage.end() );
-            BodyID b( *bodyIt );
+            BodyID b( bodyIt.getBodyID() );
 
             // TODO assert that we indeed do not need the shadow copy anymore
             WALBERLA_ASSERT( b->MPITrait.getOwner().blockID_ == sender.blockID_, "Only owner is allowed to send removal notifications.\n" << b->MPITrait.getOwner().blockID_ << " != "<< sender.blockID_ );
@@ -184,7 +184,7 @@ void parseMessage(Owner sender, mpi::RecvBuffer& rb, const domain_decomposition:
          // Remove invalid shadow copy.
          auto bodyIt = shadowStorage.find( objparam.sid_ );
          WALBERLA_ASSERT_UNEQUAL( bodyIt, shadowStorage.end() );
-         BodyID b( *bodyIt );
+         BodyID b( bodyIt.getBodyID() );
 
          WALBERLA_ASSERT( b->MPITrait.getOwner().blockID_ == sender.blockID_, "Only owner is allowed to send deletion notifications.\n" << b->MPITrait.getOwner().blockID_ << " != "<< sender.blockID_ );
 
@@ -203,7 +203,7 @@ void parseMessage(Owner sender, mpi::RecvBuffer& rb, const domain_decomposition:
          // Remove invalid shadow copy.
          auto bodyIt = localStorage.find( objparam.sid_ );
          WALBERLA_ASSERT_UNEQUAL( bodyIt, localStorage.end() );
-         BodyID b( *bodyIt );
+         BodyID b( bodyIt.getBodyID() );
 
          b->MPITrait.registerShadowOwner( objparam.newOwner_ );
 
@@ -221,7 +221,7 @@ void parseMessage(Owner sender, mpi::RecvBuffer& rb, const domain_decomposition:
          {
             auto bodyIt = localStorage.find( objparam.sid_ );
             WALBERLA_ASSERT_UNEQUAL( bodyIt, localStorage.end() );
-            BodyID b( *bodyIt );
+            BodyID b( bodyIt.getBodyID() );
             b->MPITrait.deregisterShadowOwner( sender );
             b->MPITrait.unsetBlockState( sender.blockID_ );
          } else
@@ -229,7 +229,7 @@ void parseMessage(Owner sender, mpi::RecvBuffer& rb, const domain_decomposition:
             auto bodyIt = shadowStorage.find( objparam.sid_ );
             if (bodyIt != shadowStorage.end() )
             {
-               BodyID b( *bodyIt );
+               BodyID b( bodyIt.getBodyID() );
                b->MPITrait.unsetBlockState( sender.blockID_ );
             }
          }
@@ -260,7 +260,7 @@ void parseForceReduceMessage(Owner sender, mpi::RecvBuffer& rb, const domain_dec
 
       auto bodyIt = localStorage.find( objparam.sid_ );
       WALBERLA_ASSERT_UNEQUAL( bodyIt, localStorage.end() );
-      BodyID b( *bodyIt );
+      BodyID b( bodyIt.getBodyID() );
 
       WALBERLA_ASSERT( !b->isRemote(), "Update notification must only concern local bodies." );
 
@@ -292,7 +292,7 @@ void parseForceDistributeMessage(Owner sender, mpi::RecvBuffer& rb, const domain
 
       auto bodyIt = shadowStorage.find( objparam.sid_ );
       WALBERLA_ASSERT_UNEQUAL( bodyIt, shadowStorage.end() );
-      BodyID b( *bodyIt );
+      BodyID b( bodyIt.getBodyID() );
 
       WALBERLA_ASSERT( b->isRemote(), "Update notification must only concern shadow bodies." );
 
diff --git a/src/pe/communication/rigidbody/Box.h b/src/pe/communication/rigidbody/Box.h
index 3a4c35a20e31b259cddd3fc0cfea4bf1d87d95d8..77f543cbc9183348f40a08d07fd95981b9800fdc 100644
--- a/src/pe/communication/rigidbody/Box.h
+++ b/src/pe/communication/rigidbody/Box.h
@@ -59,16 +59,17 @@ void marshal( mpi::SendBuffer& buffer, const Box& obj );
  */
 void unmarshal( mpi::RecvBuffer& buffer, BoxParameters& objparam );
 //*************************************************************************************************
-inline BoxID instantiate( mpi::RecvBuffer& buffer, const math::AABB& domain, const math::AABB& block, BoxID& newBody )
+inline BoxPtr instantiate( mpi::RecvBuffer& buffer, const math::AABB& domain, const math::AABB& block, BoxID& newBody )
 {
    BoxParameters subobjparam;
    unmarshal( buffer, subobjparam );
    correctBodyPosition(domain, block.center(), subobjparam.gpos_);
-   newBody = new Box( subobjparam.sid_, subobjparam.uid_, subobjparam.gpos_, subobjparam.rpos_, subobjparam.q_, subobjparam.lengths_, subobjparam.material_, false, subobjparam.communicating_, subobjparam.infiniteMass_ );
-   newBody->setLinearVel( subobjparam.v_ );
-   newBody->setAngularVel( subobjparam.w_ );
-   newBody->MPITrait.setOwner( subobjparam.mpiTrait_.owner_ );
-   return newBody;
+   auto bx = std::make_unique<Box>( subobjparam.sid_, subobjparam.uid_, subobjparam.gpos_, subobjparam.rpos_, subobjparam.q_, subobjparam.lengths_, subobjparam.material_, false, subobjparam.communicating_, subobjparam.infiniteMass_ );
+   bx->setLinearVel( subobjparam.v_ );
+   bx->setAngularVel( subobjparam.w_ );
+   bx->MPITrait.setOwner( subobjparam.mpiTrait_.owner_ );
+   newBody = bx.get();
+   return bx;
 }
 
 }  // namespace communication
diff --git a/src/pe/communication/rigidbody/Capsule.h b/src/pe/communication/rigidbody/Capsule.h
index 7f99b1d1d47d7fb4e96713213f3ecababd718915..3ef02f07d1d4b95fbb3010be10e61eb158772cca 100644
--- a/src/pe/communication/rigidbody/Capsule.h
+++ b/src/pe/communication/rigidbody/Capsule.h
@@ -61,16 +61,17 @@ void unmarshal( mpi::RecvBuffer& buffer, CapsuleParameters& objparam );
 //*************************************************************************************************
 
 
-inline CapsuleID instantiate( mpi::RecvBuffer& buffer, const math::AABB& domain, const math::AABB& block, CapsuleID& newBody )
+inline CapsulePtr instantiate( mpi::RecvBuffer& buffer, const math::AABB& domain, const math::AABB& block, CapsuleID& newBody )
 {
    CapsuleParameters subobjparam;
    unmarshal( buffer, subobjparam );
    correctBodyPosition(domain, block.center(), subobjparam.gpos_);
-   newBody = new Capsule( subobjparam.sid_, subobjparam.uid_, subobjparam.gpos_, subobjparam.rpos_, subobjparam.q_, subobjparam.radius_, subobjparam.length_, subobjparam.material_, false, subobjparam.communicating_, subobjparam.infiniteMass_ );
-   newBody->setLinearVel( subobjparam.v_ );
-   newBody->setAngularVel( subobjparam.w_ );
-   newBody->MPITrait.setOwner( subobjparam.mpiTrait_.owner_ );
-   return newBody;
+   auto cp = std::make_unique<Capsule>( subobjparam.sid_, subobjparam.uid_, subobjparam.gpos_, subobjparam.rpos_, subobjparam.q_, subobjparam.radius_, subobjparam.length_, subobjparam.material_, false, subobjparam.communicating_, subobjparam.infiniteMass_ );
+   cp->setLinearVel( subobjparam.v_ );
+   cp->setAngularVel( subobjparam.w_ );
+   cp->MPITrait.setOwner( subobjparam.mpiTrait_.owner_ );
+   newBody = cp.get();
+   return cp;
 }
 
 }  // namespace communication
diff --git a/src/pe/communication/rigidbody/Ellipsoid.h b/src/pe/communication/rigidbody/Ellipsoid.h
index 57916d268a30dbfea187071a18b4270ee63c83f3..a292c5b1dd1bc9bfbd2fb487260571fa26bc9756 100644
--- a/src/pe/communication/rigidbody/Ellipsoid.h
+++ b/src/pe/communication/rigidbody/Ellipsoid.h
@@ -61,16 +61,17 @@ void unmarshal( mpi::RecvBuffer& buffer, EllipsoidParameters& objparam );
 //*************************************************************************************************
 
 
-inline EllipsoidID instantiate( mpi::RecvBuffer& buffer, const math::AABB& domain, const math::AABB& block, EllipsoidID& newBody )
+inline EllipsoidPtr 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;
+   auto el = std::make_unique<Ellipsoid>( subobjparam.sid_, subobjparam.uid_, subobjparam.gpos_, subobjparam.rpos_, subobjparam.q_, subobjparam.semiAxes_, subobjparam.material_, false, subobjparam.communicating_, subobjparam.infiniteMass_ );
+   el->setLinearVel( subobjparam.v_ );
+   el->setAngularVel( subobjparam.w_ );
+   el->MPITrait.setOwner( subobjparam.mpiTrait_.owner_ );
+   newBody = el.get();
+   return el;
 }
 
 }  // namespace communication
diff --git a/src/pe/communication/rigidbody/Sphere.h b/src/pe/communication/rigidbody/Sphere.h
index 55881f65aa0e3c5e02cd0ea099093dd16ef45379..3e7da560142c29a93161b691975f403808f09fba 100644
--- a/src/pe/communication/rigidbody/Sphere.h
+++ b/src/pe/communication/rigidbody/Sphere.h
@@ -61,16 +61,17 @@ void unmarshal( mpi::RecvBuffer& buffer, SphereParameters& objparam );
 //*************************************************************************************************
 
 
-inline SphereID instantiate( mpi::RecvBuffer& buffer, const math::AABB& domain, const math::AABB& block, SphereID& newBody )
+inline SpherePtr instantiate( mpi::RecvBuffer& buffer, const math::AABB& domain, const math::AABB& block, SphereID& newBody )
 {
    SphereParameters subobjparam;
    unmarshal( buffer, subobjparam );
    correctBodyPosition(domain, block.center(), subobjparam.gpos_);
-   newBody = new Sphere( subobjparam.sid_, subobjparam.uid_, subobjparam.gpos_, subobjparam.rpos_, subobjparam.q_, subobjparam.radius_, subobjparam.material_, false, subobjparam.communicating_, subobjparam.infiniteMass_ );
-   newBody->setLinearVel( subobjparam.v_ );
-   newBody->setAngularVel( subobjparam.w_ );
-   newBody->MPITrait.setOwner( subobjparam.mpiTrait_.owner_ );
-   return newBody;
+   auto sp = std::make_unique<Sphere>( subobjparam.sid_, subobjparam.uid_, subobjparam.gpos_, subobjparam.rpos_, subobjparam.q_, subobjparam.radius_, subobjparam.material_, false, subobjparam.communicating_, subobjparam.infiniteMass_ );
+   sp->setLinearVel( subobjparam.v_ );
+   sp->setAngularVel( subobjparam.w_ );
+   sp->MPITrait.setOwner( subobjparam.mpiTrait_.owner_ );
+   newBody = sp.get();
+   return sp;
 }
 
 }  // namespace communication
diff --git a/src/pe/communication/rigidbody/Squirmer.h b/src/pe/communication/rigidbody/Squirmer.h
index 9029c93586fa36c5d3b4e29d2aaefe74a4d5a4d1..3c9b3fbe094c6cef756168c6bfd63078825f001c 100644
--- a/src/pe/communication/rigidbody/Squirmer.h
+++ b/src/pe/communication/rigidbody/Squirmer.h
@@ -61,16 +61,17 @@ void unmarshal( mpi::RecvBuffer& buffer, SquirmerParameters& objparam );
 //*************************************************************************************************
 
 
-inline SquirmerID instantiate( mpi::RecvBuffer& buffer, const math::AABB& domain, const math::AABB& block, SquirmerID& newBody )
+inline SquirmerPtr instantiate( mpi::RecvBuffer& buffer, const math::AABB& domain, const math::AABB& block, SquirmerID& newBody )
 {
    SquirmerParameters subobjparam;
    unmarshal( buffer, subobjparam );
    correctBodyPosition(domain, block.center(), subobjparam.gpos_);
-   newBody = new Squirmer( subobjparam.sid_, subobjparam.uid_, subobjparam.gpos_, subobjparam.rpos_, subobjparam.q_, subobjparam.radius_, subobjparam.squirmerVelocity_, subobjparam.squirmerBeta_, subobjparam.material_, false, subobjparam.communicating_, subobjparam.infiniteMass_ );
-   newBody->setLinearVel( subobjparam.v_ );
-   newBody->setAngularVel( subobjparam.w_ );
-   newBody->MPITrait.setOwner( subobjparam.mpiTrait_.owner_ );
-   return newBody;
+   auto sq = std::make_unique<Squirmer>( subobjparam.sid_, subobjparam.uid_, subobjparam.gpos_, subobjparam.rpos_, subobjparam.q_, subobjparam.radius_, subobjparam.squirmerVelocity_, subobjparam.squirmerBeta_, subobjparam.material_, false, subobjparam.communicating_, subobjparam.infiniteMass_ );
+   sq->setLinearVel( subobjparam.v_ );
+   sq->setAngularVel( subobjparam.w_ );
+   sq->MPITrait.setOwner( subobjparam.mpiTrait_.owner_ );
+   newBody = sq.get();
+   return sq;
 }
 
 }  // namespace communication
diff --git a/src/pe/communication/rigidbody/Union.h b/src/pe/communication/rigidbody/Union.h
index 69c2d6bb172ac8589b56f68e7ecc2946ac0b471f..544c131efff56184a82296a67c073784b0f585a4 100644
--- a/src/pe/communication/rigidbody/Union.h
+++ b/src/pe/communication/rigidbody/Union.h
@@ -72,12 +72,12 @@ void marshal( mpi::SendBuffer& buffer, const Union<BodyTypeTuple>& obj )
    buffer << static_cast<size_t> (obj.size());                  // Encoding the number of contained bodies
 
    // Encoding the contained primitives
-   const typename Union<BodyTypeTuple>::ConstIterator begin( obj.begin() );
-   const typename Union<BodyTypeTuple>::ConstIterator end  ( obj.end()   );
-   for(  typename Union<BodyTypeTuple>::ConstIterator body=begin; body!=end; ++body )
+   const typename Union<BodyTypeTuple>::const_iterator begin( obj.begin() );
+   const typename Union<BodyTypeTuple>::const_iterator end  ( obj.end()   );
+   for(  typename Union<BodyTypeTuple>::const_iterator body=begin; body!=end; ++body )
    {
       buffer << body->getTypeID();
-      MarshalDynamically<BodyTypeTuple>::execute( buffer, **body );
+      MarshalDynamically<BodyTypeTuple>::execute( buffer, *body );
    }
 }
 
@@ -110,34 +110,35 @@ void unmarshal( mpi::RecvBuffer& buffer, UnionParameters& objparam )
 
 
 template <typename BodyTypeTuple>
-inline Union<BodyTypeTuple>* instantiate( mpi::RecvBuffer& buffer, const math::AABB& domain, const math::AABB& block, Union<BodyTypeTuple>*& newBody )
+inline std::unique_ptr<Union<BodyTypeTuple>> instantiate( mpi::RecvBuffer& buffer, const math::AABB& domain, const math::AABB& block, Union<BodyTypeTuple>*& newBody )
 {
    UnionParameters subobjparam;
    unmarshal( buffer, subobjparam );
    correctBodyPosition(domain, block.center(), subobjparam.gpos_);
-   newBody = new Union<BodyTypeTuple>( subobjparam.sid_,
-                                       subobjparam.uid_,
-                                       subobjparam.gpos_,
-                                       subobjparam.rpos_,
-                                       subobjparam.q_,
-                                       false,
-                                       subobjparam.communicating_,
-                                       subobjparam.infiniteMass_ );
-   newBody->setLinearVel( subobjparam.v_ );
-   newBody->setAngularVel( subobjparam.w_ );
-   newBody->MPITrait.setOwner( subobjparam.mpiTrait_.owner_ );
+   auto un = std::make_unique<Union<BodyTypeTuple>>( subobjparam.sid_,
+                                                     subobjparam.uid_,
+                                                     subobjparam.gpos_,
+                                                     subobjparam.rpos_,
+                                                     subobjparam.q_,
+                                                     false,
+                                                     subobjparam.communicating_,
+                                                     subobjparam.infiniteMass_ );
+   un->setLinearVel( subobjparam.v_ );
+   un->setAngularVel( subobjparam.w_ );
+   un->MPITrait.setOwner( subobjparam.mpiTrait_.owner_ );
 
    // Decoding the contained primitives
    for( size_t i = 0; i < subobjparam.size_; ++i )
    {
       decltype ( static_cast<BodyID>(nullptr)->getTypeID() ) type;
       buffer >> type;
-      BodyID obj = UnmarshalDynamically<BodyTypeTuple>::execute(buffer, type, domain, block);
+      BodyPtr obj( UnmarshalDynamically<BodyTypeTuple>::execute(buffer, type, domain, block) );
       obj->setRemote( true );
-      newBody->add(obj);
+      un->add(std::move(obj));
    }
 
-   return newBody;
+   newBody = un.get();
+   return un;
 }
 
 }  // namespace communication
diff --git a/src/pe/contact/Contact.cpp b/src/pe/contact/Contact.cpp
index ed9e68e28cc7bff136a697d4b1716f3421f92a23..7bc7dc04127ff734e3153201f3037fb3663cf72e 100644
--- a/src/pe/contact/Contact.cpp
+++ b/src/pe/contact/Contact.cpp
@@ -62,17 +62,8 @@ Contact::Contact( GeomID g1, GeomID g2, const Vec3& gpos, const Vec3& normal, re
    WALBERLA_ASSERT_FLOAT_EQUAL( normal.sqrLength(), real_c(1), "Invalid contact normal\n" << g1 << "\n" << g2 );
    WALBERLA_ASSERT( !( b1_->hasInfiniteMass() && b2_->hasInfiniteMass() ), "Invalid contact between two rigid bodies with infinite masses" );
 
-   // Registering the contact with both attached rigid bodies
-//   if( !b1_->hasInfiniteMass() ) b1_->registerContact( this );
-//   if( !b2_->hasInfiniteMass() ) b2_->registerContact( this );
-
-   // Merging the contact graph
-//   if( !b1_->hasInfiniteMass() && !b2_->hasInfiniteMass() )
-//      mergeNodes( b1_, b2_ );
-
    // Debugging output
    WALBERLA_LOG_DETAIL( "         => contact-id = " << id_ );
-
 }
 //*************************************************************************************************
 
diff --git a/src/pe/cr/DEM.impl.h b/src/pe/cr/DEM.impl.h
index eb3a6b4e325616444808d21cf8d26db9fe9cc364..a3560e50fd94ba9daf71b71032deaa4ad3a9ec58 100644
--- a/src/pe/cr/DEM.impl.h
+++ b/src/pe/cr/DEM.impl.h
@@ -120,10 +120,6 @@ void DEMSolver<Integrator,ContactResolver>::timestep( real_t dt )
       {
          WALBERLA_LOG_DETAIL( "Time integration of body with system id " << bodyIt->getSystemID());// << "\n" << *bodyIt );
 
-         // Resetting the contact node and removing all attached contacts
-      //      bodyIt->resetNode();
-         bodyIt->clearContacts();
-
          // Checking the state of the body
          WALBERLA_ASSERT( bodyIt->checkInvariants(), "Invalid body state detected" );
          WALBERLA_ASSERT( !bodyIt->hasSuperBody(), "Invalid superordinate body detected" );
@@ -131,7 +127,7 @@ void DEMSolver<Integrator,ContactResolver>::timestep( real_t dt )
          // Moving the body according to the acting forces (don't move a sleeping body)
          if( bodyIt->isAwake() && !bodyIt->hasInfiniteMass() )
          {
-            integrate_( *bodyIt, dt, *this );
+            integrate_( bodyIt.getBodyID(), dt, *this );
          }
          
          // Resetting the acting forces
@@ -147,9 +143,8 @@ void DEMSolver<Integrator,ContactResolver>::timestep( real_t dt )
       if (tt_ != NULL) tt_->stop("Integration");
 
       // Reset forces of shadow copies
-      for( auto bodyIt = shadowStorage.begin(); bodyIt != shadowStorage.end(); ++bodyIt ) {
-         bodyIt->clearContacts();
-         bodyIt->resetForceAndTorque();
+      for( auto& body : shadowStorage ) {
+         body.resetForceAndTorque();
       }
 
    }
diff --git a/src/pe/cr/HCSITS.impl.h b/src/pe/cr/HCSITS.impl.h
index 457cd4c299c88731b6e35058e59706a34774ea59..a6d6258b5ca1fbe10212d08c061cfd4dc979b08c 100644
--- a/src/pe/cr/HCSITS.impl.h
+++ b/src/pe/cr/HCSITS.impl.h
@@ -290,7 +290,7 @@ inline void HardContactSemiImplicitTimesteppingSolvers::timestep( const real_t d
          body->index_ = j;
          WALBERLA_CHECK( body->hasInfiniteMass(), "Global bodies need to have infinite mass as they are not communicated!" );
 
-         initializeVelocityCorrections( *body, bodyCache.dv_[j], bodyCache.dw_[j], dt ); // use applied external forces to calculate starting velocity
+         initializeVelocityCorrections( body.getBodyID(), bodyCache.dv_[j], bodyCache.dw_[j], dt ); // use applied external forces to calculate starting velocity
 
          bodyCache.v_[j] = body->getLinearVel();
          bodyCache.w_[j] = body->getAngularVel();
@@ -300,7 +300,7 @@ inline void HardContactSemiImplicitTimesteppingSolvers::timestep( const real_t d
          body->wake(); // BUGFIX: Force awaking of all bodies!
          body->index_ = j;
 
-         initializeVelocityCorrections( *body, bodyCache.dv_[j], bodyCache.dw_[j], dt ); // use applied external forces to calculate starting velocity
+         initializeVelocityCorrections( body.getBodyID(), bodyCache.dv_[j], bodyCache.dw_[j], dt ); // use applied external forces to calculate starting velocity
 
          if( body->isAwake() && !body->hasInfiniteMass() ) {
             bodyCache.v_[j] = body->getLinearVel() + getGlobalLinearAcceleration() * dt;
@@ -317,7 +317,7 @@ inline void HardContactSemiImplicitTimesteppingSolvers::timestep( const real_t d
          body->wake(); // BUGFIX: Force awaking of all bodies!
          body->index_ = j;
 
-         initializeVelocityCorrections( *body, bodyCache.dv_[j], bodyCache.dw_[j], dt );
+         initializeVelocityCorrections( body.getBodyID(), bodyCache.dv_[j], bodyCache.dw_[j], dt );
 
          // Velocities of shadow copies will be initialized by velocity synchronization.
 #ifndef NDEBUG
@@ -349,7 +349,7 @@ inline void HardContactSemiImplicitTimesteppingSolvers::timestep( const real_t d
          body->index_ = j;
          WALBERLA_CHECK( body->hasInfiniteMass(), "Global bodies need to have infinite mass as they are not communicated!" );
 
-         initializeVelocityCorrections( *body, bodyCache.dv_[j], bodyCache.dw_[j], dt ); // use applied external forces to calculate starting velocity
+         initializeVelocityCorrections( body.getBodyID(), bodyCache.dv_[j], bodyCache.dw_[j], dt ); // use applied external forces to calculate starting velocity
 
          bodyCache.v_[j] = body->getLinearVel();
          bodyCache.w_[j] = body->getAngularVel();
@@ -457,10 +457,10 @@ inline void HardContactSemiImplicitTimesteppingSolvers::timestep( const real_t d
          WALBERLA_LOG_DETAIL( "Integrating position of global body " << *body << " with velocity " << body->getLinearVel() << "" );
          if (body->hasInfiniteMass())
          {
-            integratePositions( *body, bodyCache.v_[j], bodyCache.w_[j], dt );
+            integratePositions( body.getBodyID(), bodyCache.v_[j], bodyCache.w_[j], dt );
          } else
          {
-            integratePositions( *body, bodyCache.v_[j] + bodyCache.dv_[j], bodyCache.w_[j] + bodyCache.dw_[j], dt );
+            integratePositions( body.getBodyID(), bodyCache.v_[j] + bodyCache.dv_[j], bodyCache.w_[j] + bodyCache.dw_[j], dt );
          }
          WALBERLA_LOG_DETAIL( "Result:\n" << *body << "");
       }
@@ -476,11 +476,11 @@ inline void HardContactSemiImplicitTimesteppingSolvers::timestep( const real_t d
       BodyStorage& localStorage = (*storage)[0];
       BodyStorage& shadowStorage = (*storage)[1];
 
-      for( auto body = ConcatIterator<BodyStorage::Iterator>
+      for( auto body = ConcatIterator<BodyStorage::iterator>
            (localStorage.begin(),
             localStorage.end(),
             shadowStorage.begin(),
-            shadowStorage.end()); body != ConcatIterator<BodyStorage::Iterator>(); ++body )
+            shadowStorage.end()); body != ConcatIterator<BodyStorage::iterator>(); ++body )
       {
          if (!body->isCommunicating())
          {
@@ -492,10 +492,10 @@ inline void HardContactSemiImplicitTimesteppingSolvers::timestep( const real_t d
          WALBERLA_LOG_DETAIL( "Integrating position of body with infinite mass " << *body << " with velocity " << bodyCache.v_[j] << "" );
          if( body->hasInfiniteMass() )
          {
-            integratePositions( *body, bodyCache.v_[j], bodyCache.w_[j], dt );
+            integratePositions( &(*body), bodyCache.v_[j], bodyCache.w_[j], dt );
          } else
          {
-            integratePositions( *body, bodyCache.v_[j] + bodyCache.dv_[j], bodyCache.w_[j] + bodyCache.dw_[j], dt );
+            integratePositions( &(*body), bodyCache.v_[j] + bodyCache.dv_[j], bodyCache.w_[j] + bodyCache.dw_[j], dt );
          }
          WALBERLA_LOG_DETAIL( "Result:\n" << *body << "" );
       }
@@ -1433,7 +1433,7 @@ inline void HardContactSemiImplicitTimesteppingSolvers::parseVelocityCorrection(
 
       auto bodyIt = bodyStorage.find( objparam.sid_ );
       WALBERLA_ASSERT(bodyIt != bodyStorage.end(), "Body not found!");
-      BodyID b( *bodyIt );
+      BodyID b( bodyIt.getBodyID() );
       bodyCache.v_[b->index_] += relaxationParam_*objparam.dv_;
       bodyCache.w_[b->index_] += relaxationParam_*objparam.dw_;
 
@@ -1462,7 +1462,7 @@ inline void HardContactSemiImplicitTimesteppingSolvers::parseVelocityCorrectionS
 
       auto bodyIt = bodyStorage.find( objparam.sid_ );
       WALBERLA_ASSERT(bodyIt != bodyStorage.end(), "Body not found!");
-      BodyID b( *bodyIt );
+      BodyID b( bodyIt.getBodyID() );
       WALBERLA_ASSERT( b->isRemote(), "Update notification must only concern shadow copies." );
 
       bodyCache.v_[b->index_] = objparam.dv_;
@@ -1531,12 +1531,12 @@ inline void HardContactSemiImplicitTimesteppingSolvers::synchronizeVelocities( )
 
          WALBERLA_LOG_DETAIL( "Sending velocity correction " << bodyCache.dv_[i] << ", " << bodyCache.dw_[i] << " of body " << body->getSystemID() << " to owner process " << body->MPITrait.getOwner() << ".");
 
-         packNotificationWithoutSender(body->MPITrait.getOwner(), sb, RigidBodyVelocityCorrectionNotification( *(*body), bodyCache.dv_[i], bodyCache.dw_[i] ));
+         packNotificationWithoutSender(body->MPITrait.getOwner(), sb, RigidBodyVelocityCorrectionNotification( *body, bodyCache.dv_[i], bodyCache.dw_[i] ));
       }
 
       for( auto bodyIt = localStorage.begin(); bodyIt != localStorage.end(); ++bodyIt )
       {
-         BodyID b(*bodyIt);
+         BodyID b(bodyIt.getBodyID());
          for (auto shadows = b->MPITrait.beginShadowOwners(); shadows != b->MPITrait.endShadowOwners(); ++shadows )
          {
             recvRanks.insert(shadows->rank_);
@@ -1630,7 +1630,7 @@ inline void HardContactSemiImplicitTimesteppingSolvers::synchronizeVelocities( )
 
             mpi::SendBuffer& sb = syncVelBS.sendBuffer( shadow->rank_ );
             if (sb.isEmpty()) sb << walberla::uint8_c(0);
-            packNotificationWithoutSender(*shadow, sb, RigidBodyVelocityCorrectionNotification( *(*body), bodyCache.v_[i], bodyCache.w_[i] ));
+            packNotificationWithoutSender(*shadow, sb, RigidBodyVelocityCorrectionNotification( *body, bodyCache.v_[i], bodyCache.w_[i] ));
 
             WALBERLA_LOG_DETAIL( "Sending velocity update " << bodyCache.v_[i] << ", " << bodyCache.w_[i] << " of body " << body->getSystemID() << " to process " << *shadow << " having a shadow copy.");
          }
@@ -1638,7 +1638,7 @@ inline void HardContactSemiImplicitTimesteppingSolvers::synchronizeVelocities( )
 
       for( auto bodyIt = shadowStorage.begin(); bodyIt != shadowStorage.end(); ++bodyIt )
       {
-         BodyID b(*bodyIt);
+         BodyID b(bodyIt.getBodyID());
          recvRanks.insert(b->MPITrait.getOwner().rank_);
       }
    }
@@ -1795,10 +1795,6 @@ inline void HardContactSemiImplicitTimesteppingSolvers::integratePositions( Body
    if ( body->isFixed() )
       return;
 
-   // Resetting the contact node and removing all attached contacts
-   body->resetNode();
-   body->clearContacts();
-
    if( body->isAwake() )
    {
       if ( isSpeedLimiterActive() )
diff --git a/src/pe/cr/PlainIntegrator.impl.h b/src/pe/cr/PlainIntegrator.impl.h
index b96a6e2e0e46cac743062832c3ee4e8fc19f906b..e2a45b2f8eda5405f858db551101d17f545e6537 100644
--- a/src/pe/cr/PlainIntegrator.impl.h
+++ b/src/pe/cr/PlainIntegrator.impl.h
@@ -68,13 +68,9 @@ void PlainIntegratorSolver<Integrator>::timestep( const real_t dt )
          WALBERLA_ASSERT( bd->checkInvariants(), "Invalid body state detected" );
          WALBERLA_ASSERT( !bd->hasSuperBody(), "Invalid superordinate body detected" );
 
-         // Resetting the contact node and removing all attached contacts
-      //      bd->resetNode();
-         bd->clearContacts();
-
          // Moving the body according to the acting forces (don't move a sleeping body)
          if( bd->isAwake() && !bd->hasInfiniteMass() ) {
-            integrate_( *bd, dt, *this );
+            integrate_( bd.getBodyID(), dt, *this );
          }
 
          // Resetting the acting forces
diff --git a/src/pe/extern/libccd/.gitignore b/src/pe/extern/libccd/.gitignore
deleted file mode 100644
index e3fa61345b7e1907936cb6042b7afbdb57c1ae68..0000000000000000000000000000000000000000
--- a/src/pe/extern/libccd/.gitignore
+++ /dev/null
@@ -1,5 +0,0 @@
-*.o
-*.a
-ccd/config.h
-ccd/config.h.in
-
diff --git a/src/pe/extern/libccd/BSD-LICENSE b/src/pe/extern/libccd/BSD-LICENSE
deleted file mode 100644
index 1cb2dc53db0a2e70017314ee88fd59eb375920c5..0000000000000000000000000000000000000000
--- a/src/pe/extern/libccd/BSD-LICENSE
+++ /dev/null
@@ -1,44 +0,0 @@
-libccd
--------
-
-Copyright (c)2010-2012 Daniel Fiser <danfis@danfis.cz>,
-Intelligent and Mobile Robotics Group, Department of Cybernetics,
-Faculty of Electrical Engineering, Czech Technical University in Prague.
-All rights reserved.
-
-
-This work was supported by SYMBRION and REPLICATOR projects.
-The SYMBRION project is funded by European Commission within the work
-"Future and Emergent Technologies Proactive" under grant agreement no.
-216342.
-The REPLICATOR project is funded within the work programme "Cognitive
-Systems, Interaction, Robotics" under grant agreement no. 216240.
-http://www.symbrion.eu/
-http://www.replicators.eu/
-
-
-Redistribution and use in source and binary forms, with or without
-modification, are permitted provided that the following conditions are met:
-
- - Redistributions of source code must retain the above copyright notice,
-   this list of conditions and the following disclaimer.
-
- - Redistributions in binary form must reproduce the above copyright
-   notice, this list of conditions and the following disclaimer in the
-   documentation and/or other materials provided with the distribution.
-
- - Neither the name of the University nor the names of its contributors
-   may be used to endorse or promote products derived from this software
-   without specific prior written permission.
-
-THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
-AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
-IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
-ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
-LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
-CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
-SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
-INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
-CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
-ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
-POSSIBILITY OF SUCH DAMAGE.
diff --git a/src/pe/extern/libccd/alloc.h b/src/pe/extern/libccd/alloc.h
deleted file mode 100644
index 7cba3a4d71e81c399282cb2a0298a39acf407d51..0000000000000000000000000000000000000000
--- a/src/pe/extern/libccd/alloc.h
+++ /dev/null
@@ -1,50 +0,0 @@
-/***
- * libccd
- * ---------------------------------
- * Copyright (c)2010 Daniel Fiser <danfis@danfis.cz>
- *
- *
- *  This file is part of libccd.
- *
- *  Distributed under the OSI-approved BSD License (the "License");
- *  see accompanying file BDS-LICENSE for details or see
- *  <http://www.opensource.org/licenses/bsd-license.php>.
- *
- *  This software is distributed WITHOUT ANY WARRANTY; without even the
- *  implied warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.
- *  See the License for more information.
- */
-
-#ifndef __CCD_ALLOC_H__
-#define __CCD_ALLOC_H__
-
-#include <stdlib.h>
-
-#ifdef __cplusplus
-extern "C" {
-#endif /* __cplusplus */
-
-/**
- * Functions and macros required for memory allocation.
- */
-
-/* Memory allocation: */
-#define __CCD_ALLOC_MEMORY(type, ptr_old, size) \
-    (type *)realloc((void *)ptr_old, (size))
-
-/** Allocate memory for one element of type.  */
-#define CCD_ALLOC(type) \
-    __CCD_ALLOC_MEMORY(type, NULL, sizeof(type))
-
-/** Allocate memory for array of elements of type type.  */
-#define CCD_ALLOC_ARR(type, num_elements) \
-    __CCD_ALLOC_MEMORY(type, NULL, sizeof(type) * (num_elements))
-
-#define CCD_REALLOC_ARR(ptr, type, num_elements) \
-    __CCD_ALLOC_MEMORY(type, ptr, sizeof(type) * (num_elements))
-
-#ifdef __cplusplus
-} /* extern "C" */
-#endif /* __cplusplus */
-
-#endif /* __CCD_ALLOC_H__ */
diff --git a/src/pe/extern/libccd/ccd.c b/src/pe/extern/libccd/ccd.c
deleted file mode 100644
index 79b2f42c91287df03239384228afe79aee6d6935..0000000000000000000000000000000000000000
--- a/src/pe/extern/libccd/ccd.c
+++ /dev/null
@@ -1,1001 +0,0 @@
-/***
- * libccd
- * ---------------------------------
- * Copyright (c)2012 Daniel Fiser <danfis@danfis.cz>
- *
- *
- *  This file is part of libccd.
- *
- *  Distributed under the OSI-approved BSD License (the "License");
- *  see accompanying file BDS-LICENSE for details or see
- *  <http://www.opensource.org/licenses/bsd-license.php>.
- *
- *  This software is distributed WITHOUT ANY WARRANTY; without even the
- *  implied warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.
- *  See the License for more information.
- */
-
-#include <stdio.h>
-#include <float.h>
-#include <ccd/ccd.h>
-#include <ccd/vec3.h>
-#include "simplex.h"
-#include "polytope.h"
-#include "alloc.h"
-#include "dbg.h"
-
-
-/** Performs GJK algorithm. Returns 0 if intersection was found and simplex
- *  is filled with resulting polytope. */
-static int __ccdGJK(const void *obj1, const void *obj2,
-                    const ccd_t *ccd, ccd_simplex_t *simplex);
-
-/** Performs GJK+EPA algorithm. Returns 0 if intersection was found and
- *  pt is filled with resulting polytope and nearest with pointer to
- *  nearest element (vertex, edge, face) of polytope to origin. */
-static int __ccdGJKEPA(const void *obj1, const void *obj2,
-                       const ccd_t *ccd,
-                       ccd_pt_t *pt, ccd_pt_el_t **nearest);
-
-
-/** Returns true if simplex contains origin.
- *  This function also alteres simplex and dir according to further
- *  processing of GJK algorithm. */
-static int doSimplex(ccd_simplex_t *simplex, ccd_vec3_t *dir);
-static int doSimplex2(ccd_simplex_t *simplex, ccd_vec3_t *dir);
-static int doSimplex3(ccd_simplex_t *simplex, ccd_vec3_t *dir);
-static int doSimplex4(ccd_simplex_t *simplex, ccd_vec3_t *dir);
-
-/** d = a x b x c */
-_ccd_inline void tripleCross(const ccd_vec3_t *a, const ccd_vec3_t *b,
-                             const ccd_vec3_t *c, ccd_vec3_t *d);
-
-
-/** Transforms simplex to polytope. It is assumed that simplex has 4
- *  vertices. */
-static int simplexToPolytope4(const void *obj1, const void *obj2,
-                              const ccd_t *ccd,
-                              ccd_simplex_t *simplex,
-                              ccd_pt_t *pt, ccd_pt_el_t **nearest);
-
-/** Transforms simplex to polytope, three vertices required */
-static int simplexToPolytope3(const void *obj1, const void *obj2,
-                              const ccd_t *ccd,
-                              const ccd_simplex_t *simplex,
-                              ccd_pt_t *pt, ccd_pt_el_t **nearest);
-
-/** Transforms simplex to polytope, two vertices required */
-static int simplexToPolytope2(const void *obj1, const void *obj2,
-                              const ccd_t *ccd,
-                              const ccd_simplex_t *simplex,
-                              ccd_pt_t *pt, ccd_pt_el_t **nearest);
-
-/** Expands polytope using new vertex v.
- *  Return 0 on success, -2 on memory allocation failure.*/
-static int expandPolytope(ccd_pt_t *pt, ccd_pt_el_t *el,
-                          const ccd_support_t *newv);
-
-/** Finds next support point (at stores it in out argument).
- *  Returns 0 on success, -1 otherwise */
-static int nextSupport(const void *obj1, const void *obj2, const ccd_t *ccd,
-                       const ccd_pt_el_t *el,
-                       ccd_support_t *out);
-
-
-
-void ccdFirstDirDefault(const void *o1, const void *o2, ccd_vec3_t *dir)
-{
-    (void)(o1);
-    (void)(o2);
-
-    ccdVec3Set(dir, CCD_ONE, CCD_ZERO, CCD_ZERO);
-}
-
-int ccdGJKIntersect(const void *obj1, const void *obj2, const ccd_t *ccd)
-{
-    ccd_simplex_t simplex;
-    return __ccdGJK(obj1, obj2, ccd, &simplex) == 0;
-}
-
-int ccdGJKSeparate(const void *obj1, const void *obj2, const ccd_t *ccd,
-                   ccd_vec3_t *sep)
-{
-    ccd_pt_t polytope;
-    ccd_pt_el_t *nearest;
-    int ret;
-
-    ccdPtInit(&polytope);
-
-    ret = __ccdGJKEPA(obj1, obj2, ccd, &polytope, &nearest);
-
-    // set separation vector
-    if (nearest)
-        ccdVec3Copy(sep, &nearest->witness);
-
-    ccdPtDestroy(&polytope);
-
-    return ret;
-}
-
-
-static int penEPAPosCmp(const void *a, const void *b)
-{
-    ccd_pt_vertex_t *v1, *v2;
-    v1 = *(ccd_pt_vertex_t **)a;
-    v2 = *(ccd_pt_vertex_t **)b;
-
-    if (ccdEq(v1->dist, v2->dist)){
-        return 0;
-    }else if (v1->dist < v2->dist){
-        return -1;
-    }else{
-        return 1;
-    }
-}
-
-static int penEPAPos(const ccd_pt_t *pt, const ccd_pt_el_t * nearest,
-                     ccd_vec3_t *pos)
-{
-    (void)(nearest);
-
-    ccd_pt_vertex_t *v;
-    ccd_pt_vertex_t **vs;
-    size_t i, len;
-    ccd_real_t scale;
-
-    // compute median
-    len = 0;
-    ccdListForEachEntry(&pt->vertices, v, ccd_pt_vertex_t, list){
-        len++;
-    }
-
-    vs = CCD_ALLOC_ARR(ccd_pt_vertex_t *, len);
-    if (vs == NULL)
-        return -1;
-
-    i = 0;
-    ccdListForEachEntry(&pt->vertices, v, ccd_pt_vertex_t, list){
-        vs[i++] = v;
-    }
-
-    qsort(vs, len, sizeof(ccd_pt_vertex_t *), penEPAPosCmp);
-
-    ccdVec3Set(pos, CCD_ZERO, CCD_ZERO, CCD_ZERO);
-    scale = CCD_ZERO;
-    if (len % 2 == 1)
-        len++;
-
-    for (i = 0; i < len / 2; i++){
-        ccdVec3Add(pos, &vs[i]->v.v1);
-        ccdVec3Add(pos, &vs[i]->v.v2);
-        scale += CCD_REAL(2.);
-    }
-    ccdVec3Scale(pos, CCD_ONE / scale);
-
-    free(vs);
-
-    return 0;
-}
-
-int ccdGJKPenetration(const void *obj1, const void *obj2, const ccd_t *ccd,
-                      ccd_real_t *depth, ccd_vec3_t *dir, ccd_vec3_t *pos)
-{
-    ccd_pt_t polytope;
-    ccd_pt_el_t *nearest;
-    int ret;
-
-    ccdPtInit(&polytope);
-
-    ret = __ccdGJKEPA(obj1, obj2, ccd, &polytope, &nearest);
-
-    // set separation vector
-    if (ret == 0 && nearest){
-        // compute depth of penetration
-        *depth = CCD_SQRT(nearest->dist);
-
-        // store normalized direction vector
-        ccdVec3Copy(dir, &nearest->witness);
-        ccdVec3Normalize(dir);
-
-        // compute position
-        if (penEPAPos(&polytope, nearest, pos) != 0){
-            ccdPtDestroy(&polytope);
-            return -2;
-        }
-    }
-
-    ccdPtDestroy(&polytope);
-
-    return ret;
-}
-
-
-static int __ccdGJK(const void *obj1, const void *obj2,
-                    const ccd_t *ccd, ccd_simplex_t *simplex)
-{
-    unsigned long iterations;
-    ccd_vec3_t dir; // direction vector
-    ccd_support_t last; // last support point
-    int do_simplex_res;
-
-    // initialize simplex struct
-    ccdSimplexInit(simplex);
-
-    // get first direction
-    ccd->first_dir(obj1, obj2, &dir);
-    // get first support point
-    __ccdSupport(obj1, obj2, &dir, ccd, &last);
-    // and add this point to simplex as last one
-    ccdSimplexAdd(simplex, &last);
-
-    // set up direction vector to as (O - last) which is exactly -last
-    ccdVec3Copy(&dir, &last.v);
-    ccdVec3Scale(&dir, -CCD_ONE);
-
-    // start iterations
-    for (iterations = 0UL; iterations < ccd->max_iterations; ++iterations) {
-        // obtain support point
-        __ccdSupport(obj1, obj2, &dir, ccd, &last);
-
-        // check if farthest point in Minkowski difference in direction dir
-        // isn't somewhere before origin (the test on negative dot product)
-        // - because if it is, objects are not intersecting at all.
-        if (ccdVec3Dot(&last.v, &dir) < CCD_ZERO){
-            return -1; // intersection not found
-        }
-
-        // add last support vector to simplex
-        ccdSimplexAdd(simplex, &last);
-
-        // if doSimplex returns 1 if objects intersect, -1 if objects don't
-        // intersect and 0 if algorithm should continue
-        do_simplex_res = doSimplex(simplex, &dir);
-        if (do_simplex_res == 1){
-            return 0; // intersection found
-        }else if (do_simplex_res == -1){
-            return -1; // intersection not found
-        }
-
-        if (ccdIsZero(ccdVec3Len2(&dir))){
-            return -1; // intersection not found
-        }
-    }
-
-    // intersection wasn't found
-    return -1;
-}
-
-static int __ccdGJKEPA(const void *obj1, const void *obj2,
-                       const ccd_t *ccd,
-                       ccd_pt_t *polytope, ccd_pt_el_t **nearest)
-{
-    ccd_simplex_t simplex;
-    ccd_support_t supp; // support point
-    int ret, size;
-
-    *nearest = NULL;
-
-    // run GJK and obtain terminal simplex
-    ret = __ccdGJK(obj1, obj2, ccd, &simplex);
-    if (ret != 0)
-        return -1;
-
-    // transform simplex to polytope - simplex won't be used anymore
-    size = ccdSimplexSize(&simplex);
-    if (size == 4){
-        ret = simplexToPolytope4(obj1, obj2, ccd, &simplex, polytope, nearest);
-    }else if (size == 3){
-        ret = simplexToPolytope3(obj1, obj2, ccd, &simplex, polytope, nearest);
-    }else{ // size == 2
-        ret = simplexToPolytope2(obj1, obj2, ccd, &simplex, polytope, nearest);
-    }
-
-    if (ret == -1){
-        // touching contact
-        return 0;
-    }else if (ret == -2){
-        // failed memory allocation
-        return -2;
-    }
-
-
-    while (1){
-        // get triangle nearest to origin
-        *nearest = ccdPtNearest(polytope);
-
-        // get next support point
-        if (nextSupport(obj1, obj2, ccd, *nearest, &supp) != 0)
-            break;
-
-        // expand nearest triangle using new point - supp
-        if (expandPolytope(polytope, *nearest, &supp) != 0)
-            return -2;
-    }
-
-    return 0;
-}
-
-
-
-static int doSimplex2(ccd_simplex_t *simplex, ccd_vec3_t *dir)
-{
-    const ccd_support_t *A, *B;
-    ccd_vec3_t AB, AO, tmp;
-    ccd_real_t dot;
-
-    // get last added as A
-    A = ccdSimplexLast(simplex);
-    // get the other point
-    B = ccdSimplexPoint(simplex, 0);
-    // compute AB oriented segment
-    ccdVec3Sub2(&AB, &B->v, &A->v);
-    // compute AO vector
-    ccdVec3Copy(&AO, &A->v);
-    ccdVec3Scale(&AO, -CCD_ONE);
-
-    // dot product AB . AO
-    dot = ccdVec3Dot(&AB, &AO);
-
-    // check if origin doesn't lie on AB segment
-    ccdVec3Cross(&tmp, &AB, &AO);
-    if (ccdIsZero(ccdVec3Len2(&tmp)) && dot > CCD_ZERO){
-        return 1;
-    }
-
-    // check if origin is in area where AB segment is
-    if (ccdIsZero(dot) || dot < CCD_ZERO){
-        // origin is in outside are of A
-        ccdSimplexSet(simplex, 0, A);
-        ccdSimplexSetSize(simplex, 1);
-        ccdVec3Copy(dir, &AO);
-    }else{
-        // origin is in area where AB segment is
-
-        // keep simplex untouched and set direction to
-        // AB x AO x AB
-        tripleCross(&AB, &AO, &AB, dir);
-    }
-
-    return 0;
-}
-
-static int doSimplex3(ccd_simplex_t *simplex, ccd_vec3_t *dir)
-{
-    const ccd_support_t *A, *B, *C;
-    ccd_vec3_t AO, AB, AC, ABC, tmp;
-    ccd_real_t dot, dist;
-
-    // get last added as A
-    A = ccdSimplexLast(simplex);
-    // get the other points
-    B = ccdSimplexPoint(simplex, 1);
-    C = ccdSimplexPoint(simplex, 0);
-
-    // check touching contact
-    dist = ccdVec3PointTriDist2(ccd_vec3_origin, &A->v, &B->v, &C->v, NULL);
-    if (ccdIsZero(dist)){
-        return 1;
-    }
-
-    // check if triangle is really triangle (has area > 0)
-    // if not simplex can't be expanded and thus no itersection is found
-    if (ccdVec3Eq(&A->v, &B->v) || ccdVec3Eq(&A->v, &C->v)){
-        return -1;
-    }
-
-    // compute AO vector
-    ccdVec3Copy(&AO, &A->v);
-    ccdVec3Scale(&AO, -CCD_ONE);
-
-    // compute AB and AC segments and ABC vector (perpendircular to triangle)
-    ccdVec3Sub2(&AB, &B->v, &A->v);
-    ccdVec3Sub2(&AC, &C->v, &A->v);
-    ccdVec3Cross(&ABC, &AB, &AC);
-
-    ccdVec3Cross(&tmp, &ABC, &AC);
-    dot = ccdVec3Dot(&tmp, &AO);
-    if (ccdIsZero(dot) || dot > CCD_ZERO){
-        dot = ccdVec3Dot(&AC, &AO);
-        if (ccdIsZero(dot) || dot > CCD_ZERO){
-            // C is already in place
-            ccdSimplexSet(simplex, 1, A);
-            ccdSimplexSetSize(simplex, 2);
-            tripleCross(&AC, &AO, &AC, dir);
-        }else{
-ccd_do_simplex3_45:
-            dot = ccdVec3Dot(&AB, &AO);
-            if (ccdIsZero(dot) || dot > CCD_ZERO){
-                ccdSimplexSet(simplex, 0, B);
-                ccdSimplexSet(simplex, 1, A);
-                ccdSimplexSetSize(simplex, 2);
-                tripleCross(&AB, &AO, &AB, dir);
-            }else{
-                ccdSimplexSet(simplex, 0, A);
-                ccdSimplexSetSize(simplex, 1);
-                ccdVec3Copy(dir, &AO);
-            }
-        }
-    }else{
-        ccdVec3Cross(&tmp, &AB, &ABC);
-        dot = ccdVec3Dot(&tmp, &AO);
-        if (ccdIsZero(dot) || dot > CCD_ZERO){
-            goto ccd_do_simplex3_45;
-        }else{
-            dot = ccdVec3Dot(&ABC, &AO);
-            if (ccdIsZero(dot) || dot > CCD_ZERO){
-                ccdVec3Copy(dir, &ABC);
-            }else{
-                ccd_support_t Ctmp;
-                ccdSupportCopy(&Ctmp, C);
-                ccdSimplexSet(simplex, 0, B);
-                ccdSimplexSet(simplex, 1, &Ctmp);
-
-                ccdVec3Copy(dir, &ABC);
-                ccdVec3Scale(dir, -CCD_ONE);
-            }
-        }
-    }
-
-    return 0;
-}
-
-static int doSimplex4(ccd_simplex_t *simplex, ccd_vec3_t *dir)
-{
-    const ccd_support_t *A, *B, *C, *D;
-    ccd_vec3_t AO, AB, AC, AD, ABC, ACD, ADB;
-    int B_on_ACD, C_on_ADB, D_on_ABC;
-    int AB_O, AC_O, AD_O;
-    ccd_real_t dist;
-
-    // get last added as A
-    A = ccdSimplexLast(simplex);
-    // get the other points
-    B = ccdSimplexPoint(simplex, 2);
-    C = ccdSimplexPoint(simplex, 1);
-    D = ccdSimplexPoint(simplex, 0);
-
-    // check if tetrahedron is really tetrahedron (has volume > 0)
-    // if it is not simplex can't be expanded and thus no intersection is
-    // found
-    dist = ccdVec3PointTriDist2(&A->v, &B->v, &C->v, &D->v, NULL);
-    if (ccdIsZero(dist)){
-        return -1;
-    }
-
-    // check if origin lies on some of tetrahedron's face - if so objects
-    // intersect
-    dist = ccdVec3PointTriDist2(ccd_vec3_origin, &A->v, &B->v, &C->v, NULL);
-    if (ccdIsZero(dist))
-        return 1;
-    dist = ccdVec3PointTriDist2(ccd_vec3_origin, &A->v, &C->v, &D->v, NULL);
-    if (ccdIsZero(dist))
-        return 1;
-    dist = ccdVec3PointTriDist2(ccd_vec3_origin, &A->v, &B->v, &D->v, NULL);
-    if (ccdIsZero(dist))
-        return 1;
-    dist = ccdVec3PointTriDist2(ccd_vec3_origin, &B->v, &C->v, &D->v, NULL);
-    if (ccdIsZero(dist))
-        return 1;
-
-    // compute AO, AB, AC, AD segments and ABC, ACD, ADB normal vectors
-    ccdVec3Copy(&AO, &A->v);
-    ccdVec3Scale(&AO, -CCD_ONE);
-    ccdVec3Sub2(&AB, &B->v, &A->v);
-    ccdVec3Sub2(&AC, &C->v, &A->v);
-    ccdVec3Sub2(&AD, &D->v, &A->v);
-    ccdVec3Cross(&ABC, &AB, &AC);
-    ccdVec3Cross(&ACD, &AC, &AD);
-    ccdVec3Cross(&ADB, &AD, &AB);
-
-    // side (positive or negative) of B, C, D relative to planes ACD, ADB
-    // and ABC respectively
-    B_on_ACD = ccdSign(ccdVec3Dot(&ACD, &AB));
-    C_on_ADB = ccdSign(ccdVec3Dot(&ADB, &AC));
-    D_on_ABC = ccdSign(ccdVec3Dot(&ABC, &AD));
-
-    // whether origin is on same side of ACD, ADB, ABC as B, C, D
-    // respectively
-    AB_O = ccdSign(ccdVec3Dot(&ACD, &AO)) == B_on_ACD;
-    AC_O = ccdSign(ccdVec3Dot(&ADB, &AO)) == C_on_ADB;
-    AD_O = ccdSign(ccdVec3Dot(&ABC, &AO)) == D_on_ABC;
-
-    if (AB_O && AC_O && AD_O){
-        // origin is in tetrahedron
-        return 1;
-
-    // rearrange simplex to triangle and call doSimplex3()
-    }else if (!AB_O){
-        // B is farthest from the origin among all of the tetrahedron's
-        // points, so remove it from the list and go on with the triangle
-        // case
-
-        // D and C are in place
-        ccdSimplexSet(simplex, 2, A);
-        ccdSimplexSetSize(simplex, 3);
-    }else if (!AC_O){
-        // C is farthest
-        ccdSimplexSet(simplex, 1, D);
-        ccdSimplexSet(simplex, 0, B);
-        ccdSimplexSet(simplex, 2, A);
-        ccdSimplexSetSize(simplex, 3);
-    }else{ // (!AD_O)
-        ccdSimplexSet(simplex, 0, C);
-        ccdSimplexSet(simplex, 1, B);
-        ccdSimplexSet(simplex, 2, A);
-        ccdSimplexSetSize(simplex, 3);
-    }
-
-    return doSimplex3(simplex, dir);
-}
-
-static int doSimplex(ccd_simplex_t *simplex, ccd_vec3_t *dir)
-{
-    if (ccdSimplexSize(simplex) == 2){
-        // simplex contains segment only one segment
-        return doSimplex2(simplex, dir);
-    }else if (ccdSimplexSize(simplex) == 3){
-        // simplex contains triangle
-        return doSimplex3(simplex, dir);
-    }else{ // ccdSimplexSize(simplex) == 4
-        // tetrahedron - this is the only shape which can encapsule origin
-        // so doSimplex4() also contains test on it
-        return doSimplex4(simplex, dir);
-    }
-}
-
-
-_ccd_inline void tripleCross(const ccd_vec3_t *a, const ccd_vec3_t *b,
-                             const ccd_vec3_t *c, ccd_vec3_t *d)
-{
-    ccd_vec3_t e;
-    ccdVec3Cross(&e, a, b);
-    ccdVec3Cross(d, &e, c);
-}
-
-
-
-/** Transforms simplex to polytope. It is assumed that simplex has 4
- *  vertices! */
-static int simplexToPolytope4(const void *obj1, const void *obj2,
-                              const ccd_t *ccd,
-                              ccd_simplex_t *simplex,
-                              ccd_pt_t *pt, ccd_pt_el_t **nearest)
-{
-    const ccd_support_t *a, *b, *c, *d;
-    int use_polytope3;
-    ccd_real_t dist;
-    ccd_pt_vertex_t *v[4];
-    ccd_pt_edge_t *e[6];
-    size_t i;
-
-    a = ccdSimplexPoint(simplex, 0);
-    b = ccdSimplexPoint(simplex, 1);
-    c = ccdSimplexPoint(simplex, 2);
-    d = ccdSimplexPoint(simplex, 3);
-
-    // check if origin lies on some of tetrahedron's face - if so use
-    // simplexToPolytope3()
-    use_polytope3 = 0;
-    dist = ccdVec3PointTriDist2(ccd_vec3_origin, &a->v, &b->v, &c->v, NULL);
-    if (ccdIsZero(dist)){
-        use_polytope3 = 1;
-    }
-    dist = ccdVec3PointTriDist2(ccd_vec3_origin, &a->v, &c->v, &d->v, NULL);
-    if (ccdIsZero(dist)){
-        use_polytope3 = 1;
-        ccdSimplexSet(simplex, 1, c);
-        ccdSimplexSet(simplex, 2, d);
-    }
-    dist = ccdVec3PointTriDist2(ccd_vec3_origin, &a->v, &b->v, &d->v, NULL);
-    if (ccdIsZero(dist)){
-        use_polytope3 = 1;
-        ccdSimplexSet(simplex, 2, d);
-    }
-    dist = ccdVec3PointTriDist2(ccd_vec3_origin, &b->v, &c->v, &d->v, NULL);
-    if (ccdIsZero(dist)){
-        use_polytope3 = 1;
-        ccdSimplexSet(simplex, 0, b);
-        ccdSimplexSet(simplex, 1, c);
-        ccdSimplexSet(simplex, 2, d);
-    }
-
-    if (use_polytope3){
-        ccdSimplexSetSize(simplex, 3);
-        return simplexToPolytope3(obj1, obj2, ccd, simplex, pt, nearest);
-    }
-
-    // no touching contact - simply create tetrahedron
-    for (i = 0; i < 4; i++){
-        v[i] = ccdPtAddVertex(pt, ccdSimplexPoint(simplex, (int)i));
-    }
-    
-    e[0] = ccdPtAddEdge(pt, v[0], v[1]);
-    e[1] = ccdPtAddEdge(pt, v[1], v[2]);
-    e[2] = ccdPtAddEdge(pt, v[2], v[0]);
-    e[3] = ccdPtAddEdge(pt, v[3], v[0]);
-    e[4] = ccdPtAddEdge(pt, v[3], v[1]);
-    e[5] = ccdPtAddEdge(pt, v[3], v[2]);
-
-    // ccdPtAdd*() functions return NULL either if the memory allocation
-    // failed of if any of the input pointers are NULL, so the bad
-    // allocation can be checked by the last calls of ccdPtAddFace()
-    // because the rest of the bad allocations eventually "bubble up" here
-    if (ccdPtAddFace(pt, e[0], e[1], e[2]) == NULL
-            || ccdPtAddFace(pt, e[3], e[4], e[0]) == NULL
-            || ccdPtAddFace(pt, e[4], e[5], e[1]) == NULL
-            || ccdPtAddFace(pt, e[5], e[3], e[2]) == NULL){
-        return -2;
-    }
-
-    return 0;
-}
-
-/** Transforms simplex to polytope, three vertices required */
-static int simplexToPolytope3(const void *obj1, const void *obj2,
-                              const ccd_t *ccd,
-                              const ccd_simplex_t *simplex,
-                              ccd_pt_t *pt, ccd_pt_el_t **nearest)
-{
-    const ccd_support_t *a, *b, *c;
-    ccd_support_t d, d2;
-    ccd_vec3_t ab, ac, dir;
-    ccd_pt_vertex_t *v[5];
-    ccd_pt_edge_t *e[9];
-    ccd_real_t dist, dist2;
-
-    *nearest = NULL;
-
-    a = ccdSimplexPoint(simplex, 0);
-    b = ccdSimplexPoint(simplex, 1);
-    c = ccdSimplexPoint(simplex, 2);
-
-    // If only one triangle left from previous GJK run origin lies on this
-    // triangle. So it is necessary to expand triangle into two
-    // tetrahedrons connected with base (which is exactly abc triangle).
-
-    // get next support point in direction of normal of triangle
-    ccdVec3Sub2(&ab, &b->v, &a->v);
-    ccdVec3Sub2(&ac, &c->v, &a->v);
-    ccdVec3Cross(&dir, &ab, &ac);
-    __ccdSupport(obj1, obj2, &dir, ccd, &d);
-    dist = ccdVec3PointTriDist2(&d.v, &a->v, &b->v, &c->v, NULL);
-
-    // and second one take in opposite direction
-    ccdVec3Scale(&dir, -CCD_ONE);
-    __ccdSupport(obj1, obj2, &dir, ccd, &d2);
-    dist2 = ccdVec3PointTriDist2(&d2.v, &a->v, &b->v, &c->v, NULL);
-
-    // check if face isn't already on edge of minkowski sum and thus we
-    // have touching contact
-    if (ccdIsZero(dist) || ccdIsZero(dist2)){
-        v[0] = ccdPtAddVertex(pt, a);
-        v[1] = ccdPtAddVertex(pt, b);
-        v[2] = ccdPtAddVertex(pt, c);
-        e[0] = ccdPtAddEdge(pt, v[0], v[1]);
-        e[1] = ccdPtAddEdge(pt, v[1], v[2]);
-        e[2] = ccdPtAddEdge(pt, v[2], v[0]);
-        *nearest = (ccd_pt_el_t *)ccdPtAddFace(pt, e[0], e[1], e[2]);
-        if (*nearest == NULL)
-            return -2;
-
-        return -1;
-    }
-
-    // form polyhedron
-    v[0] = ccdPtAddVertex(pt, a);
-    v[1] = ccdPtAddVertex(pt, b);
-    v[2] = ccdPtAddVertex(pt, c);
-    v[3] = ccdPtAddVertex(pt, &d);
-    v[4] = ccdPtAddVertex(pt, &d2);
-
-    e[0] = ccdPtAddEdge(pt, v[0], v[1]);
-    e[1] = ccdPtAddEdge(pt, v[1], v[2]);
-    e[2] = ccdPtAddEdge(pt, v[2], v[0]);
-
-    e[3] = ccdPtAddEdge(pt, v[3], v[0]);
-    e[4] = ccdPtAddEdge(pt, v[3], v[1]);
-    e[5] = ccdPtAddEdge(pt, v[3], v[2]);
-
-    e[6] = ccdPtAddEdge(pt, v[4], v[0]);
-    e[7] = ccdPtAddEdge(pt, v[4], v[1]);
-    e[8] = ccdPtAddEdge(pt, v[4], v[2]);
-
-    if (ccdPtAddFace(pt, e[3], e[4], e[0]) == NULL
-            || ccdPtAddFace(pt, e[4], e[5], e[1]) == NULL
-            || ccdPtAddFace(pt, e[5], e[3], e[2]) == NULL
-
-            || ccdPtAddFace(pt, e[6], e[7], e[0]) == NULL
-            || ccdPtAddFace(pt, e[7], e[8], e[1]) == NULL
-            || ccdPtAddFace(pt, e[8], e[6], e[2]) == NULL){
-        return -2;
-    }
-
-    return 0;
-}
-
-/** Transforms simplex to polytope, two vertices required */
-static int simplexToPolytope2(const void *obj1, const void *obj2,
-                              const ccd_t *ccd,
-                              const ccd_simplex_t *simplex,
-                              ccd_pt_t *pt, ccd_pt_el_t **nearest)
-{
-    const ccd_support_t *a, *b;
-    ccd_vec3_t ab, ac, dir;
-    ccd_support_t supp[4];
-    ccd_pt_vertex_t *v[6];
-    ccd_pt_edge_t *e[12];
-    size_t i;
-    int found;
-
-    a = ccdSimplexPoint(simplex, 0);
-    b = ccdSimplexPoint(simplex, 1);
-
-    // This situation is a bit tricky. If only one segment comes from
-    // previous run of GJK - it means that either this segment is on
-    // minkowski edge (and thus we have touch contact) or it it isn't and
-    // therefore segment is somewhere *inside* minkowski sum and it *must*
-    // be possible to fully enclose this segment with polyhedron formed by
-    // at least 8 triangle faces.
-
-    // get first support point (any)
-    found = 0;
-    for (i = 0; i < ccd_points_on_sphere_len; i++){
-        __ccdSupport(obj1, obj2, &ccd_points_on_sphere[i], ccd, &supp[0]);
-        if (!ccdVec3Eq(&a->v, &supp[0].v) && !ccdVec3Eq(&b->v, &supp[0].v)){
-            found = 1;
-            break;
-        }
-    }
-    if (!found)
-        goto simplexToPolytope2_touching_contact;
-
-    // get second support point in opposite direction than supp[0]
-    ccdVec3Copy(&dir, &supp[0].v);
-    ccdVec3Scale(&dir, -CCD_ONE);
-    __ccdSupport(obj1, obj2, &dir, ccd, &supp[1]);
-    if (ccdVec3Eq(&a->v, &supp[1].v) || ccdVec3Eq(&b->v, &supp[1].v))
-        goto simplexToPolytope2_touching_contact;
-
-    // next will be in direction of normal of triangle a,supp[0],supp[1]
-    ccdVec3Sub2(&ab, &supp[0].v, &a->v);
-    ccdVec3Sub2(&ac, &supp[1].v, &a->v);
-    ccdVec3Cross(&dir, &ab, &ac);
-    __ccdSupport(obj1, obj2, &dir, ccd, &supp[2]);
-    if (ccdVec3Eq(&a->v, &supp[2].v) || ccdVec3Eq(&b->v, &supp[2].v))
-        goto simplexToPolytope2_touching_contact;
-
-    // and last one will be in opposite direction
-    ccdVec3Scale(&dir, -CCD_ONE);
-    __ccdSupport(obj1, obj2, &dir, ccd, &supp[3]);
-    if (ccdVec3Eq(&a->v, &supp[3].v) || ccdVec3Eq(&b->v, &supp[3].v))
-        goto simplexToPolytope2_touching_contact;
-
-    goto simplexToPolytope2_not_touching_contact;
-simplexToPolytope2_touching_contact:
-    v[0] = ccdPtAddVertex(pt, a);
-    v[1] = ccdPtAddVertex(pt, b);
-    *nearest = (ccd_pt_el_t *)ccdPtAddEdge(pt, v[0], v[1]);
-    if (*nearest == NULL)
-        return -2;
-
-    return -1;
-
-simplexToPolytope2_not_touching_contact:
-    // form polyhedron
-    v[0] = ccdPtAddVertex(pt, a);
-    v[1] = ccdPtAddVertex(pt, &supp[0]);
-    v[2] = ccdPtAddVertex(pt, b);
-    v[3] = ccdPtAddVertex(pt, &supp[1]);
-    v[4] = ccdPtAddVertex(pt, &supp[2]);
-    v[5] = ccdPtAddVertex(pt, &supp[3]);
-
-    e[0] = ccdPtAddEdge(pt, v[0], v[1]);
-    e[1] = ccdPtAddEdge(pt, v[1], v[2]);
-    e[2] = ccdPtAddEdge(pt, v[2], v[3]);
-    e[3] = ccdPtAddEdge(pt, v[3], v[0]);
-
-    e[4] = ccdPtAddEdge(pt, v[4], v[0]);
-    e[5] = ccdPtAddEdge(pt, v[4], v[1]);
-    e[6] = ccdPtAddEdge(pt, v[4], v[2]);
-    e[7] = ccdPtAddEdge(pt, v[4], v[3]);
-
-    e[8]  = ccdPtAddEdge(pt, v[5], v[0]);
-    e[9]  = ccdPtAddEdge(pt, v[5], v[1]);
-    e[10] = ccdPtAddEdge(pt, v[5], v[2]);
-    e[11] = ccdPtAddEdge(pt, v[5], v[3]);
-
-    if (ccdPtAddFace(pt, e[4], e[5], e[0]) == NULL
-            || ccdPtAddFace(pt, e[5], e[6], e[1]) == NULL
-            || ccdPtAddFace(pt, e[6], e[7], e[2]) == NULL
-            || ccdPtAddFace(pt, e[7], e[4], e[3]) == NULL
-
-            || ccdPtAddFace(pt, e[8],  e[9],  e[0]) == NULL
-            || ccdPtAddFace(pt, e[9],  e[10], e[1]) == NULL
-            || ccdPtAddFace(pt, e[10], e[11], e[2]) == NULL
-            || ccdPtAddFace(pt, e[11], e[8],  e[3]) == NULL){
-        return -2;
-    }
-
-    return 0;
-}
-
-/** Expands polytope's tri by new vertex v. Triangle tri is replaced by
- *  three triangles each with one vertex in v. */
-static int expandPolytope(ccd_pt_t *pt, ccd_pt_el_t *el,
-                          const ccd_support_t *newv)
-{
-    ccd_pt_vertex_t *v[5];
-    ccd_pt_edge_t *e[8];
-    ccd_pt_face_t *f[2];
-
-
-    // element can be either segment or triangle
-    if (el->type == CCD_PT_EDGE){
-        // In this case, segment should be replaced by new point.
-        // Simpliest case is when segment stands alone and in this case
-        // this segment is replaced by two other segments both connected to
-        // newv.
-        // Segment can be also connected to max two faces and in that case
-        // each face must be replaced by two other faces. To do this
-        // correctly it is necessary to have correctly ordered edges and
-        // vertices which is exactly what is done in following code.
-        //
-
-        ccdPtEdgeVertices((const ccd_pt_edge_t *)el, &v[0], &v[2]);
-
-        ccdPtEdgeFaces((ccd_pt_edge_t *)el, &f[0], &f[1]);
-
-        if (f[0]){
-            ccdPtFaceEdges(f[0], &e[0], &e[1], &e[2]);
-            if (e[0] == (ccd_pt_edge_t *)el){
-                e[0] = e[2];
-            }else if (e[1] == (ccd_pt_edge_t *)el){
-                e[1] = e[2];
-            }
-            ccdPtEdgeVertices(e[0], &v[1], &v[3]);
-            if (v[1] != v[0] && v[3] != v[0]){
-                e[2] = e[0];
-                e[0] = e[1];
-                e[1] = e[2];
-                if (v[1] == v[2])
-                    v[1] = v[3];
-            }else{
-                if (v[1] == v[0])
-                    v[1] = v[3];
-            }
-
-            if (f[1]){
-                ccdPtFaceEdges(f[1], &e[2], &e[3], &e[4]);
-                if (e[2] == (ccd_pt_edge_t *)el){
-                    e[2] = e[4];
-                }else if (e[3] == (ccd_pt_edge_t *)el){
-                    e[3] = e[4];
-                }
-                ccdPtEdgeVertices(e[2], &v[3], &v[4]);
-                if (v[3] != v[2] && v[4] != v[2]){
-                    e[4] = e[2];
-                    e[2] = e[3];
-                    e[3] = e[4];
-                    if (v[3] == v[0])
-                        v[3] = v[4];
-                }else{
-                    if (v[3] == v[2])
-                        v[3] = v[4];
-                }
-            }
-
-
-            v[4] = ccdPtAddVertex(pt, newv);
-
-            ccdPtDelFace(pt, f[0]);
-            if (f[1]){
-                ccdPtDelFace(pt, f[1]);
-                ccdPtDelEdge(pt, (ccd_pt_edge_t *)el);
-            }
-
-            e[4] = ccdPtAddEdge(pt, v[4], v[2]);
-            e[5] = ccdPtAddEdge(pt, v[4], v[0]);
-            e[6] = ccdPtAddEdge(pt, v[4], v[1]);
-            if (f[1])
-                e[7] = ccdPtAddEdge(pt, v[4], v[3]);
-
-
-            if (ccdPtAddFace(pt, e[1], e[4], e[6]) == NULL
-                    || ccdPtAddFace(pt, e[0], e[6], e[5]) == NULL){
-                return -2;
-            }
-
-            if (f[1]){
-                if (ccdPtAddFace(pt, e[3], e[5], e[7]) == NULL
-                        || ccdPtAddFace(pt, e[4], e[7], e[2]) == NULL){
-                    return -2;
-                }
-            }else{
-                if (ccdPtAddFace(pt, e[4], e[5], (ccd_pt_edge_t *)el) == NULL)
-                    return -2;
-            }
-        }
-    }else{ // el->type == CCD_PT_FACE
-        // replace triangle by tetrahedron without base (base would be the
-        // triangle that will be removed)
-
-        // get triplet of surrounding edges and vertices of triangle face
-        ccdPtFaceEdges((const ccd_pt_face_t *)el, &e[0], &e[1], &e[2]);
-        ccdPtEdgeVertices(e[0], &v[0], &v[1]);
-        ccdPtEdgeVertices(e[1], &v[2], &v[3]);
-
-        // following code sorts edges to have e[0] between vertices 0-1,
-        // e[1] between 1-2 and e[2] between 2-0
-        if (v[2] != v[1] && v[3] != v[1]){
-            // swap e[1] and e[2] 
-            e[3] = e[1];
-            e[1] = e[2];
-            e[2] = e[3];
-        }
-        if (v[3] != v[0] && v[3] != v[1])
-            v[2] = v[3];
-
-        // remove triangle face
-        ccdPtDelFace(pt, (ccd_pt_face_t *)el);
-
-        // expand triangle to tetrahedron
-        v[3] = ccdPtAddVertex(pt, newv);
-        e[3] = ccdPtAddEdge(pt, v[3], v[0]);
-        e[4] = ccdPtAddEdge(pt, v[3], v[1]);
-        e[5] = ccdPtAddEdge(pt, v[3], v[2]);
-
-        if (ccdPtAddFace(pt, e[3], e[4], e[0]) == NULL
-                || ccdPtAddFace(pt, e[4], e[5], e[1]) == NULL
-                || ccdPtAddFace(pt, e[5], e[3], e[2]) == NULL){
-            return -2;
-        }
-    }
-
-    return 0;
-}
-
-/** Finds next support point (and stores it in out argument).
- *  Returns 0 on success, -1 otherwise */
-static int nextSupport(const void *obj1, const void *obj2, const ccd_t *ccd,
-                       const ccd_pt_el_t *el,
-                       ccd_support_t *out)
-{
-    ccd_vec3_t *a, *b, *c;
-    ccd_real_t dist;
-
-    if (el->type == CCD_PT_VERTEX)
-        return -1;
-
-    // touch contact
-    if (ccdIsZero(el->dist))
-        return -1;
-
-    __ccdSupport(obj1, obj2, &el->witness, ccd, out);
-
-    // Compute dist of support point along element witness point direction
-    // so we can determine whether we expanded a polytope surrounding the
-    // origin a bit.
-    dist = ccdVec3Dot(&out->v, &el->witness);
-
-    if (dist - el->dist < ccd->epa_tolerance)
-        return -1;
-
-    if (el->type == CCD_PT_EDGE){
-        // fetch end points of edge
-        ccdPtEdgeVec3((ccd_pt_edge_t *)el, &a, &b);
-
-        // get distance from segment
-        dist = ccdVec3PointSegmentDist2(&out->v, a, b, NULL);
-    }else{ // el->type == CCD_PT_FACE
-        // fetch vertices of triangle face
-        ccdPtFaceVec3((ccd_pt_face_t *)el, &a, &b, &c);
-
-        // check if new point can significantly expand polytope
-        dist = ccdVec3PointTriDist2(&out->v, a, b, c, NULL);
-    }
-
-    if (dist < ccd->epa_tolerance)
-        return -1;
-
-    return 0;
-}
diff --git a/src/pe/extern/libccd/ccd/ccd.h b/src/pe/extern/libccd/ccd/ccd.h
deleted file mode 100644
index fb8df75a081c6484c7f9445adb82c2b4f30fe786..0000000000000000000000000000000000000000
--- a/src/pe/extern/libccd/ccd/ccd.h
+++ /dev/null
@@ -1,154 +0,0 @@
-/***
- * libccd
- * ---------------------------------
- * Copyright (c)2010,2011 Daniel Fiser <danfis@danfis.cz>
- *
- *
- *  This file is part of libccd.
- *
- *  Distributed under the OSI-approved BSD License (the "License");
- *  see accompanying file BDS-LICENSE for details or see
- *  <http://www.opensource.org/licenses/bsd-license.php>.
- *
- *  This software is distributed WITHOUT ANY WARRANTY; without even the
- *  implied warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.
- *  See the License for more information.
- */
-
-#ifndef __CCD_H__
-#define __CCD_H__
-
-#include <ccd/vec3.h>
-
-#ifdef __cplusplus
-extern "C" {
-#endif /* __cplusplus */
-
-/**
- * Type of *support* function that takes pointer to 3D object and direction
- * and returns (via vec argument) furthest point from object in specified
- * direction.
- */
-typedef void (*ccd_support_fn)(const void *obj, const ccd_vec3_t *dir,
-                               ccd_vec3_t *vec);
-
-/**
- * Returns (via dir argument) first direction vector that will be used in
- * initialization of algorithm.
- */
-typedef void (*ccd_first_dir_fn)(const void *obj1, const void *obj2,
-                                 ccd_vec3_t *dir);
-
-
-/**
- * Returns (via center argument) geometric center (some point near center)
- * of given object.
- */
-typedef void (*ccd_center_fn)(const void *obj1, ccd_vec3_t *center);
-
-/**
- * Main structure of CCD algorithm.
- */
-struct _ccd_t {
-    ccd_first_dir_fn first_dir; //!< Returns initial direction where first
-                                //!< support point will be searched
-    ccd_support_fn support1; //!< Function that returns support point of
-                             //!< first object
-    ccd_support_fn support2; //!< Function that returns support point of
-                             //!< second object
-
-    ccd_center_fn center1; //!< Function that returns geometric center of
-                           //!< first object
-    ccd_center_fn center2; //!< Function that returns geometric center of
-                           //!< second object
-
-    unsigned long max_iterations; //!< Maximal number of iterations
-    ccd_real_t epa_tolerance;
-    ccd_real_t mpr_tolerance; //!< Boundary tolerance for MPR algorithm
-    ccd_real_t dist_tolerance;
-};
-typedef struct _ccd_t ccd_t;
-
-/**
- * Default first direction.
- */
-_ccd_export void ccdFirstDirDefault(const void *o1, const void *o2,
-                                    ccd_vec3_t *dir);
-
-#define CCD_INIT(ccd) \
-    do { \
-        (ccd)->first_dir = ccdFirstDirDefault; \
-        (ccd)->support1 = NULL; \
-        (ccd)->support2 = NULL; \
-        (ccd)->center1  = NULL; \
-        (ccd)->center2  = NULL; \
-        \
-        (ccd)->max_iterations = (unsigned long)-1; \
-        (ccd)->epa_tolerance = CCD_REAL(0.0001); \
-        (ccd)->mpr_tolerance = CCD_REAL(0.0001); \
-        (ccd)->dist_tolerance = CCD_REAL(1E-6); \
-    } while(0)
-
-
-/**
- * Returns true if two given objects interest.
- */
-_ccd_export int ccdGJKIntersect(const void *obj1, const void *obj2,
-                                const ccd_t *ccd);
-
-/**
- * This function computes separation vector of two objects. Separation
- * vector is minimal translation of obj2 to get obj1 and obj2 speparated
- * (without intersection).
- * Returns 0 if obj1 and obj2 intersect and sep is filled with translation
- * vector. If obj1 and obj2 don't intersect -1 is returned.
- * If memory allocation fails -2 is returned.
- */
-_ccd_export int ccdGJKSeparate(const void *obj1, const void *obj2,
-                               const ccd_t *ccd, ccd_vec3_t *sep);
-
-/**
- * Computes penetration of obj2 into obj1.
- * Depth of penetration, direction and position is returned. It means that
- * if obj2 is translated by distance depth in direction dir objects will
- * have touching contact, pos should be position in global coordinates
- * where force should take a place.
- *
- * CCD+EPA algorithm is used.
- *
- * Returns 0 if obj1 and obj2 intersect and depth, dir and pos are filled
- * if given non-NULL pointers.
- * If obj1 and obj2 don't intersect -1 is returned.
- * If memory allocation fails -2 is returned.
- */
-_ccd_export int ccdGJKPenetration(const void *obj1, const void *obj2,
-                                  const ccd_t *ccd, ccd_real_t *depth,
-                                  ccd_vec3_t *dir, ccd_vec3_t *pos);
-
-/**
- * Returns true if two given objects intersect - MPR algorithm is used.
- */
-_ccd_export int ccdMPRIntersect(const void *obj1, const void *obj2,
-                                const ccd_t *ccd);
-
-/**
- * Computes penetration of obj2 into obj1.
- * Depth of penetration, direction and position is returned, i.e. if obj2
- * is translated by computed depth in resulting direction obj1 and obj2
- * would have touching contact. Position is point in global coordinates
- * where force should be take a place.
- *
- * Minkowski Portal Refinement algorithm is used (MPR, a.k.a. XenoCollide,
- * see Game Programming Gem 7).
- *
- * Returns 0 if obj1 and obj2 intersect, otherwise -1 is returned.
- */
-_ccd_export int ccdMPRPenetration(const void *obj1, const void *obj2,
-                                  const ccd_t *ccd, ccd_real_t *depth,
-                                  ccd_vec3_t *dir, ccd_vec3_t *pos);
-
-#ifdef __cplusplus
-} /* extern "C" */
-#endif /* __cplusplus */
-
-#endif /* __CCD_H__ */
diff --git a/src/pe/extern/libccd/ccd/compiler.h b/src/pe/extern/libccd/ccd/compiler.h
deleted file mode 100644
index b022e26e47875e9f2392b82adfadbd430d5b69b6..0000000000000000000000000000000000000000
--- a/src/pe/extern/libccd/ccd/compiler.h
+++ /dev/null
@@ -1,83 +0,0 @@
-/***
- * libccd
- * ---------------------------------
- * Copyright (c)2010 Daniel Fiser <danfis@danfis.cz>
- *
- *
- *  This file is part of libccd.
- *
- *  Distributed under the OSI-approved BSD License (the "License");
- *  see accompanying file BDS-LICENSE for details or see
- *  <http://www.opensource.org/licenses/bsd-license.php>.
- *
- *  This software is distributed WITHOUT ANY WARRANTY; without even the
- *  implied warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.
- *  See the License for more information.
- */
-
-#ifndef __CCD_COMPILER_H__
-#define __CCD_COMPILER_H__
-
-#include <stddef.h>
-
-#define ccd_offsetof(TYPE, MEMBER) offsetof(TYPE, MEMBER)
-
-#define ccd_container_of(ptr, type, member) \
-    (type *)( (char *)ptr - ccd_offsetof(type, member))
-
-
-/**
- * Marks exported function.
- */
-//#ifdef _WIN32
-//# ifdef libccd_EXPORTS
-//#   define _ccd_export __declspec(dllexport)
-//# else /* libccd_EXPORTS */
-//#   define _ccd_export __declspec(dllimport)
-//# endif /* libccd_EXPORTS */
-//#else /* _WIN32 */
-//# define _ccd_export
-//#endif /* _WIN32 */
-
-#define _ccd_export
-
-
-/**
- * Marks inline function.
- */
-#ifdef __GNUC__
-# define _ccd_inline static inline __attribute__((always_inline))
-#else /* __GNUC__ */
-# define _ccd_inline static __inline
-#endif /* __GNUC__ */
-
-
-/**
- * __prefetch(x)  - prefetches the cacheline at "x" for read
- * __prefetchw(x) - prefetches the cacheline at "x" for write
- */
-#ifdef __GNUC__
-# define _ccd_prefetch(x) __builtin_prefetch(x)
-# define _ccd_prefetchw(x) __builtin_prefetch(x,1)
-#else /* __GNUC__ */
-# define _ccd_prefetch(x) ((void)0)
-# define _ccd_prefetchw(x) ((void)0)
-#endif /* __GNUC__ */
-
-
-#ifdef __ICC
-// disable unused parameter warning
-# pragma warning(disable:869)
-// disable annoying "operands are evaluated in unspecified order" warning
-# pragma warning(disable:981)
-#endif /* __ICC */
-
-#ifdef _MSC_VER
-// disable unsafe function warning
-# ifndef _CRT_SECURE_NO_WARNINGS
-#  define _CRT_SECURE_NO_WARNINGS
-# endif
-#endif /* _MSC_VER */
-
-#endif /* __CCD_COMPILER_H__ */
-
diff --git a/src/pe/extern/libccd/ccd/config.h.cmake.in b/src/pe/extern/libccd/ccd/config.h.cmake.in
deleted file mode 100644
index 0515009164d1c01f11f1728cb4913338bbe808e8..0000000000000000000000000000000000000000
--- a/src/pe/extern/libccd/ccd/config.h.cmake.in
+++ /dev/null
@@ -1,7 +0,0 @@
-#ifndef __CCD_CONFIG_H__
-#define __CCD_CONFIG_H__
-
-#cmakedefine CCD_SINGLE
-#cmakedefine CCD_DOUBLE
-
-#endif /* __CCD_CONFIG_H__ */
diff --git a/src/pe/extern/libccd/ccd/config.h.m4 b/src/pe/extern/libccd/ccd/config.h.m4
deleted file mode 100644
index 4c11fbab0c11d9e9fd63ddd177fb262514732ee9..0000000000000000000000000000000000000000
--- a/src/pe/extern/libccd/ccd/config.h.m4
+++ /dev/null
@@ -1,7 +0,0 @@
-#ifndef __CCD_CONFIG_H__
-#define __CCD_CONFIG_H__
-
-ifdef(`USE_SINGLE', `#define CCD_SINGLE')
-ifdef(`USE_DOUBLE', `#define CCD_DOUBLE')
-
-#endif /* __CCD_CONFIG_H__ */
diff --git a/src/pe/extern/libccd/ccd/quat.h b/src/pe/extern/libccd/ccd/quat.h
deleted file mode 100644
index fc67532109560e904d7b90f02554c15b0f0a0ccf..0000000000000000000000000000000000000000
--- a/src/pe/extern/libccd/ccd/quat.h
+++ /dev/null
@@ -1,239 +0,0 @@
-/***
- * libccd
- * ---------------------------------
- * Copyright (c)2010 Daniel Fiser <danfis@danfis.cz>
- *
- *
- *  This file is part of libccd.
- *
- *  Distributed under the OSI-approved BSD License (the "License");
- *  see accompanying file BDS-LICENSE for details or see
- *  <http://www.opensource.org/licenses/bsd-license.php>.
- *
- *  This software is distributed WITHOUT ANY WARRANTY; without even the
- *  implied warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.
- *  See the License for more information.
- */
-
-#ifndef __CCD_QUAT_H__
-#define __CCD_QUAT_H__
-
-#include <ccd/compiler.h>
-#include <ccd/vec3.h>
-
-#ifdef __cplusplus
-extern "C" {
-#endif /* __cplusplus */
-
-struct _ccd_quat_t {
-    ccd_real_t q[4]; //!< x, y, z, w
-};
-typedef struct _ccd_quat_t ccd_quat_t;
-
-#define CCD_QUAT(name, x, y, z, w) \
-    ccd_quat_t name = { {x, y, z, w} }
-
-_ccd_inline ccd_real_t ccdQuatLen2(const ccd_quat_t *q);
-_ccd_inline ccd_real_t ccdQuatLen(const ccd_quat_t *q);
-
-_ccd_inline void ccdQuatSet(ccd_quat_t *q, ccd_real_t x, ccd_real_t y, ccd_real_t z, ccd_real_t w);
-_ccd_inline void ccdQuatCopy(ccd_quat_t *dest, const ccd_quat_t *src);
-
-_ccd_inline int ccdQuatNormalize(ccd_quat_t *q);
-
-_ccd_inline void ccdQuatSetAngleAxis(ccd_quat_t *q,
-                                     ccd_real_t angle, const ccd_vec3_t *axis);
-
-_ccd_inline void ccdQuatScale(ccd_quat_t *q, ccd_real_t k);
-
-/**
- * q = q * q2
- */
-_ccd_inline void ccdQuatMul(ccd_quat_t *q, const ccd_quat_t *q2);
-
-/**
- * q = a * b
- */
-_ccd_inline void ccdQuatMul2(ccd_quat_t *q,
-                             const ccd_quat_t *a, const ccd_quat_t *b);
-
-/**
- * Inverts quaternion.
- * Returns 0 on success.
- */
-_ccd_inline int ccdQuatInvert(ccd_quat_t *q);
-_ccd_inline int ccdQuatInvert2(ccd_quat_t *dest, const ccd_quat_t *src);
-
-
-/**
- * Rotate vector v by quaternion q.
- */
-_ccd_inline void ccdQuatRotVec(ccd_vec3_t *v, const ccd_quat_t *q);
-
-
-/**** INLINES ****/
-_ccd_inline ccd_real_t ccdQuatLen2(const ccd_quat_t *q)
-{
-    ccd_real_t len;
-
-    len  = q->q[0] * q->q[0];
-    len += q->q[1] * q->q[1];
-    len += q->q[2] * q->q[2];
-    len += q->q[3] * q->q[3];
-
-    return len;
-}
-
-_ccd_inline ccd_real_t ccdQuatLen(const ccd_quat_t *q)
-{
-    return CCD_SQRT(ccdQuatLen2(q));
-}
-
-_ccd_inline void ccdQuatSet(ccd_quat_t *q, ccd_real_t x, ccd_real_t y, ccd_real_t z, ccd_real_t w)
-{
-    q->q[0] = x;
-    q->q[1] = y;
-    q->q[2] = z;
-    q->q[3] = w;
-}
-
-_ccd_inline void ccdQuatCopy(ccd_quat_t *dest, const ccd_quat_t *src)
-{
-    *dest = *src;
-}
-
-
-_ccd_inline int ccdQuatNormalize(ccd_quat_t *q)
-{
-    ccd_real_t len = ccdQuatLen(q);
-    if (len < CCD_EPS)
-        return 0;
-
-    ccdQuatScale(q, CCD_ONE / len);
-    return 1;
-}
-
-_ccd_inline void ccdQuatSetAngleAxis(ccd_quat_t *q,
-                                     ccd_real_t angle, const ccd_vec3_t *axis)
-{
-    ccd_real_t a, x, y, z, n, s;
-
-    a = angle/2;
-    x = ccdVec3X(axis);
-    y = ccdVec3Y(axis);
-    z = ccdVec3Z(axis);
-    n = CCD_SQRT(x*x + y*y + z*z);
-
-    // axis==0? (treat this the same as angle==0 with an arbitrary axis)
-    if (n < CCD_EPS){
-        q->q[0] = q->q[1] = q->q[2] = CCD_ZERO;
-        q->q[3] = CCD_ONE;
-    }else{
-#ifdef CCD_DOUBLE
-        s = sin(a)/n;
-#else
-        s = sinf(a)/n;
-#endif
-
-#ifdef CCD_DOUBLE
-        q->q[3] = cos(a);
-#else
-        q->q[3] = cosf(a);
-#endif
-        q->q[0] = x*s;
-        q->q[1] = y*s;
-        q->q[2] = z*s;
-
-        ccdQuatNormalize(q);
-    }
-}
-
-
-_ccd_inline void ccdQuatScale(ccd_quat_t *q, ccd_real_t k)
-{
-    size_t i;
-    for (i = 0; i < 4; i++)
-        q->q[i] *= k;
-}
-
-_ccd_inline void ccdQuatMul(ccd_quat_t *q, const ccd_quat_t *q2)
-{
-    ccd_quat_t a;
-    ccdQuatCopy(&a, q);
-    ccdQuatMul2(q, &a, q2);
-}
-
-_ccd_inline void ccdQuatMul2(ccd_quat_t *q,
-                             const ccd_quat_t *a, const ccd_quat_t *b)
-{
-    q->q[0] = a->q[3] * b->q[0]
-                + a->q[0] * b->q[3]
-                + a->q[1] * b->q[2]
-                - a->q[2] * b->q[1];
-    q->q[1] = a->q[3] * b->q[1]
-                + a->q[1] * b->q[3]
-                - a->q[0] * b->q[2]
-                + a->q[2] * b->q[0];
-    q->q[2] = a->q[3] * b->q[2]
-                + a->q[2] * b->q[3]
-                + a->q[0] * b->q[1]
-                - a->q[1] * b->q[0];
-    q->q[3] = a->q[3] * b->q[3]
-                - a->q[0] * b->q[0]
-                - a->q[1] * b->q[1]
-                - a->q[2] * b->q[2];
-}
-
-_ccd_inline int ccdQuatInvert(ccd_quat_t *q)
-{
-    ccd_real_t len2 = ccdQuatLen2(q);
-    if (len2 < CCD_EPS)
-        return -1;
-
-    len2 = CCD_ONE / len2;
-
-    q->q[0] = -q->q[0] * len2;
-    q->q[1] = -q->q[1] * len2;
-    q->q[2] = -q->q[2] * len2;
-    q->q[3] = q->q[3] * len2;
-
-    return 0;
-}
-_ccd_inline int ccdQuatInvert2(ccd_quat_t *dest, const ccd_quat_t *src)
-{
-    ccdQuatCopy(dest, src);
-    return ccdQuatInvert(dest);
-}
-
-_ccd_inline void ccdQuatRotVec(ccd_vec3_t *v, const ccd_quat_t *q)
-{
-    // original version: 31 mul + 21 add
-    // optimized version: 18 mul + 12 add
-    // formula: v = v + 2 * cross(q.xyz, cross(q.xyz, v) + q.w * v)
-    ccd_real_t cross1_x, cross1_y, cross1_z, cross2_x, cross2_y, cross2_z;
-    ccd_real_t x, y, z, w;
-    ccd_real_t vx, vy, vz;
-
-    vx = ccdVec3X(v);
-    vy = ccdVec3Y(v);
-    vz = ccdVec3Z(v);
-
-    w = q->q[3];
-    x = q->q[0];
-    y = q->q[1];
-    z = q->q[2];
-
-    cross1_x = y * vz - z * vy + w * vx;
-    cross1_y = z * vx - x * vz + w * vy;
-    cross1_z = x * vy - y * vx + w * vz;
-    cross2_x = y * cross1_z - z * cross1_y;
-    cross2_y = z * cross1_x - x * cross1_z;
-    cross2_z = x * cross1_y - y * cross1_x;
-    ccdVec3Set(v, vx + 2 * cross2_x, vy + 2 * cross2_y, vz + 2 * cross2_z);
-}
-
-#ifdef __cplusplus
-} /* extern "C" */
-#endif /* __cplusplus */
-
-#endif /* __CCD_QUAT_H__ */
diff --git a/src/pe/extern/libccd/ccd/vec3.h b/src/pe/extern/libccd/ccd/vec3.h
deleted file mode 100644
index b0c1b338f59cfc8e5c55903c1dae9d14463bb4dc..0000000000000000000000000000000000000000
--- a/src/pe/extern/libccd/ccd/vec3.h
+++ /dev/null
@@ -1,339 +0,0 @@
-/***
- * libccd
- * ---------------------------------
- * Copyright (c)2010-2013 Daniel Fiser <danfis@danfis.cz>
- *
- *
- *  This file is part of libccd.
- *
- *  Distributed under the OSI-approved BSD License (the "License");
- *  see accompanying file BDS-LICENSE for details or see
- *  <http://www.opensource.org/licenses/bsd-license.php>.
- *
- *  This software is distributed WITHOUT ANY WARRANTY; without even the
- *  implied warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.
- *  See the License for more information.
- */
-
-#ifndef __CCD_VEC3_H__
-#define __CCD_VEC3_H__
-
-#include <math.h>
-#include <float.h>
-#include <stdlib.h>
-#include <ccd/compiler.h>
-#include <ccd/config.h>
-
-#ifdef __cplusplus
-extern "C" {
-#endif /* __cplusplus */
-
-
-#ifndef CCD_SINGLE
-# ifndef CCD_DOUBLE
-#  error You must define CCD_SINGLE or CCD_DOUBLE
-# endif /* CCD_DOUBLE */
-#endif /* CCD_SINGLE */
-
-#ifdef WIN32
-# define CCD_FMIN(x, y) ((x) < (y) ? (x) : (y))
-#endif /* WIN32 */
-
-#ifdef CCD_SINGLE
-# ifdef CCD_DOUBLE
-#  error You can define either CCD_SINGLE or CCD_DOUBLE, not both!
-# endif /* CCD_DOUBLE */
-
-typedef float ccd_real_t;
-
-//# define CCD_EPS 1E-6
-# define CCD_EPS FLT_EPSILON
-
-# define CCD_REAL_MAX FLT_MAX
-
-# define CCD_REAL(x) (x ## f)   /*!< form a constant */
-# define CCD_SQRT(x) (sqrtf(x)) /*!< square root */
-# define CCD_FABS(x) (fabsf(x)) /*!< absolute value */
-# define CCD_FMAX(x, y) (fmaxf((x), (y))) /*!< maximum of two floats */
-
-# ifndef CCD_FMIN
-#  define CCD_FMIN(x, y) (fminf((x), (y))) /*!< minimum of two floats */
-# endif /* CCD_FMIN */
-
-#endif /* CCD_SINGLE */
-
-#ifdef CCD_DOUBLE
-typedef double ccd_real_t;
-
-//# define CCD_EPS 1E-10
-# define CCD_EPS DBL_EPSILON
-
-# define CCD_REAL_MAX DBL_MAX
-
-# define CCD_REAL(x) (x)       /*!< form a constant */
-# define CCD_SQRT(x) (sqrt(x)) /*!< square root */
-# define CCD_FABS(x) (fabs(x)) /*!< absolute value */
-# define CCD_FMAX(x, y) (fmax((x), (y))) /*!< maximum of two floats */
-
-# ifndef CCD_FMIN
-#  define CCD_FMIN(x, y) (fmin((x), (y))) /*!< minimum of two floats */
-# endif /* CCD_FMIN */
-
-#endif /* CCD_DOUBLE */
-
-#define CCD_ONE CCD_REAL(1.)
-#define CCD_ZERO CCD_REAL(0.)
-
-struct _ccd_vec3_t {
-    ccd_real_t v[3];
-};
-typedef struct _ccd_vec3_t ccd_vec3_t;
-
-
-/**
- * Holds origin (0,0,0) - this variable is meant to be read-only!
- */
-extern ccd_vec3_t *ccd_vec3_origin;
-
-/**
- * Array of points uniformly distributed on unit sphere.
- */
-extern ccd_vec3_t *ccd_points_on_sphere;
-extern size_t ccd_points_on_sphere_len;
-
-/** Returns sign of value. */
-_ccd_inline int ccdSign(ccd_real_t val);
-/** Returns true if val is zero. **/
-_ccd_inline int ccdIsZero(ccd_real_t val);
-/** Returns true if a and b equal. **/
-_ccd_inline int ccdEq(ccd_real_t a, ccd_real_t b);
-
-
-#define CCD_VEC3_STATIC(x, y, z) \
-    { { (x), (y), (z) } }
-
-#define CCD_VEC3(name, x, y, z) \
-    ccd_vec3_t name = CCD_VEC3_STATIC((x), (y), (z))
-
-_ccd_inline ccd_real_t ccdVec3X(const ccd_vec3_t *v);
-_ccd_inline ccd_real_t ccdVec3Y(const ccd_vec3_t *v);
-_ccd_inline ccd_real_t ccdVec3Z(const ccd_vec3_t *v);
-
-/**
- * Returns true if a and b equal.
- */
-_ccd_inline int ccdVec3Eq(const ccd_vec3_t *a, const ccd_vec3_t *b);
-
-/**
- * Returns squared length of vector.
- */
-_ccd_inline ccd_real_t ccdVec3Len2(const ccd_vec3_t *v);
-
-/**
- * Returns distance between a and b.
- */
-_ccd_inline ccd_real_t ccdVec3Dist2(const ccd_vec3_t *a, const ccd_vec3_t *b);
-
-
-_ccd_inline void ccdVec3Set(ccd_vec3_t *v, ccd_real_t x, ccd_real_t y, ccd_real_t z);
-
-/**
- * v = w
- */
-_ccd_inline void ccdVec3Copy(ccd_vec3_t *v, const ccd_vec3_t *w);
-
-/**
- * Substracts coordinates of vector w from vector v. v = v - w
- */
-_ccd_inline void ccdVec3Sub(ccd_vec3_t *v, const ccd_vec3_t *w);
-
-/**
- * Adds coordinates of vector w to vector v. v = v + w
- */
-_ccd_inline void ccdVec3Add(ccd_vec3_t *v, const ccd_vec3_t *w);
-
-/**
- * d = v - w
- */
-_ccd_inline void ccdVec3Sub2(ccd_vec3_t *d, const ccd_vec3_t *v, const ccd_vec3_t *w);
-
-/**
- * d = d * k;
- */
-_ccd_inline void ccdVec3Scale(ccd_vec3_t *d, ccd_real_t k);
-
-/**
- * Normalizes given vector to unit length.
- */
-_ccd_inline void ccdVec3Normalize(ccd_vec3_t *d);
-
-
-/**
- * Dot product of two vectors.
- */
-_ccd_inline ccd_real_t ccdVec3Dot(const ccd_vec3_t *a, const ccd_vec3_t *b);
-
-/**
- * Cross product: d = a x b.
- */
-_ccd_inline void ccdVec3Cross(ccd_vec3_t *d, const ccd_vec3_t *a, const ccd_vec3_t *b);
-
-
-/**
- * Returns distance^2 of point P to segment ab.
- * If witness is non-NULL it is filled with coordinates of point from which
- * was computed distance to point P.
- */
-_ccd_export ccd_real_t ccdVec3PointSegmentDist2(const ccd_vec3_t *P,
-                                                const ccd_vec3_t *a,
-                                                const ccd_vec3_t *b,
-                                                ccd_vec3_t *witness);
-
-/**
- * Returns distance^2 of point P from triangle formed by triplet a, b, c.
- * If witness vector is provided it is filled with coordinates of point
- * from which was computed distance to point P.
- */
-_ccd_export ccd_real_t ccdVec3PointTriDist2(const ccd_vec3_t *P,
-                                            const ccd_vec3_t *a,
-                                            const ccd_vec3_t *b,
-                                            const ccd_vec3_t *c,
-                                            ccd_vec3_t *witness);
-
-
-/**** INLINES ****/
-_ccd_inline int ccdSign(ccd_real_t val)
-{
-    if (ccdIsZero(val)){
-        return 0;
-    }else if (val < CCD_ZERO){
-        return -1;
-    }
-    return 1;
-}
-
-_ccd_inline int ccdIsZero(ccd_real_t val)
-{
-    return CCD_FABS(val) < CCD_EPS;
-}
-
-_ccd_inline int ccdEq(ccd_real_t _a, ccd_real_t _b)
-{
-    ccd_real_t ab;
-    ccd_real_t a, b;
-
-    ab = CCD_FABS(_a - _b);
-    if (CCD_FABS(ab) < CCD_EPS)
-        return 1;
-
-    a = CCD_FABS(_a);
-    b = CCD_FABS(_b);
-    if (b > a){
-        return ab < CCD_EPS * b;
-    }else{
-        return ab < CCD_EPS * a;
-    }
-}
-
-
-_ccd_inline ccd_real_t ccdVec3X(const ccd_vec3_t *v)
-{
-    return v->v[0];
-}
-
-_ccd_inline ccd_real_t ccdVec3Y(const ccd_vec3_t *v)
-{
-    return v->v[1];
-}
-
-_ccd_inline ccd_real_t ccdVec3Z(const ccd_vec3_t *v)
-{
-    return v->v[2];
-}
-
-_ccd_inline int ccdVec3Eq(const ccd_vec3_t *a, const ccd_vec3_t *b)
-{
-    return ccdEq(ccdVec3X(a), ccdVec3X(b))
-            && ccdEq(ccdVec3Y(a), ccdVec3Y(b))
-            && ccdEq(ccdVec3Z(a), ccdVec3Z(b));
-}
-
-_ccd_inline ccd_real_t ccdVec3Len2(const ccd_vec3_t *v)
-{
-    return ccdVec3Dot(v, v);
-}
-
-_ccd_inline ccd_real_t ccdVec3Dist2(const ccd_vec3_t *a, const ccd_vec3_t *b)
-{
-    ccd_vec3_t ab;
-    ccdVec3Sub2(&ab, a, b);
-    return ccdVec3Len2(&ab);
-}
-
-_ccd_inline void ccdVec3Set(ccd_vec3_t *v, ccd_real_t x, ccd_real_t y, ccd_real_t z)
-{
-    v->v[0] = x;
-    v->v[1] = y;
-    v->v[2] = z;
-}
-
-_ccd_inline void ccdVec3Copy(ccd_vec3_t *v, const ccd_vec3_t *w)
-{
-    *v = *w;
-}
-
-_ccd_inline void ccdVec3Sub(ccd_vec3_t *v, const ccd_vec3_t *w)
-{
-    v->v[0] -= w->v[0];
-    v->v[1] -= w->v[1];
-    v->v[2] -= w->v[2];
-}
-_ccd_inline void ccdVec3Sub2(ccd_vec3_t *d, const ccd_vec3_t *v, const ccd_vec3_t *w)
-{
-    d->v[0] = v->v[0] - w->v[0];
-    d->v[1] = v->v[1] - w->v[1];
-    d->v[2] = v->v[2] - w->v[2];
-}
-
-_ccd_inline void ccdVec3Add(ccd_vec3_t *v, const ccd_vec3_t *w)
-{
-    v->v[0] += w->v[0];
-    v->v[1] += w->v[1];
-    v->v[2] += w->v[2];
-}
-
-_ccd_inline void ccdVec3Scale(ccd_vec3_t *d, ccd_real_t k)
-{
-    d->v[0] *= k;
-    d->v[1] *= k;
-    d->v[2] *= k;
-}
-
-_ccd_inline void ccdVec3Normalize(ccd_vec3_t *d)
-{
-    ccd_real_t k = CCD_ONE / CCD_SQRT(ccdVec3Len2(d));
-    ccdVec3Scale(d, k);
-}
-
-_ccd_inline ccd_real_t ccdVec3Dot(const ccd_vec3_t *a, const ccd_vec3_t *b)
-{
-    ccd_real_t dot;
-
-    dot  = a->v[0] * b->v[0];
-    dot += a->v[1] * b->v[1];
-    dot += a->v[2] * b->v[2];
-    return dot;
-}
-
-_ccd_inline void ccdVec3Cross(ccd_vec3_t *d, const ccd_vec3_t *a, const ccd_vec3_t *b)
-{
-    d->v[0] = (a->v[1] * b->v[2]) - (a->v[2] * b->v[1]);
-    d->v[1] = (a->v[2] * b->v[0]) - (a->v[0] * b->v[2]);
-    d->v[2] = (a->v[0] * b->v[1]) - (a->v[1] * b->v[0]);
-}
-
-#ifdef __cplusplus
-} /* extern "C" */
-#endif /* __cplusplus */
-
-#endif /* __CCD_VEC3_H__ */
diff --git a/src/pe/extern/libccd/dbg.h b/src/pe/extern/libccd/dbg.h
deleted file mode 100644
index f4852c1a06a5b852fc80fadf3fa56e5dfd00a973..0000000000000000000000000000000000000000
--- a/src/pe/extern/libccd/dbg.h
+++ /dev/null
@@ -1,65 +0,0 @@
-/***
- * libccd
- * ---------------------------------
- * Copyright (c)2010 Daniel Fiser <danfis@danfis.cz>
- *
- *
- *  This file is part of libccd.
- *
- *  Distributed under the OSI-approved BSD License (the "License");
- *  see accompanying file BDS-LICENSE for details or see
- *  <http://www.opensource.org/licenses/bsd-license.php>.
- *
- *  This software is distributed WITHOUT ANY WARRANTY; without even the
- *  implied warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.
- *  See the License for more information.
- */
-
-#ifndef __CCD_DBG_H__
-#define __CCD_DBG_H__
-
-/**
- * Some macros which can be used for printing debug info to stderr if macro
- * NDEBUG not defined.
- *
- * DBG_PROLOGUE can be specified as string and this string will be
- * prepended to output text
- */
-#ifndef NDEBUG
-
-#include <stdio.h>
-
-#ifndef DBG_PROLOGUE
-# define DBG_PROLOGUE
-#endif
-
-# define DBG(format, ...) do { \
-    fprintf(stderr, DBG_PROLOGUE "%s :: " format "\n", __func__, ## __VA_ARGS__); \
-    fflush(stderr); \
-    } while (0)
-
-# define DBG2(str) do { \
-    fprintf(stderr, DBG_PROLOGUE "%s :: " str "\n", __func__); \
-    fflush(stderr); \
-    } while (0)
-
-# define DBG_VEC3(vec, prefix) do {\
-    fprintf(stderr, DBG_PROLOGUE "%s :: %s[%lf %lf %lf]\n", \
-            __func__, prefix, ccdVec3X(vec), ccdVec3Y(vec), ccdVec3Z(vec)); \
-    fflush(stderr); \
-    } while (0)
-/*
-# define DBG_VEC3(vec, prefix) do {\
-    fprintf(stderr, DBG_PROLOGUE "%s :: %s[%.20lf %.20lf %.20lf]\n", \
-            __func__, prefix, ccdVec3X(vec), ccdVec3Y(vec), ccdVec3Z(vec)); \
-    fflush(stderr); \
-    } while (0)
-*/
-
-#else
-# define DBG(format, ...)
-# define DBG2(str)
-# define DBG_VEC3(v, prefix)
-#endif
-
-#endif /* __CCD_DBG_H__ */
diff --git a/src/pe/extern/libccd/list.h b/src/pe/extern/libccd/list.h
deleted file mode 100644
index 40ea6328fa2633f430623662322ddd5cb65613a3..0000000000000000000000000000000000000000
--- a/src/pe/extern/libccd/list.h
+++ /dev/null
@@ -1,155 +0,0 @@
-/***
- * libccd
- * ---------------------------------
- * Copyright (c)2010 Daniel Fiser <danfis@danfis.cz>
- *
- *
- *  This file is part of libccd.
- *
- *  Distributed under the OSI-approved BSD License (the "License");
- *  see accompanying file BDS-LICENSE for details or see
- *  <http://www.opensource.org/licenses/bsd-license.php>.
- *
- *  This software is distributed WITHOUT ANY WARRANTY; without even the
- *  implied warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.
- *  See the License for more information.
- */
-
-#ifndef __CCD_LIST_H__
-#define __CCD_LIST_H__
-
-#include <string.h>
-#include <ccd/compiler.h>
-
-#ifdef __cplusplus
-extern "C" {
-#endif /* __cplusplus */
-
-struct _ccd_list_t {
-    struct _ccd_list_t *next, *prev;
-};
-typedef struct _ccd_list_t ccd_list_t;
-
-
-
-/**
- * Get the struct for this entry.
- * @ptr:	the &ccd_list_t pointer.
- * @type:	the type of the struct this is embedded in.
- * @member:	the name of the list_struct within the struct.
- */
-#define ccdListEntry(ptr, type, member) \
-    ccd_container_of(ptr, type, member)
-
-/**
- * Iterates over list.
- */
-#define ccdListForEach(list, item) \
-        for (item = (list)->next; \
-             _ccd_prefetch((item)->next), item != (list); \
-             item = (item)->next)
-
-/**
- * Iterates over list safe against remove of list entry
- */
-#define ccdListForEachSafe(list, item, tmp) \
-	    for (item = (list)->next, tmp = (item)->next; \
-             item != (list); \
-		     item = tmp, tmp = (item)->next)
-
-/**
- * Iterates over list of given type.
- * @pos:	the type * to use as a loop cursor.
- * @head:	the head for your list.
- * @member:	the name of the list_struct within the struct.
- */
-#define ccdListForEachEntry(head, pos, postype, member)                 \
-	for (pos = ccdListEntry((head)->next, postype, member);	\
-	     _ccd_prefetch(pos->member.next), &pos->member != (head); 	\
-	     pos = ccdListEntry(pos->member.next, postype, member))
-
-/**
- * Iterates over list of given type safe against removal of list entry
- * @pos:	the type * to use as a loop cursor.
- * @n:		another type * to use as temporary storage
- * @head:	the head for your list.
- * @member:	the name of the list_struct within the struct.
- */
-#define ccdListForEachEntrySafe(head, pos, postype, n, ntype, member)         \
-    for (pos = ccdListEntry((head)->next, postype, member),             \
-		 n = ccdListEntry(pos->member.next, postype, member);	\
-	     &pos->member != (head); 					\
-	     pos = n, n = ccdListEntry(n->member.next, ntype, member))
-
-
-/**
- * Initialize list.
- */
-_ccd_inline void ccdListInit(ccd_list_t *l);
-
-_ccd_inline ccd_list_t *ccdListNext(ccd_list_t *l);
-_ccd_inline ccd_list_t *ccdListPrev(ccd_list_t *l);
-
-/**
- * Returns true if list is empty.
- */
-_ccd_inline int ccdListEmpty(const ccd_list_t *head);
-
-/**
- * Appends item to end of the list l.
- */
-_ccd_inline void ccdListAppend(ccd_list_t *l, ccd_list_t *item);
-
-/**
- * Removes item from list.
- */
-_ccd_inline void ccdListDel(ccd_list_t *item);
-
-
-
-///
-/// INLINES:
-///
-
-_ccd_inline void ccdListInit(ccd_list_t *l)
-{
-    l->next = l;
-    l->prev = l;
-}
-
-_ccd_inline ccd_list_t *ccdListNext(ccd_list_t *l)
-{
-    return l->next;
-}
-
-_ccd_inline ccd_list_t *ccdListPrev(ccd_list_t *l)
-{
-    return l->prev;
-}
-
-_ccd_inline int ccdListEmpty(const ccd_list_t *head)
-{
-    return head->next == head;
-}
-
-_ccd_inline void ccdListAppend(ccd_list_t *l, ccd_list_t *new_element)
-{
-    new_element->prev = l->prev;
-    new_element->next = l;
-    l->prev->next = new_element;
-    l->prev = new_element;
-}
-
-_ccd_inline void ccdListDel(ccd_list_t *item)
-{
-    item->next->prev = item->prev;
-    item->prev->next = item->next;
-    item->next = item;
-    item->prev = item;
-}
-
-#ifdef __cplusplus
-} /* extern "C" */
-#endif /* __cplusplus */
-
-#endif /* __CCD_LIST_H__ */
diff --git a/src/pe/extern/libccd/mpr.c b/src/pe/extern/libccd/mpr.c
deleted file mode 100644
index 5f5533a5bad9ba5a78be456f88e0ea0162af7b72..0000000000000000000000000000000000000000
--- a/src/pe/extern/libccd/mpr.c
+++ /dev/null
@@ -1,547 +0,0 @@
-/***
- * libccd
- * ---------------------------------
- * Copyright (c)2010,2011 Daniel Fiser <danfis@danfis.cz>
- *
- *
- *  This file is part of libccd.
- *
- *  Distributed under the OSI-approved BSD License (the "License");
- *  see accompanying file BDS-LICENSE for details or see
- *  <http://www.opensource.org/licenses/bsd-license.php>.
- *
- *  This software is distributed WITHOUT ANY WARRANTY; without even the
- *  implied warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.
- *  See the License for more information.
- */
-
-#include <stdlib.h>
-#include <ccd/ccd.h>
-#include "simplex.h"
-#include "dbg.h"
-
-/** Finds origin (center) of Minkowski difference (actually it can be any
- *  interior point of Minkowski difference. */
-_ccd_inline void findOrigin(const void *obj1, const void *obj2, const ccd_t *ccd,
-                            ccd_support_t *center);
-
-/** Discovers initial portal - that is tetrahedron that intersects with
- *  origin ray (ray from center of Minkowski diff to (0,0,0).
- *
- *  Returns -1 if already recognized that origin is outside Minkowski
- *  portal.
- *  Returns 1 if origin lies on v1 of simplex (only v0 and v1 are present
- *  in simplex).
- *  Returns 2 if origin lies on v0-v1 segment.
- *  Returns 0 if portal was built.
- */
-static int discoverPortal(const void *obj1, const void *obj2,
-                          const ccd_t *ccd, ccd_simplex_t *portal);
-
-
-/** Expands portal towards origin and determine if objects intersect.
- *  Already established portal must be given as argument.
- *  If intersection is found 0 is returned, -1 otherwise */
-static int refinePortal(const void *obj1, const void *obj2,
-                        const ccd_t *ccd, ccd_simplex_t *portal);
-
-/** Finds penetration info by expanding provided portal. */
-static void findPenetr(const void *obj1, const void *obj2, const ccd_t *ccd,
-                       ccd_simplex_t *portal,
-                       ccd_real_t *depth, ccd_vec3_t *dir, ccd_vec3_t *pos);
-
-/** Finds penetration info if origin lies on portal's v1 */
-static void findPenetrTouch(const void *obj1, const void *obj2, const ccd_t *ccd,
-                            ccd_simplex_t *portal,
-                            ccd_real_t *depth, ccd_vec3_t *dir, ccd_vec3_t *pos);
-
-/** Find penetration info if origin lies on portal's segment v0-v1 */
-static void findPenetrSegment(const void *obj1, const void *obj2, const ccd_t *ccd,
-                              ccd_simplex_t *portal,
-                              ccd_real_t *depth, ccd_vec3_t *dir, ccd_vec3_t *pos);
-
-/** Finds position vector from fully established portal */
-static void findPos(const void *obj1, const void *obj2, const ccd_t *ccd,
-                    const ccd_simplex_t *portal, ccd_vec3_t *pos);
-
-/** Extends portal with new support point.
- *  Portal must have face v1-v2-v3 arranged to face outside portal. */
-_ccd_inline void expandPortal(ccd_simplex_t *portal,
-                              const ccd_support_t *v4);
-
-/** Fill dir with direction outside portal. Portal's v1-v2-v3 face must be
- *  arranged in correct order! */
-_ccd_inline void portalDir(const ccd_simplex_t *portal, ccd_vec3_t *dir);
-
-/** Returns true if portal encapsules origin (0,0,0), dir is direction of
- *  v1-v2-v3 face. */
-_ccd_inline int portalEncapsulesOrigin(const ccd_simplex_t *portal,
-                                       const ccd_vec3_t *dir);
-
-/** Returns true if portal with new point v4 would reach specified
- *  tolerance (i.e. returns true if portal can _not_ significantly expand
- *  within Minkowski difference).
- *
- *  v4 is candidate for new point in portal, dir is direction in which v4
- *  was obtained. */
-_ccd_inline int portalReachTolerance(const ccd_simplex_t *portal,
-                                     const ccd_support_t *v4,
-                                     const ccd_vec3_t *dir,
-                                     const ccd_t *ccd);
-
-/** Returns true if portal expanded by new point v4 could possibly contain
- *  origin, dir is direction in which v4 was obtained. */
-_ccd_inline int portalCanEncapsuleOrigin(const ccd_simplex_t *portal,   
-                                         const ccd_support_t *v4,
-                                         const ccd_vec3_t *dir);
-
-
-int ccdMPRIntersect(const void *obj1, const void *obj2, const ccd_t *ccd)
-{
-    ccd_simplex_t portal;
-    int res;
-
-    // Phase 1: Portal discovery - find portal that intersects with origin
-    // ray (ray from center of Minkowski diff to origin of coordinates)
-    res = discoverPortal(obj1, obj2, ccd, &portal);
-    if (res < 0)
-        return 0;
-    if (res > 0)
-        return 1;
-
-    // Phase 2: Portal refinement
-    res = refinePortal(obj1, obj2, ccd, &portal);
-    return (res == 0 ? 1 : 0);
-}
-
-int ccdMPRPenetration(const void *obj1, const void *obj2, const ccd_t *ccd,
-                      ccd_real_t *depth, ccd_vec3_t *dir, ccd_vec3_t *pos)
-{
-    ccd_simplex_t portal;
-    int res;
-
-    // Phase 1: Portal discovery
-    res = discoverPortal(obj1, obj2, ccd, &portal);
-    if (res < 0){
-        // Origin isn't inside portal - no collision.
-        return -1;
-
-    }else if (res == 1){
-        // Touching contact on portal's v1.
-        findPenetrTouch(obj1, obj2, ccd, &portal, depth, dir, pos);
-
-    }else if (res == 2){
-        // Origin lies on v0-v1 segment.
-        findPenetrSegment(obj1, obj2, ccd, &portal, depth, dir, pos);
-
-    }else if (res == 0){
-        // Phase 2: Portal refinement
-        res = refinePortal(obj1, obj2, ccd, &portal);
-        if (res < 0)
-            return -1;
-
-        // Phase 3. Penetration info
-        findPenetr(obj1, obj2, ccd, &portal, depth, dir, pos);
-    }
-
-    return 0;
-}
-
-
-
-_ccd_inline void findOrigin(const void *obj1, const void *obj2, const ccd_t *ccd,
-                            ccd_support_t *center)
-{
-    ccd->center1(obj1, &center->v1);
-    ccd->center2(obj2, &center->v2);
-    ccdVec3Sub2(&center->v, &center->v1, &center->v2);
-}
-
-static int discoverPortal(const void *obj1, const void *obj2,
-                          const ccd_t *ccd, ccd_simplex_t *portal)
-{
-    ccd_vec3_t dir, va, vb;
-    ccd_real_t dot;
-    int cont;
-
-    // vertex 0 is center of portal
-    findOrigin(obj1, obj2, ccd, ccdSimplexPointW(portal, 0));
-    ccdSimplexSetSize(portal, 1);
-
-    if (ccdVec3Eq(&ccdSimplexPoint(portal, 0)->v, ccd_vec3_origin)){
-        // Portal's center lies on origin (0,0,0) => we know that objects
-        // intersect but we would need to know penetration info.
-        // So move center little bit...
-        ccdVec3Set(&va, CCD_EPS * CCD_REAL(10.), CCD_ZERO, CCD_ZERO);
-        ccdVec3Add(&ccdSimplexPointW(portal, 0)->v, &va);
-    }
-
-
-    // vertex 1 = support in direction of origin
-    ccdVec3Copy(&dir, &ccdSimplexPoint(portal, 0)->v);
-    ccdVec3Scale(&dir, CCD_REAL(-1.));
-    ccdVec3Normalize(&dir);
-    __ccdSupport(obj1, obj2, &dir, ccd, ccdSimplexPointW(portal, 1));
-    ccdSimplexSetSize(portal, 2);
-
-    // test if origin isn't outside of v1
-    dot = ccdVec3Dot(&ccdSimplexPoint(portal, 1)->v, &dir);
-    if (ccdIsZero(dot) || dot < CCD_ZERO)
-        return -1;
-
-
-    // vertex 2
-    ccdVec3Cross(&dir, &ccdSimplexPoint(portal, 0)->v,
-                       &ccdSimplexPoint(portal, 1)->v);
-    if (ccdIsZero(ccdVec3Len2(&dir))){
-        if (ccdVec3Eq(&ccdSimplexPoint(portal, 1)->v, ccd_vec3_origin)){
-            // origin lies on v1
-            return 1;
-        }else{
-            // origin lies on v0-v1 segment
-            return 2;
-        }
-    }
-
-    ccdVec3Normalize(&dir);
-    __ccdSupport(obj1, obj2, &dir, ccd, ccdSimplexPointW(portal, 2));
-    dot = ccdVec3Dot(&ccdSimplexPoint(portal, 2)->v, &dir);
-    if (ccdIsZero(dot) || dot < CCD_ZERO)
-        return -1;
-
-    ccdSimplexSetSize(portal, 3);
-
-    // vertex 3 direction
-    ccdVec3Sub2(&va, &ccdSimplexPoint(portal, 1)->v,
-                     &ccdSimplexPoint(portal, 0)->v);
-    ccdVec3Sub2(&vb, &ccdSimplexPoint(portal, 2)->v,
-                     &ccdSimplexPoint(portal, 0)->v);
-    ccdVec3Cross(&dir, &va, &vb);
-    ccdVec3Normalize(&dir);
-
-    // it is better to form portal faces to be oriented "outside" origin
-    dot = ccdVec3Dot(&dir, &ccdSimplexPoint(portal, 0)->v);
-    if (dot > CCD_ZERO){
-        ccdSimplexSwap(portal, 1, 2);
-        ccdVec3Scale(&dir, CCD_REAL(-1.));
-    }
-
-    while (ccdSimplexSize(portal) < 4){
-        __ccdSupport(obj1, obj2, &dir, ccd, ccdSimplexPointW(portal, 3));
-        dot = ccdVec3Dot(&ccdSimplexPoint(portal, 3)->v, &dir);
-        if (ccdIsZero(dot) || dot < CCD_ZERO)
-            return -1;
-
-        cont = 0;
-
-        // test if origin is outside (v1, v0, v3) - set v2 as v3 and
-        // continue
-        ccdVec3Cross(&va, &ccdSimplexPoint(portal, 1)->v,
-                          &ccdSimplexPoint(portal, 3)->v);
-        dot = ccdVec3Dot(&va, &ccdSimplexPoint(portal, 0)->v);
-        if (dot < CCD_ZERO && !ccdIsZero(dot)){
-            ccdSimplexSet(portal, 2, ccdSimplexPoint(portal, 3));
-            cont = 1;
-        }
-
-        if (!cont){
-            // test if origin is outside (v3, v0, v2) - set v1 as v3 and
-            // continue
-            ccdVec3Cross(&va, &ccdSimplexPoint(portal, 3)->v,
-                              &ccdSimplexPoint(portal, 2)->v);
-            dot = ccdVec3Dot(&va, &ccdSimplexPoint(portal, 0)->v);
-            if (dot < CCD_ZERO && !ccdIsZero(dot)){
-                ccdSimplexSet(portal, 1, ccdSimplexPoint(portal, 3));
-                cont = 1;
-            }
-        }
-
-        if (cont){
-            ccdVec3Sub2(&va, &ccdSimplexPoint(portal, 1)->v,
-                             &ccdSimplexPoint(portal, 0)->v);
-            ccdVec3Sub2(&vb, &ccdSimplexPoint(portal, 2)->v,
-                             &ccdSimplexPoint(portal, 0)->v);
-            ccdVec3Cross(&dir, &va, &vb);
-            ccdVec3Normalize(&dir);
-        }else{
-            ccdSimplexSetSize(portal, 4);
-        }
-    }
-
-    return 0;
-}
-
-static int refinePortal(const void *obj1, const void *obj2,
-                        const ccd_t *ccd, ccd_simplex_t *portal)
-{
-    ccd_vec3_t dir;
-    ccd_support_t v4;
-
-    while (1){
-        // compute direction outside the portal (from v0 throught v1,v2,v3
-        // face)
-        portalDir(portal, &dir);
-
-        // test if origin is inside the portal
-        if (portalEncapsulesOrigin(portal, &dir))
-            return 0;
-
-        // get next support point
-        __ccdSupport(obj1, obj2, &dir, ccd, &v4);
-
-        // test if v4 can expand portal to contain origin and if portal
-        // expanding doesn't reach given tolerance
-        if (!portalCanEncapsuleOrigin(portal, &v4, &dir)
-                || portalReachTolerance(portal, &v4, &dir, ccd)){
-            return -1;
-        }
-
-        // v1-v2-v3 triangle must be rearranged to face outside Minkowski
-        // difference (direction from v0).
-        expandPortal(portal, &v4);
-    }
-
-    return -1;
-}
-
-
-static void findPenetr(const void *obj1, const void *obj2, const ccd_t *ccd,
-                       ccd_simplex_t *portal,
-                       ccd_real_t *depth, ccd_vec3_t *pdir, ccd_vec3_t *pos)
-{
-    ccd_vec3_t dir;
-    ccd_support_t v4;
-    unsigned long iterations;
-
-    iterations = 0UL;
-    while (1){
-        // compute portal direction and obtain next support point
-        portalDir(portal, &dir);
-        __ccdSupport(obj1, obj2, &dir, ccd, &v4);
-
-        // reached tolerance -> find penetration info
-        if (portalReachTolerance(portal, &v4, &dir, ccd)
-                || iterations > ccd->max_iterations){
-            *depth = ccdVec3PointTriDist2(ccd_vec3_origin,
-                                          &ccdSimplexPoint(portal, 1)->v,
-                                          &ccdSimplexPoint(portal, 2)->v,
-                                          &ccdSimplexPoint(portal, 3)->v,
-                                          pdir);
-            *depth = CCD_SQRT(*depth);
-            ccdVec3Normalize(pdir);
-
-            // barycentric coordinates:
-            findPos(obj1, obj2, ccd, portal, pos);
-
-            return;
-        }
-
-        expandPortal(portal, &v4);
-
-        iterations++;
-    }
-}
-
-static void findPenetrTouch(const void *obj1, const void *obj2, const ccd_t *ccd,
-                            ccd_simplex_t *portal,
-                            ccd_real_t *depth, ccd_vec3_t *dir, ccd_vec3_t *pos)
-{
-   (void)(obj1);
-   (void)(obj2);
-   (void)(ccd);
-    // Touching contact on portal's v1 - so depth is zero and direction
-    // is unimportant and pos can be guessed
-    *depth = CCD_REAL(0.);
-    ccdVec3Copy(dir, ccd_vec3_origin);
-
-    ccdVec3Copy(pos, &ccdSimplexPoint(portal, 1)->v1);
-    ccdVec3Add(pos, &ccdSimplexPoint(portal, 1)->v2);
-    ccdVec3Scale(pos, 0.5);
-}
-
-static void findPenetrSegment(const void *obj1, const void *obj2, const ccd_t *ccd,
-                              ccd_simplex_t *portal,
-                              ccd_real_t *depth, ccd_vec3_t *dir, ccd_vec3_t *pos)
-{
-    (void)(obj1);
-    (void)(obj2);
-    (void)(ccd);
-
-    /*
-    ccd_vec3_t vec;
-    ccd_real_t k;
-    */
-
-    // Origin lies on v0-v1 segment.
-    // Depth is distance to v1, direction also and position must be
-    // computed
-
-    ccdVec3Copy(pos, &ccdSimplexPoint(portal, 1)->v1);
-    ccdVec3Add(pos, &ccdSimplexPoint(portal, 1)->v2);
-    ccdVec3Scale(pos, CCD_REAL(0.5));
-
-    /*
-    ccdVec3Sub2(&vec, &ccdSimplexPoint(portal, 1)->v,
-                      &ccdSimplexPoint(portal, 0)->v);
-    k  = CCD_SQRT(ccdVec3Len2(&ccdSimplexPoint(portal, 0)->v));
-    k /= CCD_SQRT(ccdVec3Len2(&vec));
-    ccdVec3Scale(&vec, -k);
-    ccdVec3Add(pos, &vec);
-    */
-
-    ccdVec3Copy(dir, &ccdSimplexPoint(portal, 1)->v);
-    *depth = CCD_SQRT(ccdVec3Len2(dir));
-    ccdVec3Normalize(dir);
-}
-
-
-static void findPos(const void *obj1, const void *obj2, const ccd_t *ccd,
-                    const ccd_simplex_t *portal, ccd_vec3_t *pos)
-{
-    (void)(obj1);
-    (void)(obj2);
-    (void)(ccd);
-
-    ccd_vec3_t dir;
-    size_t i;
-    ccd_real_t b[4], sum, inv;
-    ccd_vec3_t vec, p1, p2;
-
-    portalDir(portal, &dir);
-
-    // use barycentric coordinates of tetrahedron to find origin
-    ccdVec3Cross(&vec, &ccdSimplexPoint(portal, 1)->v,
-                       &ccdSimplexPoint(portal, 2)->v);
-    b[0] = ccdVec3Dot(&vec, &ccdSimplexPoint(portal, 3)->v);
-
-    ccdVec3Cross(&vec, &ccdSimplexPoint(portal, 3)->v,
-                       &ccdSimplexPoint(portal, 2)->v);
-    b[1] = ccdVec3Dot(&vec, &ccdSimplexPoint(portal, 0)->v);
-
-    ccdVec3Cross(&vec, &ccdSimplexPoint(portal, 0)->v,
-                       &ccdSimplexPoint(portal, 1)->v);
-    b[2] = ccdVec3Dot(&vec, &ccdSimplexPoint(portal, 3)->v);
-
-    ccdVec3Cross(&vec, &ccdSimplexPoint(portal, 2)->v,
-                       &ccdSimplexPoint(portal, 1)->v);
-    b[3] = ccdVec3Dot(&vec, &ccdSimplexPoint(portal, 0)->v);
-
-	sum = b[0] + b[1] + b[2] + b[3];
-
-    if (ccdIsZero(sum) || sum < CCD_ZERO){
-		b[0] = CCD_REAL(0.);
-
-        ccdVec3Cross(&vec, &ccdSimplexPoint(portal, 2)->v,
-                           &ccdSimplexPoint(portal, 3)->v);
-        b[1] = ccdVec3Dot(&vec, &dir);
-        ccdVec3Cross(&vec, &ccdSimplexPoint(portal, 3)->v,
-                           &ccdSimplexPoint(portal, 1)->v);
-        b[2] = ccdVec3Dot(&vec, &dir);
-        ccdVec3Cross(&vec, &ccdSimplexPoint(portal, 1)->v,
-                           &ccdSimplexPoint(portal, 2)->v);
-        b[3] = ccdVec3Dot(&vec, &dir);
-
-		sum = b[1] + b[2] + b[3];
-	}
-
-	inv = CCD_REAL(1.) / sum;
-
-    ccdVec3Copy(&p1, ccd_vec3_origin);
-    ccdVec3Copy(&p2, ccd_vec3_origin);
-    for (i = 0; i < 4; i++){
-        ccdVec3Copy(&vec, &ccdSimplexPoint(portal, (int)i)->v1);
-        ccdVec3Scale(&vec, b[i]);
-        ccdVec3Add(&p1, &vec);
-
-        ccdVec3Copy(&vec, &ccdSimplexPoint(portal, (int)i)->v2);
-        ccdVec3Scale(&vec, b[i]);
-        ccdVec3Add(&p2, &vec);
-    }
-    ccdVec3Scale(&p1, inv);
-    ccdVec3Scale(&p2, inv);
-
-    ccdVec3Copy(pos, &p1);
-    ccdVec3Add(pos, &p2);
-    ccdVec3Scale(pos, 0.5);
-}
-
-_ccd_inline void expandPortal(ccd_simplex_t *portal,
-                              const ccd_support_t *v4)
-{
-    ccd_real_t dot;
-    ccd_vec3_t v4v0;
-
-    ccdVec3Cross(&v4v0, &v4->v, &ccdSimplexPoint(portal, 0)->v);
-    dot = ccdVec3Dot(&ccdSimplexPoint(portal, 1)->v, &v4v0);
-    if (dot > CCD_ZERO){
-        dot = ccdVec3Dot(&ccdSimplexPoint(portal, 2)->v, &v4v0);
-        if (dot > CCD_ZERO){
-            ccdSimplexSet(portal, 1, v4);
-        }else{
-            ccdSimplexSet(portal, 3, v4);
-        }
-    }else{
-        dot = ccdVec3Dot(&ccdSimplexPoint(portal, 3)->v, &v4v0);
-        if (dot > CCD_ZERO){
-            ccdSimplexSet(portal, 2, v4);
-        }else{
-            ccdSimplexSet(portal, 1, v4);
-        }
-    }
-}
-
-_ccd_inline void portalDir(const ccd_simplex_t *portal, ccd_vec3_t *dir)
-{
-    ccd_vec3_t v2v1, v3v1;
-
-    ccdVec3Sub2(&v2v1, &ccdSimplexPoint(portal, 2)->v,
-                       &ccdSimplexPoint(portal, 1)->v);
-    ccdVec3Sub2(&v3v1, &ccdSimplexPoint(portal, 3)->v,
-                       &ccdSimplexPoint(portal, 1)->v);
-    ccdVec3Cross(dir, &v2v1, &v3v1);
-    ccdVec3Normalize(dir);
-}
-
-_ccd_inline int portalEncapsulesOrigin(const ccd_simplex_t *portal,
-                                       const ccd_vec3_t *dir)
-{
-    ccd_real_t dot;
-    dot = ccdVec3Dot(dir, &ccdSimplexPoint(portal, 1)->v);
-    return ccdIsZero(dot) || dot > CCD_ZERO;
-}
-
-_ccd_inline int portalReachTolerance(const ccd_simplex_t *portal,
-                                     const ccd_support_t *v4,
-                                     const ccd_vec3_t *dir,
-                                     const ccd_t *ccd)
-{
-    ccd_real_t dv1, dv2, dv3, dv4;
-    ccd_real_t dot1, dot2, dot3;
-
-    // find the smallest dot product of dir and {v1-v4, v2-v4, v3-v4}
-
-    dv1 = ccdVec3Dot(&ccdSimplexPoint(portal, 1)->v, dir);
-    dv2 = ccdVec3Dot(&ccdSimplexPoint(portal, 2)->v, dir);
-    dv3 = ccdVec3Dot(&ccdSimplexPoint(portal, 3)->v, dir);
-    dv4 = ccdVec3Dot(&v4->v, dir);
-
-    dot1 = dv4 - dv1;
-    dot2 = dv4 - dv2;
-    dot3 = dv4 - dv3;
-
-    dot1 = CCD_FMIN(dot1, dot2);
-    dot1 = CCD_FMIN(dot1, dot3);
-
-    return ccdEq(dot1, ccd->mpr_tolerance) || dot1 < ccd->mpr_tolerance;
-}
-
-_ccd_inline int portalCanEncapsuleOrigin(const ccd_simplex_t *portal,   
-                                         const ccd_support_t *v4,
-                                         const ccd_vec3_t *dir)
-{
-    (void)(portal);
-
-    ccd_real_t dot;
-    dot = ccdVec3Dot(&v4->v, dir);
-    return ccdIsZero(dot) || dot > CCD_ZERO;
-}
diff --git a/src/pe/extern/libccd/polytope.c b/src/pe/extern/libccd/polytope.c
deleted file mode 100644
index 527016cdee360a5eba62e8063720b63c3b010a81..0000000000000000000000000000000000000000
--- a/src/pe/extern/libccd/polytope.c
+++ /dev/null
@@ -1,298 +0,0 @@
-/***
- * libccd
- * ---------------------------------
- * Copyright (c)2010 Daniel Fiser <danfis@danfis.cz>
- *
- *
- *  This file is part of libccd.
- *
- *  Distributed under the OSI-approved BSD License (the "License");
- *  see accompanying file BDS-LICENSE for details or see
- *  <http://www.opensource.org/licenses/bsd-license.php>.
- *
- *  This software is distributed WITHOUT ANY WARRANTY; without even the
- *  implied warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.
- *  See the License for more information.
- */
-
-#include <stdio.h>
-#include <float.h>
-#include "polytope.h"
-#include "alloc.h"
-
-_ccd_inline void _ccdPtNearestUpdate(ccd_pt_t *pt, ccd_pt_el_t *el)
-{
-    if (ccdEq(pt->nearest_dist, el->dist)){
-        if (el->type < pt->nearest_type){
-            pt->nearest = el;
-            pt->nearest_dist = el->dist;
-            pt->nearest_type = el->type;
-        }
-    }else if (el->dist < pt->nearest_dist){
-        pt->nearest = el;
-        pt->nearest_dist = el->dist;
-        pt->nearest_type = el->type;
-    }
-}
-
-static void _ccdPtNearestRenew(ccd_pt_t *pt)
-{
-    ccd_pt_vertex_t *v;
-    ccd_pt_edge_t *e;
-    ccd_pt_face_t *f;
-
-    pt->nearest_dist = CCD_REAL_MAX;
-    pt->nearest_type = 3;
-    pt->nearest = NULL;
-
-    ccdListForEachEntry(&pt->vertices, v, ccd_pt_vertex_t, list){
-        _ccdPtNearestUpdate(pt, (ccd_pt_el_t *)v);
-    }
-
-    ccdListForEachEntry(&pt->edges, e, ccd_pt_edge_t, list){
-        _ccdPtNearestUpdate(pt, (ccd_pt_el_t *)e);
-    }
-
-    ccdListForEachEntry(&pt->faces, f, ccd_pt_face_t, list){
-        _ccdPtNearestUpdate(pt, (ccd_pt_el_t *)f);
-    }
-}
-
-
-
-void ccdPtInit(ccd_pt_t *pt)
-{
-    ccdListInit(&pt->vertices);
-    ccdListInit(&pt->edges);
-    ccdListInit(&pt->faces);
-
-    pt->nearest = NULL;
-    pt->nearest_dist = CCD_REAL_MAX;
-    pt->nearest_type = 3;
-}
-
-void ccdPtDestroy(ccd_pt_t *pt)
-{
-    ccd_pt_face_t *f, *f2;
-    ccd_pt_edge_t *e, *e2;
-    ccd_pt_vertex_t *v, *v2;
-
-    // first delete all faces
-    ccdListForEachEntrySafe(&pt->faces, f, ccd_pt_face_t, f2, ccd_pt_face_t, list){
-        ccdPtDelFace(pt, f);
-    }
-
-    // delete all edges
-    ccdListForEachEntrySafe(&pt->edges, e, ccd_pt_edge_t, e2, ccd_pt_edge_t, list){
-        ccdPtDelEdge(pt, e);
-    }
-
-    // delete all vertices
-    ccdListForEachEntrySafe(&pt->vertices, v, ccd_pt_vertex_t, v2, ccd_pt_vertex_t, list){
-        ccdPtDelVertex(pt, v);
-    }
-}
-
-
-ccd_pt_vertex_t *ccdPtAddVertex(ccd_pt_t *pt, const ccd_support_t *v)
-{
-    ccd_pt_vertex_t *vert;
-
-    vert = CCD_ALLOC(ccd_pt_vertex_t);
-    if (vert == NULL)
-        return NULL;
-
-    vert->type = CCD_PT_VERTEX;
-    ccdSupportCopy(&vert->v, v);
-
-    vert->dist = ccdVec3Len2(&vert->v.v);
-    ccdVec3Copy(&vert->witness, &vert->v.v);
-
-    ccdListInit(&vert->edges);
-
-    // add vertex to list
-    ccdListAppend(&pt->vertices, &vert->list);
-
-    // update position in .nearest array
-    _ccdPtNearestUpdate(pt, (ccd_pt_el_t *)vert);
-
-    return vert;
-}
-
-ccd_pt_edge_t *ccdPtAddEdge(ccd_pt_t *pt, ccd_pt_vertex_t *v1,
-                                          ccd_pt_vertex_t *v2)
-{
-    const ccd_vec3_t *a, *b;
-    ccd_pt_edge_t *edge;
-
-    if (v1 == NULL || v2 == NULL)
-        return NULL;
-
-    edge = CCD_ALLOC(ccd_pt_edge_t);
-    if (edge == NULL)
-        return NULL;
-
-    edge->type = CCD_PT_EDGE;
-    edge->vertex[0] = v1;
-    edge->vertex[1] = v2;
-    edge->faces[0] = edge->faces[1] = NULL;
-
-    a = &edge->vertex[0]->v.v;
-    b = &edge->vertex[1]->v.v;
-    edge->dist = ccdVec3PointSegmentDist2(ccd_vec3_origin, a, b, &edge->witness);
-
-    ccdListAppend(&edge->vertex[0]->edges, &edge->vertex_list[0]);
-    ccdListAppend(&edge->vertex[1]->edges, &edge->vertex_list[1]);
-
-    ccdListAppend(&pt->edges, &edge->list);
-
-    // update position in .nearest array
-    _ccdPtNearestUpdate(pt, (ccd_pt_el_t *)edge);
-
-    return edge;
-}
-
-ccd_pt_face_t *ccdPtAddFace(ccd_pt_t *pt, ccd_pt_edge_t *e1,
-                                          ccd_pt_edge_t *e2,
-                                          ccd_pt_edge_t *e3)
-{
-    const ccd_vec3_t *a, *b, *c;
-    ccd_pt_face_t *face;
-    ccd_pt_edge_t *e;
-    size_t i;
-
-    if (e1 == NULL || e2 == NULL || e3 == NULL)
-        return NULL;
-
-    face = CCD_ALLOC(ccd_pt_face_t);
-    if (face == NULL)
-        return NULL;
-
-    face->type = CCD_PT_FACE;
-    face->edge[0] = e1;
-    face->edge[1] = e2;
-    face->edge[2] = e3;
-
-    // obtain triplet of vertices
-    a = &face->edge[0]->vertex[0]->v.v;
-    b = &face->edge[0]->vertex[1]->v.v;
-    e = face->edge[1];
-    if (e->vertex[0] != face->edge[0]->vertex[0]
-            && e->vertex[0] != face->edge[0]->vertex[1]){
-        c = &e->vertex[0]->v.v;
-    }else{
-        c = &e->vertex[1]->v.v;
-    }
-    face->dist = ccdVec3PointTriDist2(ccd_vec3_origin, a, b, c, &face->witness);
-
-
-    for (i = 0; i < 3; i++){
-        if (face->edge[i]->faces[0] == NULL){
-            face->edge[i]->faces[0] = face;
-        }else{
-            face->edge[i]->faces[1] = face;
-        }
-    }
-
-    ccdListAppend(&pt->faces, &face->list);
-
-    // update position in .nearest array
-    _ccdPtNearestUpdate(pt, (ccd_pt_el_t *)face);
-
-    return face;
-}
-
-
-void ccdPtRecomputeDistances(ccd_pt_t *pt)
-{
-    ccd_pt_vertex_t *v;
-    ccd_pt_edge_t *e;
-    ccd_pt_face_t *f;
-    const ccd_vec3_t *a, *b, *c;
-    ccd_real_t dist;
-
-    ccdListForEachEntry(&pt->vertices, v, ccd_pt_vertex_t, list){
-        dist = ccdVec3Len2(&v->v.v);
-        v->dist = dist;
-        ccdVec3Copy(&v->witness, &v->v.v);
-    }
-
-    ccdListForEachEntry(&pt->edges, e, ccd_pt_edge_t, list){
-        a = &e->vertex[0]->v.v;
-        b = &e->vertex[1]->v.v;
-        dist = ccdVec3PointSegmentDist2(ccd_vec3_origin, a, b, &e->witness);
-        e->dist = dist;
-    }
-
-    ccdListForEachEntry(&pt->faces, f, ccd_pt_face_t, list){
-        // obtain triplet of vertices
-        a = &f->edge[0]->vertex[0]->v.v;
-        b = &f->edge[0]->vertex[1]->v.v;
-        e = f->edge[1];
-        if (e->vertex[0] != f->edge[0]->vertex[0]
-                && e->vertex[0] != f->edge[0]->vertex[1]){
-            c = &e->vertex[0]->v.v;
-        }else{
-            c = &e->vertex[1]->v.v;
-        }
-
-        dist = ccdVec3PointTriDist2(ccd_vec3_origin, a, b, c, &f->witness);
-        f->dist = dist;
-    }
-}
-
-ccd_pt_el_t *ccdPtNearest(ccd_pt_t *pt)
-{
-    if (!pt->nearest){
-        _ccdPtNearestRenew(pt);
-    }
-    return pt->nearest;
-}
-
-
-void ccdPtDumpSVT(ccd_pt_t *pt, const char *fn)
-{
-    FILE *fout;
-
-    fout = fopen(fn, "a");
-    if (fout == NULL)
-        return;
-
-    ccdPtDumpSVT2(pt, fout);
-
-    fclose(fout);
-}
-
-void ccdPtDumpSVT2(ccd_pt_t *pt, FILE *fout)
-{
-    ccd_pt_vertex_t *v, *a, *b, *c;
-    ccd_pt_edge_t *e;
-    ccd_pt_face_t *f;
-    size_t i;
-
-    fprintf(fout, "-----\n");
-
-    fprintf(fout, "Points:\n");
-    i = 0;
-    ccdListForEachEntry(&pt->vertices, v, ccd_pt_vertex_t, list){
-        v->id = (int)(i++);
-        fprintf(fout, "%lf %lf %lf\n",
-                ccdVec3X(&v->v.v), ccdVec3Y(&v->v.v), ccdVec3Z(&v->v.v));
-    }
-
-    fprintf(fout, "Edges:\n");
-    ccdListForEachEntry(&pt->edges, e, ccd_pt_edge_t, list){
-        fprintf(fout, "%d %d\n", e->vertex[0]->id, e->vertex[1]->id);
-    }
-
-    fprintf(fout, "Faces:\n");
-    ccdListForEachEntry(&pt->faces, f, ccd_pt_face_t, list){
-        a = f->edge[0]->vertex[0];
-        b = f->edge[0]->vertex[1];
-        c = f->edge[1]->vertex[0];
-        if (c == a || c == b){
-            c = f->edge[1]->vertex[1];
-        }
-        fprintf(fout, "%d %d %d\n", a->id, b->id, c->id);
-    }
-}
diff --git a/src/pe/extern/libccd/polytope.h b/src/pe/extern/libccd/polytope.h
deleted file mode 100644
index b671224ebf904208fe7b9b3aee7267fcf7aec696..0000000000000000000000000000000000000000
--- a/src/pe/extern/libccd/polytope.h
+++ /dev/null
@@ -1,322 +0,0 @@
-/***
- * libccd
- * ---------------------------------
- * Copyright (c)2010 Daniel Fiser <danfis@danfis.cz>
- *
- *
- *  This file is part of libccd.
- *
- *  Distributed under the OSI-approved BSD License (the "License");
- *  see accompanying file BDS-LICENSE for details or see
- *  <http://www.opensource.org/licenses/bsd-license.php>.
- *
- *  This software is distributed WITHOUT ANY WARRANTY; without even the
- *  implied warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.
- *  See the License for more information.
- */
-
-#ifndef __CCD_POLYTOPE_H__
-#define __CCD_POLYTOPE_H__
-
-#include <stdlib.h>
-#include <stdio.h>
-#include "support.h"
-#include "list.h"
-
-#ifdef __cplusplus
-extern "C" {
-#endif /* __cplusplus */
-
-#define CCD_PT_VERTEX 1
-#define CCD_PT_EDGE   2
-#define CCD_PT_FACE   3
-
-
-#define __CCD_PT_EL \
-    int type;           /*! type of element */ \
-    ccd_real_t dist;        /*! distance from origin */ \
-    ccd_vec3_t witness; /*! witness point of projection of origin */ \
-    ccd_list_t list;    /*! list of elements of same type */
-
-/**
- * General polytope element.
- * Could be vertex, edge or triangle.
- */
-struct _ccd_pt_el_t {
-    __CCD_PT_EL
-};
-typedef struct _ccd_pt_el_t ccd_pt_el_t;
-
-struct _ccd_pt_edge_t;
-struct _ccd_pt_face_t;
-
-/**
- * Polytope's vertex.
- */
-struct _ccd_pt_vertex_t {
-    __CCD_PT_EL
-
-    int id;
-    ccd_support_t v;
-    ccd_list_t edges; //!< List of edges
-};
-typedef struct _ccd_pt_vertex_t ccd_pt_vertex_t;
-
-/**
- * Polytope's edge.
- */
-struct _ccd_pt_edge_t {
-    __CCD_PT_EL
-
-    ccd_pt_vertex_t *vertex[2]; //!< Reference to vertices
-    struct _ccd_pt_face_t *faces[2]; //!< Reference to faces
-
-    ccd_list_t vertex_list[2]; //!< List items in vertices' lists
-};
-typedef struct _ccd_pt_edge_t ccd_pt_edge_t;
-
-/**
- * Polytope's triangle faces.
- */
-struct _ccd_pt_face_t {
-    __CCD_PT_EL
-
-    ccd_pt_edge_t *edge[3]; //!< Reference to surrounding edges
-};
-typedef struct _ccd_pt_face_t ccd_pt_face_t;
-
-
-/**
- * Struct containing polytope.
- */
-struct _ccd_pt_t {
-    ccd_list_t vertices; //!< List of vertices
-    ccd_list_t edges; //!< List of edges
-    ccd_list_t faces; //!< List of faces
-
-    ccd_pt_el_t *nearest;
-    ccd_real_t nearest_dist;
-    int nearest_type;
-};
-typedef struct _ccd_pt_t ccd_pt_t;
-
-
-void ccdPtInit(ccd_pt_t *pt);
-void ccdPtDestroy(ccd_pt_t *pt);
-
-/**
- * Returns vertices surrounding given triangle face.
- */
-_ccd_inline void ccdPtFaceVec3(const ccd_pt_face_t *face,
-                               ccd_vec3_t **a,
-                               ccd_vec3_t **b,
-                               ccd_vec3_t **c);
-_ccd_inline void ccdPtFaceVertices(const ccd_pt_face_t *face,
-                                   ccd_pt_vertex_t **a,
-                                   ccd_pt_vertex_t **b,
-                                   ccd_pt_vertex_t **c);
-_ccd_inline void ccdPtFaceEdges(const ccd_pt_face_t *f,
-                                ccd_pt_edge_t **a,
-                                ccd_pt_edge_t **b,
-                                ccd_pt_edge_t **c);
-
-_ccd_inline void ccdPtEdgeVec3(const ccd_pt_edge_t *e,
-                               ccd_vec3_t **a,
-                               ccd_vec3_t **b);
-_ccd_inline void ccdPtEdgeVertices(const ccd_pt_edge_t *e,
-                                   ccd_pt_vertex_t **a,
-                                   ccd_pt_vertex_t **b);
-_ccd_inline void ccdPtEdgeFaces(const ccd_pt_edge_t *e,
-                                ccd_pt_face_t **f1,
-                                ccd_pt_face_t **f2);
-
-
-/**
- * Adds vertex to polytope and returns pointer to newly created vertex.
- */
-ccd_pt_vertex_t *ccdPtAddVertex(ccd_pt_t *pt, const ccd_support_t *v);
-_ccd_inline ccd_pt_vertex_t *ccdPtAddVertexCoords(ccd_pt_t *pt,
-                                                  ccd_real_t x, ccd_real_t y, ccd_real_t z);
-
-/**
- * Adds edge to polytope.
- */
-ccd_pt_edge_t *ccdPtAddEdge(ccd_pt_t *pt, ccd_pt_vertex_t *v1,
-                                          ccd_pt_vertex_t *v2);
-
-/**
- * Adds face to polytope.
- */
-ccd_pt_face_t *ccdPtAddFace(ccd_pt_t *pt, ccd_pt_edge_t *e1,
-                                          ccd_pt_edge_t *e2,
-                                          ccd_pt_edge_t *e3);
-
-/**
- * Deletes vertex from polytope.
- * Returns 0 on success, -1 otherwise.
- */
-_ccd_inline int ccdPtDelVertex(ccd_pt_t *pt, ccd_pt_vertex_t *);
-_ccd_inline int ccdPtDelEdge(ccd_pt_t *pt, ccd_pt_edge_t *);
-_ccd_inline int ccdPtDelFace(ccd_pt_t *pt, ccd_pt_face_t *);
-
-
-/**
- * Recompute distances from origin for all elements in pt.
- */
-void ccdPtRecomputeDistances(ccd_pt_t *pt);
-
-/**
- * Returns nearest element to origin.
- */
-ccd_pt_el_t *ccdPtNearest(ccd_pt_t *pt);
-
-
-void ccdPtDumpSVT(ccd_pt_t *pt, const char *fn);
-void ccdPtDumpSVT2(ccd_pt_t *pt, FILE *);
-
-
-/**** INLINES ****/
-_ccd_inline ccd_pt_vertex_t *ccdPtAddVertexCoords(ccd_pt_t *pt,
-                                                  ccd_real_t x, ccd_real_t y, ccd_real_t z)
-{
-    ccd_support_t s;
-    ccdVec3Set(&s.v, x, y, z);
-    return ccdPtAddVertex(pt, &s);
-}
-
-_ccd_inline int ccdPtDelVertex(ccd_pt_t *pt, ccd_pt_vertex_t *v)
-{
-    // test if any edge is connected to this vertex
-    if (!ccdListEmpty(&v->edges))
-        return -1;
-
-    // delete vertex from main list
-    ccdListDel(&v->list);
-
-    if ((void *)pt->nearest == (void *)v){
-        pt->nearest = NULL;
-    }
-
-    free(v);
-    return 0;
-}
-
-_ccd_inline int ccdPtDelEdge(ccd_pt_t *pt, ccd_pt_edge_t *e)
-{
-    // text if any face is connected to this edge (faces[] is always
-    // aligned to lower indices)
-    if (e->faces[0] != NULL)
-        return -1;
-
-    // disconnect edge from lists of edges in vertex struct
-    ccdListDel(&e->vertex_list[0]);
-    ccdListDel(&e->vertex_list[1]);
-
-    // disconnect edge from main list
-    ccdListDel(&e->list);
-
-    if ((void *)pt->nearest == (void *)e){
-        pt->nearest = NULL;
-    }
-
-    free(e);
-    return 0;
-}
-
-_ccd_inline int ccdPtDelFace(ccd_pt_t *pt, ccd_pt_face_t *f)
-{
-    ccd_pt_edge_t *e;
-    size_t i;
-
-    // remove face from edges' recerence lists
-    for (i = 0; i < 3; i++){
-        e = f->edge[i];
-        if (e->faces[0] == f){
-            e->faces[0] = e->faces[1];
-        }
-        e->faces[1] = NULL;
-    }
-
-    // remove face from list of all faces
-    ccdListDel(&f->list);
-
-    if ((void *)pt->nearest == (void *)f){
-        pt->nearest = NULL;
-    }
-
-    free(f);
-    return 0;
-}
-
-_ccd_inline void ccdPtFaceVec3(const ccd_pt_face_t *face,
-                               ccd_vec3_t **a,
-                               ccd_vec3_t **b,
-                               ccd_vec3_t **c)
-{
-    *a = &face->edge[0]->vertex[0]->v.v;
-    *b = &face->edge[0]->vertex[1]->v.v;
-
-    if (face->edge[1]->vertex[0] != face->edge[0]->vertex[0]
-            && face->edge[1]->vertex[0] != face->edge[0]->vertex[1]){
-        *c = &face->edge[1]->vertex[0]->v.v;
-    }else{
-        *c = &face->edge[1]->vertex[1]->v.v;
-    }
-}
-
-_ccd_inline void ccdPtFaceVertices(const ccd_pt_face_t *face,
-                                   ccd_pt_vertex_t **a,
-                                   ccd_pt_vertex_t **b,
-                                   ccd_pt_vertex_t **c)
-{
-    *a = face->edge[0]->vertex[0];
-    *b = face->edge[0]->vertex[1];
-
-    if (face->edge[1]->vertex[0] != face->edge[0]->vertex[0]
-            && face->edge[1]->vertex[0] != face->edge[0]->vertex[1]){
-        *c = face->edge[1]->vertex[0];
-    }else{
-        *c = face->edge[1]->vertex[1];
-    }
-}
-
-_ccd_inline void ccdPtFaceEdges(const ccd_pt_face_t *f,
-                                ccd_pt_edge_t **a,
-                                ccd_pt_edge_t **b,
-                                ccd_pt_edge_t **c)
-{
-    *a = f->edge[0];
-    *b = f->edge[1];
-    *c = f->edge[2];
-}
-
-_ccd_inline void ccdPtEdgeVec3(const ccd_pt_edge_t *e,
-                               ccd_vec3_t **a,
-                               ccd_vec3_t **b)
-{
-    *a = &e->vertex[0]->v.v;
-    *b = &e->vertex[1]->v.v;
-}
-
-_ccd_inline void ccdPtEdgeVertices(const ccd_pt_edge_t *e,
-                                   ccd_pt_vertex_t **a,
-                                   ccd_pt_vertex_t **b)
-{
-    *a = e->vertex[0];
-    *b = e->vertex[1];
-}
-
-_ccd_inline void ccdPtEdgeFaces(const ccd_pt_edge_t *e,
-                                ccd_pt_face_t **f1,
-                                ccd_pt_face_t **f2)
-{
-    *f1 = e->faces[0];
-    *f2 = e->faces[1];
-}
-
-
-#ifdef __cplusplus
-} /* extern "C" */
-#endif /* __cplusplus */
-
-#endif /* __CCD_POLYTOPE_H__ */
diff --git a/src/pe/extern/libccd/simplex.h b/src/pe/extern/libccd/simplex.h
deleted file mode 100644
index 8ae09c7123b32d709baace7f8eb0ad7872f9d7c0..0000000000000000000000000000000000000000
--- a/src/pe/extern/libccd/simplex.h
+++ /dev/null
@@ -1,104 +0,0 @@
-/***
- * libccd
- * ---------------------------------
- * Copyright (c)2010 Daniel Fiser <danfis@danfis.cz>
- *
- *
- *  This file is part of libccd.
- *
- *  Distributed under the OSI-approved BSD License (the "License");
- *  see accompanying file BDS-LICENSE for details or see
- *  <http://www.opensource.org/licenses/bsd-license.php>.
- *
- *  This software is distributed WITHOUT ANY WARRANTY; without even the
- *  implied warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.
- *  See the License for more information.
- */
-
-#ifndef __CCD_SIMPLEX_H__
-#define __CCD_SIMPLEX_H__
-
-#include <ccd/compiler.h>
-#include "support.h"
-
-#ifdef __cplusplus
-extern "C" {
-#endif /* __cplusplus */
-
-struct _ccd_simplex_t {
-    ccd_support_t ps[4];
-    int last; //!< index of last added point
-};
-typedef struct _ccd_simplex_t ccd_simplex_t;
-
-
-_ccd_inline void ccdSimplexInit(ccd_simplex_t *s);
-_ccd_inline int ccdSimplexSize(const ccd_simplex_t *s);
-_ccd_inline const ccd_support_t *ccdSimplexLast(const ccd_simplex_t *s);
-_ccd_inline const ccd_support_t *ccdSimplexPoint(const ccd_simplex_t *s, int idx);
-_ccd_inline ccd_support_t *ccdSimplexPointW(ccd_simplex_t *s, int idx);
-
-_ccd_inline void ccdSimplexAdd(ccd_simplex_t *s, const ccd_support_t *v);
-_ccd_inline void ccdSimplexSet(ccd_simplex_t *s, size_t pos, const ccd_support_t *a);
-_ccd_inline void ccdSimplexSetSize(ccd_simplex_t *s, int size);
-_ccd_inline void ccdSimplexSwap(ccd_simplex_t *s, size_t pos1, size_t pos2);
-
-
-/**** INLINES ****/
-
-_ccd_inline void ccdSimplexInit(ccd_simplex_t *s)
-{
-    s->last = -1;
-}
-
-_ccd_inline int ccdSimplexSize(const ccd_simplex_t *s)
-{
-    return s->last + 1;
-}
-
-_ccd_inline const ccd_support_t *ccdSimplexLast(const ccd_simplex_t *s)
-{
-    return ccdSimplexPoint(s, s->last);
-}
-
-_ccd_inline const ccd_support_t *ccdSimplexPoint(const ccd_simplex_t *s, int idx)
-{
-    // here is no check on boundaries
-    return &s->ps[idx];
-}
-_ccd_inline ccd_support_t *ccdSimplexPointW(ccd_simplex_t *s, int idx)
-{
-    return &s->ps[idx];
-}
-
-_ccd_inline void ccdSimplexAdd(ccd_simplex_t *s, const ccd_support_t *v)
-{
-    // here is no check on boundaries in sake of speed
-    ++s->last;
-    ccdSupportCopy(s->ps + s->last, v);
-}
-
-_ccd_inline void ccdSimplexSet(ccd_simplex_t *s, size_t pos, const ccd_support_t *a)
-{
-    ccdSupportCopy(s->ps + pos, a);
-}
-
-_ccd_inline void ccdSimplexSetSize(ccd_simplex_t *s, int size)
-{
-    s->last = size - 1;
-}
-
-_ccd_inline void ccdSimplexSwap(ccd_simplex_t *s, size_t pos1, size_t pos2)
-{
-    ccd_support_t supp;
-
-    ccdSupportCopy(&supp, &s->ps[pos1]);
-    ccdSupportCopy(&s->ps[pos1], &s->ps[pos2]);
-    ccdSupportCopy(&s->ps[pos2], &supp);
-}
-
-#ifdef __cplusplus
-} /* extern "C" */
-#endif /* __cplusplus */
-
-#endif /* __CCD_SIMPLEX_H__ */
diff --git a/src/pe/extern/libccd/support.c b/src/pe/extern/libccd/support.c
deleted file mode 100644
index 6c7f1cc65863be3c974aed1a37df2c9bd3623c50..0000000000000000000000000000000000000000
--- a/src/pe/extern/libccd/support.c
+++ /dev/null
@@ -1,34 +0,0 @@
-/***
- * libccd
- * ---------------------------------
- * Copyright (c)2010 Daniel Fiser <danfis@danfis.cz>
- *
- *
- *  This file is part of libccd.
- *
- *  Distributed under the OSI-approved BSD License (the "License");
- *  see accompanying file BDS-LICENSE for details or see
- *  <http://www.opensource.org/licenses/bsd-license.php>.
- *
- *  This software is distributed WITHOUT ANY WARRANTY; without even the
- *  implied warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.
- *  See the License for more information.
- */
-
-#include "support.h"
-
-void __ccdSupport(const void *obj1, const void *obj2,
-                  const ccd_vec3_t *_dir, const ccd_t *ccd,
-                  ccd_support_t *supp)
-{
-    ccd_vec3_t dir;
-
-    ccdVec3Copy(&dir, _dir);
-
-    ccd->support1(obj1, &dir, &supp->v1);
-
-    ccdVec3Scale(&dir, -CCD_ONE);
-    ccd->support2(obj2, &dir, &supp->v2);
-
-    ccdVec3Sub2(&supp->v, &supp->v1, &supp->v2);
-}
diff --git a/src/pe/extern/libccd/support.h b/src/pe/extern/libccd/support.h
deleted file mode 100644
index 3372f5ef216e6d2c2e834e3dd45b2ec368a88913..0000000000000000000000000000000000000000
--- a/src/pe/extern/libccd/support.h
+++ /dev/null
@@ -1,55 +0,0 @@
-/***
- * libccd
- * ---------------------------------
- * Copyright (c)2010 Daniel Fiser <danfis@danfis.cz>
- *
- *
- *  This file is part of libccd.
- *
- *  Distributed under the OSI-approved BSD License (the "License");
- *  see accompanying file BDS-LICENSE for details or see
- *  <http://www.opensource.org/licenses/bsd-license.php>.
- *
- *  This software is distributed WITHOUT ANY WARRANTY; without even the
- *  implied warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.
- *  See the License for more information.
- */
-
-#ifndef __CCD_SUPPORT_H__
-#define __CCD_SUPPORT_H__
-
-#include <ccd/ccd.h>
-
-#ifdef __cplusplus
-extern "C" {
-#endif /* __cplusplus */
-
-struct _ccd_support_t {
-    ccd_vec3_t v;  //!< Support point in minkowski sum
-    ccd_vec3_t v1; //!< Support point in obj1
-    ccd_vec3_t v2; //!< Support point in obj2
-};
-typedef struct _ccd_support_t ccd_support_t;
-
-_ccd_inline void ccdSupportCopy(ccd_support_t *, const ccd_support_t *s);
-
-/**
- * Computes support point of obj1 and obj2 in direction dir.
- * Support point is returned via supp.
- */
-void __ccdSupport(const void *obj1, const void *obj2,
-                  const ccd_vec3_t *dir, const ccd_t *ccd,
-                  ccd_support_t *supp);
-
-
-/**** INLINES ****/
-_ccd_inline void ccdSupportCopy(ccd_support_t *d, const ccd_support_t *s)
-{
-    *d = *s;
-}
-
-#ifdef __cplusplus
-} /* extern "C" */
-#endif /* __cplusplus */
-
-#endif /* __CCD_SUPPORT_H__ */
diff --git a/src/pe/extern/libccd/vec3.c b/src/pe/extern/libccd/vec3.c
deleted file mode 100644
index f0a331f4fd486a8919dcc23902568cfa3b903540..0000000000000000000000000000000000000000
--- a/src/pe/extern/libccd/vec3.c
+++ /dev/null
@@ -1,211 +0,0 @@
-/***
- * libccd
- * ---------------------------------
- * Copyright (c)2010 Daniel Fiser <danfis@danfis.cz>
- *
- *
- *  This file is part of libccd.
- *
- *  Distributed under the OSI-approved BSD License (the "License");
- *  see accompanying file BDS-LICENSE for details or see
- *  <http://www.opensource.org/licenses/bsd-license.php>.
- *
- *  This software is distributed WITHOUT ANY WARRANTY; without even the
- *  implied warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.
- *  See the License for more information.
- */
-
-#include <stdio.h>
-#include <ccd/vec3.h>
-#include "dbg.h"
-
-static CCD_VEC3(__ccd_vec3_origin, CCD_ZERO, CCD_ZERO, CCD_ZERO);
-ccd_vec3_t *ccd_vec3_origin = &__ccd_vec3_origin;
-
-static ccd_vec3_t points_on_sphere[] = {
-	CCD_VEC3_STATIC(CCD_REAL( 0.000000), CCD_REAL(-0.000000), CCD_REAL(-1.000000)),
-	CCD_VEC3_STATIC(CCD_REAL( 0.723608), CCD_REAL(-0.525725), CCD_REAL(-0.447219)),
-	CCD_VEC3_STATIC(CCD_REAL(-0.276388), CCD_REAL(-0.850649), CCD_REAL(-0.447219)),
-	CCD_VEC3_STATIC(CCD_REAL(-0.894426), CCD_REAL(-0.000000), CCD_REAL(-0.447216)),
-	CCD_VEC3_STATIC(CCD_REAL(-0.276388), CCD_REAL( 0.850649), CCD_REAL(-0.447220)),
-	CCD_VEC3_STATIC(CCD_REAL( 0.723608), CCD_REAL( 0.525725), CCD_REAL(-0.447219)),
-	CCD_VEC3_STATIC(CCD_REAL( 0.276388), CCD_REAL(-0.850649), CCD_REAL( 0.447220)),
-	CCD_VEC3_STATIC(CCD_REAL(-0.723608), CCD_REAL(-0.525725), CCD_REAL( 0.447219)),
-	CCD_VEC3_STATIC(CCD_REAL(-0.723608), CCD_REAL( 0.525725), CCD_REAL( 0.447219)),
-	CCD_VEC3_STATIC(CCD_REAL( 0.276388), CCD_REAL( 0.850649), CCD_REAL( 0.447219)),
-	CCD_VEC3_STATIC(CCD_REAL( 0.894426), CCD_REAL( 0.000000), CCD_REAL( 0.447216)),
-	CCD_VEC3_STATIC(CCD_REAL(-0.000000), CCD_REAL( 0.000000), CCD_REAL( 1.000000)), 
-	CCD_VEC3_STATIC(CCD_REAL( 0.425323), CCD_REAL(-0.309011), CCD_REAL(-0.850654)),
-	CCD_VEC3_STATIC(CCD_REAL(-0.162456), CCD_REAL(-0.499995), CCD_REAL(-0.850654)),
-	CCD_VEC3_STATIC(CCD_REAL( 0.262869), CCD_REAL(-0.809012), CCD_REAL(-0.525738)),
-	CCD_VEC3_STATIC(CCD_REAL( 0.425323), CCD_REAL( 0.309011), CCD_REAL(-0.850654)),
-	CCD_VEC3_STATIC(CCD_REAL( 0.850648), CCD_REAL(-0.000000), CCD_REAL(-0.525736)),
-	CCD_VEC3_STATIC(CCD_REAL(-0.525730), CCD_REAL(-0.000000), CCD_REAL(-0.850652)),
-	CCD_VEC3_STATIC(CCD_REAL(-0.688190), CCD_REAL(-0.499997), CCD_REAL(-0.525736)),
-	CCD_VEC3_STATIC(CCD_REAL(-0.162456), CCD_REAL( 0.499995), CCD_REAL(-0.850654)),
-	CCD_VEC3_STATIC(CCD_REAL(-0.688190), CCD_REAL( 0.499997), CCD_REAL(-0.525736)),
-	CCD_VEC3_STATIC(CCD_REAL( 0.262869), CCD_REAL( 0.809012), CCD_REAL(-0.525738)),
-	CCD_VEC3_STATIC(CCD_REAL( 0.951058), CCD_REAL( 0.309013), CCD_REAL( 0.000000)),
-	CCD_VEC3_STATIC(CCD_REAL( 0.951058), CCD_REAL(-0.309013), CCD_REAL( 0.000000)),
-	CCD_VEC3_STATIC(CCD_REAL( 0.587786), CCD_REAL(-0.809017), CCD_REAL( 0.000000)),
-	CCD_VEC3_STATIC(CCD_REAL( 0.000000), CCD_REAL(-1.000000), CCD_REAL( 0.000000)),
-	CCD_VEC3_STATIC(CCD_REAL(-0.587786), CCD_REAL(-0.809017), CCD_REAL( 0.000000)),
-	CCD_VEC3_STATIC(CCD_REAL(-0.951058), CCD_REAL(-0.309013), CCD_REAL(-0.000000)),
-	CCD_VEC3_STATIC(CCD_REAL(-0.951058), CCD_REAL( 0.309013), CCD_REAL(-0.000000)),
-	CCD_VEC3_STATIC(CCD_REAL(-0.587786), CCD_REAL( 0.809017), CCD_REAL(-0.000000)),
-	CCD_VEC3_STATIC(CCD_REAL(-0.000000), CCD_REAL( 1.000000), CCD_REAL(-0.000000)),
-	CCD_VEC3_STATIC(CCD_REAL( 0.587786), CCD_REAL( 0.809017), CCD_REAL(-0.000000)),
-	CCD_VEC3_STATIC(CCD_REAL( 0.688190), CCD_REAL(-0.499997), CCD_REAL( 0.525736)),
-	CCD_VEC3_STATIC(CCD_REAL(-0.262869), CCD_REAL(-0.809012), CCD_REAL( 0.525738)),
-	CCD_VEC3_STATIC(CCD_REAL(-0.850648), CCD_REAL( 0.000000), CCD_REAL( 0.525736)),
-	CCD_VEC3_STATIC(CCD_REAL(-0.262869), CCD_REAL( 0.809012), CCD_REAL( 0.525738)),
-	CCD_VEC3_STATIC(CCD_REAL( 0.688190), CCD_REAL( 0.499997), CCD_REAL( 0.525736)),
-	CCD_VEC3_STATIC(CCD_REAL( 0.525730), CCD_REAL( 0.000000), CCD_REAL( 0.850652)),
-	CCD_VEC3_STATIC(CCD_REAL( 0.162456), CCD_REAL(-0.499995), CCD_REAL( 0.850654)),
-	CCD_VEC3_STATIC(CCD_REAL(-0.425323), CCD_REAL(-0.309011), CCD_REAL( 0.850654)),
-	CCD_VEC3_STATIC(CCD_REAL(-0.425323), CCD_REAL( 0.309011), CCD_REAL( 0.850654)),
-	CCD_VEC3_STATIC(CCD_REAL( 0.162456), CCD_REAL( 0.499995), CCD_REAL( 0.850654))
-};
-ccd_vec3_t *ccd_points_on_sphere = points_on_sphere;
-size_t ccd_points_on_sphere_len = sizeof(points_on_sphere) / sizeof(ccd_vec3_t);
-
-
-_ccd_inline ccd_real_t __ccdVec3PointSegmentDist2(const ccd_vec3_t *P,
-                                                  const ccd_vec3_t *x0,
-                                                  const ccd_vec3_t *b,
-                                                  ccd_vec3_t *witness)
-{
-    // The computation comes from solving equation of segment:
-    //      S(t) = x0 + t.d
-    //          where - x0 is initial point of segment
-    //                - d is direction of segment from x0 (|d| > 0)
-    //                - t belongs to <0, 1> interval
-    // 
-    // Than, distance from a segment to some point P can be expressed:
-    //      D(t) = |x0 + t.d - P|^2
-    //          which is distance from any point on segment. Minimization
-    //          of this function brings distance from P to segment.
-    // Minimization of D(t) leads to simple quadratic equation that's
-    // solving is straightforward.
-    //
-    // Bonus of this method is witness point for free.
-
-    ccd_real_t dist, t;
-    ccd_vec3_t d, a;
-
-    // direction of segment
-    ccdVec3Sub2(&d, b, x0);
-
-    // precompute vector from P to x0
-    ccdVec3Sub2(&a, x0, P);
-
-    t  = -CCD_REAL(1.) * ccdVec3Dot(&a, &d);
-    t /= ccdVec3Len2(&d);
-
-    if (t < CCD_ZERO || ccdIsZero(t)){
-        dist = ccdVec3Dist2(x0, P);
-        if (witness)
-            ccdVec3Copy(witness, x0);
-    }else if (t > CCD_ONE || ccdEq(t, CCD_ONE)){
-        dist = ccdVec3Dist2(b, P);
-        if (witness)
-            ccdVec3Copy(witness, b);
-    }else{
-        if (witness){
-            ccdVec3Copy(witness, &d);
-            ccdVec3Scale(witness, t);
-            ccdVec3Add(witness, x0);
-            dist = ccdVec3Dist2(witness, P);
-        }else{
-            // recycling variables
-            ccdVec3Scale(&d, t);
-            ccdVec3Add(&d, &a);
-            dist = ccdVec3Len2(&d);
-        }
-    }
-
-    return dist;
-}
-
-ccd_real_t ccdVec3PointSegmentDist2(const ccd_vec3_t *P,
-                                    const ccd_vec3_t *x0, const ccd_vec3_t *b,
-                                    ccd_vec3_t *witness)
-{
-    return __ccdVec3PointSegmentDist2(P, x0, b, witness);
-}
-
-ccd_real_t ccdVec3PointTriDist2(const ccd_vec3_t *P,
-                                const ccd_vec3_t *x0, const ccd_vec3_t *B,
-                                const ccd_vec3_t *C,
-                                ccd_vec3_t *witness)
-{
-    // Computation comes from analytic expression for triangle (x0, B, C)
-    //      T(s, t) = x0 + s.d1 + t.d2, where d1 = B - x0 and d2 = C - x0 and
-    // Then equation for distance is:
-    //      D(s, t) = | T(s, t) - P |^2
-    // This leads to minimization of quadratic function of two variables.
-    // The solution from is taken only if s is between 0 and 1, t is
-    // between 0 and 1 and t + s < 1, otherwise distance from segment is
-    // computed.
-
-    ccd_vec3_t d1, d2, a;
-    ccd_real_t u, v, w, p, q, r;
-    ccd_real_t s, t, dist, dist2;
-    ccd_vec3_t witness2;
-
-    ccdVec3Sub2(&d1, B, x0);
-    ccdVec3Sub2(&d2, C, x0);
-    ccdVec3Sub2(&a, x0, P);
-
-    u = ccdVec3Dot(&a, &a);
-    v = ccdVec3Dot(&d1, &d1);
-    w = ccdVec3Dot(&d2, &d2);
-    p = ccdVec3Dot(&a, &d1);
-    q = ccdVec3Dot(&a, &d2);
-    r = ccdVec3Dot(&d1, &d2);
-
-    s = (q * r - w * p) / (w * v - r * r);
-    t = (-s * r - q) / w;
-
-    if ((ccdIsZero(s) || s > CCD_ZERO)
-            && (ccdEq(s, CCD_ONE) || s < CCD_ONE)
-            && (ccdIsZero(t) || t > CCD_ZERO)
-            && (ccdEq(t, CCD_ONE) || t < CCD_ONE)
-            && (ccdEq(t + s, CCD_ONE) || t + s < CCD_ONE)){
-
-        if (witness){
-            ccdVec3Scale(&d1, s);
-            ccdVec3Scale(&d2, t);
-            ccdVec3Copy(witness, x0);
-            ccdVec3Add(witness, &d1);
-            ccdVec3Add(witness, &d2);
-
-            dist = ccdVec3Dist2(witness, P);
-        }else{
-            dist  = s * s * v;
-            dist += t * t * w;
-            dist += CCD_REAL(2.) * s * t * r;
-            dist += CCD_REAL(2.) * s * p;
-            dist += CCD_REAL(2.) * t * q;
-            dist += u;
-        }
-    }else{
-        dist = __ccdVec3PointSegmentDist2(P, x0, B, witness);
-
-        dist2 = __ccdVec3PointSegmentDist2(P, x0, C, &witness2);
-        if (dist2 < dist){
-            dist = dist2;
-            if (witness)
-                ccdVec3Copy(witness, &witness2);
-        }
-
-        dist2 = __ccdVec3PointSegmentDist2(P, B, C, &witness2);
-        if (dist2 < dist){
-            dist = dist2;
-            if (witness)
-                ccdVec3Copy(witness, &witness2);
-        }
-    }
-
-    return dist;
-}
diff --git a/src/pe/fcd/AnalyticCollisionDetection.h b/src/pe/fcd/AnalyticCollisionDetection.h
index 8b6fa28538b785034d8a1e94bf371f6940aeb403..5318eaae9176b0a826447a2b18de1e0a716b4269 100644
--- a/src/pe/fcd/AnalyticCollisionDetection.h
+++ b/src/pe/fcd/AnalyticCollisionDetection.h
@@ -2124,7 +2124,7 @@ bool collide( Union<BodyTypeTuple>* bd1, BodyB* bd2, Container& container )
    bool collision = false;
    for( auto it=bd1->begin(); it!=bd1->end(); ++it )
    {
-      collision |= SingleCast<BodyTypeTuple, AnalyticSingleCollideFunctor<BodyB, Container>, bool>::execute(*it, func);
+      collision |= SingleCast<BodyTypeTuple, AnalyticSingleCollideFunctor<BodyB, Container>, bool>::execute(it.getBodyID(), func);
    }
    return collision;
 }
@@ -2146,7 +2146,7 @@ bool collide( Union<BodyTypeTupleA>* bd1, Union<BodyTypeTupleB>* bd2, Container&
    {
       for( auto it2=bd2->begin(); it2!=bd2->end(); ++it2 )
       {
-         collision |= DoubleCast<BodyTypeTupleA, BodyTypeTupleB, AnalyticCollideFunctor<Container>, bool>::execute(*it1, *it2, func);
+         collision |= DoubleCast<BodyTypeTupleA, BodyTypeTupleB, AnalyticCollideFunctor<Container>, bool>::execute(it1.getBodyID(), it2.getBodyID(), func);
       }
    }
    return collision;
diff --git a/src/pe/fcd/GJKEPACollideFunctor.h b/src/pe/fcd/GJKEPACollideFunctor.h
index 35a72fcac6b958447dbcd04a30fb25349898e1d8..051d5bcbcace3bb1b99aeb497b789ae4f7895c42 100644
--- a/src/pe/fcd/GJKEPACollideFunctor.h
+++ b/src/pe/fcd/GJKEPACollideFunctor.h
@@ -169,7 +169,7 @@ namespace gjkepa{
       bool collision = false;
       for( auto it=a->begin(); it!=a->end(); ++it )
       {
-         collision |= SingleCast<BodyTupleA, GJKEPASingleCollideFunctor<BodyB, Container>, bool>::execute(*it, func);
+         collision |= SingleCast<BodyTupleA, GJKEPASingleCollideFunctor<BodyB, Container>, bool>::execute(it.getBodyID(), func);
       }
       return collision;
    }
@@ -187,7 +187,7 @@ namespace gjkepa{
       {
          for( auto it2=b->begin(); it2!=b->end(); ++it2 )
          {
-            collision |= DoubleCast<BodyTupleA, BodyTupleB, GJKEPACollideFunctor<Container>, bool>::execute(*it1, *it2, func);
+            collision |= DoubleCast<BodyTupleA, BodyTupleB, GJKEPACollideFunctor<Container>, bool>::execute(it1.getBodyID(), it2.getBodyID(), func);
          }
       }
       return collision;
@@ -200,7 +200,7 @@ namespace gjkepa{
       bool collision = false;
       for( auto it=a->begin(); it!=a->end(); ++it )
       {
-         collision |= SingleCast<BodyTupleA, GJKEPASingleCollideFunctor<Plane, Container>, bool>::execute(*it, func);
+         collision |= SingleCast<BodyTupleA, GJKEPASingleCollideFunctor<Plane, Container>, bool>::execute(it.getBodyID(), func);
       }
       return collision;
    }
diff --git a/src/pe/rigidbody/BodyIterators.h b/src/pe/rigidbody/BodyIterators.h
index aaf7d7daf55ea5698822197817db048bdebee7a4..5f4ef40fc5bc9be40788c60af43cff52dea9972e 100644
--- a/src/pe/rigidbody/BodyIterators.h
+++ b/src/pe/rigidbody/BodyIterators.h
@@ -33,104 +33,109 @@ class BodyIterator
 {
 public:
 
-    template< typename T >
-    class iterator : public std::iterator< std::input_iterator_tag, typename T::value_type, typename T::difference_type, typename T::pointer, typename T::reference >
-    {
-        friend class BodyIterator;
-    public:
-        iterator & operator++()    { ++it_; checkStateAndAdapt(); return *this; }      // prefix ++X
-        iterator   operator++(int) { iterator it( *this ); operator++(); return it; }; // postfix X++
-
-        bool operator==( const iterator & rhs ) const
-        {
-            if (ended_ || rhs.ended_)
+   template< typename T >
+   class iterator : public std::iterator< std::input_iterator_tag, typename T::value_type, typename T::difference_type, typename T::pointer, typename T::reference >
+   {
+      friend class BodyIterator;
+   public:
+      iterator & operator++()    { ++it_; checkStateAndAdapt(); return *this; }      // prefix ++X
+      iterator   operator++(int) { iterator it( *this ); operator++(); return it; }; // postfix X++
+
+      bool operator==( const iterator & rhs ) const
+      {
+         if (ended_ || rhs.ended_)
+         {
+            if (ended_ == rhs.ended_)
             {
-               if (ended_ == rhs.ended_)
-               {
-                  return true;
-               }
-               else
-               {
-                  return false;
-               }
+               return true;
             }
-
-            return it_ == rhs.it_;
-        }
-        bool operator!=( const iterator & rhs ) const { return !(*this == rhs); }
-
-        typename T::value_type operator*()  { return *it_; }
-        typename T::pointer    operator->() { return *it_; }
-
-    private:
-
-        iterator( const T& begin,
-                  const T& localEnd,
-                  const T& shadowBegin,
-                  const T& shadowEnd )
-            : it_(begin)
-            , itLocalEnd_(localEnd)
-            , itShadowBegin_(shadowBegin)
-            , itShadowEnd_(shadowEnd)
-            , local_(true)
-            , ended_(false)
-        {
-            checkStateAndAdapt();
-        }
-
-        iterator( )
-            : ended_( true )
-        {}
-
-        void checkStateAndAdapt()
-        {
-            if( local_ && it_ == itLocalEnd_ )
+            else
             {
-               it_ = itShadowBegin_;
-               local_ = false;
+               return false;
             }
+         }
 
-            if( it_ == itShadowEnd_ )
-            {
-                ended_ = true;
-            }
-        }
-
-        T it_;
-        T itLocalEnd_;
-        T itShadowBegin_;
-        T itShadowEnd_;
-
-        bool local_;
-
-        bool ended_;
-    };
-
-    static inline iterator<pe::BodyStorage::Bodies::Iterator>         begin(      IBlock & block,
-                                                                            const BlockDataID & bodyStorageId)
-    {
-        pe::Storage * storage = block.getData< pe::Storage >( bodyStorageId );
-        return iterator<pe::BodyStorage::Bodies::Iterator> ( (*storage)[0].begin(), (*storage)[0].end(), (*storage)[1].begin(), (*storage)[1].end() );
-    }
-
-    template< typename C >
-    static inline iterator<pe::BodyStorage::Bodies::CastIterator<C> > begin(      IBlock & block,
-                                                                            const BlockDataID & bodyStorageId)
-    {
-        pe::Storage * storage = block.getData< pe::Storage >( bodyStorageId );
-        return iterator<pe::BodyStorage::Bodies::CastIterator<C> > ( (*storage)[0].begin<C>(), (*storage)[0].end<C>(), (*storage)[1].begin<C>(), (*storage)[1].end<C>() );
-    }
-
-
-    static inline iterator<pe::BodyStorage::Bodies::Iterator>             end()
-    {
-        return iterator<pe::BodyStorage::Bodies::Iterator> ( );
-    }
-    template< typename C >
-    static inline iterator<pe::BodyStorage::Bodies::CastIterator<C> >     end()
-    {
-        return iterator<pe::BodyStorage::Bodies::CastIterator<C> > ( );
-    }
+         //std::vector::iterator cannot be compared between different instances (assert!)
+         if (local_ == rhs.local_)
+            return it_ == rhs.it_;
+         else
+            return false;
+      }
+      bool operator!=( const iterator & rhs ) const { return !(*this == rhs); }
+
+      typename T::reference  operator*()  { return *it_; }
+      typename T::pointer    operator->() { return it_.operator->(); }
+      typename T::pointer    getBodyID()  { return it_.getBodyID(); }
+
+   private:
+
+      iterator( const T& begin,
+                const T& localEnd,
+                const T& shadowBegin,
+                const T& shadowEnd )
+         : it_(begin)
+         , itLocalEnd_(localEnd)
+         , itShadowBegin_(shadowBegin)
+         , itShadowEnd_(shadowEnd)
+         , local_(true)
+         , ended_(false)
+      {
+         checkStateAndAdapt();
+      }
+
+      iterator( )
+         : ended_( true )
+      {}
+
+      void checkStateAndAdapt()
+      {
+         if( local_ && it_ == itLocalEnd_ )
+         {
+            it_ = itShadowBegin_;
+            local_ = false;
+         }
+
+         if( !local_ && it_ == itShadowEnd_ )
+         {
+            ended_ = true;
+         }
+      }
+
+      T it_;
+      T itLocalEnd_;
+      T itShadowBegin_;
+      T itShadowEnd_;
+
+      bool local_; //!< still in local storage?
+
+      bool ended_;
+   };
+
+   static inline iterator<pe::BodyStorage::iterator>         begin(      IBlock & block,
+                                                                         const BlockDataID & bodyStorageId)
+   {
+      pe::Storage * storage = block.getData< pe::Storage >( bodyStorageId );
+      return iterator<pe::BodyStorage::iterator> ( (*storage)[0].begin(), (*storage)[0].end(), (*storage)[1].begin(), (*storage)[1].end() );
+   }
+
+   template< typename C >
+   static inline iterator<pe::BodyStorage::cast_iterator<C> > begin(      IBlock & block,
+                                                                          const BlockDataID & bodyStorageId)
+   {
+      pe::Storage * storage = block.getData< pe::Storage >( bodyStorageId );
+      return iterator<pe::BodyStorage::cast_iterator<C> > ( (*storage)[0].begin<C>(), (*storage)[0].end<C>(), (*storage)[1].begin<C>(), (*storage)[1].end<C>() );
+   }
+
+
+   static inline iterator<pe::BodyStorage::iterator>             end()
+   {
+      return iterator<pe::BodyStorage::iterator> ( );
+   }
+   template< typename C >
+   static inline iterator<pe::BodyStorage::cast_iterator<C> >     end()
+   {
+      return iterator<pe::BodyStorage::cast_iterator<C> > ( );
+   }
 
 }; // class BodyIterator
 
@@ -140,87 +145,88 @@ class LocalBodyIterator
 {
 public:
 
-    template< typename T >
-    class iterator : public std::iterator< std::input_iterator_tag, typename T::value_type, typename T::difference_type, typename T::pointer, typename T::reference >
-    {
-        friend class LocalBodyIterator;
-    public:
-        iterator & operator++()    { ++it_; checkStateAndAdapt(); return *this; }      // prefix ++X
-        iterator   operator++(int) { iterator it( *this ); operator++(); return it; }; // postfix X++
-
-        bool operator==( const iterator & rhs ) const
-        {
-            if (ended_ || rhs.ended_)
+   template< typename T >
+   class iterator : public std::iterator< std::input_iterator_tag, typename T::value_type, typename T::difference_type, typename T::pointer, typename T::reference >
+   {
+      friend class LocalBodyIterator;
+   public:
+      iterator & operator++()    { ++it_; checkStateAndAdapt(); return *this; }      // prefix ++X
+      iterator   operator++(int) { iterator it( *this ); operator++(); return it; }; // postfix X++
+
+      bool operator==( const iterator & rhs ) const
+      {
+         if (ended_ || rhs.ended_)
+         {
+            if (ended_ == rhs.ended_)
             {
-               if (ended_ == rhs.ended_)
-               {
-                  return true;
-               }
-               else
-               {
-                  return false;
-               }
+               return true;
             }
-
-            return it_ == rhs.it_;
-        }
-        bool operator!=( const iterator & rhs ) const { return !(*this == rhs); }
-
-        typename T::value_type operator*()  { return *it_; }
-        typename T::pointer    operator->() { return *it_; }
-
-    private:
-
-        iterator( const T& begin,
-                  const T& end )
-            : it_(begin), itEnd_(end), ended_( false )
-        {
-            checkStateAndAdapt();
-        }
-
-        iterator( )
-            : ended_( true )
-        {}
-
-        void checkStateAndAdapt()
-        {
-            if( it_ == itEnd_ )
+            else
             {
-                ended_ = true;
+               return false;
             }
-        }
-
-        T it_;
-        T itEnd_;
-
-        bool ended_;
-    };
-
-    static inline iterator<pe::BodyStorage::Bodies::Iterator>         begin(      IBlock & block,
-                                                                            const BlockDataID & bodyStorageId)
-    {
-        pe::Storage * storage = block.getData< pe::Storage >( bodyStorageId );
-        return iterator<pe::BodyStorage::Bodies::Iterator> ( (*storage)[0].begin(), (*storage)[0].end() );
-    }
-
-    template< typename C >
-    static inline iterator<pe::BodyStorage::Bodies::CastIterator<C> > begin(      IBlock & block,
-                                                                            const BlockDataID & bodyStorageId)
-    {
-        pe::Storage * storage = block.getData< pe::Storage >( bodyStorageId );
-        return iterator<pe::BodyStorage::Bodies::CastIterator<C> > ( (*storage)[0].begin<C>(), (*storage)[0].end<C>() );
-    }
-
-
-    static inline iterator<pe::BodyStorage::Bodies::Iterator>             end()
-    {
-        return iterator<pe::BodyStorage::Bodies::Iterator> ( );
-    }
-    template< typename C >
-    static inline iterator<pe::BodyStorage::Bodies::CastIterator<C> >     end()
-    {
-        return iterator<pe::BodyStorage::Bodies::CastIterator<C> > ( );
-    }
+         }
+
+         return it_ == rhs.it_;
+      }
+      bool operator!=( const iterator & rhs ) const { return !(*this == rhs); }
+
+      typename T::reference  operator*()  { return *it_; }
+      typename T::pointer    operator->() { return it_.operator->(); }
+      typename T::pointer    getBodyID()  { return it_.getBodyID(); }
+
+   private:
+
+      iterator( const T& begin,
+                const T& end )
+         : it_(begin), itEnd_(end), ended_( false )
+      {
+         checkStateAndAdapt();
+      }
+
+      iterator( )
+         : ended_( true )
+      {}
+
+      void checkStateAndAdapt()
+      {
+         if( it_ == itEnd_ )
+         {
+            ended_ = true;
+         }
+      }
+
+      T it_;
+      T itEnd_;
+
+      bool ended_;
+   };
+
+   static inline iterator<pe::BodyStorage::iterator>         begin(      IBlock & block,
+                                                                         const BlockDataID & bodyStorageId)
+   {
+      pe::BodyStorage& storage = (*block.getData< pe::Storage >( bodyStorageId ))[0];
+      return iterator<pe::BodyStorage::iterator> ( storage.begin(), storage.end() );
+   }
+
+   template< typename C >
+   static inline iterator<pe::BodyStorage::cast_iterator<C> > begin(      IBlock & block,
+                                                                          const BlockDataID & bodyStorageId)
+   {
+      pe::Storage * storage = block.getData< pe::Storage >( bodyStorageId );
+      return iterator<pe::BodyStorage::cast_iterator<C> > ( (*storage)[0].begin<C>(), (*storage)[0].end<C>() );
+   }
+
+
+   static inline iterator<pe::BodyStorage::iterator>             end()
+   {
+      return iterator<pe::BodyStorage::iterator> ( );
+   }
+   template< typename C >
+   static inline iterator<pe::BodyStorage::cast_iterator<C> >     end()
+   {
+      return iterator<pe::BodyStorage::cast_iterator<C> > ( );
+   }
 
 }; // class LocalBodyIterator
 
@@ -229,87 +235,88 @@ class ShadowBodyIterator
 {
 public:
 
-    template< typename T >
-    class iterator : public std::iterator< std::input_iterator_tag, typename T::value_type, typename T::difference_type, typename T::pointer, typename T::reference >
-    {
-        friend class ShadowBodyIterator;
-    public:
-        iterator & operator++()    { ++it_; checkStateAndAdapt(); return *this; }      // prefix ++X
-        iterator   operator++(int) { iterator it( *this ); operator++(); return it; }; // postfix X++
-
-        bool operator==( const iterator & rhs ) const
-        {
-            if (ended_ || rhs.ended_)
+   template< typename T >
+   class iterator : public std::iterator< std::input_iterator_tag, typename T::value_type, typename T::difference_type, typename T::pointer, typename T::reference >
+   {
+      friend class ShadowBodyIterator;
+   public:
+      iterator & operator++()    { ++it_; checkStateAndAdapt(); return *this; }      // prefix ++X
+      iterator   operator++(int) { iterator it( *this ); operator++(); return it; }; // postfix X++
+
+      bool operator==( const iterator & rhs ) const
+      {
+         if (ended_ || rhs.ended_)
+         {
+            if (ended_ == rhs.ended_)
             {
-               if (ended_ == rhs.ended_)
-               {
-                  return true;
-               }
-               else
-               {
-                  return false;
-               }
+               return true;
             }
-
-            return it_ == rhs.it_;
-        }
-        bool operator!=( const iterator & rhs ) const { return !(*this == rhs); }
-
-        typename T::value_type operator*()  { return *it_; }
-        typename T::pointer    operator->() { return *it_; }
-
-    private:
-
-        iterator( const T& begin,
-                  const T& end )
-            : it_(begin), itEnd_(end), ended_( false )
-        {
-            checkStateAndAdapt();
-        }
-
-        iterator( )
-            : ended_( true )
-        {}
-
-        void checkStateAndAdapt()
-        {
-            if( it_ == itEnd_ )
+            else
             {
-                ended_ = true;
+               return false;
             }
-        }
-
-        T it_;
-        T itEnd_;
-
-        bool ended_;
-    };
-
-    static inline iterator<pe::BodyStorage::Bodies::Iterator>         begin(      IBlock & block,
-                                                                            const BlockDataID & bodyStorageId)
-    {
-        pe::Storage * storage = block.getData< pe::Storage >( bodyStorageId );
-        return iterator<pe::BodyStorage::Bodies::Iterator> ( (*storage)[1].begin(), (*storage)[1].end() );
-    }
-
-    template< typename C >
-    static inline iterator<pe::BodyStorage::Bodies::CastIterator<C> > begin(      IBlock & block,
-                                                                            const BlockDataID & bodyStorageId)
-    {
-        pe::Storage * storage = block.getData< pe::Storage >( bodyStorageId );
-        return iterator<pe::BodyStorage::Bodies::CastIterator<C> > ( (*storage)[1].begin<C>(), (*storage)[1].end<C>() );
-    }
-
-
-    static inline iterator<pe::BodyStorage::Bodies::Iterator>             end()
-    {
-        return iterator<pe::BodyStorage::Bodies::Iterator> ( );
-    }
-    template< typename C >
-    static inline iterator<pe::BodyStorage::Bodies::CastIterator<C> >     end()
-    {
-        return iterator<pe::BodyStorage::Bodies::CastIterator<C> > ( );
-    }
+         }
+
+         return it_ == rhs.it_;
+      }
+      bool operator!=( const iterator & rhs ) const { return !(*this == rhs); }
+
+      typename T::reference  operator*()  { return *it_; }
+      typename T::pointer    operator->() { return it_.operator->(); }
+      typename T::pointer    getBodyID()  { return it_.getBodyID(); }
+
+   private:
+
+      iterator( const T& begin,
+                const T& end )
+         : it_(begin), itEnd_(end), ended_( false )
+      {
+         checkStateAndAdapt();
+      }
+
+      iterator( )
+         : ended_( true )
+      {}
+
+      void checkStateAndAdapt()
+      {
+         if( it_ == itEnd_ )
+         {
+            ended_ = true;
+         }
+      }
+
+      T it_;
+      T itEnd_;
+
+      bool ended_;
+   };
+
+   static inline iterator<pe::BodyStorage::iterator>         begin(      IBlock & block,
+                                                                         const BlockDataID & bodyStorageId)
+   {
+      pe::Storage * storage = block.getData< pe::Storage >( bodyStorageId );
+      return iterator<pe::BodyStorage::iterator> ( (*storage)[1].begin(), (*storage)[1].end() );
+   }
+
+   template< typename C >
+   static inline iterator<pe::BodyStorage::cast_iterator<C> > begin(      IBlock & block,
+                                                                          const BlockDataID & bodyStorageId)
+   {
+      pe::Storage * storage = block.getData< pe::Storage >( bodyStorageId );
+      return iterator<pe::BodyStorage::cast_iterator<C> > ( (*storage)[1].begin<C>(), (*storage)[1].end<C>() );
+   }
+
+
+   static inline iterator<pe::BodyStorage::iterator>             end()
+   {
+      return iterator<pe::BodyStorage::iterator> ( );
+   }
+   template< typename C >
+   static inline iterator<pe::BodyStorage::cast_iterator<C> >     end()
+   {
+      return iterator<pe::BodyStorage::cast_iterator<C> > ( );
+   }
 
 }; // class ShadowBodyIterator
 
diff --git a/src/pe/rigidbody/BodyStorage.h b/src/pe/rigidbody/BodyStorage.h
index 0ab0ef40b58ab71e2712d2de3132e2ad388baa66..7e4475e9a7ab8dfd8e19730ae27ee141391a15f3 100644
--- a/src/pe/rigidbody/BodyStorage.h
+++ b/src/pe/rigidbody/BodyStorage.h
@@ -26,25 +26,22 @@
 // Includes
 //*************************************************************************************************
 
-#include <algorithm>
-#include <functional>
-#include <map>
-#include <vector>
 #include <core/NonCopyable.h>
 #include <core/debug/Debug.h>
-#include <core/ptrvector/policies/PtrDelete.h>
-#include <core/ptrvector/PtrVector.h>
 #include <pe/rigidbody/RigidBody.h>
+#include <pe/rigidbody/RigidBodyCastIterator.h>
+#include <pe/rigidbody/RigidBodyIterator.h>
 #include <pe/Types.h>
 
+#include <algorithm>
 #include <functional>
+#include <map>
+#include <memory>
+#include <vector>
 
 namespace walberla {
 namespace pe {
 
-
-
-
 //=================================================================================================
 //
 //  CLASS DEFINITION
@@ -62,13 +59,18 @@ class BodyStorage : private NonCopyable
 public:
    //**Type definitions****************************************************************************
    //! Container for the bodies contained in the simulation world.
-   typedef PtrVector<BodyType, PtrDelete>  Bodies;
+   using VectorContainer = std::vector< std::unique_ptr<RigidBody> >;
+   using ConstVectorContainer = std::vector< std::unique_ptr<const RigidBody> >;
    //**********************************************************************************************
 
    //**Type definitions****************************************************************************
-   typedef Bodies::SizeType       SizeType;       //!< Size type of the body storage.
-   typedef Bodies::Iterator       Iterator;       //!< Iterator over non-const bodies.
-   typedef Bodies::ConstIterator  ConstIterator;  //!< Iterator over constant bodies.
+   using size_type             = VectorContainer::size_type;           //!< Size type of the body storage.
+   using iterator              = RigidBodyIterator;                    //!< Iterator over non-const bodies.
+   using const_iterator        = ConstRigidBodyIterator;               //!< Iterator over constant bodies.
+   template <typename C> //cast type
+   using cast_iterator         = RigidBodyCastIterator<C>;
+   template <typename C> //cast type
+   using const_cast_iterator   = ConstRigidBodyCastIterator<C>;
    //**********************************************************************************************
 
    //**Constructors********************************************************************************
@@ -88,45 +90,58 @@ public:
    //**Utility functions***************************************************************************
    /*!\name Utility functions */
    //@{
-   inline bool          isEmpty () const;
-   inline SizeType      size    () const;
-   template< typename T >
-   inline SizeType      size    () const;
-   inline Iterator      begin   ();
-   inline ConstIterator begin   () const;
-   template< typename T >
-   inline typename Bodies::template CastIterator<T> begin();
-   template< typename T >
-   inline typename Bodies::template ConstCastIterator<T> begin() const;
-   inline Iterator      end     ();
-   inline ConstIterator end     () const;
-   template< typename T >
-   inline typename Bodies::template CastIterator<T> end();
-   template< typename T >
-   inline typename Bodies::template ConstCastIterator<T> end() const;
-   inline BodyID        at      ( SizeType index );
-   inline ConstBodyID   at      ( SizeType index ) const;
-   inline Iterator      find    ( id_t sid );
-   inline ConstIterator find    ( id_t sid ) const;
-   inline Iterator      find    ( ConstBodyID body );
-   inline ConstIterator find    ( ConstBodyID body ) const;
-   inline void          validate();
+   inline bool           isEmpty () const;
+   inline size_type      size    () const;
+
+   inline iterator       begin   ();
+   inline const_iterator begin   () const;
+   inline const_iterator cbegin  () const;
+   inline iterator       end     ();
+   inline const_iterator end     () const;
+   inline const_iterator cend    () const;
+
+   template< typename C >
+   inline cast_iterator<C> begin();
+   template< typename C >
+   inline const_cast_iterator<C> begin() const;
+   template< typename C >
+   inline const_cast_iterator<C> cbegin() const;
+   template< typename C >
+   inline cast_iterator<C> end();
+   template< typename C >
+   inline const_cast_iterator<C> end() const;
+   template< typename C >
+   inline const_cast_iterator<C> cend() const;
+
+   inline RigidBody&         front();
+   inline const RigidBody&   front() const;
+   inline RigidBody&         back();
+   inline const RigidBody&   back() const;
+
+   inline BodyID         at      ( size_type index );
+   inline ConstBodyID    at      ( size_type index ) const;
+   inline iterator       find    ( id_t sid );
+   inline const_iterator find    ( id_t sid ) const;
+   inline iterator       find    ( ConstBodyID body );
+   inline const_iterator find    ( ConstBodyID body ) const;
+   inline void           validate();
    //@}
    //**********************************************************************************************
 
    //**Add/Remove functions************************************************************************
    /*!\name Add/Remove functions */
    //@{
-   inline void          add     ( BodyID body );
-   inline void          remove  ( const id_t sid );
-   inline void          remove  ( BodyID body );
-   inline ConstIterator remove  ( ConstIterator pos );
-   inline Iterator      remove  ( Iterator pos );
-   inline void          release ( const id_t sid );
-   inline void          release ( BodyID body );
-   inline ConstIterator release  ( ConstIterator pos );
-   inline Iterator      release  ( Iterator pos );
-   inline void          clear   ();
+   [[deprecated]] inline RigidBody&     add     ( BodyID body );
+   inline RigidBody&     add     ( std::unique_ptr<RigidBody>&& body );
+   inline iterator       remove  ( const id_t sid );
+   inline iterator       remove  ( BodyID body );
+   inline const_iterator remove  ( const_iterator pos );
+   inline iterator       remove  ( iterator pos );
+   inline std::unique_ptr<RigidBody> release ( const id_t sid );
+   inline std::unique_ptr<RigidBody> release ( BodyID body );
+   inline std::unique_ptr<RigidBody> release ( const_iterator& pos );
+   inline std::unique_ptr<RigidBody> release ( iterator& pos );
+   inline void           clear   ();
    //@}
    //**********************************************************************************************
 
@@ -147,8 +162,8 @@ private:
    //**Member variables****************************************************************************
    /*!\name Member variables */
    //@{
-   Bodies bodies_;  //!< The rigid bodies contained in the simulation world.
-   std::map<id_t, SizeType> bodyIDs_;   //!< The association of system IDs to rigid bodies.
+   VectorContainer           bodies_;    //!< The rigid bodies contained in the simulation world.
+   std::map<id_t, size_type> bodyIDs_;   //!< The association of system IDs to rigid bodies.
 
    std::map< std::string, std::function<void (BodyID)> > addCallbacks_;
    std::map< std::string, std::function<void (BodyID)> > removeCallbacks_;
@@ -170,9 +185,9 @@ private:
 /*!\brief The standard constructor.
  */
 inline BodyStorage::BodyStorage()
-	: bodies_( 1000 )
-	, bodyIDs_()
+   : bodyIDs_()
 {
+   bodies_.reserve(1000);
 }
 //*************************************************************************************************
 
@@ -195,7 +210,7 @@ inline BodyStorage::~BodyStorage()
 {
    WALBERLA_ASSERT_EQUAL(addCallbacks_.size(), 0, "Still add callbacks registered!");
    WALBERLA_ASSERT_EQUAL(removeCallbacks_.size(), 0, "Still remove callbacks registered!");
-	clear();
+   clear();
 }
 //*************************************************************************************************
 
@@ -216,7 +231,7 @@ inline BodyStorage::~BodyStorage()
 
 inline bool BodyStorage::isEmpty() const
 {
-   return bodies_.isEmpty();
+   return bodies_.empty();
 }
 //*************************************************************************************************
 
@@ -227,7 +242,7 @@ inline bool BodyStorage::isEmpty() const
  * \return The number of rigid bodies.
  */
 
-inline BodyStorage::SizeType BodyStorage::size() const
+inline BodyStorage::size_type BodyStorage::size() const
 {
    return bodies_.size();
 }
@@ -235,33 +250,27 @@ inline BodyStorage::SizeType BodyStorage::size() const
 
 
 //*************************************************************************************************
-/*!\brief Returns the number of rigid bodies of type \a T contained in the body storage.
- *
- * \return The number of rigid bodies of type \a T.
+/*!\brief Returns an iterator to the first contained rigid body.
  *
- * \b Note: The total number of objects of type \a T is not cached but recalculated each time the
- * function is called. Using the templated version of size() to calculate the total number objects
- * of type \a T is therefore more expensive than using the non-template version of size() to get
- * the total number of pointers in the vector!
+ * \return Iterator to the first contained rigid body.
  */
 
-template< typename T >  // Cast type
-inline BodyStorage::SizeType BodyStorage::size() const
+inline BodyStorage::iterator BodyStorage::begin()
 {
-   return bodies_.template size<T>();
+   return BodyStorage::iterator(bodies_.begin());
 }
 //*************************************************************************************************
 
 
 //*************************************************************************************************
-/*!\brief Returns an iterator to the first contained rigid body.
+/*!\brief Returns a constant iterator to the first contained rigid body.
  *
- * \return Iterator to the first contained rigid body.
+ * \return Constant iterator to the first contained rigid body.
  */
 
-inline BodyStorage::Iterator BodyStorage::begin()
+inline BodyStorage::const_iterator BodyStorage::begin() const
 {
-   return bodies_.begin();
+   return cbegin();
 }
 //*************************************************************************************************
 
@@ -272,9 +281,48 @@ inline BodyStorage::Iterator BodyStorage::begin()
  * \return Constant iterator to the first contained rigid body.
  */
 
-inline BodyStorage::ConstIterator BodyStorage::begin() const
+inline BodyStorage::const_iterator BodyStorage::cbegin() const
+{
+   return BodyStorage::const_iterator(bodies_.cbegin());
+}
+//*************************************************************************************************
+
+
+//*************************************************************************************************
+/*!\brief Returns an iterator just past the last contained rigid body.
+ *
+ * \return Iterator just past the last contained rigid body.
+ */
+
+inline BodyStorage::iterator BodyStorage::end()
+{
+   return BodyStorage::iterator(bodies_.end());
+}
+//*************************************************************************************************
+
+
+//*************************************************************************************************
+/*!\brief Returns a constant iterator just past the last contained rigid body.
+ *
+ * \return Constant iterator just past the last contained rigid body.
+ */
+
+inline BodyStorage::const_iterator BodyStorage::end() const
+{
+   return cend();
+}
+//*************************************************************************************************
+
+
+//*************************************************************************************************
+/*!\brief Returns a constant iterator just past the last contained rigid body.
+ *
+ * \return Constant iterator just past the last contained rigid body.
+ */
+
+inline BodyStorage::const_iterator BodyStorage::cend() const
 {
-   return bodies_.begin();
+   return BodyStorage::const_iterator(bodies_.cend());
 }
 //*************************************************************************************************
 
@@ -285,10 +333,10 @@ inline BodyStorage::ConstIterator BodyStorage::begin() const
  * \return Iterator to the first contained rigid body.
  */
 
-template< typename T >  // Cast Type
-inline BodyStorage::Bodies::template CastIterator<T> BodyStorage::begin()
+template< typename C >  // Cast Type
+inline BodyStorage::cast_iterator<C> BodyStorage::begin()
 {
-   return bodies_.template begin<T>();
+   return BodyStorage::cast_iterator<C>(bodies_.begin(), bodies_.end());
 }
 //*************************************************************************************************
 
@@ -299,50 +347,52 @@ inline BodyStorage::Bodies::template CastIterator<T> BodyStorage::begin()
  * \return Constant iterator to the first contained rigid body.
  */
 
-template< typename T >  // Cast Type
-inline BodyStorage::Bodies::template ConstCastIterator<T> BodyStorage::begin() const
+template< typename C >  // Cast Type
+inline BodyStorage::const_cast_iterator<C> BodyStorage::begin() const
 {
-   return bodies_.template begin<T>();
+   return cbegin();
 }
 //*************************************************************************************************
 
 
 //*************************************************************************************************
-/*!\brief Returns an iterator just past the last contained rigid body.
+/*!\brief Returns a constant iterator to the first contained rigid body.
  *
- * \return Iterator just past the last contained rigid body.
+ * \return Constant iterator to the first contained rigid body.
  */
 
-inline BodyStorage::Iterator BodyStorage::end()
+template< typename C >  // Cast Type
+inline BodyStorage::const_cast_iterator<C> BodyStorage::cbegin() const
 {
-   return bodies_.end();
+   return BodyStorage::const_cast_iterator<C>(bodies_.begin(), bodies_.end());
 }
 //*************************************************************************************************
 
 
 //*************************************************************************************************
-/*!\brief Returns a constant iterator just past the last contained rigid body.
+/*!\brief Returns an iterator just past the last contained rigid body.
  *
- * \return Constant iterator just past the last contained rigid body.
+ * \return Iterator just past the last contained rigid body.
  */
 
-inline BodyStorage::ConstIterator BodyStorage::end() const
+template< typename C >  // Cast Type
+inline BodyStorage::cast_iterator<C> BodyStorage::end()
 {
-   return bodies_.end();
+   return BodyStorage::cast_iterator<C>(bodies_.end(), bodies_.end());
 }
 //*************************************************************************************************
 
 
 //*************************************************************************************************
-/*!\brief Returns an iterator just past the last contained rigid body.
+/*!\brief Returns a constant iterator just past the last contained rigid body.
  *
- * \return Iterator just past the last contained rigid body.
+ * \return Constant iterator just past the last contained rigid body.
  */
 
-template< typename T >  // Cast Type
-inline BodyStorage::Bodies::template CastIterator<T> BodyStorage::end()
+template< typename C >  // Cast Type
+inline BodyStorage::const_cast_iterator<C> BodyStorage::end() const
 {
-   return bodies_.template end<T>();
+   return cend();
 }
 //*************************************************************************************************
 
@@ -353,13 +403,30 @@ inline BodyStorage::Bodies::template CastIterator<T> BodyStorage::end()
  * \return Constant iterator just past the last contained rigid body.
  */
 
-template< typename T >  // Cast Type
-inline BodyStorage::Bodies::template ConstCastIterator<T> BodyStorage::end() const
+template< typename C >  // Cast Type
+inline BodyStorage::const_cast_iterator<C> BodyStorage::cend() const
 {
-   return bodies_.template end<T>();
+   return BodyStorage::const_cast_iterator<C>(bodies_.end(), bodies_.end());
 }
 //*************************************************************************************************
 
+inline RigidBody&         BodyStorage::front()
+{
+   return *bodies_.front();
+}
+inline const RigidBody&   BodyStorage::front() const
+{
+   return *bodies_.front();
+}
+inline RigidBody&         BodyStorage::back()
+{
+   return *bodies_.back();
+}
+inline const RigidBody&   BodyStorage::back() const
+{
+   return *bodies_.back();
+}
+
 
 //*************************************************************************************************
 /*!\brief Returns a handle to the indexed rigid body.
@@ -370,10 +437,10 @@ inline BodyStorage::Bodies::template ConstCastIterator<T> BodyStorage::end() con
  * \b Note: No runtime check is performed to ensure the validity of the access index.
  */
 
-inline BodyID BodyStorage::at( SizeType index )
+inline BodyID BodyStorage::at( size_type index )
 {
    WALBERLA_ASSERT( index < size(), "Invalid body index" );
-   return bodies_[index];
+   return bodies_[index].get();
 }
 //*************************************************************************************************
 
@@ -387,10 +454,10 @@ inline BodyID BodyStorage::at( SizeType index )
  * \b Note: No runtime check is performed to ensure the validity of the access index.
  */
 
-inline ConstBodyID BodyStorage::at( SizeType index ) const
+inline ConstBodyID BodyStorage::at( size_type index ) const
 {
    WALBERLA_ASSERT( index < size(), "Invalid body index" );
-   return bodies_[index];
+   return bodies_[index].get();
 }
 //*************************************************************************************************
 
@@ -406,13 +473,13 @@ inline ConstBodyID BodyStorage::at( SizeType index ) const
  * iterator just past the end of the last body contained in the body storage.
  */
 
-inline BodyStorage::Iterator BodyStorage::find( id_t sid )
+inline BodyStorage::iterator BodyStorage::find( id_t sid )
 {
-   std::map<id_t, SizeType>::const_iterator pos = bodyIDs_.find( sid );
+   std::map<id_t, size_type>::const_iterator pos = bodyIDs_.find( sid );
    if( pos == bodyIDs_.end() )
-      return bodies_.end();
+      return BodyStorage::iterator(bodies_.end());
 
-   return bodies_.begin() + static_cast<Bodies::Iterator::difference_type>(pos->second);
+   return BodyStorage::iterator(bodies_.begin() + static_cast<VectorContainer::iterator::difference_type>(pos->second));
 }
 //*************************************************************************************************
 
@@ -428,13 +495,13 @@ inline BodyStorage::Iterator BodyStorage::find( id_t sid )
  * a constant iterator just past the end of the last body contained in the body storage.
  */
 
-inline BodyStorage::ConstIterator BodyStorage::find( id_t sid ) const
+inline BodyStorage::const_iterator BodyStorage::find( id_t sid ) const
 {
-   std::map<id_t, SizeType>::const_iterator pos = bodyIDs_.find( sid );
+   std::map<id_t, size_type>::const_iterator pos = bodyIDs_.find( sid );
    if( pos == bodyIDs_.end() )
-      return bodies_.end();
+      return BodyStorage::const_iterator(bodies_.end());
 
-   return bodies_.begin() + static_cast<Bodies::Iterator::difference_type>(pos->second);
+   return BodyStorage::const_iterator(bodies_.begin() + static_cast<VectorContainer::iterator::difference_type>(pos->second));
 }
 //*************************************************************************************************
 
@@ -450,9 +517,9 @@ inline BodyStorage::ConstIterator BodyStorage::find( id_t sid ) const
  * just past the end of the last body contained in the body storage.
  */
 
-inline BodyStorage::Iterator BodyStorage::find( ConstBodyID body )
+inline BodyStorage::iterator BodyStorage::find( ConstBodyID body )
 {
-   return find( body->getSystemID() );
+   return BodyStorage::iterator(find( body->getSystemID() ));
 }
 //*************************************************************************************************
 
@@ -468,9 +535,9 @@ inline BodyStorage::Iterator BodyStorage::find( ConstBodyID body )
  * constant iterator just past the end of the last body contained in the body storage.
  */
 
-inline BodyStorage::ConstIterator BodyStorage::find( ConstBodyID body ) const
+inline BodyStorage::const_iterator BodyStorage::find( ConstBodyID body ) const
 {
-   return find( body->getSystemID() );
+   return BodyStorage::const_iterator(find( body->getSystemID() ));
 }
 //*************************************************************************************************
 
@@ -494,16 +561,45 @@ inline BodyStorage::ConstIterator BodyStorage::find( ConstBodyID body ) const
  * logarithmic unless reallocation occurs.
  */
 
-inline void BodyStorage::add( BodyID body )
+inline RigidBody& BodyStorage::add( BodyID body )
 {
    WALBERLA_ASSERT( bodyIDs_.find( body->getSystemID() ) == bodyIDs_.end(), "Body with same system ID already added." );
    bodyIDs_[ body->getSystemID() ] = bodies_.size();
-   bodies_.pushBack( body );
+   bodies_.push_back( std::unique_ptr<RigidBody>(body) );
 
    for (auto it = addCallbacks_.begin(); it != addCallbacks_.end(); ++it)
    {
-      it->second(body);
+      it->second(bodies_.back().get());
    }
+
+   return *bodies_.back();
+}
+//*************************************************************************************************
+
+
+//*************************************************************************************************
+/*!\brief Adding a rigid body to the body storage.
+ *
+ * \param body The new rigid body to be added to the body storage.
+ * \return void
+ *
+ * This function adds a rigid body to the body storage. Adding bodies with non-unique system ID or
+ * adding the same body multiple times results in undefined behaviour. The time complexity is
+ * logarithmic unless reallocation occurs.
+ */
+
+inline RigidBody& BodyStorage::add( std::unique_ptr<RigidBody>&& body )
+{
+   WALBERLA_ASSERT( bodyIDs_.find( body->getSystemID() ) == bodyIDs_.end(), "Body with same system ID already added." );
+   bodyIDs_[ body->getSystemID() ] = bodies_.size();
+   bodies_.push_back( std::move(body) );
+
+   for (auto it = addCallbacks_.begin(); it != addCallbacks_.end(); ++it)
+   {
+      it->second(bodies_.back().get());
+   }
+
+   return *bodies_.back();
 }
 //*************************************************************************************************
 
@@ -520,23 +616,25 @@ inline void BodyStorage::add( BodyID body )
  * The last element is swapped to the actual position and the length is reduced by one.
  */
 
-inline void BodyStorage::remove( const id_t sid )
+inline
+BodyStorage::iterator BodyStorage::remove( const id_t sid )
 {
-   std::map<id_t, SizeType>::iterator it = bodyIDs_.find( sid );
+   std::map<id_t, size_type>::iterator it = bodyIDs_.find( sid );
    WALBERLA_ASSERT( it != bodyIDs_.end(), "The body's system ID was not registered." );
 
    // Move last element to deleted place and update the system ID to index mapping.
-   SizeType i = it->second;
+   size_type i = it->second;
 
    for (auto cb = removeCallbacks_.begin(); cb != removeCallbacks_.end(); ++cb)
    {
-      cb->second( bodies_[i] );
+      cb->second( bodies_[i].get() );
    }
 
    bodyIDs_[ bodies_.back()->getSystemID() ] = i;
    std::swap( bodies_[i], bodies_.back() );
    bodyIDs_.erase( it );
-   bodies_.popBack();
+   bodies_.pop_back();
+   return iterator(bodies_.begin() + int_c(i));
 }
 //*************************************************************************************************
 
@@ -552,10 +650,9 @@ inline void BodyStorage::remove( const id_t sid )
  * the element to be removed. The time complexity is logarithmic unless reallocation occurs.
  */
 
-inline BodyStorage::ConstIterator BodyStorage::remove( ConstIterator pos )
+inline BodyStorage::const_iterator BodyStorage::remove( const_iterator pos )
 {
-   remove( (*pos)->getSystemID() );
-   return pos;
+   return remove( pos->getSystemID() );
 }
 //*************************************************************************************************
 
@@ -571,10 +668,9 @@ inline BodyStorage::ConstIterator BodyStorage::remove( ConstIterator pos )
  * the element to be removed. The time complexity is logarithmic unless reallocation occurs.
  */
 
-inline BodyStorage::Iterator BodyStorage::remove( Iterator pos )
+inline BodyStorage::iterator BodyStorage::remove( iterator pos )
 {
-   remove( (*pos)->getSystemID() );
-   return pos;
+   return remove( pos->getSystemID() );
 }
 //*************************************************************************************************
 
@@ -590,9 +686,10 @@ inline BodyStorage::Iterator BodyStorage::remove( Iterator pos )
  * the element to be removed. The time complexity is logarithmic unless reallocation occurs.
  */
 
-inline void BodyStorage::remove( BodyID body )
+inline
+BodyStorage::iterator BodyStorage::remove( BodyID body )
 {
-   remove( body->getSystemID() );
+   return remove( body->getSystemID() );
 }
 //*************************************************************************************************
 
@@ -609,23 +706,22 @@ inline void BodyStorage::remove( BodyID body )
  * Last element is swapped to the actual position and length is reduced by 1.
  */
 
-inline void BodyStorage::release( const id_t sid )
+inline std::unique_ptr<RigidBody> BodyStorage::release( const id_t sid )
 {
-   std::map<id_t, SizeType>::iterator it = bodyIDs_.find( sid );
+   std::map<id_t, size_type>::iterator it = bodyIDs_.find( sid );
    WALBERLA_ASSERT( it != bodyIDs_.end(), "The body's system ID was not registered." );
 
    // Move last element to deleted place and update the system ID to index mapping.
-   SizeType i = it->second;
+   size_type i = it->second;
 
-   for (auto cb = removeCallbacks_.begin(); cb != removeCallbacks_.end(); ++cb)
-   {
-      cb->second( bodies_[i] );
-   }
+   std::for_each(removeCallbacks_.begin(), removeCallbacks_.end(), [&](auto& cb){cb.second( bodies_[i].get() );});
 
    bodyIDs_[ bodies_.back()->getSystemID() ] = i;
    std::swap( bodies_[i], bodies_.back() );
    bodyIDs_.erase( it );
-   bodies_.releaseBack();
+   auto tmp = std::move(bodies_.back());
+   bodies_.pop_back();
+   return tmp;
 }
 //*************************************************************************************************
 
@@ -641,10 +737,12 @@ inline void BodyStorage::release( const id_t sid )
  * the element to be released. The time complexity is logarithmic unless reallocation occurs.
  */
 
-inline BodyStorage::ConstIterator BodyStorage::release( ConstIterator pos )
+inline std::unique_ptr<RigidBody> BodyStorage::release( const_iterator& pos )
 {
-   release( (*pos)->getSystemID() );
-   return pos;
+   auto diff = std::distance(bodies_.cbegin(), pos.get());
+   auto tmp = release( pos->getSystemID() );
+   pos = const_iterator(bodies_.begin() + diff);
+   return tmp;
 }
 //*************************************************************************************************
 
@@ -661,10 +759,12 @@ inline BodyStorage::ConstIterator BodyStorage::release( ConstIterator pos )
  * the element to be released. The time complexity is logarithmic unless reallocation occurs.
  */
 
-inline BodyStorage::Iterator BodyStorage::release( Iterator pos )
+inline std::unique_ptr<RigidBody> BodyStorage::release( iterator& pos )
 {
-   release( (*pos)->getSystemID() );
-   return pos;
+   auto diff = std::distance(bodies_.begin(), pos.get());
+   auto tmp = release( pos->getSystemID() );
+   pos = iterator(bodies_.begin() + diff);
+   return tmp;
 }
 //*************************************************************************************************
 
@@ -681,9 +781,9 @@ inline BodyStorage::Iterator BodyStorage::release( Iterator pos )
  * the element to be released. The time complexity is logarithmic unless reallocation occurs.
  */
 
-inline void BodyStorage::release( BodyID body )
+inline std::unique_ptr<RigidBody> BodyStorage::release( BodyID body )
 {
-   release( body->getSystemID() );
+   return release( body->getSystemID() );
 }
 //*************************************************************************************************
 
@@ -704,7 +804,7 @@ inline void BodyStorage::clear()
    {
       for (auto cb = removeCallbacks_.begin(); cb != removeCallbacks_.end(); ++cb)
       {
-         cb->second( *bodyIt );
+         cb->second( bodyIt->get() );
       }
    }
 
@@ -766,7 +866,7 @@ inline void          BodyStorage::clearRemoveCallbacks       ( )
 inline void BodyStorage::validate()
 {
    std::vector<bool> tmp(bodies_.size());
-   std::map<id_t, SizeType>::iterator it = bodyIDs_.begin();
+   std::map<id_t, size_type>::iterator it = bodyIDs_.begin();
    while( it != bodyIDs_.end() ) {
       WALBERLA_ASSERT(tmp[it->second] == false, "Two system IDs point to the same storage index.");
       tmp[it->second] = true;
diff --git a/src/pe/rigidbody/BoxFactory.cpp b/src/pe/rigidbody/BoxFactory.cpp
index 6473eaa18e60f971b71d31614cdd3cb13b82889e..6f471ba55ee6fbf1a9838eef6b2da5ec91255958 100644
--- a/src/pe/rigidbody/BoxFactory.cpp
+++ b/src/pe/rigidbody/BoxFactory.cpp
@@ -26,6 +26,9 @@
 #include "pe/rigidbody/BodyStorage.h"
 #include "pe/rigidbody/Box.h"
 
+#include <core/logging/Logging.h>
+#include <core/UniqueID.h>
+
 namespace walberla {
 namespace pe {
 
@@ -40,32 +43,31 @@ BoxID createBox(       BodyStorage& globalStorage, BlockStorage& blocks, BlockDa
    if( lengths[0] <= real_t(0) || lengths[1] <= real_t(0) || lengths[2] <= real_t(0) )
       throw std::invalid_argument( "Invalid side length" );
 
-   BoxID box = NULL;
+   BoxID box = nullptr;
 
    if (global)
    {
       const id_t sid = UniqueID<RigidBody>::createGlobal();
       WALBERLA_ASSERT_EQUAL(communicating, false);
       WALBERLA_ASSERT_EQUAL(infiniteMass, true);
-      box = new Box(sid, uid, gpos, Vec3(0,0,0), Quat(), lengths, material, global, false, true);
-      globalStorage.add(box);
+      BoxPtr bx = std::make_unique<Box>(sid, uid, gpos, Vec3(0,0,0), Quat(), lengths, material, global, false, true);
+      box = static_cast<BoxID>(&globalStorage.add(std::move(bx)));
    } else
    {
-      for (auto it = blocks.begin(); it != blocks.end(); ++it){
-         IBlock* block = (&(*it));
-         if (block->getAABB().contains(gpos))
+      for (auto& block : blocks){
+         if (block.getAABB().contains(gpos))
          {
             const id_t sid( UniqueID<RigidBody>::create() );
 
-            Storage* bs = block->getData<Storage>(storageID);
-            box = new Box(sid, uid, gpos, Vec3(0,0,0), Quat(), lengths, material, global, communicating, infiniteMass);
-            box->MPITrait.setOwner(Owner(MPIManager::instance()->rank(), block->getId().getID()));
-            (*bs)[0].add(box);
+            BodyStorage& bs = (*block.getData<Storage>(storageID))[0];
+            BoxPtr bx = std::make_unique<Box>(sid, uid, gpos, Vec3(0,0,0), Quat(), lengths, material, global, communicating, infiniteMass);
+            bx->MPITrait.setOwner(Owner(MPIManager::instance()->rank(), block.getId().getID()));
+            box = static_cast<BoxID>(&bs.add(std::move(bx)));
          }
       }
    }
 
-   if (box != NULL)
+   if (box != nullptr)
    {
       // Logging the successful creation of the box
       WALBERLA_LOG_DETAIL(
diff --git a/src/pe/rigidbody/CapsuleFactory.cpp b/src/pe/rigidbody/CapsuleFactory.cpp
index 44b7839d11d8587d6415f9d0d52d3c885a4e6b23..325af9701f1c78bcf1497f2717fa586372d3e80b 100644
--- a/src/pe/rigidbody/CapsuleFactory.cpp
+++ b/src/pe/rigidbody/CapsuleFactory.cpp
@@ -26,6 +26,9 @@
 #include "pe/rigidbody/BodyStorage.h"
 #include "pe/rigidbody/Capsule.h"
 
+#include <core/logging/Logging.h>
+#include <core/UniqueID.h>
+
 namespace walberla {
 namespace pe {
 
@@ -40,32 +43,31 @@ CapsuleID createCapsule(   BodyStorage& globalStorage, BlockStorage& blocks, Blo
    WALBERLA_ASSERT_GREATER( radius, real_t(0), "Invalid capsule radius" );
    WALBERLA_ASSERT_GREATER( length, real_t(0), "Invalid capsule length" );
 
-   CapsuleID capsule = NULL;
+   CapsuleID capsule = nullptr;
 
    if (global)
    {
       const id_t sid = UniqueID<RigidBody>::createGlobal();
       WALBERLA_ASSERT_EQUAL(communicating, false);
       WALBERLA_ASSERT_EQUAL(infiniteMass, true);
-      capsule = new Capsule(sid, uid, gpos, Vec3(0,0,0), Quat(), radius, length, material, global, false, true);
-      globalStorage.add(capsule);
+      CapsulePtr cp = std::make_unique<Capsule>(sid, uid, gpos, Vec3(0,0,0), Quat(), radius, length, material, global, false, true);
+      capsule = static_cast<CapsuleID>(&globalStorage.add(std::move(cp)));
    } else
    {
-      for (auto it = blocks.begin(); it != blocks.end(); ++it){
-         IBlock* block = (&(*it));
-         if (block->getAABB().contains(gpos))
+      for (auto& block : blocks){
+         if (block.getAABB().contains(gpos))
          {
             const id_t sid( UniqueID<RigidBody>::create() );
 
-            Storage* bs = block->getData<Storage>(storageID);
-            capsule = new Capsule(sid, uid, gpos, Vec3(0,0,0), Quat(), radius, length, material, global, communicating, infiniteMass);
-            capsule->MPITrait.setOwner(Owner(MPIManager::instance()->rank(), block->getId().getID()));
-            (*bs)[0].add(capsule);
+            BodyStorage& bs = (*block.getData<Storage>(storageID))[0];
+            CapsulePtr cp = std::make_unique<Capsule>(sid, uid, gpos, Vec3(0,0,0), Quat(), radius, length, material, global, communicating, infiniteMass);
+            cp->MPITrait.setOwner(Owner(MPIManager::instance()->rank(), block.getId().getID()));
+            capsule = static_cast<CapsuleID>(&bs.add( std::move(cp) ));
          }
       }
    }
 
-   if (capsule != NULL)
+   if (capsule != nullptr)
    {
       // Logging the successful creation of the box
       WALBERLA_LOG_DETAIL(
diff --git a/src/pe/rigidbody/CylindricalBoundaryFactory.cpp b/src/pe/rigidbody/CylindricalBoundaryFactory.cpp
index a82ea0c684ad7dbc7f60c2d02b99fb7eb6ded2f2..6cbf752cb9bb0c29f576e34c7ce336b2077ec4b1 100644
--- a/src/pe/rigidbody/CylindricalBoundaryFactory.cpp
+++ b/src/pe/rigidbody/CylindricalBoundaryFactory.cpp
@@ -24,7 +24,8 @@
 #include "pe/rigidbody/BodyStorage.h"
 #include "pe/rigidbody/CylindricalBoundary.h"
 
-#include "core/logging/Logging.h"
+#include <core/logging/Logging.h>
+#include <core/UniqueID.h>
 
 namespace walberla {
 namespace pe {
@@ -37,9 +38,9 @@ CylindricalBoundaryID createCylindricalBoundary( BodyStorage& globalStorage,
 
    const id_t sid( UniqueID<RigidBody>::createGlobal() );
 
-   CylindricalBoundaryID cb = new CylindricalBoundary( sid, uid, gpos, radius, material );
+   CylindricalBoundaryPtr cbPtr = std::make_unique<CylindricalBoundary>( sid, uid, gpos, radius, material );
 
-   globalStorage.add(cb);
+   CylindricalBoundaryID cb = static_cast<CylindricalBoundaryID>(&globalStorage.add(std::move(cbPtr)));
 
    // Logging the successful creation of the plane
    WALBERLA_LOG_DETAIL( "Created cylindrical boundary " << sid << "\n" << cb);
diff --git a/src/pe/rigidbody/EllipsoidFactory.cpp b/src/pe/rigidbody/EllipsoidFactory.cpp
index e848dd28527d9c763c55bab2b7e02467c82b05a3..8e8835b23d2507e053026d6cd081a95907436ddb 100644
--- a/src/pe/rigidbody/EllipsoidFactory.cpp
+++ b/src/pe/rigidbody/EllipsoidFactory.cpp
@@ -25,40 +25,42 @@
 #include "pe/rigidbody/BodyStorage.h"
 #include "pe/rigidbody/Ellipsoid.h"
 
+#include <core/logging/Logging.h>
+#include <core/UniqueID.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 )
+                             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;
+   EllipsoidID ellipsoid = nullptr;
 
    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);
+      EllipsoidPtr el = std::make_unique<Ellipsoid>(sid, uid, gpos, Vec3(0,0,0), Quat(), semiAxes, material, global, false, true);
+      ellipsoid = static_cast<EllipsoidID>(&globalStorage.add(std::move(el)));
    } else
    {
-      for (auto it = blocks.begin(); it != blocks.end(); ++it){
-         IBlock* block = (&(*it));
-         if (block->getAABB().contains(gpos))
+      for (auto& block : blocks){
+         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, communicating, infiniteMass);
-            ellipsoid->MPITrait.setOwner(Owner(MPIManager::instance()->rank(), block->getId().getID()));
-            (*bs)[0].add(ellipsoid);
+            BodyStorage& bs = (*block.getData<Storage>(storageID))[0];
+            EllipsoidPtr el = std::make_unique<Ellipsoid>(sid, uid, gpos, Vec3(0,0,0), Quat(), semiAxes, material, global, communicating, infiniteMass);
+            el->MPITrait.setOwner(Owner(MPIManager::instance()->rank(), block.getId().getID()));
+            ellipsoid = static_cast<EllipsoidID>(&bs.add(std::move(el)));
          }
       }
    }
@@ -67,12 +69,12 @@ EllipsoidID createEllipsoid( BodyStorage& globalStorage, BlockStorage& blocks, B
    {
       // Logging the successful creation of the Ellipsoid
       WALBERLA_LOG_DETAIL(
-                "Created Ellipsoid " << ellipsoid->getSystemID() << "\n"
-             << "   User-ID         = " << uid << "\n"
-             << "   Global position = " << gpos << "\n"
-             << "   Semi-axes       = " << semiAxes << "\n"
-             << "   LinVel          = " << ellipsoid->getLinearVel() << "\n"
-             << "   Material        = " << Material::getName( material )
+               "Created Ellipsoid " << ellipsoid->getSystemID() << "\n"
+               << "   User-ID         = " << uid << "\n"
+               << "   Global position = " << gpos << "\n"
+               << "   Semi-axes       = " << semiAxes << "\n"
+               << "   LinVel          = " << ellipsoid->getLinearVel() << "\n"
+               << "   Material        = " << Material::getName( material )
                );
    }
 
diff --git a/src/pe/rigidbody/Node.h b/src/pe/rigidbody/Node.h
deleted file mode 100644
index 584d44e4f5bb66ce8c2d55d7891992178776160e..0000000000000000000000000000000000000000
--- a/src/pe/rigidbody/Node.h
+++ /dev/null
@@ -1,137 +0,0 @@
-//======================================================================================================================
-//
-//  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 Node.h
-//! \author Klaus Iglberger
-//! \author Sebastian Eibl <sebastian.eibl@fau.de>
-//
-//======================================================================================================================
-
-#pragma once
-
-
-//*************************************************************************************************
-// Includes
-//*************************************************************************************************
-
-#include <pe/Types.h>
-#include <core/DataTypes.h>
-
-namespace walberla {
-namespace pe {
-
-//=================================================================================================
-//
-//  SPECIALIZATION FOR THE UNION FIND ALGORITHM
-//
-//=================================================================================================
-
-//*************************************************************************************************
-/*!\brief Node for the 'Union Find' algorithm.
- *
- * This specialization of the BodyTrait class template adapts rigid bodies to the 'Union Find'
- * batch generation algorithm. In this algorithm, rigid bodies act as nodes in a graph and
- * contacts between the rigid bodies act as edges.
- */
-class Node
-{
-public:
-
-   //**Constructor*********************************************************************************
-   /*!\name Constructor */
-   //@{
-   explicit inline Node();
-   //@}
-   //**********************************************************************************************
-
-   //**Node functions******************************************************************************
-   /*!\name Node functions */
-   //@{
-   inline ConstNodeID getRoot()   const;
-   inline void        resetNode() const;
-   //@}
-   //**********************************************************************************************
-
-public:
-   //**Member variables****************************************************************************
-   /*!\name Member variables */
-   //@{
-   mutable ConstNodeID root_;  //!< The root of the contact graph containing the rigid body.
-   mutable size_t rank_;       //!< The current rank of the rigid body in the contact graph.
-   mutable size_t batch_;      //!< Index of the batch containing all contacts in the contact graph.
-   //@}
-   //**********************************************************************************************
-};
-//*************************************************************************************************
-
-
-//*************************************************************************************************
-/*!\brief Default constructor for the BodyTrait<UnionFind> specialization.
- */
-inline Node::Node()
-   : rank_ ( 0 )
-   , batch_( 0 )
-{
-   root_ = this ;
-}
-//*************************************************************************************************
-
-
-//*************************************************************************************************
-/*!\brief Calculating the current root of the contact graph.
- *
- * \return The current root of the contact graph.
- *
- * This function returns the current root of the contact graph and compresses the graph for
- * a fast access to the root node.
- */
-inline ConstNodeID Node::getRoot() const
-{
-   ConstNodeID root( this );
-
-   // Traverse the contact graph to find the root node
-   while( root->root_ != root ) {
-      root = root->root_;
-   }
-
-   // Retraverse the graph for graph compression
-   ConstNodeID tmp( this );
-   while( tmp->root_ != tmp ) {
-      ConstNodeID last = tmp;
-      tmp = tmp->root_;
-      last->root_ = root;
-   }
-
-   return root;
-}
-//*************************************************************************************************
-
-
-//*************************************************************************************************
-/*!\brief Resetting the data members of the node.
- *
- * \return void
- */
-inline void Node::resetNode() const
-{
-   root_  = this;
-   rank_  = 0;
-   batch_ = 0;
-}
-//*************************************************************************************************
-
-} // namespace pe
-
-} // namespace walberla
diff --git a/src/pe/rigidbody/PlaneFactory.cpp b/src/pe/rigidbody/PlaneFactory.cpp
index 2f780a12a1d1377fcf72b283d082e0e3a1320884..650054ee07b68655d53d5e959122d35ed66ea2ce 100644
--- a/src/pe/rigidbody/PlaneFactory.cpp
+++ b/src/pe/rigidbody/PlaneFactory.cpp
@@ -26,7 +26,8 @@
 #include "pe/rigidbody/BodyStorage.h"
 #include "pe/rigidbody/Plane.h"
 
-#include "core/logging/Logging.h"
+#include <core/logging/Logging.h>
+#include <core/UniqueID.h>
 
 namespace walberla {
 namespace pe {
@@ -44,9 +45,8 @@ PlaneID createPlane( BodyStorage& globalStorage, id_t uid, Vec3 normal, const Ve
 
    const id_t sid( UniqueID<RigidBody>::createGlobal() );
 
-   PlaneID plane = new Plane( sid, uid, gpos, normal, normal*gpos, material );
-
-   globalStorage.add(plane);
+   PlanePtr pl = std::make_unique<Plane>( sid, uid, gpos, normal, normal*gpos, material );
+   PlaneID plane = static_cast<PlaneID>(&globalStorage.add(std::move(pl)));
 
    // Logging the successful creation of the plane
    WALBERLA_LOG_DETAIL( "Created plane " << sid << "\n"
diff --git a/src/pe/rigidbody/RigidBody.cpp b/src/pe/rigidbody/RigidBody.cpp
index 45d40109e31a29020a55517e8c28b1b5fecddf2f..f23bde4e08d0e5c320a89a356cfdb3cdc2988e18 100644
--- a/src/pe/rigidbody/RigidBody.cpp
+++ b/src/pe/rigidbody/RigidBody.cpp
@@ -33,8 +33,7 @@ namespace pe{
  * \param uid The user-specific ID of the rigid body.
  */
 RigidBody::RigidBody( id_t const typeID, id_t sid, id_t uid )
-   : Node()
-   , awake_( true )           // Sleep mode flag
+   : awake_( true )           // Sleep mode flag
    , mass_( 0 )               // Total mass of the rigid body
    , invMass_( 0 )            // Inverse total mass of the rigid body
    , motion_(sleepThreshold)  // The current motion of the rigid body
@@ -57,7 +56,6 @@ RigidBody::RigidBody( id_t const typeID, id_t sid, id_t uid )
    , toBeDeleted_(false)      // deletion flag
    , sid_    (sid)            // System-specific body index
    , uid_    (uid)            // User-specific body ID
-   , contacts_()              // Vector of the currently attached contacts
    , typeID_(typeID)          // geometry type
 {
    sb_ = this;           // The superordinate rigid body
diff --git a/src/pe/rigidbody/RigidBody.h b/src/pe/rigidbody/RigidBody.h
index 004423cc12a1e14f3f9de124046895db2c0add15..fdebc3ff61424cbbdcc213d45bb2d473b6d07014 100644
--- a/src/pe/rigidbody/RigidBody.h
+++ b/src/pe/rigidbody/RigidBody.h
@@ -28,16 +28,12 @@
 #include "core/math/Matrix3.h"
 #include "core/math/Quaternion.h"
 #include "core/math/Vector3.h"
-#include <pe/rigidbody/Node.h>
 
 #include "core/NonCopyable.h"
 #include "core/DataTypes.h"
-#include "core/ptrvector/PtrVector.h"
-#include "core/ptrvector/policies/NoDelete.h"
 #include "core/debug/Debug.h"
 #include "core/logging/Logging.h"
 #include "core/math/AABB.h"
-#include "pe/attachable/Attachable.h"
 #include "pe/rigidbody/MPIRigidBodyTrait.h"
 
 namespace walberla{
@@ -49,8 +45,7 @@ class Union;
 /**
  * \ingroup pe
  */
-class RigidBody : public Node
-                , public ccd::HashGridsBodyTrait
+class RigidBody : public ccd::HashGridsBodyTrait
                 , public cr::HCSITSBodyTrait
                 , private NonCopyable
 {
@@ -58,21 +53,6 @@ private:
    template <typename BodyTypeTuple>
    friend class Union;
 
-   //**Type definitions****************************************************************************
-   typedef PtrVector<RigidBody,NoDelete>   Bodies;       //!< Vector for superordinate bodies containing this rigid body.
-   typedef PtrVector<Contact,NoDelete>     Contacts;     //!< Vector for attached contacts.
-   typedef PtrVector<Attachable,NoDelete>  Attachables;  //!< Vector for attachables.
-   //**********************************************************************************************
-
-public:
-   //**Type definitions****************************************************************************
-   typedef Contacts::Iterator          ContactIterator;            //!< Iterator over the currently attached contacts.
-   typedef Contacts::ConstIterator     ConstContactIterator;       //!< ConstIterator over the currently attached contacts.
-   typedef Attachables::Iterator       AttachableIterator;         //!< Iterator over the currently attached attachables.
-   typedef Attachables::ConstIterator  ConstAttachableIterator;    //!< ConstIterator over the currently attached attachables.
-   typedef Bodies::Iterator            AttachedBodyIterator;       //!< Iterator over the currently attached rigid bodies.
-   typedef Bodies::ConstIterator       ConstAttachedBodyIterator;  //!< ConstIterator over the currently attached rigid bodies.
-   //**********************************************************************************************
 protected:
    //**Constructor*********************************************************************************
    /*!\name Constructor */
@@ -275,38 +255,6 @@ public:
    //@}
    //**********************************************************************************************
 
-   //**Contact functions***************************************************************************
-   /*!\name Contact functions */
-   //@{
-   inline void                 registerContact( ContactID contact );
-   inline bool                 hasContacts  () const;
-   inline void                 clearContacts();
-   inline size_t               countContacts() const;
-   inline ContactIterator      beginContacts();
-   inline ConstContactIterator beginContacts() const;
-   inline ContactIterator      endContacts  ();
-   inline ConstContactIterator endContacts  () const;
-   //@}
-   //**********************************************************************************************
-
-   //**Attachable functions************************************************************************
-   /*!\name Attachable functions */
-   //@{
-          void                      registerAttachable  ( AttachableID attachable );
-          void                      deregisterAttachable( AttachableID attachable );
-   inline bool                      isAttached          () const;
-   inline size_t                    countAttachables    () const;
-   inline AttachableIterator        beginAttachables    ();
-   inline ConstAttachableIterator   beginAttachables    () const;
-   inline AttachableIterator        endAttachables      ();
-   inline ConstAttachableIterator   endAttachables      () const;
-   inline AttachedBodyIterator      beginAttachedBodies ();
-   inline ConstAttachedBodyIterator beginAttachedBodies () const;
-   inline AttachedBodyIterator      endAttachedBodies   ();
-   inline ConstAttachedBodyIterator endAttachedBodies   () const;
-   //@}
-   //**********************************************************************************************
-
    //**MPI functions*******************************************************************************
    /*!\name MPI functions */
    //@{
@@ -460,9 +408,6 @@ protected:
    bool toBeDeleted_;         //!< This flag marks the body for deletion during the next synchronization (only works on local bodies)
    id_t sid_;                 //!< The unique system-specific body ID.
    id_t uid_;                 //!< The user-specific body ID.
-   Contacts contacts_;        //!< Vector for the currently attached contacts.
-   Attachables attachables_;  //!< Vector for the currently attached attachables.
-   Bodies attachedBodies_;    //!< Vector for the currently attached rigid bodies.
    //@}
    //**********************************************************************************************
 
@@ -1943,240 +1888,6 @@ inline void RigidBody::signalFixation()
 
 
 
-//=================================================================================================
-//
-//  CONTACT FUNCTIONS
-//
-//=================================================================================================
-
-//*************************************************************************************************
-/*!\brief Registering a single attached contact with the rigid body.
- *
- * \param contact The contact to be registered with the rigid body.
- * \return void
- */
-inline void RigidBody::registerContact( ContactID contact )
-{
-   WALBERLA_ASSERT( !hasSuperBody(), "Invalid contact on subordinate body detected" );
-   contacts_.pushBack( contact );
-}
-//*************************************************************************************************
-
-
-//*************************************************************************************************
-/*!\brief Clears all contacts registered with the rigid body.
- *
- * \return void
- */
-inline void RigidBody::clearContacts()
-{
-   contacts_.clear();
-}
-//*************************************************************************************************
-
-
-//*************************************************************************************************
-/*!\brief Returns whether any contacts are registered with the rigid body.
- *
- * \return \a true if at least one contact is registered with the rigid body, \a false if not.
- */
-inline bool RigidBody::hasContacts() const
-{
-   return !contacts_.isEmpty();
-}
-//*************************************************************************************************
-
-
-//*************************************************************************************************
-/*!\brief Returns the number of currently registered contacts.
- *
- * \return The number of registered contacts.
- */
-inline size_t RigidBody::countContacts() const
-{
-   return contacts_.size();
-}
-//*************************************************************************************************
-
-
-//*************************************************************************************************
-/*!\brief Returns an iterator to the first attached contact.
- *
- * \return Iterator to the first attached contact.
- */
-inline RigidBody::ContactIterator RigidBody::beginContacts()
-{
-   return contacts_.begin();
-}
-//*************************************************************************************************
-
-
-//*************************************************************************************************
-/*!\brief Returns an iterator to the first attached contact.
- *
- * \return Iterator to the first attached contact.
- */
-inline RigidBody::ConstContactIterator RigidBody::beginContacts() const
-{
-   return contacts_.begin();
-}
-//*************************************************************************************************
-
-
-//*************************************************************************************************
-/*!\brief Returns an iterator just past the last attached contact.
- *
- * \return Iterator just past the last attached contact.
- */
-inline RigidBody::ContactIterator RigidBody::endContacts()
-{
-   return contacts_.end();
-}
-//*************************************************************************************************
-
-
-//*************************************************************************************************
-/*!\brief Returns an iterator just past the last attached contact.
- *
- * \return Iterator just past the last attached contact.
- */
-inline RigidBody::ConstContactIterator RigidBody::endContacts() const
-{
-   return contacts_.end();
-}
-//*************************************************************************************************
-
-
-
-
-//=================================================================================================
-//
-//  ATTACHABLE FUNCTIONS
-//
-//=================================================================================================
-
-//*************************************************************************************************
-/*!\brief Returns whether any attachable is registered with the rigid body.
- *
- * \return \a true in case at least one attachable is registered with the rigid body, \a false otherwise.
- */
-inline bool RigidBody::isAttached() const
-{
-   return !attachables_.isEmpty();
-}
-//*************************************************************************************************
-
-
-//*************************************************************************************************
-/*!\brief Returns the number of currently registered attachables.
- *
- * \return The number of registered attachables.
- */
-inline size_t RigidBody::countAttachables() const
-{
-   return attachables_.size();
-}
-//*************************************************************************************************
-
-
-//*************************************************************************************************
-/*!\brief Returns an iterator to the first registered attachable.
- *
- * \return Iterator to the first registered attachable.
- */
-inline RigidBody::AttachableIterator RigidBody::beginAttachables()
-{
-   return attachables_.begin();
-}
-//*************************************************************************************************
-
-
-//*************************************************************************************************
-/*!\brief Returns an iterator to the first registered attachable.
- *
- * \return Iterator to the first registered attachable.
- */
-inline RigidBody::ConstAttachableIterator RigidBody::beginAttachables() const
-{
-   return attachables_.begin();
-}
-//*************************************************************************************************
-
-
-//*************************************************************************************************
-/*!\brief Returns an iterator just past the last registered attachable.
- *
- * \return Iterator just past the last registered attachable.
- */
-inline RigidBody::AttachableIterator RigidBody::endAttachables()
-{
-   return attachables_.end();
-}
-//*************************************************************************************************
-
-
-//*************************************************************************************************
-/*!\brief Returns an iterator just past the last registered attachable.
- *
- * \return Iterator just past the last registered attachable.
- */
-inline RigidBody::ConstAttachableIterator RigidBody::endAttachables() const
-{
-   return attachables_.end();
-}
-//*************************************************************************************************
-
-
-//*************************************************************************************************
-/*!\brief Returns an iterator to the first attached rigid body.
- *
- * \return Iterator to the first attached rigid body.
- */
-inline RigidBody::AttachedBodyIterator RigidBody::beginAttachedBodies()
-{
-   return attachedBodies_.begin();
-}
-//*************************************************************************************************
-
-
-//*************************************************************************************************
-/*!\brief Returns an iterator to the first attached rigid body.
- *
- * \return Iterator to the first attached rigid body.
- */
-inline RigidBody::ConstAttachedBodyIterator RigidBody::beginAttachedBodies() const
-{
-   return attachedBodies_.begin();
-}
-//*************************************************************************************************
-
-
-//*************************************************************************************************
-/*!\brief Returns an iterator just past the last attached rigid body.
- *
- * \return Iterator just past the last attached rigid body.
- */
-inline RigidBody::AttachedBodyIterator RigidBody::endAttachedBodies()
-{
-   return attachedBodies_.end();
-}
-//*************************************************************************************************
-
-
-//*************************************************************************************************
-/*!\brief Returns an iterator just past the last attached rigid body.
- *
- * \return Iterator just past the last attached rigid body.
- */
-inline RigidBody::ConstAttachedBodyIterator RigidBody::endAttachedBodies() const
-{
-   return attachedBodies_.end();
-}
-//*************************************************************************************************
-
-
-
-
 //=================================================================================================
 //
 //  MPI FUNCTIONS
@@ -3347,73 +3058,6 @@ inline void RigidBody::update( const Quat& dq )
 
 
 
-//=================================================================================================
-//
-//  ATTACHABLE FUNCTIONS
-//
-//=================================================================================================
-
-//*************************************************************************************************
-/*!\brief Registering a single attachable with the rigid body.
- *
- * \param attachable The attachable to be registered with the rigid body.
- * \return void
- *
- * This function must NOT be called explicitly, but is reserved for internal use only!
- * Attachables are automatically registered during their construction process.
- */
-inline void RigidBody::registerAttachable( AttachableID attachable )
-{
-   // Registering the attachable
-   attachables_.pushBack( attachable );
-
-   // Registering all newly attached rigid bodies
-   const Attachable::Iterator end( attachable->end() );
-   for( Attachable::Iterator body=attachable->begin(); body!=end; ++body ) {
-      const AttachedBodyIterator bbegin( attachedBodies_.begin() );
-      const AttachedBodyIterator bend  ( attachedBodies_.end()   );
-      if( *body != this && std::find( bbegin, bend, *body ) == bend )
-         attachedBodies_.pushBack( *body );
-   }
-}
-//*************************************************************************************************
-
-
-//*************************************************************************************************
-/*!\brief Deregistering a single attachable from the rigid body.
- *
- * \param attachable The attachable to be deregistered from the rigid body.
- * \return void
- *
- * This function must NOT be called explicitly, but is reserved for internal use only!
- * Attachables are automatically deregistered during their destruction process.
- */
-inline void RigidBody::deregisterAttachable( AttachableID attachable )
-{
-   // Deregistering the attachable
-   Attachables::Iterator pos( std::find( attachables_.begin(), attachables_.end(), attachable ) );
-   WALBERLA_ASSERT( pos != attachables_.end(), "Attachable is not registered" );
-   attachables_.erase( pos );
-
-   // Deregistering all attached rigid bodies
-   attachedBodies_.clear();
-
-   // Recreating the vector of attached rigid bodies
-   for( Attachables::Iterator it=attachables_.begin(); it!=attachables_.end(); ++it ) {
-      const Attachable::Iterator end( it->end() );
-      for( Attachable::Iterator body=it->begin(); body!=end; ++body ) {
-         const AttachedBodyIterator bbegin( attachedBodies_.begin() );
-         const AttachedBodyIterator bend  ( attachedBodies_.end()   );
-         if( *body != this && std::find( bbegin, bend, *body ) == bend )
-            attachedBodies_.pushBack( *body );
-      }
-   }
-}
-//*************************************************************************************************
-
-
-
-
 //=================================================================================================
 //
 //  SIMULATION FUNCTIONS
diff --git a/src/pe/rigidbody/RigidBodyCastIterator.h b/src/pe/rigidbody/RigidBodyCastIterator.h
new file mode 100644
index 0000000000000000000000000000000000000000..7241baa63ce3b6eac5bafb20be5571016f992526
--- /dev/null
+++ b/src/pe/rigidbody/RigidBodyCastIterator.h
@@ -0,0 +1,315 @@
+//======================================================================================================================
+//
+//  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 RigidBodyCastIterator.h
+//! \author Klaus Iglberger
+//! \author Sebastian Eibl <sebastian.eibl@fau.de>
+//
+//======================================================================================================================
+
+#pragma once
+
+#include "RigidBody.h"
+
+#include <memory>
+#include <type_traits>
+#include <vector>
+
+namespace walberla {
+namespace pe {
+
+//*************************************************************************************************
+/*!\brief Dynamic cast iterator for polymorphic pointer vectors.
+ *
+ * The RigidBodyCastIterator is a forward iterator which only selects elements of type C.
+ * Dereferencing this iterator will implicitly cast to C.
+ */
+template <typename C>
+class RigidBodyCastIterator
+{
+   static_assert(std::is_base_of<RigidBody, C>::value && !std::is_base_of<C, RigidBody>::value,
+                 "C has to be strictly derived from RigidBody");
+
+   template <typename C2>
+   friend inline bool operator==( const RigidBodyCastIterator<C2>& lhs, const RigidBodyCastIterator<C2>& rhs );
+   template <typename C2>
+   friend inline bool operator!=( const RigidBodyCastIterator<C2>& lhs, const RigidBodyCastIterator<C2>& rhs );
+public:
+   using ContainerType         = std::vector< std::unique_ptr<RigidBody> >;
+   //**Type definitions****************************************************************************
+   // STL iterator requirements
+   using iterator_category     = std::forward_iterator_tag;
+   using value_type            = C;
+   using pointer               = C*;
+   using reference             = C&;
+   using difference_type       = std::ptrdiff_t;
+   //**********************************************************************************************
+
+   //**Constructors********************************************************************************
+   /*!\name Constructors */
+   //@{
+   inline RigidBodyCastIterator() {}
+   explicit inline RigidBodyCastIterator( const typename ContainerType::iterator& begin, const typename ContainerType::iterator& end );
+
+   RigidBodyCastIterator( const RigidBodyCastIterator<C>& it) = default;
+   RigidBodyCastIterator( RigidBodyCastIterator<C>&& it) = default;
+
+   RigidBodyCastIterator& operator=(const RigidBodyCastIterator<C>& it) = default;
+   RigidBodyCastIterator& operator=(RigidBodyCastIterator<C>&& it) = default;
+
+   //**Operators***********************************************************************************
+   /*!\name Operators */
+   //@{
+   inline RigidBodyCastIterator<C>&   operator++();
+   inline RigidBodyCastIterator<C>    operator++( int ) {RigidBodyCastIterator<C> tmp(*this); ++(*this); return tmp;}
+   //@}
+   //**********************************************************************************************
+
+   //**Access operators****************************************************************************
+   /*!\name Access operators */
+   //@{
+   inline reference operator*()   {return static_cast<reference>( *(cur_->get()) );}
+   inline pointer   operator->()  {return static_cast<pointer>(     cur_->get()  );}
+   //@}
+   //**********************************************************************************************
+
+   //**Utility functions***************************************************************************
+   /*!\name Utility functions */
+   //@{
+   inline pointer getBodyID() {return static_cast<pointer>(cur_->get());}
+   //@}
+   //**********************************************************************************************
+
+private:
+   //**Member variables****************************************************************************
+   /*!\name Member variables */
+   //@{
+   typename ContainerType::iterator cur_;  //!< Pointer to the current memory location.
+   typename ContainerType::iterator end_;  //!< Pointer to the element one past the last element in the element range.
+   //@}
+   //**********************************************************************************************
+};
+//*************************************************************************************************
+
+
+//*************************************************************************************************
+/*!\brief Standard constructor for CastIterator.
+ *
+ * \param begin The beginning of the element range.
+ * \param end The end of the element range.
+ */
+template< typename C >  // Cast type
+inline RigidBodyCastIterator<C>::RigidBodyCastIterator( const typename ContainerType::iterator& begin,
+                                                        const typename ContainerType::iterator& end )
+   : cur_(begin)  // Pointer to the current memory location
+   , end_(end)    // Pointer to the element one past the last element in the element range
+{
+   cur_ = std::find_if(cur_, end_, [](const ContainerType::iterator::value_type& p) {return p->getTypeID() == C::getStaticTypeID();});
+}
+//*************************************************************************************************
+
+//*************************************************************************************************
+/*!\brief Pre-increment operator.
+ *
+ * \return Reference to the incremented cast iterator.
+ */
+template< typename C >
+inline RigidBodyCastIterator<C>& RigidBodyCastIterator<C>::operator++()
+{
+   cur_ = std::find_if(++cur_, end_, [](const ContainerType::iterator::value_type& p) {return p->getTypeID() == C::getStaticTypeID();});
+
+   return *this;
+}
+//*************************************************************************************************
+
+
+//**********************************************************************************************
+/*!\brief Equality comparison between two CastIterator objects.
+//
+// \param lhs The left hand side cast iterator.
+// \param rhs The right hand side cast iterator.
+// \return \a true if the iterators point to the same element, \a false if not.
+*/
+template< typename C >
+inline bool operator==( const RigidBodyCastIterator<C>& lhs, const RigidBodyCastIterator<C>& rhs )
+{
+   return lhs.cur_ == rhs.cur_;
+}
+//**********************************************************************************************
+
+//**********************************************************************************************
+/*!\brief Inequality comparison between two CastIterator objects.
+//
+// \param lhs The left hand side cast iterator.
+// \param rhs The right hand side cast iterator.
+// \return \a true if the iterators don't point to the same element, \a false if they do.
+*/
+template< typename C >
+inline bool operator!=( const RigidBodyCastIterator<C>& lhs, const RigidBodyCastIterator<C>& rhs )
+{
+   return !operator==(lhs, rhs);
+}
+//**********************************************************************************************
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+template <typename C>
+class ConstRigidBodyCastIterator
+{
+   static_assert(std::is_base_of<RigidBody, C>::value && !std::is_base_of<C, RigidBody>::value,
+                 "C has to be strictly derived from RigidBody");
+
+   template <typename C2>
+   friend bool operator==( const ConstRigidBodyCastIterator<C2>& lhs, const ConstRigidBodyCastIterator<C2>& rhs );
+   template <typename C2>
+   friend bool operator!=( const ConstRigidBodyCastIterator<C2>& lhs, const ConstRigidBodyCastIterator<C2>& rhs );
+public:
+   using ContainerType         = std::vector< std::unique_ptr<RigidBody> >;
+   //**Type definitions****************************************************************************
+   // STL iterator requirements
+   using iterator_category     = std::forward_iterator_tag;
+   using value_type            = C const;
+   using pointer               = C const *;
+   using reference             = C const &;
+   using difference_type       = std::ptrdiff_t;
+   //**********************************************************************************************
+
+   //**Constructors********************************************************************************
+   /*!\name Constructors */
+   //@{
+   inline ConstRigidBodyCastIterator() {}
+   explicit inline ConstRigidBodyCastIterator( const typename ContainerType::const_iterator& begin,
+                                               const typename ContainerType::const_iterator& end );
+
+   ConstRigidBodyCastIterator( const ConstRigidBodyCastIterator<C>& it) = default;
+   ConstRigidBodyCastIterator( ConstRigidBodyCastIterator<C>&& it) = default;
+
+   ConstRigidBodyCastIterator& operator=(const ConstRigidBodyCastIterator<C>& it) = default;
+   ConstRigidBodyCastIterator& operator=(ConstRigidBodyCastIterator<C>&& it) = default;
+
+   //**Operators***********************************************************************************
+   /*!\name Operators */
+   //@{
+   inline ConstRigidBodyCastIterator<C>&   operator++();
+   inline ConstRigidBodyCastIterator<C>    operator++( int ) {ConstRigidBodyCastIterator<C> tmp(*this); ++(*this); return tmp;}
+   //@}
+   //**********************************************************************************************
+
+   //**Access operators****************************************************************************
+   /*!\name Access operators */
+   //@{
+   inline reference operator*()   {return static_cast<reference>( *(cur_->get()) );}
+   inline pointer   operator->()  {return static_cast<pointer>(     cur_->get()  );}
+   //@}
+   //**********************************************************************************************
+
+   //**Utility functions***************************************************************************
+   /*!\name Utility functions */
+   //@{
+   inline pointer getBodyID() {return static_cast<pointer>(cur_->get());}
+   //@}
+   //**********************************************************************************************
+
+private:
+   //**Member variables****************************************************************************
+   /*!\name Member variables */
+   //@{
+   typename ContainerType::const_iterator cur_;  //!< Pointer to the current memory location.
+   typename ContainerType::const_iterator end_;  //!< Pointer to the element one past the last element in the element range.
+   //@}
+   //**********************************************************************************************
+};
+//*************************************************************************************************
+
+
+//*************************************************************************************************
+/*!\brief Standard constructor for CastIterator.
+ *
+ * \param begin The beginning of the element range.
+ * \param end The end of the element range.
+ */
+template< typename C >  // Cast type
+inline ConstRigidBodyCastIterator<C>::ConstRigidBodyCastIterator( const typename ContainerType::const_iterator& begin,
+                                                                  const typename ContainerType::const_iterator& end )
+   : cur_(begin)  // Pointer to the current memory location
+   , end_(end)    // Pointer to the element one past the last element in the element range
+{
+   cur_ = std::find_if(cur_, end_, [](const ContainerType::const_iterator::value_type& p) {return p->getTypeID() == C::getStaticTypeID();});
+}
+//*************************************************************************************************
+
+//*************************************************************************************************
+/*!\brief Pre-increment operator.
+ *
+ * \return Reference to the incremented cast iterator.
+ */
+template< typename C >
+inline ConstRigidBodyCastIterator<C>& ConstRigidBodyCastIterator<C>::operator++()
+{
+   cur_ = std::find_if(++cur_, end_, [](const ContainerType::const_iterator::value_type& p) {return p->getTypeID() == C::getStaticTypeID();});
+
+   return *this;
+}
+//*************************************************************************************************
+
+
+//**********************************************************************************************
+/*!\brief Equality comparison between two CastIterator objects.
+//
+// \param lhs The left hand side cast iterator.
+// \param rhs The right hand side cast iterator.
+// \return \a true if the iterators point to the same element, \a false if not.
+*/
+template< typename C >
+inline bool operator==( const ConstRigidBodyCastIterator<C>& lhs, const ConstRigidBodyCastIterator<C>& rhs )
+{
+   return lhs.cur_ == rhs.cur_;
+}
+//**********************************************************************************************
+
+//**********************************************************************************************
+/*!\brief Inequality comparison between two CastIterator objects.
+//
+// \param lhs The left hand side cast iterator.
+// \param rhs The right hand side cast iterator.
+// \return \a true if the iterators don't point to the same element, \a false if they do.
+*/
+template< typename C >
+inline bool operator!=( const ConstRigidBodyCastIterator<C>& lhs, const ConstRigidBodyCastIterator<C>& rhs )
+{
+   return !operator==(lhs, rhs);
+}
+//**********************************************************************************************
+
+} // namespace pe
+} // namespace walberla
diff --git a/src/pe/rigidbody/RigidBodyIterator.h b/src/pe/rigidbody/RigidBodyIterator.h
new file mode 100644
index 0000000000000000000000000000000000000000..8e2be67d2f299dde26ebb313b4a17e4c22642301
--- /dev/null
+++ b/src/pe/rigidbody/RigidBodyIterator.h
@@ -0,0 +1,367 @@
+//======================================================================================================================
+//
+//  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 RigidBodyIterator.h
+//! \author Klaus Iglberger
+//! \author Sebastian Eibl <sebastian.eibl@fau.de>
+//
+//======================================================================================================================
+
+#pragma once
+
+#include "RigidBody.h"
+
+#include <memory>
+#include <type_traits>
+#include <vector>
+
+namespace walberla {
+namespace pe {
+
+//*************************************************************************************************
+/*!\brief Implementation of an iterator for pointer vectors.
+ *
+ * This iterator implementation will automatically dereference twice at dereference calls!
+ * Therefore the return types are not unique_ptrs but RigidBodys.
+ */
+class RigidBodyIterator
+{
+   friend inline bool operator==( const RigidBodyIterator& lhs, const RigidBodyIterator& rhs );
+   friend inline bool operator!=( const RigidBodyIterator& lhs, const RigidBodyIterator& rhs );
+   friend inline bool operator>( const RigidBodyIterator& lhs, const RigidBodyIterator& rhs );
+   friend inline bool operator<( const RigidBodyIterator& lhs, const RigidBodyIterator& rhs );
+   friend inline bool operator>=( const RigidBodyIterator& lhs, const RigidBodyIterator& rhs );
+   friend inline bool operator<=( const RigidBodyIterator& lhs, const RigidBodyIterator& rhs );
+public:
+   using ContainerType         = std::vector< std::unique_ptr<RigidBody> >;
+   //**Type definitions****************************************************************************
+   // STL iterator requirements
+   using iterator_category     = std::random_access_iterator_tag;
+   using value_type            = RigidBody;
+   using pointer               = RigidBody*;
+   using reference             = RigidBody&;
+   using difference_type       = std::ptrdiff_t;
+   //**********************************************************************************************
+
+   //**Constructors********************************************************************************
+   /*!\name Constructors */
+   //@{
+   inline RigidBodyIterator() {}
+   explicit inline RigidBodyIterator( const typename ContainerType::iterator& it ) : it_(it) {}
+
+   RigidBodyIterator( const RigidBodyIterator& it) = default;
+   RigidBodyIterator( RigidBodyIterator&& it) = default;
+
+   RigidBodyIterator& operator=(const RigidBodyIterator& it) = default;
+   RigidBodyIterator& operator=(RigidBodyIterator&& it) = default;
+
+   //**Operators***********************************************************************************
+   /*!\name Operators */
+   //@{
+   inline RigidBodyIterator&   operator++()                    {++it_; return *this;}
+   inline RigidBodyIterator    operator++( int )               {RigidBodyIterator tmp(*this); ++(*this); return tmp;}
+   inline RigidBodyIterator&   operator--()                    {--it_; return *this;}
+   inline RigidBodyIterator    operator--( int )               {RigidBodyIterator tmp(*this); --(*this); return tmp;}
+   inline RigidBodyIterator&   operator+=( difference_type n ) {it_ += n; return *this;}
+   inline RigidBodyIterator    operator+ ( difference_type n ) const {return RigidBodyIterator( it_ + n );}
+   inline RigidBodyIterator&   operator-=( difference_type n ) {it_ -= n; return *this;}
+   inline RigidBodyIterator    operator- ( difference_type n ) const {return RigidBodyIterator( it_ - n );}
+   inline difference_type      operator- ( const RigidBodyIterator& it ) const {return it_ - it.it_;}
+   //@}
+   //**********************************************************************************************
+
+   //**Access operators****************************************************************************
+   /*!\name Access operators */
+   //@{
+   inline reference operator[]( difference_type n ) const {return *it_[n];}
+   inline reference operator*()                     const {return **it_;}
+   inline pointer   operator->()                    const {return it_->get();}
+   //@}
+   //**********************************************************************************************
+
+   //**Utility functions***************************************************************************
+   /*!\name Utility functions */
+   //@{
+   inline pointer getBodyID() {return it_->get();}
+   inline ContainerType::iterator get() const {return it_;}
+   //@}
+   //**********************************************************************************************
+
+private:
+   //**Member variables****************************************************************************
+   /*!\name Member variables */
+   //@{
+   typename ContainerType::iterator it_;  //!< wrapped iterator
+   //@}
+   //**********************************************************************************************
+};
+//*************************************************************************************************
+
+
+//*************************************************************************************************
+/*!\brief Equality comparison between two PtrIterator objects.
+ *
+ * \param lhs The left-hand side pointer iterator.
+ * \param rhs The right-hand side pointer iterator.
+ * \return \a true if the iterators point to the same element, \a false if not.
+ */
+inline bool operator==( const RigidBodyIterator& lhs, const RigidBodyIterator& rhs )
+{
+   return lhs.it_ == rhs.it_;
+}
+//*************************************************************************************************
+
+
+//*************************************************************************************************
+/*!\brief Inequality comparison between two PtrIterator objects.
+ *
+ * \param lhs The left-hand side pointer iterator.
+ * \param rhs The right-hand side pointer iterator.
+ * \return \a true if the iterators don't point to the same element, \a false if they do.
+ */
+inline bool operator!=( const RigidBodyIterator& lhs, const RigidBodyIterator& rhs )
+{
+   return !operator==(lhs, rhs);
+}
+//*************************************************************************************************
+
+
+//*************************************************************************************************
+/*!\brief Less-than comparison between two PtrIterator objects.
+ *
+ * \param lhs The left-hand side pointer iterator.
+ * \param rhs The right-hand side pointer iterator.
+ * \return \a true if the left-hand side iterator points to a lower element, \a false if not.
+ */
+inline bool operator<( const RigidBodyIterator& lhs, const RigidBodyIterator& rhs )
+{
+   return lhs.it_ < rhs.it_;
+}
+//*************************************************************************************************
+
+
+//*************************************************************************************************
+/*!\brief Greater-than comparison between two PtrIterator objects.
+ *
+ * \param lhs The left-hand side pointer iterator.
+ * \param rhs The right-hand side pointer iterator.
+ * \return \a true if the left-hand side iterator points to a higher element, \a false if not.
+ */
+inline bool operator>( const RigidBodyIterator& lhs, const RigidBodyIterator& rhs )
+{
+   return lhs.it_ > rhs.it_;
+}
+//*************************************************************************************************
+
+
+//*************************************************************************************************
+/*!\brief Less-or-equal-than comparison between two PtrIterator objects.
+ *
+ * \param lhs The left-hand side pointer iterator.
+ * \param rhs The right-hand side pointer iterator.
+ * \return \a true if the left-hand side iterator points to a lower or the same element, \a false if not.
+ */
+inline bool operator<=( const RigidBodyIterator& lhs, const RigidBodyIterator& rhs )
+{
+   return lhs.it_ <= rhs.it_;
+}
+//*************************************************************************************************
+
+
+//*************************************************************************************************
+/*!\brief Greater-or-equal-than comparison between two PtrIterator objects.
+ *
+ * \param lhs The left-hand side pointer iterator.
+ * \param rhs The right-hand side pointer iterator.
+ * \return \a true if the left-hand side iterator points to a higher or the same element, \a false if not.
+ */
+inline bool operator>=( const RigidBodyIterator& lhs, const RigidBodyIterator& rhs )
+{
+   return lhs.it_ >= rhs.it_;
+}
+//*************************************************************************************************
+
+
+
+
+
+
+
+
+
+
+
+
+class ConstRigidBodyIterator
+{
+   friend inline bool operator==( const ConstRigidBodyIterator& lhs, const ConstRigidBodyIterator& rhs );
+   friend inline bool operator!=( const ConstRigidBodyIterator& lhs, const ConstRigidBodyIterator& rhs );
+   friend inline bool operator>( const ConstRigidBodyIterator& lhs, const ConstRigidBodyIterator& rhs );
+   friend inline bool operator<( const ConstRigidBodyIterator& lhs, const ConstRigidBodyIterator& rhs );
+   friend inline bool operator>=( const ConstRigidBodyIterator& lhs, const ConstRigidBodyIterator& rhs );
+   friend inline bool operator<=( const ConstRigidBodyIterator& lhs, const ConstRigidBodyIterator& rhs );
+public:
+   using ContainerType         = std::vector< std::unique_ptr<RigidBody> >;
+   //**Type definitions****************************************************************************
+   // STL iterator requirements
+   using iterator_category     = std::random_access_iterator_tag;
+   using value_type            = RigidBody const;
+   using pointer               = RigidBody const *;
+   using reference             = RigidBody const &;
+   using difference_type       = std::ptrdiff_t;
+   //**********************************************************************************************
+
+   //**Constructors********************************************************************************
+   /*!\name Constructors */
+   //@{
+   inline ConstRigidBodyIterator() {}
+   inline ConstRigidBodyIterator( const RigidBodyIterator& it ) : it_(it.get()) {}
+   explicit inline ConstRigidBodyIterator( const typename ContainerType::iterator& it ) : it_(it) {}
+   explicit inline ConstRigidBodyIterator( const typename ContainerType::const_iterator& it ) : it_(it) {}
+
+   ConstRigidBodyIterator( const ConstRigidBodyIterator& it) = default;
+   ConstRigidBodyIterator( ConstRigidBodyIterator&& it) = default;
+
+   ConstRigidBodyIterator& operator=(const ConstRigidBodyIterator& it) = default;
+   ConstRigidBodyIterator& operator=(ConstRigidBodyIterator&& it) = default;
+
+   //**Operators***********************************************************************************
+   /*!\name Operators */
+   //@{
+   inline ConstRigidBodyIterator&   operator++()                    {++it_; return *this;}
+   inline ConstRigidBodyIterator    operator++( int )               {ConstRigidBodyIterator tmp(*this); ++(*this); return tmp;}
+   inline ConstRigidBodyIterator&   operator--()                    {--it_; return *this;}
+   inline ConstRigidBodyIterator    operator--( int )               {ConstRigidBodyIterator tmp(*this); --(*this); return tmp;}
+   inline ConstRigidBodyIterator&   operator+=( difference_type n ) {it_ += n; return *this;}
+   inline ConstRigidBodyIterator    operator+ ( difference_type n ) const {return ConstRigidBodyIterator( it_ + n );}
+   inline ConstRigidBodyIterator&   operator-=( difference_type n ) {it_ -= n; return *this;}
+   inline ConstRigidBodyIterator    operator- ( difference_type n ) const {return ConstRigidBodyIterator( it_ - n );}
+   inline difference_type operator- ( const ConstRigidBodyIterator& it ) const {return it_ - it.it_;}
+   //@}
+   //**********************************************************************************************
+
+   //**Access operators****************************************************************************
+   /*!\name Access operators */
+   //@{
+   inline reference operator[]( difference_type n ) const {return *it_[n];}
+   inline reference operator*()                     const {return **it_;}
+   inline pointer   operator->()                    const {return it_->get();}
+   //@}
+   //**********************************************************************************************
+
+   //**Utility functions***************************************************************************
+   /*!\name Utility functions */
+   //@{
+   inline pointer getBodyID() const {return it_->get();}
+   inline ContainerType::const_iterator get() const {return it_;}
+   //@}
+   //**********************************************************************************************
+
+private:
+   //**Member variables****************************************************************************
+   /*!\name Member variables */
+   //@{
+   typename ContainerType::const_iterator it_;  //!< wrapped iterator
+   //@}
+   //**********************************************************************************************
+};
+//*************************************************************************************************
+
+
+//*************************************************************************************************
+/*!\brief Equality comparison between two PtrIterator objects.
+ *
+ * \param lhs The left-hand side pointer iterator.
+ * \param rhs The right-hand side pointer iterator.
+ * \return \a true if the iterators point to the same element, \a false if not.
+ */
+inline bool operator==( const ConstRigidBodyIterator& lhs, const ConstRigidBodyIterator& rhs )
+{
+   return lhs.it_ == rhs.it_;
+}
+//*************************************************************************************************
+
+
+//*************************************************************************************************
+/*!\brief Inequality comparison between two PtrIterator objects.
+ *
+ * \param lhs The left-hand side pointer iterator.
+ * \param rhs The right-hand side pointer iterator.
+ * \return \a true if the iterators don't point to the same element, \a false if they do.
+ */
+inline bool operator!=( const ConstRigidBodyIterator& lhs, const ConstRigidBodyIterator& rhs )
+{
+   return !operator==(lhs, rhs);
+}
+//*************************************************************************************************
+
+
+//*************************************************************************************************
+/*!\brief Less-than comparison between two PtrIterator objects.
+ *
+ * \param lhs The left-hand side pointer iterator.
+ * \param rhs The right-hand side pointer iterator.
+ * \return \a true if the left-hand side iterator points to a lower element, \a false if not.
+ */
+inline bool operator<( const ConstRigidBodyIterator& lhs, const ConstRigidBodyIterator& rhs )
+{
+   return lhs.it_ < rhs.it_;
+}
+//*************************************************************************************************
+
+
+//*************************************************************************************************
+/*!\brief Greater-than comparison between two PtrIterator objects.
+ *
+ * \param lhs The left-hand side pointer iterator.
+ * \param rhs The right-hand side pointer iterator.
+ * \return \a true if the left-hand side iterator points to a higher element, \a false if not.
+ */
+inline bool operator>( const ConstRigidBodyIterator& lhs, const ConstRigidBodyIterator& rhs )
+{
+   return lhs.it_ > rhs.it_;
+}
+//*************************************************************************************************
+
+
+//*************************************************************************************************
+/*!\brief Less-or-equal-than comparison between two PtrIterator objects.
+ *
+ * \param lhs The left-hand side pointer iterator.
+ * \param rhs The right-hand side pointer iterator.
+ * \return \a true if the left-hand side iterator points to a lower or the same element, \a false if not.
+ */
+inline bool operator<=( const ConstRigidBodyIterator& lhs, const ConstRigidBodyIterator& rhs )
+{
+   return lhs.it_ <= rhs.it_;
+}
+//*************************************************************************************************
+
+
+//*************************************************************************************************
+/*!\brief Greater-or-equal-than comparison between two PtrIterator objects.
+ *
+ * \param lhs The left-hand side pointer iterator.
+ * \param rhs The right-hand side pointer iterator.
+ * \return \a true if the left-hand side iterator points to a higher or the same element, \a false if not.
+ */
+inline bool operator>=( const ConstRigidBodyIterator& lhs, const ConstRigidBodyIterator& rhs )
+{
+   return lhs.it_ >= rhs.it_;
+}
+//*************************************************************************************************
+
+} // namespace pe
+} // namespace walberla
diff --git a/src/pe/rigidbody/SphereFactory.cpp b/src/pe/rigidbody/SphereFactory.cpp
index c9487031def01e2876e0d2bf6d341c1e9681825b..d88774c45f10803f84eaa1ac9f8fcd5a21fb91c6 100644
--- a/src/pe/rigidbody/SphereFactory.cpp
+++ b/src/pe/rigidbody/SphereFactory.cpp
@@ -26,6 +26,9 @@
 #include "pe/rigidbody/BodyStorage.h"
 #include "pe/rigidbody/Sphere.h"
 
+#include <core/logging/Logging.h>
+#include <core/UniqueID.h>
+
 namespace walberla {
 namespace pe {
 
@@ -39,32 +42,31 @@ SphereID createSphere( BodyStorage& globalStorage, BlockStorage& blocks, BlockDa
    if( radius <= real_c(0) )
       throw std::invalid_argument( "Invalid sphere radius" );
 
-   SphereID sphere = NULL;
+   SphereID sphere = nullptr;
 
    if (global)
    {
       const id_t sid = UniqueID<RigidBody>::createGlobal();
       WALBERLA_ASSERT_EQUAL(communicating, false);
       WALBERLA_ASSERT_EQUAL(infiniteMass, true);
-      sphere = new Sphere(sid, uid, gpos, Vec3(0,0,0), Quat(), radius, material, global, false, true);
-      globalStorage.add(sphere);
+      SpherePtr sp = std::make_unique<Sphere>(sid, uid, gpos, Vec3(0,0,0), Quat(), radius, material, global, false, true);
+      sphere = static_cast<SphereID>(&globalStorage.add( std::move(sp) ));
    } else
    {
-      for (auto it = blocks.begin(); it != blocks.end(); ++it){
-         IBlock* block = (&(*it));
-         if (block->getAABB().contains(gpos))
+      for (auto& block : blocks){
+         if (block.getAABB().contains(gpos))
          {
             const id_t sid( UniqueID<RigidBody>::create() );
 
-            Storage* bs = block->getData<Storage>(storageID);
-            sphere = new Sphere(sid, uid, gpos, Vec3(0,0,0), Quat(), radius, material, global, communicating, infiniteMass);
-            sphere->MPITrait.setOwner(Owner(MPIManager::instance()->rank(), block->getId().getID()));
-            (*bs)[0].add(sphere);
+            BodyStorage& bs = (*block.getData<Storage>(storageID))[0];
+            SpherePtr sp = std::make_unique<Sphere>(sid, uid, gpos, Vec3(0,0,0), Quat(), radius, material, global, communicating, infiniteMass);
+            sp->MPITrait.setOwner(Owner(MPIManager::instance()->rank(), block.getId().getID()));
+            sphere = static_cast<SphereID>(&bs.add( std::move(sp) ));
          }
       }
    }
 
-   if (sphere != NULL)
+   if (sphere != nullptr)
    {
       // Logging the successful creation of the sphere
       WALBERLA_LOG_DETAIL(
diff --git a/src/pe/rigidbody/SquirmerFactory.cpp b/src/pe/rigidbody/SquirmerFactory.cpp
index 41b495b75aa008cf3aae1278b98ad450302b9cd7..7160ff2d0fa80e72dcf28a5f3fe7445e0679999f 100644
--- a/src/pe/rigidbody/SquirmerFactory.cpp
+++ b/src/pe/rigidbody/SquirmerFactory.cpp
@@ -24,6 +24,9 @@
 #include "pe/rigidbody/BodyStorage.h"
 #include "pe/rigidbody/Squirmer.h"
 
+#include <core/logging/Logging.h>
+#include <core/UniqueID.h>
+
 namespace walberla {
 namespace pe {
 
@@ -38,32 +41,31 @@ SquirmerID createSquirmer( BodyStorage& globalStorage, BlockStorage& blocks, Blo
    if( radius <= real_c(0) )
       throw std::invalid_argument( "Invalid squirmer radius" );
 
-   SquirmerID squirmer = NULL;
+   SquirmerID squirmer = nullptr;
 
    if (global)
    {
       const id_t sid = UniqueID<RigidBody>::createGlobal();
       WALBERLA_ASSERT_EQUAL(communicating, false);
       WALBERLA_ASSERT_EQUAL(infiniteMass, true);
-      squirmer = new Squirmer(sid, uid, gpos, Vec3(0,0,0), Quat(), radius, squirmerVelocity, squirmerBeta, material, global, false, true);
-      globalStorage.add(squirmer);
+      SquirmerPtr sq = std::make_unique<Squirmer>(sid, uid, gpos, Vec3(0,0,0), Quat(), radius, squirmerVelocity, squirmerBeta, material, global, false, true);
+      squirmer = static_cast<SquirmerID>(&globalStorage.add(std::move(sq)));
    } else
    {
-      for (auto it = blocks.begin(); it != blocks.end(); ++it){
-         IBlock* block = (&(*it));
-         if (block->getAABB().contains(gpos))
+      for (auto& block : blocks){
+         if (block.getAABB().contains(gpos))
          {
             const id_t sid( UniqueID<RigidBody>::create() );
 
-            Storage* bs = block->getData<Storage>(storageID);
-            squirmer = new Squirmer(sid, uid, gpos, Vec3(0,0,0), Quat(), radius, squirmerVelocity, squirmerBeta, material, global, communicating, infiniteMass);
-            squirmer->MPITrait.setOwner(Owner(MPIManager::instance()->rank(), block->getId().getID()));
-            (*bs)[0].add(squirmer);
+            BodyStorage& bs = (*block.getData<Storage>(storageID))[0];
+            SquirmerPtr sq = std::make_unique<Squirmer>(sid, uid, gpos, Vec3(0,0,0), Quat(), radius, squirmerVelocity, squirmerBeta, material, global, communicating, infiniteMass);
+            sq->MPITrait.setOwner(Owner(MPIManager::instance()->rank(), block.getId().getID()));
+            squirmer = static_cast<SquirmerID>(&bs.add( std::move(sq) ));
          }
       }
    }
 
-   if (squirmer != NULL)
+   if (squirmer != nullptr)
    {
       // Logging the successful creation of the squirmer
       WALBERLA_LOG_DETAIL(
diff --git a/src/pe/rigidbody/StorageDataHandling.h b/src/pe/rigidbody/StorageDataHandling.h
index 6e3b0d2dafad64473a5b63f325e00bdccd49eb38..2d49a98c84b21d11d4c3bf51da230e62c648ebae 100644
--- a/src/pe/rigidbody/StorageDataHandling.h
+++ b/src/pe/rigidbody/StorageDataHandling.h
@@ -88,8 +88,8 @@ void StorageDataHandling<BodyTuple>::serialize( IBlock * const block, const Bloc
       {
          WALBERLA_ABORT( "Body to be stored not contained within block!" );
       }
-      marshal( buffer, RigidBodyCopyNotification( **bodyIt ) );
-      MarshalDynamically<BodyTuple>::execute( buffer, **bodyIt );
+      marshal( buffer, RigidBodyCopyNotification( *bodyIt ) );
+      MarshalDynamically<BodyTuple>::execute( buffer, *bodyIt );
    }
 }
 
@@ -146,8 +146,8 @@ void StorageDataHandling<BodyTuple>::serializeCoarseToFine( Block * const block,
       }
       if( childAABB.contains( bodyIt->getPosition()) )
       {
-         marshal( buffer, RigidBodyCopyNotification( **bodyIt ) );
-         MarshalDynamically<BodyTuple>::execute( buffer, **bodyIt );
+         marshal( buffer, RigidBodyCopyNotification( *bodyIt ) );
+         MarshalDynamically<BodyTuple>::execute( buffer, *bodyIt );
          ++numOfParticles;
       }
    }
@@ -166,8 +166,8 @@ void StorageDataHandling<BodyTuple>::serializeFineToCoarse( Block * const block,
       {
          WALBERLA_ABORT( "Body to be stored not contained within block!" );
       }
-      marshal( buffer, RigidBodyCopyNotification( **bodyIt ) );
-      MarshalDynamically<BodyTuple>::execute( buffer, **bodyIt );
+      marshal( buffer, RigidBodyCopyNotification( *bodyIt ) );
+      MarshalDynamically<BodyTuple>::execute( buffer, *bodyIt );
    }
 }
 
@@ -214,16 +214,16 @@ void StorageDataHandling<BodyTuple>::deserializeImpl( IBlock * const block, cons
       typename RigidBodyCopyNotification::Parameters objparam;
       unmarshal( buffer, objparam );
 
-      BodyID bd = UnmarshalDynamically<BodyTuple>::execute(buffer, objparam.geomType_, block->getBlockStorage().getDomain(), block->getAABB());
+      auto bd = UnmarshalDynamically<BodyTuple>::execute(buffer, objparam.geomType_, block->getBlockStorage().getDomain(), block->getAABB());
       bd->setRemote( false );
       bd->MPITrait.setOwner(Owner(MPIManager::instance()->rank(), block->getId().getID()));
 
       if ( !block->getAABB().contains( bd->getPosition()) )
       {
-         WALBERLA_ABORT("Loaded body not contained within block!\n" << "aabb: " << block->getAABB() << "\nparticle:" << bd );
+         WALBERLA_ABORT("Loaded body not contained within block!\n" << "aabb: " << block->getAABB() << "\nparticle:" << *bd );
       }
       WALBERLA_ASSERT_EQUAL(localBodyStorage.find( bd->getSystemID() ), localBodyStorage.end());
-      localBodyStorage.add(bd);
+      localBodyStorage.add(std::move(bd));
 
       --numBodies;
    }
diff --git a/src/pe/rigidbody/Union.h b/src/pe/rigidbody/Union.h
index 81c8128f6a4253f8d17f43801fef4a39f9e056b7..fe6e0ce94f1e975feaae4cea68b97aa6aa85ad1d 100644
--- a/src/pe/rigidbody/Union.h
+++ b/src/pe/rigidbody/Union.h
@@ -27,7 +27,10 @@
 //*************************************************************************************************
 
 #include <pe/Config.h>
+#include <pe/rigidbody/BodyStorage.h>
 #include <pe/rigidbody/RigidBody.h>
+#include <pe/rigidbody/RigidBodyCastIterator.h>
+#include <pe/rigidbody/RigidBodyIterator.h>
 #include <pe/Types.h>
 
 #include <core/debug/Debug.h>
@@ -65,23 +68,24 @@ class Union : public RigidBody
 {
 public:
    //**Type definitions****************************************************************************
-   typedef BodyTypeTuple                     BodyTypeTupleT;
+   using BodyTypeTupleT        = BodyTypeTuple;
 
-   typedef PtrVector<RigidBody, PtrDelete>   Bodies;             //!< Iterator over the contained rigid bodies.
-   typedef Bodies::Iterator                  Iterator;           //!< Iterator over the contained rigid bodies.
-   typedef Bodies::ConstIterator             ConstIterator;      //!< Constant iterator over the contained rigid bodies.
-   //**++++++++++++++++****************************************************************************
+   //**********************************************************************************************
 
-   //**Forward declarations for nested classes*****************************************************
-//   template< typename C > class CastIterator;
-//   template< typename C > class ConstCastIterator;
+   using size_type             = BodyStorage::size_type;           //!< Size type of the body storage.
+   using iterator              = BodyStorage::iterator;            //!< Iterator over non-const bodies.
+   using const_iterator        = BodyStorage::const_iterator;      //!< Iterator over constant bodies.
+   template <typename C> //cast type
+   using cast_iterator         = BodyStorage::cast_iterator<C>;
+   template <typename C> //cast type
+   using const_cast_iterator   = BodyStorage::const_cast_iterator<C>;
    //**********************************************************************************************
 
    //**Constructors********************************************************************************
    /*!\name Constructors */
    //@{
    explicit Union( id_t sid, id_t uid, const Vec3& gpos, const Vec3& rpos, const Quat& q,
-                    const bool global, const bool communicating, const bool infiniteMass );
+                   const bool global, const bool communicating, const bool infiniteMass );
    //@}
    //**********************************************************************************************
 
@@ -95,28 +99,35 @@ public:
 
 public:
    //**Get functions*******************************************************************************
-   /*!\name Get functions */
+   /*!\name BodyStorage functions */
    //@{
-                          inline bool                 isEmpty() const;
-                          inline size_t               size()    const;
-   template< typename C > inline size_t               size()    const {return bodies_.size<C>();}
-                          inline BodyID               getBody( size_t index );
-                          inline ConstBodyID          getBody( size_t index ) const;
-
-                          inline Iterator                     begin()       {return bodies_.begin();}
-                          inline ConstIterator                begin() const {return bodies_.begin();}
-   template< typename C > inline Bodies::CastIterator<C>      begin()       {return bodies_.begin<C>();}
-   template< typename C > inline Bodies::ConstCastIterator<C> begin() const {return bodies_.begin<C>();}
-
-                          inline Iterator                     end()         {return bodies_.end();}
-                          inline ConstIterator                end() const   {return bodies_.end();}
-   template< typename C > inline Bodies::CastIterator<C>      end()         {return bodies_.end<C>();}
-   template< typename C > inline Bodies::ConstCastIterator<C> end() const   {return bodies_.end<C>();}
-
-   virtual inline real_t getVolume()         const;
+   inline bool                 isEmpty() const {return bodies_.isEmpty();}
+   inline size_t               size()    const {return bodies_.size();}
+
+   inline iterator       begin   ()       {return bodies_.begin();}
+   inline const_iterator begin   () const {return bodies_.begin();}
+   inline const_iterator cbegin  () const {return bodies_.cbegin();}
+   inline iterator       end     ()       {return bodies_.end();}
+   inline const_iterator end     () const {return bodies_.end();}
+   inline const_iterator cend    () const {return bodies_.cend();}
+
+   template< typename C >
+   inline cast_iterator<C> begin()              {return bodies_.begin<C>();}
+   template< typename C >
+   inline const_cast_iterator<C> begin() const  {return bodies_.begin<C>();}
+   template< typename C >
+   inline const_cast_iterator<C> cbegin() const {return bodies_.cbegin<C>();}
+   template< typename C >
+   inline cast_iterator<C> end()                {return bodies_.end<C>();}
+   template< typename C >
+   inline const_cast_iterator<C> end() const    {return bodies_.end<C>();}
+   template< typename C >
+   inline const_cast_iterator<C> cend() const   {return bodies_.cend<C>();}
    //@}
    //**********************************************************************************************
 
+   virtual inline real_t getVolume()         const;
+
    //**Set functions*******************************************************************************
    /*!\name Set functions */
    //@{
@@ -146,7 +157,7 @@ public:
    //**Rigid body manager functions****************************************************************
    /*!\name Rigid body manager functions */
    //@{
-   void add   ( BodyID body );
+   inline RigidBody& add( std::unique_ptr<RigidBody>&& body );
    //@}
    //**********************************************************************************************
 
@@ -183,18 +194,13 @@ protected:
    /*!\name Utility functions */
    //@{
    inline virtual void calcBoundingBox() WALBERLA_OVERRIDE;  // Calculation of the axis-aligned bounding box
-                  void calcCenterOfMass(); ///< updates the center of mass (gpos)
+   void calcCenterOfMass(); ///< updates the center of mass (gpos)
    inline         void calcInertia();      // Calculation of the moment of inertia
    //@}
    //**********************************************************************************************
 
-   //**Member variables****************************************************************************
-   /*!\name Member variables */
-   //@{
-   PtrVector<RigidBody, PtrDelete> bodies_;  //!< Rigid bodies contained in the union.
-   //@}
-   //**********************************************************************************************
 private:
+   BodyStorage bodies_;  //!< Rigid bodies contained in the union.
    std::vector<id_t> containedTypeIDs_;
 
    static id_t staticTypeID_;  //< type id of sphere, will be set by SetBodyTypeIDs
@@ -228,7 +234,7 @@ private:
  */
 template <typename BodyTypeTuple>
 Union<BodyTypeTuple>::Union( id_t sid, id_t uid, const Vec3& gpos, const Vec3& rpos, const Quat& q,
-                const bool global, const bool communicating, const bool /*infiniteMass*/ )
+                             const bool global, const bool communicating, const bool /*infiniteMass*/ )
    : RigidBody( getStaticTypeID(), sid, uid )  // Initialization of the parent class
 {
    // Initializing the instantiated union
@@ -241,7 +247,7 @@ Union<BodyTypeTuple>::Union( id_t sid, id_t uid, const Vec3& gpos, const Vec3& r
    calcInertia();
 
    setGlobal( global );
-//   setMass( infiniteMass );
+   //   setMass( infiniteMass );
    setCommunicating( communicating );
    setFinite( true );
 }
@@ -322,7 +328,7 @@ void Union<BodyTypeTuple>::calcBoundingBox()
    // Setting the bounding box of an empty union
    if( bodies_.isEmpty() ) {
       aabb_ = math::AABB(
-            gpos_[0] - real_t(0.01),
+                 gpos_[0] - real_t(0.01),
             gpos_[1] - real_t(0.01),
             gpos_[2] - real_t(0.01),
             gpos_[0] + real_t(0.01),
@@ -334,7 +340,7 @@ void Union<BodyTypeTuple>::calcBoundingBox()
    // Using the bounding box of the first contained bodies as initial bounding box
    // and merging it with the bounding boxes of all other bodies
    else {
-      aabb_ = bodies_[0]->getAABB();
+      aabb_ = bodies_.begin()->getAABB();
       for( auto b=bodies_.begin()+1; b!=bodies_.end(); ++b )
          aabb_.merge( b->getAABB() );
    }
@@ -366,7 +372,7 @@ void Union<BodyTypeTuple>::calcCenterOfMass()
    // Calculating the center of mass of a single body
    if( bodies_.size() == 1 )
    {
-      const BodyID body( bodies_[0] );
+      const BodyID body( bodies_.begin().getBodyID() );
       gpos_ = body->getPosition();
       mass_ = body->getMass();
       if( !isFixed() && mass_ > real_t(0) )
@@ -512,8 +518,8 @@ void Union<BodyTypeTuple>::setPositionImpl( real_t px, real_t py, real_t pz )
    gpos_ = gpos;
 
    // Updating the contained bodies
-   for( auto b=bodies_.begin(); b!=bodies_.end(); ++b )
-      (*b)->update( dp );
+   for( auto& b : bodies_ )
+      b.update( dp );
 
    Union<BodyTypeTuple>::calcBoundingBox();    // Setting the axis-aligned bounding box
    wake();               // Waking the union from sleep mode
@@ -548,8 +554,8 @@ void Union<BodyTypeTuple>::setOrientationImpl( real_t r, real_t i, real_t j, rea
    R_ = q_.toRotationMatrix();
 
    // Updating the contained bodies
-   for( auto b=bodies_.begin(); b!=bodies_.end(); ++b )
-      (*b)->update( dq );
+   for( auto& b : bodies_ )
+      b.update( dq );
 
    Union<BodyTypeTuple>::calcBoundingBox();  // Setting the axis-aligned bounding box
    wake();             // Waking the union from sleep mode
@@ -586,8 +592,8 @@ void Union<BodyTypeTuple>::update( const Vec3& dp )
    gpos_ += dp;
 
    // Updating the contained bodies
-   for( auto b=bodies_.begin(); b!=bodies_.end(); ++b )
-      (*b)->update( dp );
+   for( auto& b : bodies_ )
+      b.update( dp );
 
    // Setting the axis-aligned bounding box
    Union<BodyTypeTuple>::calcBoundingBox();
@@ -623,8 +629,8 @@ void Union<BodyTypeTuple>::update( const Quat& dq )
    R_ = q_.toRotationMatrix();
 
    // Updating the contained bodies
-   for( auto b=bodies_.begin(); b!=bodies_.end(); ++b )
-      (*b)->update( dq );
+   for( auto& b : bodies_ )
+      b.update( dq );
 
    // Setting the axis-aligned bounding box
    Union<BodyTypeTuple>::calcBoundingBox();
@@ -694,38 +700,38 @@ void Union<BodyTypeTuple>::update( const Quat& dq )
  *    union (to make the entire union (in-)visible.
  */
 template <typename BodyTypeTuple>
-void Union<BodyTypeTuple>::add( BodyID body )
+RigidBody& Union<BodyTypeTuple>::add( std::unique_ptr<RigidBody>&& body )
 {
    // Checking for "self-assignment"
-   if( body == BodyID( this ) ) return;
+   if( body.get() == BodyID( this ) ) return *this;
 
    // Checking the global flags of the body and the union in MPI parallel simulations
    if( body->isGlobal() ^ global_ )
       throw std::logic_error( "Global flags of body and union do not match" );
 
+   // Registering the rigid body
+   auto& bd = bodies_.add( std::move(body) );
+
    Vec3 oldCenterOfMass = getPosition();
    Vec3 oldImpulse      = getLinearVel() * getMass();
 
-   Vec3 bodyCenterOfMass = body->getPosition();
-   Vec3 bodyImpulse      = body->getLinearVel() * body->getMass();
+   Vec3 bodyCenterOfMass = bd.getPosition();
+   Vec3 bodyImpulse      = bd.getLinearVel() * bd.getMass();
 
-
-   // Registering the rigid body
-   body->setSB(this);
-   bodies_.pushBack( body );
+   bd.setSB(this); //having a superbody will forward all getVel calls to superbody!!!
 
    // Updating the axis-aligned bounding box
    if( bodies_.size() == 1 )
-      aabb_ = body->getAABB();
+      aabb_ = bd.getAABB();
    else
-      aabb_.merge( body->getAABB() );
+      aabb_.merge( bd.getAABB() );
 
    // Setting the union's total mass and center of mass
    calcCenterOfMass();
 
    // Setting the contained primitives' relative position in reference to the center of mass
-   for( auto b=bodies_.begin(); b!=bodies_.end(); ++b )
-      ( *b )->calcRelPosition();
+   for( auto& b : bodies_ )
+      b.calcRelPosition();
 
    // Setting the moment of inertia
    calcInertia();
@@ -735,11 +741,13 @@ void Union<BodyTypeTuple>::add( BodyID body )
    setAngularVel( Vec3(0,0,0) );
    addImpulseAtPos( oldImpulse, oldCenterOfMass);
    addImpulseAtPos( bodyImpulse, bodyCenterOfMass);
-   body->setLinearVel( Vec3(0,0,0) );
-   body->setAngularVel( Vec3(0,0,0) );
+   bd.setLinearVel( Vec3(0,0,0) );
+   bd.setAngularVel( Vec3(0,0,0) );
 
    // Signaling the internal modification to the superordinate body
    signalModification();
+
+   return bodies_.back();
 }
 //*************************************************************************************************
 
@@ -771,8 +779,8 @@ void Union<BodyTypeTuple>::translateImpl( real_t dx, real_t dy, real_t dz )
    gpos_ += dp;
 
    // Updating the contained bodies
-   for( auto b=bodies_.begin(); b!=bodies_.end(); ++b )
-      (*b)->update( dp );
+   for( auto& b : bodies_ )
+      b.update( dp );
 
    Union<BodyTypeTuple>::calcBoundingBox();    // Setting the axis-aligned bounding box
    wake();               // Waking the union from sleep mode
@@ -811,8 +819,8 @@ void Union<BodyTypeTuple>::rotateImpl( const Quat& dq )
    R_ = q_.toRotationMatrix();  // Updating the rotation of the union
 
    // Updating the contained bodies
-   for( auto b=bodies_.begin(); b!=bodies_.end(); ++b )
-      (*b)->update( dq );
+   for( auto& b : bodies_ )
+      b.update( dq );
 
    Union<BodyTypeTuple>::calcBoundingBox();  // Setting the axis-aligned bounding box
    wake();             // Waking the union from sleep mode
@@ -843,8 +851,8 @@ void Union<BodyTypeTuple>::rotateAroundOriginImpl( const Quat& dq )
    gpos_ = dq.rotate( gpos_ );     // Updating the global position of the union
 
    // Updating the contained bodies
-   for( auto b=bodies_.begin(); b!=bodies_.end(); ++b )
-      (*b)->update( dq );
+   for( auto& b : bodies_ )
+      b.update( dq );
 
    Union<BodyTypeTuple>::calcBoundingBox();    // Setting the axis-aligned bounding box
    wake();               // Waking the union from sleep mode
@@ -876,8 +884,8 @@ void Union<BodyTypeTuple>::rotateAroundPointImpl( const Vec3& point, const Quat
    gpos_ = point + dq.rotate( dp );  // Updating the global position of the union
 
    // Updating the contained bodies
-   for( auto b=bodies_.begin(); b!=bodies_.end(); ++b )
-      (*b)->update( dq );
+   for( auto& b : bodies_ )
+      b.update( dq );
 
    Union<BodyTypeTuple>::calcBoundingBox();    // Setting the axis-aligned bounding box
    wake();               // Waking the union from sleep mode
@@ -906,8 +914,8 @@ template <typename BodyTypeTuple>
 bool Union<BodyTypeTuple>::containsRelPointImpl( real_t px, real_t py, real_t pz ) const
 {
    const Vec3 gpos( pointFromBFtoWF( px, py, pz ) );
-   for( auto b=bodies_.begin(); b!=bodies_.end(); ++b )
-      if( b->containsPoint( gpos ) ) return true;
+   for( auto& b : bodies_ )
+      if( b.containsPoint( gpos ) ) return true;
    return false;
 }
 //*************************************************************************************************
@@ -929,9 +937,10 @@ bool Union<BodyTypeTuple>::isSurfaceRelPointImpl( real_t px, real_t py, real_t p
    bool surface( false );
    const Vec3 gpos( pointFromBFtoWF( px, py, pz ) );
 
-   for( auto b=bodies_.begin(); b!=bodies_.end(); ++b ) {
-      if( b->containsPoint( gpos ) ) return false;
-      else if( b->isSurfacePoint( gpos ) ) surface = true;
+   for( auto& b : bodies_ )
+   {
+      if( b.containsPoint( gpos ) ) return false;
+      else if( b.isSurfacePoint( gpos ) ) surface = true;
    }
 
    return surface;
@@ -974,8 +983,8 @@ void Union<BodyTypeTuple>::handleModification()
    calcCenterOfMass();
 
    // Setting the contained primitives' relative position in reference to the center of mass
-   for( auto b=bodies_.begin(); b!=bodies_.end(); ++b )
-      ( *b )->calcRelPosition();
+   for( auto& b : bodies_ )
+      b.calcRelPosition();
 
    // Setting the moment of inertia
    calcInertia();
@@ -1004,8 +1013,8 @@ void Union<BodyTypeTuple>::handleTranslation()
    calcCenterOfMass();
 
    // Setting the contained bodies' relative position in reference to the center of mass
-   for( auto b=bodies_.begin(); b!=bodies_.end(); ++b )
-      ( *b )->calcRelPosition();
+   for( auto& b : bodies_ )
+      b.calcRelPosition();
 
    // Setting the moment of inertia
    calcInertia();
@@ -1155,65 +1164,6 @@ std::ostream& operator<<( std::ostream& os, Union<BodyTypeTuple> const * u )
 //
 //=================================================================================================
 
-//*************************************************************************************************
-/*!\brief Returns whether the union contains any bodies or not.
- *
- * \return \a true if the union is empty, \a false if it not.
- */
-template <typename BodyTypeTuple>
-inline bool Union<BodyTypeTuple>::isEmpty() const
-{
-   return bodies_.size() == 0;
-}
-//*************************************************************************************************
-
-
-//*************************************************************************************************
-/*!\brief Returns the number of rigid bodies contained in the union.
- *
- * \return The number of rigid bodies.
- */
-template <typename BodyTypeTuple>
-inline size_t Union<BodyTypeTuple>::size() const
-{
-   return bodies_.size();
-}
-//*************************************************************************************************
-
-
-//*************************************************************************************************
-/*!\brief Returns a handle to the indexed rigid body.
- *
- * \param index Access index. The index has to be in the range \f$[0..size-1]\f$.
- * \return Handle to the accessed body.
- *
- * \b Note: No runtime check is performed to insure the validity of the access index.
- */
-template <typename BodyTypeTuple>
-inline BodyID Union<BodyTypeTuple>::getBody( size_t index )
-{
-   WALBERLA_ASSERT_LESS( index, bodies_.size(), "Invalid access index" );
-   return bodies_[index];
-}
-//*************************************************************************************************
-
-
-//*************************************************************************************************
-/*!\brief Returns a constant handle to the indexed rigid body.
- *
- * \param index Access index. The index has to be in the range \f$[0..size-1]\f$.
- * \return Constant handle to the accessed body.
- *
- * \b Note: No runtime check is performed to insure the validity of the access index.
- */
-template <typename BodyTypeTuple>
-inline ConstBodyID Union<BodyTypeTuple>::getBody( size_t index ) const
-{
-   WALBERLA_ASSERT_LESS( index, bodies_.size(), "Invalid access index" );
-   return bodies_[index];
-}
-//*************************************************************************************************
-
 
 //*************************************************************************************************
 /*!\brief Returns unique type id of this type
diff --git a/src/pe/rigidbody/UnionFactory.h b/src/pe/rigidbody/UnionFactory.h
index e237e0dd31d83642f80663ed48b7c083498cad63..eeff2ffe8c578fe05924c8fea440afb89b33e5e2 100644
--- a/src/pe/rigidbody/UnionFactory.h
+++ b/src/pe/rigidbody/UnionFactory.h
@@ -31,8 +31,10 @@
 #include "pe/rigidbody/Union.h"
 #include "pe/Types.h"
 
-#include "blockforest/BlockForest.h"
-#include "core/debug/Debug.h"
+#include <blockforest/BlockForest.h>
+#include <core/debug/Debug.h>
+#include <core/logging/Logging.h>
+#include <core/UniqueID.h>
 
 namespace walberla {
 namespace pe {
@@ -73,32 +75,31 @@ Union<BodyTypeTuple>* createUnion(   BodyStorage& globalStorage, BlockStorage& b
    if (Union<BodyTypeTuple>::getStaticTypeID() == std::numeric_limits<id_t>::max())
       throw std::runtime_error("Union TypeID not initalized!");
 
-   Union<BodyTypeTuple>* bd = NULL;
+   Union<BodyTypeTuple>* bd = nullptr;
 
    if (global)
    {
       const id_t sid = UniqueID<RigidBody>::createGlobal();
       WALBERLA_ASSERT_EQUAL(communicating, false);
       WALBERLA_ASSERT_EQUAL(infiniteMass, true);
-      bd = new Union<BodyTypeTuple>(sid, uid, gpos, Vec3(0,0,0), Quat(), global, false, true);
-      globalStorage.add(bd);
+      auto un = std::make_unique<Union<BodyTypeTuple>>(sid, uid, gpos, Vec3(0,0,0), Quat(), global, false, true);
+      bd = static_cast<Union<BodyTypeTuple>*>(&globalStorage.add(std::move(un)));
    } else
    {
-      for (auto it = blocks.begin(); it != blocks.end(); ++it){
-         IBlock* block = (&(*it));
-         if (block->getAABB().contains(gpos))
+      for (auto& block : blocks){
+         if (block.getAABB().contains(gpos))
          {
             const id_t sid( UniqueID<RigidBody>::create() );
 
-            Storage* bs = block->getData<Storage>(storageID);
-            bd = new Union<BodyTypeTuple>(sid, uid, gpos, Vec3(0,0,0), Quat(), global, communicating, infiniteMass);
-            bd->MPITrait.setOwner(Owner(MPIManager::instance()->rank(), block->getId().getID()));
-            (*bs)[0].add(bd);
+            BodyStorage& bs = (*block.getData<Storage>(storageID))[0];
+            auto un = std::make_unique<Union<BodyTypeTuple>>(sid, uid, gpos, Vec3(0,0,0), Quat(), global, communicating, infiniteMass);
+            un->MPITrait.setOwner(Owner(MPIManager::instance()->rank(), block.getId().getID()));
+            bd = static_cast<Union<BodyTypeTuple>*>(&bs.add(std::move(un)));
          }
       }
    }
 
-   if (bd != NULL)
+   if (bd != nullptr)
    {
       // Logging the successful creation of the box
       WALBERLA_LOG_DETAIL(
@@ -143,7 +144,6 @@ BoxID createBox( Union<BodyTypeTuple>* un,
    if( lengths[0] <= real_t(0) || lengths[1] <= real_t(0) || lengths[2] <= real_t(0) )
       throw std::invalid_argument( "Invalid side length" );
 
-   BoxID box = NULL;
    id_t  sid = 0;
 
    if (global)
@@ -156,9 +156,8 @@ BoxID createBox( Union<BodyTypeTuple>* un,
       sid = UniqueID<RigidBody>::create();
    }
 
-   box = new Box(sid, uid, gpos, Vec3(0,0,0), Quat(), lengths, material, global, communicating, infiniteMass);
+   std::unique_ptr<Box> box = std::make_unique<Box>(sid, uid, gpos, Vec3(0,0,0), Quat(), lengths, material, global, communicating, infiniteMass);
    box->MPITrait.setOwner( un->MPITrait.getOwner() );
-   un->add(box);
 
    if (box != NULL)
    {
@@ -173,7 +172,7 @@ BoxID createBox( Union<BodyTypeTuple>* un,
                );
    }
 
-   return box;
+   return static_cast<BoxID> (&(un->add(std::move(box))));
 }
 
 //*************************************************************************************************
@@ -215,7 +214,6 @@ CapsuleID createCapsule( Union<BodyTypeTuple>* un,
    if( length <= real_c(0) )
       throw std::invalid_argument( "Invalid capsule length" );
 
-   CapsuleID capsule = NULL;
    id_t      sid     = 0;
 
    if (global)
@@ -228,16 +226,15 @@ CapsuleID createCapsule( Union<BodyTypeTuple>* un,
       sid = UniqueID<RigidBody>::create();
    }
 
-   capsule = new Capsule(sid, uid, gpos, Vec3(0,0,0), Quat(), radius, length, material, global, communicating, infiniteMass);
+   std::unique_ptr<Capsule> capsule = std::make_unique<Capsule>(sid, uid, gpos, Vec3(0,0,0), Quat(), radius, length, material, global, communicating, infiniteMass);
    capsule->MPITrait.setOwner( un->MPITrait.getOwner() );
-   un->add(capsule);
 
    if (capsule != NULL)
    {
-      WALBERLA_LOG_DETAIL("Created capsule " << capsule->getSystemID() << "\n" << capsule);
+      WALBERLA_LOG_DETAIL("Created capsule " << capsule->getSystemID() << "\n" << *capsule);
    }
 
-   return capsule;
+   return static_cast<CapsuleID>(&(un->add(std::move(capsule))));
 }
 
 //*************************************************************************************************
@@ -275,7 +272,6 @@ SphereID createSphere( Union<BodyTypeTuple>* un,
       throw std::invalid_argument( "Invalid sphere radius" );
 
    id_t sid(0);
-   SphereID sphere = NULL;
 
    if (global)
    {
@@ -288,9 +284,8 @@ SphereID createSphere( Union<BodyTypeTuple>* un,
       sid = UniqueID<RigidBody>::create();
    }
 
-   sphere = new Sphere(sid, uid, gpos, Vec3(0,0,0), Quat(), radius, material, global, communicating, infiniteMass);
+   std::unique_ptr<Sphere> sphere = std::make_unique<Sphere>(sid, uid, gpos, Vec3(0,0,0), Quat(), radius, material, global, communicating, infiniteMass);
    sphere->MPITrait.setOwner( un->MPITrait.getOwner() );
-   un->add( sphere );
 
    if (sphere != NULL)
    {
@@ -305,7 +300,7 @@ SphereID createSphere( Union<BodyTypeTuple>* un,
                );
    }
 
-   return sphere;
+   return static_cast<SphereID>(&(un->add( std::move(sphere) )));
 }
 
 }  // namespace pe
diff --git a/src/pe/synchronization/RemoveAndNotify.h b/src/pe/synchronization/RemoveAndNotify.h
index 8eaf880fe6e80157b7199d37dbf5b75606925a53..6c8d27c0d89e6e7ec2f9cc34464f9c28c3593d4e 100644
--- a/src/pe/synchronization/RemoveAndNotify.h
+++ b/src/pe/synchronization/RemoveAndNotify.h
@@ -44,7 +44,7 @@ namespace pe {
  * This function removes the rigid body from the body storage and generates deletion notifications.
  */
 inline
-BodyStorage::Iterator removeAndNotify( Owner me, mpi::BufferSystem& bs, BodyStorage& localStorage, BodyStorage::Iterator body )
+BodyStorage::iterator removeAndNotify( Owner me, mpi::BufferSystem& bs, BodyStorage& localStorage, BodyStorage::iterator body )
 {
    using namespace walberla::pe::communication;
 
@@ -57,7 +57,7 @@ BodyStorage::Iterator removeAndNotify( Owner me, mpi::BufferSystem& bs, BodyStor
          WALBERLA_LOG_DETAIL( "__Notify registered process " << (*it) << " of deletion of body " << body->getSystemID() );
          mpi::SendBuffer& sb = bs.sendBuffer(it->rank_);
          if (sb.isEmpty()) sb << walberla::uint8_c(0);
-         packNotification(me, *it, sb, RigidBodyDeletionNotification( **body ));
+         packNotification(me, *it, sb, RigidBodyDeletionNotification( *body ));
       }
    }
 
diff --git a/src/pe/synchronization/SyncForces.h b/src/pe/synchronization/SyncForces.h
index 0bca535418797cfdfdf9f18979a169f607f80e87..aa83e65196722fd0fea23a262671ea97919de3ff 100644
--- a/src/pe/synchronization/SyncForces.h
+++ b/src/pe/synchronization/SyncForces.h
@@ -74,7 +74,7 @@ void reduceForces( BlockStorage& blocks, BlockDataID storageID )
 
          WALBERLA_LOG_DETAIL( "__Sending force contribution " << bodyIt->getForce() << ", " << bodyIt->getTorque() << " of body " <<
                               bodyIt->getSystemID() << " to owner block " << bodyIt->MPITrait.getOwner().blockID_ << ".\n");
-         packNotification(me, bodyIt->MPITrait.getOwner(), sb, RigidBodyForceNotification( *(*bodyIt) ) );
+         packNotification(me, bodyIt->MPITrait.getOwner(), sb, RigidBodyForceNotification( *bodyIt ) );
 
       }
 
@@ -135,7 +135,7 @@ void reduceForces( BlockStorage& blocks, BlockDataID storageID, BodyStorage& glo
       {
          if (it->hasInfiniteMass()) continue;
 
-         const Vec3 f( (*it)->getForce() ), tau( (*it)->getTorque() );
+         const Vec3 f( it->getForce() ), tau( it->getTorque() );
          reductionBuffer[i++] = f[0];
          reductionBuffer[i++] = f[1];
          reductionBuffer[i++] = f[2];
@@ -150,8 +150,8 @@ void reduceForces( BlockStorage& blocks, BlockDataID storageID, BodyStorage& glo
       for( auto it = globalBodyStorage.begin(); it != globalBodyStorage.end(); ++it )
       {
          if (it->hasInfiniteMass()) continue;
-         (*it)->setForce ( Vec3( reductionBuffer[i], reductionBuffer[i + 1], reductionBuffer[i + 2] ) );
-         (*it)->setTorque( Vec3( reductionBuffer[i + 3], reductionBuffer[i + 4], reductionBuffer[i + 5] ) );
+         it->setForce ( Vec3( reductionBuffer[i], reductionBuffer[i + 1], reductionBuffer[i + 2] ) );
+         it->setTorque( Vec3( reductionBuffer[i + 3], reductionBuffer[i + 4], reductionBuffer[i + 5] ) );
       }
    }
    WALBERLA_LOG_DETAIL( "Sync force on global bodies finished." );
@@ -198,7 +198,7 @@ void distributeForces( BlockStorage& blocks, BlockDataID storageID )
 
             WALBERLA_LOG_DETAIL( "__Sending force contribution " << bodyIt->getForce() << ", " << bodyIt->getTorque() << " of body " <<
                                  bodyIt->getSystemID() << " to shadow owner " << sownerIt->blockID_ << ".\n");
-            packNotification(me, *sownerIt, sb, RigidBodyForceNotification( *(*bodyIt) ) );
+            packNotification(me, *sownerIt, sb, RigidBodyForceNotification( *bodyIt ) );
          }
       }
 
diff --git a/src/pe/synchronization/SyncNextNeighbors.h b/src/pe/synchronization/SyncNextNeighbors.h
index 56da2bc958e39edf2994882fcc9e2b69fd5d2c77..63aa9a2081db232e11a8677cca8eec6253c7fd40 100644
--- a/src/pe/synchronization/SyncNextNeighbors.h
+++ b/src/pe/synchronization/SyncNextNeighbors.h
@@ -67,7 +67,7 @@ void generateSynchonizationMessages(mpi::BufferSystem& bs, const Block& block, B
       }
 
       const Vec3 gpos( body->getPosition() );
-      BodyID     b   ( *body );
+      BodyID     b   ( body.getBodyID() );
 
       if (body->isMarkedForDeletion())
       {
@@ -155,7 +155,7 @@ void generateSynchonizationMessages(mpi::BufferSystem& bs, const Block& block, B
             WALBERLA_LOG_DETAIL( "Sending deletion notifications for body " << body->getSystemID() << " due to outflow." );
 
             // Registered processes receive removal notification in the remove() routine.
-            //todelete.push_back( *body );
+            //todelete.push_back( body.getBodyID() );
             body = removeAndNotify( me, bs, localStorage, body );
 
             // Note: Attached shadow copies are not deleted here. Instead we rely on the deferred deletion since we no
@@ -189,8 +189,7 @@ void generateSynchonizationMessages(mpi::BufferSystem& bs, const Block& block, B
             b->setRemote( true );
 
             // Move body to shadow copy storage.
-            body = localStorage.release( body );
-            shadowStorage.add( b );
+            shadowStorage.add( localStorage.release( body ) );
 
             // Note: We cannot determine here whether we require the body since we do not have up to date shadow copies.
             // However, we will most likely have to keep the body since it typically still intersects the process subdomain.
diff --git a/src/pe/synchronization/SyncShadowOwners.h b/src/pe/synchronization/SyncShadowOwners.h
index e02cea958b435d39f54e749aa237c696cec6d8ca..cc0400d11d50f78597a9be5fd53666c84f386ae0 100644
--- a/src/pe/synchronization/SyncShadowOwners.h
+++ b/src/pe/synchronization/SyncShadowOwners.h
@@ -79,7 +79,7 @@ void updateAndMigrate( BlockForest& forest, BlockDataID storageID, const bool sy
 
       for( auto bodyIt = localStorage.begin(); bodyIt != localStorage.end(); )
       {
-         BodyID b (*bodyIt);
+         BodyID b (bodyIt.getBodyID());
 
          if( !b->isCommunicating() && !syncNonCommunicatingBodies ) {
             ++bodyIt;
@@ -131,8 +131,7 @@ void updateAndMigrate( BlockForest& forest, BlockDataID storageID, const bool sy
                b->setRemote( true );
 
                // Move body to shadow copy storage.
-               bodyIt = localStorage.release( bodyIt );
-               shadowStorage.add( b );
+               shadowStorage.add( localStorage.release( bodyIt ) );
 
                b->MPITrait.deregisterShadowOwner( owner );
 
@@ -220,7 +219,7 @@ void checkAndResolveOverlap( BlockForest& forest, BlockDataID storageID, const r
 
       for( auto bodyIt = localStorage.begin(); bodyIt != localStorage.end(); ++bodyIt)
       {
-         BodyID b (*bodyIt);
+         BodyID b (bodyIt.getBodyID());
 
          if( !b->isCommunicating() && !syncNonCommunicatingBodies ) continue;
 
@@ -256,7 +255,7 @@ void checkAndResolveOverlap( BlockForest& forest, BlockDataID storageID, const r
       }
       for( auto bodyIt = shadowStorage.begin(); bodyIt != shadowStorage.end(); )
       {
-         BodyID b (*bodyIt);
+         BodyID b (bodyIt.getBodyID());
          WALBERLA_ASSERT(!b->isGlobal(), "Global body in ShadowStorage!");
          bool isInsideDomain = forest.getDomain().contains( b->getAABB(), -dx );
 
diff --git a/src/pe/utility/CreateWorld.cpp b/src/pe/utility/CreateWorld.cpp
index aa7df4ea465ba248072214d637bfd28c3af1df97..919840085ee8b298083a7fcafa13c51488c5fd9a 100644
--- a/src/pe/utility/CreateWorld.cpp
+++ b/src/pe/utility/CreateWorld.cpp
@@ -79,7 +79,7 @@ std::unique_ptr<SetupBlockForest> createSetupBlockForest(const math::AABB simula
       blocks[2] = 2;
    }
 
-   auto sforest = std::unique_ptr<SetupBlockForest>( new SetupBlockForest() );
+   auto sforest = std::make_unique<SetupBlockForest>( );
    sforest->addWorkloadMemorySUIDAssignmentFunction( blockforest::uniformWorkloadAndMemoryAssignment );
    sforest->addRefinementSelectionFunction( FixedRefinementLevelSelector(initialRefinementLevel) );
    sforest->init( simulationDomain, blocks[0], blocks[1], blocks[2], isPeriodic[0], isPeriodic[1], isPeriodic[2] );
@@ -103,7 +103,7 @@ shared_ptr<BlockForest> createBlockForest(const math::AABB simulationDomain,
    }
 
    std::unique_ptr<SetupBlockForest> sforest( createSetupBlockForest( simulationDomain, blocks, isPeriodic, numberOfProcesses, initialRefinementLevel ));
-   return shared_ptr< BlockForest >( new BlockForest( uint_c( MPIManager::instance()->rank() ), *sforest, false ) );
+   return std::make_shared< BlockForest >( uint_c( MPIManager::instance()->rank() ), *sforest, false );
 }
 
 shared_ptr<BlockForest> createBlockForest(const math::AABB simulationDomain,
@@ -142,7 +142,7 @@ shared_ptr<BlockForest> createBlockForest(const math::AABB simulationDomain,
 
    WALBERLA_LOG_INFO_ON_ROOT( "Production Run!" );
    WALBERLA_LOG_INFO_ON_ROOT( "Creating the block structure: loading from file \'" << sbffile << "\' ..." );
-   return shared_ptr< BlockForest >( new BlockForest( uint_c( MPIManager::instance()->rank() ), sbffile.c_str(), true, false ) );
+   return std::make_shared< BlockForest >( uint_c( MPIManager::instance()->rank() ), sbffile.c_str(), true, false );
 }
 
 
diff --git a/src/pe/utility/DestroyBody.h b/src/pe/utility/DestroyBody.h
index 189203fda04a9ed28db4049a2e1044ee8078bf39..20da0f1c86a8d84596e674adc1045b89c9b1ce13 100644
--- a/src/pe/utility/DestroyBody.h
+++ b/src/pe/utility/DestroyBody.h
@@ -49,7 +49,7 @@ void destroyBody(BodyStorage& globalStorage, BlockStorage& blocks, BlockDataID s
    {
       for (auto bodyIt = globalStorage.begin(); bodyIt != globalStorage.end(); )
       {
-         if ( p(*bodyIt) )
+         if ( p(bodyIt.getBodyID()) )
          {
             bodyIt = globalStorage.remove( bodyIt );
          } else
@@ -68,7 +68,7 @@ void destroyBody(BodyStorage& globalStorage, BlockStorage& blocks, BlockDataID s
          BodyStorage& localStorage = storage[StorageType::LOCAL];
          for (auto bodyIt = localStorage.begin(); bodyIt != localStorage.end(); )
          {
-            if ( p(*bodyIt) )
+            if ( p(bodyIt.getBodyID()) )
             {
                bodyIt = localStorage.remove( bodyIt );
             } else
@@ -82,7 +82,7 @@ void destroyBody(BodyStorage& globalStorage, BlockStorage& blocks, BlockDataID s
          BodyStorage& shadowStorage = storage[StorageType::SHADOW];
          for (auto bodyIt = shadowStorage.begin(); bodyIt != shadowStorage.end(); )
          {
-            if ( p(*bodyIt) )
+            if ( p(bodyIt.getBodyID()) )
             {
                bodyIt = shadowStorage.remove( bodyIt );
             } else
diff --git a/src/pe/utility/GetBody.cpp b/src/pe/utility/GetBody.cpp
index 31fc394cc8e5f3d30110fea8a342c9d639f43677..6d0b695f85915d2cae66ef9fb573420462b9a2ce 100644
--- a/src/pe/utility/GetBody.cpp
+++ b/src/pe/utility/GetBody.cpp
@@ -30,7 +30,7 @@ BodyID getBody(BodyStorage& globalStorage, BlockStorage& blocks, BlockDataID sto
       auto bodyIt = globalStorage.find(sid);
       if (bodyIt != globalStorage.end())
       {
-         return *bodyIt;
+         return bodyIt.getBodyID();
       }
    }
 
@@ -44,7 +44,7 @@ BodyID getBody(BodyStorage& globalStorage, BlockStorage& blocks, BlockDataID sto
          auto bodyIt = localStorage.find(sid);
          if (bodyIt != localStorage.end())
          {
-            return *bodyIt;
+            return bodyIt.getBodyID();
          }
       }
       if (storageSelect & StorageSelect::SHADOW)
@@ -53,7 +53,7 @@ BodyID getBody(BodyStorage& globalStorage, BlockStorage& blocks, BlockDataID sto
          auto bodyIt = shadowStorage.find(sid);
          if (bodyIt != shadowStorage.end())
          {
-            return *bodyIt;
+            return bodyIt.getBodyID();
          }
       }
    }
diff --git a/src/pe/vtk/BodyVtkOutput.cpp b/src/pe/vtk/BodyVtkOutput.cpp
index b2e154dc1248df49ad1a76db4fbf48814beb1137..6d13f3df3bc051fa85bfdb7e558d1d265f667cbd 100644
--- a/src/pe/vtk/BodyVtkOutput.cpp
+++ b/src/pe/vtk/BodyVtkOutput.cpp
@@ -44,14 +44,14 @@ std::vector< DefaultBodyVTKOutput::Attributes > DefaultBodyVTKOutput::getAttribu
 void DefaultBodyVTKOutput::configure()
 {
    bodies_.clear();
-   for( auto blockIt = blockStorage_.begin(); blockIt != blockStorage_.end(); ++blockIt )
+   for( const auto& block : blockStorage_ )
    {
 
-      const Storage& bs = *(blockIt->getData<const Storage>( storageID_ ));
+      const BodyStorage& bs = (*(block.getData<const Storage>( storageID_ )))[0];
 
-      for( auto it = bs[0].begin(); it != bs[0].end(); ++it )
+      for( const auto& body : bs )
       {
-         bodies_.push_back( *it );
+         bodies_.push_back( &body );
       }
    }
 }
diff --git a/src/pe/vtk/EllipsoidVtkOutput.cpp b/src/pe/vtk/EllipsoidVtkOutput.cpp
index 4ec85db3819595a1300a9999295d44ab91182b8d..816fb2028208731672f30f2cb8cb3e98dfadcdb0 100644
--- a/src/pe/vtk/EllipsoidVtkOutput.cpp
+++ b/src/pe/vtk/EllipsoidVtkOutput.cpp
@@ -47,16 +47,16 @@ void EllipsoidVtkOutput::configure()
    bodies_.clear();
    tensorGlyphs_.clear();
 
-   for( auto blockIt = blockStorage_.begin(); blockIt != blockStorage_.end(); ++blockIt )
+   for( auto& block : blockStorage_ )
    {
 
-      const Storage& bs = *(blockIt->getData<const Storage>( storageID_ ));
+      const BodyStorage& localStorage = (*(block.getData<const Storage>( storageID_ )))[0];
 
-      for( auto it = bs[0].begin(); it != bs[0].end(); ++it )
+      for( auto& body : localStorage )
       {
-         if (it->getTypeID() == Ellipsoid::getStaticTypeID())
+         if (body.getTypeID() == Ellipsoid::getStaticTypeID())
          {
-            auto ellipsoid = static_cast<ConstEllipsoidID> (*it);
+            auto ellipsoid = static_cast<ConstEllipsoidID> (&body);
             bodies_.push_back(ellipsoid);
 
             // compute tensor glyph for visualization with ParaView (tensorGlyph)
diff --git a/src/pe/vtk/EllipsoidVtkOutput.h b/src/pe/vtk/EllipsoidVtkOutput.h
index e12f91879e6aaf0d2eb3d74f04b5d5ab3103feea..680410dcea6377fa1eadd83f64664df7f1acb4ba 100644
--- a/src/pe/vtk/EllipsoidVtkOutput.h
+++ b/src/pe/vtk/EllipsoidVtkOutput.h
@@ -59,7 +59,7 @@ private:
 
    ConstBlockDataID storageID_;
    const BlockStorage & blockStorage_;
-   std::vector< ConstEllipsoidID > bodies_;
+   std::vector< Ellipsoid const * > bodies_;
    std::vector< std::array<real_t,6> > tensorGlyphs_;
 };
 
diff --git a/src/pe/vtk/SphereVtkOutput.cpp b/src/pe/vtk/SphereVtkOutput.cpp
index 90672a0f60d3ae642f87d21f325a5d3fcb992e9e..626685a43610f3c4cd205351f7b3835cb41a3655 100644
--- a/src/pe/vtk/SphereVtkOutput.cpp
+++ b/src/pe/vtk/SphereVtkOutput.cpp
@@ -49,49 +49,49 @@ std::vector< SphereVtkOutput::Attributes > SphereVtkOutput::getAttributes() cons
 void SphereVtkOutput::configure()
 {
    bodies_.clear();
-   for( auto blockIt = blockStorage_.begin(); blockIt != blockStorage_.end(); ++blockIt )
+   for( auto& block : blockStorage_ )
    {
 
-      const Storage& bs = *(blockIt->getData<const Storage>( storageID_ ));
+      const BodyStorage& localStorage = (*(block.getData<const Storage>( storageID_ )))[0];
 
-      for( auto it = bs[0].begin(); it != bs[0].end(); ++it )
+      for( auto& body : localStorage )
       {
-         if (it->getTypeID() == Sphere::getStaticTypeID() || it->getTypeID() == Squirmer::getStaticTypeID())
-            bodies_.push_back( static_cast<ConstSphereID> (*it) );
-         if (it->getTypeID() == Union<boost::tuple<Sphere> >::getStaticTypeID())
+         if (body.getTypeID() == Sphere::getStaticTypeID() || body.getTypeID() == Squirmer::getStaticTypeID())
+            bodies_.push_back( static_cast<Sphere const *> (&body) );
+         if (body.getTypeID() == Union<boost::tuple<Sphere> >::getStaticTypeID())
          {
-            auto un = static_cast<Union<boost::tuple<Sphere> > const * > (*it);
+            auto un = static_cast<Union<boost::tuple<Sphere> > const * > (&body);
             for( auto it2 = un->begin(); it2 != un->end(); ++it2 )
             {
                if (it2->getTypeID() == Sphere::getStaticTypeID())
-                  bodies_.push_back( static_cast<ConstSphereID> (*it2) );
+                  bodies_.push_back( static_cast<ConstSphereID> (it2.getBodyID()) );
             }
          }
-         if (it->getTypeID() == Union<boost::tuple<Squirmer> >::getStaticTypeID())
+         if (body.getTypeID() == Union<boost::tuple<Squirmer> >::getStaticTypeID())
          {
-            auto un = static_cast<Union<boost::tuple<Squirmer> > const * > (*it);
+            auto un = static_cast<Union<boost::tuple<Squirmer> > const * > (&body);
             for( auto it2 = un->begin(); it2 != un->end(); ++it2 )
             {
                if (it2->getTypeID() == Squirmer::getStaticTypeID())
-                  bodies_.push_back( static_cast<ConstSphereID> (*it2) );
+                  bodies_.push_back( static_cast<ConstSphereID> (it2.getBodyID()) );
             }
          }
-         if (it->getTypeID() == Union<boost::tuple<Sphere,Squirmer> >::getStaticTypeID())
+         if (body.getTypeID() == Union<boost::tuple<Sphere,Squirmer> >::getStaticTypeID())
          {
-            auto un = static_cast<Union<boost::tuple<Sphere,Squirmer> > const * > (*it);
+            auto un = static_cast<Union<boost::tuple<Sphere,Squirmer> > const * > (&body);
             for( auto it2 = un->begin(); it2 != un->end(); ++it2 )
             {
                if (it2->getTypeID() == Sphere::getStaticTypeID() || it2->getTypeID() == Squirmer::getStaticTypeID())
-                  bodies_.push_back( static_cast<ConstSphereID> (*it2) );
+                  bodies_.push_back( static_cast<ConstSphereID> (it2.getBodyID()) );
             }
          }
-         if (it->getTypeID() == Union<boost::tuple<Squirmer,Sphere> >::getStaticTypeID())
+         if (body.getTypeID() == Union<boost::tuple<Squirmer,Sphere> >::getStaticTypeID())
          {
-            auto un = static_cast<Union<boost::tuple<Squirmer,Sphere> > const * > (*it);
+            auto un = static_cast<Union<boost::tuple<Squirmer,Sphere> > const * > (&body);
             for( auto it2 = un->begin(); it2 != un->end(); ++it2 )
             {
                if (it2->getTypeID() == Sphere::getStaticTypeID() || it2->getTypeID() == Squirmer::getStaticTypeID())
-                  bodies_.push_back( static_cast<ConstSphereID> (*it2) );
+                  bodies_.push_back( static_cast<ConstSphereID> (it2.getBodyID()) );
             }
          }
       }
diff --git a/src/pe/vtk/SphereVtkOutput.h b/src/pe/vtk/SphereVtkOutput.h
index 238c1fa8e305fc95813ce798d841b7705b90f95b..1878795f32bab639db91289edf4318c57e019d5c 100644
--- a/src/pe/vtk/SphereVtkOutput.h
+++ b/src/pe/vtk/SphereVtkOutput.h
@@ -61,7 +61,7 @@ private:
 
    ConstBlockDataID storageID_;
    const BlockStorage & blockStorage_;
-   std::vector< ConstSphereID > bodies_;
+   std::vector< Sphere const * > bodies_;
 };
 
 
diff --git a/src/pe_coupling/discrete_particle_methods/evaluators/AddedMassForceEvaluator.h b/src/pe_coupling/discrete_particle_methods/evaluators/AddedMassForceEvaluator.h
index a318686d8865509b3597d3361852c51c42c1ffcc..c452f886443049220e2529456a8fc5673175bd98 100644
--- a/src/pe_coupling/discrete_particle_methods/evaluators/AddedMassForceEvaluator.h
+++ b/src/pe_coupling/discrete_particle_methods/evaluators/AddedMassForceEvaluator.h
@@ -121,7 +121,7 @@ void AddedMassForceEvaluator< FlagField_T, FieldInterpolator_T, Distributor_T >
 
       for( auto bodyIt = pe::LocalBodyIterator::begin(*blockIt, bodyStorageID_); bodyIt != pe::LocalBodyIterator::end(); ++bodyIt )
       {
-         if(!dpmBodySelectorFct_(*bodyIt)) continue;
+         if(!dpmBodySelectorFct_(bodyIt.getBodyID())) continue;
 
          Vector3<real_t> forceOnFluid( real_t(0) );
 
diff --git a/src/pe_coupling/discrete_particle_methods/evaluators/BodyVelocityTimeDerivativeEvaluator.h b/src/pe_coupling/discrete_particle_methods/evaluators/BodyVelocityTimeDerivativeEvaluator.h
index 229be174ef1951b0e2601a65717457683c8d1467..7345398b7c123bd7aa83f658738a1d799dd209c5 100644
--- a/src/pe_coupling/discrete_particle_methods/evaluators/BodyVelocityTimeDerivativeEvaluator.h
+++ b/src/pe_coupling/discrete_particle_methods/evaluators/BodyVelocityTimeDerivativeEvaluator.h
@@ -63,7 +63,7 @@ public:
       {
          for( auto bodyIt = pe::BodyIterator::begin(*blockIt, bodyStorageID_); bodyIt != pe::BodyIterator::end(); ++bodyIt )
          {
-            if(!dpmBodySelectorFct_(*bodyIt)) continue;
+            if(!dpmBodySelectorFct_(bodyIt.getBodyID())) continue;
 
             bodyVelocityMap_.insert( std::pair<walberla::id_t, Vector3< real_t > >( bodyIt->getSystemID(), bodyIt->getLinearVel() ) );
          }
diff --git a/src/pe_coupling/discrete_particle_methods/evaluators/InteractionForceEvaluator.h b/src/pe_coupling/discrete_particle_methods/evaluators/InteractionForceEvaluator.h
index 33a2e7d76fa567bd754214a4ccba52280c31299e..17d3121898ec8e306502ffa55528c73fd9350282 100644
--- a/src/pe_coupling/discrete_particle_methods/evaluators/InteractionForceEvaluator.h
+++ b/src/pe_coupling/discrete_particle_methods/evaluators/InteractionForceEvaluator.h
@@ -136,7 +136,7 @@ void InteractionForceEvaluator< FlagField_T, FieldInterpolator_T, Distributor_T
 
       for( auto bodyIt = pe::LocalBodyIterator::begin(*blockIt, bodyStorageID_); bodyIt != pe::LocalBodyIterator::end(); ++bodyIt )
       {
-         if(!dpmBodySelectorFct_(*bodyIt)) continue;
+         if(!dpmBodySelectorFct_(bodyIt.getBodyID())) continue;
 
          Vector3<real_t> forceOnFluid( real_t(0) );
 
@@ -154,7 +154,7 @@ void InteractionForceEvaluator< FlagField_T, FieldInterpolator_T, Distributor_T
 
          // evaluate drag force
          Vector3<real_t> bodyVelocity = bodyIt->getLinearVel();
-         real_t bodyDiameter = getSphereEquivalentDiameter( *(*bodyIt) );
+         real_t bodyDiameter = getSphereEquivalentDiameter( *bodyIt );
          real_t bodyVolume = bodyIt->getVolume();
          real_t fluidDensity( real_t(1) );
 
diff --git a/src/pe_coupling/discrete_particle_methods/evaluators/LiftForceEvaluator.h b/src/pe_coupling/discrete_particle_methods/evaluators/LiftForceEvaluator.h
index d04ca701ca8788abc87b424b4b9b3a3328109819..de8518349033f98cfddec3c1bd1f5678b440c151 100644
--- a/src/pe_coupling/discrete_particle_methods/evaluators/LiftForceEvaluator.h
+++ b/src/pe_coupling/discrete_particle_methods/evaluators/LiftForceEvaluator.h
@@ -115,7 +115,7 @@ void LiftForceEvaluator< FlagField_T, FieldInterpolator_T, Distributor_T >
 
       for( auto bodyIt = pe::LocalBodyIterator::begin(*blockIt, bodyStorageID_); bodyIt != pe::LocalBodyIterator::end(); ++bodyIt )
       {
-         if(!dpmBodySelectorFct_(*bodyIt)) continue;
+         if(!dpmBodySelectorFct_(bodyIt.getBodyID())) continue;
 
          Vector3<real_t> forceOnFluid( real_t(0) );
 
@@ -123,7 +123,7 @@ void LiftForceEvaluator< FlagField_T, FieldInterpolator_T, Distributor_T >
          Vector3<real_t> bodyVelocity = bodyIt->getLinearVel();
 
          real_t fluidDensity( real_t(1) );
-         real_t bodyDiameter = getSphereEquivalentDiameter( *(*bodyIt) );
+         real_t bodyDiameter = getSphereEquivalentDiameter( *bodyIt );
 
          // interpolate fluid velocity and fluid curl to body position
          Vector3<real_t> fluidVelocity( real_t(0) );
diff --git a/src/pe_coupling/discrete_particle_methods/evaluators/LubricationForceEvaluator.h b/src/pe_coupling/discrete_particle_methods/evaluators/LubricationForceEvaluator.h
index e99d3e88f63befc5ef49a932eb395af4cad82a76..63723eaa49018c758a7052455cce4e628db3afb8 100644
--- a/src/pe_coupling/discrete_particle_methods/evaluators/LubricationForceEvaluator.h
+++ b/src/pe_coupling/discrete_particle_methods/evaluators/LubricationForceEvaluator.h
@@ -100,7 +100,7 @@ void LubricationForceEvaluator::operator ()()
          // lubrication forces for spheres
          if ( body1It->getTypeID() == pe::Sphere::getStaticTypeID() )
          {
-            pe::SphereID sphereI = static_cast<pe::SphereID> ( *body1It );
+            pe::SphereID sphereI = static_cast<pe::SphereID> ( body1It.getBodyID() );
 
             auto copyBody1It = body1It;
             // loop over all rigid bodies after current body1 to avoid double forces
@@ -109,7 +109,7 @@ void LubricationForceEvaluator::operator ()()
                // sphere-sphere lubrication
                if ( body2It->getTypeID() == pe::Sphere::getStaticTypeID() )
                {
-                  pe::SphereID sphereJ = static_cast<pe::SphereID>( *body2It );
+                  pe::SphereID sphereJ = static_cast<pe::SphereID>( body2It.getBodyID() );
                   treatLubricationSphrSphr( sphereI, sphereJ, blockIt->getAABB() );
                }
             }
@@ -121,19 +121,19 @@ void LubricationForceEvaluator::operator ()()
       {
          if ( body1It->getTypeID() == pe::Sphere::getStaticTypeID() )
          {
-            pe::SphereID sphereI = static_cast<pe::SphereID> ( *body1It );
+            pe::SphereID sphereI = static_cast<pe::SphereID> ( body1It.getBodyID() );
 
             for (auto body2It = globalBodyStorage_->begin(); body2It != globalBodyStorage_->end(); ++body2It)
             {
                if ( body2It->getTypeID() == pe::Plane::getStaticTypeID() )
                {
                   // sphere-plane lubrication
-                  pe::PlaneID planeJ = static_cast<pe::PlaneID>( *body2It );
+                  pe::PlaneID planeJ = static_cast<pe::PlaneID>( body2It.getBodyID() );
                   treatLubricationSphrPlane( sphereI, planeJ );
                } else if ( body2It->getTypeID() == pe::Sphere::getStaticTypeID() )
                {
                   // sphere-sphere lubrication
-                  pe::SphereID sphereJ = static_cast<pe::SphereID>( *body2It );
+                  pe::SphereID sphereJ = static_cast<pe::SphereID>( body2It.getBodyID() );
                   treatLubricationSphrSphr( sphereI, sphereJ, blockIt->getAABB() );
                }
             }
diff --git a/src/pe_coupling/discrete_particle_methods/evaluators/SolidVolumeFractionFieldEvaluator.h b/src/pe_coupling/discrete_particle_methods/evaluators/SolidVolumeFractionFieldEvaluator.h
index 1c83df74afc0eba7d192454f350932307f27bef4..586b51bc25d29259c632ed1bb76dcfc4cd08c43b 100644
--- a/src/pe_coupling/discrete_particle_methods/evaluators/SolidVolumeFractionFieldEvaluator.h
+++ b/src/pe_coupling/discrete_particle_methods/evaluators/SolidVolumeFractionFieldEvaluator.h
@@ -89,7 +89,7 @@ public:
       // assign the local bodies' volume to the cell, depending on the chosen Distributor_T
       for( auto bodyIt = pe::LocalBodyIterator::begin(*block, bodyStorageID_); bodyIt != pe::LocalBodyIterator::end(); ++bodyIt )
       {
-         if(!dpmBodySelectorFct_(*bodyIt)) continue;
+         if(!dpmBodySelectorFct_(bodyIt.getBodyID())) continue;
 
          real_t bodyVolume = bodyIt->getVolume();
          const Vector3<real_t> bodyPosition = bodyIt->getPosition();
diff --git a/src/pe_coupling/discrete_particle_methods/utility/BodyVelocityInitializer.h b/src/pe_coupling/discrete_particle_methods/utility/BodyVelocityInitializer.h
index 75ebeef5b370820e5f306b06c0f954c45b7dd16e..9ffbad829435d7797e2727455005d769d752344c 100644
--- a/src/pe_coupling/discrete_particle_methods/utility/BodyVelocityInitializer.h
+++ b/src/pe_coupling/discrete_particle_methods/utility/BodyVelocityInitializer.h
@@ -82,7 +82,7 @@ public:
 
          for( auto bodyIt = pe::LocalBodyIterator::begin(*blockIt, bodyStorageID_); bodyIt != pe::LocalBodyIterator::end(); ++bodyIt )
          {
-            if(!dpmBodySelectorFct_(*bodyIt)) continue;
+            if(!dpmBodySelectorFct_(bodyIt.getBodyID())) continue;
 
             Vector3<real_t> forceOnFluid( real_t(0) );
 
diff --git a/src/pe_coupling/mapping/BodyMapping.h b/src/pe_coupling/mapping/BodyMapping.h
index 4af2e9d3195ebaddc0ce3a6a08108c2c4d28d10d..48f05c2ee0a25b28f492ae927b052d9b9dbdc97d 100644
--- a/src/pe_coupling/mapping/BodyMapping.h
+++ b/src/pe_coupling/mapping/BodyMapping.h
@@ -106,14 +106,14 @@ void mapBodies( StructuredBlockStorage & blockStorage, const BlockDataID & bound
 
       for( auto bodyIt = pe::BodyIterator::begin(*blockIt, bodyStorageID); bodyIt != pe::BodyIterator::end(); ++bodyIt )
       {
-         if( mappingBodySelectorFct(*bodyIt))
-            mapBody<BoundaryHandling_T>( *bodyIt, *blockIt, blockStorage, boundaryHandlingID, obstacle );
+         if( mappingBodySelectorFct(bodyIt.getBodyID()))
+            mapBody<BoundaryHandling_T>( bodyIt.getBodyID(), *blockIt, blockStorage, boundaryHandlingID, obstacle );
       }
 
       for( auto bodyIt = globalBodyStorage.begin(); bodyIt != globalBodyStorage.end(); ++bodyIt )
       {
-         if( mappingBodySelectorFct(*bodyIt))
-            mapBody< BoundaryHandling_T >( *bodyIt, *blockIt, blockStorage, boundaryHandlingID, obstacle );
+         if( mappingBodySelectorFct(bodyIt.getBodyID()))
+            mapBody< BoundaryHandling_T >( bodyIt.getBodyID(), *blockIt, blockStorage, boundaryHandlingID, obstacle );
       }
    }
 }
diff --git a/src/pe_coupling/momentum_exchange_method/BodyMapping.h b/src/pe_coupling/momentum_exchange_method/BodyMapping.h
index 7651386b613b476a00d5070c1550884930a003f9..c44bc77e6a525f78c41f91127bac7def5b59096a 100644
--- a/src/pe_coupling/momentum_exchange_method/BodyMapping.h
+++ b/src/pe_coupling/momentum_exchange_method/BodyMapping.h
@@ -102,13 +102,13 @@ public:
 
       for( auto bodyIt = pe::BodyIterator::begin(*block, bodyStorageID_); bodyIt != pe::BodyIterator::end(); ++bodyIt )
       {
-         if( mappingBodySelectorFct_(*bodyIt) )
-            mapBodyAndUpdateMapping(*bodyIt, block, boundaryHandling, flagField , bodyField, obstacle, formerObstacle, dx, dy, dz);
+         if( mappingBodySelectorFct_(bodyIt.getBodyID()) )
+            mapBodyAndUpdateMapping(bodyIt.getBodyID(), block, boundaryHandling, flagField , bodyField, obstacle, formerObstacle, dx, dy, dz);
       }
       for( auto bodyIt = globalBodyStorage_->begin(); bodyIt != globalBodyStorage_->end(); ++bodyIt)
       {
-         if( mappingBodySelectorFct_(*bodyIt))
-            mapBodyAndUpdateMapping(*bodyIt, block, boundaryHandling, flagField , bodyField, obstacle, formerObstacle, dx, dy, dz);
+         if( mappingBodySelectorFct_(bodyIt.getBodyID()))
+            mapBodyAndUpdateMapping(bodyIt.getBodyID(), block, boundaryHandling, flagField , bodyField, obstacle, formerObstacle, dx, dy, dz);
       }
    }
 
@@ -273,13 +273,13 @@ void mapMovingBodies( StructuredBlockStorage & blockStorage, const BlockDataID &
    {
       for (auto bodyIt = pe::BodyIterator::begin(*blockIt, bodyStorageID); bodyIt != pe::BodyIterator::end(); ++bodyIt)
       {
-         if( mappingBodySelectorFct(*bodyIt))
-            mapMovingBody< BoundaryHandling_T >( *bodyIt, *blockIt, blockStorage, boundaryHandlingID, bodyFieldID, obstacle );
+         if( mappingBodySelectorFct(bodyIt.getBodyID()))
+            mapMovingBody< BoundaryHandling_T >( bodyIt.getBodyID(), *blockIt, blockStorage, boundaryHandlingID, bodyFieldID, obstacle );
       }
       for( auto bodyIt = globalBodyStorage.begin(); bodyIt != globalBodyStorage.end(); ++bodyIt)
       {
-         if( mappingBodySelectorFct(*bodyIt))
-            mapMovingBody< BoundaryHandling_T >( *bodyIt, *blockIt, blockStorage, boundaryHandlingID, bodyFieldID, obstacle );
+         if( mappingBodySelectorFct(bodyIt.getBodyID()))
+            mapMovingBody< BoundaryHandling_T >( bodyIt.getBodyID(), *blockIt, blockStorage, boundaryHandlingID, bodyFieldID, obstacle );
       }
    }
 }
diff --git a/src/pe_coupling/momentum_exchange_method/restoration/PDFReconstruction.h b/src/pe_coupling/momentum_exchange_method/restoration/PDFReconstruction.h
index d21808bb3cc8bae4084ea24bff84b6051dd30253..808fc089f1038092a5a54460f5e6bee46eea3e12 100644
--- a/src/pe_coupling/momentum_exchange_method/restoration/PDFReconstruction.h
+++ b/src/pe_coupling/momentum_exchange_method/restoration/PDFReconstruction.h
@@ -163,18 +163,18 @@ void PDFReconstruction< LatticeModel_T, BoundaryHandling_T, Reconstructer_T >
 
       for( auto bodyIt = pe::BodyIterator::begin(*block, bodyStorageID_); bodyIt != pe::BodyIterator::end(); ++bodyIt )
       {
-         if( !movingBodySelectorFct_(*bodyIt) )
+         if( !movingBodySelectorFct_(bodyIt.getBodyID()) )
             continue;
 
-         CellInterval cellBB = getCellBB( *bodyIt, *block, *blockStorage_, numberOfGhostLayersToInclude );
+         CellInterval cellBB = getCellBB( bodyIt.getBodyID(), *block, *blockStorage_, numberOfGhostLayersToInclude );
          reconstructPDFsInCells(cellBB, block, flagField, formerObstacle );
       }
       for( auto bodyIt = globalBodyStorage_->begin(); bodyIt != globalBodyStorage_->end(); ++bodyIt )
       {
-         if( !movingBodySelectorFct_(*bodyIt) )
+         if( !movingBodySelectorFct_(bodyIt.getBodyID()) )
             continue;
 
-         CellInterval cellBB = getCellBB( *bodyIt, *block, *blockStorage_, numberOfGhostLayersToInclude );
+         CellInterval cellBB = getCellBB( bodyIt.getBodyID(), *block, *blockStorage_, numberOfGhostLayersToInclude );
          reconstructPDFsInCells(cellBB, block, flagField, formerObstacle );
       }
    }
@@ -191,18 +191,18 @@ void PDFReconstruction< LatticeModel_T, BoundaryHandling_T, Reconstructer_T >
 
       for( auto bodyIt = pe::BodyIterator::begin(*block, bodyStorageID_); bodyIt != pe::BodyIterator::end(); ++bodyIt )
       {
-         if( !movingBodySelectorFct_(*bodyIt) )
+         if( !movingBodySelectorFct_(bodyIt.getBodyID()) )
             continue;
 
-         CellInterval cellBB = getCellBB( *bodyIt, *block, *blockStorage_, numberOfGhostLayersToInclude );
+         CellInterval cellBB = getCellBB( bodyIt.getBodyID(), *block, *blockStorage_, numberOfGhostLayersToInclude );
          updateFlags(cellBB, boundaryHandling, flagField, bodyField, formerObstacle, fluid);
       }
       for( auto bodyIt = globalBodyStorage_->begin(); bodyIt != globalBodyStorage_->end(); ++bodyIt )
       {
-         if( !movingBodySelectorFct_(*bodyIt) )
+         if( !movingBodySelectorFct_(bodyIt.getBodyID()) )
             continue;
 
-         CellInterval cellBB = getCellBB( *bodyIt, *block, *blockStorage_, numberOfGhostLayersToInclude );
+         CellInterval cellBB = getCellBB( bodyIt.getBodyID(), *block, *blockStorage_, numberOfGhostLayersToInclude );
          updateFlags(cellBB, boundaryHandling, flagField, bodyField, formerObstacle, fluid);
       }
    }
diff --git a/src/pe_coupling/partially_saturated_cells_method/BodyAndVolumeFractionMapping.h b/src/pe_coupling/partially_saturated_cells_method/BodyAndVolumeFractionMapping.h
index 9f28d42e6bd41f2226766c3e6b88bb8ad0b350ec..1a387105bd936ff7437c0d4b6cd102aecda40d0b 100644
--- a/src/pe_coupling/partially_saturated_cells_method/BodyAndVolumeFractionMapping.h
+++ b/src/pe_coupling/partially_saturated_cells_method/BodyAndVolumeFractionMapping.h
@@ -178,18 +178,18 @@ void BodyAndVolumeFractionMapping::initialize()
 
       for( auto bodyIt = pe::BodyIterator::begin( *blockIt, bodyStorageID_); bodyIt != pe::BodyIterator::end(); ++bodyIt )
       {
-         if( mappingBodySelectorFct_(*bodyIt) )
+         if( mappingBodySelectorFct_(bodyIt.getBodyID()) )
          {
-            mapPSMBodyAndVolumeFraction( *bodyIt, *blockIt, *blockStorage_, bodyAndVolumeFractionFieldID_ );
+            mapPSMBodyAndVolumeFraction( bodyIt.getBodyID(), *blockIt, *blockStorage_, bodyAndVolumeFractionFieldID_ );
             lastUpdatedPositionMap_.insert( std::pair< walberla::id_t, Vector3< real_t > >( bodyIt->getSystemID(), bodyIt->getPosition() ) );
          }
       }
 
       for( auto bodyIt = globalBodyStorage_->begin(); bodyIt != globalBodyStorage_->end(); ++bodyIt )
       {
-         if( mappingBodySelectorFct_(*bodyIt) )
+         if( mappingBodySelectorFct_(bodyIt.getBodyID()) )
          {
-            mapPSMBodyAndVolumeFraction(*bodyIt, *blockIt, *blockStorage_, bodyAndVolumeFractionFieldID_);
+            mapPSMBodyAndVolumeFraction(bodyIt.getBodyID(), *blockIt, *blockStorage_, bodyAndVolumeFractionFieldID_);
             lastUpdatedPositionMap_.insert( std::pair< walberla::id_t, Vector3< real_t > >( bodyIt->getSystemID(), bodyIt->getPosition() ) );
          }
       }
@@ -207,17 +207,17 @@ void BodyAndVolumeFractionMapping::update()
 
       for( auto bodyIt = pe::BodyIterator::begin( *blockIt, bodyStorageID_); bodyIt != pe::BodyIterator::end(); ++bodyIt )
       {
-         if( mappingBodySelectorFct_(*bodyIt) )
+         if( mappingBodySelectorFct_(bodyIt.getBodyID()) )
          {
-            updatePSMBodyAndVolumeFraction(*bodyIt, *blockIt, bodyAndVolumeFractionField, tempLastUpdatedPositionMap);
+            updatePSMBodyAndVolumeFraction(bodyIt.getBodyID(), *blockIt, bodyAndVolumeFractionField, tempLastUpdatedPositionMap);
          }
       }
 
       for( auto bodyIt = globalBodyStorage_->begin(); bodyIt != globalBodyStorage_->end(); ++bodyIt )
       {
-         if( mappingBodySelectorFct_(*bodyIt) )
+         if( mappingBodySelectorFct_(bodyIt.getBodyID()) )
          {
-            updatePSMBodyAndVolumeFraction(*bodyIt, *blockIt, bodyAndVolumeFractionField, tempLastUpdatedPositionMap);
+            updatePSMBodyAndVolumeFraction(bodyIt.getBodyID(), *blockIt, bodyAndVolumeFractionField, tempLastUpdatedPositionMap);
          }
       }
 
diff --git a/src/pe_coupling/utility/LubricationCorrection.h b/src/pe_coupling/utility/LubricationCorrection.h
index c318c66c6e42b3a3bdb3cafc50854b8f82c60569..4a5afa830d09352ff53ae87c69e88c96c7533eae 100644
--- a/src/pe_coupling/utility/LubricationCorrection.h
+++ b/src/pe_coupling/utility/LubricationCorrection.h
@@ -89,7 +89,7 @@ void LubricationCorrection::operator ()()
          // lubrication forces for spheres
          if ( body1It->getTypeID() == pe::Sphere::getStaticTypeID() )
          {
-            pe::SphereID sphereI = static_cast<pe::SphereID> ( *body1It );
+            pe::SphereID sphereI = static_cast<pe::SphereID> ( body1It.getBodyID() );
 
             auto copyBody1It = body1It;
             // loop over all rigid bodies after current body1 to avoid double forces
@@ -98,7 +98,7 @@ void LubricationCorrection::operator ()()
                // sphere-sphere lubrication
                if ( body2It->getTypeID() == pe::Sphere::getStaticTypeID() )
                {
-                  pe::SphereID sphereJ = static_cast<pe::SphereID>( *body2It );
+                  pe::SphereID sphereJ = static_cast<pe::SphereID>( body2It.getBodyID() );
                   treatLubricationSphrSphr( sphereI, sphereJ, blockIt->getAABB() );
                }
             }
@@ -110,19 +110,19 @@ void LubricationCorrection::operator ()()
       {
          if ( body1It->getTypeID() == pe::Sphere::getStaticTypeID() )
          {
-            pe::SphereID sphereI = static_cast<pe::SphereID> ( *body1It );
+            pe::SphereID sphereI = static_cast<pe::SphereID> ( body1It.getBodyID() );
 
             for (auto body2It = globalBodyStorage_->begin(); body2It != globalBodyStorage_->end(); ++body2It)
             {
                if ( body2It->getTypeID() == pe::Plane::getStaticTypeID() )
                {
                   // sphere-plane lubrication
-                  pe::PlaneID planeJ = static_cast<pe::PlaneID>( *body2It );
+                  pe::PlaneID planeJ = static_cast<pe::PlaneID>( body2It.getBodyID() );
                   treatLubricationSphrPlane( sphereI, planeJ );
                } else if ( body2It->getTypeID() == pe::Sphere::getStaticTypeID() )
                {
                   // sphere-sphere lubrication
-                  pe::SphereID sphereJ = static_cast<pe::SphereID>( *body2It );
+                  pe::SphereID sphereJ = static_cast<pe::SphereID>( body2It.getBodyID() );
                   treatLubricationSphrSphr( sphereI, sphereJ, blockIt->getAABB() );
                }
             }
diff --git a/tests/core/CMakeLists.txt b/tests/core/CMakeLists.txt
index 6885cf97661fa507cc240a45207b63a62aec4a43..ddb9907147e78dbba1d6a9d5fd07be6f2dc033e6 100644
--- a/tests/core/CMakeLists.txt
+++ b/tests/core/CMakeLists.txt
@@ -143,13 +143,6 @@ waLBerla_execute_test( NAME SetReductionTest27 COMMAND $<TARGET_FILE:SetReductio
 waLBerla_compile_test( FILES mpi/ProbeVsExtraMessage.cpp DEPENDS postprocessing)
 
 
-##############
-# ptrvector #
-##############
-
-waLBerla_compile_test( NAME PtrVector FILES ptrvector/PtrVector.cpp )
-waLBerla_execute_test( NAME PtrVector )
-
 ##############
 # selectable #
 ##############
diff --git a/tests/core/ptrvector/PtrVector.cpp b/tests/core/ptrvector/PtrVector.cpp
deleted file mode 100644
index 8ae38bdeaff93a71ac5187d7c223f87670de0b7f..0000000000000000000000000000000000000000
--- a/tests/core/ptrvector/PtrVector.cpp
+++ /dev/null
@@ -1,98 +0,0 @@
-//======================================================================================================================
-//
-//  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 PtrVector.cpp
-//! \ingroup field
-//! \author Sebastian Eibl <sebastian.eibl@fau.de>
-//
-//======================================================================================================================
-
-#include "core/ptrvector/PtrVector.h"
-#include "core/ptrvector/policies/NoDelete.h"
-
-#include "core/debug/TestSubsystem.h"
-   
-using namespace walberla;
-
-struct A{
-    int c;
-    int val;
-    static int refCount;
-
-    explicit A(int v = 0) : c(0), val(v) { ++refCount; }
-    virtual ~A(){ --refCount; }
-};
-
-struct B : public A {
-    B() : A(1) {}
-};
-
-struct C : public A {
-    C() : A(2) {}
-};
-
-int A::refCount = 0;
-
-int main( int /*argc*/, char** /*argv*/ )
-{
-    debug::enterTestMode();
-    {
-        auto a = new A();
-        auto b = new B();
-        auto c = new C();
-
-        WALBERLA_CHECK_EQUAL(A::refCount, 3);
-
-        PtrVector<A, PtrDelete> vec;
-        vec.pushBack(a);
-        vec.pushBack(b);
-        vec.pushBack(c);
-
-        WALBERLA_CHECK_EQUAL(A::refCount, 3);
-
-        for (auto it = vec.begin(); it!=vec.end(); ++it){
-            ++(it->c);
-        }
-        for (auto it = vec.begin<B>(); it!=vec.end<B>(); ++it){
-            ++(it->c);
-        }
-
-        WALBERLA_CHECK_EQUAL(a->c, 1);
-        WALBERLA_CHECK_EQUAL(b->c, 2);
-
-        vec.erase(++vec.begin());
-        int sum = 0;
-        for (auto it = vec.begin(); it!=vec.end(); ++it){
-            sum += it->val;
-        }
-
-        WALBERLA_CHECK_EQUAL(sum, 2);
-    }
-    WALBERLA_CHECK_EQUAL(A::refCount, 0);
-
-    auto a = new A();
-    auto b = new B();
-    WALBERLA_CHECK_EQUAL(A::refCount, 2);
-    {
-        PtrVector<A, NoDelete> vec;
-        vec.pushBack(a);
-        vec.pushBack(b);
-        WALBERLA_CHECK_EQUAL(A::refCount, 2);
-    }
-    WALBERLA_CHECK_EQUAL(A::refCount, 2);
-
-    delete a;
-    delete b;
-}
diff --git a/tests/pe/BodyFlags.cpp b/tests/pe/BodyFlags.cpp
index 901e85c71bf41bb6b0c4238fc8fff6aa6e550db1..db7e82ce8f6b3c4890ffb3949bbe5fba7b3dcc4f 100644
--- a/tests/pe/BodyFlags.cpp
+++ b/tests/pe/BodyFlags.cpp
@@ -71,12 +71,12 @@ int main( int argc, char ** argv )
 
    MaterialID iron = Material::find("iron");
 
-   SphereID refGlobalSphere = new Sphere(1, 0, Vec3(9, 9, 9), Vec3(0,0,0), Quat(), 3, iron, true, false, true);
-   refGlobalSphere->setLinearVel(Vec3(2,2,2));
+   Sphere refGlobalSphere(1, 0, Vec3(9, 9, 9), Vec3(0,0,0), Quat(), 3, iron, true, false, true);
+   refGlobalSphere.setLinearVel(Vec3(2,2,2));
    SphereID globalSphere = createSphere( *globalStorage, forest->getBlockStorage(), storageID, 0, Vec3(9,9,9), 3, iron, true, false, true);
    globalSphere->setLinearVel(Vec3(2,2,2));
 
-   SphereID refFixedSphere = new Sphere(2, 0, Vec3(9,9,14), Vec3(0,0,0), Quat(), 3, iron, false, false, true);
+   Sphere refFixedSphere(2, 0, Vec3(9,9,14), Vec3(0,0,0), Quat(), 3, iron, false, false, true);
    SphereID fixedSphere = createSphere( *globalStorage, forest->getBlockStorage(), storageID, 0, Vec3(9,9,14), 3, iron, false, false, true);
    walberla::id_t fixedSphereID = 0;
    if (fixedSphere != NULL) fixedSphereID = fixedSphere->getSystemID();
@@ -87,16 +87,16 @@ int main( int argc, char ** argv )
 
    cr.setGlobalLinearAcceleration(Vec3(0, 0, real_c(-9.81)));
 
-   checkVitalParameters(refGlobalSphere, globalSphere);
+   checkVitalParameters(&refGlobalSphere, globalSphere);
    for (auto it = forest->begin(); it != forest->end(); ++it)
    {
       blockforest::Block& block = *(dynamic_cast<blockforest::Block*>(&(*it)));
-      if (block.getAABB().intersects(refFixedSphere->getAABB()))
+      if (block.getAABB().intersects(refFixedSphere.getAABB()))
       {
          SphereID fixed = static_cast<SphereID> (getBody(*globalStorage, forest->getBlockStorage(), storageID, fixedSphereID));
          WALBERLA_ASSERT_NOT_NULLPTR(fixed);
-         checkVitalParameters(refFixedSphere,fixed);
-         if (!block.getAABB().contains(refFixedSphere->getPosition()))
+         checkVitalParameters(&refFixedSphere,fixed);
+         if (!block.getAABB().contains(refFixedSphere.getPosition()))
          {
             WALBERLA_ASSERT(fixed->isRemote());
          }
@@ -108,17 +108,17 @@ int main( int argc, char ** argv )
    syncShadowOwners<BodyTuple>( forest->getBlockForest(), storageID, NULL, real_c(0.0), false);
    WALBERLA_LOG_PROGRESS_ON_ROOT("*** SIMULATION - END ***");
 
-   refGlobalSphere->setPosition(Vec3(11,11,11));
-   checkVitalParameters(refGlobalSphere, globalSphere);
+   refGlobalSphere.setPosition(Vec3(11,11,11));
+   checkVitalParameters(&refGlobalSphere, globalSphere);
    for (auto it = forest->begin(); it != forest->end(); ++it)
    {
       blockforest::Block& block = *(dynamic_cast<blockforest::Block*>(&(*it)));
-      if (block.getAABB().intersects(refFixedSphere->getAABB()))
+      if (block.getAABB().intersects(refFixedSphere.getAABB()))
       {
          SphereID fixed = static_cast<SphereID> (getBody(*globalStorage, forest->getBlockStorage(), storageID, fixedSphereID));
          WALBERLA_ASSERT_NOT_NULLPTR(fixed);
-         checkVitalParameters(refFixedSphere, fixed);
-         if (!block.getAABB().contains(refFixedSphere->getPosition()))
+         checkVitalParameters(&refFixedSphere, fixed);
+         if (!block.getAABB().contains(refFixedSphere.getPosition()))
          {
             WALBERLA_ASSERT(fixed->isRemote());
          }
diff --git a/tests/pe/BodyIterators.cpp b/tests/pe/BodyIterators.cpp
index a14fc38c543ae11787bd3ed28bda4f1ed1f1f803..0dbfcc53cbeb966e45afd28e7fdbb1d36294e8ce 100644
--- a/tests/pe/BodyIterators.cpp
+++ b/tests/pe/BodyIterators.cpp
@@ -14,7 +14,7 @@
 //  with waLBerla (see COPYING.txt). If not, see <http://www.gnu.org/licenses/>.
 //
 //! \file BodyIterators.cpp
-//! \author Florian Schornbaum <florian.schornbaum@fau.de>
+//! \author Sebastian Eibl <sebastian.eibl@fau.de>
 //
 //======================================================================================================================
 
diff --git a/tests/pe/BodyStorage.cpp b/tests/pe/BodyStorage.cpp
index 644f29a1c063de596a9d15081778b2bd23ef784f..498f973944f7d2529ea84072253a4516616104db 100644
--- a/tests/pe/BodyStorage.cpp
+++ b/tests/pe/BodyStorage.cpp
@@ -54,18 +54,21 @@ int main( int argc, char** argv )
     MaterialID iron = Material::find("iron");
     {
         BodyStorage storage;
-        auto bd1 = new Body1(1, iron);
-        auto bd2 = new Body2(2, iron);
-        auto bd3 = new Body2(3, iron);
-        auto bd4 = new Body2(4, iron);
+        auto bd1Ptr = std::make_unique<Body1>(1, iron);
+        auto bd2Ptr = std::make_unique<Body2>(2, iron);
+        auto bd3Ptr = std::make_unique<Body2>(3, iron);
+        auto bd4Ptr = std::make_unique<Body2>(4, iron);
+
+        auto bd2 = bd2Ptr.get();
+        auto bd3 = bd3Ptr.get();
 
         WALBERLA_CHECK_EQUAL(Body1::refCount, 1);
         WALBERLA_CHECK_EQUAL(Body2::refCount, 3);
 
-        storage.add(bd1);
-        storage.add(bd2);
-        storage.add(bd3);
-        storage.add(bd4);
+        storage.add(std::move(bd1Ptr));
+        storage.add(std::move(bd2Ptr));
+        storage.add(std::move(bd3Ptr));
+        storage.add(std::move(bd4Ptr));
 
         WALBERLA_CHECK_EQUAL(storage.size(), 4);
         WALBERLA_CHECK_EQUAL(Body1::refCount, 1);
diff --git a/tests/pe/Collision.cpp b/tests/pe/Collision.cpp
index 98ce1fc4a43f00854911c54a7373f0c86e50cbf8..bd77d39c049c6fd6dc3ec7ad7c629f159afa2615 100644
--- a/tests/pe/Collision.cpp
+++ b/tests/pe/Collision.cpp
@@ -18,7 +18,6 @@
 //
 //======================================================================================================================
 
-#include "pe/collision/GJKEPAHelper.h"
 #include "pe/fcd/AnalyticCollisionDetection.h"
 #include "pe/utility/BodyCast.h"
 
@@ -31,6 +30,7 @@
 #include "pe/rigidbody/Sphere.h"
 #include "pe/rigidbody/Plane.h"
 #include "pe/rigidbody/Union.h"
+#include "pe/rigidbody/UnionFactory.h"
 
 #include "pe/rigidbody/SetBodyTypeIDs.h"
 #include "pe/Types.h"
@@ -125,20 +125,14 @@ void BoxTest()
    std::vector<Contact> contacts;
    fcd::AnalyticCollideFunctor< std::vector<Contact> > collideFunc(contacts);
 
-   real_t penetrationDepth = real_t(0);
-   Vec3   contactPoint = Vec3();
-   Vec3   contactNormal = Vec3();
-
 //   std::vector<Contact> contacts;
 
    // BOX <-> BOX
    WALBERLA_LOG_INFO("BOX <-> BOX");
-   WALBERLA_CHECK( !collideGJK(&b1, &b3, contactPoint, contactNormal, penetrationDepth) );
    WALBERLA_CHECK( !collideFunc(&b1, &b3) );
 //   WALBERLA_LOG_WARNING("contactPoint    : " << contactPoint);
 //   WALBERLA_LOG_WARNING("contactNormal   : " << contactNormal);
 //   WALBERLA_LOG_WARNING("penetrationDepth: " << penetrationDepth);
-   WALBERLA_CHECK(  collideGJK(&b1, &b2, contactPoint, contactNormal, penetrationDepth) );
    WALBERLA_CHECK(  collideFunc(&b1, &b2) );
 //   WALBERLA_LOG_WARNING("contactPoint    : " << contactPoint);
 //   WALBERLA_LOG_WARNING("contactNormal   : " << contactNormal);
@@ -146,29 +140,24 @@ void BoxTest()
 
 
    b4.setPosition( (Vec3(0,0,1) * real_t(sqrt(3)) + Vec3(0,0,1)) * 0.999);
-   WALBERLA_CHECK( collideGJK(&b1, &b4, contactPoint, contactNormal, penetrationDepth) );
    WALBERLA_CHECK( collideFunc(&b1, &b4) );
 //   WALBERLA_LOG_WARNING("contactPoint    : " << contacts.back().getPosition());
 //   WALBERLA_LOG_WARNING("contactNormal   : " << contacts.back().getNormal());
 //   WALBERLA_LOG_WARNING("penetrationDepth: " << contacts.back().getDistance());
 
    b4.setPosition( (Vec3(0,0,1) * real_t(sqrt(3)) + Vec3(0,0,1)) * 1.001);
-   WALBERLA_CHECK( !collideGJK(&b1, &b4, contactPoint, contactNormal, penetrationDepth) );
    WALBERLA_CHECK( !collideFunc(&b1, &b4) );
 
    b5.setPosition( (Vec3(0,0,1) * real_t(sqrt(3)) + Vec3(0,0,1)) * 0.99);
-   WALBERLA_CHECK( collideGJK(&b1, &b5, contactPoint, contactNormal, penetrationDepth) );
    WALBERLA_CHECK( collideFunc(&b1, &b5) );
 //   WALBERLA_LOG_WARNING("contactPoint    : " << contacts.back().getPosition());
 //   WALBERLA_LOG_WARNING("contactNormal   : " << contacts.back().getNormal());
 //   WALBERLA_LOG_WARNING("penetrationDepth: " << contacts.back().getDistance());
 
    b5.setPosition( (Vec3(0,0,1) * real_t(sqrt(3)) + Vec3(0,0,1)) * 1.01);
-   WALBERLA_CHECK( !collideGJK(&b1, &b5, contactPoint, contactNormal, penetrationDepth) );
    WALBERLA_CHECK( !collideFunc(&b1, &b5) );
 
    Sphere s1(126, 0, Vec3(real_t(1.5), real_t(1.5), real_t(1.5)), Vec3(0,0,0), Quat(), 1, iron, false, true, false);
-   WALBERLA_CHECK( collideGJK(&b1, &s1, contactPoint, contactNormal, penetrationDepth) );
    WALBERLA_CHECK( collideFunc(&b1, &s1) );
 //   WALBERLA_LOG_WARNING("contactPoint    : " << contactPoint);
 //   WALBERLA_LOG_WARNING("contactNormal   : " << contactNormal);
@@ -186,13 +175,6 @@ void CapsuleTest()
 
    // CAPSULE <-> SPHERE
    WALBERLA_LOG_INFO("CAPSULE <-> SPHERE");
-   sp1.setPosition(real_t(2.9), 0, 0);
-   WALBERLA_CHECK( collideGJK(&c1, &sp1, contacts) );
-//   WALBERLA_LOG_WARNING("contactPoint    : " << contacts.at(0).getPosition());
-//   WALBERLA_LOG_WARNING("contactNormal   : " << contacts.at(0).getNormal());
-//   WALBERLA_LOG_WARNING("penetrationDepth: " << contacts.at(0).getDistance());
-   sp1.setPosition(real_t(3.1), 0, 0);
-   WALBERLA_CHECK( !collideGJK(&c1, &sp1, contacts) );
 
    sp1.setPosition(0, real_t(1.9), 0);
    WALBERLA_CHECK( collideFunc(&c1, &sp1) );
@@ -224,8 +206,6 @@ void CapsuleTest()
 
 void CapsuleTest2()
 {
-   setMaxGJKIterations(10000);
-   setEPATolerance(real_t(0.00000001));
    const real_t   static_cof  ( real_t(0.1) / 2 );   // Coefficient of static friction. Roughly 0.85 with high variation depending on surface roughness for low stresses. Note: pe doubles the input coefficient of friction for material-material contacts.
    const real_t   dynamic_cof ( static_cof ); // Coefficient of dynamic friction. Similar to static friction for low speed friction.
    MaterialID     material = createMaterial( "granular", real_t( 1.0 ), 0, static_cof, dynamic_cof, real_t( 0.5 ), 1, 1, 0, 0 );
@@ -239,10 +219,6 @@ void CapsuleTest2()
    WALBERLA_LOG_DEVEL( c1 );
    WALBERLA_LOG_DEVEL( sp1 );
 
-   real_t penetrationDepth = real_t(0);
-   Vec3   contactPoint = Vec3();
-   Vec3   contactNormal = Vec3();
-
    WALBERLA_LOG_INFO("CAPSULE TEST");
    Vec2 distance;
    distance[0] = (sp1.getPosition() - c1.getPosition())[0];
@@ -250,36 +226,20 @@ void CapsuleTest2()
 
    std::cout << std::setprecision(10);
    WALBERLA_LOG_DEVEL("DISTANCE: " << distance.length());
-   WALBERLA_LOG_DEVEL("GJK: " << getMaxGJKIterations());
-   WALBERLA_LOG_DEVEL("EPA: " << getEPATolerance() );
-   WALBERLA_LOG_DEVEL(" CAPSULE <-> SPHERE (GJK) ");
-   WALBERLA_LOG_DEVEL( collideGJK(&c1, &sp1, contactPoint, contactNormal, penetrationDepth) );
-   WALBERLA_LOG_WARNING("contactPoint    : " << contactPoint);
-   WALBERLA_LOG_WARNING("contactNormal   : " << contactNormal);
-   WALBERLA_LOG_WARNING("penetrationDepth: " << penetrationDepth);
-   WALBERLA_LOG_DEVEL(" SPHERE <-> CAPSULE (GJK) ");
-   WALBERLA_LOG_DEVEL( collideGJK(&sp1, &c1, contactPoint, contactNormal, penetrationDepth) );
-   WALBERLA_LOG_WARNING("contactPoint    : " << contactPoint);
-   WALBERLA_LOG_WARNING("contactNormal   : " << contactNormal);
-   WALBERLA_LOG_WARNING("penetrationDepth: " << penetrationDepth);
    WALBERLA_LOG_DEVEL(" SPHERE <-> CAPSULE (ANALYTICAL) ");
    WALBERLA_LOG_DEVEL( collide(&sp1, &c1, contacts) );
    WALBERLA_LOG_WARNING("contactPoint    : " << contacts.at(0).getPosition());
    WALBERLA_LOG_WARNING("contactNormal   : " << contacts.at(0).getNormal());
    WALBERLA_LOG_WARNING("penetrationDepth: " << contacts.at(0).getDistance());
-
 }
 
 void UnionTest()
 {
    typedef Union< boost::tuple<Sphere> > UnionT;
-   MaterialID iron = Material::find("iron");
    UnionT  un1(120, 0, Vec3(0,0,0), Vec3(0,0,0), Quat(), false, true, false);
    UnionT  un2(121, 0, Vec3(real_t(1.5),0,0), Vec3(0,0,0), Quat(), false, true, false);
-   SphereID sp1 = new Sphere(123, 1, Vec3(0,0,0), Vec3(0,0,0), Quat(), 1, iron, false, true, false);
-   un1.add(sp1);
-   SphereID sp2 = new Sphere(124, 2, Vec3(real_t(1.5),0,0), Vec3(0,0,0), Quat(), 1, iron, false, true, false);
-   un2.add(sp2);
+   auto sp1 = createSphere(&un1, 123, Vec3(0,0,0), 1);
+   auto sp2 = createSphere(&un2, 124, Vec3(real_t(1.5),0,0), 1);
 
    std::vector<Contact> contacts;
 
diff --git a/tests/pe/CollisionTobiasGJK.cpp b/tests/pe/CollisionTobiasGJK.cpp
index 2c3fa052afaa27c40dc7de3145e6999ddd13d04c..46889c1eab92e2fb883173cb525fb324a510e00b 100644
--- a/tests/pe/CollisionTobiasGJK.cpp
+++ b/tests/pe/CollisionTobiasGJK.cpp
@@ -32,6 +32,7 @@
 #include "pe/rigidbody/Sphere.h"
 #include "pe/rigidbody/Plane.h"
 #include "pe/rigidbody/Union.h"
+#include "pe/rigidbody/UnionFactory.h"
 #include "pe/rigidbody/Ellipsoid.h"
 
 #include "pe/rigidbody/SetBodyTypeIDs.h"
@@ -355,32 +356,29 @@ void UnionTest(){
    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>> *unsub = new Union<boost::tuple<Sphere>>(192, 192, Vec3(0,real_t(3.8),0), Vec3(0,0,0), Quat(), false, true, false);
+   using UnionT = Union<boost::tuple<Sphere>>;
+   auto unsub = std::make_unique<UnionT>(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 );
-
-   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);
+   auto sp1 = createSphere(unsub.get(), 180, Vec3(-3,real_t(3.8),0), real_t(3.0));
+   auto sp2 = createSphere(unsub.get(), 181, Vec3(3,real_t(3.8),0), real_t(3.0));
 
    //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);
+   Union<boost::tuple<Sphere, Union<boost::tuple<Sphere>>>> un(193, 193, Vec3(0, 0, 0), Vec3(0,0,0), Quat(), false, true, false);
+   createSphere(&un, 182, Vec3(0,real_t(6),0), real_t(3.0));
+   un.add(std::move(unsub));
 
 
    PossibleContacts pcs;
-   pcs.push_back(std::pair<Union<boost::tuple<Sphere,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));
+      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));
+      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!");
    }
@@ -390,25 +388,25 @@ void UnionTest(){
    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));
+      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));
+      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, 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();
    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));
+      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));
+      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!");
    }
@@ -417,9 +415,9 @@ void UnionTest(){
    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));
+      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));
+      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!");
    }
diff --git a/tests/pe/ForceSync.cpp b/tests/pe/ForceSync.cpp
index f0901d1b7039d9082dd6ed3569af9bf364d72f40..b97d408077ded3ce50672eddca66ff49b0bc35a0 100644
--- a/tests/pe/ForceSync.cpp
+++ b/tests/pe/ForceSync.cpp
@@ -98,12 +98,12 @@ int main( int argc, char ** argv )
 
       for (auto bodyIt = localStorage.begin(); bodyIt != localStorage.end(); ++bodyIt)
       {
-         BodyID b = *bodyIt;
+         BodyID b = bodyIt.getBodyID();
          b->addForce( Vec3(1,0,0) );
       }
       for (auto bodyIt = shadowStorage.begin(); bodyIt != shadowStorage.end(); ++bodyIt)
       {
-         BodyID b = *bodyIt;
+         BodyID b = bodyIt.getBodyID();
          b->addForce( Vec3(0,1,0) );
       }
    }
@@ -127,12 +127,12 @@ int main( int argc, char ** argv )
 
 //      for (auto bodyIt = localStorage.begin(); bodyIt != localStorage.end(); ++bodyIt)
 //      {
-//         BodyID b = *bodyIt;
+//         BodyID b = bodyIt.getBodyID();
 //         WALBERLA_LOG_DEVEL("LOCAL\n" << b << "\nForce: " << b->getForce());
 //      }
 //      for (auto bodyIt = shadowStorage.begin(); bodyIt != shadowStorage.end(); ++bodyIt)
 //      {
-//         BodyID b = *bodyIt;
+//         BodyID b = bodyIt.getBodyID();
 //         WALBERLA_LOG_DEVEL("SHADOW\n" << b << "\nForce: " << b->getForce());
 //      }
 //   }
diff --git a/tests/pe/HashGrids.cpp b/tests/pe/HashGrids.cpp
index a1790653be79c8c55fb3a8edab2b760371948bcb..7b203cbe8e0baf7173e7d4f3ee231f2996d883b9 100644
--- a/tests/pe/HashGrids.cpp
+++ b/tests/pe/HashGrids.cpp
@@ -115,7 +115,8 @@ int main( int argc, char** argv )
                      iron, true, false, true);
 
     syncShadowOwners<BodyTuple>( forest->getBlockForest(), storageID);
-    for (int step=0; step < 100; ++step){
+    for (int step=0; step < 100; ++step)
+    {
        cr( real_c(0.1) );
        syncShadowOwners<BodyTuple>( forest->getBlockForest(), storageID);
 
diff --git a/tests/pe/Marshalling.cpp b/tests/pe/Marshalling.cpp
index 30da9d3be10fbad20a2e545dbfa915a85a988ac0..985269bd619ad65d799ffc62efb6524deefa82e1 100644
--- a/tests/pe/Marshalling.cpp
+++ b/tests/pe/Marshalling.cpp
@@ -41,11 +41,14 @@ using namespace walberla::pe::communication;
 typedef boost::tuple<Sphere>       UnionTypeTuple;
 typedef Union< UnionTypeTuple >    UnionT;
 typedef UnionT*                    UnionID;
+typedef std::unique_ptr<UnionT>    UnionPtr;
 
 typedef boost::tuple<Box, Capsule, Sphere, Squirmer, UnionT, Ellipsoid> BodyTuple ;
 
 void testBox()
 {
+   WALBERLA_LOG_INFO_ON_ROOT("*** testBox ***");
+
    MaterialID iron = Material::find("iron");
 
    Box b1(759846, 1234794, Vec3(real_c(1), real_c(2), real_c(3)), Vec3(0,0,0), Quat(), Vec3(1,2,3), iron, false, true, false);
@@ -56,7 +59,8 @@ void testBox()
    MarshalDynamically<BodyTuple>::execute(sb, b1);
    mpi::RecvBuffer rb(sb);
 
-   BoxID b2 = static_cast<BoxID> (UnmarshalDynamically<BodyTuple>::execute(rb, Box::getStaticTypeID(), math::AABB(Vec3(-100,-100,-100), Vec3(100,100,100)), math::AABB(Vec3(-100,-100,-100), Vec3(100,100,100))));
+   auto bPtr = UnmarshalDynamically<BodyTuple>::execute(rb, Box::getStaticTypeID(), math::AABB(Vec3(-100,-100,-100), Vec3(100,100,100)), math::AABB(Vec3(-100,-100,-100), Vec3(100,100,100)));
+   BoxID b2 = static_cast<BoxID>(bPtr.get());
 
    WALBERLA_CHECK_FLOAT_EQUAL(b1.getPosition(), b2->getPosition());
    WALBERLA_CHECK_FLOAT_EQUAL(b1.getLinearVel(), b2->getLinearVel());
@@ -68,6 +72,8 @@ void testBox()
 
 void testCapsule()
 {
+   WALBERLA_LOG_INFO_ON_ROOT("*** testCapsule ***");
+
    MaterialID iron = Material::find("iron");
 
    Capsule c1(759846, 1234794, Vec3(real_c(1), real_c(2), real_c(3)), Vec3(0,0,0), Quat(), 5, 7, iron, false, false, false);
@@ -78,7 +84,8 @@ void testCapsule()
    MarshalDynamically<BodyTuple>::execute(sb, c1);
    mpi::RecvBuffer rb(sb);
 
-   CapsuleID c2 = static_cast<CapsuleID> (UnmarshalDynamically<BodyTuple>::execute(rb, Capsule::getStaticTypeID(), math::AABB(Vec3(-100,-100,-100), Vec3(100,100,100)), math::AABB(Vec3(-100,-100,-100), Vec3(100,100,100))));
+   auto cPtr = UnmarshalDynamically<BodyTuple>::execute(rb, Capsule::getStaticTypeID(), math::AABB(Vec3(-100,-100,-100), Vec3(100,100,100)), math::AABB(Vec3(-100,-100,-100), Vec3(100,100,100)));
+   CapsuleID c2 = static_cast<CapsuleID> (cPtr.get());
 
    WALBERLA_CHECK_FLOAT_EQUAL(c1.getPosition(), c2->getPosition());
    WALBERLA_CHECK_FLOAT_EQUAL(c1.getLinearVel(), c2->getLinearVel());
@@ -91,6 +98,8 @@ void testCapsule()
 
 void testSphere()
 {
+   WALBERLA_LOG_INFO_ON_ROOT("*** testSphere ***");
+
    MaterialID iron = Material::find("iron");
 
    Sphere s1(759846, 1234794, Vec3(real_c(1), real_c(2), real_c(3)), Vec3(0,0,0), Quat(), 5, iron, false, false, false);
@@ -101,7 +110,8 @@ void testSphere()
    MarshalDynamically<BodyTuple>::execute(sb, s1);
    mpi::RecvBuffer rb(sb);
 
-   SphereID s2 = static_cast<SphereID> (UnmarshalDynamically<BodyTuple>::execute(rb, Sphere::getStaticTypeID(), math::AABB(Vec3(-100,-100,-100), Vec3(100,100,100)), math::AABB(Vec3(-100,-100,-100), Vec3(100,100,100))));
+   auto sPtr = UnmarshalDynamically<BodyTuple>::execute(rb, Sphere::getStaticTypeID(), math::AABB(Vec3(-100,-100,-100), Vec3(100,100,100)), math::AABB(Vec3(-100,-100,-100), Vec3(100,100,100)));
+   SphereID s2 = static_cast<SphereID> (sPtr.get());
 
    WALBERLA_CHECK_FLOAT_EQUAL(s1.getPosition(), s2->getPosition());
    WALBERLA_CHECK_FLOAT_EQUAL(s1.getLinearVel(), s2->getLinearVel());
@@ -113,6 +123,8 @@ void testSphere()
 
 void testSquirmer()
 {
+   WALBERLA_LOG_INFO_ON_ROOT("*** testSquirmer ***");
+
    MaterialID iron = Material::find("iron");
 
    Squirmer s1(759846, 1234794, Vec3(real_c(1), real_c(2), real_c(3)), Vec3(0,0,0), Quat(), real_c(5), real_c(0.1), real_c(4.93), iron, false, false, false);
@@ -123,7 +135,8 @@ void testSquirmer()
    MarshalDynamically<BodyTuple>::execute(sb, s1);
    mpi::RecvBuffer rb(sb);
 
-   SquirmerID s2 = static_cast<SquirmerID> (UnmarshalDynamically<BodyTuple>::execute(rb, Squirmer::getStaticTypeID(), math::AABB(Vec3(-100,-100,-100), Vec3(100,100,100)), math::AABB(Vec3(-100,-100,-100), Vec3(100,100,100))));
+   auto sPtr = UnmarshalDynamically<BodyTuple>::execute(rb, Squirmer::getStaticTypeID(), math::AABB(Vec3(-100,-100,-100), Vec3(100,100,100)), math::AABB(Vec3(-100,-100,-100), Vec3(100,100,100)));
+   SquirmerID s2 = static_cast<SquirmerID> (sPtr.get());
 
    WALBERLA_CHECK_FLOAT_EQUAL(s1.getSquirmerVelocity(), s2->getSquirmerVelocity());
    WALBERLA_CHECK_FLOAT_EQUAL(s1.getSquirmerBeta(), s2->getSquirmerBeta());
@@ -131,6 +144,8 @@ void testSquirmer()
 
 void testEllipsoid()
 {
+   WALBERLA_LOG_INFO_ON_ROOT("*** testEllipsoid ***");
+
    MaterialID iron = Material::find("iron");
 
    Ellipsoid e1(759847, 1234795, Vec3(real_c(1), real_c(2), real_c(3)), Vec3(0,0,0), Quat(), Vec3(3,1,5), iron, false, false, false);
@@ -141,7 +156,8 @@ void testEllipsoid()
    MarshalDynamically<BodyTuple>::execute(sb, e1);
    mpi::RecvBuffer rb(sb);
 
-   EllipsoidID e2 = static_cast<EllipsoidID> (UnmarshalDynamically<BodyTuple>::execute(rb, Ellipsoid::getStaticTypeID(), math::AABB(Vec3(-100,-100,-100), Vec3(100,100,100)), math::AABB(Vec3(-100,-100,-100), Vec3(100,100,100))));
+   auto ePtr = UnmarshalDynamically<BodyTuple>::execute(rb, Ellipsoid::getStaticTypeID(), math::AABB(Vec3(-100,-100,-100), Vec3(100,100,100)), math::AABB(Vec3(-100,-100,-100), Vec3(100,100,100)));
+   EllipsoidID e2 = static_cast<EllipsoidID>(ePtr.get());
 
    WALBERLA_CHECK_FLOAT_EQUAL(e1.getPosition(), e2->getPosition());
    WALBERLA_CHECK_FLOAT_EQUAL(e1.getLinearVel(), e2->getLinearVel());
@@ -153,6 +169,7 @@ void testEllipsoid()
 
 void testUnion()
 {
+   WALBERLA_LOG_INFO_ON_ROOT("*** testUnion ***");
    UnionT u1(159, 423, Vec3(real_c(1), real_c(2), real_c(3)), Vec3(0,0,0), Quat(), false, false, false);
    SphereID s11 = createSphere< UnionTypeTuple >(&u1, 1234794, Vec3(real_c(1), real_c(2), real_c(3)), 2);
    SphereID s21 = createSphere< UnionTypeTuple >(&u1, 4567789, Vec3(real_c(3), real_c(2), real_c(3)), real_c(1.5));
@@ -163,15 +180,16 @@ void testUnion()
    MarshalDynamically<BodyTuple>::execute(sb, u1);
    mpi::RecvBuffer rb(sb);
 
-   UnionID u2 = static_cast<UnionID> (UnmarshalDynamically<BodyTuple>::execute(rb, UnionT::getStaticTypeID(), math::AABB(Vec3(-100,-100,-100), Vec3(100,100,100)), math::AABB(Vec3(-100,-100,-100), Vec3(100,100,100))));
+   auto uPtr = UnmarshalDynamically<BodyTuple>::execute(rb, UnionT::getStaticTypeID(), math::AABB(Vec3(-100,-100,-100), Vec3(100,100,100)), math::AABB(Vec3(-100,-100,-100), Vec3(100,100,100)));
+   UnionID u2 = static_cast<UnionID>(uPtr.get());
    WALBERLA_CHECK_NOT_NULLPTR( u2 );
 
    WALBERLA_CHECK_EQUAL(u1.size(), 2);
    WALBERLA_CHECK_EQUAL(u1.size(), u2->size());
 
    //getting spheres of second union
-   SphereID s12 = static_cast<SphereID> (*(u2->begin()));
-   SphereID s22 = static_cast<SphereID> (*(++(u2->begin())));
+   SphereID s12 = static_cast<SphereID> (u2->begin().getBodyID());
+   SphereID s22 = static_cast<SphereID> ((++(u2->begin())).getBodyID());
    WALBERLA_CHECK_UNEQUAL( s12, s22 );
 
    WALBERLA_CHECK_FLOAT_EQUAL( s11->getPosition(),    s12->getPosition());
diff --git a/tests/pe/ParallelEquivalence.cpp b/tests/pe/ParallelEquivalence.cpp
index 6a211cd6739ad9613e682832b7269d9d5184531a..03aac6f6fa2f73fdc69422287f9dc6e1a45504cf 100644
--- a/tests/pe/ParallelEquivalence.cpp
+++ b/tests/pe/ParallelEquivalence.cpp
@@ -157,7 +157,7 @@ void sim(shared_ptr< StructuredBlockForest > forest, std::vector<BodyData>& res,
       BodyStorage& localStorage = (*storage)[0];
       for (auto bodyIt = localStorage.begin(); bodyIt != localStorage.end(); ++bodyIt)
       {
-         BodyID b = *bodyIt;
+         BodyID b = bodyIt.getBodyID();
          res.push_back(BodyData(b->getID(), b->getPosition(), b->getLinearVel()));
       }
    }
diff --git a/tests/pe/RigidBody.cpp b/tests/pe/RigidBody.cpp
index 58926892dfa4786e5baaa3a9413bbf04df96390c..ad3e0facb2e5fb3900fccbc4c042b5a341640c1c 100644
--- a/tests/pe/RigidBody.cpp
+++ b/tests/pe/RigidBody.cpp
@@ -39,10 +39,6 @@ void move( BodyStorage& storage, real_t dt )
       WALBERLA_ASSERT( it->checkInvariants(), "Invalid capsule state detected" );
       WALBERLA_ASSERT( !it->hasSuperBody(), "Invalid superordinate body detected" );
 
-      // Resetting the contact node and removing all attached contacts
-//      it->resetNode();
-      it->clearContacts();
-
       // Moving the capsule according to the acting forces (don't move a sleeping body)
       if( it->isAwake() ) {
          if( !it->hasInfiniteMass() ) {
@@ -86,10 +82,10 @@ void move( BodyStorage& storage, real_t dt )
 void checkRotationFunctions()
 {
    MaterialID iron = Material::find("iron");
-   auto sp1 = shared_ptr<Sphere>( new Sphere(0, 0, Vec3(0,0,0), Vec3(0,0,0), Quat(), 1, iron, false, true, false) );
-   auto sp2 = shared_ptr<Sphere>( new Sphere(0, 0, Vec3(0,0,0), Vec3(0,0,0), Quat(), 1, iron, false, true, false) );
-   auto sp3 = shared_ptr<Sphere>( new Sphere(0, 0, Vec3(0,0,0), Vec3(0,0,0), Quat(), 1, iron, false, true, false) );
-   auto sp4 = shared_ptr<Sphere>( new Sphere(0, 0, Vec3(0,0,0), Vec3(0,0,0), Quat(), 1, iron, false, true, false) );
+   auto sp1 = std::make_shared<Sphere>( 0, 0, Vec3(0,0,0), Vec3(0,0,0), Quat(), 1, iron, false, true, false );
+   auto sp2 = std::make_shared<Sphere>( 0, 0, Vec3(0,0,0), Vec3(0,0,0), Quat(), 1, iron, false, true, false );
+   auto sp3 = std::make_shared<Sphere>( 0, 0, Vec3(0,0,0), Vec3(0,0,0), Quat(), 1, iron, false, true, false );
+   auto sp4 = std::make_shared<Sphere>( 0, 0, Vec3(0,0,0), Vec3(0,0,0), Quat(), 1, iron, false, true, false );
 
    sp1->rotate( 1, 0, 0, math::M_PI * real_t(0.5));
    sp1->rotate( 0, 1, 0, math::M_PI * real_t(0.5));
@@ -125,7 +121,7 @@ void checkRotationFunctions()
 void checkPointFunctions()
 {
    MaterialID iron = Material::find("iron");
-   auto sp1 = shared_ptr<Sphere>( new Sphere(0, 0, Vec3(10,10,10), Vec3(0,0,0), Quat(), 1, iron, false, true, false) );
+   auto sp1 = std::make_shared<Sphere>( 0, 0, Vec3(10,10,10), Vec3(0,0,0), Quat(), 1, iron, false, true, false );
 
    WALBERLA_CHECK( sp1->containsPoint( 10, 10, 10 ) );
    WALBERLA_CHECK( sp1->containsPoint( real_c(10.9), 10, 10 ) );
@@ -152,8 +148,8 @@ int main( int argc, char** argv )
 
    MaterialID iron = Material::find("iron");
    BodyStorage storage;
-   SphereID sphere = new Sphere(0, 0, Vec3(0,0,0), Vec3(0,0,0), Quat(), 1, iron, false, true, false);
-   storage.add(sphere);
+   SpherePtr spPtr = std::make_unique<Sphere>(0, 0, Vec3(0,0,0), Vec3(0,0,0), Quat(), 1, iron, false, true, false);
+   SphereID sphere = static_cast<SphereID>(&storage.add(std::move(spPtr)));
 
    Vec3 x0 = Vec3(-2,2,0);
    Vec3 v0 = Vec3(-1,-1,1);
diff --git a/tests/pe/ShadowCopy.cpp b/tests/pe/ShadowCopy.cpp
index da6c31cefce79edc7a5234d0a0ef575a862a37c8..8c5dba7c4ad95888979f9b6c88f6109fc763d475 100644
--- a/tests/pe/ShadowCopy.cpp
+++ b/tests/pe/ShadowCopy.cpp
@@ -19,6 +19,7 @@
 //======================================================================================================================
 
 #include "pe/basic.h"
+#include "pe/rigidbody/UnionFactory.h"
 #include "pe/utility/GetBody.h"
 #include "pe/utility/DestroyBody.h"
 
@@ -95,12 +96,9 @@ int main( int argc, char** argv )
    destroyBodyBySID( *globalBodyStorage, forest->getBlockStorage(), storageID, sid );
 
    WALBERLA_LOG_PROGRESS_ON_ROOT( " *** UNION *** ");
-   MaterialID iron = Material::find("iron");
    UnionT* un   = createUnion< boost::tuple<Sphere> >( *globalBodyStorage, forest->getBlockStorage(), storageID, 0, Vec3(2,2,2) );
-   SphereID sp1 = new Sphere( 10, 0, Vec3(real_t(4.9),2,2), Vec3(0,0,0), Quat(), real_t(1)  , iron, false, true, false );
-   SphereID sp2 = new Sphere( 11, 0, Vec3(3,2,2)          , Vec3(0,0,0), Quat(), real_t(1.5), iron, false, true, false );
-   un->add(sp1);
-   un->add(sp2);
+   auto sp1 = createSphere(un, 10, Vec3(real_t(4.9),2,2), real_t(1));
+   auto sp2 = createSphere(un, 11, Vec3(3,2,2), real_t(1.5));
    un->setPosition( Vec3( real_t(4.9), 2, 2) );
    auto relPosSp1 = sp1->getRelPosition();
    auto relPosSp2 = sp2->getRelPosition();
@@ -113,8 +111,8 @@ int main( int argc, char** argv )
    syncCall();
 
    un  = static_cast<UnionT*> (getBody( *globalBodyStorage, forest->getBlockStorage(), storageID, sid, StorageSelect::LOCAL ));
-   sp1 = static_cast<SphereID> (*(un->begin()));
-   sp2 = static_cast<SphereID> (*(++(un->begin())));
+   sp1 = static_cast<SphereID> (un->begin().getBodyID());
+   sp2 = static_cast<SphereID> ((++(un->begin())).getBodyID());
    WALBERLA_CHECK_NOT_NULLPTR(sp1);
    WALBERLA_CHECK_NOT_NULLPTR(sp2);
    WALBERLA_CHECK_EQUAL( sp1->getTypeID(), Sphere::getStaticTypeID() );
@@ -152,8 +150,8 @@ int main( int argc, char** argv )
 
    posUnion = Vec3(real_t(0.9),2,2);
    un  = static_cast<UnionT*> (getBody( *globalBodyStorage, forest->getBlockStorage(), storageID, sid, StorageSelect::LOCAL ));
-   sp1 = static_cast<SphereID> (*(un->begin()));
-   sp2 = static_cast<SphereID> (*(++(un->begin())));
+   sp1 = static_cast<SphereID> (un->begin().getBodyID());
+   sp2 = static_cast<SphereID> ((++(un->begin())).getBodyID());
    WALBERLA_CHECK_NOT_NULLPTR(sp1);
    WALBERLA_CHECK_NOT_NULLPTR(sp2);
    WALBERLA_CHECK_EQUAL( sp1->getTypeID(), Sphere::getStaticTypeID() );
diff --git a/tests/pe/SimpleCCD.cpp b/tests/pe/SimpleCCD.cpp
index 3bb6539082b84879307701ad0ceccc08a782bf48..527a846383cb8931c3215e5a16352093918f4699 100644
--- a/tests/pe/SimpleCCD.cpp
+++ b/tests/pe/SimpleCCD.cpp
@@ -60,7 +60,7 @@ int main( int argc, char** argv )
     math::seedRandomGenerator(1337);
 
     for (uint_t i = 0; i < 100; ++i)
-      storage[0].add( new Sphere(UniqueID<Sphere>::createGlobal(), 0, Vec3( math::realRandom(real_c(0), real_c(10)), math::realRandom(real_c(0), real_c(10)), math::realRandom(real_c(0), real_c(10))), Vec3(0,0,0), Quat(), 1, iron, false, false, false) );
+      storage[0].add( std::make_unique<Sphere>(UniqueID<Sphere>::createGlobal(), 0, Vec3( math::realRandom(real_c(0), real_c(10)), math::realRandom(real_c(0), real_c(10)), math::realRandom(real_c(0), real_c(10))), Vec3(0,0,0), Quat(), 1, iron, false, false, false) );
 
     sccd.generatePossibleContacts();
 
@@ -70,7 +70,7 @@ int main( int argc, char** argv )
 
     WALBERLA_LOG_DEVEL( s_fcd.getContacts().size() );
 
-    BodyID bd = *(storage[0].begin() + 5);
+    BodyID bd = (storage[0].begin() + 5).getBodyID();
     storage[0].remove( bd );
 
     sccd.generatePossibleContacts();
@@ -84,14 +84,14 @@ int main( int argc, char** argv )
 
     bs.clear();
 
-    bs.add( new Sphere(UniqueID<Sphere>::createGlobal(), 0, Vec3( math::realRandom(real_c(0), real_c(10)), math::realRandom(real_c(0), real_c(10)), math::realRandom(real_c(0), real_c(10))), Vec3(0,0,0), Quat(), 1, iron, false, false, false) );
+    bs.add( std::make_unique<Sphere>(UniqueID<Sphere>::createGlobal(), 0, Vec3( math::realRandom(real_c(0), real_c(10)), math::realRandom(real_c(0), real_c(10)), math::realRandom(real_c(0), real_c(10))), Vec3(0,0,0), Quat(), 1, iron, false, false, false) );
 
     WcTimingPool pool;
     for (int runs = 0; runs < 10; ++runs)
     {
        auto oldSize = bs.size();
        for (uint_t i = 0; i < oldSize; ++i)
-         bs.add( new Sphere(UniqueID<Sphere>::createGlobal(), 0, Vec3( math::realRandom(real_c(0), real_c(10)), math::realRandom(real_c(0), real_c(10)), math::realRandom(real_c(0), real_c(10))), Vec3(0,0,0), Quat(), 0.5, iron, false, false, false) );
+         bs.add( std::make_unique<Sphere>(UniqueID<Sphere>::createGlobal(), 0, Vec3( math::realRandom(real_c(0), real_c(10)), math::realRandom(real_c(0), real_c(10)), math::realRandom(real_c(0), real_c(10))), Vec3(0,0,0), Quat(), 0.5, iron, false, false, false) );
        pool["SCCD"].start();
        sccd.generatePossibleContacts();
        pool["SCCD"].end();
diff --git a/tests/pe/SyncEquivalence.cpp b/tests/pe/SyncEquivalence.cpp
index c311e1d6728ebd00a11b86451b0a8b208ef94d33..6afd4f8670bc0ceeac07567021c9581516887241 100644
--- a/tests/pe/SyncEquivalence.cpp
+++ b/tests/pe/SyncEquivalence.cpp
@@ -115,7 +115,7 @@ void createSimulation(math::AABB& simulationDomain,
     info.ccdID               = info.forest->addBlockData(ccd::createHashGridsDataHandling( info.globalBodyStorage, info.storageID ), "CCD");
     info.fcdID               = info.forest->addBlockData(fcd::createGenericFCDDataHandling<BodyTuple, fcd::AnalyticCollideFunctor>(), "FCD");
 
-    info.cr = shared_ptr<cr::ICR>(new cr::HCSITS(info.globalBodyStorage, info.forest->getBlockStoragePointer(), info.storageID, info.ccdID, info.fcdID) );
+    info.cr = std::make_shared<cr::HCSITS>(info.globalBodyStorage, info.forest->getBlockStoragePointer(), info.storageID, info.ccdID, info.fcdID );
 
     int numParticles = int_c(0);
 
@@ -227,7 +227,7 @@ int main( int argc, char ** argv )
                   WALBERLA_CHECK_EQUAL(shadowOwnersIt1->blockID_, shadowOwnersIt2->blockID_);
                }
 
-               checkVitalParameters( static_cast<SphereID>(*bodyIt1), static_cast<SphereID>(*bodyIt2) );
+               checkVitalParameters( static_cast<SphereID>(bodyIt1.getBodyID()), static_cast<SphereID>(bodyIt2.getBodyID()) );
 
             }
         }
diff --git a/tests/pe/Synchronization.cpp b/tests/pe/Synchronization.cpp
index c8ad534ffe4d84a832ba4e4986eacc1e62d72501..17d33c41be16009ad7a04fd91239f9f76ccdaaa1 100644
--- a/tests/pe/Synchronization.cpp
+++ b/tests/pe/Synchronization.cpp
@@ -39,7 +39,7 @@ using namespace walberla::blockforest;
 
 typedef boost::tuple<Sphere> BodyTuple ;
 
-void checkSphere(StructuredBlockForest& forest, BlockDataID storageID, walberla::id_t sid, SphereID ref, const Vec3& newPos)
+void checkSphere(StructuredBlockForest& forest, BlockDataID storageID, walberla::id_t sid, Sphere& ref, const Vec3& newPos)
 {
    for (auto it = forest.begin(); it != forest.end(); ++it)
    {
@@ -47,37 +47,37 @@ void checkSphere(StructuredBlockForest& forest, BlockDataID storageID, walberla:
       Storage& storage  = *(block.getData<Storage>(storageID));
       BodyStorage& shadowStorage = storage[1];
 
-      if (block.getAABB().contains( ref->getPosition() ))
+      if (block.getAABB().contains( ref.getPosition() ))
       {
-         WALBERLA_CHECK_EQUAL( storage[StorageType::LOCAL].size(), 1, "pos: " <<  ref->getPosition() << "\nradius: " << ref->getRadius() <<  "\ndomain: " << block.getAABB() );
-         WALBERLA_CHECK_EQUAL( shadowStorage.size(), 0, "pos: " << ref->getPosition() << "\nradius: " << ref->getRadius() <<  "\ndomain: " << block.getAABB() );
-         SphereID bd = static_cast<SphereID> (*(storage[StorageType::LOCAL].find( sid )));
+         WALBERLA_CHECK_EQUAL( storage[StorageType::LOCAL].size(), 1, "pos: " <<  ref.getPosition() << "\nradius: " << ref.getRadius() <<  "\ndomain: " << block.getAABB() );
+         WALBERLA_CHECK_EQUAL( shadowStorage.size(), 0, "pos: " << ref.getPosition() << "\nradius: " << ref.getRadius() <<  "\ndomain: " << block.getAABB() );
+         SphereID bd = static_cast<SphereID> (storage[StorageType::LOCAL].find( sid ).getBodyID());
          WALBERLA_CHECK_NOT_NULLPTR(bd);
-         checkVitalParameters(bd, ref);
+         checkVitalParameters(bd, &ref);
          WALBERLA_LOG_DEVEL("#shadows: " << bd->MPITrait.sizeShadowOwners() << " #block states set: " << bd->MPITrait.getBlockStateSize() << "\nowner domain: " << block.getAABB() << "\nowner: " << bd->MPITrait.getOwner());
          bd->setPosition( newPos );
-      } else if (forest.periodicIntersect(block.getAABB(), ref->getAABB()) )
+      } else if (forest.periodicIntersect(block.getAABB(), ref.getAABB()) )
       {
-         WALBERLA_CHECK_EQUAL( storage[StorageType::LOCAL].size(), 0, "pos: " << ref->getPosition() << "\nradius: " << ref->getRadius() <<  "\ndomain: " << block.getAABB() );
-         WALBERLA_CHECK_EQUAL( shadowStorage.size(), 1, "pos: " << ref->getPosition() << "\nradius: " << ref->getRadius() <<  "\ndomain: " << block.getAABB() );
-         SphereID bd = static_cast<SphereID> (*(shadowStorage.find( sid )));
+         WALBERLA_CHECK_EQUAL( storage[StorageType::LOCAL].size(), 0, "pos: " << ref.getPosition() << "\nradius: " << ref.getRadius() <<  "\ndomain: " << block.getAABB() );
+         WALBERLA_CHECK_EQUAL( shadowStorage.size(), 1, "pos: " << ref.getPosition() << "\nradius: " << ref.getRadius() <<  "\ndomain: " << block.getAABB() );
+         SphereID bd = static_cast<SphereID> (shadowStorage.find( sid ).getBodyID());
          WALBERLA_CHECK_NOT_NULLPTR(bd);
-         auto backupPos =ref->getPosition();
-         auto correctedPos = ref->getPosition();
+         auto backupPos =ref.getPosition();
+         auto correctedPos = ref.getPosition();
          pe::communication::correctBodyPosition(forest.getDomain(), block.getAABB().center(), correctedPos);
-         ref->setPosition(correctedPos);
-         checkVitalParameters(bd, ref);
-         ref->setPosition(backupPos);
+         ref.setPosition(correctedPos);
+         checkVitalParameters(bd, &ref);
+         ref.setPosition(backupPos);
       } else
       {
-         WALBERLA_CHECK_EQUAL( storage[StorageType::LOCAL].size(), 0, "pos: " << ref->getPosition() << "\nradius: " << ref->getRadius() <<  "\ndomain: " << block.getAABB() );
-         WALBERLA_CHECK_EQUAL( shadowStorage.size(), 0, "pos: " << ref->getPosition() << "\nradius: " << ref->getRadius() <<  "\ndomain: " << block.getAABB() );
+         WALBERLA_CHECK_EQUAL( storage[StorageType::LOCAL].size(), 0, "pos: " << ref.getPosition() << "\nradius: " << ref.getRadius() <<  "\ndomain: " << block.getAABB() );
+         WALBERLA_CHECK_EQUAL( shadowStorage.size(), 0, "pos: " << ref.getPosition() << "\nradius: " << ref.getRadius() <<  "\ndomain: " << block.getAABB() );
       }
    }
-   WALBERLA_LOG_PROGRESS("checked pos: " << ref->getPosition() << " | new pos: " << newPos);
+   WALBERLA_LOG_PROGRESS("checked pos: " << ref.getPosition() << " | new pos: " << newPos);
    auto temp = newPos;
    forest.mapToPeriodicDomain(temp);
-   ref->setPosition(temp);
+   ref.setPosition(temp);
 }
 
 int main( int argc, char ** argv )
@@ -117,9 +117,9 @@ int main( int argc, char ** argv )
 
    MaterialID iron = Material::find("iron");
    walberla::id_t sid = 123;
-   SphereID refSphere = new Sphere(1, 0, Vec3(15, 15, 15), Vec3(0,0,0), Quat(), 3, iron, false, true, false);
-   refSphere->setLinearVel(4, 5, 6);
-   refSphere->setAngularVel( 1, 2, 3);
+   Sphere refSphere(1, 0, Vec3(15, 15, 15), Vec3(0,0,0), Quat(), 3, iron, false, true, false);
+   refSphere.setLinearVel(4, 5, 6);
+   refSphere.setAngularVel( 1, 2, 3);
    Vec3 gpos = Vec3(15, 15, 15);
 
    SphereID sphere = createSphere( globalStorage, forest->getBlockStorage(), storageID, 0, gpos, 3);
diff --git a/tests/pe/SynchronizationLargeBody.cpp b/tests/pe/SynchronizationLargeBody.cpp
index ab7123da2b10ac22081e798867e8584315fdc9d7..6bf99cc4700baa403698e23964c9a73139c14953 100644
--- a/tests/pe/SynchronizationLargeBody.cpp
+++ b/tests/pe/SynchronizationLargeBody.cpp
@@ -51,7 +51,7 @@ void checkSphere(StructuredBlockForest& forest, BlockDataID storageID, walberla:
       {
          WALBERLA_CHECK_EQUAL( storage[StorageType::LOCAL].size(), 1 );
          WALBERLA_CHECK_EQUAL( shadowStorage.size(), 0 );
-         SphereID bd = static_cast<SphereID> (*(storage[0].find( sid )));
+         SphereID bd = static_cast<SphereID> (storage[0].find( sid ).getBodyID());
          WALBERLA_CHECK_NOT_NULLPTR(bd);
          checkVitalParameters(bd, ref);
          bd->setPosition( newPos );
@@ -59,7 +59,7 @@ void checkSphere(StructuredBlockForest& forest, BlockDataID storageID, walberla:
       {
          WALBERLA_CHECK_EQUAL( storage[0].size(), 0 );
          WALBERLA_CHECK_EQUAL( shadowStorage.size(), 1 );
-         SphereID bd = static_cast<SphereID> (*(shadowStorage.find( sid )));
+         SphereID bd = static_cast<SphereID> (shadowStorage.find( sid ).getBodyID());
          WALBERLA_CHECK_NOT_NULLPTR(bd);
          auto backupPos =ref->getPosition();
          auto correctedPos = ref->getPosition();
@@ -92,7 +92,7 @@ void checkSphere(StructuredBlockForest& forest, BlockDataID storageID, walberla:
       {
          WALBERLA_CHECK_EQUAL( storage[StorageType::LOCAL].size(), 1 );
          WALBERLA_CHECK_EQUAL( shadowStorage.size(), 0 );
-         SphereID bd = static_cast<SphereID> (*(storage[0].find( sid )));
+         SphereID bd = static_cast<SphereID> (storage[0].find( sid ).getBodyID());
          WALBERLA_CHECK_NOT_NULLPTR(bd);
          checkVitalParameters(bd, ref);
          bd->setPosition( newPos );
@@ -100,7 +100,7 @@ void checkSphere(StructuredBlockForest& forest, BlockDataID storageID, walberla:
       {
          WALBERLA_CHECK_EQUAL( storage[0].size(), 0 );
          WALBERLA_CHECK_EQUAL( shadowStorage.size(), 1, "ref_sphere: " << ref << "\n" << block.getAABB() );
-         SphereID bd = static_cast<SphereID> (*(shadowStorage.find( sid )));
+         SphereID bd = static_cast<SphereID> (shadowStorage.find( sid ).getBodyID());
          WALBERLA_CHECK_NOT_NULLPTR(bd);
          auto backupPos =ref->getPosition();
          auto correctedPos = ref->getPosition();
@@ -161,9 +161,9 @@ int main( int argc, char ** argv )
    walberla::id_t sid = 123;
    Vec3 gpos = Vec3(3.5, 3.5, 3.5);
    const real_t r = real_c(1.6);
-   SphereID refSphere = new Sphere(1, 0, gpos, Vec3(0,0,0), Quat(), r, iron, false, true, false);
-   refSphere->setLinearVel(4, 5, 6);
-   refSphere->setAngularVel( 1, 2, 3);
+   Sphere refSphere(1, 0, gpos, Vec3(0,0,0), Quat(), r, iron, false, true, false);
+   refSphere.setLinearVel(4, 5, 6);
+   refSphere.setAngularVel( 1, 2, 3);
 
 
    SphereID sphere = createSphere( *globalStorage, forest->getBlockStorage(), storageID, 0, gpos, r);
@@ -203,10 +203,10 @@ int main( int argc, char ** argv )
       for (int i = 0; i < 21; ++i)
       {
          syncShadowOwners<BodyTuple>(forest->getBlockForest(), storageID);
-         Vec3 pos = refSphere->getPosition() + delta;
+         Vec3 pos = refSphere.getPosition() + delta;
          if (!forest->getDomain().contains( pos, real_c(0.5) ))
             forest->mapToPeriodicDomain(pos);
-         checkSphere(*forest, storageID, sid, refSphere, pos);
+         checkSphere(*forest, storageID, sid, &refSphere, pos);
       }
    }
    WALBERLA_LOG_PROGRESS("TEST WITHOUT DX ... finished");
@@ -225,10 +225,10 @@ int main( int argc, char ** argv )
       for (int i = 0; i < 21; ++i)
       {
          syncShadowOwners<BodyTuple>(forest->getBlockForest(), storageID, NULL, dx);
-         Vec3 pos = refSphere->getPosition() + delta;
+         Vec3 pos = refSphere.getPosition() + delta;
          if (!forest->getDomain().contains( pos, real_c(0.5) ))
             forest->mapToPeriodicDomain(pos);
-         checkSphere(*forest, storageID, sid, refSphere, pos, dx);
+         checkSphere(*forest, storageID, sid, &refSphere, pos, dx);
       }
    }
    syncShadowOwners<BodyTuple>(forest->getBlockForest(), storageID, NULL, dx);
diff --git a/tests/pe/Union.cpp b/tests/pe/Union.cpp
index 6cd0aec53e1e26b978b0085a67a04ae74b15fc0e..1a814c28a1a6a789eadf5a34d1c352f23b0baf8b 100644
--- a/tests/pe/Union.cpp
+++ b/tests/pe/Union.cpp
@@ -25,6 +25,7 @@
 
 #include "pe/basic.h"
 #include "pe/rigidbody/Union.h"
+#include "pe/rigidbody/UnionFactory.h"
 #include "pe/ccd/SimpleCCDDataHandling.h"
 #include "pe/synchronization/SyncNextNeighbors.h"
 #include "pe/vtk/BodyVtkOutput.h"
@@ -75,12 +76,9 @@ void SnowManFallingOnPlane()
 
    createPlane( *globalBodyStorage, 0, Vec3(0,0,1), Vec3(0,0,0) );
 
-   MaterialID iron = Material::find("iron");
-   UnionType* un   = createUnion< boost::tuple<Sphere> >( *globalBodyStorage, forest->getBlockStorage(), storageID, 0, Vec3(5,5,5) );
-   SphereID sp1 = new Sphere( 10, 0, Vec3(5,5,1), Vec3(0,0,0), Quat(), real_t(1)  , iron, false, true, false );
-   SphereID sp2 = new Sphere( 11, 0, Vec3(real_t(6.7),5,real_t(1.2)), Vec3(0,0,0), Quat(), real_t(1.1), iron, false, true, false );
-   un->add(sp1);
-   un->add(sp2);
+   UnionType* un  = createUnion< boost::tuple<Sphere> >( *globalBodyStorage, forest->getBlockStorage(), storageID, 0, Vec3(5,5,5) );
+   auto sp1       = createSphere(un, 10, Vec3(5,5,1), real_t(1));
+   auto sp2       = createSphere(un, 11, Vec3(real_t(6.7),5,real_t(1.2)), real_t(1.1));
 
    auto distance = (sp1->getPosition() - sp2->getPosition()).length();
 
@@ -106,14 +104,15 @@ void ImpulsCarryover()
 {
    MaterialID iron = Material::find("iron");
 
-   UnionType* un   = new Union< boost::tuple<Sphere> >(12, 0, Vec3(0,0,0), Vec3(0,0,0), Quat(), false, true, false);
-   SphereID sp1    = new Sphere( 10, 0, Vec3( 1,0,0), Vec3(0,0,0), Quat(), real_t(1), iron, false, true, false );
-   SphereID sp2    = new Sphere( 11, 0, Vec3(-1,0,0), Vec3(0,0,0), Quat(), real_t(1), iron, false, true, false );
+   auto un  = std::make_unique<UnionType>(12, 0, Vec3(0,0,0), Vec3(0,0,0), Quat(), false, true, false);
+   auto sp1 = std::make_unique<Sphere>( 10, 0, Vec3( 1,0,0), Vec3(0,0,0), Quat(), real_t(1), iron, false, true, false );
+   auto sp2 = std::make_unique<Sphere>( 11, 0, Vec3(-1,0,0), Vec3(0,0,0), Quat(), real_t(1), iron, false, true, false );
+
    sp1->setLinearVel(Vec3(0,real_c(+1),0));
    sp2->setLinearVel(Vec3(0,real_c(-1),0));
 
-   un->add(sp1);
-   un->add(sp2);
+   un->add( std::move(sp1) );
+   un->add( std::move(sp2) );
 
    WALBERLA_CHECK_FLOAT_EQUAL( un->getPosition(),  Vec3(0,0,0) );
    WALBERLA_CHECK_FLOAT_EQUAL( un->getLinearVel(), Vec3(0,0,0) );
diff --git a/tests/pe_coupling/momentum_exchange_method/LubricationCorrectionMEM.cpp b/tests/pe_coupling/momentum_exchange_method/LubricationCorrectionMEM.cpp
index 124cbad2177e85087ac82ee78c9263d3dd61d498..26a984714fff6dac3d8c0452df6e79331aacffc6 100644
--- a/tests/pe_coupling/momentum_exchange_method/LubricationCorrectionMEM.cpp
+++ b/tests/pe_coupling/momentum_exchange_method/LubricationCorrectionMEM.cpp
@@ -165,14 +165,14 @@ private:
       {
          for( auto curSphereIt = pe::BodyIterator::begin<pe::Sphere>( *blockIt, bodyStorageID_); curSphereIt != pe::BodyIterator::end<pe::Sphere>(); ++curSphereIt )
          {
-            pe::SphereID sphereI = ( *curSphereIt );
+            pe::SphereID sphereI = ( curSphereIt.getBodyID() );
             if ( sphereI->getID() == id1_ )
             {
                for( auto blockIt2 = blocks_->begin(); blockIt2 != blocks_->end(); ++blockIt2 )
                {
                   for( auto oSphereIt = pe::BodyIterator::begin<pe::Sphere>( *blockIt2, bodyStorageID_); oSphereIt != pe::BodyIterator::end<pe::Sphere>(); ++oSphereIt )
                   {
-                     pe::SphereID sphereJ = ( *oSphereIt );
+                     pe::SphereID sphereJ = ( oSphereIt.getBodyID() );
                      if ( sphereJ->getID() == id2_ )
                      {
                         gap = pe::getSurfaceDistance( sphereI, sphereJ );
@@ -285,14 +285,14 @@ private:
       {
          for( auto curSphereIt = pe::BodyIterator::begin<pe::Sphere>( *blockIt, bodyStorageID_); curSphereIt != pe::BodyIterator::end<pe::Sphere>(); ++curSphereIt )
          {
-            pe::SphereID sphereI = ( *curSphereIt );
+            pe::SphereID sphereI = ( curSphereIt.getBodyID() );
             if ( sphereI->getID() == id1_ )
             {
                for( auto globalBodyIt = globalBodyStorage_->begin(); globalBodyIt != globalBodyStorage_->end(); ++globalBodyIt)
                {
                   if( globalBodyIt->getID() == id2_ )
                   {
-                     pe::PlaneID planeJ = static_cast<pe::PlaneID>( *globalBodyIt );
+                     pe::PlaneID planeJ = static_cast<pe::PlaneID>( globalBodyIt.getBodyID() );
                      gap = pe::getSurfaceDistance(sphereI, planeJ);
                      break;
                   }