From 341fcb881f2e0a655e20dc9ede006262d09afb5d Mon Sep 17 00:00:00 2001
From: tkunz <tkunz@253336fb-580f-4252-a368-f3cef5a2a82b>
Date: Mon, 17 Sep 2012 22:41:06 +0000
Subject: [PATCH] Made code build with Visual Studio 2010.
- In intersect.h/cpp: Replaced some STL functions with their boost counterparts because VS 2010 does not fully support C99/C++11.
- In test/test_fcl_utility.h: Added NOMINMAX definition in order to avoid name clashes on Windows.
- In test/test_fcl_math.cpp: Only include simd/math_simd_details.h if FCL_HAVE_SSE is set. Caused compilation errors on Windows because simd/math_simd_details.h includes GCC-specific instructions.
- In test/test_fcl_math.cpp: Converted the constant parameter of the sqrt function from int to double because Visual Studio was not able to deduct the correct version of the overloaded sqrt function.
- In narrowphase/narrowphase.h: Fixed some issues with template specialization. It looks like some parts of the code were not correctly replaced after some copy-and-paste. This caused specialized templated member functions to not be declared in the header file. Thus, Visual Studio used the generic version instead. But, since the cpp defines specialized versions, the linker complained about functions being defined twice. Don't know why it compiled with GCC.
- In BVH/BV_splitter.cpp: It seems like Visual Studio does not automatically compile a fully specialized version of a templated class even if a specialized member function is defined. Thus, needed to add statements to force a compilation of specialized class templates.
- In BVH/BVH_model.cpp: Fixed a bug that caused a segmentation fault somewhere else (probably platform-independent).
- In ccd/taylor_model.cpp: Fixed a typo that did not cause any trouble for me but might when checking assertions.
git-svn-id: https://kforge.ros.org/fcl/fcl_ros@194 253336fb-580f-4252-a368-f3cef5a2a82b
---
trunk/fcl/include/fcl/data_types.h | 2 +-
trunk/fcl/include/fcl/intersect.h | 583 +++++++++---------
.../fcl/include/fcl/narrowphase/narrowphase.h | 32 +-
trunk/fcl/src/BVH/BVH_model.cpp | 1 +
trunk/fcl/src/BVH/BV_splitter.cpp | 5 +
trunk/fcl/src/ccd/taylor_model.cpp | 2 +-
trunk/fcl/src/intersect.cpp | 11 +-
trunk/fcl/test/test_fcl_math.cpp | 12 +-
trunk/fcl/test/test_fcl_utility.h | 1 +
9 files changed, 330 insertions(+), 319 deletions(-)
diff --git a/trunk/fcl/include/fcl/data_types.h b/trunk/fcl/include/fcl/data_types.h
index f7818774..32a71920 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 13d282f8..c1f3f2fa 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 e9435bbf..36c36101 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 8b45dcef..4333af68 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 847ef9e9..da4f605e 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 0851ed88..d6df18e5 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 70975242..1885838a 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 db57b143..62f36693 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 5ca5bab3..7ec32412 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>
--
GitLab