X-Git-Url: http://de.git.xonotic.org/?a=blobdiff_plain;f=misc%2Fmediasource%2Fextra%2Fnetradiant-src%2Flibs%2Fmathlib%2Fm4x4.c;fp=misc%2Fmediasource%2Fextra%2Fnetradiant-src%2Flibs%2Fmathlib%2Fm4x4.c;h=0000000000000000000000000000000000000000;hb=65bc0125990fdbce3dd3232d61f1554cb7e317cb;hp=3b11461015c5413f39462e135228cc9e4e3fd2d0;hpb=4dab6b8497e278118a8237e150d276e91501bfb0;p=voretournament%2Fvoretournament.git diff --git a/misc/mediasource/extra/netradiant-src/libs/mathlib/m4x4.c b/misc/mediasource/extra/netradiant-src/libs/mathlib/m4x4.c deleted file mode 100644 index 3b114610..00000000 --- a/misc/mediasource/extra/netradiant-src/libs/mathlib/m4x4.c +++ /dev/null @@ -1,1878 +0,0 @@ -/* -Copyright (C) 2001-2006, William Joseph. -All Rights Reserved. - -This file is part of GtkRadiant. - -GtkRadiant is free software; you can redistribute it and/or modify -it under the terms of the GNU General Public License as published by -the Free Software Foundation; either version 2 of the License, or -(at your option) any later version. - -GtkRadiant is distributed in the hope that it will be useful, -but WITHOUT ANY WARRANTY; without even the implied warranty of -MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the -GNU General Public License for more details. - -You should have received a copy of the GNU General Public License -along with GtkRadiant; if not, write to the Free Software -Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301 USA -*/ - -#include "mathlib.h" - -const m4x4_t g_m4x4_identity = { - 1, 0, 0, 0, - 0, 1, 0, 0, - 0, 0, 1, 0, - 0, 0, 0, 1, -}; - -void m4x4_identity(m4x4_t matrix) -{ - matrix[1] = matrix[2] = matrix[3] = - matrix[4] = matrix[6] = matrix[7] = - matrix[8] = matrix[9] = matrix[11] = - matrix[12] = matrix[13] = matrix[14] = 0; - - matrix[0] = matrix[5] = matrix[10] = matrix[15] = 1; -} - -m4x4Handedness_t m4x4_handedness(const m4x4_t matrix) -{ - vec3_t cross; - CrossProduct(matrix+0, matrix+4, cross); - return (DotProduct(matrix+8, cross) < 0) ? eLeftHanded : eRightHanded; -} - -void m4x4_assign(m4x4_t matrix, const m4x4_t other) -{ - M4X4_COPY(matrix, other); -} - -void m4x4_translation_for_vec3(m4x4_t matrix, const vec3_t translation) -{ - matrix[1] = matrix[2] = matrix[3] = - matrix[4] = matrix[6] = matrix[7] = - matrix[8] = matrix[9] = matrix[11] = 0; - - matrix[0] = matrix[5] = matrix[10] = matrix[15] = 1; - - matrix[12] = translation[0]; - matrix[13] = translation[1]; - matrix[14] = translation[2]; -} - -/* -clockwise rotation around X, Y, Z, facing along axis - 1 0 0 cy 0 sy cz sz 0 - 0 cx sx 0 1 0 -sz cz 0 - 0 -sx cx -sy 0 cy 0 0 1 - -rows of Z by cols of Y - cy*cz -sy*cz+sz -sy*sz+cz --sz*cy -sz*sy+cz - - .. or something like that.. - -final rotation is Z * Y * X - cy*cz -sx*-sy*cz+cx*sz cx*-sy*sz+sx*cz --cy*sz sx*sy*sz+cx*cz -cx*-sy*sz+sx*cz - sy -sx*cy cx*cy -*/ - -/* transposed -| cy.cz + 0.sz + sy.0 cy.-sz + 0 .cz + sy.0 cy.0 + 0 .0 + sy.1 | -| sx.sy.cz + cx.sz + -sx.cy.0 sx.sy.-sz + cx.cz + -sx.cy.0 sx.sy.0 + cx.0 + -sx.cy.1 | -| -cx.sy.cz + sx.sz + cx.cy.0 -cx.sy.-sz + sx.cz + cx.cy.0 -cx.sy.0 + 0 .0 + cx.cy.1 | -*/ -void m4x4_rotation_for_vec3(m4x4_t matrix, const vec3_t euler, eulerOrder_t order) -{ - double cx, sx, cy, sy, cz, sz; - - cx = cos(DEG2RAD(euler[0])); - sx = sin(DEG2RAD(euler[0])); - cy = cos(DEG2RAD(euler[1])); - sy = sin(DEG2RAD(euler[1])); - cz = cos(DEG2RAD(euler[2])); - sz = sin(DEG2RAD(euler[2])); - - switch(order) - { - case eXYZ: - -#if 1 - - { - matrix[0] = (vec_t)(cy*cz); - matrix[1] = (vec_t)(cy*sz); - matrix[2] = (vec_t)-sy; - matrix[4] = (vec_t)(sx*sy*cz + cx*-sz); - matrix[5] = (vec_t)(sx*sy*sz + cx*cz); - matrix[6] = (vec_t)(sx*cy); - matrix[8] = (vec_t)(cx*sy*cz + sx*sz); - matrix[9] = (vec_t)(cx*sy*sz + -sx*cz); - matrix[10] = (vec_t)(cx*cy); - } - - matrix[12] = matrix[13] = matrix[14] = matrix[3] = matrix[7] = matrix[11] = 0; - matrix[15] = 1; - -#else - - m4x4_identity(matrix); - matrix[5] =(vec_t) cx; matrix[6] =(vec_t) sx; - matrix[9] =(vec_t)-sx; matrix[10]=(vec_t) cx; - - { - m4x4_t temp; - m4x4_identity(temp); - temp[0] =(vec_t) cy; temp[2] =(vec_t)-sy; - temp[8] =(vec_t) sy; temp[10]=(vec_t) cy; - m4x4_premultiply_by_m4x4(matrix, temp); - m4x4_identity(temp); - temp[0] =(vec_t) cz; temp[1] =(vec_t) sz; - temp[4] =(vec_t)-sz; temp[5] =(vec_t) cz; - m4x4_premultiply_by_m4x4(matrix, temp); - } -#endif - - break; - - case eYZX: - m4x4_identity(matrix); - matrix[0] =(vec_t) cy; matrix[2] =(vec_t)-sy; - matrix[8] =(vec_t) sy; matrix[10]=(vec_t) cy; - - { - m4x4_t temp; - m4x4_identity(temp); - temp[5] =(vec_t) cx; temp[6] =(vec_t) sx; - temp[9] =(vec_t)-sx; temp[10]=(vec_t) cx; - m4x4_premultiply_by_m4x4(matrix, temp); - m4x4_identity(temp); - temp[0] =(vec_t) cz; temp[1] =(vec_t) sz; - temp[4] =(vec_t)-sz; temp[5] =(vec_t) cz; - m4x4_premultiply_by_m4x4(matrix, temp); - } - break; - - case eZXY: - m4x4_identity(matrix); - matrix[0] =(vec_t) cz; matrix[1] =(vec_t) sz; - matrix[4] =(vec_t)-sz; matrix[5] =(vec_t) cz; - - { - m4x4_t temp; - m4x4_identity(temp); - temp[5] =(vec_t) cx; temp[6] =(vec_t) sx; - temp[9] =(vec_t)-sx; temp[10]=(vec_t) cx; - m4x4_premultiply_by_m4x4(matrix, temp); - m4x4_identity(temp); - temp[0] =(vec_t) cy; temp[2] =(vec_t)-sy; - temp[8] =(vec_t) sy; temp[10]=(vec_t) cy; - m4x4_premultiply_by_m4x4(matrix, temp); - } - break; - - case eXZY: - m4x4_identity(matrix); - matrix[5] =(vec_t) cx; matrix[6] =(vec_t) sx; - matrix[9] =(vec_t)-sx; matrix[10]=(vec_t) cx; - - { - m4x4_t temp; - m4x4_identity(temp); - temp[0] =(vec_t) cz; temp[1] =(vec_t) sz; - temp[4] =(vec_t)-sz; temp[5] =(vec_t) cz; - m4x4_premultiply_by_m4x4(matrix, temp); - m4x4_identity(temp); - temp[0] =(vec_t) cy; temp[2] =(vec_t)-sy; - temp[8] =(vec_t) sy; temp[10]=(vec_t) cy; - m4x4_premultiply_by_m4x4(matrix, temp); - } - break; - - case eYXZ: - -/* transposed -| cy.cz + sx.sy.-sz + -cx.sy.0 0.cz + cx.-sz + sx.0 sy.cz + -sx.cy.-sz + cx.cy.0 | -| cy.sz + sx.sy.cz + -cx.sy.0 0.sz + cx.cz + sx.0 sy.sz + -sx.cy.cz + cx.cy.0 | -| cy.0 + sx.sy.0 + -cx.sy.1 0.0 + cx.0 + sx.1 sy.0 + -sx.cy.0 + cx.cy.1 | -*/ - -#if 1 - - { - matrix[0] = (vec_t)(cy*cz + sx*sy*-sz); - matrix[1] = (vec_t)(cy*sz + sx*sy*cz); - matrix[2] = (vec_t)(-cx*sy); - matrix[4] = (vec_t)(cx*-sz); - matrix[5] = (vec_t)(cx*cz); - matrix[6] = (vec_t)(sx); - matrix[8] = (vec_t)(sy*cz + -sx*cy*-sz); - matrix[9] = (vec_t)(sy*sz + -sx*cy*cz); - matrix[10] = (vec_t)(cx*cy); - } - - matrix[12] = matrix[13] = matrix[14] = matrix[3] = matrix[7] = matrix[11] = 0; - matrix[15] = 1; - -#else - - m4x4_identity(matrix); - matrix[0] =(vec_t) cy; matrix[2] =(vec_t)-sy; - matrix[8] =(vec_t) sy; matrix[10]=(vec_t) cy; - - { - m4x4_t temp; - m4x4_identity(temp); - temp[5] =(vec_t) cx; temp[6] =(vec_t) sx; - temp[9] =(vec_t)-sx; temp[10]=(vec_t) cx; - m4x4_premultiply_by_m4x4(matrix, temp); - m4x4_identity(temp); - temp[0] =(vec_t) cz; temp[1] =(vec_t) sz; - temp[4] =(vec_t)-sz; temp[5] =(vec_t) cz; - m4x4_premultiply_by_m4x4(matrix, temp); - } -#endif - break; - - case eZYX: -#if 1 - - { - matrix[0] = (vec_t)(cy*cz); - matrix[4] = (vec_t)(cy*-sz); - matrix[8] = (vec_t)sy; - matrix[1] = (vec_t)(sx*sy*cz + cx*sz); - matrix[5] = (vec_t)(sx*sy*-sz + cx*cz); - matrix[9] = (vec_t)(-sx*cy); - matrix[2] = (vec_t)(cx*-sy*cz + sx*sz); - matrix[6] = (vec_t)(cx*-sy*-sz + sx*cz); - matrix[10] = (vec_t)(cx*cy); - } - - matrix[12] = matrix[13] = matrix[14] = matrix[3] = matrix[7] = matrix[11] = 0; - matrix[15] = 1; - -#else - - m4x4_identity(matrix); - matrix[0] =(vec_t) cz; matrix[1] =(vec_t) sz; - matrix[4] =(vec_t)-sz; matrix[5] =(vec_t) cz; - { - m4x4_t temp; - m4x4_identity(temp); - temp[0] =(vec_t) cy; temp[2] =(vec_t)-sy; - temp[8] =(vec_t) sy; temp[10]=(vec_t) cy; - m4x4_premultiply_by_m4x4(matrix, temp); - m4x4_identity(temp); - temp[5] =(vec_t) cx; temp[6] =(vec_t) sx; - temp[9] =(vec_t)-sx; temp[10]=(vec_t) cx; - m4x4_premultiply_by_m4x4(matrix, temp); - } - -#endif - break; - - } -} - -void m4x4_scale_for_vec3(m4x4_t matrix, const vec3_t scale) -{ - matrix[1] = matrix[2] = matrix[3] = - matrix[4] = matrix[6] = matrix[7] = - matrix[8] = matrix[9] = matrix[11] = - matrix[12] = matrix[13] = matrix[14] = 0; - - matrix[15] = 1; - - matrix[0] = scale[0]; - matrix[5] = scale[1]; - matrix[10] = scale[2]; -} - -void m4x4_rotation_for_quat(m4x4_t matrix, const vec4_t quat) -{ -#if 0 - const double xx = quat[0] * quat[0]; - const double xy = quat[0] * quat[1]; - const double xz = quat[0] * quat[2]; - const double xw = quat[0] * quat[3]; - - const double yy = quat[1] * quat[1]; - const double yz = quat[1] * quat[2]; - const double yw = quat[1] * quat[3]; - - const double zz = quat[2] * quat[2]; - const double zw = quat[2] * quat[3]; - - matrix[0] = 1 - 2 * ( yy + zz ); - matrix[4] = 2 * ( xy - zw ); - matrix[8] = 2 * ( xz + yw ); - - matrix[1] = 2 * ( xy + zw ); - matrix[5] = 1 - 2 * ( xx + zz ); - matrix[9] = 2 * ( yz - xw ); - - matrix[2] = 2 * ( xz - yw ); - matrix[6] = 2 * ( yz + xw ); - matrix[10] = 1 - 2 * ( xx + yy ); -#else - const double x2 = quat[0] + quat[0]; - const double y2 = quat[1] + quat[1]; - const double z2 = quat[2] + quat[2]; - const double xx = quat[0] * x2; - const double xy = quat[0] * y2; - const double xz = quat[0] * z2; - const double yy = quat[1] * y2; - const double yz = quat[1] * z2; - const double zz = quat[2] * z2; - const double wx = quat[3] * x2; - const double wy = quat[3] * y2; - const double wz = quat[3] * z2; - - matrix[0] = (vec_t)( 1.0 - (yy + zz) ); - matrix[4] = (vec_t)(xy - wz); - matrix[8] = (vec_t)(xz + wy); - - matrix[1] = (vec_t)(xy + wz); - matrix[5] = (vec_t)( 1.0 - (xx + zz) ); - matrix[9] = (vec_t)(yz - wx); - - matrix[2] = (vec_t)(xz - wy); - matrix[6] = (vec_t)(yz + wx); - matrix[10] = (vec_t)( 1.0 - (xx + yy) ); -#endif - - matrix[3] = matrix[7] = matrix[11] = matrix[12] = matrix[13] = matrix[14] = 0; - matrix[15] = 1; -} - -void m4x4_rotation_for_axisangle(m4x4_t matrix, const vec3_t axis, double angle) -{ - vec4_t quat; - quat_for_axisangle(quat, axis, angle); - m4x4_rotation_for_quat(matrix, quat); -} - -void m4x4_frustum(m4x4_t matrix, - vec_t left, vec_t right, - vec_t bottom, vec_t top, - vec_t nearval, vec_t farval) -{ - matrix[0] = (vec_t)( (2*nearval) / (right-left) ); - matrix[1] = 0; - matrix[2] = 0; - matrix[3] = 0; - - matrix[4] = 0; - matrix[5] = (vec_t)( (2*nearval) / (top-bottom) ); - matrix[6] = 0; - matrix[7] = 0; - - matrix[8] = (vec_t)( (right+left) / (right-left) ); - matrix[9] = (vec_t)( (top+bottom) / (top-bottom) ); - matrix[10] = (vec_t)( -(farval+nearval) / (farval-nearval) ); - matrix[11] =-1; - - matrix[12] = 0; - matrix[13] = 0; - matrix[14] = (vec_t)( -(2*farval*nearval) / (farval-nearval) ); - matrix[15] = 0; -} - - -void m4x4_get_translation_vec3(const m4x4_t matrix, vec3_t translation) -{ - translation[0] = matrix[12]; - translation[1] = matrix[13]; - translation[2] = matrix[14]; -} - -void m4x4_get_rotation_vec3(const m4x4_t matrix, vec3_t euler, eulerOrder_t order) -{ - double a, ca; - - switch(order) - { - case eXYZ: - a = asin(-matrix[2]); - ca = cos(a); - euler[1] = (vec_t)RAD2DEG(a); /* Calculate Y-axis angle */ - - if (fabs(ca) > 0.005) /* Gimbal lock? */ - { - /* No, so get Z-axis angle */ - euler[2] = (vec_t)RAD2DEG(atan2(matrix[1] / ca, matrix[0]/ ca)); - - /* Get X-axis angle */ - euler[0] = (vec_t)RAD2DEG(atan2(matrix[6] / ca, matrix[10] / ca)); - } - else /* Gimbal lock has occurred */ - { - /* Set Z-axis angle to zero */ - euler[2] = 0; - - /* And calculate X-axis angle */ - euler[0] = (vec_t)RAD2DEG(atan2(-matrix[9], matrix[5])); - } - break; - case eYZX: - /* NOT IMPLEMENTED */ - break; - case eZXY: - /* NOT IMPLEMENTED */ - break; - case eXZY: - /* NOT IMPLEMENTED */ - break; - case eYXZ: - a = asin(matrix[6]); - ca = cos(a); - euler[0] = (vec_t)RAD2DEG(a); /* Calculate X-axis angle */ - - if (fabs(ca) > 0.005) /* Gimbal lock? */ - { - /* No, so get Y-axis angle */ - euler[1] = (vec_t)RAD2DEG(atan2(-matrix[2] / ca, matrix[10]/ ca)); - - /* Get Z-axis angle */ - euler[2] = (vec_t)RAD2DEG(atan2(-matrix[4] / ca, matrix[5] / ca)); - } - else /* Gimbal lock has occurred */ - { - /* Set Z-axis angle to zero */ - euler[2] = 0; - - /* And calculate Y-axis angle */ - euler[1] = (vec_t)RAD2DEG(atan2(matrix[8], matrix[0])); - } - break; - case eZYX: - a = asin(matrix[8]); - ca = cos(a); - euler[1] = (vec_t)RAD2DEG(a); /* Calculate Y-axis angle */ - - if (fabs(ca) > 0.005) /* Gimbal lock? */ - { - /* No, so get X-axis angle */ - euler[0] = (vec_t)RAD2DEG(atan2(-matrix[9] / ca, matrix[10]/ ca)); - - /* Get Z-axis angle */ - euler[2] = (vec_t)RAD2DEG(atan2(-matrix[4] / ca, matrix[0] / ca)); - } - else /* Gimbal lock has occurred */ - { - /* Set X-axis angle to zero */ - euler[0] = 0; - - /* And calculate Z-axis angle */ - euler[2] = (vec_t)RAD2DEG(atan2(matrix[1], matrix[5])); - } - break; - } - - /* return only positive angles in [0,360] */ - if (euler[0] < 0) euler[0] += 360; - if (euler[1] < 0) euler[1] += 360; - if (euler[2] < 0) euler[2] += 360; -} - -void m4x4_get_scale_vec3(const m4x4_t matrix, vec3_t scale) -{ - scale[0] = VectorLength(matrix+0); - scale[1] = VectorLength(matrix+4); - scale[2] = VectorLength(matrix+8); -} - -void m4x4_get_transform_vec3(const m4x4_t matrix, vec3_t translation, vec3_t euler, eulerOrder_t order, vec3_t scale) -{ - m4x4_t normalised; - m4x4_assign(normalised, matrix); - scale[0] = VectorNormalize(normalised+0, normalised+0); - scale[1] = VectorNormalize(normalised+4, normalised+4); - scale[2] = VectorNormalize(normalised+8, normalised+8); - if(m4x4_handedness(normalised) == eLeftHanded) - { - VectorNegate(normalised+0, normalised+0); - VectorNegate(normalised+4, normalised+4); - VectorNegate(normalised+8, normalised+8); - scale[0] = -scale[0]; - scale[1] = -scale[1]; - scale[2] = -scale[2]; - } - m4x4_get_rotation_vec3(normalised, euler, order); - m4x4_get_translation_vec3(matrix, translation); -} - -void m4x4_translate_by_vec3(m4x4_t matrix, const vec3_t translation) -{ - m4x4_t temp; - m4x4_translation_for_vec3(temp, translation); - m4x4_multiply_by_m4x4(matrix, temp); -} - -void m4x4_rotate_by_vec3(m4x4_t matrix, const vec3_t euler, eulerOrder_t order) -{ - m4x4_t temp; - m4x4_rotation_for_vec3(temp, euler, order); - m4x4_multiply_by_m4x4(matrix, temp); -} - -void m4x4_scale_by_vec3(m4x4_t matrix, const vec3_t scale) -{ - m4x4_t temp; - m4x4_scale_for_vec3(temp, scale); - m4x4_multiply_by_m4x4(matrix, temp); -} - -void m4x4_rotate_by_quat(m4x4_t matrix, const vec4_t rotation) -{ - m4x4_t temp; - m4x4_rotation_for_quat(temp, rotation); - m4x4_multiply_by_m4x4(matrix, temp); -} - -void m4x4_rotate_by_axisangle(m4x4_t matrix, const vec3_t axis, double angle) -{ - m4x4_t temp; - m4x4_rotation_for_axisangle(temp, axis, angle); - m4x4_multiply_by_m4x4(matrix, temp); -} - -void m4x4_transform_by_vec3(m4x4_t matrix, const vec3_t translation, const vec3_t euler, eulerOrder_t order, const vec3_t scale) -{ - m4x4_translate_by_vec3(matrix, translation); - m4x4_rotate_by_vec3(matrix, euler, order); - m4x4_scale_by_vec3(matrix, scale); -} - -void m4x4_pivoted_rotate_by_vec3(m4x4_t matrix, const vec3_t euler, eulerOrder_t order, const vec3_t pivotpoint) -{ - vec3_t vec3_temp; - VectorNegate(pivotpoint, vec3_temp); - - m4x4_translate_by_vec3(matrix, pivotpoint); - m4x4_rotate_by_vec3(matrix, euler, order); - m4x4_translate_by_vec3(matrix, vec3_temp); -} - -void m4x4_pivoted_scale_by_vec3(m4x4_t matrix, const vec3_t scale, const vec3_t pivotpoint) -{ - vec3_t vec3_temp; - VectorNegate(pivotpoint, vec3_temp); - - m4x4_translate_by_vec3(matrix, pivotpoint); - m4x4_scale_by_vec3(matrix, scale); - m4x4_translate_by_vec3(matrix, vec3_temp); -} - -void m4x4_pivoted_transform_by_vec3(m4x4_t matrix, const vec3_t translation, const vec3_t euler, eulerOrder_t order, const vec3_t scale, const vec3_t pivotpoint) -{ - vec3_t vec3_temp; - - VectorAdd(pivotpoint, translation, vec3_temp); - m4x4_translate_by_vec3(matrix, vec3_temp); - m4x4_rotate_by_vec3(matrix, euler, order); - m4x4_scale_by_vec3(matrix, scale); - VectorNegate(pivotpoint, vec3_temp); - m4x4_translate_by_vec3(matrix, vec3_temp); -} - -void m4x4_pivoted_transform_by_rotation(m4x4_t matrix, const vec3_t translation, const m4x4_t rotation, const vec3_t scale, const vec3_t pivotpoint) -{ - vec3_t vec3_temp; - - VectorAdd(pivotpoint, translation, vec3_temp); - m4x4_translate_by_vec3(matrix, vec3_temp); - m4x4_multiply_by_m4x4(matrix, rotation); - m4x4_scale_by_vec3(matrix, scale); - VectorNegate(pivotpoint, vec3_temp); - m4x4_translate_by_vec3(matrix, vec3_temp); -} - -void m4x4_pivoted_rotate_by_quat(m4x4_t matrix, const vec4_t rotation, const vec3_t pivotpoint) -{ - vec3_t vec3_temp; - VectorNegate(pivotpoint, vec3_temp); - - m4x4_translate_by_vec3(matrix, pivotpoint); - m4x4_rotate_by_quat(matrix, rotation); - m4x4_translate_by_vec3(matrix, vec3_temp); -} - -void m4x4_pivoted_rotate_by_axisangle(m4x4_t matrix, const vec3_t axis, double angle, const vec3_t pivotpoint) -{ - vec3_t vec3_temp; - VectorNegate(pivotpoint, vec3_temp); - - m4x4_translate_by_vec3(matrix, pivotpoint); - m4x4_rotate_by_axisangle(matrix, axis, angle); - m4x4_translate_by_vec3(matrix, vec3_temp); -} - -/* -A = A.B - -A0 = B0 * A0 + B1 * A4 + B2 * A8 + B3 * A12 -A4 = B4 * A0 + B5 * A4 + B6 * A8 + B7 * A12 -A8 = B8 * A0 + B9 * A4 + B10* A8 + B11* A12 -A12= B12* A0 + B13* A4 + B14* A8 + B15* A12 - -A1 = B0 * A1 + B1 * A5 + B2 * A9 + B3 * A13 -A5 = B4 * A1 + B5 * A5 + B6 * A9 + B7 * A13 -A9 = B8 * A1 + B9 * A5 + B10* A9 + B11* A13 -A13= B12* A1 + B13* A5 + B14* A9 + B15* A13 - -A2 = B0 * A2 + B1 * A6 + B2 * A10+ B3 * A14 -A6 = B4 * A2 + B5 * A6 + B6 * A10+ B7 * A14 -A10= B8 * A2 + B9 * A6 + B10* A10+ B11* A14 -A14= B12* A2 + B13* A6 + B14* A10+ B15* A14 - -A3 = B0 * A3 + B1 * A7 + B2 * A11+ B3 * A15 -A7 = B4 * A3 + B5 * A7 + B6 * A11+ B7 * A15 -A11= B8 * A3 + B9 * A7 + B10* A11+ B11* A15 -A15= B12* A3 + B13* A7 + B14* A11+ B15* A15 -*/ - -void m4x4_multiply_by_m4x4(m4x4_t dst, const m4x4_t src) -{ - vec_t dst0, dst1, dst2, dst3; - -#if 1 - - dst0 = src[0] * dst[0] + src[1] * dst[4] + src[2] * dst[8] + src[3] * dst[12]; - dst1 = src[4] * dst[0] + src[5] * dst[4] + src[6] * dst[8] + src[7] * dst[12]; - dst2 = src[8] * dst[0] + src[9] * dst[4] + src[10]* dst[8] + src[11]* dst[12]; - dst3 = src[12]* dst[0] + src[13]* dst[4] + src[14]* dst[8] + src[15]* dst[12]; - dst[0] = dst0; dst[4] = dst1; dst[8] = dst2; dst[12]= dst3; - - dst0 = src[0] * dst[1] + src[1] * dst[5] + src[2] * dst[9] + src[3] * dst[13]; - dst1 = src[4] * dst[1] + src[5] * dst[5] + src[6] * dst[9] + src[7] * dst[13]; - dst2 = src[8] * dst[1] + src[9] * dst[5] + src[10]* dst[9] + src[11]* dst[13]; - dst3 = src[12]* dst[1] + src[13]* dst[5] + src[14]* dst[9] + src[15]* dst[13]; - dst[1] = dst0; dst[5] = dst1; dst[9] = dst2; dst[13]= dst3; - - dst0 = src[0] * dst[2] + src[1] * dst[6] + src[2] * dst[10]+ src[3] * dst[14]; - dst1 = src[4] * dst[2] + src[5] * dst[6] + src[6] * dst[10]+ src[7] * dst[14]; - dst2 = src[8] * dst[2] + src[9] * dst[6] + src[10]* dst[10]+ src[11]* dst[14]; - dst3 = src[12]* dst[2] + src[13]* dst[6] + src[14]* dst[10]+ src[15]* dst[14]; - dst[2] = dst0; dst[6] = dst1; dst[10]= dst2; dst[14]= dst3; - - dst0 = src[0] * dst[3] + src[1] * dst[7] + src[2] * dst[11]+ src[3] * dst[15]; - dst1 = src[4] * dst[3] + src[5] * dst[7] + src[6] * dst[11]+ src[7] * dst[15]; - dst2 = src[8] * dst[3] + src[9] * dst[7] + src[10]* dst[11]+ src[11]* dst[15]; - dst3 = src[12]* dst[3] + src[13]* dst[7] + src[14]* dst[11]+ src[15]* dst[15]; - dst[3] = dst0; dst[7] = dst1; dst[11]= dst2; dst[15]= dst3; - -#else - - vec_t * p = dst; - for(int i=0;i<4;i++) - { - dst1 = src[0] * p[0]; - dst1 += src[1] * p[4]; - dst1 += src[2] * p[8]; - dst1 += src[3] * p[12]; - dst2 = src[4] * p[0]; - dst2 += src[5] * p[4]; - dst2 += src[6] * p[8]; - dst2 += src[7] * p[12]; - dst3 = src[8] * p[0]; - dst3 += src[9] * p[4]; - dst3 += src[10] * p[8]; - dst3 += src[11] * p[12]; - dst4 = src[12] * p[0]; - dst4 += src[13] * p[4]; - dst4 += src[14] * p[8]; - dst4 += src[15] * p[12]; - - p[0] = dst1; - p[4] = dst2; - p[8] = dst3; - p[12] = dst4; - p++; - } - -#endif -} - -/* -A = B.A - -A0 = A0 * B0 + A1 * B4 + A2 * B8 + A3 * B12 -A1 = A0 * B1 + A1 * B5 + A2 * B9 + A3 * B13 -A2 = A0 * B2 + A1 * B6 + A2 * B10+ A3 * B14 -A3 = A0 * B3 + A1 * B7 + A2 * B11+ A3 * B15 - -A4 = A4 * B0 + A5 * B4 + A6 * B8 + A7 * B12 -A5 = A4 * B1 + A5 * B5 + A6 * B9 + A7 * B13 -A6 = A4 * B2 + A5 * B6 + A6 * B10+ A7 * B14 -A7 = A4 * B3 + A5 * B7 + A6 * B11+ A7 * B15 - -A8 = A8 * B0 + A9 * B4 + A10* B8 + A11* B12 -A9 = A8 * B1 + A9 * B5 + A10* B9 + A11* B13 -A10= A8 * B2 + A9 * B6 + A10* B10+ A11* B14 -A11= A8 * B3 + A9 * B7 + A10* B11+ A11* B15 - -A12= A12* B0 + A13* B4 + A14* B8 + A15* B12 -A13= A12* B1 + A13* B5 + A14* B9 + A15* B13 -A14= A12* B2 + A13* B6 + A14* B10+ A15* B14 -A15= A12* B3 + A13* B7 + A14* B11+ A15* B15 -*/ - -void m4x4_premultiply_by_m4x4(m4x4_t dst, const m4x4_t src) -{ - vec_t dst0, dst1, dst2, dst3; - -#if 1 - - dst0 = dst[0] * src[0] + dst[1] * src[4] + dst[2] * src[8] + dst[3] * src[12]; - dst1 = dst[0] * src[1] + dst[1] * src[5] + dst[2] * src[9] + dst[3] * src[13]; - dst2 = dst[0] * src[2] + dst[1] * src[6] + dst[2] * src[10]+ dst[3] * src[14]; - dst3 = dst[0] * src[3] + dst[1] * src[7] + dst[2] * src[11]+ dst[3] * src[15]; - dst[0] = dst0; dst[1] = dst1; dst[2] = dst2; dst[3]= dst3; - - dst0 = dst[4] * src[0] + dst[5] * src[4] + dst[6] * src[8] + dst[7] * src[12]; - dst1 = dst[4] * src[1] + dst[5] * src[5] + dst[6] * src[9] + dst[7] * src[13]; - dst2 = dst[4] * src[2] + dst[5] * src[6] + dst[6] * src[10]+ dst[7] * src[14]; - dst3 = dst[4] * src[3] + dst[5] * src[7] + dst[6] * src[11]+ dst[7] * src[15]; - dst[4] = dst0; dst[5] = dst1; dst[6] = dst2; dst[7]= dst3; - - dst0 = dst[8] * src[0] + dst[9] * src[4] + dst[10]* src[8] + dst[11]* src[12]; - dst1 = dst[8] * src[1] + dst[9] * src[5] + dst[10]* src[9] + dst[11]* src[13]; - dst2 = dst[8] * src[2] + dst[9] * src[6] + dst[10]* src[10]+ dst[11]* src[14]; - dst3 = dst[8] * src[3] + dst[9] * src[7] + dst[10]* src[11]+ dst[11]* src[15]; - dst[8] = dst0; dst[9] = dst1; dst[10] = dst2; dst[11]= dst3; - - dst0 = dst[12]* src[0] + dst[13]* src[4] + dst[14]* src[8] + dst[15]* src[12]; - dst1 = dst[12]* src[1] + dst[13]* src[5] + dst[14]* src[9] + dst[15]* src[13]; - dst2 = dst[12]* src[2] + dst[13]* src[6] + dst[14]* src[10]+ dst[15]* src[14]; - dst3 = dst[12]* src[3] + dst[13]* src[7] + dst[14]* src[11]+ dst[15]* src[15]; - dst[12] = dst0; dst[13] = dst1; dst[14] = dst2; dst[15]= dst3; - -#else - - vec_t* p = dst; - for(int i=0;i<4;i++) - { - dst1 = src[0] * p[0]; - dst2 = src[1] * p[0]; - dst3 = src[2] * p[0]; - dst4 = src[3] * p[0]; - dst1 += src[4] * p[1]; - dst2 += src[5] * p[1]; - dst3 += src[6] * p[1]; - dst4 += src[7] * p[1]; - dst1 += src[8] * p[2]; - dst2 += src[9] * p[2]; - dst4 += src[11] * p[2]; - dst3 += src[10] * p[2]; - dst1 += src[12] * p[3]; - dst2 += src[13] * p[3]; - dst3 += src[14] * p[3]; - dst4 += src[15] * p[3]; - - *p++ = dst1; - *p++ = dst2; - *p++ = dst3; - *p++ = dst4; - } - -#endif -} - -void m4x4_orthogonal_multiply_by_m4x4(m4x4_t dst, const m4x4_t src) -{ - vec_t dst0, dst1, dst2, dst3; - - dst0 = src[0] * dst[0] + src[1] * dst[4] + src[2] * dst[8]; - dst1 = src[4] * dst[0] + src[5] * dst[4] + src[6] * dst[8]; - dst2 = src[8] * dst[0] + src[9] * dst[4] + src[10]* dst[8]; - dst3 = src[12]* dst[0] + src[13]* dst[4] + src[14]* dst[8] + dst[12]; - dst[0] = dst0; dst[4] = dst1; dst[8] = dst2; dst[12]= dst3; - - dst0 = src[0] * dst[1] + src[1] * dst[5] + src[2] * dst[9]; - dst1 = src[4] * dst[1] + src[5] * dst[5] + src[6] * dst[9]; - dst2 = src[8] * dst[1] + src[9] * dst[5] + src[10]* dst[9]; - dst3 = src[12]* dst[1] + src[13]* dst[5] + src[14]* dst[9] + dst[13]; - dst[1] = dst0; dst[5] = dst1; dst[9] = dst2; dst[13]= dst3; - - dst0 = src[0] * dst[2] + src[1] * dst[6] + src[2] * dst[10]; - dst1 = src[4] * dst[2] + src[5] * dst[6] + src[6] * dst[10]; - dst2 = src[8] * dst[2] + src[9] * dst[6] + src[10]* dst[10]; - dst3 = src[12]* dst[2] + src[13]* dst[6] + src[14]* dst[10]+ dst[14]; - dst[2] = dst0; dst[6] = dst1; dst[10]= dst2; dst[14]= dst3; -} - -void m4x4_orthogonal_premultiply_by_m4x4(m4x4_t dst, const m4x4_t src) -{ - vec_t dst0, dst1, dst2; - - dst0 = dst[0] * src[0] + dst[1] * src[4] + dst[2] * src[8]; - dst1 = dst[0] * src[1] + dst[1] * src[5] + dst[2] * src[9]; - dst2 = dst[0] * src[2] + dst[1] * src[6] + dst[2] * src[10]; - dst[0] = dst0; dst[1] = dst1; dst[2] = dst2; - - dst0 = dst[4] * src[0] + dst[5] * src[4] + dst[6] * src[8]; - dst1 = dst[4] * src[1] + dst[5] * src[5] + dst[6] * src[9]; - dst2 = dst[4] * src[2] + dst[5] * src[6] + dst[6] * src[10]; - dst[4] = dst0; dst[5] = dst1; dst[6] = dst2; - - dst0 = dst[8] * src[0] + dst[9] * src[4] + dst[10]* src[8]; - dst1 = dst[8] * src[1] + dst[9] * src[5] + dst[10]* src[9]; - dst2 = dst[8] * src[2] + dst[9] * src[6] + dst[10]* src[10]; - dst[8] = dst0; dst[9] = dst1; dst[10] = dst2; - - dst0 = dst[12]* src[0] + dst[13]* src[4] + dst[14]* src[8] + dst[15]* src[12]; - dst1 = dst[12]* src[1] + dst[13]* src[5] + dst[14]* src[9] + dst[15]* src[13]; - dst2 = dst[12]* src[2] + dst[13]* src[6] + dst[14]* src[10]+ dst[15]* src[14]; - dst[12] = dst0; dst[13] = dst1; dst[14] = dst2; -} - -void m4x4_transform_point(const m4x4_t matrix, vec3_t point) -{ - float out1, out2, out3; - - out1 = matrix[0] * point[0] + matrix[4] * point[1] + matrix[8] * point[2] + matrix[12]; - out2 = matrix[1] * point[0] + matrix[5] * point[1] + matrix[9] * point[2] + matrix[13]; - out3 = matrix[2] * point[0] + matrix[6] * point[1] + matrix[10] * point[2] + matrix[14]; - - point[0] = out1; - point[1] = out2; - point[2] = out3; -} - -void m4x4_transform_normal(const m4x4_t matrix, vec3_t normal) -{ - float out1, out2, out3; - - out1 = matrix[0] * normal[0] + matrix[4] * normal[1] + matrix[8] * normal[2]; - out2 = matrix[1] * normal[0] + matrix[5] * normal[1] + matrix[9] * normal[2]; - out3 = matrix[2] * normal[0] + matrix[6] * normal[1] + matrix[10] * normal[2]; - - normal[0] = out1; - normal[1] = out2; - normal[2] = out3; -} - -void m4x4_transform_vec4(const m4x4_t matrix, vec4_t vector) -{ - float out1, out2, out3, out4; - - out1 = matrix[0] * vector[0] + matrix[4] * vector[1] + matrix[8] * vector[2] + matrix[12] * vector[3]; - out2 = matrix[1] * vector[0] + matrix[5] * vector[1] + matrix[9] * vector[2] + matrix[13] * vector[3]; - out3 = matrix[2] * vector[0] + matrix[6] * vector[1] + matrix[10] * vector[2] + matrix[14] * vector[3]; - out4 = matrix[3] * vector[0] + matrix[7] * vector[1] + matrix[11] * vector[2] + matrix[15] * vector[3]; - - vector[0] = out1; - vector[1] = out2; - vector[2] = out3; - vector[3] = out4; -} - -#define CLIP_X_LT_W(p) ((p)[0] < (p)[3]) -#define CLIP_X_GT_W(p) ((p)[0] > -(p)[3]) -#define CLIP_Y_LT_W(p) ((p)[1] < (p)[3]) -#define CLIP_Y_GT_W(p) ((p)[1] > -(p)[3]) -#define CLIP_Z_LT_W(p) ((p)[2] < (p)[3]) -#define CLIP_Z_GT_W(p) ((p)[2] > -(p)[3]) - -clipmask_t homogenous_clip_point(const vec4_t clipped) -{ - clipmask_t result = CLIP_FAIL; - if(CLIP_X_LT_W(clipped)) result &= ~CLIP_LT_X; // X < W - if(CLIP_X_GT_W(clipped)) result &= ~CLIP_GT_X; // X > -W - if(CLIP_Y_LT_W(clipped)) result &= ~CLIP_LT_Y; // Y < W - if(CLIP_Y_GT_W(clipped)) result &= ~CLIP_GT_Y; // Y > -W - if(CLIP_Z_LT_W(clipped)) result &= ~CLIP_LT_Z; // Z < W - if(CLIP_Z_GT_W(clipped)) result &= ~CLIP_GT_Z; // Z > -W - return result; -} - -clipmask_t m4x4_clip_point(const m4x4_t matrix, const vec3_t point, vec4_t clipped) -{ - clipped[0] = point[0]; - clipped[1] = point[1]; - clipped[2] = point[2]; - clipped[3] = 1; - m4x4_transform_vec4(matrix, clipped); - return homogenous_clip_point(clipped); -} - - -unsigned int homogenous_clip_triangle(vec4_t clipped[9]) -{ - vec4_t buffer[9]; - unsigned int rcount = 3; - unsigned int wcount = 0; - vec_t const* rptr = clipped[0]; - vec_t* wptr = buffer[0]; - const vec_t* p0; - const vec_t* p1; - unsigned char b0, b1; - - unsigned int i; - double scale; - - p0 = rptr; - b0 = CLIP_X_LT_W(p0); - for(i=0; i= 0.0) pos += det; else neg += det; - - det = src[1] * src[6] * src[8]; - if (det >= 0.0) pos += det; else neg += det; - - det = src[2] * src[4] * src[9]; - if (det >= 0.0) pos += det; else neg += det; - - det = -src[2] * src[5] * src[8]; - if (det >= 0.0) pos += det; else neg += det; - - det = -src[1] * src[4] * src[10]; - if (det >= 0.0) pos += det; else neg += det; - - det = -src[0] * src[6] * src[9]; - if (det >= 0.0) pos += det; else neg += det; - - det = pos + neg; -#elif 0 - float det - = (src[0] * src[5] * src[10]) - + (src[1] * src[6] * src[8]) - + (src[2] * src[4] * src[9]) - - (src[2] * src[5] * src[8]) - - (src[1] * src[4] * src[10]) - - (src[0] * src[6] * src[9]); -#else - float det - = src[0] * ( src[5]*src[10] - src[9]*src[6] ) - - src[1] * ( src[4]*src[10] - src[8]*src[6] ) - + src[2] * ( src[4]*src[9] - src[8]*src[5] ); - -#endif - - if (det*det < 1e-25) - return 1; - - det = 1.0f / det; - matrix[0] = ( (src[5]*src[10]- src[6]*src[9] )*det); - matrix[1] = (- (src[1]*src[10]- src[2]*src[9] )*det); - matrix[2] = ( (src[1]*src[6] - src[2]*src[5] )*det); - matrix[4] = (- (src[4]*src[10]- src[6]*src[8] )*det); - matrix[5] = ( (src[0]*src[10]- src[2]*src[8] )*det); - matrix[6] = (- (src[0]*src[6] - src[2]*src[4] )*det); - matrix[8] = ( (src[4]*src[9] - src[5]*src[8] )*det); - matrix[9] = (- (src[0]*src[9] - src[1]*src[8] )*det); - matrix[10]= ( (src[0]*src[5] - src[1]*src[4] )*det); - } - - /* Do the translation part */ - matrix[12] = - (src[12] * matrix[0] + - src[13] * matrix[4] + - src[14] * matrix[8]); - matrix[13] = - (src[12] * matrix[1] + - src[13] * matrix[5] + - src[14] * matrix[9]); - matrix[14] = - (src[12] * matrix[2] + - src[13] * matrix[6] + - src[14] * matrix[10]); - - return 0; -} - -void quat_identity(vec4_t quat) -{ - quat[0] = quat[1] = quat[2] = 0; - quat[3] = 1; -} - -void quat_multiply_by_quat(vec4_t quat, const vec4_t other) -{ - const vec_t x = quat[3]*other[0] + quat[0]*other[3] + quat[1]*other[2] - quat[2]*other[1]; - const vec_t y = quat[3]*other[1] + quat[1]*other[3] + quat[2]*other[0] - quat[0]*other[2]; - const vec_t z = quat[3]*other[2] + quat[2]*other[3] + quat[0]*other[1] - quat[1]*other[0]; - const vec_t w = quat[3]*other[3] - quat[0]*other[0] - quat[1]*other[1] - quat[2]*other[2]; - quat[0] = x; - quat[1] = y; - quat[2] = z; - quat[3] = w; -} - -void quat_conjugate(vec4_t quat) -{ - VectorNegate(quat, quat); -} - -//! quaternion from two unit vectors -void quat_for_unit_vectors(vec4_t quat, const vec3_t from, const vec3_t to) -{ - CrossProduct(from, to, quat); - quat[3] = DotProduct(from, to); -} - -void quat_normalise(vec4_t quat) -{ - const vec_t n = 1 / ( quat[0] * quat[0] + quat[1] * quat[1] + quat[2] * quat[2] + quat[3] * quat[3] ); - quat[0] *= n; - quat[1] *= n; - quat[2] *= n; - quat[3] *= n; -} - -void quat_for_axisangle(vec4_t quat, const vec3_t axis, double angle) -{ - angle *= 0.5; - - quat[3] = (float)sin(angle); - - quat[0] = axis[0] * quat[3]; - quat[1] = axis[1] * quat[3]; - quat[2] = axis[2] * quat[3]; - quat[3] = (float)cos(angle); -} - -void m3x3_multiply_by_m3x3(m3x3_t matrix, const m3x3_t matrix_src) -{ - float *pDest = matrix; - float out1, out2, out3; - int i; - - for(i=0;i<3;i++) - { - out1 = matrix_src[0] * pDest[0]; - out1 += matrix_src[1] * pDest[3]; - out1 += matrix_src[2] * pDest[6]; - out2 = matrix_src[3] * pDest[0]; - out2 += matrix_src[4] * pDest[3]; - out2 += matrix_src[5] * pDest[6]; - out3 = matrix_src[6] * pDest[0]; - out3 += matrix_src[7] * pDest[3]; - out3 += matrix_src[8] * pDest[6]; - - pDest[0] = out1; - pDest[3] = out2; - pDest[6] = out3; - - pDest++; - } -} - -void m3x3_transform_vec3(const m3x3_t matrix, vec3_t vector) -{ - float out1, out2, out3; - - out1 = matrix[0] * vector[0]; - out1 += matrix[3] * vector[1]; - out1 += matrix[6] * vector[2]; - out2 = matrix[1] * vector[0]; - out2 += matrix[4] * vector[1]; - out2 += matrix[7] * vector[2]; - out3 = matrix[2] * vector[0]; - out3 += matrix[5] * vector[1]; - out3 += matrix[8] * vector[2]; - - vector[0] = out1; - vector[1] = out2; - vector[2] = out3; -} - -float m3_det( m3x3_t mat ) -{ - float det; - - det = mat[0] * ( mat[4]*mat[8] - mat[7]*mat[5] ) - - mat[1] * ( mat[3]*mat[8] - mat[6]*mat[5] ) - + mat[2] * ( mat[3]*mat[7] - mat[6]*mat[4] ); - - return( det ); -} - -int m3_inverse( m3x3_t mr, m3x3_t ma ) -{ - float det = m3_det( ma ); - - if (det == 0 ) - { - return 1; - } - - - mr[0] = ma[4]*ma[8] - ma[5]*ma[7] / det; - mr[1] = -( ma[1]*ma[8] - ma[7]*ma[2] ) / det; - mr[2] = ma[1]*ma[5] - ma[4]*ma[2] / det; - - mr[3] = -( ma[3]*ma[8] - ma[5]*ma[6] ) / det; - mr[4] = ma[0]*ma[8] - ma[6]*ma[2] / det; - mr[5] = -( ma[0]*ma[5] - ma[3]*ma[2] ) / det; - - mr[6] = ma[3]*ma[7] - ma[6]*ma[4] / det; - mr[7] = -( ma[0]*ma[7] - ma[6]*ma[1] ) / det; - mr[8] = ma[0]*ma[4] - ma[1]*ma[3] / det; - - return 0; -} - -void m4_submat( m4x4_t mr, m3x3_t mb, int i, int j ) -{ - int ti, tj, idst, jdst; - - for ( ti = 0; ti < 4; ti++ ) - { - if ( ti < i ) - idst = ti; - else if ( ti > i ) - idst = ti-1; - else - continue; - - for ( tj = 0; tj < 4; tj++ ) - { - if ( tj < j ) - jdst = tj; - else if ( tj > j ) - jdst = tj-1; - else - continue; - - mb[idst*3 + jdst] = mr[ti*4 + tj ]; - } - } -} - -float m4_det( m4x4_t mr ) -{ - float det, result = 0, i = 1; - m3x3_t msub3; - int n; - - for ( n = 0; n < 4; n++, i *= -1 ) - { - m4_submat( mr, msub3, 0, n ); - - det = m3_det( msub3 ); - result += mr[n] * det * i; - } - - return result; -} - -int m4x4_invert(m4x4_t matrix) -{ - float mdet = m4_det( matrix ); - m3x3_t mtemp; - int i, j, sign; - m4x4_t m4x4_temp; - -#if 0 - if ( fabs( mdet ) < 0.0000000001 ) - return 1; -#endif - - m4x4_assign(m4x4_temp, matrix); - - for ( i = 0; i < 4; i++ ) - for ( j = 0; j < 4; j++ ) - { - sign = 1 - ( (i +j) % 2 ) * 2; - - m4_submat( m4x4_temp, mtemp, i, j ); - - matrix[i+j*4] = ( m3_det( mtemp ) * sign ) / mdet; /* FIXME: try using * inverse det and see if speed/accuracy are good enough */ - } - - return 0; -} -#if 0 -void m4x4_solve_ge(m4x4_t matrix, vec4_t x) -{ - int indx[4]; - int c,r; - int i; - int best; - float scale[4]; - float f, pivot; - float aug[4]; - float recip, ratio; - float* p; - - for(r=0; r<4; r++) - { - aug[r] = 0; - indx[r] = r; - } - - for (r=0; r<4; r++) - { - scale[r] = 0; - for (c=0; c<4; c++, p++) - { - if (fabs(*p) > scale[r]) - { - scale[r] = (float)fabs(*p); - } - } - } - - for (c=0; c<3; c++) - { - pivot = 0; - for (r=c; r<4; r++) - { - f = (float)fabs(matrix[(indx[r]<<2)+c]) / scale[indx[r]]; - if (f > pivot) - { - pivot = f; - best = r; - } - } - - i = indx[c]; - indx[c] = indx[best]; - indx[best] = i; - - recip = 1 / matrix[(indx[c]<<2)+c]; - - for (r=c+1; r<4; r++) - { - p = matrix + (indx[r]<<2); - ratio = p[c] * recip; - - for (i=c+1; i<4; i++) - p[i] -= ratio * matrix[(indx[c]<<2)+i]; - aug[indx[r]] -= ratio * aug[indx[c]]; - } - } - - x[indx[3]] = aug[indx[3]] / matrix[(indx[3]<<2)+3]; - for(r=2; r>=0; r--) - { - f = aug[indx[r]]; - p = matrix + (indx[r]<<2); - recip = 1 / p[r]; - for(c=(r+1); c<4; c++) - { - f -= (p[c] * x[indx[c]]); - } - x[indx[r]] = f * recip; - } -} -#endif - -#define N 3 - -int matrix_solve_ge(vec_t* matrix, vec_t* aug, vec3_t x) -{ - int indx[N]; - int c,r; - int i; - int best; - float scale[N]; - float f, pivot; - float ratio; - float* p; - - for(r=0; r scale[r]) - { - scale[r] = (float)fabs(*p); - } - } - } - - for (c=0; c pivot) - { - pivot = f; - best = r; - } - } - - if(best == -1) return 1; - - i = indx[c]; - indx[c] = indx[best]; - indx[best] = i; - - for (r=c+1; r=0; r--) - { - f = aug[indx[r]]; - p = matrix + (indx[r]*N); - for(c=(r+1); c=0;i--) - { - temp = b[i]; - for(j=(i+1);j