1 // ============================================================================= 2 // PROJECT CHRONO - http://projectchrono.org 3 // 4 // Copyright (c) 2014 projectchrono.org 5 // All rights reserved. 6 // 7 // Use of this source code is governed by a BSD-style license that can be found 8 // in the LICENSE file at the top level of the distribution and at 9 // http://projectchrono.org/license-chrono.txt. 10 // 11 // ============================================================================= 12 13 #ifndef CHCONSTRAINTTWOTUPLESCONTACTN_H 14 #define CHCONSTRAINTTWOTUPLESCONTACTN_H 15 16 #include "chrono/solver/ChConstraintTwoTuplesFrictionT.h" 17 18 namespace chrono { 19 20 class ChApi ChConstraintTwoTuplesContactNall { 21 public: 22 /// Get the friction coefficient GetFrictionCoefficient()23 double GetFrictionCoefficient() const { return friction; } 24 /// Set the friction coefficient SetFrictionCoefficient(double mcoeff)25 void SetFrictionCoefficient(double mcoeff) { friction = mcoeff; } 26 27 /// Get the cohesion GetCohesion()28 double GetCohesion() const { return cohesion; } 29 /// Set the cohesion SetCohesion(double mcoh)30 void SetCohesion(double mcoh) { cohesion = mcoh; } 31 32 protected: 33 double friction; ///< friction coefficient 'f', for sqrt(Tx^2+Ty^2)<f*Nz 34 double cohesion; ///< cohesion 'c', non-negative, for sqrt(Tx^2+Ty^2)<f*(Nz+c) 35 }; 36 37 /// This class is inherited from the ChConstraintTwoTuples, 38 /// It is used to represent the normal reaction between two objects, 39 /// each represented by a tuple of ChVariables objects, 40 /// ONLY when also two ChConstraintTwoTuplesFrictionT objects are 41 /// used to represent friction. (If these two tangent constraint 42 /// are not used, for frictionless case, please use a simple ChConstraintTwo 43 /// with the CONSTRAINT_UNILATERAL mode.) 44 /// Differently from an unilateral constraint, this does not enforce 45 /// projection on positive constraint, since it will be up to the 'companion' 46 /// ChConstraintTwoTuplesFrictionT objects to call a projection on the cone, by 47 /// modifying all the three components (normal, u, v) at once. 48 /// Templates Ta and Tb are of ChVariableTupleCarrier_Nvars classes 49 50 template <class Ta, class Tb> 51 class ChApi ChConstraintTwoTuplesContactN : public ChConstraintTwoTuples<Ta, Tb>, 52 public ChConstraintTwoTuplesContactNall { 53 protected: 54 ChConstraintTwoTuplesFrictionT<Ta, Tb>* constraint_U; ///< U tangential component 55 ChConstraintTwoTuplesFrictionT<Ta, Tb>* constraint_V; ///< V tangential component 56 57 public: 58 /// Default constructor ChConstraintTwoTuplesContactN()59 ChConstraintTwoTuplesContactN() { 60 this->mode = CONSTRAINT_FRIC; 61 friction = 0.0; 62 cohesion = 0.0; 63 constraint_U = constraint_V = 0; 64 } 65 66 /// Copy constructor ChConstraintTwoTuplesContactN(const ChConstraintTwoTuplesContactN & other)67 ChConstraintTwoTuplesContactN(const ChConstraintTwoTuplesContactN& other) : ChConstraintTwoTuples<Ta, Tb>(other) { 68 friction = other.friction; 69 cohesion = other.cohesion; 70 constraint_U = other.constraint_U; 71 constraint_V = other.constraint_V; 72 } 73 ~ChConstraintTwoTuplesContactN()74 virtual ~ChConstraintTwoTuplesContactN() {} 75 76 /// "Virtual" copy constructor (covariant return type). Clone()77 virtual ChConstraintTwoTuplesContactN* Clone() const override { return new ChConstraintTwoTuplesContactN(*this); } 78 79 /// Assignment operator: copy from other object 80 ChConstraintTwoTuplesContactN& operator=(const ChConstraintTwoTuplesContactN& other) { 81 if (&other == this) 82 return *this; 83 // copy parent class data 84 ChConstraintTwoTuples<Ta, Tb>::operator=(other); 85 86 friction = other.friction; 87 cohesion = other.cohesion; 88 constraint_U = other.constraint_U; 89 constraint_V = other.constraint_V; 90 return *this; 91 } 92 93 /// Get pointer to U tangential component GetTangentialConstraintU()94 ChConstraintTwoTuplesFrictionT<Ta, Tb>* GetTangentialConstraintU() const { return constraint_U; } 95 /// Get pointer to V tangential component GetTangentialConstraintV()96 ChConstraintTwoTuplesFrictionT<Ta, Tb>* GetTangentialConstraintV() const { return constraint_V; } 97 98 /// Set pointer to U tangential component SetTangentialConstraintU(ChConstraintTwoTuplesFrictionT<Ta,Tb> * mconstr)99 void SetTangentialConstraintU(ChConstraintTwoTuplesFrictionT<Ta, Tb>* mconstr) { constraint_U = mconstr; } 100 /// Set pointer to V tangential component SetTangentialConstraintV(ChConstraintTwoTuplesFrictionT<Ta,Tb> * mconstr)101 void SetTangentialConstraintV(ChConstraintTwoTuplesFrictionT<Ta, Tb>* mconstr) { constraint_V = mconstr; } 102 103 /// Project the value of a possible 'l_i' value of constraint reaction onto admissible set. 104 /// This will also modify the l_i values of the two tangential friction constraints 105 /// (projection onto the friction cone, as by Anitescu-Tasora theory). Project()106 virtual void Project() override { 107 if (!constraint_U || !constraint_V) 108 return; 109 110 // Anitescu-Tasora projection on cone generator and polar cone 111 // (contractive, but performs correction on three components: normal,u,v) 112 113 double f_n = this->l_i + this->cohesion; 114 115 // no friction? project to axis of upper cone 116 if (friction == 0) { 117 constraint_U->Set_l_i(0); 118 constraint_V->Set_l_i(0); 119 if (f_n < 0) 120 this->Set_l_i(0); 121 return; 122 } 123 124 double f_u = constraint_U->Get_l_i(); 125 double f_v = constraint_V->Get_l_i(); 126 127 double mu2 = friction * friction; 128 double f_n2 = f_n * f_n; 129 double f_t2 = (f_v * f_v + f_u * f_u); 130 131 // inside lower cone or close to origin? reset normal, u, v to zero! 132 if ((f_n <= 0 && f_t2 < f_n2 / mu2) || (f_n < 1e-14 && f_n > -1e-14)) { 133 this->Set_l_i(0); 134 constraint_U->Set_l_i(0); 135 constraint_V->Set_l_i(0); 136 return; 137 } 138 139 // inside upper cone? keep untouched! 140 if (f_t2 < f_n2 * mu2) 141 return; 142 143 // project orthogonally to generator segment of upper cone 144 double f_t = sqrt(f_t2); 145 double f_n_proj = (f_t * friction + f_n) / (mu2 + 1); 146 double f_t_proj = f_n_proj * friction; 147 double tproj_div_t = f_t_proj / f_t; 148 double f_u_proj = tproj_div_t * f_u; 149 double f_v_proj = tproj_div_t * f_v; 150 151 this->Set_l_i(f_n_proj - this->cohesion); 152 constraint_U->Set_l_i(f_u_proj); 153 constraint_V->Set_l_i(f_v_proj); 154 } 155 }; 156 157 } // end namespace chrono 158 159 #endif 160