int LegsDetector::getLocation(int personID) //throw (LegsDetectorException)
{
if ((personID >= 0) && (personID < _howMany))
{
if ( RTOD(_target[personID].bearing) <-45 ) return +3;
else if ( RTOD(_target[personID].bearing) <-15 ) return +2;
else if ( RTOD(_target[personID].bearing) < 0 ) return +1;
else if ( RTOD(_target[personID].bearing) <+15 ) return -1;
else if ( RTOD(_target[personID].bearing) <+45 ) return -2;
else return -3;
}
//throw LegsDetectorException("Index out of boundaries in LegsDetector::getDirection(int personID)", EINVAL);
return 0;
}
void
graph_error_func (struct experiment_params *params)
{
double speed, angle, err;
FILE *f;
gsl_vector *args;
args = gsl_vector_alloc (params->minimizer_dimen);
if ((f = fopen ("err.dat", "w")) == NULL) {
fprintf (stderr, "can't create err.dat\n");
exit (1);
}
for (speed = 0; speed < 80; speed += 1) {
for (angle = DTOR (0); angle < DTOR (50); angle += DTOR (1)) {
gsl_vector_set (args, 0, speed);
gsl_vector_set (args, 1, angle);
err = compute_error_func (args, params);
fprintf (f, "%.14g %.14g %.14g\n",
speed, RTOD (angle), err);
}
fprintf (f, "\n");
}
fclose (f);
}
const float Collision::segment_Point_atan(const Segment* segment, const GSvector2* point)const
{
GSvector2 pp = segment->vector();
GSvector2 pm = *point - segment->p2;
float dot = gsVector2Dot(&pp,&pm);
float ccw = gsVector2CCW(&pp, &pm);
return RTOD(std::atan2(ccw, dot));
}
double
compute_error_func (const gsl_vector *proposed_vars, void *params_raw)
{
struct experiment_params *params = params_raw;
double dist_err, secs_err, err;
char filename[1000];
double proposed_speed, proposed_angle;
proposed_speed = gsl_vector_get (proposed_vars, 0);
proposed_angle = gsl_vector_get (proposed_vars, 1);
if (vflag) {
sprintf (filename, "sim%04.1f-%04.1f.dat",
proposed_speed, RTOD (proposed_angle));
if ((outf = fopen (filename, "w")) == NULL) {
fprintf (stderr, "can't create %s\n", filename);
exit (1);
}
printf ("%s\n", filename);
}
do_simulation (proposed_vars, params);
if (vflag) {
fclose (outf);
}
dist_err = params->sim_dist - params->observed_dist;
secs_err = params->sim_secs - params->observed_secs;
err = dist_err * dist_err + 100 * secs_err * secs_err;
if (vflag) {
printf ("%10.6f %10.6f %10.6f %10.6f %10.6f %10.6f\n",
params->sim_secs,
gsl_vector_get (proposed_vars, 0),
RTOD (gsl_vector_get (proposed_vars, 1)),
dist_err, secs_err, err);
}
return (err);
}
int TE::SetupOdom() {
// Setup the output position device
if (!(odom = deviceTable->GetDevice(odom_addr))) {
PLAYER_ERROR("TE::SetupOdom Unable to locate suitable output position device");
return -1;
}
if (odom->Subscribe(InQueue) != 0) {
PLAYER_ERROR("TE::SetupOdom Unable to subscribe to output position device");
return -1;
}
// Setup the input position device
if (!(localize = deviceTable->GetDevice(localize_addr))) {
PLAYER_ERROR("TE::SetupOdom Unable to locate suitable input position device");
return -1;
}
if (localize->Subscribe(InQueue) != 0) {
PLAYER_ERROR("TE::SetupOdom Unable to subscribe to input position device");
return -1;
}
// Get the odometry geometry
Message* msg = 0;
if (!(msg = odom->Request(InQueue, PLAYER_MSGTYPE_REQ, PLAYER_POSITION2D_REQ_GET_GEOM, NULL, 0, NULL, false))
|| (msg->GetHeader()->size != sizeof (player_position2d_geom_t))) {
PLAYER_ERROR("TE::SetupOdom Failed to get geometry of underlying position device");
if (msg)
delete msg;
return (-1);
}
player_position2d_geom_t* geom = (player_position2d_geom_t*) msg->GetPayload();
robot_geom = *geom;
PLAYER_MSG5(0, "TE::SetupOdom Robot geom: %.3f %.3f %.3f %.3f %.3f",
robot_geom.size.sl,
robot_geom.size.sw,
robot_geom.pose.px,
robot_geom.pose.py,
RTOD(robot_geom.pose.pa));
delete msg;
memset(&odom_pose, 0, sizeof (player_pose_t));
memset(&odom_vel, 0, sizeof (player_pose_t));
odom_stall = false;
return 0;
}
void
TE::ProcessCommand(player_msghdr_t*, player_position2d_cmd_pos_t* cmd) {
PLAYER_MSG3(2, "TE::ProcessCommand New goal: (%.3f %.3f %.3f)",
cmd->pos.px,
cmd->pos.py,
RTOD(cmd->pos.pa));
// position control; cache the goal and we'll process it in the main
// loop. also cache vel.px, which tells us whether to go forward or
// backward to get to the goal.
goal = cmd->pos;
active_goal = true;
turning_in_place = false;
stall = false;
GlobalTime->GetTimeDouble(&translate_start_time);
last_odom_pose = odom_pose;
}
void
TE::ProcessCommand(player_msghdr_t*, player_position2d_cmd_vel_t* cmd) {
if (!cmd->state) {
PutPositionCmd(0.0, 0.0);
active_goal = false;
} else {
PLAYER_MSG2(2, "TE::ProcessCommand Stopped by velocity command (%.3f %.3f)",
cmd->vel.px, RTOD(cmd->vel.pa));
// TODO: bylo wylaczone w ostatniej wersji, ale wtedy nie zatrzymuje sie
PutPositionCmd(cmd->vel.px, cmd->vel.pa);
active_goal = false;
}
if (cmd->vel.px < 0)
dir = -1;
else
dir = 1;
}
/**
* @brief Disactivate all robot collisions.
* @param r The robot
*/
void p3d_setAllDofPassive(p3d_rob * r){
for(int j = 0; j < r->mg->nbGraphs; j++){
p3d_multiGraphJoint* mgJoints = r->mg->mgJoints[j];
for(int i = mgJoints->nbJoints - 1; i >= 0; i--){//Activer les contraintes pour les jnts utilisé et desactiver les collisions
p3d_jnt * jnt = r->joints[mgJoints->joints[i]];
if(mgJoints->cntrts[i] != -1){
p3d_cntrt * ct = r->cntrt_manager->cntrts[mgJoints->cntrts[i]];
if(ct){
ct->argu_d[0] = RTOD(jnt->dof_data[0].v);
if(p3d_update_constraint(ct, 1)) {
if (ct->enchained != NULL)
p3d_reenchain_cntrts(ct);
p3d_col_deactivate_one_cntrt_pairs(ct);
}
}
}
p3d_col_deactivate_obj_env(jnt->o);
}
r->mg->active[j] = 0;
}
p3d_col_deactivate_rob(r);
// p3d_desactivate_col_check_all();//mettre a -1 tous les bodys
// p3d_autocol_activate_rob(r); //traitement des paires
}
void
draw (void)
{
GLfloat light0_diffuse[] = {1, 1, 1, 0};
GLfloat light0_position[] = {1, 1, 1, 0};
GLfloat light1_ambient[] = {.5, .5, .5, 0};
GLfloat light1_position[] = {0, 0, 1, 0};
GLfloat light2_ambient[] = {.1, .1, .1, 0};
GLfloat light2_position[] = {100, 100, 100, 0};
glLoadIdentity ();
glClear (GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT);
distanceLookAt (player.p.x, player.p.y, player.p.z + .7, player.camdist,
playercamera.theta, playercamera.phi);
color_coords (1);
glDisable (GL_LIGHTING);
glBegin (GL_LINES);
glColor3f (1, 0, 0);
glVertex3f (player.p.x, player.p.y, player.p.z + 1);
glVertex3f (player.p.x + player.vel.x * 10,
player.p.y + player.vel.y * 10,
player.p.z + player.vel.z * 10 + 1);
glEnd ();
glLightfv (GL_LIGHT0, GL_DIFFUSE, light0_diffuse);
glLightfv (GL_LIGHT0, GL_POSITION, light0_position);
glLightfv (GL_LIGHT1, GL_AMBIENT, light1_ambient);
glLightfv (GL_LIGHT1, GL_POSITION, light1_position);
glLightfv (GL_LIGHT2, GL_AMBIENT, light2_ambient);
glLightfv (GL_LIGHT2, GL_POSITION, light2_position);
glEnable (GL_LIGHTING);
glEnable (GL_LIGHT1);
glEnable (GL_LIGHT2);
glCallList (terrain_list);
//start test
//end test
glDisable (GL_LIGHT2);
glPushMatrix ();
glTranslatef (player.p.x, player.p.y, player.p.z + .7);
glRotatef (RTOD (player.theta), 0, 0, 1);
glRotatef (90, 1, 0, 0);
glEnable (GL_LIGHT0);
glutSolidTeapot (1);
glDisable (GL_LIGHT0);
glDisable (GL_LIGHTING);
glFlush ();
SDL_GL_SwapBuffers ();
}
int
main (int argc, char **argv)
{
struct experiment_params params;
double dx, dy;
double speed, angle;
gsl_vector *solution;
int c;
double compute_start, delta;
while ((c = getopt (argc, argv, "v")) != EOF) {
switch (c) {
case 'v':
vflag = 1;
break;
default:
usage ();
}
}
memset (¶ms, 0, sizeof params);
params.observed_hit[0] = 0;
params.observed_hit[1] = 0;
params.observed_hit[2] = 1;
params.observed_bounce[0] = 25;
params.observed_bounce[1] = 0;
params.observed_bounce[2] = 0;
params.observed_secs = 1.359;
dx = params.observed_bounce[0] - params.observed_hit[0];
dy = params.observed_bounce[1] - params.observed_hit[1];
params.observed_dist = hypot (dy, dx);
params.simulator_dimen = 4;
params.odesys.function = sim_func;
params.odesys.dimension = params.simulator_dimen;
params.odesys.params = ¶ms;
params.stepper = gsl_odeiv_step_alloc (gsl_odeiv_step_rk8pd,
params.simulator_dimen);
params.controller = gsl_odeiv_control_y_new (1e-6, 0.0);
params.evolver = gsl_odeiv_evolve_alloc (params.simulator_dimen);
params.minimizer_dimen = 2;
params.starting_point = gsl_vector_alloc (params.minimizer_dimen);
params.minimizer_step_sizes = gsl_vector_alloc (params.minimizer_dimen);
params.minimizer = gsl_multimin_fminimizer_alloc
(gsl_multimin_fminimizer_nmsimplex2, params.minimizer_dimen);
gsl_vector_set_all (params.starting_point, 0);
gsl_vector_set_all (params.minimizer_step_sizes, 1.0);
if (0) {
graph_error_func (¶ms);
}
compute_start = get_secs ();
solve_by_simulation (¶ms);
delta = get_secs () - compute_start;
solution = gsl_multimin_fminimizer_x (params.minimizer);
speed = gsl_vector_get (solution, 0);
angle = gsl_vector_get (solution, 1);
printf ("speed = %8.3f angle = %8.3f; compute time %.3fms\n",
speed * METERS_PER_SEC_TO_MPH,
RTOD (angle),
delta * 1000);
return (0);
}
/*
* Update the screen.
*/
void scr_update(void)
{
cell *bp;
cell *sp;
cell so;
cell cur_so = 0;
int i;
int j;
int ccol;
/* Always leave cursor after last displayed point */
curscreen[D_LAST * B_COLS - 1] = -1;
if (score != curscore) {
moveto(0, 0);
printf("Score: %d", score);
curscore = score;
}
/* Draw preview of next pattern */
if ((showpreview) && (nextshape != lastshape)) {
int i;
static int r = 5, c = 2;
int tr, tc, t;
lastshape = nextshape;
/* Clean */
resume_normal();
moveto(r - 1, c - 1);
putstr(" ");
moveto(r, c - 1);
putstr(" ");
moveto(r + 1, c - 1);
putstr(" ");
moveto(r + 2, c - 1);
putstr(" ");
moveto(r - 3, c - 2);
putstr("Next shape:");
/* Draw */
start_standout(nextshape->color);
moveto(r, 2 * c);
putstr(" ");
for (i = 0; i < 3; i++) {
t = c + r * B_COLS;
t += nextshape->off[i];
tr = t / B_COLS;
tc = t % B_COLS;
moveto(tr, 2*tc);
putstr(" ");
}
resume_normal();
}
bp = &board[D_FIRST * B_COLS];
sp = &curscreen[D_FIRST * B_COLS];
for (j = D_FIRST; j < D_LAST; j++) {
ccol = -1;
for (i = 0; i < B_COLS; bp++, sp++, i++) {
if (*sp == (so = *bp))
continue;
*sp = so;
if (i != ccol) {
if (cur_so) {
resume_normal();
cur_so = 0;
}
moveto(RTOD(j), CTOD(i));
}
if (so != cur_so) {
if (so)
start_standout(so);
else
resume_normal();
cur_so = so;
}
putstr(" ");
ccol = i + 1;
/*
* Look ahead a bit, to avoid extra motion if
* we will be redrawing the cell after the next.
* Motion probably takes four or more characters,
* so we save even if we rewrite two cells
* `unnecessarily'. Skip it all, though, if
* the next cell is a different color.
*/
if ((i > STOP) || (sp[1] != bp[1]) || (so != bp[1]))
continue;
if (sp[2] != bp[2])
sp[1] = -1;
else if ((i < STOP) && (so == bp[2]) && (sp[3] != bp[3])) {
sp[2] = -1;
sp[1] = -1;
}
}
}
if (cur_so)
resume_normal();
console_flush(console);
}
