Методом Нелдера-Мида. Ошибка в коде.

#include <vector>
#include <numeric>
#include <algorithm>
#include <iostream>
#include <cmath>
#define N 3
#define T 1.0
 
using namespace std;
 
struct point {
    double x,y,z;
    point( double cX, double cY, double cZ ) {
        x = cX; y = cY; z = cZ;
    }
    point point::operator +( point &other ) {
        return point( x + other.x, y + other.y, z + other.z );
    }
    point point::operator *( double &other ) {
        return point( x*other, y*other, z*other);
    }
    point point::operator -( point &other ) {
        return point( x - other.x, y - other.y, z - other.z );
    }
    void point::operator =( point &other ) {
        x = other.x;
        y = other.y;
        z = other.z;
    }
    void point::operator +=( point &other ) {
        x += other.x;
        y += other.y;
        z += other.z;
    }
    void Show() {
        cout << x << " " << y << " " << z << endl;
    }
    double Function() {
        return 3 * pow( x - 2, 2 ) + 3 * pow( y + 6, 2 ) + 4 * pow( z + 7, 2 );
    }
 
};
 
double Test(vector<double> vec, double C) {
    double tmp = double();
    for( int i = 0; i < vec.size(); i ++) {
        tmp += pow(vec[i] - C, 2.0);
    }
    return sqrt( ( 1.0 / ((double)N + 1.0) ) * tmp );
}
 
int NotHorL(int I) {
    for( int i = 0; i <= N; i++) {
        if( i != I ) {
            return i;
        }
    }
    return -1;
}
 
int main() {
    vector<point> vectorOfPoints, vectorOfC;
    vector<double> vectorOfFunctions, tmp;
    point R( 0, 0, 0 ), C( 0, 0, 0 ), S( 0, 0, 0 ), E( 0, 0, 0 ), O( 0, 0, 0 );
    double dFh, dFl, temp, eps, alpha = 1, betha = 0.5, gama = 3.0;
    int iL, iH;
 
    
    for(int j = 0; j <=N; j++) {
        for(int k = 0; k < N; k++) {
            if(j == 0) {
                tmp.push_back(0);
                continue;
            }
            if(j == k+1) {
                tmp.push_back( (T / (N * sqrt(2.0))) * ( sqrt( (double)N + 1.0 - 1 + (double)N ) ) );
                continue;
            }
            tmp.push_back( (T / (N * sqrt(2.0))) * ( sqrt( (double)N + 1.0 - 1 ) ) );
        }
    }
 
    for(int i = 0; i < tmp.size(); i++) {
        vectorOfPoints.push_back(point( tmp[i], tmp[i + 1], tmp[i + 2] ) );
        i += 2;
    }
    tmp.clear();
    //
    cout << "eps: ";
    cin >> eps;
    while(true) {
        for(int i = 0; i < vectorOfPoints.size(); i++) {
            vectorOfFunctions.push_back(vectorOfPoints[i].Function());
        }
        
        dFh = *max_element(vectorOfFunctions.begin(), vectorOfFunctions.end());
        dFl = *min_element(vectorOfFunctions.begin(), vectorOfFunctions.end());
        iL = (min_element(vectorOfFunctions.begin(), vectorOfFunctions.end()) - vectorOfFunctions.begin());
        iH = (max_element(vectorOfFunctions.begin(), vectorOfFunctions.end()) - vectorOfFunctions.begin());
 
        for(int i = 0; i < vectorOfPoints.size(); i++) {
            O += vectorOfPoints[i];
        }
 
        temp =  (double)( 1.0 / (double)N );
        C = ( O * temp) - vectorOfPoints[iH];
 
        cout << Test(vectorOfFunctions, C.Function()) << endl;
        if(Test(vectorOfFunctions, C.Function()) < eps) {
            break;
        }
 
        O = C - vectorOfPoints[iH];
        R = C + ( O * alpha );
 
        if(R.Function() < dFl) { //Ветвь 1
            O = R - C;
            E = C + ( O * gama );
 
            if( E.Function() < R.Function() ) {
                vectorOfPoints[iH] = E;
            } else {
                vectorOfPoints[iH] = R;
            }
        } else { // Ветвь 2
            if( R.Function() > vectorOfFunctions[NotHorL(iH)] ) {
                if( R.Function() < vectorOfPoints[iH].Function() ) {
                    vectorOfPoints[iH] = R;
                }
 
                O = vectorOfPoints[iH] - C;
                S = C + ( O * betha );
 
                if( S.Function() < vectorOfFunctions[iH] ) {
                    vectorOfPoints[iH] = S;
                } else {
                    O = vectorOfPoints[NotHorL(iL)] - vectorOfPoints[iL]; temp = 0.5;
                    vectorOfPoints[NotHorL(iL)] = vectorOfPoints[iL] + ( O * temp );
                }
            } else {
                vectorOfPoints[iH] = R;
            }
        }
        vectorOfFunctions.clear();
        O = point( 0, 0, 0 );
        C.Show();
    }
    cout << "Final result: ";
    C.Show();
    return 0;
}

point CalculateC(vector<point> vec, int H) {
    double t;
    point O( 0, 0, 0 );
    for(int i = 0; i < vec.size(); i++) {
        O += vec[i];
    }
    O = O - vec[H];
    t = 1.0 / (double)N;
    return O * t;
}