aboutsummaryrefslogtreecommitdiffstatshomepage
path: root/libraries/ode-0.9/ode/src/export-dif.cpp
diff options
context:
space:
mode:
authordan miller2007-10-21 08:36:32 +0000
committerdan miller2007-10-21 08:36:32 +0000
commit2f8d7092bc2c9609fa98d6888106b96f38b22828 (patch)
treeda6c37579258cc965b52a75aee6135fe44237698 /libraries/ode-0.9/ode/src/export-dif.cpp
parent* Committing new PolicyManager based on an ACL system. (diff)
downloadopensim-SC-2f8d7092bc2c9609fa98d6888106b96f38b22828.zip
opensim-SC-2f8d7092bc2c9609fa98d6888106b96f38b22828.tar.gz
opensim-SC-2f8d7092bc2c9609fa98d6888106b96f38b22828.tar.bz2
opensim-SC-2f8d7092bc2c9609fa98d6888106b96f38b22828.tar.xz
libraries moved to opensim-libs, a new repository
Diffstat (limited to 'libraries/ode-0.9/ode/src/export-dif.cpp')
-rw-r--r--libraries/ode-0.9/ode/src/export-dif.cpp568
1 files changed, 0 insertions, 568 deletions
diff --git a/libraries/ode-0.9/ode/src/export-dif.cpp b/libraries/ode-0.9/ode/src/export-dif.cpp
deleted file mode 100644
index 577d3e1..0000000
--- a/libraries/ode-0.9/ode/src/export-dif.cpp
+++ /dev/null
@@ -1,568 +0,0 @@
1/*************************************************************************
2 * *
3 * Open Dynamics Engine, Copyright (C) 2001,2002 Russell L. Smith. *
4 * All rights reserved. Email: russ@q12.org Web: www.q12.org *
5 * *
6 * This library is free software; you can redistribute it and/or *
7 * modify it under the terms of EITHER: *
8 * (1) The GNU Lesser General Public License as published by the Free *
9 * Software Foundation; either version 2.1 of the License, or (at *
10 * your option) any later version. The text of the GNU Lesser *
11 * General Public License is included with this library in the *
12 * file LICENSE.TXT. *
13 * (2) The BSD-style license that is included with this library in *
14 * the file LICENSE-BSD.TXT. *
15 * *
16 * This library is distributed in the hope that it will be useful, *
17 * but WITHOUT ANY WARRANTY; without even the implied warranty of *
18 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the files *
19 * LICENSE.TXT and LICENSE-BSD.TXT for more details. *
20 * *
21 *************************************************************************/
22
23/*
24 * Export a DIF (Dynamics Interchange Format) file.
25 */
26
27
28// @@@ TODO:
29// * export all spaces, and geoms in spaces, not just ones attached to bodies
30// (separate export function?)
31// * say the space each geom is in, so reader can construct space heirarchy
32// * limot --> separate out into limits and motors?
33// * make sure ODE-specific parameters divided out
34
35
36#include "ode/ode.h"
37#include "objects.h"
38#include "joint.h"
39#include "collision_kernel.h"
40
41//***************************************************************************
42// utility
43
44struct PrintingContext {
45 FILE *file; // file to write to
46 int precision; // digits of precision to print
47 int indent; // number of levels of indent
48
49 void printIndent();
50 void printReal (dReal x);
51 void print (const char *name, int x);
52 void print (const char *name, dReal x);
53 void print (const char *name, const dReal *x, int n=3);
54 void print (const char *name, const char *x=0);
55 void printNonzero (const char *name, dReal x);
56 void printNonzero (const char *name, const dReal x[3]);
57};
58
59
60void PrintingContext::printIndent()
61{
62 for (int i=0; i<indent; i++) fputc ('\t',file);
63}
64
65
66void PrintingContext::print (const char *name, int x)
67{
68 printIndent();
69 fprintf (file,"%s = %d,\n",name,x);
70}
71
72
73void PrintingContext::printReal (dReal x)
74{
75 if (x==dInfinity) {
76 fprintf (file,"inf");
77 }
78 else if (x==-dInfinity) {
79 fprintf (file,"-inf");
80 }
81 else {
82 fprintf (file,"%.*g",precision,x);
83 }
84}
85
86
87void PrintingContext::print (const char *name, dReal x)
88{
89 printIndent();
90 fprintf (file,"%s = ",name);
91 printReal (x);
92 fprintf (file,",\n");
93}
94
95
96void PrintingContext::print (const char *name, const dReal *x, int n)
97{
98 printIndent();
99 fprintf (file,"%s = {",name);
100 for (int i=0; i<n; i++) {
101 printReal (x[i]);
102 if (i < n-1) fputc (',',file);
103 }
104 fprintf (file,"},\n");
105}
106
107
108void PrintingContext::print (const char *name, const char *x)
109{
110 printIndent();
111 if (x) {
112 fprintf (file,"%s = \"%s\",\n",name,x);
113 }
114 else {
115 fprintf (file,"%s\n",name);
116 }
117}
118
119
120void PrintingContext::printNonzero (const char *name, dReal x)
121{
122 if (x != 0) print (name,x);
123}
124
125
126void PrintingContext::printNonzero (const char *name, const dReal x[3])
127{
128 if (x[0] != 0 && x[1] != 0 && x[2] != 0) print (name,x);
129}
130
131//***************************************************************************
132// joints
133
134
135static void printLimot (PrintingContext &c, dxJointLimitMotor &limot, int num)
136{
137 if (num >= 0) {
138 c.printIndent();
139 fprintf (c.file,"limit%d = {\n",num);
140 }
141 else {
142 c.print ("limit = {");
143 }
144 c.indent++;
145 c.print ("low_stop",limot.lostop);
146 c.print ("high_stop",limot.histop);
147 c.printNonzero ("bounce",limot.bounce);
148 c.print ("ODE = {");
149 c.indent++;
150 c.printNonzero ("stop_erp",limot.stop_erp);
151 c.printNonzero ("stop_cfm",limot.stop_cfm);
152 c.indent--;
153 c.print ("},");
154 c.indent--;
155 c.print ("},");
156
157 if (num >= 0) {
158 c.printIndent();
159 fprintf (c.file,"motor%d = {\n",num);
160 }
161 else {
162 c.print ("motor = {");
163 }
164 c.indent++;
165 c.printNonzero ("vel",limot.vel);
166 c.printNonzero ("fmax",limot.fmax);
167 c.print ("ODE = {");
168 c.indent++;
169 c.printNonzero ("fudge_factor",limot.fudge_factor);
170 c.printNonzero ("normal_cfm",limot.normal_cfm);
171 c.indent--;
172 c.print ("},");
173 c.indent--;
174 c.print ("},");
175}
176
177
178static const char *getJointName (dxJoint *j)
179{
180 switch (j->vtable->typenum) {
181 case dJointTypeBall: return "ball";
182 case dJointTypeHinge: return "hinge";
183 case dJointTypeSlider: return "slider";
184 case dJointTypeContact: return "contact";
185 case dJointTypeUniversal: return "universal";
186 case dJointTypeHinge2: return "ODE_hinge2";
187 case dJointTypeFixed: return "fixed";
188 case dJointTypeNull: return "null";
189 case dJointTypeAMotor: return "ODE_angular_motor";
190 case dJointTypeLMotor: return "ODE_linear_motor";
191 case dJointTypePR: return "PR";
192 }
193 return "unknown";
194}
195
196
197static void printBall (PrintingContext &c, dxJoint *j)
198{
199 dxJointBall *b = (dxJointBall*) j;
200 c.print ("anchor1",b->anchor1);
201 c.print ("anchor2",b->anchor2);
202}
203
204
205static void printHinge (PrintingContext &c, dxJoint *j)
206{
207 dxJointHinge *h = (dxJointHinge*) j;
208 c.print ("anchor1",h->anchor1);
209 c.print ("anchor2",h->anchor2);
210 c.print ("axis1",h->axis1);
211 c.print ("axis2",h->axis2);
212 c.print ("qrel",h->qrel,4);
213 printLimot (c,h->limot,-1);
214}
215
216
217static void printSlider (PrintingContext &c, dxJoint *j)
218{
219 dxJointSlider *s = (dxJointSlider*) j;
220 c.print ("axis1",s->axis1);
221 c.print ("qrel",s->qrel,4);
222 c.print ("offset",s->offset);
223 printLimot (c,s->limot,-1);
224}
225
226
227static void printContact (PrintingContext &c, dxJoint *j)
228{
229 dxJointContact *ct = (dxJointContact*) j;
230 int mode = ct->contact.surface.mode;
231 c.print ("pos",ct->contact.geom.pos);
232 c.print ("normal",ct->contact.geom.normal);
233 c.print ("depth",ct->contact.geom.depth);
234 //@@@ may want to write the geoms g1 and g2 that are involved, for debugging.
235 // to do this we must have written out all geoms in all spaces, not just
236 // geoms that are attached to bodies.
237 c.print ("mu",ct->contact.surface.mu);
238 if (mode & dContactMu2) c.print ("mu2",ct->contact.surface.mu2);
239 if (mode & dContactBounce) c.print ("bounce",ct->contact.surface.bounce);
240 if (mode & dContactBounce) c.print ("bounce_vel",ct->contact.surface.bounce_vel);
241 if (mode & dContactSoftERP) c.print ("soft_ERP",ct->contact.surface.soft_erp);
242 if (mode & dContactSoftCFM) c.print ("soft_CFM",ct->contact.surface.soft_cfm);
243 if (mode & dContactMotion1) c.print ("motion1",ct->contact.surface.motion1);
244 if (mode & dContactMotion2) c.print ("motion2",ct->contact.surface.motion2);
245 if (mode & dContactSlip1) c.print ("slip1",ct->contact.surface.slip1);
246 if (mode & dContactSlip2) c.print ("slip2",ct->contact.surface.slip2);
247 int fa = 0; // friction approximation code
248 if (mode & dContactApprox1_1) fa |= 1;
249 if (mode & dContactApprox1_2) fa |= 2;
250 if (fa) c.print ("friction_approximation",fa);
251 if (mode & dContactFDir1) c.print ("fdir1",ct->contact.fdir1);
252}
253
254
255static void printUniversal (PrintingContext &c, dxJoint *j)
256{
257 dxJointUniversal *u = (dxJointUniversal*) j;
258 c.print ("anchor1",u->anchor1);
259 c.print ("anchor2",u->anchor2);
260 c.print ("axis1",u->axis1);
261 c.print ("axis2",u->axis2);
262 c.print ("qrel1",u->qrel1,4);
263 c.print ("qrel2",u->qrel2,4);
264 printLimot (c,u->limot1,1);
265 printLimot (c,u->limot2,2);
266}
267
268
269static void printHinge2 (PrintingContext &c, dxJoint *j)
270{
271 dxJointHinge2 *h = (dxJointHinge2*) j;
272 c.print ("anchor1",h->anchor1);
273 c.print ("anchor2",h->anchor2);
274 c.print ("axis1",h->axis1);
275 c.print ("axis2",h->axis2);
276 c.print ("v1",h->v1); //@@@ much better to write out 'qrel' here, if it's available
277 c.print ("v2",h->v2);
278 c.print ("susp_erp",h->susp_erp);
279 c.print ("susp_cfm",h->susp_cfm);
280 printLimot (c,h->limot1,1);
281 printLimot (c,h->limot2,2);
282}
283
284static void printPR (PrintingContext &c, dxJoint *j)
285{
286 dxJointPR *pr = (dxJointPR*) j;
287 c.print ("anchor2",pr->anchor2);
288 c.print ("axisR1",pr->axisR1);
289 c.print ("axisR2",pr->axisR2);
290 c.print ("axisP1",pr->axisP1);
291 c.print ("qrel",pr->qrel,4);
292 c.print ("offset",pr->offset);
293 printLimot (c,pr->limotP,1);
294 printLimot (c,pr->limotR,2);
295}
296
297
298static void printFixed (PrintingContext &c, dxJoint *j)
299{
300 dxJointFixed *f = (dxJointFixed*) j;
301 c.print ("qrel",f->qrel);
302 c.print ("offset",f->offset);
303}
304
305static void printLMotor (PrintingContext &c, dxJoint *j)
306{
307 dxJointLMotor *a = (dxJointLMotor*) j;
308 c.print("num", a->num);
309 c.printIndent();
310 fprintf (c.file,"rel = {%d,%d,%d},\n",a->rel[0],a->rel[1],a->rel[2]);
311 c.print ("axis1",a->axis[0]);
312 c.print ("axis2",a->axis[1]);
313 c.print ("axis3",a->axis[2]);
314 for (int i=0; i<3; i++) printLimot (c,a->limot[i],i+1);
315}
316
317
318static void printAMotor (PrintingContext &c, dxJoint *j)
319{
320 dxJointAMotor *a = (dxJointAMotor*) j;
321 c.print ("num",a->num);
322 c.print ("mode",a->mode);
323 c.printIndent();
324 fprintf (c.file,"rel = {%d,%d,%d},\n",a->rel[0],a->rel[1],a->rel[2]);
325 c.print ("axis1",a->axis[0]);
326 c.print ("axis2",a->axis[1]);
327 c.print ("axis3",a->axis[2]);
328 for (int i=0; i<3; i++) printLimot (c,a->limot[i],i+1);
329 c.print ("angle1",a->angle[0]);
330 c.print ("angle2",a->angle[1]);
331 c.print ("angle3",a->angle[2]);
332}
333
334//***************************************************************************
335// geometry
336
337static void printGeom (PrintingContext &c, dxGeom *g);
338
339static void printSphere (PrintingContext &c, dxGeom *g)
340{
341 c.print ("type","sphere");
342 c.print ("radius",dGeomSphereGetRadius (g));
343}
344
345
346static void printBox (PrintingContext &c, dxGeom *g)
347{
348 dVector3 sides;
349 dGeomBoxGetLengths (g,sides);
350 c.print ("type","box");
351 c.print ("sides",sides);
352}
353
354
355
356static void printCapsule (PrintingContext &c, dxGeom *g)
357{
358 dReal radius,length;
359 dGeomCapsuleGetParams (g,&radius,&length);
360 c.print ("type","capsule");
361 c.print ("radius",radius);
362 c.print ("length",length);
363}
364
365
366static void printPlane (PrintingContext &c, dxGeom *g)
367{
368 dVector4 e;
369 dGeomPlaneGetParams (g,e);
370 c.print ("type","plane");
371 c.print ("normal",e);
372 c.print ("d",e[3]);
373}
374
375
376
377static void printRay (PrintingContext &c, dxGeom *g)
378{
379 dReal length = dGeomRayGetLength (g);
380 c.print ("type","ray");
381 c.print ("length",length);
382}
383
384
385
386static void printGeomTransform (PrintingContext &c, dxGeom *g)
387{
388 dxGeom *g2 = dGeomTransformGetGeom (g);
389 const dReal *pos = dGeomGetPosition (g2);
390 dQuaternion q;
391 dGeomGetQuaternion (g2,q);
392 c.print ("type","transform");
393 c.print ("pos",pos);
394 c.print ("q",q,4);
395 c.print ("geometry = {");
396 c.indent++;
397 printGeom (c,g2);
398 c.indent--;
399 c.print ("}");
400}
401
402
403
404static void printTriMesh (PrintingContext &c, dxGeom *g)
405{
406 c.print ("type","trimesh");
407 //@@@ i don't think that the trimesh accessor functions are really
408 // sufficient to read out all the triangle data, and anyway we
409 // should have a method of not duplicating trimesh data that is
410 // shared.
411}
412
413
414static void printGeom (PrintingContext &c, dxGeom *g)
415{
416 unsigned long category = dGeomGetCategoryBits (g);
417 if (category != (unsigned long)(~0)) {
418 c.printIndent();
419 fprintf (c.file,"category_bits = %lu\n",category);
420 }
421 unsigned long collide = dGeomGetCollideBits (g);
422 if (collide != (unsigned long)(~0)) {
423 c.printIndent();
424 fprintf (c.file,"collide_bits = %lu\n",collide);
425 }
426 if (!dGeomIsEnabled (g)) {
427 c.print ("disabled",1);
428 }
429 switch (g->type) {
430 case dSphereClass: printSphere (c,g); break;
431 case dBoxClass: printBox (c,g); break;
432 case dCapsuleClass: printCapsule (c,g); break;
433 case dPlaneClass: printPlane (c,g); break;
434 case dRayClass: printRay (c,g); break;
435 case dGeomTransformClass: printGeomTransform (c,g); break;
436 case dTriMeshClass: printTriMesh (c,g); break;
437 }
438}
439
440//***************************************************************************
441// world
442
443void dWorldExportDIF (dWorldID w, FILE *file, const char *prefix)
444{
445 PrintingContext c;
446 c.file = file;
447#if defined(dSINGLE)
448 c.precision = 7;
449#else
450 c.precision = 15;
451#endif
452 c.indent = 1;
453
454 fprintf (file,"-- Dynamics Interchange Format v0.1\n\n%sworld = dynamics.world {\n",prefix);
455 c.print ("gravity",w->gravity);
456 c.print ("ODE = {");
457 c.indent++;
458 c.print ("ERP",w->global_erp);
459 c.print ("CFM",w->global_cfm);
460 c.print ("auto_disable = {");
461 c.indent++;
462 c.print ("linear_threshold",w->adis.linear_average_threshold);
463 c.print ("angular_threshold",w->adis.angular_average_threshold);
464 c.print ("average_samples",(int)w->adis.average_samples);
465 c.print ("idle_time",w->adis.idle_time);
466 c.print ("idle_steps",w->adis.idle_steps);
467 fprintf (file,"\t\t},\n\t},\n}\n");
468 c.indent -= 3;
469
470 // bodies
471 int num = 0;
472 fprintf (file,"%sbody = {}\n",prefix);
473 for (dxBody *b=w->firstbody; b; b=(dxBody*)b->next) {
474 b->tag = num;
475 fprintf (file,"%sbody[%d] = dynamics.body {\n\tworld = %sworld,\n",prefix,num,prefix);
476 c.indent++;
477 c.print ("pos",b->posr.pos);
478 c.print ("q",b->q,4);
479 c.print ("lvel",b->lvel);
480 c.print ("avel",b->avel);
481 c.print ("mass",b->mass.mass);
482 fprintf (file,"\tI = {{");
483 for (int i=0; i<3; i++) {
484 for (int j=0; j<3; j++) {
485 c.printReal (b->mass.I[i*4+j]);
486 if (j < 2) fputc (',',file);
487 }
488 if (i < 2) fprintf (file,"},{");
489 }
490 fprintf (file,"}},\n");
491 c.printNonzero ("com",b->mass.c);
492 c.print ("ODE = {");
493 c.indent++;
494 if (b->flags & dxBodyFlagFiniteRotation) c.print ("finite_rotation",1);
495 if (b->flags & dxBodyDisabled) c.print ("disabled",1);
496 if (b->flags & dxBodyNoGravity) c.print ("no_gravity",1);
497 if (b->flags & dxBodyAutoDisable) {
498 c.print ("auto_disable = {");
499 c.indent++;
500 c.print ("linear_threshold",b->adis.linear_average_threshold);
501 c.print ("angular_threshold",b->adis.angular_average_threshold);
502 c.print ("average_samples",(int)b->adis.average_samples);
503 c.print ("idle_time",b->adis.idle_time);
504 c.print ("idle_steps",b->adis.idle_steps);
505 c.print ("time_left",b->adis_timeleft);
506 c.print ("steps_left",b->adis_stepsleft);
507 c.indent--;
508 c.print ("},");
509 }
510 c.printNonzero ("facc",b->facc);
511 c.printNonzero ("tacc",b->tacc);
512 if (b->flags & dxBodyFlagFiniteRotationAxis) {
513 c.print ("finite_rotation_axis",b->finite_rot_axis);
514 }
515 c.indent--;
516 c.print ("},");
517 if (b->geom) {
518 c.print ("geometry = {");
519 c.indent++;
520 for (dxGeom *g=b->geom; g; g=g->body_next) {
521 c.print ("{");
522 c.indent++;
523 printGeom (c,g);
524 c.indent--;
525 c.print ("},");
526 }
527 c.indent--;
528 c.print ("},");
529 }
530 c.indent--;
531 c.print ("}");
532 num++;
533 }
534
535 // joints
536 num = 0;
537 fprintf (file,"%sjoint = {}\n",prefix);
538 for (dxJoint *j=w->firstjoint; j; j=(dxJoint*)j->next) {
539 c.indent++;
540 const char *name = getJointName (j);
541 fprintf (file,
542 "%sjoint[%d] = dynamics.%s_joint {\n"
543 "\tworld = %sworld,\n"
544 "\tbody = {"
545 ,prefix,num,name,prefix);
546
547 if ( j->node[0].body )
548 fprintf (file,"%sbody[%d]",prefix,j->node[0].body->tag);
549 if ( j->node[1].body )
550 fprintf (file,",%sbody[%d]",prefix,j->node[1].body->tag);
551
552 switch (j->vtable->typenum) {
553 case dJointTypeBall: printBall (c,j); break;
554 case dJointTypeHinge: printHinge (c,j); break;
555 case dJointTypeSlider: printSlider (c,j); break;
556 case dJointTypeContact: printContact (c,j); break;
557 case dJointTypeUniversal: printUniversal (c,j); break;
558 case dJointTypeHinge2: printHinge2 (c,j); break;
559 case dJointTypeFixed: printFixed (c,j); break;
560 case dJointTypeAMotor: printAMotor (c,j); break;
561 case dJointTypeLMotor: printLMotor (c,j); break;
562 case dJointTypePR: printPR (c,j); break;
563 }
564 c.indent--;
565 c.print ("}");
566 num++;
567 }
568}
generated by cgit v1.1 at 2024-12-26 23:31:53 +0000