Planet
navi homePPSaboutscreenshotsdownloaddevelopmentforum

source: orxonox.OLD/trunk/src/lib/math/matrix.cc @ 5668

Last change on this file since 5668 was 5668, checked in by bensch, 19 years ago

orxonox/trunk: two of 3 eigenvectors

File size: 3.8 KB
Line 
1
2#include <stdio.h>
3#include <math.h>
4#include "matrix.h"
5
6
7Vector Matrix::getEigenValues() const
8{
9  Vector eigVl;
10
11  float a = 0;
12  float b = 0;
13
14  float c[3];
15
16  // c[0] is the determinante of mat
17  c[0] = this->m11 * this->m22 * this->m33 +
18      2* this->m12 * this->m13 * this->m23 -
19      this->m11 * this->m23 * this->m23 -
20      this->m22 * this->m13 * this->m13 -
21      this->m33 * this->m12 * this->m12;
22
23  // c[1] is the trace of a
24  c[1] = this->m11 * this->m22 -
25      this->m12 * this->m12 +
26      this->m11 * this->m33 -
27      this->m13 * this->m13 +
28      this->m22 * this->m33 -
29      this->m23 * this->m23;
30
31  // c[2] is the sum of the diagonal elements
32  c[2] = this->m11 +
33      this->m22 +
34      this->m33;
35
36
37  // Computing the roots:
38  a = (3.0*c[1] - c[2]*c[2]) / 3.0;
39  b = (-2.0*c[2]*c[2]*c[2] + 9.0*c[1]*c[2] - 27.0*c[0]) / 27.0;
40
41  float Q = b*b/4.0 + a*a*a/27.0;
42
43  // 3 distinct Roots
44  if (Q < 0)
45  {
46    printf("good\n");
47    float psi = atan2(sqrt(-Q), -b/2.0);
48    float p = sqrt((b/2.0)*(b/2.0) - Q);
49
50    eigVl.x = c[2]/3.0 + 2 * pow(p, 1/3.0) * cos(psi/3.0);
51    eigVl.y = c[2]/3.0 - pow(p, 1/3.0) * (cos(psi/3.0)
52        + sqrt(3.0) * sin(psi/3.0));
53    eigVl.z = c[2]/3.0 - pow(p, 1/3.0) * (cos(psi/3.0)
54        - sqrt(3.0) * sin(psi/3.0));
55
56  }
57  // 2 Distinct Roots
58  else if (Q == 0)
59  {
60    eigVl.x = c[2]/3.0 + pow(b/2.0, 1.0/3.0);
61    eigVl.y = c[2]/3.0 + pow(b/2.0, 1.0/3.0);
62    eigVl.z = c[2]/3.0 + 2* pow(b/2.0, 1.0/3.0);
63  }
64  // 1 Root (not calculating anything.)
65  else if (Q > 0)
66  {
67    printf("This Matrix is a multiple of the Identity matrix (lambda * I3))\n");
68    eigVl.x = eigVl.y = eigVl.z = 1;
69  }
70  printf("%f %f %f\n", eigVl.x, eigVl.y, eigVl.z);
71  return eigVl;
72
73}
74
75void Matrix::getEigenVectors(Vector& a, Vector& b, Vector& c) const
76{
77  Vector eigVl = this->getEigenValues();
78
79  float eigVal[3] = { eigVl.x, eigVl.y, eigVl.z };
80
81  Vector eigVc[3];
82  /* eigenvec test */
83  for(int i = 0; i < 3; i++)
84  {
85    eigVc[i].x = -1/this->m13*(this->m33 - eigVal[i]) + (this->m32*(-this->m31*this->m32 + this->m12*this->m33 - this->m12*eigVal[i])) /
86        this->m13*(-this->m13*this->m22 - this->m12*this->m23 + this->m13*eigVal[i]);
87
88    eigVc[i].y = -( -this->m13*this->m23 + this->m12*this->m33 - this->m12*eigVal[i]) /
89        (-this->m31*this->m22 + this->m12*this->m23 + this->m13*eigVal[i]);
90
91    eigVc[i].z = 1.0f;
92
93
94    printf("home brew: %f, %f, %f\n", eigVc[i].x, eigVc[i].y, eigVc[i].z);
95  }
96
97
98  // EigenVectors
99//   for (int i = 0; i < 3; ++i)
100//   {
101//     printf (":: i = %d\n", i);
102//     Matrix M = *this -  Matrix::identity() * eigVal[i];
103//     Vector m1, m2, m3;
104//
105//     M.getTransposed().toVectors(m1, m2, m3);
106//     Matrix U ( M.m22*M.m33 - M.m23*M.m23, M.m13*M.m23 - M.m12*M.m33, M.m12*M.m23 - M.m13*M.m22,
107//                M.m13*M.m23 - M.m12*M.m33, M.m11*M.m33 - M.m13*M.m13, M.m12*M.m13 - M.m23*M.m11,
108//                M.m12*M.m23 - M.m13*M.m22, M.m13*M.m12 - M.m23*M.m11, M.m11*M.m22 - M.m12*M.m12);
109//     U.debug();
110//
111//     Vector u1, u2, u3;
112//     U.toVectors(u1, u2, u3);
113//
114// /*
115//     u1 = m2.cross(m3);
116//     u2 = m3.cross(m1);
117//     u3 = m1.cross(m2);
118// */
119//
120//     u1 /= u1.len();
121//     u2 /= u2.len();
122//     u3 /= u3.len();
123//
124//
125//     printf("%f, %f, %f\n", u1.x, u1.y, u1.z);
126//     printf("%f, %f, %f\n", u2.x, u2.y, u2.z);
127//     printf("%f, %f, %f\n", u3.x, u3.y, u3.z);
128
129
130
131//     u1 = M*u1;
132//     u2 = M*u2;
133//     u3 = M*u3;
134    //
135//     printf("%f, %f, %f\n", u1.x, u1.y, u1.z);
136//     printf("%f, %f, %f\n", u2.x, u2.y, u2.z);
137//     printf("%f, %f, %f\n", u3.x, u3.y, u3.z);
138//     printf("\n\n");
139//  }
140
141
142
143  this->debug();
144}
145
146void Matrix::debug() const
147{
148  printf("| %f | %f | %f |\n", this->m11, this->m12, this->m13 );
149  printf("| %f | %f | %f |\n", this->m21, this->m22, this->m23 );
150  printf("| %f | %f | %f |\n", this->m31, this->m32, this->m33 );
151
152}
153
Note: See TracBrowser for help on using the repository browser.