[Orxonox-commit 4237] r8908 - code/branches/formation/src/orxonox/controllers

[willis at orxonox.net]

Author: willis
Date: 2011-10-26 20:34:40 +0200 (Wed, 26 Oct 2011)
New Revision: 8908

Modified:
   code/branches/formation/src/orxonox/controllers/ArtificialController.cc
   code/branches/formation/src/orxonox/controllers/ArtificialController.h
Log:
formationflight: different form of formation + correct orientation added

Modified: code/branches/formation/src/orxonox/controllers/ArtificialController.cc
===================================================================
--- code/branches/formation/src/orxonox/controllers/ArtificialController.cc	2011-10-26 14:22:52 UTC (rev 8907)
+++ code/branches/formation/src/orxonox/controllers/ArtificialController.cc	2011-10-26 18:34:40 UTC (rev 8908)
@@ -79,6 +79,7 @@
         this->specificMasterActionHoldCount_  = 0;
         this->bShooting_ = false;
         this->bHasTargetPosition_ = false;
+	this->bHasTargetOrientation_=false;
         this->speedCounter_ = 0.2f;
         this->targetPosition_ = Vector3::ZERO;
 
@@ -371,9 +372,14 @@
 
             if (distance < 300)
             {
+		 if (bHasTargetOrientation_)
+		    {
+			copyTargetOrientation();
+		    }
                 if (distance < 40)
                 {
                     this->getControllableEntity()->moveFrontBack(0.8f*SPEED_MASTER);
+		   
                 } else this->getControllableEntity()->moveFrontBack(1.2f*SPEED_MASTER);
 
             } else {
@@ -384,6 +390,7 @@
         if (distance < 10)
         {
             this->positionReached();
+	    bHasTargetOrientation_=false;
         }
     }
 
@@ -392,6 +399,25 @@
         this->moveToPosition(this->targetPosition_);
     }
 
+    void ArtificialController::copyOrientation(const Quaternion& orient)
+    {
+	//roll angle in radian, difference between master and slave
+	float diff=orient.getRoll().valueRadians()-(this->getControllableEntity()->getOrientation().getRoll().valueRadians());
+	if ((diff<math::twoPi && diff>math::pi) || diff>(math::pi)*3)
+	{
+		diff=diff-math::twoPi;
+	}
+	this->getControllableEntity()->rotateRoll(1.0f*ROTATEFACTOR_MASTER*diff);
+    }
+
+    void ArtificialController::copyTargetOrientation()
+    {
+	if (bHasTargetOrientation_)
+  	{
+		copyOrientation(targetOrientation_);
+	}
+    }
+
     /**
         @brief Unregisters a slave from its master. Initiated by a slave.
     */
@@ -475,11 +501,11 @@
             this->myMaster_ = 0;
         }
     }
-
-    /**
+ /**
         @brief Commands the slaves of a master into a formation. Sufficiently fast not to be called within tick. Initiated by a master.
     */
-    void ArtificialController::commandSlaves()
+
+void ArtificialController::commandSlaves()
     {
         if(this->state_ != MASTER) return;
 
@@ -493,27 +519,28 @@
             this->slaves_.front()->setTargetPosition(dest);
         }
         else
+	// formation:
         {
             dest += 1.0f*orient*WorldEntity::BACK;
             Vector3 pos = Vector3::ZERO;
+	    bool left=true;
             int i = 1;
-
+	    
             for(std::vector<ArtificialController*>::iterator it = slaves_.begin(); it != slaves_.end(); it++)
             {
                 pos = Vector3::ZERO;
-                if (i <= 1) pos += dest  + (float)FORMATION_WIDTH*(orient*WorldEntity::LEFT);
-                if (i == 2) pos += dest  + (float)FORMATION_WIDTH*(orient*WorldEntity::RIGHT);
-                if (i == 3) pos += dest  + (float)FORMATION_WIDTH*(orient*WorldEntity::UP);
-                if (i >= 4)
-                {
-                    pos += dest  + (float)FORMATION_WIDTH*(orient*WorldEntity::DOWN);
-                    i = 1;
-                    dest += (float)FORMATION_LENGTH*(orient*WorldEntity::BACK);
-                    (*it)->setTargetPosition(pos);
-                    continue;
-                }
-                i++;
+		if (left)
+		{
+                    pos+=dest+i*FORMATION_WIDTH*(orient*WorldEntity::LEFT);
+		} else
+		{
+		    pos+=dest+i*FORMATION_WIDTH*(orient*WorldEntity::RIGHT);
+		    i++;
+		    dest+=FORMATION_LENGTH*(orient*WorldEntity::BACK);
+		}		
+		(*it)->setTargetOrientation(orient);
                 (*it)->setTargetPosition(pos);
+		left=!left;
             }
         }
     }
@@ -820,6 +847,18 @@
         this->bHasTargetPosition_ = true;
     }
 
+    void ArtificialController::setTargetOrientation(const Quaternion& orient)
+    {
+	this->targetOrientation_=orient;	
+	this->bHasTargetOrientation_=true;
+    }
+
+    void ArtificialController::setTargetOrientation(Pawn* target)
+    {
+	if (target)
+	    setTargetOrientation(target->getOrientation());
+    }
+
     void ArtificialController::setTarget(Pawn* target)
     {
         this->target_ = target;

Modified: code/branches/formation/src/orxonox/controllers/ArtificialController.h
===================================================================
--- code/branches/formation/src/orxonox/controllers/ArtificialController.h	2011-10-26 14:22:52 UTC (rev 8907)
+++ code/branches/formation/src/orxonox/controllers/ArtificialController.h	2011-10-26 18:34:40 UTC (rev 8908)
@@ -95,6 +95,8 @@
 
             void moveToPosition(const Vector3& target);
             void moveToTargetPosition();
+	    void copyOrientation(const Quaternion& orient);
+	    void copyTargetOrientation();
 
             virtual void positionReached() {}
 
@@ -123,6 +125,9 @@
             void setTargetPosition(const Vector3& target);
             void searchRandomTargetPosition();
 
+	    void setTargetOrientation(const Quaternion& orient);
+	    void setTargetOrientation(Pawn* target);
+
             void setTarget(Pawn* target);
             void searchNewTarget();
             void forgetTarget();
@@ -137,6 +142,10 @@
 
             bool bHasTargetPosition_;
             Vector3 targetPosition_;
+
+	    bool bHasTargetOrientation_;
+	    Quaternion targetOrientation_;
+
             WeakPtr<Pawn> target_;
             bool bShooting_;