00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
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
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>
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;
00086 ANNpoint queryPt;
00087 ANNidxArray nnIdx;
00088 ANNdistArray dists;
00089 ANNkd_tree* kdTree;
00090
00091 Base::Builder3D log_error;
00092
00093
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();
00101 int a_nbNear = 1;
00102 queryPt = annAllocPt(a_dim);
00103 dataPts = annAllocPts(a_nbPnts, a_dim);
00104 nnIdx = new ANNidx[a_nbNear];
00105 dists = new ANNdist[a_nbNear];
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(
00118 dataPts,
00119 a_nbPnts,
00120 a_dim);
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(
00130 queryPt,
00131 a_nbNear,
00132 nnIdx,
00133 dists
00134 );
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
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;
00160 delete [] dists;
00161 delete kdTree;
00162 annClose();
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);
00182 sec2 = time(NULL);
00183
00184 Runtime_BestFit << "Tesselate Shape: " << sec2 - sec1 << " sec" << std::endl;
00185
00186 sec1 = time(NULL);
00187 Comp_Weights();
00188 sec2 = time(NULL);
00189
00190 Runtime_BestFit << "Compute Weights: " << sec2 - sec1 << " sec" << std::endl;
00191
00192
00193
00194
00195
00196
00197
00198
00199
00200
00201
00202
00203
00204
00205
00206
00207
00208
00209
00210
00211 sec1 = time(NULL);
00212
00213 MeshFit_Coarse();
00214 ShapeFit_Coarse();
00215
00216
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);
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
00289 sec1 = time(NULL);
00290 Coarse_correction();
00291 sec2 = time(NULL);
00292 Runtime_BestFit << "Coarse Correction: " << sec2-sec1 << " sec" << endl;
00293
00294
00295
00296
00297
00298
00299
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
00325
00326
00327
00328
00329
00330
00331
00332
00333
00334
00335
00336
00337
00338
00339
00340
00341
00342
00343
00344
00345
00346
00347
00348
00349
00350
00351
00352
00353
00354
00355
00356
00357
00358
00359
00360
00361
00362
00363
00364
00365
00366
00367
00368
00369
00370
00371
00372
00373
00374
00375
00376
00377
00378
00379
00380
00381
00382
00383
00384
00385
00386
00387
00388
00389
00390
00391
00392
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
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
00418
00419 error_tmp = ANN();
00420
00421
00422
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;
00448 int maxIter = 100;
00449
00450
00451 int mult = 2;
00452
00453 double val, tmp = 1e+10, delta, delta_tmp = 0.0;
00454 Base::Matrix4D Tx,Ty,Tz,Rx,Ry,Rz,M;
00455
00456 ofstream anOutputFile;
00457 anOutputFile.open("c:/outputBestFit.txt");
00458 anOutputFile.precision(7);
00459
00460 int c=0;
00461
00462
00463
00464 std::vector<double> del_x(3,0.0);
00465 std::vector<double> x(3,0.0);
00466
00467 Base::Vector3f centr_l,centr_r, cent;
00468
00469
00470
00471
00472 std::vector<double> Jac(3);
00473 std::vector< std::vector<double> > H;
00474
00475 time_t seconds1, seconds2, sec1, sec2;
00476 seconds1 = time(NULL);
00477
00478 while (true)
00479 {
00480
00481 seconds1 = time(NULL);
00482
00483
00484
00485
00486
00487
00488 delta = delta_tmp;
00489 delta_tmp = ANN();
00490 delta = delta - delta_tmp ;
00491
00492 if (c==maxIter || delta < ERR_TOL && c>1) break;
00493
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;
00501
00502
00503
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
00526
00527
00528
00529
00530
00531
00532
00533
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);
00545 TransMat(Ty,centr_r.y,2);
00546 TransMat(Tz,centr_r.z,3);
00547
00548 M = Tx*Ty*Tz;
00549 PointTransform(m_LSPnts[1],M);
00550 PointTransform(m_pntCloud_1,M);
00551
00552 m_CadMesh.Transform(M);
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
00559 while (true)
00560 {
00561
00562 Jac = Comp_Jacobi(x);
00563 H = Comp_Hess(x);
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;
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);
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);
00591 for (unsigned int i=0; i<x.size(); ++i)
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
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;
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
00629
00630 sec2 = time(NULL);
00631
00632 anOutputFile << c+1 << " - Trafo: " << seconds2 - seconds1 << " sec" << endl;
00633 ++c;
00634 }
00635
00636 anOutputFile.close();
00637 return true;
00638
00639
00640
00641
00642
00643
00644
00645
00646
00647
00648
00649
00650
00651
00652
00653
00654
00655
00656
00657
00658
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
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
00779 Handle_Poly_Triangulation aTr = BRep_Tool::Triangulation(aFace,aLocation);
00780 if (!aTr.IsNull())
00781 {
00782
00783 const TColgp_Array1OfPnt& aNodes = aTr->Nodes();
00784
00785
00786 const Poly_Array1OfTriangle& triangles = aTr->Triangles();
00787
00788
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
00794
00795 Standard_Integer nnn = aTr->NbTriangles();
00796 Standard_Integer nt,n1,n2,n3;
00797 for ( nt = 1 ; nt < nnn+1 ; nt++)
00798 {
00799
00800 triangles(nt).Get(n1,n2,n3);
00801
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
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;
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
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
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
00990
00991
00992
00993
00995
00996
00997
00998
00999
01000
01002
01003
01004
01005
01006
01007
01008
01009
01010
01011
01012
01015
01016
01017
01018
01019
01020
01021
01022
01023
01024
01025
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;
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
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
01110
01111
01112
01113
01114
01115
01116
01117
01118
01119
01120
01121
01122
01123
01124
01125
01126
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
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
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
01177
01178 log3d_cad.saveToFile("c:/CAD_CoordSys.iv");
01179
01180 PointTransform(m_pntCloud_2,T5*T1);
01181
01182
01183
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);
01202
01203 log3d_mesh.saveToFile("c:/Mesh_CoordSys.iv");
01204
01205
01206
01207
01208
01209
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;
01226 gp_Trsf trafo;
01227
01228
01229
01230
01231
01232
01233
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
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
01259
01260
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
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
01283
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);
01305
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
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
01344
01345 BRepTools::Clean(aface);
01346
01347
01348
01349
01350
01351
01352
01353
01354 BRepMesh_IncrementalMesh Mesh(aface,deflection);
01355 }
01356 TopLoc_Location aLocation;
01357
01358 Handle_Poly_Triangulation aTr = BRep_Tool::Triangulation(aface,aLocation);
01359 if (!aTr.IsNull())
01360 {
01361
01362 const TColgp_Array1OfPnt& aNodes = aTr->Nodes();
01363
01364 const Poly_Array1OfTriangle& triangles = aTr->Triangles();
01365
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
01370
01371 Standard_Integer nnn = aTr->NbTriangles();
01372 Standard_Integer nt,n1,n2,n3;
01373 for ( nt = 1 ; nt < nnn+1 ; nt++)
01374 {
01375
01376 triangles(nt).Get(n1,n2,n3);
01377
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
01385 MeshCore::MeshGeomFacet Face(Points[0],Points[1],Points[2]);
01386 builder.AddFacet(Face);
01387 }
01388
01389 }
01390
01391 else
01392 {
01393 throw Base::Exception("Empty face triangulation\n");
01394 }
01395
01396
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
01412
01413
01414
01415
01416
01417
01418
01419
01420
01421
01422
01423
01424
01425
01426
01427
01428
01429
01430
01431
01432
01433
01434
01435
01436
01437
01438
01439
01440 BRepMesh_IncrementalMesh Mesh(shape,deflection);
01441
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
01453 Handle_Poly_Triangulation aTr = BRep_Tool::Triangulation(aFace,aLocation);
01454 if (!aTr.IsNull())
01455 {
01456
01457 const TColgp_Array1OfPnt& aNodes = aTr->Nodes();
01458
01459 const Poly_Array1OfTriangle& triangles = aTr->Triangles();
01460
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
01465
01466 Standard_Integer nnn = aTr->NbTriangles();
01467 Standard_Integer nt,n1,n2,n3;
01468 for ( nt = 1 ; nt < nnn+1 ; nt++)
01469 {
01470
01471 triangles(nt).Get(n1,n2,n3);
01472
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
01480 MeshCore::MeshGeomFacet Face(Points[0],Points[1],Points[2]);
01481 builder.AddFacet(Face);
01482 }
01483 }
01484
01485 else
01486 {
01487 throw Base::Exception("Empty face triangulation\n");
01488 }
01489 }
01490
01491 builder.Finish();
01492
01493
01494
01495
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
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
01526 for (std::set<unsigned long>::const_iterator it = faceSet.begin(); it != faceSet.end(); ++it)
01527 {
01528
01529 t_face = M.GetFacet(*it);
01530
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
01556
01557
01558
01559
01560
01561
01562
01563
01564
01565
01566
01567
01568
01569
01570
01571
01572
01573
01574
01575
01576
01577
01578
01579
01580
01581
01582
01583
01584
01585
01586
01587
01588
01589
01590
01591
01592
01593
01594
01595
01596
01597
01598
01599
01600
01601
01602
01603
01604
01605
01606
01607
01608
01609
01610
01611
01612
01613
01614
01615
01616
01617
01618
01619
01620
01621
01622
01623
01624
01625
01626
01627
01628
01629
01630
01631
01632
01633
01634
01635
01636
01637
01638
01639
01640
01641
01642
01643
01644
01645
01646
01647
01648
01649
01650
01651
01652
01653
01654
01655
01656
01657
01658
01659
01660
01661
01662
01663
01664
01665
01666
01667
01668
01669
01670
01671
01672
01673
01674
01675
01676
01677
01678
01679
01680
01681
01682
01683
01684
01685
01686
01687
01688
01689
01690
01691
01692
01693
01694
01695
01696
01697
01698
01699
01700
01701
01702
01703
01704
01705
01706
01707
01708
01709
01710
01711
01712
01713
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))
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))
01765 {
01766 c++;
01767
01768 text << p_it->x << ", " << p_it->y << ", " << p_it->z << "; " << m_normals[i].x << ", " << m_normals[i].y << ", " << m_normals[i].z;
01769
01770
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
01795
01796
01797
01798
01799
01800
01801
01802
01803
01804
01805
01806
01807
01808
01809
01810
01811
01812
01813
01814
01815
01816
01817
01818
01819
01820
01821
01822
01823
01824
01825
01826
01827
01828
01829
01830
01831
01832
01833
01834
01835
01836
01837
01838
01839
01840
01841
01842
01843
01844
01845
01846
01847
01848
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))
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))
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 }