From e56b9430034523b0917916d4ee1596fd9f8d4ecf Mon Sep 17 00:00:00 2001
From: josh <josh@bd8a9e45-a331-0410-811e-c64571078777>
Date: Tue, 29 Sep 2009 02:10:15 +0000
Subject: [PATCH] setting up mass and rotation in setupGeom()

git-svn-id: svn://anubis/misc/OdeWorld@152 bd8a9e45-a331-0410-811e-c64571078777
---
 OdeWorld.cc | 35 +++++++++++++++++++++++++++++------
 OdeWorld.h  |  2 +-
 2 files changed, 30 insertions(+), 7 deletions(-)

diff --git a/OdeWorld.cc b/OdeWorld.cc
index daafba1..1207a78 100644
--- a/OdeWorld.cc
+++ b/OdeWorld.cc
@@ -149,7 +149,10 @@ dGeomID OdeWorld::addCube(const string & name, bool static_data,
     if (args.size() != 9)
         return 0;
     dGeomID id = dCreateBox(m_space, args[0], args[1], args[2]);
-    setupGeom(name, static_data, body, id, true,
+    dMass mass;
+    dMassSetBox(&mass, 1.0, args[0], args[1], args[2]);
+    dMassTranslate(&mass, args[3], args[4], args[5]);
+    setupGeom(name, static_data, body, id, true, &mass,
             args[3], args[4], args[5],
             args[6], args[7], args[8]);
     return id;
@@ -161,7 +164,10 @@ dGeomID OdeWorld::addSphere(const string & name, bool static_data,
     if (args.size() != 4)
         return 0;
     dGeomID id = dCreateSphere(m_space, args[0]);
-    setupGeom(name, static_data, body, id, true,
+    dMass mass;
+    dMassSetSphere(&mass, 1.0, args[0]);
+    dMassTranslate(&mass, args[1], args[2], args[3]);
+    setupGeom(name, static_data, body, id, true, &mass,
             args[1], args[2], args[3],
             0.0, 0.0, 0.0);
     return id;
@@ -173,7 +179,10 @@ dGeomID OdeWorld::addCylinder(const string & name, bool static_data,
     if (args.size() != 8)
         return 0;
     dGeomID id = dCreateCylinder(m_space, args[0], args[1]);
-    setupGeom(name, static_data, body, id, true,
+    dMass mass;
+    dMassSetCylinder(&mass, 1.0, 3, args[0], args[1]);
+    dMassTranslate(&mass, args[2], args[3], args[4]);
+    setupGeom(name, static_data, body, id, true, &mass,
             args[2], args[3], args[4],
             args[5], args[6], args[7]);
     return id;
@@ -185,7 +194,10 @@ dGeomID OdeWorld::addCCylinder(const string & name, bool static_data,
     if (args.size() != 8)
         return 0;
     dGeomID id = dCreateCCylinder(m_space, args[0], args[1]);
-    setupGeom(name, static_data, body, id, true,
+    dMass mass;
+    dMassSetCylinder(&mass, 1.0, 3, args[0], args[1]);
+    dMassTranslate(&mass, args[2], args[3], args[4]);
+    setupGeom(name, static_data, body, id, true, &mass,
             args[2], args[3], args[4],
             args[5], args[6], args[7]);
     return id;
@@ -199,13 +211,13 @@ dGeomID OdeWorld::addPlane(const string & name, bool static_data,
     float a, b, c, d;
     /* TODO: find a, b, c, d from args */
     dGeomID id = dCreatePlane(m_space, a, b, c, d);
-    setupGeom(name, static_data, body, id, false,
+    setupGeom(name, static_data, body, id, false, NULL,
             0.0, 0.0, 0.0, 0.0, 0.0, 0.0);
     return id;
 }
 
 void OdeWorld::setupGeom(const std::string & name, bool static_data,
-        dBodyID * body, dGeomID geom, bool placeable,
+        dBodyID * body, dGeomID geom, bool placeable, dMass * mass,
         float locx, float locy, float locz,
         float rotx, float roty, float rotz)
 {
@@ -214,4 +226,15 @@ void OdeWorld::setupGeom(const std::string & name, bool static_data,
     if (placeable)
         dGeomSetBody(geom, *body);
     /* TODO: set up mass, location, and rotation */
+    if (placeable)
+    {
+        dMatrix3 rot;
+        dRFromEulerAngles(rot, rotx, roty, rotz);
+        dGeomSetRotation(geom, rot);
+
+        dMass origmass;
+        dBodyGetMass(*body, &origmass);
+        dMassAdd(&origmass, mass);
+        dBodySetMass(*body, &origmass);
+    }
 }
diff --git a/OdeWorld.h b/OdeWorld.h
index 22aab79..91bbe0d 100644
--- a/OdeWorld.h
+++ b/OdeWorld.h
@@ -40,7 +40,7 @@ class OdeWorld
         std::map<std::string, dGeomID> m_objects;
 
         void setupGeom(const std::string & name, bool static_data,
-                dBodyID * body, dGeomID geom, bool placeable,
+                dBodyID * body, dGeomID geom, bool placeable, dMass * mass,
                 float locx, float locy, float locz,
                 float rotx, float roty, float rotz);
 };