エージェントの衝突_old

テスト実行

#include "Controller.h"
 #include "ControllerEvent.h"
 #include "Logger.h"
 #include "math.h"
 
 #define PI 3.141592
 #define DEG2RAD(DEG) ( (PI) * (DEG) / 180.0 )
 
 
 class AgentController: public Controller {
 public:
   double onAction(ActionEvent &evt); 
 
   void onInit(InitEvent &evt);
 
   //衝突時に呼出される関数onCollisionの利用を宣言します。
   void onCollision(CollisionEvent &evt);
 
 private:
 
   //衝突フラグ
   bool Colli;
   int Colli_cnt;
   double vel;
 };
 
 void AgentController::onInit(InitEvent &evt)
 {
   Colli = false;
   Colli_cnt = 0; 
  
  //"robot_000"の速度設定 
  if(strstr(myname() , "000")!= NULL)
    {
      vel = 5;
    }
 
  //"robot_001"の速度設定
  else  if(strstr(myname() , "001")!= NULL)
    { 
    vel = 7;
    }
  else
    vel = 0;
 }
 //衝突時に呼び出されます。
 void AgentController::onCollision(CollisionEvent &evt) {
 
  if (Colli == false && Colli_cnt == 0){
    typedef CollisionEvent::WithC C;
 
    // 衝突相手の名前を得る。
    const C & with = evt.getWith();
    for (C::const_iterator i=with.begin(); i!=with.end(); i++) {
 
      //衝突相手の名前をsに代入します。
       std::string s = *i;
 
      //衝突相手の名前表示します。
      LOG_MSG(("\"%s\"", s.c_str()));
      SimObj *my = getObj(myname());
 
      // 自分のy軸周りの回転を得ます(クオータニオン)
      double qy = my->qy();
 
      //クオータニオンから回転角を導出します
      double theta = 2*asin(qy);
 
      double dy = theta + DEG2RAD(-180);
      if (dy <= -PI) {
	dy = -1*dy - PI;
      }
 
      //体全体を180°後に回転します。
      my->setAxisAndAngle(0, 1.0, 0, dy);
      Colli = true;
      Colli_cnt = 3;
    }
  }
 }
 
 double AgentController::onAction(ActionEvent &evt) {
 
   try {
 
    SimObj *my = getObj(myname());
 
    //自分の位置を得ます。
    double x = my->x();
    double y = my->y();
    double z = my->z();
 
    //y軸周りの自分の回転を得ます。（クオータニオン）
 
    double qy = my->qy();
 
    //クオータニオンから回転角を導出します。
    double theta = 2*asin(qy);
 
    double dx = 0;
    double dz = 0;
 
    //移動する方向を決定します。
    dx = sin(theta) * vel;
    dz = cos(theta) * vel;
 
    //移動します。
    my->setPosition( x + dx, y , z + dz );
 
    if (Colli_cnt > 0)
      {
	if (--Colli_cnt <=0)
	  Colli = false;
      }
 
  } catch (SimObj::Exception &) {
  }
 
  return 0.5;
 
 }
 extern "C" Controller * createController() {
  return new AgentController;
 }

recent(20)