public static QuadraticPolynomial operator /(QuadraticPolynomial a, double b)
{
var res = new QuadraticPolynomial();
for (var i = 0; i < 3; i++)
{
for (var j = 0; j < 3; j++)
{
res._coeffs[i, j] = a._coeffs[i, j] / b;
}
}
return(res);
}
/// <summary> Generate (number lanes + 1) piecewise polynoms surrounding the lanes. They will be used for the 3D mesh' vertices placing </summary>
/// <param name="anchorPoints">double[numberOfPoints,3] where 3 are the dimensions(x,y,z)</param>
/// <param name="laneWidths">double[numberOfLanes] width of each lane</param>
public static QuadraticPolynomial[,] GenerateDividerPolynoms(double[,] anchorPoints, double[] laneWidths)
{
double[] dVectorUp =
{
0,
1,
0
};
var polynomials = new QuadraticPolynomial[laneWidths.Length + 1, anchorPoints.Length - 1];
var centralPPolynom = PathPPolynom(anchorPoints);
// Compute Cross Vectors
var centralCrossValues = new double[anchorPoints.Length][];
centralCrossValues[0] = centralPPolynom[0].CalculateFirstDerivative(0).Cross(dVectorUp);
centralCrossValues[0][1] = 0;
centralCrossValues[0] = centralCrossValues[0].Normalize();
for (var i = 0; i < centralPPolynom.Length; i++)
{
centralCrossValues[i + 1] = centralPPolynom[i].CalculateFirstDerivative(1).Cross(dVectorUp);
centralCrossValues[i + 1][1] = 0;
centralCrossValues[i + 1] = centralCrossValues[i + 1].Normalize();
}
// Compute Cross Divider Distances
double totalRoadWidth = 0;
foreach (var lw in laneWidths)
{
totalRoadWidth += lw;
}
var dividerDistances = new double[laneWidths.Length + 1];
dividerDistances[0] = totalRoadWidth / 2;
for (var i = 1; i < dividerDistances.Length; i++)
{
dividerDistances[i] = dividerDistances[i - 1] - laneWidths[i - 1];
}
// Compute Divider-wise Points
var dividerPoints = new double[laneWidths.Length + 1, anchorPoints.Length, 3];
for (var i = 0; i <= laneWidths.Length; i++)
{
for (var j = 0; j < anchorPoints.Length; j++)
{
for (int k = 0; k <= 2; k++)
{
dividerPoints[i, j, k] = anchorPoints[j, k] + centralCrossValues[j][k] * dividerDistances[i];
}
}
}
// Compute Divider polynoms - We cannot path the divider polys the same way we did with the central one (see online docs). We use relative point distance to scale the central polynoms
for (var i = 0; i <= laneWidths.Length; i++)
{
for (var j = 0; j < anchorPoints.Length - 1; j++)
{
var dist = new[]
{
anchorPoints[j + 1, 0] - anchorPoints[j, 0],
anchorPoints[j + 1, 1] - anchorPoints[j, 1],
anchorPoints[j + 1, 2] - anchorPoints[j, 2]
};
var ddist = new[]
{
dividerPoints[i, j + 1, 0] - dividerPoints[i, j, 0],
dividerPoints[i, j + 1, 1] - dividerPoints[i, j, 1],
dividerPoints[i, j + 1, 2] - dividerPoints[i, j, 2]
};
var ponderation = new[]
{
Math.Abs(dist[0]) < 0.001 ? 0 : ddist[0] / dist[0],
Math.Abs(dist[1]) < 0.001 ? 0 : ddist[1] / dist[1],
Math.Abs(dist[2]) < 0.001 ? 0 : ddist[2] / dist[2]
};
if (j == 0)
{
polynomials[i, j] = new QuadraticPolynomial
{
_coeffs =
{
