#include?“Coordinate.h“


Coordinate::Coordinate（void）
{
}


Coordinate::~Coordinate（void）
{
}

//已知四條直線如何計算變換參數
void?Coordinate::CalCoorTransPara（CoorTransPara?&transPara
??????????????????????????????????LinePara?W1
??????????????????????????????????LinePara?W2?
??????????????????????????????????LinePara?R1?
??????????????????????????????????LinePara?R2）
{
????double?theta?=?（?W1.Rho?-?R1.Rho?+?W2.Rho?-?R2.Rho?）/2;
????//double?theta?=?（?W1.Rho?-?R1.Rho）;
????//求解出Xw?Yw?Xr?Yr
????double?Xw?=?（double）（W1.b?-?W2.b）/（W2.a?-?W1.a）;
????double?Yw?=?W1.a*Xw?+?W1.b;

????double?Xr?=?（double）（R1.b?-?R2.b）/（R2.a?-?R1.a）;
????double?Yr?=?R1.a*Xr?+?R1.b;


????int?Tx?=?（int）（Xw?-?cos（theta）*Xr?+?sin（theta）*Yr）;
????int?Ty?=?（int）（Yw?-?sin（theta）*Xr?-?cos（theta）*Yr）;
????//交點判定，場地上的幾條直線都是有角點的
????iPoint?crossPoint;//交點
????iPoint?vectorW1vectorR1;//向量
????//iPoint?vectorR2vectorW2;
????if?（W1.startPoint.x?==?W2.startPoint.x?&&?W1.startPoint.y?==?W2.startPoint.y）
????{
????????crossPoint?=?ipoint（W1.startPoint.xW1.startPoint.y）;
????????vectorW1?=?ipoint（W1.endPoint.x?-?W1.startPoint.x?W1.endPoint.y?-?W1.startPoint.y）;
????????//vectorW2?=?ipoint（W2.endPoint.x?-?W2.startPoint.x?W2.endPoint.y?-?W2.startPoint.y）;
????}else?if?（W1.endPoint.x?==?W2.startPoint.x?&&?W1.endPoint.y?==?W2.startPoint.y）
????{
????????crossPoint?=?ipoint（W1.endPoint.xW1.endPoint.y）;
????????vectorW1?=?ipoint（W1.startPoint.x?-?W1.endPoint.x?W1.startPoint.y?-?W1.endPoint.y）;
????????//vectorW2?=?ipoint（W2.endPoint.x?-?W2.startPoint.x?W2.endPoint.y?-?W2.startPoint.y）;
????}else?if?（W1.startPoint.x?==?W2.endPoint.x?&&?W1.startPoint.y?==?W2.endPoint.y）
????{
?????????crossPoint?=?ipoint（W1.startPoint.xW1.startPoint.y）;
?????????vectorW1?=?ipoint（W1.endPoint.x?-?W1.startPoint.x?W1.endPoint.y?-?W1.startPoint.y）;
?????????//vectorW2?=?ipoint（W2.startPoint.x?-?W2.endPoint.x?W2.startPoint.y?-?W2.endPoint.y）;
????}else?if?（W1.endPoint.x?==?W2.endPoint.x?&&?W1.endPoint.y?==?W2.endPoint.y）
????{
?????????crossPoint?=?ipoint（W1.endPoint.xW1.endPoint.y）;
?????????vectorW1?=?ipoint（W1.startPoint.x?-?W1.endPoint.x?W1.startPoint.y?-?W1.endPoint.y）;
?????????//vectorW2?=?ipoint（W2.startPoint.x?-?W2.endPoint.x?W2.startPoint.y?-?W2.endPoint.y）;
????}
????//將激光雷達下的兩個點旋轉到W系下
????transPara.theta?=?theta;
????transPara.Tx?=?Tx;
????transPara.Ty?=?Ty;
????iPoint?R1ToW;
????//iPoint?R2ToW;
????TransformCoord（transParaR1.startPointR1ToW）;
????//TransformCoord（transParaR2.startPointR2ToW）;
????vectorR1.x?=?R1ToW.x?-?crossPoint.x;
????vectorR1.y?=?R1ToW.y?-?crossPoint.y;
????//判斷是否在同一側？
????if?（vectorW1.x?*?vectorR1.x?+?vectorW1.y*vectorR1.y?????{
????????//旋轉角度差了180度，需要調轉180度
????????transPara.theta?=?theta?+?PI;
????????transPara.Tx?=?（int）（Xw?-?cos（transPara.theta）*Xr?+?sin（transPara.theta）*Yr）;
????????transPara.Ty?=?（int）（Yw?-?sin（transPara.theta）*Xr?-?cos（transPara.theta）*Yr）;
????}else{

????}
????//數據測試

?屬性????????????大小?????日期????時間???名稱
-----------?---------??----------?-----??----
?????文件????????4520??2013-02-26?09:37??Coordinate.cpp
?????文件????????2556??2013-02-23?16:27??Coordinate.h
?????文件???????28810??2013-02-26?19:31??OpenRadar.cpp
?????文件????????3710??2013-02-26?18:40??OpenRadar.h
?????文件???????14454??2013-02-19?22:58??QSort.h
?????文件???????17788??2013-02-26?20:34??Radar.cpp
?????文件????????3895??2013-02-26?19:26??Serial.h
?????文件???????16690??2013-02-26?18:20??URG.h
?????文件???????10253??2013-02-26?19:56??WeightedFit.cpp
?????文件????????2329??2013-02-26?09:33??WeightedFit.h