#ifndef TESTFUN_H
#define TESTFUN_H

#include "linalg.h"
#include "tools.h"
#include "config.h"

const double pi=fabs(acos(-1.));

// Rosenbrock function
void Domain_Rosenbrock(RTBox box) {
  box.lb=-2.0 ; box.ub=2.0 ;
}

double Objective_Rosenbrock(RCRVector x) {
   double a=x(1)-x(0)*x(0) ;
   double b=1-x(0) ;
   return 100*a*a + b*b ;
}

void Gradient_Rosenbrock(RCRVector x, RCRVector grad) {
  grad(0)=200*(x(1)-x(0)*x(0))*(-2*x(0))-2*(1-x(0)) ;
  grad(1)=200*(x(1)-x(0)*x(0)) ;
}

// McCormic function
void Domain_McCormick(RTBox box) {
  box.lb(0)=-1.5 ; box.ub(0)=4.0 ;
  box.lb(1)=-3.0 ; box.ub(1)=4.0 ;
}

double Objective_McCormick(RCRVector x) {
  return sin(x(0)+x(1)) + pow(x(0)-x(1),2.0) - 1.5*x(0) + 2.5*x(1) + 1.0 ;
}

void Gradient_McCormick(RCRVector x, RCRVector grad) {
  grad(0)=cos(x(0)+x(1)) + 2*(x(0)-x(1)) - 1.5 ;
  grad(1)=cos(x(0)+x(1)) - 2*(x(0)-x(1)) + 2.5 ;
}

// Box and Betts function
void Domain_BoxBetts(RTBox box) {
  box.lb(0)=0.9 ; box.ub(0)=1.2 ;
  box.lb(1)=9.0 ; box.ub(1)=11.2 ;
  box.lb(2)=0.9 ; box.ub(2)=1.2 ;
}

double Objective_BoxBetts(RCRVector x) {
  double x0=x(0),x1=x(1),x2=x(2) ;
  double sum=0.0 ;
  for (int i=1 ; i<=10 ; i++)
    sum+=pow(exp(-0.1*i*x0)-exp(-0.1*i*x1)-(exp(-0.1*i)-exp(-1.0*i))*x2,2.0);
  return sum ;
}

void Gradient_BoxBetts(RCRVector x, RCRVector grad) {
  double x0=x(0),x1=x(1),x2=x(2) ;
  double g0=0.0, g1=0.0, g2=0.0 ;
  for (int i=1 ; i<=10 ; i++) {
    g0 += -0.2*(exp(-0.1*i*x0)-exp(-0.1*i*x1)
	  -(exp(-0.1*i)-exp(-1.0*i))*x2)*i*exp(-0.1*i*x0);
    g1 += 0.2*(exp(-0.1*i*x0)-exp(-0.1*i*x1)-(exp(-0.1*i)
	  -exp(-1.0*i))*x2)*i*exp(-0.1*i*x1);
    g2 += 2.0*(exp(-0.1*i*x0)-exp(-0.1*i*x1)
	  -(exp(-0.1*i)-exp(-1.0*i))*x2)*(-exp(-0.1*i)+exp(-1.0*i));
  }
  grad(0)=g0 ; grad(1)=g1 ; grad(2)=g2 ;
}

// Paviani function
void Domain_Paviani(RTBox box) {
 box.lb=2.001 ; box.ub=9.999 ;
}

double Objective_Paviani(RCRVector x) { 
  double a,b,sum=0.0, mul=1.0 ;
  int n=x.GetLength() ;
  for (int i=0 ; i<n ; i++) {
    a=log(x(i)-2.0) ; b=log(10.0-x(i)) ;
    sum+= a*a + b*b ;
    mul*= x(i) ;
  }
  return sum - pow(mul,0.2) ;
}

void Gradient_Paviani(RCRVector x, RCRVector grad) {
  double sum, mul=1.0 ;
  int n=10 ;
  for (int i=0 ; i<n ; i++) {
    mul*= x(i) ;
  }

  for (int j=0 ; j<n ; j++) {
    sum=2*log(x(j)-2.0)/(x(j)-2.0) - 2*log(10.0-x(j))/(10.0-x(j)) ;
    grad(j) = sum - 0.2*(mul/x(j))/pow(mul,0.8) ;
  }
}

