← Back to branch summary

~ubuntu-branches/ubuntu/maverick/freecad/maverick

« back to all changes in this revision

Viewing changes to src/Mod/Mesh/App/WildMagic4/Wm4LinearSystem.cpp

  • Committer: Bazaar Package Importer
  • Author(s): Teemu Ikonen
  • Date: 2009-07-16 18:37:41 UTC
  • Revision ID: james.westby@ubuntu.com-20090716183741-oww9kcxqrk991i1n
Tags: upstream-0.8.2237
Import upstream version 0.8.2237

Show diffs side-by-side

added added

removed removed

Lines of Context:
src/Mod/Mesh/App/WildMagic4/Wm4LinearSystem.cpp
 
1
// Wild Magic Source Code
 
2
// David Eberly
 
3
// http://www.geometrictools.com
 
4
// Copyright (c) 1998-2007
 
5
//
 
6
// This library is free software; you can redistribute it and/or modify it
 
7
// under the terms of the GNU Lesser General Public License as published by
 
8
// the Free Software Foundation; either version 2.1 of the License, or (at
 
9
// your option) any later version.  The license is available for reading at
 
10
// either of the locations:
 
11
//     http://www.gnu.org/copyleft/lgpl.html
 
12
//     http://www.geometrictools.com/License/WildMagicLicense.pdf
 
13
// The license applies to versions 0 through 4 of Wild Magic.
 
14
//
 
15
// Version: 4.0.0 (2006/06/28)
 
16
 
 
17
#include "Wm4FoundationPCH.h"
 
18
#include "Wm4LinearSystem.h"
 
19
 
 
20
namespace Wm4
 
