CWE-ou-minkata/MOULOpenSourceClientPlugin/Plasma20/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/>.

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;
		}
	}
}