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;
{
X x(0, hasher(1), key_equal(1));
X y(0, hasher(2), key_equal(2));
swap_test_impl(x, y);
}
{
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::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);
}
#if BOOST_UNORDERED_SWAP_METHOD == 1
{
test::random_values<X> vx(100, generator), vy(50, 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));
try {
swap_test_impl(x, y);
BOOST_ERROR("Using swap method 1, "
"swapping with unequal allocators didn't throw.");
} catch (std::runtime_error) {}
}
#else
{
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));
swap_test_impl(x, y);
}
{
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));
swap_test_impl(x, y);
swap_test_impl(x, y);
}
#endif
}
void load_fields_from_fft(const CH_FFT& fft , Triangulation& T ) {
int Nb = fft.Nx();
c_array vx = fft.field_vel_x();
c_array vy = fft.field_vel_y();
c_array al = fft.field_f();
for(F_v_it vit=T.vertices_begin();
vit != T.vertices_end();
vit++) {
int nx = vit->nx.val();
int ny = vit->ny.val();
// "right" ordering
int i = ( Nb - 1 ) - ny ;
int j = nx;
// "wrong" ordering
// int i = nx;
// int j = ny;
vit->U.set( Vector_2( real(vx(i,j)) , real(vy(i,j)) ) );
vit->alpha.set( real( al(i,j) ) );
// TODO: return more fields (chem pot, pressure, force, etc)
}
return;
}
/**
* Description not yet available.
* \param
*/
dmatrix laplace_approximation_calculator::get_gradient_for_hessian_calcs
(const dmatrix& local_Hess,double & f)
{
int us=local_Hess.indexmax();
int nvar=us*us;
independent_variables cy(1,nvar);
cy.initialize();
int ii=1; int i,j;
for (i=1;i<=us;i++)
for (j=1;j<=us;j++)
cy(ii++)=local_Hess(i,j);
dvar_vector vy=dvar_vector(cy);
dvar_matrix vHess(1,us,1,us);
ii=1;
for (i=1;i<=us;i++)
for (j=1;j<=us;j++)
vHess(i,j)=vy(ii++);
dvariable vf=0.0;
int sgn=0;
vf+=0.5*ln_det(vHess,sgn);
f=value(vf);
dvector g(1,nvar);
gradcalc(nvar,g);
dmatrix hessadjoint(1,us,1,us);
ii=1;
for (i=1;i<=us;i++)
for (j=1;j<=us;j++)
hessadjoint(i,j)=g(ii++);
return hessadjoint;
}
void swap_tests1(X*, test::random_generator generator = test::default_generator)
{
{
test::check_instances check_;
X x;
swap_test_impl(x, x);
}
{
test::check_instances check_;
X x,y;
swap_test_impl(x, y);
}
{
test::check_instances check_;
test::random_values<X> v(1000, generator);
X x, y(v.begin(), v.end());
swap_test_impl(x, y);
swap_test_impl(x, y);
}
{
test::check_instances check_;
test::random_values<X> vx(1000, generator), vy(1000, generator);
X x(vx.begin(), vx.end()), y(vy.begin(), vy.end());
swap_test_impl(x, y);
swap_test_impl(x, y);
}
}
const RNBoolean R3Matrix::
HasMirror(void) const
{
// Return whether matrix transformation has mirror operator
R2Vector vx(m[0][0], m[1][0]);
R2Vector vy(m[0][1], m[1][1]);
return RNIsNegative(vx % vy);
}
const bool R3Matrix::HasMirror(void) const
{
// Return whether matrix matrix has mirror operator
R3Vector vx(m[0][0], m[1][0], m[2][0]);
R3Vector vy(m[0][1], m[1][1], m[2][1]);
R3Vector vz(m[0][2], m[1][2], m[2][2]);
return (vz.Dot(vx % vy) < 0);
}
// Changed from setVelocity: by inamura on 2013-12-30
void SimObjBase::setLinearVelocity(double vx_,double vy_,double vz_)
{
// Set is not permitted in dynamics on mode
if (!dynamics()) { return; }
vx(vx_);
vy(vy_);
vz(vz_);
}
void Renderer::drawPolygon(const std::vector<SDL_Point> &points, const SDL_Color &color)
{
int n = points.size();
std::unique_ptr<int16_t[]> vx(new int16_t[n]);
std::unique_ptr<int16_t[]> vy(new int16_t[n]);
convertPointVectorToPolygonArrays(points, vx.get(), vy.get());
drawPolygon(vx.get(), vy.get(), n, color);
}
const RNBoolean R4Matrix::
HasMirror(void) const
{
// Return whether matrix transformation has mirror operator
R3Vector vx(m[0][0], m[1][0], m[2][0]);
R3Vector vy(m[0][1], m[1][1], m[2][1]);
R3Vector vz(m[0][2], m[1][2], m[2][2]);
return RNIsNegative(vz.Dot(vx % vy));
}
void runEmitter()
{
vec3f vel = vec3f(0, emitterVel, 0);
vec3f vx(1, 0, 0);
vec3f vy(0, 0, 1);
vec3f spread(emitterSpread, 0.0f, emitterSpread);
psystem->sphereEmitter(emitterIndex, cursorPosLag, vel, spread, emitterRadius, (int) emitterRate*timestep, particleLifetime, particleLifetime*0.1);
if (emitterIndex > numParticles-1)
emitterIndex = 0;
}
void SetUp() override {
std::vector<float> vx{1.0, 2.0, 3.0};
std::vector<float> vy(vx);
std::vector<float> magnitude(vx);
std::vector<float> orientation(vx); // Don't really care about orientation
std::vector<cv::Point_<float>> value_locations{cv::Point_<float>(0, 0),
cv::Point_<float>(1, 1),
cv::Point_<float>(2, 2)};
of.reset(new OpticalFlow<>(vx, vy, magnitude, orientation, value_locations,
num_rows_, num_cols_));
}
//---------------------------------------------------------
void NDG2D::dtscale2D(DVec& dtscale)
//---------------------------------------------------------
{
// function dtscale = dtscale2D;
// Purpose : Compute inscribed circle diameter as characteristic
// for grid to choose timestep
DMat vx, vy; DVec len1,len2,len3,sper,s1,s2,s3,Area;
IVec vmask1, vmask2, vmask3, vmask;
// Find vertex nodes
vmask1 = find( abs(s+r+2.0), '<', NODETOL);
vmask2 = find( abs( r-1.0), '<', NODETOL);
vmask3 = find( abs( s-1.0), '<', NODETOL);
vmask = concat(vmask1,vmask2,vmask3);
assert(vmask.size()==3); // expect 1 node in each vmask[i]
vx = x(vmask,All); vy = y(vmask,All);
// Compute semi-perimeter and area
len1 = sqrt( sqr(vx(1,All)-vx(2,All)) + sqr(vy(1,All)-vy(2,All)));
len2 = sqrt( sqr(vx(2,All)-vx(3,All)) + sqr(vy(2,All)-vy(3,All)));
len3 = sqrt( sqr(vx(3,All)-vx(1,All)) + sqr(vy(3,All)-vy(1,All)));
sper = 0.5 * (len1 + len2 + len3);
//Area = sqrt( sper.* (sper-len1).* (sper-len2).* (sper-len3) );
//Area = sqrt( sper.dm(sper-len1).dm(sper-len2).dm(sper-len3) );
s1=(sper-len1); s2=(sper-len2); s3=(sper-len3);
Area = sqrt( sper.dm(s1.dm(s2.dm(s3))) );
// Compute scale using radius of inscribed circle
dtscale = Area.dd(sper);
}
void plot(){
TFile* f_Zee = new TFile("../tagAndProbe/testNewWriteFromPAT_DYToEE-PYTHIA-TAUEFF.root");
TFile* f_Ztautau = new TFile("../tagAndProbe/testNewWriteFromPAT_DYToTauTau-PYTHIA-TAUEFF.root");
f_Zee->cd();
TTree* tree_Zee = (TTree*) gDirectory->Get("etoTauMargLoose90/fitter_tree");
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 ZeeAll = (float)tree_Zee->GetEntries("mcTrue");
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("electronPreIDOutput<=%f && mcTrue",-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("electronPreIDOutput<=%f && mcTrue: ",-0.1+cut) <<ZeeCut/ZeeAll << " --- " << ZtautauCut/ZtautauAll << std::endl;
}
h2->SetXTitle("efficiency of mva<X cut on electrons");
h2->SetYTitle("efficiency of mva<X cut on taus");
h2->SetAxisRange(0,1.0,"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->SetMarkerSize(1.0);
graph->SetMarkerColor(kRed);
graph->Draw("P");
}
Vector<Real, STATE_NF> State::vector_y_flux(const _3Vec& X, const _3Vec& V) const {
Vector<Real, STATE_NF> flux = 0.0;
Real v, v0, p;
v = vy(X);
v0 = v - V[1];
p = pg(X);
for (int i = 0; i < STATE_NF; i++) {
flux[i] = (*this)[i] * v0;
}
flux[et_index] += v * p;
#ifndef USE_LZ
flux[sy_index] += p;
#else
flux[sy_index] += X[0] * p;
#endif
return flux;
}
void test_buildCubic(/* Real */ ae_vector* x,
/* Real */ ae_vector* y,
ae_int_t n,
spline1dinterpolant* c,
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]);
}
spline1d::Spline1DCoeff scoeff;
scoeff.buildCubic(vx, vy);
_spline1dinterpolant_clear(c);
ae_vector_set_length(&c->x, n, _state);
ae_vector_set_length(&c->c, 4*(n-1), _state);
c->periodic = scoeff.m_periodic;
c->k = scoeff.m_k;
c->n = scoeff.m_n;
for (int i=0; i<=c->n-1; i++) {
c->x.ptr.p_double[i] = scoeff.m_x[i];
}
for(int i=0; i<=c->n-2; i++) {
c->c.ptr.p_double[4*i+0] = scoeff.m_c[4*i+0];
c->c.ptr.p_double[4*i+1] = scoeff.m_c[4*i+1];
c->c.ptr.p_double[4*i+2] = scoeff.m_c[4*i+2];
c->c.ptr.p_double[4*i+3] = scoeff.m_c[4*i+3];
}
ae_frame_leave(_state);
//fprintf(stderr, "===== test end =====\n");
}
PetscErrorCode feel_petsc_pre_solve(KSP ksp,Vec x,Vec y,void* ctx)
{
BackendPetsc<double> * b = static_cast<BackendPetsc<double>*> ( ctx );
if ( b->preSolve() )
{
vector_ptrtype vx( vec( x, b->dataMap() ) );
vector_ptrtype vy( vec( y, b->dataMap() ) );
b->preSolve( vx, vy );
}
else
{
LOG(WARNING) << "call feel_petsc_pre_solve without pre 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_pre_solve";
return 0;
}
void TangramPieces::update(){
float speed = 3.25;
int window_height = glutGet(GLUT_WINDOW_HEIGHT);
int window_width = glutGet(GLUT_WINDOW_WIDTH);
float clipX = 1.0 / (window_width / speed);
float clipY = - 1.0 / (window_height / speed);
glm::mat4 view = Camera::getInstance()->getView();
glm::vec2 mouse = Input::getInstance()->getMouseMotion();
if(GameManager::getInstance()->isMouseOver(_id)){
if(Input::getInstance()->mouseWasPressed(GLUT_LEFT_BUTTON)){
glm::mat4 t = glm::inverse(view)* glm::translate(glm::vec3(mouse.x*clipX, mouse.y*clipY, 0)) * view;
translate(t[3][0], t[3][1], t[3][2]);
}
else if(Input::getInstance()->mouseWasPressed(GLUT_RIGHT_BUTTON)){
glm::mat4 tx = glm::inverse(view)* glm::translate(glm::vec3(0, 1, 0)) * view;
glm::vec3 vx(tx[3][0],tx[3][1],tx[3][2]);
glm::normalize(vx);
rotate(vx.x, vx.y, vx.z, mouse.x);
glm::mat4 ty = glm::inverse(view)* glm::translate(glm::vec3(1, 0, 0)) * view;
glm::vec3 vy(ty[3][0],ty[3][1],ty[3][2]);
glm::normalize(vy);
rotate(vy.x, vy.y, vy.z, mouse.y);
}
if(Input::getInstance()->keyWasPressed('W'))
_noiseIntensity = MIN(_noiseIntensity + 0.0025, 1.5);
else if(Input::getInstance()->keyWasPressed('S'))
_noiseIntensity = MAX(0.25, _noiseIntensity - 0.0025);
if(Input::getInstance()->keyWasPressed('D'))
_noiseScale = MIN(_noiseScale + 0.0025, 1.0);
else if(Input::getInstance()->keyWasPressed('A'))
_noiseScale = MAX(0.25, _noiseScale - 0.0025);
}
}
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");
}
// ベクトルを向きとした回転行列を取得する
D3DXMATRIX* D3DXPlus::MatrixRotationVector(D3DXMATRIX* pOut, const D3DXVECTOR3* pvDirection)
{
D3DXMATRIX& m = *pOut;
// 向きのベクトルをz軸の正方向として、その座標系に変換する行列を作る
D3DXVECTOR3 vz;
D3DXVec3Normalize(&vz, pvDirection); // z軸決定
D3DXVECTOR3 vy(0.0f, 1.0f, 0.0f); // 適当に上側を向いたベクトル(外積計算用)
D3DXVECTOR3 vx;
D3DXVec3Cross(&vx, &vy, &vz);
D3DXVec3Normalize(&vx, &vx); // x軸決定
D3DXVec3Cross(&vy, &vz, &vx);
D3DXVec3Normalize(&vy, &vy); // y軸決定
// TODO: pDirectionがY軸と同じ場合の処理＝外積がゼロになる
m(0, 0) = vx.x;
m(0, 1) = vx.y;
m(0, 2) = vx.z;
m(0, 3) = 0.0f;
m(1, 0) = vy.x;
m(1, 1) = vy.y;
m(1, 2) = vy.z;
m(1, 3) = 0.0f;
m(2, 0) = vz.x;
m(2, 1) = vz.y;
m(2, 2) = vz.z;
m(2, 3) = 0.0f;
m(3, 0) = 0.0f;
m(3, 1) = 0.0f;
m(3, 2) = 0.0f;
m(3, 3) = 1.0f;
return pOut;
}
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");
}
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";
}
// 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;
}
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;
}
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);
}
}
}
/**
* Description not yet available.
* \param
*/
double do_gauss_hermite_block_diagonal_multi(const dvector& x,
const dvector& u0,const dmatrix& Hess,const dvector& _xadjoint,
const dvector& _uadjoint,const dmatrix& _Hessadjoint,
function_minimizer * pmin)
{
ADUNCONST(dvector,xadjoint)
ADUNCONST(dvector,uadjoint)
//ADUNCONST(dmatrix,Hessadjoint)
dvector & w= *(pmin->multinomial_weights);
const int xs=x.size();
const int us=u0.size();
gradient_structure::set_NO_DERIVATIVES();
int nsc=pmin->lapprox->num_separable_calls;
const ivector lrea = (*pmin->lapprox->num_local_re_array)(1,nsc);
int hroom = sum(square(lrea));
int nvar=x.size()+u0.size()+hroom;
independent_variables y(1,nvar);
// need to set random effects active together with whatever
// init parameters should be active in this phase
initial_params::set_inactive_only_random_effects();
initial_params::set_active_random_effects();
/*int onvar=*/initial_params::nvarcalc();
initial_params::xinit(y); // get the initial values into the
// do we need this next line?
y(1,xs)=x;
int i,j;
// contribution for quadratic prior
if (quadratic_prior::get_num_quadratic_prior()>0)
{
//Hess+=quadratic_prior::get_cHessian_contribution();
int & vxs = (int&)(xs);
quadratic_prior::get_cHessian_contribution(Hess,vxs);
}
// Here need hooks for sparse matrix structures
dvar3_array & block_diagonal_vhessian=
*pmin->lapprox->block_diagonal_vhessian;
block_diagonal_vhessian.initialize();
dvar3_array& block_diagonal_ch=
*pmin->lapprox->block_diagonal_vch;
//dvar3_array(*pmin->lapprox->block_diagonal_ch);
int ii=xs+us+1;
d3_array& bdH=(*pmin->lapprox->block_diagonal_hessian);
int ic;
for (ic=1;ic<=nsc;ic++)
{
int lus=lrea(ic);
for (i=1;i<=lus;i++)
for (j=1;j<=lus;j++)
y(ii++)=bdH(ic)(i,j);
}
dvector g(1,nvar);
gradcalc(0,g);
gradient_structure::set_YES_DERIVATIVES();
dvar_vector vy=dvar_vector(y);
//initial_params::stddev_vscale(d,vy);
ii=xs+us+1;
if (initial_df1b2params::have_bounded_random_effects)
{
cerr << "can't do importance sampling with bounded random effects"
" at present" << endl;
ad_exit(1);
}
else
{
for (int ic=1;ic<=nsc;ic++)
{
int lus=lrea(ic);
if (lus>0)
{
for (i=1;i<=lus;i++)
{
for (j=1;j<=lus;j++)
{
block_diagonal_vhessian(ic,i,j)=vy(ii++);
}
}
block_diagonal_ch(ic)=
choleski_decomp(inv(block_diagonal_vhessian(ic)));
}
}
}
int nsamp=pmin->lapprox->use_gauss_hermite;
pmin->lapprox->in_gauss_hermite_phase=1;
dvar_vector sample_value(1,nsamp);
sample_value.initialize();
dvar_vector tau(1,us);;
// !!! This only works for one random efect in each separable call
// at present.
if (pmin->lapprox->gh->mi)
{
delete pmin->lapprox->gh->mi;
pmin->lapprox->gh->mi=0;
}
pmin->lapprox->gh->mi=new multi_index(1,nsamp,
pmin->lapprox->multi_random_effects);
multi_index & mi = *(pmin->lapprox->gh->mi);
//for (int is=1;is<=nsamp;is++)
dvector& xx=pmin->lapprox->gh->x;
do
{
int offset=0;
pmin->lapprox->num_separable_calls=0;
//pmin->lapprox->gh->is=is;
for (ic=1;ic<=nsc;ic++)
{
int lus=lrea(ic);
// will need vector stuff here when more than one random effect
if (lus>0)
{
//tau(offset+1,offset+lus).shift(1)=block_diagonal_ch(ic)(1,1)*
// pmin->lapprox->gh->x(is);
dvector xv(1,lus);
for (int iu=1;iu<=lus;iu++)
{
xv(iu)= xx(mi()(iu));
}
tau(offset+1,offset+lus).shift(1)=block_diagonal_ch(ic)*xv;
offset+=lus;
}
}
// have to reorder the terms to match the block diagonal hessian
imatrix & ls=*(pmin->lapprox->block_diagonal_re_list);
int mmin=ls.indexmin();
int mmax=ls.indexmax();
int ii=1;
int i;
for (i=mmin;i<=mmax;i++)
{
int cmin=ls(i).indexmin();
int cmax=ls(i).indexmax();
for (int j=cmin;j<=cmax;j++)
{
vy(ls(i,j))+=tau(ii++);
}
}
if (ii-1 != us)
{
cerr << "error in interface" << endl;
ad_exit(1);
}
initial_params::reset(vy); // get the values into the model
ii=1;
for (i=mmin;i<=mmax;i++)
{
int cmin=ls(i).indexmin();
int cmax=ls(i).indexmax();
for (int j=cmin;j<=cmax;j++)
{
vy(ls(i,j))-=tau(ii++);
}
}
*objective_function_value::pobjfun=0.0;
pmin->AD_uf_outer();
++mi;
}
while(mi.get_depth()<=pmin->lapprox->multi_random_effects);
nsc=pmin->lapprox->num_separable_calls;
dvariable vf=pmin->do_gauss_hermite_integration();
int sgn=0;
dvariable ld=0.0;
if (ad_comm::no_ln_det_choleski_flag)
{
for (int ic=1;ic<=nsc;ic++)
{
if (allocated(block_diagonal_vhessian(ic)))
{
ld+=w(2*ic)*ln_det(block_diagonal_vhessian(ic),sgn);
}
}
ld*=0.5;
}
else
{
for (int ic=1;ic<=nsc;ic++)
{
if (allocated(block_diagonal_vhessian(ic)))
{
ld+=w(2*ic)*ln_det_choleski(block_diagonal_vhessian(ic));
}
}
ld*=0.5;
}
vf+=ld;
//vf+=us*0.91893853320467241;
double f=value(vf);
gradcalc(nvar,g);
// put uhat back into the model
gradient_structure::set_NO_DERIVATIVES();
vy(xs+1,xs+us).shift(1)=u0;
initial_params::reset(vy); // get the values into the model
gradient_structure::set_YES_DERIVATIVES();
pmin->lapprox->in_gauss_hermite_phase=0;
ii=1;
for (i=1;i<=xs;i++)
xadjoint(i)=g(ii++);
for (i=1;i<=us;i++)
uadjoint(i)=g(ii++);
for (ic=1;ic<=nsc;ic++)
{
int lus=lrea(ic);
for (i=1;i<=lus;i++)
{
for (j=1;j<=lus;j++)
{
(*pmin->lapprox->block_diagonal_vhessianadjoint)(ic)(i,j)=g(ii++);
}
}
}
return f;
}
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 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
}
void WiimoteTracker::wiimoteEventCallback(Misc::CallbackData* cbData)
{
/* Read the current instantaneous acceleration vector: */
Vector newAcceleration=wiimote->getAcceleration(0);
/* Update the filtered acceleration vector: */
if(firstEvent)
acceleration=newAcceleration;
else
{
Vector da=newAcceleration-lastAcceleration;
Scalar trust=Math::exp(-Geometry::sqr(da)*Scalar(50))*Scalar(0.2);
acceleration+=(newAcceleration-acceleration)*trust;
}
lastAcceleration=newAcceleration;
/* Calculate an intermediate rotation based on the filtered acceleration vector: */
Vector previousY=wiipos.getDirection(1);
Scalar yaw=Math::acos(previousY[1]/Math::sqrt(Math::sqr(previousY[0])+Math::sqr(previousY[1])));
if(previousY[0]>Scalar(0))
yaw=-yaw;
Scalar axz=Math::sqrt(Math::sqr(acceleration[0])+Math::sqr(acceleration[2]));
Scalar roll=Math::acos(acceleration[2]/axz);
if(acceleration[0]>Scalar(0))
roll=-roll;
Scalar pitch=Math::acos(axz/Math::sqrt(Math::sqr(acceleration[1])+Math::sqr(axz)));
if(acceleration[1]<Scalar(0))
pitch=-pitch;
Transform::Rotation wiirot=Transform::Rotation::rotateZ(yaw);
wiirot*=Transform::Rotation::rotateX(pitch);
wiirot*=Transform::Rotation::rotateY(roll);
/* Update the wiimote's orientation based on the acceleration vector only: */
wiipos=Transform(wiipos.getTranslation(),wiirot);
/* Store the IR camera targets: */
int numValidTargets=0;
for(int i=0;i<4;++i)
{
pixelValids[i]=wiimote->getIRTarget(i).valid;
if(pixelValids[i])
{
for(int j=0;j<2;++j)
pixels[i][j]=Scalar(wiimote->getIRTarget(i).pos[j]);
++numValidTargets;
}
}
if(numValidTargets>0)
{
if(numValidTargets==4)
{
/* Project the "up" vector into camera space: */
typedef Geometry::Vector<CameraFitter::Scalar,2> PVector;
PVector vy(acceleration[0],acceleration[2]);
vy.normalize();
PVector vx=-Geometry::normal(vy);
vx.normalize();
/* Find the leftmost, rightmost, and topmost points: */
Scalar minX,maxX,minY,maxY;
int minXIndex,maxXIndex,minYIndex,maxYIndex;
minX=minY=Math::Constants<Scalar>::max;
maxX=maxY=Math::Constants<Scalar>::min;
minXIndex=maxXIndex=minYIndex=maxYIndex=-1;
for(int i=0;i<4;++i)
{
Scalar x=pixels[i]*vx;
Scalar y=pixels[i]*vy;
if(minX>x)
{
minX=x;
minXIndex=i;
}
if(maxX<x)
{
maxX=x;
maxXIndex=i;
}
if(minY>y)
{
minY=y;
minYIndex=i;
}
if(maxY<y)
{
maxY=y;
maxYIndex=i;
}
}
/* Create the pixel-target map: */
pixelMap[minXIndex]=0;
pixelMap[maxYIndex]=1;
pixelMap[maxXIndex]=2;
for(int i=0;i<4;++i)
if(i!=minXIndex&&i!=maxYIndex&&i!=maxXIndex)
pixelMap[i]=3;
}
else
{
/* Project the target points into camera space using the previous camera position/orientation and match closest pairs: */
wiiCamera.setTransform(wiipos);
for(int pixelIndex=0;pixelIndex<4;++pixelIndex)
if(pixelValids[pixelIndex])
{
Scalar minDist2=Geometry::sqrDist(pixels[pixelIndex],wiiCamera.project(0));
int minIndex=0;
for(int i=1;i<4;++i)
{
Scalar dist2=Geometry::sqrDist(pixels[pixelIndex],wiiCamera.project(i));
if(minDist2>dist2)
{
minDist2=dist2;
minIndex=i;
}
}
pixelMap[pixelIndex]=minIndex;
}
}
/* Re-project the new pixel positions: */
wiiCamera.setTransform(homeTransform);
for(int i=0;i<4;++i)
wiiCamera.invalidatePixel(i);
for(int i=0;i<4;++i)
if(pixelValids[i])
wiiCamera.setPixel(pixelMap[i],pixels[i]);
wiiCamera.setTransform(homeTransform);
wiiCameraMinimizer.minimize(wiiCamera);
if(firstEvent)
wiipos=wiiCamera.getTransform();
else
{
/* Filter the reconstructed camera transformation: */
Transform deltaWP=Geometry::invert(wiipos);
deltaWP.leftMultiply(wiiCamera.getTransform());
Vector t=deltaWP.getTranslation();
t*=Scalar(0.05);
Vector r=deltaWP.getRotation().getScaledAxis();
r*=Scalar(0.05);
deltaWP=Transform(t,Transform::Rotation::rotateScaledAxis(r));
wiipos.leftMultiply(deltaWP);
}
}
wiipos.renormalize();
firstEvent=false;
if(wiimote->getButtonState(Wiimote::BUTTON_HOME))
wiipos=homeTransform;
if(reportEvents)
{
/* Update the VR device state: */
for(int i=0;i<13;++i)
setButtonState(i,wiimote->getButtonState(i));
for(int i=0;i<2;++i)
setValuatorState(i,wiimote->getJoystickValue(i));
Vrui::VRDeviceState::TrackerState ts;
ts.positionOrientation=PositionOrientation(wiipos);
ts.linearVelocity=Vrui::VRDeviceState::TrackerState::LinearVelocity::zero;
ts.angularVelocity=Vrui::VRDeviceState::TrackerState::AngularVelocity::zero;
setTrackerState(0,ts);
}
}
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;
}
