#include?“OpenRadar.h“
#define?pi?3.141592653

OpenRadar::OpenRadar（void）
{
}
OpenRadar::~OpenRadar（void）
{
}
bool?OpenRadar::RadarRead（char?*fileName）{
????FILE*?fp?=?NULL;
????int?rho?=?0;
????double?theta;
????double?deltaTeta;
#ifdef?UTM30LX
???????theta?=?（135.0?+?90）*pi/180;
???????deltaTeta?=?-0.25*pi/180;
#elif?defined?URG04LX
????theta?=?（120.0?+?90）*pi/180;
????deltaTeta?=?-0.36*pi/180;
#endif

????int?totalCnt?=?0;
????fp?=?fopen（fileName“r“）;
????if?（fp?==?NULL）
????{
????????//cout<<“failed?to?read“<????????return?false;
????}else?{
????????//cout<<“successed?to?read“<????????RadarRho.clear（）;
????????RadarTheta.clear（）;

????????while（!feof（fp））
????????{

????????????fscanf（fp?“%d?“?&rho）;
????????????RadarRho.push_back（rho）;
????????????RadarTheta.push_back（theta）;
????????????theta?+=?deltaTeta;
????????????//printf（“%d??“?rho）;
????????}
????????//cout<<“Total?Count:?“<????????//
????}
????fclose（fp）;
????return?true;
}

void?OpenRadar::CreateRadarImage（IplImage*?RadarImagevector&?RadarRhovector&?RadarTheta）{
????//RadarImage?=?cvCreateImage（cvSize（RadarImageWdithRadarImageHeight）IPL_DEPTH_8U1）;
????cvZero（RadarImage）;
????//在中心加上一個圓心
????//int?halfWidth??=?RadarImageWdith/2;
????//int?halfHeight?=?RadarImageHeight/2;
????int?dx?=??RadarImageWdith/2;
????int?dy?=??RadarImageHeight*3/4;

????cvCircle（RadarImage?cvPoint（dxdy）3?CV_RGB（0255255）?-1?80）;

????int?xy;
????double?thetarho;
????unsigned?char?*?pPixel?=?0;
????


????//顏色
????int?colorIndex?=?0colorRGB;
????int?R?=?255G?=?0B?=?0;
????int?pointCnt?=?0;
??

????for?（int?i?=?0;?i?????{
????????//theta?=?（pointCnt/4.0?-?45）*pi/180;
????????theta?=?RadarTheta.at（i）;
????????rho?=?RadarRho.at（i）;
????????if?（rho?????????{
???????????//雷達數據斷點標志
????????????colorRGB?=?usualColor[colorIndex];
????????????R?=?colorRGB/65536;
????????????G?=?（colorRGB%65536）/256;
????????????B?=?colorRGB%256;?
????????????colorIndex?=?（colorIndex?+?1）%10;
????????}else?{
????????????pointCnt++;
????????}

????????x?=?（int）（rho*cos（theta）/10）?+?dx;
????????y?=?（int）（-rho*sin（theta）/10）+?dy;

????????if?（x?>=?0?&&?x?=?0?&&?y?????????{
????????????pPixel?=?（unsigned?char*）RadarImage->imageData?+?y*RadarImage->widthStep?+?3*x;
????????????pPixel[0]?=?B;
????????????pPixel[1]?=?G;
????????????pPixel[2]?=?R;
????????}else{
????????????//cout<<“x:?“<????????}
????}

}

int?OpenRadar::BreakRadarRho（）{
????int?breakCnt?=?0;
????int?rho?=?0;
????
????int?lastRho?=?RadarRho.at（0）;
????double?theta?=?RadarTheta.at（0）;
????int?dis?=?0;
????int?Dmax?=?50;

????BreakedRadarRho.clear（）;
????BreakedRadarTheta.clear（）;

????BreakedRadarRho.push_back（lastRho）;
????BreakedRadarTheta.push_back（theta）;

????for?（int?i?=?1;?i