C++ LLQuaternion::setQuat方法代码示例

// 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;
}

BOOL LLToolPlacer::addObject( LLPCode pcode, S32 x, S32 y, U8 use_physics )
{
    LLVector3 ray_start_region;
    LLVector3 ray_end_region;
    LLViewerRegion* regionp = NULL;
    BOOL b_hit_land = FALSE;
    S32 hit_face = -1;
    LLViewerObject* hit_obj = NULL;
    U8 state = 0;
    BOOL success = raycastForNewObjPos( x, y, &hit_obj, &hit_face, &b_hit_land, &ray_start_region, &ray_end_region, &regionp );
    if( !success )
    {
        return FALSE;
    }

    if( hit_obj && (hit_obj->isAvatar() || hit_obj->isAttachment()) )
    {
        // Can't create objects on avatars or attachments
        return FALSE;
    }

    if (NULL == regionp)
    {
        llwarns << "regionp was NULL; aborting function." << llendl;
        return FALSE;
    }

    if (regionp->getRegionFlags() & REGION_FLAGS_SANDBOX)
    {
        LLFirstUse::useSandbox();
    }

    // Set params for new object based on its PCode.
    LLQuaternion	rotation;
    LLVector3		scale = DEFAULT_OBJECT_SCALE;
    U8				material = LL_MCODE_WOOD;
    BOOL			create_selected = FALSE;
    LLVolumeParams	volume_params;

    switch (pcode)
    {
    case LL_PCODE_LEGACY_GRASS:
        //  Randomize size of grass patch
        scale.setVec(10.f + ll_frand(20.f), 10.f + ll_frand(20.f),  1.f + ll_frand(2.f));
        state = rand() % LLVOGrass::sMaxGrassSpecies;
        break;


    case LL_PCODE_LEGACY_TREE:
    case LL_PCODE_TREE_NEW:
        state = rand() % LLVOTree::sMaxTreeSpecies;
        break;

    case LL_PCODE_SPHERE:
    case LL_PCODE_CONE:
    case LL_PCODE_CUBE:
    case LL_PCODE_CYLINDER:
    case LL_PCODE_TORUS:
    case LLViewerObject::LL_VO_SQUARE_TORUS:
    case LLViewerObject::LL_VO_TRIANGLE_TORUS:
    default:
        create_selected = TRUE;
        break;
    }

    // Play creation sound
    if (gAudiop)
    {
        F32 volume = gSavedSettings.getBOOL("MuteUI") ? 0.f : gSavedSettings.getF32("AudioLevelUI");
        gAudiop->triggerSound( LLUUID(gSavedSettings.getString("UISndObjectCreate")), gAgent.getID(), volume);
    }

    gMessageSystem->newMessageFast(_PREHASH_ObjectAdd);
    gMessageSystem->nextBlockFast(_PREHASH_AgentData);
    gMessageSystem->addUUIDFast(_PREHASH_AgentID, gAgent.getID());
    gMessageSystem->addUUIDFast(_PREHASH_SessionID, gAgent.getSessionID());
    gMessageSystem->addUUIDFast(_PREHASH_GroupID, gAgent.getGroupID());
    gMessageSystem->nextBlockFast(_PREHASH_ObjectData);
    gMessageSystem->addU8Fast(_PREHASH_Material,	material);

    U32 flags = 0;		// not selected
    if (use_physics)
    {
        flags |= FLAGS_USE_PHYSICS;
    }
    if (create_selected)
    {
        flags |= FLAGS_CREATE_SELECTED;
    }
    gMessageSystem->addU32Fast(_PREHASH_AddFlags, flags );

    LLPCode volume_pcode;	// ...PCODE_VOLUME, or the original on error
    switch (pcode)
    {
    case LL_PCODE_SPHERE:
        rotation.setQuat(90.f * DEG_TO_RAD, LLVector3::y_axis);

        volume_params.setType( LL_PCODE_PROFILE_CIRCLE_HALF, LL_PCODE_PATH_CIRCLE );
        volume_params.setBeginAndEndS( 0.f, 1.f );
        volume_params.setBeginAndEndT( 0.f, 1.f );
//.........这里部分代码省略.........

//.........这里部分代码省略.........
//	llinfos << "theta        : " << theta << llendl;
//	llinfos << "bRot         : " << bRot << llendl;
//	llinfos << "theta abbcAng theta-abbcAng: " << theta*180.0/F_PI << " " << abbcAng*180.0f/F_PI << " " << (theta - abbcAng)*180.0f/F_PI << llendl;

	//-------------------------------------------------------------------------
	// compute rotation that rotates new A->C to A->G
	//-------------------------------------------------------------------------
	// rotate B->C by bRot
	bcVec = bcVec * bRot;

	// update A->C
	acVec = abVec + bcVec;

	LLQuaternion cgRot;
	cgRot.shortestArc( acVec, agVec );

//	llinfos << "bcVec : " << bcVec << llendl;
//	llinfos << "acVec : " << acVec << llendl;
//	llinfos << "cgRot : " << cgRot << llendl;

	// update A->B and B->C with rotation from C to G
	abVec = abVec * cgRot;
	bcVec = bcVec * cgRot;
	abcNorm = abcNorm * cgRot;
	acVec = abVec + bcVec;

	//-------------------------------------------------------------------------
	// compute the normal of the APG plane
	//-------------------------------------------------------------------------
	if (are_parallel(agVec, poleVec, 0.001f))
	{
		// the solution plane is undefined ==> we're done
		return;
	}
	LLVector3 apgNorm = poleVec % agVec;
	apgNorm.normVec();

	if (!mbUseBAxis)
	{
		//---------------------------------------------------------------------
		// compute the normal of the new ABC plane
		// (only necessary if we're NOT using mBAxis)
		//---------------------------------------------------------------------
		if( are_parallel(abVec, bcVec, 0.001f) )
		{
			// G is either too close or too far away
			// we'll use the old ABCnormal 
		}
		else
		{
			abcNorm = abVec % bcVec;
		}
		abcNorm.normVec();
	}

	//-------------------------------------------------------------------------
	// calcuate plane rotation
	//-------------------------------------------------------------------------
	LLQuaternion pRot;
	if ( are_parallel( abcNorm, apgNorm, 0.001f) )
	{
		if (abcNorm * apgNorm < 0.0f)
		{
			// we must be PI radians off ==> rotate by PI around agVec
			pRot.setQuat(F_PI, agVec);
		}
		else
		{
			// we're done
		}
	}
	else
	{
		pRot.shortestArc( abcNorm, apgNorm );
	}

//	llinfos << "abcNorm = " << abcNorm << llendl;
//	llinfos << "apgNorm = " << apgNorm << llendl;
//	llinfos << "pRot = " << pRot << llendl;

	//-------------------------------------------------------------------------
	// compute twist rotation
	//-------------------------------------------------------------------------
	LLQuaternion twistRot( mTwist, agVec );

//	llinfos	<< "twist    : " << mTwist*180.0/F_PI << llendl;
//	llinfos << "agNormVec: " << agNormVec << llendl;
//	llinfos << "twistRot : " << twistRot << llendl;

	//-------------------------------------------------------------------------
	// compute rotation of A
	//-------------------------------------------------------------------------
	LLQuaternion aRot = cgRot * pRot * twistRot;

	//-------------------------------------------------------------------------
	// apply the rotations
	//-------------------------------------------------------------------------
	mJointB->setWorldRotation( mJointB->getWorldRotation() * bRot );
	mJointA->setWorldRotation( mJointA->getWorldRotation() * aRot );
}

//.........这里部分代码省略.........
	gMessageSystem->addUUIDFast(_PREHASH_AgentID, gAgent.getID());
	gMessageSystem->addUUIDFast(_PREHASH_SessionID, gAgent.getSessionID());
	//MOYMOD 2009-05, If avatar is in land group/land owner group,
	//	it rezzes it with it to prevent autoreturn/whatever
	if(gSavedSettings.getBOOL("mm_alwaysRezWithLandGroup")){
		LLParcel *parcel = LLViewerParcelMgr::getInstance()->getAgentParcel();
		if(gAgent.isInGroup(parcel->getGroupID())){
			gMessageSystem->addUUIDFast(_PREHASH_GroupID, parcel->getGroupID());
		}else if(gAgent.isInGroup(parcel->getOwnerID())){
			gMessageSystem->addUUIDFast(_PREHASH_GroupID, parcel->getOwnerID());
		}else gMessageSystem->addUUIDFast(_PREHASH_GroupID, gAgent.getGroupID());
	}else gMessageSystem->addUUIDFast(_PREHASH_GroupID, gAgent.getGroupID());
	gMessageSystem->nextBlockFast(_PREHASH_ObjectData);
	gMessageSystem->addU8Fast(_PREHASH_Material,	material);

	U32 flags = 0;		// not selected
	if (use_physics || gSavedSettings.getBOOL("EmeraldBuildPrefs_Physical"))
	{
		flags |= FLAGS_USE_PHYSICS;
	}
	//if (create_selected)
// [RLVa:KB] - Checked: 2009-07-04 (RLVa-1.0.0b) | Added: RLVa-1.0.0b
	if ( (create_selected) && (!gRlvHandler.hasBehaviour(RLV_BHVR_EDIT)) )
// [/RLVa:KB]
	{
		flags |= FLAGS_CREATE_SELECTED;
	}
	gMessageSystem->addU32Fast(_PREHASH_AddFlags, flags );

	LLPCode volume_pcode;	// ...PCODE_VOLUME, or the original on error
	switch (pcode)
	{
	case LL_PCODE_SPHERE:
		rotation.setQuat(90.f * DEG_TO_RAD, LLVector3::y_axis);

		volume_params.setType( LL_PCODE_PROFILE_CIRCLE_HALF, LL_PCODE_PATH_CIRCLE );
		volume_params.setBeginAndEndS( 0.f, 1.f );
		volume_params.setBeginAndEndT( 0.f, 1.f );
		volume_params.setRatio	( 1, 1 );
		volume_params.setShear	( 0, 0 );
		LLVolumeMessage::packVolumeParams(&volume_params, gMessageSystem);
		volume_pcode = LL_PCODE_VOLUME;
		break;

	case LL_PCODE_TORUS:
		rotation.setQuat(90.f * DEG_TO_RAD, LLVector3::y_axis);

		volume_params.setType( LL_PCODE_PROFILE_CIRCLE, LL_PCODE_PATH_CIRCLE );
		volume_params.setBeginAndEndS( 0.f, 1.f );
		volume_params.setBeginAndEndT( 0.f, 1.f );
		volume_params.setRatio	( 1.f, 0.25f );	// "top size"
		volume_params.setShear	( 0, 0 );
		LLVolumeMessage::packVolumeParams(&volume_params, gMessageSystem);
		volume_pcode = LL_PCODE_VOLUME;
		break;

	case LLViewerObject::LL_VO_SQUARE_TORUS:
		rotation.setQuat(90.f * DEG_TO_RAD, LLVector3::y_axis);

		volume_params.setType( LL_PCODE_PROFILE_SQUARE, LL_PCODE_PATH_CIRCLE );
		volume_params.setBeginAndEndS( 0.f, 1.f );
		volume_params.setBeginAndEndT( 0.f, 1.f );
		volume_params.setRatio	( 1.f, 0.25f );	// "top size"
		volume_params.setShear	( 0, 0 );
		LLVolumeMessage::packVolumeParams(&volume_params, gMessageSystem);
		volume_pcode = LL_PCODE_VOLUME;