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));
added
removed
tests/bullet/Extras/glui/algebra3.cpp
