[Personalrobots-commit] SF.net SVN: personalrobots:[16813] pkg/trunk

[<is...@us...>]

Revision: 16813
          http://personalrobots.svn.sourceforge.net/personalrobots/?rev=16813&view=rev
Author:   isucan
Date:     2009-06-06 21:09:43 +0000 (Sat, 06 Jun 2009)

Log Message:
-----------
optimized self filter + more thorough tests

Modified Paths:
--------------
    pkg/trunk/motion_planning/planning_environment/collision_checks.yaml
    pkg/trunk/util/new_robot_self_filter/self_see_filter.h
    pkg/trunk/util/new_robot_self_filter/test_filter.cpp
    pkg/trunk/world_models/collision_space/CMakeLists.txt
    pkg/trunk/world_models/collision_space/test/test_point_inclusion.cpp
    pkg/trunk/world_models/test_collision_space/src/test_cs_point_inclusion.cpp

Added Paths:
-----------
    pkg/trunk/motion_planning/planning_environment/planning_environment.launch
    pkg/trunk/world_models/collision_space/include/collision_space/bodies.h
    pkg/trunk/world_models/collision_space/src/collision_space/bodies.cpp

Removed Paths:
-------------
    pkg/trunk/util/new_robot_self_filter/.build_version
    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_environment/collision_checks.yaml
===================================================================
--- pkg/trunk/motion_planning/planning_environment/collision_checks.yaml	2009-06-06 17:46:09 UTC (rev 16812)
+++ pkg/trunk/motion_planning/planning_environment/collision_checks.yaml	2009-06-06 21:09:43 UTC (rev 16813)
@@ -53,10 +53,14 @@
 ## list of links that the robot can see with its sensors (used to remove 
 ## parts of the robot from scanned data)
 self_see:
+  r_upper_arm_link
+  l_upper_arm_link
   l_upper_arm_roll_link
   r_upper_arm_roll_link
   l_elbow_flex_link
   r_elbow_flex_link
+  r_forearm_link
+  l_forearm_link
   l_forearm_roll_link
   r_forearm_roll_link
   l_wrist_flex_link
@@ -64,9 +68,13 @@
   l_wrist_roll_link
   r_wrist_roll_link
   l_gripper_l_finger_link
+  l_gripper_l_finger_tip_link
   l_gripper_r_finger_link
+  l_gripper_r_finger_tip_link
   r_gripper_l_finger_link
+  r_gripper_l_finger_tip_link
   r_gripper_r_finger_link
+  r_gripper_r_finger_tip_link
   r_shoulder_pan_link
   r_shoulder_lift_link
   l_shoulder_pan_link
@@ -76,7 +84,7 @@
   
 
 ## The padding to be added for the body parts the robot can see
-self_see_padd: 0.1
+self_see_padd: 0.0
 
 ## The scaling to be considered for the body parts the robot can see
 self_see_scale: 1.0

Added: pkg/trunk/motion_planning/planning_environment/planning_environment.launch
===================================================================
--- pkg/trunk/motion_planning/planning_environment/planning_environment.launch	                        (rev 0)
+++ pkg/trunk/motion_planning/planning_environment/planning_environment.launch	2009-06-06 21:09:43 UTC (rev 16813)
@@ -0,0 +1,8 @@
+
+<launch> 
+
+	<param name="robotdesc/pr2" command="$(find xacro)/xacro.py '$(find pr2_defs)/robots/pr2.xacro.xml'" />
+	<rosparam command="load" ns="robotdesc/pr2_planning" file="$(find planning_environment)/planning.yaml" />
+	<rosparam command="load" ns="robotdesc/pr2_collision" file="$(find planning_environment)/collision_checks.yaml" />
+
+</launch>

Deleted: pkg/trunk/util/new_robot_self_filter/.build_version
===================================================================
--- pkg/trunk/util/new_robot_self_filter/.build_version	2009-06-06 17:46:09 UTC (rev 16812)
+++ pkg/trunk/util/new_robot_self_filter/.build_version	2009-06-06 21:09:43 UTC (rev 16813)
@@ -1 +0,0 @@
-https://ros.svn.sf.net/svnroot/personalrobots/pkg/trunk/util/new_robot_self_filter:0

Modified: pkg/trunk/util/new_robot_self_filter/self_see_filter.h
===================================================================
--- pkg/trunk/util/new_robot_self_filter/self_see_filter.h	2009-06-06 17:46:09 UTC (rev 16812)
+++ pkg/trunk/util/new_robot_self_filter/self_see_filter.h	2009-06-06 21:09:43 UTC (rev 16813)
@@ -34,7 +34,7 @@
 #include <ros/console.h>
 #include <robot_msgs/PointCloud.h>
 #include <planning_environment/robot_models.h>
-#include <collision_space/point_inclusion.h>
+#include <collision_space/bodies.h>
 #include <tf/transform_listener.h>
 #include <string>
 #include <algorithm>
@@ -71,7 +71,7 @@
   /** \brief Construct the filter */
   SelfFilter(void) : rm_("robot_description"), tf_(*ros::Node::instance(), true, ros::Duration(10))
   {
-      tf_.setExtrapolationLimit(ros::Duration().fromSec(10));
+      tf_.setExtrapolationLimit(ros::Duration().fromSec(10));      
   }
     
 
@@ -86,6 +86,10 @@
     
   virtual bool configure(void)
   {
+      // keep only the points that are outside of the robot
+      // for testing purposes this may be changed to true
+      invert_ = false;
+      
       std::vector<std::string> links = rm_.getSelfSeeLinks();
       double scale = rm_.getSelfSeeScale();
       double padd  = rm_.getSelfSeePadding();
@@ -120,7 +124,7 @@
       
       for (unsigned int i = 0 ; i < bodies_.size() ; ++i)
 	  ROS_INFO("Self see link %s with volume %f", bodies_[i].name.c_str(), bodies_[i].body->computeVolume());
-      
+
       return true;
   }
     
@@ -137,6 +141,7 @@
       {
 	  const unsigned int bs = bodies_.size();
 	  const unsigned int np = data_in.pts.size();
+	  std::vector<collision_space::bodies::BoundingSphere> bspheres(bs);
 	  
 	  // place the links in the frame of the pointcloud
 	  for (unsigned int i = 0 ; i < bs ; ++i)
@@ -147,8 +152,13 @@
 
 	      // set it for each body; we also include the offset specified in URDF
 	      bodies_[i].body->setPose(transf * bodies_[i].constTransf);
+	      bodies_[i].body->computeBoundingSphere(bspheres[i]);
 	  }
 	  
+	  // compute a sphere that bounds the entire robot
+	  collision_space::bodies::BoundingSphere bound;
+	  collision_space::bodies::mergeBoundingSpheres(bspheres, bound);	  
+
 	  // we now decide which points we keep
 	  std::vector<bool> keep(np);
 
