Exemple #1
0
/** 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;
}
Exemple #2
0
/** 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());
}
    float SupportPolygon::getSquaredDistLine(Eigen::Vector2f& p, Eigen::Vector2f& pt1, Eigen::Vector2f& pt2)
    {
        //nearestPointOnLine
        Eigen::Vector2f lp = p - pt1;
        Eigen::Vector2f dir = (pt2 - pt1);
        dir.normalize();
        float lambda = dir.dot(lp);
        Eigen::Vector2f ptOnLine = pt1 + lambda * dir;

        //distPointLine
        return (ptOnLine - p).squaredNorm();
    }
Exemple #4
0
bool mrpt::pbmap::isInHull(PointT& point3D, pcl::PointCloud<PointT>::Ptr hull3D)
{
	Eigen::Vector2f normalLine;  // This vector points inward the hull
	Eigen::Vector2f r;
	for (size_t i = 1; i < hull3D->size(); i++)
	{
		normalLine[0] = hull3D->points[i - 1].y - hull3D->points[i].y;
		normalLine[1] = hull3D->points[i].x - hull3D->points[i - 1].x;
		r[0] = point3D.x - hull3D->points[i].x;
		r[1] = point3D.y - hull3D->points[i].y;
		if ((r.dot(normalLine)) < 0) return false;
	}
	return true;
}
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;
		v1.Zero();
		TEST_OP("Matrix<10, 10> * Vector<10>", m1 * v1);
		TEST_OP("Matrix<10, 10> + Matrix<10, 10>", m1 + m2);
		TEST_OP("Matrix<10, 10> * Matrix<10, 10>", m1 * m2);
	}

	{
		warnx("Nonsymmetric matrix operations test");
		// test nonsymmetric +, -, +=, -=

		const Eigen::Matrix<float, 2, 3> m1_orig =
			(Eigen::Matrix<float, 2, 3>() << 1, 2, 3,
			 4, 5, 6).finished();

		Eigen::Matrix<float, 2, 3> m1(m1_orig);

		Eigen::Matrix<float, 2, 3> m2;
		m2 << 2, 4, 6,
		8, 10, 12;

		Eigen::Matrix<float, 2, 3> m3;
		m3 << 3, 6, 9,
		12, 15, 18;

		if (m1 + m2 != m3) {
			warnx("Matrix<2, 3> + Matrix<2, 3> failed!");
			printEigen(m1 + m2);
			printf("!=\n");
			printEigen(m3);
			rc = 1;
		}

		if (m3 - m2 != m1) {
			warnx("Matrix<2, 3> - Matrix<2, 3> failed!");
			printEigen(m3 - m2);
			printf("!=\n");
			printEigen(m1);
			rc = 1;
		}

		m1 += m2;

		if (m1 != m3) {
			warnx("Matrix<2, 3> += Matrix<2, 3> failed!");
			printEigen(m1);
			printf("!=\n");
			printEigen(m3);
			rc = 1;
		}

		m1 -= m2;

		if (m1 != m1_orig) {
			warnx("Matrix<2, 3> -= Matrix<2, 3> failed!");
			printEigen(m1);
			printf("!=\n");
			printEigen(m1_orig);
			rc = 1;
		}

	}

	/* QUATERNION TESTS CURRENTLY FAILING
	{
		// test conversion rotation matrix to quaternion and back
		Eigen::Matrix3f R_orig;
		Eigen::Matrix3f R;
		Eigen::Quaternionf q;
		float diff = 0.1f;
		float tol = 0.00001f;

		warnx("Quaternion transformation methods test.");

		for (float roll = -M_PI_F; roll <= M_PI_F; roll += diff) {
			for (float pitch = -M_PI_2_F; pitch <= M_PI_2_F; pitch += diff) {
				for (float yaw = -M_PI_F; yaw <= M_PI_F; yaw += diff) {
					R_orig.eulerAngles(roll, pitch, yaw);
					q = R_orig; //from_dcm
					R = q.toRotationMatrix();

					for (size_t i = 0; i < 3; i++) {
						for (size_t j = 0; j < 3; j++) {
							if (fabsf(R_orig(i, j) - R(i, j)) > tol) {
								warnx("Quaternion method 'from_dcm' or 'toRotationMatrix' outside tolerance!");
								rc = 1;
							}
						}
					}
				}
			}
		}

		// test against some known values
		tol = 0.0001f;
		Eigen::Quaternionf q_true = {1.0f, 0.0f, 0.0f, 0.0f};
		R_orig.setIdentity();
		q = R_orig;

		for (size_t i = 0; i < 4; i++) {
			if (!q.isApprox(q_true, tol)) {
				warnx("Quaternion method 'from_dcm()' outside tolerance!");
				rc = 1;
			}
		}

		q_true = quatFromEuler(0.3f, 0.2f, 0.1f);
		q = {0.9833f, 0.1436f, 0.1060f, 0.0343f};

		for (size_t i = 0; i < 4; i++) {
			if (!q.isApprox(q_true, tol)) {
				warnx("Quaternion method 'eulerAngles()' outside tolerance!");
				rc = 1;
			}
		}

		q_true = quatFromEuler(-1.5f, -0.2f, 0.5f);
		q = {0.7222f, -0.6391f, -0.2386f, 0.1142f};

		for (size_t i = 0; i < 4; i++) {
			if (!q.isApprox(q_true, tol)) {
				warnx("Quaternion method 'eulerAngles()' outside tolerance!");
				rc = 1;
			}
		}

		q_true = quatFromEuler(M_PI_2_F, -M_PI_2_F, -M_PI_F / 3);
		q = {0.6830f, 0.1830f, -0.6830f, 0.1830f};

		for (size_t i = 0; i < 4; i++) {
			if (!q.isApprox(q_true, tol)) {
				warnx("Quaternion method 'eulerAngles()' outside tolerance!");
				rc = 1;
			}
		}

	}

	{
		// test quaternion method "rotate" (rotate vector by quaternion)
		Eigen::Vector3f vector = {1.0f, 1.0f, 1.0f};
		Eigen::Vector3f vector_q;
		Eigen::Vector3f vector_r;
		Eigen::Quaternionf q;
		Eigen::Matrix3f R;
		float diff = 0.1f;
		float tol = 0.00001f;

		warnx("Quaternion vector rotation method test.");

		for (float roll = -M_PI_F; roll <= M_PI_F; roll += diff) {
			for (float pitch = -M_PI_2_F; pitch <= M_PI_2_F; pitch += diff) {
				for (float yaw = -M_PI_F; yaw <= M_PI_F; yaw += diff) {
					R.eulerAngles(roll, pitch, yaw);
					q = quatFromEuler(roll, pitch, yaw);
					vector_r = R * vector;
					vector_q = q._transformVector(vector);

					for (int i = 0; i < 3; i++) {
						if (fabsf(vector_r(i) - vector_q(i)) > tol) {
							warnx("Quaternion method 'rotate' outside tolerance");
							rc = 1;
						}
					}
				}
			}
		}

		// test some values calculated with matlab
		tol = 0.0001f;
		q = quatFromEuler(M_PI_2_F, 0.0f, 0.0f);
		vector_q = q._transformVector(vector);
		Eigen::Vector3f vector_true = {1.00f, -1.00f, 1.00f};

		for (size_t i = 0; i < 3; i++) {
			if (fabsf(vector_true(i) - vector_q(i)) > tol) {
				warnx("Quaternion method 'rotate' outside tolerance");
				rc = 1;
			}
		}

		q = quatFromEuler(0.3f, 0.2f, 0.1f);
		vector_q = q._transformVector(vector);
		vector_true = {1.1566, 0.7792, 1.0273};

		for (size_t i = 0; i < 3; i++) {
			if (fabsf(vector_true(i) - vector_q(i)) > tol) {
				warnx("Quaternion method 'rotate' outside tolerance");
				rc = 1;
			}
		}

		q = quatFromEuler(-1.5f, -0.2f, 0.5f);
		vector_q = q._transformVector(vector);
		vector_true = {0.5095, 1.4956, -0.7096};

		for (size_t i = 0; i < 3; i++) {
			if (fabsf(vector_true(i) - vector_q(i)) > tol) {
				warnx("Quaternion method 'rotate' outside tolerance");
				rc = 1;
			}
		}

		q = quatFromEuler(M_PI_2_F, -M_PI_2_F, -M_PI_F / 3.0f);
		vector_q = q._transformVector(vector);
		vector_true = { -1.3660, 0.3660, 1.0000};

		for (size_t i = 0; i < 3; i++) {
			if (fabsf(vector_true(i) - vector_q(i)) > tol) {
				warnx("Quaternion method 'rotate' outside tolerance");
				rc = 1;
			}
		}
	}
	*/
	return rc;
}
Exemple #6
0
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 against ships of lower index...  
    // Figure out this ship's hashes: It has 4 in 2 dimensions
    std::unordered_set<uint> collision_shortlist;
    std::pair<int, int> my_hash;
    for (int dx=-1; dx < 2; dx+=2)
      for (int dy=-1; dy < 2; dy+=2)
      {
        float x_mod = pos_i(0) + float(dx)*rad;
        float y_mod = pos_i(1) + float(dy)*rad;
        my_hash = std::make_pair(int(x_mod / map_grid), 
                                 int(y_mod / map_grid));
        if (bins.count(my_hash) > 0)
        {
            // Already exists - shortlist others and add self
            std::vector<uint> current_bin = bins.find(my_hash)->second; 
            // -->first is the key as it returns a key/value pair
            for (uint bin_idx: current_bin)
              if (bin_idx != i)
                collision_shortlist.insert(bin_idx);
            current_bin.push_back(i);
        }
        else
        {
            // didnt exist - add self, and push into map
            std::vector<uint> current_bin;
            current_bin.push_back(i);
            bins.insert(std::make_pair(my_hash, current_bin));
        }
      }

    for (uint j: collision_shortlist) {  // =i+1; j<n; j++) {
      collide_checks ++;
      // std::cout << "Checking " << i << ", " << j << "\n";
      Eigen::Vector2f pos_j;
      pos_j(0) = states(j,0);
      pos_j(1) = states(j,1);
      Eigen::Vector2f vel_j;
      vel_j(0) = states(j,2);
      vel_j(1) = states(j,3);
      float theta_j = states(j,4);
      float w_j = states(j,5);

      Eigen::Vector2f dP = pos_j - pos_i;
      float dist = dP.norm() + eps - diameter;
      Eigen::Vector2f dPhat = dP / (dP.norm() + eps);
      if (dist < 0) {
        // we have a collision interaction
        
        // A. Direct collision: apply linear spring normal force
        float f_magnitude = - dist * k_elastic; // dist < =
        if ((vel_j - vel_i).dot(pos_j - pos_i) > 0)
            f_magnitude *= ship_restitution;
        Eigen::Vector2f f_norm = f_magnitude * dPhat;
        f(i, 0) -= f_norm(0);
        f(i, 1) -= f_norm(1);
        f(j, 0) += f_norm(0);
        f(j, 1) += f_norm(1);

        // B. Surface frictions: approximate spin effects
        Eigen::Vector2f perp;  // surface tangent pointing +theta direction
        perp(0) = -dPhat(1);
        perp(1) = dPhat(0);
        
        // relative velocities of surfaces
        float v_rel = rad*w_i + rad*w_j + perp.dot(vel_i - vel_j);
        float fric = f_magnitude * mu * sigmoid(v_rel);
        Eigen::Vector2f f_fric = fric * perp;
        f(i, 0) += f_fric(0);
        f(i, 1) += f_fric(1);
        f(j, 0) -= f_fric(0);
        f(j, 1) -= f_fric(1);
        trq(i) -= fric * rad;
        trq(j) -= fric * rad;
      }  // end collision
    } // end loop 3. opposing ship


    // 4. Wall single body collisions
    // compute distance to wall and local normals
    float wall_dist, norm_x, norm_y;
    interpolate_map(pos_i(0), pos_i(1), wall_dist, norm_x, norm_y, map, params);
    float dist = wall_dist - rad;
    if (dist < 0)
    {
      /* if (dist < -1.) */
      /*     assert(false); */

      // Spring force
      float f_norm_mag = -dist*k_elastic;  // dist is negative, f_norm is +ve
      if (norm_x*vel_i(0) + norm_y*vel_i(1) > 0)
          f_norm_mag *= wall_restitution;
      
      if (dist > -rad*0.25)
      {
        // not significantly through wall yet
        f(i, 0) += f_norm_mag * norm_x;
        f(i, 1) += f_norm_mag * norm_y;
      }
      else
      {
        // uh-oh - lets just SET normal forces and seriously damp vel
        f(i, 0) = f_norm_mag * norm_x;
        f(i, 1) = f_norm_mag * norm_y;
        f(i, 0) -= 100. * vel_i(0);
        f(i, 1) -= 100. * vel_i(1);
        
      }
      // Surface friction
      Eigen::Vector2f perp;  // surface tangent pointing +theta direction
      perp(0) = -norm_y;
      perp(1) = norm_x;
      float v_rel = w_i * rad + vel_i(0)*norm_y - vel_i(1)*norm_x;
      float fric = f_norm_mag * mu_wall * sigmoid(v_rel);
      f(i, 0) -= fric*norm_y;
      f(i, 1) += fric*norm_x;
      trq(i) -= fric * rad;
    }
  } // end loop current ship

  // std::cout << "Collision checks:" << collide_checks << "\n";
  // Compose the vector of derivatives:
  float vmax = 40.0;
  for (int i=0; i<n; i++)
  {
    float vx = states(i,2);
    float vy = states(i,3);
    float speed = std::sqrt(vx*vx + vy*vy);
    if (speed > vmax)
    {
      vx *= vmax/speed;
      vy *= vmax/speed;
    }
    
    // x_dot = vx
    derivs(i, 0) = vx;
    // y_dot = vy
    derivs(i, 1) = vy; 
    // vx_dot = fx / m
    float ax = f(i,0)/masses(i);
    float ay = f(i,1)/masses(i);
    
    derivs(i, 2) = ax;
    // vy_dot = fy / m
    derivs(i, 3) = ay;
    // theta_dot = omega
    derivs(i, 4) = states(i, 5);
    // omega_dot = T_r / (inertia_mass_ratio*m)
    derivs(i,5) = trq(i) / (inertia_mass_ratio * masses(i));
  }
  
  // ux uy vx vy  theta omega
  // 0  1  2  3   4     5
}
template <typename PointT> void
pcl16::approximatePolygon2D (const typename pcl16::PointCloud<PointT>::VectorType &polygon, 
                           typename pcl16::PointCloud<PointT>::VectorType &approx_polygon, 
                           float threshold, bool refine, bool closed)
{
  approx_polygon.clear ();
  if (polygon.size () < 3)
    return;
  
  std::vector<std::pair<unsigned, unsigned> > intervals;
  std::pair<unsigned,unsigned> interval (0, 0);
  
  if (closed)
  {
    float max_distance = .0f;
    for (unsigned idx = 1; idx < polygon.size (); ++idx)
    {
      float distance = (polygon [0].x - polygon [idx].x) * (polygon [0].x - polygon [idx].x) + 
                       (polygon [0].y - polygon [idx].y) * (polygon [0].y - polygon [idx].y);

      if (distance > max_distance)
      {
        max_distance = distance;
        interval.second = idx;
      }
    }

    for (unsigned idx = 1; idx < polygon.size (); ++idx)
    {
      float distance = (polygon [interval.second].x - polygon [idx].x) * (polygon [interval.second].x - polygon [idx].x) + 
                       (polygon [interval.second].y - polygon [idx].y) * (polygon [interval.second].y - polygon [idx].y);

      if (distance > max_distance)
      {
        max_distance = distance;
        interval.first = idx;
      }
    }

    if (max_distance < threshold * threshold)
      return;

    intervals.push_back (interval);
    std::swap (interval.first, interval.second);
    intervals.push_back (interval);
  }
  else
  {
    interval.first = 0;
    interval.second = static_cast<unsigned int> (polygon.size ()) - 1;
    intervals.push_back (interval);
  }
  
  std::vector<unsigned> result;
  // recursively refine
  while (!intervals.empty ())
  {
    std::pair<unsigned, unsigned>& currentInterval = intervals.back ();
    float line_x = polygon [currentInterval.first].y - polygon [currentInterval.second].y;
    float line_y = polygon [currentInterval.second].x - polygon [currentInterval.first].x;
    float line_d = polygon [currentInterval.first].x * polygon [currentInterval.second].y - polygon [currentInterval.first].y * polygon [currentInterval.second].x;
    
    float linelen = 1.0f / sqrt (line_x * line_x + line_y * line_y);
    
    line_x *= linelen;
    line_y *= linelen;
    line_d *= linelen;
    
    float max_distance = 0.0;
    unsigned first_index = currentInterval.first + 1;
    unsigned max_index = 0;

    // => 0-crossing
    if (currentInterval.first > currentInterval.second)
    {
      for (unsigned idx = first_index; idx < polygon.size(); idx++)
      {
        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;
        }
      }
      first_index = 0;
    }

    for (unsigned int idx = first_index; idx < currentInterval.second; idx++)
    {
      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;
      covariance.coeffRef (0) -= centroid [0] * centroid [0];
      covariance.coeffRef (1) -= centroid [0] * centroid [1];
      covariance.coeffRef (3) -= centroid [1] * centroid [1];
      
      float eval;
      Eigen::Vector2f normal;
      eigen22 (covariance, eval, normal);

      // select the one which is more "parallel" to the original line
      Eigen::Vector2f direction;
      direction [0] = polygon[result[nIdx]].x - polygon[result[rIdx]].x;
      direction [1] = polygon[result[nIdx]].y - polygon[result[rIdx]].y;
      direction.normalize ();
      
      if (fabs (direction.dot (normal)) > float(M_SQRT1_2))
      {
        std::swap (normal [0], normal [1]);
        normal [0] = -normal [0];
      }
      
      // needs to be on the left side of the edge
      if (direction [0] * normal [1] < direction [1] * normal [0])
        normal *= -1.0;
      
      lines [rIdx].head<2> () = normal;
      lines [rIdx] [2] = -normal.dot (centroid);
    }
    
    float threshold2 = threshold * threshold;
    for (unsigned rIdx = 0; rIdx < lines.size (); ++rIdx)
    {
      unsigned nIdx = rIdx + 1;
      if (nIdx == result.size ())
        nIdx = 0;      
      
      Eigen::Vector3f vertex = lines [rIdx].cross (lines [nIdx]);
      vertex /= vertex [2];
      vertex [2] = 0.0;

      PointT point;      
      // test whether we need another edge since the intersection point is too far away from the original vertex
      Eigen::Vector3f pq = polygon [result[nIdx]].getVector3fMap () - vertex;
      pq [2] = 0.0;
      
      float distance = pq.squaredNorm ();
      if (distance > threshold2)
      {
        // test whether the old point is inside the new polygon or outside
        if ((pq [0] * lines [rIdx] [0] + pq [1] * lines [rIdx] [1] < 0.0) &&
            (pq [0] * lines [nIdx] [0] + pq [1] * lines [nIdx] [1] < 0.0) )
        {
          float distance1 = lines [rIdx] [0] * polygon[result[nIdx]].x + lines [rIdx] [1] * polygon[result[nIdx]].y + lines [rIdx] [2];
          float distance2 = lines [nIdx] [0] * polygon[result[nIdx]].x + lines [nIdx] [1] * polygon[result[nIdx]].y + lines [nIdx] [2];

          point.x = polygon[result[nIdx]].x - distance1 * lines [rIdx] [0];
          point.y = polygon[result[nIdx]].y - distance1 * lines [rIdx] [1];

          approx_polygon.push_back (point);

          vertex [0] = polygon[result[nIdx]].x - distance2 * lines [nIdx] [0];
          vertex [1] = polygon[result[nIdx]].y - distance2 * lines [nIdx] [1];
        }
      }
      point.getVector3fMap () = vertex;
      approx_polygon.push_back (point);
    }
  }
  else
  {
    // we have a new polygon in results, but inverted (clockwise <-> counter-clockwise)    
    for (std::vector<unsigned>::reverse_iterator it = result.rbegin (); it != result.rend (); ++it)
      approx_polygon.push_back (polygon [*it]);
  }
}