CWE-ou-minkata/Sources/Plasma/PubUtilLib/plAvatar/plAvCallbackAction.cpp

/*==LICENSE==*

CyanWorlds.com Engine - MMOG client, server and tools
Copyright (C) 2011  Cyan Worlds, Inc.

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 3 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, see <http://www.gnu.org/licenses/>.

Additional permissions under GNU GPL version 3 section 7

If you modify this Program, or any covered work, by linking or
combining it with any of RAD Game Tools Bink SDK, Autodesk 3ds Max SDK,
NVIDIA PhysX SDK, Microsoft DirectX SDK, OpenSSL library, Independent
JPEG Group JPEG library, Microsoft Windows Media SDK, or Apple QuickTime SDK
(or a modified version of those libraries),
containing parts covered by the terms of the Bink SDK EULA, 3ds Max EULA,
PhysX SDK EULA, DirectX SDK EULA, OpenSSL and SSLeay licenses, IJG
JPEG Library README, Windows Media SDK EULA, or QuickTime SDK EULA, the
licensors of this Program grant you additional
permission to convey the resulting work. Corresponding Source for a
non-source form of such a combination shall include the source code for
the parts of OpenSSL and IJG JPEG Library used as well as that of the covered
work.

You can contact Cyan Worlds, Inc. by email legal@cyan.com
 or by snail mail at:
      Cyan Worlds, Inc.
      14617 N Newport Hwy
      Mead, WA   99021

*==LICENSE==*/
#include "plAvCallbackAction.h"
#include "plMessage/plLOSHitMsg.h"

#include "plArmatureMod.h" // for LOS enum type
#include "plMatrixChannel.h"
#include "hsTimer.h"
#include "plPhysicalControllerCore.h"

// Generic geom utils.
hsBool LinearVelocity(hsVector3 &outputV, float elapsed, hsMatrix44 &prevMat, hsMatrix44 &curMat);
void AngularVelocity(hsScalar &outputV, float elapsed, hsMatrix44 &prevMat, hsMatrix44 &curMat);
float AngleRad2d (float x1, float y1, float x3, float y3);
inline hsVector3 GetYAxis(hsMatrix44 &mat)
{
    return hsVector3(mat.fMap[1][0], mat.fMap[1][1], mat.fMap[1][2]);
}

plAnimatedController::plAnimatedController(plSceneObject* rootObject, plAGApplicator* rootApp, plPhysicalControllerCore* controller)
    : fRootObject(rootObject)
    , fRootApp(rootApp)
    , fController(controller)
    , fTurnStr(0.f)
    , fAnimAngVel(0.f)
    , fAnimPosVel(0.f, 0.f, 0.f)
{
}   

void plAnimatedController::RecalcVelocity(double timeNow, double timePrev, hsBool useAnim /* = true */)
{
    if (useAnim)
    {
        // while you may think it would be correct to cache this,
        // what we're actually asking is "what would the animation's
        // position be at the previous time given its *current*
        // parameters (particularly blends)"
        hsMatrix44 prevMat = ((plMatrixChannel *)fRootApp->GetChannel())->Value(timePrev, true);        
        hsMatrix44 curMat = ((plMatrixChannel *)fRootApp->GetChannel())->Value(timeNow, true);
        
        // If we get a valid linear velocity (ie, we didn't wrap around in the anim),
        // use it.  Otherwise just reuse the previous frames velocity.
        hsVector3 linearVel;
        if (LinearVelocity(linearVel, (float)(timeNow - timePrev), prevMat, curMat))
            fAnimPosVel = linearVel;

        // Automatically sets fAnimAngVel
        AngularVelocity(fAnimAngVel, (float)(timeNow - timePrev), prevMat, curMat);
    }
    else
    {
        fAnimPosVel.Set(0.f, 0.f, 0.f);
        fAnimAngVel = 0.f;
    }

    if (fController)
        fController->SetVelocities(fAnimPosVel, fAnimAngVel + fTurnStr);
}

///////////////////////////////////////////////////////////////////////////

const hsScalar plWalkingController::kControlledFlightThreshold = 1.f; // seconds

