Debian Codegraph @

plugins/core/Standard/qRANSAC_SD/extern/RANSAC_SD/Cone.cpp

raw
1 #include "Cone.h"
2 #include "Plane.h"
3 #include "LevMarFitting.h"
4 #include <MiscLib/Performance.h>
5 #include <GfxTL/IndexedIterator.h>
6 #include "LevMarLSWeight.h"
7 #include <GfxTL/Mean.h>
8 #ifdef DOPARALLEL
9 #include <omp.h>
10 #endif
11
12 extern int dmat_solve ( int n, int rhs_num, double a[] );
13
14 Cone::Cone()
15 : m_angularRotatedRadians(0),
16 m_angle(0),
17 m_n2d()
18 {}
19
20 Cone::Cone(const Vec3f &center, const Vec3f &axisDir, float angle)
21 : m_angularRotatedRadians(0)
22 {
23 m_center = center;
24 m_axisDir = axisDir;
25 m_angle = angle;
26 m_normal = Vec3f(std::cos(-m_angle), std::sin(-m_angle), 0);
27 m_normalY = m_normal[1] * m_axisDir;
28 m_n2d[0] = std::cos(m_angle);
29 m_n2d[1] = -std::sin(m_angle);
30 m_hcs.FromNormal(m_axisDir);
31 m_angularRotatedRadians = 0;
32 }
33
34 Cone::Cone(const Vec3f &p1, const Vec3f &p2, const Vec3f &p3,
35 const Vec3f &n1, const Vec3f &n2, const Vec3f &n3)
36 : m_angularRotatedRadians(0)
37 {
38 if(!Init(p1, p2, p3, n1, n2, n3))
39 throw ParallelPlanesError();
40 }
41
42 bool Cone::Init(const MiscLib::Vector< Vec3f > &samples)
43 {
44 if(samples.size() < 6)
45 return false;
46 size_t c = samples.size() >> 1;
47 return Init(samples[0], samples[1], samples[2],
48 samples[c], samples[c + 1], samples[c + 2]);
49 }
50
51 class LevMarPlaneDistance
52 {
53 public:
54 enum { NumParams = 3 };
55 typedef float ScalarType;
56
57 template< class IteratorT >
58 ScalarType Chi(const ScalarType *params, IteratorT begin, IteratorT end,
59 ScalarType *values, ScalarType *temp) const
60 {
61 ScalarType chi = 0;
62 intptr_t size = end - begin;
63 #ifdef DOPARALLEL
64 #pragma omp parallel for schedule(static) reduction(+:chi)
65 #endif
66 for(intptr_t i = 0; i < size; ++i)
67 {
68 values[i] = 0;
69 for(unsigned int j = 0; j < 3; ++j)
70 values[i] += begin[i][j] * params[j];
71 values[i] -= begin[i][3];
72 chi += values[i] * values[i];
73 }
74 return chi;
75 }
76
77 template< class IteratorT >
78 void Derivatives(const ScalarType *params, IteratorT begin, IteratorT end,
79 ScalarType *values, ScalarType *temp, ScalarType *matrix) const
80 {
81 intptr_t size = end - begin;
82 #ifdef DOPARALLEL
83 #pragma omp parallel for schedule(static)
84 #endif
85 for(intptr_t i = 0; i < size; ++i)
86 {
87 for(unsigned int j = 0; j < 3; ++j)
88 matrix[i * NumParams + j] = begin[i][j];
89 }
90 }
91
92 void Normalize(ScalarType *) const {}
93 };
94
95 bool Cone::InitAverage(const MiscLib::Vector< Vec3f > &samples)
96 {
97 // setup all the planes
98 size_t c = samples.size() / 2;
99 MiscLib::Vector< GfxTL::Vector4Df > planes(c);
100 #ifdef DOPARALLEL
101 #pragma omp parallel for schedule(static)
102 #endif
103 for (int i = 0; i < static_cast<int>(c); ++i)
104 {
105 for(unsigned int j = 0; j < 3; ++j)
106 planes[i][j] = samples[i][j];
107 planes[i][3] = samples[i].dot(samples[i + c]);
108 }
109 // compute center by intersecting the three planes given by (p1, n1)
110 // (p2, n2) and (p3, n3)
111 // set up linear system
112 double a[4 * 3];
113 double d1 = samples[0].dot(samples[c + 0]);
114 double d2 = samples[1].dot(samples[c + 1]);
115 double d3 = samples[2].dot(samples[c + 2]);
116 // column major
117 a[0 + 0 * 3] = samples[c + 0][0];
118 a[1 + 0 * 3] = samples[c + 1][0];
119 a[2 + 0 * 3] = samples[c + 2][0];
120 a[0 + 1 * 3] = samples[c + 0][1];
121 a[1 + 1 * 3] = samples[c + 1][1];
122 a[2 + 1 * 3] = samples[c + 2][1];
123 a[0 + 2 * 3] = samples[c + 0][2];
124 a[1 + 2 * 3] = samples[c + 1][2];
125 a[2 + 2 * 3] = samples[c + 2][2];
126 a[0 + 3 * 3] = d1;
127 a[1 + 3 * 3] = d2;
128 a[2 + 3 * 3] = d3;
129 if(dmat_solve(3, 1, a))
130 return false;
131 m_center[0] = static_cast<float>(a[0 + 3 * 3]);
132 m_center[1] = static_cast<float>(a[1 + 3 * 3]);
133 m_center[2] = static_cast<float>(a[2 + 3 * 3]);
134
135 LevMarPlaneDistance planeDistance;
136 LevMar(planes.begin(), planes.end(), planeDistance,
137 (float *)m_center);
138
139 MiscLib::Vector< GfxTL::Vector3Df > spoints(c);
140 #ifdef DOPARALLEL
141 #pragma omp parallel for schedule(static)
142 #endif
143 for (int i = 0; i < static_cast<int>(c); ++i)
144 {
145 spoints[i] = GfxTL::Vector3Df(samples[i] - m_center);
146 spoints[i].Normalize();
147 }
148 GfxTL::Vector3Df axisDir;
149 GfxTL::MeanOfNormals(spoints.begin(), spoints.end(), &axisDir);
150 m_axisDir = GfxTL::Vector3Df(axisDir);
151
152 // make sure axis points in good direction
153 // the axis is defined to point into the interior of the cone
154 float heightSum = 0;
155 #ifdef DOPARALLEL
156 #pragma omp parallel for schedule(static) reduction(+:heightSum)
157 #endif
158 for(int i = 0; i < static_cast<int>(c); ++i)
159 heightSum += Height(samples[i]);
160 if(heightSum < 0)
161 m_axisDir *= -1;
162
163 float angleReduction = 0;
164 #ifdef DOPARALLEL
165 #pragma omp parallel for schedule(static) reduction(+:angleReduction)
166 #endif
167 for(int i = 0; i < static_cast<int>(c); ++i)
168 {
169 float angle = m_axisDir.dot(samples[i + c]);
170 if(angle < -1) // clamp angle to [-1, 1]
171 angle = -1;
172 else if(angle > 1)
173 angle = 1;
174 if(angle < 0)
175 // m_angle = omega + 90
176 angle = std::acos(angle) - float(M_PI) / 2;
177 else
178 // m_angle = 90 - omega
179 angle = float(M_PI) / 2 - std::acos(angle);
180 angleReduction += angle;
181 }
182 angleReduction /= c;
183 m_angle = angleReduction;
184 if(m_angle < 1.0e-6 || m_angle > float(M_PI) / 2 - 1.0e-6)
185 return false;
186 //if(m_angle > 1.3962634015954636615389526147909) // 80 degrees
187 if(m_angle > 1.4835298641951801403851371532153f) // 85 degrees
188 return false;
189 m_normal = Vec3f(std::cos(-m_angle), std::sin(-m_angle), 0);
190 m_normalY = m_normal[1] * m_axisDir;
191 m_n2d[0] = std::cos(m_angle);
192 m_n2d[1] = -std::sin(m_angle);
193 m_hcs.FromNormal(m_axisDir);
194 m_angularRotatedRadians = 0;
195 return true;
196 }
197
198 bool Cone::Init(const Vec3f &center, const Vec3f &axisDir, float angle)
199 {
200 if(angle > 1.4835298641951801403851371532153) // do not allow flat cones
201 return false;
202 m_center = center;
203 m_axisDir = axisDir;
204 m_angle = angle;
205 m_normal = Vec3f(std::cos(-m_angle), std::sin(-m_angle), 0);
206 m_normalY = m_normal[1] * m_axisDir;
207 m_n2d[0] = std::cos(m_angle);
208 m_n2d[1] = -std::sin(m_angle);
209 m_hcs.FromNormal(m_axisDir);
210 m_angularRotatedRadians = 0;
211 return true;
212 }
213
214 bool Cone::Init(const Vec3f &p1, const Vec3f &p2, const Vec3f &p3,
215 const Vec3f &n1, const Vec3f &n2, const Vec3f &n3)
216 {
217 //float ncheck = std::max(n2.dot(n3), std::max(n1.dot(n2), n1.dot(n3)));
218 //if(ncheck > 0.999)
219 // return false;
220 // compute center by intersecting the three planes given by (p1, n1)
221 // (p2, n2) and (p3, n3)
222 // set up linear system
223 double a[4 * 3];
224 double d1 = p1.dot(n1);
225 double d2 = p2.dot(n2);
226 double d3 = p3.dot(n3);
227 // column major
228 a[0 + 0 * 3] = n1[0];
229 a[1 + 0 * 3] = n2[0];
230 a[2 + 0 * 3] = n3[0];
231 a[0 + 1 * 3] = n1[1];
232 a[1 + 1 * 3] = n2[1];
233 a[2 + 1 * 3] = n3[1];
234 a[0 + 2 * 3] = n1[2];
235 a[1 + 2 * 3] = n2[2];
236 a[2 + 2 * 3] = n3[2];
237 a[0 + 3 * 3] = d1;
238 a[1 + 3 * 3] = d2;
239 a[2 + 3 * 3] = d3;
240 if(dmat_solve(3, 1, a))
241 return false;
242 m_center[0] = static_cast<float>(a[0 + 3 * 3]);
243 m_center[1] = static_cast<float>(a[1 + 3 * 3]);
244 m_center[2] = static_cast<float>(a[2 + 3 * 3]);
245
246 // compute axisDir
247 Vec3f s1 = p1 - m_center;
248 Vec3f s2 = p2 - m_center;
249 Vec3f s3 = p3 - m_center;
250 s1.normalize();
251 s2.normalize();
252 s3.normalize();
253 Plane pl(s1 + m_center, s2 + m_center, s3 + m_center);
254 m_axisDir = pl.getNormal();
255 // make sure axis points in direction of s1
256 // this defines the side of the cone!!!
257 if(m_axisDir.dot(s1) < 0)
258 m_axisDir *= -1;
259 m_angle = 0;
260 float angle = m_axisDir.dot(n1);
261 if(angle < -1) // clamp angle to [-1, 1]
262 angle = -1;
263 else if(angle > 1)
264 angle = 1;
265 if(angle < 0)
266 // m_angle = omega + 90
267 angle = std::acos(angle) - float(M_PI) / 2;
268 else
269 // m_angle = 90 - omega
270 angle = float(M_PI) / 2 - std::acos(angle);
271 m_angle += angle;
272 angle = m_axisDir.dot(n2);
273 if(angle < -1) // clamp angle to [-1, 1]
274 angle = -1;
275 else if(angle > 1)
276 angle = 1;
277 if(angle < 0)
278 // m_angle = omega + 90
279 angle = std::acos(angle) - float(M_PI) / 2;
280 else
281 // m_angle = 90 - omega
282 angle = float(M_PI) / 2 - std::acos(angle);
283 m_angle += angle;
284 angle = m_axisDir.dot(n3);
285 if(angle < -1) // clamp angle to [-1, 1]
286 angle = -1;
287 else if(angle > 1)
288 angle = 1;
289 if(angle < 0)
290 // m_angle = omega + 90
291 angle = std::acos(angle) - float(M_PI) / 2;
292 else
293 // m_angle = 90 - omega
294 angle = float(M_PI) / 2 - std::acos(angle);
295 m_angle += angle;
296 m_angle /= 3;
297 if(m_angle < 1.0e-6 || m_angle > float(M_PI) / 2 - 1.0e-6)
298 return false;
299 //if(m_angle > 1.3962634015954636615389526147909) // 80 degrees
300 if(m_angle > 1.4835298641951801403851371532153f) // 85 degrees
301 return false;
302 m_normal = Vec3f(std::cos(-m_angle), std::sin(-m_angle), 0);
303 m_normalY = m_normal[1] * m_axisDir;
304 m_n2d[0] = std::cos(m_angle);
305 m_n2d[1] = -std::sin(m_angle);
306 m_hcs.FromNormal(m_axisDir);
307 m_angularRotatedRadians = 0;
308 return true;
309 }
310
311 bool Cone::Init(bool binary, std::istream *i)
312 {
313 float rotate = 0;
314 if(binary)
315 {
316 i->read((char *)&m_center, sizeof(m_center));
317 i->read((char *)&m_axisDir, sizeof(m_axisDir));
318 i->read((char *)&m_angle, sizeof(m_angle));
319 i->read((char *)&rotate,
320 sizeof(rotate));
321 }
322 else
323 {
324 for(size_t j = 0; j < 3; ++j)
325 (*i) >> m_center[j];
326 for(size_t j = 0; j < 3; ++j)
327 (*i) >> m_axisDir[j];
328 (*i) >> m_angle;
329 (*i) >> rotate;
330 }
331 m_normal = Vec3f(std::cos(-m_angle), std::sin(-m_angle), 0);
332 m_normalY = m_normal[1] * m_axisDir;
333 m_n2d[0] = std::cos(m_angle);
334 m_n2d[1] = -std::sin(m_angle);
335 m_hcs.FromNormal(m_axisDir);
336 m_angularRotatedRadians = 0;
337 RotateAngularDirection(rotate);
338 return true;
339 }
340
341 void Cone::Init(float *array)
342 {
343 float rotate = 0;
344 for(int i=0;i<3;i++){
345 m_center[i] = array[i];
346 m_axisDir[i] = array[3+i];
347 }
348 m_angle = array[6];
349 rotate = array[7];
350
351 m_normal = Vec3f(std::cos(-m_angle), std::sin(-m_angle), 0);
352 m_normalY = m_normal[1] * m_axisDir;
353 m_n2d[0] = std::cos(m_angle);
354 m_n2d[1] = -std::sin(m_angle);
355 m_hcs.FromNormal(m_axisDir);
356 m_angularRotatedRadians = 0;
357 RotateAngularDirection(rotate);
358 }
359
360
361 void Cone::Init(FILE *i)
362 {
363 float rotate = 0;
364 size_t readrtn; //unused return warning suppresion
365 readrtn = fread(&m_center, sizeof(m_center), 1, i);
366 readrtn = fread(&m_axisDir, sizeof(m_axisDir), 1, i);
367 readrtn = fread(&m_angle, sizeof(m_angle), 1, i);
368 readrtn = fread(&rotate, sizeof(rotate), 1, i);
369 m_normal = Vec3f(std::cos(-m_angle), std::sin(-m_angle), 0);
370 m_normalY = m_normal[1] * m_axisDir;
371 m_n2d[0] = std::cos(m_angle);
372 m_n2d[1] = -std::sin(m_angle);
373 m_hcs.FromNormal(m_axisDir);
374 m_angularRotatedRadians = 0;
375 RotateAngularDirection(rotate);
376 }
377
378 void Cone::Project(const Vec3f &p, Vec3f *pp) const
379 {
380 // this is for one-sided cone !!!
381 Vec3f s = p - m_center;
382 float g = s.dot(m_axisDir); // distance to plane orhogonal to
383 // axisdir through center
384 // distance to axis
385 float sqrS = s.sqrLength();
386 float f = sqrS - (g * g);
387 if(f <= 0)
388 f = 0;
389 else
390 f = std::sqrt(f);
391 float da = m_n2d[0] * f;
392 float db = m_n2d[1] * g;
393 if(g < 0 && da - db < 0) // is inside other side of cone -> disallow
394 {
395 *pp = m_center;
396 return;
397 }
398 float dist = -(da + db);
399 // need normal
400 Vec3f plx = s - g * m_axisDir;
401 plx.normalize();
402 Vec3f n = m_normal[0] * plx + m_normalY;
403 *pp = p + dist * n;
404 }
405
406 void Cone::Parameters(const Vec3f &p, std::pair< float, float > *param) const
407 {
408 // parametrize
409 Vec3f s = p - m_center;
410 float height = m_axisDir.dot(s);
411 float planex = s.dot(m_hcs[0].Data());
412 float planey = s.dot(m_hcs[1].Data());
413 float l = planex * planex + planey * planey;
414 if(l > 0)
415 {
416 planex /= l;
417 planey /= l;
418 }
419 float angle = std::atan2(planey, planex);
420 if(angle < 0)
421 angle += float(2 * M_PI);
422 /*Vec3f axisDiff = s - height * m_axisDir;
423 axisDiff.normalize();
424 float angle = m_angular.dot(axisDiff);
425 if(angle < -1) // clamp angle to [-1, 1]
426 angle = -1;
427 else if(angle > 1)
428 angle = 1;
429 if(m_angular.cross(axisDiff).dot(m_axisDir) < 0)
430 angle = std::acos(-angle) + M_PI;
431 else
432 angle = std::acos(angle);
433 // angle ok*/
434 // get length from height
435 //float length = height / std::cos(m_angle);
436 //param->first = length;
437 // this should be more precise than a division with std::cos:
438 // this is for two sided cone!
439 // distance to axis
440 float sqrS = s.sqrLength();
441 float f = sqrS - (height * height);
442 if(f <= 0)
443 f = 0;
444 else
445 f = std::sqrt(f);
446 float sdist = fabs(m_n2d[0] * f + ((height < 0)? -1 : 1) * m_n2d[1] * height);
447 float length = std::sqrt(sqrS + sdist * sdist);
448 param->first = /*(height < 0)? -length :*/ length;
449 param->second = angle;
450 /*// get normal for p
451 Vec3f pln = s.cross(m_axisDir);
452 Vec3f plx = m_axisDir.cross(pln);
453 Vec3f n;
454 if(plx.normalize() < 1.0e-6)
455 {
456 *param = std::make_pair(0.0f, angle);
457 return height;
458 }
459 if(height < 0)
460 n = m_normal[0] * plx - m_normalY;
461 else
462 n = m_normal[0] * plx + m_normalY;
463 Vec3f l = n.cross(pln);
464 l.normalize();
465 // make sure l points in direction of axis
466 if(m_axisDir.dot(l) < 0)
467 l *= -1;
468 // project p on line m_center + lambda * l
469 // get lambda
470 float lambda = s.dot(l);
471 // make sure l points in direction of axis
472 if(m_axisDir.dot(l) < 0)
473 {
474 if(lambda > 0)
475 {
476 *param = std::make_pair(s.length(), angle);
477 return height;
478 }
479 }
480 else if(lambda < 0)
481 {
482 *param = std::make_pair(s.length(), angle);
483 return height;
484 }
485 *param = std::make_pair(*fabs(lambda), angle);*/
486 }
487
488 void Cone::RotateAngularDirection(float radians)
489 {
490 GfxTL::Quaternion< float > q;
491 q.RotationRad(radians, m_axisDir[0], m_axisDir[1], m_axisDir[2]);
492 Vec3f vvec;
493 q.Rotate(AngularDirection(), &vvec);
494 m_hcs[0] = GfxTL::Vector3Df(vvec);
495 m_hcs[1] = GfxTL::Vector3Df(m_axisDir.cross(Vec3f(m_hcs[0].Data())));
496 m_angularRotatedRadians += radians;
497 }
498
499 //void Cone::AngularDirection(const Vec3f &angular)
500 //{
501 // m_hcs[0] = GfxTL::Vector3Df(angular);
502 // m_hcs[1] = GfxTL::Vector3Df(m_axisDir.cross(Vec3f(m_hcs[0].Data())));
503 //}
504
505 float ConeDistance(const float *param, const float *x)
506 {
507 Vec3f s;
508 for(unsigned int i = 0; i < 3; ++i)
509 s[i] = x[i] - param[i];
510 float g = fabs(s[0] * param[3] + s[1] * param[4] + s[2] * param[5]);
511 float f = s.sqrLength() - (g * g);
512 if(f <= 0)
513 f = 0;
514 else
515 f = std::sqrt(f);
516 return std::cos(param[6]) * f - std::sin(param[6]) * g;
517 //- param[7];
518 }
519
520 void ConeDistanceDerivatives(const float *param, const float *x,
521 float *gradient)
522 {
523 Vec3f s;
524 for(unsigned int i = 0; i < 3; ++i)
525 s[i] = x[i] - param[i];
526 float g = fabs(s[0] * param[3] + s[1] * param[4] + s[2] * param[5]);
527 float f = s.sqrLength() - (g * g);
528 if(f <= 0)
529 f = 0;
530 else
531 f = std::sqrt(f);
532 float ggradient[6];
533 for(unsigned int i = 0; i < 3; ++i)
534 ggradient[i] = -param[i + 3];
535 for(unsigned int i = 0; i < 3; ++i)
536 ggradient[i + 3] = s[i] - param[i + 3] * g;
537 float fgradient[6];
538 if(f < 1.0e-6)
539 {
540 fgradient[0] = std::sqrt(1 - param[3] * param[3]);
541 fgradient[1] = std::sqrt(1 - param[4] * param[4]);
542 fgradient[2] = std::sqrt(1 - param[5] * param[5]);
543 }
544 else
545 {
546 fgradient[0] = (param[3] * g - s[0]) / f;
547 fgradient[1] = (param[4] * g - s[1]) / f;
548 fgradient[2] = (param[5] * g - s[2]) / f;
549 }
550 fgradient[3] = g * fgradient[0];
551 fgradient[4] = g * fgradient[1];
552 fgradient[5] = g * fgradient[2];
553 float sinPhi = -std::sin(param[6]);
554 float cosPhi = std::cos(param[6]);
555 for(unsigned int i = 0; i < 6; ++i)
556 gradient[i] = cosPhi * fgradient[i] + sinPhi * ggradient[i];
557 gradient[6] = f * sinPhi - g * cosPhi;
558 //gradient[7] = -1;
559 }
560
561 void NormalizeConeParams(float *param)
562 {
563 // normalize direction
564 float l = std::sqrt(param[3] * param[3] + param[4] * param[4] +
565 param[5] * param[5]);
566 for(unsigned int i = 3; i < 6; ++i)
567 param[i] /= l;
568 // normalize angle
569 param[6] -= std::floor(param[6] / (2 * float(M_PI))) * (2 * float(M_PI)); // param[6] %= 2*M_PI
570 if(param[6] > M_PI)
571 {
572 param[6] -= std::floor(param[6] / float(M_PI)) * float(M_PI); // param[6] %= M_PI
573 for(unsigned int i = 3; i < 6; ++i)
574 param[i] *= -1;
575 }
576 if(param[6] > float(M_PI) / 2)
577 param[6] = float(M_PI) - param[6];
578 }
579
580 bool Cone::LeastSquaresFit(const PointCloud &pc,
581 MiscLib::Vector< size_t >::const_iterator begin,
582 MiscLib::Vector< size_t >::const_iterator end)
583 {
584 bool retVal = LeastSquaresFit(GfxTL::IndexIterate(begin, pc.begin()),
585 GfxTL::IndexIterate(end, pc.begin()));
586 return retVal;
587 }
588
589 bool Cone::Interpolate(const MiscLib::Vector< Cone > &cones,
590 const MiscLib::Vector< float > &weights, Cone *ic)
591 {
592 Vec3f center(0, 0, 0);
593 Vec3f axisDir(0, 0, 0);
594 float omega = 0;
595 for(size_t i = 0; i < cones.size(); ++i)
596 {
597 center += weights[i] * cones[i].Center();
598 axisDir += weights[i] * cones[i].AxisDirection();
599 omega += weights[i] * cones[i].Angle();
600 }
601 axisDir.normalize();
602 return ic->Init(center, axisDir, omega);
603 }
604
605 void Cone::Serialize(bool binary, std::ostream *o) const
606 {
607 if(binary)
608 {
609 o->write((const char *)&m_center, sizeof(m_center));
610 o->write((const char *)&m_axisDir, sizeof(m_axisDir));
611 o->write((const char *)&m_angle, sizeof(m_angle));
612 o->write((const char *)&m_angularRotatedRadians,
613 sizeof(m_angularRotatedRadians));
614 }
615 else
616 {
617 (*o) << m_center[0] << " " << m_center[1] << " " << m_center[2] << " "
618 << m_axisDir[0] << " " << m_axisDir[1] << " " << m_axisDir[2] << " "
619 << m_angle << " " << m_angularRotatedRadians << " ";
620 }
621 }
622
623 size_t Cone::SerializedSize()
624 {
625 return sizeof(Vec3f)
626 + sizeof(Vec3f)
627 + sizeof(float)
628 + sizeof(float);
629 }
630
631 void Cone::Serialize(FILE *o) const
632 {
633 fwrite(&m_center, sizeof(m_center), 1, o);
634 fwrite(&m_axisDir, sizeof(m_axisDir), 1, o);
635 fwrite(&m_angle, sizeof(m_angle), 1, o);
636 fwrite(&m_angularRotatedRadians, sizeof(m_angularRotatedRadians), 1, o);
637 }
638
639 size_t Cone::SerializedFloatSize()
640 {
641 return 8;
642 }
643
644 void Cone::Serialize(float* array) const
645 {
646 for(int i=0;i<3;i++){
647 array[i] = m_center[i];
648 array[i+3] = m_axisDir[i];
649 }
650 array[6] = m_angle;
651 array[7] = m_angularRotatedRadians;
652 }
653
654
655 void Cone::Transform(float scale, const Vec3f &translate)
656 {
657 m_center *= scale;
658 m_center += translate;
659 }
660
661 void Cone::Transform(const GfxTL::MatrixXX< 3, 3, float > &rot,
662 const GfxTL::Vector3Df &trans)
663 {
664 m_center = Vec3f((rot * GfxTL::Vector3Df(m_center) + trans).Data());
665 m_axisDir = Vec3f((rot * GfxTL::Vector3Df(m_axisDir)).Data());
666 m_hcs[0] = rot * m_hcs[0];
667 m_hcs[1] = rot * m_hcs[1];
668 m_normalY = m_normal[1] * m_axisDir;
669 }
670
671 Cone::ConeInfo Cone::GetInfo(const PointCloud& pc, size_t startingIndx, size_t endingIndx) const
672 {
673 if (startingIndx > endingIndx)
674 {
675 std::swap(startingIndx, endingIndx);
676 }
677 size_t sampleSize = endingIndx - startingIndx;
678 if (endingIndx >= pc.size() || sampleSize >= pc.size())
679 {
680 return ConeInfo();
681 }
682
683 ConeInfo rtrn;
684
685 rtrn.center = Center();
686 rtrn.angle = Angle();
687 rtrn.axisDirection = AxisDirection();
688
689 rtrn.minPoint = rtrn.maxPoint = pc[startingIndx];
690 rtrn.minHeight = rtrn.maxHeight = Height(pc[startingIndx]);
691 for (size_t i = startingIndx; i <= endingIndx; i++)
692 {
693 float h = Height(pc[i].pos);
694 if (h < rtrn.minHeight)
695 {
696 rtrn.minHeight = h;
697 rtrn.minPoint = pc[i].pos;
698 }
699 else if (h > rtrn.maxHeight)
700 {
701 rtrn.maxHeight = h;
702 rtrn.maxPoint = pc[i].pos;
703 }
704 }
705 rtrn.height = rtrn.maxHeight - rtrn.minHeight;
706 rtrn.minRadius = std::tan(rtrn.angle) * rtrn.minHeight;
707 rtrn.maxRadius = std::tan(rtrn.angle) * rtrn.maxHeight;
708
709 return rtrn;
710 }
711
712 Cone::ConeInfo Cone::GetInfo(const MiscLib::Vector< Vec3f >& samples) const
713 {
714 ConeInfo rtrn;
715
716 rtrn.center = Center();
717 rtrn.angle = Angle();
718 rtrn.axisDirection = AxisDirection();
719
720
721 rtrn.minPoint = rtrn.maxPoint = samples[0];
722 rtrn.minHeight = rtrn.maxHeight = Height(samples[0]);
723 for (size_t i = 0; i < samples.size(); i++)
724 {
725 float h = Height(samples[i]);
726 if (h < rtrn.minHeight)
727 {
728 rtrn.minHeight = h;
729 rtrn.minPoint = samples[i];
730 }
731 else if (h > rtrn.maxHeight)
732 {
733 rtrn.maxHeight = h;
734 rtrn.maxPoint = samples[i];
735 }
736 }
737 rtrn.height = rtrn.maxHeight - rtrn.minHeight;
738 rtrn.minRadius = std::tan(rtrn.angle) * rtrn.minHeight;
739 rtrn.maxRadius = std::tan(rtrn.angle) * rtrn.maxHeight;
740
741 return rtrn;
742 }