// Generalized Rosenbrock function
void Domain_Generalized_Rosenbrock(RTBox box) {
  box.lb=-30.0 ; box.ub=30.0 ;
}

double Objective_Generalized_Rosenbrock(RCRVector x) {
  double s = 0.0;
  int i, n=x.GetLength();
  for( i = 1; i < n; i++ ) 
    s += 100.0*pow(x(i)-x(i-1)*x(i-1),2) + pow(x(i-1)-1.0,2);
   return s;
}

void Gradient_Generalized_Rosenbrock(RCRVector x, RCRVector grad) {
  int i, n=x.GetLength();
  grad(0) = -400.0*(x(1)-x(0)*x(0))*x(0)+2.0*(x(1)-1.0);
  for( i = 1; i < n-1; i++ ) 
    grad(i) = -400.0*(x(i+1)-x(i)*x(i))*x(i)+2.0*(x(i+1)-1.0) +
      200.0*(x(i)-x(i-1)*x(i-1));
  grad(n-1)=200.0*(x(n-1)-x(n-2)*x(n-2));
}

// Glodstein and Price function
void Domain_Goldstein_Price(RTBox box) {
  box.lb=-2.0 ; box.ub=2.0 ;
}

double Objective_Goldstein_Price(RCRVector x) {
  return (1.0+pow(x(0)+x(1)+1.0,2)*
    (19.0-14.0*x(0)+3.0*x(0)*x(0)-14.0*x(1)+6.0*x(0)*x(1)+3.0*x(1)*x(1)))*
    (30.0+pow(2.0*x(0)-3.0*x(1),2)*
    (18.0-32.0*x(0)+12.0*x(0)*x(0)+48.0*x(1)-36.0*x(0)*x(1)+27.0*x(1)*x(1)));
}

void Gradient_Goldstein_Price(RCRVector x, RCRVector grad) {
  // Numerical calculations of gradients
  const double step = 1e-6;
  double F, Fplus;
  int i;

  RCRVector xlocal(2);
  for(i = 0; i <= 1; i++ ) xlocal(i) = x(i);
  F = Objective_Goldstein_Price( xlocal ); 
  for(i = 1 ; i>=0; --i) {
	xlocal(i) += step;
	Fplus = Objective_Goldstein_Price( xlocal );
	xlocal(i) -= step; // original x value
	grad(i) = (Fplus-F)/step;
  }
}

// Shekel functions
/* The Matrix a and vector c are needed in the Shekel function */
static double a[10][4]={ { 4,4,4,4 } ,
			 { 1,1,1,1 } ,
			 { 8,8,8,8 } ,
			 { 6,6,6,6 } ,
			 { 3,7,3,7 } ,
			 { 2,9,2,9 } ,
			 { 5,5,3,3 } ,
			 { 8,1,8,1 } ,
			 { 6,2,6,2 } ,
			 {7,3.6,7,3.6} };
static double c[10]= { .1 , .2 , .2 , .4 , .4 , .6 , .3, .7 , .5 , .5 };

void Domain_Shekel(RTBox box) {
  box.lb=0.0 ; box.ub=10.0 ;
}

double Objective_Shekel(RCRVector x) {
  int n=x.GetLength() ;
  double R=0.0, S;
  for(int i=0;i<10;i++) {
    S=0;
    for(int j=0;j<n;j++) S+=pow(x(j)-a[i][j],2);
    R-=1/(S+c[i]);
  }
  return R;
}

void Gradient_Shekel(RCRVector x, RCRVector grad) {
  int n=x.GetLength() ;
  double R;
  for(int k=0;k<n;k++) {
    R=0.0;
    for(int i=0;i<10;i++) {
      R+=(2.0*x(k)-2.0*a[i][k])/(pow(pow(x(0)-a[i][0],2.0)+pow(x(1)-a[i][1],2.0)
                                +pow(x(2)-a[i][2],2.0)+pow(x(3)-a[i][3],2.0)+c[i],2.0));
    }
    grad(k)=R;
  }
}

