point.hpp
Go to the documentation of this file.
1 /*
2  * This file is part of ArmarX.
3  *
5  *
6  * ArmarX is free software; you can redistribute it and/or modify
7  * it under the terms of the GNU General Public License version 2 as
9  *
10  * ArmarX is distributed in the hope that it will be useful, but
11  * WITHOUT ANY WARRANTY; without even the implied warranty of
12  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
13  * GNU General Public License for more details.
14  *
15  * You should have received a copy of the GNU General Public License
16  * along with this program. If not, see <http://www.gnu.org/licenses/>.
17  *
18  * @package
19  * @author
20  * @date
22  * GNU General Public License
23  */
24 #pragma once
25
26 #include <algorithm>
27 #include <limits>
28 #include <stdlib.h>
29 #include <iostream>
30 #include <cmath>
31
32 template<class Point>
33 Point cross(const Point& x, const Point& y)
34 {
35  Point z;
36  z[0] = x[1] * y[2] - y[1] * x[2];
37  z[1] = x[2] * y[0] - y[2] * x[0];
38  z[2] = x[0] * y[1] - y[0] * x[1];
39  return z;
40 }
41
42 template<class Point>
43 Point sub(const Point& x, const Point& y)
44 {
45  Point z;
46  z[0] = x[0] - y[0];
47  z[1] = x[1] - y[1];
48  z[2] = x[2] - y[2];
49  return z;
50 }
51
52 template<class Point>
53 double dot(const Point& x, const Point& y)
54 {
55  return x[0] * y[0] + x[1] * y[1] + x[2] * y[2];
56 }
57
58 template<class Point>
59 bool collinear(const Point& p1, const Point& p2, const Point& p3)
60 {
61  //x1 * (y2 - y3) + x2 * (y3 - y1) + x3 * (y1 - y2)
62  return p1[0] * (p2[1] - p3[1]) + p2[0] * (p3[1] - p1[1]) + p3[0] * (p1[1] - p2[1]) == 0;
63 }
64
65 template<class Point>
66 bool coplanar(const Point& p1, const Point& p2, const Point& p3, const Point& p4)
67 {
68  Point a = sub(p3, p1);
69  Point b = sub(p2, p1);
70  Point c = sub(p4, p3);
71
72  Point d = cross(b, c);
73  return 0 == dot(a, d);
74 }
75
76 template<class Point>
77 bool inFront(const Point& p1, const Point& p2, const Point& p3, const Point& p4)
78 {
79  Point x = sub(p2, p1);
80  Point y = sub(p3, p1);
81  Point n = cross(x, y);
82
83  return dot(n, sub(p4, p1)) >= 0;
84 }
85
86
87 template<class Point>
88 double distance(const Point& a, const Point& b)
89 {
90  return sqrt(std::pow(a[0] - b[0], 2) + std::pow(a[1] - b[1], 2) + std::pow(a[2] - b[2], 2));
91 }
92
93 template<class Point>
94 double norm(const Point& a)
95 {
96  return sqrt(std::pow(a[0], 2) + std::pow(a[1], 2) + std::pow(a[2], 2));
97 }
98
99 template<class Point>
100 double angle(const Point& a, const Point& b, const Point& c)
101 {
102  Point v1 = sub(a, b);
103  Point v2 = sub(c, b);
104  double d1 = norm(v1);
105  double d2 = norm(v2);
106  double prod = dot(v1, v2);
107
108  return std::max(0.0, acos(prod / (d1 * d2)));
109 }
110
111 /*
112  * Returns the distance from the point p to the line defined by xy.
113  */
114 template<class Point>
115 double distanceToLine(const Point& x, const Point& y, const Point& p)
116 {
117  // d^2 = (|x - p|^2 * |y - x|^2 - ((x - p) * (y - x))^2) / |y - x|^2
118  Point px = sub(x, p);
119  Point xy = sub(y, x);
120  double nom = pow(norm(px), 2) * pow(norm(xy), 2) - pow(dot(px, xy), 2);
121  double d = nom / pow(norm(xy), 2);
122  return sqrt(d);
123 }
124
125 inline double clamp(double x, double a, double b)
126 {
127  return (x < a) ? a : ((x > b) ? b : x);
128 }
129
130 /*
131  * Returns the distance from the point p to the CCW triangle defined by xyz.
132  */
133 template<class Point>
134 double distanceToTriangle(const Point& x, const Point& y,
135  const Point& z, const Point& p)
136 {
137  Point diff = sub(x, p);
138  Point edge0 = sub(y, x);
139  Point edge1 = sub(z, x);
140
141  double a = dot(edge0, edge0);
142  double b = dot(edge0, edge1);
143  double c = dot(edge1, edge1);
144  double d = dot(edge0, diff);
145  double e = dot(edge1, diff);
146  double f = dot(diff, diff);
147
148  double det = a * c - b * b;
149  double s = b * e - c * d;
150  double t = b * d - a * e;
151
152  if (s + t < det)
153  {
154  if (s < 0.0)
155  {
156  if (t < 0.0)
157  {
158  if (d < 0.0)
159  {
160  s = clamp(-d / a, 0.0, 1.0);
161  t = 0.0;
162  }
163  else
164  {
165  s = 0.0;
166  t = clamp(-e / c, 0.0, 1.0);
167  }
168  }
169  else
170  {
171  s = 0.0;
172  t = clamp(-e / c, 0.0, 1.0);
173  }
174  }
175  else if (t < 0.0)
176  {
177  s = clamp(-d / a, 0.0, 1.0);
178  t = 0.0;
179  }
180  else
181  {
182  float invDet = 1.0 / det;
183  s *= invDet;
184  t *= invDet;
185  }
186  }
187  else
188  {
189  if (s < 0.0)
190  {
191  float tmp0 = b + d;
192  float tmp1 = c + e;
193
194  if (tmp1 > tmp0)
195  {
196  float numer = tmp1 - tmp0;
197  float denom = a - 2 * b + c;
198  s = clamp(numer / denom, 0.0, 1.0);
199  t = 1 - s;
200  }
201  else
202  {
203  t = clamp(-e / c, 0.0, 1.0);
204  s = 0.0;
205  }
206  }
207  else if (t < 0.0)
208  {
209  if (a + d > b + e)
210  {
211  float numer = c + e - b - d;
212  float denom = a - 2 * b + c;
213  s = clamp(numer / denom, 0.0, 1.0);
214  t = 1 - s;
215  }
216  else
217  {
218  s = clamp(-e / c, 0.0, 1.0);
219  t = 0.0;
220  }
221  }
222  else
223  {
224  float numer = c + e - b - d;
225  float denom = a - 2 * b + c;
226  s = clamp(numer / denom, 0.0, 1.0);
227  t = 1.0 - s;
228  }
229  }
230
231  double q = a * s * s + 2 * b * s * t + c * t * t + 2 * d * s + 2 * e * t + f;
232  return sqrt(q);
233 }
234
GfxTL::sqrt
VectorXD< D, T > sqrt(const VectorXD< D, T > &a)
Definition: VectorXD.h:662
distanceToLine
double distanceToLine(const Point &x, const Point &y, const Point &p)
Definition: point.hpp:115
c
constexpr T c
Definition: UnscentedKalmanFilterTest.cpp:43
distanceToTriangle
double distanceToTriangle(const Point &x, const Point &y, const Point &z, const Point &p)
Definition: point.hpp:134
clamp
double clamp(double x, double a, double b)
Definition: point.hpp:125
armarx::ctrlutil::a
double a(double t, double a0, double j)
Definition: CtrlUtil.h:45
inFront
bool inFront(const Point &p1, const Point &p2, const Point &p3, const Point &p4)
Definition: point.hpp:77
Point
Definition: PointCloud.h:21
cross
Point cross(const Point &x, const Point &y)
Definition: point.hpp:33
max
T max(T t1, T t2)
Definition: gdiam.h:48
q
#define q
sub
Point sub(const Point &x, const Point &y)
Definition: point.hpp:43
collinear
bool collinear(const Point &p1, const Point &p2, const Point &p3)
Definition: point.hpp:59
dot
double dot(const Point &x, const Point &y)
Definition: point.hpp:53
angle
double angle(const Point &a, const Point &b, const Point &c)
Definition: point.hpp:100
distance
double distance(const Point &a, const Point &b)
Definition: point.hpp:88
armarx::ctrlutil::s
double s(double t, double s0, double v0, double a0, double j)
Definition: CtrlUtil.h:33
coplanar
bool coplanar(const Point &p1, const Point &p2, const Point &p3, const Point &p4)
Definition: point.hpp:66
norm
double norm(const Point &a)
Definition: point.hpp:94