USARBot.BallHSensor

/* Ball Helper Sensor for AIBO robots
 * It locates the ball in 3D, calculates its 2D position on the screen
 * and its radius in pixels, reports distance of the ball center from
 * the camera and if it's visible or not. It also calculates ball occlusions
 * by sampling ball outline and checking the visibility of each sample.
 * This test is called "corona test" and reports to the controller a
 * number that codes in each bit the corresponding sample visibility.
 * The corona is always aligned with the Y axis of the screen and it
 * adapts to perspective distortions of the ball.
 * by Marco Zaratti - marco.zaratti@gmail.com
*/
class BallHSensor extends Sensor config(USARBot);

const RAD2DEG = 57.2957795;

//Cameras in USARSim hove no resolution information
//so I'm hardwiring here AIBO cam resolution. This
//should be changed in the future.
var config int CameraXres;
var config int CameraYres;
var config float BallRadiusUU;
var config byte BallSamples;
var array<vector> CoronaSamples;
var config float LODhalf, LODcenter;
var int uuLODhalf, uuLODcenter;
var Soccerball KBall;

function PostNetBeginPlay()
{
    local float CoronaStep;
    local float CoronaTime;
    local int i;

    Super.PostNetBeginPlay();

    //Take a reference to the ball
    if(Role == ROLE_Authority)
        foreach AllActors(class'SoccerBall',KBall) break;

    if(KBall != none)
    {
        if((BallSamples % 2) != 0)
            BallSamples++;
        CoronaSamples.Insert(0,BallSamples);
        CoronaStep = 2*PI / BallSamples;

        //Counter-Clockwise circle from noon + CoronaStep
        //This allows to get a clockwise (from noon) corona coding
        for(i=0; i < BallSamples; i++)
        {
            CoronaTime = PI/2 + CoronaStep * (i+1);
            CoronaSamples[i].Y = BallRadiusUU * Cos(CoronaTime);
            CoronaSamples[i].Z = BallRadiusUU * Sin(CoronaTime);
        }
    }
}

function ConvertParam(USARConverter converter)
{
    if (converter!=NONE)
    {
        uuLODhalf = converter.LengthToUU(LODhalf);
        uuLODcenter = converter.LengthToUU(LODcenter);
    }
    else
    {
        uuLODhalf = LODhalf;
        uuLODcenter = LODcenter;
    }
}