double Objective_Shekel5(RCRVector x) {
  int n=x.GetLength() ;
  double R=0.0, S;
  for(int i=0;i<5;i++) {
    S=0;
    for(int j=0;j<n;j++) S+=pow(x(j)-a[i][j],2);
    R-=1/(S+c[i]);
  }
  return R;
}

void Gradient_Shekel5(RCRVector x, RCRVector grad) {
  int n=x.GetLength() ;
  double R;
  for(int k=0;k<n;k++) {
    R=0.0;
    for(int i=0;i<5;i++) {
      R+=(2.0*x(k)-2.0*a[i][k])/(pow(pow(x(0)-a[i][0],2.0)+pow(x(1)-a[i][1],2.0)
                                +pow(x(2)-a[i][2],2.0)+pow(x(3)-a[i][3],2.0)+c[i],2.0));
    }
    grad(k)=R;
  }
}

double Objective_Shekel7(RCRVector x) {
  int n=x.GetLength() ;
  double R=0.0, S;
  for(int i=0;i<7;i++) {
    S=0;
    for(int j=0;j<n;j++) S+=pow(x(j)-a[i][j],2);
    R-=1/(S+c[i]);
  }
  return R;
}

void Gradient_Shekel7(RCRVector x, RCRVector grad) {
  int n=x.GetLength() ;
  double R;
  for(int k=0;k<n;k++) {
    R=0.0;
    for(int i=0;i<7;i++) {
      R+=(2.0*x(k)-2.0*a[i][k])/(pow(pow(x(0)-a[i][0],2.0)+pow(x(1)-a[i][1],2.0)
                                +pow(x(2)-a[i][2],2.0)+pow(x(3)-a[i][3],2.0)+c[i],2.0));
    }
    grad(k)=R;
  }
}

// Levy functions
void Domain_Levy(RTBox box) {
  int n=(box.lb).GetLength() ;
  switch (n) {
  case 4 :
    box.lb=-10.0 ; box.ub=10.0 ;
    break ;
  default:
    box.lb=-5.0 ; box.ub=5.0 ;
  }
}

double Objective_Levy(RCRVector x) {
  int n=x.GetLength();
  double sum=0.0;

  for (int i=0 ; i<=n-2 ; i++)
    sum+=pow(x(i)-1,2.0)*(1+pow(sin(3*pi*x(i+1)),2.0));
  return pow(sin(3*pi*x(0)),2.0) + sum + (x(n-1)-1)*(1+pow(sin(2*pi*x(n-1)),2.0));
}


void Gradient_Levy(RCRVector x, RCRVector grad) {
  int n=x.GetLength();

  grad(0)=6*sin(3*pi*x(0))*cos(3*pi*x(0))*pi + 2*(x(0)-1)*(1+pow(sin(3*pi*x(1)),2.0));

  for (int i=1 ; i<=n-2 ; i++)
    grad(i)=6*pow(x(i-1)-1,2.0)*sin(3*pi*x(i))*cos(3*pi*x(i))*pi
      + 2*(x(i)-1)*(1+pow(sin(3*pi*x(i+1)),2.0)) ;

  grad(n-1)=6*pow(x(n-2)-1,2.0)*sin(3*pi*x(n-1))*cos(3*pi*x(n-1))*pi
    + 1 + pow(sin(2*pi*x(n-1)),2.0) + 4*(x(n-1)-1)*sin(2*pi*x(n-1))*cos(2*pi*x(n-1))*pi;
}

// Griewank function
void Domain_Griewank(RTBox box) {
  box.lb=-500 ; box.ub=700 ;
}

double Objective_Griewank(RCRVector x) {
  double sum=0 ;
  double prod=1 ;
  for (int i=0 ; i<10 ; i++) {
    sum+=x(i)*x(i) ;
    prod*=cos(x(i)/sqrt(double(i+1))) ;
  }
  return sum/4000.0-prod + 1 ;
}

