USARBot.RFIDVictSensor

// ===============================================================
// USARBot.RFIDVictSensor
//
// Added additional functionality for the Automatic Referee
// Mentar Mahmudi - International University Bremen - m.mahmudi@iu-bremen.de
// ===============================================================

class RFIDVictSensor extends sensor config(USARBot);

//distance for the ranges
var config float VictMinRange;
var config float FPMinRange;
var config float MaxRange;
var config float VictStatusRange;
var float uuVictMinRangeSquare;
var float uuFPMinRangeSquare;
var float uuMaxRangeSquare;
var float uuVictStatusRangeSquare;
var config float RFIDFOV;  // default value is around 30 deg.

function ConvertParam(USARConverter converter)
{
    if (converter != None) {
        uuMaxRangeSquare = converter.LengthToUU(MaxRange) * converter.LengthToUU(MaxRange);
        uuVictMinRangeSquare = converter.LengthToUU(VictMinRange) * converter.LengthToUU(VictMinRange);
        uuFPMinRangeSquare = converter.LengthToUU(FPMinRange) * converter.LengthToUU(FPMinRange); 
        uuVictStatusRangeSquare = converter.LengthToUU(VictStatusRange) * converter.LengthToUU(VictStatusRange);
    } else {
        uuMaxRangeSquare = MaxRange*MaxRange;
        uuVictMinRangeSquare = VictMinRange*VictMinRange;
        uuFPMinRangeSquare = FPMinRange*FPMinRange;
        uuVictStatusRangeSquare = VictStatusRange * VictStatusRange;
    }
}

function float VectLenSq(vector v) {
    return v.x * v.x + v.y * v.y; // + v.z * v.z;
}

/* return value represents
 0: cannot see tag
 1: can see tag
 2: can tell if false alarm
 3: can tell id of victim
 4: can tell status of victim
*/
function int reportLevel( vector tagLocation ) {
    local vector diff;
    local float vectSq;
    local vector HitLocation, HitNormal;

    if( !inFOV(tag_pos(tagLocation)) )
        return 0;
    diff = tagLocation - Location;
    vectSq = VectLenSq(diff);
    if (vectSq > uuMaxRangeSquare) { // cannot see the tag
        return 0;
    }
    Trace( HitLocation, HitNormal, tagLocation );
//	Log("Trace return: "$HitLocation$" "$HitNormal);
    if( HitLocation.x!=0 || HitLocation.y!=0 || HitLocation.z!=0 ) // can't see tag
        return 0;
    if( vectSq < uuVictStatusRangeSquare ) { // can identify the victim status
        return 4;
    }
    if( vectSq < uuVictMinRangeSquare ) { // can tell ID of the victim
        return 3;
    }
    if( vectSq < uuFPMinRangeSquare ) { // can tell if a false alarm
        return 2;
    }
    // should only be here if we can see the tag so return that we can see it, but don't know what it is
    return 1;
}

function String GetData()
{
    local string Outstring;
    local int i, repLevel;
    local vector pos;
    local array<VictRFIDTag> tags;
    local array<SeerPawn> seerTags;
    
    Outstring="";
    
    //Prints the position of the tag(s) in terms of the coordinate system with
    //the robot tag sensor being at the origin.
    tags = USARDeathMatch(Level.Game).VictRFIDTags;
    for (i = 0; i < tags.length; i++) {
        repLevel = reportLevel( tags[i].Location );
        if( repLevel > 0 )
            pos = tag_pos(tags[i].Location);
        if( repLevel > 1 && tags[i].id == -1  )  // false alarm tag
            Outstring = Outstring$" {ID FalsePositive} {Status None} {Location "$converter.VectorString(pos, 2)$"}";
        else if( repLevel == 1 )
            Outstring = Outstring$" {ID None} {Status None} {Location "$converter.VectorString(pos, 2)$"}";
        else if( repLevel == 2 )
            Outstring = Outstring$" {ID None} {Status None} {Location "$converter.VectorString(pos, 2)$"}";
        else if( repLevel == 4 )
            Outstring = Outstring$" {ID "$tags[i].name$"} {Status "$tags[i].status$"} {Location "$converter.VectorString(pos, 2)$"}";
        else if( repLevel == 3 )
            Outstring = Outstring$" {ID "$tags[i].name$"} {Status None} {Location "$converter.VectorString(pos, 2)$"}";
    }


    //Prints the position of the tag(s) in terms of the coordinate system with
    //the robot tag sensor being at the origin.
    seerTags = USARDeathMatch(Level.Game).SeerVictims;
    for (i = 0; i < seerTags.length; i++) {
        repLevel = reportLevel( seerTags[i].Location );
        if( repLevel > 0 )
            pos = tag_pos(seerTags[i].Location);
        if( repLevel > 1 && seerTags[i].id == -1  )  // false alarm tag
            Outstring = Outstring$" {ID FalsePositive} {Status None} {Location "$converter.VectorString(pos, 2)$"}";
        else if( repLevel == 1 )
            Outstring = Outstring$" {ID None} {Status None} {Location "$converter.VectorString(pos, 2)$"}";
        else if( repLevel == 2 )
            Outstring = Outstring$" {ID None} {Status None} {Location "$converter.VectorString(pos, 2)$"}";
        else if( repLevel == 4 )
            Outstring = Outstring$" {ID "$seerTags[i].victName$"} {Status "$seerTags[i].status$"} {Location "$converter.VectorString(pos, 2)$"}";
        else if( repLevel == 3 )
            Outstring = Outstring$" {ID "$seerTags[i].victName$"} {Status None} {Location "$converter.VectorString(pos, 2)$"}";
    }

    if (Outstring != "") Outstring = "{Name "$ItemName$"}"$OutString;
    //log(Outstring);
    return Outstring;
}

