Menu
▾
▴
[Personalrobots-commit] SF.net SVN: personalrobots:[12092] pkg/trunk
|
From: <is...@us...> - 2009-03-04 02:51:09
|
Revision: 12092
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=12092&view=rev
Author: isucan
Date: 2009-03-04 02:51:07 +0000 (Wed, 04 Mar 2009)
Log Message:
-----------
support for meshes
Modified Paths:
--------------
pkg/trunk/motion_planning/planning_models/CMakeLists.txt
pkg/trunk/motion_planning/planning_models/src/kinematic.cpp
pkg/trunk/motion_planning/planning_models/src/load_mesh.cpp
pkg/trunk/motion_planning/planning_models/test/test_kinematic.cpp
pkg/trunk/world_models/collision_space/CMakeLists.txt
pkg/trunk/world_models/collision_space/include/collision_space/point_inclusion.h
pkg/trunk/world_models/collision_space/src/collision_space/point_inclusion.cpp
Modified: pkg/trunk/motion_planning/planning_models/CMakeLists.txt
===================================================================
--- pkg/trunk/motion_planning/planning_models/CMakeLists.txt 2009-03-04 02:42:07 UTC (rev 12091)
+++ pkg/trunk/motion_planning/planning_models/CMakeLists.txt 2009-03-04 02:51:07 UTC (rev 12092)
@@ -3,7 +3,7 @@
rospack(planning_models)
rospack_add_boost_directories()
-set(CMAKE_BUILD_TYPE Release)
+#set(CMAKE_BUILD_TYPE Release)
rospack_add_library(planning_models src/kinematic.cpp
src/load_mesh.cpp)
Modified: pkg/trunk/motion_planning/planning_models/src/kinematic.cpp
===================================================================
--- pkg/trunk/motion_planning/planning_models/src/kinematic.cpp 2009-03-04 02:42:07 UTC (rev 12091)
+++ pkg/trunk/motion_planning/planning_models/src/kinematic.cpp 2009-03-04 02:51:07 UTC (rev 12092)
@@ -52,7 +52,7 @@
};
// load a mesh
- shapes::Mesh* load_binary_stl(const char *filename);
+ shapes::Mesh* create_mesh_from_binary_stl(const char *filename);
}
const std::string& planning_models::KinematicModel::getModelName(void) const
Modified: pkg/trunk/motion_planning/planning_models/src/load_mesh.cpp
===================================================================
--- pkg/trunk/motion_planning/planning_models/src/load_mesh.cpp 2009-03-04 02:42:07 UTC (rev 12091)
+++ pkg/trunk/motion_planning/planning_models/src/load_mesh.cpp 2009-03-04 02:51:07 UTC (rev 12092)
@@ -34,7 +34,6 @@
#include <cstdio>
#include <cmath>
-#include <cassert>
#include <LinearMath/btVector3.h>
#include <algorithm>
#include <vector>
@@ -80,9 +79,90 @@
}
};
- shapes::Mesh* load_binary_stl(const char *filename)
+ shapes::Mesh* create_mesh_from_vertices(const std::vector<btVector3> &source)
{
+ if (source.size() < 3)
+ return NULL;
+
+ std::set<myVertex, ltVertexValue> vertices;
+ std::vector<unsigned int> triangles;
+
+ for (unsigned int i = 0 ; i < source.size() / 3 ; ++i)
+ {
+ // check if we have new vertices
+ myVertex vt;
+
+ vt.point = source[3 * i];
+ std::set<myVertex, ltVertexValue>::iterator p1 = vertices.find(vt);
+ if (p1 == vertices.end())
+ {
+ vt.index = vertices.size();
+ vertices.insert(vt);
+ }
+ else
+ vt.index = p1->index;
+ triangles.push_back(vt.index);
+
+ vt.point = source[3 * i + 1];
+ std::set<myVertex, ltVertexValue>::iterator p2 = vertices.find(vt);
+ if (p2 == vertices.end())
+ {
+ vt.index = vertices.size();
+ vertices.insert(vt);
+ }
+ else
+ vt.index = p2->index;
+ triangles.push_back(vt.index);
+
+ vt.point = source[3 * i + 2];
+ std::set<myVertex, ltVertexValue>::iterator p3 = vertices.find(vt);
+ if (p3 == vertices.end())
+ {
+ vt.index = vertices.size();
+ vertices.insert(vt);
+ }
+ else
+ vt.index = p3->index;
+ triangles.push_back(vt.index);
+ }
+
+ // sort our vertices
+ std::vector<myVertex> vt;
+ vt.insert(vt.begin(), vertices.begin(), vertices.end());
+ std::sort(vt.begin(), vt.end(), ltVertexIndex());
+
+ // copy the data to a mesh structure
+ unsigned int nt = triangles.size() / 3;
+
+ shapes::Mesh *mesh = new shapes::Mesh(vt.size(), nt);
+ for (unsigned int i = 0 ; i < vt.size() ; ++i)
+ {
+ mesh->vertices[3 * i ] = vt[i].point.getX();
+ mesh->vertices[3 * i + 1] = vt[i].point.getY();
+ mesh->vertices[3 * i + 2] = vt[i].point.getZ();
+ }
+
+ std::copy(triangles.begin(), triangles.end(), mesh->triangles);
+
+ // compute normals
+ for (unsigned int i = 0 ; i < nt ; ++i)
+ {
+ btVector3 s1 = vt[triangles[i * 3 ]].point - vt[triangles[i * 3 + 1]].point;
+ btVector3 s2 = vt[triangles[i * 3 + 1]].point - vt[triangles[i * 3 + 2]].point;
+ btVector3 normal = s1.cross(s2);
+ normal.normalize();
+ mesh->normals[3 * i ] = normal.getX();
+ mesh->normals[3 * i + 1] = normal.getY();
+ mesh->normals[3 * i + 2] = normal.getZ();
+ }
+
+ return mesh;
+ }
+
+ shapes::Mesh* create_mesh_from_binary_stl(const char *filename)
+ {
+
FILE* input = fopen(filename, "r");
if (!input)
return NULL;
@@ -107,10 +187,8 @@
// make sure we have read enough data
if (50 * numTriangles + 84 <= fileSize)
{
+ std::vector<btVector3> vertices;
- std::set<myVertex, ltVertexValue> vertices;
- std::vector<unsigned int> triangles;
-
for (unsigned int currentTriangle = 0 ; currentTriangle < numTriangles ; ++currentTriangle)
{
// skip the normal
@@ -145,77 +223,14 @@
// skip attribute
pos += 2;
- // check if we have new vertices
- myVertex vt;
-
- vt.point = v1;
- std::set<myVertex, ltVertexValue>::iterator p1 = vertices.find(vt);
- if (p1 == vertices.end())
- {
- vt.index = vertices.size();
- vertices.insert(vt);
- }
- else
- vt.index = p1->index;
- triangles.push_back(vt.index);
-
- vt.point = v2;
- std::set<myVertex, ltVertexValue>::iterator p2 = vertices.find(vt);
- if (p2 == vertices.end())
- {
- vt.index = vertices.size();
- vertices.insert(vt);
- }
- else
- vt.index = p2->index;
- triangles.push_back(vt.index);
-
- vt.point = v3;
- std::set<myVertex, ltVertexValue>::iterator p3 = vertices.find(vt);
- if (p3 == vertices.end())
- {
- vt.index = vertices.size();
- vertices.insert(vt);
- }
- else
- vt.index = p3->index;
- triangles.push_back(vt.index);
+ vertices.push_back(v1);
+ vertices.push_back(v2);
+ vertices.push_back(v3);
}
delete[] buffer;
- // sort our vertices
- std::vector<myVertex> vt;
- vt.insert(vt.begin(), vertices.begin(), vertices.end());
- std::sort(vt.begin(), vt.end(), ltVertexIndex());
-
-
- // copy the data to a mesh structure
- unsigned int nt = triangles.size() / 3;
-
- shapes::Mesh *mesh = new shapes::Mesh(vt.size(), nt);
- for (unsigned int i = 0 ; i < vt.size() ; ++i)
- {
- mesh->vertices[3 * i ] = vt[i].point.getX();
- mesh->vertices[3 * i + 1] = vt[i].point.getY();
- mesh->vertices[3 * i + 2] = vt[i].point.getZ();
- }
-
- std::copy(triangles.begin(), triangles.end(), mesh->triangles);
-
- // compute normals
- for (unsigned int i = 0 ; i < nt ; ++i)
- {
- btVector3 s1 = vt[triangles[i * 3 ]].point - vt[triangles[i * 3 + 1]].point;
- btVector3 s2 = vt[triangles[i * 3 + 1]].point - vt[triangles[i * 3 + 2]].point;
- btVector3 normal = s1.cross(s2);
- normal.normalize();
- mesh->normals[3 * i ] = normal.getX();
- mesh->normals[3 * i + 1] = normal.getY();
- mesh->normals[3 * i + 2] = normal.getZ();
- }
-
- return mesh;
+ return create_mesh_from_vertices(vertices);
}
}
Modified: pkg/trunk/motion_planning/planning_models/test/test_kinematic.cpp
===================================================================
--- pkg/trunk/motion_planning/planning_models/test/test_kinematic.cpp 2009-03-04 02:42:07 UTC (rev 12091)
+++ pkg/trunk/motion_planning/planning_models/test/test_kinematic.cpp 2009-03-04 02:51:07 UTC (rev 12092)
@@ -822,6 +822,18 @@
delete model;
}
+namespace planning_models
+{
+ shapes::Mesh* create_mesh_from_binary_stl(const char *filename);
+}
+
+TEST(Loading, Mesh)
+{
+ planning_models::shapes::Mesh *m = planning_models::create_mesh_from_binary_stl("/home/isucan/base.stl");
+ EXPECT_TRUE(m != NULL);
+ delete m;
+}
+
int main(int argc, char **argv)
{
testing::InitGoogleTest(&argc, argv);
Modified: pkg/trunk/world_models/collision_space/CMakeLists.txt
===================================================================
--- pkg/trunk/world_models/collision_space/CMakeLists.txt 2009-03-04 02:42:07 UTC (rev 12091)
+++ pkg/trunk/world_models/collision_space/CMakeLists.txt 2009-03-04 02:51:07 UTC (rev 12092)
@@ -15,4 +15,5 @@
# Unit tests
rospack_add_gtest(test_point_inclusion test/test_point_inclusion.cpp)
+rospack_link_boost(test_point_inclusion thread)
target_link_libraries(test_point_inclusion collision_space)
Modified: pkg/trunk/world_models/collision_space/include/collision_space/point_inclusion.h
===================================================================
--- pkg/trunk/world_models/collision_space/include/collision_space/point_inclusion.h 2009-03-04 02:42:07 UTC (rev 12091)
+++ pkg/trunk/world_models/collision_space/include/collision_space/point_inclusion.h 2009-03-04 02:51:07 UTC (rev 12092)
@@ -39,8 +39,10 @@
#include <planning_models/shapes.h>
#include <LinearMath/btTransform.h>
-#include <cmath>
+#include <LinearMath/btAlignedObjectArray.h>
+#include <cstdlib>
+
/**
This set of classes allows quickly detecting whether a given point
is inside an object or not. Only basic (simple) types of objects
@@ -142,25 +144,13 @@
{
}
- virtual bool containsPoint(const btVector3 &p) const
- {
- return (m_center - p).length2() < m_radius2;
- }
+ virtual bool containsPoint(const btVector3 &p) const;
protected:
- virtual void useDimensions(const planning_models::shapes::Shape *shape) // radius
- {
- m_radius = static_cast<const planning_models::shapes::Sphere*>(shape)->radius;
- }
+ virtual void useDimensions(const planning_models::shapes::Shape *shape);
+ virtual void updateInternalData(void);
- virtual void updateInternalData(void)
- {
- m_radius2 = m_radius * m_scale + m_padding;
- m_radius2 = m_radius2 * m_radius2;
- m_center = m_pose.getOrigin();
- }
-
btVector3 m_center;
double m_radius;
double m_radius2;
@@ -183,47 +173,13 @@
{
}
- virtual bool containsPoint(const btVector3 &p) const
- {
- btVector3 v = p - m_center;
- double pH = v.dot(m_normalH);
-
- if (fabs(pH) > m_length2)
- return false;
-
- double pB1 = v.dot(m_normalB1);
- double remaining = m_radius2 - pB1 * pB1;
-
- if (remaining < 0.0)
- return false;
- else
- {
- double pB2 = v.dot(m_normalB2);
- return pB2 * pB2 < remaining;
- }
- }
+ virtual bool containsPoint(const btVector3 &p) const;
protected:
- virtual void useDimensions(const planning_models::shapes::Shape *shape) // (length, radius)
- {
- m_length = static_cast<const planning_models::shapes::Cylinder*>(shape)->length;
- m_radius = static_cast<const planning_models::shapes::Cylinder*>(shape)->radius;
- }
-
- virtual void updateInternalData(void)
- {
- m_radius2 = m_radius * m_scale + m_padding;
- m_radius2 = m_radius2 * m_radius2;
- m_length2 = (m_scale * m_length + m_padding) / 2.0;
- m_center = m_pose.getOrigin();
+ virtual void useDimensions(const planning_models::shapes::Shape *shape);
+ virtual void updateInternalData(void);
- const btMatrix3x3& basis = m_pose.getBasis();
- m_normalB1 = basis.getColumn(0);
- m_normalB2 = basis.getColumn(1);
- m_normalH = basis.getColumn(2);
- }
-
btVector3 m_center;
btVector3 m_normalH;
btVector3 m_normalB1;
@@ -253,53 +209,13 @@
{
}
- virtual bool containsPoint(const btVector3 &p) const
- {
- btVector3 v = p - m_center;
- double pL = v.dot(m_normalL);
-
- if (fabs(pL) > m_length2)
- return false;
-
- double pW = v.dot(m_normalW);
-
- if (fabs(pW) > m_width2)
- return false;
-
- double pH = v.dot(m_normalH);
-
- if (fabs(pH) > m_height2)
- return false;
-
- return true;
- }
+ virtual bool containsPoint(const btVector3 &p) const;
protected:
- virtual void useDimensions(const planning_models::shapes::Shape *shape) // (x, y, z) = (length, width, height)
- {
- const double *size = static_cast<const planning_models::shapes::Box*>(shape)->size;
- m_length = size[0];
- m_width = size[1];
- m_height = size[2];
- }
+ virtual void useDimensions(const planning_models::shapes::Shape *shape); // (x, y, z) = (length, width, height)
+ virtual void updateInternalData(void);
- virtual void updateInternalData(void)
- {
- double s2 = m_scale / 2.0;
- double p2 = m_padding / 2.0;
- m_length2 = m_length * s2 + p2;
- m_width2 = m_width * s2 + p2;
- m_height2 = m_height * s2 + p2;
-
- m_center = m_pose.getOrigin();
-
- const btMatrix3x3& basis = m_pose.getBasis();
- m_normalL = basis.getColumn(0);
- m_normalW = basis.getColumn(1);
- m_normalH = basis.getColumn(2);
- }
-
btVector3 m_center;
btVector3 m_normalL;
btVector3 m_normalW;
@@ -313,6 +229,36 @@
double m_height2;
};
+
+ class ConvexMesh : public Body
+ {
+ public:
+
+ ConvexMesh(void) : Body()
+ {
+ }
+
+ ConvexMesh(const planning_models::shapes::Shape *shape) : Body()
+ {
+ setDimensions(shape);
+ }
+
+ virtual ~ConvexMesh(void)
+ {
+ }
+
+ virtual bool containsPoint(const btVector3 &p) const;
+
+ protected:
+
+ virtual void useDimensions(const planning_models::shapes::Shape *shape);
+ virtual void updateInternalData(void);
+
+ btAlignedObjectArray<btVector3> m_planes;
+ btTransform m_iPose;
+ };
+
+
Body* createBodyFromShape(const planning_models::shapes::Shape *shape);
}
Modified: pkg/trunk/world_models/collision_space/src/collision_space/point_inclusion.cpp
===================================================================
--- pkg/trunk/world_models/collision_space/src/collision_space/point_inclusion.cpp 2009-03-04 02:42:07 UTC (rev 12091)
+++ pkg/trunk/world_models/collision_space/src/collision_space/point_inclusion.cpp 2009-03-04 02:51:07 UTC (rev 12092)
@@ -36,7 +36,11 @@
#include "collision_space/point_inclusion.h"
#include "collision_space/output.h"
+#include <LinearMath/btConvexHull.h>
+#include <LinearMath/btGeometryUtil.h>
+#include <cmath>
+
static collision_space::msg::Interface MSG;
collision_space::bodies::Body* collision_space::bodies::createBodyFromShape(const planning_models::shapes::Shape *shape)
@@ -54,6 +58,9 @@
case planning_models::shapes::Shape::CYLINDER:
body = new collision_space::bodies::Cylinder(shape);
break;
+ case planning_models::shapes::Shape::MESH:
+ body = new collision_space::bodies::ConvexMesh(shape);
+ break;
default:
MSG.error("Creating body from shape: Unknown shape type %d", shape->type);
break;
@@ -61,3 +68,140 @@
return body;
}
+
+bool collision_space::bodies::Sphere::containsPoint(const btVector3 &p) const
+{
+ return (m_center - p).length2() < m_radius2;
+}
+
+void collision_space::bodies::Sphere::useDimensions(const planning_models::shapes::Shape *shape) // radius
+{
+ m_radius = static_cast<const planning_models::shapes::Sphere*>(shape)->radius;
+}
+
+void collision_space::bodies::Sphere::updateInternalData(void)
+{
+ m_radius2 = m_radius * m_scale + m_padding;
+ m_radius2 = m_radius2 * m_radius2;
+ m_center = m_pose.getOrigin();
+}
+
+bool collision_space::bodies::Cylinder::containsPoint(const btVector3 &p) const
+{
+ btVector3 v = p - m_center;
+ double pH = v.dot(m_normalH);
+
+ if (fabs(pH) > m_length2)
+ return false;
+
+ double pB1 = v.dot(m_normalB1);
+ double remaining = m_radius2 - pB1 * pB1;
+
+ if (remaining < 0.0)
+ return false;
+ else
+ {
+ double pB2 = v.dot(m_normalB2);
+ return pB2 * pB2 < remaining;
+ }
+}
+
+void collision_space::bodies::Cylinder::useDimensions(const planning_models::shapes::Shape *shape) // (length, radius)
+{
+ m_length = static_cast<const planning_models::shapes::Cylinder*>(shape)->length;
+ m_radius = static_cast<const planning_models::shapes::Cylinder*>(shape)->radius;
+}
+
+void collision_space::bodies::Cylinder::updateInternalData(void)
+{
+ m_radius2 = m_radius * m_scale + m_padding;
+ m_radius2 = m_radius2 * m_radius2;
+ m_length2 = (m_scale * m_length + m_padding) / 2.0;
+ m_center = m_pose.getOrigin();
+
+ const btMatrix3x3& basis = m_pose.getBasis();
+ m_normalB1 = basis.getColumn(0);
+ m_normalB2 = basis.getColumn(1);
+ m_normalH = basis.getColumn(2);
+}
+
+bool collision_space::bodies::Box::containsPoint(const btVector3 &p) const
+{
+ btVector3 v = p - m_center;
+ double pL = v.dot(m_normalL);
+
+ if (fabs(pL) > m_length2)
+ return false;
+
+ double pW = v.dot(m_normalW);
+
+ if (fabs(pW) > m_width2)
+ return false;
+
+ double pH = v.dot(m_normalH);
+
+ if (fabs(pH) > m_height2)
+ return false;
+
+ return true;
+}
+
+void collision_space::bodies::Box::useDimensions(const planning_models::shapes::Shape *shape) // (x, y, z) = (length, width, height)
+{
+ const double *size = static_cast<const planning_models::shapes::Box*>(shape)->size;
+ m_length = size[0];
+ m_width = size[1];
+ m_height = size[2];
+}
+
+void collision_space::bodies::Box::updateInternalData(void)
+{
+ double s2 = m_scale / 2.0;
+ double p2 = m_padding / 2.0;
+ m_length2 = m_length * s2 + p2;
+ m_width2 = m_width * s2 + p2;
+ m_height2 = m_height * s2 + p2;
+
+ m_center = m_pose.getOrigin();
+
+ const btMatrix3x3& basis = m_pose.getBasis();
+ m_normalL = basis.getColumn(0);
+ m_normalW = basis.getColumn(1);
+ m_normalH = basis.getColumn(2);
+}
+
+bool collision_space::bodies::ConvexMesh::containsPoint(const btVector3 &p) const
+{
+ btVector3 ip = (m_iPose * p) * m_scale;
+ return btGeometryUtil::isPointInsidePlanes(m_planes, ip, btScalar(m_padding));
+}
+
+void collision_space::bodies::ConvexMesh::useDimensions(const planning_models::shapes::Shape *shape)
+{
+ const planning_models::shapes::Mesh *mesh = static_cast<const planning_models::shapes::Mesh*>(shape);
+ m_planes.clear();
+
+ btVector3 *vertices = new btVector3[mesh->vertexCount];
+ for(unsigned int i = 0; i < mesh->vertexCount ; ++i)
+ {
+ vertices[i].setX(mesh->vertices[3 * i ]);
+ vertices[i].setY(mesh->vertices[3 * i + 1]);
+ vertices[i].setZ(mesh->vertices[3 * i + 2]);
+ }
+
+ HullDesc hd(QF_TRIANGLES, mesh->vertexCount, vertices);
+ HullResult hr;
+ HullLibrary hl;
+ if (hl.CreateConvexHull(hd, hr) == QE_OK)
+ btGeometryUtil::getPlaneEquationsFromVertices(hr.m_OutputVertices, m_planes);
+ else
+ MSG.error("Unable to compute convex hull");
+
+ hl.ReleaseResult(hr);
+ delete[] vertices;
+}
+
+void collision_space::bodies::ConvexMesh::updateInternalData(void)
+{
+ m_iPose = m_pose.inverse();
+}
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
