00001 // Magic Software, Inc. 00002 // http://www.magic-software.com 00003 // Copyright (c) 2000, All Rights Reserved 00004 // 00005 // Source code from Magic Software is supplied under the terms of a license 00006 // agreement and may not be copied or disclosed except in accordance with the 00007 // terms of that agreement. The various license agreements may be found at 00008 // the Magic Software web site. This file is subject to the license 00009 // 00010 // FREE SOURCE CODE 00011 // http://www.magic-software.com/License.html/free.pdf 00012 00013 #include "MgcEigen.h" 00014 #include "MgcGaussPointsFit.h" 00015 00016 //---------------------------------------------------------------------------- 00017 /*void MgcGaussPointsFit (int iQuantity, const MgcVector2* akPoint, 00018 MgcVector2& rkCenter, MgcVector2 akAxis[2], MgcReal afExtent[2]) 00019 { 00020 // compute mean of points 00021 rkCenter = akPoint[0]; 00022 int i; 00023 for (i = 1; i < iQuantity; i++) 00024 rkCenter += akPoint[i]; 00025 MgcReal fInvQuantity = 1.0/iQuantity; 00026 rkCenter *= fInvQuantity; 00027 00028 // compute covariances of points 00029 MgcReal fSumXX = 0.0, fSumXY = 0.0, fSumYY = 0.0; 00030 for (i = 0; i < iQuantity; i++) 00031 { 00032 MgcVector2 kDiff = akPoint[i] - rkCenter; 00033 fSumXX += kDiff.x*kDiff.x; 00034 fSumXY += kDiff.x*kDiff.y; 00035 fSumYY += kDiff.y*kDiff.y; 00036 } 00037 fSumXX *= fInvQuantity; 00038 fSumXY *= fInvQuantity; 00039 fSumYY *= fInvQuantity; 00040 00041 // solve eigensystem of covariance matrix 00042 MgcEigen kES(2); 00043 kES.Matrix(0,0) = fSumXX; 00044 kES.Matrix(0,1) = fSumXY; 00045 kES.Matrix(1,0) = fSumXY; 00046 kES.Matrix(1,1) = fSumYY; 00047 kES.IncrSortEigenStuff2(); 00048 00049 akAxis[0].x = kES.GetEigenvector(0,0); 00050 akAxis[0].y = kES.GetEigenvector(1,0); 00051 akAxis[1].x = kES.GetEigenvector(0,1); 00052 akAxis[1].y = kES.GetEigenvector(1,1); 00053 00054 afExtent[0] = kES.GetEigenvalue(0); 00055 afExtent[1] = kES.GetEigenvalue(1); 00056 }*/ 00057 //---------------------------------------------------------------------------- 00058 void MgcGaussPointsFit (int iQuantity, const MgcVector3* akPoint, 00059 MgcVector3& rkCenter, MgcVector3 akAxis[3], MgcReal afExtent[3]) 00060 { 00061 // compute mean of points 00062 rkCenter = akPoint[0]; 00063 int i; 00064 for (i = 1; i < iQuantity; i++) 00065 rkCenter += akPoint[i]; 00066 MgcReal fInvQuantity = 1.0/iQuantity; 00067 rkCenter *= fInvQuantity; 00068 00069 // compute covariances of points 00070 MgcReal fSumXX = 0.0, fSumXY = 0.0, fSumXZ = 0.0; 00071 MgcReal fSumYY = 0.0, fSumYZ = 0.0, fSumZZ = 0.0; 00072 for (i = 0; i < iQuantity; i++) 00073 { 00074 MgcVector3 kDiff = akPoint[i] - rkCenter; 00075 fSumXX += kDiff.x*kDiff.x; 00076 fSumXY += kDiff.x*kDiff.y; 00077 fSumXZ += kDiff.x*kDiff.z; 00078 fSumYY += kDiff.y*kDiff.y; 00079 fSumYZ += kDiff.y*kDiff.z; 00080 fSumZZ += kDiff.z*kDiff.z; 00081 } 00082 fSumXX *= fInvQuantity; 00083 fSumXY *= fInvQuantity; 00084 fSumXZ *= fInvQuantity; 00085 fSumYY *= fInvQuantity; 00086 fSumYZ *= fInvQuantity; 00087 fSumZZ *= fInvQuantity; 00088 00089 // compute eigenvectors for covariance matrix 00090 MgcEigen kES(3); 00091 kES.Matrix(0,0) = fSumXX; 00092 kES.Matrix(0,1) = fSumXY; 00093 kES.Matrix(0,2) = fSumXZ; 00094 kES.Matrix(1,0) = fSumXY; 00095 kES.Matrix(1,1) = fSumYY; 00096 kES.Matrix(1,2) = fSumYZ; 00097 kES.Matrix(2,0) = fSumXZ; 00098 kES.Matrix(2,1) = fSumYZ; 00099 kES.Matrix(2,2) = fSumZZ; 00100 kES.IncrSortEigenStuff3(); 00101 00102 akAxis[0].x = kES.GetEigenvector(0,0); 00103 akAxis[0].y = kES.GetEigenvector(1,0); 00104 akAxis[0].z = kES.GetEigenvector(2,0); 00105 akAxis[1].x = kES.GetEigenvector(0,1); 00106 akAxis[1].y = kES.GetEigenvector(1,1); 00107 akAxis[1].z = kES.GetEigenvector(2,1); 00108 akAxis[2].x = kES.GetEigenvector(0,2); 00109 akAxis[2].y = kES.GetEigenvector(1,2); 00110 akAxis[2].z = kES.GetEigenvector(2,2); 00111 00112 afExtent[0] = kES.GetEigenvalue(0); 00113 afExtent[1] = kES.GetEigenvalue(1); 00114 afExtent[2] = kES.GetEigenvalue(2); 00115 } 00116 //---------------------------------------------------------------------------- 00117 /*bool MgcGaussPointsFit (int iQuantity, const MgcVector2* akPoint, 00118 const bool* abValid, MgcVector2& rkCenter, MgcVector2 akAxis[2], 00119 MgcReal afExtent[2]) 00120 { 00121 // compute mean of points 00122 rkCenter = MgcVector2::ZERO; 00123 int i, iValidQuantity = 0; 00124 for (i = 0; i < iQuantity; i++) 00125 { 00126 if ( abValid[i] ) 00127 { 00128 rkCenter += akPoint[i]; 00129 iValidQuantity++; 00130 } 00131 } 00132 if ( iValidQuantity == 0 ) 00133 return false; 00134 00135 MgcReal fInvQuantity = 1.0/iValidQuantity; 00136 rkCenter *= fInvQuantity; 00137 00138 // compute covariances of points 00139 MgcReal fSumXX = 0.0, fSumXY = 0.0, fSumYY = 0.0; 00140 for (i = 0; i < iQuantity; i++) 00141 { 00142 if ( abValid[i] ) 00143 { 00144 MgcVector2 kDiff = akPoint[i] - rkCenter; 00145 fSumXX += kDiff.x*kDiff.x; 00146 fSumXY += kDiff.x*kDiff.y; 00147 fSumYY += kDiff.y*kDiff.y; 00148 } 00149 } 00150 fSumXX *= fInvQuantity; 00151 fSumXY *= fInvQuantity; 00152 fSumYY *= fInvQuantity; 00153 00154 // solve eigensystem of covariance matrix 00155 MgcEigen kES(2); 00156 kES.Matrix(0,0) = fSumXX; 00157 kES.Matrix(0,1) = fSumXY; 00158 kES.Matrix(1,0) = fSumXY; 00159 kES.Matrix(1,1) = fSumYY; 00160 kES.IncrSortEigenStuff2(); 00161 00162 akAxis[0].x = kES.GetEigenvector(0,0); 00163 akAxis[0].y = kES.GetEigenvector(1,0); 00164 akAxis[1].x = kES.GetEigenvector(0,1); 00165 akAxis[1].y = kES.GetEigenvector(1,1); 00166 00167 afExtent[0] = kES.GetEigenvalue(0); 00168 afExtent[1] = kES.GetEigenvalue(1); 00169 00170 return true; 00171 }*/ 00172 //---------------------------------------------------------------------------- 00173 /*bool MgcGaussPointsFit (int iQuantity, const MgcVector3* akPoint, 00174 const bool* abValid, MgcVector3& rkCenter, MgcVector3 akAxis[3], 00175 MgcReal afExtent[3]) 00176 { 00177 // compute mean of points 00178 rkCenter = MgcVector3::ZERO; 00179 int i, iValidQuantity = 0; 00180 for (i = 0; i < iQuantity; i++) 00181 { 00182 if ( abValid[i] ) 00183 { 00184 rkCenter += akPoint[i]; 00185 iValidQuantity++; 00186 } 00187 } 00188 if ( iValidQuantity == 0 ) 00189 return false; 00190 00191 MgcReal fInvQuantity = 1.0/iValidQuantity; 00192 rkCenter *= fInvQuantity; 00193 00194 // compute covariances of points 00195 MgcReal fSumXX = 0.0, fSumXY = 0.0, fSumXZ = 0.0; 00196 MgcReal fSumYY = 0.0, fSumYZ = 0.0, fSumZZ = 0.0; 00197 for (i = 0; i < iQuantity; i++) 00198 { 00199 if ( abValid[i] ) 00200 { 00201 MgcVector3 kDiff = akPoint[i] - rkCenter; 00202 fSumXX += kDiff.x*kDiff.x; 00203 fSumXY += kDiff.x*kDiff.y; 00204 fSumXZ += kDiff.x*kDiff.z; 00205 fSumYY += kDiff.y*kDiff.y; 00206 fSumYZ += kDiff.y*kDiff.z; 00207 fSumZZ += kDiff.z*kDiff.z; 00208 } 00209 } 00210 fSumXX *= fInvQuantity; 00211 fSumXY *= fInvQuantity; 00212 fSumXZ *= fInvQuantity; 00213 fSumYY *= fInvQuantity; 00214 fSumYZ *= fInvQuantity; 00215 fSumZZ *= fInvQuantity; 00216 00217 // compute eigenvectors for covariance matrix 00218 MgcEigen kES(3); 00219 kES.Matrix(0,0) = fSumXX; 00220 kES.Matrix(0,1) = fSumXY; 00221 kES.Matrix(0,2) = fSumXZ; 00222 kES.Matrix(1,0) = fSumXY; 00223 kES.Matrix(1,1) = fSumYY; 00224 kES.Matrix(1,2) = fSumYZ; 00225 kES.Matrix(2,0) = fSumXZ; 00226 kES.Matrix(2,1) = fSumYZ; 00227 kES.Matrix(2,2) = fSumZZ; 00228 kES.IncrSortEigenStuff3(); 00229 00230 akAxis[0].x = kES.GetEigenvector(0,0); 00231 akAxis[0].y = kES.GetEigenvector(1,0); 00232 akAxis[0].z = kES.GetEigenvector(2,0); 00233 akAxis[1].x = kES.GetEigenvector(0,1); 00234 akAxis[1].y = kES.GetEigenvector(1,1); 00235 akAxis[1].z = kES.GetEigenvector(2,1); 00236 akAxis[2].x = kES.GetEigenvector(0,2); 00237 akAxis[2].y = kES.GetEigenvector(1,2); 00238 akAxis[2].z = kES.GetEigenvector(2,2); 00239 00240 afExtent[0] = kES.GetEigenvalue(0); 00241 afExtent[1] = kES.GetEigenvalue(1); 00242 afExtent[2] = kES.GetEigenvalue(2); 00243 00244 return true; 00245 }*/ 00246 //----------------------------------------------------------------------------
1.4.6-NO