Debian Codegraph @

src/ConvexPolyWithJointSet.cc

raw
1 /////////////////////////////////////////////////////////////
2 // //
3 // Copyright (c) 2007-2017 by The University of Queensland //
4 // Centre for Geoscience Computing //
5 // http://earth.uq.edu.au/centre-geoscience-computing //
6 // //
7 // Primary Business: Brisbane, Queensland, Australia //
8 // Licensed under the Open Software License version 3.0 //
9 // http://www.apache.org/licenses/LICENSE-2.0 //
10 // //
11 /////////////////////////////////////////////////////////////
12
13 #include "ConvexPolyWithJointSet.h"
14
15 ConvexPolyWithJointSet::ConvexPolyWithJointSet()
16 {}
17
18 /*!
19 Construct polyhedron - without planes its just a box
20
21 \param pmin minimum point of bounding box
22 \param pmax maximum point of bounding box
23 */
24 ConvexPolyWithJointSet::ConvexPolyWithJointSet(const Vector3& pmin,const Vector3& pmax)
25 : ConvexPolyhedron(pmin,pmax)
26 {}
27
28 ConvexPolyWithJointSet::~ConvexPolyWithJointSet()
29 {}
30
31 /*!
32 get point inside the box. The Argument is ignored
33 */
34 Vector3 ConvexPolyWithJointSet::getAPoint(int)const
35 {
36 double px,py,pz;
37 Vector3 res;
38
39 // std::cout << m_pmin << " - " << m_pmax << std::endl;
40 do{
41 px=m_random(m_pmin.x(),m_pmax.x());
42 py=m_random(m_pmin.y(),m_pmax.y());
43 pz=m_random(m_pmin.z(),m_pmax.z());
44 res=Vector3(px,py,pz);
45 } while (!isIn(res));
46 return res;
47 }
48
49 /*!
50 check if point is inside
51
52 \param p the point
53 */
54 bool ConvexPolyWithJointSet::isIn(const Vector3& p) const
55 {
56 bool res;
57 // std::cout << p << std::endl;
58 // check inside bounding box
59 res= ((p.x()>m_pmin.x()) && (p.x()<m_pmax.x()) &&
60 (p.y()>m_pmin.y()) && (p.y()<m_pmax.y()) &&
61 (p.z()>m_pmin.z()) && (p.z()<m_pmax.z()));
62
63 // std::cout << "inside bbx : " << res << std::endl;
64 // if inside, check against planes
65 if(res){
66 vector<Plane>::const_iterator iter=m_planes.begin();
67
68 while((iter!=m_planes.end()) && (res)){
69 Vector3 normal=iter->getNormal();
70 Vector3 pdist=p-(iter->getOrig());
71 res=(normal*pdist>0);
72 iter++;
73 }
74 }
75 return res;
76 }
77
78 /*!
79 check if a Sphere is inside and doesn't intersect any planes
80
81 \param S the Sphere
82 */
83 bool ConvexPolyWithJointSet::isIn(const Sphere& S)
84 {
85 bool res;
86
87 double r=S.Radius();
88 Vector3 p=S.Center();
89
90 // std::cout << "rad: " << r << " pos: " << p << std::endl;
91 // inside bbx
92 res=(p.X()>m_pmin.X()+r) && (p.X()<m_pmax.X()-r) &&
93 (p.Y()>m_pmin.Y()+r) && (p.Y()<m_pmax.Y()-r) &&
94 (p.Z()>m_pmin.Z()+r) && (p.Z()<m_pmax.Z()-r);
95
96 // if inside, check against planes
97 if(res){
98 vector<Plane>::const_iterator iter=m_planes.begin();
99
100 while((iter!=m_planes.end()) && (res)){
101 Vector3 normal=iter->getNormal();
102 Vector3 pdist=p-(iter->getOrig());
103 res=(normal*pdist>r);
104 iter++;
105 }
106 }
107
108 if(res){
109 // check intersection with joints
110 vector<Triangle3D>::iterator iter=m_joints.begin();
111 double dist=2*r;
112 while((iter!=m_joints.end()) && res){
113 dist=iter->getDist(p);
114 res=(dist>r);
115 iter++;
116 }
117 }
118
119 return res;
120 }
121
122 void ConvexPolyWithJointSet::addJoints(const TriPatchSet& t)
123 {
124 for(vector<Triangle3D>::const_iterator iter=t.triangles_begin();
125 iter!=t.triangles_end();
126 iter++){
127 m_joints.push_back(*iter);
128 }
129 }