/// <summary>
        /// 左右のホイールの移動量を、X,Y座標に変換
        /// </summary>
        /// <param name="resXYWR">右車輪のX,Y座標</param>
        /// <param name="resXYWL">左車輪のX,Y座標</param>
        /// <param name="resAng">現在の向き</param>
        /// <param name="reWR">右車輪のREncoderの値 絶対値</param>
        /// <param name="reWL">左車輪のREncoderの値 絶対値</param>
        /// <param name="reOldWR">右車輪のREncoderの値 絶対値 (前回値)</param>
        /// <param name="reOldWL">左車輪のREncoderの値 絶対値 (前回値)</param>
        public static void CalcWheelPlotXY(ref PointD resXYWR, ref PointD resXYWL, ref double resAng, double reWR, double reWL, double reOldWR, double reOldWL)
        {
            double mov;
            double deg = REncodeDataToDegree(reWR, reWL);
            double ang = (deg-90.0) * Math.PI / 180.0;

            // 右車輪
            mov = WheelRotateToLength(reWR - reOldWR);

            resXYWR.X += (mov * -Math.Cos(ang));
            resXYWR.Y += (mov * Math.Sin(ang));

            // 左車輪
            mov = WheelRotateToLength(reWL - reOldWL);

            resXYWL.X += (mov * -Math.Cos(ang));
            resXYWL.Y += (mov * Math.Sin(ang));

            resAng = deg * Math.PI / 180.0;
        }
        /// <summary>
        /// 左右のホイールの移動量を、X,Y座標に変換
        /// </summary>
        /// <param name="xyWR">右車輪のX,Y座標</param>
        /// <param name="xyWL">左車輪のX,Y座標</param>
        /// <param name="reWR">右車輪のREncoderの値 絶対値</param>
        /// <param name="reWL">左車輪のREncoderの値 絶対値</param>
        public static void CalcWheelPlotXY(out PointD[] xyWR, out PointD[] xyWL, double[] reWR, double[] reWL )
        {
            xyWR = new PointD[reWR.Length];
            xyWL = new PointD[reWR.Length];

            // 初期位置は上向き
            // 動力のある左の車輪が0,0基準
            xyWR[0] = new PointD();
            xyWL[0] = new PointD();

            xyWR[0].X = 500; xyWR[0].Y = 0;
            xyWL[0].X = 0; xyWL[0].Y = 0;

            // MakeLine
            for (int i = 1; i < reWR.Length; i++)
            {
                double mov;
                double deg = REncodeDataToDegree(reWR[i], reWL[i]) - 90.0;
                double ang = deg * Math.PI / 180.0;

                xyWR[i] = new PointD();
                xyWL[i] = new PointD();

                // 右車輪
                mov = WheelRotateToLength(reWR[i] - reWR[i - 1]);

                xyWR[i].X = xyWR[i - 1].X + (mov * -Math.Cos(ang));
                xyWR[i].Y = xyWR[i - 1].Y + (mov * Math.Sin(ang));

                // 左車輪
                mov = WheelRotateToLength(reWL[i] - reWL[i - 1]);

                xyWL[i].X = xyWL[i - 1].X + (mov * -Math.Cos(ang));
                xyWL[i].Y = xyWL[i - 1].Y + (mov * Math.Sin(ang));
            }

            //x11 = x1 * Math.cos(alpha) - y1 * Math.sin(alpha);
            //y11 = x1 * Math.sin(alpha) + y1 * Math.cos(alpha);
        }
        /// <summary>
        /// Limited Slip Plot
        /// </summary>
        /// <param name="xyWR"></param>
        /// <param name="xyWL"></param>
        /// <param name="OreWR"></param>
        /// <param name="OreWL"></param>
        /// <param name="lspLv"></param>
        /// <param name="lsp"></param>
        public static void CalcWheelPlotXY_LSP(out PointD[] xyWR, out PointD[] xyWL, double[] OreWR, double[] OreWL, int lspLv, double lsp)
        {
            xyWR = new PointD[OreWR.Length];
            xyWL = new PointD[OreWR.Length];

            // 初期位置は上向き
            // 動力のある左の車輪が0,0基準
            xyWR[0] = new PointD();
            xyWL[0] = new PointD();

            xyWR[0].X = 500; xyWR[0].Y = 0;
            xyWL[0].X = 0; xyWL[0].Y = 0;

            double[] reWR = new double[OreWR.Length];
            double[] reWL = new double[OreWR.Length];

            // Copy Work
            {
                for (int i = 0; i < reWR.Length; i++)
                {
                    reWR[i] = OreWR[i];
                    reWL[i] = OreWL[i];
                }
            }

            // Limited Slip Pulse
            {
                int lv = lspLv;    // Lv10 , lsp 0.52

                for (int i = lv; i < reWR.Length; i++)
                {
                    if ((reWR[i] - reWR[i - lv]) / (reWL[i] - reWL[i - lv]) < lsp)
                    {
                        double def = (reWR[i] - reWR[i - lv]);
                        double odef = ((reWL[i] - reWL[i - lv]) - def) / (double)lv;
                        /*
                        for (int n = 1; n < lv; n++)
                        {
                            reWL[i - lv + n] = reWL[i - lv] + (def / (double)lv);
                        }
                        */

                        for (int n = (i - lv); n < reWR.Length; n++)
                        {
                            reWL[n] -= odef;
                        }
                    }
                }
            }

            // MakeLine
            for (int i = 1; i < reWR.Length; i++)
            {
                double mov;
                double deg = REncodeDataToDegree(reWR[i], reWL[i]) - 90.0;
                double ang = deg * Math.PI / 180.0;

                xyWR[i] = new PointD();
                xyWL[i] = new PointD();

                // 右車輪
                mov = WheelRotateToLength(reWR[i] - reWR[i - 1]);

                xyWR[i].X = xyWR[i - 1].X + (mov * -Math.Cos(ang));
                xyWR[i].Y = xyWR[i - 1].Y + (mov * Math.Sin(ang));

                // 左車輪
                mov = WheelRotateToLength(reWL[i] - reWL[i - 1]);

                xyWL[i].X = xyWL[i - 1].X + (mov * -Math.Cos(ang));
                xyWL[i].Y = xyWL[i - 1].Y + (mov * Math.Sin(ang));
            }

            //x11 = x1 * Math.cos(alpha) - y1 * Math.sin(alpha);
            //y11 = x1 * Math.sin(alpha) + y1 * Math.cos(alpha);
        }