//多段直线跟踪
public void FollowMulLine(ShipData boat)
{
HUST_1_Demo.Form1.TargetLine line = new Form1.TargetLine();
line.Start = HUST_1_Demo.Form1.tarMultiLine.ElementAt(HUST_1_Demo.Form1.followLineID); //线段起始点
line.End = HUST_1_Demo.Form1.tarMultiLine.ElementAt(HUST_1_Demo.Form1.followLineID + 1); //线端终点
line.LineK = (double)(line.Start.Y - line.End.Y) / (double)(line.Start.X - line.End.X);
line.LineB = (line.Start.Y - line.LineK * line.Start.X) / 1000.0f;
if (line.End.X < line.Start.X)//判断是否为逆向直线
{
line.isReverse = true;
}
FollowLine(boat, line);
if (Math.Sqrt((boat.X_mm - line.End.X) * (boat.X_mm - line.End.X) + (boat.Y_mm - line.End.Y) * (boat.Y_mm - line.End.Y)) < 4000)
{
HUST_1_Demo.Form1.followLineID++;
}
if (HUST_1_Demo.Form1.followLineID == HUST_1_Demo.Form1.tarMultiLine.Count - 1)
{
HUST_1_Demo.Form1.followLineID = 0;
}
}
//椭圆轨迹跟随
public void FollowOval(ShipData boat, HUST_1_Demo.Form1.TargetOval oval)
{
switch (HUST_1_Demo.Form1.SetOvalPathID)
{
case 0:
{
HUST_1_Demo.Form1.TargetLine line = new Form1.TargetLine();
line.Start = oval.Pt1;
line.End = oval.Pt2;
line.LineK = oval.K1;
line.LineB = oval.B1 / 1000d;
FollowLine(boat, line);
if (Math.Sqrt((boat.X_mm - oval.Pt2.X) * (boat.X_mm - oval.Pt2.X) + (boat.Y_mm - oval.Pt2.Y) * (boat.Y_mm - oval.Pt2.Y)) < 2000)
{
HUST_1_Demo.Form1.SetOvalPathID = 1;
}
break;
}
case 1:
{
HUST_1_Demo.Form1.TargetCircle circle = new Form1.TargetCircle();
circle.x = oval.OriPt1.X / 1000d;
circle.y = oval.OriPt1.Y / 1000d;
circle.Radius = oval.R / 1000d;
FollowCircle(boat, circle);
if (Math.Sqrt((boat.X_mm - oval.Pt3.X) * (boat.X_mm - oval.Pt3.X) + (boat.Y_mm - oval.Pt3.Y) * (boat.Y_mm - oval.Pt3.Y)) < 2000)
{
HUST_1_Demo.Form1.SetOvalPathID = 2;
}
break;
}
case 2:
{
HUST_1_Demo.Form1.TargetLine line = new Form1.TargetLine();
line.Start = oval.Pt3;
line.End = oval.Pt4;
line.LineK = oval.K1;
line.LineB = oval.B3 / 1000d;
line.isReverse = true;
FollowLine(boat, line);
if (Math.Sqrt((boat.X_mm - oval.Pt4.X) * (boat.X_mm - oval.Pt4.X) + (boat.Y_mm - oval.Pt4.Y) * (boat.Y_mm - oval.Pt4.Y)) < 2000)
{
HUST_1_Demo.Form1.SetOvalPathID = 3;
}
break;
}
case 3:
{
HUST_1_Demo.Form1.TargetCircle circle = new Form1.TargetCircle();
circle.x = oval.OriPt2.X / 1000d;
circle.y = oval.OriPt2.Y / 1000d;
circle.Radius = oval.R / 1000d;
FollowCircle(boat, circle);
if (Math.Sqrt((boat.X_mm - oval.Pt1.X) * (boat.X_mm - oval.Pt1.X) + (boat.Y_mm - oval.Pt1.Y) * (boat.Y_mm - oval.Pt1.Y)) < 2000)
{
HUST_1_Demo.Form1.SetOvalPathID = 0;
}
break;
}
}
}
