IoObject *IoODEBody_addRelTorque(IoODEBody *self, IoObject *locals, IoMessage *m)
{
const double x = IoMessage_locals_doubleArgAt_(m, locals, 0);
const double y = IoMessage_locals_doubleArgAt_(m, locals, 1);
const double z = IoMessage_locals_doubleArgAt_(m, locals, 2);
IoODEBody_assertValidBody(self, locals, m);
dBodyAddRelTorque(BODYID, x, y, z);
return self;
}
IoObject *IoODEBody_setLinearVelocity(IoODEBody *self, IoObject *locals, IoMessage *m)
{
const double x = IoMessage_locals_doubleArgAt_(m, locals, 0);
const double y = IoMessage_locals_doubleArgAt_(m, locals, 1);
const double z = IoMessage_locals_doubleArgAt_(m, locals, 2);
IoODEBody_assertValidBody(self, locals, m);
dBodySetLinearVel(BODYID, x, y, z);
return self;
}
IoObject *IoODEBody_mass(IoODEBody *self, IoObject *locals, IoMessage *m)
{
IoODEBody_assertValidBody(self, locals, m);
{
IoODEMass *mass = IoODEMass_new(IOSTATE);
dBodyGetMass(BODYID, IoODEMass_dMassStruct(mass));
return mass;
}
}
IoObject *IoODEBody_setMass(IoODEBody *self, IoObject *locals, IoMessage *m)
{
IoODEBody_assertValidBody(self, locals, m);
{
dMass *mass = IoMessage_locals_odeMassStructArgAt_(m, locals, 0);
dBodySetMass(BODYID, mass);
}
return self;
}
IoObject *IoODEBody_quaternion(IoODEBody *self, IoObject *locals, IoMessage *m)
{
IoODEBody_assertValidBody(self, locals, m);
{
const dReal *q = dBodyGetQuaternion(BODYID);
IoSeq *v = IoSeq_makeFloatArrayOfSize_(IOSTATE, 4);
UArray *u = IoSeq_rawUArray(v);
UArray_at_put_(u, 0, q[0]);
UArray_at_put_(u, 1, q[1]);
UArray_at_put_(u, 2, q[2]);
UArray_at_put_(u, 3, q[3]);
return v;
}
}
IoObject *IoODEBody_glMultMatrix(IoODEBody *self, IoObject *locals, IoMessage *m)
{
IoODEBody_assertValidBody(self, locals, m);
{
dBodyID bodyId = BODYID;
const dReal *pos = dBodyGetPosition(bodyId);
const dReal *R = dBodyGetRotation(bodyId);
GLfloat matrix[16] = {R[0], R[4], R[8], 0, R[1], R[5], R[9], 0, R[2], R[6], R[10], 0, pos[0], pos[1], pos[2], 1};
glMultMatrixf(matrix);
return self;
}
}
IoObject *IoODEBody_setRotation(IoODEBody *self, IoObject *locals, IoMessage *m)
{
IoODEBody_assertValidBody(self, locals, m);
{
dMatrix3 R;
R[0] = IoMessage_locals_doubleArgAt_(m, locals, 0);
R[1] = IoMessage_locals_doubleArgAt_(m, locals, 1);
R[2] = IoMessage_locals_doubleArgAt_(m, locals, 2);
R[3] = 0;
R[4] = IoMessage_locals_doubleArgAt_(m, locals, 3);
R[5] = IoMessage_locals_doubleArgAt_(m, locals, 4);
R[6] = IoMessage_locals_doubleArgAt_(m, locals, 5);
R[7] = 0;
R[8] = IoMessage_locals_doubleArgAt_(m, locals, 6);
R[9] = IoMessage_locals_doubleArgAt_(m, locals, 7);
R[10] = IoMessage_locals_doubleArgAt_(m, locals, 8);
R[11] = 0;
dBodySetRotation(BODYID, R);
return self;
}
}
IoObject *IoODEBody_rotation(IoODEBody *self, IoObject *locals, IoMessage *m)
{
IoODEBody_assertValidBody(self, locals, m);
{
const dReal *R = dBodyGetRotation(BODYID);
IoSeq *v = IoSeq_makeFloatArrayOfSize_(IOSTATE, 9);
UArray *u = IoSeq_rawUArray(v);
UArray_at_put_(u, 0, R[0]);
UArray_at_put_(u, 1, R[4]);
UArray_at_put_(u, 2, R[8]);
UArray_at_put_(u, 3, R[1]);
UArray_at_put_(u, 4, R[5]);
UArray_at_put_(u, 5, R[9]);
UArray_at_put_(u, 6, R[2]);
UArray_at_put_(u, 7, R[6]);
UArray_at_put_(u, 8, R[10]);
return v;
}
}
IoObject *IoODEBody_linearVelocity(IoODEBody *self, IoObject *locals, IoMessage *m)
{
IoODEBody_assertValidBody(self, locals, m);
return IoVector_newWithODEPoint(IOSTATE, dBodyGetLinearVel(BODYID));
}
IoObject *IoODEBody_torque(IoODEBody *self, IoObject *locals, IoMessage *m)
{
IoODEBody_assertValidBody(self, locals, m);
return IoVector_newWithODEPoint(IOSTATE, dBodyGetTorque(BODYID));
}
IoObject *IoODEBody_force(IoODEBody *self, IoObject *locals, IoMessage *m)
{
IoODEBody_assertValidBody(self, locals, m);
return IoSeq_newWithODEPoint(IOSTATE, dBodyGetForce(BODYID));
}
