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);

}

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;
     }
   }
}


/*
* Change Log
* 
* $Log: MathFunctions.cpp,v $
* Revision 1.1.1.1  2004/05/22 17:16:58  cvsadm
* created new repository GT2004_WM
*
* Revision 1.5  2004/04/08 15:33:01  wachter
* GT04 checkin of Microsoft-Hellounds
*
* Revision 1.4  2004/03/16 14:00:21  juengel
* Integrated Improvments from "Günne"
* -ATH2004ERS7Behavior
* -ATHHeadControl
* -KickSelectionTable
* -KickEditor
*
* Revision 1.3  2004/03/08 00:58:56  roefer
* Interfaces should be const
* Revision 1.2  2004/03/11 17:32:20  loetzsch
* added input function "angle-to"
*
* Revision 1.2  2004/01/08 13:51:22  loetzsch
* added input function sin and cos
*
* Revision 1.1  2003/10/22 22:18:44  loetzsch
* prepared the cloning of the GT2003BehaviorControl
*
* Revision 1.1  2003/10/06 13:39:28  cvsadm
* Created GT2004 (M.J.)
*
* Revision 1.2  2003/07/02 17:48:01  risler
* added math function abs
*
* Revision 1.1.1.1  2003/07/02 09:40:23  cvsadm
* created new repository for the competitions in Padova from the 
* tamara CVS (Tuesday 2:00 pm)
*
* removed unused solutions
*
* Revision 1.3  2003/06/24 10:08:23  risler
* added function normalize
*
* Revision 1.2  2003/06/19 14:48:24  risler
* symbol random added
*
* Revision 1.1  2003/05/07 17:54:39  juengel
* Added missing symbols to GT2003BehaviorControl.
*
*/

---