81 lines
3.6 KiB
C#
81 lines
3.6 KiB
C#
using RobotApp.Common.Shares.Model;
|
|
|
|
namespace RobotApp.Common.Shares;
|
|
|
|
public static class MathExtensions
|
|
{
|
|
public static (double x, double y) CurveDegreeTwo(double t, double x1, double y1, double controlPointX, double controlPointY, double x2, double y2)
|
|
{
|
|
var x = (1 - t) * (1 - t) * x1 + 2 * t * (1 - t) * controlPointX + t * t * x2;
|
|
var y = (1 - t) * (1 - t) * y1 + 2 * t * (1 - t) * controlPointY + t * t * y2;
|
|
return (x, y);
|
|
}
|
|
|
|
public static (double x, double y) CurveDegreeThree(double t, double x1, double y1, double controlPoint1X, double controlPoint1Y, double controlPoint2X, double controlPoint2Y, double x2, double y2)
|
|
{
|
|
var x = Math.Pow(1 - t, 3) * x1 + 3 * Math.Pow(1 - t, 2) * t * controlPoint1X + 3 * Math.Pow(t, 2) * (1 - t) * controlPoint2X + Math.Pow(t, 3) * x2; ;
|
|
var y = Math.Pow(1 - t, 3) * y1 + 3 * Math.Pow(1 - t, 2) * t * controlPoint1Y + 3 * Math.Pow(t, 2) * (1 - t) * controlPoint2Y + Math.Pow(t, 3) * y2;
|
|
return (x, y);
|
|
}
|
|
|
|
|
|
public static (double x, double y) Curve(double t, EdgeCalculatorModel edge)
|
|
{
|
|
if (edge.TrajectoryDegree == Enums.TrajectoryDegree.One)
|
|
{
|
|
return (edge.X1 + t * (edge.X2 - edge.X1), edge.Y1 + t * (edge.Y2 - edge.Y1));
|
|
}
|
|
else if (edge.TrajectoryDegree == Enums.TrajectoryDegree.Two)
|
|
{
|
|
return CurveDegreeTwo(t, edge.X1, edge.Y1, edge.ControlPoint1X, edge.ControlPoint1Y, edge.X2, edge.Y2);
|
|
}
|
|
else
|
|
{
|
|
return CurveDegreeThree(t, edge.X1, edge.Y1, edge.ControlPoint1X, edge.ControlPoint1Y, edge.ControlPoint2X, edge.ControlPoint2Y, edge.X2, edge.Y2);
|
|
}
|
|
}
|
|
|
|
public static (double x, double y) Curve(this EdgeCalculatorModel edge, double t)
|
|
{
|
|
return Curve(t, edge);
|
|
}
|
|
|
|
public static double GetEdgeLength(this EdgeCalculatorModel edge)
|
|
{
|
|
double distance = 0;
|
|
if (edge.TrajectoryDegree == Enums.TrajectoryDegree.One)
|
|
{
|
|
distance = Math.Sqrt(Math.Pow(edge.X1 - edge.X2, 2) + Math.Pow(edge.Y1 - edge.Y2, 2));
|
|
}
|
|
else if (edge.TrajectoryDegree == Enums.TrajectoryDegree.Two)
|
|
{
|
|
var length = Math.Sqrt(Math.Pow(edge.X1 - edge.X2, 2) + Math.Pow(edge.Y1 - edge.Y2, 2));
|
|
if (length == 0) return 0;
|
|
double step = 0.1 / length;
|
|
|
|
for (double t = step; t <= 1.001; t += step)
|
|
{
|
|
(double x1, double y1) = CurveDegreeTwo(t - step, edge.X1, edge.Y1, edge.ControlPoint1X, edge.ControlPoint1Y, edge.X2, edge.Y2);
|
|
(double x2, double y2) = CurveDegreeTwo(t, edge.X1, edge.Y1, edge.ControlPoint1X, edge.ControlPoint1Y, edge.X2, edge.Y2);
|
|
distance += Math.Sqrt(Math.Pow(x1 - x2, 2) + Math.Pow(y1 - y2, 2));
|
|
}
|
|
}
|
|
else
|
|
{
|
|
var length = Math.Sqrt(Math.Pow(edge.X1 - edge.X2, 2) + Math.Pow(edge.Y1 - edge.Y2, 2));
|
|
if (length == 0) return 0;
|
|
double step = 0.1 / length;
|
|
for (double t = step; t <= 1.001; t += step)
|
|
{
|
|
var sTime = t - step;
|
|
(var sx, var sy) = CurveDegreeThree(1 - sTime, edge.X1, edge.Y1, edge.ControlPoint1X, edge.ControlPoint1Y, edge.ControlPoint2X, edge.ControlPoint2Y, edge.X2, edge.Y2);
|
|
sTime = t;
|
|
(var ex, var ey) = CurveDegreeThree(1 - sTime, edge.X1, edge.Y1, edge.ControlPoint1X, edge.ControlPoint1Y, edge.ControlPoint2X, edge.ControlPoint2Y, edge.X2, edge.Y2);
|
|
|
|
distance += Math.Sqrt(Math.Pow(sx - ex, 2) + Math.Pow(sy - ey, 2));
|
|
}
|
|
}
|
|
return distance;
|
|
}
|
|
}
|