EXT-6953 WIP fixed walk cycle foot slip feedback to not speed up animation so much

also, made camera default target and offset live-settable for debugging

reviewed by vir

2 files changed, 110 insertions, 166 deletions

diff --git a/indra/llcharacter/llkeyframewalkmotion.cpp b/indra/llcharacter/llkeyframewalkmotion.cpp
index f814618fc1..0587e5642c 100644
--- a/indra/llcharacter/llkeyframewalkmotion.cpp
+++ b/indra/llcharacter/llkeyframewalkmotion.cpp
@@ -44,32 +44,31 @@
 //-----------------------------------------------------------------------------
 // Macros
 //-----------------------------------------------------------------------------
-const F32 MAX_WALK_PLAYBACK_SPEED = 8.f;	// max m/s for which we adjust walk cycle speed
-
-const F32 MIN_WALK_SPEED = 0.1f;	// minimum speed at which we use velocity for down foot detection
-const F32 MAX_TIME_DELTA = 2.f;		//max two seconds a frame for calculating interpolation
-F32 SPEED_ADJUST_MAX = 2.5f; // maximum adjustment of walk animation playback speed
-F32 SPEED_ADJUST_MAX_SEC = 3.f;	// maximum adjustment to walk animation playback speed for a second
-F32 ANIM_SPEED_MAX = 10.0f; // absolute upper limit on animation speed
-F32 ANIM_SPEED_MIN = 0.0f; // absolute lower limit on animation speed
-const F32 DRIFT_COMP_MAX_TOTAL = 0.07f;//0.55f; // maximum drift compensation overall, in any direction 
-const F32 DRIFT_COMP_MAX_SPEED = 4.f; // speed at which drift compensation total maxes out
+const F32 MAX_WALK_PLAYBACK_SPEED = 8.f;		// max m/s for which we adjust walk cycle speed
+
+const F32 MIN_WALK_SPEED = 0.1f;				// minimum speed at which we use velocity for down foot detection
+const F32 TIME_EPSILON = 0.001f;				// minumum frame time
+const F32 MAX_TIME_DELTA = 2.f;					// max two seconds a frame for calculating interpolation
+F32 SPEED_ADJUST_MAX_SEC = 2.f;					// maximum adjustment to walk animation playback speed for a second
+F32 ANIM_SPEED_MAX = 1.5f;						// absolute upper limit on animation speed
+const F32 DRIFT_COMP_MAX_TOTAL = 0.1f;			// maximum drift compensation overall, in any direction 
+const F32 DRIFT_COMP_MAX_SPEED = 4.f;			// speed at which drift compensation total maxes out
 const F32 MAX_ROLL = 0.6f;
+const F32 PELVIS_COMPENSATION_WIEGHT = 0.7f; 	// proportion of foot drift that is compensated by moving the avatar directly
+const F32 SPEED_ADJUST_TIME_CONSTANT = 0.1f; 	// time constant for speed adjustment interpolation
 
 //-----------------------------------------------------------------------------
 // LLKeyframeWalkMotion()
 // Class Constructor
 //-----------------------------------------------------------------------------
 LLKeyframeWalkMotion::LLKeyframeWalkMotion(const LLUUID &id)
-  : LLKeyframeMotion(id),
-    
+:	LLKeyframeMotion(id),
     mCharacter(NULL),
     mCyclePhase(0.0f),
     mRealTimeLast(0.0f),
     mAdjTimeLast(0.0f),
     mDownFoot(0)
-{
-}
+{}
 
 
 //-----------------------------------------------------------------------------
@@ -77,8 +76,7 @@ LLKeyframeWalkMotion::LLKeyframeWalkMotion(const LLUUID &id)
 // Class Destructor
 //-----------------------------------------------------------------------------
 LLKeyframeWalkMotion::~LLKeyframeWalkMotion()
-{
-}
+{}
 
 
 //-----------------------------------------------------------------------------
