1 |
#include "libutil/Vec23.hxx" |
#include "libutil/Vec23.hxx" |
2 |
|
#include "libutil/Debug.hxx" |
3 |
|
|
4 |
namespace Fisheye { |
namespace Fisheye { |
5 |
|
DBGVAR(dbg, "Fisheye.general"); |
6 |
|
|
7 |
using namespace Vec23; |
using namespace Vec23; |
8 |
|
|
39 |
} |
} |
40 |
return 1; |
return 1; |
41 |
} |
} |
42 |
|
float func(float r) const { |
43 |
|
return 1 + z * atan(r) / r; |
44 |
|
} |
45 |
|
float inverse(float r2) const { |
46 |
|
if(z <= 0) return 1; // Fail gracefully |
47 |
|
if (r2 > 0) { |
48 |
|
float r_orig = sqrt(r2); |
49 |
|
float ylow = r_orig; |
50 |
|
float yhigh = r_orig + z * M_PI / 2; |
51 |
|
// Then, loop a little bit // XXX Improve |
52 |
|
for(int i=0; i<15; i++) { |
53 |
|
float y = 0.5*(ylow+yhigh); |
54 |
|
float c = y * func(y); |
55 |
|
if(c < r_orig) { |
56 |
|
ylow = y; |
57 |
|
} else { |
58 |
|
yhigh = y; |
59 |
|
} |
60 |
|
DBG(dbg) << |
61 |
|
"r_orig: " << r_orig << |
62 |
|
"ylow: " << ylow << |
63 |
|
"yhigh: " << yhigh << |
64 |
|
"c: " << c << "\n"; |
65 |
|
} |
66 |
|
return 0.5*(ylow+yhigh) / r_orig; |
67 |
|
} |
68 |
|
return 1; |
69 |
|
} |
70 |
}; |
}; |
71 |
|
|
72 |
|
template<class F> struct inverse_vector_mag_isotropic ; |
73 |
|
|
74 |
template<class F> struct vector_mag_isotropic { |
template<class F> struct vector_mag_isotropic { |
75 |
vector_mag_isotropic() { } |
vector_mag_isotropic() { } |
76 |
vector_mag_isotropic(F &f) : f(f) { } |
vector_mag_isotropic(F &f) : f(f) { } |
77 |
|
void setMag(float mag) { f = F(mag); } |
78 |
F f; |
F f; |
79 |
|
typedef inverse_vector_mag_isotropic<F> InverseType; |
80 |
ZPt operator() (const ZPt &p) const { |
ZPt operator() (const ZPt &p) const { |
81 |
float r2 = p.x * p.x + p.y * p.y; |
float r2 = p.x * p.x + p.y * p.y; |
82 |
float m = f(r2); |
float m = f(r2); |
83 |
return ZPt(m * p.x, m * p.y, p.z); |
return ZPt(m * p.x, m * p.y, p.z); |
84 |
} |
} |
85 |
}; |
}; |
86 |
|
|
87 |
|
template<class F> struct inverse_vector_mag_isotropic { |
88 |
|
inverse_vector_mag_isotropic() { } |
89 |
|
inverse_vector_mag_isotropic(F &f) : f(f) { } |
90 |
|
void setMag(float mag) { f = F(mag); } |
91 |
|
F f; |
92 |
|
typedef vector_mag_isotropic<F> InverseType ; |
93 |
|
ZPt operator() (const ZPt &p) const { |
94 |
|
float r2 = p.x * p.x + p.y * p.y; |
95 |
|
float m = f.inverse(r2); |
96 |
|
return ZPt(m * p.x, m * p.y, p.z); |
97 |
|
} |
98 |
|
}; |
99 |
|
|
100 |
} |
} |