/* ====================================================================== libvec.c Eric Haines et al. Ernie Wright 01 Jun 05 A library of vector, matrix, and random number operations. Modified for use in a LightWave Modeler plug-in. Routines that print or exit have been modified or removed. ====================================================================== */ /* Modified: 1 October 1992 * Alexander R. Enzmann * Object generation routines split off to keep file size down * * Modified: 17 Jan 1993 * Eduard [esp] Schwan * Removed unused local variables, added include for * exit() call * * Modified: 1 November 1994 * Alexander R. Enzmann * Added routines to invert matrices, transform normals, and * determine rotate/scale/translate from a transform matrix. * Modified computation of perspective view matrix to be a * little cleaner. * * Modified: 4 December 1996 * Eric Haines * Lint cleanup. */ #include #include "libvec.h" /* * Normalize the vector (X,Y,Z) so that X*X + Y*Y + Z*Z = 1. * * The normalization divisor is returned. If the divisor is zero, no * normalization occurs. * */ #ifdef ANSI_FN_DEF double lib_normalize_vector(COORD3 cvec) #else double lib_normalize_vector(cvec) COORD3 cvec; #endif { double divisor; divisor = sqrt( (double)DOT_PRODUCT(cvec, cvec) ); if (divisor > 0.0) { cvec[X] /= divisor; cvec[Y] /= divisor; cvec[Z] /= divisor; } return divisor; } /* Find two vectors, basis1 and basis2, that form an orthogonal basis with the vector axis. It is assumed that axis is non-zero. */ #ifdef ANSI_FN_DEF void lib_create_orthogonal_vectors(COORD3 axis, COORD3 basis1, COORD3 basis2) #else void lib_create_orthogonal_vectors(axis, basis1, basis2) COORD3 axis, basis1, basis2; #endif { if (fabs(axis[Z]) < EPSILON) { SET_COORD3(basis1, 0.0, 0.0, 1.0); } else if (fabs(axis[Y]) < EPSILON) { SET_COORD3(basis1, 0.0, 1.0, 0.0); } else { SET_COORD3(basis1, 1.0, 0.0, 0.0); } CROSS(basis2, axis, basis1); (void)lib_normalize_vector(basis2); CROSS(basis1, basis2, axis); (void)lib_normalize_vector(basis1); } /* * Set all matrix elements to zero. */ #ifdef ANSI_FN_DEF void lib_zero_matrix(MATRIX mx) #else void lib_zero_matrix(mx) MATRIX mx; #endif { int i, j; for (i=0;i<4;++i) for (j=0;j<4;++j) mx[i][j] = 0.0; } /* * Create identity matrix. */ #ifdef ANSI_FN_DEF void lib_create_identity_matrix(MATRIX mx) #else void lib_create_identity_matrix(mx) MATRIX mx; #endif { int i; lib_zero_matrix(mx); for (i=0;i<4;++i) mx[i][i] = 1.0; } /* * Copy one matrix to another */ #ifdef ANSI_FN_DEF void lib_copy_matrix(MATRIX mres, MATRIX mx) #else void lib_copy_matrix(mres, mx) MATRIX mres, mx; #endif { int i, j; for (i=0;i<4;i++) for (j=0;j<4;j++) mres[i][j] = mx[i][j]; } /* * Create a rotation matrix along the given axis by the given angle in radians. */ #ifdef ANSI_FN_DEF void lib_create_rotate_matrix(MATRIX mx, int axis, double angle) #else void lib_create_rotate_matrix(mx, axis, angle) MATRIX mx; int axis; double angle; #endif { double cosine, sine; lib_zero_matrix(mx); cosine = cos((double)angle); sine = sin((double)angle); switch (axis) { case X_AXIS: mx[0][0] = 1.0; mx[1][1] = cosine; mx[2][2] = cosine; mx[1][2] = sine; mx[2][1] = -sine; break ; case Y_AXIS: mx[1][1] = 1.0; mx[0][0] = cosine; mx[2][2] = cosine; mx[2][0] = sine; mx[0][2] = -sine; break; case Z_AXIS: mx[2][2] = 1.0; mx[0][0] = cosine; mx[1][1] = cosine; mx[0][1] = sine; mx[1][0] = -sine; break; default: break; } mx[3][3] = 1.0; } /* Turn a triplet of rotations about x, y, and z (in that order) into an equivalent rotation around a single axis. Rotation of the angle t about the axis (X,Y,Z) is given by: | X^2+(1-X^2) Cos(t), XY(1-Cos(t)) + Z Sin(t), XZ(1-Cos(t))-Y Sin(t) | M = | XY(1-Cos(t))-Z Sin(t), Y^2+(1-Y^2) Cos(t), YZ(1-Cos(t))+X Sin(t) | | XZ(1-Cos(t))+Y Sin(t), YZ(1-Cos(t))-X Sin(t), Z^2+(1-Z^2) Cos(t) | Rotation about the three axes (angles a1, a2, a3) can be represented as the product of the individual rotation matrices: | 1 0 0 | | Cos(a2) 0 -Sin(a2) | | Cos(a3) Sin(a3) 0 | | 0 Cos(a1) Sin(a1) |*| 0 1 0 |*| -Sin(a3) Cos(a3) 0 | | 0 -Sin(a1) Cos(a1) | | Sin(a2) 0 Cos(a2) | | 0 0 1 | Mx My Mz We now want to solve for X, Y, Z, and t given 9 equations in 4 unknowns. Using the diagonal elements of the two matrices, we get: X^2+(1-X^2) Cos(t) = M[0][0] Y^2+(1-Y^2) Cos(t) = M[1][1] Z^2+(1-Z^2) Cos(t) = M[2][2] Adding the three equations, we get: X^2 + Y^2 + Z^2 - (M[0][0] + M[1][1] + M[2][2]) = - (3 - X^2 - Y^2 - Z^2) Cos(t) Since (X^2 + Y^2 + Z^2) = 1, we can rewrite as: Cos(t) = (1 - (M[0][0] + M[1][1] + M[2][2])) / 2 Solving for t, we get: t = Acos(((M[0][0] + M[1][1] + M[2][2]) - 1) / 2) We can substitute t into the equations for X^2, Y^2, and Z^2 above to get the values for X, Y, and Z. To find the proper signs we note that: 2 X Sin(t) = M[1][2] - M[2][1] 2 Y Sin(t) = M[2][0] - M[0][2] 2 Z Sin(t) = M[0][1] - M[1][0] */ #ifdef ANSI_FN_DEF void lib_calc_rotation_axis(COORD3 rot_angles, COORD4 axis) #else void lib_calc_rotation_axis(rot_angles, axis) COORD3 rot_angles; COORD4 axis; #endif { COORD3 axis1, axis2; MATRIX mx, mx1, mx2; double cost, cost1, sint; double s1, s2, s3; int i; /* See if we are only rotating about a single axis */ if (fabs(rot_angles[0]) < EPSILON) { if (fabs(rot_angles[1]) < EPSILON) { SET_COORD4(axis, 0.0, 0.0, 1.0, rot_angles[2]); return; } else if (fabs(rot_angles[2]) < EPSILON) { SET_COORD4(axis, 0.0, 1.0, 0.0, rot_angles[1]); return; } } else if ((fabs(rot_angles[1]) < EPSILON) && (fabs(rot_angles[2]) < EPSILON)) { SET_COORD4(axis, 1.0, 0.0, 0.0, rot_angles[0]); return; } /* Make the rotation matrix */ SET_COORD3(axis1, 1.0, 0.0, 0.0); lib_create_axis_rotate_matrix(mx, axis1, rot_angles[0]); SET_COORD3(axis2, 0.0, 1.0, 0.0); lib_create_axis_rotate_matrix(mx2, axis2, rot_angles[1]); lib_matrix_multiply(mx1, mx, mx2); SET_COORD3(axis2, 0.0, 0.0, 1.0); lib_create_axis_rotate_matrix(mx2, axis2, rot_angles[2]); lib_matrix_multiply(mx, mx1, mx2); cost = ((mx[0][0] + mx[1][1] + mx[2][2]) - 1.0) / 2.0; if (cost < -1.0) cost = -1.0; else if (cost > 1.0 - EPSILON) { /* Bad angle - this would cause a crash */ SET_COORD4(axis, 1.0, 0.0, 0.0, 0.0); return; } cost1 = 1.0 - cost; SET_COORD4(axis, sqrt((mx[0][0] - cost) / cost1), sqrt((mx[1][1] - cost) / cost1), sqrt((mx[2][2] - cost) / cost1), acos(cost)); sint = 2.0 * sqrt(1.0 - cost * cost); /* This is actually 2 Sin(t) */ /* Determine the proper signs */ for (i=0;i<8;i++) { s1 = (i & 1 ? -1.0 : 1.0); s2 = (i & 2 ? -1.0 : 1.0); s3 = (i & 4 ? -1.0 : 1.0); if (fabs(s1*axis[0]*sint-mx[1][2]+mx[2][1]) < EPSILON2 && fabs(s2*axis[1]*sint-mx[2][0]+mx[0][2]) < EPSILON2 && fabs(s3*axis[2]*sint-mx[0][1]+mx[1][0]) < EPSILON2) { /* We found the right combination of signs */ axis[0] *= s1; axis[1] *= s2; axis[2] *= s3; return; } } } /* * Create a rotation matrix along the given axis by the given angle in radians. * The axis is a set of direction cosines. */ #ifdef ANSI_FN_DEF void lib_create_axis_rotate_matrix(MATRIX mx, COORD3 axis, double angle) #else void lib_create_axis_rotate_matrix(mx, axis, angle) MATRIX mx; COORD3 axis; double angle; #endif { double cosine, one_minus_cosine, sine; cosine = cos((double)angle); sine = sin((double)angle); one_minus_cosine = 1.0 - cosine; mx[0][0] = SQR(axis[X]) + (1.0 - SQR(axis[X])) * cosine; mx[0][1] = axis[X] * axis[Y] * one_minus_cosine + axis[Z] * sine; mx[0][2] = axis[X] * axis[Z] * one_minus_cosine - axis[Y] * sine; mx[0][3] = 0.0; mx[1][0] = axis[X] * axis[Y] * one_minus_cosine - axis[Z] * sine; mx[1][1] = SQR(axis[Y]) + (1.0 - SQR(axis[Y])) * cosine; mx[1][2] = axis[Y] * axis[Z] * one_minus_cosine + axis[X] * sine; mx[1][3] = 0.0; mx[2][0] = axis[X] * axis[Z] * one_minus_cosine + axis[Y] * sine; mx[2][1] = axis[Y] * axis[Z] * one_minus_cosine - axis[X] * sine; mx[2][2] = SQR(axis[Z]) + (1.0 - SQR(axis[Z])) * cosine; mx[2][3] = 0.0; mx[3][0] = 0.0; mx[3][1] = 0.0; mx[3][2] = 0.0; mx[3][3] = 1.0; } /* Create translation matrix */ #ifdef ANSI_FN_DEF void lib_create_translate_matrix(MATRIX mx, COORD3 vec) #else void lib_create_translate_matrix(mx, vec) MATRIX mx; COORD3 vec; #endif { lib_create_identity_matrix(mx); mx[3][0] = vec[X]; mx[3][1] = vec[Y]; mx[3][2] = vec[Z]; } /* Create scaling matrix */ #ifdef ANSI_FN_DEF void lib_create_scale_matrix(MATRIX mx, COORD3 vec) #else void lib_create_scale_matrix(mx, vec) MATRIX mx; COORD3 vec; #endif { lib_zero_matrix(mx); mx[0][0] = vec[X]; mx[1][1] = vec[Y]; mx[2][2] = vec[Z]; mx[3][3] = 1.0; } /* Given a point and a direction, find the transform that brings a point in a canonical coordinate system into a coordinate system defined by that position and direction. Both the forward and inverse transform matrices are calculated. */ #ifdef ANSI_FN_DEF void lib_create_canonical_matrix(MATRIX trans, MATRIX itrans, COORD3 origin, COORD3 up) #else void lib_create_canonical_matrix(trans, itrans, origin, up) MATRIX trans, itrans; COORD3 origin; COORD3 up; #endif { MATRIX trans1, trans2; COORD3 tmpv; double ang; /* Translate "origin" to <0, 0, 0> */ SET_COORD3(tmpv, -origin[X], -origin[Y], -origin[Z]); lib_create_translate_matrix(trans1, tmpv); /* Determine the axis to rotate about */ if (fabs(up[Z]) == 1.0) { SET_COORD3(tmpv, 1.0, 0.0, 0.0); } else { SET_COORD3(tmpv, -up[Y], up[X], 0.0); } lib_normalize_vector(tmpv); ang = acos(up[Z]); /* Calculate the forward matrix */ lib_create_axis_rotate_matrix(trans2, tmpv, -ang); lib_matrix_multiply(trans, trans1, trans2); /* Calculate the inverse transform */ lib_create_axis_rotate_matrix(trans1, tmpv, ang); lib_create_translate_matrix(trans2, origin); lib_matrix_multiply(itrans, trans1, trans2); } /* Find the transformation that takes the current eye position and orientation and puts it at {0, 0, 0} with the up vector aligned with {0, 1, 0} */ #ifdef ANSI_FN_DEF void lib_create_view_matrix(MATRIX trans, COORD3 from, COORD3 at, COORD3 up, int xres, int yres, double angle, double aspect) #else void lib_create_view_matrix(trans, from, at, up, xres, yres, angle, aspect) MATRIX trans; COORD3 from, at, up; int xres, yres; double angle, aspect; #endif { double xscale, yscale, view_dist; COORD3 right, up_dir; COORD3 Va; MATRIX Tv, Tt, Tx; /* Change the view angle into radians */ angle = PI * angle / 180.0; /* Set the hither clipping plane */ view_dist = 0.001; /* Translate point of interest to the origin */ SET_COORD3(Va, -from[X], -from[Y], -from[Z]); lib_create_translate_matrix(Tv, Va); /* Move the eye by the same amount */ SUB3_COORD3(Va, at, from); /* Make sure this is a valid sort of setup */ // if (Va[X] == 0.0 && Va[Y] == 0.0 && Va[Z] == 0.0) { // fprintf(stderr, "Degenerate perspective transformation\n"); // exit(1); // } /* Get the up vector to be perpendicular to the view vector */ lib_normalize_vector(Va); COPY_COORD3(up_dir, up); CROSS(right, up_dir, Va); lib_normalize_vector(right); CROSS(up_dir, Va, right); lib_normalize_vector(up_dir); /* Create the orthogonal view transformation */ Tt[0][0] = right[0]; Tt[1][0] = right[1]; Tt[2][0] = right[2]; Tt[3][0] = 0; Tt[0][1] = up_dir[0]; Tt[1][1] = up_dir[1]; Tt[2][1] = up_dir[2]; Tt[3][1] = 0; Tt[0][2] = Va[0]; Tt[1][2] = Va[1]; Tt[2][2] = Va[2]; Tt[3][2] = 0; Tt[0][3] = 0; Tt[1][3] = 0; Tt[2][3] = 0; Tt[3][3] = 1; lib_matrix_multiply(Tx, Tv, Tt); lib_copy_matrix(Tv, Tx); /* Now add in the perspective transformation */ lib_create_identity_matrix(Tt); Tt[2][2] = 1.0 / (1.0 + view_dist); Tt[3][2] = view_dist / (1.0 + view_dist); Tt[2][3] = 1.0; Tt[3][3] = 0.0; lib_matrix_multiply(Tx, Tv, Tt); lib_copy_matrix(Tv, Tx); /* Determine how much to scale things by */ yscale = (double)yres / (2.0 * tan(angle/2.0)); xscale = yscale * (double)xres / ((double)yres * aspect); SET_COORD3(Va, -xscale, -yscale, 1.0); lib_create_scale_matrix(Tt, Va); lib_matrix_multiply(Tx, Tv, Tt); lib_copy_matrix(Tv, Tx); /* Translate the points so that <0,0> is at the center of the screen */ SET_COORD3(Va, xres/2, yres/2, 0.0); lib_create_translate_matrix(Tt, Va); /* We now have the final transformation matrix */ lib_matrix_multiply(trans, Tv, Tt); } /* Figure out the vector and angle that takes a viewpoint given in from/at/up to the orthogonal basis x/y/-z. This code assumes that the combination of from/at/up is not degenerate - that from is not colocated with at and that the up direction is not the same as the direction between from and at. */ /* This is how we get from a from/at/up description to an axis/angle description: Given a view description as two points (eye location, point of interest) and a vector (nominal up direction), we can create an orthogonal basis with the following form (it takes a couple of cross products + vector normalization): |R0 R1 R2| |U0 U1 U2| |V0 V1 V2| Where R is the "right" vector, U is the "up" vector, and "V" is the direction of view. The default view basis in VRML 2.0 is then given by the following matrix: | 1 0 0| | 0 1 0| | 0 0 -1| So what we want to do is create a transformation that takes the first basis into the second basis, i.e. |R0 R1 R2| | x0 x1 x2 | | 1 0 0| |U0 U1 U2| * | y0 y1 y2 | = | 0 1 0| |V0 V1 V2| | z0 z1 z2 | | 0 0 -1| M1 * M2 = M3 What is now necessary is to determine the values in M2. Since the form of M2 is a rotation by an angle t about the unit vector (X,Y,Z), we know it has the form: | X^2+(1-X^2) Cos(t), XY(1-Cos(t)) + Z Sin(t), XZ(1-Cos(t))-Y Sin(t) | | XY(1-Cos(t))-Z Sin(t), Y^2+(1-Y^2) Cos(t), YZ(1-Cos(t))+X Sin(t) | | XZ(1-Cos(t))+Y Sin(t), YZ(1-Cos(t))-X Sin(t), Z^2+(1-Z^2) Cos(t) | Now solve for t, X, Y, and Z. By looking at the diagonal elements of M2 and the diagonal elements of transpose(M1) * M3, we get the following three equations: R0 = X^2 + (1 - X^2) Cos(t) U1 = Y^2 + (1 - Y^2) Cos(t) V2 = -(Z^2 + (1 - Z^2) Cos(t)) Adding them together we get: R0 + U1 - V2 = X^2 + Y^2 + Z^2 + (3 - X^2 - Y^2 - Z^2) Cos(t) Since X^2 + Y^2 + Z^2 = 1, we can rewrite as: R0 + U1 - V2 = 1 + 2 Cos(t) So we can now solve for t: Cos(t) = (R0 + U1 - V2 - 1) / 2, or t = Arccos((R0 + U1 - V2 - 1) / 2) Substituting back into the three equations above, we can solve for X, Y, and Z: X^2 = (R0 - Cos(t)) / (1 - Cos(t)) Y^2 = (U1 - Cos(t)) / (1 - Cos(t)) Z^2 = (V2 - Cos(t)) / (1 - Cos(t)) Note that this equation is degenerate when Cos(t) = 1. Since this implies that the angle of rotation is zero (e.g., the original view basis is already the one we want), we can choose any axis we want for the rotation. Note also that if the original view basis is left handed then there is no pure rotation matrix that takes it to the VRML one, and the value of Cos(t) can be out of the range -1 to 1. All that's left is to find the proper signs for X, Y, Z, and t. Again turning to the elements of the rotation matrix, we can deduce that: V1 + U2 = -2 X Sin(t) R2 + V0 = 2 Y Sin(t) U0 - R1 = 2 Z Sin(t) By checking these values we can make sure that we have the proper sign for the angle and each component of the axis of rotation. */ void lib_calc_view_vector(from, at, up, axis) COORD3 from, at, up; COORD4 axis; { COORD3 U, R, V; double cost, sint; int i; double s1, s2, s3; /* Create an orthogonal view basis from the input from/at/up values */ SUB3_COORD3(V, at, from); lib_normalize_vector(V); CROSS(R, V, up); lib_normalize_vector(R); CROSS(U, R, V); lib_normalize_vector(U); /* Let's solve for the axis and angle */ cost = 0.5 * (R[0] + U[1] - V[2] - 1.0); if (fabs(cost - 1.0) < EPSILON2) { /* Everything is already aligned */ axis[0] = 1.0; axis[1] = 0.0; axis[2] = 0.0; axis[3] = 0.0; return; } else { axis[0] = sqrt((R[0] - cost) / (1.0 - cost)); axis[1] = sqrt((U[1] - cost) / (1.0 - cost)); axis[2] = sqrt((-V[2] - cost) / (1.0 - cost)); axis[3] = acos(cost); } sint = 2.0 * sqrt(1.0 - cost * cost); /* Determine the proper signs the hard way - try all combinations. This could be rewritten as one complex, nested, if statement but I didn't feel like writing it that way. */ for (i=0;i<8;i++) { s1 = (i & 1 ? -1.0 : 1.0); s2 = (i & 2 ? -1.0 : 1.0); s3 = (i & 4 ? -1.0 : 1.0); if (fabs(V[1]+U[2]-s1*axis[0]*sint) < EPSILON2 && fabs(R[2]+V[0]+s2*axis[1]*sint) < EPSILON2 && fabs(U[0]-R[1]+s3*axis[2]*sint) < EPSILON2) { /* We found the right combination of signs */ axis[0] *= s1; axis[1] *= s2; axis[2] *= s3; return; } } /* This is bad, shouldn't ever get here */ } /* * Multiply a vector by a matrix. */ #ifdef ANSI_FN_DEF void lib_transform_vector(COORD3 vres, COORD3 vec, MATRIX mx) #else void lib_transform_vector(vres, vec, mx) COORD3 vres, vec; MATRIX mx; #endif { COORD3 vtemp; vtemp[X] = vec[X]*mx[0][0] + vec[Y]*mx[1][0] + vec[Z]*mx[2][0]; vtemp[Y] = vec[X]*mx[0][1] + vec[Y]*mx[1][1] + vec[Z]*mx[2][1]; vtemp[Z] = vec[X]*mx[0][2] + vec[Y]*mx[1][2] + vec[Z]*mx[2][2]; COPY_COORD3(vres, vtemp); } /* * Multiply a normal by a matrix (i.e. multiply by transpose). */ #ifdef ANSI_FN_DEF void lib_transform_normal(COORD3 vres, COORD3 vec, MATRIX mx) #else void lib_transform_normal(vres, vec, mx) COORD3 vres, vec; MATRIX mx; #endif { COORD3 vtemp; vtemp[X] = vec[X]*mx[0][0] + vec[Y]*mx[0][1] + vec[Z]*mx[0][2]; vtemp[Y] = vec[X]*mx[1][0] + vec[Y]*mx[1][1] + vec[Z]*mx[1][2]; vtemp[Z] = vec[X]*mx[2][0] + vec[Y]*mx[2][1] + vec[Z]*mx[2][2]; COPY_COORD3(vres, vtemp); } /* * Multiply a point by a matrix. */ #ifdef ANSI_FN_DEF void lib_transform_point(COORD3 vres, COORD3 vec, MATRIX mx) #else void lib_transform_point(vres, vec, mx) COORD3 vres, vec; MATRIX mx; #endif { COORD3 vtemp; vtemp[X] = vec[X]*mx[0][0] + vec[Y]*mx[1][0] + vec[Z]*mx[2][0] + mx[3][0]; vtemp[Y] = vec[X]*mx[0][1] + vec[Y]*mx[1][1] + vec[Z]*mx[2][1] + mx[3][1]; vtemp[Z] = vec[X]*mx[0][2] + vec[Y]*mx[1][2] + vec[Z]*mx[2][2] + mx[3][2]; COPY_COORD3(vres, vtemp); } /* * Multiply a 4 element vector by a matrix. Typically used for * homogenous transformation from world space to screen space. */ #ifdef ANSI_FN_DEF void lib_transform_coord(COORD4 vres, COORD4 vec, MATRIX mx) #else void lib_transform_coord(vres, vec, mx) COORD4 vres, vec; MATRIX mx; #endif { COORD4 vtemp; vtemp[X] = vec[X]*mx[0][0]+vec[Y]*mx[1][0]+vec[Z]*mx[2][0]+vec[W]*mx[3][0]; vtemp[Y] = vec[X]*mx[0][1]+vec[Y]*mx[1][1]+vec[Z]*mx[2][1]+vec[W]*mx[3][1]; vtemp[Z] = vec[X]*mx[0][2]+vec[Y]*mx[1][2]+vec[Z]*mx[2][2]+vec[W]*mx[3][2]; vtemp[W] = vec[X]*mx[0][3]+vec[Y]*mx[1][3]+vec[Z]*mx[2][3]+vec[W]*mx[3][3]; COPY_COORD4(vres, vtemp); } /* Determinant of a 3x3 matrix */ #ifdef ANSI_FN_DEF static double det3x3(double a1, double a2, double a3, double b1, double b2, double b3, double c1, double c2, double c3) #else static double det3x3(a1, a2, a3, b1, b2, b3, c1, c2, c3) double a1, a2, a3; double b1, b2, b3; double c1, c2, c3; #endif { double ans; ans = a1 * (b2 * c3 - b3 * c2) - b1 * (a2 * c3 - a3 * c2) + c1 * (a2 * b3 - a3 * b2); return ans; } /* Adjoint of a 4x4 matrix - used in the computation of the inverse of a 4x4 matrix */ #ifdef ANSI_FN_DEF static void adjoint(MATRIX out_mat, MATRIX in_mat) #else static void adjoint(out_mat, in_mat) MATRIX out_mat, in_mat; #endif { double a1, a2, a3, a4, b1, b2, b3, b4; double c1, c2, c3, c4, d1, d2, d3, d4; a1 = in_mat[0][0]; b1 = in_mat[0][1]; c1 = in_mat[0][2]; d1 = in_mat[0][3]; a2 = in_mat[1][0]; b2 = in_mat[1][1]; c2 = in_mat[1][2]; d2 = in_mat[1][3]; a3 = in_mat[2][0]; b3 = in_mat[2][1]; c3 = in_mat[2][2]; d3 = in_mat[2][3]; a4 = in_mat[3][0]; b4 = in_mat[3][1]; c4 = in_mat[3][2]; d4 = in_mat[3][3]; /* row column labeling reversed since we transpose rows & columns */ out_mat[0][0] = det3x3( b2, b3, b4, c2, c3, c4, d2, d3, d4); out_mat[1][0] = - det3x3( a2, a3, a4, c2, c3, c4, d2, d3, d4); out_mat[2][0] = det3x3( a2, a3, a4, b2, b3, b4, d2, d3, d4); out_mat[3][0] = - det3x3( a2, a3, a4, b2, b3, b4, c2, c3, c4); out_mat[0][1] = - det3x3( b1, b3, b4, c1, c3, c4, d1, d3, d4); out_mat[1][1] = det3x3( a1, a3, a4, c1, c3, c4, d1, d3, d4); out_mat[2][1] = - det3x3( a1, a3, a4, b1, b3, b4, d1, d3, d4); out_mat[3][1] = det3x3( a1, a3, a4, b1, b3, b4, c1, c3, c4); out_mat[0][2] = det3x3( b1, b2, b4, c1, c2, c4, d1, d2, d4); out_mat[1][2] = - det3x3( a1, a2, a4, c1, c2, c4, d1, d2, d4); out_mat[2][2] = det3x3( a1, a2, a4, b1, b2, b4, d1, d2, d4); out_mat[3][2] = - det3x3( a1, a2, a4, b1, b2, b4, c1, c2, c4); out_mat[0][3] = - det3x3( b1, b2, b3, c1, c2, c3, d1, d2, d3); out_mat[1][3] = det3x3( a1, a2, a3, c1, c2, c3, d1, d2, d3); out_mat[2][3] = - det3x3( a1, a2, a3, b1, b2, b3, d1, d2, d3); out_mat[3][3] = det3x3( a1, a2, a3, b1, b2, b3, c1, c2, c3); } /* Determinant of a 4x4 matrix */ #ifdef ANSI_FN_DEF double lib_matrix_det4x4(MATRIX mat) #else double lib_matrix_det4x4(mat) MATRIX mat; #endif { double ans; double a1, a2, a3, a4, b1, b2, b3, b4, c1, c2, c3, c4, d1, d2, d3, d4; a1 = mat[0][0]; b1 = mat[0][1]; c1 = mat[0][2]; d1 = mat[0][3]; a2 = mat[1][0]; b2 = mat[1][1]; c2 = mat[1][2]; d2 = mat[1][3]; a3 = mat[2][0]; b3 = mat[2][1]; c3 = mat[2][2]; d3 = mat[2][3]; a4 = mat[3][0]; b4 = mat[3][1]; c4 = mat[3][2]; d4 = mat[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; } /* Find the inverse of a 4x4 matrix */ #ifdef ANSI_FN_DEF void lib_invert_matrix(MATRIX out_mat, MATRIX in_mat) #else void lib_invert_matrix(out_mat, in_mat) MATRIX out_mat, in_mat; #endif { int i, j; double det; adjoint(out_mat, in_mat); det = lib_matrix_det4x4(in_mat); if (fabs(det) < EPSILON) { lib_create_identity_matrix(out_mat); return; } for (i=0;i<4;i++) for (j=0;j<4;j++) out_mat[i][j] /= det; } /* * Compute transpose of matrix. */ #ifdef ANSI_FN_DEF void lib_transpose_matrix( MATRIX mxres, MATRIX mx ) #else void lib_transpose_matrix( mxres, mx ) MATRIX mxres, mx; #endif { int i, j; for (i=0;i<4;i++) for (j=0;j<4;j++) mxres[j][i] = mx[i][j]; } /* * Multiply two 4x4 matrices. Note that mxres had better not be * the same as either mx1 or mx2 or bad results will be returned. */ #ifdef ANSI_FN_DEF void lib_matrix_multiply(MATRIX mxres, MATRIX mx1, MATRIX mx2) #else void lib_matrix_multiply(mxres, mx1, mx2) MATRIX mxres, mx1, mx2; #endif { int i, j; for (i=0;i<4;i++) for (j=0;j<4;j++) mxres[i][j] = mx1[i][0]*mx2[0][j] + mx1[i][1]*mx2[1][j] + mx1[i][2]*mx2[2][j] + mx1[i][3]*mx2[3][j]; } /* Performs a 3D clip of a line segment from start to end against the box defined by bounds. The actual values of start and end are modified. */ #ifdef ANSI_FN_DEF int lib_clip_to_box(COORD3 start, COORD3 end, double bounds[2][3]) #else int lib_clip_to_box(start, end, bounds) COORD3 start, end; double bounds[2][3]; #endif { int i; double d, t, dir, pos; double tmin, tmax; COORD3 D; SUB3_COORD3(D, end, start); d = lib_normalize_vector(D); tmin = 0.0; tmax = d; for (i=0;i<3;i++) { pos = start[i]; dir = D[i]; if (dir < -EPSILON) { t = (bounds[0][i] - pos) / dir; if (t < tmin) return 0; if (t <= tmax) tmax = t; t = (bounds[1][i] - pos) / dir; if (t >= tmin) { if (t > tmax) return 0; tmin = t; } } else if (dir > EPSILON) { t = (bounds[1][i] - pos) / dir; if (t < tmin) return 0; if (t <= tmax) tmax = t; t = (bounds[0][i] - pos) / dir; if (t >= tmin) { if (t > tmax) return 0; tmin = t; } } else if (pos < bounds[0][i] || pos > bounds[1][i]) return 0; } if (tmax < d) { end[X] = start[X] + tmax * D[X]; end[Y] = start[Y] + tmax * D[Y]; end[Z] = start[Z] + tmax * D[Z]; } if (tmin > 0.0) { start[X] = start[X] + tmin * D[X]; start[Y] = start[Y] + tmin * D[Y]; start[Z] = start[Z] + tmin * D[Z]; } return 1; } /* * Rotate a vector pointing towards the major-axis faces (i.e. the major-axis * component of the vector is defined as the largest value) 90 degrees to * another cube face. Mod_face is a face number. * * If the routine is called six times, with mod_face=0..5, the vector will be * rotated to each face of a cube. Rotations are: * mod_face = 0 mod 3, +Z axis rotate * mod_face = 1 mod 3, +X axis rotate * mod_face = 2 mod 3, -Y axis rotate */ #ifdef ANSI_FN_DEF void lib_rotate_cube_face(COORD3 vec, int major_axis, int mod_face) #else void lib_rotate_cube_face(vec, major_axis, mod_face) COORD3 vec; int major_axis, mod_face; #endif { double swap; mod_face = (mod_face+major_axis) % 3; if (mod_face == 0) { swap = vec[X]; vec[X] = -vec[Y]; vec[Y] = swap; } else if (mod_face == 1) { swap = vec[Y]; vec[Y] = -vec[Z]; vec[Z] = swap; } else { swap = vec[X]; vec[X] = -vec[Z]; vec[Z] = swap; } } /* * Portable gaussian random number generator (from "Numerical Recipes", GASDEV) * Returns a uniform random deviate between 0.0 and 1.0. 'iseed' must be * less than M1 to avoid repetition, and less than (2**31-C1)/A1 [= 300718] * to avoid overflow. */ #define M1 134456 #define IA1 8121 #define IC1 28411 #define RM1 1.0/M1 #ifdef ANSI_FN_DEF double lib_gauss_rand(long iseed) #else double lib_gauss_rand(iseed) long iseed; #endif { double fac, v1, v2, r; long ix1, ix2; ix2 = iseed; do { ix1 = (IC1+ix2*IA1) % M1; ix2 = (IC1+ix1*IA1) % M1; v1 = ix1 * 2.0 * RM1 - 1.0; v2 = ix2 * 2.0 * RM1 - 1.0; r = v1*v1 + v2*v2; } while ( r >= 1.0 ); fac = sqrt((double)(-2.0 * log((double)r) / r)); return v1 * fac; }