23 #ifndef __ND_CONJUGATE_GRADIENT_H__
24 #define __ND_CONJUGATE_GRADIENT_H__
26 #include "ndCoreStdafx.h"
29 #include "ndClassAlloc.h"
30 #include "ndGeneralVector.h"
31 #include "ndGeneralMatrix.h"
33 #define D_USE_JACOBI_PRECONDITIONER
41 ,m_preconditoner(preconditonerBuffer)
44 #ifdef D_USE_JACOBI_PRECONDITIONER
46 const T* row = m_matrix;
47 for (ndInt32 i = 0; i < size; ++i)
49 m_preconditoner[i] = T(1.0f) / row[i];
105 void PreconditionerSolve(
const T*
const input, T*
const output)
107 #ifdef D_USE_JACOBI_PRECONDITIONER
108 for (ndInt32 i = 0; i < m_size; ++i)
110 output[i] = input[i] * m_preconditoner[i];
116 void MatrixTimeVector(
const T*
const input, T*
const output)
118 ndMatrixTimeVector(m_size, m_matrix, input, output);
128 template<
class T,
class ndMatrixOperator = ndDefaultMatrixOperator<T>>
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);
140 T SolveInternal(ndInt32 size, T tolerance, T*
const x,
const T*
const b,
const T*
const matrix, T*
const preconditionerBuffer)
const;
148 template<
class T,
class ndMatrixOperator>
151 SetBuffers(
nullptr,
nullptr,
nullptr,
nullptr);
154 template<
class T,
class ndMatrixOperator>
157 SetBuffers(r0, z0, p0, q0);
160 template<
class T,
class ndMatrixOperator>
165 template<
class T,
class ndMatrixOperator>
174 template<
class T,
class ndMatrixOperator>
179 return SolveInternal(size, tolerance, x, b, matrix, preconditionerBuffer);
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);
194 template<
class T,
class ndMatrixOperator>
197 ndMatrixOperator matrixOper(size, matrix, preconditionerBuffer);
199 matrixOper.MatrixTimeVector(x, m_z0);
200 ndSub(size, m_r0, b, m_z0);
201 matrixOper.PreconditionerSolve(m_r0, m_p0);
204 T num = ndDotProduct(size, m_r0, m_p0);
206 for (ndInt32 j = 0; (j < size) && (error2 > tolerance); ++j)
208 matrixOper.MatrixTimeVector(m_p0, m_z0);
209 T den = ndDotProduct(size, m_p0, m_z0);
211 ndAssert(fabs(den) > T(0.0f));
214 ndScaleAdd(size, x, x, m_p0, alpha);
217 ndScaleAdd(size, m_r0, m_r0, m_z0, -alpha);
221 matrixOper.MatrixTimeVector(x, m_z0);
222 ndSub(size, m_r0, b, m_z0);
225 matrixOper.PreconditionerSolve(m_r0, m_q0);
227 T num1 = ndDotProduct(size, m_r0, m_q0);
229 ndScaleAdd(size, m_p0, m_q0, m_p0, beta);
230 num = ndDotProduct(size, m_r0, m_q0);
234 ndAssert(iter <= size);