Created
June 27, 2015 08:34
-
-
Save zeux/bedeac66c22b802047b7 to your computer and use it in GitHub Desktop.
A fast version of nlerp that is very close to slerp, with some analysis/derivation code.
This file contains hidden or bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
#include <math.h> | |
#include <stdio.h> | |
struct Q { float x, y, z, w; }; | |
float dot(Q l, Q r) | |
{ | |
return l.x * r.x + l.y * r.y + l.z * r.z + l.w * r.w; | |
} | |
Q unit(Q q) | |
{ | |
float rs = 1 / sqrtf(dot(q, q)); | |
return { q.x * rs, q.y * rs, q.z * rs, q.w * rs }; | |
} | |
Q lerp(Q l, Q r, float lt, float rt) | |
{ | |
return { l.x * lt + r.x * rt, l.y * lt + r.y * rt, l.z * lt + r.z * rt, l.w * lt + r.w * rt }; | |
} | |
Q lerp(Q l, Q r, float t) | |
{ | |
return lerp(l, r, 1 - t, t); | |
} | |
Q nlerp(Q l, Q r, float t) | |
{ | |
float lt = 1 - t; | |
float rt = dot(l, r) > 0 ? t : -t; | |
return unit(lerp(l, r, lt, rt)); | |
} | |
Q slerp(Q l, Q r, float t) | |
{ | |
float ca = dot(l, r); | |
float ls = (ca > 0) ? +1 : -1; | |
if (fabsf(ca) < 0.999f) | |
{ | |
float a = acosf(ca); | |
float rsa = 1 / sinf(a); | |
return lerp(l, r, sinf((1 - t) * a) * rsa, sinf(t * a) * rsa * ls); | |
} | |
else | |
{ | |
return lerp(l, r, 1 - t, ls * t); | |
} | |
} | |
// http://number-none.com/product/Hacking%20Quaternions/ | |
Q nnlerp(Q l, Q r, float t) | |
{ | |
float ca = dot(l, r); | |
float f = 1 - 0.7878088f * fabsf(ca); | |
float k = 0.5069269f * f * f; | |
float ot = t * (t * (2 * t - 3) * k + k + 1); | |
float lt = 1 - ot; | |
float rt = ca > 0 ? ot : -ot; | |
return unit(lerp(l, r, lt, rt)); | |
} | |
// a better approximation for the spline of the same shape as above | |
Q fnlerp(Q l, Q r, float t) | |
{ | |
float ca = dot(l, r); | |
float d = fabsf(ca); | |
float k = d * (d * 0.167127f - 0.631176f) + 0.461218f; | |
float ot = t * (t * (2 * t - 3) * k + k + 1); | |
float lt = 1 - ot; | |
float rt = ca > 0 ? ot : -ot; | |
return unit(lerp(l, r, lt, rt)); | |
} | |
Q axisangle(float x, float y, float z, float a) | |
{ | |
float sa = sinf(a / 2); | |
float ca = cosf(a / 2); | |
return { x * sa, y * sa, z * sa, ca }; | |
} | |
#if 1 | |
int main() | |
{ | |
Q l = axisangle(1, 0, 0, 0.1f); | |
Q r = axisangle(1, 0, 0, 2.5f); | |
Q m = fnlerp(l, r, 2.f / 3); | |
printf("%f\n", acosf(m.w) * 2); | |
} | |
#else | |
// given d = dot(Q0, Q1) and t, we need to find the optimal ot such that nlerp is closest to slerp | |
// without a loss of generality we assume that Q0 is identity and Q1 is a rotation by `angle' radian around X axis | |
// ca = cos(angle / 2), sa = sin(angle / 2), Q0 = (0, 0, 0, 1), Q1 = (sa, 0, 0, ca) | |
// (so d = ca; sa is positive => sa = sqrt(1 - d^2)) | |
// for a given d and t, the resulting .w is cos(acos(d) * t) | |
// nlerp(Q0, Q1, ot).w = ((1 - ot) + d * ot) / sqrt((1 - d^2) * ot^2 + ((1 - ot) + d * ot)^2) | |
double findot(double d, double t) | |
{ | |
double reqw = cos(acos(d) * t); | |
double minot = 0; | |
double maxot = 1; | |
while ((maxot - minot) > 1e-9) | |
{ | |
double ot = (minot + maxot) / 2; | |
double uw = (1 - ot) + d * ot; | |
double w = uw / sqrt((1 - d * d) * ot * ot + uw * uw); | |
if (w > reqw) | |
minot = ot; | |
else | |
maxot = ot; | |
} | |
return minot; | |
} | |
// least-squares regression for quadratic curve fitting | |
void lsqfit(const double* x, const double* y, size_t size, double& a, double& b, double& c) | |
{ | |
double s40 = 0, s30 = 0, s20 = 0, s10 = 0, s00 = 0, s21 = 0, s11 = 0, s01 = 0; | |
for (size_t i = 0; i < size; ++i) | |
{ | |
s40 += x[i] * x[i] * x[i] * x[i]; | |
s30 += x[i] * x[i] * x[i]; | |
s20 += x[i] * x[i]; | |
s10 += x[i]; | |
s00 += 1; | |
s21 += x[i] * x[i] * y[i]; | |
s11 += x[i] * y[i]; | |
s01 += y[i]; | |
} | |
double d = | |
s40*(s20 * s00 - s10 * s10) - | |
s30*(s30 * s00 - s10 * s20) + | |
s20*(s30 * s10 - s20 * s20); | |
a = | |
(s21*(s20 * s00 - s10 * s10) - | |
s11*(s30 * s00 - s10 * s20) + | |
s01*(s30 * s10 - s20 * s20)) / d; | |
b = | |
(s40*(s11 * s00 - s01 * s10) - | |
s30*(s21 * s00 - s01 * s20) + | |
s20*(s21 * s10 - s11 * s20)) / d; | |
c = | |
(s40*(s20 * s01 - s10 * s11) - | |
s30*(s30 * s01 - s10 * s21) + | |
s20*(s30 * s11 - s20 * s21)) / d; | |
} | |
int main() | |
{ | |
#if 0 | |
// 2d grid | |
for (double d = 0; d <= 1; d += 1e-2) | |
for (double t = 0; t <= 1; t += 1e-2) | |
{ | |
double ot = findot(d, t); | |
if (d > 0 && t > 0) | |
printf("%f %f %f\n", d, t, ot / t); | |
} | |
#endif | |
#if 0 | |
// 1-level LSQ | |
double d = 0.362358; | |
double X[2000], Y[2000]; | |
size_t i = 0; | |
for (double t = 0; t <= 1; t += 1e-3) | |
{ | |
double ot = findot(d, t); | |
if (t > 0) | |
{ | |
X[i] = t; | |
Y[i] = ot / t; | |
i++; | |
} | |
} | |
double a, b, c; | |
lsqfit(X, Y, i, a, b, c); | |
printf("%f %f %f\n", a, b, c); | |
printf("%f\n", findot(d, 0.334)); | |
#endif | |
#if 0 | |
// 2-level LSQ | |
size_t i = 0; | |
double A[2000], B[2000], C[2000], D[2000]; | |
for (double d = 0; d <= 1; d += 1e-3) | |
{ | |
double X[2000], Y[2000]; | |
size_t j = 0; | |
for (double t = 0; t <= 1; t += 1e-3) | |
{ | |
double ot = findot(d, t); | |
if (t > 0) | |
{ | |
X[j] = t; | |
Y[j] = ot / t; | |
j++; | |
} | |
} | |
lsqfit(X, Y, j, A[i], B[i], C[i]); | |
D[i] = d; | |
i++; | |
} | |
double aa, ab, ac; | |
lsqfit(D, A, i, aa, ab, ac); | |
double ba, bb, bc; | |
lsqfit(D, B, i, ba, bb, bc); | |
double ca, cb, cc; | |
lsqfit(D, C, i, ca, cb, cc); | |
printf("a: %f %f %f\n", aa, ab, ac); | |
printf("b: %f %f %f\n", ba, bb, bc); | |
printf("c: %f %f %f\n", ca, cb, cc); | |
#endif | |
#if 1 | |
// solve coeffs of a spline directly | |
// assuming that the spline is in this form: | |
// ot = t * (t * (2 * t - 3) * k + k + 1); | |
// and we just need to find k = k(d), we get: | |
static double D[2*1000*1000]; | |
static double K[2*1000*1000]; | |
size_t i = 0; | |
for (double d = 0; d <= 1; d += 1e-3) | |
{ | |
for (double t = 0; t <= 1; t += 1e-3) | |
{ | |
double ot = findot(d, t); | |
if (t > 0) | |
{ | |
// given ot, let's find k! | |
// ot / t = (t * (2 * t - 3) + 1) * k + 1 | |
double k = fabs(ot - t) < 1e-3 ? 0 : (ot / t - 1) / (t * (2 * t - 3) + 1); | |
D[i] = d; | |
K[i] = k; | |
i++; | |
} | |
} | |
} | |
double a, b, c; | |
lsqfit(D, K, i, a, b, c); | |
printf("%f %f %f\n", a, b, c); | |
#endif | |
} | |
#endif |
Sign up for free
to join this conversation on GitHub.
Already have an account?
Sign in to comment