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