[216] | 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 | } |
---|