Isis 3 Programmer Reference
ck3sdn.cpp
1#include <cfloat>
2#include <cmath>
3#include <iomanip>
4
5#include "Camera.h"
6
7using namespace std;
8using namespace Isis;
9//#include "f2c.h"
10
11/* Table of constant values */
12
13static int c__4 = 4;
14/* Builtin functions */
15// double sqrt(doublereal), asin(doublereal);
16extern int moved_(doublereal *arrfrm, integer *ndim, doublereal *arrto);
17extern 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 */
21int 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
Isis
This is free and unencumbered software released into the public domain.
Definition Apollo.h:16
std
Namespace for the standard library.