/// <summary>
/// 他のマーカとの距離を返す
/// </summary>
/// <param name="B"></param>
/// <returns></returns>
public double GetDistance(MarkPoint B)
{
double dx = (x - B.x);
double dy = (y - B.y);
return(Math.Sqrt((dx * dx) + (dy * dy)));
}
/// <summary>
/// マーカーと同じか比較
/// </summary>
/// <param name="B"></param>
/// <returns></returns>
public bool IsEqual(MarkPoint B)
{
if (x == B.x && y == B.y && theta == B.theta)
{
return(true);
}
return(false);
}
// -------------------------------------------------------------------------------------------------
//
/// <summary>
///
/// </summary>
/// <param name="fnameMapBmp"></param>
/// <param name="worldWith">実際のマップサイズ mm</param>
/// <param name="worldHeight">実際のマップサイズ mm</param>
public LocationSystem(MapData _mapData)
{
//
mapData = _mapData;
mapBmp = BitmapExtensions.Load(mapData.MapImageFileName);
if (null == mapBmp)
{
mapBmp = new Bitmap(mapData.MapImageFileName);
}
// スケール
MapTom = mapData.Resolution; // (mapData.RealWidth / (double)mapBmp.Width); // 実サイズ（m）/ピクセル数 ＝ １ピクセルを何mとするか
mToMap = 1.0 / MapTom;
RTS = new Rooting(mapData);
R1 = new MarkPoint(0, 0, 0);
oldR1 = new MarkPoint(0, 0, 0); // 距離取得用 前回位置
A1 = new MarkPoint(0, 0, 0); // ROS AMCL位置
E1 = new MarkPoint(0, 0, 0); // Encoader Plot
}
/// <summary>
///
/// </summary>
/// <param name="B"></param>
public void Set(MarkPoint B)
{
Set(B.x, B.y, B.theta);
}
public MarkPoint(MarkPoint mkp) : this(mkp.x, mkp.y, mkp.theta)
{
}
/// <summary>
/// ROS座標からマップ座標系へ変換
/// </summary>
/// <param name="mkp"></param>
/// <param name="mapScale"></param>
public DrawMarkPoint(MarkPoint mkp, LocationSystem locSys)
{
x = (float)(mkp.x * locSys.mToMap);
y = (float)(-mkp.y * locSys.mToMap);
theta = (float)(-mkp.theta);
}
