00001 /**************************************************************************** 00002 00003 This file is part of the GLC-lib library. 00004 Copyright (C) 2005-2008 Laurent Ribon (laumaya@users.sourceforge.net) 00005 Copyright (C) 2009 Pierre Soetewey 00006 Version 2.0.0 Beta 1, packaged on April 2010. 00007 00008 http://glc-lib.sourceforge.net 00009 00010 GLC-lib is free software; you can redistribute it and/or modify 00011 it under the terms of the GNU General Public License as published by 00012 the Free Software Foundation; either version 2 of the License, or 00013 (at your option) any later version. 00014 00015 GLC-lib is distributed in the hope that it will be useful, 00016 but WITHOUT ANY WARRANTY; without even the implied warranty of 00017 MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 00018 GNU General Public License for more details. 00019 00020 You should have received a copy of the GNU General Public License 00021 along with GLC-lib; if not, write to the Free Software 00022 Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA 00023 00024 *****************************************************************************/ 00025 00026 #include "glc_distance.h" 00027 #include "../geometry/glc_mesh.h" 00028 #include <stdio.h> 00029 #include <math.h> 00030 #include <../PQP/PQP.h> 00031 00032 #define PI 3.14159265359 00033 #define LISTS 0 00034 00035 // Default constructor 00036 GLC_Distance::GLC_Distance() 00037 : GLC_Object("DistMin") 00038 , m_ListOfInstances1() 00039 , m_ListOfInstances2() 00040 , m_Point1() 00041 , m_Point2() 00042 , m_DistanceMini(0.0) 00043 , m_RelativeError(0.0) 00044 , m_AbsoluteError(0.0) 00045 { 00046 00047 } 00048 00049 // Construct a distmin with 2 GLC_3DViewInstance 00050 GLC_Distance::GLC_Distance(const GLC_3DViewInstance& instance1, const GLC_3DViewInstance& instance2) 00051 : GLC_Object("DistMin") 00052 , m_ListOfInstances1() 00053 , m_ListOfInstances2() 00054 , m_Point1() 00055 , m_Point2() 00056 , m_DistanceMini(0.0) 00057 , m_RelativeError(0.0) 00058 , m_AbsoluteError(0.0) 00059 { 00060 m_ListOfInstances1.append(instance1); 00061 m_ListOfInstances2.append(instance2); 00062 } 00063 00064 // Copy Constructor 00065 GLC_Distance::GLC_Distance(const GLC_Distance& distance) 00066 : GLC_Object(distance) 00067 , m_ListOfInstances1(distance.m_ListOfInstances1) 00068 , m_ListOfInstances2(distance.m_ListOfInstances2) 00069 , m_Point1(distance.m_Point1) 00070 , m_Point2(distance.m_Point2) 00071 , m_DistanceMini(distance.m_DistanceMini) 00072 , m_RelativeError(distance.m_RelativeError) 00073 , m_AbsoluteError(distance.m_AbsoluteError) 00074 { 00075 00076 } 00077 00078 GLC_Distance::~GLC_Distance() 00079 { 00080 qDebug() << "GLC_Distance::~GLC_Distance()"; 00081 } 00082 00084 // name Set Functions 00086 00087 // Clear the 2 groups 00088 void GLC_Distance::clear() 00089 { 00090 m_ListOfInstances1.clear(); 00091 m_ListOfInstances2.clear(); 00092 m_Point1.setVect(GLC_Point3d()); 00093 m_Point2.setVect(GLC_Point3d()); 00094 m_DistanceMini= 0.0; 00095 } 00096 00097 // Add instance in group 1 00098 void GLC_Distance::addInstanceInGroup1(const GLC_3DViewInstance& instance) 00099 { 00100 m_ListOfInstances1.append(instance); 00101 } 00102 00103 // Add instances list in group 1 00104 void GLC_Distance::addInstancesInGroup1(const QList<GLC_3DViewInstance>& instances) 00105 { 00106 m_ListOfInstances1.append(instances); 00107 } 00108 00109 // Add instance in group 1 00110 void GLC_Distance::addInstanceInGroup2(const GLC_3DViewInstance& instance) 00111 { 00112 m_ListOfInstances2.append(instance); 00113 } 00114 00115 // Add instances list in group 1 00116 void GLC_Distance::addInstancesInGroup2(const QList<GLC_3DViewInstance>& instances) 00117 { 00118 m_ListOfInstances2.append(instances); 00119 } 00120 00121 // Compute the minimum distance between the 2 groups 00122 void GLC_Distance::computeMinimumDistance() 00123 { 00124 DistanceResult distanceResult; 00125 if (!m_ListOfInstances1.isEmpty() && !m_ListOfInstances2.isEmpty()) 00126 { 00127 distanceResult= minimumDistance(m_ListOfInstances1, m_ListOfInstances2); 00128 } 00129 else 00130 { 00131 qDebug() << "a list is empty"; 00132 distanceResult.m_Distance= -1.0; 00133 distanceResult.m_Point1= GLC_Point3d(); 00134 distanceResult.m_Point2= GLC_Point3d(); 00135 } 00136 00137 m_DistanceMini= distanceResult.m_Distance; 00138 m_Point1= distanceResult.m_Point1; 00139 m_Point2= distanceResult.m_Point2; 00140 00141 } 00142 00144 // private services functions 00146 00147 // Return distance mini beween to instance 00148 GLC_Distance::DistanceResult GLC_Distance::minimumDistance(QList<GLC_3DViewInstance> &GLC_3DViewList1, QList<GLC_3DViewInstance> &GLC_3DViewList2) const 00149 { 00150 DistanceResult distanceResult; 00151 distanceResult.m_Distance= 0.0; 00152 00153 PQP_Model m1, m2; 00154 GLC_3DViewInstance instance1; 00155 GLC_3DViewInstance instance2; 00156 GLC_Matrix4x4 instance1Matrix; 00157 GLC_Matrix4x4 instance2Matrix; 00158 00159 m1.BeginModel(); 00160 const int GLC_3DViewList1Size= GLC_3DViewList1.size(); 00161 for(int i=0; i < GLC_3DViewList1Size; ++i){ 00162 00163 instance1= GLC_3DViewList1.at(i); 00164 00166 QList<GLC_Mesh*> listOfMesh1; 00167 { 00168 const int size= instance1.numberOfGeometry(); 00169 qDebug() << "Size = " << size; 00170 for (int i= 0; i < size; ++i) 00171 { 00172 GLC_Mesh* pMesh= dynamic_cast<GLC_Mesh*>(instance1.geomAt(i)); 00173 if (NULL != pMesh) listOfMesh1.append(pMesh); 00174 } 00175 } 00176 if (listOfMesh1.isEmpty()) 00177 { 00178 qDebug() << "listOfMesh1 empty"; 00179 return distanceResult; 00180 } 00181 00182 instance1Matrix= instance1.matrix(); 00183 00184 // Avoid scaling problem (useful?) 00185 //instance1Matrix= instance1Matrix.isomorphMatrix(); 00186 00187 // Add first mesh's triangles to the PQP model 00188 addMeshTrianglesToPQP(&m1, listOfMesh1, instance1Matrix); 00189 } 00190 m1.EndModel(); 00191 00192 m2.BeginModel(); 00193 const int GLC_3DViewList2Size= GLC_3DViewList2.size(); 00194 for(int i=0; i < GLC_3DViewList2Size; ++i){ 00195 00196 instance2= GLC_3DViewList2.at(i); 00197 00199 QList<GLC_Mesh*> listOfMesh2; 00200 { 00201 const int size= instance2.numberOfGeometry(); 00202 for (int i= 0; i < size; ++i) 00203 { 00204 GLC_Mesh* pMesh= dynamic_cast<GLC_Mesh*>(instance2.geomAt(i)); 00205 if (NULL != pMesh) listOfMesh2.append(pMesh); 00206 } 00207 } 00208 if (listOfMesh2.isEmpty()) 00209 { 00210 qDebug() << "listOfMesh2 empty"; 00211 return distanceResult; 00212 } 00213 00214 00215 instance2Matrix= instance2.matrix(); 00216 00217 // Avoid scaling problem (useful?) 00218 //instance2Matrix= instance2Matrix.isomorphMatrix(); 00219 00220 // Add second mesh's triangles to the PQP model 00221 addMeshTrianglesToPQP(&m2, listOfMesh2, instance2Matrix); 00222 00223 } 00224 m2.EndModel(); 00225 00226 // PQP Translation 00227 PQP_REAL T1[3]; 00228 PQP_REAL T2[3]; 00229 // PQP rotation 00230 PQP_REAL R1[3][3]; 00231 PQP_REAL R2[3][3]; 00232 00233 // Null Vectors (no translation) 00234 T1[0]= 0; 00235 T1[1]= 0; 00236 T1[2]= 0; 00237 00238 T2[0]= 0; 00239 T2[1]= 0; 00240 T2[2]= 0; 00241 00242 // Unity Matrix (no rotation) 00243 R1[0][0]= 1; 00244 R1[1][0]= 0; 00245 R1[2][0]= 0; 00246 R1[0][1]= 0; 00247 R1[1][1]= 1; 00248 R1[2][1]= 0; 00249 R1[0][2]= 0; 00250 R1[1][2]= 0; 00251 R1[2][2]= 1; 00252 00253 R2[0][0]= 1; 00254 R2[1][0]= 0; 00255 R2[2][0]= 0; 00256 R2[0][1]= 0; 00257 R2[1][1]= 1; 00258 R2[2][1]= 0; 00259 R2[0][2]= 0; 00260 R2[1][2]= 0; 00261 R2[2][2]= 1; 00262 00263 PQP_DistanceResult dres; 00264 PQP_Distance(&dres,R1,T1,&m1,R2,T2,&m2,m_RelativeError, m_AbsoluteError); 00265 double distance = dres.Distance(); 00266 00267 const PQP_REAL * p1 = dres.P1(); 00268 const PQP_REAL * p2 = dres.P2(); 00269 00270 GLC_Point3d point1(p1[0],p1[1],p1[2]); 00271 GLC_Point3d point2(p2[0],p2[1],p2[2]); 00272 00273 distanceResult.m_Distance= distance; 00274 distanceResult.m_Point1= point1; 00275 distanceResult.m_Point2= point2; 00276 00277 //qDebug() << "Distance " << distanceResult.m_Distance; 00278 return distanceResult; 00279 } 00280 00281 void GLC_Distance::getPQPPoint(double &p0, double &p1, double &p2, const double x, const double y, const double z, const GLC_Matrix4x4& instanceMatrix) const 00282 { 00283 GLC_Vector3d vector(x, y, z); 00284 vector= instanceMatrix * vector; 00285 p0= vector.x(); 00286 p1= vector.y(); 00287 p2= vector.z(); 00288 } 00289 00290 // Add mesh triangles to PQP model 00291 void GLC_Distance::addMeshTrianglesToPQP(PQP_Model* pPQP_Model, const QList<GLC_Mesh*> listOfMeshes, const GLC_Matrix4x4& instanceMatrix) const 00292 { 00293 int generalCount=0; 00294 00295 // List of axis scaling values 00296 //QList<double> scaleFactors; 00297 //scaleFactors << instanceMatrix.scalingX() << instanceMatrix.scalingY() << instanceMatrix.scalingZ(); 00298 //scaleFactors << 1.0 << 1.0 << 1.0; 00299 00300 int i, j, k, l, size2, size3, size4, pos1, pos2, pos3; 00301 GLC_Mesh * pMesh=NULL; 00302 GLC_uint materialId; 00303 GLfloatVector positionVector; 00304 PQP_REAL p1[3], p2[3], p3[3]; 00305 const int size= listOfMeshes.size(); 00306 00307 for (i= 0; i < size; ++i) 00308 { 00309 pMesh= listOfMeshes.at(i); 00310 positionVector= pMesh->positionVector(); 00311 size2= pMesh->materialCount(); 00312 for(j=0; j< size2;++j){ 00313 00314 materialId= pMesh->material(pMesh->materialIds().at(j))->id(); 00315 if(pMesh->lodContainsMaterial(0, materialId)){ 00316 00317 if (pMesh->containsTriangles(0, materialId)){ 00318 00319 QVector<GLuint> index= pMesh->getTrianglesIndex(0, materialId); 00320 size3= index.size(); 00321 for (k= 0; k < size3; k= k+3) 00322 { 00323 pos1= index.at(k)*3; 00324 pos2= index.at(k+1)*3; 00325 pos3= index.at(k+2)*3; 00326 00327 getPQPPoint(p1[0], p1[1], p1[2], positionVector.at(pos1), positionVector.at(pos1+1), positionVector.at(pos1+2), instanceMatrix); 00328 getPQPPoint(p2[0], p2[1], p2[2], positionVector.at(pos2), positionVector.at(pos2+1), positionVector.at(pos2+2), instanceMatrix); 00329 getPQPPoint(p3[0], p3[1], p3[2], positionVector.at(pos3), positionVector.at(pos3+1), positionVector.at(pos3+2), instanceMatrix); 00330 00331 pPQP_Model->AddTri(p1, p2, p3, generalCount); 00332 ++generalCount; 00333 } 00334 } 00335 00336 if (pMesh->containsStrips(0, materialId)){ 00337 00338 QList<QVector<GLuint> > index= pMesh->getStripsIndex(0, materialId); 00339 size3= index.size(); 00340 for (k= 0; k < size3; ++k) 00341 { 00342 pos1= index.at(k).at(0)*3; 00343 pos2= index.at(k).at(1)*3; 00344 pos3= index.at(k).at(2)*3; 00345 00346 getPQPPoint(p1[0], p1[1], p1[2], positionVector.at(pos1), positionVector.at(pos1+1), positionVector.at(pos1+2), instanceMatrix); 00347 getPQPPoint(p2[0], p2[1], p2[2], positionVector.at(pos2), positionVector.at(pos2+1), positionVector.at(pos2+2), instanceMatrix); 00348 getPQPPoint(p3[0], p3[1], p3[2], positionVector.at(pos3), positionVector.at(pos3+1), positionVector.at(pos3+2), instanceMatrix); 00349 00350 pPQP_Model->AddTri(p1, p2, p3, generalCount); 00351 ++generalCount; 00352 00353 size4= index.at(k).size(); 00354 for (l= 3; l < size4; ++l) 00355 { 00356 pos1= index.at(k).at(l)*3; 00357 pos2= index.at(k).at(l-1)*3; 00358 pos3= index.at(k).at(l-2)*3; 00359 00360 getPQPPoint(p1[0], p1[1], p1[2], positionVector.at(pos1), positionVector.at(pos1+1), positionVector.at(pos1+2), instanceMatrix); 00361 getPQPPoint(p2[0], p2[1], p2[2], positionVector.at(pos2), positionVector.at(pos2+1), positionVector.at(pos2+2), instanceMatrix); 00362 getPQPPoint(p3[0], p3[1], p3[2], positionVector.at(pos3), positionVector.at(pos3+1), positionVector.at(pos3+2), instanceMatrix); 00363 00364 pPQP_Model->AddTri(p1, p2, p3, generalCount); 00365 ++generalCount; 00366 } 00367 } 00368 } 00369 00370 if (pMesh->containsFans(0, materialId)){ 00371 00372 QList<QVector<GLuint> > index= pMesh->getFansIndex(0, materialId); 00373 size3= index.size(); 00374 for (k= 0; k < size3; ++k) 00375 { 00376 pos1= index.at(k).at(0)*3; 00377 pos2= index.at(k).at(1)*3; 00378 pos3= index.at(k).at(2)*3; 00379 00380 getPQPPoint(p1[0], p1[1], p1[2], positionVector.at(pos1), positionVector.at(pos1+1), positionVector.at(pos1+2), instanceMatrix); 00381 getPQPPoint(p2[0], p2[1], p2[2], positionVector.at(pos2), positionVector.at(pos2+1), positionVector.at(pos2+2), instanceMatrix); 00382 getPQPPoint(p3[0], p3[1], p3[2], positionVector.at(pos3), positionVector.at(pos3+1), positionVector.at(pos3+2), instanceMatrix); 00383 00384 pPQP_Model->AddTri(p1, p2, p3, generalCount); 00385 ++generalCount; 00386 00387 size4= index.at(k).size(); 00388 for (l= 3; l < size4; ++l) 00389 { 00390 pos2= index.at(k).at(l-1)*3; 00391 pos3= index.at(k).at(l)*3; 00392 00393 getPQPPoint(p2[0], p2[1], p2[2], positionVector.at(pos2), positionVector.at(pos2+1), positionVector.at(pos2+2), instanceMatrix); 00394 getPQPPoint(p3[0], p3[1], p3[2], positionVector.at(pos3), positionVector.at(pos3+1), positionVector.at(pos3+2), instanceMatrix); 00395 00396 pPQP_Model->AddTri(p1, p2, p3, generalCount); 00397 ++generalCount; 00398 } 00399 } 00400 } 00401 } 00402 } 00403 } 00404 }