function vector tag_pos(vector tag_posIn) {

    local vector tag_pos, robot_pos, VX, VY, VZ, tmp, pos;
    local Rotator robot_rot;
    local Matrix T;
    local Plane PX, PY, PZ, PW;
    
    tag_pos = tag_posIn;
    robot_pos = Location;
    robot_rot = Rotation;
        
    // calculating the position of the tag
    GetUnAxes (robot_rot, VX, VY, VZ);
    PX.x = VX.x; PX.y = VX.y; PX.z = VX.z; PX.W = 0;
    PY.x = VY.x; PY.y = VY.y; PY.z = VY.z; PY.W = 0;
    PZ.x = VZ.x; PZ.y = VZ.y; PZ.z = VZ.z; PZ.W = 0;
    tmp.x = VX.x;
    tmp.y = VY.x;
    tmp.z = VZ.x;
    PW.x = - ( tmp Dot robot_pos ); 
    tmp.x = VX.y;
    tmp.y = VY.y;
    tmp.z = VZ.y;
    PW.y = - ( tmp Dot robot_pos);
    tmp.x = VX.z;
    tmp.y = VY.z;
    tmp.z = VZ.z;
    PW.z = - ( tmp Dot robot_pos); 
    PW.W = 1;
    T.XPlane = PX;
    T.YPlane = PY;
    T.ZPlane = PZ;
    T.WPlane = PW;
    
    //use converter here
    pos.x = converter.LengthFromUU(T.XPlane.X * tag_pos.x + T.YPlane.x * tag_pos.y + T.zplane.x * tag_pos.z + T.wplane.x);
    pos.y = converter.LengthFromUU(T.XPlane.Y * tag_pos.x + T.YPlane.Y * tag_pos.y + T.zplane.Y * tag_pos.z + T.wplane.Y);
    pos.z = converter.LengthFromUU(-(T.XPlane.Z * tag_pos.x + T.YPlane.Z * tag_pos.y + T.zplane.Z * tag_pos.z + T.wplane.Z));
    
    return pos;
    
    //yaw = robot_rot.yaw*2*Pi/65536;
    //the calculation is correct.
    //x = ((tag_pos.x - robot_pos.x) * cos(yaw) + (tag_pos.y - robot_pos.y) * sin(yaw))/52.5;
    //y = (-(tag_pos.x - robot_pos.x) * sin(yaw) + (tag_pos.y - robot_pos.y) * cos(yaw))/52.5;
}

function String GetConfData()
{
    local string outstring;
    outstring = Super.GetConfData();
    outstring = outstring@"{MaxRange "$MaxRange$"} {VictMinRange "$VictMinRange$"} {FPMinRange "$FPMinRange$"}";
    return outstring;
}

function bool inFOV(vector pos) {
    local float angle;
    local float llimit,ulimit;
    //local string s;
    ulimit = RFIDFOV/2;
    llimit = -ulimit;
    
    angle = Atan(pos.y,pos.x);
    
    //s="x,y,z"@pos.x@" "@pos.y@" "@pos.z@" ulimit "@ulimit@" llimit "@llimit@" horizangle "@angle;
    //log(s);
    
    if((angle <  llimit) || (angle >  ulimit)) {
        //log(s);
        return false;
    }
        

    //angle = Atan(pos.z,pos.x);
    // commented out due to z coordinate problem
    //s = s@" vertangle "@angle;
/*  
    if((angle <  llimit) || (angle > ulimit)) {
        //log(s);
        return false;
    }
*/
    //log(s);
    return true;
}

defaultproperties
{
//Scaled with 4.762 at Mon Sep 25 14:21:52 EDT 2006
    MaxRange=20
    FPMinRange=5
    VictMinRange=3
    VictStatusRange=1
    ItemType="VictRFID"
    RFIDFOV=1.57
    drawscale=0.4762
}
// RFIDFOV was 0.52