21
{
 
22
//----------------------------------------------------------------------------
 
23
template <class Real>
 
24
LinearSystem<Real>::LinearSystem ()
 
25
{
 
26
    ZeroTolerance = Math<Real>::ZERO_TOLERANCE;
 
27
}
 
28
//----------------------------------------------------------------------------
 
29
template <class Real>
 
30
bool LinearSystem<Real>::Solve2 (const Real aafA[2][2], const Real afB[2],
 
31
    Real afX[2])
 
32
{
 
33
    Real fDet = aafA[0][0]*aafA[1][1]-aafA[0][1]*aafA[1][0];
 
34
    if (Math<Real>::FAbs(fDet) < ZeroTolerance)
 
35
    {
 
36
        return false;
 
37
    }
 
38
 
 
39
    Real fInvDet = ((Real)1.0)/fDet;
 
40
    afX[0] = (aafA[1][1]*afB[0]-aafA[0][1]*afB[1])*fInvDet;
 
41
    afX[1] = (aafA[0][0]*afB[1]-aafA[1][0]*afB[0])*fInvDet;
 
42
    return true;
 
43
}
 
44
//----------------------------------------------------------------------------
 
45
template <class Real>
 
46
bool LinearSystem<Real>::Solve3 (const Real aafA[3][3], const Real afB[3],
 
47
    Real afX[3])
 
48
{
 
49
    Real aafAInv[3][3];
 
50
    aafAInv[0][0] = aafA[1][1]*aafA[2][2]-aafA[1][2]*aafA[2][1];
 
51
    aafAInv[0][1] = aafA[0][2]*aafA[2][1]-aafA[0][1]*aafA[2][2];
 
52
    aafAInv[0][2] = aafA[0][1]*aafA[1][2]-aafA[0][2]*aafA[1][1];
 
53
    aafAInv[1][0] = aafA[1][2]*aafA[2][0]-aafA[1][0]*aafA[2][2];
 
54
    aafAInv[1][1] = aafA[0][0]*aafA[2][2]-aafA[0][2]*aafA[2][0];
 
55
    aafAInv[1][2] = aafA[0][2]*aafA[1][0]-aafA[0][0]*aafA[1][2];
 
56
    aafAInv[2][0] = aafA[1][0]*aafA[2][1]-aafA[1][1]*aafA[2][0];
 
57
    aafAInv[2][1] = aafA[0][1]*aafA[2][0]-aafA[0][0]*aafA[2][1];
 
58
    aafAInv[2][2] = aafA[0][0]*aafA[1][1]-aafA[0][1]*aafA[1][0];
 
59
    Real fDet = aafA[0][0]*aafAInv[0][0] + aafA[0][1]*aafAInv[1][0] +
 
60
        aafA[0][2]*aafAInv[2][0];
 
61
 
 
62
    if (Math<Real>::FAbs(fDet) < ZeroTolerance)
 
63
    {
 
64
        return false;
 
65
    }
 
66
 
 
67
    Real fInvDet = ((Real)1.0)/fDet;
 
68
    for (int iRow = 0; iRow < 3; iRow++)
 
69
    {
 
70
        for (int iCol = 0; iCol < 3; iCol++)
 
71
        {
 
72
            aafAInv[iRow][iCol] *= fInvDet;
 
73
        }
 
74
    }
 
75
 
 
76
    afX[0] = aafAInv[0][0]*afB[0]+aafAInv[0][1]*afB[1]+aafAInv[0][2]*afB[2];
 
77
    afX[1] = aafAInv[1][0]*afB[0]+aafAInv[1][1]*afB[1]+aafAInv[1][2]*afB[2];
 
78
    afX[2] = aafAInv[2][0]*afB[0]+aafAInv[2][1]*afB[1]+aafAInv[2][2]*afB[2];
 
79
    return true;
 
80
}
 
81
//----------------------------------------------------------------------------
 
82
template <class Real>
 
83
bool LinearSystem<Real>::Inverse (const GMatrix<Real>& rkA,
 
84
    GMatrix<Real>& rkInvA)
 
85
{
 
86
    // computations are performed in-place
 
87
    assert(rkA.GetRows() == rkA.GetColumns());
 
88
    int iSize = rkInvA.GetRows();
 
89
    rkInvA = rkA;
 
90
 
 
91
    int* aiColIndex = WM4_NEW int[iSize];
 
92
    int* aiRowIndex = WM4_NEW int[iSize];
 
93
    bool* abPivoted = WM4_NEW bool[iSize];
 
94
    memset(abPivoted,0,iSize*sizeof(bool));
 
95
 
 
96
    int i1, i2, iRow = 0, iCol = 0;
 
97
    Real fSave;
 
98
 
 
99
    // elimination by full pivoting
 
100
    for (int i0 = 0; i0 < iSize; i0++)
 
101
    {
 
102
        // search matrix (excluding pivoted rows) for maximum absolute entry
 
103
        Real fMax = 0.0f;
 
104
        for (i1 = 0; i1 < iSize; i1++)
 
105
        {
 
106
            if (!abPivoted[i1])
 
107
            {
 
108
                for (i2 = 0; i2 < iSize; i2++)
 
109
                {
 
110
                    if (!abPivoted[i2])
 
111
                    {
 
112
                        Real fAbs = Math<Real>::FAbs(rkInvA[i1][i2]);
 
113
                        if (fAbs > fMax)
 
114
                        {
 
115
                            fMax = fAbs;
 
116
                            iRow = i1;
 
117
                            iCol = i2;
 
118
                        }
 
119
                    }
 
120
                }
 
121
            }
 
122
        }
 
123
 
 
124
        if (fMax == (Real)0.0)
 
125
        {
 
126
            // matrix is not invertible
 
127
            WM4_DELETE[] aiColIndex;
 
128
            WM4_DELETE[] aiRowIndex;
 
129
            WM4_DELETE[] abPivoted;
 
130
            return false;
 
131
        }
 
132
 
 
133
        abPivoted[iCol] = true;
 
134
 
 
135
        // swap rows so that A[iCol][iCol] contains the pivot entry
 
136
        if (iRow != iCol)
 
137
        {
 
138
            rkInvA.SwapRows(iRow,iCol);
 
139
        }
 
140
 
 
141
        // keep track of the permutations of the rows
 
142
        aiRowIndex[i0] = iRow;
 
143
        aiColIndex[i0] = iCol;
 
144
 
 
145
        // scale the row so that the pivot entry is 1
 
146
        Real fInv = ((Real)1.0)/rkInvA[iCol][iCol];
 
147
        rkInvA[iCol][iCol] = (Real)1.0;
 
148
        for (i2 = 0; i2 < iSize; i2++)
 
149
        {
 
150
            rkInvA[iCol][i2] *= fInv;
 
151
        }
 
152
 
 
153
        // zero out the pivot column locations in the other rows
 
154
        for (i1 = 0; i1 < iSize; i1++)
 
155
        {
 
156
            if (i1 != iCol)
 
157
            {
 
158
                fSave = rkInvA[i1][iCol];
 
159
                rkInvA[i1][iCol] = (Real)0.0;
 
160
                for (i2 = 0; i2 < iSize; i2++)
 
161
                {
 
162
                    rkInvA[i1][i2] -= rkInvA[iCol][i2]*fSave;
 
163
                }
 
164
            }
 
165
        }
 
166
    }
 
167
 
 
168
    // reorder rows so that A[][] stores the inverse of the original matrix
 
169
    for (i1 = iSize-1; i1 >= 0; i1--)
 
170
    {
 
171
        if (aiRowIndex[i1] != aiColIndex[i1])
 
172
        {
 
173
            for (i2 = 0; i2 < iSize; i2++)
 
174
            {
 
175
                fSave = rkInvA[i2][aiRowIndex[i1]];
 
176
                rkInvA[i2][aiRowIndex[i1]] = rkInvA[i2][aiColIndex[i1]];
 
177
                rkInvA[i2][aiColIndex[i1]] = fSave;
 
178
            }
 
179
        }
 
180
    }
 
181
 
 
182
    WM4_DELETE[] aiColIndex;
 
183
    WM4_DELETE[] aiRowIndex;
 
184
    WM4_DELETE[] abPivoted;
 
185
    return true;
 
186
}
 
187
//----------------------------------------------------------------------------
 
188
template <class Real>
 
189
bool LinearSystem<Real>::Solve (const GMatrix<Real>& rkA, const Real* afB,
 
190
    Real* afX)
 
191
{
 
192
    // computations are performed in-place
 
193
    int iSize = rkA.GetColumns();
 
194
    GMatrix<Real> kInvA = rkA;
 
195
    size_t uiSize = iSize*sizeof(Real); 
 
196
    System::Memcpy(afX,uiSize,afB,uiSize);
 
197
 
 
198
    int* aiColIndex = WM4_NEW int[iSize];
 
199
    int* aiRowIndex = WM4_NEW int[iSize];
 
200
    bool* abPivoted = WM4_NEW bool[iSize];
 
201
    memset(abPivoted,0,iSize*sizeof(bool));
 
202
 
 
203
    int i1, i2, iRow = 0, iCol = 0;
 
204
    Real fSave;
 
205
 
 
206
    // elimination by full pivoting
 
207
    for (int i0 = 0; i0 < iSize; i0++)
 
208
    {
 
209
        // search matrix (excluding pivoted rows) for maximum absolute entry
 
210
        Real fMax = 0.0f;
 
211
        for (i1 = 0; i1 < iSize; i1++)
 
212
        {
 
213
            if (!abPivoted[i1])
 
214
            {
 
215
                for (i2 = 0; i2 < iSize; i2++)
 
216
                {
 
217
                    if (!abPivoted[i2])
 
218
                    {
 
219
                        Real fAbs = Math<Real>::FAbs(kInvA[i1][i2]);
 
220
                        if (fAbs > fMax)
 
221
                        {
 
222
                            fMax = fAbs;
 
223
                            iRow = i1;
 
224
                            iCol = i2;
 
225
                        }
 
226
                    }
 
227
                }
 
228
            }
 
229
        }
 
230
 
 
231
        if (fMax == (Real)0.0)
 
232
        {
 
233
            // matrix is not invertible
 
234
            WM4_DELETE[] aiColIndex;
 
235
            WM4_DELETE[] aiRowIndex;
 
236
            WM4_DELETE[] abPivoted;
 
237
            return false;
 
238
        }
 
239
 
 
240
        abPivoted[iCol] = true;
 
241
 
 
242
        // swap rows so that A[iCol][iCol] contains the pivot entry
 
243
        if (iRow != iCol)
 
244
        {
 
245
            kInvA.SwapRows(iRow,iCol);
 
246
 
 
247
            fSave = afX[iRow];
 
248
            afX[iRow] = afX[iCol];
 
249
            afX[iCol] = fSave;
 
250
        }
 
251
 
 
252
        // keep track of the permutations of the rows
 
253
        aiRowIndex[i0] = iRow;
 
254
        aiColIndex[i0] = iCol;
 
255
 
 
256
        // scale the row so that the pivot entry is 1
 
257
        Real fInv = ((Real)1.0)/kInvA[iCol][iCol];
 
258
        kInvA[iCol][iCol] = (Real)1.0;
 
259
        for (i2 = 0; i2 < iSize; i2++)
 
260
        {
 
261
            kInvA[iCol][i2] *= fInv;
 
262
        }
 
263
        afX[iCol] *= fInv;
 
264
 
 
265
        // zero out the pivot column locations in the other rows
 
266
        for (i1 = 0; i1 < iSize; i1++)
 
267
        {
 
268
            if (i1 != iCol)
 
269
            {
 
270
                fSave = kInvA[i1][iCol];
 
271
                kInvA[i1][iCol] = (Real)0.0;
 
272
                for (i2 = 0; i2 < iSize; i2++)
 
273
                    kInvA[i1][i2] -= kInvA[iCol][i2]*fSave;
 
274
                afX[i1] -= afX[iCol]*fSave;
 
275
            }
 
276
        }
 
277
    }
 
278
 
 
279
    // reorder rows so that A[][] stores the inverse of the original matrix
 
280
    for (i1 = iSize-1; i1 >= 0; i1--)
 
281
    {
 
282
        if (aiRowIndex[i1] != aiColIndex[i1])
 
283
        {
 
284
            for (i2 = 0; i2 < iSize; i2++)
 
285
            {
 
286
                fSave = kInvA[i2][aiRowIndex[i1]];
 
287
                kInvA[i2][aiRowIndex[i1]] = kInvA[i2][aiColIndex[i1]];
 
288
                kInvA[i2][aiColIndex[i1]] = fSave;
 
289
            }
 
290
        }
 
291
    }
 
292
 
 
293
    WM4_DELETE[] aiColIndex;
 
294
    WM4_DELETE[] aiRowIndex;
 
295
    WM4_DELETE[] abPivoted;
 
296
    return true;
 
297
}
 
298
//----------------------------------------------------------------------------
 
299
template <class Real>
 
300
bool LinearSystem<Real>::SolveTri (int iSize, Real* afA, Real* afB,
 
301
    Real* afC, Real* afR, Real* afU)
 
302
{
 
303
    if (afB[0] == (Real)0.0)
 
304
    {
 
305
        return false;
 
306
    }
 
307
 
 
308
    Real* afD = WM4_NEW Real[iSize-1];
 
309
    Real fE = afB[0];
 
310
    Real fInvE = ((Real)1.0)/fE;
 
311
    afU[0] = afR[0]*fInvE;
 
312
 
 
313
    int i0, i1;
 
314
    for (i0 = 0, i1 = 1; i1 < iSize; i0++, i1++)
 
315
    {
 
316
        afD[i0] = afC[i0]*fInvE;
 
317
        fE = afB[i1] - afA[i0]*afD[i0];
 
318
        if (fE == (Real)0.0)
 
319
        {
 
320
            WM4_DELETE[] afD;
 
321
            return false;
 
322
        }
 
323
        fInvE = ((Real)1.0)/fE;
 
324
        afU[i1] = (afR[i1] - afA[i0]*afU[i0])*fInvE;
 
325
    }
 
326
 
 
327
    for (i0 = iSize-1, i1 = iSize-2; i1 >= 0; i0--, i1--)
 
328
    {
 
329
        afU[i1] -= afD[i1]*afU[i0];
 
330
    }
 
331
 
 
332
    WM4_DELETE[] afD;
 
333
    return true;
 
334
}
 
335
//----------------------------------------------------------------------------
 
336
template <class Real>
 
337
bool LinearSystem<Real>::SolveConstTri (int iSize, Real fA, Real fB, Real fC,
 
338
    Real* afR, Real* afU)
 
339
{
 
340
    if (fB == (Real)0.0)
 
341
    {
 
342
        return false;
 
343
    }
 
344
 
 
345
    Real* afD = WM4_NEW Real[iSize-1];
 
346
    Real fE = fB;
 
347
    Real fInvE = ((Real)1.0)/fE;
 
348
    afU[0] = afR[0]*fInvE;
 
349
 
 
350
    int i0, i1;
 
351
    for (i0 = 0, i1 = 1; i1 < iSize; i0++, i1++)
 
352
    {
 
353
        afD[i0] = fC*fInvE;
 
354
        fE = fB - fA*afD[i0];
 
355
        if (fE == (Real)0.0)
 
356
        {
 
357
            WM4_DELETE[] afD;
 
358
            return false;
 
359
        }
 
360
        fInvE = ((Real)1.0)/fE;
 
361
        afU[i1] = (afR[i1] - fA*afU[i0])*fInvE;
 
362
    }
 
363
    for (i0 = iSize-1, i1 = iSize-2; i1 >= 0; i0--, i1--)
 
364
    {
 
365
        afU[i1] -= afD[i1]*afU[i0];
 
366
    }
 
367
 
 
368
    WM4_DELETE[] afD;
 
369
    return true;
 
370
}
 
371
//----------------------------------------------------------------------------
 
372
 
 
373
//----------------------------------------------------------------------------
 
374
// conjugate gradient methods
 
375
//----------------------------------------------------------------------------
 
376
template <class Real>
 
377
Real LinearSystem<Real>::Dot (int iSize, const Real* afU, const Real* afV)
 
378
{
 
379
    Real fDot = (Real)0.0;
 
380
    for (int i = 0; i < iSize; i++)
 
381
    {
 
382
        fDot += afU[i]*afV[i];
 
383
    }
 
384
    return fDot;
 
385
}
 
386
//----------------------------------------------------------------------------
 
387
template <class Real>
 
388
void LinearSystem<Real>::Multiply (const GMatrix<Real>& rkA, const Real* afX,
 
389
    Real* afProd)
 
390
{
 
391
    int iSize = rkA.GetRows();
 
392
    memset(afProd,0,iSize*sizeof(Real));
 
393
    for (int iRow = 0; iRow < iSize; iRow++)
 
394
    {
 
395
        for (int iCol = 0; iCol < iSize; iCol++)
 
396
        {
 
397
            afProd[iRow] += rkA[iRow][iCol]*afX[iCol];
 
398
        }
 
399
    }
 
400
}
 
401
//----------------------------------------------------------------------------
 
402
template <class Real>
 
403
void LinearSystem<Real>::Multiply (int iSize, const SparseMatrix& rkA,
 
404
    const Real* afX, Real* afProd)
 
405
{
 
406
    memset(afProd,0,iSize*sizeof(Real));
 
407
    typename SparseMatrix::const_iterator pkIter = rkA.begin();
 
408
    for (/**/; pkIter != rkA.end(); pkIter++)
 
409
    {
 
410
        int i = pkIter->first.first;
 
411
        int j = pkIter->first.second;
 
412
        Real fValue = pkIter->second;
 
413
        afProd[i] += fValue*afX[j];
 
414
        if (i != j)
 
415
        {
 
416
            afProd[j] += fValue*afX[i];
 
417
        }
 
418
    }
 
419
}
 
420
//----------------------------------------------------------------------------
 
421
template <class Real>
 
422
void LinearSystem<Real>::UpdateX (int iSize, Real* afX, Real fAlpha,
 
423
    const Real* afP)
 
424
{
 
425
    for (int i = 0; i < iSize; i++)
 
426
    {
 
427
        afX[i] += fAlpha*afP[i];
 
428
    }
 
429
}
 
430
//----------------------------------------------------------------------------
 
431
template <class Real>
 
432
void LinearSystem<Real>::UpdateR (int iSize, Real* afR, Real fAlpha,
 
433
    const Real* afW)
 
434
{
 
435
    for (int i = 0; i < iSize; i++)
 
436
    {
 
437
        afR[i] -= fAlpha*afW[i];
 
438
    }
 
439
}
 
440
//----------------------------------------------------------------------------
 
441
template <class Real>
 
442
void LinearSystem<Real>::UpdateP (int iSize, Real* afP, Real fBeta,
 
443
    const Real* afR)
 
444
{
 
445
    for (int i = 0; i < iSize; i++)
 
446
    {
 
447
        afP[i] = afR[i] + fBeta*afP[i];
 
448
    }
 
449
}
 
450
//----------------------------------------------------------------------------
 
451
template <class Real>
 
452
bool LinearSystem<Real>::SolveSymmetricCG (const GMatrix<Real>& rkA,
 
453
    const Real* afB, Real* afX)
 
454
{
 
455
    // based on the algorithm in "Matrix Computations" by Golum and Van Loan
 
456
    assert(rkA.GetRows() == rkA.GetColumns());
 
457
    int iSize = rkA.GetRows();
 
458
    Real* afR = WM4_NEW Real[iSize];
 
459
    Real* afP = WM4_NEW Real[iSize];
 
460
    Real* afW = WM4_NEW Real[iSize];
 
461
 
 
462
    // first iteration
 
463
    size_t uiSize = iSize*sizeof(Real);
 
464
    memset(afX,0,uiSize);
 
465
    System::Memcpy(afR,uiSize,afB,uiSize);
 
466
    Real fRho0 = Dot(iSize,afR,afR);
 
467
    System::Memcpy(afP,uiSize,afR,uiSize);
 
468
    Multiply(rkA,afP,afW);
 
469
    Real fAlpha = fRho0/Dot(iSize,afP,afW);
 
470
    UpdateX(iSize,afX,fAlpha,afP);
 
471
    UpdateR(iSize,afR,fAlpha,afW);
 
472
    Real fRho1 = Dot(iSize,afR,afR);
 
473
 
 
474
    // remaining iterations
 
475
    const int iMax = 1024;
 
476
    int i;
 
477
    for (i = 1; i < iMax; i++)
 
478
    {
 
479
        Real fRoot0 = Math<Real>::Sqrt(fRho1);
 
480
        Real fNorm = Dot(iSize,afB,afB);
 
481
        Real fRoot1 = Math<Real>::Sqrt(fNorm);
 
482
        if (fRoot0 <= ZeroTolerance*fRoot1)
 
483
        {
 
484
            break;
 
485
        }
 
486
 
 
487
        Real fBeta = fRho1/fRho0;
 
488
        UpdateP(iSize,afP,fBeta,afR);
 
489
        Multiply(rkA,afP,afW);
 
490
        fAlpha = fRho1/Dot(iSize,afP,afW);
 
491
        UpdateX(iSize,afX,fAlpha,afP);
 
492
        UpdateR(iSize,afR,fAlpha,afW);
 
493
        fRho0 = fRho1;
 
494
        fRho1 = Dot(iSize,afR,afR);
 
495
    }
 
496
 
 
497
    WM4_DELETE[] afW;
 
498
    WM4_DELETE[] afP;
 
499
    WM4_DELETE[] afR;
 
500
 
 
501
    return i < iMax;
 
502
}
 
503
//----------------------------------------------------------------------------
 
504
template <class Real>
 
505
bool LinearSystem<Real>::SolveSymmetricCG (int iSize,
 
506
    const SparseMatrix& rkA, const Real* afB, Real* afX)
 
507
{
 
508
    // based on the algorithm in "Matrix Computations" by Golum and Van Loan
 
509
    Real* afR = WM4_NEW Real[iSize];
 
510
    Real* afP = WM4_NEW Real[iSize];
 
511
    Real* afW = WM4_NEW Real[iSize];
 
512
 
 
513
    // first iteration
 
514
    size_t uiSize = iSize*sizeof(Real);
 
515
    memset(afX,0,uiSize);
 
516
    System::Memcpy(afR,uiSize,afB,uiSize);
 
517
    Real fRho0 = Dot(iSize,afR,afR);
 
518
    System::Memcpy(afP,uiSize,afR,uiSize);
 
519
    Multiply(iSize,rkA,afP,afW);
 
520
    Real fAlpha = fRho0/Dot(iSize,afP,afW);
 
521
    UpdateX(iSize,afX,fAlpha,afP);
 
522
    UpdateR(iSize,afR,fAlpha,afW);
 
523
    Real fRho1 = Dot(iSize,afR,afR);
 
524
 
 
525
    // remaining iterations
 
526
    const int iMax = 1024;
 
527
    int i;
 
528
    for (i = 1; i < iMax; i++)
 
529
    {
 
530
        Real fRoot0 = Math<Real>::Sqrt(fRho1);
 
531
        Real fNorm = Dot(iSize,afB,afB);
 
532
        Real fRoot1 = Math<Real>::Sqrt(fNorm);
 
533
        if (fRoot0 <= ZeroTolerance*fRoot1)
 
534
        {
 
535
            break;
 
536
        }
 
537
 
 
538
        Real fBeta = fRho1/fRho0;
 
539
        UpdateP(iSize,afP,fBeta,afR);
 
540
        Multiply(iSize,rkA,afP,afW);
 
541
        fAlpha = fRho1/Dot(iSize,afP,afW);
 
542
        UpdateX(iSize,afX,fAlpha,afP);
 
543
        UpdateR(iSize,afR,fAlpha,afW);
 
544
        fRho0 = fRho1;
 
545
        fRho1 = Dot(iSize,afR,afR);
 
546
    }
 
547
 
 
548
    WM4_DELETE[] afW;
 
549
    WM4_DELETE[] afP;
 
550
    WM4_DELETE[] afR;
 
551
 
 
552
    return i < iMax;
 
553
}
 
554
//----------------------------------------------------------------------------
 
555
 
 
556
//----------------------------------------------------------------------------
 
557
// banded matrices
 
558
//----------------------------------------------------------------------------
 
559
template <class Real>
 
560
bool LinearSystem<Real>::ForwardEliminate (int iReduceRow,
 
561
    BandedMatrix<Real>& rkA, Real* afB)
 
562
{
 
563
    // the pivot must be nonzero in order to proceed
 
564
    Real fDiag = rkA(iReduceRow,iReduceRow);
 
565
    if (fDiag == (Real)0.0)
 
566
    {
 
567
        return false;
 
568
    }
 
569
 
 
570
    Real fInvDiag = ((Real)1.0)/fDiag;
 
571
    rkA(iReduceRow,iReduceRow) = (Real)1.0;
 
572
 
 
573
    // multiply row to be consistent with diagonal term of 1
 
574
    int iColMin = iReduceRow + 1;
 
575
    int iColMax = iColMin + rkA.GetUBands();
 
576
    if (iColMax > rkA.GetSize())
 
577
    {
 
578
        iColMax = rkA.GetSize();
 
579
    }
 
580
 
 
581
    int iCol;
 
582
    for (iCol = iColMin; iCol < iColMax; iCol++)
 
583
    {
 
584
        rkA(iReduceRow,iCol) *= fInvDiag;
 
585
    }
 
586
 
 
587
    afB[iReduceRow] *= fInvDiag;
 
588
 
 
589
    // reduce remaining rows
 
590
    int iRowMin = iReduceRow + 1;
 
591
    int iRowMax = iRowMin + rkA.GetLBands();
 
592
    if (iRowMax > rkA.GetSize())
 
593
    {
 
594
        iRowMax = rkA.GetSize();
 
595
    }
 
596
 
 
597
    for (int iRow = iRowMin; iRow < iRowMax; iRow++)
 
598
    {
 
599
        Real fMult = rkA(iRow,iReduceRow);
 
600
        rkA(iRow,iReduceRow) = (Real)0.0;
 
601
        for (iCol = iColMin; iCol < iColMax; iCol++)
 
602
        {
 
603
            rkA(iRow,iCol) -= fMult*rkA(iReduceRow,iCol);
 
604
        }
 
605
        afB[iRow] -= fMult*afB[iReduceRow];
 
606
    }
 
607
 
 
608
    return true;
 
609
}
 
610
//----------------------------------------------------------------------------
 
611
template <class Real>
 
612
bool LinearSystem<Real>::SolveBanded (const BandedMatrix<Real>& rkA,
 
613
    const Real* afB, Real* afX)
 
614
{
 
615
    BandedMatrix<Real> kTmp = rkA;
 
616
    int iSize = rkA.GetSize();
 
617
    size_t uiSize = iSize*sizeof(Real); 
 
618
    System::Memcpy(afX,uiSize,afB,uiSize);
 
619
 
 
620
    // forward elimination
 
621
    int iRow;
 
622
    for (iRow = 0; iRow < iSize; iRow++)
 
623
    {
 
624
        if (!ForwardEliminate(iRow,kTmp,afX))
 
625
        {
 
626
            return false;
 
627
        }
 
628
    }
 
629
 
 
630
    // backward substitution
 
631
    for (iRow = iSize-2; iRow >= 0; iRow--)
 
632
    {
 
633
        int iColMin = iRow + 1;
 
634
        int iColMax = iColMin + kTmp.GetUBands();
 
635
        if (iColMax > iSize)
 
636
        {
 
637
            iColMax = iSize;
 
638
        }
 
639
        for (int iCol = iColMin; iCol < iColMax; iCol++)
 
640
        {
 
641
            afX[iRow] -= kTmp(iRow,iCol)*afX[iCol];
 
642
        }
 
643
    }
 
644
 
 
645
    return true;
 
646
}
 
647
//----------------------------------------------------------------------------
 
648
template <class Real>
 
649
bool LinearSystem<Real>::ForwardEliminate (int iReduceRow,
 
650
    BandedMatrix<Real>& rkA, GMatrix<Real>& rkB)
 
651
{
 
652
    // the pivot must be nonzero in order to proceed
 
653
    Real fDiag = rkA(iReduceRow,iReduceRow);
 
654
    if (fDiag == (Real)0.0)
 
655
    {
 
656
        return false;
 
657
    }
 
658
 
 
659
    Real fInvDiag = ((Real)1.0)/fDiag;
 
660
    rkA(iReduceRow,iReduceRow) = (Real)1.0;
 
661
 
 
662
    // multiply row to be consistent with diagonal term of 1
 
663
    int iColMin = iReduceRow + 1;
 
664
    int iColMax = iColMin + rkA.GetUBands();
 
665
    if (iColMax > rkA.GetSize())
 
666
    {
 
667
        iColMax = rkA.GetSize();
 
668
    }
 
669
 
 
670
    int iCol;
 
671
    for (iCol = iColMin; iCol < iColMax; iCol++)
 
672
    {
 
673
        rkA(iReduceRow,iCol) *= fInvDiag;
 
674
    }
 
675
    for (iCol = 0; iCol <= iReduceRow; iCol++)
 
676
    {
 
677
        rkB(iReduceRow,iCol) *= fInvDiag;
 
678
    }
 
679
 
 
680
    // reduce remaining rows
 
681
    int iRowMin = iReduceRow + 1;
 
682
    int iRowMax = iRowMin + rkA.GetLBands();
 
683
    if (iRowMax > rkA.GetSize())
 
684
    {
 
685
        iRowMax = rkA.GetSize();
 
686
    }
 
687
 
 
688
    for (int iRow = iRowMin; iRow < iRowMax; iRow++)
 
689
    {
 
690
        Real fMult = rkA(iRow,iReduceRow);
 
691
        rkA(iRow,iReduceRow) = (Real)0.0;
 
692
        for (iCol = iColMin; iCol < iColMax; iCol++)
 
693
        {
 
694
            rkA(iRow,iCol) -= fMult*rkA(iReduceRow,iCol);
 
695
        }
 
696
        for (iCol = 0; iCol <= iReduceRow; iCol++)
 
697
        {
 
698
            rkB(iRow,iCol) -= fMult*rkB(iReduceRow,iCol);
 
699
        }
 
700
    }
 
701
 
 
702
    return true;
 
703
}
 
704
//----------------------------------------------------------------------------
 
705
template <class Real>
 
706
void LinearSystem<Real>::BackwardEliminate (int iReduceRow,
 
707
    BandedMatrix<Real>& rkA, GMatrix<Real>& rkB)
 
708
{
 
709
    int iRowMax = iReduceRow - 1;
 
710
    int iRowMin = iReduceRow - rkA.GetUBands();
 
711
    if (iRowMin < 0)
 
712
    {
 
713
        iRowMin = 0;
 
714
    }
 
715
 
 
716
    for (int iRow = iRowMax; iRow >= iRowMin; iRow--)
 
717
    {
 
718
        Real fMult = rkA(iRow,iReduceRow);
 
719
        rkA(iRow,iReduceRow) = (Real)0.0;
 
720
        for (int iCol = 0; iCol < rkB.GetColumns(); iCol++)
 
721
        {
 
722
            rkB(iRow,iCol) -= fMult*rkB(iReduceRow,iCol);
 
723
        }
 
724
    }
 
725
}
 
726
//----------------------------------------------------------------------------
 
727
template <class Real>
 
728
bool LinearSystem<Real>::Invert (const BandedMatrix<Real>& rkA,
 
729
    GMatrix<Real>& rkInvA)
 
730
{
 
731
    int iSize = rkA.GetSize();
 
732
    BandedMatrix<Real> kTmp = rkA;
 
733
    int iRow;
 
734
    for (iRow = 0; iRow < iSize; iRow++)
 
735
    {
 
736
        for (int iCol = 0; iCol < iSize; iCol++)
 
737
        {
 
738
            if (iRow != iCol)
 
739
            {
 
740
                rkInvA(iRow,iCol) = (Real)0.0;
 
741
            }
 
742
            else
 
743
            {
 
744
                rkInvA(iRow,iRow) = (Real)1.0;
 
745
            }
 
746
        }
 
747
    }
 
748
 
 
749
    // forward elimination
 
750
    for (iRow = 0; iRow < iSize; iRow++)
 
751
    {
 
752
        if (!ForwardEliminate(iRow,kTmp,rkInvA))
 
753
        {
 
754
            return false;
 
755
        }
 
756
    }
 
757
 
 
758
    // backward elimination
 
759
    for (iRow = iSize-1; iRow >= 1; iRow--)
 
760
    {
 
761
        BackwardEliminate(iRow,kTmp,rkInvA);
 
762
    }
 
763
 
 
764
    return true;
 
765
}
 
766
//----------------------------------------------------------------------------
 
767
 
 
768
//----------------------------------------------------------------------------
 
769
// explicit instantiation
 
770
//----------------------------------------------------------------------------
 
771
template WM4_FOUNDATION_ITEM
 
772
class LinearSystem<float>;
 
773
 
 
774
template WM4_FOUNDATION_ITEM
 
775
class LinearSystem<double>;
 
776
//----------------------------------------------------------------------------
 
777
}