void Gradient_Griewank(RCRVector x, RCRVector grad) {
  double prod=1 ;
  for (int i=0 ; i<10 ; i++) {
    prod*=cos(x(i)/sqrt(double(i+1))) ;
  }
  for (int i=0 ; i<10 ; i++) {
    grad(i)=x(i)/2000.0 + 1/sqrt(double(i+1))
           *prod/cos(x(i)/sqrt(double(i+1)))*sin(x(i)/sqrt(double(i+1))) ;
  }
}

// Six Hump Camel Back function
void Domain_Camel6(RTBox box) {
 box.lb=-5.0 ; box.ub=5.0 ;
}

double Objective_Camel6(RCRVector x) {
  double x1=x(0),x2=x(1) ;
  return 4.0*x1*x1-0.21E1*pow(x1,4.0)+pow(x1,6.0)/3+x1*x2-4.0*x2*x2 
    + 4.0*pow(x2,4.0);
}

void Gradient_Camel6(RCRVector x, RCRVector grad) {
  double x1=x(0),x2=x(1) ;
  grad(0) = 8.0*x1-0.84E1*x1*x1*x1+2.0*pow(x1,5.0)+x2;
  grad(1) = x1-8.0*x2+16.0*x2*x2*x2;
}

// Branin function
void Domain_Branin(RTBox box) {
  box.lb=-10.0 ; box.ub=10.0 ;
}

double Objective_Branin(RCRVector x) {
  return pow(1.0-2.0*x(1)+sin(4.0*pi*x(1))/20.0-x(0),2)+
	pow(x(1)-sin(2.0*pi*x(0))/2.0,2);
}

void Gradient_Branin(RCRVector x, RCRVector grad) {
  grad(0) = -2.0*(1.0-2.0*x(1)+sin(4.0*pi*x(1))/20.0-x(0))-
	2.0*(x(1)-sin(2.0*pi*x(0))/2.0)*cos(2.0*pi*x(0))*pi;
  grad(1) = 2.0*(1.0-2.0*x(1)+sin(4.0*pi*x(1))/20.0-x(0))*
	(-2.0+cos(4.0*pi*x(1))*pi/5.0)+ 2.0*(x(1)-sin(2.0*pi*x(0))/2.0);
}

// Shubert function
void Domain_Shubert(RTBox box) {
  box.lb=-10.0 ; box.ub=10.0 ;
}

double Objective_Shubert(RCRVector x) {
   return -sin(2.0*x(0)+1.0)-2.0*sin(3.0*x(0)+2.0)-3.0*sin(4.0*x(0)+3.0)
          -4.0*sin(5.0*x(0)+4.0)-5.0*sin(6.0*x(0)+5.0)-sin(2.0*x(1)+1.0)
          -2.0*sin(3.0*x(1)+2.0)-3.0*sin(4.0*x(1)+3.0)-4.0*sin(5.0*x(1)+4.0)
          -5.0*sin(6.0*x(1)+5.0);
}

void Gradient_Shubert(RCRVector x, RCRVector grad) {
   grad(0) = -2.0*cos(2.0*x(0)+1.0)-6.0*cos(3.0*x(0)+2.0)-12.0*cos(4.0*x(0)+3.0)
             -20.0*cos(5.0*x(0)+4.0)-30.0*cos(6.0*x(0)+5.0);
   grad(1) = -2.0*cos(2.0*x(1)+1.0)-6.0*cos(3.0*x(1)+2.0)-12.0*cos(4.0*x(1)+3.0)
             -20.0*cos(5.0*x(1)+4.0)-30.0*cos(6.0*x(1)+5.0);
}

// Hansen function
void Domain_Hansen(RTBox box) {
  box.lb=-10.0 ; box.ub=10.0 ;
}

double Objective_Hansen(RCRVector x) {
  return (cos(1.0)+2.0*cos(x(0)+2.0)+3.0*cos(2.0*x(0)+3.0)+4.0*cos(3.0*x(0)
        +4.0)+5.0*cos(4.0*x(0)+5.0))*(cos(2.0*x(1)+1.0)+2.0*cos(3.0*x(1)+2.0)   
        +3.0*cos(4.0*x(1)+3.0)+4.0*cos(5.0*x(1)+4.0)+5.0*cos(6.0*x(1)+5.0));
}