plWalkingController::plWalkingController(plSceneObject* rootObject, plAGApplicator* rootApp, plPhysicalControllerCore* controller)
    : plAnimatedController(rootObject, rootApp, controller)
    , fHitGroundInThisAge(false)
    , fWaitingForGround(false)
    , fControlledFlightTime(0)
    , fControlledFlight(0)
    , fImpactTime(0.f)
    , fImpactVelocity(0.f, 0.f, 0.f)
    , fClearImpact(false)
    , fGroundLastFrame(false)
{
    if (fController)
    {
        fWalkingStrategy= TRACKED_NEW plWalkingStrategy(fController);
        fController->SetMovementSimulationInterface(fWalkingStrategy);
    }
    else
        fWalkingStrategy = nil;
}

void plWalkingController::RecalcVelocity(double timeNow, double timePrev, hsBool useAnim)
{
    if (!fHitGroundInThisAge && fController && fController->IsEnabled() && fWalkingStrategy->IsOnGround())
        fHitGroundInThisAge = true; // if we're not pinned and we're not in an age yet, we are now.

    if (fClearImpact)
    {
        fImpactTime = 0.f;
        fImpactVelocity.Set(0.f, 0.f, 0.f);
    }

    if (fController && !fWalkingStrategy->IsOnGround())
    {
        fImpactTime = fWalkingStrategy->GetAirTime();
        fImpactVelocity = fController->GetLinearVelocity();
        fClearImpact = false;
    }
    else
        fClearImpact = true;
    
    if (IsControlledFlight())
    {
        if (fWalkingStrategy && fWalkingStrategy->IsOnGround())
            fControlledFlightTime = fWalkingStrategy->GetAirTime();
        if(fGroundLastFrame&&(fWalkingStrategy && !fWalkingStrategy->IsOnGround()))
        {
            //we have started to leave the ground tell the movement strategy in case it cares
            fWalkingStrategy->StartJump();
        }
        if (fControlledFlightTime > kControlledFlightThreshold)
            EnableControlledFlight(false);
    }
    if (fWalkingStrategy)
        fGroundLastFrame = fWalkingStrategy->IsOnGround();
    else
        fGroundLastFrame=false;
    plAnimatedController::RecalcVelocity(timeNow, timePrev, useAnim);
}

void plWalkingController::Reset(bool newAge)
{
    
    ActivateController();
    if (newAge)
    {
        if (fWalkingStrategy)
        fWalkingStrategy->ResetAirTime();
        fHitGroundInThisAge = false;
    }
}
 void plWalkingController::ActivateController()
{
    if (fWalkingStrategy)
    {
        fWalkingStrategy->RefreshConnectionToControllerCore();
    }
    else
    {   
        fWalkingStrategy= TRACKED_NEW plWalkingStrategy(fController);
        fWalkingStrategy->RefreshConnectionToControllerCore();

    }
}

