From acb7c2ea4017e25e209016877dea899badb85e27 Mon Sep 17 00:00:00 2001
From: josh <josh@bd8a9e45-a331-0410-811e-c64571078777>
Date: Mon, 5 Oct 2009 20:02:25 +0000
Subject: [PATCH] added pushTransform(), fixed gravity to -Z axis by default

git-svn-id: svn://anubis/misc/OdeWorld@158 bd8a9e45-a331-0410-811e-c64571078777
---
 OdeWorld.cc | 28 +++++++++++++++++++++++++++-
 OdeWorld.h  |  2 ++
 2 files changed, 29 insertions(+), 1 deletion(-)

diff --git a/OdeWorld.cc b/OdeWorld.cc
index ef9b6b7..4acc2f0 100644
--- a/OdeWorld.cc
+++ b/OdeWorld.cc
@@ -3,6 +3,7 @@
 #include <vector>
 #include <fstream>
 #include <iostream>
+#include <GL/gl.h>
 using namespace std;
 
 #define WORLD_STEP 0.001
@@ -61,7 +62,7 @@ OdeWorld::OdeWorld()
     m_world = dWorldCreate();
     m_space = dHashSpaceCreate(0);
     m_contactJointGroup = dJointGroupCreate(0);
-    setGravity(0, -9.81, 0);
+    setGravity(0, 0, -9.81);
 }
 
 OdeWorld::~OdeWorld()
@@ -243,3 +244,28 @@ void OdeWorld::setupGeom(const std::string & name, bool static_data,
         dBodySetMass(*body, &origmass);
     }
 }
+
+/* push an OpenGL matrix onto the matrix stack for a given
+ * ODE body position and rotation */
+void OdeWorld::pushTransform(const float pos[3], const float R[12])
+{
+    GLfloat matrix[16];
+    matrix[0] = R[0];
+    matrix[1] = R[4];
+    matrix[2] = R[8];
+    matrix[3] = 0;
+    matrix[4] = R[1];
+    matrix[5] = R[5];
+    matrix[6] = R[9];
+    matrix[7] = 0;
+    matrix[8] = R[2];
+    matrix[9] = R[6];
+    matrix[10] = R[10];
+    matrix[11] = 0;
+    matrix[12] = pos[0];
+    matrix[13] = pos[1];
+    matrix[14] = pos[2];
+    matrix[15] = 1;
+    glPushMatrix();
+    glMultMatrixf(matrix);
+}
diff --git a/OdeWorld.h b/OdeWorld.h
index 8b23509..de66f30 100644
--- a/OdeWorld.h
+++ b/OdeWorld.h
@@ -33,6 +33,8 @@ class OdeWorld
         friend void OdeWorld_collide_callback(void * data,
                 dGeomID o1, dGeomID o2);
 
+        static void pushTransform(const float pos[3], const float R[12]);
+
     protected:
         dWorldID        m_world;
         dSpaceID        m_space;