/*==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 "plPhysicalControllerCore.h"

#include "plArmatureMod.h"
#include "plSwimRegion.h"
#include "plMatrixChannel.h"
#include "../pnSceneObject/plCoordinateInterface.h"
#include "../../NucleusLib/inc/plPhysical.h"
#include "../../NucleusLib/pnMessage/plCorrectionMsg.h"

// Gravity constants
#define kGravity -32.174f
#define kTerminalVelocity kGravity

static inline hsVector3 GetYAxis(hsMatrix44 &mat) { return hsVector3(mat.fMap[1][0], mat.fMap[1][1], mat.fMap[1][2]); }
static float AngleRad2d(float x1, float y1, float x3, float y3);

bool CompareMatrices(const hsMatrix44 &matA, const hsMatrix44 &matB, float tolerance);


// plPhysicalControllerCore
plPhysicalControllerCore::plPhysicalControllerCore(plKey OwnerSceneObject, hsScalar height, hsScalar radius) 
	: fOwner(OwnerSceneObject), 
	fWorldKey(nil), 
	fHeight(height), 
	fRadius(radius), 
	fLOSDB(plSimDefs::kLOSDBNone), 
	fMovementStrategy(nil), 
	fSimLength(0.0f), 
	fLocalRotation(0.0f, 0.0f, 0.0f, 1.0f), 
	fLocalPosition(0.0f, 0.0f, -2000.0f), 
	fLastLocalPosition(0.0f, 0.0f, 0.0f),
	fLinearVelocity(0.0f, 0.0f, 0.0f), 
	fAchievedLinearVelocity(0.0f, 0.0f, 0.0f), 
	fPushingPhysical(nil), 
	fFacingPushingPhysical(false),
	fSeeking(false), 
	fEnabled(false), 
	fEnableChanged(false)
{
	fLastGlobalLoc.Reset();
	fPrevSubworldW2L.Reset();
}

const plCoordinateInterface* plPhysicalControllerCore::GetSubworldCI()
{
	if (fWorldKey)
	{
		plSceneObject* so = plSceneObject::ConvertNoRef(fWorldKey->ObjectIsLoaded());
		if (so)
			return so->GetCoordinateInterface();
	}

	return nil;
}

void plPhysicalControllerCore::IncrementAngle(hsScalar deltaAngle)
{
	hsVector3 axis;
	hsScalar angle;

	fLocalRotation.NormalizeIfNeeded();
	fLocalRotation.GetAngleAxis(&angle, &axis);
	if (axis.fZ < 0)
		angle = (2.0f * hsScalarPI) - angle; // axis is backwards, so reverse the angle too

	angle += deltaAngle;

	// make sure we wrap around
	if (angle < 0.0f)
		angle = (2.0f * hsScalarPI) + angle; // angle is -, so this works like a subtract
	if (angle >= (2.0f * hsScalarPI))
		angle = angle - (2.0f * hsScalarPI);

	// set the new angle
	axis.Set(0.0f, 0.0f, 1.0f);
	fLocalRotation.SetAngleAxis(angle, axis);
}

void plPhysicalControllerCore::IApply(hsScalar delSecs)
{
	fSimLength = delSecs;

	// Match controller to owner if transform has changed since the last frame
	plSceneObject* so = plSceneObject::ConvertNoRef(fOwner->ObjectIsLoaded());
	const hsMatrix44& l2w = so->GetCoordinateInterface()->GetLocalToWorld();
	if (!CompareMatrices(fLastGlobalLoc, l2w, 0.0001f))
		SetGlobalLoc(l2w);

	if (fEnabled)
	{
		// Convert velocity from avatar to world space
		if (!fLinearVelocity.IsEmpty())
		{
			fLinearVelocity = l2w * fLinearVelocity;
		
			const plCoordinateInterface* subworldCI = GetSubworldCI();
			if (subworldCI)
				fLinearVelocity = subworldCI->GetWorldToLocal() * fLinearVelocity;
		}

		fMovementStrategy->Apply(delSecs);
	}
}
void plPhysicalControllerCore::IUpdate(int numSubSteps, hsScalar alpha)
{
	if (fEnabled)
	{
		// Update local position and acheived velocity
		fLastLocalPosition = fLocalPosition;
		GetPositionSim(fLocalPosition);
		hsVector3 displacement = (hsVector3)(fLocalPosition - fLastLocalPosition);
		fAchievedLinearVelocity = displacement / fSimLength;

		displacement /= (hsScalar)numSubSteps;
		fLastLocalPosition = fLocalPosition - displacement;
		hsPoint3 interpLocalPos = fLastLocalPosition + (displacement * alpha);

		// Update global location
		fLocalRotation.MakeMatrix(&fLastGlobalLoc);
		fLastGlobalLoc.SetTranslate(&interpLocalPos);
		const plCoordinateInterface* subworldCI = GetSubworldCI();
		if (subworldCI)
		{
			const hsMatrix44& subL2W = subworldCI->GetLocalToWorld();
			fLastGlobalLoc = subL2W * fLastGlobalLoc;
			fPrevSubworldW2L = subworldCI->GetWorldToLocal();
		}

		fMovementStrategy->Update(fSimLength);
		ISendCorrectionMessages(true);
	}
	else
	{
		fAchievedLinearVelocity.Set(0.0f, 0.0f, 0.0f);

		// Update global location if in a subworld
		const plCoordinateInterface* subworldCI = GetSubworldCI();
		if (subworldCI)
		{
			hsMatrix44 l2s = fPrevSubworldW2L * fLastGlobalLoc;
			const hsMatrix44& subL2W = subworldCI->GetLocalToWorld();
			fLastGlobalLoc = subL2W * l2s;
			fPrevSubworldW2L = subworldCI->GetWorldToLocal();

			ISendCorrectionMessages();
		}
	}

	if (fEnableChanged)
		IHandleEnableChanged();
}
void plPhysicalControllerCore::IUpdateNonPhysical(hsScalar alpha)
{
	// Update global location if owner transform hasn't changed.
	plSceneObject* so = plSceneObject::ConvertNoRef(fOwner->ObjectIsLoaded());
	const hsMatrix44& l2w = so->GetCoordinateInterface()->GetLocalToWorld();
	if (CompareMatrices(fLastGlobalLoc, l2w, 0.0001f))
	{
		if (fEnabled)
		{
			hsVector3 displacement = (hsVector3)(fLocalPosition - fLastLocalPosition);
			hsPoint3 interpLocalPos = fLastLocalPosition + (displacement * alpha);

			fLocalRotation.MakeMatrix(&fLastGlobalLoc);
			fLastGlobalLoc.SetTranslate(&interpLocalPos);
			const plCoordinateInterface* subworldCI = GetSubworldCI();
			if (subworldCI)
			{
				const hsMatrix44& subL2W = subworldCI->GetLocalToWorld();
				fLastGlobalLoc = subL2W * fLastGlobalLoc;
				fPrevSubworldW2L = subworldCI->GetWorldToLocal();
			}

			ISendCorrectionMessages();
		}
		else
		{
			// Update global location if in a subworld
			const plCoordinateInterface* subworldCI = GetSubworldCI();
			if (subworldCI)
			{
				hsMatrix44 l2s = fPrevSubworldW2L * fLastGlobalLoc;
				const hsMatrix44& subL2W = subworldCI->GetLocalToWorld();
				fLastGlobalLoc = subL2W * l2s;
				fPrevSubworldW2L = subworldCI->GetWorldToLocal();

				ISendCorrectionMessages();
			}
		}
	}
}

void plPhysicalControllerCore::ISendCorrectionMessages(bool dirtySynch)
{
	plCorrectionMsg* corrMsg = TRACKED_NEW plCorrectionMsg();
	corrMsg->fLocalToWorld = fLastGlobalLoc;
	corrMsg->fLocalToWorld.GetInverse(&corrMsg->fWorldToLocal);
	corrMsg->fDirtySynch = dirtySynch;
	corrMsg->AddReceiver(fOwner);
	corrMsg->Send();
}


// Movement Strategy
plMovementStrategy::plMovementStrategy(plPhysicalControllerCore* controller)
	: fController(controller)
{
}

void plMovementStrategy::Reset(bool newAge) { fController->SetMovementStrategy(this); }


// Animated Movement Strategy
plAnimatedMovementStrategy::plAnimatedMovementStrategy(plAGApplicator* rootApp, plPhysicalControllerCore* controller)
	: plMovementStrategy(controller),
	fRootApp(rootApp),
	fAnimLinearVel(0.0f, 0.0f, 0.0f),
	fAnimAngularVel(0.0f),
	fTurnStr(0.0f)
{
}

void plAnimatedMovementStrategy::RecalcVelocity(double timeNow, hsScalar elapsed, hsBool useAnim)
{
	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(timeNow - elapsed, true);		
		hsMatrix44 curMat = ((plMatrixChannel *)fRootApp->GetChannel())->Value(timeNow, true);
		
		IRecalcLinearVelocity(elapsed, prevMat, curMat);
		IRecalcAngularVelocity(elapsed, prevMat, curMat);
	}
	else
	{
		fAnimLinearVel.Set(0.0f, 0.0f, 0.0f);
		fAnimAngularVel = 0.0f;
	}

	// Update controller rotation
	hsScalar zRot = fAnimAngularVel + fTurnStr;
	if (hsABS(zRot) > 0.0001f)
		fController->IncrementAngle(zRot * elapsed);

	// Update controller velocity
	fController->SetLinearVelocity(fAnimLinearVel);
}

void plAnimatedMovementStrategy::IRecalcLinearVelocity(float elapsed, hsMatrix44 &prevMat, hsMatrix44 &curMat)
{
	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)
	{
		fAnimLinearVel.Set(0.f, 0.f, 0.f);
	} 
	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 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)
			{
				fAnimLinearVel = prev2Now;
			}
		} 
	}
}

void plAnimatedMovementStrategy::IRecalcAngularVelocity(float elapsed, hsMatrix44 &prevMat, hsMatrix44 &curMat)
{
	fAnimAngularVel = 0.0f;
	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.0f, -1.0f, 0.0f);	// 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)
		{
			fAnimAngularVel = appliedVelocity;
		}
	}
}


// Walking Strategy
plWalkingStrategy::plWalkingStrategy(plAGApplicator* rootApp, plPhysicalControllerCore* controller)
	: plAnimatedMovementStrategy(rootApp, controller),
	fSlidingNormals(),
	fImpactVelocity(0.0f, 0.0f, 0.0f),
	fImpactTime(0.0f),
	fTimeInAir(0.0f),
	fControlledFlightTime(0.0f),
	fControlledFlight(0),
	fGroundHit(false),
	fFalseGround(false),
	fHeadHit(false),
	fClearImpact(false),
	fHitGroundInThisAge(false)
{
}

void plWalkingStrategy::Apply(hsScalar delSecs)
{
	hsVector3 velocity = fController->GetLinearVelocity();
	hsVector3 achievedVelocity = fController->GetAchievedLinearVelocity();

	// Add in gravity if the avatar's z velocity isn't being set explicitly
	if (hsABS(velocity.fZ) < 0.001f)
	{
		// Get our previous z velocity.  If we're on the ground, clamp it to zero at
		// the largest, so we won't launch into the air if we're running uphill.
		hsScalar prevZVel = achievedVelocity.fZ;
		if (IsOnGround())
			prevZVel = hsMinimum(prevZVel, 0.0f);

		velocity.fZ = prevZVel + (kGravity * delSecs);
	}

	// If we're airborne and the velocity isn't set, use the velocity from
	// the last frame so we maintain momentum.
	if (!IsOnGround() && velocity.fX == 0.0f && velocity.fY == 0.0f)
	{
		velocity.fX = achievedVelocity.fX;
		velocity.fY = achievedVelocity.fY;
	}

	if (!fGroundHit && fSlidingNormals.Count())
	{
		// We're not on solid ground, so we should be sliding against whatever
		// we're hitting (like a rock cliff). Each vector in fSlidingNormals is
		// the surface normal of a collision that's too steep to be ground, so
		// we project our current velocity onto that plane and slide along the
		// wall.
		//
		// Also, sometimes PhysX reports a bunch of collisions from the wall,
		// but nothing from underneath (when there should be). So if we're not
		// touching ground, we offset the avatar in the direction of the 
		// surface normal(s). This doesn't fix the issue 100%, but it's a hell
		// of a lot better than nothing, and suitable duct tape until a future
		// PhysX revision fixes the issue.
		//
		// Yes, there's room for optimization here if we care.
		hsVector3 offset(0.0f, 0.0f, 0.0f);
		for (int i = 0; i < fSlidingNormals.GetCount(); i++)
		{
			offset += fSlidingNormals[i];
			hsVector3 velNorm = velocity;

			if (velNorm.MagnitudeSquared() > 0.0f)
				velNorm.Normalize();

			if (velNorm * fSlidingNormals[i] < 0.0f)
			{
				hsVector3 proj = (velNorm % fSlidingNormals[i]) % fSlidingNormals[i];
				if (velNorm * proj < 0.0f)
					proj *= -1.0f;

				velocity = velocity.Magnitude() * proj;
			}
		}
		if (offset.MagnitudeSquared() > 0.0f)
		{
			// 5 ft/sec is roughly the speed we walk backwards.
			// The higher the value, the less likely you'll trip
			// the bug, and this seems reasonable.
			offset.Normalize();
			velocity += offset * 5.0f;
		}
	}

	if (velocity.fZ < kTerminalVelocity)
		velocity.fZ = kTerminalVelocity;

	// Convert to a displacement vector
	hsVector3 displacement = velocity * delSecs;

	// Reset vars and move the controller
	fController->SetPushingPhysical(nil);
	fController->SetFacingPushingPhysical(false);
	fGroundHit = fFalseGround = fHeadHit = false;
	fSlidingNormals.SetCount(0);

	unsigned int collideResults = 0;
	unsigned int collideFlags = 1<<plSimDefs::kGroupStatic | 1<<plSimDefs::kGroupAvatarBlocker | 1<<plSimDefs::kGroupDynamic;
	if (!fController->IsSeeking())
		collideFlags |= (1<<plSimDefs::kGroupExcludeRegion);

	fController->Move(displacement, collideFlags, collideResults);

	if ((!fGroundHit) && (collideResults & kBottom))
		fFalseGround = true;

	if (collideResults & kTop)
		fHeadHit = true;
}
void plWalkingStrategy::Update(hsScalar delSecs)
{
	if (fGroundHit || fFalseGround)
		fTimeInAir = 0.0f;
	else
	{
		fTimeInAir += delSecs;
		if (fHeadHit)
		{ 
			// If we're airborne and hit our head, override achieved velocity to avoid being shoved sideways
			hsVector3 velocity = fController->GetLinearVelocity();
			hsVector3 achievedVelocity = fController->GetAchievedLinearVelocity();
		
			achievedVelocity.fX = velocity.fX;
			achievedVelocity.fY = velocity.fY;
			if (achievedVelocity.fZ > 0.0f)
				achievedVelocity.fZ = 0.0f;

			fController->OverrideAchievedLinearVelocity(achievedVelocity);
		}
	}

	hsVector3 zeroVelocity(0.f, 0.f, 0.f);
	fController->SetLinearVelocity(zeroVelocity);

	if (!fHitGroundInThisAge && IsOnGround())
		fHitGroundInThisAge = true;

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

	if (IsOnGround())
		fClearImpact = true;
	else
	{
		fImpactTime = fTimeInAir;
		fImpactVelocity = fController->GetAchievedLinearVelocity();
		// convert orientation from subworld to avatar-local coordinates
		fImpactVelocity = (hsVector3)fController->GetLocalRotation().Rotate(&fImpactVelocity);
		fClearImpact = false;
	}
}

void plWalkingStrategy::AddContactNormals(hsVector3& vec)
{
	hsScalar dot = vec * kAvatarUp;
	if (dot >= 0.5f)
		fGroundHit = true;
	else
		fSlidingNormals.Append(vec);
}

void plWalkingStrategy::Reset(bool newAge)
{
	plMovementStrategy::Reset(newAge);
	fImpactVelocity.Set(0.0f, 0.0f, 0.0f);
	fImpactTime = 0.0f;
	if (newAge)
	{
		fTimeInAir = 0.0f;
		fClearImpact = true;
		fHitGroundInThisAge = false;
		fSlidingNormals.SetCount(0);
	}
}

void plWalkingStrategy::RecalcVelocity(double timeNow, hsScalar elapsed, hsBool useAnim)
{
	if (fControlledFlight != 0)
	{
		if (IsOnGround())
			fControlledFlightTime = fTimeInAir;

		if (fControlledFlightTime > kControlledFlightThreshold)
			EnableControlledFlight(false);
	}

	plAnimatedMovementStrategy::RecalcVelocity(timeNow, elapsed, useAnim);
}

bool plWalkingStrategy::EnableControlledFlight(bool status)
{
	if (status)
	{
		if (fControlledFlight == 0)
			fControlledFlightTime = 0.0f;

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

	return status;
}

plPhysical* plWalkingStrategy::GetPushingPhysical() const { return fController->GetPushingPhysical(); }
bool plWalkingStrategy::GetFacingPushingPhysical() const { return fController->GetFacingPushingPhysical(); }

const hsScalar plWalkingStrategy::kAirTimeThreshold = 0.1f;
const hsScalar plWalkingStrategy::kControlledFlightThreshold = 1.0f;


// Swim Strategy
plSwimStrategy::plSwimStrategy(plAGApplicator* rootApp, plPhysicalControllerCore* controller) 
	: plAnimatedMovementStrategy(rootApp, controller),
	fBuoyancy(0.0f),
	fSurfaceHeight(0.0f),
	fCurrentRegion(nil),
	fOnGround(false),
	fHadContacts(false)
{
}

void plSwimStrategy::Apply(hsScalar delSecs)
{
	hsVector3 velocity = fController->GetLinearVelocity();
	hsVector3 achievedVelocity = fController->GetAchievedLinearVelocity();

	IAdjustBuoyancy();
	
	//trying to dampen the oscillations
	hsScalar retardent = 0.0f;
	static hsScalar finalBobSpeed = 0.5f;
	if ((achievedVelocity.fZ > finalBobSpeed) || (achievedVelocity.fZ < -finalBobSpeed))
		retardent = achievedVelocity.fZ * -0.90f;

	hsScalar zacc = (1.0f - fBuoyancy) * kGravity + retardent;
	velocity.fZ += (zacc * delSecs);

	velocity.fZ += achievedVelocity.fZ;

	// Water Current
	if (fCurrentRegion != nil)
	{
		hsScalar angCurrent = 0.0f;
		hsVector3 linCurrent(0.0f, 0.0f, 0.0f);
		fCurrentRegion->GetCurrent(fController, linCurrent, angCurrent, delSecs);

		if (hsABS(angCurrent) > 0.0001f)
			fController->IncrementAngle(angCurrent * delSecs);

		velocity += linCurrent;

		if (velocity.fZ > fCurrentRegion->fMaxUpwardVel)
			velocity.fZ = fCurrentRegion->fMaxUpwardVel;
	}

	if (velocity.fZ < kTerminalVelocity)
		velocity.fZ = kTerminalVelocity;

	// Convert to displacement vector
	hsVector3 displacement = velocity * delSecs;

	// Reset vars and move controller //
	fController->SetPushingPhysical(nil);
	fController->SetFacingPushingPhysical(false);
	fHadContacts = fOnGround = false;

	unsigned int collideResults = 0;
	unsigned int collideFlags = 1<<plSimDefs::kGroupStatic | 1<<plSimDefs::kGroupAvatarBlocker | 1<<plSimDefs::kGroupDynamic;
	if (!fController->IsSeeking())
		collideFlags |= (1<<plSimDefs::kGroupExcludeRegion);

	fController->Move(displacement, collideFlags, collideResults);

	if ((collideResults & kBottom) || (collideResults & kSides))
		fHadContacts = true;
}

void plSwimStrategy::AddContactNormals(hsVector3& vec)
{
	hsScalar dot = vec * kAvatarUp;
	if (dot >= kSlopeLimit)
		fOnGround = true;
}

void plSwimStrategy::SetSurface(plSwimRegionInterface *region, hsScalar surfaceHeight)
{
	fCurrentRegion = region;
	fSurfaceHeight = surfaceHeight;
}
void plSwimStrategy::IAdjustBuoyancy()
{
	// "surface depth" refers to the depth our handle object should be below
	// the surface for the avatar to be "at the surface"
	static const float surfaceDepth = 4.0f;
	// 1.0 = neutral buoyancy
	// 0 = no buoyancy (normal gravity)
	// 2.0 = opposite of gravity, floating upwards
	static const float buoyancyAtSurface = 1.0f;

	if (fCurrentRegion == nil)
	{
		fBuoyancy = 0.f;
		return;
	}

	hsPoint3 posSim;
	fController->GetPositionSim(posSim);
	float depth = fSurfaceHeight - posSim.fZ;

	// this isn't a smooth transition but hopefully it won't be too obvious
	if (depth <= 0.0) //all the away above water
		fBuoyancy = 0.f; // Same as being above ground. Plain old gravity.
	else if (depth >= 5.0f) 
		fBuoyancy = 3.0f; //completely Submereged
	else 
		fBuoyancy = depth / surfaceDepth;
}


// Dynamic Walking Strategy
plDynamicWalkingStrategy::plDynamicWalkingStrategy(plAGApplicator* rootApp, plPhysicalControllerCore* controller)
	: plWalkingStrategy(rootApp, controller)
{
}

void plDynamicWalkingStrategy::Apply(hsScalar delSecs)
{
	hsVector3 velocity = fController->GetLinearVelocity();
	hsVector3 achievedVelocity = fController->GetAchievedLinearVelocity();

	// Add in gravity if the avatar's z velocity isn't being set explicitly
	if (hsABS(velocity.fZ) < 0.001f)
	{
		// Get our previous z velocity.  If we're on the ground, clamp it to zero at
		// the largest, so we won't launch into the air if we're running uphill.
		hsScalar prevZVel = achievedVelocity.fZ;
		if (IsOnGround())
			prevZVel = hsMinimum(prevZVel, 0.f);

		velocity.fZ = prevZVel + (kGravity * delSecs);
	}

	if (velocity.fZ < kTerminalVelocity)
		velocity.fZ = kTerminalVelocity;

	fController->SetPushingPhysical(nil);
	fController->SetFacingPushingPhysical(false);
	fGroundHit = fFalseGround = false;

	hsScalar groundZVelocity;
	if (ICheckForGround(groundZVelocity))
		velocity.fZ += groundZVelocity;

	fController->SetLinearVelocitySim(velocity);
}

bool plDynamicWalkingStrategy::ICheckForGround(hsScalar& zVelocity)
{
	std::vector<plControllerSweepRecord> groundHits;
	UInt32 collideFlags = 1<<plSimDefs::kGroupStatic | 1<<plSimDefs::kGroupAvatarBlocker | 1<<plSimDefs::kGroupDynamic;

	hsPoint3 startPos;
	fController->GetPositionSim(startPos);
	hsPoint3 endPos = startPos;

	// Set sweep length
	startPos.fZ += 0.05f;
	endPos.fZ -= 0.05f;

	int possiblePlatformCount = fController->SweepControllerPath(startPos, endPos, true, true, collideFlags, groundHits);
	if (possiblePlatformCount)
	{
		zVelocity = -FLT_MAX;

		std::vector<plControllerSweepRecord>::iterator curRecord; 
		for (curRecord = groundHits.begin(); curRecord != groundHits.end(); ++curRecord)
		{
			if (curRecord->ObjHit != nil)
			{
				hsVector3 objVelocity;
				curRecord->ObjHit->GetLinearVelocitySim(objVelocity);
				if (objVelocity.fZ > zVelocity)
					zVelocity = objVelocity.fZ;

				fGroundHit = true;
			}
		}
	}

	return fGroundHit;
}


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

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