Geometry.h

Go to the documentation of this file.

// Copyright Epic Games, Inc. All Rights Reserved.
#pragma once
 
#include "Core/Types.h"
#include "Geo/GeoEnum.h"
#include "Math/Aabb.h"
#include "Math/MathConst.h"
#include "Math/MatrixH.h"
#include "Math/Point.h"
 
namespace UE::CADKernel
{
enum EPolygonSide : uint8
{
    Side01 = 0,
    Side12,
    Side20,
    Side23,
    Side30,
};
 
namespace IntersectionTool
{
CADKERNEL_API void SetTolerance(const double Tolerance);
}
 
 
CADKERNEL_API inline FVector2d ComputeCircumCircleCenter(const FVector2d& InPoint0, const FVector2d& InPoint1, const FVector2d& InPoint2)
{
    FVector2d Segment_P0_P1 = InPoint1 - InPoint0;
    FVector2d Segment_P0_P2 = InPoint2 - InPoint0;
 
    // D = 2(BuCv - BvCu)
    double D = 2. * Segment_P0_P1 ^ Segment_P0_P2;
    if (FMath::IsNearlyZero(D, SMALL_NUMBER_SQUARE))
    {
        return FVector2d::ZeroVector;
    }
 
    // CenterU  = 1/D * (Cv.|B|.|B| - By.|C|.|C|) = 1/D * CBv ^ SquareNorms 
    // CenterV  = 1/D * (Bu.|B|.|B| - Cu.|C|.|C|) = -1/D * SquareNorms ^ CBu
    double SquareNormB = Segment_P0_P1.SquaredLength();
    double SquareNormC = Segment_P0_P2.SquaredLength();
    double CenterU = (SquareNormB * Segment_P0_P2.Y - SquareNormC * Segment_P0_P1.Y) / D;
    double CenterV = (SquareNormC * Segment_P0_P1.X - SquareNormB * Segment_P0_P2.X) / D;
 
    return FVector2d(CenterU, CenterV) + InPoint0;
}
 
CADKERNEL_API inline FVector ComputeCircumCircleCenter(const FVector& Point0, const FVector& Point1, const FVector& Point2)
{
    FVector Test = Point1 - Point0;
    FVector AxisZ = (Point1 - Point0) ^ (Point2 - Point0);
    AxisZ.Normalize();
 
    FVector AxisX = Point1 - Point0;
    AxisX.Normalize();
    FVector AxisY = AxisZ ^ AxisX;
 
    FMatrixH Matrix(Point0, AxisX, AxisY, AxisZ);
    FMatrixH MatrixInverse = Matrix;
    MatrixInverse.Inverse();
 
    FVector2d D2Point1 = FVectorUtil::FromVector(MatrixInverse * Point1);
    FVector2d D2Point2 = FVectorUtil::FromVector(MatrixInverse * Point2);
 
    double D = 2. * D2Point1 ^ D2Point2;
    if (FMath::IsNearlyZero(D, SMALL_NUMBER_SQUARE))
    {
        return FVector::ZeroVector;
    }
 
    double SquareNormB = D2Point1.SquaredLength();
    double SquareNormC = D2Point2.SquaredLength();
 
    double CenterU = (SquareNormB * D2Point2.Y - SquareNormC * D2Point1.Y) / D;
    double CenterV = (SquareNormC * D2Point1.X - SquareNormB * D2Point2.X) / D;
 
    FVector Center2D(CenterU, CenterV, 0);
    return Matrix * Center2D;
}
 
 
CADKERNEL_API inline FVector2d ComputeCircumCircleCenterAndSquareRadius(const FVector2d& InPoint0, const FVector2d& InPoint1, const FVector2d& InPoint2, double& OutSquareRadius)
{
    FVector2d Segment_P0_P1 = InPoint1 - InPoint0;
    FVector2d Segment_P0_P2 = InPoint2 - InPoint0;
 
    double D = 2. * Segment_P0_P1 ^ Segment_P0_P2;
    if (FMath::IsNearlyZero(D, SMALL_NUMBER_SQUARE))
    {
        OutSquareRadius = 0;
        return FVector2d::ZeroVector;
    }
 
    double SquareNormB = Segment_P0_P1.SquaredLength();
    double SquareNormC = Segment_P0_P2.SquaredLength();
    double CenterU = (SquareNormB * Segment_P0_P2.Y - SquareNormC * Segment_P0_P1.Y) / D;
    double CenterV = (SquareNormC * Segment_P0_P1.X - SquareNormB * Segment_P0_P2.X) / D;
 
    FVector2d Center(CenterU, CenterV);
    OutSquareRadius = Center.SquaredLength();
 
    return Center + InPoint0;
}
 
template<class PointType>
inline PointType ProjectPointOnSegment(const PointType& Point, const PointType& InSegmentA, const PointType& InSegmentB, double& OutCoordinate, bool bRestrictCoodinateToInside = true)
{
    PointType Segment = InSegmentB - InSegmentA;
 
    double SquaredLength = Segment | Segment;
 
    if (SquaredLength <= 0.0)
    {
        OutCoordinate = 0.0;
        return InSegmentA;
    }
    else
    {
        PointType APoint = Point - InSegmentA;
        OutCoordinate = (APoint | Segment) / SquaredLength;
 
        if (bRestrictCoodinateToInside)
        {
            if (OutCoordinate < 0.0)
            {
                OutCoordinate = 0.0;
                return InSegmentA;
            }
 
            if (OutCoordinate > 1.0)
            {
                OutCoordinate = 1.0;
                return InSegmentB;
            }
        }
 
        PointType ProjectedPoint = Segment * OutCoordinate;
        ProjectedPoint += InSegmentA;
        return ProjectedPoint;
    }
}
 
inline FVector ProjectPointOnPlane(const FVector& Point, const FVector& Origin, const FVector& InNormal, double& OutDistance)
{
    FVector Normal = InNormal;
    ensureCADKernel(!FMath::IsNearlyZero(Normal.Length()));
    Normal.Normalize();
 
    FVector OP = Point - Origin;
    OutDistance = OP | Normal;
 
    return Point - (Normal * OutDistance);
}
 
template<class PointType>
inline PointType PointOnSegment(const PointType& InSegmentA, const PointType& InSegmentB, double InCoordinate)
{
    PointType Segment = InSegmentB - InSegmentA;
    return InSegmentA + Segment * InCoordinate;
}
 
template<class PointType>
inline double DistanceOfPointToSegment(const PointType& Point, const PointType& SegmentPoint1, const PointType& SegmentPoint2)
{
    double Coordinate;
    return PointType::Distance(ProjectPointOnSegment<PointType>(Point, SegmentPoint1, SegmentPoint2, Coordinate, /*bRestrictCoodinateToInside*/ true), Point);
}
 
template<class PointType>
inline double SquareDistanceOfPointToSegment(const PointType& Point, const PointType& SegmentPoint1, const PointType& SegmentPoint2)
{
    double Coordinate;
    return FVector2d::DistSquared(ProjectPointOnSegment<PointType>(Point, SegmentPoint1, SegmentPoint2, Coordinate, /*bRestrictCoodinateToInside*/ true), Point);
}
 
template<class PointType>
inline double DistanceOfPointToLine(const PointType& Point, const PointType& LinePoint1, const PointType& LineDirection)
{
    double Coordinate;
    return ProjectPointOnSegment<PointType>(Point, LinePoint1, LinePoint1 + LineDirection, Coordinate, /*bRestrictCoodinateToInside*/ false).Distance(Point);
}
 
CADKERNEL_API double ComputeCurvature(const FVector& Gradient, const FVector& Laplacian);
CADKERNEL_API double ComputeCurvature(const FVector& normal, const FVector& Gradient, const FVector& Laplacian);
 
template<class PointType>
inline double CoordinateOfProjectedPointOnSegment(const PointType& Point, const PointType& InSegmentA, const PointType& InSegmentB, bool bRestrictCoodinateToInside = true)
{
    PointType Segment = InSegmentB - InSegmentA;
 
    double SquaredLength = Segment | Segment;
 
    if (SquaredLength <= 0.0)
    {
        return 0.0;
    }
    else
    {
        PointType APoint = Point - InSegmentA;
        double Coordinate = (APoint | Segment) / SquaredLength;
 
        if (bRestrictCoodinateToInside)
        {
            if (Coordinate < 0.0)
            {
                return 0.0;
            }
 
            if (Coordinate > 1.0)
            {
                return 1.0;
            }
        }
 
        return Coordinate;
    }
}
 
CADKERNEL_API void FindLoopIntersectionsWithIso(const EIso Iso, const double IsoParameter, const TArray<TArray<FVector2d>>& Loops, TArray<double>& OutIntersections);
 
template<class PointType>
struct CADKERNEL_API TSegment
{
    const PointType& Point0;
    const PointType& Point1;
    const PointType Dummy = PointType::ZeroVector;
 
