void UCheatManager::TestCollisionDistance()
{
	APlayerController* PC = GetOuterAPlayerController();
	if(PC)
	{
		// Get view location to act as start point
		FVector ViewLoc;
		FRotator ViewRot;
		PC->GetPlayerViewPoint(ViewLoc, ViewRot);

		FlushPersistentDebugLines( GetOuterAPlayerController()->GetWorld() );//change the GetWorld

		// calculate from viewloc
		for (FObjectIterator Iter(AVolume::StaticClass()); Iter; ++Iter)
		{
			AVolume * Volume = Cast<AVolume>(*Iter);

			if (Volume->GetClass()->GetDefaultObject() != Volume)
			{
				FVector ClosestPoint(0,0,0);
				float Distance = Volume->GetBrushComponent()->GetDistanceToCollision(ViewLoc, ClosestPoint);
				float NormalizedDistance = FMath::Clamp<float>(Distance, 0.f, 1000.f)/1000.f;
				FColor DrawColor(255*NormalizedDistance, 255*(1-NormalizedDistance), 0);
				DrawDebugLine(GetWorld(), ViewLoc, ClosestPoint, DrawColor, true);

				UE_LOG(LogCheatManager, Log, TEXT("Distance to (%s) is %0.2f"), *Volume->GetName(), Distance);
			}
		}
	}
}
void ADEPRECATED_VolumeAdaptiveBuilder::GetShortestPathToDestination_DebugRealtime(bool Reset, FVector origin, FVector destination, UDoNNavigationVolumeComponent* originVolume, UDoNNavigationVolumeComponent* destinationVolume, bool DrawDebug, TArray<FVector> &PathSolutionRaw, TArray<FVector> &PathSolutionOptimized, bool DrawDebugVolumes)
{
	if (!originVolume || !destinationVolume)
		return;

	if (goalFound__)
	{
		UE_LOG(LogTemp, Log, TEXT("Goal already reached."));
		return;
	}	

	if (Reset)
	{	
		goalFound__ = false;
		frontier__.put(originVolume, 0);
		//came_fron__.Add(originVolume, originVolume);
		VolumeVsCostMap__.Add(originVolume, 0);
		entryPointMap__.Add(originVolume, origin);
		neighborId__ = 0;
		neighbor__ = NULL;		
	}	

	if (!frontier__.empty())
	{	
		if (neighborId__ == 0)
		{
			current__ = frontier__.get();
			if (DrawDebug)
			{
				FlushPersistentDebugLines(GetWorld());
				//DrawDebugLine(GetWorld(), *entryPointMap__.Find(current__), *entryPointMap__.Find(*came_fron__.Find(current__)), FColor::Green, true, -1.f, 0, 10.f);
			}
			
		}
			
		
		if (NavGraph.neighbors(current__).Num() == 0)
			return;
		
		HideDebugVolume(neighbor__, DrawDebug);

		neighbor__ = NavGraph.neighbors(current__)[neighborId__];

		ExpandFrontierTowardsTarget(current__, neighbor__, frontier__, entryPointMap__, goalFound__, originVolume, destinationVolume, origin, destination, VolumeVsCostMap__, DrawDebug, PathVolumeSolutionMap__);

		if (neighborId__ == NavGraph.neighbors(current__).Num() - 1)
			neighborId__ = 0;
		else
		{
			neighborId__++;
			HideDebugVolume(current__, DrawDebug);
		}
			
	}
}
void ADEPRECATED_VolumeAdaptiveBuilder::CleanUp()
{
	for (UDoNNavigationVolumeComponent* volume : NAVVolumeComponents)
		if (volume)
			volume->CreationMethod = EComponentCreationMethod::UserConstructionScript;
			//volume->bCreatedByConstructionScript_DEPRECATED = true;

	DestroyConstructedComponents();
	NAVVolumeComponents.Empty();

	FlushPersistentDebugLines(GetWorld());
}
// Called every frame
void AOctreeSearch::Tick( float DeltaTime )
{
	Super::Tick( DeltaTime );
  FlushPersistentDebugLines(GetWorld());
  if (PhDeltaTime > 0) {
    ComputeCubeSize();
    CreateOctree();
    for (int32 i = 0; i < Particles.Num(); i++) {
      Particles[i].Velocity += PhDeltaTime*Particles[i].Acceleration;
      Particles[i].Position += PhDeltaTime*Particles[i].Velocity;
    }
  }
  DrawOctreeBoxes(ParticleOctree);
}
void ADEPRECATED_VolumeAdaptiveBuilder::PostEditChangeProperty(FPropertyChangedEvent& PropertyChangedEvent)
{
	Super::PostEditChangeProperty(PropertyChangedEvent);

	UProperty* PropertyThatChanged = PropertyChangedEvent.Property;
	FName PropertyName = PropertyThatChanged != NULL ? PropertyThatChanged->GetFName() : NAME_None;	

	if (PropertyName == GET_MEMBER_NAME_CHECKED(ADEPRECATED_VolumeAdaptiveBuilder, IsVisibleNavigableVolumes) ||
		PropertyName == GET_MEMBER_NAME_CHECKED(ADEPRECATED_VolumeAdaptiveBuilder, IsVisibleBlockedVolumes) ||
		PropertyName == GET_MEMBER_NAME_CHECKED(ADEPRECATED_VolumeAdaptiveBuilder, IsVisibleInGame)
		)
	{
		for (UDoNNavigationVolumeComponent* volume : NAVVolumeComponents)
		{
			if (!volume)
				continue;

			if (volume->CanNavigate)
				volume->SetVisibility(IsVisibleNavigableVolumes);
			else
				volume->SetVisibility(IsVisibleBlockedVolumes);

			volume->SetHiddenInGame(!IsVisibleInGame);
		}
	}

	if (GenerateNavigationVolumes)
	{
		ADEPRECATED_VolumeAdaptiveBuilder::ConstructBuilder();
		GenerateNavigationVolumes = false;
	}	
	
	if (RegenerateNAVNetwork || PropertyName == GET_MEMBER_NAME_CHECKED(ADEPRECATED_VolumeAdaptiveBuilder, DisplayNAVNeighborGraph))
	{
		FlushPersistentDebugLines(GetWorld());		
		BuildNAVNetwork();
		RegenerateNAVNetwork = false;
	}

	if (CleanUpAllData)
	{
		CleanUp();
		CleanUpAllData = false;
	}

	
}
void UNavigationComponent::DrawDebugCurrentPath(UCanvas* Canvas, FColor PathColor, const uint32 NextPathPointIndex)
{
	if (MyNavData == NULL)
	{
		return;
	}

	const bool bPersistent = Canvas == NULL;

	if (bPersistent)
	{
		FlushPersistentDebugLines( GetWorld() );
	}

	DrawDebugBox(GetWorld(), CurrentGoal, FVector(10.f), PathColor, bPersistent);
	if (Path.IsValid())
	{
		MyNavData->DrawDebugPath(Path.Get(), PathColor, Canvas, bPersistent, NextPathPointIndex);
	}
}
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 FNavMeshPath::OffsetFromCorners(float Distance)
{
	SCOPE_CYCLE_COUNTER(STAT_Navigation_OffsetFromCorners);

	const ARecastNavMesh* MyOwner = Cast<ARecastNavMesh>(GetNavigationDataUsed());
	if (PathPoints.Num() == 0 || PathPoints.Num() > 100)
	{
		// skip it, there is not need to offset that path from performance point of view
		return;
	}

#if DEBUG_DRAW_OFFSET
	GInternalDebugWorld_ = MyOwner->GetWorld();
	FlushDebugStrings(GInternalDebugWorld_);
	FlushPersistentDebugLines(GInternalDebugWorld_);
#endif

	if (bCorridorEdgesGenerated == false)
	{
		GeneratePathCorridorEdges(); 
	}
	const float DistanceSq = Distance * Distance;
	int32 CurrentEdge = 0;
	bool bNeedToCopyResults = false;
	int32 SingleNodePassCount = 0;

	FNavPathPoint* PathPoint = PathPoints.GetData();
	// it's possible we'll be inserting points into the path, so we need to buffer the result
	TArray<FPathPointInfo> FirstPassPoints;
	FirstPassPoints.Reserve(PathPoints.Num() + 2);
	FirstPassPoints.Add(FPathPointInfo(*PathPoint, FVector::ZeroVector, FVector::ZeroVector));
	++PathPoint;

	// for every point on path find a related corridor edge
	for (int32 PathNodeIndex = 1; PathNodeIndex < PathPoints.Num()-1 && CurrentEdge < PathCorridorEdges.Num();)
	{
		if (FNavMeshNodeFlags(PathPoint->Flags).PathFlags & RECAST_STRAIGHTPATH_OFFMESH_CONNECTION)
		{
			// put both ends
			FirstPassPoints.Add(FPathPointInfo(*PathPoint, FVector(0), FVector(0)));
			FirstPassPoints.Add(FPathPointInfo(*(PathPoint+1), FVector(0), FVector(0)));
			PathNodeIndex += 2;
			PathPoint += 2;
			continue;
		}

		int32 CloserPoint = -1;
		const FNavigationPortalEdge* Edge = &PathCorridorEdges[CurrentEdge];
		for (int32 EdgeIndex = CurrentEdge; EdgeIndex < PathCorridorEdges.Num(); ++Edge, ++EdgeIndex)
		{
			const float DistToSequence = FMath::PointDistToSegmentSquared(PathPoint->Location, Edge->Left, Edge->Right);
			if (DistToSequence <= FMath::Square(KINDA_SMALL_NUMBER))
			{
				const float LeftDistanceSq = FVector::DistSquared(PathPoint->Location, Edge->Left);
				const float RightDistanceSq = FVector::DistSquared(PathPoint->Location, Edge->Right);
				if (LeftDistanceSq > DistanceSq && RightDistanceSq > DistanceSq)
				{
					++CurrentEdge;
				}
				else
				{
					CloserPoint = LeftDistanceSq < RightDistanceSq ? 0 : 1;
					CurrentEdge = EdgeIndex;
				}
				break;
			}
		}

		if (CloserPoint >= 0)
		{
			bNeedToCopyResults = true;

			Edge = &PathCorridorEdges[CurrentEdge];
			const float ActualOffset = FPlatformMath::Min(Edge->GetLength()/2, Distance);

			FNavPathPoint NewPathPoint = *PathPoint;
			// apply offset 

			const FVector EdgePt0 = Edge->GetPoint(CloserPoint);
			const FVector EdgePt1 = Edge->GetPoint((CloserPoint+1)%2);
			const FVector EdgeDir = EdgePt1 - EdgePt0;
			const FVector EdgeOffset = EdgeDir.GetSafeNormal() * ActualOffset;
			NewPathPoint.Location = EdgePt0 + EdgeOffset;
			// update NodeRef (could be different if this is n-th pass on the same PathPoint
			NewPathPoint.NodeRef = Edge->ToRef;
			FirstPassPoints.Add(FPathPointInfo(NewPathPoint, EdgePt0, EdgePt1));

			// if we've found a matching edge it's possible there's also another one there using the same edge. 
			// that's why we need to repeat the process with the same path point and next edge
			++CurrentEdge;

			// we need to know if we did more than one iteration on a given point
			// if so then we should not add that point in following "else" statement
			++SingleNodePassCount;
		}
		else
		{
			if (SingleNodePassCount == 0)
			{
				// store unchanged
				FirstPassPoints.Add(FPathPointInfo(*PathPoint, FVector(0), FVector(0)));
			}
			else
			{
				SingleNodePassCount = 0;
			}

			++PathNodeIndex;
			++PathPoint;
		}
	}

	if (bNeedToCopyResults)
	{
		if (FirstPassPoints.Num() < 3 || !MyOwner->bUseBetterOffsetsFromCorners)
		{
			FNavPathPoint EndPt = PathPoints.Last();

			PathPoints.Reset();
			for (int32 Index=0; Index < FirstPassPoints.Num(); ++Index)
			{
				PathPoints.Add(FirstPassPoints[Index].Point);
			}

			PathPoints.Add(EndPt);
			return;
		}

		TArray<FNavPathPoint> DestinationPathPoints;
		DestinationPathPoints.Reserve(FirstPassPoints.Num() + 2);

		// don't forget the last point
		FirstPassPoints.Add(FPathPointInfo(PathPoints[PathPoints.Num()-1], FVector::ZeroVector, FVector::ZeroVector));

		int32 StartPointIndex = 0;
		int32 LastVisiblePointIndex = 0;
		int32 TestedPointIndex = 1;
		int32 LastPointIndex = FirstPassPoints.Num()-1;

		const int32 MaxSteps = 200;
		for (int32 StepsLeft = MaxSteps; StepsLeft >= 0; StepsLeft--)
		{ 
			if (StartPointIndex == TestedPointIndex || StepsLeft == 0)
			{
				// something went wrong, or exceeded limit of steps (= went even more wrong)
				DestinationPathPoints.Reset();
				break;
			}

			const FNavMeshNodeFlags LastVisibleFlags(FirstPassPoints[LastVisiblePointIndex].Point.Flags);
			const FNavMeshNodeFlags StartPointFlags(FirstPassPoints[StartPointIndex].Point.Flags);
			bool bWantsVisibilityInsert = true;

			if (StartPointFlags.PathFlags & RECAST_STRAIGHTPATH_OFFMESH_CONNECTION) 
			{
				AppendPathPointsHelper(DestinationPathPoints, FirstPassPoints, StartPointIndex);
				AppendPathPointsHelper(DestinationPathPoints, FirstPassPoints, StartPointIndex + 1);

				StartPointIndex++;
				LastVisiblePointIndex = StartPointIndex;
				TestedPointIndex = LastVisiblePointIndex + 1;
				
				// skip inserting new points
				bWantsVisibilityInsert = false;
			}
			
			bool bVisible = false; 
			if (((LastVisibleFlags.PathFlags & RECAST_STRAIGHTPATH_OFFMESH_CONNECTION) == 0) && (StartPointFlags.Area == LastVisibleFlags.Area))
			{
				FPathPointInfo LastVisiblePoint;
				bVisible = CheckVisibility( &FirstPassPoints[StartPointIndex], &FirstPassPoints[TestedPointIndex], PathCorridorEdges, Distance, &LastVisiblePoint );
				if (!bVisible)
				{
					if (LastVisiblePoint.Point.Location.IsNearlyZero())
					{
						DestinationPathPoints.Reset();
						break;
					}
					else if (StartPointIndex == LastVisiblePointIndex)
					{
						/** add new point only if we don't see our next location otherwise use last visible point*/
						LastVisiblePoint.Point.Flags = FirstPassPoints[LastVisiblePointIndex].Point.Flags;
						LastVisiblePointIndex = FirstPassPoints.Insert( LastVisiblePoint, StartPointIndex+1 );
						LastPointIndex = FirstPassPoints.Num()-1;

						// TODO: potential infinite loop - keeps inserting point without visibility
					}
				}
			}

			if (bWantsVisibilityInsert)
			{
				if (bVisible) 
				{ 
#if PATH_OFFSET_KEEP_VISIBLE_POINTS
					AppendPathPointsHelper(DestinationPathPoints, FirstPassPoints, StartPointIndex);
					LastVisiblePointIndex = TestedPointIndex;
					StartPointIndex = LastVisiblePointIndex;
					TestedPointIndex++;
#else
					LastVisiblePointIndex = TestedPointIndex;
					TestedPointIndex++;
#endif
				} 
				else
				{ 
					AppendPathPointsHelper(DestinationPathPoints, FirstPassPoints, StartPointIndex);
					StartPointIndex = LastVisiblePointIndex;
					TestedPointIndex = LastVisiblePointIndex + 1;
				} 
			}

			// if reached end of path, add current and last points to close it and leave loop
			if (TestedPointIndex > LastPointIndex) 
			{
				AppendPathPointsHelper(DestinationPathPoints, FirstPassPoints, StartPointIndex);
				AppendPathPointsHelper(DestinationPathPoints, FirstPassPoints, LastPointIndex);
				break; 
			} 
		} 

		if (DestinationPathPoints.Num())
		{
			PathPoints = DestinationPathPoints;
		}
	}
}