matrix.h
1 /*** ********************************************************************
2 Matrix class implementation.
3 Mar 13 2008, HE Zhao
4 http://hezhao.net
5 he@hezhao.net
6 ************************************************************************/
7 
8 #ifndef _MATRIX_H_
9 #define _MATRIX_H_
10 
11 #include <cstring>
12 
13 namespace VoxelFarm
14 {
15  namespace Algebra
16  {
18  class QEFMatrix
19  {
20  public:
21  QEFMatrix()
22  {
23  memset(m, 0, 16 * sizeof(double));
24  }
25  QEFMatrix(double c)
26  {
27  for (int i = 0; i < 16; i++)
28  {
29  m[i] = c;
30  }
31  }
32  QEFMatrix(double m11, double m12, double m13, double m14,
33  double m21, double m22, double m23, double m24,
34  double m31, double m32, double m33, double m34,
35  double m41, double m42, double m43, double m44)
36  {
37  m[0] = m11;
38  m[1] = m12;
39  m[2] = m13;
40  m[3] = m14;
41  m[4] = m21;
42  m[5] = m22;
43  m[6] = m23;
44  m[7] = m24;
45  m[8] = m31;
46  m[9] = m32;
47  m[10] = m33;
48  m[11] = m34;
49  m[12] = m41;
50  m[13] = m42;
51  m[14] = m43;
52  m[15] = m44;
53  }
54  QEFMatrix(const QEFMatrix& q)
55  {
56  memcpy(m, q.m, 16 * sizeof(double));
57  }
58 
59  /*
60  * matrix constructor for a specific plane
61  *
62  * plane - a 4-dimension array
63  */
64  QEFMatrix(const double plane[])
65  {
66  const double& a = plane[0];
67  const double& b = plane[1];
68  const double& c = plane[2];
69  const double& d = plane[3];
70  /*m[0] = a*a;
71  m[1] = a*b;
72  m[2] = a*c;
73  m[3] = a*d;
74  m[4] = b*a;
75  m[5] = b*b;
76  m[6] = b*c;
77  m[7] = b*d;
78  m[8] = c*a;
79  m[9] = c*b;
80  m[10] = c*c;
81  m[11] = c*d;
82  m[12] = d*a;
83  m[13] = d*b;
84  m[14] = d*c;
85  m[15] = d*d;*/
86  m[0] = a*a;
87  m[5] = b*b;
88  m[10] = c*c;
89  m[15] = d*d;
90  m[1] = m[4] = a*b;
91  m[2] = m[8] = a*c;
92  m[3] = m[12] = a*d;
93  m[6] = m[9] = b*c;
94  m[7] = m[13] = b*d;
95  m[11] = m[14] = c*d;
96  }
97 
98  QEFMatrix(double plane1[], double plane2[])
99  {
100  double a1 = plane1[0];
101  double b1 = plane1[1];
102  double c1 = plane1[2];
103  double d1 = plane1[3];
104  double a2 = plane2[0];
105  double b2 = plane2[1];
106  double c2 = plane2[2];
107  double d2 = plane2[3];
108  m[0] = a1*a1 + a2*a2;
109  m[1] = a1*b1 + a2*b2;
110  m[2] = a1*c1 + a2*c2;
111  m[3] = a1*d1 + a2*d2;
112  m[4] = a1*b1 + a2*b2;
113  m[5] = b1*b1 + b2*b2;
114  m[6] = b1*c1 + b2*c2;
115  m[7] = b1*d1 + b2*d2;
116  m[8] = a1*c1 + a2*c2;
117  m[9] = b1*c1 + b2*c2;
118  m[10] = c1*c1 + c2*c2;
119  m[11] = c1*d1 + c2*d2;
120  m[12] = a1*d1 + a2*d2;
121  m[13] = b1*d1 + b2*d2;
122  m[14] = c1*d1 + c2*d2;
123  m[15] = d1*d1 + d2*d2;
124  }
125 
126  double operator[](int c) const
127  {
128  return m[c];
129  }
130 
131  /*
132  * calculate determinant of a 3*3 matrix
133  *
134  * a(mn) - index of matrix element
135  */
136  double det(int a11, int a12, int a13,
137  int a21, int a22, int a23,
138  int a31, int a32, int a33)
139  {
140  double det = m[a11]*m[a22]*m[a33] + m[a13]*m[a21]*m[a32] + m[a12]*m[a23]*m[a31]
141  - m[a13]*m[a22]*m[a31] - m[a11]*m[a23]*m[a32] - m[a12]*m[a21]*m[a33];
142  return det;
143  }
144 
145  /* For test only */
146  void print()
147  {
148  /*
149  for (int i = 0; i < 16; )
150  {
151  printf("%.8lf ", m[i++]);
152  if (i % 4 == 0)
153  {
154  printf("\n");
155  }
156  }
157  */
158  }
159 
160  inline const QEFMatrix operator+(const QEFMatrix& n) const
161  {
162  return QEFMatrix(
163  m[0]+n[0], m[1]+n[1], m[2]+n[2], m[3]+n[3],
164  m[4]+n[4], m[5]+n[5], m[6]+n[6], m[7]+n[7],
165  m[8]+n[8], m[9]+n[9], m[10]+n[10], m[11]+n[11],
166  m[12]+n[12], m[13]+n[13], m[14]+n[14], m[15]+n[15]);
167  }
168 
169  inline QEFMatrix& operator+=(const QEFMatrix& n)
170  {
171  m[0]+=n[0];
172  m[1]+=n[1];
173  m[2]+=n[2];
174  m[3]+=n[3];
175  m[4]+=n[4];
176  m[5]+=n[5];
177  m[6]+=n[6];
178  m[7]+=n[7];
179  m[8]+=n[8];
180  m[9]+=n[9];
181  m[10]+=n[10];
182  m[11]+=n[11];
183  m[12]+=n[12];
184  m[13]+=n[13];
185  m[14]+=n[14];
186  m[15]+=n[15];
187  return *this;
188  }
189 
190  inline const QEFMatrix operator+(const double& c) const
191  {
192  return QEFMatrix(c+m[0], c+m[1], c+m[2], c+m[3],
193  c+m[4], c+m[5], c+m[6], c+m[7],
194  c+m[8], c+m[9], c+m[10], c+m[11],
195  c+m[12], c+m[13], c+m[14], c+m[15]);
196  }
197 
198  inline QEFMatrix& operator+=(const double& c)
199  {
200  for (int i = 0; i < 16; i++)
201  {
202  m[i] += c;
203  }
204  return *this;
205  }
206 
207  inline const QEFMatrix operator*(const double& c) const
208  {
209  return QEFMatrix(c*m[0], c*m[1], c*m[2], c*m[3],
210  c*m[4], c*m[5], c*m[6], c*m[7],
211  c*m[8], c*m[9], c*m[10], c*m[11],
212  c*m[12], c*m[13], c*m[14], c*m[15]);
213  }
214 
215  inline QEFMatrix& operator*=(const double& c)
216  {
217  for (int i = 0; i < 16; i++)
218  {
219  m[i] *= c;
220  }
221  return *this;
222  }
223 
224  inline QEFMatrix& operator=(const double& c)
225  {
226  for (int i = 0; i < 16; i++)
227  {
228  m[i] = c;
229  }
230  return *this;
231  }
232 
233  public:
234  double m[16];
235  };
236 
237  inline double vertex_error(const QEFMatrix& q, const double& x, const double& y, const double& z)
238  {
239  return q[0]*x*x + 2*q[1]*x*y + 2*q[2]*x*z + 2*q[3]*x + q[5]*y*y
240  + 2*q[6]*y*z + 2*q[7]*y + q[10]*z*z + 2*q[11]*z + q[15];
241  };
242 
243  inline bool solveQEF(QEFMatrix& q_bar, double* vx, double* vy, double* vz, double *error)
244  {
245  if (q_bar[1] != q_bar[4] || q_bar[2] != q_bar[8] || q_bar[6] != q_bar[9] ||
246  q_bar[3] != q_bar[12] || q_bar[7] != q_bar[13] || q_bar[11] != q_bar[14])
247  {
248  return false;
249  }
250 
251  QEFMatrix q_delta = QEFMatrix(q_bar[0], q_bar[1], q_bar[2], q_bar[3],
252  q_bar[4], q_bar[5], q_bar[6], q_bar[7],
253  q_bar[8], q_bar[9], q_bar[10], q_bar[11],
254  0, 0, 0, 1);
255 
256  double det = q_delta.det(0, 1, 2, 4, 5, 6, 8, 9, 10);
257  if ((det != 0))
258  {
259  *vx = -1/det*(q_delta.det(1, 2, 3, 5, 6, 7, 9, 10, 11));
260  *vy = 1/det*(q_delta.det(0, 2, 3, 4, 6, 7, 8, 10, 11));
261  *vz = -1/det*(q_delta.det(0, 1, 3, 4, 5, 7, 8, 9, 11));
262  }
263  *error = vertex_error(q_bar, *vx, *vy, *vz);
264  return true;
265  }
266  }
267 
268 }
269 
270 #endif
Contains all classes and functions for the VoxelFarm engine.
A matrix for minimizing Quadratic Error functions.
Definition: matrix.h:18