PageRenderTime 77ms CodeModel.GetById 21ms app.highlight 51ms RepoModel.GetById 2ms app.codeStats 0ms

/indra/llcharacter/llkeyframewalkmotion.cpp

https://bitbucket.org/lindenlab/viewer-beta/
C++ | 392 lines | 192 code | 70 blank | 130 comment | 5 complexity | 8c05f29a14f77b68a16afed9b5e3c0a3 MD5 | raw file
  1/** 
  2 * @file llkeyframewalkmotion.cpp
  3 * @brief Implementation of LLKeyframeWalkMotion class.
  4 *
  5 * $LicenseInfo:firstyear=2001&license=viewerlgpl$
  6 * Second Life Viewer Source Code
  7 * Copyright (C) 2010, Linden Research, Inc.
  8 * 
  9 * This library is free software; you can redistribute it and/or
 10 * modify it under the terms of the GNU Lesser General Public
 11 * License as published by the Free Software Foundation;
 12 * version 2.1 of the License only.
 13 * 
 14 * This library is distributed in the hope that it will be useful,
 15 * but WITHOUT ANY WARRANTY; without even the implied warranty of
 16 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
 17 * Lesser General Public License for more details.
 18 * 
 19 * You should have received a copy of the GNU Lesser General Public
 20 * License along with this library; if not, write to the Free Software
 21 * Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301  USA
 22 * 
 23 * Linden Research, Inc., 945 Battery Street, San Francisco, CA  94111  USA
 24 * $/LicenseInfo$
 25 */
 26
 27//-----------------------------------------------------------------------------
 28// Header Files
 29//-----------------------------------------------------------------------------
 30#include "linden_common.h"
 31
 32#include "llkeyframewalkmotion.h"
 33#include "llcharacter.h"
 34#include "llmath.h"
 35#include "m3math.h"
 36#include "llcriticaldamp.h"
 37
 38//-----------------------------------------------------------------------------
 39// Macros
 40//-----------------------------------------------------------------------------
 41const F32 MAX_WALK_PLAYBACK_SPEED = 8.f;		// max m/s for which we adjust walk cycle speed
 42
 43const F32 MIN_WALK_SPEED = 0.1f;				// minimum speed at which we use velocity for down foot detection
 44const F32 TIME_EPSILON = 0.001f;				// minumum frame time
 45const F32 MAX_TIME_DELTA = 2.f;					// max two seconds a frame for calculating interpolation
 46F32 SPEED_ADJUST_MAX_SEC = 2.f;					// maximum adjustment to walk animation playback speed for a second
 47F32 ANIM_SPEED_MAX = 1.5f;						// absolute upper limit on animation speed
 48const F32 DRIFT_COMP_MAX_TOTAL = 0.1f;			// maximum drift compensation overall, in any direction 
 49const F32 DRIFT_COMP_MAX_SPEED = 4.f;			// speed at which drift compensation total maxes out
 50const F32 MAX_ROLL = 0.6f;
 51const F32 PELVIS_COMPENSATION_WIEGHT = 0.7f; 	// proportion of foot drift that is compensated by moving the avatar directly
 52const F32 SPEED_ADJUST_TIME_CONSTANT = 0.1f; 	// time constant for speed adjustment interpolation
 53
 54//-----------------------------------------------------------------------------
 55// LLKeyframeWalkMotion()
 56// Class Constructor
 57//-----------------------------------------------------------------------------
 58LLKeyframeWalkMotion::LLKeyframeWalkMotion(const LLUUID &id)
 59:	LLKeyframeMotion(id),
 60    mCharacter(NULL),
 61    mCyclePhase(0.0f),
 62    mRealTimeLast(0.0f),
 63    mAdjTimeLast(0.0f),
 64    mDownFoot(0)
 65{}
 66
 67
 68//-----------------------------------------------------------------------------
 69// ~LLKeyframeWalkMotion()
 70// Class Destructor
 71//-----------------------------------------------------------------------------
 72LLKeyframeWalkMotion::~LLKeyframeWalkMotion()
 73{}
 74
 75
 76//-----------------------------------------------------------------------------
 77// LLKeyframeWalkMotion::onInitialize()
 78//-----------------------------------------------------------------------------
 79LLMotion::LLMotionInitStatus LLKeyframeWalkMotion::onInitialize(LLCharacter *character)
 80{
 81	mCharacter = character;
 82
 83	return LLKeyframeMotion::onInitialize(character);
 84}
 85
 86//-----------------------------------------------------------------------------
 87// LLKeyframeWalkMotion::onActivate()
 88//-----------------------------------------------------------------------------
 89BOOL LLKeyframeWalkMotion::onActivate()
 90{
 91	mRealTimeLast = 0.0f;
 92	mAdjTimeLast = 0.0f;
 93
 94	return LLKeyframeMotion::onActivate();
 95}
 96
 97//-----------------------------------------------------------------------------
 98// LLKeyframeWalkMotion::onDeactivate()
 99//-----------------------------------------------------------------------------
