C++ FVector::GetSafeNormal2D方法代码示例

void UVehiclePathFollowingComponent::FollowPathSegment(float DeltaTime)
{
    if (MovementComp)
    {
        UWheeledVehicleMovementComponent* VehicleMoveComp =
            Cast<UWheeledVehicleMovementComponent>(MovementComp);

        AWheeledVehicle* Owner = Cast<AWheeledVehicle>(VehicleMoveComp->GetOwner());

        if (Owner && VehicleMoveComp)
        {

            FVector VehicleLocation = Owner->GetActorLocation();
            FVector Destination = GetCurrentTargetLocation();
            FVector DirectionToDestion = Destination - VehicleLocation;

            float DistanceToDestination = DirectionToDestion.Size();

            FRotator RotatorToDestination = FRotationMatrix::MakeFromX(DirectionToDestion.GetSafeNormal2D()).Rotator();
            float DeltaYaw = (RotatorToDestination - Owner->GetActorRotation()).Yaw;
            bool DestinationInFront = DeltaYaw - 70.0f <= EPSILON && DeltaYaw + 70.0f >= EPSILON;

            float DesiredSteering = FMath::GetMappedRangeValueClamped(FVector2D(-180.0f, 180.0f), FVector2D(-1.0f, 1.0f), DeltaYaw);

            if (!DestinationInFront && DistanceToDestination < 1e3) // reverse
                DesiredSteering = -DesiredSteering;

            VehicleMoveComp->SetHandbrakeInput(false);

            // The DesiredSteering includes the sensed steering already
            CurrentSteering += UPIDController::NextValue(HeadingController, DesiredSteering - CurrentSteering, DeltaTime);
            VehicleMoveComp->SetSteeringInput(CurrentSteering);

            float PercentDistanceLeft = DistanceToDestination / InitialDistanceToDestination;

            float DesiredThrottle = 0.0f;

            // #TODO Turn hardcoded values into variables
            if (DestinationInFront)
                DesiredThrottle = FMath::Clamp(PercentDistanceLeft, 0.35f, 0.8f);
            else
                DesiredThrottle = -0.5f; // default throttle when going in reverse

            /* #TODO Fix Math:
                Allow user to specify a maximum vehicle speed that the PID
                controller should approach.
            */
            CurrentThrottle += UPIDController::NextValue(VelocityController, DesiredThrottle - CurrentThrottle, DeltaTime);

            VehicleMoveComp->SetThrottleInput(CurrentThrottle);

#ifdef NDEBUG
            DrawDebugPoint(GetWorld(), Destination, 50.0f, FColor::Blue);
            DrawDebugLine(GetWorld(), VehicleLocation, VehicleLocation + DirectionToDestion, FColor::Yellow);

            if (GEngine)
            {
                GEngine->AddOnScreenDebugMessage(0, 1.0f, FColor::Cyan, FString::Printf(TEXT("Throttle %0.3f Steering: %0.3f"), CurrentThrottle, CurrentSteering));
                GEngine->AddOnScreenDebugMessage(1, 1.0f, FColor::Cyan, FString::Printf(TEXT("Distance to destination: %0.2f"), DistanceToDestination));
            }
#endif

        }
    }
}

//RickH - We could probably significantly improve speed if we put separate Z checks in place and did everything else in 2D.
FVector UAvoidanceManager::GetAvoidanceVelocity_Internal(const FNavAvoidanceData& inAvoidanceData, float DeltaTime, int32* inIgnoreThisUID)
{
#if !(UE_BUILD_SHIPPING || UE_BUILD_TEST)
	if (!bSystemActive)
	{
		return inAvoidanceData.Velocity;
	}
#endif
	if (DeltaTime <= 0.0f)
	{
		return inAvoidanceData.Velocity;
	}

	FVector ReturnVelocity = inAvoidanceData.Velocity * DeltaTime;
	float MaxSpeed = ReturnVelocity.Size2D();
	float 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;
	}

	bool Unobstructed = true;
#if !(UE_BUILD_SHIPPING || UE_BUILD_TEST)
	bool DebugMode = IsDebugOnForAll() || (inIgnoreThisUID ? IsDebugOnForUID(*inIgnoreThisUID) : false);
#endif

	//If we're moving very slowly, just push forward. Not sure it's worth avoiding at this speed, though I could be wrong.
	if (MaxSpeed < 0.01f)
	{
		return inAvoidanceData.Velocity;
	}
	AllCones.Empty(AllCones.Max());

	//DrawDebugDirectionalArrow(GetWorld(), inAvoidanceData.Center, inAvoidanceData.Center + inAvoidanceData.Velocity, 2.5f, FColor(0,255,255), true, 0.05f, SDPG_MAX);

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

		//
		//Start with a few fast-rejects
		//

		//If the object has expired, ignore it
		if (OtherObject.ShouldBeIgnored())
		{
			continue;
		}

		//If other object is not in avoided group, ignore it
		if (inAvoidanceData.ShouldIgnoreGroup(OtherObject.GroupMask))
		{
			continue;
		}

		//RickH - We should have a max-radius parameter/option here, so I'm just going to hardcode one for now.
		//if ((OtherObject.Radius + _AvoidanceData.Radius + MaxSpeed + OtherObject.Velocity.Size2D()) < FVector::Dist(OtherObject.Center, _AvoidanceData.Center))
		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 we are moving away from the obstacle, ignore it. Even if we're the slower one, let the other obstacle path around us.
		if ((ReturnVelocity | (OtherObject.Center - inAvoidanceData.Center)) <= 0.0f)
		{
			continue;
		}

		//Create data for the avoidance routine
		{
			FVector PointAWorld = inAvoidanceData.Center;
			FVector PointBRelative = OtherObject.Center - PointAWorld;
			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;
//.........这里部分代码省略.........