Commit 17112790 authored by jpan's avatar jpan
Browse files

This line, and those below, will be ignored--

M    fcl/test/test_core_utility.h
M    fcl/test/test_core_distance.cpp
M    fcl/test/test_core_continuous_collision.cpp
M    fcl/include/fcl/traversal_node_bvhs.h
M    fcl/include/fcl/motion_base.h
M    fcl/include/fcl/simple_setup.h
M    fcl/include/fcl/motion.h
M    fcl/include/fcl/transform.h
M    fcl/manifest.xml
M    fcl/src/motion.cpp
M    fcl/src/traversal_node_bvhs.cpp
M    fcl/CMakeLists.txt


git-svn-id: https://kforge.ros.org/fcl/fcl_ros@35 253336fb-580f-4252-a368-f3cef5a2a82b
parent cdb8b6d9
......@@ -32,7 +32,7 @@ set(LIBRARY_OUTPUT_PATH ${PROJECT_SOURCE_DIR}/lib)
add_definitions(-DUSE_PQP=0)
add_definitions(-DUSE_SVMLIGHT=0)
rosbuild_add_library(${PROJECT_NAME} src/AABB.cpp src/OBB.cpp src/RSS.cpp src/vec_3f.cpp src/traversal_node_base.cpp src/traversal_node_bvhs.cpp src/intersect.cpp src/motion.cpp src/BV_fitter.cpp src/BV_splitter.cpp src/BVH_model.cpp src/BVH_utility.cpp src/transform.cpp src/simple_setup.cpp src/geometric_shapes.cpp src/geometric_shapes_utility.cpp src/geometric_shapes_intersect.cpp src/collision_node.cpp src/traversal_recurse.cpp src/broad_phase_collision.cpp src/collision.cpp src/collision_func_matrix.cpp src/interval_tree.cpp)
rosbuild_add_library(${PROJECT_NAME} src/AABB.cpp src/OBB.cpp src/RSS.cpp src/vec_3f.cpp src/traversal_node_base.cpp src/traversal_node_bvhs.cpp src/intersect.cpp src/motion.cpp src/BV_fitter.cpp src/BV_splitter.cpp src/BVH_model.cpp src/BVH_utility.cpp src/transform.cpp src/simple_setup.cpp src/geometric_shapes.cpp src/geometric_shapes_utility.cpp src/geometric_shapes_intersect.cpp src/collision_node.cpp src/traversal_recurse.cpp src/broad_phase_collision.cpp src/collision.cpp src/collision_func_matrix.cpp src/interval_tree.cpp src/conservative_advancement.cpp)
rosbuild_add_gtest(test_core_collision test/test_core_collision.cpp)
target_link_libraries(test_core_collision fcl)
......@@ -56,4 +56,10 @@ rosbuild_add_gtest(test_core_front_list test/test_core_front_list.cpp)
target_link_libraries(test_core_front_list fcl)
rosbuild_add_gtest(test_core_continuous_collision test/test_core_continuous_collision.cpp)
target_link_libraries(test_core_continuous_collision fcl)
\ No newline at end of file
target_link_libraries(test_core_continuous_collision fcl)
rosbuild_add_executable(test_core_deformable_object test/test_core_deformable_object.cpp)
target_link_libraries(test_core_deformable_object fcl)
rosbuild_add_executable(test_core_conservative_advancement test/test_core_conservative_advancement.cpp)
target_link_libraries(test_core_conservative_advancement fcl)
\ No newline at end of file
......@@ -48,6 +48,10 @@ namespace fcl
/** \brief Linear interpolation motion
* Each Motion is assumed to have constant linear velocity and angular velocity
* The motion is R(t)(p - p_ref) + p_ref + T(t)
* Therefore, R(0) = R0, R(1) = R1
* T(0) = T0 + R0 p_ref - p_ref
* T(1) = T1 + R1 p_ref - p_ref
*/
template<typename BV>
class InterpMotion : public MotionBase<BV>
......@@ -60,6 +64,8 @@ public:
angular_axis = Vec3f(1, 0, 0);
angular_vel = 0;
/** Default reference point is local zero point */
/** Default linear velocity is zero */
}
......@@ -73,6 +79,8 @@ public:
/** Current time is zero, so the transformation is t1 */
t = t1;
/** Default reference point is local zero point */
/** Compute the velocities for the motion */
computeVelocity();
}
......@@ -87,6 +95,8 @@ public:
t2 = SimpleTransform(R2, T2 - matMulVec(R2, O));
t = t1;
reference_p = O;
/** Compute the velocities for the motion */
computeVelocity();
}
......@@ -99,33 +109,29 @@ public:
{
if(dt > 1) dt = 1;
t.T = t1.T + linear_vel * dt;
t.q = absoluteRotation(dt);
t.q.toRotation(t.R);
t.setQuatRotation(absoluteRotation(dt));
t.setTranslation(linear_vel * dt + t1.transform(reference_p) - t.getQuatRotation().transform(reference_p));
return true;
}
/** \brief Compute the motion bound for a bounding volume, given the closest direction n between two query objects
* according to mu < |v * n| + ||w x n||(r + max(||ci*||)) where ||ci*|| = ||ci x w||. w is the angular axis (normalized)
* and ci are the endpoints of the generator primitives of RSS.
* Notice that all bv parameters are in the local frame of the object, but n should be in the global frame (the reason is that the motion (t1, t2 and t) is in global frame)
/** \brief Compute the motion bound for a bounding volume along a given direction n
* For general BV, not implemented so return trivial 0
*/
BVH_REAL computeMotionBound(const BV& bv, const Vec3f& n) const { return 0.0; }
/** \brief Compute the motion bound for a triangle, given the closest direction n between two query objects
* according to mu < |v * | + ||w x n||(max||ci*||) where ||ci*|| = ||ci x w||. w is the angular axis (normalized)
/** \brief Compute the motion bound for a triangle along a given direction n
* according to mu < |v * n| + ||w x n||(max||ci*||) where ||ci*|| = ||ci x w||. w is the angular axis (normalized)
* and ci are the triangle vertex coordinates.
* Notice that the triangle is in the local frame of the object, but n should be in the global frame (the reason is that the motion (t1, t2 and t) is in global frame)
*/
BVH_REAL computeMotionBound(const Vec3f& a, const Vec3f& b, const Vec3f& c, const Vec3f& n) const
{
BVH_REAL c_proj_max = (a.cross(angular_axis)).sqrLength();
BVH_REAL c_proj_max = ((a - reference_p).cross(angular_axis)).sqrLength();
BVH_REAL tmp;
tmp = (b.cross(angular_axis)).sqrLength();
tmp = ((b - reference_p).cross(angular_axis)).sqrLength();
if(tmp > c_proj_max) c_proj_max = tmp;
tmp = (c.cross(angular_axis)).sqrLength();
tmp = ((c - reference_p).cross(angular_axis)).sqrLength();
if(tmp > c_proj_max) c_proj_max = tmp;
c_proj_max = sqrt(c_proj_max);
......@@ -138,33 +144,33 @@ public:
}
/** \brief Get the rotation and translation in current step */
void getCurrentTransformation(Vec3f R[3], Vec3f& T) const
void getCurrentTransform(Vec3f R[3], Vec3f& T) const
{
for(int i = 0; i < 3; ++i)
{
R[i] = t.R[i];
R[i] = t.getRotation()[i];
}
T = t.T;
T = t.getTranslation();
}
void getCurrentRotation(Vec3f R[3]) const
{
for(int i = 0; i < 3; ++i)
R[i] = t.R[i];
R[i] = t.getRotation()[i];
}
void getCurrentTranslation(Vec3f& T) const
{
T = t.T;
T = t.getTranslation();
}
protected:
void computeVelocity()
{
linear_vel = t2.T - t1.T;
SimpleQuaternion deltaq = t2.q * t1.q.inverse();
linear_vel = t2.transform(reference_p) - t1.transform(reference_p);
SimpleQuaternion deltaq = t2.getQuatRotation() * t1.getQuatRotation().inverse();
deltaq.toAxisAngle(angular_axis, angular_vel);
}
......@@ -179,7 +185,7 @@ protected:
SimpleQuaternion absoluteRotation(BVH_REAL t) const
{
SimpleQuaternion delta_t = deltaRotation(t);
return delta_t * t1.q;
return delta_t * t1.getQuatRotation();
}
/** \brief The transformation at time 0 */
......@@ -199,8 +205,17 @@ protected:
/** \brief Angular velocity axis */
Vec3f angular_axis;
/** \brief Reference point for the motion (in the object's local frame) */
Vec3f reference_p;
};
/** \brief Compute the motion bound for a bounding volume along a given direction n
* according to mu < |v * n| + ||w x n||(r + max(||ci*||)) where ||ci*|| = ||ci x w||. w is the angular axis (normalized)
* and ci are the endpoints of the generator primitives of RSS.
* Notice that all bv parameters are in the local frame of the object, but n should be in the global frame (the reason is that the motion (t1, t2 and t) is in global frame)
*/
template<>
BVH_REAL InterpMotion<RSS>::computeMotionBound(const RSS& bv, const Vec3f& n) const;
......
......@@ -59,7 +59,7 @@ public:
virtual BVH_REAL computeMotionBound(const Vec3f& a, const Vec3f& b, const Vec3f& c, const Vec3f& n) const = 0;
/** \brief Get the rotation and translation in current step */
virtual void getCurrentTransformation(Vec3f R[3], Vec3f& T) const = 0;
virtual void getCurrentTransform(Vec3f R[3], Vec3f& T) const = 0;
virtual void getCurrentRotation(Vec3f R[3]) const = 0;
......
......@@ -616,10 +616,6 @@ bool initialize(MeshConservativeAdvancementTraversalNode<BV>& node, BVHModel<BV>
node.w = w;
//HOW?
//node.motion1 = new InterpMotion<BV>
//node.motion2 = new InterpMotion<BV>
return true;
}
......@@ -644,8 +640,6 @@ inline bool initialize(MeshConservativeAdvancementTraversalNodeRSS& node, const
relativeTransform(R1, T1, R2, T2, node.R, node.T);
return true;
}
......
......@@ -207,7 +207,7 @@ public:
bool isIdentity() const
{
return (R[0][0] == 1) && (R[0][1] == 0) && (R[0][2] == 0) && (R[1][0] == 0) && (R[1][1] == 1) && (R[1][2] == 1) && (R[2][0] == 0) && (R[2][1] == 0) && (R[2][2] == 1)
return (R[0][0] == 1) && (R[0][1] == 0) && (R[0][2] == 0) && (R[1][0] == 0) && (R[1][1] == 1) && (R[1][2] == 0) && (R[2][0] == 0) && (R[2][1] == 0) && (R[2][2] == 1)
&& (T[0] == 0) && (T[1] == 0) && (T[2] == 0);
}
......
......@@ -1038,6 +1038,19 @@ public:
Vec3f T;
};
struct ConservativeAdvancementStackData
{
ConservativeAdvancementStackData(const Vec3f& P1_, const Vec3f& P2_, int c1_, int c2_, BVH_REAL d_) : P1(P1_), P2(P2_), c1(c1_), c2(c2_), d(d_) {}
Vec3f P1;
Vec3f P2;
int c1;
int c2;
BVH_REAL d;
};
// when using this default version, must refit the BVH in current configuration (R_t, T_t) into default configuration
template<typename BV>
class MeshConservativeAdvancementTraversalNode : public MeshDistanceTraversalNode<BV>
......@@ -1061,7 +1074,7 @@ public:
Vec3f P1, P2;
BVH_REAL d = this->model1->getBV(b1).distance(this->model2->getBV(b2), &P1, &P2);
stack.push_back(StackData(P1, P2, b1, b2, d));
stack.push_back(ConservativeAdvancementStackData(P1, P2, b1, b2, d));
return d;
}
......@@ -1105,11 +1118,9 @@ public:
}
/** n is the local frame of object 1 */
// n is the local frame of object 1
Vec3f n = P2 - P1;
/** turn n into the global frame
* nothing is done here because in this case we assume the body is in original configuration (I, 0)
*/
// here n is already in global frame as we assume the body is in original configuration (I, 0) for general BVH
BVH_REAL bound1 = motion1->computeMotionBound(p1, p2, p3, n);
BVH_REAL bound2 = motion2->computeMotionBound(q1, q2, q3, n);
......@@ -1124,14 +1135,14 @@ public:
{
if((c >= w * (this->min_distance - this->abs_err)) && (c * (1 + this->rel_err) >= w * this->min_distance))
{
const StackData& data = stack.back();
const ConservativeAdvancementStackData& data = stack.back();
BVH_REAL d = data.d;
Vec3f n;
int c1, c2;
if(d > c)
{
const StackData& data2 = stack[stack.size() - 2];
const ConservativeAdvancementStackData& data2 = stack[stack.size() - 2];
d = data2.d;
n = data2.P2 - data2.P1;
c1 = data2.c1;
......@@ -1147,9 +1158,6 @@ public:
if(c != d) std::cout << "there is some problem here" << std::endl;
// (this->tree1 + c1)->bv.axis[0] * n[0] + (this->tree1 + c1)->bv.axis[1] * n[1] + (this->tree1 + c1)->bv.axis[2] * n[2];
BVH_REAL bound1 = motion1->computeMotionBound((this->tree1 + c1)->bv, n);
BVH_REAL bound2 = motion2->computeMotionBound((this->tree2 + c2)->bv, n);
......@@ -1163,7 +1171,7 @@ public:
}
else
{
const StackData& data = stack.back();
const ConservativeAdvancementStackData& data = stack.back();
BVH_REAL d = data.d;
if(d > c)
......@@ -1190,18 +1198,7 @@ public:
MotionBase<BV>* motion1;
MotionBase<BV>* motion2;
struct StackData
{
StackData(const Vec3f& P1_, const Vec3f& P2_, int c1_, int c2_, BVH_REAL d_) : P1(P1_), P2(P2_), c1(c1_), c2(c2_), d(d_) {}
Vec3f P1;
Vec3f P2;
int c1;
int c2;
BVH_REAL d;
};
mutable std::vector<StackData> stack;
mutable std::vector<ConservativeAdvancementStackData> stack;
};
......
......@@ -10,6 +10,7 @@
<url>http://ros.org/wiki/fcl</url>
<depend package="libccd"/>
<depend package="ann"/>
<depend package="assimp"/>
<!-- <depend package="PQP"/> -->
<!-- <depend package="svm_light"/> -->
<export>
......
......@@ -43,13 +43,13 @@ namespace fcl
template<>
BVH_REAL InterpMotion<RSS>::computeMotionBound(const RSS& bv, const Vec3f& n) const
{
BVH_REAL c_proj_max = (bv.Tr.cross(angular_axis)).sqrLength();
BVH_REAL c_proj_max = ((bv.Tr - reference_p).cross(angular_axis)).sqrLength();
BVH_REAL tmp;
tmp = ((bv.Tr + bv.axis[0] * bv.l[0]).cross(angular_axis)).sqrLength();
tmp = ((bv.Tr + bv.axis[0] * bv.l[0] - reference_p).cross(angular_axis)).sqrLength();
if(tmp > c_proj_max) c_proj_max = tmp;
tmp = ((bv.Tr + bv.axis[1] * bv.l[1]).cross(angular_axis)).sqrLength();
tmp = ((bv.Tr + bv.axis[1] * bv.l[1] - reference_p).cross(angular_axis)).sqrLength();
if(tmp > c_proj_max) c_proj_max = tmp;
tmp = ((bv.Tr + bv.axis[0] * bv.l[0] + bv.axis[1] * bv.l[1]).cross(angular_axis)).sqrLength();
tmp = ((bv.Tr + bv.axis[0] * bv.l[0] + bv.axis[1] * bv.l[1] - reference_p).cross(angular_axis)).sqrLength();
if(tmp > c_proj_max) c_proj_max = tmp;
c_proj_max = sqrt(c_proj_max);
......
......@@ -393,14 +393,14 @@ bool MeshConservativeAdvancementTraversalNode<OBB>::canStop(BVH_REAL c) const
{
if((c >= w * (this->min_distance - this->abs_err)) && (c * (1 + this->rel_err) >= w * this->min_distance))
{
const StackData& data = stack.back();
const ConservativeAdvancementStackData& data = stack.back();
BVH_REAL d = data.d;
Vec3f n;
int c1, c2;
if(d > c)
{
const StackData& data2 = stack[stack.size() - 2];
const ConservativeAdvancementStackData& data2 = stack[stack.size() - 2];
d = data2.d;
n = data2.P2 - data2.P1;
c1 = data2.c1;
......@@ -431,7 +431,7 @@ bool MeshConservativeAdvancementTraversalNode<OBB>::canStop(BVH_REAL c) const
}
else
{
const StackData& data = stack.back();
const ConservativeAdvancementStackData& data = stack.back();
BVH_REAL d = data.d;
if(d > c)
......@@ -448,14 +448,14 @@ bool MeshConservativeAdvancementTraversalNode<RSS>::canStop(BVH_REAL c) const
{
if((c >= w * (this->min_distance - this->abs_err)) && (c * (1 + this->rel_err) >= w * this->min_distance))
{
const StackData& data = stack.back();
const ConservativeAdvancementStackData& data = stack.back();
BVH_REAL d = data.d;
Vec3f n;
int c1, c2;
if(d > c)
{
const StackData& data2 = stack[stack.size() - 2];
const ConservativeAdvancementStackData& data2 = stack[stack.size() - 2];
d = data2.d;
n = data2.P2 - data2.P1;
c1 = data2.c1;
......@@ -486,7 +486,7 @@ bool MeshConservativeAdvancementTraversalNode<RSS>::canStop(BVH_REAL c) const
}
else
{
const StackData& data = stack.back();
const ConservativeAdvancementStackData& data = stack.back();
BVH_REAL d = data.d;
if(d > c)
......@@ -514,7 +514,7 @@ BVH_REAL MeshConservativeAdvancementTraversalNodeRSS::BVTesting(int b1, int b2)
Vec3f P1, P2;
BVH_REAL d = distance(R, T, model1->getBV(b1).bv, model2->getBV(b2).bv, &P1, &P2);
stack.push_back(StackData(P1, P2, b1, b2, d));
stack.push_back(ConservativeAdvancementStackData(P1, P2, b1, b2, d));
return d;
}
......@@ -560,17 +560,20 @@ void MeshConservativeAdvancementTraversalNodeRSS::leafTesting(int b1, int b2) co
}
/** n is the local frame of object 1 */
/** n is the local frame of object 1, pointing from object 1 to object2 */
Vec3f n = P2 - P1;
/** turn n into the global frame */
Vec3f R[3];
motion1->getCurrentRotation(R);
Vec3f n_transformed = matMulVec(R, n);
/** turn n into the global frame, pointing from object 1 to object 2 */
Vec3f R0[3];
motion1->getCurrentRotation(R0);
Vec3f n_transformed = matMulVec(R0, n);
n_transformed.normalize();
BVH_REAL bound1 = motion1->computeMotionBound(t11, t12, t13, n_transformed);
BVH_REAL bound2 = motion2->computeMotionBound(t21, t22, t23, n_transformed);
BVH_REAL bound2 = motion2->computeMotionBound(t21, t22, t23, -n_transformed);
BVH_REAL cur_delta_t = d / (bound1 + bound2);
BVH_REAL bound = bound1 + bound2;
if(bound < d) bound = d;
BVH_REAL cur_delta_t = d / bound;
if(cur_delta_t < delta_t)
delta_t = cur_delta_t;
......@@ -580,14 +583,14 @@ bool MeshConservativeAdvancementTraversalNodeRSS::canStop(BVH_REAL c) const
{
if((c >= w * (min_distance - abs_err)) && (c * (1 + rel_err) >= w * min_distance))
{
const StackData& data = stack.back();
const ConservativeAdvancementStackData& data = stack.back();
BVH_REAL d = data.d;
Vec3f n;
int c1, c2;
if(d > c)
{
const StackData& data2 = stack[stack.size() - 2];
const ConservativeAdvancementStackData& data2 = stack[stack.size() - 2];
d = data2.d;
n = data2.P2 - data2.P1;
c1 = data2.c1;
......@@ -603,16 +606,15 @@ bool MeshConservativeAdvancementTraversalNodeRSS::canStop(BVH_REAL c) const
if(c != d) std::cout << "there is some problem here" << std::endl;
/** n is in local frame of RSS c1 */
/** turn n into the global frame */
// n is in local frame of RSS c1, so we need to turn n into the global frame
Vec3f n_transformed = model1->getBV(c1).bv.axis[0] * n[0] + model1->getBV(c1).bv.axis[1] * n[2] + model1->getBV(c1).bv.axis[2] * n[2];
Vec3f R[3];
motion1->getCurrentRotation(R);
n_transformed = matMulVec(R, n_transformed);
Vec3f R0[3];
motion1->getCurrentRotation(R0);
n_transformed = matMulVec(R0, n_transformed);
n_transformed.normalize();
BVH_REAL bound1 = motion1->computeMotionBound(model1->getBV(c1).bv, n_transformed);
BVH_REAL bound2 = motion2->computeMotionBound(model2->getBV(c2).bv, n_transformed);
BVH_REAL bound2 = motion2->computeMotionBound(model2->getBV(c2).bv, -n_transformed);
BVH_REAL cur_delta_t = c / (bound1 + bound2);
if(cur_delta_t < delta_t)
......@@ -624,7 +626,7 @@ bool MeshConservativeAdvancementTraversalNodeRSS::canStop(BVH_REAL c) const
}
else
{
const StackData& data = stack.back();
const ConservativeAdvancementStackData& data = stack.back();
BVH_REAL d = data.d;
if(d > c)
......
......@@ -39,6 +39,7 @@
#include "fcl/simple_setup.h"
#include "test_core_utility.h"
#include <gtest/gtest.h>
#include <boost/timer.hpp>
using namespace fcl;
......@@ -62,8 +63,13 @@ bool exhaustive = true;
bool enable_contact = true;
unsigned int n_dcd_samples = 10;
TEST(ccd_test, mesh_mesh)
TEST(ccd_test, mesh_mesh_bottomup)
{
double t_ccd = 0;
double t_dcd = 0;
std::vector<Vec3f> p1, p2;
std::vector<Triangle> t1, t2;
loadOBJFile("test/env.obj", p1, t1);
......@@ -73,7 +79,7 @@ TEST(ccd_test, mesh_mesh)
std::vector<Transform> transforms2; // t1
BVH_REAL extents[] = {-3000, -3000, 0, 3000, 3000, 3000};
BVH_REAL delta_trans[] = {10, 10, 10};
int n = 1000;
int n = 100;
bool verbose = false;
generateRandomTransform(extents, transforms, transforms2, delta_trans, 0.005 * 2 * 3.1415, n);
......@@ -82,8 +88,13 @@ TEST(ccd_test, mesh_mesh)
for(unsigned int i = 0; i < transforms.size(); ++i)
{
boost::timer timer1;
res = discrete_continuous_collide_Test<AABB>(transforms[i], transforms2[i], p1, t1, p2, t2, SPLIT_METHOD_MEDIAN, n_dcd_samples, verbose);
t_dcd += timer1.elapsed();
boost::timer timer2;
res2 = continuous_collide_Test<AABB>(transforms[i], transforms2[i], p1, t1, p2, t2, SPLIT_METHOD_MEDIAN, true, verbose);
t_ccd += timer2.elapsed();
if(res)
ASSERT_TRUE(res == res2);
else
......@@ -93,8 +104,54 @@ TEST(ccd_test, mesh_mesh)
}
}
std::cout << "dcd timing: " << t_dcd << " sec" << std::endl;
std::cout << "ccd timing: " << t_ccd << " sec" << std::endl;
}
TEST(ccd_test, mesh_mesh_topdown)
{
double t_ccd = 0;
double t_dcd = 0;
std::vector<Vec3f> p1, p2;
std::vector<Triangle> t1, t2;
loadOBJFile("test/env.obj", p1, t1);
loadOBJFile("test/rob.obj", p2, t2);
std::vector<Transform> transforms; // t0
std::vector<Transform> transforms2; // t1
BVH_REAL extents[] = {-3000, -3000, 0, 3000, 3000, 3000};
BVH_REAL delta_trans[] = {10, 10, 10};
int n = 100;
bool verbose = false;
generateRandomTransform(extents, transforms, transforms2, delta_trans, 0.005 * 2 * 3.1415, n);
bool res, res2;
for(unsigned int i = 0; i < transforms.size(); ++i)
{
boost::timer timer1;
res = discrete_continuous_collide_Test<AABB>(transforms[i], transforms2[i], p1, t1, p2, t2, SPLIT_METHOD_MEDIAN, n_dcd_samples, verbose);
t_dcd += timer1.elapsed();
boost::timer timer2;
res2 = continuous_collide_Test<AABB>(transforms[i], transforms2[i], p1, t1, p2, t2, SPLIT_METHOD_MEDIAN, false, verbose);
t_ccd += timer2.elapsed();
if(res)
ASSERT_TRUE(res == res2);
else
{
if(res2)
std::cout << "CCD detects collision missed in DCD" << std::endl;
}
}
std::cout << "dcd timing: " << t_dcd << " sec" << std::endl;
std::cout << "ccd timing: " << t_ccd << " sec" << std::endl;
}
int main(int argc, char **argv)
{
testing::InitGoogleTest(&argc, argv);
......
......@@ -40,6 +40,7 @@
#include "fcl/simple_setup.h"
#include "test_core_utility.h"
#include <gtest/gtest.h>
#include <boost/timer.hpp>
#if USE_PQP
......@@ -64,6 +65,11 @@ void distance_Test2(const Transform& tf,
DistanceRes& distance_result,
bool verbose = true);
bool collide_Test_OBB(const Transform& tf,
const std::vector<Vec3f>& vertices1, const std::vector<Triangle>& triangles1,
const std::vector<Vec3f>& vertices2, const std::vector<Triangle>& triangles2, SplitMethodType split_method, bool verbose);
bool verbose = false;
BVH_REAL DELTA = 0.001;
......@@ -92,9 +98,16 @@ TEST(collision_core, mesh_distance)
generateRandomTransform(extents, transforms, transforms2, delta_trans, 0.005 * 2 * 3.1415, n);
double dis_time = 0;
double col_time = 0;
DistanceRes res, res_now;
for(unsigned int i = 0; i < transforms.size(); ++i)
{