private void CalculateTimes(double s, int trajectoryInstance, out double tj, out double ta, out double tv)
{
switch (trajectoryInstance)
{
case 1:
case 3:
tj = Math.Sqrt(VelocityMax / JerkMax);
ta = tj;
tv = s / VelocityMax;
return;
case 2:
case 4:
tj = MathematicTools.GetCubeRoot(s / (2 * JerkMax));
ta = tj;
tv = 2 * tj;
return;
case 5:
tj = AccelerationMax / JerkMax;
ta = VelocityMax / AccelerationMax;
tv = s / VelocityMax;
return;
case 6:
tj = AccelerationMax / JerkMax;
ta = 0.5 * (Math.Sqrt((4 * s * JerkMax * JerkMax + AccelerationMax * AccelerationMax * AccelerationMax) / (AccelerationMax * JerkMax * JerkMax)) - AccelerationMax / JerkMax);
tv = ta + tj;
return;
default:
throw new ArgumentOutOfRangeException($"TrajectoryInstance must be between 1 and 6");
}
}
public void CubeRootWorking()
{
var random = new Random((int)DateTime.Now.Ticks);
for (var i = 0; i < 10; i++)
{
var number = random.NextDouble() * 1e6;
var root = MathematicTools.GetCubeRoot(Math.Pow(number, 3));
Assert.Equal(number, root, 5);
}
}
private static double SolveCubicEquation(double A, double B, double C, double D)
{
var p = (9 * A * C - 3 * B * B) / (9 * A * A);
var q = (2 * B * B * B - 9 * A * B * C + 27 * A * A * D) / (27 * A * A * A);
var diskriminante = (27 * A * A * D * D + 4 * B * B * B * D - 18 * A * B * C * D + 4 * A * C * C * C - B * B * C * C) / (108 * A * A * A * A);
var d1Mats = double.NaN;
if (true) // p < 0
{
var h1 = Math.Sqrt(-p * 4 / 3);
var h2 = -q / 2 * Math.Sqrt(-27.0 / (p * p * p));
if (Math.Abs(h2) > 1)
{
// ERROR!
}
var h3 = 1.0 / 3 * Math.Acos(h2);
var h4 = B / (3 * A);
if (diskriminante < 0)
{
var x2 = -h1 *Math.Cos(h3 + Math.PI / 3) - h4;
var x1 = h1 * Math.Cos(h3) - h4;
var x3 = -h1 *Math.Cos(h3 - Math.PI / 3) - h4;
if (Math.Abs(x1) < Math.Abs(x2) && Math.Abs(x1) < Math.Abs(x3))
{
d1Mats = x1;
}
else if (Math.Abs(x2) < Math.Abs(x1) && Math.Abs(x2) < Math.Abs(x3))
{
d1Mats = x2;
}
else
{
d1Mats = x3;
}
}
else if (diskriminante == 0 && p == 0)
{
d1Mats = -B / (3 * A);
}
else if (diskriminante == 0 && p != 0)
{
var x1 = (B * B * B - 4 * A * B * C + 9 * A * A * D) / (3 * A * A * C - A * B * B);
var x2 = (A * B * C - 9 * A * A * D) / (6 * A * A * C - 2 * A * B * B);
if (Math.Abs(x1) < Math.Abs(x2))
{
d1Mats = x1;
}
else
{
d1Mats = x2;
}
}
else if (diskriminante > 0)
{
var u = -(q / 2) + Math.Sqrt(diskriminante);
if (u >= 0)
{
u = MathematicTools.GetCubeRoot(u);
}
else
{
u = -MathematicTools.GetCubeRoot(-u);
}
var v = -(q / 2) - Math.Sqrt(diskriminante);
if (v >= 0)
{
v = MathematicTools.GetCubeRoot(v);
}
else
{
v = -MathematicTools.GetCubeRoot(-v);
}
d1Mats = u + v - B / (3 * A);
}
}
return(d1Mats);
}