Unreal Engine 的 RVO 系统

RVO 系统的接入

在unreal engine中也集成了一个简单版本的rvo， 类名为UAvoidanceManager，每个World都会创建一个。时机在initworld中，安排在物理与寻路的后面：

void UWorld::InitWorld(const InitializationValues IVS)
{
	if (!ensure(!bIsWorldInitialized))
	{
		return;
	}

	FCoreUObjectDelegates::GetPostGarbageCollect().AddUObject(this, &UWorld::OnPostGC);

	InitializeSubsystems();

	FWorldDelegates::OnPreWorldInitialization.Broadcast(this, IVS);

	AWorldSettings* WorldSettings = GetWorldSettings();
	if (IVS.bInitializeScenes)
	{

	#if WITH_EDITOR
		bEnableTraceCollision = IVS.bEnableTraceCollision;
		bForceUseMovementComponentInNonGameWorld = IVS.bForceUseMovementComponentInNonGameWorld;
	#endif


		if (IVS.bCreatePhysicsScene)
		{
			// Create the physics scene
			CreatePhysicsScene(WorldSettings);
		}

		bShouldSimulatePhysics = IVS.bShouldSimulatePhysics;
		
		// Save off the value used to create the scene, so this UWorld can recreate its scene later
		bRequiresHitProxies = IVS.bRequiresHitProxies;
		GetRendererModule().AllocateScene(this, bRequiresHitProxies, IVS.bCreateFXSystem, GetFeatureLevel());
	}

	// Prepare AI systems
	if (WorldSettings)
	{
		if (IVS.bCreateNavigation || IVS.bCreateAISystem)
		{
			if (IVS.bCreateNavigation)
			{
				FNavigationSystem::AddNavigationSystemToWorld(*this, FNavigationSystemRunMode::InvalidMode, WorldSettings->GetNavigationSystemConfig(), /*bInitializeForWorld=*/false);
			}
			if (IVS.bCreateAISystem && WorldSettings->IsAISystemEnabled())
			{
				CreateAISystem();
			}
		}
	}
	
	if (GEngine->AvoidanceManagerClass != NULL)
	{
		AvoidanceManager = NewObject<UAvoidanceManager>(this, GEngine->AvoidanceManagerClass);
	}
}

这里的AvoidanceManagerClass是根据配置的类路径来创建的：

	/** Sets the AvoidanceManager class, which can be overridden to change AI crowd behavior. */
	UPROPERTY(globalconfig, noclear, meta=(MetaClass="/Script/Engine.AvoidanceManager", DisplayName="Avoidance Manager Class"))
	FSoftClassPath AvoidanceManagerClassName;
	
	UPROPERTY()
	TSubclassOf<class UAvoidanceManager>  AvoidanceManagerClass;

Engine在启动的时候会加载这个class:

	if (AvoidanceManagerClassName.IsValid())
	{
		LoadEngineClass<UAvoidanceManager>(AvoidanceManagerClassName, AvoidanceManagerClass);
	}

在BaseEngine.ini里这个路径填的就是AvoidanceManager，当然我们可以在工程目录下的DefaultEngine.ini里覆盖:

AvoidanceManagerClassName=/Script/Engine.AvoidanceManager

在CharacterMovementComponent上有一个开关选项,来配置是否使用RVO来驱动碰撞避免：

/** If set, component will use RVO avoidance. This only runs on the server. */
UPROPERTY(Category="Character Movement: Avoidance", EditAnywhere, BlueprintReadOnly)
uint8 bUseRVOAvoidance:1;

在开启了RVO支持之后，就会向AvoidanceManager里注册当前CharacterMovementComponent:

void UCharacterMovementComponent::SetUpdatedComponent(USceneComponent* NewUpdatedComponent)
{
	// 省略其他代码
	if (bUseRVOAvoidance && IsValid(NewUpdatedComponent))
	{
		UAvoidanceManager* AvoidanceManager = GetWorld()->GetAvoidanceManager();
		if (AvoidanceManager)
		{
			AvoidanceManager->RegisterMovementComponent(this, AvoidanceWeight);
		}
	}
}

这个RVO的介入也可以在运行时开关，暴露了SetAvoidanceEnabled接口，来触发RegisterMovementComponent或者RemoveAvoidanceObject:

