diff --git a/trunk/fcl/CMakeLists.txt b/trunk/fcl/CMakeLists.txt
index 385c74f7e208c2f22dca8eb1132b9445557b74bd..5fae2c406d8bde5ddda9261c15b29e92c9823267 100644
--- a/trunk/fcl/CMakeLists.txt
+++ b/trunk/fcl/CMakeLists.txt
@@ -53,4 +53,7 @@ rosbuild_add_gtest(test_core_broad_phase test/test_core_broad_phase.cpp)
target_link_libraries(test_core_broad_phase fcl)
rosbuild_add_gtest(test_core_front_list test/test_core_front_list.cpp)
-target_link_libraries(test_core_front_list fcl)
\ No newline at end of file
+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
diff --git a/trunk/fcl/test/test_core_continuous_collision.cpp b/trunk/fcl/test/test_core_continuous_collision.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..8fd1bd53ee142793057cd73a3c1cb6d6d353292d
--- /dev/null
+++ b/trunk/fcl/test/test_core_continuous_collision.cpp
@@ -0,0 +1,233 @@
+/*
+ * 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 */
+
+#include "fcl/traversal_node_bvhs.h"
+#include "fcl/collision_node.h"
+#include "fcl/simple_setup.h"
+#include "test_core_utility.h"
+#include <gtest/gtest.h>
+
+using namespace fcl;
+
+template<typename BV>
+bool continuous_collide_Test(const Transform& tf1, const Transform& tf2,
+ const std::vector<Vec3f>& vertices1, const std::vector<Triangle>& triangles1,
+ const std::vector<Vec3f>& vertices2, const std::vector<Triangle>& triangles2,
+ SplitMethodType split_method,
+ bool refit_bottomup, bool verbose);
+
+template<typename BV>
+bool discrete_continuous_collide_Test(const Transform& tf1, const Transform& tf2,
+ const std::vector<Vec3f>& vertices1, const std::vector<Triangle>& triangles1,
+ const std::vector<Vec3f>& vertices2, const std::vector<Triangle>& triangles2,
+ SplitMethodType split_method,
+ unsigned int nsamples,
+ bool verbose);
+
+int num_max_contacts = -1;
+bool exhaustive = true;
+bool enable_contact = true;
+unsigned int n_dcd_samples = 10;
+
+TEST(ccd_test, mesh_mesh)
+{
+ 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 = 1000;
+ 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)
+ {
+ res = discrete_continuous_collide_Test<AABB>(transforms[i], transforms2[i], p1, t1, p2, t2, SPLIT_METHOD_MEDIAN, n_dcd_samples, verbose);
+ res2 = continuous_collide_Test<AABB>(transforms[i], transforms2[i], p1, t1, p2, t2, SPLIT_METHOD_MEDIAN, true, verbose);
+ if(res)
+ ASSERT_TRUE(res == res2);
+ else
+ {
+ if(res2)
+ std::cout << "CCD detects collision missed in DCD" << std::endl;
+ }
+ }
+
+}
+
+int main(int argc, char **argv)
+{
+ testing::InitGoogleTest(&argc, argv);
+ return RUN_ALL_TESTS();
+}
+
+template<typename BV>
+bool continuous_collide_Test(const Transform& tf1, const Transform& tf2,
+ const std::vector<Vec3f>& vertices1, const std::vector<Triangle>& triangles1,
+ const std::vector<Vec3f>& vertices2, const std::vector<Triangle>& triangles2,
+ SplitMethodType split_method,
+ bool refit_bottomup, bool verbose)
+{
+ BVHModel<BV> m1;
+ BVHModel<BV> m2;
+ m1.bv_splitter.reset(new BVSplitter<BV>(split_method));
+ m2.bv_splitter.reset(new BVSplitter<BV>(split_method));
+
+ std::vector<Vec3f> vertices1_new(vertices1.size());
+ for(unsigned int i = 0; i < vertices1_new.size(); ++i)
+ {
+ vertices1_new[i] = matMulVec(tf1.R, vertices1[i]) + tf1.T;
+ }
+
+ m1.beginModel();
+ m1.addSubModel(vertices1_new, triangles1);
+ m1.endModel();
+
+ m2.beginModel();
+ m2.addSubModel(vertices2, triangles2);
+ m2.endModel();
+
+ MeshCollisionTraversalNode<BV> node0;
+
+ if(!initialize<BV>(node0, m1, m2))
+ std::cout << "initialize error" << std::endl;
+
+ node0.enable_statistics = verbose;
+ node0.num_max_contacts = num_max_contacts;
+ node0.exhaustive = exhaustive;
+ node0.enable_contact = enable_contact;
+
+ collide(&node0);
+ if(node0.pairs.size() > 0)
+ return true;
+
+ // update
+ for(unsigned int i = 0; i < vertices1_new.size(); ++i)
+ {
+ vertices1_new[i] = matMulVec(tf2.R, vertices1[i]) + tf2.T;
+ }
+
+ m1.beginUpdateModel();
+ m1.updateSubModel(vertices1_new);
+ m1.endUpdateModel(true, refit_bottomup);
+
+ m2.beginUpdateModel();
+ m2.updateSubModel(vertices2);
+ m2.endUpdateModel(true, refit_bottomup);
+
+ MeshContinuousCollisionTraversalNode<BV> node;
+
+ if(!initialize<BV>(node, m1, m2))
+ std::cout << "initialize error" << std::endl;
+
+ node.enable_statistics = verbose;
+ node.num_max_contacts = num_max_contacts;
+ node.exhaustive = exhaustive;
+ node.enable_contact = enable_contact;
+
+ collide(&node);
+
+ if(node.pairs.size() > 0)
+ return true;
+ else
+ return false;
+}
+
+template<typename BV>
+bool discrete_continuous_collide_Test(const Transform& tf1, const Transform& tf2,
+ const std::vector<Vec3f>& vertices1, const std::vector<Triangle>& triangles1,
+ const std::vector<Vec3f>& vertices2, const std::vector<Triangle>& triangles2,
+ SplitMethodType split_method,
+ unsigned int nsamples,
+ bool verbose)
+{
+ std::vector<Vec3f> vertices1_t1(vertices1.size());
+ for(unsigned int i = 0; i < vertices1_t1.size(); ++i)
+ {
+ vertices1_t1[i] = matMulVec(tf1.R, vertices1[i]) + tf1.T;
+ }
+
+ std::vector<Vec3f> vertices1_t2(vertices1.size());
+ for(unsigned int i = 0; i < vertices1_t2.size(); ++i)
+ {
+ vertices1_t2[i] = matMulVec(tf2.R, vertices1[i]) + tf2.T;
+ }
+
+ std::vector<Vec3f> vertices1_t(vertices1.size());
+
+ for(unsigned int i = 0; i <= nsamples; ++i)
+ {
+ BVHModel<BV> m1;
+ BVHModel<BV> m2;
+ m1.bv_splitter.reset(new BVSplitter<BV>(split_method));
+ m2.bv_splitter.reset(new BVSplitter<BV>(split_method));
+
+ BVH_REAL delta = i / (BVH_REAL)nsamples;
+
+ for(unsigned int j = 0; j < vertices1_t.size(); ++j)
+ vertices1_t[j] = vertices1_t1[j] * (1 - delta) + vertices1_t2[j] * delta;
+
+ m1.beginModel();
+ m1.addSubModel(vertices1_t, triangles1);
+ m1.endModel();
+
+ m2.beginModel();
+ m2.addSubModel(vertices2, triangles2);
+ m2.endModel();
+
+ MeshCollisionTraversalNode<BV> node;
+ if(!initialize<BV>(node, m1, m2))
+ std::cout << "initialize error" << std::endl;
+
+ node.enable_statistics = verbose;
+ node.num_max_contacts = num_max_contacts;
+ node.exhaustive = exhaustive;
+ node.enable_contact = enable_contact;
+
+ collide(&node);
+
+ if(node.pairs.size() > 0) return true;
+ }
+
+ return false;
+}
diff --git a/trunk/fcl/test/test_core_front_list.cpp b/trunk/fcl/test/test_core_front_list.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..1cf230398947a40f959609cd5f0f37e938d4fbab
--- /dev/null
+++ b/trunk/fcl/test/test_core_front_list.cpp
@@ -0,0 +1,431 @@
+/*
+ * 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 */
+
+#include "fcl/traversal_node_bvhs.h"
+#include "fcl/collision_node.h"
+#include "fcl/simple_setup.h"
+#include "test_core_utility.h"
+#include <gtest/gtest.h>
+
+using namespace fcl;
+
+template<typename BV>
+bool collide_front_list_Test(const Transform& tf1, const Transform& tf2,
+ const std::vector<Vec3f>& vertices1, const std::vector<Triangle>& triangles1,
+ const std::vector<Vec3f>& vertices2, const std::vector<Triangle>& triangles2,
+ SplitMethodType split_method,
+ bool refit_bottomup, bool verbose);
+
+bool collide_front_list_OBB_Test(const Transform& tf1, const Transform& tf2,
+ 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 collide_front_list_RSS_Test(const Transform& tf1, const Transform& tf2,
+ 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);
+
+template<typename BV>
+bool collide_Test(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);
+
+// TODO: randomly still have some runtime error
+TEST(collision_test, front_list)
+{
+ //srand(time(NULL));
+ 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[] = {1, 1, 1};
+ int n = 10;
+ 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)
+ {
+ res = collide_Test<AABB>(transforms2[i], p1, t1, p2, t2, SPLIT_METHOD_MEDIAN, verbose);
+ res2 = collide_front_list_Test<AABB>(transforms[i], transforms2[i], p1, t1, p2, t2, SPLIT_METHOD_MEDIAN, false, verbose);
+ ASSERT_TRUE(res == res2);
+ res = collide_Test<AABB>(transforms2[i], p1, t1, p2, t2, SPLIT_METHOD_MEAN, verbose);
+ res2 = collide_front_list_Test<AABB>(transforms[i], transforms2[i], p1, t1, p2, t2, SPLIT_METHOD_MEAN, false, verbose);
+ ASSERT_TRUE(res == res2);
+ res = collide_Test<AABB>(transforms2[i], p1, t1, p2, t2, SPLIT_METHOD_BV_CENTER, verbose);
+ res2 = collide_front_list_Test<AABB>(transforms[i], transforms2[i], p1, t1, p2, t2, SPLIT_METHOD_BV_CENTER, false, verbose);
+ ASSERT_TRUE(res == res2);
+ }
+
+ for(unsigned int i = 0; i < transforms.size(); ++i)
+ {
+ res = collide_Test<OBB>(transforms2[i], p1, t1, p2, t2, SPLIT_METHOD_MEDIAN, verbose);
+ res2 = collide_front_list_Test<OBB>(transforms[i], transforms2[i], p1, t1, p2, t2, SPLIT_METHOD_MEDIAN, false, verbose);
+ ASSERT_TRUE(res == res2);
+ res = collide_Test<OBB>(transforms2[i], p1, t1, p2, t2, SPLIT_METHOD_MEAN, verbose);
+ res2 = collide_front_list_Test<OBB>(transforms[i], transforms2[i], p1, t1, p2, t2, SPLIT_METHOD_MEAN, false, verbose);
+ ASSERT_TRUE(res == res2);
+ res = collide_Test<OBB>(transforms2[i], p1, t1, p2, t2, SPLIT_METHOD_BV_CENTER, verbose);
+ res2 = collide_front_list_Test<OBB>(transforms[i], transforms2[i], p1, t1, p2, t2, SPLIT_METHOD_BV_CENTER, false, verbose);
+ ASSERT_TRUE(res == res2);
+ }
+
+ for(unsigned int i = 0; i < transforms.size(); ++i)
+ {
+ res = collide_Test<RSS>(transforms2[i], p1, t1, p2, t2, SPLIT_METHOD_MEDIAN, verbose);
+ res2 = collide_front_list_Test<RSS>(transforms[i], transforms2[i], p1, t1, p2, t2, SPLIT_METHOD_MEDIAN, false, verbose);
+ ASSERT_TRUE(res == res2);
+ res = collide_Test<RSS>(transforms2[i], p1, t1, p2, t2, SPLIT_METHOD_MEAN, verbose);
+ res2 = collide_front_list_Test<RSS>(transforms[i], transforms2[i], p1, t1, p2, t2, SPLIT_METHOD_MEAN, false, verbose);
+ ASSERT_TRUE(res == res2);
+ res = collide_Test<RSS>(transforms2[i], p1, t1, p2, t2, SPLIT_METHOD_BV_CENTER, verbose);
+ res2 = collide_front_list_Test<RSS>(transforms[i], transforms2[i], p1, t1, p2, t2, SPLIT_METHOD_BV_CENTER, false, verbose);
+ ASSERT_TRUE(res == res2);
+ }
+
+ for(unsigned int i = 0; i < transforms.size(); ++i)
+ {
+ res = collide_Test<KDOP<16> >(transforms2[i], p1, t1, p2, t2, SPLIT_METHOD_MEDIAN, verbose);
+ res2 = collide_front_list_Test<KDOP<16> >(transforms[i], transforms2[i], p1, t1, p2, t2, SPLIT_METHOD_MEDIAN, false, verbose);
+ ASSERT_TRUE(res == res2);
+ res = collide_Test<KDOP<16> >(transforms2[i], p1, t1, p2, t2, SPLIT_METHOD_MEAN, verbose);
+ res2 = collide_front_list_Test<KDOP<16> >(transforms[i], transforms2[i], p1, t1, p2, t2, SPLIT_METHOD_MEAN, false, verbose);
+ ASSERT_TRUE(res == res2);
+ res = collide_Test<KDOP<16> >(transforms2[i], p1, t1, p2, t2, SPLIT_METHOD_BV_CENTER, verbose);
+ res2 = collide_front_list_Test<KDOP<16> >(transforms[i], transforms2[i], p1, t1, p2, t2, SPLIT_METHOD_BV_CENTER, false, verbose);
+ ASSERT_TRUE(res == res2);
+ }
+
+ for(unsigned int i = 0; i < transforms.size(); ++i)
+ {
+ res = collide_Test<KDOP<18> >(transforms2[i], p1, t1, p2, t2, SPLIT_METHOD_MEDIAN, verbose);
+ res2 = collide_front_list_Test<KDOP<18> >(transforms[i], transforms2[i], p1, t1, p2, t2, SPLIT_METHOD_MEDIAN, false, verbose);
+ ASSERT_TRUE(res == res2);
+ res = collide_Test<KDOP<18> >(transforms2[i], p1, t1, p2, t2, SPLIT_METHOD_MEAN, verbose);
+ res2 = collide_front_list_Test<KDOP<18> >(transforms[i], transforms2[i], p1, t1, p2, t2, SPLIT_METHOD_MEAN, false, verbose);
+ ASSERT_TRUE(res == res2);
+ res = collide_Test<KDOP<18> >(transforms2[i], p1, t1, p2, t2, SPLIT_METHOD_BV_CENTER, verbose);
+ res2 = collide_front_list_Test<KDOP<18> >(transforms[i], transforms2[i], p1, t1, p2, t2, SPLIT_METHOD_BV_CENTER, false, verbose);
+ ASSERT_TRUE(res == res2);
+ }
+
+ for(unsigned int i = 0; i < transforms.size(); ++i)
+ {
+ res = collide_Test<KDOP<24> >(transforms2[i], p1, t1, p2, t2, SPLIT_METHOD_MEDIAN, verbose);
+ res2 = collide_front_list_Test<KDOP<24> >(transforms[i], transforms2[i], p1, t1, p2, t2, SPLIT_METHOD_MEDIAN, false, verbose);
+ ASSERT_TRUE(res == res2);
+ res = collide_Test<KDOP<24> >(transforms2[i], p1, t1, p2, t2, SPLIT_METHOD_MEAN, verbose);
+ res2 = collide_front_list_Test<KDOP<24> >(transforms[i], transforms2[i], p1, t1, p2, t2, SPLIT_METHOD_MEAN, false, verbose);
+ ASSERT_TRUE(res == res2);
+ res = collide_Test<KDOP<24> >(transforms2[i], p1, t1, p2, t2, SPLIT_METHOD_BV_CENTER, verbose);
+ res2 = collide_front_list_Test<KDOP<24> >(transforms[i], transforms2[i], p1, t1, p2, t2, SPLIT_METHOD_BV_CENTER, false, verbose);
+ ASSERT_TRUE(res == res2);
+ }
+
+ for(unsigned int i = 0; i < transforms.size(); ++i)
+ {
+ res = collide_Test<OBB>(transforms2[i], p1, t1, p2, t2, SPLIT_METHOD_MEDIAN, verbose);
+ res2 = collide_front_list_OBB_Test(transforms[i], transforms2[i], p1, t1, p2, t2, SPLIT_METHOD_MEDIAN, verbose);
+ ASSERT_TRUE(res == res2);
+ res = collide_Test<OBB>(transforms2[i], p1, t1, p2, t2, SPLIT_METHOD_MEAN, verbose);
+ res2 = collide_front_list_OBB_Test(transforms[i], transforms2[i], p1, t1, p2, t2, SPLIT_METHOD_MEAN, verbose);
+ ASSERT_TRUE(res == res2);
+ res = collide_Test<OBB>(transforms2[i], p1, t1, p2, t2, SPLIT_METHOD_BV_CENTER, verbose);
+ res2 = collide_front_list_OBB_Test(transforms[i], transforms2[i], p1, t1, p2, t2, SPLIT_METHOD_BV_CENTER, verbose);
+ ASSERT_TRUE(res == res2);
+ }
+
+ for(unsigned int i = 0; i < transforms.size(); ++i)
+ {
+ res = collide_Test<RSS>(transforms2[i], p1, t1, p2, t2, SPLIT_METHOD_MEDIAN, verbose);
+ res2 = collide_front_list_RSS_Test(transforms[i], transforms2[i], p1, t1, p2, t2, SPLIT_METHOD_MEDIAN, verbose);
+ ASSERT_TRUE(res == res2);
+ res = collide_Test<RSS>(transforms2[i], p1, t1, p2, t2, SPLIT_METHOD_MEAN, verbose);
+ res2 = collide_front_list_RSS_Test(transforms[i], transforms2[i], p1, t1, p2, t2, SPLIT_METHOD_MEAN, verbose);
+ ASSERT_TRUE(res == res2);
+ res = collide_Test<RSS>(transforms2[i], p1, t1, p2, t2, SPLIT_METHOD_BV_CENTER, verbose);
+ res2 = collide_front_list_RSS_Test(transforms[i], transforms2[i], p1, t1, p2, t2, SPLIT_METHOD_BV_CENTER, verbose);
+ ASSERT_TRUE(res == res2);
+ }
+}
+
+
+int main(int argc, char **argv)
+{
+ testing::InitGoogleTest(&argc, argv);
+ return RUN_ALL_TESTS();
+}
+
+
+template<typename BV>
+bool collide_front_list_Test(const Transform& tf1, const Transform& tf2,
+ const std::vector<Vec3f>& vertices1, const std::vector<Triangle>& triangles1,
+ const std::vector<Vec3f>& vertices2, const std::vector<Triangle>& triangles2,
+ SplitMethodType split_method,
+ bool refit_bottomup, bool verbose)
+{
+ BVHModel<BV> m1;
+ BVHModel<BV> m2;
+ m1.bv_splitter.reset(new BVSplitter<BV>(split_method));
+ m2.bv_splitter.reset(new BVSplitter<BV>(split_method));
+
+ BVHFrontList front_list;
+
+
+ std::vector<Vec3f> vertices1_new(vertices1.size());
+ for(unsigned int i = 0; i < vertices1_new.size(); ++i)
+ {
+ vertices1_new[i] = matMulVec(tf1.R, vertices1[i]) + tf1.T;
+ }
+
+ m1.beginModel();
+ m1.addSubModel(vertices1_new, triangles1);
+ m1.endModel();
+
+ m2.beginModel();
+ m2.addSubModel(vertices2, triangles2);
+ m2.endModel();
+
+ MeshCollisionTraversalNode<BV> node;
+
+ if(!initialize<BV>(node, m1, m2))
+ std::cout << "initialize error" << std::endl;
+
+ node.enable_statistics = verbose;
+ node.num_max_contacts = std::numeric_limits<int>::max(); // front technique needs all the contacts;
+ node.exhaustive = true;
+ node.enable_contact = false;
+
+ collide(&node, &front_list);
+
+ if(verbose) std::cout << "front list size " << front_list.size() << std::endl;
+
+
+ // update the mesh
+ for(unsigned int i = 0; i < vertices1.size(); ++i)
+ {
+ vertices1_new[i] = matMulVec(tf2.R, vertices1[i]) + tf2.T;
+ }
+
+ m1.beginReplaceModel();
+ m1.replaceSubModel(vertices1_new);
+ m1.endReplaceModel(true, refit_bottomup);
+
+ m2.beginReplaceModel();
+ m2.replaceSubModel(vertices2);
+ m2.endReplaceModel(true, refit_bottomup);
+
+ node.pairs.clear();
+ collide(&node, &front_list);
+
+ if(node.pairs.size() > 0)
+ return true;
+ else
+ return false;
+}
+
+
+bool collide_front_list_OBB_Test(const Transform& tf1, const Transform& tf2,
+ 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)
+{
+ BVHModel<OBB> m1;
+ BVHModel<OBB> m2;
+ m1.bv_splitter.reset(new BVSplitter<OBB>(split_method));
+ m2.bv_splitter.reset(new BVSplitter<OBB>(split_method));
+
+ BVHFrontList front_list;
+
+ m1.beginModel();
+ m1.addSubModel(vertices1, triangles1);
+ m1.endModel();
+
+ m2.beginModel();
+ m2.addSubModel(vertices2, triangles2);
+ m2.endModel();
+
+ Vec3f R2[3];
+ R2[0] = Vec3f(1, 0, 0);
+ R2[1] = Vec3f(0, 1, 0);
+ R2[2] = Vec3f(0, 0, 1);
+ Vec3f T2;
+
+ m1.setTransform(tf1.R, tf1.T);
+ m2.setTransform(R2, T2);
+
+ MeshCollisionTraversalNodeOBB node;
+
+ if(!initialize(node, (const BVHModel<OBB>&)m1, (const BVHModel<OBB>&)m2))
+ std::cout << "initialize error" << std::endl;
+
+ node.enable_statistics = verbose;
+ node.num_max_contacts = std::numeric_limits<int>::max(); // front technique needs all the contacts;
+ node.exhaustive = true;
+ node.enable_contact = false;
+
+ collide(&node, &front_list);
+
+ if(verbose) std::cout << "front list size " << front_list.size() << std::endl;
+
+
+ // update the mesh
+ m1.setTransform(tf2.R, tf2.T);
+ if(!initialize(node, (const BVHModel<OBB>&)m1, (const BVHModel<OBB>&)m2))
+ std::cout << "initialize error" << std::endl;
+
+ node.pairs.clear();
+ collide(&node, &front_list);
+
+ if(node.pairs.size() > 0)
+ return true;
+ else
+ return false;
+}
+
+
+bool collide_front_list_RSS_Test(const Transform& tf1, const Transform& tf2,
+ 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)
+{
+ BVHModel<RSS> m1;
+ BVHModel<RSS> m2;
+ m1.bv_splitter.reset(new BVSplitter<RSS>(split_method));
+ m2.bv_splitter.reset(new BVSplitter<RSS>(split_method));
+
+ BVHFrontList front_list;
+
+ m1.beginModel();
+ m1.addSubModel(vertices1, triangles1);
+ m1.endModel();
+
+ m2.beginModel();
+ m2.addSubModel(vertices2, triangles2);
+ m2.endModel();
+
+ Vec3f R2[3];
+ R2[0] = Vec3f(1, 0, 0);
+ R2[1] = Vec3f(0, 1, 0);
+ R2[2] = Vec3f(0, 0, 1);
+ Vec3f T2;
+
+ m1.setTransform(tf1.R, tf1.T);
+ m2.setTransform(R2, T2);
+
+ MeshCollisionTraversalNodeRSS node;
+
+ if(!initialize(node, (const BVHModel<RSS>&)m1, (const BVHModel<RSS>&)m2))
+ std::cout << "initialize error" << std::endl;
+
+ node.enable_statistics = verbose;
+ node.num_max_contacts = std::numeric_limits<int>::max(); // front technique needs all the contacts;
+ node.exhaustive = true;
+ node.enable_contact = false;
+
+ collide(&node, &front_list);
+
+ if(verbose) std::cout << "front list size " << front_list.size() << std::endl;
+
+
+ // update the mesh
+ m1.setTransform(tf2.R, tf2.T);
+ if(!initialize(node, (const BVHModel<RSS>&)m1, (const BVHModel<RSS>&)m2))
+ std::cout << "initialize error" << std::endl;
+
+ node.pairs.clear();
+ collide(&node, &front_list);
+
+ if(node.pairs.size() > 0)
+ return true;
+ else
+ return false;
+}
+
+
+template<typename BV>
+bool collide_Test(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)
+{
+ BVHModel<BV> m1;
+ BVHModel<BV> m2;
+ m1.bv_splitter.reset(new BVSplitter<BV>(split_method));
+ m2.bv_splitter.reset(new BVSplitter<BV>(split_method));
+
+ m1.beginModel();
+ m1.addSubModel(vertices1, triangles1);
+ m1.endModel();
+
+ m2.beginModel();
+ m2.addSubModel(vertices2, triangles2);
+ m2.endModel();
+
+ Vec3f R2[3];
+ R2[0] = Vec3f(1, 0, 0);
+ R2[1] = Vec3f(0, 1, 0);
+ R2[2] = Vec3f(0, 0, 1);
+ Vec3f T2;
+
+ m1.setTransform(tf.R, tf.T);
+ m2.setTransform(R2, T2);
+
+ MeshCollisionTraversalNode<BV> node;
+
+ if(!initialize<BV>(node, m1, m2))
+ std::cout << "initialize error" << std::endl;
+
+ node.enable_statistics = verbose;
+ node.num_max_contacts = std::numeric_limits<int>::max();
+ node.exhaustive = true;
+ node.enable_contact = false;
+
+ collide(&node);
+
+
+ if(node.pairs.size() > 0)
+ return true;
+ else
+ return false;
+}
+