    TSegment(const PointType& InPoint0, const PointType& InPoint1)
        : Point0(InPoint0)
        , Point1(InPoint1)
    {
    }
 
    constexpr const PointType& operator[](int32 Index) const
    {
        ensureCADKernel(Index < 2);
        switch (Index)
        {
        case 0:
            return Point0;
        case 1:
            return Point1;
        default:
            return Dummy;
        }
    }
 
    double SquaredLength() const
    {
        return FVector2d::DistSquared(Point0, Point1);
    }
 
    PointType GetVector() const
    {
        return Point1 - Point0;
    }
};
 
using FSegment2D = TSegment<FVector2d>;
using FSegment3D = TSegment<FVector>;
 
template<class PointType>
struct CADKERNEL_API TTriangle
{
    const PointType& Point0;
    const PointType& Point1;
    const PointType& Point2;
 
    TTriangle(const PointType& InPoint0, const PointType& InPoint1, const PointType& InPoint2)
        : Point0(InPoint0)
        , Point1(InPoint1)
        , Point2(InPoint2)
    {
    }
 
    constexpr const PointType& operator[](int32 Index) const
    {
        ensureCADKernel(Index < 3);
        switch (Index)
        {
        case 0:
            return Point0;
        case 1:
            return Point1;
        case 2:
            return Point2;
        default:
            return PointType::ZeroVector;
        }
    }
 
