Skip to content
GitLab
Unverified Commit f8fe2d8a authored by Joseph Mirabel's avatar Joseph Mirabel Committed by GitHub
Browse files

Merge pull request #146 from jcarpent/topic/devel 

Improce code efficiency + use shared memory between Numpy and Eigen
parents cf5feb8d 3a0cdbbc
Hide whitespace changes
Inline Side-by-side
CMakeLists.txt
View file @ f8fe2d8a
......@@ -74,7 +74,7 @@ PROJECT(${PROJECT_NAME} ${PROJECT_ARGS})
add_required_dependency("eigen3 >= 3.0.0")
if (BUILD_PYTHON_INTERFACE)
ADD_COMPILE_DEPENDENCY("eigenpy >= 2.0")
ADD_COMPILE_DEPENDENCY("eigenpy >= 2.2")
endif ()
# Add a cache variable to allow not compiling and running tests
......
cmake @ 321eb1cc
Compare 93ec987e...321eb1cc
Subproject commit 93ec987ecdc016039c8731e203943e5a83eb96d5
Subproject commit 321eb1ccf1d94570eb564f3659b13ef3ef82239e
include/hpp/fcl/narrowphase/narrowphase.h
View file @ f8fe2d8a
......@@ -299,6 +299,8 @@ namespace fcl
mutable Vec3f cached_guess;
};
#pragma GCC diagnostic push
#pragma GCC diagnostic ignored "-Wc99-extensions"
/// \name Shape intersection specializations
/// \{
......@@ -403,6 +405,7 @@ namespace fcl
#undef HPP_FCL_DECLARE_SHAPE_DISTANCE_PAIR
/// \}
#pragma GCC diagnostic pop
}
} // namespace hpp
......
package.xml
View file @ f8fe2d8a
<?xml version="1.0"?>
<package format="2">
<name>hpp-fcl</name>
<version>1.3.0</version>
<version>1.4.0</version>
<description>An extension of the Flexible Collision Library.</description>
<!-- The maintainer listed here is for the ROS release to receive emails for the buildfarm.
Please check the repository URL for full list of authors and maintainers. -->
......
python/collision-geometries.cc
View file @ f8fe2d8a
......@@ -35,6 +35,7 @@
#include <boost/python.hpp>
#include <eigenpy/registration.hpp>
#include <eigenpy/eigen-to-python.hpp>
#include "fcl.hh"
......@@ -138,10 +139,7 @@ void exposeShapes ()
.def (dv::init<Box>())
.def (dv::init<Box, FCL_REAL,FCL_REAL,FCL_REAL>())
.def (dv::init<Box, const Vec3f&>())
.add_property("halfSide",
make_getter(&Box::halfSide, return_value_policy<return_by_value>()),
make_setter(&Box::halfSide, return_value_policy<return_by_value>()),
doxygen::class_attrib_doc<Box>("halfSide"))
.DEF_RW_CLASS_ATTRIB(Box,halfSide)
;
class_ <Capsule, bases<ShapeBase>, shared_ptr<Capsule> >
......@@ -263,11 +261,11 @@ void exposeCollisionGeometries ()
class_<AABB>("AABB",
"A class describing the AABB collision structure, which is a box in 3D space determined by two diagonal points",
no_init)
.def(init<>("Default constructor"))
.def(init<Vec3f>(bp::arg("v"),"Creating an AABB at position v with zero size."))
.def(init<Vec3f,Vec3f>(bp::args("a","b"),"Creating an AABB with two endpoints a and b."))
.def(init<AABB,Vec3f>(bp::args("core","delta"),"Creating an AABB centered as core and is of half-dimension delta."))
.def(init<Vec3f,Vec3f,Vec3f>(bp::args("a","b","c"),"Creating an AABB contains three points."))
.def(init<>(bp::arg("self"), "Default constructor"))
.def(init<Vec3f>(bp::args("self","v"),"Creating an AABB at position v with zero size."))
.def(init<Vec3f,Vec3f>(bp::args("self","a","b"),"Creating an AABB with two endpoints a and b."))
.def(init<AABB,Vec3f>(bp::args("self","core","delta"),"Creating an AABB centered as core and is of half-dimension delta."))
.def(init<Vec3f,Vec3f,Vec3f>(bp::args("self","a","b","c"),"Creating an AABB contains three points."))
.def("contain",
(bool (AABB::*)(const Vec3f &) const)&AABB::contain,
......
python/collision.cc
View file @ f8fe2d8a
//
// Software License Agreement (BSD License)
//
// Copyright (c) 2019 CNRS-LAAS INRIA
// Copyright (c) 2019-2020 CNRS-LAAS INRIA
// Author: Joseph Mirabel
// All rights reserved.
//
......@@ -35,6 +35,7 @@
#include <boost/python.hpp>
#include <boost/python/suite/indexing/vector_indexing_suite.hpp>
#include <eigenpy/eigen-to-python.hpp>
#include <eigenpy/registration.hpp>
#include <hpp/fcl/fwd.hh>
......@@ -83,7 +84,6 @@ void exposeCollisionAPI ()
doxygen::class_doc<CollisionRequest>(), no_init)
.def (dv::init<CollisionRequest>())
.def (dv::init<CollisionRequest, const CollisionRequestFlag, size_t>())
.DEF_RW_CLASS_ATTRIB (CollisionRequest, num_max_contacts )
.DEF_RW_CLASS_ATTRIB (CollisionRequest, enable_contact )
.DEF_RW_CLASS_ATTRIB (CollisionRequest, enable_distance_lower_bound)
......@@ -97,21 +97,15 @@ void exposeCollisionAPI ()
if(!eigenpy::register_symbolic_link_to_registered_type<Contact>())
{
class_ <Contact> ("Contact",
doxygen::class_doc<Contact>(), init<>())
doxygen::class_doc<Contact>(), init<>(arg("self"),"Default constructor"))
//.def(init<CollisionGeometryPtr_t, CollisionGeometryPtr_t, int, int>())
//.def(init<CollisionGeometryPtr_t, CollisionGeometryPtr_t, int, int, Vec3f, Vec3f, FCL_REAL>())
.DEF_RO_CLASS_ATTRIB (Contact, o1)
.DEF_RO_CLASS_ATTRIB (Contact, o2)
.DEF_RW_CLASS_ATTRIB (Contact, b1)
.DEF_RW_CLASS_ATTRIB (Contact, b2)
.add_property("normal",
make_getter(&Contact::normal, return_value_policy<return_by_value>()),
make_setter(&Contact::normal, return_value_policy<return_by_value>()),
doxygen::class_attrib_doc<Contact>("normal"))
.add_property("pos",
make_getter(&Contact::pos, return_value_policy<return_by_value>()),
make_setter(&Contact::pos, return_value_policy<return_by_value>()),
doxygen::class_attrib_doc<Contact>("pos"))
.DEF_RW_CLASS_ATTRIB (Contact, normal)
.DEF_RW_CLASS_ATTRIB (Contact, pos)
.DEF_RW_CLASS_ATTRIB (Contact, penetration_depth)
.def (self == self)
.def (self != self)
......
python/distance.cc
View file @ f8fe2d8a
......@@ -36,6 +36,7 @@
#include <boost/python/suite/indexing/vector_indexing_suite.hpp>
#include <eigenpy/registration.hpp>
#include <eigenpy/eigen-to-python.hpp>
#include "fcl.hh"
......@@ -65,6 +66,8 @@ using namespace boost::python;
using namespace hpp::fcl;
namespace dv = doxygen::visitor;
struct DistanceRequestWrapper
{
static Vec3f getNearestPoint1(const DistanceResult & res) { return res.nearest_points[0]; }
......@@ -77,7 +80,10 @@ void exposeDistanceAPI ()
{
class_ <DistanceRequest> ("DistanceRequest",
doxygen::class_doc<DistanceRequest>(),
init<optional<bool,FCL_REAL,FCL_REAL> >())
init<optional<bool,FCL_REAL,FCL_REAL> >((arg("self"),
arg("enable_nearest_points"),
arg("rel_err"),
arg("abs_err")),"Constructor"))
.DEF_RW_CLASS_ATTRIB (DistanceRequest, enable_nearest_points)
.DEF_RW_CLASS_ATTRIB (DistanceRequest, rel_err)
.DEF_RW_CLASS_ATTRIB (DistanceRequest, abs_err)
......@@ -87,12 +93,11 @@ void exposeDistanceAPI ()
if(!eigenpy::register_symbolic_link_to_registered_type<DistanceResult>())
{
class_ <DistanceResult> ("DistanceResult",
doxygen::class_doc<DistanceResult>(), init<>())
doxygen::class_doc<DistanceResult>(),
no_init)
.def (dv::init<DistanceResult>())
.DEF_RW_CLASS_ATTRIB (DistanceResult, min_distance)
.add_property("normal",
make_getter(&DistanceResult::normal, return_value_policy<return_by_value>()),
make_setter(&DistanceResult::normal, return_value_policy<return_by_value>()),
doxygen::class_attrib_doc<DistanceResult>("normal"))
.DEF_RW_CLASS_ATTRIB(DistanceResult, normal)
//.def_readwrite ("nearest_points", &DistanceResult::nearest_points)
.def("getNearestPoint1",&DistanceRequestWrapper::getNearestPoint1,
doxygen::class_attrib_doc<DistanceResult>("nearest_points"))
......
python/fcl.cc
View file @ f8fe2d8a
......@@ -64,10 +64,10 @@ void exposeMeshLoader ()
{
class_ <MeshLoader, boost::shared_ptr<MeshLoader> > ("MeshLoader",
doxygen::class_doc<MeshLoader>(),
init< optional< NODE_TYPE> >(arg("node_type"),
init< optional< NODE_TYPE> >((arg("self"),arg("node_type")),
doxygen::constructor_doc<MeshLoader, const NODE_TYPE&>()))
.def ("load",&MeshLoader::load,
load_overloads(args("filename","scale"),
load_overloads((arg("self"),arg("filename"),arg("scale")),
doxygen::member_func_doc(&MeshLoader::load)))
;
}
......@@ -77,7 +77,7 @@ void exposeMeshLoader ()
class_ <CachedMeshLoader, bases<MeshLoader>, boost::shared_ptr<CachedMeshLoader> > (
"CachedMeshLoader",
doxygen::class_doc<MeshLoader>(),
init< optional< NODE_TYPE> >(arg("node_type"),
init< optional< NODE_TYPE> >((arg("self"),arg("node_type")),
doxygen::constructor_doc<CachedMeshLoader, const NODE_TYPE&>()))
;
}
......
python/math.cc
View file @ f8fe2d8a
......@@ -80,7 +80,7 @@ void exposeMaths ()
eigenpy::enableEigenPySpecific<Matrix3f>();
eigenpy::enableEigenPySpecific<Vec3f >();
class_ <Transform3f> ("Transform3f", doxygen::class_doc<Transform3f>(), init<>(doxygen::constructor_doc<Transform3f>()))
class_ <Transform3f> ("Transform3f", doxygen::class_doc<Transform3f>(), init<>(arg("self"),doxygen::constructor_doc<Transform3f>()))
.def (init<Matrix3f, Vec3f> (doxygen::constructor_doc<Transform3f, const Matrix3f::MatrixBase&, const Vec3f::MatrixBase&>()))
.def (init<Quaternion3f, Vec3f>(doxygen::constructor_doc<Transform3f, const Quaternion3f& , const Vec3f::MatrixBase&>()))
.def (init<Matrix3f> (doxygen::constructor_doc<Transform3f, const Matrix3f& >()))
......
src/narrowphase/details.h
View file @ f8fe2d8a
......@@ -2019,7 +2019,7 @@ namespace fcl {
// FCL_REAL* penetration_depth, Vec3f* normal)
{
static const FCL_REAL eps (sqrt (std::numeric_limits<FCL_REAL>::epsilon()));
Plane new_s2 = transform(s2, tf2);
const Plane new_s2 = transform(s2, tf2);
const Matrix3f& R = tf1.getRotation();
const Vec3f& T = tf1.getTranslation();
......@@ -2027,7 +2027,7 @@ namespace fcl {
const Vec3f Q (R.transpose() * new_s2.n);
const Vec3f A (Q.cwiseProduct(s1.halfSide));
FCL_REAL signed_dist = new_s2.signedDistance(T);
const FCL_REAL signed_dist = new_s2.signedDistance(T);
distance = std::abs(signed_dist) - A.lpNorm<1>();
if(distance > 0) {
// Is the box above or below the plane
......@@ -2048,11 +2048,6 @@ namespace fcl {
return false;
}
Vec3f axis[3];
axis[0] = R.col(0);
axis[1] = R.col(1);
axis[2] = R.col(2);
// find the deepest point
Vec3f p = T;
......@@ -2065,29 +2060,29 @@ namespace fcl {
std::abs(Q[0] + 1) < planeIntersectTolerance<FCL_REAL>())
{
int sign2 = (A[0] > 0) ? -sign : sign;
p += R.col(0) * (s1.halfSide[0] * sign2);
p.noalias() += R.col(0) * (s1.halfSide[0] * sign2);
}
else if(std::abs(Q[1] - 1) < planeIntersectTolerance<FCL_REAL>() ||
std::abs(Q[1] + 1) < planeIntersectTolerance<FCL_REAL>())
{
int sign2 = (A[1] > 0) ? -sign : sign;
p += R.col(1) * (s1.halfSide[1] * sign2);
p.noalias() += R.col(1) * (s1.halfSide[1] * sign2);
}
else if(std::abs(Q[2] - 1) < planeIntersectTolerance<FCL_REAL>() ||
std::abs(Q[2] + 1) < planeIntersectTolerance<FCL_REAL>())
{
int sign2 = (A[2] > 0) ? -sign : sign;
p += R.col(2) * (s1.halfSide[2] * sign2);
p.noalias() += R.col(2) * (s1.halfSide[2] * sign2);
}
else
{
Vec3f tmp(sign * R * s1.halfSide);
p += (A.array() > 0).select (- tmp, tmp);
p.noalias() += (A.array() > 0).select (- tmp, tmp);
}
// compute the contact point by project the deepest point onto the plane
if (signed_dist > 0) normal = -new_s2.n; else normal = new_s2.n;
p1 = p2 = p - new_s2.n * new_s2.signedDistance(p);
p1 = p2.noalias() = p - new_s2.n * new_s2.signedDistance(p);
return true;
}
......
Supports Markdown
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment