void TextEditComponent::render(const Eigen::Affine3f& parentTrans)
{
	Eigen::Affine3f trans = getTransform() * parentTrans;
	renderChildren(trans);

	// text + cursor rendering
	// offset into our "text area" (padding)
	trans.translation() += Eigen::Vector3f(getTextAreaPos().x(), getTextAreaPos().y(), 0);

	Eigen::Vector2i clipPos((int)trans.translation().x(), (int)trans.translation().y());
	Eigen::Vector3f dimScaled = trans * Eigen::Vector3f(getTextAreaSize().x(), getTextAreaSize().y(), 0); // use "text area" size for clipping
	Eigen::Vector2i clipDim((int)dimScaled.x() - trans.translation().x(), (int)dimScaled.y() - trans.translation().y());
	Renderer::pushClipRect(clipPos, clipDim);

	trans.translate(Eigen::Vector3f(-mScrollOffset.x(), -mScrollOffset.y(), 0));
	trans = roundMatrix(trans);

	Renderer::setMatrix(trans);

	if(mTextCache)
	{
		mFont->renderTextCache(mTextCache.get());
	}

	// pop the clip early to allow the cursor to be drawn outside of the "text area"
	Renderer::popClipRect();

	// draw cursor
	if(mEditing)
	{
		Eigen::Vector2f cursorPos;
		if(isMultiline())
		{
			cursorPos = mFont->getWrappedTextCursorOffset(mText, getTextAreaSize().x(), mCursor);
		}else{
			cursorPos = mFont->sizeText(mText.substr(0, mCursor));
			cursorPos[1] = 0;
		}

		float cursorHeight = mFont->getHeight() * 0.8f;
		Renderer::drawRect(cursorPos.x(), cursorPos.y() + (mFont->getHeight() - cursorHeight) / 2, 2.0f, cursorHeight, 0x000000FF);
	}
}

void VideoComponent::applyTheme(const std::shared_ptr<ThemeData>& theme, const std::string& view, const std::string& element, unsigned int properties)
{
	using namespace ThemeFlags;

	const ThemeData::ThemeElement* elem = theme->getElement(view, element, "video");
	if(!elem)
	{
		return;
	}

	Eigen::Vector2f scale = getParent() ? getParent()->getSize() : Eigen::Vector2f((float)Renderer::getScreenWidth(), (float)Renderer::getScreenHeight());

	if ((properties & POSITION) && elem->has("pos"))
	{
		Eigen::Vector2f denormalized = elem->get<Eigen::Vector2f>("pos").cwiseProduct(scale);
		setPosition(Eigen::Vector3f(denormalized.x(), denormalized.y(), 0));
		mStaticImage.setPosition(Eigen::Vector3f(denormalized.x(), denormalized.y(), 0));
	}

	if(properties & ThemeFlags::SIZE)
	{
		if(elem->has("size"))
			setResize(elem->get<Eigen::Vector2f>("size").cwiseProduct(scale));
		else if(elem->has("maxSize"))
			setMaxSize(elem->get<Eigen::Vector2f>("maxSize").cwiseProduct(scale));
	}

	// position + size also implies origin
	if (((properties & ORIGIN) || ((properties & POSITION) && (properties & ThemeFlags::SIZE))) && elem->has("origin"))
		setOrigin(elem->get<Eigen::Vector2f>("origin"));

	if(elem->has("default"))
		mConfig.defaultVideoPath = elem->get<std::string>("default");

	if((properties & ThemeFlags::DELAY) && elem->has("delay"))
		mConfig.startDelay = (unsigned)(elem->get<float>("delay") * 1000.0f);

	if (elem->has("showSnapshotNoVideo"))
		mConfig.showSnapshotNoVideo = elem->get<bool>("showSnapshotNoVideo");

	if (elem->has("showSnapshotDelay"))
		mConfig.showSnapshotDelay = elem->get<bool>("showSnapshotDelay");
}

//this could probably be optimized
//draws text and ensures it's never longer than xLen
void Font::drawWrappedText(std::string text, const Eigen::Vector2f& offset, float xLen, unsigned int color)
{
	float y = offset.y();

	std::string line, word, temp;
	Eigen::Vector2f textSize;
	size_t space, newline;

	while(text.length() > 0 || !line.empty()) //while there's text or we still have text to render
	{
		space = text.find(' ', 0);
		if(space == std::string::npos)
			space = text.length() - 1;


		word = text.substr(0, space + 1);

		//check if the next word contains a newline
		newline = word.find('\n', 0);
		if(newline != std::string::npos)
		{
			word = word.substr(0, newline);
			text.erase(0, newline + 1);
		}else{
			text.erase(0, space + 1);
		}

		temp = line + word;

		textSize = sizeText(temp);

		//if we're on the last word and it'll fit on the line, just add it to the line
		if((textSize.x() <= xLen && text.length() == 0) || newline != std::string::npos)
		{
			line = temp;
			word = "";
		}


		//if the next line will be too long or we're on the last of the text, render it
		if(textSize.x() > xLen || text.length() == 0 || newline != std::string::npos)
		{
			//render line now
			if(textSize.x() > 0) //make sure it's not blank
				drawText(line, Eigen::Vector2f(offset.x(), y), color);

			//increment y by height and some extra padding for the next line
			y += textSize.y() + 4;

			//move the word we skipped to the next line
			line = word;
		}else{
			//there's still space, continue building the line
			line = temp;
		}

	}
}

