Debian Codegraph @

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

raw
1 #include "ConePrimitiveShape.h"
2 #include "PrimitiveShapeVisitor.h"
3 #include "ScoreComputer.h"
4 #include "Bitmap.h"
5 #include <GfxTL/NullClass.h>
6 #include <GfxTL/MathHelper.h>
7 #include <limits>
8 #include <algorithm>
9 #include <iostream>
10 #include <MiscLib/Performance.h>
11 #include "TorusPrimitiveShape.h"
12 #include "CylinderPrimitiveShape.h"
13 #include "SpherePrimitiveShape.h"
14 #include "PlanePrimitiveShape.h"
15
16 extern MiscLib::performance_t totalTime_coneConnected;
17 #undef max
18 #undef min
19
20 ConePrimitiveShape::ConePrimitiveShape(const Cone &cone, float maxRadius, float maxAngle, float maxLength)
21 : m_cone(cone)
22 , m_maxRadius(maxRadius)
23 , m_maxAngle(maxAngle)
24 , m_maxLength(maxLength)
25 {}
26
27 size_t ConePrimitiveShape::Identifier() const
28 {
29 return 3;
30 }
31
32 PrimitiveShape *ConePrimitiveShape::Clone() const
33 {
34 return new ConePrimitiveShape(*this);
35 }
36
37 float ConePrimitiveShape::Distance(const Vec3f &p) const
38 {
39 return m_cone.Distance(p);
40 }
41
42 float ConePrimitiveShape::SignedDistance(const Vec3f &p) const
43 {
44 return m_cone.SignedDistance(p);
45 }
46
47 float ConePrimitiveShape::NormalDeviation(const Vec3f &p, const Vec3f &n) const
48 {
49 Vec3f normal;
50 m_cone.Normal(p, &normal);
51 return n.dot(normal);
52 }
53
54 void ConePrimitiveShape::DistanceAndNormalDeviation(
55 const Vec3f &p, const Vec3f &n, std::pair< float, float > *dn) const
56 {
57 Vec3f normal;
58 dn->first = m_cone.DistanceAndNormal(p, &normal);
59 dn->second = n.dot(normal);
60 }
61
62 void ConePrimitiveShape::Project(const Vec3f &p, Vec3f *pp) const
63 {
64 m_cone.Project(p, pp);
65 }
66
67 void ConePrimitiveShape::Normal(const Vec3f &p, Vec3f *n) const
68 {
69 m_cone.Normal(p, n);
70 }
71
72 unsigned int ConePrimitiveShape::ConfidenceTests(unsigned int numTests,
73 float epsilon, float normalThresh, float rms, const PointCloud &pc,
74 const MiscLib::Vector< size_t > &indices) const
75 {
76 return BasePrimitiveShape::ConfidenceTests< Cone >(numTests, epsilon,
77 normalThresh, rms, pc, indices);
78 }
79
80 void ConePrimitiveShape::Description(std::string *s) const
81 {
82 *s = "Cone";
83 }
84
85 bool ConePrimitiveShape::Fit(const PointCloud &pc, float epsilon,
86 float normalThresh, MiscLib::Vector< size_t >::const_iterator begin,
87 MiscLib::Vector< size_t >::const_iterator end)
88 {
89 Cone fit = m_cone;
90 if(fit.LeastSquaresFit(pc, begin, end))
91 {
92 m_cone = fit;
93 return true;
94 }
95 return false;
96
97 }
98
99 PrimitiveShape *ConePrimitiveShape::LSFit(const PointCloud &pc, float epsilon,
100 float normalThresh, MiscLib::Vector< size_t >::const_iterator begin,
101 MiscLib::Vector< size_t >::const_iterator end,
102 std::pair< size_t, float > *score) const
103 {
104 Cone fit = m_cone;
105 if(fit.LeastSquaresFit(pc, begin, end))
106 {
107 score->first = -1;
108 return new ConePrimitiveShape(fit);
109 }
110 score->first = 0;
111 return NULL;
112 }
113
114 LevMarFunc< float > *ConePrimitiveShape::SignedDistanceFunc() const
115 {
116 return new ConeLevMarFunc(m_cone);
117 }
118
119 void ConePrimitiveShape::Serialize(std::ostream *o, bool binary) const
120 {
121 if(binary)
122 {
123 const char id = 3;
124 (*o) << id;
125 }
126 else
127 (*o) << "3" << " ";
128 m_cone.Serialize(binary, o);
129 if(!binary)
130 (*o) << std::endl;
131 }
132
133 size_t ConePrimitiveShape::SerializedSize() const
134 {
135 return m_cone.SerializedSize() + 1;
136 }
137
138 void ConePrimitiveShape::Parameters(const Vec3f &p,
139 std::pair< float, float > *param) const
140 {
141 m_cone.Parameters(p, param); // this gives us length and angle
142 // select parametrization with smaller stretch
143 if(m_cone.Angle() < float(M_PI / 4)) // angle of cone less than 45 degrees
144 {
145 // parameterize in the following way:
146 // u = length
147 // v = arc length
148 float r = m_cone.RadiusAtLength(param->first);
149 param->second = (param->second - float(M_PI)) * r; // convert to arc length and centralize
150 }
151 else
152 {
153 float l = param->first;
154 param->first = std::sin(param->second) * l;
155 param->second = std::cos(param->second) * l;
156 }
157 }
158
159 void ConePrimitiveShape::Parameters(GfxTL::IndexedIterator<
160 MiscLib::Vector< size_t >::iterator, PointCloud::const_iterator > begin,
161 GfxTL::IndexedIterator< MiscLib::Vector< size_t >::iterator,
162 PointCloud::const_iterator > end,
163 MiscLib::Vector< std::pair< float, float > > *bmpParams) const
164 {
165 ParametersImpl(begin, end, bmpParams);
166 }
167
168 void ConePrimitiveShape::Parameters(GfxTL::IndexedIterator< IndexIterator,
169 PointCloud::const_iterator > begin,
170 GfxTL::IndexedIterator< IndexIterator,
171 PointCloud::const_iterator > end,
172 MiscLib::Vector< std::pair< float, float > > *bmpParams) const
173 {
174 ParametersImpl(begin, end, bmpParams);
175 }
176
177 void ConePrimitiveShape::Transform(float scale, const Vec3f &translate)
178 {
179 m_cone.Transform(scale, translate);
180 }
181
182 void ConePrimitiveShape::Transform(const GfxTL::MatrixXX< 3, 3, float > &rot,
183 const GfxTL::Vector3Df &trans)
184 {
185 m_cone.Transform(rot, trans);
186 }
187
188 void ConePrimitiveShape::Visit(PrimitiveShapeVisitor *visitor) const
189 {
190 visitor->Visit(*this);
191 }
192
193 //float ConePrimitiveShape::Height() const
194 //{
195 // return m_extBbox.Max()[0] - m_extBbox.Min()[0];
196 //}
197 //
198 //float ConePrimitiveShape::MinHeight() const
199 //{
200 // return m_extBbox.Min()[0];
201 //}
202 //
203 //float ConePrimitiveShape::MaxHeight() const
204 //{
205 // return m_extBbox.Max()[0];
206 //}
207
208 void ConePrimitiveShape::SuggestSimplifications(const PointCloud &pc,
209 MiscLib::Vector< size_t >::const_iterator begin,
210 MiscLib::Vector< size_t >::const_iterator end, float distThresh,
211 MiscLib::Vector< MiscLib::RefCountPtr< PrimitiveShape > > *suggestions) const
212 {
213 // sample the bounding box in parameter space at 25 locations
214 // these points are used to estimate the other shapes
215 // if the shapes succeed the suggestion is returned
216 MiscLib::Vector< Vec3f > samples(2 * 25);
217 float uStep = (m_extBbox.Max()[0] - m_extBbox.Min()[0]) / 4;
218 float vStep = (m_extBbox.Max()[1] - m_extBbox.Min()[1]) / 4;
219 float u = m_extBbox.Min()[0];
220 for(unsigned int i = 0; i < 5; ++i, u += uStep)
221 {
222 float v = m_extBbox.Min()[1];
223 for(unsigned int j = 0; j < 5; ++j, v += vStep)
224 {
225 float bmpu, bmpv;
226 if(m_cone.Angle() >= M_PI / 4)
227 {
228 bmpu = std::sin(v) * u;
229 bmpv = std::cos(v) * u;
230 }
231 else
232 {
233 bmpu = u;
234 float r = m_cone.RadiusAtLength(u);
235 bmpv = (v - float(M_PI)) * r;
236 }
237 InSpace(bmpu, bmpv, &samples[i * 5 + j],
238 &samples[i * 5 + j + 25]);
239 }
240 }
241 size_t c = samples.size() / 2;
242
243 float d = 0;
244 float bestSum = 0;
245
246 for (size_t i = 0; i < c; ++i)
247 {
248 d = m_cone.Distance(samples[i]);
249 bestSum += d;
250 }
251
252 // now check all the shape types
253 Cylinder cylinder;
254 if(cylinder.InitAverage(samples))
255 {
256 cylinder.LeastSquaresFit(samples.begin(), samples.begin() + c);
257 bool failed = false;
258 float sum = 0;
259 for (size_t i = 0; i < c; ++i)
260 {
261 d = cylinder.Distance(samples[i]);
262 sum += d;
263 if (d > distThresh)
264 {
265 failed = true;
266 break;
267 }
268 }
269 if(!failed)
270 {
271 if (sum < bestSum)
272 {
273 bestSum = sum;
274 suggestions->push_back(new CylinderPrimitiveShape(cylinder));
275 suggestions->back()->Release();
276 }
277 }
278 }
279 Sphere sphere;
280 if(sphere.Init(samples))
281 {
282 sphere.LeastSquaresFit(samples.begin(), samples.begin() + c);
283 bool failed = false;
284 float sum = 0;
285 for(size_t i = 0; i < c; ++i)
286 {
287 d = sphere.Distance(samples[i]);
288 sum += d;
289 if (d > distThresh)
290 {
291 failed = true;
292 break;
293 }
294 }
295 if(!failed)
296 {
297 if (sum < bestSum)
298 {
299 bestSum = sum;
300 suggestions->push_back(new SpherePrimitiveShape(sphere));
301 suggestions->back()->Release();
302 }
303 }
304 }
305 Plane plane;
306 if(plane.LeastSquaresFit(samples.begin(), samples.begin() + c))
307 {
308 bool failed = false;
309 float sum = 0;
310 for(size_t i = 0; i < c; ++i)
311 {
312 d = plane.Distance(samples[i]);
313 sum += d;
314 if (d > distThresh)
315 {
316 failed = true;
317 break;
318 }
319 }
320 if(!failed)
321 {
322 if (sum < bestSum)
323 {
324 bestSum = sum;
325 suggestions->push_back(new PlanePrimitiveShape(plane));
326 suggestions->back()->Release();
327 }
328 }
329 }
330
331 /*// simpler shapes are suggested if the maximal curvature of the cone
332 // is small compared to the extend in relation to the distThresh
333
334 float meanRadius, length, meanLength, radialExtent;
335 // the cone is parameterized as length and angle
336 // in this case the cone is parametrized as length and arclength
337 meanRadius = (m_cone.RadiusAtLength(m_extBbox.Min()[0])
338 + m_cone.RadiusAtLength(m_extBbox.Max()[0])) / 2;
339 length = m_extBbox.Max()[0] - m_extBbox.Min()[0];
340 meanLength = (m_extBbox.Max()[0] + m_extBbox.Min()[0]) / 2;
341 // the radial extent
342 radialExtent = m_extBbox.Max()[1] - m_extBbox.Min()[1];
343 // We suggest a cylinder if the opening angle of the cone is so small
344 // that over the whole height the difference is less than distThresh
345 if(std::sin(m_cone.Angle()) * length / 2 < distThresh)
346 {
347 // construct the cylinder
348 // it has the same axis as the cone
349 // and we use the average radius of the cone
350 Cylinder cylinder(m_cone.AxisDirection(), m_cone.Center(),
351 meanRadius);
352 suggestions->push_back(new CylinderPrimitiveShape(cylinder));
353 suggestions->back()->Release();
354 }
355
356 // We suggest a sphere if a curvature of mean radius along the height
357 // does not introduce too large an error
358 float sphereRadius = std::tan(m_cone.Angle()) * meanLength;
359 float radiusDiff = (std::sqrt(sphereRadius * sphereRadius + length * length / 4)
360 - sphereRadius) / 2;
361 if(radiusDiff < distThresh)
362 {
363 // the center of the sphere is given as the point on the axis
364 // with the height of the mean length
365 Vec3f center = (meanLength / std::cos(m_cone.Angle()))
366 * m_cone.AxisDirection() + m_cone.Center();
367 Sphere sphere(center, sphereRadius + radiusDiff);
368 suggestions->push_back(new SpherePrimitiveShape(sphere));
369 suggestions->back()->Release();
370 }
371
372 // We suggest a plane if the mean radius causes only a small error
373 // for this we need the angular extent in the curved direction of the cone
374 radiusDiff = meanRadius - std::sin(radialExtent) * meanRadius;
375 if(radiusDiff < distThresh)
376 {
377 GfxTL::Vector2Df bboxCenter;
378 m_extBbox.Center(&bboxCenter);
379 Vec3f pos, normal;
380 InSpace(bboxCenter[0], bboxCenter[1] * m_cone.RadiusAtLength(bboxCenter[0]),
381 &pos, &normal);
382 Plane plane(pos, normal);
383 suggestions->push_back(new PlanePrimitiveShape(plane));
384 suggestions->back()->Release();
385 }*/
386 }
387
388 bool ConePrimitiveShape::Similar(float tolerance,
389 const ConePrimitiveShape &shape) const
390 {
391 return m_cone.Angle() <= (1.f + tolerance) * shape.m_cone.Angle()
392 && (1.f + tolerance) * m_cone.Angle() >= shape.m_cone.Angle();
393 }
394
395 void ConePrimitiveShape::BitmapExtent(float epsilon,
396 GfxTL::AABox< GfxTL::Vector2Df > *bbox,
397 MiscLib::Vector< std::pair< float, float > > *params,
398 size_t *uextent, size_t *vextent)
399 {
400 *uextent = static_cast<size_t>(std::ceil((bbox->Max()[0] - bbox->Min()[0]) / epsilon)); // no wrappig along u direction
401 *vextent = static_cast<size_t>(std::ceil((bbox->Max()[1] - bbox->Min()[1]) / epsilon)) + 1; // add one for wrapping
402 if((*vextent) * (*uextent) > 1e6 && m_cone.Angle() < float(M_PI / 4))
403 {
404 // try to reparameterize
405 // try to find cut in the outer regions
406 MiscLib::Vector< float > angularParams;//(params->size());
407 angularParams.reserve(params->size());
408 float outer = 3.f * std::max(fabs(bbox->Min()[0]), fabs(bbox->Max()[0])) / 4.f;
409 for(size_t i = 0; i < params->size(); ++i)
410 if((*params)[i].first > outer)
411 angularParams.push_back(((*params)[i].second
412 / m_cone.RadiusAtLength((*params)[i].first)) + float(M_PI));
413 std::sort(angularParams.begin(), angularParams.end());
414 // try to find a large gap
415 float maxGap = 0;
416 float lower, upper;
417 for(size_t i = 1; i < angularParams.size(); ++i)
418 {
419 float gap = angularParams[i] - angularParams[i - 1];
420 if(gap > maxGap)
421 {
422 maxGap = gap;
423 lower = angularParams[i - 1];
424 upper = angularParams[i];
425 }
426 }
427 // reparameterize with new angular cut
428 float newCut = (lower + upper) / 2.f;
429 m_cone.RotateAngularDirection(newCut);
430 bbox->Min()[1] = std::numeric_limits< float >::infinity();
431 bbox->Max()[1] = -std::numeric_limits< float >::infinity();
432 for(size_t i = 0; i < params->size(); ++i)
433 {
434 float r = m_cone.RadiusAtLength((*params)[i].first);
435 (*params)[i].second = ((*params)[i].second / r) + float(M_PI) - newCut;
436 if((*params)[i].second < 0)
437 (*params)[i].second = 2 * float(M_PI) + (*params)[i].second;
438 (*params)[i].second = ((*params)[i].second - float(M_PI)) * r;
439 if((*params)[i].second < bbox->Min()[1])
440 bbox->Min()[1] = (*params)[i].second;
441 if((*params)[i].second > bbox->Max()[1])
442 bbox->Max()[1] = (*params)[i].second;
443 }
444 *vextent = static_cast<size_t>(std::floor((bbox->Max()[1] - bbox->Min()[1]) / epsilon)) + 1;
445 }
446 }
447
448 void ConePrimitiveShape::InBitmap(const std::pair< float, float > &param,
449 float epsilon, const GfxTL::AABox< GfxTL::Vector2Df > &bbox,
450 size_t uextent, size_t vextent, std::pair< int, int > *inBmp) const
451 {
452 // convert u = length and v = arc length into bitmap coordinates
453 inBmp->first = static_cast<int>(std::floor((param.first - bbox.Min()[0]) / epsilon));
454 inBmp->second = static_cast<int>(std::floor((param.second - bbox.Min()[1]) / epsilon));
455 }
456
457 void ConePrimitiveShape::PreWrapBitmap(
458 const GfxTL::AABox< GfxTL::Vector2Df > &bbox, float epsilon,
459 size_t uextent, size_t vextent, MiscLib::Vector< char > *bmp) const
460 {
461 if(m_cone.Angle() >= float(M_PI / 4))
462 return;
463 // for wrapping we copy the first pixel to the last one in each v column
464 for(size_t u = 0; u < uextent; ++u)
465 {
466 // determine the coordinates of the last pixel in the column
467 // get the radius of the column
468 float r = m_cone.RadiusAtLength(u * epsilon + bbox.Min()[0]);
469 size_t v = static_cast<size_t>(std::floor((2 * M_PI * r - bbox.Min()[1]) / epsilon)) + 1;
470 if(v >= vextent)
471 continue;
472 if((*bmp)[u])
473 (*bmp)[v * uextent + u] = (*bmp)[u]; // do the wrap
474 // if(!(*bmp)[u * vextent + v])
475 // (*bmp)[u * vextent + v] = (*bmp)[u * vextent]; // do the wrap
476 }
477 }
478
479 void ConePrimitiveShape::WrapBitmap(
480 const GfxTL::AABox< GfxTL::Vector2Df > &bbox, float epsilon, bool *uwrap,
481 bool *vwrap) const
482 {
483 *uwrap = *vwrap = false;
484 }
485
486 void ConePrimitiveShape::WrapComponents(
487 const GfxTL::AABox< GfxTL::Vector2Df > &bbox,
488 float epsilon, size_t uextent, size_t vextent,
489 MiscLib::Vector< int > *componentImg,
490 MiscLib::Vector< std::pair< int, size_t > > *labels) const
491 {
492 if(m_cone.Angle() >= float(M_PI / 4))
493 return;
494 // for wrapping we copy the first pixel to the last one in each v column
495 for(size_t u = 0; u < uextent; ++u)
496 {
497 // determine the coordinates of the last pixel in the column
498 // get the radius of the column
499 float r = m_cone.RadiusAtLength(u * epsilon + bbox.Min()[0]);
500 size_t v = static_cast<size_t>(std::floor((2 * M_PI * r - bbox.Min()[1]) / epsilon)) + 1;
501 if(v >= vextent)
502 continue;
503 if((*componentImg)[u])
504 (*componentImg)[v * uextent + u] = (*componentImg)[u]; // do the wrap
505 }
506 // relabel the components
507 MiscLib::Vector< std::pair< int, size_t > > tempLabels(*labels);
508 for(size_t u = 0; u < uextent; ++u)
509 {
510 float r = m_cone.RadiusAtLength(u * epsilon + bbox.Min()[0]);
511 size_t v = static_cast<size_t>(std::floor((2 * M_PI * r - bbox.Min()[1]) / epsilon)) + 1;
512 if(v >= vextent)
513 continue;
514 if(!(*componentImg)[v * uextent + u])
515 continue;
516 // get the neighbors
517 int n[8];
518 size_t i = 0;
519 if(v >= 1)
520 {
521 size_t prevRow = (v - 1) * uextent;
522 if(u >= 1)
523 n[i++] = (*componentImg)[prevRow + u - 1];
524 n[i++] = (*componentImg)[prevRow + u];
525 if(u < uextent - 1)
526 n[i++] = (*componentImg)[prevRow + u + 1];
527 }
528 size_t row = v * uextent;
529 if(u >= 1)
530 n[i++] = (*componentImg)[row + u - 1];
531 if(u < uextent - 1)
532 n[i++] = (*componentImg)[row + u + 1];
533 if(v < vextent - 1)
534 {
535 size_t nextRow = (v + 1) * uextent;
536 if(u >= 1)
537 n[i++] = (*componentImg)[nextRow + u - 1];
538 n[i++] = (*componentImg)[nextRow + u];
539 if(u < uextent - 1)
540 n[i++] = (*componentImg)[nextRow + u + 1];
541 }
542 // associate labels
543 int l = (*componentImg)[v * uextent + u];
544 for(size_t j = 0; j < i; ++j)
545 if(n[j])
546 AssociateLabel(l, n[j], &tempLabels);
547 }
548 // condense labels
549 for(int i = static_cast<int>(tempLabels.size()) - 1; i > 0; --i)
550 tempLabels[i].first = ReduceLabel(i, tempLabels);
551 MiscLib::Vector< int > condensed(tempLabels.size());
552 labels->clear();
553 labels->reserve(condensed.size());
554 int count = 0;
555 for(size_t i = 0; i < tempLabels.size(); ++i)
556 if(i == tempLabels[i].first)
557 {
558 labels->push_back(std::make_pair(count, tempLabels[i].second));
559 condensed[i] = count;
560 ++count;
561 }
562 else
563 (*labels)[condensed[tempLabels[i].first]].second
564 += tempLabels[i].second;
565 // set new component ids
566 for(size_t i = 0; i < componentImg->size(); ++i)
567 (*componentImg)[i] =
568 condensed[tempLabels[(*componentImg)[i]].first];
569 }
570
571 void ConePrimitiveShape::SetExtent(const GfxTL::AABox< GfxTL::Vector2Df > &bbox,
572 const MiscLib::Vector< int > &componentsImg, size_t uextent,
573 size_t vextent, float epsilon, int label)
574 {}
575
576 bool ConePrimitiveShape::InSpace(float length, float arcLength, Vec3f *p,
577 Vec3f *n) const
578 {
579 float angle;
580 if(m_cone.Angle() >= float(M_PI / 4))
581 {
582 //angle = std::atan2(arcLength, length);
583 angle = std::atan2(length, arcLength);
584 length = std::sqrt(length * length + arcLength * arcLength);
585 // angle = std::acos(GfxTL::Math< float >::Clamp(arcLength / length, -1.f, 1.f));
586 }
587 else
588 angle = (arcLength / m_cone.RadiusAtLength(length)) + float(M_PI);
589 GfxTL::Quaternion< float > q;
590 q.RotationRad(angle, m_cone.AxisDirection()[0],
591 m_cone.AxisDirection()[1], m_cone.AxisDirection()[2]);
592 Vec3f vvec;
593 q.Rotate(m_cone.AngularDirection(), &vvec);
594 *p = std::sin(m_cone.Angle()) * fabs(length) * vvec +
595 std::cos(m_cone.Angle()) * length * m_cone.AxisDirection() +
596 m_cone.Center();
597 m_cone.Normal(*p, n);
598 return true;
599 }
600
601 bool ConePrimitiveShape::InSpace(size_t u, size_t v, float epsilon,
602 const GfxTL::AABox< GfxTL::Vector2Df > &bbox, size_t uextent,
603 size_t vextent, Vec3f *p, Vec3f *n) const
604 {
605 float length, angle;
606 if(m_cone.Angle() >= float(M_PI / 4))
607 {
608 float uf = ((float(u) + .5f) * epsilon) + bbox.Min()[0];
609 float vf = ((float(v) + .5f) * epsilon) + bbox.Min()[1];
610 length = std::sqrt(uf * uf + vf * vf);
611 //angle = std::atan2(vf, uf);
612 angle = std::atan2(uf, vf);
613 //angle = std::acos(GfxTL::Math< float >::Clamp(vf / length, -1.f, 1.f));
614 }
615 else
616 {
617 length = ((float(u) + .5f) * epsilon) + bbox.Min()[0];
618 float arcLength = ((float(v) + .5f) * epsilon) + bbox.Min()[1];
619 angle = (arcLength / m_cone.RadiusAtLength(length)) + float(M_PI);
620 }
621 if(angle > 2 * float(M_PI))
622 return false;
623 //float angle = ((v * epsilon) / m_cone.RadiusAtLength(std::max(fabs(bbox.Min()[0]), fabs(bbox.Max()[0])))
624 // + bbox.Min()[1]);
625 GfxTL::Quaternion< float > q;
626 q.RotationRad(angle, m_cone.AxisDirection()[0],
627 m_cone.AxisDirection()[1], m_cone.AxisDirection()[2]);
628 Vec3f vvec;
629 q.Rotate(m_cone.AngularDirection(), &vvec);
630 *p = std::sin(m_cone.Angle()) * fabs(length) * vvec +
631 std::cos(m_cone.Angle()) * length * m_cone.AxisDirection() +
632 m_cone.Center();
633 // TODO: this is very lazy and should be optimized!
634 m_cone.Normal(*p, n);
635 return true;
636 }