Menu
▾
▴
[Personalrobots-commit] SF.net SVN: personalrobots:[20443] pkg/trunk
|
From: <is...@us...> - 2009-08-02 20:45:07
|
Revision: 20443
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=20443&view=rev
Author: isucan
Date: 2009-08-02 20:44:59 +0000 (Sun, 02 Aug 2009)
Log Message:
-----------
clean some function names, began work on concave shapes
Modified Paths:
--------------
pkg/trunk/motion_planning/planning_environment/src/util/construct_object.cpp
pkg/trunk/motion_planning/planning_models/src/kinematic.cpp
pkg/trunk/util/geometric_shapes/include/geometric_shapes/bodies.h
pkg/trunk/util/geometric_shapes/include/geometric_shapes/shapes.h
pkg/trunk/util/geometric_shapes/src/bodies.cpp
pkg/trunk/util/geometric_shapes/src/load_mesh.cpp
pkg/trunk/util/geometric_shapes/src/shapes.cpp
pkg/trunk/world_models/collision_space/src/environmentODE.cpp
pkg/trunk/world_models/collision_space/src/environment_objects.cpp
Modified: pkg/trunk/motion_planning/planning_environment/src/util/construct_object.cpp
===================================================================
--- pkg/trunk/motion_planning/planning_environment/src/util/construct_object.cpp 2009-08-02 19:42:28 UTC (rev 20442)
+++ pkg/trunk/motion_planning/planning_environment/src/util/construct_object.cpp 2009-08-02 20:44:59 UTC (rev 20443)
@@ -84,7 +84,7 @@
vertices[i].setValue(obj.vertices[i].x, obj.vertices[i].y, obj.vertices[i].z);
for (unsigned int i = 0 ; i < obj.triangles.size() ; ++i)
triangles[i] = obj.triangles[i];
- shape = shapes::create_mesh_from_vertices(vertices, triangles);
+ shape = shapes::createMeshFromVertices(vertices, triangles);
}
}
}
Modified: pkg/trunk/motion_planning/planning_models/src/kinematic.cpp
===================================================================
--- pkg/trunk/motion_planning/planning_models/src/kinematic.cpp 2009-08-02 19:42:28 UTC (rev 20442)
+++ pkg/trunk/motion_planning/planning_models/src/kinematic.cpp 2009-08-02 20:44:59 UTC (rev 20443)
@@ -265,7 +265,7 @@
if (filename.rfind(".stl") == std::string::npos)
filename += ".stl";
// std::cout << "Loading '" << filename << "'" << std::endl;
- shapes::Mesh *mesh = shapes::create_mesh_from_binary_stl(filename.c_str());
+ shapes::Mesh *mesh = shapes::createMeshFromBinaryStl(filename.c_str());
shape = mesh;
}
break;
@@ -852,12 +852,12 @@
dest->constGeomTrans = src->constGeomTrans;
dest->globalTransFwd = src->globalTransFwd;
dest->globalTrans = src->globalTrans;
- dest->shape = shapes::clone_shape(src->shape);
+ dest->shape = shapes::cloneShape(src->shape);
for (unsigned int i = 0 ; i < src->attachedBodies.size() ; ++i)
{
AttachedBody *ab = new AttachedBody(dest);
ab->attachTrans = src->attachedBodies[i]->attachTrans;
- ab->shape = shapes::clone_shape(src->attachedBodies[i]->shape);
+ ab->shape = shapes::cloneShape(src->attachedBodies[i]->shape);
ab->globalTrans = src->attachedBodies[i]->globalTrans;
dest->attachedBodies.push_back(ab);
}
Modified: pkg/trunk/util/geometric_shapes/include/geometric_shapes/bodies.h
===================================================================
--- pkg/trunk/util/geometric_shapes/include/geometric_shapes/bodies.h 2009-08-02 19:42:28 UTC (rev 20442)
+++ pkg/trunk/util/geometric_shapes/include/geometric_shapes/bodies.h 2009-08-02 20:44:59 UTC (rev 20443)
@@ -39,6 +39,8 @@
#include "geometric_shapes/shapes.h"
#include <LinearMath/btTransform.h>
+#include <BulletCollision/CollisionShapes/btBvhTriangleMeshShape.h>
+#include <BulletCollision/CollisionShapes/btTriangleMesh.h>
#include <vector>
/**
@@ -295,7 +297,61 @@
double m_height2;
double m_radiusB;
};
+
+ class Mesh : public Body
+ {
+ Mesh(void) : Body()
+ {
+ m_type = shapes::MESH;
+ m_btMeshShape = NULL;
+ m_btMesh = NULL;
+ m_center.setValue(0, 0, 0);
+ m_aabbMin.setValue(0, 0, 0);
+ m_aabbMax.setValue(0, 0, 0);
+ }
+
+ Mesh(const shapes::Shape *shape) : Body()
+ {
+ m_type = shapes::MESH;
+ m_btMeshShape = NULL;
+ m_btMesh = NULL;
+ m_center.setValue(0, 0, 0);
+ m_aabbMin.setValue(0, 0, 0);
+ m_aabbMax.setValue(0, 0, 0);
+ setDimensions(shape);
+ }
+
+ virtual ~Mesh(void)
+ {
+ if (m_btMeshShape)
+ delete m_btMeshShape;
+ if (m_btMesh)
+ delete m_btMesh;
+ }
+
+ /** \brief The mesh is considered to be concave, so this function is implemented with raycasting. This is a bit slow. */
+ virtual bool containsPoint(const btVector3 &p) const;
+
+ /** \brief This function is approximate. It returns the volume of the AABB enclosing the shape */
+ virtual double computeVolume(void) const;
+ virtual void computeBoundingSphere(BoundingSphere &sphere) const;
+ virtual bool intersectsRay(const btVector3& origin, const btVector3 &dir, std::vector<btVector3> *intersections = NULL, unsigned int count = 0);
+
+ protected:
+
+ virtual void useDimensions(const shapes::Shape *shape);
+ virtual void updateInternalData(void);
+
+ btBvhTriangleMeshShape *m_btMeshShape;
+ btTriangleMesh *m_btMesh;
+ btTransform m_iPose;
+ btVector3 m_center;
+ btVector3 m_aabbMin;
+ btVector3 m_aabbMax;
+
+ };
+
/** \brief Definition of a convex mesh. Convex hull is computed for a given shape::Mesh */
class ConvexMesh : public Body
{
@@ -320,8 +376,10 @@
{
}
+
virtual bool containsPoint(const btVector3 &p) const;
virtual double computeVolume(void) const;
+
virtual void computeBoundingSphere(BoundingSphere &sphere) const;
virtual bool intersectsRay(const btVector3& origin, const btVector3 &dir, std::vector<btVector3> *intersections = NULL, unsigned int count = 0);
Modified: pkg/trunk/util/geometric_shapes/include/geometric_shapes/shapes.h
===================================================================
--- pkg/trunk/util/geometric_shapes/include/geometric_shapes/shapes.h 2009-08-02 19:42:28 UTC (rev 20442)
+++ pkg/trunk/util/geometric_shapes/include/geometric_shapes/shapes.h 2009-08-02 20:44:59 UTC (rev 20443)
@@ -225,23 +225,23 @@
constructed using index values from the triangles
vector. Triangle k has vertices at index values triangles[3k],
triangles[3k+1], triangles[3k+2] */
- Mesh* create_mesh_from_vertices(const std::vector<btVector3> &vertices, const std::vector<unsigned int> &triangles);
+ Mesh* createMeshFromVertices(const std::vector<btVector3> &vertices, const std::vector<unsigned int> &triangles);
/** \brief Load a mesh from a set of vertices. Every 3 vertices
are considered a triangle. Repeating vertices are identified
and the set of triangle indices is constructed. The normal at
each triangle is also computed */
- Mesh* create_mesh_from_vertices(const std::vector<btVector3> &source);
+ Mesh* createMeshFromVertices(const std::vector<btVector3> &source);
/** \brief Load a mesh from a binary STL file. Normals are
recomputed and repeating vertices are identified. */
- Mesh* create_mesh_from_binary_stl(const char *filename);
+ Mesh* createMeshFromBinaryStl(const char *filename);
/** \brief Create a copy of a shape */
- Shape* clone_shape(const Shape *shape);
+ Shape* cloneShape(const Shape *shape);
/** \brief Create a copy of a static shape */
- StaticShape* clone_shape(const StaticShape *shape);
+ StaticShape* cloneShape(const StaticShape *shape);
}
Modified: pkg/trunk/util/geometric_shapes/src/bodies.cpp
===================================================================
--- pkg/trunk/util/geometric_shapes/src/bodies.cpp 2009-08-02 19:42:28 UTC (rev 20442)
+++ pkg/trunk/util/geometric_shapes/src/bodies.cpp 2009-08-02 20:44:59 UTC (rev 20443)
@@ -36,6 +36,7 @@
#include "geometric_shapes/bodies.h"
#include <LinearMath/btConvexHull.h>
+#include <BulletCollision/CollisionShapes/btTriangleShape.h>
#include <algorithm>
#include <iostream>
#include <cmath>
@@ -469,6 +470,133 @@
return true;
}
+bool bodies::Mesh::containsPoint(const btVector3 &p) const
+{
+ return false;
+}
+
+namespace bodies
+{
+ namespace detail
+ {
+ class myTriangleCallback : public btTriangleCallback
+ {
+ public:
+
+ myTriangleCallback(bool keep_triangles) : keep_triangles_(keep_triangles)
+ {
+ found_intersections = false;
+ }
+
+ virtual void processTriangle(btVector3 *triangle, int partId, int triangleIndex)
+ {
+ found_intersections = true;
+ if (keep_triangles_)
+ triangles.push_back(btTriangleShape(triangle[0], triangle[1], triangle[2]));
+ }
+
+ bool found_intersections;
+ std::vector<btTriangleShape> triangles;
+
+ private:
+
+ bool keep_triangles_;
+ };
+ }
+}
+
+bool bodies::Mesh::intersectsRay(const btVector3& origin, const btVector3& dir, std::vector<btVector3> *intersections, unsigned int count)
+{
+ if (m_btMeshShape)
+ {
+ // transform the ray into the coordinate frame of the mesh
+ btVector3 orig(m_iPose * origin);
+ btVector3 dr(m_iPose.getBasis() * dir);
+ /// \todo should do intersection with AABB first; the target for performRayCast is the intersection if the box
+ detail::myTriangleCallback cb(intersections != NULL);
+ m_btMeshShape->performRaycast(&cb, orig, dr);
+ if (cb.found_intersections)
+ {
+ if (intersections)
+ {
+ /// \todo find the intersections with list of triangles
+ }
+ return true;
+ }
+ else
+ return false;
+ }
+ else
+ return false;
+}
+
+void bodies::Mesh::useDimensions(const shapes::Shape *shape)
+{
+ const shapes::Mesh *mesh = static_cast<const shapes::Mesh*>(shape);
+ if (m_btMeshShape)
+ delete m_btMeshShape;
+ if (m_btMesh)
+ delete m_btMesh;
+
+ m_btMesh = new btTriangleMesh();
+ const unsigned int nt = mesh->triangleCount / 3;
+ for (unsigned int i = 0 ; i < nt ; ++i)
+ {
+ const unsigned int i3 = i *3;
+ const unsigned int v1 = 3 * mesh->triangles[i3];
+ const unsigned int v2 = 3 * mesh->triangles[i3 + 1];
+ const unsigned int v3 = 3 * mesh->triangles[i3 + 2];
+ m_btMesh->addTriangle(btVector3(mesh->vertices[v1], mesh->vertices[v1 + 1], mesh->vertices[v1 + 2]),
+ btVector3(mesh->vertices[v2], mesh->vertices[v2 + 1], mesh->vertices[v2 + 2]),
+ btVector3(mesh->vertices[v3], mesh->vertices[v3 + 1], mesh->vertices[v3 + 2]), true);
+ }
+
+ m_btMeshShape = new btBvhTriangleMeshShape(m_btMesh, true);
+}
+
+void bodies::Mesh::updateInternalData(void)
+{
+ if (m_btMeshShape)
+ {
+ m_btMeshShape->setLocalScaling(btVector3(m_scale, m_scale, m_scale));
+ m_btMeshShape->setMargin(m_padding);
+ }
+ m_iPose = m_pose.inverse();
+ m_center = m_pose.getOrigin();
+
+ btTransform id;
+ id.setIdentity();
+ m_btMeshShape->getAabb(id, m_aabbMin, m_aabbMax);
+ /// \todo check if the AABB computation includes padding & scaling; if not, include it
+}
+
+double bodies::Mesh::computeVolume(void) const
+{
+ if (m_btMeshShape)
+ {
+ // approximation; volume of AABB at identity transform
+ btVector3 d = m_aabbMax - m_aabbMin;
+ return d.x() * d.y() * d.z();
+ }
+ else
+ return 0.0;
+}
+
+void bodies::Mesh::computeBoundingSphere(BoundingSphere &sphere) const
+{
+ if (m_btMeshShape)
+ {
+ sphere.center = m_center + (m_aabbMax + m_aabbMin) / 2.0;
+ btVector3 d = (m_aabbMax - m_aabbMin) / 2.0;
+ sphere.radius = d.length();
+ }
+ else
+ {
+ sphere.center = m_center;
+ sphere.radius = 0.0;
+ }
+}
+
bool bodies::ConvexMesh::containsPoint(const btVector3 &p) const
{
// if (m_boundingBox.containsPoint(p))
Modified: pkg/trunk/util/geometric_shapes/src/load_mesh.cpp
===================================================================
--- pkg/trunk/util/geometric_shapes/src/load_mesh.cpp 2009-08-02 19:42:28 UTC (rev 20442)
+++ pkg/trunk/util/geometric_shapes/src/load_mesh.cpp 2009-08-02 20:44:59 UTC (rev 20443)
@@ -80,7 +80,7 @@
};
}
- shapes::Mesh* create_mesh_from_vertices(const std::vector<btVector3> &vertices, const std::vector<unsigned int> &triangles)
+ shapes::Mesh* createMeshFromVertices(const std::vector<btVector3> &vertices, const std::vector<unsigned int> &triangles)
{
unsigned int nt = triangles.size() / 3;
shapes::Mesh *mesh = new shapes::Mesh(vertices.size(), nt);
@@ -107,7 +107,7 @@
return mesh;
}
- shapes::Mesh* create_mesh_from_vertices(const std::vector<btVector3> &source)
+ shapes::Mesh* createMeshFromVertices(const std::vector<btVector3> &source)
{
if (source.size() < 3)
return NULL;
@@ -188,7 +188,7 @@
return mesh;
}
- shapes::Mesh* create_mesh_from_binary_stl(const char *filename)
+ shapes::Mesh* createMeshFromBinaryStl(const char *filename)
{
FILE* input = fopen(filename, "r");
@@ -258,7 +258,7 @@
delete[] buffer;
- return create_mesh_from_vertices(vertices);
+ return createMeshFromVertices(vertices);
}
}
Modified: pkg/trunk/util/geometric_shapes/src/shapes.cpp
===================================================================
--- pkg/trunk/util/geometric_shapes/src/shapes.cpp 2009-08-02 19:42:28 UTC (rev 20442)
+++ pkg/trunk/util/geometric_shapes/src/shapes.cpp 2009-08-02 20:44:59 UTC (rev 20443)
@@ -36,7 +36,7 @@
#include "geometric_shapes/shapes.h"
-shapes::Shape* shapes::clone_shape(const shapes::Shape *shape)
+shapes::Shape* shapes::cloneShape(const shapes::Shape *shape)
{
shapes::Shape *result = NULL;
switch (shape->type)
@@ -72,7 +72,7 @@
return result;
}
-shapes::StaticShape* shapes::clone_shape(const shapes::StaticShape *shape)
+shapes::StaticShape* shapes::cloneShape(const shapes::StaticShape *shape)
{
shapes::StaticShape *result = NULL;
switch (shape->type)
Modified: pkg/trunk/world_models/collision_space/src/environmentODE.cpp
===================================================================
--- pkg/trunk/world_models/collision_space/src/environmentODE.cpp 2009-08-02 19:42:28 UTC (rev 20442)
+++ pkg/trunk/world_models/collision_space/src/environmentODE.cpp 2009-08-02 20:44:59 UTC (rev 20443)
@@ -966,13 +966,13 @@
int idx = shapePtrs[dGeomGetData(it->second->geoms[i])];
if (idx < 0) // static geom
{
- shapes::StaticShape *newShape = shapes::clone_shape(ns.staticShape[-idx - 1]);
+ shapes::StaticShape *newShape = shapes::cloneShape(ns.staticShape[-idx - 1]);
dGeomSetData(newGeom, reinterpret_cast<void*>(newShape));
env->m_objects->addObject(it->first, newShape);
}
else // movable geom
{
- shapes::Shape *newShape = shapes::clone_shape(ns.shape[idx]);
+ shapes::Shape *newShape = shapes::cloneShape(ns.shape[idx]);
dGeomSetData(newGeom, reinterpret_cast<void*>(newShape));
env->m_objects->addObject(it->first, newShape, ns.shapePose[idx]);
}
@@ -987,13 +987,13 @@
int idx = shapePtrs[dGeomGetData(geoms[i])];
if (idx < 0) // static geom
{
- shapes::StaticShape *newShape = shapes::clone_shape(ns.staticShape[-idx - 1]);
+ shapes::StaticShape *newShape = shapes::cloneShape(ns.staticShape[-idx - 1]);
dGeomSetData(newGeom, reinterpret_cast<void*>(newShape));
env->m_objects->addObject(it->first, newShape);
}
else // movable geom
{
- shapes::Shape *newShape = shapes::clone_shape(ns.shape[idx]);
+ shapes::Shape *newShape = shapes::cloneShape(ns.shape[idx]);
dGeomSetData(newGeom, reinterpret_cast<void*>(newShape));
env->m_objects->addObject(it->first, newShape, ns.shapePose[idx]);
}
Modified: pkg/trunk/world_models/collision_space/src/environment_objects.cpp
===================================================================
--- pkg/trunk/world_models/collision_space/src/environment_objects.cpp 2009-08-02 19:42:28 UTC (rev 20442)
+++ pkg/trunk/world_models/collision_space/src/environment_objects.cpp 2009-08-02 20:44:59 UTC (rev 20443)
@@ -132,10 +132,10 @@
NamespaceObjects &ns = c->m_objects[it->first];
unsigned int n = it->second.staticShape.size();
for (unsigned int i = 0 ; i < n ; ++i)
- ns.staticShape.push_back(shapes::clone_shape(it->second.staticShape[i]));
+ ns.staticShape.push_back(shapes::cloneShape(it->second.staticShape[i]));
n = it->second.shape.size();
for (unsigned int i = 0 ; i < n ; ++i)
- ns.shape.push_back(shapes::clone_shape(it->second.shape[i]));
+ ns.shape.push_back(shapes::cloneShape(it->second.shape[i]));
ns.shapePose = it->second.shapePose;
}
return c;
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
