/******************************************************************************\ Copyright 1993 The Regents of the University of California. Modification Copyright 1995 The University of North Carolina at Chapel Hill. All Rights Reserved. Permission to use, copy, modify and distribute this software and its documentation for educational, research and non-profit purposes, without fee, and without a written agreement is hereby granted, provided that the above copyright notice and the following three paragraphs appear in all copies. IN NO EVENT SHALL THE UNIVERSITY OF CALIFORNIA OR THE UNIVERSITY OF NORTH CAROLINA AT CHAPEL HILL BE LIABLE TO ANY PARTY FOR DIRECT, INDIRECT, SPECIAL, INCIDENTAL, OR CONSEQUENTIAL DAMAGES, INCLUDING LOST PROFITS, ARISING OUT OF THE USE OF THIS SOFTWARE AND ITS DOCUMENTATION, EVEN IF THE UNIVERSITY OF CALIFORNIA OR THE UNIVERSITY OF N. CAROLINA HAVE BEEN ADVISED OF THE POSSIBILITY OF SUCH DAMAGES. Permission to use, copy, modify and distribute this software and its documentation for educational, research and non-profit purposes, without fee, and without a written agreement is hereby granted, provided that the above copyright notice and the following three paragraphs appear in all copies. THE UNIVERSITY OF CALIFORNIA AND THE UNIVERSITY OF NORTH CAROLINA SPECIFICALLY DISCLAIM ANY WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE. THE SOFTWARE PROVIDED HEREUNDER IS ON AN "AS IS" BASIS, AND THE UNIVERSITY OF CALIFORNIA HAS NO OBLIGATIONS TO PROVIDE MAINTENANCE, SUPPORT, UPDATES, ENHANCEMENTS, OR MODIFICATIONS. The authors may be contacted via: US Mail: Brian Mirtich J. Cohen/M. Lin/D. Manocha/K. Ponamgi 387 Soda Hall Department of Computer Science Computer Science Division Sitterson Hall, CB #3175 University of California University of N. Carolina Berkeley, CA 94720 Chapel Hill, NC 27599-3175 Phone: (510) 642-8149 (919)962-1749 EMail: mirtich@cs.berkeley.edu {cohenj,lin,manocha,ponamgi}@cs.unc.edu \*****************************************************************************/ /*****************************************************************************\ matrix.c -- Description : This is a collection of routines for manipulating matrices and vectors. \*****************************************************************************/ /*----------------------------- Local Includes -----------------------------*/ #include #include #include /*----------------------------- Local Constants -----------------------------*/ #ifndef M_PI # define M_PI 3.14159265358979323846 #endif /*------------------------------ Local Macros -------------------------------*/ #define sqr(x) ((x) * (x)) /*------------------------------- Local Types -------------------------------*/ /*------------------------ Local Function Prototypes ------------------------*/ /*------------------------------ Local Globals ------------------------------*/ /* identity matrix */ col_Mat4 __col_mat4IDENTITY = {{1, 0, 0, 0}, {0, 1, 0, 0}, {0, 0, 1, 0}, {0, 0, 0, 1}}; col_Mat3 __col_mat3IDENTITY = {{1, 0, 0}, {0, 1, 0}, {0, 0, 1}}; /*---------------------------------Functions-------------------------------- */ /* ============================================================================= matrix stuff The following operations all deal with transformation matrices, 4 x 4 matrices corresponding to rigid body transformations of homogeneous coordinates. ============================================================================= */ /*****************************************************************************\ @ __col_mat4Add() ----------------------------------------------------------------------------- description : matrix add: a + b => c input : output : notes : c can point to the same matrix as a or b \*****************************************************************************/ void __col_mat4Add(col_Mat4 a, col_Mat4 b, col_Mat4 c) { int i, j; for (i = 0; i < 4; i++) for (j = 0; j < 4; j++) c[i][j] = a[i][j] + b[i][j]; } /** End of __col_mat4Add() **/ /*****************************************************************************\ @ __col_mat4Sub() ----------------------------------------------------------------------------- description : matrix subtract: a - b => c input : output : notes : c can point to the same matrix as a or b \*****************************************************************************/ void __col_mat4Sub(col_Mat4 a, col_Mat4 b, col_Mat4 c) { int i, j; for (i = 0; i < 4; i++) for (j = 0; j < 4; j++) c[i][j] = a[i][j] - b[i][j]; } /** End of __col_mat4Sub() **/ /*****************************************************************************\ @ __col_mat4Mult() ----------------------------------------------------------------------------- description : matrix multiply: a * b => c input : output : notes : c should not point to the same matrix as a or b! \*****************************************************************************/ void __col_mat4Mult(col_Mat4 a, col_Mat4 b, col_Mat4 c) { int i, j; for (i = 0; i < 4; i++) for (j = 0; j < 4; j++) c[i][j] = a[i][0] * b[0][j] + a[i][1] * b[1][j] + a[i][2] * b[2][j] + a[i][3] * b[3][j]; } /** End of __col_mat4Mult() **/ /*****************************************************************************\ @ __col_mat3Mult() ----------------------------------------------------------------------------- description : matrix multiply: a * b => c input : output : notes : c should not point to the same matrix as a or b! \*****************************************************************************/ void __col_mat3Mult(col_Mat3 a, col_Mat3 b, col_Mat3 c) { int i, j; for (i = 0; i < 3; i++) for (j = 0; j < 3; j++) c[i][j] = a[i][0] * b[0][j] + a[i][1] * b[1][j] + a[i][2] * b[2][j]; } /** End of __col_mat3Mult() **/ /*****************************************************************************\ @ __col_mat4Copy() ----------------------------------------------------------------------------- description : matrix copy: source => dest input : output : notes : \*****************************************************************************/ void __col_mat4Copy(col_Mat4 source, col_Mat4 dest) { int i, j; for (i = 0; i < 4; i++) for (j = 0; j < 4; j++) dest[i][j] = source[i][j]; } /** End of __col_mat4Copy() **/ /*****************************************************************************\ @ __col_mat3Copy() ----------------------------------------------------------------------------- description : matrix copy: source => dest input : output : notes : \*****************************************************************************/ void __col_mat3Copy(col_Mat3 source, col_Mat3 dest) { int i, j; for (i = 0; i < 3; i++) for (j = 0; j < 3; j++) dest[i][j] = source[i][j]; } /** End of __col_mat3Copy() **/ /*****************************************************************************\ @ __col_mat4Transpose() ----------------------------------------------------------------------------- description : matrix transpose: (source)^T => dest input : output : notes : source and dest must be distinct! \*****************************************************************************/ void __col_mat4Transpose(col_Mat4 source, col_Mat4 dest) { int i, j; for (i = 0; i < 4; i++) for (j = 0; j < 4; j++) dest[i][j] = source[j][i]; } /** End of __col_mat4Transpose() **/ /*****************************************************************************\ @ __col_mat3Transpose() ----------------------------------------------------------------------------- description : matrix transpose: (source)^T => dest input : output : notes : source and dest must be distinct! \*****************************************************************************/ void __col_mat3Transpose(col_Mat3 source, col_Mat3 dest) { int i, j; for (i = 0; i < 3; i++) for (j = 0; j < 3; j++) dest[i][j] = source[j][i]; } /** End of __col_mat3Transpose() **/ /*****************************************************************************\ @ __col_mat4Print() ----------------------------------------------------------------------------- description : matrix print input : If name is non-null, we also print a small title first. output : notes : \*****************************************************************************/ void __col_mat4Print(col_Mat4 M, char *name) { int i; if (name[0] != '\0') printf("matrix %s\n", name); for (i = 0; i < 4; i++) printf("%+10.6f %+10.6f %+10.6f %+10.6f\n", M[i][0], M[i][1], M[i][2], M[i][3]); } /** End of __col_mat4Print() **/ /*****************************************************************************\ @ __col_mat3Print() ----------------------------------------------------------------------------- description : matrix print input : If name is non-null, we also print a small title first. output : notes : \*****************************************************************************/ void __col_mat3Print(col_Mat3 M, char *name) { int i; if (name[0] != '\0') printf("matrix %s\n", name); for (i = 0; i < 3; i++) printf("%+10.6f %+10.6f %+10.6f\n", M[i][0], M[i][1], M[i][2]); } /** End of __col_mat3Print() **/ /*****************************************************************************\ @ __col_matMultXform() ----------------------------------------------------------------------------- description : transformation matrix multiply: a * b => c input : output : notes : This routine is much faster than the general 4 x 4 matrix multiply above, but only works properly if a and b are SE(3) transformation matrices. c should not point to the same matrix as a or b! \*****************************************************************************/ void __col_matMultXform(col_Mat4 a, col_Mat4 b, col_Mat4 c) { int i, j; /* Rc = Ra Rb */ c[0][0] = a[0][0]*b[0][0] + a[0][1]*b[1][0] + a[0][2]*b[2][0]; c[0][1] = a[0][0]*b[0][1] + a[0][1]*b[1][1] + a[0][2]*b[2][1]; c[0][2] = a[0][0]*b[0][2] + a[0][1]*b[1][2] + a[0][2]*b[2][2]; c[1][0] = a[1][0]*b[0][0] + a[1][1]*b[1][0] + a[1][2]*b[2][0]; c[1][1] = a[1][0]*b[0][1] + a[1][1]*b[1][1] + a[1][2]*b[2][1]; c[1][2] = a[1][0]*b[0][2] + a[1][1]*b[1][2] + a[1][2]*b[2][2]; c[2][0] = a[2][0]*b[0][0] + a[2][1]*b[1][0] + a[2][2]*b[2][0]; c[2][1] = a[2][0]*b[0][1] + a[2][1]*b[1][1] + a[2][2]*b[2][1]; c[2][2] = a[2][0]*b[0][2] + a[2][1]*b[1][2] + a[2][2]*b[2][2]; /* Vc = Ra Vb + Va */ c[0][3] = a[0][0]*b[0][3] + a[0][1]*b[1][3] + a[0][2]*b[2][3] + a[0][3]; c[1][3] = a[1][0]*b[0][3] + a[1][1]*b[1][3] + a[1][2]*b[2][3] + a[1][3]; c[2][3] = a[2][0]*b[0][3] + a[2][1]*b[1][3] + a[2][2]*b[2][3] + a[2][3]; /* the rest */ c[3][0] = c[3][1] = c[3][2] = 0.0; c[3][3] = 1.0; } /** End of __col_matMultXform() **/ /*****************************************************************************\ @ __col_matInvertXform() ----------------------------------------------------------------------------- description : transformation matrix inversion: Inverse(M) => inv input : output : notes : M and inv should not point to the same matrix. We assume M is a transformation matrix; this will not properly invert arbitrary 4x4 matrices. Doesn't work for transformations with scaling \*****************************************************************************/ void __col_matInvertXform(col_Mat4 M, col_Mat4 inv) { /* we invert the rotation part by transposing it */ inv[0][0] = M[0][0]; inv[0][1] = M[1][0]; inv[0][2] = M[2][0]; inv[1][0] = M[0][1]; inv[1][1] = M[1][1]; inv[1][2] = M[2][1]; inv[2][0] = M[0][2]; inv[2][1] = M[1][2]; inv[2][2] = M[2][2]; /* the new displacement vector is given by: d' = -(R^-1) * d */ inv[0][3] = - inv[0][0]*M[0][3] - inv[0][1]*M[1][3] - inv[0][2]*M[2][3]; inv[1][3] = - inv[1][0]*M[0][3] - inv[1][1]*M[1][3] - inv[1][2]*M[2][3]; inv[2][3] = - inv[2][0]*M[0][3] - inv[2][1]*M[1][3] - inv[2][2]*M[2][3]; /* the rest stays the same */ inv[3][0] = inv[3][1] = inv[3][2] = 0.0; inv[3][3] = 1.0; } /** End of __col_matInvertXform() **/ /*****************************************************************************\ @ __col_matInvertArbitraryXform() ----------------------------------------------------------------------------- description : inversion for arbitrary transformation matrix input : output : notes : the above inversion doesn't work if matrices have scaling in them -- this borrowed from PPHIGS implementation at UNC-Chapel Hill \*****************************************************************************/ int __col_matInvertArbitraryXform(col_Mat4 mat, col_Mat4 invmat) { double det; col_Mat4 cofac; cofac[0][0] = (mat[1][1]*mat[2][2] - mat[2][1]*mat[1][2]); cofac[0][1] = -(mat[1][0]*mat[2][2] - mat[2][0]*mat[1][2]); cofac[0][2] = (mat[1][0]*mat[2][1] - mat[2][0]*mat[1][1]); cofac[1][0] = -(mat[0][1]*mat[2][2] - mat[2][1]*mat[0][2]); cofac[1][1] = (mat[0][0]*mat[2][2] - mat[2][0]*mat[0][2]); cofac[1][2] = -(mat[0][0]*mat[2][1] - mat[2][0]*mat[0][1]); cofac[2][0] = (mat[0][1]*mat[1][2] - mat[1][1]*mat[0][2]); cofac[2][1] = -(mat[0][0]*mat[1][2] - mat[1][0]*mat[0][2]); cofac[2][2] = (mat[0][0]*mat[1][1] - mat[1][0]*mat[0][1]); det = mat[0][0]*cofac[0][0] + mat[0][1]*cofac[0][1] + mat[0][2]*cofac[0][2]; if (det == 0.) return -1; invmat[0][0] = cofac[0][0]/det; invmat[0][1] = cofac[1][0]/det; invmat[0][2] = cofac[2][0]/det; invmat[1][0] = cofac[0][1]/det; invmat[1][1] = cofac[1][1]/det; invmat[1][2] = cofac[2][1]/det; invmat[2][0] = cofac[0][2]/det; invmat[2][1] = cofac[1][2]/det; invmat[2][2] = cofac[2][2]/det; invmat[0][3] = -(invmat[0][0]*mat[0][3]+ invmat[0][1]*mat[1][3]+ invmat[0][2]*mat[2][3]); invmat[1][3] = -(invmat[1][0]*mat[0][3]+ invmat[1][1]*mat[1][3]+ invmat[1][2]*mat[2][3]); invmat[2][3] = -(invmat[2][0]*mat[0][3]+ invmat[2][1]*mat[1][3]+ invmat[2][2]*mat[2][3]); invmat[3][0] = invmat[3][1] = invmat[3][2] = 0.; invmat[3][3] = 1.0; return 0; } /** End of __col_matInvertArbitraryXform() **/ /*****************************************************************************\ @ __col_matBuildXform() ----------------------------------------------------------------------------- description : build xformation matrix axes is a string (e.g. "xyz", "yzy", or "zx") which specifies the order of axes rotations. For example, "zx" means rotate about the z-axis, then about the x-axis. All rotations are w.r.t. to the current frame, not a fixed frame. angles[i] specifies the amount of rotation in degrees (!) about the axis specified by the ith character of axes. dx, dy, and dz are the displacement components of the xformation matrix. The matrix is returned in M. input : output : notes : Note that the resulting matrix applies these transformations in order from right to left, for premultiplying a column vector \*****************************************************************************/ void __col_matBuildXform(char *axes, __col_Real angles[], __col_Real dx, __col_Real dy, __col_Real dz, col_Mat4 M) { col_Mat4 Mi, Mold; __col_Real s, c; __col_mat4Copy(__col_mat4IDENTITY, M); for (; *axes; axes++) { __col_mat4Copy(M, Mold); __col_mat4Copy(__col_mat4IDENTITY, Mi); s = sin(*angles * (M_PI / 180.0)); c = cos(*angles * (M_PI / 180.0)); angles++; if (*axes == 'x') { Mi[1][1] = Mi[2][2] = c; Mi[1][2] = -s; Mi[2][1] = s; } else if (*axes == 'y') { Mi[0][0] = Mi[2][2] = c; Mi[2][0] = -s; Mi[0][2] = s; } else { Mi[0][0] = Mi[1][1] = c; Mi[0][1] = -s; Mi[1][0] = s; } __col_mat4Mult(Mold, Mi, M); } M[0][3] = dx; M[1][3] = dy; M[2][3] = dz; M[3][0] = M[3][1] = M[3][2] = 0.0; M[3][3] = 1.0; } /** End of __col_matBuildXform() **/ /* ============================================================================= xformation stuff The following routines transform points and vectors using transformation matrices. Points and vectors are both elements of R^3, but are transformed differently. Points are homogenized prior to transforming, meaning that a 1 is added as a fourth coordinate, before multiplying by the 4x4 transformation matrix. For vectors, on the other hand, the fourth coordinate is 0. The fourth coordinate of the transformed point or vector is never computed, since it is always 1 or 0 respectively, and this value is implicitly assumed. ============================================================================= */ /*****************************************************************************\ @ __col_xformPoint() ----------------------------------------------------------------------------- description : transform a point: M * (p 1) => (p2 1) input : output : notes : \*****************************************************************************/ void __col_xformPoint(col_Mat4 M, col_Vect3 p, col_Vect3 p2) { int i; for (i = 0; i < 3; i++) p2[i] = M[i][0] * p[0] + M[i][1] * p[1] + M[i][2] * p[2] + M[i][3]; } /** End of __col_xformPoint() **/ /*****************************************************************************\ @ __col_xformVect() ----------------------------------------------------------------------------- description : transform a vector: M * (v 0) => (v2 0) input : output : notes : \*****************************************************************************/ void __col_xformVect(col_Mat4 M, col_Vect3 v, col_Vect3 v2) { int i; for (i = 0; i < 3; i++) v2[i] = M[i][0] * v[0] + M[i][1] * v[1] + M[i][2] * v[2]; } /** End of __col_xformVect() **/ /*****************************************************************************\ @ __col_xform4() ----------------------------------------------------------------------------- description : transform a 4-vector: M * x => x2 This routine does a full transformation of a vector x in R^4. Unlike, xformPoint and xformVect, no value is implictly assumed for the fourth coordinate of x; whatever value is there is actually used. Also, all four components of the resultant vector are computed, not just the first three. input : output : notes : \*****************************************************************************/ void __col_xform4(col_Mat4 M, col_Vect4 x, col_Vect4 x2) { int i; for (i = 0; i < 4; i++) x2[i] = M[i][0] * x[0] + M[i][1] * x[1] + M[i][2] * x[2] + M[i][3] * x[3]; } /** End of __col_xform4() **/ /*****************************************************************************\ @ __col_xform3() ----------------------------------------------------------------------------- description : transform a 3-vector: M * x => x2 This routine does a full transformation of a vector x in R^3. input : output : notes : \*****************************************************************************/ void __col_xform3(col_Mat3 M, col_Vect3 x, col_Vect3 x2) { int i; for (i = 0; i < 3; i++) x2[i] = M[i][0] * x[0] + M[i][1] * x[1] + M[i][2] * x[2]; } /** End of __col_xform3() **/ /* ============================================================================= vector stuff The following operations manipulate vectors. The vectors are assumed to be in R^3. ============================================================================= */ /*****************************************************************************\ @ __col_vectCopy() ----------------------------------------------------------------------------- description : vector copy: src => dest input : output : notes : \*****************************************************************************/ void __col_vectCopy(col_Vect3 src, col_Vect3 dest) { dest[0] = src[0]; dest[1] = src[1]; dest[2] = src[2]; } /** End of __col_vectCopy() **/ /*****************************************************************************\ @ __col_vectAdd() ----------------------------------------------------------------------------- description : vector addition: a + b => c input : output : notes : \*****************************************************************************/ void __col_vectAdd(col_Vect3 a, col_Vect3 b, col_Vect3 c) { c[0] = a[0] + b[0]; c[1] = a[1] + b[1]; c[2] = a[2] + b[2]; } /** End of __col_vectAdd() **/ /*****************************************************************************\ @ __col_vectSub() ----------------------------------------------------------------------------- description : vector subtraction: a - b => c input : output : notes : \*****************************************************************************/ void __col_vectSub(col_Vect3 a, col_Vect3 b, col_Vect3 c) { c[0] = a[0] - b[0]; c[1] = a[1] - b[1]; c[2] = a[2] - b[2]; } /** End of __col_vectSub() **/ /*****************************************************************************\ @ __col_vectNeg() ----------------------------------------------------------------------------- description : vector negation: -src => dest input : output : notes : \*****************************************************************************/ void __col_vectNeg(col_Vect3 src, col_Vect3 dest) { dest[0] = - src[0]; dest[1] = - src[1]; dest[2] = - src[2]; } /** End of __col_vectNeg() **/ /*****************************************************************************\ @ __col_vectScale() ----------------------------------------------------------------------------- description : vector scaling: k * src => dest input : output : notes : \*****************************************************************************/ void __col_vectScale(col_Vect3 src, col_Vect3 dest, __col_Real k) { dest[0] = k * src[0]; dest[1] = k * src[1]; dest[2] = k * src[2]; } /** End of __col_vectScale() **/ /*****************************************************************************\ @ __col_vectNormalize() ----------------------------------------------------------------------------- description : vector normalize: src / |src| => dest input : output : notes : \*****************************************************************************/ void __col_vectNormalize(col_Vect3 src, col_Vect3 dest) { __col_Real l; l = __col_vectNorm(src); dest[0] = src[0] / l; dest[1] = src[1] / l; dest[2] = src[2] / l; } /** End of __col_vectNormalize() **/ /*****************************************************************************\ @ __col_vectNorm() ----------------------------------------------------------------------------- description : compute vector norm input : output : notes : \*****************************************************************************/ __col_Real __col_vectNorm(col_Vect3 v) { return sqrt((double) ( sqr(v[0]) + sqr(v[1]) + sqr(v[2]) ) ); } /** End of __col_vectNorm() **/ /*****************************************************************************\ @ __col_vectEqual() ----------------------------------------------------------------------------- description : strict vector equality input : output : Return true if the vectors are exactly equal, else return false. notes : \*****************************************************************************/ int __col_vectEqual(col_Vect3 a, col_Vect3 b) { return (a[0] == b[0] && a[1] == b[1] && a[2] == b[2]); } /** End of __col_vectEqual() **/ /*****************************************************************************\ @ __col_vectDotProd() ----------------------------------------------------------------------------- description : vector dot product: return a . b input : output : notes : \*****************************************************************************/ __col_Real __col_vectDotProd(col_Vect3 a, col_Vect3 b) { return a[0] * b[0] + a[1] * b[1] + a[2] * b[2]; } /** End of __col_vectDotProd() **/ /*****************************************************************************\ @ __col_vectXprod() ----------------------------------------------------------------------------- description : vector cross product: a x b => c input : output : notes : c should not point to the same vector as a or b! \*****************************************************************************/ void __col_vectXprod(col_Vect3 a, col_Vect3 b, col_Vect3 c) { 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]; } /** End of __col_vectXprod() **/ /* ============================================================================= misc. stuff The following are miscellaneous operations for points, vectors, etc. ============================================================================= */ /*****************************************************************************\ @ __col_planeDist() ----------------------------------------------------------------------------- description : plane distance Compute the distance from a point to a plane. The plane is specified by four coefficients, i.e. the plane equation is plane[0] * x + plane[1] * y + plane[2] * z + plane[3] = 0. We assume that the first three plane coordinates form a unit vector (the unit normal to the plane). The "distance" returned is actually a signed distance. That is, if the point lies on the side of the plane to which the unit normal points, the result is positive. If the point lies on the other side, the distance is negative. input : output : notes : \*****************************************************************************/ __col_Real __col_planeDist(col_Vect4 plane, col_Vect3 point) { return plane[0] * point[0] + plane[1] * point[1] + plane[2] * point[2] + plane[3]; } /** End of __col_planeDist() **/ /*****************************************************************************\ @ __col_displacePoint() ----------------------------------------------------------------------------- description : displace point: point + lambda * vect => result Compute the point which is displaced (lambda * vect) from point. input : output : notes : \*****************************************************************************/ void __col_displacePoint(col_Vect3 point, col_Vect3 vect, __col_Real lambda, col_Vect3 result) { result[0] = point[0] + lambda * vect[0]; result[1] = point[1] + lambda * vect[1]; result[2] = point[2] + lambda * vect[2]; } /** End of __col_displacePoint() **/ /*****************************************************************************\ \*****************************************************************************/