////////////////////////////////////////////////////////////////////// // // Pixie // // Copyright © 1999 - 2003, Okan Arikan // // Contact: okan@cs.berkeley.edu // // This library 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. // // This library 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 this library; if not, write to the Free Software // Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA // /////////////////////////////////////////////////////////////////////// /////////////////////////////////////////////////////////////////////// // // File : algebra.cpp // Classes : - // Description : This file implements some basic algebra operations // //////////////////////////////////////////////////////////////////////// #include #include "algebra.h" // First some misc functions used in matrix inversion /////////////////////////////////////////////////////////////////////// // Function : det2x2 // Description : 2 by 2 determinant computation // Return Value : Determinant // Comments : // Date last edited : 6/20/2002 // Matrix: // | a b | // | c d | static inline double det2x2( const double a,const double b,const double c,const double d) { return a * d - b * c; } /////////////////////////////////////////////////////////////////////// // Function : det3x3 // Description : 3 by 3 determinant computation // Return Value : Determinant // Comments : // Date last edited : 6/20/2002 // Matrix: // | a1 a2 a3 | // | b1 b2 b3 | // | c1 c2 c3 | static inline double det3x3(const double a1,const double a2,const double a3,const double b1,const double b2,const double b3,const double c1,const double c2,const double c3 ) { return a1 * det2x2( b2, b3, c2, c3 ) - b1 * det2x2( a2, a3, c2, c3 ) + c1 * det2x2( a2, a3, b2, b3 ); } /////////////////////////////////////////////////////////////////////// // Function : det4x4 // Description : 4 by 4 determinant computation // Return Value : Determinant // Comments : // Date last edited : 6/20/2002 // Matrix: // | a1 a2 a3 a4 | // | b1 b2 b3 b4 | // | c1 c2 c3 c4 | // | d1 d2 d3 d4 | static inline double det4x4( const float *m ) { double a1, a2, a3, a4, b1, b2, b3, b4, c1, c2, c3, c4, d1, d2, d3, d4; a1 = m[element(0,0)]; b1 = m[element(0,1)]; c1 = m[element(0,2)]; d1 = m[element(0,3)]; a2 = m[element(1,0)]; b2 = m[element(1,1)]; c2 = m[element(1,2)]; d2 = m[element(1,3)]; a3 = m[element(2,0)]; b3 = m[element(2,1)]; c3 = m[element(2,2)]; d3 = m[element(2,3)]; a4 = m[element(3,0)]; b4 = m[element(3,1)]; c4 = m[element(3,2)]; d4 = m[element(3,3)]; return 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); } /////////////////////////////////////////////////////////////////////// // Function : adjoint // Description : Compute the adjoint of a 4 by 4 matrix // Return Value : - // Comments : // Date last edited : 6/20/2002 // Construct the adjoint matrix // | a1 a2 a3 a4 | // | b1 b2 b3 b4 | // | c1 c2 c3 c4 | // | d1 d2 d3 d4 | static inline void adjoint( float *out,const float *in) { float a1, a2, a3, a4, b1, b2, b3, b4; float c1, c2, c3, c4, d1, d2, d3, d4; a1 = in[element(0,0)]; b1 = in[element(0,1)]; c1 = in[element(0,2)]; d1 = in[element(0,3)]; a2 = in[element(1,0)]; b2 = in[element(1,1)]; c2 = in[element(1,2)]; d2 = in[element(1,3)]; a3 = in[element(2,0)]; b3 = in[element(2,1)]; c3 = in[element(2,2)]; d3 = in[element(2,3)]; a4 = in[element(3,0)]; b4 = in[element(3,1)]; c4 = in[element(3,2)]; d4 = in[element(3,3)]; out[element(0,0)] = (float) det3x3( b2, b3, b4, c2, c3, c4, d2, d3, d4); out[element(1,0)] = (float) -det3x3( a2, a3, a4, c2, c3, c4, d2, d3, d4); out[element(2,0)] = (float) det3x3( a2, a3, a4, b2, b3, b4, d2, d3, d4); out[element(3,0)] = (float) -det3x3( a2, a3, a4, b2, b3, b4, c2, c3, c4); out[element(0,1)] = (float) -det3x3( b1, b3, b4, c1, c3, c4, d1, d3, d4); out[element(1,1)] = (float) det3x3( a1, a3, a4, c1, c3, c4, d1, d3, d4); out[element(2,1)] = (float) -det3x3( a1, a3, a4, b1, b3, b4, d1, d3, d4); out[element(3,1)] = (float) det3x3( a1, a3, a4, b1, b3, b4, c1, c3, c4); out[element(0,2)] = (float) det3x3( b1, b2, b4, c1, c2, c4, d1, d2, d4); out[element(1,2)] = (float) -det3x3( a1, a2, a4, c1, c2, c4, d1, d2, d4); out[element(2,2)] = (float) det3x3( a1, a2, a4, b1, b2, b4, d1, d2, d4); out[element(3,2)] = (float) -det3x3( a1, a2, a4, b1, b2, b4, c1, c2, c4); out[element(0,3)] = (float) -det3x3( b1, b2, b3, c1, c2, c3, d1, d2, d3); out[element(1,3)] = (float) det3x3( a1, a2, a3, c1, c2, c3, d1, d2, d3); out[element(2,3)] = (float) -det3x3( a1, a2, a3, b1, b2, b3, d1, d2, d3); out[element(3,3)] = (float) det3x3( a1, a2, a3, b1, b2, b3, c1, c2, c3); } /////////////////////////////////////////////////////////////////////// // Function : invert // Description : invert a 4 by 4 matrix // Return Value : Determinant // Comments : // Date last edited : 6/20/2002 int invertm(float *d,const float *s) { int i; double det; adjoint(d,s); det = det4x4(s); if (det == 0) { identitym(d); return TRUE; } for (i=0; i<16; i++) d[i] = (float) (d[i] / det); return FALSE; } /////////////////////////////////////////////////////////////////////// // Function : skewsymm // Description : compute the skewed symmetic matrix // Return Value : - // Comments : // Date last edited : 6/20/2002 void skewsymm(float *dest,const float *src) { identitym(dest); dest[element(0,0)] = 0; dest[element(1,1)] = 0; dest[element(2,2)] = 0; dest[element(0,1)] = -src[COMP_Z]; dest[element(0,2)] = src[COMP_Y]; dest[element(1,2)] = -src[COMP_X]; dest[element(1,0)] = src[COMP_Z]; dest[element(2,0)] = -src[COMP_Y]; dest[element(2,1)] = src[COMP_X]; } /////////////////////////////////////////////////////////////////////// // Function : determinantm // Description : 4 by 4 determinant computation // Return Value : Determinant // Comments : // Date last edited : 6/20/2002 float determinantm(const float *r) { return (float) det4x4(r); } /////////////////////////////////////////////////////////////////////// // Function : identitym // Description : create identity matrix // Return Value : - // Comments : // Date last edited : 6/20/2002 void identitym(float *r) { int i; for (i=0;i<16;i++) r[i] = 0; r[element(0,0)] = 1; r[element(1,1)] = 1; r[element(2,2)] = 1; r[element(3,3)] = 1; } /////////////////////////////////////////////////////////////////////// // Function : translatem // Description : create translation matrix // Return Value : - // Comments : // Date last edited : 6/20/2002 void translatem(float *r,const float tx,const float ty,const float tz) { identitym(r); r[element(0,3)] = tx; r[element(1,3)] = ty; r[element(2,3)] = tz; } /////////////////////////////////////////////////////////////////////// // Function : scalem // Description : create scale matrix // Return Value : Determinant // Comments : // Date last edited : 6/20/2002 void scalem(float *r,const float sx,const float sy,const float sz) { identitym(r); r[element(0,0)] = sx; r[element(1,1)] = sy; r[element(2,2)] = sz; } /////////////////////////////////////////////////////////////////////// // Function : rotatem // Description : create a rotation matrix // Return Value : - // Comments : the angle is in degrees // Date last edited : 6/20/2002 void rotatem(float *r,const float *v,const float angle) { rotatem(r,v[COMP_X],v[COMP_Y],v[COMP_Z],angle); } /////////////////////////////////////////////////////////////////////// // Function : rotatem // Description : create a rotation matrix // Return Value : - // Comments : the angle is in degrees // Date last edited : 6/20/2002 void rotatem(float *r,float x,float y,float z,const float angle) { double s = cos(radians(angle/2)); double sp = sin(radians(angle/2)); double l = sqrt(x*x + y*y + z*z); double a = x*sp/l; double b = y*sp/l; double c = z*sp/l; l = sqrt(a*a + b*b + c*c + s*s); a /= l; b /= l; c /= l; s /= l; r[element(0,0)] = (float) (1-2*b*b-2*c*c); r[element(0,1)] = (float) (2*a*b-2*s*c); r[element(0,2)] = (float) (2*a*c+2*s*b); r[element(0,3)] = 0; r[element(1,0)] = (float) (2*a*b+2*s*c); r[element(1,1)] = (float) (1-2*a*a-2*c*c); r[element(1,2)] = (float) (2*b*c-2*s*a); r[element(1,3)] = 0; r[element(2,0)] = (float) (2*a*c-2*s*b); r[element(2,1)] = (float) (2*b*c+2*s*a); r[element(2,2)] = (float) (1-2*a*a-2*b*b); r[element(2,3)] = 0; r[element(3,0)] = 0; r[element(3,1)] = 0; r[element(3,2)] = 0; r[element(3,3)] = 1; } /////////////////////////////////////////////////////////////////////// // Function : skewm // Description : create a skew matrix // Return Value : - // Comments : the angle is in degrees // Date last edited : 6/20/2002 void skewm(float *r,const float angle,const float dx1,const float dy1,const float dz1,const float dx2,const float dy2,const float dz2) { vector v1,v2,d; matrix R,S,tmp,tmp2; float costheta,sintheta,skew; v1[COMP_X] = dx1; v1[COMP_Y] = dy1; v1[COMP_Z] = dz1; normalizev(v1); v2[COMP_X] = dx2; v2[COMP_Y] = dy2; v2[COMP_Z] = dz2; normalizev(v2); crossvv(d,v1,v2); normalizev(d); costheta = dotvv(v1,v2); sintheta = sqrtf(1 - costheta*costheta); skew = tanf(radians(angle) + acos(sintheta))*sintheta - costheta; crossvv(v1,d,v2); normalizev(v1); R[element(0,0)] = d[COMP_X]; R[element(1,0)] = d[COMP_Y]; R[element(2,0)] = d[COMP_Z]; R[element(3,0)] = 0; R[element(0,1)] = v1[COMP_X]; R[element(1,1)] = v1[COMP_Y]; R[element(2,1)] = v1[COMP_Z]; R[element(3,1)] = 0; R[element(0,2)] = v2[COMP_X]; R[element(1,2)] = v2[COMP_Y]; R[element(2,2)] = v2[COMP_Z]; R[element(3,2)] = 0; R[element(0,3)] = 0; R[element(1,3)] = 0; R[element(2,3)] = 0; R[element(3,3)] = 1; identitym(S); S[element(2,1)] = -skew; transposem(tmp,R); mulmm(tmp2,S,tmp); mulmm(r,R,tmp2); } #define S 0 #define VX 1 #define VY 2 #define VZ 3 /////////////////////////////////////////////////////////////////////// // Function : rotatem // Description : create a rotation matrix // Return Value : - // Comments : // Date last edited : 6/20/2002 void rotatem(float *to,const float *from) { to[element(0,0)] = 1-2*from[VY]*from[VY] - 2*from[VZ]*from[VZ]; to[element(0,1)] = 2*from[VX]*from[VY] - 2*from[S]*from[VZ]; to[element(0,2)] = 2*from[VX]*from[VZ] + 2*from[S]*from[VY]; to[element(1,0)] = 2*from[VX]*from[VY] + 2*from[S]*from[VZ]; to[element(1,1)] = 1-2*from[VX]*from[VX] - 2*from[VZ]*from[VZ]; to[element(1,2)] = 2*from[VY]*from[VZ] - 2*from[S]*from[VX]; to[element(2,0)] = 2*from[VX]*from[VZ] - 2*from[S]*from[VY]; to[element(2,1)] = 2*from[VY]*from[VZ] + 2*from[S]*from[VX]; to[element(2,2)] = 1-2*from[VX]*from[VX] - 2*from[VY]*from[VY]; to[element(0,3)] = 0; to[element(1,3)] = 0; to[element(2,3)] = 0; to[element(3,3)] = 1; to[element(3,0)] = 0; to[element(3,1)] = 0; to[element(3,2)] = 0; } #undef S #undef VX #undef VY #undef VZ