Isis 3 Programmer Reference
ck3sdn.cpp
1 #include <cfloat>
2 #include <cmath>
3 #include <iomanip>
4 
5 #include "Camera.h"
6 
7 using namespace std;
8 using namespace Isis;
9 //#include "f2c.h"
10 
11 /* Table of constant values */
12 
13 static int c__4 = 4;
14 /* Builtin functions */
15 // double sqrt(doublereal), asin(doublereal);
16 extern int moved_(doublereal *arrfrm, integer *ndim, doublereal *arrto);
17 extern int qmini(double *init, double *final, double frac, double *qintrp);
18 
19 /* $Procedure CK3SDN( Down sample type 3 CK data prepared for writing ) */
20 /* Subroutine */
21 int ck3sdn(double sdntol, bool avflag, int *nrec,
22  double *sclkdp, double *quats, double *avvs,
23  int nints, double *starts, double *dparr, int *intarr) {
24  /* System generated locals */
25  int i__1, i__2;
26 
27  /* Local variables */
28  doublereal frac, dneg;
29  integer left;
30  doublereal dpos, dist2;
31  integer i__, j;
32  doublereal angle;
33  integer keepf;
34  integer keepl;
35  doublereal qlneg[4];
36  logical fitok;
37  integer right;
38  doublereal dist2a, dist2b;
39  doublereal qkeepf[4];
40  doublereal qkeepl[4];
41  integer intcrf, ndropd, intcrl;
42  integer intnrf;
43  logical skipit;
44  doublereal qlinpt[4], qintrp[4];
45 
46  /* $ Abstract */
47 
48  /* Down sample type 3 CK data prepared for writing. */
49 
50  /* $ Disclaimer */
51 
52  /* THIS SOFTWARE AND ANY RELATED MATERIALS WERE CREATED BY THE */
53  /* CALIFORNIA INSTITUTE OF TECHNOLOGY (CALTECH) UNDER A U.S. */
54  /* GOVERNMENT CONTRACT WITH THE NATIONAL AERONAUTICS AND SPACE */
55  /* ADMINISTRATION (NASA). THE SOFTWARE IS TECHNOLOGY AND SOFTWARE */
56  /* PUBLICLY AVAILABLE UNDER U.S. EXPORT LAWS AND IS PROVIDED "AS-IS" */
57  /* TO THE RECIPIENT WITHOUT WARRANTY OF ANY KIND, INCLUDING ANY */
58  /* WARRANTIES OF PERFORMANCE OR MERCHANTABILITY OR FITNESS FOR A */
59  /* PARTICULAR USE OR PURPOSE (AS SET FORTH IN UNITED STATES UCC */
60  /* SECTIONS 2312-2313) OR FOR ANY PURPOSE WHATSOEVER, FOR THE */
61  /* SOFTWARE AND RELATED MATERIALS, HOWEVER USED. */
62 
63  /* IN NO EVENT SHALL CALTECH, ITS JET PROPULSION LABORATORY, OR NASA */
64  /* BE LIABLE FOR ANY DAMAGES AND/OR COSTS, INCLUDING, BUT NOT */
65  /* LIMITED TO, INCIDENTAL OR CONSEQUENTIAL DAMAGES OF ANY KIND, */
66  /* INCLUDING ECONOMIC DAMAGE OR INJURY TO PROPERTY AND LOST PROFITS, */
67  /* REGARDLESS OF WHETHER CALTECH, JPL, OR NASA BE ADVISED, HAVE */
68  /* REASON TO KNOW, OR, IN FACT, SHALL KNOW OF THE POSSIBILITY. */
69 
70  /* RECIPIENT BEARS ALL RISK RELATING TO QUALITY AND PERFORMANCE OF */
71  /* THE SOFTWARE AND ANY RELATED MATERIALS, AND AGREES TO INDEMNIFY */
72  /* CALTECH AND NASA FOR ALL THIRD-PARTY CLAIMS RESULTING FROM THE */
73  /* ACTIONS OF RECIPIENT IN THE USE OF THE SOFTWARE. */
74 
75  /* $ Required_Reading */
76 
77  /* CK */
78  /* DAF */
79  /* ROTATIONS */
80  /* SCLK */
81 
82  /* $ Keywords */
83 
84  /* POINTING */
85  /* UTILITY */
86 
87  /* $ Declarations */
88  /* $ Brief_I/O */
89 
90  /* Variable I/O Description */
91  /* -------- --- -------------------------------------------------- */
92  /* SDNTOL I Tolerance used for sampling down. */
93  /* AVFLAG I True if angular velocity data is set. */
94  /* NREC I/O Number of pointing records. */
95  /* SCLKDP I/O Encoded SCLK times. */
96  /* QUATS I/O Quaternions representing instrument pointing. */
97  /* AVVS I/O Angular velocity vectors. */
98  /* NINTS I Number of intervals. */
99  /* STARTS I Encoded SCLK interval start times. */
100  /* DPARR I Double precision work array. */
101  /* INTARR I Integer work array. */
102 
103  /* $ Detailed_Input */
104 
105  /* SDNTOL is the angular tolerance, in radians, to be used to */
106  /* down sample the input CK type 3 pointing data. */
107  /* SDNTOL must be a non-negative number. */
108 
109  /* AVFLAG is a logical flag indicating whether or not */
110  /* the angular velocity data should be processed. */
111 
112  /* NREC is the number of pointing instances in the input */
113  /* buffer. */
114 
115  /* SCLKDP are the encoded spacecraft clock times associated with */
116  /* each pointing instance. These times must be strictly */
117  /* increasing. */
118 
119  /* QUATS is the quaternion buffer. */
120 
121  /* AVVS is the angular velocity vector buffer. */
122 
123  /* If AVFLAG is FALSE then this array is ignored by the */
124  /* routine; however it still must be supplied as part of */
125  /* the calling sequence. */
126 
127  /* NINTS is the number of intervals that the pointing instances */
128  /* are partitioned into. */
129 
130  /* STARTS are the start times of each of the interpolation */
131  /* intervals. These times must be strictly increasing */
132  /* and must coincide with times for which the input */
133  /* quaternion buffer contains pointing. */
134 
135  /* DPARR is a double precision work array. */
136 
137  /* INTARR is an integer work array. */
138 
139  /* $ Detailed_Output */
140 
141  /* NREC is the number of pointing instances in the buffer */
142  /* after down sampling. */
143 
144  /* SCLKDP is the encoded spacecraft clock time buffer after */
145  /* down sampling. */
146 
147  /* QUATS is the quaternion buffer after down sampling. */
148 
149  /* AVVS is the angular velocity vector buffer after down */
150  /* sampling. */
151 
152  /* $ Parameters */
153 
154  /* None. */
155 
156  /* $ Exceptions */
157 
158  /* 1) If the number of pointing records is not greater than zero, */
159  /* the error SPICE(INVALIDNUMBEROFRECORDS) is signaled. */
160 
161  /* 2) If the number of interval starts is not greater than zero, */
162  /* the error SPICE(INVALIDNUMBEROFINTERVALS) is signaled. */
163 
164  /* 3) If the number of interval starts is not is not less than */
165  /* or equal to the number of records, the error */
166  /* SPICE(BUFFERSIZESMISMATCH) is signaled. */
167 
168  /* 4) If the first interval start time is not the same as the */
169  /* first record time, the error SPICE(FIRSTRECORDMISMATCH) */
170  /* is signaled. */
171 
172  /* 5) If the down sampling tolerance is not a non-negative number, */
173  /* the error SPICE(BADDOWNSAMPLINGTOL) is signaled. */
174 
175  /* 6) If record times buffer does not contain any of the times */
176  /* from interval start times buffers, the error */
177  /* SPICE(INTERVALSTARTNOTFOUND) is signaled. */
178 
179  /* $ Files */
180 
181  /* None. */
182 
183  /* $ Particulars */
184 
185  /* This routine eliminates from the input quaternion and angular */
186  /* rate buffers all data points for which type 3 CK interpolation */
187  /* between bounding points that are not eliminated would produce */
188  /* result that is within specified tolerance of the input attitude. */
189  /* The elimination, refered to in these comments as "down sampling", */
190  /* is done within each individual interpolation interval (as */
191  /* specified in the input interval starts buffer), with intervals */
192  /* boundaries unchanged. */
193 
194  /* $ Examples */
195 
196  /* Normally this routine would be called immediately before the */
197  /* CKW03 is called and be supplied with the input time, quaternion, */
198  /* angular rate, and interval start buffers that were fully and */
199  /* properly prepared for the CKW03 input, like this: */
200 
201  /* CALL CK3SDN ( SDNTOL, ARFLAG, */
202  /* . NREC, SCLKDP, QUATS, AVVS, NINTS, STARTS, */
203  /* . DPARR, INTARR ) */
204 
205  /* CALL CKW03 ( HANDLE, SCLKDP(1), SCLKDP(NREC), */
206  /* . INSTID, FRMNAM, ARFLAG, SEGID, */
207  /* . NREC, SCLKDP, QUATS, AVVS, NINTS, STARTS ) */
208 
209  /* $ Restrictions */
210 
211  /* None. */
212 
213  /* $ Literature_References */
214 
215  /* None. */
216 
217  /* $ Author_and_Institution */
218 
219  /* N.J. Bachman (JPL) */
220  /* B.V. Semenov (JPL) */
221 
222  /* $ Version */
223 
224  /* - Beta Version 1.1.0, 19-SEP-2005 (BVS)(FST) */
225 
226  /* Incorporated Scott's shrinking window search algorithm to */
227  /* speed up down sampling. */
228 
229  /* - Beta Version 1.0.0, 29-JUL-2005 (BVS)(NJB) */
230 
231  /* -& */
232  /* $ Index_Entries */
233 
234  /* sample down ck type_3 pointing data prepared for writing */
235 
236  /* -& */
237 
238  /* Local variables. */
239 
240 
241  /* SPICELIB functions. */
242 
243 
244  /* Standard SPICE error handling. */
245 
246  if(return_c()) {
247  return 0;
248  }
249  else {
250  chkin_c("CK3SDN");
251  }
252 
253  /* Let's do some sanity checks that needed to make sure that future */
254  /* loops and comparisons don't blow up. First, verify that the */
255  /* number pointing records is greater that zero. */
256 
257  if(*nrec <= 0) {
258  setmsg_c("The number of pointing records must be greater than zero. I"
259  "t was #.");
260  errint_c("#", *nrec);
261  sigerr_c("SPICE(INVALIDNUMBEROFRECORDS)");
262  chkout_c("CK3SDN");
263  return 0;
264  }
265 
266  /* Then, verify that the number intervals is greater that zero. */
267 
268  if(nints <= 0) {
269  setmsg_c("The number of interval starts must be greater than zero. It"
270  " was #.");
271  errint_c("#", nints);
272  sigerr_c("SPICE(INVALIDNUMBEROFINTERVALS)");
273  chkout_c("CK3SDN");
274  return 0;
275  }
276 
277  /* Then, verify that the number intervals is less than or equal to */
278  /* the number of records. */
279 
280  if(nints > *nrec) {
281  setmsg_c("The number of interval starts, #, is not less than or equal"
282  " to the number of records, #.");
283  errint_c("#", nints);
284  errint_c("#", *nrec);
285  sigerr_c("SPICE(BUFFERSIZESMISMATCH)");
286  chkout_c("CK3SDN");
287  return 0;
288  }
289 
290  /* Then verify that the first time in the intervals array is the same */
291  /* as the first time in the records array. */
292 
293  if(sclkdp[0] != starts[0]) {
294  setmsg_c("The first interval start time, #, is not the same as the fi"
295  "rst record time, #.");
296  errdp_c("#", *sclkdp);
297  errdp_c("#", *starts);
298  sigerr_c("SPICE(FIRSTRECORDMISMATCH)");
299  chkout_c("CK3SDN");
300  return 0;
301  }
302 
303  /* Finally verify that input down sampling tolerance is not positive */
304  /* number. */
305 
306  if(sdntol < 0.) {
307  setmsg_c("The down sampling tolerance must be a non-negative number. "
308  "It was #.");
309  errdp_c("#", sdntol);
310  sigerr_c("SPICE(BADDOWNSAMPLINGTOL)");
311  chkout_c("CK3SDN");
312  return 0;
313  }
314 
315  /* This variable will hold to the index of the pointing record that */
316  /* matches the start of the next interval. For the first interval */
317  /* it is set to one. */
318 
319  intnrf = 1;
320 
321  /* We will count the number of points that were dropped. */
322 
323  ndropd = 0;
324 
325  /* Loop through interpolation intervals. */
326 
327  i__1 = nints;
328  for(i__ = 1; i__ <= i__1; ++i__) {
329 
330  /* Assign the index of the pointing record that matches the */
331  /* begin time of this interval. */
332 
333  intcrf = intnrf;
334 
335  /* Find the index of the pointing record that ends this interval. */
336  /* If this the last interval, it is the last pointing record in */
337  /* pointing buffer. */
338 
339  if(i__ == nints) {
340  intcrl = *nrec;
341  }
342  else {
343 
344  /* This is not the last interval. To get its end time we need */
345  /* to find the pointing record that matches the start of the */
346  /* next interval and pick the record before it. */
347 
348  /* First we find index of the pointing record that corresponds */
349  /* to the start of the next interval. */
350 
351  i__2 = *nrec - intcrf + 1;
352  intnrf = bsrchd_c(starts[i__], i__2, &sclkdp[intcrf - 1]);
353  if(intnrf != 0) {
354 
355  /* Found index must be adjusted to be relative to the */
356  /* beginning of the buffer. Currently it is relative to the */
357  /* start of the current interval. */
358 
359  intnrf = intnrf + intcrf - 1;
360 
361  /* The index of the last record belonging to this interval */
362  /* in the found index minus 1. */
363 
364  intcrl = intnrf - 1;
365  }
366  else {
367 
368  /* We did not find such record. The input buffer must have */
369  /* been formed improperly for this to happen. Signal an */
370  /* error. */
371 
372  setmsg_c("Cannot find pointing record with time that matches "
373  "the start time # (encoded SCLK ticks) of the interpo"
374  "lation interval number #.");
375  errdp_c("#", starts[i__]);
376  i__2 = i__ + 1;
377  errint_c("#", i__2);
378  sigerr_c("SPICE(INTERVALSTARTNOTFOUND)");
379  chkout_c("CK3SDN");
380  return 0;
381  }
382  }
383 
384  /* Let's look at the indexes of the pointing records */
385  /* corresponding to the begin and end of this interval. If they */
386  /* are the same (meaning it's a singleton interval) or if they */
387  /* are next to each other (meaning that the whole set of */
388  /* interval's pointing data is comprised of only its begin */
389  /* and end points) there is no down sampling to do. */
390 
391  skipit = intcrf == intcrl || intcrf == intcrl - 1;
392 
393  /* Set initial values for a binary search. */
394 
395  keepf = intcrf;
396  left = intcrf;
397  right = intcrl;
398  while(! skipit && keepf < intcrl) {
399 
400  /* Set the right endpoint of the interval by dividing the */
401  /* binary search region in half. */
402 
403  keepl = (left + right) / 2;
404 
405  /* Unitize bracketing quaternions as QMINI seems to be */
406  /* very sensitive to that. :) */
407 
408  vhatg_c(&quats[(keepf << 2) - 4], c__4, qkeepf);
409  vhatg_c(&quats[(keepl << 2) - 4], c__4, qkeepl);
410 
411  /* Pick the closer of the right quaternion or its negative to */
412  /* QKEEPF for input into QMINI to ensure that QMINI does */
413  /* interpolation in the "shortest arc" direction. */
414 
415  vminug_c(qkeepl, c__4, qlneg);
416  dpos = vdistg_c(qkeepl, qkeepf, c__4);
417  dneg = vdistg_c(qlneg, qkeepf, c__4);
418  if(dneg < dpos) {
419  moved_(qlneg, (integer *) &c__4, qlinpt);
420  }
421  else {
422  moved_(qkeepl, (integer *) &c__4, qlinpt);
423  }
424 
425  /* Check all records between the currently picked window ends */
426  /* to see if interpolated pointing is within tolerance of the */
427  /* actual pointing. */
428 
429  fitok = true;
430  j = keepf + 1;
431  while(j <= keepl - 1 && fitok) {
432 
433  /* Compute interpolation fraction for this pointing record. */
434 
435  if(sclkdp[keepl - 1] - sclkdp[keepf - 1] != 0.) {
436  frac = (sclkdp[j - 1] - sclkdp[keepf - 1]) / (sclkdp[
437  keepl - 1] - sclkdp[keepf - 1]);
438  }
439  else {
440  sigerr_c("SPICE(CK3SDNBUG)");
441  chkout_c("CK3SDN");
442  return 0;
443  }
444 
445  /* Call Nat's fast quaternion interpolation routine to */
446  /* compute interpolated rotation for this point. */
447 
448  qmini(qkeepf, qlinpt, frac, qintrp);
449 
450  /* Find the squared distance between the interpolated */
451  /* and input quaternions. */
452 
453  dist2a = (quats[(j << 2) - 4] - qintrp[0]) * (quats[(j << 2)
454  - 4] - qintrp[0]) + (quats[(j << 2) - 3] - qintrp[1])
455  * (quats[(j << 2) - 3] - qintrp[1]) + (quats[(j << 2)
456  - 2] - qintrp[2]) * (quats[(j << 2) - 2] - qintrp[2])
457  + (quats[(j << 2) - 1] - qintrp[3]) * (quats[(j << 2)
458  - 1] - qintrp[3]);
459  dist2b = (quats[(j << 2) - 4] + qintrp[0]) * (quats[(j << 2)
460  - 4] + qintrp[0]) + (quats[(j << 2) - 3] + qintrp[1])
461  * (quats[(j << 2) - 3] + qintrp[1]) + (quats[(j << 2)
462  - 2] + qintrp[2]) * (quats[(j << 2) - 2] + qintrp[2])
463  + (quats[(j << 2) - 1] + qintrp[3]) * (quats[(j << 2)
464  - 1] + qintrp[3]);
465  dist2 = min(dist2a, dist2b);
466 
467  /* The rotation angle theta is related to the distance by */
468  /* the formula */
469 
470  /* || Q1 - Q2 || = 2 * | sin(theta/4) | */
471 
472  angle = asin(sqrt(dist2) / 2.) * 4.;
473 
474  /* Compare the angle with specified threshold. */
475 
476  fitok = fitok && abs(angle) <= sdntol;
477 
478  /* Increment index to move to the next record. */
479 
480  ++j;
481  }
482 
483  /* Was the fit OK? */
484 
485  if(fitok) {
486 
487  /* Fit was OK. Check if left and right are equal; if so we */
488  /* found the point that were were looking for. */
489 
490  if(left == right) {
491 
492  /* Mark all records between fist and last with DPMAX. */
493 
494  i__2 = keepl - 1;
495  for(j = keepf + 1; j <= i__2; ++j) {
496  sclkdp[j - 1] = dpmax_c();
497  ++ndropd;
498  }
499 
500  /* Set first point for the next search to be equal to */
501  /* the to the found point. */
502 
503  keepf = keepl;
504 
505  /* Reset window boundaries for binary search. */
506 
507  left = keepl;
508  right = intcrl;
509  }
510  else {
511 
512  /* Left and right sides haven't converged yet; shift */
513  /* left side of the binary search window forward. */
514 
515  left = keepl + 1;
516  }
517  }
518  else {
519 
520  /* No fit; shift right side of the binary search window */
521  /* backwards. */
522 
523  right = keepl - 1;
524 
525  /* If right side when "over" the left side, set left side */
526  /* to be equal to the right side. */
527 
528  if(right < left) {
529  left = right;
530  }
531  }
532  }
533  }
534 
535  /* At this point all records that are to be removed, if any, have */
536  /* been "tagged" with DPMAX in the times buffer. We need to re-sort */
537  /* the buffers to push these records to the bottom and re-set the */
538  /* number of records to indicate that only the top portion should be */
539  /* used. */
540 
541  if(ndropd != 0) {
542 
543  /* Since SCLKs were the ones "marked" by DPMAX, we will use them */
544  /* to get the order vector. */
545 
546  orderd_c(sclkdp, *nrec, (SpiceInt *) intarr);
547 
548  /* Now, with the order vector in hand, sort the SCLKs ... */
549 
550  reordd_c(intarr, *nrec, sclkdp);
551 
552  /* ... then sort quaternions (element by element) ... */
553 
554  for(i__ = 0; i__ <= 3; ++i__) {
555  i__1 = *nrec;
556  for(j = 1; j <= i__1; ++j) {
557  dparr[j - 1] = quats[i__ + (j << 2) - 4];
558  }
559  reordd_c(intarr, *nrec, dparr);
560  i__1 = *nrec;
561  for(j = 1; j <= i__1; ++j) {
562  quats[i__ + (j << 2) - 4] = dparr[j - 1];
563  }
564  }
565 
566  /* ... and, finally, if requested, sort AVs (also element by */
567  /* element) ... */
568 
569  if(avflag) {
570  for(i__ = 1; i__ <= 3; ++i__) {
571  i__1 = *nrec;
572  for(j = 1; j <= i__1; ++j) {
573  dparr[j - 1] = avvs[i__ + j * 3 - 4];
574  }
575  reordd_c(intarr, *nrec, dparr);
576  i__1 = *nrec;
577  for(j = 1; j <= i__1; ++j) {
578  avvs[i__ + j * 3 - 4] = dparr[j - 1];
579  }
580  }
581  }
582 
583  /* Reset the number of points. */
584 
585  *nrec -= ndropd;
586  }
587 
588  /* All done. Check out. */
589 
590  chkout_c("CK3SDN");
591  return 0;
592 } /* ck3sdn_ */
593 
std
Namespace for the standard library.
Isis
This is free and unencumbered software released into the public domain.
Definition: Apollo.h:16