1 /* 2 3 -Procedure npedln_c ( Nearest point on ellipsoid to line ) 4 5 -Abstract 6 7 Find nearest point on a triaxial ellipsoid to a specified line, 8 and the distance from the ellipsoid to the line. 9 10 -Disclaimer 11 12 THIS SOFTWARE AND ANY RELATED MATERIALS WERE CREATED BY THE 13 CALIFORNIA INSTITUTE OF TECHNOLOGY (CALTECH) UNDER A U.S. 14 GOVERNMENT CONTRACT WITH THE NATIONAL AERONAUTICS AND SPACE 15 ADMINISTRATION (NASA). THE SOFTWARE IS TECHNOLOGY AND SOFTWARE 16 PUBLICLY AVAILABLE UNDER U.S. EXPORT LAWS AND IS PROVIDED "AS-IS" 17 TO THE RECIPIENT WITHOUT WARRANTY OF ANY KIND, INCLUDING ANY 18 WARRANTIES OF PERFORMANCE OR MERCHANTABILITY OR FITNESS FOR A 19 PARTICULAR USE OR PURPOSE (AS SET FORTH IN UNITED STATES UCC 20 SECTIONS 2312-2313) OR FOR ANY PURPOSE WHATSOEVER, FOR THE 21 SOFTWARE AND RELATED MATERIALS, HOWEVER USED. 22 23 IN NO EVENT SHALL CALTECH, ITS JET PROPULSION LABORATORY, OR NASA 24 BE LIABLE FOR ANY DAMAGES AND/OR COSTS, INCLUDING, BUT NOT 25 LIMITED TO, INCIDENTAL OR CONSEQUENTIAL DAMAGES OF ANY KIND, 26 INCLUDING ECONOMIC DAMAGE OR INJURY TO PROPERTY AND LOST PROFITS, 27 REGARDLESS OF WHETHER CALTECH, JPL, OR NASA BE ADVISED, HAVE 28 REASON TO KNOW, OR, IN FACT, SHALL KNOW OF THE POSSIBILITY. 29 30 RECIPIENT BEARS ALL RISK RELATING TO QUALITY AND PERFORMANCE OF 31 THE SOFTWARE AND ANY RELATED MATERIALS, AND AGREES TO INDEMNIFY 32 CALTECH AND NASA FOR ALL THIRD-PARTY CLAIMS RESULTING FROM THE 33 ACTIONS OF RECIPIENT IN THE USE OF THE SOFTWARE. 34 35 -Required_Reading 36 37 ELLIPSES 38 39 -Keywords 40 41 ELLIPSOID 42 GEOMETRY 43 MATH 44 45 */ 46 47 #include "SpiceUsr.h" 48 #include "SpiceZfc.h" 49 #include "SpiceZmc.h" 50 #undef npedln_c 51 52 npedln_c(SpiceDouble a,SpiceDouble b,SpiceDouble c,ConstSpiceDouble linept[3],ConstSpiceDouble linedr[3],SpiceDouble pnear[3],SpiceDouble * dist)53 void npedln_c ( SpiceDouble a, 54 SpiceDouble b, 55 SpiceDouble c, 56 ConstSpiceDouble linept[3], 57 ConstSpiceDouble linedr[3], 58 SpiceDouble pnear[3], 59 SpiceDouble * dist ) 60 61 /* 62 63 -Brief_I/O 64 65 Variable I/O Description 66 -------- --- -------------------------------------------------- 67 a I Length of ellipsoid's semi-axis in the x direction 68 b I Length of ellipsoid's semi-axis in the y direction 69 c I Length of ellipsoid's semi-axis in the z direction 70 linept I Point on line 71 linedr I Direction vector of line 72 pnear O Nearest point on ellipsoid to line 73 dist O Distance of ellipsoid from line 74 75 -Detailed_Input 76 77 a, 78 b, 79 c are the lengths of the semi-axes of a triaxial 80 ellipsoid which is centered at the origin and 81 oriented so that its axes lie on the x-, y- and 82 z- coordinate axes. a, b, and c are the lengths of 83 the semi-axes that point in the x, y, and z 84 directions respectively. 85 86 linept 87 linedr are, respectively, a point and a direction vector 88 that define a line. The line is the set of vectors 89 90 linept + t * linedr 91 92 where t is any real number. 93 94 -Detailed_Output 95 96 pnear is the point on the ellipsoid that is closest to 97 the line, if the line doesn't intersect the 98 ellipsoid. 99 100 If the line intersects the ellipsoid, pnear will 101 be a point of intersection. If linept is outside 102 of the ellipsoid, pnear will be the closest point 103 of intersection. If linept is inside the 104 ellipsoid, pnear will not necessarily be the 105 closest point of intersection. 106 107 108 dist is the distance of the line from the ellipsoid. 109 This is the minimum distance between any point on 110 the line and any point on the ellipsoid. 111 112 If the line intersects the ellipsoid, dist is zero. 113 114 -Parameters 115 116 None. 117 118 -Exceptions 119 120 If this routine detects an error, the output arguments nearp and 121 dist are not modified. 122 123 1) If the length of any semi-axis of the ellipsoid is 124 non-positive, the error SPICE(INVALIDAXISLENGTH) is signaled. 125 126 2) If the line's direction vector is the zero vector, the error 127 SPICE(ZEROVECTOR) is signaled. 128 129 3) If the length of any semi-axis of the ellipsoid is zero after 130 the semi-axis lengths are scaled by the reciprocal of the 131 magnitude of the longest semi-axis and then squared, the error 132 SPICE(DEGENERATECASE) is signaled. 133 134 4) If the input ellipsoid is extremely flat or needle-shaped 135 and has its shortest axis close to perpendicular to the input 136 line, numerical problems could cause this routine's algorithm 137 to fail, in which case the error SPICE(DEGENERATECASE) is 138 signaled. 139 140 -Files 141 142 None. 143 144 -Particulars 145 146 For any ellipsoid and line, if the line does not intersect the 147 ellipsoid, there is a unique point on the ellipsoid that is 148 closest to the line. Therefore, the distance dist between 149 ellipsoid and line is well-defined. The unique line segment of 150 length dist that connects the line and ellipsoid is normal to 151 both of these objects at its endpoints. 152 153 If the line intersects the ellipsoid, the distance between the 154 line and ellipsoid is zero. 155 156 -Examples 157 158 1) We can find the distance between an instrument optic axis ray 159 and the surface of a body modelled as a tri-axial ellipsoid 160 using this routine. If the instrument position and pointing 161 unit vector in body-fixed coordinates are 162 163 linept = ( 1.0e6, 2.0e6, 3.0e6 ) 164 165 and 166 167 linedr = ( -4.472091234e-1 168 -8.944182469e-1, 169 -4.472091234e-3 ) 170 171 and the body semi-axes lengths are 172 173 a = 7.0e5 174 b = 7.0e5 175 c = 6.0e5, 176 177 then the call to npedln_c 178 179 npedln_c ( a, b, c, linept, linedr, pnear, &dist ); 180 181 yields a value for pnear, the nearest point on the body to 182 the optic axis ray, of 183 184 ( -.16333110792340931E+04, 185 -.32666222157820771E+04, 186 .59999183350006724E+06 ) 187 188 and a value for dist, the distance to the ray, of 189 190 .23899679338299707E+06 191 192 (These results were obtained on a PC-Linux system under gcc.) 193 194 In some cases, it may not be clear that the closest point 195 on the line containing an instrument boresight ray is on 196 the boresight ray itself; the point may lie on the ray 197 having the same vertex as the boresight ray and pointing in 198 the opposite direction. To rule out this possibility, we 199 can make the following test: 200 201 /. 202 Find the difference vector between the closest point 203 on the ellipsoid to the line containing the boresight 204 ray and the boresight ray's vertex. Find the 205 angular separation between this difference vector 206 and the boresight ray. If the angular separation 207 does not exceed pi/2, we have the nominal geometry. 208 Otherwise, we have an error. 209 ./ 210 211 vsub_c ( pnear, linept, diff ); 212 213 sep = vsep_c ( diff, linedr ); 214 215 if ( sep <= halfpi_c() ) 216 { 217 [ perform normal processing ] 218 } 219 else 220 { 221 [ handle error case ] 222 } 223 224 225 -Restrictions 226 227 None. 228 229 -Literature_References 230 231 None. 232 233 -Author_and_Institution 234 235 N.J. Bachman (JPL) 236 237 -Version 238 239 -CSPICE Version 1.1.0, 01-JUN-2010 (NJB) 240 241 Added touchd_ calls to tests for squared, scaled axis length 242 underflow. This forces rounding to zero in certain cases where 243 it otherwise might not occur due to use of extended registers. 244 245 -CSPICE Version 1.0.1, 06-DEC-2002 (NJB) 246 247 Outputs shown in header example have been corrected to 248 be consistent with those produced by this routine. 249 250 -CSPICE Version 1.0.0, 03-SEP-1999 (NJB) 251 252 -Index_Entries 253 254 distance between line and ellipsoid 255 distance between line of sight and body 256 nearest point on ellipsoid to line 257 258 -& 259 */ 260 261 { /* Begin npedln_c */ 262 263 264 265 /* 266 Local variables 267 */ 268 269 SpiceBoolean found [2]; 270 SpiceBoolean ifound; 271 SpiceBoolean xfound; 272 273 SpiceDouble oppdir [3]; 274 SpiceDouble mag; 275 SpiceDouble normal [3]; 276 SpiceDouble prjpt [3]; 277 SpiceDouble prjnpt [3]; 278 SpiceDouble pt [2][3]; 279 SpiceDouble scale; 280 SpiceDouble scla; 281 SpiceDouble scla2; 282 SpiceDouble sclb; 283 SpiceDouble sclb2; 284 SpiceDouble sclc; 285 SpiceDouble sclc2; 286 SpiceDouble sclpt [3]; 287 SpiceDouble udir [3]; 288 289 SpiceEllipse cand; 290 SpiceEllipse prjel; 291 292 SpiceInt i; 293 294 SpicePlane candpl; 295 SpicePlane prjpl; 296 297 298 /* 299 Static variables 300 */ 301 302 303 /* 304 Participate in error tracing. 305 */ 306 307 chkin_c ( "npedln_c" ); 308 309 310 311 /* 312 The algorithm used in this routine has two parts. The first 313 part handles the case where the input line and ellipsoid 314 intersect. Our procedure is simple in that case; we just 315 call surfpt_c twice, passing it first one ray determined by the 316 input line, then a ray pointing in the opposite direction. 317 The second part of the algorithm handles the case where surfpt_c 318 doesn't find an intersection. 319 320 Finding the nearest point on the ellipsoid to the line, when the 321 two do not intersect, is a matter of following four steps: 322 323 1) Find the points on the ellipsoid where the surface normal 324 is normal to the line's direction. This set of points is 325 an ellipse centered at the origin. The point we seek MUST 326 lie on this `candidate' ellipse. 327 328 2) Project the candidate ellipse onto a plane that is normal 329 to the line's direction. This projection preserves 330 distance from the line; the nearest point to the line on 331 this new ellipse is the projection of the nearest point to 332 the line on the candidate ellipse, and these two points are 333 exactly the same distance from the line. If computed using 334 infinite-precision arithmetic, this projection would be 335 guaranteed to be non-degenerate as long as the input 336 ellipsoid were non-degenerate. This can be verified by 337 taking the inner product of the scaled normal to the candidate 338 ellipse plane and the line's unitized direction vector 339 (these vectors are called normal and udir in the code below); 340 the inner product is strictly greater than 1 if the ellipsoid 341 is non-degenerate. 342 343 3) The nearest point on the line to the projected ellipse will 344 be contained in the plane onto which the projection is done; 345 we find this point and then find the nearest point to it on 346 the projected ellipse. The distance between these two points 347 is the distance between the line and the ellipsoid. 348 349 4) Finally, we find the point on the candidate ellipse that was 350 projected to the nearest point to the line on the projected 351 ellipse that was found in step 3. This is the nearest point 352 on the ellipsoid to the line. 353 354 355 356 Glossary of Geometric Variables 357 358 359 a, 360 b, 361 c Input ellipsoid's semi-axis lengths. 362 363 point Point of intersection of line and ellipsoid 364 if the intersection is non-empty. 365 366 candpl Plane containing candidate ellipse. 367 368 normal Normal vector to the candidate plane candpl. 369 370 cand Candidate ellipse. 371 372 linept, 373 linedr, Point and direction vector on input line. 374 375 udir Unitized line direction vector. 376 377 prjpl Projection plane; the candidate ellipse is 378 projected onto this plane to yield prjel. 379 380 prjel Projection of the candidate ellipse cand onto 381 the projection plane prjel. 382 383 prjpt Projection of line point. 384 385 prjnpt Nearest point on projected ellipse to 386 projection of line point. 387 388 pnear Nearest point on ellipsoid to line. 389 390 */ 391 392 393 394 /* 395 We need a valid normal vector. 396 */ 397 398 unorm_c ( linedr, udir, &mag ); 399 400 if ( mag == 0. ) 401 { 402 setmsg_c( "Line direction vector is the zero vector. " ); 403 sigerr_c( "SPICE(ZEROVECTOR)" ); 404 chkout_c( "npedln_c" ); 405 return; 406 } 407 408 409 if ( ( a <= 0. ) 410 || ( b <= 0. ) 411 || ( c <= 0. ) ) 412 { 413 setmsg_c ( "Semi-axis lengths: a = #, b = #, c = #." ); 414 errdp_c ( "#", a ); 415 errdp_c ( "#", b ); 416 errdp_c ( "#", c ); 417 sigerr_c ( "SPICE(INVALIDAXISLENGTH)" ); 418 chkout_c ( "npedln_c" ); 419 return; 420 } 421 422 423 /* 424 Scale the semi-axes lengths for better numerical behavior. 425 If squaring any one of the scaled lengths causes it to 426 underflow to zero, we cannot continue the computation. Otherwise, 427 scale the viewing point too. 428 */ 429 430 scale = maxd_c ( 3, a, b, c ); 431 432 scla = a / scale; 433 sclb = b / scale; 434 sclc = c / scale; 435 436 scla2 = scla*scla; 437 sclb2 = sclb*sclb; 438 sclc2 = sclc*sclc; 439 440 if ( ( (SpiceDouble)touchd_(&scla2) == 0. ) 441 || ( (SpiceDouble)touchd_(&sclb2) == 0. ) 442 || ( (SpiceDouble)touchd_(&sclc2) == 0. ) ) 443 { 444 setmsg_c ( "Semi-axis too small: a = #, b = #, c = #. " ); 445 errdp_c ( "#", a ); 446 errdp_c ( "#", b ); 447 errdp_c ( "#", c ); 448 sigerr_c ( "SPICE(DEGENERATECASE)" ); 449 chkout_c ( "npedln_c" ); 450 return; 451 } 452 453 454 /* 455 Scale linept. 456 */ 457 sclpt[0] = linept[0] / scale; 458 sclpt[1] = linept[1] / scale; 459 sclpt[2] = linept[2] / scale; 460 461 /* 462 Hand off the intersection case to surfpt_c. surfpt_c determines 463 whether rays intersect a body, so we treat the line as a pair 464 of rays. 465 */ 466 467 vminus_c ( udir, oppdir ); 468 469 surfpt_c ( sclpt, udir, scla, sclb, sclc, pt[0], &(found[0]) ); 470 surfpt_c ( sclpt, oppdir, scla, sclb, sclc, pt[1], &(found[1]) ); 471 472 for ( i = 0; i < 2; i++ ) 473 { 474 if ( found[i] ) 475 { 476 *dist = 0.0; 477 478 vequ_c ( pt[i], pnear ); 479 vscl_c ( scale, pnear, pnear ); 480 chkout_c ( "npedln_c" ); 481 return; 482 } 483 } 484 485 486 /* 487 Getting here means the line doesn't intersect the ellipsoid. 488 489 Find the candidate ellipse CAND. NORMAL is a normal vector to 490 the plane containing the candidate ellipse. Mathematically the 491 ellipse must exist, since it's the intersection of an ellipsoid 492 centered at the origin and a plane containing the origin. Only 493 numerical problems can prevent the intersection from being found. 494 */ 495 496 normal[0] = udir[0] / scla2; 497 normal[1] = udir[1] / sclb2; 498 normal[2] = udir[2] / sclc2; 499 500 nvc2pl_c ( normal, 0., &candpl ); 501 502 inedpl_c ( scla, sclb, sclc, &candpl, &cand, &xfound ); 503 504 if ( !xfound ) 505 { 506 setmsg_c ( "Candidate ellipse could not be found." ); 507 sigerr_c ( "SPICE(DEGENERATECASE)" ); 508 chkout_c ( "npedln_c" ); 509 return; 510 } 511 512 /* 513 Project the candidate ellipse onto a plane orthogonal to the 514 line. We'll call the plane prjpl and the projected ellipse prjel. 515 */ 516 nvc2pl_c ( udir, 0., &prjpl ); 517 pjelpl_c ( &cand, &prjpl, &prjel ); 518 519 520 /* 521 Find the point on the line lying in the projection plane, and 522 then find the near point PRJNPT on the projected ellipse. Here 523 PRJPT is the point on the line lying in the projection plane. 524 The distance between PRJPT and PRJNPT is DIST. 525 */ 526 527 vprjp_c ( sclpt, &prjpl, prjpt ); 528 npelpt_c ( prjpt, &prjel, prjnpt, dist ); 529 530 531 /* 532 Find the near point pnear on the ellipsoid by taking the inverse 533 orthogonal projection of prjnpt; this is the point on the 534 candidate ellipse that projects to prjnpt. Note that the 535 output dist was computed in step 3 and needs only to be re-scaled. 536 537 The inverse projection of pnear ought to exist, but may not 538 be calculable due to numerical problems (this can only happen 539 when the input ellipsoid is extremely flat or needle-shaped). 540 */ 541 542 vprjpi_c ( prjnpt, &prjpl, &candpl, pnear, &ifound ); 543 544 if ( !ifound ) 545 { 546 setmsg_c ( "Inverse projection could not be found." ); 547 sigerr_c ( "SPICE(DEGENERATECASE)" ); 548 chkout_c ( "npedln_c" ); 549 return; 550 } 551 552 /* 553 Undo the scaling. 554 */ 555 556 vscl_c ( scale, pnear, pnear ); 557 558 *dist *= scale; 559 560 561 chkout_c ( "npedln_c" ); 562 563 } /* End npedln_c */ 564 565