private bool isChangeValid(Pose2D Changes)
{
float[] validChanges;
validChanges = getMaxDetectedValidTransformation();
bool changesValid = (MathHelper.VectorLength(Changes.Position) < validChanges[1]) &&
(System.Math.Abs(Changes.GetNormalizedRotation()) < validChanges[0]);
return(changesValid);
}
private Pose2D compound(Pose2D t1, Pose2D t2)
{
Pose2D t_ret = new Pose2D();
t_ret.X = t2.X * System.Math.Cos(t1.Rotation) - t2.Y * System.Math.Sin(t1.Rotation) + t1.X;
t_ret.Y = t2.X * System.Math.Sin(t1.Rotation) + t2.Y * System.Math.Cos(t1.Rotation) + t1.Y;
t_ret.Rotation = t1.Rotation + t2.Rotation;
// Make angle [-pi,pi)
t_ret.Rotation = t_ret.GetNormalizedRotation();
return(t_ret);
}
private bool ExceedDelta(Pose2D dpose)
{
bool extend = false;
extend = extend || System.Math.Abs(dpose.X) > CHANGE_MAXTRANSLATION;
extend = extend || System.Math.Abs(dpose.Y) > CHANGE_MAXTRANSLATION;
double dradians = dpose.GetNormalizedRotation();
double dangle = dradians / System.Math.PI * 180;
extend = extend || System.Math.Abs(dangle) > CHANGE_MAXROTATION;
return(extend);
}
public static bool ExceedThreshold(Pose2D dpose)
{
bool extend = false;
extend = extend || System.Math.Abs(dpose.X) > MATCH_MAXTRANSLATION;
extend = extend || System.Math.Abs(dpose.Y) > MATCH_MAXTRANSLATION;
double dradians = dpose.GetNormalizedRotation();
double dangle = dradians / System.Math.PI * 180;
extend = extend || System.Math.Abs(dangle) > MATCH_MAXROTATION;
return(extend);
}
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);
}