@@ -157,9 +167,11 @@
 	  {
 	      btVector3 pt = btVector3(btScalar(data_in.pts[i].x), btScalar(data_in.pts[i].y), btScalar(data_in.pts[i].z));
 	      bool out = true;
-	      for (unsigned int j = 0 ; out && j < bs ; ++j)
-		  out = !bodies_[j].body->containsPoint(pt);
-	      keep[i] = out;
+	      if (bound.center.distance(pt) < bound.radius)
+		  for (unsigned int j = 0 ; out && j < bs ; ++j)
+		      out = !bodies_[j].body->containsPoint(pt);
+	      
+	      keep[i] = invert_ ? !out : out;
 	  }
 	  
 	  
@@ -199,10 +211,11 @@
 protected:
   
   planning_environment::RobotModels rm_;
-  
+  ros::NodeHandle                   nh_;  
   tf::TransformListener             tf_;
   std::vector<SeeLink>              bodies_;
-
+  bool                              invert_;
+  
 };
 
 typedef robot_msgs::PointCloud robot_msgs_PointCloud;

Modified: pkg/trunk/util/new_robot_self_filter/test_filter.cpp
===================================================================
--- pkg/trunk/util/new_robot_self_filter/test_filter.cpp	2009-06-06 17:46:09 UTC (rev 16812)
+++ pkg/trunk/util/new_robot_self_filter/test_filter.cpp	2009-06-06 21:09:43 UTC (rev 16813)
@@ -36,39 +36,95 @@
 
 #include <ros/ros.h>
 #include "self_see_filter.h"
+#include <visualization_msgs/Marker.h>
 
-class test
+class TestSelfFilter
 {
 public:
-    test(void)
+
+    TestSelfFilter(void)
     {
-	sf.configure();
-	sb = nh.subscribe("tilt_scan_filtered", 1, &test::myfilter, this);
-	pub = nh.advertise<robot_msgs::PointCloud>("my_point_cloud2", 10);
-
+	id_ = 1;
+	sf_.configure();
+	vmPub_ = nodeHandle_.advertise<visualization_msgs::Marker>("visualization_marker", 10240);
     }
     
-    void myfilter(const robot_msgs::PointCloudConstPtr &pc)
+    void sendPoint(double x, double y, double z)
     {
+	visualization_msgs::Marker mk;
+
+	mk.header.stamp = ros::Time::now();
+
+	mk.header.frame_id = "base_link";
+
+	mk.ns = "test_self_filter";
+	mk.id = id_++;
+	mk.type = visualization_msgs::Marker::SPHERE;
+	mk.action = visualization_msgs::Marker::ADD;
+	mk.pose.position.x = x;
+	mk.pose.position.y = y;
+	mk.pose.position.z = z;
+	mk.pose.orientation.w = 1.0;
+
+	mk.scale.x = mk.scale.y = mk.scale.z = 0.02;
+
+	mk.color.a = 1.0;
+	mk.color.r = 1.0;
+	mk.color.g = 0.04;
+	mk.color.b = 0.04;
+
+	vmPub_.publish(mk);
+    }
+    void run(void)
+    {
+	robot_msgs::PointCloud in;
 	robot_msgs::PointCloud out;
-	sf.update(*pc, out);
-	pub.publish(out);
+	
+	in.header.stamp = ros::Time::now();
+	in.header.frame_id = "base_link";
+	
+	const unsigned int N = 100000;	
+	in.pts.resize(N);
+	for (unsigned int i = 0 ; i < N ; ++i)
+	{
+	    in.pts[i].x = uniform(1);
+	    in.pts[i].y = uniform(1);
+	    in.pts[i].z = uniform(1);
+	}
+	
+	ros::WallTime tm = ros::WallTime::now();
+	sf_.update(in, out);
+	printf("%f points per second\n", (double)N/(ros::WallTime::now() - tm).toSec());
+	
+	for (unsigned int i = 0 ; i < out.pts.size() ; ++i)
+	{
+	    sendPoint(out.pts[i].x, out.pts[i].y, out.pts[i].z);
+	}
+	
+	ros::spin();	
     }
+protected:
+    
+    double uniform(double magnitude)
+    {
+	return (2.0 * drand48() - 1.0) * magnitude;
+    }
 
-    ros::Subscriber sb;
-    ros::Publisher pub;
-    ros::NodeHandle nh;        
-    filters::SelfFilter<robot_msgs::PointCloud> sf;
+    ros::Publisher                              vmPub_;
+    ros::NodeHandle                             nodeHandle_;        
+    filters::SelfFilter<robot_msgs::PointCloud> sf_;
+    int                                         id_;
 };
 
     
 int main(int argc, char **argv)
 {
-    ros::init(argc, argv, "ttt");
-    test t;
-    ros::spin();
-       
+    ros::init(argc, argv, "test_self_filter");
 
+    TestSelfFilter t;
+    sleep(1);
+    t.run();
+    
     return 0;
 }
 

Modified: pkg/trunk/world_models/collision_space/CMakeLists.txt
===================================================================
--- pkg/trunk/world_models/collision_space/CMakeLists.txt	2009-06-06 17:46:09 UTC (rev 16812)
+++ pkg/trunk/world_models/collision_space/CMakeLists.txt	2009-06-06 21:09:43 UTC (rev 16813)
@@ -7,7 +7,7 @@
 set(CMAKE_BUILD_TYPE Release)
 
 rospack_add_library(collision_space src/collision_space/output.cpp
-                                    src/collision_space/point_inclusion.cpp
+                                    src/collision_space/bodies.cpp
 				    src/collision_space/environment.cpp
 				    src/collision_space/environmentODE.cpp)	
 rospack_link_boost(collision_space thread)

