#include "object.h"
#include <GL/gl.h>

using namespace std;

Object::~Object()
{
}
void Object::render() const
{
	glPushMatrix();

	const dReal *p = dBodyGetPosition(body);
	const dReal *r = dBodyGetRotation(body);
	float m[16];
	m[ 0] = r[ 0];m[ 1] = r[ 4];m[ 2] = r[ 8];m[ 3] = 0;
	m[ 4] = r[ 1];m[ 5] = r[ 5];m[ 6] = r[ 9];m[ 7] = 0;
	m[ 8] = r[ 2];m[ 9] = r[ 6];m[10] = r[10];m[11] = 0;
	m[12] = p[ 0];m[13] = p[ 1];m[14] = p[ 2];m[15] = 1;
	glMultMatrixf(m);
	
	renderInLocalFrame();
	glPopMatrix();
}
void Object::setPosition(dReal x,dReal y,dReal z)
{
	// Equivalent to dGeomSetPosition(geom,x,y,z);
	dBodySetPosition(body,x,y,z);
}