C++ LLQuaternion类代码示例

// linear interpolation
LLQuaternion lerp(F32 t, const LLQuaternion &p, const LLQuaternion &q)
{
	LLQuaternion r;
	r = t * (q - p) + p;
	r.normalize();
	return r;
}

void LLToolGrab::startGrab()
{
	// Compute grab_offset in the OBJECT's root's coordinate frame
	// (sometimes root == object)
	LLViewerObject* objectp = mGrabPick.getObject();
	if (!objectp)
	{
		return;
	}

	LLViewerObject *root = (LLViewerObject *)objectp->getRoot();

	// drag from center
	LLVector3d grab_start_global = root->getPositionGlobal();

	// Where the grab starts, relative to the center of the root object of the set.
	// JC - This code looks wonky, but I believe it does the right thing.
	// Otherwise, when you grab a linked object set, it "pops" on the start
	// of the drag.
	LLVector3d grab_offsetd = root->getPositionGlobal() - objectp->getPositionGlobal();

	LLVector3 grab_offset;
	grab_offset.setVec(grab_offsetd);

	LLQuaternion rotation = root->getRotation();
	rotation.conjQuat();
	grab_offset = grab_offset * rotation;

	// This planar drag starts at the grab point
	mDragStartPointGlobal = grab_start_global;
	mDragStartFromCamera = grab_start_global - gAgent.getCameraPositionGlobal();

	LLMessageSystem	*msg = gMessageSystem;
	msg->newMessageFast(_PREHASH_ObjectGrab);
	msg->nextBlockFast(_PREHASH_AgentData);
	msg->addUUIDFast(_PREHASH_AgentID, gAgent.getID());
	msg->addUUIDFast(_PREHASH_SessionID, gAgent.getSessionID());
	msg->nextBlockFast(_PREHASH_ObjectData);
	msg->addU32Fast(_PREHASH_LocalID, objectp->mLocalID);
	msg->addVector3Fast(_PREHASH_GrabOffset, grab_offset );
	msg->nextBlock("SurfaceInfo");
	msg->addVector3("UVCoord", LLVector3(mGrabPick.mUVCoords));
	msg->addVector3("STCoord", LLVector3(mGrabPick.mSTCoords));
	msg->addS32Fast(_PREHASH_FaceIndex, mGrabPick.mObjectFace);
	msg->addVector3("Position", mGrabPick.mIntersection);
	msg->addVector3("Normal", mGrabPick.mNormal);
	msg->addVector3("Binormal", mGrabPick.mBinormal);
	msg->sendMessage( objectp->getRegion()->getHost());

	mGrabOffsetFromCenterInitial = grab_offset;
	mGrabHiddenOffsetFromCamera = mDragStartFromCamera;

	mGrabTimer.reset();
}

void lggBeamMaps::fireCurrentBeams(LLPointer<LLHUDEffectSpiral> mBeam, const LLColor4U& rgb)
{
	if (mScale == 0.0f)
	{
		return;
	}

	static LLCachedControl<std::string> colorf(gSavedSettings, "FSBeamColorFile");
	bool colorsDisabled = (colorf().empty());
	
	for (std::vector<lggBeamData>::iterator it = mDots.begin(); it != mDots.end(); ++it)
	{
		LLColor4U myColor = rgb;
		if (colorsDisabled)
		{
			myColor = (*it).c;
		}

		F32 distanceAdjust = dist_vec(mBeam->getPositionGlobal(), gAgent.getPositionGlobal());
		F32 pulse = (F32)(.75f + sinf(gFrameTimeSeconds * 1.0f) * 0.25f);
		LLVector3d offset = (*it).p;
		offset.mdV[VY] *= -1.f;
		offset *= pulse * mScale * distanceAdjust * 0.1f;
		
		LLVector3 beamLine = LLVector3( mBeam->getPositionGlobal() - gAgent.getPositionGlobal());
		LLVector3 beamLineFlat = beamLine;
		beamLineFlat.mV[VZ]= 0.0f;

		LLVector3 newDirFlat = LLVector3::x_axis;
		beamLine.normalize();
		beamLineFlat.normalize();
		LLQuaternion change;
		change.shortestArc(newDirFlat, beamLineFlat);
		offset.rotVec(change);
		newDirFlat.rotVec(change);
		change.shortestArc(newDirFlat, beamLine);
		offset.rotVec(change);

		LLPointer<LLHUDEffectSpiral> myBeam = (LLHUDEffectSpiral *)LLHUDManager::getInstance()->createViewerEffect(LLHUDObject::LL_HUD_EFFECT_BEAM);
		myBeam->setPositionGlobal(mBeam->getPositionGlobal() + offset + (LLVector3d(beamLine) * sinf(gFrameTimeSeconds * 2.0f) * 0.2f));
		
		myBeam->setColor(myColor);
		myBeam->setTargetObject(mBeam->getTargetObject());
		myBeam->setSourceObject(mBeam->getSourceObject());
		myBeam->setNeedsSendToSim(mBeam->getNeedsSendToSim());
		myBeam->setDuration(mDuration * 1.2f);
	}
}

