From c12bc7391e4281877c854940a162b4084578f096 Mon Sep 17 00:00:00 2001 From: Andrew Tribick Date: Thu, 26 Oct 2023 20:29:34 +0200 Subject: [PATCH] Refactor sampled orbit and orientation classes - Store sample times and rotations in separate vectors to reduce padding - Do not permit out-of-order timestamps in sample files - Do not permit empty sampled trajectories or rotations - Pre-rotate the sampled trajectory positions and velocities --- src/celephem/samporbit.cpp | 862 ++++++++++++++++++------------------ src/celephem/samporbit.h | 7 +- src/celephem/samporient.cpp | 177 ++++---- 3 files changed, 507 insertions(+), 539 deletions(-) diff --git a/src/celephem/samporbit.cpp b/src/celephem/samporbit.cpp index be4f0ea5ce4..8ce24bff961 100644 --- a/src/celephem/samporbit.cpp +++ b/src/celephem/samporbit.cpp @@ -14,9 +14,11 @@ #include #include +#include #include #include #include +#include #include #include #include @@ -43,47 +45,64 @@ namespace celestia::ephem namespace { -// Trajectories are sampled adaptively for rendering. MaxSampleInterval -// is the maximum time (in days) between samples. The threshold angle -// is the maximum angle allowed between path segments. -//static const double MinSampleInterval = 1.0 / 1440.0; // one minute -//static const double MaxSampleInterval = 50.0; -//static const double SampleThresholdAngle = 2.0; - -// Position-only sample -template struct Sample +struct InterpolationParameters { + Eigen::Vector3d p0; + Eigen::Vector3d v0; + Eigen::Vector3d p1; + Eigen::Vector3d v1; double t; - Eigen::Matrix position; + double ih; }; -// Position + velocity sample -template struct SampleXYZV + +Eigen::Vector3d +cubicInterpolate(const InterpolationParameters& params) { - double t; - Eigen::Matrix position; - Eigen::Matrix velocity; -}; + Eigen::Vector3d a = 2.0 * (params.p0 - params.p1) + params.v1 + params.v0; + Eigen::Vector3d b = 3.0 * (params.p1 - params.p0) - 2.0 * params.v0 - params.v1; + return params.p0 + params.t * (params.v0 + params.t * (b + params.t * a)); +} -template bool operator<(const Sample& a, const Sample& b) +Eigen::Vector3d +cubicInterpolateVelocity(const InterpolationParameters& params) { - return a.t < b.t; + Eigen::Vector3d a3 = 3.0 * (2.0 * (params.p0 - params.p1) + params.v1 + params.v0); + Eigen::Vector3d b2 = 2.0 * (3.0 * (params.p1 - params.p0) - 2.0 * params.v0 - params.v1); + return (params.v0 + params.t * (b2 + params.t * a3)) * params.ih; } -template bool operator<(const SampleXYZV& a, const SampleXYZV& b) + +template +bool +validateSamples(const T& orbit, const fs::path& filename) { - return a.t < b.t; + if (orbit.hasSamples()) + return true; + + GetLogger()->error(_("Sampled orbit {} contains no valid samples."), filename); + return false; } -template class SampledOrbit : public CachingOrbit +// Trajectories are sampled adaptively for rendering. MaxSampleInterval +// is the maximum time (in days) between samples. The threshold angle +// is the maximum angle allowed between path segments. +//static const double MinSampleInterval = 1.0 / 1440.0; // one minute +//static const double MaxSampleInterval = 50.0; +//static const double SampleThresholdAngle = 2.0; + + +template +class SampledOrbit : public CachingOrbit { public: SampledOrbit(TrajectoryInterpolation /*_interpolation*/); ~SampledOrbit() override = default; void addSample(double t, const Eigen::Matrix& position); + bool hasSamples() const; double getPeriod() const override; double getBoundingRadius() const override; @@ -96,21 +115,25 @@ template class SampledOrbit : public CachingOrbit void sample(double startTime, double endTime, OrbitSampleProc& proc) const override; private: - std::vector> samples; + std::vector sampleTimes; + std::vector> positions; double boundingRadius; double period; - mutable int lastSample; + mutable std::uint32_t lastSample; TrajectoryInterpolation interpolation; - Eigen::Vector3d computePositionLinear(double jd, int n) const; - Eigen::Vector3d computePositionCubic(double jd, int n) const; - Eigen::Vector3d computeVelocityLinear(double jd, int n) const; - Eigen::Vector3d computeVelocityCubic(double jd, int n) const; + Eigen::Vector3d computePositionLinear(double, std::uint32_t) const; + Eigen::Vector3d computePositionCubic(double, std::uint32_t, std::uint32_t) const; + Eigen::Vector3d computeVelocityLinear(std::uint32_t) const; + Eigen::Vector3d computeVelocityCubic(double, std::uint32_t, std::uint32_t) const; + + void initializeCubic(double, std::uint32_t, std::uint32_t, InterpolationParameters&) const; }; -template SampledOrbit::SampledOrbit(TrajectoryInterpolation _interpolation) : +template +SampledOrbit::SampledOrbit(TrajectoryInterpolation _interpolation) : boundingRadius(0.0), period(1.0), lastSample(0), @@ -119,306 +142,250 @@ template SampledOrbit::SampledOrbit(TrajectoryInterpolation _inte } -template void SampledOrbit::addSample(double t, const Eigen::Matrix& position) +template +void +SampledOrbit::addSample(double t, const Eigen::Matrix& position) { + if (sampleTimes.size() >= std::numeric_limits::max()) + return; + double r = position.template cast().norm(); if (r > boundingRadius) boundingRadius = r; - Sample& samp = samples.emplace_back(); - samp.position = position; - samp.t = t; -} + sampleTimes.push_back(t); -template double SampledOrbit::getPeriod() const -{ - return samples[samples.size() - 1].t - samples[0].t; + // Add correction for Celestia's coordinate system + positions.emplace_back(position.x(), position.z(), -position.y()); } -template bool SampledOrbit::isPeriodic() const +template +bool +SampledOrbit::hasSamples() const { - return false; + return !sampleTimes.empty(); } -template void SampledOrbit::getValidRange(double& begin, double& end) const +template +double +SampledOrbit::getPeriod() const { - begin = samples[0].t; - end = samples[samples.size() - 1].t; + return sampleTimes.back() - sampleTimes.front(); } -template double SampledOrbit::getBoundingRadius() const +template +bool +SampledOrbit::isPeriodic() const { - return boundingRadius; + return false; } -Eigen::Vector3d cubicInterpolate(const Eigen::Vector3d& p0, const Eigen::Vector3d& v0, - const Eigen::Vector3d& p1, const Eigen::Vector3d& v1, - double t) +template +void +SampledOrbit::getValidRange(double& begin, double& end) const { - return p0 + (((2.0 * (p0 - p1) + v1 + v0) * (t * t * t)) + - ((3.0 * (p1 - p0) - 2.0 * v0 - v1) * (t * t)) + - (v0 * t)); + begin = sampleTimes.front(); + end = sampleTimes.back(); } -Eigen::Vector3d cubicInterpolateVelocity(const Eigen::Vector3d& p0, const Eigen::Vector3d& v0, - const Eigen::Vector3d& p1, const Eigen::Vector3d& v1, - double t) +template +double +SampledOrbit::getBoundingRadius() const { - return ((2.0 * (p0 - p1) + v1 + v0) * (3.0 * t * t)) + - ((3.0 * (p1 - p0) - 2.0 * v0 - v1) * (2.0 * t)) + - v0; + return boundingRadius; } -template Eigen::Vector3d SampledOrbit::computePosition(double jd) const +template +Eigen::Vector3d +SampledOrbit::computePosition(double jd) const { - Eigen::Vector3d pos; - if (samples.size() == 0) - { - pos = Eigen::Vector3d::Zero(); - } - else if (samples.size() == 1) + if (sampleTimes.size() == 1) + return positions.front().template cast(); + + std::uint32_t n = lastSample; + const auto maxSample = static_cast(sampleTimes.size()); + if (n < 1 || n >= maxSample || jd < sampleTimes[n - 1] || jd > sampleTimes[n]) { - pos = samples[0].position.template cast(); + auto iter = std::lower_bound(sampleTimes.begin(), sampleTimes.end(), jd); + n = static_cast(iter - sampleTimes.begin()); + lastSample = n; } - else - { - Sample samp; - samp.t = jd; - int n = lastSample; - if (n < 1 || n >= (int) samples.size() || jd < samples[n - 1].t || jd > samples[n].t) - { - auto iter = std::lower_bound(samples.begin(), samples.end(), samp); - n = iter == samples.end() - ? samples.size() - : iter - samples.begin(); - - lastSample = n; - } + if (n == 0) + return positions.front().template cast(); + if (n == maxSample) + return positions.back().template cast(); - if (n == 0) - { - pos = samples[n].position.template cast(); - } - else if (n < (int) samples.size()) - { - switch (interpolation) - { - case TrajectoryInterpolation::Linear: - pos = computePositionLinear(jd, n); - break; - case TrajectoryInterpolation::Cubic: - pos = computePositionCubic(jd, n); - break; - default: // Unknown interpolation type - pos = Eigen::Vector3d::Zero(); - break; - } - } - else - { - pos = samples[n - 1].position.template cast(); - } + switch (interpolation) + { + case TrajectoryInterpolation::Linear: + return computePositionLinear(jd, n); + case TrajectoryInterpolation::Cubic: + return computePositionCubic(jd, n, maxSample - 1); + default: // Unknown interpolation type + assert(0); + return Eigen::Vector3d::Zero(); } - - // Add correction for Celestia's coordinate system - return Eigen::Vector3d(pos.x(), pos.z(), -pos.y()); } -template Eigen::Vector3d SampledOrbit::computePositionLinear(double jd, - int n) const +template +Eigen::Vector3d +SampledOrbit::computePositionLinear(double jd, std::uint32_t n) const { - Sample s0 = samples[n - 1]; - Sample s1 = samples[n]; + assert(n > 0); + double t = (jd - sampleTimes[n - 1]) / (sampleTimes[n] - sampleTimes[n - 1]); - double t = (jd - s0.t) / (s1.t - s0.t); - return Eigen::Vector3d(celmath::lerp(t, (double) s0.position.x(), (double) s1.position.x()), - celmath::lerp(t, (double) s0.position.y(), (double) s1.position.y()), - celmath::lerp(t, (double) s0.position.z(), (double) s1.position.z())); + const Eigen::Matrix& s0 = positions[n - 1]; + const Eigen::Matrix& s1 = positions[n]; + return Eigen::Vector3d(celmath::lerp(t, static_cast(s0.x()), static_cast(s1.x())), + celmath::lerp(t, static_cast(s0.y()), static_cast(s1.y())), + celmath::lerp(t, static_cast(s0.z()), static_cast(s1.z()))); } -template Eigen::Vector3d SampledOrbit::computePositionCubic(double jd, - int n) const +template +Eigen::Vector3d +SampledOrbit::computePositionCubic(double jd, std::uint32_t n2, std::uint32_t nMax) const { - Sample s0 = n > 1 - ? samples[n - 2] - : samples[n - 1]; - Sample s1 = samples[n - 1]; - Sample s2 = samples[n]; - Sample s3 = n < (int) samples.size() - 1 - ? samples[n + 1] - : samples[n]; - - double h = s2.t - s1.t; - double ih = 1.0 / h; - double t = (jd - s1.t) * ih; - Eigen::Vector3d p0 = s1.position.template cast(); - Eigen::Vector3d p1 = s2.position.template cast(); - - Eigen::Vector3d v10 = p0 - s0.position.template cast(); - Eigen::Vector3d v21 = p1 - p0; - Eigen::Vector3d v32 = s3.position.template cast() - p1; - - // Estimate velocities by averaging the differences at adjacent spans - // (except at the end spans, where we just use a single velocity.) - Eigen::Vector3d v0 = n > 1 - ? (v10 * (0.5 / (s1.t - s0.t)) + v21 * (0.5 * ih)) * h - : v21; - - Eigen::Vector3d v1 = n < ((int) samples.size() - 1) - ? (v21 * (0.5 * ih) + v32 * (0.5 / (s3.t - s2.t))) * h - : v21; - - return cubicInterpolate(p0, v0, p1, v1, t); + InterpolationParameters params; + initializeCubic(jd, n2, nMax, params); + return cubicInterpolate(params); } -template Eigen::Vector3d SampledOrbit::computeVelocity(double jd) const +template +Eigen::Vector3d +SampledOrbit::computeVelocity(double jd) const { - Eigen::Vector3d vel; - if (samples.size() < 2) + if (sampleTimes.size() < 2) + return Eigen::Vector3d::Zero(); + + std::uint32_t n = lastSample; + const auto maxSample = static_cast(sampleTimes.size()); + if (n < 1 || n >= maxSample || jd < sampleTimes[n - 1] || jd > sampleTimes[n]) { - vel = Eigen::Vector3d::Zero(); + auto iter = std::lower_bound(sampleTimes.begin(), sampleTimes.end(), jd); + n = static_cast(iter - sampleTimes.begin()); + lastSample = n; } - else - { - Sample samp; - samp.t = jd; - int n = lastSample; - if (n < 1 || n >= (int) samples.size() || jd < samples[n - 1].t || jd > samples[n].t) - { - auto iter = std::lower_bound(samples.begin(), samples.end(), samp); - n = iter == samples.end() - ? samples.size() - : iter - samples.begin(); - lastSample = n; - } + if (n == 0 || n == maxSample) + return Eigen::Vector3d::Zero(); - if (n == 0) - { - vel = Eigen::Vector3d::Zero(); - } - else if (n < (int) samples.size()) - { - switch (interpolation) - { - case TrajectoryInterpolation::Linear: - vel = computeVelocityLinear(jd, n); - break; - case TrajectoryInterpolation::Cubic: - vel = computeVelocityCubic(jd, n); - break; - default: // Unknown interpolation type - vel = Eigen::Vector3d::Zero(); - break; - } - } - else - { - vel = Eigen::Vector3d::Zero(); - } + switch (interpolation) + { + case TrajectoryInterpolation::Linear: + return computeVelocityLinear(n); + case TrajectoryInterpolation::Cubic: + return computeVelocityCubic(jd, n, maxSample - 1); + default: // Unknown interpolation type + assert(0); + return Eigen::Vector3d::Zero(); } - - return Eigen::Vector3d(vel.x(), vel.z(), -vel.y()); } -template Eigen::Vector3d SampledOrbit::computeVelocityLinear(double jd, - int n) const +template +Eigen::Vector3d +SampledOrbit::computeVelocityLinear(std::uint32_t n) const { - Sample s0 = samples[n - 1]; - Sample s1 = samples[n]; + assert(n > 0); + double dtRecip = 1.0 / (sampleTimes[n] - sampleTimes[n - 1]); + return (positions[n].template cast() - positions[n - 1].template cast()) * dtRecip; +} + - double dtRecip = 1.0 / (s1.t - s0.t); - return (s1.position.template cast() - s0.position.template cast()) * dtRecip; +template +Eigen::Vector3d +SampledOrbit::computeVelocityCubic(double jd, std::uint32_t n2, std::uint32_t nMax) const +{ + InterpolationParameters params; + initializeCubic(jd, n2, nMax, params); + return cubicInterpolateVelocity(params); } -template Eigen::Vector3d SampledOrbit::computeVelocityCubic(double jd, - int n) const +template +void +SampledOrbit::initializeCubic(double jd, + std::uint32_t n2, + std::uint32_t nMax, + InterpolationParameters& params) const { - Sample s0 = n > 1 - ? samples[n - 2] - : samples[n - 1]; - Sample s1 = samples[n - 1]; - Sample s2 = samples[n]; - Sample s3 = n < ((int) samples.size() - 1) - ? samples[n + 1] - : samples[n]; + assert(n2 > 0 && n2 <= nMax); + std::uint32_t n0 = std::max(n2, std::uint32_t(2)) - 2; + std::uint32_t n1 = n2 - 1; + std::uint32_t n3 = std::min(n2 + 1, nMax); - double h = s2.t - s1.t; - double ih = 1.0 / h; - double t = (jd - s1.t) * ih; - Eigen::Vector3d p0 = s1.position.template cast(); - Eigen::Vector3d p1 = s2.position.template cast(); + double h = sampleTimes[n2] - sampleTimes[n1]; + params.ih = 1.0 / h; + params.t = (jd - sampleTimes[n1]) * params.ih; + params.p0 = positions[n1].template cast(); + params.p1 = positions[n2].template cast(); - Eigen::Vector3d v10 = p0 - s0.position.template cast(); - Eigen::Vector3d v21 = p1 - p0; - Eigen::Vector3d v32 = s3.position.template cast() - p1; + Eigen::Vector3d v10 = params.p0 - positions[n0].template cast(); + Eigen::Vector3d v21 = params.p1 - params.p0; + Eigen::Vector3d v32 = positions[n3].template cast() - params.p1; // Estimate velocities by averaging the differences at adjacent spans // (except at the end spans, where we just use a single velocity.) - Eigen::Vector3d v0 = n > 1 - ? (v10 * (0.5 / (s1.t - s0.t)) + v21 * (0.5 * ih)) * h + params.v0 = n2 > 1 + ? (v10 * (0.5 / (sampleTimes[n1] - sampleTimes[n0])) + v21 * (0.5 * params.ih)) * h : v21; - Eigen::Vector3d v1 = n < ((int) samples.size() - 1) - ? (v21 * (0.5 * ih) + v32 * (0.5 / (s3.t - s2.t))) * h + params.v1 = n2 < nMax + ? (v21 * (0.5 * params.ih) + v32 * (0.5 / (sampleTimes[n3] - sampleTimes[n2]))) * h : v21; - - return cubicInterpolateVelocity(p0, v0, p1, v1, t) * (1.0 / h); } -template void SampledOrbit::sample(double /* startTime */, double /* endTime */, - OrbitSampleProc& proc) const +template +void SampledOrbit::sample(double /* startTime */, double /* endTime */, + OrbitSampleProc& proc) const { - for (unsigned int i = 0; i < samples.size(); i++) + for (std::uint32_t i = 0; i < sampleTimes.size(); ++i) { Eigen::Vector3d v; - Eigen::Vector3d p = samples[i].position.template cast(); + Eigen::Vector3d p = positions[i].template cast(); - if (samples.size() == 1) + if (sampleTimes.size() == 1) { v = Eigen::Vector3d::Zero(); } else if (i == 0) { - double dtRecip = 1.0 / (samples[i + 1].t - samples[i].t); - v = (samples[i + 1].position.template cast() - p) * dtRecip; + double dtRecip = 1.0 / (sampleTimes[i + 1] - sampleTimes[i]); + v = (positions[i + 1].template cast() - p) * dtRecip; } - else if (i == samples.size() - 1) + else if (i == sampleTimes.size() - 1) { - double dtRecip = 1.0 / (samples[i].t - samples[i - 1].t); - v = (p - samples[i - 1].position.template cast()) * dtRecip; + double dtRecip = 1.0 / (sampleTimes[i] - sampleTimes[i - 1]); + v = (p - positions[i - 1].template cast()) * dtRecip; } else { - double dt0Recip = 1.0 / (samples[i + 1].t - samples[i].t); - Eigen::Vector3d v0 = (samples[i + 1].position.template cast() - p) * dt0Recip; - double dt1Recip = 1.0 / (samples[i].t - samples[i - 1].t); - Eigen::Vector3d v1 = (p - samples[i - 1].position.template cast()) * dt1Recip; + double dt0Recip = 1.0 / (sampleTimes[i + 1] - sampleTimes[i]); + Eigen::Vector3d v0 = (positions[i + 1].template cast() - p) * dt0Recip; + double dt1Recip = 1.0 / (sampleTimes[i] - sampleTimes[i - 1]); + Eigen::Vector3d v1 = (p - positions[i - 1].template cast()) * dt1Recip; v = (v0 + v1) * 0.5; } - proc.sample(samples[i].t, - Eigen::Vector3d(p.x(), p.z(), -p.y()), - Eigen::Vector3d(v.x(), v.z(), -v.y())); + proc.sample(sampleTimes[i], p, v); } } // Sampled orbit with positions and velocities -template class SampledOrbitXYZV : public CachingOrbit +template +class SampledOrbitXYZV : public CachingOrbit { public: SampledOrbitXYZV(TrajectoryInterpolation /*_interpolation*/); @@ -427,6 +394,7 @@ template class SampledOrbitXYZV : public CachingOrbit void addSample(double t, const Eigen::Matrix& position, const Eigen::Matrix& velocity); + bool hasSamples() const; double getPeriod() const override; double getBoundingRadius() const override; @@ -439,16 +407,26 @@ template class SampledOrbitXYZV : public CachingOrbit void sample(double startTime, double endTime, OrbitSampleProc& proc) const override; private: - std::vector> samples; + struct SampleXYZV + { + Eigen::Matrix position; + Eigen::Matrix velocity; + }; + + void initializeCubic(double, std::uint32_t, InterpolationParameters&) const; + + std::vector sampleTimes; + std::vector samples; double boundingRadius; double period; - mutable int lastSample; + mutable std::uint32_t lastSample; TrajectoryInterpolation interpolation; }; -template SampledOrbitXYZV::SampledOrbitXYZV(TrajectoryInterpolation _interpolation) : +template +SampledOrbitXYZV::SampledOrbitXYZV(TrajectoryInterpolation _interpolation) : boundingRadius(0.0), period(1.0), lastSample(0), @@ -460,191 +438,182 @@ template SampledOrbitXYZV::SampledOrbitXYZV(TrajectoryInterpolat // Add a new sample to the trajectory: // Position in km // Velocity in km/Julian day -template void SampledOrbitXYZV::addSample(double t, - const Eigen::Matrix& position, - const Eigen::Matrix& velocity) +template +void +SampledOrbitXYZV::addSample(double t, + const Eigen::Matrix& position, + const Eigen::Matrix& velocity) { + if (sampleTimes.size() >= std::numeric_limits::max()) + return; + double r = position.template cast().norm(); if (r > boundingRadius) boundingRadius = r; - SampleXYZV& samp = samples.emplace_back(); - samp.t = t; - samp.position = position; - samp.velocity = velocity; + sampleTimes.push_back(t); + + SampleXYZV& samp = samples.emplace_back(); + // Add correction for Celestia's coordinate system + samp.position = Eigen::Matrix(position.x(), position.z(), -position.y()); + samp.velocity = Eigen::Matrix(velocity.x(), velocity.z(), -velocity.y()); } -template double SampledOrbitXYZV::getPeriod() const + +template +bool +SampledOrbitXYZV::hasSamples() const { - if (samples.empty()) - return 0.0; + return !sampleTimes.empty(); +} - return samples[samples.size() - 1].t - samples[0].t; + +template +double +SampledOrbitXYZV::getPeriod() const +{ + return sampleTimes.back() - sampleTimes.front(); } -template bool SampledOrbitXYZV::isPeriodic() const +template +bool +SampledOrbitXYZV::isPeriodic() const { return false; } -template void SampledOrbitXYZV::getValidRange(double& begin, double& end) const +template +void +SampledOrbitXYZV::getValidRange(double& begin, double& end) const { - begin = samples[0].t; - end = samples[samples.size() - 1].t; + begin = sampleTimes.front(); + end = sampleTimes.back(); } -template double SampledOrbitXYZV::getBoundingRadius() const +template +double +SampledOrbitXYZV::getBoundingRadius() const { return boundingRadius; } -template Eigen::Vector3d SampledOrbitXYZV::computePosition(double jd) const +template +Eigen::Vector3d +SampledOrbitXYZV::computePosition(double jd) const { + if (sampleTimes.size() == 1) + return samples.front().position.template cast(); + Eigen::Vector3d pos; - if (samples.size() == 0) - { - pos = Eigen::Vector3d::Zero(); - } - else if (samples.size() == 1) + std::uint32_t n = lastSample; + const auto maxSample = static_cast(sampleTimes.size()); + if (n < 1 || n >= maxSample || jd < sampleTimes[n - 1] || jd > sampleTimes[n]) { - pos = samples[0].position.template cast(); + auto iter = std::lower_bound(sampleTimes.begin(), sampleTimes.end(), jd); + n = static_cast(iter - sampleTimes.begin()); + lastSample = n; } - else - { - SampleXYZV samp; - samp.t = jd; - int n = lastSample; - - if (n < 1 || n >= (int) samples.size() || jd < samples[n - 1].t || jd > samples[n].t) - { - auto iter = std::lower_bound(samples.begin(), samples.end(), samp); - if (iter == samples.end()) - n = samples.size(); - else - n = iter - samples.begin(); - lastSample = n; - } + if (n == 0) + return samples.front().position.template cast(); + if (n == maxSample) + return samples.back().position.template cast(); - if (n == 0) - { - pos = samples[n].position.template cast(); - } - else if (n < (int) samples.size()) - { - SampleXYZV s0 = samples[n - 1]; - SampleXYZV s1 = samples[n]; + if (interpolation == TrajectoryInterpolation::Linear) + { + double t = (jd - sampleTimes[n - 1]) / (sampleTimes[n] - sampleTimes[n - 1]); - if (interpolation == TrajectoryInterpolation::Linear) - { - double t = (jd - s0.t) / (s1.t - s0.t); + Eigen::Vector3d p0 = samples[n - 1].position.template cast(); + Eigen::Vector3d p1 = samples[n].position.template cast(); + return p0 + t * (p1 - p0); + } - Eigen::Vector3d p0 = s0.position.template cast(); - Eigen::Vector3d p1 = s1.position.template cast(); - pos = p0 + t * (p1 - p0); - } - else if (interpolation == TrajectoryInterpolation::Cubic) - { - double h = s1.t - s0.t; - double ih = 1.0 / h; - double t = (jd - s0.t) * ih; - - Eigen::Vector3d p0 = s0.position.template cast(); - Eigen::Vector3d v0 = s0.velocity.template cast(); - Eigen::Vector3d p1 = s1.position.template cast(); - Eigen::Vector3d v1 = s1.velocity.template cast(); - pos = cubicInterpolate(p0, v0 * h, p1, v1 * h, t); - } - else - { - // Unknown interpolation type - pos = Eigen::Vector3d::Zero(); - } - } - else - { - pos = samples[n - 1].position.template cast(); - } + if (interpolation == TrajectoryInterpolation::Cubic) + { + InterpolationParameters params; + initializeCubic(jd, n, params); + return cubicInterpolate(params); } - // Add correction for Celestia's coordinate system - return Eigen::Vector3d(pos.x(), pos.z(), -pos.y()); + // Unknown interpolation type + assert(0); + return Eigen::Vector3d::Zero(); } // Velocity is computed as the derivative of the interpolating function // for position. -template Eigen::Vector3d SampledOrbitXYZV::computeVelocity(double jd) const +template +Eigen::Vector3d +SampledOrbitXYZV::computeVelocity(double jd) const { - Eigen::Vector3d vel(Eigen::Vector3d::Zero()); + if (sampleTimes.size() < 2) + return Eigen::Vector3d::Zero(); - if (samples.size() >= 2) + std::uint32_t n = lastSample; + const auto maxSample = static_cast(sampleTimes.size()); + if (n < 1 || n >= maxSample || jd < sampleTimes[n - 1] || jd > sampleTimes[n]) { - SampleXYZV samp; - samp.t = jd; - int n = lastSample; - - if (n < 1 || n >= (int) samples.size() || jd < samples[n - 1].t || jd > samples[n].t) - { - auto iter = std::lower_bound(samples.begin(), samples.end(), samp); - if (iter == samples.end()) - n = samples.size(); - else - n = iter - samples.begin(); + auto iter = std::lower_bound(sampleTimes.begin(), sampleTimes.end(), jd); + n = static_cast(iter - sampleTimes.begin()); + lastSample = n; + } - lastSample = n; - } + if (n == 0 || n == maxSample) + return Eigen::Vector3d::Zero(); - if (n > 0 && n < (int) samples.size()) - { - SampleXYZV s0 = samples[n - 1]; - SampleXYZV s1 = samples[n]; + if (interpolation == TrajectoryInterpolation::Linear) + { + double hRecip = 1.0 / (sampleTimes[n] - sampleTimes[n - 1]); + return (samples[n].position.template cast() - samples[n - 1].position.template cast()) * + hRecip * + astro::daysToSecs(1.0); + } - if (interpolation == TrajectoryInterpolation::Linear) - { - double hRecip = 1.0 / (s1.t - s0.t); - vel = (s1.position.template cast() - s0.position.template cast()) * - hRecip * - astro::daysToSecs(1.0); - } - else if (interpolation == TrajectoryInterpolation::Cubic) - { - double h = s1.t - s0.t; - double ih = 1.0 / h; - double t = (jd - s0.t) * ih; + if (interpolation == TrajectoryInterpolation::Cubic) + { + InterpolationParameters params; + initializeCubic(jd, n, params); + return cubicInterpolateVelocity(params); + } - Eigen::Vector3d p0 = s0.position.template cast(); - Eigen::Vector3d p1 = s1.position.template cast(); - Eigen::Vector3d v0 = s0.velocity.template cast(); - Eigen::Vector3d v1 = s1.velocity.template cast(); + // Unknown interpolation type + assert(0); + return Eigen::Vector3d::Zero(); +} - vel = cubicInterpolateVelocity(p0, v0 * h, p1, v1 * h, t) * ih; - } - else - { - // Unknown interpolation type - vel = Eigen::Vector3d::Zero(); - } - } - } - // Add correction for Celestia's coordinate system - return Eigen::Vector3d(vel.x(), vel.z(), -vel.y()); +template +void +SampledOrbitXYZV::initializeCubic(double jd, + std::uint32_t n, + InterpolationParameters& params) const +{ + assert(n > 0); + double h = sampleTimes[n] - sampleTimes[n - 1]; + params.ih = 1.0 / h; + params.t = (jd - sampleTimes[n - 1]) * params.ih; + params.p0 = samples[n - 1].position.template cast(); + params.v0 = samples[n - 1].velocity.template cast() * h; + params.p1 = samples[n].position.template cast(); + params.v1 = samples[n].velocity.template cast() * h; } -template void SampledOrbitXYZV::sample(double /* startTime */, double /* endTime */, - OrbitSampleProc& proc) const +template +void +SampledOrbitXYZV::sample(double /* startTime */, double /* endTime */, + OrbitSampleProc& proc) const { - for (const auto& sample : samples) + for (std::uint32_t i = 0; i < sampleTimes.size(); ++i) { - proc.sample(sample.t, - Eigen::Vector3d(sample.position.x(), sample.position.z(), -sample.position.y()), - Eigen::Vector3d(sample.velocity.x(), sample.velocity.z(), -sample.velocity.y())); + proc.sample(sampleTimes[i], + samples[i].position.template cast(), + samples[i].velocity.template cast()); } } @@ -655,41 +624,30 @@ template void SampledOrbitXYZV::sample(double /* startTime */, do bool SkipComments(std::istream& in) { bool inComment = false; - bool done = false; - - int c = in.get(); - while (!done) + for (;;) { - if (in.eof()) + int c = in.get(); + if (!in.good()) + return false; + + if (inComment) { - done = true; + if (c == '\n') + inComment = false; } else { - if (inComment) + if (c == '#') { - if (c == '\n') - inComment = false; + inComment = true; } - else + else if (!std::isspace(static_cast(c))) { - if (c == '#') - { - inComment = true; - } - else if (std::isspace(static_cast(c)) == 0) - { - in.unget(); - done = true; - } + in.unget(); + return in.good(); } } - - if (!done) - c = in.get(); } - - return in.good(); } @@ -720,28 +678,27 @@ LoadSampledOrbit(const fs::path& filename, TrajectoryInterpolation interpolation auto orbit = std::make_unique>(interpolation); double lastSampleTime = -std::numeric_limits::infinity(); - while (in.good()) + bool hasOutOfOrderSamples = false; + for (;;) { double tdb; Eigen::Matrix position; - in >> tdb; - in >> position.x(); - in >> position.y(); - in >> position.z(); - - if (in.good()) + in >> tdb >> position.x() >> position.y() >> position.z(); + if (!in.good()) { - // Skip samples with duplicate times; such trajectories are invalid, but - // are unfortunately used in some existing add-ons. - if (tdb != lastSampleTime) - { - orbit->addSample(tdb, position); - lastSampleTime = tdb; - } + if (in.eof() && validateSamples(*orbit, filename)) + return orbit; + + return nullptr; } - } - return orbit; + // Skip samples with duplicate or out-of-order times; such trajectories + // are invalid, but are unfortunately used in some existing add-ons. + if (!checkSampleOrdering(tdb, lastSampleTime, hasOutOfOrderSamples, filename)) + continue; + + orbit->addSample(tdb, position); + } } @@ -774,37 +731,37 @@ LoadSampledOrbitXYZV(const fs::path& filename, TrajectoryInterpolation interpola auto orbit = std::make_unique>(interpolation); + bool hasOutOfOrderSamples = false; double lastSampleTime = -std::numeric_limits::infinity(); - while (in.good()) + for (;;) { double tdb = 0.0; Eigen::Matrix position; Eigen::Matrix velocity; - in >> tdb; - in >> position.x(); - in >> position.y(); - in >> position.z(); - in >> velocity.x(); - in >> velocity.y(); - in >> velocity.z(); + in >> tdb + >> position.x() >> position.y() >> position.z() + >> velocity.x() >> velocity.y() >> velocity.z(); + + if (!in.good()) + { + if (in.eof() && validateSamples(*orbit, filename)) + return orbit; + + return nullptr; + } + + if (!checkSampleOrdering(tdb, lastSampleTime, hasOutOfOrderSamples, filename)) + continue; // Convert velocities from km/sec to km/Julian day velocity = velocity * astro::daysToSecs(1.0); - if (in.good()) - { - if (tdb != lastSampleTime) - { - orbit->addSample(tdb, position, velocity); - lastSampleTime = tdb; - } - } + orbit->addSample(tdb, position, velocity); } - - return orbit; } + bool ParseXYZVBinaryHeader(std::istream& in, const fs::path& filename) { @@ -850,9 +807,11 @@ ParseXYZVBinaryHeader(std::istream& in, const fs::path& filename) return true; } + /* Load a binary xyzv sampled trajectory file. */ -template std::unique_ptr> +template +std::unique_ptr> LoadSampledOrbitXYZVBinary(const fs::path& filename, TrajectoryInterpolation interpolation) { std::ifstream in(filename, std::ios::binary); @@ -862,16 +821,25 @@ LoadSampledOrbitXYZVBinary(const fs::path& filename, TrajectoryInterpolation int return nullptr; } - ParseXYZVBinaryHeader(in, filename); + if (!ParseXYZVBinaryHeader(in, filename)) + { + GetLogger()->error(_("Could not read XYZV binary file {}.\n"), filename); + return nullptr; + } auto orbit = std::make_unique>(interpolation); double lastSampleTime = -std::numeric_limits::infinity(); - - while (in.good()) + bool hasOutOfOrderSamples = false; + for (;;) { std::array data; if (!in.read(data.data(), data.size())) /* Flawfinder: ignore */ - break; + { + if (in.eof() && validateSamples(*orbit, filename)) + return orbit; + + return nullptr; + } double tdb; Eigen::Vector3d position; @@ -884,13 +852,12 @@ LoadSampledOrbitXYZVBinary(const fs::path& filename, TrajectoryInterpolation int // Convert velocities from km/sec to km/Julian day velocity *= astro::daysToSecs(1.0); - if (tdb != lastSampleTime) - { - Eigen::Matrix pos = position.cast(); - Eigen::Matrix vel = velocity.cast(); - orbit->addSample(tdb, pos, vel); - lastSampleTime = tdb; - } + if (!checkSampleOrdering(tdb, lastSampleTime, hasOutOfOrderSamples, filename)) + continue; + + Eigen::Matrix pos = position.cast(); + Eigen::Matrix vel = velocity.cast(); + orbit->addSample(tdb, pos, vel); } return orbit; @@ -898,6 +865,7 @@ LoadSampledOrbitXYZVBinary(const fs::path& filename, TrajectoryInterpolation int } // end unnamed namespace + /*! Load a trajectory file containing single precision positions. */ std::unique_ptr @@ -966,4 +934,26 @@ LoadXYZVBinaryDoublePrec(const fs::path& filename, TrajectoryInterpolation inter return LoadSampledOrbitXYZVBinary(filename, interpolation); } + +bool +checkSampleOrdering(double tdb, + double& lastSampleTime, + bool& hasOutOfOrderSamples, + const fs::path& filename) +{ + if (tdb > lastSampleTime) + { + lastSampleTime = tdb; + return true; + } + + if (!hasOutOfOrderSamples) + { + GetLogger()->warn(_("Skipping out-of-order samples in {}."), filename); + hasOutOfOrderSamples = true; + } + + return false; +} + } // end namespace celestia::ephem diff --git a/src/celephem/samporbit.h b/src/celephem/samporbit.h index 6df9fe331fd..4d21b869fe8 100644 --- a/src/celephem/samporbit.h +++ b/src/celephem/samporbit.h @@ -37,4 +37,9 @@ std::unique_ptr LoadXYZVTrajectorySinglePrec(const fs::path& filename, Tr std::unique_ptr LoadXYZVBinarySinglePrec(const fs::path& filename, TrajectoryInterpolation interpolation); std::unique_ptr LoadXYZVBinaryDoublePrec(const fs::path& filename, TrajectoryInterpolation interpolation); -} +bool checkSampleOrdering(double tdb, + double& lastSampleTime, + bool& hasOutOfOrderSamples, + const fs::path& filename); + +} // end namespace celestia::ephem diff --git a/src/celephem/samporient.cpp b/src/celephem/samporient.cpp index e343f7c714c..2d6c49e7547 100644 --- a/src/celephem/samporient.cpp +++ b/src/celephem/samporient.cpp @@ -14,14 +14,21 @@ #include "samporient.h" #include +#include #include +#include #include #include #include #include +#include +#include #include "rotation.h" +#include "samporbit.h" + +using celestia::util::GetLogger; namespace celestia::ephem { @@ -29,14 +36,6 @@ namespace celestia::ephem namespace { -struct OrientationSample -{ - Eigen::Quaternionf q; - double t; -}; - -using OrientationSampleVector = std::vector; - /*! * Sampled orientation files are ASCII text files containing a sequence of * time stamped quaternion keys. Each record in the file has the form: @@ -60,12 +59,6 @@ using OrientationSampleVector = std::vector; * a single line. */ - -bool operator<(const OrientationSample& a, const OrientationSample& b) -{ - return a.t < b.t; -} - /*! SampledOrientation is a rotation model that interpolates a sequence * of quaternion keyframes. Typically, an instance of SampledRotation will * be created from a file with LoadSampledOrientation(). @@ -80,6 +73,7 @@ class SampledOrientation : public RotationModel * should have monotonically increasing time values. */ void addSample(double tjd, const Eigen::Quaternionf& q); + bool empty() const; /*! The orientation of a sampled rotation model is entirely due * to spin (i.e. there's no notion of an equatorial frame.) @@ -91,32 +85,37 @@ class SampledOrientation : public RotationModel void getValidRange(double& begin, double& end) const override; + private: Eigen::Quaternionf getOrientation(double tjd) const; private: - OrientationSampleVector samples; - mutable int lastSample{0}; - - enum InterpolationType - { - Linear = 0, - Cubic = 1, - }; - - InterpolationType interpolation{Linear}; + // Storing sample times and rotations separately avoids padding due to + // the 16-byte alignment of Quaternionf + std::vector sampleTimes; + std::vector rotations; + mutable std::uint32_t lastSample{0}; }; void SampledOrientation::addSample(double t, const Eigen::Quaternionf& q) { - // TODO: add a check for out of sequence samples - OrientationSample& samp = samples.emplace_back(); - samp.t = t; + if (sampleTimes.size() >= std::numeric_limits::max()) + return; + + sampleTimes.push_back(t); + // 90 degree rotation about x-axis to convert orientation to Celestia's // coordinate system. - samp.q = q * celmath::XRot90; + rotations.push_back(q * celmath::XRot90); +} + + +bool +SampledOrientation::empty() const +{ + return sampleTimes.empty(); } @@ -130,7 +129,7 @@ SampledOrientation::spin(double tjd) const double SampledOrientation::getPeriod() const { - return samples[samples.size() - 1].t - samples[0].t; + return sampleTimes.back() - sampleTimes.front(); } @@ -142,77 +141,46 @@ bool SampledOrientation::isPeriodic() const void SampledOrientation::getValidRange(double& begin, double& end) const { - begin = samples[0].t; - end = samples[samples.size() - 1].t; + begin = sampleTimes.front(); + end = sampleTimes.back(); } Eigen::Quaternionf SampledOrientation::getOrientation(double tjd) const { - Eigen::Quaternionf orientation; - if (samples.size() == 0) - { - orientation = Eigen::Quaternionf::Identity(); - } - else if (samples.size() == 1) + if (sampleTimes.size() == 1) + return rotations.front(); + + std::uint32_t n = lastSample; + const auto maxSample = static_cast(sampleTimes.size()); + // Do a binary search to find the samples that define the orientation + // at the current time. Cache the previous sample used and avoid + // the search if the covers the requested time. + if (n < 1 || n >= maxSample || tjd < sampleTimes[n - 1] || tjd > sampleTimes[n]) { - orientation = samples[0].q; + auto iter = std::lower_bound(sampleTimes.begin(), sampleTimes.end(), tjd); + n = static_cast(iter - sampleTimes.begin()); + lastSample = n; } - else - { - OrientationSample samp; - samp.t = tjd; - int n = lastSample; - - // Do a binary search to find the samples that define the orientation - // at the current time. Cache the previous sample used and avoid - // the search if the covers the requested time. - if (n < 1 || n >= (int) samples.size() || tjd < samples[n - 1].t || tjd > samples[n].t) - { - auto iter = std::lower_bound(samples.begin(), - samples.end(), - samp); - if (iter == samples.end()) - n = samples.size(); - else - n = iter - samples.begin(); - - lastSample = n; - } - if (n == 0) - { - orientation = samples[0].q; - } - else if (n < (int) samples.size()) - { - if (interpolation == Linear) - { - OrientationSample s0 = samples[n - 1]; - OrientationSample s1 = samples[n]; - - auto t = (float) ((tjd - s0.t) / (s1.t - s0.t)); - orientation = s0.q.slerp(t, s1.q); - } - else if (interpolation == Cubic) - { - // TODO: add support for cubic interpolation of quaternions - assert(0); - } - else - { - // Unknown interpolation type - orientation = Eigen::Quaternionf::Identity(); - } - } - else - { - orientation = samples[samples.size() - 1].q; - } - } + if (n == 0) + return rotations.front(); + else if (n == maxSample) + return rotations.back(); + + auto t = static_cast((tjd - sampleTimes[n - 1]) / (sampleTimes[n] - sampleTimes[n - 1])); + return rotations[n - 1].slerp(t, rotations[n]); +} + - return orientation; +bool validateSamples(const SampledOrientation& sampOrientation, const fs::path& filename) +{ + if (!sampOrientation.empty()) + return true; + + GetLogger()->error(_("Sampled rotation {} contains no valid samples.\n"), filename); + return false; } } // end unnamed namespace @@ -226,24 +194,29 @@ LoadSampledOrientation(const fs::path& filename) return nullptr; auto sampOrientation = std::make_unique(); - while (in.good()) + double lastSampleTime = -std::numeric_limits::infinity(); + bool hasOutOfOrderSamples = false; + for (;;) { double tjd; Eigen::Quaternionf q; - in >> tjd; - in >> q.w(); - in >> q.x(); - in >> q.y(); - in >> q.z(); - q.normalize(); - - if (in.good()) + in >> tjd >> q.w() >> q.x() >> q.y() >> q.z(); + if (!in.good()) { - sampOrientation->addSample(tjd, q); + if (in.eof() && validateSamples(*sampOrientation, filename)) + return sampOrientation; + + return nullptr; } - } - return sampOrientation; + q.normalize(); + + if (!checkSampleOrdering(tjd, lastSampleTime, hasOutOfOrderSamples, filename)) + continue; + + sampOrientation->addSample(tjd, q); + lastSampleTime = tjd; + } } } // end namespace celestia::ephem