Newton Dynamics  4.00
ndConjugateGradient.h
1 
2 /* Copyright (c) <2003-2022> <Julio Jerez, Newton Game Dynamics>
3 *
4 * This software is provided 'as-is', without any express or implied
5 * warranty. In no event will the authors be held liable for any damages
6 * arising from the use of this software.
7 *
8 * Permission is granted to anyone to use this software for any purpose,
9 * including commercial applications, and to alter it and redistribute it
10 * freely, subject to the following restrictions:
11 *
12 * 1. The origin of this software must not be misrepresented; you must not
13 * claim that you wrote the original software. If you use this software
14 * in a product, an acknowledgment in the product documentation would be
15 * appreciated but is not required.
16 *
17 * 2. Altered source versions must be plainly marked as such, and must not be
18 * misrepresented as being the original software.
19 *
20 * 3. This notice may not be removed or altered from any source distribution.
21 */
22 
23 #ifndef __ND_CONJUGATE_GRADIENT_H__
24 #define __ND_CONJUGATE_GRADIENT_H__
25 
26 #include "ndCoreStdafx.h"
27 #include "ndTypes.h"
28 #include "ndUtils.h"
29 #include "ndClassAlloc.h"
30 #include "ndGeneralVector.h"
31 #include "ndGeneralMatrix.h"
32 
33 #define D_USE_JACOBI_PRECONDITIONER
34 
35 template<class T>
37 {
38  public:
39  ndDefaultMatrixOperator(ndInt32 size, const T* const matrix, T* const preconditonerBuffer)
40  :m_matrix(matrix)
41  ,m_preconditoner(preconditonerBuffer)
42  ,m_size(size)
43  {
44 #ifdef D_USE_JACOBI_PRECONDITIONER
45  // use Jacobi preconditiner
46  const T* row = m_matrix;
47  for (ndInt32 i = 0; i < size; ++i)
48  {
49  m_preconditoner[i] = T(1.0f) / row[i];
50  row += m_size;
51  }
52 #endif
53 
54  // trying Gauss sidel preconditoner later, but does no seem to get any better reasult than Jacobi.
55  //memcpy(A, matrix, sizeof(A));
56  //
57  //dCholeskyFactorization(size, size, &A[0][0]);
58  //for (ndInt32 i = 2; i < 6; ++i)
59  //{
60  // for (ndInt32 j = 0; j < i - 1; ++j)
61  // {
62  // A[i][j] = 0.0f;
63  // }
64  //}
65 
66  //for (ndInt32 i = 0; i < 6; ++i)
67  //{
68  // for (ndInt32 j = i + 2; j < 6; ++j)
69  // {
70  // A[i][j] = 0.0f;
71  // A[j][i] = 0.0f;
72  // }
73  //}
74  //
75  //for (ndInt32 i = 0; i < 5; ++i)
76  //{
77  // T val = dAbs(A[i][i] * T(0.99f));
78  // if (val < dAbs(A[i][i + 1]))
79  // {
80  // //val *= dSign(A[i][i + 1]);
81  // val = 0;
82  // A[i][i + 1] = val;
83  // A[i + 1][i] = val;
84  // //A[i][i + 1] = 0;
85  // //A[i + 1][i] = 0;
86  // }
87  //}
88  //
89  //for (ndInt32 i = 1; i < 6; ++i)
90  //{
91  // T val = dAbs(A[i][i] * T(0.99f));
92  // if (val < dAbs(A[i][i - 1]))
93  // {
94  // //val *= dSign(A[i][i - 1]);
95  // val = 0;
96  // A[i][i - 1] = val;
97  // A[i - 1][i] = val;
98  // }
99  //}
100  //
101  //
102  //dCholeskyFactorization(size, size, &A[0][0]);
103  }
104 
105  void PreconditionerSolve(const T* const input, T* const output)
106  {
107 #ifdef D_USE_JACOBI_PRECONDITIONER
108  for (ndInt32 i = 0; i < m_size; ++i)
109  {
110  output[i] = input[i] * m_preconditoner[i];
111  }
112 #endif
113  //dSolveCholesky(m_size, m_size, &A[0][0], output, input);
114  }
115 
116  void MatrixTimeVector(const T* const input, T* const output)
117  {
118  ndMatrixTimeVector(m_size, m_matrix, input, output);
119  }
120 
121  const T* m_matrix;
122  T* m_preconditoner;
123  ndInt32 m_size;
124 
125  T A[6][6];
126 };
127 
128 template<class T, class ndMatrixOperator = ndDefaultMatrixOperator<T>>
130 {
131  public:
133  ndConjugateGradient(T* const r0, T* const z0, T* const p0, T* const q0);
135 
136  void SetBuffers(T* const r0, T* const z0, T* const p0, T* const q0);
137  T Solve(ndInt32 size, T tolerance, T* const x, const T* const b, const T* const matrix, T* const preconditionerBuffer);
138 
139  private:
140  T SolveInternal(ndInt32 size, T tolerance, T* const x, const T* const b, const T* const matrix, T* const preconditionerBuffer) const;
141 
142  T* m_r0;
143  T* m_z0;
144  T* m_p0;
145  T* m_q0;
146 };
147 
148 template<class T, class ndMatrixOperator>
150 {
151  SetBuffers(nullptr, nullptr, nullptr, nullptr);
152 }
153 
154 template<class T, class ndMatrixOperator>
155 ndConjugateGradient<T, ndMatrixOperator>::ndConjugateGradient(T* const r0, T* const z0, T* const p0, T* const q0)
156 {
157  SetBuffers(r0, z0, p0, q0);
158 }
159 
160 template<class T, class ndMatrixOperator>
162 {
163 }
164 
165 template<class T, class ndMatrixOperator>
166 void ndConjugateGradient<T, ndMatrixOperator>::SetBuffers(T* const r0, T* const z0, T* const p0, T* const q0)
167 {
168  m_r0 = r0;
169  m_z0 = z0;
170  m_p0 = p0;
171  m_q0 = q0;
172 }
173 
174 template<class T, class ndMatrixOperator>
175 T ndConjugateGradient<T, ndMatrixOperator>::Solve(ndInt32 size, T tolerance, T* const x, const T* const b, const T* const matrix, T* const preconditionerBuffer)
176 {
177  if (m_r0)
178  {
179  return SolveInternal(size, tolerance, x, b, matrix, preconditionerBuffer);
180  }
181  else
182  {
183  T* const r0 = ndAlloca(T, size);
184  T* const z0 = ndAlloca(T, size);
185  T* const p0 = ndAlloca(T, size);
186  T* const q0 = ndAlloca(T, size);
187  SetBuffers(r0, z0, p0, q0);
188  T error = SolveInternal(size, tolerance, x, b, matrix, preconditionerBuffer);
189  SetBuffers(nullptr, nullptr, nullptr, nullptr);
190  return error;
191  }
192 }
193 
194 template<class T, class ndMatrixOperator>
195 T ndConjugateGradient<T, ndMatrixOperator>::SolveInternal(ndInt32 size, T tolerance, T* const x, const T* const b, const T* const matrix, T* const preconditionerBuffer) const
196 {
197  ndMatrixOperator matrixOper(size, matrix, preconditionerBuffer);
198 
199  matrixOper.MatrixTimeVector(x, m_z0);
200  ndSub(size, m_r0, b, m_z0);
201  matrixOper.PreconditionerSolve(m_r0, m_p0);
202 
203  ndInt32 iter = 0;
204  T num = ndDotProduct(size, m_r0, m_p0);
205  T error2 = num;
206  for (ndInt32 j = 0; (j < size) && (error2 > tolerance); ++j)
207  {
208  matrixOper.MatrixTimeVector(m_p0, m_z0);
209  T den = ndDotProduct(size, m_p0, m_z0);
210 
211  ndAssert(fabs(den) > T(0.0f));
212  T alpha = num / den;
213 
214  ndScaleAdd(size, x, x, m_p0, alpha);
215  if ((j % 50) != 49)
216  {
217  ndScaleAdd(size, m_r0, m_r0, m_z0, -alpha);
218  }
219  else
220  {
221  matrixOper.MatrixTimeVector(x, m_z0);
222  ndSub(size, m_r0, b, m_z0);
223  }
224 
225  matrixOper.PreconditionerSolve(m_r0, m_q0);
226 
227  T num1 = ndDotProduct(size, m_r0, m_q0);
228  T beta = num1 / num;
229  ndScaleAdd(size, m_p0, m_q0, m_p0, beta);
230  num = ndDotProduct(size, m_r0, m_q0);
231  iter++;
232  error2 = num;
233  }
234  ndAssert(iter <= size);
235  return num;
236 }
237 
238 #endif
ndClassAlloc
Base class for providing memory allocation for all other engine classes.
Definition: ndClassAlloc.h:30
ndConjugateGradient
Definition: ndConjugateGradient.h:130
ndDefaultMatrixOperator
Definition: ndConjugateGradient.h:37