char *loadPageFrom(off_t page, u_int file_id) {
if(!fileCache) {
fileCache = newGenericBinaryTree();
}
u_int storKey = file_id << 4;
storKey = storKey | (u_int) page;
printf("DEBUG :: loadPageFrom :: page ::= %d :: file_id ::= %d\n",page,file_id);
char *entryData = (char *)getKeyData(fileCache, storKey);
if(entryData == NULL) {
readPageFrom(page, file_id);
}
entryData = (char *)getKeyData(fileCache,storKey); // bad, bad, bad
// printf("DEBUG :: loadPageFrom :: entryData ::= %s.10\n",entryData);
return entryData;
}
void
setKeymap(char *p, int lineno, int verbose)
{
unsigned char *map = NULL;
char *s, *emsg;
int c, f;
s = getQWord(&p);
c = getKey(s);
if (c < 0) { /* error */
if (lineno > 0)
/* FIXME: gettextize? */
emsg = Sprintf("line %d: unknown key '%s'", lineno, s)->ptr;
else
/* FIXME: gettextize? */
emsg = Sprintf("defkey: unknown key '%s'", s)->ptr;
record_err_message(emsg);
if (verbose)
disp_message_nsec(emsg, FALSE, 1, TRUE, FALSE);
return;
}
s = getWord(&p);
f = getFuncList(s);
if (f < 0) {
if (lineno > 0)
/* FIXME: gettextize? */
emsg = Sprintf("line %d: invalid command '%s'", lineno, s)->ptr;
else
/* FIXME: gettextize? */
emsg = Sprintf("defkey: invalid command '%s'", s)->ptr;
record_err_message(emsg);
if (verbose)
disp_message_nsec(emsg, FALSE, 1, TRUE, FALSE);
return;
}
if (c & K_MULTI) {
unsigned char **mmap = NULL;
int i, j, m = MULTI_KEY(c);
if (m & K_ESCD)
map = EscDKeymap;
else if (m & K_ESCB)
map = EscBKeymap;
else if (m & K_ESC)
map = EscKeymap;
else
map = GlobalKeymap;
if (map[m & 0x7F] == FUNCNAME_multimap)
mmap = (unsigned char **)getKeyData(m);
else
map[m & 0x7F] = FUNCNAME_multimap;
if (!mmap) {
mmap = New_N(unsigned char *, 4);
for (i = 0; i < 4; i++) {
mmap[i] = New_N(unsigned char, 128);
for (j = 0; j < 128; j++)
mmap[i][j] = FUNCNAME_nulcmd;
}
mmap[0][ESC_CODE] = FUNCNAME_escmap;
mmap[1]['['] = FUNCNAME_escbmap;
mmap[1]['O'] = FUNCNAME_escbmap;
}
void render() {
glClearColor(0.0, 0.0, 0.0, 1);
glClearDepth(1.0f);
glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT);
glLoadIdentity();
float mtime, seconds, useconds;
gettimeofday(&new_timeval, NULL);
seconds = new_timeval.tv_sec - previous_timeval.tv_sec;
useconds = new_timeval.tv_usec - previous_timeval.tv_usec;
mtime = seconds + useconds/1000000;
getHuniplacerData()->modeldata.deltatime = mtime;
float dif = mtime * 40;
previous_timeval = new_timeval;
if (getKeyData()->key_left)
yRotation += dif;
if (getKeyData()->key_right)
yRotation -= dif;
if (getKeyData()->key_plus)
getHuniplacerData()->modeldata.robot->setSpeed(getHuniplacerData()->modeldata.robot->getSpeed() + dif*3);
if (getKeyData()->key_min)
getHuniplacerData()->modeldata.robot->setSpeed(getHuniplacerData()->modeldata.robot->getSpeed() - dif*3);
if (getKeyData()->key_up)
xRotation += dif;
if (getKeyData()->key_down)
xRotation -= dif;
if (getKeyData()->key_w)
getHuniplacerData()->modeldata.pivot.y += dif;
if (getKeyData()->key_s)
getHuniplacerData()->modeldata.pivot.y -= dif;
if (getKeyData()->key_d)
getHuniplacerData()->modeldata.pivot.x += dif;
if (getKeyData()->key_a)
getHuniplacerData()->modeldata.pivot.x -= dif;
if (getKeyData()->key_q)
getHuniplacerData()->modeldata.pivot.z += dif;
if (getKeyData()->key_e)
getHuniplacerData()->modeldata.pivot.z -= dif;
if (getHuniplacerData()->modeldata.pivot.x < huniplacer::measures::MIN_X)
getHuniplacerData()->modeldata.pivot.x = huniplacer::measures::MIN_X;
if (getHuniplacerData()->modeldata.pivot.x > huniplacer::measures::MAX_X)
getHuniplacerData()->modeldata.pivot.x = huniplacer::measures::MAX_X;
if (getHuniplacerData()->modeldata.pivot.y < huniplacer::measures::MIN_Y)
getHuniplacerData()->modeldata.pivot.y = huniplacer::measures::MIN_Y;
if (getHuniplacerData()->modeldata.pivot.y > huniplacer::measures::MAX_Y)
getHuniplacerData()->modeldata.pivot.y = huniplacer::measures::MAX_Y;
if (getHuniplacerData()->modeldata.pivot.z < huniplacer::measures::MIN_Z)
getHuniplacerData()->modeldata.pivot.z = huniplacer::measures::MIN_Z;
if (getHuniplacerData()->modeldata.pivot.z > huniplacer::measures::MAX_Z)
getHuniplacerData()->modeldata.pivot.z = huniplacer::measures::MAX_Z;
if(getHuniplacerData()->modeldata.robot->getSpeed() < 20)
getHuniplacerData()->modeldata.robot->setSpeed(20);
if(getHuniplacerData()->modeldata.robot->getSpeed() > 500)
getHuniplacerData()->modeldata.robot->setSpeed(500);
glRotatef(xRotation, 1, 0, 0);
glRotatef(yRotation, 0, 1, 0);
glColor3f(1.0f, 0, 0);
glPushMatrix();
glTranslatef(getHuniplacerData()->modeldata.pivot.x + 0.5, getHuniplacerData()->modeldata.pivot.z, getHuniplacerData()->modeldata.pivot.y);
drawCube(0, 0, 0, 5, 1, 1);
glPopMatrix();
glPushMatrix();
glColor3f(0, 1, 0);
glTranslatef(getHuniplacerData()->modeldata.pivot.x, getHuniplacerData()->modeldata.pivot.z + 0.5, getHuniplacerData()->modeldata.pivot.y);
drawCube(0, 0, 0, 1, 5, 1);
glPopMatrix();
glPushMatrix();
glColor3f(0, 0, 1);
glTranslatef(getHuniplacerData()->modeldata.pivot.x, getHuniplacerData()->modeldata.pivot.z, getHuniplacerData()->modeldata.pivot.y + 0.5);
drawCube(0, 0, 0, 1, 1, 5);
glPopMatrix();
getHuniplacerData()->modeldata.robot->draw();
glColor4f(0.3, 0.3, 0.3, 0.1);
glPushMatrix();
glDepthMask(GL_FALSE);
glCallList(cubelist);
glDepthMask(GL_TRUE);
glPopMatrix();
glLoadIdentity();
char str[256];
glDisable(GL_DEPTH_TEST);
glColor4f(1.0f, 0.0f, 0.0f, 1.0f);
glRasterPos2i(-(screen_width / 5), 65);
sprintf(str, "pivot location x:%g", getHuniplacerData()->modeldata.pivot.x);
glutBitmapString(GLUT_BITMAP_HELVETICA_10, (unsigned char*) str);
glRasterPos2i(-(screen_width / 5), 55);
sprintf(str, "pivot location y:%g", getHuniplacerData()->modeldata.pivot.y);
glutBitmapString(GLUT_BITMAP_HELVETICA_10, (unsigned char*) str);
glRasterPos2i(-(screen_width / 5), 45);
sprintf(str, "pivot location z:%g", getHuniplacerData()->modeldata.pivot.z);
glutBitmapString(GLUT_BITMAP_HELVETICA_10, (unsigned char*) str);
float rx, ry, rz;
getHuniplacerData()->modeldata.robot->getPosition(&rx, &ry, &rz);
glRasterPos2i(-(screen_width / 5), 35);
sprintf(str, "robot location x:%g", rx);
glutBitmapString(GLUT_BITMAP_HELVETICA_10, (unsigned char*) str);
glRasterPos2i(-(screen_width / 5), 25);
sprintf(str, "robot location y:%g", ry);
glutBitmapString(GLUT_BITMAP_HELVETICA_10, (unsigned char*) str);
glRasterPos2i(-(screen_width / 5), 15);
sprintf(str, "robot location z:%g", rz);
glutBitmapString(GLUT_BITMAP_HELVETICA_10, (unsigned char*) str);
glRasterPos2i(-(screen_width / 5), 5);
sprintf(str, "speed:%g", getHuniplacerData()->modeldata.robot->getSpeed());
glutBitmapString(GLUT_BITMAP_HELVETICA_10, (unsigned char*) str);
glEnable(GL_DEPTH_TEST);
glutSwapBuffers();
ros::spinOnce();
if(!ros::ok()){
glutDestroyWindow(glutGetWindow());
}
}