function String GetData()
{
    local string outstring, visStr;
    //BallCamDir is the vector pointing from the camera to the ball
    //BallCamPos is the ball position in camera coordinates
    local vector BallPos, CamLoc, BallCamDir, BallCamPos;
    local rotator CamRot;
    local float ballDist;
    local int x2d,y2d,r;        //2D X and Y positions, r = ball radius in pixel
    local int x2dMod, y2dMod;   //x2d and y2d ABS values
    local int halfResX, halfResY;
    local byte outerZone;       // bits string used to compute ball zone
    local float K, K1, cornerRadius;
    local bool visible;         //TRUE if the ball is visible
    local int i;
    //Occlusione testing variables
    local Actor HitObj;
    local vector HitLoc, HitNorm;
    local int HiddenCount, CoronaCode, DoubleCode;
    local vector CoronaSample, CoronaCamDir;

    if(KBall != none)
    {
        BallPos = KBall.Location;
        CamLoc = Location;  //This sensor should be attached at the same camera position
        CamRot = Rotation;  //This sensor should be attached at the same camera position
        BallCamDir = BallPos - CamLoc;

        //We're going into camera coordinates (inverse transform)
        BallCamPos = BallCamDir << CamRot;

        //if < 0 than the ball is behind the AIBO and is not visible
        if(BallCamPos.X > 0)
        {
            ballDist = VSize(BallCamPos);

            halfResX =  CameraXres/2;
            halfResY =  CameraYres/2;

            //Computes perspective transformation constant
            K = float(halfResX)/(Tan(KRobot(Platform).CameraZoom/(2*RAD2DEG)));
            K1= K / BallCamPos.X;
            //Make perspective transformation
            x2dMod = int(K1 * BallCamPos.Y);
            y2dMod = int(K1 * BallCamPos.Z);

            //(0,0) is up left
            x2d =halfResX + x2dMod;
            y2d =halfResY - y2dMod;

            //find ball radius in pixels
            r = int(K1 * BallRadiusUU);

            //Take 2D position ABS value
            if(x2dMod < 0)
                x2dMod = -x2dMod;
            if(y2dMod < 0 )
                y2dMod = -y2dMod;

            //Determine ball zone with a bit mapping
            if(x2dMod > halfResX)
                outerZone = 1;
            if(y2dMod > halfResY)
                outerZone = outerZone | 2;

            //Check visibility
            switch(outerZone)
            {
                case 0: //center visible
                    visible = true;
                    break;
                case 1: //center Right
                    if(x2dMod < (halfResX + r))
                        visible = true;
                    break;
                case 2: //center Down
                    if(y2dMod < (halfResY + r))
                        visible = true;
                    break;
                case 3: //center at Corner
                    cornerRadius = Square(x2dMod - halfResX) + Square(y2dMod - halfResY);
                    if(cornerRadius < r*r)
                        visible = true;
            }
        }

        //Now we do occlusion test
        if(visible)
        {
            //Manages LODcenter (ball very far)
            if(ballDist > uuLODcenter)
            {
                //*5 in start vector is used to avoid self-collision with AIBO head
                BallCamDir = Normal(BallCamDir);
                HitObj = Trace(HitLoc, HitNorm, BallPos, CamLoc + BallCamDir*5,true);
                if(HitObj.IsA('SoccerBall'))
                    CoronaCode = 2**BallSamples -1;
                else
                    visible = false;
            }
            //Manages LODhalf or ball near
            else
            {
                //*5 in start vector is used to avoid self-collision with AIBO head
                //BallCamDir = Normal(BallCamDir);
                for(i=0; i < BallSamples; i++)
                {
                    //Assumes the sample is visible
                    CoronaCode = CoronaCode << 1;
                    CoronaCode = CoronaCode | 1;

                    //Align the corona to Y axis of the screen.
                    //That means that we build the corona around the ball
                    //in local camera coordinates. Then we transform these
                    //coordinates in world reference (see later).
                    //Local transform:
                    CoronaSample = CoronaSamples[i] >> rotator(BallCamPos);
                    CoronaSample += BallCamPos;

                     //Check sample visibility (I'm recycling x2dMod and y2dMod vars)
                    x2dMod = int(K * CoronaSample.Y / CoronaSample.X);
                    y2dMod = int(K * CoronaSample.Z / CoronaSample.X);
                    if((x2dMod < -halfResX || x2dMod > halfResX) || (y2dMod < -halfResY || y2dMod > halfResY))
                    {
                        HiddenCount++;
                        CoronaCode = CoronaCode ^ 1;    //XOR
                    }
                    else    //If sample is visible check collisions
                    {
                        //Corona transformed in World reference
                        CoronaSample = CoronaSample >> CamRot;
                        CoronaSample += CamLoc;

                        //Avoids collisions with ground (Trace() ray hits the ground)
                        //TODO: This control assumes that the ball is always at Z >= 0
                        //      A more general solutions should be implemented.
                        if(CoronaSample.Z < 0)
                            CoronaSample.Z = -CoronaSample.Z;

                       //Make collision test for current sample
                        CoronaCamDir = Normal(CoronaSample - CamLoc);
                        HitObj = Trace(HitLoc, HitNorm, CoronaSample, CamLoc + CoronaCamDir*5,true);
                        if(HitObj != none)
                            if(!HitObj.IsA('SoccerBall'))
                            {
                                CoronaCode = CoronaCode ^ 1;    //XOR
                                HiddenCount++;
                            }
                    }

                    //Manages LODhalf
                    if(ballDist > uuLODhalf)
                    {
                        i++;
                        DoubleCode = CoronaCode & 1;
                        if(DoubleCode == 0)
                            HiddenCount++;
                        CoronaCode = CoronaCode << 1;
                        CoronaCode = CoronaCode | DoubleCode;
                    }
                }

                if(HiddenCount == BallSamples)
                    visible = false;
            }
        }// end of occlusione test

        //OK, at last we're ready to build output string
        if(visible)
            visStr="}{Visible true}{Pos2D "$x2d$","$y2d$"}{Radius "$r$"}{Dist "$converter.Str_LengthFromUU(ballDist)$"}{Corona "$CoronaCode$"}";
        else
            visStr="}{Visible false}{Pos2D 0,0}{Radius 0}{Dist 0}{Corona 0}";

        outstring="{Name "$ItemName$"}"$"{Pos3D "$converter.Str_LengthVectorFromUU(BallPos)$visStr;
    }
    return outstring;
}

defaultproperties
{
    ItemType="Helper"
    CameraXres=208  //pixel
    CameraYres=160  //pixel
    BallRadiusUU=10 //unreal units
    BallSamples=12  //Corona samples for occlusion test (must be an even number)
    LODhalf=0.8     //meter, distance at which halve corona samples
    LODcenter=2     //meter, distance at which consider only ball center
}