Форум на Исходниках.RU

// Функция getDistance2Edge вычисляет квадрат расстояния от точки до ребра треугольника

bool getDistance2Edge ( double a, double b, double c, double & d )

{

// Проверим попадает ли проекция точки на ребро?

const double t = fabs ( b - c );

if ( t >= a ) return false;

// Вычислим квадрат расстояния до ребра

d = b + c - 0.5 * ( a + t * t / a );

if ( d > 0 )

d *= 0.5;

else

d = 0;

return true;

}

double getDistance2Facet ( double a, double b, double c, double d, double e, double f, double q )

{

// Вычислим скалярные произведения

const double p1 = 0.5 * ( a + c - b );

const double p2 = 0.5 * ( d + a - e );

const double p3 = 0.5 * ( c + d - f );

// Проверим попадает ли проекция точки внутрь треугольника?

const double g = a * p3 - p1 * p2;

if ( g <= 0 ) return q;

// Если треугольник - вырожденный, то вернём квадрат расстояния до ребра

const double h = a * c - p1 * p1;

if ( h <= 0 ) return q;

const double gg = g * g;

const double ah = a * h;

if ( 4 * gg * c >= ah * h ) return q;

// Вычислим квадрат расстояния до плоскости треугольника

q -= gg / ah;

return q < 0 ? 0 : q;

}

double getDistance2Triangle ( double ab, double bc, double ca, double ap, double bp, double cp )

{

double d = _min ( ap, bp, cp );

if ( ! d ) return 0;

// Вычислим квадраты расстояний до рёбер треугольника

double d1, d2, d3;

nat edge = 0; // номер ближайшего к точке ребра

nat count = 0; // количество рёбер на которые есть проекция точки

if ( getDistance2Edge ( ab, ap, bp, d1 ) )

{

if ( d > d1 ) d = d1, edge = 1;

++count;

}

if ( getDistance2Edge ( bc, bp, cp, d2 ) )

{

if ( d > d2 ) d = d2, edge = 2;

++count;

}

if ( getDistance2Edge ( ca, cp, ap, d3 ) )

{

if ( d > d3 ) d = d3, edge = 3;

++count;

}

if ( ! d ) return 0;

// Если точка проецируется хотя бы на 2 ребра треугольника

if ( count > 1 )

{

switch ( edge )

{

case 1: return getDistance2Facet ( ab, bc, ca, ap, bp, cp, d );

case 2: return getDistance2Facet ( bc, ca, ab, bp, cp, ap, d );

case 3: return getDistance2Facet ( ca, ab, bc, cp, ap, bp, d );

}

return d;

}

double getDistance2 ( const Triangle3d & t, const Vector3d & p )

{

// Вычислим квадраты расстояний до вершин треугольника

const double ap = qmod ( t.a - p );

const double bp = qmod ( t.b - p );

const double cp = qmod ( t.c - p );

// Вычислим квадраты расстояний между вершинами треугольника

const double ab = qmod ( t.a - t.b );

const double bc = qmod ( t.b - t.c );

const double ca = qmod ( t.c - t.a );

// Основной алгоритм

return getDistance2Triangle ( ab, bc, ca, ap, bp, cp );

}