/S19/indra/llcharacter/lljoint.cpp

/** 

 * @file lljoint.cpp

 * @brief Implementation of LLJoint class.

 *

 * $LicenseInfo:firstyear=2001&license=viewergpl$

 * 

 * Copyright (c) 2001-2009, Linden Research, Inc.

 * 

 * Second Life Viewer Source Code

 * 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://secondlifegrid.net/programs/open_source/licensing/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://secondlifegrid.net/programs/open_source/licensing/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.

 * $/LicenseInfo$

 */



//-----------------------------------------------------------------------------

// Header Files

//-----------------------------------------------------------------------------

#include "linden_common.h"



#include "lljoint.h"



#include "llmath.h"



S32 LLJoint::sNumUpdates = 0;

S32 LLJoint::sNumTouches = 0;



//-----------------------------------------------------------------------------

// LLJoint()

// Class Constructor

//-----------------------------------------------------------------------------

LLJoint::LLJoint()

{

	mName = "unnamed";

	mParent = NULL;

	mXform.setScaleChildOffset(TRUE);

	mXform.setScale(LLVector3(1.0f, 1.0f, 1.0f));

	mDirtyFlags = MATRIX_DIRTY | ROTATION_DIRTY | POSITION_DIRTY;

	mUpdateXform = TRUE;

	mJointNum = -1;

	touch();

}





//-----------------------------------------------------------------------------

// LLJoint()

// Class Constructor

//-----------------------------------------------------------------------------

LLJoint::LLJoint(const std::string &name, LLJoint *parent)

{

	mName = "unnamed";

	mParent = NULL;

	mXform.setScaleChildOffset(TRUE);

	mXform.setScale(LLVector3(1.0f, 1.0f, 1.0f));

	mDirtyFlags = MATRIX_DIRTY | ROTATION_DIRTY | POSITION_DIRTY;

	mJointNum = 0;



	setName(name);

	if (parent)

	{

		parent->addChild( this );

	}

	touch();

}



//-----------------------------------------------------------------------------

// ~LLJoint()

// Class Destructor

//-----------------------------------------------------------------------------

LLJoint::~LLJoint()

{

	if (mParent)

	{

		mParent->removeChild( this );

	}

	removeAllChildren();

}





//-----------------------------------------------------------------------------

// setup()

//-----------------------------------------------------------------------------

void LLJoint::setup(const std::string &name, LLJoint *parent)

{

	setName(name);

	if (parent)

	{

		parent->addChild( this );

	}

}



//-----------------------------------------------------------------------------

// touch()

// Sets all dirty flags for all children, recursively.

//-----------------------------------------------------------------------------

void LLJoint::touch(U32 flags)

{

	if ((flags | mDirtyFlags) != mDirtyFlags)

	{

		sNumTouches++;

		mDirtyFlags |= flags;

		U32 child_flags = flags;

		if (flags & ROTATION_DIRTY)

		{

			child_flags |= POSITION_DIRTY;

		}



		for (child_list_t::iterator iter = mChildren.begin();

			 iter != mChildren.end(); ++iter)

		{

			LLJoint* joint = *iter;

			joint->touch(child_flags);

		}

	}

}



//-----------------------------------------------------------------------------

// getRoot()

//-----------------------------------------------------------------------------

LLJoint *LLJoint::getRoot()

{

	if ( getParent() == NULL )

	{

		return this;

	}

	return getParent()->getRoot();

}





//-----------------------------------------------------------------------------

// findJoint()

//-----------------------------------------------------------------------------

LLJoint *LLJoint::findJoint( const std::string &name )

{

	if (name == getName())

		return this;



	for (child_list_t::iterator iter = mChildren.begin();

		 iter != mChildren.end(); ++iter)

	{

		LLJoint* joint = *iter;

		LLJoint *found = joint->findJoint(name);

		if (found)

		{

			return found;

		}

	}



	return NULL;	

}





//--------------------------------------------------------------------

// addChild()

//--------------------------------------------------------------------

void LLJoint::addChild(LLJoint* joint)

{

	if (joint->mParent)

		joint->mParent->removeChild(joint);



	mChildren.push_back(joint);

	joint->mXform.setParent(&mXform);

	joint->mParent = this;	

	joint->touch();

}





//--------------------------------------------------------------------

// removeChild()

//--------------------------------------------------------------------

void LLJoint::removeChild(LLJoint* joint)

