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);
}
}
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);
}