void UCharacterMovementComponent::SetAvoidanceEnabled(bool bEnable)
{
	if (bUseRVOAvoidance != bEnable)
	{
		bUseRVOAvoidance = bEnable;

		const int32 OldAvoidanceUID = AvoidanceUID;
		AvoidanceUID = 0;

		// this is a safety check - it's possible to not have CharacterOwner at this point if this function gets
		// called too early
		ensure(GetCharacterOwner());
		if (GetCharacterOwner() != nullptr)
		{
			UWorld* World = GetWorld();
			UAvoidanceManager* AvoidanceManager = World ? World->GetAvoidanceManager() : nullptr;

			if (AvoidanceManager)
			{
				if (bEnable)
				{
					AvoidanceManager->RegisterMovementComponent(this, AvoidanceWeight);
				}
				else if (!AvoidanceManager->IsAutoPurgeEnabled())
				{
					// When disabling avoidance the object needs to be removed immediately if the manager isn't set to auto purge. 
					// Otherwise we would leak the object since there isn't anything left to remove it.
					AvoidanceManager->RemoveAvoidanceObject(OldAvoidanceUID);
				}
			}
		}
	}
}

避让速度计算

在接入了RVO之后，在更新移动速度的最后面，会调用到CalcAvoidanceVelocity:

void UCharacterMovementComponent::CalcVelocity(float DeltaTime, float Friction, bool bFluid, float BrakingDeceleration)
{
	// 省略所有的速度计算相关代码
	// 最后的两行是通过rvo去调整
	if (bUseRVOAvoidance)
	{
		CalcAvoidanceVelocity(DeltaTime);
	}
}

CalcAvoidanceVelocity里的逻辑就是调用AvoidanceManager->GetAvoidanceVelocityForComponent这个接口来计算避障速度，

void UCharacterMovementComponent::CalcAvoidanceVelocity(float DeltaTime)
{
	SCOPE_CYCLE_COUNTER(STAT_AI_ObstacleAvoidance);

	UAvoidanceManager* AvoidanceManager = GetWorld()->GetAvoidanceManager();
	if (AvoidanceWeight >= 1.0f || AvoidanceManager == NULL || GetCharacterOwner() == NULL)
	{
		return;
	}

	if (GetCharacterOwner()->GetLocalRole() != ROLE_Authority)
	{
		return;
	}
	//Adjust velocity only if we're in "Walking" mode. We should also check if we're dazed, being knocked around, maybe off-navmesh, etc.
	UCapsuleComponent *OurCapsule = GetCharacterOwner()->GetCapsuleComponent();
	if (!Velocity.IsZero() && IsMovingOnGround() && OurCapsule)
	{
		//See if we're doing a locked avoidance move already, and if so, skip the testing and just do the move.
		if (AvoidanceLockTimer > 0.0f)
		{
			Velocity = AvoidanceLockVelocity;
		}
		else
		{
			FVector NewVelocity = AvoidanceManager->GetAvoidanceVelocityForComponent(this);
			if (bUseRVOPostProcess)
			{
				PostProcessAvoidanceVelocity(NewVelocity);
			}

			if (!NewVelocity.Equals(Velocity))		//Really want to branch hint that this will probably not pass
			{
				//Had to divert course, lock this avoidance move in for a short time. This will make us a VO, so unlocked others will know to avoid us.
				Velocity = NewVelocity;
				SetAvoidanceVelocityLock(AvoidanceManager, AvoidanceManager->LockTimeAfterAvoid);
			}
			else
			{
				//Although we didn't divert course, our velocity for this frame is decided. We will not reciprocate anything further, so treat as a VO for the remainder of this frame.
				SetAvoidanceVelocityLock(AvoidanceManager, AvoidanceManager->LockTimeAfterClean);	//10 ms of lock time should be adequate.
			}
		}
		//RickH - We might do better to do this later in our update
		AvoidanceManager->UpdateRVO(this);

		bWasAvoidanceUpdated = true;
	}
}

计算完成之后，再调用AvoidanceManager->SetAvoidanceVelocityLock接口将当前角色的速度脱离RVO的控制一个很短的时间，这样可以避免移动速度的震荡，这两个时间窗口常量都在UAvoidanceManager里初始化的：

UAvoidanceManager::UAvoidanceManager(const FObjectInitializer& ObjectInitializer) : Super(ObjectInitializer)
{
	DefaultTimeToLive = 1.5f;
	LockTimeAfterAvoid = 0.2f; // 每0.2s才更新一次rvo的速度
	LockTimeAfterClean = 0.001f; // 注释里说这个应该是10ms 但是目前是1ms 其实意义差不多 就是为了让当前帧的速度不再被rvo影响
	// 省略后续代码
}

