00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020 #include <math.h>
00021
00022 #define ZERO(x) ((x) < tolerance && (x) > -tolerance)
00023 #define TOLERANCE 1.0e-10
00024 static double tolerance = TOLERANCE;
00025
00026 int
00027 dig_set_distance_to_line_tolerance (double t)
00028 {
00029 if (t <= 0.0)
00030 t = TOLERANCE;
00031 tolerance = t;
00032
00033 return 0;
00034 }
00035
00036
00037
00038
00039
00040
00041
00042
00043
00044
00045
00046
00047
00048 double
00049 dig_distance2_point_to_line (
00050 double x, double y, double z,
00051 double x1, double y1, double z1,
00052 double x2, double y2, double z2,
00053 int with_z,
00054 double *px, double *py, double *pz,
00055 double *pdist,
00056 int *status)
00057 {
00058 register double dx, dy, dz;
00059 register double dpx, dpy, dpz;
00060 register double tpx, tpy, tpz;
00061 double t;
00062 int st;
00063
00064 st = 0;
00065
00066 if ( !with_z ) {
00067 z = 0;
00068 z1 = 0;
00069 z2 = 0;
00070 }
00071
00072 dx = x2 - x1;
00073 dy = y2 - y1;
00074 dz = z2 - z1;
00075
00076 if (ZERO (dx) && ZERO (dy) && ZERO (dz) ) {
00077 dx = x1 - x;
00078 dy = y1 - y;
00079 dz = z1 - z;
00080 tpx = x1;
00081 tpy = y1;
00082 tpz = z1;
00083 } else {
00084 t = (dx * (x - x1) + dy * (y - y1) + dz * (z - z1)) / (dx * dx + dy * dy + dz * dz);
00085
00086 if (t < 0.0) {
00087 t = 0.0;
00088 st = -1;
00089 } else if (t > 1.0) {
00090 t = 1.0;
00091 st = 1;
00092 }
00093
00094
00095 tpx = dx * t + x1;
00096 tpy = dy * t + y1;
00097 tpz = dz * t + z1;
00098 dx = tpx - x;
00099 dy = tpy - y;
00100 dz = tpz - z;
00101 }
00102
00103 if ( px ) *px = tpx;
00104 if ( py ) *py = tpy;
00105 if ( pz ) *pz = tpz;
00106 if ( status ) *status = st;
00107
00108 if ( pdist ) {
00109 dpx = tpx - x1;
00110 dpy = tpy - y1;
00111 dpz = tpz - z1;
00112 *pdist = sqrt ( dpx * dpx + dpy * dpy + dpz * dpz );
00113 }
00114
00115 return (dx * dx + dy * dy + dz * dz);
00116 }
00117