C++ R2函数代码示例

ResultAlign2D get_complete_alignment_no_preprocessing(
    const cv::Mat &input, const cv::Mat &INPUT, const cv::Mat &POLAR1,
    cv::Mat &m_to_align, const cv::Mat &POLAR2, bool apply) {

  IMP_LOG_TERSE("starting complete 2D alignment with no preprocessing"
                << std::endl);

  cv::Mat aux1, aux2, aux3, aux4;  // auxiliary matrices
  cv::Mat AUX1, AUX2, AUX3;        // ffts
  algebra::Transformation2D transformation1, transformation2;
  double angle1 = 0, angle2 = 0;
  ResultAlign2D RA = get_rotational_alignment_no_preprocessing(POLAR1, POLAR2);
  angle1 = RA.first.get_rotation().get_angle();
  get_transformed(m_to_align, aux1, RA.first);  // rotate
  get_fft_using_optimal_size(aux1, AUX1);
  RA = get_translational_alignment_no_preprocessing(INPUT, AUX1);
  algebra::Vector2D shift1 = RA.first.get_translation();
  transformation1.set_rotation(angle1);
  transformation1.set_translation(shift1);
  get_transformed(m_to_align, aux2, transformation1);  // rotate
  double ccc1 = get_cross_correlation_coefficient(input, aux2);
  // Check the opposed angle
  if (angle1 < PI) {
    angle2 = angle1 + PI;
  } else {
    angle2 = angle1 - PI;
  }
  algebra::Rotation2D R2(angle2);
  algebra::Transformation2D tr(R2);
  get_transformed(m_to_align, aux3, tr);  // rotate
  get_fft_using_optimal_size(aux3, AUX3);

  RA = get_translational_alignment_no_preprocessing(INPUT, AUX3);
  algebra::Vector2D shift2 = RA.first.get_translation();
  transformation2.set_rotation(angle2);
  transformation2.set_translation(shift2);
  get_transformed(m_to_align, aux3, transformation2);
  double ccc2 = get_cross_correlation_coefficient(input, aux3);

  if (ccc2 > ccc1) {
    if (apply) {
      aux3.copyTo(m_to_align);
    }
    IMP_LOG_VERBOSE(" Align2D complete Transformation= "
                    << transformation2 << " cross_correlation = " << ccc2
                    << std::endl);
    return ResultAlign2D(transformation2, ccc2);
  } else {
    if (apply) {
      aux3.copyTo(m_to_align);
    }
    IMP_LOG_VERBOSE(" Align2D complete Transformation= "
                    << transformation1 << " cross_correlation = " << ccc1
                    << std::endl);
    return ResultAlign2D(transformation1, ccc1);
  }
}

IMPEM2D_BEGIN_NAMESPACE

ResultAlign2D get_complete_alignment(const cv::Mat &input, cv::Mat &m_to_align,
                                     bool apply) {
  IMP_LOG_TERSE("starting complete 2D alignment " << std::endl);
  cv::Mat autoc1, autoc2, aux1, aux2, aux3;
  algebra::Transformation2D transformation1, transformation2;
  ResultAlign2D RA;
  get_autocorrelation2d(input, autoc1);
  get_autocorrelation2d(m_to_align, autoc2);
  RA = get_rotational_alignment(autoc1, autoc2, false);
  double angle1 = RA.first.get_rotation().get_angle();
  get_transformed(m_to_align, aux1, RA.first);  // rotate
  RA = get_translational_alignment(input, aux1);
  algebra::Vector2D shift1 = RA.first.get_translation();
  transformation1.set_rotation(angle1);
  transformation1.set_translation(shift1);
  get_transformed(m_to_align, aux2, transformation1);
  double ccc1 = get_cross_correlation_coefficient(input, aux2);
  // Check for both angles that can be the solution
  double angle2;
  if (angle1 < PI) {
    angle2 = angle1 + PI;
  } else {
    angle2 = angle1 - PI;
  }
  // rotate
  algebra::Rotation2D R2(angle2);
  algebra::Transformation2D tr(R2);
  get_transformed(m_to_align, aux3, tr);

  RA = get_translational_alignment(input, aux3);
  algebra::Vector2D shift2 = RA.first.get_translation();
  transformation2.set_rotation(angle2);
  transformation2.set_translation(shift2);
  get_transformed(m_to_align, aux3, transformation2);
  double ccc2 = get_cross_correlation_coefficient(input, aux3);
  if (ccc2 > ccc1) {
    if (apply) {
      aux3.copyTo(m_to_align);
    }
    IMP_LOG_VERBOSE(" Transformation= " << transformation2
                                        << " cross_correlation = " << ccc2
                                        << std::endl);
    return em2d::ResultAlign2D(transformation2, ccc2);
  } else {
    if (apply) {
      aux2.copyTo(m_to_align);
    }
    IMP_LOG_VERBOSE(" Transformation= " << transformation1
                                        << " cross_correlation = " << ccc1
                                        << std::endl);
    return em2d::ResultAlign2D(transformation1, ccc1);
  }
}

