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