.. _program_listing_file_interfaces_spice_cheby.cc: Program Listing for File spice_cheby.cc ======================================= |exhale_lsh| :ref:`Return to documentation for file ` (``interfaces/spice_cheby.cc``) .. |exhale_lsh| unicode:: U+021B0 .. UPWARDS ARROW WITH TIP LEFTWARDS .. code-block:: cpp #include "lupnt/interfaces/spice_cheby.h" #include "lupnt/numerics/math_utils.h" namespace lupnt { /* DAF/SPK file format notes: ic[0] - target code ic[1] - center code ic[2] - frame code ic[3] - representation code (2 == Chebyshev position only) ic[4] - initial address of array ic[5] - final address of array len = ic[5] - ic[4] + 1 dc[0] - initial epoch of data (seconds relative to J2000) dc[1] - final epoch of data (seconds relative to J2000) seg[len-4] - initial epoch of the first record (seconds relative to J2000) seg[len-3] - interval length of each record (seconds) seg[len-2] - elements in each record seg[len-1] - number of records seg[len-1] * seg[len-2] + 4 == len n = seg[len-2] num = (n - 2) / 3 rec[0] - midpoint of interval covered by record (seconds relative to J2000) rec[1] - radius of interval (seconds) rec[2..num+1] - X coefficients (constant term first) rec[num+2..2*num+1] - Y coefficients rec[2*num+2..n-1] - Z coefficients For t, evaluate the Chebyshev polynomials T_n at (t - rec[0]) / rec[1], multiply by the coefficients, and sum. The derivatives of the polynomials can be used to compute the velocity. See cheby_eval() here. The results are in km and km/s. Note that all times are ephemeris times, and so do not take into account leap seconds. A SpiceDouble is simply a C double. A SpiceInt is an integer type whose size is half that of double, so that two SpiceInt's fit in a SpiceDouble. */ void cheby_eval(double x, double* scale, double* coeff, long num, double* f, double* df) { double x2, w0 = 0., w1 = 0., dw0 = 0., dw1 = 0., tmp; x = (x - scale[0]) / scale[1]; x2 = x * 2.; while (--num) { tmp = dw1; dw1 = dw0; dw0 = w0 * 2. + dw0 * x2 - tmp; tmp = w1; w1 = w0; w0 = coeff[num] + (x2 * w0 - tmp); } *f = coeff[0] + (x * w0 - w1); *df = (w0 + x * dw0 - dw1) / scale[1]; } Vec2 cheby_eval_ad(Real x, double* scale, double* coeff, long num) { Real x2, w0 = 0., w1 = 0., dw0 = 0., dw1 = 0., tmp; x = (x - scale[0]) / scale[1]; x2 = x * 2.; while (--num) { tmp = dw1; dw1 = dw0; dw0 = w0 * 2. + dw0 * x2 - tmp; tmp = w1; w1 = w0; w0 = coeff[num] + (x2 * w0 - tmp); } Vec2 ret_state{coeff[0] + (x * w0 - w1), (w0 + x * dw0 - dw1) / scale[1]}; return ret_state; } int cheby_posvel(double t, double* seg, long len, double pos[3], double vel[3]) { long k, num; k = (long)floor((t - seg[len - 4]) / // seg[len-4] is initial epoch seg[len - 3]); // seg[len-3] is record span if (k < 0 || k >= (long)seg[len - 1]) // seg[len-1] is number of records return 1; num = (long)seg[len - 2]; // seg[len-2] is size of record seg += k * num; // point seg to the record for t num = (num - 2) / 3; // number of coefficients cheby_eval(t, seg, seg + 2, num, pos, vel); cheby_eval(t, seg, seg + 2 + num, num, pos + 1, vel + 1); cheby_eval(t, seg, seg + 2 + 2 * num, num, pos + 2, vel + 2); return 0; } Vec6 cheby_posvel_ad(Real t, double* seg, long len) { long k, num; k = (long)floor((t.val() - seg[len - 4]) / // seg[len-4] is initial epoch seg[len - 3]); // seg[len-3] is record span if (k < 0 || k >= (long)seg[len - 1]) // seg[len-1] is number of records return Vec6::Zero(); num = (long)seg[len - 2]; // seg[len-2] is size of record seg += k * num; // point seg to the record for t num = (num - 2) / 3; // number of coefficients Vec2 xdx = cheby_eval_ad(t, seg, seg + 2, num); Vec2 ydy = cheby_eval_ad(t, seg, seg + 2 + num, num); Vec2 zdz = cheby_eval_ad(t, seg, seg + 2 + 2 * num, num); Vec6 posvel{xdx[0], ydy[0], zdz[0], xdx[1], ydy[1], zdz[1]}; return posvel; } int cheby_verify(double* seg, long len) { double recs = seg[len - 1], // number of records elts = seg[len - 2], // elements (doubles) in each record span = seg[len - 3], // time span of each record in seconds init = seg[len - 4]; // initial epoch in seconds relative to J2000 long n, k; double *p, *q; if (recs != (long)recs || // recs is an integer elts != (long)elts || // elts is an integer (long)recs * (long)elts + 4 != len || // total length is correct 3 * (((long)elts - 2) / 3) + 2 != elts || // integer number of coeffs seg[0] - seg[1] != init || // 1st start is init span != 2 * seg[1]) // 1st radius matches span return 1; n = (long)recs; k = (long)elts; p = seg; while (--n) { q = p + k; // scan all q following p if (q[1] != p[1] || // all radii the same q[0] - q[1] != p[0] + p[1]) // next start is last end return 1; p = q; } return 0; } void cheby_err(char const* msg, ...) { fputs("cheby error: ", stderr); va_list ap; va_start(ap, msg); vfprintf(stderr, msg, ap); va_end(ap); putc('\n', stderr); } int cheby_segment(SpiceInt daf, SpiceDouble* dc, SpiceInt* ic, segment_t* s) { SpiceDouble* last; // save segment codes s->target = ic[0]; s->center = ic[1]; s->frame = ic[2]; // allocate memory for the segment and read it in s->len = ic[5] - ic[4] + 1; // number of doubles in segment // s->seg = malloc(s->len * sizeof(SpiceDouble)); s->seg = (double*)malloc(s->len * sizeof(double)); if (s->seg == NULL) { cheby_err("out of memory"); return 1; } dafgda_c(daf, ic[4], ic[5], s->seg); // load segment if (failed_c()) { reset_c(); free(s->seg); s->seg = NULL; cheby_err("could not read SPK segment from file"); return 1; } // verify the integrity of the segment last = s->seg + s->len - 4 - (long)(s->seg[s->len - 2]); if (cheby_verify(s->seg, s->len) || // segment structure ok dc[0] != s->seg[s->len - 4] || // start epoch matches dc[1] != last[0] + last[1]) { // end epoch matches free(s->seg); s->seg = NULL; cheby_err("SPK segment format is invalid"); return 1; } // return loaded segment return 0; } segment_t* spk_extract(char const* path, long* segs) { SpiceInt daf; SpiceBoolean found; union { SpiceDouble d[128]; SpiceChar c[1024]; } sum; const SpiceInt nd = 2, ni = 6; SpiceDouble dc[nd]; SpiceInt ic[ni]; segment_t *spk, *mem; // turn off error reporting and aborts for SPICE functions errprt_c("set", 0, (char*)"none"); erract_c("set", 0, (char*)"return"); // open the file and verifiy that it is a DAF SPK file dafopr_c(path, &daf); // open SPK file for reading if (failed_c()) { reset_c(); cheby_err("could not open %s as a DAF", path); return NULL; } dafgsr_c(daf, 1, 1, 128, sum.d, &found); // read first record if (failed_c() || !found || memcmp(sum.c, "DAF/SPK ", 8)) { reset_c(); dafcls_c(daf); cheby_err("%s is not an SPK file", path); return NULL; } // count the number of Chebyshev position-only segments in the DAF file *segs = 0; dafbfs_c(daf); // begin forward search while (daffna_c(&found), found) { // find the next array dafgs_c(sum.d); // get array summary dafus_c(sum.d, nd, ni, dc, ic); // unpack the array summary if (failed_c()) break; if (ic[3] == 2) // Chebyshev position only (*segs)++; // count segment } if (failed_c() || *segs == 0) { reset_c(); dafcls_c(daf); cheby_err("file error or Chebyshev position-only segments in %s", path); return NULL; } // allocate table of segment descriptors spk = (segment_t*)malloc(*segs * sizeof(segment_t)); if (spk == NULL) { dafcls_c(daf); cheby_err("out of memory"); return NULL; } // read and save the Chebyshev position-only segments *segs = 0; dafbfs_c(daf); // begin forward search while (daffna_c(&found), found) { // find the next array dafgs_c(sum.d); // get array summary dafus_c(sum.d, nd, ni, dc, ic); // unpack the array summary if (failed_c()) break; if (ic[3] == 2 && !cheby_segment(daf, dc, ic, spk + *segs)) (*segs)++; } if (failed_c() || *segs == 0) { reset_c(); dafcls_c(daf); free(segs); cheby_err("no valid Chebyshev position-only segments in %s", path); return NULL; } // close the DAF file and return segment table dafcls_c(daf); errprt_c("set", 0, (char*)"short"); erract_c("set", 0, (char*)"abort"); mem = (segment_t*)realloc(spk, *segs * sizeof(segment_t)); if (mem != NULL) spk = mem; return spk; } void spk_free(segment_t* s, long n) { long i; for (i = 0; i < n; i++) free(s[i].seg); free(s); } } // namespace lupnt