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..dd15b7a741c7d2a4f288d603ee3e464ef54f8b40 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())
@@ -93,6 +118,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/__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/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/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/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..38c316743621c81e628312c8d3ba76d67935f3c3 100644
--- a/src/mesa_pd/data/ParticleAccessor.h
+++ b/src/mesa_pd/data/ParticleAccessor.h
@@ -132,6 +132,14 @@ 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);}
+   
 
    id_t getInvalidUid() const {return UniqueID<data::Particle>::invalidID();}
    size_t getInvalidIdx() const {return std::numeric_limits<size_t>::max();}
@@ -261,6 +269,14 @@ 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_;}
+   
 
    id_t getInvalidUid() const {return UniqueID<data::Particle>::invalidID();}
    size_t getInvalidIdx() const {return std::numeric_limits<size_t>::max();}
@@ -293,6 +309,8 @@ 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_;
 };
 
 } //namespace data
diff --git a/src/mesa_pd/data/ParticleStorage.h b/src/mesa_pd/data/ParticleStorage.h
index 633563ca6f2d26ee7f51c569568b0151bb3f3ce9..942b475ce7f569a8da16a71a5b4489f406159325 100644
--- a/src/mesa_pd/data/ParticleStorage.h
+++ b/src/mesa_pd/data/ParticleStorage.h
@@ -152,6 +152,14 @@ 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);}
+      
 
       size_t getIdx() const {return i_;}
    public:
@@ -296,6 +304,14 @@ 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; }
+   
 
    /**
     * @brief creates a new particle and returns an iterator pointing to it
@@ -406,6 +422,8 @@ 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::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 +454,8 @@ ParticleStorage::Particle& ParticleStorage::Particle::operator=(const ParticleSt
    getNewContactHistoryRef() = rhs.getNewContactHistory();
    getTemperatureRef() = rhs.getTemperature();
    getHeatFluxRef() = rhs.getHeatFlux();
+   getDvRef() = rhs.getDv();
+   getDwRef() = rhs.getDw();
    return *this;
 }
 
@@ -463,6 +483,8 @@ 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());
    return *this;
 }
 
@@ -492,6 +514,8 @@ 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" <<
          "================================" << std::endl;
    return os;
 }
@@ -590,6 +614,8 @@ 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));
    uid_.back() = uid;
    uidToIdx_[uid] = uid_.size() - 1;
    return iterator(this, size() - 1);
@@ -643,6 +669,8 @@ inline ParticleStorage::iterator ParticleStorage::erase(iterator& it)
    newContactHistory_.pop_back();
    temperature_.pop_back();
    heatFlux_.pop_back();
+   dv_.pop_back();
+   dw_.pop_back();
    return it;
 }
 
@@ -683,6 +711,8 @@ inline void ParticleStorage::reserve(const size_t size)
    newContactHistory_.reserve(size);
    temperature_.reserve(size);
    heatFlux_.reserve(size);
+   dv_.reserve(size);
+   dw_.reserve(size);
 }
 
 inline void ParticleStorage::clear()
@@ -708,6 +738,8 @@ inline void ParticleStorage::clear()
    newContactHistory_.clear();
    temperature_.clear();
    heatFlux_.clear();
+   dv_.clear();
+   dw_.clear();
    uidToIdx_.clear();
 }
 
@@ -734,6 +766,8 @@ 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() );
    return uid_.size();
 }
 template <typename Selector, typename Accessor, typename Func, typename... Args>
@@ -1065,6 +1099,24 @@ 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();}
+};
 
 } //namespace data
 } //namespace mesa_pd
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/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/tests/mesa_pd/CMakeLists.txt b/tests/mesa_pd/CMakeLists.txt
index 57a73030ad4212e4539773bd1c15e0d4b8fc882b..5cfc0f96ff816f5daa33dcc057c2a4c1763d964c 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 )
 
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/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: