#include <limits>
#include <memory>
#include "CatmullRomSpline.h"
using namespace boost;
using namespace std;
CatmullRomSpline::CatmullRomSpline(unsigned long inStride, unsigned long inOrder, float* inPointsP) throw() :
mOrder(inOrder)
{
try
{
mPointsP = shared_array<Vertex>(new Vertex[mOrder]);
// Copy in the points to our internal array
Vertex* theNextPointP = mPointsP.get();
const Vertex* theEndPointP = theNextPointP + mOrder;
inStride -= 3; // We'll be incrementing for 3 of the stride value
// so our stride can be decremented here
while (theNextPointP < theEndPointP)
{
Vertex& thePoint = *theNextPointP++;
thePoint.x = *inPointsP++;
thePoint.y = *inPointsP++;
thePoint.z = *inPointsP++;
inPointsP += inStride;
}
}
catch (const bad_alloc&)
{}
}
void CatmullRomSpline::Eval(float f, float* outVertexP) const throw()
{
// Determine which control point we're actually talking about
const float t = f * mOrder;
// Get the index of the mPointsP point corresponding
// to the global time parameter
if (t <= 0.0) // Boundary condition
{
::memmove(outVertexP, &(mPointsP[0].x), sizeof(Vertex));
}
else if (t >= mOrder) // Boundary condition
{
::memmove(outVertexP, &(mPointsP[mOrder - 1].x), sizeof(Vertex));
}
else // Common condition
{
// Start of local time interval.
const unsigned u0 = unsigned(t);
// Calc the local u relative to start of interval
const float u = t - u0;
// Grab the 4 mPointsP points for the interval
// replicating the endpoints as necessary
const unsigned theIndexAtMinusOne = numeric_limits<unsigned>::max();
Vertex* cpP[4];
for (unsigned i = 0; i < 4; i++)
{
unsigned index = u0 - 1 + i;
if (index == theIndexAtMinusOne) // Effectively == -1
index = 0;
else if (index >= mOrder)
index = mOrder - 1;
cpP[i] = mPointsP.get() + index;
}
const Vertex& cp0 = *cpP[0];
const Vertex& cp1 = *cpP[1];
const Vertex& cp2 = *cpP[2];
const Vertex& cp3 = *cpP[3];
// Evaluate the polynomial at u
Vertex theVertex;
theVertex.x =
0.5 * (
(2 * cp1.x) +
u * (
(-cp0.x + cp2.x) +
u * (
(2 * cp0.x - 5 * cp1.x + 4 * cp2.x - cp3.x) +
u * (-cp0.x + 3 * cp1.x - 3 * cp2.x + cp3.x)
)
)
);
theVertex.y =
0.5 * (
(2 * cp1.y) +
u * (
(-cp0.y + cp2.y) +
u * (
(2 * cp0.y - 5 * cp1.y + 4 * cp2.y - cp3.y) +
u * (-cp0.y + 3 * cp1.y - 3 * cp2.y + cp3.y)
)
)
);
theVertex.z =
0.5 * (
(2 * cp1.z) +
u * (
(-cp0.z + cp2.z) +
u * (
(2 * cp0.z - 5 * cp1.z + 4 * cp2.z - cp3.z) +
u * (-cp0.z + 3 * cp1.z - 3 * cp2.z + cp3.z)
)
)
);
::memmove(outVertexP, &(theVertex.x), sizeof(Vertex));
}
}