void ImportTracker::wear(LLSD &prim)
{
	LLMessageSystem* msg = gMessageSystem;
	msg->newMessageFast(_PREHASH_ObjectAttach);
	msg->nextBlockFast(_PREHASH_AgentData);
	msg->addUUIDFast(_PREHASH_AgentID, gAgent.getID() );
	msg->addUUIDFast(_PREHASH_SessionID, gAgent.getSessionID());
	msg->addU8Fast(_PREHASH_AttachmentPoint, U8(prim["Attachment"].asInteger()));
	
	msg->nextBlockFast(_PREHASH_ObjectData);
	msg->addU32Fast(_PREHASH_ObjectLocalID, prim["LocalID"].asInteger());
	msg->addQuatFast(_PREHASH_Rotation, LLQuaternion(0.0f, 0.0f, 0.0f, 1.0f));
	
	msg->sendReliable(gAgent.getRegion()->getHost());

	LLVector3 position = prim["attachpos"];
	
	LLSD rot = prim["attachrot"];
	LLQuaternion rotq;
	rotq.mQ[VX] = (F32)(rot[0].asReal());
	rotq.mQ[VY] = (F32)(rot[1].asReal());
	rotq.mQ[VZ] = (F32)(rot[2].asReal());
	rotq.mQ[VW] = (F32)(rot[3].asReal());
	LLVector3 rotation = rotq.packToVector3();
	U8 data[256];
	
	LLMessageSystem* msg2 = gMessageSystem;
	msg2->newMessageFast(_PREHASH_MultipleObjectUpdate);
	msg2->nextBlockFast(_PREHASH_AgentData);
	msg2->addUUIDFast(_PREHASH_AgentID, gAgent.getID());
	msg2->addUUIDFast(_PREHASH_SessionID, gAgent.getSessionID());
	
	msg2->nextBlockFast(_PREHASH_ObjectData);
	msg2->addU32Fast(_PREHASH_ObjectLocalID, prim["LocalID"].asInteger());
	msg2->addU8Fast(_PREHASH_Type, U8(0x01 | 0x08));
	htonmemcpy(&data[0], &(position.mV), MVT_LLVector3, 12);
	msg2->addBinaryDataFast(_PREHASH_Data, data, 12);
	
	msg2->nextBlockFast(_PREHASH_ObjectData);
	msg2->addU32Fast(_PREHASH_ObjectLocalID, prim["LocalID"].asInteger());
	msg2->addU8Fast(_PREHASH_Type, U8(0x02 | 0x08));
	htonmemcpy(&data[0], &(rotation.mV), MVT_LLQuaternion, 12); 
	msg2->addBinaryDataFast(_PREHASH_Data, data, 12);
	
	msg2->sendReliable(gAgent.getRegion()->getHost());
	llinfos << "POSITIONED, IMPORT COMPLETED" << llendl;
	cleanUp();
}

	void llquat_test_object_t::test<22>()
	{
		//test case for void LLQuaternion::getEulerAngles(F32 *roll, F32 *pitch, F32 *yaw) const fn
		F32 roll = -12.0f;
		F32 pitch = -22.43f;
		F32 yaw = 11.0f;

		LLQuaternion llquat;
		llquat.getEulerAngles(&roll, &pitch, &yaw);
		ensure(
			"LLQuaternion::getEulerAngles(F32 *roll, F32 *pitch, F32 *yaw) failed",
			is_approx_equal(0.000f, llquat.mQ[0]) &&
			is_approx_equal(0.000f, llquat.mQ[1]) &&
			is_approx_equal(0.000f, llquat.mQ[2]) &&
			is_approx_equal(1.000f, llquat.mQ[3]));
	}

