Debian Codegraph @

src/TriWithLines2D.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 "TriWithLines2D.h"
14
15 //-- system includes --
16 #include <cstdlib>
17
18 using std::rand;
19
20 // --- STL includes ---
21 #include <utility>
22 #include <map>
23
24 using std::make_pair;
25 using std::map;
26
27 //--- IO includes ---
28 #include <iostream>
29
30
31 /*!
32 helper function, return random value between min & max
33 */
34 double TriWithLines2D::m_random(double imin,double imax) const
35 {
36 return imin+((imax-imin)*((double)(rand())/(double)(RAND_MAX)));
37 }
38
39 TriWithLines2D::TriWithLines2D()
40 {}
41
42 /*!
43 Construct Triangle
44
45 \param p0 first corner of triangle box
46 \param p1 second corner of triangle box
47 \param p2 third corner of triangle box
48 */
49 TriWithLines2D::TriWithLines2D(const Vector3& p0,const Vector3& p1,const Vector3& p2)
50 : m_p0(p0),m_p1(p1),m_p2(p2)
51 {
52 Vector3 pmin, pmax;
53 double minV,maxV;
54 Line2D l;
55
56 int i;
57
58 for (i=0;i<3;i++) {
59 minV=p0[i];
60 maxV=p0[i];
61 if (p1[i] < minV) minV = p1[i];
62 if (p1[i] > maxV) maxV = p1[i];
63 if (p2[i] < minV) minV = p2[i];
64 if (p2[i] > maxV) maxV = p2[i];
65 pmin[i] = minV;
66 pmax[i] = maxV;
67 }
68 m_pmin=(pmin);
69 m_pmax=(pmax);
70
71 m_a0=((p1[1] - p0[1])/(p1[0]-p0[0]));
72 m_b0=(1.0);
73 m_c0=(p0[1] - m_a0*p0[0]);
74
75 m_a1=((p2[1] - p1[1])/(p2[0]-p1[0]));
76 m_b1=(1.0);
77 m_c1=(p1[1] - m_a1*p1[0]);
78
79 m_a2=((p0[1] - p2[1])/(p0[0]-p2[0]));
80 m_b2=(1.0);
81 m_c2=(p2[1] - m_a2*p2[0]);
82
83 l = Line2D(p0,p1);
84 addLine(l);
85 l = Line2D(p1,p2);
86 addLine(l);
87 l = Line2D(p2,p0);
88 addLine(l);
89
90 }
91
92 /*!
93 add a line
94
95 \param l the line
96 */
97 void TriWithLines2D::addLine(const Line2D& l)
98 {
99 m_lines.push_back(l);
100 }
101
102 /*!
103 get bounding box
104 */
105 pair<Vector3,Vector3> TriWithLines2D::getBoundingBox()
106 {
107 return make_pair(m_pmin,m_pmax);
108 }
109
110 /*!
111 get point inside the box. The Argument is ignored
112 */
113 Vector3 TriWithLines2D::getAPoint(int) const
114 {
115 double px,py;
116 bool found_a_point;
117 Vector3 aPoint;
118
119 found_a_point = false;
120 do {
121 px=m_random(m_pmin.x(),m_pmax.x());
122 py=m_random(m_pmin.y(),m_pmax.y());
123 aPoint = Vector3(px,py,0.0);
124 found_a_point = isIn(aPoint);
125 } while (found_a_point==false);
126
127 return Vector3(px,py,0.0);
128 }
129
130 /*!
131 Return closest plane to a given point. Assumes that the list of Planes/Lines is not empty.
132
133 \param p the point
134 \warning check non-empty list of lines (via hasPlane()) before invoking
135 */
136 Line2D TriWithLines2D::getClosestPlane(const Vector3& p)
137 {
138 std::cout << "getClosestPlane : " << p << std::endl;
139
140 vector<Line2D>::iterator PL=m_lines.begin();
141 double dist=PL->getDist(p);
142
143 for(vector<Line2D>::iterator iter=m_lines.begin();
144 iter!=m_lines.end();
145 iter++){
146 double ndist=iter->getDist(p);
147 std::cout << "Line: " << *iter << " Distance: " << ndist << std::endl;
148 if(ndist<dist){
149 PL=iter;
150 dist=ndist;
151 }
152 }
153 std::cout << "closest line: " << *PL << " Distance: " << dist << std::endl;
154
155 return (*PL);
156 }
157
158 const map<double, const Line2D*> TriWithLines2D::getClosestPlanes(const Vector3& p,int nmax) const
159 {
160 map<double,const Line2D*> res;
161
162 for(vector<Line2D>::const_iterator iter=m_lines.begin();
163 iter!=m_lines.end();
164 iter++){
165 double ndist=iter->getDist(p);
166 const Line2D *l=&(*iter);
167 res.insert(make_pair(ndist,l));
168 }
169
170 return res;
171 }
172
173
174 /*!
175 check if point is inside
176
177 \param p the point
178 */
179 bool TriWithLines2D::isIn(const Vector3& p) const
180 {
181 bool inside0,inside1,inside2,inbox;
182 double dp,dv;
183 Vector3 cp,cv;
184
185 inside0 = false;
186 inside1 = false;
187 inside2 = false;
188
189 // dp = m_a0*p[0] + m_b0*p[1] + m_c0;
190 // dv = m_a0*m_p2[0] + m_b0*m_p2[1] + m_c0;
191
192 cp = cross(m_p1-m_p0,p-m_p0);
193 cv = cross(m_p1-m_p0,m_p2-m_p0);
194 dp = cp.z();
195 dv = cv.z();
196 if (dp*dv>0.0) inside0=true;
197
198 // dp = m_a1*p[0] + m_b1*p[1] + m_c1;
199 // dv = m_a1*m_p0[0] + m_b1*m_p0[1] + m_c1;
200
201 cp = cross(m_p2-m_p1,p-m_p1);
202 cv = cross(m_p2-m_p1,m_p0-m_p1);
203 dp = cp.z();
204 dv = cv.z();
205 if (dp*dv>0.0) inside1=true;
206
207 // dp = m_a2*p[0] + m_b2*p[1] + m_c2;
208 // dv = m_a2*m_p1[0] + m_b2*m_p1[1] + m_c2;
209
210 cp = cross(m_p0-m_p2,p-m_p2);
211 cv = cross(m_p0-m_p2,m_p1-m_p2);
212 dp = cp.z();
213 dv = cv.z();
214 if (dp*dv>0.0) inside2=true;
215
216 inbox = ((p.x()>m_pmin.x()) && (p.x()<m_pmax.x()) && (p.y()>m_pmin.y()) && (p.y()<m_pmax.y()));
217
218 // if ((inside0) && (inside1) && (inside2) && (inbox)) std::cout << "inside " << p << std::endl;
219
220
221 return ((inside0) && (inside1) && (inside2) && inbox);
222
223 // return ((p.x()>m_pmin.x()) && (p.x()<m_pmax.x()) && (p.y()>m_pmin.y()) && (p.y()<m_pmax.y()));
224 }
225
226 /*!
227 check if a Sphere is inside and doesn't intersect any lines
228
229 \param S the Sphere
230 */
231 bool TriWithLines2D::isIn(const Sphere& S)
232 {
233 double r=S.Radius();
234 Vector3 p=S.Center();
235
236 // std::cout << "TriWithLines2D rad: " << r << " pos: " << p << std::endl;
237
238 // bool inside=(p.X()>m_pmin.X()+r) && (p.X()<m_pmax.X()-r) && (p.Y()>m_pmin.Y()+r) && (p.Y()<m_pmax.Y()-r);
239 bool inside = isIn(p);
240
241 vector<Line2D>::iterator iter=m_lines.begin();
242
243 double dist=2*r;
244 while(iter!=m_lines.end() && dist>r){
245 dist=iter->getDist(p);
246 iter++;
247 }
248
249 return inside && (dist>r);
250 }
251
252 ostream& operator<< (ostream& ost, const TriWithLines2D& L)
253 {
254 ost << L.m_p0 << " to " << L.m_p1 << " to " << L.m_p2;
255
256 return ost;
257 }
258