src/InvKin/KatanaKinematicsDecisionAlgorithms.cpp
Go to the documentation of this file.
1 /***************************************************************************
2  * Copyright (C) 2006 by Tiziano Mueller *
3  * tiziano.mueller@neuronics.ch *
4  * *
5  * This program is free software; you can redistribute it and/or modify *
6  * it under the terms of the GNU General Public License as published by *
7  * the Free Software Foundation; either version 2 of the License, or *
8  * (at your option) any later version. *
9  * *
10  * This program is distributed in the hope that it will be useful, *
11  * but WITHOUT ANY WARRANTY; without even the implied warranty of *
12  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the *
13  * GNU General Public License for more details. *
14  * *
15  * You should have received a copy of the GNU General Public License *
16  * along with this program; if not, write to the *
17  * Free Software Foundation, Inc., *
18  * 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA. *
19  ***************************************************************************/
20 
23 
24 namespace KNI {
25 
26 
28 KinematicsDefaultEncMinAlgorithm::operator() (t_iter targetEnc_begin, t_iter targetEnc_end, c_iter currentEnc_begin, c_iter currentEnc_end ) {
29  // assert on distance(tE_beg, tE_enc) != distance(cE_beg, cE_enc)
30  //assert(::std::distance(targetEnc_begin, targetEnc_end) == ::std::distance(currentEnc_begin, currentEnc_end) && "Numbers of target encoders and current encoders provided don't match" );
31  using namespace KNI_MHF;
32  double dist(0), sum(0), mindist=1000000;
33  t_iter index = targetEnc_end;
34 
35  for(t_iter target = targetEnc_begin; target != targetEnc_end; ++target) {
36  sum = 0;
37 
38  c_iter t = (*target).begin();
39  c_iter c = currentEnc_begin;
40  while( t != (*target).end() && c != currentEnc_end) {
41  sum += pow2<double>( *t - *c);
42  ++t;
43  ++c;
44  }
45  dist = sqrt(sum);
46  if(dist < mindist) {
47  index = target;
48  mindist = dist;
49  }
50  }
51 
52  return index;
53 }
54 
55 
56 }
t_iter operator()(t_iter targetEnc_begin, t_iter targetEnc_end, c_iter currentEnc_begin, c_iter currentEnc_end)
Real sum(const BaseMatrix &B)
Definition: newmat.h:2105
Definition: Timer.h:30


kni
Author(s): Martin Günther
autogenerated on Fri Jun 7 2019 22:06:44