/** * @file lljoint.cpp * @brief Implementation of LLJoint class. * * Copyright (c) 2001-$CurrentYear$, Linden Research, Inc. * $License$ */ //----------------------------------------------------------------------------- // 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()) { this->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; } //----------------------------------------------------------------------------- // setConstraintSilhouette() //----------------------------------------------------------------------------- void LLJoint::setConstraintSilhouette(LLDynamicArray& silhouette) { S32 i; mConstraintSilhouette.reset(); for (i = 0; i < silhouette.count(); i++) { if (i % 2 == 1) { // skip normals continue; } mConstraintSilhouette[i / 2] = silhouette[i]; } LLQuaternion inv_parent_rotation = LLQuaternion::DEFAULT; if (getParent()) { inv_parent_rotation = ~getParent()->getWorldRotation(); } for (i = 0; i < mConstraintSilhouette.count(); i++) { LLVector3 vert = mConstraintSilhouette[i]; vert -= getWorldPosition(); vert.normVec(); vert = vert * inv_parent_rotation; } } //----------------------------------------------------------------------------- // 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