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
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
|
/////////////////////////////////////////////////////////////
// //
// 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 "Sphere3DFitter.h"
// --- project includes ---
#include "sphere_fitting/utils/nvector.h"
#include "sphere_fitting/utils/nfunction.h"
#include "sphere_fitting/utils/simplex.h"
#include "fit_3d_sphere.h"
// --- IO includes ---
#include <iostream>
Sphere FitSphere3D(const AGeometricObject* GO1,
const AGeometricObject* GO2,
const AGeometricObject* GO3,
const AGeometricObject* GO4,
const Vector3& spos,
int max_iter,double prec)
{
Sphere res;
simplex_method<double,3> *simplex;
nvector<double,3> start,sol;
fit_3d_sphere_fn* sfn;
// set initial position to barycenter of input spheres
start[0]=spos.x();
start[1]=spos.y();
start[2]=spos.z();
// set fitting function
sfn=new fit_3d_sphere_fn(GO1,GO2,GO3,GO4);
// solve for center
simplex=new simplex_method<double,3>(sfn);
sol=simplex->solve(prec,start,max_iter);
// calc radius : min radius relative to 3 spheres - tol
Vector3 center=Vector3(sol[0],sol[1],sol[2]);
double r1=GO1->getDist(center);
double r2=GO2->getDist(center);
double r3=GO3->getDist(center);
double r4=GO4->getDist(center);
double r=(r1<r2) ? r1 : r2;
r= (r < r3) ? r : r3;
r= (r < r4) ? r : r4;
res=Sphere(center,r-0.1*prec);
// clean up
delete simplex;
delete sfn;
return res;
}
|
