 |
MechSys
1.0
Computing library for simulations in continuum and discrete mechanics
|
|
MechSys
1.0
Computing library for simulations in continuum and discrete mechanics
|
00001 /************************************************************************ 00002 * MechSys - Open Library for Mechanical Systems * 00003 * Copyright (C) 2005 Dorival M. Pedroso, Raul Durand * 00004 * Copyright (C) 2009 Sergio Galindo * 00005 * * 00006 * This program is free software: you can redistribute it and/or modify * 00007 * it under the terms of the GNU General Public License as published by * 00008 * the Free Software Foundation, either version 3 of the License, or * 00009 * any later version. * 00010 * * 00011 * This program is distributed in the hope that it will be useful, * 00012 * but WITHOUT ANY WARRANTY; without even the implied warranty of * 00013 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the * 00014 * GNU General Public License for more details. * 00015 * * 00016 * You should have received a copy of the GNU General Public License * 00017 * along with this program. If not, see <http://www.gnu.org/licenses/> * 00018 ************************************************************************/ 00019 00020 #ifndef MECHSYS_DEM_INTERACTON_H 00021 #define MECHSYS_DEM_INTERACTON_H 00022 00023 // Std Lib 00024 #include <math.h> 00025 #include <map> 00026 #include <vector> 00027 #include <utility> 00028 00029 // MechSys 00030 #include <mechsys/dem/particle.h> 00031 00032 // typedefs 00033 typedef std::map<std::pair<int,int>,Vec3_t> FrictionMap_t; 00034 typedef Array<pair<int,int> > ListContacts_t; 00035 00036 class Interacton //General class for interactons 00037 { 00038 public: 00039 00040 // Constructor and destructor 00041 Interacton () {} 00042 virtual ~Interacton () {} 00043 00044 // Methods 00045 virtual bool UpdateContacts (double alpha) =0; 00046 virtual void CalcForce (double dt = 0.0) =0; 00047 virtual void UpdateParameters () =0; 00048 00049 // Data 00050 Particle * P1; 00051 Particle * P2; 00052 size_t I1; 00053 size_t I2; 00054 }; 00055 00056 class CInteracton: public Interacton // Interacton for collision 00057 { 00058 public: 00059 // Constructor 00060 CInteracton (Particle * Pt1, Particle * Pt2); 00061 CInteracton () {}; 00062 00063 // Methods 00064 virtual bool UpdateContacts (double alpha); 00065 virtual void CalcForce (double dt = 0.0); 00066 virtual void UpdateParameters (); 00067 00068 // Data 00069 double Kn; 00070 double Kt; 00071 double Gn; 00072 double Gt; 00073 double Mu; 00074 double Epot; 00075 double dEvis; 00076 double dEfric; 00077 size_t Nc; 00078 size_t Nsc; 00079 size_t Nr; 00080 Vec3_t Fn; 00081 Vec3_t Fnet; 00082 Vec3_t Ftnet; 00083 Vec3_t Xc; 00084 Mat3_t B; 00085 ListContacts_t Lee; 00086 ListContacts_t Lvf; 00087 ListContacts_t Lfv; 00088 ListContacts_t Lvt; 00089 ListContacts_t Ltv; 00090 ListContacts_t Lvc; 00091 ListContacts_t Lcv; 00092 FrictionMap_t Fdee; 00093 FrictionMap_t Fdvf; 00094 FrictionMap_t Fdfv; 00095 FrictionMap_t Fdvt; 00096 FrictionMap_t Fdtv; 00097 FrictionMap_t Fdvc; 00098 FrictionMap_t Fdcv; 00099 protected: 00100 template<typename FeatureA_T, typename FeatureB_T> 00101 void _update_disp_calc_force (FeatureA_T & A, FeatureB_T & B, FrictionMap_t & FMap, ListContacts_t & L, double dt); 00102 template<typename FeatureA_T, typename FeatureB_T> 00103 void _update_contacts (FeatureA_T & A, FeatureB_T & B, ListContacts_t & L, double alpha); 00104 }; 00105 00106 class CInteractonSphere: public CInteracton // Collision interacton for spheres 00107 { 00108 public: 00109 // Methods 00110 CInteractonSphere (Particle * Pt1, Particle * Pt2); 00111 bool UpdateContacts (double alpha); 00112 void CalcForce (double dt = 0.0); 00113 void UpdateParameters (); 00114 00115 // Data 00116 FrictionMap_t Fdvv; 00117 ListContacts_t Lvv; 00118 Vec3_t Fdr; 00119 double beta; 00120 double eta; 00121 00122 protected: 00123 void _update_rolling_resistance(double dt); 00124 }; 00125 00126 class BInteracton: public Interacton // Interacton for cohesion 00127 { 00128 public: 00129 BInteracton (Particle * Pt1, Particle * Pt2, size_t Fi1, size_t Fi2); 00130 00131 // Methods 00132 bool UpdateContacts (double alpha); 00133 void CalcForce (double dt = 0.0); 00134 void UpdateParameters (); 00135 00136 // Data 00137 size_t F1; 00138 size_t F2; 00139 double Area; 00140 double Bn; 00141 double Bt; 00142 double Bm; 00143 double Gn; 00144 double Gt; 00145 double L0; 00146 double An; 00147 double eps; 00148 bool valid; 00149 double s1,t1; 00150 double s2,t2; 00151 Vec3_t Fnet; 00152 00153 }; 00154 00156 00157 // Collision interacton 00158 00159 inline CInteracton::CInteracton (Particle * Pt1, Particle * Pt2) 00160 { 00161 P1 = Pt1; 00162 P2 = Pt2; 00163 I1 = P1->Index; 00164 I2 = P2->Index; 00165 double r1 = pow(P1->Props.V,1.0/3.0); 00166 double r2 = pow(P2->Props.V,1.0/3.0); 00167 Kn = (r1+r2)*ReducedValue(Pt1->Props.Kn,Pt2->Props.Kn); 00168 Kt = (r1+r2)*ReducedValue(Pt1->Props.Kt,Pt2->Props.Kt); 00169 Gn = 2*ReducedValue(Pt1->Props.Gn,Pt2->Props.Gn)*ReducedValue(Pt1->Props.m,Pt2->Props.m); 00170 Gt = 2*ReducedValue(Pt1->Props.Gt,Pt2->Props.Gt)*ReducedValue(Pt1->Props.m,Pt2->Props.m); 00171 Mu = 2*ReducedValue(Pt1->Props.Mu,Pt2->Props.Mu); 00172 CalcForce(0.0); 00173 } 00174 00175 inline bool CInteracton::UpdateContacts (double alpha) 00176 { 00177 Lee.Resize(0); 00178 Lvf.Resize(0); 00179 Lfv.Resize(0); 00180 Lvt.Resize(0); 00181 Ltv.Resize(0); 00182 Lvc.Resize(0); 00183 Lcv.Resize(0); 00184 if (Distance(P1->x,P2->x)<=P1->Dmax+P2->Dmax+2*alpha) 00185 { 00186 _update_contacts (P1->Edges ,P2->Edges ,Lee,alpha); 00187 _update_contacts (P1->Verts ,P2->Faces ,Lvf,alpha); 00188 _update_contacts (P1->Faces ,P2->Verts ,Lfv,alpha); 00189 _update_contacts (P1->Verts ,P2->Tori ,Lvt,alpha); 00190 _update_contacts (P1->Tori ,P2->Verts ,Ltv,alpha); 00191 _update_contacts (P1->Verts ,P2->Cylinders ,Lvc,alpha); 00192 _update_contacts (P1->Cylinders ,P2->Verts ,Lcv,alpha); 00193 if (Lee.Size()>0||Lvf.Size()>0||Lfv.Size()>0||Ltv.Size()>0||Lvt.Size()>0||Lcv.Size()>0||Lvc.Size()>0) return true; 00194 else return false; 00195 } 00196 else return false; 00197 } 00198 00199 inline void CInteracton::CalcForce (double dt) 00200 { 00201 Epot = 0.0; 00202 dEvis = 0.0; 00203 dEfric = 0.0; 00204 Nc = 0.0; 00205 Nsc = 0.0; 00206 Nr = 0.0; 00207 Fnet = 0.0; 00208 Ftnet = 0.0; 00209 Xc = 0.0; 00210 _update_disp_calc_force (P1->Edges ,P2->Edges ,Fdee,Lee,dt); 00211 _update_disp_calc_force (P1->Verts ,P2->Faces ,Fdvf,Lvf,dt); 00212 _update_disp_calc_force (P1->Faces ,P2->Verts ,Fdfv,Lfv,dt); 00213 _update_disp_calc_force (P1->Verts ,P2->Tori ,Fdvt,Lvt,dt); 00214 _update_disp_calc_force (P1->Tori ,P2->Verts ,Fdtv,Ltv,dt); 00215 _update_disp_calc_force (P1->Verts ,P2->Cylinders ,Fdvc,Lvc,dt); 00216 _update_disp_calc_force (P1->Cylinders ,P2->Verts ,Fdcv,Lcv,dt); 00217 00218 //If there is at least a contact, increase the coordination number of the particles 00219 if (Nc>0) 00220 { 00221 P1->Cn++; 00222 P2->Cn++; 00223 } 00224 } 00225 00226 inline void CInteracton::UpdateParameters () 00227 { 00228 Kn = 2*ReducedValue(P1->Props.Kn,P2->Props.Kn); 00229 Kt = 2*ReducedValue(P1->Props.Kt,P2->Props.Kt); 00230 Gn = 2*ReducedValue(P1->Props.Gn,P2->Props.Gn)*ReducedValue(P1->Props.m,P2->Props.m); 00231 Gt = 2*ReducedValue(P1->Props.Gt,P2->Props.Gt)*ReducedValue(P1->Props.m,P2->Props.m); 00232 Mu = 2*ReducedValue(P1->Props.Mu,P2->Props.Mu); 00233 } 00234 00235 template<typename FeatureA_T, typename FeatureB_T> 00236 inline void CInteracton::_update_disp_calc_force (FeatureA_T & A, FeatureB_T & B, FrictionMap_t & FMap, ListContacts_t & L, double dt) 00237 { 00238 // update 00239 for (size_t k=0; k<L.Size(); ++k) 00240 { 00241 size_t i = L[k].first; 00242 size_t j = L[k].second; 00243 Vec3_t xi, xf; 00244 Distance ((*A[i]), (*B[j]), xi, xf); 00245 double dist = norm(xf-xi); 00246 double delta = P1->Props.R + P2->Props.R - dist; 00247 if (delta>0) 00248 { 00249 if (delta > 0.8*(P1->Props.R+P2->Props.R)) throw new Fatal("Interacton::_update_disp_calc_force: Maximun overlap detected between particles %d(%d) and %d(%d)",P1->Index,P1->Tag,P2->Index,P2->Tag); 00250 00251 // Count a contact 00252 Nc++; 00253 00254 // update force 00255 Vec3_t n = (xf-xi)/dist; 00256 Vec3_t x = xi+n*((P1->Props.R*P1->Props.R-P2->Props.R*P2->Props.R+dist*dist)/(2*dist)); 00257 Xc += x; 00258 Vec3_t t1,t2,x1,x2; 00259 Rotation(P1->w,P1->Q,t1); 00260 Rotation(P2->w,P2->Q,t2); 00261 x1 = x - P1->x; 00262 x2 = x - P2->x; 00263 Vec3_t vrel = -((P2->v-P1->v)+cross(t2,x2)-cross(t1,x1)); 00264 Vec3_t vt = vrel - dot(n,vrel)*n; 00265 Fn = Kn*delta*n; 00266 Fnet += Fn; 00267 pair<int,int> p; 00268 p = make_pair(i,j); 00269 if (FMap.count(p)==0) FMap[p] = 0.0,0.0,0.0; 00270 FMap[p] += vt*dt; 00271 FMap[p] -= dot(FMap[p],n)*n; 00272 Vec3_t tan = FMap[p]; 00273 if (norm(tan)>0.0) tan/=norm(tan); 00274 if (norm(FMap[p])>Mu*norm(Fn)/Kt) 00275 { 00276 // Count a sliding contact 00277 Nsc++; 00278 FMap[p] = Mu*norm(Fn)/Kt*tan; 00279 dEfric += Kt*dot(FMap[p],vt)*dt; 00280 } 00281 Ftnet += Kt*FMap[p]; 00282 Vec3_t F = Fn + Kt*FMap[p] + Gn*dot(n,vrel)*n + Gt*vt; 00283 if (dot(F,n)<0) F-=dot(F,n)*n; 00284 #ifdef USE_THREAD 00285 std::lock_guard<std::mutex> lk1(P1->mtex); 00286 std::lock_guard<std::mutex> lk2(P2->mtex); 00287 #endif 00288 P1->F += -F; 00289 P2->F += F; 00290 dEvis += (Gn*dot(vrel-vt,vrel-vt)+Gt*dot(vt,vt))*dt; 00291 // torque 00292 Vec3_t T, Tt; 00293 Tt = cross (x1,F); 00294 Quaternion_t q; 00295 Conjugate (P1->Q,q); 00296 Rotation (Tt,q,T); 00297 P1->T -= T; 00298 Tt = cross (x2,F); 00299 Conjugate (P2->Q,q); 00300 Rotation (Tt,q,T); 00301 P2->T += T; 00302 //Transfering the branch vector information 00303 Vec3_t nor = n; 00304 for (size_t m=0;m<3;m++) 00305 { 00306 for (size_t n=0;n<3;n++) 00307 { 00308 P1->M(m,n) -= F(m)*x1(n); 00309 P2->M(m,n) += F(m)*x2(n); 00310 P1->B(m,n) += nor(m)*nor(n); 00311 P2->B(m,n) += nor(m)*nor(n); 00312 this->B(m,n) = nor(m)*nor(n); 00313 } 00314 } 00315 00316 // potential energy 00317 Epot += 0.5*Kn*delta*delta+0.5*Kt*dot(FMap[p],FMap[p]); 00318 } 00319 } 00320 } 00321 00322 template<typename FeatureA_T, typename FeatureB_T> 00323 inline void CInteracton::_update_contacts (FeatureA_T & A, FeatureB_T & B, ListContacts_t & L, double alpha) 00324 { 00325 for (size_t i=0; i<A.Size(); ++i) 00326 for (size_t j=0; j<B.Size(); ++j) 00327 { 00328 if (Distance ((*A[i]), (*B[j]))<=P1->Props.R+P2->Props.R+2*alpha) 00329 { 00330 pair<int,int> p; 00331 p = make_pair(i,j); 00332 L.Push(p); 00333 } 00334 } 00335 00336 } 00337 00338 //Collision interacton for spheres 00339 00340 inline CInteractonSphere::CInteractonSphere (Particle * Pt1, Particle * Pt2) 00341 { 00342 P1 = Pt1; 00343 P2 = Pt2; 00344 I1 = P1->Index; 00345 I2 = P2->Index; 00346 Kn = 2*ReducedValue(Pt1->Props.Kn,Pt2->Props.Kn); 00347 Kt = 2*ReducedValue(Pt1->Props.Kt,Pt2->Props.Kt); 00348 Gn = 2*ReducedValue(Pt1->Props.Gn,Pt2->Props.Gn)*ReducedValue(Pt1->Props.m,Pt2->Props.m); 00349 Gt = 2*ReducedValue(Pt1->Props.Gt,Pt2->Props.Gt)*ReducedValue(Pt1->Props.m,Pt2->Props.m); 00350 Mu = 2*ReducedValue(Pt1->Props.Mu,Pt2->Props.Mu); 00351 beta = 2*ReducedValue(Pt1->Props.Beta,Pt2->Props.Beta); 00352 eta = 2*ReducedValue(Pt1->Props.Eta,Pt2->Props.Eta); 00353 Nc = 0; 00354 Nsc = 0; 00355 00356 Epot = 0.0; 00357 Fdr = 0.0, 0.0, 0.0; 00358 Lvv.Push(make_pair(0,0)); 00359 00360 CalcForce(0.0); 00361 } 00362 00363 inline void CInteractonSphere::_update_rolling_resistance(double dt) 00364 { 00365 Vec3_t t1,t2; 00366 Rotation(P1->w,P1->Q,t1); 00367 Rotation(P2->w,P2->Q,t2); 00368 Vec3_t Normal = Fn/norm(Fn); 00369 Vec3_t Vr = P1->Props.R*P2->Props.R*cross(Vec3_t(t1 - t2),Normal)/(P1->Props.R+P2->Props.R); 00370 Fdr += Vr*dt; 00371 Fdr -= dot(Fdr,Normal)*Normal; 00372 Vec3_t tan = Fdr; 00373 if (norm(tan)>0.0) tan/=norm(tan); 00374 double Kr = beta*Kt; 00375 if (norm(Fdr)>eta*Mu*norm(Fn)/Kr) 00376 { 00377 Fdr = eta*Mu*norm(Fn)/Kr*tan; 00378 Nr++; 00379 } 00380 Vec3_t Ft = -Kr*Fdr; 00381 00382 Vec3_t Tt = P1->Props.R*cross(Normal,Ft); 00383 Vec3_t T; 00384 #ifdef USE_THREAD 00385 std::lock_guard<std::mutex> lk1(P1->mtex); 00386 std::lock_guard<std::mutex> lk2(P2->mtex); 00387 #endif 00388 Quaternion_t q; 00389 Conjugate (P1->Q,q); 00390 Rotation (Tt,q,T); 00391 P1->T += T; 00392 00393 Tt = P2->Props.R*cross(Normal,Ft); 00394 Conjugate (P2->Q,q); 00395 Rotation (Tt,q,T); 00396 P2->T -= T; 00397 } 00398 00399 inline void CInteractonSphere::CalcForce(double dt) 00400 { 00401 Epot = 0.0; 00402 dEvis = 0.0; 00403 dEfric = 0.0; 00404 Nc = 0.0; 00405 Nsc = 0.0; 00406 Nr = 0.0; 00407 Fnet = 0.0; 00408 Ftnet = 0.0; 00409 Xc = 0.0; 00410 _update_disp_calc_force (P1->Verts,P2->Verts,Fdvv,Lvv,dt); 00411 if (Epot>0.0) _update_rolling_resistance(dt); 00412 00413 00414 //If there is at least a contact, increase the coordination number of the particles 00415 if (Nc>0) 00416 { 00417 P1->Cn++; 00418 P2->Cn++; 00419 } 00420 } 00421 00422 inline bool CInteractonSphere::UpdateContacts (double alpha) 00423 { 00424 if (Distance(P1->x,P2->x)<=P1->Dmax+P2->Dmax+2*alpha) return true; 00425 else return false; 00426 } 00427 00428 inline void CInteractonSphere::UpdateParameters () 00429 { 00430 Kn = 2*ReducedValue(P1->Props.Kn,P2->Props.Kn); 00431 Kt = 2*ReducedValue(P1->Props.Kt,P2->Props.Kt); 00432 Gn = 2*ReducedValue(P1->Props.Gn,P2->Props.Gn)*ReducedValue(P1->Props.m,P2->Props.m); 00433 Gt = 2*ReducedValue(P1->Props.Gt,P2->Props.Gt)*ReducedValue(P1->Props.m,P2->Props.m); 00434 Mu = 2*ReducedValue(P1->Props.Mu,P2->Props.Mu); 00435 beta = 2*ReducedValue(P1->Props.Beta,P2->Props.Beta); 00436 eta = 2*ReducedValue(P1->Props.Eta,P2->Props.Eta); 00437 } 00438 00439 //Cohesion interacton 00440 00441 inline BInteracton::BInteracton (Particle * Pt1, Particle * Pt2, size_t Fi1, size_t Fi2) 00442 { 00443 P1 = Pt1; 00444 P2 = Pt2; 00445 F1 = Fi1; 00446 if (Fi2>=P2->Faces.Size()) 00447 { 00448 F2 = P2->Faces.Size()-1; 00449 Area = P1->Faces[F1]->Area(); 00450 } 00451 else 00452 { 00453 F2 = Fi2; 00454 Area = 0.5*(P1->Faces[F1]->Area()+P2->Faces[F2]->Area()); 00455 } 00456 I1 = P1->Index; 00457 I2 = P2->Index; 00458 Bn = 2*ReducedValue(P1->Props.Bn,P2->Props.Bn)*Area; 00459 Bt = 2*ReducedValue(P1->Props.Bt,P2->Props.Bt)*Area; 00460 Bm = 2*ReducedValue(P1->Props.Bm,P2->Props.Bm)*Area; 00461 Gn = 2*ReducedValue(P1->Props.Gn,P2->Props.Gn)*ReducedValue(P1->Props.m,P2->Props.m); 00462 Gt = 2*ReducedValue(P1->Props.Gt,P2->Props.Gt)*ReducedValue(P1->Props.m,P2->Props.m); 00463 eps = 2*ReducedValue(P1->Props.eps,P2->Props.eps); 00464 00465 Vec3_t n1,n2; 00466 P1->Faces[F1]->Normal(n1); 00467 P2->Faces[F2]->Normal(n2); 00468 Vec3_t c1,c2; 00469 An = 0.0; 00470 valid = true; 00471 P1->Faces[F1]->Centroid(c1); 00472 P2->Faces[F2]->Centroid(c2); 00473 L0 = dot(n1,c2-c1); 00474 if (Fi2>=P2->Faces.Size()) c2 = c1; 00475 Vec3_t V = 0.5*(c1+c2); 00476 00477 Face * F = P1->Faces[F1]; 00478 double a = dot(*F->Edges[0]->X0-V , F->Edges[0]->dL); 00479 double b = dot(F->Edges[0]->dL , F->Edges[0]->dL); 00480 double c = dot(F->Edges[0]->dL , F->Edges[1]->dL); 00481 double d = dot(*F->Edges[0]->X0-V , F->Edges[1]->dL); 00482 double f = dot(F->Edges[1]->dL , F->Edges[1]->dL); 00483 s1 = (c*d-a*f)/(b*f-c*c); 00484 t1 = (a*c-b*d)/(b*f-c*c); 00485 00486 Vec3_t p1 = -s1*F->Edges[0]->dL - t1*F->Edges[1]->dL; 00487 00488 F = P2->Faces[F2]; 00489 a = dot(*F->Edges[0]->X0-V , F->Edges[0]->dL); 00490 b = dot(F->Edges[0]->dL , F->Edges[0]->dL); 00491 c = dot(F->Edges[0]->dL , F->Edges[1]->dL); 00492 d = dot(*F->Edges[0]->X0-V , F->Edges[1]->dL); 00493 f = dot(F->Edges[1]->dL , F->Edges[1]->dL); 00494 s2 = (c*d-a*f)/(b*f-c*c); 00495 t2 = (a*c-b*d)/(b*f-c*c); 00496 00497 Vec3_t p2 = -s2*F->Edges[0]->dL - t2*F->Edges[1]->dL; 00498 00499 double arg = dot(p1,p2)/(norm(p1)*norm(p2)); 00500 if (arg> 1.0) arg= 1.0; 00501 if (arg<-1.0) arg=-1.0; 00502 An = acos(arg); 00503 if (dot(cross(p1,p2),n1)<0) An = -An; 00504 } 00505 00506 inline bool BInteracton::UpdateContacts (double alpha) 00507 { 00508 return valid; 00509 } 00510 00511 inline void BInteracton::CalcForce(double dt) 00512 { 00513 if (valid) 00514 { 00515 // Calculate the normal vector and centroid of the contact face 00516 Vec3_t n1,n2,n; 00517 P1->Faces[F1]->Normal(n1); 00518 P2->Faces[F2]->Normal(n2); 00519 n = 0.5*(n1-n2); 00520 n/=norm(n); 00521 00522 Face * F = P1->Faces[F1]; 00523 Vec3_t pro1 = *F->Edges[0]->X0 + s1*F->Edges[0]->dL + t1*F->Edges[1]->dL; 00524 Vec3_t p1 = -s1*F->Edges[0]->dL - t1*F->Edges[1]->dL; 00525 F = P2->Faces[F2]; 00526 Vec3_t pro2 = *F->Edges[0]->X0 + s2*F->Edges[0]->dL + t2*F->Edges[1]->dL; 00527 Vec3_t p2 = -s2*F->Edges[0]->dL - t2*F->Edges[1]->dL; 00528 00529 // Normal force 00530 double delta = (dot(pro2-pro1,n)-L0)/L0; 00531 if (delta<0.0) delta = 0.0; 00532 00533 // Tangential Force 00534 Vec3_t x = 0.5*(pro1+pro2); 00535 Vec3_t t1,t2,x1,x2; 00536 Rotation(P1->w,P1->Q,t1); 00537 Rotation(P2->w,P2->Q,t2); 00538 x1 = x - P1->x; 00539 x2 = x - P2->x; 00540 Vec3_t vrel = -((P2->v-P1->v)+cross(t2,x2)-cross(t1,x1)); 00541 Vec3_t vt = vrel - dot(n,vrel)*n; 00542 Vec3_t td = pro2-pro1-dot(pro2-pro1,n)*n; 00543 Vec3_t Fn = (-Bn*(delta) - Gn*dot(n,vrel))*n; 00544 Vec3_t Ft = -Bt*td/L0 - Gt*vt; 00545 00546 #ifdef USE_THREAD 00547 std::lock_guard<std::mutex> lk1(P1->mtex); 00548 std::lock_guard<std::mutex> lk2(P2->mtex); 00549 #endif 00550 //Adding forces and torques 00551 Fnet = Fn+Ft; 00552 P1->F -= Fnet; 00553 P2->F += Fnet; 00554 Vec3_t T, Tt; 00555 Tt = cross (x1,Fnet); 00556 Quaternion_t q1; 00557 Conjugate (P1->Q,q1); 00558 Rotation (Tt,q1,T); 00559 P1->T -= T; 00560 Tt = cross (x2,Fnet); 00561 Quaternion_t q2; 00562 Conjugate (P2->Q,q2); 00563 Rotation (Tt,q2,T); 00564 P2->T += T; 00565 00566 //Torque 00567 double arg = dot(p1,p2)/(norm(p1)*norm(p2)); 00568 if (arg> 1.0) arg= 1.0; 00569 if (arg<-1.0) arg=-1.0; 00570 double Ant = acos(arg); 00571 if (dot(cross(p1,p2),n)<0) Ant = -Ant; 00572 Tt = -Bm*(Ant-An)*n/L0; 00573 Rotation (Tt,q1,T); 00574 P1->T -= T; 00575 Rotation (Tt,q2,T); 00576 P2->T += T; 00577 00578 //Breaking point 00579 if ((L0*fabs(delta)+norm(td)+0.0*fabs(Ant-An)*L0>L0*eps)&&(eps>=0.0)) 00580 { 00581 valid = false; 00582 } 00583 } 00584 } 00585 00586 inline void BInteracton::UpdateParameters () 00587 { 00588 Bn = 2*ReducedValue(P1->Props.Bn,P2->Props.Bn)*Area; 00589 Bt = 2*ReducedValue(P1->Props.Bt,P2->Props.Bt)*Area; 00590 Bm = 2*ReducedValue(P1->Props.Bm,P2->Props.Bm)*Area; 00591 Gn = 2*ReducedValue(P1->Props.Gn,P2->Props.Gn)*ReducedValue(P1->Props.m,P2->Props.m); 00592 Gt = 2*ReducedValue(P1->Props.Gt,P2->Props.Gt)*ReducedValue(P1->Props.m,P2->Props.m); 00593 eps = 2*ReducedValue(P1->Props.eps,P2->Props.eps); 00594 } 00595 #endif // MECHSYS_DEM_INTERACTON_H
1.7.6.1