void Molecular_system::calcLocWithTestPos(Sample_point * sample,
                                          Array1 <doublevar> & tpos,
                                          Array1 <doublevar> & Vtest) {

  int nions=sample->ionSize();
  int nelectrons=sample->electronSize();
  Vtest.Resize(nelectrons + 1);
  Vtest = 0; 
  Array1 <doublevar> oldpos(3);
  //cout << "Calculating local energy\n";
  sample->getElectronPos(0, oldpos);
  sample->setElectronPosNoNotify(0, tpos);
  
  Array1 <doublevar> R(5);

  sample->updateEIDist();
  sample->updateEEDist();

  for(int i=0; i < nions; i++) {
    sample->getEIDist(0, i, R);
    Vtest(nelectrons)+=-sample->getIonCharge(i)/R(0);
  }
  doublevar dist = 0.0; 
  for(int d=0; d<3; d++)  {
    dist += (oldpos(d) - tpos(d))*(oldpos(d) - tpos(d)); 
  }
  dist = sqrt(dist); 
  Vtest(0) = 1/dist; 
  Array1 <doublevar> R2(5);
  for(int i=1; i< nelectrons; i++) {
    sample->getEEDist(0,i,R2);
    Vtest(i)= 1/R2(0);
  }
  sample->setElectronPosNoNotify(0, oldpos);
  sample->updateEIDist();
  sample->updateEEDist();
  //cout << "elec-elec: " << elecElec << endl;
  //cout << "pot " << pot << endl;

  for(int d=0; d< 3; d++) 
    Vtest(nelectrons) -=electric_field(d)*tpos(d);
}

void FirstOrderType1RTest::testBuildFirstOrderType1R2()
{
  std::cout << "--> Test: constructor data (2)." <<std::endl;
  SP::FirstOrderType1R R2(new FirstOrderType1R("TestPlugin:hT1", "TestPlugin:gT1", "TestPlugin:Jh0T1", "TestPlugin:Jg0T1"));
  CPPUNIT_ASSERT_EQUAL_MESSAGE("testBuildFirstOrderType1R2b : ", R2->getType() == RELATION::FirstOrder, true);
  CPPUNIT_ASSERT_EQUAL_MESSAGE("testBuildFirstOrderType1R2c : ", R2->getSubType() == RELATION::Type1R, true);
  //  CPPUNIT_ASSERT_EQUAL_MESSAGE("testBuildFirstOrderType1R2d : ", R2->gethName()=="TestPlugin:hT1", true);
  //  CPPUNIT_ASSERT_EQUAL_MESSAGE("testBuildFirstOrderType1R2e : ", R2->getgName()=="TestPlugin:gT1", true);
  //  CPPUNIT_ASSERT_EQUAL_MESSAGE("testBuildFirstOrderType1R2e : ", R2->getJachName(0)=="TestPlugin:Jh0T1", true);
  //  CPPUNIT_ASSERT_EQUAL_MESSAGE("testBuildFirstOrderType1R2g : ", R2->getJacgName(0)=="TestPlugin:Jg0T1", true);
  std::cout << "--> Constructor2 test ended with success." <<std::endl;
}

