//-----------------------------------------------------------------------------
// High-level collision detection predicates
//-----------------------------------------------------------------------------
bool CollisionPredicates::collides(const MeshEntity& entity,
const Point& point)
{
// Intersection is only implemented for simplex meshes
if (!entity.mesh().type().is_simplex())
{
dolfin_error("Cell.cpp",
"intersect cell and point",
"Intersection is only implemented for simplex meshes");
}
// Get data
const MeshGeometry& g = entity.mesh().geometry();
const unsigned int* v = entity.entities(0);
const std::size_t tdim = entity.mesh().topology().dim();
const std::size_t gdim = entity.mesh().geometry().dim();
// Pick correct specialized implementation
if (tdim == 1 && gdim == 1)
return collides_segment_point_1d(g.point(v[0])[0], g.point(v[1])[0], point[0]);
if (tdim == 1 && gdim == 2)
return collides_segment_point_2d(g.point(v[0]), g.point(v[1]), point);
if (tdim == 1 && gdim == 3)
return collides_segment_point_3d(g.point(v[0]), g.point(v[1]), point);
if (tdim == 2 && gdim == 2)
return collides_triangle_point_2d(g.point(v[0]),
g.point(v[1]),
g.point(v[2]),
point);
if (tdim == 2 && gdim == 3)
return collides_triangle_point_3d(g.point(v[0]),
g.point(v[1]),
g.point(v[2]),
point);
if (tdim == 3)
return collides_tetrahedron_point_3d(g.point(v[0]),
g.point(v[1]),
g.point(v[2]),
g.point(v[3]),
point);
dolfin_error("CollisionPredicates.cpp",
"compute entity-point collision",
"Not implemented for dimensions %d / %d", tdim, gdim);
return false;
}
//-----------------------------------------------------------------------------
double HexahedronCell::volume(const MeshEntity& cell) const
{
if (cell.dim() != 2)
{
dolfin_error("HexahedronCell.cpp",
"compute volume (area) of cell",
"Illegal mesh entity");
}
// Get mesh geometry
const MeshGeometry& geometry = cell.mesh().geometry();
// Get the coordinates of the four vertices
const unsigned int* vertices = cell.entities(0);
const Point p0 = geometry.point(vertices[0]);
const Point p1 = geometry.point(vertices[1]);
const Point p2 = geometry.point(vertices[2]);
const Point p3 = geometry.point(vertices[3]);
const Point p4 = geometry.point(vertices[4]);
const Point p5 = geometry.point(vertices[5]);
dolfin_error("HexahedronCell.cpp",
"compute volume of hexahedron",
"Not implemented");
return 0.0;
}
//-----------------------------------------------------------------------------
double QuadrilateralCell::volume(const MeshEntity& cell) const
{
if (cell.dim() != 2)
{
dolfin_error("QuadrilateralCell.cpp",
"compute volume (area) of cell",
"Illegal mesh entity");
}
// Get mesh geometry
const MeshGeometry& geometry = cell.mesh().geometry();
// Get the coordinates of the four vertices
const unsigned int* vertices = cell.entities(0);
const Point p0 = geometry.point(vertices[0]);
const Point p1 = geometry.point(vertices[1]);
const Point p2 = geometry.point(vertices[2]);
const Point p3 = geometry.point(vertices[3]);
if (geometry.dim() == 2)
{
const Point c = (p0 - p2).cross(p1 - p3);
return 0.5 * c.norm();
}
else
dolfin_error("QuadrilateralCell.cpp",
"compute volume of quadrilateral",
"Only know how to compute volume in R^2");
// FIXME: could work in R^3 but need to check co-planarity
return 0.0;
}
//-----------------------------------------------------------------------------
bool
CollisionDetection::collides_tetrahedron_point(const MeshEntity& tetrahedron,
const Point& point)
{
dolfin_assert(tetrahedron.mesh().topology().dim() == 3);
// Get the vertices as points
const MeshGeometry& geometry = tetrahedron.mesh().geometry();
const unsigned int* vertices = tetrahedron.entities(0);
return collides_tetrahedron_point(geometry.point(vertices[0]),
geometry.point(vertices[1]),
geometry.point(vertices[2]),
geometry.point(vertices[3]),
point);
}
//-----------------------------------------------------------------------------
double IntervalCell::volume(const MeshEntity& interval) const
{
// Check that we get an interval
if (interval.dim() != 1)
{
dolfin_error("IntervalCell.cpp",
"compute volume (length) of interval cell",
"Illegal mesh entity, not an interval");
}
// Get mesh geometry
const MeshGeometry& geometry = interval.mesh().geometry();
// Get the coordinates of the two vertices
const unsigned int* vertices = interval.entities(0);
const double* x0 = geometry.x(vertices[0]);
const double* x1 = geometry.x(vertices[1]);
// Compute length of interval (line segment)
double sum = 0.0;
for (std::size_t i = 0; i < geometry.dim(); ++i)
{
const double dx = x1[i] - x0[i];
sum += dx*dx;
}
return std::sqrt(sum);
}
//-----------------------------------------------------------------------------
bool CollisionDetection::collides_interval_point(const MeshEntity& entity,
const Point& point)
{
// Get coordinates
const MeshGeometry& geometry = entity.mesh().geometry();
const unsigned int* vertices = entity.entities(0);
return collides_interval_point(geometry.point(vertices[0]),
geometry.point(vertices[1]),
point);
}
//-----------------------------------------------------------------------------
bool
CollisionDetection::collides_triangle_triangle(const MeshEntity& triangle_0,
const MeshEntity& triangle_1)
{
dolfin_assert(triangle_0.mesh().topology().dim() == 2);
dolfin_assert(triangle_1.mesh().topology().dim() == 2);
// Get vertices as points
const MeshGeometry& geometry_0 = triangle_0.mesh().geometry();
const unsigned int* vertices_0 = triangle_0.entities(0);
const MeshGeometry& geometry_1 = triangle_1.mesh().geometry();
const unsigned int* vertices_1 = triangle_1.entities(0);
return collides_triangle_triangle(geometry_0.point(vertices_0[0]),
geometry_0.point(vertices_0[1]),
geometry_0.point(vertices_0[2]),
geometry_1.point(vertices_1[0]),
geometry_1.point(vertices_1[1]),
geometry_1.point(vertices_1[2]));
}
//-----------------------------------------------------------------------------
bool CollisionDetection::collides_triangle_point(const MeshEntity& triangle,
const Point& point)
{
dolfin_assert(triangle.mesh().topology().dim() == 2);
const MeshGeometry& geometry = triangle.mesh().geometry();
const unsigned int* vertices = triangle.entities(0);
if (triangle.mesh().geometry().dim() == 2)
return collides_triangle_point_2d(geometry.point(vertices[0]),
geometry.point(vertices[1]),
geometry.point(vertices[2]),
point);
else
return collides_triangle_point(geometry.point(vertices[0]),
geometry.point(vertices[1]),
geometry.point(vertices[2]),
point);
}
//-----------------------------------------------------------------------------
double TriangleCell::volume(const MeshEntity& triangle) const
{
// Check that we get a triangle
if (triangle.dim() != 2)
{
dolfin_error("TriangleCell.cpp",
"compute volume (area) of triangle cell",
"Illegal mesh entity, not a triangle");
}
// Get mesh geometry
const MeshGeometry& geometry = triangle.mesh().geometry();
// Get the coordinates of the three vertices
const unsigned int* vertices = triangle.entities(0);
const double* x0 = geometry.x(vertices[0]);
const double* x1 = geometry.x(vertices[1]);
const double* x2 = geometry.x(vertices[2]);
if (geometry.dim() == 2)
{
// Compute area of triangle embedded in R^2
double v2 = (x0[0]*x1[1] + x0[1]*x2[0] + x1[0]*x2[1])
- (x2[0]*x1[1] + x2[1]*x0[0] + x1[0]*x0[1]);
// Formula for volume from http://mathworld.wolfram.com
return 0.5 * std::abs(v2);
}
else if (geometry.dim() == 3)
{
// Compute area of triangle embedded in R^3
const double v0 = (x0[1]*x1[2] + x0[2]*x2[1] + x1[1]*x2[2])
- (x2[1]*x1[2] + x2[2]*x0[1] + x1[1]*x0[2]);
const double v1 = (x0[2]*x1[0] + x0[0]*x2[2] + x1[2]*x2[0])
- (x2[2]*x1[0] + x2[0]*x0[2] + x1[2]*x0[0]);
const double v2 = (x0[0]*x1[1] + x0[1]*x2[0] + x1[0]*x2[1])
- (x2[0]*x1[1] + x2[1]*x0[0] + x1[0]*x0[1]);
// Formula for volume from http://mathworld.wolfram.com
return 0.5*sqrt(v0*v0 + v1*v1 + v2*v2);
}
else
{
dolfin_error("TriangleCell.cpp",
"compute volume of triangle",
"Only know how to compute volume when embedded in R^2 or R^3");
}
return 0.0;
}
//-----------------------------------------------------------------------------
bool
CollisionDetection::collides_tetrahedron_triangle(const MeshEntity& tetrahedron,
const MeshEntity& triangle)
{
dolfin_assert(tetrahedron.mesh().topology().dim() == 3);
dolfin_assert(triangle.mesh().topology().dim() == 2);
// Get the vertices of the tetrahedron as points
const MeshGeometry& geometry_tet = tetrahedron.mesh().geometry();
const unsigned int* vertices_tet = tetrahedron.entities(0);
// Get the vertices of the triangle as points
const MeshGeometry& geometry_tri = triangle.mesh().geometry();
const unsigned int* vertices_tri = triangle.entities(0);
return collides_tetrahedron_triangle(geometry_tet.point(vertices_tet[0]),
geometry_tet.point(vertices_tet[1]),
geometry_tet.point(vertices_tet[2]),
geometry_tet.point(vertices_tet[3]),
geometry_tri.point(vertices_tri[0]),
geometry_tri.point(vertices_tri[1]),
geometry_tri.point(vertices_tri[2]));
}
//-----------------------------------------------------------------------------
bool
CollisionDetection::collides_interval_interval(const MeshEntity& interval_0,
const MeshEntity& interval_1)
{
// Get coordinates
const MeshGeometry& geometry_0 = interval_0.mesh().geometry();
const MeshGeometry& geometry_1 = interval_1.mesh().geometry();
const unsigned int* vertices_0 = interval_0.entities(0);
const unsigned int* vertices_1 = interval_1.entities(0);
const double x00 = geometry_0.point(vertices_0[0])[0];
const double x01 = geometry_0.point(vertices_0[1])[0];
const double x10 = geometry_1.point(vertices_1[0])[0];
const double x11 = geometry_1.point(vertices_1[1])[0];
const double a0 = std::min(x00, x01);
const double b0 = std::max(x00, x01);
const double a1 = std::min(x10, x11);
const double b1 = std::max(x10, x11);
// Check for collisions
const double dx = std::min(b0 - a0, b1 - a1);
const double eps = std::max(DOLFIN_EPS_LARGE, DOLFIN_EPS_LARGE*dx);
return b1 > a0 - eps && a1 < b0 + eps;
}
//-----------------------------------------------------------------------------
double IntervalCell::volume(const MeshEntity& interval) const
{
// Check that we get an interval
if (interval.dim() != 1)
{
dolfin_error("IntervalCell.cpp",
"compute volume (length) of interval cell",
"Illegal mesh entity, not an interval");
}
// Get mesh geometry
const MeshGeometry& geometry = interval.mesh().geometry();
// Get the coordinates of the two vertices
const unsigned int* vertices = interval.entities(0);
const Point x0 = geometry.point(vertices[0]);
const Point x1 = geometry.point(vertices[1]);
return x1.distance(x0);
}
//-----------------------------------------------------------------------------
double TriangleCell::diameter(const MeshEntity& triangle) const
{
// Check that we get a triangle
if (triangle.dim() != 2)
{
dolfin_error("TriangleCell.cpp",
"compute diameter of triangle cell",
"Illegal mesh entity, not a triangle");
}
// Get mesh geometry
const MeshGeometry& geometry = triangle.mesh().geometry();
// Only know how to compute the diameter when embedded in R^2 or R^3
if (geometry.dim() != 2 && geometry.dim() != 3)
dolfin_error("TriangleCell.cpp",
"compute diameter of triangle",
"Only know how to compute diameter when embedded in R^2 or R^3");
// Get the coordinates of the three vertices
const unsigned int* vertices = triangle.entities(0);
const Point p0 = geometry.point(vertices[0]);
const Point p1 = geometry.point(vertices[1]);
const Point p2 = geometry.point(vertices[2]);
// FIXME: Assuming 3D coordinates, could be more efficient if
// FIXME: if we assumed 2D coordinates in 2D
// Compute side lengths
const double a = p1.distance(p2);
const double b = p0.distance(p2);
const double c = p0.distance(p1);
// Formula for diameter (2*circumradius) from http://mathworld.wolfram.com
return 0.5*a*b*c / volume(triangle);
}
//-----------------------------------------------------------------------------
bool CollisionPredicates::collides(const MeshEntity& entity_0,
const MeshEntity& entity_1)
{
// Intersection is only implemented for simplex meshes
if (!entity_0.mesh().type().is_simplex() ||
!entity_1.mesh().type().is_simplex())
{
dolfin_error("Cell.cpp",
"intersect cell and point",
"intersection is only implemented for simplex meshes");
}
// Get data
const MeshGeometry& g0 = entity_0.mesh().geometry();
const MeshGeometry& g1 = entity_1.mesh().geometry();
const unsigned int* v0 = entity_0.entities(0);
const unsigned int* v1 = entity_1.entities(0);
const std::size_t d0 = entity_0.dim();
const std::size_t d1 = entity_1.dim();
const std::size_t gdim = g0.dim();
dolfin_assert(gdim == g1.dim());
// Pick correct specialized implementation
if (d0 == 1 && d1 == 1)
{
return collides_segment_segment(g0.point(v0[0]),
g0.point(v0[1]),
g1.point(v1[0]),
g1.point(v1[1]),
gdim);
}
if (d0 == 1 && d1 == 2)
{
return collides_triangle_segment(g1.point(v1[0]),
g1.point(v1[1]),
g1.point(v1[2]),
g0.point(v0[0]),
g0.point(v0[1]),
gdim);
}
if (d0 == 2 && d1 == 1)
{
return collides_triangle_segment(g0.point(v0[0]),
g0.point(v0[1]),
g0.point(v0[2]),
g1.point(v1[0]),
g1.point(v1[1]),
gdim);
}
if (d0 == 2 && d1 == 2)
{
return collides_triangle_triangle(g0.point(v0[0]),
g0.point(v0[1]),
g0.point(v0[2]),
g1.point(v1[0]),
g1.point(v1[1]),
g1.point(v1[2]),
gdim);
}
if (d0 == 2 && d1 == 3)
{
return collides_tetrahedron_triangle_3d(g1.point(v1[0]),
g1.point(v1[1]),
g1.point(v1[2]),
g1.point(v1[3]),
g0.point(v0[0]),
g0.point(v0[1]),
g0.point(v0[2]));
}
if (d0 == 3 && d1 == 2)
{
return collides_tetrahedron_triangle_3d(g0.point(v0[0]),
g0.point(v0[1]),
g0.point(v0[2]),
g0.point(v0[3]),
g1.point(v1[0]),
g1.point(v1[1]),
g1.point(v1[2]));
}
if (d0 == 3 && d1 == 3)
{
return collides_tetrahedron_tetrahedron_3d(g0.point(v0[0]),
g0.point(v0[1]),
g0.point(v0[2]),
g0.point(v0[3]),
g1.point(v1[0]),
g1.point(v1[1]),
g1.point(v1[2]),
g1.point(v1[3]));
}
dolfin_error("CollisionPredicates.cpp",
"compute entity-entity collision",
"Not implemented for topological dimensions %d / %d and geometrical dimension %d", d0, d1, gdim);
return false;
}
//-----------------------------------------------------------------------------
bool
CollisionDetection::collides_tetrahedron_tetrahedron
(const MeshEntity& tetrahedron_0,
const MeshEntity& tetrahedron_1)
{
// This algorithm checks whether two tetrahedra intersect.
// Algorithm and source code from Fabio Ganovelli, Federico Ponchio
// and Claudio Rocchini: Fast Tetrahedron-Tetrahedron Overlap
// Algorithm, Journal of Graphics Tools, 7(2), 2002. DOI:
// 10.1080/10867651.2002.10487557. Source code available at
// http://web.archive.org/web/20031130075955/http://www.acm.org/jgt/papers/GanovelliPonchioRocchini02/tet_a_tet.html
dolfin_assert(tetrahedron_0.mesh().topology().dim() == 3);
dolfin_assert(tetrahedron_1.mesh().topology().dim() == 3);
// Get the vertices as points
const MeshGeometry& geometry = tetrahedron_0.mesh().geometry();
const unsigned int* vertices = tetrahedron_0.entities(0);
const MeshGeometry& geometry_q = tetrahedron_1.mesh().geometry();
const unsigned int* vertices_q = tetrahedron_1.entities(0);
std::vector<Point> V1(4), V2(4);
for (std::size_t i = 0; i < 4; ++i)
{
V1[i] = geometry.point(vertices[i]);
V2[i] = geometry_q.point(vertices_q[i]);
}
// Get the vectors between V2 and V1[0]
std::vector<Point> P_V1(4);
for (std::size_t i = 0; i < 4; ++i)
P_V1[i] = V2[i]-V1[0];
// Data structure for edges of V1 and V2
std::vector<Point> e_v1(5), e_v2(5);
e_v1[0] = V1[1] - V1[0];
e_v1[1] = V1[2] - V1[0];
e_v1[2] = V1[3] - V1[0];
Point n = e_v1[1].cross(e_v1[0]);
// Maybe flip normal. Normal should be outward.
if (n.dot(e_v1[2]) > 0)
n *= -1;
std::vector<int> masks(4);
std::vector<std::vector<double>> Coord_1(4, std::vector<double>(4));
if (separating_plane_face_A_1(P_V1, n, Coord_1[0], masks[0]))
return false;
n = e_v1[0].cross(e_v1[2]);
// Maybe flip normal
if (n.dot(e_v1[1]) > 0)
n *= -1;
if (separating_plane_face_A_1(P_V1, n, Coord_1[1], masks[1]))
return false;
if (separating_plane_edge_A(Coord_1, masks, 0, 1))
return false;
n = e_v1[2].cross(e_v1[1]);
// Maybe flip normal
if (n.dot(e_v1[0]) > 0)
n *= -1;
if (separating_plane_face_A_1(P_V1, n, Coord_1[2], masks[2]))
return false;
if (separating_plane_edge_A(Coord_1, masks, 0, 2))
return false;
if (separating_plane_edge_A(Coord_1, masks, 1,2))
return false;
e_v1[4] = V1[3] - V1[1];
e_v1[3] = V1[2] - V1[1];
n = e_v1[3].cross(e_v1[4]);
// Maybe flip normal. Note the < since e_v1[0]=v1-v0.
if (n.dot(e_v1[0]) < 0)
n *= -1;
if (separating_plane_face_A_2(V1, V2, n, Coord_1[3], masks[3]))
return false;
if (separating_plane_edge_A(Coord_1, masks, 0, 3))
return false;
if (separating_plane_edge_A(Coord_1, masks, 1, 3))
return false;
if (separating_plane_edge_A(Coord_1, masks, 2, 3))
return false;
if ((masks[0] | masks[1] | masks[2] | masks[3] )!= 15)
return true;
// From now on, if there is a separating plane, it is parallel to a
// face of b.
std::vector<Point> P_V2(4);
for (std::size_t i = 0; i < 4; ++i)
P_V2[i] = V1[i] - V2[0];
e_v2[0] = V2[1] - V2[0];
e_v2[1] = V2[2] - V2[0];
e_v2[2] = V2[3] - V2[0];
n = e_v2[1].cross(e_v2[0]);
// Maybe flip normal
if (n.dot(e_v2[2])>0)
n *= -1;
if (separating_plane_face_B_1(P_V2, n))
return false;
n=e_v2[0].cross(e_v2[2]);
// Maybe flip normal
if (n.dot(e_v2[1]) > 0)
n *= -1;
if (separating_plane_face_B_1(P_V2, n))
return false;
n = e_v2[2].cross(e_v2[1]);
// Maybe flip normal
if (n.dot(e_v2[0]) > 0)
n *= -1;
if (separating_plane_face_B_1(P_V2, n))
return false;
e_v2[4] = V2[3] - V2[1];
e_v2[3] = V2[2] - V2[1];
n = e_v2[3].cross(e_v2[4]);
// Maybe flip normal. Note the < since e_v2[0] = V2[1] - V2[0].
if (n.dot(e_v2[0]) < 0)
n *= -1;
if (separating_plane_face_B_2(V1, V2, n))
return false;
return true;
}
