DistFuncs.cpp
Go to the documentation of this file.
1 #include "DistFuncs.h"
2 #if 1
3 std::ostream &operator<<(std::ostream &ost, const Point& p)
4 {
5  ost << "(" << p.x << ", " << p.y << ", " << p.z << ")";
6  return ost;
7 }
8 
9 #endif
10 
24 inline float SegSegDist(const Point& u0, const Point& u,
25  const Point& v0, const Point& v,
26  Point& cp0, Point& cp1)
27 {
28  Point w = u0 - v0;
29  float a = u|u; // always >= 0
30  float b = u|v;
31  float c = v|v; // always >= 0
32  float d = u|w;
33  float e = v|w;
34  float D = a*c - b*b; // always >= 0
35  float sc, sN, sD = D; // sc = sN / sD, default sD = D >= 0
36  float tc, tN, tD = D; // tc = tN / tD, default tD = D >= 0
37 
38  // compute the line parameters of the two closest points
39 #define EPS 1e-8
40  if (D < EPS) { // the lines are almost parallel
41  sN = 0.0; // force using point P0 on segment S1
42  sD = 1.0; // to prevent possible division by 0.0 later
43  tN = e;
44  tD = c;
45  }
46  else { // get the closest points on the infinite lines
47  sN = (b*e - c*d);
48  tN = (a*e - b*d);
49  if (sN < 0.0) { // sc < 0 => the s=0 edge is visible
50  sN = 0.0;
51  tN = e;
52  tD = c;
53  }
54  else if (sN > sD) { // sc > 1 => the s=1 edge is visible
55  sN = sD;
56  tN = e + b;
57  tD = c;
58  }
59  }
60 
61  if (tN < 0.0) { // tc < 0 => the t=0 edge is visible
62  tN = 0.0;
63  // recompute sc for this edge
64  if (-d < 0.0)
65  sN = 0.0;
66  else if (-d > a)
67  sN = sD;
68  else {
69  sN = -d;
70  sD = a;
71  }
72  }
73  else if (tN > tD) { // tc > 1 => the t=1 edge is visible
74  tN = tD;
75  // recompute sc for this edge
76  if ((-d + b) < 0.0)
77  sN = 0;
78  else if ((-d + b) > a)
79  sN = sD;
80  else {
81  sN = (-d + b);
82  sD = a;
83  }
84  }
85  // finally do the division to get sc and tc
86  sc = (fabsf(sN) < EPS ? 0.0f : sN / sD);
87  tc = (fabsf(tN) < EPS ? 0.0f : tN / tD);
88 
89  cp0 = u0 + sc * u;
90  cp1 = v0 + tc * v;
91 
92  // get the difference of the two closest points
93  Point dP = cp0 - cp1;
94 
95  return dP.Magnitude(); // return the closest distance
96 }
97 
105 inline float PointPlaneDist(const Point& P, const Point& pointOnPlane, const Point& n,
106  Point& cp)
107 {
108  Point v = P - pointOnPlane;
109  float l = v|n;
110  cp = P-l*n;
111  return l;
112 }
113 
114 // see DistFuncs.h
115 float PointSegDist(const Point& P, const Point& u0, const Point& u1)
116 {
117  Point v = P - u0;
118  Point dir = u1 - u0;
119  float l = dir.Magnitude();
120  dir /= l;
121 
122  float x = v|dir;
123  if (x < 0){
124  return v.Magnitude();
125  }else if (x > l){
126  Point dv = P - u1;
127  return dv.Magnitude();
128  }else{
129  return sqrtf(v.SquareMagnitude() - x*x);
130  }
131 }
132 
141 inline bool PointFaceAppTest(const Point& P, const Point** vertices,
142  const Point* edges, const Point& n)
143 {
144  Point v, outer;
145  for (unsigned int i=0; i<3; i++){
146  v = P - *vertices[i];
147  outer = edges[i]^n;
148  if ((v|outer)>0) return false;
149  }
150  return true;
151 }
152 
153 // see DistFuncs.h
154 float TriTriDist(const Point& U0, const Point& U1, const Point& U2,
155  const Point& V0, const Point& V1, const Point& V2,
156  Point& cp0, Point& cp1)
157 {
158  const Point* uvertices[] = {&U0, &U1, &U2};
159  const Point* vvertices[] = {&V0, &V1, &V2};
160  float min_d, d;
161  Point p0, p1, n, vec;
162  bool overlap = true;
163 
164  // edges
165  Point uedges[3], vedges[3];
166  for (unsigned int i=0; i<3; i++){
167  uedges[i] = *uvertices[(i+1)%3] - *uvertices[i];
168  vedges[i] = *vvertices[(i+1)%3] - *vvertices[i];
169  }
170 
171  // set initial values
172  cp0 = U0;
173  cp1 = V0;
174  min_d = (cp0-V0).Magnitude();
175 
176  // vertex-vertex, vertex-edge, edge-edge
177  for (unsigned int i=0; i<3; i++){
178  for (unsigned int j=0; j<3; j++){
179  d = SegSegDist(*uvertices[i], uedges[i],
180  *vvertices[j], vedges[j],
181  p0, p1);
182  n = p0 - p1;
183  if (d <= min_d){
184  min_d = d;
185  cp0 = p0;
186  cp1 = p1;
187  // check overlap
188  vec = *uvertices[(i+2)%3] - cp0;
189  float u = (vec|n)/min_d;
190  vec = *vvertices[(j+2)%3] - cp1;
191  float v = (vec|n)/min_d;
192  // n directs from v -> u
193  if (u>=0 && v<=0) return min_d;
194 
195  if (u > 0) u = 0;
196  if (v < 0) v = 0;
197  if ((n.Magnitude() + u - v) > 0){
198  overlap = false;
199  }
200  }
201  }
202  }
203 
204  Point un = uedges[0]^uedges[1];
205  un.Normalize();
206  Point vn = vedges[0]^vedges[1];
207  vn.Normalize();
208 
209  // vertex-face, edge-face, face-face
210  // plane-triangle overlap test
211  float ds[3];
212  int rank;
213  // u and plane including v
214  rank = -1;
215  for (int i=0; i<3; i++){
216  vec = *uvertices[i] - *vvertices[0];
217  ds[i] = vec|vn;
218  }
219  if (ds[0] > 0 && ds[1] > 0 && ds[2] > 0){
220  // find rank of the nearest vertex to the plane
221  rank = ds[0] > ds[1] ? 1 : 0;
222  rank = ds[rank] > ds[2] ? 2 : rank;
223  }else if(ds[0] < 0 && ds[1] < 0 && ds[2] < 0){
224  // find rank of the nearest vertex to the plane
225  rank = ds[0] > ds[1] ? 0 : 1;
226  rank = ds[rank] > ds[2] ? rank : 2;
227  }
228  if (rank >=0){
229  overlap = false;
230  if (PointFaceAppTest(*uvertices[rank], vvertices, vedges, vn)){
231  min_d = fabsf(PointPlaneDist(*uvertices[rank], *vvertices[0], vn, p1));
232  cp0 = *uvertices[rank];
233  cp1 = p1;
234  return min_d;
235  }
236  }
237 
238  // v and plane including u
239  rank = -1;
240  for (int i=0; i<3; i++){
241  vec = *vvertices[i] - *uvertices[0];
242  ds[i] = vec|un;
243  }
244  if (ds[0] > 0 && ds[1] > 0 && ds[2] > 0){
245  // find rank of the nearest vertex to the plane
246  rank = ds[0] > ds[1] ? 1 : 0;
247  rank = ds[rank] > ds[2] ? 2 : rank;
248  }else if(ds[0] < 0 && ds[1] < 0 && ds[2] < 0){
249  // find rank of the nearest vertex to the plane
250  rank = ds[0] > ds[1] ? 0 : 1;
251  rank = ds[rank] > ds[2] ? rank : 2;
252  }
253  if (rank >= 0){
254  overlap = false;
255  if (PointFaceAppTest(*vvertices[rank], uvertices, uedges, un)){
256  min_d = fabsf(PointPlaneDist(*vvertices[rank], *uvertices[0], un, p0));
257  cp0 = p0;
258  cp1 = *vvertices[rank];
259  return min_d;
260  }
261  }
262 
263  if (overlap){
264  return 0;
265  }else{
266  return min_d;
267  }
268 }
269 
270 // see DistFuncs.h
271 float SegSegDist(const Point& u0, const Point& u1,
272  const Point& v0, const Point& v1)
273 {
274  Point cp0, cp1;
275  return SegSegDist(u0, u1-u0, v0, v1-v0, cp0, cp1);
276 }
277 
278 #if 0
279 
280 int main()
281 {
282  Point p0(0,0,0), p1(2,0,0), p2(1, 1, -1), p3(1, 1, 1);
283  Point cp1, cp2, n;
284  float d;
285 
286  d= SegSegDist(p0, p1, p2, p3, cp1, cp2);
287  std::cout << "test1 : d = " << d << ", cp1 = " << cp1
288  << ", cp2 = " << cp2 << std::endl;
289 
290  Point p4(0,1,0), p5(2,1,0);
291  d= SegSegDist(p0, p1, p4, p5, cp1, cp2);
292  std::cout << "test2 : d = " << d << ", cp1 = " << cp1
293  << ", cp2 = " << cp2 << std::endl;
294 
295  d= SegSegDist(p0, p1, p0, p1, cp1, cp2);
296  std::cout << "test3 : d = " << d << ", cp1 = " << cp1
297  << ", cp2 = " << cp2 << std::endl;
298 
299  Point p6(0, 2, 0), p7(-2, 1, 0);
300  d = TriTriDist(p0, p1, p5, p4, p6, p7, cp1, cp2);
301  std::cout << "test4 : d = " << d << ", cp1 = " << cp1 << ", cp2 = " << cp2
302  << std::endl;
303 
304  Point p8(3, -1, 1), p9(3, 2, 1), p10(-3, -1, 1);
305  d = TriTriDist(p0, p1, p5, p8, p9, p10, cp1, cp2);
306  std::cout << "test5 : d = " << d << ", cp1 = " << cp1 << ", cp2 = " << cp2
307  << std::endl;
308 
309  d = TriTriDist(p0, p1, p2, p8, p9, p10, cp1, cp2);
310  std::cout << "test6 : d = " << d << ", cp1 = " << cp1 << ", cp2 = " << cp2
311  << std::endl;
312  std::cout << "answer: d = 1, cp1 = (0, 0, 0), cp2 = (0, 0, 1)"
313  << std::endl;
314 
315  d = TriTriDist(p2, p0, p1, p8, p9, p10, cp1, cp2);
316  std::cout << "test7 : d = " << d << ", cp1 = " << cp1 << ", cp2 = " << cp2
317  << std::endl;
318  std::cout << "answer: d = 1, cp1 = (0, 0, 0), cp2 = (0, 0, 1)"
319  << std::endl;
320 
321  d = TriTriDist(p1, p2, p0, p8, p9, p10, cp1, cp2);
322  std::cout << "test8 : d = " << d << ", cp1 = " << cp1 << ", cp2 = " << cp2
323  << std::endl;
324  std::cout << "answer: d = 1, cp1 = (2, 0, 0), cp2 = (2, 0, 1)"
325  << std::endl;
326 
327  {
328  Point pp0(-2, 2, 0), pp1(-2, -2, 0), pp2(5, -2, 0),
329  pp3(-0.1, 0.4, -0.1), pp4(-0.1, -0.4, -0.1), pp5(-0.1, -0.4, 0.1);
330  d = TriTriDist(pp0, pp1, pp2, pp3, pp4, pp5, cp1, cp2);
331  std::cout << "test9 : d = " << d << ", cp1 = " << cp1 << ", cp2 = "
332  << cp2 << std::endl;
333  std::cout << "answer: d = 0.0" << std::endl;
334  }
335 
336  {
337  Point pp0(-2, 2, 0), pp1(-2, -2, 0), pp2(5, -2, 0),
338  pp3(-0.1, 0.4, -0.1), pp4(-0.1, -0.4, -0.1), pp5(-0.1, -0.4, 0.1);
339  d = TriTriDist(pp0, pp1, pp2, pp3, pp4, pp5, cp1, cp2);
340  std::cout << "test9 : d = " << d << ", cp1 = " << cp1 << ", cp2 = "
341  << cp2 << std::endl;
342  std::cout << "answer: d = 0.0" << std::endl;
343  }
344 
345  {
346  Point p0(0.1, 0.4, 0), p1(-0.1, 0.4, 0), p2(0.1, -0.4, 0);
347  Point p3(0.05, 0.45, -0.15), p4(0.05, 0.45, 0.05), p5(0.05, -0.35, -0.15);
348  d = TriTriDist(p0, p1, p2, p3, p4, p5, cp1, cp2);
349  std::cout << "test10 : d = " << d << ", cp1 = " << cp1 << ", cp2 = "
350  << cp2 << std::endl;
351  std::cout << "answer: d = 0.0" << std::endl;
352  }
353  return 0;
354 }
355 #endif
bool PointFaceAppTest(const Point &P, const Point **vertices, const Point *edges, const Point &n)
check whether a point is in Voroni region of a face
Definition: DistFuncs.cpp:141
int c
Definition: autoplay.py:16
* x
Definition: IceUtils.h:98
int main(int argc, argv)
Definition: ansi2knr.c:320
float SegSegDist(const Point &u0, const Point &u, const Point &v0, const Point &v, Point &cp0, Point &cp1)
compute the minimum distance and the closest points between two line segments
Definition: DistFuncs.cpp:24
inline_ float Magnitude() const
Computes magnitude.
Definition: OPC_IceHook.h:220
#define EPS
w
png_uint_32 i
Definition: png.h:2735
long b
Definition: jpegint.h:371
def j(str, encoding="cp932")
string a
std::ostream & operator<<(std::ostream &ost, const Point &p)
Definition: DistFuncs.cpp:3
float PointPlaneDist(const Point &P, const Point &pointOnPlane, const Point &n, Point &cp)
compute signed distance between a point and a plane
Definition: DistFuncs.cpp:105
float PointSegDist(const Point &P, const Point &u0, const Point &u1)
compute distance between a point and a line segment
Definition: DistFuncs.cpp:115
float TriTriDist(const Point &U0, const Point &U1, const Point &U2, const Point &V0, const Point &V1, const Point &V2, Point &cp0, Point &cp1)
compute the minimum distance and the closest points between two triangles
Definition: DistFuncs.cpp:154
inline_ Point & Normalize()
Normalizes the vector.
Definition: OPC_IceHook.h:270
inline_ float SquareMagnitude() const
Computes square magnitude.
Definition: OPC_IceHook.h:218


openhrp3
Author(s): AIST, General Robotix Inc., Nakamura Lab of Dept. of Mechano Informatics at University of Tokyo
autogenerated on Sat Apr 13 2019 02:14:21