bool plWalkingController::EnableControlledFlight(bool status)
{
    if (status)
    {
        if (fControlledFlight == 0)
            fControlledFlightTime = 0.f;

        ++fControlledFlight;
        fWaitingForGround = true;
    }
    else
        fControlledFlight = __max(--fControlledFlight, 0);

    return status;
}
plWalkingController::~plWalkingController()
{
    delete fWalkingStrategy;
    if (fController)
            fController->SetMovementSimulationInterface(nil);
}
#if 0
void plWalkingController::Update()
{
//  double elapsed = time.asDouble() - getRefresh().asDouble();
//  setRefresh(time);
//  
//  hsBool isPhysical = !fPhysical->GetProperty(plSimulationInterface::kPinned);
//  const Havok::Vector3 straightUp(0.0f, 0.0f, 1.0f);
//  hsBool alreadyInAge = fHitGroundInThisAge;
//  
//  int numContacts = fPhysical->GetNumContacts();
//  bool ground = false;
//  fPushingPhysical = nil;
//  int i, j;

/*  for(i = 0; i < numContacts; i++)
    {
        plHKPhysical *contactPhys = fPhysical->GetContactPhysical(i);
        if (!contactPhys)
            continue; // Physical no longer exists. Skip it.

        const Havok::ContactPoint *contact = fPhysical->GetContactPoint(i);
        hsScalar dotUp = straightUp.dot(contact->m_normal);
        if (dotUp > .5)
            ground = true;
        else if (contactPhys->GetProperty(plSimulationInterface::kAvAnimPushable))
        {
            hsPoint3 position;
            hsQuat rotation;
            fPhysical->GetPositionAndRotationSim(&position, &rotation);
        
            hsQuat inverseRotation = rotation.Inverse();
            hsVector3 normal(contact->m_normal.x, contact->m_normal.y, contact->m_normal.z);
            fFacingPushingPhysical = (inverseRotation.Rotate(&kAvatarForward).InnerProduct(normal) < 0 ? true : false);
        
            fPushingPhysical = contactPhys;
        }
    }

    // We need to check for the case where the avatar hasn't collided with "ground", but is colliding
    // with a few other objects so that he's not actually falling (wedged in between some slopes).
    // We do this by answering the following question (in 2d top-down space): "If you sort the contact 
    // normals by angle, is there a large enough gap between normals?"
    // 
    // If you think in terms of geometry, this means a collection of surfaces are all pushing on you.
    // If they're pushing from all sides, you have nowhere to go, and you won't fall. There needs to be
    // a gap, so that you're pushed out and have somewhere to fall. This is the same as finding a gap 
    // larger than 180 degrees between sorted normals.
    // 
    // The problem is that on top of that, the avatar needs enough force to shove him out that gap (he
    // has to overcome friction). I deal with that by making the threshold (360 - (180 - 60) = 240). I've
    // seen up to 220 reached in actual gameplay in a situation where we'd want this to take effect. 
    // This is the same running into 2 walls where the angle between them is 60.
    const hsScalar threshold = hsScalarDegToRad(240);
    if (!ground && numContacts >= 2)
    {
        // Can probably do a special case for exactly 2 contacts. Not sure if it's worth it...

        fCollisionAngles.SetCount(numContacts);
        for (i = 0; i < numContacts; i++)
        {
            const Havok::ContactPoint *contact = fPhysical->GetContactPoint(i);
            fCollisionAngles[i] = hsATan2(contact->m_normal.y, contact->m_normal.x);
        }

        // numContacts is rarely larger than 6, so let's do a simple bubble sort.
        for (i = 0; i < numContacts; i++)
        {
            for (j = i + 1; j < numContacts; j++)
            {
                if (fCollisionAngles[i] > fCollisionAngles[j])
                {
                    hsScalar tempAngle = fCollisionAngles[i];
                    fCollisionAngles[i] = fCollisionAngles[j];
                    fCollisionAngles[j] = tempAngle;
                }
            }
        }
        
        // sorted, now we check.
        for (i = 1; i < numContacts; i++)
        {
            if (fCollisionAngles[i] - fCollisionAngles[i - 1] >= threshold)
                break;
        }
        
        if (i == numContacts)
        {
            // We got to the end. Check the last with the first and make your decision.
            if (!(fCollisionAngles[0] - fCollisionAngles[numContacts - 1] >= (threshold - 2 * hsScalarPI)))
                ground = true;
        }
    }
*/

    bool ground = fController ? fController->GotGroundHit() : true;
    bool isPhysical = true;

    if (!fHitGroundInThisAge && isPhysical)
        fHitGroundInThisAge = true; // if we're not pinned and we're not in an age yet, we are now.
    
    if (IsControlledFlight())
        fControlledFlightTime += (hsScalar)elapsed;
    if (fControlledFlightTime > kControlledFlightThreshold && numContacts > 0)
        EnableControlledFlight(false);
    
    if (ground || !isPhysical)
    {
        if (!IsControlledFlight() && !IsOnGround())
        {
            // The first ground contact in an age doesn't count.
//          if (alreadyInAge)
//          {
//              hsVector3 vel;
//              fPhysical->GetLinearVelocitySim(vel);
//              fImpactVel = vel.fZ;
//              fTimeInAirPeak = (hsScalar)(fTimeInAir + elapsed);
//          }
            
            fWaitingForGround = false;              
        }           
        fTimeInAir = 0;
    }
    else if (elapsed < plSimulationMgr::GetInstance()->GetMaxDelta())
    {
        // If the simultation skipped a huge chunk of time, we didn't process the
        // collisions, which could trick us into thinking we've just gone a long
        // time without hitting ground. So we only count the time if this wasn't
        // the case.
        fTimeInAir += (hsScalar)elapsed;
    }
    
    
    // Tweakage so that we still fall under the right conditions.
    // If we're in controlled flight, or standing still with ground solidly under us (probe hit). We only use anim velocity.
//  if (!IsControlledFlight() && !(ground && fProbeHitGround && fAnimPosVel.fX == 0 && fAnimPosVel.fY == 0)) 
//  {
//      hsVector3 curV;
//      fPhysical->GetLinearVelocitySim(curV);
//      fAnimPosVel.fZ = curV.fZ;
// 
//      // Prevents us from going airborn from running up bumps/inclines.
//      if (IsOnGround() && fAnimPosVel.fZ > 0.f)
//          fAnimPosVel.fZ = 0.f;
//      
//      // Unless we're on the ground and moving, or standing still with a probe hit, we use the sim's other axes too.
//      if (!(IsOnGround() && (fProbeHitGround || fAnimPosVel.fX != 0 || fAnimPosVel.fY != 0)))
//      {
//          fAnimPosVel.fX = curV.fX;
//          fAnimPosVel.fY = curV.fY;
//      }
//  }
// 
//  fPhysical->SetLinearVelocitySim(fAnimPosVel);
//  fPhysical->SetSpin(fAnimAngVel + fTurnStr, hsVector3(0.0f, 0.0f, 1.0f));    
}
#endif


