PeriDyno 1.0.0
Loading...
Searching...
No Matches
Distance3D.inl
Go to the documentation of this file.
1namespace dyno
2{
3 template<typename Real, typename Coord, DeviceType deviceType, typename IndexType>
4 DYN_FUNC bool calculateSignedDistance2TriangleSet(ProjectedPoint3D<Real>& p3d, Coord point, Array<Coord, deviceType>& vertices, Array<TopologyModule::Triangle, deviceType>& indices, List<IndexType>& list, Real dHat)
5 {
6 //This value is important, if the result is wrong, first check eps
7 Real eps = 16 * EPSILON;
8
9 p3d.signed_distance = REAL_MAX;
10
11 bool validate = false;
12
13 TPoint3D<Real> pi = TPoint3D<Real>(point);
14 for (uint j = 0; j < list.size(); j++)
15 {
16 TopologyModule::Triangle index = indices[list[j]];
17 TTriangle3D<Real> tj(vertices[index[0]], vertices[index[1]], vertices[index[2]]);
18
19 Coord nj = tj.normal();
20
21 TPoint3D<Real> proj = pi.project(tj);
22 Real d = (pi.origin - proj.origin).norm();
23
24 //If the new triangle is closer, use the new triangle to update p3d
25 if (glm::abs(d) < glm::abs(p3d.signed_distance) - eps)
26 {
27 p3d.signed_distance = d;
28 p3d.point = proj.origin;
29 p3d.id = list[j];
30
31 validate = true;
32 }
33 //Otherwise, if the two triangles have relatively equal distances to the point,
34 // further compare the distance from the point's projection to the corresponding triangle
35 else if (glm::abs(d) < glm::abs(p3d.signed_distance) + eps)
36 {
37 typename TTriangle3D<Real>::Param tParam;
38
39 TopologyModule::Triangle index_min = indices[p3d.id];
40 TTriangle3D<Real> t_min(vertices[index_min[0]], vertices[index_min[1]], vertices[index_min[2]]);
41
42 TPlane3D<Real> plane_j(tj.v[0], tj.normal());
43 TPlane3D<Real> plane_min(t_min.v[0], t_min.normal());
44
45 TPoint3D<Real> pn_j = pi.project(plane_j);
46 TPoint3D<Real> pn_min = pi.project(plane_min);
47
48 Real d_j = glm::abs(pn_j.distance(tj));
49 Real d_min_j = glm::abs(pn_min.distance(t_min));
50
51 if (d_j < d_min_j)
52 {
53 p3d.signed_distance = d;
54 p3d.point = proj.origin;
55 p3d.id = list[j];
56
57 validate = true;
58 }
59 }
60 }
61
62 //Calculate the normal
63 if (validate)
64 {
65 TopologyModule::Triangle index = indices[p3d.id];
66 TTriangle3D<Real> tri(vertices[index[0]], vertices[index[1]], vertices[index[2]]);
67
68 Coord n = pi.origin - p3d.point;
69 n = n.norm() > EPSILON ? n.normalize() : tri.normal();
70
71 //Check whether the point is located inside or not
72 p3d.signed_distance = n.dot(tri.normal()) > 0 ? p3d.signed_distance : -p3d.signed_distance;
73 p3d.signed_distance -= dHat;
74 p3d.normal = n;
75 }
76
77 return validate;
78 }
79
80
81
82 template<typename Real, typename Coord, DeviceType deviceType, typename IndexType>
83 DYN_FUNC bool calculateDistance2TriangleSet(ProjectedPoint3D<Real>& p3d, Coord point, Array<Coord, deviceType>& vertices, Array<TopologyModule::Triangle, deviceType>& indices, List<IndexType>& list, Real dHat)
84 {
85 p3d.signed_distance = REAL_MAX;
86
87 bool validate = false;
88
89 TPoint3D<Real> pi = TPoint3D<Real>(point);
90 for (uint j = 0; j < list.size(); j++)
91 {
92 TopologyModule::Triangle index = indices[list[j]];
93 TTriangle3D<Real> tj(vertices[index[0]], vertices[index[1]], vertices[index[2]]);
94
95 Coord nj = tj.normal();
96
97 TPoint3D<Real> proj = pi.project(tj);
98 Real d = (pi.origin - proj.origin).norm();
99
100 //If the new triangle is closer, use the new triangle to update p3d
101 if (glm::abs(d) < glm::abs(p3d.signed_distance))
102 {
103 p3d.signed_distance = d;
104 p3d.point = proj.origin;
105 p3d.id = list[j];
106
107 validate = true;
108 }
109 }
110
111 //Calculate the normal
112 if (validate)
113 {
114 TopologyModule::Triangle index = indices[p3d.id];
115 TTriangle3D<Real> tri(vertices[index[0]], vertices[index[1]], vertices[index[2]]);
116
117 Coord n = pi.origin - p3d.point;
118 n = n.norm() > EPSILON ? n.normalize() : tri.normal();
119
120 p3d.signed_distance -= dHat;
121 p3d.normal = n;
122 }
123
124 return validate;
125 }
126
127 template <typename Coord,DeviceType deviceType, typename IndexType>
128 DYN_FUNC void SO_ComputeObjectAndNormal(
129 Coord& pobject,
130 Coord& pnormal,
131 Array<Coord, deviceType>& surf_points,
132 Array<TopologyModule::Edge, deviceType>& edge,
133 Array<TopologyModule::Triangle, deviceType>& surf_triangles,
134 Array<TopologyModule::Tri2Edg, deviceType>& t2e,
135 Array<Coord, deviceType>& edgeN,
136 Array<Coord, deviceType>& vertexN,
137 Coord ppos,
138 IndexType surf_id)
139 {
140 int p = surf_triangles[surf_id][0];
141 int q = surf_triangles[surf_id][1];
142 int r = surf_triangles[surf_id][2];
143 Coord p0 = surf_points[p];
144 Coord p1 = surf_points[q];
145 Coord p2 = surf_points[r];
146
147 int eid00 = t2e[surf_id][0];
148 int eid11 = t2e[surf_id][1];
149 int eid22 = t2e[surf_id][2];
150 int te00 = glm::min(p, q), te01 = glm::max(p, q);
151 int te10 = glm::min(q, r), te11 = glm::max(q, r);
152 int te20 = glm::min(r, p), te21 = glm::max(r, p);
153 int eid0, eid1, eid2;
154 if (edge[eid00][0] == te00&& edge[eid00][1] == te01)
155 {
156 eid0 = eid00;
157 if (edge[eid11][0] == te10&& edge[eid11][1] == te11)
158 {
159 eid1 = eid11;
160 eid2 = eid22;
161 }
162 else
163 {
164 eid1 = eid22;
165 eid2 = eid11;
166 }
167 }
168 else if (edge[eid00][0] == te10&& edge[eid00][1] == te11)
169 {
170 eid1 = eid00;
171 if (edge[eid11][0] == te00&& edge[eid11][1] == te01)
172 {
173 eid0 = eid11;
174 eid2 = eid22;
175 }
176 else
177 {
178 eid2 = eid11;
179 eid0 = eid22;
180 }
181 }
182 else if (edge[eid00][0] == te20 && edge[eid00][1] == te21)
183 {
184 eid2 = eid00;
185 if (edge[eid11][0] == te00 && edge[eid11][1] == te01)
186 {
187 eid0 = eid11;
188 eid1 = eid22;
189 }
190 else
191 {
192 eid1 = eid11;
193 eid0 = eid22;
194 }
195 }
196
197 Coord dir = p0 - ppos;
198 Coord e0 = p1 - p0;
199 Coord e1 = p2 - p0;
200 Coord e2 = p2 - p1;
201 Real a = e0.dot(e0);
202 Real b = e0.dot(e1);
203 Real c = e1.dot(e1);
204 Real d = e0.dot(dir);
205 Real e = e1.dot(dir);
206 Real f = dir.dot(dir);
207
208 Real det = a * c - b * b;
209 Real s = b * e - c * d;
210 Real t = b * d - a * e;
211
212 Real maxL = maximum(maximum(e0.norm(), e1.norm()), e2.norm());
213 //handle degenerate triangles
214 if (det < REAL_EPSILON * maxL * maxL)
215 {
216 Real g = e2.normSquared();
217 Real l_max = a;
218
219 Coord op0 = p0;
220 Coord op1 = p1;
221 EKey oe(p, q);
222 if (c > l_max)
223 {
224 op0 = p0;
225 op1 = p2;
226 oe = EKey(p, r);
227
228 l_max = c;
229 }
230 if (g > l_max)
231 {
232 op0 = p1;
233 op1 = p2;
234 oe = EKey(q, r);
235 }
236
237 Coord el = ppos - op0;
238 Coord edir = op1 - op0;
239 if (edir.normSquared() < REAL_EPSILON_SQUARED)
240 {
241 pobject = surf_points[oe[0]];
242 pnormal = vertexN[oe[0]];
243 return;
244 }
245
246 Real et = el.dot(edir) / edir.normSquared();
247
248 if (et <= 0)
249 {
250 pobject = surf_points[oe[0]];
251 pnormal = vertexN[oe[0]];
252 return;
253 }
254 else if (et >= 1)
255 {
256 pobject = surf_points[oe[1]];
257 pnormal = vertexN[oe[1]];
258 return;
259 }
260 else
261 {
262 Coord eq = op0 + et * edir;
263 pobject = eq;
264 if (oe == EKey(edge[eid0][0], edge[eid0][1]))
265 {
266 pnormal = edgeN[eid0];
267 return;
268 }
269 else if (oe == EKey(edge[eid1][0], edge[eid1][1]))
270 {
271 pnormal = edgeN[eid1];
272 return;
273 }
274 else if (oe == EKey(edge[eid2][0], edge[eid2][1]))
275 {
276 pnormal = edgeN[eid2];
277 return;
278 }
279 }
280 }
281 if (s + t <= det)
282 {
283 if (s < 0)
284 {
285 if (t < 0)
286 {
287 //region 4
288 s = 0;
289 t = 0;
290 }
291 else
292 {
293 // region 3
294 s = 0;
295 t = (e >= 0 ? 0 : (-e >= c ? 1 : -e / c));
296 }
297 }
298 else
299 {
300 if (t < 0)
301 {
302 //region 5
303 s = (d >= 0 ? 0 : (-d >= a ? 1 : -d / a));
304 t = 0;
305 }
306 else
307 {
308 //region 0
309 Real invDet = 1 / det;
310 s *= invDet;
311 t *= invDet;
312 }
313 }
314 }
315 else
316 {
317 if (s < 0)
318 {
319 //region 2
320 s = 0;
321 t = 1;
322 }
323 else if (t < 0)
324 {
325 //region 6
326 s = 1;
327 t = 0;
328 }
329 else
330 {
331 //region 1
332 Real numer = c + e - b - d;
333 if (numer <= 0) {
334 s = 0;
335 }
336 else {
337 Real denom = a - 2 * b + c; // positive quantity
338 s = (numer >= denom ? 1 : numer / denom);
339 }
340 t = 1 - s;
341 }
342 }
343 pobject = (p0 + s * e0 + t * e1);
344 if (s == 0 && t == 0)
345 {
346 pnormal = vertexN[p];
347 //printf("111: %d %d %d, %d, %f %f %f \n", p, q, r, p, pnormal[0], pnormal[1], pnormal[2]);
348 return;
349 }
350 else if (s == 0 && t == 1)
351 {
352 pnormal = vertexN[r];
353 //printf("222: %d %d %d, %d, %f %f %f \n", p, q, r, r, pnormal[0], pnormal[1], pnormal[2]);
354 return;
355 }
356 else if (s == 1 && t == 0)
357 {
358 pnormal = vertexN[q];
359 //printf("333: %d %d %d, %d, %f %f %f \n", p, q, r, r, pnormal[0], pnormal[1], pnormal[2]);
360 return;
361 }
362 else if (s == 0 && t < 1)
363 {
364 pnormal = edgeN[eid2];
365 //printf("444: %d %d %d, %d %d %d, %f %f %f \n", p, q, r, eid2, edge[eid2][0], edge[eid2][1], pnormal[0], pnormal[1], pnormal[2]);
366 return;
367 }
368 else if (s < 1 && t == 0)
369 {
370 pnormal = edgeN[eid0];
371 //printf("555: %d %d %d, %d %d %d, %f %f %f \n", p, q, r, eid0, edge[eid0][0], edge[eid0][1], pnormal[0], pnormal[1], pnormal[2]);
372 return;
373 }
374 else if (s + t == 1)
375 {
376 pnormal = edgeN[eid1];
377 //printf("666: %d %d %d, %d %d %d, %f %f %f \n", p, q, r, eid1, edge[eid1][0], edge[eid1][1], pnormal[0], pnormal[1], pnormal[2]);
378 return;
379 }
380 else
381 {
382 pnormal = (p1 - p0).cross(p2 - p0);
383 pnormal.normalize();
384 //printf("777: %d %d %d, %f %f %f \n", p, q, r, pnormal[0], pnormal[1], pnormal[2]);
385 return;
386 }
387 }
388 template<typename Real, typename Coord, DeviceType deviceType, typename IndexType>
389 DYN_FUNC bool calculateSignedDistance2TriangleSetFromNormal(
390 ProjectedPoint3D<Real>& p3d,
391 Coord point,
392 Array<Coord, deviceType>& vertices,
393 Array<TopologyModule::Edge, deviceType>& edges,
394 Array<TopologyModule::Triangle, deviceType>& triangles,
395 Array<TopologyModule::Tri2Edg, deviceType>& t2e,
396 Array<Coord, deviceType>& edgeNormal,
397 Array<Coord, deviceType>& vertexNormal,
398 List<IndexType>& list,
399 Real dHat)
400 {
401 Real eps = 10 * EPSILON;
402
403 p3d.signed_distance = REAL_MAX;
404
405 bool validate = false;
406 for (uint j = 0; j < list.size(); j++)
407 {
408 Coord pnormal(0), pobject(0);
409
410 SO_ComputeObjectAndNormal(pobject, pnormal, vertices, edges, triangles, t2e, edgeNormal, vertexNormal, point, list[j]);
411
412 Real sign = (point - pobject).dot(pnormal) > Real(0) ? Real(1) : Real(-1);
413 Real d = sign * (point - pobject).norm();
414
415 //If the new triangle is closer, use the new triangle to update p3d
416 if (glm::abs(d) < glm::abs(p3d.signed_distance) - eps)
417 {
418 p3d.signed_distance = d;
419 p3d.point = pobject;
420 p3d.normal = pnormal;
421 p3d.id = list[j];
422
423 validate = true;
424 }
425 }
426 return validate;
427 }
428
429}
Real
double Real
Definition Typedef.inl:23
dyno::Array
This class is designed to be elegant, so it can be directly passed to GPU as parameters.
Definition Array.h:27
dyno::List
Be aware do not use this structure on GPU if the data size is large.
Definition List.h:21
dyno::List::size
DYN_FUNC uint size()
Definition List.inl:69
dyno::TPlane3D
2D geometric primitives in three-dimensional space
Definition Primitive3D.h:456
dyno::TPoint3D
0D geometric primitive in three-dimensional space
Definition Primitive3D.h:109
dyno::TPoint3D::distance
DYN_FUNC Real distance(const TPoint3D< Real > &pt) const
Definition Primitive3D.inl:500
dyno::TTriangle3D::normal
DYN_FUNC Coord3D normal() const
Definition Primitive3D.inl:2915
dyno::TopologyModule::Triangle
Vector< PointType, 3 > Triangle
Definition TopologyModule.h:31
dyno
This is an implementation of AdditiveCCD based on peridyno.
Definition Array.h:25
dyno::sign
DYN_FUNC int sign(T const &a, T const EPS=EPSILON)
Definition SimpleMath.h:39
dyno::REAL_EPSILON_SQUARED
constexpr Real REAL_EPSILON_SQUARED
Definition Typedef.inl:44
dyno::REAL_MAX
constexpr Real REAL_MAX
Definition Typedef.inl:45
dyno::calculateSignedDistance2TriangleSet
DYN_FUNC bool calculateSignedDistance2TriangleSet(ProjectedPoint3D< Real > &p3d, Coord point, Array< Coord, deviceType > &vertices, Array< TopologyModule::Triangle, deviceType > &indices, List< IndexType > &list, Real dHat=0)
Calculate the signed distance from a point to a triangular mesh.
Definition Distance3D.inl:4
dyno::SO_ComputeObjectAndNormal
DYN_FUNC void SO_ComputeObjectAndNormal(Coord &pobject, Coord &pnormal, Array< Coord, deviceType > &surf_points, Array< TopologyModule::Edge, deviceType > &edge, Array< TopologyModule::Triangle, deviceType > &surf_triangles, Array< TopologyModule::Tri2Edg, deviceType > &t2e, Array< Coord, deviceType > &edgeN, Array< Coord, deviceType > &vertexN, Coord ppos, IndexType surf_id)
Definition Distance3D.inl:128
dyno::calculateDistance2TriangleSet
DYN_FUNC bool calculateDistance2TriangleSet(ProjectedPoint3D< Real > &p3d, Coord point, Array< Coord, deviceType > &vertices, Array< TopologyModule::Triangle, deviceType > &indices, List< IndexType > &list, Real dHat=0)
Calculate the distance from a point to a triangular mesh.
Definition Distance3D.inl:83
dyno::cross
DYN_FUNC Vector< T, 3 > cross(Vector< T, 3 > const &U, Vector< T, 3 > const &V)
Definition SimpleMath.h:211
dyno::dot
DYN_FUNC T dot(Vector< T, 2 > const &U, Vector< T, 2 > const &V)
Definition SimpleMath.h:199
dyno::EPSILON
constexpr Real EPSILON
Definition Typedef.inl:42
dyno::REAL_EPSILON
constexpr Real REAL_EPSILON
Definition Typedef.inl:43
dyno::uint
unsigned int uint
Definition VkProgram.h:14
dyno::maximum
DYN_FUNC T maximum(const T &v0, const T &v1)
Definition SimpleMath.h:160
dyno::calculateSignedDistance2TriangleSetFromNormal
DYN_FUNC bool calculateSignedDistance2TriangleSetFromNormal(ProjectedPoint3D< Real > &p3d, Coord point, Array< Coord, deviceType > &vertices, Array< TopologyModule::Edge, deviceType > &edges, Array< TopologyModule::Triangle, deviceType > &triangles, Array< TopologyModule::Tri2Edg, deviceType > &t2e, Array< Coord, deviceType > &edgeNormal, Array< Coord, deviceType > &vertexNormal, List< IndexType > &list, Real dHat=0)
Definition Distance3D.inl:389
Generated by doxygen 1.13.2