Ejemplo n.º 1
0
}

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);
}
Ejemplo n.º 3
0
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;
}
Ejemplo n.º 4
0
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;
}
Ejemplo n.º 6
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;
}
Ejemplo n.º 7
0
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>();
	}
}
Ejemplo n.º 8
0
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;
  
}
Ejemplo n.º 9
0
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 );
}
Ejemplo n.º 10
0
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;
}
Ejemplo n.º 11
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();
  }
Ejemplo n.º 12
0
/* 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
Ejemplo n.º 13
0
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);
}
Ejemplo n.º 14
0
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;
}
Ejemplo n.º 15
0
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];
        }
    }
}
Ejemplo n.º 16
0
/*! 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;
}
Ejemplo n.º 17
0
/* 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;
}
Ejemplo n.º 18
0
void
foo (int p1, int *, int)
{
  if (p1 == 0)
    throw H ();
}
Ejemplo n.º 19
0
//*****************************************************************************************
//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;
}
Ejemplo n.º 20
0
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;
}
Ejemplo n.º 21
0
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;
}
Ejemplo n.º 22
0
    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);
    }
Ejemplo n.º 23
0
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;
}
Ejemplo n.º 24
0
//*****************************************************************************
//
//! 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;
}
Ejemplo n.º 26
0
Archivo: qn.c Proyecto: ziolai/petsc
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);
}
Ejemplo n.º 27
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;
}
Ejemplo n.º 28
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;
    }
Ejemplo n.º 29
0
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;
    
}
Ejemplo n.º 30
0
  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;
  }