Equal Length Spline         Generating new spline which has equal distance between points

Introduction

I use this for my AI where the vehicle will follow the spline created in your favorite 3D design. However, it is not fair to expect the graphics artist to place points in the spline with exact same distance in between. Why same distance you may ask. One of the uses is in calculating path cost. Path cost in this context is expressed as a floating point number between 0.000 -> 1.000. 1 means the bending of the road path is so extreme that the car velocity should be 0 (which of course doesn't happen). 1.000 means straight line path. The cost is calculated by the change of spline's tangent - the bigger the change, the bigger the cost. It is not possible (or at least not direct) to calculate cost without having equal distance spline in the first place.

Of course, I believe this has uses in other application.

Algorithm

The algorithm is quite simple, but it has taken me sometime to stabilize the code. The algorithm has a lot of similarity with binary search which first starts with top, bottom and median. Median is always (top+bottom)/2.

Algorithm:

(a) Assign bottom = first point in spline, top = second point. 
  (b) Median = (bottom+first)/2.
  (c) Find distance (d) between top and bottom.
  (d) if (d < wantedDistance) then top=top+1. Loop back to (b).

Comment: by now, d is always > wantedDistance.

(e) median = (bottom+first)/2
  (f) Find distance (d) between bottom and median
  (g) if d < wantedDistance then top = (top+median)/2 else bottom = (bottom+median)/2
  (h) loop back to (e)
  (i) store the point (p). 
  (j) bottom = p, top = (next spline point)
  (k) loop back to (e)

Note: The algorithm above may not be very accurate, just to serve to understand the code below.

Okay here is the code:

void getEqualDistanceSpline(SimpleSpline& splineSrc, SimpleSpline* splineDest, Real wantedDistance)
{
    Real lastInterpPoint = 0.0;
    Real length;
    Vector3 start = splineSrc.getPoint(0);
    Vector3 end;
    Real wantedDistanceSquared = wantedDistance*wantedDistance;

    splineDest->setAutoCalculate(false);
    splineDest->addPoint(start);

    for (int j = 1; j < splineSrc.getNumPoints();) {

        // first find the points where the length exceed wanted length..
        end = splineSrc.getPoint(j);
        length = (end-start).squaredLength();

        while (length < wantedDistanceSquared && j < splineSrc.getNumPoints()-1) {
            end = splineSrc.getPoint(++j);
            length = (end-start).squaredLength();
            // if enter the loops then we have to reset lastInterPoint..
            lastInterpPoint = 0.0;
        }

        if (j == splineSrc.getNumPoints() -1)
            break;

        // okay found it.. lets refine
        Real partStart = lastInterpPoint;
        Real partEnd = 1.0;
        Real partMid;
        Vector3 partPoint;
        Real partLen;
        const Vector3& refPoint = splineSrc.interpolate(j-1, lastInterpPoint);
        Real squaredDist = wantedDistance-(start-refPoint).length();
        squaredDist *= squaredDist;
        
        do {
            partMid = (partStart+partEnd)/2;
            partPoint = splineSrc.interpolate(j-1, partMid);
            partLen = (partPoint-refPoint).squaredLength();
            if (fabs(partLen-squaredDist)< 1 || fabs(partStart-partEnd) < 1e-5)
                break;
            if (partLen > squaredDist)
                partEnd = partMid;
            else
                partStart = partMid;
        } while (true);

        // once we reach here.. the exact point has been discovered..
        start = splineSrc.interpolate(j-1, partMid);
        //LOG("\tstart = " + StringConverter::toString(start) + ", lastInterpPoint = " + StringConverter::toString(partMid));
        // and remember the last interpolation point
        lastInterpPoint = partMid;

        splineDest->addPoint(start);
    }
    splineDest->recalcTangents();
}