USARBot.USARConverter

/*
 * This class is used to convert units. You can subclass this class and define new
 * defaultproperties. In the USARBot.ini file, in [USARBot.KRobot] section, change
 * ConverterClass to your class.
 *
 * In this file, we use SI Units. Here UU means Unreal Units. It can be any units used in Unreal.
 */

class USARConverter extends Object config(USARBot);

//Meters <-> UU
var config float C_MeterToUU;

//Angles <-> UU
var config float C_AngleToDegree;
var config float C_AngleToURot;

//right hand coord or left hand coord
var config bool RightHand;

var config byte NumberPrecision;

//=======================================
// Functions used for print out data
//.......................................
// You possibly needn't overwrite these functions
//=======================================
static final function String FloatString(float Value, optional int Precision) {
    local int IntPart;
    local float FloatPart;
    local string IntString, FloatString;

    if ( Precision == 0 )
        Precision = Default.NumberPrecision; // default 4
    else
        Precision = Max(Precision, 1);

    if ( Value < 0 ) {
        IntString = "-";
        Value *= -1;
    }

    IntPart = int(Value);
    FloatPart = Value - IntPart;
    IntString = IntString$string(IntPart);
    FloatString = string(int(FloatPart * 10 ** Precision));
    while (Len(FloatString) < Precision)
        FloatString = "0" $ FloatString;
    return IntString$"."$FloatString;
}

static final function String VectorString(vector vec, optional int Precision) {
    return FloatString(vec.X,Precision)$","$FloatString(vec.Y,Precision)$","$FloatString(vec.Z,Precision);
}

static final function String Str_LengthFromUU(float uu,  optional int Precision) {
    return FloatString(LengthFromUU(uu),Precision);
}

static final function String Str_SpeedFromUU(float uu, optional int Precision) {
    return FloatString(SpeedFromUU(uu),Precision);
}

static final function String Str_AngleFromUU(int uu, optional int Precision) {
    return FloatString(AngleFromUU(uu),Precision);
}

static final function String Str_AngleFromDeg(int deg, optional int Precision) {
    return FloatString(AngleFromDeg(deg),Precision);
}

static final function String Str_SpinSpeedFromUU(int uu, optional int Precision) {
    return FloatString(SpinSpeedFromUU(uu),Precision);
}

static final function String Str_RotatorFromUU(rotator rot, optional int Precision) {
    return VectorString(RotatorFromUU(rot),Precision);
}

static final function String Str_LengthVectorFromUU(vector vec, optional int Precision) {
    return VectorString(LengthVectorFromUU(vec),Precision);
}

static final function String Str_VelocityVectorFromUU(vector vec, optional int Precision) {
    return VectorString(VelocityVectorFromUU(vec),Precision);
}


//=========================================
// Functions used to convert data
//=========================================
static final function float LengthFromUU(float uu) {
    return uu/default.C_MeterToUU;
}

static final function float LengthToUU(float m) {
    return m*default.C_MeterToUU;
}

static final function float SpeedFromUU(float uu) {
    return uu/default.C_MeterToUU;
}

static final function float SpeedToUU(float m) {
    return m*default.C_MeterToUU;
}

static final function float AngleFromUU(int uu) {
    return uu/default.C_AngleToURot;
}

static final function int AngleToUU(float rad) {
    return int(rad*default.C_AngleToURot);
}

static final function float AngleFromDeg(int deg) {
    return deg/default.C_AngleToDegree;
}

static final function int AngleToDeg(float rad) {
    return int(rad*default.C_AngleToDegree);
}

static final function float SpinSpeedFromUU(int uu) {
    return uu/default.C_AngleToURot;
}

static final function int SpinSpeedToUU(float rad) {
    return int(rad*default.C_AngleToURot);
}

/*
 * Convert a rotator holds Picth, Yaw, and Roll in Unreal to a rotator holds Roll, Pitch, and Yaw in the real world
 * The return value is a vector NOT rotator because roatar only support int element. And we want float number as
 * element.
 */
static final function vector RotatorFromUU(rotator rot) {
    local vector vec;
    vec.X = rot.Roll/default.C_AngleToURot;
    vec.Y = rot.Pitch/default.C_AngleToURot;
    vec.Z = rot.Yaw/default.C_AngleToURot;
    return vec;
}

static final function rotator RotatorToUU(vector vec) {
    local rotator rot;
    rot.Roll  = vec.X*default.C_AngleToURot;
    rot.Pitch = vec.Y*default.C_AngleToURot;
    rot.Yaw   = vec.Z*default.C_AngleToURot;
    return rot;
}

/*
 * Convert left hand coord in UU to right hand coord for length vector.
 */
static final function vector LengthVectorFromUU(vector vec) {
    local vector res;
    res.X =  vec.X/default.C_MeterToUU;
    res.Y =  vec.Y/default.C_MeterToUU;
    if(default.RightHand)
        res.Z = -vec.Z/default.C_MeterToUU;
    else
        res.Z = vec.Z/default.C_MeterToUU;
    return res;
}

static final function vector LengthVectorToUU(vector vec) {
    local vector res;
    res.X =  vec.X*default.C_MeterToUU;
    res.Y =  vec.Y*default.C_MeterToUU;
    if(default.RightHand)
        res.Z = -vec.Z*default.C_MeterToUU;
    else
        res.Z = vec.Z*default.C_MeterToUU;
    return res;
}

/*
 * Convert left hand coord in UU to right hand coord for velocity vector.
 */
static final function vector VelocityVectorFromUU(vector vec) {
    local vector res;
    res.X =  vec.X/default.C_MeterToUU;
    res.Y =  vec.Y/default.C_MeterToUU;
    if(default.RightHand)
        res.Z = -vec.Z/default.C_MeterToUU;
    else
        res.Z = vec.Z/default.C_MeterToUU;
    return res;
}

static final function vector VelocityVectorToUU(vector vec) {
    local vector res;
    res.X =  vec.X*default.C_MeterToUU;
    res.Y =  vec.Y*default.C_MeterToUU;
    if(default.RightHand)
        res.Z = -vec.Z*default.C_MeterToUU;
    else
        res.Z = vec.Z*default.C_MeterToUU;
    return res;
}

/*
 *  normRad_ZeroTo2PI
 *  This function normalizes radians value to be between [0,2PI]. 
 *  It works for both negative radain values and cumulative values
 *  of radains, values > 2PI and values < -2PI.  Positive rotations 
 *  are clockwise.
 */
static final function float normRad_ZeroTo2PI(float rads)
{
       local float nRads;
       nRads = rads % (2*pi);
       if( nRads < 0 )
       {
           nRads = nRads + (2*PI);
       }
       return nRads;
}

defaultproperties
{
    // 1m = 250uu , 1uu =4mm
    C_MeterToUU=250

    //Angle = Radians
    C_AngleToDegree=57.2957795131;
    C_AngleToURot=10430.3783505;

    RightHand=true

    NumberPrecision=4
}