[ODE] Help with setting and getting rotation

[Matthews, Steve]

Okay, next time I'll throw real data in.  Apparently this does work; my dummy data was too dummy.  sorry all.

-----Original Message-----
From: Matthews, Steve 
Sent: Monday, November 25, 2002 1:37 PM
To: ode@q12.org
Subject: [ODE] Help with setting and getting rotation


Could somebody please look at the following code?  I am trying to set the position and rotation of a body from a 4*4 matrix of floats, and I also want to then be able to query that position and rotation.  
   I seem to be able to set the position and retrieve it correctly, but the rotation comes back as garbage when I try to retrieve it and I don't know why.  The code will compile as a standalone main with ode as the only dependency. The input matrix (inputMat) is just dummy numbers; if I'm imposing something or assuming something I shouldn't, please tell me.

Thanks much,
   Steve

**********************************************

#include "ode/ode.h"

void setPhysicsPosition(dBodyID bodyID, float setMat[4][4])
{
      dBodySetPosition(bodyID, (dReal)setMat[3][0], (dReal)setMat[3][1], (dReal)setMat[3][2]);

      dMatrix3 R;

      R[0] =  (dReal)setMat[0][0];
      R[1] =  (dReal)setMat[0][1];
      R[2] =  (dReal)setMat[0][2];
      R[3] =  0.0;
      R[4] =  (dReal)setMat[1][0];
      R[5] =  (dReal)setMat[1][1];
      R[6] =  (dReal)setMat[1][2];
      R[7] =  0.0;
      R[8] =  (dReal)setMat[2][0];
      R[9] =  (dReal)setMat[2][1];
      R[10] = (dReal)setMat[2][2];
      R[11] = 0.0;

      dBodySetRotation(bodyID, R);
}

void getPhysicsPosition(dBodyID bodyID, float getMat[4][4])
{
      const dReal *getPos;
      getPos = dBodyGetPosition(bodyID);

      const dReal *getRot;
      getRot = dBodyGetRotation(bodyID);

      getMat[0][0] = (float)getRot[0];
      getMat[0][1] = (float)getRot[1];
      getMat[0][2] = (float)getRot[2];
      getMat[0][3] = (float)getRot[3];
      getMat[1][0] = (float)getRot[4];
      getMat[1][1] = (float)getRot[5];
      getMat[1][2] = (float)getRot[6];
      getMat[1][3] = (float)getRot[7];
      getMat[2][0] = (float)getRot[8];
      getMat[2][1] = (float)getRot[9];
      getMat[2][2] = (float)getRot[10];
      getMat[2][3] = (float)getRot[11];

      getMat[3][0] = (float)getPos[0];
      getMat[3][1] = (float)getPos[1];
      getMat[3][2] = (float)getPos[2];

      getMat[3][3] = 1.0f;
}

int main()
{
   dWorldID worldID = dWorldCreate();
   dBodyID  bodyID  = dBodyCreate(worldID);

   //create the input matrix
   float inputMat[4][4] = {{1.0f, 0.1f, 0.2f, 0.0f},  
                           {0.3f, 1.0f, 0.4f, 0.0f},
                           {0.5f, 0.6f, 1.0f, 0.0f},
                           {1.0f, 2.0f, 3.0f, 1.0f}};

   //set the position of the physics body
   setPhysicsPosition(bodyID, inputMat);

   //print the input to console
   printf("input matrix:\n\n");
   printf("%f %f %f %f\n",   inputMat[0][0], inputMat[0][1], inputMat[0][2], inputMat[0][3]);
   printf("%f %f %f %f\n",   inputMat[1][0], inputMat[1][1], inputMat[1][2], inputMat[1][3]);
   printf("%f %f %f %f\n",   inputMat[2][0], inputMat[2][1], inputMat[2][2], inputMat[2][3]);
   printf("%f %f %f %f\n\n", inputMat[3][0], inputMat[3][1], inputMat[3][2], inputMat[3][3]);

   //initialize the output matrix
   float outMat[4][4] = {0.f};

   //return the position back to 4*4 matrix format in newPos
   getPhysicsPosition(bodyID, outMat);

   //print the output to console
   printf("output matrix:\n\n");
   printf("%f %f %f %f\n",   outMat[0][0], outMat[0][1], outMat[0][2], outMat[0][3]);
   printf("%f %f %f %f\n",   outMat[1][0], outMat[1][1], outMat[1][2], outMat[1][3]);
   printf("%f %f %f %f\n",   outMat[2][0], outMat[2][1], outMat[2][2], outMat[2][3]);
   printf("%f %f %f %f\n\n", outMat[3][0], outMat[3][1], outMat[3][2], outMat[3][3]);

   return 0;
}
_______________________________________________
ODE mailing list
ODE@q12.org
http://q12.org/mailman/listinfo/ode