/////////////////////////////////////////////////////////////
//                                                         //
// Copyright (c) 2007-2014 by The University of Queensland //
// Centre for Geoscience Computing                         //
// http://earth.uq.edu.au/centre-geoscience-computing      //
//                                                         //
// Primary Business: Brisbane, Queensland, Australia       //
// Licensed under the Open Software License version 3.0    //
// http://www.opensource.org/licenses/osl-3.0.php          //
//                                                         //
/////////////////////////////////////////////////////////////

#include "fit_3d_2spheres_2lines.h"
#include <cmath>

using std::sqrt;
using std::fabs;

fit_3d_2spheres_2lines_fn::fit_3d_2spheres_2lines_fn(const Vector3& sc1, double r1,
						     const Vector3& sc2, double r2,
						     const Vector3& o1, const Vector3& n1,
						     const Vector3& o2, const Vector3& n2)
{
  m_p1=sc1;
  m_p2=sc2;
  m_r1=r1;
  m_r2=r2;
  m_orig1=o1;
  m_nor1=n1;
  m_orig2=o2;
  m_nor2=n2; 
}

double fit_3d_2spheres_2lines_fn::operator()(const nvector<double,3>& data) const
{
  double x=data[0];
  double y=data[1];
  double z=data[2];

  double ra=sqrt((x-m_p1.x())*(x-m_p1.x())+(y-m_p1.y())*(y-m_p1.y())+(z-m_p1.z())*(z-m_p1.z()))-m_r1;
  double rb=sqrt((x-m_p2.x())*(x-m_p2.x())+(y-m_p2.y())*(y-m_p2.y())+(z-m_p2.z())*(z-m_p2.z()))-m_r2;
  double rc=dot((Vector3(x,y,z)-m_orig1),m_nor1);
  double rd=dot((Vector3(x,y,z)-m_orig2),m_nor2);

  double rq=0.25*(ra+rb+rc+rd);

  double dr=sqrt((rq-ra)*(rq-ra)+(rq-rb)*(rq-rb)+(rq-rc)*(rq-rc)+(rq-rd)*(rq-rd));

  return dr; 
}
