newmat7.cpp
Go to the documentation of this file.
00001 
00002 
00003 
00006 
00007 // Copyright (C) 1991,2,3,4: R B Davies
00008 
00009 #include "include.h"
00010 
00011 #include "newmat.h"
00012 #include "newmatrc.h"
00013 
00014 #ifdef use_namespace
00015 namespace NEWMAT {
00016 #endif
00017 
00018 
00019 #ifdef DO_REPORT
00020 #define REPORT { static ExeCounter ExeCount(__LINE__,7); ++ExeCount; }
00021 #else
00022 #define REPORT {}
00023 #endif
00024 
00025 
00026 //***************************** solve routines ******************************/
00027 
00028 GeneralMatrix* GeneralMatrix::MakeSolver()
00029 {
00030    REPORT
00031    GeneralMatrix* gm = new CroutMatrix(*this);
00032    MatrixErrorNoSpace(gm); gm->ReleaseAndDelete(); return gm;
00033 }
00034 
00035 GeneralMatrix* Matrix::MakeSolver()
00036 {
00037    REPORT
00038    GeneralMatrix* gm = new CroutMatrix(*this);
00039    MatrixErrorNoSpace(gm); gm->ReleaseAndDelete(); return gm;
00040 }
00041 
00042 void CroutMatrix::Solver(MatrixColX& mcout, const MatrixColX& mcin)
00043 {
00044    REPORT
00045    int i = mcin.skip; Real* el = mcin.data-i; Real* el1 = el;
00046    while (i--) *el++ = 0.0;
00047    el += mcin.storage; i = nrows_val - mcin.skip - mcin.storage;
00048    while (i--) *el++ = 0.0;
00049    lubksb(el1, mcout.skip);
00050 }
00051 
00052 
00053 // Do we need check for entirely zero output?
00054 
00055 void UpperTriangularMatrix::Solver(MatrixColX& mcout,
00056    const MatrixColX& mcin)
00057 {
00058    REPORT
00059    int i = mcin.skip-mcout.skip; Real* elx = mcin.data-i;
00060    while (i-- > 0) *elx++ = 0.0;
00061    int nr = mcin.skip+mcin.storage;
00062    elx = mcin.data+mcin.storage; Real* el = elx;
00063    int j = mcout.skip+mcout.storage-nr;
00064    int nc = ncols_val-nr; i = nr-mcout.skip;
00065    while (j-- > 0) *elx++ = 0.0;
00066    Real* Ael = store + (nr*(2*ncols_val-nr+1))/2; j = 0;
00067    while (i-- > 0)
00068    {
00069       elx = el; Real sum = 0.0; int jx = j++; Ael -= nc;
00070       while (jx--) sum += *(--Ael) * *(--elx);
00071       elx--; *elx = (*elx - sum) / *(--Ael);
00072    }
00073 }
00074 
00075 void LowerTriangularMatrix::Solver(MatrixColX& mcout,
00076    const MatrixColX& mcin)
00077 {
00078    REPORT
00079    int i = mcin.skip-mcout.skip; Real* elx = mcin.data-i;
00080    while (i-- > 0) *elx++ = 0.0;
00081    int nc = mcin.skip; i = nc+mcin.storage; elx = mcin.data+mcin.storage;
00082    int nr = mcout.skip+mcout.storage; int j = nr-i; i = nr-nc;
00083    while (j-- > 0) *elx++ = 0.0;
00084    Real* el = mcin.data; Real* Ael = store + (nc*(nc+1))/2; j = 0;
00085    while (i-- > 0)
00086    {
00087       elx = el; Real sum = 0.0; int jx = j++; Ael += nc;
00088       while (jx--) sum += *Ael++ * *elx++;
00089       *elx = (*elx - sum) / *Ael++;
00090    }
00091 }
00092 
00093 //******************* carry out binary operations *************************/
00094 
00095 static GeneralMatrix*
00096    GeneralMult(GeneralMatrix*,GeneralMatrix*,MultipliedMatrix*,MatrixType);
00097 static GeneralMatrix*
00098    GeneralSolv(GeneralMatrix*,GeneralMatrix*,BaseMatrix*,MatrixType);
00099 static GeneralMatrix*
00100    GeneralSolvI(GeneralMatrix*,BaseMatrix*,MatrixType);
00101 static GeneralMatrix*
00102    GeneralKP(GeneralMatrix*,GeneralMatrix*,KPMatrix*,MatrixType);
00103 
00104 GeneralMatrix* MultipliedMatrix::Evaluate(MatrixType mt)
00105 {
00106    REPORT
00107    gm2 = ((BaseMatrix*&)bm2)->Evaluate();
00108    gm2 = gm2->Evaluate(gm2->type().MultRHS());     // no symmetric on RHS
00109    gm1 = ((BaseMatrix*&)bm1)->Evaluate();
00110    return GeneralMult(gm1, gm2, this, mt);
00111 }
00112 
00113 GeneralMatrix* SolvedMatrix::Evaluate(MatrixType mt)
00114 {
00115    REPORT
00116    gm1 = ((BaseMatrix*&)bm1)->Evaluate();
00117    gm2 = ((BaseMatrix*&)bm2)->Evaluate();
00118    return GeneralSolv(gm1,gm2,this,mt);
00119 }
00120 
00121 GeneralMatrix* KPMatrix::Evaluate(MatrixType mt)
00122 {
00123    REPORT
00124    gm1 = ((BaseMatrix*&)bm1)->Evaluate();
00125    gm2 = ((BaseMatrix*&)bm2)->Evaluate();
00126    return GeneralKP(gm1,gm2,this,mt);
00127 }
00128 
00129 // routines for adding or subtracting matrices of identical storage structure
00130 
00131 static void Add(GeneralMatrix* gm, GeneralMatrix* gm1, GeneralMatrix* gm2)
00132 {
00133    REPORT
00134    Real* s1=gm1->Store(); Real* s2=gm2->Store();
00135    Real* s=gm->Store(); int i=gm->Storage() >> 2;
00136    while (i--)
00137    {
00138        *s++ = *s1++ + *s2++; *s++ = *s1++ + *s2++;
00139        *s++ = *s1++ + *s2++; *s++ = *s1++ + *s2++;
00140    }
00141    i=gm->Storage() & 3; while (i--) *s++ = *s1++ + *s2++;
00142 }
00143 
00144 static void AddTo(GeneralMatrix* gm, const GeneralMatrix* gm2)
00145 {
00146    REPORT
00147    const Real* s2=gm2->Store(); Real* s=gm->Store(); int i=gm->Storage() >> 2;
00148    while (i--)
00149    { *s++ += *s2++; *s++ += *s2++; *s++ += *s2++; *s++ += *s2++; }
00150    i=gm->Storage() & 3; while (i--) *s++ += *s2++;
00151 }
00152 
00153 void GeneralMatrix::PlusEqual(const GeneralMatrix& gm)
00154 {
00155    REPORT
00156    if (nrows_val != gm.nrows_val || ncols_val != gm.ncols_val)
00157       Throw(IncompatibleDimensionsException(*this, gm));
00158    AddTo(this, &gm);
00159 }
00160 
00161 static void Subtract(GeneralMatrix* gm, GeneralMatrix* gm1, GeneralMatrix* gm2)
00162 {
00163    REPORT
00164    Real* s1=gm1->Store(); Real* s2=gm2->Store();
00165    Real* s=gm->Store(); int i=gm->Storage() >> 2;
00166    while (i--)
00167    {
00168        *s++ = *s1++ - *s2++; *s++ = *s1++ - *s2++;
00169        *s++ = *s1++ - *s2++; *s++ = *s1++ - *s2++;
00170    }
00171    i=gm->Storage() & 3; while (i--) *s++ = *s1++ - *s2++;
00172 }
00173 
00174 static void SubtractFrom(GeneralMatrix* gm, const GeneralMatrix* gm2)
00175 {
00176    REPORT
00177    const Real* s2=gm2->Store(); Real* s=gm->Store(); int i=gm->Storage() >> 2;
00178    while (i--)
00179    { *s++ -= *s2++; *s++ -= *s2++; *s++ -= *s2++; *s++ -= *s2++; }
00180    i=gm->Storage() & 3; while (i--) *s++ -= *s2++;
00181 }
00182 
00183 void GeneralMatrix::MinusEqual(const GeneralMatrix& gm)
00184 {
00185    REPORT
00186    if (nrows_val != gm.nrows_val || ncols_val != gm.ncols_val)
00187       Throw(IncompatibleDimensionsException(*this, gm));
00188    SubtractFrom(this, &gm);
00189 }
00190 
00191 static void ReverseSubtract(GeneralMatrix* gm, const GeneralMatrix* gm2)
00192 {
00193    REPORT
00194    const Real* s2=gm2->Store(); Real* s=gm->Store(); int i=gm->Storage() >> 2;
00195    while (i--)
00196    {
00197       *s = *s2++ - *s; s++; *s = *s2++ - *s; s++;
00198       *s = *s2++ - *s; s++; *s = *s2++ - *s; s++;
00199    }
00200    i=gm->Storage() & 3; while (i--) { *s = *s2++ - *s; s++; }
00201 }
00202 
00203 static void SP(GeneralMatrix* gm, GeneralMatrix* gm1, GeneralMatrix* gm2)
00204 {
00205    REPORT
00206    Real* s1=gm1->Store(); Real* s2=gm2->Store();
00207    Real* s=gm->Store(); int i=gm->Storage() >> 2;
00208    while (i--)
00209    {
00210        *s++ = *s1++ * *s2++; *s++ = *s1++ * *s2++;
00211        *s++ = *s1++ * *s2++; *s++ = *s1++ * *s2++;
00212    }
00213    i=gm->Storage() & 3; while (i--) *s++ = *s1++ * *s2++;
00214 }
00215 
00216 static void SP(GeneralMatrix* gm, GeneralMatrix* gm2)
00217 {
00218    REPORT
00219    Real* s2=gm2->Store(); Real* s=gm->Store(); int i=gm->Storage() >> 2;
00220    while (i--)
00221    { *s++ *= *s2++; *s++ *= *s2++; *s++ *= *s2++; *s++ *= *s2++; }
00222    i=gm->Storage() & 3; while (i--) *s++ *= *s2++;
00223 }
00224 
00225 // routines for adding or subtracting matrices of different storage structure
00226 
00227 static void AddDS(GeneralMatrix* gm, GeneralMatrix* gm1, GeneralMatrix* gm2)
00228 {
00229    REPORT
00230    int nr = gm->Nrows();
00231    MatrixRow mr1(gm1, LoadOnEntry); MatrixRow mr2(gm2, LoadOnEntry);
00232    MatrixRow mr(gm, StoreOnExit+DirectPart);
00233    while (nr--) { mr.Add(mr1,mr2); mr1.Next(); mr2.Next(); mr.Next(); }
00234 }
00235 
00236 static void AddDS(GeneralMatrix* gm, GeneralMatrix* gm2)
00237 // Add into first argument
00238 {
00239    REPORT
00240    int nr = gm->Nrows();
00241    MatrixRow mr(gm, StoreOnExit+LoadOnEntry+DirectPart);
00242    MatrixRow mr2(gm2, LoadOnEntry);
00243    while (nr--) { mr.Add(mr2); mr.Next(); mr2.Next(); }
00244 }
00245 
00246 static void SubtractDS
00247    (GeneralMatrix* gm, GeneralMatrix* gm1, GeneralMatrix* gm2)
00248 {
00249    REPORT
00250    int nr = gm->Nrows();
00251    MatrixRow mr1(gm1, LoadOnEntry); MatrixRow mr2(gm2, LoadOnEntry);
00252    MatrixRow mr(gm, StoreOnExit+DirectPart);
00253    while (nr--) { mr.Sub(mr1,mr2); mr1.Next(); mr2.Next(); mr.Next(); }
00254 }
00255 
00256 static void SubtractDS(GeneralMatrix* gm, GeneralMatrix* gm2)
00257 {
00258    REPORT
00259    int nr = gm->Nrows();
00260    MatrixRow mr(gm, LoadOnEntry+StoreOnExit+DirectPart);
00261    MatrixRow mr2(gm2, LoadOnEntry);
00262    while (nr--) { mr.Sub(mr2); mr.Next(); mr2.Next(); }
00263 }
00264 
00265 static void ReverseSubtractDS(GeneralMatrix* gm, GeneralMatrix* gm2)
00266 {
00267    REPORT
00268    int nr = gm->Nrows();
00269    MatrixRow mr(gm, LoadOnEntry+StoreOnExit+DirectPart);
00270    MatrixRow mr2(gm2, LoadOnEntry);
00271    while (nr--) { mr.RevSub(mr2); mr2.Next(); mr.Next(); }
00272 }
00273 
00274 static void SPDS(GeneralMatrix* gm, GeneralMatrix* gm1, GeneralMatrix* gm2)
00275 {
00276    REPORT
00277    int nr = gm->Nrows();
00278    MatrixRow mr1(gm1, LoadOnEntry); MatrixRow mr2(gm2, LoadOnEntry);
00279    MatrixRow mr(gm, StoreOnExit+DirectPart);
00280    while (nr--) { mr.Multiply(mr1,mr2); mr1.Next(); mr2.Next(); mr.Next(); }
00281 }
00282 
00283 static void SPDS(GeneralMatrix* gm, GeneralMatrix* gm2)
00284 // SP into first argument
00285 {
00286    REPORT
00287    int nr = gm->Nrows();
00288    MatrixRow mr(gm, StoreOnExit+LoadOnEntry+DirectPart);
00289    MatrixRow mr2(gm2, LoadOnEntry);
00290    while (nr--) { mr.Multiply(mr2); mr.Next(); mr2.Next(); }
00291 }
00292 
00293 static GeneralMatrix* GeneralMult1(GeneralMatrix* gm1, GeneralMatrix* gm2,
00294    MultipliedMatrix* mm, MatrixType mtx)
00295 {
00296    REPORT
00297    Tracer tr("GeneralMult1");
00298    int nr=gm1->Nrows(); int nc=gm2->Ncols();
00299    if (gm1->Ncols() !=gm2->Nrows())
00300       Throw(IncompatibleDimensionsException(*gm1, *gm2));
00301    GeneralMatrix* gmx = mtx.New(nr,nc,mm);
00302 
00303    MatrixCol mcx(gmx, StoreOnExit+DirectPart);
00304    MatrixCol mc2(gm2, LoadOnEntry);
00305    while (nc--)
00306    {
00307       MatrixRow mr1(gm1, LoadOnEntry, mcx.Skip());
00308       Real* el = mcx.Data();                         // pointer to an element
00309       int n = mcx.Storage();
00310       while (n--) { *(el++) = DotProd(mr1,mc2); mr1.Next(); }
00311       mc2.Next(); mcx.Next();
00312    }
00313    gmx->ReleaseAndDelete(); gm1->tDelete(); gm2->tDelete(); return gmx;
00314 }
00315 
00316 static GeneralMatrix* GeneralMult2(GeneralMatrix* gm1, GeneralMatrix* gm2,
00317    MultipliedMatrix* mm, MatrixType mtx)
00318 {
00319    // version that accesses by row only - not good for thin matrices
00320    // or column vectors in right hand term.
00321    REPORT
00322    Tracer tr("GeneralMult2");
00323    int nr=gm1->Nrows(); int nc=gm2->Ncols();
00324    if (gm1->Ncols() !=gm2->Nrows())
00325       Throw(IncompatibleDimensionsException(*gm1, *gm2));
00326    GeneralMatrix* gmx = mtx.New(nr,nc,mm);
00327 
00328    MatrixRow mrx(gmx, LoadOnEntry+StoreOnExit+DirectPart);
00329    MatrixRow mr1(gm1, LoadOnEntry);
00330    while (nr--)
00331    {
00332       MatrixRow mr2(gm2, LoadOnEntry, mr1.Skip());
00333       Real* el = mr1.Data();                         // pointer to an element
00334       int n = mr1.Storage();
00335       mrx.Zero();
00336       while (n--) { mrx.AddScaled(mr2, *el++); mr2.Next(); }
00337       mr1.Next(); mrx.Next();
00338    }
00339    gmx->ReleaseAndDelete(); gm1->tDelete(); gm2->tDelete(); return gmx;
00340 }
00341 
00342 static GeneralMatrix* mmMult(GeneralMatrix* gm1, GeneralMatrix* gm2)
00343 {
00344    // matrix multiplication for type Matrix only
00345    REPORT
00346    Tracer tr("MatrixMult");
00347 
00348    int nr=gm1->Nrows(); int ncr=gm1->Ncols(); int nc=gm2->Ncols();
00349    if (ncr != gm2->Nrows()) Throw(IncompatibleDimensionsException(*gm1,*gm2));
00350 
00351    Matrix* gm = new Matrix(nr,nc); MatrixErrorNoSpace(gm);
00352 
00353    Real* s1=gm1->Store(); Real* s2=gm2->Store(); Real* s=gm->Store();
00354 
00355    if (ncr)
00356    {
00357       while (nr--)
00358       {
00359          Real* s2x = s2; int j = ncr;
00360          Real* sx = s; Real f = *s1++; int k = nc;
00361          while (k--) *sx++ = f * *s2x++;
00362          while (--j)
00363             { sx = s; f = *s1++; k = nc; while (k--) *sx++ += f * *s2x++; }
00364          s = sx;
00365       }
00366    }
00367    else *gm = 0.0;
00368 
00369    gm->ReleaseAndDelete(); gm1->tDelete(); gm2->tDelete(); return gm;
00370 }
00371 
00372 static GeneralMatrix* GeneralMult(GeneralMatrix* gm1, GeneralMatrix* gm2,
00373    MultipliedMatrix* mm, MatrixType mtx)
00374 {
00375    if ( Rectangular(gm1->type(), gm2->type(), mtx))
00376       { REPORT return mmMult(gm1, gm2); }
00377    Compare(gm1->type() * gm2->type(),mtx);
00378    int nr = gm2->Nrows(); int nc = gm2->Ncols();
00379    if (nc <= 5 && nr > nc) { REPORT return GeneralMult1(gm1, gm2, mm, mtx); }
00380    REPORT return GeneralMult2(gm1, gm2, mm, mtx);
00381 }
00382 
00383 static GeneralMatrix* GeneralKP(GeneralMatrix* gm1, GeneralMatrix* gm2,
00384    KPMatrix* kp, MatrixType mtx)
00385 {
00386    REPORT
00387    Tracer tr("GeneralKP");
00388    int nr1 = gm1->Nrows(); int nc1 = gm1->Ncols();
00389    int nr2 = gm2->Nrows(); int nc2 = gm2->Ncols();
00390    Compare((gm1->type()).KP(gm2->type()),mtx);
00391    GeneralMatrix* gmx = mtx.New(nr1*nr2, nc1*nc2, kp);
00392    MatrixRow mrx(gmx, LoadOnEntry+StoreOnExit+DirectPart);
00393    MatrixRow mr1(gm1, LoadOnEntry);
00394    for (int i = 1; i <= nr1; ++i)
00395    {
00396       MatrixRow mr2(gm2, LoadOnEntry);
00397       for (int j = 1; j <= nr2; ++j)
00398          { mrx.KP(mr1,mr2); mr2.Next(); mrx.Next(); }
00399       mr1.Next();
00400    }
00401    gmx->ReleaseAndDelete(); gm1->tDelete(); gm2->tDelete(); return gmx;
00402 }
00403 
00404 static GeneralMatrix* GeneralSolv(GeneralMatrix* gm1, GeneralMatrix* gm2,
00405    BaseMatrix* sm, MatrixType mtx)
00406 {
00407    REPORT
00408    Tracer tr("GeneralSolv");
00409    Compare(gm1->type().i() * gm2->type(),mtx);
00410    int nr = gm1->Nrows();
00411    if (nr != gm1->Ncols()) Throw(NotSquareException(*gm1));
00412    int nc = gm2->Ncols();
00413    if (gm1->Ncols() != gm2->Nrows())
00414       Throw(IncompatibleDimensionsException(*gm1, *gm2));
00415    GeneralMatrix* gmx = mtx.New(nr,nc,sm); MatrixErrorNoSpace(gmx);
00416    Real* r = new Real [nr]; MatrixErrorNoSpace(r);
00417    MONITOR_REAL_NEW("Make   (GenSolv)",nr,r)
00418    GeneralMatrix* gms = gm1->MakeSolver();
00419    Try
00420    {
00421 
00422       MatrixColX mcx(gmx, r, StoreOnExit+DirectPart);   // copy to and from r
00423          // this must be inside Try so mcx is destroyed before gmx
00424       MatrixColX mc2(gm2, r, LoadOnEntry);
00425       while (nc--) { gms->Solver(mcx, mc2); mcx.Next(); mc2.Next(); }
00426    }
00427    CatchAll
00428    {
00429       if (gms) gms->tDelete();
00430       delete gmx;                   // <--------------------
00431       gm2->tDelete();
00432       MONITOR_REAL_DELETE("Delete (GenSolv)",nr,r)
00433                           // AT&T version 2.1 gives an internal error
00434       delete [] r;
00435       ReThrow;
00436    }
00437    gms->tDelete(); gmx->ReleaseAndDelete(); gm2->tDelete();
00438    MONITOR_REAL_DELETE("Delete (GenSolv)",nr,r)
00439                           // AT&T version 2.1 gives an internal error
00440    delete [] r;
00441    return gmx;
00442 }
00443 
00444 // version for inverses - gm2 is identity
00445 static GeneralMatrix* GeneralSolvI(GeneralMatrix* gm1, BaseMatrix* sm,
00446    MatrixType mtx)
00447 {
00448    REPORT
00449    Tracer tr("GeneralSolvI");
00450    Compare(gm1->type().i(),mtx);
00451    int nr = gm1->Nrows();
00452    if (nr != gm1->Ncols()) Throw(NotSquareException(*gm1));
00453    int nc = nr;
00454    // DiagonalMatrix I(nr); I = 1;
00455    IdentityMatrix I(nr);
00456    GeneralMatrix* gmx = mtx.New(nr,nc,sm); MatrixErrorNoSpace(gmx);
00457    Real* r = new Real [nr]; MatrixErrorNoSpace(r);
00458    MONITOR_REAL_NEW("Make   (GenSolvI)",nr,r)
00459    GeneralMatrix* gms = gm1->MakeSolver();
00460    Try
00461    {
00462 
00463       MatrixColX mcx(gmx, r, StoreOnExit+DirectPart);   // copy to and from r
00464          // this must be inside Try so mcx is destroyed before gmx
00465       MatrixColX mc2(&I, r, LoadOnEntry);
00466       while (nc--) { gms->Solver(mcx, mc2); mcx.Next(); mc2.Next(); }
00467    }
00468    CatchAll
00469    {
00470       if (gms) gms->tDelete();
00471       delete gmx;
00472       MONITOR_REAL_DELETE("Delete (GenSolvI)",nr,r)
00473                           // AT&T version 2.1 gives an internal error
00474       delete [] r;
00475       ReThrow;
00476    }
00477    gms->tDelete(); gmx->ReleaseAndDelete();
00478    MONITOR_REAL_DELETE("Delete (GenSolvI)",nr,r)
00479                           // AT&T version 2.1 gives an internal error
00480    delete [] r;
00481    return gmx;
00482 }
00483 
00484 GeneralMatrix* InvertedMatrix::Evaluate(MatrixType mtx)
00485 {
00486    // Matrix Inversion - use solve routines
00487    Tracer tr("InvertedMatrix::Evaluate");
00488    REPORT
00489    gm=((BaseMatrix*&)bm)->Evaluate();
00490    return GeneralSolvI(gm,this,mtx);
00491 }
00492 
00493 //*************************** New versions ************************
00494 
00495 GeneralMatrix* AddedMatrix::Evaluate(MatrixType mtd)
00496 {
00497    REPORT
00498    Tracer tr("AddedMatrix::Evaluate");
00499    gm1=((BaseMatrix*&)bm1)->Evaluate(); gm2=((BaseMatrix*&)bm2)->Evaluate();
00500    int nr=gm1->Nrows(); int nc=gm1->Ncols();
00501    if (nr!=gm2->Nrows() || nc!=gm2->Ncols())
00502    {
00503       Try { Throw(IncompatibleDimensionsException(*gm1, *gm2)); }
00504       CatchAll
00505       {
00506          gm1->tDelete(); gm2->tDelete();
00507          ReThrow;
00508       }
00509    }
00510    MatrixType mt1 = gm1->type(), mt2 = gm2->type(); MatrixType mts = mt1 + mt2;
00511    if (!mtd) { REPORT mtd = mts; }
00512    else if (!(mtd.DataLossOK || mtd >= mts))
00513    {
00514       REPORT
00515       gm1->tDelete(); gm2->tDelete();
00516       Throw(ProgramException("Illegal Conversion", mts, mtd));
00517    }
00518    GeneralMatrix* gmx;
00519    bool c1 = (mtd == mt1), c2 = (mtd == mt2);
00520    if ( c1 && c2 && (gm1->SimpleAddOK(gm2) == 0) )
00521    {
00522       if (gm1->reuse()) { REPORT AddTo(gm1,gm2); gm2->tDelete(); gmx = gm1; }
00523       else if (gm2->reuse()) { REPORT AddTo(gm2,gm1); gmx = gm2; }
00524       else
00525       {
00526          REPORT
00527          // what if new throws an exception
00528          Try { gmx = mt1.New(nr,nc,this); }
00529          CatchAll
00530          {
00531             ReThrow;
00532          }
00533          gmx->ReleaseAndDelete(); Add(gmx,gm1,gm2);
00534       }
00535    }
00536    else
00537    {
00538       if (c1 && c2)
00539       {
00540          short SAO = gm1->SimpleAddOK(gm2);
00541          if (SAO & 1) { REPORT c1 = false; }
00542          if (SAO & 2) { REPORT c2 = false; }
00543       }
00544       if (c1 && gm1->reuse() )               // must have type test first
00545          { REPORT AddDS(gm1,gm2); gm2->tDelete(); gmx = gm1; }
00546       else if (c2 && gm2->reuse() )
00547          { REPORT AddDS(gm2,gm1); if (!c1) gm1->tDelete(); gmx = gm2; }
00548       else
00549       {
00550          REPORT
00551          Try { gmx = mtd.New(nr,nc,this); }
00552          CatchAll
00553          {
00554             if (!c1) gm1->tDelete(); if (!c2) gm2->tDelete();
00555             ReThrow;
00556          }
00557          AddDS(gmx,gm1,gm2);
00558          if (!c1) gm1->tDelete(); if (!c2) gm2->tDelete();
00559          gmx->ReleaseAndDelete();
00560       }
00561    }
00562    return gmx;
00563 }
00564 
00565 GeneralMatrix* SubtractedMatrix::Evaluate(MatrixType mtd)
00566 {
00567    REPORT
00568    Tracer tr("SubtractedMatrix::Evaluate");
00569    gm1=((BaseMatrix*&)bm1)->Evaluate(); gm2=((BaseMatrix*&)bm2)->Evaluate();
00570    int nr=gm1->Nrows(); int nc=gm1->Ncols();
00571    if (nr!=gm2->Nrows() || nc!=gm2->Ncols())
00572    {
00573       Try { Throw(IncompatibleDimensionsException(*gm1, *gm2)); }
00574       CatchAll
00575       {
00576          gm1->tDelete(); gm2->tDelete();
00577          ReThrow;
00578       }
00579    }
00580    MatrixType mt1 = gm1->type(), mt2 = gm2->type(); MatrixType mts = mt1 + mt2;
00581    if (!mtd) { REPORT mtd = mts; }
00582    else if (!(mtd.DataLossOK || mtd >= mts))
00583    {
00584       gm1->tDelete(); gm2->tDelete();
00585       Throw(ProgramException("Illegal Conversion", mts, mtd));
00586    }
00587    GeneralMatrix* gmx;
00588    bool c1 = (mtd == mt1), c2 = (mtd == mt2);
00589    if ( c1 && c2 && (gm1->SimpleAddOK(gm2) == 0) )
00590    {
00591       if (gm1->reuse())
00592          { REPORT SubtractFrom(gm1,gm2); gm2->tDelete(); gmx = gm1; }
00593       else if (gm2->reuse()) { REPORT ReverseSubtract(gm2,gm1); gmx = gm2; }
00594       else
00595       {
00596          REPORT
00597          Try { gmx = mt1.New(nr,nc,this); }
00598          CatchAll
00599          {
00600             ReThrow;
00601          }
00602          gmx->ReleaseAndDelete(); Subtract(gmx,gm1,gm2);
00603       }
00604    }
00605    else
00606    {
00607       if (c1 && c2)
00608       {
00609          short SAO = gm1->SimpleAddOK(gm2);
00610          if (SAO & 1) { REPORT c1 = false; }
00611          if (SAO & 2) { REPORT c2 = false; }
00612       }
00613       if (c1 && gm1->reuse() )               // must have type test first
00614          { REPORT SubtractDS(gm1,gm2); gm2->tDelete(); gmx = gm1; }
00615       else if (c2 && gm2->reuse() )
00616       {
00617          REPORT ReverseSubtractDS(gm2,gm1);
00618          if (!c1) gm1->tDelete(); gmx = gm2;
00619       }
00620       else
00621       {
00622          REPORT
00623          // what if New throws and exception
00624          Try { gmx = mtd.New(nr,nc,this); }
00625          CatchAll
00626          {
00627             if (!c1) gm1->tDelete(); if (!c2) gm2->tDelete();
00628             ReThrow;
00629          }
00630          SubtractDS(gmx,gm1,gm2);
00631          if (!c1) gm1->tDelete(); if (!c2) gm2->tDelete();
00632          gmx->ReleaseAndDelete();
00633       }
00634    }
00635    return gmx;
00636 }
00637 
00638 GeneralMatrix* SPMatrix::Evaluate(MatrixType mtd)
00639 {
00640    REPORT
00641    Tracer tr("SPMatrix::Evaluate");
00642    gm1=((BaseMatrix*&)bm1)->Evaluate(); gm2=((BaseMatrix*&)bm2)->Evaluate();
00643    int nr=gm1->Nrows(); int nc=gm1->Ncols();
00644    if (nr!=gm2->Nrows() || nc!=gm2->Ncols())
00645    {
00646       Try { Throw(IncompatibleDimensionsException(*gm1, *gm2)); }
00647       CatchAll
00648       {
00649          gm1->tDelete(); gm2->tDelete();
00650          ReThrow;
00651       }
00652    }
00653    MatrixType mt1 = gm1->type(), mt2 = gm2->type();
00654    MatrixType mts = mt1.SP(mt2);
00655    if (!mtd) { REPORT mtd = mts; }
00656    else if (!(mtd.DataLossOK || mtd >= mts))
00657    {
00658       gm1->tDelete(); gm2->tDelete();
00659       Throw(ProgramException("Illegal Conversion", mts, mtd));
00660    }
00661    GeneralMatrix* gmx;
00662    bool c1 = (mtd == mt1), c2 = (mtd == mt2);
00663    if ( c1 && c2 && (gm1->SimpleAddOK(gm2) == 0) )
00664    {
00665       if (gm1->reuse()) { REPORT SP(gm1,gm2); gm2->tDelete(); gmx = gm1; }
00666       else if (gm2->reuse()) { REPORT SP(gm2,gm1); gmx = gm2; }
00667       else
00668       {
00669          REPORT
00670          Try { gmx = mt1.New(nr,nc,this); }
00671          CatchAll
00672          {
00673             ReThrow;
00674          }
00675          gmx->ReleaseAndDelete(); SP(gmx,gm1,gm2);
00676       }
00677    }
00678    else
00679    {
00680       if (c1 && c2)
00681       {
00682          short SAO = gm1->SimpleAddOK(gm2);
00683          if (SAO & 1) { REPORT c2 = false; }    // c1 and c2 swapped
00684          if (SAO & 2) { REPORT c1 = false; }
00685       }
00686       if (c1 && gm1->reuse() )               // must have type test first
00687          { REPORT SPDS(gm1,gm2); gm2->tDelete(); gmx = gm1; }
00688       else if (c2 && gm2->reuse() )
00689          { REPORT SPDS(gm2,gm1); if (!c1) gm1->tDelete(); gmx = gm2; }
00690       else
00691       {
00692          REPORT
00693          // what if New throws and exception
00694          Try { gmx = mtd.New(nr,nc,this); }
00695          CatchAll
00696          {
00697             if (!c1) gm1->tDelete(); if (!c2) gm2->tDelete();
00698             ReThrow;
00699          }
00700          SPDS(gmx,gm1,gm2);
00701          if (!c1) gm1->tDelete(); if (!c2) gm2->tDelete();
00702          gmx->ReleaseAndDelete();
00703       }
00704    }
00705    return gmx;
00706 }
00707 
00708 
00709 
00710 //*************************** norm functions ****************************/
00711 
00712 Real BaseMatrix::norm1() const
00713 {
00714    // maximum of sum of absolute values of a column
00715    REPORT
00716    GeneralMatrix* gm = ((BaseMatrix&)*this).Evaluate();
00717    int nc = gm->Ncols(); Real value = 0.0;
00718    MatrixCol mc(gm, LoadOnEntry);
00719    while (nc--)
00720       { Real v = mc.SumAbsoluteValue(); if (value < v) value = v; mc.Next(); }
00721    gm->tDelete(); return value;
00722 }
00723 
00724 Real BaseMatrix::norm_infinity() const
00725 {
00726    // maximum of sum of absolute values of a row
00727    REPORT
00728    GeneralMatrix* gm = ((BaseMatrix&)*this).Evaluate();
00729    int nr = gm->Nrows(); Real value = 0.0;
00730    MatrixRow mr(gm, LoadOnEntry);
00731    while (nr--)
00732       { Real v = mr.SumAbsoluteValue(); if (value < v) value = v; mr.Next(); }
00733    gm->tDelete(); return value;
00734 }
00735 
00736 //********************** Concatenation and stacking *************************/
00737 
00738 GeneralMatrix* ConcatenatedMatrix::Evaluate(MatrixType mtx)
00739 {
00740    REPORT
00741    Tracer tr("Concatenate");
00742       gm2 = ((BaseMatrix*&)bm2)->Evaluate();
00743       gm1 = ((BaseMatrix*&)bm1)->Evaluate();
00744       Compare(gm1->type() | gm2->type(),mtx);
00745       int nr=gm1->Nrows(); int nc = gm1->Ncols() + gm2->Ncols();
00746       if (nr != gm2->Nrows())
00747          Throw(IncompatibleDimensionsException(*gm1, *gm2));
00748       GeneralMatrix* gmx = mtx.New(nr,nc,this);
00749       MatrixRow mr1(gm1, LoadOnEntry); MatrixRow mr2(gm2, LoadOnEntry);
00750       MatrixRow mr(gmx, StoreOnExit+DirectPart);
00751       while (nr--) { mr.ConCat(mr1,mr2); mr1.Next(); mr2.Next(); mr.Next(); }
00752       gmx->ReleaseAndDelete(); gm1->tDelete(); gm2->tDelete(); return gmx;
00753 }
00754 
00755 GeneralMatrix* StackedMatrix::Evaluate(MatrixType mtx)
00756 {
00757    REPORT
00758    Tracer tr("Stack");
00759       gm2 = ((BaseMatrix*&)bm2)->Evaluate();
00760       gm1 = ((BaseMatrix*&)bm1)->Evaluate();
00761       Compare(gm1->type() & gm2->type(),mtx);
00762       int nc=gm1->Ncols();
00763       int nr1 = gm1->Nrows(); int nr2 = gm2->Nrows();
00764       if (nc != gm2->Ncols())
00765          Throw(IncompatibleDimensionsException(*gm1, *gm2));
00766       GeneralMatrix* gmx = mtx.New(nr1+nr2,nc,this);
00767       MatrixRow mr1(gm1, LoadOnEntry); MatrixRow mr2(gm2, LoadOnEntry);
00768       MatrixRow mr(gmx, StoreOnExit+DirectPart);
00769       while (nr1--) { mr.Copy(mr1); mr1.Next(); mr.Next(); }
00770       while (nr2--) { mr.Copy(mr2); mr2.Next(); mr.Next(); }
00771       gmx->ReleaseAndDelete(); gm1->tDelete(); gm2->tDelete(); return gmx;
00772 }
00773 
00774 // ************************* equality of matrices ******************** //
00775 
00776 static bool RealEqual(Real* s1, Real* s2, int n)
00777 {
00778    int i = n >> 2;
00779    while (i--)
00780    {
00781       if (*s1++ != *s2++) return false; if (*s1++ != *s2++) return false;
00782       if (*s1++ != *s2++) return false; if (*s1++ != *s2++) return false;
00783    }
00784    i = n & 3; while (i--) if (*s1++ != *s2++) return false;
00785    return true;
00786 }
00787 
00788 static bool intEqual(int* s1, int* s2, int n)
00789 {
00790    int i = n >> 2;
00791    while (i--)
00792    {
00793       if (*s1++ != *s2++) return false; if (*s1++ != *s2++) return false;
00794       if (*s1++ != *s2++) return false; if (*s1++ != *s2++) return false;
00795    }
00796    i = n & 3; while (i--) if (*s1++ != *s2++) return false;
00797    return true;
00798 }
00799 
00800 
00801 bool operator==(const BaseMatrix& A, const BaseMatrix& B)
00802 {
00803    Tracer tr("BaseMatrix ==");
00804    REPORT
00805    GeneralMatrix* gmA = ((BaseMatrix&)A).Evaluate();
00806    GeneralMatrix* gmB = ((BaseMatrix&)B).Evaluate();
00807 
00808    if (gmA == gmB)                            // same matrix
00809       { REPORT gmA->tDelete(); return true; }
00810 
00811    if ( gmA->Nrows() != gmB->Nrows() || gmA->Ncols() != gmB->Ncols() )
00812                                               // different dimensions
00813       { REPORT gmA->tDelete(); gmB->tDelete(); return false; }
00814 
00815    // check for CroutMatrix or BandLUMatrix
00816    MatrixType AType = gmA->type(); MatrixType BType = gmB->type();
00817    if (AType.CannotConvert() || BType.CannotConvert() )
00818    {
00819       REPORT
00820       bool bx = gmA->IsEqual(*gmB);
00821       gmA->tDelete(); gmB->tDelete();
00822       return bx;
00823    }
00824 
00825    // is matrix storage the same
00826    // will need to modify if further matrix structures are introduced
00827    if (AType == BType && gmA->bandwidth() == gmB->bandwidth())
00828    {                                          // compare store
00829       REPORT
00830       bool bx = RealEqual(gmA->Store(),gmB->Store(),gmA->Storage());
00831       gmA->tDelete(); gmB->tDelete();
00832       return bx;
00833    }
00834 
00835    // matrix storage different - just subtract
00836    REPORT  return is_zero(*gmA-*gmB);
00837 }
00838 
00839 bool operator==(const GeneralMatrix& A, const GeneralMatrix& B)
00840 {
00841    Tracer tr("GeneralMatrix ==");
00842    // May or may not call tDeletes
00843    REPORT
00844 
00845    if (&A == &B)                              // same matrix
00846       { REPORT return true; }
00847 
00848    if ( A.Nrows() != B.Nrows() || A.Ncols() != B.Ncols() )
00849       { REPORT return false; }                // different dimensions
00850 
00851    // check for CroutMatrix or BandLUMatrix
00852    MatrixType AType = A.Type(); MatrixType BType = B.Type();
00853    if (AType.CannotConvert() || BType.CannotConvert() )
00854       { REPORT  return A.IsEqual(B); }
00855 
00856    // is matrix storage the same
00857    // will need to modify if further matrix structures are introduced
00858    if (AType == BType && A.bandwidth() == B.bandwidth())
00859       { REPORT return RealEqual(A.Store(),B.Store(),A.Storage()); }
00860 
00861    // matrix storage different - just subtract
00862    REPORT  return is_zero(A-B);
00863 }
00864 
00865 bool GeneralMatrix::is_zero() const
00866 {
00867    REPORT
00868    Real* s=store; int i = storage >> 2;
00869    while (i--)
00870    {
00871       if (*s++) return false; if (*s++) return false;
00872       if (*s++) return false; if (*s++) return false;
00873    }
00874    i = storage & 3; while (i--) if (*s++) return false;
00875    return true;
00876 }
00877 
00878 bool is_zero(const BaseMatrix& A)
00879 {
00880    Tracer tr("BaseMatrix::is_zero");
00881    REPORT
00882    GeneralMatrix* gm1 = 0; bool bx;
00883    Try { gm1=((BaseMatrix&)A).Evaluate(); bx = gm1->is_zero(); }
00884    CatchAll { if (gm1) gm1->tDelete(); ReThrow; }
00885    gm1->tDelete();
00886    return bx;
00887 }
00888 
00889 // IsEqual functions - insist matrices are of same type
00890 // as well as equal values to be equal
00891 
00892 bool GeneralMatrix::IsEqual(const GeneralMatrix& A) const
00893 {
00894    Tracer tr("GeneralMatrix IsEqual");
00895    if (A.type() != type())                       // not same types
00896       { REPORT return false; }
00897    if (&A == this)                               // same matrix
00898       { REPORT  return true; }
00899    if (A.nrows_val != nrows_val || A.ncols_val != ncols_val)
00900                                                  // different dimensions
00901    { REPORT return false; }
00902    // is matrix storage the same - compare store
00903    REPORT
00904    return RealEqual(A.store,store,storage);
00905 }
00906 
00907 bool CroutMatrix::IsEqual(const GeneralMatrix& A) const
00908 {
00909    Tracer tr("CroutMatrix IsEqual");
00910    if (A.type() != type())                       // not same types
00911       { REPORT return false; }
00912    if (&A == this)                               // same matrix
00913       { REPORT  return true; }
00914    if (A.nrows_val != nrows_val || A.ncols_val != ncols_val)
00915                                                  // different dimensions
00916    { REPORT return false; }
00917    // is matrix storage the same - compare store
00918    REPORT
00919    return RealEqual(A.store,store,storage)
00920       && intEqual(((CroutMatrix&)A).indx, indx, nrows_val);
00921 }
00922 
00923 
00924 bool BandLUMatrix::IsEqual(const GeneralMatrix& A) const
00925 {
00926    Tracer tr("BandLUMatrix IsEqual");
00927    if (A.type() != type())                       // not same types
00928       { REPORT  return false; }
00929    if (&A == this)                               // same matrix
00930       { REPORT  return true; }
00931    if ( A.Nrows() != nrows_val || A.Ncols() != ncols_val
00932       || ((BandLUMatrix&)A).m1 != m1 || ((BandLUMatrix&)A).m2 != m2 )
00933                                                  // different dimensions
00934    { REPORT  return false; }
00935 
00936    // matrix storage the same - compare store
00937    REPORT
00938    return RealEqual(A.Store(),store,storage)
00939       && RealEqual(((BandLUMatrix&)A).store2,store2,storage2)
00940       && intEqual(((BandLUMatrix&)A).indx, indx, nrows_val);
00941 }
00942 
00943 
00944 // ************************* cross products ******************** //
00945 
00946 inline void crossproduct_body(Real* a, Real* b, Real* c)
00947 {
00948    c[0] = a[1] * b[2] - a[2] * b[1];
00949    c[1] = a[2] * b[0] - a[0] * b[2];
00950    c[2] = a[0] * b[1] - a[1] * b[0];
00951 }
00952 
00953 Matrix crossproduct(const Matrix& A, const Matrix& B)
00954 {
00955    REPORT
00956    int ac = A.Ncols(); int ar = A.Nrows();
00957    int bc = B.Ncols(); int br = B.Nrows();
00958    Real* a = A.Store(); Real* b = B.Store();
00959    if (ac == 3)
00960    {
00961       if (bc != 3 || ar != 1 || br != 1)
00962          { Tracer et("crossproduct"); IncompatibleDimensionsException(A, B); }
00963       REPORT
00964       RowVector C(3);  Real* c = C.Store(); crossproduct_body(a, b, c);
00965       return (Matrix&)C;
00966    }
00967    else
00968    {
00969       if (ac != 1 || bc != 1 || ar != 3 || br != 3)
00970          { Tracer et("crossproduct"); IncompatibleDimensionsException(A, B); }
00971       REPORT
00972       ColumnVector C(3);  Real* c = C.Store(); crossproduct_body(a, b, c);
00973       return (Matrix&)C;
00974    }
00975 }
00976 
00977 ReturnMatrix crossproduct_rows(const Matrix& A, const Matrix& B)
00978 {
00979    REPORT
00980    int n = A.Nrows();
00981    if (A.Ncols() != 3 || B.Ncols() != 3 || n != B.Nrows())
00982    {
00983       Tracer et("crossproduct_rows"); IncompatibleDimensionsException(A, B);
00984    }
00985    Matrix C(n, 3);
00986    Real* a = A.Store(); Real* b = B.Store(); Real* c = C.Store();
00987    if (n--)
00988    {
00989       for (;;)
00990       {
00991          crossproduct_body(a, b, c);
00992          if (!(n--)) break;
00993          a += 3; b += 3; c += 3;
00994       }
00995    }
00996 
00997    return C.ForReturn();
00998 }
00999 
01000 ReturnMatrix crossproduct_columns(const Matrix& A, const Matrix& B)
01001 {
01002    REPORT
01003    int n = A.Ncols();
01004    if (A.Nrows() != 3 || B.Nrows() != 3 || n != B.Ncols())
01005    {
01006       Tracer et("crossproduct_columns");
01007       IncompatibleDimensionsException(A, B);
01008    }
01009    Matrix C(3, n);
01010    Real* a = A.Store(); Real* b = B.Store(); Real* c = C.Store();
01011    Real* an = a+n; Real* an2 = an+n;
01012    Real* bn = b+n; Real* bn2 = bn+n;
01013    Real* cn = c+n; Real* cn2 = cn+n;
01014 
01015    int i = n; 
01016    while (i--)
01017    {
01018       *c++   = *an    * *bn2   - *an2   * *bn;
01019       *cn++  = *an2++ * *b     - *a     * *bn2++;
01020       *cn2++ = *a++   * *bn++  - *an++  * *b++;
01021    }
01022 
01023    return C.ForReturn();
01024 }
01025 
01026 
01027 #ifdef use_namespace
01028 }
01029 #endif
01030 
01032 


kni
Author(s): Neuronics AG (see AUTHORS.txt); ROS wrapper by Martin Günther
autogenerated on Mon Oct 6 2014 10:45:33