#include "PeriodicOrbit.hh" #include "Vector.hh" #include "Matrix.hh" #include "Clock.hh" #include /*************************************************************************** ************************* Utility Routines ************************* ***************************************************************************/ /* * computes and returns alpha so that Frobenius norm of * (alpha*A - B) is minimized. */ double ComputeProjectionFactor(const Matrix& A, const Matrix& B){ assert(A.getm()==B.getm()); assert(A.getn()==B.getn()); double temp1=0.0, temp2=0.0; size_t i,j; for(j=0; j < A.getn(); j++) for(i=0; i < A.getm(); i++){ double a, b; a = A(i,j); b = B(i,j); temp1 += a*a; temp2 += a*b; }//temp1=tr(A^T*A) and temp2=tr(A^T*B) return temp2/temp1; } void ComputeResidual(const Matrix& X, double w, const Matrix& dX, Matrix& Res, vfTYPE vfield){ size_t d = X.getm(); size_t n = X.getn(); assert((d==Res.getm())&&(n==Res.getn())); assert((d==dX.getm())&&(n==dX.getn())); assert(w>0); size_t i; for(i=0; i < n; i++)//Res will first hold vfield(X) vfield(X.getColumn(i), Res.getColumn(i)); Matrix shadowm(dX); shadowm.scale(w); Res.sub(dX);//Res = vfield(X) - w*X'(t) shadowm.scale(1/w); } void ComputeResidual(const Matrix& X, double w, Matrix& Res, vfTYPE vfield){ Matrix dX(X.getm(), X.getn()); diff(X, dX); ComputeResidual(X, w, dX, Res, vfield); } void ComputeLinearization(const Matrix& X, Matrix& A, jTYPE jacobian){ size_t d = X.getm(); size_t n = X.getn(); assert((d*d==A.getm())&&(n==A.getn())); size_t i; Matrix mm; for(i=0; i < n; i++){ mm.shadow(A.getColumn(i), d, d); jacobian(X.getColumn(i), mm); } } void ComputeGradDirn(double w, const Matrix& dX, const Matrix& Res, const Matrix& A, Matrix& gradX, double& gradw){ size_t d = dX.getm(); size_t n = dX.getn(); assert((d==Res.getm())&&(n==Res.getn())); assert((d*d==A.getm())&&(n==A.getn())); assert((d==gradX.getm())&&(n==gradX.getn())); Matrix a(d,d); Matrix m; Matrix dRes(d,n); diff(Res, dRes); dRes.scale(w);//dRes = w dr(t) size_t i; for(i=0; i < n; i++){ m.shadow(A.getColumn(i),d,d); a = m; a.transpose(); Vector vtemp1; vtemp1.shadow(Res.getColumn(i)); Vector vtemp2; vtemp2.shadow(gradX.getColumn(i)); mult(a, vtemp1, vtemp2); } gradX.add(dRes);//gradX = A^T(t)r(t) + w dr(t) gradw = 0; Vector v(d); for(i=0; i < n; i++){ v = Res.getColumn(i); v.mul(dX.getColumn(i)); size_t j; for(j=0; j < d; j++) gradw += v(j); }//gradw = sigma((r(t), dx(t))); gradw = -gradw*2*PI/n; } void ComputeGradDirn(const Matrix& X, double w, Matrix& gradX, double& gradw, vfTYPE vfield, jTYPE jacobian){ Matrix dX(X.getm(), X.getn()); Matrix A(X.getm()*X.getm(), X.getn()); Matrix Res(X.getm(), X.getn()); diff(X,dX); ComputeLinearization(X, A, jacobian); ComputeResidual(X, w, Res, vfield); ComputeGradDirn(w, dX, Res, A, gradX, gradw); } static double timeF=0.0; static double timeA=0.0; static Clock clk; //4th order Gauss-Legendre-Lie method void FindFundSolnA(const Matrix& A, Matrix& Y){ clk.wtic(); int m = A.getm(); int n = A.getn(); assert((m==Y.getm())&&(n==Y.getn())); int d = (int) sqrt((double)m); assert(d*d==m); double c1 = 1.0/2.0 - sqrt(3.0)/6.0; double c2 = 1.0/2.0 + sqrt(3.0)/6.0; Matrix A1(m,n), A2(m, n); double h = 2*PI/n; shift(A, c1*h, A1); shift(A, c2*h, A2); A1.scale(h); A2.scale(h); //Matrix V(2,2);//no need to form V //V(0,0) = 1.0/2.0; //V(0,1) = 1.0/2.0; //V(1,0) = -sqrt(3.0); //V(1,1) = sqrt(3.0); Matrix B1(m,n), B2(m,n); //B_i = V(i,0)A_1 + V(i,1)A_2 B1 = A1; B1.add(A2); B1.scale(1.0/2.0); B2 = A2; B2.sub(A1); B2.scale(sqrt(3.0)); int i; for(i=0; i < n; i++){ Matrix b1, b2; b1.shadow(B1.getColumn(i), d, d); b2.shadow(B2.getColumn(i), d, d); Matrix temp1(d,d), temp2(d,d); mult(b1, b2, temp1); mult(b2, b1, temp2); temp1.sub(temp2); //temp1 = [b1, b2] temp1.scale(-1.0/12.0); temp1.add(b1);//temp1 = b1 - [b1, b2]/12 Matrix yy; yy.shadow(Y.getColumn(i), d, d); expm(temp1, yy); } timeF += clk.wtoc(); } //6th order Gauss-Legendre-Lie method void FindFundSolnB(const Matrix& A, Matrix& Y){ clk.wtic(); size_t m = A.getm(); size_t n = A.getn(); assert((m == Y.getm()) && (n==Y.getn())); size_t d = (size_t)sqrt((double)m); assert(m==d*d); double h = 2*PI/n; static double c1 = 1.0/2.0 - sqrt(15.0)/10; static double c2 = 1.0/2.0; static double c3 = 1.0/2.0 + sqrt(15.0)/10; Matrix A1(m,n), A2(m,n), A3(m,n); size_t i; shift(A, c1*h, A1); shift(A, c2*h, A2); shift(A, c3*h, A3); A1.scale(h); A2.scale(h); A3.scale(h); Matrix V(3,3); V(0,0) = 0; V(0,1) = 1; V(0,2) = 0; V(1,0) = -1.290994448735806e+00; V(1,1) = 0; V(1,2) = 1.290994448735806e+00; V(2,0) = 3.333333333333333e+00; V(2,1) = -6.666666666666666e+00; V(2,2) = 3.333333333333333e+00; Matrix B1(m,n), B2(m,n), B3(m,n); B1 = A2; A1.scale(V(1,0)); A3.scale(V(1,2)); B2 = A1; B2.add(A3); A1.scale(V(2,0)/V(1,0)); A2.scale(V(2,1)); A3.scale(V(2,2)/V(1,2)); B3 = A1; B3.add(A2); B3.add(A3); Matrix C1(A1), C2(A2), C3(A3); C1 = B3; C1.scale(1.0/12.0); C1.add(B1); Vector v; Matrix b1, b2, b3; Matrix cc2, cc3; Matrix temp1(d,d), temp2(d,d), temp3(d,d); for(i=0; i < n; i++){ v.shadow(B1.getColumn(i)); b1.shadow(v, d, d); v.shadow(B2.getColumn(i)); b2.shadow(v, d, d); v.shadow(B3.getColumn(i)); b3.shadow(v, d, d); v.shadow(C2.getColumn(i)); cc2.shadow(v, d, d); v.shadow(C3.getColumn(i)); cc3.shadow(v, d, d); temp1 = b3; temp1.scale(1.0/20.0); temp1.add(b1); mult(temp1, b2, temp2); cc2 = temp2; mult(b2, temp1, temp2); cc2.sub(temp2); cc2.scale(-1.0/12.0); temp1 = b3; temp1.scale(1.0/360.0); temp2 = cc2; temp2.scale(1.0/60.0); temp1.sub(temp2); mult(b1, temp1, temp2); mult(temp1, b1, temp3); temp1 = temp2; temp1.sub(temp3); mult(b1, temp1, temp2); mult(temp1, b1, temp3); temp1 = temp2; temp1.sub(temp3); mult(b2, cc2, temp2); mult(cc2, b2, temp3); cc3 = temp2; cc3.sub(temp3); cc3.scale(-1.0/20.0); cc3.add(temp1); } Matrix M(C1); //M = C1 + C2 + C3 M.add(C2); M.add(C3); Matrix mm, yy; for(i=0; i < n; i++){ v.shadow(M.getColumn(i)); mm.shadow(v, d, d); v.shadow(Y.getColumn(i)); yy.shadow(v, d, d); expm(mm, yy); } timeF += clk.wtoc(); } //4th order, uses simpson's rule void ApplyFundSolnA(const Matrix& A, const Matrix& g, Matrix& gg){ clk.wtic(); int m = A.getm(); int n = A.getn(); int d = (int)sqrt((double)m); assert(m==d*d); assert((d==g.getm())&&(n==g.getn())); assert((d==gg.getm())&&(n==gg.getn())); double h = 2*PI/n; Matrix YY(d*d,2*n); Vector vA(d*d*2*n); Matrix AA; AA.shadow(vA,d*d, 2*n); Matrix AAA; AAA.shadow(vA, 2*d*d, n); AAA.getShadow(0,d*d-1, 0, n-1) = A; Matrix mtemp; mtemp.shadow(AAA.getShadow(d*d, 2*d*d-1, 0, n-1)); shift(A, h/2, mtemp); FindFundSolnA(AA, YY); Matrix g1(d, n); shift(g, h/2, g1); int i; for(i=0; i < n; i++){ Vector v(d); gg.getColumn(i) = g.getColumn((i+1)%n); v = g.getColumn(i); Matrix y; y.shadow(YY.getColumn(2*i),d,d); mult(y,v); y.shadow(YY.getColumn(2*i+1),d,d); mult(y,v); (gg.getColumn(i)).add(v); v = g1.getColumn(i); mult(y,v); v.scale(4); (gg.getColumn(i)).add(v); (gg.getColumn(i)).scale(h/6.0); } timeA+=clk.wtoc(); } //4th order Gauss-Legendre method void ApplyFundSolnB(const Matrix& A, const Matrix& g, Matrix& gg){ clk.wtic(); int m = A.getm(); int n = A.getn(); int d = (int)sqrt((double)m); assert(m==d*d); assert((d==g.getm())&&(n==g.getn())); assert((d==gg.getm())&&(n==gg.getn())); double c1 = 0.5 - sqrt(3.0)/6.0; double c2 = 0.5 + sqrt(3.0)/6.0; double b1 = 0.5; double b2 = 0.5; double a11 = 0.25; double a12 = 0.25 - sqrt(3.0)/6.0; double a21 = 0.25 + sqrt(3.0)/6.0; double a22 = 0.25; double h = 2*PI/n; Matrix A1(m,n), A2(m,n); shift(A, c1*h, A1); shift(A, c2*h, A2); Matrix g1(d,n), g2(d,n); shift(g, c1*h, g1); shift(g, c2*h, g2); Matrix rhs22(2,2); rhs22(0,0) = a11; rhs22(0,1) = a12; rhs22(1,0) = a21; rhs22(1,1) = a22; rhs22.transpose(); int i; for(i=0; i < n; i++){ Matrix a1, a2; a1.shadow(A1.getColumn(i), d, d); a2.shadow(A2.getColumn(i), d, d); Vector rhs(2*d); Matrix rhs_m; rhs_m.shadow(rhs, d, 2); Matrix temprhs(d,2); temprhs.getColumn(0) = g1.getColumn(i); temprhs.getColumn(1) = g2.getColumn(i); mult(temprhs, rhs22, rhs_m); rhs.scale(h); //this ends setting up rhs Matrix lhs(2*d, 2*d); lhs.getShadow(0, d-1, 0, d-1) = a1; (lhs.getShadow(0, d-1, 0, d-1)).scale(a11); lhs.getShadow(0, d-1, d, 2*d-1) = a2; (lhs.getShadow(0, d-1, d, 2*d-1)).scale(a12); lhs.getShadow(d, 2*d-1, 0, d-1) = a1; (lhs.getShadow(d, 2*d-1, 0, d-1)).scale(a21); lhs.getShadow(d, 2*d-1, d, 2*d-1) = a2; (lhs.getShadow(d, 2*d-1, d, 2*d-1)).scale(a22); lhs.scale(-1); int j; for(j=0; j < 2*d; j++)//ends set up of lhs lhs(j,j) = lhs(j,j)+1; Vector xi(2*d); solve(lhs, rhs, xi); Vector v1(d), v2(d); v1 = xi.getShadow(0, d-1); v2 = xi.getShadow(d, 2*d-1); mult(a1, v1); mult(a2, v2); v1.add(g1.getColumn(i)); v2.add(g2.getColumn(i)); v1.scale(b1*h); v2.scale(b2*h); gg.getColumn(i) = v1; (gg.getColumn(i)).add(v2); } timeA+=clk.wtoc(); } //6th order Gauss-Legendre method void ApplyFundSolnC(const Matrix& A, const Matrix& g, Matrix& gg){ clk.wtic(); size_t m = A.getm(); size_t n = A.getn(); size_t d = (size_t)sqrt((double)m); assert(m==d*d); assert((d==g.getm())&&(n==g.getn())); assert((d==gg.getm())&&(n==gg.getn())); double c1 = 0.5 - sqrt(15.0)/10.0; double c2 = 0.5; double c3 = 0.5 + sqrt(15.0)/10.0; double b1 = 5.0/18.0; double b2 = 4.0/9.0; double b3 = 5.0/18.0; double a11 = 5.0/36.0; double a12 = 2.0/9.0 - sqrt(15.0)/15.0; double a13 = 5.0/36.0 - sqrt(15.0)/30.0; double a21 = 5.0/36.0 + sqrt(15.0)/24.0; double a22 = 2.0/9.0; double a23 = 5.0/36.0 - sqrt(15.0)/24.0; double a31 = 5.0/36.0 + sqrt(15.0)/30.0; double a32 = 2.0/9.0 + sqrt(15.0)/15.0; double a33 = 5.0/ 36.0; double h = 2*PI/n; Matrix A1(m, n), A2(m, n), A3(m, n); shift(A, c1*h, A1); shift(A, c2*h, A2); shift(A, c3*h, A3); Matrix g1(d, n), g2(d, n), g3(d, n); shift(g, c1*h, g1); shift(g, c2*h, g2); shift(g, c3*h, g3); Matrix rhs33(3,3); rhs33(0,0)=a11; rhs33(0,1)=a12; rhs33(0,2)=a13; rhs33(1,0)=a21; rhs33(1,1)=a22; rhs33(1,2)=a23; rhs33(2,0)=a31; rhs33(2,1)=a32; rhs33(2,2)=a33; rhs33.transpose(); int i; for(i=0; i < n; i++){ Matrix a1, a2, a3; a1.shadow(A1.getColumn(i), d, d); a2.shadow(A2.getColumn(i), d, d); a3.shadow(A3.getColumn(i), d, d); Vector rhs(3*d); Matrix rhs_m; rhs_m.shadow(rhs, d, 3); Matrix temprhs(d,3); temprhs.getColumn(0) = g1.getColumn(i); temprhs.getColumn(1) = g2.getColumn(i); temprhs.getColumn(2) = g3.getColumn(i); mult(temprhs, rhs33, rhs_m); rhs.scale(h); //this ends setting up rhs Matrix lhs(3*d, 3*d); lhs.getShadow(0,d-1, 0, d-1) = a1; (lhs.getShadow(0,d-1, 0, d-1)).scale(a11); lhs.getShadow(0,d-1, d, 2*d-1) = a2; (lhs.getShadow(0,d-1, d, 2*d-1)).scale(a12); lhs.getShadow(0,d-1, 2*d, 3*d-1) = a3; (lhs.getShadow(0,d-1, 2*d, 3*d-1)).scale(a13); lhs.getShadow(d,2*d-1, 0, d-1) = a1; (lhs.getShadow(d,2*d-1, 0, d-1)).scale(a21); lhs.getShadow(d,2*d-1, d, 2*d-1) = a2; (lhs.getShadow(d,2*d-1, d, 2*d-1)).scale(a22); lhs.getShadow(d,2*d-1, 2*d, 3*d-1) = a3; (lhs.getShadow(d,2*d-1, 2*d, 3*d-1)).scale(a23); lhs.getShadow(2*d,3*d-1, 0, d-1) = a1; (lhs.getShadow(2*d,3*d-1, 0, d-1)).scale(a31); lhs.getShadow(2*d,3*d-1, d, 2*d-1) = a2; (lhs.getShadow(2*d,3*d-1, d, 2*d-1)).scale(a32); lhs.getShadow(2*d,3*d-1, 2*d, 3*d-1) = a3; (lhs.getShadow(2*d,3*d-1, 2*d, 3*d-1)).scale(a33); lhs.scale(-h); int j; for(j=0; j < 3*d; j++)//ends set up of lhs lhs(j,j) = lhs(j,j)+1; Vector xi(3*d); solve(lhs, rhs, xi); Vector v1(d), v2(d), v3(d); v1 = xi.getShadow(0, d-1); v2 = xi.getShadow(d, 2*d-1); v3 = xi.getShadow(2*d, 3*d-1); mult(a1, v1); mult(a2, v2); mult(a3, v3); v1.add(g1.getColumn(i)); v2.add(g2.getColumn(i)); v3.add(g3.getColumn(i)); v1.scale(b1*h); v2.scale(b2*h); v3.scale(b3*h); gg.getColumn(i) = v1; (gg.getColumn(i)).add(v2); (gg.getColumn(i)).add(v3); } timeA+=clk.wtoc(); } //8th order Gauss-Legendre method void ApplyFundSolnD(const Matrix& A, const Matrix& g, Matrix& gg){ clk.wtic(); size_t m = A.getm(); size_t n = A.getn(); size_t d = (size_t)sqrt((double)m); assert(m==d*d); assert((d==g.getm())&&(n==g.getn())); assert((d==gg.getm())&&(n==gg.getn())); static double c1 = 6.943184420297371e-02; static double c2 = 3.300094782075719e-01; static double c3 = 6.699905217924281e-01; static double c4 = 9.305681557970262e-01; static double b1 = 1.739274225687269e-01; static double b2 = 3.260725774312730e-01; static double b3 = 3.260725774312730e-01; static double b4 = 1.739274225687269e-01; static double a11 = 8.696371128436346e-02; static double a22 = 1.630362887156365e-01; static double a33 = 1.630362887156365e-01; static double a44 = 8.696371128436346e-02; static double a12 = -2.660418008499880e-02; static double a21 = 1.881181174998681e-01; static double a13 = 1.262746268940472e-02; static double a31 = 1.671919219741888e-01; static double a14 = -3.555149685795692e-03; static double a41 = 1.774825722545226e-01; static double a23 = -2.788042860247092e-02; static double a32 = 3.539530060337440e-01; static double a24 = 6.735500594538160e-03; static double a42 = 3.134451147418683e-01; static double a34 = -1.419069493114115e-02; static double a43 = 3.526767575162718e-01; double h = 2*PI/n; Matrix A1(m,n), A2(m,n), A3(m,n), A4(m,n); size_t i; shift(A, c1*h, A1); shift(A, c2*h, A2); shift(A, c3*h, A3); shift(A, c4*h, A4); Matrix g1(d,n), g2(d,n), g3(d,n), g4(d,n); shift(g, c1*h, g1); shift(g, c2*h, g2); shift(g, c3*h, g3); shift(g, c4*h, g4); Vector vvv; Matrix mm; Matrix a1, a2, a3, a4; Vector rhs(4*d); Matrix rhs_m, temprhs(d,4); rhs_m.shadow(rhs, d, 4); Matrix rhs44(4,4); Matrix lhs(4*d, 4*d); Vector xi(4*d); Vector v1(d), v2(d), v3(d), v4(d); for(i=0; i < n; i++){ vvv.shadow(A1.getColumn(i)); a1.shadow(vvv, d, d); vvv.shadow(A2.getColumn(i)); a2.shadow(vvv, d, d); vvv.shadow(A3.getColumn(i)); a3.shadow(vvv, d, d); vvv.shadow(A4.getColumn(i)); a4.shadow(vvv, d, d); temprhs.getColumn(0) = g1.getColumn(i); temprhs.getColumn(1) = g2.getColumn(i); temprhs.getColumn(2) = g3.getColumn(i); temprhs.getColumn(3) = g4.getColumn(i); rhs44(0,0)=a11; rhs44(0,1)=a12; rhs44(0,2)=a13; rhs44(0,3)=a14; rhs44(1,0)=a21; rhs44(1,1)=a22; rhs44(1,2)=a23; rhs44(1,3)=a24; rhs44(2,0)=a31; rhs44(2,1)=a32; rhs44(2,2)=a33; rhs44(2,3)=a34; rhs44(3,0)=a41; rhs44(3,1)=a42; rhs44(3,2)=a43; rhs44(3,3)=a44; rhs44.transpose(); mult(temprhs, rhs44, rhs_m); rhs.scale(h);//this sets up rhs lhs.getShadow(0,d-1, 0, d-1) = a1; (lhs.getShadow(0,d-1, 0, d-1)).scale(a11); lhs.getShadow(0,d-1, d, 2*d-1) = a2; (lhs.getShadow(0,d-1, d, 2*d-1)).scale(a12); lhs.getShadow(0,d-1, 2*d, 3*d-1) = a3; (lhs.getShadow(0,d-1, 2*d, 3*d-1)).scale(a13); lhs.getShadow(0,d-1, 3*d, 4*d-1) = a4; (lhs.getShadow(0,d-1, 3*d, 4*d-1)).scale(a14); lhs.getShadow(d,2*d-1, 0, d-1) = a1; (lhs.getShadow(d,2*d-1, 0, d-1)).scale(a21); lhs.getShadow(d,2*d-1, d, 2*d-1) = a2; (lhs.getShadow(d,2*d-1, d, 2*d-1)).scale(a22); lhs.getShadow(d,2*d-1, 2*d, 3*d-1) = a3; (lhs.getShadow(d,2*d-1, 2*d, 3*d-1)).scale(a23); lhs.getShadow(d,2*d-1, 3*d, 4*d-1) = a4; (lhs.getShadow(d,2*d-1, 3*d, 4*d-1)).scale(a24); lhs.getShadow(2*d,3*d-1, 0, d-1) = a1; (lhs.getShadow(2*d,3*d-1, 0, d-1)).scale(a31); lhs.getShadow(2*d,3*d-1, d, 2*d-1) = a2; (lhs.getShadow(2*d,3*d-1, d, 2*d-1)).scale(a32); lhs.getShadow(2*d,3*d-1, 2*d, 3*d-1) = a3; (lhs.getShadow(2*d,3*d-1, 2*d, 3*d-1)).scale(a33); lhs.getShadow(2*d,3*d-1, 3*d, 4*d-1) = a4; (lhs.getShadow(2*d,3*d-1, 3*d, 4*d-1)).scale(a34); lhs.getShadow(3*d,4*d-1, 0, d-1) = a1; (lhs.getShadow(3*d,4*d-1, 0, d-1)).scale(a41); lhs.getShadow(3*d,4*d-1, d, 2*d-1) = a2; (lhs.getShadow(3*d,4*d-1, d, 2*d-1)).scale(a42); lhs.getShadow(3*d,4*d-1, 2*d, 3*d-1) = a3; (lhs.getShadow(3*d,4*d-1, 2*d, 3*d-1)).scale(a43); lhs.getShadow(3*d,4*d-1, 3*d, 4*d-1) = a4; (lhs.getShadow(3*d,4*d-1, 3*d, 4*d-1)).scale(a44); lhs.scale(-h); size_t j; for(j=0; j < 4*d; j++) lhs(j,j) = lhs(j,j)+1; solve(lhs, rhs, xi); mult(a1, xi.getShadow(0, d-1), v1); mult(a2, xi.getShadow(d, 2*d-1), v2); mult(a3, xi.getShadow(2*d, 3*d-1), v3); mult(a4, xi.getShadow(3*d, 4*d-1), v4); v1.add(g1.getColumn(i)); v2.add(g2.getColumn(i)); v3.add(g3.getColumn(i)); v4.add(g4.getColumn(i)); v1.scale(b1*h); v2.scale(b2*h); v3.scale(b3*h); v4.scale(b4*h); gg.getColumn(i)= v1; (gg.getColumn(i)).add(v2); (gg.getColumn(i)).add(v3); (gg.getColumn(i)).add(v4); } timeA+=clk.wtoc(); } void printstuff(){ cout<<"time in FindFundSoln: "<