source: code/branches/campaignHS15/src/orxonox/controllers/FlyingController.cc @ 10903

/*
 *   ORXONOX - the hottest 3D action shooter ever to exist
 *                    > www.orxonox.net <
 *
 *
 *   License notice:
 *
 *   This program is free software; you can redistribute it and/or
 *   modify it under the terms of the GNU General Public License
 *   as published by the Free Software Foundation; either version 2
 *   of the License, or ( at your option )any later version.
 *
 *   This program is distributed in the hope that it will be useful,
 *   but WITHOUT ANY WARRANTY; without even the implied warranty of
 *   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 *   GNU General Public License for more details.
 *
 *   You should have received a copy of the GNU General Public License
 *   along with this program; if not, write to the Free Software
 *   Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301, USA.
 *
 *   Author:
 *      Gani Aliguzhinov

 *   Co-authors:
 *      Dominik Solenicki
 *
 */
#include "controllers/FlyingController.h"
#include "core/XMLPort.h"
#include "worldentities/pawns/SpaceShip.h"
#include "util/Math.h"
#include <OgreMatrix3.h>

namespace orxonox
{    
    RegisterClass (FlyingController);
    
    FlyingController::FlyingController(Context* context): CommonController(context)
    {
        RegisterObject(FlyingController);        
        this->rotationProgress_ = 0;
        this->spread_ = 200;
        this->tolerance_ = 80;
    }
    FlyingController::~FlyingController() 
    {

    }

    void FlyingController::XMLPort(Element& xmlelement, XMLPort::Mode mode)
    {
        XMLPortParam(FlyingController, "spread", setSpread, getSpread,  xmlelement, mode);
        XMLPortParam(FlyingController, "formationMode", setFormationModeXML, getFormationModeXML,  xmlelement, mode);
        SUPER(FlyingController, XMLPort, xmlelement, mode);
    }
   
    void FlyingController::setFormationModeXML(std::string val)
    {
        const std::string valUpper = getUppercase(val);
        FormationMode::Value value;
        
        if ( valUpper == "WALL" )
            value = FormationMode::WALL;
        else if ( valUpper == "FINGER4" )
            value = FormationMode::FINGER4;
        else if ( valUpper == "DIAMOND" )
            value = FormationMode::DIAMOND;
        else
            ThrowException(ParseError, std::string( "Attempting to set an unknown FormationMode: '" )+ val + "'.");
        this->setFormationMode(value);
    }
    std::string FlyingController::getFormationModeXML() const
    {
        switch ( this->formationMode_ )
        {
            case FormationMode::WALL:
            { return "WALL"; break; }
            case FormationMode::FINGER4:
            { return "FINGER4"; break; }
            case FormationMode::DIAMOND:
            { return "DIAMOND"; break; }
            default:
                return "DIAMOND"; break;
        }
    }
    void FlyingController::stopMoving()
    {
        this->bHasTargetPosition_ = false;
    }
    void FlyingController::moveToPosition(const Vector3& target, float dt)
    {
        ControllableEntity* entity = this->getControllableEntity();

        float distance = ( target - entity->getPosition() ).length();

        if ( distance >= this->tolerance_ )
        {
            Vector2 coord = get2DViewCoordinates
                ( entity->getPosition() , 
                entity->getOrientation()  * WorldEntity::FRONT, 
                entity->getOrientation()  * WorldEntity::UP, 
                target );
            float rotateX = -clamp( coord.x * 10, -1.0f, 1.0f );
            float rotateY = clamp( coord.y * 10, -1.0f, 1.0f );
            entity->rotateYaw( ROTATEFACTOR * rotateX * dt );
            entity->rotatePitch( ROTATEFACTOR * rotateY * dt );
          
            if (distance > this->tolerance_*1.5f || (rotateX > -0.01 && rotateX < 0.01 && rotateY > -0.01 && rotateY < 0.01))
                entity->moveFrontBack( SPEED * dt );
                copyTargetOrientation(dt);
        }
        else
        {      
            bHasTargetPosition_ = false;
        }
    }
    