void LLTemplateMessageReader::getQuat(const char *block, const char *var, 
									  LLQuaternion &q, S32 blocknum)
{
	LLVector3 vec;
	getData(block, var, &vec.mV[0], sizeof(vec.mV), blocknum);
	if( vec.isFinite() )
	{
		q.unpackFromVector3( vec );
	}
	else
	{
		llwarns << "non-finite in getQuatFast " << block << " " << var 
				<< llendl;
		q.loadIdentity();
	}
}

// SJB: This code is correct for a logicly stored (non-transposed) matrix;
//		Our matrices are stored transposed, OpenGL style, so this generates the
//		INVERSE quaternion (-x, -y, -z, w)!
//		Because we use similar logic in LLQuaternion::getMatrix3,
//		we are internally consistant so everything works OK :)
LLQuaternion	LLMatrix3::quaternion() const
{
	LLQuaternion	quat;
	F32		tr, s, q[4];
	U32		i, j, k;
	U32		nxt[3] = {1, 2, 0};

	tr = mMatrix[0][0] + mMatrix[1][1] + mMatrix[2][2];

	// check the diagonal
	if (tr > 0.f) 
	{
		s = (F32)sqrt (tr + 1.f);
		quat.mQ[VS] = s / 2.f;
		s = 0.5f / s;
		quat.mQ[VX] = (mMatrix[1][2] - mMatrix[2][1]) * s;
		quat.mQ[VY] = (mMatrix[2][0] - mMatrix[0][2]) * s;
		quat.mQ[VZ] = (mMatrix[0][1] - mMatrix[1][0]) * s;
	} 
	else
	{		
		// diagonal is negative
		i = 0;
		if (mMatrix[1][1] > mMatrix[0][0]) 
			i = 1;
		if (mMatrix[2][2] > mMatrix[i][i]) 
			i = 2;

		j = nxt[i];
		k = nxt[j];


		s = (F32)sqrt ((mMatrix[i][i] - (mMatrix[j][j] + mMatrix[k][k])) + 1.f);

		q[i] = s * 0.5f;

		if (s != 0.f) 
			s = 0.5f / s;

		q[3] = (mMatrix[j][k] - mMatrix[k][j]) * s;
		q[j] = (mMatrix[i][j] + mMatrix[j][i]) * s;
		q[k] = (mMatrix[i][k] + mMatrix[k][i]) * s;

		quat.setQuat(q);
	}
	return quat;
}

void LLToolGrab::startGrab()
{
	// Compute grab_offset in the OBJECT's root's coordinate frame
	// (sometimes root == object)
	LLViewerObject* objectp = mGrabPick.getObject();
	if (!objectp)
	{
		return;
	}

	LLViewerObject *root = (LLViewerObject *)objectp->getRoot();

	// drag from center
	LLVector3d grab_start_global = root->getPositionGlobal();

	// Where the grab starts, relative to the center of the root object of the set.
	// JC - This code looks wonky, but I believe it does the right thing.
	// Otherwise, when you grab a linked object set, it "pops" on the start
	// of the drag.
	LLVector3d grab_offsetd = root->getPositionGlobal() - objectp->getPositionGlobal();

	LLVector3 grab_offset;
	grab_offset.setVec(grab_offsetd);

	LLQuaternion rotation = root->getRotation();
	rotation.conjQuat();
	grab_offset = grab_offset * rotation;

	// This planar drag starts at the grab point
	mDragStartPointGlobal = grab_start_global;
	mDragStartFromCamera = grab_start_global - gAgentCamera.getCameraPositionGlobal();

	send_ObjectGrab_message(objectp, mGrabPick, grab_offset);

	mGrabOffsetFromCenterInitial = grab_offset;
	mGrabHiddenOffsetFromCamera = mDragStartFromCamera;

	mGrabTimer.reset();

	mLastUVCoords = mGrabPick.mUVCoords;
	mLastSTCoords = mGrabPick.mSTCoords;
	mLastFace = mGrabPick.mObjectFace;
	mLastIntersection = mGrabPick.mIntersection;
	mLastNormal = mGrabPick.mNormal;
	mLastBinormal = mGrabPick.mBinormal;
	mLastGrabPos = LLVector3(-1.f, -1.f, -1.f);
}