Copied: pkg/trunk/world_models/collision_space/include/collision_space/bodies.h (from rev 16812, pkg/trunk/world_models/collision_space/include/collision_space/point_inclusion.h)
===================================================================
--- pkg/trunk/world_models/collision_space/include/collision_space/bodies.h	                        (rev 0)
+++ pkg/trunk/world_models/collision_space/include/collision_space/bodies.h	2009-06-06 21:09:43 UTC (rev 16813)
@@ -0,0 +1,320 @@
+/*********************************************************************
+* Software License Agreement (BSD License)
+* 
+*  Copyright (c) 2008, 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 the Willow Garage 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 Ioan Sucan */
+
+#ifndef COLLISION_SPACE_POINT_INCLUSION_
+#define COLLISION_SPACE_POINT_INCLUSION_
+
+#include <planning_models/shapes.h>
+#include <LinearMath/btTransform.h>
+#include <cstdlib>
+#include <vector>
+
+/**
+   This set of classes allows quickly detecting whether a given point
+   is inside an object or not. Only basic (simple) types of objects
+   are supported: spheres, cylinders, boxes. This capability is useful
+   when removing points from inside the robot (when the robot sees its
+   arms, for example).
+*/
+
+namespace collision_space
+{
+    
+    namespace bodies
+    {
+	
+	struct BoundingSphere
+	{
+	    btVector3 center;
+	    double    radius;
+	};
+		
+	/** A body is a shape + its pose. Point inclusion can be
+	    tested, volumes and bounding spheres can be computed.*/
+	class Body
+	{
+	public:
+	    
+	    Body(void)
+	    {
+		m_scale = 1.0;
+		m_padding = 0.0;
+		m_pose.setIdentity();
+	    }
+	    
+	    virtual ~Body(void)
+	    {
+	    }
+	    
+	    /** If the dimension of the body should be scaled, this
+		method sets the scale. Default is 1.0 */
+	    void setScale(double scale)
+	    {
+		m_scale = scale;
+		updateInternalData();
+	    }
+	    
+	    /** Retrieve the current scale */
+	    double getScale(void) const
+	    {
+		return m_scale;
+	    }
+	    
+	    /** If constant padding should be added to the body, this
+		method sets the padding. Default is 0.0 */
+	    void setPadding(double padd)
+	    {
+		m_padding = padd;
+		updateInternalData();
+	    }
+	    
+	    /** Retrieve the current padding */
+	    double getPadding(void) const
+	    {
+		return m_padding;
+	    }
+	    
+	    /** Set the pose of the body. Default is identity */
+	    void setPose(const btTransform &pose)
+	    {
+		m_pose = pose;
+		updateInternalData();
+	    }
+	    
+	    /** Retrieve the pose of the body */
+	    const btTransform& getPose(void) const
+	    {
+		return m_pose;
+	    }
+	    
+	    /** Set the dimensions of the body (from corresponding shape) */
+	    void setDimensions(const planning_models::shapes::Shape *shape)
+	    {
+		useDimensions(shape);
+		updateInternalData();
+	    }
+	    
+	    /** Check is a point is inside the body */
+	    bool containsPoint(double x, double y, double z) const
+	    {
+		return containsPoint(btVector3(btScalar(x), btScalar(y), btScalar(z)));
+	    }
+	    
+	    /** Check is a point is inside the body */
+	    virtual bool containsPoint(const btVector3 &p) const = 0;	
+	    
+	    /** Compute the volume of the body. This method includes
+		changes induced by scaling and padding */
+	    virtual double computeVolume(void) const = 0;
+	    
+	    /** Compute the bounding radius for the body, in its current
+		pose. Scaling and padding are accounted for. */
+	    virtual void computeBoundingSphere(BoundingSphere &sphere) const = 0;
+	    
+	protected:
+	    
+	    virtual void updateInternalData(void) = 0;
+	    virtual void useDimensions(const planning_models::shapes::Shape *shape) = 0;
+	    
+	    btTransform m_pose;	
+	    double      m_scale;
+	    double      m_padding;	    
+	};
+	
+	class Sphere : public Body
+	{
+	public:
+            Sphere(void) : Body()
+	    {
+		m_radius = 0.0;
+	    }
+	    
+	    Sphere(const planning_models::shapes::Shape *shape) : Body()
+	    {
+		setDimensions(shape);
+	    }
+	    
+	    virtual ~Sphere(void)
+	    {
+	    }
+	    
+	    virtual bool containsPoint(const btVector3 &p) const;
+	    virtual double computeVolume(void) const;
+	    virtual void computeBoundingSphere(BoundingSphere &sphere) const;
+
+	protected:
+	    
+	    virtual void useDimensions(const planning_models::shapes::Shape *shape);
+	    virtual void updateInternalData(void);
+	    
+	    btVector3 m_center;
+	    double    m_radius;	
+	    double    m_radiusU;
+	    double    m_radius2;		    
+	};
+        
+	class Cylinder : public Body
+	{
+	public:
+	    Cylinder(void) : Body()
+	    {
+		m_length = m_radius = 0.0;
+	    }
+
+	    Cylinder(const planning_models::shapes::Shape *shape) : Body()
+	    {
+		setDimensions(shape);
+	    }
+	    
+	    virtual ~Cylinder(void)
+	    {
+	    }
+	    
+	    virtual bool containsPoint(const btVector3 &p) const;
+	    virtual double computeVolume(void) const;
+	    virtual void computeBoundingSphere(BoundingSphere &sphere) const;
+
+	protected:
+	    
+	    virtual void useDimensions(const planning_models::shapes::Shape *shape);
+	    virtual void updateInternalData(void);
+
+	    btVector3 m_center;
+	    btVector3 m_normalH;
+	    btVector3 m_normalB1;
+	    btVector3 m_normalB2;
+	    
+	    double    m_length;
+	    double    m_length2;	
+	    double    m_radius;
+	    double    m_radiusU;
+	    double    m_radiusB;
+	    double    m_radius2;
+	};
+	
+	
+	class Box : public Body
+	{
+	public: 
+	    Box(void) : Body()
+	    {
+		m_length = m_width = m_height = 0.0;
+	    }
+	    
+	    Box(const planning_models::shapes::Shape *shape) : Body()
+	    {
+		setDimensions(shape);
+	    }
+	    
+	    virtual ~Box(void)
+	    {
+	    }
+	    
+	    virtual bool containsPoint(const btVector3 &p) const;
+	    virtual double computeVolume(void) const;
+	    virtual void computeBoundingSphere(BoundingSphere &sphere) const;
+
+	protected:
+	    
+	    virtual void useDimensions(const planning_models::shapes::Shape *shape); // (x, y, z) = (length, width, height)	    
+	    virtual void updateInternalData(void);
+	    
+	    btVector3 m_center;
+	    btVector3 m_normalL;
+	    btVector3 m_normalW;
+	    btVector3 m_normalH;
+	    
+	    double    m_length;
+	    double    m_width;
+	    double    m_height;	
+	    double    m_length2;
+	    double    m_width2;
+	    double    m_height2;	
+	    double    m_radiusB;
+	};
+	
+
+	class ConvexMesh : public Body
+	{
+	public:
+	    
+	    ConvexMesh(void) : Body()
+	    {
+		m_meshCenter.setValue(btScalar(0), btScalar(0), btScalar(0));
+	    }
+
+	    ConvexMesh(const planning_models::shapes::Shape *shape) : Body()
+	    {
+		m_meshCenter.setValue(btScalar(0), btScalar(0), btScalar(0));
+		setDimensions(shape);
+	    }
+	    
+	    virtual ~ConvexMesh(void)
+	    {
+	    }
+	    
+	    virtual bool containsPoint(const btVector3 &p) const;
+	    virtual double computeVolume(void) const;
+	    virtual void computeBoundingSphere(BoundingSphere &sphere) const;
+
+	protected:
+	    
+	    virtual void useDimensions(const planning_models::shapes::Shape *shape);
+	    virtual void updateInternalData(void);
+	    
+	    unsigned int countVerticesBehindPlane(const btVector4& planeNormal) const;
+	    bool isPointInsidePlanes(const btVector3& point) const;
+	    
+	    std::vector<btVector4>    m_planes;
+	    std::vector<btVector3>    m_vertices;
+	    std::vector<unsigned int> m_triangles;
+	    btTransform               m_iPose;
+
+	    btVector3                 m_center;
+	    btVector3                 m_meshCenter;
+	    double                    m_radiusB;
+	};
+	
+	
+	/** Create a body from a given shape */
+	Body* createBodyFromShape(const planning_models::shapes::Shape *shape);
+
+	/** Compute a bounding sphere to enclose a set of bounding spheres */
+	void mergeBoundingSpheres(const std::vector<BoundingSphere> &spheres, BoundingSphere &mergedSphere);
+    }
+}
+
+#endif


