← Back to branch summary

~ubuntu-branches/ubuntu/wily/dolfin/wily-proposed

« back to all changes in this revision

Viewing changes to dolfin/geometry/CollisionDetection.cpp

  • Committer: Package Import Robot
  • Author(s): Johannes Ring
  • Date: 2014-09-22 14:35:34 UTC
  • mfrom: (1.1.17) (19.1.23 sid)
  • Revision ID: package-import@ubuntu.com-20140922143534-0yi89jyuqbgdxwm9
Tags: 1.4.0+dfsg-4
http://bugs.debian.org/760658
* debian/control: Disable libcgal-dev on i386, mipsel and sparc.
* debian/rules: Remove bad directives in pkg-config file dolfin.pc
  (closes: #760658).
* Remove debian/libdolfin-dev.lintian-overrides.

Show diffs side-by-side

added added

removed removed

Lines of Context:
dolfin/geometry/CollisionDetection.cpp
 
1
// Copyright (C) 2014 Anders Logg and August Johansson
 
2
//
 
3
// This file is part of DOLFIN.
 
4
//
 
5
// DOLFIN is free software: you can redistribute it and/or modify
 
6
// it under the terms of the GNU Lesser General Public License as published by
 
7
// the Free Software Foundation, either version 3 of the License, or
 
8
// (at your option) any later version.
 
9
//
 
10
// DOLFIN is distributed in the hope that it will be useful,
 
11
// but WITHOUT ANY WARRANTY; without even the implied warranty of
 
12
// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
 
13
// GNU Lesser General Public License for more details.
 
14
//
 
15
// You should have received a copy of the GNU Lesser General Public License
 
16
// along with DOLFIN. If not, see <http://www.gnu.org/licenses/>.
 
17
//
 
18
// First added:  2014-02-03
 
19
// Last changed: 2014-02-24
 
20
//
 
21
//-----------------------------------------------------------------------------
 
22
// Special note regarding the function collides_tetrahedron_tetrahedron
 
23
//-----------------------------------------------------------------------------
 
24
//
 
25
// The source code for the tetrahedron-tetrahedron collision test is
 
26
// from Fabio Ganovelli, Federico Ponchio and Claudio Rocchini: Fast
 
27
// Tetrahedron-Tetrahedron Overlap Algorithm, Journal of Graphics
 
28
// Tools, 7(2), 2002, and is under the following copyright:
 
29
//
 
30
// Visual Computing Group
 
31
// IEI Institute, CNUCE Institute, CNR Pisa
 
32
//
 
33
// Copyright(C) 2002 by Fabio Ganovelli, Federico Ponchio and Claudio
 
34
// Rocchini
 
35
//
 
36
// All rights reserved.
 
37
//
 
38
// Permission to use, copy, modify, distribute and sell this software
 
39
// and its documentation for any purpose is hereby granted without
 
40
// fee, provided that the above copyright notice appear in all copies
 
41
// and that both that copyright notice and this permission notice
 
42
// appear in supporting documentation. the author makes no
 
43
// representations about the suitability of this software for any
 
44
// purpose. It is provided "as is" without express or implied
 
45
// warranty.
 
46
//
 
47
//-----------------------------------------------------------------------------
 
48
// Special note regarding the function collides_triangle_triangle
 
49
//-----------------------------------------------------------------------------
 
50
//
 
51
// The source code for the triangle-triangle collision test is from
 
52
// Tomas Moller: A Fast Triangle-Triangle Intersection Test, Journal
 
53
// of Graphics Tools, 2(2), 1997, and is in the public domain.
 
54
//
 
55
//-----------------------------------------------------------------------------
 
56
 
 
57
#include <dolfin/mesh/MeshEntity.h>
 
58
#include "Point.h"
 
59
#include "CollisionDetection.h"
 
60
 
 
61
using namespace dolfin;
 
62
 
 
63
//-----------------------------------------------------------------------------
 
64
bool CollisionDetection::collides(const MeshEntity& entity,
 
65
                                  const Point& point)
 
66
{
 
67
  switch (entity.dim())
 
68
  {
 
69
  case 0:
 
70
    dolfin_not_implemented();
 
71
    break;
 
72
  case 1:
 
73
    return collides_interval_point(entity, point);
 
74
  case 2:
 
75
    return collides_triangle_point(entity, point);
 
76
  case 3:
 
77
    return collides_tetrahedron_point(entity, point);
 
78
  default:
 
79
    dolfin_error("CollisionDetection.cpp",
 
80
                 "collides entity with point",
 
81
                 "Unknown dimension of entity");
 
82
  }
 
83
 
 
84
  return false;
 
85
}
 
86
//-----------------------------------------------------------------------------
 
87
bool
 
88
CollisionDetection::collides(const MeshEntity& entity_0,
 
89
                             const MeshEntity& entity_1)
 
90
{
 
91
  switch (entity_0.dim())
 
92
  {
 
93
  case 0:
 
94
    // Collision with PointCell
 
95
    dolfin_not_implemented();
 
96
    break;
 
97
  case 1:
 
98
    // Collision with interval
 
99
    switch (entity_1.dim())
 
100
    {
 
101
    case 0:
 
102
      dolfin_not_implemented();
 
103
      break;
 
104
    case 1:
 
105
      return collides_interval_interval(entity_1, entity_0);
 
106
      break;
 
107
    case 2:
 
108
      dolfin_not_implemented();
 
109
      break;
 
110
    case 3:
 
111
      dolfin_not_implemented();
 
112
      break;
 
113
    default:
 
114
      dolfin_error("CollisionDetection.cpp",
 
115
                   "collides entity_0 with entity_1",
 
116
                   "Unknown dimension of entity_1 in IntervalCell collision");
 
117
    }
 
118
    break;
 
119
  case 2:
 
120
    // Collision with triangle
 
121
    switch (entity_1.dim())
 
122
    {
 
123
    case 0:
 
124
      dolfin_not_implemented();
 
125
      break;
 
126
    case 1:
 
127
      dolfin_not_implemented();
 
128
      break;
 
129
    case 2:
 
130
      return collides_triangle_triangle(entity_0, entity_1);
 
131
    case 3:
 
132
      return collides_tetrahedron_triangle(entity_1, entity_0);
 
133
    default:
 
134
      dolfin_error("CollisionDetection.cpp",
 
135
                   "collides entity_0 with entity_1",
 
136
                   "Unknown dimension of entity_1 in TriangleCell collision");
 
137
    }
 
138
    break;
 
139
  case 3:
 
140
    // Collision with tetrahedron
 
141
    switch (entity_1.dim())
 
142
    {
 
143
    case 0:
 
144
     dolfin_not_implemented();
 
145
      break;
 
146
    case 1:
 
147
      dolfin_not_implemented();
 
148
      break;
 
149
    case 2:
 
150
      return collides_tetrahedron_triangle(entity_0, entity_1);
 
151
      break;
 
152
    case 3:
 
153
      return collides_tetrahedron_tetrahedron(entity_0, entity_1);
 
154
      break;
 
155
    default:
 
156
      dolfin_error("CollisionDetection.cpp",
 
157
                   "collides entity_0 with entity_1",
 
158
                   "Unknown dimension of entity_1 in TetrahedronCell collision");
 
159
    }
 
160
    break;
 
161
  default:
 
162
    dolfin_error("CollisionDetection.cpp",
 
163
                 "collides entity_0 with entity_1",
 
164
                 "Unknown dimension of entity_0");
 
165
  }
 
166
 
 
167
  return false;
 
168
}
 
169
//-----------------------------------------------------------------------------
 
170
bool CollisionDetection::collides_interval_point(const MeshEntity& entity,
 
171
                                                 const Point& point)
 
172
{
 
173
  // Get coordinates
 
174
  const MeshGeometry& geometry = entity.mesh().geometry();
 
175
  const unsigned int* vertices = entity.entities(0);
 
176
  const double x0 = geometry.point(vertices[0])[0];
 
177
  const double x1 = geometry.point(vertices[1])[0];
 
178
  const double x = point.x();
 
179
 
 
180
  // Check for collision
 
181
  const double dx = std::abs(x1 - x0);
 
182
  const double eps = std::max(DOLFIN_EPS_LARGE, DOLFIN_EPS_LARGE*dx);
 
183
  return ((x >= x0 - eps && x <= x1 + eps) ||
 
184
          (x >= x1 - eps && x <= x0 + eps));
 
185
}
 
186
//-----------------------------------------------------------------------------
 
187
bool
 
188
CollisionDetection::collides_interval_interval(const MeshEntity& interval_0,
 
189
                                               const MeshEntity& interval_1)
 
190
{
 
191
  // Get coordinates
 
192
  const MeshGeometry& geometry_0 = interval_0.mesh().geometry();
 
193
  const MeshGeometry& geometry_1 = interval_1.mesh().geometry();
 
194
  const unsigned int* vertices_0 = interval_0.entities(0);
 
195
  const unsigned int* vertices_1 = interval_1.entities(0);
 
196
  const double x00 = geometry_0.point(vertices_0[0])[0];
 
197
  const double x01 = geometry_0.point(vertices_0[1])[0];
 
198
  const double x10 = geometry_1.point(vertices_1[0])[0];
 
199
  const double x11 = geometry_1.point(vertices_1[1])[0];
 
200
 
 
201
  const double a0 = std::min(x00, x01);
 
202
  const double b0 = std::max(x00, x01);
 
203
  const double a1 = std::min(x10, x11);
 
204
  const double b1 = std::max(x10, x11);
 
205
 
 
206
  // Check for collisions
 
207
  const double dx = std::min(b0 - a0, b1 - a1);
 
208
  const double eps = std::max(DOLFIN_EPS_LARGE, DOLFIN_EPS_LARGE*dx);
 
209
  return b1 > a0 - eps && a1 < b0 + eps;
 
210
}
 
211
//-----------------------------------------------------------------------------
 
212
bool CollisionDetection::collides_triangle_point(const MeshEntity& triangle,
 
213
                                                 const Point& point)
 
214
{
 
215
  dolfin_assert(triangle.mesh().topology().dim() == 2);
 
216
 
 
217
  const MeshGeometry& geometry = triangle.mesh().geometry();
 
218
  const unsigned int* vertices = triangle.entities(0);
 
219
  return collides_triangle_point(geometry.point(vertices[0]),
 
220
                                 geometry.point(vertices[1]),
 
221
                                 geometry.point(vertices[2]),
 
222
                                 point);
 
223
}
 
224
//-----------------------------------------------------------------------------
 
225
bool
 
226
CollisionDetection::collides_triangle_triangle(const MeshEntity& triangle_0,
 
227
                                               const MeshEntity& triangle_1)
 
228
{
 
229
  dolfin_assert(triangle_0.mesh().topology().dim() == 2);
 
230
  dolfin_assert(triangle_1.mesh().topology().dim() == 2);
 
231
 
 
232
  // Get vertices as points
 
233
  const MeshGeometry& geometry_0 = triangle_0.mesh().geometry();
 
234
  const unsigned int* vertices_0 = triangle_0.entities(0);
 
235
  const MeshGeometry& geometry_1 = triangle_1.mesh().geometry();
 
236
  const unsigned int* vertices_1 = triangle_1.entities(0);
 
237
 
 
238
  return collides_triangle_triangle(geometry_0.point(vertices_0[0]),
 
239
                                    geometry_0.point(vertices_0[1]),
 
240
                                    geometry_0.point(vertices_0[2]),
 
241
                                    geometry_1.point(vertices_1[0]),
 
242
                                    geometry_1.point(vertices_1[1]),
 
243
                                    geometry_1.point(vertices_1[2]));
 
244
}
 
245
//-----------------------------------------------------------------------------
 
246
bool
 
247
CollisionDetection::collides_tetrahedron_point(const MeshEntity& tetrahedron,
 
248
                                               const Point& point)
 
249
{
 
250
  dolfin_assert(tetrahedron.mesh().topology().dim() == 3);
 
251
 
 
252
  // Get the vertices as points
 
253
  const MeshGeometry& geometry = tetrahedron.mesh().geometry();
 
254
  const unsigned int* vertices = tetrahedron.entities(0);
 
255
 
 
256
  return collides_tetrahedron_point(geometry.point(vertices[0]),
 
257
                                    geometry.point(vertices[1]),
 
258
                                    geometry.point(vertices[2]),
 
259
                                    geometry.point(vertices[3]),
 
260
                                    point);
 
261
}
 
262
//-----------------------------------------------------------------------------
 
263
bool
 
264
CollisionDetection::collides_tetrahedron_triangle(const MeshEntity& tetrahedron,
 
265
                                                  const MeshEntity& triangle)
 
266
{
 
267
  dolfin_assert(tetrahedron.mesh().topology().dim() == 3);
 
268
  dolfin_assert(triangle.mesh().topology().dim() == 2);
 
269
 
 
270
  // Get the vertices of the tetrahedron as points
 
271
  const MeshGeometry& geometry_tet = tetrahedron.mesh().geometry();
 
272
  const unsigned int* vertices_tet = tetrahedron.entities(0);
 
273
 
 
274
  // Get the vertices of the triangle as points
 
275
  const MeshGeometry& geometry_tri = triangle.mesh().geometry();
 
276
  const unsigned int* vertices_tri = triangle.entities(0);
 
277
 
 
278
  return collides_tetrahedron_triangle(geometry_tet.point(vertices_tet[0]),
 
279
                                       geometry_tet.point(vertices_tet[1]),
 
280
                                       geometry_tet.point(vertices_tet[2]),
 
281
                                       geometry_tet.point(vertices_tet[3]),
 
282
                                       geometry_tri.point(vertices_tri[0]),
 
283
                                       geometry_tri.point(vertices_tri[1]),
 
284
                                       geometry_tri.point(vertices_tri[2]));
 
285
}
 
286
//-----------------------------------------------------------------------------
 
287
bool
 
288
CollisionDetection::collides_tetrahedron_tetrahedron
 
289
(const MeshEntity& tetrahedron_0,
 
290
 const MeshEntity& tetrahedron_1)
 
291
{
 
292
  // This algorithm checks whether two tetrahedra intersect.
 
293
 
 
294
  // Algorithm and source code from Fabio Ganovelli, Federico Ponchio
 
295
  // and Claudio Rocchini: Fast Tetrahedron-Tetrahedron Overlap
 
296
  // Algorithm, Journal of Graphics Tools, 7(2), 2002. DOI:
 
297
  // 10.1080/10867651.2002.10487557. Source code available at
 
298
  // http://web.archive.org/web/20031130075955/http://www.acm.org/jgt/papers/GanovelliPonchioRocchini02/tet_a_tet.html
 
299
 
 
300
  dolfin_assert(tetrahedron_0.mesh().topology().dim() == 3);
 
301
  dolfin_assert(tetrahedron_1.mesh().topology().dim() == 3);
 
302
 
 
303
  // Get the vertices as points
 
304
  const MeshGeometry& geometry = tetrahedron_0.mesh().geometry();
 
305
  const unsigned int* vertices = tetrahedron_0.entities(0);
 
306
  const MeshGeometry& geometry_q = tetrahedron_1.mesh().geometry();
 
307
  const unsigned int* vertices_q = tetrahedron_1.entities(0);
 
308
  std::vector<Point> V1(4), V2(4);
 
309
  for (std::size_t i = 0; i < 4; ++i)
 
310
  {
 
311
    V1[i] = geometry.point(vertices[i]);
 
312
    V2[i] = geometry_q.point(vertices_q[i]);
 
313
  }
 
314
 
 
315
  // Get the vectors between V2 and V1[0]
 
316
  std::vector<Point> P_V1(4);
 
317
  for (std::size_t i = 0; i < 4; ++i)
 
318
    P_V1[i] = V2[i]-V1[0];
 
319
 
 
320
  // Data structure for edges of V1 and V2
 
321
  std::vector<Point> e_v1(5), e_v2(5);
 
322
  e_v1[0] = V1[1] - V1[0];
 
323
  e_v1[1] = V1[2] - V1[0];
 
324
  e_v1[2] = V1[3] - V1[0];
 
325
  Point n = e_v1[1].cross(e_v1[0]);
 
326
 
 
327
  // Maybe flip normal. Normal should be outward.
 
328
  if (n.dot(e_v1[2]) > 0)
 
329
    n *= -1;
 
330
  std::vector<int> masks(4);
 
331
  std::vector<std::vector<double> > Coord_1(4, std::vector<double>(4));
 
332
  if (separating_plane_face_A_1(P_V1, n, Coord_1[0], masks[0]))
 
333
    return false;
 
334
  n = e_v1[0].cross(e_v1[2]);
 
335
 
 
336
  // Maybe flip normal
 
337
  if (n.dot(e_v1[1]) > 0)
 
338
    n *= -1;
 
339
  if (separating_plane_face_A_1(P_V1, n, Coord_1[1], masks[1]))
 
340
    return false;
 
341
  if (separating_plane_edge_A(Coord_1, masks, 0, 1))
 
342
    return false;
 
343
  n = e_v1[2].cross(e_v1[1]);
 
344
 
 
345
  // Maybe flip normal
 
346
  if (n.dot(e_v1[0]) > 0)
 
347
    n *= -1;
 
348
  if (separating_plane_face_A_1(P_V1, n, Coord_1[2], masks[2]))
 
349
    return false;
 
350
  if (separating_plane_edge_A(Coord_1, masks, 0, 2))
 
351
    return false;
 
352
  if (separating_plane_edge_A(Coord_1, masks, 1,2))
 
353
    return false;
 
354
  e_v1[4] = V1[3] - V1[1];
 
355
  e_v1[3] = V1[2] - V1[1];
 
356
  n = e_v1[3].cross(e_v1[4]);
 
357
 
 
358
  // Maybe flip normal. Note the < since e_v1[0]=v1-v0.
 
359
  if (n.dot(e_v1[0]) < 0)
 
360
    n *= -1;
 
361
  if (separating_plane_face_A_2(V1, V2, n, Coord_1[3], masks[3]))
 
362
    return false;
 
363
  if (separating_plane_edge_A(Coord_1, masks, 0, 3))
 
364
    return false;
 
365
  if (separating_plane_edge_A(Coord_1, masks, 1, 3))
 
366
    return false;
 
367
  if (separating_plane_edge_A(Coord_1, masks, 2, 3))
 
368
    return false;
 
369
  if ((masks[0] | masks[1] | masks[2] | masks[3] )!= 15)
 
370
    return true;
 
371
 
 
372
  // From now on, if there is a separating plane, it is parallel to a
 
373
  // face of b.
 
374
  std::vector<Point> P_V2(4);
 
375
  for (std::size_t i = 0; i < 4; ++i)
 
376
    P_V2[i] = V1[i] - V2[0];
 
377
  e_v2[0] = V2[1] - V2[0];
 
378
  e_v2[1] = V2[2] - V2[0];
 
379
  e_v2[2] = V2[3] - V2[0];
 
380
  n = e_v2[1].cross(e_v2[0]);
 
381
 
 
382
  // Maybe flip normal
 
383
  if (n.dot(e_v2[2])>0)
 
384
    n *= -1;
 
385
  if (separating_plane_face_B_1(P_V2, n))
 
386
    return false;
 
387
  n=e_v2[0].cross(e_v2[2]);
 
388
 
 
389
  // Maybe flip normal
 
390
  if (n.dot(e_v2[1]) > 0)
 
391
    n *= -1;
 
392
  if (separating_plane_face_B_1(P_V2, n))
 
393
    return false;
 
394
  n = e_v2[2].cross(e_v2[1]);
 
395
 
 
396
  // Maybe flip normal
 
397
  if (n.dot(e_v2[0]) > 0)
 
398
    n *= -1;
 
399
  if (separating_plane_face_B_1(P_V2, n))
 
400
    return false;
 
401
  e_v2[4] = V2[3] - V2[1];
 
402
  e_v2[3] = V2[2] - V2[1];
 
403
  n = e_v2[3].cross(e_v2[4]);
 
404
 
 
405
  // Maybe flip normal. Note the < since e_v2[0] = V2[1] - V2[0].
 
406
  if (n.dot(e_v2[0]) < 0)
 
407
    n *= -1;
 
408
  if (separating_plane_face_B_2(V1, V2, n))
 
409
    return false;
 
410
 
 
411
  return true;
 
412
}
 
413
//-----------------------------------------------------------------------------
 
414
bool
 
415
CollisionDetection::collides_edge_edge(const Point& a,
 
416
                                       const Point& b,
 
417
                                       const Point& c,
 
418
                                       const Point& d)
 
419
{
 
420
  const double tol = DOLFIN_EPS_LARGE;
 
421
 
 
422
  // Check if two edges are the same
 
423
  if ((a - c).norm() < tol and (b - d).norm() < tol)
 
424
    return false;
 
425
  if ((a - d).norm() < tol and (b - c).norm() < tol)
 
426
    return false;
 
427
 
 
428
  // Get edges as vectors and compute the normal
 
429
  const Point L1 = b - a;
 
430
  const Point L2 = d - c;
 
431
  const Point n = L1.cross(L2);
 
432
 
 
433
  // Check if L1 and L2 are coplanar
 
434
  const Point ca = c - a;
 
435
  if (std::abs(ca.dot(n)) > tol)
 
436
    return false;
 
437
 
 
438
  // Find orthogonal plane with normal n1
 
439
  const Point n1 = n.cross(L1);
 
440
  const double n1dotL2 = n1.dot(L2);
 
441
  if (std::abs(n1dotL2) < tol)
 
442
    return false;
 
443
  const double t = n1.dot(a - c) / n1dotL2;
 
444
  if (t <= 0 or t >= 1)
 
445
    return false;
 
446
 
 
447
  // Find orthogonal plane with normal n2
 
448
  const Point n2 = n.cross(L2);
 
449
  const double n2dotL1 = n2.dot(L1);
 
450
  if (std::abs(n2dotL1) < tol)
 
451
    return false;
 
452
  const double s = n2.dot(c - a) / n2dotL1;
 
453
  if (s <= 0 or s >= 1)
 
454
    return false;
 
455
 
 
456
  return true;
 
457
}
 
458
//-----------------------------------------------------------------------------
 
459
bool CollisionDetection::collides_triangle_point(const Point& p0,
 
460
                                                 const Point& p1,
 
461
                                                 const Point& p2,
 
462
                                                 const Point &point)
 
463
{
 
464
  // Algorithm from http://www.blackpawn.com/texts/pointinpoly/
 
465
  // See also "Real-Time Collision Detection" by Christer Ericson.
 
466
  //
 
467
  // We express AP as a linear combination of the vectors AB and
 
468
  // AC. Point is inside triangle iff AP is a convex combination.
 
469
  //
 
470
  // Note: This function may be optimized if only 2D vectors and inner
 
471
  // products need to be computed.
 
472
 
 
473
  // Compute vectors
 
474
  const Point v1 = p1 - p0;
 
475
  const Point v2 = p2 - p0;
 
476
  const Point v = point - p0;
 
477
 
 
478
  // Compute entries of linear system
 
479
  const double a11 = v1.dot(v1);
 
480
  const double a12 = v1.dot(v2);
 
481
  const double a22 = v2.dot(v2);
 
482
  const double b1 = v.dot(v1);
 
483
  const double b2 = v.dot(v2);
 
484
 
 
485
  // Solve linear system
 
486
  const double inv_det = 1.0 / (a11*a22 - a12*a12);
 
487
  const double x1 = inv_det*( a22*b1 - a12*b2);
 
488
  const double x2 = inv_det*(-a12*b1 + a11*b2);
 
489
 
 
490
  // Tolerance for numeric test (using vector v1)
 
491
  const double dx = std::abs(v1.x());
 
492
  const double dy = std::abs(v1.y());
 
493
  const double eps = std::max(DOLFIN_EPS_LARGE, DOLFIN_EPS_LARGE*std::max(dx, dy));
 
494
 
 
495
  // Check if point is inside
 
496
  return x1 >= -eps && x2 >= -eps && x1 + x2 <= 1.0 + eps;
 
497
}
 
498
//-----------------------------------------------------------------------------
 
499
bool
 
500
CollisionDetection::collides_triangle_triangle(const Point& p0,
 
501
                                               const Point& p1,
 
502
                                               const Point& p2,
 
503
                                               const Point& q0,
 
504
                                               const Point& q1,
 
505
                                               const Point& q2)
 
506
{
 
507
  // Algorithm and code from Tomas Moller: A Fast Triangle-Triangle
 
508
  // Intersection Test, Journal of Graphics Tools, 2(2), 1997. Source
 
509
  // code is available at
 
510
  // http://fileadmin.cs.lth.se/cs/Personal/Tomas_Akenine-Moller/code/opttritri.txt
 
511
 
 
512
  // First check if the triangles are the same. We need to do this
 
513
  // separately if we do _not_ allow for adjacent edges to be
 
514
  // classified as colliding (see the edge_edge_test).
 
515
 
 
516
  const Point Vmid = (p0 + p1 + p2) / 3.;
 
517
  const Point Umid = (q0 + q1 + q2) / 3.;
 
518
  if ((Vmid-Umid).norm() < DOLFIN_EPS_LARGE)
 
519
    return true;
 
520
 
 
521
  Point E1, E2;
 
522
  Point N1, N2;
 
523
  double d1, d2;
 
524
  double du0, du1, du2, dv0, dv1, dv2;
 
525
  Point D;
 
526
  double isect1[2], isect2[2];
 
527
  double du0du1, du0du2, dv0dv1, dv0dv2;
 
528
  int index;
 
529
  double vp0, vp1, vp2;
 
530
  double up0, up1, up2;
 
531
  double bb, cc, max;
 
532
 
 
533
  // Compute plane equation of triangle(p0,p1,p2)
 
534
  E1 = p1-p0;
 
535
  E2 = p2-p0;
 
536
  N1 = E1.cross(E2);
 
537
  d1 = -N1.dot(p0);
 
538
 
 
539
  // Plane equation 1: N1.X+d1=0. Put q0,q1,q2 into plane equation 1
 
540
  // to compute signed distances to the plane
 
541
  du0 = N1.dot(q0)+d1;
 
542
  du1 = N1.dot(q1)+d1;
 
543
  du2 = N1.dot(q2)+d1;
 
544
 
 
545
  // Coplanarity robustness check
 
546
  if (std::abs(du0) < DOLFIN_EPS_LARGE)
 
547
    du0 = 0.0;
 
548
  if (std::abs(du1) < DOLFIN_EPS_LARGE)
 
549
    du1 = 0.0;
 
550
  if (std::abs(du2) < DOLFIN_EPS_LARGE)
 
551
    du2 = 0.0;
 
552
  du0du1 = du0*du1;
 
553
  du0du2 = du0*du2;
 
554
 
 
555
  // Same sign on all of them + not equal 0?
 
556
  if (du0du1>0. && du0du2>0.)
 
557
    return false;
 
558
 
 
559
  // Compute plane of triangle (q0,q1,q2)
 
560
  E1 = q1-q0;
 
561
  E2 = q2-q0;
 
562
  N2 = E1.cross(E2);
 
563
  d2 = -N2.dot(q0);
 
564
  // Plane equation 2: N2.X+d2=0. Put p0,p1,p2 into plane equation 2
 
565
  dv0 = N2.dot(p0)+d2;
 
566
  dv1 = N2.dot(p1)+d2;
 
567
  dv2 = N2.dot(p2)+d2;
 
568
 
 
569
  // Coplanarity check
 
570
  if (std::abs(dv0) < DOLFIN_EPS_LARGE)
 
571
    dv0 = 0.0;
 
572
  if (std::abs(dv1) < DOLFIN_EPS_LARGE)
 
573
    dv1 = 0.0;
 
574
  if (std::abs(dv2) < DOLFIN_EPS_LARGE)
 
575
    dv2 = 0.0;
 
576
  dv0dv1 = dv0*dv1;
 
577
  dv0dv2 = dv0*dv2;
 
578
 
 
579
  // Same sign on all of them + not equal 0 ?
 
580
  if (dv0dv1>0. && dv0dv2>0.)
 
581
    return false;
 
582
 
 
583
  // Compute direction of intersection line
 
584
  D = N1.cross(N2);
 
585
 
 
586
  // Compute and index to the largest component of D
 
587
  max = (double)std::abs(D[0]);
 
588
  index = 0;
 
589
  bb = (double)std::abs(D[1]);
 
590
  cc = (double)std::abs(D[2]);
 
591
  if (bb > max)
 
592
    max = bb, index = 1;
 
593
  if (cc > max)
 
594
    max = cc, index = 2;
 
595
 
 
596
  // This is the simplified projection onto L
 
597
  vp0 = p0[index];
 
598
  vp1 = p1[index];
 
599
  vp2 = p2[index];
 
600
 
 
601
  up0 = q0[index];
 
602
  up1 = q1[index];
 
603
  up2 = q2[index];
 
604
 
 
605
  // Compute interval for triangle 1
 
606
  double a, b, c, x0, x1;
 
607
  if (compute_intervals(vp0, vp1, vp2, dv0, dv1, dv2, dv0dv1, dv0dv2,
 
608
                        a, b, c, x0, x1))
 
609
    return coplanar_tri_tri(N1, p0, p1, p2, q0, q1, q2);
 
610
 
 
611
  // Compute interval for triangle 2
 
612
  double d, e, f, y0, y1;
 
613
  if (compute_intervals(up0, up1, up2, du0, du1, du2, du0du1, du0du2,
 
614
                        d, e, f, y0, y1))
 
615
    return coplanar_tri_tri(N1, p0, p1, p2, q0, q1, q2);
 
616
 
 
617
  double xx, yy, xxyy, tmp;
 
618
  xx = x0*x1;
 
619
  yy = y0*y1;
 
620
  xxyy = xx*yy;
 
621
 
 
622
  tmp = a*xxyy;
 
623
  isect1[0] = tmp+b*x1*yy;
 
624
  isect1[1] = tmp+c*x0*yy;
 
625
 
 
626
  tmp = d*xxyy;
 
627
  isect2[0] = tmp+e*xx*y1;
 
628
  isect2[1] = tmp+f*xx*y0;
 
629
 
 
630
  if (isect1[0] > isect1[1])
 
631
    std::swap(isect1[0], isect1[1]);
 
632
  if (isect2[0] > isect2[1])
 
633
    std::swap(isect2[0], isect2[1]);
 
634
 
 
635
  if (isect1[1] < isect2[0] ||
 
636
      isect2[1] < isect1[0])
 
637
    return false;
 
638
 
 
639
  return true;
 
640
}
 
641
//-----------------------------------------------------------------------------
 
642
bool
 
643
CollisionDetection::collides_tetrahedron_point(const Point& p0,
 
644
                                               const Point& p1,
 
645
                                               const Point& p2,
 
646
                                               const Point& p3,
 
647
                                               const Point& point)
 
648
{
 
649
  // Algorithm from http://www.blackpawn.com/texts/pointinpoly/
 
650
  // See also "Real-Time Collision Detection" by Christer Ericson.
 
651
  //
 
652
  // We express AP as a linear combination of the vectors AB, AC and
 
653
  // AD. Point is inside triangle iff AP is a convex combination.
 
654
 
 
655
  // Compute vectors
 
656
  const Point v1 = p1 - p0;
 
657
  const Point v2 = p2 - p0;
 
658
  const Point v3 = p3 - p0;
 
659
  const Point v = point - p0;
 
660
 
 
661
  // Compute entries of linear system
 
662
  const double a11 = v1.dot(v1);
 
663
  const double a12 = v1.dot(v2);
 
664
  const double a13 = v1.dot(v3);
 
665
  const double a22 = v2.dot(v2);
 
666
  const double a23 = v2.dot(v3);
 
667
  const double a33 = v3.dot(v3);
 
668
  const double b1 = v.dot(v1);
 
669
  const double b2 = v.dot(v2);
 
670
  const double b3 = v.dot(v3);
 
671
 
 
672
  // Compute subdeterminants
 
673
  const double d11 = a22*a33 - a23*a23;
 
674
  const double d12 = a12*a33 - a23*a13;
 
675
  const double d13 = a12*a23 - a22*a13;
 
676
  const double d22 = a11*a33 - a13*a13;
 
677
  const double d23 = a11*a23 - a12*a13;
 
678
  const double d33 = a11*a22 - a12*a12;
 
679
 
 
680
  // Compute inverse of determinant determinant
 
681
  const double inv_det = 1.0 / (a11*d11 - a12*d12 + a13*d13);
 
682
 
 
683
  // Solve linear system
 
684
  const double x1 = inv_det*( d11*b1 - d12*b2 + d13*b3);
 
685
  const double x2 = inv_det*(-d12*b1 + d22*b2 - d23*b3);
 
686
  const double x3 = inv_det*( d13*b1 - d23*b2 + d33*b3);
 
687
 
 
688
  // Tolerance for numeric test (using vector v1)
 
689
  const double dx = std::abs(v1.x());
 
690
  const double dy = std::abs(v1.y());
 
691
  const double dz = std::abs(v1.z());
 
692
  const double eps = std::max(DOLFIN_EPS_LARGE, DOLFIN_EPS_LARGE*std::max(dx, std::max(dy, dz)));
 
693
 
 
694
  // Check if point is inside cell
 
695
  return x1 >= -eps && x2 >= -eps && x3 >= -eps && x1 + x2 + x3 <= 1.0 + eps;
 
696
}
 
697
//-----------------------------------------------------------------------------
 
698
bool
 
699
CollisionDetection::collides_tetrahedron_triangle(const Point& p0,
 
700
                                                  const Point& p1,
 
701
                                                  const Point& p2,
 
702
                                                  const Point& p3,
 
703
                                                  const Point& q0,
 
704
                                                  const Point& q1,
 
705
                                                  const Point& q2)
 
706
{
 
707
  // Collision is determined by first if any triangle vertex is inside
 
708
  // the tetrahedron. If not, we continue checking the intersection of
 
709
  // the triangle with the four faces of the tetrahedron.
 
710
 
 
711
  // Triangle vertex in tetrahedron collision
 
712
  if (collides_tetrahedron_point(p0, p1, p2, p3, q0))
 
713
    return true;
 
714
  if (collides_tetrahedron_point(p0, p1, p2, p3, q1))
 
715
    return true;
 
716
  if (collides_tetrahedron_point(p0, p1, p2, p3, q2))
 
717
    return true;
 
718
 
 
719
  // Triangle-triangle collision tests
 
720
  if (collides_triangle_triangle(q0, q1, q2, p1, p2, p3))
 
721
    return true;
 
722
  if (collides_triangle_triangle(q0, q1, q2, p0, p2, p3))
 
723
    return true;
 
724
  if (collides_triangle_triangle(q0, q1, q2, p0, p1, p3))
 
725
    return true;
 
726
  if (collides_triangle_triangle(q0, q1, q2, p0, p1, p2))
 
727
    return true;
 
728
 
 
729
  return false;
 
730
}
 
731
//-----------------------------------------------------------------------------
 
732
bool CollisionDetection::edge_edge_test(int i0,
 
733
                                        int i1,
 
734
                                        double Ax,
 
735
                                        double Ay,
 
736
                                        const Point& V0,
 
737
                                        const Point& U0,
 
738
                                        const Point& U1)
 
739
{
 
740
  // Helper function for triangle triangle collision. Test edge vs
 
741
  // edge.
 
742
 
 
743
  // Here we have the option of classifying adjacent edges of two
 
744
  // triangles as colliding by changing > to >= and < to <= below.
 
745
 
 
746
  const double Bx = U0[i0] - U1[i0];
 
747
  const double By = U0[i1] - U1[i1];
 
748
  const double Cx = V0[i0] - U0[i0];
 
749
  const double Cy = V0[i1] - U0[i1];
 
750
  const double f = Ay*Bx - Ax*By;
 
751
  const double d = By*Cx - Bx*Cy;
 
752
 
 
753
  if ((f > 0 && d >= 0 && d <= f) ||
 
754
      (f < 0 && d <= 0 && d >= f))
 
755
  {
 
756
    const double e = Ax*Cy - Ay*Cx;
 
757
    if (f > 0)
 
758
    {
 
759
      // Allow or not allow adjacent edges as colliding:
 
760
      //if (e >= 0 && e <= f) return true;
 
761
      if (e > 0 && e < f)
 
762
        return true;
 
763
    }
 
764
    else
 
765
    {
 
766
      // Allow or not allow adjacent edges as colliding:
 
767
      //if (e <= 0 && e >= f) return true;
 
768
      if (e < 0 && e > f)
 
769
        return true;
 
770
    }
 
771
  }
 
772
  return false;
 
773
}
 
774
//-----------------------------------------------------------------------------
 
775
bool CollisionDetection::edge_against_tri_edges(int i0,
 
776
                                                int i1,
 
777
                                                const Point& V0,
 
778
                                                const Point& V1,
 
779
                                                const Point& U0,
 
780
                                                const Point& U1,
 
781
                                                const Point& U2)
 
782
{
 
783
  // Helper function for triangle triangle collision
 
784
  const double Ax = V1[i0] - V0[i0];
 
785
  const double Ay = V1[i1] - V0[i1];
 
786
 
 
787
  // Test edge U0,U1 against V0,V1
 
788
  if (edge_edge_test(i0, i1, Ax, Ay, V0, U0, U1))
 
789
    return true;
 
790
 
 
791
  // Test edge U1,U2 against V0,V1
 
792
  if (edge_edge_test(i0, i1, Ax, Ay, V0, U1, U2))
 
793
    return true;
 
794
 
 
795
  // Test edge U2,U1 against V0,V1
 
796
  if (edge_edge_test(i0, i1, Ax, Ay, V0, U2, U0))
 
797
    return true;
 
798
 
 
799
  return false;
 
800
}
 
801
//-----------------------------------------------------------------------------
 
802
bool CollisionDetection::point_in_tri(int i0,
 
803
                                      int i1,
 
804
                                      const Point& V0,
 
805
                                      const Point& U0,
 
806
                                      const Point& U1,
 
807
                                      const Point& U2)
 
808
{
 
809
  // Helper function for triangle triangle collision
 
810
  // Is T1 completly inside T2?
 
811
  // Check if V0 is inside tri(U0,U1,U2)
 
812
  double a = U1[i1] - U0[i1];
 
813
  double b = -(U1[i0] - U0[i0]);
 
814
  double c = -a*U0[i0] - b*U0[i1];
 
815
  const double d0 = a*V0[i0] + b*V0[i1] + c;
 
816
 
 
817
  a = U2[i1] - U1[i1];
 
818
  b = -(U2[i0] - U1[i0]);
 
819
  c = -a*U1[i0] - b*U1[i1];
 
820
  const double d1 = a*V0[i0] + b*V0[i1] + c;
 
821
 
 
822
  a = U0[i1] - U2[i1];
 
823
  b = -(U0[i0] - U2[i0]);
 
824
  c = -a*U2[i0] - b*U2[i1];
 
825
  const double d2 = a*V0[i0] + b*V0[i1] + c;
 
826
 
 
827
  if (d0*d1 > 0.)
 
828
  {
 
829
    if (d0*d2 > 0.)
 
830
      return true;
 
831
  }
 
832
 
 
833
  return false;
 
834
}
 
835
//-----------------------------------------------------------------------------
 
836
bool CollisionDetection::coplanar_tri_tri(const Point& N,
 
837
                                          const Point& V0,
 
838
                                          const Point& V1,
 
839
                                          const Point& V2,
 
840
                                          const Point& U0,
 
841
                                          const Point& U1,
 
842
                                          const Point& U2)
 
843
{
 
844
  // Helper function for triangle triangle collision
 
845
 
 
846
  double A[3];
 
847
  int i0,i1;
 
848
 
 
849
  // First project onto an axis-aligned plane, that maximizes the area
 
850
  // of the triangles, compute indices: i0,i1.
 
851
  A[0] = std::abs(N[0]);
 
852
  A[1] = std::abs(N[1]);
 
853
  A[2] = std::abs(N[2]);
 
854
 
 
855
  if (A[0] > A[1])
 
856
  {
 
857
    if (A[0] > A[2])
 
858
    {
 
859
      i0 = 1;      // A[0] is greatest
 
860
      i1 = 2;
 
861
    }
 
862
    else
 
863
    {
 
864
      i0 = 0;      // A[2] is greatest
 
865
      i1 = 1;
 
866
    }
 
867
  }
 
868
  else   // A[0]<=A[1]
 
869
  {
 
870
    if (A[2] > A[1])
 
871
    {
 
872
      i0 = 0;      // A[2] is greatest
 
873
      i1 = 1;
 
874
    }
 
875
    else
 
876
    {
 
877
      i0 = 0;      // A[1] is greatest
 
878
      i1 = 2;
 
879
    }
 
880
  }
 
881
 
 
882
  // Test all edges of triangle 1 against the edges of triangle 2
 
883
  if (edge_against_tri_edges(i0, i1, V0, V1, U0, U1, U2))
 
884
    return true;
 
885
  if (edge_against_tri_edges(i0, i1, V1, V2, U0, U1, U2))
 
886
    return true;
 
887
  if (edge_against_tri_edges(i0, i1, V2, V0, U0, U1, U2))
 
888
    return true;
 
889
 
 
890
  // Finally, test if tri1 is totally contained in tri2 or vice versa
 
891
  if (point_in_tri(i0, i1, V0, U0, U1, U2))
 
892
    return true;
 
893
  if (point_in_tri(i0, i1, U0, V0, V1, V2))
 
894
    return true;
 
895
 
 
896
  return false;
 
897
}
 
898
//-----------------------------------------------------------------------------
 
899
bool CollisionDetection::compute_intervals(double VV0,
 
900
                                           double VV1,
 
901
                                           double VV2,
 
902
                                           double D0,
 
903
                                           double D1,
 
904
                                           double D2,
 
905
                                           double D0D1,
 
906
                                           double D0D2,
 
907
                                           double& A,
 
908
                                           double& B,
 
909
                                           double& C,
 
910
                                           double& X0,
 
911
                                           double& X1)
 
912
{
 
913
  // Helper function for triangle triangle collision
 
914
 
 
915
  if (D0D1 > 0.)
 
916
  {
 
917
    // Here we know that D0D2<=0.0, that is D0, D1 are on the same
 
918
    // side, D2 on the other or on the plane
 
919
    A = VV2;
 
920
    B = (VV0 - VV2)*D2;
 
921
    C = (VV1 - VV2)*D2;
 
922
    X0 = D2 - D0;
 
923
    X1 = D2 - D1;
 
924
  }
 
925
  else if (D0D2 > 0.)
 
926
  {
 
927
    // Here we know that d0d1<=0.0
 
928
    A = VV1;
 
929
    B = (VV0 - VV1)*D1;
 
930
    C = (VV2 - VV1)*D1;
 
931
    X0 = D1 - D0;
 
932
    X1 = D1 - D2;
 
933
  }
 
934
  else if (D1*D2 > 0. || D0 != 0.)
 
935
  {
 
936
    // Here we know that d0d1<=0.0 or that D0!=0.0
 
937
    A = VV0;
 
938
    B = (VV1 - VV0)*D0;
 
939
    C = (VV2 - VV0)*D0;
 
940
    X0 = D0 - D1;
 
941
    X1 = D0 - D2;
 
942
  }
 
943
  else if (D1 != 0.)
 
944
  {
 
945
    A = VV1;
 
946
    B = (VV0 - VV1)*D1;
 
947
    C = (VV2 - VV1)*D1;
 
948
    X0 = D1 - D0;
 
949
    X1 = D1 - D2;
 
950
  }
 
951
  else if (D2 != 0.)
 
952
  {
 
953
    A = VV2;
 
954
    B = (VV0 - VV2)*D2;
 
955
    C = (VV1 - VV2)*D2;
 
956
    X0 = D2 - D0;
 
957
    X1 = D2 - D1;
 
958
  }
 
959
  else {
 
960
    // Go to coplanar test
 
961
    return true;
 
962
  }
 
963
 
 
964
  return false;
 
965
}
 
966
//-----------------------------------------------------------------------------
 
967
bool
 
968
CollisionDetection::separating_plane_face_A_1(const std::vector<Point>& pv1,
 
969
                                              const Point& n,
 
970
                                              std::vector<double>& coord,
 
971
                                              int&  mask_edges)
 
972
{
 
973
  // Helper function for tetrahedron-tetrahedron collision test:
 
974
  // checks if plane pv1 is a separating plane. Stores local
 
975
  // coordinates and the mask bit mask_edges.
 
976
 
 
977
  mask_edges = 0;
 
978
  const int shifts[4] = {1, 2, 4, 8};
 
979
 
 
980
  for (std::size_t i = 0; i < 4; ++i)
 
981
  {
 
982
    coord[i] = pv1[i].dot(n);
 
983
    if (coord[i] > 0)
 
984
      mask_edges |= shifts[i];
 
985
  }
 
986
 
 
987
  return (mask_edges == 15);
 
988
}
 
989
//-----------------------------------------------------------------------------
 
990
bool
 
991
CollisionDetection::separating_plane_face_A_2(const std::vector<Point>& V1,
 
992
                                              const std::vector<Point>& V2,
 
993
                                              const Point& n,
 
994
                                              std::vector<double>& coord,
 
995
                                              int&  mask_edges)
 
996
{
 
997
  // Helper function for tetrahedron-tetrahedron collision test:
 
998
  // checks if plane v1,v2 is a separating plane. Stores local
 
999
  // coordinates and the mask bit mask_edges.
 
1000
 
 
1001
  mask_edges = 0;
 
1002
  const int shifts[4] = {1, 2, 4, 8};
 
1003
 
 
1004
  for (std::size_t i = 0; i < 4; ++i)
 
1005
  {
 
1006
    coord[i] = (V2[i] - V1[1]).dot(n);
 
1007
    if (coord[i] > 0)
 
1008
      mask_edges |= shifts[i];
 
1009
  }
 
1010
 
 
1011
  return (mask_edges == 15);
 
1012
}
 
1013
//-----------------------------------------------------------------------------
 
1014
bool
 
1015
CollisionDetection::separating_plane_edge_A
 
1016
(const std::vector<std::vector<double> >& coord_1,
 
1017
 const std::vector<int>& masks,
 
1018
 int f0,
 
1019
 int f1)
 
1020
{
 
1021
  // Helper function for tetrahedron-tetrahedron collision: checks if
 
1022
  // edge is in the plane separating faces f0 and f1.
 
1023
 
 
1024
  const std::vector<double>& coord_f0 = coord_1[f0];
 
1025
  const std::vector<double>& coord_f1 = coord_1[f1];
 
1026
 
 
1027
  int maskf0 = masks[f0];
 
1028
  int maskf1 = masks[f1];
 
1029
 
 
1030
  if ((maskf0 | maskf1) != 15) // if there is a vertex of b
 
1031
    return false; // included in (-,-) return false
 
1032
 
 
1033
  maskf0 &= (maskf0 ^ maskf1); // exclude the vertices in (+,+)
 
1034
  maskf1 &= (maskf0 ^ maskf1);
 
1035
 
 
1036
  // edge 0: 0--1
 
1037
  if ((maskf0 & 1) && // the vertex 0 of b is in (-,+)
 
1038
      (maskf1 & 2)) // the vertex 1 of b is in (+,-)
 
1039
    if ((coord_f0[1]*coord_f1[0] - coord_f0[0]*coord_f1[1]) > 0)
 
1040
      // the edge of b (0,1) intersect (-,-) (see the paper)
 
1041
      return false;
 
1042
 
 
1043
  if ((maskf0 & 2) &&
 
1044
      (maskf1 & 1))
 
1045
    if ((coord_f0[1]*coord_f1[0] - coord_f0[0]*coord_f1[1]) < 0)
 
1046
      return false;
 
1047
 
 
1048
  // edge 1: 0--2
 
1049
  if ((maskf0 & 1) &&
 
1050
      (maskf1 & 4))
 
1051
    if ((coord_f0[2]*coord_f1[0] - coord_f0[0]*coord_f1[2]) > 0)
 
1052
      return false;
 
1053
 
 
1054
  if ((maskf0 & 4) &&
 
1055
      (maskf1 & 1))
 
1056
    if ((coord_f0[2]*coord_f1[0] - coord_f0[0]*coord_f1[2]) < 0)
 
1057
      return false;
 
1058
 
 
1059
  // edge 2: 0--3
 
1060
  if ((maskf0 & 1) &&
 
1061
      (maskf1 & 8))
 
1062
    if ((coord_f0[3]*coord_f1[0] - coord_f0[0]*coord_f1[3]) > 0)
 
1063
      return false;
 
1064
 
 
1065
  if ((maskf0 & 8) &&
 
1066
      (maskf1 & 1))
 
1067
    if ((coord_f0[3]*coord_f1[0] - coord_f0[0]*coord_f1[3]) < 0)
 
1068
      return false;
 
1069
 
 
1070
  // edge 3: 1--2
 
1071
  if ((maskf0 & 2) &&
 
1072
      (maskf1 & 4))
 
1073
    if ((coord_f0[2]*coord_f1[1] - coord_f0[1]*coord_f1[2]) > 0)
 
1074
      return false;
 
1075
 
 
1076
  if ((maskf0 & 4) &&
 
1077
      (maskf1 & 2))
 
1078
    if ((coord_f0[2]*coord_f1[1] - coord_f0[1]*coord_f1[2]) < 0)
 
1079
      return false;
 
1080
 
 
1081
  // edge 4: 1--3
 
1082
  if ((maskf0 & 2) &&
 
1083
      (maskf1 & 8))
 
1084
    if ((coord_f0[3]*coord_f1[1] - coord_f0[1]*coord_f1[3]) > 0)
 
1085
      return false;
 
1086
 
 
1087
  if ((maskf0 & 8) &&
 
1088
      (maskf1 & 2))
 
1089
    if ((coord_f0[3]*coord_f1[1] - coord_f0[1]*coord_f1[3]) < 0)
 
1090
      return false;
 
1091
 
 
1092
  // edge 5: 2--3
 
1093
  if ((maskf0 & 4) &&
 
1094
      (maskf1 & 8))
 
1095
    if ((coord_f0[3]*coord_f1[2] - coord_f0[2]*coord_f1[3]) > 0)
 
1096
      return false;
 
1097
 
 
1098
  if ((maskf0 & 8) &&
 
1099
      (maskf1 & 4))
 
1100
    if ((coord_f0[3]*coord_f1[2] - coord_f0[2]*coord_f1[3]) < 0)
 
1101
      return false;
 
1102
 
 
1103
  // Now there exists a separating plane supported by the edge shared
 
1104
  // by f0 and f1.
 
1105
  return true;
 
1106
}
 
1107
//-----------------------------------------------------------------------------