#include?
#include?
#include?
#ifdef?_MSC_VER
???typedef?__int64?int64_t;	//?Define?it?from?MSVC‘s?internal?type
#else
???#include?		//?Use?the?C99?official?header
#endif
#include?“CoreSLAM.h“

void?
ts_map_init（ts_map_t?*map）????????????????????????????????//初始化地圖把地圖每個(gè)柵格的值（value）都置為（TS_OBSTACLE?+?TS_NO_OBSTACLE）?/?2；介于障礙和無障礙之間（未知）
{
????int?x?y?initval;
????ts_map_pixel_t?*ptr;
????initval?=?（TS_OBSTACLE?+?TS_NO_OBSTACLE）?/?2;
????for?（ptr?=?map->map?y?=?0;?y?	for?（x?=?0;?x?	????*ptr?=?initval;
	}
????}
}

int
ts_distance_scan_to_map（ts_scan_t?*scan?ts_map_t?*map?ts_position_t?*pos）???????????????//把機(jī)器人坐標(biāo)系下的障礙點(diǎn)映射到全局坐標(biāo)系地圖下，計(jì)算
{
????double?c?s;
????int?i?x?y?nb_points?=?0;
????int64_t?sum;

????c?=?cos（pos->theta?*?M_PI?/?180）;
????s?=?sin（pos->theta?*?M_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）floor（（pos->x?+?c?*?scan->x[i]?-?s?*?scan->y[i]）?*?TS_MAP_SCALE?+?0.5）;????//x，y是雷達(dá)點(diǎn)在全局地圖中所在的的柵格行數(shù)列數(shù)
????????????y?=?（int）floor（（pos->y?+?s?*?scan->x[i]?+?c?*?scan->y[i]）?*?TS_MAP_SCALE?+?0.5）;
????????????//?Check?boundaries
????????????if?（x?>=?0?&&?x?=?0?&&?y?????????????????sum?+=?map->map[y?*?TS_MAP_SIZE?+?x];???????????????????????????//sum計(jì)算這一幀每個(gè)雷達(dá)點(diǎn)轉(zhuǎn)化到全局地圖中所在的柵格點(diǎn)的值（value）之和
????????????????nb_points++;
????????????}
????????}
????}
????if?（nb_points）?sum?=?sum?*?1024?/?nb_points;
????else?sum?=?2000000000;
????return?（int）sum;
}

#define?SWAP（x?y）?（x?^=?y?^=?x?^=?y）

void
ts_map_laser_ray（ts_map_t?*map?int?x1?int?y1?int?x2?int?y2?
?????????????????int?xp?int?yp?int?value?int?alpha）
																	//?x1?y1:?機(jī)器人在全局地圖的位置，單位:cm
																	//?x2?y2:?某一個(gè)雷達(dá)點(diǎn)所在的“圓洞”（holes）的最遠(yuǎn)端在全局地圖的位置，單位：cm
																	//?xp?yp:?某一個(gè)雷達(dá)點(diǎn)在全局地圖的位置，單位：cm?
																	//?alpha:?TS_NO_OBSTACLE時(shí)，?q?=?50?/?4;?TS_OBSTACLE。
																	//?如果該雷達(dá)點(diǎn)處值（柵格值）為TS_NO_OBSTACLEvalue則為TS_NO_OBSTACLE，65500
																	//?如果其他情況value則為TS_NO_OBSTACLE

{
????int?x2c?y2c?dx?dy?dxc?dyc?error?errorv?derrorv?x;
????int?incv?sincv?incerrorv?incptrx?incptry?pixval?horiz?diago;
????ts_map_pixel_t?*ptr;

????if?（x1?=?TS_MAP_SIZE?||?y1?=?TS_MAP_SIZE）
????????return;?//?Robot?is?out?of?map
????
????x2c?=?x2;?y2c?=?y2;
????//?Clipping?????????????????????????????????????????????????????????????????
	//?如果雷達(dá)點(diǎn)的位置超出地圖范圍，就取雷達(dá)點(diǎn)所在的那條線與地圖的交點(diǎn)處為（x2c，y2c）；
	//?否則雷達(dá)點(diǎn)位置未超出地圖范圍，就取真實(shí)雷達(dá)點(diǎn)坐標(biāo)為（x2c，y2c）；
????if?（x2c?????????if?（x2c?==?x1）?return;
????????y2c?+=?（y2c?-?y1）?*?（-x2c）?/?（x2c?-?x1）;
????????x2c?=?0;
????}
????if?（x2c?>=?TS_MAP_SIZE）?{
????????if?（x1?==?x2c）?return;
????????y2c?+=?（y2c?-?y1）?*?（TS_MAP_SIZE?-?1?-?x2c）?/?（x2c?-?x1）;
????????x2c?=?TS_MAP_SIZE?-?1;
????}
????if?（y2c?????????if?（y1?==?y2c）?return;
????????x2c?+=

?屬性????????????大小?????日期????時(shí)間???名稱
-----------?---------??----------?-----??----
?????目錄???????????0??2016-03-26?17:07??TINYSLAM\
?????文件????????1017??2011-05-11?16:33??TINYSLAM\CMakeLists.txt
?????文件????????9363??2016-03-15?15:42??TINYSLAM\CoreSLAM.c
?????文件????????4713??2016-03-15?22:09??TINYSLAM\CoreSLAM.h
?????文件????????3191??2011-05-11?16:33??TINYSLAM\CoreSLAM_ext.c
?????文件????????1666??2011-05-11?16:33??TINYSLAM\CoreSLAM_loop_closing.c
?????文件????????4060??2016-03-09?21:09??TINYSLAM\CoreSLAM_random.c
?????文件????????5283??2011-05-11?16:34??TINYSLAM\CoreSLAM_state.c
?????文件?????????332??2011-05-11?16:34??TINYSLAM\Makefile.am
?????文件??????????33??2011-05-11?16:34??TINYSLAM\authors.txt
?????文件??????????69??2011-05-11?16:34??TINYSLAM\bootstrap.txt
?????文件?????????774??2011-05-11?16:35??TINYSLAM\cible.bmp
?????文件????????1863??2011-05-11?16:34??TINYSLAM\configure.ac
?????文件?????1344527??2011-05-11?16:35??TINYSLAM\exp4.dat
?????文件?????????652??2011-05-11?16:35??TINYSLAM\gnuplot.txt
?????文件???????12342??2011-05-11?16:35??TINYSLAM\icone.bmp
?????文件???????????0??2011-05-11?16:34??TINYSLAM\news.txt
?????文件??????????47??2011-05-11?16:34??TINYSLAM\readme.txt
?????文件?????1323008??2011-05-11?16:34??TINYSLAM\test_lab.dat
?????文件??????901120??2011-05-11?16:35??TINYSLAM\test_lab2.dat
?????文件???????13306??2016-03-18?15:13??TINYSLAM\test_lab_reverse.c
?????文件???????11660??2011-05-11?16:34??TINYSLAM\test_loop_closing.c