File failed to load: https://isis.astrogeology.usgs.gov/9.0.0/Object/assets/jax/output/NativeMML/config.js
Isis 3 Programmer Reference
SpiceRotation.cpp
1
5
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
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;
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;
180 p_TC = rotToCopy.p_TC;
181
182 p_CJ = rotToCopy.p_CJ;
183 p_degree = rotToCopy.p_degree;
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"];
564 p_constantFrames.clear();
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
1883 if (type == PolyFunctionOverSpice && !m_orientation) {
1884 QString msg = "The quaternion SPICE tables are no longer available. "
1885 "Either re-run spiceinit or set OVEREXISTING to False.";
1886 throw IException(IException::User, msg, _FILEINFO_);
1887 }
1892
1893 // Load the functions with the coefficients
1894 function1.SetCoefficients(coeffAng1);
1895 function2.SetCoefficients(coeffAng2);
1896 function3.SetCoefficients(coeffAng3);
1897
1898 // Compute the base time
1900
1901 // Save the current coefficients
1902 p_coefficients[0] = coeffAng1;
1903 p_coefficients[1] = coeffAng2;
1904 p_coefficients[2] = coeffAng3;
1905
1906 // Set the flag indicating p_degree has been applied to the camera angles, the
1907 // coefficients of the polynomials have been saved, and the cache reloaded from
1908 // the polynomials
1909 p_degreeApplied = true;
1910 // p_source = PolyFunction;
1911 p_source = type;
1912
1913 // Update the current rotation
1914 double et = p_et;
1915 p_et = -DBL_MAX;
1916 SetEphemerisTime(et);
1917
1919 return;
1920 }
1921
1922
1941
1942 // Check to see if rotation is already stored as a polynomial
1943 if (p_source == PckPolyFunction) {
1944 p_hasAngularVelocity = true;
1945 return;
1946 }
1947
1948 // No target body orientation information available -- throw an error
1950 QString msg = "Target body orientation information not available. Rerun spiceinit.";
1951 throw IException(IException::User, msg, _FILEINFO_);
1952 }
1953
1954 /* I think we have nothing to do, but check for available coefficients and set the Source
1955 // Set the scales
1956 p_dscale = 0.000011574074074; // seconds to days conversion
1957 p_tscale = 3.1688087814029D-10; // seconds to Julian centuries conversion
1958 //Set the quadratic part of the equations
1959 //TODO consider just hard coding this part in evaluate
1960 Isis::PolynomialUnivariate functionRa(p_degree); // Basis function fit to target body pole ra
1961 Isis::PolynomialUnivariate functionDec(p_degree); // Basis function fit to target body pole dec
1962 Isis::PolynomialUnivariate functionPm(p_degree); // Basis function fit to target body pm
1963
1964 // Load the functions with the coefficients
1965 function1.SetCoefficients(p_raPole);
1966 function2.SetCoefficients(p_decPole);
1967 function3.SetCoefficients(p_pm);
1968
1969 int numNutPrec = p_sysNutPrec0.size;
1970 if (numNutPrec > 0) {
1971 // Set the trigonometric part of the equation
1972 Isis::TrigBasis functionRaNutPrec("raNutPrec", Isis::TrigBasis::Sin, numNutPrec);
1973 Isis::TrigBasis functionRaNutPrec("decNutPrec", Isis::TrigBasis::Cos, numNutPrec);
1974 Isis::TrigBasis functionRaNutPrec("pmNutPrec", Isis::TrigBasis::Sin, numNutPrec);
1975 // Load the functions with the coefficients
1976 functionRaNutPrec.SetTCoefficients(p_raNutPrec);
1977 functionDecNutPrec.SetTCoefficients(p_decNutPrec);
1978 functionPmNutPrec.SetTCoefficients(p_pmNutPrec);
1979 functionRaNutPrec.SetConstants(p_sysNutPrec0);
1980 functionDecNutPrec.SetConstants(p_sysNutPrec0);
1981 functionPmNutPrec.SetConstants(p_sysNutPrec0);
1982 functionRaNutPrec.SetLCoefficients(p_sysNutPrec1);
1983 functionDecNutPrec.SetLCoefficients(p_sysNutPrec1);
1984 functionPmNutPrec.SetLCoefficients(p_sysNutPrec1);
1985 }
1986 */
1987
1988 // Set the flag indicating p_degree has been applied to the planet angles, the
1989 // coefficients of the polynomials have been saved, and the cache reloaded from
1990 // TODO cache reloaded???
1991 // the polynomials.
1992 // ****At least for the planet angles, I don't think we want to reload the cache until just
1993 // before we write out the table
1994 p_degreeApplied = true;
1995 p_hasAngularVelocity = true;
1997 return;
1998 }
1999
2000
2020 void SpiceRotation::setPckPolynomial(const std::vector<Angle> &raCoeff,
2021 const std::vector<Angle> &decCoeff,
2022 const std::vector<Angle> &pmCoeff) {
2023 // Just set the constants and let usePckPolynomial() do the rest
2024 m_raPole = raCoeff;
2025 m_decPole = decCoeff;
2026 m_pm = pmCoeff;
2028 // Apply new function parameters
2030 }
2031
2032
2043 void SpiceRotation::GetPolynomial(std::vector<double> &coeffAng1,
2044 std::vector<double> &coeffAng2,
2045 std::vector<double> &coeffAng3) {
2046 coeffAng1 = p_coefficients[0];
2047 coeffAng2 = p_coefficients[1];
2048 coeffAng3 = p_coefficients[2];
2049
2050 return;
2051 }
2052
2053
2065 void SpiceRotation::getPckPolynomial(std::vector<Angle> &raCoeff,
2066 std::vector<Angle> &decCoeff,
2067 std::vector<Angle> &pmCoeff) {
2068 raCoeff = m_raPole;
2069 decCoeff = m_decPole;
2070 pmCoeff = m_pm;
2071
2072 return;
2073 }
2074
2075
2080 if (p_noOverride) {
2081 p_baseTime = (p_cacheTime.at(0) + p_cacheTime.at(p_cacheTime.size() - 1)) / 2.;
2083 // Take care of case where 1st and last times are the same
2084 if (p_timeScale == 0) p_timeScale = 1.0;
2085 }
2086 else {
2089 }
2090
2091 return;
2092 }
2093
2094
2102 void SpiceRotation::SetOverrideBaseTime(double baseTime, double timeScale) {
2103 p_overrideBaseTime = baseTime;
2104 p_overrideTimeScale = timeScale;
2105 p_noOverride = false;
2106 return;
2107 }
2108
2109 void SpiceRotation::SetCacheTime(std::vector<double> cacheTime) {
2110 // Do not reset the cache times if they are already loaded.
2111 if (p_cacheTime.size() <= 0) {
2112 p_cacheTime = cacheTime;
2113 }
2114 }
2115
2129 double SpiceRotation::DPolynomial(const int coeffIndex) {
2130 double derivative;
2131 double time = (p_et - p_baseTime) / p_timeScale;
2132
2133 if (coeffIndex > 0 && coeffIndex <= p_degree) {
2134 derivative = pow(time, coeffIndex);
2135 }
2136 else if (coeffIndex == 0) {
2137 derivative = 1;
2138 }
2139 else {
2140 QString msg = "Unable to evaluate the derivative of the SPICE rotation fit polynomial for "
2141 "the given coefficient index [" + toString(coeffIndex) + "]. "
2142 "Index is negative or exceeds degree of polynomial ["
2143 + toString(p_degree) + "]";
2144 throw IException(IException::Programmer, msg, _FILEINFO_);
2145 }
2146 return derivative;
2147 }
2148
2149
2165 const int coeffIndex) {
2166 const double p_dayScale = 86400; // number of seconds in a day
2167 // Number of 24 hour days in a Julian century = 36525
2168 const double p_centScale = p_dayScale * 36525;
2169 double time = 0.;
2170
2171 switch (partialVar) {
2172 case WRT_RightAscension:
2173 case WRT_Declination:
2174 time = p_et / p_centScale;
2175 break;
2176 case WRT_Twist:
2177 time = p_et / p_dayScale;
2178 break;
2179 }
2180
2181 double derivative;
2182
2183 switch (coeffIndex) {
2184 case 0:
2185 derivative = 1;
2186 break;
2187 case 1:
2188 case 2:
2189 derivative = pow(time, coeffIndex);
2190 break;
2191 default:
2192 QString msg = "Unable to evaluate the derivative of the target body rotation fit polynomial "
2193 "for the given coefficient index [" + toString(coeffIndex) + "]. "
2194 "Index is negative or exceeds degree of polynomial ["
2195 + toString(p_degree) + "]";
2196 throw IException(IException::Programmer, msg, _FILEINFO_);
2197 }
2198
2199 // Now apply any difference to the partials due to building the rotation matrix on the angles
2200 // 90. + ra, 90. - dec, and pm. The only partial that changes is dec.
2201 if (partialVar == WRT_Declination) {
2202 derivative = -derivative;
2203 }
2204
2205 return derivative;
2206 }
2207
2208
2224 std::vector<double> SpiceRotation::ToReferencePartial(std::vector<double> &lookJ,
2225 SpiceRotation::PartialType partialVar,
2226 int coeffIndex) {
2228 //TODO** To save time possibly save partial matrices
2229
2230 // Get the rotation angles and form the derivative matrix for the partialVar
2231 std::vector<double> angles = Angles(p_axis3, p_axis2, p_axis1);
2232 int angleIndex = partialVar;
2233 int axes[3] = {p_axis1, p_axis2, p_axis3};
2234
2235 double angle = angles.at(angleIndex);
2236
2237 double dmatrix[3][3];
2238 drotat_(&angle, (integer *) axes + angleIndex, (doublereal *) dmatrix);
2239 // Transpose to obtain row-major order
2240 xpose_c(dmatrix, dmatrix);
2241
2242 // Get the derivative of the polynomial with respect to partialVar
2243 double dpoly = 0.;
2244 switch (m_frameType) {
2245 case UNKNOWN: // For now let everything go through for backward compatability
2246 case CK:
2247 case DYN:
2248 dpoly = DPolynomial(coeffIndex);
2249 break;
2250 case PCK:
2251 dpoly = DPckPolynomial(partialVar, coeffIndex);
2252 break;
2253 default:
2254 QString msg = "Only CK, DYN, and PCK partials can be calculated";
2255 throw IException(IException::Programmer, msg, _FILEINFO_);
2256 }
2257
2258 // Multiply dpoly to complete dmatrix
2259 for (int row = 0; row < 3; row++) {
2260 for (int col = 0; col < 3; col++) {
2261 dmatrix[row][col] *= dpoly;
2262 }
2263 }
2264
2265 // Apply the other 2 angles and chain them all together
2266 double dCJ[3][3];
2267 switch (angleIndex) {
2268 case 0:
2269 rotmat_c(dmatrix, angles[1], axes[1], dCJ);
2270 rotmat_c(dCJ, angles[2], axes[2], dCJ);
2271 break;
2272 case 1:
2273 rotate_c(angles[0], axes[0], dCJ);
2274 mxm_c(dmatrix, dCJ, dCJ);
2275 rotmat_c(dCJ, angles[2], axes[2], dCJ);
2276 break;
2277 case 2:
2278 rotate_c(angles[0], axes[0], dCJ);
2279 rotmat_c(dCJ, angles[1], axes[1], dCJ);
2280 mxm_c(dmatrix, dCJ, dCJ);
2281 break;
2282 }
2283
2284 // Multiply the constant matrix to rotate to the targeted reference frame
2285 double dTJ[3][3];
2286 mxm_c((SpiceDouble *) &p_TC[0], dCJ[0], dTJ);
2287
2288 // Finally rotate the J2000 vector with the derivative matrix, dTJ to
2289 // get the vector in the targeted reference frame.
2290 std::vector<double> lookdT(3);
2291
2292 mxv_c(dTJ, (const SpiceDouble *) &lookJ[0], (SpiceDouble *) &lookdT[0]);
2293
2295 return lookdT;
2296 }
2297
2298
2307 double SpiceRotation::WrapAngle(double compareAngle, double angle) {
2309 double diff1 = compareAngle - angle;
2310
2311 if (diff1 < -1 * pi_c()) {
2312 angle -= twopi_c();
2313 }
2314 else if (diff1 > pi_c()) {
2315 angle += twopi_c();
2316 }
2317
2319 return angle;
2320 }
2321
2322
2336 // Adjust the degree for the data
2337 if (p_fullCacheSize == 1) {
2338 degree = 0;
2339 }
2340 else if (p_fullCacheSize == 2) {
2341 degree = 1;
2342 }
2343 // If polynomials have not been applied yet then simply set the degree and return
2344 if (!p_degreeApplied) {
2345 p_degree = degree;
2346 }
2347
2348 // Otherwise the existing polynomials need to be either expanded ...
2349 else if (p_degree < degree) { // (increase the number of terms)
2350 std::vector<double> coefAngle1(p_coefficients[0]),
2351 coefAngle2(p_coefficients[1]),
2352 coefAngle3(p_coefficients[2]);
2353
2354 for (int icoef = p_degree + 1; icoef <= degree; icoef++) {
2355 coefAngle1.push_back(0.);
2356 coefAngle2.push_back(0.);
2357 coefAngle3.push_back(0.);
2358 }
2359 p_degree = degree;
2360 SetPolynomial(coefAngle1, coefAngle2, coefAngle3, p_source);
2361 }
2362 // ... or reduced (decrease the number of terms)
2363 else if (p_degree > degree) {
2364 std::vector<double> coefAngle1(degree + 1),
2365 coefAngle2(degree + 1),
2366 coefAngle3(degree + 1);
2367
2368 for (int icoef = 0; icoef <= degree; icoef++) {
2369 coefAngle1[icoef] = p_coefficients[0][icoef];
2370 coefAngle2[icoef] = p_coefficients[1][icoef];
2371 coefAngle3[icoef] = p_coefficients[2][icoef];
2372 }
2373 p_degree = degree;
2374 SetPolynomial(coefAngle1, coefAngle2, coefAngle3, p_source);
2375 }
2376 }
2377
2378
2387
2388
2397
2398
2405 p_source = source;
2406 return;
2407 }
2408
2409
2416 return p_baseTime;
2417 }
2418
2419
2426 return p_timeScale;
2427 }
2428
2429
2442 void SpiceRotation::SetAxes(int axis1, int axis2, int axis3) {
2443 if (axis1 < 1 || axis2 < 1 || axis3 < 1 || axis1 > 3 || axis2 > 3 || axis3 > 3) {
2444 QString msg = "A rotation axis is outside the valid range of 1 to 3";
2445 throw IException(IException::Programmer, msg, _FILEINFO_);
2446 }
2447 p_axis1 = axis1;
2448 p_axis2 = axis2;
2449 p_axis3 = axis3;
2450 }
2451
2452
2466 int count = 0;
2467
2468 double observStart = p_fullCacheStartTime + p_timeBias;
2469 double observEnd = p_fullCacheEndTime + p_timeBias;
2470 // Next line added 12-03-2009 to allow observations to cross segment boundaries
2471 double currentTime = observStart;
2472 bool timeLoaded = false;
2473
2474 // Get number of ck loaded for this rotation. This method assumes only one SpiceRotation
2475 // object is loaded.
2477 ktotal_c("ck", (SpiceInt *) &count);
2478
2479 // Downsize the loaded cache
2480 if ((p_source == Memcache) && p_minimizeCache == Yes) {
2481 // Multiple ck case, type 5 ck case, or PolyFunctionOverSpice
2482 // final step -- downsize loaded cache and reload
2483
2484 if (p_fullCacheSize != (int) p_cacheTime.size()) {
2485 QString msg =
2486 "Full cache size does NOT match cache size in LoadTimeCache -- should never happen";
2487 throw IException(IException::Programmer, msg, _FILEINFO_);
2488 }
2489
2490 SpiceDouble timeSclkdp[p_fullCacheSize];
2491 SpiceDouble quats[p_fullCacheSize][4];
2492 double avvs[p_fullCacheSize][3]; // Angular velocity vector
2493
2494 // We will treat et as the sclock time and avoid converting back and forth
2495 std::vector<ale::Rotation> fullRotationCache = m_orientation->getRotations();
2496 std::vector<ale::Vec3d> angularVelocities = m_orientation->getAngularVelocities();
2497 for (int r = 0; r < p_fullCacheSize; r++) {
2498 timeSclkdp[r] = p_cacheTime[r];
2499 std::vector<double> rotationMatrix = fullRotationCache[r].toRotationMatrix();
2500 SpiceDouble CJ[9] = { rotationMatrix[0], rotationMatrix[1], rotationMatrix[2],
2501 rotationMatrix[3], rotationMatrix[4], rotationMatrix[5],
2502 rotationMatrix[6], rotationMatrix[7], rotationMatrix[8]
2503 };
2504 m2q_c(CJ, quats[r]);
2506 ale::Vec3d angularVelocity = angularVelocities[r];
2507 vequ_c((SpiceDouble *) &angularVelocity.x, avvs[r]);
2508 }
2509 }
2510
2511 double cubeStarts = timeSclkdp[0]; //,timsSclkdp[ckBlob.Records()-1] };
2512 double radTol = 0.000000017453; //.000001 degrees Make this instrument dependent TODO
2513 SpiceInt avflag = 1; // Don't use angular velocity for now
2514 SpiceInt nints = 1; // Always make an observation a single interpolation interval
2515 double dparr[p_fullCacheSize]; // Double precision work array
2516 SpiceInt intarr[p_fullCacheSize]; // Integer work array
2517 SpiceInt sizOut = p_fullCacheSize; // Size of downsized cache
2518
2519 ck3sdn(radTol, avflag, (int *) &sizOut, timeSclkdp, (doublereal *) quats,
2520 (SpiceDouble *) avvs, nints, &cubeStarts, dparr, (int *) intarr);
2521
2522 // Clear full cache and load with downsized version
2523 p_cacheTime.clear();
2524 std::vector<double> av;
2525 av.resize(3);
2526
2527 if (m_orientation) {
2528 delete m_orientation;
2529 m_orientation = NULL;
2530 }
2531
2532 std::vector<ale::Rotation> rotationCache;
2533 std::vector<ale::Vec3d> avCache;
2534
2535 for (int r = 0; r < sizOut; r++) {
2536 SpiceDouble et;
2537 // sct2e_c(spcode, timeSclkdp[r], &et);
2538 et = timeSclkdp[r];
2539 p_cacheTime.push_back(et);
2540 std::vector<double> CJ(9);
2541 q2m_c(quats[r], (SpiceDouble( *)[3]) &CJ[0]);
2542 rotationCache.push_back(CJ);
2543 vequ_c(avvs[r], (SpiceDouble *) &av[0]);
2544 avCache.push_back(av);
2545 }
2546
2547 if (p_TC.size() > 1 ) {
2548 m_orientation = new ale::Orientations(rotationCache, p_cacheTime, avCache,
2549 ale::Rotation(p_TC), p_constantFrames, p_timeFrames);
2550 }
2551 else {
2552 m_orientation = new ale::Orientations(rotationCache, p_cacheTime, std::vector<ale::Vec3d>(),
2553 ale::Rotation(1,0,0,0), p_constantFrames, p_timeFrames);
2554 }
2555 timeLoaded = true;
2557 }
2558 else if (count == 1 && p_minimizeCache == Yes) {
2559 // case of a single ck -- read instances and data straight from kernel for given time range
2560 SpiceInt handle;
2561
2562 // Define some Naif constants
2563 int FILESIZ = 128;
2564 int TYPESIZ = 32;
2565 int SOURCESIZ = 128;
2566// double DIRSIZ = 100;
2567
2568 SpiceChar file[FILESIZ];
2569 SpiceChar filtyp[TYPESIZ]; // kernel type (ck, ek, etc.)
2570 SpiceChar source[SOURCESIZ];
2571
2572 SpiceBoolean found;
2573 bool observationSpansToNextSegment = false;
2574
2575 double segStartEt;
2576 double segStopEt;
2577
2578 kdata_c(0, "ck", FILESIZ, TYPESIZ, SOURCESIZ, file, filtyp, source, &handle, &found);
2579 dafbfs_c(handle);
2580 daffna_c(&found);
2581 int spCode = ((int)(p_constantFrames[0] / 1000)) * 1000;
2582
2583 while (found) {
2584 observationSpansToNextSegment = false;
2585 double sum[10]; // daf segment summary
2586 double dc[2]; // segment starting and ending times in tics
2587 SpiceInt ic[6]; // segment summary values:
2588 // instrument code for platform,
2589 // reference frame code,
2590 // data type,
2591 // velocity flag,
2592 // offset to quat 1,
2593 // offset to end.
2594 dafgs_c(sum);
2595 dafus_c(sum, (SpiceInt) 2, (SpiceInt) 6, dc, ic);
2596
2597 // Don't read type 5 ck here
2598 if (ic[2] == 5) break;
2599
2600 // Check times for type 3 ck segment if spacecraft matches
2601 if (ic[0] == spCode && ic[2] == 3) {
2602 sct2e_c((int) spCode / 1000, dc[0], &segStartEt);
2603 sct2e_c((int) spCode / 1000, dc[1], &segStopEt);
2605 double et;
2606
2607 // Get times for this segment
2608 if (currentTime >= segStartEt && currentTime <= segStopEt) {
2609
2610 // Check for a gap in the time coverage by making sure the time span of the observation
2611 // does not cross a segment unless the next segment starts where the current one ends
2612 if (observationSpansToNextSegment && currentTime > segStartEt) {
2613 QString msg = "Observation crosses segment boundary--unable to interpolate pointing";
2614 throw IException(IException::Programmer, msg, _FILEINFO_);
2615 }
2616 if (observEnd > segStopEt) {
2617 observationSpansToNextSegment = true;
2618 }
2619
2620 // Extract necessary header parameters
2621 int dovelocity = ic[3];
2622 int end = ic[5];
2623 double val[2];
2624 dafgda_c(handle, end - 1, end, val);
2625// int nints = (int) val[0];
2626 int ninstances = (int) val[1];
2627 int numvel = dovelocity * 3;
2628 int quatnoff = ic[4] + (4 + numvel) * ninstances - 1;
2629// int nrdir = (int) (( ninstances - 1 ) / DIRSIZ); /* sclkdp directory records */
2630 int sclkdp1off = quatnoff + 1;
2631 int sclkdpnoff = sclkdp1off + ninstances - 1;
2632// int start1off = sclkdpnoff + nrdir + 1;
2633// int startnoff = start1off + nints - 1;
2634 int sclkSpCode = spCode / 1000;
2635
2636 // Now get the times
2637 std::vector<double> sclkdp(ninstances);
2638 dafgda_c(handle, sclkdp1off, sclkdpnoff, (SpiceDouble *) &sclkdp[0]);
2639
2640 int instance = 0;
2641 sct2e_c(sclkSpCode, sclkdp[0], &et);
2642
2643 while (instance < (ninstances - 1) && et < currentTime) {
2644 instance++;
2645 sct2e_c(sclkSpCode, sclkdp[instance], &et);
2646 }
2647
2648 if (instance > 0) instance--;
2649 sct2e_c(sclkSpCode, sclkdp[instance], &et);
2650
2651 while (instance < (ninstances - 1) && et < observEnd) {
2652 p_cacheTime.push_back(et - p_timeBias);
2653 instance++;
2654 sct2e_c(sclkSpCode, sclkdp[instance], &et);
2655 }
2656 p_cacheTime.push_back(et - p_timeBias);
2657
2658 if (!observationSpansToNextSegment) {
2659 timeLoaded = true;
2661 break;
2662 }
2663 else {
2664 currentTime = segStopEt;
2665 }
2666 }
2667 }
2668 dafcs_c(handle); // Continue search in daf last searched
2669 daffna_c(&found); // Find next forward array in current daf
2670 }
2671 }
2672 else if (count == 0 && p_source != Nadir && p_minimizeCache == Yes) {
2673 QString msg = "No camera kernels loaded...Unable to determine time cache to downsize";
2674 throw IException(IException::User, msg, _FILEINFO_);
2675 }
2676
2677 // Load times according to cache size (body rotations) -- handle first round of type 5 ck case
2678 // and multiple ck case --Load a time for every line scan line and downsize later
2679 if (! (timeLoaded || (p_cacheTime.size() > 1))) {
2680 double cacheSlope = 0.0;
2681 if (p_fullCacheSize > 1)
2682 cacheSlope = (p_fullCacheEndTime - p_fullCacheStartTime) / (double)(p_fullCacheSize - 1);
2683 for (int i = 0; i < p_fullCacheSize; i++)
2684 p_cacheTime.push_back(p_fullCacheStartTime + (double) i * cacheSlope);
2685 if (p_source == Nadir) {
2687 }
2688 }
2690 }
2691
2692
2700 std::vector<double> SpiceRotation::GetFullCacheTime() {
2701
2702 // No time cache was initialized -- throw an error
2703 if (p_fullCacheSize < 1) {
2704 QString msg = "Time cache not available -- rerun spiceinit";
2705 throw IException(IException::User, msg, _FILEINFO_);
2706 }
2707
2708 std::vector<double> fullCacheTime;
2709 double cacheSlope = 0.0;
2710 if (p_fullCacheSize > 1) {
2711 cacheSlope = (p_fullCacheEndTime - p_fullCacheStartTime) / (double)(p_fullCacheSize - 1);
2712 }
2713
2714 for (int i = 0; i < p_fullCacheSize; i++)
2715 fullCacheTime.push_back(p_fullCacheStartTime + (double) i * cacheSlope);
2716
2717 return fullCacheTime;
2718 }
2719
2720
2734 // The code for this method was extracted from the Naif routine rotget written by N.J. Bachman &
2735 // W.L. Taber (JPL)
2736 int center;
2737 FrameType type;
2738 int typid;
2739 SpiceBoolean found;
2740 int frmidx; // Frame chain index for current frame
2741 SpiceInt nextFrame; // Naif frame code of next frame
2743 std::vector<int> frameCodes;
2744 std::vector<int> frameTypes;
2745 frameCodes.push_back(p_constantFrames[0]);
2746
2747 while (frameCodes[frameCodes.size()-1] != J2000Code) {
2748 frmidx = frameCodes.size() - 1;
2749 // First get the frame type (Note:: we may also need to save center if we use dynamic frames)
2750 // (Another note: the type returned is the Naif from type. This is not quite the same as the
2751 // SpiceRotation enumerated FrameType. The SpiceRotation FrameType differentiates between
2752 // pck types. FrameTypes of 2, 6, and 7 will all be considered to be Naif frame type 2. The
2753 // logic for FrameTypes in this method is correct for all types except type 7. Current pck
2754 // do not exercise this option. Should we ever use pck with a target body not referenced to
2755 // the J2000 frame and epoch, both this method and loadPCFromSpice will need to be modified.
2756 frinfo_c((SpiceInt) frameCodes[frmidx],
2757 (SpiceInt *) &center,
2758 (SpiceInt *) &type,
2759 (SpiceInt *) &typid, &found);
2760
2761 if (!found) {
2762
2763 if (p_source == Nadir) {
2764 frameTypes.push_back(0);
2765 break;
2766 }
2767
2768 QString msg = "The frame" + toString((int) frameCodes[frmidx]) + " is not supported by Naif";
2769 throw IException(IException::Programmer, msg, _FILEINFO_);
2770 }
2771
2772 double matrix[3][3];
2773
2774 // To get the next link in the frame chain, use the frame type
2775 if (type == INERTL || type == PCK) {
2776 nextFrame = J2000Code;
2777 }
2778 else if (type == CK) {
2779 ckfrot_((SpiceInt *) &typid, &et, (double *) matrix, &nextFrame, (logical *) &found);
2780
2781 if (!found) {
2782
2783 if (p_source == Nadir) {
2784 frameTypes.push_back(0);
2785 break;
2786 }
2787
2788 QString msg = "The ck rotation from frame " + toString(frameCodes[frmidx]) + " can not "
2789 + "be found due to no pointing available at requested time or a problem with the "
2790 + "frame";
2791 throw IException(IException::Programmer, msg, _FILEINFO_);
2792 }
2793 }
2794 else if (type == TK) {
2795 tkfram_((SpiceInt *) &typid, (double *) matrix, &nextFrame, (logical *) &found);
2796 if (!found) {
2797 QString msg = "The tk rotation from frame " + toString(frameCodes[frmidx]) +
2798 " can not be found";
2799 throw IException(IException::Programmer, msg, _FILEINFO_);
2800 }
2801 }
2802 else if (type == DYN) {
2803 //
2804 // Unlike the other frame classes, the dynamic frame evaluation
2805 // routine ZZDYNROT requires the input frame ID rather than the
2806 // dynamic frame class ID. ZZDYNROT also requires the center ID
2807 // we found via the FRINFO call.
2808
2809 zzdynrot_((SpiceInt *) &typid, (SpiceInt *) &center, &et, (double *) matrix, &nextFrame);
2810 }
2811
2812 else {
2813 QString msg = "The frame " + toString(frameCodes[frmidx]) +
2814 " has a type " + toString(type) + " not supported by your version of Naif Spicelib."
2815 + "You need to update.";
2816 throw IException(IException::Programmer, msg, _FILEINFO_);
2817
2818 }
2819 frameCodes.push_back(nextFrame);
2820 frameTypes.push_back(type);
2821 }
2822
2823 if ((int) frameCodes.size() == 1 && p_source != Nadir) { // Must be Sky
2824 p_constantFrames.push_back(frameCodes[0]);
2825 p_timeFrames.push_back(frameCodes[0]);
2826 return;
2827 }
2828
2829 int nConstants = 0;
2830 p_constantFrames.clear();
2831 while (frameTypes[nConstants] == TK && nConstants < (int) frameTypes.size()) nConstants++;
2832
2833
2834 for (int i = 0; i <= nConstants; i++) {
2835 p_constantFrames.push_back(frameCodes[i]);
2836 }
2837
2838 if (p_source != Nadir) {
2839 for (int i = nConstants; i < (int) frameCodes.size(); i++) {
2840 p_timeFrames.push_back(frameCodes[i]);
2841 }
2842 }
2843 else {
2844 // Nadir rotation is from spacecraft to J2000
2845 p_timeFrames.push_back(frameCodes[nConstants]);
2846 p_timeFrames.push_back(J2000Code);
2847 }
2849 }
2850
2851
2857 std::vector<double> SpiceRotation::Matrix() {
2859 std::vector<double> TJ;
2860 TJ.resize(9);
2861 mxm_c((SpiceDouble *) &p_TC[0], (SpiceDouble *) &p_CJ[0], (SpiceDouble( *) [3]) &TJ[0]);
2863 return TJ;
2864 }
2865
2866
2872 std::vector<double> SpiceRotation::ConstantRotation() {
2874 std::vector<double> q;
2875 q.resize(4);
2876 m2q_c((SpiceDouble( *)[3]) &p_TC[0], &q[0]);
2878 return q;
2879 }
2880
2881
2887 std::vector<double> &SpiceRotation::ConstantMatrix() {
2888 return p_TC;
2889 }
2890
2891
2897 void SpiceRotation::SetConstantMatrix(std::vector<double> constantMatrix) {
2898 p_TC = constantMatrix;
2899 return;
2900 }
2901
2907 std::vector<double> SpiceRotation::TimeBasedRotation() {
2909 std::vector<double> q;
2910 q.resize(4);
2911 m2q_c((SpiceDouble( *)[3]) &p_CJ[0], &q[0]);
2913 return q;
2914 }
2915
2916
2922 std::vector<double> &SpiceRotation::TimeBasedMatrix() {
2923 return p_CJ;
2924 }
2925
2926
2932 void SpiceRotation::SetTimeBasedMatrix(std::vector<double> timeBasedMatrix) {
2933 p_CJ = timeBasedMatrix;
2934 return;
2935 }
2936
2937
2944 FrameTrace(et);
2945 // Get constant rotation which applies in all cases
2946 int targetFrame = p_constantFrames[0];
2947 int fromFrame = p_timeFrames[0];
2948 p_TC.resize(9);
2949 refchg_((SpiceInt *) &fromFrame, (SpiceInt *) &targetFrame, &et, (doublereal *) &p_TC[0]);
2950 // Transpose to obtain row-major order
2951 xpose_c((SpiceDouble( *)[3]) &p_TC[0], (SpiceDouble( *)[3]) &p_TC[0]);
2952 }
2953
2954
2976
2977 // Make sure the angles have been fit to polynomials so we can calculate the derivative
2978 if (p_source < PolyFunction ) {
2979 QString msg = "The SpiceRotation pointing angles must be fit to polynomials in order to ";
2980 msg += "compute angular velocity.";
2981 throw IException(IException::Programmer, msg, _FILEINFO_);
2982 }
2983 if (p_source == PckPolyFunction) {
2984 QString msg = "Planetary angular velocity must be fit computed with PCK polynomials ";
2985 throw IException(IException::Programmer, msg, _FILEINFO_);
2986 }
2987 std::vector<double> dCJdt;
2988 dCJdt.resize(9);
2989
2990 switch (m_frameType) {
2991 // Treat all cases the same except for target body rotations
2992 case UNKNOWN:
2993 case INERTL:
2994 case TK:
2995 case DYN:
2996 case CK:
2997 DCJdt(dCJdt);
2998 break;
2999 case PCK:
3000 case BPC:
3001 case NOTJ2000PCK:
3002 break;
3003 }
3004 double omega[3][3];
3005 mtxm_c((SpiceDouble( *)[3]) &dCJdt[0], (SpiceDouble( *)[3]) &p_CJ[0], omega);
3006 p_av[0] = omega[2][1];
3007 p_av[1] = omega[0][2];
3008 p_av[2] = omega[1][0];
3010 }
3011
3012
3019 void SpiceRotation::DCJdt(std::vector<double> &dCJ) {
3021
3022 // Get the rotation angles and axes
3023 std::vector<double> angles = Angles(p_axis3, p_axis2, p_axis1);
3024 int axes[3] = {p_axis1, p_axis2, p_axis3};
3025
3026 double dmatrix[3][3];
3027 double dangle;
3028 double wmatrix[3][3]; // work matrix
3029 dCJ.assign(9, 0.);
3030
3031 for (int angleIndex = 0; angleIndex < 3; angleIndex++) {
3032 drotat_(&(angles[angleIndex]), (integer *) axes + angleIndex, (doublereal *) dmatrix);
3033 // Transpose to obtain row-major order
3034 xpose_c(dmatrix, dmatrix);
3035
3036 // To get the derivative of the polynomial fit to the angle with respect to time
3037 // first create the function object for this angle and load its coefficients
3039 function.SetCoefficients(p_coefficients[angleIndex]);
3040
3041 // Evaluate the derivative of function at p_et
3042 // dangle = function.DerivativeVar((p_et - p_baseTime) / p_timeScale);
3043 dangle = function.DerivativeVar((p_et - p_baseTime) / p_timeScale) / p_timeScale;
3044
3045 // Multiply dangle to complete dmatrix
3046 for (int row = 0; row < 3; row++) {
3047 for (int col = 0; col < 3; col++) {
3048 dmatrix[row][col] *= dangle;
3049 }
3050 }
3051 // Apply the other 2 angles and chain them all together
3052 switch (angleIndex) {
3053 case 0:
3054 rotmat_c(dmatrix, angles[1], axes[1], dmatrix);
3055 rotmat_c(dmatrix, angles[2], axes[2], dmatrix);
3056 break;
3057 case 1:
3058 rotate_c(angles[0], axes[0], wmatrix);
3059 mxm_c(dmatrix, wmatrix, dmatrix);
3060 rotmat_c(dmatrix, angles[2], axes[2], dmatrix);
3061 break;
3062 case 2:
3063 rotate_c(angles[0], axes[0], wmatrix);
3064 rotmat_c(wmatrix, angles[1], axes[1], wmatrix);
3065 mxm_c(dmatrix, wmatrix, dmatrix);
3066 break;
3067 }
3068 int i, j;
3069 for (int index = 0; index < 9; index++) {
3070 i = index / 3;
3071 j = index % 3;
3072 dCJ[index] += dmatrix[i][j];
3073 }
3074 }
3075
3077 }
3078
3079
3085 std::vector<double> SpiceRotation::StateTJ() {
3086 std::vector<double> stateTJ(36);
3087
3088 // Build the state matrix for the time-based rotation from the matrix and angulary velocity
3089 double stateCJ[6][6];
3090 rav2xf_c(&p_CJ[0], &p_av[0], stateCJ);
3091// (SpiceDouble (*) [3]) &p_CJ[0]
3092 int irow = 0;
3093 int jcol = 0;
3094 int vpos = 0;
3095
3096 for (int row = 3; row < 6; row++) {
3097 irow = row - 3;
3098 vpos = irow * 3;
3099
3100 for (int col = 0; col < 3; col++) {
3101 jcol = col + 3;
3102 // Fill the upper left corner
3103 stateTJ[irow*6 + col] = p_TC[vpos] * stateCJ[0][col] + p_TC[vpos+1] * stateCJ[1][col]
3104 + p_TC[vpos+2] * stateCJ[2][col];
3105 // Fill the lower left corner
3106 stateTJ[row*6 + col] = p_TC[vpos] * stateCJ[3][col] + p_TC[vpos+1] * stateCJ[4][col]
3107 + p_TC[vpos+2] * stateCJ[5][col];
3108 // Fill the upper right corner
3109 stateTJ[irow*6 + jcol] = 0;
3110 // Fill the lower right corner
3111 stateTJ[row*6 +jcol] = stateTJ[irow*6 + col];
3112 }
3113 }
3114 return stateTJ;
3115 }
3116
3117
3128 std::vector<double> SpiceRotation::Extrapolate(double timeEt) {
3130
3132 return p_CJ;
3133 }
3134
3135 double diffTime = timeEt - p_et;
3136 std::vector<double> CJ(9, 0.0);
3137 double dmat[3][3];
3138
3139 // Create a rotation matrix for the axis and magnitude of the angular velocity multiplied by
3140 // the time difference
3141 axisar_c((SpiceDouble *) &p_av[0], diffTime*vnorm_c((SpiceDouble *) &p_av[0]), dmat);
3142
3143 // Rotate from the current time to the desired time assuming constant angular velocity
3144 mxm_c(dmat, (SpiceDouble *) &p_CJ[0], (SpiceDouble( *)[3]) &CJ[0]);
3146 return CJ;
3147 }
3148
3149
3157 void SpiceRotation::SetFullCacheParameters(double startTime, double endTime, int cacheSize) {
3158 // Save full cache parameters
3159 p_fullCacheStartTime = startTime;
3160 p_fullCacheEndTime = endTime;
3161 p_fullCacheSize = cacheSize;
3162 }
3163
3164
3173 // Get a count of all the loaded kernels
3174 SpiceInt count;
3175 ktotal_c("PCK", &count);
3176
3177 // Define some Naif constants
3178 int FILESIZ = 128;
3179 int TYPESIZ = 32;
3180 int SOURCESIZ = 128;
3181 SpiceChar file[FILESIZ];
3182 SpiceChar filetype[TYPESIZ];
3183 SpiceChar source[SOURCESIZ];
3184 SpiceInt handle;
3185 SpiceBoolean found;
3186
3187 // Get the name of each loaded kernel. The accuracy of this test depends on the use of the
3188 // "bpc" suffix to name a binary pck. If a binary bpc does not have the "bpc" suffix, it will not
3189 // be found by this test. This test was suggested by Boris Semenov at Naif.
3190 for (SpiceInt knum = 0; knum < count; knum++){
3191 kdata_c(knum, "PCK", FILESIZ, TYPESIZ, SOURCESIZ, file, filetype, source, &handle, &found);
3192
3193 // search file for "bpc"
3194 if (strstr(file, "bpc") != NULL) {
3195 m_frameType = BPC;
3196 }
3197 }
3198 }
3199
3200
3209 SpiceInt frameCode = p_constantFrames[0];
3210 SpiceBoolean found;
3211 SpiceInt centerBodyCode;
3212 SpiceInt frameClass;
3213 SpiceInt classId;
3214 frinfo_c(frameCode, &centerBodyCode, &frameClass, &classId, &found);
3215
3216 if (found) {
3217 if (frameClass == 2 || (centerBodyCode > 0 && frameClass != 3)) {
3218 m_frameType = PCK;
3219 // Load the PC information while it is available and set member values
3220 loadPCFromSpice(centerBodyCode);
3221 }
3222 else if (p_constantFrames.size() > 1) {
3223 for (std::vector<int>::size_type idx = 1; idx < p_constantFrames.size(); idx++) {
3224 frameCode = p_constantFrames[idx];
3225 frinfo_c(frameCode, &centerBodyCode, &frameClass, &classId, &found);
3226 if (frameClass == 3) m_frameType = CK;
3227 }
3228 }
3229 else {
3230 switch (frameClass) {
3231 case 1:
3233 break;
3234 // case 2: handled in first if block
3235 case 3:
3236 m_frameType = CK;
3237 break;
3238 case 4:
3239 m_frameType = TK;
3240 break;
3241 case 5:
3242 m_frameType = DYN;
3243 break;
3244 default:
3246 }
3247 }
3248 }
3249 }
3250
3251
3261 // If the cache has only one rotation, set it
3263 if (p_cacheTime.size() == 1) {
3264 p_CJ = m_orientation->getRotations()[0].toRotationMatrix();
3266 ale::Vec3d av = m_orientation->getAngularVelocities()[0];
3267 p_av[0] = av.x;
3268 p_av[1] = av.y;
3269 p_av[2] = av.z;
3270 }
3271 }
3272 // Otherwise determine the interval to interpolate
3273 else {
3274 p_CJ = m_orientation->interpolateTimeDep(p_et).toRotationMatrix();
3275
3277 ale::Vec3d av = m_orientation->interpolateAV(p_et);
3278 p_av[0] = av.x;
3279 p_av[1] = av.y;
3280 p_av[2] = av.z;
3281 }
3282 }
3284 }
3285
3286
3293 // TODO what about spk time bias and mission setting of light time corrections
3294 // That information has only been passed to the SpicePosition class and
3295 // is not available to this class, but probably should be applied to the
3296 // spkez call.
3297
3298 // Make sure the constant frame is loaded. This method also does the frame trace.
3300 if (p_timeFrames.size() == 0) InitConstantRotation(p_et);
3301
3302 SpiceDouble stateJ[6]; // Position and velocity vector in J2000
3303 SpiceDouble lt;
3304 SpiceInt spkCode = p_constantFrames[0] / 1000;
3305 spkez_c((SpiceInt) spkCode, p_et, "J2000", "LT+S",
3306 (SpiceInt) p_targetCode, stateJ, &lt);
3307 // reverse the position to be relative to the spacecraft. This may be
3308 // a mission dependent value and possibly the sense of the velocity as well.
3309 SpiceDouble sJ[3], svJ[3];
3310 vpack_c(-1 * stateJ[0], -1 * stateJ[1], -1 * stateJ[2], sJ);
3311 vpack_c(stateJ[3], stateJ[4], stateJ[5], svJ);
3312 twovec_c(sJ,
3313 p_axisP,
3314 svJ,
3315 p_axisV,
3316 (SpiceDouble( *)[3]) &p_CJ[0]);
3318 }
3319
3320
3333 SpiceInt j2000 = J2000Code;
3334
3335 SpiceDouble time = p_et + p_timeBias;
3336 // Make sure the constant frame is loaded. This method also does the frame trace.
3337 if (p_timeFrames.size() == 0) InitConstantRotation(p_et);
3338 int toFrame = p_timeFrames[0];
3339
3340 // First try getting the entire state matrix (6x6), which includes CJ and the angular velocity
3341 double stateCJ[6][6];
3342 frmchg_((integer *) &j2000, (integer *) &toFrame, &time, (doublereal *) stateCJ);
3343
3344 // If Naif fails attempting to get the state matrix, assume the angular velocity vector is
3345 // not available and just get the rotation matrix. First turn off Naif error reporting and
3346 // return any error without printing them.
3347 SpiceBoolean ckfailure = failed_c();
3348 reset_c(); // Reset Naif error system to allow caller to recover
3349
3350 if (!ckfailure) {
3351 xpose6_c(stateCJ, stateCJ);
3352 xf2rav_c(stateCJ, (SpiceDouble( *)[3]) &p_CJ[0], (SpiceDouble *) &p_av[0]);
3353 p_hasAngularVelocity = true;
3354 }
3355 else {
3356 refchg_((integer *) &j2000, (integer *) &toFrame, &time, (SpiceDouble *) &p_CJ[0]);
3357
3358 if (failed_c()) {
3359 char naifstr[64];
3360 getmsg_c("SHORT", sizeof(naifstr), naifstr);
3361 reset_c(); // Reset Naif error system to allow caller to recover
3362
3363 if (eqstr_c(naifstr, "SPICE(UNKNOWNFRAME)")) {
3364 Isis::IString msg = Isis::IString((int) p_constantFrames[0]) + " is an unrecognized " +
3365 "reference frame code. Has the mission frames kernel been loaded?";
3366 throw IException(IException::Io, msg, _FILEINFO_);
3367 }
3368 else {
3369 Isis::IString msg = "No pointing available at requested time [" +
3370 Isis::IString(p_et + p_timeBias) + "] for frame code [" +
3371 Isis::IString((int) p_constantFrames[0]) + "]";
3372 throw IException(IException::Io, msg, _FILEINFO_);
3373 }
3374 }
3375
3376 // Transpose to obtain row-major order
3377 xpose_c((SpiceDouble( *)[3]) &p_CJ[0], (SpiceDouble( *)[3]) &p_CJ[0]);
3378 }
3379
3381 }
3382
3383
3394
3395 // Load the functions with the coefficients
3396 function1.SetCoefficients(p_coefficients[0]);
3397 function2.SetCoefficients(p_coefficients[1]);
3398 function3.SetCoefficients(p_coefficients[2]);
3399
3400 std::vector<double> rtime;
3401 rtime.push_back((p_et - p_baseTime) / p_timeScale);
3402 std::vector<double> angles;
3403 angles.push_back(function1.Evaluate(rtime));
3404 angles.push_back(function2.Evaluate(rtime));
3405 angles.push_back(function3.Evaluate(rtime));
3406
3407 // Get the first angle back into the range Naif expects [-180.,180.]
3408 if (angles[0] <= -1 * pi_c()) {
3409 angles[0] += twopi_c();
3410 }
3411 else if (angles[0] > pi_c()) {
3412 angles[0] -= twopi_c();
3413 }
3414 return angles;
3415 }
3416
3417
3429
3430 // Load the functions with the coefficients
3431 function1.SetCoefficients(p_coefficients[0]);
3432 function2.SetCoefficients(p_coefficients[1]);
3433 function3.SetCoefficients(p_coefficients[2]);
3434
3435 std::vector<double> rtime;
3436 rtime.push_back((p_et - p_baseTime) / p_timeScale);
3437 double angle1 = function1.Evaluate(rtime);
3438 double angle2 = function2.Evaluate(rtime);
3439 double angle3 = function3.Evaluate(rtime);
3440
3441 // Get the first angle back into the range Naif expects [-180.,180.]
3442 if (angle1 < -1 * pi_c()) {
3443 angle1 += twopi_c();
3444 }
3445 else if (angle1 > pi_c()) {
3446 angle1 -= twopi_c();
3447 }
3448
3449 eul2m_c((SpiceDouble) angle3, (SpiceDouble) angle2, (SpiceDouble) angle1,
3451 (SpiceDouble( *)[3]) &p_CJ[0]);
3452
3454 if ( p_degree == 0){
3455 ale::Vec3d av = m_orientation->getAngularVelocities()[0];
3456 p_av[0] = av.x;
3457 p_av[1] = av.y;
3458 p_av[2] = av.z;
3459 }
3460 else {
3461 ComputeAv();
3462 }
3463 }
3465 }
3466
3467
3477 std::vector<double> cacheAngles(3);
3478 std::vector<double> cacheVelocity(3);
3479 cacheAngles = Angles(p_axis3, p_axis2, p_axis1);
3480 cacheVelocity = p_av;
3482 std::vector<double> polyAngles(3);
3483 // The decomposition fails because the angles are outside the valid range for Naif
3484 // polyAngles = Angles(p_axis3, p_axis2, p_axis1);
3485 polyAngles = EvaluatePolyFunction();
3486 std::vector<double> polyVelocity(3);
3487 polyVelocity = p_av;
3488 std::vector<double> angles(3);
3489
3490 for (int index = 0; index < 3; index++) {
3491 angles[index] = cacheAngles[index] + polyAngles[index];
3492 p_av[index] += cacheVelocity[index];
3493 }
3494
3495 if (angles[0] <= -1 * pi_c()) {
3496 angles[0] += twopi_c();
3497 }
3498 else if (angles[0] > pi_c()) {
3499 angles[0] -= twopi_c();
3500 }
3501
3502 if (angles[2] <= -1 * pi_c()) {
3503 angles[2] += twopi_c();
3504 }
3505 else if (angles[2] > pi_c()) {
3506 angles[2] -= twopi_c();
3507 }
3508
3509 eul2m_c((SpiceDouble) angles[2], (SpiceDouble) angles[1], (SpiceDouble) angles[0],
3511 (SpiceDouble( *)[3]) &p_CJ[0]);
3512 }
3513
3514
3521 // Set the scales
3522 double dTime = p_et / 86400.; // seconds to days conversion
3523 double centTime = dTime / 36525; // seconds to Julian centuries conversion
3524 double secondsPerJulianCentury = 86400. * 36525.;
3525
3526 // Calculate the quadratic part of the expression for the angles in degrees
3527 // Note: phi = ra + 90. and delta = 90 - dec for comparing results with older Isis 2 data
3528 Angle ra = m_raPole[0] + centTime*(m_raPole[1] + m_raPole[2]*centTime);
3529 Angle dec = m_decPole[0] + centTime*(m_decPole[1] + m_decPole[2]*centTime);
3530 Angle pm = m_pm[0] + dTime*(m_pm[1] + m_pm[2]*dTime);
3531 Angle dra = (m_raPole[1] + 2.*m_raPole[2]*centTime) / secondsPerJulianCentury;
3532 Angle ddec = (m_decPole[1] + 2.*m_decPole[2]*centTime) / secondsPerJulianCentury;
3533 Angle dpm = (m_pm[1] + 2.*m_pm[2]*dTime) / 86400;
3534
3535 // Now add the nutation/precession (trig part) adjustment to the expression if any
3536 int numNutPrec = (int) m_raNutPrec.size();
3537 Angle theta;
3538 double dtheta;
3539 double costheta;
3540 double sintheta;
3541
3542 for (int ia = 0; ia < numNutPrec; ia++) {
3543 theta = m_sysNutPrec0[ia] + m_sysNutPrec1[ia]*centTime;
3544 dtheta = m_sysNutPrec1[ia].degrees() * DEG2RAD;
3545 costheta = cos(theta.radians());
3546 sintheta = sin(theta.radians());
3547 ra += Angle(m_raNutPrec[ia] * sintheta, Angle::Degrees);
3548 dec += Angle(m_decNutPrec[ia] * costheta, Angle::Degrees);
3549 pm += Angle(m_pmNutPrec[ia] * sintheta, Angle::Degrees);
3550 dra += Angle(m_raNutPrec[ia] * dtheta / secondsPerJulianCentury * costheta, Angle::Degrees);
3551 ddec -= Angle(m_decNutPrec[ia] * dtheta / secondsPerJulianCentury * sintheta, Angle::Degrees);
3552 dpm += Angle(m_pmNutPrec[ia] * dtheta / secondsPerJulianCentury * costheta, Angle::Degrees);
3553 }
3554
3555 // Get pm in correct range
3556 pm = Angle(fmod(pm.degrees(), 360), Angle::Degrees); // Get pm back into the domain range
3557
3558 if (ra.radians() < -1 * pi_c()) {
3559 ra += Angle(twopi_c(), Angle::Radians);
3560 }
3561 else if (ra.radians() > pi_c()) {
3562 ra -= Angle(twopi_c(), Angle::Radians);
3563 }
3564
3565 if (pm.radians() < -1 * pi_c()) {
3566 pm += Angle(twopi_c(), Angle::Radians);
3567 }
3568 else if (pm.radians() > pi_c()) {
3569 pm -= Angle(twopi_c(), Angle::Radians);
3570 }
3571
3572 // Convert to Euler angles to get the matrix
3573 SpiceDouble phi = ra.radians() + HALFPI;
3574 SpiceDouble delta = HALFPI - dec.radians();
3575 SpiceDouble w = pm.radians();
3576 SpiceDouble dphi = dra.radians();
3577 SpiceDouble ddelta = -ddec.radians();
3578 SpiceDouble dw = dpm.radians();
3579
3580 // Load the Euler angles and their derivatives into an array and create the state matrix
3581 SpiceDouble angsDangs[6];
3582 SpiceDouble BJs[6][6];
3583 vpack_c(w, delta, phi, angsDangs);
3584 vpack_c(dw, ddelta, dphi, &angsDangs[3]);
3585 eul2xf_c (angsDangs, p_axis3, p_axis2, p_axis1, BJs);
3586
3587 // Decompose the state matrix to the rotation and its angular velocity
3588 xf2rav_c(BJs, (SpiceDouble( *)[3]) &p_CJ[0], (SpiceDouble *) &p_av[0]);
3589 }
3590}
Defines an angle and provides unit conversions.
Definition Angle.h:45
double radians() const
Convert an angle to a double.
Definition Angle.h:226
@ 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
double Evaluate(const std::vector< double > &vars)
Compute the equation using the input variables.
int Coefficients() const
Returns the number of coefficients for the equation.
double Coefficient(int i) const
Returns the ith coefficient.
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.
int Solve(Isis::LeastSquares::SolveMethod method=SVD)
After all the data has been registered through AddKnown, invoke this method to solve the system of eq...
void AddKnown(const std::vector< double > &input, double expected, double weight=1.0)
Invoke this method for each set of knowns.
Utility class for creating and using cartesean line equations.
double Slope()
Compute the slope of the line.
double Intercept()
Compute the intercept of the line.
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
int size() const
Returns the number of values stored in this keyword.
Definition PvlKeyword.h:133
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 ...
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 > ToMatrix()
Converts quaternion to 3x3 rotational matrix.
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:371
PvlObject & Label()
The Table's label.
Definition Table.cpp:318
int Fields() const
Returns the number of fields that are currently in the record.
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