From e19c3161620c307b44451863dce68191036fe3fb Mon Sep 17 00:00:00 2001
From: Sebastian Eibl <sebastian.eibl@fau.de>
Date: Wed, 30 May 2018 16:50:18 +0200
Subject: [PATCH] replaced PtrVector by std::vector<std::unique_ptr<RigidBody>>

---
 src/mesh/pe/vtk/PeVTKMeshWriter.h             |   8 +-
 src/pe/Types.h                                |  60 +--
 src/pe/ccd/HashGrids.cpp                      |  40 +-
 src/pe/ccd/SimpleCCD.cpp                      |   8 +-
 src/pe/communication/ParseMessage.h           |  23 +-
 src/pe/cr/DEM.impl.h                          |   2 +-
 src/pe/cr/HCSITS.impl.h                       |  32 +-
 src/pe/cr/PlainIntegrator.impl.h              |   2 +-
 src/pe/rigidbody/BodyIterators.h              | 501 +++++++++---------
 src/pe/rigidbody/BodyStorage.h                | 382 ++++++++-----
 src/pe/rigidbody/RigidBodyCastIterator.h      | 332 ++++++++++++
 src/pe/rigidbody/RigidBodyIterator.h          | 413 +++++++++++++++
 src/pe/rigidbody/StorageDataHandling.h        |  12 +-
 src/pe/synchronization/RemoveAndNotify.h      |   4 +-
 src/pe/synchronization/SyncForces.h           |  10 +-
 src/pe/synchronization/SyncNextNeighbors.h    |   7 +-
 src/pe/synchronization/SyncShadowOwners.h     |   9 +-
 src/pe/utility/DestroyBody.h                  |   6 +-
 src/pe/utility/GetBody.cpp                    |   6 +-
 src/pe/vtk/BodyVtkOutput.cpp                  |   8 +-
 src/pe/vtk/EllipsoidVtkOutput.cpp             |  10 +-
 src/pe/vtk/EllipsoidVtkOutput.h               |   2 +-
 src/pe/vtk/SphereVtkOutput.cpp                |  34 +-
 src/pe/vtk/SphereVtkOutput.h                  |   2 +-
 .../evaluators/AddedMassForceEvaluator.h      |   2 +-
 .../BodyVelocityTimeDerivativeEvaluator.h     |   2 +-
 .../evaluators/InteractionForceEvaluator.h    |   4 +-
 .../evaluators/LiftForceEvaluator.h           |   4 +-
 .../evaluators/LubricationForceEvaluator.h    |  10 +-
 .../SolidVolumeFractionFieldEvaluator.h       |   2 +-
 .../utility/BodyVelocityInitializer.h         |   2 +-
 src/pe_coupling/mapping/BodyMapping.h         |   8 +-
 .../momentum_exchange_method/BodyMapping.h    |  16 +-
 .../restoration/PDFReconstruction.h           |  16 +-
 .../BodyAndVolumeFractionMapping.h            |  16 +-
 .../utility/LubricationCorrection.h           |  10 +-
 tests/pe/BodyIterators.cpp                    |   2 +-
 tests/pe/ForceSync.cpp                        |   8 +-
 tests/pe/HashGrids.cpp                        |   3 +-
 tests/pe/ParallelEquivalence.cpp              |   2 +-
 tests/pe/SimpleCCD.cpp                        |   2 +-
 tests/pe/SyncEquivalence.cpp                  |   2 +-
 tests/pe/Synchronization.cpp                  |   4 +-
 tests/pe/SynchronizationLargeBody.cpp         |   8 +-
 .../LubricationCorrectionMEM.cpp              |   8 +-
 45 files changed, 1426 insertions(+), 618 deletions(-)
 create mode 100644 src/pe/rigidbody/RigidBodyCastIterator.h
 create mode 100644 src/pe/rigidbody/RigidBodyIterator.h

diff --git a/src/mesh/pe/vtk/PeVTKMeshWriter.h b/src/mesh/pe/vtk/PeVTKMeshWriter.h
index 55721ae20..72685c362 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/Types.h b/src/pe/Types.h
index 764bf4bb7..c71ee8090 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/ccd/HashGrids.cpp b/src/pe/ccd/HashGrids.cpp
index 31d4c96cc..ce6b8c993 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 dee87d16b..2fcb2b9e6 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/communication/ParseMessage.h b/src/pe/communication/ParseMessage.h
index 5735beb15..96b422cd7 100644
--- a/src/pe/communication/ParseMessage.h
+++ b/src/pe/communication/ParseMessage.h
@@ -88,7 +88,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 +112,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 +142,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 +163,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 +183,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 +202,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 +220,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 +228,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 +259,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 +291,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/cr/DEM.impl.h b/src/pe/cr/DEM.impl.h
index eb3a6b4e3..afb7debf6 100644
--- a/src/pe/cr/DEM.impl.h
+++ b/src/pe/cr/DEM.impl.h
@@ -131,7 +131,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
diff --git a/src/pe/cr/HCSITS.impl.h b/src/pe/cr/HCSITS.impl.h
index 457cd4c29..7046d8a8e 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_);
       }
    }
