Line data Source code
1 : #include "EllipseGenerator.h"
2 : #include "Timer.h"
3 : #include "Logger.h"
4 :
5 : #include <cmath>
6 : #include <stdexcept>
7 :
8 : namespace elsa
9 : {
10 : template <typename data_t>
11 : void EllipseGenerator<data_t>::drawFilledEllipse2d(DataContainer<data_t>& dc, data_t amplitude,
12 : Vec2 const& center, Vec2 sizes, data_t angle)
13 167 : {
14 : // sanity check
15 167 : if (dc.getDataDescriptor().getNumberOfDimensions() != 2)
16 0 : throw InvalidArgumentError(
17 0 : "EllipseGenerator::drawFilledEllipse2d: can only work on 2d DataContainers");
18 :
19 : // don't draw anything if size is 0
20 167 : if (sizes[0] == 0 && sizes[1] == 0)
21 80 : return;
22 :
23 : // convert to radians
24 87 : auto angleRad = angle * pi<double> / 180.0f;
25 :
26 : // special case: circle or no rotation
27 87 : if (sizes[0] == sizes[1] || std::fmod(angle, 180.0) == 0) {
28 49 : drawShearedFilledEllipse2d(dc, amplitude, center, sizes, {1, 0});
29 49 : return;
30 49 : }
31 :
32 : // convert rotation by angle into shearing
33 38 : auto theta = std::atan2(static_cast<real_t>(-sizes[1]) * std::tan(angleRad), sizes[0]);
34 :
35 38 : Vec2 shear;
36 38 : shear[0] = static_cast<index_t>(
37 38 : std::floor((static_cast<real_t>(sizes[0]) * std::cos(theta) * std::cos(angleRad))
38 38 : - (static_cast<real_t>(sizes[1]) * std::sin(theta) * std::sin(angleRad))));
39 38 : shear[1] = static_cast<index_t>(
40 38 : std::floor((static_cast<real_t>(sizes[0]) * std::cos(theta) * std::sin(angleRad))
41 38 : + (static_cast<real_t>(sizes[1]) * std::sin(theta) * std::cos(angleRad))));
42 :
43 38 : Vec2 shearedSizes;
44 38 : shearedSizes[0] = std::abs(shear[0]);
45 38 : shearedSizes[1] = sizes[1] * sizes[0] / shearedSizes[0];
46 :
47 38 : drawShearedFilledEllipse2d(dc, amplitude, center, shearedSizes, shear);
48 38 : }
49 :
50 : template <typename data_t>
51 : void EllipseGenerator<data_t>::drawFilledEllipsoid3d(DataContainer<data_t>& dc,
52 : data_t amplitude, Vec3 center, Vec3 sizes,
53 : data_t phi, data_t theta, data_t psi)
54 71 : {
55 : // sanity check
56 71 : if (dc.getDataDescriptor().getNumberOfDimensions() != 3)
57 0 : throw InvalidArgumentError(
58 0 : "EllipseGenerator::drawFilledEllipsoid3d: can only work on 3d DataContainers");
59 :
60 : // enables small optimizations
61 71 : bool hasRotation = (std::abs(phi) + std::abs(theta) + std::abs(psi)) > 0;
62 :
63 : // convert to radians
64 71 : auto phiRad = phi * pi<double> / 180.0;
65 71 : auto thetaRad = theta * pi<double> / 180.0;
66 71 : auto psiRad = psi * pi<double> / 180.0;
67 :
68 71 : auto cosPhi = std::cos(phiRad);
69 71 : auto sinPhi = std::sin(phiRad);
70 71 : auto cosTheta = std::cos(thetaRad);
71 71 : auto sinTheta = std::sin(thetaRad);
72 71 : auto cosPsi = std::cos(psiRad);
73 71 : auto sinPsi = std::sin(psiRad);
74 :
75 : // setup ZXZ Euler rotation matrix
76 71 : Eigen::Matrix<data_t, 3, 3> rot;
77 71 : rot(0, 0) = static_cast<real_t>(cosPhi * cosPsi - cosTheta * sinPhi * sinPsi);
78 71 : rot(0, 1) = static_cast<real_t>(cosPsi * sinPhi + cosPhi * cosTheta * sinPsi);
79 71 : rot(0, 2) = static_cast<real_t>(sinTheta * sinPsi);
80 :
81 71 : rot(1, 0) = static_cast<real_t>(-cosPhi * sinPsi - cosTheta * cosPsi * sinPhi);
82 71 : rot(1, 1) = static_cast<real_t>(cosPhi * cosTheta * cosPsi - sinPhi * sinPsi);
83 71 : rot(1, 2) = static_cast<real_t>(cosPsi * sinTheta);
84 :
85 71 : rot(2, 0) = static_cast<real_t>(sinPhi * sinTheta);
86 71 : rot(2, 1) = static_cast<real_t>(-cosPhi * sinTheta);
87 71 : rot(2, 2) = static_cast<real_t>(cosTheta);
88 :
89 : // enables safe early abort
90 71 : index_t maxSize = sizes.maxCoeff();
91 :
92 : // precomputations
93 71 : index_t asq = sizes[0] * sizes[0];
94 71 : index_t bsq = sizes[1] * sizes[1];
95 71 : index_t csq = sizes[2] * sizes[2];
96 :
97 71 : IndexVector_t idx(3);
98 :
99 : // loop over everything... (very inefficient!)
100 1255 : for (index_t x = 0; x < dc.getDataDescriptor().getNumberOfCoefficientsPerDimension()[0];
101 1184 : ++x) {
102 1184 : if (x < center[0] - maxSize || x > center[0] + maxSize)
103 753 : continue;
104 431 : idx[0] = x;
105 :
106 18607 : for (index_t y = 0; y < dc.getDataDescriptor().getNumberOfCoefficientsPerDimension()[1];
107 18176 : ++y) {
108 18176 : if (y < center[1] - maxSize || y > center[1] + maxSize)
109 6073 : continue;
110 12103 : idx[1] = dc.getDataDescriptor().getNumberOfCoefficientsPerDimension()[1] - 1
111 12103 : - y; // flip y axis
112 :
113 12103 : for (index_t z = 0;
114 743015 : z < dc.getDataDescriptor().getNumberOfCoefficientsPerDimension()[2]; ++z) {
115 730912 : if (z < center[2] - maxSize || z > center[2] + maxSize)
116 154001 : continue;
117 576911 : idx[2] = z;
118 :
119 : // center it
120 576911 : index_t xc = x - center[0];
121 576911 : index_t yc = y - center[1];
122 576911 : index_t zc = z - center[2];
123 :
124 : // check ellipsoid equation
125 576911 : data_t aPart = (hasRotation) ? static_cast<data_t>(xc) * rot(0, 0)
126 161919 : + static_cast<data_t>(yc) * rot(0, 1)
127 161919 : + static_cast<data_t>(zc) * rot(0, 2)
128 576911 : : static_cast<data_t>(xc);
129 576911 : aPart *= aPart / static_cast<data_t>(asq);
130 :
131 576911 : data_t bPart = (hasRotation) ? static_cast<data_t>(xc) * rot(1, 0)
132 161919 : + static_cast<data_t>(yc) * rot(1, 1)
133 161919 : + static_cast<data_t>(zc) * rot(1, 2)
134 576911 : : static_cast<data_t>(yc);
135 576911 : bPart *= bPart / static_cast<data_t>(bsq);
136 :
137 576911 : data_t cPart = (hasRotation) ? static_cast<data_t>(xc) * rot(2, 0)
138 161919 : + static_cast<data_t>(yc) * rot(2, 1)
139 161919 : + static_cast<data_t>(zc) * rot(2, 2)
140 576911 : : static_cast<data_t>(zc);
141 576911 : cPart *= cPart / static_cast<data_t>(csq);
142 :
143 576911 : if (aPart + bPart + cPart <= 1.0)
144 143375 : dc(idx) += amplitude;
145 576911 : }
146 12103 : }
147 431 : }
148 71 : }
149 :
150 : template <typename data_t>
151 : void EllipseGenerator<data_t>::drawShearedFilledEllipse2d(DataContainer<data_t>& dc,
152 : data_t amplitude, Vec2 const& center,
153 : Vec2 sizes, Vec2 const& shear)
154 87 : {
155 87 : auto twoSizeXSquared = 2 * sizes[0] * sizes[0];
156 87 : auto twoSizeYSquared = 2 * sizes[1] * sizes[1];
157 :
158 : // setup first ellipse part where major axis of "advance" is the y axis
159 87 : auto x = sizes[0];
160 87 : index_t y = 0;
161 :
162 87 : auto xChange = sizes[1] * sizes[1] * (1 - 2 * sizes[0]);
163 87 : auto yChange = sizes[0] * sizes[0];
164 :
165 87 : index_t ellipseError = 0;
166 87 : auto xStop = twoSizeYSquared * sizes[0];
167 87 : index_t yStop = 0;
168 :
169 : // draw the first ellipse part
170 662 : while (xStop >= yStop) {
171 575 : drawShearedLinePairs2d(dc, amplitude, center, x, y, shear);
172 575 : y += 1;
173 575 : yStop += twoSizeXSquared;
174 575 : ellipseError += yChange;
175 575 : yChange += twoSizeXSquared;
176 :
177 : // check if x update is necessary
178 575 : if ((2 * ellipseError + xChange) > 0) {
179 219 : x -= 1;
180 219 : xStop -= twoSizeYSquared;
181 219 : ellipseError += xChange;
182 219 : xChange += twoSizeYSquared;
183 219 : }
184 575 : }
185 :
186 : // setup second ellipse part where major axis of "advance" is the x axis
187 87 : x = 0;
188 87 : y = sizes[1];
189 :
190 87 : xChange = sizes[1] * sizes[1];
191 87 : yChange = sizes[0] * sizes[0] * (1 - 2 * sizes[1]);
192 :
193 87 : ellipseError = 0;
194 87 : xStop = 0;
195 87 : yStop = twoSizeXSquared * sizes[1];
196 :
197 : // draw the second ellipse part
198 608 : while (xStop < yStop) {
199 521 : x += 1;
200 521 : xStop += twoSizeYSquared;
201 521 : ellipseError += xChange;
202 521 : xChange += twoSizeYSquared;
203 :
204 : // check if y update is necessary
205 521 : if ((2 * ellipseError + yChange) > 0) {
206 : // we only draw once the y axis is updated, to avoid line overlays (since we draw
207 : // lines along x axis), else we would have multiple lines stacking up the amplitude
208 : // (which is additive)
209 235 : drawShearedLinePairs2d(dc, amplitude, center, x - 1, y, shear);
210 :
211 235 : y -= 1;
212 235 : yStop -= twoSizeXSquared;
213 235 : ellipseError += yChange;
214 235 : yChange += twoSizeXSquared;
215 235 : }
216 521 : }
217 87 : }
218 :
219 : template <typename data_t>
220 : void EllipseGenerator<data_t>::drawShearedLinePairs2d(DataContainer<data_t>& dc,
221 : data_t amplitude, Vec2 center,
222 : index_t xOffset, index_t yOffset,
223 : Vec2 shear)
224 810 : {
225 810 : IndexVector_t coord(2);
226 :
227 : // draw the line along the x axis
228 39428 : for (index_t x = center[0] - xOffset; x <= center[0] + xOffset; ++x) {
229 38618 : auto shearTerm = (x - center[0]) * shear[1] / shear[0];
230 38618 : coord[0] = x;
231 38618 : coord[1] = center[1] + yOffset + shearTerm;
232 : // flip y axis
233 38618 : coord[1] = dc.getDataDescriptor().getNumberOfCoefficientsPerDimension()[1] - coord[1];
234 :
235 : // bounds check coord just to be sure (we're not performance critical here anyway)
236 38618 : if (coord[0] < 0
237 38618 : || coord[0] >= dc.getDataDescriptor().getNumberOfCoefficientsPerDimension()[0])
238 0 : throw InvalidArgumentError("EllipseGenerator::drawShearedLinePairs2d: drawing "
239 0 : "coordinate (x) out of bounds");
240 38618 : if (coord[1] < 0
241 38618 : || coord[1] >= dc.getDataDescriptor().getNumberOfCoefficientsPerDimension()[1])
242 0 : throw InvalidArgumentError("EllipseGenerator::drawShearedLinePairs2d: drawing "
243 0 : "coordinate (y) out of bounds");
244 :
245 38618 : dc(coord) += amplitude;
246 :
247 38618 : if (yOffset != 0) {
248 37225 : coord[1] = center[1] - yOffset + shearTerm;
249 : // flip y axis
250 37225 : coord[1] =
251 37225 : dc.getDataDescriptor().getNumberOfCoefficientsPerDimension()[1] - coord[1];
252 :
253 37225 : if (coord[1] < 0
254 37225 : || coord[1] >= dc.getDataDescriptor().getNumberOfCoefficientsPerDimension()[1])
255 0 : throw InvalidArgumentError("EllipseGenerator::drawShearedLinePairs2d: drawing "
256 0 : "coordinate (y) out of bounds");
257 :
258 37225 : dc(coord) += amplitude;
259 37225 : }
260 38618 : }
261 810 : }
262 :
263 : // ------------------------------------------
264 : // explicit template instantiation
265 : template class EllipseGenerator<float>;
266 : template class EllipseGenerator<double>;
267 :
268 : } // namespace elsa
|
