best_fit.cpp

Go to the documentation of this file.
00001 /***************************************************************************
00002  *   Copyright (c) 2007                                                    *
00003  *   Joachim Zettler <Joachim.Zettler@gmx.de>                              *
00004  *   Human Rezai <human@mytum.de>                                          *
00005  *                                                                         *
00006  *   This file is part of the FreeCAD CAx development system.              *
00007  *                                                                         *
00008  *   This library is free software; you can redistribute it and/or         *
00009  *   modify it under the terms of the GNU Library General Public           *
00010  *   License as published by the Free Software Foundation; either          *
00011  *   version 2 of the License, or (at your option) any later version.      *
00012  *                                                                         *
00013  *   This library  is distributed in the hope that it will be useful,      *
00014  *   but WITHOUT ANY WARRANTY; without even the implied warranty of        *
00015  *   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the         *
00016  *   GNU Library General Public License for more details.                  *
00017  *                                                                         *
00018  *   You should have received a copy of the GNU Library General Public     *
00019  *   License along with this library; see the file COPYING.LIB. If not,    *
00020  *   write to the Free Software Foundation, Inc., 59 Temple Place,         *
00021  *   Suite 330, Boston, MA  02111-1307, USA                                *
00022  *                                                                         *
00023  ***************************************************************************/
00024 
00025 /*****************best_fit.CPP*****************
00026 * Contains implementations from best_fit.h
00027 *
00028 *
00029 *********************************************/
00030 
00031 #include "PreCompiled.h"
00032 #include "best_fit.h"
00033 #include "routine.h"
00034 #include <strstream>
00035 
00036 #include <Mod/Mesh/App/Core/Grid.h>
00037 #include <Mod/Mesh/App/Core/Builder.h>
00038 #include <Mod/Mesh/App/Core/TopoAlgorithm.h>
00039 
00040 #include <Base/Builder3D.h>
00041 
00042 #include <BRep_Tool.hxx>
00043 #include "BRepUtils.h"
00044 #include <BRepBuilderAPI_Sewing.hxx>
00045 
00046 //#include <BRepMeshAdapt.hxx>
00047 #include <BRepTools.hxx>
00048 #include <BRepMesh_IncrementalMesh.hxx>
00049 #include <BRepMesh.hxx>
00050 
00051 #include <BRepGProp.hxx>
00052 #include <TopExp_Explorer.hxx>
00053 #include <TopoDS.hxx>
00054 #include <BRepBuilderAPI_Transform.hxx>
00055 #include <GProp_PrincipalProps.hxx>
00056 
00057 #include <Handle_Poly_Triangulation.hxx>
00058 #include <Poly_Triangulation.hxx>
00059 
00060 #include <ANN/ANN.h> // ANN declarations
00061 
00062 #include <SMESH_Gen.hxx>
00063 
00064 
00065 best_fit::best_fit()
00066 {
00067     m_LSPnts.resize(2);
00068 }
00069 
00070 best_fit::~best_fit()
00071 {
00072 }
00073 
00074 void best_fit::Load(const MeshCore::MeshKernel &mesh, const TopoDS_Shape &cad)
00075 {
00076     m_Mesh = mesh;
00077     m_Cad  = cad;
00078 
00079     m_MeshWork = m_Mesh;
00080 
00081 }
00082 
00083 double best_fit::ANN()
00084 {
00085     ANNpointArray   dataPts;                // data points
00086     ANNpoint        queryPt;                // query point
00087     ANNidxArray     nnIdx;                    // near neighbor indices
00088     ANNdistArray    dists;                    // near neighbor distances
00089     ANNkd_tree*     kdTree;                    // search structure
00090 
00091         Base::Builder3D log_error;
00092 
00093     //MeshCore::MeshPointArray meshPnts = m_MeshWork.GetPoints();
00094         m_pntCloud_2;
00095     Base::Vector3f projPoint;
00096 
00097     double error = 0.0;
00098 
00099     int a_dim = 3;
00100     int a_nbPnts =(int) m_pntCloud_2.size();        // Size vom eingescanntem Netz
00101     int a_nbNear = 1;                               // anzahl der rückgabewerte
00102     queryPt = annAllocPt(a_dim);                    // allocate query point storage
00103     dataPts = annAllocPts(a_nbPnts, a_dim);         // allocate data points storage
00104     nnIdx = new ANNidx[a_nbNear];                   // allocate near neigh indices
00105     dists = new ANNdist[a_nbNear];                  // allocatenear neighbor dists
00106 
00107     m_LSPnts[0].clear();
00108     m_LSPnts[1].clear();
00109 
00110     for (int i=0; i<a_nbPnts; ++i)
00111     {
00112         dataPts[i][0] = m_pntCloud_2[i].x;
00113         dataPts[i][1] = m_pntCloud_2[i].y;
00114         dataPts[i][2] = m_pntCloud_2[i].z;
00115     }
00116 
00117     kdTree = new ANNkd_tree(        // build search structure
00118         dataPts,                    // the data points
00119         a_nbPnts,            // number of points
00120         a_dim);                     // dimension of space
00121 
00122 
00123     for (unsigned int i = 0 ; i < m_pntCloud_1.size() ; i++ )
00124     {
00125         queryPt[0] = m_pntCloud_1[i].x;
00126         queryPt[1] = m_pntCloud_1[i].y;
00127         queryPt[2] = m_pntCloud_1[i].z;
00128 
00129         kdTree->annkSearch(                        // search
00130             queryPt,                                                       // query point
00131             a_nbNear,                                                  // number of near neighbors
00132             nnIdx,                                 // nearest neighbors (returned)
00133             dists                                  // distance (returned)
00134         );                                         // error bound
00135 
00136         m_LSPnts[1].push_back(m_pntCloud_1[i]);
00137         m_LSPnts[0].push_back(m_pntCloud_2[nnIdx[0]]);
00138 
00139                 if(m_pntCloud_1[i].z <= m_pntCloud_2[nnIdx[0]].z)
00140                 {
00141                         log_error.addSingleLine(m_pntCloud_1[i],m_pntCloud_2[nnIdx[0]],8,1,0,0);
00142                 }
00143                 else
00144                 {
00145                         log_error.addSingleLine(m_pntCloud_1[i],m_pntCloud_2[nnIdx[0]],8,0,1,0);
00146                 }
00147 
00148                 //if(dists[0] > error)
00149                         error += dists[0];
00150 
00151     }
00152 
00153         log_error.saveToFile("c:/errorVec_fit.iv");
00154 
00155     error /= double(m_pntCloud_1.size());
00156     m_weights_loc = m_weights;
00157 
00158 
00159     delete [] nnIdx; // clean things up
00160     delete [] dists;
00161     delete kdTree;
00162     annClose(); // done with ANN
00163 
00164     return error;
00165 }
00166 
00167 bool best_fit::Perform()
00168 {
00169     Base::Matrix4D M;
00170 
00171     ofstream Runtime_BestFit;
00172         time_t sec1, sec2;
00173 
00174         Runtime_BestFit.open("c:/Runtime_BestFit.txt");
00175         Runtime_BestFit << "Runtime Best-Fit" << std::endl;
00176 
00177 
00178     cout << "tesselate shape" << endl;
00179 
00180         sec1 = time(NULL);
00181           Tesselate_Shape(m_Cad, m_CadMesh, 1); // Tesselates m_Cad Shape and stores Tesselation in m_CadMesh 
00182         sec2 = time(NULL);
00183 
00184         Runtime_BestFit << "Tesselate Shape: " << sec2 - sec1 << " sec" << std::endl;  
00185 
00186         sec1 = time(NULL);
00187       Comp_Weights(); // m_pntCloud_1, m_weights, m_normals des/r Cad-Meshs/Punktewolke werden hier gefüllt
00188     sec2 = time(NULL);
00189 
00190         Runtime_BestFit << "Compute Weights: " << sec2 - sec1 << " sec" << std::endl;  
00191         
00192 
00193         /*RotMat(M, 180, 1);
00194     m_MeshWork.Transform(M);
00195         return true;*/
00196 
00197 
00198         //MeshCore::MeshPointArray pntarr = m_MeshWork.GetPoints();
00199         //MeshCore::MeshFacetArray facetarr = m_MeshWork.GetFacets();
00200 
00201         //for(int i=0; i<pntarr.size(); ++i)
00202         //{
00203         //      pntarr[i].x -= 200;
00204         //      pntarr[i].y -= 200;
00205         //      pntarr[i].z += 50;
00206         //}
00207 
00208         //m_MeshWork.Assign(pntarr,facetarr);
00209 
00210 
00211     sec1 = time(NULL);
00212         
00213         MeshFit_Coarse();  // Transformation Mesh -> CAD
00214     ShapeFit_Coarse(); // Translation    CAD  -> Origin
00215 
00216         //return true;
00217         
00218     M.setToUnity();
00219     M[0][3] = m_cad2orig.X();
00220     M[1][3] = m_cad2orig.Y();
00221     M[2][3] = m_cad2orig.Z();
00222 
00223     m_CadMesh.Transform(M); // besser: tesselierung nach der trafo !!!
00224     m_MeshWork.Transform(M);
00225     PointTransform(m_pntCloud_1,M);
00226 
00227         MeshCore::MeshPointArray pnts = m_MeshWork.GetPoints();
00228 
00229         for (unsigned int i=0; i<pnts.size(); ++i)
00230         {
00231                 m_pntCloud_2.push_back(pnts[i]);
00232 
00233         }
00234 
00235     Runtime_BestFit << "- Error: " << ANN() << endl;
00236 
00237     Coarse_correction();
00238 
00239         sec2 = time(NULL);
00240         Runtime_BestFit << "Coarse Correction: " << sec2-sec1 << " sec" << endl;
00241 
00242     sec1 = time(NULL);
00243         LSM();
00244         sec2 = time(NULL);
00245     Runtime_BestFit << "Least-Square-Matching: " << sec2-sec1 << " sec" << endl;
00246         Runtime_BestFit.close();
00247 
00248     Base::Matrix4D T;
00249     T.setToUnity();
00250     T[0][3] = -m_cad2orig.X();
00251     T[1][3] = -m_cad2orig.Y();
00252     T[2][3] = -m_cad2orig.Z();
00253     m_MeshWork.Transform(T);
00254     m_CadMesh.Transform(T);
00255 
00256         m_Mesh = m_MeshWork;
00257     CompTotalError();
00258 
00259     return true;
00260 
00261 
00262 }
00263 
00264 bool best_fit::Perform_PointCloud()
00265 {
00266         Base::Matrix4D M;
00267 
00268         ofstream Runtime_BestFit;
00269         time_t sec1, sec2;
00270 
00271         Runtime_BestFit.open("c:/Runtime_BestFit_PntCld.txt");
00272         Runtime_BestFit << "Runtime Best-Fit" << std::endl;
00273 
00274         PointCloud_Coarse();  
00275 
00276         M.setToUnity();
00277 
00278         for(unsigned int i=0; i<m_pntCloud_1.size(); i++)
00279                 m_weights.push_back(1.0);
00280         
00281         M[0][3] = m_cad2orig.X();
00282         M[1][3] = m_cad2orig.Y();
00283         M[2][3] = m_cad2orig.Z();
00284 
00285         PointTransform(m_pntCloud_1,M);
00286         PointTransform(m_pntCloud_2,M);
00287 
00288         //Runtime_BestFit << "- Error: " << ANN() << endl;
00289     sec1 = time(NULL);
00290         Coarse_correction();
00291         sec2 = time(NULL);
00292         Runtime_BestFit << "Coarse Correction: " << sec2-sec1 << " sec" << endl;
00293 
00294         //M[0][3] = m_cad2orig.X();
00295         //M[1][3] = m_cad2orig.Y();
00296         //M[2][3] = m_cad2orig.Z();
00297 
00298         //PointTransform(m_pntCloud_1,M);
00299         //PointTransform(m_pntCloud_2,M);
00300 
00301         sec1 = time(NULL);
00302         LSM();
00303         sec2 = time(NULL);
00304         Runtime_BestFit << "Least-Square-Matching: " << sec2-sec1 << " sec" << endl;
00305         Runtime_BestFit.close();
00306 
00307         Base::Matrix4D T;
00308         T.setToUnity();
00309         T[0][3] = -m_cad2orig.X();
00310         T[1][3] = -m_cad2orig.Y();
00311         T[2][3] = -m_cad2orig.Z();
00312         PointTransform(m_pntCloud_1, T);
00313         PointTransform(m_pntCloud_2, T);
00314         m_MeshWork.Transform(T);
00315         m_CadMesh.Transform(T);
00316 
00317         m_Mesh = m_MeshWork;
00318         CompTotalError();
00319 
00320         return true;
00321 }
00322 
00323 /*
00324 bool best_fit::Intersect(const Base::Vector3f &normal,const MeshCore::MeshKernel &mesh, Base::Vector3f &P, Base::Vector3f &I)
00325 {
00326     MeshCore::MeshFacetIterator f_it(mesh);
00327 
00328     for (f_it.Begin(); f_it.More(); f_it.Next())
00329     {
00330         if (intersect_RayTriangle(normal, *f_it, P, I) == 1)
00331             return true;
00332     }
00333 
00334     return false;
00335 }
00336 
00337 int best_fit::intersect_RayTriangle(const Base::Vector3f &normal,const MeshCore::MeshGeomFacet &T, Base::Vector3f &P, Base::Vector3f &I)
00338 {
00339     Base::Vector3f    u, v, n, null(0.0,0.0,0.0), J;          // triangle vectors
00340     Base::Vector3f    dir, w0, w;                             // ray vectors
00341     float             r, a, b;                                // params to calc ray-plane intersect
00342 
00343     J = P+normal;
00344 
00345     // get triangle edge vectors and plane normal
00346     u = T._aclPoints[1] - T._aclPoints[0];
00347     v = T._aclPoints[2] - T._aclPoints[0];
00348     n = u % v;                // cross product
00349     if (n == null)            // triangle is degenerate
00350         return -1;            // do not deal with this case
00351 
00352     dir = normal;    // ray direction vector
00353     w0  = P - T._aclPoints[0];
00354     a   = n * w0;
00355     b   = n * dir;
00356     if (fabs(b) < SMALL_NUM)      // ray is parallel to triangle plane
00357     {
00358         if (a == 0)                // ray lies in triangle plane
00359             return 2;
00360         else return 0;             // ray disjoint from plane
00361     }
00362 
00363     // get intersect point of ray with triangle plane
00364     r = -a / b;
00365 
00366     //if (r < 0.0)                   // ray goes away from triangle
00367     //    return 0;                  // => no intersect
00368     // for a segment, also test if (r > 1.0) => no intersect
00369 
00370     dir.Scale(r,r,r);
00371     I = P + dir;              // intersect point of ray and plane
00372 
00373     // is I inside T?
00374     float    uu, uv, vv, wu, wv, D;
00375     uu = u*u;
00376     uv = u*v;
00377     vv = v*v;
00378     w = I - T._aclPoints[0];
00379     wu = w*u;
00380     wv = w*v;
00381     D = uv * uv - uu * vv;
00382 
00383     // get and test parametric coords
00384     float s, t;
00385     s = (uv * wv - vv * wu) / D;
00386     if (s < 0.0 || s > 1.0)        // I is outside T
00387         return 0;
00388     t = (uv * wu - uu * wv) / D;
00389     if (t < 0.0 || (s + t) > 1.0)  // I is outside T
00390         return 0;
00391 
00392     return 1;                      // I is in T
00393 }
00394 */
00395 
00396 bool best_fit::Coarse_correction()
00397 {
00398     double error, error_tmp, rot = 0.0;
00399     Base::Matrix4D M,T;
00400         ofstream CoarseCorr;
00401 
00402         CoarseCorr.open("c:/CoarseCorr.txt");
00403 
00404     std::vector<Base::Vector3f> m_pntCloud_Work = m_pntCloud_2;
00405 
00406     T.setToUnity();
00407     best_fit befi; 
00408     
00409         //error = CompError_GetPnts(m_pnts, m_normals)[0];  // startfehler    int n=360/rstep_corr;
00410     
00411         error = ANN();
00412 
00413     for (int i=1; i<4; ++i)
00414     {
00415         RotMat(M, 180, i);
00416         PointTransform(m_pntCloud_2, M);
00417                 //m_MeshWork.Transform(M);
00418 
00419         error_tmp = ANN();
00420                 
00421                 //error_tmp = CompError_GetPnts(m_pnts, m_normals)[0];
00422         //error_tmp = befi.CompTotalError(m_MeshWork);
00423                 
00424                 CoarseCorr << i << ", " << error_tmp << endl;
00425 
00426         if (error_tmp < error)
00427         {
00428             T = M;
00429             error = error_tmp;
00430         }
00431 
00432         m_pntCloud_2 = m_pntCloud_Work;
00433     }
00434 
00435         CoarseCorr << "BEST CHOICE: " << error << endl;
00436         CoarseCorr.close();
00437     PointTransform(m_pntCloud_2, T);
00438         m_MeshWork.Transform(T);
00439 
00440 
00441     return true;
00442 }
00443 
00444 
00445 bool best_fit::LSM()
00446 {
00447     double TOL  = 0.05;          // Abbruchkriterium des Newton-Verfahren
00448     int maxIter = 100;            // maximale Anzahl von Iterationen für den Fall,
00449                                                              // dass das Abbruchkriterium nicht erfüllt wird
00450 
00451    int mult = 2;                 // zur Halbierung der Schrittweite bei Misserfolg des Newton Verfahrens
00452 
00453     double val, tmp = 1e+10, delta, delta_tmp = 0.0;
00454     Base::Matrix4D Tx,Ty,Tz,Rx,Ry,Rz,M;   // Transformaitonsmatrizen
00455 
00456         ofstream anOutputFile;
00457         anOutputFile.open("c:/outputBestFit.txt");
00458     anOutputFile.precision(7);
00459 
00460       int c=0; // Laufvariable
00461     
00462 
00463 
00464     std::vector<double> del_x(3,0.0);
00465     std::vector<double>     x(3,0.0); // Startparameter entspricht Nullvektor
00466 
00467     Base::Vector3f centr_l,centr_r, cent;  // Schwerpunkte der Punktesätze
00468 
00469     // Newton Verfahren: 1. Löse  H*del_x = -J
00470     //                   2. Setze x = x + del_x
00471 
00472     std::vector<double> Jac(3);           // 1.Ableitung der Fehlerfunktion (Jacobi-Matrix)
00473     std::vector< std::vector<double> > H; // 2.Ableitung der Fehlerfunktion (Hesse-Matrix)
00474 
00475     time_t seconds1, seconds2, sec1, sec2;
00476     seconds1 = time(NULL);
00477 
00478     while (true)
00479     {
00480 
00481         seconds1 = time(NULL);
00482         //m_Mesh = m_MeshWork;
00483         
00484                 // Fehlerberechnung vom CAD -> Mesh
00485         //tmp = CompError_GetPnts(m_pnts, m_normals);      // hier: - Berechnung der LS-Punktesätze
00486         //      CompTotalError()                           //       - Berechnung der zugehörigen Gewichtungen
00487 
00488                 delta = delta_tmp;
00489         delta_tmp = ANN();          // gibt durchschnittlichen absoluten Fehler aus
00490                 delta = delta - delta_tmp ; // hier wird die Fehlerverbesserung zum vorigen Iterationsschritt gespeichert
00491 
00492                 if (c==maxIter || delta < ERR_TOL && c>1) break; // Abbruchkriterium (falls maximale Iterationsschrite erreicht
00493                                                                                                  //                   oder falls Fehleränderung unsignifikant gering)
00494 
00495         seconds2 = time(NULL);
00496                 anOutputFile << c << ", " << delta_tmp << ", " << delta << "    -    Time: " << seconds2 - seconds1 << " sec" << endl;
00497                 seconds1 = time(NULL);
00498 
00499                 sec1 = time(NULL);
00500         for (unsigned int i=0; i<x.size(); ++i) x[i] = 0.0; // setzt startwerte für newton auf null
00501 
00502 
00503         // Berechne gewichtete Schwerpunkte und verschiebe die Punktesätze entsprechend:
00504         centr_l.Scale(0.0,0.0,0.0);
00505         centr_r.Scale(0.0,0.0,0.0);
00506 
00507         Base::Vector3f p,q;
00508                 double Sum = 0.0;
00509         for (unsigned int i=0; i<m_LSPnts[0].size(); ++i)
00510         {
00511             p = m_LSPnts[0][i];
00512             q = m_LSPnts[1][i];
00513 
00514             p.Scale((float) m_weights_loc[i],(float) m_weights_loc[i],(float) m_weights_loc[i]);
00515             q.Scale((float) m_weights_loc[i],(float) m_weights_loc[i],(float) m_weights_loc[i]);
00516 
00517                         Sum += m_weights_loc[i];
00518                         centr_l += p;
00519             centr_r += q;
00520         }
00521 
00522                 centr_l.Scale( -1.0f/float(Sum), -1.0f/float(Sum), -1.0f/float(Sum));
00523                 centr_r.Scale( -1.0f/(float)Sum, -1.0f/(float)Sum, -1.0f/(float)Sum);
00524 
00525        /* float s = (float) m_LSPnts[0].size();
00526         s = float(-1.0/s);
00527 
00528         centr_l.Scale(s,s,s);
00529         centr_r.Scale(s,s,s);*/
00530 
00531 
00532 
00533         // Verschiebung der Schwerpunkte zum Ursprung
00534         TransMat(Tx,centr_l.x,1);
00535         TransMat(Ty,centr_l.y,2);
00536         TransMat(Tz,centr_l.z,3);
00537 
00538         M = Tx*Ty*Tz;
00539         PointTransform(m_LSPnts[0],M);
00540                 PointTransform(m_pntCloud_1,M);
00541                 PointTransform(m_pntCloud_2,M);
00542         m_MeshWork.Transform(M);
00543 
00544         TransMat(Tx,centr_r.x,1); // Berechnung der Translationsmatrix in x-Richtung
00545         TransMat(Ty,centr_r.y,2); // Berechnung der Translationsmatrix in y-Richtung
00546         TransMat(Tz,centr_r.z,3); // Berechnung der Translationsmatrix in z-Richtung
00547 
00548         M = Tx*Ty*Tz;                  // Zusammenfügen zu einer Gesamttranslationsmatrix
00549         PointTransform(m_LSPnts[1],M); // Anwendung der Translation auf m_LSPnts
00550         PointTransform(m_pntCloud_1,M);
00551                 //PointNormalTransform(m_pnts, m_normals, M); // Anwendung der Translation auf m_pnts
00552         m_CadMesh.Transform(M);                       // Anwendung der Translation auf das CadMesh
00553 
00554         sec2 = time(NULL);
00555                 anOutputFile << c+1 << " - Initialisierung und Transformation um gewichtete Schwerpunkte: " << sec2 - sec1 << " sec" << endl;
00556 
00557                 sec1 = time(NULL);
00558         // Newton-Verfahren zur Berechnung der Rotationsmatrix:
00559         while (true)
00560         {
00561 
00562             Jac = Comp_Jacobi(x);  // berechne 1.Ableitung
00563             H   = Comp_Hess(x);    // berechne 2.Ableitung
00564 
00565             val = 0.0;
00566             for (unsigned int i=0; i<Jac.size(); ++i)
00567             {
00568                 val += Jac[i]*Jac[i];
00569             }
00570             val = sqrt(val);
00571 
00572             if (val < TOL) break; // Abbruchkriterium des Newton-Verfahren
00573 
00574             if (val>tmp && mult < 1e+4)
00575             {
00576                 for (unsigned int i=0; i<del_x.size(); ++i)
00577                 {
00578                     x[i] -= del_x[i]/double(mult);  // Halbiere Schrittweite falls keine Verbesserung
00579                 }
00580                 mult *= 2;
00581                 continue;
00582             }
00583             else
00584             {
00585                 mult = 2;
00586             }
00587 
00588             tmp = val;
00589 
00590             del_x = Routines::NewtonStep(Jac,H);      // löst Gl.system:          H*del_x = -J
00591             for (unsigned int i=0; i<x.size(); ++i)   // nächster Iterationswert: x = x + del_x
00592             {
00593                 x[i] += del_x[i];
00594             }
00595         }
00596 
00597                 sec2 = time(NULL);
00598                 anOutputFile << c+1 << " - Newton: " << seconds2 - seconds1 << " sec" << endl;
00599         sec1 = time(NULL);
00600         // Rotiere und verschiebe zurück zum Ursprung der !!! CAD-Geometrie !!!
00601         RotMat  (Rx,(x[0]*180.0/PI),1);
00602         RotMat  (Ry,(x[1]*180.0/PI),2);
00603         RotMat  (Rz,(x[2]*180.0/PI),3);
00604 
00605                 Base::Matrix4D R = Rx*Ry*Rz;
00606                 centr_l.Scale(-1.0, -1.0, -1.0);
00607 
00608                 cent.x =(float)  (R[0][0]*centr_l.x + R[0][1]*centr_l.y + R[0][2]*centr_l.z);
00609                 cent.y =(float)  (R[1][0]*centr_l.x + R[1][1]*centr_l.y + R[1][2]*centr_l.z);
00610                 cent.z =(float)  (R[2][0]*centr_l.x + R[2][1]*centr_l.y + R[2][2]*centr_l.z);
00611                          
00612                 TransMat(Tx, -centr_r.x - cent.x + centr_l.x, 1);
00613         TransMat(Ty, -centr_r.y - cent.y + centr_l.y, 2);
00614                 TransMat(Tz, -centr_r.z - cent.z + centr_l.z, 3);
00615 
00616         M = Tx*Ty*Tz*Rx*Ry*Rz; // Rotiere zuerst !!! (Rotationen stets um den Nullpunkt...)
00617         
00618                 PointTransform(m_pntCloud_2,M);
00619                 m_MeshWork.Transform(M);
00620 
00621                 TransMat(Tx, -centr_r.x, 1);
00622                 TransMat(Ty, -centr_r.y, 2);
00623                 TransMat(Tz, -centr_r.z, 3);
00624 
00625         M = Tx*Ty*Tz;
00626                 PointTransform(m_pntCloud_1,M);
00627         m_CadMesh.Transform(M);
00628         //PointNormalTransform(m_pnts, m_normals, M);
00629 
00630                 sec2 = time(NULL);
00631                 
00632                 anOutputFile << c+1 << " - Trafo: " << seconds2 - seconds1 << " sec" << endl;
00633         ++c;  //Erhöhe Laufvariable 
00634     }
00635 
00636         anOutputFile.close();
00637 return true;
00638 
00639     /*TransMat(Tx,-centr_l.x,1);
00640        TransMat(Ty,-centr_l.y,2);
00641     TransMat(Tz,-centr_l.z,3);
00642 
00643     M = Tx*Ty*Tz;
00644     PointTransform(m_LSPnts[0],M);
00645 
00646     TransMat(Tx,-centr_r.x,1);
00647        TransMat(Ty,-centr_r.y,2);
00648     TransMat(Tz,-centr_r.z,3);
00649 
00650     M = Tx*Ty*Tz;
00651     PointTransform(m_LSPnts[1],M);
00652 
00653     Base::Builder3D log;
00654 
00655     for(unsigned int i=0; i<m_LSPnts[0].size(); ++i)
00656      log.addSingleArrow(m_LSPnts[0][i],m_LSPnts[1][i]);
00657 
00658     log.saveToFile("c:/newton_pnts.iv");*/
00659 
00660   
00661 }
00662 
00663 
00664 std::vector<double> best_fit::Comp_Jacobi(const std::vector<double> &x)
00665 {
00666     std::vector<double> F(3,0.0);
00667     double s1 = sin(x[0]), c1 = cos(x[0]);
00668     double s2 = sin(x[1]), c2 = cos(x[1]);
00669     double s3 = sin(x[2]), c3 = cos(x[2]);
00670 
00671     Base::Vector3f p,q;
00672 
00673     for (unsigned int i=0; i<m_LSPnts[0].size(); ++i)
00674     {
00675         p = m_LSPnts[0][i];
00676         q = m_LSPnts[1][i];
00677 
00678         F[0] += ( q.y * ( (-c1*s2*c3-s1*s3) * p.x +  (c1*s2*s3-s1*c3) * p.y + (-c1*c2) * p.z ) +
00679                   q.z * ( (-s1*s2*c3+c1*s3) * p.x +  (s1*s2*s3+c1*c3) * p.y + (-s1*c2) * p.z ) )*m_weights_loc[i];
00680 
00681         F[1] += ( q.x * (          (-s2*c3) * p.x +           (s2*s3) * p.y +    (-c2) * p.z ) +
00682                   q.y * (       (-s1*c2*c3) * p.x +        (s1*c2*s3) * p.y +  (s1*s2) * p.z ) +
00683                   q.z * (        (c1*c2*c3) * p.x +       (-c1*c2*s3) * p.y + (-c1*s2) * p.z ) )*m_weights_loc[i];
00684 
00685         F[2] += ( q.x * (          (-c2*s3) * p.x +          (-c2*c3) * p.y) +
00686                   q.y * (  (s1*s2*s3+c1*c3) * p.x +  (s1*s2*c3-c1*s3) * p.y) +
00687                   q.z * ( (-c1*s2*s3+s1*c3) * p.x + (-c1*s2*c3-s1*s3) * p.y) )*m_weights_loc[i];
00688     }
00689 
00690     return F;
00691 }
00692 
00693 std::vector<std::vector<double> > best_fit::Comp_Hess(const std::vector<double> &x)
00694 {
00695     std::vector<std::vector<double> > DF(3);
00696     for (unsigned int i=0; i<DF.size(); ++i)
00697     {
00698         DF[i].resize(3,0.0);
00699     }
00700 
00701     double s1 = sin(x[0]), c1 = cos(x[0]);
00702     double s2 = sin(x[1]), c2 = cos(x[1]);
00703     double s3 = sin(x[2]), c3 = cos(x[2]);
00704 
00705     double sum1 = 0.0, sum2 = 0.0, sum3 = 0.0, sum4 = 0.0, sum5 = 0.0, sum6 = 0.0;
00706 
00707     Base::Vector3f p,q;
00708 
00709     for (unsigned int i=0; i<m_LSPnts[0].size(); ++i)
00710     {
00711         p = m_LSPnts[0][i];
00712         q = m_LSPnts[1][i];
00713 
00714         sum1      = q.y * (  (s1*s2*c3-c1*s3) * p.x + (-s1*s2*s3-c1*c3) * p.y +  (s1*c2) * p.z ) +
00715                     q.z * ( (-c1*s2*c3-s1*s3) * p.x +  (c1*s2*s3-s1*c3) * p.y + (-c1*c2) * p.z );
00716 
00717         sum2      = q.x * (          (-c2*c3) * p.x +           (c2*s3) * p.y +     (s2) * p.z ) +
00718                     q.y * (        (s1*s2*c3) * p.x +       (-s1*s2*s3) * p.y +  (s1*c2) * p.z ) +
00719                     q.z * (       (-c1*s2*c3) * p.x +        (c1*s2*s3) * p.y + (-c1*c2) * p.z );
00720 
00721         sum3      = q.x * (          (-c2*c3) * p.x +           (c2*s3) * p.y) +
00722                     q.y * (  (s1*s2*c3-c1*s3) * p.x + (-s1*s2*s3-c1*c3) * p.y) +
00723                     q.z * ( (-c1*s2*c3-s1*s3) * p.x +  (c1*s2*s3-s1*c3) * p.y);
00724 
00725         sum4      = q.y * (       (-c1*c2*c3) * p.x +        (c1*c2*s3) * p.y +  (c1*s2) * p.z ) +
00726                     q.z * (       (-s1*c2*c3) * p.x +        (s1*c2*s3) * p.y +  (s1*s2) * p.z );
00727 
00728         sum5      = q.y * (  (c1*s2*s3-s1*c3) * p.x +  (c1*s2*c3+s1*s3) * p.y) +
00729                     q.z * (  (s1*s2*s3+c1*c3) * p.x +  (s1*s2*c3-c1*s3) * p.y);
00730 
00731         sum6      = q.x * (           (s2*s3) * p.x +           (s2*c3) * p.y) +
00732                     q.y * (        (s1*c2*s3) * p.x +        (s1*c2*c3) * p.y) +
00733                     q.z * (       (-c1*c2*s3) * p.x +       (-c1*c2*c3) * p.y);
00734 
00735         DF[0][0] += sum1*m_weights_loc[i];
00736         DF[1][1] += sum2*m_weights_loc[i];
00737         DF[2][2] += sum3*m_weights_loc[i];
00738         DF[0][1] += sum4*m_weights_loc[i];
00739         DF[1][0] += sum4*m_weights_loc[i];
00740         DF[0][2] += sum5*m_weights_loc[i];
00741         DF[2][0] += sum5*m_weights_loc[i];
00742         DF[1][2] += sum6*m_weights_loc[i];
00743         DF[2][1] += sum6*m_weights_loc[i];
00744     }
00745 
00746     return DF;
00747 }
00748 
00749 bool best_fit::Comp_Weights()
00750 {
00751         double weight_low = 1, weight_high = 2;
00752     TopExp_Explorer aExpFace;
00753     MeshCore::MeshKernel FaceMesh;
00754     MeshCore::MeshFacetArray facetArr;
00755 
00756     MeshCore::MeshKernel mesh1,mesh2;
00757     MeshCore::MeshBuilder builder1(mesh1);
00758     MeshCore::MeshBuilder builder2(mesh2);
00759     builder1.Initialize(1000);
00760     builder2.Initialize(1000);
00761 
00762     Base::Vector3f Points[3];
00763     TopLoc_Location aLocation;
00764 
00765     bool bf;
00766 
00767     // explores all faces  ------------  Hauptschleife
00768     for (aExpFace.Init(m_Cad,TopAbs_FACE);aExpFace.More();aExpFace.Next())
00769     {
00770         TopoDS_Face aFace = TopoDS::Face(aExpFace.Current());
00771 
00772         bf = false;
00773         for (unsigned int i=0; i<m_LowFaces.size(); ++i)
00774         {
00775             if (m_LowFaces[i].HashCode(IntegerLast()) == aFace.HashCode(IntegerLast())) bf = true;
00776         }
00777 
00778         // takes the triangulation of the face aFace:
00779         Handle_Poly_Triangulation aTr = BRep_Tool::Triangulation(aFace,aLocation);
00780         if (!aTr.IsNull()) // if this triangulation is not NULL
00781         {
00782             // takes the array of nodes for this triangulation:
00783             const TColgp_Array1OfPnt& aNodes = aTr->Nodes();
00784 
00785             // takes the array of triangles for this triangulation:
00786             const Poly_Array1OfTriangle& triangles = aTr->Triangles();
00787 
00788             // create array of node points in absolute coordinate system
00789             TColgp_Array1OfPnt aPoints(1, aNodes.Length());
00790             for ( Standard_Integer i = 1; i <= aNodes.Length(); i++)
00791                 aPoints(i) = aNodes(i).Transformed(aLocation);
00792 
00793             // Takes the node points of each triangle of this triangulation.
00794             // takes a number of triangles:
00795             Standard_Integer nnn = aTr->NbTriangles();
00796             Standard_Integer nt,n1,n2,n3;
00797             for ( nt = 1 ; nt < nnn+1 ; nt++)
00798             {
00799                 // takes the node indices of each triangle in n1,n2,n3:
00800                 triangles(nt).Get(n1,n2,n3);
00801                 // takes the node points:
00802                 gp_Pnt aPnt1 = aPoints(n1);
00803                 Points[0].Set(float(aPnt1.X()),float(aPnt1.Y()),float(aPnt1.Z()));
00804                 gp_Pnt aPnt2 = aPoints(n2);
00805                 Points[1].Set((float) aPnt2.X(),(float) aPnt2.Y(),(float) aPnt2.Z());
00806                 gp_Pnt aPnt3 = aPoints(n3);
00807                 Points[2].Set((float) aPnt3.X(),(float) aPnt3.Y(),(float) aPnt3.Z());
00808 
00809                 // give the occ faces to the internal mesh structure of freecad
00810                 MeshCore::MeshGeomFacet Face(Points[0],Points[1],Points[2]);
00811 
00812                 if (bf == false) builder1.AddFacet(Face);
00813                 else   builder2.AddFacet(Face);
00814 
00815             }
00816         }
00817     }
00818 
00819     builder1.Finish();
00820     builder2.Finish();
00821 
00822     m_pntCloud_1.clear();
00823     m_weights.clear();
00824 
00825     MeshCore::MeshPointArray pnts = mesh1.GetPoints();
00826 
00827     for (unsigned int i=0; i<pnts.size(); ++i)
00828     {
00829         m_pntCloud_1.push_back(pnts[i]);
00830         m_weights.push_back(weight_low);
00831     }
00832 
00833     pnts = mesh2.GetPoints();
00834 
00835     for (unsigned int i=0; i<pnts.size(); ++i)
00836     {
00837         m_pntCloud_1.push_back(pnts[i]);
00838         m_weights.push_back(weight_high);
00839     }
00840 
00841     m_normals = Comp_Normals(mesh1);
00842     std::vector<Base::Vector3f> tmp = Comp_Normals(mesh2);
00843 
00844     for (unsigned int i=0; i<tmp.size(); ++i)
00845     {
00846         m_normals.push_back(tmp[i]);
00847     }
00848 
00849     return true;
00850 }
00851 
00852 bool best_fit::RotMat(Base::Matrix4D &M, double degree, int axis)
00853 {
00854     M.setToUnity();
00855     degree = 2*PI*degree/360;  // trafo bogenmaß
00856 
00857     switch (axis)
00858     {
00859     case 1:
00860         M[1][1]=cos(degree);
00861         M[1][2]=-sin(degree);
00862         M[2][1]=sin(degree);
00863         M[2][2]=cos(degree);
00864         break;
00865     case 2:
00866         M[0][0]=cos(degree);
00867         M[0][2]=-sin(degree);
00868         M[2][0]=sin(degree);
00869         M[2][2]=cos(degree);
00870         break;
00871     case 3:
00872         M[0][0]=cos(degree);
00873         M[0][1]=-sin(degree);
00874         M[1][0]=sin(degree);
00875         M[1][1]=cos(degree);
00876         break;
00877     default:
00878         throw Base::Exception("second input value differs from 1,2,3 (x,y,z)");
00879     }
00880 
00881     return true;
00882 }
00883 
00884 bool best_fit::TransMat(Base::Matrix4D &M, double trans, int axis)
00885 {
00886     M.setToUnity();
00887 
00888     switch (axis)
00889     {
00890     case 1:
00891         M[0][3] = trans;
00892         break;
00893     case 2:
00894         M[1][3] = trans;
00895         break;
00896     case 3:
00897         M[2][3] = trans;
00898         break;
00899     default:
00900         throw Base::Exception("second input value differs from 1,2,3 (x,y,z)");
00901     }
00902 
00903     return true;
00904 }
00905 
00906 bool best_fit::PointNormalTransform(std::vector<Base::Vector3f> &pnts,
00907                                     std::vector<Base::Vector3f> &normals,
00908                                     Base::Matrix4D              &M)
00909 {
00910     int m = pnts.size();
00911     Base::Vector3f pnt,normal;
00912 
00913     for (int i=0; i<m; ++i)
00914     {
00915         pnt.x  = float(M[0][0]*pnts[i].x + M[0][1]*pnts[i].y + M[0][2]*pnts[i].z + M[0][3]);
00916         pnt.y  = float(M[1][0]*pnts[i].x + M[1][1]*pnts[i].y + M[1][2]*pnts[i].z + M[1][3]);
00917         pnt.z  = float(M[2][0]*pnts[i].x + M[2][1]*pnts[i].y + M[2][2]*pnts[i].z + M[2][3]);
00918 
00919         pnts[i] = pnt;
00920 
00921         normal.x = float(M[0][0]*normals[i].x + M[0][1]*normals[i].y + M[0][2]*normals[i].z);
00922         normal.y = float(M[1][0]*normals[i].x + M[1][1]*normals[i].y + M[1][2]*normals[i].z);
00923         normal.z = float(M[2][0]*normals[i].x + M[2][1]*normals[i].y + M[2][2]*normals[i].z);
00924 
00925         normals[i] = normal;
00926     }
00927 
00928     return true;
00929 }
00930 
00931 bool best_fit::output_best_fit_mesh()
00932 {
00933         
00934         SMDS_NodeIteratorPtr aNodeIter = m_meshtobefit->GetMeshDS()->nodesIterator();
00935 
00936         for(;aNodeIter->more();) 
00937         {
00938                 const SMDS_MeshNode* aNode = aNodeIter->next();
00939                 m_meshtobefit->GetMeshDS()->MoveNode(aNode,m_pntCloud_2[(aNode->GetID()-1)].x,m_pntCloud_2[(aNode->GetID()-1)].y,m_pntCloud_2[(aNode->GetID()-1)].z);
00940         }
00941         m_meshtobefit->ExportUNV("c:/best_fit_mesh.unv");
00942 
00943         return true;
00944 }
00945 
00946 bool best_fit::Initialize_Mesh_Geometrie_1()
00947 {
00948         m_aMeshGen1 = new SMESH_Gen();
00949         m_referencemesh = m_aMeshGen1->CreateMesh(1,false);
00950         m_referencemesh->UNVToMesh("c:/cad_mesh_cenaero.unv");
00951 
00952         m_pntCloud_1.clear();
00953 
00954         //add the nodes
00955         SMDS_NodeIteratorPtr aNodeIter = m_referencemesh->GetMeshDS()->nodesIterator();
00956         for(;aNodeIter->more();) {
00957                 const SMDS_MeshNode* aNode = aNodeIter->next();
00958                 Base::Vector3f a3DVector;
00959                 a3DVector.Set((float) aNode->X(),(float)  aNode->Y(),(float)  aNode->Z()),
00960                 m_pntCloud_1.push_back(a3DVector);
00961     }
00962 
00963 
00964 
00965         return true;
00966 }
00967 
00968 
00969 
00970 bool best_fit::Initialize_Mesh_Geometrie_2()
00971 {
00972 
00973         m_aMeshGen2 = new SMESH_Gen();
00974         m_meshtobefit = m_aMeshGen2->CreateMesh(1,false);
00975         m_meshtobefit->UNVToMesh("c:/mesh_cenaero.unv");
00976 
00977         m_pntCloud_2.clear();
00978 
00979         //add the nodes
00980         SMDS_NodeIteratorPtr aNodeIter = m_meshtobefit->GetMeshDS()->nodesIterator();
00981         for(;aNodeIter->more();) {
00982                 const SMDS_MeshNode* aNode = aNodeIter->next();
00983                 Base::Vector3f a3DVector;
00984                 a3DVector.Set((float) aNode->X(),(float)  aNode->Y(), (float) aNode->Z()),
00985                 m_pntCloud_2.push_back(a3DVector);
00986         }
00987 
00989         //SMDS_EdgeIteratorPtr  anEdgeIter = Reference_Mesh->GetMeshDS()->edgesIterator();
00990         //for(;anEdgeIter->more();) {
00991         //      const SMDS_MeshEdge* anElem = anEdgeIter->next();
00992         //      myElements.push_back( anElem->GetID() );
00993         //}
00995         //SMDS_FaceIteratorPtr  aFaceIter = Reference_Mesh->GetMeshDS()->facesIterator();
00996         //for(;aFaceIter->more();) {
00997         //      const SMDS_MeshFace* anElem = aFaceIter->next();
00998         //      int element_node_count = anElem->NbNodes();
00999         //      myElements.push_back( anElem->GetID() );
01000         //}
01002         //SMDS_VolumeIteratorPtr aVolumeIter = Reference_Mesh->GetMeshDS()->volumesIterator();
01003         //for(;aVolumeIter->more();) {
01004         //      const SMDS_MeshVolume* anElem = aVolumeIter->next();
01005         //      myElements.push_back( anElem->GetID() );
01006         //}
01007 
01008         //int testsize = myElements.size();
01009 
01010         //SMDS_VolumeTool aTooling;
01011 
01012 
01015         //for (unsigned int i=0;i<myElements.size();i++)
01016         //{
01017         //      const SMDS_MeshElement* CurrentElement = Reference_Mesh->GetMeshDS()->FindElement(myElements[i]);
01018         //      if (CurrentElement->GetType() == SMDSAbs_Volume) 
01019         //      {
01020         //              //We encountered a Surface-Element like a triangle and we have to check if its a triangle or not
01021         //              aTooling.Set(CurrentElement);
01022         //              //Now we have to check what kind of volume element we have
01023         //              if(aTooling.GetVolumeType()== SMDS_VolumeTool::HEXA)
01024         //              {
01025         //                      //Found a HEXA Element
01026         //              }
01027         //      }
01028         //}
01029 
01030         return true;
01031 }
01032 
01033 bool best_fit::PointTransform(std::vector<Base::Vector3f> &pnts, const Base::Matrix4D &M)
01034 {
01035     int m = pnts.size();
01036     Base::Vector3f pnt;
01037 
01038     for (int i=0; i<m; ++i)
01039     {
01040         pnt.x  =  float(M[0][0]*pnts[i].x + M[0][1]*pnts[i].y + M[0][2]*pnts[i].z + M[0][3]);
01041         pnt.y  =  float(M[1][0]*pnts[i].x + M[1][1]*pnts[i].y + M[1][2]*pnts[i].z + M[1][3]);
01042         pnt.z  =  float(M[2][0]*pnts[i].x + M[2][1]*pnts[i].y + M[2][2]*pnts[i].z + M[2][3]);
01043 
01044         pnts[i] = pnt;
01045     }
01046 
01047     return true;
01048 }
01049 
01050 bool best_fit::PointCloud_Coarse()
01051 {
01052         GProp_GProps prop;
01053         GProp_PrincipalProps pprop;
01054 
01055         MeshCore::PlaneFit FitFunc_1, FitFunc_2;
01056 
01057         Base::Vector3f pnt(0.0,0.0,0.0);
01058         Base::Vector3f DirA_1, DirB_1, DirC_1, Grav_1,
01059                            DirA_2, DirB_2, DirC_2, Grav_2;
01060         Base::Vector3f x,y,z;
01061         Base::Builder3D log3d_mesh, log3d_cad;
01062         gp_Pnt orig;
01063 
01064         gp_Vec v1,v2,v3,v,vec; // Hauptachsenrichtungen
01065         gp_Trsf trafo;
01066 
01067         FitFunc_1.Clear();
01068         FitFunc_2.Clear();
01069 
01070         FitFunc_1.AddPoints(m_pntCloud_1);
01071         FitFunc_2.AddPoints(m_pntCloud_2);
01072         
01073         FitFunc_1.Fit();
01074         FitFunc_2.Fit();
01075 
01076         DirA_1 = FitFunc_1.GetDirU();
01077         DirB_1 = FitFunc_1.GetDirV();
01078         DirC_1 = FitFunc_1.GetNormal();
01079         Grav_1 = FitFunc_1.GetGravity();
01080 
01081         m_cad2orig.SetX(-Grav_1.x);
01082         m_cad2orig.SetY(-Grav_1.y);
01083         m_cad2orig.SetZ(-Grav_1.z);
01084 
01085         DirA_2 = FitFunc_2.GetDirU();
01086         DirB_2 = FitFunc_2.GetDirV();
01087         DirC_2 = FitFunc_2.GetNormal();
01088         Grav_2 = FitFunc_2.GetGravity();
01089 
01090         Base::Matrix4D T5, T1;
01091 
01092         // Füllt Matrix T5 
01093         T5[0][0] = DirA_1.x;
01094         T5[1][0] = DirA_1.y;
01095         T5[2][0] = DirA_1.z;
01096 
01097         T5[0][1] = DirB_1.x;
01098         T5[1][1] = DirB_1.y;
01099         T5[2][1] = DirB_1.z;
01100 
01101         T5[0][2] = DirC_1.x;
01102         T5[1][2] = DirC_1.y;
01103         T5[2][2] = DirC_1.z;
01104 
01105         T5[0][3] = Grav_1.x;
01106         T5[1][3] = Grav_1.y;
01107         T5[2][3] = Grav_1.z;
01108 
01109         /*T5[0][0] = DirA_1.x;
01110         T5[0][1] = DirA_1.y;
01111         T5[0][2] = DirA_1.z;
01112 
01113         T5[1][0] = DirB_1.x;
01114         T5[1][1] = DirB_1.y;
01115         T5[1][2] = DirB_1.z;
01116 
01117         T5[2][0] = DirC_1.x;
01118         T5[2][1] = DirC_1.y;
01119         T5[2][2] = DirC_1.z;
01120 
01121         T5[0][3] = Grav_1.x;
01122         T5[1][3] = Grav_1.y;
01123         T5[2][3] = Grav_1.z;*/
01124 
01125 
01126         // Füllt Matrix T1
01127         T1[0][0] = DirA_2.x;
01128         T1[1][0] = DirA_2.y;
01129         T1[2][0] = DirA_2.z;
01130 
01131         T1[0][1] = DirB_2.x;
01132         T1[1][1] = DirB_2.y;
01133         T1[2][1] = DirB_2.z;
01134 
01135         T1[0][2] = DirC_2.x;
01136         T1[1][2] = DirC_2.y;
01137         T1[2][2] = DirC_2.z;
01138 
01139         T1[0][3] = Grav_2.x;
01140         T1[1][3] = Grav_2.y;
01141         T1[2][3] = Grav_2.z;
01142 
01143         v1.SetX(T5[0][0]);v1.SetY(T5[0][1]);v1.SetZ(T5[0][2]);
01144         v2.SetX(T5[1][0]);v2.SetY(T5[1][1]);v2.SetZ(T5[1][2]);
01145         v3.SetX(T5[2][0]);v3.SetY(T5[2][1]);v3.SetZ(T5[2][2]);
01146 
01147         v1.Normalize();
01148         v2.Normalize();
01149         v3.Normalize();
01150 
01151         v = v1;
01152         v.Cross(v2);
01153 
01154         // right-hand-system check
01155         if ( v.Dot(v3) < 0.0 )
01156                 v3 *= -1;
01157 
01158         T1.inverse();
01159 
01160         orig.SetX(T5[0][3]);orig.SetY(T5[1][3]);orig.SetZ(T5[2][3]);
01161 
01162         // plot CAD -> local coordinate system
01163 
01164         x.x =  50.0f*(float)v1.X();     x.y =  50.0f*(float)v1.Y();     x.z =  50.0f*(float)v1.Z();
01165         y.x =  50.0f*(float)v2.X();     y.y =  50.0f*(float)v2.Y();     y.z =  50.0f*(float)v2.Z();
01166         z.x =  50.0f*(float)v3.X();     z.y =  50.0f*(float)v3.Y();     z.z =  50.0f*(float)v3.Z();
01167 
01168         pnt.x = (float) orig.X();
01169         pnt.y = (float) orig.Y();
01170         pnt.z = (float) orig.Z();
01171 
01172         log3d_cad.addSingleArrow(pnt,x,3,1,0,0);
01173         log3d_cad.addSingleArrow(pnt,y,3,0,1,0);
01174         log3d_cad.addSingleArrow(pnt,z,3,0,0,1);
01175 
01176         //log3d_cad.addSinglePoint(pnt,6,1,1,1);
01177 
01178         log3d_cad.saveToFile("c:/CAD_CoordSys.iv");
01179 
01180         PointTransform(m_pntCloud_2,T5*T1);
01181 
01182         //m_MeshWork.Transform(T1);
01183         // plot Mesh -> local coordinate system
01184 
01185         v1.SetX(T1[0][0]);v1.SetY(T1[0][1]);v1.SetZ(T1[0][2]);
01186         v2.SetX(T1[1][0]);v2.SetY(T1[1][1]);v2.SetZ(T1[1][2]);
01187         v3.SetX(T1[2][0]);v3.SetY(T1[2][1]);v3.SetZ(T1[2][2]);
01188 
01189         T1.inverse();
01190         orig.SetX(T1[0][3]);orig.SetY(T1[1][3]);orig.SetZ(T1[2][3]);
01191 
01192         x.x =  50.0f*(float)v1.X();     x.y =  50.0f*(float)v1.Y();     x.z =  50.0f*(float)v1.Z();
01193         y.x =  50.0f*(float)v2.X();     y.y =  50.0f*(float)v2.Y();     y.z =  50.0f*(float)v2.Z();
01194         z.x =  50.0f*(float)v3.X();     z.y =  50.0f*(float)v3.Y();     z.z =  50.0f*(float)v3.Z();
01195 
01196         pnt.x = (float) orig.X();
01197         pnt.y = (float) orig.Y();
01198         pnt.z = (float) orig.Z();
01199 
01200         log3d_mesh.addSingleArrow(pnt,x,3,1,0,0);log3d_mesh.addSingleArrow(pnt,y,3,0,1,0);log3d_mesh.addSingleArrow(pnt,z,3,0,0,1);
01201         log3d_mesh.addSinglePoint(0,0,0,20,1,1,1); // plotte Ursprung
01202         //log3d_mesh.addSinglePoint(pnt,6,0,0,0);
01203         log3d_mesh.saveToFile("c:/Mesh_CoordSys.iv");
01204 
01205         /*for(int i=0; i< m_pntCloud_2.size(); i++)
01206         {
01207                 m_pntCloud_2[i].x = m_pntCloud_2[i].x + Grav_1.x - Grav_2.x;
01208                 m_pntCloud_2[i].y = m_pntCloud_2[i].y + Grav_1.y - Grav_2.y;
01209                 m_pntCloud_2[i].z = m_pntCloud_2[i].z + Grav_1.z - Grav_2.z;
01210         }*/
01211 
01212         return true;
01213 }
01214 bool best_fit::MeshFit_Coarse()
01215 {
01216 
01217     GProp_GProps prop;
01218     GProp_PrincipalProps pprop;
01219 
01220     Base::Vector3f pnt(0.0,0.0,0.0);
01221     Base::Vector3f x,y,z;
01222     Base::Builder3D log3d_mesh, log3d_cad;
01223     gp_Pnt orig;
01224 
01225     gp_Vec v1,v2,v3,v,vec; // Hauptachsenrichtungen
01226     gp_Trsf trafo;
01227 
01228    /* BRepGProp::SurfaceProperties(m_Cad, prop);
01229     pprop = prop.PrincipalProperties();
01230 
01231     v1 = pprop.FirstAxisOfInertia();
01232     v2 = pprop.SecondAxisOfInertia();
01233     v3 = pprop.ThirdAxisOfInertia();*/
01234 
01235         MeshCore::MeshEigensystem pca(m_CadMesh);
01236     pca.Evaluate();
01237     Base::Matrix4D T5 =  pca.Transform();
01238 
01239         v1.SetX(T5[0][0]);v1.SetY(T5[0][1]);v1.SetZ(T5[0][2]);
01240     v2.SetX(T5[1][0]);v2.SetY(T5[1][1]);v2.SetZ(T5[1][2]);
01241     v3.SetX(T5[2][0]);v3.SetY(T5[2][1]);v3.SetZ(T5[2][2]);
01242 
01243     v1.Normalize();
01244     v2.Normalize();
01245     v3.Normalize();
01246 
01247 
01248     v = v1;
01249     v.Cross(v2);
01250 
01251     // right-hand-system check
01252     if ( v.Dot(v3) < 0.0 )
01253         v3 *= -1;
01254 
01255         T5.inverse();
01256 
01257     orig.SetX(T5[0][3]);orig.SetY(T5[1][3]);orig.SetZ(T5[2][3]);
01258     //orig  = prop.CentreOfMass();
01259 
01260     // plot CAD -> local coordinate system
01261 
01262         x.x =  50.0f*(float)v1.X();     x.y =  50.0f*(float)v1.Y();     x.z =  50.0f*(float)v1.Z();
01263     y.x =  50.0f*(float)v2.X(); y.y =  50.0f*(float)v2.Y();     y.z =  50.0f*(float)v2.Z();
01264     z.x =  50.0f*(float)v3.X(); z.y =  50.0f*(float)v3.Y();     z.z =  50.0f*(float)v3.Z();
01265 
01266     pnt.x = (float) orig.X();
01267         pnt.y = (float) orig.Y();
01268         pnt.z = (float) orig.Z();
01269 
01270     log3d_cad.addSingleArrow(pnt,x,3,1,0,0);
01271         log3d_cad.addSingleArrow(pnt,y,3,0,1,0);
01272         log3d_cad.addSingleArrow(pnt,z,3,0,0,1);
01273         
01274         //log3d_cad.addSinglePoint(pnt,6,1,1,1);
01275         
01276         log3d_cad.saveToFile("c:/CAD_CoordSys.iv");
01277 
01278     MeshCore::MeshEigensystem pca2(m_MeshWork);
01279     pca2.Evaluate();
01280     Base::Matrix4D T1 =  pca2.Transform();
01281     m_MeshWork.Transform(T5*T1);
01282         //m_MeshWork.Transform(T1);
01283     // plot Mesh -> local coordinate system
01284 
01285         
01286     v1.SetX(T1[0][0]);v1.SetY(T1[0][1]);v1.SetZ(T1[0][2]);
01287     v2.SetX(T1[1][0]);v2.SetY(T1[1][1]);v2.SetZ(T1[1][2]);
01288     v3.SetX(T1[2][0]);v3.SetY(T1[2][1]);v3.SetZ(T1[2][2]);
01289 
01290 
01291     
01292     T1.inverse();
01293     orig.SetX(T1[0][3]);orig.SetY(T1[1][3]);orig.SetZ(T1[2][3]);
01294 
01295         x.x = 50.0f*(float)v1.X();      x.y = 50.0f*(float)v1.Y();      x.z = 50.0f*(float)v1.Z();
01296     y.x = 50.0f*(float)v2.X();  y.y = 50.0f*(float)v2.Y();      y.z = 50.0f*(float)v2.Z();
01297     z.x = 50.0f*(float)v3.X();  z.y = 50.0f*(float)v3.Y();      z.z = 50.0f*(float)v3.Z();
01298 
01299     pnt.x = (float) orig.X();
01300         pnt.y = (float) orig.Y();
01301         pnt.z = (float) orig.Z();
01302 
01303         log3d_mesh.addSingleArrow(pnt,x,3,1,0,0);log3d_mesh.addSingleArrow(pnt,y,3,0,1,0);log3d_mesh.addSingleArrow(pnt,z,3,0,0,1);
01304         log3d_mesh.addSinglePoint(0,0,0,20,1,1,1); // plotte Ursprung
01305     //log3d_mesh.addSinglePoint(pnt,6,0,0,0);
01306         log3d_mesh.saveToFile("c:/Mesh_CoordSys.iv");
01307 
01308     return true;
01309 }
01310 
01311 bool best_fit::ShapeFit_Coarse()
01312 {
01313     GProp_GProps prop;
01314     BRepGProp    SurfProp;
01315     gp_Trsf      trafo;
01316     gp_Pnt       orig;
01317 
01318     SurfProp.SurfaceProperties(m_Cad, prop);
01319     orig  = prop.CentreOfMass();
01320 
01321     // CAD-Mesh -> zurück zum Ursprung
01322     m_cad2orig.SetX(-orig.X());
01323     m_cad2orig.SetY(-orig.Y());
01324     m_cad2orig.SetZ(-orig.Z());
01325 
01326     trafo.SetTranslation(m_cad2orig);
01327     BRepBuilderAPI_Transform trsf(trafo);
01328 
01329     trsf.Perform(m_Cad);
01330     m_Cad = trsf.Shape();
01331 
01332     return true;
01333 }
01334 
01335 bool best_fit::Tesselate_Face(const TopoDS_Face &aface, MeshCore::MeshKernel &mesh, float deflection)
01336 {
01337     Base::Builder3D aBuild;
01338     MeshCore::MeshBuilder builder(mesh);
01339     builder.Initialize(1000);
01340     Base::Vector3f Points[3];
01341     if (!BRepTools::Triangulation(aface,0.1))
01342     {
01343         // removes all the triangulations of the faces ,
01344         // and all the polygons on the triangulations of the edges:
01345         BRepTools::Clean(aface);
01346 
01347         // adds a triangulation of the shape aShape with the deflection aDeflection:
01348         //BRepMesh_IncrementalMesh Mesh(pcShape->getShape(),aDeflection);
01349 
01350 /*The next two lines have been from the occ6.2 adapt mesh library. They do not work within OCC6.3
01351       TriangleAdapt_Parameters MeshingParams;
01352        BRepMeshAdapt::Mesh(aface,deflection,MeshingParams);
01353                                                                                                                                 */
01354            BRepMesh_IncrementalMesh Mesh(aface,deflection);
01355     }
01356     TopLoc_Location aLocation;
01357     // takes the triangulation of the face aFace:
01358     Handle_Poly_Triangulation aTr = BRep_Tool::Triangulation(aface,aLocation);
01359     if (!aTr.IsNull()) // if this triangulation is not NULL
01360     {
01361         // takes the array of nodes for this triangulation:
01362         const TColgp_Array1OfPnt& aNodes = aTr->Nodes();
01363         // takes the array of triangles for this triangulation:
01364         const Poly_Array1OfTriangle& triangles = aTr->Triangles();
01365         // create array of node points in absolute coordinate system
01366         TColgp_Array1OfPnt aPoints(1, aNodes.Length());
01367         for ( Standard_Integer i = 1; i < aNodes.Length()+1; i++)
01368             aPoints(i) = aNodes(i).Transformed(aLocation);
01369         // Takes the node points of each triangle of this triangulation.
01370         // takes a number of triangles:
01371         Standard_Integer nnn = aTr->NbTriangles();
01372         Standard_Integer nt,n1,n2,n3;
01373         for ( nt = 1 ; nt < nnn+1 ; nt++)
01374         {
01375             // takes the node indices of each triangle in n1,n2,n3:
01376             triangles(nt).Get(n1,n2,n3);
01377             // takes the node points:
01378             gp_Pnt aPnt1 = aPoints(n1);
01379             Points[0].Set(float(aPnt1.X()),float(aPnt1.Y()),float(aPnt1.Z()));
01380             gp_Pnt aPnt2 = aPoints(n2);
01381             Points[1].Set((float) aPnt2.X(),(float) aPnt2.Y(),(float) aPnt2.Z());
01382             gp_Pnt aPnt3 = aPoints(n3);
01383             Points[2].Set((float) aPnt3.X(),(float) aPnt3.Y(),(float) aPnt3.Z());
01384             // give the occ faces to the internal mesh structure of freecad
01385             MeshCore::MeshGeomFacet Face(Points[0],Points[1],Points[2]);
01386             builder.AddFacet(Face);
01387         }
01388 
01389     }
01390     // if the triangulation of only one face is not possible to get
01391     else
01392     {
01393         throw Base::Exception("Empty face triangulation\n");
01394     }
01395 
01396     // finish FreeCAD Mesh Builder and exit with new mesh
01397     builder.Finish();
01398     return true;
01399 }
01400 
01401 
01402 bool best_fit::Tesselate_Shape(const TopoDS_Shape &shape, MeshCore::MeshKernel &mesh, float deflection)
01403 {
01404     Base::Builder3D aBuild;
01405 
01406     MeshCore::MeshDefinitions::_fMinPointDistanceD1 = (float) 0.0001;
01407     MeshCore::MeshBuilder builder(mesh);
01408     builder.Initialize(1000);
01409     Base::Vector3f Points[3];
01410 
01411     //bool check = BRepUtils::CheckTopologie(shape);
01412     //if (!check)
01413     //{
01414     //    cout <<"an error"<< endl;
01415     //}
01416 
01417     //BRepBuilderAPI_Sewing aSewer;
01418     //aSewer.Load(shape);
01419     //aSewer.Perform();
01420     //aSewer.IsModified(shape);
01421     //const TopoDS_Shape& asewedShape = aSewer.SewedShape();
01422     //if (asewedShape.IsNull())
01423     //{
01424     //    cout << "Nothing Sewed" << endl;
01425     //}
01426     //int test = aSewer.NbFreeEdges();
01427     //int test1 = aSewer.NbMultipleEdges();
01428     //int test2 = aSewer.NbDegeneratedShapes();
01429 
01430     // adds a triangulation of the shape aShape with the deflection deflection:
01431 /*
01432     TriangleAdapt_Parameters MeshParams;
01433     MeshParams._minAngle = 30.0;
01434     //MeshParams._minNbPntsPerEdgeLine = 3;
01435     //MeshParams._minNbPntsPerEdgeOther = 3;
01436     //MeshParams._minEdgeSplit = 3;
01437         MeshParams._maxTriangleSideSize = 10; //10
01438         MeshParams._maxArea = 10; //50
01439         */
01440     BRepMesh_IncrementalMesh Mesh(shape,deflection);
01441     //BRepMesh::Mesh(shape,deflection);
01442     TopExp_Explorer aExpFace;
01443 
01444 
01445     for (aExpFace.Init(shape,TopAbs_FACE);aExpFace.More();aExpFace.Next())
01446     {
01447 
01448         TopoDS_Face aFace = TopoDS::Face(aExpFace.Current());
01449 
01450         TopLoc_Location aLocation;
01451 
01452         // takes the triangulation of the face aFace:
01453         Handle_Poly_Triangulation aTr = BRep_Tool::Triangulation(aFace,aLocation);
01454         if (!aTr.IsNull()) // if this triangulation is not NULL
01455         {
01456             // takes the array of nodes for this triangulation:
01457             const TColgp_Array1OfPnt& aNodes = aTr->Nodes();
01458             // takes the array of triangles for this triangulation:
01459             const Poly_Array1OfTriangle& triangles = aTr->Triangles();
01460             // create array of node points in absolute coordinate system
01461             TColgp_Array1OfPnt aPoints(1, aNodes.Length());
01462             for ( Standard_Integer i = 1; i <= aNodes.Length(); i++)
01463                 aPoints(i) = aNodes(i).Transformed(aLocation);
01464             // Takes the node points of each triangle of this triangulation.
01465             // takes a number of triangles:
01466             Standard_Integer nnn = aTr->NbTriangles();
01467             Standard_Integer nt,n1,n2,n3;
01468             for ( nt = 1 ; nt < nnn+1 ; nt++)
01469             {
01470                 // takes the node indices of each triangle in n1,n2,n3:
01471                 triangles(nt).Get(n1,n2,n3);
01472                 // takes the node points:
01473                 gp_Pnt aPnt1 = aPoints(n1);
01474                 Points[0].Set(float(aPnt1.X()),float(aPnt1.Y()),float(aPnt1.Z()));
01475                 gp_Pnt aPnt2 = aPoints(n2);
01476                 Points[1].Set((float) aPnt2.X(),(float) aPnt2.Y(),(float) aPnt2.Z());
01477                 gp_Pnt aPnt3 = aPoints(n3);
01478                 Points[2].Set((float) aPnt3.X(),(float) aPnt3.Y(),(float) aPnt3.Z());
01479                 // give the occ faces to the internal mesh structure of freecad
01480                 MeshCore::MeshGeomFacet Face(Points[0],Points[1],Points[2]);
01481                 builder.AddFacet(Face);
01482             }
01483         }
01484         // if the triangulation of only one face is not possible to get
01485         else
01486         {
01487             throw Base::Exception("Empty face triangulation\n");
01488         }
01489     }
01490     // finish FreeCAD Mesh Builder and exit with new mesh
01491     builder.Finish();
01492 
01493     /*MeshCore::MeshAlgorithm algo(mesh);
01494     std::list< std::vector <unsigned long> > BoundariesIndex;
01495     algo.GetMeshBorders(BoundariesIndex);*/
01496 
01497     return true;
01498 }
01499 
01500 std::vector<Base::Vector3f> best_fit::Comp_Normals(MeshCore::MeshKernel &M)
01501 {
01502     Base::Builder3D log3d;
01503     Base::Vector3f normal,local_normal,origPoint;
01504     MeshCore::MeshRefPointToFacets rf2pt(M);
01505     MeshCore::MeshGeomFacet        t_face;
01506     MeshCore::MeshPoint mPnt;
01507     std::vector<Base::Vector3f>    normals;
01508 
01509     int NumOfPoints = M.CountPoints();
01510     float local_Area;
01511     float fArea;
01512 
01513     for (int i=0; i<NumOfPoints; ++i)
01514     {
01515         // Satz von Dreiecken zu jedem Punkt
01516         mPnt = M.GetPoint(i);
01517         origPoint.x = mPnt.x;
01518         origPoint.y = mPnt.y;
01519         origPoint.z = mPnt.z;
01520 
01521         const std::set<unsigned long>& faceSet = rf2pt[i];
01522         fArea = 0.0;
01523         normal.Set(0.0,0.0,0.0);
01524 
01525         // Iteriere über die Dreiecke zu jedem Punkt
01526         for (std::set<unsigned long>::const_iterator it = faceSet.begin(); it != faceSet.end(); ++it)
01527         {
01528             // Zweimal derefernzieren, um an das MeshFacet zu kommen und dem Kernel uebergeben, dass er ein MeshGeomFacet liefert
01529             t_face = M.GetFacet(*it);
01530             // Flaecheninhalt aufsummieren
01531             local_Area = t_face.Area();
01532             local_normal = t_face.GetNormal();
01533             if (local_normal.z < 0)
01534             {
01535                 local_normal = local_normal * (-1);
01536             }
01537 
01538             fArea  = fArea  + local_Area;
01539             normal = normal + local_Area*local_normal;
01540 
01541         }
01542 
01543         normal.Normalize();
01544         normals.push_back(normal);
01545 
01546         log3d.addSingleArrow(origPoint,origPoint+normal,1,0,0,0);
01547     }
01548 
01549     log3d.saveToFile("c:/normals.iv");
01550 
01551     return normals;
01552 }
01553 
01554 /*
01555 double best_fit::CompError(std::vector<Base::Vector3f> &pnts, std::vector<Base::Vector3f> &normals)
01556 {
01557     double err_avg = 0.0;
01558     double err_max = 0.0;
01559     double sqrdis = 0.0;
01560 
01561     MeshCore::MeshFacetGrid aFacetGrid(m_CadMesh);
01562     MeshCore::MeshAlgorithm malg(m_CadMesh);
01563     MeshCore::MeshAlgorithm malg2(m_CadMesh);
01564 
01565     Base::Vector3f origPoint, projPoint, distVec;
01566     unsigned long  facetIndex;
01567 
01568     int NumOfPoints = pnts.size();
01569     int c = 0;
01570 
01571     for (int i=0; i<NumOfPoints; ++i)
01572     {
01573         if (!malg.NearestFacetOnRay(pnts[i], normals[i], aFacetGrid, projPoint, facetIndex))  // gridoptimiert
01574         {
01575             if (malg2.NearestFacetOnRay(pnts[i], normals[i], projPoint, facetIndex))
01576             {
01577                 distVec  = projPoint-pnts[i];
01578                 sqrdis   = distVec*distVec;
01579                 err_avg += sqrdis;
01580             }
01581             else
01582                 ++c;
01583         }
01584         else
01585         {
01586             distVec  = projPoint-pnts[i];
01587             sqrdis   = distVec*distVec;
01588             err_avg += sqrdis;
01589         }
01590     }
01591 
01592     if (c==NumOfPoints)
01593         return 1e+10;
01594 
01595     return err_avg/(NumOfPoints-c);
01596 }
01597 
01598 std::vector<double> best_fit::CompError_GetPnts(std::vector<Base::Vector3f> pnts,
01599         std::vector<Base::Vector3f> &normals)
01600 
01601 {
01602     double err_avg = 0.0;
01603     double dist;
01604 
01605     std::vector<double> errVec(2);    // errVec[0]: avg. error, errVec[1]: max. error
01606 
01607     MeshCore::MeshFacetGrid aFacetGrid(m_Mesh);
01608     MeshCore::MeshAlgorithm malg(m_Mesh);
01609     unsigned long  facetIndex;
01610 
01611     Base::Vector3f origPoint, projPoint, distVec;
01612     //Base::Builder3D log;
01613 
01614     unsigned int NumOfPoints = pnts.size();
01615     int c = 0;
01616 
01617     m_LSPnts[0].clear();
01618     m_LSPnts[1].clear();
01619 
01620     m_weights_loc.clear();
01621 
01622     for (unsigned int i=0; i<NumOfPoints; ++i)
01623     {
01624         if (!malg.NearestFacetOnRay(pnts[i], normals[i], aFacetGrid, projPoint, facetIndex))
01625                // !Intersect(normals[i], *m_Mesh, pnts[i], projPoint)  // gridoptimiert
01626         {
01627             ++c;
01628         }
01629         else
01630         {
01631             m_LSPnts[0].push_back(projPoint);
01632             m_LSPnts[1].push_back(pnts[i]);
01633             m_weights_loc.push_back(m_weights[i]);
01634 
01635             //log.addSingleArrow(pnts[i], projPoint);
01636         }
01637     }
01638 
01639     double max_err = 0.0;
01640 
01641     for (unsigned int i=0; i<NumOfPoints-c; ++i)
01642     {
01643         distVec  = m_LSPnts[0][i]-m_LSPnts[1][i];
01644         dist     = distVec*distVec;
01645         err_avg += dist;
01646 
01647         if (dist > max_err)
01648             max_err = dist;
01649     }
01650 
01651     //log.saveToFile("c:/intersection.iv");
01652 
01653     errVec[0] = err_avg /= (NumOfPoints-c);  // durchschnittlicher Fehlerquadrat
01654     errVec[1] = max_err;         // maximaler Fehlerquadrat
01655 
01656     //cout << c << " projections failed" << endl;
01657 
01658     return errVec;
01659 }
01660 
01661 double best_fit::CompError(std::vector<Base::Vector3f> &pnts, std::vector<Base::Vector3f> &normals, bool plot)
01662 {
01663     if (plot==false)
01664         return CompError(pnts, normals);
01665     else
01666     {
01667         Base::Builder3D log3d;
01668         double err_avg = 0.0;
01669         double err_max = 0.0;
01670         double sqrdis  = 0.0;
01671 
01672 
01673         MeshCore::MeshFacetGrid aFacetGrid(m_CadMesh);
01674         MeshCore::MeshAlgorithm malg(m_CadMesh);
01675         MeshCore::MeshAlgorithm malg2(m_CadMesh);
01676 
01677         Base::Vector3f projPoint, distVec;
01678         unsigned long  facetIndex;
01679 
01680         int NumOfPoints = pnts.size();
01681         int c=0;
01682 
01683         for (int i=0; i<NumOfPoints; ++i)
01684         {
01685             if (!malg.NearestFacetOnRay(pnts[i], normals[i], aFacetGrid, projPoint, facetIndex))   // gridoptimiert
01686             {
01687                 if (malg2.NearestFacetOnRay(pnts[i], normals[i], projPoint, facetIndex))
01688                 {
01689 
01690                     log3d.addSingleArrow(pnts[i],projPoint, 3, 0,0,0);
01691                     distVec  = projPoint-pnts[i];
01692                     sqrdis   = distVec*distVec;
01693                     err_avg += sqrdis;
01694                 }
01695                 else
01696                     c++;
01697 
01698             }
01699             else
01700             {
01701                 log3d.addSingleArrow(pnts[i],projPoint, 3, 0,0,0);
01702                 distVec  = projPoint-pnts[i];
01703                 sqrdis   = distVec*distVec;
01704                 err_avg += sqrdis;
01705             }
01706         }
01707 
01708         log3d.saveToFile("c:/projection.iv");
01709 
01710         if (c>(NumOfPoints/2))
01711             return 1e+10;
01712 
01713         return err_avg/(NumOfPoints-c);
01714     }
01715 }
01716 */
01717 
01718 double best_fit::CompTotalError()
01719 {
01720     Base::Builder3D log3d;
01721     double err_avg = 0.0;
01722     double err_max = 0.0;
01723     double sqrdis  = 0.0;
01724 
01725     std::vector<int> FailProj;
01726 
01727     MeshCore::MeshFacetGrid aFacetGrid(m_Mesh,10);
01728     MeshCore::MeshAlgorithm malg(m_Mesh);
01729     MeshCore::MeshAlgorithm malg2(m_Mesh);
01730     MeshCore::MeshPointIterator p_it(m_CadMesh);
01731 
01732     Base::Vector3f projPoint, distVec, projPoint2;
01733     unsigned long  facetIndex;
01734     std::stringstream text;
01735     m_error.resize(m_CadMesh.CountPoints());
01736 
01737     unsigned int c=0;
01738     int i=0;
01739 
01740         
01741     m_LSPnts[0].clear();
01742     m_LSPnts[1].clear();
01743     for (p_it.Begin(); p_it.More(); p_it.Next())
01744     {
01745         if (malg.NearestFacetOnRay(*p_it, m_normals[i], aFacetGrid, projPoint, facetIndex))   // gridoptimiert
01746         {
01747             log3d.addSingleArrow(*p_it, projPoint, 3, 0,0,0);
01748             distVec  = projPoint - *p_it;
01749             sqrdis   = distVec*distVec;
01750 
01751                         m_LSPnts[1].push_back(*p_it);
01752             m_LSPnts[0].push_back(projPoint);
01753 
01754             if (((projPoint.z - p_it->z) / m_normals[i].z ) > 0)
01755                 m_error[i] = sqrt(sqrdis);
01756             else
01757                 m_error[i] = -sqrt(sqrdis);
01758 
01759             err_avg += sqrdis;
01760         }
01761         else
01762         {
01763 
01764             if (!malg2.NearestFacetOnRay(*p_it, m_normals[i], projPoint, facetIndex))   // nicht gridoptimiert
01765             {
01766                 c++;
01767                 //m_normals[i].Scale(-10,-10,-10);
01768                 text << p_it->x << ", " << p_it->y << ", " << p_it->z << "; " << m_normals[i].x << ", " << m_normals[i].y << ", " << m_normals[i].z;
01769                 //log3d.addSingleArrow(*p_it, *p_it + m_normals[i], 4, 1,0,0);
01770                 //log3d.addText(*p_it,(text.str()).c_str());
01771             }
01772             else
01773                         {
01774                                 log3d.addSingleArrow(*p_it, projPoint, 3, 0,0,0);
01775                                 distVec  = projPoint - *p_it;
01776                                 sqrdis   = distVec*distVec;
01777 
01778                                 m_LSPnts[1].push_back(*p_it);
01779                                 m_LSPnts[0].push_back(projPoint);
01780 
01781                                 if (((projPoint.z - p_it->z) / m_normals[i].z ) > 0)
01782                                         m_error[i] = sqrt(sqrdis);
01783                                 else
01784                                         m_error[i] = -sqrt(sqrdis);
01785 
01786                                 err_avg += sqrdis;
01787                         }
01788                 }
01789         
01790 
01791         ++i;
01792     }
01793 
01794     //for (p_it.Begin(); p_it.More(); p_it.Next())
01795 //   {
01796     //    if (!malg.NearestFacetOnRay(*p_it, m_normals[i], aFacetGrid, projPoint, facetIndex))   // gridoptimiert
01797 //       {
01798 //           if (malg2.NearestFacetOnRay(*p_it, m_normals[i], projPoint, facetIndex))
01799 //           {
01800 
01801 //               log3d.addSingleArrow(*p_it, projPoint, 3, 0,0,0);
01802 //               distVec  = projPoint - *p_it;
01803 //               sqrdis   = distVec*distVec;
01804 
01805 //               if (((projPoint.z - p_it->z) / m_normals[i].z ) > 0)
01806 //                   m_error[i] = sqrt(sqrdis);
01807 //               else
01808 //                   m_error[i] = -sqrt(sqrdis);
01809 
01810 //               err_avg += sqrdis;
01811 //           }
01812 //           else
01813 //           {
01814 //               c++;
01815 //               FailProj.push_back(i);
01816 //           }
01817 //       }
01818 //       else
01819 //       {
01820 //           distVec  = projPoint-*p_it;
01821 //           sqrdis   = distVec*distVec;
01822 
01823 //           m_normals[i].Scale(-1,-1,-1);
01824 
01825 //           if (malg.NearestFacetOnRay(*p_it, m_normals[i], aFacetGrid, projPoint2, facetIndex))
01826 //           {
01827 //               distVec  = projPoint2-*p_it;
01828 //               if (sqrdis > distVec*distVec)
01829 //               {
01830 //                   sqrdis   = distVec*distVec;
01831 //                   log3d.addSingleArrow(*p_it, projPoint2, 3, 0,0,0);
01832 //               }
01833 //               else
01834 //               {
01835 //                   log3d.addSingleArrow(*p_it, projPoint, 3, 0,0,0);
01836 //               }
01837 
01838 //           }
01839 //           m_normals[p_it.Position()].Scale(-1,-1,-1);
01840 
01841 //           if (((projPoint.z - p_it->z) / m_normals[i].z ) > 0)
01842 //               m_error[i] = sqrt(sqrdis);
01843 //           else
01844 //               m_error[i] = -sqrt(sqrdis);
01845 
01846 //           err_avg += sqrdis;
01847 //       }
01848 //       ++i;
01849     //}
01850 
01851 
01852     MeshCore::MeshRefPointToPoints vv_it(m_CadMesh);
01853     MeshCore::MeshPointArray::_TConstIterator v_beg = m_CadMesh.GetPoints().begin();
01854 
01855     std::set<unsigned long>::const_iterator v_it;
01856     for (unsigned int i=0; i<FailProj.size(); ++i)
01857     {
01858         const std::set<unsigned long>& PntNei = vv_it[FailProj[i]];
01859         m_error[FailProj[i]] = 0.0;
01860 
01861         for (v_it = PntNei.begin(); v_it !=PntNei.end(); ++v_it)
01862         {
01863             m_error[FailProj[i]] += m_error[v_beg[*v_it]._ulProp];
01864         }
01865 
01866         m_error[FailProj[i]] /= double(PntNei.size());
01867     }
01868 
01869 
01870     log3d.saveToFile("c:/projection.iv");
01871 
01872     if (c>(m_CadMesh.CountPoints()/2))
01873         return 1e+10;
01874 
01875     return err_avg/(m_CadMesh.CountPoints()-c);
01876 
01877 }
01878 
01879 double best_fit::CompTotalError(MeshCore::MeshKernel &mesh)
01880 {
01881     double err_avg = 0.0;
01882     double err_max = 0.0;
01883     double sqrdis  = 0.0;
01884 
01885     std::vector<int> FailProj;
01886 
01887     MeshCore::MeshFacetGrid aFacetGrid(mesh,10);
01888     MeshCore::MeshAlgorithm malg(mesh);
01889     MeshCore::MeshAlgorithm malg2(mesh);
01890     MeshCore::MeshPointIterator p_it(m_CadMesh);
01891 
01892     Base::Vector3f projPoint, distVec, projPoint2;
01893     unsigned long  facetIndex;
01894     std::stringstream text;
01895     m_error.resize(m_CadMesh.CountPoints());
01896 
01897     unsigned int c=0;
01898     int i=0;
01899 
01900     for (p_it.Begin(); p_it.More(); p_it.Next())
01901     {
01902         if (malg.NearestFacetOnRay(*p_it, m_normals[i], aFacetGrid, projPoint, facetIndex))   // gridoptimiert
01903         {
01904             distVec  = projPoint - *p_it;
01905             sqrdis   = distVec*distVec;
01906 
01907             if (((projPoint.z - p_it->z) / m_normals[i].z ) > 0)
01908                 m_error[i] += sqrt(sqrdis);
01909             else
01910                 m_error[i] += -sqrt(sqrdis);
01911 
01912             err_avg += sqrdis;
01913         }
01914         else
01915         {
01916 
01917             if (!malg2.NearestFacetOnRay(*p_it, m_normals[i], projPoint, facetIndex))   // nicht gridoptimiert
01918             {
01919                 c++;
01920                                 FailProj.push_back(i);
01921             }
01922             else
01923                         {
01924                                 distVec  = projPoint - *p_it;
01925                                 sqrdis   = distVec*distVec;
01926 
01927                                 if (((projPoint.z - p_it->z) / m_normals[i].z ) > 0)
01928                                         m_error[i] += sqrt(sqrdis);
01929                                 else
01930                                         m_error[i] += -sqrt(sqrdis);
01931 
01932                                 err_avg += sqrdis;
01933                         }
01934                 }
01935         
01936 
01937         ++i;
01938     }
01939 
01940     MeshCore::MeshRefPointToPoints vv_it(m_CadMesh);
01941     MeshCore::MeshPointArray::_TConstIterator v_beg = m_CadMesh.GetPoints().begin();
01942 
01943         double error;
01944         std::set<unsigned long>::const_iterator v_it;
01945     for (unsigned int i=0; i<FailProj.size(); ++i)
01946     {
01947         const std::set<unsigned long>& PntNei = vv_it[FailProj[i]];
01948                 error = 0.0;
01949 
01950 
01951         for (v_it = PntNei.begin(); v_it !=PntNei.end(); ++v_it)
01952         {
01953                         error += m_error[v_beg[*v_it]._ulProp];
01954         }
01955 
01956                 error /= double(PntNei.size());
01957         m_error[FailProj[i]] += error;
01958     }
01959 
01960 
01961     if (c>(m_CadMesh.CountPoints()/2))
01962         return 1e+10;
01963 
01964     return err_avg/(m_CadMesh.CountPoints()-c);
01965 }

Generated on Wed Nov 23 18:59:58 2011 for FreeCAD by  doxygen 1.6.1