diff --git a/test/test_fcl_utility.cpp b/test/test_fcl_utility.cpp
index e5c0404b26d55aa50098f18c2ab00253e2474652..e3bda2d8870ebb4be1550eaec6b228864ebcc291 100644
--- a/test/test_fcl_utility.cpp
+++ b/test/test_fcl_utility.cpp
@@ -4,6 +4,7 @@
#include "fcl/distance.h"
#include <cstdio>
#include <cstddef>
+#include <fstream>
namespace fcl
{
@@ -185,6 +186,29 @@ void loadOBJFile(const char* filename, std::vector<Vec3f>& points, std::vector<T
}
+void saveOBJFile(const char* filename, std::vector<Vec3f>& points, std::vector<Triangle>& triangles)
+{
+ std::ofstream os(filename);
+ if(!os)
+ {
+ std::cerr << "file not exist" << std::endl;
+ return;
+ }
+
+ for(std::size_t i = 0; i < points.size(); ++i)
+ {
+ os << "v " << points[i][0] << " " << points[i][1] << " " << points[i][2] << std::endl;
+ }
+
+ for(std::size_t i = 0; i < triangles.size(); ++i)
+ {
+ os << "f " << triangles[i][0] + 1 << " " << triangles[i][1] + 1 << " " << triangles[i][2] + 1 << std::endl;
+ }
+
+ os.close();
+}
+
+
void eulerToMatrix(FCL_REAL a, FCL_REAL b, FCL_REAL c, Matrix3f& R)
{
FCL_REAL c1 = cos(a);
diff --git a/test/test_fcl_utility.h b/test/test_fcl_utility.h
index f33415043143d01a7b1b8eac09296e3f68511f19..45d58936011be0c515ad9d610ec92cb1767a50e3 100644
--- a/test/test_fcl_utility.h
+++ b/test/test_fcl_utility.h
@@ -83,6 +83,8 @@ private:
/// @brief Load an obj mesh file
void loadOBJFile(const char* filename, std::vector<Vec3f>& points, std::vector<Triangle>& triangles);
+void saveOBJFile(const char* filename, std::vector<Vec3f>& points, std::vector<Triangle>& triangles);
+
/// @brief Generate one random transform whose translation is constrained by extents and rotation without constraints.
/// The translation is (x, y, z), and extents[0] <= x <= extents[3], extents[1] <= y <= extents[4], extents[2] <= z <= extents[5]
void generateRandomTransform(FCL_REAL extents[6], Transform3f& transform);
diff --git a/test/test_fcl_xmldata.cpp b/test/test_fcl_xmldata.cpp
index f4ffbf900b6889f66686d8a9dd058bcd8742c912..506b58f4ad5eb50334c6e1827aaa0c1a0ad4db3f 100644
--- a/test/test_fcl_xmldata.cpp
+++ b/test/test_fcl_xmldata.cpp
@@ -84,7 +84,7 @@ static void loadSceneFile(const std::string& filename,
tri->Attribute("g2", &v2);
tri->Attribute("g3", &v3);
- Triangle t(v1, v2, v3);
+ Triangle t(v1-1, v2-1, v3-1);
if(tri_id - 1 == (int)triangles.size())
triangles.push_back(t);
@@ -215,6 +215,53 @@ BOOST_AUTO_TEST_CASE(scene_test)
// std::cout << result1.numContacts() << std::endl;
}
+static std::string num2string(int i)
+{
+ std::string result;
+ std::ostringstream convert;
+ convert << i;
+ result = convert.str();
+ return result;
+}
+
+static void xml2obj(const std::string& in_filename, const std::string& out_filename_base)
+{
+ std::vector<std::vector<Vec3f> > points_array;
+ std::vector<std::vector<Triangle> > triangles_array;
+ std::vector<std::pair<Transform3f, Transform3f> > motions;
+ loadSceneFile(in_filename, points_array, triangles_array, motions);
+
+ std::size_t n_obj = points_array.size();
+ // save objs in local frame
+ for(std::size_t i = 0; i < n_obj; ++i)
+ {
+ std::string out_filenameL = out_filename_base + num2string(i+1) + "L.obj";
+ saveOBJFile(out_filenameL.c_str(), points_array[i], triangles_array[i]);
+ }
+
+ // save objs in frame 1
+ for(std::size_t i = 0; i < n_obj; ++i)
+ {
+ std::string out_filenameF1 = out_filename_base + num2string(i+1) + "Frame1.obj";
+ std::vector<Vec3f> points(points_array[i].size());
+ for(std::size_t j = 0; j < points.size(); ++j)
+ points[j] = motions[i].first.transform(points_array[i][j]);
+
+ saveOBJFile(out_filenameF1.c_str(), points, triangles_array[i]);
+ }
+
+ // save objs in frame 2
+ for(std::size_t i = 0; i < n_obj; ++i)
+ {
+ std::string out_filenameF2 = out_filename_base + num2string(i+1) + "Frame2.obj";
+ std::vector<Vec3f> points(points_array[i].size());
+ for(std::size_t j = 0; j < points.size(); ++j)
+ points[j] = motions[i].second.transform(points_array[i][j]);
+
+ saveOBJFile(out_filenameF2.c_str(), points, triangles_array[i]);
+ }
+}
+
static void scenePenetrationTest(const std::string& filename)
{
std::vector<std::vector<Vec3f> > points_array;
@@ -261,6 +308,7 @@ static void scenePenetrationTest(const std::string& filename)
std::cout << "pd value 2 " << result1.pd_value << std::endl;
}
+
BOOST_AUTO_TEST_CASE(scene_test_penetration)
{
boost::filesystem::path path(TEST_RESOURCES_DIR);
@@ -292,3 +340,36 @@ BOOST_AUTO_TEST_CASE(scene_test_penetration)
scenePenetrationTest(filename9);
}
+
+
+BOOST_AUTO_TEST_CASE(xml2obj_test)
+{
+ boost::filesystem::path path(TEST_RESOURCES_DIR);
+ std::string filename1 = (path / "scenario-1-2-3/Model_1_Scenario_1.txt").string();
+ xml2obj(filename1, "Model_1_Scenario_1");
+
+ std::string filename2 = (path / "scenario-1-2-3/Model_1_Scenario_2.txt").string();
+ xml2obj(filename2, "Model_1_Scenario_2");
+
+ std::string filename3 = (path / "scenario-1-2-3/Model_1_Scenario_3.txt").string();
+ xml2obj(filename3, "Model_1_Scenario_3");
+
+ std::string filename4 = (path / "scenario-1-2-3/Model_2_Scenario_1.txt").string();
+ xml2obj(filename4, "Model_2_Scenario_1");
+
+ std::string filename5 = (path / "scenario-1-2-3/Model_2_Scenario_2.txt").string();
+ xml2obj(filename5, "Model_2_Scenario_2");
+
+ std::string filename6 = (path / "scenario-1-2-3/Model_2_Scenario_3.txt").string();
+ xml2obj(filename6, "Model_2_Scenario_3");
+
+ std::string filename7 = (path / "scenario-1-2-3/Model_3_Scenario_1.txt").string();
+ xml2obj(filename7, "Model_3_Scenario_1");
+
+ std::string filename8 = (path / "scenario-1-2-3/Model_3_Scenario_2.txt").string();
+ xml2obj(filename8, "Model_3_Scenario_2");
+
+ std::string filename9 = (path / "scenario-1-2-3/Model_3_Scenario_3.txt").string();
+ xml2obj(filename9, "Model_3_Scenario_3");
+
+}