//--------------------------------------------decomposeProhMats-------------------------------------------//
// through the projection matrix we can get nearly all the information of the cameras
// the position of the camera
// the direction of the camera
// the focal of the camera
// the axises of the camera
void Camera::decomposeProjMats()
{

    // 1.0 get direction
    this->dir_.at<float>(0) = this->project_.at<float>(2,0);
    this->dir_.at<float>(1) = this->project_.at<float>(2,1);
    this->dir_.at<float>(2) = this->project_.at<float>(2,2);
    this->dir_ = this->dir_/ norm(this->dir_);

    // 2.0 get position
    cv::Mat KR(3,3,CV_32FC1);
    cv::Mat KT(3,1,CV_32FC1);
    for(int i=0; i<3; i++)
    {
        for(int j=0; j<3; j++)
        {
            KR.at<float>(i,j) = this->project_.at<float>(i,j);
        }
    }
    for(int i=0; i<3; i++)
        KT.at<float>(i,0) = this->project_.at<float>(i,3);

    this->pos_ = -KR.inv()* KT;

    // 3.0 compute the focal
    cv::Mat R0(3,1, CV_32FC1);
    cv::Mat R1(3, 1, CV_32FC1);
    cv::Mat R2(3, 1, CV_32FC1);

    for(int i=0; i<3; i++)
    {
        R0.at<float>(i) = KR.at<float>(0, i);
        R1.at<float>(i) = KR.at<float>(1, i);
        R2.at<float>(i) = KR.at<float>(2, i);
    }

    this->focal_ = 0.5*abs(norm(R0.cross(R2)))+ 0.5*abs(norm(R1.cross(R2)));


    // 4.0 axises of the camera
    this->zaxis_ = this->dir_;
    this->yaxis_ = this->zaxis_.cross(R0);
    this->yaxis_ = this->yaxis_/norm(this->yaxis_);

    this->xaxis_ = this->yaxis_.cross(this->zaxis_);
    this->xaxis_ = this->xaxis_/norm(this->xaxis_);

    KR.release();
    KT.release();
    R0.release();
    R1.release();
    R2.release();
}

TEST(TestBookingCalendar, testDuplicateReservation) {
    BookingCalendar calendar(2015);
    Room R1(1, 20, 300, 400, 405), R2(2, 15, 200, 300, 302);
    calendar.reserveRoom(R1, 2015, 12, 13);
    calendar.reserveRoom(R2, 2015, 12, 13);
    try {
        calendar.reserveRoom(R2, 2015, 12, 13);
    } catch (const RoomIsAlreadyRegisteredOnThisDayException & err) {
        ASSERT_STREQ("Room with id#2 is already registered in day#13", err.what());
    }

}

   TypeOfFE_P2ttdc(): TypeOfFE(0,0,6,1,Data,3,1,6,6,Pi_h_coef)
    { const R2 Pt[] = { Shrink(R2(0,0)), Shrink(R2(1,0)), Shrink(R2(0,1)),
	                Shrink(R2(0.5,0.5)),Shrink(R2(0,0.5)),Shrink(R2(0.5,0)) };
      for (int i=0;i<NbDoF;i++) {
       pij_alpha[i]= IPJ(i,i,0);
       P_Pi_h[i]=Pt[i]; }
     }

template <class Type,class TypeB> void bench_r2d_assoc
    (
          const OperAssocMixte & op,
          Type *,
          TypeB *,
          Pt2di sz,
          INT   x0,
          INT   x1
    )
{
    Im2D<Type,TypeB> I(sz.x,sz.y,(Type)0);
    Im2D_REAL8       R1(sz.x,sz.y,0.0);
    Im2D_REAL8       R2(sz.x,sz.y,0.0);
    Im2D_REAL8       R3(sz.x,sz.y,0.0);

    ELISE_COPY(I.all_pts(),255*frandr(),I.out());

    ELISE_COPY
    (
         I.all_pts(),
         linear_red(op,I.in(),x0,x1),
         R1.out()
    );

    TypeB vdef;
    op.set_neutre(vdef);
    ELISE_COPY
    (
         I.all_pts(),
         rect_red(op,I.in(vdef),Box2di(Pt2di(x0,0),Pt2di(x1,0))),
         R2.out()
    );

    ELISE_COPY
    (
         I.lmr_all_pts(Pt2di(1,0)),
         linear_red(op,I.in(),x0,x1),
         R3.out()
    );

    REAL d12,d23;
    ELISE_COPY 
    (
         R1.all_pts(),
         Virgule(
              Abs(R1.in()-R2.in()),
              Abs(R2.in()-R3.in())
         ),
         Virgule(VMax(d12),VMax(d23))
    );
    BENCH_ASSERT((d12<epsilon)&&(d23<epsilon));
}

