double RadarGroundMap::ComputeXv(SpiceDouble X[3]) {
// Get the spacecraft position (Xsc) and velocity (Vsc) in body fixed
// coordinates
SpiceRotation *bodyFrame = p_camera->BodyRotation();
SpicePosition *spaceCraft = p_camera->InstrumentPosition();
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 the state to body-fixed
std::vector<double> bfSsc(6);
bfSsc = bodyFrame->ReferenceVector(Ssc);
// Extract the body-fixed position and velocity
std::vector<double> Vsc(3);
std::vector<double> Xsc(3);
vequ_c ( &bfSsc[0], &Xsc[0] );
vequ_c ( &bfSsc[3], &Vsc[0] );
// Compute the slant range
SpiceDouble lookB[3];
vsub_c(&Xsc[0],X,lookB);
p_slantRange = vnorm_c(lookB);
// Compute and return xv
double xv = -2.0 * vdot_c(lookB,&Vsc[0]) / (vnorm_c(lookB) * p_waveLength);
return xv;
}
void gfq2 (SpiceDouble et, SpiceDouble *value) {
SpiceDouble v[3], lt;
spkezp_c(1,et,"J2000","NONE",0,v,<);
sprintf(s,"%f %f %f",v[0],v[1],v[2]);
*value = vnorm_c(v);
}
void gfq5 (SpiceDouble et, SpiceDouble *value) {
SpiceDouble u[3], v[3], lt;
spkezp_c(10,et,REF,"NONE",601,u,<);
spkezp_c( 5,et,REF,"NONE",601,v,<);
// set global variable "s" to extra data, caller not required to use it
// This is really ugly; if global variable 'extra' is set, print out
// extra information; this should only be used when printing results
// NOT during the search phase (TODO: this is ugly, better way?)
if (vnorm_c(u)<vnorm_c(v)) {
strcpy(s, "SUN IS CLOSER");
} else {
strcpy(s, "POSSIBLE TRANSIT");
}
*value = vsep_c(u,v)*180./pi_c();
}
/** 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);
}
void npelpt_c ( ConstSpiceDouble point [3],
ConstSpiceEllipse * ellips,
SpiceDouble pnear [3],
SpiceDouble * dist )
/*
-Brief_I/O
Variable I/O Description
-------- --- --------------------------------------------------
point I Point whose distance to an ellipse is to be found.
ellips I A CSPICE ellipse.
pnear O Nearest point on ellipse to input point.
dist O Distance of input point to ellipse.
-Detailed_Input
ellips is a CSPICE ellipse that represents an ellipse
in three-dimensional space.
point is a point in 3-dimensional space.
-Detailed_Output
pnear is the nearest point on ellips to point.
dist is the distance between point and pnear. This is
the distance between point and the ellipse.
-Parameters
None.
-Exceptions
1) Invalid ellipses will be diagnosed by routines called by
this routine.
2) Ellipses having one or both semi-axis lengths equal to zero
are turned away at the door; the error SPICE(DEGENERATECASE)
is signalled.
3) If the geometric ellipse represented by ellips does not
have a unique point nearest to the input point, any point
at which the minimum distance is attained may be returned
in pnear.
-Files
None.
-Particulars
Given an ellipse and a point in 3-dimensional space, if the
orthogonal projection of the point onto the plane of the ellipse
is on or outside of the ellipse, then there is a unique point on
the ellipse closest to the original point. This routine finds
that nearest point on the ellipse. If the projection falls inside
the ellipse, there may be multiple points on the ellipse that are
at the minimum distance from the original point. In this case,
one such closest point will be returned.
This routine returns a distance, rather than an altitude, in
contrast to the CSPICE routine nearpt_c. Because our ellipse is
situated in 3-space and not 2-space, the input point is not
`inside' or `outside' the ellipse, so the notion of altitude does
not apply to the problem solved by this routine. In the case of
nearpt_c, the input point is on, inside, or outside the ellipsoid,
so it makes sense to speak of its altitude.
-Examples
1) For planetary rings that can be modelled as flat disks with
elliptical outer boundaries, the distance of a point in
space from a ring's outer boundary can be computed using this
routine. Suppose center, smajor, and sminor are the center,
semi-major axis, and semi-minor axis of the ring's boundary.
Suppose also that scpos is the position of a spacecraft.
scpos, center, smajor, and sminor must all be expressed in
the same coordinate system. We can find the distance from
the spacecraft to the ring using the code fragment
#include "SpiceUsr.h"
.
.
.
/.
Make a CSPICE ellipse representing the ring,
then use npelpt_c to find the distance between
the spacecraft position and RING.
./
cgv2el_c ( center, smajor, sminor, ring );
npelpt_c ( scpos, ring, pnear, &dist );
2) The problem of finding the distance of a line from a tri-axial
ellipsoid can be reduced to the problem of finding the
distance between the same line and an ellipse; this problem in
turn can be reduced to the problem of finding the distance
between an ellipse and a point. The routine npedln_c carries
out this process and uses npelpt_c to find the ellipse-to-point
distance.
-Restrictions
None.
-Literature_References
None.
-Author_and_Institution
N.J. Bachman (JPL)
-Version
-CSPICE Version 1.0.0, 02-SEP-1999 (NJB)
-Index_Entries
nearest point on ellipse to point
-&
*/
{ /* Begin npelpt_c */
/*
Local variables
*/
SpiceDouble center [3];
SpiceDouble majlen;
SpiceDouble minlen;
SpiceDouble rotate [3][3];
SpiceDouble scale;
SpiceDouble smajor [3];
SpiceDouble sminor [3];
SpiceDouble tmppnt [3];
SpiceDouble prjpnt [3];
/*
Participate in error tracing.
*/
chkin_c ( "npelpt_c" );
/*
Here's an overview of our solution:
Let ELPL be the plane containing the ELLIPS, and let PRJ be
the orthogonal projection of the POINT onto ELPL. Let X be
any point in the plane ELPL. According to the Pythagorean
Theorem,
2 2 2
|| POINT - X || = || POINT - PRJ || + || PRJ - X ||.
Then if we can find a point X on ELLIPS that minimizes the
rightmost term, that point X is the closest point on the
ellipse to POINT.
So, we find the projection PRJ, and then solve the problem of
finding the closest point on ELLIPS to PRJ. To solve this
problem, we find a triaxial ellipsoid whose intersection with
the plane ELPL is precisely ELLIPS, and two of whose axes lie
in the plane ELPL. The closest point on ELLIPS to PRJ is also
the closest point on the ellipsoid to ELLIPS. But we have the
SPICELIB routine NEARPT on hand to find the closest point on an
ellipsoid to a specified point, so we've reduced our problem to
a solved problem.
There is a subtle point to worry about here: if PRJ is outside
of ELLIPS (PRJ is in the same plane as ELLIPS, so `outside'
does make sense here), then the closest point on ELLIPS to PRJ
coincides with the closest point on the ellipsoid to PRJ,
regardless of how we choose the z-semi-axis length of the
ellipsoid. But the correspondence may be lost if PRJ is inside
the ellipse, if we don't choose the z-semi-axis length
correctly.
Though it takes some thought to verify this (and we won't prove
it here), making the z-semi-axis of the ellipsoid longer than
the other two semi-axes is sufficient to maintain the
coincidence of the closest point on the ellipsoid to PRJPNT and
the closest point on the ellipse to PRJPNT.
*/
/*
Find the ellipse's center and semi-axes.
*/
el2cgv_c ( ellips, center, smajor, sminor );
/*
Find the lengths of the semi-axes, and scale the vectors to try
to prevent arithmetic unpleasantness. Degenerate ellipses are
turned away at the door.
*/
minlen = vnorm_c (sminor);
majlen = vnorm_c (smajor);
if ( MinVal ( majlen, minlen ) == 0.0 )
{
setmsg_c ( "Ellipse semi-axis lengths: # #." );
errdp_c ( "#", majlen );
errdp_c ( "#", minlen );
sigerr_c ( "SPICE(DEGENERATECASE)" );
chkout_c ( "npelpt_c" );
return;
}
scale = 1.0 / majlen;
vscl_c ( scale, smajor, smajor );
vscl_c ( scale, sminor, sminor );
/*
Translate ellipse and point so that the ellipse is centered at
the origin. Scale the point's coordinates to maintain the
correct relative position to the scaled ellipse.
*/
vsub_c ( point, center, tmppnt );
vscl_c ( scale, tmppnt, tmppnt );
/*
We want to reduce the problem to a two-dimensional one. We'll
work in a coordinate system whose x- and y- axes are aligned with
the semi-major and semi-minor axes of the input ellipse. The
z-axis is picked to give us a right-handed system. We find the
matrix that transforms coordinates to our new system using twovec_c.
*/
twovec_c ( smajor, 1, sminor, 2, rotate );
/*
Apply the coordinate transformation to our scaled input point.
*/
mxv_c ( rotate, tmppnt, tmppnt );
/*
We must find the distance between the orthogonal projection of
tmppnt onto the x-y plane and the ellipse. The projection is
just
( TMPPNT[0], TMPPNT[1], 0 );
we'll call this projection prjpnt.
*/
vpack_c ( tmppnt[0], tmppnt[1], 0.0, prjpnt );
/*
Now we're ready to find the distance between and a triaxial
ellipsoid whose intersection with the x-y plane is the ellipse
and whose third semi-axis lies on the z-axis.
Because we've scaled the ellipse's axes so as to give the longer
axis length 1, a length of 2.0 suffices for the ellipsoid's
z-semi-axis.
Find the nearest point to prjpnt on the ellipoid, pnear.
*/
nearpt_c ( prjpnt, 1.0, minlen/majlen, 2.0, pnear, dist );
/*
Scale the near point coordinates back to the original scale.
*/
vscl_c ( majlen, pnear, pnear );
/*
Apply the required inverse rotation and translation to pnear.
Compute the point-to-ellipse distance.
*/
mtxv_c ( rotate, pnear, pnear );
vadd_c ( pnear, center, pnear );
*dist = vdist_c ( pnear, point );
chkout_c ( "npelpt_c" );
} /* End npelpt_c */
void gfq (SpiceDouble et, SpiceDouble *value) {
SpiceDouble v[3], lt;
spkezp_c(1,et,"J2000","NONE",0,v,<);
*value = vnorm_c(v);
}
/** 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();
}