//----------------------------------------------------------------------------- // 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; }