Go to the documentation of this file.00001
00002
00003
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 }