/** Cache J2000 rotation over existing cached time range using polynomials
*
* This method will reload an internal cache with matrices
* formed from rotation angles fit to polynomials over a time
* range.
*
* @param function1 The first polynomial function used to
* find the rotation angles
* @param function2 The second polynomial function used to
* find the rotation angles
* @param function3 The third polynomial function used to
* find the rotation angles
*/
void LineScanCameraRotation::ReloadCache() {
NaifStatus::CheckErrors();
// Make sure caches are already loaded
if(!p_cachesLoaded) {
QString msg = "A LineScanCameraRotation cache has not been loaded yet";
throw IException(IException::Programmer, msg, _FILEINFO_);
}
// Clear existing matrices from cache
p_cache.clear();
// Create polynomials fit to angles & use to reload cache
Isis::PolynomialUnivariate function1(p_degree);
Isis::PolynomialUnivariate function2(p_degree);
Isis::PolynomialUnivariate function3(p_degree);
// Get the coefficients of the polynomials already fit to the angles of rotation defining [CI]
std::vector<double> coeffAng1;
std::vector<double> coeffAng2;
std::vector<double> coeffAng3;
GetPolynomial(coeffAng1, coeffAng2, coeffAng3);
// Reset linear term to center around zero -- what works best is either roll-avg & pitchavg+ or pitchavg+ & yawavg-
// coeffAng1[1] -= 0.0000158661225;
// coeffAng2[1] = 0.0000308433;
// coeffAng3[0] = -0.001517547;
if(p_pitchRate) coeffAng2[1] = p_pitchRate;
if(p_yaw) coeffAng3[0] = p_yaw;
// Load the functions with the coefficients
function1.SetCoefficients(coeffAng1);
function2.SetCoefficients(coeffAng2);
function3.SetCoefficients(coeffAng3);
double CI[3][3];
double IJ[3][3];
std::vector<double> rtime;
SpiceRotation *prot = p_spi->bodyRotation();
std::vector<double> CJ;
CJ.resize(9);
for(std::vector<double>::size_type pos = 0; pos < p_cacheTime.size(); pos++) {
double et = p_cacheTime.at(pos);
rtime.push_back((et - GetBaseTime()) / GetTimeScale());
double angle1 = function1.Evaluate(rtime);
double angle2 = function2.Evaluate(rtime);
double angle3 = function3.Evaluate(rtime);
rtime.clear();
// Get the first angle back into the range Naif expects [180.,180.]
if(angle1 < -1 * pi_c()) {
angle1 += twopi_c();
}
else if(angle1 > pi_c()) {
angle1 -= twopi_c();
}
eul2m_c((SpiceDouble) angle3, (SpiceDouble) angle2, (SpiceDouble) angle1,
p_axis3, p_axis2, p_axis1,
CI);
mxm_c((SpiceDouble( *)[3]) & (p_jitter->SetEphemerisTimeHPF(et))[0], CI, CI);
prot->SetEphemerisTime(et);
mxm_c((SpiceDouble( *)[3]) & (p_cacheIB.at(pos))[0], (SpiceDouble( *)[3]) & (prot->Matrix())[0], IJ);
mxm_c(CI, IJ, (SpiceDouble( *)[3]) &CJ[0]);
p_cache.push_back(CJ); // J2000 to constant frame
}
// Set source to cache to get updated values
SetSource(SpiceRotation::Memcache);
// Make sure SetEphemerisTime updates the matrix by resetting it twice (in case the first one
// matches the current et. p_et is private and not available from the child class
NaifStatus::CheckErrors();
SetEphemerisTime(p_cacheTime[0]);
SetEphemerisTime(p_cacheTime[1]);
NaifStatus::CheckErrors();
}
void rav2xf_c ( ConstSpiceDouble rot [3][3],
ConstSpiceDouble av [3],
SpiceDouble xform [6][6] )
/*
-Brief_I/O
VARIABLE I/O DESCRIPTION
-------- --- --------------------------------------------------
rot I Rotation matrix.
av I Angular velocity vector.
xform O State transformation associated with rot and av.
-Detailed_Input
rot is a rotation that gives the transformation from
some frame frame1 to another frame frame2.
av is the angular velocity of the transformation.
In other words, if p is the position of a fixed
point in frame2, then from the point of view of
frame1, p rotates (in a right handed sense) about
an axis parallel to av. Moreover the rate of rotation
in radians per unit time is given by the length of
av.
More formally, the velocity v of p in frame1 is
given by
t
v = av x ( rot * p )
-Detailed_Output
xform is a state transformation matrix associated
with rot and av. If s1 is the state of an object
with respect to frame1, then the state s2 of the
object with respect to frame2 is given by
s2 = xform * s1
where "*" denotes matrix-vector multiplication.
-Parameters
None.
-Exceptions
Error free.
1) No checks are performed on ROT to ensure that it is indeed
a rotation matrix.
-Files
None.
-Particulars
This routine is essentially a macro routine for converting
a rotation and angular velocity of the rotation to the
equivalent state transformation matrix.
This routine is an inverse of xf2rav_c.
-Examples
Suppose that you wanted to determine state transformation
matrix from a platform frame to the J2000 frame.
/.
The following call obtains the J2000-to-platform transformation
matrix and platform angular velocity at the time of interest.
The time value is expressed as encoded SCLK.
./
ckgpav_c ( ckid, time, tol, "J2000", rot, av, &clkout, &fnd );
/.
Recall that rot and av are the rotation and angular velocity
of the transformation from J2000 to the platform frame.
./
if ( fnd )
{
/.
First get the state transformation from J2000 to the platform
frame.
./
rav2xf_c ( rot, av, j2plt );
/.
Invert the state transformation matrix (using invstm_c) to
the desired state transformation matrix.
./
invstm_c ( j2plt, xform );
}
-Restrictions
None.
-Literature_References
None.
-Author_and_Institution
N.J. Bachman (JPL)
W.L. Taber (JPL)
-Version
-CSPICE Version 1.0.1, 12-APR-2007 (EDW)
Edit to abstract.
-CSPICE Version 1.0.0, 18-JUN-1999 (WLT) (NJB)
-Index_Entries
State transformation to rotation and angular velocity
-&
*/
{ /* Begin rav2xf_c */
/*
Local variables
*/
SpiceDouble drdt [3][3];
SpiceDouble omegat [3][3];
SpiceInt i;
SpiceInt j;
/*
Error free: no tracing required.
A state transformation matrix xform has the following form
[ | ]
| r | 0 |
| | |
| -----+-----|
| dr | |
| -- | r |
[ dt | ]
where r is a rotation and dr/dt is the time derivative of that
rotation. From this we can immediately fill in most of the
state transformation matrix.
*/
for ( i = 0; i < 3; i++ )
{
for ( j = 0; j < 3; j++ )
{
xform[i ][j ] = rot [i][j];
xform[i+3][j+3] = rot [i][j];
xform[i ][j+3] = 0.;
}
}
/*
Now for the rest.
Recall that rot is a transformation that converts positions
in some frame frame1 to positions in a second frame frame2.
The angular velocity matrix omega (the cross product matrix
corresponding to av) has the following property.
If p is the position of an object that is stationary with
respect to frame2 then the velocity v of that object in frame1
is given by:
t
v = omega * rot * p
But v is also given by
t
d rot
v = ----- * p
dt
So that
t
t d rot
omega * rot = -------
dt
Hence
d rot t
----- = rot * omega
dt
From this discussion we can see that we need omega transpose.
Here it is.
*/
omegat[0][0] = 0.0;
omegat[1][0] = -av[2];
omegat[2][0] = av[1];
omegat[0][1] = av[2];
omegat[1][1] = 0.0;
omegat[2][1] = -av[0];
omegat[0][2] = -av[1];
omegat[1][2] = av[0];
omegat[2][2] = 0.0;
mxm_c ( rot, omegat, drdt );
for ( i = 0; i < 3; i++ )
{
for ( j = 0; j < 3; j++ )
{
xform[i+3][j] = drdt [i][j];
}
}
} /* End rav2xf_c */
