使用迭代逼近的方式求解线段与圆柱体的交点

需求来源:

UE4.15版本

我们的VR项目中,需要实现护盾的功能,玩家的左手柄方向的延长线与玩家周围的一个圆柱体相交的位置,即为护盾的位置,而护盾的方向,与圆柱体相切,如下图所示.

绿色为护盾,红色的为圆柱体,蓝色的线从左手柄射出.

玩家的左手一定会在圆柱体内,那么要解决的问题就变为,求解线段与圆柱体的交点.

如何使用迭代的方式求解呢,思路如下:

在玩家的左手柄方向的延长线上选择一点,此点的位置要在圆柱体外.设为PointOut.玩家左手柄位置为PointIn,求出两个点的中点:PointMiddle.那么交点一定位于[PointIn, PointMiddle] 或者[PointIn, PointOut]之间,以此类推,就可以不断逼近交点.

UCLASS()
class SHOOTERGAME_API UShooterBlueprintFunctionLibrary : public UBlueprintFunctionLibrary
{
    GENERATED_UCLASS_BODY()
    //求解一个点是否在圆柱体内
    //LowCircleCenter:圆柱体底面的圆心位置
    //HightCircleCenter:圆柱体的顶面的圆心位置
    //R:圆柱体的半径
    //TestPoint需要测试的点
    UFUNCTION(BlueprintPure, Category = "Math")
    static bool IsInCylinder(FVector LowCircleCenter, FVector HighCircleCenter, float R, FVector TestPoint);

    //线段与圆柱的交点 采用迭代逼近的方式
    //两个点必须一个在圆柱体内,一个在圆柱体外
    //LowCircleCenter:圆柱体底面的圆心位置
    //HightCircleCenter:圆柱体的顶面的圆心位置
    //R:圆柱体的半径
    //StartPoint:线段起始点
    //EndPoint:线段终点
    //Tolerance:迭代精度
    //HasIntersection:是否有交点
    //InSurface:交点的位置,在柱面上 0,在底面 -1,在顶面 1
    UFUNCTION(BlueprintPure, Category = "Math")
    static FVector LineAndCylinderIntersection(FVector LowCircleCenter, FVector HighCircleCenter, float R, FVector StartPoint, FVector EndPoint, float Tolerance, bool& HasIntersection, int32& InSurface);
}

 

//求解一个点是否在圆柱体内
//LowCircleCenter:圆柱体底面的圆心位置
//HightCircleCenter:圆柱体的顶面的圆心位置
//R:圆柱体的半径
//TestPoint需要测试的点

bool UShooterBlueprintFunctionLibrary::IsInCylinder(FVector LowCircleCenter, FVector HighCircleCenter, float R, FVector TestPoint)
{
    FVector MiddlePoint = (LowCircleCenter + HighCircleCenter) / 2;

    FVector CylinderDir = (HighCircleCenter - LowCircleCenter);

    float Height = CylinderDir.Size();

    CylinderDir.Normalize();

    FVector StartDir = TestPoint - MiddlePoint;

    float StartProj = FVector::DotProduct(StartDir, CylinderDir);
    //判断此点是否在圆柱体的高度范围内
    if (FMath::Abs(StartProj) > Height / 2)
    {
        return false;
    }
    
    //判断此点是否在圆柱体的横向距离内
    float TestDist = FMath::PointDistToLine(TestPoint, CylinderDir, MiddlePoint);

    return TestDist <= R;

}

//求解线段与圆柱体的交点,两个点必须一个在圆柱体内,一个在圆柱体外
//LowCircleCenter:圆柱体底面的圆心位置
//HightCircleCenter:圆柱体的顶面的圆心位置
//R:圆柱体的半径
//StartPoint:线段起始点
//EndPoint:线段终点
//Tolerance:迭代精度
//HasIntersection:是否有交点
//InSurface:交点的位置,在柱面上 0,在底面 -1,在顶面 1
FVector UShooterBlueprintFunctionLibrary::LineAndCylinderIntersection(FVector LowCircleCenter, FVector HighCircleCenter, float R, FVector StartPoint, FVector EndPoint, float Tolerance, bool& HasIntersection, int32& InSurface)
{
    Tolerance = Tolerance <= 0.1f ? 0.1f : Tolerance;
    //两个点必须一个在圆柱体内,一个在圆柱体外
    if (!(IsInCylinder(LowCircleCenter, HighCircleCenter, R, StartPoint) ^ IsInCylinder(LowCircleCenter, HighCircleCenter, R, EndPoint)))
    {
        HasIntersection = false;
        return FVector::ZeroVector;
    }

    HasIntersection = true;

    FVector Point1 = StartPoint;
    FVector Point2 = EndPoint;

    FVector MiddlePoint = (Point1 + Point2) / 2;

    float DiffSize = (Point1 - Point2).SizeSquared();

    int32 Times = 0;
    //迭代算法
    while (DiffSize > Tolerance)
    {
        //选择下一个迭代点
        if (IsInCylinder(LowCircleCenter, HighCircleCenter, R, Point1) ^ IsInCylinder(LowCircleCenter, HighCircleCenter, R, MiddlePoint))
        {
            Point2 = MiddlePoint;
        }
        else
        {
            Point1 = MiddlePoint;
        }

        MiddlePoint = (Point1 + Point2) / 2;

        DiffSize = (Point1 - Point2).SizeSquared();

        Times++;

        if (Times >= 40)
        {
            break;
        }
    }

    FVector CircleCenterMiddlePoint = (LowCircleCenter + HighCircleCenter) / 2;
    FVector CircleCenterDir = (HighCircleCenter - LowCircleCenter);
    CircleCenterDir.Normalize();

    float TestDist = FMath::PointDistToLine(MiddlePoint, CircleCenterDir, CircleCenterMiddlePoint);
    //交点在侧面,那么交点与圆柱体中心线的距离小于Tolerance
    if ((TestDist -  R) * (TestDist - R) <= Tolerance)
    {
        InSurface = 0;
    }
    else
    {
        //可以依据交点与中心点的方向来判断交点在底面还是顶面
        if (FVector::DotProduct(MiddlePoint - CircleCenterMiddlePoint, CircleCenterDir) >= 0.0f)
        {
            InSurface = 1;
        }
        else
        {
            InSurface = -1;
        }
    }

    return MiddlePoint;
}