bool Triangulate::Process(const Vector2fVector &contour,Vector2fVector &result)
{
/* allocate and initialize list of Vertices in polygon */
int n = contour.size();
if ( n < 3 ) return false;
int *V = new int[n];
/* we want a counter-clockwise polygon in V */
if ( 0.0f < Area(contour) )
for (int v=0; v<n; v++) V[v] = v;
else
for(int v=0; v<n; v++) V[v] = (n-1)-v;
int nv = n;
/* remove nv-2 Vertices, creating 1 triangle every time */
int count = 2*nv; /* error detection */
for(int m=0, v=nv-1; nv>2; )
{
/* if we loop, it is probably a non-simple polygon */
if (0 >= (count--))
{
//** Triangulate: ERROR - probable bad polygon!
return false;
}
/* three consecutive vertices in current polygon, <u,v,w> */
int u = v ; if (nv <= u) u = 0; /* previous */
v = u+1; if (nv <= v) v = 0; /* new v */
int w = v+1; if (nv <= w) w = 0; /* next */
if ( Snip(contour,u,v,w,nv,V) )
{
int a,b,c,s,t;
/* true names of the vertices */
a = V[u]; b = V[v]; c = V[w];
/* output Triangle */
result.push_back( contour[a] );
result.push_back( contour[b] );
result.push_back( contour[c] );
m++;
/* remove v from remaining polygon */
for(s=v,t=v+1;t<nv;s++,t++) V[s] = V[t]; nv--;
/* resest error detection counter */
count = 2*nv;
}
}
delete V;
return true;
}
float Triangulate::Area(const Vector2fVector &contour) {
int n = contour.size();
float A = 0.0f;
for (int p = n - 1, q = 0; q < n; p = q++) {
A += contour[p].x * contour[q].y - contour[q].x * contour[p].y;
}
return A * 0.5f;
}
int main(int argc, char** argv)
{
// data
vector<completeInformation> compl_info, selected_info;
vector<vecPairsList> extractedLines;
// read robot's info
parseRobotInfo("/home/ubisum/fuerte_workspace/thesis_pack/src/files/robotInfo.txt", compl_info);
// select relevant poses
selected_info = selectFrames(compl_info);
// compute lines for each pose
for(int i = 0; i<(int)selected_info.size(); i++)
{
Vector2fVector vector;
for(int j = 0; j<(int)selected_info[i].points.size(); j++)
{
// extract a coordinate of a scan's point
coordinate coord = selected_info[i].points[j];
// fill a vector
Vector2f temp_vec;
temp_vec << coord.first, coord.second;
// store vector
vector.push_back(temp_vec);
}
// compute and store lines for current scan
extractedLines.push_back(computeLines(vector));
}
Vector4f a(15,12,24,20);
obtainNewExtremes(a,Vector4f(15,16,19,22));
//cout << "Projection " << endl << projectPoint(Vector4f(15,12,24,20), Vector2f(10,14)) << endl;
cout << "Projection " << endl << projectByEquation(Vector3f(2,-1,0), Vector2f(2,3)) << endl;
// vector<vecPairsList> reducedVector(extractedLines.begin()+1, extractedLines.begin()+2);
int init = 0;
int end = 1;
vector<vecPairsList> reducedVector;
for(int k = init; k<= end; k++)
reducedVector.push_back(extractedLines[k]);
//MatrixXf m = mergeLines(createVector());
MatrixXf m = mergeLines(reducedVector);
//MatrixXf m = mergeLines(extractedLines);
cout << "Size m: " << m.rows() << " " << m.cols() << endl;
printLinesByExtremes(m.block(6,0,4,m.cols()), MatrixXf::Identity(3,3), "merged_lines.txt");
cout << endl << "Proiezione punto" << endl;
cout << projectByEquation(Vector3f(2, -1, 0), Vector2f(2,3)) << endl << endl;
Vector4f line1(2,3,2,1);
Vector4f line2(2,1,5,1);
MatrixXf two_lines(4,2);
two_lines.block(0,0,4,1) = line1;
two_lines.block(0,1,4,1) = line2;
float angle = 45*M_PI/180;
MatrixXf prova_t(3,3);
prova_t << cos(angle), -sin(angle), 5, sin(angle), cos(angle), 2, 0, 0, 1;
// prova_t << 1,0,5,0,1,0,0,0,1;
// MatrixXf prova_t = MatrixXf::Identity(3,3);
MatrixXf transf_vectors(4,2);
transf_vectors.block(0,0,2,2) = transformVectors(two_lines.block(0,0,2,2), prova_t);
transf_vectors.block(2,0,2,2) = transformVectors(two_lines.block(2,0,2,2), prova_t);
cout << "transf_vectors" << endl << transf_vectors << endl;
remove("prova_transf.txt");
FILE* prova_transf = fopen("prova_transf.txt", "a");
stringstream ss_prova;
for(int w = 0; w<transf_vectors.cols(); w++)
{
Vector4f column = transf_vectors.block(0,w,4,1);
ss_prova << column(0) << "\t" << column(1) << "\n" <<
column(2) << "\t" << column(3) << "\n\n";
fputs(ss_prova.str().c_str(), prova_transf);
}
Vector2f polar1 = polarRepresentation(line1.block(0,0,2,1), line1.block(2,0,2,1));
//Vector2f polar2 = polarRepresentation(transf_vectors.block(0,0,2,1), transf_vectors.block(2,0,2,1));
Vector2f polar2 = new_transformRT(transf_vectors.block(0,0,4,1));
cout << "differenza theta " << 180*fabs(polar1(1)-polar2(1))/M_PI << endl;
}
