} template<typename Real> Complex<Real> WilkinsonShift( const Matrix<Complex<Real>>& H ) { DEBUG_CSE typedef Complex<Real> F; const Int n = H.Height(); DEBUG_ONLY( if( n < 2 ) LogicError("WilkinsonShift requires n >= 2"); ) const Int offset = n-2; const Real zero = Real(0); const F& eta00 = H(offset,offset); const F& eta01 = H(offset,offset+1); const F& eta10 = H(offset+1,offset); const F& eta11 = H(offset+1,offset+1); // NOTE: // eta10 should be real, but it is possibly negative, and so we will // interpret it as a complex number so that the square-root is well-defined DEBUG_ONLY( if( ImagPart(eta10) != zero ) LogicError("Subdiagonal assumed real"); ) F shift = eta11; const F gamma = Sqrt(eta01)*Sqrt(eta10); Real sigma = OneAbs(gamma); if( sigma != zero )

ConstIndexArray EndCapBSplineBasisPatchFactory::GetPatchPoints( Vtr::internal::Level const * level, Index faceIndex, PatchTableFactory::PatchFaceTag const * /*levelPatchTags*/, int levelVertOffset) { // XXX: For now, always create new 16 indices for each patch. // we'll optimize later to share all regular control points with // other patches as well as to try to make extra ordinary verts watertight. int offset = _refiner->GetNumVerticesTotal(); for (int i = 0; i < 16; ++i) { _patchPoints.push_back(_numVertices + offset); ++_numVertices; } // XXX: temporary hack. we should traverse topology and find existing // vertices if available // // Reorder gregory basis stencils into regular bezier GregoryBasis::ProtoBasis basis(*level, faceIndex, levelVertOffset, -1); std::vector<GregoryBasis::Point> bezierCP; bezierCP.reserve(16); bezierCP.push_back(basis.P[0]); bezierCP.push_back(basis.Ep[0]); bezierCP.push_back(basis.Em[1]); bezierCP.push_back(basis.P[1]); bezierCP.push_back(basis.Em[0]); bezierCP.push_back(basis.Fp[0]); // arbitrary bezierCP.push_back(basis.Fp[1]); // arbitrary bezierCP.push_back(basis.Ep[1]); bezierCP.push_back(basis.Ep[3]); bezierCP.push_back(basis.Fp[3]); // arbitrary bezierCP.push_back(basis.Fp[2]); // arbitrary bezierCP.push_back(basis.Em[2]); bezierCP.push_back(basis.P[3]); bezierCP.push_back(basis.Em[3]); bezierCP.push_back(basis.Ep[2]); bezierCP.push_back(basis.P[2]); // Apply basis conversion from bezier to b-spline float Q[4][4] = {{ 6, -7, 2, 0}, { 0, 2, -1, 0}, { 0, -1, 2, 0}, { 0, 2, -7, 6} }; std::vector<GregoryBasis::Point> H(16); for (int i = 0; i < 4; ++i) { for (int j = 0; j < 4; ++j) { for (int k = 0; k < 4; ++k) { if (Q[i][k] != 0) H[i*4+j] += bezierCP[j+k*4] * Q[i][k]; } } } for (int i = 0; i < 4; ++i) { for (int j = 0; j < 4; ++j) { GregoryBasis::Point p; for (int k = 0; k < 4; ++k) { if (Q[j][k] != 0) p += H[i*4+k] * Q[j][k]; } _vertexStencils.push_back(p); } } int varyingIndices[] = { 0, 0, 1, 1, 0, 0, 1, 1, 3, 3, 2, 2, 3, 3, 2, 2,}; for (int i = 0; i < 16; ++i) { _varyingStencils.push_back(basis.V[varyingIndices[i]]); } ++_numPatches; return ConstIndexArray(&_patchPoints[(_numPatches-1)*16], 16); }

int InterPolynomial::checkIfValidityIsInBound(Vector ddv, unsigned k, double bound, double rho) // input: k,bound,rho // output: j,ddv { // check validity around x_k // bound is epsilon in the paper // return index of the worst point of J // if (j==-1) then everything OK : next : trust region step // else model step: replace x_j by x_k+d where d // is calculated with LAGMAX unsigned i,N=nPtsUsed, n=dim(); int j; Polynomial *pp=NewtonBasis; Vector *xx=NewtonPoints, xk=xx[k],vd; Vector Distance(N); double *dist=Distance, *dd=dist, distMax, vmax, tmp; Matrix H(n,n); Vector GXk(n); //,D(n); for (i=0; i<N; i++) *(dd++)=xk.euclidianDistance(xx[i]); while (true) { dd=dist; j=-1; distMax=2*rho; for (i=0; i<N; i++) { if (*dd>distMax) { j=i; distMax=*dd; }; dd++; } if (j<0) return -1; // to prevent to choose the same point once again: dist[j]=0; pp[j].gradientHessian(xk,GXk,H); // d=H.multiply(xk); // d.add(G); tmp=M*distMax*distMax*distMax; if (tmp*rho*(GXk.euclidianNorm()+0.5*rho*H.frobeniusNorm())>=bound) { /* vd=L2NormMinimizer(pp[j], xk, rho); vd+=xk; vmax=condorAbs(pp[j](vd)); Vector vd2=L2NormMinimizer(pp[j], xk, rho); vd2+=xk; double vmax2=condorAbs(pp[j](vd)); if (vmax<vmax2) { vmax=vmax2; vd=vd2; } */ vd=LAGMAXModified(GXk,H,rho,vmax); // tmp=vd.euclidianNorm(); vd+=xk; vmax=condorAbs(pp[j](vd)); if (tmp*vmax>=bound) break; } } if (j>=0) ddv.copyFrom(vd); return j; }

int alg1(int *dK, int *dX, int *dY, Cipher *ciph, int m) { int dH[32]; int dJ[32]; int dHx, dHmax; int imax; for (int i = 0; i < 32; i++) dJ[i] = 0; dHx = H(dK, dX, dY, ciph, m); //cout << "H* = " << dHx << endl; STEP2: for(int i = 0; i < 32; i++) { if(dK[i] == 2) { int dH0, dH1; dK[i] = 0; dH0 = H(dK, dX, dY, ciph, m); dK[i] = 1; dH1 = H(dK, dX, dY, ciph, m); if((dH0 < dHx)&&(dHx < dH1)) { dH[i] = dH1; dJ[i] = 1; } else if((dH1 < dHx)&&(dHx < dH0)) { dH[i] = dH0; dJ[i] = 0; } else { dH[i] = dHx; } dK[i] = 2; } else if(dK[i] == 0) { int dH05, dH1; dK[i] = 2; dH05 = H(dK, dX, dY, ciph, m); dK[i] = 1; dH1 = H(dK, dX, dY, ciph, m); if((dH05 < dHx)&&(dHx < dH1)) { dH[i] = dH1; dJ[i] = 1; } else if((dH1 < dHx)&&(dHx < dH05)) { dH[i] = dH05; dJ[i] = 2; } else { dH[i] = dHx; } dK[i] = 0; } else if(dK[i] == 1) { int dH0, dH05; dK[i] = 0; dH0 = H(dK, dX, dY, ciph, m); dK[i] = 2; dH05 = H(dK, dX, dY, ciph, m); if((dH0 < dHx)&&(dHx < dH05)) { dH[i] = dH05; dJ[i] = 2; } else if((dH05 < dHx)&&(dHx < dH0)) { dH[i] = dH0; dJ[i] = 0; } else { dH[i] = dHx; } dK[i] = 1; } } dHmax = dH[0]; imax = 0; for(int i = 0; i < 32; i++) { if(dH[i] > dHmax) { dHmax = dH[i]; imax = i; } } if(dHmax > dHx) { dK[imax] = dJ[imax]; dHx = dHmax; goto STEP2; } return dHmax; }

int main (int argc, char * argv[]) { // SET UP ARGUMENTS po::options_description desc("Options"); desc.add_options() ("input", po::value<std::string>()->required(), "Input hypergraph file") ("output", po::value<std::string>()->default_value("out.dat"), "Output transversals file") ("verbosity,v", po::value<int>()->default_value(0)->implicit_value(1), "Write verbose debugging output (-v2 for trace output)") ("algorithm,a", po::value<std::string>()->default_value("pmmcs"), "Algorithm to use (pmmcs, prs, fka, berge, bm)") ("num-threads,t", po::value<int>()->default_value(1), "Number of threads to run in parallel") ("cutoff-size,c", po::value<int>()->default_value(0), "Maximum size set to return (0: no limit)"); po::positional_options_description p; p.add("input", 1); p.add("output", 1); po::variables_map vm; po::store(po::command_line_parser(argc, argv).options(desc).positional(p).run(), vm); if (vm.count("help") or argc == 1) { std::cout << desc << std::endl; return 1; }; const size_t num_threads = (vm["num-threads"].as<int>()); const size_t cutoff_size = (vm["cutoff-size"].as<int>()); po::notify(vm); // Process input file BOOST_LOG_TRIVIAL(debug) << "Loading hypergraph from file."; std::string input_file(vm["input"].as<std::string>()); agdmhs::Hypergraph H (input_file); BOOST_LOG_TRIVIAL(debug) << "Loading complete."; // Process logging-related options int verbosity = vm["verbosity"].as<int>(); switch (verbosity) { case 1: boost::log::core::get()->set_filter (boost::log::trivial::severity >= boost::log::trivial::debug); break; case 2: boost::log::core::get()->set_filter (boost::log::trivial::severity >= boost::log::trivial::trace); break; default: boost::log::core::get()->set_filter (boost::log::trivial::severity >= boost::log::trivial::warning); break; } // Print input information std::cout << "Input has " << H.num_verts() << " vertices and " << H.num_edges() << " edges." << std::endl; // Run chosen algorithm std::string algname = vm["algorithm"].as<std::string>(); std::unique_ptr<agdmhs::MHSAlgorithm> mhs_algorithm; if (algname == "berge") { mhs_algorithm = std::unique_ptr<agdmhs::MHSAlgorithm> (new agdmhs::BergeAlgorithm(cutoff_size)); } else if (algname == "bm") { mhs_algorithm = std::unique_ptr<agdmhs::MHSAlgorithm> (new agdmhs::ParBMAlgorithm (num_threads)); } else if (algname == "fka") { mhs_algorithm = std::unique_ptr<agdmhs::MHSAlgorithm> (new agdmhs::FKAlgorithmA()); } else if (algname == "mmcs" or algname == "pmmcs") { mhs_algorithm = std::unique_ptr<agdmhs::MHSAlgorithm> (new agdmhs::MMCSAlgorithm(num_threads, cutoff_size)); } else if (algname == "rs" or algname == "prs") { mhs_algorithm = std::unique_ptr<agdmhs::MHSAlgorithm> (new agdmhs::RSAlgorithm(num_threads, cutoff_size)); } else { std::stringstream error_message; error_message << "Did not recognize requested algorithm " << algname << "."; throw po::invalid_option_value(error_message.str()); } BOOST_LOG_TRIVIAL(debug) << "Running algorithm " << algname; agdmhs::Hypergraph Htrans = mhs_algorithm->transversal(H); std::cout << "Found " << Htrans.num_edges() << " hitting sets." << std::endl; BOOST_LOG_TRIVIAL(debug) << "Algorithm complete."; // Print results BOOST_LOG_TRIVIAL(debug) << "Writing result file."; std::string output_file(vm["output"].as<std::string>()); Htrans.write_to_file(output_file); BOOST_LOG_TRIVIAL(debug) << "Writing complete."; return 0; }

static inline int NCR5380_pwrite(struct Scsi_Host *instance, unsigned char *addr, int len) { int *ctrl = &((struct NCR5380_hostdata *)instance->hostdata)->ctrl; int oldctrl = *ctrl; unsigned long *laddr; #ifdef NOT_EFFICIENT int iobase = instance->io_port; int dma_io = iobase & ~(0x3C0000>>2); #else volatile unsigned char *iobase = (unsigned char *)ioaddr(instance->io_port); volatile unsigned char *dma_io = (unsigned char *)((int)iobase & ~0x3C0000); #endif if(!len) return 0; CTRL(iobase, 0x02); laddr = (unsigned long *)addr; while(len >= 32) { int status; unsigned long v; status = STAT(iobase); if(status & 0x80) goto end; if(!(status & 0x40)) continue; v=*laddr++; OUT2(L(v),dma_io); OUT2(H(v),dma_io); v=*laddr++; OUT2(L(v),dma_io); OUT2(H(v),dma_io); v=*laddr++; OUT2(L(v),dma_io); OUT2(H(v),dma_io); v=*laddr++; OUT2(L(v),dma_io); OUT2(H(v),dma_io); v=*laddr++; OUT2(L(v),dma_io); OUT2(H(v),dma_io); v=*laddr++; OUT2(L(v),dma_io); OUT2(H(v),dma_io); v=*laddr++; OUT2(L(v),dma_io); OUT2(H(v),dma_io); v=*laddr++; OUT2(L(v),dma_io); OUT2(H(v),dma_io); len -= 32; if(len == 0) break; } addr = (unsigned char *)laddr; CTRL(iobase, 0x12); while(len > 0) { int status; status = STAT(iobase); if(status & 0x80) goto end; if(status & 0x40) { OUT(*addr++, dma_io); if(--len == 0) break; } status = STAT(iobase); if(status & 0x80) goto end; if(status & 0x40) { OUT(*addr++, dma_io); if(--len == 0) break; } } end: CTRL(iobase, oldctrl|0x40); return len; }

std::vector<INTPOINT> TERRAIN::GetPath(INTPOINT start, INTPOINT goal) { try { //Check that the two points are within the bounds of the map MAPTILE *startTile = GetTile(start); MAPTILE *goalTile = GetTile(goal); if(!Within(start) || !Within(goal) || start == goal || startTile == NULL || goalTile == NULL) return std::vector<INTPOINT>(); //Check if a path exists if(!startTile->m_walkable || !goalTile->m_walkable || startTile->m_set != goalTile->m_set) return std::vector<INTPOINT>(); //Init Search long numTiles = m_size.x * m_size.y; for(long l=0;l<numTiles;l++) { m_pMapTiles[l].f = m_pMapTiles[l].g = FLT_MAX; //Clear F,G m_pMapTiles[l].open = m_pMapTiles[l].closed = false; //Reset Open and Closed } std::vector<MAPTILE*> open; //Create Our Open list startTile->g = 0; //Init our starting point (SP) startTile->f = H(start, goal); startTile->open = true; open.push_back(startTile); //Add SP to the Open list bool found = false; // Search as long as a path hasnt been found, while(!found && !open.empty()) // or there is no more tiles to search { MAPTILE * best = open[0]; // Find the best tile (i.e. the lowest F value) int bestPlace = 0; for(int i=1;i<(int)open.size();i++) if(open[i]->f < best->f) { best = open[i]; bestPlace = i; } if(best == NULL)break; //No path found open[bestPlace]->open = false; // Take the best node out of the Open list open.erase(open.begin() + bestPlace); if(best->m_mappos == goal) //If the goal has been found { std::vector<INTPOINT> p, p2; MAPTILE *point = best; while(point->m_mappos != start) // Generate path { p.push_back(point->m_mappos); point = point->m_pParent; if(p.size() > 500) //Too long path, something is wrong return std::vector<INTPOINT>(); } for(int i=p.size()-1;i!=0;i--) // Reverse path p2.push_back(p[i]); p2.push_back(goal); return p2; } else { for(int i=0;i<8;i++) // otherwise, check the neighbors of the if(best->m_pNeighbors[i] != NULL) // best tile { bool inList = false; // Generate new G and F value float newG = best->g + 1.0f; float d = H(best->m_mappos, best->m_pNeighbors[i]->m_mappos); float newF = newG + H(best->m_pNeighbors[i]->m_mappos, goal) + best->m_pNeighbors[i]->m_cost * 5.0f * d; if(best->m_pNeighbors[i]->open || best->m_pNeighbors[i]->closed) { if(newF < best->m_pNeighbors[i]->f) // If the new F value is lower { best->m_pNeighbors[i]->g = newG; // update the values of this tile best->m_pNeighbors[i]->f = newF; best->m_pNeighbors[i]->m_pParent = best; } inList = true; } if(!inList) // If the neighbor tile isn't in the Open or Closed list { best->m_pNeighbors[i]->f = newF; //Set the values best->m_pNeighbors[i]->g = newG; best->m_pNeighbors[i]->m_pParent = best; best->m_pNeighbors[i]->open = true; open.push_back(best->m_pNeighbors[i]); //Add it to the open list } } best->closed = true; //The best tile has now been searched, add it to the Closed list } } return std::vector<INTPOINT>(); //No path found, return an empty path } catch(...) { debug.Print("Error in TERRAIN::GetPath()"); return std::vector<INTPOINT>(); } }

unsigned long CSysSolve::FGMRES(const CSysVector & b, CSysVector & x, CMatrixVectorProduct & mat_vec, CPreconditioner & precond, double tol, unsigned long m, bool monitoring) { int rank = 0; #ifndef NO_MPI rank = MPI::COMM_WORLD.Get_rank(); #endif /*--- Check the subspace size ---*/ if (m < 1) { if (rank == 0) cerr << "CSysSolve::FGMRES: illegal value for subspace size, m = " << m << endl; #ifdef NO_MPI exit(1); #else MPI::COMM_WORLD.Abort(1); MPI::Finalize(); #endif } /*--- Check the subspace size ---*/ if (m > 1000) { if (rank == 0) cerr << "CSysSolve::FGMRES: illegal value for subspace size (too high), m = " << m << endl; #ifdef NO_MPI exit(1); #else MPI::COMM_WORLD.Abort(1); MPI::Finalize(); #endif } /*--- Define various arrays Note: elements in w and z are initialized to x to avoid creating a temporary CSysVector object for the copy constructor ---*/ vector<CSysVector> w(m+1, x); vector<CSysVector> z(m+1, x); vector<double> g(m+1, 0.0); vector<double> sn(m+1, 0.0); vector<double> cs(m+1, 0.0); vector<double> y(m, 0.0); vector<vector<double> > H(m+1, vector<double>(m, 0.0)); /*--- Calculate the norm of the rhs vector ---*/ double norm0 = b.norm(); /*--- Calculate the initial residual (actually the negative residual) and compute its norm ---*/ mat_vec(x,w[0]); w[0] -= b; double beta = w[0].norm(); if ( (beta < tol*norm0) || (beta < eps) ) { /*--- System is already solved ---*/ if (rank == 0) cout << "CSysSolve::FGMRES(): system solved by initial guess." << endl; return 0; } /*--- Normalize residual to get w_{0} (the negative sign is because w[0] holds the negative residual, as mentioned above) ---*/ w[0] /= -beta; /*--- Initialize the RHS of the reduced system ---*/ g[0] = beta; /*--- Set the norm to the initial initial residual value ---*/ norm0 = beta; /*--- Output header information including initial residual ---*/ int i = 0; if ((monitoring) && (rank == 0)) { writeHeader("FGMRES", tol, beta); writeHistory(i, beta, norm0); } /*--- Loop over all serach directions ---*/ for (i = 0; i < m; i++) { /*--- Check if solution has converged ---*/ if (beta < tol*norm0) break; /*--- Precondition the CSysVector w[i] and store result in z[i] ---*/ precond(w[i], z[i]); /*--- Add to Krylov subspace ---*/ mat_vec(z[i], w[i+1]); /*--- Modified Gram-Schmidt orthogonalization ---*/ modGramSchmidt(i, H, w); /*--- Apply old Givens rotations to new column of the Hessenberg matrix then generate the new Givens rotation matrix and apply it to the last two elements of H[:][i] and g ---*/ for (int k = 0; k < i; k++) applyGivens(sn[k], cs[k], H[k][i], H[k+1][i]); generateGivens(H[i][i], H[i+1][i], sn[i], cs[i]); applyGivens(sn[i], cs[i], g[i], g[i+1]); /*--- Set L2 norm of residual and check if solution has converged ---*/ beta = fabs(g[i+1]); /*--- Output the relative residual if necessary ---*/ if ((((monitoring) && (rank == 0)) && ((i+1) % 100 == 0)) && (rank == 0)) writeHistory(i+1, beta, norm0); } /*--- Solve the least-squares system and update solution ---*/ solveReduced(i, H, g, y); for (int k = 0; k < i; k++) { x.Plus_AX(y[k], z[k]); } if ((monitoring) && (rank == 0)) { cout << "# FGMRES final (true) residual:" << endl; cout << "# Iteration = " << i << ": |res|/|res0| = " << beta/norm0 << endl; } // /*--- Recalculate final (neg.) residual (this should be optional) ---*/ // mat_vec(x, w[0]); // w[0] -= b; // double res = w[0].norm(); // // if (fabs(res - beta) > tol*10) { // if (rank == 0) { // cout << "# WARNING in CSysSolve::FGMRES(): " << endl; // cout << "# true residual norm and calculated residual norm do not agree." << endl; // cout << "# res - beta = " << res - beta << endl; // } // } return i; }

void TestTransitions( M & machine ) { machine.initiate(); ActionArray init = { Entry< S0< M > >, Entry< S1< M > >, Entry< Default0< S1< M > > >, Entry< S11< M > >, Entry< Default2< S1< M > > >, Entry< Default1< S0< M > > >, Entry< Default2< S0< M > > > }; machine.CompareToExpectedActionSequence( init ); machine.process_event( A() ); ActionArray a1 = { Exit< Default2< S1< M > > >, Exit< S11< M > >, Exit< Default0< S1< M > > >, Exit< S1< M > >, Trans< S0< M >, A >, Entry< S1< M > >, Entry< Default0< S1< M > > >, Entry< S11< M > >, Entry< Default2< S1< M > > > }; machine.CompareToExpectedActionSequence( a1 ); machine.process_event( B() ); ActionArray b1 = { Exit< Default2< S1< M > > >, Exit< S11< M > >, Exit< Default0< S1< M > > >, Exit< S1< M > >, Trans< S0< M >, B >, Entry< S1< M > >, Entry< Default0< S1< M > > >, Entry< S11< M > >, Entry< Default2< S1< M > > > }; machine.CompareToExpectedActionSequence( b1 ); machine.process_event( C() ); ActionArray c1 = { Exit< Default2< S1< M > > >, Exit< S11< M > >, Exit< Default0< S1< M > > >, Exit< S1< M > >, Trans< S0< M >, C >, Entry< S2< M > >, Entry< Default0< S2< M > > >, Entry< Default1< S2< M > > >, Entry< S21< M > >, Entry< Default0< S21< M > > >, Entry< S211< M > >, Entry< Default2< S21< M > > > }; machine.CompareToExpectedActionSequence( c1 ); machine.process_event( D() ); ActionArray d2 = { Exit< Default2< S21< M > > >, Exit< S211< M > >, Exit< Default0< S21< M > > >, Exit< S21< M > >, Trans< S0< M >, D >, Entry< S21< M > >, Entry< Default0< S21< M > > >, Entry< S211< M > >, Entry< Default2< S21< M > > > }; machine.CompareToExpectedActionSequence( d2 ); machine.process_event( E() ); ActionArray e2 = { Exit< Default2< S0< M > > >, Exit< Default1< S0< M > > >, Exit< Default2< S21< M > > >, Exit< S211< M > >, Exit< Default0< S21< M > > >, Exit< S21< M > >, Exit< Default1< S2< M > > >, Exit< Default0< S2< M > > >, Exit< S2< M > >, Exit< S0< M > >, Entry< S0< M > >, Entry< S2< M > >, Entry< Default0< S2< M > > >, Entry< Default1< S2< M > > >, Entry< S21< M > >, Entry< Default0< S21< M > > >, Entry< S211< M > >, Entry< Default2< S21< M > > >, Entry< Default1< S0< M > > >, Entry< Default2< S0< M > > > }; machine.CompareToExpectedActionSequence( e2 ); machine.process_event( F() ); ActionArray f2 = { Exit< Default2< S21< M > > >, Exit< S211< M > >, Exit< Default0< S21< M > > >, Exit< S21< M > >, Exit< Default1< S2< M > > >, Exit< Default0< S2< M > > >, Exit< S2< M > >, Trans< S0< M >, F >, Entry< S1< M > >, Entry< Default0< S1< M > > >, Entry< S11< M > >, Entry< Default2< S1< M > > > }; machine.CompareToExpectedActionSequence( f2 ); machine.process_event( G() ); ActionArray g1 = { Exit< Default2< S1< M > > >, Exit< S11< M > >, Exit< Default0< S1< M > > >, Exit< S1< M > >, Trans< S0< M >, G >, Entry< S2< M > >, Entry< Default0< S2< M > > >, Entry< Default1< S2< M > > >, Entry< S21< M > >, Entry< Default0< S21< M > > >, Entry< S211< M > >, Entry< Default2< S21< M > > > }; machine.CompareToExpectedActionSequence( g1 ); machine.process_event( H() ); ActionArray h2 = { Exit< Default2< S21< M > > >, Exit< S211< M > >, Exit< Default0< S21< M > > >, Exit< S21< M > >, Trans< S0< M >, H >, Entry< S21< M > >, Entry< Default0< S21< M > > >, Entry< S211< M > >, Entry< Default2< S21< M > > > }; machine.CompareToExpectedActionSequence( h2 ); BOOST_REQUIRE( machine.GetUnconsumedEventCount() == 0 ); machine.process_event( A() ); BOOST_REQUIRE( machine.GetUnconsumedEventCount() == 1 ); ActionArray a2 = { }; machine.CompareToExpectedActionSequence( a2 ); machine.process_event( B() ); ActionArray b2 = { Exit< Default2< S21< M > > >, Exit< S211< M > >, Exit< Default0< S21< M > > >, Exit< S21< M > >, Trans< S0< M >, B >, Entry< S21< M > >, Entry< Default0< S21< M > > >, Entry< S211< M > >, Entry< Default2< S21< M > > > }; machine.CompareToExpectedActionSequence( b2 ); machine.process_event( C() ); ActionArray c2 = { Exit< Default2< S21< M > > >, Exit< S211< M > >, Exit< Default0< S21< M > > >, Exit< S21< M > >, Exit< Default1< S2< M > > >, Exit< Default0< S2< M > > >, Exit< S2< M > >, Trans< S0< M >, C >, Entry< S1< M > >, Entry< Default0< S1< M > > >, Entry< S11< M > >, Entry< Default2< S1< M > > > }; machine.CompareToExpectedActionSequence( c2 ); machine.process_event( D() ); ActionArray d1 = { Exit< Default2< S0< M > > >, Exit< Default1< S0< M > > >, Exit< Default2< S1< M > > >, Exit< S11< M > >, Exit< Default0< S1< M > > >, Exit< S1< M > >, Exit< S0< M > >, Entry< S0< M > >, Entry< S1< M > >, Entry< Default0< S1< M > > >, Entry< S11< M > >, Entry< Default2< S1< M > > >, Entry< Default1< S0< M > > >, Entry< Default2< S0< M > > > }; machine.CompareToExpectedActionSequence( d1 ); machine.process_event( F() ); ActionArray f1 = { Exit< Default2< S1< M > > >, Exit< S11< M > >, Exit< Default0< S1< M > > >, Exit< S1< M > >, Trans< S0< M >, F >, Entry< S2< M > >, Entry< Default0< S2< M > > >, Entry< Default1< S2< M > > >, Entry< S21< M > >, Entry< Default0< S21< M > > >, Entry< S211< M > >, Entry< Default2< S21< M > > > }; machine.CompareToExpectedActionSequence( f1 ); machine.process_event( G() ); ActionArray g2 = { Exit< Default2< S0< M > > >, Exit< Default1< S0< M > > >, Exit< Default2< S21< M > > >, Exit< S211< M > >, Exit< Default0< S21< M > > >, Exit< S21< M > >, Exit< Default1< S2< M > > >, Exit< Default0< S2< M > > >, Exit< S2< M > >, Exit< S0< M > >, Entry< S0< M > >, Entry< S1< M > >, Entry< Default0< S1< M > > >, Entry< S11< M > >, Entry< Default2< S1< M > > >, Entry< Default1< S0< M > > >, Entry< Default2< S0< M > > > }; machine.CompareToExpectedActionSequence( g2 ); machine.process_event( H() ); ActionArray h1 = { Trans< S11< M >, H > }; machine.CompareToExpectedActionSequence( h1 ); machine.process_event( E() ); ActionArray e1 = { Exit< Default2< S0< M > > >, Exit< Default1< S0< M > > >, Exit< Default2< S1< M > > >, Exit< S11< M > >, Exit< Default0< S1< M > > >, Exit< S1< M > >, Exit< S0< M > >, Entry< S0< M > >, Entry< S2< M > >, Entry< Default0< S2< M > > >, Entry< Default1< S2< M > > >, Entry< S21< M > >, Entry< Default0< S21< M > > >, Entry< S211< M > >, Entry< Default2< S21< M > > >, Entry< Default1< S0< M > > >, Entry< Default2< S0< M > > > }; machine.CompareToExpectedActionSequence( e1 ); machine.terminate(); ActionArray term = { Exit< Default2< S0< M > > >, Exit< Default1< S0< M > > >, Exit< Default2< S21< M > > >, Exit< S211< M > >, Exit< Default0< S21< M > > >, Exit< S21< M > >, Exit< Default1< S2< M > > >, Exit< Default0< S2< M > > >, Exit< S2< M > >, Exit< S0< M > > }; machine.CompareToExpectedActionSequence( term ); machine.ThrowAction( &Entry< Default0< S1< M > > > ); BOOST_REQUIRE_THROW( machine.initiate(), TransitionTestException ); ActionArray initThrow1 = { Entry< S0< M > >, Entry< S1< M > >, &::Throw< &::Entry< Default0< S1< M > > > >, Dtor< S1< M > >, Dtor< S0< M > > }; machine.CompareToExpectedActionSequence( initThrow1 ); BOOST_REQUIRE( machine.terminated() ); machine.ThrowAction( &Entry< S11< M > > ); BOOST_REQUIRE_THROW( machine.initiate(), TransitionTestException ); ActionArray initThrow2 = { Entry< S0< M > >, Entry< S1< M > >, Entry< Default0< S1< M > > >, &::Throw< &::Entry< S11< M > > >, Dtor< Default0< S1< M > > >, Dtor< S1< M > >, Dtor< S0< M > > }; machine.CompareToExpectedActionSequence( initThrow2 ); BOOST_REQUIRE( machine.terminated() ); machine.ThrowAction( &Trans< S0< M >, A > ); machine.initiate(); BOOST_REQUIRE_THROW( machine.process_event( A() ), TransitionTestException ); ActionArray a1Throw1 = { Entry< S0< M > >, Entry< S1< M > >, Entry< Default0< S1< M > > >, Entry< S11< M > >, Entry< Default2< S1< M > > >, Entry< Default1< S0< M > > >, Entry< Default2< S0< M > > >, Exit< Default2< S1< M > > >, Exit< S11< M > >, Exit< Default0< S1< M > > >, Exit< S1< M > >, &::Throw< &::Trans< S0< M >, A > >, Dtor< Default2< S0< M > > >, Dtor< Default1< S0< M > > >, Dtor< S0< M > > }; machine.CompareToExpectedActionSequence( a1Throw1 ); BOOST_REQUIRE( machine.terminated() ); machine.ThrowAction( &Entry< S211< M > > ); machine.initiate(); BOOST_REQUIRE_THROW( machine.process_event( C() ), TransitionTestException ); ActionArray c1Throw1 = { Entry< S0< M > >, Entry< S1< M > >, Entry< Default0< S1< M > > >, Entry< S11< M > >, Entry< Default2< S1< M > > >, Entry< Default1< S0< M > > >, Entry< Default2< S0< M > > >, Exit< Default2< S1< M > > >, Exit< S11< M > >, Exit< Default0< S1< M > > >, Exit< S1< M > >, Trans< S0< M >, C >, Entry< S2< M > >, Entry< Default0< S2< M > > >, Entry< Default1< S2< M > > >, Entry< S21< M > >, Entry< Default0< S21< M > > >, &::Throw< &::Entry< S211< M > > >, Dtor< Default2< S0< M > > >, Dtor< Default1< S0< M > > >, Dtor< Default0< S21< M > > >, Dtor< S21< M > >, Dtor< Default1< S2< M > > >, Dtor< Default0< S2< M > > >, Dtor< S2< M > >, Dtor< S0< M > > }; machine.CompareToExpectedActionSequence( c1Throw1 ); BOOST_REQUIRE( machine.terminated() ); machine.ThrowAction( &ExitFn< S11< M > > ); machine.initiate(); BOOST_REQUIRE_THROW( machine.process_event( C() ), TransitionTestException ); ActionArray c1Throw2 = { Entry< S0< M > >, Entry< S1< M > >, Entry< Default0< S1< M > > >, Entry< S11< M > >, Entry< Default2< S1< M > > >, Entry< Default1< S0< M > > >, Entry< Default2< S0< M > > >, Exit< Default2< S1< M > > >, &::Throw< &::ExitFn< S11< M > > >, Dtor< S11< M > >, Dtor< Default2< S0< M > > >, Dtor< Default1< S0< M > > >, Dtor< Default0< S1< M > > >, Dtor< S1< M > >, Dtor< S0< M > > }; machine.CompareToExpectedActionSequence( c1Throw2 ); BOOST_REQUIRE( machine.terminated() ); BOOST_REQUIRE( machine.GetUnconsumedEventCount() == 1 ); }

int main(int argc, char** argv) { if(argc != 6) { std::cerr << "Usage: " << argv[0] << " in.png out.png match.txt left|right H.txt" <<std::endl; return 1; } // Read image size_t nx, ny; LWImage<unsigned char> in(0,0,0); in.data = read_png_u8_rgb(argv[1], &nx, &ny); in.w = static_cast<int>(nx); in.h = static_cast<int>(ny); in.comps=3; if(! in.data) { std::cerr << "Error reading image " << argv[1] << std::endl; return 1; } // Read correspondences std::vector<Match> match; if(! loadMatch(argv[3],match)) { std::cerr << "Failed reading " << argv[3] << std::endl; return 1; } // Read left/right image bool bLeft = (strcmp(argv[4],"left")==0); if(!bLeft && strcmp(argv[4], "right")!=0) { std::cerr << "Error: arg 4 must be 'left' or 'right'" << std::endl; return 1; } // Read homography libNumerics::Homography H; std::ifstream f(argv[5]); if((f >> H.mat()).fail()) { std::cerr << "Error reading homography file " << argv[2] << std::endl; return 1; } // Drawing lines int delta = in.h/(NUM_LINES+1); if(delta==0) delta=1; for(int i=delta; i < in.h; i+=delta) draw_horizontal_dashed_line(in.data, in.w, in.h, i, LENGTH_ON, LENGTH_OFF, CYAN); // Drawing SIFT std::vector<Match>::const_iterator it=match.begin(); for(; it != match.end(); ++it) { double x=it->x1, y=it->y1; if(! bLeft) { x = it->x2; y=it->y2; } H(x,y); int ix = static_cast<int>(std::floor(x+0.5)); int iy = static_cast<int>(std::floor(y+0.5)); if(0<=ix && ix<in.w && 0<=iy && iy<in.h) draw_cross(in.data, in.w, in.h, ix, iy, CROSS_HALF_LENGTH, RED); } // Write image if(write_png_u8(argv[2], in.data, in.w, in.h, in.comps) != 0) { std::cerr << "Error writing file " << argv[3] << std::endl; return 1; } return 0; }

HSVColor::HSVColor(const Color& c) { const RGBColor* rgbc = NULL; const HSVColor* hsvc = NULL; const HSVConeColor* hsvcc = NULL; if ( (rgbc = dynamic_cast<const RGBColor*>(&c)) != NULL) { appear_t rgbMin, rgbMax, delta; appear_t r = rgbc->R(), g = rgbc->G(), b = rgbc->B(); rgbMin = std::min(r,std::min(g,b)); rgbMax = std::max(r,std::max(g,b)); delta = rgbMax - rgbMin; V() = rgbMax; S() = 0; if (rgbMax > 0) S() = delta / rgbMax; // fixme: handle floating point check here. NUKLEI_ASSERT( 0 <= S() && S() <= 1 ); H() = 0; if (delta > 0) { if (rgbMax == r && rgbMax != g) H() += (g - b) / delta; if (rgbMax == g && rgbMax != b) H() += (2 + (b - r) / delta); if (rgbMax == b && rgbMax != r) H() += (4 + (r - g) / delta); H() *= 60; if (H() < 0) H() += 360; } H() *= M_PI / 180; } else if ( (hsvc = dynamic_cast<const HSVColor*>(&c)) != NULL) c_ = hsvc->c_; else if ( (hsvcc = dynamic_cast<const HSVConeColor*>(&c)) != NULL) { NUKLEI_ASSERT(hsvcc->W() > 0); V() = hsvcc->WV() / hsvcc->W(); if (V() > 0) { S() = std::sqrt( std::pow(hsvcc->SCosH(), 2) + std::pow(hsvcc->SSinH(), 2) ) / V(); if (hsvcc->SSinH() >= 0) H() = ACos(hsvcc->SCosH() / (V()*S())); else H() = 2*M_PI-ACos(hsvcc->SCosH() / (V()*S())); } else if (V() == 0) { S() = 1; H() = 0; } else NUKLEI_ASSERT(false); } else NUKLEI_THROW("Unknown color type"); assertConsistency(); }

/* Print out on stderr a line consisting of the test in S, a colon, a space, a message describing the meaning of the signal number PINFO and a newline. If S is NULL or "", the colon and space are omitted. */ void psiginfo (const siginfo_t *pinfo, const char *s) { char buf[512]; FILE *fp = __fmemopen (buf, sizeof (buf), "w"); if (fp == NULL) { const char *colon; if (s == NULL || *s == '\0') s = colon = ""; else colon = ": "; __fxprintf (NULL, "%s%ssignal %d\n", s, colon, pinfo->si_signo); return; } if (s != NULL && *s != '\0') fprintf (fp, "%s: ", s); const char *desc; if (pinfo->si_signo >= 0 && pinfo->si_signo < NSIG && ((desc = _sys_siglist[pinfo->si_signo]) != NULL #ifdef SIGRTMIN || (pinfo->si_signo >= SIGRTMIN && pinfo->si_signo < SIGRTMAX) #endif )) { #ifdef SIGRTMIN if (desc == NULL) { if (pinfo->si_signo - SIGRTMIN < SIGRTMAX - pinfo->si_signo) { if (pinfo->si_signo == SIGRTMIN) fprintf (fp, "SIGRTMIN ("); else fprintf (fp, "SIGRTMIN+%d (", pinfo->si_signo - SIGRTMIN); } else { if (pinfo->si_signo == SIGRTMAX) fprintf (fp, "SIGRTMAX ("); else fprintf (fp, "SIGRTMAX-%d (", SIGRTMAX - pinfo->si_signo); } } else #endif fprintf (fp, "%s (", _(desc)); const char *base = NULL; const uint8_t *offarr = NULL; size_t offarr_len = 0; switch (pinfo->si_signo) { #define H(sig) \ case sig: \ base = C(codestrs_, sig).str; \ offarr = C (codes_, sig); \ offarr_len = sizeof (C (codes_, sig)) / sizeof (C (codes_, sig)[0]);\ break H (SIGILL); H (SIGFPE); H (SIGSEGV); H (SIGBUS); H (SIGTRAP); H (SIGCHLD); H (SIGPOLL); } const char *str = NULL; if (offarr != NULL && pinfo->si_code >= 1 && pinfo->si_code <= offarr_len) str = base + offarr[pinfo->si_code - 1]; else switch (pinfo->si_code) { case SI_USER: str = N_("Signal sent by kill()"); break; case SI_QUEUE: str = N_("Signal sent by sigqueue()"); break; case SI_TIMER: str = N_("Signal generated by the expiration of a timer"); break; case SI_ASYNCIO: str = N_("\ Signal generated by the completion of an asynchronous I/O request"); break; case SI_MESGQ: str = N_("\ Signal generated by the arrival of a message on an empty message queue"); break; #ifdef SI_TKILL case SI_TKILL: str = N_("Signal sent by tkill()"); break; #endif #ifdef SI_ASYNCNL case SI_ASYNCNL: str = N_("\ Signal generated by the completion of an asynchronous name lookup request"); break; #endif #ifdef SI_SIGIO case SI_SIGIO: str = N_("\ Signal generated by the completion of an I/O request"); break; #endif #ifdef SI_KERNEL case SI_KERNEL: str = N_("Signal sent by the kernel"); break; #endif } if (str != NULL) fprintf (fp, "%s ", _(str)); else fprintf (fp, "%d ", pinfo->si_code); if (pinfo->si_signo == SIGILL || pinfo->si_signo == SIGFPE || pinfo->si_signo == SIGSEGV || pinfo->si_signo == SIGBUS) fprintf (fp, "[%p])\n", pinfo->si_addr); else if (pinfo->si_signo == SIGCHLD) fprintf (fp, "%ld %d %ld)\n", (long int) pinfo->si_pid, pinfo->si_status, (long int) pinfo->si_uid); else if (pinfo->si_signo == SIGPOLL) fprintf (fp, "%ld)\n", (long int) pinfo->si_band); else fprintf (fp, "%ld %ld)\n", (long int) pinfo->si_pid, (long int) pinfo->si_uid); } else

static void sha1_step(struct sha1_ctxt *ctxt) { unsigned int a, b, c, d, e, tmp; size_t t, s; #if BYTE_ORDER == LITTLE_ENDIAN struct sha1_ctxt tctxt; memcpy(&tctxt.m.b8[0], &ctxt->m.b8[0], 64); ctxt->m.b8[0] = tctxt.m.b8[3]; ctxt->m.b8[1] = tctxt.m.b8[2]; ctxt->m.b8[2] = tctxt.m.b8[1]; ctxt->m.b8[3] = tctxt.m.b8[0]; ctxt->m.b8[4] = tctxt.m.b8[7]; ctxt->m.b8[5] = tctxt.m.b8[6]; ctxt->m.b8[6] = tctxt.m.b8[5]; ctxt->m.b8[7] = tctxt.m.b8[4]; ctxt->m.b8[8] = tctxt.m.b8[11]; ctxt->m.b8[9] = tctxt.m.b8[10]; ctxt->m.b8[10] = tctxt.m.b8[9]; ctxt->m.b8[11] = tctxt.m.b8[8]; ctxt->m.b8[12] = tctxt.m.b8[15]; ctxt->m.b8[13] = tctxt.m.b8[14]; ctxt->m.b8[14] = tctxt.m.b8[13]; ctxt->m.b8[15] = tctxt.m.b8[12]; ctxt->m.b8[16] = tctxt.m.b8[19]; ctxt->m.b8[17] = tctxt.m.b8[18]; ctxt->m.b8[18] = tctxt.m.b8[17]; ctxt->m.b8[19] = tctxt.m.b8[16]; ctxt->m.b8[20] = tctxt.m.b8[23]; ctxt->m.b8[21] = tctxt.m.b8[22]; ctxt->m.b8[22] = tctxt.m.b8[21]; ctxt->m.b8[23] = tctxt.m.b8[20]; ctxt->m.b8[24] = tctxt.m.b8[27]; ctxt->m.b8[25] = tctxt.m.b8[26]; ctxt->m.b8[26] = tctxt.m.b8[25]; ctxt->m.b8[27] = tctxt.m.b8[24]; ctxt->m.b8[28] = tctxt.m.b8[31]; ctxt->m.b8[29] = tctxt.m.b8[30]; ctxt->m.b8[30] = tctxt.m.b8[29]; ctxt->m.b8[31] = tctxt.m.b8[28]; ctxt->m.b8[32] = tctxt.m.b8[35]; ctxt->m.b8[33] = tctxt.m.b8[34]; ctxt->m.b8[34] = tctxt.m.b8[33]; ctxt->m.b8[35] = tctxt.m.b8[32]; ctxt->m.b8[36] = tctxt.m.b8[39]; ctxt->m.b8[37] = tctxt.m.b8[38]; ctxt->m.b8[38] = tctxt.m.b8[37]; ctxt->m.b8[39] = tctxt.m.b8[36]; ctxt->m.b8[40] = tctxt.m.b8[43]; ctxt->m.b8[41] = tctxt.m.b8[42]; ctxt->m.b8[42] = tctxt.m.b8[41]; ctxt->m.b8[43] = tctxt.m.b8[40]; ctxt->m.b8[44] = tctxt.m.b8[47]; ctxt->m.b8[45] = tctxt.m.b8[46]; ctxt->m.b8[46] = tctxt.m.b8[45]; ctxt->m.b8[47] = tctxt.m.b8[44]; ctxt->m.b8[48] = tctxt.m.b8[51]; ctxt->m.b8[49] = tctxt.m.b8[50]; ctxt->m.b8[50] = tctxt.m.b8[49]; ctxt->m.b8[51] = tctxt.m.b8[48]; ctxt->m.b8[52] = tctxt.m.b8[55]; ctxt->m.b8[53] = tctxt.m.b8[54]; ctxt->m.b8[54] = tctxt.m.b8[53]; ctxt->m.b8[55] = tctxt.m.b8[52]; ctxt->m.b8[56] = tctxt.m.b8[59]; ctxt->m.b8[57] = tctxt.m.b8[58]; ctxt->m.b8[58] = tctxt.m.b8[57]; ctxt->m.b8[59] = tctxt.m.b8[56]; ctxt->m.b8[60] = tctxt.m.b8[63]; ctxt->m.b8[61] = tctxt.m.b8[62]; ctxt->m.b8[62] = tctxt.m.b8[61]; ctxt->m.b8[63] = tctxt.m.b8[60]; #endif a = H(0); b = H(1); c = H(2); d = H(3); e = H(4); for (t = 0; t < 20; t++) { s = t & 0x0f; if (t >= 16) W(s) = S(1, W((s+13) & 0x0f) ^ W((s+8) & 0x0f) ^ W((s+2) & 0x0f) ^ W(s)); tmp = S(5, a) + F0(b, c, d) + e + W(s) + K(t); e = d; d = c; c = S(30, b); b = a; a = tmp; } for (t = 20; t < 40; t++) { s = t & 0x0f; W(s) = S(1, W((s+13) & 0x0f) ^ W((s+8) & 0x0f) ^ W((s+2) & 0x0f) ^ W(s)); tmp = S(5, a) + F1(b, c, d) + e + W(s) + K(t); e = d; d = c; c = S(30, b); b = a; a = tmp; } for (t = 40; t < 60; t++) { s = t & 0x0f; W(s) = S(1, W((s+13) & 0x0f) ^ W((s+8) & 0x0f) ^ W((s+2) & 0x0f) ^ W(s)); tmp = S(5, a) + F2(b, c, d) + e + W(s) + K(t); e = d; d = c; c = S(30, b); b = a; a = tmp; } for (t = 60; t < 80; t++) { s = t & 0x0f; W(s) = S(1, W((s+13) & 0x0f) ^ W((s+8) & 0x0f) ^ W((s+2) & 0x0f) ^ W(s)); tmp = S(5, a) + F3(b, c, d) + e + W(s) + K(t); e = d; d = c; c = S(30, b); b = a; a = tmp; } H(0) = H(0) + a; H(1) = H(1) + b; H(2) = H(2) + c; H(3) = H(3) + d; H(4) = H(4) + e; bzero(&ctxt->m.b8[0], 64); }

static RBuffer* create(RBin* bin, const ut8 *code, int codelen, const ut8 *data, int datalen, RBinArchOptions *opt) { ut32 filesize, code_va, code_pa, phoff; ut32 p_start, p_phoff, p_phdr; ut32 p_ehdrsz, p_phdrsz; ut16 ehdrsz, phdrsz; ut32 p_vaddr, p_paddr, p_fs, p_fs2; ut32 baddr; int is_arm = 0; RBuffer *buf = r_buf_new (); r_return_val_if_fail (bin && opt && opt->arch, NULL); is_arm = !strcmp (opt->arch, "arm"); // XXX: hardcoded if (is_arm) { baddr = 0x40000; } else { baddr = 0x8048000; } #define B(x,y) r_buf_append_bytes(buf,(const ut8*)(x),y) #define D(x) r_buf_append_ut32(buf,x) #define H(x) r_buf_append_ut16(buf,x) #define Z(x) r_buf_append_nbytes(buf,x) #define W(x,y,z) r_buf_write_at(buf,x,(const ut8*)(y),z) #define WZ(x,y) p_tmp=r_buf_size (buf);Z(x);W(p_tmp,y,strlen(y)) B ("\x7F" "ELF" "\x01\x01\x01\x00", 8); Z (8); H (2); // ET_EXEC if (is_arm) { H (40); // e_machne = EM_ARM } else { H (3); // e_machne = EM_I386 } D (1); p_start = r_buf_size (buf); D (-1); // _start p_phoff = r_buf_size (buf); D (-1); // phoff -- program headers offset D (0); // shoff -- section headers offset D (0); // flags p_ehdrsz = r_buf_size (buf); H (-1); // ehdrsz p_phdrsz = r_buf_size (buf); H (-1); // phdrsz H (1); H (0); H (0); H (0); // phdr: p_phdr = r_buf_size (buf); D (1); D (0); p_vaddr = r_buf_size (buf); D (-1); // vaddr = $$ p_paddr = r_buf_size (buf); D (-1); // paddr = $$ p_fs = r_buf_size (buf); D (-1); // filesize p_fs2 = r_buf_size (buf); D (-1); // filesize D (5); // flags D (0x1000); // align ehdrsz = p_phdr; phdrsz = r_buf_size (buf) - p_phdr; code_pa = r_buf_size (buf); code_va = code_pa + baddr; phoff = 0x34;//p_phdr ; filesize = code_pa + codelen + datalen; W (p_start, &code_va, 4); W (p_phoff, &phoff, 4); W (p_ehdrsz, &ehdrsz, 2); W (p_phdrsz, &phdrsz, 2); code_va = baddr; // hack W (p_vaddr, &code_va, 4); code_pa = baddr; // hack W (p_paddr, &code_pa, 4); W (p_fs, &filesize, 4); W (p_fs2, &filesize, 4); B (code, codelen); if (data && datalen > 0) { //ut32 data_section = buf->length; eprintf ("Warning: DATA section not support for ELF yet\n"); B (data, datalen); } return buf; }

void ckbot::chain_rate::tip_base_step(std::vector<double> s, std::vector<double> T) { int N = c.num_links(); std::vector<double> q(N); std::vector<double> qd(N); for(int i=0; i<N; ++i) { q[i] = s[i]; qd[i] = s[N+i]; } Eigen::Matrix3d Iden = Eigen::Matrix3d::Identity(); Eigen::Matrix3d Zero = Eigen::Matrix3d::Zero(); /* Declare and initialized all of the loop variables */ /* TODO: Can we allocate all of this on the heap *once* for a particular * rate object, and then just use pointers to them after? Would this be fast? * Re-initializing these every time through here has to be slow... */ Eigen::MatrixXd pp(6,6); pp = Eigen::MatrixXd::Zero(6,6); Eigen::VectorXd zp(6); zp << 0,0,0,0,0,0; Eigen::VectorXd grav(6); grav << 0,0,0,0,0,9.81; Eigen::Matrix3d L_oc_tilde; Eigen::Matrix3d R_cur; Eigen::MatrixXd M_cur(6,6); Eigen::MatrixXd M_cm(6,6); M_cm = Eigen::MatrixXd::Zero(6,6); Eigen::Matrix3d J_o; Eigen::Vector3d r_i_ip(0,0,0); Eigen::Vector3d r_i_cm(0,0,0); Eigen::MatrixXd phi(6,6); Eigen::MatrixXd phi_cm(6,6); Eigen::MatrixXd p_cur(6,6); Eigen::VectorXd H_b_frame_star(6); Eigen::VectorXd H_w_frame_star(6); Eigen::RowVectorXd H(6); double D = 0.0; Eigen::VectorXd G(6); Eigen::MatrixXd tau_tilde(6,6); Eigen::Vector3d omega(0,0,0); Eigen::Matrix3d omega_cross; Eigen::VectorXd a(6); Eigen::VectorXd b(6); Eigen::VectorXd z(6); double C = 0.0; double CF = 0.0; double SPOLES = 12.0; double RPOLES = 14.0; double epsilon = 0.0; double mu = 0.0; /* Recurse from the tip to the base of this chain */ for (int i = N-1; i >= 0; --i) { module_link& cur = c.get_link(i); /* Rotation from the world to this link */ R_cur = c.get_current_R(i); /* Vector, in world frame, from inbound joint to outbound joint of * this link */ r_i_ip = R_cur*(cur.get_r_ip1() - cur.get_r_im1()); /* Operator to transform spatial vectors from outbound joint * to the inbound joint */ phi = get_body_trans(r_i_ip); /* Vector from the inbound joint to the center of mass */ r_i_cm = R_cur*(-cur.get_r_im1()); /* Operator to transform spatial vectors from the center of mass * to the inbound joint */ phi_cm = get_body_trans(r_i_cm); /* Cross product matrix corresponding to the vector from * the inbound joint to the center of mass */ L_oc_tilde = get_cross_mat(r_i_cm); /* 3x3 Inertia matrix of this link about its inbound joint and wrt the * world coordinate system. * * NOTE: Similarity transform of get_I_cm() because it is wrt * the module frame */ J_o = R_cur*cur.get_I_cm()*R_cur.transpose() - cur.get_mass()*L_oc_tilde*L_oc_tilde; /* Spatial mass matrix of this link about its inbound joint and wrt the * world coordinate system. * */ M_cur << J_o, cur.get_mass()*L_oc_tilde, -cur.get_mass()*L_oc_tilde, cur.get_mass()*Iden; /* Spatial mass matrix of this link about its cm and wrt the * world coordinate system. * * NOTE: Similarity transform of get_I_cm() because it is wrt * the module frame */ M_cm << R_cur*cur.get_I_cm()*R_cur.transpose(), Zero, Zero, cur.get_mass()*Iden; /* Articulated Body Spatial inertia matrix of the links from * this one all the way to the tip of the chain (accounting for * articulated body spatial inertia that gets through each joint) * * pp is p_cur from the previous (outbound) link. This is the * zero vector when we are on the farthest outbound link (the tip) * * In much the same way that we use a similarity transform to change * the frame of the inertia matrix, a similarity transform moves * pp from the previous module's base joint to this module's base joint. */ p_cur = phi*pp*phi.transpose() + M_cur; /* The joint matrix, in the body frame, that maps the change * in general coordinates relating this link to the next inward link. * * It essentially defines what "gets through" a link. IE: * H_b_frame_star = [0,0,1,0,0,0] means that movements and forces * in the joint's rotational Z axis get through. This is a single * DOF rotational joint. * * H_b_frame_star = eye(6) means that forces in all 6 * (3 rotational, 3 linear) get through the joint using * 6 independent general coordinates. This is a * free 6DOF joint. * * H_b_frame_star = [0,0,2,0,0,1] is helical joint that rotates twice * for every single linear unit moved. It is controlled by a single * general coordinate. * * (NOTE/TODO: Using anything but [0,0,1,0,0,0] breaks the code right now.) */ H_b_frame_star = cur.get_joint_matrix().transpose(); /* To transform a spatial vector (6 x 1) from one coordinate * system to another, multiply it by the 6x6 matrix with * the rotation matrix form one frame to the other on the diagonals */ Eigen::MatrixXd tmp_6x6(6,6); tmp_6x6 << R_cur, Eigen::Matrix3d::Zero(), Eigen::Matrix3d::Zero(), R_cur; /* Joint matrix in the world frame */ H_w_frame_star = tmp_6x6*H_b_frame_star; /* As a row vector */ H = H_w_frame_star.transpose(); D = H*p_cur*H.transpose(); /* TODO:Could use H_w_frame_star..but..clarity */ /* TODO: This needs to be changed to D.inverse() for 6DOF * and D needs to be made a variable sized Eigen Matrix */ G = p_cur*H.transpose()*(1.0/D); /* Articulated body projection operator through this joint. This defines * which part of the articulated body spatial inertia gets through this joint */ tau_tilde = Eigen::MatrixXd::Identity(6,6) - G*H; /* pp for the next time around the loop */ pp = tau_tilde*p_cur; omega = c.get_angular_velocity(i); omega_cross = get_cross_mat(omega); /* Gyroscopic force */ b << omega_cross*J_o*omega, cur.get_mass()*omega_cross*omega_cross*r_i_cm; /* Coriolis and centripetal accel */ a << 0,0,0, omega_cross*omega_cross*r_i_cm; /* z is the total spatial force seen by this link at its inward joint * phi*zp = Force through joint from tip-side module * b = Gyroscopic force (velocity dependent) * p_cur*a = Coriolis and Centripetal forces * phi_cm*M_cm*grav = Force of grav at CM transformed to inbound jt */ z = phi*zp + b + p_cur*a + phi_cm*M_cm*grav; /* Velocity related damping force in the joint */ /* TODO: 6DOF change. epsilon will be an matrix when the joint * matrix is not a row matrix, so using C in the calculation * of epsilon will break things. */ C = -cur.get_damping()*qd[i]; /* Cogging force which is a function of the joint angle and * the motor specifics. */ CF = cur.get_rotor_cogging()*sin(RPOLES*(q[i]-cur.get_cogging_offset())) + cur.get_stator_cogging()*sin(SPOLES*(q[i]-cur.get_cogging_offset())); /* The "Effective" force through the joint that can actually create * accelerations. IE: Forces that are in directions in which the joint * can actually move. */ /* TODO: 6DOF change. Epsilon will not be 1x1 for a 6DOF joint, so * both T[i] and C used here as they are will break things. */ epsilon = T[i] + C + CF - H*z; mu = (1/D)*epsilon; /* 6DOF change: D will be a matrix, need to invert it */ zp = z + G*epsilon; /* base_tip_step needs G, a, and mu. So store them in the chain object.*/ mu_all[i] = mu; int cur_index=0; for (int k=(6*i); k <= (6*(i+1)-1); ++k,++cur_index) { G_all[k] = G[cur_index]; a_all[k] = a[cur_index]; } } }

/*! control \brief calculate the number of packets sent by a same source over a given period of time. If too many packets are sent, following packets are rejected Parameters required: function = hash; backup = /etc/honeybrid/control.tb expiration = 600 max_packet = 1000 \param[in] pkts, struct that contain the packet to control \param[out] set result to 1 if rate limit reached, 0 otherwise */ mod_result_t mod_control(struct mod_args *args) { gchar *backup_file; if (args->pkt == NULL) { printdbg("%s Error, NULL packet\n", H(6)); return REJECT; } printdbg("%s Module called\n", H(args->pkt->conn->id)); mod_result_t result = DEFER; int expiration; int max_packet; gchar *param; gchar **info; GKeyFile *backup; GTimeVal t; g_get_current_time(&t); gint now = (t.tv_sec); char src[INET_ADDRSTRLEN]; inet_ntop(AF_INET, &(args->pkt->packet.ip->saddr), src, INET_ADDRSTRLEN); /*! get the backup file for this module */ if (NULL == (backup = (GKeyFile *) g_hash_table_lookup(args->node->config, "backup"))) { /*! We can't decide */ printdbg("%s mandatory argument 'backup' undefined!\n", H(args->pkt->conn->id)); return result; } /*! get the backup file path for this module */ if (NULL == (backup_file = (gchar *) g_hash_table_lookup(args->node->config, "backup_file"))) { /*! We can't decide */ printdbg("%s error, backup file path missing\n", H(args->pkt->conn->id)); return result; } /*! get control parameters */ if (NULL == (param = (gchar *) g_hash_table_lookup(args->node->config, "expiration"))) { /*! no value set for expiration, we go with the default one */ expiration = 600; } else { expiration = atoi(param); } if (NULL == (param = (gchar *) g_hash_table_lookup(args->node->config, "max_packet"))) { /*! no value set for expiration, we go with the default one */ max_packet = 1000; } else { max_packet = atoi(param); } if (NULL == (info = g_key_file_get_string_list(backup, "source", /* generic group name \todo: group by port number? */ src, NULL, NULL))) { printdbg("%s IP not found... new entry created\n", H(args->pkt->conn->id)); info = malloc(3 * sizeof(char *)); /*! 20 characters should be enough to hold even very large numbers */ info[0] = malloc(20 * sizeof(gchar)); info[1] = malloc(20 * sizeof(gchar)); info[2] = malloc(20 * sizeof(gchar)); g_snprintf(info[0], 20, "1"); /*! counter */ g_snprintf(info[1], 20, "%d", now); /*! first seen */ g_snprintf(info[2], 20, "0"); /*! duration */ } else { /*! We check if we need to expire this entry */ int age = atoi(info[2]); if (age > expiration) { printdbg("%s IP found but expired... entry renewed\n", H(args->pkt->conn->id)); g_snprintf(info[0], 20, "1"); /*! counter */ g_snprintf(info[1], 20, "%d", now); /*! first seen */ g_snprintf(info[2], 20, "0"); /*! duration */ } else { printdbg("%s IP found... entry updated\n", H(args->pkt->conn->id)); g_snprintf(info[0], 20, "%d", atoi(info[0]) + 1); /*! counter */ g_snprintf(info[2], 20, "%d", now - atoi(info[1])); /*! duration */ } } if (atoi(info[0]) > max_packet) { printdbg("%s Rate limit reached! Packet rejected\n", H(args->pkt->conn->id)); result = REJECT; } else { printdbg("%s Rate limit not reached. Packet accepted\n", H(args->pkt->conn->id)); result = ACCEPT; } g_key_file_set_string_list(backup, "source", src, (const gchar * const *) info, 3); save_backup(backup, backup_file); return result; }

/* Returns a vector of length N corresponding to each link's qdd */ std::vector<double> ckbot::chain_rate::base_tip_step(std::vector<double> s, std::vector<double> T) { int N = c.num_links(); std::vector<double> q(N); std::vector<double> qd(N); std::vector<double> qdd(N); for(int i=0; i<N; ++i) { q[i] = s[i]; qd[i] = s[N+i]; } Eigen::VectorXd grav(6); grav << 0,0,0,0,0,9.81; Eigen::Matrix3d R_cur; Eigen::Vector3d r_i_ip; Eigen::MatrixXd phi(6,6); Eigen::VectorXd alpha(6); alpha = Eigen::VectorXd::Zero(6); Eigen::VectorXd alpha_p(6); Eigen::VectorXd G(6); Eigen::VectorXd a(6); Eigen::VectorXd H_b_frame_star(6); Eigen::VectorXd H_w_frame_star(6); Eigen::RowVectorXd H(6); for (int i=0; i<N; ++i) { module_link& cur = c.get_link(i); /* 3x3 Rotation matrix from this link's coordinate * frame to the world frame */ R_cur = c.get_current_R(i); /* Vector, in world frame, from this link's inbound joint to its * outbound joint */ r_i_ip = R_cur*(cur.get_r_ip1() - cur.get_r_im1()); /* 6x6 Spatial body operator matrix that transforms spatial * vectors from the outbound joint to the inbound joint in * the world frame */ phi = get_body_trans(r_i_ip); /* Transform the spatial accel vector from the inbound * link's inbound joint to this link's inbound joint */ alpha_p = phi.transpose()*alpha; /* These are calculated in tip_base_step */ int cur_index = 0; for (int k = (6*i); k <= (6*(i+1)-1); ++k, ++cur_index) { G[cur_index] = G_all[k]; a[cur_index] = a_all[k]; } /* The angular acceleration of this link's joint */ /* TODO: 6DOF changes here...This gets a whoooole lotta broken */ qdd[i] = mu_all[i] - G.transpose()*alpha_p; /* Joint matrix in body frame. Check tip_base_step for the definition */ /* TODO: 6DOF changes here a plenty */ H_b_frame_star = cur.get_joint_matrix().transpose(); Eigen::MatrixXd tmp_6x6(6,6); tmp_6x6 << R_cur, Eigen::Matrix3d::Zero(), Eigen::Matrix3d::Zero(), R_cur; /* Joint matrix in the world frame */ H_w_frame_star = tmp_6x6*H_b_frame_star; /* As a row vector */ H = H_w_frame_star.transpose(); /* Spatial acceleration of this link at its inbound joint in * its frame (ie: including its joint accel) */ alpha = alpha_p + H.transpose()*qdd[i] + a; } return qdd; }

void foo (int p1, int *, int) { if (p1 == 0) throw H (); }

//***************************************************************************************** //Creer un nouveau fichier netcdf de output avec mes valeurs... Semble ok //* //Reproduit les dimensions et les variables des fichiers u,v et w... Semble ok //* //Creer les variables FR frontieres et SO de source... Semble ok //* //Creer la variable C de concentration... Semble ok //* //Copier les attributs... Semble ok //* //Creer les attributs pour C, FR et SO... Semble ok et a finir (plus tard) //* //Inserer les valeurs dans les dimensions... A tester //* //Inserer les valeurs dans u,v et w a partir des blocs... A tester //* //Inserer les valeurs dans FR et SO... A tester //* //Attention! La date n'a rien a voir maintenant avec le temps en output! //* void cdf_new(s_nc_all_files *c, s_all_parameters *p, s_memory_blocs *b){ //* //***************************************************************************************** int status; int Cdimshape_p[NVDIM]; int Udimshape_p[NVDIM-1]; float val; float range[2]; short int short_fill; c->out.file.name_p=p->filename; //Passe le nom du fichier de output. strcpy(c->out.x.name_p,"X"); //Passe le nom des dimensions. strcpy(c->out.y.name_p,"Y"); strcpy(c->out.time.name_p,"TIME"); strcpy(c->out.c.name_p,"C"); //Passe le nom de la variable de concentration. strcpy(c->out.u.name_p,"U"); strcpy(c->out.v.name_p,"V"); #ifdef BLOC_DIM_3D strcpy(c->out.z.name_p,"Z"); strcpy(c->out.w.name_p,"W"); #endif strcpy(c->out.fr.name_p,"FR"); strcpy(c->out.so.name_p,"SO"); H(nc_create(c->out.file.name_p, NC_CLOBBER, &(c->out.file.ncid))) //Definit les dimensions, leurs variables respectives, et leurs attributs. //Manque de creer les attributs pour la variable time! (optionnel) H(CDF_DEF_DIM(x)) H(CDF_DEF_DIM(y)) H(nc_def_dim(c->out.file.ncid, c->out.time.name_p, p->x.N/p->x.D+1, &(c->out.time.id))) H(CDF_DEF_DVAR(x)) H(CDF_DEF_DVAR(y)) H(CDF_DEF_DVAR(time)) H(CDF_COPY_DATT(x,axis)) H(CDF_COPY_DATT(y,axis)) H(CDF_COPY_DATT(x,units)) H(CDF_COPY_DATT(y,units)) #ifdef BLOC_DIM_3D H(CDF_DEF_DIM(z)) H(CDF_DEF_DVAR(z)) H(CDF_COPY_DATT(z,axis)) H(CDF_COPY_DATT(z,units)) #endif //Definit les variables et leurs attributs. Cdimshape_p[O_TIME]=c->out.time.id; Cdimshape_p[O_LAT]=c->out.y.id; Cdimshape_p[O_LON]=c->out.x.id; Udimshape_p[U_LAT]=c->out.y.id; Udimshape_p[U_LON]=c->out.x.id; #ifdef BLOC_DIM_3D Udimshape_p[U_DEPTH]=c->out.z.id; Cdimshape_p[O_DEPTH]=c->out.z.id; #endif #ifdef CALCSWITCH_ON H(CDF_DEF_CVAR(c)) #endif H(CDF_DEF_UVAR(u)) H(CDF_DEF_UVAR(v)) #ifdef BLOC_DIM_3D H(CDF_DEF_UVAR(w)) #endif H(CDF_DEF_BVAR(fr)) H(CDF_DEF_BVAR(so)) val=MYFILL; short_fill=-32767; range[0]=0.0; range[1]=1.0; #ifdef CALCSWITCH_ON H(CDF_PUT_F_ATT(var, c, fill, &(val))) H(CDF_PUT_F_ATT(var, c, missing, &(val))) H(CDF_COPY_ATT(var,c,units)) H(nc_put_att_float(c->out.file.ncid, c->out.c.id, "valid_range", NC_FLOAT, 2, range)) #endif H(CDF_PUT_F_ATT(var, u, fill, &(val))) H(CDF_PUT_F_ATT(var, u, missing, &(val))) H(CDF_COPY_ATT(var,u,units)) H(CDF_PUT_F_ATT(var, v, fill, &(val))) H(CDF_PUT_F_ATT(var, v, missing, &(val))) H(CDF_COPY_ATT(var,v,units)) H(CDF_PUT_S_ATT(var, fr, fill, &(short_fill))) #ifdef BLOC_DIM_3D H(CDF_PUT_F_ATT(var, w, fill, &(val))) H(CDF_PUT_F_ATT(var, w, missing, &(val))) H(CDF_COPY_ATT(var,w,units)) #endif //Remplit les champs des dimensions et des variables. #ifdef CALCSWITCH_ON H(CDF_INQ_VAR(c,id)) #endif H(CDF_INQ_VAR(so,id)) H(CDF_INQ_VAR(fr,id)) H(CDF_INQ_VAR(u,id)) H(CDF_INQ_VAR(v,id)) H(CDF_INQ_VAR(x,varid)) H(CDF_INQ_VAR(y,varid)) H(CDF_INQ_VAR(time,varid)) H(CDF_INQ_DIM(x)) H(CDF_INQ_DIM(y)) #ifdef BLOC_DIM_3D H(CDF_INQ_VAR(w,id)) H(CDF_INQ_VAR(z,varid)) H(CDF_INQ_DIM(z)) #endif H(CDF_INQ_DIM(time)) #ifdef CALCSWITCH_ON H(CDF_INQ_ATTID(c,id,missing)) H(CDF_INQ_ATTID(c,id,fill)) H(CDF_INQ_ATTID(c,id,units)) #endif H(CDF_INQ_ATTID(u,id,missing)) H(CDF_INQ_ATTID(u,id,fill)) H(CDF_INQ_ATTID(u,id,units)) H(CDF_INQ_ATTID(v,id,missing)) H(CDF_INQ_ATTID(v,id,fill)) H(CDF_INQ_ATTID(v,id,units)) H(CDF_INQ_ATTID(x,varid,axis)) H(CDF_INQ_ATTID(y,varid,axis)) H(CDF_INQ_ATTID(x,varid,units)) H(CDF_INQ_ATTID(y,varid,units)) #ifdef CALCSWITCH_ON H(CDF_INQ_ATT(c,id,missing)) H(CDF_INQ_ATT(c,id,fill)) H(CDF_INQ_ATT(c,id,units)) #endif H(CDF_INQ_ATT(u,id,missing)) H(CDF_INQ_ATT(u,id,fill)) H(CDF_INQ_ATT(u,id,units)) H(CDF_INQ_ATT(v,id,missing)) H(CDF_INQ_ATT(v,id,fill)) H(CDF_INQ_ATT(v,id,units)) H(CDF_INQ_ATT(x,varid,axis)) H(CDF_INQ_ATT(y,varid,axis)) H(CDF_INQ_ATT(x,varid,units)) H(CDF_INQ_ATT(y,varid,units)) #ifdef BLOC_DIM_3D H(CDF_INQ_ATTID(w,id,missing)) H(CDF_INQ_ATTID(w,id,fill)) H(CDF_INQ_ATTID(w,id,units)) H(CDF_INQ_ATTID(z,varid,axis)) H(CDF_INQ_ATTID(z,varid,units)) H(CDF_INQ_ATT(w,id,missing)) H(CDF_INQ_ATT(w,id,fill)) H(CDF_INQ_ATT(w,id,units)) H(CDF_INQ_ATT(z,varid,axis)) H(CDF_INQ_ATT(z,varid,units)) #endif H(nc_enddef(c->out.file.ncid)) //Insere les valeurs dans les dimensions (excepte le temps). CDF_FILL_DIM(x,lon_p) CDF_FILL_DIM(y,lat_p) #ifdef BLOC_DIM_3D CDF_FILL_DIM(z,depth_p) #endif cdf_fill_time(&(c->out), p); //Insere les valeurs dans les variables u,v,w,so et fr. CDF_FILL_VAR(V_u,u) CDF_FILL_VAR(V_v,v) #ifdef BLOC_DIM_3D CDF_FILL_VAR(V_w,w) #endif CDF_FILL_BVAR(C_so,so) CDF_FILL_BVAR(C_fr,fr) return; }

int test_vec3_ctor() { int Error = 0; #if(GLM_HAS_INITIALIZER_LISTS) { glm::vec3 a{ 0, 1, 2 }; std::vector<glm::vec3> v = { {0, 1, 2}, {4, 5, 6}, {8, 9, 0} }; } { glm::dvec3 a{ 0, 1, 2 }; std::vector<glm::dvec3> v = { {0, 1, 2}, {4, 5, 6}, {8, 9, 0} }; } #endif #if(GLM_HAS_ANONYMOUS_UNION && defined(GLM_SWIZZLE)) { glm::vec3 A = glm::vec3(1.0f, 2.0f, 3.0f); glm::vec3 B = A.xyz; glm::vec3 C(A.xyz); glm::vec3 D(A.xyz()); glm::vec3 E(A.x, A.yz); glm::vec3 F(A.x, A.yz()); glm::vec3 G(A.xy, A.z); glm::vec3 H(A.xy(), A.z); Error += glm::all(glm::equal(A, B)) ? 0 : 1; Error += glm::all(glm::equal(A, C)) ? 0 : 1; Error += glm::all(glm::equal(A, D)) ? 0 : 1; Error += glm::all(glm::equal(A, E)) ? 0 : 1; Error += glm::all(glm::equal(A, F)) ? 0 : 1; Error += glm::all(glm::equal(A, G)) ? 0 : 1; Error += glm::all(glm::equal(A, H)) ? 0 : 1; } #endif//(GLM_HAS_ANONYMOUS_UNION && defined(GLM_SWIZZLE)) { glm::vec3 A(1); glm::vec3 B(1, 1, 1); Error += A == B ? 0 : 1; } { std::vector<glm::vec3> Tests; Tests.push_back(glm::vec3(glm::vec2(1, 2), 3)); Tests.push_back(glm::vec3(1, glm::vec2(2, 3))); Tests.push_back(glm::vec3(1, 2, 3)); Tests.push_back(glm::vec3(glm::vec4(1, 2, 3, 4))); for(std::size_t i = 0; i < Tests.size(); ++i) Error += Tests[i] == glm::vec3(1, 2, 3) ? 0 : 1; } return Error; }

static void doTheRounds(uint32 X[16], uint32 state[4]) { uint32 a, b, c, d; a = state[0]; b = state[1]; c = state[2]; d = state[3]; /* round 1 */ a = b + ROT_LEFT((a + F(b, c, d) + X[0] + 0xd76aa478), 7); /* 1 */ d = a + ROT_LEFT((d + F(a, b, c) + X[1] + 0xe8c7b756), 12); /* 2 */ c = d + ROT_LEFT((c + F(d, a, b) + X[2] + 0x242070db), 17); /* 3 */ b = c + ROT_LEFT((b + F(c, d, a) + X[3] + 0xc1bdceee), 22); /* 4 */ a = b + ROT_LEFT((a + F(b, c, d) + X[4] + 0xf57c0faf), 7); /* 5 */ d = a + ROT_LEFT((d + F(a, b, c) + X[5] + 0x4787c62a), 12); /* 6 */ c = d + ROT_LEFT((c + F(d, a, b) + X[6] + 0xa8304613), 17); /* 7 */ b = c + ROT_LEFT((b + F(c, d, a) + X[7] + 0xfd469501), 22); /* 8 */ a = b + ROT_LEFT((a + F(b, c, d) + X[8] + 0x698098d8), 7); /* 9 */ d = a + ROT_LEFT((d + F(a, b, c) + X[9] + 0x8b44f7af), 12); /* 10 */ c = d + ROT_LEFT((c + F(d, a, b) + X[10] + 0xffff5bb1), 17); /* 11 */ b = c + ROT_LEFT((b + F(c, d, a) + X[11] + 0x895cd7be), 22); /* 12 */ a = b + ROT_LEFT((a + F(b, c, d) + X[12] + 0x6b901122), 7); /* 13 */ d = a + ROT_LEFT((d + F(a, b, c) + X[13] + 0xfd987193), 12); /* 14 */ c = d + ROT_LEFT((c + F(d, a, b) + X[14] + 0xa679438e), 17); /* 15 */ b = c + ROT_LEFT((b + F(c, d, a) + X[15] + 0x49b40821), 22); /* 16 */ /* round 2 */ a = b + ROT_LEFT((a + G(b, c, d) + X[1] + 0xf61e2562), 5); /* 17 */ d = a + ROT_LEFT((d + G(a, b, c) + X[6] + 0xc040b340), 9); /* 18 */ c = d + ROT_LEFT((c + G(d, a, b) + X[11] + 0x265e5a51), 14); /* 19 */ b = c + ROT_LEFT((b + G(c, d, a) + X[0] + 0xe9b6c7aa), 20); /* 20 */ a = b + ROT_LEFT((a + G(b, c, d) + X[5] + 0xd62f105d), 5); /* 21 */ d = a + ROT_LEFT((d + G(a, b, c) + X[10] + 0x02441453), 9); /* 22 */ c = d + ROT_LEFT((c + G(d, a, b) + X[15] + 0xd8a1e681), 14); /* 23 */ b = c + ROT_LEFT((b + G(c, d, a) + X[4] + 0xe7d3fbc8), 20); /* 24 */ a = b + ROT_LEFT((a + G(b, c, d) + X[9] + 0x21e1cde6), 5); /* 25 */ d = a + ROT_LEFT((d + G(a, b, c) + X[14] + 0xc33707d6), 9); /* 26 */ c = d + ROT_LEFT((c + G(d, a, b) + X[3] + 0xf4d50d87), 14); /* 27 */ b = c + ROT_LEFT((b + G(c, d, a) + X[8] + 0x455a14ed), 20); /* 28 */ a = b + ROT_LEFT((a + G(b, c, d) + X[13] + 0xa9e3e905), 5); /* 29 */ d = a + ROT_LEFT((d + G(a, b, c) + X[2] + 0xfcefa3f8), 9); /* 30 */ c = d + ROT_LEFT((c + G(d, a, b) + X[7] + 0x676f02d9), 14); /* 31 */ b = c + ROT_LEFT((b + G(c, d, a) + X[12] + 0x8d2a4c8a), 20); /* 32 */ /* round 3 */ a = b + ROT_LEFT((a + H(b, c, d) + X[5] + 0xfffa3942), 4); /* 33 */ d = a + ROT_LEFT((d + H(a, b, c) + X[8] + 0x8771f681), 11); /* 34 */ c = d + ROT_LEFT((c + H(d, a, b) + X[11] + 0x6d9d6122), 16); /* 35 */ b = c + ROT_LEFT((b + H(c, d, a) + X[14] + 0xfde5380c), 23); /* 36 */ a = b + ROT_LEFT((a + H(b, c, d) + X[1] + 0xa4beea44), 4); /* 37 */ d = a + ROT_LEFT((d + H(a, b, c) + X[4] + 0x4bdecfa9), 11); /* 38 */ c = d + ROT_LEFT((c + H(d, a, b) + X[7] + 0xf6bb4b60), 16); /* 39 */ b = c + ROT_LEFT((b + H(c, d, a) + X[10] + 0xbebfbc70), 23); /* 40 */ a = b + ROT_LEFT((a + H(b, c, d) + X[13] + 0x289b7ec6), 4); /* 41 */ d = a + ROT_LEFT((d + H(a, b, c) + X[0] + 0xeaa127fa), 11); /* 42 */ c = d + ROT_LEFT((c + H(d, a, b) + X[3] + 0xd4ef3085), 16); /* 43 */ b = c + ROT_LEFT((b + H(c, d, a) + X[6] + 0x04881d05), 23); /* 44 */ a = b + ROT_LEFT((a + H(b, c, d) + X[9] + 0xd9d4d039), 4); /* 45 */ d = a + ROT_LEFT((d + H(a, b, c) + X[12] + 0xe6db99e5), 11); /* 46 */ c = d + ROT_LEFT((c + H(d, a, b) + X[15] + 0x1fa27cf8), 16); /* 47 */ b = c + ROT_LEFT((b + H(c, d, a) + X[2] + 0xc4ac5665), 23); /* 48 */ /* round 4 */ a = b + ROT_LEFT((a + I(b, c, d) + X[0] + 0xf4292244), 6); /* 49 */ d = a + ROT_LEFT((d + I(a, b, c) + X[7] + 0x432aff97), 10); /* 50 */ c = d + ROT_LEFT((c + I(d, a, b) + X[14] + 0xab9423a7), 15); /* 51 */ b = c + ROT_LEFT((b + I(c, d, a) + X[5] + 0xfc93a039), 21); /* 52 */ a = b + ROT_LEFT((a + I(b, c, d) + X[12] + 0x655b59c3), 6); /* 53 */ d = a + ROT_LEFT((d + I(a, b, c) + X[3] + 0x8f0ccc92), 10); /* 54 */ c = d + ROT_LEFT((c + I(d, a, b) + X[10] + 0xffeff47d), 15); /* 55 */ b = c + ROT_LEFT((b + I(c, d, a) + X[1] + 0x85845dd1), 21); /* 56 */ a = b + ROT_LEFT((a + I(b, c, d) + X[8] + 0x6fa87e4f), 6); /* 57 */ d = a + ROT_LEFT((d + I(a, b, c) + X[15] + 0xfe2ce6e0), 10); /* 58 */ c = d + ROT_LEFT((c + I(d, a, b) + X[6] + 0xa3014314), 15); /* 59 */ b = c + ROT_LEFT((b + I(c, d, a) + X[13] + 0x4e0811a1), 21); /* 60 */ a = b + ROT_LEFT((a + I(b, c, d) + X[4] + 0xf7537e82), 6); /* 61 */ d = a + ROT_LEFT((d + I(a, b, c) + X[11] + 0xbd3af235), 10); /* 62 */ c = d + ROT_LEFT((c + I(d, a, b) + X[2] + 0x2ad7d2bb), 15); /* 63 */ b = c + ROT_LEFT((b + I(c, d, a) + X[9] + 0xeb86d391), 21); /* 64 */ state[0] += a; state[1] += b; state[2] += c; state[3] += d; }

double LT() const { size_t E = m_root_node.labels_size; return E * 8 + E * H(E, m_root_node.n_internal_nodes + m_root_node.n_leaves - 1); }

static inline int NCR5380_pwrite(struct Scsi_Host *host, unsigned char *addr, int len) { unsigned long *laddr; void __iomem *dma = priv(host)->dma + 0x2000; if(!len) return 0; writeb(0x02, priv(host)->base + CTRL); laddr = (unsigned long *)addr; while(len >= 32) { unsigned int status; unsigned long v; status = readb(priv(host)->base + STAT); if(status & 0x80) goto end; if(!(status & 0x40)) continue; v=*laddr++; writew(L(v), dma); writew(H(v), dma); v=*laddr++; writew(L(v), dma); writew(H(v), dma); v=*laddr++; writew(L(v), dma); writew(H(v), dma); v=*laddr++; writew(L(v), dma); writew(H(v), dma); v=*laddr++; writew(L(v), dma); writew(H(v), dma); v=*laddr++; writew(L(v), dma); writew(H(v), dma); v=*laddr++; writew(L(v), dma); writew(H(v), dma); v=*laddr++; writew(L(v), dma); writew(H(v), dma); len -= 32; if(len == 0) break; } addr = (unsigned char *)laddr; writeb(0x12, priv(host)->base + CTRL); while(len > 0) { unsigned int status; status = readb(priv(host)->base + STAT); if(status & 0x80) goto end; if(status & 0x40) { writeb(*addr++, dma); if(--len == 0) break; } status = readb(priv(host)->base + STAT); if(status & 0x80) goto end; if(status & 0x40) { writeb(*addr++, dma); if(--len == 0) break; } } end: writeb(priv(host)->ctrl | 0x40, priv(host)->base + CTRL); return len; }

//***************************************************************************** // //! Set the random number generator seed. //! //! Seed the random number generator by running a MD4 hash on the entropy pool. //! Note that the entropy pool may change from beneath us, but for the purposes //! of generating random numbers that is not a concern. Also, the MD4 hash was //! broken long ago, but since it is being used to generate random numbers //! instead of providing security this is not a concern. //! //! \return New seed value. // //***************************************************************************** uint32_t RandomSeed(void) { uint32_t ui32A, ui32B, ui32C, ui32D, ui32Temp, ui32Idx; // // Initialize the digest. // ui32A = 0x67452301; ui32B = 0xefcdab89; ui32C = 0x98badcfe; ui32D = 0x10325476; // // Perform the first round of operations. // #define F(a, b, c, d, k, s) \ { \ ui32Temp = a + (d ^ (b & (c ^ d))) + g_pui32RandomEntropy[k]; \ a = (ui32Temp << s) | (ui32Temp >> (32 - s)); \ } for(ui32Idx = 0; ui32Idx < 16; ui32Idx += 4) { F(ui32A, ui32B, ui32C, ui32D, ui32Idx + 0, 3); F(ui32D, ui32A, ui32B, ui32C, ui32Idx + 1, 7); F(ui32C, ui32D, ui32A, ui32B, ui32Idx + 2, 11); F(ui32B, ui32C, ui32D, ui32A, ui32Idx + 3, 19); } // // Perform the second round of operations. // #define G(a, b, c, d, k, s) \ { \ ui32Temp = (a + ((b & c) | (b & d) | (c & d)) + \ g_pui32RandomEntropy[k] + 0x5a827999); \ a = (ui32Temp << s) | (ui32Temp >> (32 - s)); \ } for(ui32Idx = 0; ui32Idx < 4; ui32Idx++) { G(ui32A, ui32B, ui32C, ui32D, ui32Idx + 0, 3); G(ui32D, ui32A, ui32B, ui32C, ui32Idx + 4, 5); G(ui32C, ui32D, ui32A, ui32B, ui32Idx + 8, 9); G(ui32B, ui32C, ui32D, ui32A, ui32Idx + 12, 13); } // // Perform the third round of operations. // #define H(a, b, c, d, k, s) \ { \ ui32Temp = a + (b ^ c ^ d) + g_pui32RandomEntropy[k] + 0x6ed9eba1; \ a = (ui32Temp << s) | (ui32Temp >> (32 - s)); \ } for(ui32Idx = 0; ui32Idx < 4; ui32Idx += 2) { H(ui32A, ui32B, ui32C, ui32D, ui32Idx + 0, 3); H(ui32D, ui32A, ui32B, ui32C, ui32Idx + 8, 9); H(ui32C, ui32D, ui32A, ui32B, ui32Idx + 4, 11); H(ui32B, ui32C, ui32D, ui32A, ui32Idx + 12, 15); if(ui32Idx == 2) { ui32Idx -= 3; } } // // Use the first word of the resulting digest as the random number seed. // return(ui32A + 0x67452301); }

// -------------------------------------------------------------------------- // main(Number of arguments, Argument values) // Description : This is the entry point of the program. // Return value : SUCCESS:0 ERROR:-1 // -------------------------------------------------------------------------- int main(int argc, char **argv) { // AR.Drone class ARDrone ardrone; // Initialize if (!ardrone.open()) { printf("Failed to initialize.\n"); return -1; } // Battery printf("Battery = %d%%\n", ardrone.getBatteryPercentage()); // Map cv::Mat map = cv::Mat::zeros(500, 500, CV_8UC3); // Kalman filter cv::KalmanFilter kalman(6, 4, 0); // Sampling time [s] const double dt = 0.033; // Transition matrix (x, y, z, vx, vy, vz) cv::Mat1f F(6, 6); F << 1.0, 0.0, 0.0, dt, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, dt, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, dt, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1.0; kalman.transitionMatrix = F; // Measurement matrix (0, 0, z, vx, vy, vz) cv::Mat1f H(4, 6); H << 0, 0, 1, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 1; kalman.measurementMatrix = H; // Process noise covairance (x, y, z, vx, vy, vz) cv::Mat1f Q(6, 6); Q << 0.1, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.1, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.1, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.3, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.3, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.3; kalman.processNoiseCov = Q; // Measurement noise covariance (z, vx, vy, vz) cv::Mat1f R(4, 4); R << 0.1, 0.0, 0.00, 0.00, 0.0, 0.1, 0.00, 0.00, 0.0, 0.0, 0.05, 0.00, 0.0, 0.0, 0.00, 0.05; kalman.measurementNoiseCov = R; // Main loop while (1) { // Key input int key = cv::waitKey(33); if (key == 0x1b) break; // Update if (!ardrone.update()) break; // Get an image cv::Mat image = ardrone.getImage(); // Prediction cv::Mat prediction = kalman.predict(); // Altitude double altitude = ardrone.getAltitude(); // Orientations double roll = ardrone.getRoll(); double pitch = ardrone.getPitch(); double yaw = ardrone.getYaw(); // Velocities double vx, vy, vz; double velocity = ardrone.getVelocity(&vx, &vy, &vz); cv::Mat V = (cv::Mat1f(3,1) << vx, vy, vz); // Rotation matrices cv::Mat RZ = (cv::Mat1f(3,3) << cos(yaw), -sin(yaw), 0.0, sin(yaw), cos(yaw), 0.0, 0.0, 0.0, 1.0); cv::Mat RY = (cv::Mat1f(3,3) << cos(pitch), 0.0, sin(pitch), 0.0, 1.0, 0.0, -sin(pitch), 0.0, cos(pitch)); cv::Mat RX = (cv::Mat1f(3,3) << 1.0, 0.0, 0.0, 0.0, cos(roll), -sin(roll), 0.0, sin(roll), cos(roll)); // Time [s] static int64 last = cv::getTickCount(); double dt = (cv::getTickCount() - last) / cv::getTickFrequency(); last = cv::getTickCount(); // Local movements (z, vx, vy, vz) cv::Mat1f M = RZ * RY * RX * V * dt; cv::Mat measurement = (cv::Mat1f(4,1) << altitude, M(0,0), M(1,0), M(2,0)); // Correction cv::Mat1f estimated = kalman.correct(measurement); // Position (x, y, z) double pos[3] = {estimated(0,0), estimated(1,0), estimated(2,0)}; printf("x = %3.2fm, y = %3.2fm, z = %3.2fm\n", pos[0], pos[1], pos[2]); // Take off / Landing if (key == ' ') { if (ardrone.onGround()) ardrone.takeoff(); else ardrone.landing(); } // Move double x = 0.0, y = 0.0, z = 0.0, r = 0.0; if (key == 0x260000) x = 1.0; if (key == 0x280000) x = -1.0; if (key == 0x250000) r = 1.0; if (key == 0x270000) r = -1.0; if (key == 'q') z = 1.0; if (key == 'a') z = -1.0; ardrone.move3D(x, y, z, r); // Change camera static int mode = 0; if (key == 'c') ardrone.setCamera(++mode%4); // Display the image cv::circle(map, cv::Point(-pos[1]*100.0 + map.cols/2, -pos[0]*100.0 + map.rows/2), 2, CV_RGB(255,0,0)); cv::imshow("map", map); cv::imshow("camera", image); } // See you ardrone.close(); return 0; }

PetscErrorCode SNESQNApply_LBFGS(SNES snes,PetscInt it,Vec Y,Vec X,Vec Xold,Vec D,Vec Dold) { PetscErrorCode ierr; SNES_QN *qn = (SNES_QN*)snes->data; Vec W = snes->work[3]; Vec *dX = qn->U; Vec *dF = qn->V; PetscScalar *alpha = qn->alpha; PetscScalar *beta = qn->beta; PetscScalar *dXtdF = qn->dXtdF; PetscScalar *dFtdX = qn->dFtdX; PetscScalar *YtdX = qn->YtdX; /* ksp thing for Jacobian scaling */ PetscInt k,i,j,g,lits; PetscInt m = qn->m; PetscScalar t; PetscInt l = m; PetscFunctionBegin; if (it < m) l = it; ierr = VecCopy(D,Y);CHKERRQ(ierr); if (it > 0) { k = (it - 1) % l; ierr = VecCopy(D,dF[k]);CHKERRQ(ierr); ierr = VecAXPY(dF[k], -1.0, Dold);CHKERRQ(ierr); ierr = VecCopy(X, dX[k]);CHKERRQ(ierr); ierr = VecAXPY(dX[k], -1.0, Xold);CHKERRQ(ierr); if (qn->singlereduction) { ierr = VecMDotBegin(dF[k],l,dX,dXtdF);CHKERRQ(ierr); ierr = VecMDotBegin(dX[k],l,dF,dFtdX);CHKERRQ(ierr); ierr = VecMDotBegin(Y,l,dX,YtdX);CHKERRQ(ierr); ierr = VecMDotEnd(dF[k],l,dX,dXtdF);CHKERRQ(ierr); ierr = VecMDotEnd(dX[k],l,dF,dFtdX);CHKERRQ(ierr); ierr = VecMDotEnd(Y,l,dX,YtdX);CHKERRQ(ierr); for (j = 0; j < l; j++) { H(k, j) = dFtdX[j]; H(j, k) = dXtdF[j]; } /* copy back over to make the computation of alpha and beta easier */ for (j = 0; j < l; j++) dXtdF[j] = H(j, j); } else { ierr = VecDot(dX[k], dF[k], &dXtdF[k]);CHKERRQ(ierr); } if (qn->scale_type == SNES_QN_SCALE_LINESEARCH) { ierr = SNESLineSearchGetLambda(snes->linesearch,&qn->scaling);CHKERRQ(ierr); } } /* outward recursion starting at iteration k's update and working back */ for (i=0; i<l; i++) { k = (it-i-1)%l; if (qn->singlereduction) { /* construct t = dX[k] dot Y as Y_0 dot dX[k] + sum(-alpha[j]dX[k]dF[j]) */ t = YtdX[k]; for (j=0; j<i; j++) { g = (it-j-1)%l; t -= alpha[g]*H(k, g); } alpha[k] = t/H(k,k); } else { ierr = VecDot(dX[k],Y,&t);CHKERRQ(ierr); alpha[k] = t/dXtdF[k]; } if (qn->monitor) { ierr = PetscViewerASCIIAddTab(qn->monitor,((PetscObject)snes)->tablevel+2);CHKERRQ(ierr); ierr = PetscViewerASCIIPrintf(qn->monitor, "it: %D k: %D alpha: %14.12e\n", it, k, (double)PetscRealPart(alpha[k]));CHKERRQ(ierr); ierr = PetscViewerASCIISubtractTab(qn->monitor,((PetscObject)snes)->tablevel+2);CHKERRQ(ierr); } ierr = VecAXPY(Y,-alpha[k],dF[k]);CHKERRQ(ierr); } if (qn->scale_type == SNES_QN_SCALE_JACOBIAN) { ierr = KSPSolve(snes->ksp,Y,W);CHKERRQ(ierr); SNESCheckKSPSolve(snes); ierr = KSPGetIterationNumber(snes->ksp,&lits);CHKERRQ(ierr); snes->linear_its += lits; ierr = VecCopy(W, Y);CHKERRQ(ierr); } else { ierr = VecScale(Y, qn->scaling);CHKERRQ(ierr); } if (qn->singlereduction) { ierr = VecMDot(Y,l,dF,YtdX);CHKERRQ(ierr); } /* inward recursion starting at the first update and working forward */ for (i = 0; i < l; i++) { k = (it + i - l) % l; if (qn->singlereduction) { t = YtdX[k]; for (j = 0; j < i; j++) { g = (it + j - l) % l; t += (alpha[g] - beta[g])*H(g, k); } beta[k] = t / H(k, k); } else { ierr = VecDot(dF[k], Y, &t);CHKERRQ(ierr); beta[k] = t / dXtdF[k]; } ierr = VecAXPY(Y, (alpha[k] - beta[k]), dX[k]);CHKERRQ(ierr); if (qn->monitor) { ierr = PetscViewerASCIIAddTab(qn->monitor,((PetscObject)snes)->tablevel+2);CHKERRQ(ierr); ierr = PetscViewerASCIIPrintf(qn->monitor, "it: %D k: %D alpha - beta: %14.12e\n", it, k, (double)PetscRealPart(alpha[k] - beta[k]));CHKERRQ(ierr); ierr = PetscViewerASCIISubtractTab(qn->monitor,((PetscObject)snes)->tablevel+2);CHKERRQ(ierr); } } PetscFunctionReturn(0); }

int nmppsFFT8192Inv28888Ref_f(const nm32sc* src, nm32sc* dst) { vec<cmplx<double> > X(8192); vec<cmplx<double> > Y(8192); vec<cmplx<double> > G(8192); vec<cmplx<double> > N(8192); vec<cmplx<double> > H(8192); vec<cmplx<double> > J(8192); cmplx<double> z; cmplx<double> t; for(int i=0; i<8192; i++){ X[i].re=src[i].re; X[i].im=src[i].im; } // Вывод: //----------------- 0 ----------------- //for(int k=0; k<8192; k++){ // y[k]=expIFFT<8192>(0)*x[0]; // for(int n=1; n<8192; n++){ // y[k]+=expIFFT<8192>(n*k)*x[n]; // } //} //------------- 1 --------------- // n=8*n+i // for(int k=0; k<8192; k++) // for(int i=0; i<8; i++) // for(int n=0; n<8192/8; n++) // y[k]+=expIFFT<8192>(8*n*k)*expIFFT<8192>(k*i)*x[8*n+i]; //------------- 2 --------------- // n=8*n+j //for(int k=0; k<8192; k++) // for(int i=0; i<8; i++) // for(int j=0; j<8; j++) // for(int n=0; n<8192/8/8; n++) // y[k]+=expIFFT<8192>(8*(8*n+j)*k)*expIFFT<8192>(k*i)*x[8*(8*n+j)+i]; //------------- 3 --------------- // n=8*n+h //for(int k=0; k<8192; k++) // for(int i=0; i<8; i++) // for(int j=0; j<8; j++) // for(int h=0; h<8; h++) // for(int n=0; n<8192/8/8/8; n++) // y[k]+=expIFFT<8192>(8*(8*(8*n+h)+j)*k)*expIFFT<8192>(k*i)*x[8*(8*(8*n+h)+j)+i]; //------------- 4 --------------- // n=8*n+g //for(int k=0; k<8192; k++) // for(int i=0; i<8; i++) // for(int j=0; j<8; j++) // for(int h=0; h<8; h++) // for(int g=0; g<8; g++) // for(int n=0; n<8192/8/8/8/8; n++) // y[k]+=expIFFT<8192>(8*(8*(8*(8*n+g)+h)+j)*k)*expIFFT<8192>(k*i)*x[8*(8*(8*(8*n+g)+h)+j)+i]; //------------- 5 --------------- //for(int k=0; k<8192; k++) // for(int i=0; i<8; i++) // for(int j=0; j<8; j++) // for(int h=0; h<8; h++) // for(int g=0; g<8; g++) // for(int n=0; n<2; n++) // y[k]+= x[4096*n+512*g+64*h+8*j+i]* // expIFFT<8192>(4096*n*k)* // expIFFT<8192>(512*g*k)* // expIFFT<8192>(64*h*k)* // expIFFT<8192>(8*j*k)* // expIFFT<8192>(i*k); //------------- 6 --------------- //for(int k=0; k<8192; k++) // for(int i=0; i<8; i++) // for(int j=0; j<8; j++) // for(int h=0; h<8; h++) // for(int g=0; g<8; g++){ // cmplx<double> sumN; // for(int n=0; n<2; n++) // sumN+= x[4096*n+512*g+64*h+8*j+i]* // expIFFT<8192>(4096*n*k); // y[k]+=sumN* expIFFT<8192>(512*g*k)* // expIFFT<8192>(64*h*k)* // expIFFT<8192>(8*j*k)* // expIFFT<8192>(i*k); // } //------------- 7 --------------- //for(int k=0; k<8192; k++) // for(int i=0; i<8; i++) // for(int j=0; j<8; j++) // for(int h=0; h<8; h++){ // cmplx<double> sumG; // for(int g=0; g<8; g++){ // cmplx<double> sumN; // for(int n=0; n<2; n++) // sumN+= x[4096*n+512*g+64*h+8*j+i]* // expIFFT<8192>(4096*n*k); // sumG+= sumN*expIFFT<8192>(512*g*k); // } // y[k]+=sumG* expIFFT<8192>(64*h*k)* // expIFFT<8192>(8*j*k)* // expIFFT<8192>(i*k); // } //------------- 8 --------------- //for(int k=0; k<8192; k++) // for(int i=0; i<8; i++) // for(int j=0; j<8; j++){ // cmplx<double> sumH; // for(int h=0; h<8; h++){ // cmplx<double> sumG; // for(int g=0; g<8; g++){ // cmplx<double> sumN; // for(int n=0; n<2; n++) // sumN+= x[4096*n+512*g+64*h+8*j+i]* // expIFFT<8192>(4096*n*k); // sumG+= sumN*expIFFT<8192>(512*g*k); // } // sumH+=sumG* expIFFT<8192>(64*h*k); // } // y[k]+=sumH* expIFFT<8192>(8*j*k)* // expIFFT<8192>(i*k); // } //------------- 9 --------------- //for(int k=0; k<8192; k++){ // for(int i=0; i<8; i++){ // cmplx<double> sumJ; // for(int j=0; j<8; j++){ // cmplx<double> sumH; // for(int h=0; h<8; h++){ // cmplx<double> sumG; // for(int g=0; g<8; g++){ // cmplx<double> sumN; // for(int n=0; n<2; n++){ // sumN+= x[4096*n+512*g+64*h+8*j+i]* // expIFFT<8192>(4096*n*k); // } // sumG+= sumN*expIFFT<8192>(512*g*k); // } // sumH+=sumG* expIFFT<8192>(64*h*k); // } // sumJ+=sumH* expIFFT<8192>(8*j*k); // } // y[k]+=sumJ* expIFFT<8192>(i*k); // } //} //------------- 10 --------------- //for(int k=0; k<2; k++){ // for(int i=0; i<8; i++){ // for(int j=0; j<8; j++){ // for(int h=0; h<8; h++){ // for(int g=0; g<8; g++){ // cmplx<double> sumN; // for(int n=0; n<2; n++){ // sumN+= x[4096*n+512*g+64*h+8*j+i]* // expIFFT<8192>(4096*n*k); // } // N[4096*k+512*g+64*h+8*j+i]=sumN; // } // } // } // } //} //for(int k=0; k<8192; k++){ // for(int i=0; i<8; i++){ // cmplx<double> sumJ; // for(int j=0; j<8; j++){ // cmplx<double> sumH; // for(int h=0; h<8; h++){ // cmplx<double> sumG; // for(int g=0; g<8; g++){ // sumG+= N[4096*(k%2)+512*g+64*h+8*j+i]*expIFFT<8192>(512*g*k); // } // sumH+=sumG* expIFFT<8192>(64*h*k); // } // sumJ+=sumH* expIFFT<8192>(8*j*k); // } // y[k]+=sumJ* expIFFT<8192>(i*k); // } //} //------------- 11 --------------- //for(int k=0; k<2; k++){ // for(int i=0; i<8; i++){ // for(int j=0; j<8; j++){ // for(int h=0; h<8; h++){ // for(int g=0; g<8; g++){ // cmplx<double> sumN; // for(int n=0; n<2; n++){ // sumN+= x[4096*n+512*g+64*h+8*j+i]*expIFFT<8192>(4096*n*k); // } // N[4096*k+512*g+64*h+8*j+i]=sumN; // } // } // } // } //} //for(int k=0; k<16; k++){ // for(int i=0; i<8; i++){ // for(int j=0; j<8; j++){ // for(int h=0; h<8; h++){ // cmplx<double> sumG; // for(int g=0; g<8; g++){ // sumG+= N[4096*(k%2)+512*g+64*h+8*j+i]*expIFFT<8192>(512*g*k); // } // G[512*(k%16)+64*h+8*j+i]=sumG; // } // } // } //} // //for(int k=0; k<8192; k++){ // for(int i=0; i<8; i++){ // cmplx<double> sumJ; // for(int j=0; j<8; j++){ // cmplx<double> sumH; // for(int h=0; h<8; h++){ // sumH+=G[512*(k%16)+64*h+8*j+i]*expIFFT<8192>(64*h*k); // } // sumJ+=sumH* expIFFT<8192>(8*j*k); // } // y[k]+=sumJ* expIFFT<8192>(i*k); // } //} //------------- 12 --------------- //for(int k=0; k<2; k++){ // for(int i=0; i<8; i++){ // for(int j=0; j<8; j++){ // for(int h=0; h<8; h++){ // for(int g=0; g<8; g++){ // cmplx<double> sumN; // for(int n=0; n<2; n++){ // sumN+= x[4096*n+512*g+64*h+8*j+i]*expIFFT<8192>(4096*n*k); // } // N[4096*k+512*g+64*h+8*j+i]=sumN; // } // } // } // } //} //for(int k=0; k<16; k++){ // for(int i=0; i<8; i++){ // for(int j=0; j<8; j++){ // for(int h=0; h<8; h++){ // cmplx<double> sumG; // for(int g=0; g<8; g++){ // sumG+= N[4096*(k%2)+512*g+64*h+8*j+i]*expIFFT<8192>(512*g*k); // } // G[512*(k%16)+64*h+8*j+i]=sumG; // } // } // } //} // //for(int k=0; k<128; k++){ // for(int i=0; i<8; i++){ // for(int j=0; j<8; j++){ // cmplx<double> sumH; // for(int h=0; h<8; h++){ // sumH+=G[512*(k%16)+64*h+8*j+i]*expIFFT<8192>(64*h*k); // } // H[64*(k%128)+8*j+i]=sumH; // } // } //} // //for(int k=0; k<8192; k++){ // for(int i=0; i<8; i++){ // cmplx<double> sumJ; // for(int j=0; j<8; j++){ // sumJ+=H[64*(k%128)+8*j+i]* expIFFT<8192>(8*j*k); // } // y[k]+=sumJ* expIFFT<8192>(i*k); // } //} //------------- 13 --------------- //for(int k=0; k<2; k++){ // for(int i=0; i<8; i++){ // for(int j=0; j<8; j++){ // for(int h=0; h<8; h++){ // for(int g=0; g<8; g++){ // cmplx<double> sumN; // for(int n=0; n<2; n++){ // sumN+= x[4096*n+512*g+64*h+8*j+i]*expIFFT<8192>(4096*n*k); // } // N[4096*k+512*g+64*h+8*j+i]=sumN; // } // } // } // } //} //for(int k=0; k<16; k++){ // for(int i=0; i<8; i++){ // for(int j=0; j<8; j++){ // for(int h=0; h<8; h++){ // cmplx<double> sumG; // for(int g=0; g<8; g++){ // sumG+= N[4096*(k%2)+512*g+64*h+8*j+i]*expIFFT<8192>(512*g*k); // } // G[512*(k%16)+64*h+8*j+i]=sumG; // } // } // } //} // //for(int k=0; k<128; k++){ // for(int i=0; i<8; i++){ // for(int j=0; j<8; j++){ // cmplx<double> sumH; // for(int h=0; h<8; h++){ // sumH+=G[512*(k%16)+64*h+8*j+i]*expIFFT<8192>(64*h*k); // } // H[64*(k%128)+8*j+i]=sumH; // } // } //} // //for(int k=0; k<1024; k++){ // for(int i=0; i<8; i++){ // cmplx<double> sumJ; // for(int j=0; j<8; j++){ // sumJ+=H[64*(k%128)+8*j+i]* expIFFT<8192>(8*j*k); // } // J[8*(k%1024)+i]=sumJ; // } //} // //for(int k=0; k<8192; k++){ // for(int i=0; i<8; i++){ // y[k]+=J[8*(k%1024)+i]* expIFFT<8192>(i*k); // } //} //------------- 13 --------------- //for(int k=0; k<2; k++){ // for(int i=0; i<8; i++){ // for(int j=0; j<8; j++){ // for(int h=0; h<8; h++){ // for(int g=0; g<8; g++){ // for(int n=0; n<2; n++){ // N[4096*k+512*g+64*h+8*j+i]+= x[4096*n+512*g+64*h+8*j+i]*expIFFT<8192>(4096*n*k); // } // } // } // } // } //} //for(int k=0; k<16; k++){ // for(int i=0; i<8; i++){ // for(int j=0; j<8; j++){ // for(int h=0; h<8; h++){ // for(int g=0; g<8; g++){ // G[512*(k%16)+64*h+8*j+i]+= N[4096*(k%2)+512*g+64*h+8*j+i]*expIFFT<8192>(512*g*k); // } // } // } // } //} // //for(int k=0; k<128; k++){ // for(int i=0; i<8; i++){ // for(int j=0; j<8; j++){ // for(int h=0; h<8; h++){ // H[64*(k%128)+8*j+i]+=G[512*(k%16)+64*h+8*j+i]*expIFFT<8192>(64*h*k); // } // } // } //} // //for(int k=0; k<1024; k++){ // for(int i=0; i<8; i++){ // for(int j=0; j<8; j++){ // J[8*(k%1024)+i]+=H[64*(k%128)+8*j+i]* expIFFT<8192>(8*j*k); // } // } //} // //for(int k=0; k<8192; k++){ // for(int i=0; i<8; i++){ // y[k]+=J[8*(k%1024)+i]* expIFFT<8192>(i*k); // } //} //------------- 14 --------------- // kk=512*g+64*h+8*j+i //for(int k=0; k<2; k++){ // for(int kk=0; kk<4096; kk++){ // for(int n=0; n<2; n++){ // N[4096*k+kk]+= x[4096*n+kk]* expIFFT<8192>(4096*n*k); // } // } //} //for(int k=0; k<16; k++){ // for(int kk=0; kk<512; kk++){ // for(int g=0; g<8; g++){ // G[512*(k%16)+kk]+= N[4096*(k%2)+512*g+kk]* expIFFT<8192>(512*g*k); // } // } //} // //for(int k=0; k<128; k++){ // for(int kk=0; kk<64; kk++){ // for(int h=0; h<8; h++){ // H[64*(k%128)+kk]+=G[512*(k%16)+64*h+kk]* expIFFT<8192>(64*h*k); // } // } //} // //for(int k=0; k<1024; k++){ // for(int i=0; i<8; i++){ // for(int j=0; j<8; j++){ // J[8*(k%1024)+i]+=H[64*(k%128)+8*j+i]* expIFFT<8192>(8*j*k); // } // } //} // //for(int k=0; k<8192; k++){ // for(int i=0; i<8; i++){ // y[k]+=J[8*(k%1024)+i]* expIFFT<8192>(i*k); // } //} // //------------- 15 --------------- //for(int k=0; k<2; k++){ // for(int kk=0; kk<4096; kk++){ // N[4096*k+kk]+=X[kk]+X[4096+kk]*expIFFT<8192>(4096*k); // } //} // //for(int k=0; k<16; k++){ // for(int kk=0; kk<512; kk++){ // for(int g=0; g<8; g++){ // G[512*(k%16)+kk]+= N[4096*(k%2)+512*g+kk]* expIFFT<8192>(512*g*k); // } // } //} // //for(int k=0; k<128; k++){ // for(int kk=0; kk<64; kk++){ // for(int h=0; h<8; h++){ // H[64*(k%128)+kk]+=G[512*(k%16)+64*h+kk]* expIFFT<8192>(64*h*k); // } // } //} // //for(int k=0; k<1024; k++){ // for(int i=0; i<8; i++){ // for(int j=0; j<8; j++){ // J[8*(k%1024)+i]+=H[64*(k%128)+8*j+i]* expIFFT<8192>(8*j*k); // } // } //} // //for(int k=0; k<8192; k++){ // for(int i=0; i<8; i++){ // Y[k]+=J[8*(k%1024)+i]* expIFFT<8192>(i*k); // } //} //=========== Final FFT algorithm: ========================= // ---------------- 0.0 -------------------- for(int k=0; k<2; k++){ for(int kk=0; kk<4096; kk++){ N[4096*k+kk]+=X[kk]+X[4096+kk]*expIFFT<8192>(4096*k); } } // ---------------- 1.0 -------------------- for(int kk=0; kk<512; kk++){ for(int k2=0; k2<2; k2++){ for(int k1=0; k1<8; k1++){ // rep 8 int k=k1*2+k2; // for(int k=0; k<16; k++) for(int g=0; g<8; g++){ // vsum G[1024*k1+512*k2+kk]+= N[4096*k2+512*g+kk]* expIFFT<8192>(512*g*k); } } } } // ---------------- 2.0 -------------------- for(int kk=0; kk<64; kk++){ for(int k2=0; k2<16; k2++){ for(int k1=0; k1<8; k1++){ // rep 8 int k=k1*16+k2; // for(int k=0; k<128; k++){ for(int h=0; h<8; h++){ // vsum H[1024*k1+64*k2+kk]+=G[512*k2+64*h+kk]* expIFFT<8192>(64*h*k); } } } } // ---------------- 3.0 -------------------- for(int k2=0; k2<128; k2++){ for(int k1=0; k1<8; k1++){ for(int i=0; i<8; i++){ // rep 8 int k=k1*128+k2; // for(int k=0; k<1024; k++){ for(int j=0; j<8; j++){ // vsum J[1024*k1+8*k2+i]+=H[64*k2+8*j+i]*expIFFT<8192>(8*j*k); } } } } // ---------------- 4.0 -------------------- for(int k2=0; k2<1024; k2++){ for(int k1=0; k1<8; k1++){ // rep 8 int k=1024*k1+k2; // for(int k=0; k<8192; k++){ for(int i=0; i<8; i++){ // vsum Y[1024*k1+k2]+=J[8*k2+i]*expIFFT<8192>(i*k); } } } for(int i=0; i<8192; i++){ dst[i].re=floor(Y[i].re/8192+0.5); dst[i].im=floor(Y[i].im/8192+0.5); } return 0; }

int maximalRectangle(vector<vector<char> > &matrix) { // Start typing your C/C++ solution below // DO NOT write int main() function if(matrix.empty()) return 0; int m = matrix.size(); int n = matrix[0].size(); vector< vector<area> > D(m+1, vector<area>(n+1)); // maximum area with matrix[i][j] as the top left element vector< vector<int> > W(m, vector<int> (n, 0)); // maximum single line area starting with matrix[i][j] vector< vector<int> > H(m, vector<int> (n, 0)); //maximum single column area starting with matrix[i][j] int q; int i, j; int gmax = 0; for(i = 0; i < m; i++){ q = 0; for(j = n-1; j >= 0; j--){ if(matrix[i][j] == '0'){ W[i][j] = 0; } else{ W[i][j] = 1 + q; } q = W[i][j]; } } for(j = 0; j < n; j++){ q = 0; for(i = m-1; i >= 0; i--){ if(matrix[i][j] == '0'){ H[i][j] = 0; } else{ H[i][j] = 1 + q; } q = H[i][j]; } } for(i = 0; i <= m; i++) D[i][n].w = D[i][n].h = 0; for(j = 0; j <= n; j++) D[m][j].w = D[m][j].h = 0; int cur; for(i = m-1; i >= 0; i--){ for(j = n-1; j >= 0; j--){ //compute D if(matrix[i][j] == '0') D[i][j].w = D[i][j].h = 0; else{ D[i][j].w = W[i][j]; D[i][j].h = 1; cur = H[i][j]; if(W[i][j] < H[i][j]){ D[i][j].h = H[i][j]; D[i][j].w = 1; } cur = min(W[i][j], D[i+1][j].w) * (1 + D[i+1][j].h); if(cur > D[i][j].w * D[i][j].h){ D[i][j].w = min(W[i][j], D[i+1][j].w); D[i][j].h = 1 + D[i+1][j].h; } cur = min(H[i][j], D[i][j+1].h) * (1 + D[i][j+1].w); if(cur > D[i][j].w * D[i][j].h){ D[i][j].w = 1 + D[i][j+1].w; D[i][j].h = min(H[i][j], D[i][j+1].h); } } gmax = max(gmax, D[i][j].w * D[i][j].h); } } return gmax; }

void softAbsMetric::checkEvolution(const double epsilon) { baseHamiltonian::checkEvolution(epsilon); // Hessian std::cout.precision(6); int width = 12; int nColumn = 5; std::cout << "Potential Hessian (d^{2}V/dq^{i}dq^{j}):" << std::endl; std::cout << " " << std::setw(nColumn * width) << std::setfill('-') << "" << std::setfill(' ') << std::endl; std::cout << " " << std::setw(width) << std::left << "Row" << std::setw(width) << std::left << "Column" << std::setw(width) << std::left << "Analytic" << std::setw(width) << std::left << "Finite" << std::setw(width) << std::left << "Delta / " << std::endl; std::cout << " " << std::setw(width) << std::left << "(i)" << std::setw(width) << std::left << "(j)" << std::setw(width) << std::left << "Derivative" << std::setw(width) << std::left << "Difference" << std::setw(width) << std::left << "Stepsize^{2}" << std::endl; std::cout << " " << std::setw(nColumn * width) << std::setfill('-') << "" << std::setfill(' ') << std::endl; fComputeH(); for(int i = 0; i < mDim; ++i) { VectorXd temp = VectorXd::Zero(mDim); mQ(i) += epsilon; temp += gradV(); mQ(i) -= 2.0 * epsilon; temp -= gradV(); mQ(i) += epsilon; temp /= 2.0 * epsilon; for(int j = 0; j < mDim; ++j) { std::cout << " " << std::setw(width) << std::left << i << std::setw(width) << std::left << j << std::setw(width) << std::left << mH(i, j) << std::setw(width) << std::left << temp(j) << std::setw(width) << std::left << (mH(i, j) - temp(j)) / (epsilon * epsilon) << std::endl; } } std::cout << " " << std::setw(nColumn * width) << std::setfill('-') << "" << std::setfill(' ') << std::endl; std::cout << std::endl; // Gradient of the Hessian std::cout.precision(6); width = 12; nColumn = 6; std::cout << "Gradient of the Hessian (d^{3}V/dq^{i}dq^{j}dq^{k}):" << std::endl; std::cout << " " << std::setw(nColumn * width) << std::setfill('-') << "" << std::setfill(' ') << std::endl; std::cout << " " << std::setw(width) << std::left << "Component" << std::setw(width) << std::left << "Row" << std::setw(width) << std::left << "Column" << std::setw(width) << std::left << "Analytic" << std::setw(width) << std::left << "Finite" << std::setw(width) << std::left << "Delta /" << std::endl; std::cout << " " << std::setw(width) << std::left << "(i)" << std::setw(width) << std::left << "(j)" << std::setw(width) << std::left << "(k)" << std::setw(width) << std::left << "Derivative" << std::setw(width) << std::left << "Difference" << std::setw(width) << std::left << "Stepsize^{2}" << std::endl; std::cout << " " << std::setw(nColumn * width) << std::setfill('-') << "" << std::setfill(' ') << std::endl; for(int k = 0; k < mDim; ++k) { mAuxMatrixOne.setZero(); mQ(k) += epsilon; fComputeH(); mAuxMatrixOne += mH; mQ(k) -= 2.0 * epsilon; fComputeH(); mAuxMatrixOne -= mH; mQ(k) += epsilon; mAuxMatrixOne /= 2.0 * epsilon; fComputeGradH(k); for(int i = 0; i < mDim; ++i) { for(int j = 0; j < mDim; ++j) { std::cout << " " << std::setw(width) << std::left << k << std::setw(width) << std::left << i << std::setw(width) << std::left << j << std::setw(width) << std::left << mGradH.block(0, k * mDim, mDim, mDim)(i, j) << std::setw(width) << std::left << mAuxMatrixOne(i, j) << std::setw(width) << std::left << (mGradH.block(0, k * mDim, mDim, mDim)(i, j) - mAuxMatrixOne(i, j)) / (epsilon * epsilon) << std::endl; } } } std::cout << " " << std::setw(nColumn * width) << std::setfill('-') << "" << std::setfill(' ') << std::endl; std::cout << std::endl; // Metric std::cout.precision(6); width = 12; nColumn = 6; std::cout << "Gradient of the metric (dLambda^{jk}/dq^{i}):" << std::endl; std::cout << " " << std::setw(nColumn * width) << std::setfill('-') << "" << std::setfill(' ') << std::endl; std::cout << " " << std::setw(width) << std::left << "Component" << std::setw(width) << std::left << "Row" << std::setw(width) << std::left << "Column" << std::setw(width) << std::left << "Analytic" << std::setw(width) << std::left << "Finite" << std::setw(width) << std::left << "Delta /" << std::endl; std::cout << " " << std::setw(width) << std::left << "(i)" << std::setw(width) << std::left << "(j)" << std::setw(width) << std::left << "(k)" << std::setw(width) << std::left << "Derivative" << std::setw(width) << std::left << "Difference" << std::setw(width) << std::left << "Epsilon^{2}" << std::endl; std::cout << " " << std::setw(nColumn * width) << std::setfill('-') << "" << std::setfill(' ') << std::endl; fComputeMetric(); fPrepareSpatialGradients(); for(int k = 0; k < mDim; ++k) { // Approximate metric gradient MatrixXd temp = MatrixXd::Zero(mDim, mDim); MatrixXd G = MatrixXd::Zero(mDim, mDim); mQ(k) += epsilon; fComputeMetric(); G.noalias() = mEigenDeco.eigenvectors() * mSoftAbsLambda.asDiagonal() * mEigenDeco.eigenvectors().transpose(); temp += G; mQ(k) -= 2.0 * epsilon; fComputeMetric(); G.noalias() = mEigenDeco.eigenvectors() * mSoftAbsLambda.asDiagonal() * mEigenDeco.eigenvectors().transpose(); temp -= G; mQ(k) += epsilon; temp /= 2.0 * epsilon; // Exact metric gradient fComputeMetric(); fComputeGradH(k); mAuxMatrixOne.noalias() = mGradH.block(0, k * mDim, mDim, mDim) * mEigenDeco.eigenvectors(); mAuxMatrixTwo.noalias() = mEigenDeco.eigenvectors().transpose() * mAuxMatrixOne; mAuxMatrixOne.noalias() = mPseudoJ.cwiseProduct(mAuxMatrixTwo); mCacheMatrix.noalias() = mAuxMatrixOne * mEigenDeco.eigenvectors().transpose(); MatrixXd gradG = mEigenDeco.eigenvectors() * mCacheMatrix; // Compare for(int i = 0; i < mDim; ++i) { for(int j = 0; j < mDim; ++j) { std::cout << " " << std::setw(width) << std::left << k << std::setw(width) << std::left << i << std::setw(width) << std::left << j << std::setw(width) << std::left << gradG(i, j) << std::setw(width) << std::left << temp(i, j) << std::setw(width) << std::left << (gradG(i, j) - temp(i, j)) / (epsilon * epsilon) << std::endl; } } } std::cout << " " << std::setw(nColumn * width) << std::setfill('-') << "" << std::setfill(' ') << std::endl; std::cout << std::endl; // Hamiltonian VectorXd gradH = dTaudq(); gradH += dPhidq(); std::cout << "pDot (-dH/dq^{i}):" << std::endl; std::cout << " " << std::setw(nColumn * width) << std::setfill('-') << "" << std::setfill(' ') << std::endl; std::cout << " " << std::setw(width) << std::left << "Component" << std::setw(width) << std::left << "Analytic" << std::setw(width) << std::left << "Finite" << std::setw(width) << std::left << "Delta /" << std::endl; std::cout << " " << std::setw(width) << std::left << "(i)" << std::setw(width) << std::left << "Derivative" << std::setw(width) << std::left << "Difference" << std::setw(width) << std::left << "Epsilon^{2}" << std::endl; std::cout << " " << std::setw(nColumn * width) << std::setfill('-') << "" << std::setfill(' ') << std::endl; for(int i = 0; i < mDim; ++i) { // Finite Differences double temp = 0.0; mQ(i) += epsilon; temp -= H(); mQ(i) -= 2.0 * epsilon; temp += H(); mQ(i) += epsilon; temp /= 2.0 * epsilon; // Exact double minusGradH = -gradH(i); std::cout << " " << std::setw(width) << std::left << i << std::setw(width) << std::left << minusGradH << std::setw(width) << std::left << temp << std::setw(width) << std::left << (minusGradH - temp) / (epsilon * epsilon) << std::endl; } std::cout << " " << std::setw(nColumn * width) << std::setfill('-') << "" << std::setfill(' ') << std::endl; std::cout << std::endl; }

bool SolverSLAM2DLinear::solveOrientation() { assert(_optimizer->indexMapping().size() + 1 == _optimizer->vertices().size() && "Needs to operate on full graph"); assert(_optimizer->vertex(0)->fixed() && "Graph is not fixed by vertex 0"); VectorXD b, x; // will be used for theta and x/y update b.setZero(_optimizer->indexMapping().size()); x.setZero(_optimizer->indexMapping().size()); typedef Eigen::Matrix<double, 1, 1, Eigen::ColMajor> ScalarMatrix; ScopedArray<int> blockIndeces(new int[_optimizer->indexMapping().size()]); for (size_t i = 0; i < _optimizer->indexMapping().size(); ++i) blockIndeces[i] = i+1; SparseBlockMatrix<ScalarMatrix> H(blockIndeces.get(), blockIndeces.get(), _optimizer->indexMapping().size(), _optimizer->indexMapping().size()); // building the structure, diagonal for each active vertex for (size_t i = 0; i < _optimizer->indexMapping().size(); ++i) { OptimizableGraph::Vertex* v = _optimizer->indexMapping()[i]; int poseIdx = v->hessianIndex(); ScalarMatrix* m = H.block(poseIdx, poseIdx, true); m->setZero(); } HyperGraph::VertexSet fixedSet; // off diagonal for each edge for (SparseOptimizer::EdgeContainer::const_iterator it = _optimizer->activeEdges().begin(); it != _optimizer->activeEdges().end(); ++it) { # ifndef NDEBUG EdgeSE2* e = dynamic_cast<EdgeSE2*>(*it); assert(e && "Active edges contain non-odometry edge"); // # else EdgeSE2* e = static_cast<EdgeSE2*>(*it); # endif OptimizableGraph::Vertex* from = static_cast<OptimizableGraph::Vertex*>(e->vertices()[0]); OptimizableGraph::Vertex* to = static_cast<OptimizableGraph::Vertex*>(e->vertices()[1]); int ind1 = from->hessianIndex(); int ind2 = to->hessianIndex(); if (ind1 == -1 || ind2 == -1) { if (ind1 == -1) fixedSet.insert(from); // collect the fixed vertices if (ind2 == -1) fixedSet.insert(to); continue; } bool transposedBlock = ind1 > ind2; if (transposedBlock){ // make sure, we allocate the upper triangle block std::swap(ind1, ind2); } ScalarMatrix* m = H.block(ind1, ind2, true); m->setZero(); } // walk along the Minimal Spanning Tree to compute the guess for the robot orientation assert(fixedSet.size() == 1); VertexSE2* root = static_cast<VertexSE2*>(*fixedSet.begin()); VectorXD thetaGuess; thetaGuess.setZero(_optimizer->indexMapping().size()); UniformCostFunction uniformCost; HyperDijkstra hyperDijkstra(_optimizer); hyperDijkstra.shortestPaths(root, &uniformCost); HyperDijkstra::computeTree(hyperDijkstra.adjacencyMap()); ThetaTreeAction thetaTreeAction(thetaGuess.data()); HyperDijkstra::visitAdjacencyMap(hyperDijkstra.adjacencyMap(), &thetaTreeAction); // construct for the orientation for (SparseOptimizer::EdgeContainer::const_iterator it = _optimizer->activeEdges().begin(); it != _optimizer->activeEdges().end(); ++it) { EdgeSE2* e = static_cast<EdgeSE2*>(*it); VertexSE2* from = static_cast<VertexSE2*>(e->vertices()[0]); VertexSE2* to = static_cast<VertexSE2*>(e->vertices()[1]); double omega = e->information()(2,2); double fromThetaGuess = from->hessianIndex() < 0 ? 0. : thetaGuess[from->hessianIndex()]; double toThetaGuess = to->hessianIndex() < 0 ? 0. : thetaGuess[to->hessianIndex()]; double error = normalize_theta(-e->measurement().rotation().angle() + toThetaGuess - fromThetaGuess); bool fromNotFixed = !(from->fixed()); bool toNotFixed = !(to->fixed()); if (fromNotFixed || toNotFixed) { double omega_r = - omega * error; if (fromNotFixed) { b(from->hessianIndex()) -= omega_r; (*H.block(from->hessianIndex(), from->hessianIndex()))(0,0) += omega; if (toNotFixed) { if (from->hessianIndex() > to->hessianIndex()) (*H.block(to->hessianIndex(), from->hessianIndex()))(0,0) -= omega; else (*H.block(from->hessianIndex(), to->hessianIndex()))(0,0) -= omega; } } if (toNotFixed ) { b(to->hessianIndex()) += omega_r; (*H.block(to->hessianIndex(), to->hessianIndex()))(0,0) += omega; } } } // solve orientation typedef LinearSolverCSparse<ScalarMatrix> SystemSolver; SystemSolver linearSystemSolver; linearSystemSolver.init(); bool ok = linearSystemSolver.solve(H, x.data(), b.data()); if (!ok) { cerr << __PRETTY_FUNCTION__ << "Failure while solving linear system" << endl; return false; } // update the orientation of the 2D poses and set translation to 0, GN shall solve that root->setToOrigin(); for (size_t i = 0; i < _optimizer->indexMapping().size(); ++i) { VertexSE2* v = static_cast<VertexSE2*>(_optimizer->indexMapping()[i]); int poseIdx = v->hessianIndex(); SE2 poseUpdate(0, 0, normalize_theta(thetaGuess(poseIdx) + x(poseIdx))); v->setEstimate(poseUpdate); } return true; }