Revision: 7733
http://playerstage.svn.sourceforge.net/playerstage/?rev=7733&view=rev
Author: natepak
Date: 2009-05-28 23:16:05 +0000 (Thu, 28 May 2009)
Log Message:
-----------
Updates to the bullet physics engine
Modified Paths:
--------------
code/gazebo/branches/bullet/server/physics/bullet/BulletBody.cc
code/gazebo/branches/bullet/server/physics/bullet/BulletGeom.cc
code/gazebo/branches/bullet/server/physics/bullet/BulletGeom.hh
code/gazebo/branches/bullet/server/physics/bullet/BulletMotionState.cc
code/gazebo/branches/bullet/server/physics/bullet/BulletMotionState.hh
code/gazebo/branches/bullet/server/physics/bullet/BulletPhysics.cc
code/gazebo/branches/bullet/server/physics/bullet/BulletRayGeom.cc
code/gazebo/branches/bullet/server/physics/ode/ODEGeom.cc
code/gazebo/branches/bullet/server/physics/ode/ODERayGeom.cc
Added Paths:
-----------
code/gazebo/branches/bullet/server/physics/BulletMapGeom.cc
code/gazebo/branches/bullet/server/physics/BulletMapGeom.hh
Removed Paths:
-------------
code/gazebo/branches/bullet/server/physics/bullet/BulletMapGeom.cc
code/gazebo/branches/bullet/server/physics/bullet/BulletMapGeom.hh
Copied: code/gazebo/branches/bullet/server/physics/BulletMapGeom.cc (from rev
7719, code/gazebo/branches/bullet/server/physics/bullet/BulletMapGeom.cc)
===================================================================
--- code/gazebo/branches/bullet/server/physics/BulletMapGeom.cc
(rev 0)
+++ code/gazebo/branches/bullet/server/physics/BulletMapGeom.cc 2009-05-28
23:16:05 UTC (rev 7733)
@@ -0,0 +1,437 @@
+/*
+ * Gazebo - Outdoor Multi-Robot Simulator
+ * Copyright (C) 2003
+ * Nate Koenig & Andrew Howard
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program; if not, write to the Free Software
+ * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
+ *
+ */
+/* Desc: Map geometry
+ * Author: Nate Koenig
+ * Date: 14 July 2008
+ * CVS: $Id: MapGeom.cc 7661 2009-05-14 15:54:32Z natepak $
+ */
+
+
+#include <iostream>
+#include <string.h>
+#include <math.h>
+
+#include "GazeboConfig.hh"
+#include "OgreVisual.hh"
+#include "Image.hh"
+#include "GazeboError.hh"
+#include "Simulator.hh"
+#include "Global.hh"
+#include "Body.hh"
+#include "MapGeom.hh"
+
+using namespace gazebo;
+
+unsigned int MapGeom::geomCounter = 0;
+
+//////////////////////////////////////////////////////////////////////////////
+// Constructor
+MapGeom::MapGeom(Body *body)
+ : Geom(body)
+{
+ this->root = new QuadNode(NULL);
+
+ Param::Begin(&this->parameters);
+ this->negativeP = new ParamT<int>("negative", 0, 0);
+ this->thresholdP = new ParamT<double>( "threshold", 200.0, 0);
+ this->wallHeightP = new ParamT<double>( "height", 1.0, 0 );
+ this->scaleP = new ParamT<double>("scale",1.0,0);
+ this->materialP = new ParamT<std::string>("material", "", 0);
+ this->granularityP = new ParamT<int>("granularity", 5, 0);
+ Param::End();
+
+}
+
+
+//////////////////////////////////////////////////////////////////////////////
+// Destructor
+MapGeom::~MapGeom()
+{
+ if (this->root)
+ delete this->root;
+
+ if (this->mapImage)
+ delete this->mapImage;
+ this->mapImage = NULL;
+
+ delete this->negativeP;
+ delete this->thresholdP;
+ delete this->wallHeightP;
+ delete this->scaleP;
+ delete this->materialP;
+ delete this->granularityP;
+}
+
+//////////////////////////////////////////////////////////////////////////////
+/// Update function.
+void MapGeom::Update()
+{
+ Geom::Update();
+}
+
+////////////////////////////////////////////////////////////////////////////////
+/// Load the heightmap
+void MapGeom::Load(XMLConfigNode *node)
+{
+ Geom::Load(node);
+
+ std::string imageFilename = node->GetString("image","",1);
+
+ this->negativeP->Load(node);
+ this->thresholdP->Load(node);
+ this->wallHeightP->Load(node);
+ this->scaleP->Load(node);
+ this->materialP->Load(node);
+ this->granularityP->Load(node);
+
+ // Make sure they are ok
+ if (this->scaleP->GetValue() <= 0) this->scaleP->SetValue( 0.1 );
+ if (this->thresholdP->GetValue() <=0) this->thresholdP->SetValue(200);
+ if (this->wallHeightP->GetValue() <= 0) this->wallHeightP->SetValue( 1.0 );
+
+ // Load the image
+ this->mapImage = new Image();
+
+ std::list<std::string>::iterator iter;
+ GazeboConfig *gzcfg = Simulator::Instance()->GetGazeboConfig();
+ std::string filename;
+
+ for (iter = gzcfg->GetGazeboPaths().begin();
+ iter != gzcfg->GetGazeboPaths().end(); iter++)
+ {
+ filename = (*iter) + "/Media/materials/textures/" + imageFilename;
+ if (this->mapImage->Load(filename) >= 0)
+ {
+ break;
+ }
+ }
+
+ if (!this->mapImage->Valid())
+ gzthrow(std::string("Unable to open image file[") + imageFilename + "]" );
+
+ this->root->x = 0;
+ this->root->y = 0;
+
+ this->root->width = this->mapImage->GetWidth();
+ this->root->height = this->mapImage->GetHeight();
+
+ this->BuildTree(this->root);
+
+ this->merged = true;
+ while (this->merged)
+ {
+ this->merged =false;
+ this->ReduceTree(this->root);
+ }
+
+ this->CreateBoxes(this->root);
+
+ if (this->visualNode)
+ this->visualNode->MakeStatic();
+}
+
+
+//////////////////////////////////////////////////////////////////////////////
+// Create the boxes
+void MapGeom::CreateBoxes(QuadNode *node)
+{
+ if (node->leaf)
+ {
+ if (!node->valid || !node->occupied)
+ return;
+
+ std::ostringstream stream;
+
+ // Create the box geometry
+ Geom *newBox = this->physicsEngine->CreateGeom( "box", this->body );
+
+ if (!newBox)
+ gzthrow("Unable to make an ode box geom");
+
+ XMLConfig *boxConfig = new XMLConfig();
+
+ stream << "<gazebo:world
xmlns:gazebo=\"http://playerstage.sourceforge.net/gazebo/xmlschema/#gz\";
xmlns:geom=\"http://playerstage.sourceforge.net/gazebo/xmlschema/#geom\";>";
+
+ float x = (node->x + node->width / 2.0) * this->scaleP->GetValue();
+ float y = (node->y + node->height / 2.0) * this->scaleP->GetValue();
+ float z = this->wallHeightP->GetValue() / 2.0;
+ float xSize = (node->width) * this->scaleP->GetValue();
+ float ySize = (node->height) * this->scaleP->GetValue();
+ float zSize = this->wallHeightP->GetValue();
+
+ char geomName[256];
+ sprintf(geomName,"map_geom_%d",geomCounter++);
+
+ stream << "<geom:box name='" << geomName << "'>";
+ stream << " <mass>0.0</mass>";
+ stream << " <xyz>" << x << " " << y << " " << z << "</xyz>";
+ stream << " <rpy>0 0 0</rpy>";
+ stream << " <size>" << xSize << " " << ySize << " " << zSize << "</size>";
+ stream << " <visual>";
+ stream << " <mesh>unit_box</mesh>";
+ stream << " <material>" << this->materialP->GetValue() << "</material>";
+ stream << " <size>" << xSize << " " << ySize << " " << zSize << "</size>";
+ stream << " </visual>";
+ stream << "</geom:box>";
+ stream << "</gazebo:world>";
+
+ boxConfig->LoadString( stream.str() );
+
+ newBox->Load( boxConfig->GetRootNode()->GetChild() );
+ delete boxConfig;
+ }
+ else
+ {
+ std::deque<QuadNode*>::iterator iter;
+ for (iter = node->children.begin(); iter != node->children.end(); iter++)
+ {
+ this->CreateBoxes(*iter);
+ }
+ }
+}
+
+//////////////////////////////////////////////////////////////////////////////
+// Reduce the size of the quad tree
+void MapGeom::ReduceTree(QuadNode *node)
+{
+ std::deque<QuadNode*>::iterator iter;
+
+ if (!node->valid)
+ return;
+
+ if (!node->leaf)
+ {
+ unsigned int count = 0;
+ int size = node->children.size();
+
+ for (int i = 0; i < size; i++)
+ {
+ if (node->children[i]->valid)
+ {
+ this->ReduceTree(node->children[i]);
+ }
+ if (node->children[i]->leaf)
+ count++;
+ }
+
+ if (node->parent && count == node->children.size())
+ {
+ for (iter = node->children.begin(); iter != node->children.end(); iter++)
+ {
+ node->parent->children.push_back( *iter );
+ (*iter)->parent = node->parent;
+ }
+ node->valid = false;
+ }
+ else
+ {
+ bool done = false;
+ while (!done)
+ {
+ done = true;
+ for (iter = node->children.begin();
+ iter != node->children.end();iter++ )
+ {
+ if (!(*iter)->valid)
+ {
+ node->children.erase(iter, iter+1);
+ done = false;
+ break;
+ }
+ }
+ }
+ }
+
+ }
+ else
+ {
+ this->Merge(node, node->parent);
+ }
+}
+
+//////////////////////////////////////////////////////////////////////////////
+// Merege quad tree cells
+void MapGeom::Merge(QuadNode *nodeA, QuadNode *nodeB)
+{
+ std::deque<QuadNode*>::iterator iter;
+
+ if (!nodeB)
+ return;
+
+ if (nodeB->leaf)
+ {
+ if (nodeB->occupied != nodeA->occupied)
+ return;
+
+ if ( nodeB->x == nodeA->x + nodeA->width &&
+ nodeB->y == nodeA->y &&
+ nodeB->height == nodeA->height )
+ {
+ nodeA->width += nodeB->width;
+ nodeB->valid = false;
+ nodeA->valid = true;
+
+ this->merged = true;
+ }
+
+ if (nodeB->x == nodeA->x &&
+ nodeB->width == nodeA->width &&
+ nodeB->y == nodeA->y + nodeA->height )
+ {
+ nodeA->height += nodeB->height;
+ nodeB->valid = false;
+ nodeA->valid = true;
+
+ this->merged = true;
+ }
+ }
+ else
+ {
+
+ for (iter = nodeB->children.begin(); iter != nodeB->children.end(); iter++)
+ {
+ if ((*iter)->valid)
+ {
+ this->Merge(nodeA, (*iter));
+ }
+ }
+ }
+}
+
+
+//////////////////////////////////////////////////////////////////////////////
+// Construct the quad tree
+void MapGeom::BuildTree(QuadNode *node)
+{
+ QuadNode *newNode = NULL;
+ unsigned int freePixels, occPixels;
+
+ this->GetPixelCount(node->x, node->y, node->width, node->height,
+ freePixels, occPixels);
+
+ //int diff = labs(freePixels - occPixels);
+
+ if ((int)(node->width*node->height) > (**this->granularityP))
+ {
+ float newX, newY;
+ float newW, newH;
+
+ newY = node->y;
+ newW = node->width / 2.0;
+ newH = node->height / 2.0;
+
+ // Create the children for the node
+ for (int i=0; i<2; i++)
+ {
+ newX = node->x;
+
+ for (int j=0; j<2; j++)
+ {
+ newNode = new QuadNode(node);
+ newNode->x = (unsigned int)newX;
+ newNode->y = (unsigned int)newY;
+
+ if (j == 0)
+ newNode->width = (unsigned int)floor(newW);
+ else
+ newNode->width = (unsigned int)ceil(newW);
+
+ if (i==0)
+ newNode->height = (unsigned int)floor(newH);
+ else
+ newNode->height = (unsigned int)ceil(newH);
+
+ node->children.push_back(newNode);
+
+ this->BuildTree(newNode);
+
+ newX += newNode->width;
+
+ if (newNode->width == 0 || newNode->height ==0)
+ newNode->valid = false;
+ }
+
+ if (i==0)
+ newY += floor(newH);
+ else
+ newY += ceil(newH);
+ }
+
+ //node->occupied = true;
+ node->occupied = false;
+ node->leaf = false;
+ }
+ else if (occPixels == 0)
+ {
+ node->occupied = false;
+ node->leaf = true;
+ }
+ else
+ {
+ node->occupied = true;
+ node->leaf = true;
+ }
+
+}
+
+//////////////////////////////////////////////////////////////////////////////
+// Get a count of pixels within a given area
+void MapGeom::GetPixelCount(unsigned int xStart, unsigned int yStart,
+ unsigned int width, unsigned int height,
+ unsigned int &freePixels,
+ unsigned int &occPixels )
+{
+ Color pixColor;
+ unsigned char v;
+ unsigned int x,y;
+
+ freePixels = occPixels = 0;
+
+ for (y = yStart; y < yStart + height; y++)
+ {
+ for (x = xStart; x < xStart + width; x++)
+ {
+ pixColor = this->mapImage->GetPixel(x, y);
+
+ v = (unsigned char)(255 * ((pixColor.R() + pixColor.G() + pixColor.B())
/ 3.0));
+ if (this->negativeP->GetValue())
+ v = 255 - v;
+
+ if (v > this->thresholdP->GetValue())
+ freePixels++;
+ else
+ occPixels++;
+ }
+ }
+}
+
+//////////////////////////////////////////////////////////////////////////////
+/// Save child parameters
+void MapGeom::Save(std::string &prefix, std::ostream &stream)
+{
+ Geom::Save(prefix, stream);
+ stream << prefix << *(this->negativeP) << "\n";
+ stream << prefix << *(this->thresholdP) << "\n";
+ stream << prefix << *(this->wallHeightP) << "\n";
+ stream << prefix << *(this->scaleP) << "\n";
+ stream << prefix << *(this->materialP) << "\n";
+ stream << prefix << *(this->granularityP) << "\n";
+}
Property changes on: code/gazebo/branches/bullet/server/physics/BulletMapGeom.cc
___________________________________________________________________
Added: svn:mergeinfo
+
/code/branches/federation/gazebo/server/physics/bullet/BulletMapGeom.cc:7371-7550
/code/gazebo/branches/ogre-1.4.9/server/physics/bullet/BulletMapGeom.cc:7137-7636
Copied: code/gazebo/branches/bullet/server/physics/BulletMapGeom.hh (from rev
7719, code/gazebo/branches/bullet/server/physics/bullet/BulletMapGeom.hh)
===================================================================
--- code/gazebo/branches/bullet/server/physics/BulletMapGeom.hh
(rev 0)
+++ code/gazebo/branches/bullet/server/physics/BulletMapGeom.hh 2009-05-28
23:16:05 UTC (rev 7733)
@@ -0,0 +1,197 @@
+/*
+ * Gazebo - Outdoor Multi-Robot Simulator
+ * Copyright (C) 2003
+ * Nate Koenig & Andrew Howard
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program; if not, write to the Free Software
+ * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
+ *
+ */
+/* Desc: Occupancy grid geom
+ * Author: Nate Koenig
+ * Date: 14 Jly 2008
+ * CVS: $Id: BulletMapGeom.hh 7634 2009-05-11 01:43:31Z natepak $
+ */
+
+#ifndef BulletMAPGEOM_HH
+#define BulletMAPGEOM_HH
+
+#include <deque>
+
+#include "Vector2.hh"
+#include "Param.hh"
+#include "BulletGeom.hh"
+
+namespace gazebo
+{
+ class Image;
+
+ /// \addtogroup gazebo_physics_geom
+ /// \{
+ /** \defgroup gazebo_map_geom Map geom
+ \brief Map geom
+
+ \par Attributes
+ The following attributes are supported.
+
+ - image (string)
+ - Binary image that defines an occupancy grid
+ - Default: (empty)
+
+ - scale (float)
+ - Scaling factor
+ - Default: 1
+
+ - granularity (int)
+ - Degree of coarseness when determing if an image area is occupied.
Units are pixels
+ - Default: 5
+
+ - threshold (unsigned char)
+ - Grayscale threshold. A pixel value greater than this amount is
considered free space
+ - Default: 200
+
+ - negative (bool)
+ - True if the image pixel values should be inverted.
+ - Default: false
+
+ - material (string)
+ - Material to apply to the map
+ - Default: (empty)
+
+
+
+ \par Example
+ \verbatim
+ <geom:map name="map_geom">
+ <image>map.png</image>
+ <scale>0.1</scale>
+ </geom:map>
+ \endverbatim
+ */
+ /// \}
+ /// \addtogroup gazebo_map_geom
+ /// \{
+
+
+ class SpaceTree;
+ class QuadNode;
+
+ /// \brief Map geom
+ class BulletMapGeom : public BulletGeom
+ {
+ /// \brief Constructor
+ public: BulletMapGeom(Body *body);
+
+ /// \brief Destructor
+ public: virtual ~BulletMapGeom();
+
+ /// \brief Update function
+ public: void Update();
+
+ /// \brief Load the heightmap
+ public: virtual void Load(XMLConfigNode *node);
+
+ /// \brief Save child parameters
+ public: void Save(std::string &prefix, std::ostream &stream);
+
+ /// \brief Build the quadtree
+ private: void BuildTree(QuadNode *node);
+
+ /// \brief Get the number of free and occupied pixels in a given area
+ private: void GetPixelCount(unsigned int xStart, unsigned int yStart,
+ unsigned int width, unsigned int height,
+ unsigned int &freePixels,
+ unsigned int &occPixels );
+
+ /// \brief Reduce the number of nodes in the tree.
+ private: void ReduceTree(QuadNode *node);
+
+ /// \brief Try to merge to nodes
+ private: void Merge(QuadNode *nodeA, QuadNode *nodeB);
+
+ private: void CreateBox();
+
+ /// \brief Create the boxes for the map
+ private: void CreateBoxes(QuadNode *node);
+
+ // The scale factor to apply to the geoms
+ private: ParamT<double> *scaleP;
+
+ // Negative image?
+ private: ParamT<int> *negativeP;
+
+ // Image color threshold used for extrusion
+ private: ParamT<double> *thresholdP;
+
+ // The color of the walls
+ private: ParamT<std::string> *materialP;
+
+ // The amount of acceptable error in the model
+ private: ParamT<int> *granularityP;
+
+ private: ParamT<double> *wallHeightP;
+
+ private: Image *mapImage;
+
+ private: QuadNode *root;
+
+ private: bool merged;
+ private: static unsigned int geomCounter;
+ };
+
+
+ class QuadNode
+ {
+ public: QuadNode( QuadNode *_parent )
+ {
+ parent = _parent;
+ occupied = false;
+ leaf = true;
+ valid = true;
+ }
+
+ public: ~QuadNode()
+ {
+ std::deque<QuadNode*>::iterator iter;
+ for (iter = children.begin(); iter != children.end(); iter++)
+ delete (*iter);
+ }
+
+ public: void Print(std::string space)
+ {
+ std::deque<QuadNode*>::iterator iter;
+
+ printf("%sXY[%d %d] WH[%d %d] O[%d] L[%d]
V[%d]\n",space.c_str(),x,y,width, height, occupied, leaf, valid);
+ space += " ";
+ for (iter = children.begin(); iter != children.end(); iter++)
+ if ((*iter)->occupied)
+ (*iter)->Print(space);
+ }
+
+ public: unsigned int x, y;
+ public: unsigned int width, height;
+
+ public: QuadNode *parent;
+ public: std::deque<QuadNode*> children;
+ public: bool occupied;
+ public: bool leaf;
+
+ public: bool valid;
+
+ };
+
+ /// \}
+}
+
+#endif
Property changes on: code/gazebo/branches/bullet/server/physics/BulletMapGeom.hh
___________________________________________________________________
Added: svn:mergeinfo
+
/code/branches/federation/gazebo/server/physics/bullet/BulletMapGeom.hh:7371-7550
/code/gazebo/branches/ogre-1.4.9/server/physics/bullet/BulletMapGeom.hh:7137-7636
Modified: code/gazebo/branches/bullet/server/physics/bullet/BulletBody.cc
===================================================================
--- code/gazebo/branches/bullet/server/physics/bullet/BulletBody.cc
2009-05-28 00:48:43 UTC (rev 7732)
+++ code/gazebo/branches/bullet/server/physics/bullet/BulletBody.cc
2009-05-28 23:16:05 UTC (rev 7733)
@@ -168,6 +168,7 @@
*this->mass += *(geom->GetMass());
+ bgeom->SetCompoundShapeIndex( this->compoundShape->getNumChildShapes() );
this->compoundShape->addChildShape(trans, bgeom->GetCollisionShape());
}
Modified: code/gazebo/branches/bullet/server/physics/bullet/BulletGeom.cc
===================================================================
--- code/gazebo/branches/bullet/server/physics/bullet/BulletGeom.cc
2009-05-28 00:48:43 UTC (rev 7732)
+++ code/gazebo/branches/bullet/server/physics/bullet/BulletGeom.cc
2009-05-28 23:16:05 UTC (rev 7733)
@@ -65,6 +65,8 @@
void BulletGeom::Load(XMLConfigNode *node)
{
Geom::Load(node);
+ std::cout << "BulletGeom[" << this->GetName() << "] Set Init Pose[" <<
this->GetPose() << "]\n";
+ this->visualNode->SetPose( this->GetPose() );
}
////////////////////////////////////////////////////////////////////////////////
@@ -85,7 +87,6 @@
// Set the pose relative to the body
void BulletGeom::SetPose(const Pose3d &newPose, bool updateCoM)
{
- printf("Set geom pose\n");
}
////////////////////////////////////////////////////////////////////////////////
@@ -100,14 +101,30 @@
}
////////////////////////////////////////////////////////////////////////////////
+/// Get the position of the geom relative to the parent body
+Vector3 BulletGeom::GetRelativePosition()
+{
+ return Vector3();
+}
+
+////////////////////////////////////////////////////////////////////////////////
+/// Get the rotation of the geom relative to the parent body
+Quatern BulletGeom::GetRelativeRotation()
+{
+ return Quatern();
+}
+
+
+////////////////////////////////////////////////////////////////////////////////
// Set the position
-void BulletGeom::SetPosition(const Vector3 &pos)
+void BulletGeom::SetRelativePosition(const Vector3 &pos)
{
+
}
////////////////////////////////////////////////////////////////////////////////
// Set the rotation
-void BulletGeom::SetRotation(const Quatern &rot)
+void BulletGeom::SetRelativeRotation(const Quatern &rot)
{
}
@@ -166,4 +183,9 @@
return this->collisionShape;
}
-
+////////////////////////////////////////////////////////////////////////////////
+// Set the index of the compound shape
+void BulletGeom::SetCompoundShapeIndex( int index )
+{
+ this->compoundShapeIndex = 0;
+}
Modified: code/gazebo/branches/bullet/server/physics/bullet/BulletGeom.hh
===================================================================
--- code/gazebo/branches/bullet/server/physics/bullet/BulletGeom.hh
2009-05-28 00:48:43 UTC (rev 7732)
+++ code/gazebo/branches/bullet/server/physics/bullet/BulletGeom.hh
2009-05-28 23:16:05 UTC (rev 7733)
@@ -73,14 +73,20 @@
/// \brief Return the pose of the geom
public: virtual Pose3d GetPose() const;
-
+
+ /// \brief Get the position of the geom relative to the parent body
+ public: virtual Vector3 GetRelativePosition();
+
+ /// \brief Get the rotation of the geom relative to the parent body
+ public: virtual Quatern GetRelativeRotation();
+
/// \brief Set the position
/// \param pos Vector3 position
- public: void SetPosition(const Vector3 &pos);
+ public: void SetRelativePosition(const Vector3 &pos);
/// \brief Set the rotation
/// \param rot Quaternion rotation
- public: void SetRotation(const Quatern &rot);
+ public: void SetRelativeRotation(const Quatern &rot);
/// \brief Set the category bits, used during collision detection
/// \param bits The bits
@@ -105,8 +111,13 @@
/// \brief Get the bullet collision shape
public: btCollisionShape *GetCollisionShape() const;
+ /// \brief Set the index of the compound shape
+ public: void SetCompoundShapeIndex( int index );
+
protected: BulletPhysics *bulletPhysics;
protected: btCollisionShape *collisionShape;
+
+ protected: int compoundShapeIndex;
};
/// \}
Deleted: code/gazebo/branches/bullet/server/physics/bullet/BulletMapGeom.cc
===================================================================
--- code/gazebo/branches/bullet/server/physics/bullet/BulletMapGeom.cc
2009-05-28 00:48:43 UTC (rev 7732)
+++ code/gazebo/branches/bullet/server/physics/bullet/BulletMapGeom.cc
2009-05-28 23:16:05 UTC (rev 7733)
@@ -1,439 +0,0 @@
-/*
- * Gazebo - Outdoor Multi-Robot Simulator
- * Copyright (C) 2003
- * Nate Koenig & Andrew Howard
- *
- * This program is free software; you can redistribute it and/or modify
- * it under the terms of the GNU General Public License as published by
- * the Free Software Foundation; either version 2 of the License, or
- * (at your option) any later version.
- *
- * This program is distributed in the hope that it will be useful,
- * but WITHOUT ANY WARRANTY; without even the implied warranty of
- * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
- * GNU General Public License for more details.
- *
- * You should have received a copy of the GNU General Public License
- * along with this program; if not, write to the Free Software
- * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
- *
- */
-/* Desc: Map geometry
- * Author: Nate Koenig
- * Date: 14 July 2008
- * CVS: $Id: BulletMapGeom.cc 7661 2009-05-14 15:54:32Z natepak $
- */
-
-
-#include <iostream>
-#include <string.h>
-#include <math.h>
-
-#include "BulletPhysics.hh"
-#include "GazeboConfig.hh"
-#include "OgreVisual.hh"
-#include "Image.hh"
-#include "BulletBoxGeom.hh"
-#include "GazeboError.hh"
-#include "Simulator.hh"
-#include "Global.hh"
-#include "Body.hh"
-#include "BulletMapGeom.hh"
-
-using namespace gazebo;
-
-unsigned int BulletMapGeom::geomCounter = 0;
-
-//////////////////////////////////////////////////////////////////////////////
-// Constructor
-BulletMapGeom::BulletMapGeom(Body *body)
- : BulletGeom(body)
-{
- this->root = new QuadNode(NULL);
-
- Param::Begin(&this->parameters);
- this->negativeP = new ParamT<int>("negative", 0, 0);
- this->thresholdP = new ParamT<double>( "threshold", 200.0, 0);
- this->wallHeightP = new ParamT<double>( "height", 1.0, 0 );
- this->scaleP = new ParamT<double>("scale",1.0,0);
- this->materialP = new ParamT<std::string>("material", "", 0);
- this->granularityP = new ParamT<int>("granularity", 5, 0);
- Param::End();
-
-}
-
-
-//////////////////////////////////////////////////////////////////////////////
-// Destructor
-BulletMapGeom::~BulletMapGeom()
-{
- if (this->root)
- delete this->root;
-
- if (this->mapImage)
- delete this->mapImage;
- this->mapImage = NULL;
-
- delete this->negativeP;
- delete this->thresholdP;
- delete this->wallHeightP;
- delete this->scaleP;
- delete this->materialP;
- delete this->granularityP;
-}
-
-//////////////////////////////////////////////////////////////////////////////
-/// Update function.
-void BulletMapGeom::Update()
-{
- BulletGeom::Update();
-}
-
-////////////////////////////////////////////////////////////////////////////////
-/// Load the heightmap
-void BulletMapGeom::Load(XMLConfigNode *node)
-{
- BulletGeom::Load(node);
-
- std::string imageFilename = node->GetString("image","",1);
-
- this->negativeP->Load(node);
- this->thresholdP->Load(node);
- this->wallHeightP->Load(node);
- this->scaleP->Load(node);
- this->materialP->Load(node);
- this->granularityP->Load(node);
-
- // Make sure they are ok
- if (this->scaleP->GetValue() <= 0) this->scaleP->SetValue( 0.1 );
- if (this->thresholdP->GetValue() <=0) this->thresholdP->SetValue(200);
- if (this->wallHeightP->GetValue() <= 0) this->wallHeightP->SetValue( 1.0 );
-
- // Load the image
- this->mapImage = new Image();
-
- std::list<std::string>::iterator iter;
- GazeboConfig *gzcfg = Simulator::Instance()->GetGazeboConfig();
- std::string filename;
-
- for (iter = gzcfg->GetGazeboPaths().begin();
- iter != gzcfg->GetGazeboPaths().end(); iter++)
- {
- filename = (*iter) + "/Media/materials/textures/" + imageFilename;
- if (this->mapImage->Load(filename) >= 0)
- {
- break;
- }
- }
-
- if (!this->mapImage->Valid())
- gzthrow(std::string("Unable to open image file[") + imageFilename + "]" );
-
- this->root->x = 0;
- this->root->y = 0;
-
- this->root->width = this->mapImage->GetWidth();
- this->root->height = this->mapImage->GetHeight();
-
- this->BuildTree(this->root);
-
- this->merged = true;
- while (this->merged)
- {
- this->merged =false;
- this->ReduceTree(this->root);
- }
-
- this->CreateBoxes(this->root);
-
- if (this->visualNode)
- this->visualNode->MakeStatic();
-}
-
-
-//////////////////////////////////////////////////////////////////////////////
-// Create the Bullet boxes
-void BulletMapGeom::CreateBoxes(QuadNode *node)
-{
- if (node->leaf)
- {
- if (!node->valid || !node->occupied)
- return;
-
- std::ostringstream stream;
-
- // Create the box geometry
- BulletBoxGeom *newBox =
dynamic_cast<BulletBoxGeom*>(this->bulletPhysics->CreateGeom( "box", this->body
));
-
- if (!newBox)
- gzthrow("Unable to make an ode box geom");
-
- XMLConfig *boxConfig = new XMLConfig();
-
- stream << "<gazebo:world
xmlns:gazebo=\"http://playerstage.sourceforge.net/gazebo/xmlschema/#gz\";
xmlns:geom=\"http://playerstage.sourceforge.net/gazebo/xmlschema/#geom\";>";
-
- float x = (node->x + node->width / 2.0) * this->scaleP->GetValue();
- float y = (node->y + node->height / 2.0) * this->scaleP->GetValue();
- float z = this->wallHeightP->GetValue() / 2.0;
- float xSize = (node->width) * this->scaleP->GetValue();
- float ySize = (node->height) * this->scaleP->GetValue();
- float zSize = this->wallHeightP->GetValue();
-
- char geomName[256];
- sprintf(geomName,"map_geom_%d",geomCounter++);
-
- stream << "<geom:box name='" << geomName << "'>";
- stream << " <mass>0.0</mass>";
- stream << " <xyz>" << x << " " << y << " " << z << "</xyz>";
- stream << " <rpy>0 0 0</rpy>";
- stream << " <size>" << xSize << " " << ySize << " " << zSize << "</size>";
- stream << " <visual>";
- stream << " <mesh>unit_box</mesh>";
- stream << " <material>" << this->materialP->GetValue() << "</material>";
- stream << " <size>" << xSize << " " << ySize << " " << zSize << "</size>";
- stream << " </visual>";
- stream << "</geom:box>";
- stream << "</gazebo:world>";
-
- boxConfig->LoadString( stream.str() );
-
- newBox->Load( boxConfig->GetRootNode()->GetChild() );
- delete boxConfig;
- }
- else
- {
- std::deque<QuadNode*>::iterator iter;
- for (iter = node->children.begin(); iter != node->children.end(); iter++)
- {
- this->CreateBoxes(*iter);
- }
- }
-}
-
-//////////////////////////////////////////////////////////////////////////////
-// Reduce the size of the quad tree
-void BulletMapGeom::ReduceTree(QuadNode *node)
-{
- std::deque<QuadNode*>::iterator iter;
-
- if (!node->valid)
- return;
-
- if (!node->leaf)
- {
- unsigned int count = 0;
- int size = node->children.size();
-
- for (int i = 0; i < size; i++)
- {
- if (node->children[i]->valid)
- {
- this->ReduceTree(node->children[i]);
- }
- if (node->children[i]->leaf)
- count++;
- }
-
- if (node->parent && count == node->children.size())
- {
- for (iter = node->children.begin(); iter != node->children.end(); iter++)
- {
- node->parent->children.push_back( *iter );
- (*iter)->parent = node->parent;
- }
- node->valid = false;
- }
- else
- {
- bool done = false;
- while (!done)
- {
- done = true;
- for (iter = node->children.begin();
- iter != node->children.end();iter++ )
- {
- if (!(*iter)->valid)
- {
- node->children.erase(iter, iter+1);
- done = false;
- break;
- }
- }
- }
- }
-
- }
- else
- {
- this->Merge(node, node->parent);
- }
-}
-
-//////////////////////////////////////////////////////////////////////////////
-// Merege quad tree cells
-void BulletMapGeom::Merge(QuadNode *nodeA, QuadNode *nodeB)
-{
- std::deque<QuadNode*>::iterator iter;
-
- if (!nodeB)
- return;
-
- if (nodeB->leaf)
- {
- if (nodeB->occupied != nodeA->occupied)
- return;
-
- if ( nodeB->x == nodeA->x + nodeA->width &&
- nodeB->y == nodeA->y &&
- nodeB->height == nodeA->height )
- {
- nodeA->width += nodeB->width;
- nodeB->valid = false;
- nodeA->valid = true;
-
- this->merged = true;
- }
-
- if (nodeB->x == nodeA->x &&
- nodeB->width == nodeA->width &&
- nodeB->y == nodeA->y + nodeA->height )
- {
- nodeA->height += nodeB->height;
- nodeB->valid = false;
- nodeA->valid = true;
-
- this->merged = true;
- }
- }
- else
- {
-
- for (iter = nodeB->children.begin(); iter != nodeB->children.end(); iter++)
- {
- if ((*iter)->valid)
- {
- this->Merge(nodeA, (*iter));
- }
- }
- }
-}
-
-
-//////////////////////////////////////////////////////////////////////////////
-// Construct the quad tree
-void BulletMapGeom::BuildTree(QuadNode *node)
-{
- QuadNode *newNode = NULL;
- unsigned int freePixels, occPixels;
-
- this->GetPixelCount(node->x, node->y, node->width, node->height,
- freePixels, occPixels);
-
- //int diff = labs(freePixels - occPixels);
-
- if ((int)(node->width*node->height) > (**this->granularityP))
- {
- float newX, newY;
- float newW, newH;
-
- newY = node->y;
- newW = node->width / 2.0;
- newH = node->height / 2.0;
-
- // Create the children for the node
- for (int i=0; i<2; i++)
- {
- newX = node->x;
-
- for (int j=0; j<2; j++)
- {
- newNode = new QuadNode(node);
- newNode->x = (unsigned int)newX;
- newNode->y = (unsigned int)newY;
-
- if (j == 0)
- newNode->width = (unsigned int)floor(newW);
- else
- newNode->width = (unsigned int)ceil(newW);
-
- if (i==0)
- newNode->height = (unsigned int)floor(newH);
- else
- newNode->height = (unsigned int)ceil(newH);
-
- node->children.push_back(newNode);
-
- this->BuildTree(newNode);
-
- newX += newNode->width;
-
- if (newNode->width == 0 || newNode->height ==0)
- newNode->valid = false;
- }
-
- if (i==0)
- newY += floor(newH);
- else
- newY += ceil(newH);
- }
-
- //node->occupied = true;
- node->occupied = false;
- node->leaf = false;
- }
- else if (occPixels == 0)
- {
- node->occupied = false;
- node->leaf = true;
- }
- else
- {
- node->occupied = true;
- node->leaf = true;
- }
-
-}
-
-//////////////////////////////////////////////////////////////////////////////
-// Get a count of pixels within a given area
-void BulletMapGeom::GetPixelCount(unsigned int xStart, unsigned int yStart,
- unsigned int width, unsigned int height,
- unsigned int &freePixels,
- unsigned int &occPixels )
-{
- Color pixColor;
- unsigned char v;
- unsigned int x,y;
-
- freePixels = occPixels = 0;
-
- for (y = yStart; y < yStart + height; y++)
- {
- for (x = xStart; x < xStart + width; x++)
- {
- pixColor = this->mapImage->GetPixel(x, y);
-
- v = (unsigned char)(255 * ((pixColor.R() + pixColor.G() + pixColor.B())
/ 3.0));
- if (this->negativeP->GetValue())
- v = 255 - v;
-
- if (v > this->thresholdP->GetValue())
- freePixels++;
- else
- occPixels++;
- }
- }
-}
-
-//////////////////////////////////////////////////////////////////////////////
-/// Save child parameters
-void BulletMapGeom::Save(std::string &prefix, std::ostream &stream)
-{
- BulletGeom::Save(prefix, stream);
- stream << prefix << *(this->negativeP) << "\n";
- stream << prefix << *(this->thresholdP) << "\n";
- stream << prefix << *(this->wallHeightP) << "\n";
- stream << prefix << *(this->scaleP) << "\n";
- stream << prefix << *(this->materialP) << "\n";
- stream << prefix << *(this->granularityP) << "\n";
-}
Deleted: code/gazebo/branches/bullet/server/physics/bullet/BulletMapGeom.hh
===================================================================
--- code/gazebo/branches/bullet/server/physics/bullet/BulletMapGeom.hh
2009-05-28 00:48:43 UTC (rev 7732)
+++ code/gazebo/branches/bullet/server/physics/bullet/BulletMapGeom.hh
2009-05-28 23:16:05 UTC (rev 7733)
@@ -1,197 +0,0 @@
-/*
- * Gazebo - Outdoor Multi-Robot Simulator
- * Copyright (C) 2003
- * Nate Koenig & Andrew Howard
- *
- * This program is free software; you can redistribute it and/or modify
- * it under the terms of the GNU General Public License as published by
- * the Free Software Foundation; either version 2 of the License, or
- * (at your option) any later version.
- *
- * This program is distributed in the hope that it will be useful,
- * but WITHOUT ANY WARRANTY; without even the implied warranty of
- * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
- * GNU General Public License for more details.
- *
- * You should have received a copy of the GNU General Public License
- * along with this program; if not, write to the Free Software
- * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
- *
- */
-/* Desc: Occupancy grid geom
- * Author: Nate Koenig
- * Date: 14 Jly 2008
- * CVS: $Id: BulletMapGeom.hh 7634 2009-05-11 01:43:31Z natepak $
- */
-
-#ifndef BulletMAPGEOM_HH
-#define BulletMAPGEOM_HH
-
-#include <deque>
-
-#include "Vector2.hh"
-#include "Param.hh"
-#include "BulletGeom.hh"
-
-namespace gazebo
-{
- class Image;
-
- /// \addtogroup gazebo_physics_geom
- /// \{
- /** \defgroup gazebo_map_geom Map geom
- \brief Map geom
-
- \par Attributes
- The following attributes are supported.
-
- - image (string)
- - Binary image that defines an occupancy grid
- - Default: (empty)
-
- - scale (float)
- - Scaling factor
- - Default: 1
-
- - granularity (int)
- - Degree of coarseness when determing if an image area is occupied.
Units are pixels
- - Default: 5
-
- - threshold (unsigned char)
- - Grayscale threshold. A pixel value greater than this amount is
considered free space
- - Default: 200
-
- - negative (bool)
- - True if the image pixel values should be inverted.
- - Default: false
-
- - material (string)
- - Material to apply to the map
- - Default: (empty)
-
-
-
- \par Example
- \verbatim
- <geom:map name="map_geom">
- <image>map.png</image>
- <scale>0.1</scale>
- </geom:map>
- \endverbatim
- */
- /// \}
- /// \addtogroup gazebo_map_geom
- /// \{
-
-
- class SpaceTree;
- class QuadNode;
-
- /// \brief Map geom
- class BulletMapGeom : public BulletGeom
- {
- /// \brief Constructor
- public: BulletMapGeom(Body *body);
-
- /// \brief Destructor
- public: virtual ~BulletMapGeom();
-
- /// \brief Update function
- public: void Update();
-
- /// \brief Load the heightmap
- public: virtual void Load(XMLConfigNode *node);
-
- /// \brief Save child parameters
- public: void Save(std::string &prefix, std::ostream &stream);
-
- /// \brief Build the quadtree
- private: void BuildTree(QuadNode *node);
-
- /// \brief Get the number of free and occupied pixels in a given area
- private: void GetPixelCount(unsigned int xStart, unsigned int yStart,
- unsigned int width, unsigned int height,
- unsigned int &freePixels,
- unsigned int &occPixels );
-
- /// \brief Reduce the number of nodes in the tree.
- private: void ReduceTree(QuadNode *node);
-
- /// \brief Try to merge to nodes
- private: void Merge(QuadNode *nodeA, QuadNode *nodeB);
-
- private: void CreateBox();
-
- /// \brief Create the boxes for the map
- private: void CreateBoxes(QuadNode *node);
-
- // The scale factor to apply to the geoms
- private: ParamT<double> *scaleP;
-
- // Negative image?
- private: ParamT<int> *negativeP;
-
- // Image color threshold used for extrusion
- private: ParamT<double> *thresholdP;
-
- // The color of the walls
- private: ParamT<std::string> *materialP;
-
- // The amount of acceptable error in the model
- private: ParamT<int> *granularityP;
-
- private: ParamT<double> *wallHeightP;
-
- private: Image *mapImage;
-
- private: QuadNode *root;
-
- private: bool merged;
- private: static unsigned int geomCounter;
- };
-
-
- class QuadNode
- {
- public: QuadNode( QuadNode *_parent )
- {
- parent = _parent;
- occupied = false;
- leaf = true;
- valid = true;
- }
-
- public: ~QuadNode()
- {
- std::deque<QuadNode*>::iterator iter;
- for (iter = children.begin(); iter != children.end(); iter++)
- delete (*iter);
- }
-
- public: void Print(std::string space)
- {
- std::deque<QuadNode*>::iterator iter;
-
- printf("%sXY[%d %d] WH[%d %d] O[%d] L[%d]
V[%d]\n",space.c_str(),x,y,width, height, occupied, leaf, valid);
- space += " ";
- for (iter = children.begin(); iter != children.end(); iter++)
- if ((*iter)->occupied)
- (*iter)->Print(space);
- }
-
- public: unsigned int x, y;
- public: unsigned int width, height;
-
- public: QuadNode *parent;
- public: std::deque<QuadNode*> children;
- public: bool occupied;
- public: bool leaf;
-
- public: bool valid;
-
- };
-
- /// \}
-}
-
-#endif
Modified: code/gazebo/branches/bullet/server/physics/bullet/BulletMotionState.cc
===================================================================
--- code/gazebo/branches/bullet/server/physics/bullet/BulletMotionState.cc
2009-05-28 00:48:43 UTC (rev 7732)
+++ code/gazebo/branches/bullet/server/physics/bullet/BulletMotionState.cc
2009-05-28 23:16:05 UTC (rev 7733)
@@ -50,11 +50,17 @@
}
////////////////////////////////////////////////////////////////////////////////
+/// Set the center of mass offset
+void BulletMotionState::SetCoMOffset( const Pose3d &com )
+{
+ this->comOffset = com;
+}
+
+////////////////////////////////////////////////////////////////////////////////
// Set the visual node
void BulletMotionState::SetVisual(OgreVisual *vis)
{
this->visual = vis;
- //this->visual->SetPose(this->pose);
}
////////////////////////////////////////////////////////////////////////////////
@@ -69,10 +75,6 @@
void BulletMotionState::SetPosition(const Vector3 &pos)
{
this->pose.pos = pos;
-
- /*if (this->visual)
- this->visual->SetPose(this->pose);
- */
}
////////////////////////////////////////////////////////////////////////////////
@@ -87,30 +89,37 @@
void BulletMotionState::SetRotation(const Quatern &rot)
{
this->pose.rot = rot;
- /*if (this->visual)
- this->visual->SetPose(this->pose);
- */
}
////////////////////////////////////////////////////////////////////////////////
// Get the world transform
void BulletMotionState::getWorldTransform(btTransform &worldTrans) const
{
- worldTrans.setOrigin( btVector3( this->pose.pos.x, this->pose.pos.y,
this->pose.pos.z) );
- worldTrans.setRotation( btQuaternion( this->pose.rot.x, this->pose.rot.y,
this->pose.rot.z, this->pose.rot.u) );
+ Pose3d result = this->comOffset.GetInverse() * this->pose;
+
+ worldTrans.setOrigin( btVector3( result.pos.x, result.pos.y, result.pos.z) );
+ worldTrans.setRotation( btQuaternion( result.rot.x, result.rot.y,
+ result.rot.z, result.rot.u) );
}
////////////////////////////////////////////////////////////////////////////////
// Set the world transform
void BulletMotionState::setWorldTransform(const btTransform &worldTrans)
{
+ if (this->visual == NULL)
+ return;
+
btVector3 origin = worldTrans.getOrigin();
btQuaternion rot = worldTrans.getRotation();
- this->pose.pos.Set(origin.x(), origin.y(), origin.z());
- this->pose.rot.Set( rot.x(), rot.y(), rot.z(), rot.w());
+ Pose3d newPose;
- /*if (this->visual)
- this->visual->SetPose(this->pose);
- */
+ newPose.pos.Set( origin.x(), origin.y(), origin.z() );
+ newPose.rot.Set( rot.w(), rot.x(), rot.y(), rot.z() );
+
+ std::cout << "New Rot[" << newPose.rot << "] OldRo[" << this->pose.rot <<
"]\n";
+
+ this->pose = newPose;
+
+ this->visual->SetPose(this->pose * this->comOffset);
}
Modified: code/gazebo/branches/bullet/server/physics/bullet/BulletMotionState.hh
===================================================================
--- code/gazebo/branches/bullet/server/physics/bullet/BulletMotionState.hh
2009-05-28 00:48:43 UTC (rev 7732)
+++ code/gazebo/branches/bullet/server/physics/bullet/BulletMotionState.hh
2009-05-28 23:16:05 UTC (rev 7733)
@@ -48,6 +48,9 @@
/// \brief Set the visual
public: void SetVisual(OgreVisual *vis);
+ /// \brief Set the center of mass offset
+ public: void SetCoMOffset( const Pose3d &com );
+
/// Get the position
public: Vector3 GetPosition() const;
@@ -68,7 +71,7 @@
private: OgreVisual *visual;
private: Pose3d pose;
-
+ private: Pose3d comOffset;
};
}
#endif
Modified: code/gazebo/branches/bullet/server/physics/bullet/BulletPhysics.cc
===================================================================
--- code/gazebo/branches/bullet/server/physics/bullet/BulletPhysics.cc
2009-05-28 00:48:43 UTC (rev 7732)
+++ code/gazebo/branches/bullet/server/physics/bullet/BulletPhysics.cc
2009-05-28 23:16:05 UTC (rev 7733)
@@ -143,6 +143,8 @@
steps = std::max(steps,1);
+ //time = 0.000001;
+ //steps = 1;
this->dynamicsWorld->stepSimulation(time, steps, (**this->stepTimeP));
this->lastUpdateTime = Simulator::Instance()->GetRealTime();
Modified: code/gazebo/branches/bullet/server/physics/bullet/BulletRayGeom.cc
===================================================================
--- code/gazebo/branches/bullet/server/physics/bullet/BulletRayGeom.cc
2009-05-28 00:48:43 UTC (rev 7732)
+++ code/gazebo/branches/bullet/server/physics/bullet/BulletRayGeom.cc
2009-05-28 23:16:05 UTC (rev 7733)
@@ -104,7 +104,7 @@
this->relativeStartPos = posStart;
this->relativeEndPos = posEnd;
- this->globalStartPos =
this->body->GetPose().CoordPositionAdd(this->relativeStartPos);
+ this->globalStartPos = this->body->GetPose().CoordPositionAdd(
this->relativeStartPos);
this->globalEndPos =
this->body->GetPose().CoordPositionAdd(this->relativeEndPos);
// Compute the direction of the ray
Modified: code/gazebo/branches/bullet/server/physics/ode/ODEGeom.cc
===================================================================
--- code/gazebo/branches/bullet/server/physics/ode/ODEGeom.cc 2009-05-28
00:48:43 UTC (rev 7732)
+++ code/gazebo/branches/bullet/server/physics/ode/ODEGeom.cc 2009-05-28
23:16:05 UTC (rev 7733)
@@ -49,7 +49,6 @@
{
this->odePhysics = dynamic_cast<ODEPhysics*>(this->physicsEngine);
-
this->SetSpaceId( ((ODEBody*)this->body)->GetSpaceId());
this->geomId = NULL;
@@ -90,8 +89,6 @@
void ODEGeom::Update()
{
Geom::Update();
-
- //std::cout << "Geom[" << this->GetName() << "] Position[" <<
this->GetPose().pos << "]\n";
}
////////////////////////////////////////////////////////////////////////////////
Modified: code/gazebo/branches/bullet/server/physics/ode/ODERayGeom.cc
===================================================================
--- code/gazebo/branches/bullet/server/physics/ode/ODERayGeom.cc
2009-05-28 00:48:43 UTC (rev 7732)
+++ code/gazebo/branches/bullet/server/physics/ode/ODERayGeom.cc
2009-05-28 23:16:05 UTC (rev 7733)
@@ -92,8 +92,10 @@
{
Vector3 dir;
- this->globalStartPos =
this->body->GetPose().CoordPositionAdd(this->relativeStartPos);
- this->globalEndPos =
this->body->GetPose().CoordPositionAdd(this->relativeEndPos);
+ this->globalStartPos = this->body->GetPose().CoordPositionAdd(
+ this->relativeStartPos);
+ this->globalEndPos = this->body->GetPose().CoordPositionAdd(
+ this->relativeEndPos);
dir = this->globalEndPos - this->globalStartPos;
dir.Normalize();
@@ -115,8 +117,10 @@
this->relativeStartPos = posStart;
this->relativeEndPos = posEnd;
- this->globalStartPos =
this->body->GetPose().CoordPositionAdd(this->relativeStartPos);
- this->globalEndPos =
this->body->GetPose().CoordPositionAdd(this->relativeEndPos);
+ this->globalStartPos = this->body->GetPose().CoordPositionAdd(
+ this->relativeStartPos);
+ this->globalEndPos = this->body->GetPose().CoordPositionAdd(
+ this->relativeEndPos);
// Compute the direction of the ray
dir = this->globalEndPos - this->globalStartPos;
This was sent by the SourceForge.net collaborative development platform, the
world's largest Open Source development site.
------------------------------------------------------------------------------
Register Now for Creativity and Technology (CaT), June 3rd, NYC. CaT
is a gathering of tech-side developers & brand creativity professionals. Meet
the minds behind Google Creative Lab, Visual Complexity, Processing, &
iPhoneDevCamp as they present alongside digital heavyweights like Barbarian
Group, R/GA, & Big Spaceship. http://p.sf.net/sfu/creativitycat-com
_______________________________________________
Playerstage-commit mailing list
[email protected]
https://lists.sourceforge.net/lists/listinfo/playerstage-commit
