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

# Tools/KickSelectionTable.cpp

Go to the documentation of this file.

    00001 /**
    00002 * @file KickSelectionTable.cpp
    00003 * Implementation of class KickSelectionTable.
    00004 *
    00005 * @author <A href=mailto:juengel@informatik.hu-berlin.de>Matthias Jüngel</A>
    00006 */
    00007
    00008 #ifdef APERIOS1_3_2
    00009 #include "Platform/GTAssert.h"
    00010 #include <stdio.h>
    00011 #include <iostream.h>
    00012 #endif
    00013 #include "KickSelectionTable.h"
    00014 #include "Platform/SystemCall.h"
    00015 #include "Tools/Math/Common.h"
    00016 #include "Tools/Streams/InStreams.h"
    00017 #include "Tools/Debugging/Debugging.h"
    00018 #include "Tools/Location.h"
    00019 #include "Tools/Math/Geometry.h"
    00020 #include "Tools/Debugging/Debugging.h"
    00021 #include "Tools/Debugging/DebugDrawings.h"
    00022
    00023 KickSelectionTable::KickSelectionTable() : numberOfSectors(12)
    00024 {
    00025   memset(action,0,sizeof(action));
    00026 }
    00027
    00028 KickSelectionTable::~KickSelectionTable()
    00029 {
    00030 }
    00031
    00032 KickSelectionTable::KickSelectionTableID KickSelectionTable::getTableIDFromName(const char* name)
    00033 {
    00034   for (int i=0;i<numberOfKickSelectionTableIDs;i++)
    00035   {
    00036     if (strcmp(name,getKickSelectionTableIDName((KickSelectionTableID)i))==0)
    00037     {
    00038       return (KickSelectionTableID)i;
    00039     }
    00040   }
    00041
    00042   return inCenterOfField;
    00043 }
    00044
    00045 KickSelectionTable::ActionID KickSelectionTable::getActionIDFromShortName(const char* name)
    00046 {
    00047   for (int i=0;i<numberOfActions;i++)
    00048   {
    00049     if (strcmp(name,getShortActionName((ActionID)i))==0)
    00050     {
    00051       return (ActionID)i;
    00052     }
    00053   }
    00054
    00055   return nothing;
    00056 }
    00057
    00058 void KickSelectionTable::load(const char* fileName)
    00059 {
    00060   InTextFile f(getLocation().getModelFilename(fileName));
    00061   if (f.exists())
    00062     f >> *this;
    00063
    00064   //OutTextFile f2(getLocation().getModelFilename("test.kst"));
    00065   //f2 << *this;
    00066 }
    00067
    00068 int KickSelectionTable::getSectorFromAngle(double destinationAngle) const {
    00069   return (int)((normalize(fromDegrees(destinationAngle) + pi + pi2 / KickSelectionTable::numberOfSectors / 2) + pi) / pi2 * KickSelectionTable::numberOfSectors);
    00070 }
    00071
    00072 Vector2<double> KickSelectionTable::searchKickInNeighbourhood(
    00073   double ballOffsetX, double ballOffsetY,
    00074   double destinationAngle, KickSelectionTableID kickSelectionTableID,
    00075   RobotPose robotPose, double maxDistance, double& kick
    00076   ) const
    00077 {
    00078 //  Vector2<double> ballAbsolute = Geometry::relative2FieldCoord(robotPose,ballOffsetX,ballOffsetY);
    00079 //  robotPose.translation.x=ballAbsolute.x; // hack to get ball position on field with robot rotation.
    00080 //  robotPose.translation.y=ballAbsolute.y;
    00081   int currx=(int)(ballOffsetX / 10.0);
    00082   int curry=(int)(ballOffsetY / 10.0) + KickSelectionTable::yRange / 2;
    00083   int sector=getSectorFromAngle(destinationAngle);
    00084   Vector2<double> found=Vector2<double>(10000,10000);
    00085 //  Vector2<double> founddxdy=Vector2<double>(10000,10000);
    00086   int distance=0;
    00087   int dx=0;
    00088   while (distance<maxDistance){
    00089     for (int dy=-dx; dy<=dx; dy++){
    00090 //      #ifdef APERIOS1_3_2
    00091 //      cout << "search\n";
    00092 //      #endif
    00093       int x=currx-dx;  // ball verschieben, wie es sich verhalten würde, wenn wir den roboter um dx,dy bewegen
    00094       int y=curry-dy;
    00095
    00096       // roboter auch verschieben, um den punkt zu erhalten, wo der roboter sich hinbewegen müßte (absolut für debug drawings)
    00097       Vector2<double> testPoint = Geometry::relative2FieldCoord(robotPose,dx*10,dy*10);
    00098
    00099       //checken, ob der neue virtuelle ballpunkt noch in der kicktable ist.
    00100       if (x >= 0 && x < KickSelectionTable::xRange && y>=0 && y<KickSelectionTable::yRange){
    00101         CROSS(selfLocatorField,testPoint.x,testPoint.y,4,1,0,Drawings::skyblue);
    00102 //        OUTPUT(idText,text,"Search x="<<x<<" y="<<y<<" sec="<<sector<<" ==> "<<(int)action[x][y][sector][kickSelectionTableID]);
    00103         //checken, ob am virtuellen ballpunkt ein kick möglich ist
    00104         if (action[x][y][sector][kickSelectionTableID]!= nothing){
    00105           // korrekturvektor speichern
    00106           Vector2<double> current= Vector2<double>(dx*10,dy*10);
    00107           //checken, ob korrekturvektor in maximaldistanz und wir bisher keinen besseren kennen
    00108           if ((current.abs()< maxDistance) && (current.abs() < found.abs())){
    00109             // wenn dem so ist, neuen korrekturvektor speichern und debug drawing malen
    00110             CROSS(selfLocatorField,testPoint.x,testPoint.y, 4, 1, 0, Drawings::blue);
    00111
    00112 //            OUTPUT(idText,text,"Found: dx="<<current.x<<" dy="<<current.y);
    00113             found=current;
    00114             kick=action[x][y][sector][kickSelectionTableID];
    00115
    00116 //            founddxdy=Vector2<double>(dx,dy);
    00117           }
    00118         }
    00119       } else {
    00120         CROSS(selfLocatorField,testPoint.x,testPoint.y,4,1,0,Drawings::black);
    00121       }
    00122     }
    00123
    00124
    00125     dx++;
    00126     distance = (int) (Vector2<double>(-dx*10,0)).abs();
    00127   }
    00128   Vector2<double> finalPoint = Geometry::relative2FieldCoord(robotPose,found.x,found.y);
    00129   CIRCLE(selfLocatorField,finalPoint.x,finalPoint.y, 10, 10, 0, Drawings::blue);
    00130   DEBUG_DRAWING_FINISHED(selfLocatorField);
    00131   return found;
    00132
    00133 }
    00134
    00135 KickSelectionTable::ActionID KickSelectionTable::retrieveKick
    00136 (
    00137  double ballOffsetX, double ballOffsetY,
    00138  double destinationAngle,
    00139  double destinationAngleWidth,
    00140  KickSelectionTableID kickSelectionTableID
    00141 ) const
    00142 {
    00143   int x,y;
    00144   x = (int)(ballOffsetX / 10.0);
    00145   y = (int)(ballOffsetY / 10.0) + KickSelectionTable::yRange / 2;
    00146
    00147   if(x < 0 || x >= KickSelectionTable::xRange ||
    00148     y < 0 || y >= KickSelectionTable::yRange)
    00149     return nothing;
    00150
    00151   int minSector = getSectorFromAngle(destinationAngle - destinationAngleWidth / 2.0);
    00152   int maxSector = getSectorFromAngle(destinationAngle + destinationAngleWidth / 2.0) + 1;
    00153
    00154   if (maxSector > numberOfSectors) maxSector -= numberOfSectors;
    00155   if (maxSector == 0) maxSector = numberOfSectors;
    00156
    00157   KickSelectionTable::ActionID selectedAction = nothing;
    00158   double minAngleDist = pi;
    00159   for (int i = minSector; i != maxSector; i++)
    00160   {
    00161     if (i >= numberOfSectors) i -= numberOfSectors;
    00162     KickSelectionTable::ActionID sectorAction;
    00163     sectorAction = action[x][y][i][kickSelectionTableID];
    00164     if (sectorAction != nothing)
    00165     {
    00166       double centerAngle = normalize(pi * ((double)i) / ((double)numberOfSectors));
    00167       double angleDist = fabs(normalize(fromDegrees(destinationAngle) - centerAngle));
    00168       if (angleDist < minAngleDist)
    00169       {
    00170         selectedAction = sectorAction;
    00171         minAngleDist = angleDist;
    00172       }
    00173     }
    00174   }
    00175
    00176   return selectedAction;
    00177 }
    00178
    00179 In& operator>>(In& stream, KickSelectionTable& kickSelectionTable)
    00180 {
    00181   KickSelectionTable::KickSelectionTableID tableID;
    00182   KickSelectionTable::ActionID actionID;
    00183   int x,y,s;
    00184   char buf[200];
    00185   memset(kickSelectionTable.action,0,sizeof(kickSelectionTable.action));
    00186
    00187   kickSelectionTable.numberOfSectors = 0;
    00188   while (1)
    00189   {
    00190     stream >> buf;
    00191     if (strcmp(buf,"___")==0) break;
    00192     tableID = kickSelectionTable.getTableIDFromName(buf);
    00193     while(1)
    00194     {
    00195       stream >> buf;
    00196       if (strcmp(buf,"___")==0) break;
    00197
    00198       x = atoi(buf);
    00199
    00200       stream >> y >> s >> buf;
    00201       if (s >= kickSelectionTable.numberOfSectors)
    00202       {
    00203         kickSelectionTable.numberOfSectors = s + 1;
    00204         ASSERT(kickSelectionTable.numberOfSectors <= KickSelectionTable::maxNumberOfSectors);
    00205       }
    00206       actionID = kickSelectionTable.getActionIDFromShortName(buf);
    00207       kickSelectionTable.action[x][y][s][tableID] = actionID;
    00208     }
    00209   }
    00210   return stream;
    00211 }
    00212
    00213 Out& operator<<(Out& stream, const KickSelectionTable& kickSelectionTable)
    00214 {
    00215   int t,x,y,s;
    00216   for (t=0; t<KickSelectionTable::numberOfKickSelectionTableIDs; t++)
    00217   {
    00218     stream << kickSelectionTable.getKickSelectionTableIDName((KickSelectionTable::KickSelectionTableID)t);
    00219
    00220     for (x = 0; x< kickSelectionTable.xRange; x++)
    00221     {
    00222       for (y = 0; y< kickSelectionTable.yRange; y++)
    00223       {
    00224         for (s = 0; s< kickSelectionTable.numberOfSectors; s++)
    00225         {
    00226           /*if (kickSelectionTable.action[x][y][s][t] == KickSelectionTable::nothing &&
    00227               kickSelectionTable.action[x][y][(s+1) % kickSelectionTable.numberOfSectors ][t] != KickSelectionTable::nothing &&
    00228               kickSelectionTable.action[x][y][(s+1) % kickSelectionTable.numberOfSectors ][t] ==
    00229                 kickSelectionTable.action[x][y][(s+kickSelectionTable.numberOfSectors-1) % kickSelectionTable.numberOfSectors ][t]
    00230               )
    00231           {
    00232             stream << x << y << s << kickSelectionTable.getShortActionName(
    00233               kickSelectionTable.action[x][y][(s+1) % kickSelectionTable.numberOfSectors ][t]);
    00234           }
    00235           else
    00236           */
    00237           {
    00238             if (kickSelectionTable.action[x][y][s][t] != KickSelectionTable::nothing ||
    00239                 // always write one entry from last sector, so number of sectors can be determined correctly
    00240                 (x==0 && y == 0 && s == kickSelectionTable.numberOfSectors-1 && t == 0))
    00241             {
    00242               stream << x << y << s << kickSelectionTable.getShortActionName(
    00243                 kickSelectionTable.action[x][y][s][t]);
    00244             }
    00245           }
    00246         }
    00247       }
    00248     }
    00249     stream << "___";
    00250   }
    00251   stream << "___";
    00252   return stream;
    00253 }
    00254
    00255 In& operator>>(In& stream,KickCase& kickCase)
    00256 {
    00257   stream.read(&kickCase,sizeof(KickCase));
    00258   return stream;
    00259 }
    00260
    00261 Out& operator<<(Out& stream, const KickCase& kickCase)
    00262 {
    00263   stream.write(&kickCase,sizeof(KickCase));
    00264   return stream;
    00265 }