//.........这里部分代码省略.........
      Mat3>
      KernelType;

    KernelType kernel(
      xL, imageL.Width(), imageL.Height(),
      xR, imageR.Width(), imageR.Height(),
      false); // configure as point to point error model.

    Mat3 H;
    std::pair<double,double> ACRansacOut = ACRANSAC(kernel, vec_inliers, 1024, &H,
      std::numeric_limits<double>::infinity(),
      true);
    const double & thresholdH = ACRansacOut.first;
    const double & NFAH = ACRansacOut.second;

    // Check the homography support some point to be considered as valid
    if (vec_inliers.size() > KernelType::MINIMUM_SAMPLES *2.5) {

      std::cout << "\nFound a homography under the confidence threshold of: "
        << thresholdH << " pixels\n\twith: " << vec_inliers.size() << " inliers"
        << " from: " << vec_PutativeMatches.size()
         << " putatives correspondences"
        << std::endl;

      //Show homography validated point and compute residuals
      std::vector<double> vec_residuals(vec_inliers.size(), 0.0);
      svgDrawer svgStream( imageL.Width() + imageR.Width(), max(imageL.Height(), imageR.Height()));
      svgStream.drawImage(jpg_filenameL, imageL.Width(), imageL.Height());
      svgStream.drawImage(jpg_filenameR, imageR.Width(), imageR.Height(), imageL.Width());
      for ( size_t i = 0; i < vec_inliers.size(); ++i)  {
        size_t idx = vec_inliers[i];
        const SIOPointFeature & LL = featsL[vec_PutativeMatches[vec_inliers[i]]._i];
        const SIOPointFeature & RR = featsR[vec_PutativeMatches[vec_inliers[i]]._j];
        const Vec2f L = LL.coords();
        const Vec2f R = RR.coords();
        svgStream.drawLine(L.x(), L.y(), R.x()+imageL.Width(), R.y(), svgStyle().stroke("green", 2.0));
        svgStream.drawCircle(L.x(), L.y(), LL.scale(), svgStyle().stroke("yellow", 2.0));
        svgStream.drawCircle(R.x()+imageL.Width(), R.y(), RR.scale(),svgStyle().stroke("yellow", 2.0));
        // residual computation
        vec_residuals[i] = std::sqrt(KernelType::ErrorT::Error(H,
                                       LL.coords().cast<double>(),
                                       RR.coords().cast<double>()));
      }
      string out_filename = "04_ACRansacHomography.svg";
      ofstream svgFile( out_filename.c_str() );
      svgFile << svgStream.closeSvgFile().str();
      svgFile.close();

      // Display some statistics of reprojection errors
      float dMin, dMax, dMean, dMedian;
      minMaxMeanMedian<float>(vec_residuals.begin(), vec_residuals.end(),
                            dMin, dMax, dMean, dMedian);

      std::cout << std::endl
        << "Homography matrix estimation, residuals statistics:" << "\n"
        << "\t-- Residual min:\t" << dMin << std::endl
        << "\t-- Residual median:\t" << dMedian << std::endl
        << "\t-- Residual max:\t "  << dMax << std::endl
        << "\t-- Residual mean:\t " << dMean << std::endl;

      // Perform GUIDED MATCHING
      // Use the computed model with all possible point couples
      //  and keep the one that have the error under the AC-RANSAC precision
      //  value.

      Mat xL, xR;

void RenderGrid::draw ()
{
   _width = _cnvOpt.width;
   _height = _cnvOpt.height;
   _rc.fontsClear();

   bool enableRefAtoms = refAtoms.size() > 0 && _factory.isItemMolecule(objs[0]);
   if (enableRefAtoms && refAtoms.size() != objs.size())
      throw Error("Number of reference atoms should be same as the number of objects");
   bool enableTitles = titles.size() > 0;
   if (enableTitles && titles.size() != objs.size())
      throw Error("Number of titles should be same as the number of objects");

   nRows = (objs.size() + nColumns - 1) / nColumns;

   commentSize.set(0,0);
   commentOffset = 0;
   if (comment >= 0) {
      _factory.getItem(comment).init();
      _factory.getItem(comment).estimateSize();
      commentSize.copy(_factory.getItem(comment).size);
      commentOffset = _cnvOpt.commentOffset;
   }

   maxsz.set(0,0);
   Vec2f refSizeLT, refSizeRB;
   Array<float> columnExtentLeft, columnExtentRight, rowExtentTop, rowExtentBottom;
   columnExtentLeft.clear_resize(nColumns);
   columnExtentRight.clear_resize(nColumns);
   columnExtentLeft.fill(0);
   columnExtentRight.fill(0);
   rowExtentTop.clear_resize(nRows);
   rowExtentBottom.clear_resize(nRows);
   rowExtentTop.fill(0);
   rowExtentBottom.fill(0);
   for (int i = 0; i < objs.size(); ++i) {
      if (enableRefAtoms)
         _factory.getItemMolecule(objs[i]).refAtom = refAtoms[i];
      _factory.getItem(objs[i]).init();
      _factory.getItem(objs[i]).setObjScale(_getObjScale(objs[i]));
      _factory.getItem(objs[i]).estimateSize();
      if (enableRefAtoms) {
         const Vec2f& r = _factory.getItemMolecule(objs[i]).refAtomPos;
         Vec2f d;
         d.diff(_factory.getItemMolecule(objs[i]).size, r);
         refSizeLT.max(r);
         int col = i % nColumns;
         int row = i / nColumns;
         columnExtentLeft[col] = __max(columnExtentLeft[col], r.x);
         columnExtentRight[col] = __max(columnExtentRight[col], d.x);
         rowExtentTop[row] = __max(rowExtentTop[row], r.y);
         rowExtentBottom[row] = __max(rowExtentBottom[row], d.y);
         refSizeRB.max(d);
      } else {
         maxsz.max(_factory.getItem(objs[i]).size);
      }
   }
   if (enableRefAtoms)
      maxsz.sum(refSizeLT, refSizeRB);

   maxTitleSize.set(0,0);
   titleOffset = 0;
   if (enableTitles) {
      titleOffset = _cnvOpt.titleOffset;
      for (int i = 0; i < titles.size(); ++i) {
         _factory.getItem(titles[i]).init();
         _factory.getItem(titles[i]).estimateSize();
         maxTitleSize.max(_factory.getItem(titles[i]).size);
      }
   }

   outerMargin.x = (float)(minMarg + _cnvOpt.marginX);
   outerMargin.y = (float)(minMarg + _cnvOpt.marginY);

   _width = __min(_width, _getMaxWidth());
   _height = __min(_height, _getMaxHeight());
   scale = _getScale(_width, _height);
   if (_width < 1)
      _width = _getDefaultWidth(scale);
   if (_height < 1)
      _height = _getDefaultHeight(scale);

   _rc.initContext(_width, _height);
   cellsz.set(__max(maxsz.x * scale, maxTitleSize.x),
      maxsz.y * scale + maxTitleSize.y + titleOffset);
   clientArea.set(cellsz.x * nColumns + _cnvOpt.gridMarginX * (nColumns - 1),
      cellsz.y * nRows + _cnvOpt.gridMarginY * (nRows - 1));
   _rc.init();
   if (_cnvOpt.xOffset > 0 || _cnvOpt.yOffset > 0)
      _rc.translate((float)_cnvOpt.xOffset, (float)_cnvOpt.yOffset);
   _rc.translate(outerMargin.x, outerMargin.y);
   if (_cnvOpt.commentPos == COMMENT_POS_TOP) {
      _drawComment();
      _rc.translate(0, (float)commentOffset);
   }
   _rc.storeTransform();
   {
      _rc.translate((_width - clientArea.x) / 2 - outerMargin.x, (_height - commentSize.y - commentOffset - clientArea.y) / 2 - outerMargin.y);
      for (int i = 0; i < objs.size(); ++i) {
         _rc.storeTransform();
//.........这里部分代码省略.........

// React to size changes
void reshape(Vec2f Size, SimpleSceneManager *mgr)
{
    mgr->resize(Size.x(), Size.y());
}

OGLText::OGLText(const Vec2f& position, const Color& color, const std::string& text, float size) :_position(position.X(), position.Y(), -1.f), m_text(text), _fontLoc("sansserif.glf"), _color(color)
{
    m_font = new GLFont();
    m_font->Create(_fontLoc);
    //Approximation of size based on 12px 24px 22px
    _size = size * (1 - (1 / m_font->m_height));
    _width = m_font->m_width * text.size();
    _rot = 0.f;
}

void SerialTestApp::mouseDrag( MouseEvent event )
{
    mousePos = event.getPos();
    Vec2f offset = mousePos - getWindowCenter();

    float speed = offset.length();
    offset.normalize();
    float amtLeftWheel = 0;
    float amtRightWheel = 0;
    
    // Turning scheme:
    // 0-90°
    // 0 == other wheel moves forward @ speed
    // 90 == other wheel moves backwards @ speed
    
    // Never account for moving backwards.
    // Hard left or right is all we can do.
    float yRange = (MAX((offset.y*-1), 0.0)*2.0f) - 1.0f; // -1..1
    //yRange*=-1;
    
    // Always having one wheel moving forward ensures we're
    // driving forward. We can't drive backwards.
    if(offset.x < 0){
        amtRightWheel = 1;
        amtLeftWheel = yRange;// -1..1 //offset.y*-1;
    }else{
        amtLeftWheel = 1;
        amtRightWheel = yRange;// -1..1 //offset.y*-1;
    }
    
    // Making the lw / rw amount a function of the speed
    const static int MAX_SPEED = 200;
    float speedScalar = MIN((speed/(float)MAX_SPEED), 1.0);
    amtLeftWheel *= speedScalar;
    amtRightWheel *= speedScalar;
    
    int lw = 255+(amtLeftWheel*255); // 0..255..500
    int rw = 255+(amtRightWheel*255); // 0..255..500
    string directions = "" + boost::lexical_cast<string>((int)lw) + "," +
                             boost::lexical_cast<string>((int)rw) + ",\n";

    console() << directions << "\n";
    serial.writeString(directions);
    
    // SIM Arduino code
    long val = (lw*(long)1000)+rw;
    int rVal = val % 1000;
    int lVal = (val - rVal) * 0.001;
    
    int lDirection = lVal >= 255 ? 1 : -1;
    int rDirection = rVal >= 255 ? 1 : -1;
    int lAbsVal = abs(lVal-255);
    int rAbsVal = abs(rVal-255);

    console() << "lw : " << lw << " rw: " << rw;
    console() << " lAbsVal : " << lAbsVal << " rAbsVal: " << rAbsVal << "\n";
    
    
    _ardLDir = lDirection;
    _ardRDir = rDirection;
    _ardLVal = lAbsVal;
    _ardRVal = rAbsVal;

}

void Draw::mouseReleaseEvent( QMouseEvent *e ) {
  //  printf("mouse release event %d\n", e->button());
  // draw
  _mousePressed = 0;
  if (_tool == D_DRAW && _currDPath) {  // draw
    //_pressure = 2.*_sens; 
    if (_currDPath->getNumElements() > 1) {
      _parent->setCursor(Qt::WaitCursor);
      Vec2f loc = unproject(e->x(), e->y(), _h);
      _currDPath->addVertex(loc.x(), loc.y(), mapPressure());
      _dc->addPath(_currDPath);

      //for (int i=0; i<2; i++)
      //_currDPath->fair();
      _currDPath->resample(NULL);
      _currDPath->redoStroke();

      bool res=false;
      if (!_freeDraw)
	res = _currDPath->correspondRoto2(_is->getRotoCurves(_frame), _w, _h);
      _currDPath->calculateStrokeDisplayList();
      if (res) {
	_corrShow = 1;
	propagatePathEverywhere(_currDPath);
      }
      else _currDPath = NULL;
      emit restoreCursor();
    }
    else {
      delete _currDPath;
      _currDPath = NULL;
    }
  }

  /*
  else if (_tool == D_PAINT && _currDPath) { // paint
    Bboxf2D bbox = _currDPath->calcBbox();
    DrawPatch *dch = new DrawPatch((int)floor(bbox.lower.x()),(int)floor(bbox.lower.y()),
				   (int)ceil(bbox.width()), (int)ceil(bbox.height()),
				   _currColor);
    dch->takeStroke(_currDPath->giveStroke());
    _currDPath->forgetStroke();
    _dc->addPatch(dch);
    //printf("patch corner: %f %f, window: %f %f\n",dch->x(), dch->y(),
    //   bbox.width(), bbox.height());
    delete _currDPath;
    _currDPath = NULL;

    if (_propPatch && _propMode==1)
      keyframeInterpolatePatch(dch);
  }
  */

  else if (_tool == D_SELECT) { // select
    if (_selected && _dragDirty) {
      regeneratePath(_selected);
      _selected->fixLoc();
      _dragDirty = false;
    }
  }
  else if (_tool == D_OVERDRAW && _currDPath) {// overdraw 
    if (_currDPath->getNumElements() > 1) {
      _currDPath->resample(NULL);
      spliceDrawCurve();
    }
    else {
      delete _currDPath; _currDPath=NULL;
      _spliceFlag=-1; 
    }
  }

  else if (_tool==D_DROPPER) {
    _captureColor = true;
    _captureColorLoc.Set(e->x(), _h-e->y());
    _parent->updateGL();
    _captureColor = false;
  }

  else if (_tool==D_NUDGE) { // nudge
    _spliceIndex=-1;
  }

  //_minimalRenderMode = 0;
  _parent->updateGL();
}

void SpringCam::dragCam( const Vec2f &posOffset, float distFromCenter )
{
	mEyeNode.mAcc += Vec3f( posOffset.xy(), distFromCenter );
}

//----------------------------------------------------------------------
// the next edge normal is computed as a pi/2 clockwise rotation of
// the direction from this point to the next one. The edge normals
// do thus point to the left of the path.
// Author: afischle
//----------------------------------------------------------------------
static Vec2f computeEdgeNormal(const Vec2f &a, const Vec2f &b, bool cw)
{
    Vec2f d = b - a;
    d.normalize();
    return cw == true ? Vec2f(d.y(), -d.x()) : Vec2f(-d.y(), d.x());
}

void StageSelect::drawScrollbar() {
	Vec2f pos = Vec2f(-15, -50);
	Vec2f size = Vec2f(870, 760);
	drawCenterTexture(pos,size, scrollbar_tex_);
	drawCenterTexture(pos+Vec2f(-22,0), Vec2f(size.x()*0.2,size.y()*0.6), scroll_tex_);
}

float Boid::dist(Vec2f v1,Vec2f v2)
{
    return v1.distance(v2);
}

	void update(float dt, vector<Planet*>& planets, paramStruct& params)
	{
		if(charging && charge < MAXCHARGE) charge += dt * 1.5f;
		
		pars = params;
		
		if(jumpInert > .0f)
			jumpInert-=dt;
		if(jumpInert < .0f)
			jumpInert = .0f;
		
		if(turn != params.turn)
		{
			charging = false;
			charge = .0f;
		}
		
		turn = params.turn;
		
		pos += vel * dt;
		
		if(pos.x > getWindowWidth())
		{
			pos.x = pos.x-getWindowWidth();
		}
		
		if(pos.x < 0)
		{
			pos.x = getWindowWidth()+pos.x;
		}
		
		if(pos.y > getWindowHeight())
		{
			pos.y = pos.y-getWindowHeight();
		}
		
		if(pos.y < 0)
		{
			pos.y = getWindowHeight()+pos.y;
		}
		
		Planet* closest = planets[0];
		float dst = 999999999.0f;
		vector<Planet*>::iterator it;
		for(it = planets.begin(); it < planets.end(); it++)
		{
			Vec2f moonToPlanet = ((*it)->pos - pos);
			
			if(moonToPlanet.length() < dst)
			{
				closest = (*it);
				dst = moonToPlanet.length();
			}
				
			
		//	vel += moonToPlanet.normalized() * 1.0f * (*it)->radius * (*it)->radius * 3.14 / (math<float>::max(moonToPlanet.length() * moonToPlanet.length(), 80.0f));

		//	vel += moonToPlanet.normalized() * params.distFactor * moonToPlanet.length();//20000.0f/(math<float>::max(moonToPlanet.length(), 100.0f));
			
		//	vel += (-params.repulse)* moonToPlanet.normalized() / moonToPlanet.length();
		}
		
		nearest = closest;
		
		Vec2f moonToPlanet = closest->pos - pos;
		
		if(!closestPlanet && moonToPlanet.length() > closest->radius * 2.5f)
		{
			vel += moonToPlanet.normalized() * 36.5f * pars.speed *
					closest->radius * closest->radius * closest->radius * (2.0f/3.0f) * M_PI 
					/ (math<float>::max(moonToPlanet.length() * moonToPlanet.length(), 80.0f));
		}
		if(!jumpInert && !closestPlanet && moonToPlanet.length() < closest->radius * 1.7f && losing.size() == 0)
		{
			closestPlanet = closest;
			
		}
		if(closestPlanet && !jumpInert && losing.size() == 0)
		{
			Vec2f m2cp = (closestPlanet->pos - pos);
			
			Vec2f r = pos - closestPlanet->pos;
			r.normalize();
			r.rotate(M_PI/1.97f);
		  
			vel = r * 300.0f * pars.speed;
			
			if(m2cp.length() < closestPlanet->radius)
			{
				pos += -m2cp.normalized() * (5.0f + closestPlanet->radius - m2cp.length());
			}
		}
		
		//vel /= planets.size();
		
		//
//		Vec2f newpos = (pos + vel * dt);
//		for(it = planets.begin(); it < planets.end(); it++)
//		{
//			if(newpos.distance((*it)->pos) < (*it)->radius * 1.5f)
//.........这里部分代码省略.........

Vec2f MoleculeLayoutSmoothingSegment::_getPosition(Vec2f p) {
   Vec2f point;
   point.copy(p);
   point.rotate(_finish - _start);
   return point + _start;
}

MoleculeLayoutSmoothingSegment::MoleculeLayoutSmoothingSegment(MoleculeLayoutGraphSmart& mol, Vec2f& start, Vec2f& finish) :
_graph(mol),
_start(start),
_finish(finish)
{
   _center.zero();
   Vec2f diameter = (_finish - _start);
   _length = diameter.length();
   Vec2f rotate_vector = diameter / diameter.lengthSqr();
   rotate_vector.y *= -1;
   _pos.clear_resize(_graph.vertexEnd());

   bool is_line = false;
   for (int v : _graph.vertices()) if (_graph.getVertex(v).degree() == 1) is_line = true;

   if (!is_line) {
      for (int v : _graph.vertices()) {
         _pos[v].copy(_graph.getPos(v));

         _pos[v] -= _start;
         _pos[v].rotate(rotate_vector);
      }
   }
   else {
	   // this is straight line
	   QS_DEF(Array<int>, vert);
	   vert.clear(); // list or vertices in order of connection
	   for (int v : _graph.vertices()) if (_graph.getVertex(v).degree() == 1) {
		   vert.push(v);
		   break;
	   }
	   vert.push(_graph.getVertex(vert[0]).neiVertex(_graph.getVertex(vert[0]).neiBegin()));
	   while (vert.size() < _graph.vertexCount()) {
		   for (int n = _graph.getVertex(vert.top()).neiBegin(); n != _graph.getVertex(vert.top()).neiEnd(); n = _graph.getVertex(vert.top()).neiNext(n))
			   if (_graph.getVertex(vert.top()).neiVertex(n) != vert[vert.size() - 2]) {
				   vert.push(_graph.getVertex(vert.top()).neiVertex(n));
			   }
	   }
	   for (int i = 0; i < vert.size(); i++) _pos[vert[i]].set(i * 1. / (vert.size() - 1), 0);
   }

   // double ternary search of center of component
   float MLx = 0, Ly = 0, MRx = 0, Ry = 0, Lx, Rx;
   for (int v : _graph.vertices()) {
	   MLx = __min(MLx, _pos[v].x);
	   MRx = __max(MRx, _pos[v].x);
	   Ly = __min(Ly, _pos[v].y);
	   Ry = __max(Ry, _pos[v].y);
   }
   while (Ry - Ly > EPSILON) {
	   float dy = (Ry - Ly) / 3;
	   float ry[2];
	   float My = Ly + dy;
	   for (int i = 0; i < 2; i++) {
		   Lx = MLx, Rx = MRx;
		   float rx[2];
		   while (Rx - Lx > EPSILON) {
			   float dx = (Rx - Lx) / 3;
			   float Mx = Lx + dx;
			   for (int j = 0; j < 2; j++) {
				   rx[j] = calc_radius(Vec2f(Mx, My));
				   Mx += dx;
			   }
			   if (rx[0] > rx[1]) Lx += dx; else Rx -= dx;
		   }
		   ry[i] = calc_radius(Vec2f(Rx, My));
		   My += dy;
	   }
	   if (ry[0] > ry[1]) Ly += dy; else Ry -= dy;
   }
   _center = Vec2f(Rx, Ry);
   _radius = calc_radius(_center);
   /*
   _radius = 0;
   Vec2f center(0.5, 0);
   for (int v : _graph.vertices()) {
      float dist = (center - _pos[v]).length();
      if (dist > _radius) _radius = dist;
   }
   _center = center;*/
   _square = 0;
}

void BorderLayout::updateLayout(const MFUnrecChildComponentPtr* Components, const Component* ParentComponent) const
{
	Pnt2f borderTopLeft, borderBottomRight;
	dynamic_cast<const ComponentContainer*>(ParentComponent)->getInsideInsetsBounds(borderTopLeft, borderBottomRight);
	Vec2f borderSize(borderBottomRight-borderTopLeft);

	Real32 NorthHeight(0);
	Real32 SouthHeight(0);
	Real32 WestWidth(0);
	Real32 EastWidth(0);
	Vec2f size;
	Vec2f offset;

	// the first pass through gets some crucial dimensions to determine
	// the sizes of the buttons
	for(UInt32 i = 0 ; i<Components->size(); ++i)
    {
		if((*Components)[i]->getConstraints() != NULL)
		{
			switch (dynamic_cast<BorderLayoutConstraints*>((*Components)[i]->getConstraints())->getRegion()) {
				// don't need to do anything with the center quite yet
				case BorderLayoutConstraints::BORDER_NORTH:
					NorthHeight = (*Components)[i]->getPreferredSize().y();
					break;
				case BorderLayoutConstraints::BORDER_EAST:
					EastWidth = (*Components)[i]->getPreferredSize().x();
					break;
				case BorderLayoutConstraints::BORDER_SOUTH:
					SouthHeight = (*Components)[i]->getPreferredSize().y();
					break;
				case BorderLayoutConstraints::BORDER_WEST:
					WestWidth = (*Components)[i]->getPreferredSize().x();
					break;
				default:
					break; 
			}
		}
	}
    Pnt2f Pos;
	// this second pass sets its size and draws them
	for(UInt32 i = 0 ; i<Components->size(); ++i)
    {
		// Find its region and draw it accordingly
		if((*Components)[i]->getConstraints() != NULL)
		{
			switch (dynamic_cast<BorderLayoutConstraints*>((*Components)[i]->getConstraints())->getRegion()) {
				case BorderLayoutConstraints::BORDER_CENTER: 
					// set up the size of the button and its extra displacement
					if ((*Components)[i]->getMaxSize().x() < borderSize.x()-(WestWidth+EastWidth))
					{
						size[0] = (*Components)[i]->getMaxSize().x();
						offset[0] = (borderSize.x()-(WestWidth+EastWidth)-size[0])/2 + WestWidth;
					}
					else
					{
						size[0] = borderSize.x()-(WestWidth+EastWidth);
						offset[0] = WestWidth;
					}
					if ((*Components)[i]->getMaxSize().y() < borderSize.y()-(NorthHeight+SouthHeight))
					{
						size[1] = (*Components)[i]->getMaxSize().y();
						offset[1] = (borderSize.y()-(NorthHeight+SouthHeight)-size[1])/2 + NorthHeight;
					}
					else
					{
						size[1] = borderSize.y()-(NorthHeight+SouthHeight);
						offset[1] = NorthHeight;
					}
					break;
				case BorderLayoutConstraints::BORDER_NORTH:
					// set up the size of the button and its extra displacement
					size[1] = (*Components)[i]->getPreferredSize().y();
					offset[1] = 0;
					if ((*Components)[i]->getMaxSize().x() < borderSize.x())
					{
						size[0] = (*Components)[i]->getMaxSize().x();
						offset[0] = (borderSize.x()-size[0])/2;
					}
					else
					{
						size[0] = borderSize.x();
						offset[0] = 0;
					}
					break;
				case BorderLayoutConstraints::BORDER_EAST:
					// set up the size of the button and its extra displacement
					size[0] = (*Components)[i]->getPreferredSize().x();
					offset[0] = borderSize.x()-size.x();
					if ((*Components)[i]->getMaxSize().y() < borderSize.y()-(NorthHeight+SouthHeight))
					{
						size[1] = (*Components)[i]->getMaxSize().y();
						offset[1] = (borderSize.y()-size[1]-(NorthHeight+SouthHeight))/2+NorthHeight;
					}
					else
					{
						size[1] = borderSize.y()-(NorthHeight+SouthHeight);
						offset[1] = NorthHeight;
					}
					break;
				case BorderLayoutConstraints::BORDER_SOUTH:
//.........这里部分代码省略.........

//.........这里部分代码省略.........
  // Essential geometry filtering of putative matches
  {
    Mat3 K;
    //read K from file
    if (!readIntrinsic(stlplus::create_filespec(sInputDir,"K","txt"), K))
    {
      std::cerr << "Cannot read intrinsic parameters." << std::endl;
      return EXIT_FAILURE;
    }

    //A. prepare the corresponding putatives points
    Mat xL(2, vec_PutativeMatches.size());
    Mat xR(2, vec_PutativeMatches.size());
    for (size_t k = 0; k < vec_PutativeMatches.size(); ++k)  {
      const SIOPointFeature & imaL = featsL[vec_PutativeMatches[k]._i];
      const SIOPointFeature & imaR = featsR[vec_PutativeMatches[k]._j];
      xL.col(k) = imaL.coords().cast<double>();
      xR.col(k) = imaR.coords().cast<double>();
    }

    //B. robust estimation of the essential matrix
    std::vector<size_t> vec_inliers;
    Mat3 E;
    std::pair<size_t, size_t> size_imaL(imageL.Width(), imageL.Height());
    std::pair<size_t, size_t> size_imaR(imageR.Width(), imageR.Height());
    double thresholdE = 0.0, NFA = 0.0;
    if (robustEssential(
      K, K,         // intrinsics
      xL, xR,       // corresponding points
      &E,           // essential matrix
      &vec_inliers, // inliers
      size_imaL,    // Left image size
      size_imaR,    // Right image size
      &thresholdE,  // Found AContrario Theshold
      &NFA,         // Found AContrario NFA
      std::numeric_limits<double>::infinity()))
    {
      std::cout << "\nFound an Essential matrix under the confidence threshold of: "
        << thresholdE << " pixels\n\twith: " << vec_inliers.size() << " inliers"
        << " from: " << vec_PutativeMatches.size()
        << " putatives correspondences"
        << std::endl;

      // Show Essential validated point
      svgDrawer svgStream( imageL.Width() + imageR.Width(), max(imageL.Height(), imageR.Height()));
      svgStream.drawImage(jpg_filenameL, imageL.Width(), imageL.Height());
      svgStream.drawImage(jpg_filenameR, imageR.Width(), imageR.Height(), imageL.Width());
      for ( size_t i = 0; i < vec_inliers.size(); ++i)  {
        size_t idx = vec_inliers[i];
        const SIOPointFeature & LL = featsL[vec_PutativeMatches[vec_inliers[i]]._i];
        const SIOPointFeature & RR = featsR[vec_PutativeMatches[vec_inliers[i]]._j];
        const Vec2f L = LL.coords();
        const Vec2f R = RR.coords();
        svgStream.drawLine(L.x(), L.y(), R.x()+imageL.Width(), R.y(), svgStyle().stroke("green", 2.0));
        svgStream.drawCircle(L.x(), L.y(), LL.scale(), svgStyle().stroke("yellow", 2.0));
        svgStream.drawCircle(R.x()+imageL.Width(), R.y(), RR.scale(),svgStyle().stroke("yellow", 2.0));
      }
      string out_filename = "04_ACRansacEssential.svg";
      ofstream svgFile( out_filename.c_str() );
      svgFile << svgStream.closeSvgFile().str();
      svgFile.close();

      //C. Extract the rotation and translation of the camera from the essential matrix
      Mat3 R;
      Vec3 t;
      if (!estimate_Rt_fromE(K, K, xL, xR, E, vec_inliers,
        &R, &t))
      {
        std::cerr << " /!\\ Failed to compute initial R|t for the initial pair" << std::endl;
        return false;
      }
      std::cout << std::endl
        << "-- Rotation|Translation matrices: --" << std::endl
        << R << std::endl << std::endl << t << std::endl;

      // Build Left and Right Camera
      PinholeCamera camL(K, Mat3::Identity(), Vec3::Zero());
      PinholeCamera camR(K, R, t);

      //C. Triangulate and export inliers as a PLY scene
      std::vector<Vec3> vec_3DPoints;
      triangulateAndSaveResult(
        camL, camR,
        vec_inliers,
        xL, xR, vec_3DPoints);

      // Export as PLY (camera pos + 3Dpoints)
      std::vector<Vec3> vec_camPos;
      vec_camPos.push_back( camL._C );
      vec_camPos.push_back( camR._C );
      exportToPly(vec_3DPoints, vec_camPos, "EssentialGeometry.ply");

    }
    else  {
      std::cout << "ACRANSAC was unable to estimate a rigid essential matrix"
        << std::endl;
    }
  }
  return EXIT_SUCCESS;
}

	void World::simulate(float dt)
	{
		mAllocator.clearFrame();

		int vIterNum = 50;
		int pIterNum = 10;

		mColManager.preocss( dt );

		for( RigidBodyList::iterator iter = mRigidBodies.begin() ,itEnd = mRigidBodies.end();
			iter != itEnd ; ++iter )
		{
			RigidBody* body = *iter;
			body->saveState();
			if ( body->getMotionType() != BodyMotion::eStatic )
			{
				if ( body->getMotionType() == BodyMotion::eDynamic )
					body->addLinearImpulse( body->mMass * mGrivaty * dt );
				body->applyImpulse();

				body->mLinearVel *= 1.0 / ( 1 + body->mLinearDamping );
			}
		}

		ContactManifold** sortedContact = new ( mAllocator ) ContactManifold* [ mColManager.mMainifolds.size() ];
		int numMainfold = mColManager.mMainifolds.size(); 

		int idxStatic = mColManager.mMainifolds.size() - 1;
		int idxNormal = 0;

		for( int i = 0 ; i < mColManager.mMainifolds.size() ; ++i )
		{
			ContactManifold& cm = *mColManager.mMainifolds[i];

			RigidBody* bodyA = static_cast< RigidBody* >( cm.mContect.object[0] );
			RigidBody* bodyB = static_cast< RigidBody* >( cm.mContect.object[1] );

			if ( bodyA->getMotionType() == BodyMotion::eStatic || 
				 bodyB->getMotionType() == BodyMotion::eStatic )
			{
				sortedContact[idxStatic--] = &cm;
			}
			else
			{
				sortedContact[idxNormal++] = &cm;
			}

		}

		for( int i = 0 ; i < numMainfold ; ++i )
		{
			ContactManifold& cm = *sortedContact[i];
			Contact& c = cm.mContect;

			Vec2f cp = 0.5 * ( c.pos[0] + c.pos[1] );

			RigidBody* bodyA = static_cast< RigidBody* >( c.object[0] );
			RigidBody* bodyB = static_cast< RigidBody* >( c.object[1] );

			Vec2f vA = bodyA->getVelFromWorldPos( cp );
			Vec2f vB = bodyB->getVelFromWorldPos( cp );
			float vrel = c.normal.dot( vB - vA );
			float relectParam = 1.0f;
			cm.velParam = 0;
			if ( vrel < -1 )
			{
				cm.velParam = -relectParam * vrel;
			}
			

			//cm.impulse = 0;

			////warm start
			Vec2f rA = cp - bodyA->mPosCenter;
			Vec2f rB = cp - bodyB->mPosCenter;
			Vec2f dp = cm.impulse * c.normal;

			bodyA->mLinearVel -= dp * bodyA->mInvMass;
			//bodyA->mAngularVel -= rA.cross( dp ) * bodyA->mInvI;
			bodyB->mLinearVel += dp * bodyB->mInvMass;
			//bodyB->mAngularVel += rB.cross( dp ) * bodyB->mInvI;
		}

		if ( numMainfold != 0 )
			jumpDebug();
		//std::sort( sortedContact.begin() , sortedContact.end() , DepthSort() );

		for( int nIter = 0 ; nIter < vIterNum ; ++nIter )
		{
			for( int i = 0 ; i < numMainfold ; ++i )
			{
				ContactManifold& cm = *sortedContact[i];
				Contact& c = cm.mContect;

				RigidBody* bodyA = static_cast< RigidBody* >( c.object[0] );
				RigidBody* bodyB = static_cast< RigidBody* >( c.object[1] );

				Vec2f cp = 0.5 * ( c.pos[0] + c.pos[1] );

				Vec2f vA = bodyA->getVelFromWorldPos( cp );
//.........这里部分代码省略.........

void StageSelect::drawCenterBox(Vec2f pos, Vec2f size, Color color) {
	drawFillBox(pos.x(), pos.y(), size.x(), size.y(), color, 0, Vec2f(1, 1), size / 2);
}

void MapScreen::scroll(Vec2f new_cam_pos, Camera& cam)
{
	new_cam_pos = new_cam_pos + Vec2f(-16 - 8, -16 - 8);
	cam.position(Vec2f(round(new_cam_pos.x()), round(new_cam_pos.y())));
}

void StageSelect::drawCenterTexture(Vec2f pos, Vec2f size, Texture image) {
	drawTextureBox(pos.x(), pos.y(), size.x(), size.y(), 0, 0, image.width(), image.height(), image, Color::white,
		0, Vec2f(1, 1), size / 2);
}

Vec2f GravityCalculation(Vec2f move)
{
  return Vec2f(move.x(), move.y() - GR);
}