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; } SidewaysFromB.Set(-TowardB.Y, TowardB.X, 0.0f); //Build collision cone (two planes) and store for later use. We might consider some fast rejection here to see if we can skip the cone entirely. //RickH - If we built these cones in 2D, we could avoid all the cross-product matrix stuff and just use (y, -x) 90-degree rotation. { FVector PointPlane[2]; FVector EffectiveVelocityB; FVelocityAvoidanceCone NewCone; //Use RVO (as opposed to VO) only for objects that are not overridden to max weight AND that are currently moving toward us. if ((OtherObject.OverrideWeightTime <= CurrentTime) && ((OtherObject.Velocity|PointBRelative) < 0.0f)) { float OtherWeight = (OtherObject.Weight + (1.0f - inAvoidanceData.Weight)) * 0.5f; //Use the average of what the other wants to be and what we want it to be. EffectiveVelocityB = ((inAvoidanceData.Velocity * (1.0f - OtherWeight)) + (OtherObject.Velocity * OtherWeight)) * DeltaTime; } else { EffectiveVelocityB = OtherObject.Velocity * DeltaTime; //This is equivalent to VO (not RVO) because the other object is not going to reciprocate our avoidance. } checkSlow(EffectiveVelocityB.Z == 0.0f); //Make the left plane 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 checkSlow((((PointBRelative+EffectiveVelocityB)|NewCone.ConePlane[0]) - NewCone.ConePlane[0].W) > 0.0f); #if !(UE_BUILD_SHIPPING || UE_BUILD_TEST) if (DebugMode) { // DrawDebugDirectionalArrow(MyWorld, EffectiveVelocityB + PointAWorld, PointPlane[0] + PointAWorld, 50.0f, FColor(64,64,64), true, 0.05f, SDPG_MAX); // DrawDebugLine(MyWorld, PointAWorld, EffectiveVelocityB + PointAWorld, FColor(64,64,64), true, 0.05f, SDPG_MAX, 5.0f); } #endif //Make the right plane 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 !(UE_BUILD_SHIPPING || UE_BUILD_TEST) if (DebugMode) { // DrawDebugDirectionalArrow(MyWorld, EffectiveVelocityB + PointAWorld, PointPlane[0] + PointAWorld, 50.0f, FColor(64,64,64), true, 0.05f, SDPG_MAX); } #endif 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); } //Find a good velocity that isn't inside a cone. if (AllCones.Num()) { float AngleCurrent; float AngleF = ReturnVelocity.HeadingAngle(); float BestScore = 0.0f; float 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. float 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()); float 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; #if !(UE_BUILD_SHIPPING || UE_BUILD_TEST) if (DebugMode) { DrawDebugDirectionalArrow(MyWorld, inAvoidanceData.Center + inAvoidanceData.Velocity, inAvoidanceData.Center + (ReturnVelocity / DeltaTime), 75.0f, FColor(64,255,64), true, 2.0f, SDPG_MAX); } #endif } return ReturnVelocity / DeltaTime; //Remove prediction-time scaling }