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

…and implement them (taking the impelementation from Stage 3.2.2). Also, squash an unused var warning
keevi7c3l · Feb 15, 2012 · a879cca · a879cca
1 parent f0e1c49
commit a879cca
Show file tree

Hide file tree

Showing 3 changed files with 37 additions and 7 deletions.
diff --git a/libstage/blockgroup.cc b/libstage/blockgroup.cc
@@ -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() )

diff --git a/libstage/model_position.cc b/libstage/model_position.cc
@@ -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 )
 {

diff --git a/libstage/stage.hh b/libstage/stage.hh
@@ -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. */