Permalink
Browse files

Move SetGlobalVelocity() and GetGlobalVelocity() into ModelPosition, …

…and implement them (taking the impelementation from Stage 3.2.2). Also, squash an unused var warning
  • Loading branch information...
1 parent f0e1c49 commit a879cca8286ba70e88b055040c1af3f256af71b9 Brian Gerkey committed Feb 15, 2012
Showing with 37 additions and 7 deletions.
  1. +1 −1 libstage/blockgroup.cc
  2. +32 −0 libstage/model_position.cc
  3. +4 −6 libstage/stage.hh
View
@@ -169,7 +169,7 @@ static void combineCallback(GLdouble coords[3],
// render each block as a polygon extruded into Z
void BlockGroup::BuildDisplayList()
{
- static bool init = false;
+ //static bool init = false;
static GLUtesselator *tobj = NULL;
if( ! mod.world->IsGUI() )
View
@@ -158,6 +158,38 @@ void ModelPosition::SetVelocity( const Velocity& val )
CallCallbacks( CB_VELOCITY );
}
+// get the model's velocity in the global frame
+Velocity ModelPosition::GetGlobalVelocity() const
+{
+ Pose gpose = GetGlobalPose();
+
+ double cosa = cos( gpose.a );
+ double sina = sin( gpose.a );
+
+ Velocity gv;
+ gv.x = velocity.x * cosa - velocity.y * sina;
+ gv.y = velocity.x * sina + velocity.y * cosa;
+ gv.a = velocity.a;
+
+ return gv;
+}
+
+// set the model's velocity in the global frame
+void ModelPosition::SetGlobalVelocity( const Velocity& gv
+ )
+{
+ Pose gpose = GetGlobalPose();
+
+ double cosa = cos( gpose.a );
+ double sina = sin( gpose.a );
+
+ Velocity lv;
+ lv.x = gv.x * cosa + gv.y * sina;
+ lv.y = -gv.x * sina + gv.y * cosa;
+ lv.a = gv.a;
+
+ this->SetVelocity( lv );
+}
void ModelPosition::Load( void )
{
View
@@ -2304,12 +2304,6 @@ namespace Stg
/** get the pose of a model in the global CS */
Pose GetGlobalPose() const;
- /** get the velocity of a model in the global CS */
- Velocity GetGlobalVelocity() const;
-
- /* set the velocity of a model in the global coordinate system */
- void SetGlobalVelocity( const Velocity& gvel );
-
/** subscribe to a model's data */
void Subscribe();
@@ -2976,6 +2970,10 @@ namespace Stg
frame. */
Velocity GetVelocity() const { return velocity; }
void SetVelocity( const Velocity& val );
+ /** get the velocity of a model in the global CS */
+ Velocity GetGlobalVelocity() const;
+ /* set the velocity of a model in the global coordinate system */
+ void SetGlobalVelocity( const Velocity& gvel );
/** Specify a point in space. Arrays of Waypoints can be attached to
Models and visualized. */

0 comments on commit a879cca

Please sign in to comment.