Property changes on: pkg/trunk/world_models/collision_space/include/collision_space/bodies.h
___________________________________________________________________
Added: svn:mergeinfo
   + 

Deleted: 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-06-06 17:46:09 UTC (rev 16812)
+++ pkg/trunk/world_models/collision_space/include/collision_space/point_inclusion.h	2009-06-06 21:09:43 UTC (rev 16813)
@@ -1,311 +0,0 @@
-/*********************************************************************
-* Software License Agreement (BSD License)
-* 
-*  Copyright (c) 2008, 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 the Willow Garage 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 Ioan Sucan */
-
-#ifndef COLLISION_SPACE_POINT_INCLUSION_
-#define COLLISION_SPACE_POINT_INCLUSION_
-
-#include <planning_models/shapes.h>
-#include <LinearMath/btTransform.h>
-#include <cstdlib>
-#include <vector>
-
-/**
-   This set of classes allows quickly detecting whether a given point
-   is inside an object or not. Only basic (simple) types of objects
-   are supported: spheres, cylinders, boxes. This capability is useful
-   when removing points from inside the robot (when the robot sees its
-   arms, for example).
-*/
-
-namespace collision_space
-{
-    
-    namespace bodies
-    {
-	
-	/** A body is a shape + its pose. Point inclusion can be
-	    tested, volumes and bounding spheres can be computed.*/
-	class Body
-	{
-	public:
-	    
-	    Body(void)
-	    {
-		m_scale = 1.0;
-		m_padding = 0.0;
-		m_pose.setIdentity();
-	    }
-	    
-	    virtual ~Body(void)
-	    {
-	    }
-	    
-	    /** If the dimension of the body should be scaled, this
-		method sets the scale. Default is 1.0 */
-	    void setScale(double scale)
-	    {
-		m_scale = scale;
-		updateInternalData();
-	    }
-	    
-	    /** Retrieve the current scale */
-	    double getScale(void) const
-	    {
-		return m_scale;
-	    }
-	    
-	    /** If constant padding should be added to the body, this
-		method sets the padding. Default is 0.0 */
-	    void setPadding(double padd)
-	    {
-		m_padding = padd;
-		updateInternalData();
-	    }
-	    
-	    /** Retrieve the current padding */
-	    double getPadding(void) const
-	    {
-		return m_padding;
-	    }
-	    
-	    /** Set the pose of the body. Default is identity */
-	    void setPose(const btTransform &pose)
-	    {
-		m_pose = pose;
-		updateInternalData();
-	    }
-	    
-	    /** Retrieve the pose of the body */
-	    const btTransform& getPose(void) const
-	    {
-		return m_pose;
-	    }
-	    
-	    /** Set the dimensions of the body (from corresponding shape) */
-	    void setDimensions(const planning_models::shapes::Shape *shape)
-	    {
-		useDimensions(shape);
-		updateInternalData();
-	    }
-	    
-	    /** Check is a point is inside the body */
-	    bool containsPoint(double x, double y, double z) const
-	    {
-		return containsPoint(btVector3(btScalar(x), btScalar(y), btScalar(z)));
-	    }
-	    
-	    /** Check is a point is inside the body */
-	    virtual bool containsPoint(const btVector3 &p) const = 0;	
-	    
-	    /** Compute the volume of the body. This method includes
-		changes induced by scaling and padding */
-	    virtual double computeVolume(void) const = 0;
-	    
-	    /** Compute the bounding radius for the body, in its current
-		pose. Scaling and padding are accounted for. */
-	    virtual void computeBoundingSphere(btVector3 &center, double &radius) const = 0;
-	    
-	protected:
-	    
-	    virtual void updateInternalData(void) = 0;
-	    virtual void useDimensions(const planning_models::shapes::Shape *shape) = 0;
-	    
-	    btTransform m_pose;	
-	    double      m_scale;
-	    double      m_padding;	    
-	};
-	
-	class Sphere : public Body
-	{
-	public:
-            Sphere(void) : Body()
-	    {
-		m_radius = 0.0;
-	    }
-	    
-	    Sphere(const planning_models::shapes::Shape *shape) : Body()
-	    {
-		setDimensions(shape);
-	    }
-	    
-	    virtual ~Sphere(void)
-	    {
-	    }
-	    
-	    virtual bool containsPoint(const btVector3 &p) const;
-	    virtual double computeVolume(void) const;
-	    virtual void computeBoundingSphere(btVector3 &center, double &radius) const;
-
-	protected:
-	    
-	    virtual void useDimensions(const planning_models::shapes::Shape *shape);
-	    virtual void updateInternalData(void);
-	    
-	    btVector3 m_center;
-	    double    m_radius;	
-	    double    m_radiusU;
-	    double    m_radius2;		    
-	};
-        
-	class Cylinder : public Body
-	{
-	public:
-	    Cylinder(void) : Body()
-	    {
-		m_length = m_radius = 0.0;
-	    }
-
-	    Cylinder(const planning_models::shapes::Shape *shape) : Body()
-	    {
-		setDimensions(shape);
-	    }
-	    
-	    virtual ~Cylinder(void)
-	    {
-	    }
-	    
-	    virtual bool containsPoint(const btVector3 &p) const;
-	    virtual double computeVolume(void) const;
-	    virtual void computeBoundingSphere(btVector3 &center, double &radius) const;
-
-	protected:
-	    
-	    virtual void useDimensions(const planning_models::shapes::Shape *shape);
-	    virtual void updateInternalData(void);
-
-	    btVector3 m_center;
-	    btVector3 m_normalH;
-	    btVector3 m_normalB1;
-	    btVector3 m_normalB2;
-	    
-	    double    m_length;
-	    double    m_length2;	
-	    double    m_radius;
-	    double    m_radiusU;
-	    double    m_radiusB;
-	    double    m_radius2;
-	};
-	
-	
-	class Box : public Body
-	{
-	public: 
-	    Box(void) : Body()
-	    {
-		m_length = m_width = m_height = 0.0;
-	    }
-	    
-	    Box(const planning_models::shapes::Shape *shape) : Body()
-	    {
-		setDimensions(shape);
-	    }
-	    
-	    virtual ~Box(void)
-	    {
-	    }
-	    
-	    virtual bool containsPoint(const btVector3 &p) const;
-	    virtual double computeVolume(void) const;
-	    virtual void computeBoundingSphere(btVector3 &center, double &radius) const;
-
-	protected:
-	    
-	    virtual void useDimensions(const planning_models::shapes::Shape *shape); // (x, y, z) = (length, width, height)	    
-	    virtual void updateInternalData(void);
-	    
-	    btVector3 m_center;
-	    btVector3 m_normalL;
-	    btVector3 m_normalW;
-	    btVector3 m_normalH;
-	    
-	    double    m_length;
-	    double    m_width;
-	    double    m_height;	
-	    double    m_length2;
-	    double    m_width2;
-	    double    m_height2;	
-	    double    m_radiusB;
-	};
-	
-
-	class ConvexMesh : public Body
-	{
-	public:
-	    
-	    ConvexMesh(void) : Body()
-	    {
-		m_meshCenter.setValue(btScalar(0), btScalar(0), btScalar(0));
-	    }
-
-	    ConvexMesh(const planning_models::shapes::Shape *shape) : Body()
-	    {
-		m_meshCenter.setValue(btScalar(0), btScalar(0), btScalar(0));
-		setDimensions(shape);
-	    }
-	    
-	    virtual ~ConvexMesh(void)
-	    {
-	    }
-	    
-	    virtual bool containsPoint(const btVector3 &p) const;
-	    virtual double computeVolume(void) const;
-	    virtual void computeBoundingSphere(btVector3 &center, double &radius) const;
-
-	protected:
-	    
-	    virtual void useDimensions(const planning_models::shapes::Shape *shape);
-	    virtual void updateInternalData(void);
-	    
-	    unsigned int countVerticesBehindPlane(const btVector4& planeNormal) const;
-	    bool isPointInsidePlanes(const btVector3& point) const;
-	    
-	    std::vector<btVector4>    m_planes;
-	    std::vector<btVector3>    m_vertices;
-	    std::vector<unsigned int> m_triangles;
-	    btTransform               m_iPose;
-
-	    btVector3                 m_center;
-	    btVector3                 m_meshCenter;
-	    double                    m_radiusB;
-	};
-	
-	
-	Body* createBodyFromShape(const planning_models::shapes::Shape *shape);
-	
-    }
-}
-
-#endif

