inline segment_t
to_segment(const rendering::ray_t& ray)
{
const vector_t v1 = ray.origin + ray.direction * ray.min;
const vector_t v2 = ray.origin + ray.direction * ray.max;
return segment_t(to_point(v1), to_point(v2));
}
void SkConvertQuadToCubic(const SkPoint src[3], SkPoint dst[4]) {
Sk2s scale(SkDoubleToScalar(2.0 / 3.0));
Sk2s s0 = from_point(src[0]);
Sk2s s1 = from_point(src[1]);
Sk2s s2 = from_point(src[2]);
dst[0] = src[0];
dst[1] = to_point(s0 + (s1 - s0) * scale);
dst[2] = to_point(s2 + (s1 - s2) * scale);
dst[3] = src[2];
}
void SkQuadToCoeff(const SkPoint pts[3], SkPoint coeff[3]) {
Sk2s p0 = from_point(pts[0]);
Sk2s p1 = from_point(pts[1]);
Sk2s p2 = from_point(pts[2]);
Sk2s p1minus2 = p1 - p0;
coeff[0] = to_point(p2 - p1 - p1 + p0); // A * t^2
coeff[1] = to_point(p1minus2 + p1minus2); // B * t
coeff[2] = pts[0]; // C
}
static PyObject *py_distance(PyObject *self, PyObject *args){
PyObject *aa, *bb;
Point *p_aa, *p_bb;
double result;
if(!PyArg_ParseTuple(args, "OO", &aa, &bb)){
return NULL;
};
p_aa = to_point(aa);
p_bb = to_point(bb);
result = distance(p_aa, p_bb);
return Py_BuildValue("d", result);
};
void paint_julia_prev(infoptr pic, point mouse_pos)
{
struct picture_info prev_pic;
dpoint p, tmp;
int n = 0;
prev_pic.area = get_prev_rect(pic);
init_coords(&prev_pic); /* Asetetaan sopiva skaalaus */
/* Julia-vakio hiiren koordinaateista */
prev_pic.julia_c = to_dpoint(mouse_pos, pic);
SRGP_setClipRectangle(prev_pic.area);
SRGP_setColor(SRGP_BLACK); /* Peitetään vanha kuva */
SRGP_fillRectangleCoord(prev_pic.area.bottom_left.x + 1,
prev_pic.area.bottom_left.y + 1,
prev_pic.area.top_right.x - 1,
prev_pic.area.top_right.y - 1);
SRGP_setColor(SRGP_WHITE); /* Valkea reunus */
SRGP_rectangle(prev_pic.area);
p.x = 0; /* Inverse functionin */
p.y = 0; /* alkupiste */
while(n < JULIA_LIMIT)
{ /* z[n+1] = sqrt(z[n] - c) */
tmp.x = p.x - prev_pic.julia_c.x;
tmp.y = p.y - prev_pic.julia_c.y;
p = c_sqrt(tmp);
SRGP_point(to_point(p, &prev_pic));
n++;
}
SRGP_setClipRectangle(pic->area);
}
void SkChopQuadAt(const SkPoint src[3], SkPoint dst[5], SkScalar t) {
SkASSERT(t > 0 && t < SK_Scalar1);
Sk2s p0 = from_point(src[0]);
Sk2s p1 = from_point(src[1]);
Sk2s p2 = from_point(src[2]);
Sk2s tt(t);
Sk2s p01 = interp(p0, p1, tt);
Sk2s p12 = interp(p1, p2, tt);
dst[0] = to_point(p0);
dst[1] = to_point(p01);
dst[2] = to_point(interp(p01, p12, tt));
dst[3] = to_point(p12);
dst[4] = to_point(p2);
}
void SkCubicToCoeff(const SkPoint pts[4], SkPoint coeff[4]) {
Sk2s p0 = from_point(pts[0]);
Sk2s p1 = from_point(pts[1]);
Sk2s p2 = from_point(pts[2]);
Sk2s p3 = from_point(pts[3]);
const Sk2s three(3);
Sk2s p1minusp2 = p1 - p2;
Sk2s D = p0;
Sk2s A = p3 + three * p1minusp2 - D;
Sk2s B = three * (D - p1minusp2 - p1);
Sk2s C = three * (p1 - D);
coeff[0] = to_point(A);
coeff[1] = to_point(B);
coeff[2] = to_point(C);
coeff[3] = to_point(D);
}
Waypoint
Waypoints::GenerateTakeoffPoint(const GeoPoint& location,
const fixed terrain_alt) const
{
// fallback: create a takeoff point
Waypoint to_point(location);
to_point.elevation = terrain_alt;
to_point.file_num = -1;
to_point.name = _T("(takeoff)");
to_point.type = Waypoint::Type::OUTLANDING;
return to_point;
}
static bool _convert_polygon(CPOLYGON *_object, GB_TYPE type, GB_VALUE *conv)
{
if (type != GB.FindClass("PointF[]"))
return true;
if (THIS)
{
// Polygon --> PointF[]
GB_ARRAY a;
int i;
GEOM_POINTF **data;
GB.Array.New(&a, GB.FindClass("PointF"), POLY->size()); // + THIS->closed);
data = (GEOM_POINTF **)GB.Array.Get(a, 0);
for(i = 0; i < (int)POLY->size(); i++)
{
data[i] = from_point((*POLY)[i]);
GB.Ref(data[i]);
}
/*if (closed)
{
data[i] = from_point((*POLY)[0]);
GB.Ref(data[i]);
}*/
conv->_object.value = a;
return false;
}
else
{
// PointF[] --> Polygon
CPOLYGON *p;
GB_ARRAY a = (GB_ARRAY)conv->_object.value;
int size = GB.Array.Count(a);
int i;
GEOM_POINTF **points;
p = (CPOLYGON *)GB.New(GB.FindClass("Polygon"), NULL, NULL);
points = (GEOM_POINTF **)GB.Array.Get(a, 0);
for (i = 0; i < size; i++)
{
if (!points[i])
continue;
p->poly->push_back(to_point(points[i]));
}
conv->_object.value = p;
return false;
}
}
Waypoint
Waypoints::generate_takeoff_point(const GeoPoint& location,
const fixed terrain_alt) const
{
// fallback: create a takeoff point
Waypoint to_point(location, true);
to_point.altitude = terrain_alt;
to_point.file_num = -1;
to_point.name = _T("(takeoff)");
to_point.type = Waypoint::TYPE_OUTLANDING;
return to_point;
}
void SkChopCubicAt(const SkPoint src[4], SkPoint dst[7], SkScalar t) {
SkASSERT(t > 0 && t < SK_Scalar1);
Sk2s p0 = from_point(src[0]);
Sk2s p1 = from_point(src[1]);
Sk2s p2 = from_point(src[2]);
Sk2s p3 = from_point(src[3]);
Sk2s tt(t);
Sk2s ab = interp(p0, p1, tt);
Sk2s bc = interp(p1, p2, tt);
Sk2s cd = interp(p2, p3, tt);
Sk2s abc = interp(ab, bc, tt);
Sk2s bcd = interp(bc, cd, tt);
Sk2s abcd = interp(abc, bcd, tt);
dst[0] = src[0];
dst[1] = to_point(ab);
dst[2] = to_point(abc);
dst[3] = to_point(abcd);
dst[4] = to_point(bcd);
dst[5] = to_point(cd);
dst[6] = src[3];
}
SkPoint SkEvalQuadAt(const SkPoint src[3], SkScalar t) {
SkASSERT(src);
SkASSERT(t >= 0 && t <= SK_Scalar1);
const Sk2s t2(t);
Sk2s P0 = from_point(src[0]);
Sk2s P1 = from_point(src[1]);
Sk2s P2 = from_point(src[2]);
Sk2s B = P1 - P0;
Sk2s A = P2 - P1 - B;
return to_point((A * t2 + B+B) * t2 + P0);
}
void HomographyModel::estimate_from_minimal_set(const vector< ScoredMatch> &data_points)
{ // given m points estimate the parameters vector
// based on vxl rrel_homography2d_est :: fit_from_minimal_set
if ( data_points.size() < get_num_points_to_estimate())
throw runtime_error("Not enough points to estimate the HomographyModel parameters");
vnl_matrix< double > A(9, 9, 0.0);
for ( int i=0; i < get_num_points_to_estimate(); i+=1 )
{ // for i = 0,1,2,3
vgl_homg_point_2d<double> from_point(data_points[i].feature_a->x, data_points[i].feature_a->y);
vgl_homg_point_2d<double> to_point(data_points[i].feature_b->x, data_points[i].feature_b->y);
if (false)
{ // just for debugging
printf("from_points[%i] -> to_points[%i] ==", i,i);
cout << from_point << " -> " << to_point << endl;
}
A( 2*i, 0 ) = A( 2*i+1, 3 ) = from_point.x() * to_point.w();
A( 2*i, 1 ) = A( 2*i+1, 4 ) = from_point.y() * to_point.w();
A( 2*i, 2 ) = A( 2*i+1, 5 ) = from_point.w() * to_point.w();
A( 2*i, 6 ) = -1 * from_point.x() * to_point.x();
A( 2*i, 7 ) = -1 * from_point.y() * to_point.x();
A( 2*i, 8 ) = -1 * from_point.w() * to_point.x();
A( 2*i+1, 6 ) = -1 * from_point.x() * to_point.y();
A( 2*i+1, 7 ) = -1 * from_point.y() * to_point.y();
A( 2*i+1, 8 ) = -1 * from_point.w() * to_point.y();
}
vnl_svd<double> svd( A, 1.0e-8 );
if (false)
{ // just for debugging
cout << "A == " << A << endl;
cout << "svd(A) == " << svd << endl;
}
const unsigned int homog_dof_ = 8;
if ( svd.rank() < homog_dof_ )
{
// singular fit
if (true)
{ // just for debugging
cout << "svd.rank() == " << svd.rank() << endl;
for ( unsigned int i=0; i<get_num_points_to_estimate(); ++i )
{
vgl_homg_point_2d<double> from_point(data_points[i].feature_a->x, data_points[i].feature_a->y);
vgl_homg_point_2d<double> to_point(data_points[i].feature_b->x, data_points[i].feature_b->y);
cout << "from->to point[i]-> " << from_point << "->" << to_point << endl;
}
}
throw runtime_error("HomographyModel::estimate_from_minimal_set failed");
}
vnl_vector<double> params = svd.nullvector();
parameters.resize(get_num_parameters());
if (params.size() != parameters.size() )
{
throw runtime_error("HomographyModel::estimate_from_minimal_set internal error");
}
for ( unsigned int i=0; i<get_num_parameters(); i+=1 )
{
parameters[i] = params[i]; // copy the result
}
if ( false ) {
cout << "HomographyModel parameters: " << parameters << endl;
}
return;
} // end of 'HomographyModel::estimate_from_minimal_set'
void HomographyModel::compute_residuals
(const vector< ScoredMatch> &data_points, vector<float> &residuals) const
{
// residuals -> errors
// Compute the residuals relative to the given parameter vector.
// based on rrel_homography2d_est :: compute_residuals
vnl_matrix< double > H(3,3);
int r,c;
for ( r=0; r<3; ++r )
for ( c=0; c<3; ++c )
H( r, c ) = parameters[ 3*r + c ];
vnl_svd< double > svd_H( H );
if ( svd_H.rank() < 3 )
{
if (true) cout << "H == " << H << endl;
//throw runtime_error("HomographyModel::compute_residuals rank(H) < 3!!");
cout << "HomographyModel::compute_residuals rank(H) < 3!!" << endl;
}
vnl_matrix< double > H_inv( svd_H.inverse() );
residuals.resize(data_points.size());
// compute the residual of each data point
vector< ScoredMatch>::const_iterator data_points_it;
vector<float>::iterator residuals_it;
vnl_vector< double > from_point( 3 ), to_point( 3 );
vnl_vector< double > trans_pt( 3 ), inv_trans_pt( 3 );
double del_x, del_y, inv_del_x, inv_del_y;
for (data_points_it = data_points.begin(), residuals_it = residuals.begin();
data_points_it != data_points.end() && residuals_it != residuals.end();
++data_points_it, ++residuals_it)
{
// from feature a to feature b
from_point[0] = data_points_it->feature_a->x;
from_point[1] = data_points_it->feature_a->y;
from_point[2] = 1.0;
to_point[0] = data_points_it->feature_b->x;
to_point[1] = data_points_it->feature_b->y;
to_point[2] = 1.0;
trans_pt = H * from_point;
inv_trans_pt = H_inv * to_point;
if ( from_point[ 2 ] == 0 || to_point[ 2 ] == 0
|| trans_pt[ 2 ] == 0 || inv_trans_pt[ 2 ] == 0 )
{
*residuals_it = 1e10;
}
else
{
del_x = trans_pt[ 0 ] / trans_pt[ 2 ] - to_point[ 0 ] /to_point[ 2 ];
del_y = trans_pt[ 1 ] / trans_pt[ 2 ] - to_point[ 1 ] / to_point[ 2 ];
inv_del_x = inv_trans_pt[ 0 ] / inv_trans_pt[ 2 ] -from_point[ 0 ] / from_point[ 2 ];
inv_del_y = inv_trans_pt[ 1 ] / inv_trans_pt[ 2 ] - from_point[ 1 ] / from_point[ 2 ];
const double t_value = vnl_math_sqr(del_x) + vnl_math_sqr(del_y)
+ vnl_math_sqr(inv_del_x) + vnl_math_sqr(inv_del_y);
*residuals_it = vcl_sqrt( t_value );
}
} // end of 'for each data point'
return;
} // end of method FundamentalMatrixModel<F>::compute_residuals
rendering::hits_t::iterator
hit(const rendering::ray_t& ray, rendering::hits_t::iterator hits) const
{
const auto function1 = [this, &ray](const float distance) -> float
{
const vector_t point = ray.origin + ray.direction * distance;
return _function(point);
};
const auto function = [&ray, &function1](const float distance) -> boost::math::tuple<float, float>
{
constexpr float h = 1e-2f;
return boost::math::make_tuple
(
function1(distance),
(function1(distance + h) - function1(distance - h)) / (2.f * h)
);
};
const auto test = [this, &ray, &hits, &function, &function1](const value_t& value)
{
constexpr float a = 1e-4f;
const box_t& box = value.first;
const point_t point = geo::return_centroid<point_t>(box);
float distance = length((const vector_t&) point - ray.origin);
if (ray.min > distance || distance > ray.max)
return;
boost::uintmax_t max = 100;
distance = boost::math::tools::newton_raphson_iterate
(
function,
distance - _distance + std::numeric_limits<float>::epsilon(),
distance - _distance,
distance + _distance,
std::numeric_limits<float>::digits,
max
);
if (std::abs(function1(distance)) < a)
{
const vector_t point = ray.origin + ray.direction * distance;
if (geo::intersects(box, to_point(point)))
{
hits->distance = distance;
hits->normal = normal(point);
++hits;
}
}
};
const segment_t segment = to_segment(ray);
_rtree.query(geo::index::intersects(segment), boost::make_function_output_iterator(test));
return hits;
}
