void UAStarAgentComponent::DrawDebugInformation()
{
if (m_path->path.Num() == 0)
return;
for (int i = 0; i < m_path->path.Num() - 1; i++)
{
FVector pos1 = m_path->path[i];
FVector pos2 = m_path->path[i + 1];
DrawDebugPoint(
GetWorld(),
FVector(pos1.X, pos1.Y, pos1.Z),
7,
FColor(255, 255, 0)
);
DrawDebugLine(GetWorld(),
FVector(pos1.X, pos1.Y, pos1.Z), FVector(pos2.X, pos2.Y, pos2.Z),
FColor(255, 255, 0), false, -1, 0, 5.f
);
}
DrawDebugPoint(
GetWorld(),
FVector(m_path->path.Last().X, m_path->path.Last().Y, m_path->path.Last().Z),
7,
FColor(255, 255, 0)
);
}
static void VisualizeGazePoint(bool bVisualizeDetection, UWorld *World, const FVector& Point)
{
if (bVisualizeDetection)
{
DrawDebugPoint(World, Point, 3, FColor::Red);
}
}
static void VisualizeHitTestPoint(bool bVisualizeDetection, UWorld *World, const FVector& Point)
{
if (bVisualizeDetection)
{
DrawDebugPoint(World, Point, 3, FColor::Green);
}
}
FVector UTKMathFunctionLibrary::GetVelocityAtPoint(UPrimitiveComponent* Target, FVector Point, FName BoneName, bool DrawDebugInfo)
{
//FTransform Transform = Target->GetComponentTransform();
//FVector LocalLinearVelocity = Transform.InverseTransformVectorNoScale(Target->GetPhysicsLinearVelocity());
//FVector LocalAngularVelocity = Transform.InverseTransformVectorNoScale(Target->GetPhysicsAngularVelocity());
//FVector ResultPointVelocity = LocalLinearVelocity + FVector::CrossProduct(FVector::DegreesToRadians(LocalAngularVelocity), Transform.InverseTransformVectorNoScale(Point - Target->GetCenterOfMass()));
if (!Target) return FVector::ZeroVector;
//You can actually get it from the physx body instance instead.
FBodyInstance* BI = Target->GetBodyInstance(BoneName);
if (BI && BI->IsValidBodyInstance())
{
FVector PointVelocity = BI->GetUnrealWorldVelocityAtPoint(Point);
UWorld* TheWorld = Target->GetWorld();
if (DrawDebugInfo && TheWorld)
{
FColor DefaultColor(255,200,0);
DrawDebugPoint(TheWorld, Point, 10, DefaultColor);
DrawDebugString(TheWorld, Point, FString::SanitizeFloat(PointVelocity.Size()), NULL, FColor::White, 0.0f);
}
return PointVelocity;
}
return FVector::ZeroVector;
}
/// <summary> Draws one of the paths using one list from each list of lists </summary>
/// <param name="n"> The position, in each list of lists, of the list with the values for this path </param>
void UMovementTracker::DrawNthPath(size_t n)
{
//Checks if there is no mistake in the different lists
if (VectorList[n].Num() == TimeList[n].Num() && TimeList[n].Num() == NameList[n].Num())
{
//previous tells us if the previous point was drawn. It allows us to know if a line needs to be drawn between the points
//but also tells us when the path's beginning was. It allows us to start at that point next update to save on resources.
bool previous = false;
for (size_t i = PathStartArray[n]; i < VectorList[n].Num(); i++)
{
if ((TimeLength <= 0 || TimeList[n][i] >= ElapsedTime - TimeLength) && TimeList[n][i] <= ElapsedTime && NameList[n][i] == GetOwner()->GetName())
{
DrawDebugPoint(World, VectorList[n][i], 20.0f, FColor(255, 0, 0));
if (previous) //Draw the line between the previous point and this one
DrawDebugLine(World, VectorList[n][i - 1], VectorList[n][i], FColor(0, 0, 255), false, -1.0f, (uint8)'\000', 5.0f);
else //Sets the start of next update's sweep
PathStartArray[n] = i;
previous = true;
}
else
{
previous = false;
if (previous) //We are at the end of the path that we want to draw, no need to continue checking the rest of the path
break;
}
}
}
}
void AOctreeSearch::DrawOctreeBoxes(Octree* Oct)
{
if (Oct == NULL) { return; }
if (Oct->IsLeafNode() && Oct->GetParticle()) {
if(ShowOctree) DrawDebugBox(GetWorld(), Oct->GetOrigin(), FVector(Oct->GetSize(), Oct->GetSize(), Oct->GetSize()), FColor::Red, true);
DrawDebugPoint(GetWorld(), Oct->GetParticle()->Position, 10.0, FColor::Black, true);
} else {
for (int32 i = 0; i < 8; i++) { DrawOctreeBoxes(Oct->GetChild(i)); }
}
}
void ADEPRECATED_VolumeAdaptiveBuilder::VisualizeNAVResultRealTime(const TArray<FVector>& PathSolution, FVector Source, FVector Destination, bool Reset, bool DrawDebugVolumes, FColor LineColor, float LineThickness)
{
if (Reset)
{
i_viz = 0;
EntryPoint__ = Source;
FlushPersistentDebugLines(GetWorld());
DrawDebugPoint(GetWorld(), Source, 10.f, FColor::Blue, true);
return;
}
if (i_viz > PathSolution.Num() - 1)
return;
PreviousEntryPoint__ = EntryPoint__;
EntryPoint__ = PathSolution[i_viz];
DrawDebugLine(GetWorld(), PreviousEntryPoint__, EntryPoint__, LineColor, true, -1.f, 5.f, LineThickness);
DrawDebugPoint(GetWorld(), EntryPoint__, 10.f, FColor::Blue, true);
return;
}
void ADEPRECATED_VolumeAdaptiveBuilder::VisualizeNAVResult(const TArray<FVector>& PathSolution, FVector Source, FVector Destination, bool Reset, bool DrawDebugVolumes, FColor LineColor, float LineThickness)
{
FVector entryPoint = Source;
FVector previousEntryPoint;
for (int32 i = 0; i < PathSolution.Num(); i++)
{
previousEntryPoint = entryPoint;
entryPoint = PathSolution[i];
DrawDebugLine(GetWorld(), previousEntryPoint, entryPoint, LineColor, true, -1.f, 5.f, LineThickness);
DrawDebugPoint(GetWorld(), entryPoint, 10.f, FColor::Blue, true);
}
}
FVector ADEPRECATED_VolumeAdaptiveBuilder::NavigaitonEntryPointFromVector(FVector Origin, UDoNNavigationVolumeComponent* CurrentVolume, UDoNNavigationVolumeComponent* DestinationVolume, bool &VolumeTraversalImminent, bool &overlapsX, bool &overlapsY, bool &overlapsZ, bool DrawDebug)
{
// Debug info:
if (DrawDebug)
{
CurrentVolume->SetVisibility(true);
CurrentVolume->SetHiddenInGame(false);
DestinationVolume->SetVisibility(true);
DestinationVolume->SetHiddenInGame(false);
DrawDebugPoint(GetWorld(), DestinationVolume->Bounds.GetBoxExtrema(0), 10.f, FColor::Blue, true);
DrawDebugPoint(GetWorld(), DestinationVolume->Bounds.GetBoxExtrema(1), 10.f, FColor::Red, true);
}
overlapsX = PointOverlapsVolumeAxis('X', Origin, DestinationVolume);
overlapsY = PointOverlapsVolumeAxis('Y', Origin, DestinationVolume);
overlapsZ = PointOverlapsVolumeAxis('Z', Origin, DestinationVolume);
VolumeTraversalImminent = (overlapsX && overlapsY) || (overlapsY && overlapsZ) || (overlapsZ && overlapsX);
float destinationX = NearestAxisPoint('X', Origin.X, CurrentVolume, DestinationVolume, (!VolumeTraversalImminent && !overlapsX));
float destinationY = NearestAxisPoint('Y', Origin.Y, CurrentVolume, DestinationVolume, (!VolumeTraversalImminent && !overlapsY));
float destinationZ = NearestAxisPoint('Z', Origin.Z, CurrentVolume, DestinationVolume, (!VolumeTraversalImminent && !overlapsZ));
return FVector(destinationX, destinationY, destinationZ);
}
TFunction<void()> AARCharacter::SpatialComplete()
{
//DrawBox()
auto func = [&]()
{
auto func2 = [&]()
{
for (const FVector& Pos : Result.Get().Data)
{
DrawDebugPoint(GetWorld(), Pos, 20, FColor::Red, true, 10);
}
};
AsyncTask(ENamedThreads::GameThread, func2);
};
return func;
}
static bool Intersect(const PawnPath &Lhs, const PawnPath &Rhs, UWorld* EXTRA_LOG_ONLY(World))
{
EXTRA_LOG_ONLY(Lhs.DrawDebugArrow(World));
EXTRA_LOG_ONLY(Rhs.DrawDebugArrow(World));
FVector IntersectionPoint;
const bool bIntersect = FMath::SegmentIntersection2D(
Lhs.Start, Lhs.End,
Rhs.Start, Rhs.End,
IntersectionPoint);
#ifdef CARLA_AI_WALKERS_EXTRA_LOG
if (bIntersect) {
DrawDebugPoint(World, GetPointForDrawing(IntersectionPoint), 10.0f, FColor::Red, false, 2.0f);
}
#endif // CARLA_AI_WALKERS_EXTRA_LOG
return bIntersect;
}
// =============================================================================
// CWindow::DrawShape
// Draw a shape with any requested debug info
// -----------------------------------------------------------------------------
void CWindow::DrawShape(CConvexShape theShape)
{
draw(theShape);
// Draw debug info
DRAW_BOUNDS(theShape);
DRAW_ORIGIN(theShape);
#if TGL_DEBUG
// Draw each point of the shape
if (DebugOptions::drawShapePoints)
{
for (int i = 0; i < theShape.getPointCount(); i++)
{
DrawDebugPoint(theShape.GetGlobalPoint(i));
}
}
// Draw the normal to all lines
if (DebugOptions::drawShapeNormals)
{
std::list<CLine> theLines = theShape.GetGlobalLines();
FOR_EACH_IN_LIST(CLine, theLines)
{
CVector2f normal = (*it).GetNormal();
CVector2f start = (*it).GetMidpoint();
CVector2f end = start + 5.0f * normal;
sf::Vertex line[] =
{
sf::Vertex(start, CColour::Red),
sf::Vertex(end, CColour::Red)
};
draw(line, 2, sf::Lines);
}
}
void FPhATEdPreviewViewportClient::SimMouseMove(float DeltaX, float DeltaY)
{
DragX = Viewport->GetMouseX() - SharedData->LastClickPos.X;
DragY = Viewport->GetMouseY() - SharedData->LastClickPos.Y;
if (!SharedData->MouseHandle->GrabbedComponent)
{
return;
}
//We need to convert Pixel Delta into Screen position (deal with different viewport sizes)
FSceneViewFamilyContext ViewFamily(FSceneViewFamily::ConstructionValues( Viewport, GetScene(), EngineShowFlags ));
FSceneView* View = CalcSceneView(&ViewFamily);
FVector4 ScreenOldPos = View->PixelToScreen(SharedData->LastClickPos.X, SharedData->LastClickPos.Y, 1.f);
FVector4 ScreenNewPos = View->PixelToScreen(DragX + SharedData->LastClickPos.X, DragY + SharedData->LastClickPos.Y, 1.f);
FVector4 ScreenDelta = ScreenNewPos - ScreenOldPos;
FVector4 ProjectedDelta = View->ScreenToWorld(ScreenDelta);
FVector4 WorldDelta;
//Now we project new ScreenPos to xy-plane of SimGrabLocation
FVector LocalOffset = View->ViewMatrices.ViewMatrix.TransformPosition(SimGrabLocation + SimGrabZ * SimGrabPush);
float ZDistance = GetViewportType() == ELevelViewportType::LVT_Perspective ? fabs(LocalOffset.Z) : 1.f; //in the ortho case we don't need to do any fixup because there is no perspective
WorldDelta = ProjectedDelta * ZDistance;
//Now we convert back into WorldPos
FVector WorldPos = SimGrabLocation + WorldDelta + SimGrabZ * SimGrabPush;
FVector NewLocation = WorldPos;
float QuickRadius = 5 - SimGrabPush / SimHoldDistanceChangeDelta;
QuickRadius = QuickRadius < 2 ? 2 : QuickRadius;
DrawDebugPoint(GetWorld(), NewLocation, QuickRadius, FColorList::Red, false, 0.3);
SharedData->MouseHandle->SetTargetLocation(NewLocation);
SharedData->MouseHandle->GrabbedComponent->WakeRigidBody(SharedData->MouseHandle->GrabbedBoneName);
}
void UVehiclePathFollowingComponent::UpdatePathSegment()
{
#ifdef NDEBUG
if (Path.IsValid())
{
for (int i = 0; i < Path->GetPathPoints().Num(); ++i)
{
DrawDebugPoint(GetWorld(), Path->GetPathPoints()[i].Location, 40.0f, FColor::Yellow, false, 0.3f);
}
}
#endif
float DistanceLeft = (
*Path->GetPathPointLocation(Path->GetPathPoints().Num() - 1)
- MovementComp->GetActorLocation()
).Size();
if (DistanceLeft < AcceptanceRadius)
{
OnSegmentFinished();
OnPathFinished(EPathFollowingResult::Success);
}
UPathFollowingComponent::UpdatePathSegment();
}
void UVehiclePathFollowingComponent::FollowPathSegment(float DeltaTime)
{
if (MovementComp)
{
UWheeledVehicleMovementComponent* VehicleMoveComp =
Cast<UWheeledVehicleMovementComponent>(MovementComp);
AWheeledVehicle* Owner = Cast<AWheeledVehicle>(VehicleMoveComp->GetOwner());
if (Owner && VehicleMoveComp)
{
if(DesiredVehicleSpeedInKMH == 0.0f)
VehicleMoveComp->SetHandbrakeInput(true);
else
VehicleMoveComp->SetHandbrakeInput(false);
FVector VehicleLocation = Owner->GetActorLocation();
FVector Destination = GetCurrentTargetLocation();
FVector DirectionToDestination = Destination - VehicleLocation; //Vector pointing to current destination (not necessarily the final destination)
//____STEERING____
float DesiredSteering = 0.f;
float DeltaYaw = (DirectionToDestination.ToOrientationRotator() - Owner->GetActorForwardVector().ToOrientationRotator()).Yaw;
bool DestinationToRight = DeltaYaw >= 0;
if (DeltaYaw > 180.f || DeltaYaw < -180.f) {
DestinationToRight = !DestinationToRight;
if (DestinationToRight && DeltaYaw >= -360.f + MaxSteeringAngle)
{
DesiredSteering = 1.f;
}
else if (!DestinationToRight && DeltaYaw <= 360.f - MaxSteeringAngle)
{
DesiredSteering = -1.f;
}
else if(DestinationToRight)
{
DesiredSteering = FMath::GetMappedRangeValueClamped(FVector2D(-360.f, -360.f + MaxSteeringAngle), FVector2D(0, 1.0f), DeltaYaw);
}
else
{
DesiredSteering = FMath::GetMappedRangeValueClamped(FVector2D(360.f - MaxSteeringAngle, 360.f), FVector2D(-1.f, 0.f), DeltaYaw);
}
}
else
{
if (DestinationToRight && DeltaYaw >= MaxSteeringAngle)
{
DesiredSteering = 1.f;
}
else if (!DestinationToRight && DeltaYaw <= -MaxSteeringAngle)
{
DesiredSteering = -1.f;
}
else
DesiredSteering = FMath::GetMappedRangeValueClamped(FVector2D(-MaxSteeringAngle, MaxSteeringAngle), FVector2D(-1.f, 1.f), DeltaYaw);
}
CurrentSteering = DesiredSteering;
VehicleMoveComp->SetSteeringInput(CurrentSteering);
//____END STEERING____
//____THROTTLE____
float CurrentSpeedInKMH = VehicleMoveComp->GetForwardSpeed() * 0.036f; //Convert to KM/H
//TODO: Allow speed to be 0, but never 0 if DesiredSpeed is > 0
float EstiamtedDesiredSpeed = DesiredVehicleSpeedInKMH - (FMath::Abs(CurrentSteering) * DesiredVehicleSpeedInKMH * 0.8);
CurrentThrottle += UPIDController::NextValue(VelocityController, EstiamtedDesiredSpeed - CurrentSpeedInKMH, DeltaTime);
CurrentThrottle = FMath::Clamp(CurrentThrottle, 0.f, 1.f);
VehicleMoveComp->SetThrottleInput(CurrentThrottle);
//____END THROTTLE____
//Direction the wheel is facing
DesiredMoveDirection = Owner->GetActorForwardVector().ToOrientationRotator().Add(0.f, FMath::GetMappedRangeValueClamped(FVector2D(-1.f, 1.f), FVector2D(-90.f, 90.f), CurrentSteering), 0.f).Vector();
ABikeV3Pawn* BikePawn = Cast<ABikeV3Pawn>(Owner);
if(BikePawn)
BikePawn->CurrentHandlingInput = CurrentSteering;
#ifdef NDEBUG
float DistanceToDestination = DirectionToDestination.Size();
DrawDebugPoint(GetWorld(), Destination, 50.0f, FColor::Blue);
DrawDebugLine(GetWorld(), VehicleLocation, VehicleLocation + DirectionToDestination, 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));
GEngine->AddOnScreenDebugMessage(2, 1.0f, FColor::Cyan, FString::Printf(TEXT("Current Speed in KM/H: %0.2f"), CurrentSpeedInKMH));
GEngine->AddOnScreenDebugMessage(3, 1.0f, FColor::Cyan, FString::Printf(TEXT("DeltaYaw %0.3f"), DeltaYaw));
}
#endif
}
}
}
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
}
}
}
void ADEPRECATED_VolumeAdaptiveBuilder::BuildNAVNetwork()
{
NavGraph.nodes = TMap<UDoNNavigationVolumeComponent*, TArray<UDoNNavigationVolumeComponent*>>();
for (auto Iter(NAVVolumeComponents.CreateIterator()); Iter; Iter++)
{
if (!Iter)
continue;
UDoNNavigationVolumeComponent* volume = (*Iter);
TArray<UPrimitiveComponent*> neighboringVolumeComponents;
volume->UpdateBounds(); // GetBoxExtrema functions return outdated values unless this is called after a volume has grown to desired size
volume->CanNavigate = true;
volume->ShapeColor = FColor::Green;
FVector originalBoxExtent = volume->GetUnscaledBoxExtent();
volume->SetBoxExtent(FVector(originalBoxExtent.X + XBaseUnit / 2, originalBoxExtent.Y + YBaseUnit / 2, originalBoxExtent.Z + ZBaseUnit/2));
UKismetSystemLibrary::ComponentOverlapComponents_NEW(volume, volume->GetComponentTransform(), NAVOverlapQuery, UDoNNavigationVolumeComponent::StaticClass(), ActorsToIgnoreForCollision, neighboringVolumeComponents);
volume->SetBoxExtent(originalBoxExtent);
neighboringVolumeComponents.Remove(volume);
volume->UpdateBounds();
TArray<UDoNNavigationVolumeComponent*> neighbors;
for (UPrimitiveComponent* neighbor : neighboringVolumeComponents)
{
neighbor->UpdateBounds();
bool overlapsX = VolumesOverlapAxis2(volume->Bounds.GetBoxExtrema(0).X, volume->Bounds.GetBoxExtrema(1).X, neighbor->Bounds.GetBoxExtrema(0).X, neighbor->Bounds.GetBoxExtrema(1).X);
bool overlapsY = VolumesOverlapAxis2(volume->Bounds.GetBoxExtrema(0).Y, volume->Bounds.GetBoxExtrema(1).Y, neighbor->Bounds.GetBoxExtrema(0).Y, neighbor->Bounds.GetBoxExtrema(1).Y);
bool overlapsZ = VolumesOverlapAxis2(volume->Bounds.GetBoxExtrema(0).Z, volume->Bounds.GetBoxExtrema(1).Z, neighbor->Bounds.GetBoxExtrema(0).Z, neighbor->Bounds.GetBoxExtrema(1).Z);
bool pathExists = (overlapsX && overlapsY) || (overlapsY && overlapsZ) || (overlapsZ && overlapsX);
if (!pathExists)
continue;
neighbors.Add(Cast<UDoNNavigationVolumeComponent>(neighbor));
if (DisplayNAVNeighborGraph)
{
DrawDebugPoint(GetWorld(), volume->GetComponentLocation(), 10.f, FColor::Blue, true);
DrawDebugLine(GetWorld(), volume->GetComponentLocation(), neighbor->GetComponentLocation(), FColor::Red, true, -1.f, 0, 4.f);
/*if (volume->X == 88 && volume->Y == 169 && volume->Z == 7)
{
volume->ShapeColor = FColor::Red;
Cast<UDoNNavigationVolumeComponent>(neighbor)->ShapeColor = FColor::Blue;
}*/
volume->SetVisibility(true);
neighbor->SetVisibility(true);
}
}
NavGraph.nodes.Add((*Iter), neighbors);
// [Old code] Special workaround for persisting UObject volumes onto a UMAP. Not applicable for pixel builders ATM
FNAVMapContainer NAVMapContainer;
NAVMapContainer.volume = volume;
NAVMapContainer.neighbors = neighbors;
NavGraph_GCSafe.Add(NAVMapContainer);
}
}
