/**
* @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);
}
