/** * $Id:$ * ***** BEGIN GPL/BL DUAL LICENSE BLOCK ***** * * The contents of this file may be used under the terms of either the GNU * General Public License Version 2 or later (the "GPL", see * http://www.gnu.org/licenses/gpl.html ), or the Blender License 1.0 or * later (the "BL", see http://www.blender.org/BL/ ) which has to be * bought from the Blender Foundation to become active, in which case the * above mentioned GPL option does not apply. * * The Original Code is Copyright (C) 1997 by Ton Roosendaal, Frank van Beek and Joeri Kassenaar. * All rights reserved. * * The Original Code is: all of this file. * * Contributor(s): none yet. * * ***** END GPL/BL DUAL LICENSE BLOCK ***** */ /* formules voor trace */ /* i_window(left, right, bottom, top, near, far,mat) Mat3Transp(mat); Mat4Transp(mat); Mat3Inv(mat1,mat2) Mat4Inv(mat1,mat2) (mat1= inv mat2, is wel met 3x3 berekend) Mat4InvGG(mat1,mat2) (mat1= inv mat2 uit GraphicGems) Mat4Invert(mat1, mat2) DEZE IS ECHT GOED Mat3Adj(mat1,mat2) Mat3MulMat3(mat1,mat2,mat3) Mat4MulMat4(mat1,mat2,mat3) (mat1=mat2*mat3) Mat3MulSerie(answ, m1, m2, m3....) Mat4CpyMat4(mat1,mat2) (mat1=mat2) Mat3CpyMat4(mat1,mat2) Mat4CpyMat3(mat1,mat2) (geen clear) Mat3CpyMat3(mat1,mat2) Mat4SwapMat4(mat1,mat2); Mat3Clr(mat) Mat4Clr(mat) Mat3One(mat) Mat4One(mat) Mat3MulVecfl(mat,vec) Mat4MulVecfl(mat,vec) (vec is 3) Mat4MulVec4fl(mat,vec) (vec is 4) Mat3MulVec(mat,vec) Mat4MulVec(mat,vec) (vec is 3) Mat3MulFloat(mat,f) Mat4MulFloat(mat,f) Mat4MulFloat3(m,f) (alleen de scale component ) Mat3Ortho(mat) Mat4Ortho(mat) VecStar(mat, vec) matrix wordt Stermatrix van vec, soort uitprodukt met X-Y-Z-assen short EenheidsMat(mat) (mat3) short FloatCompare(v1,v2,limit) printmatrix3(mat) printmatrix4(mat) QuatToMat3(q,m) Mat3ToQuat(m, q) QuatMul(q1,q2,q3) (q1=q2*q3) NormalQuat(q) float Normalise(n) VecCopyf(v1,v2) (v1=v2) long VecLen(v1,v2) (afstand tussen twee punten) float VecLenf(v1,v2) VecAddf(v, v1,v2) VecSubf(v, v1,v2) VecMidf(v, v1,v2) VecMulf(v1,f) Crossf(vecout,vec1,vec2) float Inpf(vec1,vec2) CalcNormShort(v1,v2,v3,n) CalcNormLong(v1,v2,v3,n) CalcNormFloat(v1,v2,v3,n) CalcCent3f(cent, v1, v2, v3) CalcCent4f(cent, v1, v2, v3, v4) float Sqrt3f(f) (derdemachts wortel) float Sqrt3d(d) float DistVL2Dfl(v1,v2,v3) (afstand v1 tot lijn v2-v3 :GEEN LIJNSTUK!) float PdistVL2Dfl(v1,v2,v3) (pointdist v1 tot lijn v2-v3 :LIJNSTUK!) float AreaF2Dfl(v1,v2,v3) (oppervlakte van een 2D driehoek) float AreaQ3Dfl(v1, v2, v3, v4) (alleen bolle vierhoeken) float AreaT3Dfl(v1, v2, v3) (triangles) float AreaPoly3Dfl(nr, verts, normal) (met trapezium regel) MinMaxRGB(c) Spec(inpr,spec) */ /* ************************ FUNKTIES **************************** */ #include #include /* #include "/usr/people/include/Trace.h" */ #define SMALL_NUMBER 1.e-8 void Mat3Transp(mat) float mat[][3]; { float t; t = mat[0][1] ; mat[0][1] = mat[1][0] ; mat[1][0] = t; t = mat[0][2] ; mat[0][2] = mat[2][0] ; mat[2][0] = t; t = mat[1][2] ; mat[1][2] = mat[2][1] ; mat[2][1] = t; } void Mat4Transp(mat) float mat[][4]; { float t; t = mat[0][1] ; mat[0][1] = mat[1][0] ; mat[1][0] = t; t = mat[0][2] ; mat[0][2] = mat[2][0] ; mat[2][0] = t; t = mat[0][3] ; mat[0][3] = mat[3][0] ; mat[3][0] = t; t = mat[1][2] ; mat[1][2] = mat[2][1] ; mat[2][1] = t; t = mat[1][3] ; mat[1][3] = mat[3][1] ; mat[3][1] = t; t = mat[2][3] ; mat[2][3] = mat[3][2] ; mat[3][2] = t; } /* IS NIET GOED!!! */ void Mat4Inverto(to,from) /* uit insane/libwin/matrix.c van Haeberli */ float to[][4], from[][4]; /* LET OP: ALLEEN MET TRANSPOSE MATRIXEN */ { double wtemp[4][8]; /* doubles voor betere resolutie zeer kleine getallen */ double m0,m1,m2,m3,s; double *r0,*r1,*r2,*r3, *rtemp; r0 = wtemp[0]; r1 = wtemp[1]; r2 = wtemp[2]; r3 = wtemp[3]; r0[0] = from[0][0]; /* build up [A][I] */ r0[1] = from[1][0]; r0[2] = from[2][0]; r0[3] = from[3][0]; r0[4] = 1.0; r0[5] = 0.0; r0[6] = 0.0; r0[7] = 0.0; r1[0] = from[0][1]; r1[1] = from[1][1]; r1[2] = from[2][1]; r1[3] = from[3][1]; r1[4] = 0.0; r1[5] = 1.0; r1[6] = 0.0; r1[7] = 0.0; r2[0] = from[0][2]; r2[1] = from[1][2]; r2[2] = from[2][2]; r2[3] = from[3][2]; r2[4] = 0.0; r2[5] = 0.0; r2[6] = 1.0; r2[7] = 0.0; r3[0] = from[0][3]; r3[1] = from[1][3]; r3[2] = from[2][3]; r3[3] = from[3][3]; r3[4] = 0.0; r3[5] = 0.0; r3[6] = 0.0; r3[7] = 1.0; if (r0[0] == 0.0) { /* swap rows if needed */ if (r1[0] == 0.0) { if (r2[0] == 0.0) { if (r3[0] == 0.0) goto singular; rtemp = r0; r0 = r3; r3 = rtemp; } else { rtemp = r0; r0 = r2; r2 = rtemp; } } else { rtemp = r0; r0 = r1; r1 = rtemp; } } m1 = r1[0]/r0[0]; /* eliminate first variable */ m2 = r2[0]/r0[0]; m3 = r3[0]/r0[0]; s = r0[1]; r1[1] = r1[1] - m1 * s; r2[1] = r2[1] - m2 * s; r3[1] = r3[1] - m3 * s; s = r0[2]; r1[2] = r1[2] - m1 * s; r2[2] = r2[2] - m2 * s; r3[2] = r3[2] - m3 * s; s = r0[3]; r1[3] = r1[3] - m1 * s; r2[3] = r2[3] - m2 * s; r3[3] = r3[3] - m3 * s; s = r0[4]; if (s != 0.0) { r1[4] = r1[4] - m1 * s; r2[4] = r2[4] - m2 * s; r3[4] = r3[4] - m3 * s; } s = r0[5]; if (s != 0.0) { r1[5] = r1[5] - m1 * s; r2[5] = r2[5] - m2 * s; r3[5] = r3[5] - m3 * s; } s = r0[6]; if (s != 0.0) { r1[6] = r1[6] - m1 * s; r2[6] = r2[6] - m2 * s; r3[6] = r3[6] - m3 * s; } s = r0[7]; if (s != 0.0) { r1[7] = r1[7] - m1 * s; r2[7] = r2[7] - m2 * s; r3[7] = r3[7] - m3 * s; } if (r1[1] == 0.0) { /* swap rows if needed */ if (r2[1] == 0.0) { if (r3[1] == 0.0) goto singular; rtemp = r1; r1 = r3; r3 = rtemp; } else { rtemp = r1; r1 = r2; r2 = rtemp; } } m2 = r2[1]/r1[1]; /* eliminate second variable */ m3 = r3[1]/r1[1]; r2[2] = r2[2] - m2 * r1[2]; r3[2] = r3[2] - m3 * r1[2]; r3[3] = r3[3] - m3 * r1[3]; r2[3] = r2[3] - m2 * r1[3]; s = r1[4]; if (s != 0.0) { r2[4] = r2[4] - m2 * s; r3[4] = r3[4] - m3 * s; } s = r1[5]; if (s != 0.0) { r2[5] = r2[5] - m2 * s; r3[5] = r3[5] - m3 * s; } s = r1[6]; if (s != 0.0) { r2[6] = r2[6] - m2 * s; r3[6] = r3[6] - m3 * s; } s = r1[7]; if (s != 0.0) { r2[7] = r2[7] - m2 * s; r3[7] = r3[7] - m3 * s; } if (r2[2] == 0.0) { /* swap last 2 rows if needed */ if (r3[2] == 0.0) goto singular; rtemp = r2; r2 = r3; r3 = rtemp; } m3 = r3[2]/r2[2]; /* eliminate third variable */ r3[3] = r3[3] - m3 * r2[3]; r3[4] = r3[4] - m3 * r2[4]; r3[5] = r3[5] - m3 * r2[5]; r3[6] = r3[6] - m3 * r2[6]; r3[7] = r3[7] - m3 * r2[7]; if (r3[3] == 0.0) goto singular; s = 1.0/r3[3]; /* now back substitute row 3 */ r3[4] = r3[4] * s; r3[5] = r3[5] * s; r3[6] = r3[6] * s; r3[7] = r3[7] * s; m2 = r2[3]; /* now back substitute row 2 */ s = 1.0/r2[2]; r2[4] = s * (r2[4] - r3[4] * m2); r2[5] = s * (r2[5] - r3[5] * m2); r2[6] = s * (r2[6] - r3[6] * m2); r2[7] = s * (r2[7] - r3[7] * m2); m1 = r1[3]; r1[4] = (r1[4] - r3[4] * m1); r1[5] = (r1[5] - r3[5] * m1); r1[6] = (r1[6] - r3[6] * m1); r1[7] = (r1[7] - r3[7] * m1); m0 = r0[3]; r0[4] = (r0[4] - r3[4] * m0); r0[5] = (r0[5] - r3[5] * m0); r0[6] = (r0[6] - r3[6] * m0); r0[7] = (r0[7] - r3[7] * m0); m1 = r1[2]; /* now back substitute row 1 */ s = 1.0/r1[1]; r1[4] = s * (r1[4] - r2[4] * m1); r1[5] = s * (r1[5] - r2[5] * m1); r1[6] = s * (r1[6] - r2[6] * m1); r1[7] = s * (r1[7] - r2[7] * m1); m0 = r0[2]; r0[4] = (r0[4] - r2[4] * m0); r0[5] = (r0[5] - r2[5] * m0); r0[6] = (r0[6] - r2[6] * m0); r0[7] = (r0[7] - r2[7] * m0); m0 = r0[1]; /* now back substitute row 0 */ s = 1.0/r0[0]; r0[4] = s * (r0[4] - r1[4] * m0); r0[5] = s * (r0[5] - r1[5] * m0); r0[6] = s * (r0[6] - r1[6] * m0); r0[7] = s * (r0[7] - r1[7] * m0); to[0][0] = r0[4]; /* copy results back */ to[1][0] = r0[5]; to[2][0] = r0[6]; to[3][0] = r0[7]; to[0][1] = r1[4]; to[1][1] = r1[5]; to[2][1] = r1[6]; to[3][1] = r1[7]; to[0][2] = r2[4]; to[1][2] = r2[5]; to[2][2] = r2[6]; to[3][2] = r2[7]; to[0][3] = r3[4]; to[1][3] = r3[5]; to[2][3] = r3[6]; to[3][3] = r3[7]; return; singular: return; } /* * invertmat - * computes the inverse of mat and puts it in inverse. Returns * TRUE on success (i.e. can always find a pivot) and FALSE on failure. * Uses Gaussian Elimination with partial (maximal column) pivoting. * * Mark Segal - 1992 */ int Mat4Invert(inverse, mat) float inverse[][4], mat[][4]; { #define ABS(x) ((x)>0?(x):(-(x))) #define SWAP(a,b,t) t = a;a = b;b = t int i, j, k; double temp; float tempmat[4][4]; float max; int maxj; /* Set inverse to identity */ for (i=0; i<4; i++) for (j=0; j<4; j++) inverse[i][j] = 0; for (i=0; i<4; i++) inverse[i][i] = 1; /* Copy original matrix so we don't mess it up */ for(i = 0; i < 4; i++) for(j = 0; j <4; j++) tempmat[i][j] = mat[i][j]; for(i = 0; i < 4; i++) { /* Look for row with max pivot */ max = ABS(tempmat[i][i]); maxj = i; for(j = i + 1; j < 4; j++) { if(ABS(tempmat[j][i]) > max) { max = ABS(tempmat[j][i]); maxj = j; } } /* Swap rows if necessary */ if (maxj != i) { for( k = 0; k < 4; k++) { SWAP(tempmat[i][k],tempmat[maxj][k],temp); SWAP(inverse[i][k],inverse[maxj][k],temp); } } temp = tempmat[i][i]; if (temp == 0) return 0; /* No non-zero pivot */ for(k = 0; k < 4; k++) { tempmat[i][k] /= temp; inverse[i][k] /= temp; } for(j = 0; j < 4; j++) { if(j != i) { temp = tempmat[j][i]; for(k = 0; k < 4; k++) { tempmat[j][k] -= tempmat[i][k]*temp; inverse[j][k] -= inverse[i][k]*temp; } } } } return 1; } void Mat4Inv(m1,m2) struct Matrix4 *m1,*m2; { short a,b; float det,mat1[3][3],mat2[3][3]; void Mat3Inv(); void Mat3CpyMat4(); void Mat4CpyMat3(); Mat3CpyMat4(mat2,m2); Mat3Inv(mat1,mat2); Mat4CpyMat3(m1,mat1); } float Det2x2(a,b,c,d) float a,b,c,d; { return a*d - b*c; } float Det3x3(a1,a2,a3,b1,b2,b3,c1,c2,c3 ) float a1,a2,a3,b1,b2,b3,c1,c2,c3; { float ans; ans = a1 * Det2x2( b2, b3, c2, c3 ) - b1 * Det2x2( a2, a3, c2, c3 ) + c1 * Det2x2( a2, a3, b2, b3 ); return ans; } float Det4x4(m) float m[][4]; { float ans; float a1,a2,a3,a4,b1,b2,b3,b4,c1,c2,c3,c4,d1,d2,d3,d4; a1= m[0][0]; b1= m[0][1]; c1= m[0][2]; d1= m[0][3]; a2= m[1][0]; b2= m[1][1]; c2= m[1][2]; d2= m[1][3]; a3= m[2][0]; b3= m[2][1]; c3= m[2][2]; d3= m[2][3]; a4= m[3][0]; b4= m[3][1]; c4= m[3][2]; d4= m[3][3]; ans = a1 * Det3x3( b2, b3, b4, c2, c3, c4, d2, d3, d4) - b1 * Det3x3( a2, a3, a4, c2, c3, c4, d2, d3, d4) + c1 * Det3x3( a2, a3, a4, b2, b3, b4, d2, d3, d4) - d1 * Det3x3( a2, a3, a4, b2, b3, b4, c2, c3, c4); return ans; } void Mat4Adj(out,in) /* out = ADJ(in) */ float out[][4],in[][4]; { float a1, a2, a3, a4, b1, b2, b3, b4; float c1, c2, c3, c4, d1, d2, d3, d4; a1= in[0][0]; b1= in[0][1]; c1= in[0][2]; d1= in[0][3]; a2= in[1][0]; b2= in[1][1]; c2= in[1][2]; d2= in[1][3]; a3= in[2][0]; b3= in[2][1]; c3= in[2][2]; d3= in[2][3]; a4= in[3][0]; b4= in[3][1]; c4= in[3][2]; d4= in[3][3]; out[0][0] = Det3x3( b2, b3, b4, c2, c3, c4, d2, d3, d4); out[1][0] = - Det3x3( a2, a3, a4, c2, c3, c4, d2, d3, d4); out[2][0] = Det3x3( a2, a3, a4, b2, b3, b4, d2, d3, d4); out[3][0] = - Det3x3( a2, a3, a4, b2, b3, b4, c2, c3, c4); out[0][1] = - Det3x3( b1, b3, b4, c1, c3, c4, d1, d3, d4); out[1][1] = Det3x3( a1, a3, a4, c1, c3, c4, d1, d3, d4); out[2][1] = - Det3x3( a1, a3, a4, b1, b3, b4, d1, d3, d4); out[3][1] = Det3x3( a1, a3, a4, b1, b3, b4, c1, c3, c4); out[0][2] = Det3x3( b1, b2, b4, c1, c2, c4, d1, d2, d4); out[1][2] = - Det3x3( a1, a2, a4, c1, c2, c4, d1, d2, d4); out[2][2] = Det3x3( a1, a2, a4, b1, b2, b4, d1, d2, d4); out[3][2] = - Det3x3( a1, a2, a4, b1, b2, b4, c1, c2, c4); out[0][3] = - Det3x3( b1, b2, b3, c1, c2, c3, d1, d2, d3); out[1][3] = Det3x3( a1, a2, a3, c1, c2, c3, d1, d2, d3); out[2][3] = - Det3x3( a1, a2, a3, b1, b2, b3, d1, d2, d3); out[3][3] = Det3x3( a1, a2, a3, b1, b2, b3, c1, c2, c3); } void Mat4InvGG(out,in) /* van Graphic Gems I, out= INV(in) */ float out[][4],in[][4]; { long i, j; float det, det4x4(); /* calculate the adjoint matrix */ Mat4Adj(out,in); det = Det4x4(out); if ( fabs( det ) < SMALL_NUMBER) { return; } /* scale the adjoint matrix to get the inverse */ for (i=0; i<4; i++) for(j=0; j<4; j++) out[i][j] = out[i][j] / det; /* de laatste factor is niet altijd 1. Hierdoor moet eigenlijk nog gedeeld worden */ } void Mat3Inv(m1,m2) float m1[][3],m2[][3]; { short a,b; float det; void Mat3Adj(); /* eerst adjoint */ Mat3Adj(m1,m2); /* dan det */ det= m2[0][0]* (m2[1][1]*m2[2][2] - m2[1][2]*m2[2][1]) -m2[1][0]* (m2[0][1]*m2[2][2] - m2[0][2]*m2[2][1]) +m2[2][0]* (m2[0][1]*m2[1][2] - m2[0][2]*m2[1][1]); if(det==0) det=1; det= 1/det; for(a=0;a<3;a++) { for(b=0;b<3;b++) { m1[a][b]*=det; } } } void Mat3Adj(m1,m) float m1[][3],m[][3]; { m1[0][0]=m[1][1]*m[2][2]-m[1][2]*m[2][1]; m1[0][1]= -m[0][1]*m[2][2]+m[0][2]*m[2][1]; m1[0][2]=m[0][1]*m[1][2]-m[0][2]*m[1][1]; m1[1][0]= -m[1][0]*m[2][2]+m[1][2]*m[2][0]; m1[1][1]=m[0][0]*m[2][2]-m[0][2]*m[2][0]; m1[1][2]= -m[0][0]*m[1][2]+m[0][2]*m[1][0]; m1[2][0]=m[1][0]*m[2][1]-m[1][1]*m[2][0]; m1[2][1]= -m[0][0]*m[2][1]+m[0][1]*m[2][0]; m1[2][2]=m[0][0]*m[1][1]-m[0][1]*m[1][0]; } void Mat4MulMat4(m1,m2,m3) float *m1,*m2,*m3; { m1[0]= m2[0]*m3[0] + m2[1]*m3[4] + m2[2]*m3[8] +m2[3]*m3[12]; m1[1]= m2[0]*m3[1] + m2[1]*m3[5] + m2[2]*m3[9] +m2[3]*m3[13]; m1[2]= m2[0]*m3[2] + m2[1]*m3[6] + m2[2]*m3[10] +m2[3]*m3[14]; m1[3]= m2[0]*m3[3] + m2[1]*m3[7] + m2[2]*m3[11] +m2[3]*m3[15]; m1+=4; m2+=4; m1[0]= m2[0]*m3[0] + m2[1]*m3[4] + m2[2]*m3[8] +m2[3]*m3[12]; m1[1]= m2[0]*m3[1] + m2[1]*m3[5] + m2[2]*m3[9] +m2[3]*m3[13]; m1[2]= m2[0]*m3[2] + m2[1]*m3[6] + m2[2]*m3[10] +m2[3]*m3[14]; m1[3]= m2[0]*m3[3] + m2[1]*m3[7] + m2[2]*m3[11] +m2[3]*m3[15]; m1+=4; m2+=4; m1[0]= m2[0]*m3[0] + m2[1]*m3[4] + m2[2]*m3[8] +m2[3]*m3[12]; m1[1]= m2[0]*m3[1] + m2[1]*m3[5] + m2[2]*m3[9] +m2[3]*m3[13]; m1[2]= m2[0]*m3[2] + m2[1]*m3[6] + m2[2]*m3[10] +m2[3]*m3[14]; m1[3]= m2[0]*m3[3] + m2[1]*m3[7] + m2[2]*m3[11] +m2[3]*m3[15]; m1+=4; m2+=4; m1[0]= m2[0]*m3[0] + m2[1]*m3[4] + m2[2]*m3[8] +m2[3]*m3[12]; m1[1]= m2[0]*m3[1] + m2[1]*m3[5] + m2[2]*m3[9] +m2[3]*m3[13]; m1[2]= m2[0]*m3[2] + m2[1]*m3[6] + m2[2]*m3[10] +m2[3]*m3[14]; m1[3]= m2[0]*m3[3] + m2[1]*m3[7] + m2[2]*m3[11] +m2[3]*m3[15]; } void Mat3MulMat3(m1,m3,m2) float *m1,*m2,*m3; { m1[0]= m2[0]*m3[0] + m2[1]*m3[3] + m2[2]*m3[6]; m1[1]= m2[0]*m3[1] + m2[1]*m3[4] + m2[2]*m3[7]; m1[2]= m2[0]*m3[2] + m2[1]*m3[5] + m2[2]*m3[8]; m1+=3; m2+=3; m1[0]= m2[0]*m3[0] + m2[1]*m3[3] + m2[2]*m3[6]; m1[1]= m2[0]*m3[1] + m2[1]*m3[4] + m2[2]*m3[7]; m1[2]= m2[0]*m3[2] + m2[1]*m3[5] + m2[2]*m3[8]; m1+=3; m2+=3; m1[0]= m2[0]*m3[0] + m2[1]*m3[3] + m2[2]*m3[6]; m1[1]= m2[0]*m3[1] + m2[1]*m3[4] + m2[2]*m3[7]; m1[2]= m2[0]*m3[2] + m2[1]*m3[5] + m2[2]*m3[8]; } void Mat4CpyMat4(m1,m2) float *m1,*m2; { bcopy(m2, m1, 64); } void Mat4SwapMat4(m1,m2) float *m1,*m2; { float t; long i; for(i=0;i<16;i++) { t= *m1; *m1= *m2; *m2= t; m1++; m2++; } } void Mat3CpyMat4(m1,m2) float m1[][3]; float m2[][4]; { m1[0][0]= m2[0][0]; m1[0][1]= m2[0][1]; m1[0][2]= m2[0][2]; m1[1][0]= m2[1][0]; m1[1][1]= m2[1][1]; m1[1][2]= m2[1][2]; m1[2][0]= m2[2][0]; m1[2][1]= m2[2][1]; m1[2][2]= m2[2][2]; } void Mat4CpyMat3(m1,m2) float m1[][4]; float m2[][3]; { m1[0][0]= m2[0][0]; m1[0][1]= m2[0][1]; m1[0][2]= m2[0][2]; m1[0][3]= 0.0; m1[1][0]= m2[1][0]; m1[1][1]= m2[1][1]; m1[1][2]= m2[1][2]; m1[1][3]= 0.0; m1[2][0]= m2[2][0]; m1[2][1]= m2[2][1]; m1[2][2]= m2[2][2]; m1[2][3]=m1[3][0]=m1[3][1]= m1[3][2]= 0.0; m1[3][3]= 1.0; } void Mat3CpyMat3(m1,m2) float *m1,*m2; { bcopy(m2, m1, 36); } void Mat3MulSerie(answ, m1, m2, m3, m4, m5, m6, m7, m8) float *answ, *m1, *m2, *m3, *m4, *m5, *m6, *m7, *m8; { float temp[3][3]; if(m1==0 || m2==0) return; Mat3MulMat3(answ, m2, m1); if(m3) { Mat3MulMat3(temp, m3, answ); if(m4) { Mat3MulMat3(answ, m4, temp); if(m5) { Mat3MulMat3(temp, m5, answ); if(m6) { Mat3MulMat3(answ, m6, temp); if(m7) { Mat3MulMat3(temp, m7, answ); if(m8) { Mat3MulMat3(answ, m8, temp); } else Mat3CpyMat3(answ, temp); } } else Mat3CpyMat3(answ, temp); } } else Mat3CpyMat3(answ, temp); } } void Mat4Clr(m) float *m; { bzero(m, 64); } void Mat3Clr(m) float *m; { bzero(m, 36); } void Mat4One(m) float m[][4]; { m[0][0]= m[1][1]= m[2][2]= m[3][3]= 1.0; m[0][1]= m[0][2]= m[0][3]= 0.0; m[1][0]= m[1][2]= m[1][3]= 0.0; m[2][0]= m[2][1]= m[2][3]= 0.0; m[3][0]= m[3][1]= m[3][2]= 0.0; } void Mat3One(m) float m[][3]; { m[0][0]= m[1][1]= m[2][2]= 1.0; m[0][1]= m[0][2]= 0.0; m[1][0]= m[1][2]= 0.0; m[2][0]= m[2][1]= 0.0; } void Mat4MulVec(mat,vec) float mat[][4]; long *vec; { long x,y; x=vec[0]; y=vec[1]; vec[0]=x*mat[0][0] + y*mat[1][0] + mat[2][0]*vec[2] + mat[3][0]; vec[1]=x*mat[0][1] + y*mat[1][1] + mat[2][1]*vec[2] + mat[3][1]; vec[2]=x*mat[0][2] + y*mat[1][2] + mat[2][2]*vec[2] + mat[3][2]; } void Mat4MulVecfl(mat,vec) float mat[][4]; float *vec; { float x,y; x=vec[0]; y=vec[1]; vec[0]=x*mat[0][0] + y*mat[1][0] + mat[2][0]*vec[2] + mat[3][0]; vec[1]=x*mat[0][1] + y*mat[1][1] + mat[2][1]*vec[2] + mat[3][1]; vec[2]=x*mat[0][2] + y*mat[1][2] + mat[2][2]*vec[2] + mat[3][2]; } void Mat4MulVec4fl(mat,vec) float mat[][4]; float *vec; { float x,y,z; x=vec[0]; y=vec[1]; z= vec[2]; vec[0]=x*mat[0][0] + y*mat[1][0] + z*mat[2][0] + mat[3][0]*vec[3]; vec[1]=x*mat[0][1] + y*mat[1][1] + z*mat[2][1] + mat[3][1]*vec[3]; vec[2]=x*mat[0][2] + y*mat[1][2] + z*mat[2][2] + mat[3][2]*vec[3]; vec[3]=x*mat[0][3] + y*mat[1][3] + z*mat[2][3] + mat[3][3]*vec[3]; } void Mat3MulVec(mat,vec) float mat[][3]; long *vec; { long x,y; x=vec[0]; y=vec[1]; vec[0]=x*mat[0][0] + y*mat[1][0] + mat[2][0]*vec[2]; vec[1]=x*mat[0][1] + y*mat[1][1] + mat[2][1]*vec[2]; vec[2]=x*mat[0][2] + y*mat[1][2] + mat[2][2]*vec[2]; } void Mat3MulVecfl(mat,vec) float mat[][3]; float *vec; { float x,y; x=vec[0]; y=vec[1]; vec[0]=x*mat[0][0] + y*mat[1][0] + mat[2][0]*vec[2]; vec[1]=x*mat[0][1] + y*mat[1][1] + mat[2][1]*vec[2]; vec[2]=x*mat[0][2] + y*mat[1][2] + mat[2][2]*vec[2]; } void Mat3TransMulVecfl(mat,vec) float mat[][3]; float *vec; { float x,y; x=vec[0]; y=vec[1]; vec[0]= x*mat[0][0] + y*mat[0][1] + mat[0][2]*vec[2]; vec[1]= x*mat[1][0] + y*mat[1][1] + mat[1][2]*vec[2]; vec[2]= x*mat[2][0] + y*mat[2][1] + mat[2][2]*vec[2]; } void Mat3MulFloat(m,f) float *m; float f; { long i; for(i=0;i<9;i++) m[i]*=f; } void Mat4MulFloat(m,f) float *m; float f; { long i; for(i=0;i<16;i++) m[i]*=f; } void Mat4MulFloat3(m,f) /* alleen de scale component */ float m[][4]; float f; { long i,j; for(i=0;i<3;i++) { for(j=0;j<3;j++) { m[i][j]*=f; } } } void VecStar(mat, vec) float mat[][3], *vec; { mat[0][0]= mat[1][1]= mat[2][2]= 0.0; mat[0][1]= -vec[2]; mat[0][2]= vec[1]; mat[1][0]= vec[2]; mat[1][2]= -vec[0]; mat[2][0]= -vec[1]; mat[2][1]= vec[0]; } short EenheidsMat(mat) float mat[][3]; { if(mat[0][0]==1.0 && mat[0][1]==0.0 && mat[0][2]==0.0) if(mat[1][0]==0.0 && mat[1][1]==1.0 && mat[1][2]==0.0) if(mat[2][0]==0.0 && mat[2][1]==0.0 && mat[2][2]==1.0) return 1; return 0; } short FloatCompare(v1,v2,limit) float *v1, *v2, limit; { if( fabs(v1[0]-v2[0])0.0) { s= sqrt( tr+1.0); q[0]= s*0.5; s= 0.5/s; q[1]= (mat[1][2]-mat[2][1])*s; q[2]= (mat[2][0]-mat[0][2])*s; q[3]= (mat[0][1]-mat[1][0])*s; } else { i= 0; if(mat[1][1]>mat[0][0]) i= 1; if(mat[2][2]>mat[i][i]) i= 2; j= nxt[i]; k= nxt[j]; s= sqrt( mat[i][i]- (mat[j][j]+mat[k][k]) + 1.0); q[i+1]= s*0.5; s= 0.5/s; q[0]= (mat[j][k]-mat[k][j])*s; q[j+1]= (mat[i][j]-mat[j][i])*s; q[k+1]= (mat[i][k]-mat[k][i])*s; } } void Mat3ToQuat(wmat, q) /* uit Sig.Proc.85 pag 253 */ float wmat[][3], *q; { void Mat3Ortho(), NormalQuat(); double tr, s; float mat[3][3]; /* voor de netheid: op kopie werken */ Mat3CpyMat3(mat, wmat); Mat3Ortho(mat); /* dit moet EN op eind NormalQuat */ tr= 0.25*(1.0+mat[0][0]+mat[1][1]+mat[2][2]); if(tr>0.00001) { s= sqrt( tr); q[0]= s; s*= 4.0; q[1]= (mat[1][2]-mat[2][1])/s; q[2]= (mat[2][0]-mat[0][2])/s; q[3]= (mat[0][1]-mat[1][0])/s; } else { q[0]= 0.0; s= -0.5*(mat[1][1]+mat[2][2]); if(s>0.00001) { s= sqrt(s); q[1]= s; q[2]= mat[0][1]/(2*s); q[3]= mat[0][2]/(2*s); } else { q[1]= 0.0; s= 0.5*(1.0-mat[2][2]); if(s>0.00001) { s= sqrt(s); q[2]= s; q[3]= mat[1][2]/(2*s); } else { q[2]= 0.0; q[3]= 1.0; } } } NormalQuat(q); } void Mat3ToQuat_is_ok(wmat, q) float wmat[][3], *q; { void Mat3Ortho(); float Normalise(); float mat[3][3], matr[3][3], matn[3][3], q1[4], q2[4], hoek, si, co, nor[3]; /* voor de netheid: op kopie werken */ Mat3CpyMat3(mat, wmat); Mat3Ortho(mat); /* roteer z-as matrix op z-as */ nor[0] = mat[2][1]; /* uitprodukt met (0,0,1) */ nor[1] = -mat[2][0]; nor[2] = 0.0; Normalise(nor); co= mat[2][2]; hoek= 0.5*facos(co); co= fcos(hoek); si= fsin(hoek); q1[0]= co; q1[1]= -nor[0]*si; /* hier negatief, waarom is een raadsel */ q1[2]= -nor[1]*si; q1[3]= -nor[2]*si; /* roteer x-as van mat terug volgens inverse q1 */ QuatToMat3(q1, matr); Mat3Inv(matn, matr); Mat3MulVecfl(matn, mat[0]); /* en zet de x-asssen gelijk */ hoek= 0.5*fatan2(mat[0][1], mat[0][0]); co= fcos(hoek); si= fsin(hoek); q2[0]= co; q2[1]= 0.0; q2[2]= 0.0; q2[3]= si; QuatMul(q, q1, q2); } void Mat3ToQuatwerkt(mat, q) float mat[][3], *q; { float v1[3], v2[3], v3[3]; /* methode: met triatoquat (anim.c) , niet gebruiken omdat arith.c zelfstandig moet bliven*/ v1[0]=v1[1]=v1[2]= 0.0; /* VECCOPY(v2, mat[0]); */ /* VECCOPY(v3, mat[1]); */ /* triatoquat(v1, v2, v3, q); */ } void Mat4ToQuat(m,q) float *m, *q; { float mat[3][3]; Mat3CpyMat4(mat, m); Mat3ToQuat(mat, q); } void NormalQuat(q) float *q; { float len; len= fsqrt(q[0]*q[0]+q[1]*q[1]+q[2]*q[2]+q[3]*q[3]); if(len!=0.0) { q[0]/= len; q[1]/= len; q[2]/= len; q[3]/= len; } } float *vectoquat(float *vec, short axis, short upflag) { static float q1[4]; float up[3], q2[4], nor[3], *fp, mat[3][3], hoek, si, co, x2, y2, z2, len1, Normalise(); /* eerst roteer naar as */ if(axis>2) { x2= vec[0] ; y2= vec[1] ; z2= vec[2]; axis-= 3; } else { x2= -vec[0] ; y2= -vec[1] ; z2= -vec[2]; } q1[0]=1.0; q1[1]=q1[2]=q1[3]= 0.0; len1= fsqrt(x2*x2+y2*y2+z2*z2); if(len1 == 0.0) return(q1); if(axis==0) { /* x-as */ nor[0]= 0.0; nor[1]= -z2; nor[2]= y2; co= x2; } else if(axis==1) { /* y-as */ nor[0]= z2; nor[1]= 0.0; nor[2]= -x2; co= y2; } else { /* z-as */ nor[0]= -y2; nor[1]= x2; nor[2]= 0.0; co= z2; } co/= len1; Normalise(nor); hoek= 0.5*facos(co); co= fcos(hoek); si= fsin(hoek); q1[0]= co; q1[1]= nor[0]*si; q1[2]= nor[1]*si; q1[3]= nor[2]*si; if(axis!=upflag) { QuatToMat3(q1, mat); fp= mat[2]; if(axis==0) { if(upflag==1) hoek= 0.5*fatan2(fp[2], fp[1]); else hoek= -0.5*fatan2(fp[1], fp[2]); } else if(axis==1) { if(upflag==0) hoek= -0.5*fatan2(fp[2], fp[0]); else hoek= 0.5*fatan2(fp[0], fp[2]); } else { if(upflag==0) hoek= 0.5*fatan2(-fp[1], -fp[0]); else hoek= -0.5*fatan2(-fp[0], -fp[1]); } co= fcos(hoek); si= fsin(hoek)/len1; q2[0]= co; q2[1]= x2*si; q2[2]= y2*si; q2[3]= z2*si; QuatMul(q1,q2,q1); } return(q1); } /* **************** VIEW / PROJEKTIE ******************************** */ i_ortho(left, right, bottom, top, near, far, matrix) float left, right, bottom, top, near, far; float matrix[][4]; { float Xdelta, Ydelta, Zdelta; Xdelta = right - left; Ydelta = top - bottom; Zdelta = far - near; if (Xdelta == 0.0 || Ydelta == 0.0 || Zdelta == 0.0) { /* fprintf(stderr,"ortho: window width, height, or depth is 0.0\n"); */ return; } Mat4One(matrix); matrix[0][0] = 2.0/Xdelta; matrix[3][0] = -(right + left)/Xdelta; matrix[1][1] = 2.0/Ydelta; matrix[3][1] = -(top + bottom)/Ydelta; matrix[2][2] = -2.0/Zdelta; /* note: negate Z */ matrix[3][2] = -(far + near)/Zdelta; } i_window(left, right, bottom, top, near, far, mat) float left, right, bottom, top, near, far; float mat[][4]; { register float Xdelta, Ydelta, Zdelta; Xdelta = right - left; Ydelta = top - bottom; Zdelta = far - near; if (Xdelta == 0.0 || Ydelta == 0.0 || Zdelta == 0.0) { return; } mat[0][0] = near * 2.0/Xdelta; mat[1][1] = near * 2.0/Ydelta; mat[2][0] = (right + left)/Xdelta; /* note: negate Z */ mat[2][1] = (top + bottom)/Ydelta; mat[2][2] = -(far + near)/Zdelta; mat[2][3] = -1.0; mat[3][2] = (-2.0 * near * far)/Zdelta; mat[0][1] = mat[0][2] = mat[0][3] = mat[1][0] = mat[1][2] = mat[1][3] = mat[3][0] = mat[3][1] = mat[3][3] = 0.0; } i_translate(Tx, Ty, Tz, mat) float Tx, Ty, Tz, mat[][4]; { mat[3][0] += (Tx*mat[0][0] + Ty*mat[1][0] + Tz*mat[2][0]); mat[3][1] += (Tx*mat[0][1] + Ty*mat[1][1] + Tz*mat[2][1]); mat[3][2] += (Tx*mat[0][2] + Ty*mat[1][2] + Tz*mat[2][2]); } i_multmatrix(icand, Vm) float icand[][4], Vm[][4]; { register int row, col; float temp[4][4]; for(row=0 ; row<4 ; row++) for(col=0 ; col<4 ; col++) temp[row][col] = icand[row][0] * Vm[0][col] + icand[row][1] * Vm[1][col] + icand[row][2] * Vm[2][col] + icand[row][3] * Vm[3][col]; Mat4CpyMat4(Vm, temp); } i_rotate(angle, axis, mat) float angle; char axis; float mat[][4]; { register int row,col; float temp[4]; float cosine, sine; for(col=0; col<4 ; col++) /* init temp to zero matrix */ temp[col] = 0; angle = angle*(3.1415926535/180.0); cosine = fcos(angle); sine = fsin(angle); switch(axis){ case 'x': case 'X': for(col=0 ; col<4 ; col++) temp[col] = cosine*mat[1][col] + sine*mat[2][col]; for(col=0 ; col<4 ; col++) { mat[2][col] = - sine*mat[1][col] + cosine*mat[2][col]; mat[1][col] = temp[col]; } break; case 'y': case 'Y': for(col=0 ; col<4 ; col++) temp[col] = cosine*mat[0][col] - sine*mat[2][col]; for(col=0 ; col<4 ; col++) { mat[2][col] = sine*mat[0][col] + cosine*mat[2][col]; mat[0][col] = temp[col]; } break; case 'z': case 'Z': for(col=0 ; col<4 ; col++) temp[col] = cosine*mat[0][col] + sine*mat[1][col]; for(col=0 ; col<4 ; col++) { mat[1][col] = - sine*mat[0][col] + cosine*mat[1][col]; mat[0][col] = temp[col]; } break; } } i_polarview(dist, azimuth, incidence, twist, Vm) float dist; float azimuth, incidence, twist, Vm[][4]; { Mat4One(Vm); i_translate(0.0, 0.0, -dist, Vm); i_rotate(-twist,'z', Vm); i_rotate(-incidence,'x', Vm); i_rotate(-azimuth,'z', Vm); } i_lookat(vx, vy, vz, px, py, pz, twist, mat) float vx, vy, vz, px, py, pz; float twist, mat[][4]; { register float sine, cosine, hyp, hyp1, dx, dy, dz; float mat1[4][4]; Mat4One(mat); Mat4One(mat1); i_rotate(-twist,'z', mat); dx = px - vx; dy = py - vy; dz = pz - vz; hyp = dx * dx + dz * dz; /* hyp squared */ hyp1 = sqrt(dy*dy + hyp); hyp = sqrt(hyp); /* the real hyp */ if (hyp1 != 0.0) { /* rotate X */ sine = -dy / hyp1; cosine = hyp /hyp1; } else { sine = 0; cosine = 1.0; } mat1[1][1] = cosine; mat1[1][2] = sine; mat1[2][1] = -sine; mat1[2][2] = cosine; i_multmatrix(mat1, mat); mat1[1][1] = mat1[2][2] = 1.0; /* be careful here to reinit */ mat1[1][2] = mat1[2][1] = 0.0; /* those modified by the last */ /* paragraph */ if (hyp != 0.0) { /* rotate Y */ sine = dx / hyp; cosine = -dz / hyp; } else { sine = 0; cosine = 1.0; } mat1[0][0] = cosine; mat1[0][2] = -sine; mat1[2][0] = sine; mat1[2][2] = cosine; i_multmatrix(mat1, mat); i_translate(-vx,-vy,-vz, mat); /* translate viewpoint to origin */ } /* ************************************************ */ void EulToMat3(eul, mat) float *eul, mat[][3]; { float ci, cj, ch, si, sj, sh, cc, cs, sc, ss; ci = fcos(eul[0]); cj = fcos(eul[1]); ch = fcos(eul[2]); si = fsin(eul[0]); sj = fsin(eul[1]); sh = fsin(eul[2]); cc = ci*ch; cs = ci*sh; sc = si*ch; ss = si*sh; mat[0][0] = cj*ch; mat[1][0] = sj*sc-cs; mat[2][0] = sj*cc+ss; mat[0][1] = cj*sh; mat[1][1] = sj*ss+cc; mat[2][1] = sj*cs-sc; mat[0][2] = -sj; mat[1][2] = cj*si; mat[2][2] = cj*ci; } void EulToMat4(eul, mat) float *eul, mat[][4]; { float ci, cj, ch, si, sj, sh, cc, cs, sc, ss; ci = fcos(eul[0]); cj = fcos(eul[1]); ch = fcos(eul[2]); si = fsin(eul[0]); sj = fsin(eul[1]); sh = fsin(eul[2]); cc = ci*ch; cs = ci*sh; sc = si*ch; ss = si*sh; mat[0][0] = cj*ch; mat[1][0] = sj*sc-cs; mat[2][0] = sj*cc+ss; mat[0][1] = cj*sh; mat[1][1] = sj*ss+cc; mat[2][1] = sj*cs-sc; mat[0][2] = -sj; mat[1][2] = cj*si; mat[2][2] = cj*ci; mat[3][0]= mat[3][1]= mat[3][2]= mat[0][3]= mat[1][3]= mat[2][3]= 0.0; mat[3][3]= 1.0; } void NormalWrd(n) /* is geen WORD !! */ float n[]; { register float d; d=n[0]*n[0]+n[1]*n[1]+n[2]*n[2]; d=1/fsqrt(d); n[0]*=d; n[1]*=d; n[2]*=d; } float Normalise(n) float *n; { register float d; d= n[0]*n[0]+n[1]*n[1]+n[2]*n[2]; if(d>0.0) { d= fsqrt(d); n[0]/=d; n[1]/=d; n[2]/=d; } else { n[0]=n[1]=n[2]= 0.0; } return d; } void Mat3Ortho(mat) float mat[][3]; { Normalise(mat[0]); Normalise(mat[1]); Normalise(mat[2]); } void Mat4Ortho(mat) float mat[][4]; { float len; len= Normalise(mat[0]); if(len!=0.0) mat[0][3]/= len; len= Normalise(mat[1]); if(len!=0.0) mat[1][3]/= len; len= Normalise(mat[2]); if(len!=0.0) mat[2][3]/= len; } void VecCopyf(v1, v2) register float *v1,*v2; { v1[0]= v2[0]; v1[1]= v2[1]; v1[2]= v2[2]; } long VecLen(v1,v2) long *v1,*v2; { float x,y,z; x=v1[0]-v2[0]; y=v1[1]-v2[1]; z=v1[2]-v2[2]; return ffloor(fsqrt(x*x+y*y+z*z)); } float VecLenf(v1,v2) float *v1,*v2; { float x,y,z; x=v1[0]-v2[0]; y=v1[1]-v2[1]; z=v1[2]-v2[2]; return fsqrt(x*x+y*y+z*z); } void VecAddf(v, v1,v2) register float *v, *v1,*v2; { v[0]= v1[0]+ v2[0]; v[1]= v1[1]+ v2[1]; v[2]= v1[2]+ v2[2]; } void VecSubf(v, v1,v2) register float *v, *v1,*v2; { v[0]= v1[0]- v2[0]; v[1]= v1[1]- v2[1]; v[2]= v1[2]- v2[2]; } void VecMidf(v, v1,v2) register float *v, *v1,*v2; { v[0]= (v1[0]+ v2[0])/2.0; v[1]= (v1[1]+ v2[1])/2.0; v[2]= (v1[2]+ v2[2])/2.0; } void VecMulf(float *v1, float f) { v1[0]*= f; v1[1]*= f; v1[2]*= f; } void Crossf(c,a,b) float *c,*a,*b; { c[0] = a[1] * b[2] - a[2] * b[1]; c[1] = a[2] * b[0] - a[0] * b[2]; c[2] = a[0] * b[1] - a[1] * b[0]; } float Inpf(v1,v2) register float *v1,*v2; { return v1[0]*v2[0]+v1[1]*v2[1]+v1[2]*v2[2]; } void CalcNormShort(v1,v2,v3,n) /* is ook uitprodukt */ short *v1,*v2,*v3; float *n; { float n1[3],n2[3]; n1[0]= v1[0]-v2[0]; n2[0]= v2[0]-v3[0]; n1[1]= v1[1]-v2[1]; n2[1]= v2[1]-v3[1]; n1[2]= v1[2]-v2[2]; n2[2]= v2[2]-v3[2]; n[0]= n1[1]*n2[2]-n1[2]*n2[1]; n[1]= n1[2]*n2[0]-n1[0]*n2[2]; n[2]= n1[0]*n2[1]-n1[1]*n2[0]; Normalise(n); } void CalcNormLong(v1,v2,v3,n) long *v1,*v2,*v3; float *n; { float n1[3],n2[3]; n1[0]= v1[0]-v2[0]; n2[0]= v2[0]-v3[0]; n1[1]= v1[1]-v2[1]; n2[1]= v2[1]-v3[1]; n1[2]= v1[2]-v2[2]; n2[2]= v2[2]-v3[2]; n[0]= n1[1]*n2[2]-n1[2]*n2[1]; n[1]= n1[2]*n2[0]-n1[0]*n2[2]; n[2]= n1[0]*n2[1]-n1[1]*n2[0]; Normalise(n); } float CalcNormFloat(v1,v2,v3,n) float *v1,*v2,*v3; float *n; { float n1[3],n2[3]; n1[0]= v1[0]-v2[0]; n2[0]= v2[0]-v3[0]; n1[1]= v1[1]-v2[1]; n2[1]= v2[1]-v3[1]; n1[2]= v1[2]-v2[2]; n2[2]= v2[2]-v3[2]; n[0]= n1[1]*n2[2]-n1[2]*n2[1]; n[1]= n1[2]*n2[0]-n1[0]*n2[2]; n[2]= n1[0]*n2[1]-n1[1]*n2[0]; return Normalise(n); } void CalcCent3f(cent, v1, v2, v3) float *cent, *v1, *v2, *v3; { cent[0]= 0.33333*(v1[0]+v2[0]+v3[0]); cent[1]= 0.33333*(v1[1]+v2[1]+v3[1]); cent[2]= 0.33333*(v1[2]+v2[2]+v3[2]); } void CalcCent4f(cent, v1, v2, v3, v4) float *cent, *v1, *v2, *v3, *v4; { cent[0]= 0.25*(v1[0]+v2[0]+v3[0]+v4[0]); cent[1]= 0.25*(v1[1]+v2[1]+v3[1]+v4[1]); cent[2]= 0.25*(v1[2]+v2[2]+v3[2]+v4[2]); } float Sqrt3f(f) float f; { if(f==0.0) return 0; if(f<0) return -fexp(flog(-f)/3); else return fexp(flog(f)/3); } double Sqrt3d(d) double d; { if(d==0.0) return 0; if(d<0) return -exp(log(-d)/3); else return exp(log(d)/3); } /* afstand v1 tot lijn v2-v3 */ float DistVL2Dfl_oud(v1,v2,v3) /* met formule van Hesse :GEEN LIJNSTUK! */ float *v1,*v2,*v3; { float a[2],fi,deler; a[0]= v2[1]-v3[1]; a[1]= v3[0]-v2[0]; fi= a[0]*v2[0]+a[1]*v2[1]; deler= fsqrt(a[0]*a[0]+a[1]*a[1]); if(deler== 0.0) return 0; return fabs(v1[0]*a[0]+v1[1]*a[1] -fi)/deler; } /* afstand v1 tot lijn v2-v3 */ float DistVL2Dfl(v1,v2,v3) /* met formule van Hesse :GEEN LIJNSTUK! */ float *v1,*v2,*v3; { float a[2],deler; a[0]= v2[1]-v3[1]; a[1]= v3[0]-v2[0]; deler= fsqrt(a[0]*a[0]+a[1]*a[1]); if(deler== 0.0) return 0; return fabs((v1[0]-v2[0])*a[0]+(v1[1]-v2[1])*a[1])/deler; } float PdistVL2Dfl(v1, v2, v3) /* PointDist: WEL LIJNSTUK */ float *v1,*v2,*v3; { float labda, rc[2], pt[2], len; rc[0]= v3[0]-v2[0]; rc[1]= v3[1]-v2[1]; len= rc[0]*rc[0]+ rc[1]*rc[1]; if(len==0.0) { rc[0]= v1[0]-v2[0]; rc[1]= v1[1]-v2[1]; return fsqrt(rc[0]*rc[0]+ rc[1]*rc[1]); } labda= ( rc[0]*(v1[0]-v2[0]) + rc[1]*(v1[1]-v2[1]) )/len; if(labda<=0.0) { pt[0]= v2[0]; pt[1]= v2[1]; } else if(labda>=1.0) { pt[0]= v3[0]; pt[1]= v3[1]; } else { pt[0]= labda*rc[0]+v2[0]; pt[1]= labda*rc[1]+v2[1]; } rc[0]= pt[0]-v1[0]; rc[1]= pt[1]-v1[1]; return fsqrt(rc[0]*rc[0]+ rc[1]*rc[1]); } float AreaF2Dfl(v1,v2,v3) float *v1,*v2,*v3; { return .5*fabs( (v1[0]-v2[0])*(v2[1]-v3[1]) + (v1[1]-v2[1])*(v3[0]-v2[0]) ); } float AreaQ3Dfl(v1, v2, v3, v4) /* only convex Quadrilaterals */ float *v1, *v2, *v3, *v4; { float len, vec1[3], vec2[3], n[3]; VecSubf(vec1, v2, v1); VecSubf(vec2, v4, v1); Crossf(n, vec1, vec2); len= Normalise(n); VecSubf(vec1, v4, v3); VecSubf(vec2, v2, v3); Crossf(n, vec1, vec2); len+= Normalise(n); return (len/2.0); } float AreaT3Dfl(v1, v2, v3) /* Triangles */ float *v1, *v2, *v3; { float len, vec1[3], vec2[3], n[3]; VecSubf(vec1, v3, v2); VecSubf(vec2, v1, v2); Crossf(n, vec1, vec2); len= Normalise(n); return (len/2.0); } #define MAX2(x,y) ( (x)>(y) ? (x) : (y) ) #define MAX3(x,y,z) MAX2( MAX2((x),(y)) , (z) ) float AreaPoly3Dfl(nr, verts, normal) long nr; float *verts, *normal; { float x, y, z, area, max, *cur, *prev; long a, px=0, py=1; /* first: find dominant axis: 0==X, 1==Y, 2==Z */ x= fabs(normal[0]); y= fabs(normal[1]); z= fabs(normal[2]); max = MAX3(x, y, z); if(max==y) py=2; else if(max==x) { px=1; py= 2; } /* The Trapezium Area Rule */ prev= verts+3*(nr-1); cur= verts; area= 0; for(a=0; avec[0]) min[0]= vec[0]; if(min[1]>vec[1]) min[1]= vec[1]; if(min[2]>vec[2]) min[2]= vec[2]; if(max[0]255) c[0]=255; else if(c[0]<0) c[0]=0; if(c[1]>255) c[1]=255; else if(c[1]<0) c[1]=0; if(c[2]>255) c[2]=255; else if(c[2]<0) c[2]=0; } float Spec(inp,spec) register float inp; register short spec; { register float b1; if(inp>=1.0) return 1.0; b1=inp*inp; if((spec & 1)==0) inp=1; if(spec & 2) inp*=b1; b1*=b1; if(spec & 4) inp*=b1; b1*=b1; if(spec & 8) inp*=b1; b1*=b1; if(spec & 16) inp*=b1; b1*=b1; if(spec & 32) inp*=b1; if(spec & 64) { b1*=b1; inp*=b1; } return inp; } /* ************ NIEUWE QSORT ********************** */ int (*vergfunc)(); int vergsize, temppivot[40]; int partitie(poin, n, pivot) int *poin; int n; int *pivot; { int i=0, j= n-1, k, t1; int *next; k= 0; while(k e2[0])? 1 : ( (e1[0] < e2[0]) ? -1 : ( (e1[1] > e2[1]) ? 1 : ( (e1[1] < e2[1]) ? -1 : 0 ) ) ) ) int partitie2(poin, n, pivot) int *poin; int n; int *pivot; { int i=0, j= n-1, k, t1; int *next; temppivot[0]= pivot[0]; temppivot[1]= pivot[1]; next= poin+ 2*(n-1); while(i<=j) { while( VERGLONG2(poin, temppivot)== -1 ) { i++; poin+= 2; } while( VERGLONG2(next, temppivot)!= -1 ) { j--; next-= 2; } if(i