PageRenderTime 38ms CodeModel.GetById 2ms app.highlight 31ms RepoModel.GetById 1ms app.codeStats 1ms

/indra/llcharacter/llkeyframestandmotion.cpp

https://bitbucket.org/lindenlab/viewer-beta/
C++ | 338 lines | 197 code | 53 blank | 88 comment | 18 complexity | 078795424b52a9f38aa40f0377eb1304 MD5 | raw file
  1/** 
  2 * @file llkeyframestandmotion.cpp
  3 * @brief Implementation of LLKeyframeStandMotion 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 "llkeyframestandmotion.h"
 33#include "llcharacter.h"
 34
 35//-----------------------------------------------------------------------------
 36// Macros and consts
 37//-----------------------------------------------------------------------------
 38#define GO_TO_KEY_POSE	1
 39#define MIN_TRACK_SPEED 0.01f
 40const F32 ROTATION_THRESHOLD = 0.6f;
 41const F32 POSITION_THRESHOLD = 0.1f;
 42
 43//-----------------------------------------------------------------------------
 44// LLKeyframeStandMotion()
 45// Class Constructor
 46//-----------------------------------------------------------------------------
 47LLKeyframeStandMotion::LLKeyframeStandMotion(const LLUUID &id) : LLKeyframeMotion(id)
 48{
 49	mFlipFeet = FALSE;
 50	mCharacter = NULL;
 51
 52	// create kinematic hierarchy
 53	mPelvisJoint.addChild( &mHipLeftJoint );
 54		mHipLeftJoint.addChild( &mKneeLeftJoint );
 55			mKneeLeftJoint.addChild( &mAnkleLeftJoint );
 56	mPelvisJoint.addChild( &mHipRightJoint );
 57		mHipRightJoint.addChild( &mKneeRightJoint );
 58			mKneeRightJoint.addChild( &mAnkleRightJoint );
 59
 60	mPelvisState = NULL;
 61
 62	mHipLeftState =  NULL;
 63	mKneeLeftState =  NULL;
 64	mAnkleLeftState =  NULL;
 65
 66	mHipRightState =  NULL;
 67	mKneeRightState =  NULL;
 68	mAnkleRightState =  NULL;
 69
 70	mTrackAnkles = TRUE;
 71
 72	mFrameNum = 0;
 73}
 74
 75
 76//-----------------------------------------------------------------------------
 77// ~LLKeyframeStandMotion()
 78// Class Destructor
 79//-----------------------------------------------------------------------------
 80LLKeyframeStandMotion::~LLKeyframeStandMotion()
 81{
 82}
 83
 84
 85//-----------------------------------------------------------------------------
 86// LLKeyframeStandMotion::onInitialize()
 87//-----------------------------------------------------------------------------
 88LLMotion::LLMotionInitStatus LLKeyframeStandMotion::onInitialize(LLCharacter *character)
 89{
 90	// save character pointer for later use
 91	mCharacter = character;
 92
 93	mFlipFeet = FALSE;
 94
 95	// load keyframe data, setup pose and joint states
 96	LLMotion::LLMotionInitStatus status = LLKeyframeMotion::onInitialize(character);
 97	if ( status == STATUS_FAILURE )
 98	{
 99		return status;
100	}
101
102	// find the necessary joint states
103	LLPose *pose = getPose();
104	mPelvisState = pose->findJointState("mPelvis");
105	
106	mHipLeftState = pose->findJointState("mHipLeft");
107	mKneeLeftState = pose->findJointState("mKneeLeft");
108	mAnkleLeftState = pose->findJointState("mAnkleLeft");
109
110	mHipRightState = pose->findJointState("mHipRight");
111	mKneeRightState = pose->findJointState("mKneeRight");
112	mAnkleRightState = pose->findJointState("mAnkleRight");
113
114	if (	!mPelvisState ||
115			!mHipLeftState ||
116			!mKneeLeftState ||
117			!mAnkleLeftState ||
118			!mHipRightState ||
119			!mKneeRightState ||
120			!mAnkleRightState )
121	{
122		llinfos << getName() << ": Can't find necessary joint states" << llendl;
123		return STATUS_FAILURE;
124	}
125
126	return STATUS_SUCCESS;
127}
128
129//-----------------------------------------------------------------------------
130// LLKeyframeStandMotion::onActivate()
131//-----------------------------------------------------------------------------
132BOOL LLKeyframeStandMotion::onActivate()
133{
134	//-------------------------------------------------------------------------
135	// setup the IK solvers
136	//-------------------------------------------------------------------------
137	mIKLeft.setPoleVector( LLVector3(1.0f, 0.0f, 0.0f));
138	mIKRight.setPoleVector( LLVector3(1.0f, 0.0f, 0.0f));
139	mIKLeft.setBAxis( LLVector3(0.05f, 1.0f, 0.0f));
140	mIKRight.setBAxis( LLVector3(-0.05f, 1.0f, 0.0f));
141
142	mLastGoodPelvisRotation.loadIdentity();
143	mLastGoodPosition.clearVec();
144
145	mFrameNum = 0;
146
147	return LLKeyframeMotion::onActivate();
148}
149
150//-----------------------------------------------------------------------------
151// LLKeyframeStandMotion::onDeactivate()
152//-----------------------------------------------------------------------------
153void LLKeyframeStandMotion::onDeactivate()
154{
155	LLKeyframeMotion::onDeactivate();
156}
157
158//-----------------------------------------------------------------------------
159// LLKeyframeStandMotion::onUpdate()
160//-----------------------------------------------------------------------------
161BOOL LLKeyframeStandMotion::onUpdate(F32 time, U8* joint_mask)
162{
163	//-------------------------------------------------------------------------
164	// let the base class update the cycle
165	//-------------------------------------------------------------------------
166	BOOL status = LLKeyframeMotion::onUpdate(time, joint_mask);
167	if (!status)
168	{
169		return FALSE;
170	}
171
172	LLVector3 root_world_pos = mPelvisState->getJoint()->getParent()->getWorldPosition();
173
174	// have we received a valid world position for this avatar?
175	if (root_world_pos.isExactlyZero())
176	{
177		return TRUE;
178	}
179
180	//-------------------------------------------------------------------------
181	// Stop tracking (start locking) ankles once ease in is done.
182	// Setting this here ensures we track until we get valid foot position.
183	//-------------------------------------------------------------------------
184	if (dot(mPelvisState->getJoint()->getWorldRotation(), mLastGoodPelvisRotation) < ROTATION_THRESHOLD)
185	{
186		mLastGoodPelvisRotation = mPelvisState->getJoint()->getWorldRotation();
187		mLastGoodPelvisRotation.normalize();
188		mTrackAnkles = TRUE;
189	}
190	else if ((mCharacter->getCharacterPosition() - mLastGoodPosition).magVecSquared() > POSITION_THRESHOLD)
191	{
192		mLastGoodPosition = mCharacter->getCharacterPosition();
193		mTrackAnkles = TRUE;
194	}
195	else if (mPose.getWeight() < 1.f)
196	{
197		mTrackAnkles = TRUE;
198	}
199
200
201	//-------------------------------------------------------------------------
202	// propagate joint positions to internal versions
203	//-------------------------------------------------------------------------
204	mPelvisJoint.setPosition(
205			root_world_pos +
206			mPelvisState->getPosition() );
207
208	mHipLeftJoint.setPosition( mHipLeftState->getJoint()->getPosition() );
209	mKneeLeftJoint.setPosition( mKneeLeftState->getJoint()->getPosition() );
210	mAnkleLeftJoint.setPosition( mAnkleLeftState->getJoint()->getPosition() );
211
212	mHipLeftJoint.setScale( mHipLeftState->getJoint()->getScale() );
213	mKneeLeftJoint.setScale( mKneeLeftState->getJoint()->getScale() );
214	mAnkleLeftJoint.setScale( mAnkleLeftState->getJoint()->getScale() );
215
216	mHipRightJoint.setPosition( mHipRightState->getJoint()->getPosition() );
217	mKneeRightJoint.setPosition( mKneeRightState->getJoint()->getPosition() );
218	mAnkleRightJoint.setPosition( mAnkleRightState->getJoint()->getPosition() );
219
220	mHipRightJoint.setScale( mHipRightState->getJoint()->getScale() );
221	mKneeRightJoint.setScale( mKneeRightState->getJoint()->getScale() );
222	mAnkleRightJoint.setScale( mAnkleRightState->getJoint()->getScale() );
223	//-------------------------------------------------------------------------
224	// propagate joint rotations to internal versions
225	//-------------------------------------------------------------------------
226	mPelvisJoint.setRotation( mPelvisState->getJoint()->getWorldRotation() );
227
228#if GO_TO_KEY_POSE
229	mHipLeftJoint.setRotation( mHipLeftState->getRotation() );
230	mKneeLeftJoint.setRotation( mKneeLeftState->getRotation() );
231	mAnkleLeftJoint.setRotation( mAnkleLeftState->getRotation() );
232
233	mHipRightJoint.setRotation( mHipRightState->getRotation() );
234	mKneeRightJoint.setRotation( mKneeRightState->getRotation() );
235	mAnkleRightJoint.setRotation( mAnkleRightState->getRotation() );
236#else
237	mHipLeftJoint.setRotation( mHipLeftState->getJoint()->getRotation() );
238	mKneeLeftJoint.setRotation( mKneeLeftState->getJoint()->getRotation() );
239	mAnkleLeftJoint.setRotation( mAnkleLeftState->getJoint()->getRotation() );
240
241	mHipRightJoint.setRotation( mHipRightState->getJoint()->getRotation() );
242	mKneeRightJoint.setRotation( mKneeRightState->getJoint()->getRotation() );
243	mAnkleRightJoint.setRotation( mAnkleRightState->getJoint()->getRotation() );
244#endif
245
246	// need to wait for underlying keyframe motion to affect the skeleton
247	if (mFrameNum == 2)
248	{
249		mIKLeft.setupJoints( &mHipLeftJoint, &mKneeLeftJoint, &mAnkleLeftJoint, &mTargetLeft );
250		mIKRight.setupJoints( &mHipRightJoint, &mKneeRightJoint, &mAnkleRightJoint, &mTargetRight );
251	}
252	else if (mFrameNum < 2)
253	{
254		mFrameNum++;
255		return TRUE;
256	}
257
258	mFrameNum++;
259
260	//-------------------------------------------------------------------------
261	// compute target position by projecting ankles to the ground
262	//-------------------------------------------------------------------------
263	if ( mTrackAnkles )
264	{
265		mCharacter->getGround( mAnkleLeftJoint.getWorldPosition(), mPositionLeft, mNormalLeft);
266		mCharacter->getGround( mAnkleRightJoint.getWorldPosition(), mPositionRight, mNormalRight);
267
268		mTargetLeft.setPosition( mPositionLeft );
269		mTargetRight.setPosition( mPositionRight );
270	}
271
272	//-------------------------------------------------------------------------
273	// update solvers
274	//-------------------------------------------------------------------------
275	mIKLeft.solve();
276	mIKRight.solve();
277
278	//-------------------------------------------------------------------------
279	// make ankle rotation conform to the ground
280	//-------------------------------------------------------------------------
281	if ( mTrackAnkles )
282	{
283		LLVector4 dirLeft4 = mAnkleLeftJoint.getWorldMatrix().getFwdRow4();
284		LLVector4 dirRight4 = mAnkleRightJoint.getWorldMatrix().getFwdRow4();
285		LLVector3 dirLeft = vec4to3( dirLeft4 );
286		LLVector3 dirRight = vec4to3( dirRight4 );
287
288		LLVector3 up;
289		LLVector3 dir;
290		LLVector3 left;
291
292		up = mNormalLeft;
293		up.normVec();
294		if (mFlipFeet)
295		{
296			up *= -1.0f;
297		}
298		dir = dirLeft;
299		dir.normVec();
300		left = up % dir;
301		left.normVec();
302		dir = left % up;
303		mRotationLeft = LLQuaternion( dir, left, up );
304
305		up = mNormalRight;
306		up.normVec();
307		if (mFlipFeet)
308		{
309			up *= -1.0f;
310		}
311		dir = dirRight;
312		dir.normVec();
313		left = up % dir;
314		left.normVec();
315		dir = left % up;
316		mRotationRight = LLQuaternion( dir, left, up );
317	}
318	mAnkleLeftJoint.setWorldRotation( mRotationLeft );
319	mAnkleRightJoint.setWorldRotation( mRotationRight );
320
321	//-------------------------------------------------------------------------
322	// propagate joint rotations to joint states
323	//-------------------------------------------------------------------------
324	mHipLeftState->setRotation( mHipLeftJoint.getRotation() );
325	mKneeLeftState->setRotation( mKneeLeftJoint.getRotation() );
326	mAnkleLeftState->setRotation( mAnkleLeftJoint.getRotation() );
327
328	mHipRightState->setRotation( mHipRightJoint.getRotation() );
329	mKneeRightState->setRotation( mKneeRightJoint.getRotation() );
330	mAnkleRightState->setRotation( mAnkleRightJoint.getRotation() );
331
332	//llinfos << "Stand drift amount " << (mCharacter->getCharacterPosition() - mLastGoodPosition).magVec() << llendl;
333
334//	llinfos << "DEBUG: " << speed << " : " << mTrackAnkles << llendl;
335	return TRUE;
336}
337
338// End