//#define DEBUG
void SymList::compute_subgroup()
{
    Matrix2D<DOUBLE> I(4, 4);
    I.initIdentity();
    Matrix2D<DOUBLE> L1(4, 4), R1(4, 4), L2(4, 4), R2(4, 4), newL(4, 4), newR(4, 4);
    Matrix2D<int>    tried(true_symNo, true_symNo);
    int i, j;
    int new_chain_length;
    while (found_not_tried(tried, i, j, true_symNo))
    {
        tried(i, j) = 1;

        get_matrices(i, L1, R1);
        get_matrices(j, L2, R2);
        newL = L1 * L2;
        newR = R1 * R2;
        new_chain_length = __chain_length(i) + __chain_length(j);
        Matrix2D<DOUBLE> newR3 = newR;
        newR3.resize(3,3);
        if (newL.isIdentity() && newR3.isIdentity()) continue;

        // Try to find it in current ones
        bool found;
        found = false;
        for (int l = 0; l < SymsNo(); l++)
        {
        	get_matrices(l, L1, R1);
            if (newL.equal(L1) && newR.equal(R1))
            {
                found = true;
                break;
            }
        }

        if (!found)
        {
//#define DEBUG
#ifdef DEBUG
           std::cout << "Matrix size " << tried.Xdim() << " "
            << "trying " << i << " " << j << " "
            << "chain length=" << new_chain_length << std::endl;
            std::cout << "Result R Sh\n" << newR;
#endif
#undef DEBUG
            newR.setSmallValuesToZero();
            newL.setSmallValuesToZero();
            add_matrices(newL, newR, new_chain_length);
            tried.resize(MAT_YSIZE(tried) + 1, MAT_XSIZE(tried) + 1);
        }
    }
}

void revolute_joint_3D::doMotion(kte_pass_flag aFlag, const shared_ptr<frame_storage>& aStorage) {
  if((!mEnd) || (!mBase))
    return;
  
  mEnd->Parent = mBase->Parent;
  
  mEnd->Position = mBase->Position;
  mEnd->Velocity = mBase->Velocity;
  mEnd->Acceleration = mBase->Acceleration;

  if(!mAngle) {
    mEnd->Quat = mBase->Quat;
    mEnd->AngVelocity = mBase->AngVelocity;
    mEnd->AngAcceleration = mBase->AngAcceleration;
  } else {
    quaternion<double> tmp_quat(axis_angle<double>(mAngle->q,mAxis).getQuaternion());
    rot_mat_3D<double> R2(tmp_quat.getRotMat());
    mEnd->Quat = mBase->Quat * tmp_quat;
    mEnd->AngVelocity = (mBase->AngVelocity * R2) + mAngle->q_dot * mAxis;
    mEnd->AngAcceleration = (mBase->AngAcceleration * R2) + ((mBase->AngVelocity * R2) % (mAngle->q_dot * mAxis)) + mAngle->q_ddot * mAxis;
  
    if(mJacobian) {
      mJacobian->Parent = mEnd;
      
      mJacobian->qd_vel = vect<double,3>();
      mJacobian->qd_avel = mAxis;
      mJacobian->qd_acc = vect<double,3>();
      mJacobian->qd_aacc = vect<double,3>();
    };
  };
  
  mEnd->UpdateQuatDot();
  
  if((aFlag == store_kinematics) && (aStorage)) {
    if(!(aStorage->frame_3D_mapping[mBase]))
      aStorage->frame_3D_mapping[mBase] = shared_ptr< frame_3D<double> >(new frame_3D<double>((*mBase)),scoped_deleter());
    else
      (*(aStorage->frame_3D_mapping[mBase])) = (*mBase);
    if(!(aStorage->frame_3D_mapping[mEnd]))
      aStorage->frame_3D_mapping[mEnd] = shared_ptr< frame_3D<double> >(new frame_3D<double>((*mEnd)),scoped_deleter());
    else
      (*(aStorage->frame_3D_mapping[mEnd])) = (*mEnd);
    if(mAngle) {
      if(!(aStorage->gen_coord_mapping[mAngle]))
        aStorage->gen_coord_mapping[mAngle] = shared_ptr< gen_coord<double> >(new gen_coord<double>((*mAngle)),scoped_deleter());
      else
        (*(aStorage->gen_coord_mapping[mAngle])) = (*mAngle);
    };
  };
};

