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 #include <stdio.h>
00042 #include <math.h>
00043 #include "memvirtu.h"
00044 #include "lowparam.h"
00045 #include "lowsolid.h"
00046 #include "vectorop.h"
00047
00048
00049
00050 static matrix I =
00051 {
00052 { 1.0, 0.0, 0.0, 0.0 },
00053 { 0.0, 1.0, 0.0, 0.0 },
00054 { 0.0, 0.0, 1.0, 0.0 },
00055 { 0.0, 0.0, 0.0, 1.0 },
00056 };
00057
00058 void invmat(matrix mat, matrix inv, int n)
00059 {
00060 real detA;
00061 int i, j;
00062
00063 detA = determ(mat, n);
00064 for (i = 0; i < n; i++)
00065 {
00066 for (j = 0; j < n; j++)
00067 {
00068 inv[j][i] = cofactor(mat, n, i, j) / detA;
00069 }
00070 }
00071 }
00072
00073
00074 real cofactor(matrix mat, int dim, int i, int j)
00075 {
00076 real tmp;
00077 matrix minor;
00078 int m, n, k, l;
00079
00080 for (m = k = 0; k < dim - 1; k++)
00081 {
00082 if (m == i)
00083 {
00084 m++;
00085 }
00086 for (n = l = 0; l < dim - 1; l++)
00087 {
00088 if (n == j)
00089 {
00090 n++;
00091 }
00092 minor[k][l] = mat[m][n];
00093 n++;
00094 }
00095 m++;
00096 }
00097
00098 tmp = determ(minor, dim - 1);
00099 return((i + j) % 2 == 0 ? tmp : -tmp);
00100 }
00101
00102
00103 real determ(matrix mat, int dim)
00104 {
00105 real sum;
00106 int i;
00107
00108 if (dim == 1)
00109 {
00110 return(mat[0][0]);
00111 }
00112 for (sum = 0.0, i = 0; i < dim; i++)
00113 {
00114 sum += mat[0][i] * cofactor(mat, dim, 0, i);
00115 }
00116 return(sum);
00117 }
00118
00119
00120
00121
00122 void matmult(matrix m, matrix m1, matrix m2)
00123 {
00124 vecmult(m[0], m1[0], m2);
00125 vecmult(m[1], m1[1], m2);
00126 vecmult(m[2], m1[2], m2);
00127 vecmult(m[3], m1[3], m2);
00128 }
00129
00130
00131
00132
00133 void matminus(matrix m, matrix m1, matrix m2)
00134 {
00135 int i, j;
00136
00137 for (i = 0; i < 4; i++)
00138 {
00139 for (j = 0; j < 4; j++)
00140 {
00141 m[i][j] = m1[i][j] - m2[i][j];
00142 }
00143 }
00144 }
00145
00146
00147
00148
00149 void matplus(matrix m, matrix m1, matrix m2)
00150 {
00151 int i, j;
00152
00153 for (i = 0; i < 4; i++)
00154 {
00155 for (j = 0; j < 4; j++)
00156 {
00157 m[i][j] = m1[i][j] + m2[i][j];
00158 }
00159 }
00160 }
00161
00162
00163
00164
00165 void invrefine(matrix mat, matrix inv)
00166 {
00167 matrix R, M;
00168
00169 matmult(M, mat, inv);
00170 matminus(R, I, M);
00171 matmult(M, inv, R);
00172 matplus(inv, M, inv);
00173 }
00174
00175
00176 void matident(matrix m)
00177 {
00178 m[0][0] = 1.0;
00179 m[1][0] = m[2][0] = m[3][0] = 0.0;
00180 m[1][1] = 1.0;
00181 m[0][1] = m[2][1] = m[3][1] = 0.0;
00182 m[2][2] = 1.0;
00183 m[0][2] = m[1][2] = m[3][2] = 0.0;
00184 m[3][3] = 1.0;
00185 m[0][3] = m[1][3] = m[2][3] = 0.0;
00186 }
00187
00188
00189 void matzer(matrix m)
00190 {
00191 m[0][0] = m[1][0] = m[2][0] = m[3][0] = 0.0;
00192 m[1][1] = m[0][1] = m[2][1] = m[3][1] = 0.0;
00193 m[2][2] = m[0][2] = m[1][2] = m[3][2] = 0.0;
00194 m[3][3] = m[0][3] = m[1][3] = m[2][3] = 0.0;
00195 }
00196
00197
00198 int testident(matrix m, real eps)
00199 {
00200 int i, j;
00201
00202 for (i = 0; i < 4; i++)
00203 {
00204 for (j = 0; j < 4; j++)
00205 {
00206 if (comp(m[i][j], (i == j) ? 1.0 : 0.0, eps) != 0)
00207 {
00208 return(FALSE);
00209 }
00210 }
00211 }
00212 return(SUCCESS);
00213 }
00214
00215
00216
00217
00218 void mattrans(matrix m, real tx, real ty, real tz)
00219 {
00220 real b;
00221
00222 b = m[0][3];
00223 m[0][0] += b * tx;
00224 m[0][1] += b * ty;
00225 m[0][2] += b * tz;
00226 b = m[1][3];
00227 m[1][0] += b * tx;
00228 m[1][1] += b * ty;
00229 m[1][2] += b * tz;
00230 b = m[2][3];
00231 m[2][0] += b * tx;
00232 m[2][1] += b * ty;
00233 m[2][2] += b * tz;
00234 b = m[3][3];
00235 m[3][0] += b * tx;
00236 m[3][1] += b * ty;
00237 m[3][2] += b * tz;
00238 }
00239
00240
00241
00242
00243
00244 void matrotat(matrix mat, real rx, real ry, real rz)
00245 {
00246 real c, s, m, n;
00247
00248 rx = rx * PI / 180.0;
00249 ry = ry * PI / 180.0;
00250 rz = rz * PI / 180.0;
00251 if (comp(rx, 0.0, EPS) != 0)
00252 {
00253 c = cos(rx);
00254 s = sin(rx);
00255 m = mat[0][1];
00256 n = mat[0][2];
00257 mat[0][1] = m * c - n * s;
00258 mat[0][2] = n * c + m * s;
00259 m = mat[1][1];
00260 n = mat[1][2];
00261 mat[1][1] = m * c - n * s;
00262 mat[1][2] = n * c + m * s;
00263 m = mat[2][1];
00264 n = mat[2][2];
00265 mat[2][1] = m * c - n * s;
00266 mat[2][2] = n * c + m * s;
00267 m = mat[3][1];
00268 n = mat[3][2];
00269 mat[3][1] = m * c - n * s;
00270 mat[3][2] = n * c + m * s;
00271 }
00272 if (comp(ry, 0.0, EPS) != 0)
00273 {
00274 c = cos(ry);
00275 s = sin(ry);
00276 m = mat[0][0];
00277 n = mat[0][2];
00278 mat[0][0] = m * c + n * s;
00279 mat[0][2] = n * c - m * s;
00280 m = mat[1][0];
00281 n = mat[1][2];
00282 mat[1][0] = m * c + n * s;
00283 mat[1][2] = n * c - m * s;
00284 m = mat[2][0];
00285 n = mat[2][2];
00286 mat[2][0] = m * c + n * s;
00287 mat[2][2] = n * c - m * s;
00288 m = mat[3][0];
00289 n = mat[3][2];
00290 mat[3][0] = m * c + n * s;
00291 mat[3][2] = n * c - m * s;
00292 }
00293 if (comp(rz, 0.0, EPS) != 0)
00294 {
00295 c = cos(rz);
00296 s = sin(rz);
00297 m = mat[0][0];
00298 n = mat[0][1];
00299 mat[0][0] = m * c - n * s;
00300 mat[0][1] = n * c + m * s;
00301 m = mat[1][0];
00302 n = mat[1][1];
00303 mat[1][0] = m * c - n * s;
00304 mat[1][1] = n * c + m * s;
00305 m = mat[2][0];
00306 n = mat[2][1];
00307 mat[2][0] = m * c - n * s;
00308 mat[2][1] = n * c + m * s;
00309 m = mat[3][0];
00310 n = mat[3][1];
00311 mat[3][0] = m * c - n * s;
00312 mat[3][1] = n * c + m * s;
00313 }
00314 }
00315
00316
00317
00318
00319
00320 void matscale(matrix m, real sx, real sy, real sz)
00321 {
00322 m[0][0] *= sx;
00323 m[0][1] *= sy;
00324 m[0][2] *= sz;
00325 m[1][0] *= sx;
00326 m[1][1] *= sy;
00327 m[1][2] *= sz;
00328 m[2][0] *= sx;
00329 m[2][1] *= sy;
00330 m[2][2] *= sz;
00331 m[3][0] *= sx;
00332 m[3][1] *= sy;
00333 m[3][2] *= sz;
00334 }
00335
00336
00337 void matcopy(matrix m1, matrix m2)
00338 {
00339 int i, j;
00340
00341 for (i = 0; i < 4; i++)
00342 {
00343 for (j = 0; j < 4; j++)
00344 {
00345 m1[i][j] = m2[i][j];
00346 }
00347 }
00348 }
00349
00350
00351
00352
00353 void vecmult(vector v1, vector v2, matrix m)
00354 {
00355 vector v;
00356
00357 v[0] = v2[0] * m[0][0] + v2[1] * m[1][0] + v2[2] * m[2][0] + v2[3] * m[3][0];
00358 v[1] = v2[0] * m[0][1] + v2[1] * m[1][1] + v2[2] * m[2][1] + v2[3] * m[3][1];
00359 v[2] = v2[0] * m[0][2] + v2[1] * m[1][2] + v2[2] * m[2][2] + v2[3] * m[3][2];
00360 v[3] = v2[0] * m[0][3] + v2[1] * m[1][3] + v2[2] * m[2][3] + v2[3] * m[3][3];
00361 veccopy(v1, v);
00362 }
00363
00364
00365 void matprint(matrix m)
00366 {
00367 int i, j;
00368
00369 for (i = 0; i < 4; i++, printf("\n"))
00370 {
00371 for (j = 0; j < 4; j++)
00372 {
00373 printf("%f ", m[i][j]);
00374 }
00375 }
00376 }
00377
00378
00379 void mattranspose(matrix m, matrix mtransp)
00380 {
00381 int i, j;
00382 matrix tmp;
00383
00384 for (i = 0; i < 4; ++i)
00385 {
00386 for (j = 0; j < 4; ++j)
00387 {
00388 tmp[i][j] = m[j][i];
00389 }
00390 }
00391 matcopy(mtransp, tmp);
00392 }
00393
00394
00395 int comp(real f1, real f2, real tol)
00396 {
00397 if (fabs(f1 - f2) < tol)
00398 {
00399 return(0);
00400 }
00401 if (f1 > f2)
00402 {
00403 return(1);
00404 }
00405 return(-1);
00406 }
00407
00408
00409 real *makevec(vector v, real x, real y, real z, real w)
00410 {
00411 v[0] = x;
00412 v[1] = y;
00413 v[2] = z;
00414 v[3] = w;
00415 return(v);
00416 }
00417
00418
00419 real dot(vector v1, vector v2)
00420 {
00421 return(v1[0] * v2[0] + v1[1] * v2[1] + v1[2] * v2[2]);
00422 }
00423
00424
00425 real dot2(vector v1, vector v2, int drop)
00426 {
00427 switch (drop)
00428 {
00429 case 0:
00430 return(v1[1] * v2[1] + v1[2] * v2[2]);
00431
00432 case 1:
00433 return(v1[0] * v2[0] + v1[2] * v2[2]);
00434
00435 case 2:
00436 return(v1[0] * v2[0] + v1[1] * v2[1]);
00437 }
00438 return(0.0);
00439 }
00440
00441
00442 real vecd(vector v1, vector v2)
00443 {
00444 real dx, dy, dz;
00445
00446 dx = v1[0] - v2[0];
00447 dy = v1[1] - v2[1];
00448 dz = v1[2] - v2[2];
00449 return(sqrt(dx * dx + dy * dy + dz * dz));
00450 }
00451
00452
00453 int vecnull(vector v, real tol)
00454 {
00455 return(comp(dot(v, v), 0.0, tol) == 0);
00456 }
00457
00458
00459 int vecnull2(vector vec, real eps, int drop)
00460 {
00461 int nulx, nuly, nulz;
00462
00463 nulx = comp(vec[0], 0.0, eps);
00464 nuly = comp(vec[1], 0.0, eps);
00465 nulz = comp(vec[2], 0.0, eps);
00466 if ((drop == 0) && (nuly == 0) && (nulz == 0))
00467 {
00468 return(TRUE);
00469 }
00470 if ((drop == 1) && (nulx == 0) && (nulz == 0))
00471 {
00472 return(TRUE);
00473 }
00474 if ((drop == 2) && (nulx == 0) && (nuly == 0))
00475 {
00476 return(TRUE);
00477 }
00478 return(FALSE);
00479 }
00480
00481
00482 int vecequal(vector v1, vector v2)
00483 {
00484 real dx, dy, dz, diff;
00485
00486 dx = v1[0] - v2[0];
00487 dy = v1[1] - v2[1];
00488 dz = v1[2] - v2[2];
00489 diff = dx * dx + dy * dy + dz * dz;
00490 return(comp(diff, 0.0, EPS * EPS) == 0);
00491 }
00492
00493
00494 int veccolin(vector v1, vector v2, real eps)
00495 {
00496 vector tmp;
00497
00498 return(vecnull(cross(tmp, v1, v2), eps));
00499 }
00500
00501
00502
00503
00504 real *cross(vector v1, vector v2, vector v3)
00505 {
00506 vector v;
00507
00508 v[0] = v2[1] * v3[2] - v2[2] * v3[1];
00509 v[1] = v2[2] * v3[0] - v2[0] * v3[2];
00510 v[2] = v2[0] * v3[1] - v2[1] * v3[0];
00511 v[3] = 1.0;
00512 return(veccopy(v1, v));
00513 }
00514
00515
00516 char normalize(vector v)
00517 {
00518 real vl;
00519
00520 vl = sqrt(dot(v, v));
00521 if (comp(vl, 0.0, EPS) != 0)
00522 {
00523 v[0] /= vl;
00524 v[1] /= vl;
00525 v[2] /= vl;
00526 }
00527 return(comp(vl, 0.0, EPS) != 0);
00528 }
00529
00530
00531 real *veccopy(vector v1, vector v2)
00532 {
00533 v1[0] = v2[0];
00534 v1[1] = v2[1];
00535 v1[2] = v2[2];
00536 v1[3] = v2[3];
00537 return(v1);
00538 }
00539
00540
00541 real *vecminus(vector res, vector v1, vector v2)
00542 {
00543 int i;
00544
00545 for (i = 0; i < 3; i++)
00546 {
00547 res[i] = v1[i] - v2[i];
00548 }
00549 res[3] = 1.0;
00550 return(res);
00551 }
00552
00553
00554 real *vecplus(vector res, vector v1, vector v2)
00555 {
00556 int i;
00557
00558 for (i = 0; i < 3; i++)
00559 {
00560 res[i] = v1[i] + v2[i];
00561 }
00562 res[3] = 1.0;
00563 return(res);
00564 }
00565
00566
00567 void vecesc(vector v, vector ve, real k)
00568 {
00569 int i;
00570
00571 for (i = 0; i < 3; i++)
00572 {
00573 v[i] = k * ve[i];
00574 }
00575 v[3] = 1.0;
00576 }
00577
00578
00579 int getdrop(vector vec)
00580 {
00581 real max, tmp;
00582 int res;
00583
00584 max = fabs(vec[0]);
00585 res = 0;
00586 if ((tmp = fabs(vec[1])) > max)
00587 {
00588 max = tmp;
00589 res = 1;
00590 }
00591 if (fabs(vec[2]) > max)
00592 {
00593 res = 2;
00594 }
00595 return(res);
00596 }
00597
00598 extern FILE *trace;
00599
00600 void vecprint(vector v)
00601 {
00602 printf("%f %f %f %f\n", v[0], v[1], v[2], v[3]);
00603 }
00604
00605 real *veczer(vector v)
00606 {
00607 v[0] = v[1] = v[2] = 0.0;
00608 v[3] = 1.0;
00609 return(v);
00610 }
00611
00612 real *vecpe(vector vs, vector ve, vector v, real k)
00613 {
00614 int i;
00615
00616 for (i = 0; i < 3; i++)
00617 {
00618 v[i] = ve[i] + k * vs[i];
00619 }
00620 v[3] = 1.0;
00621 return(v);
00622 }
00623
00624 real *calc_p(vector v1, vector v2, real t, vector v)
00625 {
00626 int i;
00627
00628 for (i = 0; i < 3; i++)
00629 {
00630 v[i] = (v1[i] - v2[i]) * t + v2[i];
00631 }
00632 v[3] = 1.0;
00633 return(v);
00634 }
00635
00636 real *vec2cp(vector v, vector o)
00637 {
00638 v[0] = o[0];
00639 v[1] = o[1];
00640 v[2] = o[2];
00641 return(v);
00642 }
00643
00644 real vecd2(vector v1, vector v2)
00645 {
00646 real dx, dy;
00647
00648 dx = v1[0] - v2[0];
00649 dy = v1[1] - v2[1];
00650 return(sqrt(dx * dx + dy * dy));
00651 }
00652
00653 void rotmat(vector axis, real th, matrix rot)
00654 {
00655 real dth;
00656 real cx, sx, dx, r1, r2, r3, r12, r13, r23;
00657
00658 dth = th;
00659 cx = cos(dth);
00660 sx = sin(dth);
00661 dx = 1.0 - cx;
00662 r1 = axis[0];
00663 r2 = axis[1];
00664 r3 = axis[2];
00665 r12 = r1 * r2;
00666 r13 = r1 * r3;
00667 r23 = r2 * r3;
00668
00669 matident(rot);
00670 rot[0][0] = cx + dx * r1 * r1;
00671 rot[0][1] = sx * r3 + dx * r12;
00672 rot[0][2] = -sx * r2 + dx * r13;
00673
00674 rot[1][0] = -sx * r3 + dx * r12;
00675 rot[1][1] = cx + dx * r2 * r2;
00676 rot[1][2] = sx * r1 + dx * r23;
00677
00678 rot[2][0] = sx * r2 + dx * r13;
00679 rot[2][1] = -sx * r1 + dx * r23;
00680 rot[2][2] = cx + dx * r3 * r3;
00681 }