3 algebra3.cpp, algebra3.h - C++ Vector and Matrix Algebra routines
5 GLUI User Interface Toolkit (LGPL)
6 Copyright (c) 1998 Paul Rademacher
8 WWW: http://sourceforge.net/projects/glui/
9 Forums: http://sourceforge.net/forum/?group_id=92496
11 This library is free software; you can redistribute it and/or
12 modify it under the terms of the GNU Lesser General Public
13 License as published by the Free Software Foundation; either
14 version 2.1 of the License, or (at your option) any later version.
16 This library is distributed in the hope that it will be useful,
17 but WITHOUT ANY WARRANTY; without even the implied warranty of
18 MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
19 Lesser General Public License for more details.
21 You should have received a copy of the GNU Lesser General Public
22 License along with this library; if not, write to the Free Software
23 Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
27 /**************************************************************************
29 There are three vector classes and two matrix classes: vec2, vec3,
32 All the standard arithmetic operations are defined, with '*'
33 for dot product of two vectors and multiplication of two matrices,
34 and '^' for cross product of two vectors.
36 Additional functions include length(), normalize(), homogenize for
37 vectors, and print(), set(), apply() for all classes.
39 There is a function transpose() for matrices, but note that it
40 does not actually change the matrix,
42 When multiplied with a matrix, a vector is treated as a row vector
43 if it precedes the matrix (v*M), and as a column vector if it
44 follows the matrix (M*v).
46 Matrices are stored in row-major form.
48 A vector of one dimension (2d, 3d, or 4d) can be cast to a vector
49 of a higher or lower dimension. If casting to a higher dimension,
50 the new component is set by default to 1.0, unless a value is
52 vec3 a(1.0, 2.0, 3.0 );
53 vec4 b( a, 4.0 ); // now b == {1.0, 2.0, 3.0, 4.0};
54 When casting to a lower dimension, the vector is homogenized in
55 the lower dimension. E.g., if a 4d {X,Y,Z,W} is cast to 3d, the
56 resulting vector is {X/W, Y/W, Z/W}. It is up to the user to
57 insure the fourth component is not zero before casting.
59 There are also the following function for building matrices:
60 identity2D(), translation2D(), rotation2D(),
61 scaling2D(), identity3D(), translation3D(),
62 rotation3D(), rotation3Drad(), scaling3D(),
66 ---------------------------------------------------------------------
68 Author: Jean-Francois DOUEg
69 Revised: Paul Rademacher
70 Version 3.2 - Feb 1998
71 Revised: Nigel Stewart (GLUI Code Cleaning)
73 **************************************************************************/
76 #include "glui_internal.h"
79 #ifdef VEC_ERROR_FATAL
81 #define VEC_ERROR(E) { printf( "VERROR %s\n", E ); exit(1); }
85 #define VEC_ERROR(E) { printf( "VERROR %s\n", E ); }
89 /****************************************************************
91 * vec2 Member functions *
93 ****************************************************************/
95 /******************** vec2 CONSTRUCTORS ********************/
102 vec2::vec2(float x, float y)
108 vec2::vec2(const vec2 &v)
114 vec2::vec2(const vec3 &v) // it is up to caller to avoid divide-by-zero
116 n[VX] = v.n[VX]/v.n[VZ];
117 n[VY] = v.n[VY]/v.n[VZ];
120 vec2::vec2(const vec3 &v, int dropAxis)
124 case VX: n[VX] = v.n[VY]; n[VY] = v.n[VZ]; break;
125 case VY: n[VX] = v.n[VX]; n[VY] = v.n[VZ]; break;
126 default: n[VX] = v.n[VX]; n[VY] = v.n[VY]; break;
130 /******************** vec2 ASSIGNMENT OPERATORS ******************/
132 vec2 & vec2::operator=(const vec2 &v)
139 vec2 & vec2::operator+=(const vec2 &v)
146 vec2 & vec2::operator-=(const vec2 &v)
153 vec2 &vec2::operator*=(float d)
160 vec2 &vec2::operator/=(float d)
162 float d_inv = 1.0f/d;
168 float &vec2::operator[](int i)
170 if (i < VX || i > VY)
171 //VEC_ERROR("vec2 [] operator: illegal access; index = " << i << '\n')
172 VEC_ERROR("vec2 [] operator: illegal access" );
176 const float &vec2::operator[](int i) const
178 if (i < VX || i > VY)
179 //VEC_ERROR("vec2 [] operator: illegal access; index = " << i << '\n')
180 VEC_ERROR("vec2 [] operator: illegal access" );
185 /******************** vec2 SPECIAL FUNCTIONS ********************/
187 float vec2::length() const
189 return (float) sqrt(length2());
192 float vec2::length2() const
194 return n[VX]*n[VX] + n[VY]*n[VY];
197 vec2 &vec2::normalize() // it is up to caller to avoid divide-by-zero
203 vec2 &vec2::apply(V_FCT_PTR fct)
205 n[VX] = (*fct)(n[VX]);
206 n[VY] = (*fct)(n[VY]);
210 void vec2::set( float x, float y )
212 n[VX] = x; n[VY] = y;
215 /******************** vec2 FRIENDS *****************************/
217 vec2 operator-(const vec2 &a)
219 return vec2(-a.n[VX],-a.n[VY]);
222 vec2 operator+(const vec2 &a, const vec2& b)
224 return vec2(a.n[VX]+b.n[VX], a.n[VY]+b.n[VY]);
227 vec2 operator-(const vec2 &a, const vec2& b)
229 return vec2(a.n[VX]-b.n[VX], a.n[VY]-b.n[VY]);
232 vec2 operator*(const vec2 &a, float d)
234 return vec2(d*a.n[VX], d*a.n[VY]);
237 vec2 operator*(float d, const vec2 &a)
242 vec2 operator*(const mat3 &a, const vec2 &v)
246 av.n[VX] = a.v[0].n[VX]*v.n[VX] + a.v[0].n[VY]*v.n[VY] + a.v[0].n[VZ];
247 av.n[VY] = a.v[1].n[VX]*v.n[VX] + a.v[1].n[VY]*v.n[VY] + a.v[1].n[VZ];
248 av.n[VZ] = a.v[2].n[VX]*v.n[VX] + a.v[2].n[VY]*v.n[VY] + a.v[2].n[VZ];
253 vec2 operator*(const vec2 &v, const mat3 &a)
255 return a.transpose() * v;
258 vec3 operator*(const mat3 &a, const vec3 &v)
262 av.n[VX] = a.v[0].n[VX]*v.n[VX] + a.v[0].n[VY]*v.n[VY] + a.v[0].n[VZ]*v.n[VZ];
263 av.n[VY] = a.v[1].n[VX]*v.n[VX] + a.v[1].n[VY]*v.n[VY] + a.v[1].n[VZ]*v.n[VZ];
264 av.n[VZ] = a.v[2].n[VX]*v.n[VX] + a.v[2].n[VY]*v.n[VY] + a.v[2].n[VZ]*v.n[VZ];
269 vec3 operator*(const vec3 &v, const mat3 &a)
271 return a.transpose() * v;
274 float operator*(const vec2 &a, const vec2 &b)
276 return a.n[VX]*b.n[VX] + a.n[VY]*b.n[VY];
279 vec2 operator/(const vec2 &a, float d)
281 float d_inv = 1.0f/d;
282 return vec2(a.n[VX]*d_inv, a.n[VY]*d_inv);
285 vec3 operator^(const vec2 &a, const vec2 &b)
287 return vec3(0.0, 0.0, a.n[VX] * b.n[VY] - b.n[VX] * a.n[VY]);
290 int operator==(const vec2 &a, const vec2 &b)
292 return (a.n[VX] == b.n[VX]) && (a.n[VY] == b.n[VY]);
295 int operator!=(const vec2 &a, const vec2 &b)
300 /*ostream& operator << (ostream& s, vec2& v)
301 { return s << "| " << v.n[VX] << ' ' << v.n[VY] << " |"; }
304 /*istream& operator >> (istream& s, vec2& v) {
310 // The vectors can be formatted either as x y or | x y |
312 s >> v_tmp[VX] >> v_tmp[VY];
313 while (s >> c && isspace(c)) ;
319 s >> v_tmp[VX] >> v_tmp[VY];
327 void swap(vec2 &a, vec2 &b)
334 vec2 min_vec(const vec2 &a, const vec2 &b)
336 return vec2(MIN(a.n[VX], b.n[VX]), MIN(a.n[VY], b.n[VY]));
339 vec2 max_vec(const vec2 &a, const vec2 &b)
341 return vec2(MAX(a.n[VX], b.n[VX]), MAX(a.n[VY], b.n[VY]));
344 vec2 prod(const vec2 &a, const vec2 &b)
346 return vec2(a.n[VX] * b.n[VX], a.n[VY] * b.n[VY]);
349 /****************************************************************
351 * vec3 Member functions *
353 ****************************************************************/
359 n[VX] = n[VY] = n[VZ] = 0.0;
362 vec3::vec3(float x, float y, float z)
369 vec3::vec3(const vec3 &v)
371 n[VX] = v.n[VX]; n[VY] = v.n[VY]; n[VZ] = v.n[VZ];
374 vec3::vec3(const vec2 &v)
381 vec3::vec3(const vec2 &v, float d)
388 vec3::vec3(const vec4 &v) // it is up to caller to avoid divide-by-zero
390 n[VX] = v.n[VX] / v.n[VW];
391 n[VY] = v.n[VY] / v.n[VW];
392 n[VZ] = v.n[VZ] / v.n[VW];
395 vec3::vec3(const vec4 &v, int dropAxis)
399 case VX: n[VX] = v.n[VY]; n[VY] = v.n[VZ]; n[VZ] = v.n[VW]; break;
400 case VY: n[VX] = v.n[VX]; n[VY] = v.n[VZ]; n[VZ] = v.n[VW]; break;
401 case VZ: n[VX] = v.n[VX]; n[VY] = v.n[VY]; n[VZ] = v.n[VW]; break;
402 default: n[VX] = v.n[VX]; n[VY] = v.n[VY]; n[VZ] = v.n[VZ]; break;
407 // ASSIGNMENT OPERATORS
409 vec3 &vec3::operator=(const vec3 &v)
417 vec3 &vec3::operator+=(const vec3 &v)
425 vec3 &vec3::operator-=(const vec3& v)
433 vec3 &vec3::operator*=(float d)
441 vec3 &vec3::operator/=(float d)
443 float d_inv = 1.0f/d;
450 float &vec3::operator[](int i)
452 if (i < VX || i > VZ)
453 //VEC_ERROR("vec3 [] operator: illegal access; index = " << i << '\n')
454 VEC_ERROR("vec3 [] operator: illegal access" );
459 const float &vec3::operator[](int i) const
461 if (i < VX || i > VZ)
462 //VEC_ERROR("vec3 [] operator: illegal access; index = " << i << '\n')
463 VEC_ERROR("vec3 [] operator: illegal access" );
470 float vec3::length() const
472 return (float) sqrt(length2());
475 float vec3::length2() const
477 return n[VX]*n[VX] + n[VY]*n[VY] + n[VZ]*n[VZ];
480 vec3 &vec3::normalize() // it is up to caller to avoid divide-by-zero
486 vec3 &vec3::homogenize(void) // it is up to caller to avoid divide-by-zero
494 vec3 &vec3::apply(V_FCT_PTR fct)
496 n[VX] = (*fct)(n[VX]);
497 n[VY] = (*fct)(n[VY]);
498 n[VZ] = (*fct)(n[VZ]);
502 void vec3::set(float x, float y, float z) // set vector
509 void vec3::print(FILE *file, const char *name) const // print vector to a file
511 fprintf( file, "%s: <%f, %f, %f>\n", name, n[VX], n[VY], n[VZ] );
516 vec3 operator-(const vec3 &a)
518 return vec3(-a.n[VX],-a.n[VY],-a.n[VZ]);
521 vec3 operator+(const vec3 &a, const vec3 &b)
523 return vec3(a.n[VX]+ b.n[VX], a.n[VY] + b.n[VY], a.n[VZ] + b.n[VZ]);
526 vec3 operator-(const vec3 &a, const vec3 &b)
528 return vec3(a.n[VX]-b.n[VX], a.n[VY]-b.n[VY], a.n[VZ]-b.n[VZ]);
531 vec3 operator*(const vec3 &a, float d)
533 return vec3(d*a.n[VX], d*a.n[VY], d*a.n[VZ]);
536 vec3 operator*(float d, const vec3 &a)
541 vec3 operator*(const mat4 &a, const vec3 &v)
546 vec3 operator*(const vec3 &v, mat4 &a)
548 return a.transpose()*v;
551 float operator*(const vec3 &a, const vec3 &b)
553 return a.n[VX]*b.n[VX] + a.n[VY]*b.n[VY] + a.n[VZ]*b.n[VZ];
556 vec3 operator/(const vec3 &a, float d)
558 float d_inv = 1.0f/d;
559 return vec3(a.n[VX]*d_inv, a.n[VY]*d_inv, a.n[VZ]*d_inv);
562 vec3 operator^(const vec3 &a, const vec3 &b)
565 vec3(a.n[VY]*b.n[VZ] - a.n[VZ]*b.n[VY],
566 a.n[VZ]*b.n[VX] - a.n[VX]*b.n[VZ],
567 a.n[VX]*b.n[VY] - a.n[VY]*b.n[VX]);
570 int operator==(const vec3 &a, const vec3 &b)
572 return (a.n[VX] == b.n[VX]) && (a.n[VY] == b.n[VY]) && (a.n[VZ] == b.n[VZ]);
575 int operator!=(const vec3 &a, const vec3 &b)
580 /*ostream& operator << (ostream& s, vec3& v)
581 { return s << "| " << v.n[VX] << ' ' << v.n[VY] << ' ' << v.n[VZ] << " |"; }
583 istream& operator >> (istream& s, vec3& v) {
589 // The vectors can be formatted either as x y z or | x y z |
591 s >> v_tmp[VX] >> v_tmp[VY] >> v_tmp[VZ];
592 while (s >> c && isspace(c)) ;
598 s >> v_tmp[VX] >> v_tmp[VY] >> v_tmp[VZ];
606 void swap(vec3 &a, vec3 &b)
613 vec3 min_vec(const vec3 &a, const vec3 &b)
616 MIN(a.n[VX], b.n[VX]),
617 MIN(a.n[VY], b.n[VY]),
618 MIN(a.n[VZ], b.n[VZ]));
621 vec3 max_vec(const vec3 &a, const vec3 &b)
624 MAX(a.n[VX], b.n[VX]),
625 MAX(a.n[VY], b.n[VY]),
626 MAX(a.n[VZ], b.n[VZ]));
629 vec3 prod(const vec3 &a, const vec3 &b)
631 return vec3(a.n[VX]*b.n[VX], a.n[VY]*b.n[VY], a.n[VZ]*b.n[VZ]);
634 /****************************************************************
636 * vec4 Member functions *
638 ****************************************************************/
644 n[VX] = n[VY] = n[VZ] = 0.0;
648 vec4::vec4(float x, float y, float z, float w)
656 vec4::vec4(const vec4 &v)
664 vec4::vec4(const vec3 &v)
672 vec4::vec4(const vec3 &v, float d)
680 // ASSIGNMENT OPERATORS
682 vec4 &vec4::operator=(const vec4 &v)
691 vec4 &vec4::operator+=(const vec4 &v)
700 vec4 &vec4::operator-=(const vec4 &v)
709 vec4 &vec4::operator*=(float d)
718 vec4 &vec4::operator/=(float d)
720 float d_inv = 1.0f/d;
728 float &vec4::operator[](int i)
730 if (i < VX || i > VW)
731 //VEC_ERROR("vec4 [] operator: illegal access; index = " << i << '\n')
732 VEC_ERROR("vec4 [] operator: illegal access" );
737 const float &vec4::operator[](int i) const
739 if (i < VX || i > VW)
740 //VEC_ERROR("vec4 [] operator: illegal access; index = " << i << '\n')
741 VEC_ERROR("vec4 [] operator: illegal access" );
748 float vec4::length() const
750 return (float) sqrt(length2());
753 float vec4::length2() const
755 return n[VX]*n[VX] + n[VY]*n[VY] + n[VZ]*n[VZ] + n[VW]*n[VW];
758 vec4 &vec4::normalize() // it is up to caller to avoid divide-by-zero
764 vec4 &vec4::homogenize() // it is up to caller to avoid divide-by-zero
773 vec4 &vec4::apply(V_FCT_PTR fct)
775 n[VX] = (*fct)(n[VX]);
776 n[VY] = (*fct)(n[VY]);
777 n[VZ] = (*fct)(n[VZ]);
778 n[VW] = (*fct)(n[VW]);
782 void vec4::print(FILE *file, const char *name) const // print vector to a file
784 fprintf( file, "%s: <%f, %f, %f, %f>\n", name, n[VX], n[VY], n[VZ], n[VW]);
787 void vec4::set(float x, float y, float z, float a)
798 vec4 operator-(const vec4 &a)
800 return vec4(-a.n[VX],-a.n[VY],-a.n[VZ],-a.n[VW]);
803 vec4 operator+(const vec4 &a, const vec4 &b)
812 vec4 operator-(const vec4 &a, const vec4 &b)
821 vec4 operator*(const vec4 &a, float d)
823 return vec4(d*a.n[VX], d*a.n[VY], d*a.n[VZ], d*a.n[VW]);
826 vec4 operator*(float d, const vec4 &a)
831 vec4 operator*(const mat4 &a, const vec4 &v)
834 a.v[i].n[0]*v.n[VX] + \
835 a.v[i].n[1]*v.n[VY] + \
836 a.v[i].n[2]*v.n[VZ] + \
839 return vec4(ROWCOL(0), ROWCOL(1), ROWCOL(2), ROWCOL(3));
844 vec4 operator*(const vec4 &v, const mat4 &a)
846 return a.transpose()*v;
849 float operator*(const vec4 &a, const vec4 &b)
858 vec4 operator/(const vec4 &a, float d)
860 float d_inv = 1.0f/d;
868 int operator==(const vec4 &a, const vec4 &b)
871 (a.n[VX] == b.n[VX]) &&
872 (a.n[VY] == b.n[VY]) &&
873 (a.n[VZ] == b.n[VZ]) &&
874 (a.n[VW] == b.n[VW]);
877 int operator!=(const vec4 &a, const vec4 &b)
882 /*ostream& operator << (ostream& s, vec4& v)
883 { return s << "| " << v.n[VX] << ' ' << v.n[VY] << ' ' << v.n[VZ] << ' '
884 << v.n[VW] << " |"; }
886 istream& operator >> (istream& s, vec4& v) {
892 // The vectors can be formatted either as x y z w or | x y z w |
894 s >> v_tmp[VX] >> v_tmp[VY] >> v_tmp[VZ] >> v_tmp[VW];
895 while (s >> c && isspace(c)) ;
901 s >> v_tmp[VX] >> v_tmp[VY] >> v_tmp[VZ] >> v_tmp[VW];
909 void swap(vec4 &a, vec4 &b)
916 vec4 min_vec(const vec4 &a, const vec4 &b)
919 MIN(a.n[VX], b.n[VX]),
920 MIN(a.n[VY], b.n[VY]),
921 MIN(a.n[VZ], b.n[VZ]),
922 MIN(a.n[VW], b.n[VW]));
925 vec4 max_vec(const vec4 &a, const vec4 &b)
928 MAX(a.n[VX], b.n[VX]),
929 MAX(a.n[VY], b.n[VY]),
930 MAX(a.n[VZ], b.n[VZ]),
931 MAX(a.n[VW], b.n[VW]));
934 vec4 prod(const vec4 &a, const vec4 &b)
943 /****************************************************************
945 * mat3 member functions *
947 ****************************************************************/
953 *this = identity2D();
956 mat3::mat3(const vec3 &v0, const vec3 &v1, const vec3 &v2)
961 mat3::mat3(const mat3 &m)
968 // ASSIGNMENT OPERATORS
970 mat3 &mat3::operator=(const mat3 &m)
978 mat3 &mat3::operator+=(const mat3& m)
986 mat3 &mat3::operator-=(const mat3& m)
994 mat3 &mat3::operator*=(float d)
1002 mat3 &mat3::operator/=(float d)
1010 vec3 &mat3::operator[](int i)
1012 if (i < VX || i > VZ)
1013 //VEC_ERROR("mat3 [] operator: illegal access; index = " << i << '\n')
1014 VEC_ERROR("mat3 [] operator: illegal access" );
1019 const vec3 &mat3::operator[](int i) const
1021 if (i < VX || i > VZ)
1022 //VEC_ERROR("mat3 [] operator: illegal access; index = " << i << '\n')
1023 VEC_ERROR("mat3 [] operator: illegal access" );
1028 void mat3::set(const vec3 &v0, const vec3 &v1, const vec3 &v2)
1035 // SPECIAL FUNCTIONS
1037 mat3 mat3::transpose() const
1040 vec3(v[0][0], v[1][0], v[2][0]),
1041 vec3(v[0][1], v[1][1], v[2][1]),
1042 vec3(v[0][2], v[1][2], v[2][2]));
1045 mat3 mat3::inverse() const // Gauss-Jordan elimination with partial pivoting
1047 mat3 a(*this); // As a evolves from original mat into identity
1048 mat3 b(identity2D()); // b evolves from identity into inverse(a)
1051 // Loop over cols of a from left to right, eliminating above and below diag
1052 for (j=0; j<3; j++) // Find largest pivot in column j among rows j..2
1054 i1 = j; // Row with largest pivot candidate
1055 for (i=j+1; i<3; i++)
1056 if (fabs(a.v[i].n[j]) > fabs(a.v[i1].n[j]))
1059 // Swap rows i1 and j in a and b to put pivot on diagonal
1060 swap(a.v[i1], a.v[j]);
1061 swap(b.v[i1], b.v[j]);
1063 // Scale row j to have a unit diagonal
1064 if (a.v[j].n[j]==0.)
1065 VEC_ERROR("mat3::inverse: singular matrix; can't invert\n");
1067 b.v[j] /= a.v[j].n[j];
1068 a.v[j] /= a.v[j].n[j];
1070 // Eliminate off-diagonal elems in col j of a, doing identical ops to b
1074 b.v[i] -= a.v[i].n[j]*b.v[j];
1075 a.v[i] -= a.v[i].n[j]*a.v[j];
1082 mat3 &mat3::apply(V_FCT_PTR fct)
1093 mat3 operator-(const mat3 &a)
1095 return mat3(-a.v[0], -a.v[1], -a.v[2]);
1098 mat3 operator+(const mat3 &a, const mat3 &b)
1100 return mat3(a.v[0]+b.v[0], a.v[1]+b.v[1], a.v[2]+b.v[2]);
1103 mat3 operator-(const mat3 &a, const mat3 &b)
1105 return mat3(a.v[0]-b.v[0], a.v[1]-b.v[1], a.v[2]-b.v[2]);
1108 mat3 operator*(const mat3 &a, const mat3 &b)
1110 #define ROWCOL(i, j) \
1111 a.v[i].n[0]*b.v[0][j] + a.v[i].n[1]*b.v[1][j] + a.v[i].n[2]*b.v[2][j]
1114 vec3(ROWCOL(0,0), ROWCOL(0,1), ROWCOL(0,2)),
1115 vec3(ROWCOL(1,0), ROWCOL(1,1), ROWCOL(1,2)),
1116 vec3(ROWCOL(2,0), ROWCOL(2,1), ROWCOL(2,2)));
1121 mat3 operator*(const mat3 &a, float d)
1123 return mat3(a.v[0]*d, a.v[1]*d, a.v[2]*d);
1126 mat3 operator*(float d, const mat3 &a)
1131 mat3 operator/(const mat3 &a, float d)
1133 return mat3(a.v[0]/d, a.v[1]/d, a.v[2]/d);
1136 int operator==(const mat3 &a, const mat3 &b)
1139 (a.v[0] == b.v[0]) &&
1140 (a.v[1] == b.v[1]) &&
1144 int operator!=(const mat3 &a, const mat3 &b)
1149 /*ostream& operator << (ostream& s, mat3& m)
1150 { return s << m.v[VX] << '\n' << m.v[VY] << '\n' << m.v[VZ]; }
1152 istream& operator >> (istream& s, mat3& m) {
1155 s >> m_tmp[VX] >> m_tmp[VY] >> m_tmp[VZ];
1162 void swap(mat3 &a, mat3 &b)
1169 void mat3::print(FILE *file, const char *name) const
1173 fprintf( stderr, "%s:\n", name );
1175 for( i = 0; i < 3; i++ )
1177 fprintf( stderr, " " );
1178 for( j = 0; j < 3; j++ )
1180 fprintf( stderr, "%f ", v[i][j] );
1182 fprintf( stderr, "\n" );
1188 /****************************************************************
1190 * mat4 member functions *
1192 ****************************************************************/
1198 *this = identity3D();
1201 mat4::mat4(const vec4& v0, const vec4& v1, const vec4& v2, const vec4& v3)
1209 mat4::mat4(const mat4 &m)
1218 float a00, float a01, float a02, float a03,
1219 float a10, float a11, float a12, float a13,
1220 float a20, float a21, float a22, float a23,
1221 float a30, float a31, float a32, float a33 )
1223 v[0][0] = a00; v[0][1] = a01; v[0][2] = a02; v[0][3] = a03;
1224 v[1][0] = a10; v[1][1] = a11; v[1][2] = a12; v[1][3] = a13;
1225 v[2][0] = a20; v[2][1] = a21; v[2][2] = a22; v[2][3] = a23;
1226 v[3][0] = a30; v[3][1] = a31; v[3][2] = a32; v[3][3] = a33;
1229 // ASSIGNMENT OPERATORS
1231 mat4 &mat4::operator=(const mat4 &m)
1240 mat4 &mat4::operator+=(const mat4 &m)
1249 mat4 &mat4::operator-=(const mat4 &m)
1258 mat4 &mat4::operator*=(float d)
1267 mat4 &mat4::operator/=(float d)
1276 vec4 &mat4::operator[](int i)
1278 if (i < VX || i > VW)
1279 //VEC_ERROR("mat4 [] operator: illegal access; index = " << i << '\n')
1280 VEC_ERROR("mat4 [] operator: illegal access" );
1284 const vec4 &mat4::operator[](int i) const
1286 if (i < VX || i > VW)
1287 //VEC_ERROR("mat4 [] operator: illegal access; index = " << i << '\n')
1288 VEC_ERROR("mat4 [] operator: illegal access" );
1292 // SPECIAL FUNCTIONS;
1294 mat4 mat4::transpose() const
1297 vec4(v[0][0], v[1][0], v[2][0], v[3][0]),
1298 vec4(v[0][1], v[1][1], v[2][1], v[3][1]),
1299 vec4(v[0][2], v[1][2], v[2][2], v[3][2]),
1300 vec4(v[0][3], v[1][3], v[2][3], v[3][3]));
1303 mat4 mat4::inverse() const // Gauss-Jordan elimination with partial pivoting
1305 mat4 a(*this); // As a evolves from original mat into identity
1306 mat4 b(identity3D()); // b evolves from identity into inverse(a)
1309 // Loop over cols of a from left to right, eliminating above and below diag
1310 for (j=0; j<4; j++) // Find largest pivot in column j among rows j..3
1312 i1 = j; // Row with largest pivot candidate
1313 for (i=j+1; i<4; i++)
1314 if (fabs(a.v[i].n[j]) > fabs(a.v[i1].n[j]))
1317 // Swap rows i1 and j in a and b to put pivot on diagonal
1318 swap(a.v[i1], a.v[j]);
1319 swap(b.v[i1], b.v[j]);
1321 // Scale row j to have a unit diagonal
1322 if (a.v[j].n[j]==0.)
1323 VEC_ERROR("mat4::inverse: singular matrix; can't invert\n");
1325 b.v[j] /= a.v[j].n[j];
1326 a.v[j] /= a.v[j].n[j];
1328 // Eliminate off-diagonal elems in col j of a, doing identical ops to b
1332 b.v[i] -= a.v[i].n[j]*b.v[j];
1333 a.v[i] -= a.v[i].n[j]*a.v[j];
1340 mat4 &mat4::apply(V_FCT_PTR fct)
1349 void mat4::print(FILE *file, const char *name) const
1353 fprintf( stderr, "%s:\n", name );
1355 for( i = 0; i < 4; i++ )
1357 fprintf( stderr, " " );
1358 for( j = 0; j < 4; j++ )
1360 fprintf( stderr, "%f ", v[i][j] );
1362 fprintf( stderr, "\n" );
1366 void mat4::swap_rows(int i, int j)
1375 void mat4::swap_cols(int i, int j)
1391 mat4 operator-(const mat4 &a)
1393 return mat4(-a.v[0],-a.v[1],-a.v[2],-a.v[3]);
1396 mat4 operator+(const mat4 &a, const mat4 &b)
1405 mat4 operator-(const mat4 &a, const mat4 &b)
1414 mat4 operator*(const mat4 &a, const mat4 &b)
1416 #define ROWCOL(i, j) \
1417 a.v[i].n[0]*b.v[0][j] + \
1418 a.v[i].n[1]*b.v[1][j] + \
1419 a.v[i].n[2]*b.v[2][j] + \
1420 a.v[i].n[3]*b.v[3][j]
1423 vec4(ROWCOL(0,0), ROWCOL(0,1), ROWCOL(0,2), ROWCOL(0,3)),
1424 vec4(ROWCOL(1,0), ROWCOL(1,1), ROWCOL(1,2), ROWCOL(1,3)),
1425 vec4(ROWCOL(2,0), ROWCOL(2,1), ROWCOL(2,2), ROWCOL(2,3)),
1426 vec4(ROWCOL(3,0), ROWCOL(3,1), ROWCOL(3,2), ROWCOL(3,3))
1432 mat4 operator*(const mat4 &a, float d)
1434 return mat4(a.v[0]*d, a.v[1]*d, a.v[2]*d, a.v[3]*d);
1437 mat4 operator*(float d, const mat4 &a)
1442 mat4 operator/(const mat4 &a, float d)
1444 return mat4(a.v[0]/d, a.v[1]/d, a.v[2]/d, a.v[3]/d);
1447 int operator==(const mat4 &a, const mat4 &b)
1450 (a.v[0] == b.v[0]) &&
1451 (a.v[1] == b.v[1]) &&
1452 (a.v[2] == b.v[2]) &&
1456 int operator!=(const mat4 &a, const mat4 &b)
1461 /*ostream& operator << (ostream& s, mat4& m)
1462 { return s << m.v[VX] << '\n' << m.v[VY] << '\n' << m.v[VZ] << '\n' << m.v[VW]; }
1464 istream& operator >> (istream& s, mat4& m)
1468 s >> m_tmp[VX] >> m_tmp[VY] >> m_tmp[VZ] >> m_tmp[VW];
1475 void swap(mat4 &a, mat4 &b)
1482 /****************************************************************
1484 * 2D functions and 3D functions *
1486 ****************************************************************/
1491 vec3(1.0, 0.0, 0.0),
1492 vec3(0.0, 1.0, 0.0),
1493 vec3(0.0, 0.0, 1.0));
1496 mat3 translation2D(const vec2 &v)
1499 vec3(1.0, 0.0, v[VX]),
1500 vec3(0.0, 1.0, v[VY]),
1501 vec3(0.0, 0.0, 1.0));
1504 mat3 rotation2D(const vec2 &Center, float angleDeg)
1506 float angleRad = (float) (angleDeg * M_PI / 180.0);
1507 float c = (float) cos(angleRad);
1508 float s = (float) sin(angleRad);
1511 vec3(c, -s, Center[VX] * (1.0f-c) + Center[VY] * s),
1512 vec3(s, c, Center[VY] * (1.0f-c) - Center[VX] * s),
1513 vec3(0.0, 0.0, 1.0));
1516 mat3 scaling2D(const vec2 &scaleVector)
1519 vec3(scaleVector[VX], 0.0, 0.0),
1520 vec3(0.0, scaleVector[VY], 0.0),
1521 vec3(0.0, 0.0, 1.0));
1527 vec4(1.0, 0.0, 0.0, 0.0),
1528 vec4(0.0, 1.0, 0.0, 0.0),
1529 vec4(0.0, 0.0, 1.0, 0.0),
1530 vec4(0.0, 0.0, 0.0, 1.0));
1533 mat4 translation3D(const vec3 &v)
1536 vec4(1.0, 0.0, 0.0, v[VX]),
1537 vec4(0.0, 1.0, 0.0, v[VY]),
1538 vec4(0.0, 0.0, 1.0, v[VZ]),
1539 vec4(0.0, 0.0, 0.0, 1.0));
1542 mat4 rotation3D(const vec3 &Axis, float angleDeg)
1544 float angleRad = (float) (angleDeg * M_PI / 180.0);
1545 float c = (float) cos(angleRad);
1546 float s = (float) sin(angleRad);
1553 vec4(t * axis[VX] * axis[VX] + c,
1554 t * axis[VX] * axis[VY] - s * axis[VZ],
1555 t * axis[VX] * axis[VZ] + s * axis[VY],
1557 vec4(t * axis[VX] * axis[VY] + s * axis[VZ],
1558 t * axis[VY] * axis[VY] + c,
1559 t * axis[VY] * axis[VZ] - s * axis[VX],
1561 vec4(t * axis[VX] * axis[VZ] - s * axis[VY],
1562 t * axis[VY] * axis[VZ] + s * axis[VX],
1563 t * axis[VZ] * axis[VZ] + c,
1565 vec4(0.0, 0.0, 0.0, 1.0));
1568 mat4 rotation3Drad(const vec3 &Axis, float angleRad)
1570 float c = (float) cos(angleRad);
1571 float s = (float) sin(angleRad);
1578 vec4(t * axis[VX] * axis[VX] + c,
1579 t * axis[VX] * axis[VY] - s * axis[VZ],
1580 t * axis[VX] * axis[VZ] + s * axis[VY],
1582 vec4(t * axis[VX] * axis[VY] + s * axis[VZ],
1583 t * axis[VY] * axis[VY] + c,
1584 t * axis[VY] * axis[VZ] - s * axis[VX],
1586 vec4(t * axis[VX] * axis[VZ] - s * axis[VY],
1587 t * axis[VY] * axis[VZ] + s * axis[VX],
1588 t * axis[VZ] * axis[VZ] + c,
1590 vec4(0.0, 0.0, 0.0, 1.0));
1593 mat4 scaling3D(const vec3 &scaleVector)
1596 vec4(scaleVector[VX], 0.0, 0.0, 0.0),
1597 vec4(0.0, scaleVector[VY], 0.0, 0.0),
1598 vec4(0.0, 0.0, scaleVector[VZ], 0.0),
1599 vec4(0.0, 0.0, 0.0, 1.0));
1602 mat4 perspective3D(float d)
1605 vec4(1.0f, 0.0f, 0.0f, 0.0f),
1606 vec4(0.0f, 1.0f, 0.0f, 0.0f),
1607 vec4(0.0f, 0.0f, 1.0f, 0.0f),
1608 vec4(0.0f, 0.0f, 1.0f/d, 0.0f));