Copied: pkg/trunk/world_models/collision_space/src/collision_space/bodies.cpp (from rev 16812, pkg/trunk/world_models/collision_space/src/collision_space/point_inclusion.cpp)
===================================================================
--- pkg/trunk/world_models/collision_space/src/collision_space/bodies.cpp	                        (rev 0)
+++ pkg/trunk/world_models/collision_space/src/collision_space/bodies.cpp	2009-06-06 21:09:43 UTC (rev 16813)
@@ -0,0 +1,383 @@
+/*********************************************************************
+* Software License Agreement (BSD License)
+* 
+*  Copyright (c) 2008, 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 the Willow Garage 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 Ioan Sucan */
+
+#include "collision_space/bodies.h"
+#include "collision_space/output.h"
+#include <LinearMath/btConvexHull.h>
+#include <cmath>
+
+
+static collision_space::msg::Interface MSG;
+
+collision_space::bodies::Body* collision_space::bodies::createBodyFromShape(const planning_models::shapes::Shape *shape)
+{
+    Body *body = NULL;
+    
+    switch (shape->type)
+    {
+    case planning_models::shapes::Shape::BOX:
+	body = new collision_space::bodies::Box(shape);
+	break;
+    case planning_models::shapes::Shape::SPHERE:
+	body = new collision_space::bodies::Sphere(shape);
+	break;
+    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;
+    }
+    
+    return body;
+}
+
+void collision_space::bodies::mergeBoundingSpheres(const std::vector<BoundingSphere> &spheres, BoundingSphere &mergedSphere)
+{
+    if (spheres.empty())
+    {
+	mergedSphere.center.setValue(btScalar(0), btScalar(0), btScalar(0));
+	mergedSphere.radius = 0.0;
+    }
+    else
+    {
+	mergedSphere = spheres[0];
+	for (unsigned int i = 1 ; i < spheres.size() ; ++i)
+	{
+	    if (spheres[i].radius <= 0.0)
+		continue;
+	    double d = spheres[i].center.distance(mergedSphere.center);
+	    if (d + mergedSphere.radius <= spheres[i].radius)
+	    {
+		mergedSphere.center = spheres[i].center;
+		mergedSphere.radius = spheres[i].radius;
+	    }
+	    else
+		if (d + spheres[i].radius > mergedSphere.radius)
+		{
+		    btVector3 delta = mergedSphere.center - spheres[i].center;
+		    mergedSphere.radius = (delta.length() + spheres[i].radius + mergedSphere.radius)/2.0;
+		    mergedSphere.center = delta.normalized() * (mergedSphere.radius - spheres[i].radius) + spheres[i].center;
+		}
+	}
+    }
+}
+
+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_radiusU = m_radius * m_scale + m_padding;
+    m_radius2 = m_radiusU * m_radiusU;
+    m_center = m_pose.getOrigin();
+}
+
+double collision_space::bodies::Sphere::computeVolume(void) const
+{
+    return 4.0 * M_PI * m_radiusU * m_radiusU * m_radiusU / 3.0;
+}
+
+void collision_space::bodies::Sphere::computeBoundingSphere(BoundingSphere &sphere) const
+{
+    sphere.center = m_center;
+    sphere.radius = m_radiusU;
+}
+
+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_radiusU = m_radius * m_scale + m_padding;
+    m_radius2 = m_radiusU * m_radiusU;
+    m_length2 = m_scale * m_length / 2.0 + m_padding;
+    m_center = m_pose.getOrigin();
+    m_radiusB = std::max(m_radiusU, m_length2);
+    
+    const btMatrix3x3& basis = m_pose.getBasis();
+    m_normalB1 = basis.getColumn(0);
+    m_normalB2 = basis.getColumn(1);
+    m_normalH  = basis.getColumn(2);
+}
+
+double collision_space::bodies::Cylinder::computeVolume(void) const
+{
+    return 2.0 * M_PI * m_radiusU * m_radiusU * m_length2;
+}
+
+void collision_space::bodies::Cylinder::computeBoundingSphere(BoundingSphere &sphere) const
+{
+    sphere.center = m_center;
+    sphere.radius = m_radiusB;
+}
+
+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;
+    m_length2 = m_length * s2 + m_padding;
+    m_width2  = m_width * s2 + m_padding;
+    m_height2 = m_height * s2 + m_padding;
+    
+    m_center  = m_pose.getOrigin();
+    
+    m_radiusB = sqrt(m_length2 * m_length2 + m_width2 * m_width2 + m_height2 * m_height2);
+
+    const btMatrix3x3& basis = m_pose.getBasis();
+    m_normalL = basis.getColumn(0);
+    m_normalW = basis.getColumn(1);
+    m_normalH = basis.getColumn(2);
+}
+
+double collision_space::bodies::Box::computeVolume(void) const
+{
+    return 8.0 * m_length2 * m_width2 * m_height2;
+}
+
+void collision_space::bodies::Box::computeBoundingSphere(BoundingSphere &sphere) const
+{
+    sphere.center = m_center;
+    sphere.radius = m_radiusB;
+}
+
+bool collision_space::bodies::ConvexMesh::containsPoint(const btVector3 &p) const
+{
+    btVector3 ip = (m_iPose * p) / m_scale;
+    return isPointInsidePlanes(ip);
+}
+
+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();
+    m_triangles.clear();
+    m_vertices.clear();
+    m_radiusB = 0.0;
+    m_meshCenter.setValue(btScalar(0), btScalar(0), btScalar(0));
+
+    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)
+    {
+	MSG.inform("Convex hull has %d(%d) vertices (down from %d), %d faces", hr.m_OutputVertices.size(), hr.mNumOutputVertices, mesh->vertexCount, hr.mNumFaces);
+
+	m_vertices.reserve(hr.m_OutputVertices.size());
+	btVector3 sum(btScalar(0), btScalar(0), btScalar(0));	
+	for (int j = 0 ; j < hr.m_OutputVertices.size() ; ++j)
+	{
+	    m_vertices.push_back(hr.m_OutputVertices[j]);
+	    sum = sum + hr.m_OutputVertices[j];
+	}
+	
+	m_meshCenter = sum / (double)(hr.m_OutputVertices.size());
+	for (unsigned int j = 0 ; j < m_vertices.size() ; ++j)
+	{
+	    double dist = m_vertices[j].distance2(m_meshCenter);
+	    if (dist > m_radiusB)
+		m_radiusB = dist;
+	}
+	m_radiusB = sqrt(m_radiusB) * m_scale + m_padding;
+	
+	m_triangles.reserve(hr.m_Indices.size());
+	for (int j = 0 ; j < hr.m_Indices.size() ; ++j)
+	    m_triangles.push_back(hr.m_Indices[j]);
+	
+	for (unsigned int j = 0 ; j < hr.mNumFaces ; ++j)
+	{
+	    const btVector3 &p1 = hr.m_OutputVertices[hr.m_Indices[j * 3    ]];
+	    const btVector3 &p2 = hr.m_OutputVertices[hr.m_Indices[j * 3 + 1]];
+	    const btVector3 &p3 = hr.m_OutputVertices[hr.m_Indices[j * 3 + 2]];
+	    
+	    btVector3 edge1 = (p2 - p1);
+	    btVector3 edge2 = (p3 - p1);
+	    
+	    edge1.normalize();
+	    edge2.normalize();
+
+	    btVector3 planeNormal = edge1.cross(edge2);
+	    
+	    if (planeNormal.length2() > btScalar(1e-6))
+	    {
+		planeNormal.normalize();
+		btVector4 planeEquation(planeNormal.getX(), planeNormal.getY(), planeNormal.getZ(), -planeNormal.dot(p1));
+
+		unsigned int behindPlane = countVerticesBehindPlane(planeEquation);
+		if (behindPlane > 0)
+		{
+		    btVector4 planeEquation2(-planeEquation.getX(), -planeEquation.getY(), -planeEquation.getZ(), -planeEquation.getW());
+		    unsigned int behindPlane2 = countVerticesBehindPlane(planeEquation2);
+		    if (behindPlane2 < behindPlane)
+		    {
+			planeEquation.setValue(planeEquation2.getX(), planeEquation2.getY(), planeEquation2.getZ(), planeEquation2.getW());
+			behindPlane = behindPlane2;
+		    }
+		}
+		
+		if (behindPlane > 0)
+		    MSG.warn("Approximate plane: %u of %u points are behind the plane", behindPlane, (unsigned int)m_vertices.size());
+		
+		m_planes.push_back(planeEquation);
+	    }
+	}
+    }
+    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();
+    m_center = m_pose.getOrigin() + m_meshCenter;
+}
+
+void collision_space::bodies::ConvexMesh::computeBoundingSphere(BoundingSphere &sphere) const
+{
+    sphere.center = m_center;
+    sphere.radius = m_radiusB;
+}
+
+bool collision_space::bodies::ConvexMesh::isPointInsidePlanes(const btVector3& point) const
+{
+    unsigned int numplanes = m_planes.size();
+    for (unsigned int i = 0 ; i < numplanes ; ++i)
+    {
+	const btVector4& plane = m_planes[i];
+	btScalar dist = btScalar(plane.dot(point)) + plane.getW() - btScalar(m_padding) - btScalar(1e-6);
+	if (dist > btScalar(0))
+	    return false;
+    }
+    return true;
+}
+
+unsigned int collision_space::bodies::ConvexMesh::countVerticesBehindPlane(const btVector4& planeNormal) const
+{
+    unsigned int numvertices = m_vertices.size();
+    unsigned int result = 0;
+    for (unsigned int i = 0 ; i < numvertices ; ++i)
+    {
+	btScalar dist = btScalar(planeNormal.dot(m_vertices[i])) + btScalar(planeNormal.getW()) - btScalar(1e-6);
+	if (dist > btScalar(0))
+	    result++;
+    }
+    return result;
+}
+
+double collision_space::bodies::ConvexMesh::computeVolume(void) const
+{
+    double volume = 0.0;
+    for (unsigned int i = 0 ; i < m_triangles.size() / 3 ; ++i)
+    {
+	const btVector3 &v1 = m_vertices[m_triangles[3*i + 0]];
+        const btVector3 &v2 = m_vertices[m_triangles[3*i + 1]];
+	const btVector3 &v3 = m_vertices[m_triangles[3*i + 2]];
+	volume += v1.x()*v2.y()*v3.z() + v2.x()*v3.y()*v1.z() + v3.x()*v1.y()*v2.z() -v1.x()*v3.y()*v2.z() - v2.x()*v1.y()*v3.z() - v3.x()*v2.y()*v1.z();
+    }
+    return fabs(volume)/6.0;
+}


