void
Interpolate(fixed time, GeoPoint &loc, fixed &alt, fixed &palt) const
{
if (!Ready()) {
loc = p[num].loc;
alt = p[num].alt;
palt = p[num].palt;
return;
}
const fixed u = time_fraction(time, false);
if (!positive(u)) {
loc = p[1].loc;
alt = p[1].alt;
palt = p[1].palt;
return;
}
if (u > fixed_one) {
loc = p[2].loc;
alt = p[2].alt;
palt = p[2].palt;
return;
}
/*
ps = ( c0 c1 c2 c3)
[ 0 1 0 0] 1
[ -t 0 t 0] u
[ 2t t-3 3-2t -t] u^2
[ -t 2-t t-2 t] u^3
*/
const fixed u2 = u * u;
const fixed u3 = u2 * u;
const fixed c[4]= {-t * u3 + 2 * t * u2 - t * u,
(fixed_two - t) * u3 + (t - fixed(3)) * u2 + fixed_one,
(t - fixed_two) * u3 + (fixed(3) - 2 * t) * u2 + t * u,
t * u3 - t * u2};
loc.Latitude = (p[0].loc.Latitude * c[0] + p[1].loc.Latitude * c[1]
+ p[2].loc.Latitude * c[2] + p[3].loc.Latitude * c[3]);
loc.Longitude = (p[0].loc.Longitude * c[0] + p[1].loc.Longitude * c[1]
+ p[2].loc.Longitude * c[2] + p[3].loc.Longitude * c[3]);
alt = (p[0].alt * c[0] + p[1].alt * c[1] + p[2].alt * c[2] + p[3].alt * c[3]);
palt = (p[0].palt * c[0] + p[1].palt * c[1] +
p[2].palt * c[2] + p[3].palt * c[3]);
}
gcc_pure
Record
Interpolate(fixed time) const
{
assert(Ready());
const fixed u = time_fraction(time, false);
if (!positive(u))
return p[1];
if (u > fixed_one)
return p[2];
/*
ps = ( c0 c1 c2 c3)
[ 0 1 0 0] 1
[ -t 0 t 0] u
[ 2t t-3 3-2t -t] u^2
[ -t 2-t t-2 t] u^3
*/
const fixed u2 = u * u;
const fixed u3 = u2 * u;
const fixed c[4]= {-t * u3 + 2 * t * u2 - t * u,
(fixed_two - t) * u3 + (t - fixed(3)) * u2 + fixed_one,
(t - fixed_two) * u3 + (fixed(3) - 2 * t) * u2 + t * u,
t * u3 - t * u2};
Record r;
r.loc.Latitude = p[0].loc.Latitude * c[0] + p[1].loc.Latitude * c[1]
+ p[2].loc.Latitude * c[2] + p[3].loc.Latitude * c[3];
r.loc.Longitude = p[0].loc.Longitude * c[0] + p[1].loc.Longitude * c[1]
+ p[2].loc.Longitude * c[2] + p[3].loc.Longitude * c[3];
r.alt = p[0].alt * c[0] + p[1].alt * c[1] +
p[2].alt * c[2] + p[3].alt * c[3];
r.palt = p[0].palt * c[0] + p[1].palt * c[1] +
p[2].palt * c[2] + p[3].palt * c[3];
r.t = time;
return r;
}
