/**
* Check if aircraft has transitioned to inside sector
*
* @param ref_now Current aircraft state
* @param ref_last Previous aircraft state
*
* @return True if aircraft now inside (and was outside)
*/
gcc_pure
virtual bool check_transition_enter(const AIRCRAFT_STATE &ref_now,
const AIRCRAFT_STATE &ref_last) const {
return isInSector(ref_now) &&
!isInSector(ref_last) &&
transition_constraint(ref_now, ref_last);
}
bool
TargetMapWindow::OnMouseDown(PixelScalar x, PixelScalar y)
{
// Ignore single click event if double click detected
if (drag_mode != DRAG_NONE)
return true;
SetFocus();
drag_start.x = x;
drag_start.y = y;
drag_last = drag_start;
if (isClickOnTarget(drag_start)) {
drag_mode = isInSector(x, y)
? DRAG_TARGET
: DRAG_TARGET_OUTSIDE;
SetCapture();
PaintWindow::Invalidate();
return true;
} else if (isInSector(x, y)) {
drag_mode = DRAG_OZ;
SetCapture();
PaintWindow::Invalidate();
return true;
}
return false;
}
fixed
AATIsolineSegment::FindTargetInSector(const AATPoint& ap) {
static fixed t_target = fixed_zero;
for (fixed t_b = fixed_zero; t_b < fixed_half; t_b += fixed(0.01)) {
if (isInSector(ap, t_target + t_b)) {
t_target += t_b;
break;
}
if (isInSector(ap, t_target - t_b)) {
t_target -= t_b;
break;
}
}
return t_target;
}
bool
TargetMapWindow::OnMouseMove(PixelScalar x, PixelScalar y, unsigned keys)
{
switch (drag_mode) {
case DRAG_NONE:
break;
case DRAG_TARGET:
if (isInSector(x, y)) {
drag_last.x = x;
drag_last.y = y;
/* no full repaint: copy the map from the buffer, draw dragged
icon on top */
PaintWindow::Invalidate();
}
return true;
case DRAG_OZ:
if (manhattan_distance(drag_last, RasterPoint{x,y}) > Layout::GetHitRadius()) {
/* cancel the target move click when the finger has moved too
far since it was pressed down */
ReleaseCapture();
drag_mode = DRAG_NONE;
PaintWindow::Invalidate();
}
return true;
}
return false;
}
bool
SampledTaskPoint::update_sample_near(const AIRCRAFT_STATE& state,
TaskEvents &task_events,
const TaskProjection &projection)
{
if (isInSector(state)) {
// if sample is inside sample polygon
// return false (no update required)
// else
// add sample to polygon
// re-compute convex hull
// return true; (update required)
//
if (PolygonInterior(state.Location, m_sampled_points)) {
// do nothing
return false;
} else {
SearchPoint sp(state.Location, projection);
m_sampled_points.push_back(sp);
// only return true if hull changed
bool retval = prune_interior(m_sampled_points);
return thin_to_size(m_sampled_points, 64) || retval;
// thin to size is used here to ensure the sampled points vector
// size is bounded to reasonable values for AAT calculations.
}
}
return false;
}
bool
SampledTaskPoint::update_sample_near(const AIRCRAFT_STATE& state,
TaskEvents &task_events,
const TaskProjection &projection)
{
if (isInSector(state)) {
// if sample is inside sample polygon
// return false (no update required)
// else
// add sample to polygon
// re-compute convex hull
// return true; (update required)
//
if (PolygonInterior(state.Location, m_sampled_points)) {
// do nothing
return false;
} else {
SearchPoint sp(state.Location, projection);
m_sampled_points.push_back(sp);
// only return true if hull changed
return prune_interior(m_sampled_points);
}
}
return false;
}
vector<int> RFID::Rfid_map(VectorXd X){
int i,j,l=0;
vector<int> map;
float angle_offset=0.12;
float angle=1.12;
bool v;
for(i=0;i<9;i++)
map.push_back(0);
Eigen::MatrixXd tag(82,2);
for(i=0;i<180;i+=20)
for(j=0;j<180;j+=20){
tag(l,0)=i;
tag(l,1)=j;
l++;
}
l=0;
for(i=0;i<180;i+=20)
for(j=0;j<180;j+=20){
if((v=isInSector( tag(l,0), tag(l,1),angle_offset,angle, X(0),X(1)))==true){
l++;
map.at(l)++;
}
}
return map;
}
VectorXd Robot::compute_weight(MatrixXd cloud_particles,VectorXd tag){
float d=0.0,likelihood=0.0,normalisation,x=0.0,y=0.0,angle_offset=3.14,angle=2.1;
int i;
VectorXd weight(numberOfParticle);
for(i=0;i<numberOfParticle;i++)
weight(i)=1.0;
for(i=0;i<numberOfParticle;i++)
{
x=cloud_particles(i,0);
y=cloud_particles(i,1);
d=sqrt((x-tag(0))*(x-tag(0))+(y-tag(1))*(y-tag(1)));
if((isInSector(tag(0),tag(1),angle_offset,angle, x, y)==false)){
if (d<10)
likelihood=0.7;
else if(d>10 && d<60)
likelihood=0.9;
else
likelihood=0.5;
}
weight(i)=weight(i)*likelihood;
}
float s=0;
for(i=0;i<numberOfParticle;i++)
s=s+weight(i);
normalisation=1/s;
weight=weight*normalisation;
return weight;
}
bool
AATPoint::check_target(const AIRCRAFT_STATE& state)
{
bool moved = false;
if (isInSector(state)) {
moved = check_target_inside(state);
} else {
moved = check_target_outside(state);
}
return moved;
}
bool
StartPoint::update_sample_near(const AIRCRAFT_STATE& state,
TaskEvents &task_events,
const TaskProjection &projection)
{
if (isInSector(state)) {
if (!m_ordered_task_behaviour.check_start_speed(state, m_task_behaviour)) {
task_events.warning_start_speed();
}
}
return OrderedTaskPoint::update_sample_near(state, task_events, projection);
}
GeoPoint
CylinderZone::randomPointInSector(const fixed mag) const
{
AIRCRAFT_STATE ac;
do {
Angle dir = Angle::degrees(fixed(rand() % 360));
fixed dmag = max(min(Radius, fixed(100.0)), Radius*mag);
fixed dis = fixed((0.1 + (rand() % 90) / 100.0)) * dmag;
GeoVector vec(dis,dir);
ac.Location = vec.end_point(get_location());
} while (!isInSector(ac));
return ac.get_location();
}
bool
AATPoint::check_target(const AIRCRAFT_STATE& state, const bool known_outside)
{
if ((getActiveState() == CURRENT_ACTIVE) && (m_target_locked)) {
return false;
}
bool moved = false;
if (!known_outside && isInSector(state)) {
moved = check_target_inside(state);
} else {
moved = check_target_outside(state);
}
return moved;
}
