diff --git a/.gitlab-ci.yml b/.gitlab-ci.yml
index 1668b816f8ae5f88a920925fd4e6e00f333f1bb2..e16e20a2378830a2a792c94db732fda93509120f 100644
--- a/.gitlab-ci.yml
+++ b/.gitlab-ci.yml
@@ -745,6 +745,97 @@ gcc_8_mpionly:
 gcc_8_hybrid:
    <<: *build_definition
    image: i10git.cs.fau.de:5005/walberla/buildenvs/gcc:8
+   variables:
+      <<: *build_hybrid_variables
+      WALBERLA_BUILD_WITH_CUDA: "OFF"
+      WALBERLA_ENABLE_GUI: 0
+   only:
+      variables:
+         - $ENABLE_NIGHTLY_BUILDS
+   tags:
+      - docker
+
+gcc_8_serial_dbg:
+   <<: *build_definition
+   image: i10git.cs.fau.de:5005/walberla/buildenvs/gcc:8
+   variables:
+      <<: *build_serial_dbg_variables
+      WALBERLA_BUILD_WITH_CUDA: "OFF"
+      WALBERLA_ENABLE_GUI: 0
+   only:
+      variables:
+         - $ENABLE_NIGHTLY_BUILDS
+   tags:
+      - docker
+
+gcc_8_mpionly_dbg:
+   <<: *build_definition
+   image: i10git.cs.fau.de:5005/walberla/buildenvs/gcc:8
+   variables:
+      <<: *build_mpionly_dbg_variables
+      WALBERLA_BUILD_WITH_CUDA: "OFF"
+      WALBERLA_ENABLE_GUI: 0
+   only:
+      variables:
+         - $ENABLE_NIGHTLY_BUILDS
+   tags:
+      - docker
+
+gcc_8_hybrid_dbg:
+   <<: *build_definition
+   image: i10git.cs.fau.de:5005/walberla/buildenvs/gcc:8
+   variables:
+      <<: *build_hybrid_dbg_variables
+      WALBERLA_BUILD_WITH_CUDA: "OFF"
+      WALBERLA_ENABLE_GUI: 0
+   only:
+      variables:
+         - $ENABLE_NIGHTLY_BUILDS
+   tags:
+      - docker
+
+gcc_8_hybrid_dbg_sp:
+   <<: *build_definition
+   image: i10git.cs.fau.de:5005/walberla/buildenvs/gcc:8
+   variables:
+      <<: *build_hybrid_dbg_sp_variables
+      WALBERLA_BUILD_WITH_CUDA: "OFF"
+      WALBERLA_ENABLE_GUI: 0
+   only:
+      variables:
+         - $ENABLE_NIGHTLY_BUILDS
+   tags:
+      - docker
+
+gcc_9_serial:
+   <<: *build_definition
+   image: i10git.cs.fau.de:5005/walberla/buildenvs/gcc:9
+   variables:
+      <<: *build_serial_variables
+      WALBERLA_BUILD_WITH_CUDA: "OFF"
+      WALBERLA_ENABLE_GUI: 0
+   only:
+      variables:
+         - $ENABLE_NIGHTLY_BUILDS
+   tags:
+      - docker
+
+gcc_9_mpionly:
+   <<: *build_definition
+   image: i10git.cs.fau.de:5005/walberla/buildenvs/gcc:9
+   variables:
+      <<: *build_mpionly_variables
+      WALBERLA_BUILD_WITH_CUDA: "OFF"
+      WALBERLA_ENABLE_GUI: 0
+   only:
+      variables:
+         - $ENABLE_NIGHTLY_BUILDS
+   tags:
+      - docker
+
+gcc_9_hybrid:
+   <<: *build_definition
+   image: i10git.cs.fau.de:5005/walberla/buildenvs/gcc:9
    stage: pretest
    variables:
       <<: *build_hybrid_variables
@@ -756,9 +847,9 @@ gcc_8_hybrid:
    tags:
       - docker
 
-gcc_8_serial_dbg:
+gcc_9_serial_dbg:
    <<: *build_definition
-   image: i10git.cs.fau.de:5005/walberla/buildenvs/gcc:8
+   image: i10git.cs.fau.de:5005/walberla/buildenvs/gcc:9
    variables:
       <<: *build_serial_dbg_variables
       WALBERLA_BUILD_WITH_CUDA: "OFF"
@@ -769,9 +860,9 @@ gcc_8_serial_dbg:
    tags:
       - docker
 
-gcc_8_mpionly_dbg:
+gcc_9_mpionly_dbg:
    <<: *build_definition
-   image: i10git.cs.fau.de:5005/walberla/buildenvs/gcc:8
+   image: i10git.cs.fau.de:5005/walberla/buildenvs/gcc:9
    variables:
       <<: *build_mpionly_dbg_variables
       WALBERLA_BUILD_WITH_CUDA: "OFF"
@@ -782,9 +873,9 @@ gcc_8_mpionly_dbg:
    tags:
       - docker
 
-gcc_8_hybrid_dbg:
+gcc_9_hybrid_dbg:
    <<: *build_definition
-   image: i10git.cs.fau.de:5005/walberla/buildenvs/gcc:8
+   image: i10git.cs.fau.de:5005/walberla/buildenvs/gcc:9
    variables:
       <<: *build_hybrid_dbg_variables
       WALBERLA_BUILD_WITH_CUDA: "OFF"
@@ -795,9 +886,9 @@ gcc_8_hybrid_dbg:
    tags:
       - docker
 
-gcc_8_hybrid_dbg_sp:
+gcc_9_hybrid_dbg_sp:
    <<: *build_definition
-   image: i10git.cs.fau.de:5005/walberla/buildenvs/gcc:8
+   image: i10git.cs.fau.de:5005/walberla/buildenvs/gcc:9
    variables:
       <<: *build_hybrid_dbg_sp_variables
       WALBERLA_BUILD_WITH_CUDA: "OFF"
@@ -1786,4 +1877,4 @@ benchmark_gcc8:
 
 benchmark_clang8:
    <<: *benchmark_definition
-   image: i10git.cs.fau.de:5005/walberla/buildenvs/clang:8.0
+   image: i10git.cs.fau.de:5005/walberla/buildenvs/clang:8.0
\ No newline at end of file
diff --git a/apps/benchmarks/ComplexGeometry/ComplexGeometry.cpp b/apps/benchmarks/ComplexGeometry/ComplexGeometry.cpp
index 6272dbc8d97504afc1d18d65b9b289d297e896b4..e8d56f6c3af993167745bf4e0763f750ad35d408 100644
--- a/apps/benchmarks/ComplexGeometry/ComplexGeometry.cpp
+++ b/apps/benchmarks/ComplexGeometry/ComplexGeometry.cpp
@@ -57,6 +57,7 @@
 #include "mesh/TriangleMeshes.h"
 #include "mesh/MeshOperations.h"
 #include "mesh/DistanceComputations.h"
+#include "mesh/DistanceFunction.h"
 #include "mesh/MeshIO.h"
 #include "mesh/MatrixVectorOperations.h"
 #include "mesh/blockforest/BlockForestInitialization.h"
@@ -67,6 +68,7 @@
 #include "mesh/boundary/BoundarySetup.h"
 #include "mesh/boundary/BoundaryInfo.h"
 #include "mesh/boundary/BoundaryLocation.h"
+#include "mesh/boundary/BoundaryLocationFunction.h"
 #include "mesh/boundary/BoundaryUIDFaceDataSource.h"
 #include "mesh/boundary/ColorToBoundaryMapper.h"
 #include "mesh/vtk/VTKMeshWriter.h"
@@ -84,47 +86,6 @@
 
 namespace walberla {
 
-
-template< typename MeshDistanceType >
-struct MeshDistanceFunction
-{
-   MeshDistanceFunction( const shared_ptr< MeshDistanceType > & meshDistanceObject ) : meshDistanceObject_( meshDistanceObject ) { }
-
-   inline real_t operator()( const Vector3< real_t > & p ) const { return real_c( meshDistanceObject_->sqSignedDistance( mesh::toOpenMesh( p ) ) ); }
-
-   shared_ptr< MeshDistanceType > meshDistanceObject_;
-};
-
-template< typename MeshDistanceType >
-inline MeshDistanceFunction< MeshDistanceType > makeMeshDistanceFunction( const shared_ptr< MeshDistanceType > & meshDistanceObject )
-{
-   return MeshDistanceFunction< MeshDistanceType >( meshDistanceObject );
-}
-
-
-template< typename MeshDistanceType, typename MeshType >
-struct BoundaryLocationFunction
-{
-   BoundaryLocationFunction( const shared_ptr< MeshDistanceType > & meshDistanceObject, const shared_ptr< mesh::BoundaryLocation< MeshType > > & boundaryLocation )
-      : meshDistanceObject_( meshDistanceObject ), boundaryLocation_( boundaryLocation ) { }
-
-   inline const mesh::BoundaryInfo & operator()( const Vector3< real_t > & p ) const
-   {
-      typename MeshType::FaceHandle fh;
-      meshDistanceObject_->sqSignedDistance( mesh::toOpenMesh( p ), fh );
-      return (*boundaryLocation_)[ fh ];
-   }
-
-   shared_ptr< MeshDistanceType > meshDistanceObject_;
-   shared_ptr< mesh::BoundaryLocation< MeshType > > boundaryLocation_;
-};
-
-template< typename MeshDistanceType, typename MeshType >
-inline BoundaryLocationFunction< MeshDistanceType, MeshType > makeBoundaryLocationFunction( const shared_ptr< MeshDistanceType > & meshDistanceObject, const shared_ptr< mesh::BoundaryLocation< MeshType > > & boundaryLocation )
-{
-   return BoundaryLocationFunction< MeshDistanceType, MeshType >( meshDistanceObject, boundaryLocation );
-}
-
 template< typename MeshType >
 void vertexToFaceColor( MeshType & mesh, const typename MeshType::Color & defaultColor )
 {
diff --git a/apps/benchmarks/GranularGas/MESA_PD_GranularGas.cpp b/apps/benchmarks/GranularGas/MESA_PD_GranularGas.cpp
index 18874901dc71d904af29d874c10b421f7aee094a..3ccff9ff43fc2614a95876cc3ce1fbfac7a74c38 100644
--- a/apps/benchmarks/GranularGas/MESA_PD_GranularGas.cpp
+++ b/apps/benchmarks/GranularGas/MESA_PD_GranularGas.cpp
@@ -74,19 +74,15 @@ class ParticleAccessorWithShape : public data::ParticleAccessor
 {
 public:
    ParticleAccessorWithShape(std::shared_ptr<data::ParticleStorage>& ps, std::shared_ptr<data::ShapeStorage>& ss)
-      : ParticleAccessor(ps)
-      , ss_(ss)
+         : ParticleAccessor(ps)
+         , ss_(ss)
    {}
 
-   const walberla::real_t& getInvMass(const size_t p_idx) const {return ss_->shapes[ps_->getShapeIDRef(p_idx)]->getInvMass();}
-   walberla::real_t& getInvMassRef(const size_t p_idx) {return ss_->shapes[ps_->getShapeIDRef(p_idx)]->getInvMass();}
-   void setInvMass(const size_t p_idx, const walberla::real_t& v) { ss_->shapes[ps_->getShapeIDRef(p_idx)]->getInvMass() = v;}
+   const auto& getInvMass(const size_t p_idx) const {return ss_->shapes[ps_->getShapeID(p_idx)]->getInvMass();}
 
-   const auto& getInvInertiaBF(const size_t p_idx) const {return ss_->shapes[ps_->getShapeIDRef(p_idx)]->getInvInertiaBF();}
-   auto& getInvInertiaBFRef(const size_t p_idx) {return ss_->shapes[ps_->getShapeIDRef(p_idx)]->getInvInertiaBF();}
-   void setInvInertiaBF(const size_t p_idx, const Mat3& v) { ss_->shapes[ps_->getShapeIDRef(p_idx)]->getInvInertiaBF() = v;}
+   const auto& getInvInertiaBF(const size_t p_idx) const {return ss_->shapes[ps_->getShapeID(p_idx)]->getInvInertiaBF();}
 
-   data::BaseShape* getShape(const size_t p_idx) const {return ss_->shapes[ps_->getShapeIDRef(p_idx)].get();}
+   data::BaseShape* getShape(const size_t p_idx) const {return ss_->shapes[ps_->getShapeID(p_idx)].get();}
 private:
    std::shared_ptr<data::ShapeStorage> ss_;
 };
diff --git a/apps/benchmarks/GranularGas/MESA_PD_KernelBenchmark.cpp b/apps/benchmarks/GranularGas/MESA_PD_KernelBenchmark.cpp
index 723500d58537a6a310a4094abbb75c61759c34ae..47d20d42a087603fc02db32eb9e1363b3af8e5e0 100644
--- a/apps/benchmarks/GranularGas/MESA_PD_KernelBenchmark.cpp
+++ b/apps/benchmarks/GranularGas/MESA_PD_KernelBenchmark.cpp
@@ -99,19 +99,15 @@ class ParticleAccessorWithShape : public data::ParticleAccessor
 {
 public:
    ParticleAccessorWithShape(std::shared_ptr<data::ParticleStorage>& ps, std::shared_ptr<data::ShapeStorage>& ss)
-      : ParticleAccessor(ps)
-      , ss_(ss)
+         : ParticleAccessor(ps)
+         , ss_(ss)
    {}
 
-   const walberla::real_t& getInvMass(const size_t p_idx) const {return ss_->shapes[ps_->getShapeIDRef(p_idx)]->getInvMass();}
-   walberla::real_t& getInvMassRef(const size_t p_idx) {return ss_->shapes[ps_->getShapeIDRef(p_idx)]->getInvMass();}
-   void setInvMass(const size_t p_idx, const walberla::real_t& v) { ss_->shapes[ps_->getShapeIDRef(p_idx)]->getInvMass() = v;}
+   const auto& getInvMass(const size_t p_idx) const {return ss_->shapes[ps_->getShapeID(p_idx)]->getInvMass();}
 
-   const auto& getInvInertiaBF(const size_t p_idx) const {return ss_->shapes[ps_->getShapeIDRef(p_idx)]->getInvInertiaBF();}
-   auto& getInvInertiaBFRef(const size_t p_idx) {return ss_->shapes[ps_->getShapeIDRef(p_idx)]->getInvInertiaBF();}
-   void setInvInertiaBF(const size_t p_idx, const Mat3& v) { ss_->shapes[ps_->getShapeIDRef(p_idx)]->getInvInertiaBF() = v;}
+   const auto& getInvInertiaBF(const size_t p_idx) const {return ss_->shapes[ps_->getShapeID(p_idx)]->getInvInertiaBF();}
 
-   data::BaseShape* getShape(const size_t p_idx) const {return ss_->shapes[ps_->getShapeIDRef(p_idx)].get();}
+   data::BaseShape* getShape(const size_t p_idx) const {return ss_->shapes[ps_->getShapeID(p_idx)].get();}
 private:
    std::shared_ptr<data::ShapeStorage> ss_;
 };
diff --git a/python/mesa_pd.py b/python/mesa_pd.py
index 3f8c8a002b2ad452119186af250c504e825b8839..949463d66847775c975ed0425ec70f7b159ff493 100755
--- a/python/mesa_pd.py
+++ b/python/mesa_pd.py
@@ -33,6 +33,7 @@ if __name__ == '__main__':
    ch    = data.ContactHistory()
    lc    = data.LinkedCells()
    ss    = data.ShapeStorage(ps, shapes)
+   cs    = data.ContactStorage()
 
    ps.addProperty("position",         "walberla::mesa_pd::Vec3", defValue="real_t(0)", syncMode="ALWAYS")
    ps.addProperty("linearVelocity",   "walberla::mesa_pd::Vec3", defValue="real_t(0)", syncMode="ALWAYS")
@@ -54,14 +55,37 @@ if __name__ == '__main__':
    ps.addProperty("oldContactHistory", "std::map<walberla::id_t, walberla::mesa_pd::data::ContactHistory>", defValue="", syncMode="ALWAYS")
    ps.addProperty("newContactHistory", "std::map<walberla::id_t, walberla::mesa_pd::data::ContactHistory>", defValue="", syncMode="NEVER")
 
-   ps.addProperty("temperature",      "walberla::real_t", defValue="real_t(0)", syncMode="ALWAYS")
-   ps.addProperty("heatFlux",         "walberla::real_t", defValue="real_t(0)", syncMode="NEVER")
+   ps.addProperty("temperature",      "walberla::real_t",        defValue="real_t(0)", syncMode="ALWAYS")
+   ps.addProperty("heatFlux",         "walberla::real_t",        defValue="real_t(0)", syncMode="NEVER")
+
+   # Properties for HCSITS
+   ps.addProperty("dv",               "walberla::mesa_pd::Vec3", defValue="real_t(0)", syncMode="NEVER")
+   ps.addProperty("dw",               "walberla::mesa_pd::Vec3", defValue="real_t(0)", syncMode="NEVER")
 
    ch.addProperty("tangentialSpringDisplacement", "walberla::mesa_pd::Vec3", defValue="real_t(0)")
    ch.addProperty("isSticking",                   "bool",                    defValue="false")
    ch.addProperty("impactVelocityMagnitude",      "real_t",                  defValue="real_t(0)")
 
+   cs.addProperty("id1",              "walberla::id_t",          defValue = "walberla::id_t(-1)", syncMode="NEVER")
+   cs.addProperty("id2",              "walberla::id_t",          defValue = "walberla::id_t(-1)", syncMode="NEVER")
+   cs.addProperty("distance",         "real_t",                  defValue = "real_t(1)",          syncMode="NEVER")
+   cs.addProperty("normal",           "walberla::mesa_pd::Vec3", defValue = "real_t(0)",          syncMode="NEVER")
+   cs.addProperty("position",         "walberla::mesa_pd::Vec3", defValue = "real_t(0)",          syncMode="NEVER")
+   cs.addProperty("t",                "walberla::mesa_pd::Vec3", defValue = "real_t(0)",          syncMode="NEVER")
+   cs.addProperty("o",                "walberla::mesa_pd::Vec3", defValue = "real_t(0)",          syncMode="NEVER")
+   cs.addProperty("r1",               "walberla::mesa_pd::Vec3", defValue = "real_t(0)",          syncMode="NEVER")
+   cs.addProperty("r2",               "walberla::mesa_pd::Vec3", defValue = "real_t(0)",          syncMode="NEVER")
+   cs.addProperty("mu",               "real_t",                  defValue = "real_t(0)",          syncMode="NEVER")
+   cs.addProperty("p",                "walberla::mesa_pd::Vec3", defValue = "real_t(0)",          syncMode="NEVER")
+   cs.addProperty("diag_nto",         "walberla::mesa_pd::Mat3", defValue = "real_t(0)",          syncMode="NEVER")
+   cs.addProperty("diag_nto_inv",     "walberla::mesa_pd::Mat3", defValue = "real_t(0)",          syncMode="NEVER")
+   cs.addProperty("diag_to_inv",      "walberla::mesa_pd::Mat2", defValue = "real_t(0)",          syncMode="NEVER")
+   cs.addProperty("diag_n_inv",       "real_t",                  defValue = "real_t(0)",          syncMode="NEVER")
+   cs.addProperty("p",                "walberla::mesa_pd::Vec3", defValue = "real_t(0)",          syncMode="NEVER")
+
+
    kernels = []
+   kernels.append( kernel.DetectAndStoreContacts() )
    kernels.append( kernel.DoubleCast(shapes) )
    kernels.append( kernel.ExplicitEuler() )
    kernels.append( kernel.ExplicitEulerWithShape() )
@@ -76,6 +100,7 @@ if __name__ == '__main__':
    kernels.append( kernel.VelocityVerlet() )
    kernels.append( kernel.VelocityVerletWithShape() )
 
+
    ac = Accessor()
    for k in kernels:
       ac.mergeRequirements(k.getRequirements())
@@ -86,6 +111,7 @@ if __name__ == '__main__':
    comm.append(mpi.ClearNextNeighborSync())
    comm.append(mpi.ReduceContactHistory())
    comm.append(mpi.ReduceProperty())
+   comm.append(mpi.SyncGhostOwners(ps))
    comm.append(mpi.SyncNextNeighbors(ps))
 
 
@@ -93,6 +119,7 @@ if __name__ == '__main__':
    ch.generate(args.path + "/src/mesa_pd/")
    lc.generate(args.path + "/src/mesa_pd/")
    ss.generate(args.path + "/src/mesa_pd/")
+   cs.generate(args.path + "/src/mesa_pd/")
 
    for k in kernels:
       k.generate(args.path + "/src/mesa_pd/")
diff --git a/python/mesa_pd/data/ContactStorage.py b/python/mesa_pd/data/ContactStorage.py
new file mode 100644
index 0000000000000000000000000000000000000000..2106759319ad24e71737118a3b8962eee8053b10
--- /dev/null
+++ b/python/mesa_pd/data/ContactStorage.py
@@ -0,0 +1,29 @@
+# -*- coding: utf-8 -*-
+
+import numpy as np
+from ..Container import Container
+from ..utility import generateFile
+
+class ContactStorage(Container):
+   def __init__(self):
+      super().__init__()
+      self.addProperty("uid",                "walberla::id_t",           defValue = "walberla::id_t(-1)",   syncMode="NEVER")
+
+   def generate(self, path):
+      self.unrollDimension()
+
+      print("="*90)
+      print("Creating ContactStorage Datastructure:")
+      print("")
+      print("{0: <20}{1: <30}{2: <20}{3: <10}".format("Type", "Name", "Def. Value", "SyncMode"))
+      print("="*90)
+      for prop in self.properties:
+         print("{0: <20.19}{1: <30.29}{2: <20.19}{3: <10.9}".format(prop.type, prop.name, prop.defValue, prop.syncMode))
+      print("="*90)
+
+      context = dict()
+      context["includes"]    = self.includes
+      context["properties"]  = self.properties
+
+      generateFile(path, 'data/ContactStorage.templ.h', context, filename='data/ContactStorage.h')
+      generateFile(path, 'data/ContactAccessor.templ.h', context, filename='data/ContactAccessor.h')
diff --git a/python/mesa_pd/data/ParticleStorage.py b/python/mesa_pd/data/ParticleStorage.py
index e4efc99993b946fb2dcf7bdbdeb6c7dfb39beaa5..21c21c381bd657def2210cf79e63924dd25fa70c 100644
--- a/python/mesa_pd/data/ParticleStorage.py
+++ b/python/mesa_pd/data/ParticleStorage.py
@@ -10,12 +10,12 @@ class ParticleStorage(Container):
 
       self.addInclude("mesa_pd/data/Flags.h")
 
-      self.addProperty("uid",               "walberla::id_t",      defValue = "UniqueID<data::Particle>::invalidID()", syncMode="ALWAYS")
+      self.addProperty("uid",               "walberla::id_t",       defValue = "UniqueID<data::Particle>::invalidID()", syncMode="ALWAYS")
       self.addProperty("position",          "walberla::mesa_pd::Vec3", defValue = "real_t(0)", syncMode="ALWAYS")
-      self.addProperty("interactionRadius", "walberla::real_t",    defValue = "real_t(0)", syncMode="COPY")
+      self.addProperty("interactionRadius", "walberla::real_t",     defValue = "real_t(0)", syncMode="COPY")
       self.addProperty("flags",             "walberla::mesa_pd::data::particle_flags::FlagT", defValue = "", syncMode="COPY")
-      self.addProperty("owner",             "int",                 defValue = "-1", syncMode="COPY")
-      self.addProperty("ghostOwners",       "std::vector<int>",    defValue = "", syncMode="MIGRATION")
+      self.addProperty("owner",             "int",                  defValue = "-1", syncMode="COPY")
+      self.addProperty("ghostOwners",       "std::unordered_set<walberla::mpi::MPIRank>", defValue = "", syncMode="MIGRATION")
 
    def generate(self, path):
       self.unrollDimension()
@@ -40,7 +40,9 @@ class ParticleStorage(Container):
       generateFile(path, 'mpi/notifications/HeatFluxNotification.templ.h', context)
       generateFile(path, 'mpi/notifications/ParseMessage.templ.h', context)
       generateFile(path, 'mpi/notifications/ParticleCopyNotification.templ.h', context)
+      generateFile(path, 'mpi/notifications/NewGhostParticleNotification.templ.h', context)
       generateFile(path, 'mpi/notifications/ParticleMigrationNotification.templ.h', context)
       generateFile(path, 'mpi/notifications/ParticleRemoteMigrationNotification.templ.h', context)
+      generateFile(path, 'mpi/notifications/ParticleRemovalInformationNotification.templ.h', context)
       generateFile(path, 'mpi/notifications/ParticleRemovalNotification.templ.h', context)
       generateFile(path, 'mpi/notifications/ParticleUpdateNotification.templ.h', context)
diff --git a/python/mesa_pd/data/__init__.py b/python/mesa_pd/data/__init__.py
index 5ded7c99eb5da2cec7bb5e327e8f7a15a06a576e..a8b06695767be4d34ea93c3c1d9797b8a8798d24 100644
--- a/python/mesa_pd/data/__init__.py
+++ b/python/mesa_pd/data/__init__.py
@@ -1,11 +1,13 @@
 # -*- coding: utf-8 -*-
 
 from .ContactHistory import ContactHistory
+from .ContactStorage import ContactStorage
 from .LinkedCells import LinkedCells
 from .ParticleStorage import ParticleStorage
 from .ShapeStorage import ShapeStorage
 
 __all__ = ['ContactHistory',
+           'ContactStorage',
            'GeometryStorage',
            'LinkedCells',
            'ParticleStorage']
diff --git a/python/mesa_pd/kernel/DetectAndStoreContacts.py b/python/mesa_pd/kernel/DetectAndStoreContacts.py
new file mode 100644
index 0000000000000000000000000000000000000000..8f6c3d9d89f20f91bd5a5651b15eed1639341cf2
--- /dev/null
+++ b/python/mesa_pd/kernel/DetectAndStoreContacts.py
@@ -0,0 +1,25 @@
+# -*- coding: utf-8 -*-
+
+from mesa_pd.accessor import Accessor
+from mesa_pd.utility import generateFile
+
+class DetectAndStoreContacts:
+
+   def __init__(self):
+      self.accessor = Accessor()
+      self.accessor.require("uid",             "walberla::id_t",                                    access="g")
+      self.accessor.require("flags",           "walberla::mesa_pd::data::particle_flags::FlagT",    access="g")
+      self.accessor.require("position",        "walberla::mesa_pd::Vec3",                           access="g")
+      self.accessor.require("rotation",        "walberla::mesa_pd::Rot3",                           access="g")
+      self.accessor.require("shape",           "BaseShape*",                                        access="g")
+
+
+
+   def getRequirements(self):
+      return self.accessor
+
+
+   def generate(self, path):
+      context = dict()
+      context["interface"]        = self.accessor.properties
+      generateFile(path, 'kernel/DetectAndStoreContacts.templ.h', context)
diff --git a/python/mesa_pd/kernel/__init__.py b/python/mesa_pd/kernel/__init__.py
index 5c96ec4d7ff7fc4b38e3c7adc6aef738c13d9ca4..9ce98cad54556762bed4c4abd4c6798fa10b7694 100644
--- a/python/mesa_pd/kernel/__init__.py
+++ b/python/mesa_pd/kernel/__init__.py
@@ -1,5 +1,5 @@
 # -*- coding: utf-8 -*-
-
+from .DetectAndStoreContacts import DetectAndStoreContacts
 from .DoubleCast import DoubleCast
 from .ExplicitEuler import ExplicitEuler
 from .ExplicitEulerWithShape import ExplicitEulerWithShape
@@ -15,6 +15,7 @@ from .VelocityVerlet import VelocityVerlet
 from .VelocityVerletWithShape import VelocityVerletWithShape
 
 __all__ = ['DoubleCast',
+           'DetectAndStoreContacts',
            'ExplicitEuler',
            'ExplicitEulerWithShape',
            'ForceLJ',
diff --git a/python/mesa_pd/mpi/SyncGhostOwners.py b/python/mesa_pd/mpi/SyncGhostOwners.py
new file mode 100644
index 0000000000000000000000000000000000000000..872f5146d3b1d5e36f6f9f74b332c34bc3f63251
--- /dev/null
+++ b/python/mesa_pd/mpi/SyncGhostOwners.py
@@ -0,0 +1,16 @@
+# -*- coding: utf-8 -*-
+
+from ..utility import generateFile
+
+class SyncGhostOwners:
+   def __init__(self, p):
+      p.addProperty("position",          "walberla::mesa_pd::Vec3", defValue="real_t(0)", syncMode="ALWAYS")
+      p.addProperty("interactionRadius", "walberla::real_t",        defValue="real_t(0)", syncMode="ONCE")
+      p.addProperty("flags",             "walberla::mesa_pd::data::particle_flags::FlagT", defValue="", syncMode="ONCE")
+      p.addProperty("owner",             "int",                     defValue="-1",        syncMode="ONCE")
+      p.addProperty("ghostOwners",       "std::unordered_set<walberla::mpi::MPIRank>",    defValue="",          syncMode="NEVER")
+      p.addProperty("neighborState",     "std::unordered_set<walberla::mpi::MPIRank>",       defValue="",          syncMode="NEVER")
+
+   def generate(self, path):
+      generateFile(path, 'mpi/SyncGhostOwners.templ.h')
+      generateFile(path, 'mpi/SyncGhostOwners.templ.cpp')
diff --git a/python/mesa_pd/mpi/SyncNextNeighbors.py b/python/mesa_pd/mpi/SyncNextNeighbors.py
index 6fdc34f48955f86a4c7cad1c51b3330cfd791378..44d1851eb6218671e8740925d962d9629a044728 100644
--- a/python/mesa_pd/mpi/SyncNextNeighbors.py
+++ b/python/mesa_pd/mpi/SyncNextNeighbors.py
@@ -5,10 +5,10 @@ from ..utility import generateFile
 class SyncNextNeighbors:
    def __init__(self, p):
       p.addProperty("position",          "walberla::mesa_pd::Vec3", defValue="real_t(0)", syncMode="ALWAYS")
-      p.addProperty("interactionRadius", "walberla::real_t",    defValue="real_t(0)", syncMode="ONCE")
+      p.addProperty("interactionRadius", "walberla::real_t",        defValue="real_t(0)", syncMode="ONCE")
       p.addProperty("flags",             "walberla::mesa_pd::data::particle_flags::FlagT", defValue="", syncMode="ONCE")
-      p.addProperty("owner",             "int",                 defValue="-1",        syncMode="ONCE")
-      p.addProperty("ghostOwners",       "std::vector<int>",    defValue="",          syncMode="NEVER")
+      p.addProperty("owner",             "int",                     defValue="-1",        syncMode="ONCE")
+      p.addProperty("ghostOwners",       "std::unordered_set<walberla::mpi::MPIRank>",    defValue="",          syncMode="NEVER")
 
    def generate(self, path):
       generateFile(path, 'mpi/SyncNextNeighbors.templ.h')
diff --git a/python/mesa_pd/mpi/__init__.py b/python/mesa_pd/mpi/__init__.py
index 56849e336aa95ba05b79e8c2470f4142bb51f062..7714d1098c0e44f3d40bbe34e3fd3366347abb1f 100644
--- a/python/mesa_pd/mpi/__init__.py
+++ b/python/mesa_pd/mpi/__init__.py
@@ -4,11 +4,13 @@ from .BroadcastProperty import BroadcastProperty
 from .ClearNextNeighborSync import ClearNextNeighborSync
 from .ReduceContactHistory import ReduceContactHistory
 from .ReduceProperty import ReduceProperty
+from .SyncGhostOwners import SyncGhostOwners
 from .SyncNextNeighbors import SyncNextNeighbors
 
 __all__ = ['BroadcastProperty',
            'ClearNextNeighborSync',
            'ReduceContactHistory',
            'ReduceProperty',
+           'SyncGhostOwners',
            'SyncNextNeighbors',
            ]
diff --git a/python/mesa_pd/templates/data/ContactAccessor.templ.h b/python/mesa_pd/templates/data/ContactAccessor.templ.h
new file mode 100644
index 0000000000000000000000000000000000000000..50190bf636f3e81461634778c161d5abaafc6554
--- /dev/null
+++ b/python/mesa_pd/templates/data/ContactAccessor.templ.h
@@ -0,0 +1,126 @@
+//======================================================================================================================
+//
+//  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 ContactAccessor.h
+//! \author Sebastian Eibl <sebastian.eibl@fau.de>
+//
+//======================================================================================================================
+
+//======================================================================================================================
+//
+//  THIS FILE IS GENERATED - PLEASE CHANGE THE TEMPLATE !!!
+//
+//======================================================================================================================
+
+#pragma once
+
+#include <mesa_pd/data/IAccessor.h>
+#include <mesa_pd/data/ContactStorage.h>
+
+#include <core/UniqueID.h>
+
+#include <limits>
+
+namespace walberla {
+namespace mesa_pd {
+namespace data {
+
+/**
+ * @brief Basic ContactAccessor for the ContactStorage
+ *
+ * Provides get, set and getRef for all members of the ContactStorage.
+ * Can be used as a basis class for a more advanced ContactAccessor.
+ */
+class ContactAccessor : public IAccessor
+{
+public:
+   ContactAccessor(const std::shared_ptr<data::ContactStorage>& ps) : ps_(ps) {}
+   virtual ~ContactAccessor() = default;
+
+   {%- for prop in properties %}
+   const {{prop.type}}& get{{prop.name | capFirst}}(const size_t p_idx) const {return ps_->get{{prop.name | capFirst}}(p_idx);}
+   {{prop.type}}& get{{prop.name | capFirst}}Ref(const size_t p_idx) {return ps_->get{{prop.name | capFirst}}Ref(p_idx);}
+   void set{{prop.name | capFirst}}(const size_t p_idx, const {{prop.type}}& v) { ps_->set{{prop.name | capFirst}}(p_idx, v);}
+   {% endfor %}
+
+   id_t getInvalidUid() const {return UniqueID<data::Contact>::invalidID();}
+   size_t getInvalidIdx() const {return std::numeric_limits<size_t>::max();}
+   /**
+   * @brief Returns the index of Contact specified by uid.
+   * @param uid unique id of the Contact to be looked up
+   * @return the index of the Contact or std::numeric_limits<size_t>::max() if the Contact is not found
+   */
+   size_t uidToIdx(const id_t& uid) const {auto it = ps_->find(uid); return it != ps_->end() ? it.getIdx() : std::numeric_limits<size_t>::max();}
+   size_t size() const { return ps_->size(); }
+
+   inline size_t create(const id_t& uid);
+   inline size_t erase(const size_t& idx);
+   inline size_t find(const id_t& uid);
+protected:
+   std::shared_ptr<data::ContactStorage> ps_;
+};
+
+inline size_t ContactAccessor::create(const id_t& uid)
+{
+   auto it = ps_->create(uid);
+   return it.getIdx();
+}
+inline size_t ContactAccessor::erase(const size_t& idx)
+{
+   data::ContactStorage::iterator it(ps_.get(), idx);
+   it = ps_->erase(it);
+   return it.getIdx();
+}
+inline size_t ContactAccessor::find(const id_t& uid)
+{
+   auto it = ps_->find(uid);
+   return it.getIdx();
+}
+
+/**
+ * @brief Basic ContactAccessor which emulates a single Contact in a ContactStorage
+ * without actually needing a ContactStorage. This class is used mainly for testing purposes.
+ *
+ * Provides get, set and getRef.
+ */
+class SingleContactAccessor : public IAccessor
+{
+public:
+   virtual ~SingleContactAccessor() = default;
+
+   {%- for prop in properties %}
+   const {{prop.type}}& get{{prop.name | capFirst}}(const size_t /*p_idx*/) const {return {{prop.name}}_;}
+   void set{{prop.name | capFirst}}(const size_t /*p_idx*/, const {{prop.type}}& v) { {{prop.name}}_ = v;}
+   {{prop.type}}& get{{prop.name | capFirst}}Ref(const size_t /*p_idx*/) {return {{prop.name}}_;}
+   {% endfor %}
+
+   id_t getInvalidUid() const {return UniqueID<data::Contact>::invalidID();}
+   size_t getInvalidIdx() const {return std::numeric_limits<size_t>::max();}
+   /**
+   * @brief Returns the index of Contact specified by uid.
+   * @param uid unique id of the Contact to be looked up
+   * @return the index of the Contact or std::numeric_limits<size_t>::max() if the Contact is not found
+   */
+   size_t uidToIdx(const id_t& uid) const {return uid == uid_ ? 0 : std::numeric_limits<size_t>::max();}
+   size_t size() const { return 1; }
+private:
+   {%- for prop in properties %}
+   {{prop.type}} {{prop.name}}_;
+   {%- endfor %}
+};
+
+} //namespace data
+} //namespace mesa_pd
+} //namespace walberla
diff --git a/python/mesa_pd/templates/data/ContactStorage.templ.h b/python/mesa_pd/templates/data/ContactStorage.templ.h
new file mode 100644
index 0000000000000000000000000000000000000000..f0af2d1fa8c28264c0b3ee719631d4a6a2cb67eb
--- /dev/null
+++ b/python/mesa_pd/templates/data/ContactStorage.templ.h
@@ -0,0 +1,397 @@
+//======================================================================================================================
+//
+//  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 ContactStorage.h
+//! \author Sebastian Eibl <sebastian.eibl@fau.de>
+//! \author Tobias Leemann <tobias.leemann@fau.de>
+//
+//======================================================================================================================
+
+/**
+ * Storage for detected contacts which can be used to perform actions
+ * for all contacts, e.g. resolving, relaxation schemes...
+ * The InsertIntoContactStorage-Kernel can be used to insert a contact.
+ */
+
+//======================================================================================================================
+//
+//  THIS FILE IS GENERATED - PLEASE CHANGE THE TEMPLATE !!!
+//
+//======================================================================================================================
+
+#pragma once
+
+#include <mesa_pd/data/DataTypes.h>
+{%- for include in includes %}
+#include <{{include}}>
+{%- endfor %}
+#include <mesa_pd/data/STLOverloads.h>
+
+#include <core/Abort.h>
+#include <core/debug/Debug.h>
+#include <core/math/AABB.h>
+#include <core/OpenMP.h>
+#include <core/STLIO.h>
+#include <core/UniqueID.h>
+
+#include <atomic>
+#include <limits>
+#include <map>
+#include <type_traits>
+#include <unordered_map>
+#include <vector>
+
+namespace walberla {
+namespace mesa_pd {
+namespace data {
+
+class ContactStorage;
+
+class ContactStorage
+{
+public:
+   class Contact
+   {
+   public:
+      constexpr Contact(ContactStorage& storage, const size_t i) : storage_(storage), i_(i) {}
+      constexpr Contact(const Contact&)  = default;
+      constexpr Contact(Contact&&)  = default;
+
+      Contact& operator=(const Contact& rhs);
+      Contact& operator=(Contact&& rhs);
+
+      Contact* operator->(){return this;}
+
+      {% for prop in properties %}
+      const {{prop.type}}& get{{prop.name | capFirst}}() const {return storage_.get{{prop.name | capFirst}}(i_);}
+      {{prop.type}}& get{{prop.name | capFirst}}Ref() {return storage_.get{{prop.name | capFirst}}Ref(i_);}
+      void set{{prop.name | capFirst}}(const {{prop.type}}& v) { storage_.set{{prop.name | capFirst}}(i_, v);}
+      {% endfor %}
+
+      size_t getIdx() const {return i_;}
+   public:
+      ContactStorage& storage_;
+      const size_t i_;
+   };
+
+   class iterator
+   {
+   public:
+      using iterator_category = std::random_access_iterator_tag;
+      using value_type        = Contact;
+      using pointer           = Contact*;
+      using reference         = Contact&;
+      using difference_type   = std::ptrdiff_t;
+
+      explicit iterator(ContactStorage* storage, const size_t i) : storage_(storage), i_(i) {}
+      iterator(const iterator& it)         = default;
+      iterator(iterator&& it)              = default;
+      iterator& operator=(const iterator&) = default;
+      iterator& operator=(iterator&&)      = default;
+
+
+      Contact operator*(){return Contact{*storage_, i_};}
+      Contact operator->(){return Contact{*storage_, i_};}
+      iterator& operator++(){ ++i_; return *this; }
+      iterator operator++(int){ iterator tmp(*this); ++(*this); return tmp; }
+      iterator& operator--(){ --i_; return *this; }
+      iterator operator--(int){ iterator tmp(*this); --(*this); return tmp; }
+
+      iterator& operator+=(const size_t n){ i_+=n; return *this; }
+      iterator& operator-=(const size_t n){ i_-=n; return *this; }
+
+      friend iterator operator+(const iterator& it, const size_t n);
+      friend iterator operator+(const size_t n, const iterator& it);
+      friend iterator operator-(const iterator& it, const size_t n);
+      friend difference_type operator-(const iterator& lhs, const iterator& rhs);
+
+      friend bool operator==(const iterator& lhs, const iterator& rhs);
+      friend bool operator!=(const iterator& lhs, const iterator& rhs);
+      friend bool operator<(const iterator& lhs, const iterator& rhs);
+      friend bool operator>(const iterator& lhs, const iterator& rhs);
+      friend bool operator<=(const iterator& lhs, const iterator& rhs);
+      friend bool operator>=(const iterator& lhs, const iterator& rhs);
+
+      friend void swap(iterator& lhs, iterator& rhs);
+
+      size_t getIdx() const {return i_;}
+   private:
+      ContactStorage* storage_;
+      size_t i_;
+   };
+
+   explicit ContactStorage(const size_t size);
+
+   iterator begin() { return iterator(this, 0); }
+   iterator end()   { return iterator(this, size()); }
+   iterator operator[](const size_t n) { return iterator(this, n); }
+
+   {% for prop in properties %}
+   const {{prop.type}}& get{{prop.name | capFirst}}(const size_t idx) const {return {{prop.name}}_[idx];}
+   {{prop.type}}& get{{prop.name | capFirst}}Ref(const size_t idx) {return {{prop.name}}_[idx];}
+   void set{{prop.name | capFirst}}(const size_t idx, const {{prop.type}}& v) { {{prop.name}}_[idx] = v; }
+   {% endfor %}
+
+   /**
+    * @brief creates a new Contact and returns an iterator pointing to it
+    *
+    * \attention Use this function only if you know what you are doing!
+    * Messing with the uid might break the simulation!
+    * If you are unsure use create(bool) instead.
+    * @param uid unique id of the Contact to be created
+    * @return iterator to the newly created Contact
+    */
+   inline iterator create(const id_t& uid);
+   inline iterator create();
+   inline iterator erase(iterator& it);
+   /// Finds the entry corresponding to \p uid.
+   /// \return iterator to the object or end iterator
+   inline iterator find(const id_t& uid);
+   inline void reserve(const size_t size);
+   inline void clear();
+   inline size_t size() const;
+
+   /**
+    * Calls the provided functor \p func for all Contacts selected by the selector.
+    *
+    * Additional arguments can be provided.
+    * Call syntax for the provided functor
+    * \code
+    * func( *this, i, std::forward<Args>(args)... );
+    * \endcode
+    * \param openmp enables/disables OpenMP parallelization of the kernel call
+    */
+   template <typename Selector, typename Accessor, typename Func, typename... Args>
+   inline void forEachContact(const bool openmp, const Selector& selector,
+                              Accessor& acForPS, Func&& func, Args&&... args);
+   template <typename Selector, typename Accessor, typename Func, typename... Args>
+   inline void forEachContact(const bool openmp, const Selector& selector,
+                              Accessor& acForPS, Func&& func, Args&&... args) const;
+
+   private:
+   {%- for prop in properties %}
+   std::vector<{{prop.type}}> {{prop.name}}_ {};
+   {%- endfor %}
+   std::unordered_map<id_t, size_t> uidToIdx_;
+   static_assert(std::is_same<decltype(uid_)::value_type, id_t>::value,
+                 "Property uid of type id_t is missing. This property is required!");
+};
+using Contact = ContactStorage::Contact;
+
+inline
+ContactStorage::Contact& ContactStorage::Contact::operator=(const ContactStorage::Contact& rhs)
+{
+   {%- for prop in properties %}
+   get{{prop.name | capFirst}}Ref() = rhs.get{{prop.name | capFirst}}();
+   {%- endfor %}
+   return *this;
+}
+
+inline
+ContactStorage::Contact& ContactStorage::Contact::operator=(ContactStorage::Contact&& rhs)
+{
+   {%- for prop in properties %}
+   get{{prop.name | capFirst}}Ref() = std::move(rhs.get{{prop.name | capFirst}}Ref());
+   {%- endfor %}
+   return *this;
+}
+
+inline
+std::ostream& operator<<( std::ostream& os, const ContactStorage::Contact& p )
+{
+   os << "==========  {{StorageType | upper}}  ==========" << "\n" <<
+         "idx                 : " << p.getIdx() << "\n" <<
+   {%- for prop in properties %}
+         "{{'%-20s'|format(prop.name)}}: " << p.get{{prop.name | capFirst}}() << "\n" <<
+   {%- endfor %}
+         "================================" << std::endl;
+   return os;
+}
+
+inline
+ContactStorage::iterator operator+(const ContactStorage::iterator& it, const size_t n)
+{
+   return ContactStorage::iterator(it.storage_, it.i_+n);
+}
+
+inline
+ContactStorage::iterator operator+(const size_t n, const ContactStorage::iterator& it)
+{
+   return it + n;
+}
+
+inline
+ContactStorage::iterator operator-(const ContactStorage::iterator& it, const size_t n)
+{
+   return ContactStorage::iterator(it.storage_, it.i_-n);
+}
+
+inline
+ContactStorage::iterator::difference_type operator-(const ContactStorage::iterator& lhs, const ContactStorage::iterator& rhs)
+{
+   return int64_c(lhs.i_) - int64_c(rhs.i_);
+}
+
+inline bool operator==(const ContactStorage::iterator& lhs, const ContactStorage::iterator& rhs)
+{
+   WALBERLA_ASSERT_EQUAL(lhs.storage_, rhs.storage_);
+   return (lhs.i_ == rhs.i_);
+}
+inline bool operator!=(const ContactStorage::iterator& lhs, const ContactStorage::iterator& rhs)
+{
+   WALBERLA_ASSERT_EQUAL(lhs.storage_, rhs.storage_);
+   return (lhs.i_ != rhs.i_);
+}
+inline bool operator<(const ContactStorage::iterator& lhs, const ContactStorage::iterator& rhs)
+{
+   WALBERLA_ASSERT_EQUAL(lhs.storage_, rhs.storage_);
+   return (lhs.i_ < rhs.i_);
+}
+inline bool operator>(const ContactStorage::iterator& lhs, const ContactStorage::iterator& rhs)
+{
+   WALBERLA_ASSERT_EQUAL(lhs.storage_, rhs.storage_);
+   return (lhs.i_ > rhs.i_);
+}
+inline bool operator<=(const ContactStorage::iterator& lhs, const ContactStorage::iterator& rhs)
+{
+   WALBERLA_ASSERT_EQUAL(lhs.storage_, rhs.storage_);
+   return (lhs.i_ <= rhs.i_);
+}
+inline bool operator>=(const ContactStorage::iterator& lhs, const ContactStorage::iterator& rhs)
+{
+   WALBERLA_ASSERT_EQUAL(lhs.storage_, rhs.storage_);
+   return (lhs.i_ >= rhs.i_);
+}
+
+inline void swap(ContactStorage::iterator& lhs, ContactStorage::iterator& rhs)
+{
+   WALBERLA_ASSERT_EQUAL(lhs.storage_, rhs.storage_);
+   std::swap(lhs.i_, rhs.i_);
+}
+
+inline
+ContactStorage::ContactStorage(const size_t size)
+{
+   reserve(size);
+}
+
+
+inline ContactStorage::iterator ContactStorage::create(const id_t& uid)
+{
+   WALBERLA_ASSERT_EQUAL(uidToIdx_.find(uid),
+                         uidToIdx_.end(),
+                         "Contact with the same uid(" << uid <<") already existing at index(" << uidToIdx_.find(uid)->second << ")");
+   {%- for prop in properties %}
+   {{prop.name}}_.emplace_back({{prop.defValue}});
+   {%- endfor %}
+   uid_.back() = uid;
+   uidToIdx_[uid] = uid_.size() - 1;
+   return iterator(this, size() - 1);
+}
+
+inline ContactStorage::iterator ContactStorage::create()
+{
+   return create(UniqueID<Contact>::create());
+}
+
+inline ContactStorage::iterator ContactStorage::erase(iterator& it)
+{
+   //swap with last element and pop
+   auto last = --end();
+   auto numElementsRemoved = uidToIdx_.erase(it->getUid());
+   WALBERLA_CHECK_EQUAL(numElementsRemoved,
+                        1,
+                        "Contact with uid " << it->getUid() << " cannot be removed (not existing).");
+   if (it != last) //skip swap if last element is removed
+   {
+      *it = *last;
+      uidToIdx_[it->getUid()] = it.getIdx();
+   }
+   {%- for prop in properties %}
+   {{prop.name}}_.pop_back();
+   {%- endfor %}
+   return it;
+}
+
+inline ContactStorage::iterator ContactStorage::find(const id_t& uid)
+{
+   //linear search through uid vector
+   //auto it = std::find(uid_.begin(), uid_.end(), uid);
+   //if (it == uid_.end()) return end();
+   //return iterator(this, uint_c(std::distance(uid_.begin(), it)));
+
+   //use unordered_map for faster lookup
+   auto it = uidToIdx_.find(uid);
+   if (it == uidToIdx_.end()) return end();
+   WALBERLA_ASSERT_EQUAL(it->first, uid, "Lookup via uidToIdx map is not up to date!!!");
+   return iterator(this, it->second);
+}
+
+inline void ContactStorage::reserve(const size_t size)
+{
+   {%- for prop in properties %}
+   {{prop.name}}_.reserve(size);
+   {%- endfor %}
+}
+
+inline void ContactStorage::clear()
+{
+   {%- for prop in properties %}
+   {{prop.name}}_.clear();
+   {%- endfor %}
+   uidToIdx_.clear();
+}
+
+inline size_t ContactStorage::size() const
+{
+   {%- for prop in properties %}
+   //WALBERLA_ASSERT_EQUAL( {{properties[0].name}}_.size(), {{prop.name}}.size() );
+   {%- endfor %}
+   return {{properties[0].name}}_.size();
+}
+
+{%- for const in ["", "const"] %}
+template <typename Selector, typename Accessor, typename Func, typename... Args>
+inline void ContactStorage::forEachContact(const bool openmp, const Selector& selector,
+                                           Accessor& acForPS, Func&& func, Args&&... args) {{const}}
+{
+   WALBERLA_UNUSED(openmp);
+   const uint64_t len = size();
+   #ifdef _OPENMP
+   #pragma omp parallel for schedule(static) if (openmp)
+   #endif
+   for (int64_t i = 0; i < int64_c(len); ++i)
+      if (selector(uint64_c(i), acForPS)){
+         func( uint64_c(i), std::forward<Args>(args)... );
+      }
+}
+{%- endfor %}
+
+
+{%- for prop in properties %}
+///Predicate that selects a certain property from a Contact
+class SelectContact{{prop.name | capFirst}}
+{
+public:
+   using return_type = {{prop.type}};
+   {{prop.type}}& operator()(data::Contact& p) const {return p.get{{prop.name | capFirst}}Ref();}
+   {{prop.type}}& operator()(data::Contact&& p) const {return p.get{{prop.name | capFirst}}Ref();}
+   const {{prop.type}}& operator()(const data::Contact& p) const {return p.get{{prop.name | capFirst}}();}
+};
+{%- endfor %}
+
+} //namespace data
+} //namespace mesa_pd
+} //namespace walberla
diff --git a/python/mesa_pd/templates/data/ParticleStorage.templ.h b/python/mesa_pd/templates/data/ParticleStorage.templ.h
index c6a0f2011832bd1161af3c305892a7c7ba67cc76..7d6b1fa4b21bb6cd70a01e88518b0b5b1a8a23be 100644
--- a/python/mesa_pd/templates/data/ParticleStorage.templ.h
+++ b/python/mesa_pd/templates/data/ParticleStorage.templ.h
@@ -31,6 +31,7 @@
 #include <map>
 #include <type_traits>
 #include <unordered_map>
+#include <unordered_set>
 #include <vector>
 
 #include <mesa_pd/data/ContactHistory.h>
@@ -44,6 +45,7 @@
 #include <core/Abort.h>
 #include <core/debug/Debug.h>
 #include <core/math/AABB.h>
+#include <core/mpi/MPIWrapper.h>
 #include <core/OpenMP.h>
 #include <core/STLIO.h>
 #include <core/UniqueID.h>
diff --git a/python/mesa_pd/templates/kernel/DetectAndStoreContacts.templ.h b/python/mesa_pd/templates/kernel/DetectAndStoreContacts.templ.h
new file mode 100644
index 0000000000000000000000000000000000000000..85369e566f6a6df5f3bdd4dff6ddf4abb201b8a6
--- /dev/null
+++ b/python/mesa_pd/templates/kernel/DetectAndStoreContacts.templ.h
@@ -0,0 +1,94 @@
+//======================================================================================================================
+//
+//  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 DetectAndStoreContacts.h
+//! \author Sebastian Eibl <sebastian.eibl@fau.de>
+//! \author Tobias Leemann <tobias.leemann@fau.de>
+//
+//======================================================================================================================
+
+//======================================================================================================================
+//
+//  THIS FILE IS GENERATED - PLEASE CHANGE THE TEMPLATE !!!
+//
+//======================================================================================================================
+
+#pragma once
+
+#include <mesa_pd/collision_detection/AnalyticContactDetection.h>
+#include <mesa_pd/mpi/ContactFilter.h>
+
+#include <mesa_pd/data/DataTypes.h>
+#include <mesa_pd/data/ContactStorage.h>
+
+#include <mesa_pd/kernel/DoubleCast.h>
+#include <mesa_pd/data/Flags.h>
+
+namespace walberla {
+namespace mesa_pd {
+namespace kernel {
+
+/**
+ * Kernel which performes collision detection on a pair of two particles
+ * and inserts the contact (if existent) into the contact storage passed in the constructor.
+ * Call this kernel on each particle pair to perform contact detection and insert each contact in the contact
+ * storage.
+ * \ingroup mesa_pd_kernel
+ */
+class DetectAndStoreContacts
+{
+
+   public:
+   explicit DetectAndStoreContacts(data::ContactStorage &cs) : cs_(cs) {}
+
+   /**
+    * Compute if particle 1 and particle 2 collide. If so, insert the contact into the contact storage.
+    * \param idx1 The index of particle 1
+    * \param idx2 The index of particle 2
+    * \param ac The accessor used to access the values of the particles.
+    * \param domain The domain of the block (used to decide if the contact has to be treated by this process)
+    * \param acd The collision detection to be used. Default parameter: AnalyticContactDetection()
+    */
+   template <typename Accessor>
+   void operator()(size_t idx1, size_t idx2, Accessor &ac, const domain::IDomain& domain, collision_detection::AnalyticContactDetection acd = collision_detection::AnalyticContactDetection());
+   private:
+   data::ContactStorage& cs_;
+};
+
+
+
+template <typename Accessor>
+inline void DetectAndStoreContacts::operator()(size_t idx1, size_t idx2, Accessor &ac, const domain::IDomain& domain, collision_detection::AnalyticContactDetection acd)
+{
+   using namespace data::particle_flags;
+   kernel::DoubleCast double_cast;
+   mpi::ContactFilter contact_filter;
+   if (double_cast(idx1, idx2, ac, acd, ac ))
+   {
+      if (contact_filter(acd.getIdx1(), acd.getIdx2(), ac, acd.getContactPoint(), domain))
+      {
+         auto c = cs_.create();
+         c->setId1(acd.getIdx1());
+         c->setId2(acd.getIdx2());
+         c->setDistance(acd.getPenetrationDepth());
+         c->setNormal(acd.getContactNormal());
+         c->setPosition(acd.getContactPoint());
+      }
+   }
+}
+
+} //namespace kernel
+} //namespace mesa_pd
+} //namespace walberla
diff --git a/python/mesa_pd/templates/mpi/SyncGhostOwners.templ.cpp b/python/mesa_pd/templates/mpi/SyncGhostOwners.templ.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..da627c08addf2e50535274d581257ff67b11e7ea
--- /dev/null
+++ b/python/mesa_pd/templates/mpi/SyncGhostOwners.templ.cpp
@@ -0,0 +1,355 @@
+//======================================================================================================================
+//
+//  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 SyncGhostOwners.cpp
+//! \author Sebastian Eibl <sebastian.eibl@fau.de>
+//
+//======================================================================================================================
+
+//======================================================================================================================
+//
+//  THIS FILE IS GENERATED - PLEASE CHANGE THE TEMPLATE !!!
+//
+//======================================================================================================================
+
+#include "SyncGhostOwners.h"
+
+#include <mesa_pd/mpi/RemoveAndNotify.h>
+
+namespace walberla {
+namespace mesa_pd {
+namespace mpi {
+
+void SyncGhostOwners::operator()( data::ParticleStorage& ps,
+                                  const domain::IDomain& domain,
+                                  const real_t dx,
+                                  const bool syncNonCommunicatingBodies ) const
+{
+   if (numProcesses_ == 1) return;
+
+   //==========================================================
+   // STEP1: Update & Migrate
+   //==========================================================
+   updateAndMigrate( ps, domain, syncNonCommunicatingBodies );
+
+   //==========================================================
+   // STEP2: Check & Resolve
+   //==========================================================
+   checkAndResolveOverlap( ps, domain, dx, syncNonCommunicatingBodies );
+}
+
+void SyncGhostOwners::updateAndMigrate( data::ParticleStorage& ps,
+                                        const domain::IDomain& domain,
+                                        const bool syncNonCommunicatingBodies ) const
+{
+   using namespace walberla::mesa_pd::data::particle_flags;
+   //==========================================================
+   // STEP1: Update & Migrate
+   //==========================================================
+
+   WALBERLA_CHECK(!bs1.isCommunicationRunning());
+
+   WALBERLA_LOG_DETAIL( "Assembling of Update&Migrate starts..." );
+   std::set<walberla::mpi::MPIRank> recvRanks; // potential message senders
+   for( auto pIt = ps.begin(); pIt != ps.end(); ++pIt)
+   {
+      if (isSet( pIt->getFlags(), GHOST))
+      {
+         if (!isSet( pIt->getFlags(), NON_COMMUNICATING) || syncNonCommunicatingBodies)
+         {
+            recvRanks.insert(pIt->getOwner());
+         }
+      }
+   }
+
+   for( auto pIt = ps.begin(); pIt != ps.end(); )
+   {
+      if (isSet( pIt->getFlags(), GHOST))
+      {
+         ++pIt;
+         continue;
+      }
+
+      //==================
+      // LOCAL
+
+      //skip all particles that do not communicate (create ghost particles) on other processes
+      if (isSet( pIt->getFlags(), NON_COMMUNICATING) && !syncNonCommunicatingBodies)
+      {
+         ++pIt;
+         continue;
+      }
+
+      //correct position to make sure particle is always inside the domain!
+      //everything is decided by the master particle therefore ghost particles are not touched
+      if (!data::particle_flags::isSet( pIt->getFlags(), data::particle_flags::FIXED) &&
+          !data::particle_flags::isSet( pIt->getFlags(), data::particle_flags::GHOST))
+      {
+         domain.periodicallyMapToDomain( pIt->getPositionRef() );
+      }
+
+      // Update
+      for (auto ghostOwner : pIt->getGhostOwners())
+      {
+         WALBERLA_LOG_DETAIL( "Sending update notification for body " << pIt->getUid() << " to process " << ghostOwner );
+         walberla::mpi::SendBuffer& sb = bs1.sendBuffer(static_cast<walberla::mpi::MPIRank>(ghostOwner));
+         if (sb.isEmpty()) sb << walberla::uint8_c(0);
+         packNotification(sb, ParticleUpdateNotification( *pIt ));
+      }
+
+      //particle has left subdomain?
+      const auto newOwner = domain.findContainingProcessRank( pIt->getPosition() );
+      if( newOwner != int_c(rank_) )
+      {
+         if ( newOwner < 0)
+         {
+            // No owner found: Outflow condition.
+            WALBERLA_LOG_DETAIL( "Sending deletion notifications for body " << pIt->getUid() << " due to outflow." );
+
+            //delete body
+            pIt = removeAndNotify( bs1, ps, pIt );
+
+            continue;
+         }
+
+         // Set new owner and transform to shadow copy
+         pIt->setOwner( newOwner );
+         set( pIt->getFlagsRef(), GHOST );
+
+         // currently position is mapped to periodically to global domain,
+         // this might not be the correct position for a ghost particle
+         domain.correctParticlePosition( pIt->getPositionRef() );
+
+         // Correct registration list (exclude new owner and us - the old owner) and
+         // notify registered processes (except for new owner) of (remote) migration since they possess a ghost particle.
+         auto ownerIt = std::find( pIt->getGhostOwners().begin(), pIt->getGhostOwners().end(), newOwner );
+         WALBERLA_CHECK_UNEQUAL(ownerIt, pIt->getGhostOwners().end(), "New owner has to be former ghost owner!" );
+
+         pIt->getGhostOwnersRef().erase( ownerIt );
+
+         // Send remote migration notifications
+         for( auto ghostRank : pIt->getGhostOwners() )
+         {
+            auto& buffer( bs1.sendBuffer(static_cast<walberla::mpi::MPIRank>(ghostRank)) );
+            if (buffer.isEmpty()) buffer << walberla::uint8_c(0);
+
+            WALBERLA_LOG_DETAIL( "Sending remote migration notification for particle " <<
+                                 pIt->getUid() <<
+                                 " to process " <<
+                                 ghostRank );
+
+            packNotification(buffer, ParticleRemoteMigrationNotification( *pIt, newOwner ));
+         }
+
+         pIt->getGhostOwnersRef().insert( int_c(rank_) );
+
+         WALBERLA_LOG_DETAIL( "Sending migration notification for body " <<
+                              pIt->getUid() <<
+                              " to process " <<
+                              (newOwner) );
+
+         // Send migration notification to new owner
+         auto& sb( bs1.sendBuffer(newOwner) );
+         if (sb.isEmpty()) sb << walberla::uint8_c(0);
+         packNotification(sb, ParticleMigrationNotification( *pIt ));
+
+         pIt->getGhostOwnersRef().clear();
+
+         continue;
+      }
+      ++pIt;
+   }
+   WALBERLA_LOG_DETAIL( "Assembling of Update&Migrate ended." );
+
+   WALBERLA_LOG_DETAIL( "UM: number of recv " << recvRanks.size());
+   bs1.setReceiverInfo(recvRanks, true);
+   bs1.sendAll();
+   WALBERLA_LOG_DETAIL( "UM: number of sends " << bs1.getNumberOfSends());
+
+   // Receiving the updates for the remote rigid bodies from the connected processes
+   WALBERLA_LOG_DETAIL( "Parsing of Update&Migrate starts..." );
+   ParseMessage parseMessage;
+   for( auto it = bs1.begin(); it != bs1.end(); ++it )
+   {
+      walberla::uint8_t tmp;
+      it.buffer() >> tmp;
+      while( !it.buffer().isEmpty() )
+      {
+         parseMessage(it.rank(), it.buffer(), ps, domain);
+      }
+   }
+   WALBERLA_LOG_DETAIL( "Parsing of Update&Migrate ended." );
+}
+
+void SyncGhostOwners::checkAndResolveOverlap( data::ParticleStorage& ps,
+                                              const domain::IDomain& domain,
+                                              const real_t dx,
+                                              const bool syncNonCommunicatingBodies ) const
+{
+   using namespace walberla::mesa_pd::data::particle_flags;
+   //==========================================================
+   // STEP2: Check&Resolve
+   //==========================================================
+
+   WALBERLA_CHECK(!bs2.isCommunicationRunning());
+
+   //init buffers
+   neighborRanks_ = domain.getNeighborProcesses();
+   for( uint_t nbProcessRank : neighborRanks_ )
+   {
+      if (bs2.sendBuffer(nbProcessRank).isEmpty())
+      {
+         // fill empty buffers with a dummy byte to force transmission
+         bs2.sendBuffer(nbProcessRank) << walberla::uint8_c(0);
+      }
+   }
+   bs2.sendBuffer(int_c(rank_)) << walberla::uint8_c(0);
+
+   WALBERLA_LOG_DETAIL( "Assembling of Check&Resolve starts..." );
+
+   for( auto pIt = ps.begin(); pIt != ps.end(); )
+   {
+      //skip all particles that do not communicate (create ghost particles) on other processes
+      if (isSet( pIt->getFlags(), NON_COMMUNICATING) && !syncNonCommunicatingBodies)
+      {
+          ++pIt;
+          continue;
+      }
+
+      if (!isSet( pIt->getFlags(), GHOST))
+      {
+         //LOCAL
+
+         walberla::mpi::SendBuffer& sbMaster = bs2.sendBuffer(pIt->getOwner());
+         if (sbMaster.isEmpty()) sbMaster << walberla::uint8_c(0);
+
+         // Update (nearest) neighbor processes.
+         for( uint_t nbProcessRank : neighborRanks_ )
+         {
+            auto& sb = bs2.sendBuffer(nbProcessRank);
+            if (sb.isEmpty()) sb << walberla::uint8_c(0);
+
+            // dont send to owner!!
+            if (pIt->getOwner() == int_c(nbProcessRank)) continue;
+            // only send to neighbor which do not know this body
+            if (pIt->getNeighborState().find( int_c(nbProcessRank) ) != pIt->getNeighborState().end()) continue;
+
+            if( domain.intersectsWithProcessSubdomain( nbProcessRank, pIt->getPosition(), pIt->getInteractionRadius() + dx ) )
+            {
+               // no ghost there -> create ghost
+               WALBERLA_LOG_DETAIL( "Sending copy notification for body " << pIt->getUid() << " to process " << (nbProcessRank) << "\n master: " << pIt->getOwner());
+               packNotification(sb, ParticleCopyNotification( *pIt ));
+               packNotification(sbMaster, NewGhostParticleNotification( *pIt, int_c(nbProcessRank) ));
+               pIt->getNeighborStateRef().insert( int_c(nbProcessRank) );
+            }
+         }
+      } else
+      {
+         //GHOST
+
+         walberla::mpi::SendBuffer& sbMaster = bs2.sendBuffer(pIt->getOwner());
+         if (sbMaster.isEmpty()) sbMaster << walberla::uint8_c(0);
+
+         // Update (nearest) neighbor processes.
+         for( uint_t nbProcessRank : neighborRanks_ )
+         {
+            auto& sb = bs2.sendBuffer(nbProcessRank);
+            if (sb.isEmpty()) sb << walberla::uint8_c(0);
+
+            if (pIt->getOwner() == int_c(nbProcessRank)) continue; // dont send to owner!!
+            if (pIt->getNeighborState().find( int_c(nbProcessRank) ) != pIt->getNeighborState().end()) continue; // only send to neighbor which do not know this body
+
+            if( domain.intersectsWithProcessSubdomain( nbProcessRank, pIt->getPosition(), pIt->getInteractionRadius() + dx ) )
+            {
+               // no ghost there -> create ghost
+               WALBERLA_LOG_DETAIL( "Sending copy notification for body " << pIt->getUid() << " to process " << (nbProcessRank) << "\n master: " << pIt->getOwner());
+               packNotification(sb, ParticleCopyNotification( *pIt ));
+               packNotification(sbMaster, NewGhostParticleNotification( *pIt, int_c(nbProcessRank) ));
+               pIt->getNeighborStateRef().insert( int_c(nbProcessRank) );
+            }
+         }
+
+         if ( !domain.intersectsWithProcessSubdomain(uint_c(rank_), pIt->getPosition(), pIt->getInteractionRadius() + dx) )
+         {
+            // Delete
+            // inform nearest neighbor processes.
+            for( uint_t nbProcessRank : neighborRanks_ )
+            {
+               WALBERLA_LOG_DETAIL( "Sending removal information notification for body " << pIt->getUid() << " to process " << (nbProcessRank) );
+               auto& sb = bs2.sendBuffer(nbProcessRank);
+               if (sb.isEmpty()) sb << walberla::uint8_c(0);
+               packNotification(sb, ParticleRemovalInformationNotification( *pIt ));
+            }
+
+            //notify owner
+            WALBERLA_LOG_DETAIL( "Sending removal information notification for body " << pIt->getUid() << " to process " << (pIt->getOwner()) );
+            auto& sb = bs2.sendBuffer(pIt->getOwner());
+            if (sb.isEmpty()) sb << walberla::uint8_c(0);
+            packNotification(sb, ParticleRemovalInformationNotification( *pIt ));
+
+            pIt = ps.erase( pIt );
+            continue;
+         }
+      }
+      ++pIt;
+   }
+
+   std::set<walberla::mpi::MPIRank> recvRanks; // potential message senders
+   // schedule receives
+   for( auto pIt = ps.begin(); pIt != ps.end(); ++pIt)
+   {
+      if (isSet( pIt->getFlags(), GHOST)) continue;
+
+      //skip all particles that do not communicate (create ghost particles) on other processes
+      if (isSet( pIt->getFlags(), NON_COMMUNICATING) && !syncNonCommunicatingBodies) continue;
+
+      for( auto ghostRank : pIt->getGhostOwners() )
+      {
+         recvRanks.insert(ghostRank);
+      }
+   }
+
+   for( uint_t nbProcessRank : neighborRanks_ )
+   {
+      recvRanks.insert(int_c(nbProcessRank));
+   }
+
+   recvRanks.insert( int_c(rank_) );
+   WALBERLA_LOG_DETAIL( "Assembling of Check&Resolve ended." );
+
+   // size of buffer is unknown and changes with each send
+   WALBERLA_LOG_DETAIL( "CR: number of recv " << recvRanks.size());
+   bs2.setReceiverInfo(recvRanks, true);
+   bs2.sendAll();
+   WALBERLA_LOG_DETAIL( "CR: number of sends " << bs2.getNumberOfSends());
+
+   // Receiving the updates for the remote rigid bodies from the connected processes
+   WALBERLA_LOG_DETAIL( "Parsing of Check&Resolve starts..." );
+   ParseMessage parseMessage;
+   for( auto it = bs2.begin(); it != bs2.end(); ++it )
+   {
+      walberla::uint8_t tmp;
+      it.buffer() >> tmp;
+      while( !it.buffer().isEmpty() )
+      {
+         parseMessage(it.rank(), it.buffer(), ps, domain);
+      }
+   }
+   WALBERLA_LOG_DETAIL( "Parsing of Check&Resolve ended." );
+}
+
+}  // namespace mpi
+}  // namespace mesa_pd
+}  // namespace walberla
diff --git a/python/mesa_pd/templates/mpi/SyncGhostOwners.templ.h b/python/mesa_pd/templates/mpi/SyncGhostOwners.templ.h
new file mode 100644
index 0000000000000000000000000000000000000000..9ae5d924dbecad544239d744c1e43b2acd723c04
--- /dev/null
+++ b/python/mesa_pd/templates/mpi/SyncGhostOwners.templ.h
@@ -0,0 +1,89 @@
+//======================================================================================================================
+//
+//  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 SyncGhostOwners.h
+//! \author Sebastian Eibl <sebastian.eibl@fau.de>
+//
+//======================================================================================================================
+
+//======================================================================================================================
+//
+//  THIS FILE IS GENERATED - PLEASE CHANGE THE TEMPLATE !!!
+//
+//======================================================================================================================
+
+#pragma once
+
+#include <mesa_pd/data/DataTypes.h>
+#include <mesa_pd/data/Flags.h>
+#include <mesa_pd/data/ParticleStorage.h>
+#include <mesa_pd/domain/IDomain.h>
+#include <mesa_pd/mpi/notifications/NewGhostParticleNotification.h>
+#include <mesa_pd/mpi/notifications/PackNotification.h>
+#include <mesa_pd/mpi/notifications/ParseMessage.h>
+#include <mesa_pd/mpi/notifications/ParticleCopyNotification.h>
+#include <mesa_pd/mpi/notifications/ParticleMigrationNotification.h>
+#include <mesa_pd/mpi/notifications/ParticleRemoteMigrationNotification.h>
+#include <mesa_pd/mpi/notifications/ParticleRemovalInformationNotification.h>
+#include <mesa_pd/mpi/notifications/ParticleRemovalNotification.h>
+#include <mesa_pd/mpi/notifications/ParticleUpdateNotification.h>
+
+#include <core/mpi/BufferSystem.h>
+#include <core/logging/Logging.h>
+
+namespace walberla {
+namespace mesa_pd {
+namespace mpi {
+
+/**
+ * Kernel which updates all ghost particles.
+ *
+ * \ingroup mesa_pd_mpi
+ */
+class SyncGhostOwners
+{
+public:
+   void operator()( data::ParticleStorage& ps,
+                    const domain::IDomain& domain,
+                    const real_t dx = real_t(0),
+                    const bool syncNonCommunicatingBodies = false ) const;
+
+   int64_t getBytesSent() const { return bs1.getBytesSent() + bs2.getBytesSent(); }
+   int64_t getBytesReceived() const { return bs1.getBytesReceived() + bs2.getBytesReceived(); }
+
+   int64_t getNumberOfSends() const { return bs1.getNumberOfSends() + bs2.getNumberOfSends(); }
+   int64_t getNumberOfReceives() const { return bs1.getNumberOfReceives() + bs2.getNumberOfReceives(); }
+private:
+   void updateAndMigrate( data::ParticleStorage& ps,
+                          const domain::IDomain& domain,
+                          const bool syncNonCommunicatingBodies ) const;
+
+   void checkAndResolveOverlap( data::ParticleStorage& ps,
+                                const domain::IDomain& domain,
+                                const real_t dx,
+                                const bool syncNonCommunicatingBodies ) const;
+
+   mutable std::vector<uint_t> neighborRanks_; ///cache for neighbor ranks -> will be updated in operator()
+
+   mutable walberla::mpi::BufferSystem bs1 = walberla::mpi::BufferSystem( walberla::mpi::MPIManager::instance()->comm(), 749861);
+   mutable walberla::mpi::BufferSystem bs2 = walberla::mpi::BufferSystem( walberla::mpi::MPIManager::instance()->comm(), 255367);
+
+   int numProcesses_ = walberla::mpi::MPIManager::instance()->numProcesses();
+   int rank_         = walberla::mpi::MPIManager::instance()->rank();
+};
+
+}  // namespace mpi
+}  // namespace mesa_pd
+}  // namespace walberla
diff --git a/python/mesa_pd/templates/mpi/SyncNextNeighbors.templ.cpp b/python/mesa_pd/templates/mpi/SyncNextNeighbors.templ.cpp
index 0f3350604f5b058561beb5e68f909d9e74592999..98a3fc09a96120fbf9e4e27bde8e2b4fcde1be7e 100644
--- a/python/mesa_pd/templates/mpi/SyncNextNeighbors.templ.cpp
+++ b/python/mesa_pd/templates/mpi/SyncNextNeighbors.templ.cpp
@@ -26,6 +26,8 @@
 
 #include "SyncNextNeighbors.h"
 
+#include <mesa_pd/mpi/RemoveAndNotify.h>
+
 namespace walberla {
 namespace mesa_pd {
 namespace mpi {
@@ -66,38 +68,6 @@ void SyncNextNeighbors::operator()(data::ParticleStorage& ps,
    WALBERLA_LOG_DETAIL( "Parsing of particle synchronization response ended." );
 }
 
-/**
- * Removes a particle from the local storage and informs ghost particle holders.
- *
- * This function removes the particle from the particle storage and generates deletion notifications.
- */
-inline
-data::ParticleStorage::iterator removeAndNotify( walberla::mpi::BufferSystem& bs,
-                                                 data::ParticleStorage& ps,
-                                                 data::ParticleStorage::iterator& pIt )
-{
-   WALBERLA_ASSERT( !data::particle_flags::isSet( pIt->getFlags(), data::particle_flags::GHOST),
-                    "Trying to remove ghost particle from the particle storage." );
-
-   WALBERLA_ASSERT( !data::particle_flags::isSet( pIt->getFlags(), data::particle_flags::GLOBAL),
-                    "Trying to remove a global particle from the particle storage." );
-
-   if( !pIt->getGhostOwners().empty() )
-   {
-      // Notify registered processes (intersecting or interacting) of particle removal since they possess a shadow copy.
-      for( auto ghostRank : pIt->getGhostOwnersRef() )
-      {
-         WALBERLA_LOG_DETAIL( "__Notify registered process " << ghostRank << " of deletion of particle " << pIt->getUid() );
-         auto& sb = bs.sendBuffer(ghostRank);
-         if (sb.isEmpty()) sb << walberla::uint8_c(0);
-         packNotification(sb, ParticleRemovalNotification( *pIt ));
-      }
-   }
-
-   pIt->getGhostOwnersRef().clear();
-   return ps.erase( pIt );
-}
-
 void SyncNextNeighbors::generateSynchronizationMessages(data::ParticleStorage& ps,
                                                         const domain::IDomain& domain,
                                                         const real_t dx) const
@@ -130,7 +100,7 @@ void SyncNextNeighbors::generateSynchronizationMessages(data::ParticleStorage& p
 
          for (const auto& ghostOwner : pIt->getGhostOwners() )
          {
-            auto& buffer( bs.sendBuffer(ghostOwner) );
+            auto& buffer( bs.sendBuffer(static_cast<walberla::mpi::MPIRank>(ghostOwner)) );
 
             WALBERLA_LOG_DETAIL( "Sending removal notification for particle " << pIt->getUid() << " to process " << ghostOwner );
 
@@ -174,7 +144,7 @@ void SyncNextNeighbors::generateSynchronizationMessages(data::ParticleStorage& p
                auto& buffer( bs.sendBuffer(nbProcessRank) );
                WALBERLA_LOG_DETAIL( "Sending shadow copy notification for particle " << pIt->getUid() << " to process " << (nbProcessRank) );
                packNotification(buffer, ParticleCopyNotification( *pIt ));
-               pIt->getGhostOwnersRef().emplace_back( int_c(nbProcessRank) );
+               pIt->getGhostOwnersRef().insert( int_c(nbProcessRank) );
             }
          }
          else
@@ -232,7 +202,7 @@ void SyncNextNeighbors::generateSynchronizationMessages(data::ParticleStorage& p
 
          for( auto ghostRank : pIt->getGhostOwners() )
          {
-            auto& buffer( bs.sendBuffer(ghostRank) );
+            auto& buffer( bs.sendBuffer(static_cast<walberla::mpi::MPIRank>(ghostRank)) );
 
             WALBERLA_LOG_DETAIL( "Sending remote migration notification for particle " << pIt->getUid() <<
                                  " to process " << ghostRank );
@@ -240,7 +210,7 @@ void SyncNextNeighbors::generateSynchronizationMessages(data::ParticleStorage& p
             packNotification(buffer, ParticleRemoteMigrationNotification( *pIt, ownerRank ));
          }
 
-         pIt->getGhostOwnersRef().emplace_back( int_c(ownRank) );
+         pIt->getGhostOwnersRef().insert( int_c(ownRank) );
 
          // Send migration notification to new owner
          auto& buffer( bs.sendBuffer(ownerRank) );
diff --git a/python/mesa_pd/templates/mpi/notifications/NewGhostParticleNotification.templ.h b/python/mesa_pd/templates/mpi/notifications/NewGhostParticleNotification.templ.h
new file mode 100644
index 0000000000000000000000000000000000000000..7d8a8ce43f68a178090f062a8966e695166832ca
--- /dev/null
+++ b/python/mesa_pd/templates/mpi/notifications/NewGhostParticleNotification.templ.h
@@ -0,0 +1,100 @@
+//======================================================================================================================
+//
+//  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 NewGhostParticleNotification.h
+//! \author Sebastian Eibl <sebastian.eibl@fau.de>
+//
+//======================================================================================================================
+
+//======================================================================================================================
+//
+//  THIS FILE IS GENERATED - PLEASE CHANGE THE TEMPLATE !!!
+//
+//======================================================================================================================
+
+#pragma once
+
+#include <mesa_pd/data/DataTypes.h>
+#include <mesa_pd/data/ParticleStorage.h>
+#include <mesa_pd/mpi/notifications/NotificationType.h>
+
+#include <core/mpi/Datatype.h>
+#include <core/mpi/MPIWrapper.h>
+#include <core/mpi/RecvBuffer.h>
+#include <core/mpi/SendBuffer.h>
+
+namespace walberla {
+namespace mesa_pd {
+
+/**
+ * This notification is send to the owner of a particle
+ * to signal that a new ghost particle exists and the ghost particle list should be updated.
+ */
+class NewGhostParticleNotification
+{
+public:
+   struct Parameters
+   {
+      id_t    uid_;
+      walberla::mpi::MPIRank newOwner_;
+   };
+
+   inline explicit NewGhostParticleNotification( const data::Particle& particle, const walberla::mpi::MPIRank newOwner )
+      : particle_(particle)
+      , newOwner_(newOwner)
+   {}
+   const data::Particle& particle_;
+   walberla::mpi::MPIRank newOwner_;
+};
+
+template<>
+struct NotificationTrait<NewGhostParticleNotification>
+{
+   static const NotificationType id = NEW_GHOST_PARTICLE_NOTIFICATION;
+};
+
+}  // namespace mesa_pd
+}  // namespace walberla
+
+//======================================================================================================================
+//
+//  Send/Recv Buffer Serialization Specialization
+//
+//======================================================================================================================
+
+namespace walberla {
+namespace mpi {
+
+template< typename T,    // Element type of SendBuffer
+          typename G>    // Growth policy of SendBuffer
+mpi::GenericSendBuffer<T,G>& operator<<( mpi::GenericSendBuffer<T,G> & buf, const mesa_pd::NewGhostParticleNotification& obj )
+{
+   buf.addDebugMarker( "cn" );
+   buf << obj.particle_.getUid();
+   buf << obj.newOwner_;
+   return buf;
+}
+
+template< typename T>    // Element type  of RecvBuffer
+mpi::GenericRecvBuffer<T>& operator>>( mpi::GenericRecvBuffer<T> & buf, mesa_pd::NewGhostParticleNotification::Parameters& objparam )
+{
+   buf.readDebugMarker( "cn" );
+   buf >> objparam.uid_;
+   buf >> objparam.newOwner_;
+   return buf;
+}
+
+} // mpi
+} // walberla
diff --git a/python/mesa_pd/templates/mpi/notifications/ParseMessage.templ.h b/python/mesa_pd/templates/mpi/notifications/ParseMessage.templ.h
index 3bab14e36192a2788fef39902705731d73eb77cc..b144ddbdde45dfe7412152c864e82353515f7665 100644
--- a/python/mesa_pd/templates/mpi/notifications/ParseMessage.templ.h
+++ b/python/mesa_pd/templates/mpi/notifications/ParseMessage.templ.h
@@ -29,11 +29,13 @@
 
 #include <mesa_pd/data/ParticleStorage.h>
 #include <mesa_pd/domain/IDomain.h>
+#include <mesa_pd/mpi/notifications/NewGhostParticleNotification.h>
 #include <mesa_pd/mpi/notifications/NotificationType.h>
 #include <mesa_pd/mpi/notifications/ParticleCopyNotification.h>
 #include <mesa_pd/mpi/notifications/ParticleMigrationNotification.h>
 #include <mesa_pd/mpi/notifications/ParticleRemoteMigrationNotification.h>
 #include <mesa_pd/mpi/notifications/ParticleRemovalNotification.h>
+#include <mesa_pd/mpi/notifications/ParticleRemovalInformationNotification.h>
 #include <mesa_pd/mpi/notifications/ParticleUpdateNotification.h>
 
 #include <core/debug/Debug.h>
@@ -70,14 +72,18 @@ void ParseMessage::operator()(int sender,
 
       WALBERLA_LOG_DETAIL( "Received PARTICLE_COPY_NOTIFICATION for particle " << objparam.uid << "from neighboring process with rank " << sender );
 
-      WALBERLA_CHECK_EQUAL( ps.find(objparam.uid), ps.end(), "Ghost particle with id " << objparam.uid << " already existend.");
+      if ( ps.find(objparam.uid) == ps.end() )
+      {
+         auto pIt = createNewParticle(ps, objparam);
 
-      auto pIt = createNewParticle(ps, objparam);
+         domain.correctParticlePosition(pIt->getPositionRef());
 
-      domain.correctParticlePosition(pIt->getPositionRef());
-
-      WALBERLA_CHECK(!data::particle_flags::isSet(pIt->getFlags(), data::particle_flags::GHOST));
-      data::particle_flags::set(pIt->getFlagsRef(), data::particle_flags::GHOST);
+         //WALBERLA_CHECK(!data::particle_flags::isSet(pIt->getFlags(), data::particle_flags::GHOST));
+         data::particle_flags::set(pIt->getFlagsRef(), data::particle_flags::GHOST);
+      } else
+      {
+         WALBERLA_LOG_DETAIL("Ghost particle with id " << objparam.uid << " already existend.");
+      }
 
       WALBERLA_LOG_DETAIL( "Processed PARTICLE_COPY_NOTIFICATION for particle " << objparam.uid << "."  );
 
@@ -185,8 +191,61 @@ void ParseMessage::operator()(int sender,
 
       break;
    }
+   case NEW_GHOST_PARTICLE_NOTIFICATION: {
+      NewGhostParticleNotification::Parameters objparam;
+      rb >> objparam;
+
+      WALBERLA_LOG_DETAIL( "Received new ghost particle notification for particle " <<
+                           objparam.uid_ <<
+                           " from neighboring process with rank " <<
+                           sender <<
+                           "." );
+
+      auto pIt = ps.find( objparam.uid_ );
+      WALBERLA_CHECK_UNEQUAL( pIt, ps.end() );
+
+      pIt->getGhostOwnersRef().insert( objparam.newOwner_ );
+
+      WALBERLA_LOG_DETAIL( "Processed new ghost particle notification" );
+
+      break;
+   }
+   case PARTICLE_REMOVAL_INFORMATION_NOTIFICATION: {
+      ParticleRemovalInformationNotification::Parameters objparam;
+      rb >> objparam;
+
+      WALBERLA_LOG_DETAIL( "Received particle removal information notification for particle " <<
+                           objparam.uid_ <<
+                           " from neighboring process with rank " <<
+                           sender <<
+                           "." );
+
+      if (objparam.owner_ == receiver_)
+      {
+         using namespace walberla::mesa_pd::data::particle_flags;
+         auto pIt = ps.find( objparam.uid_ );
+         WALBERLA_CHECK_UNEQUAL( pIt, ps.end() );
+         WALBERLA_CHECK(!isSet( pIt->getFlags(), GHOST));
+
+         pIt->getGhostOwnersRef().erase( sender );
+         pIt->getNeighborStateRef().erase( sender );
+      } else
+      {
+         using namespace walberla::mesa_pd::data::particle_flags;
+         auto pIt = ps.find( objparam.uid_ );
+         if (pIt != ps.end() )
+         {
+            WALBERLA_CHECK(isSet( pIt->getFlags(), GHOST));
+            pIt->getNeighborStateRef().erase( sender );
+         }
+      }
+
+      WALBERLA_LOG_DETAIL( "Processed rigid body removal information notification" );
+
+      break;
+   }
    default:
-      throw std::runtime_error( "Received invalid notification type." );
+      WALBERLA_ABORT( "Received invalid notification type: " << notificationType << " from sender: " << sender );
    }
 }
 
diff --git a/python/mesa_pd/templates/mpi/notifications/ParticleRemovalInformationNotification.templ.h b/python/mesa_pd/templates/mpi/notifications/ParticleRemovalInformationNotification.templ.h
new file mode 100644
index 0000000000000000000000000000000000000000..15f10d48cbb415941dc1685595509265d291852f
--- /dev/null
+++ b/python/mesa_pd/templates/mpi/notifications/ParticleRemovalInformationNotification.templ.h
@@ -0,0 +1,97 @@
+//======================================================================================================================
+//
+//  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 ParticleRemovalInformationNotification.h
+//! \author Sebastian Eibl <sebastian.eibl@fau.de>
+//
+//======================================================================================================================
+
+//======================================================================================================================
+//
+//  THIS FILE IS GENERATED - PLEASE CHANGE THE TEMPLATE !!!
+//
+//======================================================================================================================
+
+#pragma once
+
+#include <mesa_pd/data/DataTypes.h>
+#include <mesa_pd/data/ParticleStorage.h>
+#include <mesa_pd/mpi/notifications/NotificationType.h>
+
+#include <core/mpi/Datatype.h>
+#include <core/mpi/RecvBuffer.h>
+#include <core/mpi/SendBuffer.h>
+
+namespace walberla {
+namespace mesa_pd {
+
+/**
+ * The ParticleRemovalInformationNotification class is used to signal other processes that a
+ * shadow copy was destroyed.
+ */
+class ParticleRemovalInformationNotification
+{
+public:
+   struct Parameters
+   {
+      id_t    uid_;
+      walberla::mpi::MPIRank owner_;
+   };
+
+   inline explicit ParticleRemovalInformationNotification( const data::Particle& particle )
+      : particle_(particle)
+   {}
+   const data::Particle& particle_;
+};
+
+template<>
+struct NotificationTrait<ParticleRemovalInformationNotification>
+{
+   static const NotificationType id = PARTICLE_REMOVAL_INFORMATION_NOTIFICATION;
+};
+
+}  // namespace mesa_pd
+}  // namespace walberla
+
+//======================================================================================================================
+//
+//  Send/Recv Buffer Serialization Specialization
+//
+//======================================================================================================================
+
+namespace walberla {
+namespace mpi {
+
+template< typename T,    // Element type of SendBuffer
+          typename G>    // Growth policy of SendBuffer
+mpi::GenericSendBuffer<T,G>& operator<<( mpi::GenericSendBuffer<T,G> & buf, const mesa_pd::ParticleRemovalInformationNotification& obj )
+{
+   buf.addDebugMarker( "ri" );
+   buf << obj.particle_.getUid();
+   buf << static_cast<walberla::mpi::MPIRank>(obj.particle_.getOwner());
+   return buf;
+}
+
+template< typename T>    // Element type  of RecvBuffer
+mpi::GenericRecvBuffer<T>& operator>>( mpi::GenericRecvBuffer<T> & buf, mesa_pd::ParticleRemovalInformationNotification::Parameters& objparam )
+{
+   buf.readDebugMarker( "ri" );
+   buf >> objparam.uid_;
+   buf >> objparam.owner_;
+   return buf;
+}
+
+} // mpi
+} // walberla
diff --git a/src/blockforest/BlockID.h b/src/blockforest/BlockID.h
index dddb36d96c9659d9e7e8d6460c5bfd939726fe78..9a28bc690cdeddbac83a206704a3eb41359bede1 100644
--- a/src/blockforest/BlockID.h
+++ b/src/blockforest/BlockID.h
@@ -260,8 +260,8 @@ class BlockID : public IBlockID {
 
 public:
 
-   inline BlockID() : id_( uint_c(0) ) {}
-   inline BlockID( const BlockID& id ) : id_( id.id_ ) {}
+   inline BlockID() = default;
+   inline BlockID( const BlockID& id ) = default;
    inline BlockID( const uint_t id ) : id_( id ) {}
    inline BlockID( const uint_t treeIndex, const uint_t treeIdMarker );
    inline BlockID( const BlockID& id, const uint_t branchId );
@@ -302,7 +302,7 @@ public:
 
 private:
 
-   uint_t id_;
+   uint_t id_ = uint_c(0);
 
 }; // class BlockID
 
diff --git a/src/core/debug/OperatorCheck.h b/src/core/debug/OperatorCheck.h
index a9b3792970dadab5ed9b7a9d0430cba83f7157f1..49bce766129c64aab0c08656d33d5a8d923b2872 100644
--- a/src/core/debug/OperatorCheck.h
+++ b/src/core/debug/OperatorCheck.h
@@ -20,6 +20,8 @@
 //
 //======================================================================================================================
 
+#pragma once
+
 #include <algorithm>
 #include <functional>
 #include <iostream>
@@ -89,4 +91,4 @@ template<class X, class Y> using has_right_shift = op_valid<X, Y, notstd::right_
 
 
 } // namespace debug
-} // namespace walberla
\ No newline at end of file
+} // namespace walberla
diff --git a/src/core/mpi/BufferDataTypeExtensions.h b/src/core/mpi/BufferDataTypeExtensions.h
index 955c153ec9d067e3ab1c48cebf6c3df52220f8fa..e1ec3755038f6e13c5df4ede2a738a9c17eb1977 100644
--- a/src/core/mpi/BufferDataTypeExtensions.h
+++ b/src/core/mpi/BufferDataTypeExtensions.h
@@ -37,6 +37,8 @@
 #include <map>
 #include <set>
 #include <string>
+#include <unordered_map>
+#include <unordered_set>
 #include <utility>
 #include <vector>
 
@@ -411,6 +413,32 @@ GenericRecvBuffer<T>& operator>>( GenericRecvBuffer<T> & buf, std::set<CK, CC, C
 template<typename T, typename C, typename A>
 struct BufferSizeTrait< std::set<T,C,A> > { static const bool constantSize = false;  };
 
+template< typename T,    // Element type of SendBuffer
+          typename G,    // Growth policy of SendBuffer
+          typename CK,   // Key type
+          typename CC,   // Comparison type
+          typename CA>   // Allocator type
+GenericSendBuffer<T,G>& operator<<( GenericSendBuffer<T,G> & buf, const std::unordered_set<CK, CC, CA> & c )
+{
+   buf.addDebugMarker( "us" );
+   sendAssocContainer(buf, c);
+   return buf;
+}
+
+template< typename T,    // Element type of RecvBuffer
+          typename CK,   // Key type
+          typename CC,   // Comparison type
+          typename CA>   // Allocator type
+GenericRecvBuffer<T>& operator>>( GenericRecvBuffer<T> & buf, std::unordered_set<CK, CC, CA> & c )
+{
+   buf.readDebugMarker( "us" );
+   recvAssocContainer(buf, c);
+   return buf;
+}
+
+template<typename T, typename C, typename A>
+struct BufferSizeTrait< std::unordered_set<T,C,A> > { static const bool constantSize = false;  };
+
 
 
 
@@ -473,6 +501,34 @@ GenericRecvBuffer<T>& operator>>( GenericRecvBuffer<T> & buf, std::map<CK, CT, C
 template<typename T, typename K, typename C, typename A>
 struct BufferSizeTrait< std::map<K,T,C,A> > { static const bool constantSize = false;  };
 
+template< typename T,    // Element type of SendBuffer
+          typename G,    // Growth policy of SendBuffer
+          typename CK,   // Key type
+          typename CT,   // Element type
+          typename CC,   // Comparison type
+          typename CA>   // Allocator type
+GenericSendBuffer<T,G>& operator<<( GenericSendBuffer<T,G> & buf, const std::unordered_map<CK, CT, CC, CA> & c )
+{
+   buf.addDebugMarker( "um" );
+   sendAssocContainer(buf, c);
+   return buf;
+}
+
+template< typename T,    // Element type of RecvBuffer
+          typename CK,   // Key type
+          typename CT,   // Element type
+          typename CC,   // Comparison type
+          typename CA>   // Allocator type
+GenericRecvBuffer<T>& operator>>( GenericRecvBuffer<T> & buf, std::unordered_map<CK, CT, CC, CA> & c )
+{
+   buf.readDebugMarker( "um" );
+   recvMap(buf, c);
+   return buf;
+}
+
+template<typename T, typename K, typename C, typename A>
+struct BufferSizeTrait< std::unordered_map<K,T,C,A> > { static const bool constantSize = false;  };
+
 
 
 
diff --git a/src/core/mpi/BufferSystem.h b/src/core/mpi/BufferSystem.h
index e4ba192e59876f68b3cf4d26d5da3fe6a10e9574..8be65ea4a355644a39381eb29944375010fbc4f5 100644
--- a/src/core/mpi/BufferSystem.h
+++ b/src/core/mpi/BufferSystem.h
@@ -189,7 +189,7 @@ public:
    /*! \name Status Queries  */
    //@{
    bool isSizeCommunicatedInNextStep() const { return (currentComm_ == &unknownSizeComm_); }
-   bool isCommunciationRunning() const       { return communicationRunning_;               }
+   bool isCommunicationRunning() const       { return communicationRunning_;               }
    bool isReceiverInformationSet() const     { return currentComm_ != NULL;                }
    //@}
    //*******************************************************************************************************************
diff --git a/src/core/mpi/OpenMPBufferSystem.impl.h b/src/core/mpi/OpenMPBufferSystem.impl.h
index dc042e8f44d870ff02d513938006c0ad981579dd..dd0efe661cc0598d802627ebfda64952dbd4e640 100644
--- a/src/core/mpi/OpenMPBufferSystem.impl.h
+++ b/src/core/mpi/OpenMPBufferSystem.impl.h
@@ -210,7 +210,7 @@ void GenericOpenMPBufferSystem<Rb, Sb>::waitOpenMP()
    WALBERLA_ASSERT_NULLPTR( ret ); // call last time to finish communication
    WALBERLA_UNUSED( ret );
 
-   WALBERLA_ASSERT( ! bs_.isCommunciationRunning() );
+   WALBERLA_ASSERT( ! bs_.isCommunicationRunning() );
 }
 
 
diff --git a/src/core/timing/TimingJSON.h b/src/core/timing/TimingJSON.h
index b6826f36762b28fc5d960e55468883675d23558c..437fe8fe24d638dc101833b69ed39093cc8d2f68 100644
--- a/src/core/timing/TimingJSON.h
+++ b/src/core/timing/TimingJSON.h
@@ -19,6 +19,8 @@
 //
 //======================================================================================================================
 
+#pragma once
+
 #include "core/extern/json.hpp"
 #include "core/timing/Timer.h"
 #include "core/timing/TimingNode.h"
@@ -69,4 +71,4 @@ void to_json( nlohmann::json& j, const TimingTree< TP >& tt )
 
 
 }
-}
\ No newline at end of file
+}
diff --git a/src/cuda/ExecutionTreeSweepGPU.h b/src/cuda/ExecutionTreeSweepGPU.h
index e5ad3d2a6abb8d7e2a8a524020b95ac632faa639..6f97277c4b75a5fe2dcdf0ec383ed8217699f1b2 100644
--- a/src/cuda/ExecutionTreeSweepGPU.h
+++ b/src/cuda/ExecutionTreeSweepGPU.h
@@ -19,6 +19,8 @@
 //
 //==============================================================================================================================================================
 
+#pragma once
+
 #include "domain_decomposition/IBlock.h"
 #include "executiontree/ExecutionTree.h"
 #include "ExecutionTreeGPU.h"
diff --git a/src/lbm/SuViscoelasticity.h b/src/lbm/SuViscoelasticity.h
index 493a3fa4987fb27d911317e93ee7686d5d1142e3..e1bd26a46f50f299a975dc64d1bcabd2ffc3b551 100644
--- a/src/lbm/SuViscoelasticity.h
+++ b/src/lbm/SuViscoelasticity.h
@@ -20,6 +20,7 @@
 //
 //======================================================================================================================
 
+#pragma once
 
 #include "blockforest/communication/UniformBufferedScheme.h"
 #include "core/math/Matrix3.h"
diff --git a/src/mesa_pd/data/ContactAccessor.h b/src/mesa_pd/data/ContactAccessor.h
new file mode 100644
index 0000000000000000000000000000000000000000..b0f76962491472c0f0efb436d67a2a919ec43cab
--- /dev/null
+++ b/src/mesa_pd/data/ContactAccessor.h
@@ -0,0 +1,255 @@
+//======================================================================================================================
+//
+//  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 ContactAccessor.h
+//! \author Sebastian Eibl <sebastian.eibl@fau.de>
+//
+//======================================================================================================================
+
+//======================================================================================================================
+//
+//  THIS FILE IS GENERATED - PLEASE CHANGE THE TEMPLATE !!!
+//
+//======================================================================================================================
+
+#pragma once
+
+#include <mesa_pd/data/IAccessor.h>
+#include <mesa_pd/data/ContactStorage.h>
+
+#include <core/UniqueID.h>
+
+#include <limits>
+
+namespace walberla {
+namespace mesa_pd {
+namespace data {
+
+/**
+ * @brief Basic ContactAccessor for the ContactStorage
+ *
+ * Provides get, set and getRef for all members of the ContactStorage.
+ * Can be used as a basis class for a more advanced ContactAccessor.
+ */
+class ContactAccessor : public IAccessor
+{
+public:
+   ContactAccessor(const std::shared_ptr<data::ContactStorage>& ps) : ps_(ps) {}
+   virtual ~ContactAccessor() = default;
+   const walberla::id_t& getUid(const size_t p_idx) const {return ps_->getUid(p_idx);}
+   walberla::id_t& getUidRef(const size_t p_idx) {return ps_->getUidRef(p_idx);}
+   void setUid(const size_t p_idx, const walberla::id_t& v) { ps_->setUid(p_idx, v);}
+   
+   const walberla::id_t& getId1(const size_t p_idx) const {return ps_->getId1(p_idx);}
+   walberla::id_t& getId1Ref(const size_t p_idx) {return ps_->getId1Ref(p_idx);}
+   void setId1(const size_t p_idx, const walberla::id_t& v) { ps_->setId1(p_idx, v);}
+   
+   const walberla::id_t& getId2(const size_t p_idx) const {return ps_->getId2(p_idx);}
+   walberla::id_t& getId2Ref(const size_t p_idx) {return ps_->getId2Ref(p_idx);}
+   void setId2(const size_t p_idx, const walberla::id_t& v) { ps_->setId2(p_idx, v);}
+   
+   const real_t& getDistance(const size_t p_idx) const {return ps_->getDistance(p_idx);}
+   real_t& getDistanceRef(const size_t p_idx) {return ps_->getDistanceRef(p_idx);}
+   void setDistance(const size_t p_idx, const real_t& v) { ps_->setDistance(p_idx, v);}
+   
+   const walberla::mesa_pd::Vec3& getNormal(const size_t p_idx) const {return ps_->getNormal(p_idx);}
+   walberla::mesa_pd::Vec3& getNormalRef(const size_t p_idx) {return ps_->getNormalRef(p_idx);}
+   void setNormal(const size_t p_idx, const walberla::mesa_pd::Vec3& v) { ps_->setNormal(p_idx, v);}
+   
+   const walberla::mesa_pd::Vec3& getPosition(const size_t p_idx) const {return ps_->getPosition(p_idx);}
+   walberla::mesa_pd::Vec3& getPositionRef(const size_t p_idx) {return ps_->getPositionRef(p_idx);}
+   void setPosition(const size_t p_idx, const walberla::mesa_pd::Vec3& v) { ps_->setPosition(p_idx, v);}
+   
+   const walberla::mesa_pd::Vec3& getT(const size_t p_idx) const {return ps_->getT(p_idx);}
+   walberla::mesa_pd::Vec3& getTRef(const size_t p_idx) {return ps_->getTRef(p_idx);}
+   void setT(const size_t p_idx, const walberla::mesa_pd::Vec3& v) { ps_->setT(p_idx, v);}
+   
+   const walberla::mesa_pd::Vec3& getO(const size_t p_idx) const {return ps_->getO(p_idx);}
+   walberla::mesa_pd::Vec3& getORef(const size_t p_idx) {return ps_->getORef(p_idx);}
+   void setO(const size_t p_idx, const walberla::mesa_pd::Vec3& v) { ps_->setO(p_idx, v);}
+   
+   const walberla::mesa_pd::Vec3& getR1(const size_t p_idx) const {return ps_->getR1(p_idx);}
+   walberla::mesa_pd::Vec3& getR1Ref(const size_t p_idx) {return ps_->getR1Ref(p_idx);}
+   void setR1(const size_t p_idx, const walberla::mesa_pd::Vec3& v) { ps_->setR1(p_idx, v);}
+   
+   const walberla::mesa_pd::Vec3& getR2(const size_t p_idx) const {return ps_->getR2(p_idx);}
+   walberla::mesa_pd::Vec3& getR2Ref(const size_t p_idx) {return ps_->getR2Ref(p_idx);}
+   void setR2(const size_t p_idx, const walberla::mesa_pd::Vec3& v) { ps_->setR2(p_idx, v);}
+   
+   const real_t& getMu(const size_t p_idx) const {return ps_->getMu(p_idx);}
+   real_t& getMuRef(const size_t p_idx) {return ps_->getMuRef(p_idx);}
+   void setMu(const size_t p_idx, const real_t& v) { ps_->setMu(p_idx, v);}
+   
+   const walberla::mesa_pd::Vec3& getP(const size_t p_idx) const {return ps_->getP(p_idx);}
+   walberla::mesa_pd::Vec3& getPRef(const size_t p_idx) {return ps_->getPRef(p_idx);}
+   void setP(const size_t p_idx, const walberla::mesa_pd::Vec3& v) { ps_->setP(p_idx, v);}
+   
+   const walberla::mesa_pd::Mat3& getDiag_nto(const size_t p_idx) const {return ps_->getDiag_nto(p_idx);}
+   walberla::mesa_pd::Mat3& getDiag_ntoRef(const size_t p_idx) {return ps_->getDiag_ntoRef(p_idx);}
+   void setDiag_nto(const size_t p_idx, const walberla::mesa_pd::Mat3& v) { ps_->setDiag_nto(p_idx, v);}
+   
+   const walberla::mesa_pd::Mat3& getDiag_nto_inv(const size_t p_idx) const {return ps_->getDiag_nto_inv(p_idx);}
+   walberla::mesa_pd::Mat3& getDiag_nto_invRef(const size_t p_idx) {return ps_->getDiag_nto_invRef(p_idx);}
+   void setDiag_nto_inv(const size_t p_idx, const walberla::mesa_pd::Mat3& v) { ps_->setDiag_nto_inv(p_idx, v);}
+   
+   const walberla::mesa_pd::Mat2& getDiag_to_inv(const size_t p_idx) const {return ps_->getDiag_to_inv(p_idx);}
+   walberla::mesa_pd::Mat2& getDiag_to_invRef(const size_t p_idx) {return ps_->getDiag_to_invRef(p_idx);}
+   void setDiag_to_inv(const size_t p_idx, const walberla::mesa_pd::Mat2& v) { ps_->setDiag_to_inv(p_idx, v);}
+   
+   const real_t& getDiag_n_inv(const size_t p_idx) const {return ps_->getDiag_n_inv(p_idx);}
+   real_t& getDiag_n_invRef(const size_t p_idx) {return ps_->getDiag_n_invRef(p_idx);}
+   void setDiag_n_inv(const size_t p_idx, const real_t& v) { ps_->setDiag_n_inv(p_idx, v);}
+   
+
+   id_t getInvalidUid() const {return UniqueID<data::Contact>::invalidID();}
+   size_t getInvalidIdx() const {return std::numeric_limits<size_t>::max();}
+   /**
+   * @brief Returns the index of Contact specified by uid.
+   * @param uid unique id of the Contact to be looked up
+   * @return the index of the Contact or std::numeric_limits<size_t>::max() if the Contact is not found
+   */
+   size_t uidToIdx(const id_t& uid) const {auto it = ps_->find(uid); return it != ps_->end() ? it.getIdx() : std::numeric_limits<size_t>::max();}
+   size_t size() const { return ps_->size(); }
+
+   inline size_t create(const id_t& uid);
+   inline size_t erase(const size_t& idx);
+   inline size_t find(const id_t& uid);
+protected:
+   std::shared_ptr<data::ContactStorage> ps_;
+};
+
+inline size_t ContactAccessor::create(const id_t& uid)
+{
+   auto it = ps_->create(uid);
+   return it.getIdx();
+}
+inline size_t ContactAccessor::erase(const size_t& idx)
+{
+   data::ContactStorage::iterator it(ps_.get(), idx);
+   it = ps_->erase(it);
+   return it.getIdx();
+}
+inline size_t ContactAccessor::find(const id_t& uid)
+{
+   auto it = ps_->find(uid);
+   return it.getIdx();
+}
+
+/**
+ * @brief Basic ContactAccessor which emulates a single Contact in a ContactStorage
+ * without actually needing a ContactStorage. This class is used mainly for testing purposes.
+ *
+ * Provides get, set and getRef.
+ */
+class SingleContactAccessor : public IAccessor
+{
+public:
+   virtual ~SingleContactAccessor() = default;
+   const walberla::id_t& getUid(const size_t /*p_idx*/) const {return uid_;}
+   void setUid(const size_t /*p_idx*/, const walberla::id_t& v) { uid_ = v;}
+   walberla::id_t& getUidRef(const size_t /*p_idx*/) {return uid_;}
+   
+   const walberla::id_t& getId1(const size_t /*p_idx*/) const {return id1_;}
+   void setId1(const size_t /*p_idx*/, const walberla::id_t& v) { id1_ = v;}
+   walberla::id_t& getId1Ref(const size_t /*p_idx*/) {return id1_;}
+   
+   const walberla::id_t& getId2(const size_t /*p_idx*/) const {return id2_;}
+   void setId2(const size_t /*p_idx*/, const walberla::id_t& v) { id2_ = v;}
+   walberla::id_t& getId2Ref(const size_t /*p_idx*/) {return id2_;}
+   
+   const real_t& getDistance(const size_t /*p_idx*/) const {return distance_;}
+   void setDistance(const size_t /*p_idx*/, const real_t& v) { distance_ = v;}
+   real_t& getDistanceRef(const size_t /*p_idx*/) {return distance_;}
+   
+   const walberla::mesa_pd::Vec3& getNormal(const size_t /*p_idx*/) const {return normal_;}
+   void setNormal(const size_t /*p_idx*/, const walberla::mesa_pd::Vec3& v) { normal_ = v;}
+   walberla::mesa_pd::Vec3& getNormalRef(const size_t /*p_idx*/) {return normal_;}
+   
+   const walberla::mesa_pd::Vec3& getPosition(const size_t /*p_idx*/) const {return position_;}
+   void setPosition(const size_t /*p_idx*/, const walberla::mesa_pd::Vec3& v) { position_ = v;}
+   walberla::mesa_pd::Vec3& getPositionRef(const size_t /*p_idx*/) {return position_;}
+   
+   const walberla::mesa_pd::Vec3& getT(const size_t /*p_idx*/) const {return t_;}
+   void setT(const size_t /*p_idx*/, const walberla::mesa_pd::Vec3& v) { t_ = v;}
+   walberla::mesa_pd::Vec3& getTRef(const size_t /*p_idx*/) {return t_;}
+   
+   const walberla::mesa_pd::Vec3& getO(const size_t /*p_idx*/) const {return o_;}
+   void setO(const size_t /*p_idx*/, const walberla::mesa_pd::Vec3& v) { o_ = v;}
+   walberla::mesa_pd::Vec3& getORef(const size_t /*p_idx*/) {return o_;}
+   
+   const walberla::mesa_pd::Vec3& getR1(const size_t /*p_idx*/) const {return r1_;}
+   void setR1(const size_t /*p_idx*/, const walberla::mesa_pd::Vec3& v) { r1_ = v;}
+   walberla::mesa_pd::Vec3& getR1Ref(const size_t /*p_idx*/) {return r1_;}
+   
+   const walberla::mesa_pd::Vec3& getR2(const size_t /*p_idx*/) const {return r2_;}
+   void setR2(const size_t /*p_idx*/, const walberla::mesa_pd::Vec3& v) { r2_ = v;}
+   walberla::mesa_pd::Vec3& getR2Ref(const size_t /*p_idx*/) {return r2_;}
+   
+   const real_t& getMu(const size_t /*p_idx*/) const {return mu_;}
+   void setMu(const size_t /*p_idx*/, const real_t& v) { mu_ = v;}
+   real_t& getMuRef(const size_t /*p_idx*/) {return mu_;}
+   
+   const walberla::mesa_pd::Vec3& getP(const size_t /*p_idx*/) const {return p_;}
+   void setP(const size_t /*p_idx*/, const walberla::mesa_pd::Vec3& v) { p_ = v;}
+   walberla::mesa_pd::Vec3& getPRef(const size_t /*p_idx*/) {return p_;}
+   
+   const walberla::mesa_pd::Mat3& getDiag_nto(const size_t /*p_idx*/) const {return diag_nto_;}
+   void setDiag_nto(const size_t /*p_idx*/, const walberla::mesa_pd::Mat3& v) { diag_nto_ = v;}
+   walberla::mesa_pd::Mat3& getDiag_ntoRef(const size_t /*p_idx*/) {return diag_nto_;}
+   
+   const walberla::mesa_pd::Mat3& getDiag_nto_inv(const size_t /*p_idx*/) const {return diag_nto_inv_;}
+   void setDiag_nto_inv(const size_t /*p_idx*/, const walberla::mesa_pd::Mat3& v) { diag_nto_inv_ = v;}
+   walberla::mesa_pd::Mat3& getDiag_nto_invRef(const size_t /*p_idx*/) {return diag_nto_inv_;}
+   
+   const walberla::mesa_pd::Mat2& getDiag_to_inv(const size_t /*p_idx*/) const {return diag_to_inv_;}
+   void setDiag_to_inv(const size_t /*p_idx*/, const walberla::mesa_pd::Mat2& v) { diag_to_inv_ = v;}
+   walberla::mesa_pd::Mat2& getDiag_to_invRef(const size_t /*p_idx*/) {return diag_to_inv_;}
+   
+   const real_t& getDiag_n_inv(const size_t /*p_idx*/) const {return diag_n_inv_;}
+   void setDiag_n_inv(const size_t /*p_idx*/, const real_t& v) { diag_n_inv_ = v;}
+   real_t& getDiag_n_invRef(const size_t /*p_idx*/) {return diag_n_inv_;}
+   
+
+   id_t getInvalidUid() const {return UniqueID<data::Contact>::invalidID();}
+   size_t getInvalidIdx() const {return std::numeric_limits<size_t>::max();}
+   /**
+   * @brief Returns the index of Contact specified by uid.
+   * @param uid unique id of the Contact to be looked up
+   * @return the index of the Contact or std::numeric_limits<size_t>::max() if the Contact is not found
+   */
+   size_t uidToIdx(const id_t& uid) const {return uid == uid_ ? 0 : std::numeric_limits<size_t>::max();}
+   size_t size() const { return 1; }
+private:
+   walberla::id_t uid_;
+   walberla::id_t id1_;
+   walberla::id_t id2_;
+   real_t distance_;
+   walberla::mesa_pd::Vec3 normal_;
+   walberla::mesa_pd::Vec3 position_;
+   walberla::mesa_pd::Vec3 t_;
+   walberla::mesa_pd::Vec3 o_;
+   walberla::mesa_pd::Vec3 r1_;
+   walberla::mesa_pd::Vec3 r2_;
+   real_t mu_;
+   walberla::mesa_pd::Vec3 p_;
+   walberla::mesa_pd::Mat3 diag_nto_;
+   walberla::mesa_pd::Mat3 diag_nto_inv_;
+   walberla::mesa_pd::Mat2 diag_to_inv_;
+   real_t diag_n_inv_;
+};
+
+} //namespace data
+} //namespace mesa_pd
+} //namespace walberla
\ No newline at end of file
diff --git a/src/mesa_pd/data/ContactStorage.h b/src/mesa_pd/data/ContactStorage.h
new file mode 100644
index 0000000000000000000000000000000000000000..b257a59a0269118d5c7b65f5d737847cc40de72c
--- /dev/null
+++ b/src/mesa_pd/data/ContactStorage.h
@@ -0,0 +1,773 @@
+//======================================================================================================================
+//
+//  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 ContactStorage.h
+//! \author Sebastian Eibl <sebastian.eibl@fau.de>
+//! \author Tobias Leemann <tobias.leemann@fau.de>
+//
+//======================================================================================================================
+
+/**
+ * Storage for detected contacts which can be used to perform actions
+ * for all contacts, e.g. resolving, relaxation schemes...
+ * The InsertIntoContactStorage-Kernel can be used to insert a contact.
+ */
+
+//======================================================================================================================
+//
+//  THIS FILE IS GENERATED - PLEASE CHANGE THE TEMPLATE !!!
+//
+//======================================================================================================================
+
+#pragma once
+
+#include <mesa_pd/data/DataTypes.h>
+#include <mesa_pd/data/STLOverloads.h>
+
+#include <core/Abort.h>
+#include <core/debug/Debug.h>
+#include <core/math/AABB.h>
+#include <core/OpenMP.h>
+#include <core/STLIO.h>
+#include <core/UniqueID.h>
+
+#include <atomic>
+#include <limits>
+#include <map>
+#include <type_traits>
+#include <unordered_map>
+#include <vector>
+
+namespace walberla {
+namespace mesa_pd {
+namespace data {
+
+class ContactStorage;
+
+class ContactStorage
+{
+public:
+   class Contact
+   {
+   public:
+      constexpr Contact(ContactStorage& storage, const size_t i) : storage_(storage), i_(i) {}
+      constexpr Contact(const Contact&)  = default;
+      constexpr Contact(Contact&&)  = default;
+
+      Contact& operator=(const Contact& rhs);
+      Contact& operator=(Contact&& rhs);
+
+      Contact* operator->(){return this;}
+
+      
+      const walberla::id_t& getUid() const {return storage_.getUid(i_);}
+      walberla::id_t& getUidRef() {return storage_.getUidRef(i_);}
+      void setUid(const walberla::id_t& v) { storage_.setUid(i_, v);}
+      
+      const walberla::id_t& getId1() const {return storage_.getId1(i_);}
+      walberla::id_t& getId1Ref() {return storage_.getId1Ref(i_);}
+      void setId1(const walberla::id_t& v) { storage_.setId1(i_, v);}
+      
+      const walberla::id_t& getId2() const {return storage_.getId2(i_);}
+      walberla::id_t& getId2Ref() {return storage_.getId2Ref(i_);}
+      void setId2(const walberla::id_t& v) { storage_.setId2(i_, v);}
+      
+      const real_t& getDistance() const {return storage_.getDistance(i_);}
+      real_t& getDistanceRef() {return storage_.getDistanceRef(i_);}
+      void setDistance(const real_t& v) { storage_.setDistance(i_, v);}
+      
+      const walberla::mesa_pd::Vec3& getNormal() const {return storage_.getNormal(i_);}
+      walberla::mesa_pd::Vec3& getNormalRef() {return storage_.getNormalRef(i_);}
+      void setNormal(const walberla::mesa_pd::Vec3& v) { storage_.setNormal(i_, v);}
+      
+      const walberla::mesa_pd::Vec3& getPosition() const {return storage_.getPosition(i_);}
+      walberla::mesa_pd::Vec3& getPositionRef() {return storage_.getPositionRef(i_);}
+      void setPosition(const walberla::mesa_pd::Vec3& v) { storage_.setPosition(i_, v);}
+      
+      const walberla::mesa_pd::Vec3& getT() const {return storage_.getT(i_);}
+      walberla::mesa_pd::Vec3& getTRef() {return storage_.getTRef(i_);}
+      void setT(const walberla::mesa_pd::Vec3& v) { storage_.setT(i_, v);}
+      
+      const walberla::mesa_pd::Vec3& getO() const {return storage_.getO(i_);}
+      walberla::mesa_pd::Vec3& getORef() {return storage_.getORef(i_);}
+      void setO(const walberla::mesa_pd::Vec3& v) { storage_.setO(i_, v);}
+      
+      const walberla::mesa_pd::Vec3& getR1() const {return storage_.getR1(i_);}
+      walberla::mesa_pd::Vec3& getR1Ref() {return storage_.getR1Ref(i_);}
+      void setR1(const walberla::mesa_pd::Vec3& v) { storage_.setR1(i_, v);}
+      
+      const walberla::mesa_pd::Vec3& getR2() const {return storage_.getR2(i_);}
+      walberla::mesa_pd::Vec3& getR2Ref() {return storage_.getR2Ref(i_);}
+      void setR2(const walberla::mesa_pd::Vec3& v) { storage_.setR2(i_, v);}
+      
+      const real_t& getMu() const {return storage_.getMu(i_);}
+      real_t& getMuRef() {return storage_.getMuRef(i_);}
+      void setMu(const real_t& v) { storage_.setMu(i_, v);}
+      
+      const walberla::mesa_pd::Vec3& getP() const {return storage_.getP(i_);}
+      walberla::mesa_pd::Vec3& getPRef() {return storage_.getPRef(i_);}
+      void setP(const walberla::mesa_pd::Vec3& v) { storage_.setP(i_, v);}
+      
+      const walberla::mesa_pd::Mat3& getDiag_nto() const {return storage_.getDiag_nto(i_);}
+      walberla::mesa_pd::Mat3& getDiag_ntoRef() {return storage_.getDiag_ntoRef(i_);}
+      void setDiag_nto(const walberla::mesa_pd::Mat3& v) { storage_.setDiag_nto(i_, v);}
+      
+      const walberla::mesa_pd::Mat3& getDiag_nto_inv() const {return storage_.getDiag_nto_inv(i_);}
+      walberla::mesa_pd::Mat3& getDiag_nto_invRef() {return storage_.getDiag_nto_invRef(i_);}
+      void setDiag_nto_inv(const walberla::mesa_pd::Mat3& v) { storage_.setDiag_nto_inv(i_, v);}
+      
+      const walberla::mesa_pd::Mat2& getDiag_to_inv() const {return storage_.getDiag_to_inv(i_);}
+      walberla::mesa_pd::Mat2& getDiag_to_invRef() {return storage_.getDiag_to_invRef(i_);}
+      void setDiag_to_inv(const walberla::mesa_pd::Mat2& v) { storage_.setDiag_to_inv(i_, v);}
+      
+      const real_t& getDiag_n_inv() const {return storage_.getDiag_n_inv(i_);}
+      real_t& getDiag_n_invRef() {return storage_.getDiag_n_invRef(i_);}
+      void setDiag_n_inv(const real_t& v) { storage_.setDiag_n_inv(i_, v);}
+      
+
+      size_t getIdx() const {return i_;}
+   public:
+      ContactStorage& storage_;
+      const size_t i_;
+   };
+
+   class iterator
+   {
+   public:
+      using iterator_category = std::random_access_iterator_tag;
+      using value_type        = Contact;
+      using pointer           = Contact*;
+      using reference         = Contact&;
+      using difference_type   = std::ptrdiff_t;
+
+      explicit iterator(ContactStorage* storage, const size_t i) : storage_(storage), i_(i) {}
+      iterator(const iterator& it)         = default;
+      iterator(iterator&& it)              = default;
+      iterator& operator=(const iterator&) = default;
+      iterator& operator=(iterator&&)      = default;
+
+
+      Contact operator*(){return Contact{*storage_, i_};}
+      Contact operator->(){return Contact{*storage_, i_};}
+      iterator& operator++(){ ++i_; return *this; }
+      iterator operator++(int){ iterator tmp(*this); ++(*this); return tmp; }
+      iterator& operator--(){ --i_; return *this; }
+      iterator operator--(int){ iterator tmp(*this); --(*this); return tmp; }
+
+      iterator& operator+=(const size_t n){ i_+=n; return *this; }
+      iterator& operator-=(const size_t n){ i_-=n; return *this; }
+
+      friend iterator operator+(const iterator& it, const size_t n);
+      friend iterator operator+(const size_t n, const iterator& it);
+      friend iterator operator-(const iterator& it, const size_t n);
+      friend difference_type operator-(const iterator& lhs, const iterator& rhs);
+
+      friend bool operator==(const iterator& lhs, const iterator& rhs);
+      friend bool operator!=(const iterator& lhs, const iterator& rhs);
+      friend bool operator<(const iterator& lhs, const iterator& rhs);
+      friend bool operator>(const iterator& lhs, const iterator& rhs);
+      friend bool operator<=(const iterator& lhs, const iterator& rhs);
+      friend bool operator>=(const iterator& lhs, const iterator& rhs);
+
+      friend void swap(iterator& lhs, iterator& rhs);
+
+      size_t getIdx() const {return i_;}
+   private:
+      ContactStorage* storage_;
+      size_t i_;
+   };
+
+   explicit ContactStorage(const size_t size);
+
+   iterator begin() { return iterator(this, 0); }
+   iterator end()   { return iterator(this, size()); }
+   iterator operator[](const size_t n) { return iterator(this, n); }
+
+   
+   const walberla::id_t& getUid(const size_t idx) const {return uid_[idx];}
+   walberla::id_t& getUidRef(const size_t idx) {return uid_[idx];}
+   void setUid(const size_t idx, const walberla::id_t& v) { uid_[idx] = v; }
+   
+   const walberla::id_t& getId1(const size_t idx) const {return id1_[idx];}
+   walberla::id_t& getId1Ref(const size_t idx) {return id1_[idx];}
+   void setId1(const size_t idx, const walberla::id_t& v) { id1_[idx] = v; }
+   
+   const walberla::id_t& getId2(const size_t idx) const {return id2_[idx];}
+   walberla::id_t& getId2Ref(const size_t idx) {return id2_[idx];}
+   void setId2(const size_t idx, const walberla::id_t& v) { id2_[idx] = v; }
+   
+   const real_t& getDistance(const size_t idx) const {return distance_[idx];}
+   real_t& getDistanceRef(const size_t idx) {return distance_[idx];}
+   void setDistance(const size_t idx, const real_t& v) { distance_[idx] = v; }
+   
+   const walberla::mesa_pd::Vec3& getNormal(const size_t idx) const {return normal_[idx];}
+   walberla::mesa_pd::Vec3& getNormalRef(const size_t idx) {return normal_[idx];}
+   void setNormal(const size_t idx, const walberla::mesa_pd::Vec3& v) { normal_[idx] = v; }
+   
+   const walberla::mesa_pd::Vec3& getPosition(const size_t idx) const {return position_[idx];}
+   walberla::mesa_pd::Vec3& getPositionRef(const size_t idx) {return position_[idx];}
+   void setPosition(const size_t idx, const walberla::mesa_pd::Vec3& v) { position_[idx] = v; }
+   
+   const walberla::mesa_pd::Vec3& getT(const size_t idx) const {return t_[idx];}
+   walberla::mesa_pd::Vec3& getTRef(const size_t idx) {return t_[idx];}
+   void setT(const size_t idx, const walberla::mesa_pd::Vec3& v) { t_[idx] = v; }
+   
+   const walberla::mesa_pd::Vec3& getO(const size_t idx) const {return o_[idx];}
+   walberla::mesa_pd::Vec3& getORef(const size_t idx) {return o_[idx];}
+   void setO(const size_t idx, const walberla::mesa_pd::Vec3& v) { o_[idx] = v; }
+   
+   const walberla::mesa_pd::Vec3& getR1(const size_t idx) const {return r1_[idx];}
+   walberla::mesa_pd::Vec3& getR1Ref(const size_t idx) {return r1_[idx];}
+   void setR1(const size_t idx, const walberla::mesa_pd::Vec3& v) { r1_[idx] = v; }
+   
+   const walberla::mesa_pd::Vec3& getR2(const size_t idx) const {return r2_[idx];}
+   walberla::mesa_pd::Vec3& getR2Ref(const size_t idx) {return r2_[idx];}
+   void setR2(const size_t idx, const walberla::mesa_pd::Vec3& v) { r2_[idx] = v; }
+   
+   const real_t& getMu(const size_t idx) const {return mu_[idx];}
+   real_t& getMuRef(const size_t idx) {return mu_[idx];}
+   void setMu(const size_t idx, const real_t& v) { mu_[idx] = v; }
+   
+   const walberla::mesa_pd::Vec3& getP(const size_t idx) const {return p_[idx];}
+   walberla::mesa_pd::Vec3& getPRef(const size_t idx) {return p_[idx];}
+   void setP(const size_t idx, const walberla::mesa_pd::Vec3& v) { p_[idx] = v; }
+   
+   const walberla::mesa_pd::Mat3& getDiag_nto(const size_t idx) const {return diag_nto_[idx];}
+   walberla::mesa_pd::Mat3& getDiag_ntoRef(const size_t idx) {return diag_nto_[idx];}
+   void setDiag_nto(const size_t idx, const walberla::mesa_pd::Mat3& v) { diag_nto_[idx] = v; }
+   
+   const walberla::mesa_pd::Mat3& getDiag_nto_inv(const size_t idx) const {return diag_nto_inv_[idx];}
+   walberla::mesa_pd::Mat3& getDiag_nto_invRef(const size_t idx) {return diag_nto_inv_[idx];}
+   void setDiag_nto_inv(const size_t idx, const walberla::mesa_pd::Mat3& v) { diag_nto_inv_[idx] = v; }
+   
+   const walberla::mesa_pd::Mat2& getDiag_to_inv(const size_t idx) const {return diag_to_inv_[idx];}
+   walberla::mesa_pd::Mat2& getDiag_to_invRef(const size_t idx) {return diag_to_inv_[idx];}
+   void setDiag_to_inv(const size_t idx, const walberla::mesa_pd::Mat2& v) { diag_to_inv_[idx] = v; }
+   
+   const real_t& getDiag_n_inv(const size_t idx) const {return diag_n_inv_[idx];}
+   real_t& getDiag_n_invRef(const size_t idx) {return diag_n_inv_[idx];}
+   void setDiag_n_inv(const size_t idx, const real_t& v) { diag_n_inv_[idx] = v; }
+   
+
+   /**
+    * @brief creates a new Contact and returns an iterator pointing to it
+    *
+    * \attention Use this function only if you know what you are doing!
+    * Messing with the uid might break the simulation!
+    * If you are unsure use create(bool) instead.
+    * @param uid unique id of the Contact to be created
+    * @return iterator to the newly created Contact
+    */
+   inline iterator create(const id_t& uid);
+   inline iterator create();
+   inline iterator erase(iterator& it);
+   /// Finds the entry corresponding to \p uid.
+   /// \return iterator to the object or end iterator
+   inline iterator find(const id_t& uid);
+   inline void reserve(const size_t size);
+   inline void clear();
+   inline size_t size() const;
+
+   /**
+    * Calls the provided functor \p func for all Contacts selected by the selector.
+    *
+    * Additional arguments can be provided.
+    * Call syntax for the provided functor
+    * \code
+    * func( *this, i, std::forward<Args>(args)... );
+    * \endcode
+    * \param openmp enables/disables OpenMP parallelization of the kernel call
+    */
+   template <typename Selector, typename Accessor, typename Func, typename... Args>
+   inline void forEachContact(const bool openmp, const Selector& selector,
+                              Accessor& acForPS, Func&& func, Args&&... args);
+   template <typename Selector, typename Accessor, typename Func, typename... Args>
+   inline void forEachContact(const bool openmp, const Selector& selector,
+                              Accessor& acForPS, Func&& func, Args&&... args) const;
+
+   private:
+   std::vector<walberla::id_t> uid_ {};
+   std::vector<walberla::id_t> id1_ {};
+   std::vector<walberla::id_t> id2_ {};
+   std::vector<real_t> distance_ {};
+   std::vector<walberla::mesa_pd::Vec3> normal_ {};
+   std::vector<walberla::mesa_pd::Vec3> position_ {};
+   std::vector<walberla::mesa_pd::Vec3> t_ {};
+   std::vector<walberla::mesa_pd::Vec3> o_ {};
+   std::vector<walberla::mesa_pd::Vec3> r1_ {};
+   std::vector<walberla::mesa_pd::Vec3> r2_ {};
+   std::vector<real_t> mu_ {};
+   std::vector<walberla::mesa_pd::Vec3> p_ {};
+   std::vector<walberla::mesa_pd::Mat3> diag_nto_ {};
+   std::vector<walberla::mesa_pd::Mat3> diag_nto_inv_ {};
+   std::vector<walberla::mesa_pd::Mat2> diag_to_inv_ {};
+   std::vector<real_t> diag_n_inv_ {};
+   std::unordered_map<id_t, size_t> uidToIdx_;
+   static_assert(std::is_same<decltype(uid_)::value_type, id_t>::value,
+                 "Property uid of type id_t is missing. This property is required!");
+};
+using Contact = ContactStorage::Contact;
+
+inline
+ContactStorage::Contact& ContactStorage::Contact::operator=(const ContactStorage::Contact& rhs)
+{
+   getUidRef() = rhs.getUid();
+   getId1Ref() = rhs.getId1();
+   getId2Ref() = rhs.getId2();
+   getDistanceRef() = rhs.getDistance();
+   getNormalRef() = rhs.getNormal();
+   getPositionRef() = rhs.getPosition();
+   getTRef() = rhs.getT();
+   getORef() = rhs.getO();
+   getR1Ref() = rhs.getR1();
+   getR2Ref() = rhs.getR2();
+   getMuRef() = rhs.getMu();
+   getPRef() = rhs.getP();
+   getDiag_ntoRef() = rhs.getDiag_nto();
+   getDiag_nto_invRef() = rhs.getDiag_nto_inv();
+   getDiag_to_invRef() = rhs.getDiag_to_inv();
+   getDiag_n_invRef() = rhs.getDiag_n_inv();
+   return *this;
+}
+
+inline
+ContactStorage::Contact& ContactStorage::Contact::operator=(ContactStorage::Contact&& rhs)
+{
+   getUidRef() = std::move(rhs.getUidRef());
+   getId1Ref() = std::move(rhs.getId1Ref());
+   getId2Ref() = std::move(rhs.getId2Ref());
+   getDistanceRef() = std::move(rhs.getDistanceRef());
+   getNormalRef() = std::move(rhs.getNormalRef());
+   getPositionRef() = std::move(rhs.getPositionRef());
+   getTRef() = std::move(rhs.getTRef());
+   getORef() = std::move(rhs.getORef());
+   getR1Ref() = std::move(rhs.getR1Ref());
+   getR2Ref() = std::move(rhs.getR2Ref());
+   getMuRef() = std::move(rhs.getMuRef());
+   getPRef() = std::move(rhs.getPRef());
+   getDiag_ntoRef() = std::move(rhs.getDiag_ntoRef());
+   getDiag_nto_invRef() = std::move(rhs.getDiag_nto_invRef());
+   getDiag_to_invRef() = std::move(rhs.getDiag_to_invRef());
+   getDiag_n_invRef() = std::move(rhs.getDiag_n_invRef());
+   return *this;
+}
+
+inline
+std::ostream& operator<<( std::ostream& os, const ContactStorage::Contact& p )
+{
+   os << "==========    ==========" << "\n" <<
+         "idx                 : " << p.getIdx() << "\n" <<
+         "uid                 : " << p.getUid() << "\n" <<
+         "id1                 : " << p.getId1() << "\n" <<
+         "id2                 : " << p.getId2() << "\n" <<
+         "distance            : " << p.getDistance() << "\n" <<
+         "normal              : " << p.getNormal() << "\n" <<
+         "position            : " << p.getPosition() << "\n" <<
+         "t                   : " << p.getT() << "\n" <<
+         "o                   : " << p.getO() << "\n" <<
+         "r1                  : " << p.getR1() << "\n" <<
+         "r2                  : " << p.getR2() << "\n" <<
+         "mu                  : " << p.getMu() << "\n" <<
+         "p                   : " << p.getP() << "\n" <<
+         "diag_nto            : " << p.getDiag_nto() << "\n" <<
+         "diag_nto_inv        : " << p.getDiag_nto_inv() << "\n" <<
+         "diag_to_inv         : " << p.getDiag_to_inv() << "\n" <<
+         "diag_n_inv          : " << p.getDiag_n_inv() << "\n" <<
+         "================================" << std::endl;
+   return os;
+}
+
+inline
+ContactStorage::iterator operator+(const ContactStorage::iterator& it, const size_t n)
+{
+   return ContactStorage::iterator(it.storage_, it.i_+n);
+}
+
+inline
+ContactStorage::iterator operator+(const size_t n, const ContactStorage::iterator& it)
+{
+   return it + n;
+}
+
+inline
+ContactStorage::iterator operator-(const ContactStorage::iterator& it, const size_t n)
+{
+   return ContactStorage::iterator(it.storage_, it.i_-n);
+}
+
+inline
+ContactStorage::iterator::difference_type operator-(const ContactStorage::iterator& lhs, const ContactStorage::iterator& rhs)
+{
+   return int64_c(lhs.i_) - int64_c(rhs.i_);
+}
+
+inline bool operator==(const ContactStorage::iterator& lhs, const ContactStorage::iterator& rhs)
+{
+   WALBERLA_ASSERT_EQUAL(lhs.storage_, rhs.storage_);
+   return (lhs.i_ == rhs.i_);
+}
+inline bool operator!=(const ContactStorage::iterator& lhs, const ContactStorage::iterator& rhs)
+{
+   WALBERLA_ASSERT_EQUAL(lhs.storage_, rhs.storage_);
+   return (lhs.i_ != rhs.i_);
+}
+inline bool operator<(const ContactStorage::iterator& lhs, const ContactStorage::iterator& rhs)
+{
+   WALBERLA_ASSERT_EQUAL(lhs.storage_, rhs.storage_);
+   return (lhs.i_ < rhs.i_);
+}
+inline bool operator>(const ContactStorage::iterator& lhs, const ContactStorage::iterator& rhs)
+{
+   WALBERLA_ASSERT_EQUAL(lhs.storage_, rhs.storage_);
+   return (lhs.i_ > rhs.i_);
+}
+inline bool operator<=(const ContactStorage::iterator& lhs, const ContactStorage::iterator& rhs)
+{
+   WALBERLA_ASSERT_EQUAL(lhs.storage_, rhs.storage_);
+   return (lhs.i_ <= rhs.i_);
+}
+inline bool operator>=(const ContactStorage::iterator& lhs, const ContactStorage::iterator& rhs)
+{
+   WALBERLA_ASSERT_EQUAL(lhs.storage_, rhs.storage_);
+   return (lhs.i_ >= rhs.i_);
+}
+
+inline void swap(ContactStorage::iterator& lhs, ContactStorage::iterator& rhs)
+{
+   WALBERLA_ASSERT_EQUAL(lhs.storage_, rhs.storage_);
+   std::swap(lhs.i_, rhs.i_);
+}
+
+inline
+ContactStorage::ContactStorage(const size_t size)
+{
+   reserve(size);
+}
+
+
+inline ContactStorage::iterator ContactStorage::create(const id_t& uid)
+{
+   WALBERLA_ASSERT_EQUAL(uidToIdx_.find(uid),
+                         uidToIdx_.end(),
+                         "Contact with the same uid(" << uid <<") already existing at index(" << uidToIdx_.find(uid)->second << ")");
+   uid_.emplace_back(walberla::id_t(-1));
+   id1_.emplace_back(walberla::id_t(-1));
+   id2_.emplace_back(walberla::id_t(-1));
+   distance_.emplace_back(real_t(1));
+   normal_.emplace_back(real_t(0));
+   position_.emplace_back(real_t(0));
+   t_.emplace_back(real_t(0));
+   o_.emplace_back(real_t(0));
+   r1_.emplace_back(real_t(0));
+   r2_.emplace_back(real_t(0));
+   mu_.emplace_back(real_t(0));
+   p_.emplace_back(real_t(0));
+   diag_nto_.emplace_back(real_t(0));
+   diag_nto_inv_.emplace_back(real_t(0));
+   diag_to_inv_.emplace_back(real_t(0));
+   diag_n_inv_.emplace_back(real_t(0));
+   uid_.back() = uid;
+   uidToIdx_[uid] = uid_.size() - 1;
+   return iterator(this, size() - 1);
+}
+
+inline ContactStorage::iterator ContactStorage::create()
+{
+   return create(UniqueID<Contact>::create());
+}
+
+inline ContactStorage::iterator ContactStorage::erase(iterator& it)
+{
+   //swap with last element and pop
+   auto last = --end();
+   auto numElementsRemoved = uidToIdx_.erase(it->getUid());
+   WALBERLA_CHECK_EQUAL(numElementsRemoved,
+                        1,
+                        "Contact with uid " << it->getUid() << " cannot be removed (not existing).");
+   if (it != last) //skip swap if last element is removed
+   {
+      *it = *last;
+      uidToIdx_[it->getUid()] = it.getIdx();
+   }
+   uid_.pop_back();
+   id1_.pop_back();
+   id2_.pop_back();
+   distance_.pop_back();
+   normal_.pop_back();
+   position_.pop_back();
+   t_.pop_back();
+   o_.pop_back();
+   r1_.pop_back();
+   r2_.pop_back();
+   mu_.pop_back();
+   p_.pop_back();
+   diag_nto_.pop_back();
+   diag_nto_inv_.pop_back();
+   diag_to_inv_.pop_back();
+   diag_n_inv_.pop_back();
+   return it;
+}
+
+inline ContactStorage::iterator ContactStorage::find(const id_t& uid)
+{
+   //linear search through uid vector
+   //auto it = std::find(uid_.begin(), uid_.end(), uid);
+   //if (it == uid_.end()) return end();
+   //return iterator(this, uint_c(std::distance(uid_.begin(), it)));
+
+   //use unordered_map for faster lookup
+   auto it = uidToIdx_.find(uid);
+   if (it == uidToIdx_.end()) return end();
+   WALBERLA_ASSERT_EQUAL(it->first, uid, "Lookup via uidToIdx map is not up to date!!!");
+   return iterator(this, it->second);
+}
+
+inline void ContactStorage::reserve(const size_t size)
+{
+   uid_.reserve(size);
+   id1_.reserve(size);
+   id2_.reserve(size);
+   distance_.reserve(size);
+   normal_.reserve(size);
+   position_.reserve(size);
+   t_.reserve(size);
+   o_.reserve(size);
+   r1_.reserve(size);
+   r2_.reserve(size);
+   mu_.reserve(size);
+   p_.reserve(size);
+   diag_nto_.reserve(size);
+   diag_nto_inv_.reserve(size);
+   diag_to_inv_.reserve(size);
+   diag_n_inv_.reserve(size);
+}
+
+inline void ContactStorage::clear()
+{
+   uid_.clear();
+   id1_.clear();
+   id2_.clear();
+   distance_.clear();
+   normal_.clear();
+   position_.clear();
+   t_.clear();
+   o_.clear();
+   r1_.clear();
+   r2_.clear();
+   mu_.clear();
+   p_.clear();
+   diag_nto_.clear();
+   diag_nto_inv_.clear();
+   diag_to_inv_.clear();
+   diag_n_inv_.clear();
+   uidToIdx_.clear();
+}
+
+inline size_t ContactStorage::size() const
+{
+   //WALBERLA_ASSERT_EQUAL( uid_.size(), uid.size() );
+   //WALBERLA_ASSERT_EQUAL( uid_.size(), id1.size() );
+   //WALBERLA_ASSERT_EQUAL( uid_.size(), id2.size() );
+   //WALBERLA_ASSERT_EQUAL( uid_.size(), distance.size() );
+   //WALBERLA_ASSERT_EQUAL( uid_.size(), normal.size() );
+   //WALBERLA_ASSERT_EQUAL( uid_.size(), position.size() );
+   //WALBERLA_ASSERT_EQUAL( uid_.size(), t.size() );
+   //WALBERLA_ASSERT_EQUAL( uid_.size(), o.size() );
+   //WALBERLA_ASSERT_EQUAL( uid_.size(), r1.size() );
+   //WALBERLA_ASSERT_EQUAL( uid_.size(), r2.size() );
+   //WALBERLA_ASSERT_EQUAL( uid_.size(), mu.size() );
+   //WALBERLA_ASSERT_EQUAL( uid_.size(), p.size() );
+   //WALBERLA_ASSERT_EQUAL( uid_.size(), diag_nto.size() );
+   //WALBERLA_ASSERT_EQUAL( uid_.size(), diag_nto_inv.size() );
+   //WALBERLA_ASSERT_EQUAL( uid_.size(), diag_to_inv.size() );
+   //WALBERLA_ASSERT_EQUAL( uid_.size(), diag_n_inv.size() );
+   return uid_.size();
+}
+template <typename Selector, typename Accessor, typename Func, typename... Args>
+inline void ContactStorage::forEachContact(const bool openmp, const Selector& selector,
+                                           Accessor& acForPS, Func&& func, Args&&... args) 
+{
+   WALBERLA_UNUSED(openmp);
+   const uint64_t len = size();
+   #ifdef _OPENMP
+   #pragma omp parallel for schedule(static) if (openmp)
+   #endif
+   for (int64_t i = 0; i < int64_c(len); ++i)
+      if (selector(uint64_c(i), acForPS)){
+         func( uint64_c(i), std::forward<Args>(args)... );
+      }
+}
+template <typename Selector, typename Accessor, typename Func, typename... Args>
+inline void ContactStorage::forEachContact(const bool openmp, const Selector& selector,
+                                           Accessor& acForPS, Func&& func, Args&&... args) const
+{
+   WALBERLA_UNUSED(openmp);
+   const uint64_t len = size();
+   #ifdef _OPENMP
+   #pragma omp parallel for schedule(static) if (openmp)
+   #endif
+   for (int64_t i = 0; i < int64_c(len); ++i)
+      if (selector(uint64_c(i), acForPS)){
+         func( uint64_c(i), std::forward<Args>(args)... );
+      }
+}
+///Predicate that selects a certain property from a Contact
+class SelectContactUid
+{
+public:
+   using return_type = walberla::id_t;
+   walberla::id_t& operator()(data::Contact& p) const {return p.getUidRef();}
+   walberla::id_t& operator()(data::Contact&& p) const {return p.getUidRef();}
+   const walberla::id_t& operator()(const data::Contact& p) const {return p.getUid();}
+};
+///Predicate that selects a certain property from a Contact
+class SelectContactId1
+{
+public:
+   using return_type = walberla::id_t;
+   walberla::id_t& operator()(data::Contact& p) const {return p.getId1Ref();}
+   walberla::id_t& operator()(data::Contact&& p) const {return p.getId1Ref();}
+   const walberla::id_t& operator()(const data::Contact& p) const {return p.getId1();}
+};
+///Predicate that selects a certain property from a Contact
+class SelectContactId2
+{
+public:
+   using return_type = walberla::id_t;
+   walberla::id_t& operator()(data::Contact& p) const {return p.getId2Ref();}
+   walberla::id_t& operator()(data::Contact&& p) const {return p.getId2Ref();}
+   const walberla::id_t& operator()(const data::Contact& p) const {return p.getId2();}
+};
+///Predicate that selects a certain property from a Contact
+class SelectContactDistance
+{
+public:
+   using return_type = real_t;
+   real_t& operator()(data::Contact& p) const {return p.getDistanceRef();}
+   real_t& operator()(data::Contact&& p) const {return p.getDistanceRef();}
+   const real_t& operator()(const data::Contact& p) const {return p.getDistance();}
+};
+///Predicate that selects a certain property from a Contact
+class SelectContactNormal
+{
+public:
+   using return_type = walberla::mesa_pd::Vec3;
+   walberla::mesa_pd::Vec3& operator()(data::Contact& p) const {return p.getNormalRef();}
+   walberla::mesa_pd::Vec3& operator()(data::Contact&& p) const {return p.getNormalRef();}
+   const walberla::mesa_pd::Vec3& operator()(const data::Contact& p) const {return p.getNormal();}
+};
+///Predicate that selects a certain property from a Contact
+class SelectContactPosition
+{
+public:
+   using return_type = walberla::mesa_pd::Vec3;
+   walberla::mesa_pd::Vec3& operator()(data::Contact& p) const {return p.getPositionRef();}
+   walberla::mesa_pd::Vec3& operator()(data::Contact&& p) const {return p.getPositionRef();}
+   const walberla::mesa_pd::Vec3& operator()(const data::Contact& p) const {return p.getPosition();}
+};
+///Predicate that selects a certain property from a Contact
+class SelectContactT
+{
+public:
+   using return_type = walberla::mesa_pd::Vec3;
+   walberla::mesa_pd::Vec3& operator()(data::Contact& p) const {return p.getTRef();}
+   walberla::mesa_pd::Vec3& operator()(data::Contact&& p) const {return p.getTRef();}
+   const walberla::mesa_pd::Vec3& operator()(const data::Contact& p) const {return p.getT();}
+};
+///Predicate that selects a certain property from a Contact
+class SelectContactO
+{
+public:
+   using return_type = walberla::mesa_pd::Vec3;
+   walberla::mesa_pd::Vec3& operator()(data::Contact& p) const {return p.getORef();}
+   walberla::mesa_pd::Vec3& operator()(data::Contact&& p) const {return p.getORef();}
+   const walberla::mesa_pd::Vec3& operator()(const data::Contact& p) const {return p.getO();}
+};
+///Predicate that selects a certain property from a Contact
+class SelectContactR1
+{
+public:
+   using return_type = walberla::mesa_pd::Vec3;
+   walberla::mesa_pd::Vec3& operator()(data::Contact& p) const {return p.getR1Ref();}
+   walberla::mesa_pd::Vec3& operator()(data::Contact&& p) const {return p.getR1Ref();}
+   const walberla::mesa_pd::Vec3& operator()(const data::Contact& p) const {return p.getR1();}
+};
+///Predicate that selects a certain property from a Contact
+class SelectContactR2
+{
+public:
+   using return_type = walberla::mesa_pd::Vec3;
+   walberla::mesa_pd::Vec3& operator()(data::Contact& p) const {return p.getR2Ref();}
+   walberla::mesa_pd::Vec3& operator()(data::Contact&& p) const {return p.getR2Ref();}
+   const walberla::mesa_pd::Vec3& operator()(const data::Contact& p) const {return p.getR2();}
+};
+///Predicate that selects a certain property from a Contact
+class SelectContactMu
+{
+public:
+   using return_type = real_t;
+   real_t& operator()(data::Contact& p) const {return p.getMuRef();}
+   real_t& operator()(data::Contact&& p) const {return p.getMuRef();}
+   const real_t& operator()(const data::Contact& p) const {return p.getMu();}
+};
+///Predicate that selects a certain property from a Contact
+class SelectContactP
+{
+public:
+   using return_type = walberla::mesa_pd::Vec3;
+   walberla::mesa_pd::Vec3& operator()(data::Contact& p) const {return p.getPRef();}
+   walberla::mesa_pd::Vec3& operator()(data::Contact&& p) const {return p.getPRef();}
+   const walberla::mesa_pd::Vec3& operator()(const data::Contact& p) const {return p.getP();}
+};
+///Predicate that selects a certain property from a Contact
+class SelectContactDiag_nto
+{
+public:
+   using return_type = walberla::mesa_pd::Mat3;
+   walberla::mesa_pd::Mat3& operator()(data::Contact& p) const {return p.getDiag_ntoRef();}
+   walberla::mesa_pd::Mat3& operator()(data::Contact&& p) const {return p.getDiag_ntoRef();}
+   const walberla::mesa_pd::Mat3& operator()(const data::Contact& p) const {return p.getDiag_nto();}
+};
+///Predicate that selects a certain property from a Contact
+class SelectContactDiag_nto_inv
+{
+public:
+   using return_type = walberla::mesa_pd::Mat3;
+   walberla::mesa_pd::Mat3& operator()(data::Contact& p) const {return p.getDiag_nto_invRef();}
+   walberla::mesa_pd::Mat3& operator()(data::Contact&& p) const {return p.getDiag_nto_invRef();}
+   const walberla::mesa_pd::Mat3& operator()(const data::Contact& p) const {return p.getDiag_nto_inv();}
+};
+///Predicate that selects a certain property from a Contact
+class SelectContactDiag_to_inv
+{
+public:
+   using return_type = walberla::mesa_pd::Mat2;
+   walberla::mesa_pd::Mat2& operator()(data::Contact& p) const {return p.getDiag_to_invRef();}
+   walberla::mesa_pd::Mat2& operator()(data::Contact&& p) const {return p.getDiag_to_invRef();}
+   const walberla::mesa_pd::Mat2& operator()(const data::Contact& p) const {return p.getDiag_to_inv();}
+};
+///Predicate that selects a certain property from a Contact
+class SelectContactDiag_n_inv
+{
+public:
+   using return_type = real_t;
+   real_t& operator()(data::Contact& p) const {return p.getDiag_n_invRef();}
+   real_t& operator()(data::Contact&& p) const {return p.getDiag_n_invRef();}
+   const real_t& operator()(const data::Contact& p) const {return p.getDiag_n_inv();}
+};
+
+} //namespace data
+} //namespace mesa_pd
+} //namespace walberla
\ No newline at end of file
diff --git a/src/mesa_pd/data/DataTypes.h b/src/mesa_pd/data/DataTypes.h
index 0ab8e22b399336031f3780c82ad8a29a5bceb415..49da2f96340f497aa6d44f8c607ef6977881c2e9 100644
--- a/src/mesa_pd/data/DataTypes.h
+++ b/src/mesa_pd/data/DataTypes.h
@@ -21,9 +21,11 @@
 #pragma once
 
 #include <core/DataTypes.h>
+#include <core/math/Matrix2.h>
 #include <core/math/Matrix3.h>
 #include <core/math/Quaternion.h>
 #include <core/math/Rot3.h>
+#include <core/math/Vector2.h>
 #include <core/math/Vector3.h>
 
 #include <mesa_pd/data/Flags.h>
@@ -32,9 +34,10 @@ namespace walberla {
 namespace mesa_pd {
 
 using Mat3 = math::Matrix3<real_t>;
+using Mat2 = math::Matrix2<real_t>;
 using Rot3 = math::Rot3<real_t>;
 using Quat = math::Quaternion<real_t>;
 using Vec3 = math::Vector3<real_t>;
-
+using Vec2 = math::Vector2<real_t>;
 }
 }
diff --git a/src/mesa_pd/data/ParticleAccessor.h b/src/mesa_pd/data/ParticleAccessor.h
index 978ab96090a0b6174c3e59eb999974adedec0553..1f85d7348ae73806abf3529c4b65e2c8c203fceb 100644
--- a/src/mesa_pd/data/ParticleAccessor.h
+++ b/src/mesa_pd/data/ParticleAccessor.h
@@ -68,9 +68,9 @@ public:
    int& getOwnerRef(const size_t p_idx) {return ps_->getOwnerRef(p_idx);}
    void setOwner(const size_t p_idx, const int& v) { ps_->setOwner(p_idx, v);}
    
-   const std::vector<int>& getGhostOwners(const size_t p_idx) const {return ps_->getGhostOwners(p_idx);}
-   std::vector<int>& getGhostOwnersRef(const size_t p_idx) {return ps_->getGhostOwnersRef(p_idx);}
-   void setGhostOwners(const size_t p_idx, const std::vector<int>& v) { ps_->setGhostOwners(p_idx, v);}
+   const std::unordered_set<walberla::mpi::MPIRank>& getGhostOwners(const size_t p_idx) const {return ps_->getGhostOwners(p_idx);}
+   std::unordered_set<walberla::mpi::MPIRank>& getGhostOwnersRef(const size_t p_idx) {return ps_->getGhostOwnersRef(p_idx);}
+   void setGhostOwners(const size_t p_idx, const std::unordered_set<walberla::mpi::MPIRank>& v) { ps_->setGhostOwners(p_idx, v);}
    
    const size_t& getShapeID(const size_t p_idx) const {return ps_->getShapeID(p_idx);}
    size_t& getShapeIDRef(const size_t p_idx) {return ps_->getShapeIDRef(p_idx);}
@@ -132,6 +132,18 @@ public:
    walberla::real_t& getHeatFluxRef(const size_t p_idx) {return ps_->getHeatFluxRef(p_idx);}
    void setHeatFlux(const size_t p_idx, const walberla::real_t& v) { ps_->setHeatFlux(p_idx, v);}
    
+   const walberla::mesa_pd::Vec3& getDv(const size_t p_idx) const {return ps_->getDv(p_idx);}
+   walberla::mesa_pd::Vec3& getDvRef(const size_t p_idx) {return ps_->getDvRef(p_idx);}
+   void setDv(const size_t p_idx, const walberla::mesa_pd::Vec3& v) { ps_->setDv(p_idx, v);}
+   
+   const walberla::mesa_pd::Vec3& getDw(const size_t p_idx) const {return ps_->getDw(p_idx);}
+   walberla::mesa_pd::Vec3& getDwRef(const size_t p_idx) {return ps_->getDwRef(p_idx);}
+   void setDw(const size_t p_idx, const walberla::mesa_pd::Vec3& v) { ps_->setDw(p_idx, v);}
+   
+   const std::unordered_set<walberla::mpi::MPIRank>& getNeighborState(const size_t p_idx) const {return ps_->getNeighborState(p_idx);}
+   std::unordered_set<walberla::mpi::MPIRank>& getNeighborStateRef(const size_t p_idx) {return ps_->getNeighborStateRef(p_idx);}
+   void setNeighborState(const size_t p_idx, const std::unordered_set<walberla::mpi::MPIRank>& v) { ps_->setNeighborState(p_idx, v);}
+   
 
    id_t getInvalidUid() const {return UniqueID<data::Particle>::invalidID();}
    size_t getInvalidIdx() const {return std::numeric_limits<size_t>::max();}
@@ -197,9 +209,9 @@ public:
    void setOwner(const size_t /*p_idx*/, const int& v) { owner_ = v;}
    int& getOwnerRef(const size_t /*p_idx*/) {return owner_;}
    
-   const std::vector<int>& getGhostOwners(const size_t /*p_idx*/) const {return ghostOwners_;}
-   void setGhostOwners(const size_t /*p_idx*/, const std::vector<int>& v) { ghostOwners_ = v;}
-   std::vector<int>& getGhostOwnersRef(const size_t /*p_idx*/) {return ghostOwners_;}
+   const std::unordered_set<walberla::mpi::MPIRank>& getGhostOwners(const size_t /*p_idx*/) const {return ghostOwners_;}
+   void setGhostOwners(const size_t /*p_idx*/, const std::unordered_set<walberla::mpi::MPIRank>& v) { ghostOwners_ = v;}
+   std::unordered_set<walberla::mpi::MPIRank>& getGhostOwnersRef(const size_t /*p_idx*/) {return ghostOwners_;}
    
    const size_t& getShapeID(const size_t /*p_idx*/) const {return shapeID_;}
    void setShapeID(const size_t /*p_idx*/, const size_t& v) { shapeID_ = v;}
@@ -261,6 +273,18 @@ public:
    void setHeatFlux(const size_t /*p_idx*/, const walberla::real_t& v) { heatFlux_ = v;}
    walberla::real_t& getHeatFluxRef(const size_t /*p_idx*/) {return heatFlux_;}
    
+   const walberla::mesa_pd::Vec3& getDv(const size_t /*p_idx*/) const {return dv_;}
+   void setDv(const size_t /*p_idx*/, const walberla::mesa_pd::Vec3& v) { dv_ = v;}
+   walberla::mesa_pd::Vec3& getDvRef(const size_t /*p_idx*/) {return dv_;}
+   
+   const walberla::mesa_pd::Vec3& getDw(const size_t /*p_idx*/) const {return dw_;}
+   void setDw(const size_t /*p_idx*/, const walberla::mesa_pd::Vec3& v) { dw_ = v;}
+   walberla::mesa_pd::Vec3& getDwRef(const size_t /*p_idx*/) {return dw_;}
+   
+   const std::unordered_set<walberla::mpi::MPIRank>& getNeighborState(const size_t /*p_idx*/) const {return neighborState_;}
+   void setNeighborState(const size_t /*p_idx*/, const std::unordered_set<walberla::mpi::MPIRank>& v) { neighborState_ = v;}
+   std::unordered_set<walberla::mpi::MPIRank>& getNeighborStateRef(const size_t /*p_idx*/) {return neighborState_;}
+   
 
    id_t getInvalidUid() const {return UniqueID<data::Particle>::invalidID();}
    size_t getInvalidIdx() const {return std::numeric_limits<size_t>::max();}
@@ -277,7 +301,7 @@ private:
    walberla::real_t interactionRadius_;
    walberla::mesa_pd::data::particle_flags::FlagT flags_;
    int owner_;
-   std::vector<int> ghostOwners_;
+   std::unordered_set<walberla::mpi::MPIRank> ghostOwners_;
    size_t shapeID_;
    walberla::mesa_pd::Rot3 rotation_;
    walberla::mesa_pd::Vec3 angularVelocity_;
@@ -293,6 +317,9 @@ private:
    std::map<walberla::id_t, walberla::mesa_pd::data::ContactHistory> newContactHistory_;
    walberla::real_t temperature_;
    walberla::real_t heatFlux_;
+   walberla::mesa_pd::Vec3 dv_;
+   walberla::mesa_pd::Vec3 dw_;
+   std::unordered_set<walberla::mpi::MPIRank> neighborState_;
 };
 
 } //namespace data
diff --git a/src/mesa_pd/data/ParticleStorage.h b/src/mesa_pd/data/ParticleStorage.h
index 633563ca6f2d26ee7f51c569568b0151bb3f3ce9..e4d766318a1b8de41ac884e1b8483e24a08b4a19 100644
--- a/src/mesa_pd/data/ParticleStorage.h
+++ b/src/mesa_pd/data/ParticleStorage.h
@@ -31,6 +31,7 @@
 #include <map>
 #include <type_traits>
 #include <unordered_map>
+#include <unordered_set>
 #include <vector>
 
 #include <mesa_pd/data/ContactHistory.h>
@@ -42,6 +43,7 @@
 #include <core/Abort.h>
 #include <core/debug/Debug.h>
 #include <core/math/AABB.h>
+#include <core/mpi/MPIWrapper.h>
 #include <core/OpenMP.h>
 #include <core/STLIO.h>
 #include <core/UniqueID.h>
@@ -88,9 +90,9 @@ public:
       int& getOwnerRef() {return storage_.getOwnerRef(i_);}
       void setOwner(const int& v) { storage_.setOwner(i_, v);}
       
-      const std::vector<int>& getGhostOwners() const {return storage_.getGhostOwners(i_);}
-      std::vector<int>& getGhostOwnersRef() {return storage_.getGhostOwnersRef(i_);}
-      void setGhostOwners(const std::vector<int>& v) { storage_.setGhostOwners(i_, v);}
+      const std::unordered_set<walberla::mpi::MPIRank>& getGhostOwners() const {return storage_.getGhostOwners(i_);}
+      std::unordered_set<walberla::mpi::MPIRank>& getGhostOwnersRef() {return storage_.getGhostOwnersRef(i_);}
+      void setGhostOwners(const std::unordered_set<walberla::mpi::MPIRank>& v) { storage_.setGhostOwners(i_, v);}
       
       const size_t& getShapeID() const {return storage_.getShapeID(i_);}
       size_t& getShapeIDRef() {return storage_.getShapeIDRef(i_);}
@@ -152,6 +154,18 @@ public:
       walberla::real_t& getHeatFluxRef() {return storage_.getHeatFluxRef(i_);}
       void setHeatFlux(const walberla::real_t& v) { storage_.setHeatFlux(i_, v);}
       
+      const walberla::mesa_pd::Vec3& getDv() const {return storage_.getDv(i_);}
+      walberla::mesa_pd::Vec3& getDvRef() {return storage_.getDvRef(i_);}
+      void setDv(const walberla::mesa_pd::Vec3& v) { storage_.setDv(i_, v);}
+      
+      const walberla::mesa_pd::Vec3& getDw() const {return storage_.getDw(i_);}
+      walberla::mesa_pd::Vec3& getDwRef() {return storage_.getDwRef(i_);}
+      void setDw(const walberla::mesa_pd::Vec3& v) { storage_.setDw(i_, v);}
+      
+      const std::unordered_set<walberla::mpi::MPIRank>& getNeighborState() const {return storage_.getNeighborState(i_);}
+      std::unordered_set<walberla::mpi::MPIRank>& getNeighborStateRef() {return storage_.getNeighborStateRef(i_);}
+      void setNeighborState(const std::unordered_set<walberla::mpi::MPIRank>& v) { storage_.setNeighborState(i_, v);}
+      
 
       size_t getIdx() const {return i_;}
    public:
@@ -232,9 +246,9 @@ public:
    int& getOwnerRef(const size_t idx) {return owner_[idx];}
    void setOwner(const size_t idx, const int& v) { owner_[idx] = v; }
    
-   const std::vector<int>& getGhostOwners(const size_t idx) const {return ghostOwners_[idx];}
-   std::vector<int>& getGhostOwnersRef(const size_t idx) {return ghostOwners_[idx];}
-   void setGhostOwners(const size_t idx, const std::vector<int>& v) { ghostOwners_[idx] = v; }
+   const std::unordered_set<walberla::mpi::MPIRank>& getGhostOwners(const size_t idx) const {return ghostOwners_[idx];}
+   std::unordered_set<walberla::mpi::MPIRank>& getGhostOwnersRef(const size_t idx) {return ghostOwners_[idx];}
+   void setGhostOwners(const size_t idx, const std::unordered_set<walberla::mpi::MPIRank>& v) { ghostOwners_[idx] = v; }
    
    const size_t& getShapeID(const size_t idx) const {return shapeID_[idx];}
    size_t& getShapeIDRef(const size_t idx) {return shapeID_[idx];}
@@ -296,6 +310,18 @@ public:
    walberla::real_t& getHeatFluxRef(const size_t idx) {return heatFlux_[idx];}
    void setHeatFlux(const size_t idx, const walberla::real_t& v) { heatFlux_[idx] = v; }
    
+   const walberla::mesa_pd::Vec3& getDv(const size_t idx) const {return dv_[idx];}
+   walberla::mesa_pd::Vec3& getDvRef(const size_t idx) {return dv_[idx];}
+   void setDv(const size_t idx, const walberla::mesa_pd::Vec3& v) { dv_[idx] = v; }
+   
+   const walberla::mesa_pd::Vec3& getDw(const size_t idx) const {return dw_[idx];}
+   walberla::mesa_pd::Vec3& getDwRef(const size_t idx) {return dw_[idx];}
+   void setDw(const size_t idx, const walberla::mesa_pd::Vec3& v) { dw_[idx] = v; }
+   
+   const std::unordered_set<walberla::mpi::MPIRank>& getNeighborState(const size_t idx) const {return neighborState_[idx];}
+   std::unordered_set<walberla::mpi::MPIRank>& getNeighborStateRef(const size_t idx) {return neighborState_[idx];}
+   void setNeighborState(const size_t idx, const std::unordered_set<walberla::mpi::MPIRank>& v) { neighborState_[idx] = v; }
+   
 
    /**
     * @brief creates a new particle and returns an iterator pointing to it
@@ -390,7 +416,7 @@ public:
    std::vector<walberla::real_t> interactionRadius_ {};
    std::vector<walberla::mesa_pd::data::particle_flags::FlagT> flags_ {};
    std::vector<int> owner_ {};
-   std::vector<std::vector<int>> ghostOwners_ {};
+   std::vector<std::unordered_set<walberla::mpi::MPIRank>> ghostOwners_ {};
    std::vector<size_t> shapeID_ {};
    std::vector<walberla::mesa_pd::Rot3> rotation_ {};
    std::vector<walberla::mesa_pd::Vec3> angularVelocity_ {};
@@ -406,6 +432,9 @@ public:
    std::vector<std::map<walberla::id_t, walberla::mesa_pd::data::ContactHistory>> newContactHistory_ {};
    std::vector<walberla::real_t> temperature_ {};
    std::vector<walberla::real_t> heatFlux_ {};
+   std::vector<walberla::mesa_pd::Vec3> dv_ {};
+   std::vector<walberla::mesa_pd::Vec3> dw_ {};
+   std::vector<std::unordered_set<walberla::mpi::MPIRank>> neighborState_ {};
    std::unordered_map<id_t, size_t> uidToIdx_;
    static_assert(std::is_same<decltype(uid_)::value_type, id_t>::value,
                  "Property uid of type id_t is missing. This property is required!");
@@ -436,6 +465,9 @@ ParticleStorage::Particle& ParticleStorage::Particle::operator=(const ParticleSt
    getNewContactHistoryRef() = rhs.getNewContactHistory();
    getTemperatureRef() = rhs.getTemperature();
    getHeatFluxRef() = rhs.getHeatFlux();
+   getDvRef() = rhs.getDv();
+   getDwRef() = rhs.getDw();
+   getNeighborStateRef() = rhs.getNeighborState();
    return *this;
 }
 
@@ -463,6 +495,9 @@ ParticleStorage::Particle& ParticleStorage::Particle::operator=(ParticleStorage:
    getNewContactHistoryRef() = std::move(rhs.getNewContactHistoryRef());
    getTemperatureRef() = std::move(rhs.getTemperatureRef());
    getHeatFluxRef() = std::move(rhs.getHeatFluxRef());
+   getDvRef() = std::move(rhs.getDvRef());
+   getDwRef() = std::move(rhs.getDwRef());
+   getNeighborStateRef() = std::move(rhs.getNeighborStateRef());
    return *this;
 }
 
@@ -492,6 +527,9 @@ std::ostream& operator<<( std::ostream& os, const ParticleStorage::Particle& p )
          "newContactHistory   : " << p.getNewContactHistory() << "\n" <<
          "temperature         : " << p.getTemperature() << "\n" <<
          "heatFlux            : " << p.getHeatFlux() << "\n" <<
+         "dv                  : " << p.getDv() << "\n" <<
+         "dw                  : " << p.getDw() << "\n" <<
+         "neighborState       : " << p.getNeighborState() << "\n" <<
          "================================" << std::endl;
    return os;
 }
@@ -590,6 +628,9 @@ inline ParticleStorage::iterator ParticleStorage::create(const id_t& uid)
    newContactHistory_.emplace_back();
    temperature_.emplace_back(real_t(0));
    heatFlux_.emplace_back(real_t(0));
+   dv_.emplace_back(real_t(0));
+   dw_.emplace_back(real_t(0));
+   neighborState_.emplace_back();
    uid_.back() = uid;
    uidToIdx_[uid] = uid_.size() - 1;
    return iterator(this, size() - 1);
@@ -643,6 +684,9 @@ inline ParticleStorage::iterator ParticleStorage::erase(iterator& it)
    newContactHistory_.pop_back();
    temperature_.pop_back();
    heatFlux_.pop_back();
+   dv_.pop_back();
+   dw_.pop_back();
+   neighborState_.pop_back();
    return it;
 }
 
@@ -683,6 +727,9 @@ inline void ParticleStorage::reserve(const size_t size)
    newContactHistory_.reserve(size);
    temperature_.reserve(size);
    heatFlux_.reserve(size);
+   dv_.reserve(size);
+   dw_.reserve(size);
+   neighborState_.reserve(size);
 }
 
 inline void ParticleStorage::clear()
@@ -708,6 +755,9 @@ inline void ParticleStorage::clear()
    newContactHistory_.clear();
    temperature_.clear();
    heatFlux_.clear();
+   dv_.clear();
+   dw_.clear();
+   neighborState_.clear();
    uidToIdx_.clear();
 }
 
@@ -734,6 +784,9 @@ inline size_t ParticleStorage::size() const
    //WALBERLA_ASSERT_EQUAL( uid_.size(), newContactHistory.size() );
    //WALBERLA_ASSERT_EQUAL( uid_.size(), temperature.size() );
    //WALBERLA_ASSERT_EQUAL( uid_.size(), heatFlux.size() );
+   //WALBERLA_ASSERT_EQUAL( uid_.size(), dv.size() );
+   //WALBERLA_ASSERT_EQUAL( uid_.size(), dw.size() );
+   //WALBERLA_ASSERT_EQUAL( uid_.size(), neighborState.size() );
    return uid_.size();
 }
 template <typename Selector, typename Accessor, typename Func, typename... Args>
@@ -925,10 +978,10 @@ public:
 class SelectParticleGhostOwners
 {
 public:
-   using return_type = std::vector<int>;
-   std::vector<int>& operator()(data::Particle& p) const {return p.getGhostOwnersRef();}
-   std::vector<int>& operator()(data::Particle&& p) const {return p.getGhostOwnersRef();}
-   const std::vector<int>& operator()(const data::Particle& p) const {return p.getGhostOwners();}
+   using return_type = std::unordered_set<walberla::mpi::MPIRank>;
+   std::unordered_set<walberla::mpi::MPIRank>& operator()(data::Particle& p) const {return p.getGhostOwnersRef();}
+   std::unordered_set<walberla::mpi::MPIRank>& operator()(data::Particle&& p) const {return p.getGhostOwnersRef();}
+   const std::unordered_set<walberla::mpi::MPIRank>& operator()(const data::Particle& p) const {return p.getGhostOwners();}
 };
 ///Predicate that selects a certain property from a Particle
 class SelectParticleShapeID
@@ -1065,6 +1118,33 @@ public:
    walberla::real_t& operator()(data::Particle&& p) const {return p.getHeatFluxRef();}
    const walberla::real_t& operator()(const data::Particle& p) const {return p.getHeatFlux();}
 };
+///Predicate that selects a certain property from a Particle
+class SelectParticleDv
+{
+public:
+   using return_type = walberla::mesa_pd::Vec3;
+   walberla::mesa_pd::Vec3& operator()(data::Particle& p) const {return p.getDvRef();}
+   walberla::mesa_pd::Vec3& operator()(data::Particle&& p) const {return p.getDvRef();}
+   const walberla::mesa_pd::Vec3& operator()(const data::Particle& p) const {return p.getDv();}
+};
+///Predicate that selects a certain property from a Particle
+class SelectParticleDw
+{
+public:
+   using return_type = walberla::mesa_pd::Vec3;
+   walberla::mesa_pd::Vec3& operator()(data::Particle& p) const {return p.getDwRef();}
+   walberla::mesa_pd::Vec3& operator()(data::Particle&& p) const {return p.getDwRef();}
+   const walberla::mesa_pd::Vec3& operator()(const data::Particle& p) const {return p.getDw();}
+};
+///Predicate that selects a certain property from a Particle
+class SelectParticleNeighborState
+{
+public:
+   using return_type = std::unordered_set<walberla::mpi::MPIRank>;
+   std::unordered_set<walberla::mpi::MPIRank>& operator()(data::Particle& p) const {return p.getNeighborStateRef();}
+   std::unordered_set<walberla::mpi::MPIRank>& operator()(data::Particle&& p) const {return p.getNeighborStateRef();}
+   const std::unordered_set<walberla::mpi::MPIRank>& operator()(const data::Particle& p) const {return p.getNeighborState();}
+};
 
 } //namespace data
 } //namespace mesa_pd
diff --git a/src/mesa_pd/data/STLOverloads.h b/src/mesa_pd/data/STLOverloads.h
index 28cf24885834109da472672f9e9c409f52682c4f..7a929e54c79fdc9af2adaae9a9aabeb8477cd8c1 100644
--- a/src/mesa_pd/data/STLOverloads.h
+++ b/src/mesa_pd/data/STLOverloads.h
@@ -23,6 +23,9 @@
 #include <mesa_pd/data/DataTypes.h>
 
 #include <map>
+#include <set>
+#include <unordered_set>
+#include <unordered_map>
 #include <vector>
 
 namespace walberla {
@@ -55,6 +58,42 @@ std::ostream& operator<<( std::ostream& os, const std::map<Key, T, Compare, Allo
    return os;
 }
 
+template< typename Key, typename Compare, typename Allocator >
+std::ostream& operator<<( std::ostream& os, const std::set<Key, Compare, Allocator>& m )
+{
+   os << "{";
+   for (auto& v : m)
+   {
+      os << v << ", ";
+   }
+   os << "}";
+   return os;
+}
+
+template< typename Key, typename T, typename Compare, typename Allocator >
+std::ostream& operator<<( std::ostream& os, const std::unordered_map<Key, T, Compare, Allocator>& m )
+{
+   os << "{";
+   for (auto& v : m)
+   {
+      os << v.first << ":" << v.second << ", ";
+   }
+   os << "}";
+   return os;
+}
+
+template< typename Key, typename Compare, typename Allocator >
+std::ostream& operator<<( std::ostream& os, const std::unordered_set<Key, Compare, Allocator>& m )
+{
+   os << "{";
+   for (auto& v : m)
+   {
+      os << v << ", ";
+   }
+   os << "}";
+   return os;
+}
+
 } //namespace data
 } //namespace mesa_pd
 } //namespace walberla
diff --git a/src/mesa_pd/data/shape/BaseShape.h b/src/mesa_pd/data/shape/BaseShape.h
index 6e243477bb6c1446a197c286f3678c5d95e33de3..24936a75b6074d15b909ee5982c155801678b58b 100644
--- a/src/mesa_pd/data/shape/BaseShape.h
+++ b/src/mesa_pd/data/shape/BaseShape.h
@@ -43,17 +43,19 @@ public:
 
    virtual real_t getVolume() const = 0;
 
-   real_t& getInvMass() {return invMass_;}
+   const real_t& getMass() const {return mass_;}
    const real_t& getInvMass() const {return invMass_;}
 
-   Mat3& getInvInertiaBF() {return invInertiaBF_;}
+   const Mat3& getInertiaBF() const {return inertiaBF_;}
    const Mat3& getInvInertiaBF() const {return invInertiaBF_;}
 
    const int& getShapeType() const {return shapeType_;}
 
    static const int INVALID_SHAPE = -1; ///< Unique *invalid* shape type identifier.\ingroup mesa_pd_shape
-private:
+protected:
+   real_t mass_         = real_t(0);       ///< mass
    real_t invMass_      = real_t(0);       ///< inverse mass
+   Mat3   inertiaBF_    = Mat3(real_t(0)); ///< inertia matrix in the body frame
    Mat3   invInertiaBF_ = Mat3(real_t(0)); ///< inverse inertia matrix in the body frame
    int    shapeType_    = INVALID_SHAPE;   ///< \ingroup mesa_pd_shape
 };
diff --git a/src/mesa_pd/data/shape/HalfSpace.h b/src/mesa_pd/data/shape/HalfSpace.h
index 734d6b4c69b8155c3cb0cdddf942d85e198d5605..9088900df7ac9302f97d2b21f20b9dbc9a0fac37 100644
--- a/src/mesa_pd/data/shape/HalfSpace.h
+++ b/src/mesa_pd/data/shape/HalfSpace.h
@@ -56,8 +56,11 @@ private:
 inline
 void HalfSpace::updateMassAndInertia(const real_t /*density*/)
 {
-   getInvMass()      = real_t(0);
-   getInvInertiaBF() = Mat3(real_t(0));
+   mass_         = std::numeric_limits<real_t>::infinity();
+   invMass_      = real_t(0);
+
+   inertiaBF_    = Mat3(std::numeric_limits<real_t>::infinity());
+   invInertiaBF_ = Mat3(real_t(0));
 }
 
 } //namespace data
diff --git a/src/mesa_pd/data/shape/Sphere.h b/src/mesa_pd/data/shape/Sphere.h
index 7244eb3edf9c48d5ac3da806464d0a5666f815f4..50106bbbc7ecc5918b574a921152ce326be98df7 100644
--- a/src/mesa_pd/data/shape/Sphere.h
+++ b/src/mesa_pd/data/shape/Sphere.h
@@ -51,8 +51,11 @@ void Sphere::updateMassAndInertia(const real_t density)
    const real_t m = (real_c(4.0)/real_c(3.0) * math::M_PI) * getRadius() * getRadius() * getRadius() * density;
    const Mat3   I = Mat3::makeDiagonalMatrix( real_c(0.4) * m * getRadius() * getRadius() );
 
-   getInvMass()      = real_t(1.0) / m;
-   getInvInertiaBF() = I.getInverse();
+   mass_         = m;
+   invMass_      = real_t(1.0) / m;
+
+   inertiaBF_    = I;
+   invInertiaBF_ = I.getInverse();
 }
 
 } //namespace data
diff --git a/src/mesa_pd/domain/BlockForestDomain.cpp b/src/mesa_pd/domain/BlockForestDomain.cpp
index 483eaa9b8db983289a37e5da1b5713c01e845c9f..4339671d40ed67464635cd3957c2bd028970b6c7 100644
--- a/src/mesa_pd/domain/BlockForestDomain.cpp
+++ b/src/mesa_pd/domain/BlockForestDomain.cpp
@@ -175,43 +175,63 @@ bool BlockForestDomain::intersectsWithProcessSubdomain(const uint_t rank, const
 {
    if (blockForest_->empty()) return false;
 
-   WALBERLA_ASSERT(std::is_sorted(neighborSubdomains_.begin(),
-                                  neighborSubdomains_.end(),
-                                  [](const auto& lhs, const auto& rhs){ return lhs.rank < rhs.rank;}));
-
-   WALBERLA_CHECK_UNEQUAL(uint_c(ownRank_), rank, "checking own domain is currently not implemented");
-
-   if (isInsideGlobalDomain(pt, radius))
+   if (uint_c(ownRank_) == rank)
    {
-      size_t idx = 0;
-      WALBERLA_ASSERT_LESS(idx, neighborSubdomains_.size());
-      while (neighborSubdomains_[idx].rank != int_c(rank))
+      //=====================
+      // LOCAL DOMAIN
+      if (isInsideGlobalDomain(pt, radius))
       {
-         ++idx;
-         WALBERLA_ASSERT_LESS(idx, neighborSubdomains_.size());
-      }
-      while (neighborSubdomains_[idx].rank == int_c(rank))
+         for (auto& aabb : localAABBs_)
+         {
+            if (sqDistancePointToAABB(pt, aabb) <= radius * radius) return true;
+         }
+      } else
       {
-         if (sqDistancePointToAABB(pt, neighborSubdomains_[idx].aabb) <= radius * radius) return true;
-         ++idx;
-         if (idx >= neighborSubdomains_.size()) break;
-         WALBERLA_ASSERT_LESS(idx, neighborSubdomains_.size());
+         for (auto& aabb : localAABBs_)
+         {
+            if (sqDistancePointToAABBPeriodic(pt, aabb, blockForest_->getDomain(), periodic_) <= radius * radius) return true;
+         }
       }
    } else
    {
-      size_t idx = 0;
-      WALBERLA_ASSERT_LESS(idx, neighborSubdomains_.size());
-      while (neighborSubdomains_[idx].rank != int_c(rank))
+      //=====================
+      // NEIGHBORING DOMAIN
+      WALBERLA_ASSERT(std::is_sorted(neighborSubdomains_.begin(),
+                                     neighborSubdomains_.end(),
+                                     [](const auto& lhs, const auto& rhs){ return lhs.rank < rhs.rank;}));
+
+      if (isInsideGlobalDomain(pt, radius))
       {
-         ++idx;
+         size_t idx = 0;
          WALBERLA_ASSERT_LESS(idx, neighborSubdomains_.size());
-      }
-      while (neighborSubdomains_[idx].rank == int_c(rank))
+         while (neighborSubdomains_[idx].rank != int_c(rank))
+         {
+            ++idx;
+            WALBERLA_ASSERT_LESS(idx, neighborSubdomains_.size());
+         }
+         while (neighborSubdomains_[idx].rank == int_c(rank))
+         {
+            if (sqDistancePointToAABB(pt, neighborSubdomains_[idx].aabb) <= radius * radius) return true;
+            ++idx;
+            if (idx >= neighborSubdomains_.size()) break;
+            WALBERLA_ASSERT_LESS(idx, neighborSubdomains_.size());
+         }
+      } else
       {
-         if (sqDistancePointToAABBPeriodic(pt, neighborSubdomains_[idx].aabb, blockForest_->getDomain(), periodic_) <= radius * radius) return true;
-         ++idx;
-         if (idx >= neighborSubdomains_.size()) break;
+         size_t idx = 0;
          WALBERLA_ASSERT_LESS(idx, neighborSubdomains_.size());
+         while (neighborSubdomains_[idx].rank != int_c(rank))
+         {
+            ++idx;
+            WALBERLA_ASSERT_LESS(idx, neighborSubdomains_.size());
+         }
+         while (neighborSubdomains_[idx].rank == int_c(rank))
+         {
+            if (sqDistancePointToAABBPeriodic(pt, neighborSubdomains_[idx].aabb, blockForest_->getDomain(), periodic_) <= radius * radius) return true;
+            ++idx;
+            if (idx >= neighborSubdomains_.size()) break;
+            WALBERLA_ASSERT_LESS(idx, neighborSubdomains_.size());
+         }
       }
    }
 
diff --git a/src/mesa_pd/domain/InfiniteDomain.h b/src/mesa_pd/domain/InfiniteDomain.h
index 27a1c2ea794787261ac117a6d679fb8b6a79443a..3e98e8246f71b48a99d4c5db8fb6ac0565ccc711 100644
--- a/src/mesa_pd/domain/InfiniteDomain.h
+++ b/src/mesa_pd/domain/InfiniteDomain.h
@@ -30,11 +30,11 @@ class InfiniteDomain : public IDomain
 {
 public:
    bool   isContainedInProcessSubdomain(const uint_t /*rank*/, const Vec3& /*pt*/) const override {return true;}
-   int    findContainingProcessRank(const Vec3& /*pt*/) const override {return mpi::MPIManager::instance()->rank();}
+   int    findContainingProcessRank(const Vec3& /*pt*/) const override {return walberla::mpi::MPIManager::instance()->rank();}
    void   periodicallyMapToDomain(Vec3& /*pt*/) const override {}
    std::vector<uint_t> getNeighborProcesses() const override {return {};}
    bool   intersectsWithProcessSubdomain(const uint_t rank, const Vec3& /*pt*/, const real_t& /*radius*/) const override
-   { return int_c(rank)==mpi::MPIManager::instance()->rank() ? true : false;}
+   { return int_c(rank)==walberla::mpi::MPIManager::instance()->rank() ? true : false;}
    void   correctParticlePosition(Vec3& /*pt*/) const override {}
 };
 
diff --git a/src/mesa_pd/kernel/DetectAndStoreContacts.h b/src/mesa_pd/kernel/DetectAndStoreContacts.h
new file mode 100644
index 0000000000000000000000000000000000000000..5a7900e43ae41c1c7799b55c48a168f2dbeb42b9
--- /dev/null
+++ b/src/mesa_pd/kernel/DetectAndStoreContacts.h
@@ -0,0 +1,94 @@
+//======================================================================================================================
+//
+//  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 DetectAndStoreContacts.h
+//! \author Sebastian Eibl <sebastian.eibl@fau.de>
+//! \author Tobias Leemann <tobias.leemann@fau.de>
+//
+//======================================================================================================================
+
+//======================================================================================================================
+//
+//  THIS FILE IS GENERATED - PLEASE CHANGE THE TEMPLATE !!!
+//
+//======================================================================================================================
+
+#pragma once
+
+#include <mesa_pd/collision_detection/AnalyticContactDetection.h>
+#include <mesa_pd/mpi/ContactFilter.h>
+
+#include <mesa_pd/data/DataTypes.h>
+#include <mesa_pd/data/ContactStorage.h>
+
+#include <mesa_pd/kernel/DoubleCast.h>
+#include <mesa_pd/data/Flags.h>
+
+namespace walberla {
+namespace mesa_pd {
+namespace kernel {
+
+/**
+ * Kernel which performes collision detection on a pair of two particles
+ * and inserts the contact (if existent) into the contact storage passed in the constructor.
+ * Call this kernel on each particle pair to perform contact detection and insert each contact in the contact
+ * storage.
+ * \ingroup mesa_pd_kernel
+ */
+class DetectAndStoreContacts
+{
+
+   public:
+   explicit DetectAndStoreContacts(data::ContactStorage &cs) : cs_(cs) {}
+
+   /**
+    * Compute if particle 1 and particle 2 collide. If so, insert the contact into the contact storage.
+    * \param idx1 The index of particle 1
+    * \param idx2 The index of particle 2
+    * \param ac The accessor used to access the values of the particles.
+    * \param domain The domain of the block (used to decide if the contact has to be treated by this process)
+    * \param acd The collision detection to be used. Default parameter: AnalyticContactDetection()
+    */
+   template <typename Accessor>
+   void operator()(size_t idx1, size_t idx2, Accessor &ac, const domain::IDomain& domain, collision_detection::AnalyticContactDetection acd = collision_detection::AnalyticContactDetection());
+   private:
+   data::ContactStorage& cs_;
+};
+
+
+
+template <typename Accessor>
+inline void DetectAndStoreContacts::operator()(size_t idx1, size_t idx2, Accessor &ac, const domain::IDomain& domain, collision_detection::AnalyticContactDetection acd)
+{
+   using namespace data::particle_flags;
+   kernel::DoubleCast double_cast;
+   mpi::ContactFilter contact_filter;
+   if (double_cast(idx1, idx2, ac, acd, ac ))
+   {
+      if (contact_filter(acd.getIdx1(), acd.getIdx2(), ac, acd.getContactPoint(), domain))
+      {
+         auto c = cs_.create();
+         c->setId1(acd.getIdx1());
+         c->setId2(acd.getIdx2());
+         c->setDistance(acd.getPenetrationDepth());
+         c->setNormal(acd.getContactNormal());
+         c->setPosition(acd.getContactPoint());
+      }
+   }
+}
+
+} //namespace kernel
+} //namespace mesa_pd
+} //namespace walberla
\ No newline at end of file
diff --git a/src/mesa_pd/mpi/RemoveAndNotify.cpp b/src/mesa_pd/mpi/RemoveAndNotify.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..c04a40f949d3149dfaa51f16ebed7d1335fa1c7e
--- /dev/null
+++ b/src/mesa_pd/mpi/RemoveAndNotify.cpp
@@ -0,0 +1,65 @@
+//======================================================================================================================
+//
+//  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 RemoveAndNotify.cpp
+//! \author Sebastian Eibl <sebastian.eibl@fau.de>
+//
+//======================================================================================================================
+
+#include "RemoveAndNotify.h"
+
+#include <mesa_pd/data/DataTypes.h>
+#include <mesa_pd/data/Flags.h>
+#include <mesa_pd/mpi/notifications/PackNotification.h>
+#include <mesa_pd/mpi/notifications/ParticleRemovalNotification.h>
+
+namespace walberla {
+namespace mesa_pd {
+namespace mpi {
+
+/**
+ * Removes a particle from the local storage and informs ghost particle holders.
+ *
+ * This function removes the particle from the particle storage and generates deletion notifications.
+ */
+data::ParticleStorage::iterator removeAndNotify( walberla::mpi::BufferSystem& bs,
+                                                 data::ParticleStorage& ps,
+                                                 data::ParticleStorage::iterator& pIt )
+{
+   WALBERLA_ASSERT( !data::particle_flags::isSet( pIt->getFlags(), data::particle_flags::GHOST),
+                    "Trying to remove ghost particle from the particle storage." );
+
+   WALBERLA_ASSERT( !data::particle_flags::isSet( pIt->getFlags(), data::particle_flags::GLOBAL),
+                    "Trying to remove a global particle from the particle storage." );
+
+   if( !pIt->getGhostOwners().empty() )
+   {
+      // Notify registered processes (intersecting or interacting) of particle removal since they possess a shadow copy.
+      for( auto ghostRank : pIt->getGhostOwnersRef() )
+      {
+         WALBERLA_LOG_DETAIL( "__Notify registered process " << ghostRank << " of deletion of particle " << pIt->getUid() );
+         auto& sb = bs.sendBuffer(static_cast<walberla::mpi::MPIRank>(ghostRank));
+         if (sb.isEmpty()) sb << walberla::uint8_c(0);
+         packNotification(sb, ParticleRemovalNotification( *pIt ));
+      }
+   }
+
+   pIt->getGhostOwnersRef().clear();
+   return ps.erase( pIt );
+}
+
+}  // namespace mpi
+}  // namespace mesa_pd
+}  // namespace walberla
diff --git a/src/mesa_pd/mpi/RemoveAndNotify.h b/src/mesa_pd/mpi/RemoveAndNotify.h
new file mode 100644
index 0000000000000000000000000000000000000000..90cf8d3f1d24ce6e165ca06bfa48d1767bebbce3
--- /dev/null
+++ b/src/mesa_pd/mpi/RemoveAndNotify.h
@@ -0,0 +1,36 @@
+//======================================================================================================================
+//
+//  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 RemoveAndNotify.h
+//! \author Sebastian Eibl <sebastian.eibl@fau.de>
+//
+//======================================================================================================================
+
+#pragma once
+
+#include <mesa_pd/data/ParticleStorage.h>
+#include <core/mpi/BufferSystem.h>
+
+namespace walberla {
+namespace mesa_pd {
+namespace mpi {
+
+data::ParticleStorage::iterator removeAndNotify( walberla::mpi::BufferSystem& bs,
+                                                 data::ParticleStorage& ps,
+                                                 data::ParticleStorage::iterator& pIt );
+
+}  // namespace mpi
+}  // namespace mesa_pd
+}  // namespace walberla
diff --git a/src/mesa_pd/mpi/SyncGhostOwners.cpp b/src/mesa_pd/mpi/SyncGhostOwners.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..bedc11c5f099fec92c9952f8bda1a557c97b4db6
--- /dev/null
+++ b/src/mesa_pd/mpi/SyncGhostOwners.cpp
@@ -0,0 +1,355 @@
+//======================================================================================================================
+//
+//  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 SyncGhostOwners.cpp
+//! \author Sebastian Eibl <sebastian.eibl@fau.de>
+//
+//======================================================================================================================
+
+//======================================================================================================================
+//
+//  THIS FILE IS GENERATED - PLEASE CHANGE THE TEMPLATE !!!
+//
+//======================================================================================================================
+
+#include "SyncGhostOwners.h"
+
+#include <mesa_pd/mpi/RemoveAndNotify.h>
+
+namespace walberla {
+namespace mesa_pd {
+namespace mpi {
+
+void SyncGhostOwners::operator()( data::ParticleStorage& ps,
+                                  const domain::IDomain& domain,
+                                  const real_t dx,
+                                  const bool syncNonCommunicatingBodies ) const
+{
+   if (numProcesses_ == 1) return;
+
+   //==========================================================
+   // STEP1: Update & Migrate
+   //==========================================================
+   updateAndMigrate( ps, domain, syncNonCommunicatingBodies );
+
+   //==========================================================
+   // STEP2: Check & Resolve
+   //==========================================================
+   checkAndResolveOverlap( ps, domain, dx, syncNonCommunicatingBodies );
+}
+
+void SyncGhostOwners::updateAndMigrate( data::ParticleStorage& ps,
+                                        const domain::IDomain& domain,
+                                        const bool syncNonCommunicatingBodies ) const
+{
+   using namespace walberla::mesa_pd::data::particle_flags;
+   //==========================================================
+   // STEP1: Update & Migrate
+   //==========================================================
+
+   WALBERLA_CHECK(!bs1.isCommunicationRunning());
+
+   WALBERLA_LOG_DETAIL( "Assembling of Update&Migrate starts..." );
+   std::set<walberla::mpi::MPIRank> recvRanks; // potential message senders
+   for( auto pIt = ps.begin(); pIt != ps.end(); ++pIt)
+   {
+      if (isSet( pIt->getFlags(), GHOST))
+      {
+         if (!isSet( pIt->getFlags(), NON_COMMUNICATING) || syncNonCommunicatingBodies)
+         {
+            recvRanks.insert(pIt->getOwner());
+         }
+      }
+   }
+
+   for( auto pIt = ps.begin(); pIt != ps.end(); )
+   {
+      if (isSet( pIt->getFlags(), GHOST))
+      {
+         ++pIt;
+         continue;
+      }
+
+      //==================
+      // LOCAL
+
+      //skip all particles that do not communicate (create ghost particles) on other processes
+      if (isSet( pIt->getFlags(), NON_COMMUNICATING) && !syncNonCommunicatingBodies)
+      {
+         ++pIt;
+         continue;
+      }
+
+      //correct position to make sure particle is always inside the domain!
+      //everything is decided by the master particle therefore ghost particles are not touched
+      if (!data::particle_flags::isSet( pIt->getFlags(), data::particle_flags::FIXED) &&
+          !data::particle_flags::isSet( pIt->getFlags(), data::particle_flags::GHOST))
+      {
+         domain.periodicallyMapToDomain( pIt->getPositionRef() );
+      }
+
+      // Update
+      for (auto ghostOwner : pIt->getGhostOwners())
+      {
+         WALBERLA_LOG_DETAIL( "Sending update notification for body " << pIt->getUid() << " to process " << ghostOwner );
+         walberla::mpi::SendBuffer& sb = bs1.sendBuffer(static_cast<walberla::mpi::MPIRank>(ghostOwner));
+         if (sb.isEmpty()) sb << walberla::uint8_c(0);
+         packNotification(sb, ParticleUpdateNotification( *pIt ));
+      }
+
+      //particle has left subdomain?
+      const auto newOwner = domain.findContainingProcessRank( pIt->getPosition() );
+      if( newOwner != int_c(rank_) )
+      {
+         if ( newOwner < 0)
+         {
+            // No owner found: Outflow condition.
+            WALBERLA_LOG_DETAIL( "Sending deletion notifications for body " << pIt->getUid() << " due to outflow." );
+
+            //delete body
+            pIt = removeAndNotify( bs1, ps, pIt );
+
+            continue;
+         }
+
+         // Set new owner and transform to shadow copy
+         pIt->setOwner( newOwner );
+         set( pIt->getFlagsRef(), GHOST );
+
+         // currently position is mapped to periodically to global domain,
+         // this might not be the correct position for a ghost particle
+         domain.correctParticlePosition( pIt->getPositionRef() );
+
+         // Correct registration list (exclude new owner and us - the old owner) and
+         // notify registered processes (except for new owner) of (remote) migration since they possess a ghost particle.
+         auto ownerIt = std::find( pIt->getGhostOwners().begin(), pIt->getGhostOwners().end(), newOwner );
+         WALBERLA_CHECK_UNEQUAL(ownerIt, pIt->getGhostOwners().end(), "New owner has to be former ghost owner!" );
+
+         pIt->getGhostOwnersRef().erase( ownerIt );
+
+         // Send remote migration notifications
+         for( auto ghostRank : pIt->getGhostOwners() )
+         {
+            auto& buffer( bs1.sendBuffer(static_cast<walberla::mpi::MPIRank>(ghostRank)) );
+            if (buffer.isEmpty()) buffer << walberla::uint8_c(0);
+
+            WALBERLA_LOG_DETAIL( "Sending remote migration notification for particle " <<
+                                 pIt->getUid() <<
+                                 " to process " <<
+                                 ghostRank );
+
+            packNotification(buffer, ParticleRemoteMigrationNotification( *pIt, newOwner ));
+         }
+
+         pIt->getGhostOwnersRef().insert( int_c(rank_) );
+
+         WALBERLA_LOG_DETAIL( "Sending migration notification for body " <<
+                              pIt->getUid() <<
+                              " to process " <<
+                              (newOwner) );
+
+         // Send migration notification to new owner
+         auto& sb( bs1.sendBuffer(newOwner) );
+         if (sb.isEmpty()) sb << walberla::uint8_c(0);
+         packNotification(sb, ParticleMigrationNotification( *pIt ));
+
+         pIt->getGhostOwnersRef().clear();
+
+         continue;
+      }
+      ++pIt;
+   }
+   WALBERLA_LOG_DETAIL( "Assembling of Update&Migrate ended." );
+
+   WALBERLA_LOG_DETAIL( "UM: number of recv " << recvRanks.size());
+   bs1.setReceiverInfo(recvRanks, true);
+   bs1.sendAll();
+   WALBERLA_LOG_DETAIL( "UM: number of sends " << bs1.getNumberOfSends());
+
+   // Receiving the updates for the remote rigid bodies from the connected processes
+   WALBERLA_LOG_DETAIL( "Parsing of Update&Migrate starts..." );
+   ParseMessage parseMessage;
+   for( auto it = bs1.begin(); it != bs1.end(); ++it )
+   {
+      walberla::uint8_t tmp;
+      it.buffer() >> tmp;
+      while( !it.buffer().isEmpty() )
+      {
+         parseMessage(it.rank(), it.buffer(), ps, domain);
+      }
+   }
+   WALBERLA_LOG_DETAIL( "Parsing of Update&Migrate ended." );
+}
+
+void SyncGhostOwners::checkAndResolveOverlap( data::ParticleStorage& ps,
+                                              const domain::IDomain& domain,
+                                              const real_t dx,
+                                              const bool syncNonCommunicatingBodies ) const
+{
+   using namespace walberla::mesa_pd::data::particle_flags;
+   //==========================================================
+   // STEP2: Check&Resolve
+   //==========================================================
+
+   WALBERLA_CHECK(!bs2.isCommunicationRunning());
+
+   //init buffers
+   neighborRanks_ = domain.getNeighborProcesses();
+   for( uint_t nbProcessRank : neighborRanks_ )
+   {
+      if (bs2.sendBuffer(nbProcessRank).isEmpty())
+      {
+         // fill empty buffers with a dummy byte to force transmission
+         bs2.sendBuffer(nbProcessRank) << walberla::uint8_c(0);
+      }
+   }
+   bs2.sendBuffer(int_c(rank_)) << walberla::uint8_c(0);
+
+   WALBERLA_LOG_DETAIL( "Assembling of Check&Resolve starts..." );
+
+   for( auto pIt = ps.begin(); pIt != ps.end(); )
+   {
+      //skip all particles that do not communicate (create ghost particles) on other processes
+      if (isSet( pIt->getFlags(), NON_COMMUNICATING) && !syncNonCommunicatingBodies)
+      {
+          ++pIt;
+          continue;
+      }
+
+      if (!isSet( pIt->getFlags(), GHOST))
+      {
+         //LOCAL
+
+         walberla::mpi::SendBuffer& sbMaster = bs2.sendBuffer(pIt->getOwner());
+         if (sbMaster.isEmpty()) sbMaster << walberla::uint8_c(0);
+
+         // Update (nearest) neighbor processes.
+         for( uint_t nbProcessRank : neighborRanks_ )
+         {
+            auto& sb = bs2.sendBuffer(nbProcessRank);
+            if (sb.isEmpty()) sb << walberla::uint8_c(0);
+
+            // dont send to owner!!
+            if (pIt->getOwner() == int_c(nbProcessRank)) continue;
+            // only send to neighbor which do not know this body
+            if (pIt->getNeighborState().find( int_c(nbProcessRank) ) != pIt->getNeighborState().end()) continue;
+
+            if( domain.intersectsWithProcessSubdomain( nbProcessRank, pIt->getPosition(), pIt->getInteractionRadius() + dx ) )
+            {
+               // no ghost there -> create ghost
+               WALBERLA_LOG_DETAIL( "Sending copy notification for body " << pIt->getUid() << " to process " << (nbProcessRank) << "\n master: " << pIt->getOwner());
+               packNotification(sb, ParticleCopyNotification( *pIt ));
+               packNotification(sbMaster, NewGhostParticleNotification( *pIt, int_c(nbProcessRank) ));
+               pIt->getNeighborStateRef().insert( int_c(nbProcessRank) );
+            }
+         }
+      } else
+      {
+         //GHOST
+
+         walberla::mpi::SendBuffer& sbMaster = bs2.sendBuffer(pIt->getOwner());
+         if (sbMaster.isEmpty()) sbMaster << walberla::uint8_c(0);
+
+         // Update (nearest) neighbor processes.
+         for( uint_t nbProcessRank : neighborRanks_ )
+         {
+            auto& sb = bs2.sendBuffer(nbProcessRank);
+            if (sb.isEmpty()) sb << walberla::uint8_c(0);
+
+            if (pIt->getOwner() == int_c(nbProcessRank)) continue; // dont send to owner!!
+            if (pIt->getNeighborState().find( int_c(nbProcessRank) ) != pIt->getNeighborState().end()) continue; // only send to neighbor which do not know this body
+
+            if( domain.intersectsWithProcessSubdomain( nbProcessRank, pIt->getPosition(), pIt->getInteractionRadius() + dx ) )
+            {
+               // no ghost there -> create ghost
+               WALBERLA_LOG_DETAIL( "Sending copy notification for body " << pIt->getUid() << " to process " << (nbProcessRank) << "\n master: " << pIt->getOwner());
+               packNotification(sb, ParticleCopyNotification( *pIt ));
+               packNotification(sbMaster, NewGhostParticleNotification( *pIt, int_c(nbProcessRank) ));
+               pIt->getNeighborStateRef().insert( int_c(nbProcessRank) );
+            }
+         }
+
+         if ( !domain.intersectsWithProcessSubdomain(uint_c(rank_), pIt->getPosition(), pIt->getInteractionRadius() + dx) )
+         {
+            // Delete
+            // inform nearest neighbor processes.
+            for( uint_t nbProcessRank : neighborRanks_ )
+            {
+               WALBERLA_LOG_DETAIL( "Sending removal information notification for body " << pIt->getUid() << " to process " << (nbProcessRank) );
+               auto& sb = bs2.sendBuffer(nbProcessRank);
+               if (sb.isEmpty()) sb << walberla::uint8_c(0);
+               packNotification(sb, ParticleRemovalInformationNotification( *pIt ));
+            }
+
+            //notify owner
+            WALBERLA_LOG_DETAIL( "Sending removal information notification for body " << pIt->getUid() << " to process " << (pIt->getOwner()) );
+            auto& sb = bs2.sendBuffer(pIt->getOwner());
+            if (sb.isEmpty()) sb << walberla::uint8_c(0);
+            packNotification(sb, ParticleRemovalInformationNotification( *pIt ));
+
+            pIt = ps.erase( pIt );
+            continue;
+         }
+      }
+      ++pIt;
+   }
+
+   std::set<walberla::mpi::MPIRank> recvRanks; // potential message senders
+   // schedule receives
+   for( auto pIt = ps.begin(); pIt != ps.end(); ++pIt)
+   {
+      if (isSet( pIt->getFlags(), GHOST)) continue;
+
+      //skip all particles that do not communicate (create ghost particles) on other processes
+      if (isSet( pIt->getFlags(), NON_COMMUNICATING) && !syncNonCommunicatingBodies) continue;
+
+      for( auto ghostRank : pIt->getGhostOwners() )
+      {
+         recvRanks.insert(ghostRank);
+      }
+   }
+
+   for( uint_t nbProcessRank : neighborRanks_ )
+   {
+      recvRanks.insert(int_c(nbProcessRank));
+   }
+
+   recvRanks.insert( int_c(rank_) );
+   WALBERLA_LOG_DETAIL( "Assembling of Check&Resolve ended." );
+
+   // size of buffer is unknown and changes with each send
+   WALBERLA_LOG_DETAIL( "CR: number of recv " << recvRanks.size());
+   bs2.setReceiverInfo(recvRanks, true);
+   bs2.sendAll();
+   WALBERLA_LOG_DETAIL( "CR: number of sends " << bs2.getNumberOfSends());
+
+   // Receiving the updates for the remote rigid bodies from the connected processes
+   WALBERLA_LOG_DETAIL( "Parsing of Check&Resolve starts..." );
+   ParseMessage parseMessage;
+   for( auto it = bs2.begin(); it != bs2.end(); ++it )
+   {
+      walberla::uint8_t tmp;
+      it.buffer() >> tmp;
+      while( !it.buffer().isEmpty() )
+      {
+         parseMessage(it.rank(), it.buffer(), ps, domain);
+      }
+   }
+   WALBERLA_LOG_DETAIL( "Parsing of Check&Resolve ended." );
+}
+
+}  // namespace mpi
+}  // namespace mesa_pd
+}  // namespace walberla
\ No newline at end of file
diff --git a/src/mesa_pd/mpi/SyncGhostOwners.h b/src/mesa_pd/mpi/SyncGhostOwners.h
new file mode 100644
index 0000000000000000000000000000000000000000..17bcdde54912bdaaeb0a60bf584f67d72590018a
--- /dev/null
+++ b/src/mesa_pd/mpi/SyncGhostOwners.h
@@ -0,0 +1,89 @@
+//======================================================================================================================
+//
+//  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 SyncGhostOwners.h
+//! \author Sebastian Eibl <sebastian.eibl@fau.de>
+//
+//======================================================================================================================
+
+//======================================================================================================================
+//
+//  THIS FILE IS GENERATED - PLEASE CHANGE THE TEMPLATE !!!
+//
+//======================================================================================================================
+
+#pragma once
+
+#include <mesa_pd/data/DataTypes.h>
+#include <mesa_pd/data/Flags.h>
+#include <mesa_pd/data/ParticleStorage.h>
+#include <mesa_pd/domain/IDomain.h>
+#include <mesa_pd/mpi/notifications/NewGhostParticleNotification.h>
+#include <mesa_pd/mpi/notifications/PackNotification.h>
+#include <mesa_pd/mpi/notifications/ParseMessage.h>
+#include <mesa_pd/mpi/notifications/ParticleCopyNotification.h>
+#include <mesa_pd/mpi/notifications/ParticleMigrationNotification.h>
+#include <mesa_pd/mpi/notifications/ParticleRemoteMigrationNotification.h>
+#include <mesa_pd/mpi/notifications/ParticleRemovalInformationNotification.h>
+#include <mesa_pd/mpi/notifications/ParticleRemovalNotification.h>
+#include <mesa_pd/mpi/notifications/ParticleUpdateNotification.h>
+
+#include <core/mpi/BufferSystem.h>
+#include <core/logging/Logging.h>
+
+namespace walberla {
+namespace mesa_pd {
+namespace mpi {
+
+/**
+ * Kernel which updates all ghost particles.
+ *
+ * \ingroup mesa_pd_mpi
+ */
+class SyncGhostOwners
+{
+public:
+   void operator()( data::ParticleStorage& ps,
+                    const domain::IDomain& domain,
+                    const real_t dx = real_t(0),
+                    const bool syncNonCommunicatingBodies = false ) const;
+
+   int64_t getBytesSent() const { return bs1.getBytesSent() + bs2.getBytesSent(); }
+   int64_t getBytesReceived() const { return bs1.getBytesReceived() + bs2.getBytesReceived(); }
+
+   int64_t getNumberOfSends() const { return bs1.getNumberOfSends() + bs2.getNumberOfSends(); }
+   int64_t getNumberOfReceives() const { return bs1.getNumberOfReceives() + bs2.getNumberOfReceives(); }
+private:
+   void updateAndMigrate( data::ParticleStorage& ps,
+                          const domain::IDomain& domain,
+                          const bool syncNonCommunicatingBodies ) const;
+
+   void checkAndResolveOverlap( data::ParticleStorage& ps,
+                                const domain::IDomain& domain,
+                                const real_t dx,
+                                const bool syncNonCommunicatingBodies ) const;
+
+   mutable std::vector<uint_t> neighborRanks_; ///cache for neighbor ranks -> will be updated in operator()
+
+   mutable walberla::mpi::BufferSystem bs1 = walberla::mpi::BufferSystem( walberla::mpi::MPIManager::instance()->comm(), 749861);
+   mutable walberla::mpi::BufferSystem bs2 = walberla::mpi::BufferSystem( walberla::mpi::MPIManager::instance()->comm(), 255367);
+
+   int numProcesses_ = walberla::mpi::MPIManager::instance()->numProcesses();
+   int rank_         = walberla::mpi::MPIManager::instance()->rank();
+};
+
+}  // namespace mpi
+}  // namespace mesa_pd
+}  // namespace walberla
\ No newline at end of file
diff --git a/src/mesa_pd/mpi/SyncNextNeighbors.cpp b/src/mesa_pd/mpi/SyncNextNeighbors.cpp
index 4d5ef12cd2747e6aa6e7c853b24598369f7ddc5f..b139495464a60095a75913a3bd046cd7301b7d53 100644
--- a/src/mesa_pd/mpi/SyncNextNeighbors.cpp
+++ b/src/mesa_pd/mpi/SyncNextNeighbors.cpp
@@ -26,6 +26,8 @@
 
 #include "SyncNextNeighbors.h"
 
+#include <mesa_pd/mpi/RemoveAndNotify.h>
+
 namespace walberla {
 namespace mesa_pd {
 namespace mpi {
@@ -66,38 +68,6 @@ void SyncNextNeighbors::operator()(data::ParticleStorage& ps,
    WALBERLA_LOG_DETAIL( "Parsing of particle synchronization response ended." );
 }
 
-/**
- * Removes a particle from the local storage and informs ghost particle holders.
- *
- * This function removes the particle from the particle storage and generates deletion notifications.
- */
-inline
-data::ParticleStorage::iterator removeAndNotify( walberla::mpi::BufferSystem& bs,
-                                                 data::ParticleStorage& ps,
-                                                 data::ParticleStorage::iterator& pIt )
-{
-   WALBERLA_ASSERT( !data::particle_flags::isSet( pIt->getFlags(), data::particle_flags::GHOST),
-                    "Trying to remove ghost particle from the particle storage." );
-
-   WALBERLA_ASSERT( !data::particle_flags::isSet( pIt->getFlags(), data::particle_flags::GLOBAL),
-                    "Trying to remove a global particle from the particle storage." );
-
-   if( !pIt->getGhostOwners().empty() )
-   {
-      // Notify registered processes (intersecting or interacting) of particle removal since they possess a shadow copy.
-      for( auto ghostRank : pIt->getGhostOwnersRef() )
-      {
-         WALBERLA_LOG_DETAIL( "__Notify registered process " << ghostRank << " of deletion of particle " << pIt->getUid() );
-         auto& sb = bs.sendBuffer(ghostRank);
-         if (sb.isEmpty()) sb << walberla::uint8_c(0);
-         packNotification(sb, ParticleRemovalNotification( *pIt ));
-      }
-   }
-
-   pIt->getGhostOwnersRef().clear();
-   return ps.erase( pIt );
-}
-
 void SyncNextNeighbors::generateSynchronizationMessages(data::ParticleStorage& ps,
                                                         const domain::IDomain& domain,
                                                         const real_t dx) const
@@ -130,7 +100,7 @@ void SyncNextNeighbors::generateSynchronizationMessages(data::ParticleStorage& p
 
          for (const auto& ghostOwner : pIt->getGhostOwners() )
          {
-            auto& buffer( bs.sendBuffer(ghostOwner) );
+            auto& buffer( bs.sendBuffer(static_cast<walberla::mpi::MPIRank>(ghostOwner)) );
 
             WALBERLA_LOG_DETAIL( "Sending removal notification for particle " << pIt->getUid() << " to process " << ghostOwner );
 
@@ -174,7 +144,7 @@ void SyncNextNeighbors::generateSynchronizationMessages(data::ParticleStorage& p
                auto& buffer( bs.sendBuffer(nbProcessRank) );
                WALBERLA_LOG_DETAIL( "Sending shadow copy notification for particle " << pIt->getUid() << " to process " << (nbProcessRank) );
                packNotification(buffer, ParticleCopyNotification( *pIt ));
-               pIt->getGhostOwnersRef().emplace_back( int_c(nbProcessRank) );
+               pIt->getGhostOwnersRef().insert( int_c(nbProcessRank) );
             }
          }
          else
@@ -232,7 +202,7 @@ void SyncNextNeighbors::generateSynchronizationMessages(data::ParticleStorage& p
 
          for( auto ghostRank : pIt->getGhostOwners() )
          {
-            auto& buffer( bs.sendBuffer(ghostRank) );
+            auto& buffer( bs.sendBuffer(static_cast<walberla::mpi::MPIRank>(ghostRank)) );
 
             WALBERLA_LOG_DETAIL( "Sending remote migration notification for particle " << pIt->getUid() <<
                                  " to process " << ghostRank );
@@ -240,7 +210,7 @@ void SyncNextNeighbors::generateSynchronizationMessages(data::ParticleStorage& p
             packNotification(buffer, ParticleRemoteMigrationNotification( *pIt, ownerRank ));
          }
 
-         pIt->getGhostOwnersRef().emplace_back( int_c(ownRank) );
+         pIt->getGhostOwnersRef().insert( int_c(ownRank) );
 
          // Send migration notification to new owner
          auto& buffer( bs.sendBuffer(ownerRank) );
diff --git a/src/mesa_pd/mpi/notifications/NewGhostParticleNotification.h b/src/mesa_pd/mpi/notifications/NewGhostParticleNotification.h
new file mode 100644
index 0000000000000000000000000000000000000000..3c929cee446dca9770755b6071d72142ae6de2c6
--- /dev/null
+++ b/src/mesa_pd/mpi/notifications/NewGhostParticleNotification.h
@@ -0,0 +1,100 @@
+//======================================================================================================================
+//
+//  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 NewGhostParticleNotification.h
+//! \author Sebastian Eibl <sebastian.eibl@fau.de>
+//
+//======================================================================================================================
+
+//======================================================================================================================
+//
+//  THIS FILE IS GENERATED - PLEASE CHANGE THE TEMPLATE !!!
+//
+//======================================================================================================================
+
+#pragma once
+
+#include <mesa_pd/data/DataTypes.h>
+#include <mesa_pd/data/ParticleStorage.h>
+#include <mesa_pd/mpi/notifications/NotificationType.h>
+
+#include <core/mpi/Datatype.h>
+#include <core/mpi/MPIWrapper.h>
+#include <core/mpi/RecvBuffer.h>
+#include <core/mpi/SendBuffer.h>
+
+namespace walberla {
+namespace mesa_pd {
+
+/**
+ * This notification is send to the owner of a particle
+ * to signal that a new ghost particle exists and the ghost particle list should be updated.
+ */
+class NewGhostParticleNotification
+{
+public:
+   struct Parameters
+   {
+      id_t    uid_;
+      walberla::mpi::MPIRank newOwner_;
+   };
+
+   inline explicit NewGhostParticleNotification( const data::Particle& particle, const walberla::mpi::MPIRank newOwner )
+      : particle_(particle)
+      , newOwner_(newOwner)
+   {}
+   const data::Particle& particle_;
+   walberla::mpi::MPIRank newOwner_;
+};
+
+template<>
+struct NotificationTrait<NewGhostParticleNotification>
+{
+   static const NotificationType id = NEW_GHOST_PARTICLE_NOTIFICATION;
+};
+
+}  // namespace mesa_pd
+}  // namespace walberla
+
+//======================================================================================================================
+//
+//  Send/Recv Buffer Serialization Specialization
+//
+//======================================================================================================================
+
+namespace walberla {
+namespace mpi {
+
+template< typename T,    // Element type of SendBuffer
+          typename G>    // Growth policy of SendBuffer
+mpi::GenericSendBuffer<T,G>& operator<<( mpi::GenericSendBuffer<T,G> & buf, const mesa_pd::NewGhostParticleNotification& obj )
+{
+   buf.addDebugMarker( "cn" );
+   buf << obj.particle_.getUid();
+   buf << obj.newOwner_;
+   return buf;
+}
+
+template< typename T>    // Element type  of RecvBuffer
+mpi::GenericRecvBuffer<T>& operator>>( mpi::GenericRecvBuffer<T> & buf, mesa_pd::NewGhostParticleNotification::Parameters& objparam )
+{
+   buf.readDebugMarker( "cn" );
+   buf >> objparam.uid_;
+   buf >> objparam.newOwner_;
+   return buf;
+}
+
+} // mpi
+} // walberla
\ No newline at end of file
diff --git a/src/mesa_pd/mpi/notifications/ParseMessage.h b/src/mesa_pd/mpi/notifications/ParseMessage.h
index 6baade76d7bfd360f187b3ba3b5d22256e2297ca..60f80dedb89e935daa9122ede7d3d7acffb8a3ba 100644
--- a/src/mesa_pd/mpi/notifications/ParseMessage.h
+++ b/src/mesa_pd/mpi/notifications/ParseMessage.h
@@ -29,11 +29,13 @@
 
 #include <mesa_pd/data/ParticleStorage.h>
 #include <mesa_pd/domain/IDomain.h>
+#include <mesa_pd/mpi/notifications/NewGhostParticleNotification.h>
 #include <mesa_pd/mpi/notifications/NotificationType.h>
 #include <mesa_pd/mpi/notifications/ParticleCopyNotification.h>
 #include <mesa_pd/mpi/notifications/ParticleMigrationNotification.h>
 #include <mesa_pd/mpi/notifications/ParticleRemoteMigrationNotification.h>
 #include <mesa_pd/mpi/notifications/ParticleRemovalNotification.h>
+#include <mesa_pd/mpi/notifications/ParticleRemovalInformationNotification.h>
 #include <mesa_pd/mpi/notifications/ParticleUpdateNotification.h>
 
 #include <core/debug/Debug.h>
@@ -70,14 +72,18 @@ void ParseMessage::operator()(int sender,
 
       WALBERLA_LOG_DETAIL( "Received PARTICLE_COPY_NOTIFICATION for particle " << objparam.uid << "from neighboring process with rank " << sender );
 
-      WALBERLA_CHECK_EQUAL( ps.find(objparam.uid), ps.end(), "Ghost particle with id " << objparam.uid << " already existend.");
+      if ( ps.find(objparam.uid) == ps.end() )
+      {
+         auto pIt = createNewParticle(ps, objparam);
 
-      auto pIt = createNewParticle(ps, objparam);
+         domain.correctParticlePosition(pIt->getPositionRef());
 
-      domain.correctParticlePosition(pIt->getPositionRef());
-
-      WALBERLA_CHECK(!data::particle_flags::isSet(pIt->getFlags(), data::particle_flags::GHOST));
-      data::particle_flags::set(pIt->getFlagsRef(), data::particle_flags::GHOST);
+         //WALBERLA_CHECK(!data::particle_flags::isSet(pIt->getFlags(), data::particle_flags::GHOST));
+         data::particle_flags::set(pIt->getFlagsRef(), data::particle_flags::GHOST);
+      } else
+      {
+         WALBERLA_LOG_DETAIL("Ghost particle with id " << objparam.uid << " already existend.");
+      }
 
       WALBERLA_LOG_DETAIL( "Processed PARTICLE_COPY_NOTIFICATION for particle " << objparam.uid << "."  );
 
@@ -184,8 +190,61 @@ void ParseMessage::operator()(int sender,
 
       break;
    }
+   case NEW_GHOST_PARTICLE_NOTIFICATION: {
+      NewGhostParticleNotification::Parameters objparam;
+      rb >> objparam;
+
+      WALBERLA_LOG_DETAIL( "Received new ghost particle notification for particle " <<
+                           objparam.uid_ <<
+                           " from neighboring process with rank " <<
+                           sender <<
+                           "." );
+
+      auto pIt = ps.find( objparam.uid_ );
+      WALBERLA_CHECK_UNEQUAL( pIt, ps.end() );
+
+      pIt->getGhostOwnersRef().insert( objparam.newOwner_ );
+
+      WALBERLA_LOG_DETAIL( "Processed new ghost particle notification" );
+
+      break;
+   }
+   case PARTICLE_REMOVAL_INFORMATION_NOTIFICATION: {
+      ParticleRemovalInformationNotification::Parameters objparam;
+      rb >> objparam;
+
+      WALBERLA_LOG_DETAIL( "Received particle removal information notification for particle " <<
+                           objparam.uid_ <<
+                           " from neighboring process with rank " <<
+                           sender <<
+                           "." );
+
+      if (objparam.owner_ == receiver_)
+      {
+         using namespace walberla::mesa_pd::data::particle_flags;
+         auto pIt = ps.find( objparam.uid_ );
+         WALBERLA_CHECK_UNEQUAL( pIt, ps.end() );
+         WALBERLA_CHECK(!isSet( pIt->getFlags(), GHOST));
+
+         pIt->getGhostOwnersRef().erase( sender );
+         pIt->getNeighborStateRef().erase( sender );
+      } else
+      {
+         using namespace walberla::mesa_pd::data::particle_flags;
+         auto pIt = ps.find( objparam.uid_ );
+         if (pIt != ps.end() )
+         {
+            WALBERLA_CHECK(isSet( pIt->getFlags(), GHOST));
+            pIt->getNeighborStateRef().erase( sender );
+         }
+      }
+
+      WALBERLA_LOG_DETAIL( "Processed rigid body removal information notification" );
+
+      break;
+   }
    default:
-      throw std::runtime_error( "Received invalid notification type." );
+      WALBERLA_ABORT( "Received invalid notification type: " << notificationType << " from sender: " << sender );
    }
 }
 
diff --git a/src/mesa_pd/mpi/notifications/ParticleMigrationNotification.h b/src/mesa_pd/mpi/notifications/ParticleMigrationNotification.h
index 095f6201928e96e0fc3be0df22497d2f6eb14434..78246ac932580a03d5530c1a9e69f044f3d73662 100644
--- a/src/mesa_pd/mpi/notifications/ParticleMigrationNotification.h
+++ b/src/mesa_pd/mpi/notifications/ParticleMigrationNotification.h
@@ -45,7 +45,7 @@ class ParticleMigrationNotification {
 public:
    struct Parameters {
       id_t uid_;
-      std::vector<int> ghostOwners_ {};
+      std::unordered_set<walberla::mpi::MPIRank> ghostOwners_ {};
       walberla::mesa_pd::Vec3 oldForce_ {real_t(0)};
       walberla::mesa_pd::Vec3 oldTorque_ {real_t(0)};
    };
diff --git a/src/mesa_pd/mpi/notifications/ParticleRemovalInformationNotification.h b/src/mesa_pd/mpi/notifications/ParticleRemovalInformationNotification.h
new file mode 100644
index 0000000000000000000000000000000000000000..aec2ccdfb7904b512f912fa9aa4fa383668c9a09
--- /dev/null
+++ b/src/mesa_pd/mpi/notifications/ParticleRemovalInformationNotification.h
@@ -0,0 +1,97 @@
+//======================================================================================================================
+//
+//  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 ParticleRemovalInformationNotification.h
+//! \author Sebastian Eibl <sebastian.eibl@fau.de>
+//
+//======================================================================================================================
+
+//======================================================================================================================
+//
+//  THIS FILE IS GENERATED - PLEASE CHANGE THE TEMPLATE !!!
+//
+//======================================================================================================================
+
+#pragma once
+
+#include <mesa_pd/data/DataTypes.h>
+#include <mesa_pd/data/ParticleStorage.h>
+#include <mesa_pd/mpi/notifications/NotificationType.h>
+
+#include <core/mpi/Datatype.h>
+#include <core/mpi/RecvBuffer.h>
+#include <core/mpi/SendBuffer.h>
+
+namespace walberla {
+namespace mesa_pd {
+
+/**
+ * The ParticleRemovalInformationNotification class is used to signal other processes that a
+ * shadow copy was destroyed.
+ */
+class ParticleRemovalInformationNotification
+{
+public:
+   struct Parameters
+   {
+      id_t    uid_;
+      walberla::mpi::MPIRank owner_;
+   };
+
+   inline explicit ParticleRemovalInformationNotification( const data::Particle& particle )
+      : particle_(particle)
+   {}
+   const data::Particle& particle_;
+};
+
+template<>
+struct NotificationTrait<ParticleRemovalInformationNotification>
+{
+   static const NotificationType id = PARTICLE_REMOVAL_INFORMATION_NOTIFICATION;
+};
+
+}  // namespace mesa_pd
+}  // namespace walberla
+
+//======================================================================================================================
+//
+//  Send/Recv Buffer Serialization Specialization
+//
+//======================================================================================================================
+
+namespace walberla {
+namespace mpi {
+
+template< typename T,    // Element type of SendBuffer
+          typename G>    // Growth policy of SendBuffer
+mpi::GenericSendBuffer<T,G>& operator<<( mpi::GenericSendBuffer<T,G> & buf, const mesa_pd::ParticleRemovalInformationNotification& obj )
+{
+   buf.addDebugMarker( "ri" );
+   buf << obj.particle_.getUid();
+   buf << static_cast<walberla::mpi::MPIRank>(obj.particle_.getOwner());
+   return buf;
+}
+
+template< typename T>    // Element type  of RecvBuffer
+mpi::GenericRecvBuffer<T>& operator>>( mpi::GenericRecvBuffer<T> & buf, mesa_pd::ParticleRemovalInformationNotification::Parameters& objparam )
+{
+   buf.readDebugMarker( "ri" );
+   buf >> objparam.uid_;
+   buf >> objparam.owner_;
+   return buf;
+}
+
+} // mpi
+} // walberla
\ No newline at end of file
diff --git a/src/mesh/DistanceFunction.h b/src/mesh/DistanceFunction.h
index b3ab0bf62bb39b4b43e2e14a3643ee99f1354cd2..3ad98451cd728b6086370583132a42574db7149b 100644
--- a/src/mesh/DistanceFunction.h
+++ b/src/mesh/DistanceFunction.h
@@ -13,19 +13,17 @@
 //  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 BoundarySetup.h
+//! \file DistanceFunction.h
 //! \ingroup mesh
 //! \author Christian Godenschwager <christian.godenschwager@fau.de>
-//! \author Christian Godenschwager <martin.bauer@fau.de>
+//! \author Martin Bauer <martin.bauer@fau.de>
 //
 //======================================================================================================================
 
-#include "core/DataTypes.h"
-
+#include "core/math/Vector3.h"
+#include "mesh/MatrixVectorOperations.h"
 
 namespace walberla {
-namespace mesh {
-
 
 template<typename MeshDistanceType>
 struct MeshDistanceFunction
@@ -48,5 +46,4 @@ makeMeshDistanceFunction( const shared_ptr <MeshDistanceType> & meshDistanceObje
    return MeshDistanceFunction<MeshDistanceType>( meshDistanceObject );
 }
 
-} // namespace mesh
 } // namespace walberla
diff --git a/src/mesh/QHull.h b/src/mesh/QHull.h
index 8789826bd94c1ccbd7b6fbad9d0ffa768b6f168e..81c4d0b098e8a7f9549fc5d612b81dcbac13662b 100644
--- a/src/mesh/QHull.h
+++ b/src/mesh/QHull.h
@@ -19,6 +19,8 @@
 //
 //======================================================================================================================
 
+#pragma once
+
 #include "core/DataTypes.h"
 #include "core/math/Vector3.h"
 
@@ -515,4 +517,4 @@ void QHull<MeshType>::initMesh()
 }
 
 } // namespace mesh
-} // namespace walberla
\ No newline at end of file
+} // namespace walberla
diff --git a/src/mesh/boundary/BoundaryLocationFunction.h b/src/mesh/boundary/BoundaryLocationFunction.h
new file mode 100644
index 0000000000000000000000000000000000000000..0cd1e160b4a77f36ea75c7b6be24dab281b6ecf1
--- /dev/null
+++ b/src/mesh/boundary/BoundaryLocationFunction.h
@@ -0,0 +1,51 @@
+//======================================================================================================================
+//
+//  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 BoundaryLocationFunction.h
+//! \ingroup mesh
+//! \author Christoph Schwarzmeier <christoph.schwarzmeier@fau.de>
+//
+//======================================================================================================================
+
+namespace walberla {
+
+template< typename MeshDistanceType, typename MeshType >
+struct BoundaryLocationFunction
+{
+   BoundaryLocationFunction(const shared_ptr< MeshDistanceType >& meshDistanceObject,
+                            const shared_ptr< mesh::BoundaryLocation< MeshType > >& boundaryLocation)
+      : meshDistanceObject_(meshDistanceObject), boundaryLocation_(boundaryLocation)
+   {}
+
+   inline const mesh::BoundaryInfo& operator()(const Vector3< real_t >& p) const
+   {
+      typename MeshType::FaceHandle fh;
+      meshDistanceObject_->sqSignedDistance(mesh::toOpenMesh(p), fh);
+      return (*boundaryLocation_)[fh];
+   }
+
+   shared_ptr< MeshDistanceType > meshDistanceObject_;
+   shared_ptr< mesh::BoundaryLocation< MeshType > > boundaryLocation_;
+};
+
+template< typename MeshDistanceType, typename MeshType >
+inline BoundaryLocationFunction< MeshDistanceType, MeshType >
+   makeBoundaryLocationFunction(const shared_ptr< MeshDistanceType >& meshDistanceObject,
+                                const shared_ptr< mesh::BoundaryLocation< MeshType > >& boundaryLocation)
+{
+   return BoundaryLocationFunction< MeshDistanceType, MeshType >(meshDistanceObject, boundaryLocation);
+}
+
+} // namespace walberla
\ No newline at end of file
diff --git a/src/mesh/pe/communication/ConvexPolyhedron.h b/src/mesh/pe/communication/ConvexPolyhedron.h
index df14bb1762f9755225f004d2730a76be617ed236..1527530b48531e74ee848e221d8143207d0298f4 100644
--- a/src/mesh/pe/communication/ConvexPolyhedron.h
+++ b/src/mesh/pe/communication/ConvexPolyhedron.h
@@ -74,7 +74,7 @@ inline mesh::pe::ConvexPolyhedronPtr instantiate( mpi::RecvBuffer& buffer, const
    mesh::pe::ConvexPolyhedronParameters subobjparam;
    unmarshal( buffer, subobjparam );
    correctBodyPosition(domain, block.center(), subobjparam.gpos_);
-   auto cp = std::make_unique<mesh::pe::ConvexPolyhedron>( subobjparam.sid_, subobjparam.uid_, subobjparam.gpos_, subobjparam.rpos_, subobjparam.q_, subobjparam.mesh_, subobjparam.material_, false, subobjparam.communicating_, subobjparam.infiniteMass_ );
+   auto cp = std::make_unique<mesh::pe::ConvexPolyhedron>( subobjparam.sid_, subobjparam.uid_, subobjparam.gpos_, subobjparam.q_, subobjparam.mesh_, subobjparam.material_, false, subobjparam.communicating_, subobjparam.infiniteMass_ );
    cp->setLinearVel( subobjparam.v_ );
    cp->setAngularVel( subobjparam.w_ );
    cp->MPITrait.setOwner( subobjparam.mpiTrait_.owner_ );
diff --git a/src/mesh/pe/rigid_body/ConvexPolyhedron.cpp b/src/mesh/pe/rigid_body/ConvexPolyhedron.cpp
index eb3c3c3ee324c606dbb3210418031af23cfd3e83..62ce1d1c5a11125d9af4f939c35a4c2d51b86c8f 100644
--- a/src/mesh/pe/rigid_body/ConvexPolyhedron.cpp
+++ b/src/mesh/pe/rigid_body/ConvexPolyhedron.cpp
@@ -62,18 +62,18 @@ namespace pe {
 * \param communicating specifies if the ConvexPolyhedron should take part in synchronization (syncNextNeighbour, syncShadowOwner)
 * \param infiniteMass specifies if the ConvexPolyhedron has infinite mass and will be treated as an obstacle
 */
-ConvexPolyhedron::ConvexPolyhedron( id_t sid, id_t uid, const Vec3& gpos, const Vec3& rpos, const Quat& q,
+ConvexPolyhedron::ConvexPolyhedron( id_t sid, id_t uid, const Vec3& gpos, const Quat& q,
                                     const TriangleMesh & mesh, MaterialID material,
                                     const bool global, const bool communicating, const bool infiniteMass )
    : GeomPrimitive( getStaticTypeID(), sid, uid, material ),  // Initialization of the parent class
      mesh_( mesh )
 {
-   init( gpos, rpos, q, global, communicating, infiniteMass );
+   init( gpos, q, global, communicating, infiniteMass );
 }
 //*************************************************************************************************
 
 
-void ConvexPolyhedron::init( const Vec3& gpos, const Vec3& rpos, const Quat& q,
+void ConvexPolyhedron::init( const Vec3& gpos,  const Quat& q,
                              const bool global, const bool communicating, const bool infiniteMass )
 {
    WALBERLA_ASSERT_FLOAT_EQUAL( (toWalberla( computeCentroid( mesh_ ) ) - Vec3() ).length(), real_t(0) );
@@ -82,14 +82,21 @@ void ConvexPolyhedron::init( const Vec3& gpos, const Vec3& rpos, const Quat& q,
    mesh_.request_face_normals();
    mesh_.update_face_normals();
 
+   // Calculate the bounding sphere radius first, as setPosition will trigger a call to calcBoundingBox(), which needs it
+   real_t maxSqRadius(0);
+   for(auto vh : mesh_.vertices())
+   {
+      real_t sqRadius = mesh_.point( vh ).sqrnorm();
+      if( sqRadius > maxSqRadius )
+         maxSqRadius = sqRadius;
+   }
+   boundingSphereRadius_ = std::sqrt( maxSqRadius );
+
    // Setting the center of the ConvexPolyhedron
-   gpos_ = gpos;
+   setPosition(gpos);
 
    // Initializing the instantiated ConvexPolyhedron
-   rpos_   = rpos;                   // Setting the relative position
-   q_      = q;                      // Setting the orientation
-   R_      = q_.toRotationMatrix();  // Setting the rotation matrix
-
+   setOrientation(q);
    setGlobal( global );
    if (infiniteMass)
    {
@@ -103,14 +110,7 @@ void ConvexPolyhedron::init( const Vec3& gpos, const Vec3& rpos, const Quat& q,
    setFinite( true );
 
    // Setting the axis-aligned bounding box
-   real_t maxSqRadius(0);
-   for(auto vh : mesh_.vertices())
-   {
-      real_t sqRadius = mesh_.point( vh ).sqrnorm();
-      if( sqRadius > maxSqRadius )
-         maxSqRadius = sqRadius;
-   }
-   boundingSphereRadius_ = std::sqrt( maxSqRadius );
+
    ConvexPolyhedron::calcBoundingBox();
 
    octandVertices_[0] = supportVertex( TriangleMesh::Normal( real_t( 1), real_t( 1), real_t( 1) ), *mesh_.vertices_begin() );
@@ -365,11 +365,12 @@ void ConvexPolyhedron::print( std::ostream& os, const char* tab ) const
 
    //   if( verboseMode )
    //   {
+   Mat3 R = getRotation();
    os << tab << "   Bounding box      = " << getAABB() << "\n"
       << tab << "   Quaternion        = " << getQuaternion() << "\n"
-      << tab << "   Rotation matrix   = ( " << setw(9) << R_[0] << " , " << setw(9) << R_[1] << " , " << setw(9) << R_[2] << " )\n"
-      << tab << "                       ( " << setw(9) << R_[3] << " , " << setw(9) << R_[4] << " , " << setw(9) << R_[5] << " )\n"
-      << tab << "                       ( " << setw(9) << R_[6] << " , " << setw(9) << R_[7] << " , " << setw(9) << R_[8] << " )\n";
+      << tab << "   Rotation matrix   = ( " << setw(9) << R[0] << " , " << setw(9) << R[1] << " , " << setw(9) << R[2] << " )\n"
+      << tab << "                       ( " << setw(9) << R[3] << " , " << setw(9) << R[4] << " , " << setw(9) << R[5] << " )\n"
+      << tab << "                       ( " << setw(9) << R[6] << " , " << setw(9) << R[7] << " , " << setw(9) << R[8] << " )\n";
 
    os << std::setiosflags(std::ios::right)
       << tab << "   Moment of inertia = ( " << setw(9) << I_[0] << " , " << setw(9) << I_[1] << " , " << setw(9) << I_[2] << " )\n"
diff --git a/src/mesh/pe/rigid_body/ConvexPolyhedron.h b/src/mesh/pe/rigid_body/ConvexPolyhedron.h
index 48f7c8ff1c8c3b87cc8bf6333a8d1f3a26679499..10e6f62a7fd629aae1727eb86e1c402753487ae9 100644
--- a/src/mesh/pe/rigid_body/ConvexPolyhedron.h
+++ b/src/mesh/pe/rigid_body/ConvexPolyhedron.h
@@ -65,7 +65,7 @@ public:
    //**Constructors********************************************************************************
    /*!\name Constructors */
    //@{
-   ConvexPolyhedron( id_t sid, id_t uid, const Vec3& gpos, const Vec3& rpos, const Quat& q,
+   ConvexPolyhedron( id_t sid, id_t uid, const Vec3& gpos, const Quat& q,
                      const TriangleMesh & mesh, MaterialID material,
                      const bool global, const bool communicating, const bool infiniteMass );
    //@}
@@ -80,7 +80,7 @@ public:
    //**********************************************************************************************
 
 public:
-   void init( const Vec3& gpos, const Vec3& rpos, const Quat& q,
+   void init( const Vec3& gpos, const Quat& q,
               const bool global, const bool communicating, const bool infiniteMass );
 
    //**Get functions*******************************************************************************
diff --git a/src/mesh/pe/rigid_body/ConvexPolyhedronFactory.cpp b/src/mesh/pe/rigid_body/ConvexPolyhedronFactory.cpp
index 6f29a33cfa7e15cc5b2e14f0c1e6a84e647a16bf..4229a58f11e3a07edee85fbb7ca33c7de3345f17 100644
--- a/src/mesh/pe/rigid_body/ConvexPolyhedronFactory.cpp
+++ b/src/mesh/pe/rigid_body/ConvexPolyhedronFactory.cpp
@@ -78,7 +78,7 @@ ConvexPolyhedronID createConvexPolyhedron( BodyStorage& globalStorage, BlockStor
       WALBERLA_CHECK_EQUAL(communicating, false, "Global bodies can not be communicating!" );
       WALBERLA_CHECK_EQUAL(infiniteMass, true, "Global bodies must have infinite mass!" );
 
-      auto cp = std::make_unique<ConvexPolyhedron>(sid, uid, gpos, Vec3(0,0,0), Quat(), mesh, material, global, false, true);
+      auto cp = std::make_unique<ConvexPolyhedron>(sid, uid, gpos, Quat(), mesh, material, global, false, true);
       poly = static_cast<ConvexPolyhedronID>(&globalStorage.add(std::move(cp)));
    } else
    {
@@ -88,7 +88,7 @@ ConvexPolyhedronID createConvexPolyhedron( BodyStorage& globalStorage, BlockStor
             const id_t sid( UniqueID<RigidBody>::create() );
 
             BodyStorage& bs = (*block.getData<Storage>(storageID))[0];
-            auto cp = std::make_unique<ConvexPolyhedron>(sid, uid, gpos, Vec3(0,0,0), Quat(), mesh, material, global, communicating, infiniteMass);
+            auto cp = std::make_unique<ConvexPolyhedron>(sid, uid, gpos,  Quat(), mesh, material, global, communicating, infiniteMass);
             cp->MPITrait.setOwner(Owner(MPIManager::instance()->rank(), block.getId().getID()));
             poly = static_cast<ConvexPolyhedronID>(&bs.add( std::move(cp) ) );
          }
diff --git a/src/pe/communication/Marshalling.cpp b/src/pe/communication/Marshalling.cpp
index bdeb9fd9d9e2d36c6d282cc57f833f99b231ac1b..9a8e930f663c6e98d9098afea371fc22baf68faa 100644
--- a/src/pe/communication/Marshalling.cpp
+++ b/src/pe/communication/Marshalling.cpp
@@ -66,10 +66,6 @@ void marshal( mpi::SendBuffer& buffer, const RigidBody& obj ) {
    buffer << obj.hasInfiniteMass();
    buffer << obj.getPosition();
    buffer << obj.hasSuperBody();
-   if( obj.hasSuperBody() )
-   {
-      buffer <<  obj.getRelPosition();
-   }
    buffer << obj.getQuaternion();
    if( !obj.hasSuperBody() )
    {
@@ -96,12 +92,6 @@ void unmarshal( mpi::RecvBuffer& buffer, RigidBodyParameters& objparam ) {
    buffer >> objparam.infiniteMass_;
    buffer >> objparam.gpos_;
    buffer >> objparam.hasSuperBody_;
-
-   if( objparam.hasSuperBody_ )
-   {
-      buffer >> objparam.rpos_;
-   }
-
    buffer >> objparam.q_;
 
    if( !objparam.hasSuperBody_ )
diff --git a/src/pe/communication/Marshalling.h b/src/pe/communication/Marshalling.h
index 225ffaa4f8dc6f6fb14580cb8f7b7736fe36b261..4136362744b9eaaeaa4ddd4eb444bedddf0f2e84 100644
--- a/src/pe/communication/Marshalling.h
+++ b/src/pe/communication/Marshalling.h
@@ -75,7 +75,7 @@ struct RigidBodyParameters {
    MPIRigidBodyTraitParameter mpiTrait_;
    bool communicating_, infiniteMass_;
    id_t sid_, uid_;
-   Vec3 gpos_, rpos_, v_, w_;
+   Vec3 gpos_, v_, w_;
    bool hasSuperBody_;
    Quat q_;
 };
diff --git a/src/pe/communication/rigidbody/Box.h b/src/pe/communication/rigidbody/Box.h
index 77f543cbc9183348f40a08d07fd95981b9800fdc..78d698f484dd7fac694c7111c9a9e9a6c5efe32f 100644
--- a/src/pe/communication/rigidbody/Box.h
+++ b/src/pe/communication/rigidbody/Box.h
@@ -64,7 +64,7 @@ inline BoxPtr instantiate( mpi::RecvBuffer& buffer, const math::AABB& domain, co
    BoxParameters subobjparam;
    unmarshal( buffer, subobjparam );
    correctBodyPosition(domain, block.center(), subobjparam.gpos_);
-   auto bx = std::make_unique<Box>( subobjparam.sid_, subobjparam.uid_, subobjparam.gpos_, subobjparam.rpos_, subobjparam.q_, subobjparam.lengths_, subobjparam.material_, false, subobjparam.communicating_, subobjparam.infiniteMass_ );
+   auto bx = std::make_unique<Box>( subobjparam.sid_, subobjparam.uid_, subobjparam.gpos_, subobjparam.q_, subobjparam.lengths_, subobjparam.material_, false, subobjparam.communicating_, subobjparam.infiniteMass_ );
    bx->setLinearVel( subobjparam.v_ );
    bx->setAngularVel( subobjparam.w_ );
    bx->MPITrait.setOwner( subobjparam.mpiTrait_.owner_ );
diff --git a/src/pe/communication/rigidbody/Capsule.h b/src/pe/communication/rigidbody/Capsule.h
index 3ef02f07d1d4b95fbb3010be10e61eb158772cca..e319d579a5865829f4b57f798bd6c68e7b72ee20 100644
--- a/src/pe/communication/rigidbody/Capsule.h
+++ b/src/pe/communication/rigidbody/Capsule.h
@@ -66,7 +66,7 @@ inline CapsulePtr instantiate( mpi::RecvBuffer& buffer, const math::AABB& domain
    CapsuleParameters subobjparam;
    unmarshal( buffer, subobjparam );
    correctBodyPosition(domain, block.center(), subobjparam.gpos_);
-   auto cp = std::make_unique<Capsule>( subobjparam.sid_, subobjparam.uid_, subobjparam.gpos_, subobjparam.rpos_, subobjparam.q_, subobjparam.radius_, subobjparam.length_, subobjparam.material_, false, subobjparam.communicating_, subobjparam.infiniteMass_ );
+   auto cp = std::make_unique<Capsule>( subobjparam.sid_, subobjparam.uid_, subobjparam.gpos_,  subobjparam.q_, subobjparam.radius_, subobjparam.length_, subobjparam.material_, false, subobjparam.communicating_, subobjparam.infiniteMass_ );
    cp->setLinearVel( subobjparam.v_ );
    cp->setAngularVel( subobjparam.w_ );
    cp->MPITrait.setOwner( subobjparam.mpiTrait_.owner_ );
diff --git a/src/pe/communication/rigidbody/Ellipsoid.h b/src/pe/communication/rigidbody/Ellipsoid.h
index a292c5b1dd1bc9bfbd2fb487260571fa26bc9756..de8787b458879475fa2c5f5b445b827198b6c97e 100644
--- a/src/pe/communication/rigidbody/Ellipsoid.h
+++ b/src/pe/communication/rigidbody/Ellipsoid.h
@@ -66,7 +66,7 @@ inline EllipsoidPtr instantiate( mpi::RecvBuffer& buffer, const math::AABB& doma
    EllipsoidParameters subobjparam;
    unmarshal( buffer, subobjparam );
    correctBodyPosition(domain, block.center(), subobjparam.gpos_);
-   auto el = std::make_unique<Ellipsoid>( subobjparam.sid_, subobjparam.uid_, subobjparam.gpos_, subobjparam.rpos_, subobjparam.q_, subobjparam.semiAxes_, subobjparam.material_, false, subobjparam.communicating_, subobjparam.infiniteMass_ );
+   auto el = std::make_unique<Ellipsoid>( subobjparam.sid_, subobjparam.uid_, subobjparam.gpos_,  subobjparam.q_, subobjparam.semiAxes_, subobjparam.material_, false, subobjparam.communicating_, subobjparam.infiniteMass_ );
    el->setLinearVel( subobjparam.v_ );
    el->setAngularVel( subobjparam.w_ );
    el->MPITrait.setOwner( subobjparam.mpiTrait_.owner_ );
diff --git a/src/pe/communication/rigidbody/Sphere.h b/src/pe/communication/rigidbody/Sphere.h
index 3e7da560142c29a93161b691975f403808f09fba..846a621e57e83d721bb09b8087d1fcf34b88e6b7 100644
--- a/src/pe/communication/rigidbody/Sphere.h
+++ b/src/pe/communication/rigidbody/Sphere.h
@@ -66,7 +66,7 @@ inline SpherePtr instantiate( mpi::RecvBuffer& buffer, const math::AABB& domain,
    SphereParameters subobjparam;
    unmarshal( buffer, subobjparam );
    correctBodyPosition(domain, block.center(), subobjparam.gpos_);
-   auto sp = std::make_unique<Sphere>( subobjparam.sid_, subobjparam.uid_, subobjparam.gpos_, subobjparam.rpos_, subobjparam.q_, subobjparam.radius_, subobjparam.material_, false, subobjparam.communicating_, subobjparam.infiniteMass_ );
+   auto sp = std::make_unique<Sphere>( subobjparam.sid_, subobjparam.uid_, subobjparam.gpos_, subobjparam.q_, subobjparam.radius_, subobjparam.material_, false, subobjparam.communicating_, subobjparam.infiniteMass_ );
    sp->setLinearVel( subobjparam.v_ );
    sp->setAngularVel( subobjparam.w_ );
    sp->MPITrait.setOwner( subobjparam.mpiTrait_.owner_ );
diff --git a/src/pe/communication/rigidbody/Squirmer.h b/src/pe/communication/rigidbody/Squirmer.h
index 3c9b3fbe094c6cef756168c6bfd63078825f001c..171b9c7420d813c5507ffc5caaf620b0b89267b8 100644
--- a/src/pe/communication/rigidbody/Squirmer.h
+++ b/src/pe/communication/rigidbody/Squirmer.h
@@ -66,7 +66,7 @@ inline SquirmerPtr instantiate( mpi::RecvBuffer& buffer, const math::AABB& domai
    SquirmerParameters subobjparam;
    unmarshal( buffer, subobjparam );
    correctBodyPosition(domain, block.center(), subobjparam.gpos_);
-   auto sq = std::make_unique<Squirmer>( subobjparam.sid_, subobjparam.uid_, subobjparam.gpos_, subobjparam.rpos_, subobjparam.q_, subobjparam.radius_, subobjparam.squirmerVelocity_, subobjparam.squirmerBeta_, subobjparam.material_, false, subobjparam.communicating_, subobjparam.infiniteMass_ );
+   auto sq = std::make_unique<Squirmer>( subobjparam.sid_, subobjparam.uid_, subobjparam.gpos_, subobjparam.q_, subobjparam.radius_, subobjparam.squirmerVelocity_, subobjparam.squirmerBeta_, subobjparam.material_, false, subobjparam.communicating_, subobjparam.infiniteMass_ );
    sq->setLinearVel( subobjparam.v_ );
    sq->setAngularVel( subobjparam.w_ );
    sq->MPITrait.setOwner( subobjparam.mpiTrait_.owner_ );
diff --git a/src/pe/communication/rigidbody/Union.h b/src/pe/communication/rigidbody/Union.h
index bfaf3788115dde028743e03d330e0dfe41959bc8..644f16a8f3f213965c64a848b10f91cd44f210cd 100644
--- a/src/pe/communication/rigidbody/Union.h
+++ b/src/pe/communication/rigidbody/Union.h
@@ -119,8 +119,7 @@ inline std::unique_ptr<Union<BodyTypes...>> instantiate( mpi::RecvBuffer& buffer
    correctBodyPosition(domain, block.center(), subobjparam.gpos_);
    auto un = std::make_unique<Union<BodyTypes...>>( subobjparam.sid_,
                                                      subobjparam.uid_,
-                                                     subobjparam.gpos_,
-                                                     subobjparam.rpos_,
+                                                     Vec3(),
                                                      subobjparam.q_,
                                                      false,
                                                      subobjparam.communicating_,
@@ -140,6 +139,10 @@ inline std::unique_ptr<Union<BodyTypes...>> instantiate( mpi::RecvBuffer& buffer
    un->setLinearVel( subobjparam.v_ );
    un->setAngularVel( subobjparam.w_ );
    newBody = un.get();
+   // Checks with global data of the union
+   WALBERLA_ASSERT_FLOAT_EQUAL(subobjparam.m_, un->getMass());
+   WALBERLA_ASSERT_FLOAT_EQUAL(subobjparam.I_, un->getBodyInertia());
+   WALBERLA_ASSERT_FLOAT_EQUAL(subobjparam.gpos_, un->getPosition());
    return un;
 }
 
diff --git a/src/pe/rigidbody/Box.cpp b/src/pe/rigidbody/Box.cpp
index ffd27fc8ebc5639cf5f6ad794c3e45c22fee8a5a..0705a4ca1f5463cd0a4a078353cb7e872b6f4db5 100644
--- a/src/pe/rigidbody/Box.cpp
+++ b/src/pe/rigidbody/Box.cpp
@@ -55,7 +55,7 @@ namespace pe {
  * \param visible Specifies if the box is visible in a visualization.
  * \param fixed \a true to fix the box, \a false to unfix it.
  */
-Box::Box( id_t sid, id_t uid, const Vec3& gpos, const Vec3& rpos, const Quat& q,
+Box::Box( id_t sid, id_t uid, const Vec3& gpos, const Quat& q,
           const Vec3& lengths, MaterialID material,
           const bool global, const bool communicating, const bool infiniteMass )
    : GeomPrimitive( getStaticTypeID(), sid, uid, material )  // Initialization of the parent class
@@ -70,10 +70,8 @@ Box::Box( id_t sid, id_t uid, const Vec3& gpos, const Vec3& rpos, const Quat& q,
    WALBERLA_ASSERT_GREATER( lengths[2], real_t(0), "Invalid side length in z-dimension" );
 
    // Initializing the instantiated box
-   gpos_   = gpos;
-   rpos_   = rpos;                   // Setting the relative position
-   q_      = q;                      // Setting the orientation
-   R_      = q_.toRotationMatrix();  // Setting the rotation matrix
+   setPosition(gpos);
+   setOrientation(q);                      // Setting the orientation
 
    setGlobal( global );
    if (infiniteMass)
@@ -267,11 +265,12 @@ void Box::print( std::ostream& os, const char* tab ) const
 
    //if( verboseMode )
    {
+      Mat3 R = getRotation();
       os << tab << "   Bounding box      = " << getAABB() << "\n"
          << tab << "   Quaternion        = " << getQuaternion() << "\n"
-         << tab << "   Rotation matrix   = ( " << setw(9) << R_[0] << " , " << setw(9) << R_[1] << " , " << setw(9) << R_[2] << " )\n"
-         << tab << "                       ( " << setw(9) << R_[3] << " , " << setw(9) << R_[4] << " , " << setw(9) << R_[5] << " )\n"
-         << tab << "                       ( " << setw(9) << R_[6] << " , " << setw(9) << R_[7] << " , " << setw(9) << R_[8] << " )\n";
+         << tab << "   Rotation matrix   = ( " << setw(9) << R[0] << " , " << setw(9) << R[1] << " , " << setw(9) << R[2] << " )\n"
+         << tab << "                       ( " << setw(9) << R[3] << " , " << setw(9) << R[4] << " , " << setw(9) << R[5] << " )\n"
+         << tab << "                       ( " << setw(9) << R[6] << " , " << setw(9) << R[7] << " , " << setw(9) << R[8] << " )\n";
 
       os << std::setiosflags(std::ios::right)
          << tab << "   Moment of inertia = ( " << setw(9) << I_[0] << " , " << setw(9) << I_[1] << " , " << setw(9) << I_[2] << " )\n"
@@ -302,22 +301,23 @@ void Box::print( std::ostream& os, const char* tab ) const
 void Box::calcBoundingBox()
 {
    using std::fabs;
-
-   const real_t xlength( real_t(0.5) * ( fabs(R_[0]*lengths_[0]) + fabs(R_[1]*lengths_[1]) + fabs(R_[2]*lengths_[2]) ) + contactThreshold );
-   const real_t ylength( real_t(0.5) * ( fabs(R_[3]*lengths_[0]) + fabs(R_[4]*lengths_[1]) + fabs(R_[5]*lengths_[2]) ) + contactThreshold );
-   const real_t zlength( real_t(0.5) * ( fabs(R_[6]*lengths_[0]) + fabs(R_[7]*lengths_[1]) + fabs(R_[8]*lengths_[2]) ) + contactThreshold );
+   Mat3 R = getRotation();
+   const real_t xlength( real_t(0.5) * ( fabs(R[0]*lengths_[0]) + fabs(R[1]*lengths_[1]) + fabs(R[2]*lengths_[2]) ) + contactThreshold );
+   const real_t ylength( real_t(0.5) * ( fabs(R[3]*lengths_[0]) + fabs(R[4]*lengths_[1]) + fabs(R[5]*lengths_[2]) ) + contactThreshold );
+   const real_t zlength( real_t(0.5) * ( fabs(R[6]*lengths_[0]) + fabs(R[7]*lengths_[1]) + fabs(R[8]*lengths_[2]) ) + contactThreshold );
    aabb_ = math::AABB(
-            gpos_[0] - xlength,
-         gpos_[1] - ylength,
-         gpos_[2] - zlength,
-         gpos_[0] + xlength,
-         gpos_[1] + ylength,
-         gpos_[2] + zlength
+            getPosition()[0] - xlength,
+         getPosition()[1] - ylength,
+         getPosition()[2] - zlength,
+         getPosition()[0] + xlength,
+         getPosition()[1] + ylength,
+         getPosition()[2] + zlength
          );
 
    //WALBERLA_ASSERT( aabb_.isValid()        , "Invalid bounding box detected" );
-   WALBERLA_ASSERT( aabb_.contains( gpos_ ), "Invalid bounding box detected" );
+   WALBERLA_ASSERT( aabb_.contains( getPosition() ), "Invalid bounding box detected" );
 }
+
 //*************************************************************************************************
 
 
diff --git a/src/pe/rigidbody/Box.h b/src/pe/rigidbody/Box.h
index 5da39e7845006907ab7fa7fb67e9707782c0e735..266aead5d9d9f4e1dfb1a6ebce1dad29a2d31544 100644
--- a/src/pe/rigidbody/Box.h
+++ b/src/pe/rigidbody/Box.h
@@ -65,7 +65,7 @@ public:
    //**Constructors********************************************************************************
    /*!\name Constructors */
    //@{
-   explicit Box( id_t sid, id_t uid, const Vec3& gpos, const Vec3& rpos, const Quat& q,
+   explicit Box( id_t sid, id_t uid, const Vec3& gpos, const Quat& q,
                  const Vec3& lengths, MaterialID material,
                  const bool global, const bool communicating, const bool infiniteMass );
    //@}
@@ -440,7 +440,7 @@ inline Vec3 Box::support( const Vec3& d ) const
                                math::sign(bfD[1])*lengths_[1]*real_t(0.5),
                                math::sign(bfD[2])*lengths_[2]*real_t(0.5) );
 
-   return gpos_ + vectorFromBFtoWF(relativSupport);
+   return getPosition() + vectorFromBFtoWF(relativSupport);
 }
 //*************************************************************************************************
 
diff --git a/src/pe/rigidbody/BoxFactory.cpp b/src/pe/rigidbody/BoxFactory.cpp
index 6f471ba55ee6fbf1a9838eef6b2da5ec91255958..2a703600420f9931015675cbe82af1c20b635818 100644
--- a/src/pe/rigidbody/BoxFactory.cpp
+++ b/src/pe/rigidbody/BoxFactory.cpp
@@ -50,7 +50,7 @@ BoxID createBox(       BodyStorage& globalStorage, BlockStorage& blocks, BlockDa
       const id_t sid = UniqueID<RigidBody>::createGlobal();
       WALBERLA_ASSERT_EQUAL(communicating, false);
       WALBERLA_ASSERT_EQUAL(infiniteMass, true);
-      BoxPtr bx = std::make_unique<Box>(sid, uid, gpos, Vec3(0,0,0), Quat(), lengths, material, global, false, true);
+      BoxPtr bx = std::make_unique<Box>(sid, uid, gpos, Quat(), lengths, material, global, false, true);
       box = static_cast<BoxID>(&globalStorage.add(std::move(bx)));
    } else
    {
@@ -60,7 +60,7 @@ BoxID createBox(       BodyStorage& globalStorage, BlockStorage& blocks, BlockDa
             const id_t sid( UniqueID<RigidBody>::create() );
 
             BodyStorage& bs = (*block.getData<Storage>(storageID))[0];
-            BoxPtr bx = std::make_unique<Box>(sid, uid, gpos, Vec3(0,0,0), Quat(), lengths, material, global, communicating, infiniteMass);
+            BoxPtr bx = std::make_unique<Box>(sid, uid, gpos, Quat(), lengths, material, global, communicating, infiniteMass);
             bx->MPITrait.setOwner(Owner(MPIManager::instance()->rank(), block.getId().getID()));
             box = static_cast<BoxID>(&bs.add(std::move(bx)));
          }
diff --git a/src/pe/rigidbody/Capsule.cpp b/src/pe/rigidbody/Capsule.cpp
index 7b58b0ce64b744bd31657b8cd63e93863fc9603d..f1d9ad9ee146d699bb7f3b062303454e394ed815 100644
--- a/src/pe/rigidbody/Capsule.cpp
+++ b/src/pe/rigidbody/Capsule.cpp
@@ -55,7 +55,7 @@ namespace pe {
  *
  * The capsule is created lying along the x-axis.
  */
-Capsule::Capsule( id_t sid, id_t uid, const Vec3& gpos, const Vec3& rpos, const Quat& q,
+Capsule::Capsule( id_t sid, id_t uid, const Vec3& gpos, const Quat& q,
                   real_t  radius, real_t  length, MaterialID material,
                   const bool global, const bool communicating, const bool infiniteMass )
    : GeomPrimitive( getStaticTypeID(), sid, uid, material )           // Initializing the base object
@@ -70,10 +70,8 @@ Capsule::Capsule( id_t sid, id_t uid, const Vec3& gpos, const Vec3& rpos, const
    WALBERLA_ASSERT_GREATER( length, real_t(0), "Invalid capsule length"  );
 
    // Initializing the instantiated capsule
-   gpos_   = gpos;
-   rpos_   = rpos;                   // Setting the relative position
-   q_      = q;                      // Setting the orientation
-   R_      = q_.toRotationMatrix();  // Setting the rotation matrix
+   setPosition(gpos);
+   setOrientation(q);
 
    setGlobal( global );
    if (infiniteMass)
@@ -171,18 +169,20 @@ bool Capsule::isSurfaceRelPointImpl( real_t px, real_t py, real_t  pz ) const
  */
 void Capsule::calcBoundingBox()
 {
-   const real_t  xlength( std::fabs( R_[0]*length_ )*real_t (0.5) + radius_ + contactThreshold );
-   const real_t  ylength( std::fabs( R_[3]*length_ )*real_t (0.5) + radius_ + contactThreshold );
-   const real_t  zlength( std::fabs( R_[6]*length_ )*real_t (0.5) + radius_ + contactThreshold );
+   Mat3 R = getRotation();
+   Vec3 gpos = getPosition();
+   const real_t  xlength( std::fabs( R[0]*length_ )*real_t (0.5) + radius_ + contactThreshold );
+   const real_t  ylength( std::fabs( R[3]*length_ )*real_t (0.5) + radius_ + contactThreshold );
+   const real_t  zlength( std::fabs( R[6]*length_ )*real_t (0.5) + radius_ + contactThreshold );
    aabb_ = math::AABB(
-            gpos_[0] - xlength,
-         gpos_[1] - ylength,
-         gpos_[2] - zlength,
-         gpos_[0] + xlength,
-         gpos_[1] + ylength,
-         gpos_[2] + zlength);
-
-   WALBERLA_ASSERT( aabb_.contains( gpos_ ), "Invalid bounding box detected" );
+           gpos[0] - xlength,
+         gpos[1] - ylength,
+         gpos[2] - zlength,
+         gpos[0] + xlength,
+         gpos[1] + ylength,
+         gpos[2] + zlength);
+
+   WALBERLA_ASSERT( aabb_.contains( getPosition() ), "Invalid bounding box detected" );
 }
 //*************************************************************************************************
 
@@ -243,11 +243,12 @@ void Capsule::print( std::ostream& os, const char* tab ) const
       << tab << "   Linear velocity   = " << getLinearVel() << "\n"
       << tab << "   Angular velocity  = " << getAngularVel() << "\n";
 
+   Mat3 R = getRotation();
    os << tab << "   Bounding box      = " << getAABB() << "\n"
       << tab << "   Quaternion        = " << getQuaternion() << "\n"
-      << tab << "   Rotation matrix   = ( " << setw(9) << R_[0] << " , " << setw(9) << R_[1] << " , " << setw(9) << R_[2] << " )\n"
-      << tab << "                       ( " << setw(9) << R_[3] << " , " << setw(9) << R_[4] << " , " << setw(9) << R_[5] << " )\n"
-      << tab << "                       ( " << setw(9) << R_[6] << " , " << setw(9) << R_[7] << " , " << setw(9) << R_[8] << " )\n";
+      << tab << "   Rotation matrix   = ( " << setw(9) << R[0] << " , " << setw(9) << R[1] << " , " << setw(9) << R[2] << " )\n"
+      << tab << "                       ( " << setw(9) << R[3] << " , " << setw(9) << R[4] << " , " << setw(9) << R[5] << " )\n"
+      << tab << "                       ( " << setw(9) << R[6] << " , " << setw(9) << R[7] << " , " << setw(9) << R[8] << " )\n";
 
    os << std::setiosflags(std::ios::right)
       << tab << "   Moment of inertia = ( " << setw(9) << I_[0] << " , " << setw(9) << I_[1] << " , " << setw(9) << I_[2] << " )\n"
diff --git a/src/pe/rigidbody/Capsule.h b/src/pe/rigidbody/Capsule.h
index c2d4c6b46ae5105ddc760321ff3ccbff06e5fa6e..8718f4c90768026a8a90999c8ed569635eb333ed 100644
--- a/src/pe/rigidbody/Capsule.h
+++ b/src/pe/rigidbody/Capsule.h
@@ -70,7 +70,7 @@ public:
    //**Constructors********************************************************************************
    /*!\name Constructors */
    //@{
-   explicit Capsule( id_t sid, id_t uid, const Vec3& gpos, const Vec3& rpos, const Quat& q,
+   explicit Capsule( id_t sid, id_t uid, const Vec3& gpos,  const Quat& q,
                      real_t  radius, real_t  length, MaterialID material,
                      const bool global, const bool communicating, const bool infiniteMass );
    //@}
@@ -97,7 +97,6 @@ public:
    /*!\name Utility functions */
    //@{
    inline virtual Vec3 support( const Vec3& d ) const;
-   inline virtual Vec3 supportContactThreshold( const Vec3& d ) const;
    //@}
    //**********************************************************************************************
 
@@ -278,30 +277,11 @@ inline Vec3 Capsule::support( const Vec3& d ) const
    const Vec3 supportSegment = Vec3( math::sign(bfD[0])*length_*real_t (0.5), real_t (0.0), real_t (0.0));
    const Vec3 supportSphere = radius_ * dnorm;
 
-   return gpos_ + vectorFromBFtoWF(supportSegment) + supportSphere;
+   return getPosition() + vectorFromBFtoWF(supportSegment) + supportSphere;
 }
 //*************************************************************************************************
 
 
-//*************************************************************************************************
-/*!\brief Estimates the point which is farthest in direction \a d.
- *
- * \param d The normalized search direction in world-frame coordinates
- * \return The support point in world-frame coordinates in direction a\ d extended by a vector in
- *         direction \a d of length \a pe::contactThreshold.
- */
-inline Vec3 Capsule::supportContactThreshold( const Vec3& d ) const
-{
-   auto len = d.sqrLength();
-   if (math::equal(len, real_t(0)))
-      return Vec3(0,0,0);
-
-   return support(d) + d*contactThreshold;
-}
-//*************************************************************************************************
-
-
-
 
 //=================================================================================================
 //
diff --git a/src/pe/rigidbody/CapsuleFactory.cpp b/src/pe/rigidbody/CapsuleFactory.cpp
index 325af9701f1c78bcf1497f2717fa586372d3e80b..349e30b2f6e897af0806fade153506fc6e7d71fd 100644
--- a/src/pe/rigidbody/CapsuleFactory.cpp
+++ b/src/pe/rigidbody/CapsuleFactory.cpp
@@ -50,7 +50,7 @@ CapsuleID createCapsule(   BodyStorage& globalStorage, BlockStorage& blocks, Blo
       const id_t sid = UniqueID<RigidBody>::createGlobal();
       WALBERLA_ASSERT_EQUAL(communicating, false);
       WALBERLA_ASSERT_EQUAL(infiniteMass, true);
-      CapsulePtr cp = std::make_unique<Capsule>(sid, uid, gpos, Vec3(0,0,0), Quat(), radius, length, material, global, false, true);
+      CapsulePtr cp = std::make_unique<Capsule>(sid, uid, gpos, Quat(), radius, length, material, global, false, true);
       capsule = static_cast<CapsuleID>(&globalStorage.add(std::move(cp)));
    } else
    {
@@ -60,7 +60,7 @@ CapsuleID createCapsule(   BodyStorage& globalStorage, BlockStorage& blocks, Blo
             const id_t sid( UniqueID<RigidBody>::create() );
 
             BodyStorage& bs = (*block.getData<Storage>(storageID))[0];
-            CapsulePtr cp = std::make_unique<Capsule>(sid, uid, gpos, Vec3(0,0,0), Quat(), radius, length, material, global, communicating, infiniteMass);
+            CapsulePtr cp = std::make_unique<Capsule>(sid, uid, gpos, Quat(), radius, length, material, global, communicating, infiniteMass);
             cp->MPITrait.setOwner(Owner(MPIManager::instance()->rank(), block.getId().getID()));
             capsule = static_cast<CapsuleID>(&bs.add( std::move(cp) ));
          }
diff --git a/src/pe/rigidbody/CylindricalBoundary.cpp b/src/pe/rigidbody/CylindricalBoundary.cpp
index 66f639e49015a66ab41caf851690e678149c507b..58f4f97ea3a4e68bdc5d85e7e2b3f329a735118c 100644
--- a/src/pe/rigidbody/CylindricalBoundary.cpp
+++ b/src/pe/rigidbody/CylindricalBoundary.cpp
@@ -71,9 +71,8 @@ CylindricalBoundary::CylindricalBoundary( id_t sid, id_t uid, const Vec3& gpos,
    WALBERLA_ASSERT_GREATER( radius, real_t(0), "Invalid cylinder radius"  );
 
    // Initializing the instantiated cylinder
-   gpos_   = gpos;
-   q_      = Quat();                 // Setting the orientation
-   R_      = q_.toRotationMatrix();  // Setting the rotation matrix
+   setPosition(gpos);
+   setOrientation(Quat());
 
    // Setting the axis-aligned bounding box
    CylindricalBoundary::calcBoundingBox();
@@ -147,7 +146,7 @@ void CylindricalBoundary::calcBoundingBox()
          +math::Limits<real_t>::inf(),
          +math::Limits<real_t>::inf());
 
-   WALBERLA_ASSERT( aabb_.contains( gpos_ ), "Invalid bounding box detected" );
+   WALBERLA_ASSERT( aabb_.contains( getPosition() ), "Invalid bounding box detected" );
 }
 //*************************************************************************************************
 
@@ -179,12 +178,12 @@ void CylindricalBoundary::print( std::ostream& os, const char* tab ) const
       << tab << "   Global position   = " << getPosition() << "\n"
       << tab << "   Linear velocity   = " << getLinearVel() << "\n"
       << tab << "   Angular velocity  = " << getAngularVel() << "\n";
-
+   Mat3 R = getRotation();
    os << tab << "   Bounding box      = " << getAABB() << "\n"
       << tab << "   Quaternion        = " << getQuaternion() << "\n"
-      << tab << "   Rotation matrix   = ( " << setw(9) << R_[0] << " , " << setw(9) << R_[1] << " , " << setw(9) << R_[2] << " )\n"
-      << tab << "                       ( " << setw(9) << R_[3] << " , " << setw(9) << R_[4] << " , " << setw(9) << R_[5] << " )\n"
-      << tab << "                       ( " << setw(9) << R_[6] << " , " << setw(9) << R_[7] << " , " << setw(9) << R_[8] << " )\n";
+      << tab << "   Rotation matrix   = ( " << setw(9) << R[0] << " , " << setw(9) << R[1] << " , " << setw(9) << R[2] << " )\n"
+      << tab << "                       ( " << setw(9) << R[3] << " , " << setw(9) << R[4] << " , " << setw(9) << R[5] << " )\n"
+      << tab << "                       ( " << setw(9) << R[6] << " , " << setw(9) << R[7] << " , " << setw(9) << R[8] << " )\n";
 }
 //*************************************************************************************************
 
diff --git a/src/pe/rigidbody/Ellipsoid.cpp b/src/pe/rigidbody/Ellipsoid.cpp
index 3c68d587590468955490f1b60f362f3fbe882007..d47b400d543a33be82c97eda8a49c78fc1603ab7 100644
--- a/src/pe/rigidbody/Ellipsoid.cpp
+++ b/src/pe/rigidbody/Ellipsoid.cpp
@@ -58,12 +58,12 @@ namespace pe {
  * \param communicating specifies if the Ellipsoid should take part in synchronization (syncNextNeighbour, syncShadowOwner)
  * \param infiniteMass specifies if the Ellipsoid has infinite mass and will be treated as an obstacle
  */
-Ellipsoid::Ellipsoid( id_t sid, id_t uid, const Vec3& gpos, const Vec3& rpos, const Quat& q,
+Ellipsoid::Ellipsoid( id_t sid, id_t uid, const Vec3& gpos, const Quat& q,
                 const Vec3& semiAxes, MaterialID material,
                 const bool global, const bool communicating, const bool infiniteMass )
-   : Ellipsoid::Ellipsoid( getStaticTypeID(), sid, uid, gpos, rpos, q, semiAxes, material, global, communicating, infiniteMass )
+   : Ellipsoid::Ellipsoid( getStaticTypeID(), sid, uid, gpos, q, semiAxes, material, global, communicating, infiniteMass )
 {}
-Ellipsoid::Ellipsoid( id_t const typeId, id_t sid, id_t uid, const Vec3& gpos, const Vec3& rpos, const Quat& q,
+Ellipsoid::Ellipsoid( id_t const typeId, id_t sid, id_t uid, const Vec3& gpos, const Quat& q,
                 const Vec3& semiAxes, MaterialID material,
                 const bool global, const bool communicating, const bool infiniteMass )
    : GeomPrimitive( typeId, sid, uid, material )  // Initialization of the parent class
@@ -76,13 +76,10 @@ Ellipsoid::Ellipsoid( id_t const typeId, id_t sid, id_t uid, const Vec3& gpos, c
    WALBERLA_ASSERT( semiAxes_[0] > real_c(0), "Invalid Ellipsoid radius" );
    WALBERLA_ASSERT( semiAxes_[1] > real_c(0), "Invalid Ellipsoid radius" );
    WALBERLA_ASSERT( semiAxes_[2] > real_c(0), "Invalid Ellipsoid radius" );
-   // Setting the center of the Ellipsoid
-   gpos_ = gpos;
 
-   // Initializing the instantiated Ellipsoid
-   rpos_   = rpos;                   // Setting the relative position
-   q_      = q;                      // Setting the orientation
-   R_      = q_.toRotationMatrix();  // Setting the rotation matrix
+   // Setting the center of the Ellipsoid
+   setPosition(gpos);
+   setOrientation(q);
 
    setGlobal( global );
    if (infiniteMass)
@@ -182,6 +179,8 @@ void Ellipsoid::print( std::ostream& os, const char* tab ) const
 {
    using std::setw;
 
+   Mat3 R = getRotation();
+
    os << tab << " Ellipsoid " << uid_ << " with semi-axis " << semiAxes_ << "\n";
 
    os << tab << "   Fixed: " << isFixed() << " , sleeping: " << !isAwake() << "\n";
@@ -199,9 +198,9 @@ void Ellipsoid::print( std::ostream& os, const char* tab ) const
 //   {
       os << tab << "   Bounding box      = " << getAABB() << "\n"
          << tab << "   Quaternion        = " << getQuaternion() << "\n"
-         << tab << "   Rotation matrix   = ( " << setw(9) << R_[0] << " , " << setw(9) << R_[1] << " , " << setw(9) << R_[2] << " )\n"
-         << tab << "                       ( " << setw(9) << R_[3] << " , " << setw(9) << R_[4] << " , " << setw(9) << R_[5] << " )\n"
-         << tab << "                       ( " << setw(9) << R_[6] << " , " << setw(9) << R_[7] << " , " << setw(9) << R_[8] << " )\n";
+         << tab << "   Rotation matrix   = ( " << setw(9) << R[1] << " , " << setw(9) << R[1] << " , " << setw(9) << R[2] << " )\n"
+         << tab << "                       ( " << setw(9) << R[3] << " , " << setw(9) << R[4] << " , " << setw(9) << R[5] << " )\n"
+         << tab << "                       ( " << setw(9) << R[6] << " , " << setw(9) << R[7] << " , " << setw(9) << R[8] << " )\n";
 
       os << std::setiosflags(std::ios::right)
          << tab << "   Moment of inertia = ( " << setw(9) << I_[0] << " , " << setw(9) << I_[1] << " , " << setw(9) << I_[2] << " )\n"
diff --git a/src/pe/rigidbody/Ellipsoid.h b/src/pe/rigidbody/Ellipsoid.h
index 573d773488bb950b74944915abafa343cdf6ad34..1cda4738157ba98c7aabe5439a4ff9547ceddbe3 100644
--- a/src/pe/rigidbody/Ellipsoid.h
+++ b/src/pe/rigidbody/Ellipsoid.h
@@ -65,10 +65,10 @@ public:
    //**Constructors********************************************************************************
    /*!\name Constructors */
    //@{
-   explicit Ellipsoid( id_t sid, id_t uid, const Vec3& gpos, const Vec3& rpos, const Quat& q,
+   explicit Ellipsoid( id_t sid, id_t uid, const Vec3& gpos, const Quat& q,
                        const Vec3& semiAxes, MaterialID material,
                        const bool global, const bool communicating, const bool infiniteMass );
-   explicit Ellipsoid( id_t const typeID, id_t sid, id_t uid, const Vec3& gpos, const Vec3& rpos, const Quat& q,
+   explicit Ellipsoid( id_t const typeID, id_t sid, id_t uid, const Vec3& rpos, const Quat& q,
                        const Vec3& semiAxes, MaterialID material,
                        const bool global, const bool communicating, const bool infiniteMass );
    //@}
@@ -259,20 +259,21 @@ inline real_t Ellipsoid::calcDensity(const Vec3& semiAxes, real_t mass )
 inline void Ellipsoid::calcBoundingBox()
 {	
    using std::fabs;
-
-   const real_t xlength( fabs(R_[0]*semiAxes_[0]) + fabs(R_[1]*semiAxes_[1]) + fabs(R_[2]*semiAxes_[2])  + contactThreshold );
-   const real_t ylength( fabs(R_[3]*semiAxes_[0]) + fabs(R_[4]*semiAxes_[1]) + fabs(R_[5]*semiAxes_[2])  + contactThreshold );
-   const real_t zlength( fabs(R_[6]*semiAxes_[0]) + fabs(R_[7]*semiAxes_[1]) + fabs(R_[8]*semiAxes_[2])  + contactThreshold );
+   Mat3 R(getRotation());
+   Vec3 gpos = getPosition();
+   const real_t xlength( fabs(R[0]*semiAxes_[0]) + fabs(R[1]*semiAxes_[1]) + fabs(R[2]*semiAxes_[2])  + contactThreshold );
+   const real_t ylength( fabs(R[3]*semiAxes_[0]) + fabs(R[4]*semiAxes_[1]) + fabs(R[5]*semiAxes_[2])  + contactThreshold );
+   const real_t zlength( fabs(R[6]*semiAxes_[0]) + fabs(R[7]*semiAxes_[1]) + fabs(R[8]*semiAxes_[2])  + contactThreshold );
    aabb_ = math::AABB(
-            gpos_[0] - xlength,
-         gpos_[1] - ylength,
-         gpos_[2] - zlength,
-         gpos_[0] + xlength,
-         gpos_[1] + ylength,
-         gpos_[2] + zlength
+           gpos[0] - xlength,
+           gpos[1] - ylength,
+           gpos[2] - zlength,
+           gpos[0] + xlength,
+           gpos[1] + ylength,
+           gpos[2] + zlength
          );
    //   WALBERLA_ASSERT( aabb_.isValid()        , "Invalid bounding box detected" );
-   WALBERLA_ASSERT( aabb_.contains( gpos_ ), "Invalid bounding box detected("<< getSystemID() <<")\n" << "pos: " << gpos_ << "\nlengths: " << xlength << "," << ylength << ", " << zlength<< "\nvel: " << getLinearVel() << "\nbox: " << aabb_ );
+   WALBERLA_ASSERT( aabb_.contains( gpos ), "Invalid bounding box detected("<< getSystemID() <<")\n" << "pos: " << getPosition() << "\nlengths: " << xlength << "," << ylength << ", " << zlength<< "\nvel: " << getLinearVel() << "\nbox: " << aabb_ );
 }
 //*************************************************************************************************
 
diff --git a/src/pe/rigidbody/EllipsoidFactory.cpp b/src/pe/rigidbody/EllipsoidFactory.cpp
index a5e46b3506115d67c011917c9109eab6f5556bf5..2136a8414bb80da7d0518b97a9aa039c290e0ca2 100644
--- a/src/pe/rigidbody/EllipsoidFactory.cpp
+++ b/src/pe/rigidbody/EllipsoidFactory.cpp
@@ -48,7 +48,7 @@ EllipsoidID createEllipsoid( BodyStorage& globalStorage, BlockStorage& blocks, B
       const id_t sid = UniqueID<RigidBody>::createGlobal();
       WALBERLA_ASSERT_EQUAL(communicating, false);
       WALBERLA_ASSERT_EQUAL(infiniteMass, true);
-      EllipsoidPtr el = std::make_unique<Ellipsoid>(sid, uid, gpos, Vec3(0,0,0), Quat(), semiAxes, material, global, false, true);
+      EllipsoidPtr el = std::make_unique<Ellipsoid>(sid, uid, gpos, Quat(), semiAxes, material, global, false, true);
       ellipsoid = static_cast<EllipsoidID>(&globalStorage.add(std::move(el)));
    } else
    {
@@ -58,7 +58,7 @@ EllipsoidID createEllipsoid( BodyStorage& globalStorage, BlockStorage& blocks, B
             const id_t sid( UniqueID<RigidBody>::create() );
 
             BodyStorage& bs = (*block.getData<Storage>(storageID))[0];
-            EllipsoidPtr el = std::make_unique<Ellipsoid>(sid, uid, gpos, Vec3(0,0,0), Quat(), semiAxes, material, global, communicating, infiniteMass);
+            EllipsoidPtr el = std::make_unique<Ellipsoid>(sid, uid, gpos, Quat(), semiAxes, material, global, communicating, infiniteMass);
             el->MPITrait.setOwner(Owner(MPIManager::instance()->rank(), block.getId().getID()));
             ellipsoid = static_cast<EllipsoidID>(&bs.add(std::move(el)));
          }
diff --git a/src/pe/rigidbody/Plane.cpp b/src/pe/rigidbody/Plane.cpp
index 872edbe3e2a7a6ab3324204ae1e12ee4984878b8..0d501c60d912bb9cdc6cdd46979af2efa5307ddf 100644
--- a/src/pe/rigidbody/Plane.cpp
+++ b/src/pe/rigidbody/Plane.cpp
@@ -87,17 +87,15 @@ Plane::Plane( id_t sid, id_t uid,
    WALBERLA_ASSERT_FLOAT_EQUAL( normal.sqrLength(), real_c(1) , "Invalid plane normal" );
 
    // Setting the global position (anchor point) of the plane
-   gpos_ = gpos;
+   setPosition(gpos);
 
    // Calculating the orientation and rotation
    // The default normal of a plane is <0,0,1>. The rotation of the plane is calculated
    // as the rotation of this default normal to the specified normal.
    if( normal[0]*normal[0] + normal[1]*normal[1] < math::Limits<real_t>::accuracy() )
-      q_ = Quat( Vec3( 1, 0, 0 ), std::acos( normal[2] ) );
+      setOrientation( Quat(Vec3( 1, 0, 0 ), std::acos( normal[2] ) ) );
    else
-      q_ = Quat( Vec3( -normal[1], normal[0], real_c(0) ), std::acos( normal[2] ) );
-
-   R_ = q_.toRotationMatrix();
+      setOrientation( Quat( Vec3( -normal[1], normal[0], real_c(0) ), std::acos( normal[2] ) ) );
 
    // Setting the axis-aligned bounding box
    Plane::calcBoundingBox();
@@ -175,11 +173,8 @@ void Plane::calcBoundingBox()
  */
 void Plane::setPositionImpl( real_t px, real_t py, real_t pz )
 {
-   gpos_[0] = px;
-   gpos_[1] = py;
-   gpos_[2] = pz;
-   d_ = normal_ * gpos_;
-
+   RigidBody::setPositionImpl(px,py,pz);
+   d_ = normal_ * getPosition();
    Plane::calcBoundingBox();    // Updating the axis-aligned bounding box of the plane
 #if MOBILE_INFINITE
    wake();               // Waking the box from sleep mode
@@ -210,16 +205,15 @@ void Plane::setPositionImpl( real_t px, real_t py, real_t pz )
  */
 void Plane::setOrientationImpl( real_t r, real_t i, real_t j, real_t k )
 {
-   q_.set(r, i, j, k);          // Updating the orientation of the plane
-   R_ = q_.toRotationMatrix();  // Updating the rotation of the plane
-
+   RigidBody::setOrientationImpl(r,i,j,k);
+   Mat3 R = getRotation();
    // Updating the normal of the plane ( R * <0,0,1> )
-   normal_[0] = R_[2];
-   normal_[1] = R_[5];
-   normal_[2] = R_[8];
+   normal_[0] = R[2];
+   normal_[1] = R[5];
+   normal_[2] = R[8];
 
    // Updating the displacement from the origin
-   d_ = normal_ * gpos_;
+   d_ = normal_ * getPosition();
 
    Plane::calcBoundingBox();  // Updating the axis-aligned bounding box of the plane
 #if MOBILE_INFINITE
@@ -256,10 +250,9 @@ void Plane::setOrientationImpl( real_t r, real_t i, real_t j, real_t k )
  */
 void Plane::translateImpl( real_t dx, real_t dy, real_t dz )
 {
-   gpos_[0] += dx;
-   gpos_[1] += dy;
-   gpos_[2] += dz;
-   d_ = normal_ * gpos_;
+
+   setPosition(getPosition() + Vec3(dx,dy,dz));
+   d_ = normal_ * getPosition();
 
    Plane::calcBoundingBox();    // Updating the axis-aligned bounding box
 #if MOBILE_INFINITE
@@ -301,16 +294,16 @@ void Plane::rotateImpl( const Quat& dq )
 //   if( ExclusiveSection::isActive() )
 //      throw std::logic_error( "Invalid rotation of a plane inside an exclusive section" );
 
-   q_ = dq * q_;                // Updating the orientation of the plane
-   R_ = q_.toRotationMatrix();  // Updating the rotation of the plane
+   setOrientation(dq * getQuaternion());
 
    // Updating the normal of the plane ( R * <0,0,1> )
-   normal_[0] = R_[2];
-   normal_[1] = R_[5];
-   normal_[2] = R_[8];
+   Mat3 R = getRotation();
+   normal_[0] = R[2];
+   normal_[1] = R[5];
+   normal_[2] = R[8];
 
    // Updating the displacement from the origin
-   d_ = normal_ * gpos_;
+   d_ = normal_ * getPosition();
 
    Plane::calcBoundingBox();  // Updating the axis-aligned bounding box of the plane
 #if MOBILE_INFINITE
@@ -342,14 +335,16 @@ void Plane::rotateImpl( const Quat& dq )
  */
 void Plane::rotateAroundOriginImpl( const Quat& dq )
 {
-   gpos_ = dq.rotate( gpos_ );     // Updating the global position of the plane
-   q_    = dq * q_;                // Updating the orientation of the plane
-   R_    = q_.toRotationMatrix();  // Updating the rotation of the plane
+
+   setPosition(dq.rotate( getPosition() ));     // Updating the global position of the plane
+   setOrientation(dq * getQuaternion());                // Updating the orientation of the plane
+
 
    // Updating the normal of the plane ( R * <0,0,1> )
-   normal_[0] = R_[2];
-   normal_[1] = R_[5];
-   normal_[2] = R_[8];
+   Mat3 R = getRotation();
+   normal_[0] = R[2];
+   normal_[1] = R[5];
+   normal_[2] = R[8];
 
    Plane::calcBoundingBox();  // Updating the axis-aligned bounding box of the plane
    signalRotation();   // Signaling the change of orientation to the superordinate body
@@ -382,19 +377,19 @@ void Plane::rotateAroundOriginImpl( const Quat& dq )
  */
 void Plane::rotateAroundPointImpl( const Vec3& point, const Quat& dq )
 {
-   const Vec3 dp( gpos_ - point );
+   const Vec3 dp( getPosition() - point );
 
-   gpos_ = point + dq.rotate( dp );  // Updating the global position of the box
-   q_    = dq * q_;                  // Updating the orientation of the box
-   R_    = q_.toRotationMatrix();    // Updating the rotation of the box
+   setPosition(point + dq.rotate( dp ) );  // Updating the global position of the box
+   setOrientation(dq * getQuaternion());   // Updating the orientation of the box
 
    // Updating the normal of the plane ( R * <0,0,1> )
-   normal_[0] = R_[2];
-   normal_[1] = R_[5];
-   normal_[2] = R_[8];
+   Mat3 R = getRotation();
+   normal_[0] = R[2];
+   normal_[1] = R[5];
+   normal_[2] = R[8];
 
    // Updating the displacement from the origin
-   d_ = normal_ * gpos_;
+   d_ = normal_ * getPosition();
 
    Plane::calcBoundingBox();  // Updating the axis-aligned bounding box of the plane
 #if MOBILE_INFINITE
@@ -447,85 +442,6 @@ bool Plane::isSurfaceRelPointImpl( real_t /*px*/, real_t /*py*/, real_t pz ) con
 //*************************************************************************************************
 
 
-
-
-
-//=================================================================================================
-//
-//  SIMULATION FUNCTIONS
-//
-//=================================================================================================
-
-//*************************************************************************************************
-/*!\brief Translation update of a subordinate plane.
- *
- * \param dp Change in the global position of the superordinate body.
- * \return void
- *
- * This update function is triggered by the superordinate body in case of a translational
- * movement. This movement involves a change of the global position, the displacement from
- * the origin and the axis-aligned bounding box.
- */
-void Plane::update( const Vec3& dp )
-{
-   // Checking the state of the plane
-   WALBERLA_ASSERT( checkInvariants(), "Invalid plane state detected" );
-   WALBERLA_ASSERT( hasSuperBody(), "Invalid superordinate body detected" );
-
-   // Updating the global position and the displacement
-   gpos_ += dp;
-   d_ = normal_ * gpos_;
-
-   // Setting the axis-aligned bounding box
-   Plane::calcBoundingBox();
-
-   // Checking the state of the plane
-   WALBERLA_ASSERT( checkInvariants(), "Invalid plane state detected" );
-}
-//*************************************************************************************************
-
-
-//*************************************************************************************************
-/*!\brief Rotation update of a subordinate plane.
- *
- * \param dq Change in the orientation of the superordinate body.
- * \return void
- *
- * This update function is triggered by the superordinate body in case of a rotational
- * movement. This movement involves a change in the global position, the displacement from
- * the origin, the orientation/rotation, the normal and the axis-aligned bounding box of
- * the plane.
- */
-void Plane::update( const Quat& dq )
-{
-   // Checking the state of the plane
-   WALBERLA_ASSERT( checkInvariants(), "Invalid plane state detected" );
-   WALBERLA_ASSERT( hasSuperBody(), "Invalid superordinate body detected" );
-
-   // Calculating the new orientation and rotation
-   q_ = dq * q_;
-   R_ = q_.toRotationMatrix();
-
-   // Updating the normal of the plane ( R * <0,0,1> )
-   normal_[0] = R_[2];
-   normal_[1] = R_[5];
-   normal_[2] = R_[8];
-
-   // Updating the global position and the displacement
-   gpos_ = sb_->getPosition() + ( sb_->getRotation() * rpos_ );
-   d_    = normal_ * gpos_;
-
-   // Setting the axis-aligned bounding box
-   Plane::calcBoundingBox();
-
-   // Checking the state of the plane
-   WALBERLA_ASSERT( checkInvariants(), "Invalid plane state detected" );
-}
-//*************************************************************************************************
-
-
-
-
 //=================================================================================================
 //
 //  OUTPUT FUNCTIONS
@@ -546,14 +462,15 @@ void Plane::print( std::ostream& os, const char* tab ) const
    os << tab << " Plane " << uid_ << " with normal " << normal_ << " and displacement " << d_ << "\n"
       << tab << "   System ID         = " << sid_ << "\n"
       << tab << "   Material          = " << Material::getName( material_ ) << "\n"
-      << tab << "   Global position   = " << gpos_ << "\n";
+      << tab << "   Global position   = " << getPosition() << "\n";
 
-   os << tab << "   Relative position = " << rpos_ << "\n"
+   os << tab << "   Relative position = " << getRelPosition() << "\n"
       << tab << "   Bounding box      = " << aabb_ << "\n"
-      << tab << "   Quaternion        = " << q_ << "\n"
-      << tab << "   Rotation matrix   = ( " << setw(9) << R_[0] << " , " << setw(9) << R_[1] << " , " << setw(9) << R_[2] << " )\n"
-      << tab << "                       ( " << setw(9) << R_[3] << " , " << setw(9) << R_[4] << " , " << setw(9) << R_[5] << " )\n"
-      << tab << "                       ( " << setw(9) << R_[6] << " , " << setw(9) << R_[7] << " , " << setw(9) << R_[8] << " )\n";
+      << tab << "   Quaternion        = " << getQuaternion()<< "\n";
+   Mat3 R = getRotation();
+   os << tab << "   Rotation matrix   = ( " << setw(9) << R[0] << " , " << setw(9) << R[1] << " , " << setw(9) << R[2] << " )\n"
+      << tab << "                       ( " << setw(9) << R[3] << " , " << setw(9) << R[4] << " , " << setw(9) << R[5] << " )\n"
+      << tab << "                       ( " << setw(9) << R[6] << " , " << setw(9) << R[7] << " , " << setw(9) << R[8] << " )\n";
 }
 //*************************************************************************************************
 
diff --git a/src/pe/rigidbody/Plane.h b/src/pe/rigidbody/Plane.h
index 589d11038aa1566861f49f866233901f1ff35703..94368f79fcba13971c0541e1bc8f60cbc848b4bf 100644
--- a/src/pe/rigidbody/Plane.h
+++ b/src/pe/rigidbody/Plane.h
@@ -138,14 +138,6 @@ protected:
    //@}
    //**********************************************************************************************
 
-   //**Simulation functions************************************************************************
-   /*!\name Simulation functions */
-   //@{
-   virtual void update( const Vec3& dp );  // Translation update of a subordinate plane
-   virtual void update( const Quat& dq );  // Rotation update of a subordinate plane
-   //@}
-   //**********************************************************************************************
-
    //**Member variables****************************************************************************
    /*!\name Member variables */
    //@{
diff --git a/src/pe/rigidbody/RigidBody.cpp b/src/pe/rigidbody/RigidBody.cpp
index f30f35ce6c244393433217aafb54e914a2f79223..78743f7969e7efbba1bff7d16ffb180cbd383df6 100644
--- a/src/pe/rigidbody/RigidBody.cpp
+++ b/src/pe/rigidbody/RigidBody.cpp
@@ -37,17 +37,13 @@ RigidBody::RigidBody( id_t const typeID, id_t sid, id_t uid )
    , mass_( 0 )               // Total mass of the rigid body
    , invMass_( 0 )            // Inverse total mass of the rigid body
    , motion_(sleepThreshold)  // The current motion of the rigid body
-   , gpos_()                  // Global position of the center of mass
-   , rpos_()                  // Relative position within the body frame of the superordinate body
    , v_()                     // Linear velocity
    , w_()                     // Angular velocity
    , force_()                 // Total force
    , torque_()                // Total torque
    , I_( real_c(0) )          // Moment of inertia
    , Iinv_( real_c(0) )       // Inverse moment of inertia
-   , q_()                     // Orientation of the body frame
-   , R_()                     // Rigid body rotation
-   , manager_(nullptr)              // The rigid body manager responsible for the rigid body
+   , manager_(nullptr)        // The rigid body manager responsible for the rigid body
    , finite_ (true)           // Finiteness flag
    , visible_(true)           // Visibility flag
    , remote_ (false)          // Remote flag
@@ -56,6 +52,9 @@ RigidBody::RigidBody( id_t const typeID, id_t sid, id_t uid )
    , toBeDeleted_(false)      // deletion flag
    , sid_    (sid)            // System-specific body index
    , uid_    (uid)            // User-specific body ID
+   , gpos_()                  // Global position of the center of mass
+   , q_()                     // Orientation of the body frame
+   , R_()                     // Rigid body rotation
    , typeID_(typeID)          // geometry type
 {
    sb_ = this;           // The superordinate rigid body
diff --git a/src/pe/rigidbody/RigidBody.h b/src/pe/rigidbody/RigidBody.h
index d987ca4e73fcbb1119489213b3008250f81df125..1ac988c750b02d95ce5e552f048c941907e2dcd8 100644
--- a/src/pe/rigidbody/RigidBody.h
+++ b/src/pe/rigidbody/RigidBody.h
@@ -98,14 +98,15 @@ public:
    inline bool           isMarkedForDeletion() const { return toBeDeleted_; }
    inline id_t           getSystemID()       const;
    inline id_t           getID()             const;
-   inline const Vec3&    getRelPosition()    const;
-   inline const Vec3&    getPosition()       const;
-   inline const Vec3     getRelLinearVel()   const;
-   inline const Vec3&    getLinearVel()      const;
-   inline const Vec3     getRelAngularVel()  const;
-   inline const Vec3&    getAngularVel()     const;
-   inline const Quat&    getQuaternion()     const;
-   inline const Mat3&    getRotation()       const;
+   inline const Vec3     getRelPosition()    const;
+   inline const Vec3     getPosition()       const;
+   inline const Vec3     getBodyLinearVel()   const;
+   inline const Vec3     getLinearVel()      const;
+   inline const Vec3     getBodyAngularVel()  const;
+   inline const Vec3 &   getAngularVel()     const;
+   inline const Quat     getRelQuaternion()  const;
+   inline const Quat     getQuaternion()     const;
+   inline const Mat3     getRotation()       const;
    inline real_t         getMass()           const;
    inline real_t         getInvMass()        const;
    virtual inline real_t getVolume()         const;
@@ -149,8 +150,10 @@ public:
    inline  void setVisible    ( bool visible );
    inline  void setPosition   ( real_t px, real_t py, real_t pz );
    inline  void setPosition   ( const Vec3& gpos );
+   inline  void setRelPosition( const Vec3& gpos );
    inline  void setOrientation( real_t r, real_t i, real_t j, real_t k );
    inline  void setOrientation( const Quat& q );
+   inline  void setRelOrientation( const Quat& q );
    inline  void setMassAndInertiaToInfinity();
 
    inline void setRelLinearVel ( real_t vx, real_t vy, real_t vz );
@@ -309,8 +312,6 @@ protected:
    virtual bool isSurfaceRelPointImpl ( real_t px, real_t py, real_t pz ) const;
 
    inline  void setMassAndInertia     ( const real_t mass, const Mat3& inertia );
-
-   inline void calcRelPosition();
    //@}
    //**********************************************************************************************
 
@@ -321,14 +322,6 @@ protected:
    //@}
    //**********************************************************************************************
 
-   //**Simulation functions************************************************************************
-   /*!\name Simulation functions */
-   //@{
-   virtual void update( const Vec3& dp );  // Translation update of a subordinate rigid body
-   virtual void update( const Quat& dq );  // Rotation update of a subordinate rigid body
-   //@}
-   //**********************************************************************************************
-
    //**Functions for internal changes in compound geometries***************************************
    /*!\name Functions for internal changes in compound geometries */
    //@{
@@ -357,12 +350,7 @@ protected:
    real_t motion_;   //!< The current motion of the rigid body.
                   /*!< If this value drops under the specified sleep threshold, the
                        rigid body will be put to sleep. */
-   Vec3 gpos_;       //!< The global position of the center of mass of this rigid body.
-   Vec3 rpos_;       //!< The relative position within the body frame of the superordinate body.
-                  /*!< If the body is contained within a superordinate Union the relative
-                       position gives the position of the body's center of mass within the
-                       body frame of the superordinate body. If the body is not contained
-                       in a Union, the relative position is 0. */
+
    mutable Vec3 v_;  //!< The linear velocity of this rigid body.
    mutable Vec3 w_;  //!< Angular velocity of this rigid body.
    Vec3 force_;      //!< Total force (external+contact) acting in the body's center of mass.
@@ -379,8 +367,7 @@ protected:
                                       I_{zx} & I_{zy} & I_{zz} \\
                                       \end{array}\right)\f] */
    Mat3 Iinv_;       //!< The inverse moment of inertia within the body frame.
-   Quat q_;          //!< The orientation of the body frame in the global world frame.
-   Mat3 R_;          //!< The rotation in reference to the global frame of reference.
+
    //@}
    //**********************************************************************************************
 
@@ -390,6 +377,7 @@ protected:
    BodyID sb_;                //!< The superordinate rigid body.
                               /*!< This data member is the connection to the superordinate body,
                                    which is either the enclosing Union or the rigid body itself. */
+
    bool finite_;              //!< Finiteness flag.
                               /*!< The flag value indicates if the rigid body is finite (\a true)
                                    or infinite (\a false). */
@@ -417,7 +405,15 @@ protected:
    AABB aabb_;  //!< Axis-aligned bounding box for the rigid body.
    //@}
    //**********************************************************************************************
+
 private:
+
+   Vec3 gpos_;       /*!< The position of the center of mass of this rigid body. If the body is contained in union
+                          this and the other properties are relative to its center / orientation. Use the respective function
+                          getPosition() / getRotation() to access the actual global Position in the world frame */
+   Quat q_;          //!< The orientation of the body frame in the global world frame.
+   Mat3 R_;          //!< The rotation in reference to the global frame of reference.
+
    id_t typeID_; //< identify geometry type
 };
 
@@ -694,29 +690,38 @@ inline id_t RigidBody::getID() const
 //*************************************************************************************************
 
 
+
+
+
 //*************************************************************************************************
-/*!\brief Returns the relative position of the rigid body within the superordinate body.
- *
- * \return The relative position of the rigid body.
+/*!\brief Returns the global position of the center of mass of the rigid body.
  *
- * If the rigid body is not contained in a superordinate body, the returned relative position will
- * be \f$ \left(\begin{array}{*{3}{c}} 0 & 0 & 0 \end{array}\right) \f$.
+ * \return The global position of the center of mass.
  */
-inline const Vec3& RigidBody::getRelPosition() const
+inline const Vec3 RigidBody::getPosition() const
 {
-   return rpos_;
+   if( !hasSuperBody() ){
+      return gpos_;
+   }
+   return sb_->getPosition() + sb_->getRotation() * getRelPosition();
+
 }
 //*************************************************************************************************
 
-
 //*************************************************************************************************
-/*!\brief Returns the global position of the center of mass of the rigid body.
+/*!\brief Returns the relative position of the rigid body within the superordinate body.
  *
- * \return The global position of the center of mass.
+ * \return The relative position of the rigid body.
+ *
+ * If this body is not containted in a superbody the zero-vector is returned.
  */
-inline const Vec3& RigidBody::getPosition() const
+inline const Vec3 RigidBody::getRelPosition() const
 {
-   return gpos_;
+   if( hasSuperBody() ){
+      return gpos_;
+   }
+   return Vec3(0, 0, 0);
+
 }
 //*************************************************************************************************
 
@@ -729,11 +734,9 @@ inline const Vec3& RigidBody::getPosition() const
  * This function returns the linear velocity of the center of mass of the rigid body in reference
  * to the body's own frame of reference.
  */
-inline const Vec3 RigidBody::getRelLinearVel() const
+inline const Vec3 RigidBody::getBodyLinearVel() const
 {
-   if( hasSuperBody() )
-      v_ = sb_->velFromWF( gpos_ );
-   return R_.getTranspose() * v_;
+   return getRotation().getTranspose() * getLinearVel();
 }
 //*************************************************************************************************
 
@@ -746,11 +749,13 @@ inline const Vec3 RigidBody::getRelLinearVel() const
  * This function returns the linear velocity of the center of mass of the rigid body in reference
  * to the global world frame.
  */
-inline const Vec3& RigidBody::getLinearVel() const
+inline const Vec3 RigidBody::getLinearVel() const
 {
-   if( hasSuperBody() )
-      v_ = sb_->velFromWF( gpos_ );
+   if( hasSuperBody() ){
+      return sb_->getLinearVel() + (sb_->getAngularVel() % (getPosition() - sb_->getPosition()));
+   }
    return v_;
+
 }
 //*************************************************************************************************
 
@@ -760,14 +765,12 @@ inline const Vec3& RigidBody::getLinearVel() const
  *
  * \return The relative angular velocity.
  *
- * This function returns the angluar velocity of the center of mass in reference to the body's
+ * This function returns the angular velocity of the center of mass in reference to the body's
  * own frame of reference.
  */
-inline const Vec3 RigidBody::getRelAngularVel() const
+inline const Vec3 RigidBody::getBodyAngularVel() const
 {
-   if( hasSuperBody() )
-      w_ = sb_->getAngularVel();
-   return R_.getTranspose() * w_;
+   return getRotation().getTranspose() * getAngularVel();
 }
 //*************************************************************************************************
 
@@ -780,10 +783,11 @@ inline const Vec3 RigidBody::getRelAngularVel() const
  * This function returns the angluar velocity of the center of mass in reference to the global
  * world frame.
  */
-inline const Vec3& RigidBody::getAngularVel() const
+inline const Vec3 & RigidBody::getAngularVel() const
 {
-   if( hasSuperBody() )
-      w_ = sb_->getAngularVel();
+   if( hasSuperBody() ) {
+      return sb_->getAngularVel();
+   }
    return w_;
 }
 //*************************************************************************************************
@@ -797,13 +801,34 @@ inline const Vec3& RigidBody::getAngularVel() const
  * This function returns the quaternion of the rigid body, which represents the orientation of
  * the body in reference to the global world frame.
  */
-inline const Quat& RigidBody::getQuaternion() const
+inline const Quat RigidBody::getQuaternion() const
 {
+   if( hasSuperBody() ){
+      return getRelQuaternion() * sb_->getQuaternion();
+   }
    return q_;
 }
 //*************************************************************************************************
 
 
+//*************************************************************************************************
+/*!\brief Returns the relative orientation of the rigid body to its superbody.
+ *
+ * \return The relative orientation of the rigid body.
+ *
+ * This function returns the quaternion, which represents the orientation of
+ * the body in reference to coordinate system of the superbody. If this body has no superbody
+ * the union-quaternion is returned.
+ */
+inline const Quat RigidBody::getRelQuaternion() const
+{
+   if( hasSuperBody() ){
+      return q_;
+   }
+   return Quat();
+}
+//*************************************************************************************************
+
 //*************************************************************************************************
 /*!\brief Returns the rotation of the rigid body.
  *
@@ -812,8 +837,11 @@ inline const Quat& RigidBody::getQuaternion() const
  * This function returns the rotation matrix of the rigid body, which represents the rotation of
  * the body in reference to the global world frame.
  */
-inline const Mat3& RigidBody::getRotation() const
+inline const Mat3 RigidBody::getRotation() const
 {
+   if( hasSuperBody() ) {
+      return getQuaternion().toRotationMatrix();
+   }
    return R_;
 }
 //*************************************************************************************************
@@ -874,7 +902,7 @@ inline const Mat3& RigidBody::getBodyInertia() const
  */
 inline const Mat3 RigidBody::getInertia() const
 {
-   return math::transformMatrixRART(R_, I_);
+   return math::transformMatrixRART(getRotation(), I_);
 }
 //*************************************************************************************************
 
@@ -898,7 +926,7 @@ inline const Mat3& RigidBody::getInvBodyInertia() const
  */
 inline const Mat3 RigidBody::getInvInertia() const
 {
-   return math::transformMatrixRART(R_, Iinv_);
+   return math::transformMatrixRART(getRotation(), Iinv_);
 }
 //*************************************************************************************************
 
@@ -941,6 +969,7 @@ inline real_t   RigidBody::getKineticEnergy()      const
    return real_c(0.5) * getMass() * getLinearVel() * getLinearVel();
 }
 //*************************************************************************************************
+
 //*************************************************************************************************
 /*!\brief Returns the rotational energy of the rigid body
  *
@@ -951,6 +980,7 @@ inline real_t   RigidBody::getRotationalEnergy()      const
    return real_c(0.5) * getAngularVel() * (getInertia() * getAngularVel());
 }
 //*************************************************************************************************
+
 //*************************************************************************************************
 /*!\brief Returns the energy of the rigid body
  *
@@ -976,7 +1006,7 @@ inline real_t   RigidBody::getEnergy()      const
  */
 inline const Vec3 RigidBody::vectorFromBFtoWF( real_t vx, real_t vy, real_t vz ) const
 {
-   return R_ * Vec3( vx, vy, vz );
+   return getRotation() * Vec3( vx, vy, vz );
 }
 //*************************************************************************************************
 
@@ -992,7 +1022,7 @@ inline const Vec3 RigidBody::vectorFromBFtoWF( real_t vx, real_t vy, real_t vz )
  */
 inline const Vec3 RigidBody::vectorFromBFtoWF( const Vec3& v ) const
 {
-   return R_ * v;
+   return getRotation()  * v;
 }
 //*************************************************************************************************
 
@@ -1010,7 +1040,7 @@ inline const Vec3 RigidBody::vectorFromBFtoWF( const Vec3& v ) const
  */
 inline const Vec3 RigidBody::pointFromBFtoWF( real_t px, real_t py, real_t pz ) const
 {
-   return gpos_ + ( R_ * Vec3( px, py, pz ) );
+   return getPosition() + ( getRotation() * Vec3( px, py, pz ) );
 }
 //*************************************************************************************************
 
@@ -1026,7 +1056,7 @@ inline const Vec3 RigidBody::pointFromBFtoWF( real_t px, real_t py, real_t pz )
  */
 inline const Vec3 RigidBody::pointFromBFtoWF( const Vec3& rpos ) const
 {
-   return gpos_ + ( R_ * rpos );
+   return getPosition() + ( getRotation() * rpos );
 }
 //*************************************************************************************************
 
@@ -1044,7 +1074,7 @@ inline const Vec3 RigidBody::pointFromBFtoWF( const Vec3& rpos ) const
  */
 inline const Vec3 RigidBody::vectorFromWFtoBF( real_t vx, real_t vy, real_t vz ) const
 {
-   return R_.getTranspose() * Vec3( vx, vy, vz );
+   return getRotation() .getTranspose() * Vec3( vx, vy, vz );
 }
 //*************************************************************************************************
 
@@ -1060,7 +1090,7 @@ inline const Vec3 RigidBody::vectorFromWFtoBF( real_t vx, real_t vy, real_t vz )
  */
 inline const Vec3 RigidBody::vectorFromWFtoBF( const Vec3& v ) const
 {
-   return R_.getTranspose() * v;
+   return getRotation() .getTranspose() * v;
 }
 //*************************************************************************************************
 
@@ -1078,7 +1108,7 @@ inline const Vec3 RigidBody::vectorFromWFtoBF( const Vec3& v ) const
  */
 inline const Vec3 RigidBody::pointFromWFtoBF( real_t px, real_t py, real_t pz ) const
 {
-   return R_.getTranspose() * ( Vec3( px, py, pz ) - gpos_ );
+   return getRotation().getTranspose() * ( Vec3( px, py, pz ) - getPosition() );
 }
 //*************************************************************************************************
 
@@ -1094,7 +1124,7 @@ inline const Vec3 RigidBody::pointFromWFtoBF( real_t px, real_t py, real_t pz )
  */
 inline const Vec3 RigidBody::pointFromWFtoBF( const Vec3& gpos ) const
 {
-   return R_.getTranspose() * ( gpos - gpos_ );
+   return getRotation().getTranspose() * ( gpos - getPosition() );
 }
 //*************************************************************************************************
 
@@ -1279,23 +1309,6 @@ inline void RigidBody::setAngularVel( const Vec3& avel )
 //*************************************************************************************************
 
 
-//*************************************************************************************************
-/*!\brief Calculation of the relative position within a superordinate body.
- *
- * \return void
- *
- * The function calculates the relative position depending on its current global position, the
- * current global position of the superordinate body and the rotation of the superordinate body.
- */
-inline void RigidBody::calcRelPosition()
-{
-   rpos_ = sb_->R_.getTranspose()*( gpos_ - sb_->gpos_ );
-}
-//*************************************************************************************************
-
-
-
-
 //=================================================================================================
 //
 //  FORCE FUNCTIONS
@@ -1374,8 +1387,12 @@ inline  void RigidBody::resetSB()
 inline void RigidBody::setForce( const Vec3& f )
 {
    // Increasing the force on this rigid body
+   if(hasSuperBody()){
+      sb_->setForce(f);
+      sb_->setTorque((getPosition() - sb_->getPosition()) % f);
+      return;
+   }
    force_ = f;
-
    wake();
 }
 //*************************************************************************************************
@@ -1389,9 +1406,13 @@ inline void RigidBody::setForce( const Vec3& f )
  */
 inline void RigidBody::setTorque( const Vec3& tau )
 {
-   // Increasing the force on this rigid body
+   // Increasing the torque on this rigid body
+      // Increasing the force on this rigid body
+   if(hasSuperBody()){
+      sb_->setTorque(tau);
+      return;
+   }
    torque_ = tau;
-
    wake();
 }
 //*************************************************************************************************
@@ -1479,7 +1500,7 @@ inline void RigidBody::addForce( const Vec3& f )
       wake();
    } else
    {
-      sb_->addForceAtPos( f, gpos_ );
+      sb_->addForceAtPos( f, getPosition() );
    }
 }
 //*************************************************************************************************
@@ -2099,6 +2120,7 @@ inline void RigidBody::setVisible( bool visible )
 //*************************************************************************************************
 inline void RigidBody::setPositionImpl( real_t px, real_t py, real_t pz )
 {
+   if ( hasSuperBody() ) return;
    if (isFixed())
       WALBERLA_ABORT("Trying to move a fixed body: " << *this);
 
@@ -2112,6 +2134,36 @@ inline void RigidBody::setPositionImpl( real_t px, real_t py, real_t pz )
 }
 //*************************************************************************************************
 
+//*************************************************************************************************
+/*!\fn void RigidBody::setRelPosition( const Vec3 &gpos )
+ * \brief Setting the global position of the rigid body.
+ *
+ * \param gpos The relative position.
+ * \return void
+ *
+ * This function sets the relatvie position of the rigid body w. r. t. the superbodies position.
+ *
+ * \b Note:
+ * - Setting the position of a rigid body without superbody will have no effect.
+ * - In case of a <b>MPI parallel simulation</b>, changing the settings of a (local) rigid body
+ *   on one process may invalidate the settings of the body on another process. In order to
+ *   synchronize all rigid bodies after local changes, the simulation has to be synchronized
+ *   by the user. Note that any changes on remote rigid
+ *   bodies are neglected and overwritten by the settings of the rigid body on its local process!
+ */
+//*************************************************************************************************
+inline void RigidBody::setRelPosition(const Vec3 &gpos) {
+
+   if ( !hasSuperBody() ) return;
+   if (isFixed())
+   WALBERLA_ABORT("Trying to move a fixed body: " << *this);
+
+   gpos_ = gpos;
+   calcBoundingBox();    // Updating the axis-aligned bounding box of the box
+   wake();               // Waking the box from sleep mode
+}
+//*************************************************************************************************
+
 //*************************************************************************************************
 /*!\fn void RigidBody::setPosition( real_t px, real_t py, real_t pz )
  * \brief Setting the global position of the rigid body.
@@ -2194,6 +2246,7 @@ inline void RigidBody::setPosition( const Vec3& gpos )
 //*************************************************************************************************
 inline void RigidBody::setOrientationImpl( real_t r, real_t i, real_t j, real_t k )
 {
+   if ( hasSuperBody() ) return;
    if (isFixed())
       WALBERLA_ABORT("Trying to rotate a fixed body: " << *this);
 
@@ -2263,6 +2316,38 @@ inline void RigidBody::setOrientation( const Quat& q )
 }
 //*************************************************************************************************
 
+//*************************************************************************************************
+/*!\fn void RigidBody::setRelOrientation( const Quat &q )
+ * \brief Setting the relative position of the rigid body.
+ *
+ * \param q The relative orientation.
+ * \return void
+ *
+ * This function sets the relative orientation of the rigid body w. r. t. the superbodies position.
+ *
+ * \b Note:
+ * - Setting the relative orientation of a rigid body without superbody will have no effect.
+ * - In case of a <b>MPI parallel simulation</b>, changing the settings of a (local) rigid body
+ *   on one process may invalidate the settings of the body on another process. In order to
+ *   synchronize all rigid bodies after local changes, the simulation has to be synchronized
+ *   by the user. Note that any changes on remote rigid
+ *   bodies are neglected and overwritten by the settings of the rigid body on its local process!
+ */
+//*************************************************************************************************
+inline void RigidBody::setRelOrientation(const Quat &q) {
+
+   if ( !hasSuperBody() ) return;
+
+   if (isFixed())
+   WALBERLA_ABORT("Trying to move a fixed body: " << *this);
+
+   q_ = q;
+
+   calcBoundingBox();    // Updating the axis-aligned bounding box of the box
+   wake();               // Waking the box from sleep mode
+}
+//*************************************************************************************************
+
 //*************************************************************************************************
 /*!\fn void RigidBody::setMassAndInertiaToInfinity( )
  * \brief Setting the mass to infinity. This will also make the inertia tensor infinite.
@@ -2990,125 +3075,6 @@ inline void RigidBody::fix()
 //*************************************************************************************************
 
 
-//=================================================================================================
-//
-//  SIMULATION FUNCTIONS
-//
-//=================================================================================================
-
-//*************************************************************************************************
-/*!\brief Translation update of a subordinate rigid body.
- *
- * \param dp Change in the global position of the superordinate rigid body.
- * \return void
- *
- * This update function is triggered by the superordinate body in case of a translational
- * movement. This movement involves a change in the global position and the axis-aligned
- * bounding box.
- */
-inline void RigidBody::update( const Vec3& dp )
-{
-   // Checking the state of the sphere
-   WALBERLA_ASSERT( checkInvariants(), "Invalid sphere state detected" );
-   WALBERLA_ASSERT( hasSuperBody(), "Invalid superordinate body detected" );
-
-   // Updating the global position
-   gpos_ += dp;
-
-   // Setting the axis-aligned bounding box
-   calcBoundingBox();
-
-   // Checking the state of the sphere
-   WALBERLA_ASSERT( checkInvariants(), "Invalid sphere state detected" );
-}
-//*************************************************************************************************
-
-
-//*************************************************************************************************
-/*!\brief Rotation update of a subordinate rigid body.
- *
- * \param dq Change in the orientation of the superordinate rigid body.
- * \return void
- *
- * This update function is triggered by the superordinate body in case of a rotational movement.
- * This movement involves a change in the global position, the orientation/rotation and the
- * axis-aligned bounding box of the sphere.
- */
-inline void RigidBody::update( const Quat& dq )
-{
-   // Checking the state of the sphere
-   WALBERLA_ASSERT( checkInvariants(), "Invalid sphere state detected" );
-   WALBERLA_ASSERT( hasSuperBody(), "Invalid superordinate body detected" );
-
-   // Calculating the new global position
-   gpos_ = sb_->getPosition() + ( sb_->getRotation() * rpos_ );
-
-   // Calculating the new orientation and rotation
-   q_ = dq * q_;
-   R_ = q_.toRotationMatrix();
-
-   // Setting the axis-aligned bounding box
-   calcBoundingBox();
-
-   // Checking the state of the sphere
-   WALBERLA_ASSERT( checkInvariants(), "Invalid sphere state detected" );
-}
-//*************************************************************************************************
-
-
-
-
-//=================================================================================================
-//
-//  SIMULATION FUNCTIONS
-//
-//=================================================================================================
-
-//*************************************************************************************************
-/*!\fn void RigidBody::update( const Vec3& dp )
- * \brief Translation update of a subordinate rigid body.
- *
- * \param dp Change in the global position of the superordinate rigid body.
- * \return void
- *
- * This function calculates the necessary updates for a subordinate rigid body contained
- * in a superordinate rigid body that has performed a translational movement. This function
- * is triggered automatically by the superordinate body in case of a translational movement.
- * All classes deriving from the RigidBody class have to implement this function to update
- * the properties of the rigid body. The following properties of the body might change due
- * to this translation. All derived classes have to make sure these properties are updated
- * correctly:
- *
- *   - the global position
- *   - the axis-aligned bounding box
- */
-//*************************************************************************************************
-
-
-//*************************************************************************************************
-/*!\fn void RigidBody::update( const Quat& dq )
- * \brief Rotation update of a subordinate rigid body.
- *
- * \param dq Change in the orientation of the superordinate rigid body.
- * \return void
- *
- * This function calculates the necessary updates for a subordinate rigid body contained
- * in a superordinate rigid body that has performed a rotational movement. The function
- * is triggered automatically by the superordinate body in case of a rotational movement.
- * All classes deriving from the RigidBody class have to implement this function to update
- * the properties of the rigid body. The following properties of the body might change due
- * to this rotation. All derived classes have to make sure these properties are updated
- * correctly:
- *
- *   - the global position
- *   - the orientation/rotation (i.e. the quaterion and the rotation matrix)
- *   - the axis-aligned bounding box
- */
-//*************************************************************************************************
-
-
-
-
 //=================================================================================================
 //
 //  FUNCTIONS FOR INTERNAL CHANGES IN COMPOUND GEOMETRIES
diff --git a/src/pe/rigidbody/Sphere.cpp b/src/pe/rigidbody/Sphere.cpp
index 2d45389cb0ddfef29b3f48b41f9f0de44f689135..c3a76c6d8a10ba68e87efddc8be1293660c64fe2 100644
--- a/src/pe/rigidbody/Sphere.cpp
+++ b/src/pe/rigidbody/Sphere.cpp
@@ -50,7 +50,6 @@ namespace pe {
  * \param sid Unique system-specific ID for the sphere.
  * \param uid User-specific ID for the sphere.
  * \param gpos Global geometric center of the sphere.
- * \param rpos The relative position within the body frame of a superordinate body.
  * \param q The orientation of the sphere's body frame in the global world frame.
  * \param radius The radius of the sphere \f$ (0..\infty) \f$.
  * \param material The material of the sphere.
@@ -58,12 +57,13 @@ namespace pe {
  * \param communicating specifies if the sphere should take part in synchronization (syncNextNeighbour, syncShadowOwner)
  * \param infiniteMass specifies if the sphere has infinite mass and will be treated as an obstacle
  */
-Sphere::Sphere( id_t sid, id_t uid, const Vec3& gpos, const Vec3& rpos, const Quat& q,
+Sphere::Sphere( id_t sid, id_t uid, const Vec3& gpos, const Quat& q,
                 real_t radius, MaterialID material,
                 const bool global, const bool communicating, const bool infiniteMass )
-   : Sphere::Sphere( getStaticTypeID(), sid, uid, gpos, rpos, q, radius, material, global, communicating, infiniteMass )
+   : Sphere::Sphere( getStaticTypeID(), sid, uid, gpos, q, radius, material, global, communicating, infiniteMass )
 {}
-Sphere::Sphere( id_t const typeId, id_t sid, id_t uid, const Vec3& gpos, const Vec3& rpos, const Quat& q,
+
+Sphere::Sphere( id_t const typeId, id_t sid, id_t uid, const Vec3& gpos, const Quat& q,
                 real_t radius, MaterialID material,
                 const bool global, const bool communicating, const bool infiniteMass )
    : GeomPrimitive( typeId, sid, uid, material )  // Initialization of the parent class
@@ -76,12 +76,8 @@ Sphere::Sphere( id_t const typeId, id_t sid, id_t uid, const Vec3& gpos, const V
    WALBERLA_ASSERT( radius > real_c(0), "Invalid sphere radius" );
 
    // Setting the center of the sphere
-   gpos_ = gpos;
-
-   // Initializing the instantiated sphere
-   rpos_   = rpos;                   // Setting the relative position
-   q_      = q;                      // Setting the orientation
-   R_      = q_.toRotationMatrix();  // Setting the rotation matrix
+   setPosition(gpos);
+   setOrientation(q);
 
    setGlobal( global );
    if (infiniteMass)
@@ -196,11 +192,12 @@ void Sphere::print( std::ostream& os, const char* tab ) const
 
    //   if( verboseMode )
    //   {
+   Mat3 R = getRotation();
    os << tab << "   Bounding box      = " << getAABB() << "\n"
       << tab << "   Quaternion        = " << getQuaternion() << "\n"
-      << tab << "   Rotation matrix   = ( " << setw(9) << R_[0] << " , " << setw(9) << R_[1] << " , " << setw(9) << R_[2] << " )\n"
-      << tab << "                       ( " << setw(9) << R_[3] << " , " << setw(9) << R_[4] << " , " << setw(9) << R_[5] << " )\n"
-      << tab << "                       ( " << setw(9) << R_[6] << " , " << setw(9) << R_[7] << " , " << setw(9) << R_[8] << " )\n";
+      << tab << "   Rotation matrix   = ( " << setw(9) << R[0] << " , " << setw(9) << R[1] << " , " << setw(9) << R[2] << " )\n"
+      << tab << "                       ( " << setw(9) << R[3] << " , " << setw(9) << R[4] << " , " << setw(9) << R[5] << " )\n"
+      << tab << "                       ( " << setw(9) << R[6] << " , " << setw(9) << R[7] << " , " << setw(9) << R[8] << " )\n";
 
    os << std::setiosflags(std::ios::right)
       << tab << "   Moment of inertia = ( " << setw(9) << I_[0] << " , " << setw(9) << I_[1] << " , " << setw(9) << I_[2] << " )\n"
diff --git a/src/pe/rigidbody/Sphere.h b/src/pe/rigidbody/Sphere.h
index 8efc9bcaee38478d19e0795b0a8125d88d9c6416..c4a1c95fc710dbf10010730df7cb7ef6dbbaab53 100644
--- a/src/pe/rigidbody/Sphere.h
+++ b/src/pe/rigidbody/Sphere.h
@@ -63,10 +63,10 @@ public:
    //**Constructors********************************************************************************
    /*!\name Constructors */
    //@{
-   explicit Sphere( id_t sid, id_t uid, const Vec3& gpos, const Vec3& rpos, const Quat& q,
+   explicit Sphere( id_t sid, id_t uid, const Vec3& gpos, const Quat& q,
                     real_t radius, MaterialID material,
                     const bool global, const bool communicating, const bool infiniteMass );
-   explicit Sphere( id_t const typeID, id_t sid, id_t uid, const Vec3& gpos, const Vec3& rpos, const Quat& q,
+   explicit Sphere( id_t const typeID, id_t sid, id_t uid, const Vec3& gpos, const Quat& q,
                     real_t radius, MaterialID material,
                     const bool global, const bool communicating, const bool infiniteMass );
    //@}
@@ -115,7 +115,6 @@ public:
    /*!\name Utility functions */
    //@{
    inline virtual Vec3 support( const Vec3& d ) const;
-   inline virtual Vec3 supportContactThreshold( const Vec3& d ) const;
    //@}
    //**********************************************************************************************
 
@@ -266,18 +265,18 @@ inline real_t Sphere::calcDensity( real_t radius, real_t mass )
 inline void Sphere::calcBoundingBox()
 {
    const real_t length( radius_ + contactThreshold );
-
+   Vec3 gpos = getPosition();
    aabb_    = AABB(
-              gpos_[0] - length,
-              gpos_[1] - length,
-              gpos_[2] - length,
-              gpos_[0] + length,
-              gpos_[1] + length,
-              gpos_[2] + length
+              gpos[0] - length,
+              gpos[1] - length,
+              gpos[2] - length,
+              gpos[0] + length,
+              gpos[1] + length,
+              gpos[2] + length
    );
 
 //   WALBERLA_ASSERT( aabb_.isValid()        , "Invalid bounding box detected" );
-   WALBERLA_ASSERT( aabb_.contains( gpos_ ), "Invalid bounding box detected("<< getSystemID() <<")\n" << "pos: " << gpos_ << "\nlength: " << length << "\nvel: " << getLinearVel() << "\nbox: " << aabb_ );
+   WALBERLA_ASSERT( aabb_.contains( gpos ), "Invalid bounding box detected("<< getSystemID() <<")\n" << "pos: " << gpos << "\nlength: " << length << "\nvel: " << getLinearVel() << "\nbox: " << aabb_ );
 }
 //*************************************************************************************************
 
@@ -317,29 +316,6 @@ inline Vec3 Sphere::support( const Vec3& d ) const
 //*************************************************************************************************
 
 
-//*************************************************************************************************
-/*!\brief Estimates the point which is farthest in direction \a d.
- *
- * \param d The normalized search direction in world-frame coordinates
- * \return The support point in world-frame coordinates in direction a\ d extended by a vector in
- *         direction \a d of length \a pe::contactThreshold.
- */
-inline Vec3 Sphere::supportContactThreshold( const Vec3& d ) const
-{
-   auto len = d.sqrLength();
-   if (!math::equal(len, real_t(0)))
-   {
-      //WALBERLA_ASSERT_FLOAT_EQUAL( len, real_t(1), "search direction not normalized!");
-      const Vec3 s = getPosition() + (getRadius() / sqrt(len) + contactThreshold) * d;
-      //std::cout << "Support in direction " << d << " with center " << getPosition() << " (r=" << getRadius() << ") is " << s << std::endl;
-      return s;
-   } else
-   {
-      return Vec3(0,0,0);
-   }
-}
-//*************************************************************************************************
-
 //=================================================================================================
 //
 //  UTILITY FUNCTIONS
@@ -409,7 +385,7 @@ inline real_t Sphere::getDepth( real_t px, real_t py, real_t pz ) const
  */
 inline real_t Sphere::getDepth( const Vec3& gpos ) const
 {
-   return ( radius_ - ( gpos - gpos_ ).length() );
+   return ( radius_ - ( gpos - getPosition() ).length() );
 }
 //*************************************************************************************************
 
@@ -477,7 +453,7 @@ inline real_t Sphere::getDistance( real_t px, real_t py, real_t pz ) const
  */
 inline real_t Sphere::getDistance( const Vec3& gpos ) const
 {
-   return ( ( gpos - gpos_ ).length() - radius_ );
+   return ( ( gpos - getPosition() ).length() - radius_ );
 }
 //*************************************************************************************************
 
diff --git a/src/pe/rigidbody/SphereFactory.cpp b/src/pe/rigidbody/SphereFactory.cpp
index d88774c45f10803f84eaa1ac9f8fcd5a21fb91c6..dfe500daa4d1246a2fedb3de3efcf56b323f6f83 100644
--- a/src/pe/rigidbody/SphereFactory.cpp
+++ b/src/pe/rigidbody/SphereFactory.cpp
@@ -49,7 +49,7 @@ SphereID createSphere( BodyStorage& globalStorage, BlockStorage& blocks, BlockDa
       const id_t sid = UniqueID<RigidBody>::createGlobal();
       WALBERLA_ASSERT_EQUAL(communicating, false);
       WALBERLA_ASSERT_EQUAL(infiniteMass, true);
-      SpherePtr sp = std::make_unique<Sphere>(sid, uid, gpos, Vec3(0,0,0), Quat(), radius, material, global, false, true);
+      SpherePtr sp = std::make_unique<Sphere>(sid, uid, gpos, Quat(), radius, material, global, false, true);
       sphere = static_cast<SphereID>(&globalStorage.add( std::move(sp) ));
    } else
    {
@@ -59,7 +59,7 @@ SphereID createSphere( BodyStorage& globalStorage, BlockStorage& blocks, BlockDa
             const id_t sid( UniqueID<RigidBody>::create() );
 
             BodyStorage& bs = (*block.getData<Storage>(storageID))[0];
-            SpherePtr sp = std::make_unique<Sphere>(sid, uid, gpos, Vec3(0,0,0), Quat(), radius, material, global, communicating, infiniteMass);
+            SpherePtr sp = std::make_unique<Sphere>(sid, uid, gpos, Quat(), radius, material, global, communicating, infiniteMass);
             sp->MPITrait.setOwner(Owner(MPIManager::instance()->rank(), block.getId().getID()));
             sphere = static_cast<SphereID>(&bs.add( std::move(sp) ));
          }
diff --git a/src/pe/rigidbody/Squirmer.cpp b/src/pe/rigidbody/Squirmer.cpp
index 99f650b00290b619fa411c1a74f77a8c193c6200..e9d386c59140f8870a0862b195c6afaf5816748e 100644
--- a/src/pe/rigidbody/Squirmer.cpp
+++ b/src/pe/rigidbody/Squirmer.cpp
@@ -48,10 +48,10 @@ namespace pe {
  * \param communicating specifies if the sphere should take part in synchronization (syncNextNeighbour, syncShadowOwner)
  * \param infiniteMass specifies if the sphere has infinite mass and will be treated as an obstacle
  */
-Squirmer::Squirmer( id_t sid, id_t uid, const Vec3& gpos, const Vec3& rpos, const Quat& q,
+Squirmer::Squirmer( id_t sid, id_t uid, const Vec3& gpos, const Quat& q,
                     real_t radius, real_t squirmerVelocity, real_t squirmerBeta, MaterialID material,
                     const bool global, const bool communicating, const bool infiniteMass )
-   : Sphere( getStaticTypeID(), sid, uid, gpos, rpos, q, radius, material, global, communicating, infiniteMass )  // Initialization of the parent class
+   : Sphere( getStaticTypeID(), sid, uid, gpos, q, radius, material, global, communicating, infiniteMass )  // Initialization of the parent class
    , squirmerVelocity_(squirmerVelocity), squirmerBeta_(squirmerBeta)
 {
 }
@@ -106,7 +106,7 @@ const Vec3 Squirmer::velFromBF( real_t px, real_t py, real_t pz ) const
  */
 const Vec3 Squirmer::velFromBF( const Vec3& rpos ) const
 {
-   return Sphere::velFromBF( rpos ) + getSquirmerVelocity( R_ * rpos );
+   return Sphere::velFromBF( rpos ) + getSquirmerVelocity( getRotation() * rpos );
 }
 //*************************************************************************************************
 
@@ -154,7 +154,7 @@ const Vec3 Squirmer::getSquirmerVelocity( const Vec3& rpos ) const
  */
 const Vec3 Squirmer::velFromWF( const Vec3& gpos ) const
 {
-   return Sphere::velFromWF( gpos ) + getSquirmerVelocity( gpos - gpos_ );
+   return Sphere::velFromWF( gpos ) + getSquirmerVelocity( gpos - getPosition() );
 }
 //*************************************************************************************************
 
diff --git a/src/pe/rigidbody/Squirmer.h b/src/pe/rigidbody/Squirmer.h
index 5dcf205c4b850019653b2079b0b280c7a536c8d0..0d9514324a7f2f092af4e36a357fa44f5b6c3335 100644
--- a/src/pe/rigidbody/Squirmer.h
+++ b/src/pe/rigidbody/Squirmer.h
@@ -32,7 +32,7 @@ public:
    //**Constructors********************************************************************************
    /*!\name Constructors */
    //@{
-   explicit Squirmer( id_t sid, id_t uid, const Vec3& gpos, const Vec3& rpos, const Quat& q,
+   explicit Squirmer( id_t sid, id_t uid, const Vec3& gpos,  const Quat& q,
                     real_t radius, real_t squirmerVelocity, real_t squirmerBeta, MaterialID material,
                     const bool global, const bool communicating, const bool infiniteMass );
    //@}
diff --git a/src/pe/rigidbody/SquirmerFactory.cpp b/src/pe/rigidbody/SquirmerFactory.cpp
index 7160ff2d0fa80e72dcf28a5f3fe7445e0679999f..caac4c9a00e3290aeabca4144d2056f4ff32c72d 100644
--- a/src/pe/rigidbody/SquirmerFactory.cpp
+++ b/src/pe/rigidbody/SquirmerFactory.cpp
@@ -48,7 +48,7 @@ SquirmerID createSquirmer( BodyStorage& globalStorage, BlockStorage& blocks, Blo
       const id_t sid = UniqueID<RigidBody>::createGlobal();
       WALBERLA_ASSERT_EQUAL(communicating, false);
       WALBERLA_ASSERT_EQUAL(infiniteMass, true);
-      SquirmerPtr sq = std::make_unique<Squirmer>(sid, uid, gpos, Vec3(0,0,0), Quat(), radius, squirmerVelocity, squirmerBeta, material, global, false, true);
+      SquirmerPtr sq = std::make_unique<Squirmer>(sid, uid, gpos, Quat(), radius, squirmerVelocity, squirmerBeta, material, global, false, true);
       squirmer = static_cast<SquirmerID>(&globalStorage.add(std::move(sq)));
    } else
    {
@@ -58,7 +58,7 @@ SquirmerID createSquirmer( BodyStorage& globalStorage, BlockStorage& blocks, Blo
             const id_t sid( UniqueID<RigidBody>::create() );
 
             BodyStorage& bs = (*block.getData<Storage>(storageID))[0];
-            SquirmerPtr sq = std::make_unique<Squirmer>(sid, uid, gpos, Vec3(0,0,0), Quat(), radius, squirmerVelocity, squirmerBeta, material, global, communicating, infiniteMass);
+            SquirmerPtr sq = std::make_unique<Squirmer>(sid, uid, gpos, Quat(), radius, squirmerVelocity, squirmerBeta, material, global, communicating, infiniteMass);
             sq->MPITrait.setOwner(Owner(MPIManager::instance()->rank(), block.getId().getID()));
             squirmer = static_cast<SquirmerID>(&bs.add( std::move(sq) ));
          }
diff --git a/src/pe/rigidbody/Union.h b/src/pe/rigidbody/Union.h
index 3311851f5c74a6e18f1bed43c551ca02ec1e4d80..2f462877ffe5c05beb9d3e6f01268a9f693edae4 100644
--- a/src/pe/rigidbody/Union.h
+++ b/src/pe/rigidbody/Union.h
@@ -55,11 +55,11 @@ namespace pe {
 //*************************************************************************************************
 /**
  * \ingroup pe
- * \brief Base class for the sphere geometry.
+ * \brief Base class for the union geometry, a rigid assebly of bodies.
  *
- * The Sphere class represents the base class for the sphere geometry. It provides
- * the basic functionality of a sphere. For a full description of the sphere geometry,
- * see the Sphere class description.
+ * The Union class represents the base class for the sphere geometry.
+ * For a full description of the union geometry,
+ * see the Union class description.
  */
 template <typename... BodyTypes>
 class Union : public RigidBody
@@ -79,7 +79,7 @@ public:
    //**Constructors********************************************************************************
    /*!\name Constructors */
    //@{
-   explicit Union( id_t sid, id_t uid, const Vec3& gpos, const Vec3& rpos, const Quat& q,
+   explicit Union( id_t sid, id_t uid, const Vec3& gpos, const Quat& q,
                    const bool global, const bool communicating, const bool infiniteMass );
    //@}
    //**********************************************************************************************
@@ -132,14 +132,6 @@ public:
 
    virtual inline bool   hasSubBodies()      const WALBERLA_OVERRIDE { return true; }
 
-   //**Simulation functions************************************************************************
-   /*!\name Simulation functions */
-   //@{
-   virtual void update( const Vec3& dp ) WALBERLA_OVERRIDE;  // Translation update of a subordinate rigid body
-   virtual void update( const Quat& dq ) WALBERLA_OVERRIDE;  // Rotation update of a subordinate rigid body
-   //@}
-   //**********************************************************************************************
-
    //**Signal functions***************************************************************************
    /*!\name Signal functions */
    //@{
@@ -189,7 +181,7 @@ protected:
    /*!\name Utility functions */
    //@{
    inline virtual void calcBoundingBox() WALBERLA_OVERRIDE;  // Calculation of the axis-aligned bounding box
-   void calcCenterOfMass(); ///< updates the center of mass (gpos)
+   inline         void calcCenterOfMass(); // Compute mass and center of gravity
    inline         void calcInertia();      // Calculation of the moment of inertia
    //@}
    //**********************************************************************************************
@@ -228,15 +220,13 @@ private:
  * \param infiniteMass specifies if the sphere has infinite mass and will be treated as an obstacle
  */
 template <typename... BodyTypes>
-Union<BodyTypes...>::Union( id_t sid, id_t uid, const Vec3& gpos, const Vec3& rpos, const Quat& q,
+Union<BodyTypes...>::Union( id_t sid, id_t uid, const Vec3& gpos, const Quat& q,
                              const bool global, const bool communicating, const bool /*infiniteMass*/ )
    : RigidBody( getStaticTypeID(), sid, uid )  // Initialization of the parent class
 {
    // Initializing the instantiated union
-   gpos_   = gpos;                   // Setting the global center of mass
-   rpos_   = rpos;                   // Setting the relative position
-   q_      = q;                      // Setting the orientation
-   R_      = q_.toRotationMatrix();  // Setting the rotation matrix
+   setPosition(gpos);                // Setting the global center of mass
+   setOrientation(q);                // Setting the orientation
 
    calcCenterOfMass();
    calcInertia();
@@ -258,7 +248,7 @@ Union<BodyTypes...>::Union( id_t sid, id_t uid, const Vec3& gpos, const Vec3& rp
 //=================================================================================================
 
 //*************************************************************************************************
-/*!\brief Destructor for the Sphere class.
+/*!\brief Destructor for the Union class.
  */
 template <typename... BodyTypes>
 Union<BodyTypes...>::~Union()
@@ -266,7 +256,7 @@ Union<BodyTypes...>::~Union()
    // Clearing the bodies
    bodies_.clear();
 
-   // Logging the destruction of the sphere
+   // Logging the destruction of the union
    WALBERLA_LOG_DETAIL( "Destroyed union " << sid_ );
 }
 //*************************************************************************************************
@@ -321,27 +311,31 @@ template <typename... BodyTypes>
 void Union<BodyTypes...>::calcBoundingBox()
 {
    // Setting the bounding box of an empty union
+   Vec3 gpos = getPosition();
    if( bodies_.isEmpty() ) {
       aabb_ = math::AABB(
-                 gpos_[0] - real_t(0.01),
-            gpos_[1] - real_t(0.01),
-            gpos_[2] - real_t(0.01),
-            gpos_[0] + real_t(0.01),
-            gpos_[1] + real_t(0.01),
-            gpos_[2] + real_t(0.01)
+              gpos[0] - real_t(0.01),
+              gpos[1] - real_t(0.01),
+              gpos[2] - real_t(0.01),
+              gpos[0] + real_t(0.01),
+              gpos[1] + real_t(0.01),
+              gpos[2] + real_t(0.01)
             );
    }
 
    // Using the bounding box of the first contained bodies as initial bounding box
    // and merging it with the bounding boxes of all other bodies
    else {
+      bodies_.begin()->calcBoundingBox();
       aabb_ = bodies_.begin()->getAABB();
-      for( auto b=bodies_.begin()+1; b!=bodies_.end(); ++b )
-         aabb_.merge( b->getAABB() );
+      for( auto &b : bodies_ ){
+         b.calcBoundingBox();
+         aabb_.merge( b.getAABB() );
+      }
    }
 
    WALBERLA_ASSERT( aabb_.checkInvariant() , "Invalid bounding box detected" );
-   WALBERLA_ASSERT( aabb_.contains( gpos_ ), "Invalid bounding box detected" );
+   WALBERLA_ASSERT( aabb_.contains( getPosition() ), "Invalid bounding box detected" );
 }
 //*************************************************************************************************
 
@@ -364,64 +358,58 @@ void Union<BodyTypes...>::calcCenterOfMass()
    // Don't calculate the center of mass of an empty union
    if( bodies_.isEmpty() ) return;
 
-   // Calculating the center of mass of a single body
-   if( bodies_.size() == 1 )
-   {
-      const BodyID body( bodies_.begin().getBodyID() );
-      gpos_ = body->getPosition();
-      mass_ = body->getMass();
-      if( !isFixed() && mass_ > real_t(0) )
-         invMass_ = real_t(1) / mass_;
-   }
-
-   // Calculating the center of mass of a union containing several bodies
-   else
+   // Resetting the center of mass
+   Vec3 gpos = Vec3(0,0,0);
+   real_t mass = real_t(0.0);
+   // Calculating the center of mass of a finite union
+   if( finite_ )
    {
-      // Resetting the center of mass
-      gpos_.reset();
-
-      // Calculating the center of mass of a finite union
-      if( finite_ )
-      {
-         // Accumulating the mass of all contained rigid bodies
-         for( auto b=bodies_.begin(); b!=bodies_.end(); ++b ) {
-            WALBERLA_ASSERT( b->isFinite(), "Invalid infinite body in finite union detected" );
-            mass_ += b->getMass();
-            gpos_ += b->getPosition() * b->getMass();
-         }
-
-         // Calculating the center of mass for unions with non-zero mass
-         if( mass_ > real_t(0) ) {
-            if( !isFixed() ) invMass_ = real_t(1) / mass_;
-            gpos_ /= mass_;
-         }
-
-         // Calculating the center of mass for unions with a mass of zero
-         else {
-            size_t counter( 0 );
-            gpos_.reset();
+      // Accumulating the mass of all contained rigid bodies
+      for( auto &b : bodies_ ) {
+         WALBERLA_ASSERT( b.isFinite(), "Invalid infinite body in finite union detected" );
+         mass += b.getMass();
+         gpos += b.getPosition() * b.getMass();
+      }
 
-            for( auto b=bodies_.begin(); b!=bodies_.end(); ++b ) {
-               gpos_ += b->getPosition();
-               ++counter;
-            }
 
-            gpos_ /= counter;
-         }
+      mass_ = mass;
+      // Calculating the center of mass for unions with non-zero mass
+      if( mass > real_t(0) ) {
+         if( !isFixed() ) invMass_ = real_t(1) / mass;
+         gpos /= mass;
       }
 
-      // Calculating the center of mass of an infinite union
+         // Calculating the center of mass for unions with a mass of zero
       else {
          size_t counter( 0 );
 
-         for( auto b=bodies_.begin(); b!=bodies_.end(); ++b ) {
-            if( b->isFinite() ) continue;
-            gpos_ += b->getPosition();
+         for( auto &b : bodies_ ) {
+            gpos += b.getPosition();
             ++counter;
          }
 
-         gpos_ /= counter;
+         gpos /= counter;
+      }
+   }
+
+      // Calculating the center of mass of an infinite union
+   else {
+      size_t counter( 0 );
+
+      for( auto &b : bodies_ ) {
+         if( b.isFinite() ) continue;
+         gpos += b.getPosition();
+         ++counter;
       }
+
+      gpos /= counter;
+   }
+
+   // Set new center of mass and adapt all relative positions
+   Vec3 shift = getQuaternion().getInverse().rotate(gpos - getPosition());
+   gpos_ = gpos;
+   for( auto &b : bodies_) {
+      b.setRelPosition(b.getRelPosition() - shift);
    }
 
    // Checking the state of the union
@@ -430,8 +418,12 @@ void Union<BodyTypes...>::calcCenterOfMass()
 //*************************************************************************************************
 
 
+
 //*************************************************************************************************
 /*!\brief Calculation of the moment of inertia in reference to the body frame of the union.
+ *
+ * Use this function only if the center of gravity is set correctly (e.g. after calling
+ * calcCenterOfMass)
  *
  * \return void
  */
@@ -455,7 +447,7 @@ void Union<BodyTypes...>::calcInertia()
    for( auto b=bodies_.begin(); b!=bodies_.end(); ++b )
    {
       mass = b->getMass();
-      pos  = b->getPosition() - gpos_;
+      pos  = getRotation() * b->getRelPosition();
 
       I_ += b->getInertia();
       I_[0] += mass * ( pos[1]*pos[1] + pos[2]*pos[2] );
@@ -468,9 +460,8 @@ void Union<BodyTypes...>::calcInertia()
       I_[7] -= mass * pos[1] * pos[2];
       I_[8] += mass * ( pos[0]*pos[0] + pos[1]*pos[1] );
    }
-
    // Rotating the moment of inertia from the global frame of reference to the body frame of reference
-   I_ = R_ * I_ * R_;
+   I_ = getRotation().getTranspose() * I_ * getRotation();
 
    // Calculating the inverse of the body moment of inertia
    if( !isFixed() ) Iinv_ = I_.getInverse();
@@ -504,17 +495,7 @@ void Union<BodyTypes...>::calcInertia()
 template <typename... BodyTypes>
 void Union<BodyTypes...>::setPositionImpl( real_t px, real_t py, real_t pz )
 {
-   Vec3 gpos = Vec3( px, py, pz );
-
-   // Calculating the position change
-   const Vec3 dp( gpos - gpos_ );
-
-   // Setting the global position
-   gpos_ = gpos;
-
-   // Updating the contained bodies
-   for( auto& b : bodies_ )
-      b.update( dp );
+   gpos_ = Vec3( px, py, pz );
 
    Union<BodyTypes...>::calcBoundingBox();    // Setting the axis-aligned bounding box
    wake();               // Waking the union from sleep mode
@@ -542,16 +523,12 @@ void Union<BodyTypes...>::setPositionImpl( real_t px, real_t py, real_t pz )
 template <typename... BodyTypes>
 void Union<BodyTypes...>::setOrientationImpl( real_t r, real_t i, real_t j, real_t k )
 {
-   const Quat q ( r, i, j, k );
-   const Quat dq( q * q_.getInverse() );
+   if ( hasSuperBody() ) return;
 
+   const Quat q ( r, i, j, k );
    q_ = q;
    R_ = q_.toRotationMatrix();
 
-   // Updating the contained bodies
-   for( auto& b : bodies_ )
-      b.update( dq );
-
    Union<BodyTypes...>::calcBoundingBox();  // Setting the axis-aligned bounding box
    wake();             // Waking the union from sleep mode
    signalRotation();   // Signaling the change of orientation to the superordinate body
@@ -560,84 +537,6 @@ void Union<BodyTypes...>::setOrientationImpl( real_t r, real_t i, real_t j, real
 
 
 
-//=================================================================================================
-//
-//  SIMULATION FUNCTIONS
-//
-//=================================================================================================
-
-//*************************************************************************************************
-/*!\brief Translation update of a subordinate union.
- *
- * \param dp Change in the global position of the superordinate union.
- * \return void
- *
- * This update function is triggered by the superordinate body in case of a translational
- * movement. This movement involves a change in the global position and the axis-aligned
- * bounding box.
- */
-template <typename... BodyTypes>
-void Union<BodyTypes...>::update( const Vec3& dp )
-{
-   // Checking the state of the union
-   WALBERLA_ASSERT( checkInvariants(), "Invalid union state detected" );
-   WALBERLA_ASSERT( hasSuperBody(), "Invalid superordinate body detected" );
-
-   // Updating the global position
-   gpos_ += dp;
-
-   // Updating the contained bodies
-   for( auto& b : bodies_ )
-      b.update( dp );
-
-   // Setting the axis-aligned bounding box
-   Union<BodyTypes...>::calcBoundingBox();
-
-   // Checking the state of the union
-   WALBERLA_ASSERT( checkInvariants(), "Invalid union state detected" );
-}
-//*************************************************************************************************
-
-
-//*************************************************************************************************
-/*!\brief Rotation update of a subordinate union.
- *
- * \param dq Change in the orientation of the superordinate union.
- * \return void
- *
- * This update function is triggered by the superordinate body in case of a rotational movement.
- * This movement involves a change in the global position, the orientation/rotation and the
- * axis-aligned bounding box of the box.
- */
-template <typename... BodyTypes>
-void Union<BodyTypes...>::update( const Quat& dq )
-{
-   // Checking the state of the union
-   WALBERLA_ASSERT( checkInvariants(), "Invalid union state detected" );
-   WALBERLA_ASSERT( hasSuperBody(), "Invalid superordinate body detected" );
-
-   // Calculating the new global position
-   gpos_ = sb_->getPosition() + ( sb_->getRotation() * rpos_ );
-
-   // Calculating the new orientation and rotation
-   q_ = dq * q_;
-   R_ = q_.toRotationMatrix();
-
-   // Updating the contained bodies
-   for( auto& b : bodies_ )
-      b.update( dq );
-
-   // Setting the axis-aligned bounding box
-   Union<BodyTypes...>::calcBoundingBox();
-
-   // Checking the state of the union
-   WALBERLA_ASSERT( checkInvariants(), "Invalid union state detected" );
-}
-//*************************************************************************************************
-
-
-
-
 //=================================================================================================
 //
 //  RIGID BODY MANAGER FUNCTIONS
@@ -660,9 +559,9 @@ void Union<BodyTypes...>::update( const Quat& dq )
  * The union takes full responsibility for the newly added body, including the necessary
  * memory management. After adding the body to the union, the body is considered part of the
  * union. All functions called on the union (as for instance all kinds of set functions,
- * translation or rotation functions) additionally affect the rigid body. However, the body
- * can still individually be positioned, oriented, translated, rotated, or made (in-)visible
- * within the union.\n\n
+ * translation or rotation functions) additionally define the values for the rigid body
+ * according to its relative position and orientation. Do not call any Set-Functions on the
+ * single rigid body.\n
  *
  *
  * \section union_add_infinite Adding infinite rigid bodies
@@ -704,16 +603,23 @@ RigidBody& Union<BodyTypes...>::add( std::unique_ptr<RigidBody>&& body )
    if( body->isGlobal() ^ global_ )
       throw std::logic_error( "Global flags of body and union do not match" );
 
+   // Only add non-moving particles to a union.
+   WALBERLA_ASSERT_EQUAL(body->getLinearVel(), Vec3());
+   WALBERLA_ASSERT_EQUAL(body->getAngularVel(), Vec3());
+
    // Registering the rigid body
    auto& bd = bodies_.add( std::move(body) );
 
-   Vec3 oldCenterOfMass = getPosition();
-   Vec3 oldImpulse      = getLinearVel() * getMass();
+   Vec3 bdglobalPos = bd.getPosition();
+   Quat bdglobalRot = bd.getQuaternion();
 
-   Vec3 bodyCenterOfMass = bd.getPosition();
-   Vec3 bodyImpulse      = bd.getLinearVel() * bd.getMass();
+   bd.setSB(this);
+   //having a superbody will forward all getVel/Pos/Rot calls to the superbody from now !
 
-   bd.setSB(this); //having a superbody will forward all getVel calls to superbody!!!
+   bd.setRelPosition(getQuaternion().getInverse().rotate(bdglobalPos - getPosition()));
+   bd.setRelOrientation(bdglobalRot * getQuaternion().getInverse());
+   // Update mass, COG and relative positions
+   calcCenterOfMass();
 
    // Updating the axis-aligned bounding box
    if( bodies_.size() == 1 )
@@ -721,24 +627,10 @@ RigidBody& Union<BodyTypes...>::add( std::unique_ptr<RigidBody>&& body )
    else
       aabb_.merge( bd.getAABB() );
 
-   // Setting the union's total mass and center of mass
-   calcCenterOfMass();
-
-   // Setting the contained primitives' relative position in reference to the center of mass
-   for( auto& b : bodies_ )
-      b.calcRelPosition();
 
    // Setting the moment of inertia
    calcInertia();
 
-   //moving impuls to union
-   setLinearVel( Vec3(0,0,0) );
-   setAngularVel( Vec3(0,0,0) );
-   addImpulseAtPos( oldImpulse, oldCenterOfMass);
-   addImpulseAtPos( bodyImpulse, bodyCenterOfMass);
-   bd.setLinearVel( Vec3(0,0,0) );
-   bd.setAngularVel( Vec3(0,0,0) );
-
    // Signaling the internal modification to the superordinate body
    signalModification();
 
@@ -773,10 +665,6 @@ void Union<BodyTypes...>::translateImpl( real_t dx, real_t dy, real_t dz )
    // Changing the global position/reference point
    gpos_ += dp;
 
-   // Updating the contained bodies
-   for( auto& b : bodies_ )
-      b.update( dp );
-
    Union<BodyTypes...>::calcBoundingBox();    // Setting the axis-aligned bounding box
    wake();               // Waking the union from sleep mode
    signalTranslation();  // Signaling the position change to the superordinate body
@@ -813,10 +701,6 @@ void Union<BodyTypes...>::rotateImpl( const Quat& dq )
    q_ = dq * q_;                // Updating the orientation of the union
    R_ = q_.toRotationMatrix();  // Updating the rotation of the union
 
-   // Updating the contained bodies
-   for( auto& b : bodies_ )
-      b.update( dq );
-
    Union<BodyTypes...>::calcBoundingBox();  // Setting the axis-aligned bounding box
    wake();             // Waking the union from sleep mode
    signalRotation();   // Signaling the change of orientation to the superordinate body
@@ -845,10 +729,6 @@ void Union<BodyTypes...>::rotateAroundOriginImpl( const Quat& dq )
    R_    = q_.toRotationMatrix();  // Updating the rotation of the union
    gpos_ = dq.rotate( gpos_ );     // Updating the global position of the union
 
-   // Updating the contained bodies
-   for( auto& b : bodies_ )
-      b.update( dq );
-
    Union<BodyTypes...>::calcBoundingBox();    // Setting the axis-aligned bounding box
    wake();               // Waking the union from sleep mode
    signalTranslation();  // Signaling the position change to the superordinate body
@@ -878,10 +758,6 @@ void Union<BodyTypes...>::rotateAroundPointImpl( const Vec3& point, const Quat &
    R_    = q_.toRotationMatrix();    // Updating the rotation of the union
    gpos_ = point + dq.rotate( dp );  // Updating the global position of the union
 
-   // Updating the contained bodies
-   for( auto& b : bodies_ )
-      b.update( dq );
-
    Union<BodyTypes...>::calcBoundingBox();    // Setting the axis-aligned bounding box
    wake();               // Waking the union from sleep mode
    signalTranslation();  // Signaling the position change to the superordinate body
@@ -977,10 +853,6 @@ void Union<BodyTypes...>::handleModification()
    // Setting the union's total mass and center of mass
    calcCenterOfMass();
 
-   // Setting the contained primitives' relative position in reference to the center of mass
-   for( auto& b : bodies_ )
-      b.calcRelPosition();
-
    // Setting the moment of inertia
    calcInertia();
 
@@ -1007,10 +879,6 @@ void Union<BodyTypes...>::handleTranslation()
    // Setting the union's total mass and center of mass
    calcCenterOfMass();
 
-   // Setting the contained bodies' relative position in reference to the center of mass
-   for( auto& b : bodies_ )
-      b.calcRelPosition();
-
    // Setting the moment of inertia
    calcInertia();
 
@@ -1074,7 +942,6 @@ void Union<BodyTypes...>::print( std::ostream& os, const char* tab ) const
    else os << "*infinite*\n";
 
    os << tab << "   Global position   = " << gpos_ << "\n"
-      << tab << "   Relative position = " << rpos_ << "\n"
       << tab << "   Linear velocity   = " << v_ << "\n"
       << tab << "   Angular velocity  = " << w_ << "\n";
 
diff --git a/src/pe/rigidbody/UnionFactory.h b/src/pe/rigidbody/UnionFactory.h
index 62fe5828e131c3ce7bfcefcb8a3f0afd48d7a046..6ad1d934e5a8391c1c44f6e4f513d15342b2c677 100644
--- a/src/pe/rigidbody/UnionFactory.h
+++ b/src/pe/rigidbody/UnionFactory.h
@@ -82,7 +82,7 @@ Union<BodyTypes...>* createUnion(   BodyStorage& globalStorage, BlockStorage& bl
       const id_t sid = UniqueID<RigidBody>::createGlobal();
       WALBERLA_ASSERT_EQUAL(communicating, false);
       WALBERLA_ASSERT_EQUAL(infiniteMass, true);
-      auto un = std::make_unique<Union<BodyTypes...>>(sid, uid, gpos, Vec3(0,0,0), Quat(), global, false, true);
+      auto un = std::make_unique<Union<BodyTypes...>>(sid, uid, gpos, Quat(), global, false, true);
       bd = static_cast<Union<BodyTypes...>*>(&globalStorage.add(std::move(un)));
    } else
    {
@@ -92,7 +92,7 @@ Union<BodyTypes...>* createUnion(   BodyStorage& globalStorage, BlockStorage& bl
             const id_t sid( UniqueID<RigidBody>::create() );
 
             BodyStorage& bs = (*block.getData<Storage>(storageID))[0];
-            auto un = std::make_unique<Union<BodyTypes...>>(sid, uid, gpos, Vec3(0,0,0), Quat(), global, communicating, infiniteMass);
+            auto un = std::make_unique<Union<BodyTypes...>>(sid, uid, gpos, Quat(), global, communicating, infiniteMass);
             un->MPITrait.setOwner(Owner(MPIManager::instance()->rank(), block.getId().getID()));
             bd = static_cast<Union<BodyTypes...>*>(&bs.add(std::move(un)));
          }
@@ -156,7 +156,7 @@ BoxID createBox( Union<BodyTypes...>* un,
       sid = UniqueID<RigidBody>::create();
    }
 
-   std::unique_ptr<Box> box = std::make_unique<Box>(sid, uid, gpos, Vec3(0,0,0), Quat(), lengths, material, global, communicating, infiniteMass);
+   std::unique_ptr<Box> box = std::make_unique<Box>(sid, uid, gpos, Quat(), lengths, material, global, communicating, infiniteMass);
    box->MPITrait.setOwner( un->MPITrait.getOwner() );
 
    if (box != NULL)
@@ -226,7 +226,7 @@ CapsuleID createCapsule( Union<BodyTypes...>* un,
       sid = UniqueID<RigidBody>::create();
    }
 
-   std::unique_ptr<Capsule> capsule = std::make_unique<Capsule>(sid, uid, gpos, Vec3(0,0,0), Quat(), radius, length, material, global, communicating, infiniteMass);
+   std::unique_ptr<Capsule> capsule = std::make_unique<Capsule>(sid, uid, gpos, Quat(), radius, length, material, global, communicating, infiniteMass);
    capsule->MPITrait.setOwner( un->MPITrait.getOwner() );
 
    if (capsule != NULL)
@@ -284,7 +284,7 @@ SphereID createSphere( Union<BodyTypes...>* un,
       sid = UniqueID<RigidBody>::create();
    }
 
-   std::unique_ptr<Sphere> sphere = std::make_unique<Sphere>(sid, uid, gpos, Vec3(0,0,0), Quat(), radius, material, global, communicating, infiniteMass);
+   std::unique_ptr<Sphere> sphere = std::make_unique<Sphere>(sid, uid, gpos, Quat(), radius, material, global, communicating, infiniteMass);
    sphere->MPITrait.setOwner( un->MPITrait.getOwner() );
 
    if (sphere != NULL)
diff --git a/tests/core/mpi/BufferSystemTest.cpp b/tests/core/mpi/BufferSystemTest.cpp
index 6de0e0ce4f9de043fae0a5dcfd5884ea06bb4d04..a2e95dc63686763c3bfe28a329c0712ab4aa1acf 100644
--- a/tests/core/mpi/BufferSystemTest.cpp
+++ b/tests/core/mpi/BufferSystemTest.cpp
@@ -219,7 +219,7 @@ void timeVaryingCommunication(const bool useIProbe)
       bs.send( rightNeighbor );
 
 
-      WALBERLA_CHECK( bs.isCommunciationRunning()  );
+      WALBERLA_CHECK( bs.isCommunicationRunning()  );
 
       for( auto it = bs.begin(); it != bs.end(); ++it )
       {
@@ -245,7 +245,7 @@ void timeVaryingCommunication(const bool useIProbe)
          WALBERLA_CHECK( it.buffer().isEmpty() );
 
       }
-      WALBERLA_CHECK( ! bs.isCommunciationRunning()  );
+      WALBERLA_CHECK( ! bs.isCommunicationRunning()  );
    }
 }
 
diff --git a/tests/core/mpi/BufferTest.cpp b/tests/core/mpi/BufferTest.cpp
index b5d43f478ea9f03da32a12627cc09d1ab7044351..887927a47179bf4cf39bc58ee49984160a38c296 100644
--- a/tests/core/mpi/BufferTest.cpp
+++ b/tests/core/mpi/BufferTest.cpp
@@ -143,15 +143,17 @@ void bufferTest()
    initCellContainer(cellVector);
    initCellContainer(cellSet);
 
-   std::vector  <bool>         boolStdVec,  boolStdVecEmpty;
-   std::vector  <unsigned int> stdVec,      stdVecEmpty;
-   std::deque   <unsigned int> stdDeque,    stdDequeEmpty;
-   std::list    <unsigned int> stdList,     stdListEmpty;
-   std::set     <unsigned int> stdSet,      stdSetEmpty;
-   std::multiset<unsigned int> stdMultiSet, stdMultiSetEmpty;
-
-   std::map     <unsigned int, walberla::int64_t> stdMap,      stdMapEmpty;
-   std::multimap<unsigned int, walberla::int64_t> stdMultiMap, stdMultiMapEmpty;
+   std::vector       <bool>         boolStdVec,      boolStdVecEmpty;
+   std::vector       <unsigned int> stdVec,          stdVecEmpty;
+   std::deque        <unsigned int> stdDeque,        stdDequeEmpty;
+   std::list         <unsigned int> stdList,         stdListEmpty;
+   std::set          <unsigned int> stdSet,          stdSetEmpty;
+   std::multiset     <unsigned int> stdMultiSet,     stdMultiSetEmpty;
+   std::unordered_set<unsigned int> stdUnorderedSet, stdUnorderedSetEmpty;
+
+   std::map          <unsigned int, walberla::int64_t> stdMap,          stdMapEmpty;
+   std::multimap     <unsigned int, walberla::int64_t> stdMultiMap,     stdMultiMapEmpty;
+   std::unordered_map<unsigned int, walberla::int64_t> stdUnorderedMap, stdUnorderedMapEmpty;
 
    std::array  < unsigned int, 19 > stdArray;
 
@@ -161,24 +163,28 @@ void bufferTest()
    initIntegerContainer(stdList);
    initIntegerAssocContainer(stdSet);
    initIntegerAssocContainer(stdMultiSet);
+   initIntegerAssocContainer(stdUnorderedSet);
    initIntegerMap(stdMap);
    initIntegerMap(stdMultiMap);
+   initIntegerMap(stdUnorderedMap);
    initStdArray(stdArray);
 
    // Create send buffer and put two values in it
    GenericSendBuffer<T> sb;
-   sb << testDouble  << testInt;
-   sb << vec         << mat;
-   sb << cell        << cellInterval;
-   sb << cellVector  << cellSet;
-   sb << boolStdVec  << boolStdVecEmpty;
-   sb << stdVec      << stdVecEmpty;
-   sb << stdDeque    << stdDequeEmpty;
-   sb << stdList     << stdListEmpty;
-   sb << stdSet      << stdSetEmpty;
-   sb << stdMultiSet << stdMultiSetEmpty;
-   sb << stdMap      << stdMapEmpty;
-   sb << stdMultiMap << stdMultiMapEmpty;
+   sb << testDouble       << testInt;
+   sb << vec              << mat;
+   sb << cell             << cellInterval;
+   sb << cellVector       << cellSet;
+   sb << boolStdVec       << boolStdVecEmpty;
+   sb << stdVec           << stdVecEmpty;
+   sb << stdDeque         << stdDequeEmpty;
+   sb << stdList          << stdListEmpty;
+   sb << stdSet           << stdSetEmpty;
+   sb << stdMultiSet      << stdMultiSetEmpty;
+   sb << stdUnorderedSet  << stdUnorderedSetEmpty;
+   sb << stdMap           << stdMapEmpty;
+   sb << stdMultiMap      << stdMultiMapEmpty;
+   sb << stdUnorderedMap  << stdUnorderedMapEmpty;
    sb << stdArray;
 
    // Copying
@@ -199,30 +205,34 @@ void bufferTest()
    CellVector      recvCellVector;
    CellSet         recvCellSet;
 
-   std::vector  <bool>         recvBoolStdVec,  recvBoolStdVecEmpty;
-   std::vector  <unsigned int> recvStdVec,      recvStdVecEmpty;
-   std::deque   <unsigned int> recvStdDeque,    recvStdDequeEmpty;
-   std::list    <unsigned int> recvStdList,     recvStdListEmpty;
-   std::set     <unsigned int> recvStdSet,      recvStdSetEmpty;
-   std::multiset<unsigned int> recvStdMultiSet, recvStdMultiSetEmpty;
+   std::vector        <bool>         recvBoolStdVec,      recvBoolStdVecEmpty;
+   std::vector        <unsigned int> recvStdVec,          recvStdVecEmpty;
+   std::deque         <unsigned int> recvStdDeque,        recvStdDequeEmpty;
+   std::list          <unsigned int> recvStdList,         recvStdListEmpty;
+   std::set           <unsigned int> recvStdSet,          recvStdSetEmpty;
+   std::multiset      <unsigned int> recvStdMultiSet,     recvStdMultiSetEmpty;
+   std::unordered_set <unsigned int> recvStdUnorderedSet, recvStdUnorderedSetEmpty;
 
-   std::map     <unsigned int, walberla::int64_t> recvStdMap,      recvStdMapEmpty;
-   std::multimap<unsigned int, walberla::int64_t> recvStdMultiMap, recvStdMultiMapEmpty;
+   std::map           <unsigned int, walberla::int64_t> recvStdMap,          recvStdMapEmpty;
+   std::multimap      <unsigned int, walberla::int64_t> recvStdMultiMap,     recvStdMultiMapEmpty;
+   std::unordered_map <unsigned int, walberla::int64_t> recvStdUnorderedMap, recvStdUnorderedMapEmpty;
 
    std::array  <unsigned int, 19> recvStdArray;
 
-   rb >> recvD           >> recvI;
-   rb >> recvVec         >> recvMat;
-   rb >> recvCell        >> recvCellInterval;
-   rb >> recvCellVector  >> recvCellSet;
-   rb >> recvBoolStdVec  >> recvBoolStdVecEmpty;
-   rb >> recvStdVec      >> recvStdVecEmpty;
-   rb >> recvStdDeque    >> recvStdDequeEmpty;
-   rb >> recvStdList     >> recvStdListEmpty;
-   rb >> recvStdSet      >> recvStdSetEmpty;
-   rb >> recvStdMultiSet >> recvStdMultiSetEmpty;
-   rb >> recvStdMap      >> recvStdMapEmpty;
-   rb >> recvStdMultiMap >> recvStdMultiMapEmpty;
+   rb >> recvD                >> recvI;
+   rb >> recvVec              >> recvMat;
+   rb >> recvCell             >> recvCellInterval;
+   rb >> recvCellVector       >> recvCellSet;
+   rb >> recvBoolStdVec       >> recvBoolStdVecEmpty;
+   rb >> recvStdVec           >> recvStdVecEmpty;
+   rb >> recvStdDeque         >> recvStdDequeEmpty;
+   rb >> recvStdList          >> recvStdListEmpty;
+   rb >> recvStdSet           >> recvStdSetEmpty;
+   rb >> recvStdMultiSet      >> recvStdMultiSetEmpty;
+   rb >> recvStdUnorderedSet  >> recvStdUnorderedSetEmpty;
+   rb >> recvStdMap           >> recvStdMapEmpty;
+   rb >> recvStdMultiMap      >> recvStdMultiMapEmpty;
+   rb >> recvStdUnorderedMap  >> recvStdUnorderedMapEmpty;
    rb >> recvStdArray;
 
    // Validate
@@ -245,14 +255,19 @@ void bufferTest()
    WALBERLA_CHECK_EQUAL(recvStdList,         stdList);
    WALBERLA_CHECK_EQUAL(recvStdListEmpty,    stdListEmpty);
 
-   WALBERLA_CHECK_EQUAL(recvStdSet,           stdSet);
-   WALBERLA_CHECK_EQUAL(recvStdSetEmpty,      stdSetEmpty);
-   WALBERLA_CHECK_EQUAL(recvStdMultiSet,      stdMultiSet);
-   WALBERLA_CHECK_EQUAL(recvStdMultiSetEmpty, stdMultiSetEmpty);
-   WALBERLA_CHECK_EQUAL(recvStdMap,           stdMap);
-   WALBERLA_CHECK_EQUAL(recvStdMapEmpty,      stdMapEmpty);
-   WALBERLA_CHECK_EQUAL(recvStdMultiMap,      stdMultiMap);
-   WALBERLA_CHECK_EQUAL(recvStdMultiMapEmpty, stdMultiMapEmpty);
+   WALBERLA_CHECK_EQUAL(recvStdSet,               stdSet);
+   WALBERLA_CHECK_EQUAL(recvStdSetEmpty,          stdSetEmpty);
+   WALBERLA_CHECK_EQUAL(recvStdMultiSet,          stdMultiSet);
+   WALBERLA_CHECK_EQUAL(recvStdMultiSetEmpty,     stdMultiSetEmpty);
+   WALBERLA_CHECK_EQUAL(recvStdUnorderedSet,      stdUnorderedSet);
+   WALBERLA_CHECK_EQUAL(recvStdUnorderedSetEmpty, stdUnorderedSetEmpty);
+
+   WALBERLA_CHECK_EQUAL(recvStdMap,               stdMap);
+   WALBERLA_CHECK_EQUAL(recvStdMapEmpty,          stdMapEmpty);
+   WALBERLA_CHECK_EQUAL(recvStdMultiMap,          stdMultiMap);
+   WALBERLA_CHECK_EQUAL(recvStdMultiMapEmpty,     stdMultiMapEmpty);
+   WALBERLA_CHECK_EQUAL(recvStdUnorderedMap,      stdUnorderedMap);
+   WALBERLA_CHECK_EQUAL(recvStdUnorderedMapEmpty, stdUnorderedMapEmpty);
 
    WALBERLA_CHECK_EQUAL(recvStdArray,   stdArray);
 }
diff --git a/tests/mesa_pd/CMakeLists.txt b/tests/mesa_pd/CMakeLists.txt
index 57a73030ad4212e4539773bd1c15e0d4b8fc882b..ae8501523febf9a160ccd85681ab8cc52f07cb99 100644
--- a/tests/mesa_pd/CMakeLists.txt
+++ b/tests/mesa_pd/CMakeLists.txt
@@ -54,6 +54,9 @@ waLBerla_compile_test( NAME   MESA_PD_Kernel_CoefficientOfRestitutionNLSD FILES
 waLBerla_execute_test( NAME   MESA_PD_Kernel_CoefficientOfRestitutionNLSDEuler COMMAND $<TARGET_FILE:MESA_PD_Kernel_CoefficientOfRestitutionNLSD> )
 waLBerla_execute_test( NAME   MESA_PD_Kernel_CoefficientOfRestitutionVelocityVerlet COMMAND $<TARGET_FILE:MESA_PD_Kernel_CoefficientOfRestitutionNLSD> --useVV )
 
+waLBerla_compile_test( NAME   MESA_PD_Kernel_DetectAndStoreContacts FILES kernel/DetectAndStoreContacts.cpp DEPENDS core )
+waLBerla_execute_test( NAME   MESA_PD_Kernel_DetectAndStoreContacts )
+
 waLBerla_compile_test( NAME   MESA_PD_Kernel_DoubleCast FILES kernel/DoubleCast.cpp DEPENDS core )
 waLBerla_execute_test( NAME   MESA_PD_Kernel_DoubleCast )
 
@@ -95,6 +98,12 @@ waLBerla_execute_test( NAME   MESA_PD_Kernel_SingleCast )
 waLBerla_compile_test( NAME   MESA_PD_Kernel_SpringDashpot FILES kernel/SpringDashpot.cpp DEPENDS core )
 waLBerla_execute_test( NAME   MESA_PD_Kernel_SpringDashpot )
 
+waLBerla_compile_test( NAME   MESA_PD_Kernel_SyncGhostOwners FILES kernel/SyncGhostOwners.cpp DEPENDS core )
+waLBerla_execute_test( NAME   MESA_PD_Kernel_SyncGhostOwners PROCESSES 27 )
+
+waLBerla_compile_test( NAME   MESA_PD_Kernel_SyncGhostOwnersLarge FILES kernel/SyncGhostOwnersLarge.cpp DEPENDS core )
+waLBerla_execute_test( NAME   MESA_PD_Kernel_SyncGhostOwnersLarge PROCESSES 27 )
+
 waLBerla_compile_test( NAME   MESA_PD_Kernel_SyncNextNeighbors FILES kernel/SyncNextNeighbors.cpp DEPENDS core )
 waLBerla_execute_test( NAME   MESA_PD_Kernel_SyncNextNeighbors PROCESSES 27 )
 
diff --git a/tests/mesa_pd/ContactDetection.cpp b/tests/mesa_pd/ContactDetection.cpp
index 1a435e2dc66336021f9fa2d4463913e8001b7b61..b60126d3d1b6043b2eb5465aab0f6d3d4454005c 100644
--- a/tests/mesa_pd/ContactDetection.cpp
+++ b/tests/mesa_pd/ContactDetection.cpp
@@ -54,19 +54,15 @@ class ParticleAccessorWithShape : public data::ParticleAccessor
 {
 public:
    ParticleAccessorWithShape(std::shared_ptr<data::ParticleStorage>& ps, std::shared_ptr<data::ShapeStorage>& ss)
-      : ParticleAccessor(ps)
-      , ss_(ss)
+         : ParticleAccessor(ps)
+         , ss_(ss)
    {}
 
-   const walberla::real_t& getInvMass(const size_t p_idx) const {return ss_->shapes[ps_->getShapeIDRef(p_idx)]->getInvMass();}
-   walberla::real_t& getInvMassRef(const size_t p_idx) {return ss_->shapes[ps_->getShapeIDRef(p_idx)]->getInvMass();}
-   void setInvMass(const size_t p_idx, const walberla::real_t& v) { ss_->shapes[ps_->getShapeIDRef(p_idx)]->getInvMass() = v;}
+   const auto& getInvMass(const size_t p_idx) const {return ss_->shapes[ps_->getShapeID(p_idx)]->getInvMass();}
 
-   const auto& getInvInertiaBF(const size_t p_idx) const {return ss_->shapes[ps_->getShapeIDRef(p_idx)]->getInvInertiaBF();}
-   auto& getInvInertiaBFRef(const size_t p_idx) {return ss_->shapes[ps_->getShapeIDRef(p_idx)]->getInvInertiaBF();}
-   void setInvInertiaBF(const size_t p_idx, const Mat3& v) { ss_->shapes[ps_->getShapeIDRef(p_idx)]->getInvInertiaBF() = v;}
+   const auto& getInvInertiaBF(const size_t p_idx) const {return ss_->shapes[ps_->getShapeID(p_idx)]->getInvInertiaBF();}
 
-   data::BaseShape* getShape(const size_t p_idx) const {return ss_->shapes[ps_->getShapeIDRef(p_idx)].get();}
+   data::BaseShape* getShape(const size_t p_idx) const {return ss_->shapes[ps_->getShapeID(p_idx)].get();}
 private:
    std::shared_ptr<data::ShapeStorage> ss_;
 };
diff --git a/tests/mesa_pd/collision_detection/AnalyticContactDetection.cpp b/tests/mesa_pd/collision_detection/AnalyticContactDetection.cpp
index bca221069559b6a618b3f0e8fa7e30f6785ca601..c191edbd6635c004829de9c1ed5ac3aa93489a09 100644
--- a/tests/mesa_pd/collision_detection/AnalyticContactDetection.cpp
+++ b/tests/mesa_pd/collision_detection/AnalyticContactDetection.cpp
@@ -38,17 +38,13 @@ class ParticleAccessorWithShape : public data::ParticleAccessor
 {
 public:
    ParticleAccessorWithShape(std::shared_ptr<data::ParticleStorage>& ps, std::shared_ptr<data::ShapeStorage>& ss)
-      : ParticleAccessor(ps)
-      , ss_(ss)
+         : ParticleAccessor(ps)
+         , ss_(ss)
    {}
 
-   const walberla::real_t& getInvMass(const size_t p_idx) const {return ss_->shapes[ps_->getShapeID(p_idx)]->getInvMass();}
-   walberla::real_t& getInvMassRef(const size_t p_idx) {return ss_->shapes[ps_->getShapeID(p_idx)]->getInvMass();}
-   void setInvMass(const size_t p_idx, const walberla::real_t& v) { ss_->shapes[ps_->getShapeID(p_idx)]->getInvMass() = v;}
+   const auto& getInvMass(const size_t p_idx) const {return ss_->shapes[ps_->getShapeID(p_idx)]->getInvMass();}
 
    const auto& getInvInertiaBF(const size_t p_idx) const {return ss_->shapes[ps_->getShapeID(p_idx)]->getInvInertiaBF();}
-   auto& getInvInertiaBFRef(const size_t p_idx) {return ss_->shapes[ps_->getShapeID(p_idx)]->getInvInertiaBF();}
-   void setInvInertiaBF(const size_t p_idx, const Mat3& v) { ss_->shapes[ps_->getShapeID(p_idx)]->getInvInertiaBF() = v;}
 
    data::BaseShape* getShape(const size_t p_idx) const {return ss_->shapes[ps_->getShapeID(p_idx)].get();}
 private:
diff --git a/tests/mesa_pd/kernel/CoefficientOfRestitutionLSD.cpp b/tests/mesa_pd/kernel/CoefficientOfRestitutionLSD.cpp
index 7f9ff90285f5f00020bc3e34769f76761cf03bff..53cff74ea690c4e55abb18894f80145063cae2ff 100644
--- a/tests/mesa_pd/kernel/CoefficientOfRestitutionLSD.cpp
+++ b/tests/mesa_pd/kernel/CoefficientOfRestitutionLSD.cpp
@@ -48,13 +48,9 @@ public:
          , ss_(ss)
    {}
 
-   const walberla::real_t& getInvMass(const size_t p_idx) const {return ss_->shapes[ps_->getShapeID(p_idx)]->getInvMass();}
-   walberla::real_t& getInvMassRef(const size_t p_idx) {return ss_->shapes[ps_->getShapeID(p_idx)]->getInvMass();}
-   void setInvMass(const size_t p_idx, const walberla::real_t& v) { ss_->shapes[ps_->getShapeID(p_idx)]->getInvMass() = v;}
+   const auto& getInvMass(const size_t p_idx) const {return ss_->shapes[ps_->getShapeID(p_idx)]->getInvMass();}
 
    const auto& getInvInertiaBF(const size_t p_idx) const {return ss_->shapes[ps_->getShapeID(p_idx)]->getInvInertiaBF();}
-   auto& getInvInertiaBFRef(const size_t p_idx) {return ss_->shapes[ps_->getShapeID(p_idx)]->getInvInertiaBF();}
-   void setInvInertiaBF(const size_t p_idx, const Mat3& v) { ss_->shapes[ps_->getShapeID(p_idx)]->getInvInertiaBF() = v;}
 
    data::BaseShape* getShape(const size_t p_idx) const {return ss_->shapes[ps_->getShapeID(p_idx)].get();}
 private:
diff --git a/tests/mesa_pd/kernel/CoefficientOfRestitutionNLSD.cpp b/tests/mesa_pd/kernel/CoefficientOfRestitutionNLSD.cpp
index c6c996a9af7d67f1ee9a2b40371b52346c4fff35..5b25815de2fd7714b3eb96032cf16eeabca28b0d 100644
--- a/tests/mesa_pd/kernel/CoefficientOfRestitutionNLSD.cpp
+++ b/tests/mesa_pd/kernel/CoefficientOfRestitutionNLSD.cpp
@@ -49,13 +49,9 @@ public:
          , ss_(ss)
    {}
 
-   const walberla::real_t& getInvMass(const size_t p_idx) const {return ss_->shapes[ps_->getShapeID(p_idx)]->getInvMass();}
-   walberla::real_t& getInvMassRef(const size_t p_idx) {return ss_->shapes[ps_->getShapeID(p_idx)]->getInvMass();}
-   void setInvMass(const size_t p_idx, const walberla::real_t& v) { ss_->shapes[ps_->getShapeID(p_idx)]->getInvMass() = v;}
+   const auto& getInvMass(const size_t p_idx) const {return ss_->shapes[ps_->getShapeID(p_idx)]->getInvMass();}
 
    const auto& getInvInertiaBF(const size_t p_idx) const {return ss_->shapes[ps_->getShapeID(p_idx)]->getInvInertiaBF();}
-   auto& getInvInertiaBFRef(const size_t p_idx) {return ss_->shapes[ps_->getShapeID(p_idx)]->getInvInertiaBF();}
-   void setInvInertiaBF(const size_t p_idx, const Mat3& v) { ss_->shapes[ps_->getShapeID(p_idx)]->getInvInertiaBF() = v;}
 
    data::BaseShape* getShape(const size_t p_idx) const {return ss_->shapes[ps_->getShapeID(p_idx)].get();}
 private:
diff --git a/tests/mesa_pd/kernel/CoefficientOfRestitutionSD.cpp b/tests/mesa_pd/kernel/CoefficientOfRestitutionSD.cpp
index b72a7eaabf7ffc56a19e6198badf6fba21dfc117..6c45e39cff3d462206fc51100ad0a304e8ba0124 100644
--- a/tests/mesa_pd/kernel/CoefficientOfRestitutionSD.cpp
+++ b/tests/mesa_pd/kernel/CoefficientOfRestitutionSD.cpp
@@ -48,13 +48,9 @@ public:
          , ss_(ss)
    {}
 
-   const walberla::real_t& getInvMass(const size_t p_idx) const {return ss_->shapes[ps_->getShapeID(p_idx)]->getInvMass();}
-   walberla::real_t& getInvMassRef(const size_t p_idx) {return ss_->shapes[ps_->getShapeID(p_idx)]->getInvMass();}
-   void setInvMass(const size_t p_idx, const walberla::real_t& v) { ss_->shapes[ps_->getShapeID(p_idx)]->getInvMass() = v;}
+   const auto& getInvMass(const size_t p_idx) const {return ss_->shapes[ps_->getShapeID(p_idx)]->getInvMass();}
 
    const auto& getInvInertiaBF(const size_t p_idx) const {return ss_->shapes[ps_->getShapeID(p_idx)]->getInvInertiaBF();}
-   auto& getInvInertiaBFRef(const size_t p_idx) {return ss_->shapes[ps_->getShapeID(p_idx)]->getInvInertiaBF();}
-   void setInvInertiaBF(const size_t p_idx, const Mat3& v) { ss_->shapes[ps_->getShapeID(p_idx)]->getInvInertiaBF() = v;}
 
    data::BaseShape* getShape(const size_t p_idx) const {return ss_->shapes[ps_->getShapeID(p_idx)].get();}
 private:
diff --git a/tests/mesa_pd/kernel/DetectAndStoreContacts.cpp b/tests/mesa_pd/kernel/DetectAndStoreContacts.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..6fba0bba4a46153e8071ee1a8381be2b107498ac
--- /dev/null
+++ b/tests/mesa_pd/kernel/DetectAndStoreContacts.cpp
@@ -0,0 +1,128 @@
+//======================================================================================================================
+//
+//  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   DetectAndStoreCollisions.cpp
+//! \author Tobias Leemann <tobias.leemann@fau.de>
+//
+//======================================================================================================================
+
+
+/** Test Collision Detection and Insertion of contacts into the contact storage */
+
+#include <mesa_pd/data/DataTypes.h>
+#include <mesa_pd/data/ContactStorage.h>
+#include <mesa_pd/data/ParticleStorage.h>
+#include <mesa_pd/data/ShapeStorage.h>
+#include <mesa_pd/kernel/DetectAndStoreContacts.h>
+#include <mesa_pd/domain/InfiniteDomain.h>
+#include <mesa_pd/kernel/ParticleSelector.h>
+
+#include <mesa_pd/data/ParticleAccessor.h>
+#include <core/Environment.h>
+#include <core/logging/Logging.h>
+
+#include <iostream>
+
+namespace walberla {
+
+using namespace walberla::mesa_pd;
+
+class ParticleAccessorWithShape : public data::ParticleAccessor
+{
+public:
+   ParticleAccessorWithShape(std::shared_ptr<data::ParticleStorage>& ps, std::shared_ptr<data::ShapeStorage>& ss)
+         : ParticleAccessor(ps)
+         , ss_(ss)
+   {}
+
+   const auto& getInvMass(const size_t p_idx) const {return ss_->shapes[ps_->getShapeID(p_idx)]->getInvMass();}
+
+   const auto& getInvInertiaBF(const size_t p_idx) const {return ss_->shapes[ps_->getShapeID(p_idx)]->getInvInertiaBF();}
+
+   data::BaseShape* getShape(const size_t p_idx) const {return ss_->shapes[ps_->getShapeID(p_idx)].get();}
+private:
+   std::shared_ptr<data::ShapeStorage> ss_;
+};
+
+
+int main( int argc, char ** argv )
+{
+   Environment env(argc, argv);
+   WALBERLA_UNUSED(env);
+   walberla::mpi::MPIManager::instance()->useWorldComm();
+
+   WALBERLA_LOG_INFO_ON_ROOT("*** SETUP - START ***");
+
+   //init data structures
+   auto ps = std::make_shared<data::ParticleStorage>(100);
+   auto ss = std::make_shared<data::ShapeStorage>();
+   ParticleAccessorWithShape accessor(ps, ss);
+
+   auto  smallSphere = ss->create<data::Sphere>( real_t(1.2) );
+
+   ss->shapes[smallSphere]->updateMassAndInertia(real_t(1));
+
+   domain::InfiniteDomain domain;
+
+   // Create four slightly overlapping spheres in a row (located at x=0,2,4,6)
+   for (int i = 0; i < 8; i+=2)
+   {
+      auto p                       = ps->create();
+      p->getPositionRef()          = Vec3(real_t(i), real_t(0), real_t(0));
+      p->getShapeIDRef()           = smallSphere;
+      p->getOwnerRef()             = walberla::mpi::MPIManager::instance()->rank();
+      p->getTypeRef()              = 0;
+   }
+
+
+   Vec3 normal(-1,0,0);
+   auto dist= real_t(-0.4);
+
+   // Create Contact Storage cs
+   data::ContactStorage cs(100);
+   cs.clear();
+
+   // Perform Collision detection (call kernel, that stores contacts into cs)
+   kernel::DetectAndStoreContacts detectAndStore(cs);
+   ps->forEachParticlePairHalf(false, kernel::ExcludeInfiniteInfinite(), accessor, detectAndStore, accessor, domain);
+
+   // Check if all three intersections were found
+   size_t contactCount = cs.size();
+   WALBERLA_CHECK_EQUAL(contactCount, 3);
+
+   // Check the contacts with the for each Contact loop.
+   // Set back contact count to 0 to now count the loop iterations.
+   contactCount = 0;
+
+   cs.forEachContact(false, kernel::SelectAll(), cs, [&normal, &dist, &contactCount](size_t idx, data::ContactStorage &css){
+      WALBERLA_CHECK_FLOAT_EQUAL(css.getNormal(idx), normal);
+      WALBERLA_CHECK_FLOAT_EQUAL(css.getPosition(idx), Vec3(real_t(2*idx+1), real_t(0), real_t(0)));
+      WALBERLA_CHECK_FLOAT_EQUAL(css.getDistance(idx), dist);
+      contactCount++;
+   }
+   ,cs);
+
+   WALBERLA_CHECK_EQUAL(contactCount, 3);
+
+   WALBERLA_LOG_INFO("Insertion test with ContactStorage successful.");
+   return EXIT_SUCCESS;
+}
+
+} //namespace walberla
+
+int main( int argc, char ** argv )
+{
+   return walberla::main(argc, argv);
+}
diff --git a/tests/mesa_pd/kernel/GenerateAnalyticContacts.cpp b/tests/mesa_pd/kernel/GenerateAnalyticContacts.cpp
index f85423e4e98a9441b0c51ba2bf1186ee7cd9ef5d..757587bebfe82f2cf82851f6473947501a05bd61 100644
--- a/tests/mesa_pd/kernel/GenerateAnalyticContacts.cpp
+++ b/tests/mesa_pd/kernel/GenerateAnalyticContacts.cpp
@@ -49,17 +49,13 @@ class ParticleAccessorWithShape : public data::ParticleAccessor
 {
 public:
    ParticleAccessorWithShape(std::shared_ptr<data::ParticleStorage>& ps, std::shared_ptr<data::ShapeStorage>& ss)
-      : ParticleAccessor(ps)
-      , ss_(ss)
+         : ParticleAccessor(ps)
+         , ss_(ss)
    {}
 
-   const walberla::real_t& getInvMass(const size_t p_idx) const {return ss_->shapes[ps_->getShapeID(p_idx)]->getInvMass();}
-   walberla::real_t& getInvMassRef(const size_t p_idx) {return ss_->shapes[ps_->getShapeID(p_idx)]->getInvMass();}
-   void setInvMass(const size_t p_idx, const walberla::real_t& v) { ss_->shapes[ps_->getShapeID(p_idx)]->getInvMass() = v;}
+   const auto& getInvMass(const size_t p_idx) const {return ss_->shapes[ps_->getShapeID(p_idx)]->getInvMass();}
 
    const auto& getInvInertiaBF(const size_t p_idx) const {return ss_->shapes[ps_->getShapeID(p_idx)]->getInvInertiaBF();}
-   auto& getInvInertiaBFRef(const size_t p_idx) {return ss_->shapes[ps_->getShapeID(p_idx)]->getInvInertiaBF();}
-   void setInvInertiaBF(const size_t p_idx, const Mat3& v) { ss_->shapes[ps_->getShapeID(p_idx)]->getInvInertiaBF() = v;}
 
    data::BaseShape* getShape(const size_t p_idx) const {return ss_->shapes[ps_->getShapeID(p_idx)].get();}
 private:
diff --git a/tests/mesa_pd/kernel/LinearSpringDashpot.cpp b/tests/mesa_pd/kernel/LinearSpringDashpot.cpp
index 9fcce832db8ac15e8202d2b7d13afec355cd052d..6ff5bede797db82bd9bf02aa970f082c5b7391ce 100644
--- a/tests/mesa_pd/kernel/LinearSpringDashpot.cpp
+++ b/tests/mesa_pd/kernel/LinearSpringDashpot.cpp
@@ -47,13 +47,9 @@ public:
          , ss_(ss)
    {}
 
-   const walberla::real_t& getInvMass(const size_t p_idx) const {return ss_->shapes[ps_->getShapeID(p_idx)]->getInvMass();}
-   walberla::real_t& getInvMassRef(const size_t p_idx) {return ss_->shapes[ps_->getShapeID(p_idx)]->getInvMass();}
-   void setInvMass(const size_t p_idx, const walberla::real_t& v) { ss_->shapes[ps_->getShapeID(p_idx)]->getInvMass() = v;}
+   const auto& getInvMass(const size_t p_idx) const {return ss_->shapes[ps_->getShapeID(p_idx)]->getInvMass();}
 
    const auto& getInvInertiaBF(const size_t p_idx) const {return ss_->shapes[ps_->getShapeID(p_idx)]->getInvInertiaBF();}
-   auto& getInvInertiaBFRef(const size_t p_idx) {return ss_->shapes[ps_->getShapeID(p_idx)]->getInvInertiaBF();}
-   void setInvInertiaBF(const size_t p_idx, const Mat3& v) { ss_->shapes[ps_->getShapeID(p_idx)]->getInvInertiaBF() = v;}
 
    data::BaseShape* getShape(const size_t p_idx) const {return ss_->shapes[ps_->getShapeID(p_idx)].get();}
 private:
diff --git a/tests/mesa_pd/kernel/LinkedCellsVsBruteForce.cpp b/tests/mesa_pd/kernel/LinkedCellsVsBruteForce.cpp
index 0340e428b05aa00a39ac331d18928b319ae703d9..ce990842f231d75c580768985b9f8ec750adcd46 100644
--- a/tests/mesa_pd/kernel/LinkedCellsVsBruteForce.cpp
+++ b/tests/mesa_pd/kernel/LinkedCellsVsBruteForce.cpp
@@ -49,17 +49,13 @@ class ParticleAccessorWithShape : public data::ParticleAccessor
 {
 public:
    ParticleAccessorWithShape(std::shared_ptr<data::ParticleStorage>& ps, std::shared_ptr<data::ShapeStorage>& ss)
-      : ParticleAccessor(ps)
-      , ss_(ss)
+         : ParticleAccessor(ps)
+         , ss_(ss)
    {}
 
-   const walberla::real_t& getInvMass(const size_t p_idx) const {return ss_->shapes[ps_->getShapeID(p_idx)]->getInvMass();}
-   walberla::real_t& getInvMassRef(const size_t p_idx) {return ss_->shapes[ps_->getShapeID(p_idx)]->getInvMass();}
-   void setInvMass(const size_t p_idx, const walberla::real_t& v) { ss_->shapes[ps_->getShapeID(p_idx)]->getInvMass() = v;}
+   const auto& getInvMass(const size_t p_idx) const {return ss_->shapes[ps_->getShapeID(p_idx)]->getInvMass();}
 
    const auto& getInvInertiaBF(const size_t p_idx) const {return ss_->shapes[ps_->getShapeID(p_idx)]->getInvInertiaBF();}
-   auto& getInvInertiaBFRef(const size_t p_idx) {return ss_->shapes[ps_->getShapeID(p_idx)]->getInvInertiaBF();}
-   void setInvInertiaBF(const size_t p_idx, const Mat3& v) { ss_->shapes[ps_->getShapeID(p_idx)]->getInvInertiaBF() = v;}
 
    data::BaseShape* getShape(const size_t p_idx) const {return ss_->shapes[ps_->getShapeID(p_idx)].get();}
 private:
diff --git a/tests/mesa_pd/kernel/SpringDashpot.cpp b/tests/mesa_pd/kernel/SpringDashpot.cpp
index 746b9b1fef33debeaafac1e124107d76f302d367..c9108e5ae9ba5a5f246305ed9cc563a593277aac 100644
--- a/tests/mesa_pd/kernel/SpringDashpot.cpp
+++ b/tests/mesa_pd/kernel/SpringDashpot.cpp
@@ -40,17 +40,13 @@ class ParticleAccessorWithShape : public data::ParticleAccessor
 {
 public:
    ParticleAccessorWithShape(std::shared_ptr<data::ParticleStorage>& ps, std::shared_ptr<data::ShapeStorage>& ss)
-      : ParticleAccessor(ps)
-      , ss_(ss)
+         : ParticleAccessor(ps)
+         , ss_(ss)
    {}
 
-   const walberla::real_t& getInvMass(const size_t p_idx) const {return ss_->shapes[ps_->getShapeID(p_idx)]->getInvMass();}
-   walberla::real_t& getInvMassRef(const size_t p_idx) {return ss_->shapes[ps_->getShapeID(p_idx)]->getInvMass();}
-   void setInvMass(const size_t p_idx, const walberla::real_t& v) { ss_->shapes[ps_->getShapeID(p_idx)]->getInvMass() = v;}
+   const auto& getInvMass(const size_t p_idx) const {return ss_->shapes[ps_->getShapeID(p_idx)]->getInvMass();}
 
    const auto& getInvInertiaBF(const size_t p_idx) const {return ss_->shapes[ps_->getShapeID(p_idx)]->getInvInertiaBF();}
-   auto& getInvInertiaBFRef(const size_t p_idx) {return ss_->shapes[ps_->getShapeID(p_idx)]->getInvInertiaBF();}
-   void setInvInertiaBF(const size_t p_idx, const Mat3& v) { ss_->shapes[ps_->getShapeID(p_idx)]->getInvInertiaBF() = v;}
 
    data::BaseShape* getShape(const size_t p_idx) const {return ss_->shapes[ps_->getShapeID(p_idx)].get();}
 private:
diff --git a/tests/mesa_pd/kernel/SyncGhostOwners.cpp b/tests/mesa_pd/kernel/SyncGhostOwners.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..cf16223ee801d8202d34b27c2bf2b062e221f831
--- /dev/null
+++ b/tests/mesa_pd/kernel/SyncGhostOwners.cpp
@@ -0,0 +1,147 @@
+//======================================================================================================================
+//
+//  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   SyncGhostOwners.cpp
+//! \author Sebastian Eibl <sebastian.eibl@fau.de>
+//
+//======================================================================================================================
+
+#include <mesa_pd/data/ParticleStorage.h>
+#include <mesa_pd/domain/BlockForestDomain.h>
+#include <mesa_pd/mpi/SyncGhostOwners.h>
+
+#include <blockforest/BlockForest.h>
+#include <blockforest/Initialization.h>
+#include <core/Environment.h>
+#include <core/logging/Logging.h>
+#include <core/mpi/Reduce.h>
+
+#include <iostream>
+#include <memory>
+
+namespace walberla {
+namespace mesa_pd {
+
+const real_t radius = real_t(1);
+
+walberla::id_t createSphere(data::ParticleStorage& ps, domain::IDomain& domain)
+{
+   walberla::id_t uid = 0;
+   auto owned = domain.isContainedInProcessSubdomain( uint_c(walberla::mpi::MPIManager::instance()->rank()), Vec3(0,0,0) );
+   if (owned)
+   {
+      data::Particle&& p          = *ps.create();
+      p.getPositionRef()          = Vec3(0,0,0);
+      p.getInteractionRadiusRef() = radius;
+      p.getRotationRef()          = Rot3(Quat());
+      p.getLinearVelocityRef()    = Vec3(1,2,3);
+      p.getAngularVelocityRef()   = Vec3(4,5,6);
+      p.getOwnerRef()             = walberla::mpi::MPIManager::instance()->rank();
+      uid = p.getUid();
+      WALBERLA_LOG_DETAIL("SPHERE CREATED");
+   }
+
+   walberla::mpi::allReduceInplace(uid, walberla::mpi::SUM);
+   return uid;
+}
+
+int main( int argc, char ** argv )
+{
+   Environment env(argc, argv);
+   WALBERLA_UNUSED(env);
+   walberla::mpi::MPIManager::instance()->useWorldComm();
+
+   //logging::Logging::instance()->setStreamLogLevel(logging::Logging::DETAIL);
+//   logging::Logging::instance()->includeLoggingToFile("MESA_PD_Kernel_SyncGhostOwners");
+//   logging::Logging::instance()->setFileLogLevel(logging::Logging::DETAIL);
+
+   //init domain partitioning
+   auto forest = blockforest::createBlockForest( AABB(-15,-15,-15,15,15,15), // simulation domain
+                                                 Vector3<uint_t>(3,3,3), // blocks in each direction
+                                                 Vector3<bool>(true, true, true) // periodicity
+                                                 );
+   domain::BlockForestDomain domain(forest);
+   std::array< bool, 3 > periodic;
+   periodic[0] = forest->isPeriodic(0);
+   periodic[1] = forest->isPeriodic(1);
+   periodic[2] = forest->isPeriodic(2);
+
+   //init data structures
+   data::ParticleStorage ps(100);
+
+   //initialize particle
+   auto uid = createSphere(ps, domain);
+   WALBERLA_LOG_DEVEL_ON_ROOT("uid: " << uid);
+
+   //init kernels
+   mpi::SyncGhostOwners SNN;
+
+   std::vector<real_t> deltas { real_t(0),
+            real_t(4.9),
+            real_t(5.1),
+            real_t(10),
+            real_t(14.9),
+            real_t(-14.9),
+            real_t(-10),
+            real_t(-5.1),
+            real_t(-4.9),
+            real_t(0)};
+
+   for (auto delta : deltas)
+   {
+      WALBERLA_LOG_DEVEL(delta);
+      auto pos = Vec3(1,-1,1) * delta;
+      WALBERLA_LOG_DETAIL("checking position: " << pos);
+      // owner moves particle to new position
+      auto pIt = ps.find(uid);
+      if (pIt != ps.end())
+      {
+         if (!data::particle_flags::isSet(pIt->getFlags(), data::particle_flags::GHOST))
+         {
+            pIt->setPosition(pos);
+         }
+      }
+
+      //sync
+      SNN(ps, domain);
+
+      //check
+      if (sqDistancePointToAABBPeriodic(pos, forest->begin()->getAABB(), forest->getDomain(), periodic) <= radius * radius)
+      {
+         WALBERLA_CHECK_EQUAL(ps.size(), 1);
+         if (forest->begin()->getAABB().contains(pos))
+         {
+            WALBERLA_CHECK(!data::particle_flags::isSet(ps.begin()->getFlags(), data::particle_flags::GHOST));
+         } else
+         {
+            WALBERLA_CHECK(data::particle_flags::isSet(ps.begin()->getFlags(), data::particle_flags::GHOST));
+         }
+      } else
+      {
+         WALBERLA_CHECK_EQUAL(ps.size(), 0);
+      }
+   }
+
+
+   return EXIT_SUCCESS;
+}
+
+} //namespace mesa_pd
+} //namespace walberla
+
+int main( int argc, char ** argv )
+{
+   return walberla::mesa_pd::main(argc, argv);
+}
diff --git a/tests/mesa_pd/kernel/SyncGhostOwnersLarge.cpp b/tests/mesa_pd/kernel/SyncGhostOwnersLarge.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..b04dd3c602f073d804f77c7d9e2e0d21e203ade2
--- /dev/null
+++ b/tests/mesa_pd/kernel/SyncGhostOwnersLarge.cpp
@@ -0,0 +1,151 @@
+//======================================================================================================================
+//
+//  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   SyncGhostOwnersLarge.cpp
+//! \author Sebastian Eibl <sebastian.eibl@fau.de>
+//
+//======================================================================================================================
+
+#include <mesa_pd/data/ParticleStorage.h>
+#include <mesa_pd/domain/BlockForestDomain.h>
+#include <mesa_pd/mpi/SyncGhostOwners.h>
+
+#include <blockforest/BlockForest.h>
+#include <blockforest/Initialization.h>
+#include <core/Environment.h>
+#include <core/logging/Logging.h>
+#include <core/mpi/Reduce.h>
+
+#include <iostream>
+#include <memory>
+
+namespace walberla {
+namespace mesa_pd {
+
+const real_t radius = real_t(20);
+
+walberla::id_t createSphere(data::ParticleStorage& ps, domain::IDomain& domain)
+{
+   walberla::id_t uid = 0;
+   auto owned = domain.isContainedInProcessSubdomain( uint_c(walberla::mpi::MPIManager::instance()->rank()), Vec3(0,0,0) );
+   if (owned)
+   {
+      data::Particle&& p          = *ps.create();
+      p.getPositionRef()          = Vec3(0,0,0);
+      p.getInteractionRadiusRef() = radius;
+      p.getRotationRef()          = Rot3(Quat());
+      p.getLinearVelocityRef()    = Vec3(1,2,3);
+      p.getAngularVelocityRef()   = Vec3(4,5,6);
+      p.getOwnerRef()             = walberla::mpi::MPIManager::instance()->rank();
+      uid = p.getUid();
+      WALBERLA_LOG_DETAIL("SPHERE CREATED");
+   }
+
+   walberla::mpi::allReduceInplace(uid, walberla::mpi::SUM);
+   return uid;
+}
+
+int main( int argc, char ** argv )
+{
+   Environment env(argc, argv);
+   WALBERLA_UNUSED(env);
+   walberla::mpi::MPIManager::instance()->useWorldComm();
+
+   //logging::Logging::instance()->setStreamLogLevel(logging::Logging::DETAIL);
+   //logging::Logging::instance()->includeLoggingToFile("MESA_PD_Kernel_SyncGhostOwnersLarge");
+   //logging::Logging::instance()->setFileLogLevel(logging::Logging::DETAIL);
+
+   //init domain partitioning
+   auto forest = blockforest::createBlockForest( AABB(-5,-5,-5,25,25,25), // simulation domain
+                                                 Vector3<uint_t>(3,3,3), // blocks in each direction
+                                                 Vector3<bool>(false, false, false) // periodicity
+                                                 );
+   domain::BlockForestDomain domain(forest);
+   std::array< bool, 3 > periodic;
+   periodic[0] = forest->isPeriodic(0);
+   periodic[1] = forest->isPeriodic(1);
+   periodic[2] = forest->isPeriodic(2);
+
+   //init data structures
+   data::ParticleStorage ps(100);
+
+   //initialize particle
+   auto uid = createSphere(ps, domain);
+   WALBERLA_LOG_DEVEL_ON_ROOT("uid: " << uid);
+
+   //init kernels
+   mpi::SyncGhostOwners SNN;
+
+   SNN(ps, domain);
+   SNN(ps, domain);
+   SNN(ps, domain);
+   SNN(ps, domain);
+
+   std::vector<real_t> deltas {
+      real_t(0),
+            real_t(4.9),
+            real_t(5.1),
+            real_t(10),
+            real_t(14.9),
+            real_t(15.1),
+            real_t(20),
+            real_t(24.9)};
+
+   for (auto delta : deltas)
+   {
+      WALBERLA_LOG_DEVEL(delta);
+      auto pos = Vec3(1,0,0) * delta;
+      WALBERLA_LOG_DETAIL("checking position: " << pos);
+      // owner moves particle to new position
+      auto pIt = ps.find(uid);
+      if (pIt != ps.end())
+      {
+         if (!data::particle_flags::isSet(pIt->getFlags(), data::particle_flags::GHOST))
+         {
+            pIt->setPosition(pos);
+         }
+      }
+
+      //sync
+      SNN(ps, domain);
+
+      //check
+      if (sqDistancePointToAABB(pos, forest->begin()->getAABB()) <= radius * radius)
+      {
+         WALBERLA_CHECK_EQUAL(ps.size(), 1);
+         if (forest->begin()->getAABB().contains(pos))
+         {
+            WALBERLA_CHECK(!data::particle_flags::isSet(ps.begin()->getFlags(), data::particle_flags::GHOST));
+         } else
+         {
+            WALBERLA_CHECK(data::particle_flags::isSet(ps.begin()->getFlags(), data::particle_flags::GHOST));
+         }
+      } else
+      {
+         WALBERLA_CHECK_EQUAL(ps.size(), 0);
+      }
+   }
+
+
+   return EXIT_SUCCESS;
+}
+
+} //namespace mesa_pd
+} //namespace walberla
+
+int main( int argc, char ** argv )
+{
+   return walberla::mesa_pd::main(argc, argv);
+}
diff --git a/tests/mesa_pd/kernel/SyncNextNeighbors.cpp b/tests/mesa_pd/kernel/SyncNextNeighbors.cpp
index 373dffbc353320457b5b87a4ad6c3448828b719e..93a7d6768ed2fb294c556cae0a6697d17c802d33 100644
--- a/tests/mesa_pd/kernel/SyncNextNeighbors.cpp
+++ b/tests/mesa_pd/kernel/SyncNextNeighbors.cpp
@@ -50,6 +50,7 @@ walberla::id_t createSphere(data::ParticleStorage& ps, domain::IDomain& domain)
       p.getAngularVelocityRef()   = Vec3(4,5,6);
       p.getOwnerRef()             = walberla::mpi::MPIManager::instance()->rank();
       uid = p.getUid();
+      WALBERLA_LOG_DETAIL("SPHERE CREATED");
    }
 
    walberla::mpi::allReduceInplace(uid, walberla::mpi::SUM);
@@ -62,7 +63,7 @@ int main( int argc, char ** argv )
    WALBERLA_UNUSED(env);
    walberla::mpi::MPIManager::instance()->useWorldComm();
 
-//   logging::Logging::instance()->setStreamLogLevel(logging::Logging::DETAIL);
+   //logging::Logging::instance()->setStreamLogLevel(logging::Logging::DETAIL);
 //   logging::Logging::instance()->includeLoggingToFile("MESA_PD_Kernel_SyncNextNeighbor");
 //   logging::Logging::instance()->setFileLogLevel(logging::Logging::DETAIL);
 
@@ -100,6 +101,7 @@ int main( int argc, char ** argv )
 
    for (auto delta : deltas)
    {
+      WALBERLA_LOG_DEVEL(delta);
       auto pos = Vec3(1,-1,1) * delta;
       WALBERLA_LOG_DETAIL("checking position: " << pos);
       // owner moves particle to new position
diff --git a/tests/mesa_pd/kernel/VelocityVerletWithShape.cpp b/tests/mesa_pd/kernel/VelocityVerletWithShape.cpp
index fc3567c1f45c6810ec451d64c2dd87f1d495f039..70d77806749c7900264f821570aad697b790d86f 100644
--- a/tests/mesa_pd/kernel/VelocityVerletWithShape.cpp
+++ b/tests/mesa_pd/kernel/VelocityVerletWithShape.cpp
@@ -45,13 +45,9 @@ public:
       sp.updateMassAndInertia(real_t(1234));
    }
 
-   const walberla::real_t& getInvMass(const size_t /*p_idx*/) const {return sp.getInvMass();}
-   walberla::real_t& getInvMassRef(const size_t /*p_idx*/) {return sp.getInvMass();}
-   void setInvMass(const size_t /*p_idx*/, const walberla::real_t& v) { sp.getInvMass() = v;}
+   const auto& getInvMass(const size_t /*p_idx*/) const {return sp.getInvMass();}
 
    const auto& getInvInertiaBF(const size_t /*p_idx*/) const {return sp.getInvInertiaBF();}
-   auto& getInvInertiaBFRef(const size_t /*p_idx*/) {return sp.getInvInertiaBF();}
-   void setInvInertiaBF(const size_t /*p_idx*/, const Mat3& v) { sp.getInvInertiaBF() = v;}
 
    data::BaseShape* getShape(const size_t /*p_idx*/) {return &sp;}
 private:
diff --git a/tests/mesh/MeshMarshalling.cpp b/tests/mesh/MeshMarshalling.cpp
index 15e6bd675fba64db480d24c900feef9216d4e82d..cc62c6a6325025599493cfbc88291bc58ec704e1 100644
--- a/tests/mesh/MeshMarshalling.cpp
+++ b/tests/mesh/MeshMarshalling.cpp
@@ -62,8 +62,8 @@ std::vector<Vector3<real_t>> generateOctahedron( const real_t radius)
 void checkMeshEquals(const mesh::TriangleMesh &m1, const mesh::TriangleMesh &m2){
 	// Very basic checks
 	WALBERLA_CHECK_FLOAT_EQUAL(mesh::computeVolume(m1), mesh::computeVolume(m2));
-	WALBERLA_CHECK_EQUAL(mesh::computeCentroid(m1), mesh::computeCentroid(m2));
-	WALBERLA_CHECK_EQUAL(mesh::computeInertiaTensor(m1), mesh::computeInertiaTensor(m2));
+	WALBERLA_CHECK_FLOAT_EQUAL(mesh::toWalberla(mesh::computeCentroid(m1)), mesh::toWalberla(mesh::computeCentroid(m2)));
+	WALBERLA_CHECK_FLOAT_EQUAL(mesh::computeInertiaTensor(m1), mesh::computeInertiaTensor(m2));
 }
 
 // Checks two convexPolyhedrons for equality
@@ -71,8 +71,10 @@ void checkConvexPolyhedronEquals(const mesh::pe::ConvexPolyhedron &b1, const mes
    WALBERLA_CHECK_FLOAT_EQUAL(b1.getPosition(), b2.getPosition());
    WALBERLA_CHECK_FLOAT_EQUAL(b1.getLinearVel(), b2.getLinearVel());
    WALBERLA_CHECK_FLOAT_EQUAL(b1.getAngularVel(), b2.getAngularVel());
-   WALBERLA_CHECK_EQUAL(b1.getInertia(), b2.getInertia());
+   WALBERLA_CHECK_FLOAT_EQUAL(b1.getInertia(), b2.getInertia());
    WALBERLA_CHECK_EQUAL(b1.getMaterial(), b2.getMaterial());
+   WALBERLA_CHECK_FLOAT_EQUAL(b1.getQuaternion(), b2.getQuaternion());
+   WALBERLA_CHECK_FLOAT_EQUAL(b1.getRotation(), b2.getRotation());
    // Check equality of the meshes
    checkMeshEquals(b1.getMesh(), b2.getMesh());
    WALBERLA_CHECK_EQUAL(b1.getID(), b2.getID());
@@ -90,7 +92,7 @@ void testConvexPolyhedron()
    
    MaterialID iron = Material::find("iron");
    
-   mesh::pe::ConvexPolyhedron b1(759846, 1234794, Vec3(real_c(1), real_c(2), real_c(3)), Vec3(0,0,0), Quat(), *octamesh, iron, false, true, false);
+   mesh::pe::ConvexPolyhedron b1(759846, 1234794, Vec3(real_c(1), real_c(2), real_c(3)), Quat(), *octamesh, iron, false, true, false);
    b1.setLinearVel(Vec3(real_c(5.2), real_c(6.3), real_c(7.4)));
    b1.setAngularVel(Vec3(real_c(1.2), real_c(2.3), real_c(3.4)));
 
@@ -99,7 +101,7 @@ void testConvexPolyhedron()
    mpi::RecvBuffer rb(sb);
 
    auto bPtr = UnmarshalDynamically<BodyTuple>::execute(rb, mesh::pe::ConvexPolyhedron::getStaticTypeID(), math::AABB(Vec3(-100,-100,-100), Vec3(100,100,100)), math::AABB(Vec3(-100,-100,-100), Vec3(100,100,100)));
-   mesh::pe::ConvexPolyhedronID b2 = static_cast<mesh::pe::ConvexPolyhedronID>(bPtr.get());
+   auto b2 = dynamic_cast<mesh::pe::ConvexPolyhedronID>(bPtr.get());
    checkConvexPolyhedronEquals(b1, *b2);
    
 }
@@ -114,11 +116,12 @@ void testUnion()
    qhull.run();
    
    MaterialID iron = Material::find("iron");
-   
-   UnionT u1(159, 423, Vec3(real_c(1), real_c(2), real_c(3)), Vec3(0,0,0), Quat(), false, false, false);
-   u1.add(std::make_unique<mesh::pe::ConvexPolyhedron>(753326, 1267824, Vec3(real_c(2), real_c(2), real_c(3)), Vec3(0,0,0), Quat(), *octamesh, iron, false, true, false));
-   u1.add(std::make_unique<mesh::pe::ConvexPolyhedron>(753246, 1233424, Vec3(real_c(-1), real_c(4), real_c(-2)), Vec3(0,0,0), Quat(), *octamesh, iron, false, true, false));
-   
+   Quat q(real_t(0.3), real_t(0.1), real_t(0.9), real_t(0.3));
+   Quat w(real_t(0.1), real_t(0.3), real_t(0.9), real_t(0.3));
+   UnionT u1(159, 423, Vec3(real_c(1), real_c(2), real_c(3)),  q, false, false, false);
+   u1.add(std::make_unique<mesh::pe::ConvexPolyhedron>(753326, 1267824, Vec3(real_c(2), real_c(2), real_c(3)), w, *octamesh, iron, false, true, false));
+   u1.add(std::make_unique<mesh::pe::ConvexPolyhedron>(753246, 1233424, Vec3(real_c(-1), real_c(4), real_c(-2)), w, *octamesh, iron, false, true, false));
+
    u1.setLinearVel(Vec3(real_c(5.2), real_c(6.3), real_c(7.4)));
    u1.setAngularVel(Vec3(real_c(1.2), real_c(2.3), real_c(3.4)));
    
@@ -127,23 +130,24 @@ void testUnion()
    mpi::RecvBuffer rb(sb);
 
    auto uPtr = UnmarshalDynamically<BodyTuple>::execute(rb, UnionT::getStaticTypeID(), math::AABB(Vec3(-100,-100,-100), Vec3(100,100,100)), math::AABB(Vec3(-100,-100,-100), Vec3(100,100,100)));
-   UnionID u2 = static_cast<UnionID>(uPtr.get());
+   auto u2 = dynamic_cast<UnionID>(uPtr.get());
    WALBERLA_CHECK_NOT_NULLPTR( u2 );
-
    WALBERLA_CHECK_EQUAL(u1.size(), 2);
    WALBERLA_CHECK_EQUAL(u1.size(), u2->size());
-   WALBERLA_CHECK_EQUAL(u1.getInertia(), u2->getInertia());
-   WALBERLA_CHECK_EQUAL(u1.getPosition(), u2->getPosition());
+   WALBERLA_CHECK_FLOAT_EQUAL(u1.getInertia(), u2->getInertia());
+   WALBERLA_CHECK_FLOAT_EQUAL(u1.getPosition(), u2->getPosition());
    WALBERLA_CHECK_FLOAT_EQUAL(u1.getLinearVel(), u2->getLinearVel());
    WALBERLA_CHECK_FLOAT_EQUAL(u1.getAngularVel(), u2->getAngularVel());
-   
+   WALBERLA_CHECK_FLOAT_EQUAL(u1.getQuaternion(), u2->getQuaternion());
+   WALBERLA_CHECK_FLOAT_EQUAL(u1.getRotation(), u2->getRotation());
+
    //getting polyhedrons of first union
-   mesh::pe::ConvexPolyhedronID p11 = static_cast<mesh::pe::ConvexPolyhedronID > (u1.begin().getBodyID());
-   mesh::pe::ConvexPolyhedronID p21 = static_cast<mesh::pe::ConvexPolyhedronID > ((++(u1.begin())).getBodyID());
+   mesh::pe::ConvexPolyhedronID p11 = dynamic_cast<mesh::pe::ConvexPolyhedronID > (u1.begin().getBodyID());
+   mesh::pe::ConvexPolyhedronID p21 = dynamic_cast<mesh::pe::ConvexPolyhedronID > ((++(u1.begin())).getBodyID());
    
    //getting polyhedrons of second union
-   mesh::pe::ConvexPolyhedronID p12 = static_cast<mesh::pe::ConvexPolyhedronID > (u2->begin().getBodyID());
-   mesh::pe::ConvexPolyhedronID p22 = static_cast<mesh::pe::ConvexPolyhedronID > ((++(u2->begin())).getBodyID());
+   mesh::pe::ConvexPolyhedronID p12 = dynamic_cast<mesh::pe::ConvexPolyhedronID > (u2->begin().getBodyID());
+   mesh::pe::ConvexPolyhedronID p22 = dynamic_cast<mesh::pe::ConvexPolyhedronID > ((++(u2->begin())).getBodyID());
    
    checkConvexPolyhedronEquals(*p11, *p12);
    checkConvexPolyhedronEquals(*p21, *p22);
diff --git a/tests/mesh/MeshPeRaytracing.cpp b/tests/mesh/MeshPeRaytracing.cpp
index ce8f27992f7d8e202e797f8e716784d852214b36..a31dd985a9747eec26394629fe0ed58540ecd584 100644
--- a/tests/mesh/MeshPeRaytracing.cpp
+++ b/tests/mesh/MeshPeRaytracing.cpp
@@ -58,9 +58,9 @@ int CpRayIntersectionTest(const int resolution = 10)
 
    const Vec3 center(1,2,3);
 
-   ConvexPolyhedron cp(0, 0, center, Vec3(0,0,0), Quat(), *mesh, Material::find("iron"), false, true, true);
+   ConvexPolyhedron cp(0, 0, center,Quat(), *mesh, Material::find("iron"), false, true, true);
    cp.rotate(real_t(1), real_t(2), real_t(3));
-   Box bx(0, 0, center, Vec3(0,0,0), Quat(), Vec3(2,2,2), Material::find("iron"), false, true, true);
+   Box bx(0, 0, center, Quat(), Vec3(2,2,2), Material::find("iron"), false, true, true);
    bx.rotate(real_t(1), real_t(2), real_t(3));
 
    real_t dx = real_t(1.0) / static_cast<real_t>(resolution);
diff --git a/tests/pe/BodyFlags.cpp b/tests/pe/BodyFlags.cpp
index fa6db7e1c4ba4dd642099424c614c265fe693e4b..cd4f10542bba745ead306a2e736442b40d3d7c40 100644
--- a/tests/pe/BodyFlags.cpp
+++ b/tests/pe/BodyFlags.cpp
@@ -70,12 +70,12 @@ int main( int argc, char ** argv )
 
    MaterialID iron = Material::find("iron");
 
-   Sphere refGlobalSphere(1, 0, Vec3(9, 9, 9), Vec3(0,0,0), Quat(), 3, iron, true, false, true);
+   Sphere refGlobalSphere(1, 0, Vec3(9, 9, 9),  Quat(), 3, iron, true, false, true);
    refGlobalSphere.setLinearVel(Vec3(2,2,2));
    SphereID globalSphere = createSphere( *globalStorage, forest->getBlockStorage(), storageID, 0, Vec3(9,9,9), 3, iron, true, false, true);
    globalSphere->setLinearVel(Vec3(2,2,2));
 
-   Sphere refFixedSphere(2, 0, Vec3(9,9,14), Vec3(0,0,0), Quat(), 3, iron, false, false, true);
+   Sphere refFixedSphere(2, 0, Vec3(9,9,14), Quat(), 3, iron, false, false, true);
    SphereID fixedSphere = createSphere( *globalStorage, forest->getBlockStorage(), storageID, 0, Vec3(9,9,14), 3, iron, false, false, true);
    walberla::id_t fixedSphereID = 0;
    if (fixedSphere != nullptr) fixedSphereID = fixedSphere->getSystemID();
diff --git a/tests/pe/BodyStorage.cpp b/tests/pe/BodyStorage.cpp
index 73063931739512c6a0208f9fe0bde6f0e328c280..1ebdc53fd1e26d048267c8a823ba0166b57c5c3e 100644
--- a/tests/pe/BodyStorage.cpp
+++ b/tests/pe/BodyStorage.cpp
@@ -31,14 +31,14 @@ using namespace walberla::pe;
 class Body1 : public Sphere {
 public:
     static int refCount;
-    Body1(walberla::id_t id, MaterialID matID) : Sphere(id, id, Vec3(0,0,0), Vec3(0,0,0), Quat(), 1, matID, false, true, false) {++refCount;}
+    Body1(walberla::id_t id, MaterialID matID) : Sphere(id, id, Vec3(0,0,0), Quat(), 1, matID, false, true, false) {++refCount;}
     ~Body1() override {--refCount;}
 };
 
 class Body2 : public Sphere {
 public:
     static int refCount;
-    Body2(walberla::id_t id, MaterialID matID) : Sphere(id, id, Vec3(0,0,0), Vec3(0,0,0), Quat(), 1, matID, false, true, false) {++refCount;}
+    Body2(walberla::id_t id, MaterialID matID) : Sphere(id, id, Vec3(0,0,0), Quat(), 1, matID, false, true, false) {++refCount;}
     ~Body2() override {--refCount;}
 };
 
diff --git a/tests/pe/CMakeLists.txt b/tests/pe/CMakeLists.txt
index ec3aab3f860d63674cc3639bb6d53652a9f6231c..ca5346fa4ba72a701e6a77ed15f7dcdbce083cbe 100644
--- a/tests/pe/CMakeLists.txt
+++ b/tests/pe/CMakeLists.txt
@@ -138,6 +138,11 @@ waLBerla_execute_test( NAME   PE_STATICTYPEIDS )
 waLBerla_compile_test( NAME   PE_UNION FILES Union.cpp DEPENDS core  )
 waLBerla_execute_test( NAME   PE_UNION )
 
+waLBerla_compile_test( NAME   PE_UNIONBEHAVIOR FILES UnionBehavior.cpp DEPENDS core  )
+if( WALBERLA_DOUBLE_ACCURACY )
+waLBerla_execute_test( NAME   PE_UNIONBEHAVIOR )
+endif()
+
 waLBerla_compile_test( NAME   PE_RAYTRACING FILES Raytracing.cpp DEPENDS core  )
 waLBerla_execute_test( NAME   PE_RAYTRACING )
 
diff --git a/tests/pe/CheckVitalParameters.cpp b/tests/pe/CheckVitalParameters.cpp
index 063b1b2b34fead971279ca42069717647b727650..77325b7263b144009fc8e6589c02140ef04e5133 100644
--- a/tests/pe/CheckVitalParameters.cpp
+++ b/tests/pe/CheckVitalParameters.cpp
@@ -34,7 +34,7 @@ int main( int argc, char ** argv )
    WALBERLA_UNUSED(env);
 
    MaterialID iron = Material::find("iron");
-   Sphere sphere(walberla::UniqueID<RigidBody>::create(), 0, Vec3(15, 15, 15), Vec3(0,0,0), Quat(), 3, iron, false, true, false );
+   Sphere sphere(walberla::UniqueID<RigidBody>::create(), 0, Vec3(15, 15, 15), Quat(), 3, iron, false, true, false );
    sphere.MPITrait.setOwner( Owner(0, walberla::blockforest::BlockID().getID() ) );
    checkVitalParameters( &sphere, &sphere );
 
diff --git a/tests/pe/Collision.cpp b/tests/pe/Collision.cpp
index 76f0545836f47bc68e42760e8fa44fc691dd9c6c..a69762d7fb928ef9e96b2b93e2712ab259c9e0c2 100644
--- a/tests/pe/Collision.cpp
+++ b/tests/pe/Collision.cpp
@@ -55,10 +55,10 @@ void checkContact(const Contact& c1, const Contact& c2)
 void SphereTest()
 {
    MaterialID iron = Material::find("iron");
-   Sphere sp1(123, 1, Vec3(0,0,0), Vec3(0,0,0), Quat(), 1, iron, false, true, false);
-   Sphere sp2(124, 2, Vec3(real_t(1.5),0,0), Vec3(0,0,0), Quat(), 1, iron, false, true, false);
-   Sphere sp3(125, 3, Vec3(real_t(3.0),0,0), Vec3(0,0,0), Quat(), 1, iron, false, true, false);
-   Sphere sp4(124, 2, Vec3(0,real_t(1.5),0), Vec3(0,0,0), Quat(), 1, iron, false, true, false);
+   Sphere sp1(123, 1, Vec3(0,0,0), Quat(), 1, iron, false, true, false);
+   Sphere sp2(124, 2, Vec3(real_t(1.5),0,0), Quat(), 1, iron, false, true, false);
+   Sphere sp3(125, 3, Vec3(real_t(3.0),0,0), Quat(), 1, iron, false, true, false);
+   Sphere sp4(124, 2, Vec3(0,real_t(1.5),0), Quat(), 1, iron, false, true, false);
    Plane  pl1(223, 1, Vec3(0,0,0), Vec3(1,1,1).getNormalized(), 0, iron);
    CylindricalBoundary cb1(333, 0, Vec3(-100,0,0), 2, iron);
 
@@ -112,13 +112,13 @@ void SphereTest()
 void BoxTest()
 {
    MaterialID iron = Material::find("iron");
-   Box b1(123, 0, Vec3(0,0,0), Vec3(0,0,0), Quat(), Vec3(2,2,2), iron, false, true, false);
-   Box b2(124, 0, Vec3(real_t(1.5),0,0), Vec3(0,0,0), Quat(), Vec3(2,2,2), iron, false, true, false);
-   Box b3(125, 0, Vec3(real_t(3.0),0,0), Vec3(0,0,0), Quat(), Vec3(2,2,2), iron, false, true, false);
-   Box b4(123, 0, Vec3(0,0,0), Vec3(0,0,0), Quat(), Vec3(2,2,2), iron, false, true, false);
+   Box b1(123, 0, Vec3(0,0,0), Quat(), Vec3(2,2,2), iron, false, true, false);
+   Box b2(124, 0, Vec3(real_t(1.5),0,0), Quat(), Vec3(2,2,2), iron, false, true, false);
+   Box b3(125, 0, Vec3(real_t(3.0),0,0), Quat(), Vec3(2,2,2), iron, false, true, false);
+   Box b4(123, 0, Vec3(0,0,0), Quat(), Vec3(2,2,2), iron, false, true, false);
    b4.rotate( Vec3(1,1,0), real_t(atan(sqrt(2))) );
 
-   Box b5(123, 0, Vec3(0,0,0), Vec3(0,0,0), Quat(), Vec3(2,2,2), iron, false, true, false);
+   Box b5(123, 0, Vec3(0,0,0), Quat(), Vec3(2,2,2), iron, false, true, false);
    b5.rotate( Vec3(0,0,1), real_t(math::M_PI * 0.25) );
    b5.rotate( Vec3(1,0,0), real_t(math::M_PI * 0.25) );
 
@@ -157,7 +157,7 @@ void BoxTest()
    b5.setPosition( (Vec3(0,0,1) * real_t(sqrt(3)) + Vec3(0,0,1)) * 1.01);
    WALBERLA_CHECK( !collideFunc(&b1, &b5) );
 
-   Sphere s1(126, 0, Vec3(real_t(1.5), real_t(1.5), real_t(1.5)), Vec3(0,0,0), Quat(), 1, iron, false, true, false);
+   Sphere s1(126, 0, Vec3(real_t(1.5), real_t(1.5), real_t(1.5)), Quat(), 1, iron, false, true, false);
    WALBERLA_CHECK( collideFunc(&b1, &s1) );
 //   WALBERLA_LOG_WARNING("contactPoint    : " << contactPoint);
 //   WALBERLA_LOG_WARNING("contactNormal   : " << contactNormal);
@@ -167,8 +167,8 @@ void BoxTest()
 void CapsuleTest()
 {
    MaterialID iron = Material::find("iron");
-   Capsule c1(100, 100, Vec3(0,0,0), Vec3(0,0,0), Quat(), 1, 2, iron, false, true, false);
-   Sphere sp1(123, 123, Vec3(0,0,0), Vec3(0,0,0), Quat(), 1, iron, false, true, false);
+   Capsule c1(100, 100, Vec3(0,0,0), Quat(), 1, 2, iron, false, true, false);
+   Sphere sp1(123, 123, Vec3(0,0,0), Quat(), 1, iron, false, true, false);
 
    std::vector<Contact> contacts;
    fcd::AnalyticCollideFunctor< std::vector<Contact> > collideFunc(contacts);
@@ -210,9 +210,9 @@ void CapsuleTest2()
    const real_t   dynamic_cof ( static_cof ); // Coefficient of dynamic friction. Similar to static friction for low speed friction.
    MaterialID     material = createMaterial( "granular", real_t( 1.0 ), 0, static_cof, dynamic_cof, real_t( 0.5 ), 1, 1, 0, 0 );
    //create obstacle
-   Capsule c1(100, 100, Vec3(10,10,0), Vec3(0,0,0), Quat(), 3, 40, material, false, true, false);
+   Capsule c1(100, 100, Vec3(10,10,0), Quat(), 3, 40, material, false, true, false);
    c1.rotate( Vec3(0,1,0), math::M_PI * real_t(0.5) );
-   Sphere sp1(123, 123, Vec3(real_t(6.5316496854295262864), real_t(10.099999999999999645), real_t(0.46999999991564372914) ), Vec3(0,0,0), Quat(), real_t(0.47), material, false, true, false);
+   Sphere sp1(123, 123, Vec3(real_t(6.5316496854295262864), real_t(10.099999999999999645), real_t(0.46999999991564372914) ), Quat(), real_t(0.47), material, false, true, false);
 
    std::vector<Contact> contacts;
 
@@ -236,8 +236,8 @@ void CapsuleTest2()
 void UnionTest()
 {
    using UnionT = Union<Sphere>;
-   UnionT  un1(120, 0, Vec3(0,0,0), Vec3(0,0,0), Quat(), false, true, false);
-   UnionT  un2(121, 0, Vec3(real_t(1.5),0,0), Vec3(0,0,0), Quat(), false, true, false);
+   UnionT  un1(120, 0, Vec3(0,0,0), Quat(), false, true, false);
+   UnionT  un2(121, 0, Vec3(real_t(1.5),0,0), Quat(), false, true, false);
    auto sp1 = createSphere(&un1, 123, Vec3(0,0,0), 1);
    auto sp2 = createSphere(&un2, 124, Vec3(real_t(1.5),0,0), 1);
 
diff --git a/tests/pe/CollisionTobiasGJK.cpp b/tests/pe/CollisionTobiasGJK.cpp
index 6473222d06c2ec09466f0701cce0735e7af7762f..41bdf1f691d44ab36dfbe8b4435cdfdbc6531909 100644
--- a/tests/pe/CollisionTobiasGJK.cpp
+++ b/tests/pe/CollisionTobiasGJK.cpp
@@ -160,9 +160,9 @@ void MainTest()
    MaterialID iron = Material::find("iron");
 
    // Original SPHERE <-> SPHERE
-   Sphere sp1(123, 1, Vec3(0,0,0), Vec3(0,0,0), Quat(), 1, iron, false, true, false);
-   Sphere sp2(124, 2, Vec3(real_t(1.5),0,0), Vec3(0,0,0), Quat(), 1, iron, false, true, false);
-   Sphere sp3(125, 3, Vec3(real_t(3.0),0,0), Vec3(0,0,0), Quat(), 1, iron, false, true, false);
+   Sphere sp1(123, 1, Vec3(0,0,0), Quat(), 1, iron, false, true, false);
+   Sphere sp2(124, 2, Vec3(real_t(1.5),0,0), Quat(), 1, iron, false, true, false);
+   Sphere sp3(125, 3, Vec3(real_t(3.0),0,0), Quat(), 1, iron, false, true, false);
 
    Vec3     normal;
    Vec3     contactPoint;
@@ -179,8 +179,8 @@ void MainTest()
    WALBERLA_LOG_INFO("Test 01: BOX <-> SPHERE");
    real_t sqr3_inv = real_t(1.0)/std::sqrt(real_t(3.0));
    real_t coordinate= real_t(5.0)* sqr3_inv + real_t(5.0); // 5*(1+ (1/sqrt(3)))
-   Box box1_1(127, 5, Vec3(0, 0, 0), Vec3(0,0,0), Quat(), Vec3(10, 10, 10), iron, false, true, false);
-   Sphere sphere1_2(130, 8, Vec3(coordinate, coordinate, coordinate), Vec3(0,0,0), Quat(), 5, iron, false, true, false);
+   Box box1_1(127, 5, Vec3(0, 0, 0), Quat(), Vec3(10, 10, 10), iron, false, true, false);
+   Sphere sphere1_2(130, 8, Vec3(coordinate, coordinate, coordinate), Quat(), 5, iron, false, true, false);
    Vec3 wp1(real_t(5.0), real_t(5.0), real_t(5.0));
    Vec3 wpm1(sqr3_inv*real_t(-0.5), sqr3_inv*real_t(-0.5), sqr3_inv*real_t(-0.5));
    Vec3 axis1(-sqr3_inv, -sqr3_inv, -sqr3_inv);
@@ -189,7 +189,7 @@ void MainTest()
    //Testcase 02 Box LongBox (touching plane)
    //Reuse box1_1
    WALBERLA_LOG_INFO("Test 02: BOX <-> LONG BOX");
-   Box box2_1(131, 9, Vec3(real_t(20.0),0,0), Vec3(0,0,0), Quat(), Vec3(real_t(30.0),1,1), iron, false, true, false);
+   Box box2_1(131, 9, Vec3(real_t(20.0),0,0),  Quat(), Vec3(real_t(30.0),1,1), iron, false, true, false);
    Vec3 wp2(5, 0, 0);
    Vec3 wpm2(real_t(-0.5),0,0);
    Vec3 axis2(-1,0,0);
@@ -197,8 +197,8 @@ void MainTest()
 
    //Testcase 03 Sphere Sphere
    WALBERLA_LOG_INFO("Test 03: SPHERE <-> SPHERE");
-   Sphere sphere3_1(129, 7, Vec3(0,0,0), Vec3(0,0,0), Quat(), 5, iron, false, true, false);
-   Sphere sphere3_2(128, 6, Vec3(real_t(10.0),0,0), Vec3(0,0,0), Quat(), 5, iron, false, true, false);
+   Sphere sphere3_1(129, 7, Vec3(0,0,0), Quat(), 5, iron, false, true, false);
+   Sphere sphere3_2(128, 6, Vec3(real_t(10.0),0,0), Quat(), 5, iron, false, true, false);
    Vec3 wp3(5, 0, 0);
    Vec3 wpm3(real_t(-0.5),0,0);
    Vec3 axis3(-1,0,0);
@@ -213,8 +213,8 @@ void MainTest()
 
    //create turned box
    real_t sqr2 = std::sqrt(real_t(2.0));
-   Box box4_1(132, 10, Vec3(real_t(5.0)*(real_t(1.0)+sqr2), real_t(-5.0), 0), Vec3(0,0,0), q4, Vec3(10, 10, 10), iron, false, true, false);
-   Box box4_2(133, 11, Vec3(0, 0, 0), Vec3(0,0,0), Quat(), Vec3(10, 10, 10), iron, false, true, false);
+   Box box4_1(132, 10, Vec3(real_t(5.0)*(real_t(1.0)+sqr2), real_t(-5.0), 0), q4, Vec3(10, 10, 10), iron, false, true, false);
+   Box box4_2(133, 11, Vec3(0, 0, 0), Quat(), Vec3(10, 10, 10), iron, false, true, false);
    Vec3 wp4(5, -5, 0);
    Vec3 wpm4(real_t(-0.25),real_t(+0.25),0);
    Vec3 collision_axis4(-sqr2/real_t(2.0),+sqr2/real_t(2.0),0);
@@ -224,8 +224,8 @@ void MainTest()
 
    //Testcase 05 Cube and Long Box non-centric (touching plane)
    WALBERLA_LOG_INFO("Test 05: CUBE <-> LONG BOX (NON_CENTRIC)");
-   Box box5_1(133, 12, Vec3(0, 0, 0), Vec3(0,0,0), Quat(), Vec3(10, 10, 10), iron, false, true, false);
-   Box box5_2(134, 13, Vec3(real_t(15.0),real_t(5.5), 0), Vec3(0,0,0), Quat(), Vec3(real_t(30.0),1,1), iron, false, true, false);
+   Box box5_1(133, 12, Vec3(0, 0, 0),  Quat(), Vec3(10, 10, 10), iron, false, true, false);
+   Box box5_2(134, 13, Vec3(real_t(15.0),real_t(5.5), 0), Quat(), Vec3(real_t(30.0),1,1), iron, false, true, false);
    Vec3 wp5(real_t(3.75), 5, 0);
    Vec3 wpm5(0, real_t(-0.5), 0);
    Vec3 axis5(0, -1, 0);
@@ -243,8 +243,8 @@ void MainTest()
    Quat q6(rot_axis6, angle6);
 
    //create turned box with pos = (5*(1+sqrt(3)), 0, 0)
-   Box box6_1(136, 14, Vec3(real_t(5.0)*(real_t(1.0)+sqr6_3), 0, 0), Vec3(0,0,0), q6, Vec3(10, 10, 10), iron, false, true, false);
-   Box box6_2(136, 15, Vec3(0, 0, 0), Vec3(0,0,0), Quat(), Vec3(10, 10, 10), iron, false, true, false);
+   Box box6_1(136, 14, Vec3(real_t(5.0)*(real_t(1.0)+sqr6_3), 0, 0),  q6, Vec3(10, 10, 10), iron, false, true, false);
+   Box box6_2(136, 15, Vec3(0, 0, 0), Quat(), Vec3(10, 10, 10), iron, false, true, false);
    Vec3 wp6(5, 0, 0);
    Vec3 wpm6(real_t(-0.5), 0, 0);
    Vec3 axis6(-1, 0, 0);
@@ -253,8 +253,8 @@ void MainTest()
    //Testcase 07:
    // BOX <-> SPHERE
    WALBERLA_LOG_INFO("Test 07: BOX <-> SPHERE");
-   Sphere sphere7_1(137, 16, Vec3(0,0,0), Vec3(0,0,0), Quat(), 5, iron, false, true, false);
-   Box box7_2(138, 17, Vec3(0, 0,real_t(7.5)), Vec3(0,0,0), Quat(), Vec3(5, 5, 5), iron, false, true, false);
+   Sphere sphere7_1(137, 16, Vec3(0,0,0),  Quat(), 5, iron, false, true, false);
+   Box box7_2(138, 17, Vec3(0, 0,real_t(7.5)), Quat(), Vec3(5, 5, 5), iron, false, true, false);
    Vec3 wpm7(0, 0, real_t(-0.5));
    Vec3 wp7(0, 0, real_t(5.0));
    Vec3 axis7(0, 0,  real_t(-1.0));
@@ -264,8 +264,8 @@ void MainTest()
    // CAPSULE <-> CAPSULE
    WALBERLA_LOG_INFO("Test 08: CAPSULE <-> CAPSULE");
    Quat q8(Vec3(0,1,0), walberla::math::M_PI/real_t(2.0)); //creates a y-axis aligned capsule
-   Capsule cap8_1(139, 18, Vec3(0,0,0), Vec3(0,0,0), Quat(), real_t(4.0), real_t(10.0), iron, false, true, false);
-   Capsule cap8_2(140, 19, Vec3(0,0, real_t(13.0)), Vec3(0,0,0), q8, real_t(4.0), real_t(10.0), iron, false, true, false);
+   Capsule cap8_1(139, 18, Vec3(0,0,0), Quat(), real_t(4.0), real_t(10.0), iron, false, true, false);
+   Capsule cap8_2(140, 19, Vec3(0,0, real_t(13.0)),  q8, real_t(4.0), real_t(10.0), iron, false, true, false);
    Vec3 wpm8(0, 0, real_t(-0.5));
    Vec3 wp8(0, 0, real_t(4.0));
    Vec3 axis8(0, 0,  real_t(-1.0));
@@ -274,8 +274,8 @@ void MainTest()
    //Testcase 09:
    // ELLIPSOID <-> ELLIPSOID
    WALBERLA_LOG_INFO("Test 09: ELLIPSOID <-> ELLIPSOID");
-   Ellipsoid ell9_1(141, 20, Vec3(0,0,0), Vec3(0,0,0), Quat(), Vec3(10,5,5), iron, false, true, false);
-   Ellipsoid ell9_2(142, 21, Vec3(15,0,0), Vec3(0,0,0), Quat(), Vec3(5,10,5), iron, false, true, false);
+   Ellipsoid ell9_1(141, 20, Vec3(0,0,0), Quat(), Vec3(10,5,5), iron, false, true, false);
+   Ellipsoid ell9_2(142, 21, Vec3(15,0,0), Quat(), Vec3(5,10,5), iron, false, true, false);
    Vec3 wpm9(real_t(-0.5), 0, 0);
    Vec3 wp9(real_t(10), 0, 0);
    Vec3 axis9(real_t(-1.0), 0, 0);
@@ -292,8 +292,8 @@ void PlaneTest()
    fcd::GenericFCD<BodyTuple, fcd::GJKEPACollideFunctor> testFCD;
 
    Plane pl(1, 1, Vec3(0, 1, 0), Vec3(0, 1, 0), real_t(1.0), iron );
-   Sphere sphere(2, 2, Vec3(0, real_t(1.9), 0), Vec3(0,0,0), Quat(), 1, iron, false, true, false);
-   Sphere sphere2(3, 3, Vec3(0, real_t(0.1), 0), Vec3(0,0,0), Quat(), 1, iron, false, true, false);
+   Sphere sphere(2, 2, Vec3(0, real_t(1.9), 0), Quat(), 1, iron, false, true, false);
+   Sphere sphere2(3, 3, Vec3(0, real_t(0.1), 0), Quat(), 1, iron, false, true, false);
 
    PossibleContacts pcs;
 
@@ -353,17 +353,17 @@ void UnionTest(){
    fcd::GenericFCD<BodyTuple, fcd::GJKEPACollideFunctor> testFCD;
 
    //A recursive union of three spheres is dropped on a box.
-   Box box(179, 179, Vec3(0,0,0), Vec3(0,0,0), Quat(), Vec3(real_t(10),real_t(2), real_t(10)), iron, false, true, false);
+   Box box(179, 179, Vec3(0,0,0), Quat(), Vec3(real_t(10),real_t(2), real_t(10)), iron, false, true, false);
 
 
    using UnionT = Union<Sphere>;
-   auto unsub = std::make_unique<UnionT>(192, 192, Vec3(0,real_t(3.8),0), Vec3(0,0,0), Quat(), false, true, false);
+   auto unsub = std::make_unique<UnionT>(192, 192, Vec3(0,real_t(3.8),0), Quat(), false, true, false);
 
    auto sp1 = createSphere(unsub.get(), 180, Vec3(-3,real_t(3.8),0), real_t(3.0));
    auto sp2 = createSphere(unsub.get(), 181, Vec3(3,real_t(3.8),0), real_t(3.0));
 
    //Create another union, and add sub union
-   Union<Sphere, Union<Sphere>> un(193, 193, Vec3(0, 0, 0), Vec3(0,0,0), Quat(), false, true, false);
+   Union<Sphere, Union<Sphere>> un(193, 193, Vec3(0,0,0), Quat(), false, true, false);
    createSphere(&un, 182, Vec3(0,real_t(6),0), real_t(3.0));
    un.add(std::move(unsub));
 
diff --git a/tests/pe/Marshalling.cpp b/tests/pe/Marshalling.cpp
index fc18f19eb667f6dac9e2098fbba53af8e00aa446..52702aa49862d535323739d5002b547801a9547e 100644
--- a/tests/pe/Marshalling.cpp
+++ b/tests/pe/Marshalling.cpp
@@ -50,7 +50,7 @@ void testBox()
 
    MaterialID iron = Material::find("iron");
 
-   Box b1(759846, 1234794, Vec3(real_c(1), real_c(2), real_c(3)), Vec3(0,0,0), Quat(), Vec3(1,2,3), iron, false, true, false);
+   Box b1(759846, 1234794, Vec3(real_c(1), real_c(2), real_c(3)), Quat(), Vec3(1,2,3), iron, false, true, false);
    b1.setLinearVel(Vec3(real_c(5.2), real_c(6.3), real_c(7.4)));
    b1.setAngularVel(Vec3(real_c(1.2), real_c(2.3), real_c(3.4)));
 
@@ -75,7 +75,7 @@ void testCapsule()
 
    MaterialID iron = Material::find("iron");
 
-   Capsule c1(759846, 1234794, Vec3(real_c(1), real_c(2), real_c(3)), Vec3(0,0,0), Quat(), 5, 7, iron, false, false, false);
+   Capsule c1(759846, 1234794, Vec3(real_c(1), real_c(2), real_c(3)), Quat(), 5, 7, iron, false, false, false);
    c1.setLinearVel(Vec3(real_c(5.2), real_c(6.3), real_c(7.4)));
    c1.setAngularVel(Vec3(real_c(1.2), real_c(2.3), real_c(3.4)));
 
@@ -101,7 +101,7 @@ void testSphere()
 
    MaterialID iron = Material::find("iron");
 
-   Sphere s1(759846, 1234794, Vec3(real_c(1), real_c(2), real_c(3)), Vec3(0,0,0), Quat(), 5, iron, false, false, false);
+   Sphere s1(759846, 1234794, Vec3(real_c(1), real_c(2), real_c(3)),  Quat(), 5, iron, false, false, false);
    s1.setLinearVel(Vec3(real_c(5.2), real_c(6.3), real_c(7.4)));
    s1.setAngularVel(Vec3(real_c(1.2), real_c(2.3), real_c(3.4)));
 
@@ -126,7 +126,7 @@ void testSquirmer()
 
    MaterialID iron = Material::find("iron");
 
-   Squirmer s1(759846, 1234794, Vec3(real_c(1), real_c(2), real_c(3)), Vec3(0,0,0), Quat(), real_c(5), real_c(0.1), real_c(4.93), iron, false, false, false);
+   Squirmer s1(759846, 1234794, Vec3(real_c(1), real_c(2), real_c(3)), Quat(), real_c(5), real_c(0.1), real_c(4.93), iron, false, false, false);
 
    mpi::SendBuffer sb;
    WALBERLA_ASSERT_UNEQUAL(Sphere::getStaticTypeID(), Squirmer::getStaticTypeID(), "Squirmer did not get its own type ID");
@@ -147,7 +147,7 @@ void testEllipsoid()
 
    MaterialID iron = Material::find("iron");
 
-   Ellipsoid e1(759847, 1234795, Vec3(real_c(1), real_c(2), real_c(3)), Vec3(0,0,0), Quat(), Vec3(3,1,5), iron, false, false, false);
+   Ellipsoid e1(759847, 1234795, Vec3(real_c(1), real_c(2), real_c(3)), Quat(), Vec3(3,1,5), iron, false, false, false);
    e1.setLinearVel(Vec3(real_c(5.2), real_c(6.3), real_c(7.4)));
    e1.setAngularVel(Vec3(real_c(1.2), real_c(2.3), real_c(3.4)));
 
@@ -169,7 +169,7 @@ void testEllipsoid()
 void testUnion()
 {
    WALBERLA_LOG_INFO_ON_ROOT("*** testUnion ***");
-   UnionT u1(159, 423, Vec3(real_c(1), real_c(2), real_c(3)), Vec3(0,0,0), Quat(), false, false, false);
+   UnionT u1(159, 423, Vec3(real_c(1), real_c(2), real_c(3)), Quat(), false, false, false);
    SphereID s11 = createSphere(&u1, 1234794, Vec3(real_c(1), real_c(2), real_c(3)), 2);
    SphereID s21 = createSphere(&u1, 4567789, Vec3(real_c(3), real_c(2), real_c(3)), real_c(1.5));
    WALBERLA_CHECK_NOT_NULLPTR( s11 );
@@ -185,6 +185,8 @@ void testUnion()
 
    WALBERLA_CHECK_EQUAL(u1.size(), 2);
    WALBERLA_CHECK_EQUAL(u1.size(), u2->size());
+   // More exhaustive tests (with inertia, rotation, etc.)
+   // can be found in tests/mesh/MeshMarshalling.cpp
 
    //getting spheres of second union
    SphereID s12 = static_cast<SphereID> (u2->begin().getBodyID());
diff --git a/tests/pe/Raytracing.cpp b/tests/pe/Raytracing.cpp
index 25e925b8acfba4148b9b7258cd8adc1a12a142fa..15dd6ae60f0e7c8ea8e2d70310672c107cca90c4 100644
--- a/tests/pe/Raytracing.cpp
+++ b/tests/pe/Raytracing.cpp
@@ -43,7 +43,7 @@ typedef std::tuple<Box, Plane, Sphere, Capsule, Ellipsoid> BodyTuple ;
 void SphereIntersectsTest()
 {
    MaterialID iron = Material::find("iron");
-   Sphere sp1(123, 1, Vec3(3,3,3), Vec3(0,0,0), Quat(), 2, iron, false, true, false);
+   Sphere sp1(123, 1, Vec3(3,3,3), Quat(), 2, iron, false, true, false);
    real_t t;
    Vec3 n;
    
@@ -62,11 +62,11 @@ void SphereIntersectsTest()
    WALBERLA_CHECK(intersects(&sp1, ray2, t, n));
    
    // sphere behind ray origin
-   Sphere sp2(123, 1, Vec3(3,-8,3), Vec3(0,0,0), Quat(), 2, iron, false, true, false);
+   Sphere sp2(123, 1, Vec3(3,-8,3), Quat(), 2, iron, false, true, false);
    WALBERLA_CHECK(!intersects(&sp2, ray1, t, n));
    
    // sphere around ray origin
-   Sphere sp3(123, 1, Vec3(3,-5,3), Vec3(0,0,0), Quat(), 2, iron, false, true, false);
+   Sphere sp3(123, 1, Vec3(3,-5,3), Quat(), 2, iron, false, true, false);
    WALBERLA_CHECK(intersects(&sp3, ray1, t, n));
    WALBERLA_CHECK_FLOAT_EQUAL(t, real_t(2));
 }
@@ -128,15 +128,15 @@ void BoxIntersectsTest() {
    real_t t;
    Vec3 n;
    
-   Box box1(127, 5, Vec3(0, -15, 0), Vec3(0, 0, 0), Quat(), Vec3(10, 10, 10), iron, false, true, false);
+   Box box1(127, 5, Vec3(0, -15, 0),  Quat(), Vec3(10, 10, 10), iron, false, true, false);
    Ray ray1(Vec3(3,-5,3), Vec3(0,1,0));
    WALBERLA_CHECK(!intersects(&box1, ray1, t, n));
    
-   Box box2(128, 5, Vec3(0, -2, 0), Vec3(0, 0, 0), Quat(), Vec3(10, 10, 10), iron, false, true, false);
+   Box box2(128, 5, Vec3(0, -2, 0), Quat(), Vec3(10, 10, 10), iron, false, true, false);
    WALBERLA_CHECK(intersects(&box2, ray1, t, n));
    WALBERLA_CHECK_FLOAT_EQUAL_EPSILON(t, real_t(8), real_t(1e-7));
    
-   Box box3(128, 5, Vec3(0, 5, 0), Vec3(0, 0, 0), Quat(), Vec3(10, 10, 10), iron, false, true, false);
+   Box box3(128, 5, Vec3(0, 5, 0), Quat(), Vec3(10, 10, 10), iron, false, true, false);
    WALBERLA_CHECK(intersects(&box3, ray1, t, n));
    WALBERLA_CHECK_FLOAT_EQUAL(t, real_t(5));
    
@@ -160,14 +160,14 @@ void BoxIntersectsTest() {
    WALBERLA_CHECK_FLOAT_EQUAL_EPSILON(t, real_t(7.0710), real_t(1e-4));
    
    Ray ray3(Vec3(3,-5,3), Vec3(2, real_t(-1.5), real_t(0.5)).getNormalized());
-   Box box4(128, 5, Vec3(0, 8, 0), Vec3(0, 0, 0), Quat(), Vec3(10, 10, 10), iron, false, true, false);
+   Box box4(128, 5, Vec3(0, 8, 0), Quat(), Vec3(10, 10, 10), iron, false, true, false);
    WALBERLA_CHECK(!intersects(&box4, ray3, t, n));
    
    Ray ray4(Vec3(3,-5,3), Vec3(-2, 3, real_t(0.5)).getNormalized());
    WALBERLA_CHECK(intersects(&box4, ray4, t, n));
    WALBERLA_CHECK_FLOAT_EQUAL_EPSILON(t, real_t(9.7068), real_t(1e-4));
    
-   Box box5(128, 5, Vec3(4, 0, 0), Vec3(0, 0, 0), Quat(), Vec3(4, 4, 4), iron, false, true, false);
+   Box box5(128, 5, Vec3(4, 0, 0), Quat(), Vec3(4, 4, 4), iron, false, true, false);
    box5.rotate(0,0,math::M_PI/4);
    Ray ray5(Vec3(0,1.5,0), Vec3(1,0,0));
    WALBERLA_CHECK(intersects(&box5, ray5, t, n));
@@ -202,7 +202,7 @@ void CapsuleIntersectsTest() {
    real_t t;
    Vec3 n;
    
-   Capsule cp1(0, 0, Vec3(2,3,3), Vec3(0,0,0), Quat(), real_t(2), real_t(2), iron, false, true, false);
+   Capsule cp1(0, 0, Vec3(2,3,3), Quat(), real_t(2), real_t(2), iron, false, true, false);
    
    // ray through the center
    Ray ray1(Vec3(3,-5,3), Vec3(0,1,0));
@@ -229,7 +229,7 @@ void EllipsoidTest() {
    real_t t;
    Vec3 n;
    
-   Ellipsoid el1(0,0, Vec3(2,3,3), Vec3(0,0,0), Quat(), Vec3(2,3,1), iron, false, true, false);
+   Ellipsoid el1(0,0, Vec3(2,3,3), Quat(), Vec3(2,3,1), iron, false, true, false);
    
    Ray ray1(Vec3(-2,3,3), Vec3(1,0,0).getNormalized());
    WALBERLA_CHECK(intersects(&el1, ray1, t, n));
diff --git a/tests/pe/RigidBody.cpp b/tests/pe/RigidBody.cpp
index f72d9ce0b5d82597fe72a36d60744edb79f4c2d2..083c6ab43dba6b7cf9fa639b57668529e316599e 100644
--- a/tests/pe/RigidBody.cpp
+++ b/tests/pe/RigidBody.cpp
@@ -82,10 +82,10 @@ void move( BodyStorage& storage, real_t dt )
 void checkRotationFunctions()
 {
    MaterialID iron = Material::find("iron");
-   auto sp1 = std::make_shared<Sphere>( 0, 0, Vec3(0,0,0), Vec3(0,0,0), Quat(), real_t(1), iron, false, true, false );
-   auto sp2 = std::make_shared<Sphere>( 0, 0, Vec3(0,0,0), Vec3(0,0,0), Quat(), real_t(1), iron, false, true, false );
-   auto sp3 = std::make_shared<Sphere>( 0, 0, Vec3(0,0,0), Vec3(0,0,0), Quat(), real_t(1), iron, false, true, false );
-   auto sp4 = std::make_shared<Sphere>( 0, 0, Vec3(0,0,0), Vec3(0,0,0), Quat(), real_t(1), iron, false, true, false );
+   auto sp1 = std::make_shared<Sphere>( 0, 0, Vec3(0,0,0), Quat(), real_t(1), iron, false, true, false );
+   auto sp2 = std::make_shared<Sphere>( 0, 0, Vec3(0,0,0), Quat(), real_t(1), iron, false, true, false );
+   auto sp3 = std::make_shared<Sphere>( 0, 0, Vec3(0,0,0), Quat(), real_t(1), iron, false, true, false );
+   auto sp4 = std::make_shared<Sphere>( 0, 0, Vec3(0,0,0), Quat(), real_t(1), iron, false, true, false );
 
    sp1->rotate( 1, 0, 0, math::M_PI * real_t(0.5));
    sp1->rotate( 0, 1, 0, math::M_PI * real_t(0.5));
@@ -121,7 +121,7 @@ void checkRotationFunctions()
 void checkPointFunctions()
 {
    MaterialID iron = Material::find("iron");
-   auto sp1 = std::make_shared<Sphere>( 0, 0, Vec3(10,10,10), Vec3(0,0,0), Quat(), real_t(1), iron, false, true, false );
+   auto sp1 = std::make_shared<Sphere>( 0, 0, Vec3(10,10,10), Quat(), real_t(1), iron, false, true, false );
 
    WALBERLA_CHECK( sp1->containsPoint( 10, 10, 10 ) );
    WALBERLA_CHECK( sp1->containsPoint( real_c(10.9), 10, 10 ) );
@@ -148,7 +148,7 @@ int main( int argc, char** argv )
 
    MaterialID iron = Material::find("iron");
    BodyStorage storage;
-   SpherePtr spPtr = std::make_unique<Sphere>(0, 0, Vec3(0,0,0), Vec3(0,0,0), Quat(), real_t(1), iron, false, true, false);
+   SpherePtr spPtr = std::make_unique<Sphere>(0, 0, Vec3(0,0,0), Quat(), real_t(1), iron, false, true, false);
    SphereID sphere = static_cast<SphereID>(&storage.add(std::move(spPtr)));
 
    Vec3 x0 = Vec3(-2,2,0);
diff --git a/tests/pe/SetBodyTypeIDs.cpp b/tests/pe/SetBodyTypeIDs.cpp
index 21e2fcd5c822e39928053d34acd52b630ea500e5..f6e1482a084681d549ad71043a1919db4a1ccddf 100644
--- a/tests/pe/SetBodyTypeIDs.cpp
+++ b/tests/pe/SetBodyTypeIDs.cpp
@@ -132,8 +132,8 @@ int main( int argc, char** argv )
    WALBERLA_CHECK_UNEQUAL(Plane::getStaticTypeID(), Capsule::getStaticTypeID());
 
    SingleTypeFunctor singleFunc;
-   Box     bx (0, 0, Vec3(0), Vec3(0), Quat(), Vec3(1), Material::find("iron"), false, false, false);
-   Capsule cap(0, 0, Vec3(0), Vec3(0), Quat(), 1, 1, Material::find("iron"), false, false, false);
+   Box     bx (0, 0, Vec3(0), Quat(), Vec3(1), Material::find("iron"), false, false, false);
+   Capsule cap(0, 0, Vec3(0), Quat(), 1, 1, Material::find("iron"), false, false, false);
 
    SingleCast<BodyTuple2, SingleTypeFunctor, void>::execute(Box::getStaticTypeID(), singleFunc);
    SingleCast<BodyTuple2, SingleTypeFunctor, void>::execute(Capsule::getStaticTypeID(), singleFunc);
diff --git a/tests/pe/SimpleCCD.cpp b/tests/pe/SimpleCCD.cpp
index 3d0d5bc7e11e844fd5b672fc0207639d8565c7a3..63e9dc2398f3057c1af3fcd00263effe3b046ef8 100644
--- a/tests/pe/SimpleCCD.cpp
+++ b/tests/pe/SimpleCCD.cpp
@@ -60,7 +60,7 @@ int main( int argc, char** argv )
     math::seedRandomGenerator(1337);
 
     for (uint_t i = 0; i < 100; ++i)
-      storage[0].add( std::make_unique<Sphere>(UniqueID<Sphere>::createGlobal(), 0, Vec3( math::realRandom(real_c(0), real_c(10)), math::realRandom(real_c(0), real_c(10)), math::realRandom(real_c(0), real_c(10))), Vec3(0,0,0), Quat(), real_t(1), iron, false, false, false) );
+      storage[0].add( std::make_unique<Sphere>(UniqueID<Sphere>::createGlobal(), 0, Vec3( math::realRandom(real_c(0), real_c(10)), math::realRandom(real_c(0), real_c(10)), math::realRandom(real_c(0), real_c(10))), Quat(), real_t(1), iron, false, false, false) );
 
     sccd.generatePossibleContacts();
 
@@ -84,14 +84,14 @@ int main( int argc, char** argv )
 
     bs.clear();
 
-    bs.add( std::make_unique<Sphere>(UniqueID<Sphere>::createGlobal(), 0, Vec3( math::realRandom(real_c(0), real_c(10)), math::realRandom(real_c(0), real_c(10)), math::realRandom(real_c(0), real_c(10))), Vec3(0,0,0), Quat(), real_t(1), iron, false, false, false) );
+    bs.add( std::make_unique<Sphere>(UniqueID<Sphere>::createGlobal(), 0, Vec3( math::realRandom(real_c(0), real_c(10)), math::realRandom(real_c(0), real_c(10)), math::realRandom(real_c(0), real_c(10))), Quat(), real_t(1), iron, false, false, false) );
 
     WcTimingPool pool;
     for (int runs = 0; runs < 10; ++runs)
     {
        auto oldSize = bs.size();
        for (uint_t i = 0; i < oldSize; ++i)
-         bs.add( std::make_unique<Sphere>(UniqueID<Sphere>::createGlobal(), 0, Vec3( math::realRandom(real_c(0), real_c(10)), math::realRandom(real_c(0), real_c(10)), math::realRandom(real_c(0), real_c(10))), Vec3(0,0,0), Quat(), real_t(0.5), iron, false, false, false) );
+         bs.add( std::make_unique<Sphere>(UniqueID<Sphere>::createGlobal(), 0, Vec3( math::realRandom(real_c(0), real_c(10)), math::realRandom(real_c(0), real_c(10)), math::realRandom(real_c(0), real_c(10))), Quat(), real_t(0.5), iron, false, false, false) );
        pool["SCCD"].start();
        sccd.generatePossibleContacts();
        pool["SCCD"].end();
diff --git a/tests/pe/Synchronization.cpp b/tests/pe/Synchronization.cpp
index 3693e9ab8e2ddb5479b3e8fdef6cb0b3360e974f..67dd068321b7d0cd72b86732349cd368548befe7 100644
--- a/tests/pe/Synchronization.cpp
+++ b/tests/pe/Synchronization.cpp
@@ -116,7 +116,7 @@ int main( int argc, char ** argv )
 
    MaterialID iron = Material::find("iron");
    walberla::id_t sid = 123;
-   Sphere refSphere(1, 0, Vec3(15, 15, 15), Vec3(0,0,0), Quat(), 3, iron, false, true, false);
+   Sphere refSphere(1, 0, Vec3(15, 15, 15), Quat(), 3, iron, false, true, false);
    refSphere.setLinearVel(4, 5, 6);
    refSphere.setAngularVel( 1, 2, 3);
    Vec3 gpos = Vec3(15, 15, 15);
diff --git a/tests/pe/SynchronizationLargeBody.cpp b/tests/pe/SynchronizationLargeBody.cpp
index 28fdc39d15e18d392b677ea6306dacb3269e6a32..94e205db7e8fba495ee8610bd286fd83d646a9cf 100644
--- a/tests/pe/SynchronizationLargeBody.cpp
+++ b/tests/pe/SynchronizationLargeBody.cpp
@@ -160,7 +160,7 @@ int main( int argc, char ** argv )
    walberla::id_t sid = 123;
    Vec3 gpos = Vec3(3.5, 3.5, 3.5);
    const real_t r = real_c(1.6);
-   Sphere refSphere(1, 0, gpos, Vec3(0,0,0), Quat(), r, iron, false, true, false);
+   Sphere refSphere(1, 0, gpos, Quat(), r, iron, false, true, false);
    refSphere.setLinearVel(4, 5, 6);
    refSphere.setAngularVel( 1, 2, 3);
 
diff --git a/tests/pe/Union.cpp b/tests/pe/Union.cpp
index 49fd6e66b6fb0bb3a3bec34cdc8e657a65fb0a28..de959af081196a78e64d369beb32aab9bb1e61dd 100644
--- a/tests/pe/Union.cpp
+++ b/tests/pe/Union.cpp
@@ -13,7 +13,7 @@
 //  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 ParallelEquivalence.cpp
+//! \file Union.cpp
 //! \author Sebastian Eibl <sebastian.eibl@fau.de>
 //
 //======================================================================================================================
@@ -49,6 +49,7 @@ typedef std::tuple<Sphere, Plane, UnionType> BodyTuple ;
 
 void SnowManFallingOnPlane()
 {
+   WALBERLA_LOG_INFO("- Performing snowman on plane test");
    shared_ptr<BodyStorage> globalBodyStorage = make_shared<BodyStorage>();
 
    // create blocks
@@ -99,25 +100,178 @@ void SnowManFallingOnPlane()
    //WALBERLA_LOG_DEVEL(un);
 }
 
-void ImpulsCarryover()
+void UnionConstruction()
 {
+
    MaterialID iron = Material::find("iron");
 
-   auto un  = std::make_unique<UnionType>(12, 0, Vec3(0,0,0), Vec3(0,0,0), Quat(), false, true, false);
-   auto sp1 = std::make_unique<Sphere>( 10, 0, Vec3( 1,0,0), Vec3(0,0,0), Quat(), real_t(1), iron, false, true, false );
-   auto sp2 = std::make_unique<Sphere>( 11, 0, Vec3(-1,0,0), Vec3(0,0,0), Quat(), real_t(1), iron, false, true, false );
+   // Construct a union of two spheres in a non-rotated state and check all its parameters
+   auto un  = std::make_unique<UnionType>(12, 0, Vec3(0,0,0), Quat(), false, true, false);
+   auto sp1 = std::make_unique<Sphere>( 10, 0, Vec3( 1,0,0), Quat(real_t(0.5), real_t(0.5),real_t(0.5),real_t(0.5)), real_t(1), iron, false, true, false );
+   auto sp2 = std::make_unique<Sphere>( 11, 0, Vec3(-1,0,0), Quat(), real_t(1), iron, false, true, false );
+
+   WALBERLA_LOG_INFO("- Adding first body.");
+   // Add first body
+   un->add( std::move(sp1) );
+   const RigidBody& subb1 =  *un->begin();
+
+   // Check relative and global position and rotation
+   WALBERLA_CHECK_FLOAT_EQUAL(un->getPosition(),Vec3(1,0,0));
+   WALBERLA_CHECK_FLOAT_EQUAL(un->getRelPosition(), Vec3(0,0,0));
+
+   WALBERLA_CHECK_FLOAT_EQUAL(subb1.getRelPosition(), Vec3(0, 0,0));
+   WALBERLA_CHECK_FLOAT_EQUAL(subb1.getPosition(), Vec3(1,0,0));
+   // Global quaternion
+   WALBERLA_CHECK_FLOAT_EQUAL(subb1.getQuaternion(), Quat(real_t(0.5), real_t(0.5),real_t(0.5),real_t(0.5)));
+   WALBERLA_CHECK_FLOAT_EQUAL(subb1.getRelQuaternion(), Quat(real_t(0.5), real_t(0.5),real_t(0.5),real_t(0.5)));
+
+   // Check mass volume and inertia
+   WALBERLA_CHECK_FLOAT_EQUAL(un->getVolume(), (real_t(4./3.)*real_t(math::M_PI)));
+   WALBERLA_CHECK_FLOAT_EQUAL(un->getMass(), un->getVolume()*Material::getDensity(iron));
+   real_t scalar_inertia = real_t(0.4)*un->getMass(); // for sphere: I = 2/5*m*r*r
+   WALBERLA_CHECK_EQUAL(un->getInertia(), Mat3(scalar_inertia,0,0,0,scalar_inertia,0,0,0,scalar_inertia));
+
+   WALBERLA_LOG_INFO("- Adding second body.");
+   un->add( std::move(sp2) );
+   const RigidBody & subb2 =  *(un->begin()+1);
+   // Check relative and global position
+   WALBERLA_CHECK_FLOAT_EQUAL(un->getPosition(),Vec3(0,0,0));
+   WALBERLA_CHECK_FLOAT_EQUAL(un->getRelPosition(), Vec3(0,0,0));
+   WALBERLA_CHECK_FLOAT_EQUAL(subb1.getRelPosition(), Vec3(1,0,0));
+   WALBERLA_CHECK_FLOAT_EQUAL(subb1.getPosition(), Vec3(1,0,0));
+   WALBERLA_CHECK_FLOAT_EQUAL(subb2.getRelPosition(), Vec3(-1,0,0));
+   WALBERLA_CHECK_FLOAT_EQUAL(subb2.getPosition(), Vec3(-1,0,0));
+
+   // Check mass volume and inertia
+   WALBERLA_CHECK_FLOAT_EQUAL(un->getVolume(), (real_t(8./3.)*real_t(math::M_PI)));
+   WALBERLA_CHECK_FLOAT_EQUAL(un->getMass(), un->getVolume()*Material::getDensity(iron));
+   // Mass of one sphere
+   real_t masssphere = real_t(4./3.)*real_t(math::M_PI)*Material::getDensity(iron);
+   Mat3 bodyinertia(real_t(2.0)*scalar_inertia, 0, 0, 0, real_t(2.0)*(scalar_inertia + masssphere),0, 0, 0, real_t(2.0)*(scalar_inertia + masssphere));
+   WALBERLA_CHECK_FLOAT_EQUAL(un->getInertia(), bodyinertia);
+
+   WALBERLA_CHECK_FLOAT_EQUAL( un->getLinearVel(), Vec3(0,0,0));
+   WALBERLA_CHECK_FLOAT_EQUAL( un->getAngularVel(), Vec3(0,0,0));
+   WALBERLA_CHECK_FLOAT_EQUAL( un->getAngularVel(), Vec3(0,0,0));
+
+   WALBERLA_LOG_INFO("- Performing Rotation.");
+   //Check values for rotated union
+   Quat rotz30(Vec3(0,0,1), real_t(math::M_PI/6.0)); // rotate by 30 deg via z axis
+   real_t sin30 = real_t(0.5);
+   real_t cos30 = real_t(sqrt(3.0)/2.0);
+   un->setOrientation(rotz30);
+   WALBERLA_CHECK_FLOAT_EQUAL(un->getQuaternion(), rotz30);
+   WALBERLA_CHECK_FLOAT_EQUAL(subb1.getRelPosition(), Vec3(1,0,0));
+   WALBERLA_CHECK_FLOAT_EQUAL(subb1.getPosition(), Vec3(cos30,sin30,0));
+   WALBERLA_CHECK_FLOAT_EQUAL(subb2.getRelPosition(), Vec3(-1,0,0));
+   WALBERLA_CHECK_FLOAT_EQUAL(subb2.getPosition(), Vec3(-cos30,-sin30,0));
+   WALBERLA_CHECK_FLOAT_EQUAL(subb1.getRelQuaternion(), Quat(real_t(0.5), real_t(0.5),real_t(0.5),real_t(0.5)));
+   WALBERLA_CHECK_FLOAT_EQUAL(subb1.getQuaternion(), Quat(real_t(0.5), real_t(0.5),real_t(0.5),real_t(0.5))*rotz30 );
+   WALBERLA_CHECK_FLOAT_EQUAL(un->getInertia(), rotz30.toRotationMatrix()*bodyinertia*rotz30.toRotationMatrix().getTranspose());
+
+   WALBERLA_LOG_INFO("- Applying velocities.");
+   //Apply a linear velocity to the union
+   un->setLinearVel(Vec3(1,0,0));
+   WALBERLA_CHECK_FLOAT_EQUAL(subb1.getLinearVel(), Vec3(1,0,0));
+   WALBERLA_CHECK_FLOAT_EQUAL(subb2.getLinearVel(), Vec3(1,0,0));
+
+   un->setAngularVel(Vec3(0,0,1));
+   WALBERLA_CHECK_FLOAT_EQUAL(subb1.getAngularVel(), Vec3(0,0,1));
+   WALBERLA_CHECK_FLOAT_EQUAL(subb2.getAngularVel(), Vec3(0,0,1));
+   WALBERLA_CHECK_FLOAT_EQUAL(subb1.getLinearVel(), Vec3(real_t(1.0-sin30),cos30,0));
+   WALBERLA_CHECK_FLOAT_EQUAL(subb2.getLinearVel(), Vec3(real_t(1.0+sin30),-cos30,0));
+
+   WALBERLA_LOG_INFO("- Constructing rotated union.");
+   //Part 2: Construct exactly the same union, but now in a rotated state.
+   auto un2  = std::make_unique<UnionType>(12, 0, Vec3(0,0,0), rotz30, false, true, false);
+   auto sp12 = std::make_unique<Sphere>( 10, 0, Vec3( 1,0,0), Quat(real_t(0.5), real_t(0.5),real_t(0.5),real_t(0.5)), real_t(1), iron, false, true, false );
+   auto sp22 = std::make_unique<Sphere>( 11, 0, Vec3(-1,0,0), Quat(real_t(0.3), real_t(0.9),real_t(0.1),real_t(0.3)), real_t(1), iron, false, true, false );
+
+   un2->add( std::move(sp12) );
+   un2->add( std::move(sp22) );
+   const RigidBody & subb12 = *un2->begin();
+   const RigidBody & subb22 =  *(un2->begin()+1);
+   // Check relative and global position and rotation
+   WALBERLA_CHECK_FLOAT_EQUAL(un2->getPosition(),Vec3(0,0,0));
+   WALBERLA_CHECK_FLOAT_EQUAL(un2->getRelPosition(), Vec3(0,0,0));
+
+   WALBERLA_CHECK_FLOAT_EQUAL(subb12.getRelPosition(), Vec3(cos30,-sin30, 0));
+   WALBERLA_CHECK_FLOAT_EQUAL(subb12.getPosition(), Vec3(1,0,0));
+
+   WALBERLA_CHECK_FLOAT_EQUAL(subb22.getRelPosition(), Vec3(-cos30,sin30, 0));
+   WALBERLA_CHECK_FLOAT_EQUAL(subb22.getPosition(), Vec3(-1,0,0));
 
-   sp1->setLinearVel(Vec3(0,real_c(+1),0));
-   sp2->setLinearVel(Vec3(0,real_c(-1),0));
+   // Global quaternion and local
+   WALBERLA_CHECK_FLOAT_EQUAL(subb12.getQuaternion(), Quat(real_t(0.5), real_t(0.5),real_t(0.5),real_t(0.5)));
+   WALBERLA_CHECK_FLOAT_EQUAL(subb12.getRelQuaternion(), subb12.getQuaternion()*rotz30.getInverse());
+   WALBERLA_CHECK_FLOAT_EQUAL(subb22.getQuaternion(), Quat(real_t(0.3), real_t(0.9),real_t(0.1),real_t(0.3)));
+   WALBERLA_CHECK_FLOAT_EQUAL(subb22.getRelQuaternion(), subb22.getQuaternion()*rotz30.getInverse());
 
+   // Inertia tensor
+   WALBERLA_CHECK_FLOAT_EQUAL(un2->getInertia(), bodyinertia);
+
+   //Rotate again by 30 deg and perform the same checks as before
+   un2->rotate(rotz30);
+   WALBERLA_CHECK_FLOAT_EQUAL(subb12.getPosition(), Vec3(cos30,sin30,0));
+   WALBERLA_CHECK_FLOAT_EQUAL(subb22.getPosition(), Vec3(-cos30,-sin30,0));
+   WALBERLA_CHECK_FLOAT_EQUAL(subb12.getQuaternion(), Quat(real_t(0.5), real_t(0.5),real_t(0.5),real_t(0.5))*rotz30 );
+   WALBERLA_CHECK_FLOAT_EQUAL(subb22.getQuaternion(), Quat(real_t(0.3), real_t(0.9),real_t(0.1),real_t(0.3))*rotz30 );
+   WALBERLA_CHECK_FLOAT_EQUAL(un->getInertia(), rotz30.toRotationMatrix()*bodyinertia*rotz30.toRotationMatrix().getTranspose());
+
+   WALBERLA_LOG_INFO("- Applying velocities.");
+   //Apply a linear velocity to the union
+   un2->setLinearVel(Vec3(1,0,0));
+   WALBERLA_CHECK_FLOAT_EQUAL(subb12.getLinearVel(), Vec3(1,0,0));
+   WALBERLA_CHECK_FLOAT_EQUAL(subb22.getLinearVel(), Vec3(1,0,0));
+
+   un2->setAngularVel(Vec3(0,0,1));
+   WALBERLA_CHECK_FLOAT_EQUAL(subb12.getAngularVel(), Vec3(0,0,1));
+   WALBERLA_CHECK_FLOAT_EQUAL(subb22.getAngularVel(), Vec3(0,0,1));
+   WALBERLA_CHECK_FLOAT_EQUAL(subb12.getLinearVel(), Vec3(real_t(1.0-sin30), cos30, 0));
+   WALBERLA_CHECK_FLOAT_EQUAL(subb22.getLinearVel(), Vec3(real_t(1.0+sin30), -cos30, 0));
+
+}
+
+void checkAABB(const AABB &a1, const AABB& a2){
+   WALBERLA_CHECK_FLOAT_EQUAL(a1.xMin(), a2.xMin());
+   WALBERLA_CHECK_FLOAT_EQUAL(a1.yMin(), a2.yMin());
+   WALBERLA_CHECK_FLOAT_EQUAL(a1.zMin(), a2.zMin());
+   WALBERLA_CHECK_FLOAT_EQUAL(a1.xMax(), a2.xMax());
+   WALBERLA_CHECK_FLOAT_EQUAL(a1.yMax(), a2.yMax());
+   WALBERLA_CHECK_FLOAT_EQUAL(a1.zMax(), a2.zMax());
+}
+
+void UnionAABB() {
+   WALBERLA_LOG_INFO("- Performing AABB test");
+   MaterialID iron = Material::find("iron");
+   // Construct a union of two spheres in a non-rotated state and check all its parameters
+   auto un  = std::make_unique<UnionType>(12, 0, Vec3(0,0,0), Quat(), false, true, false);
+   auto sp1 = std::make_unique<Sphere>( 10, 0, Vec3( 1,0,0), Quat(real_t(0.5), real_t(0.5),real_t(0.5),real_t(0.5)), real_t(1), iron, false, true, false );
+   auto sp2 = std::make_unique<Sphere>( 11, 0, Vec3(-1,0,0), Quat(), real_t(1), iron, false, true, false );
    un->add( std::move(sp1) );
    un->add( std::move(sp2) );
+   AABB aabb = un->getAABB();
+   WALBERLA_CHECK_FLOAT_EQUAL(aabb.minCorner(), Vec3(-2,-1,-1));
+   WALBERLA_CHECK_FLOAT_EQUAL(aabb.maxCorner(), Vec3(2, 1, 1));
+
+   Vec3 shift(10,10,10);
+   un->setPosition(shift);
+   aabb = un->getAABB();
+   checkAABB(aabb, AABB(real_t(8),real_t(9),real_t(9),real_t(12),real_t(11),real_t(11)));
+
+   Quat rotz30(Vec3(0,0,1), real_t(math::M_PI/6.0)); // rotate by 30 deg via z axis
+   real_t sin30 = real_t(0.5);
+   real_t cos30 = real_t(sqrt(3.0)/2.0);
+   un->setOrientation(rotz30);
+   aabb = un->getAABB();
+   checkAABB(aabb, AABB(real_t(9.0-cos30),real_t(9.0-sin30),real_t(9), real_t(11.0+cos30), real_t(11.0+sin30), real_t(11)));
+
+   aabb = un->begin()->getAABB();
+   checkAABB(aabb, AABB(real_t(9.0+cos30),real_t(9.0+sin30),real_t(9), real_t(11.0+cos30), real_t(11.0+sin30), real_t(11)));
+
+   aabb = (un->begin()+1)->getAABB();
+   checkAABB(aabb, AABB(real_t(9.0-cos30),real_t(9.0-sin30),real_t(9), real_t(11.0-cos30), real_t(11.0-sin30), real_t(11)));
 
-   WALBERLA_CHECK_FLOAT_EQUAL( un->getPosition(),  Vec3(0,0,0) );
-   WALBERLA_CHECK_FLOAT_EQUAL( un->getLinearVel(), Vec3(0,0,0) );
-   WALBERLA_CHECK_FLOAT_EQUAL( un->getAngularVel() * Vec3(1,0,0), real_t(0) );
-   WALBERLA_CHECK_FLOAT_EQUAL( un->getAngularVel() * Vec3(0,1,0), real_t(0) );
-   WALBERLA_CHECK_GREATER( un->getAngularVel() * Vec3(0,0,1), real_t(0) );
 }
 
 int main( int argc, char ** argv )
@@ -126,8 +280,11 @@ int main( int argc, char ** argv )
 
    walberla::MPIManager::instance()->initializeMPI( &argc, &argv );
 
+   UnionConstruction();
+   UnionAABB();
    SnowManFallingOnPlane();
-   ImpulsCarryover();
+
+
 
    return EXIT_SUCCESS;
 }
@@ -136,4 +293,4 @@ int main( int argc, char ** argv )
 int main( int argc, char* argv[] )
 {
   return walberla::main( argc, argv );
-}
\ No newline at end of file
+}
diff --git a/tests/pe/UnionBehavior.cpp b/tests/pe/UnionBehavior.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..51037cf4842022719aeca8d7596263863b2ced08
--- /dev/null
+++ b/tests/pe/UnionBehavior.cpp
@@ -0,0 +1,259 @@
+//======================================================================================================================
+//
+//  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   UnionBehavior.cpp
+//! \author Tobias Leemann <tobias.leemann@fau.de>
+//! \brief Testcase checking whether a single body and a union representing the same geometry behave equally.
+//======================================================================================================================
+
+//! [Includes]
+#include <pe/basic.h>
+
+#include <core/Environment.h>
+#include <core/grid_generator/HCPIterator.h>
+#include <core/grid_generator/SCIterator.h>
+#include <core/logging/Logging.h>
+#include <core/math/Random.h>
+
+#include "blockforest/StructuredBlockForest.h"
+#include "blockforest/Initialization.h"
+#include "pe/fcd/GenericFCD.h"
+
+#include "pe/rigidbody/BoxFactory.h"
+#include "vtk/VTKOutput.h"
+#include "pe/vtk/BodyVtkOutput.h"
+
+
+#include <iostream>
+#include <sstream>
+#include <fstream>
+#include <cstring>
+#include <tuple>
+
+
+//! [Includes]
+
+namespace walberla {
+using namespace walberla::pe;
+
+
+//! [BodyTypeTuple]
+using BoxUnion = Union<Box>;
+using BodyTypeTuple = std::tuple<Plane, Box, BoxUnion>;
+//! [BodyTypeTuple]
+
+// Override the collide function, so that the collisions between the two bodies themselves are not tracked,
+// only collisions with the surrounding planes are handled
+class PlaneOnlyFCD : public fcd::GenericFCD<BodyTypeTuple, fcd::AnalyticCollideFunctor>{
+public:
+   Contacts& generateContacts(PossibleContacts& possibleContacts) override
+   {
+      contacts_.clear();
+      fcd::AnalyticCollideFunctor<decltype(contacts_)> func(contacts_);
+      for (auto &c : possibleContacts)
+      {
+         if(c.first->getTypeID() == Plane::getStaticTypeID() || c.second->getTypeID() == Plane::getStaticTypeID()) {
+            DoubleCast<BodyTypeTuple, BodyTypeTuple, fcd::AnalyticCollideFunctor<decltype(contacts_)>, bool>::execute(
+                     c.first, c.second, func);
+         }
+      }
+      return contacts_;
+   }
+};
+
+
+int main( int argc, char ** argv )
+{
+
+   Environment env(argc, argv);
+   WALBERLA_UNUSED(env);
+
+   // Parameters
+   auto dt = real_t(0.005);
+   int simulationSteps = 4000;
+   auto domainsize = real_t(100);
+   // Size of the box
+   auto boxlenght = real_t(6.0);
+   auto boxwidth = real_t(2.0);
+
+   Vec3 pos(real_t(domainsize/2.0),real_t(domainsize/2.0),real_t(0.8*domainsize));
+   Vec3 v0(10,2,3); // Linear velocity of the objects
+   Vec3 w0(4,15,1); // Some angular velocity of the objects (quite high)
+   int visSpacing = 10;
+
+
+   // Simulation part
+   math::seedRandomGenerator( static_cast<unsigned int>(1337 * mpi::MPIManager::instance()->worldRank()) );
+
+
+
+   //! [Parameters]
+
+   WALBERLA_LOG_INFO_ON_ROOT("*** GLOBALBODYSTORAGE ***");
+   //! [GlobalBodyStorage]
+   shared_ptr<BodyStorage> globalBodyStorage = make_shared<BodyStorage>();
+   //! [GlobalBodyStorage]
+
+   WALBERLA_LOG_INFO_ON_ROOT("*** BLOCKFOREST ***");
+   // create forest
+   //! [BlockForest]
+   auto forest = blockforest::createBlockForest( AABB(0,0,0,domainsize,domainsize,domainsize), // simulation domain
+                                                 Vector3<uint_t>(3,1,1), // blocks in each direction
+                                                 Vector3<bool>(true, false, false) // periodicity
+                                                 );
+
+   //! [BlockForest]
+   if (!forest)
+   {
+      WALBERLA_LOG_INFO_ON_ROOT( "No BlockForest created ... exiting!");
+      return EXIT_SUCCESS;
+   }
+
+   WALBERLA_LOG_INFO_ON_ROOT("*** STORAGEDATAHANDLING ***");
+   // add block data
+   //! [StorageDataHandling]
+   auto storageID           = forest->addBlockData(createStorageDataHandling<BodyTypeTuple>(), "Storage");
+   //! [StorageDataHandling]
+   //! [AdditionalBlockData]
+   auto ccdID               = forest->addBlockData(ccd::createHashGridsDataHandling( globalBodyStorage, storageID ), "CCD");
+
+   auto sharedFCDBDH        = make_shared<blockforest::AlwaysCreateBlockDataHandling<PlaneOnlyFCD> >( );
+
+   auto fcdID               = forest->addBlockData(sharedFCDBDH, "FCD");
+   //! [AdditionalBlockData]
+
+   WALBERLA_LOG_INFO_ON_ROOT("*** INTEGRATOR ***");
+   //! [Integrator]
+   cr::DEM cr(globalBodyStorage, forest, storageID, ccdID, fcdID);
+   cr.setGlobalLinearAcceleration( Vec3(0,0,-6) );
+   //! [Integrator]
+
+   WALBERLA_LOG_INFO_ON_ROOT("*** BodyTypeTuple ***");
+   // initialize body type ids
+   //! [SetBodyTypeIDs]
+   SetBodyTypeIDs<BodyTypeTuple>::execute();
+   //! [SetBodyTypeIDs]
+
+
+   // UNCOMMENT THIS BLOCK FOR VTK OUTPUT
+   /*WALBERLA_LOG_INFO_ON_ROOT("*** VTK OUTPUT ***");
+   //! [VTK Domain Output]
+   auto vtkDomainOutput = vtk::createVTKOutput_DomainDecomposition( forest, "domain_decomposition", 1, "VTK", "simulation_step" );
+   vtkDomainOutput->write(true);
+   auto vtkSphereHelper = make_shared<DefaultBodyVTKOutput>(storageID, *forest) ;
+   auto vtkSphereOutput = vtk::createVTKOutput_PointData(vtkSphereHelper, "Bodies", 1, "VTK", "simulation_step", false, false);*/
+   //! [VTK Domain Output]
+
+   WALBERLA_LOG_INFO_ON_ROOT("*** SETUP - START ***");
+   //! [Material]
+   const real_t   static_cof  ( real_c(1.2) / 2 );   // Coefficient of static friction. Note: pe doubles the input coefficient of friction for material-material contacts.
+   const real_t   dynamic_cof ( static_cof ); // Coefficient of dynamic friction. Similar to static friction for low speed friction.
+   MaterialID     material = createMaterial( "granular", real_t( 1.0 ), 0, static_cof, dynamic_cof, real_t( 0.5 ), 1, real_t(8.11e5), real_t(6.86e1), real_t(6.86e1) );
+   //! [Material]
+
+   // Create surronding planes in y,z directions, but not in x.
+   auto simulationDomain = forest->getDomain();
+   //! [Planes]
+   createPlane(*globalBodyStorage, 100, Vec3(0,1,0), simulationDomain.minCorner(), material );
+   createPlane(*globalBodyStorage, 101, Vec3(0,-1,0), simulationDomain.maxCorner(), material );
+   createPlane(*globalBodyStorage, 102, Vec3(0,0,1), simulationDomain.minCorner(), material );
+   createPlane(*globalBodyStorage, 103, Vec3(0,0,-1), simulationDomain.maxCorner(), material );
+   //! [Planes]
+
+   //! [Gas]
+   uint_t numParticles = uint_c(0);
+
+   // Create a union of a two half boxes with the center at pos
+   BoxUnion* particleU = createUnion<Box>( *globalBodyStorage, *forest, storageID, 0, Vec3(10,10,10));
+   // add 2 parts to the union
+   createBox(particleU, 4, pos-Vec3(real_t(boxlenght/4.0),0,0), Vec3(real_t(boxlenght/2.0), boxwidth, boxwidth), material);
+   createBox(particleU, 5, pos+Vec3(real_t(boxlenght/4.0),0,0), Vec3(real_t(boxlenght/2.0), boxwidth, boxwidth), material);
+
+   if (particleU != nullptr) particleU->setLinearVel(v0);
+   if (particleU != nullptr) particleU->setAngularVel(w0);
+   if (particleU != nullptr) ++numParticles;
+
+   // Generate the same box as one particle at pos
+   BoxID particleS = createBox( *globalBodyStorage, *forest, storageID, 1, pos, Vec3(boxlenght, boxwidth, boxwidth), material );
+   if (particleS != nullptr) particleS->setLinearVel(v0);
+   if (particleS != nullptr) particleS->setAngularVel(w0);
+   if (particleS != nullptr) ++numParticles;
+
+   WALBERLA_LOG_INFO_ON_ROOT("#particles created per process: " << numParticles);
+   syncNextNeighbors<BodyTypeTuple>(*forest, storageID);
+   //! [Gas]
+
+   WALBERLA_LOG_INFO_ON_ROOT("*** SETUP - END ***");
+
+   WALBERLA_LOG_INFO_ON_ROOT("*** SIMULATION - START ***");
+   //vtkDomainOutput->write(true);
+   //! [GameLoop]
+   for (int i=0; i < simulationSteps; ++i)
+   {
+      if( i % visSpacing == 0 )
+      {
+         WALBERLA_LOG_INFO_ON_ROOT( "Timestep " << i << " / " << simulationSteps );
+         // UNCOMMENT FOR OUTPUT
+         //vtkSphereOutput->write(true);
+      }
+
+      cr.timestep( real_c(dt) );
+
+      int bdcount = 0;
+      RigidBody* refU = nullptr;
+      RigidBody* refS = nullptr;
+      for (auto blockIt = forest->begin(); blockIt != forest->end(); ++blockIt)
+      {
+         for (auto bodyIt = LocalBodyIterator::begin(*blockIt, storageID); bodyIt != LocalBodyIterator::end(); ++bodyIt)
+         {
+            if(bodyIt->getID() == 0){
+               refU = &*bodyIt;
+            }
+            if(bodyIt->getID() == 1){
+               refS = &*bodyIt;
+            }
+            bdcount ++;
+         }
+      }
+
+      // Compare with an quite large epsilon, as round of errors in collisions will amplify themselves over time
+      auto eps = real_t(1e-4);
+      WALBERLA_CHECK_GREATER_EQUAL(bdcount, 2, "Lost a body.");
+      WALBERLA_CHECK_NOT_NULLPTR(refS, "Single body was not found.");
+      WALBERLA_CHECK_NOT_NULLPTR(refU, "Union was not found.");
+      // norm(pos1-pos2) < 1e-4 -> norm*norm < 1e-8
+      WALBERLA_CHECK_LESS((refU->getPosition()-refS->getPosition()).sqrLength(), eps*eps  );
+      WALBERLA_CHECK_LESS((refU->getLinearVel()-refS->getLinearVel()).sqrLength(), eps*eps );
+      WALBERLA_CHECK_LESS((refU->getAngularVel()-refS->getAngularVel()).sqrLength(), eps*eps);
+
+      // Sums of matrix differences...
+      WALBERLA_CHECK_LESS(std::abs(Vec3(1,1,1) * ((refU->getRotation()-refS->getRotation())*Vec3(1,1,1))), real_t(9.0*eps));
+      WALBERLA_CHECK_LESS(std::abs(Vec3(1,1,1) * ((refU->getInertia()-refS->getInertia())*Vec3(1,1,1))), real_t(9.0*eps));
+      //WALBERLA_LOG_INFO_ON_ROOT("Timestep " << i << ": " <<refU->getPosition() << "/" << refS->getPosition() << (Vec3(1,1,1) * ((refU->getInertia()-refS->getInertia())*Vec3(1,1,1)))<< " " << abs(Vec3(1,1,1) * ((refU->getRotation()-refS->getRotation())*Vec3(1,1,1))));
+
+
+      syncNextNeighbors<BodyTypeTuple>(*forest, storageID);
+   }
+   //! [GameLoop]
+   WALBERLA_LOG_INFO_ON_ROOT("*** SIMULATION - END ***");
+
+   return EXIT_SUCCESS;
+}
+} // namespace walberla
+
+int main( int argc, char* argv[] )
+{
+   return walberla::main( argc, argv );
+}
diff --git a/tests/pe/VolumeInertia.cpp b/tests/pe/VolumeInertia.cpp
index 6deebadad13baf8921848700e28170d483b9cada..2ab7738bb81520c32f8b37f615a88848746f28e2 100644
--- a/tests/pe/VolumeInertia.cpp
+++ b/tests/pe/VolumeInertia.cpp
@@ -97,7 +97,7 @@ int main( int argc, char ** argv )
    Vec3   COM;
    Mat3   inertia;
 
-   Sphere sp(0, 0, Vec3(0,0,0), Vec3(0,0,0), Quat(), real_t(2.34), material, false, true, false);
+   Sphere sp(0, 0, Vec3(0,0,0), Quat(), real_t(2.34), material, false, true, false);
    calcNumeric(sp, sp.getAABB(), real_t(0.01), volume, COM, inertia);
    WALBERLA_CHECK( walberla::debug::check_functions_detail::check_float_equal_eps(sp.getVolume(), volume, real_t(10e-4)) );
    WALBERLA_CHECK( walberla::debug::check_functions_detail::check_float_equal_eps(Sphere::calcVolume( real_t(2.34) ), volume, real_t(10e-4)) );
@@ -105,7 +105,7 @@ int main( int argc, char ** argv )
    WALBERLA_CHECK( walberla::debug::check_functions_detail::check_float_equal_eps(sp.getMass(), volume * Material::getDensity(material), real_t(10e-4)) );
    WALBERLA_CHECK( walberla::debug::check_functions_detail::check_float_equal_eps(COM, Vec3(0), real_t(10e-4)) );
 
-   Box bx(0, 0, Vec3(0,0,0), Vec3(0,0,0), Quat(), Vec3(real_t(1.5), real_t(2.5), real_t(3.5)), material, false, true, false);
+   Box bx(0, 0, Vec3(0,0,0), Quat(), Vec3(real_t(1.5), real_t(2.5), real_t(3.5)), material, false, true, false);
    calcNumeric(bx, bx.getAABB(), real_t(0.01), volume, COM, inertia);
    WALBERLA_CHECK( walberla::debug::check_functions_detail::check_float_equal_eps(bx.getVolume(), volume, real_t(10e-4)) );
    WALBERLA_CHECK( walberla::debug::check_functions_detail::check_float_equal_eps(Box::calcVolume( Vec3(real_t(1.5), real_t(2.5), real_t(3.5)) ), volume, real_t(10e-4)) );
@@ -113,7 +113,7 @@ int main( int argc, char ** argv )
    WALBERLA_CHECK( walberla::debug::check_functions_detail::check_float_equal_eps(bx.getMass(), volume * Material::getDensity(material), real_t(10e-4)) );
    WALBERLA_CHECK( walberla::debug::check_functions_detail::check_float_equal_eps(COM, Vec3(0), real_t(10e-4)) );
 
-   Ellipsoid el(0, 0, Vec3(0,0,0), Vec3(0,0,0), Quat(), Vec3(real_t(1.5), real_t(2.5), real_t(3.5)), material, false, true, false);
+   Ellipsoid el(0, 0, Vec3(0,0,0), Quat(), Vec3(real_t(1.5), real_t(2.5), real_t(3.5)), material, false, true, false);
    calcNumeric(el, el.getAABB(), real_t(0.01), volume, COM, inertia);
    WALBERLA_CHECK( walberla::debug::check_functions_detail::check_float_equal_eps(el.getVolume(), volume, real_t(10e-4)) );
    WALBERLA_CHECK( walberla::debug::check_functions_detail::check_float_equal_eps(Ellipsoid::calcVolume( Vec3(real_t(1.5), real_t(2.5), real_t(3.5)) ), volume, real_t(10e-4)) );
@@ -121,7 +121,7 @@ int main( int argc, char ** argv )
    WALBERLA_CHECK( walberla::debug::check_functions_detail::check_float_equal_eps(el.getMass(), volume * Material::getDensity(material), real_t(10e-4)) );
    WALBERLA_CHECK( walberla::debug::check_functions_detail::check_float_equal_eps(COM, Vec3(0), real_t(10e-4)) );
 
-   Capsule cp(0, 0, Vec3(0,0,0), Vec3(0,0,0), Quat(), real_t(1.5), real_t(2.5), material, false, true, false);
+   Capsule cp(0, 0, Vec3(0,0,0), Quat(), real_t(1.5), real_t(2.5), material, false, true, false);
    calcNumeric(cp, cp.getAABB(), real_t(0.01), volume, COM, inertia);
    WALBERLA_CHECK( walberla::debug::check_functions_detail::check_float_equal_eps(cp.getVolume(), volume, real_t(10e-4)) );
    WALBERLA_CHECK( walberla::debug::check_functions_detail::check_float_equal_eps(Capsule::calcVolume( real_t(1.5), real_t(2.5) ), volume, real_t(10e-4)) );
diff --git a/tests/pe_coupling/geometry/PeIntersectionRatioTest.cpp b/tests/pe_coupling/geometry/PeIntersectionRatioTest.cpp
index 496f7db519c509f573c130c9b10a5c3ccaa79ae0..76e9a79baf08a1210294c33a7d711b30d8d6c35e 100644
--- a/tests/pe_coupling/geometry/PeIntersectionRatioTest.cpp
+++ b/tests/pe_coupling/geometry/PeIntersectionRatioTest.cpp
@@ -60,8 +60,7 @@ int main( int argc, char **argv )
 
    walberla::id_t sid = 0;
    walberla::id_t uid = 0;
-
-   Vector3<real_t> rPos( real_t(0));
+   
    Vector3<real_t> rotationAngles( real_t(0));
    Quaternion<real_t> quat( rotationAngles );
    pe::MaterialID material = pe::Material::find("iron");
@@ -74,7 +73,7 @@ int main( int argc, char **argv )
       Vector3<real_t> bodyPos(real_t(1), real_t(0), real_t(0));
       real_t radius = real_t(1);
 
-      pe::Sphere sphere(++sid, ++uid, bodyPos, rPos, quat, radius, material, false, false, false);
+      pe::Sphere sphere(++sid, ++uid, bodyPos, quat, radius, material, false, false, false);
 
       pe::RigidBody & rb = sphere; // otherwise not the pe_coupling/geometry version is matched
 
@@ -123,7 +122,7 @@ int main( int argc, char **argv )
       Vector3<real_t> bodyPos(real_t(1), real_t(0), real_t(0));
       Vector3<real_t> semiAxes1(real_t(1), real_t(1), real_t(1));
 
-      pe::Ellipsoid ellip1(++sid, ++uid, bodyPos, rPos, quat, semiAxes1, material, false, false, false);
+      pe::Ellipsoid ellip1(++sid, ++uid, bodyPos, quat, semiAxes1, material, false, false, false);
 
       pe::RigidBody & rb1 = ellip1; // otherwise not the pe_coupling/geometry version is matched
 
@@ -138,7 +137,7 @@ int main( int argc, char **argv )
       WALBERLA_CHECK_FLOAT_EQUAL(delta2, (std::sqrt(2) - real_t(1)) / std::sqrt(2), "Intersection ratio with ellipsoid wrong!");
 
       Vector3<real_t> semiAxes2(real_t(2), real_t(0.5), real_t(2));
-      pe::Ellipsoid ellip2(++sid, ++uid, bodyPos, rPos, quat, semiAxes2, material, false, false, false);
+      pe::Ellipsoid ellip2(++sid, ++uid, bodyPos, quat, semiAxes2, material, false, false, false);
 
       pe::RigidBody & rb2 = ellip2; // otherwise not the pe_coupling/geometry version is matched