KatanaKinematicsDecisionAlgorithms.cpp
Go to the documentation of this file.
00001 /***************************************************************************
00002  *   Copyright (C) 2006-2008 by Neuronics AG                               *
00003  *   support@neuronics.ch                                                  *
00004  ***************************************************************************/
00005 
00006 #include "KatanaKinematicsDecisionAlgorithms.h"
00007 #include "MathHelperFunctions.h"
00008 
00009 namespace AnaGuess {
00010 
00011 
00012 KinematicsDefaultEncMinAlgorithm::t_iter
00013 KinematicsDefaultEncMinAlgorithm::operator() (t_iter targetEnc_begin, t_iter targetEnc_end, c_iter currentEnc_begin, c_iter currentEnc_end ) {
00014         double dist(0), sum(0), mindist=1000000;
00015         t_iter index = targetEnc_end;
00016 
00017         for(t_iter target = targetEnc_begin; target != targetEnc_end; ++target) {
00018                 sum = 0;
00019 
00020                 c_iter t = (*target).begin();
00021                 c_iter c = currentEnc_begin;
00022                 while( t != (*target).end() && c != currentEnc_end) {
00023                         sum += MHF::pow2<double>( *t - *c);
00024                         ++t;
00025                         ++c;
00026                 }
00027                 dist = sqrt(sum);
00028                 if(dist < mindist) {
00029                         index = target;
00030                         mindist = dist;
00031                 }
00032         }
00033 
00034         return index;
00035 }
00037 
00038 KinematicsDefaultRadMinAlgorithm::t_iter
00039 KinematicsDefaultRadMinAlgorithm::operator() (t_iter targetRad_begin, t_iter targetRad_end, c_iter currentRad_begin, c_iter currentRad_end ) {
00040         double dist(0), sum(0), mindist=1000000;
00041         t_iter index = targetRad_end;
00042 
00043         for(t_iter target = targetRad_begin; target != targetRad_end; ++target) {
00044                 sum = 0;
00045 
00046                 c_iter t = (*target).begin();
00047                 c_iter c = currentRad_begin;
00048                 while( t != (*target).end() && c != currentRad_end) {
00049                         sum += MHF::pow2<double>( *t - *c);
00050                         ++t;
00051                         ++c;
00052                 }
00053                 dist = sqrt(sum);
00054                 if(dist < mindist) {
00055                         index = target;
00056                         mindist = dist;
00057                 }
00058         }
00059 
00060         return index;
00061 }
00063 
00064 } // namespace


kni
Author(s): Neuronics AG (see AUTHORS.txt); ROS wrapper by Martin Günther
autogenerated on Mon Oct 6 2014 10:45:32