#if 0

/////////////////////////////////////////////////////////////////////////

plSimDefs::ActionType plHorizontalFreezeAction::GetType()
{
    return plSimDefs::kHorizontalFreeze;
}

void plHorizontalFreezeAction::apply(Havok::Subspace &s, Havok::hkTime time)
{
    double elapsed = time.asDouble() - getRefresh().asDouble();
    setRefresh(time);
    
    int numContacts = fPhysical->GetNumContacts();
    bool ground = false;
    const Havok::Vector3 straightUp(0.0f, 0.0f, 1.0f);  
    int i;
    for(i = 0; i < numContacts; i++)
    {
        const Havok::ContactPoint *contact = fPhysical->GetContactPoint(i);
        hsScalar dotUp = straightUp.dot(contact->m_normal);
        if (dotUp > .5)
            ground = true;
    }

    hsVector3 vel;
    fPhysical->GetLinearVelocitySim(vel);
    vel.fX = 0.0;
    vel.fY = 0.0;
    if (ground)
        vel.fZ = 0;
    fPhysical->SetLinearVelocitySim(vel);
    fPhysical->ClearContacts();
}
#endif
plSwimmingController::plSwimmingController(plSceneObject* rootObject, plAGApplicator* rootApp, plPhysicalControllerCore* controller)
:plAnimatedController(rootObject,rootApp,controller)
{
    if (controller)
        fSwimmingStrategy= TRACKED_NEW plSwimStrategy(controller);
    else
        fSwimmingStrategy = nil;
}
plSwimmingController::~plSwimmingController()
{
    delete fSwimmingStrategy;
}

plRidingAnimatedPhysicalController::plRidingAnimatedPhysicalController(plSceneObject* rootObject, plAGApplicator* rootApp, plPhysicalControllerCore* controller)
: plWalkingController(rootObject, rootApp, controller)
{
    if(controller)
        fWalkingStrategy = TRACKED_NEW plRidingAnimatedPhysicalStrategy(controller);
    else
        fWalkingStrategy = nil;
}
plRidingAnimatedPhysicalController::~plRidingAnimatedPhysicalController()
{
    delete fWalkingStrategy;
    fWalkingStrategy=nil;
}


//////////////////////////////////////////////////////////////////////////


/*
Purpose:

ANGLE_RAD_2D returns the angle in radians swept out between two rays in 2D.

Discussion:

Except for the zero angle case, it should be true that

ANGLE_RAD_2D(X1,Y1,X2,Y2,X3,Y3)
+ ANGLE_RAD_2D(X3,Y3,X2,Y2,X1,Y1) = 2 * PI

Modified:

19 April 1999

Author:

John Burkardt

Parameters:

Input, float X1, Y1, X2, Y2, X3, Y3, define the rays
( X1-X2, Y1-Y2 ) and ( X3-X2, Y3-Y2 ) which in turn define the
angle, counterclockwise from ( X1-X2, Y1-Y2 ).

Output, float ANGLE_RAD_2D, the angle swept out by the rays, measured
in radians.  0 <= ANGLE_DEG_2D < 2 PI.  If either ray has zero length,
then ANGLE_RAD_2D is set to 0.
*/

