public SetCurveParams()
{
InitializeComponent();
this.TopMost = true;
curveParam = new CurveParams();
}
public void CurvePathPlan(CurveParams param)
{
pCurveLocation.startpoint = param.startpoint;
pCurveLocation.endpoint = param.endpoint;
pCurveLocation.param = param;
double unitdistance_curve = param.dv;
double unitdistance_rot = param.dw;
double pointA_X = param.endpoint.X;
double pointA_Y = param.endpoint.Y;
//double pointA_X =15;
//double pointA_Y = 15;
double th = 0;
double x_vec = 0.0;
double y_vec = 0.0;
int i = 0;
double currentX = param.startpoint.X;
double currentY = param.startpoint.Y;
//double currentX =0;
//double currentY =0;
double currentTh = param.startdir;
double x = 0.0, y = 0.0, z = 0.0, w = 0.0;
int count = 0;
while (Math.Abs(currentX - pointA_X) >= 3 || Math.Abs(currentY - pointA_Y) >= 3)
{
if (count++ > 100)
{
break;
}
//Console.WriteLine(""+ currentX+ " / " + currentY+ " / "+ currentTh + " coS(0.1) = " + Math.Cos(0.1));
x_vec = unitdistance_curve * Math.Cos(currentTh + unitdistance_rot * 0.5);
y_vec = unitdistance_curve * Math.Sin(currentTh + unitdistance_rot * 0.5);
th = Math.Atan2(y_vec, x_vec);
if (th > Math.PI)
{
th = th - 2 * Math.PI;
}
else if (th < -Math.PI)
{
th = th + 2 * Math.PI;
}
x = currentX + x_vec;
y = currentY + y_vec;
z = Math.Sin(th / 2);
w = Math.Cos(th / 2);
CoordinationXY_curveplan.Add(new Position()
{
X = x, Y = y, Z = z, W = w
});
i++;
currentX = x;
currentY = y;
currentTh = currentTh + unitdistance_rot;
}
CoordinationXY_curveplan.Add(new Position()
{
X = pointA_X, Y = pointA_Y, Z = Math.Sin(param.enddir / 2), W = Math.Cos(param.enddir / 2)
});
}