void lggBeamMaps::fireCurrentBeams(LLPointer<LLHUDEffectSpiral> mBeam, LLColor4U rgb)
{
	if (scale == 0.0f)
	{
		return;
	}

	static LLCachedControl<std::string> colorf(gSavedSettings, "FSBeamColorFile");
	bool colorsDisabled = std::string(colorf) == "===OFF===";
	
	for(int i = 0; i < (int)dots.size(); i++)
	{
		LLColor4U myColor = rgb;
		if (colorsDisabled) myColor = dots[i].c;

		F32 distanceAdjust = dist_vec(mBeam->getPositionGlobal(),gAgent.getPositionGlobal()) ;
		F32 pulse = (F32)(.75f+sinf(gFrameTimeSeconds*1.0f)*0.25f);
		LLVector3d offset = dots[i].p;
		offset.mdV[VY] *= -1;
		offset *= pulse * scale * distanceAdjust * 0.1;
		
		//llinfos << "dist is " << distanceAdjust << "scale is " << scale << llendl;
		LLVector3 beamLine = LLVector3( mBeam->getPositionGlobal() - gAgent.getPositionGlobal());
		LLVector3 beamLineFlat = beamLine;
		beamLineFlat.mV[VZ]= 0.0f;

		LLVector3 newDirFlat = LLVector3::x_axis;
		beamLine.normalize();
		beamLineFlat.normalize();
		LLQuaternion change;
		change.shortestArc(newDirFlat,beamLineFlat);
		offset.rotVec(change);
		newDirFlat.rotVec(change);
		change.shortestArc(newDirFlat,beamLine);
		offset.rotVec(change);

		LLPointer<LLHUDEffectSpiral> myBeam =  (LLHUDEffectSpiral *)LLHUDManager::getInstance()->createViewerEffect(LLHUDObject::LL_HUD_EFFECT_BEAM);
		myBeam->setPositionGlobal(mBeam->getPositionGlobal() + offset + (LLVector3d(beamLine) * sinf(gFrameTimeSeconds*2.0f) * 0.2f));
		
		myBeam->setColor(myColor);
		myBeam->setTargetObject(mBeam->getTargetObject());
		myBeam->setSourceObject(mBeam->getSourceObject());
		myBeam->setNeedsSendToSim(mBeam->getNeedsSendToSim());
		myBeam->setDuration(duration* 1.2f);
	}
}

void setup_transforms_bbox(LLBBox bbox)
{
	// translate to center
	LLVector3 center = bbox.getCenterAgent();
	gGL.translatef(center.mV[VX], center.mV[VY], center.mV[VZ]);

	// rotate
	LLQuaternion rotation = bbox.getRotation();
	F32 angle_radians, x, y, z;
	rotation.getAngleAxis(&angle_radians, &x, &y, &z);
	gGL.flush();
	gGL.rotatef(angle_radians * RAD_TO_DEG, x, y, z); 

	// scale
	LLVector3 scale = bbox.getMaxLocal() - bbox.getMinLocal();
	gGL.scalef(scale.mV[VX], scale.mV[VY], scale.mV[VZ]);
}

void setup_transforms_bbox(LLBBox bbox)
{
	// translate to center
	LLVector3 center = bbox.getCenterAgent();
	gGL.translatef(center.mV[VX], center.mV[VY], center.mV[VZ]);

	// rotate
	LLQuaternion rotation = bbox.getRotation();
	F32 angle_radians, x, y, z;
	rotation.getAngleAxis(&angle_radians, &x, &y, &z);
	// gGL has no rotate method (despite having translate and scale) presumably because
	// its authors smoke crack.  so we hack.
	gGL.flush();
	glRotatef(angle_radians * RAD_TO_DEG, x, y, z); 

	// scale
	LLVector3 scale = bbox.getMaxLocal() - bbox.getMinLocal();
	gGL.scalef(scale.mV[VX], scale.mV[VY], scale.mV[VZ]);
}

