Main Page | Namespace List | Class Hierarchy | Alphabetical List | Class List | File List | Namespace Members | Class Members | File Members | Related Pages

# Modules/BehaviorControl/CommonXabsl2Symbols/MathFunctions.cpp

Go to the documentation of this file.

    00001 /**
    00002 * @file MathFunctions.cpp
    00003 *
    00004 * Implementation of class MathFunctions.
    00005 *
    00006 * @author Matthias Jüngel
    00007 */
    00008
    00009 #include "MathFunctions.h"
    00010 #include "Tools/Math/Geometry.h"
    00011
    00012 MathFunctions::MathFunctions(const BehaviorControlInterfaces& interfaces)
    00013 : BehaviorControlInterfaces(interfaces)
    00014 {
    00015 }
    00016
    00017
    00018 void MathFunctions::registerSymbols(Xabsl2Engine& engine)
    00019 {
    00020   // "distance-to"
    00021   engine.registerDecimalInputFunction("distance-to",this,
    00022     (double (Xabsl2FunctionProvider::*)())&MathFunctions::distanceTo);
    00023   engine.registerDecimalInputFunctionParameter("distance-to","distance-to.x",distanceToX);
    00024   engine.registerDecimalInputFunctionParameter("distance-to","distance-to.y",distanceToY);
    00025
    00026   // "angle-to"
    00027   engine.registerDecimalInputFunction("angle-to",this,
    00028     (double (Xabsl2FunctionProvider::*)())&MathFunctions::angleTo);
    00029   engine.registerDecimalInputFunctionParameter("angle-to","angle-to.x",angleToX);
    00030   engine.registerDecimalInputFunctionParameter("angle-to","angle-to.y",angleToY);
    00031
    00032   // "abs"
    00033   engine.registerDecimalInputFunction("abs",this,
    00034     (double (Xabsl2FunctionProvider::*)())&MathFunctions::getAbs);
    00035   engine.registerDecimalInputFunctionParameter("abs","abs.value",absValue);
    00036
    00037   // "min"
    00038   engine.registerDecimalInputFunction("min",this,
    00039     (double (Xabsl2FunctionProvider::*)())&MathFunctions::getMin);
    00040   engine.registerDecimalInputFunctionParameter("min","min.value0",min0Value);
    00041   engine.registerDecimalInputFunctionParameter("min","min.value1",min1Value);
    00042
    00043   // "max"
    00044   engine.registerDecimalInputFunction("max",this,
    00045     (double (Xabsl2FunctionProvider::*)())&MathFunctions::getMax);
    00046   engine.registerDecimalInputFunctionParameter("max","max.value0",max0Value);
    00047   engine.registerDecimalInputFunctionParameter("max","max.value1",max1Value);
    00048
    00049   // "sgn"
    00050   engine.registerDecimalInputFunction("sgn",this,
    00051     (double (Xabsl2FunctionProvider::*)())&MathFunctions::getSgn);
    00052   engine.registerDecimalInputFunctionParameter("sgn","sgn.value",sgnValue);
    00053
    00054   // "sin"
    00055   engine.registerDecimalInputFunction("sin",this,
    00056     (double (Xabsl2FunctionProvider::*)())&MathFunctions::getSin);
    00057   engine.registerDecimalInputFunctionParameter("sin","sin.alpha",alphaValue);
    00058
    00059   // "cos"
    00060   engine.registerDecimalInputFunction("cos",this,
    00061     (double (Xabsl2FunctionProvider::*)())&MathFunctions::getCos);
    00062   engine.registerDecimalInputFunctionParameter("cos","cos.alpha",alphaValue);
    00063
    00064   // "random"
    00065   engine.registerDecimalInputSymbol("random",this,
    00066     (double (Xabsl2FunctionProvider::*)())&MathFunctions::getRandom);
    00067
    00068   // "normalize"
    00069   engine.registerDecimalInputFunction("normalize",this,
    00070     (double (Xabsl2FunctionProvider::*)())&MathFunctions::getNormalize);
    00071   engine.registerDecimalInputFunctionParameter("normalize","normalize.angle",normalizeAngle);
    00072
    00073   // "freeze-value" math.unfreeze-value has to be true (1), before freezing is possible
    00074   engine.registerEnumeratedOutputSymbol("math.unfreeze-value",(int*)&unFreezeValue);
    00075   engine.registerEnumeratedOutputSymbolEnumElement("math.unfreeze-value","true", 1);
    00076
    00077   engine.registerDecimalInputFunction("freeze",this,
    00078     (double (Xabsl2FunctionProvider::*)())&MathFunctions::getFrozenValue);
    00079   engine.registerDecimalInputFunctionParameter("freeze","freeze.value",valueToFreeze);
    00080
    00081   engine.registerDecimalInputFunction("is-in-interval",this,
    00082     (double (Xabsl2FunctionProvider::*)())&MathFunctions::isInInterval);
    00083   engine.registerDecimalInputFunctionParameter("is-in-interval","is-in-interval.a",isInIntervalA);
    00084   engine.registerDecimalInputFunctionParameter("is-in-interval","is-in-interval.b",isInIntervalB);
    00085   engine.registerDecimalInputFunctionParameter("is-in-interval","is-in-interval.value",isInIntervalValue);
    00086
    00087   engine.registerDecimalInputFunction("save-value",this,
    00088     (double (Xabsl2FunctionProvider::*)())&MathFunctions::saveValue);
    00089   engine.registerDecimalInputFunctionParameter("save-value","save-value.index",saveindex);
    00090   engine.registerDecimalInputFunctionParameter("save-value","save-value.value",savevalue);
    00091
    00092   engine.registerDecimalInputFunction("get-value",this,
    00093     (double (Xabsl2FunctionProvider::*)())&MathFunctions::getValue);
    00094   engine.registerDecimalInputFunctionParameter("get-value","get-value.index",saveindex);
    00095
    00096 }
    00097
    00098 void MathFunctions::update()
    00099 {
    00100 }
    00101
    00102 double MathFunctions::distanceTo()
    00103 {
    00104   return Geometry::distanceTo(robotPose,Vector2<double>(distanceToX,distanceToY));
    00105 }
    00106
    00107 double MathFunctions::angleTo()
    00108 {
    00109   return toDegrees(Geometry::angleTo(robotPose,Vector2<double>(angleToX,angleToY)));
    00110 }
    00111
    00112 double MathFunctions::getSgn()
    00113 {
    00114   return sgnValue > 0 ? 1 : -1;
    00115 }
    00116
    00117 double MathFunctions::getAbs()
    00118 {
    00119   return fabs(absValue);
    00120 }
    00121
    00122 double MathFunctions::getMin()
    00123 {
    00124   if (min0Value < min1Value)
    00125     return min0Value;
    00126   return min1Value;
    00127 }
    00128
    00129 double MathFunctions::getMax()
    00130 {
    00131   if (max0Value > max1Value)
    00132     return max0Value;
    00133   return max1Value;
    00134 }
    00135
    00136 double MathFunctions::getSin()
    00137 {
    00138   return sin(fromDegrees(alphaValue));
    00139 }
    00140
    00141 double MathFunctions::getCos()
    00142 {
    00143   return cos(fromDegrees(alphaValue));
    00144 }
    00145
    00146 double MathFunctions::getRandom()
    00147 {
    00148   return ((double) rand() / (RAND_MAX+1.0));
    00149 }
    00150
    00151 double MathFunctions::getNormalize()
    00152 {
    00153   return toDegrees(normalize(fromDegrees(normalizeAngle)));
    00154 }
    00155
    00156 double MathFunctions::getFrozenValue()
    00157 {
    00158   if (unFreezeValue == 1)
    00159   {
    00160     frozenValue = valueToFreeze;
    00161     unFreezeValue = 0;
    00162   }
    00163   return frozenValue;
    00164 }
    00165
    00166 double MathFunctions::isInInterval()
    00167 {
    00168    if (isInIntervalA < isInIntervalB)
    00169    {
    00170         if ((isInIntervalA <= isInIntervalValue) && (isInIntervalValue <= isInIntervalB))
    00171     {
    00172       return 1;
    00173     }
    00174     else
    00175     {
    00176       return 0;
    00177     }
    00178    }
    00179    else
    00180    {
    00181      if ((isInIntervalB <= isInIntervalValue) && (isInIntervalValue <= isInIntervalA))
    00182      {
    00183        return 1;
    00184      }
    00185      else
    00186      {
    00187        return 0;
    00188      }
    00189    }
    00190 }
    00191
    00192 double MathFunctions::saveValue()
    00193 {
    00194   savedValues[(int)(saveindex)%255] = savevalue;
    00195
    00196   return 0;
    00197 }
    00198
    00199 double MathFunctions::getValue()
    00200 {
    00201   return savedValues[(int)(saveindex)%255];
    00202 }
    00203