float getPositiveAngle(float angle)
  {
    float ret = angle; 
    if (angle > PI)   ret = TWO_PI - angle;
    if (angle < -PI)  ret = TWO_PI + angle;
    return ret;
  }