bool Geofence::inside(double lat, double lon, float altitude)
{
bool inside_fence = true;
float max_horizontal_distance = _param_max_hor_distance.get();
float max_vertical_distance = _param_max_ver_distance.get();
if (max_horizontal_distance > 1.0f || max_vertical_distance > 1.0f) {
if (_home_pos_set) {
float dist_xy = -1.0f;
float dist_z = -1.0f;
get_distance_to_point_global_wgs84(lat, lon, altitude,
_home_pos.lat, _home_pos.lon, _home_pos.alt,
&dist_xy, &dist_z);
if (max_vertical_distance > 1.0f && (dist_z > max_vertical_distance)) {
if (hrt_elapsed_time(&_last_vertical_range_warning) > GEOFENCE_RANGE_WARNING_LIMIT) {
mavlink_log_critical(_navigator->get_mavlink_log_pub(),
"Maximum altitude above home exceeded by %.1f m",
(double)(dist_z - max_vertical_distance));
_last_vertical_range_warning = hrt_absolute_time();
}
inside_fence = false;
}
if (max_horizontal_distance > 1.0f && (dist_xy > max_horizontal_distance)) {
if (hrt_elapsed_time(&_last_horizontal_range_warning) > GEOFENCE_RANGE_WARNING_LIMIT) {
mavlink_log_critical(_navigator->get_mavlink_log_pub(),
"Maximum distance from home exceeded by %.1f m",
(double)(dist_xy - max_horizontal_distance));
_last_horizontal_range_warning = hrt_absolute_time();
}
inside_fence = false;
}
}
}
// to be inside the geofence both fences have to report being inside
// as they both report being inside when not enabled
inside_fence = inside_fence && inside_polygon(lat, lon, altitude);
if (inside_fence) {
_outside_counter = 0;
return inside_fence;
} else {
_outside_counter++;
if (_outside_counter > _param_counter_threshold.get()) {
return inside_fence;
} else {
return true;
}
}
}
bool Geofence::inside(double lat, double lon, float altitude)
{
float max_horizontal_distance = _param_max_hor_distance.get();
float max_vertical_distance = _param_max_ver_distance.get();
if (max_horizontal_distance > 1 || max_vertical_distance > 1) {
if (_home_pos_set) {
float dist_xy = -1.0f;
float dist_z = -1.0f;
get_distance_to_point_global_wgs84(lat, lon, altitude,
_home_pos.lat, _home_pos.lon, _home_pos.alt,
&dist_xy, &dist_z);
if (max_vertical_distance > 0 && (dist_z > max_vertical_distance)) {
if (hrt_elapsed_time(&_last_vertical_range_warning) > GEOFENCE_RANGE_WARNING_LIMIT) {
mavlink_log_critical(_navigator->get_mavlink_log_pub(),
"Geofence exceeded max vertical distance by %.1f m",
(double)(dist_z - max_vertical_distance));
_last_vertical_range_warning = hrt_absolute_time();
}
return false;
}
if (max_horizontal_distance > 0 && (dist_xy > max_horizontal_distance)) {
if (hrt_elapsed_time(&_last_horizontal_range_warning) > GEOFENCE_RANGE_WARNING_LIMIT) {
mavlink_log_critical(_navigator->get_mavlink_log_pub(),
"Geofence exceeded max horizontal distance by %.1f m",
(double)(dist_xy - max_horizontal_distance));
_last_horizontal_range_warning = hrt_absolute_time();
}
return false;
}
}
}
bool inside_fence = inside_polygon(lat, lon, altitude);
if (inside_fence) {
_outside_counter = 0;
return inside_fence;
} else {
_outside_counter++;
if (_outside_counter > _param_counter_threshold.get()) {
return inside_fence;
} else {
return true;
}
}
}
static int
monotonate_trapezoids(int nsegs, segment_t*seg, trap_t* tr,
int flip, boxf* decomp)
{
int i, size;
int tr_start;
int tr_size = TRSIZE(nsegs);
int* visited = N_NEW(tr_size,int);
mchain = N_NEW(tr_size, monchain_t);
vert = N_NEW(nsegs+1,vertexchain_t);
mon = N_NEW(nsegs, int);
/* First locate a trapezoid which lies inside the polygon */
/* and which is triangular */
for (i = 0; i < TRSIZE(nsegs); i++)
if (inside_polygon(&tr[i], seg)) break;
tr_start = i;
/* Initialise the mon data-structure and start spanning all the */
/* trapezoids within the polygon */
for (i = 1; i <= nsegs; i++) {
mchain[i].prev = seg[i].prev;
mchain[i].next = seg[i].next;
mchain[i].vnum = i;
vert[i].pt = seg[i].v0;
vert[i].vnext[0] = seg[i].next; /* next vertex */
vert[i].vpos[0] = i; /* locn. of next vertex */
vert[i].nextfree = 1;
}
chain_idx = nsegs;
mon_idx = 0;
mon[0] = 1; /* position of any vertex in the first */
/* chain */
/* traverse the polygon */
if (tr[tr_start].u0 > 0)
size = traverse_polygon (visited, decomp, 0, seg, tr, 0, tr_start, tr[tr_start].u0, flip, TR_FROM_UP);
else if (tr[tr_start].d0 > 0)
size = traverse_polygon (visited, decomp, 0, seg, tr, 0, tr_start, tr[tr_start].d0, flip, TR_FROM_DN);
free (visited);
free (mchain);
free (vert);
free (mon);
/* return the number of rects created */
return size;
}
//判线段在任意多边形内,顶点按顺时针或逆时针给出,与边界相交返回1
int inside_polygon(point l1,point l2,int n,point* p){
point t[MAXN],tt;
int i,j,k=0;
if (!inside_polygon(l1,n,p)||!inside_polygon(l2,n,p))
return 0;
for (i=0;i<n;i++)
if (opposite_side(l1,l2,p[i],p[(i+1)%n])&&opposite_side(p[i],p[(i+1)%n],l1,l2))
return 0;
else if (dot_online_in(l1,p[i],p[(i+1)%n]))
t[k++]=l1;
else if (dot_online_in(l2,p[i],p[(i+1)%n]))
t[k++]=l2;
else if (dot_online_in(p[i],l1,l2))
t[k++]=p[i];
for (i=0;i<k;i++)
for (j=i+1;j<k;j++){
tt.x=(t[i].x+t[j].x)/2;
tt.y=(t[i].y+t[j].y)/2;
if (!inside_polygon(tt,n,p))
return 0;
}
return 1;
}
bool Geofence::inside(double lat, double lon, float altitude)
{
bool inside_fence = inside_polygon(lat, lon, altitude);
if (inside_fence) {
_outside_counter = 0;
return inside_fence;
} {
_outside_counter++;
if(_outside_counter > _param_counter_threshold.get()) {
return inside_fence;
} else {
return true;
}
}
}
int monotonate_trapezoids(
int n)
{
register int i;
int tr_start;
memset((void *)vert, 0, sizeof(vert));
memset((void *)visited, 0, sizeof(visited));
memset((void *)mchain, 0, sizeof(mchain));
memset((void *)mon, 0, sizeof(mon));
/* First locate a trapezoid which lies inside the polygon */
/* and which is triangular */
for (i = 0; i < TRSIZE; i++)
if (inside_polygon(&tr[i]))
break;
tr_start = i;
/* Initialise the mon data-structure and start spanning all the */
/* trapezoids within the polygon */
#if 0
for (i = 1; i <= n; i++)
{
mchain[i].prev = i - 1;
mchain[i].next = i + 1;
mchain[i].vnum = i;
vert[i].pt = seg[i].v0;
vert[i].vnext[0] = i + 1; /* next vertex */
vert[i].vpos[0] = i; /* locn. of next vertex */
vert[i].nextfree = 1;
}
mchain[1].prev = n;
mchain[n].next = 1;
vert[n].vnext[0] = 1;
vert[n].vpos[0] = n;
chain_idx = n;
mon_idx = 0;
mon[0] = 1; /* position of any vertex in the first */
/* chain */
#else
for (i = 1; i <= n; i++)
{
mchain[i].prev = seg[i].prev;
mchain[i].next = seg[i].next;
mchain[i].vnum = i;
vert[i].pt = seg[i].v0;
vert[i].vnext[0] = seg[i].next; /* next vertex */
vert[i].vpos[0] = i; /* locn. of next vertex */
vert[i].nextfree = 1;
}
chain_idx = n;
mon_idx = 0;
mon[0] = 1; /* position of any vertex in the first */
/* chain */
#endif
/* traverse the polygon */
if (tr[tr_start].u0 > 0)
traverse_polygon(0, tr_start, tr[tr_start].u0, TR_FROM_UP);
else if (tr[tr_start].d0 > 0)
traverse_polygon(0, tr_start, tr[tr_start].d0, TR_FROM_DN);
/* return the number of polygons created */
return newmon();
}