Property changes on: pkg/trunk/world_models/collision_space/src/collision_space/bodies.cpp
___________________________________________________________________
Added: svn:mergeinfo
   + 

Deleted: 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-06-06 17:46:09 UTC (rev 16812)
+++ pkg/trunk/world_models/collision_space/src/collision_space/point_inclusion.cpp	2009-06-06 21:09:43 UTC (rev 16813)
@@ -1,352 +0,0 @@
-/*********************************************************************
-* Software License Agreement (BSD License)
-* 
-*  Copyright (c) 2008, 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 the Willow Garage 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 Ioan Sucan */
-
-#include "collision_space/point_inclusion.h"
-#include "collision_space/output.h"
-#include <LinearMath/btConvexHull.h>
-#include <cmath>
-
-
-static collision_space::msg::Interface MSG;
-
-collision_space::bodies::Body* collision_space::bodies::createBodyFromShape(const planning_models::shapes::Shape *shape)
-{
-    Body *body = NULL;
-    
-    switch (shape->type)
-    {
-    case planning_models::shapes::Shape::BOX:
-	body = new collision_space::bodies::Box(shape);
-	break;
-    case planning_models::shapes::Shape::SPHERE:
-	body = new collision_space::bodies::Sphere(shape);
-	break;
-    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;
-    }
-    
-    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_radiusU = m_radius * m_scale + m_padding;
-    m_radius2 = m_radiusU * m_radiusU;
-    m_center = m_pose.getOrigin();
-}
-
-double collision_space::bodies::Sphere::computeVolume(void) const
-{
-    return 4.0 * M_PI * m_radiusU * m_radiusU * m_radiusU / 3.0;
-}
-
-void collision_space::bodies::Sphere::computeBoundingSphere(btVector3 &center, double &radius) const
-{
-    center = m_center;
-    radius = m_radiusU;
-}
-
-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_radiusU = m_radius * m_scale + m_padding;
-    m_radius2 = m_radiusU * m_radiusU;
-    m_length2 = m_scale * m_length / 2.0 + m_padding;
-    m_center = m_pose.getOrigin();
-    m_radiusB = std::max(m_radiusU, m_length2);
-    
-    const btMatrix3x3& basis = m_pose.getBasis();
-    m_normalB1 = basis.getColumn(0);
-    m_normalB2 = basis.getColumn(1);
-    m_normalH  = basis.getColumn(2);
-}
-
-double collision_space::bodies::Cylinder::computeVolume(void) const
-{
-    return 2.0 * M_PI * m_radiusU * m_radiusU * m_length2;
-}
-
-void collision_space::bodies::Cylinder::computeBoundingSphere(btVector3 &center, double &radius) const
-{
-    center = m_center;
-    radius = m_radiusB;
-}
-
-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;
-    m_length2 = m_length * s2 + m_padding;
-    m_width2  = m_width * s2 + m_padding;
-    m_height2 = m_height * s2 + m_padding;
-    
-    m_center  = m_pose.getOrigin();
-    
-    m_radiusB = sqrt(m_length2 * m_length2 + m_width2 * m_width2 + m_height2 * m_height2);
-
-    const btMatrix3x3& basis = m_pose.getBasis();
-    m_normalL = basis.getColumn(0);
-    m_normalW = basis.getColumn(1);
-    m_normalH = basis.getColumn(2);
-}
-
-double collision_space::bodies::Box::computeVolume(void) const
-{
-    return 8.0 * m_length2 * m_width2 * m_height2;
-}
-
-void collision_space::bodies::Box::computeBoundingSphere(btVector3 &center, double &radius) const
-{
-    center = m_center;
-    radius = m_radiusB;
-}
-
-bool collision_space::bodies::ConvexMesh::containsPoint(const btVector3 &p) const
-{
-    btVector3 ip = (m_iPose * p) / m_scale;
-    return isPointInsidePlanes(ip);
-}
-
-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();
-    m_triangles.clear();
-    m_vertices.clear();
-    m_radiusB = 0.0;
-    m_meshCenter.setValue(btScalar(0), btScalar(0), btScalar(0));
-
-    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)
-    {
-	MSG.inform("Convex hull has %d(%d) vertices (down from %d), %d faces", hr.m_OutputVertices.size(), hr.mNumOutputVertices, mesh->vertexCount, hr.mNumFaces);
-
-	m_vertices.reserve(hr.m_OutputVertices.size());
-	btVector3 sum(btScalar(0), btScalar(0), btScalar(0));	
-	for (int j = 0 ; j < hr.m_OutputVertices.size() ; ++j)
-	{
-	    m_vertices.push_back(hr.m_OutputVertices[j]);
-	    sum = sum + hr.m_OutputVertices[j];
-	}
-	
-	m_meshCenter = sum / (double)(hr.m_OutputVertices.size());
-	for (unsigned int j = 0 ; j < m_vertices.size() ; ++j)
-	{
-	    double dist = m_vertices[j].distance2(m_meshCenter);
-	    if (dist > m_radiusB)
-		m_radiusB = dist;
-	}
-	m_radiusB = sqrt(m_radiusB) * m_scale + m_padding;
-	
-	m_triangles.reserve(hr.m_Indices.size());
-	for (int j = 0 ; j < hr.m_Indices.size() ; ++j)
-	    m_triangles.push_back(hr.m_Indices[j]);
-	
-	for (unsigned int j = 0 ; j < hr.mNumFaces ; ++j)
-	{
-	    const btVector3 &p1 = hr.m_OutputVertices[hr.m_Indices[j * 3    ]];
-	    const btVector3 &p2 = hr.m_OutputVertices[hr.m_Indices[j * 3 + 1]];
-	    const btVector3 &p3 = hr.m_OutputVertices[hr.m_Indices[j * 3 + 2]];
-	    
-	    btVector3 edge1 = (p2 - p1);
-	    btVector3 edge2 = (p3 - p1);
-	    
-	    edge1.normalize();
-	    edge2.normalize();
-
-	    btVector3 planeNormal = edge1.cross(edge2);
-	    
-	    if (planeNormal.length2() > btScalar(1e-6))
-	    {
-		planeNormal.normalize();
-		btVector4 planeEquation(planeNormal.getX(), planeNormal.getY(), planeNormal.getZ(), -planeNormal.dot(p1));
-
-		unsigned int behindPlane = countVerticesBehindPlane(planeEquation);
-		if (behindPlane > 0)
-		{
-		    btVector4 planeEquation2(-planeEquation.getX(), -planeEquation.getY(), -planeEquation.getZ(), -planeEquation.getW());
-		    unsigned int behindPlane2 = countVerticesBehindPlane(planeEquation2);
-		    if (behindPlane2 < behindPlane)
-		    {
-			planeEquation.setValue(planeEquation2.getX(), planeEquation2.getY(), planeEquation2.getZ(), planeEquation2.getW());
-			behindPlane = behindPlane2;
-		    }
-		}
-		
-		if (behindPlane > 0)
-		    MSG.warn("Approximate plane: %u of %u points are behind the plane", behindPlane, (unsigned int)m_vertices.size());
-		
-		m_planes.push_back(planeEquation);
-	    }
-	}
-    }
-    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();
-    m_center = m_pose.getOrigin() + m_meshCenter;
-}
-
-void collision_space::bodies::ConvexMesh::computeBoundingSphere(btVector3 &center, double &radius) const
-{
-    center = m_center;
-    radius = m_radiusB;
-}
-
-bool collision_space::bodies::ConvexMesh::isPointInsidePlanes(const btVector3& point) const
-{
-    unsigned int numplanes = m_planes.size();
-    for (unsigned int i = 0 ; i < numplanes ; ++i)
-    {
-	const btVector4& plane = m_planes[i];
-	btScalar dist = btScalar(plane.dot(point)) + plane.getW() - btScalar(m_padding) - btScalar(1e-6);
-	if (dist > btScalar(0))
-	    return false;
-    }
-    return true;
-}
-
-unsigned int collision_space::bodies::ConvexMesh::countVerticesBehindPlane(const btVector4& planeNormal) const
-{
-    unsigned int numvertices = m_vertices.size();
-    unsigned int result = 0;
-    for (unsigned int i = 0 ; i < numvertices ; ++i)
-    {
-	btScalar dist = btScalar(planeNormal.dot(m_vertices[i])) + btScalar(planeNormal.getW()) - btScalar(1e-6);
-	if (dist > btScalar(0))
-	    result++;
-    }
-    return result;
-}
-
-double collision_space::bodies::ConvexMesh::computeVolume(void) const
-{
-    double volume = 0.0;
-    for (unsigned int i = 0 ; i < m_triangles.size() / 3 ; ++i)
-    {
-	const btVector3 &v1 = m_vertices[m_triangles[3*i + 0]];
-        const btVector3 &v2 = m_vertices[m_triangles[3*i + 1]];
-	const btVector3 &v3 = m_vertices[m_triangles[3*i + 2]];
-	volume += v1.x()*v2.y()*v3.z() + v2.x()*v3.y()*v1.z() + v3.x()*v1.y()*v2.z() -v1.x()*v3.y()*v2.z() - v2.x()*v1.y()*v3.z() - v3.x()*v2.y()*v1.z();
-    }
-    return fabs(volume)/6.0;
-}

