/**
* compute an approximative distance to a segment. Useful to estimate
* distance to a gate.
* Parameters: lat/long of point, then lat and long of A & B defining the
* segment, and pointers to lat/long of closest point
*/
double distance_to_line_dichotomy_xing(double latitude, double longitude,
double latitude_a, double longitude_a,
double latitude_b, double longitude_b,
double *x_latitude, double *x_longitude){
double p1_latitude, p1_longitude, p2_latitude, p2_longitude;
double ortho_p1, ortho_p2;
double limit;
limit = PI/(180*60*1852); // 1m precision
#ifdef DEBUG
printf("Longitude A: %.2f Longitude B: .%2f\n", radToDeg(longitude_a),
radToDeg(longitude_b));
#endif /* DEBUG */
if (fabs(longitude_a - longitude_b) > PI) {
if (longitude_a > longitude_b) {
if (longitude_a > 0.0) {
longitude_a -= TWO_PI;
} else {
longitude_b += TWO_PI;
}
} else {
if (longitude_b > 0.0) {
longitude_b -= TWO_PI;
} else {
longitude_a += TWO_PI;
}
}
}
#ifdef DEBUG
printf("AB NORM: Longitude A: %.2f Longitude B: %2f\n",
radToDeg(longitude_a), radToDeg(longitude_b));
printf("Point: Longitude: %.2f\n", radToDeg(longitude));
#endif /* DEBUG */
p1_latitude = latitude_a;
p1_longitude = longitude_a;
p2_latitude = latitude_b;
p2_longitude = longitude_b;
ortho_p1 = ortho_distance(latitude, longitude, p1_latitude, p1_longitude);
ortho_p2 = ortho_distance(latitude, longitude, p2_latitude, p2_longitude);
// ending test on distance between two points.
while (__distance((p1_latitude-p2_latitude), (p1_longitude-p2_longitude)) >
limit) {
if (ortho_p1 < ortho_p2) {
p2_longitude = (p1_longitude+p2_longitude)/2;
p2_latitude = yToLat((latToY(p1_latitude)+latToY(p2_latitude))/2);
} else {
p1_longitude = (p1_longitude+p2_longitude)/2;
p1_latitude = yToLat((latToY(p1_latitude)+latToY(p2_latitude))/2);
}
ortho_p1 = ortho_distance(latitude, longitude, p1_latitude, p1_longitude);
ortho_p2 = ortho_distance(latitude, longitude, p2_latitude, p2_longitude);
}
if (ortho_p1 < ortho_p2) {
*x_latitude = p1_latitude;
*x_longitude = p1_longitude;
return ortho_p1;
}
*x_latitude = p2_latitude;
*x_longitude = p2_longitude;
return ortho_p2;
}
/**
* compute an approximative distance to a segment. Useful to estimate
* distance to a gate. It is at best an approximation, as the intersection
* algorithm is not valid for long distances
* Parameters: lat/long of point, then lat and long of A & B defining the
* segment
*/
double distance_to_line_ratio_xing(double latitude, double longitude,
double latitude_a, double longitude_a,
double latitude_b, double longitude_b,
double *x_latitude, double *x_longitude,
double *ab_ratio) {
double dist_a, dist_b, max_dist, ab_dist, t_dist;
double ortho_a, ortho_b;
double t_latitude;
double longitude_x, latitude_x, intersect;
double longitude_y, latitude_y;
double xing_latitude, xing_longitude;
ortho_a = ortho_distance(latitude, longitude, latitude_a, longitude_a);
ortho_b = ortho_distance(latitude, longitude, latitude_b, longitude_b);
t_latitude = latToY(latitude);
latitude_a = latToY(latitude_a);
latitude_b = latToY(latitude_b);
/* some normalization */
/* normalize the line */
#ifdef DEBUG
printf("Longitude A: %.2f Longitude B: .%2f\n", radToDeg(longitude_a),
radToDeg(longitude_b));
#endif /* DEBUG */
if (fabs(longitude_a - longitude_b) > PI) {
if (longitude_a > longitude_b) {
if (longitude_a > 0.0) {
longitude_a -= TWO_PI;
} else {
longitude_b += TWO_PI;
}
} else {
if (longitude_b > 0.0) {
longitude_b -= TWO_PI;
} else {
longitude_a += TWO_PI;
}
}
}
#ifdef DEBUG
printf("AB NORM: Longitude A: %.2f Longitude B: %2f\n",
radToDeg(longitude_a), radToDeg(longitude_b));
printf("Point: Longitude: %.2f\n", radToDeg(longitude));
#endif /* DEBUG */
/* then the point */
if ((fabs(longitude-longitude_a)>PI) && (fabs(longitude-longitude_b)>PI)) {
if (longitude < longitude_a) {
longitude += TWO_PI;
} else {
longitude -= TWO_PI;
}
}
#ifdef DEBUG
printf("Point NORM: Longitude: %.2f\n", radToDeg(longitude));
#endif /* DEBUG */
dist_a = __distance((t_latitude-latitude_a), (longitude-longitude_a));
dist_b = __distance((t_latitude-latitude_b), (longitude-longitude_b));
ab_dist = __distance((latitude_a-latitude_b),(longitude_a-longitude_b));
max_dist = fmax(dist_a, dist_b);
/* we construct a line form the point, orthogonal to the segment, long of
at least max_dist */
latitude_x = t_latitude + (longitude_a - longitude_b) * max_dist / ab_dist;
longitude_x = longitude + (latitude_b - latitude_a) * max_dist / ab_dist;
latitude_y = t_latitude + (longitude_b - longitude_a) * max_dist / ab_dist;
longitude_y = longitude + (latitude_a - latitude_b) * max_dist / ab_dist;
#ifdef DEBUG
printf("Intersect point: Latitude X: %.2f, Longitude X: %.2f\n",
radToDeg(yToLat(latitude_x)), radToDeg(longitude_x));
printf("Intersect point: Latitude Y: %.2f, Longitude Y: %.2f\n",
radToDeg(yToLat(latitude_y)), radToDeg(longitude_y));
#endif /* DEBUG */
intersect = __intersects_no_norm(latitude_a, longitude_a,
latitude_b, longitude_b,
latitude_y, longitude_y,
latitude_x, longitude_x,
&xing_latitude, &xing_longitude);
if (intersect>=INTER_MIN_LIMIT && intersect<=INTER_MAX_LIMIT) {
*x_latitude = yToLat(xing_latitude);
*x_longitude = xing_longitude;
#ifdef DEBUG
printf("Intersect point: Latitude X: %.2f\n", radToDeg(*x_latitude));
printf("Intersect point: Longitude Y: %.2f\n", radToDeg(*x_longitude));
printf("Orig point: Latitude X: %.2f\n", radToDeg(latitude));
printf("Orig point: Longitude Y: %.2f\n", radToDeg(longitude));
#endif /* DEBUG */
t_dist = ortho_distance(latitude, longitude, *x_latitude, *x_longitude);
#ifdef DEBUG
printf("Min dist: %.3f, found dist: %.3f\n", max_dist, t_dist);
printf("Ortho_a: %.3f, Ortho_b: %.3f\n", ortho_a, ortho_b);
#endif /* DEBUG */
if (t_dist < ortho_a && t_dist < ortho_b) {
*ab_ratio = intersect;
return t_dist;
}
}
if (ortho_a < ortho_b) {
*x_latitude = yToLat(latitude_a);
*x_longitude = longitude_a;
*ab_ratio = -1.0;
return ortho_a;
}
*x_latitude = yToLat(latitude_b);
*x_longitude = longitude_b;
*ab_ratio = -2.0;
return ortho_b;
}
void distance (_InputIterator, _InputIterator p2)
{
__distance (p2);
}
/*
* find the end-point
* the end-point must be
* outside buondary!
*/
float
__get_part_dist(struct argu_table *table, int bound, float coor_tmp, char *part)
{
float _dist;
int x1 = table->x1;
int x2 = table->x2;
int y1 = table->y1;
int y2 = table->y2;
/* find the Y-axis lower bound endpoint */
if(strcmp(part, "Y-Lower") == 0){
/*
* take point 1 to endpoint
* the point 1 Y-coordinate
* is less than bound
*/
if(y1 < bound && y2 > bound){
_dist = __distance(coor_tmp, bound, x1, y1);
printf("Get distance %f with p1\n", _dist);
/*
* take point 2 to endpoint
* the point 2 Y-coordinate
* is less than bound
*/
}else if(y2 < bound && y1 > bound){
_dist = __distance(coor_tmp, bound, x2, y2);
printf("Get distance %f with p2\n", _dist);
/*
* means the endpoint is on
* the boundary line
*/
}else{
return 0.0;
}
/* find the Y-axis upper bound endpoint */
}else if(strcmp(part, "Y-Upper") == 0){
/*
* take point 1 to endpoint
* the point 1 Y-coordinate
* is gratter than bound
*/
if(y1 > bound && y2 < bound){
_dist = __distance(coor_tmp, bound, x1, y1);
printf("Get distance %f with P1\n", _dist);
/*
* take point 2 to endpoint
* the point 2 Y-coordinate
* is gratter than bound
*/
}else if(y2 > bound && y1 < bound){
_dist = __distance(coor_tmp, bound, x2, y2);
printf("Get distance %f with P2\n", _dist);
/*
* means the end point is on
* the bound line
*/
}else{
return 0.0;
}
/* find the X-aixs lower bound endpoint */
}else if(strcmp(part, "X-Lower") == 0){
/*
* take point 1 to endpoint
* the point 1 X-coordinate
* is less than bound
*/
if(x1 < bound && x2 > bound){
_dist = __distance(coor_tmp, bound, x1, y1);
printf("Get distance %f with P1\n", _dist);
/*
* take point 2 to endpoint
* the point 2 X-coordinate
* is less than bound
*/
}else if(x2 < bound && x1 > bound){
_dist = __distance(coor_tmp, bound, x2, y2);
printf("Get distance %f with P2\n", _dist);
/*
* means the endpoint is on
* the bound line
*/
}else{
return 0.0;
}
/* find the X-axis upper boound endpoint */
}else if(strcmp(part, "X-Upper") == 0){
/*
* take point 1 to endpoint
* the point 1 X-coordinate
* is gratter than bound
*/
if(x1 > bound && x2 < bound){
_dist = __distance(bound, coor_tmp, x1, y1);
printf("Get distance %f with P1\n", _dist);
/*
* take point 2 to endpoint
* the point 2 X-coordinate
* is gratter than bound
*/
}else if(x2 > bound && x1 < bound){
_dist = __distance(bound, coor_tmp, x2, y2);
printf("Get distance %f with P2\n", _dist);
/*
* means the endpoint is on
* the bounding line
*/
}else{
return 0.0;
}
}else{
printf("%s error parameter\n", __func__);
exit(0);
}
return _dist;
}
float get_cost(struct argu_table *table)
{
int i;
int mark[2];
float cost = 0.0;
float tmp_x;
float tmp_y;
struct node_info nInfo;
struct node_result nRes;
int x1 = table->x1;
int x2 = table->x2;
int y1 = table->y1;
int y2 = table->y2;
printf("test %d\n", test);
for(i = 0; i < NODE_MAX; i++){
nRes.node_x[i] = 0.0;
nRes.node_y[i] = 0.0;
}
nRes.node_count = 0;
if(_find_area(table, mark) == -1)
return -1;
nInfo.x1 = table->x1;
nInfo.y1 = table->y1;
nInfo.m = _slope(table);
printf("Slope : %f\n", nInfo.m);
// Point 1 area == Point 2 area
if(mark[0] == mark[1]){
switch(mark[0]){
case RED:
cost = __distance(x1, y1, x2, y2) * R_cost;
break;
case YELLOW: /* case: Y->Y or Y->R->Y */
// check with Red-Y-LB(y = 10), find X
nInfo.bound = R_y_L;
tmp_x = _chk_node(table, &nInfo, "R-Y-LB");
if(tmp_x != 0.0){
nRes.node_x[nRes.node_count] = tmp_x;
nRes.node_y[nRes.node_count] = R_y_L; //node(x, 10)
nRes.node_count++;
cost += _get_dist(table, nInfo.bound, tmp_x, "R-Y-LB");
}
// check with Red-Y-UB(y = 40), find X
nInfo.bound = R_y_H;
tmp_x = _chk_node(table, &nInfo, "R-Y-UB");
if(tmp_x != 0.0){
nRes.node_x[nRes.node_count] = tmp_x;
nRes.node_y[nRes.node_count] = R_y_H; //node(x, 40)
nRes.node_count++;
cost += _get_dist(table, nInfo.bound, tmp_x, "R-Y-UB");
}
// check with Red-X-LB(x = 10), find y
nInfo.bound = R_x_L;
tmp_y = _chk_node(table, &nInfo, "R-X-LB");
if(tmp_y != 0.0){
nRes.node_x[nRes.node_count] = R_x_L;
nRes.node_y[nRes.node_count] = tmp_y; //node(10, y)
nRes.node_count++;
cost += _get_dist(table, nInfo.bound, tmp_y, "R-X-LB");
}
// check with Red-X-UB(x = 30), find y
nInfo.bound = R_x_H;
tmp_y = _chk_node(table, &nInfo, "R-X-UB");
if(tmp_y != 0.0){
nRes.node_x[nRes.node_count] = R_x_H;
nRes.node_y[nRes.node_count] = tmp_y;
nRes.node_count++;
cost += _get_dist(table, nInfo.bound, tmp_y, "R-X-UB");
}
printf("Total %d nodes\n", nRes.node_count);
// means Yellow->Yellow, no other area crossed
if(nRes.node_count == 0){
cost = __distance(x1, y1, x2, y2);
// means Yellow->Red->Yellow
}else if(nRes.node_count == 2){
//add the cost of red area
cost += _get_dist_in_area(&nRes, 1, 2);
}else{
printf("Wrong !!! In bost of Yellow case, only 0/2 node is possiable !! ");
printf("But there are %d nodes\n", nRes.node_count);
return -1;
}
break;
case GREEN: /* CASE: G->G or G->Y->G or G->Y->R->G */
// check with Yellow-Y-UB(y = 60), find x
nInfo.bound = Y_y_H;
tmp_x = _chk_node(table, &nInfo, "Y-Y-UB");
if(tmp_x != 0.0){
nRes.node_x[nRes.node_count] = tmp_x;
nRes.node_y[nRes.node_count] = Y_y_H; //node(x, 60)
nRes.node_count++;
cost += _get_dist(table, nInfo.bound, tmp_x, "Y-Y-UB");
}
//check with Yellow-X-UB(x = 50), find y
nInfo.bound = Y_x_H;
tmp_y = _chk_node(table, &nInfo, "Y-X-UB");
if(tmp_y != 0.0){
nRes.node_x[nRes.node_count] = Y_x_H;
nRes.node_y[nRes.node_count] = tmp_y;
nRes.node_count++;
cost += _get_dist(table, nInfo.bound, tmp_y, "Y-X-UB");
}
// check with Red-Y-UB(y = 40), find x
nInfo.bound = R_y_H;
tmp_x = _chk_node(table, &nInfo, "R-Y-UB");
if(tmp_x != 0.0){
nRes.node_x[nRes.node_count] = tmp_x;
nRes.node_y[nRes.node_count] = R_y_H; //node(x, 40)
nRes.node_count++;
/*
* means G->Y->R->G case,
* the Red-Y-UB must link with Yellow-Y-UB
* in node_result array,
* array 0 stored node with Yellow-Y-UB,
* array 2 stored node with Red-Y-UB.
*/
cost += _get_dist_in_area(&nRes, 1, 3);
}
nInfo.bound = R_x_H;
tmp_y = _chk_node(table, &nInfo, "R-X-UB");
if(tmp_y != 0.0){
nRes.node_x[nRes.node_count] = R_x_H;
nRes.node_y[nRes.node_count] = tmp_y;
nRes.node_count++;
/*
* means G->Y->R->G case,
* the Red-X-UB must link with Yellow-X-UB
* in node_result array,
* array 1 stored node with Yellow-X-UB,
* array 3 stored node with Red-X-UB.
*/
cost += _get_dist_in_area(&nRes, 2, 4);
}
printf("Total %d nodes\n", nRes.node_count);
// means Green->Green, no other area crossed
if(nRes.node_count == 0){
cost = __distance(x1, y1, x2, y2);
// means Green->Yellow->Green
}else if(nRes.node_count == 2){
cost += _get_dist_in_area(&nRes, 1, 2);
// means Green->Yellow->Red->Green
}else if(nRes.node_count == 4){
// the Red area cost
cost += _get_dist_in_area(&nRes, 3, 4);
}else{
printf("Wrong !!! In both of Green case, ");
printf(" only 0/2/4 node is possible ! ");
printf("But there are %d nodes\n", nRes.node_count);
return -1;
}
break;
default:
printf("Impossible region!\n");
exit(0);
}
/* Point1 = R, Point2 = Y or Point1 = Y, Point2 = R */
}else if(mark[0] != GREEN && mark[1] != GREEN &&
mark[0] != mark[1]){
/* Only one case, R<->Y */
// check with Red-Y-LB(y = 10), find X
nInfo.bound = R_y_L;
tmp_x = _chk_node(table, &nInfo, "R-Y-LB");
if(tmp_x != 0.0){
nRes.node_x[nRes.node_count] = tmp_x;
nRes.node_x[nRes.node_count] = R_y_L; //node(x, 10)
nRes.node_count++;
cost += _get_dist(table, nInfo.bound, tmp_x, "R-Y-LB");
}
// check with Red-Y-UB(y = 40), find X
nInfo.bound = R_y_H;
tmp_x = _chk_node(table, &nInfo, "R-Y-UB");
if(tmp_x != 0.0){
nRes.node_x[nRes.node_count] = tmp_x;
nRes.node_y[nRes.node_count] = R_y_H; //node(x, 40)
nRes.node_count++;
cost += _get_dist(table, nInfo.bound, tmp_x, "R-Y-UB");
}
// check with Red-X-LB(x = 10), find y
nInfo.bound = R_x_L;
tmp_y = _chk_node(table, &nInfo, "R-X-LB");
if(tmp_y != 0.0){
nRes.node_x[nRes.node_count] = R_x_L;
nRes.node_y[nRes.node_count] = tmp_y; //node(10, y)
nRes.node_count++;
cost += _get_dist(table, nInfo.bound, tmp_y, "R-X-LB");
}
// check with Red-X-UB(x = 30), find y
nInfo.bound = R_x_H;
tmp_y = _chk_node(table, &nInfo, "R-X-UB");
if(tmp_y != 0.0){
nRes.node_x[nRes.node_count] = R_x_H;
nRes.node_y[nRes.node_count] = tmp_y; //node(30, y)
nRes.node_count++;
cost += _get_dist(table, nInfo.bound, tmp_y, "R-X-UB");
}
printf("Total %d nodes\n", nRes.node_count);
// In this case, must have one corss node!
if(nRes.node_count == 1){
if(mark[0] == RED){
cost += __distance(nRes.node_x[nRes.node_count-1],
nRes.node_y[nRes.node_count-1], x1, y1);
}else if(mark[1] == RED){
cost += __distance(nRes.node_x[nRes.node_count-1],
nRes.node_y[nRes.node_count-1], x2, y2);
}else{
printf("Error! in R<->Y case ");
printf("one of mark must be RED!\n");
return -1;
}
}else{
printf("Error! in R<->Y case, must have 1 cross node!\n");
return -1;
}
/* Point1 = R, Point2 = G */
}else if(mark[0] == RED && mark[1] == GREEN){
// only R->Y->G case
// First, check Red boundary
// check with Red-Y-LB(y = 10), find X
nInfo.bound = R_y_L;
tmp_x = _chk_node(table, &nInfo, "R-Y-LB");
if(tmp_x != 0.0){
nRes.node_x[nRes.node_count] = tmp_x;
nRes.node_x[nRes.node_count] = R_y_L; //node(x, 10)
nRes.node_count++;
cost += __distance(x1, y1, nRes.node_x[nRes.node_count-1],
nRes.node_y[nRes.node_count-1]);
}
// check with Red-Y-UB(y = 40), find X
nInfo.bound = R_y_H;
tmp_x = _chk_node(table, &nInfo, "R-Y-UB");
if(tmp_x != 0.0){
nRes.node_x[nRes.node_count] = tmp_x;
nRes.node_y[nRes.node_count] = R_y_H; //node(x, 40)
nRes.node_count++;
cost += __distance(x1, y1, nRes.node_x[nRes.node_count-1],
nRes.node_y[nRes.node_count-1]);
}
// check with Red-X-LB(x = 10), find y
nInfo.bound = R_x_L;
tmp_y = _chk_node(table, &nInfo, "R-X-LB");
if(tmp_y != 0.0){
nRes.node_x[nRes.node_count] = R_x_L;
nRes.node_y[nRes.node_count] = tmp_y; //node(10, y)
nRes.node_count++;
cost += __distance(x1, y1, nRes.node_x[nRes.node_count-1],
nRes.node_y[nRes.node_count-1]);
}
// check with Red-X-UB(x = 30), find y
nInfo.bound = R_x_H;
tmp_y = _chk_node(table, &nInfo, "R-X-UB");
if(tmp_y != 0.0){
nRes.node_x[nRes.node_count] = R_x_H;
nRes.node_y[nRes.node_count] = tmp_y; //node(30, y)
nRes.node_count++;
cost += __distance(x1, y1, nRes.node_x[nRes.node_count-1],
nRes.node_y[nRes.node_count-1]);
}
// find the node cross with Yellow
// check with Yellow-Y-UB(y = 60), find x
nInfo.bound = Y_y_H;
tmp_x = _chk_node(table, &nInfo, "Y-Y-UB");
if(tmp_x != 0.0){
nRes.node_x[nRes.node_count] = tmp_x;
nRes.node_y[nRes.node_count] = Y_y_H; //node(x, 60)
nRes.node_count++;
cost += _get_dist_in_area(&nRes, 1, 2);
}
//check with Yellow-X-UB(x = 50), find y
nInfo.bound = Y_x_H;
tmp_y = _chk_node(table, &nInfo, "Y-X-UB");
if(tmp_y != 0.0){
nRes.node_x[nRes.node_count] = Y_x_H;
nRes.node_y[nRes.node_count] = tmp_y; //node(50, y)
nRes.node_count++;
cost += _get_dist_in_area(&nRes, 1, 2);
}
// in R->Y->G case, must have 2 node
printf("Total %d node\n", nRes.node_count);
if(nRes.node_count == 2){
cost += __distance(x2, y2, nRes.node_x[nRes.node_count-1],
nRes.node_y[nRes.node_count-1]);
}else{
printf("In R->Y->G case, there are 2 corss node, but only %d node\n", nRes.node_count);
return -1;
}
/* point1 = G, point2 = R */
}else if(mark[0] == GREEN && mark[1] == RED){
// only G->Y->R case
// First check Yellow boundary
// check with Yellow-Y-UB(y = 60), find x
nInfo.bound = Y_y_H;
tmp_x = _chk_node(table, &nInfo, "Y-Y-UB");
if(tmp_x != 0.0){
nRes.node_x[nRes.node_count] = tmp_x;
nRes.node_y[nRes.node_count] = Y_y_H; //node(x, 60)
nRes.node_count++;
cost += __distance(x1, y1, nRes.node_x[nRes.node_count-1],
nRes.node_y[nRes.node_count-1]);
}
// check with Yellow-X-UB(x = 50), find y
nInfo.bound = Y_x_H;
tmp_y = _chk_node(table, &nInfo, "Y-X-UB");
if(tmp_y != 0.0){
nRes.node_x[nRes.node_count] = Y_x_H;
nRes.node_y[nRes.node_count] = tmp_y; //node(50, y)
nRes.node_count++;
cost += __distance(x1, y1, nRes.node_x[nRes.node_count-1],
nRes.node_y[nRes.node_count-1]);
}
// Secondly, check with Red bound
// check with Red-Y-LB(y = 10), find X
nInfo.bound = R_y_L;
tmp_x = _chk_node(table, &nInfo, "R-Y-LB");
if(tmp_x != 0.0){
nRes.node_x[nRes.node_count] = tmp_x;
nRes.node_x[nRes.node_count] = R_y_L; //node(x, 10)
nRes.node_count++;
cost += _get_dist_in_area(&nRes, nRes.node_count-1 ,nRes.node_count);
}
// check with Red-Y-UB(y = 40), find X
nInfo.bound = R_y_H;
tmp_x = _chk_node(table, &nInfo, "R-Y-UB");
if(tmp_x != 0.0){
nRes.node_x[nRes.node_count] = tmp_x;
nRes.node_y[nRes.node_count] = R_y_H; //node(x, 40)
nRes.node_count++;
cost += _get_dist_in_area(&nRes, nRes.node_count-1, nRes.node_count);
}
// check with Red-X-LB(x = 10), find y
nInfo.bound = R_x_L;
tmp_y = _chk_node(table, &nInfo, "R-X-LB");
if(tmp_y != 0.0){
nRes.node_x[nRes.node_count] = R_x_L;
nRes.node_y[nRes.node_count] = tmp_y; //node(10, y)
nRes.node_count++;
cost += _get_dist_in_area(&nRes, nRes.node_count-1, nRes.node_count);
}
// check with Red-X-UB(x = 30), find y
nInfo.bound = R_x_H;
tmp_y = _chk_node(table, &nInfo, "R-X-UB");
if(tmp_y != 0.0){
nRes.node_x[nRes.node_count] = R_x_H;
nRes.node_y[nRes.node_count] = tmp_y; //node(30, y)
nRes.node_count++;
cost += _get_dist_in_area(&nRes, nRes.node_count-1, nRes.node_count);
}
// in G->Y->R case , must have 2 node
printf("Total %d node\n", nRes.node_count);
if(nRes.node_count == 2){
cost += __distance(x2, y2, nRes.node_x[nRes.node_count-1],
nRes.node_y[nRes.node_count-1]);
}else{
printf("In G->Y->R case, there are 2 cross node, but only %d node\n", nRes.node_count);
return -1;
}
/* point1 = G, point2 = Y */
}else if(mark[0] == GREEN && mark[1] == YELLOW){
// G->Y->R->Y & G->Y case!
// First, check with Yellow boundary
/* !!! <Check Upper bound FIRST> !!!! */
// check with Yellow-Y-UB(y = 60), find x
nInfo.bound = Y_y_H;
tmp_x = _chk_node(table, &nInfo, "Y-Y-UB");
if(tmp_x != 0.0){
nRes.node_x[nRes.node_count] = tmp_x;
nRes.node_y[nRes.node_count] = Y_y_H; //node(x, 60)
nRes.node_count++;
cost += __distance(x1, y1, nRes.node_x[nRes.node_count-1],
nRes.node_y[nRes.node_count-1]);
}
// check with Yellow-X-UB(x = 50), find y
nInfo.bound = Y_x_H;
tmp_y = _chk_node(table, &nInfo, "Y-X-UB");
if(tmp_y != 0.0){
nRes.node_x[nRes.node_count] = Y_x_H;
nRes.node_y[nRes.node_count] = tmp_y; //node(50, y)
nRes.node_count++;
cost += __distance(x1, y1, nRes.node_x[nRes.node_count-1],
nRes.node_y[nRes.node_count-1]);
}
// Secondly, check with Red boundary
// check with Red-Y-UB(y = 40), find X
nInfo.bound = R_y_H;
tmp_x = _chk_node(table, &nInfo, "R-Y-UB");
if(tmp_x != 0.0){
nRes.node_x[nRes.node_count] = tmp_x;
nRes.node_y[nRes.node_count] = R_y_H; //node(x, 40)
nRes.node_count++;
cost += _get_dist_in_area(&nRes, nRes.node_count-1, nRes.node_count);
}
// check with Red-X-UB(x = 30), find y
nInfo.bound = R_x_H;
tmp_y = _chk_node(table, &nInfo, "R-X-UB");
if(tmp_y != 0.0){
nRes.node_x[nRes.node_count] = R_x_H;
nRes.node_y[nRes.node_count] = tmp_y; //node(30, y)
nRes.node_count++;
cost += _get_dist_in_area(&nRes, nRes.node_count-1, nRes.node_count);
}
// check with Red-Y-LB(y = 10), find X
nInfo.bound = R_y_L;
tmp_x = _chk_node(table, &nInfo, "R-Y-LB");
if(tmp_x != 0.0){
nRes.node_x[nRes.node_count] = tmp_x;
nRes.node_x[nRes.node_count] = R_y_L; //node(x, 10)
nRes.node_count++;
cost += _get_dist_in_area(&nRes, nRes.node_count-1 ,nRes.node_count);
}
// check with Red-X-LB(x = 10), find y
nInfo.bound = R_x_L;
tmp_y = _chk_node(table, &nInfo, "R-X-LB");
if(tmp_y != 0.0){
nRes.node_x[nRes.node_count] = R_x_L;
nRes.node_y[nRes.node_count] = tmp_y; //node(10, y)
nRes.node_count++;
cost += _get_dist_in_area(&nRes, nRes.node_count-1, nRes.node_count);
}
printf("Total %d node\n", nRes.node_count);
// must have 1 or 3 node
if(nRes.node_count == 3 || nRes.node_count == 1){
cost += __distance(x2, y2, nRes.node_x[nRes.node_count-1],
nRes.node_y[nRes.node_count-1]);
}else{
printf("In G->Y/G->Y->R->Y case, must have 1 or 3 node!\n");
return -1;
}
// point1 = Y, point2 = G
}else if(mark[0] == YELLOW && mark[1] == GREEN){
// First, check with Red boundary
/* !!! <Check Lower bound FIRST> !!! */
// check with Red-Y-LB(y = 10), find X
nInfo.bound = R_y_L;
tmp_x = _chk_node(table, &nInfo, "R-Y-LB");
if(tmp_x != 0.0){
nRes.node_x[nRes.node_count] = tmp_x;
nRes.node_x[nRes.node_count] = R_y_L; //node(x, 10)
nRes.node_count++;
// first node need to calculate cost with endpoint
if(nRes.node_count == 1){
cost += __distance(x1, y1,
nRes.node_x[nRes.node_count-1], nRes.node_y[nRes.node_count-1]);
}else{
cost += _get_dist_in_area(&nRes, nRes.node_count-1, nRes.node_count);
}
}
// check with Red-X-LB(x = 10), find y
nInfo.bound = R_x_L;
tmp_y = _chk_node(table, &nInfo, "R-X-LB");
if(tmp_y != 0.0){
nRes.node_x[nRes.node_count] = R_x_L;
nRes.node_y[nRes.node_count] = tmp_y; //node(10, y)
nRes.node_count++;
// first node need to calculate cost with endpoint
if(nRes.node_count == 1){
cost += __distance(x1, y1,
nRes.node_x[nRes.node_count-1], nRes.node_y[nRes.node_count-1]);
}else{
cost += _get_dist_in_area(&nRes, nRes.node_count-1, nRes.node_count);
}
}
// check with Red-Y-UB(y = 40), find x
nInfo.bound = R_y_H;
tmp_x = _chk_node(table, &nInfo, "R-Y-UB");
if(tmp_x != 0.0){
nRes.node_x[nRes.node_count] = tmp_x;
nRes.node_y[nRes.node_count] = R_y_H; //node(x, 40)
nRes.node_count++;
// first node need to calculate cost with endpoint
if(nRes.node_count == 1){
cost += __distance(x1, y1,
nRes.node_x[nRes.node_count-1], nRes.node_y[nRes.node_count-1]);
}else{
cost += _get_dist_in_area(&nRes, nRes.node_count-1, nRes.node_count);
}
}
// check with Red-X-UB(x = 30), find y
nInfo.bound = R_x_H;
tmp_y = _chk_node(table, &nInfo, "R-X-UB");
if(tmp_y != 0.0){
nRes.node_x[nRes.node_count] = R_x_H;
nRes.node_y[nRes.node_count] = tmp_y; //node(30, y)
nRes.node_count++;
// first node need to calculate cost with endpoint
if(nRes.node_count == 1){
cost += __distance(x1, y1,
nRes.node_x[nRes.node_count-1], nRes.node_y[nRes.node_count-1]);
}else{
cost += _get_dist_in_area(&nRes, nRes.node_count-1, nRes.node_count);
}
}
// Secondly, check Yellow boundary
// check with Yellow-Y-UB(y = 60), find x
nInfo.bound = Y_y_H;
tmp_x = _chk_node(table, &nInfo, "Y-Y-UB");
if(tmp_x != 0.0){
nRes.node_x[nRes.node_count] = tmp_x;
nRes.node_y[nRes.node_count] = Y_y_H; //node(x, 60)
nRes.node_count++;
cost += _get_dist_in_area(&nRes, nRes.node_count-1, nRes.node_count);
}
// check with Yellow-X-UB(x = 50), find y
nInfo.bound = Y_x_H;
tmp_y = _chk_node(table, &nInfo, "Y-X-UB");
if(tmp_y != 0.0){
nRes.node_x[nRes.node_count] = Y_x_H;
nRes.node_y[nRes.node_count] = tmp_y; //node(50, y)
nRes.node_count++;
cost += _get_dist_in_area(&nRes, nRes.node_count-1, nRes.node_count);
}
printf("Total %d node\n", nRes.node_count);
// must have 1 or 3 node
if(nRes.node_count == 3 || nRes.node_count == 1){
cost += __distance(x2, y2, nRes.node_x[nRes.node_count-1],
nRes.node_y[nRes.node_count-1]);
}else{
printf("In Y->G/Y->R->Y->G case, must have 1 or 3 node!\n");
return -1;
}
}else{
printf("F****n other case ???? F**K!!!\n");
return -1;
}
return cost;
}
float _get_dist_in_area(struct node_result *nRes, int start, int des)
{
return __distance(nRes->node_x[start-1], nRes->node_y[start-1],
nRes->node_x[des-1], nRes->node_y[des-1]);
}