public Frame2D Diff(Frame2D previos)
{
var x = previos.X - X;
var y = previos.Y - Y;
var angle = Angle.FromGrad(previos.Angle.Radian - Angle.Radian);
return new Frame2D(x,y,angle);
}
public Frame2D Apply(Frame2D arg)
{
var cos = Geometry.Cos(Angle);
var sin = Geometry.Sin(Angle);
return(new Frame2D(X + arg.X * cos - arg.Y * sin, Y + arg.X * sin + arg.Y * cos, Angle + arg.Angle));
}
public bool IsBetween(Frame2D previousLocation, Frame2D currentLocation)
{
var maxDist = previousLocation.Hypot(currentLocation);
if (this.Hypot(previousLocation) <= maxDist && this.Hypot(previousLocation) < maxDist)
return true;
return false;
}
public Frame2D Diff(Frame2D previos)
{
var x = previos.X - X;
var y = previos.Y - Y;
var angle = Angle.FromGrad(previos.Angle.Radian - Angle.Radian);
return(new Frame2D(x, y, angle));
}
public bool Equals(Frame2D other)
{
const double epsilon = 0.00001;
//Если по модулю Frame2D находятся в окрестности epsilon то они равны
return(Math.Abs(X - other.X) < epsilon && Math.Abs(Y - other.Y) < epsilon &&
Math.Abs(Angle.Radian - other.Angle.Radian) < epsilon);
}
public bool IsBetween(Frame2D previousLocation, Frame2D currentLocation)
{
var maxDist = previousLocation.Hypot(currentLocation);
if (this.Hypot(previousLocation) <= maxDist && this.Hypot(previousLocation) < maxDist)
{
return(true);
}
return(false);
}
public static double GetTriangleSquareByGeron(Frame2D A, Frame2D B, Frame2D C)
{
double a = Point2D.GetDistance(A.ToPoint2D(), B.ToPoint2D());
double b = Point2D.GetDistance(B.ToPoint2D(), C.ToPoint2D());
double c = Point2D.GetDistance(C.ToPoint2D(), A.ToPoint2D());
double p = (a + b + c)/2;
double res = p;
res *= (p - a);
res *= (p - b);
res *= (p - c);
double result = Math.Sqrt(res);
if (double.IsNaN(result))
return 0;
return result;
}
public static double GetTriangleSquareByGeron(Frame2D A, Frame2D B, Frame2D C)
{
double a = Point2D.GetDistance(A.ToPoint2D(), B.ToPoint2D());
double b = Point2D.GetDistance(B.ToPoint2D(), C.ToPoint2D());
double c = Point2D.GetDistance(C.ToPoint2D(), A.ToPoint2D());
double p = (a + b + c) / 2;
double res = p;
res *= (p - a);
res *= (p - b);
res *= (p - c);
double result = Math.Sqrt(res);
if (double.IsNaN(result))
{
return(0);
}
return(result);
}
public void Stop()
{
foreach (var id in requested.Keys.ToArray())
requested[id] = new Frame2D();
}
public void SetSpeed(string id, Frame3D speed)
{
requested[id] = new Frame2D(speed.X, speed.Y, -speed.Yaw);
}
public static double Hypot(this Frame2D current, Frame2D another)
{
return Hypot(current.X - another.X, current.Y - another.Y);
}
public static double Hypot(Frame2D frame)
{
return Hypot(frame.X, frame.Y);
}
public Frame2D Apply(Frame2D arg)
{
var cos = Geometry.Cos(Angle);
var sin = Geometry.Sin(Angle);
return new Frame2D(X + arg.X*cos - arg.Y*sin, Y + arg.X*sin + arg.Y*cos, Angle + arg.Angle);
}
public bool Equals(Frame2D other)
{
const double epsilon = 0.00001;
//Если по модулю Frame2D находятся в окрестности epsilon то они равны
return Math.Abs(X - other.X) < epsilon && Math.Abs(Y - other.Y) < epsilon &&
Math.Abs(Angle.Radian - other.Angle.Radian) < epsilon;
}
public static double Hypot(this Frame2D current, Frame2D another)
{
return(Hypot(current.X - another.X, current.Y - another.Y));
}
public static double Hypot(Frame2D frame)
{
return(Hypot(frame.X, frame.Y));
}