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