static float AngleRad2d ( float x1, float y1, float x3, float y3 )
{
    float value;
    float x;
    float y;

    x = ( x1 ) * ( x3 ) + ( y1 ) * ( y3 );
    y = ( x1 ) * ( y3 ) - ( y1 ) * ( x3 );

    if ( x == 0.0 && y == 0.0 ) {
        value = 0.0;
    }
    else 
    {
        value = atan2 ( y, x );

        if ( value < 0.0 ) 
        {
            value = (float)(value + TWO_PI);
        }
    }
    return value;
}

static hsBool LinearVelocity(hsVector3 &outputV, float elapsed, hsMatrix44 &prevMat, hsMatrix44 &curMat)
{
    bool result = false;

    hsPoint3 startPos(0.0f, 0.0f, 0.0f);                    // default position (at start of anim)
    hsPoint3 prevPos = prevMat.GetTranslate();              // position previous frame
    hsPoint3 nowPos = curMat.GetTranslate();                // position current frame

    hsVector3 prev2Now = (hsVector3)(nowPos - prevPos);     // frame-to-frame delta

    if (fabs(prev2Now.fX) < 0.0001f && fabs(prev2Now.fY) < 0.0001f && fabs(prev2Now.fZ) < 0.0001f)
    {
        outputV.Set(0.f, 0.f, 0.f);
        result = true;
    } 
    else 
    {
        hsVector3 start2Now = (hsVector3)(nowPos - startPos);   // start-to-frame delta

        float prev2NowMagSqr = prev2Now.MagnitudeSquared();
        float start2NowMagSqr = start2Now.MagnitudeSquared();

        float dot = prev2Now.InnerProduct(start2Now);

        // HANDLING ANIMATION WRAPPING:
        // the vector from the animation origin to the current frame should point in roughly
        // the same direction as the vector from the previous animation position to the
        // current animation position.
        //
        // If they don't agree (dot < 0,) then we probably mpst wrapped around. 
        // The right answer would be to compare the current frame to the start of
        // the anim loop, but it's cheaper to cheat and return false,
        // telling the caller to use the previous frame's velocity.
        if (dot > 0.0f)
        {
            prev2Now /= elapsed;

            float xfabs = fabs(prev2Now.fX);
            float yfabs = fabs(prev2Now.fY);
            float zfabs = fabs(prev2Now.fZ);
            static const float maxVel = 20.0f;
            hsBool valid = xfabs < maxVel && yfabs < maxVel && zfabs < maxVel;

            if (valid)
            {
                outputV = prev2Now;
                result = true;
            }
        } 
    }

    return result;
}

static void AngularVelocity(hsScalar &outputV, float elapsed, hsMatrix44 &prevMat, hsMatrix44 &curMat)
{
    outputV = 0.f;
    hsScalar appliedVelocity = 0.0f;
    hsVector3 prevForward = GetYAxis(prevMat);
    hsVector3 curForward = GetYAxis(curMat);

    hsScalar angleSincePrev = AngleRad2d(curForward.fX, curForward.fY, prevForward.fX, prevForward.fY);
    hsBool sincePrevSign = angleSincePrev > 0.0f;
    if (angleSincePrev > hsScalarPI)
        angleSincePrev = angleSincePrev - TWO_PI;

    const hsVector3 startForward = hsVector3(0, -1.0, 0);   // the Y orientation of a "resting" armature....
    hsScalar angleSinceStart = AngleRad2d(curForward.fX, curForward.fY, startForward.fX, startForward.fY);
    hsBool sinceStartSign = angleSinceStart > 0.0f;
    if (angleSinceStart > hsScalarPI)
        angleSinceStart = angleSinceStart - TWO_PI;

    // HANDLING ANIMATION WRAPPING:
    // under normal conditions, the angle from rest to the current frame will have the same
    // sign as the angle from the previous frame to the current frame.
    // if it does not, we have (most likely) wrapped the motivating animation from frame n back
    // to frame zero, creating a large angle from the previous frame to the current one
    if (sincePrevSign == sinceStartSign)
    {
        // signs are the same; didn't wrap; use the frame-to-frame angle difference
        appliedVelocity = angleSincePrev / elapsed; // rotation / time
        if (fabs(appliedVelocity) < 3)
        {
            outputV = appliedVelocity;
        }
    }
}