1 |
// (c) Tuomas J. Lukka |
// (c) Tuomas J. Lukka |
2 |
|
|
3 |
|
#include <boost/format.hpp> |
4 |
|
|
5 |
|
#include <vob/Debug.hxx> |
6 |
#include <vob/opt/Zero1D.hxx> |
#include <vob/opt/Zero1D.hxx> |
7 |
#include <vob/geom/Quadrics.hxx> |
#include <vob/geom/Quadrics.hxx> |
8 |
|
|
10 |
|
|
11 |
namespace Geom { |
namespace Geom { |
12 |
|
|
13 |
|
DBGVAR(dbg_quadrics, "Quadrics"); |
14 |
|
|
15 |
|
using boost::format; |
16 |
|
|
17 |
struct SymEllProblem { |
struct SymEllProblem { |
18 |
Vec point; |
VecD point; |
19 |
Vec normal; |
VecD normal; |
20 |
|
|
21 |
Vec centerPoint(float s) const { |
VecD centerPoint(double s) const { |
22 |
// Point and normal in this scaled problem |
// Point and normal in this scaled problem |
23 |
Vec p(s * point.x, point.y); |
VecD p(s * point.x, point.y); |
24 |
Vec n(normal.x / s, normal.y); |
VecD n(normal.x / s, normal.y); |
25 |
n = n.normalized(); |
n = n.normalized(); |
26 |
|
|
27 |
// Find point on x==0 |
// Find point on x==0 |
28 |
float m = - p.x / normal.x; |
double m = - p.x / n.x; |
29 |
Vec p0 = p + m * n; |
VecD p0 = p + m * n; |
30 |
// Invariant: p0.x == 0 |
// Invariant: p0.x == 0 |
31 |
|
DBG(dbg_quadrics) << |
32 |
|
format("sep: p: %s, n:%s, m:%s, p0:%s\n") |
33 |
|
% p % n % m % p0; |
34 |
return p0; |
return p0; |
35 |
|
|
36 |
} |
} |
38 |
/** Calculate the discrepancy: a number that is zero when |
/** Calculate the discrepancy: a number that is zero when |
39 |
* the s given to us is right and positive and negative otherwise. |
* the s given to us is right and positive and negative otherwise. |
40 |
*/ |
*/ |
41 |
float operator()(float s) const { |
double operator()(double s) const { |
42 |
// Point in the scaled problem |
// Point in the scaled problem |
43 |
Vec p(s * point.x, point.y); |
VecD p(s * point.x, point.y); |
44 |
|
|
45 |
Vec p0 = centerPoint(s); |
VecD p0 = centerPoint(s); |
46 |
return (p0.length() - (p0-p).length()); |
double ret = (p0.length() - (p0-p).length()); |
47 |
|
DBG(dbg_quadrics) << |
48 |
|
format("s: %s, p:%s, p0:%s ret::%s\n") |
49 |
|
% s % p % p0 % ret; |
50 |
|
return ret; |
51 |
} |
} |
52 |
|
|
53 |
}; |
}; |
58 |
prob.normal = normal; |
prob.normal = normal; |
59 |
|
|
60 |
bool succ; |
bool succ; |
61 |
float scale = Opt::findZero1D(prob, .00001, 10000, .0001, .0001, succ); |
double scale = Opt::findZero1D(prob, .001, 1000, 35, succ); |
62 |
|
|
63 |
Vec res = prob.centerPoint(scale); |
VecD res = prob.centerPoint(scale); |
64 |
res.x = res.y / scale; |
res.x = res.y / scale; |
65 |
return res; |
return res; |
66 |
} |
} |