public double DistTo(JD TowardsJD)
{
var dy = TowardsJD.Y - Y;
var dx = TowardsJD.X - X;
return(Math.Sqrt(dy * dy + dx * dx));
}
public double GetXAngleTo(JD TowardsJD)
{
var dy = TowardsJD.Y - Y;
var dx = TowardsJD.X - X;
var value = dy / dx;
if (value > 0)
{
if (dy >= 0)
{
return(Math.Atan(dy / dx));
}
else
{
return(Math.Atan(dy / dx) + Math.PI);
}
}
else
{
if (dy >= 0)
{
return(Math.Atan(dy / dx) + Math.PI);
}
else
{
return(Math.Atan(dy / dx) + Math.PI * 2);
}
}
}
double[] Vector(JD i, JD j, double factor = 1)
{
double dx = j.X - i.X;
double dy = j.Y - i.Y;
double dist = i.DistTo(j);
return(new double[] { dx / dist * factor, dy / dist * factor });
}
public PQX(string jdfile)
{
// 初始化节点
JDList = new List <JD>();
string[] altext = File.ReadAllLines(jdfile);
foreach (string line in altext)
{
if (line.StartsWith("//"))
{
continue;
}
var xx = Regex.Split(line, @"\s+");
if (xx[0] == "-1")
{
StartPoint = new JD(double.Parse(xx[1]), double.Parse(xx[2]), double.Parse(xx[3]));
StartPoint.TypeID = EnumTypeID.ST;
}
else if (xx[0] == "-2")
{
EndPoint = new JD(double.Parse(xx[1]), double.Parse(xx[2]));
EndPoint.TypeID = EnumTypeID.ED;
}
else if (xx[0] == "3")
{
JD a = new JD(line);
a.TypeID = EnumTypeID.JD3;
JDList.Add(a);
}
else
{
throw new Exception();
}
}
// 方向角
for (int i = 0; i < JDList.Count; i++)
{
double v1, v2;
if (i == 0)
{
v1 = StartPoint.GetXAngleTo(JDList[i]);
v2 = JDList[i].GetXAngleTo(JDList[i + 1]);
}
else if (i == JDList.Count - 1)
{
v1 = JDList[i - 1].GetXAngleTo(JDList[i]);
v2 = JDList[i].GetXAngleTo(EndPoint);
}
else
{
v1 = JDList[i - 1].GetXAngleTo(JDList[i]);
v2 = JDList[i].GetXAngleTo(JDList[i + 1]);
}
double aa = (v2 - v1);
JDList[i].alpha = aa;
JDList[i].Dir = aa > 0 ? LeftRightEnum.Left :LeftRightEnum.Right;
JDList[i].ak1 = ((int)JDList[i].Dir / JDList[i].R - 1 / JDList[i].R1) / JDList[i].L1;
JDList[i].ak2 = ((int)JDList[i].Dir / JDList[i].R - 1 / JDList[i].R2) / JDList[i].L2;
}
// 积木法里程
ScanTheLine();
}
