//====================================
void loadCurrentProj()
//====================================
{
int x,res;
char syscom[120],temp[200];
g_warning = S_NO;
putMsg(1,"Loading ...");
res = loadSketch(confSketchFile);
if(res == 0)
{
if(confSteps < 0) confSteps = 100;
if(confSteps > MAX_STEP) confSteps = MAX_STEP-1;
sprintf(syscom,"cd servuino;./servuino %d %d;",confSteps,g_scenSource);
x=system(syscom);
initSim();
resetSim();
readSimulation();
readSketchInfo();
setRange(confBoardType);
init(confWinMode);
saveConfig(currentConf);
unoInfo();
sprintf(temp,"Sketch load ready: %s",confSketchFile);
putMsg(msg_h-2,temp);
}
}
void simLoop (int pause)
{
if (!pause) {
const dReal step = 0.02;
const unsigned nsteps = 1;
for (unsigned i=0; i<nsteps; ++i) {
dSpaceCollide(space, 0, nearCallback);
dWorldQuickStep(world, step);
dJointGroupEmpty(contact_group);
}
} else {
dSpaceCollide(space, 0, nearCallback);
dJointGroupEmpty(contact_group);
}
// now we draw everything
unsigned ngeoms = dSpaceGetNumGeoms(space);
for (unsigned i=0; i<ngeoms; ++i) {
dGeomID g = dSpaceGetGeom(space, i);
if (g == ground)
continue; // drawstuff is already drawing it for us
drawGeom(g);
}
if (dBodyGetPosition(ball1_body)[0] < -track_len)
resetSim();
}
void NGLScene::keyPressEvent(QKeyEvent *_event)
{
// this method is called every time the main window recives a key event.
// we then switch on the key value and set the camera in the NGLScene
switch (_event->key())
{
// escape key to quite
case Qt::Key_Escape : QGuiApplication::exit(EXIT_SUCCESS); break;
// turn on wirframe rendering
case Qt::Key_W : glPolygonMode(GL_FRONT_AND_BACK,GL_LINE); break;
// turn off wire frame
case Qt::Key_S : glPolygonMode(GL_FRONT_AND_BACK,GL_FILL); break;
// show full screen
case Qt::Key_F : showFullScreen(); break;
// show windowed
case Qt::Key_N : showNormal(); break;
case Qt::Key_Space : toggleAnimation(); break;
case Qt::Key_X : stepAnimation(); break;
case Qt::Key_Left : m_rot+=5; break;
case Qt::Key_Right : m_rot-=5; break;
case Qt::Key_Down : m_y-=1.0; break;
case Qt::Key_Up : m_y+=1.0; break;
case Qt::Key_Plus :
case Qt::Key_Equal :
m_numBlocks+=5;
if(m_numBlocks > 300)
m_numBlocks=300;
resetSim();
break;
case Qt::Key_Minus :
m_numBlocks-=5;
if(m_numBlocks<10)
m_numBlocks=10;
resetSim();
break;
case Qt::Key_0 : resetSim();break;
default : break;
}
// finally update the GLWindow and re-draw
//if (isExposed())
m_firePos.set(cos(ngl::radians((m_rot)))*m_radius, m_y, sin(ngl::radians(m_rot))*m_radius);
update();
}
static void command (int cmd)
{
switch (cmd) {
case ' ':
resetSim();
break;
}
}
int MainWindow::qt_metacall(QMetaObject::Call _c, int _id, void **_a)
{
_id = QMainWindow::qt_metacall(_c, _id, _a);
if (_id < 0)
return _id;
if (_c == QMetaObject::InvokeMetaMethod) {
switch (_id) {
case 0: resetSim(); break;
case 1: { bool _r = step();
if (_a[0]) *reinterpret_cast< bool*>(_a[0]) = _r; } break;
default: ;
}
_id -= 2;
}
return _id;
}
void NGLScene::keyPressEvent(QKeyEvent *_event)
{
// this method is called every time the main window recives a key event.
// we then switch on the key value and set the camera in the NGLScene
switch (_event->key())
{
// escape key to quite
case Qt::Key_Escape : QGuiApplication::exit(EXIT_SUCCESS); break;
// turn on wirframe rendering
case Qt::Key_W : glPolygonMode(GL_FRONT_AND_BACK,GL_LINE); break;
// turn off wire frame
case Qt::Key_S : glPolygonMode(GL_FRONT_AND_BACK,GL_FILL); break;
// show full screen
case Qt::Key_F : showFullScreen(); break;
// show windowed
case Qt::Key_N : showNormal(); break;
case Qt::Key_Space : m_car->reset(); break;
case Qt::Key_X : stepAnimation(); break;
case Qt::Key_1 : addCube(); break;
case Qt::Key_2 : addSphere(); break;
case Qt::Key_3 : addCapsule(); break;
case Qt::Key_4 : addCylinder(); break;
case Qt::Key_5 : addCone(); break;
case Qt::Key_6 : addMesh(TEAPOT); break;
case Qt::Key_7 : addMesh(APPLE); break;
/* case Qt::Key_Left : m_physics->addImpulse(ngl::Vec3(-5,0.0f,0.0f)); break;
case Qt::Key_Right : m_physics->addImpulse(ngl::Vec3(5.0f,0.0f,0.0f)); break;
case Qt::Key_Up : m_physics->addImpulse(ngl::Vec3(0.0f,5.0f,0.0f)); break;
case Qt::Key_Down : m_physics->addImpulse(ngl::Vec3(0.0f,-5.0f,0.0f)); break;
*/
case Qt::Key_Left : m_car->left(); break;
case Qt::Key_Right : m_car->right(); break;
case Qt::Key_Up : m_car->accelerate(); break;
case Qt::Key_Down : m_car->stop(); break;
case Qt::Key_B : toggleBBox(); break;
case Qt::Key_R : toggleRandomPlace(); break;
case Qt::Key_0 : resetSim(); break;
default : break;
}
// finally update the GLWindow and re-draw
//if (isExposed())
renderNow();
}
//====================================
int main(int argc, char *argv[])
//====================================
{
char syscom[120];
int ch,i,x;
currentStep = 1;
currentLoop = 0;
strcpy(gplFile,"gpl.txt");
inrpt[0] = IR0;
inrpt[1] = IR1;
inrpt[2] = IR2;
inrpt[3] = IR3;
inrpt[4] = IR4;
inrpt[5] = IR5;
sprintf(syscom,"ls *.conf > %s;",fileProjList);
x=system(syscom);
sprintf(syscom,"rm %s;touch %s;",fileTemp,fileTemp);
x=system(syscom);
sprintf(syscom,"rm %s;touch %s;",fileError,fileError);
x=system(syscom);
sprintf(syscom,"rm %s;touch %s;",fileServError,fileServError);
x=system(syscom);
sprintf(syscom,"rm %s;touch %s;",fileCopyError,fileCopyError);
x=system(syscom);
err = fopen(fileError,"w"); // Issue 15
readSetting();
readConfig(currentConf);
initSim();
resetSim();
readSimulation();
readSketchInfo();
setRange(confBoardType);
init(confWinMode);
unoInfo();
show(slog);
if(confLogFile == S_YES)resetFile("log.txt");
readMsg(gplFile);
openCommand();
delwin(uno);
delwin(ser);
delwin(slog);
delwin(msg);
endwin();
fclose(err);
}
//====================================
void runMode(int stop)
//====================================
{
int ch,x,step,tmp,res=0,a=0,b=0,ir,ok=0,n=0;
char tempName[80],syscom[120],temp[80];
char command[40][40];
strcpy(tempName,"help.txt");
s_mode = S_RUN;
if(stop > 1)
{
if(stop > g_steps)stop = g_steps;
//if(stop > currentStep)
runAll(stop);
return;
}
putMsg(3,"Run Mode. Press h for help.");
while(1)
{
anyErrors();
if(g_silent==S_NO )mvwprintw(uno,board_h-2,1,"R%1d>",confWinMode);
if(g_silent==S_YES)mvwprintw(uno,board_h-2,1,"R%1d<",confWinMode);
unoInfo();
if(g_debug == 1)
readFile(confSketchFile,g_lineSketch[currentStep]);
ch = getchar();
if (ch=='q')
{
return;
}
if (ch=='h')
{
readMsg(fileInfoRun);
}
else if (ch=='c')
{
readMsg(currentConf);
}
else if (ch=='d')
{
readMsg(fileServTime);
}
else if(ch=='y' ) // scenario
{
readMsg(fileServScen);
}
else if (ch=='x')
{
readMsg(fileServScenario);
}
else if (ch=='G')
{
runAll(g_steps);
}
else if (ch=='l')
{
showLoops();
}
else if (ch=='s')
{
g_debug++;
if(g_debug > 1)g_debug = 0;
}
else if (ch=='w')
{
confWinMode++;
if(confWinMode > WIN_MODES)confWinMode = 0;
init(confWinMode);
mvwprintw(uno,board_h-2,1,"R%1d>",confWinMode);
unoInfo();
}
else if (ch=='a')
{
goStep(1);
}
else if (ch=='r')
{
goStep(loopStep[currentLoop+1]);
}
else if (ch=='o')
{
goStep(loopStep[currentLoop]);
}
else if (ch=='p')
{
goStep(loopStep[currentLoop-1]);
}
else if (ch=='z')
{
goStep(g_steps);
}
else if (ch=='k')
{
resetSim();
init(confWinMode);
unoInfo();
mvwprintw(uno,board_h-2,1,"R%1d>",confWinMode);
show(uno);
}
else if (ch=='f')// Up Arrow
{
goStep(currentStep+1);
}
else if (ch=='b')// Down Arrow
{
goStep(currentStep-1);
}
else if (ch=='R') // Right Arrow
{
runLoop(S_FORWARD);
}
else if (ch=='P') // Left Arrow
{
runLoop(S_BACKWARD);
}
else if (ch=='t')
{
runNextRead();
}
else if (ch=='j')
{
runPrevRead();
}
else if (ch=='i')
{
step = currentStep;
sprintf(temp,"(Step:%d) Enter: d/a pin value (q - cancel)",step);
putMsg(2,temp);
wgetstr(uno,temp);
n = tokCommand(command,temp);
if(strstr(command[0],"q") == NULL && n == 3)
{
g_pinNo = atoi(command[1]);
x = atoi(command[2]);
printf("%s %d %d",command[0],g_pinNo,x);
if(strstr(command[0],"a"))
{
ok = ok + checkRange(S_OK,"anapin",g_pinNo);
ok = ok + checkRange(S_OK,"anaval",x);
g_pinType = ANA;
}
if(strstr(command[0],"d"))
{
ok = ok + checkRange(S_OK,"digpin",g_pinNo);
ok = ok + checkRange(S_OK,"digval",x);
g_pinType = DIG;
}
if(ok == S_OK)
{
g_scenSource = 1;
// steps, source, pintype, pinno, pinvalue, pinstep
sprintf(syscom,"cd servuino;./servuino %d %d %d %d %d %d %d;",confSteps,g_scenSource,g_pinType,g_pinNo,x,currentStep,S_ADD);
tmp=system(syscom);
initSim();
readSketchInfo();
readSimulation();
goStep(currentStep);
readMsg(fileServScen);
}
}
else
putMsg(2,"Cancelled!");
}
else if (ch=='v')
{
step = currentStep ;
sprintf(temp," Enter value to be read at step %d (q - cancel)",step);
putMsg(2,temp);
res = analyzeEvent(simulation[step]);
if(res > 0)
{
wgetstr(uno,temp);
if(strstr(temp,"q") == NULL)
{
x = atoi(temp);
ok = S_OK;
if(res == ANA)ok = ok + checkRange(S_OK,"anaval",x);
if(res == DIG)ok = ok + checkRange(S_OK,"digval",x);
if(ok == S_OK)
{
g_scenSource = 1;
// steps, source, pintype, pinno, pinvalue, pinstep
sprintf(syscom,"cd servuino;./servuino %d %d %d %d %d %d %d;",confSteps,g_scenSource,g_pinType,g_pinNo,x,currentStep,S_ADD);
tmp=system(syscom);
initSim();
readSketchInfo();
readSimulation();
goStep(currentStep);
readMsg(fileServScen);
}
}
else
putMsg(2,"Cancelled!");
}
else
putMsg(2,"Next step is not a Read event");
}
else
{
sprintf(temp,"Unknown command: %c",ch);
putMsg(msg_h-2,temp);
}
}
return;
}
//====================================
void openCommand()
//====================================
{
struct stat st;
int ch,nsteps=1000,x,i,n,stop=0,loop,projNo = 0,ok=0,tmp;
char *p,str[120],sstr[20],fileName[120],temp[120],syscom[120];
char command[40][40];
s_mode = S_ADMIN;
g_silent = 0;
readMsg(gplFile);
while(strstr(str,"ex") == NULL)
{
anyErrors();
unoInfo();
wmove(uno,board_h-2,1);
wprintw(uno," ");
if(g_silent==S_NO )mvwprintw(uno,board_h-2,1,"A%1d>",confWinMode);
if(g_silent==S_YES)mvwprintw(uno,board_h-2,1,"A%1d<",confWinMode);
strcpy(command[0],"");
wgetstr(uno,str);
n = tokCommand(command,str);
strcpy(sstr,command[0]);
p = str;
projNo = atoi(sstr);
if(strstr(sstr,"gpl"))
{
readMsg(gplFile);
}
else if(strstr(sstr,"err"))
{
readMsg(fileTemp);
}
else if(strstr(sstr,"run"))
{
stop = 1;
if(n == 2)stop = atoi(command[1]);
stop = checkRange(HEAL,"step",stop);
runMode(stop);
if(stop==0)putMsg(2,"Back in Admin Mode!");
}
else if(strstr(sstr,"res")) // reset simulation
{
resetSim();
init(confWinMode);
unoInfo();
readMsg(currentConf);
}
else if(strstr(sstr,"help")) //
{
strcpy(fileName,fileInfoAdmin);
readMsg(fileName);
}
else if(strstr(sstr,"rem")) //
{
if(n == 4)
{
if(strstr(command[1],"a"))g_pinType = ANA;
if(strstr(command[1],"d"))g_pinType = DIG;
g_pinNo = atoi(command[2]);
g_pinStep = atoi(command[3]);
if(g_pinType == ANA)
ok = checkRange(S_OK,"anapin",g_pinNo);
if(g_pinType == DIG)
ok = checkRange(S_OK,"digpin",g_pinNo);
if(ok == S_OK)
{
g_scenSource = 1;
sprintf(syscom,"cd servuino;./servuino %d %d %d %d %d %d %d;",confSteps,g_scenSource,g_pinType,g_pinNo,0,g_pinStep,S_DELETE);
//putMsg(2,syscom);
tmp=system(syscom);
initSim();
readSketchInfo();
readSimulation();
runStep(S_FORWARD);
readMsg(fileServScen);
}
else
putMsg(2,"Wrong pin number or pin type!");
}
else
putMsg(2,"Syntax: rem <a or d> <pin> <step>");
}
else if(strstr(sstr,"add")) //
{
if(n == 5)
{
if(strstr(command[1],"a"))g_pinType = ANA;
if(strstr(command[1],"d"))g_pinType = DIG;
g_pinNo = atoi(command[2]);
g_pinStep = atoi(command[3]);
g_pinValue = atoi(command[4]);
ok = S_OK;
if(g_pinType == ANA)
ok = checkRange(S_OK,"anapin",g_pinNo);
if(g_pinType == DIG)
ok = checkRange(S_OK,"digpin",g_pinNo);
ok = ok + checkRange(S_OK,"step",g_pinStep);
if(ok == S_OK)
{
g_scenSource = 1;
sprintf(syscom,"cd servuino;./servuino %d %d %d %d %d %d %d;",confSteps,g_scenSource,g_pinType,g_pinNo,g_pinValue,g_pinStep,S_ADD);
tmp=system(syscom);
initSim();
readSketchInfo();
readSimulation();
runStep(S_FORWARD);
readMsg(fileServScen);
}
}
else
putMsg(2,"Syntax: add <a or d> <pin> <step> <value>");
}
else if(strstr(sstr,"info"))
{
if(n == 2)
{
if(strstr(command[1],"conf"))
{
readMsg(fileDefault);
}
else if(strstr(command[1],"err"))
{
readMsg(fileServError);
}
else if(strstr(command[1],"g++"))
{
readMsg(fileServComp);
}
else if(strstr(command[1],"help"))
{
readMsg(fileInfoAdmin);
}
else if(strstr(command[1],"loop"))
{
showLoops();
}
else if(strstr(command[1],"scen")) // scenario
{
readMsg(fileServScen);
}
}
else
{
readMsg(fileInfoAdmin);
}
}
else if(strstr(sstr,"proj"))
{
if(n == 2)
{
strcpy(currentConf,command[1]);
strcat(currentConf,".conf");
readConfig(currentConf);
g_warning = S_YES;
unoInfo();
}
readMsg(currentConf);
}
else if(strstr(sstr,"list"))
{
readMsg(fileProjList);
}
else if(strstr(sstr,"sketch"))
{
if(n == 2)
{
if(strstr(command[1],"conf"))
{
readMsg(confSketchFile);
}
else if(strstr(command[1],"work"))
{
readMsg(fileServSketch);
}
}
else
readMsg(fileServSketch);
}
else if(strstr(sstr,"conf"))
{
if(n == 3)
{
if(strstr(command[1],"sim"))
{
confSteps = atoi(command[2]);
}
else if(strstr(command[1],"win"))
{
confWinMode = atoi(command[2]);
if(confWinMode > WIN_MODES)confWinMode = 0;
init(confWinMode);
unoInfo();
}
else if(strstr(command[1],"sketch"))
{
strcpy(temp,command[2]);
if(stat(temp,&st) == 0)
{
strcpy(confSketchFile,command[2]);
}
else
{
sprintf(temp,"Sketch not found: %s",command[2]);// Issue 16
putMsg(msg_h-2,temp);
}
}
saveConfig(currentConf);
}
readMsg(currentConf);
}
else if(strstr(sstr,"sav")) //save config
{
if(n == 2)
{
strcpy(currentConf,command[1]);
strcat(currentConf,".conf");
}
saveConfig(currentConf);
readMsg(currentConf);
sprintf(syscom,"ls *.conf > %s;",fileProjList);
x=system(syscom);
}
else if(strstr(sstr,"del")) //delete config
{
if(n == 2)
{
strcpy(currentConf,command[1]);
strcat(currentConf,".conf");
}
if(strstr(currentConf,"default") == NULL)
{
sprintf(syscom,"rm %s;",currentConf);
x=system(syscom);
sprintf(syscom,"ls *.conf > %s;",fileProjList);
x=system(syscom);
readMsg(fileProjList);
strcpy(currentConf,fileDefault);
}
}
else if(strstr(sstr,"win")) //windows layout
{
if(n == 2)
{
confWinMode = atoi(command[1]);
if(confWinMode > WIN_MODES)confWinMode = 0;
init(confWinMode);
unoInfo();
}
}
else if(strstr(sstr,"loop"))
{
if(n == 2)loop = atoi(command[1]);
loop = checkRange(HEAL,"loop",loop);
runLoops(loop);
}
else if(strstr(sstr,"clear"))
{
sprintf(syscom,"rm servuino/sketch.ino;rm servuino/data.su;rm servuino/data.scen;");
x=system(syscom);
}
else if(strstr(sstr,"load"))
{
if(n == 2)
{
confSteps = atoi(command[1]);
}
g_scenSource = 0;
loadCurrentProj();
}
else if(projNo > 0 && projNo < 10)
{
selectProj(projNo,currentConf);
readConfig(currentConf);
g_warning = S_YES;
unoInfo();
readMsg(currentConf);
}
else if(strstr(sstr,"data"))
{
if(n == 2)
{
if(strstr(command[1],"ard"))
{
readMsg(fileServArduino);
}
else if(strstr(command[1],"cus"))
{
readMsg(fileServCustom);
}
else if(strstr(command[1],"cod"))
{
readMsg(fileServCode);
}
else if(strstr(command[1],"err"))
{
readMsg(fileServError);
}
else if(strstr(command[1],"sce"))
{
readMsg(fileServScen);
}
}
else
readMsg(fileServArduino);
}
else
{
putMsg(msg_h-2,"Unknown Admin command");
}
}
}
void start()
{
world = dWorldCreate();
dWorldSetGravity (world,0,0,-9.8);
contact_group = dJointGroupCreate(0);
space = dSimpleSpaceCreate (0);
// first, the ground plane
// it has to coincide with the plane we have in drawstuff
ground = dCreatePlane(space, 0, 0, 1, 0);
// now a ball
dMass m;
dMassSetSphere(&m, 0.1, ball_radius);
ball1_geom = dCreateSphere(space, ball_radius);
ball1_body = dBodyCreate(world);
dGeomSetBody(ball1_geom, ball1_body);
dBodySetMass(ball1_body, &m);
ball2_geom = dCreateSphere(space, ball_radius);
ball2_body = dBodyCreate(world);
dGeomSetBody(ball2_geom, ball2_body);
dBodySetMass(ball2_body, &m);
// tracks made out of boxes
dGeomID trk;
dMatrix3 r1, r2, r3;
dVector3 ro = {0, -(0.5*track_gauge + 0.5*track_width), track_elevation};
dMatrix3 s1, s2, s3;
dVector3 so = {0, 0.5*track_gauge + 0.5*track_width, track_elevation};
dRFromAxisAndAngle(r1, 1, 0, 0, track_angle);
dRFromAxisAndAngle(r2, 0, 1, 0, -track_incl);
dMultiply0_333(r3, r2, r1);
dRFromAxisAndAngle(s1, 1, 0, 0, -track_angle);
dRFromAxisAndAngle(s2, 0, 1, 0, -track_incl);
dMultiply0_333(s3, s2, s1);
trk = dCreateBox(space, track_len, track_width, track_height);
dGeomSetPosition(trk, ro[0], ro[1] + balls_sep, ro[2]);
dGeomSetRotation(trk, r3);
trk = dCreateBox(space, track_len, track_width, track_height);
dGeomSetPosition(trk, so[0], so[1] + balls_sep, so[2]);
dGeomSetRotation(trk, s3);
// tracks made out of trimesh
for (unsigned i=0; i<n_box_verts; ++i) {
dVector3 p;
dMultiply0_331(p, s3, box_verts[i]);
dAddVectors3(p, p, so);
dCopyVector3(track_verts[i], p);
}
// trimesh tracks 2, transform all vertices by s3
for (unsigned i=0; i<n_box_verts; ++i) {
dVector3 p;
dMultiply0_331(p, r3, box_verts[i]);
dAddVectors3(p, p, ro);
dCopyVector3(track_verts[n_box_verts + i], p);
}
// copy face indices
for (unsigned i=0; i<n_box_faces; ++i)
for (unsigned j=0; j<3; ++j) // each face index
track_faces[3*i+j] = box_faces[3*i+j];
for (unsigned i=0; i<n_box_faces; ++i)
for (unsigned j=0; j<3; ++j) // each face index
track_faces[3*(i + n_box_faces)+j] = box_faces[3*i+j] + n_box_verts;
mesh_data = dGeomTriMeshDataCreate();
dGeomTriMeshDataBuildSimple(mesh_data,
track_verts[0], n_track_verts,
track_faces, 3*n_track_faces);
mesh_geom = dCreateTriMesh(space, mesh_data, 0, 0, 0);
resetSim();
// initial camera position
static float xyz[3] = {-5.9414,-0.4804,2.9800};
static float hpr[3] = {32.5000,-10.0000,0.0000};
dsSetViewpoint (xyz,hpr);
dsSetSphereQuality(3);
}