void Gradient_Hansen(RCRVector x, RCRVector grad) {
   grad(0) = (-2.0*sin(x(0)+2.0)-6.0*sin(2.0*x(0)+3.0)-12.0*sin(3.0*x(0)+4.0)
           -20.0*sin(4.0*x(0)+5.0))*(cos(2.0*x(1)+1.0)+2.0*cos(3.0*x(1)+2.0)
           +3.0*cos(4.0*x(1)+3.0)+4.0*cos(5.0*x(1)+4.0)+5.0*cos(6.0*x(1)+5.0));

   grad(1) = (cos(1.0)+2.0*cos(x(0)+2.0)+3.0*cos(2.0*x(0)+3.0)+4.0*cos(3.0*x(0)
             +4.0)+5.0*cos(4.0*x(0)+5.0))*(-2.0*sin(2.0*x(1)+1.0)
             -6.0*sin(3.0*x(1)+2.0)-12.0*sin(4.0*x(1)+3.0)
             -20.0*sin(5.0*x(1)+4.0)-30.0*sin(6.0*x(1)+5.0));
}

// Cola function
void Domain_Cola(RTBox box) {
  int i;
  box.lb(0)=0; box.ub(0)=4.0;
  for( i = 1; i < 17; i++ ) {
    box.lb(i)=-4.0; box.ub(i)=4.0; }
}

double sqr( double x )
{
  return x*x;
}

double dis( int k )
{
  double d_mds[46];
  d_mds[1]=1.27;
  d_mds[2]=1.69;d_mds[3]=1.43;
  d_mds[4]=2.04;d_mds[5]=2.35;d_mds[6]=2.43;
  d_mds[7]=3.09;d_mds[8]=3.18;d_mds[9]=3.26;d_mds[10]=2.85;
  d_mds[11]=3.20;d_mds[12]=3.22;d_mds[13]=3.27;d_mds[14]=2.88;d_mds[15]=1.55;
  d_mds[16]=2.86;d_mds[17]=2.56;d_mds[18]=2.58;d_mds[19]=2.59;d_mds[20]=3.12;d_mds[21]=3.06;
  d_mds[22]=3.17;d_mds[23]=3.18;d_mds[24]=3.18;d_mds[25]=3.12;d_mds[26]=1.31;d_mds[27]=1.64;d_mds[28]=3.00;
  d_mds[29]=3.21;d_mds[30]=3.18;d_mds[31]=3.18;d_mds[32]=3.17;d_mds[33]=1.70;d_mds[34]=1.36;d_mds[35]=2.95;d_mds[36]=1.32;
  d_mds[37]=2.38;d_mds[38]=2.31;d_mds[39]=2.42;d_mds[40]=1.94;d_mds[41]=2.85;d_mds[42]=2.81;d_mds[43]=2.56;d_mds[44]=2.91;d_mds[45]=2.97;
  return d_mds[k];
}

double Objective_Cola( RCRVector x )
{
  double sum = 0.0, temp;
  int i, j, t, k = 1;
  RCRVector mt( 20 );
  mt(0) = 0; mt(1) = 0; mt(2) = x(0); mt(3) = 0;
  for( i = 4; i < 20; i++ ) mt(i) = x(i-3);
  for( i = 1; i < 10; i++ )
    for( j = 0; j < i; j++ ) {
      temp = 0.0;
      for( t = 0; t < 2; t++ )
        temp += sqr( mt(i*2+t)-mt(j*2+t) );
      sum += sqr( dis( k ) - sqrt( temp ) ); 
      k++; }
//  cout << "f(" << mt << ") = " << sum << endl;
  return sum;
}

