Program listing for file kernel/src/modelingTools/NewtonEulerDS.hpp#
Return to documentation for this file
1#ifndef NEWTONEULERNLDS_H
2#define NEWTONEULERNLDS_H
3
4#include "BoundaryCondition.hpp"
5#include "DynamicalSystem.hpp"
6#include "SecondOrderDS.hpp"
7
8
9typedef void (*FInt_NE)(double t, double *q, double *v, double *f,
10 unsigned int size_z, double *z);
11typedef void (*FExt_NE)(double t, double *f, unsigned int size_z, double *z);
12
13void computeT(SP::SiconosVector q, SP::SimpleMatrix T);
14#include "RotationQuaternion.hpp"
15
16
17void computeExtForceAtPos(SP::SiconosVector q,
18 bool isMextExpressedInInertialFrame,
19 SP::SiconosVector force, bool forceAbsRef,
20 SP::SiconosVector pos, bool posAbsRef,
21 SP::SiconosVector fExt, SP::SiconosVector mExt,
22 bool accumulate);
23
24
25class NewtonEulerDS : public SecondOrderDS {
26protected:
27
28 ACCEPT_SERIALIZATION(NewtonEulerDS);
29
30
31 void _init();
32
33
34
35
36 SP::SiconosVector _twist;
37
38
39 SP::SiconosVector _twist0;
40
41
42 SP::SiconosVector _q;
43
44
45 unsigned int _qDim;
46
47
48 SP::SiconosVector _dotq;
49
50
51 SP::SiconosVector _acceleration;
52
53
54 SiconosMemory _twistMemory;
55 SiconosMemory _qMemory;
56 SiconosMemory _forcesMemory;
57 SiconosMemory _dotqMemory;
58
59
60 SP::SiconosMatrix _I;
61
62
63 double _scalarMass;
64
65
66 SP::SimpleMatrix _T;
67
68
69 SP::SimpleMatrix _Tdot;
70
71
72 SP::SiconosVector _fExt;
73
74
75 bool _hasConstantFExt;
76
77
78 SP::SiconosVector _fInt;
79
80
81 SP::SiconosVector _mExt;
82
83
84 bool _hasConstantMExt;
85
86
87 bool _isMextExpressedInInertialFrame;
88
89
90
91
92
93 SP::SiconosVector _mInt;
94
95
96 SP::SimpleMatrix _jacobianFIntq;
97
98
99 SP::SimpleMatrix _jacobianFInttwist;
100
101
102 SP::SimpleMatrix _jacobianMIntq;
103
104
105 SP::SimpleMatrix _jacobianMInttwist;
106
107
108 SP::SimpleMatrix _jacobianMExtq;
109
110
111 SP::SiconosVector _mGyr;
112
113
114 SP::SimpleMatrix _jacobianMGyrtwist;
115
116
117
118 SP::SiconosVector _wrench;
119
120
121 SP::SimpleMatrix _jacobianWrenchq;
122
123
124 SP::SimpleMatrix _jacobianWrenchTwist;
125
126
127 bool _nullifyMGyr;
128
129
130 bool _computeJacobianFIntqByFD;
131
132
133 bool _computeJacobianFInttwistByFD;
134
135
136 bool _computeJacobianMIntqByFD;
137
138
139 bool _computeJacobianMInttwistByFD;
140
141
142 double _epsilonFD;
143
144
145 SP::PluggedObject _pluginFExt;
146
147
148 SP::PluggedObject _pluginMExt;
149
150
151 SP::PluggedObject _pluginFInt;
152
153
154 SP::PluggedObject _pluginMInt;
155
156
157
158
159
160
161
162
163 SP::PluggedObject _pluginJacqFInt;
164
165
166 SP::PluggedObject _pluginJactwistFInt;
167
168
169 SP::PluggedObject _pluginJacqMInt;
170
171
172 SP::PluggedObject _pluginJactwistMInt;
173
174 enum NewtonEulerDSRhsMatrices {
175 jacobianXBloc00,
176 jacobianXBloc01,
177 jacobianXBloc10,
178 jacobianXBloc11,
179 zeroMatrix,
180 zeroMatrixqDim,
181 numberOfRhsMatrices
182 };
183
184
185 VectorOfSMatrices _rhsMatrices;
186
187
188 NewtonEulerDS();
189
190
191 void _zeroPlugin() override;
192
193public:
194
195
196
197 NewtonEulerDS(SP::SiconosVector position, SP::SiconosVector twist,
198 double mass, SP::SiconosMatrix inertia);
199
200
201 virtual ~NewtonEulerDS();
202
203
204 void resetToInitialState() override;
205
206
207 void initRhs(double time) override;
208
209
210 void initializeNonSmoothInput(unsigned int level) override;
211
212
213 void computeRhs(double time) override;
214
215
216 void computeJacobianRhsx(double time) override;
217
218
219 void resetAllNonSmoothParts() override;
220
221
222 void resetNonSmoothPart(unsigned int level) override;
223
224
225
226 inline SP::SiconosVector forces() const override { return _wrench; }
227
228
229
230
231 inline SP::SiconosMatrix jacobianqForces() const override
232 {
233 return _jacobianWrenchq;
234 }
235
236
237 inline SP::SiconosMatrix jacobianvForces() const override
238 {
239 return _jacobianWrenchTwist;
240 }
241
242
243 virtual inline unsigned int getqDim() const { return _qDim; }
244
245
246
247
248 inline SP::SiconosVector q() const override { return _q; }
249
250
251 void setQ(const SiconosVector &newValue) override;
252
253
254 void setQPtr(SP::SiconosVector newPtr) override;
255
256
257 void setQ0(const SiconosVector &newValue) override;
258
259
260 void setQ0Ptr(SP::SiconosVector newPtr) override;
261
262
263
264
265 inline SP::SiconosVector twist() const { return _twist; }
266
267
268 inline SP::SiconosVector velocity() const override { return _twist; }
269
270 inline SP::SiconosVector twist0() const { return _twist0; }
271
272 inline SP::SiconosVector velocity0() const override { return _twist0; }
273
274 void setVelocity(const SiconosVector &newValue) override;
275
276
277 void setVelocityPtr(SP::SiconosVector newPtr) override;
278
279
280 void setVelocity0(const SiconosVector &newValue) override;
281
282
283 void setVelocity0Ptr(SP::SiconosVector newPtr) override;
284
285
286 SP::SiconosVector acceleration() const override { return _acceleration; };
287
288
289 void computeMass() override{};
290
291
292 void computeMass(SP::SiconosVector position) override{};
293
294
295 SP::SiconosVector linearVelocity(bool absoluteRef) const;
296
297
298 void linearVelocity(bool absoluteRef, SiconosVector &v) const;
299
300
301 SP::SiconosVector angularVelocity(bool absoluteRef) const;
302
303
304 void angularVelocity(bool absoluteRef, SiconosVector &w) const;
305
306
307
308
309
310
311 inline double scalarMass() const { return _scalarMass; };
312
313
314 void setScalarMass(double mass)
315 {
316 _scalarMass = mass;
317 updateMassMatrix();
318 };
319
320
321 SP::SiconosMatrix inertia() { return _I; };
322
323
324 void setInertia(SP::SiconosMatrix newInertia)
325 {
326 _I = newInertia;
327 updateMassMatrix();
328 }
329
330
331 void setInertia(double ix, double iy, double iz);
332
333
334 void updateMassMatrix();
335
336
337
338 inline SP::SiconosVector fExt() const { return _fExt; }
339
340
341 inline void setFExtPtr(SP::SiconosVector newPtr)
342 {
343 _fExt = newPtr;
344 _hasConstantFExt = true;
345 }
346
347
348 inline SP::SiconosVector mExt() const { return _mExt; }
349
350
351 inline void setMExtPtr(SP::SiconosVector newPtr)
352 {
353 _mExt = newPtr;
354 _hasConstantMExt = true;
355 }
356
357
358 inline SP::SiconosVector mGyr() const { return _mGyr; }
359
360 inline SP::SimpleMatrix T() { return _T; }
361 inline SP::SimpleMatrix Tdot()
362 {
363 assert(_Tdot);
364 return _Tdot;
365 }
366
367 inline SP::SiconosVector dotq() { return _dotq; }
368
369
370 inline const SiconosMemory &qMemory() override { return _qMemory; }
371
372
373 inline const SiconosMemory &twistMemory() { return _twistMemory; }
374
375 inline const SiconosMemory &velocityMemory() override { return _twistMemory; }
376
377
378 void initMemory(unsigned int steps) override;
379
380
381 void swapInMemory() override;
382
383 inline const SiconosMemory &forcesMemory() override { return _forcesMemory; }
384
385 inline const SiconosMemory &dotqMemory() { return _dotqMemory; }
386
387
388 double computeKineticEnergy();
389
390
391
392
393 void display(bool brief = true) const override;
394
395
396
397 inline void setIsMextExpressedInInertialFrame(bool value)
398 {
399 _isMextExpressedInInertialFrame = value;
400 if (!_jacobianMExtq)
401 _jacobianMExtq.reset(new SimpleMatrix(3, _qDim));
402 if (!_jacobianWrenchq)
403 _jacobianWrenchq.reset(new SimpleMatrix(_ndof, _qDim));
404 }
405
406 inline void setNullifyMGyr(bool value) { _nullifyMGyr = value; }
407
408 virtual void normalizeq();
409
410
411 void init_inverse_mass() override;
412
413
414 void update_inverse_mass() override;
415
416
417 inline void setComputeJacobianFIntqByFD(bool value)
418 {
419 _computeJacobianFIntqByFD = value;
420 }
421 inline void setComputeJacobianFIntvByFD(bool value)
422 {
423 _computeJacobianFInttwistByFD = value;
424 }
425 inline void setComputeJacobianMIntqByFD(bool value)
426 {
427 _computeJacobianMIntqByFD = value;
428 }
429 inline void setComputeJacobianMIntvByFD(bool value)
430 {
431 _computeJacobianMInttwistByFD = value;
432 }
433
434
435 void setComputeFExtFunction(const std::string &pluginPath,
436 const std::string &functionName)
437 {
438 _pluginFExt->setComputeFunction(pluginPath, functionName);
439 if (!_fExt)
440 _fExt.reset(new SiconosVector(3, 0));
441 _hasConstantFExt = false;
442 }
443
444
445 void setComputeMExtFunction(const std::string &pluginPath,
446 const std::string &functionName)
447 {
448 _pluginMExt->setComputeFunction(pluginPath, functionName);
449 if (!_mExt)
450 _mExt.reset(new SiconosVector(3, 0));
451 _hasConstantMExt = false;
452 }
453
454
455 void setComputeFExtFunction(FExt_NE fct)
456 {
457 _pluginFExt->setComputeFunction((void *)fct);
458 if (!_fExt)
459 _fExt.reset(new SiconosVector(3, 0));
460 _hasConstantFExt = false;
461 }
462
463
464 void setComputeMExtFunction(FExt_NE fct)
465 {
466 _pluginMExt->setComputeFunction((void *)fct);
467 if (!_mExt)
468 _mExt.reset(new SiconosVector(3, 0));
469 _hasConstantMExt = false;
470 }
471
472
473 void setComputeFIntFunction(const std::string &pluginPath,
474 const std::string &functionName)
475 {
476 _pluginFInt->setComputeFunction(pluginPath, functionName);
477 if (!_fInt)
478 _fInt.reset(new SiconosVector(3, 0));
479 }
480
481 void setComputeMIntFunction(const std::string &pluginPath,
482 const std::string &functionName)
483 {
484 _pluginMInt->setComputeFunction(pluginPath, functionName);
485 if (!_mInt)
486 _mInt.reset(new SiconosVector(3, 0));
487 }
488
489
490 void setComputeFIntFunction(FInt_NE fct)
491 {
492 _pluginFInt->setComputeFunction((void *)fct);
493 if (!_fInt)
494 _fInt.reset(new SiconosVector(3, 0));
495 }
496
497
498 void setComputeMIntFunction(FInt_NE fct)
499 {
500 _pluginMInt->setComputeFunction((void *)fct);
501 if (!_mInt)
502 _mInt.reset(new SiconosVector(3, 0));
503 }
504
505
506 void setComputeJacobianFIntqFunction(const std::string &pluginPath,
507 const std::string &functionName);
508
509
510 void setComputeJacobianFIntvFunction(const std::string &pluginPath,
511 const std::string &functionName);
512
513
514 void setComputeJacobianFIntqFunction(FInt_NE fct);
515
516
517 void setComputeJacobianFIntvFunction(FInt_NE fct);
518
519
520 void setComputeJacobianMIntqFunction(const std::string &pluginPath,
521 const std::string &functionName);
522
523 void setComputeJacobianMIntvFunction(const std::string &pluginPath,
524 const std::string &functionName);
525
526
527 void setComputeJacobianMIntqFunction(FInt_NE fct);
528
529
530 void setComputeJacobianMIntvFunction(FInt_NE fct);
531
532
533 virtual void computeFExt(double time);
534
535
536 virtual void computeFExt(double time, SP::SiconosVector fExt);
537
538
539 virtual void computeMExt(double time, SP::SiconosVector mExt);
540
541 virtual void computeMExt(double time);
542
543
544 void addExtForceAtPos(SP::SiconosVector force, bool forceAbsRef,
545 SP::SiconosVector pos = SP::SiconosVector(),
546 bool posAbsRef = false);
547
548 void computeJacobianMExtqExpressedInInertialFrameByFD(double time,
549 SP::SiconosVector q);
550 void computeJacobianMExtqExpressedInInertialFrame(double time,
551 SP::SiconosVector q);
552
553
554
555
556
557 void computeFInt(double time, SP::SiconosVector q, SP::SiconosVector v);
558
559
560 virtual void computeFInt(double time, SP::SiconosVector q,
561 SP::SiconosVector v, SP::SiconosVector fInt);
562
563
564 void computeMInt(double time, SP::SiconosVector q, SP::SiconosVector v);
565
566
567 virtual void computeMInt(double time, SP::SiconosVector q,
568 SP::SiconosVector v, SP::SiconosVector mInt);
569
570
571 void updatePlugins(double time) override{};
572
573
574 void init_forces() override;
575
576
577 virtual void computeForces(double time);
578
579
580 void computeForces(double time, SP::SiconosVector q, SP::SiconosVector twist) override;
581
582
583 void computeJacobianqForces(double time) override;
584
585
586 void computeJacobianvForces(double time) override;
587
588
589 virtual void computeMGyr(SP::SiconosVector twist);
590
591
592 virtual void computeMGyr(SP::SiconosVector twist, SP::SiconosVector mGyr);
593
594
595 virtual void computeJacobianMGyrtwist(double time);
596
597
598 virtual void computeJacobianMGyrtwistByFD(double time, SP::SiconosVector q,
599 SP::SiconosVector twist);
600
601
602
603
604
605
606
607 void computeJacobianFIntq(double time);
608
609
610 void computeJacobianFIntv(double time);
611
612
613 virtual void computeJacobianFIntq(double time, SP::SiconosVector position,
614 SP::SiconosVector twist);
615
616 void computeJacobianFIntqByFD(double time, SP::SiconosVector position,
617 SP::SiconosVector twist);
618
619
620 virtual void computeJacobianFIntv(double time, SP::SiconosVector position,
621 SP::SiconosVector twist);
622
623
624 void computeJacobianFIntvByFD(double time, SP::SiconosVector position,
625 SP::SiconosVector twist);
626
627
628 virtual void computeJacobianMIntq(double time);
629
630
631 virtual void computeJacobianMIntv(double time);
632
633
634 virtual void computeJacobianMIntq(double time, SP::SiconosVector position,
635 SP::SiconosVector twist);
636
637
638 void computeJacobianMIntqByFD(double time, SP::SiconosVector position,
639 SP::SiconosVector twist);
640
641
642 virtual void computeJacobianMIntv(double time, SP::SiconosVector position,
643 SP::SiconosVector twist);
644
645
646 void computeJacobianMIntvByFD(double time, SP::SiconosVector position,
647 SP::SiconosVector twist);
648
649 virtual void computeT();
650
651 virtual void computeTdot();
652
653 ACCEPT_STD_VISITORS();
654};
655#endif