Commit 928b1756 authored by panjia1983's avatar panjia1983
Browse files

fix some bug in gjk; add continuous collision, still need more test

parent 2e3d0640
......@@ -216,6 +216,7 @@ public:
/// @brief Fitting rule to fit a BV node to a set of geometry primitives
boost::shared_ptr<BVFitterBase<BV> > bv_fitter;
private:
int num_tris_allocated;
......
......@@ -1390,7 +1390,7 @@ void HierarchyTree<BV>::bottomup(size_t* lbeg, size_t* lend)
size_t* lcur_end = lend;
while(lbeg < lcur_end - 1)
{
size_t* min_it1, *min_it2;
size_t* min_it1 = NULL, *min_it2 = NULL;
FCL_REAL min_size = std::numeric_limits<FCL_REAL>::max();
for(size_t* it1 = lbeg; it1 < lcur_end; ++it1)
{
......
......@@ -46,15 +46,22 @@
namespace fcl
{
template<typename BV, typename ConservativeAdvancementNode, typename CollisionNode>
int conservativeAdvancement(const CollisionGeometry* o1,
const MotionBase* motion1,
const CollisionGeometry* o2,
const MotionBase* motion2,
const CollisionRequest& request,
CollisionResult& result,
FCL_REAL& toc);
template<typename NarrowPhaseSolver>
struct ConservativeAdvancementFunctionMatrix
{
typedef FCL_REAL (*ConservativeAdvancementFunc)(const CollisionGeometry* o1, const MotionBase* motion1, const CollisionGeometry* o2, const MotionBase* motion2, const NarrowPhaseSolver* nsolver, const ContinuousCollisionRequest& request, ContinuousCollisionResult& result);
ConservativeAdvancementFunc conservative_advancement_matrix[NODE_COUNT][NODE_COUNT];
ConservativeAdvancementFunctionMatrix();
};
}
#endif
......@@ -46,6 +46,64 @@
namespace fcl
{
class TranslationMotion : public MotionBase
{
public:
/// @brief Construct motion from intial and goal transform
TranslationMotion(const Transform3f& tf1,
const Transform3f& tf2) : MotionBase(),
rot(tf1.getQuatRotation()),
trans_start(tf1.getTranslation()),
trans_range(tf2.getTranslation() - tf1.getTranslation())
{
}
TranslationMotion(const Matrix3f& R, const Vec3f& T1, const Vec3f& T2) : MotionBase()
{
rot.fromRotation(R);
trans_start = T1;
trans_range = T2 - T1;
}
bool integrate(FCL_REAL dt) const
{
if(dt > 1) dt = 1;
tf = Transform3f(rot, trans_start + trans_range * dt);
return true;
}
FCL_REAL computeMotionBound(const BVMotionBoundVisitor& mb_visitor) const
{
return mb_visitor.visit(*this);
}
FCL_REAL computeMotionBound(const TriangleMotionBoundVisitor& mb_visitor) const
{
return mb_visitor.visit(*this);
}
void getCurrentTransform(Transform3f& tf_) const
{
tf_ = tf;
}
void getTaylorModel(TMatrix3& tm, TVector3& tv) const
{
}
Vec3f getVelocity() const
{
return trans_range;
}
private:
/// @brief initial and goal transforms
Quaternion3f rot;
Vec3f trans_start, trans_range;
mutable Transform3f tf;
};
class SplineMotion : public MotionBase
{
public:
......@@ -53,6 +111,19 @@ public:
SplineMotion(const Vec3f& Td0, const Vec3f& Td1, const Vec3f& Td2, const Vec3f& Td3,
const Vec3f& Rd0, const Vec3f& Rd1, const Vec3f& Rd2, const Vec3f& Rd3);
// @brief Construct motion from initial and goal transform
SplineMotion(const Matrix3f& R1, const Vec3f& T1,
const Matrix3f& R2, const Vec3f& T2) : MotionBase()
{
// TODO
}
SplineMotion(const Transform3f& tf1,
const Transform3f& tf2) : MotionBase()
{
// TODO
}
/// @brief Integrate the motion from 0 to dt
/// We compute the current transformation from zero point instead of from last integrate time, for precision.
......@@ -71,22 +142,6 @@ public:
}
/// @brief Get the rotation and translation in current step
void getCurrentTransform(Matrix3f& R, Vec3f& T) const
{
R = tf.getRotation();
T = tf.getTranslation();
}
void getCurrentRotation(Matrix3f& R) const
{
R = tf.getRotation();
}
void getCurrentTranslation(Vec3f& T) const
{
T = tf.getTranslation();
}
void getCurrentTransform(Transform3f& tf_) const
{
tf_ = tf;
......@@ -209,7 +264,7 @@ class ScrewMotion : public MotionBase
{
public:
/// @brief Default transformations are all identities
ScrewMotion()
ScrewMotion() : MotionBase()
{
// Default angular velocity is zero
axis.setValue(1, 0, 0);
......@@ -223,7 +278,8 @@ public:
/// @brief Construct motion from the initial rotation/translation and goal rotation/translation
ScrewMotion(const Matrix3f& R1, const Vec3f& T1,
const Matrix3f& R2, const Vec3f& T2) : tf1(R1, T1),
const Matrix3f& R2, const Vec3f& T2) : MotionBase(),
tf1(R1, T1),
tf2(R2, T2),
tf(tf1)
{
......@@ -267,22 +323,6 @@ public:
/// @brief Get the rotation and translation in current step
void getCurrentTransform(Matrix3f& R, Vec3f& T) const
{
R = tf.getRotation();
T = tf.getTranslation();
}
void getCurrentRotation(Matrix3f& R) const
{
R = tf.getRotation();
}
void getCurrentTranslation(Vec3f& T) const
{
T = tf.getTranslation();
}
void getCurrentTransform(Transform3f& tf_) const
{
tf_ = tf;
......@@ -438,22 +478,6 @@ public:
}
/// @brief Get the rotation and translation in current step
void getCurrentTransform(Matrix3f& R, Vec3f& T) const
{
R = tf.getRotation();
T = tf.getTranslation();
}
void getCurrentRotation(Matrix3f& R) const
{
R = tf.getRotation();
}
void getCurrentTranslation(Vec3f& T) const
{
T = tf.getTranslation();
}
void getCurrentTransform(Transform3f& tf_) const
{
tf_ = tf;
......
......@@ -52,6 +52,7 @@ class MotionBase;
class SplineMotion;
class ScrewMotion;
class InterpMotion;
class TranslationMotion;
/// @brief Compute the motion bound for a bounding volume, given the closest direction n between two query objects
class BVMotionBoundVisitor
......@@ -61,6 +62,7 @@ public:
virtual FCL_REAL visit(const SplineMotion& motion) const = 0;
virtual FCL_REAL visit(const ScrewMotion& motion) const = 0;
virtual FCL_REAL visit(const InterpMotion& motion) const = 0;
virtual FCL_REAL visit(const TranslationMotion& motion) const = 0;
};
template<typename BV>
......@@ -73,6 +75,7 @@ public:
virtual FCL_REAL visit(const SplineMotion& motion) const { return 0; }
virtual FCL_REAL visit(const ScrewMotion& motion) const { return 0; }
virtual FCL_REAL visit(const InterpMotion& motion) const { return 0; }
virtual FCL_REAL visit(const TranslationMotion& motion) const { return 0; }
protected:
BV bv;
......@@ -88,6 +91,8 @@ FCL_REAL TBVMotionBoundVisitor<RSS>::visit(const ScrewMotion& motion) const;
template<>
FCL_REAL TBVMotionBoundVisitor<RSS>::visit(const InterpMotion& motion) const;
template<>
FCL_REAL TBVMotionBoundVisitor<RSS>::visit(const TranslationMotion& motion) const;
class TriangleMotionBoundVisitor
......@@ -100,6 +105,7 @@ public:
virtual FCL_REAL visit(const SplineMotion& motion) const;
virtual FCL_REAL visit(const ScrewMotion& motion) const;
virtual FCL_REAL visit(const InterpMotion& motion) const;
virtual FCL_REAL visit(const TranslationMotion& motion) const;
protected:
Vec3f a, b, c, n;
......@@ -120,17 +126,48 @@ public:
virtual bool integrate(double dt) const = 0;
/** \brief Compute the motion bound for a bounding volume, given the closest direction n between two query objects */
virtual FCL_REAL computeMotionBound(const BVMotionBoundVisitor& mb_visitor) const= 0;
virtual FCL_REAL computeMotionBound(const BVMotionBoundVisitor& mb_visitor) const = 0;
/** \brief Compute the motion bound for a triangle, given the closest direction n between two query objects */
virtual FCL_REAL computeMotionBound(const TriangleMotionBoundVisitor& mb_visitor) const = 0;
/** \brief Get the rotation and translation in current step */
virtual void getCurrentTransform(Matrix3f& R, Vec3f& T) const = 0;
void getCurrentTransform(Matrix3f& R, Vec3f& T) const
{
Transform3f tf;
getCurrentTransform(tf);
R = tf.getRotation();
T = tf.getTranslation();
}
virtual void getCurrentRotation(Matrix3f& R) const = 0;
void getCurrentTransform(Quaternion3f& Q, Vec3f& T) const
{
Transform3f tf;
getCurrentTransform(tf);
Q = tf.getQuatRotation();
T = tf.getTranslation();
}
virtual void getCurrentTranslation(Vec3f& T) const = 0;
void getCurrentRotation(Matrix3f& R) const
{
Transform3f tf;
getCurrentTransform(tf);
R = tf.getRotation();
}
void getCurrentRotation(Quaternion3f& Q) const
{
Transform3f tf;
getCurrentTransform(tf);
Q = tf.getQuatRotation();
}
void getCurrentTranslation(Vec3f& T) const
{
Transform3f tf;
getCurrentTransform(tf);
T = tf.getTranslation();
}
virtual void getCurrentTransform(Transform3f& tf) const = 0;
......
......@@ -50,34 +50,12 @@ namespace fcl
/// performs the collision between them.
/// Return value is the number of contacts generated between the two objects.
template<typename NarrowPhaseSolver>
std::size_t collide(const CollisionObject* o1, const CollisionObject* o2,
const NarrowPhaseSolver* nsolver,
const CollisionRequest& request,
CollisionResult& result);
template<typename NarrowPhaseSolver>
std::size_t collide(const CollisionGeometry* o1, const Transform3f& tf1,
const CollisionGeometry* o2, const Transform3f& tf2,
const NarrowPhaseSolver* nsolver,
const CollisionRequest& request,
CollisionResult& result);
std::size_t collide(const CollisionObject* o1, const CollisionObject* o2,
const CollisionRequest& request,
CollisionResult& result);
std::size_t collide(const CollisionGeometry* o1, const Transform3f& tf1,
const CollisionGeometry* o2, const Transform3f& tf2,
const CollisionRequest& request,
CollisionResult& result);
std::size_t collide(const ContinuousCollisionObject* o1, const ContinuousCollisionObject* o2,
const CollisionRequest& request,
CollisionResult& result);
std::size_t collide(const CollisionGeometry* o1, const MotionBase* motion1,
const CollisionGeometry* o2, const MotionBase* motion2,
const CollisionRequest& request,
CollisionResult& result);
}
......
......@@ -48,6 +48,9 @@
namespace fcl
{
/// @brief Type of narrow phase GJK solver
enum GJKSolverType {GST_LIBCCD, GST_INDEP};
/// @brief Contact information returned by collision
struct Contact
{
......@@ -167,7 +170,7 @@ struct CollisionResult;
/// @brief request to the collision algorithm
struct CollisionRequest
{
{
/// @brief The maximum number of contacts will return
size_t num_max_contacts;
......@@ -183,16 +186,29 @@ struct CollisionRequest
/// @brief whether the cost computation is approximated
bool use_approximate_cost;
/// @brief narrow phase solver
GJKSolverType gjk_solver_type;
/// @brief whether enable gjk intial guess
bool enable_cached_gjk_guess;
/// @brief the gjk intial guess set by user
Vec3f cached_gjk_guess;
CollisionRequest(size_t num_max_contacts_ = 1,
bool enable_contact_ = false,
size_t num_max_cost_sources_ = 1,
bool enable_cost_ = false,
bool use_approximate_cost_ = true) : num_max_contacts(num_max_contacts_),
enable_contact(enable_contact_),
num_max_cost_sources(num_max_cost_sources_),
enable_cost(enable_cost_),
use_approximate_cost(use_approximate_cost_)
bool use_approximate_cost_ = true,
GJKSolverType gjk_solver_type_ = GST_LIBCCD) : num_max_contacts(num_max_contacts_),
enable_contact(enable_contact_),
num_max_cost_sources(num_max_cost_sources_),
enable_cost(enable_cost_),
use_approximate_cost(use_approximate_cost_),
gjk_solver_type(gjk_solver_type_)
{
enable_cached_gjk_guess = false;
cached_gjk_guess = Vec3f(1, 0, 0);
}
bool isSatisfied(const CollisionResult& result) const;
......@@ -208,6 +224,9 @@ private:
/// @brief cost sources
std::set<CostSource> cost_sources;
public:
Vec3f cached_gjk_guess;
public:
CollisionResult()
{
......@@ -277,6 +296,7 @@ public:
}
};
struct DistanceResult;
/// @brief request to the distance computation
......@@ -285,13 +305,29 @@ struct DistanceRequest
/// @brief whether to return the nearest points
bool enable_nearest_points;
DistanceRequest(bool enable_nearest_points_ = false) : enable_nearest_points(enable_nearest_points_)
/// @brief error threshold for approximate distance
FCL_REAL rel_err; // relative error, between 0 and 1
FCL_REAL abs_err; // absoluate error
/// @brief narrow phase solver type
GJKSolverType gjk_solver_type;
DistanceRequest(bool enable_nearest_points_ = false,
FCL_REAL rel_err_ = 0.0,
FCL_REAL abs_err_ = 0.0,
GJKSolverType gjk_solver_type_ = GST_LIBCCD) : enable_nearest_points(enable_nearest_points_),
rel_err(rel_err_),
abs_err(abs_err_),
gjk_solver_type(gjk_solver_type_)
{
}
bool isSatisfied(const DistanceResult& result) const;
};
/// @brief distance result
struct DistanceResult
{
......@@ -389,6 +425,94 @@ public:
};
enum CCDMotionType {CCDM_TRANS, CCDM_LINEAR, CCDM_SCREW, CCDM_SPLINE};
enum CCDSolverType {CCDC_NAIVE, CCDC_CONSERVATIVE_ADVANCEMENT, CCDC_RAY_SHOOTING, CCDC_POLYNOMIAL_SOLVER};
struct ContinuousCollisionRequest
{
/// @brief maximum num of iterations
std::size_t num_max_iterations;
/// @brief error in first contact time
FCL_REAL toc_err;
/// @brief ccd motion type
CCDMotionType ccd_motion_type;
/// @brief gjk solver type
GJKSolverType gjk_solver_type;
/// @brief ccd solver type
CCDSolverType ccd_solver_type;
ContinuousCollisionRequest(std::size_t num_max_iterations_ = 10,
FCL_REAL toc_err_ = 0,
CCDMotionType ccd_motion_type_ = CCDM_TRANS,
GJKSolverType gjk_solver_type_ = GST_LIBCCD,
CCDSolverType ccd_solver_type_ = CCDC_NAIVE) : num_max_iterations(num_max_iterations_),
toc_err(toc_err_),
ccd_motion_type(ccd_motion_type_),
gjk_solver_type(gjk_solver_type_),
ccd_solver_type(ccd_solver_type_)
{
}
};
/// @brief continuous collision result
struct ContinuousCollisionResult
{
/// @brief collision or not
bool is_collide;
/// @brief time of contact in [0, 1]
FCL_REAL time_of_contact;
ContinuousCollisionResult() : is_collide(false), time_of_contact(1.0)
{
}
};
enum PenetrationDepthType {PDT_LOCAL, PDT_AL};
struct PenetrationDepthMetricBase
{
virtual FCL_REAL operator() (const Transform3f& tf1, const Transform3f& tf2) const = 0;
};
struct WeightEuclideanPDMetric : public PenetrationDepthMetricBase
{
};
struct PenetrationDepthRequest
{
/// @brief PD algorithm type
PenetrationDepthType pd_type;
/// @brief gjk solver type
GJKSolverType gjk_solver_type;
PenetrationDepthRequest(PenetrationDepthType pd_type_ = PDT_LOCAL,
GJKSolverType gjk_solver_type_ = GST_LIBCCD) : pd_type(pd_type_),
gjk_solver_type(gjk_solver_type_)
{
}
};
struct PenetrationDepthResult
{
/// @brief penetration depth value
FCL_REAL pd_value;
/// @brief the transform where the collision is resolved
Transform3f resolve_trans;
};
}
......
......@@ -115,6 +115,7 @@ public:
/// @brief threshold for free (<= is free)
FCL_REAL threshold_free;
};
/// @brief the object for collision or distance computation, contains the geometry and the transform information
......
#ifndef FCL_CONTINUOUS_COLLISION_H
#define FCL_CONTINUOUS_COLLISION_H
#include "fcl/math/vec_3f.h"
#include "fcl/collision_object.h"
#include "fcl/collision_data.h"
namespace fcl
{
/// @brief continous collision checking between two objects
FCL_REAL continuous_collide(const CollisionGeometry* o1, const Transform3f& tf1_beg, const Transform3f& tf1_end,
const CollisionGeometry* o2, const Transform3f& tf2_beg, const Transform3f& tf2_end,
const ContinuousCollisionRequest& request,
ContinuousCollisionResult& result);
FCL_REAL continuous_collide(const CollisionObject* o1, const Transform3f& tf1_end,
const CollisionObject* o2, const Transform3f& tf2_end,
const ContinuousCollisionRequest& request,
ContinuousCollisionResult& result);
FCL_REAL collide(const ContinuousCollisionObject* o1, const ContinuousCollisionObject* o2,
const ContinuousCollisionRequest& request,
ContinuousCollisionResult& result);
}
#endif
......@@ -46,16 +46,6 @@ namespace fcl
/// @brief Main distance interface: given two collision objects, and the requirements for contacts, including whether return the nearest points, this function performs the distance between them.
/// Return value is the minimum distance generated between the two objects.
template<typename NarrowPhaseSolver>
FCL_REAL distance(const CollisionObject* o1, const CollisionObject* o2, const NarrowPhaseSolver* nsolver,
const DistanceRequest& request, DistanceResult& result);
template<typename NarrowPhaseSolver>
FCL_REAL distance(const CollisionGeometry* o1, const Transform3f& tf1,
const CollisionGeometry* o2, const Transform3f& tf2,
const NarrowPhaseSolver* nsolver,
const DistanceRequest& request, DistanceResult& result);
FCL_REAL distance(const CollisionObject* o1, const CollisionObject* o2,
const DistanceRequest& request, DistanceResult& result);
......
......@@ -91,13 +91,13 @@ public:
/// @brief CCD intersect between one vertex and one face, using additional filter
static bool intersect_VF_filtered(const Vec3f& a0, const Vec3f& b0, const Vec3f& c0, const Vec3f& p0,
const Vec3f& a1, const Vec3f& b1, const Vec3f& c1, const Vec3f& p1,
FCL_REAL* collision_time, Vec3f* p_i, bool useNewton = true);
const Vec3f& a1, const Vec3f& b1, const Vec3f& c1, const Vec3f& p1,
FCL_REAL* collision_time, Vec3f* p_i, bool useNewton = true);
/// @brief CCD intersect between two edges, using additional filter
static bool intersect_EE_filtered(const Vec3f& a0, const Vec3f& b0, const Vec3f& c0, const Vec3f& d0,
const Vec3f& a1, const Vec3f& b1, const Vec3f& c1, const Vec3f& d1,
FCL_REAL* collision_time, Vec3f* p_i, bool useNewton = true);
const Vec3f& a1, const Vec3f& b1, const Vec3f& c1, const Vec3f& d1,
FCL_REAL* collision_time, Vec3f* p_i, bool useNewton = true);