internal int ts_distance_scan_to_map(ts_scan_t scan, ts_position_t pos) { double c, s; int i, x, y, nb_points = 0; Int64 sum; c = exMath.Cos(pos.theta * exMath.PI / 180); s = exMath.Sin(pos.theta * exMath.PI / 180); // Translate and rotate scan to robot position // and compute the distance for (i = 0, sum = 0; i != scan.nb_points; i++) { if (scan.value[i] != TS_NO_OBSTACLE) { x = (int)exMath.Floor((pos.x + c * scan.x[i] - s * scan.y[i]) * TS_MAP_SCALE + 0.5); y = (int)exMath.Floor((pos.y + s * scan.x[i] + c * scan.y[i]) * TS_MAP_SCALE + 0.5); //Check boundaries if (x >= 0 && x < TS_MAP_SIZE && y >= 0 && y < TS_MAP_SIZE) { sum += map.map[y * TS_MAP_SIZE + x]; nb_points++; } } } if (nb_points > 0) sum = sum * 1024 / nb_points; else sum = 2000000000; return (int)sum; }
internal int ts_distance_scan_to_map(ts_scan_t scan, ts_position_t pos) { double c, s; int i, x, y, nb_points = 0; Int64 sum; c = exMath.Cos(pos.theta * exMath.PI / 180); s = exMath.Sin(pos.theta * exMath.PI / 180); // Translate and rotate scan to robot position // and compute the distance for (i = 0, sum = 0; i != scan.nb_points; i++) { if (scan.value[i] != TS_NO_OBSTACLE) { x = (int)exMath.Floor((pos.x + c * scan.x[i] - s * scan.y[i]) * TS_MAP_SCALE + 0.5); y = (int)exMath.Floor((pos.y + s * scan.x[i] + c * scan.y[i]) * TS_MAP_SCALE + 0.5); //Check boundaries if (x >= 0 && x < TS_MAP_SIZE && y >= 0 && y < TS_MAP_SIZE) { sum += map.map[y * TS_MAP_SIZE + x]; nb_points++; } } } if (nb_points > 0) { sum = sum * 1024 / nb_points; } else { sum = 2000000000; } return((int)sum); }
public static void NewScan(ts_scan_t scan, ts_position_t position) { //a.ts_distance_scan_to_map(scan, position); if (a != null) { a.ts_map_update(scan, position, 50); } }
internal void ts_map_update(ts_scan_t scan, ts_position_t pos, int quality) { //cos sin quality double c, s, q; //Pos scan rotated double x2p, y2p; //x1y1: Pos scaled ||xpyp: Absolute pos scan int i, x1, y1, x2, y2, xp, yp, value; double add, dist; c = exMath.Cos(pos.theta * exMath.PI / 180); s = exMath.Sin(pos.theta * exMath.PI / 180); x1 = (int)exMath.Floor(pos.x * TS_MAP_SCALE + 0.5); y1 = (int)exMath.Floor(pos.y * TS_MAP_SCALE + 0.5); // Translate and rotate scan to robot position for (i = 0; i != scan.nb_points; i++) { //Before TS_HOLE_WIDTH x2p = c * scan.x[i] - s * scan.y[i]; y2p = s * scan.x[i] + c * scan.y[i]; xp = (int)exMath.Floor((pos.x + x2p) * TS_MAP_SCALE + 0.5); yp = (int)exMath.Floor((pos.y + y2p) * TS_MAP_SCALE + 0.5); /*************************************************************************/ //CAUTION: If x2p or y2p are too high, the Sqrt operation will be very slow double x2p_meter = x2p / 1000; double y2p_meter = y2p / 1000; dist = exMath.Sqrt(x2p_meter * x2p_meter + y2p_meter * y2p_meter); dist *= 1000; /************************************************************************/ add = TS_HOLE_WIDTH / 2 / dist; //After TS_HOLE_WIDTH x2p *= TS_MAP_SCALE * (1 + add); y2p *= TS_MAP_SCALE * (1 + add); x2 = (int)exMath.Floor(pos.x * TS_MAP_SCALE + x2p + 0.5); y2 = (int)exMath.Floor(pos.y * TS_MAP_SCALE + y2p + 0.5); if (scan.value[i] == TS_NO_OBSTACLE) { q = quality / 2; value = TS_NO_OBSTACLE; } else { q = quality; value = TS_OBSTACLE; } ts_map_laser_ray(x1, y1, x2, y2, xp, yp, value, (int)q); } }
public static void NewScan(ts_scan_t scan, ts_position_t position) { //a.ts_distance_scan_to_map(scan, position); if (a != null) a.ts_map_update(scan, position, 50); }