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

/*
 *   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 thisPosition = this->getControllableEntity() ->getWorldPosition();
            Quaternion thisOrientation = this->getControllableEntity() ->getOrientation();

            /*this->setPositionOfTarget( getPredictedPosition( 
                thisPosition, 
                hardcoded_projectile_speed, 
                this->target_->getWorldPosition() , 
                this->target_->getVelocity()  
                ) );*/
            this->setPositionOfTarget( this->target_->getWorldPosition()  );
            this->setOrientationOfTarget( this->target_->getControllableEntity() ->getOrientation() );


            Vector3 diffVector = this->positionOfTarget_ - thisPosition;
            float diffLength = diffVector.length();
            Vector3 diffUnit = diffVector/diffLength;

            Vector3 thisForwardVector = thisOrientation * WorldEntity::FRONT;
            float thisDotProduct = diffVector.dotProduct( thisForwardVector );

            Vector3 targetForwardVector = this->orientationOfTarget_ * WorldEntity::FRONT;
            float targetDotProduct = diffVector.dotProduct( targetForwardVector );            

            float thisAngle = getAngle( thisPosition, thisForwardVector, this->positionOfTarget_ ); 
            float targetAngle = getAngle( this->positionOfTarget_, targetForwardVector, thisPosition ); 


            bool bThisIsLookingAtTarget = ( thisAngle/( diffLength*diffLength ) < math::pi/8000000.0f );
            bool bTargetIsLookingAtThis = ( targetAngle/( diffLength*diffLength ) < math::pi/8000000.0f );
            
            float angleDiff = targetAngle - thisAngle;

            //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 too close
                    if ( diffLength < 300 )
                    {
                        this->setTargetPosition( this->getControllableEntity() ->getWorldPosition()  );
                    }
                    //move closer
                    else
                    {
                        this->setTargetPosition( this->positionOfTarget_ - 0.6f*diffVector );
                    }
                }
            }
           
        }
        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 thisForwardVector = this->getControllableEntity() ->getOrientation()  * WorldEntity::FRONT;
            float thisDotProduct = diffVector.dotProduct( thisForwardVector );

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


            switch ( ( thisDotProduct > 0 )- ( thisDotProduct < 0 ))
            {
                case 1:
                {
                    switch ( ( targetDotProduct > 0 )- ( targetDotProduct < 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 ( ( targetDotProduct > 0 )- ( targetDotProduct < 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 ( ( targetDotProduct > 0 )- ( targetDotProduct < 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 targetForwardVector = this->target_->getOrientation()  * WorldEntity::FRONT;
            float targetDotProduct = diffVector.dotProduct( targetForwardVector );            
            
            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 ( targetDotProduct < 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 );
    }
   

}