Modified: pkg/trunk/world_models/collision_space/test/test_point_inclusion.cpp
===================================================================
--- pkg/trunk/world_models/collision_space/test/test_point_inclusion.cpp	2009-06-06 17:46:09 UTC (rev 16812)
+++ pkg/trunk/world_models/collision_space/test/test_point_inclusion.cpp	2009-06-06 21:09:43 UTC (rev 16813)
@@ -34,7 +34,7 @@
 
 /** \Author Ioan Sucan */
 
-#include <collision_space/point_inclusion.h>
+#include <collision_space/bodies.h>
 #include <gtest/gtest.h>
 
 TEST(SpherePointContainment, SimpleInside)

Modified: pkg/trunk/world_models/test_collision_space/src/test_cs_point_inclusion.cpp
===================================================================
--- pkg/trunk/world_models/test_collision_space/src/test_cs_point_inclusion.cpp	2009-06-06 17:46:09 UTC (rev 16812)
+++ pkg/trunk/world_models/test_collision_space/src/test_cs_point_inclusion.cpp	2009-06-06 21:09:43 UTC (rev 16813)
@@ -38,7 +38,7 @@
 #include <ros/ros.h>
 #include <algorithm>
 #include <visualization_msgs/Marker.h>
-#include <collision_space/point_inclusion.h>
+#include <collision_space/bodies.h>
 using namespace collision_space;
 
 const int TEST_TIMES  = 3;
