project-inl.h
Go to the documentation of this file.
1 /*
2  * Software License Agreement (BSD License)
3  *
4  * Copyright (c) 2011-2014, Willow Garage, Inc.
5  * Copyright (c) 2014-2016, Open Source Robotics Foundation
6  * All rights reserved.
7  *
8  * Redistribution and use in source and binary forms, with or without
9  * modification, are permitted provided that the following conditions
10  * are met:
11  *
12  * * Redistributions of source code must retain the above copyright
13  * notice, this list of conditions and the following disclaimer.
14  * * Redistributions in binary form must reproduce the above
15  * copyright notice, this list of conditions and the following
16  * disclaimer in the documentation and/or other materials provided
17  * with the distribution.
18  * * Neither the name of Open Source Robotics Foundation nor the names of its
19  * contributors may be used to endorse or promote products derived
20  * from this software without specific prior written permission.
21  *
22  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
23  * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
24  * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
25  * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
26  * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
27  * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
28  * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
29  * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
30  * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
31  * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
32  * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
33  * POSSIBILITY OF SUCH DAMAGE.
34  */
35 
38 #ifndef FCL_NARROWPHASE_DETAIL_PROJECT_INL_H
39 #define FCL_NARROWPHASE_DETAIL_PROJECT_INL_H
40 
41 #include "fcl/math/detail/project.h"
42 
43 namespace fcl
44 {
45 
46 namespace detail
47 {
48 
49 //==============================================================================
50 extern template
51 class FCL_EXPORT Project<double>;
52 
53 //==============================================================================
54 template <typename S>
55 typename Project<S>::ProjectResult Project<S>::projectLine(const Vector3<S>& a, const Vector3<S>& b, const Vector3<S>& p)
56 {
57  ProjectResult res;
58 
59  const Vector3<S> d = b - a;
60  const S l = d.squaredNorm();
61 
62  if(l > 0)
63  {
64  const S t = (p - a).dot(d);
65  res.parameterization[1] = (t >= l) ? 1 : ((t <= 0) ? 0 : (t / l));
66  res.parameterization[0] = 1 - res.parameterization[1];
67  if(t >= l) { res.sqr_distance = (p - b).squaredNorm(); res.encode = 2; /* 0x10 */ }
68  else if(t <= 0) { res.sqr_distance = (p - a).squaredNorm(); res.encode = 1; /* 0x01 */ }
69  else { res.sqr_distance = (a + d * res.parameterization[1] - p).squaredNorm(); res.encode = 3; /* 0x00 */ }
70  }
71 
72  return res;
73 }
74 
75 //==============================================================================
76 template <typename S>
77 typename Project<S>::ProjectResult Project<S>::projectTriangle(const Vector3<S>& a, const Vector3<S>& b, const Vector3<S>& c, const Vector3<S>& p)
78 {
79  ProjectResult res;
80 
81  static const size_t nexti[3] = {1, 2, 0};
82  const Vector3<S>* vt[] = {&a, &b, &c};
83  const Vector3<S> dl[] = {a - b, b - c, c - a};
84  const Vector3<S>& n = dl[0].cross(dl[1]);
85  const S l = n.squaredNorm();
86 
87  if(l > 0)
88  {
89  S mindist = -1;
90  for(size_t i = 0; i < 3; ++i)
91  {
92  if((*vt[i] - p).dot(dl[i].cross(n)) > 0) // origin is to the outside part of the triangle edge, then the optimal can only be on the edge
93  {
94  size_t j = nexti[i];
95  ProjectResult res_line = projectLine(*vt[i], *vt[j], p);
96 
97  if(mindist < 0 || res_line.sqr_distance < mindist)
98  {
99  mindist = res_line.sqr_distance;
100  res.encode = static_cast<size_t>(((res_line.encode&1)?1<<i:0) + ((res_line.encode&2)?1<<j:0));
101  res.parameterization[i] = res_line.parameterization[0];
102  res.parameterization[j] = res_line.parameterization[1];
103  res.parameterization[nexti[j]] = 0;
104  }
105  }
106  }
107 
108  if(mindist < 0) // the origin project is within the triangle
109  {
110  S d = (a - p).dot(n);
111  S s = sqrt(l);
112  Vector3<S> p_to_project = n * (d / l);
113  mindist = p_to_project.squaredNorm();
114  res.encode = 7; // m = 0x111
115  res.parameterization[0] = dl[1].cross(b - p -p_to_project).norm() / s;
116  res.parameterization[1] = dl[2].cross(c - p -p_to_project).norm() / s;
117  res.parameterization[2] = 1 - res.parameterization[0] - res.parameterization[1];
118  }
119 
120  res.sqr_distance = mindist;
121  }
122 
123  return res;
124 
125 }
126 
127 //==============================================================================
128 template <typename S>
129 typename Project<S>::ProjectResult Project<S>::projectTetrahedra(const Vector3<S>& a, const Vector3<S>& b, const Vector3<S>& c, const Vector3<S>& d, const Vector3<S>& p)
130 {
131  ProjectResult res;
132 
133  static const size_t nexti[] = {1, 2, 0};
134  const Vector3<S>* vt[] = {&a, &b, &c, &d};
135  const Vector3<S> dl[3] = {a-d, b-d, c-d};
136  S vl = triple(dl[0], dl[1], dl[2]);
137  bool ng = (vl * (a-p).dot((b-c).cross(a-b))) <= 0;
138  if(ng && std::abs(vl) > 0) // abs(vl) == 0, the tetrahedron is degenerated; if ng is false, then the last vertex in the tetrahedron does not grow toward the origin (in fact origin is on the other side of the abc face)
139  {
140  S mindist = -1;
141 
142  for(size_t i = 0; i < 3; ++i)
143  {
144  size_t j = nexti[i];
145  S s = vl * (d-p).dot(dl[i].cross(dl[j]));
146  if(s > 0) // the origin is to the outside part of a triangle face, then the optimal can only be on the triangle face
147  {
148  ProjectResult res_triangle = projectTriangle(*vt[i], *vt[j], d, p);
149  if(mindist < 0 || res_triangle.sqr_distance < mindist)
150  {
151  mindist = res_triangle.sqr_distance;
152  res.encode = static_cast<size_t>( (res_triangle.encode&1?1<<i:0) + (res_triangle.encode&2?1<<j:0) + (res_triangle.encode&4?8:0) );
153  res.parameterization[i] = res_triangle.parameterization[0];
154  res.parameterization[j] = res_triangle.parameterization[1];
155  res.parameterization[nexti[j]] = 0;
156  res.parameterization[3] = res_triangle.parameterization[2];
157  }
158  }
159  }
160 
161  if(mindist < 0)
162  {
163  mindist = 0;
164  res.encode = 15;
165  res.parameterization[0] = triple(c - p, b - p, d - p) / vl;
166  res.parameterization[1] = triple(a - p, c - p, d - p) / vl;
167  res.parameterization[2] = triple(b - p, a - p, d - p) / vl;
168  res.parameterization[3] = 1 - (res.parameterization[0] + res.parameterization[1] + res.parameterization[2]);
169  }
170 
171  res.sqr_distance = mindist;
172  }
173  else if(!ng)
174  {
175  res = projectTriangle(a, b, c, p);
176  res.parameterization[3] = 0;
177  }
178  return res;
179 }
180 
181 //==============================================================================
182 template <typename S>
183 typename Project<S>::ProjectResult Project<S>::projectLineOrigin(const Vector3<S>& a, const Vector3<S>& b)
184 {
185  ProjectResult res;
186 
187  const Vector3<S> d = b - a;
188  const S l = d.squaredNorm();
189 
190  if(l > 0)
191  {
192  const S t = - a.dot(d);
193  res.parameterization[1] = (t >= l) ? 1 : ((t <= 0) ? 0 : (t / l));
194  res.parameterization[0] = 1 - res.parameterization[1];
195  if(t >= l) { res.sqr_distance = b.squaredNorm(); res.encode = 2; /* 0x10 */ }
196  else if(t <= 0) { res.sqr_distance = a.squaredNorm(); res.encode = 1; /* 0x01 */ }
197  else { res.sqr_distance = (a + d * res.parameterization[1]).squaredNorm(); res.encode = 3; /* 0x00 */ }
198  }
199 
200  return res;
201 }
202 
203 //==============================================================================
204 template <typename S>
205 typename Project<S>::ProjectResult Project<S>::projectTriangleOrigin(const Vector3<S>& a, const Vector3<S>& b, const Vector3<S>& c)
206 {
207  ProjectResult res;
208 
209  static const size_t nexti[3] = {1, 2, 0};
210  const Vector3<S>* vt[] = {&a, &b, &c};
211  const Vector3<S> dl[] = {a - b, b - c, c - a};
212  const Vector3<S>& n = dl[0].cross(dl[1]);
213  const S l = n.squaredNorm();
214 
215  if(l > 0)
216  {
217  S mindist = -1;
218  for(size_t i = 0; i < 3; ++i)
219  {
220  if(vt[i]->dot(dl[i].cross(n)) > 0) // origin is to the outside part of the triangle edge, then the optimal can only be on the edge
221  {
222  size_t j = nexti[i];
223  ProjectResult res_line = projectLineOrigin(*vt[i], *vt[j]);
224 
225  if(mindist < 0 || res_line.sqr_distance < mindist)
226  {
227  mindist = res_line.sqr_distance;
228  res.encode = static_cast<size_t>(((res_line.encode&1)?1<<i:0) + ((res_line.encode&2)?1<<j:0));
229  res.parameterization[i] = res_line.parameterization[0];
230  res.parameterization[j] = res_line.parameterization[1];
231  res.parameterization[nexti[j]] = 0;
232  }
233  }
234  }
235 
236  if(mindist < 0) // the origin project is within the triangle
237  {
238  S d = a.dot(n);
239  S s = sqrt(l);
240  Vector3<S> o_to_project = n * (d / l);
241  mindist = o_to_project.squaredNorm();
242  res.encode = 7; // m = 0x111
243  res.parameterization[0] = dl[1].cross(b - o_to_project).norm() / s;
244  res.parameterization[1] = dl[2].cross(c - o_to_project).norm() / s;
245  res.parameterization[2] = 1 - res.parameterization[0] - res.parameterization[1];
246  }
247 
248  res.sqr_distance = mindist;
249  }
250 
251  return res;
252 
253 }
254 
255 //==============================================================================
256 template <typename S>
257 typename Project<S>::ProjectResult Project<S>::projectTetrahedraOrigin(const Vector3<S>& a, const Vector3<S>& b, const Vector3<S>& c, const Vector3<S>& d)
258 {
259  ProjectResult res;
260 
261  static const size_t nexti[] = {1, 2, 0};
262  const Vector3<S>* vt[] = {&a, &b, &c, &d};
263  const Vector3<S> dl[3] = {a-d, b-d, c-d};
264  S vl = triple(dl[0], dl[1], dl[2]);
265  bool ng = (vl * a.dot((b-c).cross(a-b))) <= 0;
266  if(ng && std::abs(vl) > 0) // abs(vl) == 0, the tetrahedron is degenerated; if ng is false, then the last vertex in the tetrahedron does not grow toward the origin (in fact origin is on the other side of the abc face)
267  {
268  S mindist = -1;
269 
270  for(size_t i = 0; i < 3; ++i)
271  {
272  size_t j = nexti[i];
273  S s = vl * d.dot(dl[i].cross(dl[j]));
274  if(s > 0) // the origin is to the outside part of a triangle face, then the optimal can only be on the triangle face
275  {
276  ProjectResult res_triangle = projectTriangleOrigin(*vt[i], *vt[j], d);
277  if(mindist < 0 || res_triangle.sqr_distance < mindist)
278  {
279  mindist = res_triangle.sqr_distance;
280  res.encode = static_cast<size_t>( (res_triangle.encode&1?1<<i:0) + (res_triangle.encode&2?1<<j:0) + (res_triangle.encode&4?8:0) );
281  res.parameterization[i] = res_triangle.parameterization[0];
282  res.parameterization[j] = res_triangle.parameterization[1];
283  res.parameterization[nexti[j]] = 0;
284  res.parameterization[3] = res_triangle.parameterization[2];
285  }
286  }
287  }
288 
289  if(mindist < 0)
290  {
291  mindist = 0;
292  res.encode = 15;
293  res.parameterization[0] = triple(c, b, d) / vl;
294  res.parameterization[1] = triple(a, c, d) / vl;
295  res.parameterization[2] = triple(b, a, d) / vl;
296  res.parameterization[3] = 1 - (res.parameterization[0] + res.parameterization[1] + res.parameterization[2]);
297  }
298 
299  res.sqr_distance = mindist;
300  }
301  else if(!ng)
302  {
303  res = projectTriangleOrigin(a, b, c);
304  res.parameterization[3] = 0;
305  }
306  return res;
307 }
308 
309 //==============================================================================
310 template <typename S>
311 Project<S>::ProjectResult::ProjectResult()
312  : parameterization{0.0, 0.0, 0.0, 0.0}, sqr_distance(-1), encode(0)
313 {
314  // Do nothing
315 }
316 
317 } // namespace detail
318 } // namespace fcl
319 
320 #endif
fcl::detail::Project::projectTetrahedra
static ProjectResult projectTetrahedra(const Vector3< S > &a, const Vector3< S > &b, const Vector3< S > &c, const Vector3< S > &d, const Vector3< S > &p)
Project point p onto tetrahedra a-b-c-d.
Definition: project-inl.h:129
fcl::detail::Project::ProjectResult::ProjectResult
ProjectResult()
Definition: project-inl.h:311
fcl::detail::Project::ProjectResult::encode
unsigned int encode
the code of the projection type
Definition: project.h:64
project.h
fcl::detail::Project< double >
template class FCL_EXPORT Project< double >
fcl::Vector3
Eigen::Matrix< S, 3, 1 > Vector3
Definition: types.h:70
fcl::detail::Project::projectTriangle
static ProjectResult projectTriangle(const Vector3< S > &a, const Vector3< S > &b, const Vector3< S > &c, const Vector3< S > &p)
Project point p onto triangle a-b-c.
Definition: project-inl.h:77
fcl::detail::Project::projectTriangleOrigin
static ProjectResult projectTriangleOrigin(const Vector3< S > &a, const Vector3< S > &b, const Vector3< S > &c)
Project origin (0) onto triangle a-b-c.
Definition: project-inl.h:205
fcl::detail::Project::projectLine
static ProjectResult projectLine(const Vector3< S > &a, const Vector3< S > &b, const Vector3< S > &p)
Project point p onto line a-b.
Definition: project-inl.h:55
fcl::detail::Project::ProjectResult
Definition: project.h:55
fcl::detail::Project::ProjectResult::parameterization
S parameterization[4]
Parameterization of the projected point (based on the simplex to be projected, use 2 or 3 or 4 of the...
Definition: project.h:58
fcl::detail::Project::ProjectResult::sqr_distance
S sqr_distance
square distance from the query point to the projected simplex
Definition: project.h:61
fcl::detail::Project::projectTetrahedraOrigin
static ProjectResult projectTetrahedraOrigin(const Vector3< S > &a, const Vector3< S > &b, const Vector3< S > &c, const Vector3< S > &d)
Project origin (0) onto tetrahedran a-b-c-d.
Definition: project-inl.h:257
fcl::triple
FCL_EXPORT Derived::RealScalar triple(const Eigen::MatrixBase< Derived > &x, const Eigen::MatrixBase< Derived > &y, const Eigen::MatrixBase< Derived > &z)
Definition: geometry-inl.h:431
fcl
Main namespace.
Definition: broadphase_bruteforce-inl.h:45
fcl::detail::Project::projectLineOrigin
static ProjectResult projectLineOrigin(const Vector3< S > &a, const Vector3< S > &b)
Project origin (0) onto line a-b.
Definition: project-inl.h:183

fcl
Author(s):
autogenerated on Tue Dec 5 2023 03:40:48