USARBot.SonarSensor

//
//	author: Erik Winter
//  created: 2005-02-22	Created the class from the USARSim RangeSensor class
//
//  modified:	2005-02-24
//				2005-02-28	Made the sonar send out the traces within a cone
//				2005-03-03	Added functionality so the getRange fctn doesn't care if it hits a surface with a to big angle
//              2005-04-04  (Jijun Wang) Added BeamAngle, so we specify Beam Angle from the configuration file
//	Todo:	Make sure that the sonar returns the closest object in a cone-shaped volume instead of along a line

class SonarSensor extends RangeSensor config (USARBot);

var config float BeamAngle;
var int uuBeamAngle;

function ConvertParam(USARConverter converter)
{
    super.ConvertParam(converter);

    if (converter!=NONE) {
        uuBeamAngle = converter.AngleToUU(BeamAngle);
    }
    else {
        uuBeamAngle = BeamAngle;
    }
}

// GetRange should look for objects within a cone
// The idea is to send out Trace's along a number of different circles.
// Todo: the sensor shouldn't measure a wall it hits with a normal different than 90 deg +/- some const DONE! Right now with 10 as const
function float GetRange()
{
    local vector HitLocation,HitNormal;
    local float range, rangeTmp,radius,yawOS,pitchOS;
    local int nrCircles, tracesPerCircle;
    local float degreesPerCircle;
    local int i,j;
    local Rotator rotWOS; // the current rotator with added offset

    //local string debugStr;

    //local vector A,B;
    local float angle;


    //
    tracesPerCircle = 8; // send out a Trace every 360/tracesPerCircle degree
    degreesPerCircle = 4*32768/180; // how much wider will the cone be for each new circle,
    nrCircles = uuBeamAngle/degreesPerCircle;
    // OBS In one direction. The cone will be degreesPerCircle*2*nrCircles degrees wide

    // send one Trace straight ahead
    if (Trace(HitLocation, HitNormal, Location + uuMaxRange*vector(curRot), Location, true)==NONE) 
        range=uuMaxRange;
    else
        range=VSize(HitLocation-Location);

    // send out Trace's along cones
    for(i=1;i<=nrCircles;i++){
        radius = i*degreesPerCircle; // i*degreesPerCircle*DegreeToRadian*RadianToURot
        for(j=0;j<tracesPerCircle;j++){

            //
            yawOS = radius*cos(j*2*Pi/tracesPerCircle); //
            pitchOS = radius*sin(j*2*Pi/tracesPerCircle);


            rotWOS = curRot;
            rotWOS.Pitch += int(yawOS);
            rotWOS.Yaw -= int(pitchOS);

            //if(i==nrCircles && j==0) A = vector(rotWOS);
            //if(i==nrCircles && j==4) B = vector(rotWOS);

            if (Trace(HitLocation, HitNormal, Location + uuMaxRange*vector(rotWOS), Location, true)==NONE){}
            else {
                rangeTmp=VSize(HitLocation-Location);
                angle = Acos(Normal(HitNormal) dot Normal(vector(rotWOS))); //
                if(rangeTmp<range && angle < 0.1745329252) range=rangeTmp; // the sensor trajectory and HitNormal should at most differ 10 degrees
            }

        }
    }

    //angle = 57.2957795131*(Acos(Normal(A) dot Normal(B)));
    //if(ItemName=="front_middle")log(angle$"X"$A.x$"X"$A.y$"X"$A.z$"X"$B.x$"X"$B.y$"X"$B.z);

    if (converter!=None)
        range=converter.LengthFromUU(range);

    if (OutputCurve.Points.length>0)
        range=InterpCurveEval(OutputCurve, range);
    range += RandRange(-Noise,Noise)*range;
    if (range>MaxRange) range=MaxRange;
    if (range<MinRange) range=MinRange;
    //log(debugStr);
    return range;
}

function String GetData()
{
    local string outstring;
    curRot=Rotation;
    outstring="{Name "$ItemName$" Range "$converter.FloatString(GetRange())$"}";
    return outstring;
}

function String GetConfData()
{
    local string outstring;
    outstring = Super.GetConfData();
    outstring = outstring@"{BeamAngle "$converter.FloatString(BeamAngle)$"}";
    return outstring;
}

defaultproperties
{
//Scaled with 4.762 at Mon Sep 25 14:21:52 EDT 2006
    bUseGroup=True
    ItemType="Sonar"
    HiddenSensor=false
    MaxRange=5.0 // = 5 meters
    MinRange=0.1 // 0.10 m
    BeamAngle = 0.3491 // 20 deg
    OutputCurve=(Points=((InVal=0.000000,OutVal=0.000000),(InVal=5.000000,OutVal=5.000000)))
    drawscale=0.4762
}