MRTriMath.h

Go to the documentation of this file.

#pragma once
 
#include "MRVector2.h"
#include "MRVector3.h"
#include <algorithm>
#include <cassert>
#include <cmath>
#include <limits>
#include <optional>
 
namespace MR
{
 
 
template <typename T>
[[nodiscard]] T circumcircleDiameterSq( const Vector3<T> & a, const Vector3<T> & b, const Vector3<T> & c )
{
    const auto ab = ( b - a ).lengthSq();
    const auto ca = ( a - c ).lengthSq();
    const auto bc = ( c - b ).lengthSq();
    if ( ab <= 0 )
        return ca;
    if ( ca <= 0 )
        return bc;
    if ( bc <= 0 )
        return ab;
    const auto f = cross( b - a, c - a ).lengthSq();
    if ( f <= 0 )
        return std::numeric_limits<T>::infinity();
    return ab * ca * bc / f;
}
 
template <typename T>
[[nodiscard]] inline T circumcircleDiameter( const Vector3<T> & a, const Vector3<T> & b, const Vector3<T> & c )
{
    return std::sqrt( circumcircleDiameterSq( a, b, c ) );
}
 
template <typename T>
[[nodiscard]] Vector3<T> circumcircleCenter( const Vector3<T> & a, const Vector3<T> & b )
{
    const auto xabSq = cross( a, b ).lengthSq();
    const auto aa = a.lengthSq();
    const auto bb = b.lengthSq();
    if ( xabSq <= 0 )
    {
        if ( aa <= 0 )
            return b / T(2);
        return a / T(2);
    }
    const auto ab = dot( a, b );
    return ( bb * ( aa - ab ) * a + aa * ( bb - ab ) * b ) / ( 2 * xabSq );
}
 
template <typename T>
[[nodiscard]] inline Vector3<T> circumcircleCenter( const Vector3<T> & a, const Vector3<T> & b, const Vector3<T> & c )
{
    return circumcircleCenter( a - c, b - c ) + c;
}
 
template <typename T>
[[nodiscard]] bool circumballCenters( const Vector3<T> & a, const Vector3<T> & b, const Vector3<T> & c, T radius,
    Vector3<T> & centerPos, 
    Vector3<T> & centerNeg )
{
    const auto rr = sqr( radius );
    const auto circRadSq = circumcircleDiameterSq( a, b, c ) / T( 4 );
    if ( rr < circRadSq )
        return false;
 
    const auto x = std::sqrt( rr - circRadSq );
    const auto xn = x * normal( a, b, c );
    const auto circCenter = circumcircleCenter( a, b, c );
    centerPos = circCenter + xn;
    centerNeg = circCenter - xn;
 
    return true;
}
 
template <typename T>
[[nodiscard]] T minTriangleAngleSin( const Vector3<T> & a, const Vector3<T> & b, const Vector3<T> & c )
{
    const auto ab = ( b - a ).length();
    const auto ca = ( a - c ).length();
    const auto bc = ( c - b ).length();
    if ( ab <= 0  || ca <= 0 || bc <= 0 )
        return 0;
    const auto f = cross( b - a, c - a ).length();
    return f * std::min( { ab, ca, bc } ) / ( ab * ca * bc );
}
 
template <typename T>
[[nodiscard]] T minTriangleAngle( const Vector3<T> & a, const Vector3<T> & b, const Vector3<T> & c )
{
    return std::asin( minTriangleAngleSin( a, b, c ) );
}
 
template<typename T>
[[nodiscard]] T triangleAspectRatio( const Vector3<T> & a, const Vector3<T> & b, const Vector3<T> & c )
{
    const auto bc = ( c - b ).length();
    const auto ca = ( a - c ).length();
    const auto ab = ( b - a ).length();
    auto halfPerimeter = ( bc + ca + ab ) / 2;
    auto den = 8 * ( halfPerimeter - bc ) * ( halfPerimeter - ca ) * ( halfPerimeter - ab );
    if ( den <= 0 )
        return std::numeric_limits<T>::max();
 
    return bc * ca * ab / den;
}
 
template<typename T>
[[nodiscard]] inline Vector3<T> dirDblArea( const Triangle3<T> & t )
{
    return cross( t[1] - t[0], t[2] - t[0] );
}
 
template<typename T>
[[nodiscard]] inline Vector3<T> dirDblArea( const Vector3<T> & q, const Vector3<T> & r )
{
    return cross( q, r );
}
 
template<typename T>
[[nodiscard]] inline Vector3<T> dirDblArea( const Vector3<T> & p, const Vector3<T> & q, const Vector3<T> & r )
{
    return cross( q - p, r - p );
}
 
template<typename T>
[[nodiscard]] inline Vector3<T> normal( const Vector3<T> & q, const Vector3<T> & r )
{
    return dirDblArea( q, r ).normalized();
}
 
template<typename T>
[[nodiscard]] inline Vector3<T> normal( const Vector3<T> & p, const Vector3<T> & q, const Vector3<T> & r )
{
    return normal( q - p, r - p );
}
 
template<typename T>
[[nodiscard]] inline Vector3<T> normal( const Triangle3<T> & t )
{
    return normal( t[1] - t[0], t[2] - t[0] );
}
 
template<typename T>
[[nodiscard]] inline T dblAreaSq( const Vector3<T> & p, const Vector3<T> & q, const Vector3<T> & r )
{
    return dirDblArea( p, q, r ).lengthSq();
}
 
template<typename T>
[[nodiscard]] inline T dblArea( const Triangle3<T> & t )
{
    return dirDblArea( t ).length();
}
 
template<typename T>
[[nodiscard]] inline T dblArea( const Vector3<T> & p, const Vector3<T> & q, const Vector3<T> & r )
{
    return dirDblArea( p, q, r ).length();
}
 
template<typename T>
[[nodiscard]] inline T area( const Vector3<T> & p, const Vector3<T> & q, const Vector3<T> & r )
{
    return dblArea( p, q, r ) / 2;
}
 
template<typename T>
[[nodiscard]] inline T dblArea( const Vector2<T> & p, const Vector2<T> & q, const Vector2<T> & r )
{
    return std::abs( cross( q - p, r - p ) );
}
 
template<typename T>
[[nodiscard]] inline T area( const Vector2<T> & p, const Vector2<T> & q, const Vector2<T> & r )
{
    return dblArea( p, q, r ) / 2;
}
 
template <typename T>
[[nodiscard]] Triangle3<T> makeDegenerate( const Triangle3<T> & t )
{
    const auto c = ( t[0] + t[1] + t[2] ) / T(3);
    int longest = 0;
    T longestSq = 0;
    for ( int i = 0; i < 3; ++i )
    {
        const auto sq = ( t[i] - c ).lengthSq();
        if ( longestSq >= sq )
            continue;
        longest = i;
        longestSq = sq;
    }
    const auto d = ( t[longest] - c ).normalized();
 
    Triangle3<T> res;
    for ( int i = 0; i < 3; ++i )
        res[i] = c + d * dot( d, t[i] - c );
    return res;
}
 
template <typename T>
[[nodiscard]] Triangle3<T> triangleWithNormal( const Triangle3<T> & t, const Vector3<T> & n )
{
    const auto c = ( t[0] + t[1] + t[2] ) / T(3);
    Triangle3<T> res;
    for ( int i = 0; i < 3; ++i )
        res[i] = t[i] - n * dot( n, t[i] - c );
 
    if ( dot( n, dirDblArea( res ) ) < 0 ) 
        res = makeDegenerate( res );
    return res;
}
 
template <typename T>
[[nodiscard]] T dihedralAngleSin( const Vector3<T>& leftNorm, const Vector3<T>& rightNorm, const Vector3<T>& edgeVec )
{
    auto edgeDir = edgeVec.normalized();
    return dot( edgeDir, cross( leftNorm, rightNorm ) );
}
 
template <typename T>
[[nodiscard]] T dihedralAngleCos( const Vector3<T>& leftNorm, const Vector3<T>& rightNorm )
{
    return dot( leftNorm, rightNorm );
}
 
template <typename T>
[[nodiscard]] T dihedralAngle( const Vector3<T>& leftNorm, const Vector3<T>& rightNorm, const Vector3<T>& edgeVec )
{
    auto sin = dihedralAngleSin( leftNorm, rightNorm, edgeVec );
    auto cos = dihedralAngleCos( leftNorm, rightNorm );
    return std::atan2( sin, cos );
}
 
template <typename T>
[[nodiscard]] std::optional<Vector2<T>> posFromTriEdgeLengths( T a, T b, T c )
{
    if ( c == 0 )
    {
        if ( a == b )
            return Vector2<T>{ a, 0 };
        else
            return {};
    }
    const auto aa = sqr( a );
    const auto y = ( aa - sqr( b ) + sqr( c ) ) / ( 2 * c );
    const auto yy = sqr( y );
    if ( yy > aa )
        return {};
    const auto x = std::sqrt( aa - yy );
    return Vector2<T>{ x, y };
}
 
template <typename T>
[[nodiscard]] std::optional<T> quadrangleOtherDiagonal( T a, T b, T c, T a1, T b1 )
{
    const auto p = posFromTriEdgeLengths( a, b, c );
    if ( !p )
        return {};
    auto p1 = posFromTriEdgeLengths( a1, b1, c );
    if ( !p1 )
        return {};
    p1->x = -p1->x;
    auto y = ( p->x * p1->y - p1->x * p->y ) / ( p->x - p1->x );
    if ( y < 0 || y > c )
        return {};
    return ( *p - *p1 ).length();
}
 
template <typename T>
[[nodiscard]] inline T tanSqOfHalfAngle( T a, T b, T c )
{
    const T den = ( a + b + c ) * ( b + c - a );
    if ( den <= 0 )
        return std::numeric_limits<T>::infinity();
    const T num = ( a + c - b ) * ( a + b - c );
    if ( num <= 0 )
        return 0;
    return num / den;
}
 
template <typename T>
[[nodiscard]] inline T cotan( const Triangle3<T> & t, T absMaxVal = std::numeric_limits<T>::max() )
{
    auto a = t[0] - t[2];
    auto b = t[1] - t[2];
    auto nom = dot( a, b );
    auto den = cross( a, b ).length();
    if ( fabs( nom ) >= absMaxVal * den )
        return absMaxVal * sgn( nom );
    return nom / den;
}
 
template <typename T>
[[nodiscard]] inline T cotan( T a, T b, T c )
{
    const T den = ( a + b + c ) * ( b + c - a );
    if ( den <= 0 )
        return -std::numeric_limits<T>::infinity();
    const T num = ( a + c - b ) * ( a + b - c );
    if ( num <= 0 )
        return std::numeric_limits<T>::infinity();
    const auto tanSq = num / den;
    return ( 1 - tanSq ) / ( 2 * std::sqrt( tanSq ) );
}
 
template <typename T>
[[nodiscard]] std::optional<Vector3<T>> gradientInTri( const Vector3<T> & b, const Vector3<T> & c, T vb, T vc )
{
    const auto bb = dot( b, b );
    const auto bc = dot( b, c );
    const auto cc = dot( c, c );
    const auto det = bb * cc - bc * bc;
    if ( det <= 0 )
        return {};
    const auto kb = ( 1 / det ) * ( cc * vb - bc * vc );
    const auto kc = ( 1 / det ) * (-bc * vb + bb * vc );
    return kb * b + kc * c;
}
 
template <typename T>
[[nodiscard]] std::optional<Vector3<T>> gradientInTri( const Vector3<T> & a, const Vector3<T> & b, const Vector3<T> & c, T va, T vb, T vc )
{
    return gradientInTri( b - a, c - a, vb - va, vc - va );
}
 
template <typename T>
[[nodiscard]] std::optional<T> findTriExitPos( const Vector3<T> & b, const Vector3<T> & c, const Vector3<T> & grad )
{
    const auto gradSq = grad.lengthSq();
    if ( gradSq <= 0 )
        return {};
    const auto d = c - b;
    const auto gort = d - ( dot( d, grad ) / gradSq ) * grad;
    const auto god = dot( gort, d );
    if ( god <= 0 )
        return {};
    const auto gob = -dot( gort, b );
    if ( gob <= 0 || gob >= god )
        return {};
    const auto a = gob / god;
    assert( a < std::numeric_limits<T>::max() );
    const auto ip = a * c + ( 1 - a ) * b;
    if ( dot( grad, ip ) >= 0 )
        return {}; 
    return a;
}
 
template <typename T>
[[nodiscard]] std::optional<Vector3<T>> tangentPlaneNormalToSpheres( const Vector3<T> & b, const Vector3<T> & c, T rb, T rc )
{
    auto grad = gradientInTri( b, c, rb, rc );
    if ( !grad.has_value() )
        return {}; 
    auto gradSq = grad->lengthSq();
    if ( gradSq >= 1 )
        return {}; 
    return sqrt( 1 - gradSq ) * normal( b, c ) - *grad; 
}
 
template <typename T>
[[nodiscard]] std::optional<Plane3<T>> tangentPlaneToSpheres( const Vector3<T> & a, const Vector3<T> & b, const Vector3<T> & c, T ra, T rb, T rc )
{
    if ( auto n = tangentPlaneNormalToSpheres( b - a, c - a, rb - ra, rc - ra ) )
        return Plane3<T>( *n, dot( *n, a ) + ra );
    return {}; 
}
 
}