void TextComponent::onTextChanged()
{
	calculateExtent();

	if(!mFont || mText.empty())
	{
		mTextCache.reset();
		return;
	}

	std::string text = mUppercase ? strToUpper(mText) : mText;

	std::shared_ptr<Font> f = mFont;
	const bool isMultiline = (mSize.y() == 0 || mSize.y() > f->getHeight()*1.2f);

	bool addAbbrev = false;
	if(!isMultiline)
	{
		size_t newline = text.find('\n');
		text = text.substr(0, newline); // single line of text - stop at the first newline since it'll mess everything up
		addAbbrev = newline != std::string::npos;
	}

	Eigen::Vector2f size = f->sizeText(text);
	if(!isMultiline && mSize.x() && text.size() && (size.x() > mSize.x() || addAbbrev))
	{
		// abbreviate text
		const std::string abbrev = "...";
		Eigen::Vector2f abbrevSize = f->sizeText(abbrev);

		while(text.size() && size.x() + abbrevSize.x() > mSize.x())
		{
			size_t newSize = Font::getPrevCursor(text, text.size());
			text.erase(newSize, text.size() - newSize);
			size = f->sizeText(text);
		}

		text.append(abbrev);

		mTextCache = std::shared_ptr<TextCache>(f->buildTextCache(text, Eigen::Vector2f(0, 0), (mColor >> 8 << 8) | mOpacity, mSize.x(), mAlignment, mLineSpacing));
	}else{

/** Compute optimal translation scale.
 * @param std::vector<Eigen::Vector4f> vector with 3D points 
 * @param double median distance of all points
 * @param camera pitch angle
 * @param camera height
 * @return double optimal scale */
double MonoOdometer5::getTranslationScale(std::vector<Eigen::Vector4f> points3D, double median, double pitch, double height)
{
    double sigma = median/50.0;
    double weight = 1.0/(2.0*sigma*sigma);

	// ds stores the height of all points from the ground (?)
    std::vector<Eigen::Vector2f> pointsPlane;
    std::vector<double> ds;
    Eigen::Vector2f inclination;
    inclination(0) = cos(-pitch);
    inclination(1) = sin(-pitch);
    for(int i=0; i<points3D.size(); i++)
    {
        double d = inclination.transpose() * points3D[i].segment<2>(1);
        ds.push_back(d);
    }
    
    int bestIndex = 0;
    double maxSum = 0.0;
    
    for(int i=0; i<points3D.size(); i++)
    {
        if(ds[i] > median / param_odometerMotionThreshold_)
        {
            double sum = 0.0;
            for(int j=0; j<points3D.size(); j++)
            {
                double dist = ds[j] - ds[i];
                sum += exp(-dist*dist*weight);
            }
            if(sum > maxSum)
            {
                maxSum = sum;
                bestIndex = i;
            }
        }        
    }
    
    double scale = height / ds[bestIndex];
    return scale;
}

/** Get edge closest to a specified point.
 * The point must be within an imaginery line segment parallel to
 * the edge, that is a line perpendicular to the edge must go
 * through the point and a point on the edge line segment.
 * @param pos_x X coordinate in global (map) frame of point
 * @param pos_y X coordinate in global (map) frame of point
 * @return edge closest to the given point, or invalid edge if
 * such an edge does not exist.
 */
NavGraphEdge
NavGraph::closest_edge(float pos_x, float pos_y) const
{
  float min_dist = std::numeric_limits<float>::max();

  NavGraphEdge rv;

  Eigen::Vector2f point(pos_x, pos_y);
  for (const NavGraphEdge &edge : edges_) {
    const Eigen::Vector2f origin(edge.from_node().x(), edge.from_node().y());
    const Eigen::Vector2f target(edge.to_node().x(), edge.to_node().y());
    const Eigen::Vector2f direction(target - origin);
    const Eigen::Vector2f direction_norm = direction.normalized();
    const Eigen::Vector2f diff = point - origin;
    const float t = direction.dot(diff) / direction.squaredNorm();

    if (t >= 0.0 && t <= 1.0) {
      // projection of the point onto the edge is within the line segment
      float distance = (diff - direction_norm.dot(diff) * direction_norm).norm();
      if (distance < min_dist) {
	min_dist = distance;
	rv = edge;
      }
    }
  }

  return rv;
}

/** Check if the edge intersects with another line segment.
 * @param x1 X coordinate of first point of line segment to test
 * @param y1 Y coordinate of first point of line segment to test
 * @param x2 X coordinate of first point of line segment to test
 * @param y2 Y coordinate of first point of line segment to test
 * @param ip upon returning true contains intersection point,
 * not modified is return value is false
 * @return true if the edge intersects with the line segment, false otherwise
 */
bool
NavGraphEdge::intersection(float x1, float y1, float x2, float y2,
			   fawkes::cart_coord_2d_t &ip) const
{
  const Eigen::Vector2f e_from(from_node_.x(), from_node_.y());
  const Eigen::Vector2f e_to(to_node_.x(), to_node_.y());
  const Eigen::Vector2f p1(x1, y1);
  const Eigen::Vector2f p2(x2, y2);
  const Eigen::Vector2f lip = line_segm_intersection(e_from, e_to, p1, p2);
#if EIGEN_VERSION_AT_LEAST(3,2,0)
  if (lip.allFinite()) {
#else
  if (workaround::allFinite(lip)) {
#endif
    ip.x = lip[0];
    ip.y = lip[1];
    return true;
  } else {
    return false;
  }
}

/** Check if the edge intersects with another line segment.
 * @param x1 X coordinate of first point of line segment to test
 * @param y1 Y coordinate of first point of line segment to test
 * @param x2 X coordinate of first point of line segment to test
 * @param y2 Y coordinate of first point of line segment to test
 * @return true if the edge intersects with the line segment, false otherwise
 */
bool
NavGraphEdge::intersects(float x1, float y1, float x2, float y2) const
{
  const Eigen::Vector2f e_from(from_node_.x(), from_node_.y());
  const Eigen::Vector2f e_to(to_node_.x(), to_node_.y());
  const Eigen::Vector2f p1(x1, y1);
  const Eigen::Vector2f p2(x2, y2);
  return line_segm_intersect(e_from, e_to, p1, p2);
}

} // end of namespace fawkes

  void draw(MapWriterInterface *interface)
  {
    if (!initialized_) return;

    hector_worldmodel_msgs::GetObjectModel data;
    if (!service_client_.call(data)) {
      ROS_ERROR_NAMED(name_, "Cannot draw victims, service %s failed", service_client_.getService().c_str());
      return;
    }

    int counter = 0;
    for(hector_worldmodel_msgs::ObjectModel::_objects_type::const_iterator it = data.response.model.objects.begin(); it != data.response.model.objects.end(); ++it) {
      const hector_worldmodel_msgs::Object& object = *it;
      if (!draw_all_objects_ && object.state.state != hector_worldmodel_msgs::ObjectState::CONFIRMED) continue;
      if (!class_id_.empty() && object.info.class_id != class_id_) continue;

      Eigen::Vector2f coords;
      coords.x() = object.pose.pose.position.x;
      coords.y() = object.pose.pose.position.y;
      interface->drawObjectOfInterest(coords, boost::lexical_cast<std::string>(++counter), MapWriterInterface::Color(240,10,10));
    }
  }

//the worst algorithm ever written
//breaks up a normal string with newlines to make it fit xLen
std::string Font::wrapText(std::string text, float xLen)
{
	std::string out;

	std::string line, word, temp;
	size_t space;

	Eigen::Vector2f textSize;

	while(text.length() > 0) //while there's text or we still have text to render
	{
		space = text.find_first_of(" \t\n");
		if(space == std::string::npos)
			space = text.length() - 1;

		word = text.substr(0, space + 1);
		text.erase(0, space + 1);

		temp = line + word;

		textSize = sizeText(temp);

		// if the word will fit on the line, add it to our line, and continue
		if(textSize.x() <= xLen)
		{
			line = temp;
			continue;
		}else{
			// the next word won't fit, so break here
			out += line + '\n';
			line = word;
		}
	}

	// whatever's left should fit
	out += line;

	return out;
}

Eigen::Vector3f Terrain::GetNormal(Eigen::Vector2f xy) const{

	//COULD HAVE BUG HERE

	int idx, idz;
	idx = (xy.x() + 0.5*m_size_x)/m_dx;
	idz = (xy.y() + 0.5*m_size_z)/m_dz;

	//interpolation
	Eigen::Vector3f upleft, upright, downleft, downright;
	downleft = m_normal_data[idx*(m_res_z+1) + idz];
	downright = m_normal_data[(idx+1)*(m_res_z+1) + idz];
	upleft = m_normal_data[idx*(m_res_z+1) + idz+1];
	upright = m_normal_data[(idx+1)*(m_res_z+1) + idz+1];

	double alpha, beta;
	alpha = 1 - (xy.x() + 0.5*m_size_x - idx*m_dx)/m_dx;
	beta = 1 - (xy.y() + 0.5*m_size_z - idz*m_dz)/m_dz;

	return alpha*beta*downleft + (1-alpha)*beta*downright + (1-beta)*alpha*upleft + (1-beta)*(1-alpha)*upright;

}

void MainWindow::onCalculateButtonPress()
{
	arcr.setLinePoint(0, 
		ui->line0x0SpinBox->value(), ui->line0y0SpinBox->value(),
		ui->line0x1SpinBox->value(), ui->line0y1SpinBox->value(), 
		ui->line0x2SpinBox->value(), ui->line0y2SpinBox->value());
	
	arcr.setLinePoint(1,
		ui->line1x0SpinBox->value(), ui->line1y0SpinBox->value(),
		ui->line1x1SpinBox->value(), ui->line1y1SpinBox->value(),
		ui->line1x2SpinBox->value(), ui->line1y2SpinBox->value());

	arcr.computeHMatrix();

	Eigen::Vector3f img(inputImage.width(), inputImage.height(), 1.0f);
	float xmin = 0;
	float xmax = 0;
	float ymin = 0;
	float ymax = 0;
	arcr.computImageSize(0, 0, inputImage.width(), inputImage.height(), xmin, xmax, ymin, ymax);

	float aspect = (xmax - xmin) / (ymax - ymin);
	outputImage = QImage(inputImage.width(), inputImage.width() / aspect, inputImage.format());
	outputImage.fill(qRgb(0, 0, 0));

	std::cout << "Output size: " << outputImage.width() << ", " << outputImage.height() << std::endl;

	float dx = (xmax - xmin) / float(outputImage.width());
	float dy = (ymax - ymin) / float(outputImage.height());

	std::cout << std::fixed << "dx, dy: " << dx << ", " << dy << std::endl;

	for (int x = 0; x < outputImage.width(); ++x)
	{
		for (int y = 0; y < outputImage.height(); ++y)
		{
			Eigen::Vector3f px(x, y, 1);

			float tx = 0.0f;
			float ty = 0.0f;
			Eigen::Vector2f t = arcr.multiplyPointMatrixInverse(xmin + x * dx, ymin + y * dy);

			if (t.x() > -1 && t.y() > -1
				&& t.x() < inputImage.width()
				&& t.y() < inputImage.height())
			{
				QRgb rgb = bilinearInterpol(inputImage, t.x(), t.y(), dx, dy);
				outputImage.setPixel(x, y, rgb);
			}
		}
	}


	ui->outputRadioButton->setChecked(true);
	update();
}

double Terrain::GetHeight(Eigen::Vector2f xy) const{
	
	Cube* temp_cube;
	Cylinder* temp_cylinder;

	double x = xy.x();
	double z = xy.y();
	//check if x y lands on a surface object
	for(int i = 0; i < m_surface_objects.size(); i++){
		
		switch (m_surface_objects[i]->m_type)
		{
		case TypeCube:
			temp_cube = dynamic_cast<Cube*>(m_surface_objects[i]);
			if(x < temp_cube->m_Center[0] + temp_cube->m_Size[0]*0.5
				&& x > temp_cube->m_Center[0] - temp_cube->m_Size[0]*0.5
				&& z < temp_cube->m_Center[2] + temp_cube->m_Size[2]*0.5
				&& z > temp_cube->m_Center[2] - temp_cube->m_Size[2]*0.5)
				return temp_cube->m_Size[1];
			break;
		case TypeCylinder:
			temp_cylinder = dynamic_cast<Cylinder*>(m_surface_objects[i]);
			if(x < temp_cylinder->m_Center[0] + temp_cylinder->m_Size[0]*0.5
				&&x > temp_cylinder->m_Center[0] - temp_cylinder->m_Size[0]*0.5
				&&z < temp_cylinder->m_Center[2] + temp_cylinder->m_Size[2]*0.5
				&&z > temp_cylinder->m_Center[0] - temp_cylinder->m_Size[2]*0.5)
				return sqrt(0.25*temp_cylinder->m_Size[1]*temp_cylinder->m_Size[1] - (x - temp_cylinder->m_Center[0])*(x - temp_cylinder->m_Center[0]));
			break;
		default:
			break;
		}
		
	
	}


	int idx, idz;
	idx = (xy.x() + 0.5*m_size_x)/m_dx;
	idz = (xy.y() + 0.5*m_size_z)/m_dz;

	//interpolation
	double upleft, upright, downleft, downright;
	downleft = m_height_data[idx*(m_res_z+1) + idz];
	downright = m_height_data[(idx+1)*(m_res_z+1) + idz];
	upleft = m_height_data[idx*(m_res_z+1) + idz+1];
	upright = m_height_data[(idx+1)*(m_res_z+1) + idz+1];

	double alpha, beta;
	alpha = 1 - (xy.x() + 0.5*m_size_x - idx*m_dx)/m_dx;
	beta = 1 - (xy.y() + 0.5*m_size_z - idz*m_dz)/m_dz;

	return alpha*beta*downleft + (1-alpha)*beta*downright + (1-beta)*alpha*upleft + (1-beta)*(1-alpha)*upright;
}

/** Get the point on edge closest to a given point.
 * The method determines a line perpendicular to the edge which goes through
 * the given point, i.e. the point must be within the imaginary line segment.
 * Then the point on the edge which crosses with that perpendicular line
 * is returned.
 * @param x X coordinate of point to get point on edge for
 * @param y Y coordinate of point to get point on edge for
 * @return coordinate of point on edge closest to given point
 * @throw Exception thrown if the point is out of the line segment and
 * no line perpendicular to the edge going through the given point can
 * be found.
 */
cart_coord_2d_t
NavGraphEdge::closest_point_on_edge(float x, float y) const
{
  const Eigen::Vector2f point(x, y);
  const Eigen::Vector2f origin(from_node_.x(), from_node_.y());
  const Eigen::Vector2f target(to_node_.x(), to_node_.y());
  const Eigen::Vector2f direction(target - origin);
  const Eigen::Vector2f direction_norm = direction.normalized();
  const Eigen::Vector2f diff = point - origin;
  const float t = direction.dot(diff) / direction.squaredNorm();

  if (t >= 0.0 && t <= 1.0) {
    // projection of the point onto the edge is within the line segment
    Eigen::Vector2f point_on_line = origin + direction_norm.dot(diff) * direction_norm;
    return cart_coord_2d_t(point_on_line[0], point_on_line[1]);
  }

  throw Exception("Point (%f,%f) is not on edge %s--%s", x, y,
		  from_.c_str(), to_.c_str());
}

template <typename PointInT, typename IntensityT> void
pcl::tracking::PyramidalKLTTracker<PointInT, IntensityT>::track (const PointCloudInConstPtr& prev_input,
                                                                 const PointCloudInConstPtr& input,
                                                                 const std::vector<FloatImageConstPtr>& prev_pyramid,
                                                                 const std::vector<FloatImageConstPtr>& pyramid,
                                                                 const pcl::PointCloud<pcl::PointUV>::ConstPtr& prev_keypoints,
                                                                 pcl::PointCloud<pcl::PointUV>::Ptr& keypoints,
                                                                 std::vector<int>& status,
                                                                 Eigen::Affine3f& motion) const
{
  std::vector<Eigen::Array2f, Eigen::aligned_allocator<Eigen::Array2f> > next_pts (prev_keypoints->size ());
  Eigen::Array2f half_win ((track_width_-1)*0.5f, (track_height_-1)*0.5f);
  pcl::TransformationFromCorrespondences transformation_computer;
  const int nb_points = prev_keypoints->size ();
  for (int level = nb_levels_ - 1; level >= 0; --level)
  {
    const FloatImage& prev = *(prev_pyramid[level*3]);
    const FloatImage& next = *(pyramid[level*3]);
    const FloatImage& grad_x = *(prev_pyramid[level*3+1]);
    const FloatImage& grad_y = *(prev_pyramid[level*3+2]);

    Eigen::ArrayXXf prev_win (track_height_, track_width_);
    Eigen::ArrayXXf grad_x_win (track_height_, track_width_);
    Eigen::ArrayXXf grad_y_win (track_height_, track_width_);
    float ratio (1./(1 << level));
    for (int ptidx = 0; ptidx < nb_points; ptidx++)
    {
      Eigen::Array2f prev_pt (prev_keypoints->points[ptidx].u * ratio,
                              prev_keypoints->points[ptidx].v * ratio);
      Eigen::Array2f next_pt;
      if (level == nb_levels_ -1)
        next_pt = prev_pt;
      else
        next_pt = next_pts[ptidx]*2.f;

      next_pts[ptidx] = next_pt;

      Eigen::Array2i iprev_point;
      prev_pt -= half_win;
      iprev_point[0] = floor (prev_pt[0]);
      iprev_point[1] = floor (prev_pt[1]);

      if (iprev_point[0] < -track_width_ || (uint32_t) iprev_point[0] >= grad_x.width ||
          iprev_point[1] < -track_height_ || (uint32_t) iprev_point[1] >= grad_y.height)
      {
        if (level == 0)
          status [ptidx] = -1;
        continue;
      }

      float a = prev_pt[0] - iprev_point[0];
      float b = prev_pt[1] - iprev_point[1];
      Eigen::Array4f weight;
      weight[0] = (1.f - a)*(1.f - b);
      weight[1] = a*(1.f - b);
      weight[2] = (1.f - a)*b;
      weight[3] = 1 - weight[0] - weight[1] - weight[2];

      Eigen::Array3f covar = Eigen::Array3f::Zero ();
      spatialGradient (prev, grad_x, grad_y, iprev_point, weight, prev_win, grad_x_win, grad_y_win, covar);

      float det = covar[0]*covar[2] - covar[1]*covar[1];
      float min_eigenvalue = (covar[2] + covar[0] - std::sqrt ((covar[0]-covar[2])*(covar[0]-covar[2]) + 4.f*covar[1]*covar[1]))/2.f;

      if (min_eigenvalue < min_eigenvalue_threshold_ || det < std::numeric_limits<float>::epsilon ())
      {
        status[ptidx] = -2;
        continue;
      }

      det = 1.f/det;
      next_pt -= half_win;

      Eigen::Array2f prev_delta (0, 0);
      for (unsigned int j = 0; j < max_iterations_; j++)
      {
        Eigen::Array2i inext_pt = next_pt.floor ().cast<int> ();

        if (inext_pt[0] < -track_width_ || (uint32_t) inext_pt[0] >= next.width ||
            inext_pt[1] < -track_height_ || (uint32_t) inext_pt[1] >= next.height)
        {
          if (level == 0)
            status[ptidx] = -1;
          break;
        }

        a = next_pt[0] - inext_pt[0];
        b = next_pt[1] - inext_pt[1];
        weight[0] = (1.f - a)*(1.f - b);
        weight[1] = a*(1.f - b);
        weight[2] = (1.f - a)*b;
        weight[3] = 1 - weight[0] - weight[1] - weight[2];
        // compute mismatch vector
        Eigen::Array2f beta = Eigen::Array2f::Zero ();
        mismatchVector (prev_win, grad_x_win, grad_y_win, next, inext_pt, weight, beta);
        // optical flow resolution
        Eigen::Vector2f delta ((covar[1]*beta[1] - covar[2]*beta[0])*det, (covar[1]*beta[0] - covar[0]*beta[1])*det);
        // update position
        next_pt[0] += delta[0]; next_pt[1] += delta[1];
        next_pts[ptidx] = next_pt + half_win;
//.........这里部分代码省略.........

int test_eigen(int argc, char *argv[])
{
	int rc = 0;
	warnx("testing eigen");

	{
		Eigen::Vector2f v;
		Eigen::Vector2f v1(1.0f, 2.0f);
		Eigen::Vector2f v2(1.0f, -1.0f);
		float data[2] = {1.0f, 2.0f};
		TEST_OP("Constructor Vector2f()", Eigen::Vector2f v3);
		TEST_OP_VERIFY("Constructor Vector2f(Vector2f)", Eigen::Vector2f v3(v1), v3.isApprox(v1));
		TEST_OP_VERIFY("Constructor Vector2f(float[])", Eigen::Vector2f v3(data), v3[0] == data[0] && v3[1] == data[1]);
		TEST_OP_VERIFY("Constructor Vector2f(float, float)", Eigen::Vector2f v3(1.0f, 2.0f), v3(0) == 1.0f && v3(1) == 2.0f);
		TEST_OP_VERIFY("Vector2f = Vector2f", v = v1, v.isApprox(v1));
		VERIFY_OP("Vector2f + Vector2f", v = v + v1, v.isApprox(v1 + v1));
		VERIFY_OP("Vector2f - Vector2f", v = v - v1, v.isApprox(v1));
		VERIFY_OP("Vector2f += Vector2f", v += v1, v.isApprox(v1 + v1));
		VERIFY_OP("Vector2f -= Vector2f", v -= v1, v.isApprox(v1));
		TEST_OP_VERIFY("Vector2f dot Vector2f", v.dot(v1), fabs(v.dot(v1) - 5.0f) <= FLT_EPSILON);
		//TEST_OP("Vector2f cross Vector2f", v1.cross(v2)); //cross product for 2d array?
	}

	{
		Eigen::Vector3f v;
		Eigen::Vector3f v1(1.0f, 2.0f, 0.0f);
		Eigen::Vector3f v2(1.0f, -1.0f, 2.0f);
		float data[3] = {1.0f, 2.0f, 3.0f};
		TEST_OP("Constructor Vector3f()", Eigen::Vector3f v3);
		TEST_OP("Constructor Vector3f(Vector3f)", Eigen::Vector3f v3(v1));
		TEST_OP("Constructor Vector3f(float[])", Eigen::Vector3f v3(data));
		TEST_OP("Constructor Vector3f(float, float, float)", Eigen::Vector3f v3(1.0f, 2.0f, 3.0f));
		TEST_OP("Vector3f = Vector3f", v = v1);
		TEST_OP("Vector3f + Vector3f", v + v1);
		TEST_OP("Vector3f - Vector3f", v - v1);
		TEST_OP("Vector3f += Vector3f", v += v1);
		TEST_OP("Vector3f -= Vector3f", v -= v1);
		TEST_OP("Vector3f * float", v1 * 2.0f);
		TEST_OP("Vector3f / float", v1 / 2.0f);
		TEST_OP("Vector3f *= float", v1 *= 2.0f);
		TEST_OP("Vector3f /= float", v1 /= 2.0f);
		TEST_OP("Vector3f dot Vector3f", v.dot(v1));
		TEST_OP("Vector3f cross Vector3f", v1.cross(v2));
		TEST_OP("Vector3f length", v1.norm());
		TEST_OP("Vector3f length squared", v1.squaredNorm());
#pragma GCC diagnostic push
#pragma GCC diagnostic ignored "-Wunused-variable"
		// Need pragma here intead of moving variable out of TEST_OP and just reference because
		// TEST_OP measures performance of vector operations.
		TEST_OP("Vector<3> element read", volatile float a = v1(0));
		TEST_OP("Vector<3> element read direct", volatile float a = v1.data()[0]);
#pragma GCC diagnostic pop
		TEST_OP("Vector<3> element write", v1(0) = 1.0f);
		TEST_OP("Vector<3> element write direct", v1.data()[0] = 1.0f);
	}

	{
		Eigen::Vector4f v(0.0f, 0.0f, 0.0f, 0.0f);
		Eigen::Vector4f v1(1.0f, 2.0f, 0.0f, -1.0f);
		Eigen::Vector4f v2(1.0f, -1.0f, 2.0f, 0.0f);
		Eigen::Vector4f vres;
		float data[4] = {1.0f, 2.0f, 3.0f, 4.0f};
		TEST_OP("Constructor Vector<4>()", Eigen::Vector4f v3);
		TEST_OP("Constructor Vector<4>(Vector<4>)", Eigen::Vector4f v3(v1));
		TEST_OP("Constructor Vector<4>(float[])", Eigen::Vector4f v3(data));
		TEST_OP("Constructor Vector<4>(float, float, float, float)", Eigen::Vector4f v3(1.0f, 2.0f, 3.0f, 4.0f));
		TEST_OP("Vector<4> = Vector<4>", v = v1);
		TEST_OP("Vector<4> + Vector<4>", v + v1);
		TEST_OP("Vector<4> - Vector<4>", v - v1);
		TEST_OP("Vector<4> += Vector<4>", v += v1);
		TEST_OP("Vector<4> -= Vector<4>", v -= v1);
		TEST_OP("Vector<4> dot Vector<4>", v.dot(v1));
	}

	{
		Eigen::Vector10f v1;
		v1.Zero();
		float data[10];
		TEST_OP("Constructor Vector<10>()", Eigen::Vector10f v3);
		TEST_OP("Constructor Vector<10>(Vector<10>)", Eigen::Vector10f v3(v1));
		TEST_OP("Constructor Vector<10>(float[])", Eigen::Vector10f v3(data));
	}

	{
		Eigen::Matrix3f m1;
		m1.setIdentity();
		Eigen::Matrix3f m2;
		m2.setIdentity();
		Eigen::Vector3f v1(1.0f, 2.0f, 0.0f);
		TEST_OP("Matrix3f * Vector3f", m1 * v1);
		TEST_OP("Matrix3f + Matrix3f", m1 + m2);
		TEST_OP("Matrix3f * Matrix3f", m1 * m2);
	}

	{
		Eigen::Matrix<float, 10, 10> m1;
		m1.setIdentity();
		Eigen::Matrix<float, 10, 10> m2;
		m2.setIdentity();
		Eigen::Vector10f v1;
//.........这里部分代码省略.........

 static inline float distance(const Eigen::Vector2f & v0, const Eigen::Vector2f & v1)
 {
     return sqrt(
         (v0.x() - v1.x()) * (v0.x() - v1.x()) +
         (v0.y() - v1.y()) * (v0.y() - v1.y()));
 }

//.........这里部分代码省略.........
    {
      float distance = fabsf (line_x * polygon[idx].x + line_y * polygon[idx].y + line_d);
      if (distance > max_distance)
      {
        max_distance = distance;
        max_index  = idx;
      }
    }

    if (max_distance > threshold)
    {
      std::pair<unsigned, unsigned> interval (max_index, currentInterval.second);
      currentInterval.second = max_index;
      intervals.push_back (interval);
    }
    else
    {
      result.push_back (currentInterval.second);
      intervals.pop_back ();
    }
  }
  
  approx_polygon.reserve (result.size ());
  if (refine)
  {
    std::vector<Eigen::Vector3f> lines (result.size ());
    std::reverse (result.begin (), result.end ());
    for (unsigned rIdx = 0; rIdx < result.size (); ++rIdx)
    {
      unsigned nIdx = rIdx + 1;
      if (nIdx == result.size ())
        nIdx = 0;
      
      Eigen::Vector2f centroid = Eigen::Vector2f::Zero ();
      Eigen::Matrix2f covariance = Eigen::Matrix2f::Zero ();
      unsigned pIdx = result[rIdx];
      unsigned num_points = 0;
      if (pIdx > result[nIdx])
      {
        num_points = static_cast<unsigned> (polygon.size ()) - pIdx;
        for (; pIdx < polygon.size (); ++pIdx)
        {
          covariance.coeffRef (0) += polygon [pIdx].x * polygon [pIdx].x;
          covariance.coeffRef (1) += polygon [pIdx].x * polygon [pIdx].y;
          covariance.coeffRef (3) += polygon [pIdx].y * polygon [pIdx].y;
          centroid [0] += polygon [pIdx].x;
          centroid [1] += polygon [pIdx].y;
        }
        pIdx = 0;
      }
      
      num_points += result[nIdx] - pIdx;
      for (; pIdx < result[nIdx]; ++pIdx)
      {
        covariance.coeffRef (0) += polygon [pIdx].x * polygon [pIdx].x;
        covariance.coeffRef (1) += polygon [pIdx].x * polygon [pIdx].y;
        covariance.coeffRef (3) += polygon [pIdx].y * polygon [pIdx].y;
        centroid [0] += polygon [pIdx].x;
        centroid [1] += polygon [pIdx].y;
      }
      
      covariance.coeffRef (2) = covariance.coeff (1);
      
      float norm = 1.0f / float (num_points);
      centroid *= norm;
      covariance *= norm;

void PathCmp::search (Eigen::Vector2f targetPos_) {
    _level = LogicSystem::getInstance()->getLevel();
    for (auto it = _openList.begin(); it != _openList.end(); ++ it) {
        if (*it != nullptr)
            delete *it;
    }
    _openList.clear();

    for (auto it = _closedList.begin(); it != _closedList.end(); ++ it) {
        if (*it != nullptr)
            delete *it;
    }
    _closedList.clear();
    _shortestPath.clear();

    auto posCmp = getEntity()->GET_CMP(PositionComponent);
    _srcIdx = _level->posToTileIndex(posCmp->getPosition());
    _tarIdx = _level->posToTileIndex(targetPos_);
    if (_srcIdx == _tarIdx)
        return;

    if (_level->isTileObstacle(_tarIdx.x(), _tarIdx.y()))
        return;

    PathNode* srcNode = new PathNode(_srcIdx);
    calcCostH(srcNode);
    _openList.push_back(srcNode);
    do {
        PathNode* curNode = _openList[0];
        _closedList.push_back(curNode);
        _openList.erase(_openList.begin());

        if (curNode->getIndex() == _tarIdx) {
            PathNode* walkNode = curNode;
            _shortestPath.clear();
            do {
                Eigen::Vector2f pos;
                pos.x() = _level->tileIndexXToPosX(walkNode->getIndex().x());
                pos.y() = _level->tileIndexYToPosY(walkNode->getIndex().y());
                _shortestPath.push_front(pos);
                walkNode = walkNode->getParent();
            } while (walkNode);
            for (auto it = _openList.begin(); it != _openList.end(); ++ it) {
                if (*it != nullptr)
                    delete *it;
            }
            _openList.clear();
            for (auto it = _closedList.begin(); it != _closedList.end(); ++ it) {
                if (*it != nullptr)
                    delete *it;
            }
            _closedList.clear();
            break;
        }

        putAdjacentTiles(curNode);
        for (auto it = _adjacentTiles.begin(); it != _adjacentTiles.end(); ++ it) {
            bool isInClosedList = false;
            for (int i = 0; i < _closedList.size(); ++ i)
                if ((*it) == _closedList[i]->getIndex())
                    isInClosedList = true;
            if (isInClosedList) continue;

            auto node = new PathNode(*it);
            int costGToAdd = calcCostG(node, curNode);
            std::vector<PathNode *>::iterator opit = _openList.begin();
            while (opit != _openList.end()) {
                if ((*opit)->getIndex() == node->getIndex())
                    break;
                ++ opit;
            }

            // already on openlist
            if (opit!= _openList.end()) {
                delete node;
                node = (*opit);
                if (curNode->getCostG() + costGToAdd < node->getCostG())
                    node->setCostG(curNode->getCostG() + costGToAdd);
            } else {
                node->setParent(curNode);
                node->setCostG(curNode->getCostG() + costGToAdd);
                calcCostH(node);
                addOpenList(node);
            }
        }
    } while (_openList.size() > 0);
}

// 2D cross product of OA and OB vectors, i.e. z-component of their 3D cross product.
// Returns a positive value, if OAB makes a counter-clockwise turn,
// negative for clockwise turn, and zero if the points are collinear.
float cross(const Eigen::Vector2f O, const Eigen::Vector2f A, const Eigen::Vector2f B) {
    return (long)(A.x() - O.x()) * (B.y() - O.y()) - (long)(A.y() - O.y()) * (B.x() - O.x());
}

void derivatives(const StateMatrix& states, StateMatrix& derivs, 
    const ControlMatrix& ctrls, const SimulationParameters& params, 
    const Map & map)
{
  float cd_a_rho = params.linearDrag;  // 0.1  coeff of drag * area * density of fluid
  float k_elastic = params.elasticity;  // 4000.  // spring constant of ships
  float rad = 1.;  // leave radius 1 - we decided to change map scale instead
  const Eigen::VectorXf &masses = params.shipDensities;  // order of 1.0 Mass of ships 
  float spin_drag_ratio = params.rotationalDrag; // 1.8;  // spin friction to translation friction
  float eps = 1e-5;  // Avoid divide by zero special cases
  float mu = params.shipFriction;  // 0.05;  // friction coefficient between ships
  float mu_wall = params.wallFriction;  //0.25*?wallFriction;  // 0.01;  // wall friction parameter
  float wall_restitution = params.wallRestitution; // 0.5
  float ship_restitution = params.shipRestitution; // circa 0.5
  float diameter = 2.*rad;  // rad(i) + rad(j) for any i, j
  float inertia_mass_ratio = 0.25;
  float map_grid = rad * 2. + eps; // must be 2*radius + eps
  std::unordered_map<std::pair<int, int>, std::vector<uint>, 
                     boost::hash<std::pair<int, int>>> bins;

  uint n = states.rows();
  Eigen::MatrixXd f = Eigen::MatrixXd::Zero(n, 2);
  Eigen::VectorXd trq = Eigen::VectorXd::Zero(n);
  // rotationalThrust Order +- 10 
  // linearThrust Order +100
  // mapscale order 10 - thats params.pixelsize
  // Accumulate forces and torques into these:
  uint collide_checks = 0;  // debug count...

  for (uint i=0; i<n; i++) {
    Eigen::Vector2f pos_i;
    pos_i(0) = states(i,0);
    pos_i(1) = states(i,1);
    Eigen::Vector2f vel_i;
    vel_i(0) = states(i,2);
    vel_i(1) = states(i,3);
    float theta_i = states(i,4);
    float w_i = states(i,5);

    // 1. Control 
    float thrusting = ctrls(i, 0);
    float turning = ctrls(i, 1);
    f(i, 0) = thrusting * params.linearThrust * cos(theta_i);
    f(i, 1) = thrusting * params.linearThrust * sin(theta_i);
    trq(i) = turning * params.rotationalThrust;

    // 2. Drag
    f(i, 0) -= cd_a_rho * vel_i(0);
    f(i, 1) -= cd_a_rho * vel_i(1);
    trq(i) -= spin_drag_ratio*cd_a_rho*w_i*rad*rad; // * abs(w_i)

    // 3. Inter-ship collisions agains