/** 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 */
/** 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();
}
void dvhat_c ( ConstSpiceDouble s1 [6],
SpiceDouble sout[6] )
/*
-Brief_I/O
VARIABLE I/O DESCRIPTION
-------- --- --------------------------------------------------
s1 I State to be normalized.
sout O Unit vector s1 / |s1|, and its time derivative.
-Detailed_Input
s1 This is any double precision state. If the position
component of the state is the zero vector, this routine
will detect it and will not attempt to divide by zero.
-Detailed_Output
sout sout is a state containing the unit vector pointing in
the direction of position component of s1 and the
derivative of the unit vector with respect to time.
sout may overwrite s1.
-Parameters
None.
-Exceptions
Error free.
1) If s1 represents the zero vector, then the position
component of sout will also be the zero vector. The
velocity component will be the velocity component
of s1.
-Files
None.
-Particulars
Let s1 be a state vector with position and velocity components p
and v respectively. From these components one can compute the
unit vector parallel to p, call it u and the derivative of u
with respect to time, du. This pair (u,du) is the state returned
by this routine in sout.
-Examples
Any numerical results shown for this example may differ between
platforms as the results depend on the SPICE kernels used as input
and the machine specific arithmetic implementation.
Suppose that 'state' gives the apparent state of a body with
respect to an observer. This routine can be used to compute the
instantaneous angular rate of the object across the sky as seen
from the observers vantage.
#include "SpiceUsr.h"
#include <stdio.h>
#include <math.h>
int main()
{
SpiceDouble et;
SpiceDouble ltime;
SpiceDouble omega;
SpiceDouble state [6];
SpiceDouble ustate [6];
SpiceChar * epoch = "Jan 1 2009";
SpiceChar * target = "MOON";
SpiceChar * frame = "J2000";
SpiceChar * abcorr = "LT+S";
SpiceChar * obsrvr = "EARTH BARYCENTER";
/.
Load SPK, PCK, and LSK kernels, use a meta kernel for convenience.
./
furnsh_c ( "standard.tm" );
/.
Define an arbitrary epoch, convert the epoch to ephemeris time.
./
str2et_c ( epoch, &et );
/.
Calculate the state of the moon with respect to the earth-moon
barycenter in J2000, corrected for light time and stellar aberration
at ET.
./
spkezr_c ( target, et, frame, abcorr, obsrvr, state, <ime );
/.
Calculate the unit vector of STATE and the derivative of the
unit vector.
./
dvhat_c ( state, ustate );
/.
Calculate the instantaneous angular velocity from the magnitude of the
derivative of the unit vector.
v = r x omega
||omega|| = ||v|| for r . v = 0
-----
||r||
||omega|| = ||v|| for ||r|| = 1
./
omega = vnorm_c( &ustate[3] );
printf( "Instantaneous angular velocity, rad/sec %.10g\n", omega );
return 0;
}
The program outputs:
Instantaneous angular velocity, rad/sec 2.48106658e-06
-Restrictions
None.
-Literature_References
None.
-Author_and_Institution
W.L. Taber (JPL)
E.D. Wright (JPL)
-Version
-CSPICE Version 1.0.1, 06-MAY-2010 (EDW)
Reordered header sections to proper NAIF convention.
Minor edit to code comments eliminating typo.
-CSPICE Version 1.0.0, 07-JUL-1999 (EDW)
-Index_Entries
State of a unit vector parallel to a state vector
-&
*/
{ /* Begin dvhat_c */
/*
Local variables
*/
SpiceDouble length;
SpiceDouble posin [3];
SpiceDouble posout[3];
SpiceDouble velin [3];
SpiceDouble velout[3];
/*
We'll do this the obvious way for now. Unpack the input vector
into two working vectors.
*/
posin[0] = s1[0];
posin[1] = s1[1];
posin[2] = s1[2];
velin[0] = s1[3];
velin[1] = s1[4];
velin[2] = s1[5];
/*
Get the position portion of the output state and the length of
the input position.
*/
unorm_c ( posin, posout, &length );
if ( length == 0. )
{
/*
If the length of the input position is zero, just copy
the input velocity to the output velocity.
*/
vequ_c ( velin, velout );
}
else
{
/*
Otherwise the derivative of the unit vector is just the
component of the input velocity perpendicular to the input
position, scaled by the reciprocal of the length of the
input position.
*/
vperp_c ( velin , posout, velout );
vscl_c ( 1./length, velout, velout );
}
/*
Pack everything and return. Hazar!
*/
sout[0] = posout[0];
sout[1] = posout[1];
sout[2] = posout[2];
sout[3] = velout[0];
sout[4] = velout[1];
sout[5] = velout[2];
} /* End dvhat_c */
void npedln_c ( SpiceDouble a,
SpiceDouble b,
SpiceDouble c,
ConstSpiceDouble linept[3],
ConstSpiceDouble linedr[3],
SpiceDouble pnear[3],
SpiceDouble * dist )
/*
-Brief_I/O
Variable I/O Description
-------- --- --------------------------------------------------
a I Length of ellipsoid's semi-axis in the x direction
b I Length of ellipsoid's semi-axis in the y direction
c I Length of ellipsoid's semi-axis in the z direction
linept I Point on line
linedr I Direction vector of line
pnear O Nearest point on ellipsoid to line
dist O Distance of ellipsoid from line
-Detailed_Input
a,
b,
c are the lengths of the semi-axes of a triaxial
ellipsoid which is centered at the origin and
oriented so that its axes lie on the x-, y- and
z- coordinate axes. a, b, and c are the lengths of
the semi-axes that point in the x, y, and z
directions respectively.
linept
linedr are, respectively, a point and a direction vector
that define a line. The line is the set of vectors
linept + t * linedr
where t is any real number.
-Detailed_Output
pnear is the point on the ellipsoid that is closest to
the line, if the line doesn't intersect the
ellipsoid.
If the line intersects the ellipsoid, pnear will
be a point of intersection. If linept is outside
of the ellipsoid, pnear will be the closest point
of intersection. If linept is inside the
ellipsoid, pnear will not necessarily be the
closest point of intersection.
dist is the distance of the line from the ellipsoid.
This is the minimum distance between any point on
the line and any point on the ellipsoid.
If the line intersects the ellipsoid, dist is zero.
-Parameters
None.
-Exceptions
If this routine detects an error, the output arguments nearp and
dist are not modified.
1) If the length of any semi-axis of the ellipsoid is
non-positive, the error SPICE(INVALIDAXISLENGTH) is signaled.
2) If the line's direction vector is the zero vector, the error
SPICE(ZEROVECTOR) is signaled.
3) If the length of any semi-axis of the ellipsoid is zero after
the semi-axis lengths are scaled by the reciprocal of the
magnitude of the longest semi-axis and then squared, the error
SPICE(DEGENERATECASE) is signaled.
4) If the input ellipsoid is extremely flat or needle-shaped
and has its shortest axis close to perpendicular to the input
line, numerical problems could cause this routine's algorithm
to fail, in which case the error SPICE(DEGENERATECASE) is
signaled.
-Files
None.
-Particulars
For any ellipsoid and line, if the line does not intersect the
ellipsoid, there is a unique point on the ellipsoid that is
closest to the line. Therefore, the distance dist between
ellipsoid and line is well-defined. The unique line segment of
length dist that connects the line and ellipsoid is normal to
both of these objects at its endpoints.
If the line intersects the ellipsoid, the distance between the
line and ellipsoid is zero.
-Examples
1) We can find the distance between an instrument optic axis ray
and the surface of a body modelled as a tri-axial ellipsoid
using this routine. If the instrument position and pointing
unit vector in body-fixed coordinates are
linept = ( 1.0e6, 2.0e6, 3.0e6 )
and
linedr = ( -4.472091234e-1
-8.944182469e-1,
-4.472091234e-3 )
and the body semi-axes lengths are
a = 7.0e5
b = 7.0e5
c = 6.0e5,
then the call to npedln_c
npedln_c ( a, b, c, linept, linedr, pnear, &dist );
yields a value for pnear, the nearest point on the body to
the optic axis ray, of
( -.16333110792340931E+04,
-.32666222157820771E+04,
.59999183350006724E+06 )
and a value for dist, the distance to the ray, of
.23899679338299707E+06
(These results were obtained on a PC-Linux system under gcc.)
In some cases, it may not be clear that the closest point
on the line containing an instrument boresight ray is on
the boresight ray itself; the point may lie on the ray
having the same vertex as the boresight ray and pointing in
the opposite direction. To rule out this possibility, we
can make the following test:
/.
Find the difference vector between the closest point
on the ellipsoid to the line containing the boresight
ray and the boresight ray's vertex. Find the
angular separation between this difference vector
and the boresight ray. If the angular separation
does not exceed pi/2, we have the nominal geometry.
Otherwise, we have an error.
./
vsub_c ( pnear, linept, diff );
sep = vsep_c ( diff, linedr );
if ( sep <= halfpi_c() )
{
[ perform normal processing ]
}
else
{
[ handle error case ]
}
-Restrictions
None.
-Literature_References
None.
-Author_and_Institution
N.J. Bachman (JPL)
-Version
-CSPICE Version 1.1.0, 01-JUN-2010 (NJB)
Added touchd_ calls to tests for squared, scaled axis length
underflow. This forces rounding to zero in certain cases where
it otherwise might not occur due to use of extended registers.
-CSPICE Version 1.0.1, 06-DEC-2002 (NJB)
Outputs shown in header example have been corrected to
be consistent with those produced by this routine.
-CSPICE Version 1.0.0, 03-SEP-1999 (NJB)
-Index_Entries
distance between line and ellipsoid
distance between line of sight and body
nearest point on ellipsoid to line
-&
*/
{ /* Begin npedln_c */
/*
Local variables
*/
SpiceBoolean found [2];
SpiceBoolean ifound;
SpiceBoolean xfound;
SpiceDouble oppdir [3];
SpiceDouble mag;
SpiceDouble normal [3];
SpiceDouble prjpt [3];
SpiceDouble prjnpt [3];
SpiceDouble pt [2][3];
SpiceDouble scale;
SpiceDouble scla;
SpiceDouble scla2;
SpiceDouble sclb;
SpiceDouble sclb2;
SpiceDouble sclc;
SpiceDouble sclc2;
SpiceDouble sclpt [3];
SpiceDouble udir [3];
SpiceEllipse cand;
SpiceEllipse prjel;
SpiceInt i;
SpicePlane candpl;
SpicePlane prjpl;
/*
Static variables
*/
/*
Participate in error tracing.
*/
chkin_c ( "npedln_c" );
/*
The algorithm used in this routine has two parts. The first
part handles the case where the input line and ellipsoid
intersect. Our procedure is simple in that case; we just
call surfpt_c twice, passing it first one ray determined by the
input line, then a ray pointing in the opposite direction.
The second part of the algorithm handles the case where surfpt_c
doesn't find an intersection.
Finding the nearest point on the ellipsoid to the line, when the
two do not intersect, is a matter of following four steps:
1) Find the points on the ellipsoid where the surface normal
is normal to the line's direction. This set of points is
an ellipse centered at the origin. The point we seek MUST
lie on this `candidate' ellipse.
2) Project the candidate ellipse onto a plane that is normal
to the line's direction. This projection preserves
distance from the line; the nearest point to the line on
this new ellipse is the projection of the nearest point to
the line on the candidate ellipse, and these two points are
exactly the same distance from the line. If computed using
infinite-precision arithmetic, this projection would be
guaranteed to be non-degenerate as long as the input
ellipsoid were non-degenerate. This can be verified by
taking the inner product of the scaled normal to the candidate
ellipse plane and the line's unitized direction vector
(these vectors are called normal and udir in the code below);
the inner product is strictly greater than 1 if the ellipsoid
is non-degenerate.
3) The nearest point on the line to the projected ellipse will
be contained in the plane onto which the projection is done;
we find this point and then find the nearest point to it on
the projected ellipse. The distance between these two points
is the distance between the line and the ellipsoid.
4) Finally, we find the point on the candidate ellipse that was
projected to the nearest point to the line on the projected
ellipse that was found in step 3. This is the nearest point
on the ellipsoid to the line.
Glossary of Geometric Variables
a,
b,
c Input ellipsoid's semi-axis lengths.
point Point of intersection of line and ellipsoid
if the intersection is non-empty.
candpl Plane containing candidate ellipse.
normal Normal vector to the candidate plane candpl.
cand Candidate ellipse.
linept,
linedr, Point and direction vector on input line.
udir Unitized line direction vector.
prjpl Projection plane; the candidate ellipse is
projected onto this plane to yield prjel.
prjel Projection of the candidate ellipse cand onto
the projection plane prjel.
prjpt Projection of line point.
prjnpt Nearest point on projected ellipse to
projection of line point.
pnear Nearest point on ellipsoid to line.
*/
/*
We need a valid normal vector.
*/
unorm_c ( linedr, udir, &mag );
if ( mag == 0. )
{
setmsg_c( "Line direction vector is the zero vector. " );
sigerr_c( "SPICE(ZEROVECTOR)" );
chkout_c( "npedln_c" );
return;
}
if ( ( a <= 0. )
|| ( b <= 0. )
|| ( c <= 0. ) )
{
setmsg_c ( "Semi-axis lengths: a = #, b = #, c = #." );
errdp_c ( "#", a );
errdp_c ( "#", b );
errdp_c ( "#", c );
sigerr_c ( "SPICE(INVALIDAXISLENGTH)" );
chkout_c ( "npedln_c" );
return;
}
/*
Scale the semi-axes lengths for better numerical behavior.
If squaring any one of the scaled lengths causes it to
underflow to zero, we cannot continue the computation. Otherwise,
scale the viewing point too.
*/
scale = maxd_c ( 3, a, b, c );
scla = a / scale;
sclb = b / scale;
sclc = c / scale;
scla2 = scla*scla;
sclb2 = sclb*sclb;
sclc2 = sclc*sclc;
if ( ( (SpiceDouble)touchd_(&scla2) == 0. )
|| ( (SpiceDouble)touchd_(&sclb2) == 0. )
|| ( (SpiceDouble)touchd_(&sclc2) == 0. ) )
{
setmsg_c ( "Semi-axis too small: a = #, b = #, c = #. " );
errdp_c ( "#", a );
errdp_c ( "#", b );
errdp_c ( "#", c );
sigerr_c ( "SPICE(DEGENERATECASE)" );
chkout_c ( "npedln_c" );
return;
}
/*
Scale linept.
*/
sclpt[0] = linept[0] / scale;
sclpt[1] = linept[1] / scale;
sclpt[2] = linept[2] / scale;
/*
Hand off the intersection case to surfpt_c. surfpt_c determines
whether rays intersect a body, so we treat the line as a pair
of rays.
*/
vminus_c ( udir, oppdir );
surfpt_c ( sclpt, udir, scla, sclb, sclc, pt[0], &(found[0]) );
surfpt_c ( sclpt, oppdir, scla, sclb, sclc, pt[1], &(found[1]) );
for ( i = 0; i < 2; i++ )
{
if ( found[i] )
{
*dist = 0.0;
vequ_c ( pt[i], pnear );
vscl_c ( scale, pnear, pnear );
chkout_c ( "npedln_c" );
return;
}
}
/*
Getting here means the line doesn't intersect the ellipsoid.
Find the candidate ellipse CAND. NORMAL is a normal vector to
the plane containing the candidate ellipse. Mathematically the
ellipse must exist, since it's the intersection of an ellipsoid
centered at the origin and a plane containing the origin. Only
numerical problems can prevent the intersection from being found.
*/
normal[0] = udir[0] / scla2;
normal[1] = udir[1] / sclb2;
normal[2] = udir[2] / sclc2;
nvc2pl_c ( normal, 0., &candpl );
inedpl_c ( scla, sclb, sclc, &candpl, &cand, &xfound );
if ( !xfound )
{
setmsg_c ( "Candidate ellipse could not be found." );
sigerr_c ( "SPICE(DEGENERATECASE)" );
chkout_c ( "npedln_c" );
return;
}
/*
Project the candidate ellipse onto a plane orthogonal to the
line. We'll call the plane prjpl and the projected ellipse prjel.
*/
nvc2pl_c ( udir, 0., &prjpl );
pjelpl_c ( &cand, &prjpl, &prjel );
/*
Find the point on the line lying in the projection plane, and
then find the near point PRJNPT on the projected ellipse. Here
PRJPT is the point on the line lying in the projection plane.
The distance between PRJPT and PRJNPT is DIST.
*/
vprjp_c ( sclpt, &prjpl, prjpt );
npelpt_c ( prjpt, &prjel, prjnpt, dist );
/*
Find the near point pnear on the ellipsoid by taking the inverse
orthogonal projection of prjnpt; this is the point on the
candidate ellipse that projects to prjnpt. Note that the
output dist was computed in step 3 and needs only to be re-scaled.
The inverse projection of pnear ought to exist, but may not
be calculable due to numerical problems (this can only happen
when the input ellipsoid is extremely flat or needle-shaped).
*/
vprjpi_c ( prjnpt, &prjpl, &candpl, pnear, &ifound );
if ( !ifound )
{
setmsg_c ( "Inverse projection could not be found." );
sigerr_c ( "SPICE(DEGENERATECASE)" );
chkout_c ( "npedln_c" );
return;
}
/*
Undo the scaling.
*/
vscl_c ( scale, pnear, pnear );
*dist *= scale;
chkout_c ( "npedln_c" );
} /* End npedln_c */
