From 90c4bdf46a4e98da35ff244ac8e3c58beb49f866 Mon Sep 17 00:00:00 2001
From: Christoph Schwarzmeier <christoph.schwarzmeier@fau.de>
Date: Thu, 18 Nov 2021 13:59:42 +0100
Subject: [PATCH] Add Vector3::getNormalizedIfNotZero() and correct
Vector3::getNormalizedOrZero()
---
.../kernel/LinearSpringDashpot.templ.h | 2 +-
.../kernel/NonLinearSpringDashpot.templ.h | 2 +-
.../templates/kernel/SpringDashpot.templ.h | 2 +-
.../kernel/SpringDashpotSpring.templ.h | 4 +--
src/core/math/Vector3.h | 36 ++++++++++++++++---
.../AnalyticCollisionFunctions.h | 2 +-
src/mesa_pd/collision_detection/EPA.cpp | 16 ++++-----
src/mesa_pd/collision_detection/EPA.h | 4 +--
src/mesa_pd/kernel/LinearSpringDashpot.h | 2 +-
src/mesa_pd/kernel/NonLinearSpringDashpot.h | 2 +-
src/mesa_pd/kernel/SpringDashpot.h | 2 +-
src/mesa_pd/kernel/SpringDashpotSpring.h | 4 +--
src/pe/cr/ContactResolvers.h | 2 +-
13 files changed, 53 insertions(+), 27 deletions(-)
diff --git a/python/mesa_pd/templates/kernel/LinearSpringDashpot.templ.h b/python/mesa_pd/templates/kernel/LinearSpringDashpot.templ.h
index 2696e0f34..efa413887 100644
--- a/python/mesa_pd/templates/kernel/LinearSpringDashpot.templ.h
+++ b/python/mesa_pd/templates/kernel/LinearSpringDashpot.templ.h
@@ -202,7 +202,7 @@ inline void LinearSpringDashpot::operator()(const size_t p_idx1,
Vec3 fTLS = stiffnessT * newTangentialSpringDisplacement +
dampingT * relVelT;
- const Vec3 t = fTLS.getNormalizedOrZero(); // tangential unit vector
+ const Vec3 t = fTLS.getNormalizedIfNotZero(); // tangential unit vector
// calculate friction force
const real_t fFrictionAbsStatic = getFrictionCoefficientStatic(ac.getType(p_idx1), ac.getType(p_idx2)) * fN.length(); // sticking, rolling
diff --git a/python/mesa_pd/templates/kernel/NonLinearSpringDashpot.templ.h b/python/mesa_pd/templates/kernel/NonLinearSpringDashpot.templ.h
index 3cc2fbd7f..8eda92ecf 100644
--- a/python/mesa_pd/templates/kernel/NonLinearSpringDashpot.templ.h
+++ b/python/mesa_pd/templates/kernel/NonLinearSpringDashpot.templ.h
@@ -207,7 +207,7 @@ inline void NonLinearSpringDashpot::operator()(const size_t p_idx1,
Vec3 fTLS = stiffnessT * newTangentialSpringDisplacement +
dampingT * relVelT;
- const Vec3 t = fTLS.getNormalizedOrZero(); // tangential unit vector
+ const Vec3 t = fTLS.getNormalizedIfNotZero(); // tangential unit vector
// calculate friction force
const real_t fFrictionAbsStatic = getFrictionCoefficientStatic(ac.getType(p_idx1), ac.getType(p_idx2)) * fN.length(); // sticking, rolling
diff --git a/python/mesa_pd/templates/kernel/SpringDashpot.templ.h b/python/mesa_pd/templates/kernel/SpringDashpot.templ.h
index 29c8a1076..bec9eb793 100644
--- a/python/mesa_pd/templates/kernel/SpringDashpot.templ.h
+++ b/python/mesa_pd/templates/kernel/SpringDashpot.templ.h
@@ -178,7 +178,7 @@ inline void SpringDashpot::operator()(const size_t p_idx1,
// Calculating the tangential force based on the model by Haff and Werner
const real_t fTabs( std::min( getDampingT(ac.getType(p_idx1), ac.getType(p_idx2)) * relVelT.length(), getFriction(ac.getType(p_idx1), ac.getType(p_idx2)) * fNabs ) );
- const Vec3 fT ( fTabs * relVelT.getNormalizedOrZero() );
+ const Vec3 fT ( fTabs * relVelT.getNormalizedIfNotZero() );
// Add normal force at contact point
addForceAtWFPosAtomic( p_idx1, ac, fN, gpos );
diff --git a/python/mesa_pd/templates/kernel/SpringDashpotSpring.templ.h b/python/mesa_pd/templates/kernel/SpringDashpotSpring.templ.h
index 873dc7106..ed2102d15 100644
--- a/python/mesa_pd/templates/kernel/SpringDashpotSpring.templ.h
+++ b/python/mesa_pd/templates/kernel/SpringDashpotSpring.templ.h
@@ -158,7 +158,7 @@ inline void SpringDashpotSpring::operator()(const size_t p_idx1,
const Vec3 relVel ( -(getVelocityAtWFPoint(p_idx1, ac, contactPoint) - getVelocityAtWFPoint(p_idx2, ac, contactPoint)) );
const real_t relVelN( math::dot(relVel, contactNormal) );
const Vec3 relVelT( relVel - ( relVelN * contactNormal ) );
- const Vec3 contactTangent = relVelT.getNormalizedOrZero();
+ const Vec3 contactTangent = relVelT.getNormalizedIfNotZero();
// Calculating the normal force based on a linear spring-dashpot force model
real_t fNabs = getStiffnessN(ac.getType(p_idx1), ac.getType(p_idx2)) * delta + getDampingN(ac.getType(p_idx1), ac.getType(p_idx2)) * relVelN;
@@ -180,7 +180,7 @@ inline void SpringDashpotSpring::operator()(const size_t p_idx1,
const auto maxTangentialForce = fNabs * getCoefficientOfFriction(ac.getType(p_idx1), ac.getType(p_idx2));
Vec3 fT = getStiffnessT(ac.getType(p_idx1), ac.getType(p_idx2)) * tangentialDisplacement;
if (length(fT) > maxTangentialForce)
- fT = maxTangentialForce * fT.getNormalizedOrZero();
+ fT = maxTangentialForce * fT.getNormalizedIfNotZero();
// store new tangential displacements
auto& ch1 = ac.getNewContactHistoryRef(p_idx1)[ac.getUid(p_idx2)];
diff --git a/src/core/math/Vector3.h b/src/core/math/Vector3.h
index f377bec15..3437faea5 100644
--- a/src/core/math/Vector3.h
+++ b/src/core/math/Vector3.h
@@ -165,6 +165,7 @@ public:
inline Type sqrLength() const;
inline Vector3<Length> getNormalized() const;
inline Vector3<Length> getNormalizedOrZero() const;
+ inline Vector3<Length> getNormalizedIfNotZero() const;
inline void reset();
inline Type* data() {return v_;}
inline Type const * data() const {return v_;}
@@ -858,23 +859,48 @@ Vector3<typename Vector3<Type>::Length> Vector3<Type>::getNormalized() const
//**********************************************************************************************************************
//**********************************************************************************************************************
-/*!\fn Vector3<Length>& Vector3<Type>::getNormalized() const
+/*!\fn Vector3<Length>& Vector3<Type>::getNormalizedOrZero() const
// \brief Calculation of the normalized vector (\f$|\vec{a}|=1\f$) without precondition.
//
-// \return The normalized vector or the original vector if vector is too small.
+// \return The normalized vector or a vector with all components equal to zero if vector is too small.
*/
template< typename Type >
Vector3<typename Vector3<Type>::Length> Vector3<Type>::getNormalizedOrZero() const
{
const Length len( length() );
+ if (floatIsEqual( len, Length(0) )) { return Vector3<Length>( static_cast<Length>( 0 ) ); }
+
+ const Length ilen( Length(1) / len );
+
+ const Vector3<Length> result ( static_cast<Length>( v_[0] ) * ilen,
+ static_cast<Length>( v_[1] ) * ilen,
+ static_cast<Length>( v_[2] ) * ilen );
+
+ WALBERLA_ASSERT_FLOAT_EQUAL( result.sqrLength(), 1.0, "initial vector: " << result );
+
+ return result;
+}
+//**********************************************************************************************************************
+
+//**********************************************************************************************************************
+/*!\fn Vector3<Length>& Vector3<Type>::getNormalizedIfNotZero() const
+// \brief Calculation of the normalized vector (\f$|\vec{a}|=1\f$) without precondition.
+//
+// \return The normalized vector or the original vector if vector is too small.
+*/
+template< typename Type >
+Vector3<typename Vector3<Type>::Length> Vector3<Type>::getNormalizedIfNotZero() const
+{
+ const Length len( length() );
+
if (floatIsEqual( len, Length(0) )) return *this;
const Length ilen( Length(1) / len );
- Vector3<Length> result ( static_cast<Length>( v_[0] ) * ilen,
- static_cast<Length>( v_[1] ) * ilen,
- static_cast<Length>( v_[2] ) * ilen );
+ const Vector3<Length> result ( static_cast<Length>( v_[0] ) * ilen,
+ static_cast<Length>( v_[1] ) * ilen,
+ static_cast<Length>( v_[2] ) * ilen );
WALBERLA_ASSERT_FLOAT_EQUAL( result.sqrLength(), 1.0, "initial vector: " << result );
diff --git a/src/mesa_pd/collision_detection/AnalyticCollisionFunctions.h b/src/mesa_pd/collision_detection/AnalyticCollisionFunctions.h
index b09f646ed..b413e8bc3 100644
--- a/src/mesa_pd/collision_detection/AnalyticCollisionFunctions.h
+++ b/src/mesa_pd/collision_detection/AnalyticCollisionFunctions.h
@@ -130,7 +130,7 @@ bool detectSphereBoxCollision( const Vec3& pos1,
if( dist < contactThreshold )
{
contactPoint = pos2+q;
- contactNormal = n.getNormalizedOrZero();
+ contactNormal = n.getNormalizedIfNotZero();
if (contactNormal.sqrLength() < real_t(0.1))
{
contactNormal = Vec3(1,0,0);
diff --git a/src/mesa_pd/collision_detection/EPA.cpp b/src/mesa_pd/collision_detection/EPA.cpp
index 033559150..70c39daee 100644
--- a/src/mesa_pd/collision_detection/EPA.cpp
+++ b/src/mesa_pd/collision_detection/EPA.cpp
@@ -306,9 +306,9 @@ bool EPA::doEPA( Support &geom1,
break;
}
- normal = current->getNormal().getNormalizedOrZero();
+ normal = current->getNormal().getNormalizedIfNotZero();
}else{
- normal = current->getClosest().getNormalizedOrZero();
+ normal = current->getClosest().getNormalizedIfNotZero();
}
//std::cerr << "Current Closest: " << current->getClosest();
//std::cerr << "New support direction: " << normal << std::endl;
@@ -472,7 +472,7 @@ bool EPA::doEPA( Support &geom1,
} while (!entryHeap.empty() && entryHeap[0]->getSqrDist() <= upperBoundSqr);
//Normal must be inverted
- retNormal = -current->getClosest().getNormalizedOrZero();
+ retNormal = -current->getClosest().getNormalizedIfNotZero();
//Calculate Witness points
const Vec3 wittnessA = current->getClosestPoint(supportA);
@@ -543,11 +543,11 @@ inline void EPA::createInitialSimplex( size_t numPoints,
axis = Vec3(real_t(0.0), real_t(0.0), real_t(1.0));
}
- Vec3 direction1 = (d % axis).getNormalizedOrZero();
+ Vec3 direction1 = (d % axis).getNormalizedIfNotZero();
Quat q(d, (real_t(2.0)/real_t(3.0)) * real_t(walberla::math::pi));
Mat3 rot = q.toRotationMatrix();
- Vec3 direction2 = (rot*direction1).getNormalizedOrZero();
- Vec3 direction3 = (rot*direction2).getNormalizedOrZero();
+ Vec3 direction2 = (rot*direction1).getNormalizedIfNotZero();
+ Vec3 direction3 = (rot*direction2).getNormalizedIfNotZero();
//add point in positive normal direction1
pushSupportMargin(geom1, geom2, direction1, margin, epaVolume, supportA, supportB);
@@ -620,7 +620,7 @@ inline void EPA::createInitialSimplex( size_t numPoints,
const Vec3 AB = B-A; //The vector A->B
const Vec3 AC = C-A; //The vector A->C
- const Vec3 ABC = (AB%AC).getNormalizedOrZero(); //The the normal pointing towards the viewer if he sees a CCW triangle ABC
+ const Vec3 ABC = (AB%AC).getNormalizedIfNotZero(); //The the normal pointing towards the viewer if he sees a CCW triangle ABC
//add point in positive normal direction
pushSupportMargin(geom1, geom2, ABC, margin, epaVolume, supportA, supportB);
@@ -850,7 +850,7 @@ inline bool EPA::searchTetrahedron(Support &geom1,
/*std::cerr << "Search Direction is: "<< newSearchDirection << std::endl;
std::cerr << "Projection of unnecc. point " << pointIndexToRemove << ": " << epaVolume[pointIndexToRemove] * newSearchDirection << std::endl;
std::cerr << "Projection of other points: " << epaVolume[(pointIndexToRemove+1)%4] * newSearchDirection << std::endl;*/
- newSearchDirection = newSearchDirection.getNormalizedOrZero();
+ newSearchDirection = newSearchDirection.getNormalizedIfNotZero();
/*supportA[pointIndexToRemove] = geom1.supportContactThreshold(newSearchDirection);
supportB[pointIndexToRemove] = geom2.supportContactThreshold(-newSearchDirection);
epaVolume[pointIndexToRemove] = supportA[pointIndexToRemove] - supportB[pointIndexToRemove];*/
diff --git a/src/mesa_pd/collision_detection/EPA.h b/src/mesa_pd/collision_detection/EPA.h
index b61b53e36..73e9fcb22 100644
--- a/src/mesa_pd/collision_detection/EPA.h
+++ b/src/mesa_pd/collision_detection/EPA.h
@@ -409,7 +409,7 @@ inline void EPA::pushSupportMargin(const Support &geom1,
{
Vec3 ndir;
if(floatIsEqual(dir.sqrLength(), real_t(1.0))){
- ndir = dir.getNormalizedOrZero();
+ ndir = dir.getNormalizedIfNotZero();
}else{
ndir = dir;
}
@@ -443,7 +443,7 @@ inline void EPA::replaceSupportMargin(const Support &geom1,
{
Vec3 ndir;
if(floatIsEqual(dir.sqrLength(), real_t(1.0))){
- ndir = dir.getNormalizedOrZero();
+ ndir = dir.getNormalizedIfNotZero();
}else{
ndir = dir;
}
diff --git a/src/mesa_pd/kernel/LinearSpringDashpot.h b/src/mesa_pd/kernel/LinearSpringDashpot.h
index 3a57f56c0..b4b35082c 100644
--- a/src/mesa_pd/kernel/LinearSpringDashpot.h
+++ b/src/mesa_pd/kernel/LinearSpringDashpot.h
@@ -306,7 +306,7 @@ inline void LinearSpringDashpot::operator()(const size_t p_idx1,
Vec3 fTLS = stiffnessT * newTangentialSpringDisplacement +
dampingT * relVelT;
- const Vec3 t = fTLS.getNormalizedOrZero(); // tangential unit vector
+ const Vec3 t = fTLS.getNormalizedIfNotZero(); // tangential unit vector
// calculate friction force
const real_t fFrictionAbsStatic = getFrictionCoefficientStatic(ac.getType(p_idx1), ac.getType(p_idx2)) * fN.length(); // sticking, rolling
diff --git a/src/mesa_pd/kernel/NonLinearSpringDashpot.h b/src/mesa_pd/kernel/NonLinearSpringDashpot.h
index 6486e0ec5..b5d732c6d 100644
--- a/src/mesa_pd/kernel/NonLinearSpringDashpot.h
+++ b/src/mesa_pd/kernel/NonLinearSpringDashpot.h
@@ -311,7 +311,7 @@ inline void NonLinearSpringDashpot::operator()(const size_t p_idx1,
Vec3 fTLS = stiffnessT * newTangentialSpringDisplacement +
dampingT * relVelT;
- const Vec3 t = fTLS.getNormalizedOrZero(); // tangential unit vector
+ const Vec3 t = fTLS.getNormalizedIfNotZero(); // tangential unit vector
// calculate friction force
const real_t fFrictionAbsStatic = getFrictionCoefficientStatic(ac.getType(p_idx1), ac.getType(p_idx2)) * fN.length(); // sticking, rolling
diff --git a/src/mesa_pd/kernel/SpringDashpot.h b/src/mesa_pd/kernel/SpringDashpot.h
index 1388e85c8..7c59af5dc 100644
--- a/src/mesa_pd/kernel/SpringDashpot.h
+++ b/src/mesa_pd/kernel/SpringDashpot.h
@@ -238,7 +238,7 @@ inline void SpringDashpot::operator()(const size_t p_idx1,
// Calculating the tangential force based on the model by Haff and Werner
const real_t fTabs( std::min( getDampingT(ac.getType(p_idx1), ac.getType(p_idx2)) * relVelT.length(), getFriction(ac.getType(p_idx1), ac.getType(p_idx2)) * fNabs ) );
- const Vec3 fT ( fTabs * relVelT.getNormalizedOrZero() );
+ const Vec3 fT ( fTabs * relVelT.getNormalizedIfNotZero() );
// Add normal force at contact point
addForceAtWFPosAtomic( p_idx1, ac, fN, gpos );
diff --git a/src/mesa_pd/kernel/SpringDashpotSpring.h b/src/mesa_pd/kernel/SpringDashpotSpring.h
index 0fbd2e684..07eed8152 100644
--- a/src/mesa_pd/kernel/SpringDashpotSpring.h
+++ b/src/mesa_pd/kernel/SpringDashpotSpring.h
@@ -215,7 +215,7 @@ inline void SpringDashpotSpring::operator()(const size_t p_idx1,
const Vec3 relVel ( -(getVelocityAtWFPoint(p_idx1, ac, contactPoint) - getVelocityAtWFPoint(p_idx2, ac, contactPoint)) );
const real_t relVelN( math::dot(relVel, contactNormal) );
const Vec3 relVelT( relVel - ( relVelN * contactNormal ) );
- const Vec3 contactTangent = relVelT.getNormalizedOrZero();
+ const Vec3 contactTangent = relVelT.getNormalizedIfNotZero();
// Calculating the normal force based on a linear spring-dashpot force model
real_t fNabs = getStiffnessN(ac.getType(p_idx1), ac.getType(p_idx2)) * delta + getDampingN(ac.getType(p_idx1), ac.getType(p_idx2)) * relVelN;
@@ -237,7 +237,7 @@ inline void SpringDashpotSpring::operator()(const size_t p_idx1,
const auto maxTangentialForce = fNabs * getCoefficientOfFriction(ac.getType(p_idx1), ac.getType(p_idx2));
Vec3 fT = getStiffnessT(ac.getType(p_idx1), ac.getType(p_idx2)) * tangentialDisplacement;
if (length(fT) > maxTangentialForce)
- fT = maxTangentialForce * fT.getNormalizedOrZero();
+ fT = maxTangentialForce * fT.getNormalizedIfNotZero();
// store new tangential displacements
auto& ch1 = ac.getNewContactHistoryRef(p_idx1)[ac.getUid(p_idx2)];
diff --git a/src/pe/cr/ContactResolvers.h b/src/pe/cr/ContactResolvers.h
index f0b328974..1af217b27 100644
--- a/src/pe/cr/ContactResolvers.h
+++ b/src/pe/cr/ContactResolvers.h
@@ -62,7 +62,7 @@ public:
// Calculating the tangential force based on the model by Haff and Werner
const real_t fTabs( std::min( getDampingT(c) * relVelT.length(), getFriction(c) * fNabs ) );
- const Vec3 fT ( fTabs * relVelT.getNormalizedOrZero() );
+ const Vec3 fT ( fTabs * relVelT.getNormalizedIfNotZero() );
// Add normal force at contact point
b1->addForceAtPos( fN, gpos );
--
GitLab