diff --git a/python/mesa_pd/templates/kernel/LinearSpringDashpot.templ.h b/python/mesa_pd/templates/kernel/LinearSpringDashpot.templ.h
index 2696e0f3430c92e8c9c3a337a4cbadb775b0ecaf..efa4138873bc7c70f41b0ee02a4250fa418e6753 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 3cc2fbd7f932392c7dfd289d98e31ac51b8fcc80..8eda92ecf5e9ea9cec31dd04ae7e44e14eab031d 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 29c8a10767cb4c22269cba173887145c1d64b496..bec9eb793a849b0a5c68b7fa94f93947b4af8ab0 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 873dc71062f35daeb1c79630b8487189ce0c687c..ed2102d1572fcfd7137b33b59878b119341210eb 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 f377bec156bae9829f08624bd9a201ade8501b61..3437faea54db1927e67ce6e7773df4428bb4db88 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 b09f646ed3201990543ace423cadfd1a650b341c..b413e8bc3a78c631cb261c224b15d72f3c115d70 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 0335591507b111e612a4d5d561eac760ab7bed66..70c39daeea9ff9c2686df4a9043255c6be127cea 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 b61b53e36af22cd8b2f864a3ff77ee9235df82e2..73e9fcb22f3aaa12a6bfc4f324cd2f206dda2768 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 3a57f56c0aabae532ada51e9e73873bf60360958..b4b35082cba39c5884b757a722baa494a5ad6750 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 6486e0ec522d83349759c3ece70f9b9d58ba13f0..b5d732c6d2b27ea535c62404baa3b75d81ace8a6 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 1388e85c86f4aecd1c78fbd82ecfe00596c3e5c0..7c59af5dcd977c56ff937a5f0aeb46e5f7f557ba 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 0fbd2e684a2722abde85312be083eea7df5d6a12..07eed81524cb9712b3ee027886947b28c973e0ce 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 f0b328974b6ca0b08b7b92624b275df93ef1e6fe..1af217b27f93dae682d2382969823c7857c0ebee 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 );