Вычисление кривизны пути

void UpdateCurveCalculations()
{
    WorldCoordinate rightPosition = null;
    Orientation rightOrient = null;
    float rightCurvature;
    if (Count > 0) {
        rightPosition = VD[0].Veh.GetMapObjectPosition();
        rightOrient = VD[0].Veh.GetMapObjectOrientation();
    }

    int j;
    for (j = 0; j < Count; ++j) {
        WorldCoordinate currentPosition = rightPosition;
        Orientation currentOrient = rightOrient;
        rightPosition = null;
        rightOrient = null;
        float leftCurvature = rightCurvature;
        float curvature;
        if (j < Count - 1) {
            rightPosition = VD[j + 1].Veh.GetMapObjectPosition();
            rightOrient = VD[j + 1].Veh.GetMapObjectOrientation();
            rightCurvature = GetCurvature(currentPosition, currentOrient, rightPosition, rightOrient);
            if (j > 0)
                curvature = 0.5f * (leftCurvature + rightCurvature);
            else
                curvature = rightCurvature;
        }
        else {
            curvature = leftCurvature;
            rightCurvature = 0.0f;
        }
        VD[j].Curvature = curvature;
    }
}


// Experimental stuff

define float Pi = 3.1415927;
define float PiOverTwo = 3.1415927 / 2;
define float PiSquared = 3.1415927 * 3.1415927;

define float sinB = 4.0 / Pi;
define float sinC = -4.0 / PiSquared;
define float sinQ = 0.775;
define float sinP = 0.225;
define float sinApproxRangeStart = 0.186842089;

final float Sin2(float x)
{
    if (x < sinApproxRangeStart)
        return x;
    float y = sinB * x + sinC * x * Math.Fabs(x);
    y = sinP * (y * Math.Fabs(y) - y) + y;
    return y;
}

final float Sin(float x)
{
    int periods = (int) (x / Pi);
    float sign = 1.0f;
    if (x < 0 != (periods % 2 != 0))
        sign = -1.0f;
    float xNorm = x - Pi * periods;
    if (xNorm < 0.0)
        xNorm = -xNorm;
    if (0.0 <= xNorm and xNorm <= PiOverTwo)
        return sign * Sin2(xNorm);
    return sign * Sin2(Pi - xNorm);
}

public float Cos(float x)
{
    int periods = (int) (x / Pi);
    float sign = 1.0f;
    if (periods % 2 != 0)
        sign = -1.0f;
    float xNorm = x - Pi * periods;
    if (xNorm < 0.0)
        xNorm = -xNorm;
    if (0.0 <= xNorm and xNorm <= PiOverTwo)
        return sign * Sin2(PiOverTwo - xNorm);
    return sign * -Sin2(xNorm - PiOverTwo);
}

// This mathematical menace was created by TRam_
float GetCurvature(WorldCoordinate p1, Orientation o1, WorldCoordinate p2, Orientation o2)
{
    float baseDeltaX = (p2.baseboardX - p1.baseboardX) * 720.0f;
    float baseDeltaY = (p2.baseboardY - p1.baseboardY) * 720.0f;
    float dx = baseDeltaX + p2.x - p1.x;
    float dy = baseDeltaY + p2.y - p1.y;

    float baseRot = o1.rz;
    float baseSin = Sin(baseRot);
    float baseCos = Cos(baseRot);

    float baseX = dx * baseCos + dy * baseSin;
    float baseY = -dx * baseSin + dy * baseCos;

    return Math.Fabs(2.0f * baseX / (baseX * baseX + baseY * baseY));
}