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 // Authors: Alessandro Tasora, Radu Serban
13 // =============================================================================
14
15 #include <cstdlib>
16 #include <algorithm>
17
18 #include "chrono/core/ChGlobal.h"
19 #include "chrono/core/ChTransform.h"
20 #include "chrono/physics/ChBody.h"
21 #include "chrono/physics/ChForce.h"
22 #include "chrono/physics/ChMarker.h"
23 #include "chrono/physics/ChSystem.h"
24
25 #include "chrono/collision/ChCollisionModelBullet.h"
26 #ifdef CHRONO_COLLISION
27 #include "chrono/collision/ChCollisionModelChrono.h"
28 #endif
29
30 namespace chrono {
31
32 using namespace collision;
33 using namespace geometry;
34
35 // Register into the object factory, to enable run-time dynamic creation and persistence
CH_FACTORY_REGISTER(ChBody)36 CH_FACTORY_REGISTER(ChBody)
37
38 ChBody::ChBody(collision::ChCollisionSystemType collision_type) {
39 marklist.clear();
40 forcelist.clear();
41
42 BFlagsSetAllOFF(); // no flags
43
44 Xforce = VNULL;
45 Xtorque = VNULL;
46
47 Force_acc = VNULL;
48 Torque_acc = VNULL;
49
50 #ifndef CHRONO_COLLISION
51 collision_type = ChCollisionSystemType::BULLET;
52 #endif
53
54 switch (collision_type) {
55 default:
56 case ChCollisionSystemType::BULLET:
57 collision_model = chrono_types::make_shared<ChCollisionModelBullet>();
58 collision_model->SetContactable(this);
59 break;
60 case ChCollisionSystemType::CHRONO:
61 #ifdef CHRONO_COLLISION
62 collision_model = chrono_types::make_shared<ChCollisionModelChrono>();
63 collision_model->SetContactable(this);
64 #endif
65 break;
66 }
67
68 density = 1000.0f;
69
70 max_speed = 0.5f;
71 max_wvel = 2.0f * float(CH_C_PI);
72
73 sleep_time = 0.6f;
74 sleep_starttime = 0;
75 sleep_minspeed = 0.1f;
76 sleep_minwvel = 0.04f;
77 SetUseSleeping(true);
78
79 variables.SetUserData((void*)this);
80
81 body_id = 0;
82 }
83
ChBody(std::shared_ptr<collision::ChCollisionModel> new_collision_model)84 ChBody::ChBody(std::shared_ptr<collision::ChCollisionModel> new_collision_model) {
85 marklist.clear();
86 forcelist.clear();
87
88 BFlagsSetAllOFF(); // no flags
89
90 Xforce = VNULL;
91 Xtorque = VNULL;
92
93 Force_acc = VNULL;
94 Torque_acc = VNULL;
95
96 collision_model = new_collision_model;
97 collision_model->SetContactable(this);
98
99 density = 1000.0f;
100
101 max_speed = 0.5f;
102 max_wvel = 2.0f * float(CH_C_PI);
103
104 sleep_time = 0.6f;
105 sleep_starttime = 0;
106 sleep_minspeed = 0.1f;
107 sleep_minwvel = 0.04f;
108 SetUseSleeping(true);
109
110 variables.SetUserData((void*)this);
111
112 body_id = 0;
113 }
114
ChBody(const ChBody & other)115 ChBody::ChBody(const ChBody& other) : ChPhysicsItem(other), ChBodyFrame(other) {
116 bflags = other.bflags;
117
118 variables = other.variables;
119 variables.SetUserData((void*)this);
120
121 gyro = other.gyro;
122
123 RemoveAllForces(); // also copy-duplicate the forces? Let the user handle this..
124 RemoveAllMarkers(); // also copy-duplicate the markers? Let the user handle this..
125
126 ChTime = other.ChTime;
127
128 // also copy-duplicate the collision model? Let the user handle this..
129 collision_model = chrono_types::make_shared<ChCollisionModelBullet>();
130 collision_model->SetContactable(this);
131
132 density = other.density;
133
134 max_speed = other.max_speed;
135 max_wvel = other.max_wvel;
136
137 sleep_time = other.sleep_time;
138 sleep_starttime = other.sleep_starttime;
139 sleep_minspeed = other.sleep_minspeed;
140 sleep_minwvel = other.sleep_minwvel;
141 }
142
~ChBody()143 ChBody::~ChBody() {
144 RemoveAllForces();
145 RemoveAllMarkers();
146 }
147
148 //// STATE BOOKKEEPING FUNCTIONS
149
IntStateGather(const unsigned int off_x,ChState & x,const unsigned int off_v,ChStateDelta & v,double & T)150 void ChBody::IntStateGather(const unsigned int off_x, // offset in x state vector
151 ChState& x, // state vector, position part
152 const unsigned int off_v, // offset in v state vector
153 ChStateDelta& v, // state vector, speed part
154 double& T // time
155 ) {
156 x.segment(off_x + 0, 3) = this->coord.pos.eigen();
157 x.segment(off_x + 3, 4) = this->coord.rot.eigen();
158 v.segment(off_v + 0, 3) = this->coord_dt.pos.eigen();
159 v.segment(off_v + 3, 3) = this->GetWvel_loc().eigen();
160 T = this->GetChTime();
161 }
162
IntStateScatter(const unsigned int off_x,const ChState & x,const unsigned int off_v,const ChStateDelta & v,const double T,bool full_update)163 void ChBody::IntStateScatter(const unsigned int off_x, // offset in x state vector
164 const ChState& x, // state vector, position part
165 const unsigned int off_v, // offset in v state vector
166 const ChStateDelta& v, // state vector, speed part
167 const double T, // time
168 bool full_update // perform complete update
169 ) {
170 this->SetCoord(x.segment(off_x, 7));
171 this->SetPos_dt(v.segment(off_v + 0, 3));
172 this->SetWvel_loc(v.segment(off_v + 3, 3));
173 this->SetChTime(T);
174 this->Update(T, full_update);
175 }
176
IntStateGatherAcceleration(const unsigned int off_a,ChStateDelta & a)177 void ChBody::IntStateGatherAcceleration(const unsigned int off_a, ChStateDelta& a) {
178 a.segment(off_a + 0, 3) = this->coord_dtdt.pos.eigen();
179 a.segment(off_a + 3, 3) = this->GetWacc_loc().eigen();
180 }
181
IntStateScatterAcceleration(const unsigned int off_a,const ChStateDelta & a)182 void ChBody::IntStateScatterAcceleration(const unsigned int off_a, const ChStateDelta& a) {
183 this->SetPos_dtdt(a.segment(off_a + 0, 3));
184 this->SetWacc_loc(a.segment(off_a + 3, 3));
185 }
186
IntStateIncrement(const unsigned int off_x,ChState & x_new,const ChState & x,const unsigned int off_v,const ChStateDelta & Dv)187 void ChBody::IntStateIncrement(const unsigned int off_x, // offset in x state vector
188 ChState& x_new, // state vector, position part, incremented result
189 const ChState& x, // state vector, initial position part
190 const unsigned int off_v, // offset in v state vector
191 const ChStateDelta& Dv // state vector, increment
192 ) {
193 // ADVANCE POSITION:
194 x_new(off_x) = x(off_x) + Dv(off_v);
195 x_new(off_x + 1) = x(off_x + 1) + Dv(off_v + 1);
196 x_new(off_x + 2) = x(off_x + 2) + Dv(off_v + 2);
197
198 // ADVANCE ROTATION: rot' = delta*rot (use quaternion for delta rotation)
199 ChQuaternion<> mdeltarot;
200 ChQuaternion<> moldrot(x.segment(off_x + 3, 4));
201 ChVector<> newwel_abs = Amatrix * ChVector<>(Dv.segment(off_v + 3, 3));
202 double mangle = newwel_abs.Length();
203 newwel_abs.Normalize();
204 mdeltarot.Q_from_AngAxis(mangle, newwel_abs);
205 ChQuaternion<> mnewrot = mdeltarot * moldrot; // quaternion product
206 x_new.segment(off_x + 3, 4) = mnewrot.eigen();
207 }
208
IntLoadResidual_F(const unsigned int off,ChVectorDynamic<> & R,const double c)209 void ChBody::IntLoadResidual_F(const unsigned int off, // offset in R residual
210 ChVectorDynamic<>& R, // result: the R residual, R += c*F
211 const double c // a scaling factor
212 ) {
213 // add applied forces to 'fb' vector
214 R.segment(off, 3) += c * Xforce.eigen();
215
216 // add applied torques to 'fb' vector, including gyroscopic torque
217 if (this->GetNoGyroTorque())
218 R.segment(off + 3, 3) += c * Xtorque.eigen();
219 else
220 R.segment(off + 3, 3) += c * (Xtorque - gyro).eigen();
221 }
222
IntLoadResidual_Mv(const unsigned int off,ChVectorDynamic<> & R,const ChVectorDynamic<> & w,const double c)223 void ChBody::IntLoadResidual_Mv(const unsigned int off, // offset in R residual
224 ChVectorDynamic<>& R, // result: the R residual, R += c*M*v
225 const ChVectorDynamic<>& w, // the w vector
226 const double c // a scaling factor
227 ) {
228 R(off + 0) += c * GetMass() * w(off + 0);
229 R(off + 1) += c * GetMass() * w(off + 1);
230 R(off + 2) += c * GetMass() * w(off + 2);
231 ChVector<> Iw = GetInertia() * ChVector<>(w.segment(off + 3, 3));
232 Iw *= c;
233 R.segment(off + 3, 3) += Iw.eigen();
234 }
235
IntToDescriptor(const unsigned int off_v,const ChStateDelta & v,const ChVectorDynamic<> & R,const unsigned int off_L,const ChVectorDynamic<> & L,const ChVectorDynamic<> & Qc)236 void ChBody::IntToDescriptor(const unsigned int off_v,
237 const ChStateDelta& v,
238 const ChVectorDynamic<>& R,
239 const unsigned int off_L,
240 const ChVectorDynamic<>& L,
241 const ChVectorDynamic<>& Qc) {
242 this->variables.Get_qb() = v.segment(off_v, 6);
243 this->variables.Get_fb() = R.segment(off_v, 6);
244 }
245
IntFromDescriptor(const unsigned int off_v,ChStateDelta & v,const unsigned int off_L,ChVectorDynamic<> & L)246 void ChBody::IntFromDescriptor(const unsigned int off_v, // offset in v
247 ChStateDelta& v,
248 const unsigned int off_L, // offset in L
249 ChVectorDynamic<>& L) {
250 v.segment(off_v, 6) = this->variables.Get_qb();
251 }
252
253 ////
254
InjectVariables(ChSystemDescriptor & mdescriptor)255 void ChBody::InjectVariables(ChSystemDescriptor& mdescriptor) {
256 this->variables.SetDisabled(!this->IsActive());
257
258 mdescriptor.InsertVariables(&this->variables);
259 }
260
VariablesFbReset()261 void ChBody::VariablesFbReset() {
262 this->variables.Get_fb().setZero();
263 }
264
VariablesFbLoadForces(double factor)265 void ChBody::VariablesFbLoadForces(double factor) {
266 // add applied forces to 'fb' vector
267 this->variables.Get_fb().segment(0, 3) += factor * Xforce.eigen();
268
269 // add applied torques to 'fb' vector, including gyroscopic torque
270 if (this->GetNoGyroTorque())
271 this->variables.Get_fb().segment(3, 3) += factor * Xtorque.eigen();
272 else
273 this->variables.Get_fb().segment(3, 3) += factor * (Xtorque - gyro).eigen();
274 }
275
VariablesFbIncrementMq()276 void ChBody::VariablesFbIncrementMq() {
277 this->variables.Compute_inc_Mb_v(this->variables.Get_fb(), this->variables.Get_qb());
278 }
279
VariablesQbLoadSpeed()280 void ChBody::VariablesQbLoadSpeed() {
281 // set current speed in 'qb', it can be used by the solver when working in incremental mode
282 this->variables.Get_qb().segment(0, 3) = GetCoord_dt().pos.eigen();
283 this->variables.Get_qb().segment(3, 3) = GetWvel_loc().eigen();
284 }
285
VariablesQbSetSpeed(double step)286 void ChBody::VariablesQbSetSpeed(double step) {
287 ChCoordsys<> old_coord_dt = this->GetCoord_dt();
288
289 // from 'qb' vector, sets body speed, and updates auxiliary data
290 this->SetPos_dt(this->variables.Get_qb().segment(0, 3));
291 this->SetWvel_loc(this->variables.Get_qb().segment(3, 3));
292
293 // apply limits (if in speed clamping mode) to speeds.
294 ClampSpeed();
295
296 // compute auxiliary gyroscopic forces
297 ComputeGyro();
298
299 // Compute accel. by BDF (approximate by differentiation);
300 if (step) {
301 this->SetPos_dtdt((this->GetCoord_dt().pos - old_coord_dt.pos) / step);
302 this->SetRot_dtdt((this->GetCoord_dt().rot - old_coord_dt.rot) / step);
303 }
304 }
305
VariablesQbIncrementPosition(double dt_step)306 void ChBody::VariablesQbIncrementPosition(double dt_step) {
307 if (!this->IsActive())
308 return;
309
310 // Updates position with incremental action of speed contained in the
311 // 'qb' vector: pos' = pos + dt * speed , like in an Eulero step.
312
313 ChVector<> newspeed(variables.Get_qb().segment(0, 3));
314 ChVector<> newwel(variables.Get_qb().segment(3, 3));
315
316 // ADVANCE POSITION: pos' = pos + dt * vel
317 this->SetPos(this->GetPos() + newspeed * dt_step);
318
319 // ADVANCE ROTATION: rot' = [dt*wwel]%rot (use quaternion for delta rotation)
320 ChQuaternion<> mdeltarot;
321 ChQuaternion<> moldrot = this->GetRot();
322 ChVector<> newwel_abs = Amatrix * newwel;
323 double mangle = newwel_abs.Length() * dt_step;
324 newwel_abs.Normalize();
325 mdeltarot.Q_from_AngAxis(mangle, newwel_abs);
326 ChQuaternion<> mnewrot = mdeltarot % moldrot;
327 this->SetRot(mnewrot);
328 }
329
SetNoSpeedNoAcceleration()330 void ChBody::SetNoSpeedNoAcceleration() {
331 this->SetPos_dt(VNULL);
332 this->SetWvel_loc(VNULL);
333 this->SetPos_dtdt(VNULL);
334 this->SetRot_dtdt(QNULL);
335 }
336
337 ////
ClampSpeed()338 void ChBody::ClampSpeed() {
339 if (this->GetLimitSpeed()) {
340 double w = 2.0 * this->coord_dt.rot.Length();
341 if (w > max_wvel)
342 coord_dt.rot *= max_wvel / w;
343
344 double v = this->coord_dt.pos.Length();
345 if (v > max_speed)
346 coord_dt.pos *= max_speed / v;
347 }
348 }
349
350 //// Utilities for coordinate transformations
351
Point_World2Body(const ChVector<> & mpoint)352 ChVector<> ChBody::Point_World2Body(const ChVector<>& mpoint) {
353 return ChFrame<double>::TransformParentToLocal(mpoint);
354 }
355
Point_Body2World(const ChVector<> & mpoint)356 ChVector<> ChBody::Point_Body2World(const ChVector<>& mpoint) {
357 return ChFrame<double>::TransformLocalToParent(mpoint);
358 }
359
Dir_World2Body(const ChVector<> & dir)360 ChVector<> ChBody::Dir_World2Body(const ChVector<>& dir) {
361 return Amatrix.transpose() * dir;
362 }
363
Dir_Body2World(const ChVector<> & dir)364 ChVector<> ChBody::Dir_Body2World(const ChVector<>& dir) {
365 return Amatrix * dir;
366 }
367
RelPoint_AbsSpeed(const ChVector<> & mrelpoint)368 ChVector<> ChBody::RelPoint_AbsSpeed(const ChVector<>& mrelpoint) {
369 return PointSpeedLocalToParent(mrelpoint);
370 }
371
RelPoint_AbsAcc(const ChVector<> & mrelpoint)372 ChVector<> ChBody::RelPoint_AbsAcc(const ChVector<>& mrelpoint) {
373 return PointAccelerationLocalToParent(mrelpoint);
374 }
375
376 // The inertia tensor functions
377
SetInertia(const ChMatrix33<> & newXInertia)378 void ChBody::SetInertia(const ChMatrix33<>& newXInertia) {
379 variables.SetBodyInertia(newXInertia);
380 }
381
SetInertiaXX(const ChVector<> & iner)382 void ChBody::SetInertiaXX(const ChVector<>& iner) {
383 variables.GetBodyInertia()(0, 0) = iner.x();
384 variables.GetBodyInertia()(1, 1) = iner.y();
385 variables.GetBodyInertia()(2, 2) = iner.z();
386 variables.GetBodyInvInertia() = variables.GetBodyInertia().inverse();
387 }
388
SetInertiaXY(const ChVector<> & iner)389 void ChBody::SetInertiaXY(const ChVector<>& iner) {
390 variables.GetBodyInertia()(0, 1) = iner.x();
391 variables.GetBodyInertia()(0, 2) = iner.y();
392 variables.GetBodyInertia()(1, 2) = iner.z();
393 variables.GetBodyInertia()(1, 0) = iner.x();
394 variables.GetBodyInertia()(2, 0) = iner.y();
395 variables.GetBodyInertia()(2, 1) = iner.z();
396 variables.GetBodyInvInertia() = variables.GetBodyInertia().inverse();
397 }
398
GetInertiaXX() const399 ChVector<> ChBody::GetInertiaXX() const {
400 ChVector<> iner;
401 iner.x() = variables.GetBodyInertia()(0, 0);
402 iner.y() = variables.GetBodyInertia()(1, 1);
403 iner.z() = variables.GetBodyInertia()(2, 2);
404 return iner;
405 }
406
GetInertiaXY() const407 ChVector<> ChBody::GetInertiaXY() const {
408 ChVector<> iner;
409 iner.x() = variables.GetBodyInertia()(0, 1);
410 iner.y() = variables.GetBodyInertia()(0, 2);
411 iner.z() = variables.GetBodyInertia()(1, 2);
412 return iner;
413 }
414
ComputeQInertia(ChMatrix44<> & mQInertia)415 void ChBody::ComputeQInertia(ChMatrix44<>& mQInertia) {
416 // [Iq]=[G'][Ix][G]
417 ChGlMatrix34<> Gl(coord.rot);
418 mQInertia = Gl.transpose() * this->GetInertia() * Gl;
419 }
420
421 //////
422
Empty_forces_accumulators()423 void ChBody::Empty_forces_accumulators() {
424 Force_acc = VNULL;
425 Torque_acc = VNULL;
426 }
427
Accumulate_force(const ChVector<> & force,const ChVector<> & appl_point,bool local)428 void ChBody::Accumulate_force(const ChVector<>& force, const ChVector<>& appl_point, bool local) {
429 ChVector<> mabsforce;
430 ChVector<> mabstorque;
431 To_abs_forcetorque(force, appl_point, local, mabsforce, mabstorque);
432
433 Force_acc += mabsforce;
434 Torque_acc += mabstorque;
435 }
436
Accumulate_torque(const ChVector<> & torque,bool local)437 void ChBody::Accumulate_torque(const ChVector<>& torque, bool local) {
438 if (local) {
439 Torque_acc += torque;
440 } else {
441 Torque_acc += Dir_World2Body(torque);
442 }
443 }
444
445 //////
446
ComputeGyro()447 void ChBody::ComputeGyro() {
448 ChVector<> Wvel = this->GetWvel_loc();
449 gyro = Vcross(Wvel, variables.GetBodyInertia() * Wvel);
450 }
451
TrySleeping()452 bool ChBody::TrySleeping() {
453 BFlagSet(BodyFlag::COULDSLEEP, false);
454
455 if (this->GetUseSleeping()) {
456 if (!this->IsActive())
457 return false;
458
459 // if not yet sleeping:
460 if ((this->coord_dt.pos.LengthInf() < this->sleep_minspeed) &&
461 (2.0 * this->coord_dt.rot.LengthInf() < this->sleep_minwvel)) {
462 if ((this->GetChTime() - this->sleep_starttime) > this->sleep_time) {
463 BFlagSet(BodyFlag::COULDSLEEP, true); // mark as sleep candidate
464 return true; // could go to sleep!
465 }
466 } else {
467 this->sleep_starttime = float(this->GetChTime());
468 }
469 }
470 return false;
471 }
472
AddMarker(std::shared_ptr<ChMarker> amarker)473 void ChBody::AddMarker(std::shared_ptr<ChMarker> amarker) {
474 // don't allow double insertion of same object
475 assert(std::find<std::vector<std::shared_ptr<ChMarker>>::iterator>(marklist.begin(), marklist.end(), amarker) ==
476 marklist.end());
477
478 amarker->SetBody(this);
479 marklist.push_back(amarker);
480
481 // If the body is already added to a system, mark the system uninitialized and out-of-date
482 if (system) {
483 system->is_initialized = false;
484 system->is_updated = false;
485 }
486 }
487
AddForce(std::shared_ptr<ChForce> aforce)488 void ChBody::AddForce(std::shared_ptr<ChForce> aforce) {
489 // don't allow double insertion of same object
490 assert(std::find<std::vector<std::shared_ptr<ChForce>>::iterator>(forcelist.begin(), forcelist.end(), aforce) ==
491 forcelist.end());
492
493 aforce->SetBody(this);
494 forcelist.push_back(aforce);
495
496 // If the body is already added to a system, mark the system uninitialized and out-of-date
497 if (system) {
498 system->is_initialized = false;
499 system->is_updated = false;
500 }
501 }
502
RemoveForce(std::shared_ptr<ChForce> mforce)503 void ChBody::RemoveForce(std::shared_ptr<ChForce> mforce) {
504 // trying to remove objects not previously added?
505 assert(std::find<std::vector<std::shared_ptr<ChForce>>::iterator>(forcelist.begin(), forcelist.end(), mforce) !=
506 forcelist.end());
507
508 // warning! linear time search
509 forcelist.erase(
510 std::find<std::vector<std::shared_ptr<ChForce>>::iterator>(forcelist.begin(), forcelist.end(), mforce));
511
512 mforce->SetBody(0);
513
514 // If the body is already added to a system, mark the system out-of-date
515 if (system) {
516 system->is_updated = false;
517 }
518 }
519
RemoveMarker(std::shared_ptr<ChMarker> mmarker)520 void ChBody::RemoveMarker(std::shared_ptr<ChMarker> mmarker) {
521 // trying to remove objects not previously added?
522 assert(std::find<std::vector<std::shared_ptr<ChMarker>>::iterator>(marklist.begin(), marklist.end(), mmarker) !=
523 marklist.end());
524
525 // warning! linear time search
526 marklist.erase(
527 std::find<std::vector<std::shared_ptr<ChMarker>>::iterator>(marklist.begin(), marklist.end(), mmarker));
528
529 mmarker->SetBody(0);
530
531 // If the body is already added to a system, mark the system out-of-date
532 if (system) {
533 system->is_updated = false;
534 }
535 }
536
RemoveAllForces()537 void ChBody::RemoveAllForces() {
538 for (auto& force : forcelist) {
539 force->SetBody(NULL);
540 }
541 forcelist.clear();
542 }
543
RemoveAllMarkers()544 void ChBody::RemoveAllMarkers() {
545 for (auto& marker : marklist) {
546 marker->SetBody(NULL);
547 }
548 marklist.clear();
549 }
550
SearchMarker(const char * m_name)551 std::shared_ptr<ChMarker> ChBody::SearchMarker(const char* m_name) {
552 return ChContainerSearchFromName<std::shared_ptr<ChMarker>, std::vector<std::shared_ptr<ChMarker>>::iterator>(
553 m_name, marklist.begin(), marklist.end());
554 }
555
SearchForce(const char * m_name)556 std::shared_ptr<ChForce> ChBody::SearchForce(const char* m_name) {
557 return ChContainerSearchFromName<std::shared_ptr<ChForce>, std::vector<std::shared_ptr<ChForce>>::iterator>(
558 m_name, forcelist.begin(), forcelist.end());
559 }
560
561 // These are the members used to UPDATE
562 // the body coordinates during the animation
563 // Also the coordinates of forces and markers
564 // linked to the body will be updated.
565
UpdateMarkers(double mytime)566 void ChBody::UpdateMarkers(double mytime) {
567 for (auto& marker : marklist) {
568 marker->Update(mytime);
569 }
570 }
571
UpdateForces(double mytime)572 void ChBody::UpdateForces(double mytime) {
573 // Initialize body force (in abs. coords) and torque (in local coords)
574 // with current values from the accumulators.
575 Xforce = Force_acc;
576 Xtorque = Torque_acc;
577
578 // Accumulate other applied forces
579 ChVector<> mforce;
580 ChVector<> mtorque;
581
582 for (auto& force : forcelist) {
583 // update positions, f=f(t,q)
584 force->Update(mytime);
585
586 force->GetBodyForceTorque(mforce, mtorque);
587 Xforce += mforce;
588 Xtorque += mtorque;
589 }
590
591 // Add gravitational forces
592 if (system) {
593 Xforce += system->Get_G_acc() * GetMass();
594 }
595 }
596
UpdateTime(double mytime)597 void ChBody::UpdateTime(double mytime) {
598 ChTime = mytime;
599 }
600
601 // UpdateALL updates the state and time
602 // of the object AND the dependant (linked)
603 // markers and forces.
604
Update(bool update_assets)605 void ChBody::Update(bool update_assets) {
606 // TrySleeping(); // See if the body can fall asleep; if so, put it to sleeping
607 ClampSpeed(); // Apply limits (if in speed clamping mode) to speeds.
608 ComputeGyro(); // Set the gyroscopic momentum.
609
610 // Also update the children "markers" and
611 // "forces" depending on the body current state.
612 UpdateMarkers(ChTime);
613
614 UpdateForces(ChTime);
615
616 // This will update assets
617 ChPhysicsItem::Update(ChTime, update_assets);
618 }
619
620 // As before, but keeps the current state.
621 // Mostly used for world reference body.
Update(double mytime,bool update_assets)622 void ChBody::Update(double mytime, bool update_assets) {
623 // For the body:
624 UpdateTime(mytime);
625
626 Update(update_assets);
627 }
628
629 // ---------------------------------------------------------------------------
630 // Body flags management
BFlagsSetAllOFF()631 void ChBody::BFlagsSetAllOFF() {
632 bflags = 0;
633 }
BFlagsSetAllON()634 void ChBody::BFlagsSetAllON() {
635 bflags = 0;
636 bflags = ~bflags;
637 }
BFlagSetON(BodyFlag mask)638 void ChBody::BFlagSetON(BodyFlag mask) {
639 bflags |= mask;
640 }
BFlagSetOFF(BodyFlag mask)641 void ChBody::BFlagSetOFF(BodyFlag mask) {
642 bflags &= ~mask;
643 }
BFlagGet(BodyFlag mask) const644 bool ChBody::BFlagGet(BodyFlag mask) const {
645 return (bflags & mask) != 0;
646 };
BFlagSet(BodyFlag mask,bool state)647 void ChBody::BFlagSet(BodyFlag mask, bool state) {
648 if (state)
649 bflags |= mask;
650 else
651 bflags &= ~mask;
652 }
653
SetBodyFixed(bool state)654 void ChBody::SetBodyFixed(bool state) {
655 variables.SetDisabled(state);
656 if (state == BFlagGet(BodyFlag::FIXED))
657 return;
658 BFlagSet(BodyFlag::FIXED, state);
659 }
660
GetBodyFixed() const661 bool ChBody::GetBodyFixed() const {
662 return BFlagGet(BodyFlag::FIXED);
663 }
664
SetEvalContactCn(bool state)665 void ChBody::SetEvalContactCn(bool state) {
666 BFlagSet(BodyFlag::EVAL_CONTACT_CN, state);
667 }
668
GetEvalContactCn() const669 bool ChBody::GetEvalContactCn() const {
670 return BFlagGet(BodyFlag::EVAL_CONTACT_CN);
671 }
672
SetEvalContactCt(bool state)673 void ChBody::SetEvalContactCt(bool state) {
674 BFlagSet(BodyFlag::EVAL_CONTACT_CT, state);
675 }
676
GetEvalContactCt() const677 bool ChBody::GetEvalContactCt() const {
678 return BFlagGet(BodyFlag::EVAL_CONTACT_CT);
679 }
680
SetEvalContactKf(bool state)681 void ChBody::SetEvalContactKf(bool state) {
682 BFlagSet(BodyFlag::EVAL_CONTACT_KF, state);
683 }
684
GetEvalContactKf() const685 bool ChBody::GetEvalContactKf() const {
686 return BFlagGet(BodyFlag::EVAL_CONTACT_KF);
687 }
688
SetEvalContactSf(bool state)689 void ChBody::SetEvalContactSf(bool state) {
690 BFlagSet(BodyFlag::EVAL_CONTACT_SF, state);
691 }
692
GetEvalContactSf() const693 bool ChBody::GetEvalContactSf() const {
694 return BFlagGet(BodyFlag::EVAL_CONTACT_SF);
695 }
696
SetShowCollisionMesh(bool state)697 void ChBody::SetShowCollisionMesh(bool state) {
698 BFlagSet(BodyFlag::SHOW_COLLMESH, state);
699 }
700
GetShowCollisionMesh() const701 bool ChBody::GetShowCollisionMesh() const {
702 return BFlagGet(BodyFlag::SHOW_COLLMESH);
703 }
704
SetLimitSpeed(bool state)705 void ChBody::SetLimitSpeed(bool state) {
706 BFlagSet(BodyFlag::LIMITSPEED, state);
707 }
708
GetLimitSpeed() const709 bool ChBody::GetLimitSpeed() const {
710 return BFlagGet(BodyFlag::LIMITSPEED);
711 }
712
SetNoGyroTorque(bool state)713 void ChBody::SetNoGyroTorque(bool state) {
714 BFlagSet(BodyFlag::NOGYROTORQUE, state);
715 }
716
GetNoGyroTorque() const717 bool ChBody::GetNoGyroTorque() const {
718 return BFlagGet(BodyFlag::NOGYROTORQUE);
719 }
720
SetUseSleeping(bool state)721 void ChBody::SetUseSleeping(bool state) {
722 BFlagSet(BodyFlag::USESLEEPING, state);
723 }
724
GetUseSleeping() const725 bool ChBody::GetUseSleeping() const {
726 return BFlagGet(BodyFlag::USESLEEPING);
727 }
728
SetSleeping(bool state)729 void ChBody::SetSleeping(bool state) {
730 BFlagSet(BodyFlag::SLEEPING, state);
731 }
732
GetSleeping() const733 bool ChBody::GetSleeping() const {
734 return BFlagGet(BodyFlag::SLEEPING);
735 }
736
IsActive()737 bool ChBody::IsActive() {
738 return !BFlagGet(BodyFlag::SLEEPING) && !BFlagGet(BodyFlag::FIXED);
739 }
740
741 // ---------------------------------------------------------------------------
742 // Collision-related functions
743
SetCollide(bool state)744 void ChBody::SetCollide(bool state) {
745 if (state == BFlagGet(BodyFlag::COLLIDE))
746 return;
747
748 if (state) {
749 SyncCollisionModels();
750 BFlagSetON(BodyFlag::COLLIDE);
751 if (GetSystem())
752 GetSystem()->GetCollisionSystem()->Add(collision_model.get());
753 } else {
754 BFlagSetOFF(BodyFlag::COLLIDE);
755 if (GetSystem())
756 GetSystem()->GetCollisionSystem()->Remove(collision_model.get());
757 }
758 }
759
GetCollide() const760 bool ChBody::GetCollide() const {
761 return BFlagGet(BodyFlag::COLLIDE);
762 }
763
SetCollisionModel(std::shared_ptr<collision::ChCollisionModel> new_collision_model)764 void ChBody::SetCollisionModel(std::shared_ptr<collision::ChCollisionModel> new_collision_model) {
765 if (collision_model) {
766 if (system)
767 system->GetCollisionSystem()->Remove(collision_model.get());
768 }
769
770 collision_model = new_collision_model;
771 collision_model->SetContactable(this);
772 }
773
SyncCollisionModels()774 void ChBody::SyncCollisionModels() {
775 if (this->GetCollide())
776 this->GetCollisionModel()->SyncPosition();
777 }
778
AddCollisionModelsToSystem()779 void ChBody::AddCollisionModelsToSystem() {
780 assert(this->GetSystem());
781 SyncCollisionModels();
782 if (this->GetCollide())
783 this->GetSystem()->GetCollisionSystem()->Add(collision_model.get());
784 }
785
RemoveCollisionModelsFromSystem()786 void ChBody::RemoveCollisionModelsFromSystem() {
787 assert(this->GetSystem());
788 if (this->GetCollide())
789 this->GetSystem()->GetCollisionSystem()->Remove(collision_model.get());
790 }
791
792 // ---------------------------------------------------------------------------
793
GetTotalAABB(ChVector<> & bbmin,ChVector<> & bbmax)794 void ChBody::GetTotalAABB(ChVector<>& bbmin, ChVector<>& bbmax) {
795 if (this->GetCollisionModel())
796 this->GetCollisionModel()->GetAABB(bbmin, bbmax);
797 else
798 ChPhysicsItem::GetTotalAABB(bbmin, bbmax); // default: infinite aabb
799 }
800
ContactableGetStateBlock_x(ChState & x)801 void ChBody::ContactableGetStateBlock_x(ChState& x) {
802 x.segment(0, 3) = this->GetCoord().pos.eigen();
803 x.segment(3, 4) = this->GetCoord().rot.eigen();
804 }
805
ContactableGetStateBlock_w(ChStateDelta & w)806 void ChBody::ContactableGetStateBlock_w(ChStateDelta& w) {
807 w.segment(0, 3) = this->GetPos_dt().eigen();
808 w.segment(3, 3) = this->GetWvel_loc().eigen();
809 }
810
ContactableIncrementState(const ChState & x,const ChStateDelta & dw,ChState & x_new)811 void ChBody::ContactableIncrementState(const ChState& x, const ChStateDelta& dw, ChState& x_new) {
812 IntStateIncrement(0, x_new, x, 0, dw);
813 }
814
GetContactPoint(const ChVector<> & loc_point,const ChState & state_x)815 ChVector<> ChBody::GetContactPoint(const ChVector<>& loc_point, const ChState& state_x) {
816 ChCoordsys<> csys(state_x.segment(0, 7));
817 return csys.TransformPointLocalToParent(loc_point);
818 }
819
GetContactPointSpeed(const ChVector<> & loc_point,const ChState & state_x,const ChStateDelta & state_w)820 ChVector<> ChBody::GetContactPointSpeed(const ChVector<>& loc_point,
821 const ChState& state_x,
822 const ChStateDelta& state_w) {
823 ChCoordsys<> csys(state_x.segment(0, 7));
824 ChVector<> abs_vel(state_w.segment(0, 3));
825 ChVector<> loc_omg(state_w.segment(3, 3));
826 ChVector<> abs_omg = csys.TransformDirectionLocalToParent(loc_omg);
827
828 return abs_vel + Vcross(abs_omg, loc_point);
829 }
830
GetContactPointSpeed(const ChVector<> & abs_point)831 ChVector<> ChBody::GetContactPointSpeed(const ChVector<>& abs_point) {
832 ChVector<> m_p1_loc = this->Point_World2Body(abs_point);
833 return this->PointSpeedLocalToParent(m_p1_loc);
834 }
835
GetCsysForCollisionModel()836 ChCoordsys<> ChBody::GetCsysForCollisionModel() {
837 return ChCoordsys<>(this->GetFrame_REF_to_abs().coord);
838 }
839
ContactForceLoadResidual_F(const ChVector<> & F,const ChVector<> & abs_point,ChVectorDynamic<> & R)840 void ChBody::ContactForceLoadResidual_F(const ChVector<>& F, const ChVector<>& abs_point, ChVectorDynamic<>& R) {
841 ChVector<> m_p1_loc = this->Point_World2Body(abs_point);
842 ChVector<> force1_loc = this->Dir_World2Body(F);
843 ChVector<> torque1_loc = Vcross(m_p1_loc, force1_loc);
844 R.segment(this->GetOffset_w() + 0, 3) += F.eigen();
845 R.segment(this->GetOffset_w() + 3, 3) += torque1_loc.eigen();
846 }
847
ContactForceLoadQ(const ChVector<> & F,const ChVector<> & point,const ChState & state_x,ChVectorDynamic<> & Q,int offset)848 void ChBody::ContactForceLoadQ(const ChVector<>& F,
849 const ChVector<>& point,
850 const ChState& state_x,
851 ChVectorDynamic<>& Q,
852 int offset) {
853 ChCoordsys<> csys(state_x.segment(0, 7));
854 ChVector<> point_loc = csys.TransformPointParentToLocal(point);
855 ChVector<> force_loc = csys.TransformDirectionParentToLocal(F);
856 ChVector<> torque_loc = Vcross(point_loc, force_loc);
857 Q.segment(offset + 0, 3) = F.eigen();
858 Q.segment(offset + 3, 3) = torque_loc.eigen();
859 }
860
ComputeJacobianForContactPart(const ChVector<> & abs_point,ChMatrix33<> & contact_plane,ChVariableTupleCarrier_1vars<6>::type_constraint_tuple & jacobian_tuple_N,ChVariableTupleCarrier_1vars<6>::type_constraint_tuple & jacobian_tuple_U,ChVariableTupleCarrier_1vars<6>::type_constraint_tuple & jacobian_tuple_V,bool second)861 void ChBody::ComputeJacobianForContactPart(const ChVector<>& abs_point,
862 ChMatrix33<>& contact_plane,
863 ChVariableTupleCarrier_1vars<6>::type_constraint_tuple& jacobian_tuple_N,
864 ChVariableTupleCarrier_1vars<6>::type_constraint_tuple& jacobian_tuple_U,
865 ChVariableTupleCarrier_1vars<6>::type_constraint_tuple& jacobian_tuple_V,
866 bool second) {
867 /*
868 ChVector<> p1 = this->Point_World2Body(abs_point);
869 ChStarMatrix33<> Ps1(p1);
870
871 ChMatrix33<> Jx1 = contact_plane.transpose();
872 if (!second)
873 Jx1 *= -1;
874
875 ChMatrix33<> Jr1 = contact_plane.transpose() * this->GetA() * Ps1;
876 if (!second)
877 Jr1 *= -1;
878
879 jacobian_tuple_N.Get_Cq().segment(0, 3) = Jx1.row(0);
880 jacobian_tuple_U.Get_Cq().segment(0, 3) = Jx1.row(1);
881 jacobian_tuple_V.Get_Cq().segment(0, 3) = Jx1.row(2);
882
883 jacobian_tuple_N.Get_Cq().segment(3, 3) = Jr1.row(0);
884 jacobian_tuple_U.Get_Cq().segment(3, 3) = Jr1.row(1);
885 jacobian_tuple_V.Get_Cq().segment(3, 3) = Jr1.row(2);
886 */
887
888 // UNROLLED VERSION - FASTER
889 ChVector<> p1 = this->Point_World2Body(abs_point);
890 double temp00 =
891 Amatrix(0, 2) * contact_plane(0, 0) + Amatrix(1, 2) * contact_plane(1, 0) + Amatrix(2, 2) * contact_plane(2, 0);
892 double temp01 =
893 Amatrix(0, 2) * contact_plane(0, 1) + Amatrix(1, 2) * contact_plane(1, 1) + Amatrix(2, 2) * contact_plane(2, 1);
894 double temp02 =
895 Amatrix(0, 2) * contact_plane(0, 2) + Amatrix(1, 2) * contact_plane(1, 2) + Amatrix(2, 2) * contact_plane(2, 2);
896 double temp10 =
897 Amatrix(0, 1) * contact_plane(0, 0) + Amatrix(1, 1) * contact_plane(1, 0) + Amatrix(2, 1) * contact_plane(2, 0);
898 double temp11 =
899 Amatrix(0, 1) * contact_plane(0, 1) + Amatrix(1, 1) * contact_plane(1, 1) + Amatrix(2, 1) * contact_plane(2, 1);
900 double temp12 =
901 Amatrix(0, 1) * contact_plane(0, 2) + Amatrix(1, 1) * contact_plane(1, 2) + Amatrix(2, 1) * contact_plane(2, 2);
902 double temp20 =
903 Amatrix(0, 0) * contact_plane(0, 0) + Amatrix(1, 0) * contact_plane(1, 0) + Amatrix(2, 0) * contact_plane(2, 0);
904 double temp21 =
905 Amatrix(0, 0) * contact_plane(0, 1) + Amatrix(1, 0) * contact_plane(1, 1) + Amatrix(2, 0) * contact_plane(2, 1);
906 double temp22 =
907 Amatrix(0, 0) * contact_plane(0, 2) + Amatrix(1, 0) * contact_plane(1, 2) + Amatrix(2, 0) * contact_plane(2, 2);
908
909 // Jx1 =
910 // [ c00, c10, c20]
911 // [ c01, c11, c21]
912 // [ c02, c12, c22]
913 // Jr1 = [ p1y*(temp00) - p1z*(temp10), p1z*(temp20) - p1x*(temp00), p1x*(temp10) - p1y*(temp20);
914 // p1y*(temp01) - p1z*(temp11), p1z*(temp21) - p1x*(temp01), p1x*(temp11) - p1y*(temp21);
915 // p1y*(temp02) - p1z*(temp12), p1z*(temp22) - p1x*(temp02), p1x*(temp12) - p1y*(temp22)];
916 if (!second) {
917 jacobian_tuple_N.Get_Cq()(0) = -contact_plane(0, 0);
918 jacobian_tuple_N.Get_Cq()(1) = -contact_plane(1, 0);
919 jacobian_tuple_N.Get_Cq()(2) = -contact_plane(2, 0);
920 jacobian_tuple_N.Get_Cq()(3) = -p1.y() * temp00 + p1.z() * temp10;
921 jacobian_tuple_N.Get_Cq()(4) = -p1.z() * temp20 + p1.x() * temp00;
922 jacobian_tuple_N.Get_Cq()(5) = -p1.x() * temp10 + p1.y() * temp20;
923
924 jacobian_tuple_U.Get_Cq()(0) = -contact_plane(0, 1);
925 jacobian_tuple_U.Get_Cq()(1) = -contact_plane(1, 1);
926 jacobian_tuple_U.Get_Cq()(2) = -contact_plane(2, 1);
927 jacobian_tuple_U.Get_Cq()(3) = -p1.y() * temp01 + p1.z() * temp11;
928 jacobian_tuple_U.Get_Cq()(4) = -p1.z() * temp21 + p1.x() * temp01;
929 jacobian_tuple_U.Get_Cq()(5) = -p1.x() * temp11 + p1.y() * temp21;
930
931 jacobian_tuple_V.Get_Cq()(0) = -contact_plane(0, 2);
932 jacobian_tuple_V.Get_Cq()(1) = -contact_plane(1, 2);
933 jacobian_tuple_V.Get_Cq()(2) = -contact_plane(2, 2);
934 jacobian_tuple_V.Get_Cq()(3) = -p1.y() * temp02 + p1.z() * temp12;
935 jacobian_tuple_V.Get_Cq()(4) = -p1.z() * temp22 + p1.x() * temp02;
936 jacobian_tuple_V.Get_Cq()(5) = -p1.x() * temp12 + p1.y() * temp22;
937 } else {
938 jacobian_tuple_N.Get_Cq()(0) = contact_plane(0, 0);
939 jacobian_tuple_N.Get_Cq()(1) = contact_plane(1, 0);
940 jacobian_tuple_N.Get_Cq()(2) = contact_plane(2, 0);
941 jacobian_tuple_N.Get_Cq()(3) = p1.y() * temp00 - p1.z() * temp10;
942 jacobian_tuple_N.Get_Cq()(4) = p1.z() * temp20 - p1.x() * temp00;
943 jacobian_tuple_N.Get_Cq()(5) = p1.x() * temp10 - p1.y() * temp20;
944
945 jacobian_tuple_U.Get_Cq()(0) = contact_plane(0, 1);
946 jacobian_tuple_U.Get_Cq()(1) = contact_plane(1, 1);
947 jacobian_tuple_U.Get_Cq()(2) = contact_plane(2, 1);
948 jacobian_tuple_U.Get_Cq()(3) = p1.y() * temp01 - p1.z() * temp11;
949 jacobian_tuple_U.Get_Cq()(4) = p1.z() * temp21 - p1.x() * temp01;
950 jacobian_tuple_U.Get_Cq()(5) = p1.x() * temp11 - p1.y() * temp21;
951
952 jacobian_tuple_V.Get_Cq()(0) = contact_plane(0, 2);
953 jacobian_tuple_V.Get_Cq()(1) = contact_plane(1, 2);
954 jacobian_tuple_V.Get_Cq()(2) = contact_plane(2, 2);
955 jacobian_tuple_V.Get_Cq()(3) = p1.y() * temp02 - p1.z() * temp12;
956 jacobian_tuple_V.Get_Cq()(4) = p1.z() * temp22 - p1.x() * temp02;
957 jacobian_tuple_V.Get_Cq()(5) = p1.x() * temp12 - p1.y() * temp22;
958 }
959 }
960
ComputeJacobianForRollingContactPart(const ChVector<> & abs_point,ChMatrix33<> & contact_plane,ChVariableTupleCarrier_1vars<6>::type_constraint_tuple & jacobian_tuple_N,ChVariableTupleCarrier_1vars<6>::type_constraint_tuple & jacobian_tuple_U,ChVariableTupleCarrier_1vars<6>::type_constraint_tuple & jacobian_tuple_V,bool second)961 void ChBody::ComputeJacobianForRollingContactPart(
962 const ChVector<>& abs_point,
963 ChMatrix33<>& contact_plane,
964 ChVariableTupleCarrier_1vars<6>::type_constraint_tuple& jacobian_tuple_N,
965 ChVariableTupleCarrier_1vars<6>::type_constraint_tuple& jacobian_tuple_U,
966 ChVariableTupleCarrier_1vars<6>::type_constraint_tuple& jacobian_tuple_V,
967 bool second) {
968 ChMatrix33<> Jr1 = contact_plane.transpose() * this->GetA();
969 if (!second)
970 Jr1 *= -1;
971
972 jacobian_tuple_N.Get_Cq().segment(0, 3).setZero();
973 jacobian_tuple_U.Get_Cq().segment(0, 3).setZero();
974 jacobian_tuple_V.Get_Cq().segment(0, 3).setZero();
975 jacobian_tuple_N.Get_Cq().segment(3, 3) = Jr1.row(0);
976 jacobian_tuple_U.Get_Cq().segment(3, 3) = Jr1.row(1);
977 jacobian_tuple_V.Get_Cq().segment(3, 3) = Jr1.row(2);
978 }
979
GetAppliedForce()980 ChVector<> ChBody::GetAppliedForce() {
981 return GetSystem()->GetBodyAppliedForce(this);
982 }
983
GetAppliedTorque()984 ChVector<> ChBody::GetAppliedTorque() {
985 return GetSystem()->GetBodyAppliedTorque(this);
986 }
987
GetContactForce()988 ChVector<> ChBody::GetContactForce() {
989 return GetSystem()->GetContactContainer()->GetContactableForce(this);
990 }
991
GetContactTorque()992 ChVector<> ChBody::GetContactTorque() {
993 return GetSystem()->GetContactContainer()->GetContactableTorque(this);
994 }
995
996 // ---------------------------------------------------------------------------
997
LoadableGetVariables(std::vector<ChVariables * > & mvars)998 void ChBody::LoadableGetVariables(std::vector<ChVariables*>& mvars) {
999 mvars.push_back(&this->Variables());
1000 }
1001
LoadableStateIncrement(const unsigned int off_x,ChState & x_new,const ChState & x,const unsigned int off_v,const ChStateDelta & Dv)1002 void ChBody::LoadableStateIncrement(const unsigned int off_x,
1003 ChState& x_new,
1004 const ChState& x,
1005 const unsigned int off_v,
1006 const ChStateDelta& Dv) {
1007 IntStateIncrement(off_x, x_new, x, off_v, Dv);
1008 }
1009
LoadableGetStateBlock_x(int block_offset,ChState & mD)1010 void ChBody::LoadableGetStateBlock_x(int block_offset, ChState& mD) {
1011 mD.segment(block_offset + 0, 3) = this->GetCoord().pos.eigen();
1012 mD.segment(block_offset + 3, 4) = this->GetCoord().rot.eigen();
1013 }
1014
LoadableGetStateBlock_w(int block_offset,ChStateDelta & mD)1015 void ChBody::LoadableGetStateBlock_w(int block_offset, ChStateDelta& mD) {
1016 mD.segment(block_offset + 0, 3) = this->GetPos_dt().eigen();
1017 mD.segment(block_offset + 3, 3) = this->GetWvel_loc().eigen();
1018 }
1019
ComputeNF(const double U,const double V,const double W,ChVectorDynamic<> & Qi,double & detJ,const ChVectorDynamic<> & F,ChVectorDynamic<> * state_x,ChVectorDynamic<> * state_w)1020 void ChBody::ComputeNF(
1021 const double U, // x coordinate of application point in absolute space
1022 const double V, // y coordinate of application point in absolute space
1023 const double W, // z coordinate of application point in absolute space
1024 ChVectorDynamic<>& Qi, // Return result of N'*F here, maybe with offset block_offset
1025 double& detJ, // Return det[J] here
1026 const ChVectorDynamic<>& F, // Input F vector, size is 6, it is {Force,Torque} in absolute coords.
1027 ChVectorDynamic<>* state_x, // if != 0, update state (pos. part) to this, then evaluate Q
1028 ChVectorDynamic<>* state_w // if != 0, update state (speed part) to this, then evaluate Q
1029 ) {
1030 ChVector<> abs_pos(U, V, W);
1031 ChVector<> absF(F.segment(0, 3));
1032 ChVector<> absT(F.segment(3, 3));
1033 ChVector<> body_absF;
1034 ChVector<> body_locT;
1035 ChCoordsys<> bodycoord;
1036 if (state_x)
1037 bodycoord = state_x->segment(0, 7); // the numerical jacobian algo might change state_x
1038 else
1039 bodycoord = this->coord;
1040
1041 // compute Q components F,T, given current state of body 'bodycoord'. Note T in Q is in local csys, F is an abs csys
1042 body_absF = absF;
1043 body_locT = bodycoord.rot.RotateBack(absT + ((abs_pos - bodycoord.pos) % absF));
1044 Qi.segment(0, 3) = body_absF.eigen();
1045 Qi.segment(3, 3) = body_locT.eigen();
1046 detJ = 1; // not needed because not used in quadrature.
1047 }
1048
1049 // ---------------------------------------------------------------------------
1050 // FILE I/O
1051
ArchiveOUT(ChArchiveOut & marchive)1052 void ChBody::ArchiveOUT(ChArchiveOut& marchive) {
1053 // version number
1054 marchive.VersionWrite<ChBody>();
1055
1056 // serialize parent class
1057 ChPhysicsItem::ArchiveOUT(marchive);
1058 // serialize parent class
1059 ChBodyFrame::ArchiveOUT(marchive);
1060
1061 // serialize all member data:
1062
1063 marchive << CHNVP(bflags);
1064 bool mflag; // more readable flag output in case of ASCII in/out
1065 mflag = BFlagGet(BodyFlag::FIXED);
1066 marchive << CHNVP(mflag, "is_fixed");
1067 mflag = BFlagGet(BodyFlag::COLLIDE);
1068 marchive << CHNVP(mflag, "collide");
1069 mflag = BFlagGet(BodyFlag::LIMITSPEED);
1070 marchive << CHNVP(mflag, "limit_speed");
1071 mflag = BFlagGet(BodyFlag::NOGYROTORQUE);
1072 marchive << CHNVP(mflag, "no_gyro_torque");
1073 mflag = BFlagGet(BodyFlag::USESLEEPING);
1074 marchive << CHNVP(mflag, "use_sleeping");
1075 mflag = BFlagGet(BodyFlag::SLEEPING);
1076 marchive << CHNVP(mflag, "is_sleeping");
1077
1078 marchive << CHNVP(marklist, "markers");
1079 marchive << CHNVP(forcelist, "forces");
1080
1081 marchive << CHNVP(body_id);
1082 marchive << CHNVP(collision_model);
1083 marchive << CHNVP(gyro);
1084 marchive << CHNVP(Xforce);
1085 marchive << CHNVP(Xtorque);
1086 // marchive << CHNVP(Force_acc); // not useful in serialization
1087 // marchive << CHNVP(Torque_acc);// not useful in serialization
1088 marchive << CHNVP(density);
1089 marchive << CHNVP(variables);
1090 marchive << CHNVP(max_speed);
1091 marchive << CHNVP(max_wvel);
1092 marchive << CHNVP(sleep_time);
1093 marchive << CHNVP(sleep_minspeed);
1094 marchive << CHNVP(sleep_minwvel);
1095 marchive << CHNVP(sleep_starttime);
1096 }
1097
1098 /// Method to allow de serialization of transient data from archives.
ArchiveIN(ChArchiveIn & marchive)1099 void ChBody::ArchiveIN(ChArchiveIn& marchive) {
1100 // version number
1101 /*int version =*/ marchive.VersionRead<ChBody>();
1102
1103 // deserialize parent class
1104 ChPhysicsItem::ArchiveIN(marchive);
1105 // deserialize parent class
1106 ChBodyFrame::ArchiveIN(marchive);
1107
1108 // stream in all member data:
1109
1110 marchive >> CHNVP(bflags);
1111 bool mflag; // more readable flag output in case of ASCII in/out
1112 marchive >> CHNVP(mflag, "is_fixed");
1113 BFlagSet(BodyFlag::FIXED, mflag);
1114 marchive >> CHNVP(mflag, "collide");
1115 BFlagSet(BodyFlag::COLLIDE, mflag);
1116 marchive >> CHNVP(mflag, "limit_speed");
1117 BFlagSet(BodyFlag::LIMITSPEED, mflag);
1118 marchive >> CHNVP(mflag, "no_gyro_torque");
1119 BFlagSet(BodyFlag::NOGYROTORQUE, mflag);
1120 marchive >> CHNVP(mflag, "use_sleeping");
1121 BFlagSet(BodyFlag::USESLEEPING, mflag);
1122 marchive >> CHNVP(mflag, "is_sleeping");
1123 BFlagSet(BodyFlag::SLEEPING, mflag);
1124
1125 std::vector<std::shared_ptr<ChMarker>> tempmarkers;
1126 std::vector<std::shared_ptr<ChForce>> tempforces;
1127 marchive >> CHNVP(tempmarkers, "markers");
1128 marchive >> CHNVP(tempforces, "forces");
1129 // trick needed because the "Add...() functions are required
1130 this->RemoveAllMarkers();
1131 for (auto i : tempmarkers) {
1132 this->AddMarker(i);
1133 }
1134 this->RemoveAllForces();
1135 for (auto i : tempforces) {
1136 this->AddForce(i);
1137 }
1138
1139 marchive >> CHNVP(body_id);
1140 marchive >> CHNVP(collision_model);
1141 collision_model->SetContactable(this);
1142 marchive >> CHNVP(gyro);
1143 marchive >> CHNVP(Xforce);
1144 marchive >> CHNVP(Xtorque);
1145 // marchive << CHNVP(Force_acc); // not useful in serialization
1146 // marchive << CHNVP(Torque_acc);// not useful in serialization
1147 marchive >> CHNVP(density);
1148 marchive >> CHNVP(variables);
1149 marchive >> CHNVP(max_speed);
1150 marchive >> CHNVP(max_wvel);
1151 marchive >> CHNVP(sleep_time);
1152 marchive >> CHNVP(sleep_minspeed);
1153 marchive >> CHNVP(sleep_minwvel);
1154 marchive >> CHNVP(sleep_starttime);
1155 }
1156
StreamOUTstate(ChStreamOutBinary & mstream)1157 void ChBody::StreamOUTstate(ChStreamOutBinary& mstream) {
1158 // Do not serialize parent classes and do not
1159 // implement versioning, because this must be efficient
1160 // and will be used just for domain decomposition.
1161 mstream << this->coord.pos.x();
1162 mstream << this->coord.pos.y();
1163 mstream << this->coord.pos.z();
1164 mstream << this->coord.rot.e0();
1165 mstream << this->coord.rot.e1();
1166 mstream << this->coord.rot.e2();
1167 mstream << this->coord.rot.e3();
1168 mstream << this->coord_dt.pos.x();
1169 mstream << this->coord_dt.pos.y();
1170 mstream << this->coord_dt.pos.z();
1171 mstream << this->coord_dt.rot.e0();
1172 mstream << this->coord_dt.rot.e1();
1173 mstream << this->coord_dt.rot.e2();
1174 mstream << this->coord_dt.rot.e3();
1175 }
1176
StreamINstate(ChStreamInBinary & mstream)1177 void ChBody::StreamINstate(ChStreamInBinary& mstream) {
1178 // Do not serialize parent classes and do not
1179 // implement versioning, because this must be efficient
1180 // and will be used just for domain decomposition.
1181 mstream >> this->coord.pos.x();
1182 mstream >> this->coord.pos.y();
1183 mstream >> this->coord.pos.z();
1184 mstream >> this->coord.rot.e0();
1185 mstream >> this->coord.rot.e1();
1186 mstream >> this->coord.rot.e2();
1187 mstream >> this->coord.rot.e3();
1188 this->SetCoord(coord);
1189 mstream >> this->coord_dt.pos.x();
1190 mstream >> this->coord_dt.pos.y();
1191 mstream >> this->coord_dt.pos.z();
1192 mstream >> this->coord_dt.rot.e0();
1193 mstream >> this->coord_dt.rot.e1();
1194 mstream >> this->coord_dt.rot.e2();
1195 mstream >> this->coord_dt.rot.e3();
1196 this->SetCoord_dt(coord_dt);
1197
1198 this->Update();
1199 this->SyncCollisionModels();
1200 }
1201
1202 } // end namespace chrono
1203