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 | |
---|
44 | struct 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 | |
---|
60 | void PrintingContext::printIndent() |
---|
61 | { |
---|
62 | for (int i=0; i<indent; i++) fputc ('\t',file); |
---|
63 | } |
---|
64 | |
---|
65 | |
---|
66 | void PrintingContext::print (const char *name, int x) |
---|
67 | { |
---|
68 | printIndent(); |
---|
69 | fprintf (file,"%s = %d,\n",name,x); |
---|
70 | } |
---|
71 | |
---|
72 | |
---|
73 | void 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 | |
---|
87 | void 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 | |
---|
96 | void 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 | |
---|
108 | void 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 | |
---|
120 | void PrintingContext::printNonzero (const char *name, dReal x) |
---|
121 | { |
---|
122 | if (x != 0) print (name,x); |
---|
123 | } |
---|
124 | |
---|
125 | |
---|
126 | void 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 | |
---|
135 | static 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 | |
---|
178 | static 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 | |
---|
197 | static 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 | |
---|
205 | static 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 | |
---|
217 | static 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 | |
---|
227 | static 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 | |
---|
255 | static 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 | |
---|
269 | static 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 | |
---|
284 | static 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 | |
---|
298 | static 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 | |
---|
305 | static 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 | |
---|
318 | static 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 | |
---|
337 | static void printGeom (PrintingContext &c, dxGeom *g); |
---|
338 | |
---|
339 | static void printSphere (PrintingContext &c, dxGeom *g) |
---|
340 | { |
---|
341 | c.print ("type","sphere"); |
---|
342 | c.print ("radius",dGeomSphereGetRadius (g)); |
---|
343 | } |
---|
344 | |
---|
345 | |
---|
346 | static 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 | |
---|
356 | static 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 | |
---|
366 | static 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 | |
---|
377 | static 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 | |
---|
386 | static 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 | |
---|
404 | static 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 | |
---|
414 | static 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 | |
---|
443 | void 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 | } |
---|