1 /*
2 This file is a part of the RepSnapper project.
3 Copyright (C) 2010 Kulitorum
4
5 This program is free software; you can redistribute it and/or modify
6 it under the terms of the GNU General Public License as published by
7 the Free Software Foundation; either version 2 of the License, or
8 (at your option) any later version.
9
10 This program is distributed in the hope that it will be useful,
11 but WITHOUT ANY WARRANTY; without even the implied warranty of
12 MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
13 GNU General Public License for more details.
14
15 You should have received a copy of the GNU General Public License along
16 with this program; if not, write to the Free Software Foundation, Inc.,
17 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA.
18 */
19 #include "triangle.h"
20 #include "geometry.h"
21
22
Triangle(const Vector3d & Point1,const Vector3d & Point2,const Vector3d & Point3)23 Triangle::Triangle(const Vector3d &Point1,
24 const Vector3d &Point2, const Vector3d &Point3)
25 { A=Point1;B=Point2;C=Point3;
26 calcNormal();
27 }
28
calcNormal()29 void Triangle::calcNormal()
30 {
31 Vector3d AA=C-A;
32 Vector3d BB=C-B;
33 Normal = normalized(AA.cross(BB));
34 }
35
transformed(const Matrix4d & T) const36 Triangle Triangle::transformed(const Matrix4d &T) const
37 {
38 return Triangle(T*A,T*B,T*C);
39 }
40
41
invertNormal()42 void Triangle::invertNormal()
43 {
44 Vector3d swap = A;
45 A=C; C=swap;
46 calcNormal();
47 }
48
mirrorX(const Vector3d & center)49 void Triangle::mirrorX(const Vector3d ¢er)
50 {
51 for (uint i = 0; i < 3; i++)
52 operator[](i).x() = center.x()-operator[](i).x();
53 invertNormal();
54 }
55
operator [](const uint index)56 Vector3d &Triangle::operator[] (const uint index)
57 {
58 switch(index) {
59 case 0: return A;
60 case 1: return B;
61 case 2: return C;
62 }
63 return A;
64 }
operator [](const uint index) const65 Vector3d const &Triangle::operator[] (const uint index) const
66 {
67 switch(index) {
68 case 0: return A;
69 case 1: return B;
70 case 2: return C;
71 }
72 return A;
73 }
74
75 // for 2 adjacent triangles test if normals match
wrongOrientationWith(Triangle const & other,double maxsqerr) const76 bool Triangle::wrongOrientationWith(Triangle const &other, double maxsqerr) const
77 {
78 // find the 2 common vertices
79 vector<int> thisv, otherv;
80 for (uint j = 0; j < 3; j++) {
81 if ( A == other[j] || A.squared_distance(other[j]) < maxsqerr){
82 thisv.push_back(0); otherv.push_back(j);
83 }
84 if ( B == other[j] || B.squared_distance(other[j]) < maxsqerr) {
85 thisv.push_back(1); otherv.push_back(j);
86 }
87 if ( C == other[j] || C.squared_distance(other[j]) < maxsqerr) {
88 thisv.push_back(2); otherv.push_back(j);
89 }
90 }
91 if (thisv.size()!=2 || otherv.size()!=2) {
92 //cerr << "only " << thisv.size() << " common vertex! " << endl;
93 return false; // "ok" because non-adjacent
94 }
95
96 int diff = thisv[1] - thisv[0];
97 const bool thisorient = ( diff == 1 || diff == -2 );
98 diff = otherv[1] - otherv[0];
99 const bool otherorient = ( diff == 1 || diff == -2 );
100 // cerr << "have 2: " << thisorient <<" / " << otherorient << endl;
101 return (thisorient == otherorient); // they have different(!) orientation
102 }
103
isConnectedTo(Triangle const & other,double maxsqerr) const104 bool Triangle::isConnectedTo(Triangle const &other, double maxsqerr) const
105 {
106 // for (uint i = 0; i < 3; i++) {
107 // Vector3d p = (Vector3d)(operator[](i));
108 // first test equal, faster
109 for (uint j = 0; j < 3; j++) {
110 if (( A == other[j])) return true;
111 if (( B == other[j])) return true;
112 if (( C == other[j])) return true;
113 }
114 if (maxsqerr>0)
115 // if not, test distance
116 for (uint j = 0; j < 3; j++) {
117 if ( A.squared_distance(other[j]) < maxsqerr) return true;
118 if ( B.squared_distance(other[j]) < maxsqerr) return true;
119 if ( C.squared_distance(other[j]) < maxsqerr) return true;
120 }
121 return false;
122 }
123
slopeAngle(const Matrix4d & T) const124 double Triangle::slopeAngle(const Matrix4d &T) const
125 {
126 const double scale = T(3,3);
127 Vector3d trans;
128 T.get_translation(trans);
129 // get scaled translation out of matrix
130 const Vector3d n = T * Normal - trans/scale;
131 return asin(n.z()/n.length());
132 }
133
134
area() const135 double Triangle::area() const
136 {
137 return 0.5* ((C-A).cross(B-A)).length() ;
138 }
139
140 // add all these to get shape volume
projectedvolume(const Matrix4d & T) const141 double Triangle::projectedvolume(const Matrix4d &T) const
142 {
143 if (Normal.z()==0) return 0;
144 Triangle xyproj = Triangle(Vector3d(A.x(),A.y(),0),
145 Vector3d(B.x(),B.y(),0),
146 Vector3d(C.x(),C.y(),0));
147 Vector3d min = GetMin(T);
148 Vector3d max = GetMax(T);
149 double vol = xyproj.area()*0.5*(max.z()+min.z());
150 if (Normal.z()<0) vol=-vol;
151 return vol;
152 }
153
GetMax(const Matrix4d & T) const154 Vector3d Triangle::GetMax(const Matrix4d &T) const
155 {
156 Vector3d max(-99999999.0, -99999999.0, -99999999.0);
157 Vector3d TA=T*A,TB=T*B,TC=T*C;
158 for (uint i = 0; i < 3; i++) {
159 max[i] = MAX(max[i], TA[i]);
160 max[i] = MAX(max[i], TB[i]);
161 max[i] = MAX(max[i], TC[i]);
162 }
163 return max;
164 }
165
GetMin(const Matrix4d & T) const166 Vector3d Triangle::GetMin(const Matrix4d &T) const
167 {
168 Vector3d min(99999999.0, 99999999.0, 99999999.0);
169 Vector3d TA=T*A,TB=T*B,TC=T*C;
170 for (uint i = 0; i < 3; i++) {
171 min[i] = MIN(min[i], TA[i]);
172 min[i] = MIN(min[i], TB[i]);
173 min[i] = MIN(min[i], TC[i]);
174 }
175 return min;
176 }
177
AccumulateMinMax(Vector3d & min,Vector3d & max,const Matrix4d & T)178 void Triangle::AccumulateMinMax(Vector3d &min, Vector3d &max, const Matrix4d &T)
179 {
180 Vector3d tmin = GetMin(T);
181 Vector3d tmax = GetMax(T);
182 for (uint i = 0; i < 3; i++) {
183 min[i] = MIN(tmin[i], min[i]);
184 max[i] = MAX(tmax[i], max[i]);
185 }
186 }
187
Translate(const Vector3d & vector)188 void Triangle::Translate(const Vector3d &vector)
189 {
190 A += vector;
191 B += vector;
192 C += vector;
193 }
194
rotate(const Vector3d & axis,double angle)195 void Triangle::rotate(const Vector3d &axis, double angle)
196 {
197 A = A.rotate(angle, axis);
198 B = B.rotate(angle, axis);
199 C = C.rotate(angle, axis);
200 calcNormal();
201 }
202
triangulateQuadrilateral(vector<Vector3d> fourpoints,vector<Triangle> & triangles)203 void triangulateQuadrilateral(vector<Vector3d> fourpoints, vector<Triangle> &triangles)
204 {
205 assert(fourpoints.size()==4);
206 vector<Triangle> tr(2);
207 double SMALL = 0.01;
208 // find diagonals
209 double dist = dist3D_Segment_to_Segment(fourpoints[0],fourpoints[2],
210 fourpoints[1],fourpoints[3],
211 SMALL*SMALL);
212 if (dist < SMALL)
213 { // found -> divide at shorter diagonal
214 if ((fourpoints[0]-fourpoints[2]).squared_length()
215 < (fourpoints[1]-fourpoints[3]).squared_length()) {
216 tr[0] = Triangle(fourpoints[0],fourpoints[1],fourpoints[2]);
217 tr[1] = Triangle(fourpoints[2],fourpoints[3],fourpoints[0]);
218 } else {
219 tr[0] = Triangle(fourpoints[0],fourpoints[1],fourpoints[3]);
220 tr[1] = Triangle(fourpoints[1],fourpoints[2],fourpoints[3]);
221 }
222 }
223 else
224 { // take other 2
225 double dist = dist3D_Segment_to_Segment(fourpoints[1],fourpoints[2],
226 fourpoints[0],fourpoints[3],
227 SMALL*SMALL);
228 if (dist < SMALL)
229 {
230 if ((fourpoints[1]-fourpoints[2]).squared_length()
231 < (fourpoints[0]-fourpoints[3]).squared_length()) {
232 tr[0] = Triangle(fourpoints[1],fourpoints[2],fourpoints[3]);
233 tr[1] = Triangle(fourpoints[0],fourpoints[1],fourpoints[2]);
234 } else {
235 tr[0] = Triangle(fourpoints[1],fourpoints[0],fourpoints[3]);
236 tr[1] = Triangle(fourpoints[0],fourpoints[2],fourpoints[3]);
237 }
238 }
239 else
240 { // take 3rd possibility, not the case here, because 2-3 is cut line
241 double dist = dist3D_Segment_to_Segment(fourpoints[0],fourpoints[1],
242 fourpoints[2],fourpoints[3],
243 SMALL*SMALL);
244 if (dist < SMALL)
245 {
246 tr[0] = Triangle(fourpoints[0],fourpoints[2],fourpoints[3]);
247 tr[1] = Triangle(fourpoints[2],fourpoints[1],fourpoints[3]);
248 }
249 else {
250 //cerr << dist << " cannot find diagonals" << endl;
251 return;
252 }
253 }
254 }
255 triangles.insert(triangles.end(), tr.begin(), tr.end());
256 }
257
SplitAtPlane(double z,vector<Triangle> & uppertriangles,vector<Triangle> & lowertriangles,const Matrix4d & T) const258 int Triangle::SplitAtPlane(double z,
259 vector<Triangle> &uppertriangles,
260 vector<Triangle> &lowertriangles,
261 const Matrix4d &T) const
262 {
263 vector<Vector3d> upper, lower;
264 if ((T*A).z()>z) upper.push_back(T*A); else lower.push_back(T*A);
265 if ((T*B).z()>z) upper.push_back(T*B); else lower.push_back(T*B);
266 if ((T*C).z()>z) upper.push_back(T*C); else lower.push_back(T*C);
267 Vector2d lstart,lend;
268 int cut = CutWithPlane(z,T,lstart,lend);
269 if (cut==0) return 0;
270 else if (cut==1) { // cut at a triangle point
271 if (upper.size()>lower.size())
272 upper.push_back(Vector3d(lstart.x(),lstart.y(),z));
273 else
274 lower.push_back(Vector3d(lstart.x(),lstart.y(),z));
275 }
276 else if (cut==2) {
277 upper.push_back(Vector3d(lstart.x(),lstart.y(),z));
278 upper.push_back(Vector3d(lend.x(),lend.y(),z));
279 lower.push_back(Vector3d(lstart.x(),lstart.y(),z));
280 lower.push_back(Vector3d(lend.x(),lend.y(),z));
281 }
282 vector<Triangle> uppertr,lowertr;
283 if (upper.size()==3) { // half of triangle with 1 triangle point
284 uppertr.push_back(Triangle(upper[0],upper[1],upper[2]));
285 }
286 else if (upper.size()==4) { // 0 and 1 are triangle points, 2 and 3 are cutline
287 triangulateQuadrilateral(upper, uppertr);
288 }
289 else cerr << "upper size " << upper.size() << endl;
290 if (lower.size()==3) {
291 lowertr.push_back(Triangle(lower[0],lower[1],lower[2]));
292 }
293 else if (lower.size()==4) {
294 triangulateQuadrilateral(lower, lowertr);
295 }
296 else cerr << "lower size " << lower.size() << endl;
297 Vector3d TN = T*Normal; TN.normalize();
298 for (guint i=0; i < uppertr.size(); i++)
299 if ((uppertr[i].Normal + TN).length()<0.1) uppertr[i].invertNormal();
300 for (guint i=0; i < lowertr.size(); i++)
301 if ((lowertr[i].Normal + TN).length()<0.1) lowertr[i].invertNormal();
302 uppertriangles.insert(uppertriangles.end(),uppertr.begin(),uppertr.end());
303 lowertriangles.insert(lowertriangles.end(),lowertr.begin(),lowertr.end());
304 return cut;
305 }
306
isInZrange(double zmin,double zmax,const Matrix4d & T) const307 bool Triangle::isInZrange(double zmin, double zmax, const Matrix4d &T) const
308 {
309 const Vector3d TA = T * A;
310 if (TA.z() < zmin || TA.z() > zmax) return false;
311 const Vector3d TB = T * B;
312 if (TB.z() < zmin || TB.z() > zmax) return false;
313 const Vector3d TC = T * C;
314 if (TC.z() < zmin || TC.z() > zmax) return false;
315 return true;
316 }
317
CutWithPlane(double z,const Matrix4d & T,Vector2d & lineStart,Vector2d & lineEnd) const318 int Triangle::CutWithPlane(double z, const Matrix4d &T,
319 Vector2d &lineStart,
320 Vector2d &lineEnd) const
321 {
322 Vector3d p;
323 double t;
324
325 const Vector3d TA = T * A;
326 const Vector3d TB = T * B;
327 const Vector3d TC = T * C;
328
329 int num_cutpoints = 0;
330 // Are the points on opposite sides of the plane?
331 if ((z <= TA.z()) != (z <= TB.z()))
332 {
333 t = (z - TA.z())/(TB.z()-TA.z());
334 p = TA + (TB - TA) * t;
335 lineStart = Vector2d(p.x(),p.y());
336 num_cutpoints = 1;
337 }
338 if ((z <= TB.z()) != (z <= TC.z()))
339 {
340 t = (z - TB.z())/(TC.z() - TB.z());
341 p = TB + (TC - TB) * t;
342 if(num_cutpoints > 0)
343 {
344 lineEnd = Vector2d(p.x(),p.y());
345 num_cutpoints = 2;
346 }
347 else
348 {
349 lineStart = Vector2d(p.x(),p.y());
350 num_cutpoints = 1;
351 }
352 }
353 if ((z <= TC.z()) != (z <= TA.z()))
354 {
355 t = (z - TC.z())/(TA.z() - TC.z());
356 p = TC + (TA - TC) * t;
357 lineEnd = Vector2d(p.x(),p.y());
358 if( lineEnd != lineStart ) num_cutpoints = 2;
359 }
360
361 return num_cutpoints;
362 }
363
draw(int gl_type) const364 void Triangle::draw(int gl_type) const
365 {
366 glBegin(gl_type);
367 glVertex3f(A.x(),A.y(),A.z());
368 glVertex3f(B.x(),B.y(),B.z());
369 glVertex3f(C.x(),C.y(),C.z());
370 glEnd();
371 }
372
373
getSTLfacet(const Matrix4d & T) const374 string Triangle::getSTLfacet(const Matrix4d &T) const
375 {
376 Vector3d TA=T*A,TB=T*B,TC=T*C,TN=T*Normal;TN.normalize();
377 stringstream sstr;
378 sstr << " facet normal " << TN.x() << " " << TN.x() << " " << TN.z() <<endl;
379 sstr << " outer loop" << endl;
380 sstr << " vertex "<< scientific << TA.x() << " " << TA.y() << " " << TA.z() <<endl;
381 sstr << " vertex "<< scientific << TB.x() << " " << TB.y() << " " << TB.z() <<endl;
382 sstr << " vertex "<< scientific << TC.x() << " " << TC.y() << " " << TC.z() <<endl;
383 sstr << " endloop" << endl;
384 sstr << " endfacet" << endl;
385 return sstr.str();
386 }
387
info() const388 string Triangle::info() const
389 {
390 ostringstream ostr;
391 ostr <<"Triangle A="<< A
392 <<", B="<< B
393 <<", C="<< C
394 <<", N="<< Normal ;
395 return ostr.str();
396 }
397