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
41
42
43
44
45
46
47
48
49
50
51
|
/////////////////////////////////////////////////////////////
// //
// 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_3d_3spheres_1line.h"
#include <cmath>
using std::sqrt;
using std::fabs;
fit_3d_3spheres_1line_fn::fit_3d_3spheres_1line_fn(const Vector3& sc1, double r1,
const Vector3& sc2, double r2,
const Vector3& sc3, double r3,
const Vector3& o, const Vector3& n)
{
m_p1=sc1;
m_p2=sc2;
m_p3=sc3;
m_r1=r1;
m_r2=r2;
m_r3=r3;
m_orig=o;
m_nor=n;
}
double fit_3d_3spheres_1line_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=sqrt((x-m_p3.x())*(x-m_p3.x())+(y-m_p3.y())*(y-m_p3.y())+(z-m_p3.z())*(z-m_p3.z()))-m_r3;
double rd=dot((Vector3(x,y,z)-m_orig),m_nor);
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;
}
|
