Head

GCC Code Coverage Report

Directory:	./		Exec	Total	Coverage
File:	src/sampling/heuristic-tools.cc	Lines:	0	211	0.0 %
Date:	2024-02-02 12:21:48	Branches:	0	254	0.0 %


#include <hpp/rbprm/sampling/heuristic-tools.hh>

namespace hpp {
namespace rbprm {
namespace sampling {

HeuristicParam::HeuristicParam(const std::map<std::string, fcl::Vec3f>& cp,
                               const fcl::Vec3f& comPos,
                               const fcl::Vec3f& comSp,
                               const fcl::Vec3f& comAcc, const std::string& sln,
                               const fcl::Transform3f& tf)
    : contactPositions_(cp),
      comPosition_(comPos),
      comSpeed_(comSp),
      comAcceleration_(comAcc),
      sampleLimbName_(sln),
      tfWorldRoot_(tf) {}
HeuristicParam::HeuristicParam(const HeuristicParam& zhp)
    : contactPositions_(zhp.contactPositions_),
      comPosition_(zhp.comPosition_),
      comSpeed_(zhp.comSpeed_),
      comAcceleration_(zhp.comAcceleration_),
      sampleLimbName_(zhp.sampleLimbName_),
      tfWorldRoot_(zhp.tfWorldRoot_) {}
HeuristicParam& HeuristicParam::operator=(const HeuristicParam& zhp) {
  if (this != &zhp) {
    this->contactPositions_.clear();
    this->contactPositions_.insert(zhp.contactPositions_.begin(),
                                   zhp.contactPositions_.end());
    this->comPosition_ = zhp.comPosition_;
    this->comSpeed_ = zhp.comSpeed_;
    this->comAcceleration_ = zhp.comAcceleration_;
    this->sampleLimbName_ = zhp.sampleLimbName_;
    this->tfWorldRoot_ = zhp.tfWorldRoot_;
  }
  return *this;
}

fcl::Vec3f transform(const fcl::Vec3f& p, const fcl::Vec3f& tr,
                     const fcl::Matrix3f& ro) {
  fcl::Vec3f res(p[0] * ro(0, 0) + p[1] * ro(0, 1) + p[2] * ro(0, 2) + tr[0],
                 p[0] * ro(1, 0) + p[1] * ro(1, 1) + p[2] * ro(1, 2) + tr[1],
                 p[0] * ro(2, 0) + p[1] * ro(2, 1) + p[2] * ro(2, 2) + tr[2]);
  return res;
}

Vec2D& Vec2D::operator=(const Vec2D& c) {
  if (this != &c) {
    this->x = c.x;
    this->y = c.y;
  }
  return *this;
}
double Vec2D::operator[](int idx) const {
  idx = idx % 2;
  if (idx == 0)
    return this->x;
  else
    return this->y;
}
double& Vec2D::operator[](int idx) {
  idx = idx % 2;
  if (idx == 0)
    return this->x;
  else
    return this->y;
}
double Vec2D::euclideanDist(const Vec2D& v1, const Vec2D& v2) {
  return std::sqrt(std::pow(v1.x - v2.x, 2) + std::pow(v1.y - v2.y, 2));
}
bool operator==(const Vec2D& v1, const Vec2D& v2) {
  // return ((v1.x == v2.x) && (v1.y == v2.y));
  return ((std::abs(v1.x - v2.x) < 1e-9) && (std::abs(v1.y - v2.y) < 1e-9));
}
bool operator!=(const Vec2D& v1, const Vec2D& v2) { return !(v1 == v2); }
std::ostream& operator<<(std::ostream& out, const Vec2D& v) {
  out << "(" << v.x << ", " << v.y << ")";
  return out;
}
Plane& Plane::operator=(const Plane& pe) {
  if (this != &pe) {
    this->a = pe.a;
    this->b = pe.b;
    this->c = pe.c;
    this->d = pe.d;
  }
  return *this;
}

fcl::Vec3f orthogonalProjection(const fcl::Vec3f& point, const Plane& plane) {
  double k(
      ((plane.a * point[0]) + (plane.b * point[1]) + (plane.c * point[2]) +
       plane.d) /
      (std::pow(plane.a, 2) + std::pow(plane.b, 2) + std::pow(plane.c, 2)));
  return fcl::Vec3f(point[0] - k * plane.a, point[1] - k * plane.b,
                    point[2] - k * plane.c);
}

std::vector<Vec2D> computeSupportPolygon(
    const std::map<std::string, fcl::Vec3f>& contactPositions) {
  Plane h_plane(0, 0, 1, 0);  // horizontal plane
  std::vector<Vec2D> res;
  for (std::map<std::string, fcl::Vec3f>::const_iterator cit =
           contactPositions.begin();
       cit != contactPositions.end(); ++cit) {
    fcl::Vec3f proj(orthogonalProjection(cit->second, h_plane));
    Vec2D vertex_2D(proj[0], proj[1]);
    res.push_back(vertex_2D);
    // res.push_back(Vec2D(cit->second[0], cit->second[1])); // because the
    // plane is horizontal, we just have to remove the z (vertical) component
  }
  return res;
}

double computeAngle(const Vec2D& center, const Vec2D& end1, const Vec2D& end2) {
  if (end1 == end2) return 0.0;

  // build vector1
  Vec2D vector1(end1.x - center.x, end1.y - center.y);
  double norm1(std::sqrt(std::pow(vector1.x, 2) + std::pow(vector1.y, 2)));

  // build vector2
  Vec2D vector2(end2.x - center.x, end2.y - center.y);
  double norm2(std::sqrt(std::pow(vector2.x, 2) + std::pow(vector2.y, 2)));

  // find the angle between the two vectors using their scalar product
  double sp((vector1.x * vector2.x) + (vector1.y * vector2.y));
  return std::acos(sp / (norm1 * norm2));
}

void scanningProcess(const Vec2D& basePoint, std::vector<Vec2D>& subset,
                     double& angle, const Vec2D& currentPoint, bool higher,
                     bool direction) {
  // higher == true --> currentPoint is above basePoint
  // higher == false --> currentPoint is below basePoint
  // direction == true --> scan to the right
  // direction == false --> scan to the left
  int higher_val = higher ? 1 : -1;
  int direction_val = direction ? 1 : -1;

  if (subset.size() == 1) {
    // init
    angle =
        computeAngle(basePoint, Vec2D(basePoint.x + direction_val, basePoint.y),
                     currentPoint);
    subset.push_back(currentPoint);
  } else if ((higher_val * currentPoint.y) >= (higher_val * subset.back().y)) {
    double opening(computeAngle(
        subset.back(), Vec2D(subset.back().x + direction_val, subset.back().y),
        currentPoint));
    if (opening <= angle) {
      subset.push_back(currentPoint);
      angle = opening;
    } else {
      subset.pop_back();
      opening =
          computeAngle(subset.back(),
                       Vec2D(subset.back().x + direction_val, subset.back().y),
                       currentPoint);
      bool convex(false);
      if (subset.size() == 1) convex = true;
      while (!convex) {
        Vec2D base(subset[subset.size() - 2]);
        angle = computeAngle(base, Vec2D(base.x + direction_val, base.y),
                             subset.back());
        if (angle < opening) {
          subset.pop_back();
          opening = computeAngle(
              subset.back(),
              Vec2D(subset.back().x + direction_val, subset.back().y),
              currentPoint);
        } else
          convex = true;
        if (subset.size() == 1) convex = true;
      }
      subset.push_back(currentPoint);
      angle = opening;
    }
  }
}

std::vector<Vec2D> convexHull(std::vector<Vec2D> set) {
  std::vector<Vec2D> res_tmp, res;

  if (!set.empty()) {
    /* sort the input set by x increasing */
    std::vector<Vec2D> sortedSet;
    while (!set.empty()) {
      unsigned int index(0);
      double min(set[index].x);
      for (unsigned int i = 0; i < set.size(); ++i) {
        if (set[i].x < min) {
          min = set[i].x;
          index = i;
        }
      }
      sortedSet.push_back(set[index]);
      set.erase(set.begin() + index);
    }

    /* first scanning, to the right */
    Vec2D basePoint(sortedSet[0]);
    std::vector<Vec2D> tr_upper_set;
    tr_upper_set.push_back(basePoint);
    std::vector<Vec2D> tr_lower_set;
    tr_lower_set.push_back(basePoint);
    double upperAngle, lowerAngle;
    for (unsigned int i = 1; i < sortedSet.size(); ++i) {
      if (sortedSet[i].y >=
          basePoint.y)  // if the point is upper than basePoint
      {
        scanningProcess(basePoint, tr_upper_set, upperAngle, sortedSet[i], true,
                        true);
      } else  // if the point is lower than basePoint
      {
        scanningProcess(basePoint, tr_lower_set, lowerAngle, sortedSet[i],
                        false, true);
      }
    }

    /* second scanning, to the left */
    basePoint = sortedSet.back();
    std::vector<Vec2D> tl_upper_set;
    tl_upper_set.push_back(basePoint);
    std::vector<Vec2D> tl_lower_set;
    tl_lower_set.push_back(basePoint);
    for (int i = (int)sortedSet.size() - 2; i >= 0; --i) {
      if (sortedSet[i].y >=
          basePoint.y)  // if the point is upper than basePoint
      {
        scanningProcess(basePoint, tl_upper_set, upperAngle, sortedSet[i], true,
                        false);
      } else  // if the point is lower than basePoint
      {
        scanningProcess(basePoint, tl_lower_set, lowerAngle, sortedSet[i],
                        false, false);
      }
    }

    /* merge the four subsets without keeping the duplicates (subsets
     * boundaries, ...) */
    for (unsigned int i = 0; i < tr_upper_set.size(); ++i) {
      res_tmp.push_back(tr_upper_set[i]);
    }
    for (int i = (int)tl_upper_set.size() - 1; i >= 0; --i) {
      res_tmp.push_back(tl_upper_set[i]);
    }
    for (unsigned int i = 0; i < tl_lower_set.size(); ++i) {
      res_tmp.push_back(tl_lower_set[i]);
    }
    for (int i = (int)tr_lower_set.size() - 1; i >= 0; --i) {
      res_tmp.push_back(tr_lower_set[i]);
    }
    for (unsigned int i = 0; i < res_tmp.size(); ++i) {
      if (!contains(res, res_tmp[i])) res.push_back(res_tmp[i]);
    }
  }

  return res;
}

Vec2D weightedCentroidConvex2D(const std::vector<Vec2D>& convexPolygon) {
  if (convexPolygon.empty())
    throw std::string(
        "Impossible to find the weighted centroid of nothing (the specified "
        "convex polygon has no vertices)");

  Vec2D res;
  if (convexPolygon.size() == 1)
    res = convexPolygon[0];
  else if (convexPolygon.size() == 2) {
    double resX((convexPolygon[0].x + convexPolygon[1].x) / 2.0);
    double resY((convexPolygon[0].y + convexPolygon[1].y) / 2.0);
    res = Vec2D(resX, resY);
  } else {
    // get the longest edge and define the minimum admissible threshold for
    // counting a vertex as a single point
    double maxDist(
        Vec2D::euclideanDist(convexPolygon.back(), convexPolygon.front()));
    for (unsigned int i = 0; i < convexPolygon.size() - 1; ++i) {
      double dist(Vec2D::euclideanDist(convexPolygon[i], convexPolygon[i + 1]));
      if (dist > maxDist) maxDist = dist;
    }
    double threshold(maxDist / 10.0);

    // shift the list until starting from a lonely (to the rear) point
    std::vector<Vec2D> shifted(convexPolygon);
    while (Vec2D::euclideanDist(shifted.back(), shifted.front()) <= threshold) {
      shifted.push_back(shifted.front());
      shifted.erase(shifted.begin());
    }

    // look over the shifted set
    std::vector<Vec2D> finalSet, localSubset;
    bool subsetOngoing = false;
    shifted.push_back(shifted.front());

    for (unsigned int i = 0; i < shifted.size() - 1; ++i) {
      if (Vec2D::euclideanDist(shifted[i], shifted[i + 1]) > threshold) {
        if (!subsetOngoing)
          finalSet.push_back(shifted[i]);
        else {
          localSubset.push_back(shifted[i]);
          double moyX(0.0), moyY(0.0);
          for (unsigned int j = 0; j < localSubset.size(); ++j) {
            moyX += localSubset[j].x;
            moyY += localSubset[j].y;
          }
          moyX /= static_cast<double>(localSubset.size());
          moyY /= static_cast<double>(localSubset.size());
          finalSet.push_back(Vec2D(moyX, moyY));
          localSubset.clear();
          subsetOngoing = false;
        }
      } else {
        localSubset.push_back(shifted[i]);
        if (!subsetOngoing) subsetOngoing = true;
      }
    }

    double resX(0.0), resY(0.0);
    for (unsigned int i = 0; i < finalSet.size(); ++i) {
      resX += finalSet[i].x;
      resY += finalSet[i].y;
    }
    resX /= static_cast<double>(finalSet.size());
    resY /= static_cast<double>(finalSet.size());
    res = Vec2D(resX, resY);
  }
  return res;
}

void removeNonGroundContacts(std::map<std::string, fcl::Vec3f>& contacts,
                             double groundThreshold) {
  std::map<std::string, fcl::Vec3f>::const_iterator cit = contacts.begin();
  double minZ(cit->second[2]);
  for (; cit != contacts.end(); ++cit) {
    if (cit->second[2] < minZ) minZ = cit->second[2];
  }
  std::vector<std::string> outOfTheGround;
  for (cit = contacts.begin(); cit != contacts.end(); ++cit) {
    if (std::abs(cit->second[2] - minZ) > std::abs(groundThreshold))
      outOfTheGround.push_back(cit->first);
  }
  for (unsigned int i = 0; i < outOfTheGround.size(); ++i)
    contacts.erase(outOfTheGround[i]);
}

}  // namespace sampling
}  // namespace rbprm
}  // namespace hpp


