added sphereCapsuleIntersect()

test/test_fcl_sphere_capsule.cpp

+/*
+ *  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 Martin Felis <martin.felis@iwr.uni-heidelberg.de> */
+
+#define BOOST_TEST_MODULE "FCL_SPHERE_CAPSULE"
+#include <boost/test/unit_test.hpp>
+
+#include "fcl/collision.h"
+#include "fcl/shape/geometric_shapes.h"
+#include "fcl/narrowphase/narrowphase.h"
+
+using namespace fcl;
+
+BOOST_AUTO_TEST_CASE(Sphere_Capsule_test_separated_z)
+{
+	GJKSolver_libccd solver;
+
+	Sphere sphere1 (50);
+	Transform3f sphere1_transform;
+	sphere1_transform.setTranslation (Vec3f (0., 0., -50));
+
+	Capsule capsule (50, 200.);
+	Transform3f capsule_transform (Vec3f (0., 0., 200));
+
+	BOOST_CHECK (!solver.shapeIntersect(sphere1, sphere1_transform, capsule, capsule_transform, NULL, NULL, NULL));
+}
+
+BOOST_AUTO_TEST_CASE(Sphere_Capsule_test_separated_z_negative)
+{
+	GJKSolver_libccd solver;
+
+	Sphere sphere1 (50);
+	Transform3f sphere1_transform;
+	sphere1_transform.setTranslation (Vec3f (0., 0., 50));
+
+	Capsule capsule (50, 200.);
+	Transform3f capsule_transform (Vec3f (0., 0., -200));
+
+	BOOST_CHECK (!solver.shapeIntersect(sphere1, sphere1_transform, capsule, capsule_transform, NULL, NULL, NULL));
+}
+
+BOOST_AUTO_TEST_CASE(Sphere_Capsule_test_separated_x)
+{
+	GJKSolver_libccd solver;
+
+	Sphere sphere1 (50);
+	Transform3f sphere1_transform;
+	sphere1_transform.setTranslation (Vec3f (0., 0., -50));
+
+	Capsule capsule (50, 200.);
+	Transform3f capsule_transform (Vec3f (150., 0., 0.));
+
+	BOOST_CHECK (!solver.shapeIntersect(sphere1, sphere1_transform, capsule, capsule_transform, NULL, NULL, NULL));
+}
+
+BOOST_AUTO_TEST_CASE(Sphere_Capsule_test_separated_capsule_rotated)
+{
+	GJKSolver_libccd solver;
+
+	Sphere sphere1 (50);
+	Transform3f sphere1_transform;
+	sphere1_transform.setTranslation (Vec3f (0., 0., -50));
+
+	Capsule capsule (50, 200.);
+	Matrix3f rotation;
+	rotation.setEulerZYX (M_PI * 0.5, 0., 0.);
+	Transform3f capsule_transform (rotation, Vec3f (150., 0., 0.));
+
+	BOOST_CHECK (!solver.shapeIntersect(sphere1, sphere1_transform, capsule, capsule_transform, NULL, NULL, NULL));
+}
+
+BOOST_AUTO_TEST_CASE(Sphere_Capsule_test_penetration_z)
+{
+	GJKSolver_libccd solver;
+
+	Sphere sphere1 (50);
+	Transform3f sphere1_transform;
+	sphere1_transform.setTranslation (Vec3f (0., 0., -50));
+
+	Capsule capsule (50, 200.);
+	Transform3f capsule_transform (Vec3f (0., 0., 125));
+
+	FCL_REAL penetration = 0.;
+	Vec3f contact_point;
+	Vec3f normal;
+
+	bool is_intersecting = solver.shapeIntersect(sphere1, sphere1_transform, capsule, capsule_transform, &contact_point, &penetration, &normal);
+
+//	std::cout << "contact _point = " << contact_point << std::endl;
+
+	BOOST_CHECK (is_intersecting);
+	BOOST_CHECK (penetration == 25.);
+	BOOST_CHECK (Vec3f (0., 0., 1.).equal(normal));
+	BOOST_CHECK (Vec3f (0., 0., 0.).equal(contact_point));
+}