void nplnpt_c ( ConstSpiceDouble linpt [3],
ConstSpiceDouble lindir [3],
ConstSpiceDouble point [3],
SpiceDouble pnear [3],
SpiceDouble * dist )
/*
-Brief_I/O
Variable I/O Description
-------- --- --------------------------------------------------
linpt,
lindir I Point on a line and the line's direction vector.
point I A second point.
pnear O Nearest point on the line to point.
dist O Distance between point and pnear.
-Detailed_Input
linpt
lindir are, respectively, a point and a direction vector
that define a line in 3-dimensional space. The
line is the set of points
linpt + t * lindir
where t is any real number.
point is a point in 3-dimensional space.
-Detailed_Output
pnear is the nearest point on the input line to the input
point.
dist is the distance between the input line and input
point.
-Parameters
None.
-Exceptions
1) If the line direction vector lindir is the zero vector, the
error SPICE(ZEROVECTOR) is signaled.
-Files
None.
-Particulars
For every line L and point P, there is a unique closest point
on L to P. Call this closest point C. It is always true that
P - C is perpendicular to L, and the length of P - C is called
the "distance" between P and L.
-Examples
1) Suppose a line passes through the point ( 1, 2, 3 ) and
has direction vector ( 0, 1, 1 ). We wish to find the
closest point on the line to the point ( -6, 9, 10 ). We
can use the code fragment
#include "SpiceUsr.h"
.
.
.
LINPT[0] = 1.0;
LINPT[1] = 2.0;
LINPT[2] = 3.0;
LINDIR[0] = 0.0;
LINDIR[1] = 1.0;
LINDIR[2] = 1.0;
POINT[0] = -6.0;
POINT[1] = 9.0;
POINT[2] = 10.0;
nplnpt_c ( linpt, lindir, point, pnear, &dist );
After the call, pnear will take the value
( 1., 9., 10. );
dist will be 7.0.
-Restrictions
None.
-Literature_References
None.
-Author_and_Institution
N.J. Bachman (JPL)
-Version
-CSPICE Version 1.0.0, 16-AUG-1999 (NJB)
-Index_Entries
distance between point and line
nearest point on line to point
-&
*/
{ /* Begin nplnpt_c */
/*
Local variables
*/
SpiceDouble trans [3];
/*
We need a real direction vector to work with.
*/
if ( vzero_c (lindir) )
{
chkin_c ( "nplnpt_c" );
setmsg_c ( "Direction vector must be non-zero." );
sigerr_c ( "SPICE(ZEROVECTOR)" );
chkout_c ( "nplnpt_c" );
return;
}
/*
We translate line and input point so as to put the line through
the origin. Then the nearest point on the translated line to the
translated point TRANS is the projection of TRANS onto the line.
*/
vsub_c ( point, linpt, trans );
vproj_c ( trans, lindir, pnear );
vadd_c ( pnear, linpt, pnear );
*dist = vdist_c ( pnear, point );
} /* End nplnpt_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 */
