/** * @brief The pseudo-ai of the beam weapons. * * @param w Weapon to do the thinking. * @param dt Current delta tick. */ static void think_beam( Weapon* w, const double dt ) { (void)dt; Pilot *p, *t; double diff; Vector2d v; /* Get pilot, if pilot is dead beam is destroyed. */ p = pilot_get(w->parent); if (p==NULL) { w->timer = -1.; /* Hack to make it get destroyed next update. */ return; } /* Check if pilot has enough energy left to keep beam active. */ p->energy -= dt*w->outfit->u.bem.energy; if (p->energy < 0.) { p->energy = 0.; w->timer = -1; return; } /* Use mount position. */ pilot_getMount( p, w->mount, &v ); w->solid->pos.x = p->solid->pos.x + v.x; w->solid->pos.y = p->solid->pos.y + v.y; /* Handle aiming. */ switch (w->outfit->type) { case OUTFIT_TYPE_BEAM: w->solid->dir = p->solid->dir; break; case OUTFIT_TYPE_TURRET_BEAM: /* Get target, if target is dead beam stops moving. */ t = pilot_get(w->target); if (t==NULL) { weapon_setTurn( w, 0. ); return; } if (w->target == w->parent) /* Invalid target, tries to follow shooter. */ diff = angle_diff(w->solid->dir, p->solid->dir); else diff = angle_diff(w->solid->dir, /* Get angle to target pos */ vect_angle(&w->solid->pos, &t->solid->pos)); weapon_setTurn( w, CLAMP( -w->outfit->u.bem.turn, w->outfit->u.bem.turn, 10 * diff * w->outfit->u.bem.turn )); break; default: return; } }
tf::Pose MotionModel::updateAction(tf::Pose& p) { double delta_rot1; if (dxy < 0.01) delta_rot1 = 0.0; else delta_rot1 = angle_diff(atan2(delta_y, delta_x), delta_yaw); double delta_trans = dxy; double delta_rot2 = angle_diff(delta_yaw, delta_rot1); double delta_rot1_noise = std::min(fabs(angle_diff(delta_rot1,0.0)), fabs(angle_diff(delta_rot1, M_PI))); double delta_rot2_noise = std::min(fabs(angle_diff(delta_rot2,0.0)), fabs(angle_diff(delta_rot2, M_PI))); double delta_rot1_hat = angle_diff(delta_rot1, sampleGaussian(alpha1 * delta_rot1_noise*delta_rot1_noise + alpha2 * delta_trans*delta_trans)); double delta_trans_hat = delta_trans - sampleGaussian(alpha3 * delta_trans*delta_trans + alpha4 * delta_rot1_noise*delta_rot1_noise + alpha4 * delta_rot2_noise*delta_rot2_noise); double delta_rot2_hat = angle_diff(delta_rot2, sampleGaussian(alpha1 * delta_rot2_noise*delta_rot2_noise + alpha2 * delta_trans*delta_trans)); delta_x = delta_trans_hat * cos(delta_rot1_hat); delta_y = delta_trans_hat * sin(delta_rot1_hat); delta_yaw = delta_rot1_hat + delta_rot2_hat; tf::Pose noisy_pose(tf::createQuaternionFromRPY(0,0,delta_yaw),tf::Vector3(delta_x,delta_y,0)); tf::Transform base_to_global_ = tf::Transform(p.getRotation()); noisy_pose.setOrigin(base_to_global_ * noisy_pose.getOrigin()); p.setOrigin(p.getOrigin() + noisy_pose.getOrigin()); p.setRotation(p.getRotation() * noisy_pose.getRotation()); }
void SDK_mainloop(void) { //#ifdef MATLAB //SDK_matlabMainLoop(); //this runs only in combination with the AscTec Simulink Toolkit //jeti telemetry can always be activated. You may deactivate this call if you don't have the AscTec Telemetry package. //SDK_jetiAscTecExampleRun(); //#else //write your own C-cod e within this function imusensor.dT = (float) RO_ALL_Data.flight_time; imusensor.dThetax = (3.14159/180.0)*(((float) RO_ALL_Data.angle_roll) / 1000.0); imusensor.dThetay = (3.14159/180.0)*(((float) RO_ALL_Data.angle_pitch) / 1000.0); imusensor.dThetaz = angle_diff((3.14159 / 180.0) * (((float) RO_ALL_Data.angle_yaw) / 1000.0),0); imusensor.dOmegax = (3.1415 / 180.0) * 0.0154 * (float) IMU_CalcData.angvel_roll; imusensor.dOmegay = (3.1415 / 180.0) * 0.0154 * (float) IMU_CalcData.angvel_nick; imusensor.dOmegaz = (3.1415 / 180.0) * 0.0154 * (float) IMU_CalcData.angvel_yaw; lab(); //SDK_jetiAscTecExampleRun(); //if (wpExampleActive) //this is used to activate the waypoint example via the jeti telemetry display // SDK_EXAMPLE_gps_waypoint_control(); //#endif }
float getAngleDiff(int32 x0, int32 y0, int32 x1, int32 y1) { if (y0 != y1 || x0 != x1) { float inner_radius = game->getWorldRadius().x; CIwFVec2 start_pos_world = worldify(x0, y0, inner_radius, game->getRotation()); CIwFVec2 end_pos_world = worldify(x1, y1, inner_radius, game->getRotation()); return 1.25 * angle_diff(start_pos_world, end_pos_world); // Feels a little more realistic } else { return 0.0; } }
/** Determine if the gradient converges or diverges to/from the centre. Return 1 if gradient diverges, 0 if it converges. Ellipse case. */ static int dir_gradient_ell( int px, int py, double ang, double *foci ) { double a; int dir = 0; /* check parameters */ if( foci == NULL ) error("dir_gradient_ell: invalid ellipse."); a = ellipse_normal_angle( (double)px, (double)py, foci ); if( angle_diff( a, ang ) < M_1_2_PI ) dir = 1; return dir; }
/** Determine if the gradient converges or diverges to/from the centre. Return 1 if gradient diverges, 0 if it converges. Circle case. */ static int dir_gradient_circ( int px, int py, double ang, double *param ) { double a; int dir = 0; /* check parameters */ if( param == NULL ) error("dir_gradient_circ: invalid param."); a = atan2( py- param[1], px-param[0] ); if( angle_diff( a, ang ) < M_1_2_PI ) dir = 1; return dir; }
/* ----------------------------------------------------------------------------- Function: interpolate_angle -Linear interpolate between angle A and B by fraction 'f'. Parameters: Returns: Notes: ----------------------------------------------------------------------------- */ INLINECALL float interpolate_angle( float from, float to, float fraction ) { float diff = angle_diff( from, to ) * fraction; if( angle_wise( to, from ) >= M_PI ) { return from - diff; } else { return from + diff; } }
void VectorLocalization2D::computeLocation(vector2f& loc, float& angle) { vector2f heading; vector2f avgHeading; currentLocation.zero(); avgHeading.zero(); for(int i=0; i<numParticles; i++){ currentLocation = currentLocation + particles[i].loc; heading.heading(particles[i].angle); avgHeading = avgHeading + heading; } currentAngle = avgHeading.angle(); currentLocation = currentLocation/float(numParticles); currentLocStdDev.zero(); currentAngleStdDev = 0.0; float xStdDev=0.0, yStdDev=0.0; for(int i=0; i<numParticles; i++){ /* xStdDev += sq(float(particles[i].loc.x-currentLocation.x)); yStdDev += sq(float(particles[i].loc.y-currentLocation.y)); currentAngleStdDev += sq(float(angle_diff(particles[i].angle,currentAngle))); */ xStdDev = max(xStdDev, float(fabs(particles[i].loc.x-currentLocation.x))); yStdDev = max(yStdDev, float(fabs(particles[i].loc.y-currentLocation.y))); currentAngleStdDev = max(currentAngleStdDev, float(fabs(angle_diff(particles[i].angle,currentAngle)))); } /* currentAngleStdDev = sqrt(currentAngleStdDev/float(numParticles-1.0)); xStdDev = sqrt(xStdDev/float(numParticles)); yStdDev = sqrt(yStdDev/float(numParticles)); */ currentLocStdDev.set(xStdDev,yStdDev); loc = currentLocation; angle = currentAngle; }
/** * @brief The AI of seeker missiles. * * @param w Weapon to do the thinking. * @param dt Current delta tick. */ static void think_seeker( Weapon* w, const double dt ) { double diff; double vel; Pilot *p; int effect; Vector2d v; double t; if (w->target == w->parent) return; /* no self shooting */ p = pilot_get(w->target); /* no null pilot_nstack */ if (p==NULL) { weapon_setThrust( w, 0. ); weapon_setTurn( w, 0. ); return; } /* Handle by status. */ switch (w->status) { case WEAPON_STATUS_OK: if (w->lockon < 0.) w->status = WEAPON_STATUS_LOCKEDON; break; case WEAPON_STATUS_LOCKEDON: /* Check to see if can get jammed */ if ((p->jam_range != 0.) && /* Target has jammer and weapon is in range */ (vect_dist(&w->solid->pos,&p->solid->pos) < p->jam_range)) { /* Check to see if weapon gets jammed */ if (RNGF() < p->jam_chance - w->outfit->u.amm.resist) { w->status = WEAPON_STATUS_JAMMED; /* Give it a nice random effect */ effect = RNG(0,3); switch (effect) { case 0: /* Stuck in left loop */ weapon_setTurn( w, w->outfit->u.amm.turn ); break; case 1: /* Stuck in right loop */ weapon_setTurn( w, -w->outfit->u.amm.turn ); break; default: /* Blow up. */ w->timer = -1.; break; } } else /* Can't get jammed anymore */ w->status = WEAPON_STATUS_UNJAMMED; } /* Purpose fallthrough */ case WEAPON_STATUS_UNJAMMED: /* Work as expected */ /* Smart seekers take into account ship velocity. */ if (w->outfit->u.amm.ai == 2) { /* Calculate time to reach target. */ vect_cset( &v, p->solid->pos.x - w->solid->pos.x, p->solid->pos.y - w->solid->pos.y ); t = vect_odist( &v ) / w->outfit->u.amm.speed; /* Calculate target's movement. */ vect_cset( &v, v.x + t*(p->solid->vel.x - w->solid->vel.x), v.y + t*(p->solid->vel.y - w->solid->vel.y) ); /* Get the angle now. */ diff = angle_diff(w->solid->dir, VANGLE(v) ); } /* Other seekers are stupid. */ else { diff = angle_diff(w->solid->dir, /* Get angle to target pos */ vect_angle(&w->solid->pos, &p->solid->pos)); } /* Set turn. */ weapon_setTurn( w, CLAMP( -w->outfit->u.amm.turn, w->outfit->u.amm.turn, 10 * diff * w->outfit->u.amm.turn )); break; case WEAPON_STATUS_JAMMED: /* Continue doing whatever */ /* Do nothing, dir_vel should be set already if needed */ break; default: WARN("Unknown weapon status for '%s'", w->outfit->name); break; } /* Limit speed here */ vel = MIN(w->outfit->u.amm.speed, VMOD(w->solid->vel) + w->outfit->u.amm.thrust*dt); vect_pset( &w->solid->vel, vel, w->solid->dir ); /*limit_speed( &w->solid->vel, w->outfit->u.amm.speed, dt );*/ }
float RSL_get_linear_value(Volume *v,float srange,float azim, float elev,float limit) { /* Compute bilinear value from four surrounding values * in Volume v. The four surrounding values * are at a constant range. * * Limit is an angle used only to reject values * in the azimuth plane. */ float db_value; double value = 0; double up_value, down_value; double delta_angle; Sweep *up_sweep,*down_sweep; get_surrounding_sweep(&down_sweep,&up_sweep,v,elev); /* Calculate interpolated value in sweep above * requested point. */ if(up_sweep == NULL) { up_value = -1; } else { up_value = get_linear_value_from_sweep(up_sweep,srange,azim,limit); } /* Calculate interpolated value in sweep below requested point. */ if(down_sweep == NULL) { down_value = -1; } else { down_value = get_linear_value_from_sweep(down_sweep,srange,azim,limit); } /* Using the interpolated values calculated at the elevation * angles in the above and below sweeps, interpolate a value * for the elvation angle at the requested point. */ if((up_value != -1) && (down_value != -1)) { delta_angle = angle_diff(up_sweep->h.elev,down_sweep->h.elev); value =((angle_diff(elev,up_sweep->h.elev)/delta_angle) * down_value) + ((angle_diff(elev,down_sweep->h.elev)/delta_angle) * up_value); } else if((up_value == -1) && (down_value == -1)) { value = -1; } else if(up_value != -1) { value = up_value; } else { value = down_value; } /* Convert back to dB value and return. */ if(value > 0) { db_value = (float)to_dB(value); return db_value; } else { return BADVAL; } }
double get_linear_value_from_sweep(Sweep *sweep,float srange,float azim, float limit) { float ccw_db_value,cw_db_value; double ccw_value,cw_value,value; double delta_angle; Ray *ccw_ray,*cw_ray; get_surrounding_ray(&ccw_ray,&cw_ray,sweep,azim); /* Assume that ccw_ray and cw_ray will be non_NULL */ if((azim - ccw_ray->h.azimuth) > limit) { ccw_value = -1; } else { ccw_db_value = RSL_get_value_from_ray(ccw_ray,srange); /* Check to make sure this is a valid value */ if (ccw_db_value == BADVAL) { ccw_value = 0; } else { ccw_value = from_dB(ccw_db_value); } } if((cw_ray->h.azimuth - azim) > limit) { cw_value = -1; } else { cw_db_value = RSL_get_value_from_ray(cw_ray,srange); /* Check to make sure this is a valid value */ if (cw_db_value == BADVAL) { cw_value = 0; } else { cw_value = from_dB(cw_db_value); } } if((cw_value != -1) && (ccw_value != -1)) { /* Both the clockwise ray and the counterclockwise * ray is valid. */ delta_angle = angle_diff(ccw_ray->h.azimuth,cw_ray->h.azimuth); value=((angle_diff(azim,cw_ray->h.azimuth)/delta_angle)*ccw_value) + ((angle_diff(azim,ccw_ray->h.azimuth)/delta_angle) * cw_value); } else if((cw_value == -1) && (ccw_value == -1)) { /* Neither ray is valid. */ value = -1; } else if(cw_value != -1) { /* cw_ray is only ray that is within limit. */ value = cw_value; } else { /* ccw_ray is only ray that is within limit. */ value = ccw_value; } return value; }
/** * @brief Handles a click event. */ static void input_clickevent( SDL_Event* event ) { unsigned int pid; int mx, my, mxr, myr, pntid, jpid; int rx, ry, rh, rw, res; int autonav; double x, y, zoom, px, py; double ang, angp, mouseang; HookParam hparam[2]; /* Generate hook. */ hparam[0].type = HOOK_PARAM_NUMBER; hparam[0].u.num = event->button.button; hparam[1].type = HOOK_PARAM_SENTINEL; hooks_runParam( "mouse", hparam ); #if !SDL_VERSION_ATLEAST(2,0,0) /* Handle zoom. */ if (event->button.button == SDL_BUTTON_WHEELUP) { input_clickZoom( 1.1 ); return; } else if (event->button.button == SDL_BUTTON_WHEELDOWN) { input_clickZoom( 0.9 ); return; } #endif /* !SDL_VERSION_ATLEAST(2,0,0) */ /* Player must not be NULL. */ if ((player.p == NULL) || player_isFlag(PLAYER_DESTROYED)) return; /* Player must not be dead. */ if (pilot_isFlag(player.p, PILOT_DEAD)) return; /* Middle mouse enables mouse flying. */ if (event->button.button == SDL_BUTTON_MIDDLE) { player_toggleMouseFly(); return; } /* Mouse targeting only uses left and right buttons. */ if (event->button.button != SDL_BUTTON_LEFT && event->button.button != SDL_BUTTON_RIGHT) return; autonav = (event->button.button == SDL_BUTTON_RIGHT) ? 1 : 0; px = player.p->solid->pos.x; py = player.p->solid->pos.y; gl_windowToScreenPos( &mx, &my, event->button.x, event->button.y ); if ((mx <= 15 || my <= 15 ) || (my >= gl_screen.h - 15 || mx >= gl_screen.w - 15)) { /* Border targeting is handled as a special case, as it uses angles, * not coordinates. */ x = (mx - (gl_screen.w / 2.)) + px; y = (my - (gl_screen.h / 2.)) + py; mouseang = atan2(py - y, px - x); angp = pilot_getNearestAng( player.p, &pid, mouseang, 1 ); ang = system_getClosestAng( cur_system, &pntid, &jpid, x, y, mouseang ); if ((ABS(angle_diff(mouseang, angp)) > M_PI / 64) || ABS(angle_diff(mouseang, ang)) < ABS(angle_diff(mouseang, angp))) pid = PLAYER_ID; /* Pilot angle is too great, or planet/jump is closer. */ if (ABS(angle_diff(mouseang, ang)) > M_PI / 64 ) jpid = pntid = -1; /* Asset angle difference is too great. */ if (!autonav && pid != PLAYER_ID) { if (input_clickedPilot(pid)) return; } else if (pntid >= 0) { /* Planet is closest. */ if (input_clickedPlanet(pntid, autonav)) return; } else if (jpid >= 0) { /* Jump point is closest. */ if (input_clickedJump(jpid, autonav)) return; } /* Fall-through and handle as a normal click. */ } /* Radar targeting requires raw coordinates. */ mxr = event->button.x; myr = gl_screen.rh - event->button.y; gui_radarGetPos( &rx, &ry ); gui_radarGetDim( &rw, &rh ); if ((mxr > rx && mxr <= rx + rw ) && (myr > ry && myr <= ry + rh )) { /* Radar */ zoom = 1.; gui_radarGetRes( &res ); x = (mxr - (rx + rw / 2.)) * res + px; y = (myr - (ry + rh / 2.)) * res + py; if (input_clickPos( event, x, y, zoom, 10. * res, 15. * res )) return; } /* Visual (on-screen) */ gl_screenToGameCoords( &x, &y, (double)mx, (double)my ); zoom = res = 1. / cam_getZoom(); input_clickPos( event, x, y, zoom, 10. * res, 15. * res ); return; }
void VectorLocalization2D::updatePointCloud(const vector< vector2f >& pointCloud, const vector< vector2f >& pointNormals, const MotionModelParams &motionParams, const PointCloudParams &pointCloudParams) { static const bool debug = false; static const bool usePermissibility = true; double tStart = GetTimeSec(); if(UseSimpleUpdate){ //Compute importance weights using the observation model float totalDensity = 0.0; int N = int(particlesRefined.size()); float totalWeight = 0.0; if(debug) printf("\nParticle weights:\n"); for(int i=0; i<N; i++){ Particle2D &p = particlesRefined[i]; p.weight = observationWeightPointCloud(p.loc, p.angle, pointCloud, pointNormals, pointCloudParams); totalWeight += p.weight; if(debug) printf("%2d: %f\n", i, p.weight); } //Normalize the importance weights for (int i=0; i<N; i++) particlesRefined[i].weight /= totalWeight; }else{ //Compute the sampling density float sqDensityKernelSize = sq(pointCloudParams.kernelSize); float totalDensity = 0.0; int N = int(particlesRefined.size()); static vector<float> samplingDensity; if(samplingDensity.size()!=N) samplingDensity.resize(N); if(debug) printf("\nParticle samplingDensity:\n"); for(int i=0; i<N; i++){ float w = 0.99; for(int j=0; j<N; j++){ if(i==j) continue; if( (particlesRefined[j].loc - particlesRefined[i].loc).sqlength() < sqDensityKernelSize && fabs(angle_diff(particlesRefined[j].angle, particlesRefined[i].angle))<RAD(20.0)) //if( (particlesRefined[j].loc - particlesRefined[i].loc).sqlength() < sqDensityKernelSize) w++; } samplingDensity[i] = w; totalDensity += w; if(debug) printf("%2d:%f\n",i,w); } // Normalize densities, not really necessary since resampling does not require normalized weights for(int i=0; i<N; i++){ samplingDensity[i] /= totalDensity; } //Compute importance weights = observation x motion / samplingDensity if(debug) printf("\nParticle weights:\n"); for(int i=0; i<N; i++){ Particle2D &p = particlesRefined[i]; float w1 = observationWeightPointCloud(p.loc, p.angle, pointCloud, pointNormals, pointCloudParams); float w2 = motionModelWeight(p.loc, p.angle, motionParams); p.weight = w1*w2/samplingDensity[i]; if(debug) printf("%2d: %f , %f , %f\n",i,w1,w2,p.weight); } } updateTime = GetTimeSec() - tStart; }
void VectorLocalization2D::updateLidar(const LidarParams &lidarParams, const MotionModelParams & motionParams) { static const bool debug = false; static const bool usePermissibility = true; double tStart = GetTimeSec(); // Transform laserpoints to robot frame vector< Vector2f > laserPoints(lidarParams.numRays); for(int i=0; i<lidarParams.numRays; i++){ laserPoints[i] = lidarParams.laserToBaseTrans + lidarParams.laserToBaseRot*lidarParams.scanHeadings[i]*lidarParams.laserScan[i]; } if(UseSimpleUpdate){ int N = int(particlesRefined.size()); float totalWeight = 0.0; //Compute importance weights if(debug) printf("\nParticle weights:\n"); for(int i=0; i<N; i++){ Particle2D &p = particlesRefined[i]; p.weight = observationWeightLidar(p.loc, p.angle, lidarParams, laserPoints); totalWeight = p.weight; if(debug) printf("%2d: %f\n", i, p.weight); } //Normalize importance weights for (int i=0; i<N; i++) particlesRefined[i].weight /= totalWeight; }else{ //Compute the sampling density float sqDensityKernelSize = sq(lidarParams.kernelSize); float totalDensity = 0.0; int N = int(particlesRefined.size()); static vector<float> samplingDensity; if(samplingDensity.size()!=N) samplingDensity.resize(N); if(debug) printf("\nParticle samplingDensity:\n"); for(int i=0; i<N; i++){ float w = 0.99; for(int j=0; j<N; j++){ if(i==j) continue; if( (particlesRefined[j].loc - particlesRefined[i].loc).sqlength() < sqDensityKernelSize && fabs(angle_diff(particlesRefined[j].angle, particlesRefined[i].angle))<RAD(20.0)) //if( (particlesRefined[j].loc - particlesRefined[i].loc).sqlength() < sqDensityKernelSize) w++; } samplingDensity[i] = w; totalDensity += w; if(debug) printf("%2d:%f\n",i,w); } // Normalize densities, not really necessary since resampling does not require normalized weights for(int i=0; i<N; i++){ samplingDensity[i] /= totalDensity; } //Compute importance weights = observation x motion / samplingDensity if(debug) printf("\nParticle weights:\n"); for(int i=0; i<N; i++){ Particle2D &p = particlesRefined[i]; float w1 = observationWeightLidar(p.loc, p.angle, lidarParams, laserPoints); float w2 = motionModelWeight(p.loc, p.angle, motionParams); p.weight = w1*w2/samplingDensity[i]; if(debug) printf("%2d: %f , %f , %f\n",i,w1,w2,p.weight); } } updateTime = GetTimeSec() - tStart; }
void TE::Main() { double g_dx, g_da; double alpha, theta, phi; double vx, va; player_pose_t relativeGoal; player_pose_t currGoal; // Fill in the TE's parameter structure current_dir = 1; for (;;) { usleep(200000); // 200 ms delay pthread_testcancel(); // call ProcessMessages(); // are we waiting for a stall to clear? if (waiting) { PutPositionCmd(0, 0); stall = true; if (verbose) PLAYER_MSG0(0, "TE::Main waiting"); continue; } // do we have a goal? if (!active_goal) { continue; } // wzgledne polozenie celu relativeGoal.px = goal.px - odom_pose.px; relativeGoal.py = goal.py - odom_pose.py; relativeGoal.pa = goal.pa; // angle from 0 to the goal (theta) theta = atan2(relativeGoal.py, relativeGoal.px); // diff betwean robot orientation angle (psi) and goal vector (theta) alpha = angle_diff(theta, odom_pose.pa); g_dx = hypot(relativeGoal.px, relativeGoal.py); if (obstacle && g_dx > dist_eps) { //PLAYER_MSG0(1, "TE: obstacle avoidance"); if (fabs(beta) > ang_eps) phi = angle_diff(fabs(beta) / beta * M_PI / 2, angle_diff(beta, alpha)); else phi = angle_diff(M_PI / 2, angle_diff(beta, alpha)); currGoal.px = cos(phi) * relativeGoal.px + sin(phi) * relativeGoal.py; currGoal.py = -sin(phi) * relativeGoal.px + cos(phi) * relativeGoal.py; currGoal.pa = relativeGoal.pa; } else currGoal = relativeGoal; // angle from 0 to the goal (theta) theta = atan2(currGoal.py, currGoal.px); // diff betwean robot orientation angle (psi) and goal vector (theta) alpha = angle_diff(theta, odom_pose.pa); // are we at the goal? g_dx = hypot(currGoal.px, currGoal.py); g_da = angle_diff(currGoal.pa, odom_pose.pa); if ((g_dx < dist_eps)) { // jestesmy bliko celu if (verbose) PLAYER_MSG0(0, "TE::Main Close to goal"); if (fabs(g_da) < ang_eps) { // z wlasciwa orientacja active_goal = false; PutPositionCmd(0.0, 0.0); if (verbose) PLAYER_MSG0(0, "TE::Main At goal"); continue; } else { // trzeba poprawić orientację po dojechaniu do celu if (verbose) PLAYER_MSG0(0, "TE::Main Correcting orientation"); vx = 0; va = k_a * va_max * tanh(10 * g_da); } } else { // sterowanie vx = vx_max * tanh(fabs(g_dx)) * fabs(cos(alpha)); va = k_a * va_max * tanh(alpha); } if (nearObstDist <= obs_dist) { vx = vx * (nearObstDist - min_dist) / (obs_dist - min_dist); } if (nearObstDist <= min_dist) { vx = 0; va = 0; } PutPositionCmd(vx, va); } }
/** * @brief Updates the lockons on the pilot's launchers * * @param p Pilot being updated. * @param o Slot being updated. * @param t Pilot that is currently the target of p (or NULL if not applicable). * @param a Angle to update if necessary. Should be initialized to -1 before the loop. * @param dt Current delta tick. */ void pilot_lockUpdateSlot( Pilot *p, PilotOutfitSlot *o, Pilot *t, double *a, double dt ) { double max, old; double x,y, ang, arc; int locked; /* No target. */ if (t == NULL) return; /* Nota seeker. */ if (!outfit_isSeeker(o->outfit)) return; /* Check arc. */ arc = o->outfit->u.lau.arc; if (arc > 0.) { /* We use an external variable to set and update the angle if necessary. */ if (*a < 0.) { x = t->solid->pos.x - p->solid->pos.x; y = t->solid->pos.y - p->solid->pos.y; ang = ANGLE( x, y ); *a = fabs( angle_diff( ang, p->solid->dir ) ); } /* Decay if not in arc. */ if (*a > arc) { /* Limit decay to the lockon time for this launcher. */ max = o->outfit->u.lau.lockon; /* When a lock is lost, immediately gain half the lock timer. * This is meant as an incentive for the aggressor not to lose the lock, * and for the target to try and break the lock. */ old = o->u.ammo.lockon_timer; o->u.ammo.lockon_timer += dt; if ((old <= 0.) && (o->u.ammo.lockon_timer > 0.)) o->u.ammo.lockon_timer += o->outfit->u.lau.lockon / 2.; /* Cap at max. */ if (o->u.ammo.lockon_timer > max) o->u.ammo.lockon_timer = max; /* Out of arc. */ o->u.ammo.in_arc = 0; return; } } /* In arc. */ o->u.ammo.in_arc = 1; locked = (o->u.ammo.lockon_timer < 0.); /* Lower timer. When the timer reaches zero, the lock is established. */ max = -o->outfit->u.lau.lockon/3.; if (o->u.ammo.lockon_timer > max) { /* Compensate for enemy hide factor. */ o->u.ammo.lockon_timer -= dt * (o->outfit->u.lau.ew_target2 / t->ew_hide); /* Cap at -max/3. */ if (o->u.ammo.lockon_timer < max) o->u.ammo.lockon_timer = max; /* Trigger lockon hook. */ if (!locked && (o->u.ammo.lockon_timer < 0.)) pilot_runHook( p, PILOT_HOOK_LOCKON ); } }
/** * @brief Handles a click event. */ static void input_clickevent( SDL_Event* event ) { unsigned int pid; Pilot *p; int mx, my, mxr, myr, pntid, jpid; int rx, ry, rh, rw, res; double x, y, m, r, rp, d, dp, px, py; double ang, angp, mouseang; Planet *pnt; JumpPoint *jp; HookParam hparam[2]; /* Generate hook. */ hparam[0].type = HOOK_PARAM_NUMBER; hparam[0].u.num = event->button.button; hparam[1].type = HOOK_PARAM_SENTINAL; hooks_runParam( "mouse", hparam ); /* Handle zoom. */ if (event->button.button == SDL_BUTTON_WHEELUP) { input_clickZoom( 1.1 ); return; } else if (event->button.button == SDL_BUTTON_WHEELDOWN) { input_clickZoom( 0.9 ); return; } /* Middle mouse enables mouse flying. */ if (event->button.button == SDL_BUTTON_MIDDLE) { player_toggleMouseFly(); return; } /* Mouse targetting is left only. */ if (event->button.button != SDL_BUTTON_LEFT) return; /* Player must not be NULL. */ if (player_isFlag(PLAYER_DESTROYED) || (player.p == NULL)) return; px = player.p->solid->pos.x; py = player.p->solid->pos.y; gl_windowToScreenPos( &mx, &my, event->button.x, event->button.y ); gl_screenToGameCoords( &x, &y, (double)mx, (double)my ); if ((mx <= 15 || my <= 15 ) || (my >= gl_screen.h - 15 || mx >= gl_screen.w - 15)) { /* Border */ x = (mx - (gl_screen.w / 2.)) + px; y = (my - (gl_screen.h / 2.)) + py; mouseang = atan2(py - y, px - x); angp = pilot_getNearestAng( player.p, &pid, mouseang, 1 ); ang = system_getClosestAng( cur_system, &pntid, &jpid, x, y, mouseang ); if ((ABS(angle_diff(mouseang, angp)) > M_PI / 64) || ABS(angle_diff(mouseang, ang)) < ABS(angle_diff(mouseang, angp))) pid = PLAYER_ID; /* Pilot angle is too great, or planet/jump is closer. */ if (ABS(angle_diff(mouseang, ang)) > M_PI / 64 ) jpid = pntid = -1; /* Asset angle difference is too great. */ } else { /* Radar targeting requires raw coordinates. */ mxr = event->button.x; myr = gl_screen.rh - event->button.y; gui_radarGetPos( &rx, &ry ); gui_radarGetDim( &rw, &rh ); if ((mxr > rx && mxr <= rx + rw ) && (myr > ry && myr <= ry + rh )) { /* Radar */ m = 1; gui_radarGetRes( &res ); x = (mxr - (rx + rw / 2.)) * res + px; y = (myr - (ry + rh / 2.)) * res + py; } else /* Visual (on-screen) */ m = res = 1. / cam_getZoom(); dp = pilot_getNearestPos( player.p, &pid, x, y, 1 ); d = system_getClosest( cur_system, &pntid, &jpid, x, y ); rp = MAX( 1.5 * PILOT_SIZE_APROX * pilot_get(pid)->ship->gfx_space->sw / 2 * m, 10. * res); if (pntid >=0) { /* Planet is closer. */ pnt = cur_system->planets[ pntid ]; r = MAX( 1.5 * pnt->radius, 100. ); } else if (jpid >= 0) { jp = &cur_system->jumps[ jpid ]; r = MAX( 1.5 * jp->radius, 100. ); } else { r = 0.; } /* Reject pilot if it's too far or a valid asset is closer. */ if (dp > pow2(rp) || (d < pow2(r) && dp < pow2(rp) && dp > d)) pid = PLAYER_ID; if (d > pow2(r)) /* Planet or jump point is too far. */ jpid = pntid = -1; } if (pid != PLAYER_ID) { /* Apply an action if already selected. */ if (!pilot_isFlag(player.p, PILOT_DEAD) && (pid == player.p->target)) { p = pilot_get(pid); if (pilot_isDisabled(p) || pilot_isFlag(p, PILOT_BOARDABLE)) player_board(); else player_hail(); } else player_targetSet( pid ); } else if (pntid >= 0) { /* Planet is closest. */ if (pntid == player.p->nav_planet) { pnt = cur_system->planets[ pntid ]; if (planet_hasService(pnt, PLANET_SERVICE_LAND) && (!areEnemies( player.p->faction, pnt->faction ) || pnt->bribed )) player_land(); else player_hailPlanet(); } else player_targetPlanetSet( pntid ); } else if (jpid >= 0) { /* Jump point is closest. */ jp = &cur_system->jumps[ jpid ]; if (jpid == player.p->nav_hyperspace) { if (space_canHyperspace(player.p)) { if (!paused) player_autonavAbort(NULL); player_jump(); } else player_autonavStart(); } else player_targetHyperspaceSet( jpid ); } }
void TeamDetector::findRobotsByModel(::google::protobuf::RepeatedPtrField< ::SSL_DetectionRobot >* robots, int team_color_id, const Image<raw8> * image, CMVision::ColorRegionList * colorlist, CMVision::RegionTree & reg_tree) { (void)image; const int MaxDetections = _other_markers_max_detections; Marker cen; // center marker Marker markers[MaxDetections]; const float marker_max_query_dist = _other_markers_max_query_distance; const float marker_max_dist = _pattern_max_dist; // partially forget old detections //decaySeen(); filter_team.init( colorlist->getRegionList(team_color_id).getInitialElement()); const CMVision::Region * reg=0; SSL_DetectionRobot * robot=0; MultiPatternModel::PatternDetectionResult res; // printf("finding robots...\n"); while((reg = filter_team.getNext()) != 0) { vector2d reg_img_center(reg->cen_x,reg->cen_y); vector3d reg_center3d; _camera_params.image2field(reg_center3d,reg_img_center,_robot_height); vector2d reg_center(reg_center3d.x,reg_center3d.y); //TODO add masking: //if(det.mask.get(reg->cen_x,reg->cen_y) >= 0.5){ // printf("if in field:\n"); if (field_filter.isInFieldOrPlayableBoundary(reg_center)) { cen.set(reg,reg_center3d,getRegionArea(reg,_robot_height)); int num_markers = 0; reg_tree.startQuery(*reg,marker_max_query_dist); double sd=0.0; CMVision::Region *mreg; while((mreg=reg_tree.getNextNearest(sd))!=0 && num_markers<MaxDetections) { //TODO: implement masking: // filter_other.check(*mreg) && det.mask.get(mreg->cen_x,mreg->cen_y)>=0.5 if(filter_others.check(*mreg) && model.usesColor(mreg->color)) { vector2d marker_img_center(mreg->cen_x,mreg->cen_y); vector3d marker_center3d; _camera_params.image2field(marker_center3d,marker_img_center,_robot_height); Marker &m = markers[num_markers]; m.set(mreg,marker_center3d,getRegionArea(mreg,_robot_height)); vector2f ofs = m.loc - cen.loc; m.dist = ofs.length(); m.angle = ofs.angle(); if(m.dist>0.0 && m.dist<marker_max_dist) { num_markers++; } } } reg_tree.endQuery(); // printf("num markers = %d\n", num_markers); if(num_markers >= 2) { CMPattern::PatternProcessing::sortMarkersByAngle(markers,num_markers); for(int i=0; i<num_markers; i++) { // DEBUG CODE: // char colorchar='?'; // if (markers[i].id==color_id_green) colorchar='g'; // if (markers[i].id==color_id_pink) colorchar='p'; // if (markers[i].id==color_id_white) colorchar='w'; // if (markers[i].id==color_id_team) colorchar='t'; // if (markers[i].id==color_id_field_green) colorchar='f'; // if (markers[i].id==color_id_cyan) colorchar='c'; // printf("%c ",colorchar); int j = (i + 1) % num_markers; markers[i].next_dist = dist(markers[i].loc,markers[j].loc); markers[i].next_angle_dist = angle_pos(angle_diff(markers[i].angle,markers[j].angle)); } if (model.findPattern(res,markers,num_markers,_pattern_fit_params,_camera_params)) { robot=addRobot(robots,res.conf,_max_robots*2); if (robot!=0) { //setup robot: robot->set_x(cen.loc.x); robot->set_y(cen.loc.y); if (_have_angle) robot->set_orientation(res.angle); robot->set_robot_id(res.id); robot->set_pixel_x(reg->cen_x); robot->set_pixel_y(reg->cen_y); robot->set_height(cen.height); } } } } } //remove items with 0-confidence: stripRobots(robots); //remove extra items: while(robots->size() > _max_robots) { robots->RemoveLast(); } }
/** * @brief Updates a camera following a pilot. */ static void cam_updatePilot( Pilot *follow, double dt ) { Pilot *target; double diag2, a, r, dir, k; double x,y, dx,dy, mx,my, targ_x,targ_y, bias_x,bias_y, vx,vy; /* Get target. */ if (!pilot_isFlag(follow, PILOT_HYPERSPACE) && (follow->target != follow->id)) target = pilot_get( follow->target ); else target = NULL; /* Real diagonal might be a bit too harsh since it can cut out the ship, * we'll just use the largest of the two. */ /*diag2 = pow2(SCREEN_W) + pow2(SCREEN_H);*/ /*diag2 = pow2( MIN(SCREEN_W, SCREEN_H) );*/ diag2 = 100*100; x = follow->solid->pos.x; y = follow->solid->pos.y; /* Compensate player movement. */ mx = x - old_X; my = y - old_Y; camera_X += mx; camera_Y += my; /* Set old position. */ old_X = x; old_Y = y; /* No bias by default. */ bias_x = 0.; bias_y = 0.; /* Bias towards target. */ if (target != NULL) { bias_x += target->solid->pos.x - x; bias_y += target->solid->pos.y - y; } /* Bias towards velocity and facing. */ vx = follow->solid->vel.x*1.5; vy = follow->solid->vel.y*1.5; dir = angle_diff( atan2(vy,vx), follow->solid->dir); dir = (M_PI - fabs(dir)) / M_PI; /* Normalize. */ vx *= dir; vy *= dir; bias_x += vx; bias_y += vy; /* Limit bias. */ if (pow2(bias_x)+pow2(bias_y) > diag2/2.) { a = atan2( bias_y, bias_x ); r = sqrt(diag2)/2.; bias_x = r*cos(a); bias_y = r*sin(a); } /* Compose the target. */ targ_x = x + bias_x; targ_y = y + bias_y; /* Head towards target. */ k = 0.5*dt/dt_mod; dx = (targ_x-camera_X)*k; dy = (targ_y-camera_Y)*k; background_moveStars( -(mx+dx), -(my+dy) ); /* Update camera. */ camera_X += dx; camera_Y += dy; /* DEBUG. */ #if 0 glColor4d( 1., 1., 1., 1. ); glBegin(GL_LINES); gl_gameToScreenCoords( &x, &y, x, y ); glVertex2d( x, y ); gl_gameToScreenCoords( &x, &y, camera_X, camera_Y ); glVertex2d( x, y ); glEnd(); /* GL_LINES */ #endif /* Update zoom. */ cam_updatePilotZoom( follow, target, dt ); }
float angle_sum( float angle1, float angle2) { return angle_diff(-1*angle1, angle2); }