[Orxonox-commit 4383] r9054 - code/branches/waypoints/src/orxonox/controllers

[scmoritz at orxonox.net]

Author: scmoritz
Date: 2012-03-23 16:02:52 +0100 (Fri, 23 Mar 2012)
New Revision: 9054

Modified:
   code/branches/waypoints/src/orxonox/controllers/FormationController.cc
Log:
kein Zittern mehr, vereinzelte ungeplante Richtungsaenderungen

Modified: code/branches/waypoints/src/orxonox/controllers/FormationController.cc
===================================================================
--- code/branches/waypoints/src/orxonox/controllers/FormationController.cc	2012-03-23 13:53:20 UTC (rev 9053)
+++ code/branches/waypoints/src/orxonox/controllers/FormationController.cc	2012-03-23 15:02:52 UTC (rev 9054)
@@ -273,10 +273,14 @@
 
         Vector2 coord = get2DViewcoordinates(this->getControllableEntity()->getPosition(), this->getControllableEntity()->getOrientation() * WorldEntity::FRONT, this->getControllableEntity()->getOrientation() * WorldEntity::UP, target);
         float distance = (target - this->getControllableEntity()->getPosition()).length();
-		if(coord.x < 0.01 && coord.y < 0.01)
+		if(coord.x < 0.0001 && coord.y < 0.0001)
 		{
-			// I AM HERE 
-			//this->getControllableEntity()->setOrientation()  
+			// I AM HERE
+			Vector3 v_temp = this->getControllableEntity()->getPosition() * target;
+			Quaternion quat = v_temp.getRotationTo(target);
+			this->getControllableEntity()->rotate(quat);
+			
+			//this->getControllableEntity()->setOrientation(this->getControllableEntity()->getPosition().getRotationTo(target) * this->getControllableEntity()->getOrientation());
 		}
 
         if(this->state_ == FREE)
@@ -284,8 +288,8 @@
             if (this->target_ || distance > 10)
             {
                 // Multiply with ROTATEFACTOR_FREE to make them a bit slower
-                this->getControllableEntity()->rotateYaw(-1.0f * ROTATEFACTOR_FREE * sgn(coord.x) * coord.x);
-                this->getControllableEntity()->rotatePitch(ROTATEFACTOR_FREE * sgn(coord.y) * coord.y);
+                this->getControllableEntity()->rotateYaw(-1.0f * ROTATEFACTOR_FREE * coord.x * 2);
+                this->getControllableEntity()->rotatePitch(ROTATEFACTOR_FREE * coord.y * 2);
             }
 
             if (this->target_ && distance <  200 && this->getControllableEntity()->getVelocity().squaredLength() > this->target_->getVelocity().squaredLength())
@@ -300,8 +304,8 @@
         {
             if (this->target_ || distance > 10)
             {
-                this->getControllableEntity()->rotateYaw(-1.0f * ROTATEFACTOR_MASTER * sgn(coord.x) * coord.x*coord.x);
-                this->getControllableEntity()->rotatePitch(ROTATEFACTOR_MASTER * sgn(coord.y) * coord.y*coord.y);
+                this->getControllableEntity()->rotateYaw(-1.0f * ROTATEFACTOR_MASTER * coord.x * 2);
+                this->getControllableEntity()->rotatePitch(ROTATEFACTOR_MASTER * coord.y * 2);
             }
 
             if (this->target_ && distance < 200 && this->getControllableEntity()->getVelocity().squaredLength() > this->target_->getVelocity().squaredLength())
@@ -315,8 +319,8 @@
         if(this->state_ == SLAVE)
         {
 
-           this->getControllableEntity()->rotateYaw(-2.0f * ROTATEFACTOR_MASTER * sgn(coord.x) * coord.x*coord.x);
-           this->getControllableEntity()->rotatePitch(2.0f * ROTATEFACTOR_MASTER * sgn(coord.y) * coord.y*coord.y);
+           this->getControllableEntity()->rotateYaw(-2.0f * ROTATEFACTOR_MASTER * coord.x * 2);
+           this->getControllableEntity()->rotatePitch(2.0f * ROTATEFACTOR_MASTER * coord.y * 2);
 
             if (distance < 300)
             {