//--------------------------------------------------------------------
// setRotation()
//--------------------------------------------------------------------
void LLJoint::setRotation( const LLQuaternion& rot )
{
	if (rot.isFinite())
	{
	//	if (mXform.getRotation() != rot)
		{
			mXform.setRotation(rot);
			touch(MATRIX_DIRTY | ROTATION_DIRTY);
		}
	}
}

void hud_render_text(const LLWString &wstr, const LLVector3 &pos_agent,
					const LLFontGL &font,
					const U8 style,
					const LLFontGL::ShadowType shadow,
					const F32 x_offset, const F32 y_offset,
					const LLColor4& color,
					const BOOL orthographic)
{
	LLViewerCamera* camera = LLViewerCamera::getInstance();
	// Do cheap plane culling
	LLVector3 dir_vec = pos_agent - camera->getOrigin();
	dir_vec /= dir_vec.magVec();

	if (wstr.empty() || (!orthographic && dir_vec * camera->getAtAxis() <= 0.f))
	{
		return;
	}

	LLVector3 right_axis;
	LLVector3 up_axis;
	if (orthographic)
	{
		right_axis.setVec(0.f, -1.f / gViewerWindow->getWorldViewHeightScaled(), 0.f);
		up_axis.setVec(0.f, 0.f, 1.f / gViewerWindow->getWorldViewHeightScaled());
	}
	else
	{
		camera->getPixelVectors(pos_agent, up_axis, right_axis);
	}
	LLCoordFrame render_frame = *camera;
	LLQuaternion rot;
	if (!orthographic)
	{
		rot = render_frame.getQuaternion();
		rot = rot * LLQuaternion(-F_PI_BY_TWO, camera->getYAxis());
		rot = rot * LLQuaternion(F_PI_BY_TWO, camera->getXAxis());
	}
	else
	{
		rot = LLQuaternion(-F_PI_BY_TWO, LLVector3(0.f, 0.f, 1.f));
		rot = rot * LLQuaternion(-F_PI_BY_TWO, LLVector3(0.f, 1.f, 0.f));
	}
	F32 angle;
	LLVector3 axis;
	rot.getAngleAxis(&angle, axis);

	LLVector3 render_pos = pos_agent + (floorf(x_offset) * right_axis) + (floorf(y_offset) * up_axis);

	//get the render_pos in screen space
	
	LLVector3 window_coordinates;
	F32& winX = window_coordinates.mV[VX];
	F32& winY = window_coordinates.mV[VY];
	F32& winZ = window_coordinates.mV[VZ];

	const LLRect& world_view_rect = gViewerWindow->getWorldViewRectRaw();
	
	glProjectf(render_pos, gGLModelView, gGLProjection, world_view_rect, window_coordinates);

	//fonts all render orthographically, set up projection``
	gGL.matrixMode(LLRender::MM_PROJECTION);
	gGL.pushMatrix();
	gGL.matrixMode(LLRender::MM_MODELVIEW);
	gGL.pushMatrix();
	LLUI::pushMatrix();
		
	gl_state_for_2d(world_view_rect.getWidth(), world_view_rect.getHeight());
	gViewerWindow->setup3DViewport();
	
	winX -= world_view_rect.mLeft;
	winY -= world_view_rect.mBottom;
	LLUI::loadIdentity();
	gGL.loadIdentity();
	LLUI::translate((F32) winX*1.0f/LLFontGL::sScaleX, (F32) winY*1.0f/(LLFontGL::sScaleY), -(((F32) winZ*2.f)-1.f));
	F32 right_x;
	
	font.render(wstr, 0, 0, 0, color, LLFontGL::LEFT, LLFontGL::BASELINE, style, shadow, wstr.length(), 1000, &right_x);
	LLUI::popMatrix();
	gGL.popMatrix();
	
	gGL.matrixMode(LLRender::MM_PROJECTION);
	gGL.popMatrix();
	gGL.matrixMode(LLRender::MM_MODELVIEW);
}

