diff --git a/trunk/fcl/include/fcl/data_types.h b/trunk/fcl/include/fcl/data_types.h
index f7818774b6a56d69cb51add6348141b1088f85ba..32a71920f09ec0247210d21550f0847982798b7d 100644
--- a/trunk/fcl/include/fcl/data_types.h
+++ b/trunk/fcl/include/fcl/data_types.h
@@ -38,7 +38,7 @@
#define FCL_DATA_TYPES_H
#include <cstddef>
-#include <inttypes.h>
+#include <cstdint>
namespace fcl
{
diff --git a/trunk/fcl/include/fcl/intersect.h b/trunk/fcl/include/fcl/intersect.h
index 13d282f85418c1c6d4e2dce8717047d0fae2f215..c1f3f2fa8b590689033ca0ecfe8c817c0e048494 100644
--- a/trunk/fcl/include/fcl/intersect.h
+++ b/trunk/fcl/include/fcl/intersect.h
@@ -1,291 +1,292 @@
-/*
- * Software License Agreement (BSD License)
- *
- * Copyright (c) 2011, Willow Garage, Inc.
- * All rights reserved.
- *
- * Redistribution and use in source and binary forms, with or without
- * modification, are permitted provided that the following conditions
- * are met:
- *
- * * Redistributions of source code must retain the above copyright
- * notice, this list of conditions and the following disclaimer.
- * * Redistributions in binary form must reproduce the above
- * copyright notice, this list of conditions and the following
- * disclaimer in the documentation and/or other materials provided
- * with the distribution.
- * * Neither the name of Willow Garage, Inc. nor the names of its
- * contributors may be used to endorse or promote products derived
- * from this software without specific prior written permission.
- *
- * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
- * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
- * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
- * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
- * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
- * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
- * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
- * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
- * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
- * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
- * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
- * POSSIBILITY OF SUCH DAMAGE.
- */
-
-/** \author Jia Pan */
-
-#ifndef FCL_INTERSECT_H
-#define FCL_INTERSECT_H
-
-#include "fcl/math/transform.h"
-
-namespace fcl
-{
-
-/// @brief A class solves polynomial degree (1,2,3) equations
-class PolySolver
-{
-public:
- /// @brief Solve a linear equation with coefficients c, return roots s and number of roots
- static int solveLinear(FCL_REAL c[2], FCL_REAL s[1]);
-
- /// @brief Solve a quadratic function with coefficients c, return roots s and number of roots
- static int solveQuadric(FCL_REAL c[3], FCL_REAL s[2]);
-
- /// @brief Solve a cubic function with coefficients c, return roots s and number of roots
- static int solveCubic(FCL_REAL c[4], FCL_REAL s[3]);
-
-private:
- /// @brief Check whether v is zero
- static inline bool isZero(FCL_REAL v);
-
- /// @brief Compute v^{1/3}
- static inline bool cbrt(FCL_REAL v);
-
- static const FCL_REAL NEAR_ZERO_THRESHOLD;
-};
-
-
-/// @brief CCD intersect kernel among primitives
-class Intersect
-{
-
-public:
-
- /// @brief CCD intersect between one vertex and one face
- /// [a0, b0, c0] and [a1, b1, c1] are points for the triangle face in time t0 and t1
- /// p0 and p1 are points for vertex in time t0 and t1
- /// p_i returns the coordinate of the collision point
- static bool intersect_VF(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);
-
- /// @brief CCD intersect between two edges
- /// [a0, b0] and [a1, b1] are points for one edge in time t0 and t1
- /// [c0, d0] and [c1, d1] are points for the other edge in time t0 and t1
- /// p_i returns the coordinate of the collision point
- static bool intersect_EE(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);
-
- /// @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);
-
- /// @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);
-
- /// @brief CCD intersect between one vertex and and one edge
- static bool intersect_VE(const Vec3f& a0, const Vec3f& b0, const Vec3f& p0,
- const Vec3f& a1, const Vec3f& b1, const Vec3f& p1,
- const Vec3f& L);
-
- /// @brief CD intersect between two triangles [P1, P2, P3] and [Q1, Q2, Q3]
- static bool intersect_Triangle(const Vec3f& P1, const Vec3f& P2, const Vec3f& P3,
- const Vec3f& Q1, const Vec3f& Q2, const Vec3f& Q3,
- Vec3f* contact_points = NULL,
- unsigned int* num_contact_points = NULL,
- FCL_REAL* penetration_depth = NULL,
- Vec3f* normal = NULL);
-
- static bool intersect_Triangle(const Vec3f& P1, const Vec3f& P2, const Vec3f& P3,
- const Vec3f& Q1, const Vec3f& Q2, const Vec3f& Q3,
- const Matrix3f& R, const Vec3f& T,
- Vec3f* contact_points = NULL,
- unsigned int* num_contact_points = NULL,
- FCL_REAL* penetration_depth = NULL,
- Vec3f* normal = NULL);
-
- static bool intersect_Triangle(const Vec3f& P1, const Vec3f& P2, const Vec3f& P3,
- const Vec3f& Q1, const Vec3f& Q2, const Vec3f& Q3,
- const Transform3f& tf,
- Vec3f* contact_points = NULL,
- unsigned int* num_contact_points = NULL,
- FCL_REAL* penetration_depth = NULL,
- Vec3f* normal = NULL);
-
-private:
-
- /// @brief Project function used in intersect_Triangle()
- static int project6(const Vec3f& ax,
- const Vec3f& p1, const Vec3f& p2, const Vec3f& p3,
- const Vec3f& q1, const Vec3f& q2, const Vec3f& q3);
-
- /// @brief Check whether one value is zero
- static inline bool isZero(FCL_REAL v);
-
- /// @brief Solve the cubic function using Newton method, also satisfies the interval restriction
- static bool solveCubicWithIntervalNewton(const Vec3f& a0, const Vec3f& b0, const Vec3f& c0, const Vec3f& d0,
- const Vec3f& va, const Vec3f& vb, const Vec3f& vc, const Vec3f& vd,
- FCL_REAL& l, FCL_REAL& r, bool bVF, FCL_REAL coeffs[], Vec3f* data = NULL);
-
- /// @brief Check whether one point p is within triangle [a, b, c]
- static bool insideTriangle(const Vec3f& a, const Vec3f& b, const Vec3f& c, const Vec3f&p);
-
- /// @brief Check whether one point p is within a line segment [a, b]
- static bool insideLineSegment(const Vec3f& a, const Vec3f& b, const Vec3f& p);
-
- /// @brief Calculate the line segment papb that is the shortest route between
- /// two lines p1p2 and p3p4. Calculate also the values of mua and mub where
- /// pa = p1 + mua (p2 - p1)
- /// pb = p3 + mub (p4 - p3)
- /// return FALSE if no solution exists.
- static bool linelineIntersect(const Vec3f& p1, const Vec3f& p2, const Vec3f& p3, const Vec3f& p4,
- Vec3f* pa, Vec3f* pb, FCL_REAL* mua, FCL_REAL* mub);
-
- /// @brief Check whether a root for VF intersection is valid (i.e. within the triangle at intersection t
- static bool checkRootValidity_VF(const Vec3f& a0, const Vec3f& b0, const Vec3f& c0, const Vec3f& p0,
- const Vec3f& va, const Vec3f& vb, const Vec3f& vc, const Vec3f& vp,
- FCL_REAL t);
-
- /// @brief Check whether a root for EE intersection is valid (i.e. within the two edges intersected at the given time
- static bool checkRootValidity_EE(const Vec3f& a0, const Vec3f& b0, const Vec3f& c0, const Vec3f& d0,
- const Vec3f& va, const Vec3f& vb, const Vec3f& vc, const Vec3f& vd,
- FCL_REAL t, Vec3f* q_i = NULL);
-
- /// @brief Check whether a root for VE intersection is valid
- static bool checkRootValidity_VE(const Vec3f& a0, const Vec3f& b0, const Vec3f& p0,
- const Vec3f& va, const Vec3f& vb, const Vec3f& vp,
- FCL_REAL t);
-
- /// @brief Solve a square function for EE intersection (with interval restriction)
- static bool solveSquare(FCL_REAL a, FCL_REAL b, FCL_REAL c,
- const Vec3f& a0, const Vec3f& b0, const Vec3f& c0, const Vec3f& d0,
- const Vec3f& va, const Vec3f& vb, const Vec3f& vc, const Vec3f& vd,
- bool bVF,
- FCL_REAL* ret);
-
- /// @brief Solve a square function for VE intersection (with interval restriction)
- static bool solveSquare(FCL_REAL a, FCL_REAL b, FCL_REAL c,
- const Vec3f& a0, const Vec3f& b0, const Vec3f& p0,
- const Vec3f& va, const Vec3f& vb, const Vec3f& vp);
-
- /// @brief Compute the cubic coefficients for VF intersection
- /// See Paper "Interactive Continuous Collision Detection between Deformable Models using Connectivity-Based Culling", Equation 1.
-
- static void computeCubicCoeff_VF(const Vec3f& a0, const Vec3f& b0, const Vec3f& c0, const Vec3f& p0,
- const Vec3f& va, const Vec3f& vb, const Vec3f& vc, const Vec3f& vp,
- FCL_REAL* a, FCL_REAL* b, FCL_REAL* c, FCL_REAL* d);
-
- /// @brief Compute the cubic coefficients for EE intersection
- static void computeCubicCoeff_EE(const Vec3f& a0, const Vec3f& b0, const Vec3f& c0, const Vec3f& d0,
- const Vec3f& va, const Vec3f& vb, const Vec3f& vc, const Vec3f& vd,
- FCL_REAL* a, FCL_REAL* b, FCL_REAL* c, FCL_REAL* d);
-
- /// @brief Compute the cubic coefficients for VE intersection
- static void computeCubicCoeff_VE(const Vec3f& a0, const Vec3f& b0, const Vec3f& p0,
- const Vec3f& va, const Vec3f& vb, const Vec3f& vp,
- const Vec3f& L,
- FCL_REAL* a, FCL_REAL* b, FCL_REAL* c);
-
- /// @brief filter for intersection, works for both VF and EE
- static bool intersectPreFiltering(const Vec3f& a0, const Vec3f& b0, const Vec3f& c0, const Vec3f& d0,
- const Vec3f& a1, const Vec3f& b1, const Vec3f& c1, const Vec3f& d1);
-
- /// @brief distance of point v to a plane n * x - t = 0
- static FCL_REAL distanceToPlane(const Vec3f& n, FCL_REAL t, const Vec3f& v);
-
- /// @brief check wether points v1, v2, v2 are on the same side of plane n * x - t = 0
- static bool sameSideOfPlane(const Vec3f& v1, const Vec3f& v2, const Vec3f& v3, const Vec3f& n, FCL_REAL t);
-
- /// @brief clip triangle v1, v2, v3 by the prism made by t1, t2 and t3. The normal of the prism is tn and is cutted up by to
- static void clipTriangleByTriangleAndEdgePlanes(const Vec3f& v1, const Vec3f& v2, const Vec3f& v3,
- const Vec3f& t1, const Vec3f& t2, const Vec3f& t3,
- const Vec3f& tn, FCL_REAL to,
- Vec3f clipped_points[], unsigned int* num_clipped_points, bool clip_triangle = false);
-
- /// @brief build a plane passed through triangle v1 v2 v3
- static bool buildTrianglePlane(const Vec3f& v1, const Vec3f& v2, const Vec3f& v3, Vec3f* n, FCL_REAL* t);
-
- /// @brief build a plane pass through edge v1 and v2, normal is tn
- static bool buildEdgePlane(const Vec3f& v1, const Vec3f& v2, const Vec3f& tn, Vec3f* n, FCL_REAL* t);
-
- /// @brief compute the points which has deepest penetration depth
- static void computeDeepestPoints(Vec3f* clipped_points, unsigned int num_clipped_points, const Vec3f& n, FCL_REAL t, FCL_REAL* penetration_depth, Vec3f* deepest_points, unsigned int* num_deepest_points);
-
- /// @brief clip polygon by plane
- static void clipPolygonByPlane(Vec3f* polygon_points, unsigned int num_polygon_points, const Vec3f& n, FCL_REAL t, Vec3f clipped_points[], unsigned int* num_clipped_points);
-
- /// @brief clip a line segment by plane
- static void clipSegmentByPlane(const Vec3f& v1, const Vec3f& v2, const Vec3f& n, FCL_REAL t, Vec3f* clipped_point);
-
- /// @brief compute the cdf(x)
- static FCL_REAL gaussianCDF(FCL_REAL x)
- {
- return 0.5 * erfc(-x / sqrt(2.0));
- }
-
-
- static const FCL_REAL EPSILON;
- static const FCL_REAL NEAR_ZERO_THRESHOLD;
- static const FCL_REAL CCD_RESOLUTION;
- static const unsigned int MAX_TRIANGLE_CLIPS = 8;
-};
-
-class TriangleDistance
-{
-public:
-
- /// @brief Returns closest points between an segment pair.
- /// The first segment is P + t * A
- /// The second segment is Q + t * B
- /// X, Y are the closest points on the two segments
- /// VEC is the vector between X and Y
- static void segPoints(const Vec3f& P, const Vec3f& A, const Vec3f& Q, const Vec3f& B,
- Vec3f& VEC, Vec3f& X, Vec3f& Y);
-
- /// @brief Compute the closest points on two triangles given their absolute coordinate, and returns the distance between them
- /// S and T are two triangles
- /// If the triangles are disjoint, P and Q give the closet points of S and T respectively. However,
- /// if the triangles overlap, P and Q are basically a random pair of points from the triangles, not
- /// coincident points on the intersection of the triangles, as might be expected.
- static FCL_REAL triDistance(const Vec3f S[3], const Vec3f T[3], Vec3f& P, Vec3f& Q);
-
- static FCL_REAL triDistance(const Vec3f& S1, const Vec3f& S2, const Vec3f& S3,
- const Vec3f& T1, const Vec3f& T2, const Vec3f& T3,
- Vec3f& P, Vec3f& Q);
-
- /// @brief Compute the closest points on two triangles given the relative transform between them, and returns the distance between them
- /// S and T are two triangles
- /// If the triangles are disjoint, P and Q give the closet points of S and T respectively. However,
- /// if the triangles overlap, P and Q are basically a random pair of points from the triangles, not
- /// coincident points on the intersection of the triangles, as might be expected.
- /// The returned P and Q are both in the coordinate of the first triangle's coordinate
- static FCL_REAL triDistance(const Vec3f S[3], const Vec3f T[3],
- const Matrix3f& R, const Vec3f& Tl,
- Vec3f& P, Vec3f& Q);
-
- static FCL_REAL triDistance(const Vec3f& S1, const Vec3f& S2, const Vec3f& S3,
- const Vec3f& T1, const Vec3f& T2, const Vec3f& T3,
- const Matrix3f& R, const Vec3f& Tl,
- Vec3f& P, Vec3f& Q);
-};
-
-
-}
-
-
-#endif
+/*
+ * Software License Agreement (BSD License)
+ *
+ * Copyright (c) 2011, Willow Garage, Inc.
+ * All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * * Redistributions of source code must retain the above copyright
+ * notice, this list of conditions and the following disclaimer.
+ * * Redistributions in binary form must reproduce the above
+ * copyright notice, this list of conditions and the following
+ * disclaimer in the documentation and/or other materials provided
+ * with the distribution.
+ * * Neither the name of Willow Garage, Inc. nor the names of its
+ * contributors may be used to endorse or promote products derived
+ * from this software without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+ * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ */
+
+/** \author Jia Pan */
+
+#ifndef FCL_INTERSECT_H
+#define FCL_INTERSECT_H
+
+#include "fcl/math/transform.h"
+#include <boost\math\special_functions\erf.hpp>
+
+namespace fcl
+{
+
+/// @brief A class solves polynomial degree (1,2,3) equations
+class PolySolver
+{
+public:
+ /// @brief Solve a linear equation with coefficients c, return roots s and number of roots
+ static int solveLinear(FCL_REAL c[2], FCL_REAL s[1]);
+
+ /// @brief Solve a quadratic function with coefficients c, return roots s and number of roots
+ static int solveQuadric(FCL_REAL c[3], FCL_REAL s[2]);
+
+ /// @brief Solve a cubic function with coefficients c, return roots s and number of roots
+ static int solveCubic(FCL_REAL c[4], FCL_REAL s[3]);
+
+private:
+ /// @brief Check whether v is zero
+ static inline bool isZero(FCL_REAL v);
+
+ /// @brief Compute v^{1/3}
+ static inline bool cbrt(FCL_REAL v);
+
+ static const FCL_REAL NEAR_ZERO_THRESHOLD;
+};
+
+
+/// @brief CCD intersect kernel among primitives
+class Intersect
+{
+
+public:
+
+ /// @brief CCD intersect between one vertex and one face
+ /// [a0, b0, c0] and [a1, b1, c1] are points for the triangle face in time t0 and t1
+ /// p0 and p1 are points for vertex in time t0 and t1
+ /// p_i returns the coordinate of the collision point
+ static bool intersect_VF(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);
+
+ /// @brief CCD intersect between two edges
+ /// [a0, b0] and [a1, b1] are points for one edge in time t0 and t1
+ /// [c0, d0] and [c1, d1] are points for the other edge in time t0 and t1
+ /// p_i returns the coordinate of the collision point
+ static bool intersect_EE(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);
+
+ /// @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);
+
+ /// @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);
+
+ /// @brief CCD intersect between one vertex and and one edge
+ static bool intersect_VE(const Vec3f& a0, const Vec3f& b0, const Vec3f& p0,
+ const Vec3f& a1, const Vec3f& b1, const Vec3f& p1,
+ const Vec3f& L);
+
+ /// @brief CD intersect between two triangles [P1, P2, P3] and [Q1, Q2, Q3]
+ static bool intersect_Triangle(const Vec3f& P1, const Vec3f& P2, const Vec3f& P3,
+ const Vec3f& Q1, const Vec3f& Q2, const Vec3f& Q3,
+ Vec3f* contact_points = NULL,
+ unsigned int* num_contact_points = NULL,
+ FCL_REAL* penetration_depth = NULL,
+ Vec3f* normal = NULL);
+
+ static bool intersect_Triangle(const Vec3f& P1, const Vec3f& P2, const Vec3f& P3,
+ const Vec3f& Q1, const Vec3f& Q2, const Vec3f& Q3,
+ const Matrix3f& R, const Vec3f& T,
+ Vec3f* contact_points = NULL,
+ unsigned int* num_contact_points = NULL,
+ FCL_REAL* penetration_depth = NULL,
+ Vec3f* normal = NULL);
+
+ static bool intersect_Triangle(const Vec3f& P1, const Vec3f& P2, const Vec3f& P3,
+ const Vec3f& Q1, const Vec3f& Q2, const Vec3f& Q3,
+ const Transform3f& tf,
+ Vec3f* contact_points = NULL,
+ unsigned int* num_contact_points = NULL,
+ FCL_REAL* penetration_depth = NULL,
+ Vec3f* normal = NULL);
+
+private:
+
+ /// @brief Project function used in intersect_Triangle()
+ static int project6(const Vec3f& ax,
+ const Vec3f& p1, const Vec3f& p2, const Vec3f& p3,
+ const Vec3f& q1, const Vec3f& q2, const Vec3f& q3);
+
+ /// @brief Check whether one value is zero
+ static inline bool isZero(FCL_REAL v);
+
+ /// @brief Solve the cubic function using Newton method, also satisfies the interval restriction
+ static bool solveCubicWithIntervalNewton(const Vec3f& a0, const Vec3f& b0, const Vec3f& c0, const Vec3f& d0,
+ const Vec3f& va, const Vec3f& vb, const Vec3f& vc, const Vec3f& vd,
+ FCL_REAL& l, FCL_REAL& r, bool bVF, FCL_REAL coeffs[], Vec3f* data = NULL);
+
+ /// @brief Check whether one point p is within triangle [a, b, c]
+ static bool insideTriangle(const Vec3f& a, const Vec3f& b, const Vec3f& c, const Vec3f&p);
+
+ /// @brief Check whether one point p is within a line segment [a, b]
+ static bool insideLineSegment(const Vec3f& a, const Vec3f& b, const Vec3f& p);
+
+ /// @brief Calculate the line segment papb that is the shortest route between
+ /// two lines p1p2 and p3p4. Calculate also the values of mua and mub where
+ /// pa = p1 + mua (p2 - p1)
+ /// pb = p3 + mub (p4 - p3)
+ /// return FALSE if no solution exists.
+ static bool linelineIntersect(const Vec3f& p1, const Vec3f& p2, const Vec3f& p3, const Vec3f& p4,
+ Vec3f* pa, Vec3f* pb, FCL_REAL* mua, FCL_REAL* mub);
+
+ /// @brief Check whether a root for VF intersection is valid (i.e. within the triangle at intersection t
+ static bool checkRootValidity_VF(const Vec3f& a0, const Vec3f& b0, const Vec3f& c0, const Vec3f& p0,
+ const Vec3f& va, const Vec3f& vb, const Vec3f& vc, const Vec3f& vp,
+ FCL_REAL t);
+
+ /// @brief Check whether a root for EE intersection is valid (i.e. within the two edges intersected at the given time
+ static bool checkRootValidity_EE(const Vec3f& a0, const Vec3f& b0, const Vec3f& c0, const Vec3f& d0,
+ const Vec3f& va, const Vec3f& vb, const Vec3f& vc, const Vec3f& vd,
+ FCL_REAL t, Vec3f* q_i = NULL);
+
+ /// @brief Check whether a root for VE intersection is valid
+ static bool checkRootValidity_VE(const Vec3f& a0, const Vec3f& b0, const Vec3f& p0,
+ const Vec3f& va, const Vec3f& vb, const Vec3f& vp,
+ FCL_REAL t);
+
+ /// @brief Solve a square function for EE intersection (with interval restriction)
+ static bool solveSquare(FCL_REAL a, FCL_REAL b, FCL_REAL c,
+ const Vec3f& a0, const Vec3f& b0, const Vec3f& c0, const Vec3f& d0,
+ const Vec3f& va, const Vec3f& vb, const Vec3f& vc, const Vec3f& vd,
+ bool bVF,
+ FCL_REAL* ret);
+
+ /// @brief Solve a square function for VE intersection (with interval restriction)
+ static bool solveSquare(FCL_REAL a, FCL_REAL b, FCL_REAL c,
+ const Vec3f& a0, const Vec3f& b0, const Vec3f& p0,
+ const Vec3f& va, const Vec3f& vb, const Vec3f& vp);
+
+ /// @brief Compute the cubic coefficients for VF intersection
+ /// See Paper "Interactive Continuous Collision Detection between Deformable Models using Connectivity-Based Culling", Equation 1.
+
+ static void computeCubicCoeff_VF(const Vec3f& a0, const Vec3f& b0, const Vec3f& c0, const Vec3f& p0,
+ const Vec3f& va, const Vec3f& vb, const Vec3f& vc, const Vec3f& vp,
+ FCL_REAL* a, FCL_REAL* b, FCL_REAL* c, FCL_REAL* d);
+
+ /// @brief Compute the cubic coefficients for EE intersection
+ static void computeCubicCoeff_EE(const Vec3f& a0, const Vec3f& b0, const Vec3f& c0, const Vec3f& d0,
+ const Vec3f& va, const Vec3f& vb, const Vec3f& vc, const Vec3f& vd,
+ FCL_REAL* a, FCL_REAL* b, FCL_REAL* c, FCL_REAL* d);
+
+ /// @brief Compute the cubic coefficients for VE intersection
+ static void computeCubicCoeff_VE(const Vec3f& a0, const Vec3f& b0, const Vec3f& p0,
+ const Vec3f& va, const Vec3f& vb, const Vec3f& vp,
+ const Vec3f& L,
+ FCL_REAL* a, FCL_REAL* b, FCL_REAL* c);
+
+ /// @brief filter for intersection, works for both VF and EE
+ static bool intersectPreFiltering(const Vec3f& a0, const Vec3f& b0, const Vec3f& c0, const Vec3f& d0,
+ const Vec3f& a1, const Vec3f& b1, const Vec3f& c1, const Vec3f& d1);
+
+ /// @brief distance of point v to a plane n * x - t = 0
+ static FCL_REAL distanceToPlane(const Vec3f& n, FCL_REAL t, const Vec3f& v);
+
+ /// @brief check wether points v1, v2, v2 are on the same side of plane n * x - t = 0
+ static bool sameSideOfPlane(const Vec3f& v1, const Vec3f& v2, const Vec3f& v3, const Vec3f& n, FCL_REAL t);
+
+ /// @brief clip triangle v1, v2, v3 by the prism made by t1, t2 and t3. The normal of the prism is tn and is cutted up by to
+ static void clipTriangleByTriangleAndEdgePlanes(const Vec3f& v1, const Vec3f& v2, const Vec3f& v3,
+ const Vec3f& t1, const Vec3f& t2, const Vec3f& t3,
+ const Vec3f& tn, FCL_REAL to,
+ Vec3f clipped_points[], unsigned int* num_clipped_points, bool clip_triangle = false);
+
+ /// @brief build a plane passed through triangle v1 v2 v3
+ static bool buildTrianglePlane(const Vec3f& v1, const Vec3f& v2, const Vec3f& v3, Vec3f* n, FCL_REAL* t);
+
+ /// @brief build a plane pass through edge v1 and v2, normal is tn
+ static bool buildEdgePlane(const Vec3f& v1, const Vec3f& v2, const Vec3f& tn, Vec3f* n, FCL_REAL* t);
+
+ /// @brief compute the points which has deepest penetration depth
+ static void computeDeepestPoints(Vec3f* clipped_points, unsigned int num_clipped_points, const Vec3f& n, FCL_REAL t, FCL_REAL* penetration_depth, Vec3f* deepest_points, unsigned int* num_deepest_points);
+
+ /// @brief clip polygon by plane
+ static void clipPolygonByPlane(Vec3f* polygon_points, unsigned int num_polygon_points, const Vec3f& n, FCL_REAL t, Vec3f clipped_points[], unsigned int* num_clipped_points);
+
+ /// @brief clip a line segment by plane
+ static void clipSegmentByPlane(const Vec3f& v1, const Vec3f& v2, const Vec3f& n, FCL_REAL t, Vec3f* clipped_point);
+
+ /// @brief compute the cdf(x)
+ static FCL_REAL gaussianCDF(FCL_REAL x)
+ {
+ return 0.5 * boost::math::erfc(-x / sqrt(2.0));
+ }
+
+
+ static const FCL_REAL EPSILON;
+ static const FCL_REAL NEAR_ZERO_THRESHOLD;
+ static const FCL_REAL CCD_RESOLUTION;
+ static const unsigned int MAX_TRIANGLE_CLIPS = 8;
+};
+
+class TriangleDistance
+{
+public:
+
+ /// @brief Returns closest points between an segment pair.
+ /// The first segment is P + t * A
+ /// The second segment is Q + t * B
+ /// X, Y are the closest points on the two segments
+ /// VEC is the vector between X and Y
+ static void segPoints(const Vec3f& P, const Vec3f& A, const Vec3f& Q, const Vec3f& B,
+ Vec3f& VEC, Vec3f& X, Vec3f& Y);
+
+ /// @brief Compute the closest points on two triangles given their absolute coordinate, and returns the distance between them
+ /// S and T are two triangles
+ /// If the triangles are disjoint, P and Q give the closet points of S and T respectively. However,
+ /// if the triangles overlap, P and Q are basically a random pair of points from the triangles, not
+ /// coincident points on the intersection of the triangles, as might be expected.
+ static FCL_REAL triDistance(const Vec3f S[3], const Vec3f T[3], Vec3f& P, Vec3f& Q);
+
+ static FCL_REAL triDistance(const Vec3f& S1, const Vec3f& S2, const Vec3f& S3,
+ const Vec3f& T1, const Vec3f& T2, const Vec3f& T3,
+ Vec3f& P, Vec3f& Q);
+
+ /// @brief Compute the closest points on two triangles given the relative transform between them, and returns the distance between them
+ /// S and T are two triangles
+ /// If the triangles are disjoint, P and Q give the closet points of S and T respectively. However,
+ /// if the triangles overlap, P and Q are basically a random pair of points from the triangles, not
+ /// coincident points on the intersection of the triangles, as might be expected.
+ /// The returned P and Q are both in the coordinate of the first triangle's coordinate
+ static FCL_REAL triDistance(const Vec3f S[3], const Vec3f T[3],
+ const Matrix3f& R, const Vec3f& Tl,
+ Vec3f& P, Vec3f& Q);
+
+ static FCL_REAL triDistance(const Vec3f& S1, const Vec3f& S2, const Vec3f& S3,
+ const Vec3f& T1, const Vec3f& T2, const Vec3f& T3,
+ const Matrix3f& R, const Vec3f& Tl,
+ Vec3f& P, Vec3f& Q);
+};
+
+
+}
+
+
+#endif
diff --git a/trunk/fcl/include/fcl/narrowphase/narrowphase.h b/trunk/fcl/include/fcl/narrowphase/narrowphase.h
index e9435bbf3fac452867aaddc5fae66dfa764d8c25..36c361013d87f7ca308a4b8b4d5aa9ed6c14ff5a 100644
--- a/trunk/fcl/include/fcl/narrowphase/narrowphase.h
+++ b/trunk/fcl/include/fcl/narrowphase/narrowphase.h
@@ -599,72 +599,72 @@ bool GJKSolver_indep::shapeIntersect<Box, Box>(const Box& s1, const Transform3f&
Vec3f* contact_points, FCL_REAL* penetration_depth, Vec3f* normal) const;
template<>
-bool GJKSolver_libccd::shapeIntersect<Sphere, Halfspace>(const Sphere& s1, const Transform3f& tf1,
+bool GJKSolver_indep::shapeIntersect<Sphere, Halfspace>(const Sphere& s1, const Transform3f& tf1,
const Halfspace& s2, const Transform3f& tf2,
Vec3f* contact_points, FCL_REAL* penetration_depth, Vec3f* normal) const;
template<>
-bool GJKSolver_libccd::shapeIntersect<Box, Halfspace>(const Box& s1, const Transform3f& tf1,
+bool GJKSolver_indep::shapeIntersect<Box, Halfspace>(const Box& s1, const Transform3f& tf1,
const Halfspace& s2, const Transform3f& tf2,
Vec3f* contact_points, FCL_REAL* penetration_depth, Vec3f* normal) const;
template<>
-bool GJKSolver_libccd::shapeIntersect<Capsule, Halfspace>(const Capsule& s1, const Transform3f& tf1,
+bool GJKSolver_indep::shapeIntersect<Capsule, Halfspace>(const Capsule& s1, const Transform3f& tf1,
const Halfspace& s2, const Transform3f& tf2,
Vec3f* contact_points, FCL_REAL* penetration_depth, Vec3f* normal) const;
template<>
-bool GJKSolver_libccd::shapeIntersect<Cylinder, Halfspace>(const Cylinder& s1, const Transform3f& tf1,
+bool GJKSolver_indep::shapeIntersect<Cylinder, Halfspace>(const Cylinder& s1, const Transform3f& tf1,
const Halfspace& s2, const Transform3f& tf2,
Vec3f* contact_points, FCL_REAL* penetration_depth, Vec3f* normal) const;
template<>
-bool GJKSolver_libccd::shapeIntersect<Cone, Halfspace>(const Cone& s1, const Transform3f& tf1,
+bool GJKSolver_indep::shapeIntersect<Cone, Halfspace>(const Cone& s1, const Transform3f& tf1,
const Halfspace& s2, const Transform3f& tf2,
Vec3f* contact_points, FCL_REAL* penetration_depth, Vec3f* normal) const;
template<>
-bool GJKSolver_libccd::shapeIntersect<Halfspace, Halfspace>(const Halfspace& s1, const Transform3f& tf1,
+bool GJKSolver_indep::shapeIntersect<Halfspace, Halfspace>(const Halfspace& s1, const Transform3f& tf1,
const Halfspace& s2, const Transform3f& tf2,
Vec3f* contact_points, FCL_REAL* penetration_depth, Vec3f* normal) const;
template<>
-bool GJKSolver_libccd::shapeIntersect<Plane, Halfspace>(const Plane& s1, const Transform3f& tf1,
+bool GJKSolver_indep::shapeIntersect<Plane, Halfspace>(const Plane& s1, const Transform3f& tf1,
const Halfspace& s2, const Transform3f& tf2,
Vec3f* contact_points, FCL_REAL* penetration_depth, Vec3f* normal) const;
template<>
-bool GJKSolver_libccd::shapeIntersect<Sphere, Plane>(const Sphere& s1, const Transform3f& tf1,
+bool GJKSolver_indep::shapeIntersect<Sphere, Plane>(const Sphere& s1, const Transform3f& tf1,
const Plane& s2, const Transform3f& tf2,
Vec3f* contact_points, FCL_REAL* penetration_depth, Vec3f* normal) const;
template<>
-bool GJKSolver_libccd::shapeIntersect<Box, Plane>(const Box& s1, const Transform3f& tf1,
+bool GJKSolver_indep::shapeIntersect<Box, Plane>(const Box& s1, const Transform3f& tf1,
const Plane& s2, const Transform3f& tf2,
Vec3f* contact_points, FCL_REAL* penetration_depth, Vec3f* normal) const;
template<>
-bool GJKSolver_libccd::shapeIntersect<Capsule, Plane>(const Capsule& s1, const Transform3f& tf1,
+bool GJKSolver_indep::shapeIntersect<Capsule, Plane>(const Capsule& s1, const Transform3f& tf1,
const Plane& s2, const Transform3f& tf2,
Vec3f* contact_points, FCL_REAL* penetration_depth, Vec3f* normal) const;
template<>
-bool GJKSolver_libccd::shapeIntersect<Cylinder, Plane>(const Cylinder& s1, const Transform3f& tf1,
+bool GJKSolver_indep::shapeIntersect<Cylinder, Plane>(const Cylinder& s1, const Transform3f& tf1,
const Plane& s2, const Transform3f& tf2,
Vec3f* contact_points, FCL_REAL* penetration_depth, Vec3f* normal) const;
template<>
-bool GJKSolver_libccd::shapeIntersect<Cone, Plane>(const Cone& s1, const Transform3f& tf1,
+bool GJKSolver_indep::shapeIntersect<Cone, Plane>(const Cone& s1, const Transform3f& tf1,
const Plane& s2, const Transform3f& tf2,
Vec3f* contact_points, FCL_REAL* penetration_depth, Vec3f* normal) const;
template<>
-bool GJKSolver_libccd::shapeIntersect<Halfspace, Plane>(const Halfspace& s1, const Transform3f& tf1,
+bool GJKSolver_indep::shapeIntersect<Halfspace, Plane>(const Halfspace& s1, const Transform3f& tf1,
const Plane& s2, const Transform3f& tf2,
Vec3f* contact_points, FCL_REAL* penetration_depth, Vec3f* normal) const;
template<>
-bool GJKSolver_libccd::shapeIntersect<Plane, Plane>(const Plane& s1, const Transform3f& tf1,
+bool GJKSolver_indep::shapeIntersect<Plane, Plane>(const Plane& s1, const Transform3f& tf1,
const Plane& s2, const Transform3f& tf2,
Vec3f* contact_points, FCL_REAL* penetration_depth, Vec3f* normal) const;
@@ -679,11 +679,11 @@ bool GJKSolver_indep::shapeTriangleIntersect(const Sphere& s, const Transform3f&
const Vec3f& P1, const Vec3f& P2, const Vec3f& P3, const Transform3f& tf2, Vec3f* contact_points, FCL_REAL* penetration_depth, Vec3f* normal) const;
template<>
-bool GJKSolver_libccd::shapeTriangleIntersect(const Halfspace& s, const Transform3f& tf1,
+bool GJKSolver_indep::shapeTriangleIntersect(const Halfspace& s, const Transform3f& tf1,
const Vec3f& P1, const Vec3f& P2, const Vec3f& P3, const Transform3f& tf2, Vec3f* contact_points, FCL_REAL* penetration_depth, Vec3f* normal) const;
template<>
-bool GJKSolver_libccd::shapeTriangleIntersect(const Plane& s, const Transform3f& tf1,
+bool GJKSolver_indep::shapeTriangleIntersect(const Plane& s, const Transform3f& tf1,
const Vec3f& P1, const Vec3f& P2, const Vec3f& P3, const Transform3f& tf2, Vec3f* contact_points, FCL_REAL* penetration_depth, Vec3f* normal) const;
diff --git a/trunk/fcl/src/BVH/BVH_model.cpp b/trunk/fcl/src/BVH/BVH_model.cpp
index 8b45dcef772ad7fd43b86239bc6397f292554ee9..4333af68f0630cb3c9cbec02829c555aa532abb5 100644
--- a/trunk/fcl/src/BVH/BVH_model.cpp
+++ b/trunk/fcl/src/BVH/BVH_model.cpp
@@ -234,6 +234,7 @@ int BVHModel<BV>::addTriangle(const Vec3f& p1, const Vec3f& p2, const Vec3f& p3)
}
tri_indices[num_tris].set(offset, offset + 1, offset + 2);
+ num_tris++;
return BVH_OK;
}
diff --git a/trunk/fcl/src/BVH/BV_splitter.cpp b/trunk/fcl/src/BVH/BV_splitter.cpp
index 847ef9e9f377700fc1d4f39d552975567484ca99..da4f605e779a592d60e4d9a8de219fc372193da6 100644
--- a/trunk/fcl/src/BVH/BV_splitter.cpp
+++ b/trunk/fcl/src/BVH/BV_splitter.cpp
@@ -279,4 +279,9 @@ bool BVSplitter<OBBRSS>::apply(const Vec3f& q) const
}
+template class BVSplitter<RSS>;
+template class BVSplitter<OBBRSS>;
+template class BVSplitter<OBB>;
+template class BVSplitter<kIOS>;
+
}
diff --git a/trunk/fcl/src/ccd/taylor_model.cpp b/trunk/fcl/src/ccd/taylor_model.cpp
index 0851ed88950e5b0a368d6569931e88d1719bef92..d6df18e5338225e2de76a92bd5076de755e05816 100644
--- a/trunk/fcl/src/ccd/taylor_model.cpp
+++ b/trunk/fcl/src/ccd/taylor_model.cpp
@@ -109,7 +109,7 @@ TaylorModel TaylorModel::operator + (const TaylorModel& other) const
TaylorModel TaylorModel::operator - (const TaylorModel& other) const
{
- assert(other.time_interval__ == time_interval_);
+ assert(other.time_interval_ == time_interval_);
return TaylorModel(coeffs_[0] - other.coeffs_[0], coeffs_[1] - other.coeffs_[1], coeffs_[2] - other.coeffs_[2], coeffs_[3] - other.coeffs_[3], r_ - other.r_, time_interval_);
}
TaylorModel& TaylorModel::operator += (const TaylorModel& other)
diff --git a/trunk/fcl/src/intersect.cpp b/trunk/fcl/src/intersect.cpp
index 709752427cf8759eece588904c2e1fb91c23877d..1885838a3ffa22b7f63504ad6334571e5c9898b3 100644
--- a/trunk/fcl/src/intersect.cpp
+++ b/trunk/fcl/src/intersect.cpp
@@ -38,6 +38,7 @@
#include <iostream>
#include <limits>
#include <vector>
+#include <boost\math\special_functions\fpclassify.hpp>
namespace fcl
{
@@ -1198,7 +1199,7 @@ void TriangleDistance::segPoints(const Vec3f& P, const Vec3f& A, const Vec3f& Q,
// clamp result so t is on the segment P,A
- if((t < 0) || std::isnan(t)) t = 0; else if(t > 1) t = 1;
+ if((t < 0) || boost::math::isnan(t)) t = 0; else if(t > 1) t = 1;
// find u for point on ray Q,B closest to point at t
@@ -1208,13 +1209,13 @@ void TriangleDistance::segPoints(const Vec3f& P, const Vec3f& A, const Vec3f& Q,
// closest points, otherwise, clamp u, recompute and
// clamp t
- if((u <= 0) || std::isnan(u))
+ if((u <= 0) || boost::math::isnan(u))
{
Y = Q;
t = A_dot_T / A_dot_A;
- if((t <= 0) || std::isnan(t))
+ if((t <= 0) || boost::math::isnan(t))
{
X = P;
VEC = Q - P;
@@ -1237,7 +1238,7 @@ void TriangleDistance::segPoints(const Vec3f& P, const Vec3f& A, const Vec3f& Q,
t = (A_dot_B + A_dot_T) / A_dot_A;
- if((t <= 0) || std::isnan(t))
+ if((t <= 0) || boost::math::isnan(t))
{
X = P;
VEC = Y - P;
@@ -1259,7 +1260,7 @@ void TriangleDistance::segPoints(const Vec3f& P, const Vec3f& A, const Vec3f& Q,
{
Y = Q + B * u;
- if((t <= 0) || std::isnan(t))
+ if((t <= 0) || boost::math::isnan(t))
{
X = P;
TMP = T.cross(B);
diff --git a/trunk/fcl/test/test_fcl_math.cpp b/trunk/fcl/test/test_fcl_math.cpp
index db57b14384dad64aa557d5a311696c947585e99f..62f36693db2a0bb9cf7542a740267fe960f2a68d 100644
--- a/trunk/fcl/test/test_fcl_math.cpp
+++ b/trunk/fcl/test/test_fcl_math.cpp
@@ -36,7 +36,9 @@
#define BOOST_TEST_MODULE "FCL_MATH"
#include <boost/test/unit_test.hpp>
-#include "fcl/simd/math_simd_details.h"
+#if FCL_HAVE_SSE
+ #include "fcl/simd/math_simd_details.h"
+#endif
#include "fcl/math/vec_3f.h"
#include "fcl/math/matrix_3f.h"
#include "fcl/broadphase/morton.h"
@@ -94,8 +96,8 @@ BOOST_AUTO_TEST_CASE(vec_test_basic_vec32)
BOOST_CHECK(abs(v1.dot(v2) - 26) < 1e-5);
v1 = Vec3f32(3.0f, 4.0f, 5.0f);
- BOOST_CHECK(abs(v1.sqrLength() - 50) < 1e-5);
- BOOST_CHECK(abs(v1.length() - sqrt(50)) < 1e-5);
+ BOOST_CHECK(abs(v1.sqrLength() - 50.0) < 1e-5);
+ BOOST_CHECK(abs(v1.length() - sqrt(50.0)) < 1e-5);
BOOST_CHECK(normalize(v1).equal(v1 / v1.length()));
}
@@ -149,8 +151,8 @@ BOOST_AUTO_TEST_CASE(vec_test_basic_vec64)
BOOST_CHECK(abs(v1.dot(v2) - 26) < 1e-5);
v1 = Vec3f64(3.0, 4.0, 5.0);
- BOOST_CHECK(abs(v1.sqrLength() - 50) < 1e-5);
- BOOST_CHECK(abs(v1.length() - sqrt(50)) < 1e-5);
+ BOOST_CHECK(abs(v1.sqrLength() - 50.0) < 1e-5);
+ BOOST_CHECK(abs(v1.length() - sqrt(50.0)) < 1e-5);
BOOST_CHECK(normalize(v1).equal(v1 / v1.length()));
diff --git a/trunk/fcl/test/test_fcl_utility.h b/trunk/fcl/test/test_fcl_utility.h
index 5ca5bab3290e1394cb00eb0c197f32ae094a3736..7ec324129b09d677ebe6de0253fa14efe1033ed9 100644
--- a/trunk/fcl/test/test_fcl_utility.h
+++ b/trunk/fcl/test/test_fcl_utility.h
@@ -42,6 +42,7 @@
#include "fcl/collision_object.h"
#ifdef _WIN32
+#define NOMINMAX // required to avoid compilation errors with Visual Studio 2010
#include <windows.h>
#else
#include <sys/time.h>