Vector3d latticeVectorHelper( Vector3d difference_along_normal, Vector3d v_dir ){
   double length = difference_along_normal.length() / (difference_along_normal.normalize().dot(v_dir));
   return v_dir * length;
 }

/*Animate the camera in the timer event function
  Note: La transformation de la camera est le seul parametre a etre animee. 
  Changer la position (ou le vecteur lateral ou up) ne fera rien lors de 
  l'animation.*/
void Widget3d::timerEvent( QTimerEvent* ipE )
{
  if ( ipE->timerId() == mAnimationTimerId )
  {
    double animationTime = min( mAnimationDuration, mAnimationTimer.elapsed() );
    double t = animationTime / (double)mAnimationDuration;
    t = inversePower(t, 3);
    
    /*Ici, on bircole une matrice (main droite) orthonormale qui réprésente
      l'orientation de la caméra (pos, lat, up, look). On interpolera
      la vielle orientation avec la nouvelle afin d'obtenir les positions,
      ainsi que les vecteurs lat, up et look intermédiaires.*/
    Vector3d lookVector( mOldCam.getLook(), mOldCam.getPos() );
    lookVector.normalise();
    Matrix4 m1( mOldCam.getLat(), mOldCam.getUp(), lookVector );
    m1 = Matrix4( toVector(mOldCam.getPos()) ) * m1;
    lookVector.set( mNewCam.getLook(), mNewCam.getPos() );
    lookVector.normalise();
    Matrix4 m2( mNewCam.getLat(), mNewCam.getUp(), lookVector );
    m2 = Matrix4( toVector(mNewCam.getPos()) ) * m2;
    
    Matrix4 iterationMatrix = 
      math::interpolate( m1, m2, t );
    Vector3d interpolatedLook = toVector(mOldCam.getLook()) * (1 - t) +
      toVector(mNewCam.getLook()) * t;
    mCam.set( toPoint(iterationMatrix.getTranslationAsVector()),
      toPoint(interpolatedLook),
      Vector3d( iterationMatrix(1, 0), iterationMatrix(1, 1), iterationMatrix(1, 2) ) );

    //--- animation de la projection    
    Camera::Projection iProj = mNewCam.getProjection();
    Camera::Projection oldProj = mOldCam.getProjection();
    Camera::Projection newProj = mNewCam.getProjection();
    iProj.mRight = oldProj.mRight * (1-t) + newProj.mRight * t;
    iProj.mLeft = oldProj.mLeft * (1-t) + newProj.mLeft * t;
    iProj.mBottom = oldProj.mBottom * (1-t) + newProj.mBottom * t;
    iProj.mTop = oldProj.mTop * (1-t) + newProj.mTop * t;
    iProj.mNear = oldProj.mNear * (1-t) + newProj.mNear * t;
    iProj.mFar = oldProj.mFar * (1-t) + newProj.mFar * t;
    iProj.mZoomFactor =  oldProj.mZoomFactor * (1-t) + newProj.mZoomFactor * t;
    iProj.mType = newProj.mType;
    iProj.mProportionalToWindow = newProj.mProportionalToWindow;
    mCam.setProjection( iProj );
    mCam.applyProjectionTransformation();
    
    if ( animationTime >= mAnimationDuration )
    {
      killTimer( mAnimationTimerId );
      mAnimationTimerId = 0;
    }

    update();
  }
  else if( ipE->timerId() == mCameraControlTimerId && !isAnimatingCamera() )
  {
    switch ( getControlType() ) 
    {
      case ctRotateAround:
      {
        Vector3d v; double a = 1.0 * PI / 180;
        if( isKeyPressed( Qt::Key_W ) )
        { v += mCam.getLat(); a *= -1; }
        if( isKeyPressed( Qt::Key_S ) )
        { v += mCam.getLat(); }
        if( isKeyPressed( Qt::Key_A ) )
        { v += Vector3d(0, 1, 0); a *= -1; }
        if( isKeyPressed( Qt::Key_D ) )
        { v += Vector3d(0, 1, 0); }
        mCam.rotate( a, v, mCam.getLook() );
      }break;
      case ctFree:
      {
        Vector3d v;
        if( isKeyPressed( Qt::Key_W ) )
        { v += Vector3d( mCam.getPos(), mCam.getLook() ); }
        if( isKeyPressed( Qt::Key_S ) )
        { v += -Vector3d( mCam.getPos(), mCam.getLook() ); }
        if( isKeyPressed( Qt::Key_A ) )
        { v += mCam.getLat() * -1; }
        if( isKeyPressed( Qt::Key_D ) )
        { v += mCam.getLat(); }
        if( isKeyPressed( Qt::Key_Q ) )
        { v += mCam.getUp(); }
        if( isKeyPressed( Qt::Key_E ) )
        { v += mCam.getUp() * -1; }
        v.normalise();
        mCam.translate( v * getCameraSpeed() );
      } break;
      default: break;
    }
    update();
  }
  else
  {
    QWidget::timerEvent( ipE );
  }
//.........这里部分代码省略.........

void ParticleState::setDirection(const Vector3d &dir) {
	direction = dir / dir.getR();
}

char* MyController::sendSceneInfo(std::string header, int camID) {
	
	m_my->setWheelVelocity(0.0, 0.0);				

	Vector3d myPos;
	m_my->getPosition(myPos);
	double x = myPos.x();
	double z = myPos.z();
	double theta = calcHeadingAngle();			// y方向の回転は無しと考える	

	// カメラがついているリンク名取得
	std::string link = ""; //"WAIST_LINK0"; //m_my->getCameraLinkName(3);
	if (camID < 3) {
		link = m_my->getCameraLinkName(camID);
	} else {
		link = "WAIST_LINK0";
	}
	//const dReal *lpos = m_my->getParts(link.c_str())->getPosition();
	Vector3d lpos;
	m_my->getParts(link.c_str())->getPosition(lpos);

	// カメラの位置取得(リンク座標系)
	Vector3d cpos;
	m_my->getCamPos(cpos, camID);

	// カメラの位置(絶対座標系, ロボットの回転はないものとする)
	//printf("linkpos: %lf %lf %lf \n", lpos.x(), lpos.y(), lpos.z());
	//printf("camerapos: %lf %lf %lf \n", cpos.x(), cpos.y(), cpos.z());
	Vector3d campos(lpos.x() + cpos.z() * sin(DEG2RAD(theta)), 
									lpos.y() + cpos.y(), 
									lpos.z() + cpos.z() * cos(DEG2RAD(theta)));
	//Vector3d campos(cpos.x(), cpos.y(), cpos.z());

	// カメラの方向取得(ロボットの回転,関節の回転はないものとする)
	Vector3d cdir;
	m_my->getCamDir(cdir, camID);

	char *replyMsg = new char[1024];
	sprintf(replyMsg, "%s %6.1lf %6.1lf %6.1lf %6.1lf %6.1lf %6.1lf %6.1lf %6.1lf %6.1lf", 
						header.c_str(), x, z, theta, campos.x(), campos.y(), campos.z(), cdir.x(), cdir.y(), cdir.z());
	printf("%s \n", replyMsg);
	m_srv->sendMsgToSrv(replyMsg);

	return replyMsg;
}

  void MomentumCorrFunc::correlateFrames(int frame1, int frame2) {
    SimInfo::MoleculeIterator mi1;
    SimInfo::MoleculeIterator mi2;
    Molecule* mol1;
    Molecule* mol2;
    Molecule::AtomIterator ai1;
    Molecule::AtomIterator ai2;
    Atom* atom1;
    Atom* atom2;

    std::vector<Vector3d> atomPositions1;
    std::vector<Vector3d> atomPositions2;
    std::vector<Vector3d> atomVelocity1;
    std::vector<Vector3d> atomVelocity2;
    int thisAtom1, thisAtom2;

    Snapshot* snapshot1 = bsMan_->getSnapshot(frame1);
    Snapshot* snapshot2 = bsMan_->getSnapshot(frame2);

    assert(snapshot1 && snapshot2);

    RealType time1 = snapshot1->getTime();
    RealType time2 = snapshot2->getTime();
       
    int timeBin = int ((time2 - time1) /deltaTime_ + 0.5);

    updateFrame(frame1);       
    atomPositions1.clear();
    for (mol1 = info_->beginMolecule(mi1); mol1 != NULL; 
         mol1 = info_->nextMolecule(mi1)) {
      for(atom1 = mol1->beginAtom(ai1); atom1 != NULL; 
          atom1 = mol1->nextAtom(ai1)) {        
        atomPositions1.push_back(atom1->getPos(frame1));
        atomVelocity1.push_back(atom1->getVel(frame1));
      }
    }
    updateFrame(frame2);       
    atomPositions2.clear();
    for (mol2 = info_->beginMolecule(mi2); mol2 != NULL; 
         mol2 = info_->nextMolecule(mi2)) {
      for(atom2 = mol2->beginAtom(ai2); atom2 != NULL; 
          atom2 = mol2->nextAtom(ai2)) {        
        atomPositions2.push_back(atom2->getPos(frame2));
        atomVelocity2.push_back(atom2->getVel(frame2));
      }
    }

    thisAtom1 = 0;

    for (mol1 = info_->beginMolecule(mi1); mol1 != NULL; 
         mol1 = info_->nextMolecule(mi1)) {
      for(atom1 = mol1->beginAtom(ai1); atom1 != NULL; 
          atom1 = mol1->nextAtom(ai1)) {
        
        Vector3d r1 = atomPositions1[thisAtom1];
        Vector3d p1 = atom1->getMass() * atomVelocity1[thisAtom1];

     	thisAtom2 = 0;

        for (mol2 = info_->beginMolecule(mi2); mol2 != NULL; 
             mol2 = info_->nextMolecule(mi2)) {
          for(atom2 = mol2->beginAtom(ai2); atom2 != NULL; 
              atom2 = mol2->nextAtom(ai2)) {
            
            Vector3d r2 = atomPositions2[thisAtom2];
            Vector3d p2 = atom2->getMass()  * atomVelocity1[thisAtom1];

            Vector3d deltaPos = (r2-r1);
            Vector3d dp2( deltaPos.x() * deltaPos.x(),
                          deltaPos.y() * deltaPos.y(),
                          deltaPos.z() * deltaPos.z());
            Vector3d pprod( p1.x() * p2.x(),
                            p1.y() * p2.y(),
                            p1.z() * p2.z());
            
            histogram_[timeBin] += outProduct(dp2, pprod);
                                           
            thisAtom2++;                    
          }
        }
        
        thisAtom1++;
      } 
    }
    
    count_[timeBin]++;
    
  }

double vectorAngle(const Vector3d &r1,const Vector3d &r2){
  return(acos(r1.dot(r2)/r1.norm()/r2.norm()));
}

bool ContainedByBox::Inside(const Vector3d& point) const
{
    return ((point.x() >= corner1.x()) && (point.x() <= corner2.x()) &&
            (point.y() >= corner1.y()) && (point.y() <= corner2.y()) &&
            (point.z() >= corner1.z()) && (point.z() <= corner2.z()));
}

// Questa funzione e' quella che in background fa tutti i conti matematici, quindi qui devi inserire 
// 1) Scrittura su file continua delle coordinate che vuoi salvare
// 2) Estrazione delle coordinate a partire dai corpi rigidi precedentemente definiti vedi ad esempio
// come e' fatto per eyeLeft e eyeRight oppure per thumb ed index
void idle()
{
	optotrak->updateMarkers();
	//cerr << deltaT << endl;
	markers = optotrak->getAllMarkers();
	// Coordinates picker
	allVisiblePlatform = isVisible(markers[1].p);
	allVisibleThumb = isVisible(markers[15].p) && isVisible(markers[17].p) && isVisible(markers[18].p);
	allVisibleIndex = isVisible(markers[13].p) && isVisible(markers[14].p) && isVisible(markers[16].p);
	allVisibleFingers = allVisibleThumb && allVisibleIndex;

	allVisiblePatch = isVisible(markers[5].p) && isVisible(markers[6].p) && isVisible(markers[7].p);
	allVisibleHead = allVisiblePatch && isVisible(markers[1].p);

	if ( allVisiblePatch )
		headEyeCoords.update(markers[5].p,markers[6].p,markers[7].p);


	if ( allVisibleThumb )
		thumbCoords.update(markers[15].p,markers[17].p,markers[18].p);
	if ( allVisibleIndex )
		indexCoords.update(markers[13].p, markers[14].p, markers[16].p );
	if ( headCalibrationDone==3 && fingerCalibrationDone==3 )
		{
		if ( !allVisibleIndex )
			occludedFrames++;
		if ( !allVisibleThumb )
			occludedFrames++;
		}
	
	if(headCalibration)
	{
	eyeLeft = headEyeCoords.getLeftEye();
	eyeRight = headEyeCoords.getRightEye();
	} else	{
	eyeRight = Vector3d(interoculardistance/2,0,0);
	eyeLeft = -eyeRight;
	}

	thumb = thumbCoords.getP1();
	index = indexCoords.getP1();

	singleMarker = markers.at(4).p.transpose();

	if( abs(index.z()-thumb.z()) > 15.0 )
		fingersAtStart=false;
	else
		fingersAtStart=true;

	if( endTrial && fingersAtStart)
	{
		advanceTrial();
	} 

	#ifdef WRITE
	// Write to file
	if ( headCalibrationDone==3 && fingerCalibrationDone==3 )
		{
		markersFile << fixed << trialNumber << "\t" << 
			eyeLeft.transpose() << "\t" << eyeRight.transpose()  << "\t" <<
			//markers.at(4).p.transpose() << "\t" << 
			index.transpose() << "\t" << 
			thumb.transpose() << "\t" << 
			fingersAtStart << "\t" <<
			isStimulusDrawn
			;

		markersFile << endl;
		}
#endif

}

void Gelatin::step(double h, const Vector3d &grav, const vector< shared_ptr<FreakFace> > faces)
{
	aTrips.clear();
	//v.setZero();
	//f.setZero();
//
//	vTrips.clear();
//    fTrips.clear();

    VectorXd b(n);
    b.setZero();

	for (int i = 0; i < particles.size(); i++) {
	    if (!particles[i]->fixed) {
	        int index = particles[i]->i;
	        double m = particles[i]->m;

	        double val = m + h * damping(0) * m;
            aTrips.push_back(Trip(index, index, val));
            aTrips.push_back(Trip(index+1, index+1, val));
            aTrips.push_back(Trip(index+2, index+2, val));

            Vector3d v = particles[i]->v;
            Vector3d fg = h * m * grav;
            b(index) = m * v(0) + fg(0);
            b(index+1) = m * v(1) + fg(1);
            b(index+2) = m * v(2) + fg(2);
        }
	}

	for (int i = 0; i < springs.size(); i++) {
	    double E = springs[i]->E;
	    double L = springs[i]->L;
	    auto p0 = springs[i]->p0;
	    auto p1 = springs[i]->p1;

	    //MatrixXd delta(3, 1);
        Vector3d delta = p1->x - p0->x;
	    double l = delta.norm();

	    Vector3d fs = E * (l - L) * (delta / l);

	    if (!springs[i]->p0->fixed) {
	        b(p0->i) += h * fs(0);
	        b(p0->i+1) += h * fs(1);
	        b(p0->i+2) += h * fs(2);
	    }
	    if (!springs[i]->p1->fixed) {
	        b(p1->i) -= h * fs(0);
            b(p1->i+1) -= h * fs(1);
            b(p1->i+2) -= h * fs(2);
	    }

        if (!springs[i]->p0->fixed && !springs[i]->p1->fixed) {
            Matrix3d ks =  (E / (l * l)) * ((1.0 - (l - L)/l) * (delta * delta.transpose())
                + ((l - L)/l) * double(delta.transpose() * delta) * Matrix3d::Identity());
            int i0 = p0->i;
            int i1 = p1->i;

            addKs(ks, i0, i1, h);
        }
	}

    Eigen::SparseMatrix<double> sparseA(n,n);
    sparseA.setFromTriplets(aTrips.begin(), aTrips.end());

    ConjugateGradient< SparseMatrix<double> > cg;
    cg.setMaxIterations(25);
    cg.setTolerance(1e-3);
    cg.compute(sparseA);

    //v = cg.solve(b);
    v = cg.solveWithGuess(b, v);

	//v = A.ldlt().solve(b);

	for (int i = 0; i < particles.size(); i++) {
	    if (!particles[i]->fixed) {
            int index = particles[i]->i;
            particles[i]->v = v.block<3,1>(index, 0);
            particles[i]->x += h * particles[i]->v;
        }
	}

	collide(faces);

	// Update position and normal buffers
	updatePosNor();
}

void ot_index_box(const Vector3d& min_point, const Vector3d& max_point, OT_ID *id)
{
// TODO OPTIMIZE

    DBL dx, dy, dz, desiredSize;
    DBL bsized, maxord;
    POW2OP_DECLARE()

    // Calculate the absolute minimum required size of the node, assuming it is perfectly centered within the node;
    // Node size must be a power of 2, and be large enough to accomodate box's biggest dimensions with maximum overhang to all sides

    // compute ideal size of the node for a perfect fit without any overhang
    dx = max_point.x() - min_point.x();
    dy = max_point.y() - min_point.y();
    dz = max_point.z() - min_point.z();
    desiredSize = max3(dx, dy, dz);

    // compute ideal size of the node for a perfect fit with full overhang to all sides
    // desiredSize /= (1 + 2 * 0.5);

    // compute best-matching power-of-two size for a perfect fit with overhang
    // (Note: theoretically this might pick a size larger than required if desiredSize is already a power of two)
    // desiredSize *= 2.0;
    POW2OP_FLOOR(bsized,desiredSize)

    // avoid divisions by zero
    if(bsized == 0.0)
        bsized = 1.0;

#ifdef SAFE_METHOD

    // This block checks for the case where the node id would cause integer
    // overflow, since it is a small buffer far away
    maxord = max3(fabs(min_point[X]), fabs(min_point[Y]), fabs(min_point[Z]));
    maxord += OT_BIAS;
    while (maxord / bsized > 1000000000.0)
    {
#ifdef RADSTATS
        overflows++;
#endif
        bsized *= 2.0;
    }
#endif // SAFE_METHOD

    // The node we chose so far would be ideal for a box of identical size positioned at the node's center,
    // but the actual box is probably somewhat off-center and therefore may have excessive overhang in some directions;
    // check and possibly fix this.

    Vector3d center = (min_point + max_point) / 2;
    id->x = (int) floor((center[X] + OT_BIAS) / bsized);
    id->y = (int) floor((center[Y] + OT_BIAS) / bsized);
    id->z = (int) floor((center[Z] + OT_BIAS) / bsized);
    POW2OP_ENCODE(id->Size, bsized)

#ifdef RADSTATS
    thisloops = 0;
#endif
    while (!ot_point_in_node(min_point, id) || !ot_point_in_node(max_point, id))
    {
        // Debug_Info("looping %d,%d,%d,%d  min=%d, max=%d\n", test_id.x, test_id.y,
        // test_id.z, test_id.Size, ot_point_in_node(min_point, &test_id),
        // ot_point_in_node(max_point, &test_id));
        ot_parent(id, id);
#ifdef RADSTATS
        totloops++;
        thisloops++;
#endif
    }
#ifdef RADSTATS
    if (thisloops < minloops)
        minloops = thisloops;
    if (thisloops > maxloops)
        maxloops = thisloops;
#endif

#ifdef OT_DEBUG
    if (id->Size > 139)
    {
        Debug_Info("unusually large id, maxdel=%.4f, bsized=%.4f, isize=%d\n",
                   maxdel, bsized, id->Size);
    }
#endif
}

void MotionModelScanMatcher::estimateCovariance(RobotPose max_pose, PoseGrid& grid, int use_fine_grid){

  int posegrid_x_dim = pose_x_dim;
  int posegrid_y_dim = pose_y_dim;
  if(use_fine_grid) posegrid_x_dim = fine_pose_x_dim;
  if(use_fine_grid) posegrid_y_dim = fine_pose_y_dim;

  Matrix3d cov_temp;
  Matrix3d K = Matrix3d::Constant(0.0);
  Vector3d u(0.0, 0.0, 0.0);

  // first normalize grid
  double sum = 0;
  double offset = grid.getVal(max_pose_ind.theta, max_pose_ind.y, max_pose_ind.x) - 25;
  double phat;
  double logp;

  RobotPose p, norm_p;
  for (int theta_ind = 0; theta_ind < pose_theta_dim; theta_ind++) {
    p.theta = theta_ind*theta_step;
    for (int x_ind = 0; x_ind < posegrid_x_dim; x_ind++) {
      p.x = (x_ind + pose_grid_offset_x - occ_x_dim/2)*x_step;
      for (int y_ind = 0; y_ind < posegrid_y_dim; y_ind++) {
        p.y = (y_ind + pose_grid_offset_y - occ_x_dim/2)*y_step;
        //p is now in the same frame as max_pose

        logp = grid.getVal(theta_ind, y_ind, x_ind);

        if(logp == -DBL_MAX){
          continue;
        }
        if(sum <= -DBL_MAX/2 || sum > DBL_MAX/2){
          ROS_WARN("[MotionModelScanMatcher::estimateCovariance] - SUM NEAR OVERFLOW!!!!!!!!!!!");
        }

        //re-scale the log prob to an actual prob
        //ROS_INFO("[MotionModelScanMatcher::estimateCovariance]\tlogp=%f",logp);
        phat = exp(logp - offset); // legit fudge but overflows still
        //phat = exp((logp / 10) + offset); // fudge

        //calc running sums
        Vector3d pose = p.toEigenVector();
        Matrix3d plus_k = phat * (pose * pose.transpose()); 
        K = K + plus_k;
        u = u + (pose * phat);
        sum += phat;
        /*
        if(phat != 0){
          ROS_INFO("[MotionModelScanMatcher::estimateCovariance] - phat = %f,\t logp = %f, offset = %f", phat, logp, offset);
          ROS_INFO("[MotionModelScanMatcher::estimateCovariance] - u = [%f %f %f]", u(0,0), u(1,0), u(2,0));
        }
        */

      }
    }
  }

  cov_temp = K / sum + (u * u.transpose())/(pow(sum,2));

  //move cov_temp's values to cov
  for(int i = 0; i < 9; i ++){
    int x_ind = i % 3;
    int y_ind = i / 3;
    cov[i] = cov_temp(x_ind, y_ind);
  }

  ROS_INFO("[MotionModelScanMatcher::estimateCovariance]\tSigma=[%e\t%e\t%e]",cov[0],cov[1],cov[2]);
  ROS_INFO("                                            \t      [%e\t%e\t%e]",cov[3],cov[4],cov[5]);
  ROS_INFO("                                            \t      [%e\t%e\t%e]",cov[6],cov[7],cov[8]);
}

//.........这里部分代码省略.........
    for (i=0; i<mxGetNumberOfElements(mxBodies);i++) {
      mxArray* mxBodyContactPts = mxGetCell(mxContactPts,i);
      int nc = mxGetNumberOfElements(mxBodyContactPts);
      if (nc<1) continue;
      
      SupportStateElement se;
      se.body_idx = (int) pBodies[i]-1;
      pr = mxGetPr(mxBodyContactPts); 
      for (j=0; j<nc; j++) {
        se.contact_pt_inds.insert((int)pr[j]-1);
      }
      se.contact_surface = (int) pContactSurfaces[i]-1;
      
      active_supports.push_back(se);
      num_active_contact_pts += nc;
      contact_bodies.insert((int)se.body_idx); 
    }
  }

  pdata->r->HandC(q,qd,(MatrixXd*)NULL,pdata->H,pdata->C,(MatrixXd*)NULL,(MatrixXd*)NULL,(MatrixXd*)NULL);

  pdata->H_float = pdata->H.topRows(6);
  pdata->H_act = pdata->H.bottomRows(nu);
  pdata->C_float = pdata->C.head(6);
  pdata->C_act = pdata->C.tail(nu);

  bool include_angular_momentum = (pdata->W_kdot.array().maxCoeff() > 1e-10);

  if (include_angular_momentum) {
    pdata->r->getCMM(q,qd,pdata->Ag,pdata->Agdot);
    pdata->Ak = pdata->Ag.topRows(3);
    pdata->Akdot = pdata->Agdot.topRows(3);
  }
  Vector3d xcom;
  // consider making all J's into row-major
  
  pdata->r->getCOM(xcom);
  pdata->r->getCOMJac(pdata->J);
  pdata->r->getCOMJacDot(pdata->Jdot);
  pdata->J_xy = pdata->J.topRows(2);
  pdata->Jdot_xy = pdata->Jdot.topRows(2);

  MatrixXd Jcom,Jcomdot;

  if (x0.size()==6) {
    Jcom = pdata->J;
    Jcomdot = pdata->Jdot;
  }
  else {
    Jcom = pdata->J_xy;
    Jcomdot = pdata->Jdot_xy;
  }
  Map<VectorXd> qdvec(qd,nq);
  
  MatrixXd B,JB,Jp,Jpdot,normals;
  int nc = contactConstraintsBV(pdata->r,num_active_contact_pts,mu,active_supports,pdata->map_ptr,B,JB,Jp,Jpdot,normals,terrain_height);
  int neps = nc*dim;

  VectorXd x_bar,xlimp;
  MatrixXd D_float(6,JB.cols()), D_act(nu,JB.cols());
  if (nc>0) {
    if (x0.size()==6) {
      // x,y,z com 
      xlimp.resize(6); 
      xlimp.topRows(3) = xcom;
      xlimp.bottomRows(3) = Jcom*qdvec;

int main( int /*argc*/, char* argv[] )
{
    const static string ui_file = "app.clicks";

    // Load configuration data
    pangolin::ParseVarsFile("app.cfg");

    InputRecordRepeat input("ui.");
    input.LoadBuffer(ui_file);

    // Target to track from
    Target target;
    target.GenerateRandom(60,25/(842.0/297.0),75/(842.0/297.0),15/(842.0/297.0),Vector2d(297,210));
    //  target.GenerateCircular(60,20,50,15,makeVector(210,210));
    //  target.GenerateEmptyCircle(60,25,75,15,200,makeVector(297,210));
    target.SaveEPS("target_A4.eps");
    cout << "Calibration target saved as: target_A4.eps" << endl;

    // Setup Video
    Var<string> video_uri("video_uri");
    VideoRecordRepeat video(video_uri, "video.pvn", 1024*1024*200);
//    VideoInput video(video_uri);

    const unsigned w = video.Width();
    const unsigned h = video.Height();

    // Create Glut window
    pangolin::CreateGlutWindowAndBind("Main",2*w+PANEL_WIDTH,h);

    // Pangolin 3D Render state
    pangolin::OpenGlRenderState s_cam;
    s_cam.Set(ProjectionMatrixRDF_TopLeft(640,480,420,420,320,240,1,1E6));
    s_cam.Set(FromTooN(toTooN(Sophus::SE3(Sophus::SO3(),Vector3d(-target.Size()[0]/2,-target.Size()[1]/2,500) ))));
    pangolin::Handler3D handler(s_cam);

    // Create viewport for video with fixed aspect
    View& vPanel = pangolin::CreatePanel("ui").SetBounds(1.0,0.0,0,PANEL_WIDTH);
    View& vVideo = pangolin::Display("Video").SetAspect((float)w/h);
    View& v3D    = pangolin::Display("3D").SetAspect((float)w/h).SetHandler(&handler);
    //  View& vDebug = pangolin::Display("Debug").SetAspect((float)w/h);

    Display("Container")
            .SetBounds(1.0,0.0,PANEL_WIDTH,1.0,false)
            .SetLayout(LayoutEqual)
            .AddDisplay(vVideo)
            .AddDisplay(v3D)
            //      .AddDisplay(vDebug)
            ;

    // OpenGl Texture for video frame
    GlTexture texRGB(w,h,GL_RGBA8);
    GlTexture tex(w,h,GL_LUMINANCE8);

    // Declare Image buffers
    CVD::ImageRef size(w,h);
    Image<Rgb<byte> > Irgb(size);
    Image<byte> I(size);
    Image<float> intI(size);
    Image<short> lI(size);
    Image<std::array<float,2> > dI(size);
    Image<byte> tI(size);

    // Camera parameters
    Matrix<float,5,1> cam_params; // = Var<Matrix<float,5,1> >("cam_params");
    cerr << "Not loading params properly" << endl;
    cam_params << 0.694621, 0.925258, 0.505055, 0.484551, 0.968455;
    FovCamera cam( w,h, w*cam_params[0],h*cam_params[1], w*cam_params[2],h*cam_params[3], cam_params[4] );

    // Last good pose
    Sophus::SE3 T_gw;
    std::clock_t last_good;
    int good_frames;

    // Pose hypothesis
    Sophus::SE3 T_hw;

    // Fixed mirrored pose
    Sophus::SE3 T_0w;

    // Stored keyframes
    boost::ptr_vector<Keyframe> keyframes;

    // Variables
    Var<bool> record("ui.Record",false,false);
    Var<bool> play("ui.Play",false,false);
    Var<bool> source("ui.Source",false,false);

    Var<bool> add_keyframe("ui.Add Keyframe",false,false);

    Var<bool> use_mirror("ui.Use Mirror",false,true);
    Var<bool> draw_mirror("ui.Draw Mirror",false,true);
//    Var<bool> calc_mirror_pose("ui.Calculate Mirrored Pose",false,false);

    Var<bool> disp_thresh("ui.Display Thresh",false);
    Var<float> at_threshold("ui.Adap Threshold",1.0,0,1.0);
    Var<int> at_window("ui.Adapt Window",w/3,1,200);
    Var<float> conic_min_area(".Conic min area",25, 0, 100);
    Var<float> conic_max_area(".Conic max area",4E4, 0, 1E5);
    Var<float> conic_min_density(".Conic min density",0.4, 0, 1.0);
    Var<float> conic_min_aspect(".Conic min aspect",0.1, 0, 1.0);
//.........这里部分代码省略.........

void MapInitializer::addObservation(Vector3d & X, Vector2d pt, Transformation<double> & pose,
        const ICamera * cam, const Transformation<double> & TbaseCam)
{
    CostFunction * costFunc = new ReprojectionErrorStereo(pt, TbaseCam, cam);
    problem.AddResidualBlock(costFunc, NULL, X.data(), pose.transData(), pose.rotData());
}

  void BondAngleDistribution::process() {
    Molecule* mol;
    Atom* atom;
    RigidBody* rb;
    int myIndex;
    SimInfo::MoleculeIterator mi;
    Molecule::RigidBodyIterator rbIter;
    Molecule::AtomIterator ai;
    StuntDouble* sd;
    Vector3d vec;
    std::vector<Vector3d> bondvec;
    RealType r;    
    int nBonds;    
    int i;
    
    DumpReader reader(info_, dumpFilename_);    
    int nFrames = reader.getNFrames();
    frameCounter_ = 0;
    
    nTotBonds_ = 0;
    
    for (int istep = 0; istep < nFrames; istep += step_) {
      reader.readFrame(istep);
      frameCounter_++;
      currentSnapshot_ = info_->getSnapshotManager()->getCurrentSnapshot();
      
      if (evaluator_.isDynamic()) {
        seleMan_.setSelectionSet(evaluator_.evaluate());
      }

      // update the positions of atoms which belong to the rigidbodies
      
      for (mol = info_->beginMolecule(mi); mol != NULL; 
           mol = info_->nextMolecule(mi)) {
        for (rb = mol->beginRigidBody(rbIter); rb != NULL; 
             rb = mol->nextRigidBody(rbIter)) {
          rb->updateAtoms();
        }        
      }           
            
      // outer loop is over the selected StuntDoubles:

      for (sd = seleMan_.beginSelected(i); sd != NULL; 
           sd = seleMan_.nextSelected(i)) {

        myIndex = sd->getGlobalIndex();
        nBonds = 0;
        bondvec.clear();
        
        // inner loop is over all other atoms in the system:
        
        for (mol = info_->beginMolecule(mi); mol != NULL; 
             mol = info_->nextMolecule(mi)) {
          for (atom = mol->beginAtom(ai); atom != NULL; 
               atom = mol->nextAtom(ai)) {

            if (atom->getGlobalIndex() != myIndex) {

              vec = sd->getPos() - atom->getPos();       

              if (usePeriodicBoundaryConditions_) 
                currentSnapshot_->wrapVector(vec);
              
              // Calculate "bonds" and make a pair list 
              
              r = vec.length();
              
              // Check to see if neighbor is in bond cutoff 
              
              if (r < rCut_) { 
                // Add neighbor to bond list's
                bondvec.push_back(vec);
                nBonds++;
                nTotBonds_++;
              }  
            }
          }
          
          
          for (int i = 0; i < nBonds-1; i++ ){
            Vector3d vec1 = bondvec[i];
            vec1.normalize();
            for(int j = i+1; j < nBonds; j++){
              Vector3d vec2 = bondvec[j];
              
              vec2.normalize();
	      
              RealType theta = acos(dot(vec1,vec2))*180.0/NumericConstant::PI;
              
              
              if (theta > 180.0){
                theta = 360.0 - theta;
              }
              int whichBin = int(theta/deltaTheta_);
              
              histogram_[whichBin] += 2;
            }
          }           
	      }
      }
//.........这里部分代码省略.........

// Convert to Printlines
void Layer::MakePrintlines(Vector3d &lastPos, //GCodeState &state,
			   vector<PLine3> &lines3,
			   double offsetZ,
			   Settings &settings) const
{
  const double linewidth      = settings.GetExtrudedMaterialWidth(thickness);
  const double cornerradius   = linewidth*settings.get_double("Slicing","CornerRadius");

  const bool clipnearest      = settings.get_boolean("Slicing","MoveNearest");

  const uint supportExtruder  = settings.GetSupportExtruder();
  const double minshelltime   = settings.get_double("Slicing","MinShelltime");

  const double maxshellspeed  = settings.get_double("Extruder","MaxShellSpeed");
  const bool ZliftAlways      = settings.get_boolean("Extruder","ZliftAlways");

  Vector2d startPoint(lastPos.x(),lastPos.y());

  const double extr_per_mm = settings.GetExtrusionPerMM(thickness);

  //vector<PLine3> lines3;
  Printlines printlines(this, &settings, offsetZ);

  vector<PLine2> lines;

  vector<Poly> polys; // intermediate collection

  // polys to keep line movements inside
  //const vector<Poly> * clippolys = &polygons;
  const vector<Poly> * clippolys = GetOuterShell();

  // 1. Skins, all but last, because they are the lowest lines, below layer Z
  if (skins > 1) {
    for(uint s = 0; s < skins; s++) {
      // z offset from bottom to top:
      double skin_z = Z - thickness + (s+1)*thickness/skins;
      if ( skin_z < 0 ){
	cerr << "Skin Z<0! " << s << " -- " << Z << " -- "<<skin_z <<" -- " << thickness <<  endl;
	continue;
      }

      // skin infill polys:
      if (skinFullInfills[s])
	polys.insert(polys.end(),
		     skinFullInfills[s]->infillpolys.begin(),
		     skinFullInfills[s]->infillpolys.end());
      // add skin infill to lines
      printlines.addPolys(INFILL, polys, false);

      polys.clear();

      // make polygons at skin_z:
      for(size_t p = 0; p < skinPolygons.size(); p++) {
	polys.push_back(Poly(skinPolygons[p], skin_z));
      }
      // add skin to lines
      printlines.addPolys(SKIN, polys, (s==0), // displace at first skin
			  maxshellspeed * 60,
			  minshelltime);
      if (s < skins-1) { // not on the last layer, this handle with all other lines
	// have to get all these separately because z changes
	printlines.makeLines(startPoint, lines);
	if (!ZliftAlways)
	  printlines.clipMovements(*clippolys, lines, clipnearest, linewidth);
	printlines.optimize(linewidth,
			    minshelltime, cornerradius, lines);
	printlines.getLines(lines, lines3, extr_per_mm);
	printlines.clear();
	lines.clear();
      }
      polys.clear();
    }
  } // last skin layer now still in lines
  lines.clear();

  // 2. Skirt
  printlines.addPolys(SKIRT, skirtPolygons, false,
		      maxshellspeed * 60,
		      minshelltime);

  // 3. Support
  if (supportInfill) {
    uint extruderbefore = settings.selectedExtruder;
    settings.SelectExtruder(supportExtruder);
    printlines.addPolys(SUPPORT, supportInfill->infillpolys, false);
    settings.SelectExtruder(extruderbefore);
  }
  // 4. all other polygons:

  //  Shells
  for(int p=shellPolygons.size()-1; p>=0; p--) { // inner to outer
    printlines.addPolys(SHELL, shellPolygons[p],
			(p==(int)(shellPolygons.size())-1),
			maxshellspeed * 60,
			minshelltime);
  }

  //  Infill
  if (normalInfill)
//.........这里部分代码省略.........

bool se2bracket_tests()
{
  bool failed = false;
  vector<Vector3d> vecs;
  Vector3d tmp;
//  tmp << 0,0,0;
//  vecs.push_back(tmp);
//  tmp << 1,0,0;
//  vecs.push_back(tmp);
//  tmp << 0,1,1;
//  vecs.push_back(tmp);
//  tmp << -1,1,0;
//  vecs.push_back(tmp);
  tmp << 20,-1,-1;
  vecs.push_back(tmp);
//  tmp << 30,5,20;
//  vecs.push_back(tmp);
  for (unsigned int i=0; i<vecs.size(); ++i)
  {
    Vector3d resDiff = vecs[i] - SE2::vee(SE2::hat(vecs[i]));
    if (resDiff.norm()>SMALL_EPS)
    {
      cerr << "Hat-vee Test" << endl;
      cerr  << "Test case: " << i <<  endl;
      cerr << resDiff.transpose() << endl;
      cerr << endl;
    }

    for (unsigned int j=0; j<vecs.size(); ++j)
    {
      Vector3d res1 = SE2::lieBracket(vecs[i],vecs[j]);
      Matrix3d hati = SE2::hat(vecs[i]);
      Matrix3d hatj = SE2::hat(vecs[j]);

      Vector3d res2 = SE2::vee(hati*hatj-hatj*hati);
      Vector3d resDiff = res1-res2;
      if (resDiff.norm()>SMALL_EPS)
      {
        cerr << "SE2 Lie Bracket Test" << endl;
        cerr  << "Test case: " << i << ", " <<j<< endl;
        cerr << vecs[i].transpose() << endl;
        cerr << vecs[j].transpose() << endl;
        cerr << res1 << endl;
        cerr << res2 << endl;
        cerr << resDiff.transpose() << endl;
        cerr << endl;
        failed = true;
      }
    }


    Vector3d omega = vecs[i];
    Matrix3d exp_x = SE2::exp(omega).matrix();
    Matrix3d expmap_hat_x = (SE2::hat(omega)).exp();
    Matrix3d DiffR = exp_x-expmap_hat_x;
    double nrm = DiffR.norm();

    if (isnan(nrm) || nrm>SMALL_EPS)
    {
      cerr << "expmap(hat(x)) - exp(x)" << endl;
      cerr  << "Test case: " << i << endl;
      cerr << exp_x <<endl;
      cerr << expmap_hat_x <<endl;
      cerr << DiffR <<endl;
      cerr << endl;
      failed = true;
    }
  }

  return failed;

}

GLThread::GLThread() {
  int numInit = (NUM_POINTS-3)/2;
  double noise_factor = 0.0;

  display_start_pos.setZero();

  vector<Vector3d> vertices;
  vector<double> angles;

  vertices.push_back(Vector3d::Zero());
  angles.push_back(0.0);
  //push back unitx so first tangent matches start_frame
  vertices.push_back(Vector3d::UnitX()*DEFAULT_REST_LENGTH);
  angles.push_back(0.0);

  Vector3d direction;
  direction(0) = 1.0;
  direction(1) = 0.0;
  direction(2) = -1.0;
  direction.normalize();
  for (int i=0; i < numInit; i++)
    {
      Vector3d noise( ((double)(rand()%10000)) / 10000.0, ((double)(rand()%10000)) / 10000.0, ((double)(rand()%10000)) / 10000.0);
      noise *= noise_factor;
      Vector3d next_Vec = vertices.back()+(direction+noise).normalized()*DEFAULT_REST_LENGTH;
      vertices.push_back(next_Vec);
      angles.push_back(0.0);

      //std::cout << positions[i] << std::endl << std::endl;
    }



  //change direction
  direction(0) = 1.0;
  direction(1) = 0.0;
  direction(2) = 1.0;
  direction.normalize();

  for (int i=0; i < numInit; i++)
    {
      Vector3d noise( ((double)(rand()%10000)) / 10000.0, ((double)(rand()%10000)) / 10000.0, ((double)(rand()%10000)) / 10000.0);
      noise *= noise_factor;
      Vector3d next_Vec = vertices.back()+(direction+noise).normalized()*DEFAULT_REST_LENGTH;
      vertices.push_back(next_Vec);
      angles.push_back(0.0);

    }

  //push back unitx so last tangent matches end_frame
  vertices.push_back(vertices.back()+Vector3d::UnitX()*DEFAULT_REST_LENGTH);
  angles.push_back(0.0);


  angles.resize(vertices.size());

  rotations[0] = Matrix3d::Identity();
  rotations[1] = Matrix3d::Identity();


  _thread = new Thread(vertices, angles, rotations[0], rotations[1]);
  //_thread->set_start_rest_length(1);
  //_thread->set_end_rest_length(1);
  //_thread->set_rest_length(DEFAULT_REST_LENGTH);

#ifdef ISOTROPIC
  to_set_bend = _thread->get_bend_coeff();
#else
  to_set_B = _thread->get_bend_matrix();
#endif
  to_set_twist = _thread->get_twist_coeff();
  to_set_grav = _thread->get_grav_coeff();


#ifdef ISOTROPIC
    _thread->set_coeffs_normalized(to_set_bend, to_set_twist, to_set_grav);
#else
    _thread->set_coeffs_normalized(to_set_B, to_set_twist, to_set_grav);
#endif

  InitContour();
  strcpy(_display_name, "");
}

// Questa funzione e' quella che in background fa tutti i conti matematici, quindi qui devi inserire 
// 1) Scrittura su file continua delle coordinate che vuoi salvare
// 2) Estrazione delle coordinate a partire dai corpi rigidi precedentemente definiti vedi ad esempio
// come e' fatto per eyeLeft e eyeRight oppure per thumb ed index
void idle()
{
	optotrak.updateMarkers();
	markers = optotrak.g