163 lines
4.3 KiB
C++
163 lines
4.3 KiB
C++
#include "G3D/platform.h"
|
|
#include "G3D/Spline.h"
|
|
|
|
namespace G3D {
|
|
|
|
float SplineBase::getFinalInterval() const {
|
|
if (extrapolationMode != SplineExtrapolationMode::CYCLIC) {
|
|
return 0;
|
|
} else if (finalInterval <= 0) {
|
|
int N = time.size();
|
|
if (N >= 2) {
|
|
return (time[1] - time[0] + time[N - 1] - time[N - 2]) * 0.5f;
|
|
} else {
|
|
return 1.0f;
|
|
}
|
|
} else {
|
|
return finalInterval;
|
|
}
|
|
}
|
|
|
|
|
|
Matrix4 SplineBase::computeBasis() {
|
|
// The standard Catmull-Rom spline basis (e.g., Watt & Watt p108)
|
|
// is for [u^3 u^2 u^1 u^0] * B * [p[0] p[1] p[2] p[3]]^T.
|
|
// We need a basis formed for:
|
|
//
|
|
// U * C * [2*p'[1] p[1] p[2] 2*p'[2]]^T
|
|
//
|
|
// U * C * [p2-p0 p1 p2 p3-p1]^T
|
|
//
|
|
// To make this transformation, compute the differences of columns in C:
|
|
// For [p0 p1 p2 p3]
|
|
Matrix4 basis =
|
|
Matrix4( -1, 3, -3, 1,
|
|
2, -5, 4, -1,
|
|
-1, 0, 1, 0,
|
|
0, 2, 0, 0) * 0.5f;
|
|
|
|
// For [-p0 p1 p2 p3]^T
|
|
basis.setColumn(0, -basis.column(0));
|
|
|
|
// For [-p0 p1 p2 p3-p1]^T
|
|
basis.setColumn(1, basis.column(1) + basis.column(3));
|
|
|
|
// For [p2-p0 p1 p2 p3-p1]^T
|
|
basis.setColumn(2, basis.column(2) - basis.column(0));
|
|
|
|
return basis;
|
|
}
|
|
|
|
|
|
float SplineBase::duration() const {
|
|
if (time.size() == 0) {
|
|
return 0;
|
|
} else {
|
|
return time.last() - time[0] + getFinalInterval();
|
|
}
|
|
}
|
|
|
|
|
|
void SplineBase::computeIndexInBounds(float s, int& i, float& u) const {
|
|
int N = time.size();
|
|
float t0 = time[0];
|
|
float tn = time[N - 1];
|
|
|
|
i = iFloor((N - 1) * (s - t0) / (tn - t0));
|
|
|
|
// Inclusive bounds for binary search
|
|
int hi = N - 1;
|
|
int lo = 0;
|
|
|
|
while ((time[i] > s) || (time[i + 1] <= s)) {
|
|
|
|
if (time[i] > s) {
|
|
// too big
|
|
hi = i - 1;
|
|
} else if (time[i + 1] <= s) {
|
|
// too small
|
|
lo = i + 1;
|
|
}
|
|
|
|
i = (hi + lo) / 2;
|
|
}
|
|
|
|
// Having exited the above loop, i must be correct, so compute u.
|
|
u = (s - time[i]) / (time[i + 1] - time[i]);
|
|
}
|
|
|
|
|
|
void SplineBase::computeIndex(float s, int& i, float& u) const {
|
|
int N = time.size();
|
|
debugAssertM(N > 0, "No control points");
|
|
float t0 = time[0];
|
|
float tn = time[N - 1];
|
|
|
|
if (N < 2) {
|
|
// No control points to work with
|
|
i = 0;
|
|
u = 0.0;
|
|
} else if (extrapolationMode == SplineExtrapolationMode::CYCLIC) {
|
|
float fi = getFinalInterval();
|
|
|
|
// Cyclic spline
|
|
if ((s < t0) || (s >= tn + fi)) {
|
|
// Cyclic, off the bottom or top
|
|
|
|
// Compute offset and reduce to the in-bounds case
|
|
|
|
float d = duration();
|
|
// Number of times we wrapped around the cyclic array
|
|
int wraps = iFloor((s - t0) / d);
|
|
|
|
debugAssert(s - d * wraps >= t0);
|
|
debugAssert(s - d * wraps < tn + getFinalInterval());
|
|
|
|
computeIndex(s - d * wraps, i, u);
|
|
i += wraps * N;
|
|
|
|
} else if (s >= tn) {
|
|
debugAssert(s < tn + fi);
|
|
// Cyclic, off the top but before the end of the last interval
|
|
i = N - 1;
|
|
u = (s - tn) / fi;
|
|
|
|
} else {
|
|
// Cyclic, in bounds
|
|
computeIndexInBounds(s, i, u);
|
|
}
|
|
|
|
} else {
|
|
// Non-cyclic
|
|
|
|
if (s < t0) {
|
|
// Non-cyclic, off the bottom. Assume points are spaced
|
|
// following the first time interval.
|
|
|
|
float dt = time[1] - t0;
|
|
float x = (s - t0) / dt;
|
|
i = iFloor(x);
|
|
u = x - i;
|
|
|
|
} else if (s >= tn) {
|
|
// Non-cyclic, off the top. Assume points are spaced following
|
|
// the last time interval.
|
|
|
|
float dt = tn - time[N - 2];
|
|
float x = N - 1 + (s - tn) / dt;
|
|
i = iFloor(x);
|
|
u = x - i;
|
|
|
|
} else {
|
|
// In bounds, non-cyclic. Assume a regular
|
|
// distribution (which gives O(1) for uniform spacing)
|
|
// and then binary search to handle the general case
|
|
// efficiently.
|
|
computeIndexInBounds(s, i, u);
|
|
|
|
} // if in bounds
|
|
} // extrapolation Mode
|
|
}
|
|
|
|
}
|