source: code/branches/AI_HS15/src/orxonox/controllers/CommonController.cc @ 10797

/*
 *   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:
 *      Fabian 'x3n' Landau
 *   Co-authors:
 *      Dominik Solenicki
 *
 */
#include "controllers/CommonController.h"
#include "core/XMLPort.h"

#include "weaponsystem/WeaponMode.h"
#include "weaponsystem/WeaponPack.h"
#include "weaponsystem/Weapon.h"
#include "weaponsystem/WeaponSlot.h"
#include "weaponsystem/WeaponSlot.h"
#include "worldentities/pawns/SpaceShip.h"

#include "Scene.h"
#include <OgreRay.h>
#include <OgreSceneQuery.h>
#include <OgreCamera.h>
#include <OgreSceneManager.h>
namespace orxonox
{

    RegisterClass(CommonController);
    float SPEED = 0.7f/0.02f;
    float ROTATEFACTOR = 0.3f/0.02f;

    CommonController::CommonController(Context* context) : Controller(context)
    {
        this->bSetupWorked = false;

        this->executingManeuver_ = false;
        this->executingMoveToPoint_ = false;

        this->maneuverType_ = ManeuverType::NONE;
        RegisterObject(CommonController);
    }


    CommonController::~CommonController()
    {
    }

    void CommonController::XMLPort(Element& xmlelement, XMLPort::Mode mode)
    {
        SUPER(CommonController, XMLPort, xmlelement, mode);
        XMLPortParam(CommonController, "formationMode", setFormationModeXML, getFormationModeXML,  xmlelement, mode);

    }
    void CommonController::setFormationModeXML(std::string val)
    {
        const std::string valUpper = getUppercase(val);
        FormationMode::Value value;
        if (valUpper == "VEE")
            value = FormationMode::VEE;
        else 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 CommonController::getFormationModeXML()
    {
        switch (this->formationMode_)
        {
            case FormationMode::VEE:
            {
                return "VEE";
                break;
            }
            case FormationMode::WALL:
            {
                return "WALL";
                break;
            }
            case FormationMode::FINGER4:
            {
                return "FINGER4";
                break;
            }
            case FormationMode::DIAMOND:
            {
                return "DIAMOND";
                break;
            }
            default:
                return "DIAMOND";
                break;

        }
    }
    void CommonController::maneuver()
    {

        if (this->target_ && this->bHasPositionOfTarget_ && this->getControllableEntity())
        {
            Vector3 diffVector = this->positionOfTarget_ - this->getControllableEntity()->getWorldPosition();
            float diffLength = diffVector.length();
            Vector3 diffUnit = diffVector/diffLength;

            Vector3 myForwardVector = this->getControllableEntity()->getOrientation() * WorldEntity::FRONT;
            float myDotProduct = diffVector.dotProduct(myForwardVector);

            Vector3 opponentForwardVector = this->target_->getOrientation() * WorldEntity::FRONT;
            float opponentDotProduct = diffVector.dotProduct(opponentForwardVector);            

            float myAngle = math::arccos ( myForwardVector.dotProduct(diffVector)/(diffLength) );
            float targetAngle = math::arccos ( opponentForwardVector.dotProduct(-diffVector)/(diffLength) );
            
            bool bThisIsLookingAtTarget = (myAngle/(diffLength*diffLength) < math::pi/8000000.0f);
            bool bTargetIsLookingAtThis = (targetAngle/(diffLength*diffLength) < math::pi/8000000.0f);
            
            float angleDiff = targetAngle - myAngle;
            //I am looking when my angle < pi/6
            //if his angle is bigger than mine
            if ( angleDiff > 0 )
            {
                //if diff is insignificant
                if ( bThisIsLookingAtTarget && bTargetIsLookingAtThis )
                {
                    //if this can make target overshoot
                    if ( diffLength < 200 )
                    {
                        Vector3* target = new Vector3 ( 0, -200, -200 );
                        moveToPoint( 
                            *target, 
                            randomInRange(45, 180) 
                            );
                    }
                    //do scissors
                    else
                    {
                        Vector3 target = (diffUnit) * 150.0f
                        Vector3* randVector = new Vector3( 
                            randomInRange(-300, 300), 
                            randomInRange(-300, 300), 
                            randomInRange(-300, 300)  
                        );
                        Vector3 projection = randVector.dotProduct(diffUnit) * diffUnit;
                        *randVector -= projection;
                        target += randVector;
                        moveToPoint( 
                            *target, 
                            randomInRange(45, 180) 
                            );
                    }
                }
                //this has advantage
                else
                {

                }
            }
           
        }
        if (this->getControllableEntity() && !this->target_)
        {
            this->maneuverType_ = ManeuverType::NONE;
        }
        orxout (internal_error) << "ManeuverType = " << this->maneuverType_ << endl;
    }
    void CommonController::chooseManeuverType()
    {

        if (this->target_ && this->bHasPositionOfTarget_ && this->getControllableEntity())
        {
            Vector3 diffVector = this->positionOfTarget_ - this->getControllableEntity()->getWorldPosition();
            Vector3 myForwardVector = this->getControllableEntity()->getOrientation() * WorldEntity::FRONT;
            float myDotProduct = diffVector.dotProduct(myForwardVector);

            Vector3 opponentForwardVector = this->target_->getOrientation() * WorldEntity::FRONT;
            float opponentDotProduct = diffVector.dotProduct(opponentForwardVector);            


            switch ((myDotProduct > 0) - (myDotProduct < 0))
            {
                case 1:
                {
                    switch ((opponentDotProduct > 0) - (opponentDotProduct < 0))
                    {
                        case 1:
                        {
                            this->maneuverType_ = ManeuverType::OFFENSIVE;
                            break;
                        }
                        case 0:
                        {
                            this->maneuverType_ = ManeuverType::OFFENSIVE;
                            break;
                        }
                        case -1:
                        {
                            this->maneuverType_ = ManeuverType::NEUTRAL;
                            break;
                        }
                    }
                    break;
                }
                case 0:
                {
                    switch ((opponentDotProduct > 0) - (opponentDotProduct < 0))
                    {
                        case 1:
                        {
                            this->maneuverType_ = ManeuverType::OFFENSIVE;
                            break;
                        }
                        case 0:
                        {
                            this->maneuverType_ = ManeuverType::NEUTRAL;
                            break;
                        }
                        case -1:
                        {
                            this->maneuverType_ = ManeuverType::DEFENCIVE;
                            break;
                        }
                    }

                    break;
                }
                case -1:
                {
                    switch ((opponentDotProduct > 0) - (opponentDotProduct < 0))
                    {
                        case 1:
                        {
                            this->maneuverType_ = ManeuverType::NEUTRAL;
                            break;
                        }
                        case 0:
                        {
                            this->maneuverType_ = ManeuverType::DEFENCIVE;
                            break;
                        }
                        case -1:
                        {
                            this->maneuverType_ = ManeuverType::DEFENCIVE;
                            break;
                        }
                    }
                    break;
                }
            }
        }
        if (this->getControllableEntity() && !this->target_)
        {
            this->maneuverType_ = ManeuverType::NONE;
        }
        orxout (internal_error) << "ManeuverType = " << this->maneuverType_ << endl;
    }
    bool CommonController::setWingman (CommonController* wingman)
    {
        return false;
    }
   
    bool CommonController::hasWingman()
    {
        return true;
    }
    void CommonController::setTarget(ControllableEntity* target)
    {
        this->target_ = target;
        orxout (internal_error) << " TARGET SET " << endl;
        
        if (this->target_ )
        {
            this->setPositionOfTarget(target_->getWorldPosition());

        }
    }
    bool CommonController::hasTarget()
    {
        if (this->target_)
            return true;
        return false;
    }
    void CommonController::setPositionOfTarget(const Vector3& target)
    {
        this->positionOfTarget_ = target;
        this->bHasPositionOfTarget_ = true;
    }
    void CommonController::setOrientationOfTarget(const Quaternion& orient)
    {
        this->orientationOfTarget_=orient;
        this->bHasOrientationOfTarget_=true;
    }

    void CommonController::setTargetPosition(const Vector3& target)
    {
        this->targetPosition_ = target;
        this->bHasTargetPosition_ = true;
    }

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

    void CommonController::setTargetOrientation(ControllableEntity* target)
    {
        if (target)
            setTargetOrientation(target->getOrientation());
    }

    /*void CommonController::spin()
    {
        this->moveToTargetPosition();
        this->getControllableEntity()->rotateRoll(8.0f);
    }
    void CommonController::turn180()
    {
        Vector2 coord = get2DViewdirection(this->getControllableEntity()->getPosition(), this->getControllableEntity()->getOrientation() * WorldEntity::FRONT, this->getControllableEntity()->getOrientation() * WorldEntity::UP, this->targetPosition_);

        this->getControllableEntity()->rotateYaw(-2.0f * sgn(coord.x) * coord.x*coord.x);
        this->getControllableEntity()->rotatePitch(2.0f * sgn(coord.y) * coord.y*coord.y);

        this->getControllableEntity()->moveFrontBack(SPEED);
    }*/



    //copy the Roll orientation of given Quaternion.
    void CommonController::copyOrientation(const Quaternion& orient, float dt)
    {
        //roll angle difference in radian
        float diff=orient.getRoll(false).valueRadians()-(this->getControllableEntity()->getOrientation().getRoll(false).valueRadians());
        while(diff>math::twoPi) diff-=math::twoPi;
        while(diff<-math::twoPi) diff+=math::twoPi;
        this->getControllableEntity()->rotateRoll(diff*ROTATEFACTOR * dt);
    }
    void CommonController::copyTargetOrientation(float dt)
    {
        if (bHasTargetOrientation_)
        {   
            copyOrientation(targetOrientation_, dt);
        }
    }




    void CommonController::moveToTargetPosition(float dt)
    {
        this->moveToPosition(this->targetPosition_, dt);
    }
    void CommonController::moveToPosition(const Vector3& target, float dt)
    {
        float factor = 1;
        if (!this->getControllableEntity())
            return;
        if (this->rank_ == Rank::DIVISIONLEADER)
            factor = 0.8;
        if (this->rank_ == Rank::SECTIONLEADER)
            factor = 0.9;
        
        //100 is (so far) the smallest tolerance (empirically found) that can be reached, 
        //with smaller distance spaceships can't reach position and go circles around it instead
        int tolerance = 60;

        ControllableEntity* entity = this->getControllableEntity();
        Vector2 coord = get2DViewCoordinates
            (entity->getPosition(), 
            entity->getOrientation() * WorldEntity::FRONT, 
            entity->getOrientation() * WorldEntity::UP, 
            target);

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

        //rotates should be in range [-1,+1], clamp cuts off all that is not
        float rotateX = clamp(coord.x * 10, -1.0f, 1.0f);
        float rotateY = clamp(coord.y * 10, -1.0f, 1.0f);

        
        if (distance > tolerance)
        {
            //Yaw and Pitch are enough to start facing the target
            this->getControllableEntity()->rotateYaw(-2.0f * ROTATEFACTOR * rotateX * dt);
            this->getControllableEntity()->rotatePitch(2.0f * ROTATEFACTOR * rotateY * dt);

            //300 works, maybe less is better
            if (distance < 400)
            {
                //Change roll when close. When Spaceship faces target, roll doesn't affect it's trajectory
                //It's important that roll is not changed in the process of changing yaw and pitch
                //Wingmen won't face same direction as Leaders, but when Leaders start moving
                //Yaw and Pitch will adapt.
                if (bHasTargetOrientation_)
                {
                    copyTargetOrientation(dt);
                }
            }

            this->getControllableEntity()->moveFrontBack(1.2f*SPEED*factor * dt);
        }
        else
        {      
            bHasTargetPosition_ = false;
            bHasTargetOrientation_ = false;
        }
    }
    float CommonController::randomInRange(float a, float b)
    {
        float random = rnd(1.0f);
        float diff = b - a;
        float r = random * diff;
        return a + r;
    }
    void CommonController::attack()
    {
        if ( !this->getControllableEntity() )
            return;
        if ( this->target_ )
        {
            this->positionOfTarget_ = getPredictedPosition( 
                this->getControllableEntity()->getWorldPosition(), 
                hardcoded_projectile_speed, 
                this->target_->getWorldPosition(), 
                this->target_->getVelocity() 
                );
            Vector3 diffVector = this->positionOfTarget_ - this->getControllableEntity()->getWorldPosition();
            float diffLength = diffVector.length();
            if (diffLength < 100)
            {
                Vector3* targetPosition;
                targetPosition = new Vector3 ( 
                    //randomInRange(200, 300), 
                    0,
                    //randomInRange(-300, -200), 
                    0,
                    randomInRange(-300, -400) 
                    );
                Quaternion rotationToTarget = (this->getControllableEntity()->getOrientation() * WorldEntity::FRONT).getRotationTo(diffVector);
                Vector3 target = rotationToTarget * (*targetPosition);
                moveToPoint( 
                    target, 
                    randomInRange(45, 180) 
                    );
                executingMoveToPoint_ = true; 
                return;
            }
            this->bShooting_ = true;
            this->positionOfTarget_ = getPredictedPosition( 
                this->getControllableEntity()->getWorldPosition(), 
                hardcoded_projectile_speed, 
                this->target_->getWorldPosition(), 
                this->target_->getVelocity() 
                );
            this->targetPosition_ = positionOfTarget_;

        }    
        else
        {
            this->chooseManeuverType();
        }
    }
    void CommonController::scissors()
    {
        if ( !this->getControllableEntity() )
            return;
        if ( this->target_ )
        {
            this->positionOfTarget_ = getPredictedPosition( 
                this->getControllableEntity()->getWorldPosition(), 
                hardcoded_projectile_speed, 
                this->target_->getWorldPosition(), 
                this->target_->getVelocity() 
                );
            Vector3 diffVector = this->positionOfTarget_ - this->getControllableEntity()->getWorldPosition();
            float diffLength = diffVector.length();
            Vector3 opponentForwardVector = this->target_->getOrientation() * WorldEntity::FRONT;
            float opponentDotProduct = diffVector.dotProduct(opponentForwardVector);            
            
            int f = (int) rnd(100.0f);
            f = (f % 2 == 0 ? 1 : -1);

            if(!this->executingMoveToPoint_)
            {
                Vector3* targetPosition;
                if ( diffLength < 100 )
                {
                    targetPosition = new Vector3 ( 
                        //f * randomInRange(200, 300), 
                        0,
                        //f * randomInRange(-300, -200), 
                        0,
                        //randomInRange(-300, -400) 
                        0
                        );
                }
                else
                {
                    if ( opponentDotProduct < 0 )
                    {
                        targetPosition = new Vector3 ( 
                        //f * randomInRange(200, 300), 
                        0,
                        //f * randomInRange(-300, -200), 
                        0,
                        //randomInRange(-300, -400) 
                        -300
                        );
                    }
                    else
                    {
                        targetPosition = new Vector3 ( 
                        //f * randomInRange(200, 300), 
                        0,
                        //f * randomInRange(-300, -200), 
                        0,
                        //randomInRange(-300, -400) 
                        300
                        );
                    }
                }
                Quaternion rotationToTarget = (this->getControllableEntity()->getOrientation() * WorldEntity::FRONT).getRotationTo(diffVector);
                Vector3 target = rotationToTarget * (*targetPosition);
                moveToPoint( 
                    target, 
                    randomInRange(45, 180) 
                    );
                executingMoveToPoint_ = true;  
            }
        }
        
        else
        {
            this->chooseManeuverType();
        }
    }
    void CommonController::gunsD()
    {
        if ( !this->getControllableEntity() )
            return;
        if ( this->target_ )
        {
            this->positionOfTarget_ = getPredictedPosition( 
                this->getControllableEntity()->getWorldPosition(), 
                hardcoded_projectile_speed, 
                this->target_->getWorldPosition(), 
                this->target_->getVelocity() 
                );
            Vector3 diffVector = this->positionOfTarget_ - this->getControllableEntity()->getWorldPosition();
            float diffLength = diffVector.length();
            if(!this->executingMoveToPoint_)
            {
                Vector3* targetPosition;
                if ( diffLength < 200 )
                {
                    targetPosition = new Vector3 ( 
                        //f * randomInRange(200, 300), 
                        0,
                        //f * randomInRange(-300, -200), 
                        0,
                        //randomInRange(-300, -400) 
                        0
                        );
                }
                else if ( diffLength < 500 )
                {
                    targetPosition = new Vector3 ( 
                        //randomInRange(100, 200), 
                        0,
                        //randomInRange(-200, -100), 
                        0,
                        //randomInRange(-400, -600) 
                        500
                        );
                }
                else
                {
                    targetPosition = new Vector3 ( 
                        //randomInRange(200, 300), 
                        0,
                        //randomInRange(-300, -200), 
                        0,
                        //randomInRange(-400, -600) 
                        500
                        );
                }
                Quaternion rotationToTarget = (this->getControllableEntity()->getOrientation() * WorldEntity::FRONT).getRotationTo(diffVector);
                Vector3 target = rotationToTarget * (*targetPosition);
                moveToPoint( 
                    target, 
                    randomInRange(45, 180) 
                    );
                executingMoveToPoint_ = true;  
            }
        }
        else
        {
            this->chooseManeuverType();
        }
    }
    //to be called in action
    //PRE: relativeTargetPosition is desired position relative to the spaceship,
    //angleRoll is the angle in degrees of Roll that should be applied by the end of the movement
    //POST: target orientation and position are set, so that it can be used by MoveAndRoll()
    void CommonController::moveToPoint(const Vector3& relativeTargetPosition, float angleRoll)
    {
        ControllableEntity* entity = this->getControllableEntity();
        if (!entity)
            return;
        Quaternion orient = entity->getWorldOrientation();
        Quaternion rotation = Quaternion(Degree(angleRoll), Vector3::UNIT_Z);

        Vector3 target = orient * relativeTargetPosition + entity->getWorldPosition();
        setTargetPosition(target);
        orient = orient * rotation;
        this->setTargetOrientation(orient);
       
    }
    //to be called in tick
    //PRE: MoveToPoint was called
    //POST: spaceship changes its yaw and pitch to point towards targetPosition_,
    //moves towards targetPosition_ by amount depending on dt and its speed,
    //rolls by amount depending on the difference between angleRoll_ and angleRolled_, dt, and 
    //angular speed
    //if position reached with a certain tolerance, and angleRolled_ = angleRoll_, returns false,
    //otherwise returns true
    //dt is normally around 0.02f, which makes it 1/0.02 = 50 frames/sec
    bool CommonController::moveAndRoll(float dt)
    {
        float factor = 1;
        if (!this->getControllableEntity())
            return false;
        if (this->rank_ == Rank::DIVISIONLEADER)
            factor = 0.8;
        if (this->rank_ == Rank::SECTIONLEADER)
            factor = 0.9;
        int tolerance = 60;
        
        ControllableEntity* entity = this->getControllableEntity();
        if (!entity)
            return true;
        Vector2 coord = get2DViewCoordinates
            (entity->getPosition(), 
            entity->getOrientation() * WorldEntity::FRONT, 
            entity->getOrientation() * WorldEntity::UP, 
            targetPosition_);

        float distance = (targetPosition_ - this->getControllableEntity()->getPosition()).length();

        //rotates should be in range [-1,+1], clamp cuts off all that is not
        float rotateX = clamp(coord.x * 10, -1.0f, 1.0f);
        float rotateY = clamp(coord.y * 10, -1.0f, 1.0f);

        
        if (distance > tolerance)
        {
            //Yaw and Pitch are enough to start facing the target
            this->getControllableEntity()->rotateYaw(-2.0f * ROTATEFACTOR * rotateX * dt);
            this->getControllableEntity()->rotatePitch(2.0f * ROTATEFACTOR * rotateY * dt);
            
            //Roll
            if (bHasTargetOrientation_)
            {
                copyTargetOrientation(dt);
            }
          
            //Move
            this->getControllableEntity()->moveFrontBack(1.2f * SPEED * factor * dt);
            //if still moving, return false
            return false;
        }
        else
        {     
            
            //if finished, return true;
            return true;
        }
    }

    float CommonController::squaredDistanceToTarget() const
    {
        if ( !this->getControllableEntity() )
            return 0;
        if ( !this->target_ )
            return ( this->getControllableEntity()->getPosition().squaredDistance(this->targetPosition_) );
        else
            return ( this->getControllableEntity()->getPosition().squaredDistance(this->target_->getPosition()) );
    }
    
    bool CommonController::isLookingAtTarget(float angle) const
    {
        if (!this->getControllableEntity())
            return false;

        return (getAngle(this->getControllableEntity()->getPosition(), this->getControllableEntity()->getOrientation() * WorldEntity::FRONT, this->targetPosition_) < angle);
    }

    bool CommonController::canFire()
    {
        float squaredDistance = squaredDistanceToTarget();
        if ( this->bShooting_ && squaredDistance < 9000000 && squaredDistance > 10000 && this->isLookingAtTarget(math::pi /(0.0002f*squaredDistance)) )
        {
            return true;
        }
        else
        {
            return false;
        }

    }
    void CommonController::doFire()
    {
        if (!this->target_ || !this->getControllableEntity())
            return;
        static const float hardcoded_projectile_speed = 750;

        this->targetPosition_ = getPredictedPosition(this->getControllableEntity()->getWorldPosition(), hardcoded_projectile_speed, this->target_->getWorldPosition(), this->target_->getVelocity());
        this->bHasTargetPosition_ = (this->targetPosition_ != Vector3::ZERO);

        Pawn* pawn = orxonox_cast<Pawn*>(this->getControllableEntity());

        if (pawn)
            //pawn->setAimPosition(this->getControllableEntity()->getWorldPosition() + 4000*(this->getControllableEntity()->getOrientation() * WorldEntity::FRONT));
            pawn->setAimPosition(this->targetPosition_);
    
        this->getControllableEntity()->fire(0);
    }
   

}