Unverified Commit 58c08c80 authored by Joseph Mirabel's avatar Joseph Mirabel Committed by GitHub
Browse files

Merge pull request #182 from jmirabel/devel

Fix calculation of normal and nearest points in GJKSolver::shapeDistance
parents cdd0b106 68e14b69
......@@ -234,7 +234,8 @@ namespace fcl
Vec3f w0, w1;
gjk.getClosestPoints (shape, w0, w1);
distance = 0;
p1 = p2 = tf1.transform (.5* (w0 + w1));
p1 = tf1.transform (w0);
p2 = tf1.transform (w1);
normal = Vec3f (0,0,0);
return false;
}
......@@ -261,7 +262,8 @@ namespace fcl
//p1 = tf1.transform (p1);
//p2 = tf1.transform (p2);
normal = (tf1.getRotation() * (p2 - p1)).normalized();
p1 = p2 = tf1.transform(p1);
p1 = tf1.transform(p1);
p2 = tf1.transform(p2);
} else {
details::EPA epa(epa_max_face_num, epa_max_vertex_num,
epa_max_iterations, epa_tolerance);
......@@ -275,15 +277,15 @@ namespace fcl
epa.getClosestPoints (shape, w0, w1);
assert (epa.depth >= -eps);
distance = (std::min) (0., -epa.depth);
// TODO should be
// normal = tf1.getRotation() * epa.normal;
normal = tf2.getRotation() * epa.normal;
p1 = p2 = tf1.transform(w0 - epa.normal*(epa.depth *0.5));
normal = tf1.getRotation() * epa.normal;
p1 = tf1.transform(w0);
p2 = tf1.transform(w1);
return false;
}
distance = -(std::numeric_limits<FCL_REAL>::max)();
gjk.getClosestPoints (shape, p1, p2);
p1 = p2 = tf1.transform (p1);
p1 = tf1.transform(p1);
p2 = tf1.transform(p2);
}
return false;
}
......
......@@ -42,7 +42,11 @@ SET(${LIBRARY_NAME}_HEADERS
fcl.hh
)
IF(NOT DOXYGEN_FOUND OR (APPLE AND PYTHON_VERSION_MAJOR LESS 3))
SET(ENABLE_PYTHON_DOXYGEN_AUTODOC TRUE CACHE BOOL "Enable automatic documentation of Python bindings from Doxygen documentation")
IF( NOT ENABLE_PYTHON_DOXYGEN_AUTODOC
OR NOT DOXYGEN_FOUND
OR (APPLE AND PYTHON_VERSION_MAJOR LESS 3)) # Disable documentation on APPLE for python 2.
SET(ENABLE_DOXYGEN_AUTODOC FALSE)
ELSE()
SET(ENABLE_DOXYGEN_AUTODOC TRUE)
......
......@@ -34,8 +34,7 @@
#include <boost/python.hpp>
#include <eigenpy/registration.hpp>
#include <eigenpy/eigen-to-python.hpp>
#include <eigenpy/eigenpy.hpp>
#include "fcl.hh"
......
......@@ -35,8 +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 <eigenpy/eigenpy.hpp>
#include <hpp/fcl/fwd.hh>
#include <hpp/fcl/collision.h>
......
......@@ -35,8 +35,7 @@
#include <boost/python.hpp>
#include <boost/python/suite/indexing/vector_indexing_suite.hpp>
#include <eigenpy/registration.hpp>
#include <eigenpy/eigen-to-python.hpp>
#include <eigenpy/eigenpy.hpp>
#include "fcl.hh"
......
......@@ -34,7 +34,7 @@
#include <boost/python.hpp>
#include <eigenpy/registration.hpp>
#include <eigenpy/eigenpy.hpp>
#include "fcl.hh"
......
......@@ -35,7 +35,7 @@
#include <boost/python.hpp>
#include <boost/python/suite/indexing/vector_indexing_suite.hpp>
#include <eigenpy/registration.hpp>
#include <eigenpy/eigenpy.hpp>
#include "fcl.hh"
......
......@@ -85,10 +85,10 @@ std::size_t ShapeShapeCollide(const CollisionGeometry* o1, const Transform3f& tf
if (distance <= 0) {
if (result.numContacts () < request.num_max_contacts) {
const Vec3f& p1 = distanceResult.nearest_points [0];
assert (!request.enable_contact || p1 == distanceResult.nearest_points [1]);
const Vec3f& p2 = distanceResult.nearest_points [1];
Contact contact (o1, o2, distanceResult.b1, distanceResult.b2,
p1,
(p1+p2)/2,
distanceResult.normal,
-distance+request.security_margin);
......
Markdown is supported
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