← Back to branch summary

~ubuntu-branches/ubuntu/saucy/python-demgengeo/saucy

« back to all changes in this revision

Viewing changes to sphere_fitting/utils/simplex.hh

  • Committer: Package Import Robot
  • Author(s): Anton Gladky
  • Date: 2011-11-18 21:47:18 UTC
  • Revision ID: package-import@ubuntu.com-20111118214718-4ysqm3dhpqwdd7gd
Tags: upstream-0.99~bzr106
Import upstream version 0.99~bzr106

Show diffs side-by-side

added added

removed removed

Lines of Context:
sphere_fitting/utils/simplex.hh
 
1
/////////////////////////////////////////////////////////////
 
2
//                                                         //
 
3
// Copyright (c) 2007-2011 by The University of Queensland //
 
4
// Earth Systems Science Computational Centre (ESSCC)      //
 
5
// http://www.uq.edu.au/esscc                              //
 
6
//                                                         //
 
7
// Primary Business: Brisbane, Queensland, Australia       //
 
8
// Licensed under the Open Software License version 3.0    //
 
9
// http://www.opensource.org/licenses/osl-3.0.php          //
 
10
//                                                         //
 
11
/////////////////////////////////////////////////////////////
 
12
 
 
13
template<class T,int n>
 
14
simplex_method<T,n>::simplex_method(nfunction<T,n>* f)
 
15
{
 
16
  m_func=f;
 
17
}
 
18
 
 
19
 
 
20
template<class T,int n>
 
21
nvector<T,n> simplex_method<T,n>::reflect(int idx)
 
22
{
 
23
  nvector<T,n> sum=nvector<T,n>(T(0));
 
24
  for(int i=0;i<n+1;i++){
 
25
    if(i!=idx){
 
26
      sum+=m_vert[i];
 
27
    }
 
28
  }
 
29
  sum+=sum;
 
30
  sum=sum/T(n);
 
31
  return sum-m_vert[idx];
 
32
}
 
33
 
 
34
template<class T,int n>
 
35
void simplex_method<T,n>::insert(const nvector<T,n>& vert,T val,int idx)
 
36
{
 
37
  bool up=true;
 
38
  bool down=true;
 
39
 
 
40
  m_vert[idx]=vert;
 
41
  m_val[idx]=val;
 
42
  
 
43
  int i=idx;
 
44
  while(up && (i<n)){
 
45
    if(m_val[i]>m_val[i+1]){
 
46
      up=false;
 
47
    } else {
 
48
      // swap
 
49
      nvector<T,n> h_vert=m_vert[i];
 
50
      T h_val=m_val[i];
 
51
      m_vert[i]=m_vert[i+1];
 
52
      m_val[i]=m_val[i+1];
 
53
      m_vert[i+1]=h_vert;
 
54
      m_val[i+1]=h_val;
 
55
      i++;
 
56
    }
 
57
  }
 
58
  while(down && (i>0)){
 
59
    if(m_val[i]<m_val[i-1]){
 
60
      down=false;
 
61
    } else {
 
62
      // swap
 
63
      nvector<T,n> h_vert=m_vert[i];
 
64
      T h_val=m_val[i];
 
65
      m_vert[i]=m_vert[i-1];
 
66
      m_val[i]=m_val[i-1];
 
67
      m_vert[i-1]=h_vert;
 
68
      m_val[i-1]=h_val;
 
69
      i--;
 
70
    }
 
71
  }
 
72
}
 
73
 
 
74
template<class T,int n>
 
75
void simplex_method<T,n>::shrink()
 
76
{
 
77
  nvector<T,n> center=m_vert[0];
 
78
  for(int i=1;i<n+1;i++){
 
79
    center+=m_vert[i];
 
80
  }
 
81
  center=center/T(n+1);
 
82
  for(int i=0;i<n+1;i++){
 
83
    m_vert[i]=center+(m_vert[i]-center)/T(2);
 
84
    m_val[i]=(*m_func)(m_vert[i]);
 
85
  }
 
86
  sort();
 
87
}
 
88
 
 
89
template<class T,int n>
 
90
void simplex_method<T,n>::sort()
 
91
{
 
92
  for(int i=0;i<n-1;i++){
 
93
    for(int j=i;j<n;j++){
 
94
      if(m_val[j]<m_val[j+1]){
 
95
        nvector<T,n> h_vert=m_vert[j];
 
96
        T h_val=m_val[j];
 
97
        m_vert[j]=m_vert[j+1];
 
98
        m_val[j]=m_val[j+1];
 
99
        m_vert[j+1]=h_vert;
 
100
        m_val[j+1]=h_val;
 
101
      }
 
102
    }
 
103
  }
 
104
}
 
105
 
 
106
template<class T,int n>
 
107
nvector<T,n> simplex_method<T,n>::solve(T lim, const nvector<T,n> & start,int max)
 
108
{
 
109
  int idx;
 
110
  for(int i=0;i<n+1;i++){
 
111
    m_val[i]=T(0);
 
112
  }
 
113
  // set initial simplex vertices
 
114
  insert(start,(*m_func)(start),0);
 
115
  for(int i=0;i<n;i++){
 
116
    nvector<T,n> temp_vert=start+nvector<T,n>::unit(i);
 
117
    T temp_val=(*m_func)(temp_vert);
 
118
    insert(temp_vert,temp_val,i+1);
 
119
  }
 
120
  T eps=m_val[0];
 
121
  int count=0;
 
122
  while((eps>lim)&&((count<max)||(max==-1))){
 
123
    idx=0;
 
124
    bool found=false;
 
125
    while((idx<n+1) && !found){
 
126
      nvector<T,n> temp_vert=reflect(idx);
 
127
      T temp_val=(*m_func)(temp_vert);
 
128
      if(temp_val<m_val[idx]){
 
129
        insert(temp_vert,temp_val,idx);
 
130
        found=true;
 
131
      } else {
 
132
        idx++;
 
133
      }
 
134
    }
 
135
    if(idx==n+1) shrink();
 
136
    //for(int i=0;i<n+1;i++){
 
137
    //  cout << m_vert[i] << " " << m_val[i] << endl;
 
138
    //}
 
139
    //cout << "------------" << endl;
 
140
    eps=m_val[n];
 
141
    count++;
 
142
    //std::cout << "step : " << count << "  eps : " << eps << std::endl;
 
143
  }
 
144
  return m_vert[n]; 
 
145
}