/**
* Extract a row from a matrix
* @param matrix The matrix that we are extracting the row for
* @param rowNumber The number of the row that we are working
* @return The matrix row that we are retrieving
*/
Matrixf GetMatrixRow(Matrixf & matrix, int rowNumber)
{
Matrixf result(matrix.ncols(), 1);
for (int column = 0; column<matrix.ncols(); column++)
{
float value = matrix.get(rowNumber, column);
result.set(column, 0, value);
}
return result;
}
/**
* Main application entry point
* @param argumentCount The amount of arguments comment in
* @param arguments The list of incomming arguments
* @returns SUCCESS or FAILURE
*/
int main(int argumentCount, char **arguments)
{
try
{
if (argumentCount != 2) throw runtime_error("USAGE: q5 <fileName>");
Matrixf vertices = ReadFile(arguments[1]);
// ---------------- Compute the robust normal ----------------
// compute centroid
Matrixf centroid(vertices.ncols(), 1);
for (unsigned int c = 0; c < vertices.ncols(); c++)
{
float total = 0;
for (unsigned int r = 0; r < vertices.nrows(); r++)
total += vertices(r, c);
centroid(c, 0) = total / vertices.nrows();
}
// find normal of our plane (formed by points 0, 1, and C)
Matrixf normEst = cross(subtract(GetMatrixRow(vertices, 0), centroid), subtract(GetMatrixRow(vertices, 1), centroid));
// find the absolute largest component (x,y,z) of the normal
for (unsigned int i = 0; i < 3; i++)
normEst(i, 0) = abs(normEst(i, 0));
int componentI;
if (normEst(0,0) > normEst(1,0) && normEst(0,0) > normEst(2,0))
componentI = 0;
else if (normEst(1,0) > normEst(0,0) && normEst(1,0) > normEst(2,0))
componentI = 1;
else
componentI = 2;
// copy vertices -- removing the largest component
unsigned int tempCount;
Matrixf projVertices(vertices.nrows(), vertices.ncols() - 1);
for (unsigned int r = 0; r < projVertices.nrows(); r++)
{
tempCount = 0;
for (unsigned int c = 0; c < vertices.ncols(); c++)
{
if (c != componentI)
{
projVertices(r, tempCount) = vertices(r, c);
tempCount++;
}
}
}
Matrixf projCentroid(centroid.nrows() - 1, 1);
tempCount = 0;
for (unsigned int r = 0; r < centroid.nrows(); r++)
{
if (r != componentI)
{
projCentroid(tempCount, 0) = centroid(r, 0);
tempCount++;
}
}
// calculate the counter-clockwise ordering of the vertices
Matrixf vecOA = subtract(GetMatrixRow(projVertices, 0), projCentroid);
Matrixf rotate(2, 2);
rotate(0, 0) = 0; rotate(0, 1) = -1;
rotate(1, 0) = 1; rotate(1, 1) = 0;
IndexAngle indexAng[5];
indexAng[0].index = 0;
indexAng[0].angle = 0.0f;
for (unsigned int r = 1; r < projVertices.nrows(); r++) // for each other vertex
{
Matrixf temp = subtract(GetMatrixRow(projVertices, r), projCentroid);
float angle = acos(dot(temp, vecOA) / (length(temp) * length(vecOA)));
// rotate ccw pi/2
Matrixf tempRot = multiply(rotate, temp);
float angleRot = acos(dot(tempRot, vecOA) / (length(tempRot) * length(vecOA)));
if ((angle < M_PI_2 && angleRot < M_PI_2) || (angle > M_PI_2 && angleRot < M_PI_2)) // angle on right
angle = (M_PI * 2) - angle;
indexAng[r].index = r;
indexAng[r].angle = angle;
}
// sort by angle size
sort(indexAng, indexAng + 5, &angle_sorter);
// compute the normal at each vertex
Matrixf normalAtVert(vertices.nrows(), vertices.ncols());
Matrixf normRunning(3, 1);
for (unsigned int i = 0; i < vertices.nrows(); i++)
{
Matrixf vecToPrev(3, 1);
if (i == 0) // wrap arround
vecToPrev = subtract(GetMatrixRow(vertices, indexAng[4].index), GetMatrixRow(vertices, indexAng[i].index));
else
vecToPrev = subtract(GetMatrixRow(vertices, indexAng[i-1].index), GetMatrixRow(vertices, indexAng[i].index));
Matrixf vecToNext(3, 1);
if (i == 4) // wrap around
vecToNext = subtract(GetMatrixRow(vertices, indexAng[0].index), GetMatrixRow(vertices, indexAng[i].index));
else
vecToNext = subtract(GetMatrixRow(vertices, indexAng[i+1].index), GetMatrixRow(vertices, indexAng[i].index));
Matrixf thisNorm = cross(vecToNext, vecToPrev);
normRunning = add(normRunning, thisNorm); // for computing the robust normal
thisNorm = normalize(thisNorm);
for (unsigned int c = 0; c < 3; c++)
normalAtVert(i, c) = thisNorm(c, 0); // for finding the weakest vertex
}
// normalise the total
Matrixf normRobust = normalize(normRunning);
cout << "Robust Normal: " << transpose(normRobust);
// ---------------- compute the new robust normal ----------------
// find weakest vertex
IndexAngle normStrength[5];
for (unsigned int i = 0; i < vertices.nrows(); i++)
{
normStrength[i].index = indexAng[i].index;
normStrength[i].angle = acos(dot(normalize(GetMatrixRow(normalAtVert, i)), normRobust));
}
// sort by angle size (largest to smallest)
sort(normStrength, normStrength + 5, &angle_sorter_inv);
unsigned int weakestIndex = normStrength[0].index;
cout << "Weakest Vertex: " << transpose(GetMatrixRow(vertices, weakestIndex));
// remove the weakest vertex
IndexAngle indexAng2[4];
tempCount = 0;
for (unsigned int i = 0; i < 5; i++)
{
if (indexAng[i].index == weakestIndex)
continue;
indexAng2[tempCount] = indexAng[i];
tempCount++;
}
// compute the normal at each vertex
Matrixf norm2Running(3, 1);
for (unsigned int i = 0; i < vertices.nrows() - 1; i++)
{
Matrixf vecToPrev(3, 1);
if (i == 0) // wrap arround
vecToPrev = subtract(GetMatrixRow(vertices, indexAng2[3].index), GetMatrixRow(vertices, indexAng2[i].index));
else
vecToPrev = subtract(GetMatrixRow(vertices, indexAng2[i-1].index), GetMatrixRow(vertices, indexAng2[i].index));
Matrixf vecToNext(3, 1);
if (i == 3) // wrap around
vecToNext = subtract(GetMatrixRow(vertices, indexAng2[0].index), GetMatrixRow(vertices, indexAng2[i].index));
else
vecToNext = subtract(GetMatrixRow(vertices, indexAng2[i+1].index), GetMatrixRow(vertices, indexAng2[i].index));
norm2Running = add(norm2Running, cross(vecToNext, vecToPrev));
}
// normalise the total
Matrixf norm2Robust = normalize(norm2Running);
cout << "New Robust Normal: " << transpose(norm2Robust);
}
catch(runtime_error error)
{
cerr << "ERROR: " << error.what() << endl;
return FAILURE;
}
return SUCCESS;
}