lib/kinematics/AnalyticalGuess/src/KatanaKinematicsDecisionAlgorithms.cpp
Go to the documentation of this file.
1 /***************************************************************************
2  * Copyright (C) 2006-2008 by Neuronics AG *
3  * support@neuronics.ch *
4  ***************************************************************************/
5 
6 #include "KatanaKinematicsDecisionAlgorithms.h"
7 #include "MathHelperFunctions.h"
8 
9 namespace AnaGuess {
10 
11 
13 KinematicsDefaultEncMinAlgorithm::operator() (t_iter targetEnc_begin, t_iter targetEnc_end, c_iter currentEnc_begin, c_iter currentEnc_end ) {
14  double dist(0), sum(0), mindist=1000000;
15  t_iter index = targetEnc_end;
16 
17  for(t_iter target = targetEnc_begin; target != targetEnc_end; ++target) {
18  sum = 0;
19 
20  c_iter t = (*target).begin();
21  c_iter c = currentEnc_begin;
22  while( t != (*target).end() && c != currentEnc_end) {
23  sum += MHF::pow2<double>( *t - *c);
24  ++t;
25  ++c;
26  }
27  dist = sqrt(sum);
28  if(dist < mindist) {
29  index = target;
30  mindist = dist;
31  }
32  }
33 
34  return index;
35 }
37 
39 KinematicsDefaultRadMinAlgorithm::operator() (t_iter targetRad_begin, t_iter targetRad_end, c_iter currentRad_begin, c_iter currentRad_end ) {
40  double dist(0), sum(0), mindist=1000000;
41  t_iter index = targetRad_end;
42 
43  for(t_iter target = targetRad_begin; target != targetRad_end; ++target) {
44  sum = 0;
45 
46  c_iter t = (*target).begin();
47  c_iter c = currentRad_begin;
48  while( t != (*target).end() && c != currentRad_end) {
49  sum += MHF::pow2<double>( *t - *c);
50  ++t;
51  ++c;
52  }
53  dist = sqrt(sum);
54  if(dist < mindist) {
55  index = target;
56  mindist = dist;
57  }
58  }
59 
60  return index;
61 }
63 
64 } // namespace
t_iter operator()(t_iter targetEnc_begin, t_iter targetEnc_end, c_iter currentEnc_begin, c_iter currentEnc_end)
t_iter operator()(t_iter targetRad_begin, t_iter targetRad_end, c_iter currentRad_begin, c_iter currentRad_end)
Real sum(const BaseMatrix &B)
Definition: newmat.h:2105


kni
Author(s): Martin Günther
autogenerated on Fri Jan 3 2020 04:01:16