USARBot.RFIDSensor

// ===============================================================
// USARBot.RFIDSensor
//
// This class simulates the RFID reader. It iterates all the 
// registered RFID tags and reports the tags that are within the
// detecting range. Single transmission is applied in tag info 
// transfer.
//
// Original created by: Alexander Kleiner and Christian 
// Modified by: Jijun Wang   -- 12/15/2005
// Mentar Mahmudi - IUB - m.mahmudi@iu-bremen.de -- 14/02/2006
// ===============================================================

class RFIDSensor extends sensor config(USARBot);

var config float MaxRange;
var config float MaxSingleShotRange;
var config float TagNoise;
var float uuMaxRangeSquare;
var float uuMaxSingleShotRangeSquare;
var array<int> detectedTags;

function ConvertParam(USARConverter converter)
{
    if (converter != None) {
        uuMaxRangeSquare = converter.LengthToUU(MaxRange) * converter.LengthToUU(MaxRange);
        uuMaxSingleShotRangeSquare = converter.LengthToUU(MaxSingleShotRange) * converter.LengthToUU(MaxSingleShotRange);
    } else {
        uuMaxRangeSquare = MaxRange*MaxRange;
        uuMaxSingleShotRangeSquare = MaxSingleShotRange*MaxSingleShotRange;
    }
}

function int getDetectedTag(int idx)
{
    local int i;
    
    for (i = 0; i < detectedTags.length; i++) {
        if (detectedTags[i] == idx)
            return i;
    }
    return -1;
}

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

function String GetData()
{
    local string Outstring;
    local int i, idx;
    local vector diff, pos;
    local array<RFIDTag> tags;
    
    Outstring="";
    
    tags = USARDeathMatch(Level.Game).RFIDTags;
    
    //Prints the position of the tag(s) in terms of the coordinate system with
    //the robot tag sensor being at the origin.
    
    for (i = 0; i < tags.length; i++) {
        diff = tags[i].Location - Location;
        if ( ( (VectLenSq(diff) < uuMaxRangeSquare) && ( !tags[i].bSingleShot ) ) ||
           ( (VectLenSq(diff) < uuMaxSingleShotRangeSquare) && ( tags[i].bSingleShot ) ) ) {
            //if (!tags[i].bSingleShot || getDetectedTag(i) == -1) {
            if (getDetectedTag(i) == -1) {
                Outstring = Outstring$" {ID "$tags[i].id$"}";
                pos = tag_pos(tags[i]);
                Outstring = Outstring$" {Location "$converter.VectorString(pos, 2)$"}";
                if (tags[i].bSingleShot) {
                    detectedTags.Insert(detectedTags.length, 1);
                    detectedTags[detectedTags.length - 1] = i;
                }
            }
        } 
        
        else if (!tags[i].bSingleShot) {
            idx = getDetectedTag(i);
            if (idx >= 0)
                detectedTags.Remove(idx, 1);
        }
    }
            
    if (Outstring!="") Outstring = "{Name "$ItemName$"}"$OutString;
    //if (Outstring!="") log(Outstring);
    return Outstring;
}

function vector tag_pos(RFIDTag aTag) {

    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 = aTag.Location;
    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));
    
    if ( ! aTag.bSingleShot ) {
      pos.x += RandRange(-TagNoise, TagNoise);
      pos.y += RandRange(-TagNoise, TagNoise);
      pos.z += RandRange(-TagNoise, TagNoise);
    
    }
    
    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$"}";
    return outstring;
}

defaultproperties
{
//Scaled with 4.762 at Mon Sep 25 14:21:52 EDT 2006
     MaxRange=2
     MaxSingleShotRange=6
     ItemType="RFID"
     TagNoise=0.6
    drawscale=0.4762
}