{

	child_list_t::iterator iter = std::find(mChildren.begin(), mChildren.end(), joint);

	if (iter != mChildren.end())

	{

		mChildren.erase(iter);

	

		joint->mXform.setParent(NULL);

		joint->mParent = NULL;

		joint->touch();

	}

}





//--------------------------------------------------------------------

// removeAllChildren()

//--------------------------------------------------------------------

void LLJoint::removeAllChildren()

{

	for (child_list_t::iterator iter = mChildren.begin();

		 iter != mChildren.end();)

	{

		child_list_t::iterator curiter = iter++;

		LLJoint* joint = *curiter;

		mChildren.erase(curiter);

		joint->mXform.setParent(NULL);

		joint->mParent = NULL;

		joint->touch();

	}

}





//--------------------------------------------------------------------

// getPosition()

//--------------------------------------------------------------------

const LLVector3& LLJoint::getPosition()

{

	return mXform.getPosition();

}





//--------------------------------------------------------------------

// setPosition()

//--------------------------------------------------------------------

void LLJoint::setPosition( const LLVector3& pos )

{

//	if (mXform.getPosition() != pos)

	{

		mXform.setPosition(pos);

		touch(MATRIX_DIRTY | POSITION_DIRTY);

	}

}





//--------------------------------------------------------------------

// getWorldPosition()

//--------------------------------------------------------------------

LLVector3 LLJoint::getWorldPosition()

{

	updateWorldPRSParent();

	return mXform.getWorldPosition();

}



//-----------------------------------------------------------------------------

// getLastWorldPosition()

//-----------------------------------------------------------------------------

LLVector3 LLJoint::getLastWorldPosition()

{

	return mXform.getWorldPosition();

}





//--------------------------------------------------------------------

// setWorldPosition()

//--------------------------------------------------------------------

void LLJoint::setWorldPosition( const LLVector3& pos )

{

	if (mParent == NULL)

	{

		this->setPosition( pos );

		return;

	}



	LLMatrix4 temp_matrix = getWorldMatrix();

	temp_matrix.mMatrix[VW][VX] = pos.mV[VX];

	temp_matrix.mMatrix[VW][VY] = pos.mV[VY];

	temp_matrix.mMatrix[VW][VZ] = pos.mV[VZ];



	LLMatrix4 parentWorldMatrix = mParent->getWorldMatrix();

	LLMatrix4 invParentWorldMatrix = parentWorldMatrix.invert();



	temp_matrix *= invParentWorldMatrix;



	LLVector3 localPos(	temp_matrix.mMatrix[VW][VX],

						temp_matrix.mMatrix[VW][VY],

						temp_matrix.mMatrix[VW][VZ] );



	setPosition( localPos );

}





//--------------------------------------------------------------------

// mXform.getRotation()

//--------------------------------------------------------------------

const LLQuaternion& LLJoint::getRotation()

{

	return mXform.getRotation();

}





//--------------------------------------------------------------------

// setRotation()

//--------------------------------------------------------------------

void LLJoint::setRotation( const LLQuaternion& rot )

{

	if (rot.isFinite())

	{

	//	if (mXform.getRotation() != rot)

		{

			mXform.setRotation(rot);

			touch(MATRIX_DIRTY | ROTATION_DIRTY);

		}

	}

}





//--------------------------------------------------------------------

// getWorldRotation()

//--------------------------------------------------------------------

LLQuaternion LLJoint::getWorldRotation()

{

	updateWorldPRSParent();



	return mXform.getWorldRotation();

}



//-----------------------------------------------------------------------------

// getLastWorldRotation()

//-----------------------------------------------------------------------------

LLQuaternion LLJoint::getLastWorldRotation()

{

	return mXform.getWorldRotation();

}



//--------------------------------------------------------------------

// setWorldRotation()

//--------------------------------------------------------------------

void LLJoint::setWorldRotation( const LLQuaternion& rot )

{

	if (mParent == NULL)

	{

		this->setRotation( rot );

		return;

	}



	LLMatrix4 temp_mat(rot);



	LLMatrix4 parentWorldMatrix = mParent->getWorldMatrix();

	parentWorldMatrix.mMatrix[VW][VX] = 0;

	parentWorldMatrix.mMatrix[VW][VY] = 0;

	parentWorldMatrix.mMatrix[VW][VZ] = 0;



	LLMatrix4 invParentWorldMatrix = parentWorldMatrix.invert();



	temp_mat *= invParentWorldMatrix;



	setRotation(LLQuaternion(temp_mat));

}





//--------------------------------------------------------------------

// getScale()

