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