Commit b2f8b539 authored by Sebastian Eibl's avatar Sebastian Eibl
Browse files

Merge branch 'MESAPDHardContactSolvers' into 'master'

Integration of Hard Contact Solvers into MESA-PD

See merge request walberla/walberla!218
parents 1969d023 1f3e8215
......@@ -90,7 +90,11 @@ if __name__ == '__main__':
kernels.append( kernel.ExplicitEuler() )
kernels.append( kernel.ExplicitEulerWithShape() )
kernels.append( kernel.ForceLJ() )
kernels.append( kernel.HCSITSRelaxationStep() )
kernels.append( kernel.HeatConduction() )
kernels.append( kernel.InitParticlesForHCSITS() )
kernels.append( kernel.InitContactsForHCSITS() )
kernels.append( kernel.IntegrateParticlesHCSITS() )
kernels.append( kernel.InsertParticleIntoLinkedCells() )
kernels.append( kernel.LinearSpringDashpot() )
kernels.append( kernel.NonLinearSpringDashpot() )
......
# -*- coding: utf-8 -*-
from mesa_pd.accessor import Accessor
from ..Container import Container
from mesa_pd.utility import generateFile
class HCSITSRelaxationStep(Container):
def __init__(self):
super().__init__()
self.addProperty("maxSubIterations", "size_t", defValue = "20")
self.addProperty("relaxationModel", "RelaxationModel", defValue = "InelasticFrictionlessContact")
self.addProperty("deltaMax", "real_t", defValue = "0")
self.addProperty("cor", "real_t", defValue = "real_t(0.2)")
self.accessor = Accessor()
self.accessor.require("uid", "walberla::id_t", access="g")
self.accessor.require("position", "walberla::mesa_pd::Vec3", access="g")
self.accessor.require("linearVelocity", "walberla::mesa_pd::Vec3", access="g")
self.accessor.require("angularVelocity", "walberla::mesa_pd::Vec3", access="g")
self.accessor.require("invMass", "walberla::real_t", access="g" )
self.accessor.require("invInertia", "walberla::mesa_pd::Mat3", access="g" )
self.accessor.require("dv", "walberla::mesa_pd::Vec3", access="gr" )
self.accessor.require("dw", "walberla::mesa_pd::Vec3", access="gr" )
def getRequirements(self):
return self.accessor
def generate(self, path):
context = dict()
context["properties"] = self.properties
context["interface"] = self.accessor.properties
generateFile(path, 'kernel/HCSITSRelaxationStep.templ.h', context)
# -*- coding: utf-8 -*-
from mesa_pd.accessor import Accessor
from ..Container import Container
from mesa_pd.utility import generateFile
class InitContactsForHCSITS(Container):
def __init__(self):
super().__init__()
self.addProperty("erp", "real_t", defValue = "real_t(0.8)")
self.addProperty("maximumPenetration", "real_t", defValue ="0")
self.paccessor = Accessor()
self.paccessor.require("uid", "walberla::id_t", access="g")
self.paccessor.require("position", "walberla::mesa_pd::Vec3", access="g")
self.paccessor.require("invInertia", "walberla::mesa_pd::Mat3", access="g" )
self.paccessor.require("invMass", "walberla::real_t", access="g" )
def getRequirements(self):
return self.paccessor
def generate(self, path):
context = dict()
context["properties"] = self.properties
context["material_parameters"] = ["friction"]
context["interface"] = self.paccessor.properties
generateFile(path, 'kernel/InitContactsForHCSITS.templ.h', context)
# -*- coding: utf-8 -*-
from mesa_pd.accessor import Accessor
from ..Container import Container
from mesa_pd.utility import generateFile
class InitParticlesForHCSITS(Container):
def __init__(self):
super().__init__()
self.addProperty("globalAcceleration", "walberla::mesa_pd::Vec3", defValue = "0")
self.accessor = Accessor()
self.accessor.require("uid", "walberla::id_t", access="g")
self.accessor.require("linearVelocity", "walberla::mesa_pd::Vec3", access="gr")
self.accessor.require("angularVelocity", "walberla::mesa_pd::Vec3", access="gr")
self.accessor.require("invMass", "walberla::real_t", access="g" )
self.accessor.require("inertia", "walberla::mesa_pd::Mat3", access="g" )
self.accessor.require("invInertia", "walberla::mesa_pd::Mat3", access="g" )
self.accessor.require("dv", "walberla::mesa_pd::Vec3", access="gr" )
self.accessor.require("dw", "walberla::mesa_pd::Vec3", access="gr" )
self.accessor.require("torque", "walberla::mesa_pd::Vec3", access="r" )
self.accessor.require("force", "walberla::mesa_pd::Vec3", access="r" )
def getRequirements(self):
return self.accessor
def generate(self, path):
context = dict()
context["properties"] = self.properties
context["interface"] = self.accessor.properties
generateFile(path, 'kernel/InitParticlesForHCSITS.templ.h', context)
# -*- coding: utf-8 -*-
from mesa_pd.accessor import Accessor
from ..Container import Container
from mesa_pd.utility import generateFile
class IntegrateParticlesHCSITS(Container):
def __init__(self):
super().__init__()
self.addProperty("speedLimiterActive", "bool", defValue = "false")
self.addProperty("speedLimitFactor", "real_t", defValue ="real_t(1.0)")
self.accessor = Accessor()
self.accessor.require("uid", "walberla::id_t", access="g")
self.accessor.require("position", "walberla::mesa_pd::Vec3", access="gr")
self.accessor.require("rotation", "walberla::mesa_pd::Rot3", access="gr")
self.accessor.require("linearVelocity", "walberla::mesa_pd::Vec3", access="gr")
self.accessor.require("angularVelocity", "walberla::mesa_pd::Vec3", access="gr")
self.accessor.require("dv", "walberla::mesa_pd::Vec3", access="g" )
self.accessor.require("dw", "walberla::mesa_pd::Vec3", access="g" )
self.accessor.require("flags", "walberla::mesa_pd::data::particle_flags::FlagT", access="g")
def getRequirements(self):
return self.accessor
def generate(self, path):
context = dict()
context["properties"] = self.properties
context["interface"] = self.accessor.properties
generateFile(path, 'kernel/IntegrateParticlesHCSITS.templ.h', context)
......@@ -4,7 +4,11 @@ from .DoubleCast import DoubleCast
from .ExplicitEuler import ExplicitEuler
from .ExplicitEulerWithShape import ExplicitEulerWithShape
from .ForceLJ import ForceLJ
from .HCSITSRelaxationStep import HCSITSRelaxationStep
from .HeatConduction import HeatConduction
from .InitParticlesForHCSITS import InitParticlesForHCSITS
from .InitContactsForHCSITS import InitContactsForHCSITS
from .IntegrateParticlesHCSITS import IntegrateParticlesHCSITS
from .InsertParticleIntoLinkedCells import InsertParticleIntoLinkedCells
from .LinearSpringDashpot import LinearSpringDashpot
from .NonLinearSpringDashpot import NonLinearSpringDashpot
......@@ -19,7 +23,11 @@ __all__ = ['DoubleCast',
'ExplicitEuler',
'ExplicitEulerWithShape',
'ForceLJ',
'HCSITSRelaxationStep',
'HeatConduction',
'InitParticlesForHCSITS',
'InitContactsForHCSITS',
'IntegrateParticlesHCSITS',
'InsertParticleIntoLinkedCells',
'LinearSpringDashpot',
'NonLinearSpringDashpot',
......
......@@ -26,7 +26,7 @@
#pragma once
#include <mesa_pd/data/IAccessor.h>
#include <mesa_pd/data/IContactAccessor.h>
#include <mesa_pd/data/ContactStorage.h>
#include <core/UniqueID.h>
......@@ -43,7 +43,7 @@ namespace data {
* 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
class ContactAccessor : public IContactAccessor
{
public:
ContactAccessor(const std::shared_ptr<data::ContactStorage>& ps) : ps_(ps) {}
......
//======================================================================================================================
//
// 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 HCSITSRelaxationStep.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/common/ParticleFunctions.h>
#include <mesa_pd/data/DataTypes.h>
#include <mesa_pd/data/IAccessor.h>
#include <mesa_pd/data/IContactAccessor.h>
#include <mesa_pd/data/Flags.h>
#include <core/logging/Logging.h>
#include <core/math/Constants.h>
#include <core/math/Limits.h>
#include <vector>
namespace walberla {
namespace mesa_pd {
namespace kernel {
/**
* Apply a HCSITS (Hard-contact semi-implicit timestepping solver) relaxation to all contacts.
* Call this kernel on all bodies to perform on relaxation step of the solver.
* There exist different friction and relaxation models. See comments on their respective methods
* for an extended documentation. Use setRelaxationModel() to set it.
* getDeltaMax() can be used to query the maximum change in the impulses applied wrt to the last iteration.
*
* Use setMaxSubIterations() to set the maximum number of iterations performed to solve optimization problems arising
* during the relaxation (in InelasticGeneralizedMaximumDissipationContact and InelasticCoulombContactByDecoupling).
*
* Use setCor() to set the coefficient of restitution between 0 and 1 for the InelasticProjectedGaussSeidel model.
* All other models describe inelastic contacts.
*
* Example of Using the HCSITS kernels in a simulation:
* \code
* // ps: Particle storage
* // cs: Contact storage
* // pa: Particle accessor
* // ca: Contact accessor
*
* // Detect contacts first and fill contact storage
*
* mesa_pd::mpi::ReduceProperty reductionKernel;
* mesa_pd::mpi::BroadcastProperty broadcastKernel;
*
* kernel::IntegrateBodiesHCSITS integration;
*
* kernel::InitContactsForHCSITS initContacts(1);
* initContacts.setFriction(0,0,real_t(0.2));
* kernel::InitBodiesForHCSITS initBodies;
*
* kernel::HCSITSRelaxationStep relaxationStep;
* relaxationStep.setCor(real_t(0.1)); // Only effective for PGSM
*
* // Init Contacts and Bodies (order is arbitrary)
* cs.forEachContact(false, kernel::SelectAll(), ca, initContacts, ca, pa);
* ps.forEachParticle(false, kernel::SelectAll(), pa, initBodies, pa, dt);
*
* // Reduce and Broadcast velocities with relaxation parameter 1 before the iteration
* VelocityUpdateNotification::Parameters::relaxationParam = real_t(1.0);
* reductionKernel.operator()<VelocityCorrectionNotification>(*ps);
* broadcastKernel.operator()<VelocityUpdateNotification>(*ps);
*
* VelocityUpdateNotification::Parameters::relaxationParam = real_t(0.8);
* for(int j = 0; j < 10; j++){
* cs.forEachContact(false, kernel::SelectAll(), ca, relaxationStep, ca, pa, dt);
* reductionKernel.operator()<VelocityCorrectionNotification>(*ps);
* broadcastKernel.operator()<VelocityUpdateNotification>(*ps);
* }
*
* ps.forEachParticle(false, kernel::SelectAll(), pa, integration, pa, dt);
* \endcode
* \ingroup mesa_pd_kernel
*
*
*/
class HCSITSRelaxationStep
{
public:
enum RelaxationModel {
InelasticFrictionlessContact,
ApproximateInelasticCoulombContactByDecoupling,
ApproximateInelasticCoulombContactByOrthogonalProjections,
InelasticCoulombContactByDecoupling,
InelasticCoulombContactByOrthogonalProjections,
InelasticGeneralizedMaximumDissipationContact,
InelasticProjectedGaussSeidel
};
// Default constructor sets the default values
HCSITSRelaxationStep() :
{%- for prop in properties %}
{{prop.name}}_( {{prop.defValue}} ){{ "," if not loop.last}}
{%- endfor %}
{}
// Getter and Setter Functions
{%- for prop in properties %}
inline const {{prop.type}}& get{{prop.name | capFirst}}() const {return {{prop.name}}_;}
inline void set{{prop.name | capFirst}}({{prop.type}} v) { {{prop.name}}_ = v;}
{% endfor %}
/**
* Perform relaxation for a single contact.
* \param cid The index of the contact with the accessor ca.
* \param ca The contact accessor.
* \param pa The particle accessor.
* \param dt The timestep size used.
*/
template <typename CAccessor, typename PAccessor>
void operator()(size_t cid, CAccessor &ca, PAccessor& pa, real_t dt);
template <typename CAccessor, typename PAccessor>
real_t relaxInelasticFrictionlessContacts(size_t cid, real_t dtinv, CAccessor &ca, PAccessor& pa);
template <typename CAccessor, typename PAccessor>
real_t relaxApproximateInelasticCoulombContactsByDecoupling(size_t cid, real_t dtinv, CAccessor &ca, PAccessor& pa);
template <typename CAccessor, typename PAccessor>
real_t relaxInelasticCoulombContactsByDecoupling(size_t cid, real_t dtinv, CAccessor &ca, PAccessor& pa );
template <typename CAccessor, typename PAccessor>
real_t relaxInelasticCoulombContactsByOrthogonalProjections(size_t cid, real_t dtinv, bool approximate, CAccessor &ca, PAccessor& pa );
template <typename CAccessor, typename PAccessor>
real_t relaxInelasticGeneralizedMaximumDissipationContacts(size_t cid, real_t dtinv, CAccessor &ca, PAccessor& pa );
template <typename CAccessor, typename PAccessor>
real_t relaxInelasticContactsByProjectedGaussSeidel(size_t cid, real_t dtinv, CAccessor &ca, PAccessor& pa );
private:
// List properties
{% for prop in properties %}
{{prop.type}} {{prop.name }}_;
{%- endfor %}
};
template <typename CAccessor, typename PAccessor>
inline void HCSITSRelaxationStep::operator()(size_t cid, CAccessor &ca, PAccessor& pa, real_t dt)
{
static_assert(std::is_base_of<data::IContactAccessor, CAccessor>::value, "please provide a valid contact accessor");
static_assert(std::is_base_of<data::IAccessor, PAccessor>::value, "please provide a valid accessor");
real_t dtinv(real_t(1.0)/dt);
switch( getRelaxationModel() ) {
case InelasticFrictionlessContact:
setDeltaMax(std::max( getDeltaMax(), relaxInelasticFrictionlessContacts(cid, dtinv, ca, pa )));
break;
case ApproximateInelasticCoulombContactByDecoupling:
setDeltaMax(std::max( getDeltaMax(), relaxApproximateInelasticCoulombContactsByDecoupling( cid, dtinv, ca, pa )));
break;
case ApproximateInelasticCoulombContactByOrthogonalProjections:
setDeltaMax(std::max( getDeltaMax(), relaxInelasticCoulombContactsByOrthogonalProjections( cid, dtinv, true, ca, pa )));
break;
case InelasticCoulombContactByDecoupling:
setDeltaMax(std::max( getDeltaMax(), relaxInelasticCoulombContactsByDecoupling( cid, dtinv, ca, pa )));
break;
case InelasticCoulombContactByOrthogonalProjections:
setDeltaMax(std::max( getDeltaMax(), relaxInelasticCoulombContactsByOrthogonalProjections( cid, dtinv, false, ca, pa )));
break;
case InelasticGeneralizedMaximumDissipationContact:
setDeltaMax(std::max( getDeltaMax(), relaxInelasticGeneralizedMaximumDissipationContacts( cid, dtinv, ca, pa )));
break;
case InelasticProjectedGaussSeidel:
setDeltaMax(std::max( getDeltaMax(), relaxInelasticContactsByProjectedGaussSeidel( cid, dtinv, ca, pa )));
break;
default:
throw std::runtime_error( "Unsupported relaxation model." );
}
WALBERLA_LOG_DETAIL("Delta Max: " << getDeltaMax());
}
//*************************************************************************************************
/*!\brief Relaxes The contact with ID cid once. The contact model is for inelastic unilateral contacts without friction.
*
* \param cid The index of the contact
* \param ca The contact accessor
* \param pa The particle accessor
* \return The largest variation of contact impulses in the L-infinity norm.
*
* This function is to be called from resolveContacts(). Separating contacts are preferred over
* persisting solutions if valid.
*/
template <typename CAccessor, typename PAccessor>
inline real_t HCSITSRelaxationStep::relaxInelasticFrictionlessContacts(size_t cid, real_t dtinv, CAccessor &ca, PAccessor& pa)
{
real_t delta_max( 0 );
// Remove velocity corrections of this contact's reaction.
size_t bId1 = ca.getId1(cid);
size_t bId2 = ca.getId2(cid);
pa.getDvRef(bId1) -= pa.getInvMass(bId1) * ca.getP(cid);
pa.getDwRef(bId1) -= pa.getInvInertia(bId1) * (ca.getR1(cid) % ca.getP(cid));
pa.getDvRef(bId2) += pa.getInvMass(bId2) * ca.getP(cid);
pa.getDwRef(bId2) += pa.getInvInertia(bId2) * (ca.getR2(cid) % ca.getP(cid));
// Calculate the relative contact VELOCITY in the global world frame (if no contact reaction is present at contact i)
Vec3 gdot ( ( pa.getLinearVelocity(bId1) + pa.getDv(bId1) ) -
( pa.getLinearVelocity(bId2) + pa.getDv(bId2) ) +
( pa.getAngularVelocity(bId1) + pa.getDw(bId1) ) % ca.getR1(cid) -
( pa.getAngularVelocity(bId2) + pa.getDw(bId2) ) % ca.getR2(cid) /* + diag_[i] * p */ );
// Change from the global world frame to the contact frame
Mat3 contactframe( ca.getNormal(cid), ca.getT(cid), ca.getO(cid) );
Vec3 gdot_nto( contactframe.getTranspose() * gdot );
// The constraint in normal direction is actually a positional constraint but instead of g_n we use g_n/dt equivalently and call it gdot_n
gdot_nto[0] += ( /* + trans( contactCache.n_[i] ) * ( contactCache.body1_[i]->getPosition() + contactCache.r1_[i] ) - ( contactCache.body2_[i]->getPosition() + contactCache.r2_[i] ) */ + ca.getDistance(cid) ) * dtinv;
if( gdot_nto[0] >= 0 ) {
// Contact is separating if no contact reaction is present at contact i.
delta_max = std::max( delta_max, std::max( std::abs( ca.getP(cid)[0] ), std::max( std::abs( ca.getP(cid)[1] ), std::abs( ca.getP(cid)[2] ) ) ) );
ca.getPRef(cid) = Vec3();
// No need to apply zero impulse.
}
else {
// Contact is persisting.
// Calculate the impulse necessary for a static contact expressed as components in the contact frame.
Vec3 p_wf( ca.getNormal(cid) * ( -ca.getDiag_n_inv(cid) * gdot_nto[0] ) );
Vec3 dp( ca.getP(cid) - p_wf );
delta_max = std::max( delta_max, std::max( std::abs( dp[0] ), std::max( std::abs( dp[1] ), std::abs( dp[2] ) ) ) );
ca.getPRef(cid) = p_wf;
// Apply impulse right away.
pa.getDvRef(bId1) += pa.getInvMass(bId1) * ca.getP(cid);
pa.getDwRef(bId1) += pa.getInvInertia(bId1) * (ca.getR1(cid) % ca.getP(cid));
pa.getDvRef(bId2) -= pa.getInvMass(bId2) * ca.getP(cid);
pa.getDwRef(bId2) -= pa.getInvInertia(bId2) * (ca.getR2(cid) % ca.getP(cid));
}
return delta_max;
}
//*************************************************************************************************
//*************************************************************************************************
/*!\brief Relaxes all contacts once. The contact model is for inelastic unilateral contacts with approximate Coulomb friction.
*
* \return The largest variation of contact impulses in the L-infinity norm.
*
* This function is to be called from resolveContacts(). Separating contacts are preferred over
* other solutions if valid. Static solutions are preferred over dynamic solutions. Dynamic
* solutions are computed by decoupling the normal from the frictional components. That is
* for a dynamic contact the normal component is relaxed first followed by the frictional
* components. The determination of the frictional components does not perform any subiterations
* and guarantees that the friction partially opposes slip.
*/
template <typename CAccessor, typename PAccessor>
inline real_t HCSITSRelaxationStep::relaxApproximateInelasticCoulombContactsByDecoupling(size_t cid, real_t dtinv, CAccessor &ca, PAccessor& pa )
{
real_t delta_max( 0 );
// Remove velocity corrections of this contact's reaction.
size_t bId1 = ca.getId1(cid);
size_t bId2 = ca.getId2(cid);
pa.getDvRef(bId1) -= pa.getInvMass(bId1) * ca.getP(cid);
pa.getDwRef(bId1) -= pa.getInvInertia(bId1) * (ca.getR1(cid) % ca.getP(cid));
pa.getDvRef(bId2) += pa.getInvMass(bId2) * ca.getP(cid);
pa.getDwRef(bId2) += pa.getInvInertia(bId2) * (ca.getR2(cid) % ca.getP(cid));
// Calculate the relative contact VELOCITY in the global world frame (if no contact reaction is present at contact i)
Vec3 gdot ( ( pa.getLinearVelocity(bId1) + pa.getDv(bId1) ) -
( pa.getLinearVelocity(bId2) + pa.getDv(bId2) ) +
( pa.getAngularVelocity(bId1) + pa.getDw(bId1) ) % ca.getR1(cid) -
( pa.getAngularVelocity(bId2) + pa.getDw(bId2) ) % ca.getR2(cid) /* + diag_[i] * p */ );
// Change from the global world frame to the contact frame
Mat3 contactframe( ca.getNormal(cid), ca.getT(cid), ca.getO(cid) );
Vec3 gdot_nto( contactframe.getTranspose() * gdot );
// The constraint in normal direction is actually a positional constraint but instead of g_n we use g_n/dt equivalently and call it gdot_n
gdot_nto[0] += ( /* + trans( contactCache.n_[i] ) * ( contactCache.body1_[i]->getPosition() + contactCache.r1_[i] ) - ( contactCache.body2_[i]->getPosition() + contactCache.r2_[i] ) */ + ca.getDistance(cid) ) * dtinv;
if( gdot_nto[0] >= 0 ) {
// Contact is separating if no contact reaction is present at contact i.
delta_max = std::max( delta_max, std::max( std::abs( ca.getP(cid)[0] ), std::max( std::abs( ca.getP(cid)[1] ), std::abs( ca.getP(cid)[2] ) ) ) );
ca.getPRef(cid) = Vec3();
// No need to apply zero impulse.
}
else {
// Contact is persisting (either static or dynamic).
// Calculate the impulse necessary for a static contact expressed as components in the contact frame.
//Vec3 p_cf( -( contactCache.diag_nto_inv_[i] * gdot_nto ) );
Vec3 p_cf( -( ca.getDiag_nto_inv(cid) * gdot_nto ) );
// Can p_cf[0] be negative even though -gdot_nto[0] > 0? Yes! Try:
// A = [0.5 -0.1 +0.1; -0.1 0.5 -0.1; +0.1 -0.1 1];
// b = [0.01 -1 -1]';
// A\b \approx [-0.19 -2.28 -1.21]'
// eig(A) \approx [ 0.40 0.56 1.04]'
//real_t flimit( contactCache.mu_[i] * p_cf[0] );
real_t flimit( ca.getMu(cid) * p_cf[0] );
real_t fsq( p_cf[1] * p_cf[1] + p_cf[2] * p_cf[2] );
if( fsq > flimit * flimit || p_cf[0] < 0 ) {
// Contact cannot be static so it must be dynamic.
// => Complementarity condition on normal reaction now turns into an equation since we know that the normal reaction is definitely not zero.
// For simplicity we change to a simpler relaxation scheme here:
// 1. Relax normal reaction with the tangential components equal to the previous values
// 2. Relax tangential components with the newly relaxed normal reaction
// Note: The better approach would be to solve the true 3x3 block problem!
// Warning: Simply projecting the frictional components is wrong since then the normal action is no longer 0 and simulations break.
// Add the action of the frictional reactions from the last iteration to the relative contact velocity in normal direction so we can relax it separately.
// TODO This can be simplified:
//p_cf = trans( contactframe ) * contactCache.p_[i];
//p_cf[0] = 0;
//p_[i] = contactframe * p_cf;
//Vec3 p_tmp = ( contactCache.t_[i] * contactCache.p_[i] ) * contactCache.t_[i] + ( contactCache.o_[i] * contactCache.p_[i] ) * contactCache.o_[i];
Vec3 p_tmp = ( ca.getT(cid) * ca.getP(cid) ) * ca.getT(cid) + ( ca.getO(cid) * ca.getP(cid) ) * ca.getO(cid);
//real_t gdot_n = gdot_nto[0] + contactCache.n_[i] * ( ( contactCache.body1_[i]->getInvInertia() * ( contactCache.r1_[i] % p_tmp ) ) % contactCache.r1_[i] + ( contactCache.body2_[i]->getInvInertia() * ( contactCache.r2_[i] % p_tmp ) ) % contactCache.r2_[i] /* + diag_[i] * p */ );
real_t gdot_n = gdot_nto[0] + ca.getNormal(cid) * ( ( pa.getInvInertia(bId1) * ( ca.getR1(cid) % p_tmp ) ) % ca.getR1(cid) + ( pa.getInvInertia(bId2) * ( ca.getR2(cid) % p_tmp ) ) % ca.getR2(cid) /* + diag_[i] * p */ );
//p_cf[0] = -( contactCache.diag_n_inv_[i] * gdot_n );
p_cf[0] = -( ca.getDiag_n_inv(cid) * gdot_n );
// We cannot be sure that gdot_n <= 0 here and thus p_cf[0] >= 0 since we just modified it with the old values of the tangential reactions! => Project
p_cf[0] = std::max( real_c( 0 ), p_cf[0] );
// Now add the action of the normal reaction to the relative contact velocity in the tangential directions so we can relax the frictional components separately.
p_tmp = ca.getNormal(cid) * p_cf[0];
Vec3 gdot2 = gdot + ( pa.getInvInertia(bId1) * ( ca.getR1(cid) % p_tmp ) ) % ca.getR1(cid) + ( pa.getInvInertia(bId2)* ( ca.getR2(cid) % p_tmp ) ) % ca.getR2(cid);
Vec2 gdot_to;
gdot_to[0] = ca.getT(cid) * gdot2;
gdot_to[1] = ca.getO(cid) * gdot2;
//Vec2 ret = -( contactCache.diag_to_inv_[i] * gdot_to );
Vec2 ret = -( ca.getDiag_to_inv(cid) * gdot_to );
p_cf[1] = ret[0];
p_cf[2] = ret[1];
flimit = ca.getMu(cid) * p_cf[0];
fsq = p_cf[1] * p_cf[1] + p_cf[2] * p_cf[2];
if( fsq > flimit * flimit ) {
const real_t f( flimit / std::sqrt( fsq ) );
p_cf[1] *= f;
p_cf[2] *= f;
}
}
else {
// Contact is static.
}
Vec3 p_wf( contactframe * p_cf );