Generated by: GCOVR (Version 4.2)

Line	Branch	Exec	Source
1			#include <hpp/rbprm/sampling/heuristic-tools.hh>
2
3			namespace hpp {
4			namespace rbprm {
5			namespace sampling {
6
7			HeuristicParam::HeuristicParam(const std::map<std::string, fcl::Vec3f>& cp,
8			const fcl::Vec3f& comPos,
9			const fcl::Vec3f& comSp,
10			const fcl::Vec3f& comAcc, const std::string& sln,
11			const fcl::Transform3f& tf)
12			: contactPositions_(cp),
13			comPosition_(comPos),
14			comSpeed_(comSp),
15			comAcceleration_(comAcc),
16			sampleLimbName_(sln),
17			tfWorldRoot_(tf) {}
18			HeuristicParam::HeuristicParam(const HeuristicParam& zhp)
19			: contactPositions_(zhp.contactPositions_),
20			comPosition_(zhp.comPosition_),
21			comSpeed_(zhp.comSpeed_),
22			comAcceleration_(zhp.comAcceleration_),
23			sampleLimbName_(zhp.sampleLimbName_),
24			tfWorldRoot_(zhp.tfWorldRoot_) {}
25			HeuristicParam& HeuristicParam::operator=(const HeuristicParam& zhp) {
26			if (this != &zhp) {
27			this->contactPositions_.clear();
28			this->contactPositions_.insert(zhp.contactPositions_.begin(),
29			zhp.contactPositions_.end());
30			this->comPosition_ = zhp.comPosition_;
31			this->comSpeed_ = zhp.comSpeed_;
32			this->comAcceleration_ = zhp.comAcceleration_;
33			this->sampleLimbName_ = zhp.sampleLimbName_;
34			this->tfWorldRoot_ = zhp.tfWorldRoot_;
35			}
36			return *this;
37			}
38
39			fcl::Vec3f transform(const fcl::Vec3f& p, const fcl::Vec3f& tr,
40			const fcl::Matrix3f& ro) {
41			fcl::Vec3f res(p[0] * ro(0, 0) + p[1] * ro(0, 1) + p[2] * ro(0, 2) + tr[0],
42			p[0] * ro(1, 0) + p[1] * ro(1, 1) + p[2] * ro(1, 2) + tr[1],
43			p[0] * ro(2, 0) + p[1] * ro(2, 1) + p[2] * ro(2, 2) + tr[2]);
44			return res;
45			}
46
47			Vec2D& Vec2D::operator=(const Vec2D& c) {
48			if (this != &c) {
49			this->x = c.x;
50			this->y = c.y;
51			}
52			return *this;
53			}
54			double Vec2D::operator[](int idx) const {
55			idx = idx % 2;
56			if (idx == 0)
57			return this->x;
58			else
59			return this->y;
60			}
61			double& Vec2D::operator[](int idx) {
62			idx = idx % 2;
63			if (idx == 0)
64			return this->x;
65			else
66			return this->y;
67			}
68			double Vec2D::euclideanDist(const Vec2D& v1, const Vec2D& v2) {
69			return std::sqrt(std::pow(v1.x - v2.x, 2) + std::pow(v1.y - v2.y, 2));
70			}
71			bool operator==(const Vec2D& v1, const Vec2D& v2) {
72			// return ((v1.x == v2.x) && (v1.y == v2.y));
73			return ((std::abs(v1.x - v2.x) < 1e-9) && (std::abs(v1.y - v2.y) < 1e-9));
74			}
75			bool operator!=(const Vec2D& v1, const Vec2D& v2) { return !(v1 == v2); }
76			std::ostream& operator<<(std::ostream& out, const Vec2D& v) {
77			out << "(" << v.x << ", " << v.y << ")";
78			return out;
79			}
80			Plane& Plane::operator=(const Plane& pe) {
81			if (this != &pe) {
82			this->a = pe.a;
83			this->b = pe.b;
84			this->c = pe.c;
85			this->d = pe.d;
86			}
87			return *this;
88			}
89
90			fcl::Vec3f orthogonalProjection(const fcl::Vec3f& point, const Plane& plane) {
91			double k(
92			((plane.a * point[0]) + (plane.b * point[1]) + (plane.c * point[2]) +
93			plane.d) /
94			(std::pow(plane.a, 2) + std::pow(plane.b, 2) + std::pow(plane.c, 2)));
95			return fcl::Vec3f(point[0] - k * plane.a, point[1] - k * plane.b,
96			point[2] - k * plane.c);
97			}
98
99			std::vector<Vec2D> computeSupportPolygon(
100			const std::map<std::string, fcl::Vec3f>& contactPositions) {
101			Plane h_plane(0, 0, 1, 0); // horizontal plane
102			std::vector<Vec2D> res;
103			for (std::map<std::string, fcl::Vec3f>::const_iterator cit =
104			contactPositions.begin();
105			cit != contactPositions.end(); ++cit) {
106			fcl::Vec3f proj(orthogonalProjection(cit->second, h_plane));
107			Vec2D vertex_2D(proj[0], proj[1]);
108			res.push_back(vertex_2D);
109			// res.push_back(Vec2D(cit->second[0], cit->second[1])); // because the
110			// plane is horizontal, we just have to remove the z (vertical) component
111			}
112			return res;
113			}
114
115			double computeAngle(const Vec2D& center, const Vec2D& end1, const Vec2D& end2) {
116			if (end1 == end2) return 0.0;
117
118			// build vector1
119			Vec2D vector1(end1.x - center.x, end1.y - center.y);
120			double norm1(std::sqrt(std::pow(vector1.x, 2) + std::pow(vector1.y, 2)));
121
122			// build vector2
123			Vec2D vector2(end2.x - center.x, end2.y - center.y);
124			double norm2(std::sqrt(std::pow(vector2.x, 2) + std::pow(vector2.y, 2)));
125
126			// find the angle between the two vectors using their scalar product
127			double sp((vector1.x * vector2.x) + (vector1.y * vector2.y));
128			return std::acos(sp / (norm1 * norm2));
129			}
130
131			void scanningProcess(const Vec2D& basePoint, std::vector<Vec2D>& subset,
132			double& angle, const Vec2D& currentPoint, bool higher,
133			bool direction) {
134			// higher == true --> currentPoint is above basePoint
135			// higher == false --> currentPoint is below basePoint
136			// direction == true --> scan to the right
137			// direction == false --> scan to the left
138			int higher_val = higher ? 1 : -1;
139			int direction_val = direction ? 1 : -1;
140
141			if (subset.size() == 1) {
142			// init
143			angle =
144			computeAngle(basePoint, Vec2D(basePoint.x + direction_val, basePoint.y),
145			currentPoint);
146			subset.push_back(currentPoint);
147			} else if ((higher_val * currentPoint.y) >= (higher_val * subset.back().y)) {
148			double opening(computeAngle(
149			subset.back(), Vec2D(subset.back().x + direction_val, subset.back().y),
150			currentPoint));
151			if (opening <= angle) {
152			subset.push_back(currentPoint);
153			angle = opening;
154			} else {
155			subset.pop_back();
156			opening =
157			computeAngle(subset.back(),
158			Vec2D(subset.back().x + direction_val, subset.back().y),
159			currentPoint);
160			bool convex(false);
161			if (subset.size() == 1) convex = true;
162			while (!convex) {
163			Vec2D base(subset[subset.size() - 2]);
164			angle = computeAngle(base, Vec2D(base.x + direction_val, base.y),
165			subset.back());
166			if (angle < opening) {
167			subset.pop_back();
168			opening = computeAngle(
169			subset.back(),
170			Vec2D(subset.back().x + direction_val, subset.back().y),
171			currentPoint);
172			} else
173			convex = true;
174			if (subset.size() == 1) convex = true;
175			}
176			subset.push_back(currentPoint);
177			angle = opening;
178			}
179			}
180			}
181
182			std::vector<Vec2D> convexHull(std::vector<Vec2D> set) {
183			std::vector<Vec2D> res_tmp, res;
184
185			if (!set.empty()) {
186			/* sort the input set by x increasing */
187			std::vector<Vec2D> sortedSet;
188			while (!set.empty()) {
189			unsigned int index(0);
190			double min(set[index].x);
191			for (unsigned int i = 0; i < set.size(); ++i) {
192			if (set[i].x < min) {
193			min = set[i].x;
194			index = i;
195			}
196			}
197			sortedSet.push_back(set[index]);
198			set.erase(set.begin() + index);
199			}
200
201			/* first scanning, to the right */
202			Vec2D basePoint(sortedSet[0]);
203			std::vector<Vec2D> tr_upper_set;
204			tr_upper_set.push_back(basePoint);
205			std::vector<Vec2D> tr_lower_set;
206			tr_lower_set.push_back(basePoint);
207			double upperAngle, lowerAngle;
208			for (unsigned int i = 1; i < sortedSet.size(); ++i) {
209			if (sortedSet[i].y >=
210			basePoint.y) // if the point is upper than basePoint
211			{
212			scanningProcess(basePoint, tr_upper_set, upperAngle, sortedSet[i], true,
213			true);
214			} else // if the point is lower than basePoint
215			{
216			scanningProcess(basePoint, tr_lower_set, lowerAngle, sortedSet[i],
217			false, true);
218			}
219			}
220
221			/* second scanning, to the left */
222			basePoint = sortedSet.back();
223			std::vector<Vec2D> tl_upper_set;
224			tl_upper_set.push_back(basePoint);
225			std::vector<Vec2D> tl_lower_set;
226			tl_lower_set.push_back(basePoint);
227			for (int i = (int)sortedSet.size() - 2; i >= 0; --i) {
228			if (sortedSet[i].y >=
229			basePoint.y) // if the point is upper than basePoint
230			{
231			scanningProcess(basePoint, tl_upper_set, upperAngle, sortedSet[i], true,
232			false);
233			} else // if the point is lower than basePoint
234			{
235			scanningProcess(basePoint, tl_lower_set, lowerAngle, sortedSet[i],
236			false, false);
237			}
238			}
239
240			/* merge the four subsets without keeping the duplicates (subsets
241			* boundaries, ...) */
242			for (unsigned int i = 0; i < tr_upper_set.size(); ++i) {
243			res_tmp.push_back(tr_upper_set[i]);
244			}
245			for (int i = (int)tl_upper_set.size() - 1; i >= 0; --i) {
246			res_tmp.push_back(tl_upper_set[i]);
247			}
248			for (unsigned int i = 0; i < tl_lower_set.size(); ++i) {
249			res_tmp.push_back(tl_lower_set[i]);
250			}
251			for (int i = (int)tr_lower_set.size() - 1; i >= 0; --i) {
252			res_tmp.push_back(tr_lower_set[i]);
253			}
254			for (unsigned int i = 0; i < res_tmp.size(); ++i) {
255			if (!contains(res, res_tmp[i])) res.push_back(res_tmp[i]);
256			}
257			}
258
259			return res;
260			}
261
262			Vec2D weightedCentroidConvex2D(const std::vector<Vec2D>& convexPolygon) {
263			if (convexPolygon.empty())
264			throw std::string(
265			"Impossible to find the weighted centroid of nothing (the specified "
266			"convex polygon has no vertices)");
267
268			Vec2D res;
269			if (convexPolygon.size() == 1)
270			res = convexPolygon[0];
271			else if (convexPolygon.size() == 2) {
272			double resX((convexPolygon[0].x + convexPolygon[1].x) / 2.0);
273			double resY((convexPolygon[0].y + convexPolygon[1].y) / 2.0);
274			res = Vec2D(resX, resY);
275			} else {
276			// get the longest edge and define the minimum admissible threshold for
277			// counting a vertex as a single point
278			double maxDist(
279			Vec2D::euclideanDist(convexPolygon.back(), convexPolygon.front()));
280			for (unsigned int i = 0; i < convexPolygon.size() - 1; ++i) {
281			double dist(Vec2D::euclideanDist(convexPolygon[i], convexPolygon[i + 1]));
282			if (dist > maxDist) maxDist = dist;
283			}
284			double threshold(maxDist / 10.0);
285
286			// shift the list until starting from a lonely (to the rear) point
287			std::vector<Vec2D> shifted(convexPolygon);
288			while (Vec2D::euclideanDist(shifted.back(), shifted.front()) <= threshold) {
289			shifted.push_back(shifted.front());
290			shifted.erase(shifted.begin());
291			}
292
293			// look over the shifted set
294			std::vector<Vec2D> finalSet, localSubset;
295			bool subsetOngoing = false;
296			shifted.push_back(shifted.front());
297
298			for (unsigned int i = 0; i < shifted.size() - 1; ++i) {
299			if (Vec2D::euclideanDist(shifted[i], shifted[i + 1]) > threshold) {
300			if (!subsetOngoing)
301			finalSet.push_back(shifted[i]);
302			else {
303			localSubset.push_back(shifted[i]);
304			double moyX(0.0), moyY(0.0);
305			for (unsigned int j = 0; j < localSubset.size(); ++j) {
306			moyX += localSubset[j].x;
307			moyY += localSubset[j].y;
308			}
309			moyX /= static_cast<double>(localSubset.size());
310			moyY /= static_cast<double>(localSubset.size());
311			finalSet.push_back(Vec2D(moyX, moyY));
312			localSubset.clear();
313			subsetOngoing = false;
314			}
315			} else {
316			localSubset.push_back(shifted[i]);
317			if (!subsetOngoing) subsetOngoing = true;
318			}
319			}
320
321			double resX(0.0), resY(0.0);
322			for (unsigned int i = 0; i < finalSet.size(); ++i) {
323			resX += finalSet[i].x;
324			resY += finalSet[i].y;
325			}
326			resX /= static_cast<double>(finalSet.size());
327			resY /= static_cast<double>(finalSet.size());
328			res = Vec2D(resX, resY);
329			}
330			return res;
331			}
332
333			void removeNonGroundContacts(std::map<std::string, fcl::Vec3f>& contacts,
334			double groundThreshold) {
335			std::map<std::string, fcl::Vec3f>::const_iterator cit = contacts.begin();
336			double minZ(cit->second[2]);
337			for (; cit != contacts.end(); ++cit) {
338			if (cit->second[2] < minZ) minZ = cit->second[2];
339			}
340			std::vector<std::string> outOfTheGround;
341			for (cit = contacts.begin(); cit != contacts.end(); ++cit) {
342			if (std::abs(cit->second[2] - minZ) > std::abs(groundThreshold))
343			outOfTheGround.push_back(cit->first);
344			}
345			for (unsigned int i = 0; i < outOfTheGround.size(); ++i)
346			contacts.erase(outOfTheGround[i]);
347			}
348
349			} // namespace sampling
350			} // namespace rbprm
351			} // namespace hpp