36 #define _USE_MATH_DEFINES 39 #define BOOST_TEST_MODULE FCL_MATH 40 #include <boost/test/included/unit_test.hpp> 51 Vec3f v1(1.0, 2.0, 3.0);
52 BOOST_CHECK(v1[0] == 1.0);
53 BOOST_CHECK(v1[1] == 2.0);
54 BOOST_CHECK(v1[2] == 3.0);
57 Vec3f v3(3.3, 4.3, 5.3);
59 BOOST_CHECK(
isEqual(v1, v2 + v3));
63 BOOST_CHECK(
isEqual(v1, v2 - v3));
66 v1.array() *= v3.array();
67 BOOST_CHECK(
isEqual(v1, v2.cwiseProduct(v3)));
68 v1.array() /= v3.array();
70 v1.array() /= v3.array();
71 BOOST_CHECK(
isEqual(v1, v2.cwiseQuotient(v3)));
72 v1.array() *= v3.array();
75 BOOST_CHECK(
isEqual(v1, v2 * 2.0));
79 BOOST_CHECK(
isEqual(v1, v2 / 2.0));
83 BOOST_CHECK(
isEqual(v1, (v2.array() + 2.0).matrix()));
87 BOOST_CHECK(
isEqual(v1, (v2.array() - 2.0).matrix()));
92 v1 =
Vec3f(1.0, 2.0, 3.0);
93 v2 =
Vec3f(3.0, 4.0, 5.0);
94 BOOST_CHECK(
isEqual((v1.cross(v2)),
Vec3f(-2.0, 4.0, -2.0)));
95 BOOST_CHECK(std::abs(v1.dot(v2) - 26) < 1e-5);
97 v1 =
Vec3f(3.0, 4.0, 5.0);
98 BOOST_CHECK(std::abs(v1.squaredNorm() - 50) < 1e-5);
99 BOOST_CHECK(std::abs(v1.norm() - sqrt(50)) < 1e-5);
100 BOOST_CHECK(
isEqual(v1.normalized(), v1 / v1.norm()));
102 v1 =
Vec3f(1.0, 2.0, 3.0);
103 v2 =
Vec3f(3.0, 4.0, 5.0);
104 BOOST_CHECK(
isEqual(v1.cross(v2),
Vec3f(-2.0, 4.0, -2.0)));
105 BOOST_CHECK(v1.dot(v2) == 26);
109 return 2 * vec.dot(input) * vec + (w * w - vec.dot(vec)) * input +
110 2 * w * vec.cross(input);
133 BOOST_CHECK(
isEqual(q * v + T, q * v + T));
static AABB rotate(const AABB &aabb, const Matrix3f &R)
Quaternion3f fromAxisAngle(const Eigen::MatrixBase< Derived > &axis, FCL_REAL angle)
BOOST_AUTO_TEST_CASE(vec_test_eigen_vec64)
Eigen::Quaternion< FCL_REAL > Quaternion3f
Eigen::Matrix< FCL_REAL, 3, 1 > Vec3f
HPP_FCL_DLLAPI Halfspace transform(const Halfspace &a, const Transform3f &tf)
bool isEqual(const Eigen::MatrixBase< Derived > &lhs, const Eigen::MatrixBase< OtherDerived > &rhs, const FCL_REAL tol=std::numeric_limits< FCL_REAL >::epsilon() *100)