diff --git a/src/pe/cr/PlainIntegrator.impl.h b/src/pe/cr/PlainIntegrator.impl.h
index b96a6e2e0..79c401060 100644
--- a/src/pe/cr/PlainIntegrator.impl.h
+++ b/src/pe/cr/PlainIntegrator.impl.h
@@ -74,7 +74,7 @@ void PlainIntegratorSolver<Integrator>::timestep( const real_t dt )
 
          // 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/rigidbody/BodyIterators.h b/src/pe/rigidbody/BodyIterators.h
index aaf7d7daf..5f4ef40fc 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 0ab0ef40b..dc4775ded 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   ();
+   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/RigidBodyCastIterator.h b/src/pe/rigidbody/RigidBodyCastIterator.h
new file mode 100644
index 000000000..22b148eff
--- /dev/null
+++ b/src/pe/rigidbody/RigidBodyCastIterator.h
@@ -0,0 +1,332 @@
+//======================================================================================================================
+//
+//  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.
+ * \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 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 000000000..a5401e3ba
--- /dev/null
+++ b/src/pe/rigidbody/RigidBodyIterator.h
@@ -0,0 +1,413 @@
+//======================================================================================================================
+//
+//  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.
+ * \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
+ */
+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/StorageDataHandling.h b/src/pe/rigidbody/StorageDataHandling.h
index 6e3b0d2da..ae407845d 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 );
    }
 }
 
diff --git a/src/pe/synchronization/RemoveAndNotify.h b/src/pe/synchronization/RemoveAndNotify.h
index 8eaf880fe..6c8d27c0d 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 0bca53541..aa83e6519 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 56da2bc95..63aa9a208 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 e02cea958..cc0400d11 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/DestroyBody.h b/src/pe/utility/DestroyBody.h
index 189203fda..20da0f1c8 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 31fc394cc..6d0b695f8 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 b2e154dc1..6d13f3df3 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 4ec85db38..816fb2028 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 e12f91879..680410dce 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 90672a0f6..626685a43 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 238c1fa8e..1878795f3 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 a318686d8..c452f8864 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 229be174e..7345398b7 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 33a2e7d76..17d312189 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 d04ca701c..de8518349 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 e99d3e88f..63723eaa4 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 1c83df74a..586b51bc2 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 75ebeef5b..9ffbad829 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 4af2e9d31..48f05c2ee 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 7651386b6..c44bc77e6 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 d21808bb3..808fc089f 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 9f28d42e6..1a387105b 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 c318c66c6..4a5afa830 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/pe/BodyIterators.cpp b/tests/pe/BodyIterators.cpp
index a14fc38c5..0dbfcc53c 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/ForceSync.cpp b/tests/pe/ForceSync.cpp
index f0901d1b7..b97d40807 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 a1790653b..7b203cbe8 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/ParallelEquivalence.cpp b/tests/pe/ParallelEquivalence.cpp
index 6a211cd67..03aac6f6f 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/SimpleCCD.cpp b/tests/pe/SimpleCCD.cpp
index 3bb653908..3e160acdd 100644
--- a/tests/pe/SimpleCCD.cpp
+++ b/tests/pe/SimpleCCD.cpp
@@ -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();
diff --git a/tests/pe/SyncEquivalence.cpp b/tests/pe/SyncEquivalence.cpp
index c311e1d67..8742ac375 100644
--- a/tests/pe/SyncEquivalence.cpp
+++ b/tests/pe/SyncEquivalence.cpp
@@ -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 c8ad534ff..41d003fb6 100644
--- a/tests/pe/Synchronization.cpp
+++ b/tests/pe/Synchronization.cpp
@@ -51,7 +51,7 @@ void checkSphere(StructuredBlockForest& forest, BlockDataID storageID, walberla:
       {
          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 )));
+         SphereID bd = static_cast<SphereID> (storage[StorageType::LOCAL].find( sid ).getBodyID());
          WALBERLA_CHECK_NOT_NULLPTR(bd);
          checkVitalParameters(bd, ref);
          WALBERLA_LOG_DEVEL("#shadows: " << bd->MPITrait.sizeShadowOwners() << " #block states set: " << bd->MPITrait.getBlockStateSize() << "\nowner domain: " << block.getAABB() << "\nowner: " << bd->MPITrait.getOwner());
@@ -60,7 +60,7 @@ void checkSphere(StructuredBlockForest& forest, BlockDataID storageID, walberla:
       {
          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 )));
+         SphereID bd = static_cast<SphereID> (shadowStorage.find( sid ).getBodyID());
          WALBERLA_CHECK_NOT_NULLPTR(bd);
          auto backupPos =ref->getPosition();
          auto correctedPos = ref->getPosition();
diff --git a/tests/pe/SynchronizationLargeBody.cpp b/tests/pe/SynchronizationLargeBody.cpp
index ab7123da2..aaef02acb 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();
diff --git a/tests/pe_coupling/momentum_exchange_method/LubricationCorrectionMEM.cpp b/tests/pe_coupling/momentum_exchange_method/LubricationCorrectionMEM.cpp
index 124cbad21..26a984714 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;
                   }
-- 
GitLab