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 &center)
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