void hud_render_text(const LLWString &wstr, const LLVector3 &pos_agent,
					const LLFontGL &font,
					const U8 style,
					const F32 x_offset, const F32 y_offset,
					const LLColor4& color,
					const BOOL orthographic)
{
	LLViewerCamera* camera = LLViewerCamera::getInstance();
	// Do cheap plane culling
	LLVector3 dir_vec = pos_agent - camera->getOrigin();
	dir_vec /= dir_vec.magVec();

	if (wstr.empty() || (!orthographic && dir_vec * camera->getAtAxis() <= 0.f))
	{
		return;
	}

	LLVector3 right_axis;
	LLVector3 up_axis;
	if (orthographic)
	{
		right_axis.setVec(0.f, -1.f / gViewerWindow->getWindowHeight(), 0.f);
		up_axis.setVec(0.f, 0.f, 1.f / gViewerWindow->getWindowHeight());
	}
	else
	{
		camera->getPixelVectors(pos_agent, up_axis, right_axis);
	}
	LLCoordFrame render_frame = *camera;
	LLQuaternion rot;
	if (!orthographic)
	{
		rot = render_frame.getQuaternion();
		rot = rot * LLQuaternion(-F_PI_BY_TWO, camera->getYAxis());
		rot = rot * LLQuaternion(F_PI_BY_TWO, camera->getXAxis());
	}
	else
	{
		rot = LLQuaternion(-F_PI_BY_TWO, LLVector3(0.f, 0.f, 1.f));
		rot = rot * LLQuaternion(-F_PI_BY_TWO, LLVector3(0.f, 1.f, 0.f));
	}
	F32 angle;
	LLVector3 axis;
	rot.getAngleAxis(&angle, axis);

	LLVector3 render_pos = pos_agent + (floorf(x_offset) * right_axis) + (floorf(y_offset) * up_axis);

	//get the render_pos in screen space
	
	F64 winX, winY, winZ;
	gluProject(render_pos.mV[0], render_pos.mV[1], render_pos.mV[2],
				gGLModelView, gGLProjection, (GLint*) gGLViewport,
				&winX, &winY, &winZ);
		
	//fonts all render orthographically, set up projection
	glMatrixMode(GL_PROJECTION);
	glPushMatrix();
	glMatrixMode(GL_MODELVIEW);
	
	LLUI::pushMatrix();
		
	gViewerWindow->setup2DRender();

	LLUI::loadIdentity();
	LLUI::translate((F32) winX*1.0f/LLFontGL::sScaleX, (F32) winY*1.0f/(LLFontGL::sScaleY), -(((F32) winZ*2.f)-1.f));
	//glRotatef(angle * RAD_TO_DEG, axis.mV[VX], axis.mV[VY], axis.mV[VZ]);
	//glScalef(right_scale, up_scale, 1.f);
	F32 right_x;
	
	font.render(wstr, 0, 0, 0, color, LLFontGL::LEFT, LLFontGL::BASELINE, style, wstr.length(), 1000, &right_x);
	LLUI::popMatrix();
	
	glMatrixMode(GL_PROJECTION);
	glPopMatrix();
	glMatrixMode(GL_MODELVIEW);
}

//-----------------------------------------------------------------------------
// 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())
	{
		LL_WARNS() << "Non finite target in editing motion with target distance of " << target_dist << 
			" and focus point " << focus_pt << " and pointAtPt: ";
		if (pointAtPt)
		{
		  LL_CONT << *pointAtPt;
		}
		else
		{
		  LL_CONT << "NULL";
		}
		LL_CONT << LL_ENDL;
		target.setVec(1.f, 1.f, 1.f);
	}
	
	mTarget.setPosition( target + mParentJoint.getPosition());

//	LL_INFOS() << "Point At: " << mTarget.getPosition() << LL_ENDL;

	// update the ikSolver
	if (!mTarget.getPosition().isExactlyZero())
	{
		LLQuaternion shoulderRot = mShoulderJoint.getRotation();
		LLQuaternion elbowRot = mElbowJoint.getRotation();
		mIKSolver.solve();

		// use blending...
		F32 slerp_amt = LLSmoothInterpolation::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;
}