1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
|
/************************************************************************
2x2 Matrix class
$Id: mat2.cxx 427 2004-09-27 04:45:31Z garland $
************************************************************************/
#include <gfx/gfx.h>
#include <gfx/mat2.h>
namespace gfx
{
Mat2 Mat2::I() { return Mat2(1,0, 0,1); }
Mat2 &Mat2::diag(double d)
{
row[0][0] = d; row[0][1] = 0;
row[1][0] = 0; row[1][1] = d;
return *this;
}
Mat2 operator*(const Mat2 &n, const Mat2& m)
{
Mat2 A;
int i,j;
for(i=0;i<2;i++)
for(j=0;j<2;j++)
A(i,j) = n[i]*m.col(j);
return A;
}
double invert(Mat2 &inv, const Mat2 &m)
{
double d = det(m);
if( d==0.0 )
return 0.0;
inv(0, 0) = m(1,1)/d;
inv(0, 1) = -m(0,1)/d;
inv(1, 0) = -m(1,0)/d;
inv(1, 1) = m(0,0)/d;
return d;
}
bool eigenvalues(const Mat2& M, Vec2& evals)
{
double B = -M(0,0)-M(1,1);
double C = det(M);
double dis = B*B - 4.0*C;
if( dis<FEQ_EPS )
return false;
else
{
double s = sqrt(dis);
evals[0] = 0.5*(-B + s);
evals[1] = 0.5*(-B - s);
return true;
}
}
bool eigenvectors(const Mat2& M, const Vec2& evals, Vec2 evecs[2])
{
evecs[0] = Vec2(-M(0,1), M(0,0)-evals[0]);
evecs[1] = Vec2(-M(0,1), M(0,0)-evals[1]);
unitize(evecs[0]);
unitize(evecs[1]);
return true;
}
bool eigen(const Mat2& M, Vec2& evals, Vec2 evecs[2])
{
bool result = eigenvalues(M, evals);
if( result )
eigenvectors(M, evals, evecs);
return result;
}
} // namespace gfx
|
