/** 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; }
/** 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(); }
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; }
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]); } }