From 33d762c7e526052db09c2f3f010f467e07da9393 Mon Sep 17 00:00:00 2001
From: vahrenkamp <vahrenkamp@042f3d55-54a8-47e9-b7fb-15903f145c44>
Date: Fri, 22 Feb 2013 13:10:12 +0000
Subject: [PATCH] Bugfix: CollisionCheck without id pointer caused segfault.
Added NULL check.
git-svn-id: http://svn.code.sf.net/p/simox/code/trunk@366 042f3d55-54a8-47e9-b7fb-15903f145c44
---
.../PQP/CollisionCheckerPQP.cpp | 27 ++---------
.../PQP/CollisionModelPQP.cpp | 45 -------------------
.../CollisionDetection/PQP/PQP++/PQP.cpp | 4 +-
3 files changed, 6 insertions(+), 70 deletions(-)
diff --git a/VirtualRobot/CollisionDetection/PQP/CollisionCheckerPQP.cpp b/VirtualRobot/CollisionDetection/PQP/CollisionCheckerPQP.cpp
index ef8a11ffe..dfb332d3d 100644
--- a/VirtualRobot/CollisionDetection/PQP/CollisionCheckerPQP.cpp
+++ b/VirtualRobot/CollisionDetection/PQP/CollisionCheckerPQP.cpp
@@ -37,31 +37,10 @@ float CollisionCheckerPQP::calculateDistance (CollisionModelPtr model1, Collisio
VR_ASSERT_MESSAGE(m1&&m2,"NULL data in ColChecker!");
float res = getMinDistance(m1,m2,model1->getCollisionModelImplementation()->getGlobalPose(),model2->getCollisionModelImplementation()->getGlobalPose(),P1,P2,trID1,trID2);
- /*if (model1->hasCollisionId(*trID2) && !model1->hasCollisionId(*trID1))
- {
- // xchange points
- int t = *trID1;
- *trID1 = *trID2;
- *trID2 = t;
- float p[3];
- for (int i=0;i<3;i++)
- {
- p[i] = P1[i];
- P1[i] = P2[i];
- P2[i] = p[i];
- }
- }*/
return res;
}
-
-// #define __convEigen42Ar(sb,rot,tr) rot[0][0] = sb[0][0]; rot[1][0] = sb[0][1]; rot[2][0] = sb[0][2]; tr[0] = sb[3][0]; \
-// rot[0][1] = sb[1][0]; rot[1][1] = sb[1][1]; rot[2][1] = sb[1][2]; tr[1] = sb[3][1]; \
-// rot[0][2] = sb[2][0]; rot[1][2] = sb[2][1]; rot[2][2] = sb[2][2]; tr[2] = sb[3][2];
-// #define __convSb2Ar4(sb,mat4) mat4[0][0] = sb[0][0]; mat4[0][1] = sb[1][0]; mat4[0][2] = sb[2][0]; mat4[0][3] = sb[3][0]; \
-// mat4[1][0] = sb[0][1]; mat4[1][1] = sb[1][1]; mat4[1][2] = sb[2][1]; mat4[1][3] = sb[3][1]; \
-// mat4[2][0] = sb[0][2]; mat4[2][1] = sb[1][2]; mat4[2][2] = sb[2][2]; mat4[2][3] = sb[3][2];
#define __convEigen2Ar(sb,rot,tr) rot[0][0] = sb(0,0); rot[0][1] = sb(0,1); rot[0][2] = sb(0,2); tr[0] = sb(0,3); \
rot[1][0] = sb(1,0); rot[1][1] = sb(1,1); rot[1][2] = sb(1,2); tr[1] = sb(1,3); \
rot[2][0] = sb(2,0); rot[2][1] = sb(2,1); rot[2][2] = sb(2,2); tr[2] = sb(2,3);
@@ -222,8 +201,10 @@ float CollisionCheckerPQP::getMinDistance(boost::shared_ptr<PQP::PQP_Model> m1,
storeP2[2] = (float)result.P2()[2];
// added code to store iDs to original PQP sources, if you get an error here, disable the ID query
- *storeID1 = result.P1_ID();
- *storeID2 = result.P2_ID();
+ if (storeID1)
+ *storeID1 = result.P1_ID();
+ if (storeID2)
+ *storeID2 = result.P2_ID();
//////////////////////////////
return (float)result.Distance();
diff --git a/VirtualRobot/CollisionDetection/PQP/CollisionModelPQP.cpp b/VirtualRobot/CollisionDetection/PQP/CollisionModelPQP.cpp
index b5d45b924..6da416fb4 100644
--- a/VirtualRobot/CollisionDetection/PQP/CollisionModelPQP.cpp
+++ b/VirtualRobot/CollisionDetection/PQP/CollisionModelPQP.cpp
@@ -99,49 +99,4 @@ void CollisionModelPQP::print()
cout << endl;
}
-/*
-int CollisionModelPQP::BuildColModel(std::map<SoNode*,int> &mIVIDMapping, std::vector<int> & vStoreIDs, SoSeparator *pAddIVModel)
-{
- if (!m_pColCheckerPQP)
- return 0;
- int nodeCount = 0;
- //int idNr = s_nID;
- int trNr = 0;
-
- delete m_pPqpModel;
- m_pPqpModel = new PQP::PQP_Model();
- m_pPqpModel->BeginModel();
-
- // add iv models
-
- std::map<SoNode*,int>::iterator iter;
- SoNode* pNode;
- int nId;
-
- for (iter = mIVIDMapping.begin(); iter != mIVIDMapping.end(); ++iter)
- {
- nodeCount++;
- pNode = iter->first;
- nId = iter->second;
-
- if (pNode!=NULL)
- {
- int trrr = m_pColCheckerPQP->oiv2ColMod(pNode,m_pPqpModel,nId,true);
- vStoreIDs.push_back(nId);
- pAddIVModel->addChild(pNode);
- if (trrr==0)
- {
- cout << __FUNCTION__ << " Null triangle object with ID " << nId << endl;
- } else
- {
- trNr += trrr;
- }
- }
- }
-
- m_pPqpModel->EndModel();
- return trNr;
-
-}*/
-
} // namespace
diff --git a/VirtualRobot/CollisionDetection/PQP/PQP++/PQP.cpp b/VirtualRobot/CollisionDetection/PQP/PQP++/PQP.cpp
index ce1cc5014..499742787 100644
--- a/VirtualRobot/CollisionDetection/PQP/PQP++/PQP.cpp
+++ b/VirtualRobot/CollisionDetection/PQP/PQP++/PQP.cpp
@@ -256,9 +256,9 @@ PQP_Model::MemUsage(int msg)
if (msg)
{
//fprintf(stderr,"Total for model %x: %d bytes\n", (unsigned int)this, total_mem);
- fprintf(stderr,"BVs: %d alloced, take %d bytes each\n",
+ fprintf(stderr,"BVs: %d alloced, take %zu bytes each\n",
num_bvs, sizeof(BV));
- fprintf(stderr,"Tris: %d alloced, take %d bytes each\n",
+ fprintf(stderr,"Tris: %d alloced, take %zu bytes each\n",
num_tris, sizeof(Tri));
}
--
GitLab