Menu
▾
▴
[Personalrobots-commit] SF.net SVN: personalrobots:[2544] pkg/trunk
|
From: <is...@us...> - 2008-08-05 04:11:55
|
Revision: 2544
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=2544&view=rev
Author: isucan
Date: 2008-08-05 04:12:01 +0000 (Tue, 05 Aug 2008)
Log Message:
-----------
code for checking whether a point is inside a box or a cylinder
Modified Paths:
--------------
pkg/trunk/util/collision_space/include/collision_space/environment.h
pkg/trunk/util/collision_space/include/collision_space/environmentODE.h
pkg/trunk/world_models/world_3d_map/manifest.xml
pkg/trunk/world_models/world_3d_map/src/world_3d_map.cpp
Added Paths:
-----------
pkg/trunk/util/collision_space/include/collision_space/util.h
Modified: pkg/trunk/util/collision_space/include/collision_space/environment.h
===================================================================
--- pkg/trunk/util/collision_space/include/collision_space/environment.h 2008-08-05 03:59:39 UTC (rev 2543)
+++ pkg/trunk/util/collision_space/include/collision_space/environment.h 2008-08-05 04:12:01 UTC (rev 2544)
@@ -32,8 +32,8 @@
* POSSIBILITY OF SUCH DAMAGE.
*********************************************************************/
-#ifndef KINEMATIC_ENVIRONMENT_MODEL_
-#define KINEMATIC_ENVIRONMENT_MODEL_
+#ifndef COLLISION_SPACE_ENVIRONMENT_MODEL_
+#define COLLISION_SPACE_ENVIRONMENT_MODEL_
#include <planning_models/kinematic.h>
#include <rosthread/mutex.h>
Modified: pkg/trunk/util/collision_space/include/collision_space/environmentODE.h
===================================================================
--- pkg/trunk/util/collision_space/include/collision_space/environmentODE.h 2008-08-05 03:59:39 UTC (rev 2543)
+++ pkg/trunk/util/collision_space/include/collision_space/environmentODE.h 2008-08-05 04:12:01 UTC (rev 2544)
@@ -32,8 +32,8 @@
* POSSIBILITY OF SUCH DAMAGE.
*********************************************************************/
-#ifndef KINEMATIC_ENVIRONMENT_MODEL_ODE_
-#define KINEMATIC_ENVIRONMENT_MODEL_ODE_
+#ifndef COLLISION_SPACE_ENVIRONMENT_MODEL_ODE_
+#define COLLISION_SPACE_ENVIRONMENT_MODEL_ODE_
#include <collision_space/environment.h>
#include <ode/ode.h>
Added: pkg/trunk/util/collision_space/include/collision_space/util.h
===================================================================
--- pkg/trunk/util/collision_space/include/collision_space/util.h (rev 0)
+++ pkg/trunk/util/collision_space/include/collision_space/util.h 2008-08-05 04:12:01 UTC (rev 2544)
@@ -0,0 +1,186 @@
+/*********************************************************************
+* 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.
+*********************************************************************/
+
+#ifndef COLLISION_SPACE_UTIL_
+#define COLLISION_SPACE_UTIL_
+
+#include <libTF/Pose3D.h>
+
+namespace collision_space
+{
+
+ class Object
+ {
+ public:
+ Object(void)
+ {
+ m_scale = 1.0;
+ }
+
+ virtual ~Object(void)
+ {
+ }
+
+ void setScale(double scale)
+ {
+ m_scale = scale;
+ }
+
+ bool containsPoint(const libTF::Pose3D &pose, const libTF::Pose3D::Position &p) const
+ {
+ /* bring point in the reference frame described by pose */
+ libTF::Pose3D::Position pt = p;
+ pose.applyToPosition(pt);
+
+ /* since the body is centered at origin, scaling the body is equivalent
+ * to scaling the coordinates of the point */
+ pt.x *= m_scale;
+ pt.y *= m_scale;
+ pt.z *= m_scale;
+
+ return containsPoint(pt);
+ }
+
+ virtual bool containsPoint(const libTF::Pose3D::Position &p) const = 0;
+
+ protected:
+
+ double m_scale;
+
+ };
+
+ class Sphere : public Object
+ {
+ public:
+ Sphere(double radius = 0) : Object()
+ {
+ setRadius(radius);
+ }
+
+ virtual ~Sphere(void)
+ {
+ }
+
+ void setRadius(double radius)
+ {
+ m_radius = radius;
+ m_radius2 = radius * radius;
+ }
+
+ virtual bool containsPoint(const libTF::Pose3D::Position &p) const
+ {
+ return p.x * p.x + p.y * p.y + p.z * p.z < m_radius2;
+ }
+
+ protected:
+
+ double m_radius2;
+ double m_radius;
+ };
+
+ class Cylinder : public Object
+ {
+ public:
+ Cylinder(double length = 0.0, double radius = 0.0) : Object()
+ {
+ setDimensions(length, radius);
+ }
+
+ virtual ~Cylinder(void)
+ {
+ }
+
+ void setDimensions(double length, double radius)
+ {
+ m_length = length;
+ m_length2 = length / 2.0;
+ m_radius = radius;
+ m_radius2 = radius * radius;
+ }
+
+ virtual bool containsPoint(const libTF::Pose3D::Position &p) const
+ {
+ if (fabs(p.z) > m_length2)
+ return false;
+ return p.x * p.x + p.y * p.y < m_radius2;
+ }
+
+ protected:
+
+ double m_length;
+ double m_length2;
+ double m_radius;
+ double m_radius2;
+ };
+
+ class Box : public Object
+ {
+ public:
+ Box(double length = 0.0, double width = 0.0, double height = 0.0) : Object()
+ {
+ setDimensions(length, width, height);
+ }
+
+ virtual ~Box(void)
+ {
+ }
+
+ void setDimensions(double length, double width, double height) // x, y, z
+ {
+ m_length = length;
+ m_length2 = length / 2.0;
+ m_width = width;
+ m_width2 = width / 2.0;
+ m_height = height;
+ m_height2 = height / 2.0;
+ }
+
+ virtual bool containsPoint(const libTF::Pose3D::Position &p) const
+ {
+ return fabs(p.x) < m_length2 && fabs(p.y) < m_width2 && fabs(p.z) < m_height2;
+ }
+
+ protected:
+ double m_length;
+ double m_width;
+ double m_height;
+ double m_length2;
+ double m_width2;
+ double m_height2;
+ };
+
+
+}
+
+#endif
Modified: pkg/trunk/world_models/world_3d_map/manifest.xml
===================================================================
--- pkg/trunk/world_models/world_3d_map/manifest.xml 2008-08-05 03:59:39 UTC (rev 2543)
+++ pkg/trunk/world_models/world_3d_map/manifest.xml 2008-08-05 04:12:01 UTC (rev 2544)
@@ -8,5 +8,5 @@
<depend package="xmlparam" />
<depend package="rosTF" />
<depend package="random_utils" />
-
+<depend package="collision_space" />
</package>
Modified: pkg/trunk/world_models/world_3d_map/src/world_3d_map.cpp
===================================================================
--- pkg/trunk/world_models/world_3d_map/src/world_3d_map.cpp 2008-08-05 03:59:39 UTC (rev 2543)
+++ pkg/trunk/world_models/world_3d_map/src/world_3d_map.cpp 2008-08-05 04:12:01 UTC (rev 2544)
@@ -90,6 +90,7 @@
#include <rostools/Log.h>
#include <rosTF/rosTF.h>
#include <random_utils/random_utils.h>
+#include <collision_space/util.h>
#include <deque>
#include <cmath>
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
