private void reconstructFeaturePosition(Feature feat)
{
Vector fpos;
Vector position = new Vector(3);
int i, k, hits = 1;
fpos = feat.get_y();
for (k = 0; k < 3; k++)
{
position[k] = fpos[k];
}
for (i = 0; i < feat.triangles.Count; i++)
{
Feature f;
int j, index = 0;
model_triangle tri = (model_triangle)feat.triangles[i];
for (j = 0; j < 3; j++)
{
f = ((model_vertex)tri.vertices[j]).vertex_feature;
if (f == feat)
{
index = j;
}
}
for (j = 0; j < 3; j++)
{
if (j != index)
{
f = ((model_vertex)tri.vertices[j]).vertex_feature;
if (!((f.get_successful_measurement_flag()) && (f.get_feature_measurement_model().fully_initialised_flag)))
{
fpos = f.get_y();
for (k = 0; k < 3; k++)
{
position[k] += fpos[k] - tri.offsets[index, j, k];
}
hits++;
}
}
}
}
if (hits > 0)
{
for (k = 0; k < 3; k++)
{
position[k] /= hits;
}
fpos = feat.get_y();
for (k = 0; k < 3; k++)
{
fpos[k] = position[k];
}
}
}
/// <summary>
/// adds a new triangle
/// </summary>
/// <param name="f1"></param>
/// <param name="f2"></param>
/// <param name="f3"></param>
private void add_triangle(Feature f1, Feature f2, Feature f3)
{
model_triangle new_tri = new model_triangle(this);
new_tri.vertices[0] = new model_vertex(f1);
new_tri.vertices[1] = new model_vertex(f2);
new_tri.vertices[2] = new model_vertex(f3);
new_tri.update_offsets();
triangles.Add(new_tri);
f1.triangles.Add(new_tri);
f2.triangles.Add(new_tri);
f3.triangles.Add(new_tri);
}
/// <summary>
/// Insert the given feature into the given triangle.
/// This creates three new triangles and removes the original
/// </summary>
/// <param name="feat"></param>
/// <param name="tri"></param>
private void update(Feature feat, model_triangle tri)
{
Feature f1 = ((model_vertex)tri.vertices[0]).vertex_feature;
Feature f2 = ((model_vertex)tri.vertices[1]).vertex_feature;
Feature f3 = ((model_vertex)tri.vertices[2]).vertex_feature;
add_triangle(feat, f1, f2);
add_triangle(feat, f2, f3);
add_triangle(feat, f3, f1);
f1.triangles.Remove(tri);
f2.triangles.Remove(tri);
f3.triangles.Remove(tri);
triangles.Remove(tri);
}
/// <summary>
/// show the mesh in the given image
/// </summary>
/// <param name="img"></param>
public void Show(classimage_mono img)
{
for (int i = 0; i < features.Count; i++)
{
Feature feat = (Feature)features[i];
if (featureVisible(feat))
{
for (int j = 0; j < feat.triangles.Count; j++)
{
model_triangle tri = (model_triangle)feat.triangles[j];
if (tri.isVisible())
{
tri.Show(img);
}
}
}
}
}
private void addFeature(Feature feat)
{
if (!features.Contains(feat))
{
int j, i = 0;
model_triangle insideTriangle = null;
Feature f;
// is this feature inside any visible triangle ?
while ((i < features.Count) && (insideTriangle == null))
{
f = (Feature)features[i];
if (featureVisible(f))
{
// examine the triangles to which this feature belongs
j = 0;
while ((j < f.triangles.Count) && (insideTriangle == null))
{
model_triangle tri = (model_triangle)f.triangles[j];
if (tri.isVisible())
{
if (tri.isInside(feat))
{
insideTriangle = tri;
}
}
j++;
}
}
i++;
}
// add the feature
features.Add(feat);
if (insideTriangle != null)
{
// insert the feature into the triangle
update(feat, insideTriangle);
}
else
{
// connect the feature to its nearest neighbours
if (features.Count > 2)
{
Feature neighbour1 = null;
Feature neighbour2 = null;
/*
* i = 0;
* while (i < features.Count)
* {
* Feature f1 = (Feature)features[i];
* if (f1 != feat)
* {
* if (featureVisible(f1))
* {
* neighbour1 = f1;
* j = i + 1;
* while (j < features.Count)
* {
* Feature f2 = (Feature)features[j];
* if ((f2 != feat) && (f2 != neighbour1))
* {
* if (featureVisible(f2))
* {
* neighbour2 = f2;
* add_triangle(feat, neighbour1, neighbour2);
* }
* }
* j++;
* }
* }
* }
* i++;
* }
*/
closest_neighbours(feat, ref neighbour1, ref neighbour2);
if ((neighbour1 != null) && (neighbour2 != null))
{
add_triangle(feat, neighbour1, neighbour2);
}
}
}
}
}
