Skip to content

Instantly share code, notes, and snippets.

@zeux
Created June 27, 2015 08:34
Show Gist options
  • Save zeux/bedeac66c22b802047b7 to your computer and use it in GitHub Desktop.
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.
#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