fix bug in case of empty rect
[scilab.git] / scilab / modules / graphic_objects / src / cpp / Triangulator.cpp
1 /*
2  *  Scilab ( http://www.scilab.org/ ) - This file is part of Scilab
3  *  Copyright (C) 2011-2012 - DIGITEO - Manuel Juliachs
4  *
5  * Copyright (C) 2012 - 2016 - Scilab Enterprises
6  *
7  * This file is hereby licensed under the terms of the GNU GPL v2.0,
8  * pursuant to article 5.3.4 of the CeCILL v.2.1.
9  * This file was originally licensed under the terms of the CeCILL v2.1,
10  * and continues to be available under such terms.
11  * For more information, see the COPYING file which you should have received
12  * along with this program.
13  *
14  */
15
16 #include "Triangulator.hxx"
17
18 #include <math.h>
19
20
21 void Triangulator::fillPoints(void)
22 {
23     points.resize(numPoints, Vector3d(0., 0., 0.));
24
25     //If any coordinate is constant project on the corresponding plane
26     if (xmin == xmax)
27     {
28         for (int i = 0; i < numPoints; i++)
29         {
30             points[i].x = inputPoints[i].z;
31             points[i].y = inputPoints[i].y;
32         }
33     }
34     else if (ymin == ymax)
35     {
36         for (int i = 0; i < numPoints; i++)
37         {
38             points[i].x = inputPoints[i].x;
39             points[i].y = inputPoints[i].z;
40         }
41     }
42     else if (zmin == zmax)
43     {
44         for (int i = 0; i < numPoints; i++)
45         {
46             points[i].x = inputPoints[i].x;
47             points[i].y = inputPoints[i].y;
48         }
49     }
50     //Otherwise, find the plane that best fit the data and rotate it to the xy plane
51     else
52     {
53         //m covariance matrix
54         //q autovector matrix
55         //d diagonal matrix
56         double m[3][3] = {{0., 0., 0.}, {0., 0., 0.}, {0., 0., 0.}};
57         //'q' and 'd' are not needed to be initialized
58         // since they are output paraments, but zero-initialize
59         // to follow coverity
60         double q[3][3] = {{0., 0., 0.}, {0., 0., 0.}, {0., 0., 0.}};
61         double d[3][3] = {{0., 0., 0.}, {0., 0., 0.}, {0., 0., 0.}};
62         Vector3d middle(0., 0., 0.);
63         Vector3d approxNormal(0., 0., 0.);
64
65         for (int i = 0; i < numPoints; i++)
66         {
67             middle.x += inputPoints[i].x;
68             middle.y += inputPoints[i].y;
69             middle.z += inputPoints[i].z;
70             Vector3d tmp = cross(minus(inputPoints[(i + 1) % numPoints], inputPoints[i]), minus(inputPoints[(i + 1) % numPoints], inputPoints[(i + 2) % numPoints]));
71             approxNormal = plus(approxNormal, tmp);
72         }
73
74         double den = sqrt(approxNormal.x * approxNormal.x + approxNormal.y * approxNormal.y + approxNormal.z * approxNormal.z);
75
76         approxNormal.x /= den;
77         approxNormal.y /= den;
78         approxNormal.z /= den;
79
80         middle.x /= numPoints;
81         middle.y /= numPoints;
82         middle.z /= numPoints;
83
84         for (int i = 0; i < numPoints; ++i)
85         {
86             points[i].x = inputPoints[i].x - middle.x;
87             points[i].y = inputPoints[i].y - middle.y;
88             points[i].z = inputPoints[i].z - middle.z;
89
90             m[0][0] += points[i].x * points[i].x;
91             m[0][1] += points[i].x * points[i].y;
92             m[0][2] += points[i].x * points[i].z;
93             m[1][1] += points[i].y * points[i].y;
94             m[1][2] += points[i].y * points[i].z;
95             m[2][2] += points[i].z * points[i].z;
96         }
97
98
99         m[1][0] = m[0][1];
100         m[2][0] = m[0][2];
101         m[2][1] = m[1][2];
102
103         diagonalize(m, q, d);
104
105         int col = d[0][0] < d[1][1] ? (d[0][0] < d[2][2] ? 0 : 2) : (d[1][1] < d[2][2] ? 1 : 2);
106
107         Vector3d normal(q[0][col], q[1][col], q[2][col]);
108
109         // if the angle between approxNomal and normal is
110         // larger than +/- pi/2, we invert the normal and keep the
111         // triangle orientation correct
112         double sign = dot(approxNormal, normal) < 0. ? -1. : 1.;
113
114         normal.x *= sign;
115         normal.y *= sign;
116         normal.z *= sign;
117
118         double c, s;
119
120         c = normal.y / sqrt(normal.x * normal.x + normal.y * normal.y);
121         s = sqrt(1. - c * c);
122         s = normal.x < 0. ? -s : s;
123
124         double rotz[3][3] = {{c, -s, 0.}, {s, c, 0.}, {0., 0., 1.}};
125
126         matrixVectorMul(rotz, normal);
127
128         c = normal.z / sqrt(normal.y * normal.y + normal.z * normal.z);
129         s = sqrt(1. - c * c);
130         s = normal.y < 0. ? s : -s;
131
132         double rotx[3][3] = {{1., 0., 0.}, {0., c, s}, {0., -s, c}};
133         double composedRot[3][3] = {{0., 0., 0.}, {0., 0., 0.}, {0., 0., 0.}};
134         matrixMatrixMul(rotx, rotz, composedRot);
135
136         xmin = ymin = zmin = std::numeric_limits<double>::max();
137         xmax = ymax = zmax = std::numeric_limits<double>::lowest();
138
139         for (int i = 0; i < numPoints; ++i)
140         {
141             matrixVectorMul(composedRot, inputPoints[i], points[i]);
142
143             xmin = xmin < points[i].x ? xmin : points[i].x;
144             ymin = ymin < points[i].y ? ymin : points[i].y;
145             zmin = zmin < points[i].z ? zmin : points[i].z;
146
147             xmax = xmax > points[i].x ? xmax : points[i].x;
148             ymax = ymax > points[i].y ? ymax : points[i].y;
149             zmax = zmax > points[i].z ? zmax : points[i].z;
150
151             points[i].z = 0.;
152         }
153     }
154 }
155
156 double Triangulator::computeArea(void)
157 {
158     double area = 0.;
159
160     for (int i = 0; i < numPoints; i++)
161     {
162         int ip1 = (i + 1) % numPoints;
163
164         area += ((points[i].x * points[ip1].y) - (points[i].y * points[ip1].x));
165     }
166
167     area *= 0.5;
168
169     return area;
170 }
171
172 void Triangulator::fillVertexIndices(void)
173 {
174     if (flipped)
175     {
176         for (int i = numPoints - 1; i >= 0; i--)
177         {
178             vertexIndices.push_back(i);
179         }
180     }
181     else
182     {
183         for (int i = 0; i < numPoints; i++)
184         {
185             vertexIndices.push_back(i);
186         }
187     }
188 }
189
190 void Triangulator::removeColinearVertices(void)
191 {
192     double dp = 0.;
193     std::list<int>::iterator vi, vim1, vip1;
194
195     std::vector<Vector3d> sievedPoints;
196     std::list<int> tmpVertexIndices;
197
198     int numColinear = 0;
199     int index = 0;
200
201     for (vi = vertexIndices.begin(); vi != vertexIndices.end(); vi++)
202     {
203         getAdjacentVertices(vi, vim1, vip1);
204
205         dp = computeDotProduct(*vim1, *vi, *vip1);
206
207         if ((!compareVertices(points[*vim1], points[*vi]) && !compareVertices(points[*vi], points[*vip1])) &&
208                 fabs(dp) < TOLERANCE)
209         {
210             numColinear++;
211         }
212         else
213         {
214             sievedPoints.push_back(points[*vi]);
215             actualVertexIndices.push_back(*vi);
216         }
217
218         index++;
219     }
220
221     points.clear();
222
223     if (flipped)
224     {
225         /* Reverse the actual vertex indices list */
226         std::vector<int> tmpList;
227
228         for (size_t i = 0; i < actualVertexIndices.size(); i++)
229         {
230             tmpList.push_back(actualVertexIndices[actualVertexIndices.size() - i - 1]);
231         }
232
233         actualVertexIndices.clear();
234
235         for (size_t i = 0; i < tmpList.size(); i++)
236         {
237             actualVertexIndices.push_back(tmpList[i]);
238         }
239
240         tmpList.clear();
241     }
242
243     if (flipped)
244     {
245         for (size_t i = 0; i < sievedPoints.size(); i++)
246         {
247             points.push_back(sievedPoints[sievedPoints.size() - i - 1]);
248         }
249     }
250     else
251     {
252         for (size_t i = 0; i < sievedPoints.size(); i++)
253         {
254             points.push_back(sievedPoints[i]);
255         }
256     }
257
258     /* Must be updated */
259     numPoints = (int)points.size();
260
261     sievedPoints.clear();
262
263     numColinearVertices = numColinear;
264 }
265
266 void Triangulator::removeDuplicateVertices(void)
267 {
268     int numDuplicateVertices = 0;
269     std::vector<int> duplicateFlagArray;
270
271     std::vector<Vector3d> sievedPoints;
272     std::vector<int> tmpActualVertexIndices;
273
274     sievedPoints.clear();
275     tmpActualVertexIndices.clear();
276
277     duplicateFlagArray.resize(points.size());
278
279     for (size_t i = 0; i < points.size(); i++)
280     {
281         int ic = ((int)i + 1) % (int)points.size();
282         int icm1 = (int)i;
283
284         Vector3d vi = points[icm1];
285         Vector3d vip1 = points[ic];
286
287         if (compareVertices(vi, vip1))
288         {
289             numDuplicateVertices++;
290             duplicateFlagArray[ic] = 1;
291         }
292         else
293         {
294             duplicateFlagArray[ic] = 0;
295         }
296     }
297
298     for (size_t i = 0; i < points.size(); i++)
299     {
300         if (duplicateFlagArray[i] == 0)
301         {
302             /* Keep as it is not a duplicate */
303             sievedPoints.push_back(points[i]);
304             tmpActualVertexIndices.push_back(actualVertexIndices[i]);
305         }
306     }
307
308     actualVertexIndices.clear();
309     points.clear();
310
311     /* Copy the two new lists */
312     for (size_t i = 0; i < tmpActualVertexIndices.size(); i++)
313     {
314         actualVertexIndices.push_back(tmpActualVertexIndices[i]);
315     }
316
317     for (size_t i = 0; i < sievedPoints.size(); i++)
318     {
319         points.push_back(sievedPoints[i]);
320     }
321
322     /* Must be updated */
323     numPoints = (int)points.size();
324
325     duplicateFlagArray.clear();
326     sievedPoints.clear();
327     tmpActualVertexIndices.clear();
328 }
329
330 void Triangulator::fillConvexVerticesList(void)
331 {
332     std::list<int>::iterator vi;
333
334     flagList.resize(vertexIndices.size());
335
336     for (vi = vertexIndices.begin(); vi != vertexIndices.end(); vi++)
337     {
338         if (isConvex(vi))
339         {
340             convexList.push_back(*vi);
341             flagList[*vi] = true;
342         }
343         else
344         {
345             reflexList.push_back(*vi);
346             flagList[*vi] = false;
347         }
348     }
349 }
350
351 void Triangulator::fillEarList(void)
352 {
353     std::list<int>::iterator vi;
354     bool res = false;
355
356     for (vi = vertexIndices.begin(); vi != vertexIndices.end(); vi++)
357     {
358         if (flagList[*vi])
359         {
360             res = isAnEar(vi);
361
362             if (res)
363             {
364                 earList.push_back(*vi);
365             }
366         }
367     }
368 }
369
370 void Triangulator::getAdjacentVertices(std::list<int>::iterator vi, std::list<int>::iterator& vim1, std::list<int>::iterator& vip1)
371 {
372     if (*vi == vertexIndices.front())
373     {
374         vim1 = vertexIndices.end();
375         vim1--;
376     }
377     else
378     {
379         vim1 = vi;
380         vim1--;
381     }
382
383     if (*vi == vertexIndices.back())
384     {
385         vip1 = vertexIndices.begin();
386     }
387     else
388     {
389         vip1 = vi;
390         vip1++;
391     }
392 }
393
394 bool Triangulator::isConvex(std::list<int>::iterator vertex)
395 {
396     double dp = 0.;
397     std::list<int>::iterator pred, succ;
398
399     getAdjacentVertices(vertex, pred, succ);
400
401     dp = computeDotProduct(*pred, *vertex, *succ);
402
403     if (dp >= 0.)
404     {
405         return true;
406     }
407     else
408     {
409         return false;
410     }
411 }
412
413 bool Triangulator::isAnEar(std::list<int>::iterator vertex)
414 {
415     bool isEar = true;
416     std::list<int>::iterator pred, succ;
417     std::list<int>::iterator vi;
418
419     Vector3d v0, v1, v2;
420
421     getAdjacentVertices(vertex, pred, succ);
422
423     v0 = points[*pred];
424     v1 = points[*vertex];
425     v2 = points[*succ];
426
427     for (vi = reflexList.begin(); vi != reflexList.end(); vi++)
428     {
429         if (*vi == *pred || *vi == *vertex || *vi == *succ)
430         {
431             continue;
432         }
433         else
434         {
435             bool res;
436
437             res = pointInTriangle(v0, v1, v2, points[*vi]);
438
439             if (res)
440             {
441                 isEar = false;
442                 break;
443             }
444         }
445
446     }
447
448     numEarTests++;
449
450     return isEar;
451 }
452
453 /* To do: streamline */
454 void Triangulator::updateVertex(std::list<int>::iterator vertex)
455 {
456     bool res = false;
457
458     if (flagList[*vertex])
459     {
460         /*
461          * Convex vertex: remove it from the ear list if not an ear any more,
462          * else add it if not already present.
463          */
464         res = isAnEar(vertex);
465
466         if (!res)
467         {
468             earList.remove(*vertex);
469             numDelEars++;
470         }
471         else
472         {
473             std::list<int>::iterator foundEar;
474
475             foundEar = find(earList.begin(), earList.end(), *vertex);
476
477             if (foundEar == earList.end())
478             {
479                 earList.push_front(*vertex);
480                 numAddEars++;
481             }
482         }
483     }
484     else
485     {
486         /*
487          * Non-convex vertex: may become convex, so its flag must be updated, as well
488          * as the reflex vertex list.
489          * Also determine whether it has become an ear and update the ear list accordingly.
490          */
491         if (isConvex(vertex))
492         {
493             flagList[*vertex] = true;
494         }
495
496         if (flagList[*vertex])
497         {
498             res = isAnEar(vertex);
499
500             if (res)
501             {
502                 std::list<int>::iterator foundEar;
503
504                 foundEar = find(earList.begin(), earList.end(), *vertex);
505
506                 if (foundEar == earList.end())
507                 {
508                     earList.push_front(*vertex);
509                     numAddEars++;
510                 }
511             }
512
513             reflexList.remove(*vertex);
514         }
515     }
516 }
517
518 double Triangulator::computeDotProduct(int im1, int i, int ip1)
519 {
520     double dp = 0.;
521     Vector3d eim1p;
522
523     Vector3d eim1 = minus(points[i], points[im1]);
524     Vector3d ei = minus(points[ip1], points[i]);
525
526     /* Normalize */
527     eim1 = normalize(eim1);
528     ei = normalize(ei);
529
530     /* Ought to use cross product */
531     eim1p = perpendicularVector(eim1);
532
533     dp = dot(eim1p, ei);
534
535     return dp;
536 }
537
538 bool Triangulator::pointInTriangle(Vector3d A, Vector3d B, Vector3d C, Vector3d P)
539 {
540     double dot00 = 0., dot01 = 0., dot02 = 0., dot11 = 0., dot12 = 0.;
541     double invDenom = 0.;
542     double u = 0., v = 0.;
543
544     Vector3d v0, v1, v2;
545
546     /* Compute vectors */
547     v0 = minus(C, A);
548     v1 = minus(B, A);
549     v2 = minus(P, A);
550
551     /* Compute dot products */
552     dot00 = dot(v0, v0);
553     dot01 = dot(v0, v1);
554     dot02 = dot(v0, v2);
555     dot11 = dot(v1, v1);
556     dot12 = dot(v1, v2);
557
558     /* Compute barycentric coordinates */
559     invDenom = 1 / (dot00 * dot11 - dot01 * dot01);
560     u = (dot11 * dot02 - dot01 * dot12) * invDenom;
561     v = (dot00 * dot12 - dot01 * dot02) * invDenom;
562
563     /* Check if point is in triangle */
564     return (u >= 0) && (v >= 0) && (u + v < 1);
565 }
566
567 Vector3d Triangulator::minus(Vector3d v0, Vector3d v1)
568 {
569     Vector3d res;
570
571     res.x = v0.x - v1.x;
572     res.y = v0.y - v1.y;
573     res.z = v0.z - v1.z;
574
575     return res;
576 }
577
578 double Triangulator::dot(Vector3d v0, Vector3d v1)
579 {
580     return v0.x * v1.x + v0.y * v1.y + v0.z * v1.z;
581 }
582
583 Vector3d Triangulator::normalize(Vector3d v)
584 {
585     double n = sqrt(v.x * v.x + v.y * v.y);
586
587     if (n < EPSILON)
588     {
589         n = 1.;
590     }
591
592     v.x /= n;
593     v.y /= n;
594
595     return v;
596 }
597
598 Vector3d Triangulator::perpendicularVector(Vector3d v)
599 {
600     Vector3d perp;
601     perp.x = -v.y;
602     perp.y = v.x;
603     perp.z = 0.;
604
605     return perp;
606 }
607
608 bool Triangulator::compareVertices(Vector3d v0, Vector3d v1)
609 {
610     if (areEqual(v0.x, v1.x) && areEqual(v0.y, v1.y))
611     {
612         return true;
613     }
614     else
615     {
616         return false;
617     }
618 }
619
620 bool Triangulator::areEqual(double x0, double x1)
621 {
622     double maxAbs = fabs(x0) > fabs(x1) ? fabs(x0) : fabs(x1);
623
624     if (fabs(x0 - x1) <= EPSILON)
625     {
626         return true;
627     }
628     else if (fabs(x0 - x1) <= EPSILON * maxAbs)
629     {
630         return true;
631     }
632     else
633     {
634         return false;
635     }
636 }
637
638 Triangulator::Triangulator(void)
639 {
640     numPoints = 0;
641     numInitPoints = 0;
642     flipped = false;
643     numAddEars = 0;
644     numDelEars = 0;
645     numSteps = 0;
646     numEarTests = 0;
647     numColinearVertices = 0;
648
649     xmin = ymin = zmin = std::numeric_limits<double>::max();
650     xmax = ymax = zmax = std::numeric_limits<double>::lowest();
651
652     inputPoints.clear();
653     points.clear();
654     vertexIndices.clear();
655     actualVertexIndices.clear();
656     earList.clear();
657     convexList.clear();
658     reflexList.clear();
659     flagList.clear();
660     triangleIndices.clear();
661 }
662
663 void Triangulator::initialize(void)
664 {
665     double area = 0.;
666
667     numPoints = (int)inputPoints.size();
668
669     fillPoints();
670     normalizePoints();
671     area = computeArea();
672
673     if (area < 0.)
674     {
675         flipped = true;
676     }
677     else
678     {
679         flipped = false;
680     }
681
682     fillVertexIndices();
683
684     numInitPoints = numPoints;
685
686     /*
687      * An additional colinear vertices removal pass should be performed
688      * after the duplicate removal pass, as some adjacent edges may become colinear.
689      */
690     removeColinearVertices();
691     removeDuplicateVertices();
692
693     /* Vertex indices must be re-filled */
694     vertexIndices.clear();
695     fillVertexIndices();
696
697     fillConvexVerticesList();
698     fillEarList();
699 }
700
701 void Triangulator::addPoint(double x, double y, double z)
702 {
703     Vector3d point(x, y, z);
704
705     if (x < xmin)
706     {
707         xmin = x;
708     }
709     if (x > xmax)
710     {
711         xmax = x;
712     }
713
714     if (y < ymin)
715     {
716         ymin = y;
717     }
718     if (y > ymax)
719     {
720         ymax = y;
721     }
722
723     if (z < zmin)
724     {
725         zmin = z;
726     }
727     if (z > zmax)
728     {
729         zmax = z;
730     }
731
732     inputPoints.push_back(point);
733 }
734
735 void Triangulator::triangulate(void)
736 {
737     int triIndex = 0;
738     std::list<int>::iterator it;
739     std::list<int>::iterator vertex, pred, succ;
740
741     numSteps = 0;
742
743     while (vertexIndices.size() >= 3 && earList.size() > 0)
744     {
745         int v0 = 0, v1 = 0, v2 = 0;
746         int v0actual = 0, v1actual = 0, v2actual = 0;
747         int vertexIndex = 0;
748
749         it = earList.begin();
750
751         /* If not found, we should break out of the loop. To be checked. */
752         vertex = find(vertexIndices.begin(), vertexIndices.end(), *it);
753         vertexIndex = *vertex;
754
755         getAdjacentVertices(vertex, pred, succ);
756
757         /* Remove */
758         vertexIndices.remove(*vertex);
759         earList.pop_front();
760
761         numDelEars++;
762
763
764         triIndex = *pred;
765         v0 = triIndex;
766         v1 = vertexIndex;
767         triIndex = *succ;
768         v2 = triIndex;
769
770         v0actual = actualVertexIndices[v0];
771         v1actual = actualVertexIndices[v1];
772         v2actual = actualVertexIndices[v2];
773
774         triangleIndices.push_back(v0actual);
775         triangleIndices.push_back(v1actual);
776         triangleIndices.push_back(v2actual);
777
778         /* Update the predecessor vertex */
779         updateVertex(pred);
780
781         /* Update the successor vertex */
782         updateVertex(succ);
783
784         numSteps++;
785     }
786 }
787
788 int Triangulator::getNumberTriangles(void)
789 {
790     return (int)(triangleIndices.size() / 3);
791 }
792
793 int* Triangulator::getIndices(void)
794 {
795     return triangleIndices.data();
796 }
797
798 int Triangulator::getNumberSteps(void)
799 {
800     return numSteps;
801 }
802
803 int Triangulator::getNumberEarTests(void)
804 {
805     return numEarTests;
806 }
807
808 void Triangulator::clear(void)
809 {
810     inputPoints.clear();
811     points.clear();
812     numPoints = 0;
813     numInitPoints = 0;
814
815     vertexIndices.clear();
816     actualVertexIndices.clear();
817     earList.clear();
818     convexList.clear();
819     reflexList.clear();
820     flagList.clear();
821     triangleIndices.clear();
822
823     numAddEars = 0;
824     numDelEars = 0;
825     numSteps = 0;
826     numEarTests = 0;
827     numColinearVertices = 0;
828 }
829
830 void Triangulator::matrixMatrixMul(double(&a)[3][3], double(&b)[3][3], double(&out)[3][3])
831 {
832     for (int i = 0; i < 3; ++i)
833     {
834         for (int j = 0; j < 3; ++j)
835         {
836             out[i][j] = 0.;
837             for (int k = 0; k < 3; ++k)
838             {
839                 out[i][j] += a[i][k] * b[k][j];
840             }
841         }
842     }
843 }
844
845 void Triangulator::matrixVectorMul(double(&m)[3][3], Vector3d & pin, Vector3d & pout)
846 {
847     pout.x = pin.x * m[0][0] + pin.y * m[0][1] + pin.z * m[0][2];
848     pout.y = pin.x * m[1][0] + pin.y * m[1][1] + pin.z * m[1][2];
849     pout.z = pin.x * m[2][0] + pin.y * m[2][1] + pin.z * m[2][2];
850 }
851
852 void Triangulator::matrixVectorMul(double(&m)[3][3], Vector3d & pinOut)
853 {
854     Vector3d copy(pinOut.x, pinOut.y, pinOut.z);
855     pinOut.x = copy.x * m[0][0] + copy.y * m[0][1] + copy.z * m[0][2];
856     pinOut.y = copy.x * m[1][0] + copy.y * m[1][1] + copy.z * m[1][2];
857     pinOut.z = copy.x * m[2][0] + copy.y * m[2][1] + copy.z * m[2][2];
858 }
859
860 void Triangulator::normalizePoints()
861 {
862     double xscale = xmax == xmin ? 1. : 1. / (xmax - xmin);
863     double yscale = ymax == ymin ? 1. : 1. / (ymax - ymin);
864     double zscale = zmax == zmin ? 1. : 1. / (zmax - zmin);
865     for (int i = 0; i < numPoints; ++i)
866     {
867         points[i].x = (points[i].x - xmin) * xscale;
868         points[i].y = (points[i].y - ymin) * yscale;
869         points[i].z = (points[i].z - zmin) * zscale;
870     }
871 }
872
873 Vector3d Triangulator::cross(Vector3d a, Vector3d b)
874 {
875     return Vector3d(
876                a.y * b.z - b.y * a.z,
877                a.z * b.x - a.x * b.z,
878                a.x * b.y - a.y * b.x
879            );
880 }
881
882 Vector3d Triangulator::plus(Vector3d v0, Vector3d v1)
883 {
884     return Vector3d(v0.x + v1.x, v0.y + v1.y, v0.z + v1.z);
885 }
886
887 void Triangulator::diagonalize(const double (&A)[3][3], double (&Q)[3][3], double (&D)[3][3])
888 {
889     // A must be a symmetric matrix.
890     // returns Q and D such that
891     // Diagonal matrix D = QT * A * Q;  and  A = Q*D*QT
892     const int maxsteps = 24;  // certainly wont need that many.
893     double q[4] = {0., 0., 0., 1.};
894     for (int i = 0; i < maxsteps; ++i)
895     {
896         // quat to matrix
897         double sqx = q[0] * q[0];
898         double sqy = q[1] * q[1];
899         double sqz = q[2] * q[2];
900         double sqw = q[3] * q[3];
901         Q[0][0] = (sqx - sqy - sqz + sqw);
902         Q[1][1] = (-sqx + sqy - sqz + sqw);
903         Q[2][2] = (-sqx - sqy + sqz + sqw);
904         double tmp1 = q[0] * q[1];
905         double tmp2 = q[2] * q[3];
906         Q[1][0] = 2. * (tmp1 + tmp2);
907         Q[0][1] = 2. * (tmp1 - tmp2);
908         tmp1 = q[0] * q[2];
909         tmp2 = q[1] * q[3];
910         Q[2][0] = 2. * (tmp1 - tmp2);
911         Q[0][2] = 2. * (tmp1 + tmp2);
912         tmp1 = q[1] * q[2];
913         tmp2 = q[0] * q[3];
914         Q[2][1] = 2. * (tmp1 + tmp2);
915         Q[1][2] = 2. * (tmp1 - tmp2);
916
917         double AQ[3][3];
918         // AQ = A * Q
919         AQ[0][0] = Q[0][0] * A[0][0] + Q[1][0] * A[0][1] + Q[2][0] * A[0][2];
920         AQ[0][1] = Q[0][1] * A[0][0] + Q[1][1] * A[0][1] + Q[2][1] * A[0][2];
921         AQ[0][2] = Q[0][2] * A[0][0] + Q[1][2] * A[0][1] + Q[2][2] * A[0][2];
922         AQ[1][0] = Q[0][0] * A[0][1] + Q[1][0] * A[1][1] + Q[2][0] * A[1][2];
923         AQ[1][1] = Q[0][1] * A[0][1] + Q[1][1] * A[1][1] + Q[2][1] * A[1][2];
924         AQ[1][2] = Q[0][2] * A[0][1] + Q[1][2] * A[1][1] + Q[2][2] * A[1][2];
925         AQ[2][0] = Q[0][0] * A[0][2] + Q[1][0] * A[1][2] + Q[2][0] * A[2][2];
926         AQ[2][1] = Q[0][1] * A[0][2] + Q[1][1] * A[1][2] + Q[2][1] * A[2][2];
927         AQ[2][2] = Q[0][2] * A[0][2] + Q[1][2] * A[1][2] + Q[2][2] * A[2][2];
928         // D = Qt * AQ
929         D[0][0] = AQ[0][0] * Q[0][0] + AQ[1][0] * Q[1][0] + AQ[2][0] * Q[2][0];
930         D[0][1] = AQ[0][0] * Q[0][1] + AQ[1][0] * Q[1][1] + AQ[2][0] * Q[2][1];
931         D[0][2] = AQ[0][0] * Q[0][2] + AQ[1][0] * Q[1][2] + AQ[2][0] * Q[2][2];
932         D[1][0] = AQ[0][1] * Q[0][0] + AQ[1][1] * Q[1][0] + AQ[2][1] * Q[2][0];
933         D[1][1] = AQ[0][1] * Q[0][1] + AQ[1][1] * Q[1][1] + AQ[2][1] * Q[2][1];
934         D[1][2] = AQ[0][1] * Q[0][2] + AQ[1][1] * Q[1][2] + AQ[2][1] * Q[2][2];
935         D[2][0] = AQ[0][2] * Q[0][0] + AQ[1][2] * Q[1][0] + AQ[2][2] * Q[2][0];
936         D[2][1] = AQ[0][2] * Q[0][1] + AQ[1][2] * Q[1][1] + AQ[2][2] * Q[2][1];
937         D[2][2] = AQ[0][2] * Q[0][2] + AQ[1][2] * Q[1][2] + AQ[2][2] * Q[2][2];
938         double o[3] = {D[1][2], D[0][2], D[0][1]};
939         double m[3] = {fabs(o[0]), fabs(o[1]), fabs(o[2])};
940
941         int k0 = (m[0] > m[1] && m[0] > m[2]) ? 0 : (m[1] > m[2]) ? 1 : 2; // index of largest element of offdiag
942         int k1 = (k0 + 1) % 3;
943         int k2 = (k0 + 2) % 3;
944         if (o[k0] == 0.)
945         {
946             break;  // diagonal already
947         }
948         double thet = (D[k2][k2] - D[k1][k1]) / (2. * o[k0]);
949         double sgn = (thet > 0.) ? 1. : -1.;
950         thet *= sgn; // make it positive
951         double t = sgn / (thet + ((thet < 1.E6) ? sqrt(thet * thet + 1.) : thet)); // sign(T)/(|T|+sqrt(T^2+1))
952         double c = 1. / sqrt(t * t + 1.); //  c= 1/(t^2+1) , t=s/c
953         if (c == 1.)
954         {
955             break;  // no room for improvement - reached machine precision.
956         }
957         double jr[4] = {0., 0., 0., 0.};
958         jr[k0] = sgn * sqrt((1. - c) / 2.); // using 1/2 angle identity sin(a/2) = sqrt((1-cos(a))/2)
959         jr[k0] *= -1.; // since our quat-to-matrix convention was for v*M instead of M*v
960         jr[3] = sqrt(1.f - jr[k0] * jr[k0]);
961         if (jr[3] == 1.)
962         {
963             break; // reached limits of floating point precision
964         }
965         q[0] = (q[3] * jr[0] + q[0] * jr[3] + q[1] * jr[2] - q[2] * jr[1]);
966         q[1] = (q[3] * jr[1] - q[0] * jr[2] + q[1] * jr[3] + q[2] * jr[0]);
967         q[2] = (q[3] * jr[2] + q[0] * jr[1] - q[1] * jr[0] + q[2] * jr[3]);
968         q[3] = (q[3] * jr[3] - q[0] * jr[0] - q[1] * jr[1] - q[2] * jr[2]);
969         double mq = sqrt(q[0] * q[0] + q[1] * q[1] + q[2] * q[2] + q[3] * q[3]);
970         q[0] /= mq;
971         q[1] /= mq;
972         q[2] /= mq;
973         q[3] /= mq;
974     }
975 }