    virtual inline PointType ProjectPoint(const PointType& InPoint, FVector2d& OutCoordinate)
    {
        PointType Segment01 = Point1 - Point0;
        PointType Segment02 = Point2 - Point0;
        double SquareLength01 = Segment01.SquaredLength();
        double SquareLength02 = Segment02.SquaredLength();
        double Seg01Seg02 = Segment01 | Segment02;
        double Det = SquareLength01 * SquareLength02 - FMath::Square(Seg01Seg02);
 
        int32 SideIndex;
        // If the 3 points are aligned
        if (FMath::IsNearlyZero(Det))
        {
            double MaxLength = SquareLength01;
            SideIndex = Side01;
            if (SquareLength02 > MaxLength)
            {
                MaxLength = SquareLength02;
                SideIndex = Side20;
            }
 
            PointType Segment12 = Point2 - Point1;
            if (Segment12.SquaredLength() > MaxLength)
            {
                SideIndex = Side12;
            }
        }
        else
        {
            // Resolve
            PointType Segment1Point = InPoint - Point0;
            double Segment1PointSegment01 = Segment1Point | Segment01;
            double Segment1PointSegment02 = Segment1Point | Segment02;
 
            OutCoordinate.X = ((Segment1PointSegment01 * SquareLength02) - (Segment1PointSegment02 * Seg01Seg02)) / Det;
            OutCoordinate.Y = ((Segment1PointSegment02 * SquareLength01) - (Segment1PointSegment02 * Seg01Seg02)) / Det;
 
            // tester la solution pour choisir parmi 4 possibilites
            if (OutCoordinate.X < 0.0)
            {
                // the project point is on the segment 02
                SideIndex = Side20;
            }
            else if (OutCoordinate.Y < 0.0)
            {
                // the project point is on the segment 01
                SideIndex = Side01;
            }
            else if ((OutCoordinate.X + OutCoordinate.Y) > 1.0)
            {
                // the project point is on the segment 12
                SideIndex = Side12;
            }
            else {
                // the project point is inside the Segment
                Segment01 = Segment01 * OutCoordinate.X;
                Segment02 = Segment02 * OutCoordinate.Y;
                PointType ProjectedPoint = Segment01 + Segment02;
                ProjectedPoint = ProjectedPoint + Point0;
                return ProjectedPoint;
            }
        }
 
        // projects the point on the nearest side
        PointType ProjectedPoint;
        double SegmentCoordinate;
        switch (SideIndex)
        {
        case Side01:
            ProjectedPoint = ProjectPointOnSegment<PointType>(InPoint, Point0, Point1, SegmentCoordinate);
            OutCoordinate.X = SegmentCoordinate;
            OutCoordinate.Y = 0.0;
            break;
        case Side20:
            ProjectedPoint = ProjectPointOnSegment<PointType>(InPoint, Point0, Point2, SegmentCoordinate);
            OutCoordinate.X = 0.0;
            OutCoordinate.Y = SegmentCoordinate;
            break;
        case Side12:
            ProjectedPoint = ProjectPointOnSegment<PointType>(InPoint, Point1, Point2, SegmentCoordinate);
            OutCoordinate.X = 1.0 - SegmentCoordinate;
            OutCoordinate.Y = SegmentCoordinate;
            break;
        }
        return ProjectedPoint;
    }
 
    virtual PointType CircumCircleCenter() const
    {
        return ComputeCircumCircleCenter(Point0, Point1, Point2);
    }
};
 
struct CADKERNEL_API FTriangle : public TTriangle<FVector>
{
    FTriangle(const FVector& InPoint0, const FVector& InPoint1, const FVector& InPoint2)
        : TTriangle<FVector>(InPoint0, InPoint1, InPoint2)
    {
    }
 