    void FlyingController::moveToTargetPosition(float dt)
    {
        this->moveToPosition (this->targetPosition_, dt);
    }
    void FlyingController::copyOrientation(const Quaternion& orient, float dt)
    {
        //inspired by
        //http://www.ogre3d.org/tikiwiki/tiki-index.php?page=Quaternion+and+Rotation+Primer&structure=Tutorials#Q._How_can_I_make_my_objects_rotate_smoothly_You_mentioned_slerp_etc_
        //how can I make my objects rotate smoothly?
       
        Quaternion myOrient = this->getControllableEntity()->getOrientation();
        this->rotationProgress_ += dt;

        if (this->rotationProgress_ > 1)
        {
            this->rotationProgress_ = 0;
            this->bHasTargetOrientation_ = false;
        }
        else
        {

            Quaternion delta = Quaternion::Slerp(rotationProgress_, myOrient, orient, true);
            
            //rotate roll builds a Quaternion in roll method of WorldEntity, then it sets orientation.
            //it is faster just to set orientation, plus that way there is no need in calculating the roll angle.
            //On the downside, however, ship might also yaw and pitch, but this effect is neglectable, as we only call 
            //copyOrientation after we move our ship, thus it doesn't affect ships's flying direction too much.
            //If you don't like the code style, you are welcomed to uncomment the code below
            //and comment out setOrientation part, it will work just fine, but it will also be a tiny bit slower.
            //P.S. apperantly it did affect ship's direction and did so way too much.
            Matrix3 orientMatrix, myMatrix;

            delta.ToRotationMatrix(orientMatrix);
            myOrient.ToRotationMatrix (myMatrix);

            Radian yRad, pRad, rRad, yMy, pMy, rMy;
            orientMatrix.ToEulerAnglesYXZ(yRad, pRad, rRad);
            myMatrix.ToEulerAnglesYXZ (yMy, pMy, rMy);

            this->getControllableEntity()->rotateRoll ((rRad.valueRadians() - rMy.valueRadians())*ROTATEFACTOR*dt);
            // this->getControllableEntity()->setOrientation(delta);
        }
        
        
    }
    //change log: increased precision, increased rotation speed
    void FlyingController::copyTargetOrientation(float dt)
    {
        if (bHasTargetOrientation_)
        {   
            copyOrientation(targetOrientation_, dt);
        }
    }
    
    void FlyingController::setTargetPosition(const Vector3& target)
    {
        this->targetPosition_ = target;
        this->bHasTargetPosition_ = true;
    }

    void FlyingController::setTargetOrientation(const Quaternion& orient)
    {
        this->targetOrientation_=orient;
        this->bHasTargetOrientation_=true;
    }

    void FlyingController::setTargetOrientation(ControllableEntity* target)
    {
        if (target)
            setTargetOrientation(target->getOrientation());
    }
    void FlyingController::boostControl()
    {
        SpaceShip* ship = orxonox_cast<SpaceShip*>(this->getControllableEntity());
        if(ship == NULL) return;
        if(ship->getBoostPower()*1.5f > ship->getInitialBoostPower() ) //upper limit ->boost
        {
            this->getControllableEntity()->boost(true);
        }
        else if(ship->getBoostPower()*4.0f < ship->getInitialBoostPower()) //lower limit ->do not boost
        {
           this->getControllableEntity()->boost(false);
        }
    }
    void FlyingController::keepFormation(const ControllableEntity* leaderEntity, Vector3& targetRelativePosition)
    {
        ControllableEntity* myEntity = this->getControllableEntity();
        Vector3 myPosition = myEntity->getWorldPosition();

        if (!leaderEntity)
        {
            return;
        }
        Quaternion orient = leaderEntity->getWorldOrientation();
        Vector3 leaderPosition = leaderEntity->getWorldPosition();

        if (!leaderEntity)
        {
            return;
        }
        Vector3 targetAbsolutePosition = 
            (leaderPosition + (orient*WorldEntity::FRONT) * (leaderEntity->getVelocity().length()/5)
             + (orient* (targetRelativePosition)));
        //let ship finish rotating. also don't call copyOrientation to often as it is a slow function.
        if (this->actionCounter_ % 6 == 0 && !this->bHasTargetOrientation_)
            this->setTargetOrientation (orient);
        this->setTargetPosition (targetAbsolutePosition);
        if ((targetAbsolutePosition - myPosition).length() > this->tolerance_ * 1.5f)
        {
            this->boostControl();
        }
        else
        {
           this->getControllableEntity()->boost(false);
        }
    }
}