1 #include "SiconosConfig.h"
2 #include "MechanicsIO.hpp"
3 #include "SiconosAlgebraProd.hpp"
4
5 #define DUMMY(X, Y) class X : public Y {}
6
7 #undef BULLET_CLASSES
8 #undef OCC_CLASSES
9 #undef MECHANISMS_CLASSES
10
11 #include <RigidBodyDS.hpp>
12 #include <RigidBody2dDS.hpp>
13
14 #define BULLET_CLASSES() \
15 REGISTER(BulletR)\
16 REGISTER(Bullet5DR)\
17 REGISTER(Bullet2dR)\
18 REGISTER(Bullet2d3DR)
19
20
21 #ifdef SICONOS_HAS_BULLET
22 #include <BulletR.hpp>
23 #include <Bullet5DR.hpp>
24 #include <Bullet2dR.hpp>
25 #include <Bullet2d3DR.hpp>
26 #else
27 #include <NewtonEuler3DR.hpp>
28 #include <NewtonEuler5DR.hpp>
29 #include <SpaceFilter.hpp>
30 DUMMY(BulletR, NewtonEuler3DR);
31 DUMMY(Bullet5DR, NewtonEuler5DR);
32 #include <Lagrangian2d2DR.cpp>
33 #include <Lagrangian2d3DR.cpp>
34 DUMMY(Bullet2dR, Lagrangian2d2DR);
35 DUMMY(Bullet2d3DR, Lagrangian2d3DR);
36 #endif
37
38 #define OCC_CLASSES() \
39 REGISTER(OccBody) \
40 REGISTER(OccR)
41 #ifdef SICONOS_HAS_OCE
42 #include <OccBody.hpp>
43 #include <OccR.hpp>
44 #else
45 #include <NewtonEulerDS.hpp>
46 #include <NewtonEuler3DR.hpp>
47 DUMMY(OccBody, NewtonEulerDS);
48 DUMMY(OccR, NewtonEuler3DR);
49 #endif
50
51 #define MECHANISMS_CLASSES() \
52 REGISTER(MBTB_FC3DContactRelation) \
53 REGISTER(MBTB_ContactRelation)
54
55 #ifdef HAVE_SICONOS_MECHANISMS
56 #include <MBTB_FC3DContactRelation.hpp>
57 #include <MBTB_ContactRelation.hpp>
58 #else
59 #include <NewtonEuler3DR.hpp>
60 #include <NewtonEuler1DR.hpp>
61 DUMMY(MBTB_FC3DContactRelation, NewtonEuler3DR);
62 DUMMY(MBTB_ContactRelation, NewtonEuler1DR);
63 #endif
64
65
66 /* all the classes that may be visited */
67 #define VISITOR_CLASSES() \
68 REGISTER(DynamicalSystem) \
69 REGISTER(LagrangianDS) \
70 REGISTER(NewtonEulerDS) \
71 REGISTER(LagrangianR) \
72 REGISTER(Disk) \
73 REGISTER(Circle) \
74 REGISTER(Lagrangian2d2DR) \
75 REGISTER(Lagrangian2d3DR) \
76 REGISTER(NewtonEulerR) \
77 REGISTER(NewtonEuler1DR) \
78 REGISTER(NewtonEuler3DR) \
79 REGISTER(NewtonEuler5DR) \
80 REGISTER(PivotJointR) \
81 REGISTER(KneeJointR) \
82 REGISTER(PrismaticJointR) \
83 REGISTER(RigidBodyDS) \
84 REGISTER(RigidBody2dDS) \
85 MECHANISMS_CLASSES() \
86 OCC_CLASSES() \
87 BULLET_CLASSES()
88
89
90 #undef SICONOS_VISITABLES
91 #define SICONOS_VISITABLES() VISITOR_CLASSES()
92
93 #include <BlockVector.hpp>
94 #include <Question.hpp>
95
96 #include <LagrangianDS.hpp>
97 #include <NewtonEulerDS.hpp>
98
99 /* ... */
100 /* to be fixed: forward mess with mpl::is_base_of who needs fully
101 * declared classes */
102 #include <SiconosKernel.hpp>
103
104 /* Mechanics visitables bodies */
105 #include "Circle.hpp"
106 #include "Disk.hpp"
107 #include "DiskDiskR.hpp"
108 #include "CircleCircleR.hpp"
109 #include "DiskPlanR.hpp"
110 #include "DiskMovingPlanR.hpp"
111 #include "SphereLDS.hpp"
112 #include "SphereLDSSphereLDSR.hpp"
113 #include "SphereNEDSSphereNEDSR.hpp"
114 #include "SphereLDSPlanR.hpp"
115 #include "SphereNEDS.hpp"
116 #include "SphereNEDSPlanR.hpp"
117 #include "ExternalBody.hpp"
118
119 #include <PivotJointR.hpp>
120 #include <KneeJointR.hpp>
121 #include <PrismaticJointR.hpp>
122
123 /* all the visitable classes must have been included at this point */
124 #include <VisitorMaker.hpp>
125
126 //#define DEBUG_MESSAGES 1
127 #include "siconos_debug.h"
128
129 using namespace Experimental;
130
131 struct GetPosition : public SiconosVisitor
132 {
133
134 SP::SiconosVector result;
135
136 template<typename T>
operator ()GetPosition137 void operator()(const T& ds)
138 {
139 result.reset(new SiconosVector(1+ds.q()->size()));
140 result->setValue(0, ds.number());
141 result->setBlock(1, *ds.q());
142 }
143 };
144
145 struct GetVelocity : public SiconosVisitor
146 {
147
148 SP::SiconosVector result;
149
150 template<typename T>
operator ()GetVelocity151 void operator()(const T& ds)
152 {
153 result.reset(new SiconosVector(1+ds.velocity()->size()));
154 result->setValue(0, ds.number());
155 result->setBlock(1, *ds.velocity());
156 }
157 };
158
159 struct ForMu : public Question<double>
160 {
161 using SiconosVisitor::visit;
visitForMu162 void visit(const NewtonImpactFrictionNSL& nsl)
163 {
164 answer = nsl . mu();
165 }
visitForMu166 void visit(const NewtonImpactRollingFrictionNSL& nsl)
167 {
168 answer = nsl . mu();
169 }
visitForMu170 void visit(const NewtonImpactNSL& nsl)
171 {
172 answer = 0.;
173 }
174 };
175
176
177 /* Get contact informations */
178 /* default: a visitor that do nothing */
179 struct ContactPointVisitor : public SiconosVisitor
180 {
181 SP::Interaction inter;
182 SiconosVector answer;
183
184 template<typename T>
operator ()ContactPointVisitor185 void operator()(const T& rel)
186 {
187 }
188
189 };
190
191 /* then specializations : */
192 template<>
operator ()(const NewtonEuler3DR & rel)193 void ContactPointVisitor::operator()(const NewtonEuler3DR& rel)
194 {
195 const SiconosVector& posa = *rel.pc1();
196 const SiconosVector& posb = *rel.pc2();
197 const SiconosVector& nc = *rel.nc();
198 DEBUG_PRINTF("posa(0)=%g\n", posa(0));
199 DEBUG_PRINTF("posa(1)=%g\n", posa(1));
200 DEBUG_PRINTF("posa(2)=%g\n", posa(2));
201
202 double id = inter->number();
203 double mu = ask<ForMu>(*inter->nonSmoothLaw());
204 const SimpleMatrix& jachqT = *rel.jachqT();
205 SiconosVector cf(jachqT.size(1));
206 prod(*inter->lambda(1), jachqT, cf, true);
207 answer.resize(23);
208
209 answer.setValue(0, mu);
210 answer.setValue(1, posa(0));
211 answer.setValue(2, posa(1));
212 answer.setValue(3, posa(2));
213 answer.setValue(4, posb(0));
214 answer.setValue(5, posb(1));
215 answer.setValue(6, posb(2));
216 answer.setValue(7, nc(0));
217 answer.setValue(8, nc(1));
218 answer.setValue(9, nc(2));
219 answer.setValue(10, cf(0));
220 answer.setValue(11, cf(1));
221 answer.setValue(12, cf(2));
222 answer.setValue(13,inter->y(0)->getValue(0));
223 answer.setValue(14,inter->y(0)->getValue(1));
224 answer.setValue(15,inter->y(0)->getValue(2));
225 answer.setValue(16,inter->y(1)->getValue(0));
226 answer.setValue(17,inter->y(1)->getValue(1));
227 answer.setValue(18,inter->y(1)->getValue(2));
228 answer.setValue(19,inter->lambda(1)->getValue(0));
229 answer.setValue(20,inter->lambda(1)->getValue(1));
230 answer.setValue(21,inter->lambda(1)->getValue(2));
231 answer.setValue(22, id);
232 }
233 /* then specializations : */
234 template<>
operator ()(const NewtonEuler5DR & rel)235 void ContactPointVisitor::operator()(const NewtonEuler5DR& rel)
236 {
237 const SiconosVector& posa = *rel.pc1();
238 const SiconosVector& posb = *rel.pc2();
239 const SiconosVector& nc = *rel.nc();
240 DEBUG_PRINTF("posa(0)=%g\n", posa(0));
241 DEBUG_PRINTF("posa(1)=%g\n", posa(1));
242 DEBUG_PRINTF("posa(2)=%g\n", posa(2));
243
244 double id = inter->number();
245 double mu = ask<ForMu>(*inter->nonSmoothLaw());
246 const SimpleMatrix& jachqT = *rel.jachqT();
247 SiconosVector cf(jachqT.size(1));
248 prod(*inter->lambda(1), jachqT, cf, true);
249 answer.resize(23);
250
251 answer.setValue(0, mu);
252 answer.setValue(1, posa(0));
253 answer.setValue(2, posa(1));
254 answer.setValue(3, posa(2));
255 answer.setValue(4, posb(0));
256 answer.setValue(5, posb(1));
257 answer.setValue(6, posb(2));
258 answer.setValue(7, nc(0));
259 answer.setValue(8, nc(1));
260 answer.setValue(9, nc(2));
261 answer.setValue(10, cf(0));
262 answer.setValue(11, cf(1));
263 answer.setValue(12, cf(2));
264 answer.setValue(13,inter->y(0)->getValue(0));
265 answer.setValue(14,inter->y(0)->getValue(1));
266 answer.setValue(15,inter->y(0)->getValue(2));
267 answer.setValue(16,inter->y(1)->getValue(0));
268 answer.setValue(17,inter->y(1)->getValue(1));
269 answer.setValue(18,inter->y(1)->getValue(2));
270 answer.setValue(19,inter->lambda(1)->getValue(0));
271 answer.setValue(20,inter->lambda(1)->getValue(1));
272 answer.setValue(21,inter->lambda(1)->getValue(2));
273 answer.setValue(22, id);
274 }
275 template<>
operator ()(const Lagrangian2d2DR & rel)276 void ContactPointVisitor::operator()(const Lagrangian2d2DR& rel)
277 {
278 const SiconosVector& posa = *rel.pc1();
279 const SiconosVector& posb = *rel.pc2();
280 const SiconosVector& nc = *rel.nc();
281 DEBUG_PRINTF("posa(0)=%g\n", posa(0));
282 DEBUG_PRINTF("posa(1)=%g\n", posa(1));
283
284 double id = inter->number();
285 double mu = ask<ForMu>(*inter->nonSmoothLaw());
286 const SimpleMatrix& jachq = *rel.jachq();
287 SiconosVector cf(jachq.size(1));
288 prod(*inter->lambda(1), jachq, cf, true);
289
290
291 answer.resize(16);
292
293 answer.setValue(0, mu);
294 answer.setValue(1, posa(0));
295 answer.setValue(2, posa(1));
296
297 answer.setValue(3, posb(0));
298 answer.setValue(4, posb(1));
299
300 answer.setValue(5, nc(0));
301 answer.setValue(6, nc(1));
302
303 answer.setValue(7, cf(0));
304 answer.setValue(8, cf(1));
305
306 answer.setValue(9,inter->y(0)->getValue(0));
307 answer.setValue(10,inter->y(0)->getValue(1));
308
309 answer.setValue(11,inter->y(1)->getValue(0));
310 answer.setValue(12,inter->y(1)->getValue(1));
311
312 answer.setValue(13,inter->lambda(1)->getValue(0));
313 answer.setValue(14,inter->lambda(1)->getValue(1));
314
315 answer.setValue(15, id);
316 };
317
318 template<>
operator ()(const Lagrangian2d3DR & rel)319 void ContactPointVisitor::operator()(const Lagrangian2d3DR& rel)
320 {
321 const SiconosVector& posa = *rel.pc1();
322 const SiconosVector& posb = *rel.pc2();
323 const SiconosVector& nc = *rel.nc();
324 DEBUG_PRINTF("posa(0)=%g\n", posa(0));
325 DEBUG_PRINTF("posa(1)=%g\n", posa(1));
326
327 double id = inter->number();
328 double mu = ask<ForMu>(*inter->nonSmoothLaw());
329 const SimpleMatrix& jachq = *rel.jachq();
330 SiconosVector cf(jachq.size(1));
331 prod(*inter->lambda(1), jachq, cf, true);
332
333
334 answer.resize(16);
335
336 answer.setValue(0, mu);
337 answer.setValue(1, posa(0));
338 answer.setValue(2, posa(1));
339
340 answer.setValue(3, posb(0));
341 answer.setValue(4, posb(1));
342
343 answer.setValue(5, nc(0));
344 answer.setValue(6, nc(1));
345
346 answer.setValue(7, cf(0));
347 answer.setValue(8, cf(1));
348
349 answer.setValue(9,inter->y(0)->getValue(0));
350 answer.setValue(10,inter->y(0)->getValue(1));
351
352 answer.setValue(11,inter->y(1)->getValue(0));
353 answer.setValue(12,inter->y(1)->getValue(1));
354
355 answer.setValue(13,inter->lambda(1)->getValue(0));
356 answer.setValue(14,inter->lambda(1)->getValue(1));
357
358 answer.setValue(15, id);
359 };
360
361
362 struct ContactPointDomainVisitor : public SiconosVisitor
363 {
364 SP::Interaction inter;
365 SiconosVector answer;
366
367 template<typename T>
operator ()ContactPointDomainVisitor368 void operator()(const T& rel)
369 {
370 }
371
372 };
373
374 template<>
operator ()(const NewtonEuler3DR & rel)375 void ContactPointDomainVisitor::operator()(const NewtonEuler3DR& rel)
376 {
377 answer.resize(2);
378
379 /*
380 * TODO: contact point domain coloring (e.g. based on broadphase).
381 * currently, domain = (x>0):1?0
382 */
383 answer.setValue(0, rel.pc1()->getValue(0) > 0);
384
385 answer.setValue(1, inter->number());
386 }
387
388 template<typename T, typename G>
visitAllVerticesForVector(const G & graph) const389 SP::SimpleMatrix MechanicsIO::visitAllVerticesForVector(const G& graph) const
390 {
391 SP::SimpleMatrix result(new SimpleMatrix());
392 typename G::VIterator vi, viend;
393 unsigned int current_row;
394 for(current_row=0,std::tie(vi,viend)=graph.vertices();
395 vi!=viend; ++vi, ++current_row)
396 {
397 T getter;
398 graph.bundle(*vi)->accept(getter);
399 const SiconosVector& data = *getter.result;
400 result->resize(current_row+1, data.size());
401 result->setRow(current_row, data);
402 }
403 return result;
404 }
405
406 template<typename T, typename G>
visitAllVerticesForDouble(const G & graph) const407 SP::SiconosVector MechanicsIO::visitAllVerticesForDouble(const G& graph) const
408 {
409 SP::SiconosVector result(new SiconosVector(graph.vertices_number()));
410 typename G::VIterator vi, viend;
411 unsigned int current_row;
412 for(current_row=0,std::tie(vi,viend)=graph.vertices();
413 vi!=viend; ++vi, ++current_row)
414 {
415 T getter;
416 graph.bundle(*vi)->accept(getter);
417 result->setValue(current_row, *getter.result);
418 }
419 return result;
420 }
421
422
positions(const NonSmoothDynamicalSystem & nsds) const423 SP::SimpleMatrix MechanicsIO::positions(const NonSmoothDynamicalSystem& nsds) const
424 {
425
426 typedef
427 Visitor < Classes < LagrangianDS, NewtonEulerDS >,
428 GetPosition >::Make Getter;
429
430 return visitAllVerticesForVector<Getter>
431 (*(nsds.topology()->dSG(0)));
432 };
433
434
velocities(const NonSmoothDynamicalSystem & nsds) const435 SP::SimpleMatrix MechanicsIO::velocities(const NonSmoothDynamicalSystem& nsds) const
436 {
437 typedef
438 Visitor < Classes < LagrangianDS, NewtonEulerDS >,
439 GetVelocity>::Make Getter;
440
441 return visitAllVerticesForVector<Getter>
442 (*nsds.topology()->dSG(0));
443 }
444
contactPoints(const NonSmoothDynamicalSystem & nsds,unsigned int index_set) const445 SP::SimpleMatrix MechanicsIO::contactPoints(const NonSmoothDynamicalSystem& nsds,
446 unsigned int index_set) const
447 {
448 SP::SimpleMatrix result(new SimpleMatrix());
449 InteractionsGraph::VIterator vi, viend;
450 if(nsds.topology()->numberOfIndexSet() > 0)
451 {
452 InteractionsGraph& graph =
453 *nsds.topology()->indexSet(index_set);
454 unsigned int current_row;
455 result->resize(graph.vertices_number(), 25);
456
457 int data_size =0;
458 for(current_row=0, std::tie(vi,viend) = graph.vertices();
459 vi!=viend; ++vi)
460 {
461 DEBUG_PRINTF("process interaction : %p\n", &*graph.bundle(*vi));
462
463 /* create a visitor for specified classes */
464 typedef Visitor < Classes <
465 NewtonEuler1DR,
466 NewtonEuler3DR,
467 NewtonEuler5DR,
468 Lagrangian2d2DR,
469 Lagrangian2d3DR>,
470 ContactPointVisitor>::Make ContactPointInspector;
471 ContactPointInspector inspector;
472 inspector.inter = graph.bundle(*vi);
473 graph.bundle(*vi)->relation()->accept(inspector);
474 SiconosVector& data = inspector.answer;
475 data_size = data.size();
476
477 if(data_size ==0)
478 {
479 // Nothing is done since the relation does not appear as a relation
480 // related to a contact points (perhaps a joint)
481 }
482 else
483 {
484 // We add at the end the number of ds1 and ds2
485 data.resize(data_size+2);
486 DEBUG_EXPR(data.display(););
487 DynamicalSystem& ds1 = *graph.properties(*vi).source;
488 DynamicalSystem& ds2 = *graph.properties(*vi).target;
489
490 data.setValue(data_size, ds1.number());
491 data.setValue(data_size+1, ds2.number());
492 DEBUG_EXPR(data.display(););
493 if(result->size(1) != data.size())
494 {
495 result->resize(graph.vertices_number(), data.size());
496 }
497 result->setRow(current_row++, data);
498 data_size +=2;
499 }
500
501 }
502 result->resize(current_row, data_size);
503 DEBUG_EXPR(result->display(););
504 }
505
506 return result;
507 }
508
domains(const NonSmoothDynamicalSystem & nsds) const509 SP::SimpleMatrix MechanicsIO::domains(const NonSmoothDynamicalSystem& nsds) const
510 {
511 SP::SimpleMatrix result(new SimpleMatrix());
512 InteractionsGraph::VIterator vi, viend;
513 if(nsds.topology()->numberOfIndexSet() > 0)
514 {
515 InteractionsGraph& graph =
516 *nsds.topology()->indexSet(1);
517 unsigned int current_row;
518 result->resize(graph.vertices_number(), 2);
519 for(current_row=0, std::tie(vi,viend) = graph.vertices();
520 vi!=viend; ++vi, ++current_row)
521 {
522 DEBUG_PRINTF("process interaction : %p\n", &*graph.bundle(*vi));
523
524 typedef Visitor < Classes <
525 NewtonEuler1DR,
526 NewtonEuler3DR,
527 PrismaticJointR,
528 KneeJointR,
529 PivotJointR>,
530 ContactPointDomainVisitor>::Make DomainInspector;
531 DomainInspector inspector;
532 inspector.inter = graph.bundle(*vi);
533 graph.bundle(*vi)->relation()->accept(inspector);
534 const SiconosVector& data = inspector.answer;
535 if(data.size() == 2) result->setRow(current_row, data);
536 }
537 }
538 return result;
539 }
540
541
542
543