From a54792053bbe26dad3b26172868a51ca646ec450 Mon Sep 17 00:00:00 2001
From: Christoph Rettinger <christoph.rettinger@fau.de>
Date: Fri, 6 May 2022 10:35:45 +0200
Subject: [PATCH] Changed mesa_pd integrators back to original variant,
allowing generaiton of extended angular momentum part
---
python/mesa_pd/kernel/ExplicitEuler.py | 3 +-
python/mesa_pd/kernel/SemiImplicitEuler.py | 3 +-
python/mesa_pd/kernel/VelocityVerlet.py | 6 +-
.../templates/kernel/ExplicitEuler.templ.h | 6 ++
.../kernel/SemiImplicitEuler.templ.h | 7 ++
.../templates/kernel/VelocityVerlet.templ.h | 69 ++++++++------
src/mesa_pd/kernel/ExplicitEuler.h | 9 +-
src/mesa_pd/kernel/ExplicitEulerWithShape.h | 6 ++
src/mesa_pd/kernel/SemiImplicitEuler.h | 10 +-
src/mesa_pd/kernel/VelocityVerlet.h | 95 +++++++++----------
10 files changed, 120 insertions(+), 94 deletions(-)
diff --git a/python/mesa_pd/kernel/ExplicitEuler.py b/python/mesa_pd/kernel/ExplicitEuler.py
index 6d7b39fe1..a41fbd733 100644
--- a/python/mesa_pd/kernel/ExplicitEuler.py
+++ b/python/mesa_pd/kernel/ExplicitEuler.py
@@ -5,8 +5,9 @@ from mesa_pd.utility import generate_file
class ExplicitEuler:
- def __init__(self, integrate_rotation=True):
+ def __init__(self, integrate_rotation=True, use_full_angular_momentum_equation=False):
self.context = {'bIntegrateRotation': integrate_rotation,
+ 'bUseFullAngularMomentumEquation': use_full_angular_momentum_equation,
'interface': [
create_access("position", "walberla::mesa_pd::Vec3", access="gs"),
create_access("linearVelocity", "walberla::mesa_pd::Vec3", access="gs"),
diff --git a/python/mesa_pd/kernel/SemiImplicitEuler.py b/python/mesa_pd/kernel/SemiImplicitEuler.py
index 6732a78cf..ce14ca022 100644
--- a/python/mesa_pd/kernel/SemiImplicitEuler.py
+++ b/python/mesa_pd/kernel/SemiImplicitEuler.py
@@ -5,8 +5,9 @@ from mesa_pd.utility import generate_file
class SemiImplicitEuler:
- def __init__(self, integrate_rotation=True):
+ def __init__(self, integrate_rotation=True, use_full_angular_momentum_equation=False):
self.context = {'bIntegrateRotation': integrate_rotation,
+ 'bUseFullAngularMomentumEquation': use_full_angular_momentum_equation,
'interface': [
create_access("position", "walberla::mesa_pd::Vec3", access="gs"),
create_access("linearVelocity", "walberla::mesa_pd::Vec3", access="gs"),
diff --git a/python/mesa_pd/kernel/VelocityVerlet.py b/python/mesa_pd/kernel/VelocityVerlet.py
index 1bb4ff445..ebcaccd69 100644
--- a/python/mesa_pd/kernel/VelocityVerlet.py
+++ b/python/mesa_pd/kernel/VelocityVerlet.py
@@ -5,8 +5,10 @@ from mesa_pd.utility import generate_file
class VelocityVerlet:
- def __init__(self, integrate_rotation=True):
- self.context = {'bIntegrateRotation': integrate_rotation, 'interface': []}
+ def __init__(self, integrate_rotation=True, use_full_angular_momentum_equation=False):
+ self.context = {'bIntegrateRotation': integrate_rotation,
+ 'bUseFullAngularMomentumEquation': use_full_angular_momentum_equation,
+ 'interface': []}
self.context['interface'].append(create_access("position", "walberla::mesa_pd::Vec3", access="gs"))
self.context['interface'].append(create_access("linearVelocity", "walberla::mesa_pd::Vec3", access="gs"))
self.context['interface'].append(create_access("invMass", "walberla::real_t", access="g"))
diff --git a/python/mesa_pd/templates/kernel/ExplicitEuler.templ.h b/python/mesa_pd/templates/kernel/ExplicitEuler.templ.h
index 4cb293517..0519fe186 100644
--- a/python/mesa_pd/templates/kernel/ExplicitEuler.templ.h
+++ b/python/mesa_pd/templates/kernel/ExplicitEuler.templ.h
@@ -90,12 +90,18 @@ inline void ExplicitEuler::operator()(const size_t idx,
ac.getLinearVelocity(idx));
{%- if bIntegrateRotation %}
+ {%- if bUseFullAngularMomentumEquation %}
// computation done in body frame: d(omega)/ dt = J^-1 ((J*omega) x omega + T), update in world frame
// see Wachs, 2019, doi:10.1007/s00707-019-02389-9, Eq. 27
const auto omegaBF = transformVectorFromWFtoBF(idx, ac, ac.getAngularVelocity(idx));
const auto torqueBF = transformVectorFromWFtoBF(idx, ac, ac.getTorque(idx));
const Vec3 wdotBF = ac.getInvInertiaBF(idx) * ( ( ac.getInertiaBF(idx) * omegaBF ) % omegaBF + torqueBF );
const Vec3 wdot = transformVectorFromBFtoWF(idx, ac, wdotBF);
+ {%- else %}
+ // note: contribution (J*omega) x omega is ignored here -> see template for other variant
+ const Vec3 wdot = math::transformMatrixRART(ac.getRotation(idx).getMatrix(),
+ ac.getInvInertiaBF(idx)) * ac.getTorque(idx);
+ {%- endif %}
// Calculating the rotation angle
const Vec3 phi( 0.5_r * wdot * dt_ * dt_ +
diff --git a/python/mesa_pd/templates/kernel/SemiImplicitEuler.templ.h b/python/mesa_pd/templates/kernel/SemiImplicitEuler.templ.h
index 528139501..c7f586ea4 100644
--- a/python/mesa_pd/templates/kernel/SemiImplicitEuler.templ.h
+++ b/python/mesa_pd/templates/kernel/SemiImplicitEuler.templ.h
@@ -84,12 +84,19 @@ inline void SemiImplicitEuler::operator()(const size_t idx,
ac.getPosition(idx));
{%- if bIntegrateRotation %}
+ {%- if bUseFullAngularMomentumEquation %}
// computation done in body frame: d(omega)/ dt = J^-1 ((J*omega) x omega + T), update in world frame
// see Wachs, 2019, doi:10.1007/s00707-019-02389-9, Eq. 27
const auto omegaBF = transformVectorFromWFtoBF(idx, ac, ac.getAngularVelocity(idx));
const auto torqueBF = transformVectorFromWFtoBF(idx, ac, ac.getTorque(idx));
const Vec3 wdotBF = ac.getInvInertiaBF(idx) * ( ( ac.getInertiaBF(idx) * omegaBF ) % omegaBF + torqueBF );
const Vec3 wdot = transformVectorFromBFtoWF(idx, ac, wdotBF);
+ {%- else %}
+ // note: contribution (J*omega) x omega is ignored here -> see template for other variant
+ const Vec3 wdot = math::transformMatrixRART(ac.getRotation(idx).getMatrix(),
+ ac.getInvInertiaBF(idx)) * ac.getTorque(idx);
+ {%- endif %}
+
ac.setAngularVelocity(idx, wdot * dt_ +
ac.getAngularVelocity(idx));
diff --git a/python/mesa_pd/templates/kernel/VelocityVerlet.templ.h b/python/mesa_pd/templates/kernel/VelocityVerlet.templ.h
index 9d37ed22e..05fffd945 100644
--- a/python/mesa_pd/templates/kernel/VelocityVerlet.templ.h
+++ b/python/mesa_pd/templates/kernel/VelocityVerlet.templ.h
@@ -50,13 +50,13 @@ namespace kernel {
* \code
{%- for prop in interface %}
{%- if 'g' in prop.access %}
- * const {{prop.type}}& get{{prop.name | capFirst}}(const size_t p_idx) const;
+ * const {{prop.type}}& get{{prop.name | capFirst}}(const size_t idx) const;
{%- endif %}
{%- if 's' in prop.access %}
- * void set{{prop.name | capFirst}}(const size_t p_idx, const {{prop.type}}& v);
+ * void set{{prop.name | capFirst}}(const size_t idx, const {{prop.type}}& v);
{%- endif %}
{%- if 'r' in prop.access %}
- * {{prop.type}}& get{{prop.name | capFirst}}Ref(const size_t p_idx);
+ * {{prop.type}}& get{{prop.name | capFirst}}Ref(const size_t idx);
{%- endif %}
*
{%- endfor %}
@@ -87,64 +87,77 @@ public:
};
template <typename Accessor>
-inline void VelocityVerletPreForceUpdate::operator()(const size_t p_idx, Accessor& ac) const
+inline void VelocityVerletPreForceUpdate::operator()(const size_t idx, Accessor& ac) const
{
static_assert(std::is_base_of<data::IAccessor, Accessor>::value, "please provide a valid accessor");
- if (!data::particle_flags::isSet( ac.getFlags(p_idx), data::particle_flags::FIXED))
+ if (!data::particle_flags::isSet( ac.getFlags(idx), data::particle_flags::FIXED))
{
- ac.setPosition(p_idx, ac.getPosition(p_idx) +
- ac.getLinearVelocity(p_idx) * dt_ +
- real_t(0.5) * ac.getInvMass(p_idx) * ac.getOldForce(p_idx) * dt_ * dt_);
+ ac.setPosition(idx, ac.getPosition(idx) +
+ ac.getLinearVelocity(idx) * dt_ +
+ real_t(0.5) * ac.getInvMass(idx) * ac.getOldForce(idx) * dt_ * dt_);
{%- if bIntegrateRotation %}
+ {%- if bUseFullAngularMomentumEquation %}
// computation done in body frame: d(omega)/ dt = J^-1 ((J*omega) x omega + T), update in world frame
// see Wachs, 2019, doi:10.1007/s00707-019-02389-9, Eq. 27
// note: this implementation (pre and post) is experimental as it is in principle unclear in which order
// angular velocities and rotations (affect again the transformations WF - BF) have to be carried out
- const auto omegaBF = transformVectorFromWFtoBF(p_idx, ac, ac.getAngularVelocity(p_idx));
- const auto torqueBF = transformVectorFromWFtoBF(p_idx, ac, ac.getOldTorque(p_idx));
- const Vec3 wdotBF = ac.getInvInertiaBF(p_idx) * ( ( ac.getInertiaBF(p_idx) * omegaBF ) % omegaBF + torqueBF );
- const Vec3 wdot = transformVectorFromBFtoWF(p_idx, ac, wdotBF);
+ const auto omegaBF = transformVectorFromWFtoBF(idx, ac, ac.getAngularVelocity(idx));
+ const auto torqueBF = transformVectorFromWFtoBF(idx, ac, ac.getOldTorque(idx));
+ const Vec3 wdotBF = ac.getInvInertiaBF(idx) * ( ( ac.getInertiaBF(idx) * omegaBF ) % omegaBF + torqueBF );
+ const Vec3 wdot = transformVectorFromBFtoWF(idx, ac, wdotBF);
+ {%- else %}
+ // note: contribution (J*omega) x omega is ignored here -> see template for other variant
+ const Vec3 wdot = math::transformMatrixRART(ac.getRotation(idx).getMatrix(),
+ ac.getInvInertiaBF(idx)) * ac.getOldTorque(idx);
+ {%- endif %}
+
// Calculating the rotation angle
- const Vec3 phi( ac.getAngularVelocity(p_idx) * dt_ + real_t(0.5) * wdot * dt_ * dt_);
+ const Vec3 phi( ac.getAngularVelocity(idx) * dt_ + real_t(0.5) * wdot * dt_ * dt_);
// Calculating the new orientation
- auto rotation = ac.getRotation(p_idx);
+ auto rotation = ac.getRotation(idx);
rotation.rotate( phi );
- ac.setRotation(p_idx, rotation);
+ ac.setRotation(idx, rotation);
{%- endif %}
}
}
template <typename Accessor>
-inline void VelocityVerletPostForceUpdate::operator()(const size_t p_idx, Accessor& ac) const
+inline void VelocityVerletPostForceUpdate::operator()(const size_t idx, Accessor& ac) const
{
static_assert(std::is_base_of<data::IAccessor, Accessor>::value, "please provide a valid accessor");
- if (!data::particle_flags::isSet( ac.getFlags(p_idx), data::particle_flags::FIXED))
+ if (!data::particle_flags::isSet( ac.getFlags(idx), data::particle_flags::FIXED))
{
- ac.setLinearVelocity(p_idx, ac.getLinearVelocity(p_idx) +
- real_t(0.5) * ac.getInvMass(p_idx) * (ac.getOldForce(p_idx) + ac.getForce(p_idx)) * dt_);
+ ac.setLinearVelocity(idx, ac.getLinearVelocity(idx) +
+ real_t(0.5) * ac.getInvMass(idx) * (ac.getOldForce(idx) + ac.getForce(idx)) * dt_);
{%- if bIntegrateRotation %}
- const auto omegaBF = transformVectorFromWFtoBF(p_idx, ac, ac.getAngularVelocity(p_idx));
- const auto torqueBF = transformVectorFromWFtoBF(p_idx, ac, 0.5_r * (ac.getOldTorque(p_idx) + ac.getTorque(p_idx)));
- const Vec3 wdotBF = ac.getInvInertiaBF(p_idx) * ( ( ac.getInertiaBF(p_idx) * omegaBF ) % omegaBF + torqueBF );
- const Vec3 wdot = transformVectorFromBFtoWF(p_idx, ac, wdotBF);
+ {%- if bUseFullAngularMomentumEquation %}
+ const auto omegaBF = transformVectorFromWFtoBF(idx, ac, ac.getAngularVelocity(idx));
+ const auto torqueBF = transformVectorFromWFtoBF(idx, ac, 0.5_r * (ac.getOldTorque(idx) + ac.getTorque(idx)));
+ const Vec3 wdotBF = ac.getInvInertiaBF(idx) * ( ( ac.getInertiaBF(idx) * omegaBF ) % omegaBF + torqueBF );
+ const Vec3 wdot = transformVectorFromBFtoWF(idx, ac, wdotBF);
+ {%- else %}
+ const auto torque = 0.5_r * (ac.getOldTorque(idx) + ac.getTorque(idx));
+ const Vec3 wdot = math::transformMatrixRART(ac.getRotation(idx).getMatrix(),
+ ac.getInvInertiaBF(idx)) * torque;
+ {%- endif %}
- ac.setAngularVelocity(p_idx, ac.getAngularVelocity(p_idx) +
+ ac.setAngularVelocity(idx, ac.getAngularVelocity(idx) +
wdot * dt_ );
{%- endif %}
}
- ac.setOldForce(p_idx, ac.getForce(p_idx));
- ac.setForce(p_idx, Vec3(real_t(0), real_t(0), real_t(0)));
+ ac.setOldForce(idx, ac.getForce(idx));
+ ac.setForce(idx, Vec3(real_t(0), real_t(0), real_t(0)));
{%- if bIntegrateRotation %}
- ac.setOldTorque(p_idx, ac.getTorque(p_idx));
- ac.setTorque(p_idx, Vec3(real_t(0), real_t(0), real_t(0)));
+ ac.setOldTorque(idx, ac.getTorque(idx));
+ ac.setTorque(idx, Vec3(real_t(0), real_t(0), real_t(0)));
{%- endif %}
}
diff --git a/src/mesa_pd/kernel/ExplicitEuler.h b/src/mesa_pd/kernel/ExplicitEuler.h
index 44afb7b9c..923cf28bc 100644
--- a/src/mesa_pd/kernel/ExplicitEuler.h
+++ b/src/mesa_pd/kernel/ExplicitEuler.h
@@ -100,12 +100,9 @@ inline void ExplicitEuler::operator()(const size_t idx,
ac.getPosition(idx));
ac.setLinearVelocity(idx, ac.getInvMass(idx) * ac.getForce(idx) * dt_ +
ac.getLinearVelocity(idx));
- // computation done in body frame: d(omega)/ dt = J^-1 ((J*omega) x omega + T), update in world frame
- // see Wachs, 2019, doi:10.1007/s00707-019-02389-9, Eq. 27
- const auto omegaBF = transformVectorFromWFtoBF(idx, ac, ac.getAngularVelocity(idx));
- const auto torqueBF = transformVectorFromWFtoBF(idx, ac, ac.getTorque(idx));
- const Vec3 wdotBF = ac.getInvInertiaBF(idx) * ( ( ac.getInertiaBF(idx) * omegaBF ) % omegaBF + torqueBF );
- const Vec3 wdot = transformVectorFromBFtoWF(idx, ac, wdotBF);
+ // note: contribution (J*omega) x omega is ignored here -> see template for other variant
+ const Vec3 wdot = math::transformMatrixRART(ac.getRotation(idx).getMatrix(),
+ ac.getInvInertiaBF(idx)) * ac.getTorque(idx);
// Calculating the rotation angle
const Vec3 phi( 0.5_r * wdot * dt_ * dt_ +
diff --git a/src/mesa_pd/kernel/ExplicitEulerWithShape.h b/src/mesa_pd/kernel/ExplicitEulerWithShape.h
index 982511b8c..a61646ff0 100644
--- a/src/mesa_pd/kernel/ExplicitEulerWithShape.h
+++ b/src/mesa_pd/kernel/ExplicitEulerWithShape.h
@@ -91,12 +91,18 @@ inline void ExplicitEuler::operator()(const size_t idx,
ac.setPosition (idx, ac.getInvMass(idx) * ac.getForce(idx) * dt_ * dt_ + ac.getLinearVelocity(idx) * dt_ + ac.getPosition(idx));
ac.setLinearVelocity(idx, ac.getInvMass(idx) * ac.getForce(idx) * dt_ + ac.getLinearVelocity(idx));
+ /*
// computation done in body frame: d(omega)/ dt = J^-1 ((J*omega) x omega + T), update in world frame
// see Wachs, 2019, doi:10.1007/s00707-019-02389-9, Eq. 27
const auto omegaBF = transformVectorFromWFtoBF(idx, ac, ac.getAngularVelocity(idx));
const auto torqueBF = transformVectorFromWFtoBF(idx, ac, ac.getTorque(idx));
const Vec3 wdotBF = ac.getInvInertiaBF(idx) * ( ( ac.getInertiaBF(idx) * omegaBF ) % omegaBF + torqueBF );
const Vec3 wdot = transformVectorFromBFtoWF(idx, ac, wdotBF);
+ */
+
+ // note: contribution (J*omega) x omega is ignored here -> see comment for other variant
+ const Vec3 wdot = math::transformMatrixRART(ac.getRotation(idx).getMatrix(),
+ ac.getInvInertiaBF(idx)) * ac.getTorque(idx);
// Calculating the rotation angle
const Vec3 phi( ac.getAngularVelocity(idx) * dt_ + wdot * dt_ * dt_);
diff --git a/src/mesa_pd/kernel/SemiImplicitEuler.h b/src/mesa_pd/kernel/SemiImplicitEuler.h
index bf8cec461..57ac8abd6 100644
--- a/src/mesa_pd/kernel/SemiImplicitEuler.h
+++ b/src/mesa_pd/kernel/SemiImplicitEuler.h
@@ -94,12 +94,10 @@ inline void SemiImplicitEuler::operator()(const size_t idx,
ac.getLinearVelocity(idx));
ac.setPosition ( idx, ac.getLinearVelocity(idx) * dt_ +
ac.getPosition(idx));
- // computation done in body frame: d(omega)/ dt = J^-1 ((J*omega) x omega + T), update in world frame
- // see Wachs, 2019, doi:10.1007/s00707-019-02389-9, Eq. 27
- const auto omegaBF = transformVectorFromWFtoBF(idx, ac, ac.getAngularVelocity(idx));
- const auto torqueBF = transformVectorFromWFtoBF(idx, ac, ac.getTorque(idx));
- const Vec3 wdotBF = ac.getInvInertiaBF(idx) * ( ( ac.getInertiaBF(idx) * omegaBF ) % omegaBF + torqueBF );
- const Vec3 wdot = transformVectorFromBFtoWF(idx, ac, wdotBF);
+ // note: contribution (J*omega) x omega is ignored here -> see template for other variant
+ const Vec3 wdot = math::transformMatrixRART(ac.getRotation(idx).getMatrix(),
+ ac.getInvInertiaBF(idx)) * ac.getTorque(idx);
+
ac.setAngularVelocity(idx, wdot * dt_ +
ac.getAngularVelocity(idx));
diff --git a/src/mesa_pd/kernel/VelocityVerlet.h b/src/mesa_pd/kernel/VelocityVerlet.h
index 93d64239d..0edcc2b54 100644
--- a/src/mesa_pd/kernel/VelocityVerlet.h
+++ b/src/mesa_pd/kernel/VelocityVerlet.h
@@ -46,37 +46,37 @@ namespace kernel {
*
* This kernel requires the following particle accessor interface
* \code
- * const walberla::mesa_pd::Vec3& getPosition(const size_t p_idx) const;
- * void setPosition(const size_t p_idx, const walberla::mesa_pd::Vec3& v);
+ * const walberla::mesa_pd::Vec3& getPosition(const size_t idx) const;
+ * void setPosition(const size_t idx, const walberla::mesa_pd::Vec3& v);
*
- * const walberla::mesa_pd::Vec3& getLinearVelocity(const size_t p_idx) const;
- * void setLinearVelocity(const size_t p_idx, const walberla::mesa_pd::Vec3& v);
+ * const walberla::mesa_pd::Vec3& getLinearVelocity(const size_t idx) const;
+ * void setLinearVelocity(const size_t idx, const walberla::mesa_pd::Vec3& v);
*
- * const walberla::real_t& getInvMass(const size_t p_idx) const;
+ * const walberla::real_t& getInvMass(const size_t idx) const;
*
- * const walberla::mesa_pd::Vec3& getForce(const size_t p_idx) const;
- * void setForce(const size_t p_idx, const walberla::mesa_pd::Vec3& v);
+ * const walberla::mesa_pd::Vec3& getForce(const size_t idx) const;
+ * void setForce(const size_t idx, const walberla::mesa_pd::Vec3& v);
*
- * const walberla::mesa_pd::Vec3& getOldForce(const size_t p_idx) const;
- * void setOldForce(const size_t p_idx, const walberla::mesa_pd::Vec3& v);
+ * const walberla::mesa_pd::Vec3& getOldForce(const size_t idx) const;
+ * void setOldForce(const size_t idx, const walberla::mesa_pd::Vec3& v);
*
- * const walberla::mesa_pd::Rot3& getRotation(const size_t p_idx) const;
- * void setRotation(const size_t p_idx, const walberla::mesa_pd::Rot3& v);
+ * const walberla::mesa_pd::Rot3& getRotation(const size_t idx) const;
+ * void setRotation(const size_t idx, const walberla::mesa_pd::Rot3& v);
*
- * const walberla::mesa_pd::Vec3& getAngularVelocity(const size_t p_idx) const;
- * void setAngularVelocity(const size_t p_idx, const walberla::mesa_pd::Vec3& v);
+ * const walberla::mesa_pd::Vec3& getAngularVelocity(const size_t idx) const;
+ * void setAngularVelocity(const size_t idx, const walberla::mesa_pd::Vec3& v);
*
- * const walberla::mesa_pd::Mat3& getInvInertiaBF(const size_t p_idx) const;
+ * const walberla::mesa_pd::Mat3& getInvInertiaBF(const size_t idx) const;
*
- * const walberla::mesa_pd::Mat3& getInertiaBF(const size_t p_idx) const;
+ * const walberla::mesa_pd::Mat3& getInertiaBF(const size_t idx) const;
*
- * const walberla::mesa_pd::Vec3& getTorque(const size_t p_idx) const;
- * void setTorque(const size_t p_idx, const walberla::mesa_pd::Vec3& v);
+ * const walberla::mesa_pd::Vec3& getTorque(const size_t idx) const;
+ * void setTorque(const size_t idx, const walberla::mesa_pd::Vec3& v);
*
- * const walberla::mesa_pd::Vec3& getOldTorque(const size_t p_idx) const;
- * void setOldTorque(const size_t p_idx, const walberla::mesa_pd::Vec3& v);
+ * const walberla::mesa_pd::Vec3& getOldTorque(const size_t idx) const;
+ * void setOldTorque(const size_t idx, const walberla::mesa_pd::Vec3& v);
*
- * const walberla::mesa_pd::data::particle_flags::FlagT& getFlags(const size_t p_idx) const;
+ * const walberla::mesa_pd::data::particle_flags::FlagT& getFlags(const size_t idx) const;
*
* \endcode
* \ingroup mesa_pd_kernel
@@ -105,56 +105,51 @@ public:
};
template <typename Accessor>
-inline void VelocityVerletPreForceUpdate::operator()(const size_t p_idx, Accessor& ac) const
+inline void VelocityVerletPreForceUpdate::operator()(const size_t idx, Accessor& ac) const
{
static_assert(std::is_base_of<data::IAccessor, Accessor>::value, "please provide a valid accessor");
- if (!data::particle_flags::isSet( ac.getFlags(p_idx), data::particle_flags::FIXED))
+ if (!data::particle_flags::isSet( ac.getFlags(idx), data::particle_flags::FIXED))
{
- ac.setPosition(p_idx, ac.getPosition(p_idx) +
- ac.getLinearVelocity(p_idx) * dt_ +
- real_t(0.5) * ac.getInvMass(p_idx) * ac.getOldForce(p_idx) * dt_ * dt_);
- // computation done in body frame: d(omega)/ dt = J^-1 ((J*omega) x omega + T), update in world frame
- // see Wachs, 2019, doi:10.1007/s00707-019-02389-9, Eq. 27
- // note: this implementation (pre and post) is experimental as it is in principle unclear in which order
- // angular velocities and rotations (affect again the transformations WF - BF) have to be carried out
- const auto omegaBF = transformVectorFromWFtoBF(p_idx, ac, ac.getAngularVelocity(p_idx));
- const auto torqueBF = transformVectorFromWFtoBF(p_idx, ac, ac.getOldTorque(p_idx));
- const Vec3 wdotBF = ac.getInvInertiaBF(p_idx) * ( ( ac.getInertiaBF(p_idx) * omegaBF ) % omegaBF + torqueBF );
- const Vec3 wdot = transformVectorFromBFtoWF(p_idx, ac, wdotBF);
+ ac.setPosition(idx, ac.getPosition(idx) +
+ ac.getLinearVelocity(idx) * dt_ +
+ real_t(0.5) * ac.getInvMass(idx) * ac.getOldForce(idx) * dt_ * dt_);
+ // note: contribution (J*omega) x omega is ignored here -> see template for other variant
+ const Vec3 wdot = math::transformMatrixRART(ac.getRotation(idx).getMatrix(),
+ ac.getInvInertiaBF(idx)) * ac.getOldTorque(idx);
+
// Calculating the rotation angle
- const Vec3 phi( ac.getAngularVelocity(p_idx) * dt_ + real_t(0.5) * wdot * dt_ * dt_);
+ const Vec3 phi( ac.getAngularVelocity(idx) * dt_ + real_t(0.5) * wdot * dt_ * dt_);
// Calculating the new orientation
- auto rotation = ac.getRotation(p_idx);
+ auto rotation = ac.getRotation(idx);
rotation.rotate( phi );
- ac.setRotation(p_idx, rotation);
+ ac.setRotation(idx, rotation);
}
}
template <typename Accessor>
-inline void VelocityVerletPostForceUpdate::operator()(const size_t p_idx, Accessor& ac) const
+inline void VelocityVerletPostForceUpdate::operator()(const size_t idx, Accessor& ac) const
{
static_assert(std::is_base_of<data::IAccessor, Accessor>::value, "please provide a valid accessor");
- if (!data::particle_flags::isSet( ac.getFlags(p_idx), data::particle_flags::FIXED))
+ if (!data::particle_flags::isSet( ac.getFlags(idx), data::particle_flags::FIXED))
{
- ac.setLinearVelocity(p_idx, ac.getLinearVelocity(p_idx) +
- real_t(0.5) * ac.getInvMass(p_idx) * (ac.getOldForce(p_idx) + ac.getForce(p_idx)) * dt_);
- const auto omegaBF = transformVectorFromWFtoBF(p_idx, ac, ac.getAngularVelocity(p_idx));
- const auto torqueBF = transformVectorFromWFtoBF(p_idx, ac, 0.5_r * (ac.getOldTorque(p_idx) + ac.getTorque(p_idx)));
- const Vec3 wdotBF = ac.getInvInertiaBF(p_idx) * ( ( ac.getInertiaBF(p_idx) * omegaBF ) % omegaBF + torqueBF );
- const Vec3 wdot = transformVectorFromBFtoWF(p_idx, ac, wdotBF);
-
- ac.setAngularVelocity(p_idx, ac.getAngularVelocity(p_idx) +
+ ac.setLinearVelocity(idx, ac.getLinearVelocity(idx) +
+ real_t(0.5) * ac.getInvMass(idx) * (ac.getOldForce(idx) + ac.getForce(idx)) * dt_);
+ const auto torque = 0.5_r * (ac.getOldTorque(idx) + ac.getTorque(idx));
+ const Vec3 wdot = math::transformMatrixRART(ac.getRotation(idx).getMatrix(),
+ ac.getInvInertiaBF(idx)) * torque;
+
+ ac.setAngularVelocity(idx, ac.getAngularVelocity(idx) +
wdot * dt_ );
}
- ac.setOldForce(p_idx, ac.getForce(p_idx));
- ac.setForce(p_idx, Vec3(real_t(0), real_t(0), real_t(0)));
- ac.setOldTorque(p_idx, ac.getTorque(p_idx));
- ac.setTorque(p_idx, Vec3(real_t(0), real_t(0), real_t(0)));
+ ac.setOldForce(idx, ac.getForce(idx));
+ ac.setForce(idx, Vec3(real_t(0), real_t(0), real_t(0)));
+ ac.setOldTorque(idx, ac.getTorque(idx));
+ ac.setTorque(idx, Vec3(real_t(0), real_t(0), real_t(0)));
}
} //namespace kernel
--
GitLab