1 files changed, 253 insertions, 0 deletions

diff --git a/linden/indra/llcharacter/lleditingmotion.cpp b/linden/indra/llcharacter/lleditingmotion.cpp
new file mode 100644
index 0000000..66608f8
--- /dev/null
+++ b/linden/indra/llcharacter/lleditingmotion.cpp
@@ -0,0 +1,253 @@
+/** 
+ * @file lleditingmotion.cpp
+ * @brief Implementation of LLEditingMotion class.
+ *
+ * Copyright (c) 2001-2007, Linden Research, Inc.
+ * 
+ * The source code in this file ("Source Code") is provided by Linden Lab
+ * to you under the terms of the GNU General Public License, version 2.0
+ * ("GPL"), unless you have obtained a separate licensing agreement
+ * ("Other License"), formally executed by you and Linden Lab.  Terms of
+ * the GPL can be found in doc/GPL-license.txt in this distribution, or
+ * online at http://secondlife.com/developers/opensource/gplv2
+ * 
+ * There are special exceptions to the terms and conditions of the GPL as
+ * it is applied to this Source Code. View the full text of the exception
+ * in the file doc/FLOSS-exception.txt in this software distribution, or
+ * online at http://secondlife.com/developers/opensource/flossexception
+ * 
+ * By copying, modifying or distributing this software, you acknowledge
+ * that you have read and understood your obligations described above,
+ * and agree to abide by those obligations.
+ * 
+ * ALL LINDEN LAB SOURCE CODE IS PROVIDED "AS IS." LINDEN LAB MAKES NO
+ * WARRANTIES, EXPRESS, IMPLIED OR OTHERWISE, REGARDING ITS ACCURACY,
+ * COMPLETENESS OR PERFORMANCE.
+ */
+
+//-----------------------------------------------------------------------------
+// Header Files
+//-----------------------------------------------------------------------------
+#include "linden_common.h"
+
+#include "lleditingmotion.h"
+#include "llcharacter.h"
+#include "llhandmotion.h"
+#include "llcriticaldamp.h"
+
+//-----------------------------------------------------------------------------
+// Constants
+//-----------------------------------------------------------------------------
+const LLQuaternion EDIT_MOTION_WRIST_ROTATION(F_PI_BY_TWO * 0.7f, LLVector3(1.0f, 0.0f, 0.0f));
+const F32 TARGET_LAG_HALF_LIFE  = 0.1f;         // half-life of IK targeting
+const F32 TORSO_LAG_HALF_LIFE = 0.2f;
+const F32 MAX_TIME_DELTA = 2.f; //max two seconds a frame for calculating interpolation
+
+S32 LLEditingMotion::sHandPose = LLHandMotion::HAND_POSE_RELAXED_R;
+S32 LLEditingMotion::sHandPosePriority = 3;
+
+//-----------------------------------------------------------------------------
+// LLEditingMotion()
+// Class Constructor
+//-----------------------------------------------------------------------------
+LLEditingMotion::LLEditingMotion( const LLUUID &id) : LLMotion(id)
+{
+        mCharacter = NULL;
+
+        // create kinematic chain
+        mParentJoint.addChild( &mShoulderJoint );
+        mShoulderJoint.addChild( &mElbowJoint );
+        mElbowJoint.addChild( &mWristJoint );
+
+        mName = "editing";
+}
+
+
+//-----------------------------------------------------------------------------
+// ~LLEditingMotion()
+// Class Destructor
+//-----------------------------------------------------------------------------
+LLEditingMotion::~LLEditingMotion()
+{
+}
+
+//-----------------------------------------------------------------------------
+// LLEditingMotion::onInitialize(LLCharacter *character)
+//-----------------------------------------------------------------------------
+LLMotion::LLMotionInitStatus LLEditingMotion::onInitialize(LLCharacter *character)
+{
+        // save character for future use
+        mCharacter = character;
+
+        // make sure character skeleton is copacetic
+        if (!mCharacter->getJoint("mShoulderLeft") ||
+                !mCharacter->getJoint("mElbowLeft") ||
+                !mCharacter->getJoint("mWristLeft"))
+        {
+                llwarns << "Invalid skeleton for editing motion!" << llendl;
+                return STATUS_FAILURE;
+        }
+
+        // get the shoulder, elbow, wrist joints from the character
+        mParentState.setJoint( mCharacter->getJoint("mShoulderLeft")->getParent() );
+        mShoulderState.setJoint( mCharacter->getJoint("mShoulderLeft") );
+        mElbowState.setJoint( mCharacter->getJoint("mElbowLeft") );
+        mWristState.setJoint( mCharacter->getJoint("mWristLeft") );
+        mTorsoState.setJoint( mCharacter->getJoint("mTorso"));
+
+        if ( ! mParentState.getJoint() )
+        {
+                llinfos << getName() << ": Can't get parent joint." << llendl;
+                return STATUS_FAILURE;
+        }
+
+        mWristOffset = LLVector3(0.0f, 0.2f, 0.0f);
+
+        // add joint states to the pose
+        mShoulderState.setUsage(LLJointState::ROT);
+        mElbowState.setUsage(LLJointState::ROT);
+        mTorsoState.setUsage(LLJointState::ROT);
+        mWristState.setUsage(LLJointState::ROT);
+        addJointState( &mShoulderState );
+        addJointState( &mElbowState );
+        addJointState( &mTorsoState );
+        addJointState( &mWristState );
+
+        // propagate joint positions to kinematic chain
+        mParentJoint.setPosition(       mParentState.getJoint()->getWorldPosition() );
+        mShoulderJoint.setPosition(     mShoulderState.getJoint()->getPosition() );
+        mElbowJoint.setPosition(        mElbowState.getJoint()->getPosition() );
+        mWristJoint.setPosition(        mWristState.getJoint()->getPosition() + mWristOffset );
+
+        // propagate current joint rotations to kinematic chain
+        mParentJoint.setRotation(       mParentState.getJoint()->getWorldRotation() );
+        mShoulderJoint.setRotation(     mShoulderState.getJoint()->getRotation() );
+        mElbowJoint.setRotation(        mElbowState.getJoint()->getRotation() );
+
+        // connect the ikSolver to the chain
+        mIKSolver.setPoleVector( LLVector3( -1.0f, 1.0f, 0.0f ) );
+        // specifying the elbow's axis will prevent bad IK for the more
+        // singular configurations, but the axis is limb-specific -- Leviathan 
+        mIKSolver.setBAxis( LLVector3( -0.682683f, 0.0f, -0.730714f ) );
+        mIKSolver.setupJoints( &mShoulderJoint, &mElbowJoint, &mWristJoint, &mTarget );
+
+        return STATUS_SUCCESS;
+}
+
+//-----------------------------------------------------------------------------
+// LLEditingMotion::onActivate()
+//-----------------------------------------------------------------------------
+BOOL LLEditingMotion::onActivate()
+{
+        // propagate joint positions to kinematic chain
+        mParentJoint.setPosition(       mParentState.getJoint()->getWorldPosition() );
+        mShoulderJoint.setPosition(     mShoulderState.getJoint()->getPosition() );
+        mElbowJoint.setPosition(        mElbowState.getJoint()->getPosition() );
+        mWristJoint.setPosition(        mWristState.getJoint()->getPosition() + mWristOffset );
+
+        // propagate current joint rotations to kinematic chain
+        mParentJoint.setRotation(       mParentState.getJoint()->getWorldRotation() );
+        mShoulderJoint.setRotation(     mShoulderState.getJoint()->getRotation() );
+        mElbowJoint.setRotation(        mElbowState.getJoint()->getRotation() );
+
+        return TRUE;
+}
+
+//-----------------------------------------------------------------------------
+// LLEditingMotion::onUpdate()
+//-----------------------------------------------------------------------------
+BOOL LLEditingMotion::onUpdate(F32 time, U8* joint_mask)
+{
+        LLVector3 focus_pt;
+        LLVector3* pointAtPt = (LLVector3*)mCharacter->getAnimationData("PointAtPoint");
+
+
+        BOOL result = TRUE;
+
+        if (!pointAtPt)
+        {
+                focus_pt = mLastSelectPt;
+                result = FALSE;
+        }
+        else
+        {
+                focus_pt = *pointAtPt;
+                mLastSelectPt = focus_pt;
+        }
+
+        focus_pt += mCharacter->getCharacterPosition();
+
+        // propagate joint positions to kinematic chain
+        mParentJoint.setPosition(       mParentState.getJoint()->getWorldPosition() );
+        mShoulderJoint.setPosition(     mShoulderState.getJoint()->getPosition() );
+        mElbowJoint.setPosition(        mElbowState.getJoint()->getPosition() );
+        mWristJoint.setPosition(        mWristState.getJoint()->getPosition() + mWristOffset );
+
+        // propagate current joint rotations to kinematic chain
+        mParentJoint.setRotation(       mParentState.getJoint()->getWorldRotation() );
+        mShoulderJoint.setRotation(     mShoulderState.getJoint()->getRotation() );
+        mElbowJoint.setRotation(        mElbowState.getJoint()->getRotation() );
+
+        // update target position from character
+        LLVector3 target = focus_pt - mParentJoint.getPosition();
+        F32 target_dist = target.normVec();
+        
+        LLVector3 edit_plane_normal(1.f / F_SQRT2, 1.f / F_SQRT2, 0.f);
+        edit_plane_normal.normVec();
+
+        edit_plane_normal.rotVec(mTorsoState.getJoint()->getWorldRotation());
+        
+        F32 dot = edit_plane_normal * target;
+
+        if (dot < 0.f)
+        {
+                target = target + (edit_plane_normal * (dot * 2.f));
+                target.mV[VZ] += clamp_rescale(dot, 0.f, -1.f, 0.f, 5.f);
+                target.normVec();
+        }
+
+        target = target * target_dist;
+        if (!target.isFinite())
+        {
+                llerrs << "Non finite target in editing motion with target distance of " << target_dist << 
+                        " and focus point " << focus_pt << llendl;
+        }
+        
+        mTarget.setPosition( target + mParentJoint.getPosition());
+
+//      llinfos << "Point At: " << mTarget.getPosition() << llendl;
+
+        // update the ikSolver
+        if (!mTarget.getPosition().isExactlyZero())
+        {
+                LLQuaternion shoulderRot = mShoulderJoint.getRotation();
+                LLQuaternion elbowRot = mElbowJoint.getRotation();
+                mIKSolver.solve();
+
+                // use blending...
+                F32 slerp_amt = LLCriticalDamp::getInterpolant(TARGET_LAG_HALF_LIFE);
+                shoulderRot = slerp(slerp_amt, mShoulderJoint.getRotation(), shoulderRot);
+                elbowRot = slerp(slerp_amt, mElbowJoint.getRotation(), elbowRot);
+
+                // now put blended values back into joints
+                llassert(shoulderRot.isFinite());
+                llassert(elbowRot.isFinite());
+                mShoulderState.setRotation(shoulderRot);
+                mElbowState.setRotation(elbowRot);
+                mWristState.setRotation(LLQuaternion::DEFAULT);
+        }
+
+        mCharacter->setAnimationData("Hand Pose", &sHandPose);
+        mCharacter->setAnimationData("Hand Pose Priority", &sHandPosePriority);
+        return result;
+}
+
+//-----------------------------------------------------------------------------
+// LLEditingMotion::onDeactivate()
+//-----------------------------------------------------------------------------
+void LLEditingMotion::onDeactivate()
+{
+}
+
+
+// End