Unverified Commit 8b49fc63 authored by Joseph Mirabel's avatar Joseph Mirabel Committed by GitHub
Browse files

Merge pull request #78 from jmirabel/python

Enable Convex / Convex queries + Add Python bindings.
parents 083fdd4c e6b8d2d0
......@@ -37,6 +37,7 @@ set(CXX_DISABLE_WERROR TRUE)
include(cmake/base.cmake)
include(cmake/eigen.cmake)
include(cmake/boost.cmake)
include(cmake/python.cmake)
include(cmake/hpp.cmake)
set(PROJECT_NAME hpp-fcl)
......@@ -57,6 +58,8 @@ IF(APPLE)
endif("${isSystemDir}" STREQUAL "-1")
ENDIF(APPLE)
OPTION(BUILD_PYTHON_INTERFACE OFF)
setup_hpp_project()
add_required_dependency("eigen3 >= 3.0.0")
......@@ -66,10 +69,13 @@ include_directories(SYSTEM ${EIGEN3_INCLUDE_DIRS})
set (RUN_TESTS TRUE CACHE BOOL "compile and run unit tests")
# Required dependencies
set(BOOST_COMPONENTS thread date_time system)
if (RUN_TESTS)
set(BOOST_COMPONENTS thread date_time filesystem system unit_test_framework)
else ()
set(BOOST_COMPONENTS thread date_time system)
set(BOOST_COMPONENTS ${BOOST_COMPONENTS} filesystem unit_test_framework)
endif ()
if (BUILD_PYTHON_INTERFACE)
FINDPYTHON()
set(BOOST_COMPONENTS ${BOOST_COMPONENTS} python)
endif ()
search_for_boost()
......@@ -153,6 +159,9 @@ SET(${PROJECT_NAME}_HEADERS
)
add_subdirectory(src)
if (BUILD_PYTHON_INTERFACE)
add_subdirectory(python)
endif ()
if (RUN_TESTS)
add_subdirectory(test)
endif ()
......
......@@ -95,9 +95,14 @@ implications = [
[ [- 8, 9, 12,], [- 10] ],
[ [ 8,- 9,- 12,], [ 10] ],
[ [- 4, 9,- 10,], [- 11,- 12] ],
[ [- 6, 5,- 11,], [- 12,- 10] ],
[ [- 8, 7,- 12,], [- 10,- 11] ],
[ [ 10, 3, 9, -12, 4, -5], [1] ],
[ [ 10, -3, 1, -4], [9] ],
[ [ 10, -3, -1, 2, -6, 11], [5] ],
[ [ -10, 11, 2, -12, -5, -1], [6] ],
[ [ -10,11,-2,1,5], [-6] ],
[ [-10,-11,12,1,-7,-2,4],[-5]],
[ [-10,-11,12,-3,2,7],[-8]],
[ [-10,-11,12,-3,-2],[-1]],
]
def set_test_values (current_tests, test_values, itest, value):
......@@ -128,6 +133,11 @@ def set_test_values (current_tests, test_values, itest, value):
raise ValueError ("Absurd case")
return remaining_tests, next_test_values
def set_tests_values (current_tests, test_values, itests, values):
for itest,value in zip(itests,values):
current_tests, test_values = set_test_values (current_tests, test_values, itest, value)
return current_tests, test_values
def apply_test_values (cases, test_values):
def canSatisfy (values, indices):
for k in indices:
......@@ -229,12 +239,12 @@ def printComments (order, indent, file):
for comment in order['comments']:
print (indent + "// " + comment, file=file)
def printOrder (order, indent = "", start=True,file=sys.stdout):
def printOrder (order, indent = "", start=True,file=sys.stdout,curTests=[]):
if start:
print ("bool GJK::projectTetrahedraOrigin(const Simplex& current, Simplex& next)", file=file)
print ("{", file=file)
print (indent+"// The code of this function was generated using doc/gjk.py", file=file)
print (indent+"const int a = 3, b = 2, c = 1, d = 0;", file=file)
print (indent+"const vertex_id_t a = 3, b = 2, c = 1, d = 0;", file=file)
for l in "abcd":
print (indent+"const Vec3f& {} (current.vertex[{}]->w);".format(l.upper(),l), file=file)
print (indent+"const FCL_REAL aa = A.squaredNorm();".format(l), file=file)
......@@ -301,20 +311,22 @@ def printOrder (order, indent = "", start=True,file=sys.stdout):
check = checks[order['test']]
check_hr = checks_hr[order['test']]
printComments (order, indent, file)
nextTests_t=curTests+["a"+str(order['test']+1),]
nextTests_f=curTests+["!a"+str(order['test']+1),]
if order['true'] is None:
if order['false'] is None:
print (indent + """assert(false && "Case {} should never happen.");""".format(check_hr))
else:
print (indent + "assert(!({} <= 0)); // Not {}".format(check, check_hr), file=file)
printOrder (order['false'], indent=indent, start=False, file=file)
print (indent + "assert(!({} <= 0)); // Not {} / {}".format(check, check_hr, ".".join(nextTests_f)), file=file)
printOrder (order['false'], indent=indent, start=False, file=file, curTests=nextTests_f)
elif order['false'] is None:
print (indent + "assert({} <= 0); // {}".format(check, check_hr), file=file)
printOrder (order['true'], indent=indent, start=False, file=file)
print (indent + "assert({} <= 0); // {} / {}".format(check, check_hr, ".".join(nextTests_t)), file=file)
printOrder (order['true'], indent=indent, start=False, file=file, curTests=nextTests_t)
else:
print (indent + "if ({} <= 0) {{ // if {}".format(check, check_hr), file=file)
printOrder (order['true'], indent=indent+" ", start=False, file=file)
print (indent + "}} else {{ // not {}".format(check_hr), file=file)
printOrder (order['false'], indent=indent+" ", start=False, file=file)
print (indent + "if ({} <= 0) {{ // if {} / {}".format(check, check_hr, ".".join(nextTests_t)), file=file)
printOrder (order['true'], indent=indent+" ", start=False, file=file, curTests=nextTests_t)
print (indent + "}} else {{ // not {} / {}".format(check_hr, ".".join(nextTests_f)), file=file)
printOrder (order['false'], indent=indent+" ", start=False, file=file, curTests=nextTests_f)
print (indent + "}} // end of {}".format(check_hr), file=file)
if start:
......
......@@ -52,11 +52,11 @@ namespace hpp
namespace fcl
{
/// @brief A class describing the bounding hierarchy of a mesh model or a point cloud model (which is viewed as a degraded version of mesh)
template<typename BV>
class BVHModel : public CollisionGeometry
{
class ConvexBase;
/// @brief A base class describing the bounding hierarchy of a mesh model or a point cloud model (which is viewed as a degraded version of mesh)
class BVHModelBase : public CollisionGeometry
{
public:
/// @brief Geometry point data
Vec3f* vertices;
......@@ -76,11 +76,8 @@ public:
/// @brief The state of BVH building process
BVHBuildState build_state;
/// @brief Split rule to split one BV node into two children
boost::shared_ptr<BVSplitterBase<BV> > bv_splitter;
/// @brief Fitting rule to fit a BV node to a set of geometry primitives
boost::shared_ptr<BVFitterBase<BV> > bv_fitter;
/// @brief Convex<Triangle> representation of this object
boost::shared_ptr< ConvexBase > convex;
/// @brief Model type described by the instance
BVHModelType getModelType() const
......@@ -94,66 +91,32 @@ public:
}
/// @brief Constructing an empty BVH
BVHModel() : vertices(NULL),
tri_indices(NULL),
prev_vertices(NULL),
num_tris(0),
num_vertices(0),
build_state(BVH_BUILD_STATE_EMPTY),
bv_splitter(new BVSplitter<BV>(SPLIT_METHOD_MEAN)),
bv_fitter(new BVFitter<BV>()),
num_tris_allocated(0),
num_vertices_allocated(0),
num_bvs_allocated(0),
num_vertex_updated(0),
primitive_indices(NULL),
bvs(NULL),
num_bvs(0)
BVHModelBase() : vertices(NULL),
tri_indices(NULL),
prev_vertices(NULL),
num_tris(0),
num_vertices(0),
build_state(BVH_BUILD_STATE_EMPTY),
num_tris_allocated(0),
num_vertices_allocated(0),
num_vertex_updated(0)
{
}
/// @brief copy from another BVH
BVHModel(const BVHModel& other);
BVHModelBase(const BVHModelBase& other);
/// @brief deconstruction, delete mesh data related.
~BVHModel()
virtual ~BVHModelBase ()
{
delete [] vertices;
delete [] tri_indices;
delete [] bvs;
delete [] prev_vertices;
delete [] primitive_indices;
}
/// @brief We provide getBV() and getNumBVs() because BVH may be compressed (in future), so we must provide some flexibility here
/// @brief Access the bv giving the its index
const BVNode<BV>& getBV(int id) const
{
assert (id < num_bvs);
return bvs[id];
}
/// @brief Access the bv giving the its index
BVNode<BV>& getBV(int id)
{
assert (id < num_bvs);
return bvs[id];
}
/// @brief Get the number of bv in the BVH
int getNumBVs() const
{
return num_bvs;
}
/// @brief Get the object type: it is a BVH
OBJECT_TYPE getObjectType() const { return OT_BVH; }
/// @brief Get the BV type: default is unknown
NODE_TYPE getNodeType() const { return BV_UNKNOWN; }
/// @brief Compute the AABB for the BVH, used for broad-phase collision
void computeLocalAABB();
......@@ -206,16 +169,16 @@ public:
/// @brief End BVH model update, will also refit or rebuild the bounding volume hierarchy
int endUpdateModel(bool refit = true, bool bottomup = true);
/// @brief Check the number of memory used
int memUsage(int msg) const;
/// @brief Build this Convex<Triangle> representation of this model.
/// \note this only takes the points of this model. It does not check that the
/// object is convex. It does not compute a convex hull.
void buildConvexRepresentation(bool share_memory);
virtual int memUsage(int msg) const = 0;
/// @brief This is a special acceleration: BVH_model default stores the BV's transform in world coordinate. However, we can also store each BV's transform related to its parent
/// @brief This is a special acceleration: BVH_model default stores the BV's transform in world coordinate. However, we can also store each BV's transform related to its parent
/// BV node. When traversing the BVH, this can save one matrix transformation.
void makeParentRelative()
{
Matrix3f I (Matrix3f::Identity());
makeParentRelativeRecurse(0, I, Vec3f());
}
virtual void makeParentRelative() = 0;
Vec3f computeCOM() const
{
......@@ -267,14 +230,96 @@ public:
return C.trace() * Matrix3f::Identity() - C;
}
private:
protected:
virtual void deleteBVs() = 0;
virtual bool allocateBVs() = 0;
/// @brief Build the bounding volume hierarchy
virtual int buildTree() = 0;
/// @brief Refit the bounding volume hierarchy
virtual int refitTree(bool bottomup) = 0;
int num_tris_allocated;
int num_vertices_allocated;
int num_bvs_allocated;
int num_vertex_updated; /// for ccd vertex update
unsigned int* primitive_indices;
};
/// @brief A class describing the bounding hierarchy of a mesh model or a point cloud model (which is viewed as a degraded version of mesh)
template<typename BV>
class BVHModel : public BVHModelBase
{
public:
/// @brief Split rule to split one BV node into two children
boost::shared_ptr<BVSplitterBase<BV> > bv_splitter;
/// @brief Fitting rule to fit a BV node to a set of geometry primitives
boost::shared_ptr<BVFitterBase<BV> > bv_fitter;
/// @brief Constructing an empty BVH
BVHModel() : BVHModelBase (),
bv_splitter(new BVSplitter<BV>(SPLIT_METHOD_MEAN)),
bv_fitter(new BVFitter<BV>()),
num_bvs_allocated(0),
primitive_indices(NULL),
bvs(NULL),
num_bvs(0)
{
}
/// @brief copy from another BVH
BVHModel(const BVHModel& other);
/// @brief deconstruction, delete mesh data related.
~BVHModel()
{
delete [] bvs;
delete [] primitive_indices;
}
/// @brief We provide getBV() and getNumBVs() because BVH may be compressed (in future), so we must provide some flexibility here
/// @brief Access the bv giving the its index
const BVNode<BV>& getBV(int id) const
{
assert (id < num_bvs);
return bvs[id];
}
/// @brief Access the bv giving the its index
BVNode<BV>& getBV(int id)
{
assert (id < num_bvs);
return bvs[id];
}
/// @brief Get the number of bv in the BVH
int getNumBVs() const
{
return num_bvs;
}
/// @brief Get the BV type: default is unknown
NODE_TYPE getNodeType() const { return BV_UNKNOWN; }
/// @brief Check the number of memory used
int memUsage(int msg) const;
/// @brief This is a special acceleration: BVH_model default stores the BV's transform in world coordinate. However, we can also store each BV's transform related to its parent
/// BV node. When traversing the BVH, this can save one matrix transformation.
void makeParentRelative()
{
Matrix3f I (Matrix3f::Identity());
makeParentRelativeRecurse(0, I, Vec3f());
}
private:
void deleteBVs();
bool allocateBVs();
int num_bvs_allocated;
unsigned int* primitive_indices;
/// @brief Bounding volume hierarchy
BVNode<BV>* bvs;
......
......@@ -203,7 +203,7 @@ public:
/// @brief compute the AABB in world space
inline void computeAABB()
{
if(isQuatIdentity(t.getQuatRotation()))
if(t.getRotation().isIdentity())
{
aabb = translate(cgeom->aabb_local, t.getTranslation());
}
......@@ -240,8 +240,6 @@ public:
return t.getRotation();
}
/// @brief get object's transform
inline const Transform3f& getTransform() const
{
......
......@@ -55,29 +55,35 @@ typedef boost::int32_t FCL_INT32;
/// @brief Triangle with 3 indices for points
class Triangle
{
/// @brief indices for each vertex of triangle
std::size_t vids[3];
public:
typedef std::size_t index_type;
typedef int size_type;
/// @brief Default constructor
Triangle() {}
/// @brief Create a triangle with given vertex indices
Triangle(std::size_t p1, std::size_t p2, std::size_t p3)
Triangle(index_type p1, index_type p2, index_type p3)
{
set(p1, p2, p3);
}
/// @brief Set the vertex indices of the triangle
inline void set(std::size_t p1, std::size_t p2, std::size_t p3)
inline void set(index_type p1, index_type p2, index_type p3)
{
vids[0] = p1; vids[1] = p2; vids[2] = p3;
}
/// @access the triangle index
inline std::size_t operator[](int i) const { return vids[i]; }
inline index_type operator[](int i) const { return vids[i]; }
inline std::size_t& operator[](int i) { return vids[i]; }
inline index_type& operator[](int i) { return vids[i]; }
static inline size_type size() { return 3; }
private:
/// @brief indices for each vertex of triangle
index_type vids[3];
};
}
......
......@@ -51,6 +51,9 @@ namespace fcl {
class Transform3f;
class AABB;
class BVHModelBase;
typedef boost::shared_ptr<BVHModelBase> BVHModelPtr_t;
}
} // namespace hpp
......
......@@ -69,18 +69,11 @@ inline bool areQuatEquals (const Quaternion3f& q1, const Quaternion3f& q2)
/// @brief Simple transform class used locally by InterpMotion
class Transform3f
{
/// @brief Whether matrix cache is set
mutable bool matrix_set;
/// @brief Matrix cache
mutable Matrix3f R;
Matrix3f R;
/// @brief Tranlation vector
Vec3f T;
/// @brief Rotation
Quaternion3f q;
const Matrix3f& getRotationInternal() const;
public:
/// @brief Default transform is no movement
......@@ -90,49 +83,40 @@ public:
}
/// @brief Construct transform from rotation and translation
Transform3f(const Matrix3f& R_, const Vec3f& T_) : matrix_set(true),
R(R_),
T(T_)
{
q = Quaternion3f(R_);
}
template <typename Matrixx3Like, typename Vector3Like>
Transform3f(const Eigen::MatrixBase<Matrixx3Like>& R_,
const Eigen::MatrixBase<Vector3Like>& T_) :
R(R_),
T(T_)
{}
/// @brief Construct transform from rotation and translation
Transform3f(const Quaternion3f& q_, const Vec3f& T_) : matrix_set(false),
T(T_),
q(q_)
{
}
template <typename Vector3Like>
Transform3f(const Quaternion3f& q_,
const Eigen::MatrixBase<Vector3Like>& T_) :
R(q_.toRotationMatrix()),
T(T_)
{}
/// @brief Construct transform from rotation
Transform3f(const Matrix3f& R_) : matrix_set(true),
R(R_),
Transform3f(const Matrix3f& R_) : R(R_),
T(Vec3f::Zero())
{
q = Quaternion3f(R_);
}
{}
/// @brief Construct transform from rotation
Transform3f(const Quaternion3f& q_) : matrix_set(false),
T(Vec3f::Zero()),
q(q_)
{
}
Transform3f(const Quaternion3f& q_) : R(q_),
T(Vec3f::Zero())
{}
/// @brief Construct transform from translation
Transform3f(const Vec3f& T_) : matrix_set(true),
T(T_),
q(Quaternion3f::Identity())
{
R.setIdentity();
}
Transform3f(const Vec3f& T_) : R(Matrix3f::Identity()),
T(T_)
{}
/// @brief operator =
Transform3f& operator = (const Transform3f& tf)
{
matrix_set = tf.matrix_set;
R = tf.R;
q = tf.q;
T = tf.T;
return *this;
}
......@@ -146,31 +130,28 @@ public:
/// @brief get rotation
inline const Matrix3f& getRotation() const
{
if(matrix_set) return R;
return getRotationInternal();
return R;
}
/// @brief get quaternion
inline const Quaternion3f& getQuatRotation() const
inline Quaternion3f getQuatRotation() const
{
return q;
return Quaternion3f (R);
}
/// @brief set transform from rotation and translation
inline void setTransform(const Matrix3f& R_, const Vec3f& T_)
template <typename Matrixx3Like, typename Vector3Like>
inline void setTransform(const Eigen::MatrixBase<Matrixx3Like>& R_, const Eigen::MatrixBase<Vector3Like>& T_)
{
R.noalias() = R_;
T.noalias() = T_;
q = Quaternion3f(R_);
matrix_set = true;
}
/// @brief set transform from rotation and translation
inline void setTransform(const Quaternion3f& q_, const Vec3f& T_)
{
matrix_set = false;
q = q_;
T.noalias() = T_;
R = q_.toRotationMatrix();
T = T_;
}
/// @brief set transform from rotation
......@@ -178,8 +159,6 @@ public:
inline void setRotation(const Eigen::MatrixBase<Derived>& R_)
{
R.noalias() = R_;
matrix_set = true;
q = Quaternion3f(R);
}
/// @brief set transform from translation
......@@ -192,53 +171,54 @@ public:
/// @brief set transform from rotation
inline void setQuatRotation(const Quaternion3f& q_)
{
matrix_set = false;
q = q_;
R = q_.toRotationMatrix();
}
/// @brief transform a given vector by the transform
template <typename Derived>
inline Vec3f transform(const Eigen::MatrixBase<Derived>& v) const
{
return q * v + T;
return R * v + T;
}
/// @brief inverse transform
inline Transform3f& inverse()
inline Transform3f& inverseInPlace()
{
matrix_set = false;
q = q.conjugate();
T = q * (-T);
R.transposeInPlace();
T = - R * T;
return *this;
}
/// @brief inverse transform
inline Transform3f inverse()
{
return Transform3f (R.transpose(), - R.transpose() * T);
}
/// @brief inverse the transform and multiply with another
inline Transform3f inverseTimes(const Transform3f& other) const
{
const Quaternion3f& q_inv = q.conjugate();
return Transform3f(q_inv * other.q, q_inv * (other.T - T));
return Transform3f(R.transpose() * other.R, R.transpose() * (other.T - T));
}
/// @brief multiply with another transform
inline const Transform3f& operator *= (const Transform3f& other)
{
matrix_set = false;