GetAvoidanceVelocityForComponent只是一个简单的转发函数，真正计算速度的函数为GetAvoidanceVelocity_Internal：

FVector UAvoidanceManager::GetAvoidanceVelocityForComponent(UCharacterMovementComponent* MovementComp)
{
	check(MovementComp);
	FNavAvoidanceData AvoidanceData(this, MovementComp);
	return GetAvoidanceVelocityIgnoringUID(AvoidanceData, DeltaTimeToPredict, MovementComp->GetRVOAvoidanceUIDFast());
}
FVector UAvoidanceManager::GetAvoidanceVelocityIgnoringUID(const FNavAvoidanceData& inAvoidanceData, float DeltaTime, int32 inIgnoreThisUID)
{
	return GetAvoidanceVelocity_Internal(inAvoidanceData, DeltaTime, &inIgnoreThisUID);
}

这个函数比较长，这里将拆成多个部分来分段介绍。首先是初始化一些变量，这里的MaxSpeed是当前帧内这个角色能移动的最大距离，

FVector ReturnVelocity = inAvoidanceData.Velocity * DeltaTime;
FVector::FReal MaxSpeed = ReturnVelocity.Size2D();
double CurrentTime;

UWorld* MyWorld = Cast<UWorld>(GetOuter());
if (MyWorld)
{
	CurrentTime = MyWorld->TimeSeconds;
}
else
{
	//No world? OK, just quietly back out and don't alter anything.
	return inAvoidanceData.Velocity;
}

然后就开始收集当前角色的速度障碍物VelocityObstacle,每个速度障碍物都是一个锥形区域，由两个平面来定义:

template<typename T>
struct alignas(16) TPlane
	: public TVector<T>
{
public:
	using FReal = T;
	using TVector<T>::X;
	using TVector<T>::Y;
	using TVector<T>::Z;

	/** The w-component. */
	T W;
};
struct FVelocityAvoidanceCone
{
	FPlane ConePlane[2];			//Left and right cone planes - these should point in toward each other. Technically, this is a convex hull, it's just unbounded.
};

结果都会放到AllCones中：

bool Unobstructed = true;
AllCones.Empty(AllCones.Max());


for (auto& AvoidanceObj : AvoidanceObjects)
{
	// 过滤掉一些不参与VO的

	// 然后再生成VO 添加到AllCones数组中
	
}

在遍历所有可能的速度障碍物的时候，首先有一些过滤条件：

if ((inIgnoreThisUID) && (*inIgnoreThisUID == AvoidanceObj.Key))
{
	continue;
}
FNavAvoidanceData& OtherObject = AvoidanceObj.Value;

// 就是之前SetAvoidanceVelocityLock里设置的锁定时间
if (OtherObject.ShouldBeIgnored())
{
	continue;
}

// 根据rvo碰撞组去过滤掉一些不需要结算的 
if (inAvoidanceData.ShouldIgnoreGroup(OtherObject.GroupMask))
{
	continue;
}

// 两者距离大于碰撞半径的也放弃
if (FVector2D(OtherObject.Center - inAvoidanceData.Center).SizeSquared() > FMath::Square(inAvoidanceData.TestRadius2D))
{
	continue;
}
// 高度差大于两者高度之和的也过滤掉
if (FMath::Abs(OtherObject.Center.Z - inAvoidanceData.Center.Z) > OtherObject.HalfHeight + inAvoidanceData.HalfHeight + HeightCheckMargin)
{
	continue;
}

//当前速度方向正在远离的也过滤掉 完全不考虑对方正在加速过来的问题
if ((ReturnVelocity | (OtherObject.Center - inAvoidanceData.Center)) <= 0.0f)
{
	continue;
}

过滤条件都通过之后，才开始创建VO:

//Create data for the avoidance routine
{
	FVector PointAWorld = inAvoidanceData.Center;
	FVector PointBRelative = OtherObject.Center - PointAWorld; // 向量AB
	FVector TowardB, SidewaysFromB;
	FVector VelAdjustment;
	FVector VelAfterAdjustment;
	float RadiusB = OtherObject.Radius + inAvoidanceData.Radius; // 

	PointBRelative.Z = 0.0f;
	TowardB = PointBRelative.GetSafeNormal2D();		//Don't care about height for this game. Rough height-checking will come in later, but even then it will be acceptable to do this.
	if (TowardB.IsZero())
	{
		//Already intersecting, or aligned vertically, scrap this whole object.
		continue;
	}
	SidewaysFromB.Set(-TowardB.Y, TowardB.X, 0.0f); // 向量AB逆时针旋转90度

	{
		FVector PointPlane[2];
		FVector EffectiveVelocityB;
		FVelocityAvoidanceCone NewCone;

		// 这里的OverrideWeightTime 代表这个时间之前认为此对象质量无穷大 
		// velocity投影为负代表角色B正在冲向角色A
		if ((OtherObject.OverrideWeightTime <= CurrentTime) && ((OtherObject.Velocity|PointBRelative) < 0.0f))
		{// 这个分支计算的是RVO 这里考虑了各自的重量 默认重量是1 所以默认算出来的是0.5
			float OtherWeight = (OtherObject.Weight + (1.0f - inAvoidanceData.Weight)) * 0.5f;			
			// 两者速度执行加权 代表对象B在本帧内的期望移动距离
			EffectiveVelocityB = ((inAvoidanceData.Velocity * (1.0f - OtherWeight)) + (OtherObject.Velocity * OtherWeight)) * DeltaTime;
		}
		else
		{
			// 这个是完全不避让的情况下 B在本帧内的移动距离 退化为简单VO
			EffectiveVelocityB = OtherObject.Velocity * DeltaTime;		
		}
		checkSlow(EffectiveVelocityB.Z == 0.0f);

		// 现在开始构建VO 圆心为 EffectiveVelocityB + PointBRelative 相当于本帧过后B相对于A的位置 然后半径为RadiusB
		//先构建左侧的边界 ，然后加上远离AB连线上的分量 这里的P1只是用来保证平面与XY平面垂直
		PointPlane[0] = EffectiveVelocityB + (PointBRelative + (SidewaysFromB * RadiusB));
		PointPlane[1].Set(PointPlane[0].X, PointPlane[0].Y, PointPlane[0].Z + 100.0f);
		NewCone.ConePlane[0] = FPlane(EffectiveVelocityB, PointPlane[0], PointPlane[1]);		//First point is relative to A, which is ZeroVector in this implementation

		// 检查B的后续位置在VO内
		checkSlow((((PointBRelative+EffectiveVelocityB)|NewCone.ConePlane[0]) - NewCone.ConePlane[0].W) > 0.0f);

		//做右侧的边界
		PointPlane[0] = EffectiveVelocityB + (PointBRelative - (SidewaysFromB * RadiusB));
		PointPlane[1].Set(PointPlane[0].X, PointPlane[0].Y, PointPlane[0].Z - 100.0f);
		NewCone.ConePlane[1] = FPlane(EffectiveVelocityB, PointPlane[0], PointPlane[1]);		//First point is relative to A, which is ZeroVector in this implementation
		checkSlow((((PointBRelative+EffectiveVelocityB)|NewCone.ConePlane[1]) - NewCone.ConePlane[1].W) > 0.0f);

		// 如果当前速度在两个半平面交内 则代表会发生碰撞 
		if ((((ReturnVelocity|NewCone.ConePlane[0]) - NewCone.ConePlane[0].W) > 0.0f)
			&& (((ReturnVelocity|NewCone.ConePlane[1]) - NewCone.ConePlane[1].W) > 0.0f))
		{
			Unobstructed = false;
		}

		AllCones.Add(NewCone);
	}
}

如果之前算出来的所有角色都不会生成速度障碍物，则当前移动速度是一个合法速度，直接返回：

if (Unobstructed)
{
	//Trivial case, our ideal velocity is available.
	return inAvoidanceData.Velocity;
}

这里就开始考虑调整速度了，此时需要将周围的阻挡边加入进来：

TArray<FNavEdgeSegment> NavEdges;
if (EdgeProviderOb.IsValid())
{
	DECLARE_SCOPE_CYCLE_COUNTER(TEXT("Avoidance: collect nav edges"), STAT_AIAvoidanceEdgeCollect, STATGROUP_AI);
	EdgeProviderInterface->GetEdges(inAvoidanceData.Center, inAvoidanceData.TestRadius2D, NavEdges);
}

这里并不是使用线性规划的方法去生成最优速度，而是构造八个可选速度，从中获取评分最高的，这八个可选速度的与当前速度的夹角分别是23，40，55，85，分布在当前速度的左右两侧：

