private Pose2D getConversions(Edge oldEdge, Edge newEdge) // Rotation , Location
{
Pose2D res = new Pose2D();
if (newEdge.type == EdgeType.EndOfWall || oldEdge.type == EdgeType.EndOfWall)
{
if (newEdge.type != oldEdge.type)
{
return(null);
}
}
if (newEdge.type == EdgeType.EndOfWall)
{
Vector2 vo, vn;
if (newEdge.point == newEdge.lines[0].head)
{
vo = oldEdge.lines[0].tail - oldEdge.lines[0].head;
vn = newEdge.lines[0].tail - newEdge.lines[0].head;
}
else // == tail
{
vo = oldEdge.lines[0].head - oldEdge.lines[0].tail;
vn = newEdge.lines[0].head - newEdge.lines[0].tail;
}
float angle = MathHelper.AngleBetweenVectors(vo, vn);
if (angle > 2 * PI || angle < -2 * PI || float.IsNaN(angle) || float.IsInfinity(angle))
{
angle = 0;
}
res.Rotation = -angle; // angle I Ke Tooye Res E Bayad Inghadr Charkhoonde Beshe
Vector2 nt = MathHelper.TranformVector(newEdge.point, Vector2.Zero, new Pose2D(RobotCURState.Position, res.GetNormalizedRotation()));
Vector2 Trans = oldEdge.point - nt;
res.Position = Trans;
}
else
{
Vector2 vo1, vo2, vn1, vn2;
float dph0 = MathHelper.GetDistance(newEdge.point, newEdge.lines[0].head), //newEdge.point.getDistance2D(newEdge.lines[0].head),
dpt0 = MathHelper.GetDistance(newEdge.point, newEdge.lines[0].tail), //newEdge.point.getDistance2D(newEdge.lines[0].tail),
dph1 = MathHelper.GetDistance(newEdge.point, newEdge.lines[1].head), //newEdge.point.getDistance2D(newEdge.lines[1].head),
dpt1 = MathHelper.GetDistance(newEdge.point, newEdge.lines[1].tail); //newEdge.point.getDistance2D(newEdge.lines[1].tail);
if (dph0 < dpt0)
{
vo1 = oldEdge.lines[0].tail - oldEdge.lines[0].head;
vn1 = newEdge.lines[0].tail - newEdge.lines[0].head;
}
else // == tail
{
vo1 = oldEdge.lines[0].head - oldEdge.lines[0].tail;
vn1 = newEdge.lines[0].head - newEdge.lines[0].tail;
}
if (dph1 < dpt1)
{
vo2 = oldEdge.lines[1].tail - oldEdge.lines[1].head;
vn2 = newEdge.lines[1].tail - newEdge.lines[1].head;
}
else // == tail
{
vo2 = oldEdge.lines[1].head - oldEdge.lines[1].tail;
vn2 = newEdge.lines[1].head - newEdge.lines[1].tail;
}
float angle1 = MathHelper.AngleBetweenVectors(vo1, vn1), angle2 = MathHelper.AngleBetweenVectors(vo2, vn2);
float angle = (angle1 + angle2) / 2f;
res.Rotation = -angle;
Vector2 nt = MathHelper.TranformVector(newEdge.point, Vector2.Zero, new Pose2D(RobotCURState.Position, res.Rotation));
Vector2 Trans = oldEdge.point - nt;
res.Position = Trans;
}
return(res);
}
