00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028
00029
00030
00031
00032
00033
00034
00035
00036
00037
00038
00039
00040
00041
00042
00043
00044
00045
00046
00047
00048
00049 #include "MatVec.h"
00050 #ifdef _WIN32
00051 #include <float.h>
00052 #define isnan _isnan
00053 #endif
00054
00055
00056
00057
00058
00059
00060
00061
00062
00063
00064
00065
00066 void
00067 SegPoints(PQP_REAL VEC[3],
00068 PQP_REAL X[3], PQP_REAL Y[3],
00069 const PQP_REAL P[3], const PQP_REAL A[3],
00070 const PQP_REAL Q[3], const PQP_REAL B[3])
00071 {
00072 PQP_REAL T[3], A_dot_A, B_dot_B, A_dot_B, A_dot_T, B_dot_T;
00073 PQP_REAL TMP[3];
00074
00075 VmV(T,Q,P);
00076 A_dot_A = VdotV(A,A);
00077 B_dot_B = VdotV(B,B);
00078 A_dot_B = VdotV(A,B);
00079 A_dot_T = VdotV(A,T);
00080 B_dot_T = VdotV(B,T);
00081
00082
00083
00084
00085 PQP_REAL t,u;
00086
00087
00088
00089
00090 PQP_REAL denom = A_dot_A*B_dot_B - A_dot_B*A_dot_B;
00091
00092 t = (A_dot_T*B_dot_B - B_dot_T*A_dot_B) / denom;
00093
00094
00095
00096 if ((t < 0) || isnan(t)) t = 0; else if (t > 1) t = 1;
00097
00098
00099
00100 u = (t*A_dot_B - B_dot_T) / B_dot_B;
00101
00102
00103
00104
00105
00106 if ((u <= 0) || isnan(u)) {
00107
00108 VcV(Y, Q);
00109
00110 t = A_dot_T / A_dot_A;
00111
00112 if ((t <= 0) || isnan(t)) {
00113 VcV(X, P);
00114 VmV(VEC, Q, P);
00115 }
00116 else if (t >= 1) {
00117 VpV(X, P, A);
00118 VmV(VEC, Q, X);
00119 }
00120 else {
00121 VpVxS(X, P, A, t);
00122 VcrossV(TMP, T, A);
00123 VcrossV(VEC, A, TMP);
00124 }
00125 }
00126 else if (u >= 1) {
00127
00128 VpV(Y, Q, B);
00129
00130 t = (A_dot_B + A_dot_T) / A_dot_A;
00131
00132 if ((t <= 0) || isnan(t)) {
00133 VcV(X, P);
00134 VmV(VEC, Y, P);
00135 }
00136 else if (t >= 1) {
00137 VpV(X, P, A);
00138 VmV(VEC, Y, X);
00139 }
00140 else {
00141 VpVxS(X, P, A, t);
00142 VmV(T, Y, P);
00143 VcrossV(TMP, T, A);
00144 VcrossV(VEC, A, TMP);
00145 }
00146 }
00147 else {
00148
00149 VpVxS(Y, Q, B, u);
00150
00151 if ((t <= 0) || isnan(t)) {
00152 VcV(X, P);
00153 VcrossV(TMP, T, B);
00154 VcrossV(VEC, B, TMP);
00155 }
00156 else if (t >= 1) {
00157 VpV(X, P, A);
00158 VmV(T, Q, X);
00159 VcrossV(TMP, T, B);
00160 VcrossV(VEC, B, TMP);
00161 }
00162 else {
00163 VpVxS(X, P, A, t);
00164 VcrossV(VEC, A, B);
00165 if (VdotV(VEC, T) < 0) {
00166 VxS(VEC, VEC, -1);
00167 }
00168 }
00169 }
00170 }
00171
00172
00173
00174
00175
00176
00177
00178
00179
00180
00181
00182
00183
00184
00185
00186
00187 PQP_REAL
00188 TriDist(PQP_REAL P[3], PQP_REAL Q[3],
00189 const PQP_REAL S[3][3], const PQP_REAL T[3][3])
00190 {
00191
00192
00193 PQP_REAL Sv[3][3], Tv[3][3];
00194 PQP_REAL VEC[3];
00195
00196 VmV(Sv[0],S[1],S[0]);
00197 VmV(Sv[1],S[2],S[1]);
00198 VmV(Sv[2],S[0],S[2]);
00199
00200 VmV(Tv[0],T[1],T[0]);
00201 VmV(Tv[1],T[2],T[1]);
00202 VmV(Tv[2],T[0],T[2]);
00203
00204
00205
00206
00207
00208
00209
00210
00211
00212 PQP_REAL V[3];
00213 PQP_REAL Z[3];
00214 PQP_REAL minP[3], minQ[3], mindd;
00215 int shown_disjoint = 0;
00216
00217 mindd = VdistV2(S[0],T[0]) + 1;
00218
00219 for (int i = 0; i < 3; i++)
00220 {
00221 for (int j = 0; j < 3; j++)
00222 {
00223
00224
00225
00226 SegPoints(VEC,P,Q,S[i],Sv[i],T[j],Tv[j]);
00227
00228 VmV(V,Q,P);
00229 PQP_REAL dd = VdotV(V,V);
00230
00231
00232
00233
00234 if (dd <= mindd)
00235 {
00236 VcV(minP,P);
00237 VcV(minQ,Q);
00238 mindd = dd;
00239
00240 VmV(Z,S[(i+2)%3],P);
00241 PQP_REAL a = VdotV(Z,VEC);
00242 VmV(Z,T[(j+2)%3],Q);
00243 PQP_REAL b = VdotV(Z,VEC);
00244
00245 if ((a <= 0) && (b >= 0)) return sqrt(dd);
00246
00247 PQP_REAL p = VdotV(V, VEC);
00248
00249 if (a < 0) a = 0;
00250 if (b > 0) b = 0;
00251 if ((p - a + b) > 0) shown_disjoint = 1;
00252 }
00253 }
00254 }
00255
00256
00257
00258
00259
00260
00261
00262
00263
00264
00265
00266
00267
00268
00269
00270
00271
00272 PQP_REAL Sn[3], Snl;
00273 VcrossV(Sn,Sv[0],Sv[1]);
00274 Snl = VdotV(Sn,Sn);
00275
00276
00277
00278 if (Snl > 1e-15)
00279 {
00280
00281
00282 PQP_REAL Tp[3];
00283
00284 VmV(V,S[0],T[0]);
00285 Tp[0] = VdotV(V,Sn);
00286
00287 VmV(V,S[0],T[1]);
00288 Tp[1] = VdotV(V,Sn);
00289
00290 VmV(V,S[0],T[2]);
00291 Tp[2] = VdotV(V,Sn);
00292
00293
00294
00295
00296 int point = -1;
00297 if ((Tp[0] > 0) && (Tp[1] > 0) && (Tp[2] > 0))
00298 {
00299 if (Tp[0] < Tp[1]) point = 0; else point = 1;
00300 if (Tp[2] < Tp[point]) point = 2;
00301 }
00302 else if ((Tp[0] < 0) && (Tp[1] < 0) && (Tp[2] < 0))
00303 {
00304 if (Tp[0] > Tp[1]) point = 0; else point = 1;
00305 if (Tp[2] > Tp[point]) point = 2;
00306 }
00307
00308
00309
00310 if (point >= 0)
00311 {
00312 shown_disjoint = 1;
00313
00314
00315
00316
00317 VmV(V,T[point],S[0]);
00318 VcrossV(Z,Sn,Sv[0]);
00319 if (VdotV(V,Z) > 0)
00320 {
00321 VmV(V,T[point],S[1]);
00322 VcrossV(Z,Sn,Sv[1]);
00323 if (VdotV(V,Z) > 0)
00324 {
00325 VmV(V,T[point],S[2]);
00326 VcrossV(Z,Sn,Sv[2]);
00327 if (VdotV(V,Z) > 0)
00328 {
00329
00330
00331
00332 VpVxS(P,T[point],Sn,Tp[point]/Snl);
00333 VcV(Q,T[point]);
00334 return sqrt(VdistV2(P,Q));
00335 }
00336 }
00337 }
00338 }
00339 }
00340
00341 PQP_REAL Tn[3], Tnl;
00342 VcrossV(Tn,Tv[0],Tv[1]);
00343 Tnl = VdotV(Tn,Tn);
00344
00345 if (Tnl > 1e-15)
00346 {
00347 PQP_REAL Sp[3];
00348
00349 VmV(V,T[0],S[0]);
00350 Sp[0] = VdotV(V,Tn);
00351
00352 VmV(V,T[0],S[1]);
00353 Sp[1] = VdotV(V,Tn);
00354
00355 VmV(V,T[0],S[2]);
00356 Sp[2] = VdotV(V,Tn);
00357
00358 int point = -1;
00359 if ((Sp[0] > 0) && (Sp[1] > 0) && (Sp[2] > 0))
00360 {
00361 if (Sp[0] < Sp[1]) point = 0; else point = 1;
00362 if (Sp[2] < Sp[point]) point = 2;
00363 }
00364 else if ((Sp[0] < 0) && (Sp[1] < 0) && (Sp[2] < 0))
00365 {
00366 if (Sp[0] > Sp[1]) point = 0; else point = 1;
00367 if (Sp[2] > Sp[point]) point = 2;
00368 }
00369
00370 if (point >= 0)
00371 {
00372 shown_disjoint = 1;
00373
00374 VmV(V,S[point],T[0]);
00375 VcrossV(Z,Tn,Tv[0]);
00376 if (VdotV(V,Z) > 0)
00377 {
00378 VmV(V,S[point],T[1]);
00379 VcrossV(Z,Tn,Tv[1]);
00380 if (VdotV(V,Z) > 0)
00381 {
00382 VmV(V,S[point],T[2]);
00383 VcrossV(Z,Tn,Tv[2]);
00384 if (VdotV(V,Z) > 0)
00385 {
00386 VcV(P,S[point]);
00387 VpVxS(Q,S[point],Tn,Sp[point]/Tnl);
00388 return sqrt(VdistV2(P,Q));
00389 }
00390 }
00391 }
00392 }
00393 }
00394
00395
00396
00397
00398
00399
00400 if (shown_disjoint)
00401 {
00402 VcV(P,minP);
00403 VcV(Q,minQ);
00404 return sqrt(mindd);
00405 }
00406 else return 0;
00407 }