GRASS Programmer's Manual
6.4.2(2012)
|
00001 00019 #include <math.h> 00020 00021 #include <grass/gis.h> 00022 #include <grass/gstypes.h> 00023 00028 #ifndef HUGE_VAL 00029 #define HUGE_VAL 1.7976931348623157e+308 00030 #endif 00031 00032 /* return codes */ 00033 #define MISSED 0 00034 #define FRONTFACE 1 00035 #define BACKFACE -1 00036 /* end Ray-Convex Polyhedron Intersection Test values */ 00037 00038 00052 int gs_los_intersect1(int surfid, float (*los)[3], float *point) 00053 { 00054 float dx, dy, dz, u_d[3]; 00055 float a[3], incr, min_incr, tlen, len; 00056 int outside, above, below, edge, istep; 00057 float b[3]; 00058 geosurf *gs; 00059 typbuff *buf; 00060 00061 G_debug(3, "gs_los_intersect1():"); 00062 00063 if (NULL == (gs = gs_get_surf(surfid))) { 00064 return (0); 00065 } 00066 00067 if (0 == GS_v3dir(los[FROM], los[TO], u_d)) { 00068 return (0); 00069 } 00070 00071 buf = gs_get_att_typbuff(gs, ATT_TOPO, 0); 00072 00073 istep = edge = below = 0; 00074 00075 len = 0.0; 00076 tlen = GS_distance(los[FROM], los[TO]); 00077 00078 incr = tlen / 1000.0; 00079 min_incr = incr / 1000.0; 00080 00081 dx = incr * u_d[X]; 00082 dy = incr * u_d[Y]; 00083 dz = incr * u_d[Z]; 00084 00085 a[X] = los[FROM][X]; 00086 a[Y] = los[FROM][Y]; 00087 a[Z] = los[FROM][Z]; 00088 00089 b[X] = a[X] - gs->x_trans; 00090 b[Y] = a[Y] - gs->y_trans; 00091 00092 if (viewcell_tri_interp(gs, buf, b, 0)) { 00093 /* expects surface coords */ 00094 b[Z] += gs->z_trans; 00095 00096 if (a[Z] < b[Z]) { 00097 /* viewing from below surface */ 00098 /* don't use this method 00099 fprintf(stderr,"view from below\n"); 00100 below = 1; 00101 */ 00102 00103 return (0); 00104 } 00105 } 00106 00107 while (incr > min_incr) { 00108 outside = 0; 00109 above = 0; 00110 b[X] = a[X] - gs->x_trans; 00111 b[Y] = a[Y] - gs->y_trans; 00112 00113 if (viewcell_tri_interp(gs, buf, b, 0)) { 00114 /* ignores masks */ 00115 b[Z] += gs->z_trans; 00116 above = (a[Z] > b[Z]); 00117 } 00118 else { 00119 outside = 1; 00120 00121 if (istep > 10) { 00122 edge = 1; 00123 below = 1; 00124 } 00125 } 00126 00127 while (outside || above) { 00128 a[X] += dx; 00129 a[Y] += dy; 00130 a[Z] += dz; 00131 len += incr; 00132 outside = 0; 00133 above = 0; 00134 b[X] = a[X] - gs->x_trans; 00135 b[Y] = a[Y] - gs->y_trans; 00136 00137 if (viewcell_tri_interp(gs, buf, b, 0)) { 00138 b[Z] += gs->z_trans; 00139 above = (a[Z] > b[Z]); 00140 } 00141 else { 00142 outside = 1; 00143 } 00144 00145 if (len > tlen) { 00146 return 0; /* over surface *//* under surface */ 00147 } 00148 } 00149 00150 /* could look for spikes here - see if any data points along 00151 shadow of line on surf go above los */ 00152 00153 /* back up several spots? */ 00154 a[X] -= (1.0 * dx); 00155 a[Y] -= (1.0 * dy); 00156 a[Z] -= (1.0 * dz); 00157 incr /= 2.0; 00158 ++istep; 00159 dx = incr * u_d[X]; 00160 dy = incr * u_d[Y]; 00161 dz = incr * u_d[Z]; 00162 } 00163 00164 if ((edge) && (b[Z] - (a[Z] + dz * 2.0) > incr * u_d[Z])) { 00165 G_debug(3, " looking under surface"); 00166 00167 return 0; 00168 } 00169 00170 point[X] = b[X]; 00171 point[Y] = b[Y]; 00172 point[Z] = b[Z] - gs->z_trans; 00173 00174 return (1); 00175 } 00176 00191 int gs_los_intersect(int surfid, float **los, float *point) 00192 { 00193 double incr; 00194 float p1, p2, u_d[3]; 00195 int above, ret, num, i, usedx; 00196 float a[3], b[3]; 00197 float bgn[3], end[3], a1[3]; 00198 geosurf *gs; 00199 typbuff *buf; 00200 Point3 *points; 00201 00202 G_debug(3, "gs_los_intersect"); 00203 00204 if (NULL == (gs = gs_get_surf(surfid))) { 00205 return (0); 00206 } 00207 00208 if (0 == GS_v3dir(los[FROM], los[TO], u_d)) { 00209 return (0); 00210 } 00211 00212 buf = gs_get_att_typbuff(gs, ATT_TOPO, 0); 00213 00214 GS_v3eq(bgn, los[FROM]); 00215 GS_v3eq(end, los[TO]); 00216 00217 bgn[X] -= gs->x_trans; 00218 bgn[Y] -= gs->y_trans; 00219 00220 end[X] -= gs->x_trans; 00221 end[Y] -= gs->y_trans; 00222 00223 /* trans? */ 00224 points = gsdrape_get_allsegments(gs, bgn, end, &num); 00225 00226 /* DEBUG 00227 { 00228 float t1[3], t2[3]; 00229 00230 t1[X] = los[FROM][X] ; 00231 t1[Y] = los[FROM][Y] ; 00232 00233 t2[X] = los[TO][X] ; 00234 t2[Y] = los[TO][Y] ; 00235 00236 GS_set_draw(GSD_FRONT); 00237 gsd_pushmatrix(); 00238 gsd_do_scale(1); 00239 gsd_translate(gs->x_trans, gs->y_trans, gs->z_trans); 00240 gsd_linewidth(1); 00241 gsd_color_func(GS_default_draw_color()); 00242 gsd_line_onsurf(gs, t1, t2); 00243 gsd_popmatrix(); 00244 GS_set_draw(GSD_BACK); 00245 gsd_flush(); 00246 } 00247 fprintf(stderr,"%d points to check\n", num); 00248 fprintf(stderr,"point0 = %.6lf %.6lf %.6lf FT =%.6lf %.6lf %.6lf\n", 00249 points[0][X],points[0][Y],points[0][Z], 00250 los[FROM][X],los[FROM][Y],los[FROM][Z]); 00251 fprintf(stderr,"incr1 = %.6lf: %.6lf %.6lf %.6lf\n",incr,u_d[X],u_d[Y],u_d[Z]); 00252 fprintf(stderr,"first point below surf\n"); 00253 fprintf(stderr,"incr2 = %f\n", (float)incr); 00254 fprintf(stderr,"(%d/%d) %f > %f\n", i,num, a[Z], points[i][Z]); 00255 fprintf(stderr,"incr3 = %f\n", (float)incr); 00256 fprintf(stderr,"all points above surf\n"); 00257 */ 00258 00259 if (num < 2) { 00260 G_debug(3, " %d points to check", num); 00261 00262 return (0); 00263 } 00264 00265 /* use larger of deltas for better precision */ 00266 usedx = (fabs(u_d[X]) > fabs(u_d[Y])); 00267 if (usedx) { 00268 incr = ((points[0][X] - (los[FROM][X] - gs->x_trans)) / u_d[X]); 00269 } 00270 else if (u_d[Y]) { 00271 incr = ((points[0][Y] - (los[FROM][Y] - gs->y_trans)) / u_d[Y]); 00272 } 00273 else { 00274 point[X] = los[FROM][X] - gs->x_trans; 00275 point[Y] = los[FROM][Y] - gs->y_trans; 00276 00277 return (viewcell_tri_interp(gs, buf, point, 1)); 00278 } 00279 00280 /* DEBUG 00281 fprintf(stderr,"-----------------------------\n"); 00282 fprintf(stderr,"%d points to check\n", num); 00283 fprintf(stderr,"incr1 = %.6lf: %.9f %.9f %.9f\n",incr,u_d[X],u_d[Y],u_d[Z]); 00284 fprintf(stderr, 00285 "\tpoint0 = %.6f %.6f %.6f\n\tFT = %.6f %.6f %.6f\n\tpoint%d = %.6f %.6f\n", 00286 points[0][X],points[0][Y],points[0][Z], 00287 los[FROM][X],los[FROM][Y],los[FROM][Z], 00288 num-1, points[num-1][X],points[num-1][Y]); 00289 */ 00290 00291 /* This should bring us right above (or below) the first point */ 00292 a[X] = los[FROM][X] + incr * u_d[X] - gs->x_trans; 00293 a[Y] = los[FROM][Y] + incr * u_d[Y] - gs->y_trans; 00294 a[Z] = los[FROM][Z] + incr * u_d[Z] - gs->z_trans; 00295 00296 if (a[Z] < points[0][Z]) { 00297 /* viewing from below surface */ 00298 /* don't use this method */ 00299 /* DEBUG 00300 fprintf(stderr,"first point below surf\n"); 00301 fprintf(stderr,"aZ= %.6f point0 = %.6f %.6f %.6f FT =%.6f %.6f %.6f\n", 00302 a[Z], points[0][X],points[0][Y],points[0][Z], 00303 los[FROM][X],los[FROM][Y],los[FROM][Z]); 00304 */ 00305 return (0); 00306 } 00307 00308 GS_v3eq(a1, a); 00309 GS_v3eq(b, a); 00310 00311 for (i = 1; i < num; i++) { 00312 if (usedx) { 00313 incr = ((points[i][X] - a1[X]) / u_d[X]); 00314 } 00315 else { 00316 incr = ((points[i][Y] - a1[Y]) / u_d[Y]); 00317 } 00318 00319 a[X] = a1[X] + (incr * u_d[X]); 00320 a[Y] = a1[Y] + (incr * u_d[Y]); 00321 a[Z] = a1[Z] + (incr * u_d[Z]); 00322 above = (a[Z] >= points[i][Z]); 00323 00324 if (above) { 00325 GS_v3eq(b, a); 00326 continue; 00327 } 00328 00329 /* 00330 * Now we know b[Z] is above points[i-1] 00331 * and a[Z] is below points[i] 00332 * Since there should only be one polygon along this seg, 00333 * just interpolate to intersect 00334 */ 00335 00336 if (usedx) { 00337 incr = ((a[X] - b[X]) / u_d[X]); 00338 } 00339 else { 00340 incr = ((a[Y] - b[Y]) / u_d[Y]); 00341 } 00342 00343 if (1 == (ret = segs_intersect(1.0, points[i][Z], 00344 0.0, points[i - 1][Z], 00345 1.0, a[Z], 0.0, b[Z], &p1, &p2))) { 00346 point[X] = points[i - 1][X] + (u_d[X] * incr * p1); 00347 point[Y] = points[i - 1][Y] + (u_d[Y] * incr * p1); 00348 point[Z] = p2; 00349 00350 return (1); 00351 } 00352 00353 G_debug(3, " line of sight error %d", ret); 00354 00355 return 0; 00356 } 00357 00358 /* over surface */ 00359 return 0; 00360 } 00361 00384 int RayCvxPolyhedronInt(Point3 org, Point3 dir, double tmax, Point4 * phdrn, 00385 int ph_num, double *tresult, int *pn) 00386 { 00387 double tnear, tfar, t, vn, vd; 00388 int fnorm_num, bnorm_num; /* front/back face # hit */ 00389 00390 tnear = -HUGE_VAL; 00391 tfar = tmax; 00392 00393 /* Test each plane in polyhedron */ 00394 for (; ph_num--;) { 00395 /* Compute intersection point T and sidedness */ 00396 vd = DOT3(dir, phdrn[ph_num]); 00397 vn = DOT3(org, phdrn[ph_num]) + phdrn[ph_num][W]; 00398 00399 if (vd == 0.0) { 00400 /* ray is parallel to plane - check if ray origin is inside plane's 00401 half-space */ 00402 if (vn > 0.0) { 00403 /* ray origin is outside half-space */ 00404 return (MISSED); 00405 } 00406 } 00407 else { 00408 /* ray not parallel - get distance to plane */ 00409 t = -vn / vd; 00410 00411 if (vd < 0.0) { 00412 /* front face - T is a near point */ 00413 if (t > tfar) { 00414 return (MISSED); 00415 } 00416 00417 if (t > tnear) { 00418 /* hit near face, update normal */ 00419 fnorm_num = ph_num; 00420 tnear = t; 00421 } 00422 } 00423 else { 00424 /* back face - T is a far point */ 00425 if (t < tnear) { 00426 return (MISSED); 00427 } 00428 00429 if (t < tfar) { 00430 /* hit far face, update normal */ 00431 bnorm_num = ph_num; 00432 tfar = t; 00433 } 00434 } 00435 } 00436 } 00437 00438 /* survived all tests */ 00439 /* Note: if ray originates on polyhedron, may want to change 0.0 to some 00440 * epsilon to avoid intersecting the originating face. 00441 */ 00442 if (tnear >= 0.0) { 00443 /* outside, hitting front face */ 00444 *tresult = tnear; 00445 *pn = fnorm_num; 00446 00447 return (FRONTFACE); 00448 } 00449 else { 00450 if (tfar < tmax) { 00451 /* inside, hitting back face */ 00452 *tresult = tfar; 00453 *pn = bnorm_num; 00454 00455 return (BACKFACE); 00456 } 00457 else { 00458 /* inside, but back face beyond tmax */ 00459 return (MISSED); 00460 } 00461 } 00462 } 00463 00469 void gs_get_databounds_planes(Point4 * planes) 00470 { 00471 float n, s, w, e, b, t; 00472 Point3 tlfront, brback; 00473 00474 GS_get_zrange(&b, &t, 0); 00475 gs_get_xrange(&w, &e); 00476 gs_get_yrange(&s, &n); 00477 00478 tlfront[X] = tlfront[Y] = 0.0; 00479 tlfront[Z] = t; 00480 00481 brback[X] = e - w; 00482 brback[Y] = n - s; 00483 brback[Z] = b; 00484 00485 /* top */ 00486 planes[0][X] = planes[0][Y] = 0.0; 00487 planes[0][Z] = 1.0; 00488 planes[0][W] = -(DOT3(planes[0], tlfront)); 00489 00490 /* bottom */ 00491 planes[1][X] = planes[1][Y] = 0.0; 00492 planes[1][Z] = -1.0; 00493 planes[1][W] = -(DOT3(planes[1], brback)); 00494 00495 /* left */ 00496 planes[2][Y] = planes[2][Z] = 0.0; 00497 planes[2][X] = -1.0; 00498 planes[2][W] = -(DOT3(planes[2], tlfront)); 00499 00500 /* right */ 00501 planes[3][Y] = planes[3][Z] = 0.0; 00502 planes[3][X] = 1.0; 00503 planes[3][W] = -(DOT3(planes[3], brback)); 00504 00505 /* front */ 00506 planes[4][X] = planes[4][Z] = 0.0; 00507 planes[4][Y] = -1.0; 00508 planes[4][W] = -(DOT3(planes[4], tlfront)); 00509 00510 /* back */ 00511 planes[5][X] = planes[5][Z] = 0.0; 00512 planes[5][Y] = 1.0; 00513 planes[5][W] = -(DOT3(planes[5], brback)); 00514 00515 return; 00516 } 00517 00529 int gs_setlos_enterdata(Point3 * los) 00530 { 00531 Point4 planes[12]; /* MAX_CPLANES + 6 - should define this */ 00532 Point3 dir; 00533 double dist, maxdist; 00534 int num, ret, retp; /* might want to tell if retp is a clipping plane */ 00535 00536 gs_get_databounds_planes(planes); 00537 num = gsd_get_cplanes(planes + 6); 00538 GS_v3dir(los[FROM], los[TO], dir); 00539 maxdist = GS_distance(los[FROM], los[TO]); 00540 00541 ret = RayCvxPolyhedronInt(los[0], dir, maxdist, 00542 planes, num + 6, &dist, &retp); 00543 00544 if (ret == MISSED) { 00545 return (0); 00546 } 00547 00548 if (ret == FRONTFACE) { 00549 GS_v3mult(dir, (float)dist); 00550 GS_v3add(los[FROM], dir); 00551 } 00552 00553 return (1); 00554 } 00555 00556 /***********************************************************************/ 00557 /* DEBUG **** 00558 void pr_plane(int pnum) 00559 { 00560 switch(pnum) 00561 { 00562 case 0: 00563 fprintf(stderr,"top plane"); 00564 00565 break; 00566 case 1: 00567 fprintf(stderr,"bottom plane"); 00568 00569 break; 00570 case 2: 00571 fprintf(stderr,"left plane"); 00572 00573 break; 00574 case 3: 00575 fprintf(stderr,"right plane"); 00576 00577 break; 00578 case 4: 00579 fprintf(stderr,"front plane"); 00580 00581 break; 00582 case 5: 00583 fprintf(stderr,"back plane"); 00584 00585 break; 00586 default: 00587 fprintf(stderr,"clipping plane %d", 6 - pnum); 00588 00589 break; 00590 } 00591 00592 return; 00593 } 00594 ******* */