Isis 3 Programmer Reference
SpiceRotation.cpp
1
6/* SPDX-License-Identifier: CC0-1.0 */
7#include "SpiceRotation.h"
8
9#include <algorithm>
10#include <cfloat>
11#include <cmath>
12#include <iomanip>
13#include <string>
14#include <vector>
15
16#include <QDebug>
17#include <QString>
18#include <SpiceUsr.h>
19#include <SpiceZfc.h>
20#include <SpiceZmc.h>
21
22#include "BasisFunction.h"
23#include "IException.h"
24#include "IString.h"
25#include "LeastSquares.h"
26#include "LineEquation.h"
27#include "NaifStatus.h"
28#include "PolynomialUnivariate.h"
29#include "Quaternion.h"
30#include "Table.h"
31#include "TableField.h"
32
33using json = nlohmann::json;
34
35// Declarations for bindings for Naif Spicelib routines that do not have
36// a wrapper
37extern int refchg_(integer *frame1, integer *frame2, doublereal *et,
38 doublereal *rotate);
39extern int frmchg_(integer *frame1, integer *frame2, doublereal *et,
40 doublereal *rotate);
41extern int invstm_(doublereal *mat, doublereal *invmat);
42// Temporary declarations for bindings for Naif supportlib routines
43
44// These three declarations should be removed once supportlib is in Isis3
45extern int ck3sdn(double sdntol, bool avflag, int *nrec,
46 double *sclkdp, double *quats, double *avvs,
47 int nints, double *starts, double *dparr,
48 int *intarr);
49
50namespace Isis {
59 p_constantFrames.push_back(frameCode);
60 p_timeBias = 0.0;
62 p_CJ.resize(9);
63 p_matrixSet = false;
64 p_et = -DBL_MAX;
65 p_degree = 2;
66 p_degreeApplied = false;
67 p_noOverride = true;
68 p_axis1 = 3;
69 p_axis2 = 1;
70 p_axis3 = 3;
73 p_av.resize(3);
79 m_orientation = NULL;
80 }
81
82
93 SpiceRotation::SpiceRotation(int frameCode, int targetCode) {
95
96 p_constantFrames.push_back(frameCode);
97 p_targetCode = targetCode;
98 p_timeBias = 0.0;
100 p_CJ.resize(9);
101 p_matrixSet = false;
102 p_et = -DBL_MAX;
103 p_axisP = 3;
104 p_degree = 2;
105 p_degreeApplied = false;
106 p_noOverride = true;
107 p_axis1 = 3;
108 p_axis2 = 1;
109 p_axis3 = 3;
111 p_hasAngularVelocity = false;
112 p_av.resize(3);
115
116 p_fullCacheSize = 0;
117 m_frameType = DYN;
119 m_orientation = NULL;
120
121 // Determine the axis for the velocity vector
122 QString key = "INS" + toString(frameCode) + "_TRANSX";
123 SpiceDouble transX[2];
124 SpiceInt number;
125 SpiceBoolean found;
126 //Read starting at element 1 (skipping element 0)
127 gdpool_c(key.toLatin1().data(), 1, 2, &number, transX, &found);
128
129 if (!found) {
130 QString msg = "Cannot find [" + key + "] in text kernels";
131 throw IException(IException::Io, msg, _FILEINFO_);
132 }
133
134 p_axisV = 2;
135 if (transX[0] < transX[1]) p_axisV = 1;
136
138 }
139
140
147 p_cacheTime = rotToCopy.p_cacheTime;
148 p_av = rotToCopy.p_av;
149 p_degree = rotToCopy.p_degree;
150 p_axis1 = rotToCopy.p_axis1;
151 p_axis2 = rotToCopy.p_axis2;
152 p_axis3 = rotToCopy.p_axis3;
153
154 p_constantFrames = rotToCopy.p_constantFrames;
155 p_timeFrames = rotToCopy.p_timeFrames;
156 p_timeBias = rotToCopy.p_timeBias;
157
158 p_et = rotToCopy.p_et;
159 p_quaternion = rotToCopy.p_quaternion;
160 p_matrixSet = rotToCopy.p_matrixSet;
161 p_source = rotToCopy.p_source;
162 p_axisP = rotToCopy.p_axisP;
163 p_axisV = rotToCopy.p_axisV;
164 p_targetCode = rotToCopy.p_targetCode;
165 p_baseTime = rotToCopy.p_baseTime;
166 p_timeScale = rotToCopy.p_timeScale;
167 p_degreeApplied = rotToCopy.p_degreeApplied;
168
169// for (std::vector<double>::size_type i = 0; i < rotToCopy.p_coefficients[0].size(); i++)
170 for (int i = 0; i < 3; i++)
171 p_coefficients[i] = rotToCopy.p_coefficients[i];
172
173 p_noOverride = rotToCopy.p_noOverride;
174 p_overrideBaseTime = rotToCopy.p_overrideBaseTime;
175 p_overrideTimeScale = rotToCopy.p_overrideTimeScale;
176 p_minimizeCache = rotToCopy.p_minimizeCache;
177 p_fullCacheStartTime = rotToCopy.p_fullCacheStartTime;
178 p_fullCacheEndTime = rotToCopy.p_fullCacheEndTime;
179 p_fullCacheSize = rotToCopy.p_fullCacheSize;
180 p_TC = rotToCopy.p_TC;
181
182 p_CJ = rotToCopy.p_CJ;
183 p_degree = rotToCopy.p_degree;
184 p_hasAngularVelocity = rotToCopy.p_hasAngularVelocity;
185 m_frameType = rotToCopy.m_frameType;
186
187 if (rotToCopy.m_orientation) {
188 m_orientation = new ale::Orientations;
189 *m_orientation = *rotToCopy.m_orientation;
190 }
191 else {
192 m_orientation = NULL;
193 }
194 }
195
196
201 if (m_orientation) {
202 delete m_orientation;
203 m_orientation = NULL;
204 }
205 }
206
207
214 void SpiceRotation::SetFrame(int frameCode) {
215 p_constantFrames[0] = frameCode;
216 }
217
218
226 return p_constantFrames[0];
227 }
228
229
242 void SpiceRotation::SetTimeBias(double timeBias) {
243 p_timeBias = timeBias;
244 }
245
246
261
262 // Save the time
263 if (p_et == et) return;
264 p_et = et;
265
266 // Read from the cache
267 if (p_source == Memcache) {
269 }
270
271 // Apply coefficients defining a function for each of the three camera angles and angular
272 // velocity if available
273 else if (p_source == PolyFunction) {
275 }
276 // Apply coefficients defining a function for each of the three camera angles and angular
277 // velocity if available
278 else if (p_source == PolyFunctionOverSpice) {
280 }
281 // Read from the kernel
282 else if (p_source == Spice) {
284 // Retrieve the J2000 (code=1) to reference rotation matrix
285 }
286 // Apply coefficients from PCK version of IAU solution for target body orientation and angular
287 // velocity???
288 else if (p_source == PckPolyFunction) {
290 }
291 // Compute from Nadir
292 else {
294 }
295
296 // Set the quaternion for this rotation
297// p_quaternion.Set ( p_CJ );
298 }
299
300
307 return p_et;
308 }
309
310
317 return (m_orientation != NULL);
318 }
319
320
329
330
349 void SpiceRotation::LoadCache(double startTime, double endTime, int size) {
350
351 // Check for valid arguments
352 if (size <= 0) {
353 QString msg = "Argument cacheSize must not be less or equal to zero";
354 throw IException(IException::Programmer, msg, _FILEINFO_);
355 }
356
357 if (startTime > endTime) {
358 QString msg = "Argument startTime must be less than or equal to endTime";
359 throw IException(IException::Programmer, msg, _FILEINFO_);
360 }
361
362 if ((startTime != endTime) && (size == 1)) {
363 QString msg = "Cache size must be more than 1 if startTime and endTime differ";
364 throw IException(IException::Programmer, msg, _FILEINFO_);
365 }
366
367 // Make sure cache isn't already loaded
368 if (p_source == Memcache) {
369 QString msg = "A SpiceRotation cache has already been created";
370 throw IException(IException::Programmer, msg, _FILEINFO_);
371 }
372
373 // Save full cache parameters
374 p_fullCacheStartTime = startTime;
375 p_fullCacheEndTime = endTime;
376 p_fullCacheSize = size;
377
378 if (m_orientation != NULL) {
379 delete m_orientation;
380 m_orientation = NULL;
381 }
382
383 // Make sure the constant frame is loaded. This method also does the frame trace.
384 if (p_timeFrames.size() == 0) InitConstantRotation(startTime);
385
386 // Set the frame type. If the frame class is PCK, load the constants.
387 if (p_source == Spice) {
388 setFrameType();
389 }
390
392 int cacheSize = p_cacheTime.size();
393
394 // Loop and load the cache
395 std::vector<ale::Rotation> rotationCache;
396 std::vector< std::vector<double> > cache;
397 std::vector<ale::Vec3d> avCache;
398 for (int i = 0; i < cacheSize; i++) {
399 double et = p_cacheTime[i];
401 rotationCache.push_back(ale::Rotation(p_CJ));
402 cache.push_back(p_CJ);
403
405 avCache.push_back(ale::Vec3d(p_av));
406 }
407 }
408
409 if (p_TC.size() > 1) {
410 m_orientation = new ale::Orientations(rotationCache, p_cacheTime, avCache,
411 ale::Rotation(p_TC), p_constantFrames, p_timeFrames);
412 }
413 else {
414 m_orientation = new ale::Orientations(rotationCache, p_cacheTime, avCache,
415 ale::Rotation(1,0,0,0), p_constantFrames, p_timeFrames);
416 }
417
419
420 // Downsize already loaded caches (both time and quats)
421 if (p_minimizeCache == Yes && cacheSize > 5) {
423 }
424 }
425
426
440 void SpiceRotation::LoadCache(double time) {
441 LoadCache(time, time, 1);
442 }
443
444
454 void SpiceRotation::LoadCache(json &isdRot){
455 if (p_source != Spice) {
456 throw IException(IException::Programmer, "SpiceRotation::LoadCache(json) only supports Spice source", _FILEINFO_);
457 }
458
459 p_timeFrames.clear();
460 p_TC.clear();
461 p_cacheTime.clear();
462 p_hasAngularVelocity = false;
463 m_frameType = CK;
464
465 if (m_orientation) {
466 delete m_orientation;
467 m_orientation = NULL;
468 }
469
470 // Load the full cache time information from the label if available
471 p_fullCacheStartTime = isdRot["ck_table_start_time"].get<double>();
472 p_fullCacheEndTime = isdRot["ck_table_end_time"].get<double>();
473 p_fullCacheSize = isdRot["ck_table_original_size"].get<double>();
474 p_cacheTime = isdRot["ephemeris_times"].get<std::vector<double>>();
475 p_timeFrames = isdRot["time_dependent_frames"].get<std::vector<int>>();
476
477 std::vector<ale::Rotation> rotationCache;
478 for (auto it = isdRot["quaternions"].begin(); it != isdRot["quaternions"].end(); it++) {
479 std::vector<double> quat = {it->at(0).get<double>(), it->at(1).get<double>(), it->at(2).get<double>(), it->at(3).get<double>()};
480 Quaternion q(quat);
481 std::vector<double> CJ = q.ToMatrix();
482 rotationCache.push_back(ale::Rotation(CJ));
483 }
484
485 std::vector<ale::Vec3d> avCache;
486 if (isdRot["angular_velocities"].size() != 0) {
487 for (auto it = isdRot["angular_velocities"].begin(); it != isdRot["angular_velocities"].end(); it++) {
488 std::vector<double> av = {it->at(0).get<double>(), it->at(1).get<double>(), it->at(2).get<double>()};
489 avCache.push_back(ale::Vec3d(av));
490 }
492 }
493
494 if (isdRot.contains("constant_frames")) {
495 p_constantFrames = isdRot["constant_frames"].get<std::vector<int>>();
496 p_TC = isdRot["constant_rotation"].get<std::vector<double>>();
497 m_orientation = new ale::Orientations(rotationCache, p_cacheTime, avCache,
498 ale::Rotation(p_TC), p_constantFrames, p_timeFrames);
499 }
500 else {
501 p_TC.resize(9);
502 ident_c((SpiceDouble( *)[3]) &p_TC[0]);
503 m_orientation = new ale::Orientations(rotationCache, p_cacheTime, avCache,
504 ale::Rotation(1,0,0,0), p_constantFrames, p_timeFrames);
505 }
506
507
510 }
511
512
538 // Clear any existing cached data to make it reentrant (KJB 2011-07-20).
539 p_timeFrames.clear();
540 p_TC.clear();
541 p_cacheTime.clear();
542
543 p_hasAngularVelocity = false;
544
545 if (m_orientation) {
546 delete m_orientation;
547 m_orientation = NULL;
548 }
549
550 // Load the constant and time-based frame traces and the constant rotation
551 if (table.Label().hasKeyword("TimeDependentFrames")) {
552 PvlKeyword labelTimeFrames = table.Label()["TimeDependentFrames"];
553 for (int i = 0; i < labelTimeFrames.size(); i++) {
554 p_timeFrames.push_back(toInt(labelTimeFrames[i]));
555 }
556 }
557 else {
558 p_timeFrames.push_back(p_constantFrames[0]);
559 p_timeFrames.push_back(J2000Code);
560 }
561
562 if (table.Label().hasKeyword("ConstantRotation")) {
563 PvlKeyword labelConstantFrames = table.Label()["ConstantFrames"];
565
566 for (int i = 0; i < labelConstantFrames.size(); i++) {
567 p_constantFrames.push_back(toInt(labelConstantFrames[i]));
568 }
569 PvlKeyword labelConstantRotation = table.Label()["ConstantRotation"];
570
571 for (int i = 0; i < labelConstantRotation.size(); i++) {
572 p_TC.push_back(toDouble(labelConstantRotation[i]));
573 }
574 }
575 else {
576 p_TC.resize(9);
577 ident_c((SpiceDouble( *)[3]) &p_TC[0]);
578 }
579
580 // Load the full cache time information from the label if available
581 if (table.Label().hasKeyword("CkTableStartTime")) {
582 p_fullCacheStartTime = toDouble(table.Label().findKeyword("CkTableStartTime")[0]);
583 }
584 if (table.Label().hasKeyword("CkTableEndTime")) {
585 p_fullCacheEndTime = toDouble(table.Label().findKeyword("CkTableEndTime")[0]);
586 }
587 if (table.Label().hasKeyword("CkTableOriginalSize")) {
588 p_fullCacheSize = toInt(table.Label().findKeyword("CkTableOriginalSize")[0]);
589 }
590
591 // Load FrameTypeCode from labels if available and the planetary constants keywords
592 if (table.Label().hasKeyword("FrameTypeCode")) {
593 m_frameType = (FrameType) toInt(table.Label().findKeyword("FrameTypeCode")[0]);
594 }
595 else {
597 }
598
599 if (m_frameType == PCK) {
600 loadPCFromTable(table.Label());
601 }
602
603 int recFields = table[0].Fields();
604
605 // Loop through and move the table to the cache. Retrieve the first record to
606 // establish the type of cache and then use the appropriate loop.
607
608 // list table of quaternion and time
609
610 std::vector<ale::Rotation> rotationCache;
611 std::vector<ale::Vec3d> avCache;
612 if (recFields == 5) {
613 for (int r = 0; r < table.Records(); r++) {
614 TableRecord &rec = table[r];
615
616 if (rec.Fields() != recFields) {
617 // throw and error
618 }
619
620 std::vector<double> j2000Quat;
621 j2000Quat.push_back((double)rec[0]);
622 j2000Quat.push_back((double)rec[1]);
623 j2000Quat.push_back((double)rec[2]);
624 j2000Quat.push_back((double)rec[3]);
625
626 Quaternion q(j2000Quat);
627 std::vector<double> CJ = q.ToMatrix();
628 rotationCache.push_back(ale::Rotation(CJ));
629
630 p_cacheTime.push_back((double)rec[4]);
631 }
632 if (p_TC.size() > 1) {
633 m_orientation = new ale::Orientations(rotationCache, p_cacheTime, avCache,
634 ale::Rotation(p_TC), p_constantFrames, p_timeFrames);
635 }
636 else {
637 m_orientation = new ale::Orientations(rotationCache, p_cacheTime, avCache,
638 ale::Rotation(1,0,0,0), p_constantFrames, p_timeFrames);
639 }
641 }
642
643 // list table of quaternion, angular velocity vector, and time
644 else if (recFields == 8) {
645 for (int r = 0; r < table.Records(); r++) {
646 TableRecord &rec = table[r];
647
648 if (rec.Fields() != recFields) {
649 // throw and error
650 }
651
652 std::vector<double> j2000Quat;
653 j2000Quat.push_back((double)rec[0]);
654 j2000Quat.push_back((double)rec[1]);
655 j2000Quat.push_back((double)rec[2]);
656 j2000Quat.push_back((double)rec[3]);
657
658
659 Quaternion q(j2000Quat);
660 std::vector<double> CJ = q.ToMatrix();
661 rotationCache.push_back(ale::Rotation(CJ));
662
663 std::vector<double> av;
664 av.push_back((double)rec[4]);
665 av.push_back((double)rec[5]);
666 av.push_back((double)rec[6]);
667 avCache.push_back(ale::Vec3d(av));
668 p_cacheTime.push_back((double)rec[7]);
670 }
671
672 if (p_TC.size() > 1) {
673 m_orientation = new ale::Orientations(rotationCache, p_cacheTime, avCache,
674 ale::Rotation(p_TC), p_constantFrames, p_timeFrames);
675 }
676 else {
677 m_orientation = new ale::Orientations(rotationCache, p_cacheTime, avCache,
678 ale::Rotation(1,0,0,0), p_constantFrames, p_timeFrames);
679 }
681 }
682
683 // coefficient table for angle1, angle2, and angle3
684 else if (recFields == 3) {
685 std::vector<double> coeffAng1, coeffAng2, coeffAng3;
686
687 for (int r = 0; r < table.Records() - 1; r++) {
688 TableRecord &rec = table[r];
689
690 if (rec.Fields() != recFields) {
691 // throw an error
692 }
693 coeffAng1.push_back((double)rec[0]);
694 coeffAng2.push_back((double)rec[1]);
695 coeffAng3.push_back((double)rec[2]);
696 }
697
698 // Take care of time parameters
699 TableRecord &rec = table[table.Records()-1];
700 double baseTime = (double)rec[0];
701 double timeScale = (double)rec[1];
702 double degree = (double)rec[2];
703 SetPolynomialDegree((int) degree);
704 SetOverrideBaseTime(baseTime, timeScale);
705 SetPolynomial(coeffAng1, coeffAng2, coeffAng3);
707 if (degree > 0) p_hasAngularVelocity = true;
708 if (degree == 0 && m_orientation->getAngularVelocities().size() > 0) {
710 }
711 }
712 else {
713 QString msg = "Expecting either three, five, or eight fields in the SpiceRotation table";
714 throw IException(IException::Programmer, msg, _FILEINFO_);
715 }
716 }
717
718
729 // Save current et
730 double et = p_et;
731 p_et = -DBL_MAX;
732
733 std::vector<ale::Rotation> rotationCache;
734 std::vector<ale::Vec3d> avCache;
735 if (p_source == PolyFunction) {
736 // Clear existing matrices from cache
737 p_cacheTime.clear();
738
739 // Load the time cache first
742
743 if (p_fullCacheSize > 1) {
744 // Load the matrix and av caches
745 for (std::vector<double>::size_type pos = 0; pos < p_cacheTime.size(); pos++) {
747 rotationCache.push_back(ale::Rotation(p_CJ));
748 avCache.push_back(ale::Vec3d(p_av));
749 }
750 }
751 else {
752 // Load the matrix for the single updated time instance
754 rotationCache.push_back(ale::Rotation(p_CJ));
755 avCache.push_back(ale::Vec3d(p_av));
756 }
757 }
758 else if (p_source == PolyFunctionOverSpice) {
759 SpiceRotation tempRot(*this);
760 std::vector<double>::size_type maxSize = p_fullCacheSize;
761
762 // Clear the existing caches
763 p_cacheTime.clear();
764
765 // Reload the time cache first
768
769 for (std::vector<double>::size_type pos = 0; pos < maxSize; pos++) {
770 tempRot.SetEphemerisTime(p_cacheTime.at(pos));
771 std::vector<double> CJ = tempRot.TimeBasedMatrix();
772 rotationCache.push_back(ale::Rotation(CJ));
774 avCache.push_back(ale::Vec3d(tempRot.AngularVelocity()));
775 }
776 }
777 }
778 else { //(p_source < PolyFunction)
779 QString msg = "The SpiceRotation has not yet been fit to a function";
780 throw IException(IException::Programmer, msg, _FILEINFO_);
781 }
782
783 if (m_orientation) {
784 delete m_orientation;
785 m_orientation = NULL;
786 }
787
788 if (p_TC.size() > 1) {
789 m_orientation = new ale::Orientations(rotationCache, p_cacheTime, avCache,
790 ale::Rotation(p_TC), p_constantFrames, p_timeFrames);
791 }
792 else {
793 m_orientation = new ale::Orientations(rotationCache, p_cacheTime, avCache,
794 ale::Rotation(1,0,0,0), p_constantFrames, p_timeFrames);
795 }
796
797 // Set source to cache and reset current et
798 // Make sure source is Memcache now
800 p_et = -DBL_MAX;
802 }
803
804
819 Table SpiceRotation::LineCache(const QString &tableName) {
820
821 // Apply the function and fill the caches
823
824 if (p_source != Memcache) {
825 QString msg = "Only cached rotations can be returned as a line cache of quaternions and time";
826 throw IException(IException::Programmer, msg, _FILEINFO_);
827 }
828 // Load the table and return it to caller
829 return Cache(tableName);
830 }
831
832
855 Table SpiceRotation::Cache(const QString &tableName) {
856 // First handle conversion of PolyFunctionOverSpiceConstant
857 // by converting it to the full Memcache and try to downsize it
859 LineCache(tableName);
860
861 //std::cout << "Full cache size is " << p_cache.size() << endl;
864
865 //std::cout << "Minimized cache size is " << p_cache.size() << endl;
866 }
867
868 // Load the list of rotations and their corresponding times
869 if (p_source == Memcache) {
870 TableField q0("J2000Q0", TableField::Double);
871 TableField q1("J2000Q1", TableField::Double);
872 TableField q2("J2000Q2", TableField::Double);
873 TableField q3("J2000Q3", TableField::Double);
875
876 TableRecord record;
877 record += q0;
878 record += q1;
879 record += q2;
880 record += q3;
881 int timePos = 4;
882
884 TableField av1("AV1", TableField::Double);
885 TableField av2("AV2", TableField::Double);
886 TableField av3("AV3", TableField::Double);
887 record += av1;
888 record += av2;
889 record += av3;
890 timePos = 7;
891 }
892
893 record += t;
894 Table table(tableName, record);
895
896 std::vector<ale::Rotation> rots = m_orientation->getRotations();
897 std::vector<ale::Vec3d> angularVelocities = m_orientation->getAngularVelocities();
898 for (int i = 0; i < (int) p_cacheTime.size(); i++) {
899 std::vector<double> quat = rots[i].toQuaternion();
900
901 // If the first component is less than zero, multiply the whole quaternion by -1. This
902 // matches NAIF.
903 if (quat[0] < 0) {
904 quat[0] = -1 * quat[0];
905 quat[1] = -1 * quat[1];
906 quat[2] = -1 * quat[2];
907 quat[3] = -1 * quat[3];
908 }
909
910 record[0] = quat[0];
911 record[1] = quat[1];
912 record[2] = quat[2];
913 record[3] = quat[3];
914
915 if (angularVelocities.size() > 0 && p_hasAngularVelocity ) {
916 ale::Vec3d angularVelocity = angularVelocities[i];
917 record[4] = angularVelocity.x;
918 record[5] = angularVelocity.y;
919 record[6] = angularVelocity.z;
920 }
921
922 record[timePos] = p_cacheTime[i];
923 table += record;
924 }
925
926 CacheLabel(table);
927 return table;
928 }
929 // Just load the position for the single epoch
930 else if (p_source == PolyFunction && p_degree == 0 && p_fullCacheSize == 1) {
931 return LineCache(tableName);
932 }
933 // Load the coefficients for the curves fit to the 3 camera angles
934 else if (p_source == PolyFunction) {
935 TableField angle1("J2000Ang1", TableField::Double);
936 TableField angle2("J2000Ang2", TableField::Double);
937 TableField angle3("J2000Ang3", TableField::Double);
938
939 TableRecord record;
940 record += angle1;
941 record += angle2;
942 record += angle3;
943
944 Table table(tableName, record);
945
946 for (int cindex = 0; cindex < p_degree + 1; cindex++) {
947 record[0] = p_coefficients[0][cindex];
948 record[1] = p_coefficients[1][cindex];
949 record[2] = p_coefficients[2][cindex];
950 table += record;
951 }
952
953 // Load one more table entry with the time adjustments for the fit equation
954 // t = (et - baseTime)/ timeScale
955 record[0] = p_baseTime;
956 record[1] = p_timeScale;
957 record[2] = (double) p_degree;
958
959 table += record;
960 CacheLabel(table);
961 return table;
962 }
963 else {
964 // throw an error -- should not get here -- invalid Spice Source
965 QString msg = "To create table source of data must be either Memcache or PolyFunction";
966 throw IException(IException::Programmer, msg, _FILEINFO_);
967 }
968 }
969
970
978 void SpiceRotation::loadPCFromSpice(int centerBody) {
980 SpiceInt centerBodyCode = (SpiceInt) centerBody;
981
982 // Retrieve the frame class from Naif. We distinguish PCK types into text PCK,
983 // binary PCK, and PCK not referenced to J2000. Isis binary PCK can
984 // not be used to solve for target body orientation because of the variety and
985 // complexity models used with binary PCK. Currently ISIS does not solve for
986 // target body orientation on bodies not referenced to J2000, but it could be
987 // changed to handle that case.
989
990 if (m_frameType == PCK) {
991 // Make sure the reference frame is J2000. We will need to modify FrameTrace and
992 // the pck methods in this class to handle this case. At the time this method was
993 // added, this case is not being used in the pck file. It is mentioned in the Naif required
994 // reading file, pck.req, as a possibility for future pck files.
995 // Look for a reference frame keyword for the body. The default is J2000.
996 QString naifKeyword = "BODY" + toString(centerBodyCode) + "_CONSTANTS_REF_FRAME" ;
997 SpiceInt numExpected;
998 SpiceInt numReturned;
999 SpiceChar naifType;
1000 SpiceDouble relativeFrameCode = 0;
1001 SpiceBoolean found;
1002 dtpool_c(naifKeyword.toLatin1().data(), &found, &numExpected, &naifType);
1003
1004 if (found) {
1005 // Go get the frame name if it is not the default J2000
1006 SpiceDouble relativeFrameCode;
1007 bodvcd_c(centerBodyCode, "CONSTANTS_REF_FRAME", 1,
1008 &numReturned, &relativeFrameCode);
1009 }
1010
1011 if (!found || relativeFrameCode == 1) { // We only work with J2000 relative frames for now
1012
1013 // Make sure the standard coefficients are available for the body code by
1014 // checking for ra
1015 naifKeyword = "BODY" + toString(centerBodyCode) + "_POLE_RA" ;
1016 dtpool_c(naifKeyword.toLatin1().data(), &found, &numExpected, &naifType);
1017
1018 if (found) {
1019 std::vector<SpiceDouble> d(3);
1020 m_raPole.resize(numExpected);
1021 m_decPole.resize(numExpected);
1022 m_pm.resize(numExpected);
1023
1024 bodvcd_c(centerBodyCode, "POLE_RA", numExpected, &numReturned, &d[0]);
1025 m_raPole[0].setDegrees(d[0]);
1026 m_raPole[1].setDegrees(d[1]);
1027 m_raPole[2].setDegrees(d[2]);
1028
1029 bodvcd_c(centerBodyCode, "POLE_DEC", numExpected, &numReturned, &d[0]);
1030 m_decPole[0].setDegrees(d[0]);
1031 m_decPole[1].setDegrees(d[1]);
1032 m_decPole[2].setDegrees(d[2]);
1033
1034 bodvcd_c(centerBodyCode, "PM", numExpected, &numReturned, &d[0]);
1035 m_pm[0].setDegrees(d[0]);
1036 m_pm[1].setDegrees(d[1]);
1037 m_pm[2].setDegrees(d[2]);
1038
1039 // ***TODO*** Get long axis value
1041
1042 // Now check for nutation/precession terms. Check for nut/prec ra values
1043 // first to see if the terms are even used for this body.
1044 naifKeyword = "BODY" + toString(centerBodyCode) + "_NUT_PREC_RA" ;
1045 dtpool_c(naifKeyword.toLatin1().data(), &found, &numReturned, &naifType);
1046 if (found) {
1047 // Get the barycenter (bc) linear coefficients first (2 for each period).
1048 // Then we can get the maximum expected coefficients.
1049 SpiceInt bcCode = centerBodyCode/100; // Ex: bc code for Jupiter (599) & its moons is 5
1050 naifKeyword = "BODY" + toString(bcCode) + "_NUT_PREC_ANGLES" ;
1051 dtpool_c(naifKeyword.toLatin1().data(), &found, &numExpected, &naifType);
1052 std::vector<double>npAngles(numExpected, 0.);
1053 bodvcd_c(bcCode, "NUT_PREC_ANGLES", numExpected, &numReturned, &npAngles[0]);
1054 numExpected /= 2.;
1055 m_raNutPrec.resize(numExpected, 0.);
1056 m_decNutPrec.resize(numExpected, 0.);
1057 m_pmNutPrec.resize(numExpected, 0.);
1058
1059 std::vector<SpiceDouble> angles(numExpected);
1060 bodvcd_c(centerBodyCode, "NUT_PREC_RA", numExpected, &numReturned, &m_raNutPrec[0]);
1061 bodvcd_c(centerBodyCode, "NUT_PREC_DEC", numExpected, &numReturned, &m_decNutPrec[0]);
1062 bodvcd_c(centerBodyCode, "NUT_PREC_PM", numExpected, &numReturned, &m_pmNutPrec[0]);
1063
1064 // Finally get the system linear terms separated into vectors of the constants and the
1065 // linear coefficients
1066
1067 for (int i = 0; i < numExpected; i++) {
1068 m_sysNutPrec0.push_back(Angle(npAngles[i*2], Angle::Degrees));
1069 m_sysNutPrec1.push_back(Angle(npAngles[i*2+1], Angle::Degrees));
1070 }
1071 } // barycenter terms
1072 } // PCK constants available
1073 } // Relative to J2000
1074 else {
1076 }
1077 } // PCK
1078
1080 }
1081
1082
1092
1093 // First clear existing cached data
1094 m_raPole.clear();
1095 m_decPole.clear();
1096 m_pm.clear();
1097 m_raNutPrec.clear();
1098 m_decNutPrec.clear();
1099 m_sysNutPrec0.clear();
1100 m_sysNutPrec1.clear();
1101 int numLoaded = 0;
1102
1103 // Load the PCK coeffcients if they are on the label
1104 if (label.hasKeyword("PoleRa")) {
1105 PvlKeyword labelCoeffs = label["PoleRa"];
1106 for (int i = 0; i < labelCoeffs.size(); i++){
1107 m_raPole.push_back(Angle(toDouble(labelCoeffs[i]), Angle::Degrees));
1108 }
1109 numLoaded += 1;
1110 }
1111 if (label.hasKeyword("PoleDec")) {
1112 PvlKeyword labelCoeffs = label["PoleDec"];
1113 for (int i = 0; i < labelCoeffs.size(); i++){
1114 m_decPole.push_back(Angle(toDouble(labelCoeffs[i]), Angle::Degrees));
1115 }
1116 numLoaded += 1;
1117 }
1118 if (label.hasKeyword("PrimeMeridian")) {
1119 PvlKeyword labelCoeffs = label["PrimeMeridian"];
1120 for (int i = 0; i < labelCoeffs.size(); i++){
1121 m_pm.push_back(Angle(toDouble(labelCoeffs[i]), Angle::Degrees));
1122 }
1123 numLoaded += 1;
1124 }
1125 if (numLoaded > 2) m_tOrientationAvailable = true;
1126
1127 if (label.hasKeyword("PoleRaNutPrec")) {
1128 PvlKeyword labelCoeffs = label["PoleRaNutPrec"];
1129 for (int i = 0; i < labelCoeffs.size(); i++){
1130 m_raNutPrec.push_back(toDouble(labelCoeffs[i]));
1131 }
1132 }
1133 if (label.hasKeyword("PoleDecNutPrec")) {
1134 PvlKeyword labelCoeffs = label["PoleDecNutPrec"];
1135 for (int i = 0; i < labelCoeffs.size(); i++){
1136 m_decNutPrec.push_back(toDouble(labelCoeffs[i]));
1137 }
1138 }
1139 if (label.hasKeyword("PmNutPrec")) {
1140 PvlKeyword labelCoeffs = label["PmNutPrec"];
1141 for (int i = 0; i < labelCoeffs.size(); i++){
1142 m_pmNutPrec.push_back(toDouble(labelCoeffs[i]));
1143 }
1144 }
1145 if (label.hasKeyword("SysNutPrec0")) {
1146 PvlKeyword labelCoeffs = label["SysNutPrec0"];
1147 for (int i = 0; i < labelCoeffs.size(); i++){
1148 m_sysNutPrec0.push_back(Angle(toDouble(labelCoeffs[i]), Angle::Degrees));
1149 }
1150 }
1151 if (label.hasKeyword("SysNutPrec1")) {
1152 PvlKeyword labelCoeffs = label["SysNutPrec1"];
1153 for (int i = 0; i < labelCoeffs.size(); i++){
1154 m_sysNutPrec1.push_back(Angle(toDouble(labelCoeffs[i]), Angle::Degrees));
1155 }
1156 }
1157
1159 }
1160
1161
1171 // Load the constant and time-based frame traces and the constant rotation
1172 // into the table as labels
1173 if (p_timeFrames.size() > 1) {
1174 table.Label() += PvlKeyword("TimeDependentFrames");
1175
1176 for (int i = 0; i < (int) p_timeFrames.size(); i++) {
1177 table.Label()["TimeDependentFrames"].addValue(toString(p_timeFrames[i]));
1178 }
1179 }
1180
1181 if (p_constantFrames.size() > 1) {
1182 table.Label() += PvlKeyword("ConstantFrames");
1183
1184 for (int i = 0; i < (int) p_constantFrames.size(); i++) {
1185 table.Label()["ConstantFrames"].addValue(toString(p_constantFrames[i]));
1186 }
1187
1188 table.Label() += PvlKeyword("ConstantRotation");
1189
1190 for (int i = 0; i < (int) p_TC.size(); i++) {
1191 table.Label()["ConstantRotation"].addValue(toString(p_TC[i]));
1192 }
1193 }
1194
1195 // Write original time coverage
1196 if (p_fullCacheStartTime != 0) {
1197 table.Label() += PvlKeyword("CkTableStartTime");
1198 table.Label()["CkTableStartTime"].addValue(toString(p_fullCacheStartTime));
1199 }
1200 if (p_fullCacheEndTime != 0) {
1201 table.Label() += PvlKeyword("CkTableEndTime");
1202 table.Label()["CkTableEndTime"].addValue(toString(p_fullCacheEndTime));
1203 }
1204 if (p_fullCacheSize != 0) {
1205 table.Label() += PvlKeyword("CkTableOriginalSize");
1206 table.Label()["CkTableOriginalSize"].addValue(toString(p_fullCacheSize));
1207 }
1208
1209 // Begin section added 06-20-2015 DAC
1210 table.Label() += PvlKeyword("FrameTypeCode");
1211 table.Label()["FrameTypeCode"].addValue(toString(m_frameType));
1212
1213 if (m_frameType == PCK) {
1214 // Write out all the body orientation constants
1215 // Pole right ascension coefficients for a quadratic equation
1216 table.Label() += PvlKeyword("PoleRa");
1217
1218 for (int i = 0; i < (int) m_raPole.size(); i++) {
1219 table.Label()["PoleRa"].addValue(toString(m_raPole[i].degrees()));
1220 }
1221
1222 // Pole right ascension, declination coefficients for a quadratic equation
1223 table.Label() += PvlKeyword("PoleDec");
1224 for (int i = 0; i < (int) m_decPole.size(); i++) {
1225 table.Label()["PoleDec"].addValue(toString(m_decPole[i].degrees()));
1226 }
1227
1228 // Prime meridian coefficients for a quadratic equation
1229 table.Label() += PvlKeyword("PrimeMeridian");
1230 for (int i = 0; i < (int) m_pm.size(); i++) {
1231 table.Label()["PrimeMeridian"].addValue(toString(m_pm[i].degrees()));
1232 }
1233
1234 if (m_raNutPrec.size() > 0) {
1235 // Pole right ascension nutation precision coefficients to the trig terms
1236 table.Label() += PvlKeyword("PoleRaNutPrec");
1237 for (int i = 0; i < (int) m_raNutPrec.size(); i++) {
1238 table.Label()["PoleRaNutPrec"].addValue(toString(m_raNutPrec[i]));
1239 }
1240
1241 // Pole declination nutation precision coefficients to the trig terms
1242 table.Label() += PvlKeyword("PoleDecNutPrec");
1243 for (int i = 0; i < (int) m_decNutPrec.size(); i++) {
1244 table.Label()["PoleDecNutPrec"].addValue(toString(m_decNutPrec[i]));
1245 }
1246
1247 // Prime meridian nutation precision coefficients to the trig terms
1248 table.Label() += PvlKeyword("PmNutPrec");
1249 for (int i = 0; i < (int) m_pmNutPrec.size(); i++) {
1250 table.Label()["PmNutPrec"].addValue(toString(m_pmNutPrec[i]));
1251 }
1252
1253 // System nutation precision constant terms of linear model of periods
1254 table.Label() += PvlKeyword("SysNutPrec0");
1255 for (int i = 0; i < (int) m_sysNutPrec0.size(); i++) {
1256 table.Label()["SysNutPrec0"].addValue(toString(m_sysNutPrec0[i].degrees()));
1257 }
1258
1259 // System nutation precision linear terms of linear model of periods
1260 table.Label() += PvlKeyword("SysNutPrec1");
1261 for (int i = 0; i < (int) m_sysNutPrec1.size(); i++) {
1262 table.Label()["SysNutPrec1"].addValue(toString(m_sysNutPrec1[i].degrees()));
1263 }
1264 }
1265 }
1266 // End section added 06-20-2015 DAC
1267
1269 }
1270
1271
1277 std::vector<double> SpiceRotation::GetCenterAngles() {
1278 // Compute the center time
1279 double etCenter = (p_fullCacheEndTime + p_fullCacheStartTime) / 2.;
1280 SetEphemerisTime(etCenter);
1281
1282 return Angles(p_axis3, p_axis2, p_axis1);
1283 }
1284
1285
1296 std::vector<double> SpiceRotation::Angles(int axis3, int axis2, int axis1) {
1298
1299 SpiceDouble ang1, ang2, ang3;
1300 m2eul_c((SpiceDouble *) &p_CJ[0], axis3, axis2, axis1, &ang3, &ang2, &ang1);
1301
1302 std::vector<double> angles;
1303 angles.push_back(ang1);
1304 angles.push_back(ang2);
1305 angles.push_back(ang3);
1306
1308 return angles;
1309 }
1310
1311
1312
1323 void SpiceRotation::SetAngles(std::vector<double> angles, int axis3, int axis2, int axis1) {
1324 eul2m_c(angles[2], angles[1], angles[0], axis3, axis2, axis1, (SpiceDouble (*)[3]) &(p_CJ[0]));
1325
1326 if (m_orientation) {
1327 delete m_orientation;
1328 m_orientation = NULL;
1329 }
1330 std::vector<ale::Rotation> rotationCache;
1331 rotationCache.push_back(ale::Rotation(p_CJ));
1332 if (p_TC.size() > 1) {
1333 m_orientation = new ale::Orientations(rotationCache, p_cacheTime, std::vector<ale::Vec3d>(),
1334 ale::Rotation(p_TC), p_constantFrames, p_timeFrames);
1335 }
1336 else {
1337 m_orientation = new ale::Orientations(rotationCache, p_cacheTime, std::vector<ale::Vec3d>(),
1338 ale::Rotation(1,0,0,0), p_constantFrames, p_timeFrames);
1339 }
1340
1341 // Reset to get the new values
1342 p_et = -DBL_MAX;
1344 }
1345
1346
1352 std::vector<double> SpiceRotation::AngularVelocity() {
1353 return p_av;
1354 }
1355
1356
1364 return p_constantFrames;
1365 }
1366
1367
1374 return p_timeFrames;
1375 }
1376
1377
1386
1387
1395 std::vector<double> SpiceRotation::J2000Vector(const std::vector<double> &rVec) {
1397
1398 std::vector<double> jVec;
1399 if (rVec.size() == 3) {
1400 double TJ[3][3];
1401 mxm_c((SpiceDouble *) &p_TC[0], (SpiceDouble *) &p_CJ[0], TJ);
1402 jVec.resize(3);
1403 mtxv_c(TJ, (SpiceDouble *) &rVec[0], (SpiceDouble *) &jVec[0]);
1404 }
1405
1406 else if (rVec.size() == 6) {
1407 // See Naif routine frmchg for the format of the state matrix. The constant rotation, TC,
1408 // has a derivative with respect to time of I.
1409 if (!p_hasAngularVelocity) {
1410 // throw an error
1411 }
1412 std::vector<double> stateTJ(36);
1413 stateTJ = StateTJ();
1414
1415 // Now invert (inverse of a state matrix is NOT simply the transpose)
1416 xpose6_c(&stateTJ[0], (SpiceDouble( *) [6]) &stateTJ[0]);
1417 double stateJT[6][6];
1418 invstm_((doublereal *) &stateTJ[0], (doublereal *) stateJT);
1419 xpose6_c(stateJT, stateJT);
1420 jVec.resize(6);
1421
1422 mxvg_c(stateJT, (SpiceDouble *) &rVec[0], 6, 6, (SpiceDouble *) &jVec[0]);
1423 }
1425 return (jVec);
1426 }
1427
1428
1444 std::vector<Angle> SpiceRotation::poleRaCoefs() {
1445 return m_raPole;
1446 }
1447
1448
1464 std::vector<Angle> SpiceRotation::poleDecCoefs() {
1465 return m_decPole;
1466 }
1467
1468
1483 std::vector<Angle> SpiceRotation::pmCoefs() {
1484 return m_pm;
1485 }
1486
1487
1502 return m_raNutPrec;
1503 }
1504
1505
1519 return m_decNutPrec;
1520 }
1521
1522
1535 std::vector<double> SpiceRotation::pmNutPrecCoefs() {
1536 return m_pmNutPrec;
1537 }
1538
1539
1553 return m_sysNutPrec0;
1554 }
1555
1556
1570 std::vector<Angle> SpiceRotation::sysNutPrecCoefs() {
1571 return m_sysNutPrec1;
1572 }
1573
1574
1594 std::vector<double> SpiceRotation::toJ2000Partial(const std::vector<double> &lookT,
1595 SpiceRotation::PartialType partialVar,
1596 int coeffIndex) {
1598
1599 std::vector<double> jVec;
1600
1601 // Get the rotation angles and form the derivative matrix for the partialVar
1602 std::vector<double> angles = Angles(p_axis3, p_axis2, p_axis1);
1603 int angleIndex = partialVar;
1604 int axes[3] = {p_axis1, p_axis2, p_axis3};
1605
1606 double angle = angles.at(angleIndex);
1607
1608 // Get TJ and apply the transpose to the input vector to get it to J2000
1609 double dmatrix[3][3];
1610 drotat_(&angle, (integer *) axes + angleIndex, (doublereal *) dmatrix);
1611 // Transpose to obtain row-major order
1612 xpose_c(dmatrix, dmatrix);
1613
1614 // Get the derivative of the polynomial with respect to partialVar
1615 double dpoly = 0.;
1616 QString msg;
1617 switch (m_frameType) {
1618 case CK:
1619 case DYN:
1620 dpoly = DPolynomial(coeffIndex);
1621 break;
1622 case PCK:
1623 dpoly = DPckPolynomial(partialVar, coeffIndex);
1624 break;
1625 case BPC:
1626 msg = "Body rotation uses a binary PCK. Solutions for this model are not supported";
1627 throw IException(IException::User, msg, _FILEINFO_);
1628 break;
1629 case NOTJ2000PCK:
1630 msg = "Body rotation uses a PCK not referenced to J2000. "
1631 "Solutions for this model are not supported";
1632 throw IException(IException::User, msg, _FILEINFO_);
1633 break;
1634 default:
1635 msg = "Solutions are not supported for this frame type.";
1636 throw IException(IException::User, msg, _FILEINFO_);
1637 }
1638
1639 // Multiply dpoly to complete dmatrix
1640 for (int row = 0; row < 3; row++) {
1641 for (int col = 0; col < 3; col++) {
1642 dmatrix[row][col] *= dpoly;
1643 }
1644 }
1645
1646 // Apply the other 2 angles and chain them all together to get J2000 to constant frame
1647 double dCJ[3][3];
1648 switch (angleIndex) {
1649 case 0:
1650 rotmat_c(dmatrix, angles[1], axes[1], dCJ);
1651 rotmat_c(dCJ, angles[2], axes[2], dCJ);
1652 break;
1653 case 1:
1654 rotate_c(angles[0], axes[0], dCJ);
1655 mxm_c(dmatrix, dCJ, dCJ);
1656 rotmat_c(dCJ, angles[2], axes[2], dCJ);
1657 break;
1658 case 2:
1659 rotate_c(angles[0], axes[0], dCJ);
1660 rotmat_c(dCJ, angles[1], axes[1], dCJ);
1661 mxm_c(dmatrix, dCJ, dCJ);
1662 break;
1663 }
1664
1665 // Multiply the constant matrix to rotate to target reference frame
1666 double dTJ[3][3];
1667 mxm_c((SpiceDouble *) &p_TC[0], dCJ[0], dTJ);
1668
1669 // Finally rotate the target vector with the transpose of the
1670 // derivative matrix, dTJ to get a J2000 vector
1671 std::vector<double> lookdJ(3);
1672
1673 mtxv_c(dTJ, (const SpiceDouble *) &lookT[0], (SpiceDouble *) &lookdJ[0]);
1674
1676 return (lookdJ);
1677 }
1678
1679
1687 std::vector<double> SpiceRotation::ReferenceVector(const std::vector<double> &jVec) {
1689
1690 std::vector<double> rVec(3);
1691
1692 if (jVec.size() == 3) {
1693 double TJ[3][3];
1694 mxm_c((SpiceDouble *) &p_TC[0], (SpiceDouble *) &p_CJ[0], TJ);
1695 rVec.resize(3);
1696 mxv_c(TJ, (SpiceDouble *) &jVec[0], (SpiceDouble *) &rVec[0]);
1697 }
1698 else if (jVec.size() == 6) {
1699 // See Naif routine frmchg for the format of the state matrix. The constant rotation, TC,
1700 // has a derivative with respect to time of I.
1701 if (!p_hasAngularVelocity) {
1702 // throw an error
1703 }
1704 std::vector<double> stateTJ(36);
1705 stateTJ = StateTJ();
1706 rVec.resize(6);
1707 mxvg_c((SpiceDouble *) &stateTJ[0], (SpiceDouble *) &jVec[0], 6, 6, (SpiceDouble *) &rVec[0]);
1708 }
1709
1711 return (rVec);
1712 }
1713
1714
1728 std::vector<double> coeffAng1, coeffAng2, coeffAng3;
1729
1730 // Rotation is already stored as a polynomial -- throw an error
1731 if (p_source == PolyFunction) {
1732 // Nothing to do
1733 return;
1734// QString msg = "Rotation already fit to a polynomial -- spiceint first to refit";
1735// throw IException(IException::User,msg,_FILEINFO_);
1736 }
1737
1738 // Adjust degree of polynomial on available data
1739 int size = m_orientation->getRotations().size();
1740 if (size == 1) {
1741 p_degree = 0;
1742 }
1743 else if (size == 2) {
1744 p_degree = 1;
1745 }
1746
1747 //Check for polynomial over original pointing constant and initialize coefficients
1748 if (type == PolyFunctionOverSpice) {
1749 coeffAng1.assign(p_degree + 1, 0.);
1750 coeffAng2.assign(p_degree + 1, 0.);
1751 coeffAng3.assign(p_degree + 1, 0.);
1752 SetPolynomial(coeffAng1, coeffAng2, coeffAng3, type);
1753 return;
1754 }
1755
1759
1760 LeastSquares *fitAng1 = new LeastSquares(function1);
1761 LeastSquares *fitAng2 = new LeastSquares(function2);
1762 LeastSquares *fitAng3 = new LeastSquares(function3);
1763
1764 // Compute the base time
1766 std::vector<double> time;
1767
1768 if (size == 1) {
1769 double t = p_cacheTime.at(0);
1771 std::vector<double> angles = Angles(p_axis3, p_axis2, p_axis1);
1772 coeffAng1.push_back(angles[0]);
1773 coeffAng2.push_back(angles[1]);
1774 coeffAng3.push_back(angles[2]);
1775 }
1776 else if (size == 2) {
1777 // Load the times and get the corresponding rotation angles
1778 p_degree = 1;
1779 double t1 = p_cacheTime.at(0);
1780 SetEphemerisTime(t1);
1781 t1 -= p_baseTime;
1782 t1 = t1 / p_timeScale;
1783 std::vector<double> angles1 = Angles(p_axis3, p_axis2, p_axis1);
1784 double t2 = p_cacheTime.at(1);
1785 SetEphemerisTime(t2);
1786 t2 -= p_baseTime;
1787 t2 = t2 / p_timeScale;
1788 std::vector<double> angles2 = Angles(p_axis3, p_axis2, p_axis1);
1789 angles2[0] = WrapAngle(angles1[0], angles2[0]);
1790 angles2[2] = WrapAngle(angles1[2], angles2[2]);
1791 double slope[3];
1792 double intercept[3];
1793
1794// Compute the linear equation for each angle and save them
1795 for (int angleIndex = 0; angleIndex < 3; angleIndex++) {
1796 Isis::LineEquation angline(t1, angles1[angleIndex], t2, angles2[angleIndex]);
1797 slope[angleIndex] = angline.Slope();
1798 intercept[angleIndex] = angline.Intercept();
1799 }
1800 coeffAng1.push_back(intercept[0]);
1801 coeffAng1.push_back(slope[0]);
1802 coeffAng2.push_back(intercept[1]);
1803 coeffAng2.push_back(slope[1]);
1804 coeffAng3.push_back(intercept[2]);
1805 coeffAng3.push_back(slope[2]);
1806 }
1807 else {
1808 // Load the known values to compute the fit equation
1809 double start1 = 0.; // value of 1st angle1 in cache
1810 double start3 = 0.; // value of 1st angle1 in cache
1811
1812 for (std::vector<double>::size_type pos = 0; pos < p_cacheTime.size(); pos++) {
1813 double t = p_cacheTime.at(pos);
1814 time.push_back((t - p_baseTime) / p_timeScale);
1816 std::vector<double> angles = Angles(p_axis3, p_axis2, p_axis1);
1817
1818// Fix 180/-180 crossovers on angles 1 and 3 before doing fit.
1819 if (pos == 0) {
1820 start1 = angles[0];
1821 start3 = angles[2];
1822 }
1823 else {
1824 angles[0] = WrapAngle(start1, angles[0]);
1825 angles[2] = WrapAngle(start3, angles[2]);
1826 }
1827
1828 fitAng1->AddKnown(time, angles[0]);
1829 fitAng2->AddKnown(time, angles[1]);
1830 fitAng3->AddKnown(time, angles[2]);
1831 time.clear();
1832
1833 }
1834 //Solve the equations for the coefficients
1835 fitAng1->Solve();
1836 fitAng2->Solve();
1837 fitAng3->Solve();
1838
1839 // Delete the least squares objects now that we have all the coefficients
1840 delete fitAng1;
1841 delete fitAng2;
1842 delete fitAng3;
1843
1844 // For now assume all three angles are fit to a polynomial. Later they may
1845 // each be fit to a unique basis function.
1846 // Fill the coefficient vectors
1847
1848 for (int i = 0; i < function1.Coefficients(); i++) {
1849 coeffAng1.push_back(function1.Coefficient(i));
1850 coeffAng2.push_back(function2.Coefficient(i));
1851 coeffAng3.push_back(function3.Coefficient(i));
1852 }
1853
1854 }
1855
1856 // Now that the coefficients have been calculated set the polynomial with them
1857 SetPolynomial(coeffAng1, coeffAng2, coeffAng3);
1858
1860 return;
1861 }
1862
1863
1878 void SpiceRotation::SetPolynomial(const std::vector<double> &coeffAng1,
1879 const std::vector<double> &coeffAng2,
1880 const std::vector<double> &coeffAng3,
1881 const Source type) {
1882
1887
1888 // Load the functions with the coefficients
1889 function1.SetCoefficients(coeffAng1);
1890 function2.SetCoefficients(coeffAng2);
1891 function3.SetCoefficients(coeffAng3);
1892
1893 // Compute the base time
1895
1896 // Save the current coefficients
1897 p_coefficients[0] = coeffAng1;
1898 p_coefficients[1] = coeffAng2;
1899 p_coefficients[2] = coeffAng3;
1900
1901 // Set the flag indicating p_degree has been applied to the camera angles, the
1902 // coefficients of the polynomials have been saved, and the cache reloaded from
1903 // the polynomials
1904 p_degreeApplied = true;
1905 // p_source = PolyFunction;
1906 p_source = type;
1907
1908 // Update the current rotation
1909 double et = p_et;
1910 p_et = -DBL_MAX;
1911 SetEphemerisTime(et);
1912
1914 return;
1915 }
1916
1917
1936
1937 // Check to see if rotation is already stored as a polynomial
1938 if (p_source == PckPolyFunction) {
1939 p_hasAngularVelocity = true;
1940 return;
1941 }
1942
1943 // No target body orientation information available -- throw an error
1945 QString msg = "Target body orientation information not available. Rerun spiceinit.";
1946 throw IException(IException::User, msg, _FILEINFO_);
1947 }
1948
1949 /* I think we have nothing to do, but check for available coefficients and set the Source
1950 // Set the scales
1951 p_dscale = 0.000011574074074; // seconds to days conversion
1952 p_tscale = 3.1688087814029D-10; // seconds to Julian centuries conversion
1953 //Set the quadratic part of the equations
1954 //TODO consider just hard coding this part in evaluate
1955 Isis::PolynomialUnivariate functionRa(p_degree); // Basis function fit to target body pole ra
1956 Isis::PolynomialUnivariate functionDec(p_degree); // Basis function fit to target body pole dec
1957 Isis::PolynomialUnivariate functionPm(p_degree); // Basis function fit to target body pm
1958
1959 // Load the functions with the coefficients
1960 function1.SetCoefficients(p_raPole);
1961 function2.SetCoefficients(p_decPole);
1962 function3.SetCoefficients(p_pm);
1963
1964 int numNutPrec = p_sysNutPrec0.size;
1965 if (numNutPrec > 0) {
1966 // Set the trigonometric part of the equation
1967 Isis::TrigBasis functionRaNutPrec("raNutPrec", Isis::TrigBasis::Sin, numNutPrec);
1968 Isis::TrigBasis functionRaNutPrec("decNutPrec", Isis::TrigBasis::Cos, numNutPrec);
1969 Isis::TrigBasis functionRaNutPrec("pmNutPrec", Isis::TrigBasis::Sin, numNutPrec);
1970 // Load the functions with the coefficients
1971 functionRaNutPrec.SetTCoefficients(p_raNutPrec);
1972 functionDecNutPrec.SetTCoefficients(p_decNutPrec);
1973 functionPmNutPrec.SetTCoefficients(p_pmNutPrec);
1974 functionRaNutPrec.SetConstants(p_sysNutPrec0);
1975 functionDecNutPrec.SetConstants(p_sysNutPrec0);
1976 functionPmNutPrec.SetConstants(p_sysNutPrec0);
1977 functionRaNutPrec.SetLCoefficients(p_sysNutPrec1);
1978 functionDecNutPrec.SetLCoefficients(p_sysNutPrec1);
1979 functionPmNutPrec.SetLCoefficients(p_sysNutPrec1);
1980 }
1981 */
1982
1983 // Set the flag indicating p_degree has been applied to the planet angles, the
1984 // coefficients of the polynomials have been saved, and the cache reloaded from
1985 // TODO cache reloaded???
1986 // the polynomials.
1987 // ****At least for the planet angles, I don't think we want to reload the cache until just
1988 // before we write out the table
1989 p_degreeApplied = true;
1990 p_hasAngularVelocity = true;
1992 return;
1993 }
1994
1995
2015 void SpiceRotation::setPckPolynomial(const std::vector<Angle> &raCoeff,
2016 const std::vector<Angle> &decCoeff,
2017 const std::vector<Angle> &pmCoeff) {
2018 // Just set the constants and let usePckPolynomial() do the rest
2019 m_raPole = raCoeff;
2020 m_decPole = decCoeff;
2021 m_pm = pmCoeff;
2023 // Apply new function parameters
2025 }
2026
2027
2038 void SpiceRotation::GetPolynomial(std::vector<double> &coeffAng1,
2039 std::vector<double> &coeffAng2,
2040 std::vector<double> &coeffAng3) {
2041 coeffAng1 = p_coefficients[0];
2042 coeffAng2 = p_coefficients[1];
2043 coeffAng3 = p_coefficients[2];
2044
2045 return;
2046 }
2047
2048
2060 void SpiceRotation::getPckPolynomial(std::vector<Angle> &raCoeff,
2061 std::vector<Angle> &decCoeff,
2062 std::vector<Angle> &pmCoeff) {
2063 raCoeff = m_raPole;
2064 decCoeff = m_decPole;
2065 pmCoeff = m_pm;
2066
2067 return;
2068 }
2069
2070
2075 if (p_noOverride) {
2076 p_baseTime = (p_cacheTime.at(0) + p_cacheTime.at(p_cacheTime.size() - 1)) / 2.;
2078 // Take care of case where 1st and last times are the same
2079 if (p_timeScale == 0) p_timeScale = 1.0;
2080 }
2081 else {
2084 }
2085
2086 return;
2087 }
2088
2089
2097 void SpiceRotation::SetOverrideBaseTime(double baseTime, double timeScale) {
2098 p_overrideBaseTime = baseTime;
2099 p_overrideTimeScale = timeScale;
2100 p_noOverride = false;
2101 return;
2102 }
2103
2104 void SpiceRotation::SetCacheTime(std::vector<double> cacheTime) {
2105 // Do not reset the cache times if they are already loaded.
2106 if (p_cacheTime.size() <= 0) {
2107 p_cacheTime = cacheTime;
2108 }
2109 }
2110
2124 double SpiceRotation::DPolynomial(const int coeffIndex) {
2125 double derivative;
2126 double time = (p_et - p_baseTime) / p_timeScale;
2127
2128 if (coeffIndex > 0 && coeffIndex <= p_degree) {
2129 derivative = pow(time, coeffIndex);
2130 }
2131 else if (coeffIndex == 0) {
2132 derivative = 1;
2133 }
2134 else {
2135 QString msg = "Unable to evaluate the derivative of the SPICE rotation fit polynomial for "
2136 "the given coefficient index [" + toString(coeffIndex) + "]. "
2137 "Index is negative or exceeds degree of polynomial ["
2138 + toString(p_degree) + "]";
2139 throw IException(IException::Programmer, msg, _FILEINFO_);
2140 }
2141 return derivative;
2142 }
2143
2144
2160 const int coeffIndex) {
2161 const double p_dayScale = 86400; // number of seconds in a day
2162 // Number of 24 hour days in a Julian century = 36525
2163 const double p_centScale = p_dayScale * 36525;
2164 double time = 0.;
2165
2166 switch (partialVar) {
2167 case WRT_RightAscension:
2168 case WRT_Declination:
2169 time = p_et / p_centScale;
2170 break;
2171 case WRT_Twist:
2172 time = p_et / p_dayScale;
2173 break;
2174 }
2175
2176 double derivative;
2177
2178 switch (coeffIndex) {
2179 case 0:
2180 derivative = 1;
2181 break;
2182 case 1:
2183 case 2:
2184 derivative = pow(time, coeffIndex);
2185 break;
2186 default:
2187 QString msg = "Unable to evaluate the derivative of the target body rotation fit polynomial "
2188 "for the given coefficient index [" + toString(coeffIndex) + "]. "
2189 "Index is negative or exceeds degree of polynomial ["
2190 + toString(p_degree) + "]";
2191 throw IException(IException::Programmer, msg, _FILEINFO_);
2192 }
2193
2194 // Now apply any difference to the partials due to building the rotation matrix on the angles
2195 // 90. + ra, 90. - dec, and pm. The only partial that changes is dec.
2196 if (partialVar == WRT_Declination) {
2197 derivative = -derivative;
2198 }
2199
2200 return derivative;
2201 }
2202
2203
2219 std::vector<double> SpiceRotation::ToReferencePartial(std::vector<double> &lookJ,
2220 SpiceRotation::PartialType partialVar,
2221 int coeffIndex) {
2223 //TODO** To save time possibly save partial matrices
2224
2225 // Get the rotation angles and form the derivative matrix for the partialVar
2226 std::vector<double> angles = Angles(p_axis3, p_axis2, p_axis1);
2227 int angleIndex = partialVar;
2228 int axes[3] = {p_axis1, p_axis2, p_axis3};
2229
2230 double angle = angles.at(angleIndex);
2231
2232 double dmatrix[3][3];
2233 drotat_(&angle, (integer *) axes + angleIndex, (doublereal *) dmatrix);
2234 // Transpose to obtain row-major order
2235 xpose_c(dmatrix, dmatrix);
2236
2237 // Get the derivative of the polynomial with respect to partialVar
2238 double dpoly = 0.;
2239 switch (m_frameType) {
2240 case UNKNOWN: // For now let everything go through for backward compatability
2241 case CK:
2242 case DYN:
2243 dpoly = DPolynomial(coeffIndex);
2244 break;
2245 case PCK:
2246 dpoly = DPckPolynomial(partialVar, coeffIndex);
2247 break;
2248 default:
2249 QString msg = "Only CK, DYN, and PCK partials can be calculated";
2250 throw IException(IException::Programmer, msg, _FILEINFO_);
2251 }
2252
2253 // Multiply dpoly to complete dmatrix
2254 for (int row = 0; row < 3; row++) {
2255 for (int col = 0; col < 3; col++) {
2256 dmatrix[row][col] *= dpoly;
2257 }
2258 }
2259
2260 // Apply the other 2 angles and chain them all together
2261 double dCJ[3][3];
2262 switch (angleIndex) {
2263 case 0:
2264 rotmat_c(dmatrix, angles[1], axes[1], dCJ);
2265 rotmat_c(dCJ, angles[2], axes[2], dCJ);
2266 break;
2267 case 1:
2268 rotate_c(angles[0], axes[0], dCJ);
2269 mxm_c(dmatrix, dCJ, dCJ);
2270 rotmat_c(dCJ, angles[2], axes[2], dCJ);
2271 break;
2272 case 2:
2273 rotate_c(angles[0], axes[0], dCJ);
2274 rotmat_c(dCJ, angles[1], axes[1], dCJ);
2275 mxm_c(dmatrix, dCJ, dCJ);
2276 break;
2277 }
2278
2279 // Multiply the constant matrix to rotate to the targeted reference frame
2280 double dTJ[3][3];
2281 mxm_c((SpiceDouble *) &p_TC[0], dCJ[0], dTJ);
2282
2283 // Finally rotate the J2000 vector with the derivative matrix, dTJ to
2284 // get the vector in the targeted reference frame.
2285 std::vector<double> lookdT(3);
2286
2287 mxv_c(dTJ, (const SpiceDouble *) &lookJ[0], (SpiceDouble *) &lookdT[0]);
2288
2290 return lookdT;
2291 }
2292
2293
2302 double SpiceRotation::WrapAngle(double compareAngle, double angle) {
2304 double diff1 = compareAngle - angle;
2305
2306 if (diff1 < -1 * pi_c()) {
2307 angle -= twopi_c();
2308 }
2309 else if (diff1 > pi_c()) {
2310 angle += twopi_c();
2311 }
2312
2314 return angle;
2315 }
2316
2317
2331 // Adjust the degree for the data
2332 if (p_fullCacheSize == 1) {
2333 degree = 0;
2334 }
2335 else if (p_fullCacheSize == 2) {
2336 degree = 1;
2337 }
2338 // If polynomials have not been applied yet then simply set the degree and return
2339 if (!p_degreeApplied) {
2340 p_degree = degree;
2341 }
2342
2343 // Otherwise the existing polynomials need to be either expanded ...
2344 else if (p_degree < degree) { // (increase the number of terms)
2345 std::vector<double> coefAngle1(p_coefficients[0]),
2346 coefAngle2(p_coefficients[1]),
2347 coefAngle3(p_coefficients[2]);
2348
2349 for (int icoef = p_degree + 1; icoef <= degree; icoef++) {
2350 coefAngle1.push_back(0.);
2351 coefAngle2.push_back(0.);
2352 coefAngle3.push_back(0.);
2353 }
2354 p_degree = degree;
2355 SetPolynomial(coefAngle1, coefAngle2, coefAngle3, p_source);
2356 }
2357 // ... or reduced (decrease the number of terms)
2358 else if (p_degree > degree) {
2359 std::vector<double> coefAngle1(degree + 1),
2360 coefAngle2(degree + 1),
2361 coefAngle3(degree + 1);
2362
2363 for (int icoef = 0; icoef <= degree; icoef++) {
2364 coefAngle1[icoef] = p_coefficients[0][icoef];
2365 coefAngle2[icoef] = p_coefficients[1][icoef];
2366 coefAngle3[icoef] = p_coefficients[2][icoef];
2367 }
2368 p_degree = degree;
2369 SetPolynomial(coefAngle1, coefAngle2, coefAngle3, p_source);
2370 }
2371 }
2372
2373
2382
2383
2392
2393
2400 p_source = source;
2401 return;
2402 }
2403
2404
2411 return p_baseTime;
2412 }
2413
2414
2421 return p_timeScale;
2422 }
2423
2424
2437 void SpiceRotation::SetAxes(int axis1, int axis2, int axis3) {
2438 if (axis1 < 1 || axis2 < 1 || axis3 < 1 || axis1 > 3 || axis2 > 3 || axis3 > 3) {
2439 QString msg = "A rotation axis is outside the valid range of 1 to 3";
2440 throw IException(IException::Programmer, msg, _FILEINFO_);
2441 }
2442 p_axis1 = axis1;
2443 p_axis2 = axis2;
2444 p_axis3 = axis3;
2445 }
2446
2447
2461 int count = 0;
2462
2463 double observStart = p_fullCacheStartTime + p_timeBias;
2464 double observEnd = p_fullCacheEndTime + p_timeBias;
2465 // Next line added 12-03-2009 to allow observations to cross segment boundaries
2466 double currentTime = observStart;
2467 bool timeLoaded = false;
2468
2469 // Get number of ck loaded for this rotation. This method assumes only one SpiceRotation
2470 // object is loaded.
2472 ktotal_c("ck", (SpiceInt *) &count);
2473
2474 // Downsize the loaded cache
2475 if ((p_source == Memcache) && p_minimizeCache == Yes) {
2476 // Multiple ck case, type 5 ck case, or PolyFunctionOverSpice
2477 // final step -- downsize loaded cache and reload
2478
2479 if (p_fullCacheSize != (int) p_cacheTime.size()) {
2480 QString msg =
2481 "Full cache size does NOT match cache size in LoadTimeCache -- should never happen";
2482 throw IException(IException::Programmer, msg, _FILEINFO_);
2483 }
2484
2485 SpiceDouble timeSclkdp[p_fullCacheSize];
2486 SpiceDouble quats[p_fullCacheSize][4];
2487 double avvs[p_fullCacheSize][3]; // Angular velocity vector
2488
2489 // We will treat et as the sclock time and avoid converting back and forth
2490 std::vector<ale::Rotation> fullRotationCache = m_orientation->getRotations();
2491 std::vector<ale::Vec3d> angularVelocities = m_orientation->getAngularVelocities();
2492 for (int r = 0; r < p_fullCacheSize; r++) {
2493 timeSclkdp[r] = p_cacheTime[r];
2494 std::vector<double> rotationMatrix = fullRotationCache[r].toRotationMatrix();
2495 SpiceDouble CJ[9] = { rotationMatrix[0], rotationMatrix[1], rotationMatrix[2],
2496 rotationMatrix[3], rotationMatrix[4], rotationMatrix[5],
2497 rotationMatrix[6], rotationMatrix[7], rotationMatrix[8]
2498 };
2499 m2q_c(CJ, quats[r]);
2501 ale::Vec3d angularVelocity = angularVelocities[r];
2502 vequ_c((SpiceDouble *) &angularVelocity.x, avvs[r]);
2503 }
2504 }
2505
2506 double cubeStarts = timeSclkdp[0]; //,timsSclkdp[ckBlob.Records()-1] };
2507 double radTol = 0.000000017453; //.000001 degrees Make this instrument dependent TODO
2508 SpiceInt avflag = 1; // Don't use angular velocity for now
2509 SpiceInt nints = 1; // Always make an observation a single interpolation interval
2510 double dparr[p_fullCacheSize]; // Double precision work array
2511 SpiceInt intarr[p_fullCacheSize]; // Integer work array
2512 SpiceInt sizOut = p_fullCacheSize; // Size of downsized cache
2513
2514 ck3sdn(radTol, avflag, (int *) &sizOut, timeSclkdp, (doublereal *) quats,
2515 (SpiceDouble *) avvs, nints, &cubeStarts, dparr, (int *) intarr);
2516
2517 // Clear full cache and load with downsized version
2518 p_cacheTime.clear();
2519 std::vector<double> av;
2520 av.resize(3);
2521
2522 if (m_orientation) {
2523 delete m_orientation;
2524 m_orientation = NULL;
2525 }
2526
2527 std::vector<ale::Rotation> rotationCache;
2528 std::vector<ale::Vec3d> avCache;
2529
2530 for (int r = 0; r < sizOut; r++) {
2531 SpiceDouble et;
2532 // sct2e_c(spcode, timeSclkdp[r], &et);
2533 et = timeSclkdp[r];
2534 p_cacheTime.push_back(et);
2535 std::vector<double> CJ(9);
2536 q2m_c(quats[r], (SpiceDouble( *)[3]) &CJ[0]);
2537 rotationCache.push_back(CJ);
2538 vequ_c(avvs[r], (SpiceDouble *) &av[0]);
2539 avCache.push_back(av);
2540 }
2541
2542 if (p_TC.size() > 1 ) {
2543 m_orientation = new ale::Orientations(rotationCache, p_cacheTime, avCache,
2544 ale::Rotation(p_TC), p_constantFrames, p_timeFrames);
2545 }
2546 else {
2547 m_orientation = new ale::Orientations(rotationCache, p_cacheTime, std::vector<ale::Vec3d>(),
2548 ale::Rotation(1,0,0,0), p_constantFrames, p_timeFrames);
2549 }
2550 timeLoaded = true;
2552 }
2553 else if (count == 1 && p_minimizeCache == Yes) {
2554 // case of a single ck -- read instances and data straight from kernel for given time range
2555 SpiceInt handle;
2556
2557 // Define some Naif constants
2558 int FILESIZ = 128;
2559 int TYPESIZ = 32;
2560 int SOURCESIZ = 128;
2561// double DIRSIZ = 100;
2562
2563 SpiceChar file[FILESIZ];
2564 SpiceChar filtyp[TYPESIZ]; // kernel type (ck, ek, etc.)
2565 SpiceChar source[SOURCESIZ];
2566
2567 SpiceBoolean found;
2568 bool observationSpansToNextSegment = false;
2569
2570 double segStartEt;
2571 double segStopEt;
2572
2573 kdata_c(0, "ck", FILESIZ, TYPESIZ, SOURCESIZ, file, filtyp, source, &handle, &found);
2574 dafbfs_c(handle);
2575 daffna_c(&found);
2576 int spCode = ((int)(p_constantFrames[0] / 1000)) * 1000;
2577
2578 while (found) {
2579 observationSpansToNextSegment = false;
2580 double sum[10]; // daf segment summary
2581 double dc[2]; // segment starting and ending times in tics
2582 SpiceInt ic[6]; // segment summary values:
2583 // instrument code for platform,
2584 // reference frame code,
2585 // data type,
2586 // velocity flag,
2587 // offset to quat 1,
2588 // offset to end.
2589 dafgs_c(sum);
2590 dafus_c(sum, (SpiceInt) 2, (SpiceInt) 6, dc, ic);
2591
2592 // Don't read type 5 ck here
2593 if (ic[2] == 5) break;
2594
2595 // Check times for type 3 ck segment if spacecraft matches
2596 if (ic[0] == spCode && ic[2] == 3) {
2597 sct2e_c((int) spCode / 1000, dc[0], &segStartEt);
2598 sct2e_c((int) spCode / 1000, dc[1], &segStopEt);
2600 double et;
2601
2602 // Get times for this segment
2603 if (currentTime >= segStartEt && currentTime <= segStopEt) {
2604
2605 // Check for a gap in the time coverage by making sure the time span of the observation
2606 // does not cross a segment unless the next segment starts where the current one ends
2607 if (observationSpansToNextSegment && currentTime > segStartEt) {
2608 QString msg = "Observation crosses segment boundary--unable to interpolate pointing";
2609 throw IException(IException::Programmer, msg, _FILEINFO_);
2610 }
2611 if (observEnd > segStopEt) {
2612 observationSpansToNextSegment = true;
2613 }
2614
2615 // Extract necessary header parameters
2616 int dovelocity = ic[3];
2617 int end = ic[5];
2618 double val[2];
2619 dafgda_c(handle, end - 1, end, val);
2620// int nints = (int) val[0];
2621 int ninstances = (int) val[1];
2622 int numvel = dovelocity * 3;
2623 int quatnoff = ic[4] + (4 + numvel) * ninstances - 1;
2624// int nrdir = (int) (( ninstances - 1 ) / DIRSIZ); /* sclkdp directory records */
2625 int sclkdp1off = quatnoff + 1;
2626 int sclkdpnoff = sclkdp1off + ninstances - 1;
2627// int start1off = sclkdpnoff + nrdir + 1;
2628// int startnoff = start1off + nints - 1;
2629 int sclkSpCode = spCode / 1000;
2630
2631 // Now get the times
2632 std::vector<double> sclkdp(ninstances);
2633 dafgda_c(handle, sclkdp1off, sclkdpnoff, (SpiceDouble *) &sclkdp[0]);
2634
2635 int instance = 0;
2636 sct2e_c(sclkSpCode, sclkdp[0], &et);
2637
2638 while (instance < (ninstances - 1) && et < currentTime) {
2639 instance++;
2640 sct2e_c(sclkSpCode, sclkdp[instance], &et);
2641 }
2642
2643 if (instance > 0) instance--;
2644 sct2e_c(sclkSpCode, sclkdp[instance], &et);
2645
2646 while (instance < (ninstances - 1) && et < observEnd) {
2647 p_cacheTime.push_back(et - p_timeBias);
2648 instance++;
2649 sct2e_c(sclkSpCode, sclkdp[instance], &et);
2650 }
2651 p_cacheTime.push_back(et - p_timeBias);
2652
2653 if (!observationSpansToNextSegment) {
2654 timeLoaded = true;
2656 break;
2657 }
2658 else {
2659 currentTime = segStopEt;
2660 }
2661 }
2662 }
2663 dafcs_c(handle); // Continue search in daf last searched
2664 daffna_c(&found); // Find next forward array in current daf
2665 }
2666 }
2667 else if (count == 0 && p_source != Nadir && p_minimizeCache == Yes) {
2668 QString msg = "No camera kernels loaded...Unable to determine time cache to downsize";
2669 throw IException(IException::User, msg, _FILEINFO_);
2670 }
2671
2672 // Load times according to cache size (body rotations) -- handle first round of type 5 ck case
2673 // and multiple ck case --Load a time for every line scan line and downsize later
2674 if (! (timeLoaded || (p_cacheTime.size() > 1))) {
2675 double cacheSlope = 0.0;
2676 if (p_fullCacheSize > 1)
2677 cacheSlope = (p_fullCacheEndTime - p_fullCacheStartTime) / (double)(p_fullCacheSize - 1);
2678 for (int i = 0; i < p_fullCacheSize; i++)
2679 p_cacheTime.push_back(p_fullCacheStartTime + (double) i * cacheSlope);
2680 if (p_source == Nadir) {
2682 }
2683 }
2685 }
2686
2687
2695 std::vector<double> SpiceRotation::GetFullCacheTime() {
2696
2697 // No time cache was initialized -- throw an error
2698 if (p_fullCacheSize < 1) {
2699 QString msg = "Time cache not available -- rerun spiceinit";
2700 throw IException(IException::User, msg, _FILEINFO_);
2701 }
2702
2703 std::vector<double> fullCacheTime;
2704 double cacheSlope = 0.0;
2705 if (p_fullCacheSize > 1) {
2706 cacheSlope = (p_fullCacheEndTime - p_fullCacheStartTime) / (double)(p_fullCacheSize - 1);
2707 }
2708
2709 for (int i = 0; i < p_fullCacheSize; i++)
2710 fullCacheTime.push_back(p_fullCacheStartTime + (double) i * cacheSlope);
2711
2712 return fullCacheTime;
2713 }
2714
2715
2729 // The code for this method was extracted from the Naif routine rotget written by N.J. Bachman &
2730 // W.L. Taber (JPL)
2731 int center;
2732 FrameType type;
2733 int typid;
2734 SpiceBoolean found;
2735 int frmidx; // Frame chain index for current frame
2736 SpiceInt nextFrame; // Naif frame code of next frame
2738 std::vector<int> frameCodes;
2739 std::vector<int> frameTypes;
2740 frameCodes.push_back(p_constantFrames[0]);
2741
2742 while (frameCodes[frameCodes.size()-1] != J2000Code) {
2743 frmidx = frameCodes.size() - 1;
2744 // First get the frame type (Note:: we may also need to save center if we use dynamic frames)
2745 // (Another note: the type returned is the Naif from type. This is not quite the same as the
2746 // SpiceRotation enumerated FrameType. The SpiceRotation FrameType differentiates between
2747 // pck types. FrameTypes of 2, 6, and 7 will all be considered to be Naif frame type 2. The
2748 // logic for FrameTypes in this method is correct for all types except type 7. Current pck
2749 // do not exercise this option. Should we ever use pck with a target body not referenced to
2750 // the J2000 frame and epoch, both this method and loadPCFromSpice will need to be modified.
2751 frinfo_c((SpiceInt) frameCodes[frmidx],
2752 (SpiceInt *) &center,
2753 (SpiceInt *) &type,
2754 (SpiceInt *) &typid, &found);
2755
2756 if (!found) {
2757
2758 if (p_source == Nadir) {
2759 frameTypes.push_back(0);
2760 break;
2761 }
2762
2763 QString msg = "The frame" + toString((int) frameCodes[frmidx]) + " is not supported by Naif";
2764 throw IException(IException::Programmer, msg, _FILEINFO_);
2765 }
2766
2767 double matrix[3][3];
2768
2769 // To get the next link in the frame chain, use the frame type
2770 if (type == INERTL || type == PCK) {
2771 nextFrame = J2000Code;
2772 }
2773 else if (type == CK) {
2774 ckfrot_((SpiceInt *) &typid, &et, (double *) matrix, &nextFrame, (logical *) &found);
2775
2776 if (!found) {
2777
2778 if (p_source == Nadir) {
2779 frameTypes.push_back(0);
2780 break;
2781 }
2782
2783 QString msg = "The ck rotation from frame " + toString(frameCodes[frmidx]) + " can not "
2784 + "be found due to no pointing available at requested time or a problem with the "
2785 + "frame";
2786 throw IException(IException::Programmer, msg, _FILEINFO_);
2787 }
2788 }
2789 else if (type == TK) {
2790 tkfram_((SpiceInt *) &typid, (double *) matrix, &nextFrame, (logical *) &found);
2791 if (!found) {
2792 QString msg = "The tk rotation from frame " + toString(frameCodes[frmidx]) +
2793 " can not be found";
2794 throw IException(IException::Programmer, msg, _FILEINFO_);
2795 }
2796 }
2797 else if (type == DYN) {
2798 //
2799 // Unlike the other frame classes, the dynamic frame evaluation
2800 // routine ZZDYNROT requires the input frame ID rather than the
2801 // dynamic frame class ID. ZZDYNROT also requires the center ID
2802 // we found via the FRINFO call.
2803
2804 zzdynrot_((SpiceInt *) &typid, (SpiceInt *) &center, &et, (double *) matrix, &nextFrame);
2805 }
2806
2807 else {
2808 QString msg = "The frame " + toString(frameCodes[frmidx]) +
2809 " has a type " + toString(type) + " not supported by your version of Naif Spicelib."
2810 + "You need to update.";
2811 throw IException(IException::Programmer, msg, _FILEINFO_);
2812
2813 }
2814 frameCodes.push_back(nextFrame);
2815 frameTypes.push_back(type);
2816 }
2817
2818 if ((int) frameCodes.size() == 1 && p_source != Nadir) { // Must be Sky
2819 p_constantFrames.push_back(frameCodes[0]);
2820 p_timeFrames.push_back(frameCodes[0]);
2821 return;
2822 }
2823
2824 int nConstants = 0;
2825 p_constantFrames.clear();
2826 while (frameTypes[nConstants] == TK && nConstants < (int) frameTypes.size()) nConstants++;
2827
2828
2829 for (int i = 0; i <= nConstants; i++) {
2830 p_constantFrames.push_back(frameCodes[i]);
2831 }
2832
2833 if (p_source != Nadir) {
2834 for (int i = nConstants; i < (int) frameCodes.size(); i++) {
2835 p_timeFrames.push_back(frameCodes[i]);
2836 }
2837 }
2838 else {
2839 // Nadir rotation is from spacecraft to J2000
2840 p_timeFrames.push_back(frameCodes[nConstants]);
2841 p_timeFrames.push_back(J2000Code);
2842 }
2844 }
2845
2846
2852 std::vector<double> SpiceRotation::Matrix() {
2854 std::vector<double> TJ;
2855 TJ.resize(9);
2856 mxm_c((SpiceDouble *) &p_TC[0], (SpiceDouble *) &p_CJ[0], (SpiceDouble( *) [3]) &TJ[0]);
2858 return TJ;
2859 }
2860
2861
2867 std::vector<double> SpiceRotation::ConstantRotation() {
2869 std::vector<double> q;
2870 q.resize(4);
2871 m2q_c((SpiceDouble( *)[3]) &p_TC[0], &q[0]);
2873 return q;
2874 }
2875
2876
2882 std::vector<double> &SpiceRotation::ConstantMatrix() {
2883 return p_TC;
2884 }
2885
2886
2892 void SpiceRotation::SetConstantMatrix(std::vector<double> constantMatrix) {
2893 p_TC = constantMatrix;
2894 return;
2895 }
2896
2902 std::vector<double> SpiceRotation::TimeBasedRotation() {
2904 std::vector<double> q;
2905 q.resize(4);
2906 m2q_c((SpiceDouble( *)[3]) &p_CJ[0], &q[0]);
2908 return q;
2909 }
2910
2911
2917 std::vector<double> &SpiceRotation::TimeBasedMatrix() {
2918 return p_CJ;
2919 }
2920
2921
2927 void SpiceRotation::SetTimeBasedMatrix(std::vector<double> timeBasedMatrix) {
2928 p_CJ = timeBasedMatrix;
2929 return;
2930 }
2931
2932
2939 FrameTrace(et);
2940 // Get constant rotation which applies in all cases
2941 int targetFrame = p_constantFrames[0];
2942 int fromFrame = p_timeFrames[0];
2943 p_TC.resize(9);
2944 refchg_((SpiceInt *) &fromFrame, (SpiceInt *) &targetFrame, &et, (doublereal *) &p_TC[0]);
2945 // Transpose to obtain row-major order
2946 xpose_c((SpiceDouble( *)[3]) &p_TC[0], (SpiceDouble( *)[3]) &p_TC[0]);
2947 }
2948
2949
2971
2972 // Make sure the angles have been fit to polynomials so we can calculate the derivative
2973 if (p_source < PolyFunction ) {
2974 QString msg = "The SpiceRotation pointing angles must be fit to polynomials in order to ";
2975 msg += "compute angular velocity.";
2976 throw IException(IException::Programmer, msg, _FILEINFO_);
2977 }
2978 if (p_source == PckPolyFunction) {
2979 QString msg = "Planetary angular velocity must be fit computed with PCK polynomials ";
2980 throw IException(IException::Programmer, msg, _FILEINFO_);
2981 }
2982 std::vector<double> dCJdt;
2983 dCJdt.resize(9);
2984
2985 switch (m_frameType) {
2986 // Treat all cases the same except for target body rotations
2987 case UNKNOWN:
2988 case INERTL:
2989 case TK:
2990 case DYN:
2991 case CK:
2992 DCJdt(dCJdt);
2993 break;
2994 case PCK:
2995 case BPC:
2996 case NOTJ2000PCK:
2997 break;
2998 }
2999 double omega[3][3];
3000 mtxm_c((SpiceDouble( *)[3]) &dCJdt[0], (SpiceDouble( *)[3]) &p_CJ[0], omega);
3001 p_av[0] = omega[2][1];
3002 p_av[1] = omega[0][2];
3003 p_av[2] = omega[1][0];
3005 }
3006
3007
3014 void SpiceRotation::DCJdt(std::vector<double> &dCJ) {
3016
3017 // Get the rotation angles and axes
3018 std::vector<double> angles = Angles(p_axis3, p_axis2, p_axis1);
3019 int axes[3] = {p_axis1, p_axis2, p_axis3};
3020
3021 double dmatrix[3][3];
3022 double dangle;
3023 double wmatrix[3][3]; // work matrix
3024 dCJ.assign(9, 0.);
3025
3026 for (int angleIndex = 0; angleIndex < 3; angleIndex++) {
3027 drotat_(&(angles[angleIndex]), (integer *) axes + angleIndex, (doublereal *) dmatrix);
3028 // Transpose to obtain row-major order
3029 xpose_c(dmatrix, dmatrix);
3030
3031 // To get the derivative of the polynomial fit to the angle with respect to time
3032 // first create the function object for this angle and load its coefficients
3034 function.SetCoefficients(p_coefficients[angleIndex]);
3035
3036 // Evaluate the derivative of function at p_et
3037 // dangle = function.DerivativeVar((p_et - p_baseTime) / p_timeScale);
3038 dangle = function.DerivativeVar((p_et - p_baseTime) / p_timeScale) / p_timeScale;
3039
3040 // Multiply dangle to complete dmatrix
3041 for (int row = 0; row < 3; row++) {
3042 for (int col = 0; col < 3; col++) {
3043 dmatrix[row][col] *= dangle;
3044 }
3045 }
3046 // Apply the other 2 angles and chain them all together
3047 switch (angleIndex) {
3048 case 0:
3049 rotmat_c(dmatrix, angles[1], axes[1], dmatrix);
3050 rotmat_c(dmatrix, angles[2], axes[2], dmatrix);
3051 break;
3052 case 1:
3053 rotate_c(angles[0], axes[0], wmatrix);
3054 mxm_c(dmatrix, wmatrix, dmatrix);
3055 rotmat_c(dmatrix, angles[2], axes[2], dmatrix);
3056 break;
3057 case 2:
3058 rotate_c(angles[0], axes[0], wmatrix);
3059 rotmat_c(wmatrix, angles[1], axes[1], wmatrix);
3060 mxm_c(dmatrix, wmatrix, dmatrix);
3061 break;
3062 }
3063 int i, j;
3064 for (int index = 0; index < 9; index++) {
3065 i = index / 3;
3066 j = index % 3;
3067 dCJ[index] += dmatrix[i][j];
3068 }
3069 }
3070
3072 }
3073
3074
3080 std::vector<double> SpiceRotation::StateTJ() {
3081 std::vector<double> stateTJ(36);
3082
3083 // Build the state matrix for the time-based rotation from the matrix and angulary velocity
3084 double stateCJ[6][6];
3085 rav2xf_c(&p_CJ[0], &p_av[0], stateCJ);
3086// (SpiceDouble (*) [3]) &p_CJ[0]
3087 int irow = 0;
3088 int jcol = 0;
3089 int vpos = 0;
3090
3091 for (int row = 3; row < 6; row++) {
3092 irow = row - 3;
3093 vpos = irow * 3;
3094
3095 for (int col = 0; col < 3; col++) {
3096 jcol = col + 3;
3097 // Fill the upper left corner
3098 stateTJ[irow*6 + col] = p_TC[vpos] * stateCJ[0][col] + p_TC[vpos+1] * stateCJ[1][col]
3099 + p_TC[vpos+2] * stateCJ[2][col];
3100 // Fill the lower left corner
3101 stateTJ[row*6 + col] = p_TC[vpos] * stateCJ[3][col] + p_TC[vpos+1] * stateCJ[4][col]
3102 + p_TC[vpos+2] * stateCJ[5][col];
3103 // Fill the upper right corner
3104 stateTJ[irow*6 + jcol] = 0;
3105 // Fill the lower right corner
3106 stateTJ[row*6 +jcol] = stateTJ[irow*6 + col];
3107 }
3108 }
3109 return stateTJ;
3110 }
3111
3112
3123 std::vector<double> SpiceRotation::Extrapolate(double timeEt) {
3125
3127 return p_CJ;
3128 }
3129
3130 double diffTime = timeEt - p_et;
3131 std::vector<double> CJ(9, 0.0);
3132 double dmat[3][3];
3133
3134 // Create a rotation matrix for the axis and magnitude of the angular velocity multiplied by
3135 // the time difference
3136 axisar_c((SpiceDouble *) &p_av[0], diffTime*vnorm_c((SpiceDouble *) &p_av[0]), dmat);
3137
3138 // Rotate from the current time to the desired time assuming constant angular velocity
3139 mxm_c(dmat, (SpiceDouble *) &p_CJ[0], (SpiceDouble( *)[3]) &CJ[0]);
3141 return CJ;
3142 }
3143
3144
3152 void SpiceRotation::SetFullCacheParameters(double startTime, double endTime, int cacheSize) {
3153 // Save full cache parameters
3154 p_fullCacheStartTime = startTime;
3155 p_fullCacheEndTime = endTime;
3156 p_fullCacheSize = cacheSize;
3157 }
3158
3159
3168 // Get a count of all the loaded kernels
3169 SpiceInt count;
3170 ktotal_c("PCK", &count);
3171
3172 // Define some Naif constants
3173 int FILESIZ = 128;
3174 int TYPESIZ = 32;
3175 int SOURCESIZ = 128;
3176 SpiceChar file[FILESIZ];
3177 SpiceChar filetype[TYPESIZ];
3178 SpiceChar source[SOURCESIZ];
3179 SpiceInt handle;
3180 SpiceBoolean found;
3181
3182 // Get the name of each loaded kernel. The accuracy of this test depends on the use of the
3183 // "bpc" suffix to name a binary pck. If a binary bpc does not have the "bpc" suffix, it will not
3184 // be found by this test. This test was suggested by Boris Semenov at Naif.
3185 for (SpiceInt knum = 0; knum < count; knum++){
3186 kdata_c(knum, "PCK", FILESIZ, TYPESIZ, SOURCESIZ, file, filetype, source, &handle, &found);
3187
3188 // search file for "bpc"
3189 if (strstr(file, "bpc") != NULL) {
3190 m_frameType = BPC;
3191 }
3192 }
3193 }
3194
3195
3204 SpiceInt frameCode = p_constantFrames[0];
3205 SpiceBoolean found;
3206 SpiceInt centerBodyCode;
3207 SpiceInt frameClass;
3208 SpiceInt classId;
3209 frinfo_c(frameCode, &centerBodyCode, &frameClass, &classId, &found);
3210
3211 if (found) {
3212 if (frameClass == 2 || (centerBodyCode > 0 && frameClass != 3)) {
3213 m_frameType = PCK;
3214 // Load the PC information while it is available and set member values
3215 loadPCFromSpice(centerBodyCode);
3216 }
3217 else if (p_constantFrames.size() > 1) {
3218 for (std::vector<int>::size_type idx = 1; idx < p_constantFrames.size(); idx++) {
3219 frameCode = p_constantFrames[idx];
3220 frinfo_c(frameCode, &centerBodyCode, &frameClass, &classId, &found);
3221 if (frameClass == 3) m_frameType = CK;
3222 }
3223 }
3224 else {
3225 switch (frameClass) {
3226 case 1:
3228 break;
3229 // case 2: handled in first if block
3230 case 3:
3231 m_frameType = CK;
3232 break;
3233 case 4:
3234 m_frameType = TK;
3235 break;
3236 case 5:
3237 m_frameType = DYN;
3238 break;
3239 default:
3241 }
3242 }
3243 }
3244 }
3245
3246
3256 // If the cache has only one rotation, set it
3258 if (p_cacheTime.size() == 1) {
3259 p_CJ = m_orientation->getRotations()[0].toRotationMatrix();
3261 ale::Vec3d av = m_orientation->getAngularVelocities()[0];
3262 p_av[0] = av.x;
3263 p_av[1] = av.y;
3264 p_av[2] = av.z;
3265 }
3266 }
3267 // Otherwise determine the interval to interpolate
3268 else {
3269 p_CJ = m_orientation->interpolateTimeDep(p_et).toRotationMatrix();
3270
3272 ale::Vec3d av = m_orientation->interpolateAV(p_et);
3273 p_av[0] = av.x;
3274 p_av[1] = av.y;
3275 p_av[2] = av.z;
3276 }
3277 }
3279 }
3280
3281
3288 // TODO what about spk time bias and mission setting of light time corrections
3289 // That information has only been passed to the SpicePosition class and
3290 // is not available to this class, but probably should be applied to the
3291 // spkez call.
3292
3293 // Make sure the constant frame is loaded. This method also does the frame trace.
3295 if (p_timeFrames.size() == 0) InitConstantRotation(p_et);
3296
3297 SpiceDouble stateJ[6]; // Position and velocity vector in J2000
3298 SpiceDouble lt;
3299 SpiceInt spkCode = p_constantFrames[0] / 1000;
3300 spkez_c((SpiceInt) spkCode, p_et, "J2000", "LT+S",
3301 (SpiceInt) p_targetCode, stateJ, &lt);
3302 // reverse the position to be relative to the spacecraft. This may be
3303 // a mission dependent value and possibly the sense of the velocity as well.
3304 SpiceDouble sJ[3], svJ[3];
3305 vpack_c(-1 * stateJ[0], -1 * stateJ[1], -1 * stateJ[2], sJ);
3306 vpack_c(stateJ[3], stateJ[4], stateJ[5], svJ);
3307 twovec_c(sJ,
3308 p_axisP,
3309 svJ,
3310 p_axisV,
3311 (SpiceDouble( *)[3]) &p_CJ[0]);
3313 }
3314
3315
3328 SpiceInt j2000 = J2000Code;
3329
3330 SpiceDouble time = p_et + p_timeBias;
3331 // Make sure the constant frame is loaded. This method also does the frame trace.
3332 if (p_timeFrames.size() == 0) InitConstantRotation(p_et);
3333 int toFrame = p_timeFrames[0];
3334
3335 // First try getting the entire state matrix (6x6), which includes CJ and the angular velocity
3336 double stateCJ[6][6];
3337 frmchg_((integer *) &j2000, (integer *) &toFrame, &time, (doublereal *) stateCJ);
3338
3339 // If Naif fails attempting to get the state matrix, assume the angular velocity vector is
3340 // not available and just get the rotation matrix. First turn off Naif error reporting and
3341 // return any error without printing them.
3342 SpiceBoolean ckfailure = failed_c();
3343 reset_c(); // Reset Naif error system to allow caller to recover
3344
3345 if (!ckfailure) {
3346 xpose6_c(stateCJ, stateCJ);
3347 xf2rav_c(stateCJ, (SpiceDouble( *)[3]) &p_CJ[0], (SpiceDouble *) &p_av[0]);
3348 p_hasAngularVelocity = true;
3349 }
3350 else {
3351 refchg_((integer *) &j2000, (integer *) &toFrame, &time, (SpiceDouble *) &p_CJ[0]);
3352
3353 if (failed_c()) {
3354 char naifstr[64];
3355 getmsg_c("SHORT", sizeof(naifstr), naifstr);
3356 reset_c(); // Reset Naif error system to allow caller to recover
3357
3358 if (eqstr_c(naifstr, "SPICE(UNKNOWNFRAME)")) {
3359 Isis::IString msg = Isis::IString((int) p_constantFrames[0]) + " is an unrecognized " +
3360 "reference frame code. Has the mission frames kernel been loaded?";
3361 throw IException(IException::Io, msg, _FILEINFO_);
3362 }
3363 else {
3364 Isis::IString msg = "No pointing available at requested time [" +
3365 Isis::IString(p_et + p_timeBias) + "] for frame code [" +
3366 Isis::IString((int) p_constantFrames[0]) + "]";
3367 throw IException(IException::Io, msg, _FILEINFO_);
3368 }
3369 }
3370
3371 // Transpose to obtain row-major order
3372 xpose_c((SpiceDouble( *)[3]) &p_CJ[0], (SpiceDouble( *)[3]) &p_CJ[0]);
3373 }
3374
3376 }
3377
3378
3389
3390 // Load the functions with the coefficients
3391 function1.SetCoefficients(p_coefficients[0]);
3392 function2.SetCoefficients(p_coefficients[1]);
3393 function3.SetCoefficients(p_coefficients[2]);
3394
3395 std::vector<double> rtime;
3396 rtime.push_back((p_et - p_baseTime) / p_timeScale);
3397 std::vector<double> angles;
3398 angles.push_back(function1.Evaluate(rtime));
3399 angles.push_back(function2.Evaluate(rtime));
3400 angles.push_back(function3.Evaluate(rtime));
3401
3402 // Get the first angle back into the range Naif expects [-180.,180.]
3403 if (angles[0] <= -1 * pi_c()) {
3404 angles[0] += twopi_c();
3405 }
3406 else if (angles[0] > pi_c()) {
3407 angles[0] -= twopi_c();
3408 }
3409 return angles;
3410 }
3411
3412
3424
3425 // Load the functions with the coefficients
3426 function1.SetCoefficients(p_coefficients[0]);
3427 function2.SetCoefficients(p_coefficients[1]);
3428 function3.SetCoefficients(p_coefficients[2]);
3429
3430 std::vector<double> rtime;
3431 rtime.push_back((p_et - p_baseTime) / p_timeScale);
3432 double angle1 = function1.Evaluate(rtime);
3433 double angle2 = function2.Evaluate(rtime);
3434 double angle3 = function3.Evaluate(rtime);
3435
3436 // Get the first angle back into the range Naif expects [-180.,180.]
3437 if (angle1 < -1 * pi_c()) {
3438 angle1 += twopi_c();
3439 }
3440 else if (angle1 > pi_c()) {
3441 angle1 -= twopi_c();
3442 }
3443
3444 eul2m_c((SpiceDouble) angle3, (SpiceDouble) angle2, (SpiceDouble) angle1,
3446 (SpiceDouble( *)[3]) &p_CJ[0]);
3447
3449 if ( p_degree == 0){
3450 ale::Vec3d av = m_orientation->getAngularVelocities()[0];
3451 p_av[0] = av.x;
3452 p_av[1] = av.y;
3453 p_av[2] = av.z;
3454 }
3455 else {
3456 ComputeAv();
3457 }
3458 }
3460 }
3461
3462
3472 std::vector<double> cacheAngles(3);
3473 std::vector<double> cacheVelocity(3);
3474 cacheAngles = Angles(p_axis3, p_axis2, p_axis1);
3475 cacheVelocity = p_av;
3477 std::vector<double> polyAngles(3);
3478 // The decomposition fails because the angles are outside the valid range for Naif
3479 // polyAngles = Angles(p_axis3, p_axis2, p_axis1);
3480 polyAngles = EvaluatePolyFunction();
3481 std::vector<double> polyVelocity(3);
3482 polyVelocity = p_av;
3483 std::vector<double> angles(3);
3484
3485 for (int index = 0; index < 3; index++) {
3486 angles[index] = cacheAngles[index] + polyAngles[index];
3487 p_av[index] += cacheVelocity[index];
3488 }
3489
3490 if (angles[0] <= -1 * pi_c()) {
3491 angles[0] += twopi_c();
3492 }
3493 else if (angles[0] > pi_c()) {
3494 angles[0] -= twopi_c();
3495 }
3496
3497 if (angles[2] <= -1 * pi_c()) {
3498 angles[2] += twopi_c();
3499 }
3500 else if (angles[2] > pi_c()) {
3501 angles[2] -= twopi_c();
3502 }
3503
3504 eul2m_c((SpiceDouble) angles[2], (SpiceDouble) angles[1], (SpiceDouble) angles[0],
3506 (SpiceDouble( *)[3]) &p_CJ[0]);
3507 }
3508
3509
3516 // Set the scales
3517 double dTime = p_et / 86400.; // seconds to days conversion
3518 double centTime = dTime / 36525; // seconds to Julian centuries conversion
3519 double secondsPerJulianCentury = 86400. * 36525.;
3520
3521 // Calculate the quadratic part of the expression for the angles in degrees
3522 // Note: phi = ra + 90. and delta = 90 - dec for comparing results with older Isis 2 data
3523 Angle ra = m_raPole[0] + centTime*(m_raPole[1] + m_raPole[2]*centTime);
3524 Angle dec = m_decPole[0] + centTime*(m_decPole[1] + m_decPole[2]*centTime);
3525 Angle pm = m_pm[0] + dTime*(m_pm[1] + m_pm[2]*dTime);
3526 Angle dra = (m_raPole[1] + 2.*m_raPole[2]*centTime) / secondsPerJulianCentury;
3527 Angle ddec = (m_decPole[1] + 2.*m_decPole[2]*centTime) / secondsPerJulianCentury;
3528 Angle dpm = (m_pm[1] + 2.*m_pm[2]*dTime) / 86400;
3529
3530 // Now add the nutation/precession (trig part) adjustment to the expression if any
3531 int numNutPrec = (int) m_raNutPrec.size();
3532 Angle theta;
3533 double dtheta;
3534 double costheta;
3535 double sintheta;
3536
3537 for (int ia = 0; ia < numNutPrec; ia++) {
3538 theta = m_sysNutPrec0[ia] + m_sysNutPrec1[ia]*centTime;
3539 dtheta = m_sysNutPrec1[ia].degrees() * DEG2RAD;
3540 costheta = cos(theta.radians());
3541 sintheta = sin(theta.radians());
3542 ra += Angle(m_raNutPrec[ia] * sintheta, Angle::Degrees);
3543 dec += Angle(m_decNutPrec[ia] * costheta, Angle::Degrees);
3544 pm += Angle(m_pmNutPrec[ia] * sintheta, Angle::Degrees);
3545 dra += Angle(m_raNutPrec[ia] * dtheta / secondsPerJulianCentury * costheta, Angle::Degrees);
3546 ddec -= Angle(m_decNutPrec[ia] * dtheta / secondsPerJulianCentury * sintheta, Angle::Degrees);
3547 dpm += Angle(m_pmNutPrec[ia] * dtheta / secondsPerJulianCentury * costheta, Angle::Degrees);
3548 }
3549
3550 // Get pm in correct range
3551 pm = Angle(fmod(pm.degrees(), 360), Angle::Degrees); // Get pm back into the domain range
3552
3553 if (ra.radians() < -1 * pi_c()) {
3554 ra += Angle(twopi_c(), Angle::Radians);
3555 }
3556 else if (ra.radians() > pi_c()) {
3557 ra -= Angle(twopi_c(), Angle::Radians);
3558 }
3559
3560 if (pm.radians() < -1 * pi_c()) {
3561 pm += Angle(twopi_c(), Angle::Radians);
3562 }
3563 else if (pm.radians() > pi_c()) {
3564 pm -= Angle(twopi_c(), Angle::Radians);
3565 }
3566
3567 // Convert to Euler angles to get the matrix
3568 SpiceDouble phi = ra.radians() + HALFPI;
3569 SpiceDouble delta = HALFPI - dec.radians();
3570 SpiceDouble w = pm.radians();
3571 SpiceDouble dphi = dra.radians();
3572 SpiceDouble ddelta = -ddec.radians();
3573 SpiceDouble dw = dpm.radians();
3574
3575 // Load the Euler angles and their derivatives into an array and create the state matrix
3576 SpiceDouble angsDangs[6];
3577 SpiceDouble BJs[6][6];
3578 vpack_c(w, delta, phi, angsDangs);
3579 vpack_c(dw, ddelta, dphi, &angsDangs[3]);
3580 eul2xf_c (angsDangs, p_axis3, p_axis2, p_axis1, BJs);
3581
3582 // Decompose the state matrix to the rotation and its angular velocity
3583 xf2rav_c(BJs, (SpiceDouble( *)[3]) &p_CJ[0], (SpiceDouble *) &p_av[0]);
3584 }
3585}
Defines an angle and provides unit conversions.
Definition Angle.h:45
double degrees() const
Get the angle in units of Degrees.
Definition Angle.h:232
@ Degrees
Degrees are generally considered more human readable, 0-360 is one circle, however most math does not...
Definition Angle.h:56
@ Radians
Radians are generally used in mathematical equations, 0-2*PI is one circle, however these are more di...
Definition Angle.h:63
void SetCoefficients(const std::vector< double > &coefs)
Set the coefficients for the equation.
Isis exception class.
Definition IException.h:91
@ User
A type of error that could only have occurred due to a mistake on the user's part (e....
Definition IException.h:126
@ Programmer
This error is for when a programmer made an API call that was illegal.
Definition IException.h:146
@ Io
A type of error that occurred when performing an actual I/O operation.
Definition IException.h:155
Adds specific functionality to C++ strings.
Definition IString.h:165
Generic least square fitting class.
Utility class for creating and using cartesean line equations.
static void CheckErrors(bool resetNaif=true)
This method looks for any naif errors that might have occurred.
Nth degree Polynomial with one variable.
double DerivativeVar(const double value)
This will take the Derivative with respect to the variable and evaluate at given value.
A single keyword-value pair.
Definition PvlKeyword.h:87
Contains Pvl Groups and Pvl Objects.
Definition PvlObject.h:61
bool hasKeyword(const QString &kname, FindOptions opts) const
See if a keyword is in the current PvlObject, or deeper inside other PvlObjects and Pvlgroups within ...
void clear()
Remove everything from the current PvlObject.
Definition PvlObject.h:343
PvlKeyword & findKeyword(const QString &kname, FindOptions opts)
Finds a keyword in the current PvlObject, or deeper inside other PvlObjects and Pvlgroups within this...
Provide operations for quaternion arithmetic.
Definition Quaternion.h:36
std::vector< double > p_quaternion
Quaternion.
Definition Quaternion.h:93
Obtain SPICE information for a spacecraft.
Definition Spice.h:283
Obtain SPICE rotation information for a body.
std::vector< double > toJ2000Partial(const std::vector< double > &lookT, PartialType partialVar, int coeffIndex)
Given a direction vector in the reference frame, compute the derivative with respect to one of the co...
void setEphemerisTimePolyFunction()
When setting the ephemeris time, updates the rotation according to a polynomial that defines the thre...
std::vector< double > GetFullCacheTime()
Return full listing (cache) of original time coverage requested.
void SetPolynomial(const Source type=PolyFunction)
Set the coefficients of a polynomial fit to each of the three camera angles for the time period cover...
std::vector< double > m_decNutPrec
Coefficients of pole decliniation nut/prec terms.
void loadPCFromTable(const PvlObject &Label)
Initialize planetary orientation constants from an cube body rotation label.
void SetConstantMatrix(std::vector< double > constantMatrix)
Set the constant 3x3 rotation TC matrix from a vector of length 9.
void SetFullCacheParameters(double startTime, double endTime, int cacheSize)
Set the full cache time parameters.
double GetTimeScale()
Accessor method to get the rotation time scale.
int Frame()
Accessor method that returns the frame code.
void setPckPolynomial(const std::vector< Angle > &raCoeff, const std::vector< Angle > &decCoeff, const std::vector< Angle > &pmCoeff)
Set the coefficients of a polynomial fit to each of the three planet angles for the time period cover...
void setFrameType()
Cached orientation information.
FrameType getFrameType()
Accessor method to get the rotation frame type.
int p_targetCode
For computing Nadir rotation only.
double p_fullCacheStartTime
Initial requested starting time of cache.
int p_degree
Degree of fit polynomial for angles.
Source p_source
The source of the rotation data.
void setEphemerisTimePckPolyFunction()
When setting the ephemeris time, updates the rotation state based on the PcK polynomial.
void CacheLabel(Table &table)
Add labels to a SpiceRotation table.
void SetTimeBasedMatrix(std::vector< double > timeBasedMatrix)
Set the time-based 3x3 rotation CJ matrix from a vector of length 9.
double p_fullCacheEndTime
Initial requested ending time of cache.
std::vector< double > Matrix()
Return the full rotation TJ as a matrix.
double EphemerisTime() const
Accessor method to get current ephemeris time.
void SetAxes(int axis1, int axis2, int axis3)
Set the axes of rotation for decomposition of a rotation matrix into 3 angles.
double GetBaseTime()
Accessor method to get the rotation base time.
std::vector< double > EvaluatePolyFunction()
Evaluate the polynomial fit function for the three pointing angles for the current ephemeris time.
std::vector< double > ToReferencePartial(std::vector< double > &lookJ, PartialType partialVar, int coeffIndex)
Compute the derivative with respect to one of the coefficients in the angle polynomial fit equation o...
int p_axisV
The axis defined by the velocity vector for defining a nadir rotation.
void ReloadCache()
Cache J2000 rotation over existing cached time range using polynomials.
bool p_hasAngularVelocity
Flag indicating whether the rotation includes angular velocity.
double p_baseTime
Base time used in fit equations.
std::vector< Angle > poleDecCoefs()
Return the coefficients used to calculate the target body pole dec.
int p_axisP
The axis defined by the spacecraft vector for defining a nadir rotation.
std::vector< Angle > m_pm
Coefficients of a quadratic polynomial fitting pole pm.
double WrapAngle(double compareAngle, double angle)
Wrap the input angle to keep it within 2pi radians of the angle to compare.
std::vector< double > GetCenterAngles()
Return the camera angles at the center time of the observation.
void setEphemerisTimeNadir()
When setting the ephemeris time, uses spacecraft nadir source to update the rotation state.
Source GetSource()
Accessor method to get the rotation source.
std::vector< Angle > m_decPole
Coefficients of a quadratic polynomial fitting pole dec.
void LoadTimeCache()
Load the time cache.
std::vector< double > & TimeBasedMatrix()
Return time-based 3x3 rotation CJ matrix as a vector of length 9.
std::vector< double > p_CJ
Rotation matrix from J2000 to first constant rotation.
std::vector< double > Angles(int axis3, int axis2, int axis1)
Return the camera angles (right ascension, declination, and twist) for the time-based matrix CJ.
void InitConstantRotation(double et)
Initialize the constant rotation.
int p_axis1
Axis of rotation for angle 1 of rotation.
int p_axis3
Axis of rotation for angle 3 of rotation.
Table Cache(const QString &tableName)
Return a table with J2000 to reference rotations.
void LoadCache(double startTime, double endTime, int size)
Cache J2000 rotation quaternion over a time range.
std::vector< double > ConstantRotation()
Return the constant 3x3 rotation TC matrix as a quaternion.
std::vector< double > ReferenceVector(const std::vector< double > &jVec)
Given a direction vector in J2000, return a reference frame direction.
std::vector< double > AngularVelocity()
Accessor method to get the angular velocity.
double p_overrideBaseTime
Value set by caller to override computed base time.
double p_overrideTimeScale
Value set by caller to override computed time scale.
std::vector< Angle > sysNutPrecCoefs()
Return the coefficients used to calculate the target body system nut/prec angles.
void SetOverrideBaseTime(double baseTime, double timeScale)
Set an override base time to be used with observations on scanners to allow all images in an observat...
Table LineCache(const QString &tableName)
Return a table with J2000 to reference rotations.
std::vector< double > J2000Vector(const std::vector< double > &rVec)
Given a direction vector in the reference frame, return a J2000 direction.
double DPckPolynomial(PartialType partialVar, const int coeffIndex)
Evaluate the derivative of the fit polynomial defined by the given coefficients with respect to the c...
DownsizeStatus p_minimizeCache
Status of downsizing the cache (set to No to ignore)
void SetEphemerisTime(double et)
Return the J2000 to reference frame quaternion at given time.
void SetFrame(int frameCode)
Change the frame to the given frame code.
std::vector< double > m_raNutPrec
Coefficients of pole right ascension nut/prec terms.
std::vector< int > TimeFrameChain()
Accessor method to get the frame chain for the rotation (begins in J2000).
std::vector< double > p_coefficients[3]
Coefficients defining functions fit to 3 pointing angles.
SpiceRotation(int frameCode)
Construct an empty SpiceRotation class using a valid Naif frame code to set up for getting rotation f...
double p_timeScale
Time scale used in fit equations.
std::vector< Angle > sysNutPrecConstants()
Return the constants used to calculate the target body system nut/prec angles.
std::vector< double > Extrapolate(double timeEt)
Extrapolate pointing for a given time assuming a constant angular velocity.
bool p_degreeApplied
Flag indicating whether or not a polynomial of degree p_degree has been created and used to fill the ...
std::vector< double > poleRaNutPrecCoefs()
Return the coefficients used to calculate the target body pole ra nut/prec coefficients.
std::vector< double > p_cacheTime
iTime for corresponding rotation
DownsizeStatus
Status of downsizing the cache.
@ Done
Cache is downsized.
@ No
Do not downsize the cache.
@ Yes
Downsize the cache.
void checkForBinaryPck()
Check loaded pck to see if any are binary and set frame type to indicate binary pck.
std::vector< Angle > poleRaCoefs()
Return the coefficients used to calculate the target body pole ra.
void setEphemerisTimePolyFunctionOverSpice()
When setting the ephemeris time, updates the rotation state based on a polynomial fit over spice kern...
std::vector< double > p_av
Angular velocity for rotation at time p_et.
void SetSource(Source source)
Resets the source of the rotation to the given value.
int p_fullCacheSize
Initial requested cache size.
void setEphemerisTimeSpice()
When setting the ephemeris time, updates the rotation state based on data read directly from NAIF ker...
bool p_noOverride
Flag to compute base time;.
bool m_tOrientationAvailable
Target orientation constants are available.
void GetPolynomial(std::vector< double > &abcAng1, std::vector< double > &abcAng2, std::vector< double > &abcAng3)
Return the coefficients of a polynomial fit to each of the three camera angles for the time period co...
std::vector< double > StateTJ()
State matrix (6x6) for rotating state vectors from J2000 to target frame.
Source
The rotation can come from one of 3 places for an Isis cube.
@ PolyFunction
From nth degree polynomial.
@ Spice
Directly from the kernels.
@ Nadir
Nadir pointing.
@ PckPolyFunction
Quadratic polynomial function with linear trignometric terms.
@ Memcache
From cached table.
@ PolyFunctionOverSpice
Kernels plus nth degree polynomial.
PartialType
This enumeration indicates whether the partial derivative is taken with respect to Right Ascension,...
@ WRT_RightAscension
With respect to Right Ascension.
@ WRT_Twist
With respect to Twist or Prime Meridian Rotation.
@ WRT_Declination
With respect to Declination.
void loadPCFromSpice(int CenterBodyCode)
Initialize planetary orientation constants from Spice PCK.
void setEphemerisTimeMemcache()
Updates rotation state based on the rotation cache.
void ComputeAv()
Compute the angular velocity from the time-based functions fit to the pointing angles This method com...
double DPolynomial(const int coeffIndex)
Evaluate the derivative of the fit polynomial defined by the given coefficients with respect to the c...
void ComputeBaseTime()
Compute the base time using cached times.
void SetPolynomialDegree(int degree)
Set the degree of the polynomials to be fit to the three camera angles for the time period covered by...
void MinimizeCache(DownsizeStatus status)
Set the downsize status to minimize cache.
void SetAngles(std::vector< double > angles, int axis3, int axis2, int axis1)
Set the rotation angles (phi, delta, and w) for the current time to define the time-based matrix CJ.
bool HasAngularVelocity()
Checks whether the rotation has angular velocities.
double p_et
Current ephemeris time.
std::vector< double > m_pmNutPrec
Coefficients of prime meridian nut/prec terms.
std::vector< Angle > m_raPole
Coefficients of a quadratic polynomial fitting pole ra.
std::vector< int > p_timeFrames
Chain of Naif frame codes in time-based rotation CJ.
void DCJdt(std::vector< double > &dRJ)
Compute the derivative of the 3x3 rotation matrix CJ with respect to time.
void SetTimeBias(double timeBias)
Apply a time bias when invoking SetEphemerisTime method.
bool p_matrixSet
Flag indicating p_TJ has been set.
std::vector< int > ConstantFrameChain()
Accessor method to get the frame chain for the constant part of the rotation (ends in target)
std::vector< double > p_TC
Rotation matrix from first constant rotation (after all time-based rotations in frame chain from J200...
void FrameTrace(double et)
Compute frame trace chain from target frame to J2000.
int p_axis2
Axis of rotation for angle 2 of rotation.
std::vector< Angle > m_sysNutPrec0
Constants of planetary system nut/prec periods.
std::vector< double > & ConstantMatrix()
Return the constant 3x3 rotation TC matrix as a vector of length 9.
void getPckPolynomial(std::vector< Angle > &raCoeff, std::vector< Angle > &decCoeff, std::vector< Angle > &pmCoeff)
Return the coefficients of a polynomial fit to each of the three planet angles.
double p_timeBias
iTime bias when reading kernels
std::vector< double > pmNutPrecCoefs()
Return the coefficients used to calculate the target body pm nut/prec coefficients.
FrameType
Enumeration for the frame type of the rotation.
@ INERTL
See Naif Frames.req document for.
@ NOTJ2000PCK
PCK frame not referenced to J2000.
@ UNKNOWN
Isis specific code for unknown frame type.
@ BPC
Isis specific code for binary pck.
bool IsCached() const
Checks if the cache is empty.
FrameType m_frameType
The type of rotation frame.
std::vector< double > poleDecNutPrecCoefs()
Return the coefficients used to calculate the target body pole dec nut/prec coefficients.
std::vector< Angle > pmCoefs()
Return the coefficients used to calculate the target body prime meridian.
std::vector< Angle > m_sysNutPrec1
Linear terms of planetary system nut/prec periods.
std::vector< int > p_constantFrames
Chain of Naif frame codes in constant rotation TC.
virtual ~SpiceRotation()
Destructor for SpiceRotation object.
Quaternion p_quaternion
Quaternion for J2000 to reference rotation at et.
void usePckPolynomial()
Set the coefficients of a polynomial fit to each of the three planet angles for the time period cover...
std::vector< double > TimeBasedRotation()
Return time-based 3x3 rotation CJ matrix as a quaternion.
Class for storing an Isis::Table's field information.
Definition TableField.h:47
@ Double
The values in the field are 8 byte doubles.
Definition TableField.h:54
Class for storing Table blobs information.
Definition Table.h:61
int Records() const
Returns the number of records.
Definition Table.cpp:313
PvlObject & Label()
The Table's label.
Definition Table.cpp:260
This is free and unencumbered software released into the public domain.
Definition Apollo.h:16
QString toString(bool boolToConvert)
Global function to convert a boolean to a string.
Definition IString.cpp:211
int toInt(const QString &string)
Global function to convert from a string to an integer.
Definition IString.cpp:93
const double DEG2RAD
Multiplier for converting from degrees to radians.
Definition Constants.h:43
const double HALFPI
The mathematical constant PI/2.
Definition Constants.h:41
double toDouble(const QString &string)
Global function to convert from a string to a double.
Definition IString.cpp:149