FVector::FReal AngleCurrent;
FVector::FReal AngleF = ReturnVelocity.HeadingAngle();
FVector::FReal BestScore = 0.0f;
FVector::FReal BestScorePotential;
FVector BestVelocity = FVector::ZeroVector;		//Worst case is we just stand completely still. Should we also allow backing up? Should we test standing still?
const int AngleCount = 4;		//Every angle will be tested both right and left.
FVector::FReal AngleOffset[AngleCount] = {FMath::DegreesToRadians<float>(23.0f), FMath::DegreesToRadians<float>(40.0f), FMath::DegreesToRadians<float>(55.0f), FMath::DegreesToRadians<float>(85.0f)};
FVector AngleVector[AngleCount<<1];

//Determine check angles
for (int i = 0; i < AngleCount; ++i)
{
	AngleCurrent = AngleF - AngleOffset[i];
	AngleVector[(i<<1)].Set(FMath::Cos(AngleCurrent), FMath::Sin(AngleCurrent), 0.0f);
	AngleCurrent = AngleF + AngleOffset[i];
	AngleVector[(i<<1) + 1].Set(FMath::Cos(AngleCurrent), FMath::Sin(AngleCurrent), 0.0f);
}

然后遍历这八个速度，开始计算得分：

//Sample velocity-space destination points and drag them back to form lines
for (int AngleToCheck = 0; AngleToCheck < (AngleCount<<1); ++AngleToCheck)
{
	FVector VelSpacePoint = AngleVector[AngleToCheck] * MaxSpeed;

	//Skip testing if we know we can't possibly get a better score than what we have already.
	//Note: This assumes the furthest point is the highest-scoring value (i.e. VelSpacePoint is not moving backward relative to ReturnVelocity)
	BestScorePotential = (VelSpacePoint|ReturnVelocity) * (VelSpacePoint|VelSpacePoint);
	if (BestScorePotential > BestScore)
	{
		const bool bAvoidsNavEdges = NavEdges.Num() > 0 ? AvoidsNavEdges(inAvoidanceData.Center, VelSpacePoint, NavEdges, inAvoidanceData.HalfHeight) : true;
		if (bAvoidsNavEdges)
		{
			FVector CandidateVelocity = AvoidCones(AllCones, FVector::ZeroVector, VelSpacePoint, AllCones.Num());
			FVector::FReal CandidateScore = (CandidateVelocity|ReturnVelocity) * (CandidateVelocity|CandidateVelocity);

			//Vectors are rated by their length and their overall forward movement.
			if (CandidateScore > BestScore)
			{
				BestScore = CandidateScore;
				BestVelocity = CandidateVelocity;
			}
		}
	}
}
ReturnVelocity = BestVelocity;

这里为了减少复杂的运算，会预先评估当前速度方向上可达到的最大评分，基本就是当前方向在基础速度方向上的投影。如果这个评分大于了之前算的最大评分，才执行后续的计算。

这里的AvoidsNavEdges实现比较简单，就是判断在这个方向下移动是否会与阻挡边相交，相交的话返回false，否则返回true：

static bool AvoidsNavEdges(const FVector& OrgLocation, const FVector& TestVelocity, const TArray<FNavEdgeSegment>& NavEdges, float MaxZDiff)
{
	DECLARE_SCOPE_CYCLE_COUNTER(TEXT("Avoidance: avoid nav edges"), STAT_AIAvoidanceEdgeAvoid, STATGROUP_AI);

	for (int32 Idx = 0; Idx < NavEdges.Num(); Idx++)
	{
		const FVector2D Seg0ToSeg1(NavEdges[Idx].P1 - NavEdges[Idx].P0);
		const FVector2D OrgToNewPos(TestVelocity);
		const FVector2D OrgToSeg0(NavEdges[Idx].P0 - OrgLocation);
		const FVector2D::FReal CrossD = FVector2D::CrossProduct(Seg0ToSeg1, OrgToNewPos);
		if (FMath::Abs(CrossD) < UE_KINDA_SMALL_NUMBER)
		{
			continue;
		}

		const FVector2D::FReal CrossS = FVector2D::CrossProduct(OrgToNewPos, OrgToSeg0) / CrossD;
		const FVector2D::FReal CrossT = FVector2D::CrossProduct(Seg0ToSeg1, OrgToSeg0) / CrossD;
		if (CrossS < 0.0f || CrossS > 1.0f || CrossT < 0.0f || CrossT > 1.0f)
		{
			continue;
		}

		const FVector CrossPt = FMath::Lerp(NavEdges[Idx].P0, NavEdges[Idx].P1, CrossS);
		const FVector2D::FReal ZDiff = FMath::Abs(OrgLocation.Z - CrossPt.Z);
		if (ZDiff > MaxZDiff)
		{
			continue;
		}

		return false;
	}
	
	return true;
}

