My Project: dynamics.mod/NewtonDynamics/source/core/dgSPDMatrix.h Source File

My Project

dgSPDMatrix.h
Go to the documentation of this file.
1 /* Copyright (c) <2003-2011> <Julio Jerez, Newton Game Dynamics>
2 *
3 * This software is provided 'as-is', without any express or implied
4 * warranty. In no event will the authors be held liable for any damages
5 * arising from the use of this software.
6 *
7 * Permission is granted to anyone to use this software for any purpose,
8 * including commercial applications, and to alter it and redistribute it
9 * freely, subject to the following restrictions:
10 *
11 * 1. The origin of this software must not be misrepresented; you must not
12 * claim that you wrote the original software. If you use this software
13 * in a product, an acknowledgment in the product documentation would be
14 * appreciated but is not required.
15 *
16 * 2. Altered source versions must be plainly marked as such, and must not be
17 * misrepresented as being the original software.
18 *
19 * 3. This notice may not be removed or altered from any source distribution.
20 */
21
22 #ifndef __dgSPDMatrix__
23 #define __dgSPDMatrix__
24
25 #include "dgStdafx.h"
26 #include "dgDebug.h"
27 #include "dgGeneralMatrix.h"
28
29 /*
30 void _BackAndForwardSustitition (void *rightsideVector,
31 void *rowPointers,
32 dgInt32 rowStrideInBytes,
33 dgInt32 typeSizeInBytes,
34 dgInt32 size);
35
36 bool _SolveByCholeskyDecomposition (void *rightsideVector,
37 void *rowPointers,
38 dgInt32 rowStrideInBytes,
39 dgInt32 typeSizeInBytes,
40 dgInt32 size);
41
42
43
44 template<class T>
45 class dgSPDMatrix: public dgGeneralMatrix<T>
46 {
47 public:
48 dgSPDMatrix (dgInt32 size);
49 dgSPDMatrix (const dgSPDMatrix<T>& src);
50 dgSPDMatrix (dgInt32 size, T *elemBuffer, dgGeneralVector<T>* m_rowBuffer);
51 dgSPDMatrix (const dgSPDMatrix<T>& src, T *elemBuffer, dgGeneralVector<T>* m_rowBuffer);
52
53 ~dgSPDMatrix ();
54
55 bool TestPSD () const;
56 bool CholeskyDecomposition();
57 void DownDateCholeskyDecomposition (dgInt32 column);
58
59
60 bool Solve (dgGeneralVector<T> &b);
61 void BackAndForwardSustitition(dgGeneralVector<T> &b);
62 };
63
64
65 // ***********************************************************************************************
66 //
67 // LinearSystem
68 //
69 // ***********************************************************************************************
70 template<class T>
71 dgSPDMatrix<T>::dgSPDMatrix (dgInt32 size)
72 :dgGeneralMatrix<T>(size, size)
73 {
74 }
75
76 template<class T>
77 dgSPDMatrix<T>::dgSPDMatrix (const dgSPDMatrix<T>& src)
78 :dgGeneralMatrix<T>(src)
79 {
80 }
81
82
83 template<class T>
84 dgSPDMatrix<T>::dgSPDMatrix (
85 dgInt32 size,
86 T *elemBuffer,
87 dgGeneralVector<T>* m_rowBuffer)
88 :dgGeneralMatrix<T>(size, size, elemBuffer, m_rowBuffer)
89 {
90 }
91
92 template<class T>
93 dgSPDMatrix<T>::dgSPDMatrix (
94 const dgSPDMatrix<T>& src,
95 T *elemBuffer,
96 dgGeneralVector<T>* m_rowBuffer)
97 :dgGeneralMatrix<T>(src, elemBuffer, m_rowBuffer)
98 {
99 }
100
101
102 template<class T>
103 dgSPDMatrix<T>::~dgSPDMatrix ()
104 {
105 }
106
107 template<class T>
108 bool dgSPDMatrix<T>::TestPSD () const
109 {
110 if (!TestSymetry ()) {
111 return false;
112 }
113
114 dgSPDMatrix<T> tmp (*this);
115 return tmp.CholeskyDecomposition();
116 }
117
118
119 template<class T>
120 void dgSPDMatrix<T>::BackAndForwardSustitition(dgGeneralVector<T> &b)
121 {
122 _BackAndForwardSustitition (b.m_columns, &m_rows[0].m_columns, sizeof (dgGeneralVector<T>), sizeof (T), m_rowCount);
123 }
124
125
126 template<class T>
127 bool dgSPDMatrix<T>::Solve (dgGeneralVector<T> &b)
128 {
129 return _SolveByCholeskyDecomposition (b.m_columns, &m_rows[0].m_columns, sizeof (dgGeneralVector<T>), sizeof (T), m_rowCount);
130 }
131
132
133
134 template<class T>
135 bool dgSPDMatrix<T>::CholeskyDecomposition()
136 {
137 dgInt32 i;
138 dgInt32 j;
139 dgInt32 k;
140 T factor;
141 T* rowK;
142 T* rowJ;
143
144 #ifdef DG_COUNT_FLOAT_OPS
145 dgInt32 memCount;
146 dgInt32 floatCount;
147
148 memCount = dgGeneralVector<T>::GetMemWrites();
149 floatCount = dgGeneralVector<T>::GetFloatOps();
150 #endif
151
152 for (j = 0; j < m_rowCount; j++) {
153 rowJ = &m_rows[j].m_columns[0];
154
155 for (k = 0; k < j; k ++ ) {
156 rowK = &m_rows[k].m_columns[0];
157
158 factor = rowK[j];
159 if (dgAbsf (factor) > 1.0e-6f) {
160 for (i = j; i < m_rowCount; i ++) {
161 rowJ[i] -= rowK[i] * factor;
162 #ifdef DG_COUNT_FLOAT_OPS
163 memCount += 1;
164 floatCount += 2;
165 #endif
166 }
167 }
168 }
169
170 factor = rowJ[j];
171 if (factor <= T (0.0f)) {
172 if (factor <= T(-5.0e-4f)) {
173 return false;
174 }
175 factor = T(1.0e-12f);
176 }
177
178 factor = T (dgSqrt (dgFloat32(factor)));
179 rowJ[j] = factor;
180 factor = T(1.0f / dgFloat32(factor));
181
182 #ifdef DG_COUNT_FLOAT_OPS
183 memCount += 1;
184 floatCount += 1;
185 #endif
186
187 for (k = j + 1; k < m_rowCount; k ++) {
188 rowJ[k] *= factor;
189 #ifdef DG_COUNT_FLOAT_OPS
190 memCount += 1;
191 floatCount += 1;
192 #endif
193 }
194 }
195
196 #ifdef DG_COUNT_FLOAT_OPS
197 dgGeneralVector<T>::SetMemWrites(memCount);
198 dgGeneralVector<T>::SetFloatOps(floatCount);
199 #endif
200
201 return true;
202
203 }
204
205
206 template<class T>
207 void dgSPDMatrix<T>::DownDateCholeskyDecomposition (dgInt32 column)
208 {
209 dgInt32 i;
210 dgInt32 j;
211
212 T *buffer;
213 dgGeneralVector<T>* rowsBuffer;
214
215 rowsBuffer = (dgGeneralVector<dgFloat32>*) dgStackAlloc (m_rowCount * sizeof (dgGeneralVector<T>));
216 buffer = (T *) dgStackAlloc ((m_rowCount + 4) * (m_rowCount + 4) * sizeof (T));
217
218 dgGeneralMatrix<T> tmp (m_rowCount, m_rowCount, buffer, rowsBuffer);
219 dgGeneralMatrix<T>& me = *this;
220 for (i = 0; i < m_rowCount; i ++) {
221 tmp[i][i] = me[i][i];
222 for (j = i + 1; j < m_rowCount; j ++) {
223 tmp[i][j] = T (0.0f);
224 tmp[j][i] = me[i][j];
225 }
226 }
227
228 me.MatrixTimeMatrixTranspose (tmp, tmp);
229 for (i = 0; i < m_rowCount; i ++) {
230 me[i][column] = T (0.0f);
231 me[column][i] = T (0.0f);
232 }
233 me[column][column] = T (1.0f);
234
235 CholeskyDecomposition();
236 }
237 */
238
239 class dgSymmetricBiconjugateGradientSolve
240 {
241 public:
242 dgSymmetricBiconjugateGradientSolve ();
243 ~dgSymmetricBiconjugateGradientSolve ();
244
245 dgFloat64 Solve (dgInt32 size, dgFloat64 tolerance, dgFloat64* const x, const dgFloat64* const b) const;
246
247 protected:
248 virtual void MatrixTimeVector (dgFloat64* const out, const dgFloat64* const v) const = 0;
249 virtual bool InversePrecoditionerTimeVector (dgFloat64* const out, const dgFloat64* const v) const = 0;
250
251 private:
252 dgFloat64 DotProduct (dgInt32 size, const dgFloat64* const b, const dgFloat64* const c) const;
253 void ScaleAdd (dgInt32 size, dgFloat64* const a, const dgFloat64* const b, dgFloat64 scale, const dgFloat64* const c) const;
254 void Sub (dgInt32 size, dgFloat64* const a, const dgFloat64* const b, const dgFloat64* const c) const;
255
256 };
257
258
259
260 #endif
261
dgSymmetricBiconjugateGradientSolve::InversePrecoditionerTimeVector
virtual bool InversePrecoditionerTimeVector(dgFloat64 *const out, const dgFloat64 *const v) const =0
dgDebug.h
dgInt32
int dgInt32
Definition: dgTypes.h:218
dgSymmetricBiconjugateGradientSolve::dgSymmetricBiconjugateGradientSolve
dgSymmetricBiconjugateGradientSolve()
Definition: dgSPDMatrix.cpp:306
dgSymmetricBiconjugateGradientSolve
Definition: dgSPDMatrix.h:239
dgFloat64
double dgFloat64
Definition: dgTypes.h:224
dgStdafx.h
dgSymmetricBiconjugateGradientSolve::Solve
dgFloat64 Solve(dgInt32 size, dgFloat64 tolerance, dgFloat64 *const x, const dgFloat64 *const b) const
Definition: dgSPDMatrix.cpp:338
dgSymmetricBiconjugateGradientSolve::MatrixTimeVector
virtual void MatrixTimeVector(dgFloat64 *const out, const dgFloat64 *const v) const =0
dgSymmetricBiconjugateGradientSolve::~dgSymmetricBiconjugateGradientSolve
~dgSymmetricBiconjugateGradientSolve()
Definition: dgSPDMatrix.cpp:310
dgGeneralMatrix.h