void Gradient_Cola(RCRVector x, RCRVector grad) {

  // Numerical calculations of gradients
  const double step = 1e-6;

  double Fminus, Fplus;
  int i;

  RCRVector xlocal(17);
  for(i = 0; i < 17; i++ ) xlocal(i) = x(i);
  for(i = 16 ; i>=0; --i) {
	xlocal(i) -= step;
	Fminus = Objective_Cola(xlocal);
	xlocal(i) += 2.0*step;
	Fplus = Objective_Cola(xlocal);
	xlocal(i) -= step; // original x value
	grad(i) = (Fplus-Fminus)/(2.0*step);
  }

}

// Fiting problem
float duom[280][10];

void Domain_myMinHeight(RTBox box) {
  box.lb(0)=-pi; box.ub(0)=pi;
  box.lb(1)=-pi; box.ub(1)=pi;
  box.lb(2)=-pi; box.ub(2)=pi;
  box.lb(3)=-120; box.ub(3)=120;
  box.lb(4)=-120; box.ub(4)=120;
  box.lb(5)=-120; box.ub(5)=120;
  box.lb(6)=-pi; box.ub(6)=pi;
  box.lb(7)=-pi; box.ub(7)=pi;
  box.lb(8)=-pi; box.ub(8)=pi;
  box.lb(9)=-120; box.ub(9)=120;
  box.lb(10)=-120; box.ub(10)=120;
  box.lb(11)=-120; box.ub(11)=120;
  FILE *fin = fopen( "links321.dat", "rt" );
  if( fin == NULL ) {
    cout << "File not found" << endl;
    exit( 1 ); }
  int i, j;
  for( i = 1; i <= 271; i++ )
    for( j = 1; j <= 9; j++ )
      fscanf( fin, "%f", &duom[i][j] );
  fclose( fin );

}

