BICPL: solve_plane.c Source File

00001 00002 /* ----------------------------- MNI Header ----------------------------------- 00003 @NAME : 00004 @INPUT : 00005 @OUTPUT : 00006 @RETURNS : 00007 @DESCRIPTION: 00008 @METHOD : 00009 @GLOBALS : 00010 @CALLS : 00011 @CREATED : Sep. 10, 1996 David MacDonald 00012 @MODIFIED : 00013 ---------------------------------------------------------------------------- */ 00014 00015 00016 #ifndef lint 00017 static char rcsid[] = "$Header: /software/source//libraries/bicpl/Geometry/solve_plane.c,v 1.13 2000/02/06 15:30:18 stever Exp $"; 00018 #endif 00019 00020 #include <volume_io/internal_volume_io.h> 00021 #include <bicpl/geom.h> 00022 #include <bicpl/trans.h> 00023 #include <bicpl/numerical.h> 00024 #include <bicpl/prog_utils.h> 00025 00026 public BOOLEAN get_interpolation_weights_2d( 00027 Real x, 00028 Real y, 00029 int n_points, 00030 Real xs[], 00031 Real ys[], 00032 Real weights[] ) 00033 { 00034 int i; 00035 Real x_fact, y_fact, constant, n; 00036 Real aa, ab, ac, bb, bc, dx, dy; 00037 Real acbb, abac, aabb, denom, aabc, abab; 00038 00039 aa = 0.0; 00040 ab = 0.0; 00041 ac = 0.0; 00042 bb = 0.0; 00043 bc = 0.0; 00044 00045 for_less( i, 0, n_points ) 00046 { 00047 dx = xs[i] - x; 00048 dy = ys[i] - y; 00049 aa += dx * dx; 00050 ab += dx * dy; 00051 ac += dx; 00052 bb += dy * dy; 00053 bc += dy; 00054 } 00055 00056 n = (Real) n_points; 00057 aabb = aa * bb; 00058 acbb = ac * bb; 00059 aabc = aa * bc; 00060 abab = ab * ab; 00061 abac = ab * ac; 00062 00063 denom = -aabb * n + ac * acbb + bc * aabc + abab*n - 2.0 * abac * bc; 00064 00065 if( denom == 0.0 ) 00066 return( FALSE ); 00067 00068 x_fact = (acbb - ab * bc) / denom; 00069 y_fact = (aabc - abac) / denom; 00070 constant = (abab - aabb) / denom - x * x_fact - y * y_fact; 00071 00072 for_less( i, 0, n_points ) 00073 weights[i] = constant + y_fact * ys[i] + x_fact * xs[i]; 00074 00075 return( TRUE ); 00076 } 00077 00078 #ifdef DEBUG 00079 #define DEBUG 00080 00081 #include <bicpl/prog_utils.h> 00082 #include <bicpl/numerical.h> 00083 00084 private void test_solution( 00085 int dim, 00086 Real x, 00087 Real y, 00088 int n_points, 00089 Real xs[], 00090 Real ys[], 00091 Real x_weights[], 00092 Real y_weights[], 00093 Real constant ) 00094 { 00095 int iter, n_iters, p; 00096 Real angle, x_trans, y_trans, xt, yt, zt, correct, value; 00097 Transform transform, rotation, translation; 00098 00099 n_iters = 100; 00100 00101 for_less( iter, 0, n_iters ) 00102 { 00103 angle = 2.0 * PI * get_random_0_to_1(); 00104 x_trans = 10.0 * get_random_0_to_1() - 5.0; 00105 y_trans = 10.0 * get_random_0_to_1() - 5.0; 00106 00107 make_rotation_transform( angle, Z, &rotation ); 00108 make_translation_transform( x_trans, y_trans, 0.0, &translation ); 00109 concat_transforms( &transform, &translation, &rotation ); 00110 00111 value = constant; 00112 00113 for_less( p, 0, n_points ) 00114 { 00115 transform_point( &transform, xs[p], ys[p], 0.0, &xt, &yt, &zt ); 00116 value += x_weights[p] * xt + y_weights[p] * yt; 00117 } 00118 00119 transform_point( &transform, x, y, 0.0, &xt, &yt, &zt ); 00120 correct = (dim == 0) ? xt : yt; 00121 00122 if( !numerically_close( value, correct, 1.0e-6 ) ) 00123 { 00124 print( "get_prediction_weights_2d_for_1_coord: %g %g\n", 00125 correct, value ); 00126 break; 00127 } 00128 } 00129 } 00130 #endif 00131 00132 private BOOLEAN get_two_point_prediction( 00133 Real x, 00134 Real y, 00135 Real x1, 00136 Real y1, 00137 Real x2, 00138 Real y2, 00139 Real *xwx1, 00140 Real *xwy1, 00141 Real *xwx2, 00142 Real *xwy2, 00143 Real *ywx1, 00144 Real *ywy1, 00145 Real *ywx2, 00146 Real *ywy2 ) 00147 { 00148 Real sx, sy, s, t, cax, cay, s_len; 00149 00150 sx = x2 - x1; 00151 sy = y2 - y1; 00152 cax = x - x1; 00153 cay = y - y1; 00154 00155 s_len = sx * sx + sy * sy; 00156 if( s_len == 0.0 ) 00157 return( FALSE ); 00158 00159 s = (cax * sx + cay * sy) / s_len; 00160 t = (cax * (-sy) + cay * sx) / s_len; 00161 00162 *xwx1 = 1.0 - s; 00163 *xwy1 = t; 00164 *xwx2 = s; 00165 *xwy2 = -t; 00166 *ywx1 = -t; 00167 *ywy1 = 1.0 - s; 00168 *ywx2 = t; 00169 *ywy2 = s; 00170 00171 return( TRUE ); 00172 } 00173 00174 public BOOLEAN get_prediction_weights_2d( 00175 Real x, 00176 Real y, 00177 int n_points, 00178 Real xs[], 00179 Real ys[], 00180 Real *x_weights[2], 00181 Real *x_constant, 00182 Real *y_weights[2], 00183 Real *y_constant ) 00184 { 00185 int dim, p, p1, p2, n_pairs; 00186 Real xwx1, xwy1, xwx2, xwy2, ywx1, ywy1, ywx2, ywy2; 00187 00188 *x_constant = 0.0; 00189 *y_constant = 0.0; 00190 00191 for_less( dim, 0, 2 ) 00192 { 00193 for_less( p, 0, n_points ) 00194 { 00195 x_weights[dim][p] = 0.0; 00196 y_weights[dim][p] = 0.0; 00197 } 00198 } 00199 00200 n_pairs = 0; 00201 for_less( p1, 0, n_points-1 ) 00202 { 00203 for_less( p2, p1+1, n_points ) 00204 { 00205 if( get_two_point_prediction( x, y, xs[p1], ys[p1], xs[p2], ys[p2], 00206 &xwx1, &xwy1, &xwx2, &xwy2, 00207 &ywx1, &ywy1, &ywx2, &ywy2 ) ) 00208 { 00209 x_weights[0][p1] += xwx1; 00210 x_weights[1][p1] += xwy1; 00211 x_weights[0][p2] += xwx2; 00212 x_weights[1][p2] += xwy2; 00213 y_weights[0][p1] += ywx1; 00214 y_weights[1][p1] += ywy1; 00215 y_weights[0][p2] += ywx2; 00216 y_weights[1][p2] += ywy2; 00217 ++n_pairs; 00218 } 00219 } 00220 } 00221 00222 for_less( dim, 0, 2 ) 00223 { 00224 for_less( p, 0, n_points ) 00225 { 00226 x_weights[dim][p] /= (Real) n_pairs; 00227 y_weights[dim][p] /= (Real) n_pairs; 00228 } 00229 } 00230 00231 #ifdef DEBUG 00232 test_solution( 0, x, y, n_points, xs, ys, x_weights[0], x_weights[1], 00233 *x_constant ); 00234 test_solution( 1, x, y, n_points, xs, ys, y_weights[0], y_weights[1], 00235 *y_constant ); 00236 #endif 00237 00238 return( TRUE ); 00239 } 00240 00241 #ifdef DEBUG 00242 private void test_solution_3d( 00243 int dim, 00244 Real x, 00245 Real y, 00246 Real z, 00247 int n_points, 00248 Real xs[], 00249 Real ys[], 00250 Real zs[], 00251 Real *weights[3] ) 00252 { 00253 int iter, n_iters, p; 00254 Real y_angle, z_angle, x_trans, y_trans, z_trans; 00255 Real correct, value, ps[3]; 00256 Transform transform, y_rotation, z_rotation, translation; 00257 00258 n_iters = 100; 00259 00260 for_less( iter, 0, n_iters ) 00261 { 00262 z_angle = 2.0 * PI * get_random_0_to_1(); 00263 y_angle = 2.0 * PI * get_random_0_to_1(); 00264 x_trans = 10.0 * get_random_0_to_1() - 5.0; 00265 y_trans = 10.0 * get_random_0_to_1() - 5.0; 00266 z_trans = 10.0 * get_random_0_to_1() - 5.0; 00267 00268 make_rotation_transform( y_angle, Y, &y_rotation ); 00269 make_rotation_transform( z_angle, Z, &z_rotation ); 00270 make_translation_transform( x_trans, y_trans, z_trans, &translation ); 00271 concat_transforms( &transform, &translation, &y_rotation ); 00272 concat_transforms( &transform, &transform, &z_rotation ); 00273 00274 value = 0.0; 00275 00276 for_less( p, 0, n_points ) 00277 { 00278 transform_point( &transform, xs[p], ys[p], zs[p], 00279 &ps[0], &ps[1], &ps[2] ); 00280 value += weights[0][p] * ps[0]; 00281 value += weights[1][p] * ps[1]; 00282 value += weights[2][p] * ps[2]; 00283 } 00284 00285 transform_point( &transform, x, y, z, &ps[0], &ps[1], &ps[2] ); 00286 correct = ps[dim]; 00287 00288 if( !numerically_close( value, correct, 1.0e-6 ) ) 00289 { 00290 print( "get_prediction_weights_3d: %d: %g %g\n", 00291 iter, correct, value ); 00292 break; 00293 } 00294 } 00295 } 00296 #endif 00297 00298 #define FACTOR 1.0e-6 00299 00300 public BOOLEAN get_prediction_weights_3d( 00301 Real x, 00302 Real y, 00303 Real z, 00304 int n_points, 00305 Real xs[], 00306 Real ys[], 00307 Real zs[], 00308 Real *x_weights[3], 00309 Real *y_weights[3], 00310 Real *z_weights[3] ) 00311 { 00312 int p, p1, dim, n_iters, iter; 00313 Real *parms, *coords[N_DIMENSIONS]; 00314 Real coord[N_DIMENSIONS]; 00315 Real len, max_len, sum; 00316 linear_least_squares lsq; 00317 BOOLEAN solved; 00318 Real y_angle, z_angle, x_trans, y_trans, z_trans; 00319 Transform transform, y_rotation, z_rotation, translation; 00320 00321 ALLOC( parms, n_points-1 ); 00322 00323 ALLOC( coords[0], n_points ); 00324 ALLOC( coords[1], n_points ); 00325 ALLOC( coords[2], n_points ); 00326 00327 max_len = 0.0; 00328 for_less( p, 0, n_points ) 00329 { 00330 len = xs[p] * xs[p] + ys[p] * ys[p] + zs[p] * zs[p]; 00331 max_len = MAX( max_len, len ); 00332 } 00333 00334 max_len = sqrt( max_len ); 00335 00336 initialize_linear_least_squares( &lsq, n_points-1 ); 00337 00338 n_iters = 100; 00339 n_iters = 1; 00340 00341 for_less( iter, 0, n_iters ) 00342 { 00343 z_angle = 2.0 * PI * get_random_0_to_1(); 00344 y_angle = 2.0 * PI * get_random_0_to_1(); 00345 x_trans = 10.0 * get_random_0_to_1() - 5.0; 00346 y_trans = 10.0 * get_random_0_to_1() - 5.0; 00347 z_trans = 10.0 * get_random_0_to_1() - 5.0; 00348 00349 make_rotation_transform( y_angle, Y, &y_rotation ); 00350 make_rotation_transform( z_angle, Z, &z_rotation ); 00351 make_translation_transform( x_trans, y_trans, z_trans, &translation ); 00352 concat_transforms( &transform, &translation, &y_rotation ); 00353 concat_transforms( &transform, &transform, &z_rotation ); 00354 00355 if( iter == 0 ) 00356 make_identity_transform( &transform ); 00357 00358 for_less( p, 0, n_points ) 00359 { 00360 transform_point( &transform, xs[p], ys[p], zs[p], 00361 &coords[0][p], &coords[1][p], &coords[2][p] ); 00362 00363 } 00364 00365 transform_point( &transform, x, y, z, 00366 &coord[0], &coord[1], &coord[2] ); 00367 00368 for_less( dim, 0, N_DIMENSIONS ) 00369 { 00370 for_less( p, 0, n_points-1 ) 00371 parms[p] = coords[dim][p] - coords[dim][n_points-1]; 00372 00373 add_to_linear_least_squares( &lsq, parms, 00374 coord[dim] - coords[dim][n_points-1] ); 00375 } 00376 } 00377 00378 for_less( p, 0, n_points-1 ) 00379 { 00380 for_less( p1, 0, n_points-1 ) 00381 parms[p1] = 0.0; 00382 parms[p] = FACTOR * max_len; 00383 00384 add_to_linear_least_squares( &lsq, parms, 0.0 ); 00385 } 00386 00387 for_less( p, 0, n_points-1 ) 00388 parms[p] = 1.0; 00389 00390 add_to_linear_least_squares( &lsq, parms, 1.0 ); 00391 00392 solved = get_linear_least_squares_solution( &lsq, parms ); 00393 00394 if( solved ) 00395 { 00396 for_less( p, 0, n_points ) 00397 for_less( dim, 0, N_DIMENSIONS ) 00398 { 00399 x_weights[dim][p] = 0.0; 00400 y_weights[dim][p] = 0.0; 00401 z_weights[dim][p] = 0.0; 00402 } 00403 00404 sum = 0.0; 00405 for_less( p, 0, n_points-1 ) 00406 { 00407 sum += parms[p]; 00408 x_weights[0][p] = parms[p]; 00409 y_weights[1][p] = parms[p]; 00410 z_weights[2][p] = parms[p]; 00411 } 00412 00413 x_weights[0][n_points-1] = 1.0 - sum; 00414 y_weights[1][n_points-1] = 1.0 - sum; 00415 z_weights[2][n_points-1] = 1.0 - sum; 00416 } 00417 00418 delete_linear_least_squares( &lsq ); 00419 FREE( parms ); 00420 FREE( coords[0] ); 00421 FREE( coords[1] ); 00422 FREE( coords[2] ); 00423 00424 if( !solved ) 00425 return( FALSE ); 00426 00427 00428 #ifdef DEBUG 00429 test_solution_3d( 0, x, y, z, n_points, xs, ys, zs, x_weights ); 00430 test_solution_3d( 1, x, y, z, n_points, xs, ys, zs, y_weights ); 00431 test_solution_3d( 2, x, y, z, n_points, xs, ys, zs, z_weights ); 00432 #endif 00433 00434 return( TRUE ); 00435 }