1 |
|
|
|
2 |
|
|
#include "hpp/rbprm/stability/support.hh" |
3 |
|
|
|
4 |
|
|
#include <math.h> |
5 |
|
|
|
6 |
|
|
using namespace Eigen; |
7 |
|
|
|
8 |
|
|
using namespace std; |
9 |
|
|
|
10 |
|
|
namespace { |
11 |
|
|
const double Epsilon = 0.001; |
12 |
|
|
|
13 |
|
|
typedef std::vector<Vector3d, Eigen::aligned_allocator<Vector3d> > T_Point; |
14 |
|
|
} // namespace |
15 |
|
|
|
16 |
|
|
// isLeft(): tests if a point is Left|On|Right of an infinite line. |
17 |
|
|
// Input: three points P0, P1, and P2 |
18 |
|
|
// Return: >0 for P2 left of the line through P0 and P1 |
19 |
|
|
// =0 for P2 on the line |
20 |
|
|
// <0 for P2 right of the line |
21 |
|
|
// See: the January 201 Algorithm "Area of 2D and 3D Triangles and Polygons" |
22 |
|
|
double isLeft(const Vector3d& P0, const Vector3d& P1, const Vector3d& P2) { |
23 |
|
|
return ((P1.x() - P0.x()) * (P2.y() - P0.y()) - |
24 |
|
|
(P2.x() - P0.x()) * (P1.y() - P0.y())); |
25 |
|
|
} |
26 |
|
|
|
27 |
|
|
const Vector3d& LeftMost(const T_Point& points) { |
28 |
|
|
unsigned int i = 0; |
29 |
|
|
for (unsigned int j = 1; j < points.size(); ++j) { |
30 |
|
|
if (points[j].x() < points[i].x()) { |
31 |
|
|
i = j; |
32 |
|
|
} |
33 |
|
|
} |
34 |
|
|
return points[i]; |
35 |
|
|
} |
36 |
|
|
|
37 |
|
|
#include <iostream> |
38 |
|
|
// http://en.wikipedia.org/wiki/Gift_wrapping_algorithm |
39 |
|
|
T_Point ConvexHull(const T_Point& points) { |
40 |
|
|
T_Point res; |
41 |
|
|
Vector3d pointOnHull = LeftMost(points); |
42 |
|
|
Vector3d endPoint; |
43 |
|
|
int i = 0; |
44 |
|
|
do { |
45 |
|
|
++i; |
46 |
|
|
Vector3d pi = pointOnHull; |
47 |
|
|
endPoint = points[0]; |
48 |
|
|
for (unsigned int j = 1; j < points.size(); ++j) { |
49 |
|
|
if ((endPoint == pointOnHull) || (isLeft(pi, endPoint, points[j]) > 0)) { |
50 |
|
|
endPoint = points[j]; |
51 |
|
|
} |
52 |
|
|
} |
53 |
|
|
res.push_back(pi); |
54 |
|
|
pointOnHull = endPoint; |
55 |
|
|
} while (endPoint != res[0]); |
56 |
|
|
res.push_back(endPoint); |
57 |
|
|
return res; |
58 |
|
|
} |
59 |
|
|
|
60 |
|
|
double DistancePointSegment(const Vector3d& pt, const Vector2d& A, |
61 |
|
|
const Vector2d& B) { |
62 |
|
|
Vector2d point(pt.x(), pt.y()); |
63 |
|
|
// first coefficient director |
64 |
|
|
double a = (A.y() - B.y()) / (A.x() - B.x()); |
65 |
|
|
double b = B.y() - (a * B.x()); |
66 |
|
|
|
67 |
|
|
// line that goes through and orthogonal to segment |
68 |
|
|
double c = -(1 / a); |
69 |
|
|
double d = point.y() - (point.x() * c); |
70 |
|
|
|
71 |
|
|
// now computing intersection point |
72 |
|
|
double Xint = (d - b) / (c - a); |
73 |
|
|
double Yint = a * Xint + b; |
74 |
|
|
|
75 |
|
|
// Xint on the segment if Aint and AB colinear and norm(Aint) < norm(AB) |
76 |
|
|
Vector2d inter(Xint, Yint); |
77 |
|
|
if ((inter - A).dot(B - A) > 0 && (inter - A).norm() < (B - A).norm()) { |
78 |
|
|
Vector2d newPosition(Xint, Yint); |
79 |
|
|
return (point - newPosition).norm(); |
80 |
|
|
} else // take closest extremity |
81 |
|
|
{ |
82 |
|
|
return min((point - A).norm(), (point - B).norm()); |
83 |
|
|
} |
84 |
|
|
} |
85 |
|
|
|
86 |
|
|
// source |
87 |
|
|
// http://softsurfer.com/Archive/algorithm_0103/algorithm_0103.htm#wn_PinPolygon() |
88 |
|
|
// wn_PnPoly(): winding double test for a Vector3 in a polygon |
89 |
|
|
// Input: P = a Vector3, |
90 |
|
|
// points_ = Point vector of size n+1 with points_[n]=points_[0] |
91 |
|
|
// Return: wn = the winding double (=0 only if P is outside points_[]) |
92 |
|
|
bool InPolygon(const T_Point& points, const Vector3d& P) { |
93 |
|
|
int wn = 0; // the winding double counter |
94 |
|
|
int n = (int)(points.size()) - 1; |
95 |
|
|
if (n < 0) { |
96 |
|
|
return false; |
97 |
|
|
} else if (n == 0) { |
98 |
|
|
Vector2d p(P.x(), P.y()); |
99 |
|
|
Vector2d p0(points[0].x(), points[0].y()); |
100 |
|
|
return ((p - p0).norm() < Epsilon); |
101 |
|
|
} else if (n == 1) { |
102 |
|
|
Vector2d p0(points[0].x(), points[0].y()); |
103 |
|
|
Vector2d p1(points[1].x(), points[1].y()); |
104 |
|
|
return (DistancePointSegment(P, p0, p1) < Epsilon); |
105 |
|
|
} |
106 |
|
|
|
107 |
|
|
// loop through all edges of the polygon |
108 |
|
|
for (int i = 0; i < n; i++) { // edge from points_[i] to points_[i+1] |
109 |
|
|
if (points[i].y() <= P.y()) { // start y <= P.y() |
110 |
|
|
if (points[i + 1].y() > P.y()) // an upward crossing |
111 |
|
|
{ |
112 |
|
|
if (isLeft(points[i], points[i + 1], P) > 0) // P left of edge |
113 |
|
|
++wn; // have a valid up intersect |
114 |
|
|
} |
115 |
|
|
} else { // start y > P.y() (no test needed) |
116 |
|
|
if (points[i + 1].y() <= P.y()) // a downward crossing |
117 |
|
|
{ |
118 |
|
|
if (isLeft(points[i], points[i + 1], P) < 0) // P right of edge |
119 |
|
|
--wn; // have a valid down intersect |
120 |
|
|
} |
121 |
|
|
} |
122 |
|
|
} |
123 |
|
|
return !(wn == 0); |
124 |
|
|
} |
125 |
|
|
|
126 |
|
|
using namespace hpp::rbprm::stability; |
127 |
|
|
|
128 |
|
|
bool hpp::rbprm::stability::Contains( |
129 |
|
|
const Eigen::Matrix<double, Eigen::Dynamic, 1> support, |
130 |
|
|
const Eigen::Vector3d& aPoint, const Eigen::VectorXd& xs, |
131 |
|
|
const Eigen::VectorXd& ys) { |
132 |
|
|
T_Point points; |
133 |
|
|
int nbPoints = (int)support.rows() / 3; |
134 |
|
|
if (nbPoints < 1) return false; |
135 |
|
|
for (int i = 0; i < nbPoints; ++i) { |
136 |
|
|
Eigen::Vector3d point = support.segment<3>(i * 3); |
137 |
|
|
points.push_back(point + Eigen::Vector3d(xs[i], ys[i], 0)); |
138 |
|
|
points.push_back(point + Eigen::Vector3d(-xs[i], ys[i], 0)); |
139 |
|
|
points.push_back(point + Eigen::Vector3d(-xs[i], -ys[i], 0)); |
140 |
|
|
points.push_back(point + Eigen::Vector3d(xs[i], -ys[i], 0)); |
141 |
|
|
} |
142 |
|
|
points = ConvexHull(points); |
143 |
|
|
return InPolygon(points, aPoint); |
144 |
|
|
} |