@@ -152,11 +152,10 @@
 	collision_space::bodies::Body *s = new collision_space::bodies::Sphere(&shape);
 	printf("Sphere volume = %f\n", s->computeVolume());
 	
-	btVector3 center;
-	double radius;
-	s->computeBoundingSphere(center, radius);
+	collision_space::bodies::BoundingSphere sphere;
+	s->computeBoundingSphere(sphere);
 	
-	printf("Bounding radius = %f\n", radius);
+	printf("Bounding radius = %f\n", sphere.radius);
 	for (int i = 0 ; i < TEST_TIMES ; ++i)
 	{
 	    visualization_msgs::Marker mk;
@@ -181,11 +180,10 @@
 	collision_space::bodies::Body *s = new collision_space::bodies::Box(&shape);
 	printf("Box volume = %f\n", s->computeVolume());
 	
-	btVector3 center;
-	double radius;
-	s->computeBoundingSphere(center, radius);
+	collision_space::bodies::BoundingSphere sphere;
+	s->computeBoundingSphere(sphere);
 	
-	printf("Bounding radius = %f\n", radius);
+	printf("Bounding radius = %f\n", sphere.radius);
 
 	for (int i = 0 ; i < TEST_TIMES ; ++i)
 	{
@@ -210,12 +208,11 @@
 	planning_models::shapes::Cylinder shape(0.5, 2.5);
 	collision_space::bodies::Body *s = new collision_space::bodies::Cylinder(&shape);
 	printf("Cylinder volume = %f\n", s->computeVolume());
+		
+	collision_space::bodies::BoundingSphere sphere;
+	s->computeBoundingSphere(sphere);
 	
-	btVector3 center;
-	double radius;
-	s->computeBoundingSphere(center, radius);
-	
-	printf("Bounding radius = %f\n", radius);
+	printf("Bounding radius = %f\n", sphere.radius);
 
 	for (int i = 0 ; i < TEST_TIMES ; ++i)
 	{
@@ -264,11 +261,10 @@
 	collision_space::bodies::Body *s = new collision_space::bodies::ConvexMesh(shape);
 	printf("Mesh volume = %f\n", s->computeVolume());
 	
-	btVector3 center;
-	double radius;
-	s->computeBoundingSphere(center, radius);
+	collision_space::bodies::BoundingSphere sphere;
+	s->computeBoundingSphere(sphere);
 	
-	printf("Bounding radius = %f\n", radius);
+	printf("Bounding radius = %f\n", sphere.radius);
 
 	for (int i = 0 ; i < TEST_TIMES ; ++i)
 	{


This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.