100void LLKeyframeWalkMotion::onDeactivate()
101{
102	mCharacter->removeAnimationData("Down Foot");		
103	LLKeyframeMotion::onDeactivate();
104}
105
106//-----------------------------------------------------------------------------
107// LLKeyframeWalkMotion::onUpdate()
108//-----------------------------------------------------------------------------
109BOOL LLKeyframeWalkMotion::onUpdate(F32 time, U8* joint_mask)
110{
111	// compute time since last update
112	F32 deltaTime = time - mRealTimeLast;
113
114	void* speed_ptr = mCharacter->getAnimationData("Walk Speed");
115	F32 speed = (speed_ptr) ? *((F32 *)speed_ptr) : 1.f;
116
117	// adjust the passage of time accordingly
118	F32 adjusted_time = mAdjTimeLast + (deltaTime * speed);
119
120	// save time for next update
121	mRealTimeLast = time;
122	mAdjTimeLast = adjusted_time;
123
124	// handle wrap around
125	if (adjusted_time < 0.0f)
126	{
127		adjusted_time = getDuration() + fmod(adjusted_time, getDuration());
128	}
129
130	// let the base class update the cycle
131	return LLKeyframeMotion::onUpdate( adjusted_time, joint_mask );
132}
133
134// End
135
136
137//-----------------------------------------------------------------------------
138// LLWalkAdjustMotion()
139// Class Constructor
140//-----------------------------------------------------------------------------
141LLWalkAdjustMotion::LLWalkAdjustMotion(const LLUUID &id) :
142	LLMotion(id),
143	mLastTime(0.f),
144	mAnimSpeed(0.f),
145	mAdjustedSpeed(0.f),
146	mRelativeDir(0.f),
147	mAnkleOffset(0.f)
148{
149	mName = "walk_adjust";
150	mPelvisState = new LLJointState;
151}
152
153//-----------------------------------------------------------------------------
154// LLWalkAdjustMotion::onInitialize()
155//-----------------------------------------------------------------------------
156LLMotion::LLMotionInitStatus LLWalkAdjustMotion::onInitialize(LLCharacter *character)
157{
158	mCharacter = character;
159	mLeftAnkleJoint = mCharacter->getJoint("mAnkleLeft");
160	mRightAnkleJoint = mCharacter->getJoint("mAnkleRight");
161
162	mPelvisJoint = mCharacter->getJoint("mPelvis");
163	mPelvisState->setJoint( mPelvisJoint );
164	if ( !mPelvisJoint )
165	{
166		llwarns << getName() << ": Can't get pelvis joint." << llendl;
167		return STATUS_FAILURE;
168	}
169
170	mPelvisState->setUsage(LLJointState::POS);
171	addJointState( mPelvisState );
172
173	return STATUS_SUCCESS;
174}
175
176//-----------------------------------------------------------------------------
177// LLWalkAdjustMotion::onActivate()
178//-----------------------------------------------------------------------------
179BOOL LLWalkAdjustMotion::onActivate()
180{
181	mAnimSpeed = 0.f;
182	mAdjustedSpeed = 0.f;
183	mRelativeDir = 1.f;
184	mPelvisState->setPosition(LLVector3::zero);
185	// store ankle positions for next frame
186	mLastLeftFootGlobalPos = mCharacter->getPosGlobalFromAgent(mLeftAnkleJoint->getWorldPosition());
187	mLastLeftFootGlobalPos.mdV[VZ] = 0.0;
188
189	mLastRightFootGlobalPos = mCharacter->getPosGlobalFromAgent(mRightAnkleJoint->getWorldPosition());
190	mLastRightFootGlobalPos.mdV[VZ] = 0.0;
191
192	F32 leftAnkleOffset = (mLeftAnkleJoint->getWorldPosition() - mCharacter->getCharacterPosition()).magVec();
193	F32 rightAnkleOffset = (mRightAnkleJoint->getWorldPosition() - mCharacter->getCharacterPosition()).magVec();
194	mAnkleOffset = llmax(leftAnkleOffset, rightAnkleOffset);
195
196	return TRUE;
197}
198
199//-----------------------------------------------------------------------------
200// LLWalkAdjustMotion::onUpdate()
201//-----------------------------------------------------------------------------
202BOOL LLWalkAdjustMotion::onUpdate(F32 time, U8* joint_mask)
203{
204	// delta_time is guaranteed to be non zero
205	F32 delta_time = llclamp(time - mLastTime, TIME_EPSILON, MAX_TIME_DELTA);
206	mLastTime = time;
207
208	// find the avatar motion vector in the XY plane
209	LLVector3 avatar_velocity = mCharacter->getCharacterVelocity() * mCharacter->getTimeDilation();
210	avatar_velocity.mV[VZ] = 0.f;
211
212	F32 speed = llclamp(avatar_velocity.magVec(), 0.f, MAX_WALK_PLAYBACK_SPEED);
213
214	// grab avatar->world transforms
215	LLQuaternion avatar_to_world_rot = mCharacter->getRootJoint()->getWorldRotation();
216
217	LLQuaternion world_to_avatar_rot(avatar_to_world_rot);
218	world_to_avatar_rot.conjugate();
219
220	LLVector3 foot_slip_vector;
221
222	// find foot drift along velocity vector
223	if (speed > MIN_WALK_SPEED)
224	{	// walking/running
225
226		// calculate world-space foot drift
227		// use global coordinates to seamlessly handle region crossings
228		LLVector3d leftFootGlobalPosition = mCharacter->getPosGlobalFromAgent(mLeftAnkleJoint->getWorldPosition());
229		leftFootGlobalPosition.mdV[VZ] = 0.0;
230		LLVector3 leftFootDelta(leftFootGlobalPosition - mLastLeftFootGlobalPos);
231		mLastLeftFootGlobalPos = leftFootGlobalPosition;
232
233		LLVector3d rightFootGlobalPosition = mCharacter->getPosGlobalFromAgent(mRightAnkleJoint->getWorldPosition());
234		rightFootGlobalPosition.mdV[VZ] = 0.0;
235		LLVector3 rightFootDelta(rightFootGlobalPosition - mLastRightFootGlobalPos);
236		mLastRightFootGlobalPos = rightFootGlobalPosition;
237
238		// get foot drift along avatar direction of motion
239		F32 left_foot_slip_amt = leftFootDelta * avatar_velocity;
240		F32 right_foot_slip_amt = rightFootDelta * avatar_velocity;
241
242		// if right foot is pushing back faster than left foot...
243		if (right_foot_slip_amt < left_foot_slip_amt)
244		{	//...use it to calculate optimal animation speed
245			foot_slip_vector = rightFootDelta;
246		} 
247		else
248		{	// otherwise use the left foot
249			foot_slip_vector = leftFootDelta;
250		}
251
252		// calculate ideal pelvis offset so that foot is glued to ground and damp towards it
253		// this will soak up transient slippage
254		//
255		// FIXME: this interacts poorly with speed adjustment
256		// mPelvisOffset compensates for foot drift by moving the avatar pelvis in the opposite
257		// direction of the drift, up to a certain limited distance
258		// but this will cause the animation playback rate calculation below to 
259		// kick in too slowly and sometimes start playing the animation in reverse.
260
261		//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));
262
263		////F32 drift_comp_max = DRIFT_COMP_MAX_TOTAL * (llclamp(speed, 0.f, DRIFT_COMP_MAX_SPEED) / DRIFT_COMP_MAX_SPEED);
264		//F32 drift_comp_max = DRIFT_COMP_MAX_TOTAL;
265
266		//// clamp pelvis offset to a 90 degree arc behind the nominal position
267		//// NB: this is an ADDITIVE amount that is accumulated every frame, so clamping it alone won't do the trick
268		//// must clamp with absolute position of pelvis in mind
269		//LLVector3 currentPelvisPos = mPelvisState->getJoint()->getPosition();
270		//mPelvisOffset.mV[VX] = llclamp( mPelvisOffset.mV[VX], -drift_comp_max, drift_comp_max );
271		//mPelvisOffset.mV[VY] = llclamp( mPelvisOffset.mV[VY], -drift_comp_max, drift_comp_max );
272		//mPelvisOffset.mV[VZ] = 0.f;
273		//
274		//mLastRightFootGlobalPos += LLVector3d(mPelvisOffset * avatar_to_world_rot);
275		//mLastLeftFootGlobalPos += LLVector3d(mPelvisOffset * avatar_to_world_rot);
276
277		//foot_slip_vector -= mPelvisOffset;
278
279		LLVector3 avatar_movement_dir = avatar_velocity;
280		avatar_movement_dir.normalize();
281
282		// planted foot speed is avatar velocity - foot slip amount along avatar movement direction
283		F32 foot_speed = speed - ((foot_slip_vector * avatar_movement_dir) / delta_time);
284
285		// multiply animation playback rate so that foot speed matches avatar speed
286		F32 min_speed_multiplier = clamp_rescale(speed, 0.f, 1.f, 0.f, 0.1f);
287		F32 desired_speed_multiplier = llclamp(speed / foot_speed, min_speed_multiplier, ANIM_SPEED_MAX);
288
289		// blend towards new speed adjustment value
290		F32 new_speed_adjust = lerp(mAdjustedSpeed, desired_speed_multiplier, LLCriticalDamp::getInterpolant(SPEED_ADJUST_TIME_CONSTANT));
291
292		// limit that rate at which the speed adjustment changes
293		F32 speedDelta = llclamp(new_speed_adjust - mAdjustedSpeed, -SPEED_ADJUST_MAX_SEC * delta_time, SPEED_ADJUST_MAX_SEC * delta_time);
294		mAdjustedSpeed += speedDelta;
295
296		// modulate speed by dot products of facing and velocity
297		// so that if we are moving sideways, we slow down the animation
298		// and if we're moving backward, we walk backward
299		// do this at the end to be more responsive to direction changes instead of in the above speed calculations
300		F32 directional_factor = (avatar_movement_dir * world_to_avatar_rot).mV[VX];
301
302		mAnimSpeed = mAdjustedSpeed * directional_factor;
303	}
304	else
305	{	// standing/turning
306
307		// damp out speed adjustment to 0
308		mAnimSpeed = lerp(mAnimSpeed, 1.f, LLCriticalDamp::getInterpolant(0.2f));
309		//mPelvisOffset = lerp(mPelvisOffset, LLVector3::zero, LLCriticalDamp::getInterpolant(0.2f));
310	}
311
312	// broadcast walk speed change
313 	mCharacter->setAnimationData("Walk Speed", &mAnimSpeed);
314
315	// set position
316	// need to update *some* joint to keep this animation active
317	mPelvisState->setPosition(mPelvisOffset);
318
319	return TRUE;
320}
321
322//-----------------------------------------------------------------------------
323// LLWalkAdjustMotion::onDeactivate()
324//-----------------------------------------------------------------------------
325void LLWalkAdjustMotion::onDeactivate()
326{
327	mCharacter->removeAnimationData("Walk Speed");
328}
329
330//-----------------------------------------------------------------------------
331// LLFlyAdjustMotion::LLFlyAdjustMotion()
332//-----------------------------------------------------------------------------
333LLFlyAdjustMotion::LLFlyAdjustMotion(const LLUUID &id)
334	: LLMotion(id),
335	  mRoll(0.f)
336{
337	mName = "fly_adjust";
338
339	mPelvisState = new LLJointState;
340}
341
342//-----------------------------------------------------------------------------
343// LLFlyAdjustMotion::onInitialize()
344//-----------------------------------------------------------------------------
345LLMotion::LLMotionInitStatus LLFlyAdjustMotion::onInitialize(LLCharacter *character)
346{
347	mCharacter = character;
348
349	LLJoint* pelvisJoint = mCharacter->getJoint("mPelvis");
350	mPelvisState->setJoint( pelvisJoint );
351	if ( !pelvisJoint )
352	{
353		llwarns << getName() << ": Can't get pelvis joint." << llendl;
354		return STATUS_FAILURE;
355	}
356
357	mPelvisState->setUsage(LLJointState::POS | LLJointState::ROT);
358	addJointState( mPelvisState );
359
360	return STATUS_SUCCESS;
361}
362
363//-----------------------------------------------------------------------------
364// LLFlyAdjustMotion::onActivate()
365//-----------------------------------------------------------------------------
366BOOL LLFlyAdjustMotion::onActivate()
367{
368	mPelvisState->setPosition(LLVector3::zero);
369	mPelvisState->setRotation(LLQuaternion::DEFAULT);
370	mRoll = 0.f;
371	return TRUE;
372}
373
374//-----------------------------------------------------------------------------
375// LLFlyAdjustMotion::onUpdate()
376//-----------------------------------------------------------------------------
377BOOL LLFlyAdjustMotion::onUpdate(F32 time, U8* joint_mask)
378{
379	LLVector3 ang_vel = mCharacter->getCharacterAngularVelocity() * mCharacter->getTimeDilation();
380	F32 speed = mCharacter->getCharacterVelocity().magVec();
381
382	F32 roll_factor = clamp_rescale(speed, 7.f, 15.f, 0.f, -MAX_ROLL);
383	F32 target_roll = llclamp(ang_vel.mV[VZ], -4.f, 4.f) * roll_factor;
384
385	// roll is critically damped interpolation between current roll and angular velocity-derived target roll
386	mRoll = lerp(mRoll, target_roll, LLCriticalDamp::getInterpolant(0.1f));
387
388	LLQuaternion roll(mRoll, LLVector3(0.f, 0.f, 1.f));
389	mPelvisState->setRotation(roll);
390
391	return TRUE;
392}