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); }