Example #1
0
bool Plane::FitToPoints(const Vector<Vec4f> &Points, Vec3f &Basis1, Vec3f &Basis2, float &NormalEigenvalue, float &ResidualError)
{
    Vec3f Centroid, Normal;

    float ScatterMatrix[3][3];
    int  Order[3];
    float DiagonalMatrix[3];
    float OffDiagonalMatrix[3];

    // Find centroid
    Centroid = Vec3f::Origin;
    float TotalWeight = 0.0f;
    for(UINT i = 0; i < Points.Length(); i++)
    {
        TotalWeight += Points[i].w;
        Centroid += Vec3f(Points[i].x, Points[i].y, Points[i].z) * Points[i].w;
    }
    Centroid /= TotalWeight;

    // Compute scatter matrix
    Find_ScatterMatrix(Points, Centroid, ScatterMatrix, Order);

    tred2(ScatterMatrix,DiagonalMatrix,OffDiagonalMatrix);
    tqli(DiagonalMatrix,OffDiagonalMatrix,ScatterMatrix);

    /*
    **    Find the smallest eigenvalue first.
    */
    float Min = DiagonalMatrix[0];
    float Max = DiagonalMatrix[0];
    UINT MinIndex = 0;
    UINT MiddleIndex = 0;
    UINT MaxIndex = 0;
    for(UINT i = 1; i < 3; i++)
    {
        if(DiagonalMatrix[i] < Min)
        {
            Min = DiagonalMatrix[i];
            MinIndex = i;
        }
        if(DiagonalMatrix[i] > Max)
        {
            Max = DiagonalMatrix[i];
            MaxIndex = i;
        }
    }
    for(UINT i = 0; i < 3; i++)
    {
        if(MinIndex != i && MaxIndex != i)
        {
            MiddleIndex = i;
        }
    }
    /*
    **    The normal of the plane is the smallest eigenvector.
    */
    for(UINT i = 0; i < 3; i++)
    {
        Normal[Order[i]] = ScatterMatrix[i][MinIndex];
        Basis1[Order[i]] = ScatterMatrix[i][MiddleIndex];
        Basis2[Order[i]] = ScatterMatrix[i][MaxIndex];
    }
    NormalEigenvalue = Math::Abs(DiagonalMatrix[MinIndex]);
    Basis1.SetLength(DiagonalMatrix[MiddleIndex]);
    Basis2.SetLength(DiagonalMatrix[MaxIndex]);

    if(!Basis1.Valid() || !Basis2.Valid() || !Normal.Valid())
    {
        *this = ConstructFromPointNormal(Centroid, Vec3f::eX);
        Basis1 = Vec3f::eY;
        Basis2 = Vec3f::eZ;
    }
    else
    {
        *this = ConstructFromPointNormal(Centroid, Normal);
    }

    ResidualError = 0.0f;
    for(UINT i = 0; i < Points.Length(); i++)
    {
        ResidualError += UnsignedDistance(Vec3f(Points[i].x, Points[i].y, Points[i].z));
    }
    ResidualError /= Points.Length();

    return true;
}