/// <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)/ピクセル数 = 1ピクセルを何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); }