@@ -149,15 +147,12 @@ BOOL LLKeyframeWalkMotion::onUpdate(F32 time, U8* joint_mask)
 LLWalkAdjustMotion::LLWalkAdjustMotion(const LLUUID &id) :
 	LLMotion(id),
 	mLastTime(0.f),
-	mAvgCorrection(0.f),
-	mSpeedAdjust(0.f),
 	mAnimSpeed(0.f),
-	mAvgSpeed(0.f),
+	mAdjustedSpeed(0.f),
 	mRelativeDir(0.f),
 	mAnkleOffset(0.f)
 {
 	mName = "walk_adjust";
-
 	mPelvisState = new LLJointState;
 }
 
@@ -189,15 +184,16 @@ LLMotion::LLMotionInitStatus LLWalkAdjustMotion::onInitialize(LLCharacter *chara
 //-----------------------------------------------------------------------------
 BOOL LLWalkAdjustMotion::onActivate()
 {
-	mAvgCorrection = 0.f;
-	mSpeedAdjust = 0.f;
 	mAnimSpeed = 0.f;
-	mAvgSpeed = 0.f;
+	mAdjustedSpeed = 0.f;
 	mRelativeDir = 1.f;
 	mPelvisState->setPosition(LLVector3::zero);
 	// store ankle positions for next frame
-	mLastLeftAnklePos = mCharacter->getPosGlobalFromAgent(mLeftAnkleJoint->getWorldPosition());
-	mLastRightAnklePos = mCharacter->getPosGlobalFromAgent(mRightAnkleJoint->getWorldPosition());
+	mLastLeftFootGlobalPos = mCharacter->getPosGlobalFromAgent(mLeftAnkleJoint->getWorldPosition());
+	mLastLeftFootGlobalPos.mdV[VZ] = 0.0;
+
+	mLastRightFootGlobalPos = mCharacter->getPosGlobalFromAgent(mRightAnkleJoint->getWorldPosition());
+	mLastRightFootGlobalPos.mdV[VZ] = 0.0;
 
 	F32 leftAnkleOffset = (mLeftAnkleJoint->getWorldPosition() - mCharacter->getCharacterPosition()).magVec();
 	F32 rightAnkleOffset = (mRightAnkleJoint->getWorldPosition() - mCharacter->getCharacterPosition()).magVec();
@@ -211,164 +207,120 @@ BOOL LLWalkAdjustMotion::onActivate()
 //-----------------------------------------------------------------------------
 BOOL LLWalkAdjustMotion::onUpdate(F32 time, U8* joint_mask)
 {
-	LLVector3 footCorrection;
-	LLVector3 vel = mCharacter->getCharacterVelocity() * mCharacter->getTimeDilation();
-	F32 deltaTime = llclamp(time - mLastTime, 0.f, MAX_TIME_DELTA);
+	// delta_time is guaranteed to be non zero
+	F32 delta_time = llclamp(time - mLastTime, TIME_EPSILON, MAX_TIME_DELTA);
 	mLastTime = time;
 
-	LLQuaternion inv_rotation = ~mPelvisJoint->getWorldRotation();
+	// find the avatar motion vector in the XY plane
+	LLVector3 avatar_velocity = mCharacter->getCharacterVelocity() * mCharacter->getTimeDilation();
+	avatar_velocity.mV[VZ] = 0.f;
 
-	// get speed and normalize velocity vector
-	LLVector3 ang_vel = mCharacter->getCharacterAngularVelocity() * mCharacter->getTimeDilation();
-	F32 speed = llmin(vel.normVec(), MAX_WALK_PLAYBACK_SPEED);
-	mAvgSpeed = lerp(mAvgSpeed, speed, LLCriticalDamp::getInterpolant(0.2f));
+	F32 speed = llclamp(avatar_velocity.magVec(), 0.f, MAX_WALK_PLAYBACK_SPEED);
 
-	// calculate facing vector in pelvis-local space 
-	// (either straight forward or back, depending on velocity)
-	LLVector3 localVel = vel * inv_rotation;
-	if (localVel.mV[VX] > 0.f)
-	{
-		mRelativeDir = 1.f;
-	}
-	else if (localVel.mV[VX] < 0.f)
-	{
-		mRelativeDir = -1.f;
-	}
+	// grab avatar->world transforms
+	LLQuaternion avatar_to_world_rot = mCharacter->getRootJoint()->getWorldRotation();
 
-	// calculate world-space foot drift
-	LLVector3 leftFootDelta;
-	LLVector3 leftFootWorldPosition = mLeftAnkleJoint->getWorldPosition();
-	LLVector3d leftFootGlobalPosition = mCharacter->getPosGlobalFromAgent(leftFootWorldPosition);
-	leftFootDelta.setVec(mLastLeftAnklePos - leftFootGlobalPosition);
-	mLastLeftAnklePos = leftFootGlobalPosition;
+	LLQuaternion world_to_avatar_rot(avatar_to_world_rot);
+	world_to_avatar_rot.conjugate();
 
-	LLVector3 rightFootDelta;
-	LLVector3 rightFootWorldPosition = mRightAnkleJoint->getWorldPosition();
-	LLVector3d rightFootGlobalPosition = mCharacter->getPosGlobalFromAgent(rightFootWorldPosition);
-	rightFootDelta.setVec(mLastRightAnklePos - rightFootGlobalPosition);
-	mLastRightAnklePos = rightFootGlobalPosition;
+	LLVector3 foot_slip_vector;
 
 	// find foot drift along velocity vector
-	if (mAvgSpeed > 0.1)
-	{
-		// walking/running
-		F32 leftFootDriftAmt = leftFootDelta * vel;
-		F32 rightFootDriftAmt = rightFootDelta * vel;
-
-		if (rightFootDriftAmt > leftFootDriftAmt)
-		{
-			footCorrection = rightFootDelta;
-		} else
-		{
-			footCorrection = leftFootDelta;
-		}
-	}
-	else
-	{
-		mAvgSpeed = ang_vel.magVec() * mAnkleOffset;
-		mRelativeDir = 1.f;
-
-		// standing/turning
-		// find the lower foot
-		if (leftFootWorldPosition.mV[VZ] < rightFootWorldPosition.mV[VZ])
-		{
-			// pivot on left foot
-			footCorrection = leftFootDelta;
-		}
+	if (speed > MIN_WALK_SPEED)
+	{	// walking/running
+
+		// calculate world-space foot drift
+		// use global coordinates to seamlessly handle region crossings
+		LLVector3d leftFootGlobalPosition = mCharacter->getPosGlobalFromAgent(mLeftAnkleJoint->getWorldPosition());
+		leftFootGlobalPosition.mdV[VZ] = 0.0;
+		LLVector3 leftFootDelta(leftFootGlobalPosition - mLastLeftFootGlobalPos);
+		mLastLeftFootGlobalPos = leftFootGlobalPosition;
+
+		LLVector3d rightFootGlobalPosition = mCharacter->getPosGlobalFromAgent(mRightAnkleJoint->getWorldPosition());
+		rightFootGlobalPosition.mdV[VZ] = 0.0;
+		LLVector3 rightFootDelta(rightFootGlobalPosition - mLastRightFootGlobalPos);
+		mLastRightFootGlobalPos = rightFootGlobalPosition;
+
+		// get foot drift along avatar direction of motion
+		F32 left_foot_slip_amt = leftFootDelta * avatar_velocity;
+		F32 right_foot_slip_amt = rightFootDelta * avatar_velocity;
+
+		// if right foot is pushing back faster than left foot...
+		if (right_foot_slip_amt < left_foot_slip_amt)
+		{	//...use it to calculate optimal animation speed
+			foot_slip_vector = rightFootDelta;
+		} 
 		else
-		{
-			// pivot on right foot
-			footCorrection = rightFootDelta;
+		{	// otherwise use the left foot
+			foot_slip_vector = leftFootDelta;
 		}
-	}
 
-	// rotate into avatar coordinates
-	footCorrection = footCorrection * inv_rotation;
+		// calculate ideal pelvis offset so that foot is glued to ground and damp towards it
+		// this will soak up transient slippage
+		//
+		// FIXME: this interacts poorly with speed adjustment
+		// mPelvisOffset compensates for foot drift by moving the avatar pelvis in the opposite
+		// direction of the drift, up to a certain limited distance
+		// but this will cause the animation playback rate calculation below to 
+		// kick in too slowly and sometimes start playing the animation in reverse.
 
-	// calculate ideal pelvis offset so that foot is glued to ground and damp towards it
-	// the amount of foot slippage this frame + the offset applied last frame
-	mPelvisOffset = mPelvisState->getPosition() + lerp(LLVector3::zero, footCorrection, LLCriticalDamp::getInterpolant(0.2f));
+		//mPelvisOffset -= PELVIS_COMPENSATION_WIEGHT * (foot_slip_vector * world_to_avatar_rot);//lerp(LLVector3::zero, -1.f * (foot_slip_vector * world_to_avatar_rot), LLCriticalDamp::getInterpolant(0.1f));
 
-	// pelvis drift (along walk direction)
-	mAvgCorrection = lerp(mAvgCorrection, footCorrection.mV[VX] * mRelativeDir, LLCriticalDamp::getInterpolant(0.1f));
+		////F32 drift_comp_max = DRIFT_COMP_MAX_TOTAL * (llclamp(speed, 0.f, DRIFT_COMP_MAX_SPEED) / DRIFT_COMP_MAX_SPEED);
+		//F32 drift_comp_max = DRIFT_COMP_MAX_TOTAL;
 
-	// calculate average velocity of foot slippage
-	F32 footSlipVelocity = (deltaTime != 0.f) ? (-mAvgCorrection / deltaTime) : 0.f;
+		//// clamp pelvis offset to a 90 degree arc behind the nominal position
+		//// NB: this is an ADDITIVE amount that is accumulated every frame, so clamping it alone won't do the trick
+		//// must clamp with absolute position of pelvis in mind
+		//LLVector3 currentPelvisPos = mPelvisState->getJoint()->getPosition();
+		//mPelvisOffset.mV[VX] = llclamp( mPelvisOffset.mV[VX], -drift_comp_max, drift_comp_max );
+		//mPelvisOffset.mV[VY] = llclamp( mPelvisOffset.mV[VY], -drift_comp_max, drift_comp_max );
+		//mPelvisOffset.mV[VZ] = 0.f;
+		//
+		//mLastRightFootGlobalPos += LLVector3d(mPelvisOffset * avatar_to_world_rot);
+		//mLastLeftFootGlobalPos += LLVector3d(mPelvisOffset * avatar_to_world_rot);
 
-	F32 newSpeedAdjust = 0.f;
-	
-	// modulate speed by dot products of facing and velocity
-	// so that if we are moving sideways, we slow down the animation
-	// and if we're moving backward, we walk backward
+		//foot_slip_vector -= mPelvisOffset;
 
-	F32 directional_factor = localVel.mV[VX] * mRelativeDir;
+		LLVector3 avatar_movement_dir = avatar_velocity;
+		avatar_movement_dir.normalize();
 
-	if (speed > 0.1f)
-	{
-		// calculate ratio of desired foot velocity to detected foot velocity
-		newSpeedAdjust = llclamp(footSlipVelocity - mAvgSpeed * (1.f - directional_factor), 
-								-SPEED_ADJUST_MAX, SPEED_ADJUST_MAX);
-		newSpeedAdjust = lerp(mSpeedAdjust, newSpeedAdjust, LLCriticalDamp::getInterpolant(0.2f));
+		// planted foot speed is avatar velocity - foot slip amount along avatar movement direction
+		F32 foot_speed = speed - ((foot_slip_vector * avatar_movement_dir) / delta_time);
 
-		F32 speedDelta = newSpeedAdjust - mSpeedAdjust;
-		speedDelta = llclamp(speedDelta, -SPEED_ADJUST_MAX_SEC * deltaTime, SPEED_ADJUST_MAX_SEC * deltaTime);
+		// multiply animation playback rate so that foot speed matches avatar speed
+		F32 desired_speed_multiplier = llclamp(speed / foot_speed, 0.f, ANIM_SPEED_MAX);
 
-		mSpeedAdjust = mSpeedAdjust + speedDelta;
-	}
-	else
-	{
-		mSpeedAdjust = lerp(mSpeedAdjust, 0.f, LLCriticalDamp::getInterpolant(0.2f));
-	}
+		// blend towards new speed adjustment value
+		F32 new_speed_adjust = lerp(mAdjustedSpeed, desired_speed_multiplier, LLCriticalDamp::getInterpolant(SPEED_ADJUST_TIME_CONSTANT));
 
-	mAnimSpeed = (mAvgSpeed + mSpeedAdjust) * mRelativeDir;
-	if (mAnimSpeed>0)
-	{
-		mAnimSpeed = llclamp(mAnimSpeed, ANIM_SPEED_MIN, ANIM_SPEED_MAX);
+		// limit that rate at which the speed adjustment changes
+		F32 speedDelta = llclamp(new_speed_adjust - mAdjustedSpeed, -SPEED_ADJUST_MAX_SEC * delta_time, SPEED_ADJUST_MAX_SEC * delta_time);
+		mAdjustedSpeed += speedDelta;
+
+		// modulate speed by dot products of facing and velocity
+		// so that if we are moving sideways, we slow down the animation
+		// and if we're moving backward, we walk backward
+		// do this at the end to be more responsive to direction changes instead of in the above speed calculations
+		F32 directional_factor = (avatar_movement_dir * world_to_avatar_rot).mV[VX];
+
+		mAnimSpeed = mAdjustedSpeed * directional_factor;
 	}
 	else
-	{
-		mAnimSpeed = llclamp(mAnimSpeed, -ANIM_SPEED_MAX, -ANIM_SPEED_MIN);
-	}
-// 	char debug_text[64];
-// 	sprintf(debug_text, "Foot slip vel: %.2f", footSlipVelocity);
-// 	mCharacter->addDebugText(debug_text);
-// 	sprintf(debug_text, "Speed: %.2f", mAvgSpeed);
-// 	mCharacter->addDebugText(debug_text);
-// 	sprintf(debug_text, "Speed Adjust: %.2f", mSpeedAdjust);
-// 	mCharacter->addDebugText(debug_text);
-// 	sprintf(debug_text, "Animation Playback Speed: %.2f", mAnimSpeed);
-// 	mCharacter->addDebugText(debug_text);
- 	mCharacter->setAnimationData("Walk Speed", &mAnimSpeed);
-	if (mCharacter->getMotionController().mIsSelf)
-	{
-//		F32 elapsed = mCharacter->getMotionController().getFrameTimer().getElapsedTimeF32();
-// 		llinfos << "PLOT elapsed: " << elapsed 
-// 				<< " footSlipVelocity: " << footSlipVelocity
-// 				<< " mAvgCorrection: " << mAvgCorrection
-// 				<< " mAvgSpeed: " << mAvgSpeed
-// 				<< " mAnimSpeed: " << mAnimSpeed
-// 				<< " ANIM_SPEED_MAX: " << ANIM_SPEED_MAX
-// 				<< " ANIM_SPEED_MIN: " << ANIM_SPEED_MIN
-// 				<< llendl;
-	}
-
-	// clamp pelvis offset to a 90 degree arc behind the nominal position
-	F32 drift_comp_max = llclamp(speed, 0.f, DRIFT_COMP_MAX_SPEED) / DRIFT_COMP_MAX_SPEED;
-	drift_comp_max *= DRIFT_COMP_MAX_TOTAL;
+	{	// standing/turning
 
-	LLVector3 currentPelvisPos = mPelvisState->getJoint()->getPosition();
+		// damp out speed adjustment to 0
+		mAnimSpeed = lerp(mAnimSpeed, 1.f, LLCriticalDamp::getInterpolant(0.2f));
+		//mPelvisOffset = lerp(mPelvisOffset, LLVector3::zero, LLCriticalDamp::getInterpolant(0.2f));
+	}
 
-	// NB: this is an ADDITIVE amount that is accumulated every frame, so clamping it alone won't do the trick
-	// must clamp with absolute position of pelvis in mind
-	mPelvisOffset.mV[VX] = llclamp( mPelvisOffset.mV[VX], -drift_comp_max - currentPelvisPos.mV[VX], drift_comp_max - currentPelvisPos.mV[VX]  );
-	mPelvisOffset.mV[VY] = llclamp( mPelvisOffset.mV[VY], -drift_comp_max - currentPelvisPos.mV[VY], drift_comp_max - currentPelvisPos.mV[VY]);
-	mPelvisOffset.mV[VZ] = 0.f;
+	// broadcast walk speed change
+ 	mCharacter->setAnimationData("Walk Speed", &mAnimSpeed);
 
 	// set position
+	// need to update *some* joint to keep this animation active
 	mPelvisState->setPosition(mPelvisOffset);
 
-	mCharacter->setAnimationData("Pelvis Offset", &mPelvisOffset);
-
 	return TRUE;
 }
 
@@ -438,14 +390,8 @@ BOOL LLFlyAdjustMotion::onUpdate(F32 time, U8* joint_mask)
 	// roll is critically damped interpolation between current roll and angular velocity-derived target roll
 	mRoll = lerp(mRoll, target_roll, LLCriticalDamp::getInterpolant(0.1f));
 
-//	llinfos << mRoll << llendl;
-
 	LLQuaternion roll(mRoll, LLVector3(0.f, 0.f, 1.f));
 	mPelvisState->setRotation(roll);
 
-//	F32 lerp_amt = LLCriticalDamp::getInterpolant(0.2f);
-//	
-//	LLVector3 pelvis_correction = mPelvisState->getPosition() - lerp(LLVector3::zero, mPelvisState->getJoint()->getPosition() + mPelvisState->getPosition(), lerp_amt);
-//	mPelvisState->setPosition(pelvis_correction);
 	return TRUE;
 }
diff --git a/indra/llcharacter/llkeyframewalkmotion.h b/indra/llcharacter/llkeyframewalkmotion.h
index 90dd4dbcac..b507e9423a 100644
--- a/indra/llcharacter/llkeyframewalkmotion.h
+++ b/indra/llcharacter/llkeyframewalkmotion.h
@@ -126,13 +126,11 @@ public:
 	LLJoint*		mRightAnkleJoint;
 	LLPointer<LLJointState>	mPelvisState;
 	LLJoint*		mPelvisJoint;
-	LLVector3d		mLastLeftAnklePos;
-	LLVector3d		mLastRightAnklePos;
+	LLVector3d		mLastLeftFootGlobalPos;
+	LLVector3d		mLastRightFootGlobalPos;
 	F32				mLastTime;
-	F32				mAvgCorrection;
-	F32				mSpeedAdjust;
+	F32				mAdjustedSpeed;
 	F32				mAnimSpeed;
-	F32				mAvgSpeed;
 	F32				mRelativeDir;
 	LLVector3		mPelvisOffset;
 	F32				mAnkleOffset;