void plotMVALeadVsLeadCharg(){
TFile* f_Ztautau = new TFile("../");
f_Ztautau->cd();
TTree* tree_Ztautau = (TTree*) gDirectory->Get("tauFakeRateAnalyzerHPS/tree");
TH2F* h2 = new TH2F("h2","",220,0,1.1,220,0,1.1);
float vxF[45];
float vyF[45];
float ZtautauAll = (float)tree_Ztautau->GetEntries("");
for(int i = 0; i<=44; i++){
float cut = 0.025*i;
float ZtautauCutLeadPi = (float) tree_Ztautau->GetEntries(Form("leadPFCandMva<=%f",-0.1+cut));
float ZtautauCutLeadCh = (float) tree_Ztautau->GetEntries(Form("leadPFChargedHadrMva<=%f",-0.1+cut));
vxF[i]=ZtautauCutLeadCh/ZtautauAll;
vyF[i]=ZtautauCutLeadPi/ZtautauAll;
}
h2->SetXTitle("efficiency of mva<X cut on lead charged");
h2->SetYTitle("efficiency of mva<X cut on lead candidate");
h2->SetAxisRange(0.95,1.02,"X");
h2->SetAxisRange(0.95,1.02,"Y");
h2->Draw();
TVectorF vx(45,vxF);
TVectorF vy(45,vyF);
TGraph* graph = new TGraph(vx,vy);
graph->SetMarkerStyle(kFullCircle);
graph->SetMarkerColor(kRed);
graph->SetMarkerSize(1.0);
graph->Draw("P");
TF1* line = new TF1("line","x",0.9,1.2);
line->Draw("SAME");
}
PetscErrorCode feel_petsc_post_solve(KSP ksp,Vec x,Vec y,void* ctx)
{
BackendPetsc<double> * b = static_cast<BackendPetsc<double>*> ( ctx );
LOG(INFO) << "call feel_petsc_post_solve";
if ( b->postSolve() )
{
vector_ptrtype vx( vec( x, b->dataMap() ) );
vector_ptrtype vy( vec( y, b->dataMap() ) );
b->postSolve( vx, vy );
}
else
{
LOG(WARNING) << "call feel_petsc_post_solve without post solve function, we duplicate the input into the output";
auto e = VecDuplicate(x, &y);
CHKERRABORT( b->comm().globalComm(), e );
e = VecCopy(x, y);
CHKERRABORT( b->comm().globalComm(), e );
}
LOG(INFO) << "call feel_petsc_post_solve done";
return 0;
}
void MAIN(void) {
Quaternion lage(1, 0,0,0);
Vector3D vx(1,0,0), v;
double giroX, giroY, giroZ;
Quaternion deltaLage;
YPR ypr;
for(int i = 0; i <100; i++) { // shall be: while(1)
ypr = lage.toYPR();
v = vx.qRotate(lage);
PRINTF("------------ Step %d\n", i);
PRINTF("Currten attitude quat:");
lage.print();
PRINTF("Currten attitude v :");
v.print();
PRINTF("Currten attitude ypr :");
ypr.print();
ypr = lage.toYPRnils();
PRINTF("Currten attitude yprN:");
ypr.print();
readGyro(giroX, giroY, giroZ);
ypr = YPR(giroX, giroY, giroZ);
PRINTF("gyro-ypr :\t\t ");
ypr.print();
deltaLage = ypr.toQuaternion().normalize();
PRINTF("gyro-quat:");
deltaLage.print();
lage = deltaLage * lage; // operator* for quaternions
}
PRINTF("----------- Test END -------------------\n");
}
void test_gridDiff2Cubic(/* Real */ ae_vector* x,
/* Real */ ae_vector* y,
ae_int_t n,
/* Real */ ae_vector* d1,
/* Real */ ae_vector* d2,
ae_state *_state)
{
//fprintf(stderr, "===== test =====\n");
ae_frame _frame_block;
ae_frame_make(_state, &_frame_block);
std::vector<double> vx(n), vy(n);
for (size_t i=0; i<n; ++i) {
vx[i] = x->ptr.p_double[i];
vy[i] = y->ptr.p_double[i];
//fprintf(stderr, "i=%d, vx=%f, vy=%f\n", i, vx[i], vy[i]);
}
//fprintf(stderr, "*** 222 \n");
std::vector<double> vd1(n), vd2(n);
spline1d::gridDiff2Cubic(vx, vy, vd1, vd2);
ae_vector_set_length(d1, n, _state);
ae_vector_set_length(d2, n, _state);
//fprintf(stderr, "*** 333 \n");
for (int i=0; i<n; ++i) {
//fprintf(stderr, "i=%d, d1=%f, vd1=%f\n", i, d1->ptr.p_double[i], vd1[i]);
//fprintf(stderr, " d2=%f, vd2=%f\n", i, d2->ptr.p_double[i], vd2[i]);
d1->ptr.p_double[i] = vd1[i];
d2->ptr.p_double[i] = vd2[i];
}
ae_frame_leave(_state);
//fprintf(stderr, "===== test end =====\n");
}
inline std::basic_ostream<Char,Traits>&
operator <<(std::basic_ostream<Char,Traits>& os, const BoolVar& x) {
Int::BoolView vx(x);
return os << vx;
}
void relaxation_time(){
int x, y, pp, i, j;
my_double u, v;
my_double invtau, rho;
my_double cu, u2;
pop p_eq;
my_double tau0;
my_double S[2][2], gamma_dot,eps;
my_double nu0 = (tau1-0.5)/3.0;
my_double nu00 = 0.5*(tau1-0.5)/3.0;
my_double lambda = 1.0;
my_double nindex = 0.1;
my_double B, tau_min;
for (y=1; y<NY+1; y++){
for (x=1; x<NX+1; x++){
rho = m(p[IDX(y,x)]);
u = vx(p[IDX(y,x)])/rho;
v = vy(p[IDX(y,x)])/rho;
u2 = u*u + v*v;
tau0 = tau[y][x];
/* equilibrium distribution */
for (pp=0; pp<9; pp++){
cu = (cx[pp]*u + cy[pp]*v);
p_eq.p[pp] = rho * wgt[pp] * (1.0 + 3.0*cu + 4.5*cu*cu - 1.5*u2 );
}
S[0][0] = S[0][1] = S[1][0] = S[1][1] = 0.0;
for (pp=0; pp<9; pp++){
S[0][0] += cx[pp]*cx[pp]*(p[IDX(y,x)].p[pp] - p_eq.p[pp]);
S[0][1] += cx[pp]*cy[pp]*(p[IDX(y,x)].p[pp] - p_eq.p[pp]);
S[1][0] += cy[pp]*cx[pp]*(p[IDX(y,x)].p[pp] - p_eq.p[pp]);
S[1][1] += cy[pp]*cy[pp]*(p[IDX(y,x)].p[pp] - p_eq.p[pp]);
}
//fprintf(stderr,"Sxx %g, Sxy %g, Syx %g, Syy %g\n", Sxx, Sxy, Syx, Syy);fflush(stderr);
/* shear rate */
eps = 0.0;
for (i=0; i<3; i++)
for (j=0; j<3; j++){
eps += S[i][j]*S[i][j];
}
gamma_dot = (1.0/(2.0*cs2*rho*tau0))*sqrt(eps);
/* fprintf(stderr,"gamma_dot %g, eps %e\n", gamma_dot, eps);fflush(stderr); */
/* Carreau-Yasuda model , has parameters nu0, nu00 , lambda, nindex */
#ifdef FLUID_RHEOLOGY_CARREAU
tau[IDX(y,x)]=((nu0 - nu00)/cs2) * pow((1.0 + pow(lambda*gamma_dot,2.0) ),(nindex-1.0)/2.0) + nu00/cs2 + 0.5;
#endif
/* Power law model */
#ifdef FLUID_RHEOLOGY_POWER_LAW
tau[IDX(y,x)]= (nu0/cs2) * pow(gamma_dot, nindex-1.0 ) + 0.5;
#endif
/* if( tau[y][x] != tau0 ) fprintf(stderr,"tau %e tau0 %e\n", tau[y][x],tau0); */
}
}
#ifdef TEMP_REHOLOGY
tau_min = 2./3.;
B = deltaT;
nu0 = (tau_min-0.5)*cs2;
nu0 = nu0*exp(B/tt[IDX(y,x)]);
for (y=1; y<NY+1; y++)
for (x=1; x<NX+1; x++){
tau[IDX(y,x)] = nu0*exp(-B/tt[IDX(y,x)])/cs2 + 0.5;
}
#endif
}
int pick_major_axis(
vector<pair<Pmwx::Halfedge_handle, Pmwx::Halfedge_handle> >& sides, // Per side: inclusive range of half-edges "consolidated" into the sides.
Polygon2& bounds, // Inset boundary in metric, first side matched to the list.
Vector2& v_x,
Vector2& v_y)
{
// special case: if we find a block with exactly ONE right angle, the longer of the two
// sides going into the right angle is hte major axis, full stop, we're done.
int right_angle = -1;
for(int i = 0; i < sides.size(); ++i)
{
int j = (i + 1) % sides.size();
int k = (i + 2) % sides.size();
Vector2 vx(Vector2(bounds[i],bounds[j]));
Vector2 vy(Vector2(bounds[j],bounds[k]));
vx.normalize();
vy.normalize();
double dot = fabs(vx.dot(vy));
if(dot < 0.087155742747658)
{
if(right_angle == -1)
right_angle = j;
else
right_angle = -2; // "more than one right angle" flag - causes us to NOT try this algo.
}
}
if(right_angle >= 0)
{
int prev = (right_angle + sides.size() - 1) % sides.size();
int next = (right_angle + 1) % sides.size();
Vector2 vp(Vector2(bounds[prev],bounds[right_angle]));
Vector2 vn(Vector2(bounds[right_angle],bounds[next]));
double pl = vp.normalize();
double nl = vn.normalize();
if(pl > nl)
v_x = vp;
else
v_x = vn;
v_y = v_x.perpendicular_ccw();
return right_angle;
}
// THIS is the algo we shipped with - it tries to minimize the short side axis of the block. This works
// okay but tends to make the diagonal of diagonal cuts (like Broadway) the main axis since (by the pythag
// theorem) that slightly reduces the block depth.
#if 0
int shortest = -1;
double thinnest_so_far = 0.0;
for(int i = 0; i < sides.size(); ++i)
{
Vector2 vx = Vector2(bounds.side(i).p1,bounds.side(i).p2);
vx.normalize();
Vector2 vy = vx.perpendicular_ccw();
double bbox[4];
bbox[0] = bbox[2] = vx.dot(Vector2(bounds[0]));
bbox[1] = bbox[3] = vy.dot(Vector2(bounds[0]));
for(int j = 0; j < sides.size(); ++j)
{
double x = vx.dot(Vector2(bounds[j]));
double y = vy.dot(Vector2(bounds[j]));
bbox[0] = dobmin2(bbox[0], x);
bbox[1] = dobmin2(bbox[1], y);
bbox[2] = dobmax2(bbox[2], x);
bbox[3] = dobmax2(bbox[3], y);
}
double xdist = fabs(bbox[2]-bbox[0]);
double ydist = fabs(bbox[3]-bbox[1]);
double my_dist = dobmin2(xdist,ydist);
if(shortest == -1 || my_dist < thinnest_so_far)
{
shortest = i;
thinnest_so_far = my_dist;
if(xdist < ydist)
{
v_x = vx.perpendicular_ccw();
v_y = vy.perpendicular_ccw();
}
else
{
v_x = vx;
v_y = vy;
}
}
}
DebugAssert(shortest >= 0);
return shortest;
#endif
#if 1
// #error This algo works 95% of the time, but 5% of the time it picks a slashed short end as the
// #error long axis, which gives a long thin block a wrong axis alignment and a huge AABB. Bad!
// The basic idea: we want to pick the grid axis MOST aligned with the block such that
// the major axis supports roads.
double best_corr = 0;
double best = -1;
Vector2 best_vec;
bool elev_ok = false;
int i, j, tries;
for(tries = 0; tries < 2; ++tries)
{
for(i = 0; i < sides.size(); ++i)
if(elev_ok || ground_road_access_for_he(sides[i].first))
{
double score = 0.0;
Vector2 si = Vector2(bounds.side(i).p1,bounds.side(i).p2);
si.normalize();
Vector2 si_n = si.perpendicular_ccw();
for(int j = 0; j < sides.size(); ++j)
if(elev_ok || ground_road_access_for_he(sides[j].first))
{
Vector2 sj(bounds.side(j).p1,bounds.side(j).p2);
double my_corr = fltmax2(fabs(si.dot(sj)),fabs(si_n.dot(sj)));
score += my_corr;
}
if(score > best_corr){
best = i;
best_corr = score;
best_vec = si;
}
}
if(best >= 0)
break;
elev_ok = true;
}
if(best >= 0)
{
Vector2 best_vec_n = best_vec.perpendicular_ccw();
int longest = -1;
double corr_len = -1;
for(int i = 0; i < sides.size(); ++i)
if(/*elev_ok ||*/ ground_road_access_for_he(sides[i].first))
{
Vector2 this_side(bounds.side(i).p1,bounds.side(i).p2);
double len = this_side.normalize();
double my_corr = fltmax2(fabs(best_vec.dot(this_side)), fabs(best_vec_n.dot(this_side)));
if(my_corr > 0.996194698091746)
{
my_corr *= len;
if(my_corr > corr_len)
{
longest = i;
corr_len = my_corr;
}
}
}
if(longest >= 0)
best = longest;
}
v_x = Vector2(bounds.side(best).p1,bounds.side(best).p2);
v_x.normalize();
v_y = v_x.perpendicular_ccw();
//printf("So far our best is %d, with axes %lf, %lf to %lf, %lf\n", best, v_x.dx,v_x.dy,v_y.dx,v_y.dy);
double bbox[4];
bbox[0] = bbox[2] = v_x.dot(Vector2(bounds[0]));
bbox[1] = bbox[3] = v_y.dot(Vector2(bounds[0]));
for(i = 1; i < sides.size(); ++i)
{
double va = v_x.dot(Vector2(bounds[i]));
double vb = v_y.dot(Vector2(bounds[i]));
bbox[0]=dobmin2(bbox[0], va);
bbox[1]=dobmin2(bbox[1], vb);
bbox[2]=dobmax2(bbox[2], va);
bbox[3]=dobmax2(bbox[3], vb);
}
//printf("Our bbox is %lf,%lf to %lf,%lf\n", bbox[0],bbox[1],bbox[2],bbox[3]);
if(0)
if((bbox[2] - bbox[0]) < (bbox[3] - bbox[1]))
{
v_x = v_x.perpendicular_ccw();
v_y = v_y.perpendicular_ccw();
//printf("Must rotate, the winner was a SHORT side.\n");
}
// best = 0;
// double best_dot = 0;
// for(i = 0; i < sides.size(); ++i)
// if(ground_road_access_for_he(sides[i].first))
// {
// Vector2 side_vec(bounds.side(i).p1,bounds.side(i).p2);
// double slen = side_vec.normalize();
// double corr = fabs(v_x.dot(side_vec));
// if(corr > best_dot)
// {
// best = i;
// corr = best_dot;
// }
// }
// DebugAssert(best >= 0.0);
// v_x = Vector2(bounds.side(best).p1,bounds.side(best).p2);
// v_x.normalize();
// v_y = v_x.perpendicular_ccw();
return best;
#endif
}
inline std::basic_ostream<Char,Traits>&
operator <<(std::basic_ostream<Char,Traits>& os, const SetVar& x) {
Gecode::Set::SetView vx(x);
return os << vx;
}
/*! \brief Implementation routine to compare SIMD vs reference functions.
*
* \param refFuncExpr Description of reference function expression
* \param simdFuncExpr Description of SIMD function expression
* \param refFunc Reference math function pointer
* \param simdFunc SIMD math function pointer
*
* The function will be tested with the range and tolerances specified in
* the SimdBaseTest class. You should not never call this function directly,
* but use the macro GMX_EXPECT_SIMD_FUNC_NEAR(refFunc,tstFunc) instead.
*/
::testing::AssertionResult
SimdMathTest::compareSimdMathFunction(const char * refFuncExpr, const char *simdFuncExpr,
real refFunc(real x), SimdReal gmx_simdcall simdFunc(SimdReal x))
{
std::vector<real> vx(GMX_SIMD_REAL_WIDTH);
std::vector<real> vref(GMX_SIMD_REAL_WIDTH);
std::vector<real> vtst(GMX_SIMD_REAL_WIDTH);
real dx, absDiff;
std::int64_t ulpDiff, maxUlpDiff;
real maxUlpDiffPos;
real refValMaxUlpDiff, simdValMaxUlpDiff;
bool absOk, signOk;
int i, iter;
int niter = s_nPoints/GMX_SIMD_REAL_WIDTH;
int npoints = niter*GMX_SIMD_REAL_WIDTH;
# if GMX_DOUBLE
union {
double r; std::int64_t i;
} conv0, conv1;
# else
union {
float r; std::int32_t i;
} conv0, conv1;
# endif
maxUlpDiff = 0;
dx = (range_.second-range_.first)/npoints;
for (iter = 0; iter < niter; iter++)
{
for (i = 0; i < GMX_SIMD_REAL_WIDTH; i++)
{
vx[i] = range_.first+dx*(iter*GMX_SIMD_REAL_WIDTH+i);
vref[i] = refFunc(vx[i]);
}
vtst = simdReal2Vector(simdFunc(vector2SimdReal(vx)));
for (i = 0, signOk = true, absOk = true; i < GMX_SIMD_REAL_WIDTH; i++)
{
absDiff = fabs(vref[i]-vtst[i]);
absOk = absOk && ( absDiff < absTol_ );
signOk = signOk && ( (vref[i] >= 0 && vtst[i] >= 0) ||
(vref[i] <= 0 && vtst[i] <= 0));
if (absDiff >= absTol_)
{
/* We replicate the trivial ulp differences comparison here rather than
* calling the lower-level routine for comparing them, since this enables
* us to run through the entire test range and report the largest deviation
* without lots of extra glue routines.
*/
conv0.r = vref[i];
conv1.r = vtst[i];
ulpDiff = llabs(conv0.i-conv1.i);
if (ulpDiff > maxUlpDiff)
{
maxUlpDiff = ulpDiff;
maxUlpDiffPos = vx[i];
refValMaxUlpDiff = vref[i];
simdValMaxUlpDiff = vtst[i];
}
}
}
if ( (absOk == false) && (signOk == false) )
{
return ::testing::AssertionFailure()
<< "Failing SIMD math function comparison due to sign differences." << std::endl
<< "Reference function: " << refFuncExpr << std::endl
<< "Simd function: " << simdFuncExpr << std::endl
<< "Test range is ( " << range_.first << " , " << range_.second << " ) " << std::endl
<< "First sign difference around x=" << std::setprecision(20) << ::testing::PrintToString(vx) << std::endl
<< "Ref values: " << std::setprecision(20) << ::testing::PrintToString(vref) << std::endl
<< "SIMD values: " << std::setprecision(20) << ::testing::PrintToString(vtst) << std::endl;
}
}
if (maxUlpDiff <= ulpTol_)
{
return ::testing::AssertionSuccess();
}
else
{
return ::testing::AssertionFailure()
<< "Failing SIMD math function ulp comparison between " << refFuncExpr << " and " << simdFuncExpr << std::endl
<< "Requested ulp tolerance: " << ulpTol_ << std::endl
<< "Requested abs tolerance: " << absTol_ << std::endl
<< "Largest Ulp difference occurs for x=" << std::setprecision(20) << maxUlpDiffPos << std::endl
<< "Ref values: " << std::setprecision(20) << refValMaxUlpDiff << std::endl
<< "SIMD values: " << std::setprecision(20) << simdValMaxUlpDiff << std::endl
<< "Ulp diff.: " << std::setprecision(20) << maxUlpDiff << std::endl;
}
}
void tst_QGeometryData::interleaveWith()
{
QVector3D a(1.1, 1.2, 1.3);
QVector3D b(2.1, 2.2, 2.3);
QVector3D c(3.1, 3.2, 3.3);
QVector3D d(4.1, 4.2, 4.3);
QVector3D vx(0.7071, 0.7071, 0.0);
QVector2D at(0.11, 0.12);
QVector2D bt(0.21, 0.22);
QVector2D ct(0.31, 0.32);
QVector2D dt(0.41, 0.42);
QVector2D tx(1.0, 1.0);
QGeometryData data;
data.appendVertex(a, b, c, d);
data.appendTexCoord(at, bt, ct, dt);
QGeometryData dat2;
// count is the smaller of the two - nothing in this null case
// also make sure the argument doesnt somehow change - its a const
// so it shouldn't...
dat2.interleaveWith(data);
QCOMPARE(data.count(), 4);
QCOMPARE(data.vertex(0), a);
QCOMPARE(dat2.count(), 0);
QCOMPARE(dat2.count(QGL::Position), 0);
QCOMPARE(dat2.fields(), quint32(0));
// dat2 is smaller and has less fields
dat2.appendVertex(a + vx, b + vx);
dat2.interleaveWith(data);
QCOMPARE(data.count(), 4);
QCOMPARE(data.vertex(0), a);
QCOMPARE(dat2.count(), 4);
QCOMPARE(dat2.count(QGL::Position), 4);
QCOMPARE(dat2.count(QGL::TextureCoord0), 0);
QCOMPARE(dat2.fields(), QGL::fieldMask(QGL::Position));
QCOMPARE(dat2.vertex(0), a + vx);
QCOMPARE(dat2.vertex(1), a);
QCOMPARE(dat2.vertex(2), b + vx);
QCOMPARE(dat2.vertex(3), b);
// full zip with both sides have 4 verts & textures
dat2.clear();
for (int i = 0; i < data.count(); ++i)
{
dat2.appendVertex(data.vertex(i) + vx);
dat2.appendTexCoord(data.texCoord(i) + tx);
}
dat2.interleaveWith(data);
QCOMPARE(dat2.count(), 8);
QCOMPARE(dat2.count(QGL::Position), 8);
QCOMPARE(dat2.count(QGL::TextureCoord0), 8);
QCOMPARE(dat2.fields(), QGL::fieldMask(QGL::Position) |
QGL::fieldMask(QGL::TextureCoord0));
QCOMPARE(dat2.vertex(0), a + vx);
QCOMPARE(dat2.vertex(1), a);
QCOMPARE(dat2.vertex(4), c + vx);
QCOMPARE(dat2.vertex(7), d);
QCOMPARE(dat2.texCoord(0), at + tx);
QCOMPARE(dat2.texCoord(3), bt);
QCOMPARE(dat2.texCoord(7), dt);
}
int main() {
#if defined(__GNUC__) && (__GNUC__ == 4) && (__GNUC_MINOR__ > 4)
std::cout << "gcc " << __GNUC__ << "." << __GNUC_MINOR__ << std::endl;
#endif
#ifdef USE_SSEVECT
std::cout << "sse vector enabled in cmssw" << std::endl;
#endif
std::cout << sizeof(Basic2DVectorF) << std::endl;
std::cout << sizeof(Basic2DVectorD) << std::endl;
std::cout << sizeof(Basic3DVectorF) << std::endl;
std::cout << sizeof(Basic3DVectorD) << std::endl;
std::cout << sizeof(Basic3DVectorLD) << std::endl;
Basic3DVectorF x(2.0f,4.0f,5.0f);
Basic3DVectorF y(-3.0f,2.0f,-5.0f);
Basic3DVectorD xd(2.0,4.0,5.0);
Basic3DVectorD yd = y;
Basic3DVectorLD xld(2.0,4.0,5.0);
Basic3DVectorLD yld = y;
Basic2DVectorF x2(2.0f,4.0f);
Basic2DVectorF y2 = y.xy();
Basic2DVectorD xd2(2.0,4.0);
Basic2DVectorD yd2 = yd.xy();
{
std::cout << dotV(x,y) << std::endl;
std::cout << normV(x) << std::endl;
std::cout << norm(x) << std::endl;
std::cout << min(x.mathVector(),y.mathVector()) << std::endl;
std::cout << max(x.mathVector(),y.mathVector()) << std::endl;
std::cout << dotV(x,yd) << std::endl;
std::cout << dotV(xd,y) << std::endl;
std::cout << dotV(xd,yd) << std::endl;
std::cout << normV(xd) << std::endl;
std::cout << norm(xd) << std::endl;
std::cout << dotV(xld,yld) << std::endl;
std::cout << normV(xld) << std::endl;
std::cout << norm(xld) << std::endl;
Basic3DVectorF z = x.cross(y);
std::cout << z << std::endl;
std::cout << -z << std::endl;
Basic3DVectorD zd = x.cross(yd);
std::cout << zd << std::endl;
std::cout << -zd << std::endl;
std::cout << xd.cross(y)<< std::endl;
std::cout << xd.cross(yd)<< std::endl;
Basic3DVectorLD zld = x.cross(yld);
std::cout << zld << std::endl;
std::cout << -zld << std::endl;
std::cout << xld.cross(y)<< std::endl;
std::cout << xld.cross(yld)<< std::endl;
std::cout << z.eta() << " " << (-z).eta() << std::endl;
std::cout << zd.eta() << " " << (-zd).eta() << std::endl;
std::cout << zld.eta() << " " << (-zld).eta() << std::endl;
#if defined( __GXX_EXPERIMENTAL_CXX0X__)
auto s = x+xd - 3.1*z;
std::cout << s << std::endl;
auto s2 = x+xld - 3.1*zd;
std::cout << s2 << std::endl;
#endif
}
{
std::cout << dotV(x2,y2) << std::endl;
std::cout << normV(x2) << std::endl;
std::cout << norm(x2) << std::endl;
std::cout << min(x2.mathVector(),y2.mathVector()) << std::endl;
std::cout << max(x2.mathVector(),y2.mathVector()) << std::endl;
std::cout << dotV(x2,yd2) << std::endl;
std::cout << dotV(xd2,y2) << std::endl;
std::cout << dotV(xd2,yd2) << std::endl;
std::cout << normV(xd2) << std::endl;
std::cout << norm(xd2) << std::endl;
Basic2DVectorF z2(x2); z2-=y2;
std::cout << z2 << std::endl;
std::cout << -z2 << std::endl;
Basic2DVectorD zd2 = x2-yd2;
std::cout << zd2 << std::endl;
std::cout << -zd2 << std::endl;
std::cout << x2.cross(y2) << std::endl;
std::cout << x2.cross(yd2) << std::endl;
std::cout << xd2.cross(y2)<< std::endl;
std::cout << xd2.cross(yd2)<< std::endl;
#if defined( __GXX_EXPERIMENTAL_CXX0X__)
auto s2 = x2+xd2 - 3.1*z2;
std::cout << s2 << std::endl;
#endif
}
{
std::cout << "f" << std::endl;
Basic3DVectorF vx(2.0f,4.0f,5.0f);
Basic3DVectorF vy(-3.0f,2.0f,-5.0f);
vx+=vy;
std::cout << vx << std::endl;
Basic3DVectorF vz(1.f,1.f,1.f);
addScaleddiff(vz,0.1f,vx,vy);
std::cout << vz << std::endl;
}
{
std::cout << "d" << std::endl;
Basic3DVectorD vx(2.0,4.0,5.0);
Basic3DVectorD vy(-3.0,2.0,-5.0);
vx+=vy;
std::cout << vx << std::endl;
Basic3DVectorD vz(1.,1.,1);
addScaleddiff(vz,0.1,vx,vy);
std::cout << vz << std::endl;
}
std::cout << "std::vector" << std::endl;
std::vector<Basic3DVectorF> vec1; vec1.reserve(50);
std::vector<float> vecf(21);
std::vector<Basic3DVectorF> vec2(51);
std::vector<Basic3DVectorF> vec3; vec3.reserve(23456);
}
void addWithParam(std::vector<MatcherHolder>& params, const WithMatcher<Matcher>& v)
{
MatcherHolder vx(v.template clone_with_new_type<Convert>());
params.push_back(vx);
}
int main(int argc, char *argv[])
{
time_t start,end;
double dif;
QCoreApplication a(argc, argv);
time (&start);
BiImage im1,im2,Im1,Im2;
if(!im1.imread("sflowg.00.jpg"))
{
printf("Error in loading frame 1!");
return -1;
}
if(!im2.imread("sflowg.01.jpg"))
{
printf("Error in loading frame 2!");
return -1;
}
//if(!im1.imread("scene1.row2.jpg"))
//{
// printf("Error in loading frame 1!");
// return -1;
//}
//if(!im2.imread("scene1.row3.jpg"))
//{
// printf("Error in loading frame 2!");
// return -1;
//}
im1.GaussianSmoothing(Im1,.8,5);
im2.GaussianSmoothing(Im2,.8,5);
Im1.imresize(0.5);
Im2.imresize(0.5);
//Im1=im1;
//Im2=im2;
double alpha=0.03*255;
double gamma=0.002*255;
BPFlow bpflow;
int wsize=7;
bpflow.setDataTermTruncation(true);
//bpflow.setTRW(true);
//bpflow.setDisplay(false);
bpflow.LoadImages(Im1.width(),Im1.height(),Im1.nchannels(),Im1.data(),Im2.data());
bpflow.setPara(alpha*2,alpha*20);
bpflow.setHomogeneousMRF(wsize);
bpflow.ComputeDataTerm();
bpflow.ComputeRangeTerm(gamma);
bpflow.MessagePassing(100,3);
//for(int i=0;i<55;i++)
//{
// double CTRW=(i+1)*0.02;
// bpflow.setCTRW(CTRW);
// printf("No.%d CTRW=%f energy=%f\n",i+1,CTRW,bpflow.MessagePassing(300,1));
//}
//bpflow.MessagePassing(60);
bpflow.ComputeVelocity();
DImage vx(Im1.width(),Im1.height()),vy(Im1.width(),Im1.height());
for(int i=0;i<Im1.npixels();i++)
{
vx.data()[i]=bpflow.flow().data()[i*2];
vy.data()[i]=bpflow.flow().data()[i*2+1];
}
vx.imwrite("vx_discrete.jpg",ImageIO::normalized);
vy.imwrite("vy_discrete.jpg",ImageIO::normalized);
time (&end);
dif = difftime (end,start);
printf ("It took you %.2lf seconds to run SIFT flow.\n", dif );
//return a.exec();
return 1;
}
//=======================================================================
// profile
// command to build a profile
//=======================================================================
Sketcher_Profile::Sketcher_Profile(const char* aCmd)
{
enum {line, circle, point, none} move;
Standard_Integer i = 1;
Standard_Real x0, y0, x, y, dx, dy;
x0 = y0 = x = y = dy = 0;
dx = 1;
Standard_Boolean first, stayfirst, face, close;
first = Standard_True;
stayfirst = face = close = Standard_False;
Standard_Integer reversed = 0;
Standard_Integer control_Tolerance = 0;
TopoDS_Shape S;
TopoDS_Vertex MP;
BRepBuilderAPI_MakeWire MW;
gp_Ax3 DummyHP(gp::XOY());
gp_Pln P(DummyHP);
TopLoc_Location TheLocation;
Handle(Geom_Surface) Surface;
myOK = Standard_False;
myError = 0;
//TCollection_AsciiString aCommand(CORBA::string_dup(aCmd));
TCollection_AsciiString aCommand ((char*)aCmd);
TCollection_AsciiString aToken = aCommand.Token(":", 1);
int n = 0;
// porting to WNT
TColStd_Array1OfAsciiString aTab (0, aCommand.Length() - 1);
if ( aCommand.Length() )
{
while(aToken.Length() != 0) {
if(aCommand.Token(":", n + 1).Length() > 0)
aTab(n) = aCommand.Token(":", n + 1);
aToken = aCommand.Token(":", ++n);
}
n = n - 1;
}
if ( aTab.Length() && aTab(0).Length() )
while(i < n) {
Standard_Real length = 0, radius = 0, angle = 0;
move = point;
int n1 = 0;
TColStd_Array1OfAsciiString a (0, aTab(0).Length());
aToken = aTab(i).Token(" ", 1);
while (aToken.Length() != 0) {
if (aTab(i).Token(" ", n1 + 1).Length() > 0)
a(n1) = aTab(i).Token(" ", n1 + 1);
aToken = aTab(i).Token(" ", ++n1);
}
n1 = n1 - 1;
switch(a(0).Value(1))
{
case 'F':
{
if (n1 != 3) goto badargs;
if (!first) {
MESSAGE("profile : The F instruction must precede all moves");
return;
}
x0 = x = a(1).RealValue();
y0 = y = a(2).RealValue();
stayfirst = Standard_True;
break;
}
case 'O':
{
if (n1 != 4) goto badargs;
P.SetLocation(gp_Pnt(a(1).RealValue(), a(2).RealValue(), a(3).RealValue()));
stayfirst = Standard_True;
break;
}
case 'P':
{
if (n1 != 7) goto badargs;
gp_Vec vn(a(1).RealValue(), a(2).RealValue(), a(3).RealValue());
gp_Vec vx(a(4).RealValue(), a(5).RealValue(), a(6).RealValue());
if (vn.Magnitude() <= Precision::Confusion() || vx.Magnitude() <= Precision::Confusion()) {
MESSAGE("profile : null direction");
return;
}
gp_Ax2 ax(P.Location(), vn, vx);
P.SetPosition(ax);
stayfirst = Standard_True;
break;
}
case 'X':
{
if (n1 != 2) goto badargs;
length = a(1).RealValue();
if (a(0) == "XX")
length -= x;
dx = 1; dy = 0;
move = line;
break;
}
case 'Y':
{
if (n1 != 2) goto badargs;
length = a(1).RealValue();
if (a(0) == "YY")
length -= y;
dx = 0; dy = 1;
move = line;
break;
}
case 'L':
{
if (n1 != 2) goto badargs;
length = a(1).RealValue();
if (Abs(length) > Precision::Confusion())
move = line;
else
move = none;
break;
}
case 'T':
{
if (n1 != 3) goto badargs;
Standard_Real vx = a(1).RealValue();
Standard_Real vy = a(2).RealValue();
if (a(0) == "TT") {
vx -= x;
vy -= y;
}
length = Sqrt(vx * vx + vy * vy);
if (length > Precision::Confusion()) {
move = line;
dx = vx / length;
dy = vy / length;
}
else
move = none;
break;
}
case 'R':
{
if (n1 != 2) goto badargs;
angle = a(1).RealValue() * PI180;
if (a(0) == "RR") {
dx = Cos(angle);
dy = Sin(angle);
}
else {
Standard_Real c = Cos(angle);
Standard_Real s = Sin(angle);
Standard_Real t = c * dx - s * dy;
dy = s * dx + c * dy;
dx = t;
}
break;
}
case 'D':
{
if (n1 != 3) goto badargs;
Standard_Real vx = a(1).RealValue();
Standard_Real vy = a(2).RealValue();
length = Sqrt(vx * vx + vy * vy);
if (length > Precision::Confusion()) {
dx = vx / length;
dy = vy / length;
}
else
move = none;
break;
}
case 'C':
{
if (n1 != 3) goto badargs;
radius = a(1).RealValue();
if (Abs(radius) > Precision::Confusion()) {
angle = a(2).RealValue() * PI180;
move = circle;
}
else
move = none;
break;
}
case 'A': // TAngential arc by end point
{
if (n1 != 3) goto badargs;
Standard_Real vx = a(1).RealValue();
Standard_Real vy = a(2).RealValue();
if (a(0) == "AA") {
vx -= x;
vy -= y;
}
Standard_Real det = dx * vy - dy * vx;
if ( Abs(det) > Precision::Confusion()) {
Standard_Real c = (dx * vx + dy * vy)
/ Sqrt((dx * dx + dy * dy) * (vx * vx + vy * vy)); // Cosine of alpha = arc of angle / 2 , alpha in [0,Pi]
radius = (vx * vx + vy * vy)* Sqrt(dx * dx + dy * dy) // radius = distance between start and end point / 2 * sin(alpha)
/ (2.0 * det); // radius is > 0 or < 0
if (Abs(radius) > Precision::Confusion()) {
angle = 2.0 * acos(c); // angle in [0,2Pi]
move = circle;
}
else
move = none;
break;
}
else
move = none;
break;
}
case 'U': // Arc by end point and radiUs
{
if (n1 != 5) goto badargs;
Standard_Real vx = a(1).RealValue();
Standard_Real vy = a(2).RealValue();
radius = a(3).RealValue();
reversed = a(4).IntegerValue();
if (a(0) == "UU") { // Absolute
vx -= x;
vy -= y;
}
Standard_Real length = Sqrt(vx * vx + vy * vy);
if ( (4.0 - (vx * vx + vy * vy) / (radius * radius) >= 0.0 ) && (length > Precision::Confusion()) ) {
Standard_Real c = 0.5 * Sqrt(4.0 - (vx * vx + vy * vy) / (radius * radius)); // Cosine of alpha = arc angle / 2 , alpha in [0,Pi/2]
angle = 2.0 * acos(c); // angle in [0,Pi]
if ( reversed == 2 )
angle = angle - 2 * PI;
dx = 0.5 * ( vy * 1.0/radius
+ vx * Sqrt(4.0 / (vx * vx + vy * vy) - 1.0 / (radius * radius)));
dy = - 0.5 * ( vx * 1.0/radius
- vy * Sqrt(4.0 / (vx * vx + vy * vy) - 1.0 / (radius * radius)));
move = circle;
}
else{
move = none;
}
break;
}
case 'E': // Arc by end point and cEnter
{
if (n1 != 7) goto badargs;
Standard_Real vx = a(1).RealValue();
Standard_Real vy = a(2).RealValue();
Standard_Real vxc = a(3).RealValue();
Standard_Real vyc = a(4).RealValue();
reversed = a(5).IntegerValue();
control_Tolerance = a(6).IntegerValue();
if (a(0) == "EE") { // Absolute
vx -= x;
vy -= y;
vxc -= x;
vyc -= y;
}
radius = Sqrt( vxc * vxc + vyc * vyc );
Standard_Real det = vx * vyc - vy * vxc;
Standard_Real length = Sqrt(vx * vx + vy * vy);
Standard_Real length2 = Sqrt((vx-vxc) * (vx-vxc) + (vy-vyc) * (vy-vyc));
Standard_Real length3 = Sqrt(vxc * vxc + vyc * vyc);
Standard_Real error = Abs(length2 - radius);
myError = error;
if ( error > Precision::Confusion() ){
MESSAGE("Warning : The specified end point is not on the Arc, distance = "<<error);
}
if ( error > Precision::Confusion() && control_Tolerance == 1) // Don't create the arc if the end point
move = none; // is too far from it
else if ( (length > Precision::Confusion()) &&
(length2 > Precision::Confusion()) &&
(length3 > Precision::Confusion()) ) {
Standard_Real c = ( radius * radius - (vx * vxc + vy * vyc) )
/ ( radius * Sqrt((vx-vxc) * (vx-vxc) + (vy-vyc) * (vy-vyc)) ) ; // Cosine of arc angle
angle = acos(c); // angle in [0,Pi]
if ( reversed == 2 )
angle = angle - 2 * PI;
if (det < 0)
angle = -angle;
dx = vyc / radius;
dy = -vxc / radius;
move = circle;
}
else {
move = none;
}
break;
}
case 'I':
{
if (n1 != 2) goto badargs;
length = a(1).RealValue();
if (a(0) == "IX") {
if (Abs(dx) < Precision::Confusion()) {
MESSAGE("profile : cannot intersect, arg "<<i-1);
return;
}
length = (length - x) / dx;
}
else if (a(0) == "IY") {
if (Abs(dy) < Precision::Confusion()) {
MESSAGE("profile : cannot intersect, arg "<<i-1);
return;
}
length = (length - y) / dy;
}
if (Abs(length) > Precision::Confusion())
move = line;
else
move = none;
break;
}
case 'W':
{
if (a(0) == "WW")
close = Standard_True;
else if(a(0) == "WF") {
close = Standard_True;
face = Standard_True;
}
i = n - 1;
break;
}
default:
{
MESSAGE("profile : unknown code " << a(i));
return;
}
}
again :
switch (move)
{
case line :
{
if (length < 0) {
length = -length;
dx = -dx;
dy = -dy;
}
Handle(Geom2d_Line) l = new Geom2d_Line(gp_Pnt2d(x,y),gp_Dir2d(dx,dy));
BRepBuilderAPI_MakeEdge ME (GeomAPI::To3d(l,P),0,length);
if (!ME.IsDone())
return;
MW.Add(ME);
x += length*dx;
y += length*dy;
break;
}
case circle :
{
Standard_Boolean sense = Standard_True;
if (radius < 0) {
radius = -radius;
sense = !sense;
dx = -dx;
dy = -dy;
}
gp_Ax2d ax(gp_Pnt2d(x-radius*dy,y+radius*dx),gp_Dir2d(dy,-dx));
if (angle < 0) {
angle = -angle;
sense = !sense;
}
Handle(Geom2d_Circle) c = new Geom2d_Circle(ax,radius,sense);
BRepBuilderAPI_MakeEdge ME (GeomAPI::To3d(c,P),0,angle);
if (!ME.IsDone())
return;
MW.Add(ME);
gp_Pnt2d p;
gp_Vec2d v;
c->D1(angle,p,v);
x = p.X();
y = p.Y();
dx = v.X() / radius;
dy = v.Y() / radius;
break;
}
case point:
{
MP = BRepBuilderAPI_MakeVertex(gp_Pnt(x, y, 0.0));
break;
}
case none:
{
i = n - 1;
break;
}
}
// update first
first = stayfirst;
stayfirst = Standard_False;
if(!(dx == 0 && dy == 0))
myLastDir.SetCoord(dx, dy, 0.0);
else
return;
myLastPoint.SetX(x);
myLastPoint.SetY(y);
// next segment....
i++;
if ((i == n) && close) {
// the closing segment
dx = x0 - x;
dy = y0 - y;
length = Sqrt(dx * dx + dy * dy);
move = line;
if (length > Precision::Confusion()) {
dx = dx / length;
dy = dy / length;
goto again;
}
}
}
// get the result, face or wire
if (move == none) {
return;
} else if (move == point) {
S = MP;
} else if (face) {
if (!MW.IsDone()) {
return;
}
BRepBuilderAPI_MakeFace MF (P, MW.Wire());
if (!MF.IsDone()) {
return;
}
S = MF;
} else {
if (!MW.IsDone()) {
return;
}
S = MW;
}
if(!TheLocation.IsIdentity())
S.Move(TheLocation);
myShape = S;
myOK = true;
return;
badargs :
MESSAGE("profile : bad number of arguments");
return;
}
void addWithParam(std::vector<MatcherHolder>& params, const T& v)
{
MatcherHolder vx( new EqualsMatcher<T>(v));
params.push_back(vx);
}
void System::Problem_generate_IC(const int param)
{
if (thisIndex == 0)
{
CkPrintf(" ********* Cloud capture ************* \n");
CkPrintf(" **** MAGNETISATION=%g \n", MAGNETISATION);
CkPrintf(" **** H/R =%g \n", HoR );
CkPrintf(" **** GRAVITY_MASS= %g \n", GM);
CkPrintf(" **** GRAVITY_EPS= %g \n", GM_EPS);
CkPrintf(" --- \n");
}
gamma_gas = 1.0;
courant_no = 0.8;
t_global = 0;
iteration = 0;
const real xcl = XCL;
const real ycl = YCL;
const real vx_cl = VX0/VUNIT;
const real vy_cl = VY0/VUNIT;
const real vorb = std::sqrt(sqr(vx_cl) + sqr(vy_cl));
const real Rinit = std::sqrt(sqr(xcl) + sqr(ycl) );
const real tinfall = Rinit/vorb;
const real dcl = (DCLOUD/DUNIT);
const real cs2 = get_cs2(vec3(xcl, ycl, 0.0)); //TCLOUD*Tunit/sqr(vunit);
if (thisIndex == 0)
{
CkPrintf("Ro= %g pc, x= %g y= %g; vx= %g vy= %g vt= %g tinfall= %g Myr [%g]\n",
Rinit,
xcl, ycl,
vx_cl, vy_cl,
vorb,
tinfall * TIMEUNIT, tinfall);
}
for (int i = 0; i < local_n; i++)
{
const Particle &pi = ptcl_list[i];
const vec3 &pos = pi.get_pos();
if (pos.abs() < BND_RADIUS || pos.abs() > RoutBND)
mesh_pnts[i].boundary = MeshPoint::DIOD;
else
mesh_pnts[i].boundary = MeshPoint::NO_BOUNDARY;
const real Rdist = (pos - vec3(xcl, ycl, 0.0)).abs();
const real inv_beta = MAGNETISATION;
real dens = dcl;
real pres = dcl * cs2;
real b0 = std::sqrt(2.0*pres * inv_beta);
real bx(0), by(0), bz(0);
#if 0
bx = by = b0/sqrt(2.0);
#else
bx = by = bz = b0/sqrt(3.0);
#endif
real vx(vx_cl), vy(vy_cl), vz(0.0);
real scalar = 1.0;
if (Rdist > RCLOUD)
{
dens = DENSvac/DUNIT;
const real csig = std::sqrt(get_cs2(pos));
vx = (1 - 2.0*drand48()) * csig;
vy = (1 - 2.0*drand48()) * csig;
vz = (1 - 2.0*drand48()) * csig;
scalar = -1.0;
}
Fluid m;
m[Fluid::DENS] = dens;
m[Fluid::ETHM] = get_cs2(pos)*dens;
m[Fluid::VELX] = vx;
m[Fluid::VELY] = vy;
m[Fluid::VELZ] = vz;
m[Fluid::BX ] = bx;
m[Fluid::BY ] = by;
m[Fluid::BZ ] = bz;
m[Fluid::PSI ] = 0.0;
m[Fluid::ENTR] = 1.0;
Wrec_list[i] = Fluid_rec(m);
mesh_pnts[i].idx = thisIndex*1000000 + i+1;
}
}
void plotVBTFWP(const string tau_ = "HPS"){
TCanvas *c1 = new TCanvas("c1","Canvas",10,30,650,600);
c1->SetGrid(0,0);
c1->SetFillStyle(4000);
c1->SetFillColor(10);
c1->SetTicky();
c1->SetObjectStat(0);
c1->SetLogx(1);
TLegend* leg = new TLegend(0.36,0.15,0.80,0.5,NULL,"brNDC");
leg->SetFillStyle(4000);
leg->SetBorderSize(0);
leg->SetFillColor(10);
leg->SetTextSize(0.03);
TFile* f_Zee = new TFile("/data_CMS/cms/lbianchini/35pb/testNewWriteFromPAT_DYToEE-PYTHIA-TAUEFF.root");
TFile* f_Ztautau = new TFile("/data_CMS/cms/lbianchini/35pb/testNewWriteFromPAT_DYToTauTau-PYTHIA-TAUEFF.root");
f_Zee->cd();
TTree* tree_Zee = (TTree*) gDirectory->Get( ("tauFakeRateAnalyzer"+tau_+"/tree").c_str() );
f_Ztautau->cd();
TTree* tree_Ztautau = (TTree*) gDirectory->Get( ("tauFakeRateAnalyzer"+tau_+"/tree").c_str() );
TH2F* h2 = new TH2F("h2","",220,0,1.1,220,0,1.1);
float vxF[45];
float vyF[45];
float vxF_VBTF[6];
float vyF_VBTF[6];
float ZeeAll = (float)tree_Zee->GetEntries("");
float ZtautauAll = (float)tree_Ztautau->GetEntries("");
std::cout << ZtautauAll << std::endl;
for(int i = 0; i<=44; i++){
float cut = 0.025*i;
float ZeeCut = (float) tree_Zee->GetEntries(Form("leadPFChargedHadrMva<=%f",-0.1+cut));
float ZtautauCut = (float) tree_Ztautau->GetEntries(Form("leadPFChargedHadrMva<=%f",-0.1+cut));
std::cout << ZtautauCut << std::endl;
vxF[i]=ZeeCut/ZeeAll;
vyF[i]=ZtautauCut/ZtautauAll;
//h2->Fill(ZeeCut/ZeeAll,ZtautauCut/ZtautauAll);
std::cout << Form("leadPFChargedHadrMva<=%f: ",-0.1+cut) <<ZeeCut/ZeeAll << " --- " << ZtautauCut/ZtautauAll << std::endl;
}
h2->SetXTitle("e#rightarrow #tau_{had} fake-rate");
h2->SetYTitle("#tau_{had} efficiency");
h2->SetAxisRange(0.02,1.0,"X");
h2->SetAxisRange(0.95,1.01,"Y");
h2->Draw();
float ids[] = {1.0,.95,.90,.85,.80,.70,.60};
for(int i = 0; i<6; i++){
float ZeeCut_i = (float) tree_Zee->GetEntries( Form("matchedID>%f",ids[i]-0.025));
float ZtautauCut_i = (float) tree_Ztautau->GetEntries(Form("matchedID>%f",ids[i]-0.025));
vxF_VBTF[i]=ZeeCut_i/ZeeAll;
vyF_VBTF[i]=ZtautauCut_i/ZtautauAll;
std::cout << Form("matchedID>%f",ids[i]-0.025) << " " << ZeeCut_i/ZeeAll << " <-- VBTF Zee ---- VBTF Ztautau ---> " << ZtautauCut_i/ZtautauAll << std::endl;
}
TVectorF vx_VBTF(6,vxF_VBTF);
TVectorF vy_VBTF(6,vyF_VBTF);
TVectorF vx(45,vxF);
TVectorF vy(45,vyF);
TGraph* graph = new TGraph(vx,vy);
TGraph* graph_VBTF = new TGraph(vx_VBTF,vy_VBTF);
if( tau_.find("HPS")!=string::npos) graph->SetMarkerStyle(kOpenCircle);
else graph->SetMarkerStyle(kOpenSquare);
graph->SetMarkerSize(1.2);
if( tau_.find("HPS")!=string::npos) graph->SetMarkerColor(kRed);
else graph->SetMarkerColor(kBlue);
graph_VBTF->SetMarkerStyle(kFullStar);
graph_VBTF->SetMarkerSize(1.8);
graph_VBTF->SetMarkerColor(kBlack);
graph->Draw("P");
graph_VBTF->Draw("P");
string tau = tau_.find("HPS")!=string::npos ? "HPS" : "Shrinking Cone";
leg->SetHeader( ("#splitline{Simulation: "+tau+" #tau_{had}-candidates}{passing tau-ID and loose isolation}").c_str() );
leg->AddEntry(graph,"#splitline{discriminator by #xi^{lch}}{-0.1#leq #xi^{lch}_{cut} #leq1.0}","P");
leg->AddEntry(graph_VBTF,"#splitline{cut-based discriminator}{WP95,90,85,80,70,60 (ID-only)}","P");
leg->Draw();
}
Real State::max_abs_x_eigen(const _3Vec& X, const _3Vec& V) const {
return fabs(vx(X) - V[0]) + cs(X);
}
void BenderQuad(
const BAvector &x ,
const BAvector &y ,
Fun fun ,
BAvector &g ,
BAvector &gx ,
BAvector &gxx )
{ // determine the base type
typedef typename BAvector::value_type Base;
// check that BAvector is a SimpleVector class
CheckSimpleVector<Base, BAvector>();
// declare the ADvector type
typedef CPPAD_TESTVECTOR(AD<Base>) ADvector;
// size of the x and y spaces
size_t n = size_t(x.size());
size_t m = size_t(y.size());
// check the size of gx and gxx
CPPAD_ASSERT_KNOWN(
g.size() == 1,
"BenderQuad: size of the vector g is not equal to 1"
);
CPPAD_ASSERT_KNOWN(
size_t(gx.size()) == n,
"BenderQuad: size of the vector gx is not equal to n"
);
CPPAD_ASSERT_KNOWN(
size_t(gxx.size()) == n * n,
"BenderQuad: size of the vector gxx is not equal to n * n"
);
// some temporary indices
size_t i, j;
// variable versions x
ADvector vx(n);
for(j = 0; j < n; j++)
vx[j] = x[j];
// declare the independent variables
Independent(vx);
// evaluate h = H(x, y)
ADvector h(m);
h = fun.h(vx, y);
// evaluate dy (x) = Newton step as a function of x through h only
ADvector dy(m);
dy = fun.dy(x, y, h);
// variable version of y
ADvector vy(m);
for(j = 0; j < m; j++)
vy[j] = y[j] + dy[j];
// evaluate G~ (x) = F [ x , y + dy(x) ]
ADvector gtilde(1);
gtilde = fun.f(vx, vy);
// AD function object that corresponds to G~ (x)
// We will make heavy use of this tape, so optimize it
ADFun<Base> Gtilde;
Gtilde.Dependent(vx, gtilde);
Gtilde.optimize();
// value of G(x)
g = Gtilde.Forward(0, x);
// initial forward direction vector as zero
BAvector dx(n);
for(j = 0; j < n; j++)
dx[j] = Base(0);
// weight, first and second order derivative values
BAvector dg(1), w(1), ddw(2 * n);
w[0] = 1.;
// Jacobian and Hessian of G(x) is equal Jacobian and Hessian of Gtilde
for(j = 0; j < n; j++)
{ // compute partials in x[j] direction
dx[j] = Base(1);
dg = Gtilde.Forward(1, dx);
gx[j] = dg[0];
// restore the dx vector to zero
dx[j] = Base(0);
// compute second partials w.r.t x[j] and x[l] for l = 1, n
ddw = Gtilde.Reverse(2, w);
for(i = 0; i < n; i++)
gxx[ i * n + j ] = ddw[ i * 2 + 1 ];
}
return;
}
// we use a tree architecture to build the subzones
int VEF::divideIntoZones(Zone z, int nbZones){
TreeZone tz(z);
TreeZone *father;
int flag, flag2, depth; // used to determine the cutting axis
flag = flag2 = depth = 0;
Vertex bottom, middle_bottom, middle_top, top;
bottom = middle_bottom = z.getZoneBottomVertex();
top = middle_top = z.getZoneTopVertex();
int id = 0;
int rootFlag = 0;
std::vector<TreeZone> parents; // store the parent trees relative to the one we manipulate
for (int i = 0; i < nbZones; i++){
if (!(i % 2)){
if (flag % 3 == 0) // we cut along the x axis
{
Vertex vx(middle_top.getX() - (middle_top.getX() / 2), middle_top.getY(), middle_top.getZ());
Zone zl(bottom, vx, -1);
TreeZone *l = new TreeZone(zl);
tz.addLeftChild(l);
//top.setX(vx.getX());
middle_top = vx;
}
if (flag % 3 == 1) // we cut along the y axis
{
Vertex vy(middle_top.getX(), middle_top.getY() - middle_top.getY() / 2, middle_top.getZ());
Zone zl(bottom, vy, -1);
TreeZone *l = new TreeZone(zl);
tz.addLeftChild(l);
//top.setY(vy.getY());
middle_top = vy;
}
if (flag % 3 == 2) // we cut along the z axis
{
Vertex vz(middle_top.getX(), middle_top.getY(), middle_top.getZ() - middle_top.getZ() / 2);
Zone zl(bottom, vz, -1);
TreeZone *l = new TreeZone(zl);
tz.addLeftChild(l);
//top.setZ(vz.getZ());
middle_top = vz;
}
}
else
{
if (flag2 % 3 == 0) // we cut along the x axis
{
Vertex vx(middle_bottom.getX() + (middle_bottom.getX() / 2), middle_bottom.getY(), middle_bottom.getZ());
Zone zr(vx, top, -1);
TreeZone *r = new TreeZone(zr);
tz.addRightChild(r);
middle_bottom = vx;
}
if (flag2 % 3 == 1) // we cut along the y axis
{
Vertex vy(middle_bottom.getX(), middle_bottom.getY() + (middle_bottom.getY() / 2), middle_bottom.getZ());
Zone zr(vy, top, -1);
TreeZone *r = new TreeZone(zr);
tz.addRightChild(r);
middle_bottom = vy;
}
if (flag2 % 3 == 2) // we cut along the z axis
{
Vertex vz(middle_bottom.getX(), middle_bottom.getY(), middle_bottom.getZ() + (middle_bottom.getZ() / 2));
Zone zr(vz, top, -1);
TreeZone *r = new TreeZone(zr);
tz.addRightChild(r);
}
if (depth){
if (!(*(father->getRightChild()) == tz))
{
if (father->getRightChild()->getLeftChild() == NULL)
{ // the brother of t doesn't exist yet
tz = *(father->getRightChild());
father = &(parents.back());
parents.pop_back();
}
}
}
parents.push_back(*father);
father = &tz;
tz = *(tz.getLeftChild());
depth++;
}
}
// depth-first search to add the zones we created (the leaves) into the zone vector
while (tz.hasLeftChild()){
father = &tz;
tz = *(tz.getLeftChild());
}
tz.getRoot().setZoneId(id);
m_zones.push_back(tz.getRoot());
id++;
// we stop when we reach the root for the second time, i.e, when all the leaves have been found
/*while(rootFlag != 1){
if (father->hasRightChild()){
father->getRightChild()->getRoot().setZoneId(id);
m_zones.push_back(father->getRightChild()->getRoot());
}
tz = father;
if (depth){
father = parents.back();
parents.pop_back();
}
depth--;
}*/
return 0;
}
void State::enforce_outflow(const OctFace& f, const _3Vec& X) {
switch (f) {
#ifdef USE_LZ
case XU:
if (vx(X) > 0.0) {
(*this)[sy_index] = X[0] * vy(X) * rho();
(*this)[sx_index] = X[1] * vy(X) * rho() / sqrt(X[0] * X[0] + X[1] * X[1]);
}
break;
case XL:
if (vx(X) < 0.0) {
(*this)[sy_index] = X[0] * vy(X) * rho();
(*this)[sx_index] = X[1] * vy(X) * rho() / sqrt(X[0] * X[0] + X[1] * X[1]);
}
break;
case YU:
if (vy(X) > 0.0) {
(*this)[sy_index] = -X[1] * vx(X) * rho();
(*this)[sx_index] = X[0] * vx(X) * rho() / sqrt(X[0] * X[0] + X[1] * X[1]);
}
break;
case YL:
if (vy(X) < 0.0) {
(*this)[sy_index] = -X[1] * vx(X) * rho();
(*this)[sx_index] = X[0] * vx(X) * rho() / sqrt(X[0] * X[0] + X[1] * X[1]);
}
break;
#else
case XU:
if (vx() > 0.0) {
set_et(et() - 0.5 * sx() * sx() / rho());
set_sx(0.0);
}
break;
case XL:
if (vx() < 0.0) {
set_et(et() - 0.5 * sx() * sx() / rho());
set_sx(0.0);
}
break;
case YU:
if (vy() > 0.0) {
set_et(et() - 0.5 * sy() * sy() / rho());
set_sy(0.0);
}
break;
case YL:
if (vy() < 0.0) {
set_et(et() - 0.5 * sy() * sy() / rho());
set_sy(0.0);
}
break;
#endif
case ZU:
if (sz() > 0.0) {
set_et(et() - 0.5 * sz() * sz() / rho());
set_sz(0.0);
}
break;
case ZL:
if (sz() < 0.0) {
set_et(et() - 0.5 * sz() * sz() / rho());
set_sz(0.0);
}
break;
}
}
inline std::basic_ostream<Char,Traits>&
operator <<(std::basic_ostream<Char,Traits>& os, const FloatVar& x) {
Float::FloatView vx(x);
return os << vx;
}
void swap_tests2(X* ptr = 0,
test::random_generator generator = test::default_generator)
{
swap_tests1(ptr);
typedef BOOST_DEDUCED_TYPENAME X::hasher hasher;
typedef BOOST_DEDUCED_TYPENAME X::key_equal key_equal;
typedef BOOST_DEDUCED_TYPENAME X::allocator_type allocator_type;
{
test::check_instances check_;
X x(0, hasher(1), key_equal(1));
X y(0, hasher(2), key_equal(2));
swap_test_impl(x, y);
}
{
test::check_instances check_;
test::random_values<X> v(1000, generator);
X x(v.begin(), v.end(), 0, hasher(1), key_equal(1));
X y(0, hasher(2), key_equal(2));
swap_test_impl(x, y);
}
{
test::check_instances check_;
test::random_values<X> vx(100, generator), vy(50, generator);
X x(vx.begin(), vx.end(), 0, hasher(1), key_equal(1));
X y(vy.begin(), vy.end(), 0, hasher(2), key_equal(2));
swap_test_impl(x, y);
swap_test_impl(x, y);
}
{
test::force_equal_allocator force_(
!test::is_propagate_on_swap<allocator_type>::value);
test::check_instances check_;
test::random_values<X> vx(50, generator), vy(100, generator);
X x(vx.begin(), vx.end(), 0, hasher(), key_equal(), allocator_type(1));
X y(vy.begin(), vy.end(), 0, hasher(), key_equal(), allocator_type(2));
if (test::is_propagate_on_swap<allocator_type>::value ||
x.get_allocator() == y.get_allocator())
{
swap_test_impl(x, y);
}
}
{
test::force_equal_allocator force_(
!test::is_propagate_on_swap<allocator_type>::value);
test::check_instances check_;
test::random_values<X> vx(100, generator), vy(100, generator);
X x(vx.begin(), vx.end(), 0, hasher(1), key_equal(1),
allocator_type(1));
X y(vy.begin(), vy.end(), 0, hasher(2), key_equal(2),
allocator_type(2));
if (test::is_propagate_on_swap<allocator_type>::value ||
x.get_allocator() == y.get_allocator())
{
swap_test_impl(x, y);
swap_test_impl(x, y);
}
}
}
void vHavokShapeFactory::ExtractScaling(const hkvMat4 &mat, hkvVec3& destScaling)
{
hkvVec3 vx(hkvNoInitialization),vy(hkvNoInitialization),vz(hkvNoInitialization);
mat.getAxisXYZ (&vx, &vy, &vz);
destScaling.set(vx.getLength(),vy.getLength(),vz.getLength());
}
//------------------------------------------------------------------------------
void CTerrain::updateVirtual()
{
const uint32 stepX = 1 << terrain.stepX; // edges count (vertices: step + 1)
const uint32 stepZ = 1 << terrain.stepZ;
const uint32 divStepX = 1 << terrain.divStepX; // count of chunks
const uint32 divStepZ = 1 << terrain.divStepZ;
const uint32 stepMeshX = stepX / divStepX; // number of edges in chunk
const uint32 stepMeshZ = stepZ / divStepZ;
QVector<SVertex> vx((stepX + 1) * (stepZ + 1));
QVector<float> heightMap((stepX + 1) * (stepZ + 1), 0.0f);
QVector<bool> usedHeightMap((stepX + 1) * (stepZ + 1), false);
QVector<int32> heightMapLayer((stepX + 1) * (stepZ + 1), -1);
float depth = 1.0f;
srand(terrain.seed);
// compute heightmap
if(!terrain.landBorder)
{
heightMap[0] = rndHeight();
heightMap[stepX] = rndHeight();
heightMap[(stepX + 1) * stepZ] = rndHeight();
heightMap[(stepX + 1) * stepZ + stepX] = rndHeight();
}
usedHeightMap[0] = true;
usedHeightMap[stepX] = true;
usedHeightMap[(stepX + 1) * stepZ] = true;
usedHeightMap[(stepX + 1) * stepZ + stepX] = true;
fractalGrid(&heightMap[0], &usedHeightMap[0], stepX, stepZ, stepX, stepZ, 0, depth);
// compute positions
float x = 0.0f;
float z = 0.0f;
const float xInc = 1.0f / stepX;
const float zInc = 1.0f / stepZ;
for(uint32 i = 0; i <= stepX; i++, x += xInc)
{
for(uint32 j = 0; j <= stepZ; j++, z += zInc)
{
const uint32 k = ((stepX + 1) * i + j);
vx[k].position = QVector3D(x - 0.5f, heightMap[k] * terrain.heightMultiplier, z - 0.5f);
vx[k].normal = QVector3D(0.0, 1.0, 0.0);
vx[k].texCoord = QVector2D(-x * 10, z * 10);
vx[k].color = QVector3D(1.0, 0.0, 0.0);
vx[k].color2 = QVector3D(0.0, 0.0, 0.0);
}
z = 0.0f;
}
// compute normals, tangents, bitangents
for(int32 v = 0; v < vx.size(); v++)
{
uint32 x = v % (stepX + 1);
uint32 z = v / (stepX + 1);
if((x > 0) && (x < stepX) && (z > 0) && (z < stepZ))
{
QVector3D &vC = vx[v].position;
QVector3D &vL = vx[v - 1].position;
QVector3D &vR = vx[v + 1].position;
QVector3D &vT = vx[v - stepX - 1].position;
QVector3D &vB = vx[v + stepX + 1].position;
vx[v].normal = QVector3D::normal(vC, vL, vT);
vx[v].normal += QVector3D::normal(vC, vT, vR);
vx[v].normal += QVector3D::normal(vC, vR, vB);
vx[v].normal += QVector3D::normal(vC, vB, vL);
vx[v].normal.normalize();
vx[v].normalTangent = QVector3D(vx[v].normal.y(), -vx[v].normal.x(), -vx[v].normal.z());
vx[v].normalBitangent = QVector3D(-vx[v].normal.x(), -vx[v].normal.z(), vx[v].normal.y());
}
}
// compute colors by layers
for(int32 v = 0; v < vx.size(); v++)
{
for(int32 l = 0; l != terrain.layers.size(); l++)
{
const SMaterialLayer *layer = &terrain.layers[l];
if(!(((layer->useHeightLimit) && ((vx[v].position.y() < layer->minHeight) || (vx[v].position.y() > layer->maxHeight))) ||
((layer->useSlopeLimit) && (((1.0f - vx[v].normal.y()) < layer->minSlope) || ((1.0f - vx[v].normal.y()) > layer->maxSlope)))))
{
vx[v].color = layer->color.toVector3D() * layer->color.w();
heightMapLayer[v] = l;
}
}
}
/*for(int32 v = 0; v < vx.size(); v++)
qDebug(QString("y %1, x %2: %3").arg(v / (stepX + 1)).arg(v % (stepX + 1)).arg(heightMapLayer[v]).toStdString().c_str());*/
// divide to meshes, fill model meshes
/*model.materials.clear();
model.materials.push_back(SMaterial(context->getShaders()->getShaderProgram(NShader::PROGRAM_TERRAIN),
context->getMaps()->addMap(SMap(":/maps/data/maps/rocks00n.png")),
context->getMaps()->addMap(SMap(":/maps/data/maps/rocks00n.png")),
context->getMaps()->addMap(SMap(":/maps/data/maps/rocks00n.png"))));*/
/*model.meshes.clear();
for(uint32 m = 0; m < (divStepX * divStepZ); m++)
{
model.meshes.push_back(SMesh());
SMesh *mesh = &model.meshes.back();
mesh->transformation = SMatrix::composeTransformation(mesh->position, mesh->rotation, mesh->scale);
mesh->faces.push_back(SFaces());
SFaces *faces = &mesh->faces.back();
mesh->vertices.resize((stepMeshX + 1) * (stepMeshZ + 1));
faces->faces.resize(stepMeshX * stepMeshZ * NTerrain::FACE_BLOCK_SIZE);
uint32 offset = (m / divStepZ) * stepMeshZ * (stepX + 1) + (m % divStepX) * stepMeshX;
uint32 mCol2 = 0;
for(uint32 mRow = 0; mRow <= stepMeshZ; mRow++, offset += (stepX + 1))
{
for(uint32 mCol = 0; mCol <= stepMeshX; mCol++, mCol2++)
{
mesh->vertices[mCol2] = vx[offset + mCol];
if((mRow == stepMeshZ) || (mCol == stepMeshX))
mesh->vertices[mCol2].color = QVector3D(0.8, 0.2, 0.2);
}
}
for(uint32 i = 0; i < stepMeshX; i++)
{
for(uint32 j = 0; j < stepMeshZ; j++)
{
const uint32 k = (stepMeshX * i + j) * NTerrain::FACE_BLOCK_SIZE;
const uint32 l0 = (stepMeshX + 1) * i + j + 0;
const uint32 l1 = (stepMeshX + 1) * i + j + 1;
const uint32 l2 = (stepMeshX + 1) * (i + 1) + j + 0;
const uint32 l3 = (stepMeshX + 1) * (i + 1) + j + 1;
faces->faces[k + 0] = SFace(l0, l1, l2);
faces->faces[k + 1] = SFace(l2, l1, l3);
}
}
}*/
model.materials.clear();
const CMap *noMap = context->getMaps()->getMap(NMap::DEFAULT_NORMAL); // default map
model.meshes.clear();
for(uint32 gz = 0; gz < divStepZ; gz++)
{
for(uint32 gx = 0; gx < divStepX; gx++)
{ // sub mesh
//const uint32 groupBase = gz * divStepZ + gx;
const uint32 vxGroupBase = gz * stepMeshZ * (stepX + 1) + gx * stepMeshX;
//qDebug("new mesh");
model.meshes.push_back(SMesh());
SMesh *mesh = &model.meshes.back();
mesh->transformation = SMatrix::composeTransformation(mesh->position, mesh->rotation, mesh->scale);
for(uint32 ez = 0; ez < stepMeshZ; ez++)
{
for(uint32 ex = 0; ex < stepMeshX; ex++)
{ // two faces block
const uint32 vx0 = vxGroupBase + ez * (stepX + 1) + ex;
const uint32 vx1 = vx0 + 1;
const uint32 vx2 = vx0 + (stepX + 1);
const uint32 vx3 = vx2 + 1;
for(uint8 v = 0; v < NTerrain::FACE_BLOCK_SIZE; v++)
{ // one face
const uint32 face[NModel::FACE_SIZE] = { (!v) ? vx0 : vx2, (!v) ? vx1 : vx1, (!v) ? vx2 : vx3 };
// material signature
QSet<int32> faceSet;
faceSet.insert(heightMapLayer[face[0]]);
faceSet.insert(heightMapLayer[face[1]]);
faceSet.insert(heightMapLayer[face[2]]);
QVector<int32> faceMats = faceSet.toList().toVector();
qSort(faceMats.begin(), faceMats.end());
//qDebug(QString("%1 %2 %3 : %4 %5 %6 : %7").arg(face[0]).arg(face[1]).arg(face[2]).arg(heightMapLayer[face[0]]).arg(heightMapLayer[face[1]]).arg(heightMapLayer[face[2]]).arg(faceMats.size()).toStdString().c_str());
/*for(int32 i : faceMats)
qDebug(QString("%1").arg(i).toStdString().c_str());*/
SFaces *faces = NULL;
for(auto f = mesh->faces.begin(); f != mesh->faces.end(); f++)
{ // find faces group by material signature
if(!f->material)
{
//qDebug("chyba mat chybi");
break;
}
else if(f->material->layerVx == faceMats)
{
//qDebug("nasel");
faces = &(*f);
break;
}
}
if(!faces)
{
SMaterial *material = NULL;
for(auto m = model.materials.begin(); m != model.materials.end(); m++)
{ // find previous used material
if(m->layerVx == faceMats)
{
//qDebug("nenasel, ale predchozí mat ano");
material = &(*m);
break;
}
}
if(!material)
{
//qDebug("nenasel");
const CMap *maps[NModel::FACE_SIZE];
for(int32 i = 0; static_cast<uint32>(i) < NModel::FACE_SIZE; i++)
{
maps[i] = (faceMats.size() > i) ? ((faceMats[i] != -1) ? terrain.layers[faceMats[i]].detailTexture : noMap) : NULL;
/*if(maps[i])
qDebug(maps[i]->getMap()->file.toStdString().c_str());
else
qDebug("no map");*/
}
model.materials.push_back(SMaterial(context->getShaders()->getShaderProgram(NShader::PROGRAM_TERRAIN), NULL, NULL, NULL, maps[0], NULL, NULL, NULL, NULL, NULL, maps[1], maps[2], faceMats));
material = &model.materials.back();
}
mesh->faces.push_back(SFaces(material));
faces = &mesh->faces.back();
}
uint16 newFace[NModel::FACE_SIZE];
SVertex newVx[NModel::FACE_SIZE] = { vx[face[0]], vx[face[1]], vx[face[2]] };
for(uint32 i = 0; i < NModel::FACE_SIZE; i++)
{ // set texture using multiplier
if(heightMapLayer[face[i]] == faceMats[0])
newVx[i].color2.setX(1.0f);
else if(heightMapLayer[face[i]] == faceMats[1])
newVx[i].color2.setY(1.0f);
else if(heightMapLayer[face[i]] == faceMats[2])
newVx[i].color2.setZ(1.0f);
//qDebug(QString("face %1: %2 %3 %4").arg(i).arg(newVx[i].color2.x()).arg(newVx[i].color2.y()).arg(newVx[i].color2.z()).toStdString().c_str());
bool found = false;
for(int32 v = 0; v < mesh->vertices.size(); v++)
{ // find same vertex
if((mesh->vertices[v].position == newVx[i].position) && (mesh->vertices[v].color2 == newVx[i].color2))
{
//qDebug("same vx found");
found = true;
newFace[i] = v;
break;
}
}
if(!found)
{
//qDebug("same vx not found");
newFace[i] = mesh->vertices.size();
mesh->vertices.push_back(newVx[i]);
}
}
faces->faces.push_back(SFace(newFace[0], newFace[1], newFace[2]));
}
}
}
//qDebug(QString("-------- face groups: %1").arg(mesh->faces.size()).toStdString().c_str());
}
}
//qDebug(QString("-------- meshes: %1").arg(model.meshes.size()).toStdString().c_str());
}
int main(int argc, char** argv)
{
// camera
CameraHandler camera;
// getopt
getopt_t* gopt = getopt_create();
getopt_add_string(gopt, 'f', "file", "", "Use static camera image");
if (!getopt_parse(gopt, argc, argv, 1)) {
getopt_do_usage(gopt);
exit(1);
}
const char* fileName = getopt_get_string(gopt, "file");
if (strncmp(fileName, "", 1)) {
// if fileName is not empty
camera.setStaticImage(fileName);
}
getopt_destroy(gopt);
// initialize with first image
image_u32_t* tempIm = camera.getImage();
CalibrationHandler::instance()->calibrateImageSize(tempIm->height, tempIm->width, true);
image_u32_destroy(tempIm);
// lcm
LcmHandler::instance()->launchThreads();
// vx
VxHandler vx(1024, 768);
vx.launchThreads();
while (1) {
CalibrationInfo calibrationInfo =
CalibrationHandler::instance()->getCalibration();
RenderInfo render;
render.im = camera.getImage();
CalibrationHandler::instance()->clipImage(render.im);
// std::array<float, 2> pos;
// if (Arm::instance()->forwardKinematics(pos)) {
// printf("pos: %f, %f\n", pos[0], pos[1]);
// }
if (buttonStates.blobDetect) {
std::vector<BlobDetector::Blob> blobs =
BlobDetector::findBlobs(render.im, calibrationInfo, blobMinPixels);
for (const auto& blob : blobs) {
std::array<int, 2> imageCoords{{blob.x, blob.y}};
std::array<float, 2> screenCoords =
CoordinateConverter::imageToScreen(imageCoords);
switch (blob.type) {
case REDBALL:
render.redBlobs.push_back(screenCoords);
break;
case GREENBALL:
render.greenBlobs.push_back(screenCoords);
break;
case BLUESQUARE:
render.blueBlobs.push_back(screenCoords);
break;
default:
break;
}
}
}
VxButtonStates buttonStates = vx.getButtonStates();
if (buttonStates.colorMask) {
maskWithColors(render.im, calibrationInfo);
}
vx.changeRenderInfo(render);
usleep(1e3);
}
}
void checkRegret(void) {
ExpectationQuad<Real>::checkRegret();
// Check v'(eps)
Real x = eps_, two(2), p1(0.1), zero(0), one(1);
Real vx(0), vy(0);
Real dv = regret(x,1);
Real t(1), diff(0), err(0);
std::cout << std::right << std::setw(20) << "CHECK REGRET: v'(eps) is correct? \n";
std::cout << std::right << std::setw(20) << "t"
<< std::setw(20) << "v'(x)"
<< std::setw(20) << "(v(x+t)-v(x-t))/2t"
<< std::setw(20) << "Error"
<< "\n";
for (int i = 0; i < 13; i++) {
vy = regret(x+t,0);
vx = regret(x-t,0);
diff = (vy-vx)/(two*t);
err = std::abs(diff-dv);
std::cout << std::scientific << std::setprecision(11) << std::right
<< std::setw(20) << t
<< std::setw(20) << dv
<< std::setw(20) << diff
<< std::setw(20) << err
<< "\n";
t *= p1;
}
std::cout << "\n";
// check v''(eps)
vx = zero;
vy = zero;
dv = regret(x,2);
t = one;
diff = zero;
err = zero;
std::cout << std::right << std::setw(20) << "CHECK REGRET: v''(eps) is correct? \n";
std::cout << std::right << std::setw(20) << "t"
<< std::setw(20) << "v''(x)"
<< std::setw(20) << "(v'(x+t)-v'(x-t))/2t"
<< std::setw(20) << "Error"
<< "\n";
for (int i = 0; i < 13; i++) {
vy = regret(x+t,1);
vx = regret(x-t,1);
diff = (vy-vx)/(two*t);
err = std::abs(diff-dv);
std::cout << std::scientific << std::setprecision(11) << std::right
<< std::setw(20) << t
<< std::setw(20) << dv
<< std::setw(20) << diff
<< std::setw(20) << err
<< "\n";
t *= p1;
}
std::cout << "\n";
// Check v'(0)
x = zero;
vx = zero;
vy = zero;
dv = regret(x,1);
t = one;
diff = zero;
err = zero;
std::cout << std::right << std::setw(20) << "CHECK REGRET: v'(0) is correct? \n";
std::cout << std::right << std::setw(20) << "t"
<< std::setw(20) << "v'(x)"
<< std::setw(20) << "(v(x+t)-v(x-t))/2t"
<< std::setw(20) << "Error"
<< "\n";
for (int i = 0; i < 13; i++) {
vy = regret(x+t,0);
vx = regret(x-t,0);
diff = (vy-vx)/(two*t);
err = std::abs(diff-dv);
std::cout << std::scientific << std::setprecision(11) << std::right
<< std::setw(20) << t
<< std::setw(20) << dv
<< std::setw(20) << diff
<< std::setw(20) << err
<< "\n";
t *= p1;
}
std::cout << "\n";
// check v''(eps)
vx = zero;
vy = zero;
dv = regret(x,2);
t = one;
diff = zero;
err = zero;
std::cout << std::right << std::setw(20) << "CHECK REGRET: v''(0) is correct? \n";
std::cout << std::right << std::setw(20) << "t"
<< std::setw(20) << "v''(x)"
<< std::setw(20) << "(v'(x+t)-v'(x-t))/2t"
<< std::setw(20) << "Error"
<< "\n";
for (int i = 0; i < 13; i++) {
vy = regret(x+t,1);
vx = regret(x-t,1);
diff = (vy-vx)/(two*t);
err = std::abs(diff-dv);
std::cout << std::scientific << std::setprecision(11) << std::right
<< std::setw(20) << t
<< std::setw(20) << dv
<< std::setw(20) << diff
<< std::setw(20) << err
<< "\n";
t *= p1;
}
std::cout << "\n";
// Check v'(0)
x = -eps_;
vx = zero;
vy = zero;
dv = regret(x,1);
t = one;
diff = zero;
err = zero;
std::cout << std::right << std::setw(20) << "CHECK REGRET: v'(-eps) is correct? \n";
std::cout << std::right << std::setw(20) << "t"
<< std::setw(20) << "v'(x)"
<< std::setw(20) << "(v(x+t)-v(x-t))/2t"
<< std::setw(20) << "Error"
<< "\n";
for (int i = 0; i < 13; i++) {
vy = regret(x+t,0);
vx = regret(x-t,0);
diff = (vy-vx)/(two*t);
err = std::abs(diff-dv);
std::cout << std::scientific << std::setprecision(11) << std::right
<< std::setw(20) << t
<< std::setw(20) << dv
<< std::setw(20) << diff
<< std::setw(20) << err
<< "\n";
t *= p1;
}
std::cout << "\n";
// check v''(eps)
vx = zero;
vy = zero;
dv = regret(x,2);
t = one;
diff = zero;
err = zero;
std::cout << std::right << std::setw(20) << "CHECK REGRET: v''(-eps) is correct? \n";
std::cout << std::right << std::setw(20) << "t"
<< std::setw(20) << "v''(x)"
<< std::setw(20) << "(v'(x+t)-v'(x-t))/2t"
<< std::setw(20) << "Error"
<< "\n";
for (int i = 0; i < 13; i++) {
vy = regret(x+t,1);
vx = regret(x-t,1);
diff = (vy-vx)/(two*t);
err = std::abs(diff-dv);
std::cout << std::scientific << std::setprecision(11) << std::right
<< std::setw(20) << t
<< std::setw(20) << dv
<< std::setw(20) << diff
<< std::setw(20) << err
<< "\n";
t *= p1;
}
std::cout << "\n";
}
bool atomic_base<Base>::rev_sparse_hes(
const vector<Base>& x ,
const local::pod_vector<size_t>& x_index ,
const local::pod_vector<size_t>& y_index ,
const InternalSparsity& for_jac_sparsity ,
bool* rev_jac_flag ,
InternalSparsity& rev_hes_sparsity )
{ CPPAD_ASSERT_UNKNOWN( for_jac_sparsity.end() == rev_hes_sparsity.end() );
size_t q = rev_hes_sparsity.end();
size_t n = x_index.size();
size_t m = y_index.size();
bool ok = false;
size_t thread = thread_alloc::thread_num();
allocate_work(thread);
bool zero_empty = true;
bool input_empty = false;
bool transpose = false;
//
// vx
vector<bool> vx(n);
for(size_t j = 0; j < n; j++)
vx[j] = x_index[j] != 0;
//
// note that s and t are vectors so transpose does not matter for bool case
vector<bool> bool_s( work_[thread]->bool_s );
vector<bool> bool_t( work_[thread]->bool_t );
//
bool_s.resize(m);
bool_t.resize(n);
//
for(size_t i = 0; i < m; i++)
{ if( y_index[i] > 0 )
bool_s[i] = rev_jac_flag[ y_index[i] ];
}
//
std::string msg = ": atomic_base.rev_sparse_hes: returned false";
if( sparsity_ == pack_sparsity_enum )
{ vectorBool& pack_r( work_[thread]->pack_r );
vectorBool& pack_u( work_[thread]->pack_u );
vectorBool& pack_v( work_[thread]->pack_h );
//
pack_v.resize(n * q);
//
local::get_internal_sparsity(
transpose, x_index, for_jac_sparsity, pack_r
);
local::get_internal_sparsity(
transpose, y_index, rev_hes_sparsity, pack_u
);
//
ok = rev_sparse_hes(vx, bool_s, bool_t, q, pack_r, pack_u, pack_v, x);
if( ! ok )
ok = rev_sparse_hes(vx, bool_s, bool_t, q, pack_r, pack_u, pack_v);
if( ! ok )
{ msg = afun_name() + msg + " sparsity = pack_sparsity_enum";
CPPAD_ASSERT_KNOWN(false, msg.c_str());
}
local::set_internal_sparsity(zero_empty, input_empty,
transpose, x_index, rev_hes_sparsity, pack_v
);
}
else if( sparsity_ == bool_sparsity_enum )
{ vector<bool>& bool_r( work_[thread]->bool_r );
vector<bool>& bool_u( work_[thread]->bool_u );
vector<bool>& bool_v( work_[thread]->bool_h );
//
bool_v.resize(n * q);
//
local::get_internal_sparsity(
transpose, x_index, for_jac_sparsity, bool_r
);
local::get_internal_sparsity(
transpose, y_index, rev_hes_sparsity, bool_u
);
//
ok = rev_sparse_hes(vx, bool_s, bool_t, q, bool_r, bool_u, bool_v, x);
if( ! ok )
ok = rev_sparse_hes(vx, bool_s, bool_t, q, bool_r, bool_u, bool_v);
if( ! ok )
{ msg = afun_name() + msg + " sparsity = bool_sparsity_enum";
CPPAD_ASSERT_KNOWN(false, msg.c_str());
}
local::set_internal_sparsity(zero_empty, input_empty,
transpose, x_index, rev_hes_sparsity, bool_v
);
}
else
{ CPPAD_ASSERT_UNKNOWN( sparsity_ == set_sparsity_enum );
vector< std::set<size_t> >& set_r( work_[thread]->set_r );
vector< std::set<size_t> >& set_u( work_[thread]->set_u );
vector< std::set<size_t> >& set_v( work_[thread]->set_h );
//
set_v.resize(n);
//
local::get_internal_sparsity(
transpose, x_index, for_jac_sparsity, set_r
);
local::get_internal_sparsity(
transpose, y_index, rev_hes_sparsity, set_u
);
//
ok = rev_sparse_hes(vx, bool_s, bool_t, q, set_r, set_u, set_v, x);
if( ! ok )
ok = rev_sparse_hes(vx, bool_s, bool_t, q, set_r, set_u, set_v);
if( ! ok )
{ msg = afun_name() + msg + " sparsity = set_sparsity_enum";
CPPAD_ASSERT_KNOWN(false, msg.c_str());
}
local::set_internal_sparsity(zero_empty, input_empty,
transpose, x_index, rev_hes_sparsity, set_v
);
}
for(size_t j = 0; j < n; j++)
{ if( x_index[j] > 0 )
rev_jac_flag[ x_index[j] ] |= bool_t[j];
}
return ok;
}
