}
template<typename Real>
Complex<Real> WilkinsonShift( const Matrix<Complex<Real>>& H )
{
DEBUG_CSE
typedef Complex<Real> F;
const Int n = H.Height();
DEBUG_ONLY(
if( n < 2 )
LogicError("WilkinsonShift requires n >= 2");
)
const Int offset = n-2;
const Real zero = Real(0);
const F& eta00 = H(offset,offset);
const F& eta01 = H(offset,offset+1);
const F& eta10 = H(offset+1,offset);
const F& eta11 = H(offset+1,offset+1);
// NOTE:
// eta10 should be real, but it is possibly negative, and so we will
// interpret it as a complex number so that the square-root is well-defined
DEBUG_ONLY(
if( ImagPart(eta10) != zero )
LogicError("Subdiagonal assumed real");
)
F shift = eta11;
const F gamma = Sqrt(eta01)*Sqrt(eta10);
Real sigma = OneAbs(gamma);
if( sigma != zero )
ConstIndexArray
EndCapBSplineBasisPatchFactory::GetPatchPoints(
Vtr::internal::Level const * level, Index faceIndex,
PatchTableFactory::PatchFaceTag const * /*levelPatchTags*/,
int levelVertOffset) {
// XXX: For now, always create new 16 indices for each patch.
// we'll optimize later to share all regular control points with
// other patches as well as to try to make extra ordinary verts watertight.
int offset = _refiner->GetNumVerticesTotal();
for (int i = 0; i < 16; ++i) {
_patchPoints.push_back(_numVertices + offset);
++_numVertices;
}
// XXX: temporary hack. we should traverse topology and find existing
// vertices if available
//
// Reorder gregory basis stencils into regular bezier
GregoryBasis::ProtoBasis basis(*level, faceIndex, levelVertOffset, -1);
std::vector<GregoryBasis::Point> bezierCP;
bezierCP.reserve(16);
bezierCP.push_back(basis.P[0]);
bezierCP.push_back(basis.Ep[0]);
bezierCP.push_back(basis.Em[1]);
bezierCP.push_back(basis.P[1]);
bezierCP.push_back(basis.Em[0]);
bezierCP.push_back(basis.Fp[0]); // arbitrary
bezierCP.push_back(basis.Fp[1]); // arbitrary
bezierCP.push_back(basis.Ep[1]);
bezierCP.push_back(basis.Ep[3]);
bezierCP.push_back(basis.Fp[3]); // arbitrary
bezierCP.push_back(basis.Fp[2]); // arbitrary
bezierCP.push_back(basis.Em[2]);
bezierCP.push_back(basis.P[3]);
bezierCP.push_back(basis.Em[3]);
bezierCP.push_back(basis.Ep[2]);
bezierCP.push_back(basis.P[2]);
// Apply basis conversion from bezier to b-spline
float Q[4][4] = {{ 6, -7, 2, 0},
{ 0, 2, -1, 0},
{ 0, -1, 2, 0},
{ 0, 2, -7, 6} };
std::vector<GregoryBasis::Point> H(16);
for (int i = 0; i < 4; ++i) {
for (int j = 0; j < 4; ++j) {
for (int k = 0; k < 4; ++k) {
if (Q[i][k] != 0) H[i*4+j] += bezierCP[j+k*4] * Q[i][k];
}
}
}
for (int i = 0; i < 4; ++i) {
for (int j = 0; j < 4; ++j) {
GregoryBasis::Point p;
for (int k = 0; k < 4; ++k) {
if (Q[j][k] != 0) p += H[i*4+k] * Q[j][k];
}
_vertexStencils.push_back(p);
}
}
int varyingIndices[] = { 0, 0, 1, 1,
0, 0, 1, 1,
3, 3, 2, 2,
3, 3, 2, 2,};
for (int i = 0; i < 16; ++i) {
_varyingStencils.push_back(basis.V[varyingIndices[i]]);
}
++_numPatches;
return ConstIndexArray(&_patchPoints[(_numPatches-1)*16], 16);
}
int InterPolynomial::checkIfValidityIsInBound(Vector ddv, unsigned k, double bound, double rho)
// input: k,bound,rho
// output: j,ddv
{
// check validity around x_k
// bound is epsilon in the paper
// return index of the worst point of J
// if (j==-1) then everything OK : next : trust region step
// else model step: replace x_j by x_k+d where d
// is calculated with LAGMAX
unsigned i,N=nPtsUsed, n=dim();
int j;
Polynomial *pp=NewtonBasis;
Vector *xx=NewtonPoints, xk=xx[k],vd;
Vector Distance(N);
double *dist=Distance, *dd=dist, distMax, vmax, tmp;
Matrix H(n,n);
Vector GXk(n); //,D(n);
for (i=0; i<N; i++) *(dd++)=xk.euclidianDistance(xx[i]);
while (true)
{
dd=dist; j=-1; distMax=2*rho;
for (i=0; i<N; i++)
{
if (*dd>distMax) { j=i; distMax=*dd; };
dd++;
}
if (j<0) return -1;
// to prevent to choose the same point once again:
dist[j]=0;
pp[j].gradientHessian(xk,GXk,H);
// d=H.multiply(xk);
// d.add(G);
tmp=M*distMax*distMax*distMax;
if (tmp*rho*(GXk.euclidianNorm()+0.5*rho*H.frobeniusNorm())>=bound)
{
/* vd=L2NormMinimizer(pp[j], xk, rho);
vd+=xk;
vmax=condorAbs(pp[j](vd));
Vector vd2=L2NormMinimizer(pp[j], xk, rho);
vd2+=xk;
double vmax2=condorAbs(pp[j](vd));
if (vmax<vmax2) { vmax=vmax2; vd=vd2; }
*/
vd=LAGMAXModified(GXk,H,rho,vmax);
// tmp=vd.euclidianNorm();
vd+=xk;
vmax=condorAbs(pp[j](vd));
if (tmp*vmax>=bound) break;
}
}
if (j>=0) ddv.copyFrom(vd);
return j;
}
int alg1(int *dK, int *dX, int *dY, Cipher *ciph, int m)
{
int dH[32];
int dJ[32];
int dHx, dHmax;
int imax;
for (int i = 0; i < 32; i++)
dJ[i] = 0;
dHx = H(dK, dX, dY, ciph, m);
//cout << "H* = " << dHx << endl;
STEP2:
for(int i = 0; i < 32; i++)
{
if(dK[i] == 2)
{
int dH0, dH1;
dK[i] = 0;
dH0 = H(dK, dX, dY, ciph, m);
dK[i] = 1;
dH1 = H(dK, dX, dY, ciph, m);
if((dH0 < dHx)&&(dHx < dH1))
{
dH[i] = dH1;
dJ[i] = 1;
}
else if((dH1 < dHx)&&(dHx < dH0))
{
dH[i] = dH0;
dJ[i] = 0;
}
else
{
dH[i] = dHx;
}
dK[i] = 2;
}
else if(dK[i] == 0)
{
int dH05, dH1;
dK[i] = 2;
dH05 = H(dK, dX, dY, ciph, m);
dK[i] = 1;
dH1 = H(dK, dX, dY, ciph, m);
if((dH05 < dHx)&&(dHx < dH1))
{
dH[i] = dH1;
dJ[i] = 1;
}
else if((dH1 < dHx)&&(dHx < dH05))
{
dH[i] = dH05;
dJ[i] = 2;
}
else
{
dH[i] = dHx;
}
dK[i] = 0;
}
else if(dK[i] == 1)
{
int dH0, dH05;
dK[i] = 0;
dH0 = H(dK, dX, dY, ciph, m);
dK[i] = 2;
dH05 = H(dK, dX, dY, ciph, m);
if((dH0 < dHx)&&(dHx < dH05))
{
dH[i] = dH05;
dJ[i] = 2;
}
else if((dH05 < dHx)&&(dHx < dH0))
{
dH[i] = dH0;
dJ[i] = 0;
}
else
{
dH[i] = dHx;
}
dK[i] = 1;
}
}
dHmax = dH[0];
imax = 0;
for(int i = 0; i < 32; i++)
{
if(dH[i] > dHmax) { dHmax = dH[i]; imax = i; }
}
if(dHmax > dHx)
{
dK[imax] = dJ[imax];
dHx = dHmax;
goto STEP2;
}
return dHmax;
}
int main (int argc, char * argv[]) {
// SET UP ARGUMENTS
po::options_description desc("Options");
desc.add_options()
("input", po::value<std::string>()->required(), "Input hypergraph file")
("output", po::value<std::string>()->default_value("out.dat"), "Output transversals file")
("verbosity,v", po::value<int>()->default_value(0)->implicit_value(1), "Write verbose debugging output (-v2 for trace output)")
("algorithm,a", po::value<std::string>()->default_value("pmmcs"), "Algorithm to use (pmmcs, prs, fka, berge, bm)")
("num-threads,t", po::value<int>()->default_value(1), "Number of threads to run in parallel")
("cutoff-size,c", po::value<int>()->default_value(0), "Maximum size set to return (0: no limit)");
po::positional_options_description p;
p.add("input", 1);
p.add("output", 1);
po::variables_map vm;
po::store(po::command_line_parser(argc, argv).options(desc).positional(p).run(), vm);
if (vm.count("help") or argc == 1) {
std::cout << desc << std::endl;
return 1;
};
const size_t num_threads = (vm["num-threads"].as<int>());
const size_t cutoff_size = (vm["cutoff-size"].as<int>());
po::notify(vm);
// Process input file
BOOST_LOG_TRIVIAL(debug) << "Loading hypergraph from file.";
std::string input_file(vm["input"].as<std::string>());
agdmhs::Hypergraph H (input_file);
BOOST_LOG_TRIVIAL(debug) << "Loading complete.";
// Process logging-related options
int verbosity = vm["verbosity"].as<int>();
switch (verbosity) {
case 1:
boost::log::core::get()->set_filter
(boost::log::trivial::severity >= boost::log::trivial::debug);
break;
case 2:
boost::log::core::get()->set_filter
(boost::log::trivial::severity >= boost::log::trivial::trace);
break;
default:
boost::log::core::get()->set_filter
(boost::log::trivial::severity >= boost::log::trivial::warning);
break;
}
// Print input information
std::cout << "Input has " << H.num_verts() << " vertices and " << H.num_edges() << " edges." << std::endl;
// Run chosen algorithm
std::string algname = vm["algorithm"].as<std::string>();
std::unique_ptr<agdmhs::MHSAlgorithm> mhs_algorithm;
if (algname == "berge") {
mhs_algorithm = std::unique_ptr<agdmhs::MHSAlgorithm> (new agdmhs::BergeAlgorithm(cutoff_size));
} else if (algname == "bm") {
mhs_algorithm = std::unique_ptr<agdmhs::MHSAlgorithm> (new agdmhs::ParBMAlgorithm (num_threads));
} else if (algname == "fka") {
mhs_algorithm = std::unique_ptr<agdmhs::MHSAlgorithm> (new agdmhs::FKAlgorithmA());
} else if (algname == "mmcs" or algname == "pmmcs") {
mhs_algorithm = std::unique_ptr<agdmhs::MHSAlgorithm> (new agdmhs::MMCSAlgorithm(num_threads, cutoff_size));
} else if (algname == "rs" or algname == "prs") {
mhs_algorithm = std::unique_ptr<agdmhs::MHSAlgorithm> (new agdmhs::RSAlgorithm(num_threads, cutoff_size));
} else {
std::stringstream error_message;
error_message << "Did not recognize requested algorithm " << algname << ".";
throw po::invalid_option_value(error_message.str());
}
BOOST_LOG_TRIVIAL(debug) << "Running algorithm " << algname;
agdmhs::Hypergraph Htrans = mhs_algorithm->transversal(H);
std::cout << "Found " << Htrans.num_edges() << " hitting sets." << std::endl;
BOOST_LOG_TRIVIAL(debug) << "Algorithm complete.";
// Print results
BOOST_LOG_TRIVIAL(debug) << "Writing result file.";
std::string output_file(vm["output"].as<std::string>());
Htrans.write_to_file(output_file);
BOOST_LOG_TRIVIAL(debug) << "Writing complete.";
return 0;
}
static inline int NCR5380_pwrite(struct Scsi_Host *instance, unsigned char *addr,
int len)
{
int *ctrl = &((struct NCR5380_hostdata *)instance->hostdata)->ctrl;
int oldctrl = *ctrl;
unsigned long *laddr;
#ifdef NOT_EFFICIENT
int iobase = instance->io_port;
int dma_io = iobase & ~(0x3C0000>>2);
#else
volatile unsigned char *iobase = (unsigned char *)ioaddr(instance->io_port);
volatile unsigned char *dma_io = (unsigned char *)((int)iobase & ~0x3C0000);
#endif
if(!len) return 0;
CTRL(iobase, 0x02);
laddr = (unsigned long *)addr;
while(len >= 32)
{
int status;
unsigned long v;
status = STAT(iobase);
if(status & 0x80)
goto end;
if(!(status & 0x40))
continue;
v=*laddr++; OUT2(L(v),dma_io); OUT2(H(v),dma_io);
v=*laddr++; OUT2(L(v),dma_io); OUT2(H(v),dma_io);
v=*laddr++; OUT2(L(v),dma_io); OUT2(H(v),dma_io);
v=*laddr++; OUT2(L(v),dma_io); OUT2(H(v),dma_io);
v=*laddr++; OUT2(L(v),dma_io); OUT2(H(v),dma_io);
v=*laddr++; OUT2(L(v),dma_io); OUT2(H(v),dma_io);
v=*laddr++; OUT2(L(v),dma_io); OUT2(H(v),dma_io);
v=*laddr++; OUT2(L(v),dma_io); OUT2(H(v),dma_io);
len -= 32;
if(len == 0)
break;
}
addr = (unsigned char *)laddr;
CTRL(iobase, 0x12);
while(len > 0)
{
int status;
status = STAT(iobase);
if(status & 0x80)
goto end;
if(status & 0x40)
{
OUT(*addr++, dma_io);
if(--len == 0)
break;
}
status = STAT(iobase);
if(status & 0x80)
goto end;
if(status & 0x40)
{
OUT(*addr++, dma_io);
if(--len == 0)
break;
}
}
end:
CTRL(iobase, oldctrl|0x40);
return len;
}
std::vector<INTPOINT> TERRAIN::GetPath(INTPOINT start, INTPOINT goal)
{
try
{
//Check that the two points are within the bounds of the map
MAPTILE *startTile = GetTile(start);
MAPTILE *goalTile = GetTile(goal);
if(!Within(start) || !Within(goal) || start == goal || startTile == NULL || goalTile == NULL)
return std::vector<INTPOINT>();
//Check if a path exists
if(!startTile->m_walkable || !goalTile->m_walkable || startTile->m_set != goalTile->m_set)
return std::vector<INTPOINT>();
//Init Search
long numTiles = m_size.x * m_size.y;
for(long l=0;l<numTiles;l++)
{
m_pMapTiles[l].f = m_pMapTiles[l].g = FLT_MAX; //Clear F,G
m_pMapTiles[l].open = m_pMapTiles[l].closed = false; //Reset Open and Closed
}
std::vector<MAPTILE*> open; //Create Our Open list
startTile->g = 0; //Init our starting point (SP)
startTile->f = H(start, goal);
startTile->open = true;
open.push_back(startTile); //Add SP to the Open list
bool found = false; // Search as long as a path hasnt been found,
while(!found && !open.empty()) // or there is no more tiles to search
{
MAPTILE * best = open[0]; // Find the best tile (i.e. the lowest F value)
int bestPlace = 0;
for(int i=1;i<(int)open.size();i++)
if(open[i]->f < best->f)
{
best = open[i];
bestPlace = i;
}
if(best == NULL)break; //No path found
open[bestPlace]->open = false;
// Take the best node out of the Open list
open.erase(open.begin() + bestPlace);
if(best->m_mappos == goal) //If the goal has been found
{
std::vector<INTPOINT> p, p2;
MAPTILE *point = best;
while(point->m_mappos != start) // Generate path
{
p.push_back(point->m_mappos);
point = point->m_pParent;
if(p.size() > 500) //Too long path, something is wrong
return std::vector<INTPOINT>();
}
for(int i=p.size()-1;i!=0;i--) // Reverse path
p2.push_back(p[i]);
p2.push_back(goal);
return p2;
}
else
{
for(int i=0;i<8;i++) // otherwise, check the neighbors of the
if(best->m_pNeighbors[i] != NULL) // best tile
{
bool inList = false; // Generate new G and F value
float newG = best->g + 1.0f;
float d = H(best->m_mappos, best->m_pNeighbors[i]->m_mappos);
float newF = newG + H(best->m_pNeighbors[i]->m_mappos, goal) + best->m_pNeighbors[i]->m_cost * 5.0f * d;
if(best->m_pNeighbors[i]->open || best->m_pNeighbors[i]->closed)
{
if(newF < best->m_pNeighbors[i]->f) // If the new F value is lower
{
best->m_pNeighbors[i]->g = newG; // update the values of this tile
best->m_pNeighbors[i]->f = newF;
best->m_pNeighbors[i]->m_pParent = best;
}
inList = true;
}
if(!inList) // If the neighbor tile isn't in the Open or Closed list
{
best->m_pNeighbors[i]->f = newF; //Set the values
best->m_pNeighbors[i]->g = newG;
best->m_pNeighbors[i]->m_pParent = best;
best->m_pNeighbors[i]->open = true;
open.push_back(best->m_pNeighbors[i]); //Add it to the open list
}
}
best->closed = true; //The best tile has now been searched, add it to the Closed list
}
}
return std::vector<INTPOINT>(); //No path found, return an empty path
}
catch(...)
{
debug.Print("Error in TERRAIN::GetPath()");
return std::vector<INTPOINT>();
}
}
unsigned long CSysSolve::FGMRES(const CSysVector & b, CSysVector & x, CMatrixVectorProduct & mat_vec,
CPreconditioner & precond, double tol, unsigned long m, bool monitoring) {
int rank = 0;
#ifndef NO_MPI
rank = MPI::COMM_WORLD.Get_rank();
#endif
/*--- Check the subspace size ---*/
if (m < 1) {
if (rank == 0) cerr << "CSysSolve::FGMRES: illegal value for subspace size, m = " << m << endl;
#ifdef NO_MPI
exit(1);
#else
MPI::COMM_WORLD.Abort(1);
MPI::Finalize();
#endif
}
/*--- Check the subspace size ---*/
if (m > 1000) {
if (rank == 0) cerr << "CSysSolve::FGMRES: illegal value for subspace size (too high), m = " << m << endl;
#ifdef NO_MPI
exit(1);
#else
MPI::COMM_WORLD.Abort(1);
MPI::Finalize();
#endif
}
/*--- Define various arrays
Note: elements in w and z are initialized to x to avoid creating
a temporary CSysVector object for the copy constructor ---*/
vector<CSysVector> w(m+1, x);
vector<CSysVector> z(m+1, x);
vector<double> g(m+1, 0.0);
vector<double> sn(m+1, 0.0);
vector<double> cs(m+1, 0.0);
vector<double> y(m, 0.0);
vector<vector<double> > H(m+1, vector<double>(m, 0.0));
/*--- Calculate the norm of the rhs vector ---*/
double norm0 = b.norm();
/*--- Calculate the initial residual (actually the negative residual)
and compute its norm ---*/
mat_vec(x,w[0]);
w[0] -= b;
double beta = w[0].norm();
if ( (beta < tol*norm0) || (beta < eps) ) {
/*--- System is already solved ---*/
if (rank == 0) cout << "CSysSolve::FGMRES(): system solved by initial guess." << endl;
return 0;
}
/*--- Normalize residual to get w_{0} (the negative sign is because w[0]
holds the negative residual, as mentioned above) ---*/
w[0] /= -beta;
/*--- Initialize the RHS of the reduced system ---*/
g[0] = beta;
/*--- Set the norm to the initial initial residual value ---*/
norm0 = beta;
/*--- Output header information including initial residual ---*/
int i = 0;
if ((monitoring) && (rank == 0)) {
writeHeader("FGMRES", tol, beta);
writeHistory(i, beta, norm0);
}
/*--- Loop over all serach directions ---*/
for (i = 0; i < m; i++) {
/*--- Check if solution has converged ---*/
if (beta < tol*norm0) break;
/*--- Precondition the CSysVector w[i] and store result in z[i] ---*/
precond(w[i], z[i]);
/*--- Add to Krylov subspace ---*/
mat_vec(z[i], w[i+1]);
/*--- Modified Gram-Schmidt orthogonalization ---*/
modGramSchmidt(i, H, w);
/*--- Apply old Givens rotations to new column of the Hessenberg matrix
then generate the new Givens rotation matrix and apply it to
the last two elements of H[:][i] and g ---*/
for (int k = 0; k < i; k++)
applyGivens(sn[k], cs[k], H[k][i], H[k+1][i]);
generateGivens(H[i][i], H[i+1][i], sn[i], cs[i]);
applyGivens(sn[i], cs[i], g[i], g[i+1]);
/*--- Set L2 norm of residual and check if solution has converged ---*/
beta = fabs(g[i+1]);
/*--- Output the relative residual if necessary ---*/
if ((((monitoring) && (rank == 0)) && ((i+1) % 100 == 0)) && (rank == 0)) writeHistory(i+1, beta, norm0);
}
/*--- Solve the least-squares system and update solution ---*/
solveReduced(i, H, g, y);
for (int k = 0; k < i; k++) {
x.Plus_AX(y[k], z[k]);
}
if ((monitoring) && (rank == 0)) {
cout << "# FGMRES final (true) residual:" << endl;
cout << "# Iteration = " << i << ": |res|/|res0| = " << beta/norm0 << endl;
}
// /*--- Recalculate final (neg.) residual (this should be optional) ---*/
// mat_vec(x, w[0]);
// w[0] -= b;
// double res = w[0].norm();
//
// if (fabs(res - beta) > tol*10) {
// if (rank == 0) {
// cout << "# WARNING in CSysSolve::FGMRES(): " << endl;
// cout << "# true residual norm and calculated residual norm do not agree." << endl;
// cout << "# res - beta = " << res - beta << endl;
// }
// }
return i;
}
void TestTransitions( M & machine )
{
machine.initiate();
ActionArray init =
{
Entry< S0< M > >,
Entry< S1< M > >,
Entry< Default0< S1< M > > >,
Entry< S11< M > >,
Entry< Default2< S1< M > > >,
Entry< Default1< S0< M > > >,
Entry< Default2< S0< M > > >
};
machine.CompareToExpectedActionSequence( init );
machine.process_event( A() );
ActionArray a1 =
{
Exit< Default2< S1< M > > >,
Exit< S11< M > >,
Exit< Default0< S1< M > > >,
Exit< S1< M > >,
Trans< S0< M >, A >,
Entry< S1< M > >,
Entry< Default0< S1< M > > >,
Entry< S11< M > >,
Entry< Default2< S1< M > > >
};
machine.CompareToExpectedActionSequence( a1 );
machine.process_event( B() );
ActionArray b1 =
{
Exit< Default2< S1< M > > >,
Exit< S11< M > >,
Exit< Default0< S1< M > > >,
Exit< S1< M > >,
Trans< S0< M >, B >,
Entry< S1< M > >,
Entry< Default0< S1< M > > >,
Entry< S11< M > >,
Entry< Default2< S1< M > > >
};
machine.CompareToExpectedActionSequence( b1 );
machine.process_event( C() );
ActionArray c1 =
{
Exit< Default2< S1< M > > >,
Exit< S11< M > >,
Exit< Default0< S1< M > > >,
Exit< S1< M > >,
Trans< S0< M >, C >,
Entry< S2< M > >,
Entry< Default0< S2< M > > >,
Entry< Default1< S2< M > > >,
Entry< S21< M > >,
Entry< Default0< S21< M > > >,
Entry< S211< M > >,
Entry< Default2< S21< M > > >
};
machine.CompareToExpectedActionSequence( c1 );
machine.process_event( D() );
ActionArray d2 =
{
Exit< Default2< S21< M > > >,
Exit< S211< M > >,
Exit< Default0< S21< M > > >,
Exit< S21< M > >,
Trans< S0< M >, D >,
Entry< S21< M > >,
Entry< Default0< S21< M > > >,
Entry< S211< M > >,
Entry< Default2< S21< M > > >
};
machine.CompareToExpectedActionSequence( d2 );
machine.process_event( E() );
ActionArray e2 =
{
Exit< Default2< S0< M > > >,
Exit< Default1< S0< M > > >,
Exit< Default2< S21< M > > >,
Exit< S211< M > >,
Exit< Default0< S21< M > > >,
Exit< S21< M > >,
Exit< Default1< S2< M > > >,
Exit< Default0< S2< M > > >,
Exit< S2< M > >,
Exit< S0< M > >,
Entry< S0< M > >,
Entry< S2< M > >,
Entry< Default0< S2< M > > >,
Entry< Default1< S2< M > > >,
Entry< S21< M > >,
Entry< Default0< S21< M > > >,
Entry< S211< M > >,
Entry< Default2< S21< M > > >,
Entry< Default1< S0< M > > >,
Entry< Default2< S0< M > > >
};
machine.CompareToExpectedActionSequence( e2 );
machine.process_event( F() );
ActionArray f2 =
{
Exit< Default2< S21< M > > >,
Exit< S211< M > >,
Exit< Default0< S21< M > > >,
Exit< S21< M > >,
Exit< Default1< S2< M > > >,
Exit< Default0< S2< M > > >,
Exit< S2< M > >,
Trans< S0< M >, F >,
Entry< S1< M > >,
Entry< Default0< S1< M > > >,
Entry< S11< M > >,
Entry< Default2< S1< M > > >
};
machine.CompareToExpectedActionSequence( f2 );
machine.process_event( G() );
ActionArray g1 =
{
Exit< Default2< S1< M > > >,
Exit< S11< M > >,
Exit< Default0< S1< M > > >,
Exit< S1< M > >,
Trans< S0< M >, G >,
Entry< S2< M > >,
Entry< Default0< S2< M > > >,
Entry< Default1< S2< M > > >,
Entry< S21< M > >,
Entry< Default0< S21< M > > >,
Entry< S211< M > >,
Entry< Default2< S21< M > > >
};
machine.CompareToExpectedActionSequence( g1 );
machine.process_event( H() );
ActionArray h2 =
{
Exit< Default2< S21< M > > >,
Exit< S211< M > >,
Exit< Default0< S21< M > > >,
Exit< S21< M > >,
Trans< S0< M >, H >,
Entry< S21< M > >,
Entry< Default0< S21< M > > >,
Entry< S211< M > >,
Entry< Default2< S21< M > > >
};
machine.CompareToExpectedActionSequence( h2 );
BOOST_REQUIRE( machine.GetUnconsumedEventCount() == 0 );
machine.process_event( A() );
BOOST_REQUIRE( machine.GetUnconsumedEventCount() == 1 );
ActionArray a2 =
{
};
machine.CompareToExpectedActionSequence( a2 );
machine.process_event( B() );
ActionArray b2 =
{
Exit< Default2< S21< M > > >,
Exit< S211< M > >,
Exit< Default0< S21< M > > >,
Exit< S21< M > >,
Trans< S0< M >, B >,
Entry< S21< M > >,
Entry< Default0< S21< M > > >,
Entry< S211< M > >,
Entry< Default2< S21< M > > >
};
machine.CompareToExpectedActionSequence( b2 );
machine.process_event( C() );
ActionArray c2 =
{
Exit< Default2< S21< M > > >,
Exit< S211< M > >,
Exit< Default0< S21< M > > >,
Exit< S21< M > >,
Exit< Default1< S2< M > > >,
Exit< Default0< S2< M > > >,
Exit< S2< M > >,
Trans< S0< M >, C >,
Entry< S1< M > >,
Entry< Default0< S1< M > > >,
Entry< S11< M > >,
Entry< Default2< S1< M > > >
};
machine.CompareToExpectedActionSequence( c2 );
machine.process_event( D() );
ActionArray d1 =
{
Exit< Default2< S0< M > > >,
Exit< Default1< S0< M > > >,
Exit< Default2< S1< M > > >,
Exit< S11< M > >,
Exit< Default0< S1< M > > >,
Exit< S1< M > >,
Exit< S0< M > >,
Entry< S0< M > >,
Entry< S1< M > >,
Entry< Default0< S1< M > > >,
Entry< S11< M > >,
Entry< Default2< S1< M > > >,
Entry< Default1< S0< M > > >,
Entry< Default2< S0< M > > >
};
machine.CompareToExpectedActionSequence( d1 );
machine.process_event( F() );
ActionArray f1 =
{
Exit< Default2< S1< M > > >,
Exit< S11< M > >,
Exit< Default0< S1< M > > >,
Exit< S1< M > >,
Trans< S0< M >, F >,
Entry< S2< M > >,
Entry< Default0< S2< M > > >,
Entry< Default1< S2< M > > >,
Entry< S21< M > >,
Entry< Default0< S21< M > > >,
Entry< S211< M > >,
Entry< Default2< S21< M > > >
};
machine.CompareToExpectedActionSequence( f1 );
machine.process_event( G() );
ActionArray g2 =
{
Exit< Default2< S0< M > > >,
Exit< Default1< S0< M > > >,
Exit< Default2< S21< M > > >,
Exit< S211< M > >,
Exit< Default0< S21< M > > >,
Exit< S21< M > >,
Exit< Default1< S2< M > > >,
Exit< Default0< S2< M > > >,
Exit< S2< M > >,
Exit< S0< M > >,
Entry< S0< M > >,
Entry< S1< M > >,
Entry< Default0< S1< M > > >,
Entry< S11< M > >,
Entry< Default2< S1< M > > >,
Entry< Default1< S0< M > > >,
Entry< Default2< S0< M > > >
};
machine.CompareToExpectedActionSequence( g2 );
machine.process_event( H() );
ActionArray h1 =
{
Trans< S11< M >, H >
};
machine.CompareToExpectedActionSequence( h1 );
machine.process_event( E() );
ActionArray e1 =
{
Exit< Default2< S0< M > > >,
Exit< Default1< S0< M > > >,
Exit< Default2< S1< M > > >,
Exit< S11< M > >,
Exit< Default0< S1< M > > >,
Exit< S1< M > >,
Exit< S0< M > >,
Entry< S0< M > >,
Entry< S2< M > >,
Entry< Default0< S2< M > > >,
Entry< Default1< S2< M > > >,
Entry< S21< M > >,
Entry< Default0< S21< M > > >,
Entry< S211< M > >,
Entry< Default2< S21< M > > >,
Entry< Default1< S0< M > > >,
Entry< Default2< S0< M > > >
};
machine.CompareToExpectedActionSequence( e1 );
machine.terminate();
ActionArray term =
{
Exit< Default2< S0< M > > >,
Exit< Default1< S0< M > > >,
Exit< Default2< S21< M > > >,
Exit< S211< M > >,
Exit< Default0< S21< M > > >,
Exit< S21< M > >,
Exit< Default1< S2< M > > >,
Exit< Default0< S2< M > > >,
Exit< S2< M > >,
Exit< S0< M > >
};
machine.CompareToExpectedActionSequence( term );
machine.ThrowAction( &Entry< Default0< S1< M > > > );
BOOST_REQUIRE_THROW( machine.initiate(), TransitionTestException );
ActionArray initThrow1 =
{
Entry< S0< M > >,
Entry< S1< M > >,
&::Throw< &::Entry< Default0< S1< M > > > >,
Dtor< S1< M > >,
Dtor< S0< M > >
};
machine.CompareToExpectedActionSequence( initThrow1 );
BOOST_REQUIRE( machine.terminated() );
machine.ThrowAction( &Entry< S11< M > > );
BOOST_REQUIRE_THROW( machine.initiate(), TransitionTestException );
ActionArray initThrow2 =
{
Entry< S0< M > >,
Entry< S1< M > >,
Entry< Default0< S1< M > > >,
&::Throw< &::Entry< S11< M > > >,
Dtor< Default0< S1< M > > >,
Dtor< S1< M > >,
Dtor< S0< M > >
};
machine.CompareToExpectedActionSequence( initThrow2 );
BOOST_REQUIRE( machine.terminated() );
machine.ThrowAction( &Trans< S0< M >, A > );
machine.initiate();
BOOST_REQUIRE_THROW( machine.process_event( A() ), TransitionTestException );
ActionArray a1Throw1 =
{
Entry< S0< M > >,
Entry< S1< M > >,
Entry< Default0< S1< M > > >,
Entry< S11< M > >,
Entry< Default2< S1< M > > >,
Entry< Default1< S0< M > > >,
Entry< Default2< S0< M > > >,
Exit< Default2< S1< M > > >,
Exit< S11< M > >,
Exit< Default0< S1< M > > >,
Exit< S1< M > >,
&::Throw< &::Trans< S0< M >, A > >,
Dtor< Default2< S0< M > > >,
Dtor< Default1< S0< M > > >,
Dtor< S0< M > >
};
machine.CompareToExpectedActionSequence( a1Throw1 );
BOOST_REQUIRE( machine.terminated() );
machine.ThrowAction( &Entry< S211< M > > );
machine.initiate();
BOOST_REQUIRE_THROW( machine.process_event( C() ), TransitionTestException );
ActionArray c1Throw1 =
{
Entry< S0< M > >,
Entry< S1< M > >,
Entry< Default0< S1< M > > >,
Entry< S11< M > >,
Entry< Default2< S1< M > > >,
Entry< Default1< S0< M > > >,
Entry< Default2< S0< M > > >,
Exit< Default2< S1< M > > >,
Exit< S11< M > >,
Exit< Default0< S1< M > > >,
Exit< S1< M > >,
Trans< S0< M >, C >,
Entry< S2< M > >,
Entry< Default0< S2< M > > >,
Entry< Default1< S2< M > > >,
Entry< S21< M > >,
Entry< Default0< S21< M > > >,
&::Throw< &::Entry< S211< M > > >,
Dtor< Default2< S0< M > > >,
Dtor< Default1< S0< M > > >,
Dtor< Default0< S21< M > > >,
Dtor< S21< M > >,
Dtor< Default1< S2< M > > >,
Dtor< Default0< S2< M > > >,
Dtor< S2< M > >,
Dtor< S0< M > >
};
machine.CompareToExpectedActionSequence( c1Throw1 );
BOOST_REQUIRE( machine.terminated() );
machine.ThrowAction( &ExitFn< S11< M > > );
machine.initiate();
BOOST_REQUIRE_THROW( machine.process_event( C() ), TransitionTestException );
ActionArray c1Throw2 =
{
Entry< S0< M > >,
Entry< S1< M > >,
Entry< Default0< S1< M > > >,
Entry< S11< M > >,
Entry< Default2< S1< M > > >,
Entry< Default1< S0< M > > >,
Entry< Default2< S0< M > > >,
Exit< Default2< S1< M > > >,
&::Throw< &::ExitFn< S11< M > > >,
Dtor< S11< M > >,
Dtor< Default2< S0< M > > >,
Dtor< Default1< S0< M > > >,
Dtor< Default0< S1< M > > >,
Dtor< S1< M > >,
Dtor< S0< M > >
};
machine.CompareToExpectedActionSequence( c1Throw2 );
BOOST_REQUIRE( machine.terminated() );
BOOST_REQUIRE( machine.GetUnconsumedEventCount() == 1 );
}
int main(int argc, char** argv)
{
if(argc != 6) {
std::cerr << "Usage: " << argv[0]
<< " in.png out.png match.txt left|right H.txt" <<std::endl;
return 1;
}
// Read image
size_t nx, ny;
LWImage<unsigned char> in(0,0,0);
in.data = read_png_u8_rgb(argv[1], &nx, &ny);
in.w = static_cast<int>(nx); in.h = static_cast<int>(ny); in.comps=3;
if(! in.data) {
std::cerr << "Error reading image " << argv[1] << std::endl;
return 1;
}
// Read correspondences
std::vector<Match> match;
if(! loadMatch(argv[3],match)) {
std::cerr << "Failed reading " << argv[3] << std::endl;
return 1;
}
// Read left/right image
bool bLeft = (strcmp(argv[4],"left")==0);
if(!bLeft && strcmp(argv[4], "right")!=0) {
std::cerr << "Error: arg 4 must be 'left' or 'right'" << std::endl;
return 1;
}
// Read homography
libNumerics::Homography H;
std::ifstream f(argv[5]);
if((f >> H.mat()).fail()) {
std::cerr << "Error reading homography file " << argv[2] << std::endl;
return 1;
}
// Drawing lines
int delta = in.h/(NUM_LINES+1);
if(delta==0) delta=1;
for(int i=delta; i < in.h; i+=delta)
draw_horizontal_dashed_line(in.data, in.w, in.h, i,
LENGTH_ON, LENGTH_OFF, CYAN);
// Drawing SIFT
std::vector<Match>::const_iterator it=match.begin();
for(; it != match.end(); ++it) {
double x=it->x1, y=it->y1;
if(! bLeft) {
x = it->x2; y=it->y2;
}
H(x,y);
int ix = static_cast<int>(std::floor(x+0.5));
int iy = static_cast<int>(std::floor(y+0.5));
if(0<=ix && ix<in.w && 0<=iy && iy<in.h)
draw_cross(in.data, in.w, in.h, ix, iy, CROSS_HALF_LENGTH, RED);
}
// Write image
if(write_png_u8(argv[2], in.data, in.w, in.h, in.comps) != 0) {
std::cerr << "Error writing file " << argv[3] << std::endl;
return 1;
}
return 0;
}
HSVColor::HSVColor(const Color& c)
{
const RGBColor* rgbc = NULL;
const HSVColor* hsvc = NULL;
const HSVConeColor* hsvcc = NULL;
if ( (rgbc = dynamic_cast<const RGBColor*>(&c)) != NULL)
{
appear_t rgbMin, rgbMax, delta;
appear_t r = rgbc->R(), g = rgbc->G(), b = rgbc->B();
rgbMin = std::min(r,std::min(g,b));
rgbMax = std::max(r,std::max(g,b));
delta = rgbMax - rgbMin;
V() = rgbMax;
S() = 0;
if (rgbMax > 0)
S() = delta / rgbMax;
// fixme: handle floating point check here.
NUKLEI_ASSERT( 0 <= S() && S() <= 1 );
H() = 0;
if (delta > 0)
{
if (rgbMax == r && rgbMax != g)
H() += (g - b) / delta;
if (rgbMax == g && rgbMax != b)
H() += (2 + (b - r) / delta);
if (rgbMax == b && rgbMax != r)
H() += (4 + (r - g) / delta);
H() *= 60;
if (H() < 0)
H() += 360;
}
H() *= M_PI / 180;
}
else if ( (hsvc = dynamic_cast<const HSVColor*>(&c)) != NULL) c_ = hsvc->c_;
else if ( (hsvcc = dynamic_cast<const HSVConeColor*>(&c)) != NULL)
{
NUKLEI_ASSERT(hsvcc->W() > 0);
V() = hsvcc->WV() / hsvcc->W();
if (V() > 0)
{
S() = std::sqrt( std::pow(hsvcc->SCosH(), 2) + std::pow(hsvcc->SSinH(), 2) ) / V();
if (hsvcc->SSinH() >= 0) H() = ACos(hsvcc->SCosH() / (V()*S()));
else H() = 2*M_PI-ACos(hsvcc->SCosH() / (V()*S()));
}
else if (V() == 0)
{
S() = 1;
H() = 0;
}
else NUKLEI_ASSERT(false);
}
else NUKLEI_THROW("Unknown color type");
assertConsistency();
}
/* Print out on stderr a line consisting of the test in S, a colon, a space,
a message describing the meaning of the signal number PINFO and a newline.
If S is NULL or "", the colon and space are omitted. */
void
psiginfo (const siginfo_t *pinfo, const char *s)
{
char buf[512];
FILE *fp = __fmemopen (buf, sizeof (buf), "w");
if (fp == NULL)
{
const char *colon;
if (s == NULL || *s == '\0')
s = colon = "";
else
colon = ": ";
__fxprintf (NULL, "%s%ssignal %d\n", s, colon, pinfo->si_signo);
return;
}
if (s != NULL && *s != '\0')
fprintf (fp, "%s: ", s);
const char *desc;
if (pinfo->si_signo >= 0 && pinfo->si_signo < NSIG
&& ((desc = _sys_siglist[pinfo->si_signo]) != NULL
#ifdef SIGRTMIN
|| (pinfo->si_signo >= SIGRTMIN && pinfo->si_signo < SIGRTMAX)
#endif
))
{
#ifdef SIGRTMIN
if (desc == NULL)
{
if (pinfo->si_signo - SIGRTMIN < SIGRTMAX - pinfo->si_signo)
{
if (pinfo->si_signo == SIGRTMIN)
fprintf (fp, "SIGRTMIN (");
else
fprintf (fp, "SIGRTMIN+%d (", pinfo->si_signo - SIGRTMIN);
}
else
{
if (pinfo->si_signo == SIGRTMAX)
fprintf (fp, "SIGRTMAX (");
else
fprintf (fp, "SIGRTMAX-%d (", SIGRTMAX - pinfo->si_signo);
}
}
else
#endif
fprintf (fp, "%s (", _(desc));
const char *base = NULL;
const uint8_t *offarr = NULL;
size_t offarr_len = 0;
switch (pinfo->si_signo)
{
#define H(sig) \
case sig: \
base = C(codestrs_, sig).str; \
offarr = C (codes_, sig); \
offarr_len = sizeof (C (codes_, sig)) / sizeof (C (codes_, sig)[0]);\
break
H (SIGILL);
H (SIGFPE);
H (SIGSEGV);
H (SIGBUS);
H (SIGTRAP);
H (SIGCHLD);
H (SIGPOLL);
}
const char *str = NULL;
if (offarr != NULL
&& pinfo->si_code >= 1 && pinfo->si_code <= offarr_len)
str = base + offarr[pinfo->si_code - 1];
else
switch (pinfo->si_code)
{
case SI_USER:
str = N_("Signal sent by kill()");
break;
case SI_QUEUE:
str = N_("Signal sent by sigqueue()");
break;
case SI_TIMER:
str = N_("Signal generated by the expiration of a timer");
break;
case SI_ASYNCIO:
str = N_("\
Signal generated by the completion of an asynchronous I/O request");
break;
case SI_MESGQ:
str = N_("\
Signal generated by the arrival of a message on an empty message queue");
break;
#ifdef SI_TKILL
case SI_TKILL:
str = N_("Signal sent by tkill()");
break;
#endif
#ifdef SI_ASYNCNL
case SI_ASYNCNL:
str = N_("\
Signal generated by the completion of an asynchronous name lookup request");
break;
#endif
#ifdef SI_SIGIO
case SI_SIGIO:
str = N_("\
Signal generated by the completion of an I/O request");
break;
#endif
#ifdef SI_KERNEL
case SI_KERNEL:
str = N_("Signal sent by the kernel");
break;
#endif
}
if (str != NULL)
fprintf (fp, "%s ", _(str));
else
fprintf (fp, "%d ", pinfo->si_code);
if (pinfo->si_signo == SIGILL || pinfo->si_signo == SIGFPE
|| pinfo->si_signo == SIGSEGV || pinfo->si_signo == SIGBUS)
fprintf (fp, "[%p])\n", pinfo->si_addr);
else if (pinfo->si_signo == SIGCHLD)
fprintf (fp, "%ld %d %ld)\n",
(long int) pinfo->si_pid, pinfo->si_status,
(long int) pinfo->si_uid);
else if (pinfo->si_signo == SIGPOLL)
fprintf (fp, "%ld)\n", (long int) pinfo->si_band);
else
fprintf (fp, "%ld %ld)\n",
(long int) pinfo->si_pid, (long int) pinfo->si_uid);
}
else
static void
sha1_step(struct sha1_ctxt *ctxt)
{
unsigned int a, b, c, d, e, tmp;
size_t t, s;
#if BYTE_ORDER == LITTLE_ENDIAN
struct sha1_ctxt tctxt;
memcpy(&tctxt.m.b8[0], &ctxt->m.b8[0], 64);
ctxt->m.b8[0] = tctxt.m.b8[3]; ctxt->m.b8[1] = tctxt.m.b8[2];
ctxt->m.b8[2] = tctxt.m.b8[1]; ctxt->m.b8[3] = tctxt.m.b8[0];
ctxt->m.b8[4] = tctxt.m.b8[7]; ctxt->m.b8[5] = tctxt.m.b8[6];
ctxt->m.b8[6] = tctxt.m.b8[5]; ctxt->m.b8[7] = tctxt.m.b8[4];
ctxt->m.b8[8] = tctxt.m.b8[11]; ctxt->m.b8[9] = tctxt.m.b8[10];
ctxt->m.b8[10] = tctxt.m.b8[9]; ctxt->m.b8[11] = tctxt.m.b8[8];
ctxt->m.b8[12] = tctxt.m.b8[15]; ctxt->m.b8[13] = tctxt.m.b8[14];
ctxt->m.b8[14] = tctxt.m.b8[13]; ctxt->m.b8[15] = tctxt.m.b8[12];
ctxt->m.b8[16] = tctxt.m.b8[19]; ctxt->m.b8[17] = tctxt.m.b8[18];
ctxt->m.b8[18] = tctxt.m.b8[17]; ctxt->m.b8[19] = tctxt.m.b8[16];
ctxt->m.b8[20] = tctxt.m.b8[23]; ctxt->m.b8[21] = tctxt.m.b8[22];
ctxt->m.b8[22] = tctxt.m.b8[21]; ctxt->m.b8[23] = tctxt.m.b8[20];
ctxt->m.b8[24] = tctxt.m.b8[27]; ctxt->m.b8[25] = tctxt.m.b8[26];
ctxt->m.b8[26] = tctxt.m.b8[25]; ctxt->m.b8[27] = tctxt.m.b8[24];
ctxt->m.b8[28] = tctxt.m.b8[31]; ctxt->m.b8[29] = tctxt.m.b8[30];
ctxt->m.b8[30] = tctxt.m.b8[29]; ctxt->m.b8[31] = tctxt.m.b8[28];
ctxt->m.b8[32] = tctxt.m.b8[35]; ctxt->m.b8[33] = tctxt.m.b8[34];
ctxt->m.b8[34] = tctxt.m.b8[33]; ctxt->m.b8[35] = tctxt.m.b8[32];
ctxt->m.b8[36] = tctxt.m.b8[39]; ctxt->m.b8[37] = tctxt.m.b8[38];
ctxt->m.b8[38] = tctxt.m.b8[37]; ctxt->m.b8[39] = tctxt.m.b8[36];
ctxt->m.b8[40] = tctxt.m.b8[43]; ctxt->m.b8[41] = tctxt.m.b8[42];
ctxt->m.b8[42] = tctxt.m.b8[41]; ctxt->m.b8[43] = tctxt.m.b8[40];
ctxt->m.b8[44] = tctxt.m.b8[47]; ctxt->m.b8[45] = tctxt.m.b8[46];
ctxt->m.b8[46] = tctxt.m.b8[45]; ctxt->m.b8[47] = tctxt.m.b8[44];
ctxt->m.b8[48] = tctxt.m.b8[51]; ctxt->m.b8[49] = tctxt.m.b8[50];
ctxt->m.b8[50] = tctxt.m.b8[49]; ctxt->m.b8[51] = tctxt.m.b8[48];
ctxt->m.b8[52] = tctxt.m.b8[55]; ctxt->m.b8[53] = tctxt.m.b8[54];
ctxt->m.b8[54] = tctxt.m.b8[53]; ctxt->m.b8[55] = tctxt.m.b8[52];
ctxt->m.b8[56] = tctxt.m.b8[59]; ctxt->m.b8[57] = tctxt.m.b8[58];
ctxt->m.b8[58] = tctxt.m.b8[57]; ctxt->m.b8[59] = tctxt.m.b8[56];
ctxt->m.b8[60] = tctxt.m.b8[63]; ctxt->m.b8[61] = tctxt.m.b8[62];
ctxt->m.b8[62] = tctxt.m.b8[61]; ctxt->m.b8[63] = tctxt.m.b8[60];
#endif
a = H(0); b = H(1); c = H(2); d = H(3); e = H(4);
for (t = 0; t < 20; t++) {
s = t & 0x0f;
if (t >= 16)
W(s) = S(1, W((s+13) & 0x0f) ^ W((s+8) & 0x0f) ^
W((s+2) & 0x0f) ^ W(s));
tmp = S(5, a) + F0(b, c, d) + e + W(s) + K(t);
e = d; d = c; c = S(30, b); b = a; a = tmp;
}
for (t = 20; t < 40; t++) {
s = t & 0x0f;
W(s) = S(1, W((s+13) & 0x0f) ^ W((s+8) & 0x0f) ^
W((s+2) & 0x0f) ^ W(s));
tmp = S(5, a) + F1(b, c, d) + e + W(s) + K(t);
e = d; d = c; c = S(30, b); b = a; a = tmp;
}
for (t = 40; t < 60; t++) {
s = t & 0x0f;
W(s) = S(1, W((s+13) & 0x0f) ^ W((s+8) & 0x0f) ^
W((s+2) & 0x0f) ^ W(s));
tmp = S(5, a) + F2(b, c, d) + e + W(s) + K(t);
e = d; d = c; c = S(30, b); b = a; a = tmp;
}
for (t = 60; t < 80; t++) {
s = t & 0x0f;
W(s) = S(1, W((s+13) & 0x0f) ^ W((s+8) & 0x0f) ^
W((s+2) & 0x0f) ^ W(s));
tmp = S(5, a) + F3(b, c, d) + e + W(s) + K(t);
e = d; d = c; c = S(30, b); b = a; a = tmp;
}
H(0) = H(0) + a;
H(1) = H(1) + b;
H(2) = H(2) + c;
H(3) = H(3) + d;
H(4) = H(4) + e;
bzero(&ctxt->m.b8[0], 64);
}
static RBuffer* create(RBin* bin, const ut8 *code, int codelen, const ut8 *data, int datalen, RBinArchOptions *opt) {
ut32 filesize, code_va, code_pa, phoff;
ut32 p_start, p_phoff, p_phdr;
ut32 p_ehdrsz, p_phdrsz;
ut16 ehdrsz, phdrsz;
ut32 p_vaddr, p_paddr, p_fs, p_fs2;
ut32 baddr;
int is_arm = 0;
RBuffer *buf = r_buf_new ();
r_return_val_if_fail (bin && opt && opt->arch, NULL);
is_arm = !strcmp (opt->arch, "arm");
// XXX: hardcoded
if (is_arm) {
baddr = 0x40000;
} else {
baddr = 0x8048000;
}
#define B(x,y) r_buf_append_bytes(buf,(const ut8*)(x),y)
#define D(x) r_buf_append_ut32(buf,x)
#define H(x) r_buf_append_ut16(buf,x)
#define Z(x) r_buf_append_nbytes(buf,x)
#define W(x,y,z) r_buf_write_at(buf,x,(const ut8*)(y),z)
#define WZ(x,y) p_tmp=r_buf_size (buf);Z(x);W(p_tmp,y,strlen(y))
B ("\x7F" "ELF" "\x01\x01\x01\x00", 8);
Z (8);
H (2); // ET_EXEC
if (is_arm) {
H (40); // e_machne = EM_ARM
} else {
H (3); // e_machne = EM_I386
}
D (1);
p_start = r_buf_size (buf);
D (-1); // _start
p_phoff = r_buf_size (buf);
D (-1); // phoff -- program headers offset
D (0); // shoff -- section headers offset
D (0); // flags
p_ehdrsz = r_buf_size (buf);
H (-1); // ehdrsz
p_phdrsz = r_buf_size (buf);
H (-1); // phdrsz
H (1);
H (0);
H (0);
H (0);
// phdr:
p_phdr = r_buf_size (buf);
D (1);
D (0);
p_vaddr = r_buf_size (buf);
D (-1); // vaddr = $$
p_paddr = r_buf_size (buf);
D (-1); // paddr = $$
p_fs = r_buf_size (buf);
D (-1); // filesize
p_fs2 = r_buf_size (buf);
D (-1); // filesize
D (5); // flags
D (0x1000); // align
ehdrsz = p_phdr;
phdrsz = r_buf_size (buf) - p_phdr;
code_pa = r_buf_size (buf);
code_va = code_pa + baddr;
phoff = 0x34;//p_phdr ;
filesize = code_pa + codelen + datalen;
W (p_start, &code_va, 4);
W (p_phoff, &phoff, 4);
W (p_ehdrsz, &ehdrsz, 2);
W (p_phdrsz, &phdrsz, 2);
code_va = baddr; // hack
W (p_vaddr, &code_va, 4);
code_pa = baddr; // hack
W (p_paddr, &code_pa, 4);
W (p_fs, &filesize, 4);
W (p_fs2, &filesize, 4);
B (code, codelen);
if (data && datalen > 0) {
//ut32 data_section = buf->length;
eprintf ("Warning: DATA section not support for ELF yet\n");
B (data, datalen);
}
return buf;
}
void
ckbot::chain_rate::tip_base_step(std::vector<double> s, std::vector<double> T)
{
int N = c.num_links();
std::vector<double> q(N);
std::vector<double> qd(N);
for(int i=0; i<N; ++i)
{
q[i] = s[i];
qd[i] = s[N+i];
}
Eigen::Matrix3d Iden = Eigen::Matrix3d::Identity();
Eigen::Matrix3d Zero = Eigen::Matrix3d::Zero();
/* Declare and initialized all of the loop variables */
/* TODO: Can we allocate all of this on the heap *once* for a particular
* rate object, and then just use pointers to them after? Would this be fast?
* Re-initializing these every time through here has to be slow...
*/
Eigen::MatrixXd pp(6,6);
pp = Eigen::MatrixXd::Zero(6,6);
Eigen::VectorXd zp(6);
zp << 0,0,0,0,0,0;
Eigen::VectorXd grav(6);
grav << 0,0,0,0,0,9.81;
Eigen::Matrix3d L_oc_tilde;
Eigen::Matrix3d R_cur;
Eigen::MatrixXd M_cur(6,6);
Eigen::MatrixXd M_cm(6,6);
M_cm = Eigen::MatrixXd::Zero(6,6);
Eigen::Matrix3d J_o;
Eigen::Vector3d r_i_ip(0,0,0);
Eigen::Vector3d r_i_cm(0,0,0);
Eigen::MatrixXd phi(6,6);
Eigen::MatrixXd phi_cm(6,6);
Eigen::MatrixXd p_cur(6,6);
Eigen::VectorXd H_b_frame_star(6);
Eigen::VectorXd H_w_frame_star(6);
Eigen::RowVectorXd H(6);
double D = 0.0;
Eigen::VectorXd G(6);
Eigen::MatrixXd tau_tilde(6,6);
Eigen::Vector3d omega(0,0,0);
Eigen::Matrix3d omega_cross;
Eigen::VectorXd a(6);
Eigen::VectorXd b(6);
Eigen::VectorXd z(6);
double C = 0.0;
double CF = 0.0;
double SPOLES = 12.0;
double RPOLES = 14.0;
double epsilon = 0.0;
double mu = 0.0;
/* Recurse from the tip to the base of this chain */
for (int i = N-1; i >= 0; --i)
{
module_link& cur = c.get_link(i);
/* Rotation from the world to this link */
R_cur = c.get_current_R(i);
/* Vector, in world frame, from inbound joint to outbound joint of
* this link
*/
r_i_ip = R_cur*(cur.get_r_ip1() - cur.get_r_im1());
/* Operator to transform spatial vectors from outbound joint
* to the inbound joint
*/
phi = get_body_trans(r_i_ip);
/* Vector from the inbound joint to the center of mass */
r_i_cm = R_cur*(-cur.get_r_im1());
/* Operator to transform spatial vectors from the center of mass
* to the inbound joint
*/
phi_cm = get_body_trans(r_i_cm);
/* Cross product matrix corresponding to the vector from
* the inbound joint to the center of mass
*/
L_oc_tilde = get_cross_mat(r_i_cm);
/* 3x3 Inertia matrix of this link about its inbound joint and wrt the
* world coordinate system.
*
* NOTE: Similarity transform of get_I_cm() because it is wrt
* the module frame
*/
J_o = R_cur*cur.get_I_cm()*R_cur.transpose() - cur.get_mass()*L_oc_tilde*L_oc_tilde;
/* Spatial mass matrix of this link about its inbound joint and wrt the
* world coordinate system.
*
*/
M_cur << J_o, cur.get_mass()*L_oc_tilde,
-cur.get_mass()*L_oc_tilde, cur.get_mass()*Iden;
/* Spatial mass matrix of this link about its cm and wrt the
* world coordinate system.
*
* NOTE: Similarity transform of get_I_cm() because it is wrt
* the module frame
*/
M_cm << R_cur*cur.get_I_cm()*R_cur.transpose(), Zero,
Zero, cur.get_mass()*Iden;
/* Articulated Body Spatial inertia matrix of the links from
* this one all the way to the tip of the chain (accounting for
* articulated body spatial inertia that gets through each joint)
*
* pp is p_cur from the previous (outbound) link. This is the
* zero vector when we are on the farthest outbound link (the tip)
*
* In much the same way that we use a similarity transform to change
* the frame of the inertia matrix, a similarity transform moves
* pp from the previous module's base joint to this module's base joint.
*/
p_cur = phi*pp*phi.transpose() + M_cur;
/* The joint matrix, in the body frame, that maps the change
* in general coordinates relating this link to the next inward link.
*
* It essentially defines what "gets through" a link. IE:
* H_b_frame_star = [0,0,1,0,0,0] means that movements and forces
* in the joint's rotational Z axis get through. This is a single
* DOF rotational joint.
*
* H_b_frame_star = eye(6) means that forces in all 6
* (3 rotational, 3 linear) get through the joint using
* 6 independent general coordinates. This is a
* free 6DOF joint.
*
* H_b_frame_star = [0,0,2,0,0,1] is helical joint that rotates twice
* for every single linear unit moved. It is controlled by a single
* general coordinate.
*
* (NOTE/TODO: Using anything but [0,0,1,0,0,0] breaks the code right now.)
*/
H_b_frame_star = cur.get_joint_matrix().transpose();
/* To transform a spatial vector (6 x 1) from one coordinate
* system to another, multiply it by the 6x6 matrix with
* the rotation matrix form one frame to the other on the diagonals
*/
Eigen::MatrixXd tmp_6x6(6,6);
tmp_6x6 << R_cur, Eigen::Matrix3d::Zero(),
Eigen::Matrix3d::Zero(), R_cur;
/* Joint matrix in the world frame */
H_w_frame_star = tmp_6x6*H_b_frame_star;
/* As a row vector */
H = H_w_frame_star.transpose();
D = H*p_cur*H.transpose(); /* TODO:Could use H_w_frame_star..but..clarity */
/* TODO: This needs to be changed to D.inverse() for 6DOF
* and D needs to be made a variable sized Eigen Matrix
*/
G = p_cur*H.transpose()*(1.0/D);
/* Articulated body projection operator through this joint. This defines
* which part of the articulated body spatial inertia gets through this joint
*/
tau_tilde = Eigen::MatrixXd::Identity(6,6) - G*H;
/* pp for the next time around the loop */
pp = tau_tilde*p_cur;
omega = c.get_angular_velocity(i);
omega_cross = get_cross_mat(omega);
/* Gyroscopic force */
b << omega_cross*J_o*omega,
cur.get_mass()*omega_cross*omega_cross*r_i_cm;
/* Coriolis and centripetal accel */
a << 0,0,0,
omega_cross*omega_cross*r_i_cm;
/* z is the total spatial force seen by this link at its inward joint
* phi*zp = Force through joint from tip-side module
* b = Gyroscopic force (velocity dependent)
* p_cur*a = Coriolis and Centripetal forces
* phi_cm*M_cm*grav = Force of grav at CM transformed to inbound jt
*/
z = phi*zp + b + p_cur*a + phi_cm*M_cm*grav;
/* Velocity related damping force in the joint */
/* TODO: 6DOF change. epsilon will be an matrix when the joint
* matrix is not a row matrix, so using C in the calculation
* of epsilon will break things.
*/
C = -cur.get_damping()*qd[i];
/* Cogging force which is a function of the joint angle and
* the motor specifics.
*/
CF = cur.get_rotor_cogging()*sin(RPOLES*(q[i]-cur.get_cogging_offset()))
+ cur.get_stator_cogging()*sin(SPOLES*(q[i]-cur.get_cogging_offset()));
/* The "Effective" force through the joint that can actually create
* accelerations. IE: Forces that are in directions in which the joint
* can actually move.
*/
/* TODO: 6DOF change. Epsilon will not be 1x1 for a 6DOF joint, so
* both T[i] and C used here as they are will break things.
*/
epsilon = T[i] + C + CF - H*z;
mu = (1/D)*epsilon; /* 6DOF change: D will be a matrix, need to invert it */
zp = z + G*epsilon;
/* base_tip_step needs G, a, and mu. So store them in the chain object.*/
mu_all[i] = mu;
int cur_index=0;
for (int k=(6*i); k <= (6*(i+1)-1); ++k,++cur_index)
{
G_all[k] = G[cur_index];
a_all[k] = a[cur_index];
}
}
}
/*! control
\brief calculate the number of packets sent by a same source over a given period of time. If too many packets are sent, following packets are rejected
Parameters required:
function = hash;
backup = /etc/honeybrid/control.tb
expiration = 600
max_packet = 1000
\param[in] pkts, struct that contain the packet to control
\param[out] set result to 1 if rate limit reached, 0 otherwise
*/
mod_result_t mod_control(struct mod_args *args) {
gchar *backup_file;
if (args->pkt == NULL) {
printdbg("%s Error, NULL packet\n", H(6));
return REJECT;
}
printdbg("%s Module called\n", H(args->pkt->conn->id));
mod_result_t result = DEFER;
int expiration;
int max_packet;
gchar *param;
gchar **info;
GKeyFile *backup;
GTimeVal t;
g_get_current_time(&t);
gint now = (t.tv_sec);
char src[INET_ADDRSTRLEN];
inet_ntop(AF_INET, &(args->pkt->packet.ip->saddr), src, INET_ADDRSTRLEN);
/*! get the backup file for this module */
if (NULL
== (backup = (GKeyFile *) g_hash_table_lookup(args->node->config,
"backup"))) {
/*! We can't decide */
printdbg("%s mandatory argument 'backup' undefined!\n",
H(args->pkt->conn->id));
return result;
}
/*! get the backup file path for this module */
if (NULL
== (backup_file = (gchar *) g_hash_table_lookup(args->node->config,
"backup_file"))) {
/*! We can't decide */
printdbg("%s error, backup file path missing\n",
H(args->pkt->conn->id));
return result;
}
/*! get control parameters */
if (NULL
== (param = (gchar *) g_hash_table_lookup(args->node->config,
"expiration"))) {
/*! no value set for expiration, we go with the default one */
expiration = 600;
} else {
expiration = atoi(param);
}
if (NULL
== (param = (gchar *) g_hash_table_lookup(args->node->config,
"max_packet"))) {
/*! no value set for expiration, we go with the default one */
max_packet = 1000;
} else {
max_packet = atoi(param);
}
if (NULL == (info = g_key_file_get_string_list(backup, "source", /* generic group name \todo: group by port number? */
src, NULL, NULL))) {
printdbg("%s IP not found... new entry created\n",
H(args->pkt->conn->id));
info = malloc(3 * sizeof(char *));
/*! 20 characters should be enough to hold even very large numbers */
info[0] = malloc(20 * sizeof(gchar));
info[1] = malloc(20 * sizeof(gchar));
info[2] = malloc(20 * sizeof(gchar));
g_snprintf(info[0], 20, "1"); /*! counter */
g_snprintf(info[1], 20, "%d", now); /*! first seen */
g_snprintf(info[2], 20, "0"); /*! duration */
} else {
/*! We check if we need to expire this entry */
int age = atoi(info[2]);
if (age > expiration) {
printdbg("%s IP found but expired... entry renewed\n",
H(args->pkt->conn->id));
g_snprintf(info[0], 20, "1"); /*! counter */
g_snprintf(info[1], 20, "%d", now); /*! first seen */
g_snprintf(info[2], 20, "0"); /*! duration */
} else {
printdbg("%s IP found... entry updated\n", H(args->pkt->conn->id));
g_snprintf(info[0], 20, "%d", atoi(info[0]) + 1); /*! counter */
g_snprintf(info[2], 20, "%d", now - atoi(info[1])); /*! duration */
}
}
if (atoi(info[0]) > max_packet) {
printdbg("%s Rate limit reached! Packet rejected\n",
H(args->pkt->conn->id));
result = REJECT;
} else {
printdbg("%s Rate limit not reached. Packet accepted\n",
H(args->pkt->conn->id));
result = ACCEPT;
}
g_key_file_set_string_list(backup, "source", src,
(const gchar * const *) info, 3);
save_backup(backup, backup_file);
return result;
}
/* Returns a vector of length N corresponding to each link's qdd */
std::vector<double>
ckbot::chain_rate::base_tip_step(std::vector<double> s, std::vector<double> T)
{
int N = c.num_links();
std::vector<double> q(N);
std::vector<double> qd(N);
std::vector<double> qdd(N);
for(int i=0; i<N; ++i)
{
q[i] = s[i];
qd[i] = s[N+i];
}
Eigen::VectorXd grav(6);
grav << 0,0,0,0,0,9.81;
Eigen::Matrix3d R_cur;
Eigen::Vector3d r_i_ip;
Eigen::MatrixXd phi(6,6);
Eigen::VectorXd alpha(6);
alpha = Eigen::VectorXd::Zero(6);
Eigen::VectorXd alpha_p(6);
Eigen::VectorXd G(6);
Eigen::VectorXd a(6);
Eigen::VectorXd H_b_frame_star(6);
Eigen::VectorXd H_w_frame_star(6);
Eigen::RowVectorXd H(6);
for (int i=0; i<N; ++i)
{
module_link& cur = c.get_link(i);
/* 3x3 Rotation matrix from this link's coordinate
* frame to the world frame
*/
R_cur = c.get_current_R(i);
/* Vector, in world frame, from this link's inbound joint to its
* outbound joint
*/
r_i_ip = R_cur*(cur.get_r_ip1() - cur.get_r_im1());
/* 6x6 Spatial body operator matrix that transforms spatial
* vectors from the outbound joint to the inbound joint in
* the world frame
*/
phi = get_body_trans(r_i_ip);
/* Transform the spatial accel vector from the inbound
* link's inbound joint to this link's inbound joint
*/
alpha_p = phi.transpose()*alpha;
/* These are calculated in tip_base_step */
int cur_index = 0;
for (int k = (6*i); k <= (6*(i+1)-1); ++k, ++cur_index)
{
G[cur_index] = G_all[k];
a[cur_index] = a_all[k];
}
/* The angular acceleration of this link's joint */
/* TODO: 6DOF changes here...This gets a whoooole lotta broken */
qdd[i] = mu_all[i] - G.transpose()*alpha_p;
/* Joint matrix in body frame. Check tip_base_step for the definition */
/* TODO: 6DOF changes here a plenty */
H_b_frame_star = cur.get_joint_matrix().transpose();
Eigen::MatrixXd tmp_6x6(6,6);
tmp_6x6 << R_cur, Eigen::Matrix3d::Zero(),
Eigen::Matrix3d::Zero(), R_cur;
/* Joint matrix in the world frame */
H_w_frame_star = tmp_6x6*H_b_frame_star;
/* As a row vector */
H = H_w_frame_star.transpose();
/* Spatial acceleration of this link at its inbound joint in
* its frame (ie: including its joint accel)
*/
alpha = alpha_p + H.transpose()*qdd[i] + a;
}
return qdd;
}
void
foo (int p1, int *, int)
{
if (p1 == 0)
throw H ();
}
//*****************************************************************************************
//Creer un nouveau fichier netcdf de output avec mes valeurs... Semble ok //*
//Reproduit les dimensions et les variables des fichiers u,v et w... Semble ok //*
//Creer les variables FR frontieres et SO de source... Semble ok //*
//Creer la variable C de concentration... Semble ok //*
//Copier les attributs... Semble ok //*
//Creer les attributs pour C, FR et SO... Semble ok et a finir (plus tard) //*
//Inserer les valeurs dans les dimensions... A tester //*
//Inserer les valeurs dans u,v et w a partir des blocs... A tester //*
//Inserer les valeurs dans FR et SO... A tester //*
//Attention! La date n'a rien a voir maintenant avec le temps en output! //*
void cdf_new(s_nc_all_files *c, s_all_parameters *p, s_memory_blocs *b){ //*
//*****************************************************************************************
int status;
int Cdimshape_p[NVDIM];
int Udimshape_p[NVDIM-1];
float val;
float range[2];
short int short_fill;
c->out.file.name_p=p->filename; //Passe le nom du fichier de output.
strcpy(c->out.x.name_p,"X"); //Passe le nom des dimensions.
strcpy(c->out.y.name_p,"Y");
strcpy(c->out.time.name_p,"TIME");
strcpy(c->out.c.name_p,"C"); //Passe le nom de la variable de concentration.
strcpy(c->out.u.name_p,"U");
strcpy(c->out.v.name_p,"V");
#ifdef BLOC_DIM_3D
strcpy(c->out.z.name_p,"Z");
strcpy(c->out.w.name_p,"W");
#endif
strcpy(c->out.fr.name_p,"FR");
strcpy(c->out.so.name_p,"SO");
H(nc_create(c->out.file.name_p, NC_CLOBBER, &(c->out.file.ncid)))
//Definit les dimensions, leurs variables respectives, et leurs attributs.
//Manque de creer les attributs pour la variable time! (optionnel)
H(CDF_DEF_DIM(x))
H(CDF_DEF_DIM(y))
H(nc_def_dim(c->out.file.ncid, c->out.time.name_p, p->x.N/p->x.D+1, &(c->out.time.id)))
H(CDF_DEF_DVAR(x))
H(CDF_DEF_DVAR(y))
H(CDF_DEF_DVAR(time))
H(CDF_COPY_DATT(x,axis))
H(CDF_COPY_DATT(y,axis))
H(CDF_COPY_DATT(x,units))
H(CDF_COPY_DATT(y,units))
#ifdef BLOC_DIM_3D
H(CDF_DEF_DIM(z))
H(CDF_DEF_DVAR(z))
H(CDF_COPY_DATT(z,axis))
H(CDF_COPY_DATT(z,units))
#endif
//Definit les variables et leurs attributs.
Cdimshape_p[O_TIME]=c->out.time.id;
Cdimshape_p[O_LAT]=c->out.y.id;
Cdimshape_p[O_LON]=c->out.x.id;
Udimshape_p[U_LAT]=c->out.y.id;
Udimshape_p[U_LON]=c->out.x.id;
#ifdef BLOC_DIM_3D
Udimshape_p[U_DEPTH]=c->out.z.id;
Cdimshape_p[O_DEPTH]=c->out.z.id;
#endif
#ifdef CALCSWITCH_ON
H(CDF_DEF_CVAR(c))
#endif
H(CDF_DEF_UVAR(u))
H(CDF_DEF_UVAR(v))
#ifdef BLOC_DIM_3D
H(CDF_DEF_UVAR(w))
#endif
H(CDF_DEF_BVAR(fr))
H(CDF_DEF_BVAR(so))
val=MYFILL;
short_fill=-32767;
range[0]=0.0;
range[1]=1.0;
#ifdef CALCSWITCH_ON
H(CDF_PUT_F_ATT(var, c, fill, &(val)))
H(CDF_PUT_F_ATT(var, c, missing, &(val)))
H(CDF_COPY_ATT(var,c,units))
H(nc_put_att_float(c->out.file.ncid, c->out.c.id, "valid_range", NC_FLOAT, 2, range))
#endif
H(CDF_PUT_F_ATT(var, u, fill, &(val)))
H(CDF_PUT_F_ATT(var, u, missing, &(val)))
H(CDF_COPY_ATT(var,u,units))
H(CDF_PUT_F_ATT(var, v, fill, &(val)))
H(CDF_PUT_F_ATT(var, v, missing, &(val)))
H(CDF_COPY_ATT(var,v,units))
H(CDF_PUT_S_ATT(var, fr, fill, &(short_fill)))
#ifdef BLOC_DIM_3D
H(CDF_PUT_F_ATT(var, w, fill, &(val)))
H(CDF_PUT_F_ATT(var, w, missing, &(val)))
H(CDF_COPY_ATT(var,w,units))
#endif
//Remplit les champs des dimensions et des variables.
#ifdef CALCSWITCH_ON
H(CDF_INQ_VAR(c,id))
#endif
H(CDF_INQ_VAR(so,id))
H(CDF_INQ_VAR(fr,id))
H(CDF_INQ_VAR(u,id))
H(CDF_INQ_VAR(v,id))
H(CDF_INQ_VAR(x,varid))
H(CDF_INQ_VAR(y,varid))
H(CDF_INQ_VAR(time,varid))
H(CDF_INQ_DIM(x))
H(CDF_INQ_DIM(y))
#ifdef BLOC_DIM_3D
H(CDF_INQ_VAR(w,id))
H(CDF_INQ_VAR(z,varid))
H(CDF_INQ_DIM(z))
#endif
H(CDF_INQ_DIM(time))
#ifdef CALCSWITCH_ON
H(CDF_INQ_ATTID(c,id,missing))
H(CDF_INQ_ATTID(c,id,fill))
H(CDF_INQ_ATTID(c,id,units))
#endif
H(CDF_INQ_ATTID(u,id,missing))
H(CDF_INQ_ATTID(u,id,fill))
H(CDF_INQ_ATTID(u,id,units))
H(CDF_INQ_ATTID(v,id,missing))
H(CDF_INQ_ATTID(v,id,fill))
H(CDF_INQ_ATTID(v,id,units))
H(CDF_INQ_ATTID(x,varid,axis))
H(CDF_INQ_ATTID(y,varid,axis))
H(CDF_INQ_ATTID(x,varid,units))
H(CDF_INQ_ATTID(y,varid,units))
#ifdef CALCSWITCH_ON
H(CDF_INQ_ATT(c,id,missing))
H(CDF_INQ_ATT(c,id,fill))
H(CDF_INQ_ATT(c,id,units))
#endif
H(CDF_INQ_ATT(u,id,missing))
H(CDF_INQ_ATT(u,id,fill))
H(CDF_INQ_ATT(u,id,units))
H(CDF_INQ_ATT(v,id,missing))
H(CDF_INQ_ATT(v,id,fill))
H(CDF_INQ_ATT(v,id,units))
H(CDF_INQ_ATT(x,varid,axis))
H(CDF_INQ_ATT(y,varid,axis))
H(CDF_INQ_ATT(x,varid,units))
H(CDF_INQ_ATT(y,varid,units))
#ifdef BLOC_DIM_3D
H(CDF_INQ_ATTID(w,id,missing))
H(CDF_INQ_ATTID(w,id,fill))
H(CDF_INQ_ATTID(w,id,units))
H(CDF_INQ_ATTID(z,varid,axis))
H(CDF_INQ_ATTID(z,varid,units))
H(CDF_INQ_ATT(w,id,missing))
H(CDF_INQ_ATT(w,id,fill))
H(CDF_INQ_ATT(w,id,units))
H(CDF_INQ_ATT(z,varid,axis))
H(CDF_INQ_ATT(z,varid,units))
#endif
H(nc_enddef(c->out.file.ncid))
//Insere les valeurs dans les dimensions (excepte le temps).
CDF_FILL_DIM(x,lon_p)
CDF_FILL_DIM(y,lat_p)
#ifdef BLOC_DIM_3D
CDF_FILL_DIM(z,depth_p)
#endif
cdf_fill_time(&(c->out), p);
//Insere les valeurs dans les variables u,v,w,so et fr.
CDF_FILL_VAR(V_u,u)
CDF_FILL_VAR(V_v,v)
#ifdef BLOC_DIM_3D
CDF_FILL_VAR(V_w,w)
#endif
CDF_FILL_BVAR(C_so,so)
CDF_FILL_BVAR(C_fr,fr)
return;
}
int test_vec3_ctor()
{
int Error = 0;
#if(GLM_HAS_INITIALIZER_LISTS)
{
glm::vec3 a{ 0, 1, 2 };
std::vector<glm::vec3> v = {
{0, 1, 2},
{4, 5, 6},
{8, 9, 0}
};
}
{
glm::dvec3 a{ 0, 1, 2 };
std::vector<glm::dvec3> v = {
{0, 1, 2},
{4, 5, 6},
{8, 9, 0}
};
}
#endif
#if(GLM_HAS_ANONYMOUS_UNION && defined(GLM_SWIZZLE))
{
glm::vec3 A = glm::vec3(1.0f, 2.0f, 3.0f);
glm::vec3 B = A.xyz;
glm::vec3 C(A.xyz);
glm::vec3 D(A.xyz());
glm::vec3 E(A.x, A.yz);
glm::vec3 F(A.x, A.yz());
glm::vec3 G(A.xy, A.z);
glm::vec3 H(A.xy(), A.z);
Error += glm::all(glm::equal(A, B)) ? 0 : 1;
Error += glm::all(glm::equal(A, C)) ? 0 : 1;
Error += glm::all(glm::equal(A, D)) ? 0 : 1;
Error += glm::all(glm::equal(A, E)) ? 0 : 1;
Error += glm::all(glm::equal(A, F)) ? 0 : 1;
Error += glm::all(glm::equal(A, G)) ? 0 : 1;
Error += glm::all(glm::equal(A, H)) ? 0 : 1;
}
#endif//(GLM_HAS_ANONYMOUS_UNION && defined(GLM_SWIZZLE))
{
glm::vec3 A(1);
glm::vec3 B(1, 1, 1);
Error += A == B ? 0 : 1;
}
{
std::vector<glm::vec3> Tests;
Tests.push_back(glm::vec3(glm::vec2(1, 2), 3));
Tests.push_back(glm::vec3(1, glm::vec2(2, 3)));
Tests.push_back(glm::vec3(1, 2, 3));
Tests.push_back(glm::vec3(glm::vec4(1, 2, 3, 4)));
for(std::size_t i = 0; i < Tests.size(); ++i)
Error += Tests[i] == glm::vec3(1, 2, 3) ? 0 : 1;
}
return Error;
}
static void
doTheRounds(uint32 X[16], uint32 state[4])
{
uint32 a,
b,
c,
d;
a = state[0];
b = state[1];
c = state[2];
d = state[3];
/* round 1 */
a = b + ROT_LEFT((a + F(b, c, d) + X[0] + 0xd76aa478), 7); /* 1 */
d = a + ROT_LEFT((d + F(a, b, c) + X[1] + 0xe8c7b756), 12); /* 2 */
c = d + ROT_LEFT((c + F(d, a, b) + X[2] + 0x242070db), 17); /* 3 */
b = c + ROT_LEFT((b + F(c, d, a) + X[3] + 0xc1bdceee), 22); /* 4 */
a = b + ROT_LEFT((a + F(b, c, d) + X[4] + 0xf57c0faf), 7); /* 5 */
d = a + ROT_LEFT((d + F(a, b, c) + X[5] + 0x4787c62a), 12); /* 6 */
c = d + ROT_LEFT((c + F(d, a, b) + X[6] + 0xa8304613), 17); /* 7 */
b = c + ROT_LEFT((b + F(c, d, a) + X[7] + 0xfd469501), 22); /* 8 */
a = b + ROT_LEFT((a + F(b, c, d) + X[8] + 0x698098d8), 7); /* 9 */
d = a + ROT_LEFT((d + F(a, b, c) + X[9] + 0x8b44f7af), 12); /* 10 */
c = d + ROT_LEFT((c + F(d, a, b) + X[10] + 0xffff5bb1), 17); /* 11 */
b = c + ROT_LEFT((b + F(c, d, a) + X[11] + 0x895cd7be), 22); /* 12 */
a = b + ROT_LEFT((a + F(b, c, d) + X[12] + 0x6b901122), 7); /* 13 */
d = a + ROT_LEFT((d + F(a, b, c) + X[13] + 0xfd987193), 12); /* 14 */
c = d + ROT_LEFT((c + F(d, a, b) + X[14] + 0xa679438e), 17); /* 15 */
b = c + ROT_LEFT((b + F(c, d, a) + X[15] + 0x49b40821), 22); /* 16 */
/* round 2 */
a = b + ROT_LEFT((a + G(b, c, d) + X[1] + 0xf61e2562), 5); /* 17 */
d = a + ROT_LEFT((d + G(a, b, c) + X[6] + 0xc040b340), 9); /* 18 */
c = d + ROT_LEFT((c + G(d, a, b) + X[11] + 0x265e5a51), 14); /* 19 */
b = c + ROT_LEFT((b + G(c, d, a) + X[0] + 0xe9b6c7aa), 20); /* 20 */
a = b + ROT_LEFT((a + G(b, c, d) + X[5] + 0xd62f105d), 5); /* 21 */
d = a + ROT_LEFT((d + G(a, b, c) + X[10] + 0x02441453), 9); /* 22 */
c = d + ROT_LEFT((c + G(d, a, b) + X[15] + 0xd8a1e681), 14); /* 23 */
b = c + ROT_LEFT((b + G(c, d, a) + X[4] + 0xe7d3fbc8), 20); /* 24 */
a = b + ROT_LEFT((a + G(b, c, d) + X[9] + 0x21e1cde6), 5); /* 25 */
d = a + ROT_LEFT((d + G(a, b, c) + X[14] + 0xc33707d6), 9); /* 26 */
c = d + ROT_LEFT((c + G(d, a, b) + X[3] + 0xf4d50d87), 14); /* 27 */
b = c + ROT_LEFT((b + G(c, d, a) + X[8] + 0x455a14ed), 20); /* 28 */
a = b + ROT_LEFT((a + G(b, c, d) + X[13] + 0xa9e3e905), 5); /* 29 */
d = a + ROT_LEFT((d + G(a, b, c) + X[2] + 0xfcefa3f8), 9); /* 30 */
c = d + ROT_LEFT((c + G(d, a, b) + X[7] + 0x676f02d9), 14); /* 31 */
b = c + ROT_LEFT((b + G(c, d, a) + X[12] + 0x8d2a4c8a), 20); /* 32 */
/* round 3 */
a = b + ROT_LEFT((a + H(b, c, d) + X[5] + 0xfffa3942), 4); /* 33 */
d = a + ROT_LEFT((d + H(a, b, c) + X[8] + 0x8771f681), 11); /* 34 */
c = d + ROT_LEFT((c + H(d, a, b) + X[11] + 0x6d9d6122), 16); /* 35 */
b = c + ROT_LEFT((b + H(c, d, a) + X[14] + 0xfde5380c), 23); /* 36 */
a = b + ROT_LEFT((a + H(b, c, d) + X[1] + 0xa4beea44), 4); /* 37 */
d = a + ROT_LEFT((d + H(a, b, c) + X[4] + 0x4bdecfa9), 11); /* 38 */
c = d + ROT_LEFT((c + H(d, a, b) + X[7] + 0xf6bb4b60), 16); /* 39 */
b = c + ROT_LEFT((b + H(c, d, a) + X[10] + 0xbebfbc70), 23); /* 40 */
a = b + ROT_LEFT((a + H(b, c, d) + X[13] + 0x289b7ec6), 4); /* 41 */
d = a + ROT_LEFT((d + H(a, b, c) + X[0] + 0xeaa127fa), 11); /* 42 */
c = d + ROT_LEFT((c + H(d, a, b) + X[3] + 0xd4ef3085), 16); /* 43 */
b = c + ROT_LEFT((b + H(c, d, a) + X[6] + 0x04881d05), 23); /* 44 */
a = b + ROT_LEFT((a + H(b, c, d) + X[9] + 0xd9d4d039), 4); /* 45 */
d = a + ROT_LEFT((d + H(a, b, c) + X[12] + 0xe6db99e5), 11); /* 46 */
c = d + ROT_LEFT((c + H(d, a, b) + X[15] + 0x1fa27cf8), 16); /* 47 */
b = c + ROT_LEFT((b + H(c, d, a) + X[2] + 0xc4ac5665), 23); /* 48 */
/* round 4 */
a = b + ROT_LEFT((a + I(b, c, d) + X[0] + 0xf4292244), 6); /* 49 */
d = a + ROT_LEFT((d + I(a, b, c) + X[7] + 0x432aff97), 10); /* 50 */
c = d + ROT_LEFT((c + I(d, a, b) + X[14] + 0xab9423a7), 15); /* 51 */
b = c + ROT_LEFT((b + I(c, d, a) + X[5] + 0xfc93a039), 21); /* 52 */
a = b + ROT_LEFT((a + I(b, c, d) + X[12] + 0x655b59c3), 6); /* 53 */
d = a + ROT_LEFT((d + I(a, b, c) + X[3] + 0x8f0ccc92), 10); /* 54 */
c = d + ROT_LEFT((c + I(d, a, b) + X[10] + 0xffeff47d), 15); /* 55 */
b = c + ROT_LEFT((b + I(c, d, a) + X[1] + 0x85845dd1), 21); /* 56 */
a = b + ROT_LEFT((a + I(b, c, d) + X[8] + 0x6fa87e4f), 6); /* 57 */
d = a + ROT_LEFT((d + I(a, b, c) + X[15] + 0xfe2ce6e0), 10); /* 58 */
c = d + ROT_LEFT((c + I(d, a, b) + X[6] + 0xa3014314), 15); /* 59 */
b = c + ROT_LEFT((b + I(c, d, a) + X[13] + 0x4e0811a1), 21); /* 60 */
a = b + ROT_LEFT((a + I(b, c, d) + X[4] + 0xf7537e82), 6); /* 61 */
d = a + ROT_LEFT((d + I(a, b, c) + X[11] + 0xbd3af235), 10); /* 62 */
c = d + ROT_LEFT((c + I(d, a, b) + X[2] + 0x2ad7d2bb), 15); /* 63 */
b = c + ROT_LEFT((b + I(c, d, a) + X[9] + 0xeb86d391), 21); /* 64 */
state[0] += a;
state[1] += b;
state[2] += c;
state[3] += d;
}
double LT() const {
size_t E = m_root_node.labels_size;
return E * 8 + E * H(E, m_root_node.n_internal_nodes + m_root_node.n_leaves - 1);
}
static inline int
NCR5380_pwrite(struct Scsi_Host *host, unsigned char *addr, int len)
{
unsigned long *laddr;
void __iomem *dma = priv(host)->dma + 0x2000;
if(!len) return 0;
writeb(0x02, priv(host)->base + CTRL);
laddr = (unsigned long *)addr;
while(len >= 32)
{
unsigned int status;
unsigned long v;
status = readb(priv(host)->base + STAT);
if(status & 0x80)
goto end;
if(!(status & 0x40))
continue;
v=*laddr++; writew(L(v), dma); writew(H(v), dma);
v=*laddr++; writew(L(v), dma); writew(H(v), dma);
v=*laddr++; writew(L(v), dma); writew(H(v), dma);
v=*laddr++; writew(L(v), dma); writew(H(v), dma);
v=*laddr++; writew(L(v), dma); writew(H(v), dma);
v=*laddr++; writew(L(v), dma); writew(H(v), dma);
v=*laddr++; writew(L(v), dma); writew(H(v), dma);
v=*laddr++; writew(L(v), dma); writew(H(v), dma);
len -= 32;
if(len == 0)
break;
}
addr = (unsigned char *)laddr;
writeb(0x12, priv(host)->base + CTRL);
while(len > 0)
{
unsigned int status;
status = readb(priv(host)->base + STAT);
if(status & 0x80)
goto end;
if(status & 0x40)
{
writeb(*addr++, dma);
if(--len == 0)
break;
}
status = readb(priv(host)->base + STAT);
if(status & 0x80)
goto end;
if(status & 0x40)
{
writeb(*addr++, dma);
if(--len == 0)
break;
}
}
end:
writeb(priv(host)->ctrl | 0x40, priv(host)->base + CTRL);
return len;
}
//*****************************************************************************
//
//! Set the random number generator seed.
//!
//! Seed the random number generator by running a MD4 hash on the entropy pool.
//! Note that the entropy pool may change from beneath us, but for the purposes
//! of generating random numbers that is not a concern. Also, the MD4 hash was
//! broken long ago, but since it is being used to generate random numbers
//! instead of providing security this is not a concern.
//!
//! \return New seed value.
//
//*****************************************************************************
uint32_t
RandomSeed(void)
{
uint32_t ui32A, ui32B, ui32C, ui32D, ui32Temp, ui32Idx;
//
// Initialize the digest.
//
ui32A = 0x67452301;
ui32B = 0xefcdab89;
ui32C = 0x98badcfe;
ui32D = 0x10325476;
//
// Perform the first round of operations.
//
#define F(a, b, c, d, k, s) \
{ \
ui32Temp = a + (d ^ (b & (c ^ d))) + g_pui32RandomEntropy[k]; \
a = (ui32Temp << s) | (ui32Temp >> (32 - s)); \
}
for(ui32Idx = 0; ui32Idx < 16; ui32Idx += 4)
{
F(ui32A, ui32B, ui32C, ui32D, ui32Idx + 0, 3);
F(ui32D, ui32A, ui32B, ui32C, ui32Idx + 1, 7);
F(ui32C, ui32D, ui32A, ui32B, ui32Idx + 2, 11);
F(ui32B, ui32C, ui32D, ui32A, ui32Idx + 3, 19);
}
//
// Perform the second round of operations.
//
#define G(a, b, c, d, k, s) \
{ \
ui32Temp = (a + ((b & c) | (b & d) | (c & d)) + \
g_pui32RandomEntropy[k] + 0x5a827999); \
a = (ui32Temp << s) | (ui32Temp >> (32 - s)); \
}
for(ui32Idx = 0; ui32Idx < 4; ui32Idx++)
{
G(ui32A, ui32B, ui32C, ui32D, ui32Idx + 0, 3);
G(ui32D, ui32A, ui32B, ui32C, ui32Idx + 4, 5);
G(ui32C, ui32D, ui32A, ui32B, ui32Idx + 8, 9);
G(ui32B, ui32C, ui32D, ui32A, ui32Idx + 12, 13);
}
//
// Perform the third round of operations.
//
#define H(a, b, c, d, k, s) \
{ \
ui32Temp = a + (b ^ c ^ d) + g_pui32RandomEntropy[k] + 0x6ed9eba1; \
a = (ui32Temp << s) | (ui32Temp >> (32 - s)); \
}
for(ui32Idx = 0; ui32Idx < 4; ui32Idx += 2)
{
H(ui32A, ui32B, ui32C, ui32D, ui32Idx + 0, 3);
H(ui32D, ui32A, ui32B, ui32C, ui32Idx + 8, 9);
H(ui32C, ui32D, ui32A, ui32B, ui32Idx + 4, 11);
H(ui32B, ui32C, ui32D, ui32A, ui32Idx + 12, 15);
if(ui32Idx == 2)
{
ui32Idx -= 3;
}
}
//
// Use the first word of the resulting digest as the random number seed.
//
return(ui32A + 0x67452301);
}
// --------------------------------------------------------------------------
// main(Number of arguments, Argument values)
// Description : This is the entry point of the program.
// Return value : SUCCESS:0 ERROR:-1
// --------------------------------------------------------------------------
int main(int argc, char **argv)
{
// AR.Drone class
ARDrone ardrone;
// Initialize
if (!ardrone.open()) {
printf("Failed to initialize.\n");
return -1;
}
// Battery
printf("Battery = %d%%\n", ardrone.getBatteryPercentage());
// Map
cv::Mat map = cv::Mat::zeros(500, 500, CV_8UC3);
// Kalman filter
cv::KalmanFilter kalman(6, 4, 0);
// Sampling time [s]
const double dt = 0.033;
// Transition matrix (x, y, z, vx, vy, vz)
cv::Mat1f F(6, 6);
F << 1.0, 0.0, 0.0, dt, 0.0, 0.0,
0.0, 1.0, 0.0, 0.0, dt, 0.0,
0.0, 0.0, 1.0, 0.0, 0.0, dt,
0.0, 0.0, 0.0, 1.0, 0.0, 0.0,
0.0, 0.0, 0.0, 0.0, 1.0, 0.0,
0.0, 0.0, 0.0, 0.0, 0.0, 1.0;
kalman.transitionMatrix = F;
// Measurement matrix (0, 0, z, vx, vy, vz)
cv::Mat1f H(4, 6);
H << 0, 0, 1, 0, 0, 0,
0, 0, 0, 1, 0, 0,
0, 0, 0, 0, 1, 0,
0, 0, 0, 0, 0, 1;
kalman.measurementMatrix = H;
// Process noise covairance (x, y, z, vx, vy, vz)
cv::Mat1f Q(6, 6);
Q << 0.1, 0.0, 0.0, 0.0, 0.0, 0.0,
0.0, 0.1, 0.0, 0.0, 0.0, 0.0,
0.0, 0.0, 0.1, 0.0, 0.0, 0.0,
0.0, 0.0, 0.0, 0.3, 0.0, 0.0,
0.0, 0.0, 0.0, 0.0, 0.3, 0.0,
0.0, 0.0, 0.0, 0.0, 0.0, 0.3;
kalman.processNoiseCov = Q;
// Measurement noise covariance (z, vx, vy, vz)
cv::Mat1f R(4, 4);
R << 0.1, 0.0, 0.00, 0.00,
0.0, 0.1, 0.00, 0.00,
0.0, 0.0, 0.05, 0.00,
0.0, 0.0, 0.00, 0.05;
kalman.measurementNoiseCov = R;
// Main loop
while (1) {
// Key input
int key = cv::waitKey(33);
if (key == 0x1b) break;
// Update
if (!ardrone.update()) break;
// Get an image
cv::Mat image = ardrone.getImage();
// Prediction
cv::Mat prediction = kalman.predict();
// Altitude
double altitude = ardrone.getAltitude();
// Orientations
double roll = ardrone.getRoll();
double pitch = ardrone.getPitch();
double yaw = ardrone.getYaw();
// Velocities
double vx, vy, vz;
double velocity = ardrone.getVelocity(&vx, &vy, &vz);
cv::Mat V = (cv::Mat1f(3,1) << vx, vy, vz);
// Rotation matrices
cv::Mat RZ = (cv::Mat1f(3,3) << cos(yaw), -sin(yaw), 0.0,
sin(yaw), cos(yaw), 0.0,
0.0, 0.0, 1.0);
cv::Mat RY = (cv::Mat1f(3,3) << cos(pitch), 0.0, sin(pitch),
0.0, 1.0, 0.0,
-sin(pitch), 0.0, cos(pitch));
cv::Mat RX = (cv::Mat1f(3,3) << 1.0, 0.0, 0.0,
0.0, cos(roll), -sin(roll),
0.0, sin(roll), cos(roll));
// Time [s]
static int64 last = cv::getTickCount();
double dt = (cv::getTickCount() - last) / cv::getTickFrequency();
last = cv::getTickCount();
// Local movements (z, vx, vy, vz)
cv::Mat1f M = RZ * RY * RX * V * dt;
cv::Mat measurement = (cv::Mat1f(4,1) << altitude, M(0,0), M(1,0), M(2,0));
// Correction
cv::Mat1f estimated = kalman.correct(measurement);
// Position (x, y, z)
double pos[3] = {estimated(0,0), estimated(1,0), estimated(2,0)};
printf("x = %3.2fm, y = %3.2fm, z = %3.2fm\n", pos[0], pos[1], pos[2]);
// Take off / Landing
if (key == ' ') {
if (ardrone.onGround()) ardrone.takeoff();
else ardrone.landing();
}
// Move
double x = 0.0, y = 0.0, z = 0.0, r = 0.0;
if (key == 0x260000) x = 1.0;
if (key == 0x280000) x = -1.0;
if (key == 0x250000) r = 1.0;
if (key == 0x270000) r = -1.0;
if (key == 'q') z = 1.0;
if (key == 'a') z = -1.0;
ardrone.move3D(x, y, z, r);
// Change camera
static int mode = 0;
if (key == 'c') ardrone.setCamera(++mode%4);
// Display the image
cv::circle(map, cv::Point(-pos[1]*100.0 + map.cols/2, -pos[0]*100.0 + map.rows/2), 2, CV_RGB(255,0,0));
cv::imshow("map", map);
cv::imshow("camera", image);
}
// See you
ardrone.close();
return 0;
}
PetscErrorCode SNESQNApply_LBFGS(SNES snes,PetscInt it,Vec Y,Vec X,Vec Xold,Vec D,Vec Dold)
{
PetscErrorCode ierr;
SNES_QN *qn = (SNES_QN*)snes->data;
Vec W = snes->work[3];
Vec *dX = qn->U;
Vec *dF = qn->V;
PetscScalar *alpha = qn->alpha;
PetscScalar *beta = qn->beta;
PetscScalar *dXtdF = qn->dXtdF;
PetscScalar *dFtdX = qn->dFtdX;
PetscScalar *YtdX = qn->YtdX;
/* ksp thing for Jacobian scaling */
PetscInt k,i,j,g,lits;
PetscInt m = qn->m;
PetscScalar t;
PetscInt l = m;
PetscFunctionBegin;
if (it < m) l = it;
ierr = VecCopy(D,Y);CHKERRQ(ierr);
if (it > 0) {
k = (it - 1) % l;
ierr = VecCopy(D,dF[k]);CHKERRQ(ierr);
ierr = VecAXPY(dF[k], -1.0, Dold);CHKERRQ(ierr);
ierr = VecCopy(X, dX[k]);CHKERRQ(ierr);
ierr = VecAXPY(dX[k], -1.0, Xold);CHKERRQ(ierr);
if (qn->singlereduction) {
ierr = VecMDotBegin(dF[k],l,dX,dXtdF);CHKERRQ(ierr);
ierr = VecMDotBegin(dX[k],l,dF,dFtdX);CHKERRQ(ierr);
ierr = VecMDotBegin(Y,l,dX,YtdX);CHKERRQ(ierr);
ierr = VecMDotEnd(dF[k],l,dX,dXtdF);CHKERRQ(ierr);
ierr = VecMDotEnd(dX[k],l,dF,dFtdX);CHKERRQ(ierr);
ierr = VecMDotEnd(Y,l,dX,YtdX);CHKERRQ(ierr);
for (j = 0; j < l; j++) {
H(k, j) = dFtdX[j];
H(j, k) = dXtdF[j];
}
/* copy back over to make the computation of alpha and beta easier */
for (j = 0; j < l; j++) dXtdF[j] = H(j, j);
} else {
ierr = VecDot(dX[k], dF[k], &dXtdF[k]);CHKERRQ(ierr);
}
if (qn->scale_type == SNES_QN_SCALE_LINESEARCH) {
ierr = SNESLineSearchGetLambda(snes->linesearch,&qn->scaling);CHKERRQ(ierr);
}
}
/* outward recursion starting at iteration k's update and working back */
for (i=0; i<l; i++) {
k = (it-i-1)%l;
if (qn->singlereduction) {
/* construct t = dX[k] dot Y as Y_0 dot dX[k] + sum(-alpha[j]dX[k]dF[j]) */
t = YtdX[k];
for (j=0; j<i; j++) {
g = (it-j-1)%l;
t -= alpha[g]*H(k, g);
}
alpha[k] = t/H(k,k);
} else {
ierr = VecDot(dX[k],Y,&t);CHKERRQ(ierr);
alpha[k] = t/dXtdF[k];
}
if (qn->monitor) {
ierr = PetscViewerASCIIAddTab(qn->monitor,((PetscObject)snes)->tablevel+2);CHKERRQ(ierr);
ierr = PetscViewerASCIIPrintf(qn->monitor, "it: %D k: %D alpha: %14.12e\n", it, k, (double)PetscRealPart(alpha[k]));CHKERRQ(ierr);
ierr = PetscViewerASCIISubtractTab(qn->monitor,((PetscObject)snes)->tablevel+2);CHKERRQ(ierr);
}
ierr = VecAXPY(Y,-alpha[k],dF[k]);CHKERRQ(ierr);
}
if (qn->scale_type == SNES_QN_SCALE_JACOBIAN) {
ierr = KSPSolve(snes->ksp,Y,W);CHKERRQ(ierr);
SNESCheckKSPSolve(snes);
ierr = KSPGetIterationNumber(snes->ksp,&lits);CHKERRQ(ierr);
snes->linear_its += lits;
ierr = VecCopy(W, Y);CHKERRQ(ierr);
} else {
ierr = VecScale(Y, qn->scaling);CHKERRQ(ierr);
}
if (qn->singlereduction) {
ierr = VecMDot(Y,l,dF,YtdX);CHKERRQ(ierr);
}
/* inward recursion starting at the first update and working forward */
for (i = 0; i < l; i++) {
k = (it + i - l) % l;
if (qn->singlereduction) {
t = YtdX[k];
for (j = 0; j < i; j++) {
g = (it + j - l) % l;
t += (alpha[g] - beta[g])*H(g, k);
}
beta[k] = t / H(k, k);
} else {
ierr = VecDot(dF[k], Y, &t);CHKERRQ(ierr);
beta[k] = t / dXtdF[k];
}
ierr = VecAXPY(Y, (alpha[k] - beta[k]), dX[k]);CHKERRQ(ierr);
if (qn->monitor) {
ierr = PetscViewerASCIIAddTab(qn->monitor,((PetscObject)snes)->tablevel+2);CHKERRQ(ierr);
ierr = PetscViewerASCIIPrintf(qn->monitor, "it: %D k: %D alpha - beta: %14.12e\n", it, k, (double)PetscRealPart(alpha[k] - beta[k]));CHKERRQ(ierr);
ierr = PetscViewerASCIISubtractTab(qn->monitor,((PetscObject)snes)->tablevel+2);CHKERRQ(ierr);
}
}
PetscFunctionReturn(0);
}
int nmppsFFT8192Inv28888Ref_f(const nm32sc* src, nm32sc* dst)
{
vec<cmplx<double> > X(8192);
vec<cmplx<double> > Y(8192);
vec<cmplx<double> > G(8192);
vec<cmplx<double> > N(8192);
vec<cmplx<double> > H(8192);
vec<cmplx<double> > J(8192);
cmplx<double> z;
cmplx<double> t;
for(int i=0; i<8192; i++){
X[i].re=src[i].re;
X[i].im=src[i].im;
}
// Вывод:
//----------------- 0 -----------------
//for(int k=0; k<8192; k++){
// y[k]=expIFFT<8192>(0)*x[0];
// for(int n=1; n<8192; n++){
// y[k]+=expIFFT<8192>(n*k)*x[n];
// }
//}
//------------- 1 ---------------
// n=8*n+i
// for(int k=0; k<8192; k++)
// for(int i=0; i<8; i++)
// for(int n=0; n<8192/8; n++)
// y[k]+=expIFFT<8192>(8*n*k)*expIFFT<8192>(k*i)*x[8*n+i];
//------------- 2 ---------------
// n=8*n+j
//for(int k=0; k<8192; k++)
// for(int i=0; i<8; i++)
// for(int j=0; j<8; j++)
// for(int n=0; n<8192/8/8; n++)
// y[k]+=expIFFT<8192>(8*(8*n+j)*k)*expIFFT<8192>(k*i)*x[8*(8*n+j)+i];
//------------- 3 ---------------
// n=8*n+h
//for(int k=0; k<8192; k++)
// for(int i=0; i<8; i++)
// for(int j=0; j<8; j++)
// for(int h=0; h<8; h++)
// for(int n=0; n<8192/8/8/8; n++)
// y[k]+=expIFFT<8192>(8*(8*(8*n+h)+j)*k)*expIFFT<8192>(k*i)*x[8*(8*(8*n+h)+j)+i];
//------------- 4 ---------------
// n=8*n+g
//for(int k=0; k<8192; k++)
// for(int i=0; i<8; i++)
// for(int j=0; j<8; j++)
// for(int h=0; h<8; h++)
// for(int g=0; g<8; g++)
// for(int n=0; n<8192/8/8/8/8; n++)
// y[k]+=expIFFT<8192>(8*(8*(8*(8*n+g)+h)+j)*k)*expIFFT<8192>(k*i)*x[8*(8*(8*(8*n+g)+h)+j)+i];
//------------- 5 ---------------
//for(int k=0; k<8192; k++)
// for(int i=0; i<8; i++)
// for(int j=0; j<8; j++)
// for(int h=0; h<8; h++)
// for(int g=0; g<8; g++)
// for(int n=0; n<2; n++)
// y[k]+= x[4096*n+512*g+64*h+8*j+i]*
// expIFFT<8192>(4096*n*k)*
// expIFFT<8192>(512*g*k)*
// expIFFT<8192>(64*h*k)*
// expIFFT<8192>(8*j*k)*
// expIFFT<8192>(i*k);
//------------- 6 ---------------
//for(int k=0; k<8192; k++)
// for(int i=0; i<8; i++)
// for(int j=0; j<8; j++)
// for(int h=0; h<8; h++)
// for(int g=0; g<8; g++){
// cmplx<double> sumN;
// for(int n=0; n<2; n++)
// sumN+= x[4096*n+512*g+64*h+8*j+i]*
// expIFFT<8192>(4096*n*k);
// y[k]+=sumN* expIFFT<8192>(512*g*k)*
// expIFFT<8192>(64*h*k)*
// expIFFT<8192>(8*j*k)*
// expIFFT<8192>(i*k);
// }
//------------- 7 ---------------
//for(int k=0; k<8192; k++)
// for(int i=0; i<8; i++)
// for(int j=0; j<8; j++)
// for(int h=0; h<8; h++){
// cmplx<double> sumG;
// for(int g=0; g<8; g++){
// cmplx<double> sumN;
// for(int n=0; n<2; n++)
// sumN+= x[4096*n+512*g+64*h+8*j+i]*
// expIFFT<8192>(4096*n*k);
// sumG+= sumN*expIFFT<8192>(512*g*k);
// }
// y[k]+=sumG* expIFFT<8192>(64*h*k)*
// expIFFT<8192>(8*j*k)*
// expIFFT<8192>(i*k);
// }
//------------- 8 ---------------
//for(int k=0; k<8192; k++)
// for(int i=0; i<8; i++)
// for(int j=0; j<8; j++){
// cmplx<double> sumH;
// for(int h=0; h<8; h++){
// cmplx<double> sumG;
// for(int g=0; g<8; g++){
// cmplx<double> sumN;
// for(int n=0; n<2; n++)
// sumN+= x[4096*n+512*g+64*h+8*j+i]*
// expIFFT<8192>(4096*n*k);
// sumG+= sumN*expIFFT<8192>(512*g*k);
// }
// sumH+=sumG* expIFFT<8192>(64*h*k);
// }
// y[k]+=sumH* expIFFT<8192>(8*j*k)*
// expIFFT<8192>(i*k);
// }
//------------- 9 ---------------
//for(int k=0; k<8192; k++){
// for(int i=0; i<8; i++){
// cmplx<double> sumJ;
// for(int j=0; j<8; j++){
// cmplx<double> sumH;
// for(int h=0; h<8; h++){
// cmplx<double> sumG;
// for(int g=0; g<8; g++){
// cmplx<double> sumN;
// for(int n=0; n<2; n++){
// sumN+= x[4096*n+512*g+64*h+8*j+i]*
// expIFFT<8192>(4096*n*k);
// }
// sumG+= sumN*expIFFT<8192>(512*g*k);
// }
// sumH+=sumG* expIFFT<8192>(64*h*k);
// }
// sumJ+=sumH* expIFFT<8192>(8*j*k);
// }
// y[k]+=sumJ* expIFFT<8192>(i*k);
// }
//}
//------------- 10 ---------------
//for(int k=0; k<2; k++){
// for(int i=0; i<8; i++){
// for(int j=0; j<8; j++){
// for(int h=0; h<8; h++){
// for(int g=0; g<8; g++){
// cmplx<double> sumN;
// for(int n=0; n<2; n++){
// sumN+= x[4096*n+512*g+64*h+8*j+i]*
// expIFFT<8192>(4096*n*k);
// }
// N[4096*k+512*g+64*h+8*j+i]=sumN;
// }
// }
// }
// }
//}
//for(int k=0; k<8192; k++){
// for(int i=0; i<8; i++){
// cmplx<double> sumJ;
// for(int j=0; j<8; j++){
// cmplx<double> sumH;
// for(int h=0; h<8; h++){
// cmplx<double> sumG;
// for(int g=0; g<8; g++){
// sumG+= N[4096*(k%2)+512*g+64*h+8*j+i]*expIFFT<8192>(512*g*k);
// }
// sumH+=sumG* expIFFT<8192>(64*h*k);
// }
// sumJ+=sumH* expIFFT<8192>(8*j*k);
// }
// y[k]+=sumJ* expIFFT<8192>(i*k);
// }
//}
//------------- 11 ---------------
//for(int k=0; k<2; k++){
// for(int i=0; i<8; i++){
// for(int j=0; j<8; j++){
// for(int h=0; h<8; h++){
// for(int g=0; g<8; g++){
// cmplx<double> sumN;
// for(int n=0; n<2; n++){
// sumN+= x[4096*n+512*g+64*h+8*j+i]*expIFFT<8192>(4096*n*k);
// }
// N[4096*k+512*g+64*h+8*j+i]=sumN;
// }
// }
// }
// }
//}
//for(int k=0; k<16; k++){
// for(int i=0; i<8; i++){
// for(int j=0; j<8; j++){
// for(int h=0; h<8; h++){
// cmplx<double> sumG;
// for(int g=0; g<8; g++){
// sumG+= N[4096*(k%2)+512*g+64*h+8*j+i]*expIFFT<8192>(512*g*k);
// }
// G[512*(k%16)+64*h+8*j+i]=sumG;
// }
// }
// }
//}
//
//for(int k=0; k<8192; k++){
// for(int i=0; i<8; i++){
// cmplx<double> sumJ;
// for(int j=0; j<8; j++){
// cmplx<double> sumH;
// for(int h=0; h<8; h++){
// sumH+=G[512*(k%16)+64*h+8*j+i]*expIFFT<8192>(64*h*k);
// }
// sumJ+=sumH* expIFFT<8192>(8*j*k);
// }
// y[k]+=sumJ* expIFFT<8192>(i*k);
// }
//}
//------------- 12 ---------------
//for(int k=0; k<2; k++){
// for(int i=0; i<8; i++){
// for(int j=0; j<8; j++){
// for(int h=0; h<8; h++){
// for(int g=0; g<8; g++){
// cmplx<double> sumN;
// for(int n=0; n<2; n++){
// sumN+= x[4096*n+512*g+64*h+8*j+i]*expIFFT<8192>(4096*n*k);
// }
// N[4096*k+512*g+64*h+8*j+i]=sumN;
// }
// }
// }
// }
//}
//for(int k=0; k<16; k++){
// for(int i=0; i<8; i++){
// for(int j=0; j<8; j++){
// for(int h=0; h<8; h++){
// cmplx<double> sumG;
// for(int g=0; g<8; g++){
// sumG+= N[4096*(k%2)+512*g+64*h+8*j+i]*expIFFT<8192>(512*g*k);
// }
// G[512*(k%16)+64*h+8*j+i]=sumG;
// }
// }
// }
//}
//
//for(int k=0; k<128; k++){
// for(int i=0; i<8; i++){
// for(int j=0; j<8; j++){
// cmplx<double> sumH;
// for(int h=0; h<8; h++){
// sumH+=G[512*(k%16)+64*h+8*j+i]*expIFFT<8192>(64*h*k);
// }
// H[64*(k%128)+8*j+i]=sumH;
// }
// }
//}
//
//for(int k=0; k<8192; k++){
// for(int i=0; i<8; i++){
// cmplx<double> sumJ;
// for(int j=0; j<8; j++){
// sumJ+=H[64*(k%128)+8*j+i]* expIFFT<8192>(8*j*k);
// }
// y[k]+=sumJ* expIFFT<8192>(i*k);
// }
//}
//------------- 13 ---------------
//for(int k=0; k<2; k++){
// for(int i=0; i<8; i++){
// for(int j=0; j<8; j++){
// for(int h=0; h<8; h++){
// for(int g=0; g<8; g++){
// cmplx<double> sumN;
// for(int n=0; n<2; n++){
// sumN+= x[4096*n+512*g+64*h+8*j+i]*expIFFT<8192>(4096*n*k);
// }
// N[4096*k+512*g+64*h+8*j+i]=sumN;
// }
// }
// }
// }
//}
//for(int k=0; k<16; k++){
// for(int i=0; i<8; i++){
// for(int j=0; j<8; j++){
// for(int h=0; h<8; h++){
// cmplx<double> sumG;
// for(int g=0; g<8; g++){
// sumG+= N[4096*(k%2)+512*g+64*h+8*j+i]*expIFFT<8192>(512*g*k);
// }
// G[512*(k%16)+64*h+8*j+i]=sumG;
// }
// }
// }
//}
//
//for(int k=0; k<128; k++){
// for(int i=0; i<8; i++){
// for(int j=0; j<8; j++){
// cmplx<double> sumH;
// for(int h=0; h<8; h++){
// sumH+=G[512*(k%16)+64*h+8*j+i]*expIFFT<8192>(64*h*k);
// }
// H[64*(k%128)+8*j+i]=sumH;
// }
// }
//}
//
//for(int k=0; k<1024; k++){
// for(int i=0; i<8; i++){
// cmplx<double> sumJ;
// for(int j=0; j<8; j++){
// sumJ+=H[64*(k%128)+8*j+i]* expIFFT<8192>(8*j*k);
// }
// J[8*(k%1024)+i]=sumJ;
// }
//}
//
//for(int k=0; k<8192; k++){
// for(int i=0; i<8; i++){
// y[k]+=J[8*(k%1024)+i]* expIFFT<8192>(i*k);
// }
//}
//------------- 13 ---------------
//for(int k=0; k<2; k++){
// for(int i=0; i<8; i++){
// for(int j=0; j<8; j++){
// for(int h=0; h<8; h++){
// for(int g=0; g<8; g++){
// for(int n=0; n<2; n++){
// N[4096*k+512*g+64*h+8*j+i]+= x[4096*n+512*g+64*h+8*j+i]*expIFFT<8192>(4096*n*k);
// }
// }
// }
// }
// }
//}
//for(int k=0; k<16; k++){
// for(int i=0; i<8; i++){
// for(int j=0; j<8; j++){
// for(int h=0; h<8; h++){
// for(int g=0; g<8; g++){
// G[512*(k%16)+64*h+8*j+i]+= N[4096*(k%2)+512*g+64*h+8*j+i]*expIFFT<8192>(512*g*k);
// }
// }
// }
// }
//}
//
//for(int k=0; k<128; k++){
// for(int i=0; i<8; i++){
// for(int j=0; j<8; j++){
// for(int h=0; h<8; h++){
// H[64*(k%128)+8*j+i]+=G[512*(k%16)+64*h+8*j+i]*expIFFT<8192>(64*h*k);
// }
// }
// }
//}
//
//for(int k=0; k<1024; k++){
// for(int i=0; i<8; i++){
// for(int j=0; j<8; j++){
// J[8*(k%1024)+i]+=H[64*(k%128)+8*j+i]* expIFFT<8192>(8*j*k);
// }
// }
//}
//
//for(int k=0; k<8192; k++){
// for(int i=0; i<8; i++){
// y[k]+=J[8*(k%1024)+i]* expIFFT<8192>(i*k);
// }
//}
//------------- 14 ---------------
// kk=512*g+64*h+8*j+i
//for(int k=0; k<2; k++){
// for(int kk=0; kk<4096; kk++){
// for(int n=0; n<2; n++){
// N[4096*k+kk]+= x[4096*n+kk]* expIFFT<8192>(4096*n*k);
// }
// }
//}
//for(int k=0; k<16; k++){
// for(int kk=0; kk<512; kk++){
// for(int g=0; g<8; g++){
// G[512*(k%16)+kk]+= N[4096*(k%2)+512*g+kk]* expIFFT<8192>(512*g*k);
// }
// }
//}
//
//for(int k=0; k<128; k++){
// for(int kk=0; kk<64; kk++){
// for(int h=0; h<8; h++){
// H[64*(k%128)+kk]+=G[512*(k%16)+64*h+kk]* expIFFT<8192>(64*h*k);
// }
// }
//}
//
//for(int k=0; k<1024; k++){
// for(int i=0; i<8; i++){
// for(int j=0; j<8; j++){
// J[8*(k%1024)+i]+=H[64*(k%128)+8*j+i]* expIFFT<8192>(8*j*k);
// }
// }
//}
//
//for(int k=0; k<8192; k++){
// for(int i=0; i<8; i++){
// y[k]+=J[8*(k%1024)+i]* expIFFT<8192>(i*k);
// }
//}
//
//------------- 15 ---------------
//for(int k=0; k<2; k++){
// for(int kk=0; kk<4096; kk++){
// N[4096*k+kk]+=X[kk]+X[4096+kk]*expIFFT<8192>(4096*k);
// }
//}
//
//for(int k=0; k<16; k++){
// for(int kk=0; kk<512; kk++){
// for(int g=0; g<8; g++){
// G[512*(k%16)+kk]+= N[4096*(k%2)+512*g+kk]* expIFFT<8192>(512*g*k);
// }
// }
//}
//
//for(int k=0; k<128; k++){
// for(int kk=0; kk<64; kk++){
// for(int h=0; h<8; h++){
// H[64*(k%128)+kk]+=G[512*(k%16)+64*h+kk]* expIFFT<8192>(64*h*k);
// }
// }
//}
//
//for(int k=0; k<1024; k++){
// for(int i=0; i<8; i++){
// for(int j=0; j<8; j++){
// J[8*(k%1024)+i]+=H[64*(k%128)+8*j+i]* expIFFT<8192>(8*j*k);
// }
// }
//}
//
//for(int k=0; k<8192; k++){
// for(int i=0; i<8; i++){
// Y[k]+=J[8*(k%1024)+i]* expIFFT<8192>(i*k);
// }
//}
//=========== Final FFT algorithm: =========================
// ---------------- 0.0 --------------------
for(int k=0; k<2; k++){
for(int kk=0; kk<4096; kk++){
N[4096*k+kk]+=X[kk]+X[4096+kk]*expIFFT<8192>(4096*k);
}
}
// ---------------- 1.0 --------------------
for(int kk=0; kk<512; kk++){
for(int k2=0; k2<2; k2++){
for(int k1=0; k1<8; k1++){ // rep 8
int k=k1*2+k2; // for(int k=0; k<16; k++)
for(int g=0; g<8; g++){ // vsum
G[1024*k1+512*k2+kk]+= N[4096*k2+512*g+kk]* expIFFT<8192>(512*g*k);
}
}
}
}
// ---------------- 2.0 --------------------
for(int kk=0; kk<64; kk++){
for(int k2=0; k2<16; k2++){
for(int k1=0; k1<8; k1++){ // rep 8
int k=k1*16+k2; // for(int k=0; k<128; k++){
for(int h=0; h<8; h++){ // vsum
H[1024*k1+64*k2+kk]+=G[512*k2+64*h+kk]* expIFFT<8192>(64*h*k);
}
}
}
}
// ---------------- 3.0 --------------------
for(int k2=0; k2<128; k2++){
for(int k1=0; k1<8; k1++){
for(int i=0; i<8; i++){ // rep 8
int k=k1*128+k2; // for(int k=0; k<1024; k++){
for(int j=0; j<8; j++){ // vsum
J[1024*k1+8*k2+i]+=H[64*k2+8*j+i]*expIFFT<8192>(8*j*k);
}
}
}
}
// ---------------- 4.0 --------------------
for(int k2=0; k2<1024; k2++){
for(int k1=0; k1<8; k1++){ // rep 8
int k=1024*k1+k2; // for(int k=0; k<8192; k++){
for(int i=0; i<8; i++){ // vsum
Y[1024*k1+k2]+=J[8*k2+i]*expIFFT<8192>(i*k);
}
}
}
for(int i=0; i<8192; i++){
dst[i].re=floor(Y[i].re/8192+0.5);
dst[i].im=floor(Y[i].im/8192+0.5);
}
return 0;
}
int maximalRectangle(vector<vector<char> > &matrix) {
// Start typing your C/C++ solution below
// DO NOT write int main() function
if(matrix.empty())
return 0;
int m = matrix.size();
int n = matrix[0].size();
vector< vector<area> > D(m+1, vector<area>(n+1)); // maximum area with matrix[i][j] as the top left element
vector< vector<int> > W(m, vector<int> (n, 0)); // maximum single line area starting with matrix[i][j]
vector< vector<int> > H(m, vector<int> (n, 0)); //maximum single column area starting with matrix[i][j]
int q;
int i, j;
int gmax = 0;
for(i = 0; i < m; i++){
q = 0;
for(j = n-1; j >= 0; j--){
if(matrix[i][j] == '0'){
W[i][j] = 0;
}
else{
W[i][j] = 1 + q;
}
q = W[i][j];
}
}
for(j = 0; j < n; j++){
q = 0;
for(i = m-1; i >= 0; i--){
if(matrix[i][j] == '0'){
H[i][j] = 0;
}
else{
H[i][j] = 1 + q;
}
q = H[i][j];
}
}
for(i = 0; i <= m; i++)
D[i][n].w = D[i][n].h = 0;
for(j = 0; j <= n; j++)
D[m][j].w = D[m][j].h = 0;
int cur;
for(i = m-1; i >= 0; i--){
for(j = n-1; j >= 0; j--){
//compute D
if(matrix[i][j] == '0')
D[i][j].w = D[i][j].h = 0;
else{
D[i][j].w = W[i][j];
D[i][j].h = 1;
cur = H[i][j];
if(W[i][j] < H[i][j]){
D[i][j].h = H[i][j];
D[i][j].w = 1;
}
cur = min(W[i][j], D[i+1][j].w) * (1 + D[i+1][j].h);
if(cur > D[i][j].w * D[i][j].h){
D[i][j].w = min(W[i][j], D[i+1][j].w);
D[i][j].h = 1 + D[i+1][j].h;
}
cur = min(H[i][j], D[i][j+1].h) * (1 + D[i][j+1].w);
if(cur > D[i][j].w * D[i][j].h){
D[i][j].w = 1 + D[i][j+1].w;
D[i][j].h = min(H[i][j], D[i][j+1].h);
}
}
gmax = max(gmax, D[i][j].w * D[i][j].h);
}
}
return gmax;
}
void softAbsMetric::checkEvolution(const double epsilon)
{
baseHamiltonian::checkEvolution(epsilon);
// Hessian
std::cout.precision(6);
int width = 12;
int nColumn = 5;
std::cout << "Potential Hessian (d^{2}V/dq^{i}dq^{j}):" << std::endl;
std::cout << " " << std::setw(nColumn * width) << std::setfill('-') << "" << std::setfill(' ') << std::endl;
std::cout << " "
<< std::setw(width) << std::left << "Row"
<< std::setw(width) << std::left << "Column"
<< std::setw(width) << std::left << "Analytic"
<< std::setw(width) << std::left << "Finite"
<< std::setw(width) << std::left << "Delta / "
<< std::endl;
std::cout << " "
<< std::setw(width) << std::left << "(i)"
<< std::setw(width) << std::left << "(j)"
<< std::setw(width) << std::left << "Derivative"
<< std::setw(width) << std::left << "Difference"
<< std::setw(width) << std::left << "Stepsize^{2}"
<< std::endl;
std::cout << " " << std::setw(nColumn * width) << std::setfill('-') << "" << std::setfill(' ') << std::endl;
fComputeH();
for(int i = 0; i < mDim; ++i)
{
VectorXd temp = VectorXd::Zero(mDim);
mQ(i) += epsilon;
temp += gradV();
mQ(i) -= 2.0 * epsilon;
temp -= gradV();
mQ(i) += epsilon;
temp /= 2.0 * epsilon;
for(int j = 0; j < mDim; ++j)
{
std::cout << " "
<< std::setw(width) << std::left << i
<< std::setw(width) << std::left << j
<< std::setw(width) << std::left << mH(i, j)
<< std::setw(width) << std::left << temp(j)
<< std::setw(width) << std::left << (mH(i, j) - temp(j)) / (epsilon * epsilon)
<< std::endl;
}
}
std::cout << " " << std::setw(nColumn * width) << std::setfill('-') << "" << std::setfill(' ') << std::endl;
std::cout << std::endl;
// Gradient of the Hessian
std::cout.precision(6);
width = 12;
nColumn = 6;
std::cout << "Gradient of the Hessian (d^{3}V/dq^{i}dq^{j}dq^{k}):" << std::endl;
std::cout << " " << std::setw(nColumn * width) << std::setfill('-') << "" << std::setfill(' ') << std::endl;
std::cout << " "
<< std::setw(width) << std::left << "Component"
<< std::setw(width) << std::left << "Row"
<< std::setw(width) << std::left << "Column"
<< std::setw(width) << std::left << "Analytic"
<< std::setw(width) << std::left << "Finite"
<< std::setw(width) << std::left << "Delta /"
<< std::endl;
std::cout << " "
<< std::setw(width) << std::left << "(i)"
<< std::setw(width) << std::left << "(j)"
<< std::setw(width) << std::left << "(k)"
<< std::setw(width) << std::left << "Derivative"
<< std::setw(width) << std::left << "Difference"
<< std::setw(width) << std::left << "Stepsize^{2}"
<< std::endl;
std::cout << " " << std::setw(nColumn * width) << std::setfill('-') << "" << std::setfill(' ') << std::endl;
for(int k = 0; k < mDim; ++k)
{
mAuxMatrixOne.setZero();
mQ(k) += epsilon;
fComputeH();
mAuxMatrixOne += mH;
mQ(k) -= 2.0 * epsilon;
fComputeH();
mAuxMatrixOne -= mH;
mQ(k) += epsilon;
mAuxMatrixOne /= 2.0 * epsilon;
fComputeGradH(k);
for(int i = 0; i < mDim; ++i)
{
for(int j = 0; j < mDim; ++j)
{
std::cout << " "
<< std::setw(width) << std::left << k
<< std::setw(width) << std::left << i
<< std::setw(width) << std::left << j
<< std::setw(width) << std::left << mGradH.block(0, k * mDim, mDim, mDim)(i, j)
<< std::setw(width) << std::left << mAuxMatrixOne(i, j)
<< std::setw(width) << std::left << (mGradH.block(0, k * mDim, mDim, mDim)(i, j) - mAuxMatrixOne(i, j)) / (epsilon * epsilon)
<< std::endl;
}
}
}
std::cout << " " << std::setw(nColumn * width) << std::setfill('-') << "" << std::setfill(' ') << std::endl;
std::cout << std::endl;
// Metric
std::cout.precision(6);
width = 12;
nColumn = 6;
std::cout << "Gradient of the metric (dLambda^{jk}/dq^{i}):" << std::endl;
std::cout << " " << std::setw(nColumn * width) << std::setfill('-') << "" << std::setfill(' ') << std::endl;
std::cout << " "
<< std::setw(width) << std::left << "Component"
<< std::setw(width) << std::left << "Row"
<< std::setw(width) << std::left << "Column"
<< std::setw(width) << std::left << "Analytic"
<< std::setw(width) << std::left << "Finite"
<< std::setw(width) << std::left << "Delta /"
<< std::endl;
std::cout << " "
<< std::setw(width) << std::left << "(i)"
<< std::setw(width) << std::left << "(j)"
<< std::setw(width) << std::left << "(k)"
<< std::setw(width) << std::left << "Derivative"
<< std::setw(width) << std::left << "Difference"
<< std::setw(width) << std::left << "Epsilon^{2}"
<< std::endl;
std::cout << " " << std::setw(nColumn * width) << std::setfill('-') << "" << std::setfill(' ') << std::endl;
fComputeMetric();
fPrepareSpatialGradients();
for(int k = 0; k < mDim; ++k)
{
// Approximate metric gradient
MatrixXd temp = MatrixXd::Zero(mDim, mDim);
MatrixXd G = MatrixXd::Zero(mDim, mDim);
mQ(k) += epsilon;
fComputeMetric();
G.noalias() = mEigenDeco.eigenvectors() * mSoftAbsLambda.asDiagonal() * mEigenDeco.eigenvectors().transpose();
temp += G;
mQ(k) -= 2.0 * epsilon;
fComputeMetric();
G.noalias() = mEigenDeco.eigenvectors() * mSoftAbsLambda.asDiagonal() * mEigenDeco.eigenvectors().transpose();
temp -= G;
mQ(k) += epsilon;
temp /= 2.0 * epsilon;
// Exact metric gradient
fComputeMetric();
fComputeGradH(k);
mAuxMatrixOne.noalias() = mGradH.block(0, k * mDim, mDim, mDim) * mEigenDeco.eigenvectors();
mAuxMatrixTwo.noalias() = mEigenDeco.eigenvectors().transpose() * mAuxMatrixOne;
mAuxMatrixOne.noalias() = mPseudoJ.cwiseProduct(mAuxMatrixTwo);
mCacheMatrix.noalias() = mAuxMatrixOne * mEigenDeco.eigenvectors().transpose();
MatrixXd gradG = mEigenDeco.eigenvectors() * mCacheMatrix;
// Compare
for(int i = 0; i < mDim; ++i)
{
for(int j = 0; j < mDim; ++j)
{
std::cout << " "
<< std::setw(width) << std::left << k
<< std::setw(width) << std::left << i
<< std::setw(width) << std::left << j
<< std::setw(width) << std::left << gradG(i, j)
<< std::setw(width) << std::left << temp(i, j)
<< std::setw(width) << std::left << (gradG(i, j) - temp(i, j)) / (epsilon * epsilon)
<< std::endl;
}
}
}
std::cout << " " << std::setw(nColumn * width) << std::setfill('-') << "" << std::setfill(' ') << std::endl;
std::cout << std::endl;
// Hamiltonian
VectorXd gradH = dTaudq();
gradH += dPhidq();
std::cout << "pDot (-dH/dq^{i}):" << std::endl;
std::cout << " " << std::setw(nColumn * width) << std::setfill('-') << "" << std::setfill(' ') << std::endl;
std::cout << " "
<< std::setw(width) << std::left << "Component"
<< std::setw(width) << std::left << "Analytic"
<< std::setw(width) << std::left << "Finite"
<< std::setw(width) << std::left << "Delta /"
<< std::endl;
std::cout << " "
<< std::setw(width) << std::left << "(i)"
<< std::setw(width) << std::left << "Derivative"
<< std::setw(width) << std::left << "Difference"
<< std::setw(width) << std::left << "Epsilon^{2}"
<< std::endl;
std::cout << " " << std::setw(nColumn * width) << std::setfill('-') << "" << std::setfill(' ') << std::endl;
for(int i = 0; i < mDim; ++i)
{
// Finite Differences
double temp = 0.0;
mQ(i) += epsilon;
temp -= H();
mQ(i) -= 2.0 * epsilon;
temp += H();
mQ(i) += epsilon;
temp /= 2.0 * epsilon;
// Exact
double minusGradH = -gradH(i);
std::cout << " "
<< std::setw(width) << std::left << i
<< std::setw(width) << std::left << minusGradH
<< std::setw(width) << std::left << temp
<< std::setw(width) << std::left << (minusGradH - temp) / (epsilon * epsilon)
<< std::endl;
}
std::cout << " " << std::setw(nColumn * width) << std::setfill('-') << "" << std::setfill(' ') << std::endl;
std::cout << std::endl;
}
bool SolverSLAM2DLinear::solveOrientation()
{
assert(_optimizer->indexMapping().size() + 1 == _optimizer->vertices().size() && "Needs to operate on full graph");
assert(_optimizer->vertex(0)->fixed() && "Graph is not fixed by vertex 0");
VectorXD b, x; // will be used for theta and x/y update
b.setZero(_optimizer->indexMapping().size());
x.setZero(_optimizer->indexMapping().size());
typedef Eigen::Matrix<double, 1, 1, Eigen::ColMajor> ScalarMatrix;
ScopedArray<int> blockIndeces(new int[_optimizer->indexMapping().size()]);
for (size_t i = 0; i < _optimizer->indexMapping().size(); ++i)
blockIndeces[i] = i+1;
SparseBlockMatrix<ScalarMatrix> H(blockIndeces.get(), blockIndeces.get(), _optimizer->indexMapping().size(), _optimizer->indexMapping().size());
// building the structure, diagonal for each active vertex
for (size_t i = 0; i < _optimizer->indexMapping().size(); ++i) {
OptimizableGraph::Vertex* v = _optimizer->indexMapping()[i];
int poseIdx = v->hessianIndex();
ScalarMatrix* m = H.block(poseIdx, poseIdx, true);
m->setZero();
}
HyperGraph::VertexSet fixedSet;
// off diagonal for each edge
for (SparseOptimizer::EdgeContainer::const_iterator it = _optimizer->activeEdges().begin(); it != _optimizer->activeEdges().end(); ++it) {
# ifndef NDEBUG
EdgeSE2* e = dynamic_cast<EdgeSE2*>(*it);
assert(e && "Active edges contain non-odometry edge"); //
# else
EdgeSE2* e = static_cast<EdgeSE2*>(*it);
# endif
OptimizableGraph::Vertex* from = static_cast<OptimizableGraph::Vertex*>(e->vertices()[0]);
OptimizableGraph::Vertex* to = static_cast<OptimizableGraph::Vertex*>(e->vertices()[1]);
int ind1 = from->hessianIndex();
int ind2 = to->hessianIndex();
if (ind1 == -1 || ind2 == -1) {
if (ind1 == -1) fixedSet.insert(from); // collect the fixed vertices
if (ind2 == -1) fixedSet.insert(to);
continue;
}
bool transposedBlock = ind1 > ind2;
if (transposedBlock){ // make sure, we allocate the upper triangle block
std::swap(ind1, ind2);
}
ScalarMatrix* m = H.block(ind1, ind2, true);
m->setZero();
}
// walk along the Minimal Spanning Tree to compute the guess for the robot orientation
assert(fixedSet.size() == 1);
VertexSE2* root = static_cast<VertexSE2*>(*fixedSet.begin());
VectorXD thetaGuess;
thetaGuess.setZero(_optimizer->indexMapping().size());
UniformCostFunction uniformCost;
HyperDijkstra hyperDijkstra(_optimizer);
hyperDijkstra.shortestPaths(root, &uniformCost);
HyperDijkstra::computeTree(hyperDijkstra.adjacencyMap());
ThetaTreeAction thetaTreeAction(thetaGuess.data());
HyperDijkstra::visitAdjacencyMap(hyperDijkstra.adjacencyMap(), &thetaTreeAction);
// construct for the orientation
for (SparseOptimizer::EdgeContainer::const_iterator it = _optimizer->activeEdges().begin(); it != _optimizer->activeEdges().end(); ++it) {
EdgeSE2* e = static_cast<EdgeSE2*>(*it);
VertexSE2* from = static_cast<VertexSE2*>(e->vertices()[0]);
VertexSE2* to = static_cast<VertexSE2*>(e->vertices()[1]);
double omega = e->information()(2,2);
double fromThetaGuess = from->hessianIndex() < 0 ? 0. : thetaGuess[from->hessianIndex()];
double toThetaGuess = to->hessianIndex() < 0 ? 0. : thetaGuess[to->hessianIndex()];
double error = normalize_theta(-e->measurement().rotation().angle() + toThetaGuess - fromThetaGuess);
bool fromNotFixed = !(from->fixed());
bool toNotFixed = !(to->fixed());
if (fromNotFixed || toNotFixed) {
double omega_r = - omega * error;
if (fromNotFixed) {
b(from->hessianIndex()) -= omega_r;
(*H.block(from->hessianIndex(), from->hessianIndex()))(0,0) += omega;
if (toNotFixed) {
if (from->hessianIndex() > to->hessianIndex())
(*H.block(to->hessianIndex(), from->hessianIndex()))(0,0) -= omega;
else
(*H.block(from->hessianIndex(), to->hessianIndex()))(0,0) -= omega;
}
}
if (toNotFixed ) {
b(to->hessianIndex()) += omega_r;
(*H.block(to->hessianIndex(), to->hessianIndex()))(0,0) += omega;
}
}
}
// solve orientation
typedef LinearSolverCSparse<ScalarMatrix> SystemSolver;
SystemSolver linearSystemSolver;
linearSystemSolver.init();
bool ok = linearSystemSolver.solve(H, x.data(), b.data());
if (!ok) {
cerr << __PRETTY_FUNCTION__ << "Failure while solving linear system" << endl;
return false;
}
// update the orientation of the 2D poses and set translation to 0, GN shall solve that
root->setToOrigin();
for (size_t i = 0; i < _optimizer->indexMapping().size(); ++i) {
VertexSE2* v = static_cast<VertexSE2*>(_optimizer->indexMapping()[i]);
int poseIdx = v->hessianIndex();
SE2 poseUpdate(0, 0, normalize_theta(thetaGuess(poseIdx) + x(poseIdx)));
v->setEstimate(poseUpdate);
}
return true;
}