    virtual FVector ComputeNormal() const
    {
        FVector Normal = (Point1 - Point0) ^ (Point2 - Point0);
        Normal.Normalize();
        return Normal;
    }
};
 
struct CADKERNEL_API FTriangle2D : public TTriangle<FVector2d>
{
    FTriangle2D(const FVector2d& InPoint0, const FVector2d& InPoint1, const FVector2d& InPoint2)
        : TTriangle<FVector2d>(InPoint0, InPoint1, InPoint2)
    {
    }
 
    FVector2d CircumCircleCenterWithSquareRadius(double& SquareRadius) const
    {
        return ComputeCircumCircleCenterAndSquareRadius(this->Point0, this->Point1, this->Point2, SquareRadius);
    }
};
 
inline FVector2d FindIntersectionOfSegments2D(const FSegment2D& SegmentAB, const FSegment2D& SegmentCD, double& OutABIntersectionCoordinate)
{
    const FVector2d AB = SegmentAB[1] - SegmentAB[0];
    const FVector2d DC = SegmentCD[0] - SegmentCD[1];
    const FVector2d AC = SegmentCD[0] - SegmentAB[0];
 
    double ParallelCoef = DC ^ AB;
    if (FMath::IsNearlyZero(ParallelCoef))
    {
        const double SquareAB = AB | AB;
        double CCoordinate = (AB | AC) / SquareAB;
 
        const FVector2d AD = SegmentCD[1] - SegmentAB[0];
        double DCoordinate = (AB | AD) / SquareAB;
 
        if (CCoordinate >= -DOUBLE_KINDA_SMALL_NUMBER && CCoordinate <= 1 + DOUBLE_KINDA_SMALL_NUMBER)
        {
            if (DCoordinate >= -DOUBLE_KINDA_SMALL_NUMBER && DCoordinate <= 1 + DOUBLE_KINDA_SMALL_NUMBER)
            {
                OutABIntersectionCoordinate = (DCoordinate + CCoordinate) * 0.5;
                return (SegmentCD[0] + SegmentCD[1]) * 0.5;
            }
 
            CCoordinate = FMath::Clamp(CCoordinate, 0., 1.);
            OutABIntersectionCoordinate = CCoordinate;
            return SegmentCD[0];
        }
        else if (DCoordinate >= -DOUBLE_KINDA_SMALL_NUMBER && DCoordinate <= 1 + DOUBLE_KINDA_SMALL_NUMBER)
        {
            DCoordinate = FMath::Clamp(DCoordinate, 0., 1.);
            OutABIntersectionCoordinate = DCoordinate;
            return SegmentCD[1];
        }
        else
        {
            OutABIntersectionCoordinate = 0.5;
            return (SegmentAB[0] + SegmentAB[1]) * 0.5;
        }
    }
 
    OutABIntersectionCoordinate = (DC ^ AC) / ParallelCoef;
    OutABIntersectionCoordinate = FMath::Clamp(OutABIntersectionCoordinate, 0., 1.);
 
    return SegmentAB[0] + AB * OutABIntersectionCoordinate;
}
 
inline FVector2d FindIntersectionOfSegments2D(const FSegment2D& SegmentAB, const FSegment2D& SegmentCD)
{
    double ABIntersectionCoordinate;
    return FindIntersectionOfSegments2D(SegmentAB, SegmentCD, ABIntersectionCoordinate);
}
 
inline bool FindIntersectionOfLines2D(const FSegment2D& LineAB, const FSegment2D& LineCD, FVector2d& OutIntersectionPoint)
{
    constexpr const double Min = -DOUBLE_SMALL_NUMBER;
    constexpr const double Max = 1. + DOUBLE_SMALL_NUMBER;
 
    const FVector2d AB = LineAB[1] - LineAB[0];
    const FVector2d DC = LineCD[0] - LineCD[1];
    const FVector2d AC = LineCD[0] - LineAB[0];
 
    double ParallelCoef = DC ^ AB;
    if (FMath::IsNearlyZero(ParallelCoef))
    {
        return false;
    }
 
    double OutABIntersectionCoordinate = (DC ^ AC) / ParallelCoef;
    OutIntersectionPoint = LineAB[0] + AB * OutABIntersectionCoordinate;
    return true;
}
 
CADKERNEL_API bool DoIntersect(const FSegment2D& SegmentAB, const FSegment2D& SegmentCD);
CADKERNEL_API bool DoIntersectInside(const FSegment2D& SegmentAB, const FSegment2D& SegmentCD);
 
inline bool AreParallel(const FSegment2D& SegmentAB, const FSegment2D& SegmentCD)
{
    const FVector2d AB = SegmentAB.GetVector().GetSafeNormal();
    const FVector2d CD = SegmentCD.GetVector().GetSafeNormal();
    const double ParallelCoef = AB ^ CD;
    return (FMath::IsNearlyZero(ParallelCoef, DOUBLE_KINDA_SMALL_NUMBER));
};
 
} // namespace UE::CADKernel