| 1 |
#ifndef CONE_HEADER |
| 2 |
#define CONE_HEADER |
| 3 |
#ifdef DOPARALLEL |
| 4 |
#include <omp.h> |
| 5 |
#endif |
| 6 |
#include "basic.h" |
| 7 |
#include "PointCloud.h" |
| 8 |
#include <GfxTL/HyperplaneCoordinateSystem.h> |
| 9 |
#include <stdexcept> |
| 10 |
#include <ostream> |
| 11 |
#include <istream> |
| 12 |
#include <stdio.h> |
| 13 |
#if !defined(_WIN32) && !defined(WIN32) |
| 14 |
#include <unistd.h> |
| 15 |
#endif |
| 16 |
#include <MiscLib/NoShrinkVector.h> |
| 17 |
#include "LevMarLSWeight.h" |
| 18 |
#include "LevMarFitting.h" |
| 19 |
|
| 20 |
#ifndef DLL_LINKAGE |
| 21 |
#define DLL_LINKAGE |
| 22 |
#endif |
| 23 |
|
| 24 |
// This implements a one sided cone! |
| 25 |
class DLL_LINKAGE Cone |
| 26 |
{ |
| 27 |
public: |
| 28 |
struct ConeInfo |
| 29 |
{ |
| 30 |
Vec3f center; |
| 31 |
Vec3f axisDirection; |
| 32 |
float angle; |
| 33 |
Vec3f minPoint; |
| 34 |
Vec3f maxPoint; |
| 35 |
float height; |
| 36 |
float minHeight; |
| 37 |
float maxHeight; |
| 38 |
float minRadius; |
| 39 |
float maxRadius; |
| 40 |
}; |
| 41 |
struct ParallelPlanesError |
| 42 |
: public std::runtime_error |
| 43 |
{ |
| 44 |
ParallelPlanesError() |
| 45 |
: std::runtime_error("Parallel planes in cone construction") |
| 46 |
{} |
| 47 |
}; |
| 48 |
enum { RequiredSamples = 3 }; |
| 49 |
Cone(); |
| 50 |
Cone(const Vec3f ¢er, const Vec3f &axisDir, float angle); |
| 51 |
Cone(const Vec3f &p1, const Vec3f &p2, const Vec3f &p3, |
| 52 |
const Vec3f &n1, const Vec3f &n2, const Vec3f &n3); |
| 53 |
bool Init(const MiscLib::Vector< Vec3f > &samples); |
| 54 |
bool InitAverage(const MiscLib::Vector< Vec3f > &samples); |
| 55 |
bool Init(const Vec3f ¢er, const Vec3f &axisDir, float angle); |
| 56 |
bool Init(const Vec3f &p1, const Vec3f &p2, const Vec3f &p3, |
| 57 |
const Vec3f &n1, const Vec3f &n2, const Vec3f &n3); |
| 58 |
bool Init(bool binary, std::istream *i); |
| 59 |
void Init(FILE *i); |
| 60 |
void Init(float* array); |
| 61 |
inline float Distance(const Vec3f &p) const; |
| 62 |
inline void Normal(const Vec3f &p, Vec3f *n) const; |
| 63 |
inline float DistanceAndNormal(const Vec3f &p, Vec3f *n) const; |
| 64 |
inline float SignedDistance(const Vec3f &p) const; |
| 65 |
inline float SignedDistanceAndNormal(const Vec3f &p, Vec3f *n) const; |
| 66 |
void Project(const Vec3f &p, Vec3f *pp) const; |
| 67 |
// Paramterizes into (length, angle) |
| 68 |
void Parameters(const Vec3f &p, |
| 69 |
std::pair< float, float > *param) const; |
| 70 |
inline float Height(const Vec3f &p) const; |
| 71 |
inline float Angle() const; |
| 72 |
inline const Vec3f &Center() const; |
| 73 |
inline const Vec3f &AxisDirection() const; |
| 74 |
Vec3f &AxisDirection() { return m_axisDir; } |
| 75 |
inline const Vec3f AngularDirection() const; |
| 76 |
//void AngularDirection(const Vec3f &angular); |
| 77 |
void RotateAngularDirection(float radians); |
| 78 |
inline float RadiusAtLength(float length) const; |
| 79 |
bool LeastSquaresFit(const PointCloud &pc, |
| 80 |
MiscLib::Vector< size_t >::const_iterator begin, |
| 81 |
MiscLib::Vector< size_t >::const_iterator end); |
| 82 |
template< class IteratorT > |
| 83 |
bool LeastSquaresFit(IteratorT begin, IteratorT end); |
| 84 |
bool Fit(const PointCloud &pc, |
| 85 |
MiscLib::Vector< size_t >::const_iterator begin, |
| 86 |
MiscLib::Vector< size_t >::const_iterator end) |
| 87 |
{ return LeastSquaresFit(pc, begin, end); } |
| 88 |
static bool Interpolate(const MiscLib::Vector< Cone > &cones, |
| 89 |
const MiscLib::Vector< float > &weights, Cone *ic); |
| 90 |
void Serialize(bool binary, std::ostream *o) const; |
| 91 |
static size_t SerializedSize(); |
| 92 |
void Serialize(FILE *o) const; |
| 93 |
void Serialize(float* array) const; |
| 94 |
static size_t SerializedFloatSize(); |
| 95 |
void Transform(float scale, const Vec3f &translate); |
| 96 |
void Transform(const GfxTL::MatrixXX< 3, 3, float > &rot, |
| 97 |
const GfxTL::Vector3Df &trans); |
| 98 |
inline unsigned int Intersect(const Vec3f &p, const Vec3f &r, |
| 99 |
float lambda[2], Vec3f interPts[2]) const; |
| 100 |
ConeInfo GetInfo(const PointCloud& pc, size_t startingIndx, size_t endingIndx) const; |
| 101 |
ConeInfo GetInfo(const MiscLib::Vector< Vec3f >& samples) const; |
| 102 |
private: |
| 103 |
template< class WeightT > |
| 104 |
class LevMarCone |
| 105 |
: public WeightT |
| 106 |
{ |
| 107 |
public: |
| 108 |
enum { NumParams = 7 }; |
| 109 |
typedef float ScalarType; |
| 110 |
|
| 111 |
template< class IteratorT > |
| 112 |
ScalarType Chi(const ScalarType *params, IteratorT begin, IteratorT end, |
| 113 |
ScalarType *values, ScalarType *temp) const |
| 114 |
{ |
| 115 |
ScalarType chi = 0; |
| 116 |
ScalarType cosPhi = std::cos(params[6]); |
| 117 |
ScalarType sinPhi = std::sin(params[6]); |
| 118 |
size_t size = end - begin; |
| 119 |
#ifdef DOPARALLEL |
| 120 |
#pragma omp parallel for schedule(static) reduction(+:chi) |
| 121 |
#endif |
| 122 |
for(int idx = 0; idx < size; ++idx) |
| 123 |
{ |
| 124 |
Vec3f s; |
| 125 |
for(unsigned int j = 0; j < 3; ++j) |
| 126 |
s[j] = begin[idx][j] - params[j]; |
| 127 |
ScalarType g = fabs(s[0] * params[3] + s[1] * params[4] + s[2] * params[5]); |
| 128 |
ScalarType f = s.sqrLength() - (g * g); |
| 129 |
if(f <= 0) |
| 130 |
f = 0; |
| 131 |
else |
| 132 |
f = std::sqrt(f); |
| 133 |
temp[idx] = f; |
| 134 |
chi += (values[idx] = WeightT::Weigh(cosPhi * f - sinPhi * g)) |
| 135 |
* values[idx]; |
| 136 |
} |
| 137 |
return chi; |
| 138 |
} |
| 139 |
|
| 140 |
template< class IteratorT > |
| 141 |
void Derivatives(const ScalarType *params, IteratorT begin, IteratorT end, |
| 142 |
const ScalarType *values, const ScalarType *temp, ScalarType *matrix) const |
| 143 |
{ |
| 144 |
ScalarType sinPhi = -std::sin(params[6]); |
| 145 |
ScalarType cosPhi = std::cos(params[6]); |
| 146 |
size_t size = end - begin; |
| 147 |
#ifdef DOPARALLEL |
| 148 |
#pragma omp parallel for schedule(static) |
| 149 |
#endif |
| 150 |
for(int idx = 0; idx < size; ++idx) |
| 151 |
{ |
| 152 |
Vec3f s; |
| 153 |
for(unsigned int j = 0; j < 3; ++j) |
| 154 |
s[j] = begin[idx][j] - params[j]; |
| 155 |
ScalarType g = fabs(s[0] * params[3] + s[1] * params[4] + s[2] * params[5]); |
| 156 |
ScalarType ggradient[6]; |
| 157 |
for(unsigned int j = 0; j < 3; ++j) |
| 158 |
ggradient[j] = -params[j + 3]; |
| 159 |
for(unsigned int j = 0; j < 3; ++j) |
| 160 |
ggradient[j + 3] = s[j] - params[j + 3] * g; |
| 161 |
ScalarType fgradient[6]; |
| 162 |
if(temp[idx] < 1.0e-6) |
| 163 |
{ |
| 164 |
fgradient[0] = std::sqrt(1 - params[3] * params[3]); |
| 165 |
fgradient[1] = std::sqrt(1 - params[4] * params[4]); |
| 166 |
fgradient[2] = std::sqrt(1 - params[5] * params[5]); |
| 167 |
} |
| 168 |
else |
| 169 |
{ |
| 170 |
fgradient[0] = (params[3] * g - s[0]) / temp[idx]; |
| 171 |
fgradient[1] = (params[4] * g - s[1]) / temp[idx]; |
| 172 |
fgradient[2] = (params[5] * g - s[2]) / temp[idx]; |
| 173 |
} |
| 174 |
fgradient[3] = g * fgradient[0]; |
| 175 |
fgradient[4] = g * fgradient[1]; |
| 176 |
fgradient[5] = g * fgradient[2]; |
| 177 |
for(unsigned int j = 0; j < 6; ++j) |
| 178 |
matrix[idx * NumParams + j] = |
| 179 |
cosPhi * fgradient[j] + sinPhi * ggradient[j]; |
| 180 |
matrix[idx * NumParams + 6] = temp[idx] * sinPhi - g * cosPhi; |
| 181 |
WeightT::template DerivWeigh< NumParams >(cosPhi * temp[idx] + sinPhi * g, |
| 182 |
matrix + idx * NumParams); |
| 183 |
} |
| 184 |
} |
| 185 |
|
| 186 |
void Normalize(float *params) const |
| 187 |
{ |
| 188 |
// normalize direction |
| 189 |
ScalarType l = std::sqrt(params[3] * params[3] + params[4] * params[4] + |
| 190 |
params[5] * params[5]); |
| 191 |
for(unsigned int i = 3; i < 6; ++i) |
| 192 |
params[i] /= l; |
| 193 |
// normalize angle |
| 194 |
params[6] -= std::floor(params[6] / (2 * ScalarType(M_PI))) * (2 * ScalarType(M_PI)); // params[6] %= 2*M_PI |
| 195 |
if(params[6] > M_PI) |
| 196 |
{ |
| 197 |
params[6] -= std::floor(params[6] / ScalarType(M_PI)) * ScalarType(M_PI); // params[6] %= M_PI |
| 198 |
for(unsigned int i = 3; i < 6; ++i) |
| 199 |
params[i] *= -1; |
| 200 |
} |
| 201 |
if(params[6] > ScalarType(M_PI) / 2) |
| 202 |
params[6] = ScalarType(M_PI) - params[6]; |
| 203 |
} |
| 204 |
}; |
| 205 |
|
| 206 |
private: |
| 207 |
Vec3f m_center; // this is the apex of the cone |
| 208 |
Vec3f m_axisDir; // the axis points into the interior of the cone |
| 209 |
float m_angle; // the opening angle |
| 210 |
Vec3f m_normal; |
| 211 |
Vec3f m_normalY; // precomputed normal part |
| 212 |
float m_n2d[2]; |
| 213 |
GfxTL::HyperplaneCoordinateSystem< float, 3 > m_hcs; |
| 214 |
float m_angularRotatedRadians; |
| 215 |
}; |
| 216 |
|
| 217 |
inline float Cone::Distance(const Vec3f &p) const |
| 218 |
{ |
| 219 |
// this is for one sided cone! |
| 220 |
Vec3f s = p - m_center; |
| 221 |
float g = s.dot(m_axisDir); // distance to plane orhogonal to |
| 222 |
// axisdir through center |
| 223 |
// distance to axis |
| 224 |
float sqrS = s.sqrLength(); |
| 225 |
float f = sqrS - (g * g); |
| 226 |
if(f <= 0) |
| 227 |
f = 0; |
| 228 |
else |
| 229 |
f = std::sqrt(f); |
| 230 |
float da = m_n2d[0] * f; |
| 231 |
float db = m_n2d[1] * g; |
| 232 |
if(g < 0 && da - db < 0) // is inside other side of cone -> disallow |
| 233 |
return std::sqrt(sqrS); |
| 234 |
return fabs(da + db); |
| 235 |
} |
| 236 |
|
| 237 |
inline void Cone::Normal(const Vec3f &p, Vec3f *n) const |
| 238 |
{ |
| 239 |
Vec3f s = p - m_center; |
| 240 |
Vec3f pln = s.cross(m_axisDir); |
| 241 |
Vec3f plx = m_axisDir.cross(pln); |
| 242 |
plx.normalize(); |
| 243 |
// we are not dealing with two-sided cone |
| 244 |
*n = m_normal[0] * plx + m_normalY; |
| 245 |
} |
| 246 |
|
| 247 |
inline float Cone::DistanceAndNormal(const Vec3f &p, Vec3f *n) const |
| 248 |
{ |
| 249 |
// this is for one-sided cone !!! |
| 250 |
Vec3f s = p - m_center; |
| 251 |
float g = s.dot(m_axisDir); // distance to plane orhogonal to |
| 252 |
// axisdir through center |
| 253 |
// distance to axis |
| 254 |
float sqrS = s.sqrLength(); |
| 255 |
float f = sqrS - (g * g); |
| 256 |
if(f <= 0) |
| 257 |
f = 0; |
| 258 |
else |
| 259 |
f = std::sqrt(f); |
| 260 |
float da = m_n2d[0] * f; |
| 261 |
float db = m_n2d[1] * g; |
| 262 |
float dist; |
| 263 |
if(g < 0 && da - db < 0) // is inside other side of cone -> disallow |
| 264 |
dist = std::sqrt(sqrS); |
| 265 |
else |
| 266 |
dist = fabs(da + db); |
| 267 |
// need normal |
| 268 |
Vec3f plx = s - g * m_axisDir; |
| 269 |
plx.normalize(); |
| 270 |
*n = m_normal[0] * plx + m_normalY; |
| 271 |
return dist; |
| 272 |
} |
| 273 |
|
| 274 |
inline float Cone::SignedDistance(const Vec3f &p) const |
| 275 |
{ |
| 276 |
// this is for one sided cone! |
| 277 |
Vec3f s = p - m_center; |
| 278 |
float g = s.dot(m_axisDir); // distance to plane orhogonal to |
| 279 |
// axisdir through center |
| 280 |
// distance to axis |
| 281 |
float sqrS = s.sqrLength(); |
| 282 |
float f = sqrS - (g * g); |
| 283 |
if(f <= 0) |
| 284 |
f = 0; |
| 285 |
else |
| 286 |
f = std::sqrt(f); |
| 287 |
float da = m_n2d[0] * f; |
| 288 |
float db = m_n2d[1] * g; |
| 289 |
if(g < 0 && da - db < 0) // is inside other side of cone -> disallow |
| 290 |
return std::sqrt(sqrS); |
| 291 |
return da + db; |
| 292 |
} |
| 293 |
|
| 294 |
inline float Cone::SignedDistanceAndNormal(const Vec3f &p, Vec3f *n) const |
| 295 |
{ |
| 296 |
// this is for one-sided cone !!! |
| 297 |
Vec3f s = p - m_center; |
| 298 |
float g = s.dot(m_axisDir); // distance to plane orhogonal to |
| 299 |
// axisdir through center |
| 300 |
// distance to axis |
| 301 |
float sqrS = s.sqrLength(); |
| 302 |
float f = sqrS - (g * g); |
| 303 |
if(f <= 0) |
| 304 |
f = 0; |
| 305 |
else |
| 306 |
f = std::sqrt(f); |
| 307 |
float da = m_n2d[0] * f; |
| 308 |
float db = m_n2d[1] * g; |
| 309 |
float dist; |
| 310 |
if(g < 0 && da - db < 0) // is inside other side of cone -> disallow |
| 311 |
dist = std::sqrt(sqrS); |
| 312 |
else |
| 313 |
dist = da + db; |
| 314 |
// need normal |
| 315 |
Vec3f plx = s - g * m_axisDir; |
| 316 |
plx.normalize(); |
| 317 |
*n = m_normal[0] * plx + m_normalY; |
| 318 |
return dist; |
| 319 |
} |
| 320 |
|
| 321 |
float Cone::Angle() const |
| 322 |
{ |
| 323 |
return m_angle; |
| 324 |
} |
| 325 |
|
| 326 |
const Vec3f &Cone::Center() const |
| 327 |
{ |
| 328 |
return m_center; |
| 329 |
} |
| 330 |
|
| 331 |
const Vec3f &Cone::AxisDirection() const |
| 332 |
{ |
| 333 |
return m_axisDir; |
| 334 |
} |
| 335 |
|
| 336 |
const Vec3f Cone::AngularDirection() const |
| 337 |
{ |
| 338 |
return Vec3f(m_hcs[0].Data()); |
| 339 |
} |
| 340 |
|
| 341 |
float Cone::RadiusAtLength(float length) const |
| 342 |
{ |
| 343 |
return std::sin(m_angle) * fabs(length); |
| 344 |
} |
| 345 |
|
| 346 |
float Cone::Height(const Vec3f &p) const |
| 347 |
{ |
| 348 |
Vec3f s = p - m_center; |
| 349 |
return m_axisDir.dot(s); |
| 350 |
} |
| 351 |
|
| 352 |
template< class IteratorT > |
| 353 |
bool Cone::LeastSquaresFit(IteratorT begin, IteratorT end) |
| 354 |
{ |
| 355 |
float param[7]; |
| 356 |
for(unsigned int i = 0; i < 3; ++i) |
| 357 |
param[i] = m_center[i]; |
| 358 |
for(unsigned int i = 0; i < 3; ++i) |
| 359 |
param[i + 3] = m_axisDir[i]; |
| 360 |
param[6] = m_angle; |
| 361 |
LevMarCone< LevMarLSWeight > levMarCone; |
| 362 |
if(!LevMar(begin, end, levMarCone, param)) |
| 363 |
return false; |
| 364 |
if(param[6] < 1.0e-6 || param[6] > float(M_PI) / 2 - 1.0e-6) |
| 365 |
return false; |
| 366 |
for(unsigned int i = 0; i < 3; ++i) |
| 367 |
m_center[i] = param[i]; |
| 368 |
for(unsigned int i = 0; i < 3; ++i) |
| 369 |
m_axisDir[i] = param[i + 3]; |
| 370 |
m_angle = param[6]; |
| 371 |
m_normal = Vec3f(std::cos(-m_angle), std::sin(-m_angle), 0); |
| 372 |
m_normalY = m_normal[1] * m_axisDir; |
| 373 |
m_n2d[0] = std::cos(m_angle); |
| 374 |
m_n2d[1] = -std::sin(m_angle); |
| 375 |
m_hcs.FromNormal(m_axisDir); |
| 376 |
m_angularRotatedRadians = 0; |
| 377 |
// it could be that the axis has flipped during fitting |
| 378 |
// we need to detect such a case |
| 379 |
// for this we run over all points and compute the sum |
| 380 |
// of their respective heights. If that sum is negative |
| 381 |
// the axis needs to be flipped. |
| 382 |
float heightSum = 0; |
| 383 |
intptr_t size = end - begin; |
| 384 |
//#ifndef _WIN64 // for some reason the Microsoft x64 compiler crashes at the next line |
| 385 |
#ifdef DOPARALLEL |
| 386 |
#pragma omp parallel for schedule(static) reduction(+:heightSum) |
| 387 |
#endif |
| 388 |
//#endif |
| 389 |
for(intptr_t i = 0; i < size; ++i) |
| 390 |
heightSum += Height(begin[i]); |
| 391 |
if(heightSum < 0) |
| 392 |
{ |
| 393 |
m_axisDir *= -1; |
| 394 |
m_hcs.FromNormal(m_axisDir); |
| 395 |
} |
| 396 |
return true; |
| 397 |
} |
| 398 |
|
| 399 |
inline unsigned int Cone::Intersect(const Vec3f &p, const Vec3f &r, |
| 400 |
float lambda[2], Vec3f interPts[2]) const |
| 401 |
{ |
| 402 |
// Set up the quadratic Q(t) = c2*t^2 + 2*c1*t + c0 that corresponds to |
| 403 |
// the cone. Let the vertex be V, the unit-length direction vector be A, |
| 404 |
// and the angle measured from the cone axis to the cone wall be Theta, |
| 405 |
// and define g = cos(Theta). A point X is on the cone wall whenever |
| 406 |
// Dot(A,(X-V)/|X-V|) = g. Square this equation and factor to obtain |
| 407 |
// (X-V)^T * (A*A^T - g^2*I) * (X-V) = 0 |
| 408 |
// where the superscript T denotes the transpose operator. This defines |
| 409 |
// a double-sided cone. The line is L(t) = P + t*D, where P is the line |
| 410 |
// origin and D is a unit-length direction vector. Substituting |
| 411 |
// X = L(t) into the cone equation above leads to Q(t) = 0. Since we |
| 412 |
// want only intersection points on the single-sided cone that lives in |
| 413 |
// the half-space pointed to by A, any point L(t) generated by a root of |
| 414 |
// Q(t) = 0 must be tested for Dot(A,L(t)-V) >= 0. |
| 415 |
using namespace std; |
| 416 |
float fAdD = m_axisDir.dot(r); |
| 417 |
float tmp = cos(m_angle); |
| 418 |
float fCosSqr = tmp*tmp; |
| 419 |
Vec3f kE = p - m_center; |
| 420 |
float fAdE = m_axisDir.dot(kE); |
| 421 |
float fDdE = r.dot(kE); |
| 422 |
float fEdE = kE.dot(kE); |
| 423 |
float fC2 = fAdD*fAdD - fCosSqr; |
| 424 |
float fC1 = fAdD*fAdE - fCosSqr*fDdE; |
| 425 |
float fC0 = fAdE*fAdE - fCosSqr*fEdE; |
| 426 |
float fdot; |
| 427 |
|
| 428 |
// Solve the quadratic. Keep only those X for which Dot(A,X-V) >= 0. |
| 429 |
unsigned int interCount = 0; |
| 430 |
if (fabs(fC2) >= 1e-7) |
| 431 |
{ |
| 432 |
// c2 != 0 |
| 433 |
float fDiscr = fC1*fC1 - fC0*fC2; |
| 434 |
if (fDiscr < 0) |
| 435 |
{ |
| 436 |
// Q(t) = 0 has no real-valued roots. The line does not |
| 437 |
// intersect the double-sided cone. |
| 438 |
return 0; |
| 439 |
} |
| 440 |
else if (fDiscr > 1e-7) |
| 441 |
{ |
| 442 |
// Q(t) = 0 has two distinct real-valued roots. However, one or |
| 443 |
// both of them might intersect the portion of the double-sided |
| 444 |
// cone "behind" the vertex. We are interested only in those |
| 445 |
// intersections "in front" of the vertex. |
| 446 |
float fRoot = sqrt(fDiscr); |
| 447 |
float fInvC2 = 1.0f/fC2; |
| 448 |
interCount = 0; |
| 449 |
|
| 450 |
float fT = (-fC1 - fRoot)*fInvC2; |
| 451 |
if(fT > 0) // intersect only in positive direction of ray |
| 452 |
{ |
| 453 |
interPts[interCount] = p + fT*r; |
| 454 |
kE = interPts[interCount] - m_center; |
| 455 |
fdot = kE.dot(m_axisDir); |
| 456 |
if (fdot > 0) |
| 457 |
{ |
| 458 |
lambda[interCount] = fT; |
| 459 |
interCount++; |
| 460 |
} |
| 461 |
} |
| 462 |
|
| 463 |
fT = (-fC1 + fRoot)*fInvC2; |
| 464 |
if(fT > 0) |
| 465 |
{ |
| 466 |
interPts[interCount] = p + fT*r; |
| 467 |
kE = interPts[interCount] - m_center; |
| 468 |
fdot = kE.dot(m_axisDir); |
| 469 |
if (fdot > 0) |
| 470 |
{ |
| 471 |
lambda[interCount] = fT; |
| 472 |
interCount++; |
| 473 |
} |
| 474 |
} |
| 475 |
} |
| 476 |
else if(fC1 / fC2 < 0) |
| 477 |
{ |
| 478 |
// one repeated real root (line is tangent to the cone) |
| 479 |
interPts[0] = p - (fC1/fC2)*r; |
| 480 |
lambda[0] = -(fC1 / fC2); |
| 481 |
kE = interPts[0] - m_center; |
| 482 |
if (kE.dot(m_axisDir) > 0) |
| 483 |
interCount = 1; |
| 484 |
else |
| 485 |
interCount = 0; |
| 486 |
} |
| 487 |
} |
| 488 |
else if (fabs(fC1) >= 1.0e-7f) |
| 489 |
{ |
| 490 |
// c2 = 0, c1 != 0 (D is a direction vector on the cone boundary) |
| 491 |
lambda[0] = -(0.5f * fC0/fC1); |
| 492 |
if(lambda[0] < 0) |
| 493 |
return 0; |
| 494 |
interPts[0] = p + lambda[0] *r; |
| 495 |
kE = interPts[0] - m_center; |
| 496 |
fdot = kE.dot(m_axisDir); |
| 497 |
if (fdot > 0) |
| 498 |
interCount = 1; |
| 499 |
else |
| 500 |
interCount = 0; |
| 501 |
} |
| 502 |
else if (abs(fC0) >= 1e-7) |
| 503 |
{ |
| 504 |
// c2 = c1 = 0, c0 != 0 |
| 505 |
interCount = 0; |
| 506 |
} |
| 507 |
else |
| 508 |
{ |
| 509 |
// c2 = c1 = c0 = 0, cone contains ray V+t*D where V is cone vertex |
| 510 |
// and D is the line direction. |
| 511 |
interCount = 1; |
| 512 |
lambda[0] = (m_center - p).dot(r); |
| 513 |
interPts[0] = m_center; |
| 514 |
} |
| 515 |
|
| 516 |
return interCount; |
| 517 |
} |
| 518 |
|
| 519 |
#endif |