コード例 #1
0
ファイル: Simulator.cpp プロジェクト: basson86/ASBR-CPP-2010
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();
}
コード例 #2
0
ファイル: demo_collision.cpp プロジェクト: JohnCrash/ode
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);
}
コード例 #3
0
ファイル: testing.cpp プロジェクト: NeoAnomaly/xray
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);
}