//--------------------------------------------------------------------

const LLVector3& LLJoint::getScale()

{

	return mXform.getScale();

}



//--------------------------------------------------------------------

// setScale()

//--------------------------------------------------------------------

void LLJoint::setScale( const LLVector3& scale )

{

//	if (mXform.getScale() != scale)

	{

		mXform.setScale(scale);

		touch();

	}



}







//--------------------------------------------------------------------

// getWorldMatrix()

//--------------------------------------------------------------------

const LLMatrix4 &LLJoint::getWorldMatrix()

{

	updateWorldMatrixParent();



	return mXform.getWorldMatrix();

}





//--------------------------------------------------------------------

// setWorldMatrix()

//--------------------------------------------------------------------

void LLJoint::setWorldMatrix( const LLMatrix4& mat )

{

llinfos << "WARNING: LLJoint::setWorldMatrix() not correctly implemented yet" << llendl;

	// extract global translation

	LLVector3 trans(	mat.mMatrix[VW][VX],

						mat.mMatrix[VW][VY],

						mat.mMatrix[VW][VZ] );



	// extract global rotation

	LLQuaternion rot( mat );



	setWorldPosition( trans );

	setWorldRotation( rot );

}



//-----------------------------------------------------------------------------

// updateWorldMatrixParent()

//-----------------------------------------------------------------------------

void LLJoint::updateWorldMatrixParent()

{

	if (mDirtyFlags & MATRIX_DIRTY)

	{

		LLJoint *parent = getParent();

		if (parent)

		{

			parent->updateWorldMatrixParent();

		}

		updateWorldMatrix();

	}

}



//-----------------------------------------------------------------------------

// updateWorldPRSParent()

//-----------------------------------------------------------------------------

void LLJoint::updateWorldPRSParent()

{

	if (mDirtyFlags & (ROTATION_DIRTY | POSITION_DIRTY))

	{

		LLJoint *parent = getParent();

		if (parent)

		{

			parent->updateWorldPRSParent();

		}



		mXform.update();

		mDirtyFlags &= ~(ROTATION_DIRTY | POSITION_DIRTY);

	}

}



//-----------------------------------------------------------------------------

// updateWorldMatrixChildren()

//-----------------------------------------------------------------------------

void LLJoint::updateWorldMatrixChildren()

{	

	if (!this->mUpdateXform) return;



	if (mDirtyFlags & MATRIX_DIRTY)

	{

		updateWorldMatrix();

	}

	for (child_list_t::iterator iter = mChildren.begin();

		 iter != mChildren.end(); ++iter)

	{

		LLJoint* joint = *iter;

		joint->updateWorldMatrixChildren();

	}

}



//-----------------------------------------------------------------------------

// updateWorldMatrix()

//-----------------------------------------------------------------------------

void LLJoint::updateWorldMatrix()

{

	if (mDirtyFlags & MATRIX_DIRTY)

	{

		sNumUpdates++;

		mXform.updateMatrix(FALSE);

		mDirtyFlags = 0x0;

	}

}



//--------------------------------------------------------------------

// getSkinOffset()

//--------------------------------------------------------------------

const LLVector3 &LLJoint::getSkinOffset()

{

	return mSkinOffset;

}





//--------------------------------------------------------------------

// setSkinOffset()

//--------------------------------------------------------------------

void LLJoint::setSkinOffset( const LLVector3& offset )

{

	mSkinOffset = offset;

}





//-----------------------------------------------------------------------------

// clampRotation()

//-----------------------------------------------------------------------------

void LLJoint::clampRotation(LLQuaternion old_rot, LLQuaternion new_rot)

{

	LLVector3 main_axis(1.f, 0.f, 0.f);



	for (child_list_t::iterator iter = mChildren.begin();

		 iter != mChildren.end(); ++iter)

	{

		LLJoint* joint = *iter;

		if (joint->isAnimatable())

		{

			main_axis = joint->getPosition();

			main_axis.normVec();

			// only care about first animatable child

			break;

		}

	}



	// 2003.03.26 - This code was just using up cpu cycles. AB



//	LLVector3 old_axis = main_axis * old_rot;

//	LLVector3 new_axis = main_axis * new_rot;



//	for (S32 i = 0; i < mConstraintSilhouette.count() - 1; i++)

//	{

//		LLVector3 vert1 = mConstraintSilhouette[i];

//		LLVector3 vert2 = mConstraintSilhouette[i + 1];



		// figure out how to clamp rotation to line on 3-sphere



//	}

}



// End