{
#if OPERATION_sec_div_qr
/* Needs (nn + dn + 1) + mpn_sec_pi1_div_qr's needs of (2nn' - dn + 1) for a
   total of 3nn + 4 limbs at tp.  Note that mpn_sec_pi1_div_qr's nn is one
   greater than ours, therefore +4 and not just +2.  */
  return 3 * nn + 4;
#endif
#if OPERATION_sec_div_r
/* Needs (nn + dn + 1) + mpn_sec_pi1_div_r's needs of (dn + 1) for a total of
   nn + 2dn + 2 limbs at tp.  */
  return nn + 2 * dn + 2;
#endif
}

RETTYPE
FNAME (Q(mp_ptr qp)
       mp_ptr np, mp_size_t nn,
       mp_srcptr dp, mp_size_t dn,
       mp_ptr tp)
{
  mp_limb_t d1, d0;
  unsigned int cnt;
  mp_limb_t inv32;

  ASSERT (dn >= 1);
  ASSERT (nn >= dn);
  ASSERT (dp[dn - 1] != 0);

  d1 = dp[dn - 1];
  count_leading_zeros (cnt, d1);

//BOOL OTsender(int nSndVals, int nOTs, int startpos, CSocket& sock, CBitVector& U, AES_KEY* vKeySeeds, CBitVector* values, BYTE* seed)
BOOL IKNPOTExtSnd::sender_routine(uint32_t id, uint64_t myNumOTs) {
	uint64_t myStartPos = id * myNumOTs;
	uint64_t wd_size_bits = m_nBlockSizeBits;
	uint64_t processedOTBlocks = min((uint64_t) NUMOTBLOCKS, ceil_divide(myNumOTs, wd_size_bits));
	uint64_t OTsPerIteration = processedOTBlocks * wd_size_bits;
	channel* chan = new channel(id, m_cRcvThread, m_cSndThread);
	uint64_t tmpctr, tmpotlen;
	uint64_t** rndmat;

	myNumOTs = min(myNumOTs + myStartPos, m_nOTs) - myStartPos;
	uint64_t lim = myStartPos + myNumOTs;

	// The vector with the received bits
#ifdef GENERATE_T_EXPLICITELY
	CBitVector vRcv(2 * m_nBaseOTs * OTsPerIteration);
#else
	CBitVector vRcv(m_nBaseOTs * OTsPerIteration);
#endif

	// Holds the reply that is sent back to the receiver
	uint32_t numsndvals = 2;
	CBitVector* vSnd;

	CBitVector* seedbuf = new CBitVector[m_nSndVals];
	for (uint32_t u = 0; u < m_nSndVals; u++)
		seedbuf[u].Create(OTsPerIteration * m_cCrypt->get_aes_key_bytes() * 8);
#ifdef ZDEBUG
	cout << "seedbuf size = " <<OTsPerIteration * AES_KEY_BITS << endl;
#endif
	vSnd = new CBitVector[numsndvals];
	for (uint32_t i = 0; i < numsndvals; i++) {
		vSnd[i].Create(OTsPerIteration * m_nBitLength);
	}

	// Contains the parts of the V matrix
	CBitVector Q(wd_size_bits * OTsPerIteration);

	uint64_t otid = myStartPos;

	uint8_t *rcvbuftmpptr, *rcvbufptr;

#ifdef OTTiming
	double totalMtxTime = 0, totalTnsTime = 0, totalHshTime = 0, totalRcvTime = 0, totalSndTime = 0, totalUnMaskTime=0;
	timeval tempStart, tempEnd;
#endif

	if(m_eSndOTFlav == Snd_GC_OT) {
		uint8_t* rnd_seed = (uint8_t*) malloc(m_nSymSecParam);
		m_cCrypt->gen_rnd(rnd_seed, m_nSymSecParam);
		chan->send(rnd_seed, m_nSymSecParam);
		initRndMatrix(&rndmat, m_nBitLength, m_nBaseOTs);
		fillRndMatrix(rnd_seed, rndmat, m_nBitLength, m_nBaseOTs, m_cCrypt);
		free(rnd_seed);
	}

	while (otid < lim) //do while there are still transfers missing
	{
		processedOTBlocks = min((uint64_t) NUMOTBLOCKS, ceil_divide(lim - otid, wd_size_bits));
		OTsPerIteration = processedOTBlocks * wd_size_bits;

#ifdef ZDEBUG
		cout << "Processing block " << nProgress << " with length: " << OTsPerIteration << ", and limit: " << lim << endl;
#endif

#ifdef OTTiming
		gettimeofday(&tempStart, NULL);
#endif
		//rcvbufptr = chan->blocking_receive_id_len(&rcvbuftmpptr, &tmpctr, &tmpotlen);
		//vRcv.AttachBuf(rcvbuftmpptr, bits_in_bytes(m_nBaseOTs * OTsPerIteration));
		ReceiveMasks(vRcv, chan, OTsPerIteration);

#ifdef OTTiming
		gettimeofday(&tempEnd, NULL);
		totalRcvTime += getMillies(tempStart, tempEnd);
		gettimeofday(&tempStart, NULL);
#endif
		BuildQMatrix(Q, otid, processedOTBlocks);
#ifdef OTTiming
		gettimeofday(&tempEnd, NULL);
		totalMtxTime += getMillies(tempStart, tempEnd);
		gettimeofday(&tempStart, NULL);
#endif
		UnMaskBaseOTs(Q, vRcv, processedOTBlocks);
#ifdef OTTiming
		gettimeofday(&tempEnd, NULL);
		totalUnMaskTime += getMillies(tempStart, tempEnd);
		gettimeofday(&tempStart, NULL);
#endif
		Q.Transpose(wd_size_bits, OTsPerIteration);
#ifdef OTTiming
		gettimeofday(&tempEnd, NULL);
		totalTnsTime += getMillies(tempStart, tempEnd);
		gettimeofday(&tempStart, NULL);
#endif
		HashValues(Q, seedbuf, vSnd, otid, min(lim - otid, OTsPerIteration), rndmat);
#ifdef OTTiming
		gettimeofday(&tempEnd, NULL);
		totalHshTime += getMillies(tempStart, tempEnd);
		gettimeofday(&tempStart, NULL);
//.........这里部分代码省略.........

  // Initialization process
void EkfNode::init() {
  /**************************************************************************
   * Initialize firstRun, time, and frame names
   **************************************************************************/
  // Set first run to true for encoders. Once a message is received, this will
  // be set to false.
  firstRunEnc_ = true;
  firstRunSys_ = true;
  // Set Times
  ros::Time currTime = ros::Time::now();
  lastEncTime_ = currTime;
  lastSysTime_ = currTime;
  // Use the ROS parameter server to initilize parameters
  if(!private_nh_.getParam("base_frame", base_frame_))
    base_frame_ = "base_link";
  if(!private_nh_.getParam("odom_frame", base_frame_))
    odom_frame_ = "odom";
  if(!private_nh_.getParam("map_frame", map_frame_))
    map_frame_ = "map";

  /**************************************************************************
   * Initialize state for ekf_odom_ and ekf_map_
   **************************************************************************/
  // Create temp array to initialize state
  double state_temp[] = {0, 0, 0, 0, 0, 0};
  // And a std::vector, which will be used to initialize an Eigen Matrix
  std::vector<double> state (state_temp, state_temp + sizeof(state_temp) / sizeof(double));
  // Check the parameter server and initialize state
  if(!private_nh_.getParam("state", state)) {
    ROS_WARN_STREAM("No state found. Using default.");
  }
  // Check to see if the size is not equal to 6
  if (state.size() != 6) {
    ROS_WARN_STREAM("state isn't 6 elements long!");
  }
  // And initialize the Matrix
  typedef Matrix<double, 6, 1> Vector6d;
  Vector6d stateMat(state.data());
  std::cout << "state_map = " << std::endl;
  std::cout << stateMat << std::endl;
  ekf_map_.initState(stateMat);

  // Odom is always initialized at all zeros.
  stateMat << 0, 0, 0, 0, 0, 0;
  ekf_odom_.initState(stateMat);
  std::cout << "state_odom = " << std::endl;
  std::cout << stateMat << std::endl;

  /**************************************************************************
   * Initialize covariance for ekf_odom_ and ekf_map_
   **************************************************************************/
  // Create temp array to initialize covariance
  double cov_temp[] = {0.01, 0, 0, 0, 0, 0,
		       0, 0.01, 0, 0, 0, 0,
		       0, 0, 0.01, 0, 0, 0,
		       0, 0, 0, 0.01, 0, 0,
		       0, 0, 0, 0, 0.01, 0,
		       0, 0, 0, 0, 0, 0.01};
  // And a std::vector, which will be used to initialize an Eigen Matrix
  std::vector<double> cov (cov_temp, cov_temp + sizeof(cov_temp) / sizeof(double));
  // Check the parameter server and initialize cov
  if(!private_nh_.getParam("covariance", cov)) {
    ROS_WARN_STREAM("No covariance found. Using default.");
  }
  // Check to see if the size is not equal to 36
  if (cov.size() != 36) {
    ROS_WARN_STREAM("cov isn't 36 elements long!");
  }
  // And initialize the Matrix
  typedef Matrix<double, 6, 6, RowMajor> Matrix66;
  Matrix66 covMat(cov.data());
  std::cout << "covariance = " << std::endl;
  std::cout << covMat << std::endl;
  ekf_map_.initCov(covMat);

  // Initialize odom covariance the same as the map covariance (this isn't
  // correct. But, since it is all an estimate anyway, it should be fine.
  ekf_odom_.initCov(covMat);

  /**************************************************************************
   * Initialize Q for ekf_odom_ and ekf_map_
   **************************************************************************/
  // Create temp array to initialize Q
  double Q_temp[] = {0.01, 0, 0, 0, 0, 0,
		     0, 0.01, 0, 0, 0, 0,
		     0, 0, 0.01, 0, 0, 0,
		     0, 0, 0, 0.01, 0, 0,
		     0, 0, 0, 0, 0.01, 0,
		     0, 0, 0, 0, 0, 0.01};
  // And a std::vector, which will be used to initialize an Eigen Matrix
  std::vector<double> Q (Q_temp, Q_temp + sizeof(Q_temp) / sizeof(double));
  // Check the parameter server and initialize Q
  if(!private_nh_.getParam("Q", Q)) {
    ROS_WARN_STREAM("No Q found. Using default.");
  }
  // Check to see if the size is not equal to 36
  if (Q.size() != 36) {
    ROS_WARN_STREAM("Q isn't 36 elements long!");
  }
//.........这里部分代码省略.........

static void
negative_s2_influence(double t, double s2, double *A1, double *A3)
{
  *A1 = g_blend(1-t, Q(s2));
  *A3 = h_blend(t-1, Q(s2));
}

 void meow() {
   evil<int> Q(0); // expected-note {{in instantiation of member function}}
 }

extern "C" magma_int_t
magma_dlaex1(
    magma_int_t n, double* d, double* q, magma_int_t ldq,
    magma_int_t* indxq, double rho, magma_int_t cutpnt,
    double* work, magma_int_t* iwork, magmaDouble_ptr dwork,
    magma_range_t range, double vl, double vu,
    magma_int_t il, magma_int_t iu,
    magma_queue_t queue,
    magma_int_t* info)
{
    /*
        -- clMAGMA (version 1.3.0) --
        Univ. of Tennessee, Knoxville
        Univ. of California, Berkeley
        Univ. of Colorado, Denver
        @date November 2014

           .. Scalar Arguments ..
          CHARACTER          RANGE
          INTEGER            IL, IU, CUTPNT, INFO, LDQ, N
          DOUBLE PRECISION   RHO, VL, VU
           ..
           .. Array Arguments ..
          INTEGER            INDXQ( * ), iwork[* )
          DOUBLE PRECISION   D( * ), Q( LDQ, * ), WORK( * ), DWORK( * )
           ..

        Purpose
        =======
        DLAEX1 computes the updated eigensystem of a diagonal
        matrix after modification by a rank-one symmetric matrix.

          T = Q(in) ( D(in) + RHO * Z*Z' ) Q'(in) = Q(out) * D(out) * Q'(out)

        where Z = Q'u, u is a vector of length N with ones in the
        CUTPNT and CUTPNT + 1 th elements and zeros elsewhere.

        The eigenvectors of the original matrix are stored in Q, and the
        eigenvalues are in D.  The algorithm consists of three stages:

        The first stage consists of deflating the size of the problem
        when there are multiple eigenvalues or if there is a zero in
        the Z vector.  For each such occurence the dimension of the
        secular equation problem is reduced by one.  This stage is
        performed by the routine DLAED2.

        The second stage consists of calculating the updated
        eigenvalues. This is done by finding the roots of the secular
        equation via the routine DLAED4 (as called by DLAED3).
        This routine also calculates the eigenvectors of the current
        problem.

        The final stage consists of computing the updated eigenvectors
        directly using the updated eigenvalues.  The eigenvectors for
        the current problem are multiplied with the eigenvectors from
        the overall problem.

        Arguments
        =========
        N      (input) INTEGER
               The dimension of the symmetric tridiagonal matrix.  N >= 0.

        D      (input/output) DOUBLE PRECISION array, dimension (N)
               On entry, the eigenvalues of the rank-1-perturbed matrix.
               On exit, the eigenvalues of the repaired matrix.

        Q      (input/output) DOUBLE PRECISION array, dimension (LDQ,N)
               On entry, the eigenvectors of the rank-1-perturbed matrix.
               On exit, the eigenvectors of the repaired tridiagonal matrix.

        LDQ    (input) INTEGER
               The leading dimension of the array Q.  LDQ >= max(1,N).

        INDXQ  (input/output) INTEGER array, dimension (N)
               On entry, the permutation which separately sorts the two
               subproblems in D into ascending order.
               On exit, the permutation which will reintegrate the
               subproblems back into sorted order,
               i.e. D( INDXQ( I = 1, N ) ) will be in ascending order.

        RHO    (input) DOUBLE PRECISION
               The subdiagonal entry used to create the rank-1 modification.

        CUTPNT (input) INTEGER
               The location of the last eigenvalue in the leading sub-matrix.
               min(1,N) <= CUTPNT <= N/2.

        WORK   (workspace) DOUBLE PRECISION array, dimension (4*N + N**2)

        IWORK  (workspace) INTEGER array, dimension (4*N)

        DWORK  (device workspace) DOUBLE PRECISION array, dimension (3*N*N/2+3*N)

        RANGE   (input) CHARACTER*1
                = 'A': all eigenvalues will be found.
                = 'V': all eigenvalues in the half-open interval (VL,VU]
                       will be found.
                = 'I': the IL-th through IU-th eigenvalues will be found.

        VL      (input) DOUBLE PRECISION
//.........这里部分代码省略.........

void Foam::sixDoFRigidBodyMotion::applyConstraints(scalar deltaT)
{
    if (constraints_.empty())
    {
        return;
    }

    if (Pstream::master())
    {
        label iteration = 0;

        bool allConverged = true;

        // constraint force accumulator
        vector cFA = vector::zero;

        // constraint moment accumulator
        vector cMA = vector::zero;

        do
        {
            allConverged = true;

            forAll(constraints_, cI)
            {
                if (sixDoFRigidBodyMotionConstraint::debug)
                {
                    Info<< "Constraint " << constraintNames_[cI] << ": ";
                }

                // constraint position
                point cP = vector::zero;

                // constraint force
                vector cF = vector::zero;

                // constraint moment
                vector cM = vector::zero;

                bool constraintConverged = constraints_[cI].constrain
                                           (
                                               *this,
                                               cFA,
                                               cMA,
                                               deltaT,
                                               cP,
                                               cF,
                                               cM
                                           );

                allConverged = allConverged && constraintConverged;

                // Accumulate constraint force
                cFA += cF;

                // Accumulate constraint moment
                cMA += cM + ((cP - centreOfMass()) ^ cF);
            }

        } while(++iteration < maxConstraintIterations_ && !allConverged);

        if (iteration >= maxConstraintIterations_)
        {
            FatalErrorIn
            (
                "Foam::sixDoFRigidBodyMotion::applyConstraints"
                "(scalar deltaT)"
            )
                    << nl << "Maximum number of sixDoFRigidBodyMotion constraint "
                    << "iterations ("
                    << maxConstraintIterations_
                    << ") exceeded." << nl
                    << exit(FatalError);
        }

        Info<< "sixDoFRigidBodyMotion constraints converged in "
            << iteration << " iterations" << endl;

        if (report_)
        {
            Info<< "Constraint force: " << cFA << nl
                << "Constraint moment: " << cMA
                << endl;
        }

        // Add the constraint forces and moments to the motion state variables
        a() += cFA/mass_;

        // The moment of constraint forces has already been added
        // during accumulation.  Moments are returned in global axes,
        // transforming to body local
        tau() += Q().T() & cMA;
    }
}

//
// inverse approximation
//

#include <stdio.h>
#include <stdlib.h>

#include "liquidfpm.internal.h"

#define DEBUG_INV_NEWTON 0

#define Q(name)     LIQUIDFPM_CONCAT(q32,name)

// computes x = inv(d) = 1/d using iterative Newtonian method:
//   x[k+1] = x[k] + x[k]*(1 - d*x[k])
Q(_t) Q(_inv_newton)( Q(_t) _x, unsigned int _n )
{
    int negate = (_x < 0);
    _x = Q(_abs)(_x);

    // initial guess: x0 = 2^-floor(log2(|_x|))
    int b = liquidfpm_msb_index(_x) - 1;          // base index
    int s = (int)Q(_fracbits) - b - 1;  // shift amount
    Q(_t) x0 = s>0 ? Q(_one)<<s : Q(_one)>>(-s);

    Q(_t) x1=0;
    Q(_t) y0=0;
    Q(_t) d=_x;
    Q(_t) dx0=0;
    Q(_t) x0y0=0;

void SOGP::train(const VectorXd &state, const VectorXd &output) {
    //check if we have initialised the system
    if (!this->initialized) {
        throw OTLException("SOGP not yet initialised");
    }

    double kstar = this->kernel->eval(state);

    //change the output format if this is a classification problem
    VectorXd mod_output;
    if (this->problem_type == SOGP::CLASSIFICATION) {
        mod_output = VectorXd::Zero(this->output_dim);
        for (unsigned int i=0; i<this->output_dim; i++) {
            mod_output(i) = -1;
        }
        mod_output(output(0)) = 1;
    } else {
        mod_output = output;
    }

    //we are just starting.
    if (this->current_size == 0) {
        this->alpha.block(0,0,1, this->output_dim) = (mod_output.array() / (kstar + this->noise)).transpose();
        this->C.block(0,0,1,1) = VectorXd::Ones(1)*-1/(kstar + this->noise);
        this->Q.block(0,0,1,1) = VectorXd::Ones(1)*1/(kstar);
        this->basis_vectors.push_back(state);
        this->current_size++;
        return;
    }

    //Test if this is a "novel" state
    VectorXd k;
    this->kernel->eval(state, this->basis_vectors, k);
    //cache Ck
    VectorXd Ck = this->C.block(0,0, this->current_size, this->current_size)*k;

    VectorXd m = k.transpose()*this->alpha.block(0,0,this->current_size, this->output_dim);
    double s2 = kstar + (k.dot(Ck));

    if (s2 < 1e-12) {
        //std::cout << "s2: " << s2 << std::endl;
        s2 = 1e-12;
    }

    double r = 0.0;
    VectorXd q;

    if (this->problem_type == SOGP::REGRESSION) {
        r = -1.0/(s2 + this->noise);
        q = (mod_output - m)*(-r);
    } else if (this->problem_type == SOGP::CLASSIFICATION) {

        double sx2 = this->noise + s2;
        double sx = sqrt(sx2);
        VectorXd z = VectorXd(this->output_dim);
        VectorXd Erfz = VectorXd(this->output_dim);
        for (unsigned int i=0; i<this->output_dim; i++) {
            z(i) = mod_output(i) * m(i) / sx;
            Erfz(i) = stdnormcdf(z(i));
            //dErfz(i) = 1.0/sqrt(2*M_PI)*exp(-(z(i)*z(i))/2.0);
            //dErfz2(i) = dErfz(i)*(-z(i));
        }

        /*
          TO CONNTINUE
        Erfz = Erf(z);

        dErfz = 1.0/sqrt(2*pi)*exp(-(z.^2)/2);
        dErfz2 = dErfz.*(-z);

        q = y/sx * (dErfz/Erfz);
        r = (1/sx2)*(dErfz2/dErfz - (dErfz/Erfz)^2);

        */
    } else {
        throw OTL::OTLException("Whoops! My problem type is wrong. How did this happen?");
    }
    VectorXd ehat = this->Q.block(0,0, this->current_size, this->current_size)*k;

    double gamma = kstar - k.dot(ehat);
    double eta = 1.0/(1.0 + gamma*r);

    if (gamma < 1e-12) {
        gamma = 0.0;
    }

    if (gamma >= this->epsilon*kstar) {
        //perform a full update
        VectorXd s = Ck;
        s.conservativeResize(this->current_size + 1);
        s(this->current_size) = 1;


        //update Q (inverse of C)
        ehat.conservativeResize(this->current_size+1);
        ehat(this->current_size) = -1;

        MatrixXd diffQ = Q.block(0,0,this->current_size+1, this->current_size+1)
                + (ehat*ehat.transpose())*(1.0/gamma);
        Q.block(0,0,this->current_size+1, this->current_size+1) = diffQ;
//.........这里部分代码省略.........

void EN
( const DistSparseMatrix<Real>& A,
  const DistMultiVec<Real>& b,
        Real lambda1,
        Real lambda2,
        DistMultiVec<Real>& x,
  const qp::affine::Ctrl<Real>& ctrl )
{
    EL_DEBUG_CSE
    const Int m = A.Height();
    const Int n = A.Width();
    const Grid& grid = A.Grid();

    DistSparseMatrix<Real> Q(grid), AHat(grid), G(grid);
    DistMultiVec<Real> c(grid), h(grid);

    // Q := | 2*lambda_2     0      0 |
    //      |     0      2*lambda_2 0 |
    //      |     0          0      2 |
    // ================================
    Zeros( Q, 2*n+m, 2*n+m );
    Q.Reserve( Q.LocalHeight() );
    for( Int iLoc=0; iLoc<Q.LocalHeight(); ++iLoc )
    {
        const Int i = Q.GlobalRow(iLoc);
        if( i < 2*n )
            Q.QueueLocalUpdate( iLoc, i, 2*lambda2 );
        else
            Q.QueueLocalUpdate( iLoc, i, Real(2) );
    }
    Q.ProcessLocalQueues();

    // c := lambda_1*[1;1;0]
    // =====================
    Zeros( c, 2*n+m, 1 );
    for( Int iLoc=0; iLoc<c.LocalHeight(); ++iLoc )
        if( c.GlobalRow(iLoc) < 2*n )
            c.SetLocal( iLoc, 0, lambda1 );

    // \hat A := [A, -A, I]
    // ====================
    // NOTE: Since A and \hat A are the same height and each distributed within
    //       columns, it is possible to form \hat A from A without communication
    const Int numLocalEntriesA = A.NumLocalEntries();
    Zeros( AHat, m, 2*n+m );
    AHat.Reserve( 2*numLocalEntriesA+AHat.LocalHeight() );
    for( Int e=0; e<numLocalEntriesA; ++e )
    {
        AHat.QueueUpdate( A.Row(e), A.Col(e),    A.Value(e) );
        AHat.QueueUpdate( A.Row(e), A.Col(e)+n, -A.Value(e) );
    }
    for( Int iLoc=0; iLoc<AHat.LocalHeight(); ++iLoc )
    {
        const Int i = AHat.GlobalRow(iLoc);
        AHat.QueueLocalUpdate( iLoc, i+2*n, Real(1) );
    }
    AHat.ProcessLocalQueues();

    // G := | -I  0 0 |
    //      |  0 -I 0 |
    // ================
    Zeros( G, 2*n, 2*n+m );
    G.Reserve( G.LocalHeight() );
    for( Int iLoc=0; iLoc<G.LocalHeight(); ++iLoc )
    {
        const Int i = G.GlobalRow(iLoc);
        G.QueueLocalUpdate( iLoc, i, Real(-1) );
    }
    G.ProcessLocalQueues();

    // h := 0
    // ======
    Zeros( h, 2*n, 1 );

    // Solve the affine QP
    // ===================
    DistMultiVec<Real> xHat(grid), y(grid), z(grid), s(grid);
    QP( Q, AHat, G, b, c, h, xHat, y, z, s, ctrl );

    // x := u - v
    // ==========
    Zeros( x, n, 1 );
    Int numRemoteUpdates = 0;
    for( Int iLoc=0; iLoc<xHat.LocalHeight(); ++iLoc )
        if( xHat.GlobalRow(iLoc) < 2*n )
            ++numRemoteUpdates;
        else
            break;
    x.Reserve( numRemoteUpdates );
    for( Int iLoc=0; iLoc<xHat.LocalHeight(); ++iLoc )
    {
        const Int i = xHat.GlobalRow(iLoc);
        if( i < n )
            x.QueueUpdate( i, 0, xHat.GetLocal(iLoc,0) );
        else if( i < 2*n )
            x.QueueUpdate( i-n, 0, -xHat.GetLocal(iLoc,0) );
        else
            break;
    }
    x.ProcessQueues();
//.........这里部分代码省略.........

void EN
( const Matrix<Real>& A,
  const Matrix<Real>& b,
        Real lambda1,
        Real lambda2,
        Matrix<Real>& x,
  const qp::affine::Ctrl<Real>& ctrl )
{
    EL_DEBUG_CSE
    const Int m = A.Height();
    const Int n = A.Width();
    const Range<Int> uInd(0,n), vInd(n,2*n), rInd(2*n,2*n+m);

    Matrix<Real> Q, c, AHat, G, h;

    // Q := | 2*lambda_2     0      0 |
    //      |     0      2*lambda_2 0 |
    //      |     0          0      2 |
    // ================================
    Zeros( Q, 2*n+m, 2*n+m );
    auto QTL = Q( IR(0,2*n), IR(0,2*n) );
    FillDiagonal( QTL, 2*lambda2 );
    auto Qrr = Q( rInd, rInd );
    FillDiagonal( Qrr, Real(1) );

    // c := lambda_1*[1;1;0]
    // =====================
    Zeros( c, 2*n+m, 1 );
    auto cuv = c( IR(0,2*n), ALL );
    Fill( cuv, lambda1 );

    // \hat A := [A, -A, I]
    // ====================
    Zeros( AHat, m, 2*n+m );
    auto AHatu = AHat( ALL, uInd );
    auto AHatv = AHat( ALL, vInd );
    auto AHatr = AHat( ALL, rInd );
    AHatu = A;
    AHatv -= A;
    FillDiagonal( AHatr, Real(1) );

    // G := | -I  0 0 |
    //      |  0 -I 0 |
    // ================
    Zeros( G, 2*n, 2*n+m );
    FillDiagonal( G, Real(-1) );

    // h := 0
    // ======
    Zeros( h, 2*n, 1 );

    // Solve the affine QP
    // ===================
    Matrix<Real> xHat, y, z, s;
    QP( Q, AHat, G, b, c, h, xHat, y, z, s, ctrl );

    // x := u - v
    // ==========
    x = xHat( uInd, ALL );
    x -= xHat( vInd, ALL );
}

void EN
( const AbstractDistMatrix<Real>& A,
  const AbstractDistMatrix<Real>& b,
        Real lambda1,
        Real lambda2,
        AbstractDistMatrix<Real>& xPre,
  const qp::affine::Ctrl<Real>& ctrl )
{
    EL_DEBUG_CSE

    DistMatrixWriteProxy<Real,Real,MC,MR> xProx( xPre );
    auto& x = xProx.Get();

    const Int m = A.Height();
    const Int n = A.Width();
    const Grid& g = A.Grid();
    const Range<Int> uInd(0,n), vInd(n,2*n), rInd(2*n,2*n+m);
    DistMatrix<Real> Q(g), c(g), AHat(g), G(g), h(g);

    // Q := | 2*lambda_2     0      0 |
    //      |     0      2*lambda_2 0 |
    //      |     0          0      2 |
    // ================================
    Zeros( Q, 2*n+m, 2*n+m );
    auto QTL = Q( IR(0,2*n), IR(0,2*n) );
    FillDiagonal( QTL, 2*lambda2 );
    auto Qrr = Q( rInd, rInd );
    FillDiagonal( Qrr, Real(1) );

    // c := lambda_1*[1;1;0]
    // =====================
    Zeros( c, 2*n+m, 1 );
    auto cuv = c( IR(0,2*n), ALL );
    Fill( cuv, lambda1 );

    // \hat A := [A, -A, I]
    // ====================
    Zeros( AHat, m, 2*n+m );
    auto AHatu = AHat( ALL, uInd );
    auto AHatv = AHat( ALL, vInd );
    auto AHatr = AHat( ALL, rInd );
    AHatu = A;
    AHatv -= A;
    FillDiagonal( AHatr, Real(1) );

    // G := | -I  0 0 |
    //      |  0 -I 0 |
    // ================
    Zeros( G, 2*n, 2*n+m );
    FillDiagonal( G, Real(-1) );

    // h := 0
    // ======
    Zeros( h, 2*n, 1 );

    // Solve the affine QP
    // ===================
    DistMatrix<Real> xHat(g), y(g), z(g), s(g);
    QP( Q, AHat, G, b, c, h, xHat, y, z, s, ctrl );

    // x := u - v
    // ==========
    x = xHat( uInd, ALL );
    x -= xHat( vInd, ALL );
}

static RBuffer* create(RBin* bin, const ut8 *code, int codelen, const ut8 *data, int datalen) {
	ut64 filesize, codeva, datava;
	ut32 ncmds, magiclen, headerlen;
	ut64 p_codefsz=0, p_codeva=0, p_codesz=0, p_codepa=0;
	ut64 p_datafsz=0, p_datava=0, p_datasz=0, p_datapa=0;
	ut64 p_cmdsize=0, p_entry=0, p_tmp=0;
	ut64 baddr = 0x100001000LL;
// TODO: baddr must be overriden with -b
	RBuffer *buf = r_buf_new ();

#define B(x,y) r_buf_append_bytes(buf,(const ut8*)x,y)
#define D(x) r_buf_append_ut32(buf,x)
#define Q(x) r_buf_append_ut64(buf,x)
#define Z(x) r_buf_append_nbytes(buf,x)
#define W(x,y,z) r_buf_write_at(buf,x,(const ut8*)y,z)
#define WZ(x,y) p_tmp=buf->length;Z(x);W(p_tmp,y,strlen(y))

	/* MACH0 HEADER */
	// 32bit B ("\xce\xfa\xed\xfe", 4); // header
	B ("\xcf\xfa\xed\xfe", 4); // header
	D (7 | 0x01000000); // cpu type (x86) | ABI64
	//D (3); // subtype (i386-all)
	D(0x80000003); // unknown subtype issue
	D (2); // filetype (executable)

	ncmds = (data && datalen>0)? 3: 2;
	
	/* COMMANDS */
	D (ncmds); // ncmds
	p_cmdsize = buf->length;
	D (-1); // headsize // cmdsize?
	D (0);//0x85); // flags
	D (0); // reserved -- only found in x86-64

	magiclen = buf->length;

	/* TEXT SEGMENT */
	D (0x19);   // cmd.LC_SEGMENT_64
	//D (124+16+8); // sizeof (cmd)
	D (124+28); // sizeof (cmd)
	WZ (16, "__TEXT");
	Q (baddr); // vmaddr
	Q (0x1000); // vmsize XXX

	Q (0); // fileoff
	p_codefsz = buf->length;
	Q (-1); // filesize
	D (7); // maxprot
	D (5); // initprot
	D (1); // nsects
	D (0); // flags
	// define section
	WZ (16, "__text");
	WZ (16, "__TEXT");
	p_codeva = buf->length; // virtual address
	Q (-1);
	p_codesz = buf->length; // size of code (end-start)
	Q (-1);
	p_codepa = buf->length; // code - baddr
	D (-1); // offset, _start-0x1000);
	D (2); // align
	D (0); // reloff
	D (0); // nrelocs
	D (0); // flags
	D (0); // reserved1
	D (0); // reserved2
	D (0); // reserved3

	if (data && datalen>0) {
		/* DATA SEGMENT */
		D (0x19);   // cmd.LC_SEGMENT_64
		D (124+28); // sizeof (cmd)
		p_tmp = buf->length;
		Z (16);
		W (p_tmp, "__TEXT", 6); // segment name
//XXX must be vmaddr+baddr
		Q (0x2000); // vmaddr
//XXX must be vmaddr+baddr
		Q (0x1000); // vmsize
		Q (0); // fileoff
		p_datafsz = buf->length;
		Q (-1); // filesize
		D (6); // maxprot
		D (6); // initprot
		D (1); // nsects
		D (0); // flags

		WZ (16, "__data");
		WZ (16, "__DATA");

		p_datava = buf->length;
		Q (-1);
		p_datasz = buf->length;
		Q (-1);
		p_datapa = buf->length;
		D (-1); //_start-0x1000);
		D (2); // align
		D (0); // reloff
		D (0); // nrelocs
		D (0); // flags
//.........这里部分代码省略.........

/* FIXME: use lldiv once it's fixed to compute quot,rem together */
struct tm *__time_to_tm(time_t t, struct tm *tm)
{
	/* months are march-based */
	static const int days_thru_month[] = {31,61,92,122,153,184,214,245,275,306,337,366};
	long long bigday;
	unsigned int day, year4, year100;
	int year, year400;
	int month;
	int leap;
	int hour, min, sec;
	int wday, mday, yday;

	/* start from 2000-03-01 (multiple of 400 years) */
	t += -946684800 - 86400*(31+29);

	bigday = Q(t, 86400);
	sec = t-bigday*86400;

	hour = sec/3600;
	sec -= hour*3600;
	min = sec/60;
	sec -= min*60;

	/* 2000-03-01 was a wednesday */
	wday = (3+bigday)%7;
	if (wday < 0) wday += 7;

	t = -946684800LL - 86400*(31+29) + 9000000;
	
	year400 = Q(bigday, DAYS_PER_400Y);
	day = bigday-year400*DAYS_PER_400Y;

	year100 = day/DAYS_PER_100Y;
	if (year100 == 4) year100--;
	day -= year100*DAYS_PER_100Y;

	year4 = day/DAYS_PER_4Y;
	if (year4 == 25) year4--;
	day -= year4*DAYS_PER_4Y;

	year = day/365;
	if (year == 4) year--;
	day -= year*365;

	leap = !year && (year4 || !year100);
	yday = day + 31+28 + leap;
	if (yday >= 365+leap) yday -= 365+leap;

	year += 4*year4 + 100*year100 + 400*year400 + 2000-1900;

	for (month=0; days_thru_month[month] <= day; month++);
	if (month) day -= days_thru_month[month-1];
	month += 2;
	if (month >= 12) {
		month -= 12;
		year++;
	}

	mday = day+1;

	tm->tm_sec = sec;
	tm->tm_min = min;
	tm->tm_hour= hour;
	tm->tm_mday= mday;
	tm->tm_mon = month;
	tm->tm_year= year;
	tm->tm_wday= wday;
	tm->tm_yday= yday;
	tm->__tm_zone = 0;
	tm->__tm_gmtoff = 0;

	return tm;
}

QuestManager::QuestManager() {
	Q("Test Quest", A1Q1_Test, a1q1);
}

int HRP2ColorDetectionProcess::RealizeTheProcess()
{
  if (!m_Computing)
    return 0;

  unsigned int lw,lh;
  double x[m_NbOfCameras],y[m_NbOfCameras];
  int x1[2]={-1,-1},x2[2]={-1,-1};
  double a3DPt[3];
  VNL::Vector<double> Q(4);
  VNL::Vector<double> QHead(3);
  if (m_FirstTime)
    {
      ReadRobotMatrix();
      m_FirstTime = false;
    }

  for(int i=0;i<m_NbOfCameras;i++)
    {
      m_ImageInputRGB[i]->GetSize(lw,lh);
      /* VW::ImageConversions::RGBBuffer_to_VWImage((char *)m_InputImage[i].Image,
	 m_ImageInputRGB[i],lw,lh);*/

      m_ColorDetector[i].FilterOnHistogram((unsigned char *)m_InputImage[i].Image,m_ImageResult[i]);


      {
	char Buffer[1024];
	sprintf(Buffer,"IntermediateImageResult_%03d.ppm",i);
	m_ImageResult[i]->WriteImage(Buffer);
      }

      m_ColorDetector[i].ComputeCoG(m_ImageResult[i],x[i],y[i]);

      if ((i==1) && (x[0]>=0.0) && (x[1]>=0.0)
	  && (y[0]>=0.0) && (y[1]>=0.0))
	{

	  x1[0] = (int) x[0]; x1[1] = (int)y[0];
	  x2[0] = (int) x[1]; x2[1] = (int)y[1];
	  m_TM.Triangulation(*m_PI[0],*m_PI[1],x1,x2,a3DPt);
	  for(int j=0;j<3;j++)
	    Q[j] = a3DPt[j];
	  Q[3] = 1.0;

	  QHead = m_HtO * Q;
	  QHead *= 0.001;
	  ODEBUG( "Triangulation: "
		   << a3DPt[0] << " "
		   << a3DPt[1] << " "
		   << a3DPt[2]);
	  ODEBUG3("QHead :" << QHead <<endl );


#if 0
	  if (!CORBA::is_nil(m_VisualServoing))
	    {
	      try
		{
		  // m_VisualServoing->SendTarget2DPosition(x[i],y[i]);
		  CORBA::Double x_left = x[0];
		  CORBA::Double y_left = y[0];
		  CORBA::Double X = QHead[0];
		  CORBA::Double Y = QHead[1];
		  CORBA::Double Z = QHead[2];

		  m_VisualServoing->SendTarget3DPosition(x_left,y_left,X,Y,Z);
		  ODEBUG("Send " << x << " " << y << " to visual servoing");
		}

	      catch(...)
		{

		  ODEBUG("Sorry the data was not send to the visual servoing plugin" );
		  if (m_NbOfProcessWithoutTrialConnection>m_IntervalBetweenConnectionTrials)
		    {
		      CORBA::release(m_VisualServoing);
		      TryConnectionToVisualServoing();
		      m_NbOfProcessWithoutTrialConnection=0;
		    }
		  else
		    m_NbOfProcessWithoutTrialConnection++;

		}
	    }
	  else
	    {
	      if (m_NbOfProcessWithoutTrialConnection>m_IntervalBetweenConnectionTrials)
		{
		  TryConnectionToVisualServoing();
		  m_NbOfProcessWithoutTrialConnection=0;
		}
	      else
		m_NbOfProcessWithoutTrialConnection++;
	    }
#endif
	}
      /*
      if ((x==-1) && (y==-1))
	{
//.........这里部分代码省略.........

int main( ){

    USING_NAMESPACE_ACADO

    // INTRODUCE THE VARIABLES:
    // -------------------------
    DifferentialState     x,y,z,vx,vy,vz,phi,theta,psi,p,q,r,u1,u2,u3,u4;
    // x, y, z : position
    // vx, vy, vz : linear velocity
    // phi, theta, psi : orientation (Yaw-Pitch-Roll = Euler(3,2,1))
    // p, q, r : angular velocity
    // u1, u2, u3, u4 : velocity of the propellers
    Control               vu1,vu2,vu3,vu4;
    // vu1, vu2, vu3, vu4 : derivative of u1, u2, u3, u4
    DifferentialEquation  f;

    // Quad constants
    const double c = 0.00001;
    const double Cf = 0.00065;
    const double d = 0.250;
    const double Jx = 0.018;
    const double Jy = 0.018;
    const double Jz = 0.026;
    const double Im = 0.0001;
    const double m = 0.9;
    const double g = 9.81;
    const double Cx = 0.1;

    // Minimization Weights
    double coeffX = .00001;
    double coeffU = coeffX*0.0001;//0.000000000000001;
    double coeffX2 = coeffX * 100.;
    double coeffX3 = coeffX * 0.00001;
    double coeffO = -coeffX * 0.1;

    // final position (used in the cost function)
    double xf = 0., yf = 0., zf = 20.;

    //
    double T = 8.; //length (in second) of the trajectory predicted in the MPC
    int nb_nodes = 20.; //number of nodes used in the Optimal Control Problem
    // 20 nodes means that the algorithm will discretized the trajectory equally into 20 pieces
    // If you increase the number of node, the solution will be more precise but calculation will take longer (~nb_nodes^2)
    // In ACADO, the commands are piecewise constant functions, constant between each node.
    double tmpc = 0.2; //time (in second) between two activation of the MPC algorithm

    // DEFINE A OPTIMAL CONTROL PROBLEM
    // -------------------------------
    OCP ocp( 0.0, T, nb_nodes );

    // DEFINE THE COST FUNCTION
    // -------------------------------
    Function h, hf;
    h << x;
    h << y;
    h << z;
    h << vu1;
    h << vu2;
    h << vu3;
    h << vu4;
    h << p;
    h << q;
    h << r;

    hf << x;
    hf << y;
    hf << z;

    DMatrix Q(10,10), Qf(3,3);
    Q(0,0) = coeffX;
    Q(1,1) = coeffX;
    Q(2,2) = coeffX;
    Q(3,3) = coeffU;
    Q(4,4) = coeffU;
    Q(5,5) = coeffU;
    Q(6,6) = coeffU;
    Q(7,7) = coeffX2;
    Q(8,8) = coeffX2;
    Q(9,9) = coeffX2;

    Qf(0,0) = 1.;
    Qf(1,1) = 1.;
    Qf(2,2) = 1.;

    DVector reff(3), ref(10);
    ref(0) = xf;
    ref(1) = yf;
    ref(2) = zf;
    ref(3) = 58.;
    ref(4) = 58.;
    ref(5) = 58.;
    ref(6) = 58.;
    ref(7) = 0.;
    ref(8) = 0.;
    ref(9) = 0.;

    reff(0) = xf;
    reff(1) = yf;
    reff(2) = zf;

//.........这里部分代码省略.........

void
MergeWidget::onBrowseChapters() {
  auto fileName = getOpenFileName(QY("Select chapter file"), QY("XML files") + Q(" (*.xml)"), ui->chapters);
  if (!fileName.isEmpty())
    m_config.m_chapters = fileName;
}

hkDemo::Result MirroredMotionDemo::stepDemo()
{
	// Check user input
	{
		if (m_env->m_gamePad->wasButtonPressed( HKG_PAD_BUTTON_1 ))
		{
			for ( int i = 0; i < 2; i++ )
			{
				if (( m_control[i]->getEaseStatus() == hkaDefaultAnimationControl::EASING_IN ) ||
					( m_control[i]->getEaseStatus() == hkaDefaultAnimationControl::EASED_IN ))
				{
					m_control[i]->easeOut( .5f );
				}
				else
				{
					m_control[i]->easeIn( .5f );
				}
			}
		}

		if (m_env->m_gamePad->wasButtonPressed( HKG_PAD_BUTTON_2 ))
		{
			m_wireframe = !m_wireframe;
			setGraphicsState( HKG_ENABLED_WIREFRAME, m_wireframe );
		}

		if (m_env->m_gamePad->wasButtonPressed( HKG_PAD_BUTTON_3 ))
		{
			m_drawSkin = !m_drawSkin;
		}

		if ( m_env->m_gamePad->wasButtonPressed( HKG_PAD_BUTTON_0 ) )
		{
			m_useExtractedMotion = !m_useExtractedMotion;
		}

		if ( m_env->m_gamePad->wasButtonPressed( HKG_PAD_BUTTON_L1 ) )
		{
			m_accumulatedMotion[0].setIdentity();
			m_accumulatedMotion[1].setIdentity();
		}

		if ( m_env->m_gamePad-&g