예제 #1
0
파일: lines.c 프로젝트: chboing/vlm
/**
 * 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;
}
예제 #2
0
파일: lines.c 프로젝트: chboing/vlm
/**
 * 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;
}
예제 #3
0
파일: pr60659.C 프로젝트: pjump/gcc
void distance (_InputIterator, _InputIterator p2)
{
    __distance (p2);
}
예제 #4
0
파일: get_cost.c 프로젝트: soso7885/trunk
/*
 * 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;
}
예제 #5
0
파일: get_cost.c 프로젝트: soso7885/trunk
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;
}
예제 #6
0
파일: get_cost.c 프로젝트: soso7885/trunk
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]);
}