int main()
{
    Rational R1(18,45);
    R1.print1();
    R1.print2();
    Rational R2(24,64);
    R2.print1();
    R2.print2();

    Rational R3 = R1.add(R2);
    R3.print1();
    R3.print2();

    return 0;
}

void Node::drawSimple(QPainter &P, MapView *theView)
{
//    if (!M_PREFS->getWireframeView() && !TEST_RFLAGS(RendererOptions::Interacting))
//        return;

    if (! ((isReadonly() || !isSelectable(theView->pixelPerM(), theView->renderOptions())) && (!isPOI() && !isWaypoint())))
        //        if (!Pt->isReadonly() && Pt->isSelectable(r))
    {
        if (!layer()) {
            qDebug() << "Node without layer: " << id().numId << xmlId();
            return;
        }
        qreal WW = theView->nodeWidth();
        if (WW >= 1) {
            QColor theColor = QColor(0,0,0,128);
            if (M_PREFS->getUseStyledWireframe() && hasPainter()) {
                const FeaturePainter* thePainter = getCurrentPainter();
                if (thePainter->DrawForeground)
                    theColor = thePainter->ForegroundColor;
                else if (thePainter->DrawBackground)
                    theColor = thePainter->BackgroundColor;
            }
            QPointF Pp(theView->toView(this));
            if (layer()->classGroups() & Layer::Special) {
                QRect R2(Pp.x()-(WW+4)/2, Pp.y()-(WW+4)/2, WW+4, WW+4);
                P.fillRect(R2,QColor(255,0,255,192));
            } else if (isWaypoint()) {
                QRect R2(Pp.x()-(WW+4)/2, Pp.y()-(WW+4)/2, WW+4, WW+4);
                P.fillRect(R2,QColor(255,0,0,192));
            }

            QRect R(Pp.x()-WW/2, Pp.y()-WW/2, WW, WW);
            P.fillRect(R,theColor);
        }
    }
}

    self& addBefore(const base& aPose) { 

      rot_mat_3D<value_type> R(this->Quat.getRotMat());
      this->Position += R * aPose.Position;
      Velocity += R * ( AngVelocity % aPose.Position );
      Acceleration += R * ( (AngVelocity % (AngVelocity % aPose.Position)) + (AngAcceleration % aPose.Position) );

      rot_mat_3D<value_type> R2(aPose.Quat.getRotMat());
      this->Quat *= aPose.Quat;
      AngAcceleration = (AngAcceleration * R2);
      AngVelocity = (AngVelocity * R2);

      UpdateQuatDot();

      return *this;
    };

void prueba15 ()
{   //--------------------------- begin -------------------------------
    std::vector<uint64_t> A, B, C ( 30 , 0) ;

    for ( uint32_t i =0 ; i < 10 ; ++i)
    {   A.push_back ( 100+i);
        B.push_back ( 80 +i);
    };
    bs_util::range<uint64_t*> R1 ( &A[0], &A[10]), R2 ( &B[0], &B[10]);
    uninit_full_merge (&C[0],R1,R2, std::less<uint64_t>() );
    for ( uint32_t i =0 ; i < 10 ; ++i)
        BOOST_CHECK ( C[i] == 80+i);
    for ( uint32_t i =10 ; i < 20 ; ++i)
        BOOST_CHECK ( C[i] == 90+i) ;


}

// [[Rcpp::export]]
VectorXd bootR2(const MatrixXd X, const VectorXd y, int nBoot){
    RNGScope scope;
    const int n(X.rows());
    // const int p(X.cols());
    VectorXd R2s(nBoot);
    MatrixXd Xi(X);
    VectorXd yi(y);
    IntegerVector prm(n);
    // double R2i(R2(Xi, yi));
    for(int i = 0; i < nBoot; ++i) {
	prm = bootPerm(n);
	Xi = shuffleMatrix(X, prm);
	yi = shuffleVector(y, prm);
	R2s(i) = R2(Xi, yi);
    }
    return R2s;
}