1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
|
/////////////////////////////////////////////////////////////
// //
// Copyright (c) 2007-2017 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.apache.org/licenses/LICENSE-2.0 //
// //
/////////////////////////////////////////////////////////////
#include "fit_2d_sphere_2lines.h"
#include <cmath>
using std::sqrt;
using std::fabs;
fit_2d_sphere_2lines_fn::fit_2d_sphere_2lines_fn(const Vector3& sc, double r,const Vector3& o1, const Vector3& n1,const Vector3& o2, const Vector3&n2)
{
m_p=sc;
m_r=r;
m_orig1=o1;
m_nor1=n1;
m_orig2=o2;
m_nor2=n2;
}
double fit_2d_sphere_2lines_fn::operator()(const nvector<double,2>& data) const
{
double x=data[0];
double y=data[1];
double ra=sqrt((x-m_p.x())*(x-m_p.x())+(y-m_p.y())*(y-m_p.y()))-m_r;
double rb=fabs(dot((Vector3(x,y,0.0)-m_orig1),m_nor1));
double rc=fabs(dot((Vector3(x,y,0.0)-m_orig2),m_nor2));
double rq=(ra+rb+rc)/3.0;
double dr=sqrt((rq-ra)*(rq-ra)+(rq-rb)*(rq-rb)+(rq-rc)*(rq-rc));
return dr;
}
|
