summaryrefslogtreecommitdiff
path: root/libs/ode-0.16.1/ode/src/export-dif.cpp
diff options
context:
space:
mode:
Diffstat (limited to 'libs/ode-0.16.1/ode/src/export-dif.cpp')
-rw-r--r--libs/ode-0.16.1/ode/src/export-dif.cpp620
1 files changed, 620 insertions, 0 deletions
diff --git a/libs/ode-0.16.1/ode/src/export-dif.cpp b/libs/ode-0.16.1/ode/src/export-dif.cpp
new file mode 100644
index 0000000..450021a
--- /dev/null
+++ b/libs/ode-0.16.1/ode/src/export-dif.cpp
@@ -0,0 +1,620 @@
+/*************************************************************************
+ * *
+ * Open Dynamics Engine, Copyright (C) 2001,2002 Russell L. Smith. *
+ * All rights reserved. Email: russ@q12.org Web: www.q12.org *
+ * *
+ * This library is free software; you can redistribute it and/or *
+ * modify it under the terms of EITHER: *
+ * (1) The GNU Lesser General Public License as published by the Free *
+ * Software Foundation; either version 2.1 of the License, or (at *
+ * your option) any later version. The text of the GNU Lesser *
+ * General Public License is included with this library in the *
+ * file LICENSE.TXT. *
+ * (2) The BSD-style license that is included with this library in *
+ * the file LICENSE-BSD.TXT. *
+ * *
+ * This library is distributed in the hope that it will be useful, *
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of *
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the files *
+ * LICENSE.TXT and LICENSE-BSD.TXT for more details. *
+ * *
+ *************************************************************************/
+
+/*
+ * Export a DIF (Dynamics Interchange Format) file.
+ */
+
+
+// @@@ TODO:
+// * export all spaces, and geoms in spaces, not just ones attached to bodies
+// (separate export function?)
+// * say the space each geom is in, so reader can construct space heirarchy
+// * limot --> separate out into limits and motors?
+// * make sure ODE-specific parameters divided out
+
+
+#include <ode/ode.h>
+#include "config.h"
+#include "objects.h"
+#include "joints/joints.h"
+#include "collision_kernel.h"
+
+//***************************************************************************
+// utility
+
+struct PrintingContext {
+ FILE *file; // file to write to
+ int precision; // digits of precision to print
+ int indent; // number of levels of indent
+
+ void printIndent();
+ void printReal (dReal x);
+ void print (const char *name, int x);
+ void print (const char *name, unsigned x);
+ void print (const char *name, dReal x);
+ void print (const char *name, const dReal *x, int n=3);
+ void print (const char *name, const char *x=0);
+ void printNonzero (const char *name, dReal x);
+ void printNonzero (const char *name, const dReal x[3]);
+};
+
+
+void PrintingContext::printIndent()
+{
+ for (int i=0; i<indent; i++) fputc ('\t',file);
+}
+
+
+void PrintingContext::print (const char *name, int x)
+{
+ printIndent();
+ fprintf (file,"%s = %d,\n",name,x);
+}
+
+void PrintingContext::print (const char *name, unsigned x)
+{
+ printIndent();
+ fprintf (file,"%s = %u,\n",name,x);
+}
+
+void PrintingContext::printReal (dReal x)
+{
+ if (x==dInfinity) {
+ fprintf (file,"inf");
+ }
+ else if (x==-dInfinity) {
+ fprintf (file,"-inf");
+ }
+ else {
+ fprintf (file,"%.*g",precision,x);
+ }
+}
+
+
+void PrintingContext::print (const char *name, dReal x)
+{
+ printIndent();
+ fprintf (file,"%s = ",name);
+ printReal (x);
+ fprintf (file,",\n");
+}
+
+
+void PrintingContext::print (const char *name, const dReal *x, int n)
+{
+ printIndent();
+ fprintf (file,"%s = {",name);
+ for (int i=0; i<n; i++) {
+ printReal (x[i]);
+ if (i < n-1) fputc (',',file);
+ }
+ fprintf (file,"},\n");
+}
+
+
+void PrintingContext::print (const char *name, const char *x)
+{
+ printIndent();
+ if (x) {
+ fprintf (file,"%s = \"%s\",\n",name,x);
+ }
+ else {
+ fprintf (file,"%s\n",name);
+ }
+}
+
+
+void PrintingContext::printNonzero (const char *name, dReal x)
+{
+ if (x != 0) print (name,x);
+}
+
+
+void PrintingContext::printNonzero (const char *name, const dReal x[3])
+{
+ if (x[0] != 0 && x[1] != 0 && x[2] != 0) print (name,x);
+}
+
+//***************************************************************************
+// joints
+
+
+static void printLimot (PrintingContext &c, dxJointLimitMotor &limot, int num)
+{
+ if (num >= 0) {
+ c.printIndent();
+ fprintf (c.file,"limit%d = {\n",num);
+ }
+ else {
+ c.print ("limit = {");
+ }
+ c.indent++;
+ c.print ("low_stop",limot.lostop);
+ c.print ("high_stop",limot.histop);
+ c.printNonzero ("bounce",limot.bounce);
+ c.print ("ODE = {");
+ c.indent++;
+ c.printNonzero ("stop_erp",limot.stop_erp);
+ c.printNonzero ("stop_cfm",limot.stop_cfm);
+ c.indent--;
+ c.print ("},");
+ c.indent--;
+ c.print ("},");
+
+ if (num >= 0) {
+ c.printIndent();
+ fprintf (c.file,"motor%d = {\n",num);
+ }
+ else {
+ c.print ("motor = {");
+ }
+ c.indent++;
+ c.printNonzero ("vel",limot.vel);
+ c.printNonzero ("fmax",limot.fmax);
+ c.print ("ODE = {");
+ c.indent++;
+ c.printNonzero ("fudge_factor",limot.fudge_factor);
+ c.printNonzero ("normal_cfm",limot.normal_cfm);
+ c.indent--;
+ c.print ("},");
+ c.indent--;
+ c.print ("},");
+}
+
+
+static const char *getJointName (dxJoint *j)
+{
+ switch (j->type()) {
+ case dJointTypeBall: return "ball";
+ case dJointTypeHinge: return "hinge";
+ case dJointTypeSlider: return "slider";
+ case dJointTypeContact: return "contact";
+ case dJointTypeUniversal: return "universal";
+ case dJointTypeHinge2: return "ODE_hinge2";
+ case dJointTypeFixed: return "fixed";
+ case dJointTypeNull: return "null";
+ case dJointTypeAMotor: return "ODE_angular_motor";
+ case dJointTypeLMotor: return "ODE_linear_motor";
+ case dJointTypePR: return "PR";
+ case dJointTypePU: return "PU";
+ case dJointTypePiston: return "piston";
+ default: return "unknown";
+ }
+}
+
+
+static void printBall (PrintingContext &c, dxJoint *j)
+{
+ dxJointBall *b = (dxJointBall*) j;
+ c.print ("anchor1",b->anchor1);
+ c.print ("anchor2",b->anchor2);
+}
+
+
+static void printHinge (PrintingContext &c, dxJoint *j)
+{
+ dxJointHinge *h = (dxJointHinge*) j;
+ c.print ("anchor1",h->anchor1);
+ c.print ("anchor2",h->anchor2);
+ c.print ("axis1",h->axis1);
+ c.print ("axis2",h->axis2);
+ c.print ("qrel",h->qrel,4);
+ printLimot (c,h->limot,-1);
+}
+
+
+static void printSlider (PrintingContext &c, dxJoint *j)
+{
+ dxJointSlider *s = (dxJointSlider*) j;
+ c.print ("axis1",s->axis1);
+ c.print ("qrel",s->qrel,4);
+ c.print ("offset",s->offset);
+ printLimot (c,s->limot,-1);
+}
+
+
+static void printContact (PrintingContext &c, dxJoint *j)
+{
+ dxJointContact *ct = (dxJointContact*) j;
+ int mode = ct->contact.surface.mode;
+ c.print ("pos",ct->contact.geom.pos);
+ c.print ("normal",ct->contact.geom.normal);
+ c.print ("depth",ct->contact.geom.depth);
+ //@@@ may want to write the geoms g1 and g2 that are involved, for debugging.
+ // to do this we must have written out all geoms in all spaces, not just
+ // geoms that are attached to bodies.
+ c.print ("mu",ct->contact.surface.mu);
+ if (mode & dContactMu2) c.print ("mu2",ct->contact.surface.mu2);
+ if (mode & dContactBounce) c.print ("bounce",ct->contact.surface.bounce);
+ if (mode & dContactBounce) c.print ("bounce_vel",ct->contact.surface.bounce_vel);
+ if (mode & dContactSoftERP) c.print ("soft_ERP",ct->contact.surface.soft_erp);
+ if (mode & dContactSoftCFM) c.print ("soft_CFM",ct->contact.surface.soft_cfm);
+ if (mode & dContactMotion1) c.print ("motion1",ct->contact.surface.motion1);
+ if (mode & dContactMotion2) c.print ("motion2",ct->contact.surface.motion2);
+ if (mode & dContactSlip1) c.print ("slip1",ct->contact.surface.slip1);
+ if (mode & dContactSlip2) c.print ("slip2",ct->contact.surface.slip2);
+ int fa = 0; // friction approximation code
+ if (mode & dContactApprox1_1) fa |= 1;
+ if (mode & dContactApprox1_2) fa |= 2;
+ if (fa) c.print ("friction_approximation",fa);
+ if (mode & dContactFDir1) c.print ("fdir1",ct->contact.fdir1);
+}
+
+
+static void printUniversal (PrintingContext &c, dxJoint *j)
+{
+ dxJointUniversal *u = (dxJointUniversal*) j;
+ c.print ("anchor1",u->anchor1);
+ c.print ("anchor2",u->anchor2);
+ c.print ("axis1",u->axis1);
+ c.print ("axis2",u->axis2);
+ c.print ("qrel1",u->qrel1,4);
+ c.print ("qrel2",u->qrel2,4);
+ printLimot (c,u->limot1,1);
+ printLimot (c,u->limot2,2);
+}
+
+
+static void printHinge2 (PrintingContext &c, dxJoint *j)
+{
+ dxJointHinge2 *h = (dxJointHinge2*) j;
+ c.print ("anchor1",h->anchor1);
+ c.print ("anchor2",h->anchor2);
+ c.print ("axis1",h->axis1);
+ c.print ("axis2",h->axis2);
+ c.print ("v1",h->v1); //@@@ much better to write out 'qrel' here, if it's available
+ c.print ("v2",h->v2);
+ c.print ("susp_erp",h->susp_erp);
+ c.print ("susp_cfm",h->susp_cfm);
+ printLimot (c,h->limot1,1);
+ printLimot (c,h->limot2,2);
+}
+
+static void printPR (PrintingContext &c, dxJoint *j)
+{
+ dxJointPR *pr = (dxJointPR*) j;
+ c.print ("anchor2",pr->anchor2);
+ c.print ("axisR1",pr->axisR1);
+ c.print ("axisR2",pr->axisR2);
+ c.print ("axisP1",pr->axisP1);
+ c.print ("qrel",pr->qrel,4);
+ c.print ("offset",pr->offset);
+ printLimot (c,pr->limotP,1);
+ printLimot (c,pr->limotR,2);
+}
+
+static void printPU (PrintingContext &c, dxJoint *j)
+{
+ dxJointPU *pu = (dxJointPU*) j;
+ c.print ("anchor1",pu->anchor1);
+ c.print ("anchor2",pu->anchor2);
+ c.print ("axis1",pu->axis1);
+ c.print ("axis2",pu->axis2);
+ c.print ("axisP",pu->axisP1);
+ c.print ("qrel1",pu->qrel1,4);
+ c.print ("qrel2",pu->qrel2,4);
+ printLimot (c,pu->limot1,1);
+ printLimot (c,pu->limot2,2);
+ printLimot (c,pu->limotP,3);
+}
+
+static void printPiston (PrintingContext &c, dxJoint *j)
+{
+ dxJointPiston *rap = (dxJointPiston*) j;
+ c.print ("anchor1",rap->anchor1);
+ c.print ("anchor2",rap->anchor2);
+ c.print ("axis1",rap->axis1);
+ c.print ("axis2",rap->axis2);
+ c.print ("qrel",rap->qrel,4);
+ printLimot (c,rap->limotP,1);
+ printLimot (c, rap->limotR, 2);
+}
+
+static void printFixed (PrintingContext &c, dxJoint *j)
+{
+ dxJointFixed *f = (dxJointFixed*) j;
+ c.print ("qrel",f->qrel);
+ c.print ("offset",f->offset);
+}
+
+static void printLMotor (PrintingContext &c, dxJoint *j)
+{
+ dxJointLMotor *a = (dxJointLMotor*) j;
+ c.print("num", a->num);
+ c.printIndent();
+ fprintf (c.file,"rel = {%d,%d,%d},\n",a->rel[0],a->rel[1],a->rel[2]);
+ c.print ("axis1",a->axis[0]);
+ c.print ("axis2",a->axis[1]);
+ c.print ("axis3",a->axis[2]);
+ for (int i=0; i<3; i++) printLimot (c,a->limot[i],i+1);
+}
+
+struct dxAMotorJointPrinter
+{
+ static void print(PrintingContext &c, dxJointAMotor *a)
+ {
+ c.print ("num",a->m_num);
+ c.print ("mode",a->m_mode);
+ c.printIndent();
+ fprintf (c.file,"rel = {%d,%d,%d},\n",a->m_rel[0],a->m_rel[1],a->m_rel[2]);
+ c.print ("axis1",a->m_axis[0]);
+ c.print ("axis2",a->m_axis[1]);
+ c.print ("axis3",a->m_axis[2]);
+ for (int i=0; i<3; i++) printLimot (c,a->m_limot[i],i+1);
+ c.print ("angle1",a->m_angle[0]);
+ c.print ("angle2",a->m_angle[1]);
+ c.print ("angle3",a->m_angle[2]);
+ }
+};
+
+static void printAMotor (PrintingContext &c, dxJoint *j)
+{
+ dxJointAMotor *a = (dxJointAMotor*) j;
+ dxAMotorJointPrinter::print(c, a);
+}
+
+//***************************************************************************
+// geometry
+
+static void printGeom (PrintingContext &c, dxGeom *g);
+
+static void printSphere (PrintingContext &c, dxGeom *g)
+{
+ c.print ("type","sphere");
+ c.print ("radius",dGeomSphereGetRadius (g));
+}
+
+
+static void printBox (PrintingContext &c, dxGeom *g)
+{
+ dVector3 sides;
+ dGeomBoxGetLengths (g,sides);
+ c.print ("type","box");
+ c.print ("sides",sides);
+}
+
+
+static void printCapsule (PrintingContext &c, dxGeom *g)
+{
+ dReal radius,length;
+ dGeomCapsuleGetParams (g,&radius,&length);
+ c.print ("type","capsule");
+ c.print ("radius",radius);
+ c.print ("length",length);
+}
+
+
+static void printCylinder (PrintingContext &c, dxGeom *g)
+{
+ dReal radius,length;
+ dGeomCylinderGetParams (g,&radius,&length);
+ c.print ("type","cylinder");
+ c.print ("radius",radius);
+ c.print ("length",length);
+}
+
+
+static void printPlane (PrintingContext &c, dxGeom *g)
+{
+ dVector4 e;
+ dGeomPlaneGetParams (g,e);
+ c.print ("type","plane");
+ c.print ("normal",e);
+ c.print ("d",e[3]);
+}
+
+
+static void printRay (PrintingContext &c, dxGeom *g)
+{
+ dReal length = dGeomRayGetLength (g);
+ c.print ("type","ray");
+ c.print ("length",length);
+}
+
+
+static void printConvex (PrintingContext &c, dxGeom * /*g*/)
+{
+ c.print ("type","convex");
+ ///@todo Print information about convex hull
+}
+
+
+
+static void printTriMesh (PrintingContext &c, dxGeom * /*g*/)
+{
+ c.print ("type","trimesh");
+ //@@@ i don't think that the trimesh accessor functions are really
+ // sufficient to read out all the triangle data, and anyway we
+ // should have a method of not duplicating trimesh data that is
+ // shared.
+}
+
+
+static void printHeightfieldClass (PrintingContext &c, dxGeom * /*g*/)
+{
+ c.print ("type","heightfield");
+ ///@todo Print information about heightfield
+}
+
+
+static void printGeom (PrintingContext &c, dxGeom *g)
+{
+ unsigned long category = dGeomGetCategoryBits (g);
+ if (category != (unsigned long)(~0)) {
+ c.printIndent();
+ fprintf (c.file,"category_bits = %lu\n",category);
+ }
+ unsigned long collide = dGeomGetCollideBits (g);
+ if (collide != (unsigned long)(~0)) {
+ c.printIndent();
+ fprintf (c.file,"collide_bits = %lu\n",collide);
+ }
+ if (!dGeomIsEnabled (g)) {
+ c.print ("disabled",1);
+ }
+ switch (g->type) {
+ case dSphereClass: printSphere (c,g); break;
+ case dBoxClass: printBox (c,g); break;
+ case dCapsuleClass: printCapsule (c,g); break;
+ case dCylinderClass: printCylinder (c,g); break;
+ case dPlaneClass: printPlane (c,g); break;
+ case dRayClass: printRay (c,g); break;
+ case dConvexClass: printConvex (c,g); break;
+ case dTriMeshClass: printTriMesh (c,g); break;
+ case dHeightfieldClass: printHeightfieldClass (c,g); break;
+ }
+}
+
+//***************************************************************************
+// world
+
+void dWorldExportDIF (dWorldID w, FILE *file, const char *prefix)
+{
+ PrintingContext c;
+ c.file = file;
+#if defined(dSINGLE)
+ c.precision = 7;
+#else
+ c.precision = 15;
+#endif
+ c.indent = 1;
+
+ fprintf (file,"-- Dynamics Interchange Format v0.1\n\n%sworld = dynamics.world {\n",prefix);
+ c.print ("gravity",w->gravity);
+ c.print ("ODE = {");
+ c.indent++;
+ c.print ("ERP",w->global_erp);
+ c.print ("CFM",w->global_cfm);
+ c.print ("auto_disable = {");
+ c.indent++;
+ c.print ("linear_threshold",w->adis.linear_average_threshold);
+ c.print ("angular_threshold",w->adis.angular_average_threshold);
+ c.print ("average_samples",(int)w->adis.average_samples);
+ c.print ("idle_time",w->adis.idle_time);
+ c.print ("idle_steps",w->adis.idle_steps);
+ fprintf (file,"\t\t},\n\t},\n}\n");
+ c.indent -= 3;
+
+ // bodies
+ int num = 0;
+ fprintf (file,"%sbody = {}\n",prefix);
+ for (dxBody *b=w->firstbody; b; b=(dxBody*)b->next) {
+ b->tag = num;
+ fprintf (file,"%sbody[%d] = dynamics.body {\n\tworld = %sworld,\n",prefix,num,prefix);
+ c.indent++;
+ c.print ("pos",b->posr.pos);
+ c.print ("q",b->q,4);
+ c.print ("lvel",b->lvel);
+ c.print ("avel",b->avel);
+ c.print ("mass",b->mass.mass);
+ fprintf (file,"\tI = {{");
+ for (int i=0; i<3; i++) {
+ for (int j=0; j<3; j++) {
+ c.printReal (b->mass.I[i*4+j]);
+ if (j < 2) fputc (',',file);
+ }
+ if (i < 2) fprintf (file,"},{");
+ }
+ fprintf (file,"}},\n");
+ c.printNonzero ("com",b->mass.c);
+ c.print ("ODE = {");
+ c.indent++;
+ if (b->flags & dxBodyFlagFiniteRotation) c.print ("finite_rotation",1);
+ if (b->flags & dxBodyDisabled) c.print ("disabled",1);
+ if (b->flags & dxBodyNoGravity) c.print ("no_gravity",1);
+ if (b->flags & dxBodyAutoDisable) {
+ c.print ("auto_disable = {");
+ c.indent++;
+ c.print ("linear_threshold",b->adis.linear_average_threshold);
+ c.print ("angular_threshold",b->adis.angular_average_threshold);
+ c.print ("average_samples",(int)b->adis.average_samples);
+ c.print ("idle_time",b->adis.idle_time);
+ c.print ("idle_steps",b->adis.idle_steps);
+ c.print ("time_left",b->adis_timeleft);
+ c.print ("steps_left",b->adis_stepsleft);
+ c.indent--;
+ c.print ("},");
+ }
+ c.printNonzero ("facc",b->facc);
+ c.printNonzero ("tacc",b->tacc);
+ if (b->flags & dxBodyFlagFiniteRotationAxis) {
+ c.print ("finite_rotation_axis",b->finite_rot_axis);
+ }
+ c.indent--;
+ c.print ("},");
+ if (b->geom) {
+ c.print ("geometry = {");
+ c.indent++;
+ for (dxGeom *g=b->geom; g; g=g->body_next) {
+ c.print ("{");
+ c.indent++;
+ printGeom (c,g);
+ c.indent--;
+ c.print ("},");
+ }
+ c.indent--;
+ c.print ("},");
+ }
+ c.indent--;
+ c.print ("}");
+ num++;
+ }
+
+ // joints
+ num = 0;
+ fprintf (file,"%sjoint = {}\n",prefix);
+ for (dxJoint *j=w->firstjoint; j; j=(dxJoint*)j->next) {
+ c.indent++;
+ const char *name = getJointName (j);
+ fprintf (file,
+ "%sjoint[%d] = dynamics.%s_joint {\n"
+ "\tworld = %sworld,\n"
+ "\tbody = {"
+ ,prefix,num,name,prefix);
+
+ if ( j->node[0].body )
+ fprintf (file,"%sbody[%d]",prefix,j->node[0].body->tag);
+ if ( j->node[1].body )
+ fprintf (file,",%sbody[%d]",prefix,j->node[1].body->tag);
+ fprintf (file,"}\n");
+
+ switch (j->type()) {
+ case dJointTypeBall: printBall (c,j); break;
+ case dJointTypeHinge: printHinge (c,j); break;
+ case dJointTypeSlider: printSlider (c,j); break;
+ case dJointTypeContact: printContact (c,j); break;
+ case dJointTypeUniversal: printUniversal (c,j); break;
+ case dJointTypeHinge2: printHinge2 (c,j); break;
+ case dJointTypeFixed: printFixed (c,j); break;
+ case dJointTypeAMotor: printAMotor (c,j); break;
+ case dJointTypeLMotor: printLMotor (c,j); break;
+ case dJointTypePR: printPR (c,j); break;
+ case dJointTypePU: printPU (c,j); break;
+ case dJointTypePiston: printPiston (c,j); break;
+ default: c.print("unknown joint");
+ }
+ c.indent--;
+ c.print ("}");
+ num++;
+ }
+}
generated by cgit v1.2.1 (git 2.18.0) at 2024-11-01 02:38:21 +0000