diff --git a/python/mesa_pd/templates/kernel/HCSITSRelaxationStep.templ.h b/python/mesa_pd/templates/kernel/HCSITSRelaxationStep.templ.h
index e18d0df56636e10c316682b575c8576a706f155f..6251349f46a34276b25516053c68f282cb17567a 100644
--- a/python/mesa_pd/templates/kernel/HCSITSRelaxationStep.templ.h
+++ b/python/mesa_pd/templates/kernel/HCSITSRelaxationStep.templ.h
@@ -223,9 +223,9 @@ inline real_t HCSITSRelaxationStep::relaxInelasticFrictionlessContacts(size_t ci
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.getDwRef(bId1) -= pa.getInvInertiaBF(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));
+ pa.getDwRef(bId2) += pa.getInvInertiaBF(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) ) -
@@ -261,9 +261,9 @@ inline real_t HCSITSRelaxationStep::relaxInelasticFrictionlessContacts(size_t ci
// 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.getDwRef(bId1) += pa.getInvInertiaBF(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));
+ pa.getDwRef(bId2) -= pa.getInvInertiaBF(bId2) * (ca.getR2(cid) % ca.getP(cid));
}
return delta_max;
@@ -292,9 +292,9 @@ inline real_t HCSITSRelaxationStep::relaxApproximateInelasticCoulombContactsByDe
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.getDwRef(bId1) -= pa.getInvInertiaBF(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));
+ pa.getDwRef(bId2) += pa.getInvInertiaBF(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)
@@ -355,8 +355,8 @@ inline real_t HCSITSRelaxationStep::relaxApproximateInelasticCoulombContactsByDe
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 */ );
+ //real_t gdot_n = gdot_nto[0] + contactCache.n_[i] * ( ( contactCache.body1_[i]->getInvInertiaBF() * ( contactCache.r1_[i] % p_tmp ) ) % contactCache.r1_[i] + ( contactCache.body2_[i]->getInvInertiaBF() * ( contactCache.r2_[i] % p_tmp ) ) % contactCache.r2_[i] /* + diag_[i] * p */ );
+ real_t gdot_n = gdot_nto[0] + ca.getNormal(cid) * ( ( pa.getInvInertiaBF(bId1) * ( ca.getR1(cid) % p_tmp ) ) % ca.getR1(cid) + ( pa.getInvInertiaBF(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 );
@@ -366,7 +366,7 @@ inline real_t HCSITSRelaxationStep::relaxApproximateInelasticCoulombContactsByDe
// 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);
+ Vec3 gdot2 = gdot + ( pa.getInvInertiaBF(bId1) * ( ca.getR1(cid) % p_tmp ) ) % ca.getR1(cid) + ( pa.getInvInertiaBF(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;
@@ -396,9 +396,9 @@ inline real_t HCSITSRelaxationStep::relaxApproximateInelasticCoulombContactsByDe
// 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.getDwRef(bId1) += pa.getInvInertiaBF(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));
+ pa.getDwRef(bId2) -= pa.getInvInertiaBF(bId2) * (ca.getR2(cid) % ca.getP(cid));
}
return delta_max;
}
@@ -428,9 +428,9 @@ inline real_t HCSITSRelaxationStep::relaxInelasticCoulombContactsByDecoupling(si
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.getDwRef(bId1) -= pa.getInvInertiaBF(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));
+ pa.getDwRef(bId2) += pa.getInvInertiaBF(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)
@@ -489,7 +489,7 @@ inline real_t HCSITSRelaxationStep::relaxInelasticCoulombContactsByDecoupling(si
// Calculate the relative contact velocity in the global world frame (if no normal contact reaction is present at contact i)
p_cf[0] = 0;
// |<- p_cf is orthogonal to the normal and drops out in next line ->|
- gdotCorrected = /* ( contactCache.body1_[i]->getInvMass() + contactCache.body2_[i]->getInvMass() ) * p_cf */ gdot + ( pa.getInvInertia(bId1) * ( ca.getR1(cid) % ( ca.getT(cid) * p_cf[1] + ca.getO(cid) * p_cf[2] ) ) ) % ca.getR1(cid) + ( pa.getInvInertia(bId2) * ( ca.getR2(cid) % ( ca.getT(cid) * p_cf[1] + ca.getO(cid) * p_cf[2] ) ) ) % ca.getR2(cid);
+ gdotCorrected = /* ( contactCache.body1_[i]->getInvMass() + contactCache.body2_[i]->getInvMass() ) * p_cf */ gdot + ( pa.getInvInertiaBF(bId1) * ( ca.getR1(cid) % ( ca.getT(cid) * p_cf[1] + ca.getO(cid) * p_cf[2] ) ) ) % ca.getR1(cid) + ( pa.getInvInertiaBF(bId2) * ( ca.getR2(cid) % ( ca.getT(cid) * p_cf[1] + ca.getO(cid) * p_cf[2] ) ) ) % ca.getR2(cid);
gdotCorrected_n = ca.getNormal(cid) * gdotCorrected + ca.getDistance(cid) * dtinv;
// Relax normal component.
@@ -498,7 +498,7 @@ inline real_t HCSITSRelaxationStep::relaxInelasticCoulombContactsByDecoupling(si
// Calculate the relative contact velocity in the global world frame (if no frictional contact reaction is present at contact i)
p_cf[1] = p_cf[2] = real_c( 0 );
// |<- p_cf is orthogonal to the tangential plane and drops out ->|
- gdotCorrected = /* ( contactCache.body1_[i]->getInvMass() + contactCache.body2_[i]->getInvMass() ) * p_cf */ gdot + ( pa.getInvInertia(bId1) * ( ca.getR1(cid) % ( ca.getNormal(cid) * p_cf[0] ) ) ) % ca.getR1(cid) + ( pa.getInvInertia(bId2) * ( ca.getR2(cid) % ( ca.getNormal(cid) * p_cf[0] ) ) ) % ca.getR2(cid);
+ gdotCorrected = /* ( contactCache.body1_[i]->getInvMass() + contactCache.body2_[i]->getInvMass() ) * p_cf */ gdot + ( pa.getInvInertiaBF(bId1) * ( ca.getR1(cid) % ( ca.getNormal(cid) * p_cf[0] ) ) ) % ca.getR1(cid) + ( pa.getInvInertiaBF(bId2) * ( ca.getR2(cid) % ( ca.getNormal(cid) * p_cf[0] ) ) ) % ca.getR2(cid);
gdotCorrected_to[0] = ca.getT(cid) * gdotCorrected;
gdotCorrected_to[1] = ca.getO(cid) * gdotCorrected;
@@ -599,9 +599,9 @@ inline real_t HCSITSRelaxationStep::relaxInelasticCoulombContactsByDecoupling(si
// 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.getDwRef(bId1) += pa.getInvInertiaBF(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));
+ pa.getDwRef(bId2) -= pa.getInvInertiaBF(bId2) * (ca.getR2(cid) % ca.getP(cid));
}
return delta_max;
@@ -642,9 +642,9 @@ inline real_t HCSITSRelaxationStep::relaxInelasticCoulombContactsByOrthogonalPro
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.getDwRef(bId1) -= pa.getInvInertiaBF(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));
+ pa.getDwRef(bId2) += pa.getInvInertiaBF(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) ) -
@@ -702,9 +702,9 @@ inline real_t HCSITSRelaxationStep::relaxInelasticCoulombContactsByOrthogonalPro
// 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.getDwRef(bId1) += pa.getInvInertiaBF(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));
+ pa.getDwRef(bId2) -= pa.getInvInertiaBF(bId2) * (ca.getR2(cid) % ca.getP(cid));
return delta_max;
}
@@ -730,9 +730,9 @@ inline real_t HCSITSRelaxationStep::relaxInelasticGeneralizedMaximumDissipationC
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.getDwRef(bId1) -= pa.getInvInertiaBF(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));
+ pa.getDwRef(bId2) += pa.getInvInertiaBF(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) ) -
@@ -946,9 +946,9 @@ inline real_t HCSITSRelaxationStep::relaxInelasticGeneralizedMaximumDissipationC
// 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.getDwRef(bId1) += pa.getInvInertiaBF(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));
+ pa.getDwRef(bId2) -= pa.getInvInertiaBF(bId2) * (ca.getR2(cid) % ca.getP(cid));
}
return delta_max;
}
@@ -1035,9 +1035,9 @@ inline real_t HCSITSRelaxationStep::relaxInelasticContactsByProjectedGaussSeidel
// 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.getDwRef(bId1) -= pa.getInvInertiaBF(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));
+ pa.getDwRef(bId2) += pa.getInvInertiaBF(bId2) * (ca.getR2(cid) % ca.getP(cid));
}
return delta_max;
}
diff --git a/python/mesa_pd/templates/kernel/InitContactsForHCSITS.templ.h b/python/mesa_pd/templates/kernel/InitContactsForHCSITS.templ.h
index e78443ff6f8b6b5e566bacc1b32beb9ff447f55b..3ccc14b5e9ac8a68119eabf9f6bb8e1be1b8caa4 100644
--- a/python/mesa_pd/templates/kernel/InitContactsForHCSITS.templ.h
+++ b/python/mesa_pd/templates/kernel/InitContactsForHCSITS.templ.h
@@ -159,9 +159,9 @@ inline void InitContactsForHCSITS::operator()(size_t c, CAccessor &ca, PAccessor
Mat3 diag = -(
math::skewSymCrossProduct(ca.getR1(c),
- math::skewSymCrossProduct(pa.getInvInertia(bId1), ca.getR1(c)))
+ math::skewSymCrossProduct(pa.getInvInertiaBF(bId1), ca.getR1(c)))
+ math::skewSymCrossProduct(ca.getR2(c),
- math::skewSymCrossProduct(pa.getInvInertia(bId2), ca.getR2(c))));
+ math::skewSymCrossProduct(pa.getInvInertiaBF(bId2), ca.getR2(c))));
diag[0] += pa.getInvMass(bId1) + pa.getInvMass(bId2);
diag[4] += pa.getInvMass(bId1) + pa.getInvMass(bId2);
diag[8] += pa.getInvMass(bId1) + pa.getInvMass(bId2);
diff --git a/python/mesa_pd/templates/kernel/InitParticlesForHCSITS.templ.h b/python/mesa_pd/templates/kernel/InitParticlesForHCSITS.templ.h
index e218cb1d5e285b2fa55f20947e90533b52443280..290423a766b2d8216aa548d2c4b5a9d842ec4014 100644
--- a/python/mesa_pd/templates/kernel/InitParticlesForHCSITS.templ.h
+++ b/python/mesa_pd/templates/kernel/InitParticlesForHCSITS.templ.h
@@ -87,7 +87,7 @@ inline void InitParticlesForHCSITS::operator()(size_t j, Accessor& ac, real_t dt
initializeVelocityCorrections( ac, j, ac.getDvRef(j), ac.getDwRef(j), dt ); // use applied external forces to calculate starting velocity
if(!isSet(particle_flags, FIXED)){ // Update velocities with global acceleration and angular velocity with euler eqn
ac.getLinearVelocityRef(j) = ac.getLinearVelocity(j) + getGlobalAcceleration() * dt;
- ac.getAngularVelocityRef(j) = ac.getAngularVelocity(j) + dt * ( ac.getInvInertia(j) * ( ( ac.getInertia(j) * ac.getAngularVelocity(j) ) % ac.getAngularVelocity(j) ) );
+ ac.getAngularVelocityRef(j) = ac.getAngularVelocity(j) + dt * ( ac.getInvInertiaBF(j) * ( ( ac.getInertiaBF(j) * ac.getAngularVelocity(j) ) % ac.getAngularVelocity(j) ) );
}
}
}
@@ -110,7 +110,7 @@ template <typename Accessor>
inline void InitParticlesForHCSITS::initializeVelocityCorrections(Accessor& ac, size_t body, Vec3& dv, Vec3& dw, real_t dt ) const
{
dv = ( ac.getInvMass(body) * dt ) * ac.getForce(body);
- dw = dt * ( ac.getInvInertia(body) * ac.getTorque(body) );
+ dw = dt * ( ac.getInvInertiaBF(body) * ac.getTorque(body) );
ac.getForceRef(body) = Vec3();
ac.getTorqueRef(body) = Vec3();
diff --git a/src/mesa_pd/kernel/HCSITSRelaxationStep.h b/src/mesa_pd/kernel/HCSITSRelaxationStep.h
index 5d9ac3a762e244e450ef7d89e7ecd4c5467a9384..bcc0a7dc8a3f6782e9cc494b3011039b19212d2a 100644
--- a/src/mesa_pd/kernel/HCSITSRelaxationStep.h
+++ b/src/mesa_pd/kernel/HCSITSRelaxationStep.h
@@ -234,9 +234,9 @@ inline real_t HCSITSRelaxationStep::relaxInelasticFrictionlessContacts(size_t ci
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.getDwRef(bId1) -= pa.getInvInertiaBF(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));
+ pa.getDwRef(bId2) += pa.getInvInertiaBF(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) ) -
@@ -272,9 +272,9 @@ inline real_t HCSITSRelaxationStep::relaxInelasticFrictionlessContacts(size_t ci
// 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.getDwRef(bId1) += pa.getInvInertiaBF(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));
+ pa.getDwRef(bId2) -= pa.getInvInertiaBF(bId2) * (ca.getR2(cid) % ca.getP(cid));
}
return delta_max;
@@ -303,9 +303,9 @@ inline real_t HCSITSRelaxationStep::relaxApproximateInelasticCoulombContactsByDe
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.getDwRef(bId1) -= pa.getInvInertiaBF(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));
+ pa.getDwRef(bId2) += pa.getInvInertiaBF(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)
@@ -366,8 +366,8 @@ inline real_t HCSITSRelaxationStep::relaxApproximateInelasticCoulombContactsByDe
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 */ );
+ //real_t gdot_n = gdot_nto[0] + contactCache.n_[i] * ( ( contactCache.body1_[i]->getInvInertiaBF() * ( contactCache.r1_[i] % p_tmp ) ) % contactCache.r1_[i] + ( contactCache.body2_[i]->getInvInertiaBF() * ( contactCache.r2_[i] % p_tmp ) ) % contactCache.r2_[i] /* + diag_[i] * p */ );
+ real_t gdot_n = gdot_nto[0] + ca.getNormal(cid) * ( ( pa.getInvInertiaBF(bId1) * ( ca.getR1(cid) % p_tmp ) ) % ca.getR1(cid) + ( pa.getInvInertiaBF(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 );
@@ -377,7 +377,7 @@ inline real_t HCSITSRelaxationStep::relaxApproximateInelasticCoulombContactsByDe
// 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);
+ Vec3 gdot2 = gdot + ( pa.getInvInertiaBF(bId1) * ( ca.getR1(cid) % p_tmp ) ) % ca.getR1(cid) + ( pa.getInvInertiaBF(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;
@@ -407,9 +407,9 @@ inline real_t HCSITSRelaxationStep::relaxApproximateInelasticCoulombContactsByDe
// 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.getDwRef(bId1) += pa.getInvInertiaBF(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));
+ pa.getDwRef(bId2) -= pa.getInvInertiaBF(bId2) * (ca.getR2(cid) % ca.getP(cid));
}
return delta_max;
}
@@ -439,9 +439,9 @@ inline real_t HCSITSRelaxationStep::relaxInelasticCoulombContactsByDecoupling(si
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.getDwRef(bId1) -= pa.getInvInertiaBF(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));
+ pa.getDwRef(bId2) += pa.getInvInertiaBF(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)
@@ -500,7 +500,7 @@ inline real_t HCSITSRelaxationStep::relaxInelasticCoulombContactsByDecoupling(si
// Calculate the relative contact velocity in the global world frame (if no normal contact reaction is present at contact i)
p_cf[0] = 0;
// |<- p_cf is orthogonal to the normal and drops out in next line ->|
- gdotCorrected = /* ( contactCache.body1_[i]->getInvMass() + contactCache.body2_[i]->getInvMass() ) * p_cf */ gdot + ( pa.getInvInertia(bId1) * ( ca.getR1(cid) % ( ca.getT(cid) * p_cf[1] + ca.getO(cid) * p_cf[2] ) ) ) % ca.getR1(cid) + ( pa.getInvInertia(bId2) * ( ca.getR2(cid) % ( ca.getT(cid) * p_cf[1] + ca.getO(cid) * p_cf[2] ) ) ) % ca.getR2(cid);
+ gdotCorrected = /* ( contactCache.body1_[i]->getInvMass() + contactCache.body2_[i]->getInvMass() ) * p_cf */ gdot + ( pa.getInvInertiaBF(bId1) * ( ca.getR1(cid) % ( ca.getT(cid) * p_cf[1] + ca.getO(cid) * p_cf[2] ) ) ) % ca.getR1(cid) + ( pa.getInvInertiaBF(bId2) * ( ca.getR2(cid) % ( ca.getT(cid) * p_cf[1] + ca.getO(cid) * p_cf[2] ) ) ) % ca.getR2(cid);
gdotCorrected_n = ca.getNormal(cid) * gdotCorrected + ca.getDistance(cid) * dtinv;
// Relax normal component.
@@ -509,7 +509,7 @@ inline real_t HCSITSRelaxationStep::relaxInelasticCoulombContactsByDecoupling(si
// Calculate the relative contact velocity in the global world frame (if no frictional contact reaction is present at contact i)
p_cf[1] = p_cf[2] = real_c( 0 );
// |<- p_cf is orthogonal to the tangential plane and drops out ->|
- gdotCorrected = /* ( contactCache.body1_[i]->getInvMass() + contactCache.body2_[i]->getInvMass() ) * p_cf */ gdot + ( pa.getInvInertia(bId1) * ( ca.getR1(cid) % ( ca.getNormal(cid) * p_cf[0] ) ) ) % ca.getR1(cid) + ( pa.getInvInertia(bId2) * ( ca.getR2(cid) % ( ca.getNormal(cid) * p_cf[0] ) ) ) % ca.getR2(cid);
+ gdotCorrected = /* ( contactCache.body1_[i]->getInvMass() + contactCache.body2_[i]->getInvMass() ) * p_cf */ gdot + ( pa.getInvInertiaBF(bId1) * ( ca.getR1(cid) % ( ca.getNormal(cid) * p_cf[0] ) ) ) % ca.getR1(cid) + ( pa.getInvInertiaBF(bId2) * ( ca.getR2(cid) % ( ca.getNormal(cid) * p_cf[0] ) ) ) % ca.getR2(cid);
gdotCorrected_to[0] = ca.getT(cid) * gdotCorrected;
gdotCorrected_to[1] = ca.getO(cid) * gdotCorrected;
@@ -610,9 +610,9 @@ inline real_t HCSITSRelaxationStep::relaxInelasticCoulombContactsByDecoupling(si
// 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.getDwRef(bId1) += pa.getInvInertiaBF(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));
+ pa.getDwRef(bId2) -= pa.getInvInertiaBF(bId2) * (ca.getR2(cid) % ca.getP(cid));
}
return delta_max;
@@ -653,9 +653,9 @@ inline real_t HCSITSRelaxationStep::relaxInelasticCoulombContactsByOrthogonalPro
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.getDwRef(bId1) -= pa.getInvInertiaBF(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));
+ pa.getDwRef(bId2) += pa.getInvInertiaBF(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) ) -
@@ -713,9 +713,9 @@ inline real_t HCSITSRelaxationStep::relaxInelasticCoulombContactsByOrthogonalPro
// 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.getDwRef(bId1) += pa.getInvInertiaBF(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));
+ pa.getDwRef(bId2) -= pa.getInvInertiaBF(bId2) * (ca.getR2(cid) % ca.getP(cid));
return delta_max;
}
@@ -741,9 +741,9 @@ inline real_t HCSITSRelaxationStep::relaxInelasticGeneralizedMaximumDissipationC
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.getDwRef(bId1) -= pa.getInvInertiaBF(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));
+ pa.getDwRef(bId2) += pa.getInvInertiaBF(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) ) -
@@ -957,9 +957,9 @@ inline real_t HCSITSRelaxationStep::relaxInelasticGeneralizedMaximumDissipationC
// 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.getDwRef(bId1) += pa.getInvInertiaBF(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));
+ pa.getDwRef(bId2) -= pa.getInvInertiaBF(bId2) * (ca.getR2(cid) % ca.getP(cid));
}
return delta_max;
}
@@ -1046,9 +1046,9 @@ inline real_t HCSITSRelaxationStep::relaxInelasticContactsByProjectedGaussSeidel
// 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.getDwRef(bId1) -= pa.getInvInertiaBF(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));
+ pa.getDwRef(bId2) += pa.getInvInertiaBF(bId2) * (ca.getR2(cid) % ca.getP(cid));
}
return delta_max;
}
diff --git a/src/mesa_pd/kernel/InitContactsForHCSITS.h b/src/mesa_pd/kernel/InitContactsForHCSITS.h
index fae1131c7515abff21d1e0fd8b57c8bfe5688e2c..9a085442940d39c20c3d1ebb935400e7176ac3f3 100644
--- a/src/mesa_pd/kernel/InitContactsForHCSITS.h
+++ b/src/mesa_pd/kernel/InitContactsForHCSITS.h
@@ -154,9 +154,9 @@ inline void InitContactsForHCSITS::operator()(size_t c, CAccessor &ca, PAccessor
Mat3 diag = -(
math::skewSymCrossProduct(ca.getR1(c),
- math::skewSymCrossProduct(pa.getInvInertia(bId1), ca.getR1(c)))
+ math::skewSymCrossProduct(pa.getInvInertiaBF(bId1), ca.getR1(c)))
+ math::skewSymCrossProduct(ca.getR2(c),
- math::skewSymCrossProduct(pa.getInvInertia(bId2), ca.getR2(c))));
+ math::skewSymCrossProduct(pa.getInvInertiaBF(bId2), ca.getR2(c))));
diag[0] += pa.getInvMass(bId1) + pa.getInvMass(bId2);
diag[4] += pa.getInvMass(bId1) + pa.getInvMass(bId2);
diag[8] += pa.getInvMass(bId1) + pa.getInvMass(bId2);
diff --git a/src/mesa_pd/kernel/InitParticlesForHCSITS.h b/src/mesa_pd/kernel/InitParticlesForHCSITS.h
index aba85f763f28e8a817e3a080c688ee608ea7adcc..ba95fbf82d6549060e50d3bdfecad954192df4df 100644
--- a/src/mesa_pd/kernel/InitParticlesForHCSITS.h
+++ b/src/mesa_pd/kernel/InitParticlesForHCSITS.h
@@ -83,7 +83,7 @@ inline void InitParticlesForHCSITS::operator()(size_t j, Accessor& ac, real_t dt
initializeVelocityCorrections( ac, j, ac.getDvRef(j), ac.getDwRef(j), dt ); // use applied external forces to calculate starting velocity
if(!isSet(particle_flags, FIXED)){ // Update velocities with global acceleration and angular velocity with euler eqn
ac.getLinearVelocityRef(j) = ac.getLinearVelocity(j) + getGlobalAcceleration() * dt;
- ac.getAngularVelocityRef(j) = ac.getAngularVelocity(j) + dt * ( ac.getInvInertia(j) * ( ( ac.getInertia(j) * ac.getAngularVelocity(j) ) % ac.getAngularVelocity(j) ) );
+ ac.getAngularVelocityRef(j) = ac.getAngularVelocity(j) + dt * ( ac.getInvInertiaBF(j) * ( ( ac.getInertiaBF(j) * ac.getAngularVelocity(j) ) % ac.getAngularVelocity(j) ) );
}
}
}
@@ -106,7 +106,7 @@ template <typename Accessor>
inline void InitParticlesForHCSITS::initializeVelocityCorrections(Accessor& ac, size_t body, Vec3& dv, Vec3& dw, real_t dt ) const
{
dv = ( ac.getInvMass(body) * dt ) * ac.getForce(body);
- dw = dt * ( ac.getInvInertia(body) * ac.getTorque(body) );
+ dw = dt * ( ac.getInvInertiaBF(body) * ac.getTorque(body) );
ac.getForceRef(body) = Vec3();
ac.getTorqueRef(body) = Vec3();
diff --git a/tests/mesa_pd/kernel/HCSITSKernels.cpp b/tests/mesa_pd/kernel/HCSITSKernels.cpp
index 4f0428f3895b8559a50136eaea35191391d4c098..dc26956395e64a6623613ef7dff38a7321e755d2 100644
--- a/tests/mesa_pd/kernel/HCSITSKernels.cpp
+++ b/tests/mesa_pd/kernel/HCSITSKernels.cpp
@@ -63,8 +63,8 @@ public:
const real_t& getMass(const size_t p_idx) const {return ss_->shapes[ps_->getShapeIDRef(p_idx)]->getMass();}
const real_t& getInvMass(const size_t p_idx) const {return ss_->shapes[ps_->getShapeIDRef(p_idx)]->getInvMass();}
- const Mat3& getInertia(const size_t p_idx) const {return ss_->shapes[ps_->getShapeIDRef(p_idx)]->getInertiaBF();}
- const Mat3& getInvInertia(const size_t p_idx) const {return ss_->shapes[ps_->getShapeIDRef(p_idx)]->getInvInertiaBF();}
+ const Mat3& getInertiaBF(const size_t p_idx) const {return ss_->shapes[ps_->getShapeIDRef(p_idx)]->getInertiaBF();}
+ const Mat3& getInvInertiaBF(const size_t p_idx) const {return ss_->shapes[ps_->getShapeIDRef(p_idx)]->getInvInertiaBF();}
data::BaseShape* getShape(const size_t p_idx) const {return ss_->shapes[ps_->getShapeIDRef(p_idx)].get();}
private: