Commit 90c4bdf4 authored by Christoph Schwarzmeier's avatar Christoph Schwarzmeier
Browse files

Add Vector3::getNormalizedIfNotZero() and correct Vector3::getNormalizedOrZero()

parent e825e0a6
......@@ -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
......
......@@ -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
......
......@@ -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 );
......
......@@ -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)];
......
......@@ -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 );
......
......@@ -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);
......
......@@ -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];*/
......
......@@ -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;
}
......
......@@ -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
......
......@@ -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
......
......@@ -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 );
......
......@@ -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)];
......
......@@ -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 );
......
Markdown is supported
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment