From 86be3e276930b1c0610c99fbc45b05d142dd3f27 Mon Sep 17 00:00:00 2001
From: Raphael Grimm <raphael.grimm@kit.edu>
Date: Tue, 2 Jul 2019 17:01:08 +0200
Subject: [PATCH] Optimize collision checking
* remove some pointer derefs
* manually inline some code
* compile pqp into VirtualRobot (library inlining)
* auto format some code
* remove unused datamembers (they were calculated, but we never used them)
---
CMakeLists.txt | 1 -
.../CollisionDetection/CMakeLists.txt | 38 +++---
.../CollisionCheckerImplementation.h | 6 +-
.../CollisionDetection/CollisionModel.h | 4 +-
.../PQP/CollisionCheckerPQP.cpp | 52 ++++----
.../PQP/CollisionCheckerPQP.h | 6 +-
.../PQP/CollisionModelPQP.h | 2 +-
.../PQP/PQP++/OBB_Disjoint.h | 113 ++++--------------
.../CollisionDetection/PQP/PQP++/PQP.cpp | 105 ++++++++--------
.../PQP/PQP++/PQP_Internal.h | 30 ++---
10 files changed, 148 insertions(+), 209 deletions(-)
diff --git a/CMakeLists.txt b/CMakeLists.txt
index 9c387d7cd..f2aa14f0d 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -160,7 +160,6 @@ add_subdirectory(VirtualRobot)
#######################################################################################
list (APPEND SIMOX_EXPORT_TARGET_LIST VirtualRobot)
list (APPEND Simox_LIBRARIES VirtualRobot)
-list(APPEND SIMOX_EXPORT_TARGET_LIST ColCheckerPQP)
#######################################################################################
diff --git a/VirtualRobot/CollisionDetection/CMakeLists.txt b/VirtualRobot/CollisionDetection/CMakeLists.txt
index 3298b79f4..067bd49be 100644
--- a/VirtualRobot/CollisionDetection/CMakeLists.txt
+++ b/VirtualRobot/CollisionDetection/CMakeLists.txt
@@ -4,24 +4,30 @@ FILE(GLOB SRCS ${PROJECT_SOURCE_DIR}/*.cpp)
# check for requested collision detection wrappers
IF (Simox_COLLISION_CHECKER_PQP)
- MESSAGE (STATUS " ** COLLISION DETECTION: PQP")
- SET (COL_CHECKER_WRAPPER_DIR ${PROJECT_SOURCE_DIR}/CollisionDetection/PQP)
- SET(VR_COLLISION_DETECTION_PQP 1)
- SET(COLLISIONDETECTION_LIB ColCheckerPQP)
- SET(COLLISIONDETECTION_DIR ${COL_CHECKER_WRAPPER_DIR}/PQP++)
- SET(COLLISIONDETECTION_INC_DIR ${COLLISIONDETECTION_DIR})
- SET(COLLISIONDETECTION_LIB_DIR ${Simox_LIB_DIR})
-
+ MESSAGE (STATUS " ** COLLISION DETECTION: PQP")
+ SET (COL_CHECKER_WRAPPER_DIR ${PROJECT_SOURCE_DIR}/CollisionDetection/PQP)
+ SET(VR_COLLISION_DETECTION_PQP 1)
+ SET(COLLISIONDETECTION_LIB ColCheckerPQP)
+ SET(COLLISIONDETECTION_DIR ${COL_CHECKER_WRAPPER_DIR}/PQP++)
+ SET(COLLISIONDETECTION_INC_DIR ${COLLISIONDETECTION_DIR})
+
+
+ ############################################################################
+ #inline pqp
+ FILE(GLOB PQP_SRCS ${COLLISIONDETECTION_DIR}/*.cpp)
+ FILE(GLOB PQP_INCS ${COLLISIONDETECTION_DIR}/*.h)
+ SET (SOURCES ${SOURCES} ${PQP_SRCS})
+ SET (INCLUDES ${INCLUDES} ${PQP_INCS})
+ target_compile_definitions(VirtualRobot PUBLIC -DUSE_PQP)
+ ############################################################################
+
# create PQP lib
- ADD_SUBDIRECTORY(${COLLISIONDETECTION_DIR})
- target_link_libraries(VirtualRobot PUBLIC ColCheckerPQP)
ELSE()
- # DUMMY
- MESSAGE (STATUS "WARNING: NO LIBRARY DEFINED FOR COLLISION DETECTION: COLLISION DETECTION IS DISABLED!")
- SET (COL_CHECKER_WRAPPER_DIR ${PROJECT_SOURCE_DIR}/CollisionDetection/Dummy)
- SET(COLLISIONDETECTION_DIR ${Simox_SOURCE_DIR}/VirtualRobot/CollisionDetection/Dummy)
- SET(COLLISIONDETECTION_INC_DIR ${COLLISIONDETECTION_DIR})
- SET(COLLISIONDETECTION_LIB_DIR ${Simox_LIB_DIR})
+ # DUMMY
+ MESSAGE (STATUS "WARNING: NO LIBRARY DEFINED FOR COLLISION DETECTION: COLLISION DETECTION IS DISABLED!")
+ SET (COL_CHECKER_WRAPPER_DIR ${PROJECT_SOURCE_DIR}/CollisionDetection/Dummy)
+ SET(COLLISIONDETECTION_DIR ${Simox_SOURCE_DIR}/VirtualRobot/CollisionDetection/Dummy)
+ SET(COLLISIONDETECTION_INC_DIR ${COLLISIONDETECTION_DIR})
ENDIF ()
MESSAGE (STATUS " ** Directory of collision detection wrapper: " ${COL_CHECKER_WRAPPER_DIR})
diff --git a/VirtualRobot/CollisionDetection/CollisionCheckerImplementation.h b/VirtualRobot/CollisionDetection/CollisionCheckerImplementation.h
index ca59dd8b9..90f7cd53a 100644
--- a/VirtualRobot/CollisionDetection/CollisionCheckerImplementation.h
+++ b/VirtualRobot/CollisionDetection/CollisionCheckerImplementation.h
@@ -59,9 +59,9 @@ namespace VirtualRobot
}
virtual ~CollisionCheckerImplementation() {}
- virtual float calculateDistance(CollisionModelPtr model1, CollisionModelPtr model2, Eigen::Vector3f& P1, Eigen::Vector3f& P2, int* trID1 = NULL, int* trID2 = NULL) = 0;
- virtual bool checkCollision(CollisionModelPtr model1, CollisionModelPtr model2) = 0; //, Eigen::Vector3f *storeContact = NULL) = 0;
- virtual bool checkCollision(CollisionModelPtr model1, const Eigen::Vector3f& point, float tolerance = 0.0f) = 0; //, Eigen::Vector3f *storeContact = NULL) = 0;
+ virtual float calculateDistance(const CollisionModelPtr& model1, const CollisionModelPtr& model2, Eigen::Vector3f& P1, Eigen::Vector3f& P2, int* trID1 = NULL, int* trID2 = NULL) = 0;
+ virtual bool checkCollision(const CollisionModelPtr& model1, const CollisionModelPtr& model2) = 0; //, Eigen::Vector3f *storeContact = NULL) = 0;
+ virtual bool checkCollision(const CollisionModelPtr& model1, const Eigen::Vector3f& point, float tolerance = 0.0f) = 0; //, Eigen::Vector3f *storeContact = NULL) = 0;
virtual void setAutomaticSizeCheck(bool checkSizeOnColModelCreation)
{
diff --git a/VirtualRobot/CollisionDetection/CollisionModel.h b/VirtualRobot/CollisionDetection/CollisionModel.h
index 523ebfd7f..bd2bf69d0 100644
--- a/VirtualRobot/CollisionDetection/CollisionModel.h
+++ b/VirtualRobot/CollisionDetection/CollisionModel.h
@@ -96,12 +96,12 @@ namespace VirtualRobot
}
#if defined(VR_COLLISION_DETECTION_PQP)
- boost::shared_ptr< CollisionModelPQP > getCollisionModelImplementation()
+ const boost::shared_ptr< CollisionModelPQP >& getCollisionModelImplementation()
{
return collisionModelImplementation;
}
#else
- boost::shared_ptr< CollisionModelDummy > getCollisionModelImplementation()
+ const boost::shared_ptr< CollisionModelDummy >& getCollisionModelImplementation()
{
return collisionModelImplementation;
}
diff --git a/VirtualRobot/CollisionDetection/PQP/CollisionCheckerPQP.cpp b/VirtualRobot/CollisionDetection/PQP/CollisionCheckerPQP.cpp
index ee3693619..77f8617a9 100644
--- a/VirtualRobot/CollisionDetection/PQP/CollisionCheckerPQP.cpp
+++ b/VirtualRobot/CollisionDetection/PQP/CollisionCheckerPQP.cpp
@@ -24,9 +24,9 @@
/// \return
///
int tri_tri_intersect_with_isectline(
- const float V0[3],const float V1[3],const float V2[3],
- const float U0[3],const float U1[3],const float U2[3],
- int *coplanar, float isectpt1[3],float isectpt2[3]);
+ const float V0[3], const float V1[3], const float V2[3],
+ const float U0[3], const float U1[3], const float U2[3],
+ int* coplanar, float isectpt1[3], float isectpt2[3]);
namespace VirtualRobot
{
@@ -39,16 +39,16 @@ namespace VirtualRobot
};
static TriTriIntersection intersectTriangles(
- Eigen::Vector3f const& U0, Eigen::Vector3f const& U1, Eigen::Vector3f const& U2,
- Eigen::Vector3f const& V0, Eigen::Vector3f const& V1, Eigen::Vector3f const& V2)
+ Eigen::Vector3f const& U0, Eigen::Vector3f const& U1, Eigen::Vector3f const& U2,
+ Eigen::Vector3f const& V0, Eigen::Vector3f const& V1, Eigen::Vector3f const& V2)
{
TriTriIntersection result;
int coplanar = 0;
int intersects = tri_tri_intersect_with_isectline(V0.data(), V1.data(), V2.data(),
- U0.data(), U1.data(), U2.data(),
- &coplanar,
- result.startPoint.data(), result.endPoint.data());
+ U0.data(), U1.data(), U2.data(),
+ &coplanar,
+ result.startPoint.data(), result.endPoint.data());
result.intersect = (intersects != 0);
result.coplanar = (coplanar != 0);
@@ -63,14 +63,14 @@ namespace VirtualRobot
{
automaticSizeCheck = true;
- PQP::PQP_REAL a[3] = {0.0f,0.0f,0.00001f};
- PQP::PQP_REAL b[3] = {0.0f,0.00001f,0.00001f};
- PQP::PQP_REAL c[3] = {0.0f,0.00001f,0.0f};
+ PQP::PQP_REAL a[3] = {0.0f, 0.0f, 0.00001f};
+ PQP::PQP_REAL b[3] = {0.0f, 0.00001f, 0.00001f};
+ PQP::PQP_REAL c[3] = {0.0f, 0.00001f, 0.0f};
pointModel.reset(new PQP::PQP_Model());
pointModel->BeginModel();
- pointModel->AddTri(a,b,c,99999998);
+ pointModel->AddTri(a, b, c, 99999998);
pointModel->EndModel();
pqpChecker = new PQP::PQP_Checker();
}
@@ -85,7 +85,7 @@ namespace VirtualRobot
}
- float CollisionCheckerPQP::calculateDistance(CollisionModelPtr model1, CollisionModelPtr model2, Eigen::Vector3f& P1, Eigen::Vector3f& P2, int* trID1, int* trID2)
+ float CollisionCheckerPQP::calculateDistance(const CollisionModelPtr& model1, const CollisionModelPtr& model2, Eigen::Vector3f& P1, Eigen::Vector3f& P2, int* trID1, int* trID2)
{
boost::shared_ptr<PQP::PQP_Model> m1 = model1->getCollisionModelImplementation()->getPQPModel();
boost::shared_ptr<PQP::PQP_Model> m2 = model2->getCollisionModelImplementation()->getPQPModel();
@@ -101,14 +101,16 @@ namespace VirtualRobot
rot[2][0] = sb(2,0); rot[2][1] = sb(2,1); rot[2][2] = sb(2,2); tr[2] = sb(2,3);
- bool CollisionCheckerPQP::checkCollision(CollisionModelPtr model1, CollisionModelPtr model2) //, Eigen::Vector3f *storeContact)
+ bool CollisionCheckerPQP::checkCollision(const CollisionModelPtr& model1, const CollisionModelPtr& model2) //, Eigen::Vector3f *storeContact)
{
BOOST_ASSERT(model1);
BOOST_ASSERT(model2);
- BOOST_ASSERT(model1->getCollisionModelImplementation());
- BOOST_ASSERT(model2->getCollisionModelImplementation());
- boost::shared_ptr<PQP::PQP_Model> m1 = model1->getCollisionModelImplementation()->getPQPModel();
- boost::shared_ptr<PQP::PQP_Model> m2 = model2->getCollisionModelImplementation()->getPQPModel();
+ const auto& Impl1 = model1->getCollisionModelImplementation();
+ const auto& Impl2 = model2->getCollisionModelImplementation();
+ BOOST_ASSERT(Impl1);
+ BOOST_ASSERT(Impl2);
+ const boost::shared_ptr<PQP::PQP_Model>& m1 = Impl1->getPQPModel();
+ const boost::shared_ptr<PQP::PQP_Model>& m2 = Impl2->getPQPModel();
BOOST_ASSERT_MSG(m1, "NULL data in ColChecker in m1!");
BOOST_ASSERT_MSG(m2, "NULL data in ColChecker in m2!");
@@ -117,8 +119,10 @@ namespace VirtualRobot
PQP::PQP_REAL T1[3];
PQP::PQP_REAL R2[3][3];
PQP::PQP_REAL T2[3];
- __convEigen2Ar(model1->getCollisionModelImplementation()->getGlobalPose(), R1, T1);
- __convEigen2Ar(model2->getCollisionModelImplementation()->getGlobalPose(), R2, T2);
+ const auto& P1 = Impl1->getGlobalPose();
+ const auto& P2 = Impl2->getGlobalPose();
+ __convEigen2Ar(P1, R1, T1);
+ __convEigen2Ar(P2, R2, T2);
pqpChecker->PQP_Collide(&result,
R1, T1, m1.get(),
R2, T2, m2.get(),
@@ -144,10 +148,10 @@ namespace VirtualRobot
(*storeContact) = t.block(0,3,3,1);
}*/
- return ((bool)(result.Colliding() != 0));
+ return result.Colliding() != 0;
}
- bool CollisionCheckerPQP::checkCollision(CollisionModelPtr model1, const Eigen::Vector3f &point, float tolerance)
+ bool CollisionCheckerPQP::checkCollision(const CollisionModelPtr& model1, const Eigen::Vector3f& point, float tolerance)
{
BOOST_ASSERT(model1);
BOOST_ASSERT(model1->getCollisionModelImplementation());
@@ -159,10 +163,10 @@ namespace VirtualRobot
PQP::PQP_REAL R2[3][3];
PQP::PQP_REAL T2[3];
Eigen::Matrix4f pointPose = Eigen::Matrix4f::Identity();
- pointPose.block<3,1>(0,3) = point;
+ pointPose.block<3, 1>(0, 3) = point;
__convEigen2Ar(model1->getCollisionModelImplementation()->getGlobalPose(), R1, T1);
__convEigen2Ar(pointPose, R2, T2);
- if(tolerance == 0.0f)
+ if (tolerance == 0.0f)
{
PQP::PQP_CollideResult result;
diff --git a/VirtualRobot/CollisionDetection/PQP/CollisionCheckerPQP.h b/VirtualRobot/CollisionDetection/PQP/CollisionCheckerPQP.h
index 8f3239e38..f11f1f489 100644
--- a/VirtualRobot/CollisionDetection/PQP/CollisionCheckerPQP.h
+++ b/VirtualRobot/CollisionDetection/PQP/CollisionCheckerPQP.h
@@ -49,9 +49,9 @@ namespace VirtualRobot
CollisionCheckerPQP();
~CollisionCheckerPQP() override;
- float calculateDistance(CollisionModelPtr model1, CollisionModelPtr model2, Eigen::Vector3f& P1, Eigen::Vector3f& P2, int* trID1 = NULL, int* trID2 = NULL) override;
- bool checkCollision(CollisionModelPtr model1, CollisionModelPtr model2) override; //, Eigen::Vector3f *storeContact = NULL);
- bool checkCollision(CollisionModelPtr model1, const Eigen::Vector3f& point, float tolerance = 0.0f) override;
+ float calculateDistance(const CollisionModelPtr& model1, const CollisionModelPtr& model2, Eigen::Vector3f& P1, Eigen::Vector3f& P2, int* trID1 = NULL, int* trID2 = NULL) override;
+ bool checkCollision(const CollisionModelPtr& model1, const CollisionModelPtr& model2) override; //, Eigen::Vector3f *storeContact = NULL);
+ bool checkCollision(const CollisionModelPtr& model1, const Eigen::Vector3f& point, float tolerance = 0.0f) override;
MultiCollisionResult checkMultipleCollisions(CollisionModelPtr const& model1, CollisionModelPtr const& model2);
diff --git a/VirtualRobot/CollisionDetection/PQP/CollisionModelPQP.h b/VirtualRobot/CollisionDetection/PQP/CollisionModelPQP.h
index 3c2bee627..b52131210 100644
--- a/VirtualRobot/CollisionDetection/PQP/CollisionModelPQP.h
+++ b/VirtualRobot/CollisionDetection/PQP/CollisionModelPQP.h
@@ -56,7 +56,7 @@ namespace VirtualRobot
*/
~CollisionModelPQP() override;
- boost::shared_ptr<PQP::PQP_Model> getPQPModel()
+ const boost::shared_ptr<PQP::PQP_Model>& getPQPModel()
{
return pqpModel;
}
diff --git a/VirtualRobot/CollisionDetection/PQP/PQP++/OBB_Disjoint.h b/VirtualRobot/CollisionDetection/PQP/PQP++/OBB_Disjoint.h
index 353c75ac6..f57735ce4 100644
--- a/VirtualRobot/CollisionDetection/PQP/PQP++/OBB_Disjoint.h
+++ b/VirtualRobot/CollisionDetection/PQP/PQP++/OBB_Disjoint.h
@@ -69,7 +69,6 @@ namespace PQP
obb_disjoint(PQP_REAL B[3][3], PQP_REAL T[3], PQP_REAL a[3], PQP_REAL b[3])
{
PQP_REAL s;
- int r;
PQP_REAL Bf[3][3];
{
@@ -90,175 +89,105 @@ namespace PQP
}
// if any of these tests are one-sided, then the polyhedra are disjoint
- r = 1;
// A1 x A2 = A0
-
- r &= (std::abs(T[0]) <=
- (a[0] + b[0] * Bf[0][0] + b[1] * Bf[0][1] + b[2] * Bf[0][2]));
-
- if (!r)
+ if (std::abs(T[0]) > (a[0] + b[0] * Bf[0][0] + b[1] * Bf[0][1] + b[2] * Bf[0][2]))
{
return 1;
}
- // B1 x B2 = B0
- s = T[0] * B[0][0] + T[1] * B[1][0] + T[2] * B[2][0];
-
- r &= (std::abs(s) <=
- (b[0] + a[0] * Bf[0][0] + a[1] * Bf[1][0] + a[2] * Bf[2][0]));
-
- if (!r)
- {
- return 2;
- }
-
// A2 x A0 = A1
-
- r &= (std::abs(T[1]) <=
- (a[1] + b[0] * Bf[1][0] + b[1] * Bf[1][1] + b[2] * Bf[1][2]));
-
- if (!r)
+ if (std::abs(T[1]) > (a[1] + b[0] * Bf[1][0] + b[1] * Bf[1][1] + b[2] * Bf[1][2]))
{
return 3;
}
// A0 x A1 = A2
-
- r &= (std::abs(T[2]) <=
- (a[2] + b[0] * Bf[2][0] + b[1] * Bf[2][1] + b[2] * Bf[2][2]));
-
- if (!r)
+ if (std::abs(T[2]) > (a[2] + b[0] * Bf[2][0] + b[1] * Bf[2][1] + b[2] * Bf[2][2]))
{
return 4;
}
+ // B1 x B2 = B0
+ s = T[0] * B[0][0] + T[1] * B[1][0] + T[2] * B[2][0];
+ if (std::abs(s) > (b[0] + a[0] * Bf[0][0] + a[1] * Bf[1][0] + a[2] * Bf[2][0]))
+ {
+ return 2;
+ }
+
// B2 x B0 = B1
s = T[0] * B[0][1] + T[1] * B[1][1] + T[2] * B[2][1];
-
- r &= (std::abs(s) <=
- (b[1] + a[0] * Bf[0][1] + a[1] * Bf[1][1] + a[2] * Bf[2][1]));
-
- if (!r)
+ if (std::abs(s) > (b[1] + a[0] * Bf[0][1] + a[1] * Bf[1][1] + a[2] * Bf[2][1]))
{
return 5;
}
// B0 x B1 = B2
s = T[0] * B[0][2] + T[1] * B[1][2] + T[2] * B[2][2];
-
- r &= (std::abs(s) <=
- (b[2] + a[0] * Bf[0][2] + a[1] * Bf[1][2] + a[2] * Bf[2][2]));
-
- if (!r)
+ if (std::abs(s) > (b[2] + a[0] * Bf[0][2] + a[1] * Bf[1][2] + a[2] * Bf[2][2]))
{
return 6;
}
// A0 x B0
s = T[2] * B[1][0] - T[1] * B[2][0];
-
- r &= (std::abs(s) <=
- (a[1] * Bf[2][0] + a[2] * Bf[1][0] +
- b[1] * Bf[0][2] + b[2] * Bf[0][1]));
-
- if (!r)
+ if (std::abs(s) > (a[1] * Bf[2][0] + a[2] * Bf[1][0] + b[1] * Bf[0][2] + b[2] * Bf[0][1]))
{
return 7;
}
// A0 x B1
s = T[2] * B[1][1] - T[1] * B[2][1];
-
- r &= (std::abs(s) <=
- (a[1] * Bf[2][1] + a[2] * Bf[1][1] +
- b[0] * Bf[0][2] + b[2] * Bf[0][0]));
-
- if (!r)
+ if (std::abs(s) > (a[1] * Bf[2][1] + a[2] * Bf[1][1] + b[0] * Bf[0][2] + b[2] * Bf[0][0]))
{
return 8;
}
// A0 x B2
s = T[2] * B[1][2] - T[1] * B[2][2];
-
- r &= (std::abs(s) <=
- (a[1] * Bf[2][2] + a[2] * Bf[1][2] +
- b[0] * Bf[0][1] + b[1] * Bf[0][0]));
-
- if (!r)
+ if (std::abs(s) > (a[1] * Bf[2][2] + a[2] * Bf[1][2] + b[0] * Bf[0][1] + b[1] * Bf[0][0]))
{
return 9;
}
// A1 x B0
s = T[0] * B[2][0] - T[2] * B[0][0];
-
- r &= (std::abs(s) <=
- (a[0] * Bf[2][0] + a[2] * Bf[0][0] +
- b[1] * Bf[1][2] + b[2] * Bf[1][1]));
-
- if (!r)
+ if (std::abs(s) > (a[0] * Bf[2][0] + a[2] * Bf[0][0] + b[1] * Bf[1][2] + b[2] * Bf[1][1]))
{
return 10;
}
// A1 x B1
s = T[0] * B[2][1] - T[2] * B[0][1];
-
- r &= (std::abs(s) <=
- (a[0] * Bf[2][1] + a[2] * Bf[0][1] +
- b[0] * Bf[1][2] + b[2] * Bf[1][0]));
-
- if (!r)
+ if (std::abs(s) > (a[0] * Bf[2][1] + a[2] * Bf[0][1] + b[0] * Bf[1][2] + b[2] * Bf[1][0]))
{
return 11;
}
// A1 x B2
s = T[0] * B[2][2] - T[2] * B[0][2];
-
- r &= (std::abs(s) <=
- (a[0] * Bf[2][2] + a[2] * Bf[0][2] +
- b[0] * Bf[1][1] + b[1] * Bf[1][0]));
-
- if (!r)
+ if (std::abs(s) > (a[0] * Bf[2][2] + a[2] * Bf[0][2] + b[0] * Bf[1][1] + b[1] * Bf[1][0]))
{
return 12;
}
// A2 x B0
s = T[1] * B[0][0] - T[0] * B[1][0];
-
- r &= (std::abs(s) <=
- (a[0] * Bf[1][0] + a[1] * Bf[0][0] +
- b[1] * Bf[2][2] + b[2] * Bf[2][1]));
-
- if (!r)
+ if (std::abs(s) > (a[0] * Bf[1][0] + a[1] * Bf[0][0] + b[1] * Bf[2][2] + b[2] * Bf[2][1]))
{
return 13;
}
// A2 x B1
s = T[1] * B[0][1] - T[0] * B[1][1];
-
- r &= (std::abs(s) <=
- (a[0] * Bf[1][1] + a[1] * Bf[0][1] +
- b[0] * Bf[2][2] + b[2] * Bf[2][0]));
-
- if (!r)
+ if (std::abs(s) > (a[0] * Bf[1][1] + a[1] * Bf[0][1] + b[0] * Bf[2][2] + b[2] * Bf[2][0]))
{
return 14;
}
// A2 x B2
s = T[1] * B[0][2] - T[0] * B[1][2];
-
- r &= (std::abs(s) <=
- (a[0] * Bf[1][2] + a[1] * Bf[0][2] +
- b[0] * Bf[2][1] + b[1] * Bf[2][0]));
-
- if (!r)
+ if (std::abs(s) > (a[0] * Bf[1][2] + a[1] * Bf[0][2] + b[0] * Bf[2][1] + b[1] * Bf[2][0]))
{
return 15;
}
diff --git a/VirtualRobot/CollisionDetection/PQP/PQP++/PQP.cpp b/VirtualRobot/CollisionDetection/PQP/PQP++/PQP.cpp
index 6a8ef8f31..32b2e9f4d 100644
--- a/VirtualRobot/CollisionDetection/PQP/PQP++/PQP.cpp
+++ b/VirtualRobot/CollisionDetection/PQP/PQP++/PQP.cpp
@@ -625,15 +625,18 @@ namespace PQP
res->num_bv_tests++;
BV_Processor p;
- if (!p.BV_Overlap(R, T, o1->child(b1), o2->child(b2)))
+ auto o1cb1 = o1->child(b1);
+ auto o2cb2 = o2->child(b2);
+
+ if (!p.BV_Overlap(R, T, o1cb1, o2cb2))
{
return;
}
// if we are, see if we test triangles next
- int l1 = o1->child(b1)->Leaf();
- int l2 = o2->child(b2)->Leaf();
+ int l1 = o1cb1->Leaf();
+ int l2 = o2cb2->Leaf();
if (l1 && l2)
{
@@ -642,8 +645,8 @@ namespace PQP
#if 1
// transform the points in b2 into space of b1, then compare
- Tri* t1 = &o1->tris[-o1->child(b1)->first_child - 1];
- Tri* t2 = &o2->tris[-o2->child(b2)->first_child - 1];
+ Tri* t1 = &o1->tris[-o1cb1->first_child - 1];
+ Tri* t2 = &o2->tris[-o2cb2->first_child - 1];
PQP_REAL q1[3], q2[3], q3[3];
PQP_REAL* p1 = t1->p1;
PQP_REAL* p2 = t1->p2;
@@ -662,8 +665,8 @@ namespace PQP
#else
PQP_REAL p[3], q[3];
- Tri* t1 = &o1->tris[-o1->child(b1)->first_child - 1];
- Tri* t2 = &o2->tris[-o2->child(b2)->first_child - 1];
+ Tri* t1 = &o1->tris[-o1cb1->first_child - 1];
+ Tri* t2 = &o2->tris[-o2cb2->first_child - 1];
if (TriDistance(res->R, res->T, t1, t2, p, q) == 0.0)
{
@@ -679,23 +682,25 @@ namespace PQP
// we dont, so decide whose children to visit next
- PQP_REAL sz1 = o1->child(b1)->GetSize();
- PQP_REAL sz2 = o2->child(b2)->GetSize();
+ PQP_REAL sz1 = o1cb1->GetSize();
+ PQP_REAL sz2 = o2cb2->GetSize();
PQP_REAL Rc[3][3], Tc[3], Ttemp[3];
if (l2 || (!l1 && (sz1 > sz2)))
{
- int c1 = o1->child(b1)->first_child;
+ int c1 = o1cb1->first_child;
int c2 = c1 + 1;
- pqp_math.MTxM(Rc, o1->child(c1)->R, R);
+ auto o1cc1 = o1->child(c1);
+
+ pqp_math.MTxM(Rc, o1cc1->R, R);
#if PQP_BV_TYPE & OBB_TYPE
- pqp_math.VmV(Ttemp, T, o1->child(c1)->To);
+ pqp_math.VmV(Ttemp, T, o1cc1->To);
#else
- pqp_math.VmV(Ttemp, T, o1->child(c1)->Tr);
+ pqp_math.VmV(Ttemp, T, o1cc1->Tr);
#endif
- pqp_math.MTxV(Tc, o1->child(c1)->R, Ttemp);
+ pqp_math.MTxV(Tc, o1cc1->R, Ttemp);
CollideRecurse(res, Rc, Tc, o1, c1, o2, b2, flag);
if ((flag == PQP_FIRST_CONTACT) && (res->num_pairs > 0))
@@ -703,25 +708,28 @@ namespace PQP
return;
}
- pqp_math.MTxM(Rc, o1->child(c2)->R, R);
+ auto o1cc2 = o1->child(c2);
+ pqp_math.MTxM(Rc, o1cc2->R, R);
#if PQP_BV_TYPE & OBB_TYPE
- pqp_math.VmV(Ttemp, T, o1->child(c2)->To);
+ pqp_math.VmV(Ttemp, T, o1cc2->To);
#else
- pqp_math.VmV(Ttemp, T, o1->child(c2)->Tr);
+ pqp_math.VmV(Ttemp, T, o1cc2->Tr);
#endif
- pqp_math.MTxV(Tc, o1->child(c2)->R, Ttemp);
+ pqp_math.MTxV(Tc, o1cc2->R, Ttemp);
CollideRecurse(res, Rc, Tc, o1, c2, o2, b2, flag);
}
else
{
- int c1 = o2->child(b2)->first_child;
+ int c1 = o2cb2->first_child;
int c2 = c1 + 1;
- pqp_math.MxM(Rc, R, o2->child(c1)->R);
+ auto o2cc1 = o2->child(c1);
+
+ pqp_math.MxM(Rc, R, o2cc1->R);
#if PQP_BV_TYPE & OBB_TYPE
- pqp_math.MxVpV(Tc, R, o2->child(c1)->To, T);
+ pqp_math.MxVpV(Tc, R, o2cc1->To, T);
#else
- pqp_math.MxVpV(Tc, R, o2->child(c1)->Tr, T);
+ pqp_math.MxVpV(Tc, R, o2cc1->Tr, T);
#endif
CollideRecurse(res, Rc, Tc, o1, b1, o2, c1, flag);
@@ -730,11 +738,12 @@ namespace PQP
return;
}
- pqp_math.MxM(Rc, R, o2->child(c2)->R);
+ auto o2cc2 = o2->child(c2);
+ pqp_math.MxM(Rc, R, o2cc2->R);
#if PQP_BV_TYPE & OBB_TYPE
- pqp_math.MxVpV(Tc, R, o2->child(c2)->To, T);
+ pqp_math.MxVpV(Tc, R, o2cc2->To, T);
#else
- pqp_math.MxVpV(Tc, R, o2->child(c2)->Tr, T);
+ pqp_math.MxVpV(Tc, R, o2cc2->Tr, T);
#endif
CollideRecurse(res, Rc, Tc, o1, b1, o2, c2, flag);
}
@@ -746,17 +755,9 @@ namespace PQP
PQP_REAL R2[3][3], PQP_REAL T2[3], PQP_Model* o2,
int flag)
{
- Timer ti;
- double t1 = ti.GetTime();
-
// make sure that the models are built
- if (o1->build_state != PQP_BUILD_STATE_PROCESSED)
- {
- return PQP_ERR_UNPROCESSED_MODEL;
- }
-
- if (o2->build_state != PQP_BUILD_STATE_PROCESSED)
+ if (o1->build_state != PQP_BUILD_STATE_PROCESSED || o2->build_state != PQP_BUILD_STATE_PROCESSED)
{
return PQP_ERR_UNPROCESSED_MODEL;
}
@@ -783,26 +784,26 @@ namespace PQP
PQP_REAL Rtemp[3][3], R[3][3], T[3];
- pqp_math.MxM(Rtemp, res->R, o2->child(0)->R);
- pqp_math.MTxM(R, o1->child(0)->R, Rtemp);
+ auto o1c0 = o1->child(0);
+ auto o2c0 = o2->child(0);
+
+ pqp_math.MxM(Rtemp, res->R, o2c0->R);
+ pqp_math.MTxM(R, o1c0->R, Rtemp);
#if PQP_BV_TYPE & OBB_TYPE
- pqp_math.MxVpV(Ttemp, res->R, o2->child(0)->To, res->T);
- pqp_math.VmV(Ttemp, Ttemp, o1->child(0)->To);
+ pqp_math.MxVpV(Ttemp, res->R, o2c0->To, res->T);
+ pqp_math.VmV(Ttemp, Ttemp, o1c0->To);
#else
- pqp_math.MxVpV(Ttemp, res->R, o2->child(0)->Tr, res->T);
- pqp_math.VmV(Ttemp, Ttemp, o1->child(0)->Tr);
+ pqp_math.MxVpV(Ttemp, res->R, o2c0->Tr, res->T);
+ pqp_math.VmV(Ttemp, Ttemp, o1c0->Tr);
#endif
- pqp_math.MTxV(T, o1->child(0)->R, Ttemp);
+ pqp_math.MTxV(T, o1c0->R, Ttemp);
// now start with both top level BVs
CollideRecurse(res, R, T, o1, 0, o2, 0, flag);
- double t2 = ti.GetTime();
- res->query_time_secs = t2 - t1;
-
return PQP_OK;
}
@@ -1112,8 +1113,8 @@ namespace PQP
PQP_REAL rel_err, PQP_REAL abs_err,
int qsize)
{
- Timer ti;
- double time1 = ti.GetTime();
+ // Timer ti;
+ // double time1 = ti.GetTime();
// make sure that the models are built
@@ -1227,8 +1228,8 @@ namespace PQP
Vprint(res->p1);
Vprint(res->p2);*/
- double time2 = ti.GetTime();
- res->query_time_secs = time2 - time1;
+ // double time2 = ti.GetTime();
+ // res->query_time_secs = time2 - time1;
return PQP_OK;
}
@@ -1535,8 +1536,8 @@ namespace PQP
PQP_REAL tolerance,
int qsize)
{
- Timer ti;
- double time1 = ti.GetTime();
+ // Timer ti;
+ // double time1 = ti.GetTime();
// make sure that the models are built
@@ -1616,8 +1617,8 @@ namespace PQP
pqp_math.VmV(u, res->p2, res->T);
pqp_math.MTxV(res->p2, res->R, u);
- double time2 = ti.GetTime();
- res->query_time_secs = time2 - time1;
+ // double time2 = ti.GetTime();
+ // res->query_time_secs = time2 - time1;
return PQP_OK;
}
diff --git a/VirtualRobot/CollisionDetection/PQP/PQP++/PQP_Internal.h b/VirtualRobot/CollisionDetection/PQP/PQP++/PQP_Internal.h
index a7b7222b2..1469fd5ad 100644
--- a/VirtualRobot/CollisionDetection/PQP/PQP++/PQP_Internal.h
+++ b/VirtualRobot/CollisionDetection/PQP/PQP++/PQP_Internal.h
@@ -92,7 +92,7 @@ namespace PQP
int num_bv_tests;
int num_tri_tests;
- double query_time_secs;
+ // double query_time_secs;
// xform from model 1 to model 2
@@ -119,10 +119,10 @@ namespace PQP
{
return num_tri_tests;
}
- double QueryTimeSecs()
- {
- return query_time_secs;
- }
+ // double QueryTimeSecs()
+ // {
+ // return query_time_secs;
+ // }
// free the list of contact pairs; ordinarily this list is reused
// for each query, and only deleted in the destructor.
@@ -157,7 +157,7 @@ namespace PQP
int num_bv_tests;
int num_tri_tests;
- double query_time_secs;
+ // double query_time_secs;
// xform from model 1 to model 2
@@ -187,10 +187,10 @@ namespace PQP
{
return num_tri_tests;
}
- double QueryTimeSecs()
- {
- return query_time_secs;
- }
+ // double QueryTimeSecs()
+ // {
+ // return query_time_secs;
+ // }
// The following distance and points established the minimum distance
// for the models, within the relative and absolute error bounds
@@ -228,7 +228,7 @@ namespace PQP
int num_bv_tests;
int num_tri_tests;
- double query_time_secs;
+ // double query_time_secs;
// xform from model 1 to model 2
@@ -253,10 +253,10 @@ namespace PQP
{
return num_tri_tests;
}
- double QueryTimeSecs()
- {
- return query_time_secs;
- }
+ // double QueryTimeSecs()
+ // {
+ // return query_time_secs;
+ // }
// If the models are closer than ( <= ) tolerance, these points
// and distance were what established this. Otherwise,
--
GitLab