这里的CrossS的意思代表这个方向的射线在目标阻挡线段上的直线的相交点与阻挡线段的向量比值，小于0或大于1都代表不会与阻挡线段相交。同时CrossT的意思是当前方向上的移动多远才会与阻挡直线相交，小于0或大于1都代表不会与阻挡直线相交。最后还要检查一下相交点的高度差，如果差值小于最大高度差，则认为也不会相交。

而AvoidCones就复杂了一些，除了判断相交之外还负责将这个方向上的速度执行收缩：

// 这里的BasePosition永远是速度0
FVector AvoidCones(TArray<FVelocityAvoidanceCone>& AllCones, const FVector& BasePosition, const FVector& DesiredPosition, const int NumConesToTest)
{
	FVector CurrentPosition = DesiredPosition;
	FVector::FReal DistanceInsidePlane_Current[2];
	FVector::FReal DistanceInsidePlane_Base[2];
	FVector::FReal Weighting[2];
	int ConePlaneIndex;

	//AllCones is non-const so that it can be reordered, but nothing should be added or removed from it.
	checkSlow(NumConesToTest <= AllCones.Num());
	TArray<FVelocityAvoidanceCone>::TIterator It = AllCones.CreateIterator();
	for (int i = 0; i < NumConesToTest; ++i, ++It)
	{
		FVelocityAvoidanceCone& CurrentCone = *It;

		//See if CurrentPosition is outside this cone. If it is, then this cone doesn't obstruct us.
		DistanceInsidePlane_Current[0] = (CurrentPosition|CurrentCone.ConePlane[0]) - CurrentCone.ConePlane[0].W;
		DistanceInsidePlane_Current[1] = (CurrentPosition|CurrentCone.ConePlane[1]) - CurrentCone.ConePlane[1].W;
		if ((DistanceInsidePlane_Current[0] <= 0.0f) || (DistanceInsidePlane_Current[1] <= 0.0f))
		{
			// 任意一个值小于零代表这个速度下不会进入当前VO
			continue;
		}

		//If we've gotten here, CurrentPosition is inside the cone. If BasePosition is also inside the cone, this entire segment is blocked.
		DistanceInsidePlane_Base[0] = (BasePosition|CurrentCone.ConePlane[0]) - CurrentCone.ConePlane[0].W;
		DistanceInsidePlane_Base[1] = (BasePosition|CurrentCone.ConePlane[1]) - CurrentCone.ConePlane[1].W;

		// 这里开始计算 当前速度方向与这两个阻挡边界的最近相交点
#define CALCULATE_WEIGHTING(index) Weighting[index] = -DistanceInsidePlane_Base[index] / (DistanceInsidePlane_Current[index] - DistanceInsidePlane_Base[index]);
		if (DistanceInsidePlane_Base[0] <= 0.0f)
		{
			CALCULATE_WEIGHTING(0);
			if (DistanceInsidePlane_Base[1] <= 0.0f)
			{
				CALCULATE_WEIGHTING(1);
				ConePlaneIndex = (Weighting[1] > Weighting[0]) ? 1 : 0;
			}
			else
			{
				ConePlaneIndex = 0;
			}
		}
		else if (DistanceInsidePlane_Base[1] <= 0.0f)
		{
			CALCULATE_WEIGHTING(1);
			ConePlaneIndex = 1;
		}
		else
		{
			// 如果基础速度也在vo内 没有抢救方法了 放弃后续计算  直接返回速度0
			return BasePosition;
		}
		// 计算出相交点 这个速度下就不会与当前VO相交了
		CurrentPosition = (CurrentPosition * Weighting[ConePlaneIndex]) + (BasePosition * (1.0f - Weighting[ConePlaneIndex]));
#undef CALCULATE_WEIGHTING

		// 这个VO不再需要去处理 扔到最后
		AllCones.Swap(i, NumConesToTest - 1);		
		// 然后重新开始一轮计算
		return AvoidCones(AllCones, BasePosition, CurrentPosition, NumConesToTest - 1);
	}

	return CurrentPosition;
}