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
}
SkVector SkEvalQuadTangentAt(const SkPoint src[3], SkScalar t) {
SkASSERT(src);
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 B = P1 - P0;
Sk2s A = P2 - P1 - B;
Sk2s T = A * Sk2s(t) + B;
return to_vector(T + T);
}
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 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);
}
SkVector SkEvalQuadTangentAt(const SkPoint src[3], SkScalar t) {
// The derivative equation is 2(b - a +(a - 2b +c)t). This returns a
// zero tangent vector when t is 0 or 1, and the control point is equal
// to the end point. In this case, use the quad end points to compute the tangent.
if ((t == 0 && src[0] == src[1]) || (t == 1 && src[1] == src[2])) {
return src[2] - src[0];
}
SkASSERT(src);
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 B = P1 - P0;
Sk2s A = P2 - P1 - B;
Sk2s T = A * Sk2s(t) + B;
return to_vector(T + T);
}
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;
}
}
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];
}
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
int Struct3D_Master:: ReadMaterial(FILE * file, int *LineNum)
/*********************************************************/
/*
analyse la description du type:
material DEF yellow Material {
diffuseColor 1.00000 1.00000 0.00000e+0
emissiveColor 0.00000e+0 0.00000e+0 0.00000e+0
specularColor 1.00000 1.00000 1.00000
ambientIntensity 1.00000
transparency 0.00000e+0
shininess 1.00000
}
ou du type:
material USE yellow
*/
{
char line[512], * text, * command;
wxString mat_name;
S3D_Material * material = NULL;
// Lecture de la commande:
command = strtok(NULL, " \t\n\r");
text = strtok(NULL, " \t\n\r");
mat_name = CONV_FROM_UTF8(text);
if ( stricmp(command, "USE") == 0 )
{
for ( material = m_Materials; material != NULL;
material = (S3D_Material *) material->Pnext)
{
if ( material->m_Name == mat_name)
{
material->SetMaterial();
return 1;
}
}
printf("ReadMaterial error: material not found\n");
return 0;
}
if ( stricmp(command, "DEF") == 0 )
{
material = new S3D_Material(this, mat_name);
material->Pnext = m_Materials;
m_Materials = material;
while ( GetLine(file, line, LineNum, 512) )
{
text = strtok(line," \t\n\r");
if ( text == NULL ) continue;
if ( text[0] == '}' )
{
material->SetMaterial();
return 0;
}
if ( stricmp (text, "diffuseColor") == 0 )
{
text = strtok(NULL," \t\n\r");
material->m_DiffuseColor.x = atof(from_point(text));
text = strtok(NULL," \t\n\r");
material->m_DiffuseColor.y = atof(from_point(text));
text = strtok(NULL," \t\n\r");
material->m_DiffuseColor.z = atof(from_point(text));
}
else if ( stricmp (text, "emissiveColor") == 0 )
{
text = strtok(NULL," \t\n\r");
material->m_EmissiveColor.x = atof(from_point(text));
text = strtok(NULL," \t\n\r");
material->m_EmissiveColor.y = atof(from_point(text));
text = strtok(NULL," \t\n\r");
material->m_EmissiveColor.z = atof(from_point(text));
}
else if ( strnicmp (text, "specularColor", 13 ) == 0 )
{
text = strtok(NULL," \t\n\r");
material->m_SpecularColor.x = atof(from_point(text));
text = strtok(NULL," \t\n\r");
material->m_SpecularColor.y = atof(from_point(text));
text = strtok(NULL," \t\n\r");
material->m_SpecularColor.z = atof(from_point(text));
}
else if ( strnicmp (text, "ambientIntensity", 16 ) == 0 )
{
text = strtok(NULL," \t\n\r");
material->m_AmbientIntensity = atof(from_point(text));
}
else if ( strnicmp (text, "transparency", 12 ) == 0 )
{
text = strtok(NULL," \t\n\r");
material->m_Transparency = atof(from_point(text));
}
else if ( strnicmp (text, "shininess", 9 ) == 0 )
{
text = strtok(NULL," \t\n\r");
material->m_Shininess = atof(from_point(text));
}
}
}
return -1;
}
int Struct3D_Master::ReadGeometry(FILE * file, int *LineNum)
/***********************************************************/
{
char line[1024], buffer[1024], *text;
int err = 1;
int nn = BUFSIZE;
double * points = NULL;
int * index = NULL;
while ( GetLine(file, line, LineNum, 512) )
{
strcpy(buffer,line);
text = strtok(buffer, " \t\n\r");
if ( * text == '}' )
{
err = 0; break;
}
if ( stricmp (text, "normalPerVertex" ) == 0 )
{
text = strtok(NULL, " ,\t\n\r");
if( stricmp (text, "TRUE") == 0 )
{
}
else
{
}
continue;
}
if ( stricmp (text, "normal" ) == 0 )
{
int coord_number;
double * buf_points = ReadCoordsList(file, line, &coord_number, LineNum);
continue;
free(buf_points);
continue;
}
if ( stricmp (text, "normalIndex" ) == 0 )
{
while ( GetLine(file, line, LineNum, 512) )
{
text = strtok(line, " ,\t\n\r");
while ( text )
{
if ( *text == ']') break;
text = strtok(NULL, " ,\t\n\r");
}
if ( text && (*text == ']') ) break;
}
continue;
}
if ( stricmp (text, "coord" ) == 0 )
{
int coord_number;
points = ReadCoordsList(file, line, &coord_number, LineNum);
}
else if ( stricmp (text, "coordIndex" ) == 0 )
{
index = (int *) MyMalloc(nn * sizeof(int) );
S3D_Vertex * coords =
(S3D_Vertex *) MyMalloc(nn * sizeof(S3D_Vertex) );
while ( GetLine(file, line, LineNum, 512) )
{
int coord_count = 0, jj;
text = strtok(line, " ,\t\n\r");
while ( text )
{
if ( *text == ']') break;
jj = atoi(from_point(text));
if ( jj < 0 )
{
S3D_Vertex * curr_coord = coords;
for ( jj = 0; jj < coord_count; jj ++ )
{
int kk = index[jj] * 3;
curr_coord->x = points[kk];
curr_coord->y = points[kk+1];
curr_coord->z = points[kk+2];
curr_coord++;
}
Set_Object_Coords(coords, coord_count );
Set_Object_Data(coords, coord_count );
coord_count = 0;
}
else
{
index[coord_count++] = jj;
}
text = strtok(NULL, " ,\t\n\r");
}
if ( text && (*text == ']') ) break;
}
free(index);
free(coords);
}
else
{
printf ("ReadGeometry error line %d <%s> \n", *LineNum, text);
break;
}
}
if ( points ) free (points);
return err;
}
double * ReadCoordsList(FILE * file, char * text_buffer, int * bufsize, int *LineNum)
/************************************************************************************/
/* Read a coordinate liste like:
coord Coordinate { point [
-5.24489 6.57640e-3 -9.42129e-2,
-5.11821 6.57421e-3 0.542654,
-3.45868 0.256565 1.32000 ] }
or:
normal Normal { vector [
0.995171 -6.08102e-6 9.81541e-2,
0.923880 -4.09802e-6 0.382683,
0.707107 -9.38186e-7 0.707107]
}
Return the coordinate list
text_buffer contains the first line of this node :
"coord Coordinate { point ["
*/
{
double * data_list = NULL;
unsigned int ii = 0, jj = 0, nn = BUFSIZE;
char * text;
bool HasData = FALSE;
bool StartData = FALSE;
bool EndData = FALSE;
bool EndNode = FALSE;
char string_num[512];
text = text_buffer;
while ( !EndNode )
{
if ( * text == 0 ) // Needs data !
{
text = text_buffer;
GetLine(file, text_buffer, LineNum, 512);
}
while ( !EndNode && *text )
{
switch ( *text )
{
case '[':
StartData = TRUE;
jj = 0; string_num[jj] = 0;
data_list = (double *) MyZMalloc(nn * sizeof(double) );
break;
case '}':
EndNode = TRUE;
break;
case ']':
case '\t':
case ' ':
case ',':
jj = 0;
if ( ! StartData || !HasData) break;
data_list[ii] = atof(from_point(string_num));
string_num[jj] = 0;
ii++;
if ( ii >= nn )
{
nn *= 2;
data_list = (double *) realloc(data_list, (nn * sizeof(double)) );
}
HasData = FALSE;
if ( *text == ']' )
{
StartData = FALSE;
EndData = TRUE;
}
break;
default:
if ( ! StartData ) break;
if ( jj >= sizeof(string_num) ) break;
string_num[jj] = *text;
jj++; string_num[jj] = 0;
HasData = TRUE;
break;
}
text++;
}
}
if ( data_list )
data_list = (double *) realloc(data_list, (ii * sizeof(double)) );
if ( bufsize ) *bufsize = ii;
return data_list;
}
