void Simulation::command(int cmd)
{
dMass mass;
static int view; // which view was set last
static std::string input = ""; // input string ("" initially)
//static dJointID tagging_joint = 0; // joint that keeps one link static
if (cmd >= 'A' && cmd <= 'B')
cmd = cmd + 'a' - 'A'; // lower case
if (mode == KeyMode) {
switch (cmd) {
case keyWriteFrame:
mywriteframes ^= 1;
break;
case keyCreateMainBody:
if (car->body_obj == 0) {
car->createBody(center);
objbin.Add(car->body_obj);
};
break;
// switch mode to input
case keyInputMode:
mode = InputMode;
std::cout << "input: ";
std::cout.flush();
break;
// abandon ship
case keyBreakWheel:
car->breakWheel();
break;
// change speed
case keyZeroSpeed:
s_speed[0] = 0;
s_speed[1] = 0;
break;
case keyRightSpeedIncrease:
s_speed[1] += 0.1;
break;
case keyRightSpeedDecrease:
s_speed[1] -= 0.1;
break;
case keyRightSpeedMax:
s_speed[1] = max_speed;
break;
case keyRightSpeedMin:
s_speed[1] = min_speed;
break;
case keyLeftSpeedIncrease:
s_speed[0] += 0.1;
break;
case keyLeftSpeedDecrease:
s_speed[0] -= 0.1;
break;
case keyLeftSpeedMax:
s_speed[0] = max_speed;
break;
case keyLeftSpeedMin:
s_speed[0] = min_speed;
break;
case keyBothSpeedIncrease:
s_speed[0] += 0.1;
s_speed[1] += 0.1;
break;
case keyBothSpeedDecrease:
s_speed[0] -= 0.1;
s_speed[1] -= 0.1;
break;
case keyBothSpeedMax:
s_speed[0] = max_speed;
s_speed[1] = max_speed;
break;
case keyBothSpeedMin:
s_speed[0] = min_speed;
s_speed[1] = min_speed;
break;
// do the twitch
case keyTwitchToggle:
twitch_flag = 1 - twitch_flag;
break;
case keyTwitchHertzHalf:
twitch_hertz /= 2.0;
break;
case keyTwitchHertzDouble:
twitch_hertz *= 2.0;
break;
// camera views change
case keyAimCamera:
aimCamera(camera_object);
break;
case keyLockCameraToVehicle: // only pos, not orientation so far
lockCamera = 1 - lockCamera;
float xyz[3], hpr[3];
dsGetViewpoint(xyz, hpr);
dBodyGetPosRelPoint(camera_object, xyz[0], xyz[1], xyz[2], cameraRelVec); // returns point in body relative coordinates
break;
case keyIncreaseViewByTwoMod:
view = (view + 2) % set_view(-1);
set_view(view);
break;
case keyIncreaseViewByOneMod:
view = (view + 1) % set_view(-1);
set_view(view);
break;
// show speed and wanted speed (uncorrect when in twitch mode)
case keyDisplayWheels:
for (int i = 0 ; i < 4 ; ++i)
car->wheel_obj[i]->toggleDrawFlag();
case keyPrintSprocketMass:
car->getSprocketMass(&mass);
printf("mass %3.3f %3.3f %3.3f %3.3f\n",
mass.mass, mass.I[0], mass.I[5],
mass.I[10]);
break;
case keyPrintManyVariables:
printf
("s_speed %3.3f | actual %3.3f | hertz %3.3f | time %3.3f | lv %d\n",
s_speed[0], car->getSprocketSpeed(),
twitch_hertz, dStopwatchTime(&timer), view);
break;
case keyEnterMovieMode:
enterMovieMode();
automat = new AutoEventFinish(simTime, event_time, m_myMachine, finish_time, m_exitMachine, camera_object); // create automat
break;
};
if (cmd >= '0' && cmd < ('0' + LINK_PARTS))
Chain::showbit[cmd - '0'] =
1 - Chain::showbit[cmd - '0'];
for (int i = 0; i < 2; ++i)
s_speed[i] =
BRACKET(s_speed[i], min_speed, max_speed);
} else if (mode == InputMode) {
// switch mode to single key
if (cmd == ' ') {
mode = KeyMode;
//process (input);
std::istringstream s(input);
int tagged_link;
s >> tagged_link;
/*
tagged_link = BRACKET(tagged_link, 0, chain_obj[0]->b_num - 1);
if (tagging_joint != 0) // delete old
dJointDestroy(tagging_joint);
tagging_joint = dJointCreateFixed(world, 0);
dJointAttach(tagging_joint, chain_obj[0]->body[tagged_link], 0);
dJointSetFixed(tagging_joint);
*/
std::cout << "\n";
// reset input
input = "";
} else {
extern "C" ODE_API int dTestSolveLCP()
{
const int n = 100;
size_t memreq = EstimateTestSolveLCPMemoryReq(n);
dxWorldProcessMemArena *arena = dxAllocateTemporaryWorldProcessMemArena(memreq, NULL, NULL);
if (arena == NULL) {
return 0;
}
int i,nskip = dPAD(n);
#ifdef dDOUBLE
const dReal tol = REAL(1e-9);
#endif
#ifdef dSINGLE
const dReal tol = REAL(1e-4);
#endif
printf ("dTestSolveLCP()\n");
dReal *A = arena->AllocateArray<dReal> (n*nskip);
dReal *x = arena->AllocateArray<dReal> (n);
dReal *b = arena->AllocateArray<dReal> (n);
dReal *w = arena->AllocateArray<dReal> (n);
dReal *lo = arena->AllocateArray<dReal> (n);
dReal *hi = arena->AllocateArray<dReal> (n);
dReal *A2 = arena->AllocateArray<dReal> (n*nskip);
dReal *b2 = arena->AllocateArray<dReal> (n);
dReal *lo2 = arena->AllocateArray<dReal> (n);
dReal *hi2 = arena->AllocateArray<dReal> (n);
dReal *tmp1 = arena->AllocateArray<dReal> (n);
dReal *tmp2 = arena->AllocateArray<dReal> (n);
double total_time = 0;
for (int count=0; count < 1000; count++) {
BEGIN_STATE_SAVE(arena, saveInner) {
// form (A,b) = a random positive definite LCP problem
dMakeRandomMatrix (A2,n,n,1.0);
dMultiply2 (A,A2,A2,n,n,n);
dMakeRandomMatrix (x,n,1,1.0);
dMultiply0 (b,A,x,n,n,1);
for (i=0; i<n; i++) b[i] += (dRandReal()*REAL(0.2))-REAL(0.1);
// choose `nub' in the range 0..n-1
int nub = 50; //dRandInt (n);
// make limits
for (i=0; i<nub; i++) lo[i] = -dInfinity;
for (i=0; i<nub; i++) hi[i] = dInfinity;
//for (i=nub; i<n; i++) lo[i] = 0;
//for (i=nub; i<n; i++) hi[i] = dInfinity;
//for (i=nub; i<n; i++) lo[i] = -dInfinity;
//for (i=nub; i<n; i++) hi[i] = 0;
for (i=nub; i<n; i++) lo[i] = -(dRandReal()*REAL(1.0))-REAL(0.01);
for (i=nub; i<n; i++) hi[i] = (dRandReal()*REAL(1.0))+REAL(0.01);
// set a few limits to lo=hi=0
/*
for (i=0; i<10; i++) {
int j = dRandInt (n-nub) + nub;
lo[j] = 0;
hi[j] = 0;
}
*/
// solve the LCP. we must make copy of A,b,lo,hi (A2,b2,lo2,hi2) for
// SolveLCP() to permute. also, we'll clear the upper triangle of A2 to
// ensure that it doesn't get referenced (if it does, the answer will be
// wrong).
memcpy (A2,A,n*nskip*sizeof(dReal));
dClearUpperTriangle (A2,n);
memcpy (b2,b,n*sizeof(dReal));
memcpy (lo2,lo,n*sizeof(dReal));
memcpy (hi2,hi,n*sizeof(dReal));
dSetZero (x,n);
dSetZero (w,n);
dStopwatch sw;
dStopwatchReset (&sw);
dStopwatchStart (&sw);
dSolveLCP (arena,n,A2,x,b2,w,nub,lo2,hi2,0);
dStopwatchStop (&sw);
double time = dStopwatchTime(&sw);
total_time += time;
double average = total_time / double(count+1) * 1000.0;
// check the solution
dMultiply0 (tmp1,A,x,n,n,1);
for (i=0; i<n; i++) tmp2[i] = b[i] + w[i];
dReal diff = dMaxDifference (tmp1,tmp2,n,1);
// printf ("\tA*x = b+w, maximum difference = %.6e - %s (1)\n",diff,
// diff > tol ? "FAILED" : "passed");
if (diff > tol) dDebug (0,"A*x = b+w, maximum difference = %.6e",diff);
int n1=0,n2=0,n3=0;
for (i=0; i<n; i++) {
if (x[i]==lo[i] && w[i] >= 0) {
n1++; // ok
}
else if (x[i]==hi[i] && w[i] <= 0) {
n2++; // ok
}
else if (x[i] >= lo[i] && x[i] <= hi[i] && w[i] == 0) {
n3++; // ok
}
else {
dDebug (0,"FAILED: i=%d x=%.4e w=%.4e lo=%.4e hi=%.4e",i,
x[i],w[i],lo[i],hi[i]);
}
}
// pacifier
printf ("passed: NL=%3d NH=%3d C=%3d ",n1,n2,n3);
printf ("time=%10.3f ms avg=%10.4f\n",time * 1000.0,average);
} END_STATE_SAVE(arena, saveInner);
}
void Simulation::simLoop(int pause)
{
static dReal last_capture_time = 0.0;
static dReal capture_dt = 1.0 / 100; // for 100 fps in simulation time
// profiling variables
this->callback_count = this->not_ground_count = this->ground_count = 0;
// use timer to track time (you need to stop the watch to update the time.
// apparently the digits change so fast it can't be read :-)
//dStopwatchStop(&this->timer);
//dStopwatchStart(&this->timer);
// if there is an automat running, run it. if it is done, delete and *zero* it
this->mainMovie();
// capture a picture every capture_dt second
#ifdef ODE_FRAME_PATCH
clearWriteFrames();
#endif
//cout << "debug: " << (dStopwatchTime(&timer)) << "\n";
if (mywriteframes && (simTime - last_capture_time > capture_dt)) {
#ifdef ODE_FRAME_PATCH
setWriteFrames();
#endif
PEXP(this->simTime);
last_capture_time = this->simTime;
}
// apply forces
for (int i = 0; i < 2; ++i) {
if (twitch_flag) {
this->car->setSpeed(i,
(int) ((dStopwatchTime(&timer) *
twitch_hertz)) %
2 ? min_speed : max_speed);
} else {
this->car->setSpeed(i, s_speed[i]);
}
}
// Accounting
// - zero number of contacts in each chain
this->car->chain_obj[0]->ZeroUserData();
this->car->chain_obj[1]->ZeroUserData();
// oh, if this was ruby!
// car.chains.each { |chain| chain.ZeroUserData() }
// time step
if (!pause) {
dSpaceCollide(space, 0, &::nearCallback);
//if (contactgroup->num) PEXP(contactgroup->num);
// Test fast step
if (use_step_fast)
//dWorldStepFast1(world, step, iterations);
dWorldQuickStep(world, step);
else
dWorldStep(world, step);
simTime += step;
// remove all contact joints
dJointGroupEmpty(contactgroup);
}
// profiling
//printf("debug: callback %4d not_ground %4d ground %4d\n", callback_count,
// not_ground_count, ground_count);
// draw
//dsSetTexture (DS_WOOD);
// set camera if it is tracking the body
if (lockCamera) {
float xyz[3], hpr[3];
dVector3 newpoint;
dsGetViewpoint(xyz, hpr);
dBodyGetRelPointPos(camera_object, cameraRelVec[0],
cameraRelVec[1], cameraRelVec[2], newpoint);
xyz[0] = newpoint[0]; xyz[1] = newpoint[1]; xyz[2] = newpoint[2];
dsSetViewpoint(xyz, hpr);
}
if (objbin.first)
objbin.first->DrawChain();
}
