Modules/BehaviorControl/CommonXabsl2Symbols/MathFunctions.cpp

Go to the documentation of this file.

/** 
* @file MathFunctions.cpp
*
* Implementation of class MathFunctions.
*
* @author Matthias Jüngel
*/

#include "MathFunctions.h"
#include "Tools/Math/Geometry.h"

MathFunctions::MathFunctions(const BehaviorControlInterfaces& interfaces)
: BehaviorControlInterfaces(interfaces)
{
}


void MathFunctions::registerSymbols(Xabsl2Engine& engine)
{
  // "distance-to"
  engine.registerDecimalInputFunction("distance-to",this,
    (double (Xabsl2FunctionProvider::*)())&MathFunctions::distanceTo);
  engine.registerDecimalInputFunctionParameter("distance-to","distance-to.x",distanceToX);
  engine.registerDecimalInputFunctionParameter("distance-to","distance-to.y",distanceToY);

  // "angle-to"
  engine.registerDecimalInputFunction("angle-to",this,
    (double (Xabsl2FunctionProvider::*)())&MathFunctions::angleTo);
  engine.registerDecimalInputFunctionParameter("angle-to","angle-to.x",angleToX);
  engine.registerDecimalInputFunctionParameter("angle-to","angle-to.y",angleToY);

  // "abs"
  engine.registerDecimalInputFunction("abs",this,
    (double (Xabsl2FunctionProvider::*)())&MathFunctions::getAbs);
  engine.registerDecimalInputFunctionParameter("abs","abs.value",absValue);

  // "min"
  engine.registerDecimalInputFunction("min",this,
    (double (Xabsl2FunctionProvider::*)())&MathFunctions::getMin);
  engine.registerDecimalInputFunctionParameter("min","min.value0",min0Value);
  engine.registerDecimalInputFunctionParameter("min","min.value1",min1Value);

  // "max"
  engine.registerDecimalInputFunction("max",this,
    (double (Xabsl2FunctionProvider::*)())&MathFunctions::getMax);
  engine.registerDecimalInputFunctionParameter("max","max.value0",max0Value);
  engine.registerDecimalInputFunctionParameter("max","max.value1",max1Value);

  // "sgn"
  engine.registerDecimalInputFunction("sgn",this,
    (double (Xabsl2FunctionProvider::*)())&MathFunctions::getSgn);
  engine.registerDecimalInputFunctionParameter("sgn","sgn.value",sgnValue);

  // "sin"
  engine.registerDecimalInputFunction("sin",this,
    (double (Xabsl2FunctionProvider::*)())&MathFunctions::getSin);
  engine.registerDecimalInputFunctionParameter("sin","sin.alpha",alphaValue);

  // "cos"
  engine.registerDecimalInputFunction("cos",this,
    (double (Xabsl2FunctionProvider::*)())&MathFunctions::getCos);
  engine.registerDecimalInputFunctionParameter("cos","cos.alpha",alphaValue);

  // "random"
  engine.registerDecimalInputSymbol("random",this,
    (double (Xabsl2FunctionProvider::*)())&MathFunctions::getRandom);

  // "normalize"
  engine.registerDecimalInputFunction("normalize",this,
    (double (Xabsl2FunctionProvider::*)())&MathFunctions::getNormalize);
  engine.registerDecimalInputFunctionParameter("normalize","normalize.angle",normalizeAngle);

  // "freeze-value" math.unfreeze-value has to be true (1), before freezing is possible
  engine.registerEnumeratedOutputSymbol("math.unfreeze-value",(int*)&unFreezeValue);
  engine.registerEnumeratedOutputSymbolEnumElement("math.unfreeze-value","true", 1);

  engine.registerDecimalInputFunction("freeze",this,
    (double (Xabsl2FunctionProvider::*)())&MathFunctions::getFrozenValue);
  engine.registerDecimalInputFunctionParameter("freeze","freeze.value",valueToFreeze);

  engine.registerDecimalInputFunction("is-in-interval",this,
    (double (Xabsl2FunctionProvider::*)())&MathFunctions::isInInterval);
  engine.registerDecimalInputFunctionParameter("is-in-interval","is-in-interval.a",isInIntervalA);
  engine.registerDecimalInputFunctionParameter("is-in-interval","is-in-interval.b",isInIntervalB);
  engine.registerDecimalInputFunctionParameter("is-in-interval","is-in-interval.value",isInIntervalValue);

  engine.registerDecimalInputFunction("save-value",this,
    (double (Xabsl2FunctionProvider::*)())&MathFunctions::saveValue);
  engine.registerDecimalInputFunctionParameter("save-value","save-value.index",saveindex);
  engine.registerDecimalInputFunctionParameter("save-value","save-value.value",savevalue);

  engine.registerDecimalInputFunction("get-value",this,
    (double (Xabsl2FunctionProvider::*)())&MathFunctions::getValue);
  engine.registerDecimalInputFunctionParameter("get-value","get-value.index",saveindex);

}

void MathFunctions::update()
{
}

double MathFunctions::distanceTo()
{
  return Geometry::distanceTo(robotPose,Vector2<double>(distanceToX,distanceToY));
}

double MathFunctions::angleTo()
{
  return toDegrees(Geometry::angleTo(robotPose,Vector2<double>(angleToX,angleToY)));
}

double MathFunctions::getSgn()
{
  return sgnValue > 0 ? 1 : -1;
}

double MathFunctions::getAbs()
{
  return fabs(absValue);
}

double MathFunctions::getMin()
{
  if (min0Value < min1Value)
    return min0Value;
  return min1Value;
}

double MathFunctions::getMax()
{
  if (max0Value > max1Value)
    return max0Value;
  return max1Value;
}

double MathFunctions::getSin()
{
  return sin(fromDegrees(alphaValue));
}

double MathFunctions::getCos()
{
  return cos(fromDegrees(alphaValue));
}

double MathFunctions::getRandom()
{
  return ((double) rand() / (RAND_MAX+1.0));
}

double MathFunctions::getNormalize()
{
  return toDegrees(normalize(fromDegrees(normalizeAngle)));
}

double MathFunctions::getFrozenValue()
{
  if (unFreezeValue == 1)
  {
    frozenValue = valueToFreeze;
    unFreezeValue = 0;
  }
  return frozenValue;
}

double MathFunctions::isInInterval()
{
   if (isInIntervalA < isInIntervalB)
   {
        if ((isInIntervalA <= isInIntervalValue) && (isInIntervalValue <= isInIntervalB))
    {
      return 1;
    }
    else
    {
      return 0;
    }
   }
   else 
   {
     if ((isInIntervalB <= isInIntervalValue) && (isInIntervalValue <= isInIntervalA))
     {
       return 1;
     }
     else
     {
       return 0;
     }
   }
}

double MathFunctions::saveValue()
{
  savedValues[(int)(saveindex)%255] = savevalue;

  return 0;
}

double MathFunctions::getValue()
{
  return savedValues[(int)(saveindex)%255];
}

---