diff --git a/GeoModelCore/GeoModelKernel/GeoModelKernel/GeoPolyhedronGeneric.h b/GeoModelCore/GeoModelKernel/GeoModelKernel/GeoPolyhedronGeneric.h new file mode 100644 index 0000000000000000000000000000000000000000..3745ed823bd3b748dc4abea7d0d73f6c7d5dc8e3 --- /dev/null +++ b/GeoModelCore/GeoModelKernel/GeoModelKernel/GeoPolyhedronGeneric.h @@ -0,0 +1,119 @@ +/* + Copyright (C) 2002-2024 CERN for the benefit of the ATLAS collaboration +*/ + +#ifndef GEOMODELKERNEL_GEOPOLYHEDRONGENERIC_H +#define GEOMODELKERNEL_GEOPOLYHEDRONGENERIC_H + +/** + * @class: GeoPolyhedronGeneric + * + * @brief This shape represents a generic polyhedron, described by an array of vertices + * and faces defined by a set of ordered vertices + */ + +#include <vector> +#include "GeoModelKernel/GeoShape.h" + +class GeoPolyhedronGeneric : public GeoShape { + public: + // Default constructor + GeoPolyhedronGeneric(){}; + + // Constructor + GeoPolyhedronGeneric(const GeoShape*); + + // Returns the volume of the shape, for mass inventory + virtual double volume() const; + + // Returns the bonding box of the shape + virtual void extent (double& xmin, double& ymin, double& zmin, + double& xmax, double& ymax, double& zmax) const; + + // Returns true if the shape contains the point, false otherwise. + virtual bool contains (double x, double y, double z) const; + + // Returns true if the shape contains the point, false otherwise. + virtual bool contains (GeoTrf::Vector3D point, bool debugMode = false) const; + + // Returns the shape type, as a string + virtual const std::string& type() const { + return getClassType(); + } + + // Returns the shape type, as a coded integer + virtual ShapeType typeID() const{ + return getClassTypeID(); + } + + // For type identification + static const std::string& getClassType() { + return s_classType; + } + + // For type identification + static ShapeType getClassTypeID() { + return s_classTypeID; + } + + // Executes a GeoShapeAction + virtual void exec(GeoShapeAction *action) const; + + // Define vertices + void addVertices(std::vector<GeoTrf::Vector3D> vertices); + + // Add another facet to the polyhedron. A minimum of four + // faces are required to create a valid solid. + void addFace(std::vector<size_t> face); + + // normals can be uploaded or calculated + void addNormal(GeoTrf::Vector3D n); + + // Returns the number of vertices of the polyhedron + unsigned int getNVertices() const { + return m_Vertices.size(); + } + + // Returns the number of (boundary) faces of the polyhedron + unsigned int getNFaces() const { + return m_Faces.size(); + } + + // check of the hermeticity of the polyhedron (all edge need to be mirrored), plus at least four faces required, false otherwise + bool isValid () const; + + // Returns coordinates of the specified vertex + GeoTrf::Vector3D getVertex(unsigned int i) const { + return m_Vertices.at(i); + } + + // Returns definition of the specified face + std::vector<size_t> getFace(unsigned int i) const { + return m_Faces.at(i); + } + + // return normal ( pointing outwards ) + GeoTrf::Vector3D faceNormal(unsigned int i) const; + + // true for point laying on the face + bool insideFace(unsigned int i, GeoTrf::Vector3D point) const; + + // method to intersect ( extra-polyhedron) edge with face + std::vector< std::pair< double, std::pair<size_t,size_t> > > edgeFaceIntersect( int i, GeoTrf::Vector3D eInit, GeoTrf::Vector3D edge ) const; + + virtual ~GeoPolyhedronGeneric() = default; +protected: + + private: + static const std::string s_classType; + static const ShapeType s_classTypeID; + + std::vector<GeoTrf::Vector3D> m_Vertices{}; + std::vector< std::vector<size_t> > m_Faces{}; + mutable std::vector<GeoTrf::Vector3D> m_Normals{}; + + double intersectLines(GeoTrf::Vector3D A, GeoTrf::Vector3D a, GeoTrf::Vector3D B, GeoTrf::Vector3D b, double& da, double& db) const; + +}; + +#endif diff --git a/GeoModelCore/GeoModelKernel/src/GeoPolyhedronGeneric.cxx b/GeoModelCore/GeoModelKernel/src/GeoPolyhedronGeneric.cxx new file mode 100755 index 0000000000000000000000000000000000000000..90b6230003a3b61cfc85a1d5cacaea5ef7ebcbd5 --- /dev/null +++ b/GeoModelCore/GeoModelKernel/src/GeoPolyhedronGeneric.cxx @@ -0,0 +1,405 @@ +/* + Copyright (C) 2002-2017 CERN for the benefit of the ATLAS collaboration +*/ + +#include "GeoModelKernel/GeoPolyhedronGeneric.h" +#include "GeoModelKernel/GeoShapeAction.h" +#include "GeoModelKernel/GeoBox.h" +#include "GeoModelKernel/GeoTrd.h" +#include "GeoModelKernel/GeoSimplePolygonBrep.h" +#include <cmath> +#include <stdexcept> +#include <iostream> + +const std::string GeoPolyhedronGeneric::s_classType = "PolyhedronGeneric"; +const ShapeType GeoPolyhedronGeneric::s_classTypeID = 0x25; + +GeoPolyhedronGeneric::GeoPolyhedronGeneric(const GeoShape* shape) { + + std::vector<GeoTrf::Vector3D> vtx; + + if (shape->type() == "Box") { + const GeoBox* box = dynamic_cast<const GeoBox*> (shape); + double hx = box->getXHalfLength(); + double hy = box->getYHalfLength(); + double hz = box->getZHalfLength(); + + vtx.push_back(GeoTrf::Vector3D(-hx,-hy, hz)); + vtx.push_back(GeoTrf::Vector3D(-hx,+hy, hz)); + vtx.push_back(GeoTrf::Vector3D(+hx,+hy, hz)); + vtx.push_back(GeoTrf::Vector3D(+hx,-hy, hz)); + vtx.push_back(GeoTrf::Vector3D(+hx,-hy,-hz)); + vtx.push_back(GeoTrf::Vector3D(+hx,+hy,-hz)); + vtx.push_back(GeoTrf::Vector3D(-hx,+hy,-hz)); + vtx.push_back(GeoTrf::Vector3D(-hx,-hy,-hz)); + } + + if (shape->type() == "Trd") { + const GeoTrd* trd = dynamic_cast<const GeoTrd*> (shape); + double hx1 = trd->getXHalfLength1(); + double hx2 = trd->getXHalfLength2(); + double hy1 = trd->getYHalfLength1(); + double hy2 = trd->getYHalfLength2(); + double hz = trd->getZHalfLength(); + + vtx.push_back(GeoTrf::Vector3D(-hx2,-hy2, hz)); + vtx.push_back(GeoTrf::Vector3D(-hx2,+hy2, hz)); + vtx.push_back(GeoTrf::Vector3D(+hx2,+hy2, hz)); + vtx.push_back(GeoTrf::Vector3D(+hx2,-hy2, hz)); + vtx.push_back(GeoTrf::Vector3D(+hx1,-hy1,-hz)); + vtx.push_back(GeoTrf::Vector3D(+hx1,+hy1,-hz)); + vtx.push_back(GeoTrf::Vector3D(-hx1,+hy1,-hz)); + vtx.push_back(GeoTrf::Vector3D(-hx1,-hy1,-hz)); + } + + if (shape->type() == "Box" || shape->type() == "Trd") { + + addVertices(vtx); + std::vector<size_t> f0 = {0,1,2,3}; + addFace(f0); + std::vector<size_t> f1 = {4,5,6,7}; + addFace(f1); + std::vector<size_t> f2 = {0,7,6,1}; + addFace(f2); + std::vector<size_t> f3 = {3,4,7,0}; + addFace(f3); + std::vector<size_t> f4 = {2,5,4,3}; + addFace(f4); + std::vector<size_t> f5 = {1,6,5,2}; + addFace(f5); + + for (unsigned int i=0; i<getNFaces(); i++) + m_Normals.push_back( + ((m_Vertices[m_Faces[i][0]]-m_Vertices[m_Faces[i][1]]).cross(m_Vertices[m_Faces[i][2]]-m_Vertices[m_Faces[i][1]])).normalized() ); + } + + if (shape->type() == "SimplePolygonBrep") { + + const GeoSimplePolygonBrep* spb = dynamic_cast<const GeoSimplePolygonBrep*> (shape); + double hz = spb->getDZ(); + unsigned int nvtx = spb->getNVertices(); + + for (unsigned int i=0; i < nvtx; i++) vtx.push_back(GeoTrf::Vector3D(spb->getXVertex(i),spb->getYVertex(i),hz)); + for (unsigned int i=0; i < nvtx; i++) vtx.push_back(GeoTrf::Vector3D(spb->getXVertex(i),spb->getYVertex(i),-hz)); + + addVertices(vtx); + std::vector<size_t> f0; for (unsigned int i=0; i < nvtx; i++) f0.push_back(i); + addFace(f0); + std::vector<size_t> f1; for (unsigned int i=2*nvtx-1; i >= nvtx; i--) f1.push_back(i); + addFace(f1); + for (unsigned int i=0; i<nvtx; i++) { + std::vector<size_t> f; + f.push_back( i == nvtx-1 ? 0 : i+1); f.push_back(i); f.push_back(nvtx+i); f.push_back( i == nvtx-1 ? nvtx : nvtx+i+1); + addFace(f); + } + + // calculate normals : flip ordering if not consistent + int flipped = 0; + for (unsigned int i=0; i<getNFaces(); i++) { + bool isFlipped = false; + m_Normals.push_back( + ( (m_Vertices[m_Faces[i][0]]-m_Vertices[m_Faces[i][1]]).cross(m_Vertices[m_Faces[i][2]]-m_Vertices[m_Faces[i][1]])).normalized() ); + if (m_Normals.back().norm()!=1) { + for (unsigned int iv=1; iv<m_Faces[i].size()-2; iv++) { + // recalculate normal using other vertices + GeoTrf::Vector3D tn = ((m_Vertices[m_Faces[i][iv]]-m_Vertices[m_Faces[i][iv+1]]).cross(m_Vertices[m_Faces[i][iv+2]]-m_Vertices[m_Faces[i][iv+1]])).normalized(); + if (tn.norm()>0) { m_Normals.back() = tn; break; } + } + } + // check orientation + for (unsigned int iv=0; iv<m_Faces[i].size()-2; iv++) { + GeoTrf::Vector3D ref = 0.5*(m_Vertices[m_Faces[i][iv]] + 0.5*(m_Vertices[m_Faces[i][iv+1]]+m_Vertices[m_Faces[i][iv+2]])); + if (insideFace(i,ref)) { + bool cn = contains( ref+1.e-3*m_Normals.back()); + if ( cn != contains( ref-1.e-3*m_Normals.back()) ) { // valid test point + if (cn) {isFlipped= true; m_Normals.back() = -1.* m_Normals.back();} // flipping normal + break; + } // end valid test point + } // search of suitable test point + } // end check oprientation + if (isFlipped && i>1) flipped++; + } + //std::cout << "spb -> polyhedron:flipped "<< flipped <<" out of " << getNFaces() << std::endl; + if ( flipped > 0 ) { // change ordering of all faces ( normals fixed already ) + for ( unsigned int ip=0 ; ip<getNFaces(); ip++) { + std::vector<size_t> vtx = getFace(ip); + for ( unsigned int iv=0; iv<vtx.size(); iv++) m_Faces[ip][iv]=vtx[vtx.size()-1-iv]; + } + } + if (!isValid() ) std::cout << " GeoPolygonGeneric constructor from SimplePolygonBrep not valid" << std::endl; + } + +} + +double GeoPolyhedronGeneric::volume () const { + if (!isValid()) + throw std::runtime_error ("Volume requested for incomplete polyhedron"); + // + GeoTrf::Vector3D refVtx = m_Vertices[0]; // reference point + double vol = 0.; + std::vector<GeoTrf::Vector3D> vtx; + + for (unsigned int i=0; i< getNFaces(); i++) { + + // retrieve vertices + std::vector<size_t> faceDef = getFace(i); + vtx.clear(); + for (auto vi : faceDef ) vtx.push_back(m_Vertices[vi]); + // normal + GeoTrf::Vector3D normal = faceNormal(i); + + // area + int n = vtx.size(); + GeoTrf::Vector3D area = vtx[0].cross(vtx[n - 1]); + for (int k = 1; k < n; ++k) area += vtx[k].cross(vtx[k-1]); + vol += 0.5 * area.norm() * (vtx[1]-refVtx).dot(normal) / 3.; + } + + return std::abs(vol); +} + +void GeoPolyhedronGeneric::extent (double& xmin, double& ymin, double& zmin, + double& xmax, double& ymax, double& zmax) const +{ + if (!isValid()) + throw std::runtime_error ("Extent requested for incomplete polyhedron"); + xmin = xmax = m_Vertices[0].x(); + ymin = ymax = m_Vertices[0].y(); + zmin = zmax = m_Vertices[0].z(); + for (size_t k = 1; k < m_Vertices.size(); ++k) + { + double x = m_Vertices[k].x(); + double y = m_Vertices[k].y(); + double z = m_Vertices[k].z(); + xmin = std::min(xmin, x); + xmax = std::max(xmax, x); + ymin = std::min(ymin, y); + ymax = std::max(ymax, y); + zmin = std::min(zmin, z); + zmax = std::max(zmax, z); + } +} + +bool GeoPolyhedronGeneric::contains (double x, double y, double z) const +{ + return this->contains( GeoTrf::Vector3D(x,y,z) ); +} + +bool GeoPolyhedronGeneric::contains ( GeoTrf::Vector3D point, bool debugMode ) const +{ + // algorithm : count intersection with faces along probe line : point+(0,0,1)*t + // odd/even number of intersections : inside/outside + + bool in = false; + GeoTrf::Vector3D probeDir(0.,0.,1.); + std::vector<GeoTrf::Vector3D> intersections; // keep trace of intersections to intervene when an edge is hit + + for (auto vi : m_Vertices) if ( (vi - point).norm()<1.e-6) return true; + + if (debugMode) std::cout <<"test inside for point:" << point.x()<<"," <<point.y()<<"," << point.z() << std::endl; + if (debugMode) for (auto vi : m_Vertices) std::cout <<"input vertices:" << vi.x() <<"," << vi.y() <<"," << vi.z() << std::endl; + + double h,pn,dist; + for (size_t i=0; i< getNFaces() ; i++) { + + // check if point lays at edge + std::vector<GeoTrf::Vector3D> vtx; + std::vector<size_t> face= getFace(i); + for (auto vi : face ) vtx.push_back(m_Vertices[vi]); + for (unsigned int iv = 0; iv < vtx.size(); iv++) { + GeoTrf::Vector3D edge = (iv<vtx.size()-1) ? vtx[iv+1] - vtx[iv] : vtx[0] - vtx[iv]; + double pt = ((point - vtx[iv]).cross(edge)).norm()/edge.norm(); + double pn = (point - vtx[iv]).dot(edge)/edge.norm()/edge.norm(); + if (debugMode && std::abs(pt)<1.e-6 && pn>=0. && pn<=1.) std::cout <<"edge hit in PHG::contains" << std::endl;; // edge hit + if (std::abs(pt)<1.e-6 && pn>=0. && pn<=1.) return true; // edge hit + } + + // + + GeoTrf::Vector3D n = this->faceNormal(i); + h = ( m_Vertices[m_Faces[i][0]] - point).dot(n); // nearest distance point - plane + + if (std::abs(h)<1.e-6 && insideFace(i,point) ) return true; // TODO check numerical limit + + pn = probeDir.dot(n); // dot product normal*probe direction + if (debugMode) std::cout << "contains:"<<i <<":" << h <<":" << pn << std::endl; + if (debugMode) std::cout <<" face normal:" << i <<":" << n.x() <<"," << n.y()<<"," << n.z() << std::endl; + if ( pn != 0 ) { + dist = h / pn; + if (debugMode) std::cout << "plane intersection:" << point+dist*probeDir <<": at distance:" << dist<< std::endl; + if (dist>0) { + GeoTrf::Vector3D ix = point + dist*probeDir ; + bool validIntersection = insideFace(i, GeoTrf::Vector3D( ix ) ); + bool edgeHit = false; + for ( auto px : intersections ) if ((ix - px).norm()<1.e-6) edgeHit = true; + if (validIntersection && !edgeHit) intersections.push_back(ix); + if (debugMode) std::cout << "valid intersection?" << validIntersection <<":edge hit?" << edgeHit << std::endl; + if (!edgeHit) in ^= validIntersection; + } + } + } + + return in; +} + +void GeoPolyhedronGeneric::addVertices(std::vector<GeoTrf::Vector3D> vertices) +{ + m_Vertices = vertices; +} + +void GeoPolyhedronGeneric::addFace(std::vector<size_t> faceVtx) +{ + m_Faces.push_back(faceVtx); +} + +void GeoPolyhedronGeneric::addNormal(GeoTrf::Vector3D n) +{ + m_Normals.push_back(n); +} + + +GeoTrf::Vector3D GeoPolyhedronGeneric::faceNormal(unsigned int i) const +{ + + if (!m_Normals.size() ) { // recalculate normals + for (unsigned int i=0; i<getNFaces(); i++) { + m_Normals.push_back( + ( (m_Vertices[m_Faces[i][0]]-m_Vertices[m_Faces[i][1]]).cross(m_Vertices[m_Faces[i][2]]-m_Vertices[m_Faces[i][1]])).normalized() ) ; + // check orientation + for (unsigned int iv=0; iv<m_Faces[i].size()-2; iv++) { + GeoTrf::Vector3D ref = 0.5*(m_Vertices[m_Faces[i][iv]] + 0.5*(m_Vertices[m_Faces[i][iv+1]]+m_Vertices[m_Faces[i][iv+2]])); + if (insideFace(i,ref)) { + bool cn = contains( ref+1.e-3*m_Normals.back()); + if ( cn != contains( ref-1.e-3*m_Normals.back()) ) { // valid test point + if (cn) m_Normals.back() = -1.* m_Normals.back(); + break; + } + } + } + } + } + + return m_Normals[i]; +} + +bool GeoPolyhedronGeneric::insideFace(unsigned int i, GeoTrf::Vector3D point) const +{ + std::vector<GeoTrf::Vector3D> vtx; + std::vector<size_t> face= getFace(i); + for (auto vi : face ) vtx.push_back(m_Vertices[vi]); + + for (auto vx : vtx) if ( (vx - point).norm()<1.e-6) return true; + + // count intersections of line defined by ref point and mid n-th edge, in + direction + GeoTrf::Vector3D probe = point - 0.5*(vtx.front()+vtx.back()); // probe direction crossing n-th edge + + if ( probe.norm() < 1.e-6) return true; // on edge + + bool in = false; + double d,dist,t; + + for (unsigned int iv = 0; iv < vtx.size(); iv++) { + + GeoTrf::Vector3D edge = (iv<vtx.size()-1) ? vtx[iv+1] - vtx[iv] : vtx[0] - vtx[iv]; + + // dist and t in units (probe, edge) + d = intersectLines( point, probe, vtx[iv], edge, dist, t); + + // + if ( std::abs(d)<1.e-6 && t>=0. && t<=1. ) { + in ^= dist>0; + if ( (vtx[iv]+t*edge-point).norm()<1.e-6 ) return true; // point on edge + } + } + + return in; + +} + +void GeoPolyhedronGeneric::exec(GeoShapeAction *action) const +{ + action->handleShape(this); +} + + +double GeoPolyhedronGeneric::intersectLines(GeoTrf::Vector3D A, GeoTrf::Vector3D a, GeoTrf::Vector3D B, GeoTrf::Vector3D b, double& da, double& db) const +{ + // lines parametrization : A + a*da , B + b*db ; a,b define "units" of distances da, db + // return value : closest distance between lines in native [mm] units + + // scalars + double ab = a.dot(b); double a2 = a.dot(a); double b2 = b.dot(b); + double ABa = (A-B).dot(a); + double ABb = (A-B).dot(b); + + // special cases + if ( !ab ) { + da = -ABa / a2; + db = ABb / b2; + return ( (A + da*a -B - db*b).norm() ); + } + + if ( a2*b2 == ab*ab ) { // parallel lines + da = 0; db = 0; + return ((A-B).cross(b)).norm()/b.norm(); + } + + da = (ABb*ab -b2*ABa) / (a2*b2 - ab*ab); + db = (ABa + a2*da) / ab; + return ( (A + da*a -B - db*b).norm() ); + +} + +std::vector< std::pair< double, std::pair<size_t,size_t> > > GeoPolyhedronGeneric::edgeFaceIntersect( int i, GeoTrf::Vector3D eInit, GeoTrf::Vector3D edge ) const +{ + std::vector< std::pair< double, std::pair<size_t,size_t> > > output; + + std::vector<GeoTrf::Vector3D> vtx; + std::vector<size_t> face= getFace(i); + for (auto vi : face ) vtx.push_back(m_Vertices[vi]); + + double d,dist,t; + for (unsigned int iv = 0; iv < vtx.size(); iv++) { + + std::pair<size_t,size_t> fvx(face[iv], (iv<face.size()-1) ? face[iv+1] : face[0]); + + GeoTrf::Vector3D fi = (iv<vtx.size()-1) ? vtx[iv+1] - vtx[iv] : vtx[0] - vtx[iv]; + + // dist and t in units (probe, edge) + d = intersectLines( vtx[iv], fi, eInit, edge, dist, t); + + if ( std::abs(d)<1.e-6 && dist>=0 && dist <= 1 && t>=0. && t<=1.) { + std::pair< double, std::pair<size_t,size_t> > ix(t, fvx); + output.push_back(ix); + } + } + return output; +} + +bool GeoPolyhedronGeneric::isValid() const +{ + if (getNFaces()<4) return false; + + // loop over edges, check that each appears with its mirrored version ( attached faces ) + for ( unsigned int ic=0; ic < getNFaces(); ic++) { + std::vector<size_t> vtx = getFace(ic); + for (unsigned int iv=0; iv<vtx.size(); iv++) { + std::pair<size_t,size_t> edge(vtx[iv], iv<vtx.size()-1 ? vtx[iv+1] : vtx[0]); // probe + bool mirrored = false; // search mirrored edge + for ( unsigned int icm=0; icm < getNFaces(); icm++) { + if (icm == ic ) continue; + std::vector<size_t> vtm = getFace(icm); + for (unsigned int ivm=0; ivm<vtm.size(); ivm++) { + std::pair<size_t,size_t> edgm(vtm[ivm], ivm<vtm.size()-1 ? vtm[ivm+1] : vtm[0]); // test + if ( edgm.first == edge.second && edgm.second == edge.first) mirrored = true; + if (mirrored) break; + } // end loop over other edges + if (mirrored) break; + } // end loop over other faces + if (!mirrored) {std::cout << "problem in polyhedron, edge not mirrored:face:edge:" << ic<<":" <<iv << std::endl; return false;} + } + } + + return true; + +}