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; }