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 "chrono/physics/ChLinkMarkers.h"
16 
17 namespace chrono {
18 
19 // Register into the object factory, to enable run-time dynamic creation and persistence
20 // CH_FACTORY_REGISTER(ChLinkMarkers)    // NO! Abstract class!
21 
ChLinkMarkers()22 ChLinkMarkers::ChLinkMarkers()
23     : marker1(NULL),
24       marker2(NULL),
25       markID1(0),
26       markID2(0),
27       relM(CSYSNORM),
28       relM_dt(CSYSNULL),
29       relM_dtdt(CSYSNULL),
30       relAngle(0),
31       relRotaxis(VNULL),
32       relWvel(VNULL),
33       relWacc(VNULL),
34       C_force(VNULL),
35       C_torque(VNULL) {}
36 
ChLinkMarkers(const ChLinkMarkers & other)37 ChLinkMarkers::ChLinkMarkers(const ChLinkMarkers& other) : ChLink(other) {
38     marker1 = NULL;
39     marker2 = NULL;
40 
41     markID1 = other.markID1;
42     markID2 = other.markID2;
43 
44     relM = other.relM;  // copy vars
45     relM_dt = other.relM_dt;
46     relM_dtdt = other.relM_dtdt;
47     relAngle = other.relAngle;
48     relRotaxis = other.relRotaxis;
49     relAxis = other.relAxis;
50     relWvel = other.relWvel;
51     relWacc = other.relWacc;
52     dist = other.dist;
53     dist_dt = other.dist_dt;
54 
55     C_force = other.C_force;
56     C_torque = other.C_torque;
57 }
58 
SetUpMarkers(ChMarker * mark1,ChMarker * mark2)59 void ChLinkMarkers::SetUpMarkers(ChMarker* mark1, ChMarker* mark2) {
60     // take care of the first link marker
61     marker1 = mark1;
62     if (mark1)
63         Body1 = (ChBodyFrame*)mark1->GetBody();
64     else
65         Body1 = NULL;
66 
67     // take care of the second link marker
68     marker2 = mark2;
69     if (mark2)
70         Body2 = (ChBodyFrame*)mark2->GetBody();
71     else
72         Body2 = NULL;
73 }
74 
ReferenceMarkers(ChMarker * mark1,ChMarker * mark2)75 bool ChLinkMarkers::ReferenceMarkers(ChMarker* mark1, ChMarker* mark2) {
76     this->SetUpMarkers(mark1, mark2);
77 
78     if (mark1)
79         SetMarkID1(mark1->GetIdentifier());
80     else
81         SetMarkID1(0);
82     if (mark2)
83         SetMarkID2(mark2->GetIdentifier());
84     else
85         SetMarkID2(0);
86 
87     return mark1 && mark2;
88 }
89 
Initialize(std::shared_ptr<ChMarker> mmark1,std::shared_ptr<ChMarker> mmark2)90 void ChLinkMarkers::Initialize(std::shared_ptr<ChMarker> mmark1, std::shared_ptr<ChMarker> mmark2) {
91     ChMarker* mm1 = mmark1.get();
92     ChMarker* mm2 = mmark2.get();
93     assert(mm1 && mm2);
94     assert(mm1 != mm2);
95     assert(mm1->GetBody() && mm2->GetBody());
96     assert(mm1->GetBody()->GetSystem() == mm2->GetBody()->GetSystem());
97 
98     ReferenceMarkers(mm1, mm2);
99 }
100 
Initialize(std::shared_ptr<ChBody> mbody1,std::shared_ptr<ChBody> mbody2,const ChCoordsys<> & mpos)101 void ChLinkMarkers::Initialize(std::shared_ptr<ChBody> mbody1,
102                                std::shared_ptr<ChBody> mbody2,
103                                const ChCoordsys<>& mpos) {
104     return Initialize(mbody1, mbody2, false, mpos, mpos);
105 }
106 
Initialize(std::shared_ptr<ChBody> mbody1,std::shared_ptr<ChBody> mbody2,bool pos_are_relative,const ChCoordsys<> & mpos1,const ChCoordsys<> & mpos2)107 void ChLinkMarkers::Initialize(std::shared_ptr<ChBody> mbody1,
108                                std::shared_ptr<ChBody> mbody2,
109                                bool pos_are_relative,
110                                const ChCoordsys<>& mpos1,
111                                const ChCoordsys<>& mpos2) {
112     assert(mbody1.get() != mbody2.get());
113     assert(mbody1->GetSystem() == mbody2->GetSystem());
114 
115     // create markers to add to the two bodies
116     std::shared_ptr<ChMarker> mmark1(new ChMarker);
117     std::shared_ptr<ChMarker> mmark2(new ChMarker);
118     mbody1->AddMarker(mmark1);
119     mbody2->AddMarker(mmark2);
120 
121     ChMarker* mm1 = mmark1.get();
122     ChMarker* mm2 = mmark2.get();
123     ReferenceMarkers(mm1, mm2);
124 
125     if (pos_are_relative) {
126         mmark1->Impose_Rel_Coord(mpos1);
127         mmark2->Impose_Rel_Coord(mpos2);
128     } else {
129         mmark1->Impose_Abs_Coord(mpos1);
130         mmark2->Impose_Abs_Coord(mpos2);
131     }
132 }
133 
134 // For all relative degrees of freedom of the two markers, compute relM, relM_dt,
135 // and relM_dtdt, as well as auxiliary data. Cache some intermediate quantities
136 // for possible reuse in UpdateState by some derived classes.
UpdateRelMarkerCoords()137 void ChLinkMarkers::UpdateRelMarkerCoords() {
138     PQw = Vsub(marker1->GetAbsCoord().pos, marker2->GetAbsCoord().pos);
139     PQw_dt = Vsub(marker1->GetAbsCoord_dt().pos, marker2->GetAbsCoord_dt().pos);
140     PQw_dtdt = Vsub(marker1->GetAbsCoord_dtdt().pos, marker2->GetAbsCoord_dtdt().pos);
141 
142     dist = Vlength(PQw);                 // distance between origins, modulus
143     dist_dt = Vdot(Vnorm(PQw), PQw_dt);  // speed between origins, modulus.
144 
145     Quaternion qtemp1;
146 
147     Quaternion temp1 = marker1->GetCoord_dt().rot;
148     Quaternion temp2 = marker2->GetCoord_dt().rot;
149 
150     if (Qnotnull(temp1) || Qnotnull(temp2)) {
151         q_AD =  //  q'qqq + qqqq'
152             Qadd(Qcross(Qconjugate(marker2->GetCoord_dt().rot),
153                         Qcross(Qconjugate(marker2->GetBody()->GetCoord().rot),
154                                Qcross((marker1->GetBody()->GetCoord().rot), (marker1->GetCoord().rot)))),
155                  Qcross(Qconjugate(marker2->GetCoord().rot),
156                         Qcross(Qconjugate(marker2->GetBody()->GetCoord().rot),
157                                Qcross((marker1->GetBody()->GetCoord().rot), (marker1->GetCoord_dt().rot)))));
158     } else
159         q_AD = QNULL;
160 
161     q_BC =  // qq'qq + qqq'q
162         Qadd(Qcross(Qconjugate(marker2->GetCoord().rot),
163                     Qcross(Qconjugate(marker2->GetBody()->GetCoord_dt().rot),
164                            Qcross((marker1->GetBody()->GetCoord().rot), (marker1->GetCoord().rot)))),
165              Qcross(Qconjugate(marker2->GetCoord().rot),
166                     Qcross(Qconjugate(marker2->GetBody()->GetCoord().rot),
167                            Qcross((marker1->GetBody()->GetCoord_dt().rot), (marker1->GetCoord().rot)))));
168 
169     // q_8 = q''qqq + 2q'q'qq + 2q'qq'q + 2q'qqq'
170     //     + 2qq'q'q + 2qq'qq' + 2qqq'q' + qqqq''
171     temp2 = marker2->GetCoord_dtdt().rot;
172     if (Qnotnull(temp2))
173         q_8 = Qcross(Qconjugate(marker2->GetCoord_dtdt().rot),
174                      Qcross(Qconjugate(Body2->GetCoord().rot),
175                             Qcross(Body1->GetCoord().rot,
176                                    marker1->GetCoord().rot)));  // q_dtdt'm2 * q'o2 * q,o1 * q,m1
177     else
178         q_8 = QNULL;
179     temp1 = marker1->GetCoord_dtdt().rot;
180     if (Qnotnull(temp1)) {
181         qtemp1 = Qcross(Qconjugate(marker2->GetCoord().rot),
182                         Qcross(Qconjugate(Body2->GetCoord().rot),
183                                Qcross(Body1->GetCoord().rot,
184                                       marker1->GetCoord_dtdt().rot)));  // q'm2 * q'o2 * q,o1 * q_dtdt,m1
185         q_8 = Qadd(q_8, qtemp1);
186     }
187     temp2 = marker2->GetCoord_dt().rot;
188     if (Qnotnull(temp2)) {
189         qtemp1 = Qcross(
190             Qconjugate(marker2->GetCoord_dt().rot),
191             Qcross(Qconjugate(Body2->GetCoord_dt().rot), Qcross(Body1->GetCoord().rot, marker1->GetCoord().rot)));
192         qtemp1 = Qscale(qtemp1, 2);  // 2( q_dt'm2 * q_dt'o2 * q,o1 * q,m1)
193         q_8 = Qadd(q_8, qtemp1);
194     }
195     temp2 = marker2->GetCoord_dt().rot;
196     if (Qnotnull(temp2)) {
197         qtemp1 = Qcross(
198             Qconjugate(marker2->GetCoord_dt().rot),
199             Qcross(Qconjugate(Body2->GetCoord().rot), Qcross(Body1->GetCoord_dt().rot, marker1->GetCoord().rot)));
200         qtemp1 = Qscale(qtemp1, 2);  // 2( q_dt'm2 * q'o2 * q_dt,o1 * q,m1)
201         q_8 = Qadd(q_8, qtemp1);
202     }
203     temp1 = marker1->GetCoord_dt().rot;
204     temp2 = marker2->GetCoord_dt().rot;
205     if (Qnotnull(temp2) && Qnotnull(temp1)) {
206         qtemp1 = Qcross(
207             Qconjugate(marker2->GetCoord_dt().rot),
208             Qcross(Qconjugate(Body2->GetCoord().rot), Qcross(Body1->GetCoord().rot, marker1->GetCoord_dt().rot)));
209         qtemp1 = Qscale(qtemp1, 2);  // 2( q_dt'm2 * q'o2 * q,o1 * q_dt,m1)
210         q_8 = Qadd(q_8, qtemp1);
211     }
212 
213     qtemp1 =
214         Qcross(Qconjugate(marker2->GetCoord().rot),
215                Qcross(Qconjugate(Body2->GetCoord_dt().rot), Qcross(Body1->GetCoord_dt().rot, marker1->GetCoord().rot)));
216     qtemp1 = Qscale(qtemp1, 2);  // 2( q'm2 * q_dt'o2 * q_dt,o1 * q,m1)
217     q_8 = Qadd(q_8, qtemp1);
218     temp1 = marker1->GetCoord_dt().rot;
219     if (Qnotnull(temp1)) {
220         qtemp1 = Qcross(
221             Qconjugate(marker2->GetCoord().rot),
222             Qcross(Qconjugate(Body2->GetCoord_dt().rot), Qcross(Body1->GetCoord().rot, marker1->GetCoord_dt().rot)));
223         qtemp1 = Qscale(qtemp1, 2);  // 2( q'm2 * q_dt'o2 * q,o1 * q_dt,m1)
224         q_8 = Qadd(q_8, qtemp1);
225     }
226     temp1 = marker1->GetCoord_dt().rot;
227     if (Qnotnull(temp1)) {
228         qtemp1 = Qcross(
229             Qconjugate(marker2->GetCoord().rot),
230             Qcross(Qconjugate(Body2->GetCoord().rot), Qcross(Body1->GetCoord_dt().rot, marker1->GetCoord_dt().rot)));
231         qtemp1 = Qscale(qtemp1, 2);  // 2( q'm2 * q'o2 * q_dt,o1 * q_dt,m1)
232         q_8 = Qadd(q_8, qtemp1);
233     }
234 
235     // q_4 = [Adtdt]'[A]'q + 2[Adt]'[Adt]'q
236     //       + 2[Adt]'[A]'qdt + 2[A]'[Adt]'qdt
237     ChMatrix33<> m2_Rel_A_dt;
238     marker2->Compute_Adt(m2_Rel_A_dt);
239     ChMatrix33<> m2_Rel_A_dtdt;
240     marker2->Compute_Adtdt(m2_Rel_A_dtdt);
241 
242     ChVector<> vtemp1;
243     ChVector<> vtemp2;
244 
245     vtemp1 = Body2->GetA_dt().transpose() * PQw;
246     vtemp2 = m2_Rel_A_dt.transpose() * vtemp1;
247     q_4 = Vmul(vtemp2, 2);  // 2[Aq_dt]'[Ao2_dt]'*Qpq,w
248 
249     vtemp1 = Body2->GetA().transpose() * PQw_dt;
250     vtemp2 = m2_Rel_A_dt.transpose() * vtemp1;
251     vtemp2 = Vmul(vtemp2, 2);  // 2[Aq_dt]'[Ao2]'*Qpq,w_dt
252     q_4 = Vadd(q_4, vtemp2);
253 
254     vtemp1 = Body2->GetA_dt().transpose() * PQw_dt;
255     vtemp2 = marker2->GetA().transpose() * vtemp1;
256     vtemp2 = Vmul(vtemp2, 2);  // 2[Aq]'[Ao2_dt]'*Qpq,w_dt
257     q_4 = Vadd(q_4, vtemp2);
258 
259     vtemp1 = Body2->GetA().transpose() * PQw;
260     vtemp2 = m2_Rel_A_dtdt.transpose() * vtemp1;
261     q_4 = Vadd(q_4, vtemp2);  //  [Aq_dtdt]'[Ao2]'*Qpq,w
262 
263     // ----------- RELATIVE MARKER COORDINATES
264 
265     // relM.pos
266     relM.pos = marker2->GetA().transpose() * (Body2->GetA().transpose() * PQw);
267 
268     // relM.rot
269     relM.rot = Qcross(Qconjugate(marker2->GetCoord().rot),
270                       Qcross(Qconjugate(marker2->GetBody()->GetCoord().rot),
271                              Qcross((marker1->GetBody()->GetCoord().rot), (marker1->GetCoord().rot))));
272 
273     // relM_dt.pos
274     relM_dt.pos = m2_Rel_A_dt.transpose() * (Body2->GetA().transpose() * PQw) +
275                   marker2->GetA().transpose() * (Body2->GetA_dt().transpose() * PQw) +
276                   marker2->GetA().transpose() * (Body2->GetA().transpose() * PQw_dt);
277 
278     // relM_dt.rot
279     relM_dt.rot = Qadd(q_AD, q_BC);
280 
281     // relM_dtdt.pos
282     relM_dtdt.pos = marker2->GetA().transpose() * (Body2->GetA_dtdt().transpose() * PQw) +
283                     marker2->GetA().transpose() * (Body2->GetA().transpose() * PQw_dtdt) + q_4;
284 
285     // relM_dtdt.rot
286     qtemp1 = Qcross(Qconjugate(marker2->GetCoord().rot),
287                     Qcross(Qconjugate(Body2->GetCoord_dtdt().rot),
288                            Qcross(Body1->GetCoord().rot,
289                                   marker1->GetCoord().rot)));  // ( q'm2 * q_dtdt'o2 * q,o1 * q,m1)
290     relM_dtdt.rot = Qadd(q_8, qtemp1);
291     qtemp1 = Qcross(Qconjugate(marker2->GetCoord().rot),
292                     Qcross(Qconjugate(Body2->GetCoord().rot),
293                            Qcross(Body1->GetCoord_dtdt().rot,
294                                   marker1->GetCoord().rot)));  // ( q'm2 * q'o2 * q_dtdt,o1 * q,m1)
295     relM_dtdt.rot = Qadd(relM_dtdt.rot, qtemp1);               // = q_8 + qq''qq + qqq''q
296 
297     // ... and also "user-friendly" relative coordinates:
298 
299     // relAngle and relAxis
300     Q_to_AngAxis(relM.rot, relAngle, relAxis);
301     // flip rel rotation axis if jerky sign
302     if (relAxis.z() < 0) {
303         relAxis = Vmul(relAxis, -1);
304         relAngle = -relAngle;
305     }
306     // rotation axis
307     relRotaxis = Vmul(relAxis, relAngle);
308     // relWvel
309     ChGwMatrix34<> relGw(relM.rot);
310     relWvel = relGw * relM_dt.rot;
311     // relWacc
312     relWacc = relGw * relM_dtdt.rot;
313 }
314 
UpdateForces(double mytime)315 void ChLinkMarkers::UpdateForces(double mytime) {
316     // reset internal force accumulators
317     C_force = VNULL;  // initialize int.forces accumulators
318     C_torque = VNULL;
319 }
320 
Update(double time,bool update_assets)321 void ChLinkMarkers::Update(double time, bool update_assets) {
322     UpdateTime(time);
323     UpdateRelMarkerCoords();
324     UpdateForces(time);
325 
326     // Update assets
327     ChPhysicsItem::Update(ChTime, update_assets);
328 }
329 
330 //// STATE BOOKKEEPING FUNCTIONS
331 
332 // Load residual R += c * F, starting at specified offset.
IntLoadResidual_F(const unsigned int off,ChVectorDynamic<> & R,const double c)333 void ChLinkMarkers::IntLoadResidual_F(const unsigned int off, ChVectorDynamic<>& R, const double c) {
334     if (!Body1 || !Body2)
335         return;
336 
337     Vector mbody_force;
338     Vector mbody_torque;
339     if (Vnotnull(C_force)) {
340         Vector m_abs_force = Body2->GetA() * (marker2->GetA() * C_force);
341 
342         if (Body2->Variables().IsActive()) {
343             Body2->To_abs_forcetorque(m_abs_force,
344                                       marker1->GetAbsCoord().pos,  // absolute application point is always marker1
345                                       false,                       // from abs. space
346                                       mbody_force, mbody_torque);  // resulting force-torque, both in abs coords
347             R.segment(Body2->Variables().GetOffset() + 0, 3) -= c * mbody_force.eigen();
348             R.segment(Body2->Variables().GetOffset() + 3, 3) -=
349                 c * Body2->TransformDirectionParentToLocal(mbody_torque).eigen();
350         }
351 
352         if (Body1->Variables().IsActive()) {
353             Body1->To_abs_forcetorque(m_abs_force,
354                                       marker1->GetAbsCoord().pos,  // absolute application point is always marker1
355                                       false,                       // from abs. space
356                                       mbody_force, mbody_torque);  // resulting force-torque, both in abs coords
357             R.segment(Body1->Variables().GetOffset() + 0, 3) += c * mbody_force.eigen();
358             R.segment(Body1->Variables().GetOffset() + 3, 3) +=
359                 c * Body1->TransformDirectionParentToLocal(mbody_torque).eigen();
360         }
361     }
362     if (Vnotnull(C_torque)) {
363         Vector m_abs_torque = Body2->GetA() * (marker2->GetA() * C_torque);
364         // load torques in 'fb' vector accumulator of body variables (torques in local coords)
365         if (Body1->Variables().IsActive()) {
366             R.segment(Body1->Variables().GetOffset() + 3, 3) +=
367                 c * Body1->TransformDirectionParentToLocal(m_abs_torque).eigen();
368         }
369         if (Body2->Variables().IsActive()) {
370             R.segment(Body2->Variables().GetOffset() + 3, 3) -=
371                 c * Body2->TransformDirectionParentToLocal(m_abs_torque).eigen();
372         }
373     }
374 }
375 
376 // SOLVER INTERFACE
377 
ConstraintsFbLoadForces(double factor)378 void ChLinkMarkers::ConstraintsFbLoadForces(double factor) {
379     if (!Body1 || !Body2)
380         return;
381 
382     if (Vnotnull(C_force)) {
383         Vector mbody_force;
384         Vector mbody_torque;
385         Vector m_abs_force = Body2->GetA() * (marker2->GetA() * C_force);
386 
387         Body2->To_abs_forcetorque(m_abs_force,
388                                   marker1->GetAbsCoord().pos,  // absolute application point is always marker1
389                                   false,                       // from abs. space
390                                   mbody_force, mbody_torque);  // resulting force-torque, both in abs coords
391         Body2->Variables().Get_fb().segment(0, 3) -= factor * mbody_force.eigen();
392         Body2->Variables().Get_fb().segment(3, 3) -=
393             factor * Body2->TransformDirectionParentToLocal(mbody_torque).eigen();
394 
395         Body1->To_abs_forcetorque(m_abs_force,
396                                   marker1->GetAbsCoord().pos,  // absolute application point is always marker1
397                                   false,                       // from abs. space
398                                   mbody_force, mbody_torque);  // resulting force-torque, both in abs coords
399         Body1->Variables().Get_fb().segment(0, 3) += factor * mbody_force.eigen();
400         Body1->Variables().Get_fb().segment(3, 3) +=
401             factor * Body1->TransformDirectionParentToLocal(mbody_torque).eigen();
402     }
403 
404     if (Vnotnull(C_torque)) {
405         Vector m_abs_torque = Body2->GetA() * (marker2->GetA() * C_torque);
406         // load torques in 'fb' vector accumulator of body variables (torques in local coords)
407         Body1->Variables().Get_fb().segment(3, 3) +=
408             factor * Body1->TransformDirectionParentToLocal(m_abs_torque).eigen();
409         Body2->Variables().Get_fb().segment(3, 3) -=
410             factor * Body2->TransformDirectionParentToLocal(m_abs_torque).eigen();
411     }
412 }
413 
ArchiveOUT(ChArchiveOut & marchive)414 void ChLinkMarkers::ArchiveOUT(ChArchiveOut& marchive) {
415     // version number
416     marchive.VersionWrite<ChLinkMarkers>();
417 
418     // serialize parent class
419     ChLink::ArchiveOUT(marchive);
420 
421     // serialize all member data:
422     marchive << CHNVP(markID1);
423     marchive << CHNVP(markID2);
424 }
425 
426 /// Method to allow de serialization of transient data from archives.
ArchiveIN(ChArchiveIn & marchive)427 void ChLinkMarkers::ArchiveIN(ChArchiveIn& marchive) {
428     // version number
429     /*int version =*/ marchive.VersionRead<ChLinkMarkers>();
430 
431     // deserialize parent class
432     ChLink::ArchiveIN(marchive);
433 
434     // deserialize all member data:
435     marchive >> CHNVP(markID1);
436     marchive >> CHNVP(markID2);
437 }
438 
439 }  // end namespace chrono
440