00001 #include "geom.h"
00002
00003 #include "error.h"
00004 #include "defines.h"
00005
00006 #include <math.h>
00007 #include <gsl/gsl_linalg.h>
00008
00019 boolean pointLeftOfLine(double px,double py,
00020 double x1,double y1,double x2,double y2)
00021 {
00022
00023
00024
00025 double det;
00026
00027 ROUNDDOWN;
00028 det=(x2*py-px*y2)-(x1*py-px*y1)+(x1*y2-x2*y1);
00029 ROUNDNEAR;
00030
00031 return(det>=0);
00032 }
00033
00034
00035 boolean pointRightOfLine(double px,double py,
00036 double x1,double y1,double x2,double y2)
00037 {
00038
00039
00040
00041 double det;
00042
00043 ROUNDUP;
00044 det=(x2*py-px*y2)-(x1*py-px*y1)+(x1*y2-x2*y1);
00045 ROUNDNEAR;
00046
00047 return (det<=0);
00048 }
00049
00050
00051
00052
00053
00054 void ComputeBoundingBox(unsigned int n,double *x,double *y,
00055 Tinterval *ix,Tinterval *iy)
00056 {
00057 unsigned int i;
00058 double x_min,x_max;
00059 double y_min,y_max;
00060
00061 x_min=x_max=x[0];
00062 y_min=y_max=y[0];
00063 for(i=1;i<n;i++)
00064 {
00065 if (x[i]>x_max) x_max=x[i];
00066 else {if (x[i]<x_min) x_min=x[i];}
00067 if (y[i]>y_max) y_max=y[i];
00068 else {if (y[i]<y_min) y_min=y[i];}
00069 }
00070 NewInterval(x_min,x_max,ix);
00071 NewInterval(y_min,y_max,iy);
00072 }
00073
00074 void ComputeBoundingBox3d(unsigned int n,double *x,double *y,double *z,
00075 Tinterval *ix,Tinterval *iy,Tinterval *iz)
00076 {
00077 unsigned int i;
00078 double x_min,x_max;
00079 double y_min,y_max;
00080 double z_min,z_max;
00081
00082 x_min=x_max=x[0];
00083 y_min=y_max=y[0];
00084 z_min=z_max=z[0];
00085 for(i=1;i<n;i++)
00086 {
00087 if (x[i]>x_max) x_max=x[i];
00088 else {if (x[i]<x_min) x_min=x[i];}
00089 if (y[i]>y_max) y_max=y[i];
00090 else {if (y[i]<y_min) y_min=y[i];}
00091 if (z[i]>z_max) z_max=z[i];
00092 else {if (z[i]<z_min) z_min=z[i];}
00093 }
00094 NewInterval(x_min,x_max,ix);
00095 NewInterval(y_min,y_max,iy);
00096 NewInterval(z_min,z_max,iz);
00097 }
00098
00099
00100
00101
00102 boolean RectangleCircleClipping(double r2,
00103 Tinterval *x,Tinterval *y,
00104 Tinterval *x_new,Tinterval *y_new)
00105 {
00106 Tinterval d,md,a,b,diameter,rad2;
00107 boolean full;
00108 double r;
00109
00110
00111
00112 r=sqrt(r2);
00113 NewInterval(-r-ZERO,+r+ZERO,&diameter);
00114 full=((Intersection(x,&diameter,x_new))&&
00115 (Intersection(y,&diameter,y_new)));
00116
00117 if (full)
00118 {
00119 NewInterval(r2-ZERO,r2+ZERO,&rad2);
00120
00121 IntervalPow(x_new,2,&d);
00122 IntervalInvert(&d,&d);
00123 IntervalAdd(&d,&rad2,&d);
00124 IntervalSqrt(&d,&d);
00125 IntervalInvert(&d,&md);
00126
00127 Intersection(y_new,&d,&a);
00128 Intersection(y_new,&md,&b);
00129
00130 full=Union(&a,&b,y_new);
00131
00132 if (full)
00133 {
00134
00135 IntervalPow(y_new,2,&d);
00136 IntervalInvert(&d,&d);
00137 IntervalAdd(&d,&rad2,&d);
00138 IntervalSqrt(&d,&d);
00139 IntervalInvert(&d,&md);
00140
00141 Intersection(x_new,&d,&a);
00142 Intersection(x_new,&md,&b);
00143
00144 full=Union(&a,&b,x_new);
00145 }
00146 }
00147 return(full);
00148 }
00149
00150
00151
00152
00153
00154 boolean BoxSphereClipping(double r2,
00155 Tinterval *x,Tinterval *y,Tinterval *z,
00156 Tinterval *x_new,Tinterval *y_new,Tinterval *z_new)
00157 {
00158 Tinterval d,md,a,b,rad2,diameter;
00159 boolean full;
00160 double r;
00161
00162
00163
00164 r=sqrt(r2);
00165 NewInterval(-r-ZERO,+r+ZERO,&diameter);
00166 full=((Intersection(x,&diameter,x_new))&&
00167 (Intersection(y,&diameter,y_new))&&
00168 (Intersection(z,&diameter,z_new)));
00169
00170 if (full)
00171 {
00172
00173 NewInterval(r2-ZERO,r2+ZERO,&rad2);
00174 IntervalPow(x_new,2,&a);
00175 IntervalPow(y_new,2,&b);
00176 IntervalAdd(&a,&b,&d);
00177 IntervalInvert(&d,&d);
00178 IntervalAdd(&d,&rad2,&d);
00179 IntervalSqrt(&d,&d);
00180 IntervalInvert(&d,&md);
00181
00182 Intersection(z_new,&d,&a);
00183 Intersection(z_new,&md,&b);
00184 full=Union(&a,&b,z_new);
00185
00186 if (full)
00187 {
00188
00189 IntervalPow(x_new,2,&a);
00190 IntervalPow(z_new,2,&b);
00191 IntervalAdd(&a,&b,&d);
00192 IntervalInvert(&d,&d);
00193 IntervalAdd(&d,&rad2,&d);
00194 IntervalSqrt(&d,&d);
00195 IntervalInvert(&d,&md);
00196
00197 Intersection(y_new,&d,&a);
00198 Intersection(y_new,&md,&b);
00199 full=Union(&a,&b,y_new);
00200
00201 if (full)
00202 {
00203
00204 IntervalPow(y_new,2,&a);
00205 IntervalPow(z_new,2,&b);
00206 IntervalAdd(&a,&b,&d);
00207 IntervalInvert(&d,&d);
00208 IntervalAdd(&d,&rad2,&d);
00209 IntervalSqrt(&d,&d);
00210 IntervalInvert(&d,&md);
00211
00212 Intersection(x_new,&d,&a);
00213 Intersection(x_new,&md,&b);
00214 full=Union(&a,&b,x_new);
00215 }
00216 }
00217 }
00218 return(full);
00219 }
00220
00221
00222
00223
00224 boolean RectangleParabolaClipping(Tinterval *x,double alpha,Tinterval *y,
00225 Tinterval *x_new,Tinterval *y_new)
00226 {
00227 Tinterval d,md,a,b,s;
00228 boolean full;
00229 double s1,s2;
00230
00231
00232 ROUNDDOWN;
00233 s1=1/alpha;
00234 ROUNDUP;
00235 s2=1/alpha;
00236 ROUNDNEAR;
00237 NewInterval(s1,s2,&s);
00238
00239 IntervalPow(x,2,&d);
00240 IntervalProduct(&d,&s,&d);
00241
00242 full=Intersection(y,&d,y_new);
00243
00244 if (full)
00245 {
00246
00247 IntervalScale(y_new,alpha,&d);
00248 IntervalSqrt(&d,&d);
00249 IntervalInvert(&d,&md);
00250
00251 Intersection(x,&d,&a);
00252 Intersection(x,&md,&b);
00253 full=Union(&a,&b,x_new);
00254 }
00255
00256 return(full);
00257 }
00258
00259
00260
00261
00262
00263
00264 boolean BoxSaddleClipping(Tinterval *x,Tinterval *y,double alpha,Tinterval *z,
00265 Tinterval *x_new,Tinterval *y_new,Tinterval *z_new)
00266
00267 {
00268 Tinterval d,s;
00269 boolean full;
00270 double s1,s2;
00271
00272
00273 ROUNDDOWN;
00274 s1=1/alpha;
00275 ROUNDUP;
00276 s2=1/alpha;
00277 ROUNDNEAR;
00278 NewInterval(s1,s2,&s);
00279
00280 IntervalProduct(x,y,&d);
00281 IntervalProduct(&s,&d,&d);
00282
00283 full=Intersection(z,&d,z_new);
00284
00285 if (full)
00286 {
00287
00288 if (IsInside(0,y))
00289 CopyInterval(x_new,x);
00290 else
00291 {
00292 IntervalDivision(z_new,y,&d);
00293 IntervalScale(&d,alpha,&d);
00294 full=Intersection(x,&d,x_new);
00295 }
00296
00297 if (full)
00298 {
00299
00300 if (IsInside(0,x_new))
00301 CopyInterval(y_new,y);
00302 else
00303 {
00304 IntervalDivision(z_new,x_new,&d);
00305 IntervalScale(&d,alpha,&d);
00306 full=Intersection(y,&d,y_new);
00307 }
00308 }
00309 }
00310 return(full);
00311 }
00312
00313 void DefineNormalVector(double *v,double *n)
00314 {
00315 unsigned int i;
00316 double norm;
00317
00318 if (v[0]!=0.0)
00319 {
00320 n[1]=n[2]=1.0;
00321 n[0]=-(v[1]+v[2])/v[0];
00322 }
00323 else
00324 {
00325 if (v[1]!=0.0)
00326 {
00327 n[0]=n[2]=1.0;
00328 n[1]=-(v[0]+v[2])/v[1];
00329 }
00330 else
00331 {
00332 if (v[2]!=0.0)
00333 {
00334 n[0]=n[1]=1.0;
00335 n[2]=-(v[0]+v[1])/v[2];
00336 }
00337 else
00338 Error("Null input vector in DefineNormalVector");
00339 }
00340 }
00341 norm=0.0;
00342 for(i=0;i<3;i++)
00343 norm=norm+v[i]*v[i];
00344 norm=sqrt(norm);
00345 for(i=0;i<3;i++)
00346 v[i]/=norm;
00347 }
00348
00349 void CrossProduct(double *v1,double *v2,double *v3)
00350 {
00351 double a,b,c;
00352
00353 a= (v1[1]*v2[2])-(v1[2]*v2[1]);
00354 b=-(v1[0]*v2[2])+(v1[2]*v2[0]);
00355 c= (v1[0]*v2[1])-(v1[1]*v2[0]);
00356
00357 v3[0]=a;
00358 v3[1]=b;
00359 v3[2]=c;
00360 }
00361
00362 double DotProduct(double *v1,double *v2)
00363 {
00364 return(v1[0]*v2[0]+v1[1]*v2[1]+v1[2]*v2[2]);
00365 }
00366
00367
00368 int tolerant_SV_decomp(int M,int N,gsl_matrix *U,gsl_matrix *V,gsl_vector *S)
00369 {
00370 gsl_matrix* VT;
00371 gsl_vector* S2;
00372 gsl_matrix* UT ;
00373 gsl_vector* temp;
00374 int i, j;
00375 int err;
00376
00377 if (M<N)
00378 {
00379 temp=gsl_vector_calloc(M);
00380 S2=gsl_vector_calloc(M);
00381 UT=gsl_matrix_calloc(M,M);
00382 VT=gsl_matrix_calloc(N,M);
00383 for(i=0;i<M;i++)
00384 {
00385 for(j=0;j<N;j++)
00386 gsl_matrix_set(VT,j,i,gsl_matrix_get(U,i,j));
00387 }
00388
00389 err=gsl_linalg_SV_decomp(VT,UT,S2,temp);
00390
00391 if (!err)
00392 {
00393 for(i=0;i<M;i++)
00394 {
00395 for(j=0;j<N;j++)
00396 {
00397 gsl_matrix_set(V,j,i,gsl_matrix_get(VT,j,i));
00398 gsl_matrix_set(U,i,j,(j<M ? gsl_matrix_get(UT,i,j):0));
00399 }
00400 gsl_vector_set(S,i,gsl_vector_get(S2,i));
00401 }
00402 }
00403
00404 gsl_matrix_free(VT);
00405 gsl_matrix_free(UT);
00406 gsl_vector_free(temp);
00407 gsl_vector_free(S2);
00408 }
00409 else
00410 {
00411 temp=gsl_vector_calloc(N);
00412 err=gsl_linalg_SV_decomp(U,V,S,temp);
00413 gsl_vector_free(temp);
00414 }
00415 return(err);
00416 }