/** Compute ground position from slant range * * @param ux Slant range distance * @param uy Doppler shift (always 0.0) * @param uz Not used * * @return conversion was successful */ bool RadarGroundMap::SetFocalPlane(const double ux, const double uy, double uz) { SpiceRotation *bodyFrame = p_camera->BodyRotation(); SpicePosition *spaceCraft = p_camera->InstrumentPosition(); // Get spacecraft position and velocity to create a state vector std::vector<double> Ssc(6); // Load the state into Ssc vequ_c ( (SpiceDouble *) &(spaceCraft->Coordinate()[0]), &Ssc[0]); vequ_c ( (SpiceDouble *) &(spaceCraft->Velocity()[0]), &Ssc[3]); // Rotate state vector to body-fixed std::vector<double> bfSsc(6); bfSsc = bodyFrame->ReferenceVector(Ssc); // Extract body-fixed position and velocity std::vector<double> Vsc(3); std::vector<double> Xsc(3); vequ_c ( &bfSsc[0], (SpiceDouble *) &(Xsc[0]) ); vequ_c ( &bfSsc[3], (SpiceDouble *) &(Vsc[0]) ); // Compute intrack, crosstrack, and radial coordinate SpiceDouble i[3]; vhat_c (&Vsc[0],i); SpiceDouble c[3]; SpiceDouble dp; dp = vdot_c(&Xsc[0],i); SpiceDouble p[3],q[3]; vscl_c(dp,i,p); vsub_c(&Xsc[0],p,q); vhat_c(q,c); SpiceDouble r[3]; vcrss_c(i,c,r); // What is the initial guess for R double radii[3]; p_camera->Radii(radii); SpiceDouble R = radii[0]; SpiceDouble lastR = DBL_MAX; SpiceDouble rlat; SpiceDouble rlon; SpiceDouble lat = DBL_MAX; SpiceDouble lon = DBL_MAX; double slantRangeSqr = (ux * p_rangeSigma) / 1000.; slantRangeSqr = slantRangeSqr*slantRangeSqr; SpiceDouble X[3]; int iter = 0; do { double normXsc = vnorm_c(&Xsc[0]); double alpha = (R*R - slantRangeSqr - normXsc*normXsc) / (2.0 * vdot_c(&Xsc[0],c)); double arg = slantRangeSqr - alpha*alpha; if (arg < 0.0) return false; double beta = sqrt(arg); if (p_lookDirection == Radar::Left) beta *= -1.0; SpiceDouble alphac[3],betar[3]; vscl_c(alpha,c,alphac); vscl_c(beta,r,betar); vadd_c(alphac,betar,alphac); vadd_c(&Xsc[0],alphac,X); // Convert X to lat,lon lastR = R; reclat_c(X,&R,&lon,&lat); rlat = lat*180.0/Isis::PI; rlon = lon*180.0/Isis::PI; R = GetRadius(rlat,rlon); iter++; } while (fabs(R-lastR) > p_tolerance && iter < 30); if (fabs(R-lastR) > p_tolerance) return false; lat = lat*180.0/Isis::PI; lon = lon*180.0/Isis::PI; while (lon < 0.0) lon += 360.0; // Compute body fixed look direction std::vector<double> lookB; lookB.resize(3); lookB[0] = X[0] - Xsc[0]; lookB[1] = X[1] - Xsc[1]; lookB[2] = X[2] - Xsc[2]; std::vector<double> lookJ = bodyFrame->J2000Vector(lookB); SpiceRotation *cameraFrame = p_camera->InstrumentRotation(); std::vector<double> lookC = cameraFrame->ReferenceVector(lookJ); SpiceDouble unitLookC[3]; vhat_c(&lookC[0],unitLookC); p_camera->SetLookDirection(unitLookC); return p_camera->Sensor::SetUniversalGround(lat,lon); }
/** Cache J2000 rotation quaternion over a time range. * * This method will load an internal cache with frames over a time * range. This prevents the NAIF kernels from being read over-and-over * again and slowing an application down due to I/O performance. Once the * cache has been loaded then the kernels can be unloaded from the NAIF * system. * * @internal * @history 2010-12-23 Debbie A. Cook Added set of full cache time * parameters */ void LineScanCameraRotation::LoadCache() { NaifStatus::CheckErrors(); double startTime = p_cacheTime[0]; int size = p_cacheTime.size(); double endTime = p_cacheTime[size-1]; SetFullCacheParameters(startTime, endTime, size); // TODO Add a label value to indicate pointing is already decomposed to line scan angles // and set p_pointingDecomposition=none,framing angles, or line scan angles. // Also add a label value to indicate jitterOffsets=jitterFileName // Then we can decide whether to simply grab the crot angles or do new decomposition and whether // to apply jitter or throw an error because jitter has already been applied. // *** May need to do a frame trace and load the frames (at least the constant ones) *** // Loop and load the cache double state[6]; double lt; NaifStatus::CheckErrors(); double R[3]; // Direction of radial axis of line scan camera double C[3]; // Direction of cross-track axis double I[3]; // Direction of in-track axis double *velocity; std::vector<double> IB(9); std::vector<double> CI(9); SpiceRotation *prot = p_spi->bodyRotation(); SpiceRotation *crot = p_spi->instrumentRotation(); for(std::vector<double>::iterator i = p_cacheTime.begin(); i < p_cacheTime.end(); i++) { double et = *i; prot->SetEphemerisTime(et); crot->SetEphemerisTime(et); // The following code will be put into method LoadIBcache() spkezr_c("MRO", et, "IAU_MARS", "NONE", "MARS", state, <); NaifStatus::CheckErrors(); // Compute the direction of the radial axis (3) of the line scan camera vscl_c(1. / vnorm_c(state), state, R); // vscl and vnorm only operate on first 3 members of state // Compute the direction of the cross-track axis (2) of the line scan camera velocity = state + 3; vscl_c(1. / vnorm_c(velocity), velocity, C); vcrss_c(R, C, C); // Compute the direction of the in-track axis (1) of the line scan camera vcrss_c(C, R, I); // Load the matrix IB and enter it into the cache vequ_c(I, (SpiceDouble( *)) &IB[0]); vequ_c(C, (SpiceDouble( *)) &IB[3]); vequ_c(R, (SpiceDouble( *)) &IB[6]); p_cacheIB.push_back(IB); // end IB code // Compute the CIcr matrix - in-track, cross-track, radial frame to constant frame mxmt_c((SpiceDouble( *)[3]) & (crot->TimeBasedMatrix())[0], (SpiceDouble( *)[3]) & (prot->Matrix())[0], (SpiceDouble( *)[3]) &CI[0]); // Put CI into parent cache to use the parent class methods on it mxmt_c((SpiceDouble( *)[3]) &CI[0], (SpiceDouble( *)[3]) &IB[0], (SpiceDouble( *)[3]) &CI[0]); p_cache.push_back(CI); } p_cachesLoaded = true; SetSource(Memcache); NaifStatus::CheckErrors(); }
/** Compute undistorted focal plane coordinate from ground position that includes a local radius * * @param lat planetocentric latitude in degrees * @param lon planetocentric longitude in degrees * @param radius local radius in meters * * @return conversion was successful */ bool RadarGroundMap::SetGround(const double lat, const double lon, const double radius) { // Get the ground point in rectangular coordinates (X) SpiceDouble X[3]; SpiceDouble rlat = lat*Isis::PI/180.0; SpiceDouble rlon = lon*Isis::PI/180.0; latrec_c(radius,rlon,rlat,X); // Compute lower bound for Doppler shift double et1 = p_camera->Spice::CacheStartTime(); p_camera->Sensor::SetEphemerisTime(et1); double xv1 = ComputeXv(X); // Compute upper bound for Doppler shift double et2 = p_camera->Spice::CacheEndTime(); p_camera->Sensor::SetEphemerisTime(et2); double xv2 = ComputeXv(X); // Make sure we bound root (xv = 0.0) if ((xv1 < 0.0) && (xv2 < 0.0)) return false; if ((xv1 > 0.0) && (xv2 > 0.0)) return false; // Order the bounds double fl,fh,xl,xh; if (xv1 < xv2) { fl = xv1; fh = xv2; xl = et1; xh = et2; } else { fl = xv2; fh = xv1; xl = et2; xh = et1; } // Iterate a max of 30 times for (int j=0; j<30; j++) { // Use the secant method to guess the next et double etGuess = xl + (xh - xl) * fl / (fl - fh); // Compute the guessed Doppler shift. Hopefully // this guess converges to zero at some point p_camera->Sensor::SetEphemerisTime(etGuess); double fGuess = ComputeXv(X); // Update the bounds double delTime; if (fGuess < 0.0) { delTime = xl - etGuess; xl = etGuess; fl = fGuess; } else { delTime = xh - etGuess; xh = etGuess; fh = fGuess; } // See if we are done if ((fabs(delTime) <= p_timeTolerance) || (fGuess == 0.0)) { SpiceRotation *bodyFrame = p_camera->BodyRotation(); SpicePosition *spaceCraft = p_camera->InstrumentPosition(); // Get body fixed spacecraft velocity and position std::vector<double> Ssc(6); // Load the state into Ssc and rotate to body-fixed vequ_c ( (SpiceDouble *) &(spaceCraft->Coordinate()[0]), &Ssc[0]); vequ_c ( (SpiceDouble *) &(spaceCraft->Velocity()[0]), &Ssc[3]); std::vector<double> bfSsc(6); bfSsc = bodyFrame->ReferenceVector(Ssc); // Extract the body-fixed position and velocity from the state std::vector<double> Vsc(3); std::vector<double> Xsc(3); vequ_c ( &bfSsc[0], (SpiceDouble *) &(Xsc[0]) ); vequ_c ( &bfSsc[3], (SpiceDouble *) &(Vsc[0]) ); // Determine if focal plane coordinate falls on the correct side of the // spacecraft. Radar has both left and right look directions. Make sure // the coordinate is on the same side as the look direction. This is done // by (X - S) . (V x S) where X=ground point vector, S=spacecraft position // vector, and V=velocity vector. If the dot product is greater than 0, then // the point is on the right side. If the dot product is less than 0, then // the point is on the left side. If the dot product is 0, then the point is // directly under the spacecraft (neither left or right) and is invalid. SpiceDouble vout1[3]; SpiceDouble vout2[3]; SpiceDouble dp; vsub_c(X,&Xsc[0],vout1); vcrss_c(&Vsc[0],&Xsc[0],vout2); dp = vdot_c(vout1,vout2); if (dp > 0.0 && p_lookDirection == Radar::Left) return false; if (dp < 0.0 && p_lookDirection == Radar::Right) return false; if (dp == 0.0) return false; // Compute body fixed look direction std::vector<double> lookB; lookB.resize(3); lookB[0] = X[0] - Xsc[0]; lookB[1] = X[1] - Xsc[1]; lookB[2] = X[2] - Xsc[2]; std::vector<double> lookJ = bodyFrame->J2000Vector(lookB); SpiceRotation *cameraFrame = p_camera->InstrumentRotation(); std::vector<double> lookC = cameraFrame->ReferenceVector(lookJ); SpiceDouble unitLookC[3]; vhat_c(&lookC[0],unitLookC); p_camera->SetLookDirection(unitLookC); p_camera->SetFocalLength(p_slantRange*1000.0); p_focalPlaneX = p_slantRange / p_rangeSigma; p_focalPlaneY = 0.0; return true; } } return false; }
void qxq_c ( ConstSpiceDouble q1 [4], ConstSpiceDouble q2 [4], SpiceDouble qout [4] ) /* -Brief_I/O VARIABLE I/O DESCRIPTION -------- --- -------------------------------------------------- q1 I First SPICE quaternion factor. q2 I Second SPICE quaternion factor. qout O Product of `q1' and `q2'. -Detailed_Input q1 is a 4-vector representing a SPICE-style quaternion. See the discussion of "Quaternion Styles" in the Particulars section below. Note that multiple styles of quaternions are in use. This routine will not work properly if the input quaternions do not conform to the SPICE convention. q2 is a second SPICE-style quaternion. -Detailed_Output qout is 4-vector representing the quaternion product q1 * q2 Representing q(i) as the sums of scalar (real) part s(i) and vector (imaginary) part v(i) respectively, q1 = s1 + v1 q2 = s2 + v2 qout has scalar part s3 defined by s3 = s1 * s2 - <v1, v2> and vector part v3 defined by v3 = s1 * v2 + s2 * v1 + v1 x v2 where the notation < , > denotes the inner product operator and x indicates the cross product operator. -Parameters None. -Exceptions Error free. -Files None. -Particulars Quaternion Styles ----------------- There are different "styles" of quaternions used in science and engineering applications. Quaternion styles are characterized by - The order of quaternion elements - The quaternion multiplication formula - The convention for associating quaternions with rotation matrices Two of the commonly used styles are - "SPICE" > Invented by Sir William Rowan Hamilton > Frequently used in mathematics and physics textbooks - "Engineering" > Widely used in aerospace engineering applications CSPICE function interfaces ALWAYS use SPICE quaternions. Quaternions of any other style must be converted to SPICE quaternions before they are passed to CSPICE functions. Relationship between SPICE and Engineering Quaternions ------------------------------------------------------ Let M be a rotation matrix such that for any vector V, M*V is the result of rotating V by theta radians in the counterclockwise direction about unit rotation axis vector A. Then the SPICE quaternions representing M are (+/-) ( cos(theta/2), sin(theta/2) A(1), sin(theta/2) A(2), sin(theta/2) A(3) ) while the engineering quaternions representing M are (+/-) ( -sin(theta/2) A(1), -sin(theta/2) A(2), -sin(theta/2) A(3), cos(theta/2) ) For both styles of quaternions, if a quaternion q represents a rotation matrix M, then -q represents M as well. Given an engineering quaternion QENG = ( q0, q1, q2, q3 ) the equivalent SPICE quaternion is QSPICE = ( q3, -q0, -q1, -q2 ) Associating SPICE Quaternions with Rotation Matrices ---------------------------------------------------- Let FROM and TO be two right-handed reference frames, for example, an inertial frame and a spacecraft-fixed frame. Let the symbols V , V FROM TO denote, respectively, an arbitrary vector expressed relative to the FROM and TO frames. Let M denote the transformation matrix that transforms vectors from frame FROM to frame TO; then V = M * V TO FROM where the expression on the right hand side represents left multiplication of the vector by the matrix. Then if the unit-length SPICE quaternion q represents M, where q = (q0, q1, q2, q3) the elements of M are derived from the elements of q as follows: +- -+ | 2 2 | | 1 - 2*( q2 + q3 ) 2*(q1*q2 - q0*q3) 2*(q1*q3 + q0*q2) | | | | | | 2 2 | M = | 2*(q1*q2 + q0*q3) 1 - 2*( q1 + q3 ) 2*(q2*q3 - q0*q1) | | | | | | 2 2 | | 2*(q1*q3 - q0*q2) 2*(q2*q3 + q0*q1) 1 - 2*( q1 + q2 ) | | | +- -+ Note that substituting the elements of -q for those of q in the right hand side leaves each element of M unchanged; this shows that if a quaternion q represents a matrix M, then so does the quaternion -q. To map the rotation matrix M to a unit quaternion, we start by decomposing the rotation matrix as a sum of symmetric and skew-symmetric parts: 2 M = [ I + (1-cos(theta)) OMEGA ] + [ sin(theta) OMEGA ] symmetric skew-symmetric OMEGA is a skew-symmetric matrix of the form +- -+ | 0 -n3 n2 | | | OMEGA = | n3 0 -n1 | | | | -n2 n1 0 | +- -+ The vector N of matrix entries (n1, n2, n3) is the rotation axis of M and theta is M's rotation angle. Note that N and theta are not unique. Let C = cos(theta/2) S = sin(theta/2) Then the unit quaternions Q corresponding to M are Q = +/- ( C, S*n1, S*n2, S*n3 ) The mappings between quaternions and the corresponding rotations are carried out by the CSPICE routines q2m_c {quaternion to matrix} m2q_c {matrix to quaternion} m2q_c always returns a quaternion with scalar part greater than or equal to zero. SPICE Quaternion Multiplication Formula --------------------------------------- Given a SPICE quaternion Q = ( q0, q1, q2, q3 ) corresponding to rotation axis A and angle theta as above, we can represent Q using "scalar + vector" notation as follows: s = q0 = cos(theta/2) v = ( q1, q2, q3 ) = sin(theta/2) * A Q = s + v Let Q1 and Q2 be SPICE quaternions with respective scalar and vector parts s1, s2 and v1, v2: Q1 = s1 + v1 Q2 = s2 + v2 We represent the dot product of v1 and v2 by <v1, v2> and the cross product of v1 and v2 by v1 x v2 Then the SPICE quaternion product is Q1*Q2 = s1*s2 - <v1,v2> + s1*v2 + s2*v1 + (v1 x v2) If Q1 and Q2 represent the rotation matrices M1 and M2 respectively, then the quaternion product Q1*Q2 represents the matrix product M1*M2 -Examples 1) Let qid, qi, qj, qk be the "basis" quaternions qid = ( 1, 0, 0, 0 ) qi = ( 0, 1, 0, 0 ) qj = ( 0, 0, 1, 0 ) qk = ( 0, 0, 0, 1 ) respectively. Then the calls qxq_c ( qi, qj, ixj ); qxq_c ( qj, qk, jxk ); qxq_c ( qk, qi, kxi ); produce the results ixj == qk jxk == qi kxi == qj All of the calls qxq_c ( qi, qi, qout ); qxq_c ( qj, qj, qout ); qxq_c ( qk, qk, qout ); produce the result qout == -qid For any quaternion Q, the calls qxq_c ( qid, q, qout ); qxq_c ( q, qid, qout ); produce the result qout == q 2) Composition of rotations: let `cmat1' and `cmat2' be two C-matrices (which are rotation matrices). Then the following code fragment computes the product cmat1 * cmat2: /. Convert the C-matrices to quaternions. ./ m2q_c ( cmat1, q1 ); m2q_c ( cmat2, q2 ); /. Find the product. ./ qxq_c ( q1, q2, qout ); /. Convert the result to a C-matrix. ./ q2m_c ( qout, cmat3 ); /. Multiply `cmat1' and `cmat2' directly. ./ mxm_c ( cmat1, cmat2, cmat4 ); /. Compare the results. The difference `diff' of `cmat3' and `cmat4' should be close to the zero matrix. ./ vsubg_c ( 9, cmat3, cmat4, diff ); -Restrictions None. -Literature_References None. -Author_and_Institution N.J. Bachman (JPL) -Version -CSPICE Version 1.0.1, 27-FEB-2008 (NJB) Updated header; added information about SPICE quaternion conventions. -CSPICE Version 1.0.0, 27-OCT-2005 (NJB) -Index_Entries quaternion times quaternion multiply quaternion by quaternion -& */ { /* Begin qxq_c */ /* Local variables */ SpiceDouble cross[3]; /* This routine is error free. */ /* Assign the scalar portion of the product `vout'. */ qout[0] = q1[0]*q2[0] - vdot_c( q1+1, q2+1 ); /* Compute the cross product term of the vector component of vout. */ vcrss_c ( q1+1, q2+1, cross ); /* Assign the vector portion of the product `vout'. */ vlcom3_c ( q1[0], q2+1, q2[0], q1+1, 1.0, cross, qout+1 ); } /* End qxq_c */