double Objective_myMinHeight( RCRVector x )
{
  double FN = (double) 0.0;
  int i, k;

  double a0[3], b0[3], c0[3];
  double a,b,c,s, area, powerArea, height;
  double R1[3][3]; // R1 is the rotation of the first dataset
  double t1[3]; // translation of the first dataset

  double R2[3][3]; // R2 is the rotation of the third dataset
  double t2[3]; // translation of the third dataset

  double p1[3], p2[3], p3[3]; //, meanPoint(3);
  
  const double  omega1  =  x( 0);
  const double  phi1    =  x( 1);
  const double  kappa1  =  x( 2);
  const double  t1x     =  x( 3);
  const double  t1y     =  x( 4);
  const double  t1z     =  x( 5);
  const double  omega2  =  x( 6);
  const double  phi2    =  x( 7);
  const double  kappa2  =  x( 8);
  const double  t2x     =  x( 9);
  const double  t2y     =  x(10);
  const double  t2z     =  x(11);

  const double  sinOmega1  =  sin(omega1);
  const double  cosOmega1  =  cos(omega1);
  const double  sinPhi1    =  sin(phi1);
  const double  cosPhi1    =  cos(phi1);
  const double  sinKappa1  =  sin(kappa1);
  const double  cosKappa1  =  cos(kappa1);
  
  const double  sinOmega2  =  sin(omega2);
  const double  cosOmega2  =  cos(omega2);
  const double  sinPhi2    =  sin(phi2);
  const double  cosPhi2    =  cos(phi2);
  const double  sinKappa2  =  sin(kappa2);
  const double  cosKappa2  =  cos(kappa2);

  t1[0] = t1x; t1[1] = t1y; t1[2] = t1z;
  t2[0] = t2x; t2[1] = t2y; t2[2] = t2z;

  R1[0][0] =  cosKappa1*cosPhi1;
  R1[0][1] = -cosPhi1*sinKappa1;
  R1[0][2] =  sinPhi1;
  R1[1][0] =  cosOmega1*sinKappa1 + cosKappa1*sinOmega1*sinPhi1;
  R1[1][1] =  cosKappa1*cosOmega1 - sinKappa1*sinOmega1*sinPhi1;
  R1[1][2] = -cosPhi1*sinOmega1;
  R1[2][0] =  sinKappa1*sinOmega1 - cosKappa1*cosOmega1*sinPhi1;
  R1[2][1] =  cosKappa1*sinOmega1 + cosOmega1*sinKappa1*sinPhi1;
  R1[2][2] =  cosOmega1*cosPhi1;
  
  R2[0][0] =  cosKappa2*cosPhi2;
  R2[0][1] = -cosPhi2*sinKappa2;
  R2[0][2] =  sinPhi2;
  R2[1][0] =  cosOmega2*sinKappa2 + cosKappa2*sinOmega2*sinPhi2;
  R2[1][1] =  cosKappa2*cosOmega2 - sinKappa2*sinOmega2*sinPhi2;
  R2[1][2] = -cosPhi2*sinOmega2;
  R2[2][0] =  sinKappa2*sinOmega2 - cosKappa2*cosOmega2*sinPhi2;
  R2[2][1] =  cosKappa2*sinOmega2 + cosOmega2*sinKappa2*sinPhi2;
  R2[2][2] =  cosOmega2*cosPhi2;

  for(i = 1; i <= 271; i++) {
    a0[0] = (double)duom[i][1]; 
    a0[1] = (double)duom[i][2];
    a0[2] = (double)duom[i][3];
    b0[0] = (double)duom[i][4]; 
    b0[1] = (double)duom[i][5]; 
    b0[2] = (double)duom[i][6];
    c0[0] = (double)duom[i][7]; 
    c0[1] = (double)duom[i][8]; 
    c0[2] = (double)duom[i][9];

	for( k = 0; k < 3; k++ ) {
	    p1[k] = R1[k][0]*a0[0]+R1[k][1]*a0[1]+R1[k][2]*a0[2];
	    p3[k] = R2[k][0]*c0[0]+R2[k][1]*c0[1]+R2[k][2]*c0[2];
	}

	// Area of a triangle (eq. 4.5 in Schaums)
	//
	// A = Sqrt( s(s-a)(s-b)(s-c) ), where
	//
	// s = (a+b+c)/2
	//
	for( k = 0; k < 3; k++ ) {
	    a0[k] = b0[k] - p1[k] - t1[k];
	    c0[k] = p3[k] - b0[k] + t2[k];
	    b0[k] = a0[k] + c0[k];
	}

	a = sqrt( sqr(a0[0]) + sqr(a0[1]) + sqr(a0[2]) ); 
	b = sqrt( sqr(b0[0]) + sqr(b0[1]) + sqr(b0[2]) ); 
	c = sqrt( sqr(c0[0]) + sqr(c0[1]) + sqr(c0[2]) );
	s = (a+b+c)/2.0;
	powerArea = s*(s-a)*(s-b)*(s-c) ; // see schaums
	
	  area = sqrt(powerArea);
    if( b > 0 ) {
      height = 2.0*area/b; // height of the triangle:  area = 0.5*h*b
    }
    else {
      height = a;
    }

//    FN += Sqr( height );
    FN += height;
  }

//    cout << FN << endl;
  return FN;

}


void Gradient_myMinHeight(RCRVector x, RCRVector grad) {

  // Gradient(0) = { dFN/d(Omega1),
  //                 dFN/d(Phi1),
  //                 dFN/d(Kappa1),
  //                 dFN/d(t1x),
  //                 dFN/d(t1y),
  //                 dFN/d(t1z),
  //                 dFN/d(Omega2),
  //                 dFN/d(Phi2),
  //                 dFN/d(Kappa2),
  //                 dFN/d(t2x),
  //                 dFN/d(t2y),
  //                 dFN/d(t2z)    }


  // Numerical calculations of gradients
  const double step = 1e-6;

  double Fminus, Fplus;
  int i;

  RCRVector xlocal(12), NumGrad(12);
  for(i = 0; i <= 11; i++ ) xlocal(i) = x(i);
  for(i = 11 ; i>=0; --i) {
	xlocal(i) -= step;
	Fminus = Objective_myMinHeight(xlocal);
	xlocal(i) += 2.0*step;
	Fplus = Objective_myMinHeight(xlocal);
	xlocal(i) -= step; // original x value
	NumGrad(i) = (Fplus-Fminus)/(2.0*step);
  }

  for(i = 0; i <= 11; i++ ) grad(i) = NumGrad(i);
  
}

#endif