Prototype/Source/Prototype/Private/Task_ObstAvoid.cpp
/* Check in a sphere using the given vector as a starting point, checking for collision. Returns the first vector that hits no obstacle.
*
*   @param startingVec The vector to start the sweep from.
*   @param fidelity Number of points to check.
*   @return The first vector that hits no obstacles in a LineTraceSingleByChannel call. Returns null vector if no clear vector found.
*/
Check in a sphere using the given vector as a starting point, checking for collision. Returns the first vector that hits no obstacle.
Check in a sphere using the given vector as a starting point checking for collision . Returns the first vector that hits no obstacle .
bool AUSARAgent::FindClearVector(FVector& targetVec, int fidelity)
{
    bool foundClearVec = false;
    FVector agentLoc = GetActorLocation();
    float phi = (1.f + sqrtf(5.f)) / 2.f;
    for (int i = 0; i < fidelity; i++) {
        float itr = float(-fidelity + 1 + 2 * i);
        float theta = 2 * PI * itr / phi;
        float sPhi = itr / fidelity;
        float cPhi = sqrtf(FMath::Square(fidelity) - FMath::Square(itr)) / fidelity;
        float x = cPhi * sinf(theta);
        float y = cPhi * cosf(theta);
        float z = sPhi;
        FVector tmpVec = agentVelocity.Rotation().RotateVector(FVector(x, y, z));   
        FVector checkVec = OBSTACLE_AVOID_DIST * tmpVec + agentLoc;                 
        FHitResult hitResult;
        FCollisionQueryParams queryParams;
        queryParams.AddIgnoredActor(this);
        FCollisionResponseParams responseParams;
        if (!GetWorld()->LineTraceSingleByChannel(hitResult, GetActorLocation(), checkVec, ECC_WorldStatic, queryParams, responseParams)) {
            foundClearVec = true;
            targetVec = checkVec - GetActorLocation();
            if (showDebug) {
                DrawDebugPoint(GetWorld(), checkVec, 10, FColor::Green, false, 0.01);
            }
            break;
        }
        else {
            if (showDebug) {
                DrawDebugPoint(GetWorld(), checkVec, 5, FColor::Magenta, false, 0.1);
            }
        }
    }
    return foundClearVec;
}
bool AUSARAgent :: FindClearVector ( FVector & targetVec , int fidelity ) { bool foundClearVec = false ; FVector agentLoc = GetActorLocation ( ) ; float phi = ( 1.f + sqrtf ( 5.f ) ) / 2.f ; for ( int i = 0 ; i < fidelity ; i ++ ) { float itr = float ( - fidelity + 1 + 2 * i ) ; float theta = 2 * PI * itr / phi ; float sPhi = itr / fidelity ; float cPhi = sqrtf ( FMath :: Square ( fidelity ) - FMath :: Square ( itr ) ) / fidelity ; float x = cPhi * sinf ( theta ) ; float y = cPhi * cosf ( theta ) ; float z = sPhi ; FVector tmpVec = agentVelocity . Rotation ( ) . RotateVector ( FVector ( x , y , z ) ) ; FVector checkVec = OBSTACLE_AVOID_DIST * tmpVec + agentLoc ; FHitResult hitResult ; FCollisionQueryParams queryParams ; queryParams . AddIgnoredActor ( this ) ; FCollisionResponseParams responseParams ; if ( ! GetWorld ( ) -> LineTraceSingleByChannel ( hitResult , GetActorLocation ( ) , checkVec , ECC_WorldStatic , queryParams , responseParams ) ) { foundClearVec = true ; targetVec = checkVec - GetActorLocation ( ) ; if ( showDebug ) { DrawDebugPoint ( GetWorld ( ) , checkVec , 10 , FColor :: Green , false , 0.01 ) ; } break ; } else { if ( showDebug ) { DrawDebugPoint ( GetWorld ( ) , checkVec , 5 , FColor :: Magenta , false , 0.1 ) ; } } } return foundClearVec ; }
Check in a sphere using the given vector as a starting point, checking for collision.
Check in a sphere using the given vector as a starting point checking for collision .
// Fibonacci sphere // remove this once agent turning is in place // check vectors closest to agent velocity first /*DEBUGGING*/ //FString safeVectorFoundText = FString::Printf(TEXT("Agent %d found safe vector (%f, %f, %f)."), agentID, targetVec.X, targetVec.Y, targetVec.Z); //GEngine->AddOnScreenDebugMessage(INDEX_NONE, 3.0f, FColor::Green, safeVectorFoundText, true); /*DEBUGGING*/ /*DEBUGGING*/ /*DEBUGGING*/