1 // Some tests for vgl_quadric_3d
2 // J.L. Mundy, June 2017.
3 #include <iostream>
4 #include <sstream>
5 #include "testlib/testlib_test.h"
6 #include "vgl/vgl_quadric_3d.h"
7 #ifdef _MSC_VER
8 #  include "vcl_msvc_warnings.h"
9 #endif
10 #include <vgl/vgl_tolerance.h>
11 // multiply square matrices
12 static std::vector<std::vector<double>>
mul(std::vector<std::vector<double>> const & a,std::vector<std::vector<double>> const & b)13 mul(std::vector<std::vector<double>> const & a, std::vector<std::vector<double>> const & b)
14 {
15   size_t N = a.size();
16   std::vector<std::vector<double>> out(N, std::vector<double>(N, 0.0));
17   for (unsigned i = 0; i < N; ++i)
18     for (unsigned j = 0; j < N; ++j)
19     {
20       double accum = a[i][0] * b[0][j];
21       for (unsigned k = 1; k < N; ++k)
22         accum += a[i][k] * b[k][j];
23       out[i][j] = accum;
24     }
25   return out;
26 }
27 static std::vector<std::vector<double>>
transform_quadric(std::vector<std::vector<double>> const & T,std::vector<std::vector<double>> const & Q)28 transform_quadric(std::vector<std::vector<double>> const & T, std::vector<std::vector<double>> const & Q)
29 {
30   std::vector<std::vector<double>> Tt(4, std::vector<double>(4, 0.0));
31   for (size_t r = 0; r < 4; ++r)
32     for (size_t c = 0; c < 4; ++c)
33       Tt[r][c] = T[c][r];
34   std::vector<std::vector<double>> temp(4, std::vector<double>(4, 0.0));
35   temp = mul(Q, T);
36   temp = mul(Tt, temp);
37   return temp;
38 }
39 static void
test_quadric()40 test_quadric()
41 {
42   /*
43    coincident_planes              x^2=0
44    imaginary_ellipsoid           (x^2)/(a^2)+(y^2)/(b^2)+(z^2)/(c^2)=-1
45    real_ellipsoid                (x^2)/(a^2)+(y^2)/(b^2)+(z^2)/(c^2)=1
46    imaginary_elliptic_cone       (x^2)/(a^2)+(y^2)/(b^2)+(z^2)/(c^2)=0
47    real_elliptic_cone            (x^2)/(a^2)+(y^2)/(b^2)-(z^2)/(c^2)=0
48    imaginary_elliptic_cylinder   (x^2)/(a^2)+(y^2)/(b^2)=-1
49    real_elliptic_cylinder        (x^2)/(a^2)+(y^2)/(b^2)=1
50    elliptic_paraboloid           z=(x^2)/(a^2)+(y^2)/(b^2)
51    hyperbolic_cylinder           (x^2)/(a^2)-(y^2)/(b^2)=-1
52    hyperbolic_paraboloid         z=(y^2)/(b^2)-(x^2)/(a^2)
53    hyperboloid_of_one_sheet      (x^2)/(a^2)+(y^2)/(b^2)-(z^2)/(c^2)=1
54    hyperboloid_of_two_sheets     (x^2)/(a^2)+(y^2)/(b^2)-(z^2)/(c^2)=-1
55    imaginary_intersecting_planes (x^2)/(a^2)+(y^2)/(b^2)=0
56    real_intersecting_planes      (x^2)/(a^2)-(y^2)/(b^2)=0
57    parabolic_cylinder             x^2+2rz=0
58    imaginary_parallel_planes      x^2=-a^2
59    real_parallel planes           x^2=a^2
60  */
61   vgl_quadric_3d<double> coincident_planes(1.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0);
62   vgl_quadric_3d<double> imaginary_ellipsoid(1.0, 1.0, 1.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1.0);
63   vgl_quadric_3d<double> real_ellipsoid(1.0, 1.0, 1.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, -1.0);
64   vgl_quadric_3d<double> imaginary_elliptic_cone(1.0, 1.0, 1.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0);
65   vgl_quadric_3d<double> real_elliptic_cone(1.0, 1.0, -1.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0);
66   vgl_quadric_3d<double> imaginary_elliptic_cylinder(1.0, 1.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1.0);
67   vgl_quadric_3d<double> real_elliptic_cylinder(1.0, 1.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, -1.0);
68   vgl_quadric_3d<double> elliptic_paraboloid(1.0, 1.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, -1.0, 0.0);
69   vgl_quadric_3d<double> hyperbolic_cylinder(1.0, -1.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1.0);
70   vgl_quadric_3d<double> hyperbolic_paraboloid(-1.0, 1.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, -1.0, 0.0);
71   vgl_quadric_3d<double> hyperboloid_of_one_sheet(1.0, 1.0, -1.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, -1.0);
72   vgl_quadric_3d<double> hyperboloid_of_two_sheets(1.0, 1.0, -1.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1.0);
73   vgl_quadric_3d<double> imaginary_intersecting_planes(1.0, 1.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0);
74   vgl_quadric_3d<double> real_intersecting_planes(1.0, -1.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0);
75   vgl_quadric_3d<double> parabolic_cylinder(1.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1.0, 0.0);
76   vgl_quadric_3d<double> imaginary_parallel_planes(1.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1.0);
77   vgl_quadric_3d<double> real_parallel_planes(1.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, -1.0);
78 
79   bool good = coincident_planes.type() == vgl_quadric_3d<double>::coincident_planes;
80   good = good && imaginary_ellipsoid.type() == vgl_quadric_3d<double>::imaginary_ellipsoid;
81   good = good && real_ellipsoid.type() == vgl_quadric_3d<double>::real_ellipsoid;
82   good = good && imaginary_elliptic_cone.type() == vgl_quadric_3d<double>::imaginary_elliptic_cone;
83   good = good && real_elliptic_cone.type() == vgl_quadric_3d<double>::real_elliptic_cone;
84   good = good && imaginary_elliptic_cylinder.type() == vgl_quadric_3d<double>::imaginary_elliptic_cylinder;
85   good = good && real_elliptic_cylinder.type() == vgl_quadric_3d<double>::real_elliptic_cylinder;
86   good = good && elliptic_paraboloid.type() == vgl_quadric_3d<double>::elliptic_paraboloid;
87   good = good && hyperbolic_cylinder.type() == vgl_quadric_3d<double>::hyperbolic_cylinder;
88   good = good && elliptic_paraboloid.type() == vgl_quadric_3d<double>::elliptic_paraboloid;
89   good = good && hyperbolic_paraboloid.type() == vgl_quadric_3d<double>::hyperbolic_paraboloid;
90   good = good && hyperboloid_of_one_sheet.type() == vgl_quadric_3d<double>::hyperboloid_of_one_sheet;
91   good = good && hyperboloid_of_two_sheets.type() == vgl_quadric_3d<double>::hyperboloid_of_two_sheets;
92   good = good && imaginary_intersecting_planes.type() == vgl_quadric_3d<double>::imaginary_intersecting_planes;
93   good = good && real_intersecting_planes.type() == vgl_quadric_3d<double>::real_intersecting_planes;
94   good = good && parabolic_cylinder.type() == vgl_quadric_3d<double>::parabolic_cylinder;
95   good = good && imaginary_parallel_planes.type() == vgl_quadric_3d<double>::imaginary_parallel_planes;
96   good = good && real_parallel_planes.type() == vgl_quadric_3d<double>::real_parallel_planes;
97   TEST("Quadric classification", good, true);
98 
99   std::vector<std::vector<double>> Q = elliptic_paraboloid.coef_matrix(), Qtrans;
100   std::vector<std::vector<double>> T(4, std::vector<double>(4, 0.0));
101   double p = 0.785, q = 0.866;
102   double cp = cos(p), sp = sin(p);
103   double cq = cos(q), sq = sin(q);
104   double tx = 1.0, ty = 2.0, tz = 3.0;
105   T[0][0] = cp;
106   T[0][1] = -sp;
107   T[0][3] = tx;
108   T[1][0] = cq * sp;
109   T[1][1] = cp * cq;
110   T[1][2] = -sq;
111   T[1][3] = ty;
112   T[2][0] = sp * sq;
113   T[2][1] = cp * sq;
114   T[2][2] = cq;
115   T[2][3] = tz;
116   T[3][3] = 1.0;
117   Qtrans = transform_quadric(T, Q); // Note T is actually the inverse transformation since transform_quadric == T^tQT
118   vgl_quadric_3d<double> test(Qtrans);
119   good = test.type() == vgl_quadric_3d<double>::elliptic_paraboloid;
120   Q = real_elliptic_cone.coef_matrix();
121   Qtrans = transform_quadric(T, Q);
122   vgl_quadric_3d<double> test2(Qtrans);
123   good = good && test2.type() == vgl_quadric_3d<double>::real_elliptic_cone;
124   TEST("Transformed quadric classification", good, true);
125   std::string name = test2.type_by_number(test2.type());
126   good = (name == "real_elliptic_cone");
127   TEST("type_by_number", good, true);
128   vgl_quadric_3d<double>::vgl_quadric_type typ = test2.type_by_name("real_elliptic_cone");
129   good = typ == test2.type();
130   TEST("type_by_name", good, true);
131   good = real_elliptic_cone.on(vgl_homg_point_3d<double>(1.0, 2.0, sqrt(5)), vgl_tolerance<double>::position);
132   TEST("on quadric surface ", good, true);
133   // stream ops
134   std::stringstream ss;
135   ss << 3.0 << ' ' << 3.0 << ' ' << -3.0 << ' ' << 0.0 << ' ' << 0.0 << ' ' << 0.0 << ' ' << 0.0 << ' ' << 0.0 << ' '
136      << 0.0 << ' ' << 0.0;
137   vgl_quadric_3d<double> qstr;
138   ss >> qstr;
139   std::cout << qstr << std::endl;
140   good = qstr.type() == test2.type_by_name("real_elliptic_cone");
141   TEST("stream ops", good, true);
142   good = good && qstr == real_elliptic_cone;
143   TEST("equal", good, true);
144   // test translation
145   // note transform quadric is T^t Q T to avoid inverses but should be T^-t Q T^-1 to translate the quadric
146   // to a new center, so use negative translations
147   std::vector<std::vector<double>> Qe = real_ellipsoid.coef_matrix(), Qet, QeE, QeET, QeETT;
148   std::vector<std::vector<double>> Te(4, std::vector<double>(4, 0.0));
149   Te[0][0] = 1.0;
150   Te[1][1] = 1.0;
151   Te[2][2] = 1.0;
152   Te[0][3] = -tx;
153   Te[1][3] = -ty;
154   Te[2][3] = -tz;
155   Te[3][3] = 1.0;
156   // a simple sphere case
157   Qet = transform_quadric(Te, Qe);
158   vgl_quadric_3d<double> tran_quad(Qet);
159   vgl_point_3d<double> cent, true_cent(tx, ty, tz);
160   good = tran_quad.center(cent) && cent == true_cent;
161   // full eccentric ellipsoid
162   vgl_quadric_3d<double> eccentric_ellipsoid(0.5, 0.25, 0.125, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, -1.0);
163   QeE = eccentric_ellipsoid.coef_matrix();
164   QeET = transform_quadric(T, QeE);
165   vgl_quadric_3d<double> tran_ecc_quad(QeET);
166   vgl_point_3d<double> rot_cent;
167   good = good && tran_ecc_quad.center(rot_cent);
168   // to prove the center is correct, translate to move the center to the origin
169   // and show that the translation dependent terms of the quadric vanish.
170   Te[0][3] = rot_cent.x();
171   Te[1][3] = rot_cent.y();
172   Te[2][3] = rot_cent.z();
173   // note again the tranform quadric function requires the inverse of the desired translation,
174   // which is just the center itself.
175   QeETT = transform_quadric(Te, QeET); // should have g = h = i == 0
176   double sum_ghi = fabs(QeETT[0][3]) + fabs(QeETT[1][3]) + fabs(QeETT[2][3]);
177   good = good && sum_ghi < 1.0e-8;
178   TEST("center", good, true);
179   Q = elliptic_paraboloid.coef_matrix();
180   std::vector<std::vector<double>> Tq(4, std::vector<double>(4, 0.0));
181   Tq[0][0] = 0.5;
182   Tq[1][1] = 1.0;
183   Tq[2][2] = 0.5;
184   Tq[3][3] = 1.0;
185   Tq[0][2] = -0.866;
186   Tq[2][0] = 0.866;
187   Tq[0][3] = 1.0;
188   Tq[2][3] = 2.0;
189   Tq[2][3] = 3.0;
190   vgl_quadric_3d<double> tr_elliptic_para(Q, Tq);
191   std::vector<std::vector<double>> Hg;
192   std::vector<std::vector<double>> Qg = tr_elliptic_para.canonical_quadric(Hg);
193   vgl_quadric_3d<double> pqst_q(Qg);
194   TEST("canonical frame ", pqst_q.type() == vgl_quadric_3d<double>::elliptic_paraboloid, true);
195 }
196 
197 TESTMAIN(test_quadric);
198