void Simulator::GetCurrentState(double s[])
{
/*
* State for each body consists of position, orientation (stored as a quaternion),
* linear velocity, and angular velocity. In the current robot model,
* the bodies are vehicle chassis and the vehicle wheels.
*/
const int n = (int) m_robotBodies.size();
for (int i = 0; i < n; ++i)
{
const dReal *pos = dBodyGetPosition (m_robotBodies[i]);
const dReal *rot = dBodyGetQuaternion(m_robotBodies[i]);
const dReal *vel = dBodyGetLinearVel (m_robotBodies[i]);
const dReal *ang = dBodyGetAngularVel(m_robotBodies[i]);
const int offset = 13 * i;
s[offset ] = pos[0];
s[offset + 1] = pos[1];
s[offset + 2] = pos[2];
s[offset + 3] = rot[0];
s[offset + 4] = rot[1];
s[offset + 5] = rot[2];
s[offset + 6] = rot[3];
s[offset + 7] = vel[0];
s[offset + 8] = vel[1];
s[offset + 9] = vel[2];
s[offset + 10] = ang[0];
s[offset + 11] = ang[1];
s[offset + 12] = ang[2];
}
s[13 * n] = dRandGetSeed();
}
static void simLoop (int pause)
{
do {
draw_all_objects_called = 0;
unsigned long seed = dRandGetSeed();
testslot[graphical_test].test_fn();
if (draw_all_objects_called) {
if (space_pressed) space_pressed = 0; else dRandSetSeed (seed);
}
}
while (!draw_all_objects_called);
}
extern "C" void dTestMatrixComparison()
{
volatile int i;
printf ("dTestMatrixComparison()\n");
dMessageFunction *orig_debug = dGetDebugHandler();
dMatrixComparison mc;
dReal A[50*50];
// make first sequence
unsigned long seed = dRandGetSeed();
for (i=1; i<49; i++) {
dMakeRandomMatrix (A,i,i+1,1.0);
mc.nextMatrix (A,i,i+1,0,"A%d",i);
}
mc.end();
//mc.dump();
// test identical sequence
dSetDebugHandler (&myDebug);
dRandSetSeed (seed);
if (setjmp (jump_buffer)) {
printf ("\tFAILED (1)\n");
}
else {
for (i=1; i<49; i++) {
dMakeRandomMatrix (A,i,i+1,1.0);
mc.nextMatrix (A,i,i+1,0,"A%d",i);
}
mc.end();
printf ("\tpassed (1)\n");
}
dSetDebugHandler (orig_debug);
// test broken sequences (with matrix error)
dRandSetSeed (seed);
volatile int passcount = 0;
for (i=1; i<49; i++) {
if (setjmp (jump_buffer)) {
passcount++;
}
else {
dSetDebugHandler (&myDebug);
dMakeRandomMatrix (A,i,i+1,1.0);
A[(i-1)*dPAD(i+1)+i] += REAL(0.01);
mc.nextMatrix (A,i,i+1,0,"A%d",i);
dSetDebugHandler (orig_debug);
}
}
mc.end();
printf ("\t%s (2)\n",(passcount == 48) ? "passed" : "FAILED");
// test broken sequences (with name error)
dRandSetSeed (seed);
passcount = 0;
for (i=1; i<49; i++) {
if (setjmp (jump_buffer)) {
passcount++;
}
else {
dSetDebugHandler (&myDebug);
dMakeRandomMatrix (A,i,i+1,1.0);
mc.nextMatrix (A,i,i+1,0,"B%d",i);
dSetDebugHandler (orig_debug);
}
}
mc.end();
printf ("\t%s (3)\n",(passcount == 48) ? "passed" : "FAILED");
// test identical sequence again
dSetDebugHandler (&myDebug);
dRandSetSeed (seed);
if (setjmp (jump_buffer)) {
printf ("\tFAILED (4)\n");
}
else {
for (i=1; i<49; i++) {
dMakeRandomMatrix (A,i,i+1,1.0);
mc.nextMatrix (A,i,i+1,0,"A%d",i);
}
mc.end();
printf ("\tpassed (4)\n");
}
dSetDebugHandler (orig_debug);
}