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