C++ LLVector3::setVec方法代码示例

LLVector3 LLSurface::resolveNormalGlobal(const LLVector3d& pos_global) const
{
    if (!mSurfaceZ)
    {
        // Hmm.  Uninitialized surface!
        return LLVector3::z_axis;
    }
    //
    // Returns the vector normal to a surface at location specified by vector v
    //
    F32 oometerspergrid = 1.f/mMetersPerGrid;
    LLVector3 normal;
    F32 dzx, dzy;

    if (pos_global.mdV[VX] >= mOriginGlobal.mdV[VX]  &&
            pos_global.mdV[VX] < mOriginGlobal.mdV[VX] + mMetersPerEdge  &&
            pos_global.mdV[VY] >= mOriginGlobal.mdV[VY]  &&
            pos_global.mdV[VY] < mOriginGlobal.mdV[VY] + mMetersPerEdge)
    {
        U32 i, j, k;
        F32 dx, dy;
        i = (U32) ((pos_global.mdV[VX] - mOriginGlobal.mdV[VX]) * oometerspergrid);
        j = (U32) ((pos_global.mdV[VY] - mOriginGlobal.mdV[VY]) * oometerspergrid );
        k = i + j*mGridsPerEdge;

        // Figure out if v is in first or second triangle of the square
        // and calculate the slopes accordingly
        //    |       |
        // -(k+N)---(k+1+N)--
        //    |  1 /  |          ^
        //    |   / 2 |          |
        //    |  /    |          j
        // --(k)----(k+1)--
        //    |       |
        //
        //      i ->
        // where N = mGridsPerEdge

        // dx and dy are incremental steps from (mSurface + k)
        dx = (F32)(pos_global.mdV[VX] - i*mMetersPerGrid - mOriginGlobal.mdV[VX]);
        dy = (F32)(pos_global.mdV[VY] - j*mMetersPerGrid - mOriginGlobal.mdV[VY]);
        if (dy > dx)
        {   // triangle 1
            dzx = *(mSurfaceZ + k + 1 + mGridsPerEdge) - *(mSurfaceZ + k + mGridsPerEdge);
            dzy = *(mSurfaceZ + k) - *(mSurfaceZ + k + mGridsPerEdge);
            normal.setVec(-dzx,dzy,1);
        }
        else
        {   // triangle 2
            dzx = *(mSurfaceZ + k) - *(mSurfaceZ + k + 1);
            dzy = *(mSurfaceZ + k + 1 + mGridsPerEdge) - *(mSurfaceZ + k + 1);
            normal.setVec(dzx,-dzy,1);
        }
    }
    normal.normVec();
    return normal;


}

void LLVOTree::updateSpatialExtents(LLVector3& newMin, LLVector3& newMax)
{
	F32 radius = getScale().magVec()*0.05f;
	LLVector3 center = getRenderPosition();

	F32 sz = mBillboardScale*mBillboardRatio*radius*0.5f; 
	LLVector3 size(sz,sz,sz);

	center += LLVector3(0, 0, size.mV[2]) * getRotation();
	
	newMin.setVec(center-size);
	newMax.setVec(center+size);
	mDrawable->setPositionGroup(center);
}

//-----------------------------------------------------------------------
void LLAudioEngine_FMOD::updateWind(LLVector3 wind_vec, F32 camera_height_above_water)
{
	LLVector3 wind_pos;
	F64 pitch;
	F64 center_freq;

	if (!mEnableWind)
	{
		return;
	}
	
	if (mWindUpdateTimer.checkExpirationAndReset(LL_WIND_UPDATE_INTERVAL))
	{
		
		// wind comes in as Linden coordinate (+X = forward, +Y = left, +Z = up)
		// need to convert this to the conventional orientation DS3D and OpenAL use
		// where +X = right, +Y = up, +Z = backwards

		wind_vec.setVec(-wind_vec.mV[1], wind_vec.mV[2], -wind_vec.mV[0]);

		// cerr << "Wind update" << endl;

		pitch = 1.0 + mapWindVecToPitch(wind_vec);
		center_freq = 80.0 * pow(pitch,2.5*(mapWindVecToGain(wind_vec)+1.0));
		
		mWindGen->mTargetFreq = (F32)center_freq;
		mWindGen->mTargetGain = (F32)mapWindVecToGain(wind_vec) * mMaxWindGain;
		mWindGen->mTargetPanGainR = (F32)mapWindVecToPan(wind_vec);
  	}
}

BOOL ray_sphere(const LLVector3 &ray_point, const LLVector3 &ray_direction,
                const LLVector3 &sphere_center, F32 sphere_radius,
                LLVector3 &intersection, LLVector3 &intersection_normal)
{
    LLVector3 ray_to_sphere = sphere_center - ray_point;
    F32 dot = ray_to_sphere * ray_direction;

    LLVector3 closest_approach = dot * ray_direction - ray_to_sphere;

    F32 shortest_distance = closest_approach.magVecSquared();
    F32 radius_squared = sphere_radius * sphere_radius;
    if (shortest_distance > radius_squared)
    {
        return FALSE;
    }

    F32 half_chord = (F32) sqrt(radius_squared - shortest_distance);
    closest_approach = sphere_center + closest_approach;			// closest_approach now in absolute coordinates
    intersection = closest_approach + half_chord * ray_direction;
    dot = ray_direction * (intersection - ray_point);
    if (dot < 0.0f)
    {
        // ray shoots away from sphere and is not inside it
        return FALSE;
    }

    shortest_distance = ray_direction * ((closest_approach - half_chord * ray_direction) - ray_point);
    if (shortest_distance > 0.0f)
    {
        // ray enters sphere
        intersection = intersection - (2.0f * half_chord) * ray_direction;
    }
    else
    {
        // do nothing
        // ray starts inside sphere and intersects as it leaves the sphere
    }

    intersection_normal = intersection - sphere_center;
    if (sphere_radius > 0.0f)
    {
        intersection_normal *= 1.0f / sphere_radius;
    }
    else
    {
        intersection_normal.setVec(0.0f, 0.0f, 0.0f);
    }

    return TRUE;
}

//-------------------------------------------------------------------------------------
BOOL LLFollowCam::updateBehindnessConstraint(LLVector3 focus, LLVector3& cam_position)
{
	BOOL constraint_active = FALSE;
	// only apply this stuff if the behindness angle is something other than opened up all the way
	if ( mBehindnessMaxAngle < FOLLOW_CAM_MAX_BEHINDNESS_ANGLE - FOLLOW_CAM_BEHINDNESS_EPSILON )
	{
		//--------------------------------------------------------------
		// horizontalized vector from focus to camera 
		//--------------------------------------------------------------
		LLVector3 horizontalVectorFromFocusToCamera;
		horizontalVectorFromFocusToCamera.setVec(cam_position - focus);
		horizontalVectorFromFocusToCamera.mV[ VZ ] = 0.0f; 
		F32 cameraZ = cam_position.mV[ VZ ];

		//--------------------------------------------------------------
		// distance of horizontalized vector
		//--------------------------------------------------------------
		F32 horizontalDistance = horizontalVectorFromFocusToCamera.magVec();

		//--------------------------------------------------------------------------------------------------
		// calculate horizontalized back vector of the subject and scale by horizontalDistance
		//--------------------------------------------------------------------------------------------------
		LLVector3 horizontalSubjectBack( -1.0f, 0.0f, 0.0f );
		horizontalSubjectBack *= mSubjectRotation;
		horizontalSubjectBack.mV[ VZ ] = 0.0f; 
		horizontalSubjectBack.normVec(); // because horizontalizing might make it shorter than 1
		horizontalSubjectBack *= horizontalDistance;

		//--------------------------------------------------------------------------------------------------
		// find the angle (in degrees) between these vectors
		//--------------------------------------------------------------------------------------------------
		F32 cameraOffsetAngle = 0.f;
		LLVector3 cameraOffsetRotationAxis;
		LLQuaternion camera_offset_rotation;
		camera_offset_rotation.shortestArc(horizontalSubjectBack, horizontalVectorFromFocusToCamera);
		camera_offset_rotation.getAngleAxis(&cameraOffsetAngle, cameraOffsetRotationAxis);
		cameraOffsetAngle *= RAD_TO_DEG;

		if ( cameraOffsetAngle > mBehindnessMaxAngle )
		{
			F32 fraction = ((cameraOffsetAngle - mBehindnessMaxAngle) / cameraOffsetAngle) * LLCriticalDamp::getInterpolant(mBehindnessLag);
			cam_position = focus + horizontalSubjectBack * (slerp(fraction, camera_offset_rotation, LLQuaternion::DEFAULT));
			cam_position.mV[VZ] = cameraZ; // clamp z value back to what it was before we started messing with it
			constraint_active = TRUE;
		}
	}
	return constraint_active;
}

void LLAudioEngine_OpenAL::updateWind(LLVector3 wind_vec, F32 camera_altitude)
{
	LLVector3 wind_pos;
	F64 pitch;
	F64 center_freq;
	ALenum error;
	
	if (!mEnableWind)
		return;
	
	if(!mWindBuf)
		return;
	
	if (mWindUpdateTimer.checkExpirationAndReset(LL_WIND_UPDATE_INTERVAL))
	{
		
		// wind comes in as Linden coordinate (+X = forward, +Y = left, +Z = up)
		// need to convert this to the conventional orientation DS3D and OpenAL use
		// where +X = right, +Y = up, +Z = backwards
		
		wind_vec.setVec(-wind_vec.mV[1], wind_vec.mV[2], -wind_vec.mV[0]);
		
		pitch = 1.0 + mapWindVecToPitch(wind_vec);
		center_freq = 80.0 * pow(pitch,2.5*(mapWindVecToGain(wind_vec)+1.0));
		
		mWindGen->mTargetFreq = (F32)center_freq;
		mWindGen->mTargetGain = (F32)mapWindVecToGain(wind_vec) * mMaxWindGain;
		mWindGen->mTargetPanGainR = (F32)mapWindVecToPan(wind_vec);
		
		alSourcei(mWindSource, AL_LOOPING, AL_FALSE);
		alSource3f(mWindSource, AL_POSITION, 0.0, 0.0, 0.0);
		alSource3f(mWindSource, AL_VELOCITY, 0.0, 0.0, 0.0);
		alSourcef(mWindSource, AL_ROLLOFF_FACTOR, 0.0);
		alSourcei(mWindSource, AL_SOURCE_RELATIVE, AL_TRUE);
	}

	// ok lets make a wind buffer now

	int processed, queued, unprocessed;
	alGetSourcei(mWindSource, AL_BUFFERS_PROCESSED, &processed);
	alGetSourcei(mWindSource, AL_BUFFERS_QUEUED, &queued);
	unprocessed = queued - processed;

	// ensure that there are always at least 3x as many filled buffers
	// queued as we managed to empty since last time.
	mNumEmptyWindALBuffers = llmin(mNumEmptyWindALBuffers + processed * 3 - unprocessed, MAX_NUM_WIND_BUFFERS-unprocessed);
	mNumEmptyWindALBuffers = llmax(mNumEmptyWindALBuffers, 0);

	//LL_INFOS("OpenAL") << "mNumEmptyWindALBuffers: " << mNumEmptyWindALBuffers	<<" (" << unprocessed << ":" << processed << ")" << LL_ENDL;

	while(processed--) // unqueue old buffers
	{
		ALuint buffer;
		int error;
		alGetError(); /* clear error */
		alSourceUnqueueBuffers(mWindSource, 1, &buffer);
		error = alGetError();
		if(error != AL_NO_ERROR)
		{
			LL_WARNS("OpenAL") << "LLAudioEngine_OpenAL::updateWind() error swapping (unqueuing) buffers" << LL_ENDL;
		}
		else
		{
			alDeleteBuffers(1, &buffer);
		}
	}

	unprocessed += mNumEmptyWindALBuffers;
	while (mNumEmptyWindALBuffers > 0) // fill+queue new buffers
	{
		ALuint buffer;
		alGetError(); /* clear error */
		alGenBuffers(1,&buffer);
		if((error=alGetError()) != AL_NO_ERROR)
		{
			LL_WARNS("OpenAL") << "LLAudioEngine_OpenAL::initWind() Error creating wind buffer: " << error << LL_ENDL;
			break;
		}

		alBufferData(buffer,
			     AL_FORMAT_STEREO16,
			     mWindGen->windGenerate(mWindBuf,
						    mWindBufSamples, 2),
			     mWindBufBytes,
			     mWindBufFreq);
		error = alGetError();
		if(error != AL_NO_ERROR)
			LL_WARNS("OpenAL") << "LLAudioEngine_OpenAL::updateWind() error swapping (bufferdata) buffers" << LL_ENDL;
		
		alSourceQueueBuffers(mWindSource, 1, &buffer);
		error = alGetError();
		if(error != AL_NO_ERROR)
			LL_WARNS("OpenAL") << "LLAudioEngine_OpenAL::updateWind() error swapping (queuing) buffers" << LL_ENDL;

		--mNumEmptyWindALBuffers;
	}

	int playing;
	alGetSourcei(mWindSource, AL_SOURCE_STATE, &playing);
	if(playing != AL_PLAYING)
//.........这里部分代码省略.........

//-----------------------------------------------------------------------------
// LLEyeMotion::onUpdate()
//-----------------------------------------------------------------------------
BOOL LLEyeMotion::onUpdate(F32 time, U8* joint_mask)
{
	// Compute eye rotation.
	LLQuaternion	target_eye_rot;
	LLVector3		eye_look_at;
	F32				vergence;

	//calculate jitter
	if (mEyeJitterTimer.getElapsedTimeF32() > mEyeJitterTime)
	{
		mEyeJitterTime = EYE_JITTER_MIN_TIME + ll_frand(EYE_JITTER_MAX_TIME - EYE_JITTER_MIN_TIME);
		mEyeJitterYaw = (ll_frand(2.f) - 1.f) * EYE_JITTER_MAX_YAW;
		mEyeJitterPitch = (ll_frand(2.f) - 1.f) * EYE_JITTER_MAX_PITCH;
		// make sure lookaway time count gets updated, because we're resetting the timer
		mEyeLookAwayTime -= llmax(0.f, mEyeJitterTimer.getElapsedTimeF32());
		mEyeJitterTimer.reset();
	} 
	else if (mEyeJitterTimer.getElapsedTimeF32() > mEyeLookAwayTime)
	{
		if (ll_frand() > 0.1f)
		{
			// blink while moving eyes some percentage of the time
			mEyeBlinkTime = mEyeBlinkTimer.getElapsedTimeF32();
		}
		if (mEyeLookAwayYaw == 0.f && mEyeLookAwayPitch == 0.f)
		{
			mEyeLookAwayYaw = (ll_frand(2.f) - 1.f) * EYE_LOOK_AWAY_MAX_YAW;
			mEyeLookAwayPitch = (ll_frand(2.f) - 1.f) * EYE_LOOK_AWAY_MAX_PITCH;
			mEyeLookAwayTime = EYE_LOOK_BACK_MIN_TIME + ll_frand(EYE_LOOK_BACK_MAX_TIME - EYE_LOOK_BACK_MIN_TIME);
		}
		else
		{
			mEyeLookAwayYaw = 0.f;
			mEyeLookAwayPitch = 0.f;
			mEyeLookAwayTime = EYE_LOOK_AWAY_MIN_TIME + ll_frand(EYE_LOOK_AWAY_MAX_TIME - EYE_LOOK_AWAY_MIN_TIME);
		}
	}

	// do blinking
	if (!mEyesClosed && mEyeBlinkTimer.getElapsedTimeF32() >= mEyeBlinkTime)
	{
		F32 leftEyeBlinkMorph = mEyeBlinkTimer.getElapsedTimeF32() - mEyeBlinkTime;
		F32 rightEyeBlinkMorph = leftEyeBlinkMorph - EYE_BLINK_TIME_DELTA;

		leftEyeBlinkMorph = llclamp(leftEyeBlinkMorph / EYE_BLINK_SPEED, 0.f, 1.f);
		rightEyeBlinkMorph = llclamp(rightEyeBlinkMorph / EYE_BLINK_SPEED, 0.f, 1.f);
		mCharacter->setVisualParamWeight("Blink_Left", leftEyeBlinkMorph);
		mCharacter->setVisualParamWeight("Blink_Right", rightEyeBlinkMorph);
		mCharacter->updateVisualParams();

		if (rightEyeBlinkMorph == 1.f)
		{
			mEyesClosed = TRUE;
			mEyeBlinkTime = EYE_BLINK_CLOSE_TIME;
			mEyeBlinkTimer.reset();
		}
	}
	else if (mEyesClosed)
	{
		if (mEyeBlinkTimer.getElapsedTimeF32() >= mEyeBlinkTime)
		{
			F32 leftEyeBlinkMorph = mEyeBlinkTimer.getElapsedTimeF32() - mEyeBlinkTime;
			F32 rightEyeBlinkMorph = leftEyeBlinkMorph - EYE_BLINK_TIME_DELTA;

			leftEyeBlinkMorph = 1.f - llclamp(leftEyeBlinkMorph / EYE_BLINK_SPEED, 0.f, 1.f);
			rightEyeBlinkMorph = 1.f - llclamp(rightEyeBlinkMorph / EYE_BLINK_SPEED, 0.f, 1.f);
			mCharacter->setVisualParamWeight("Blink_Left", leftEyeBlinkMorph);
			mCharacter->setVisualParamWeight("Blink_Right", rightEyeBlinkMorph);
			mCharacter->updateVisualParams();

			if (rightEyeBlinkMorph == 0.f)
			{
				mEyesClosed = FALSE;
				mEyeBlinkTime = EYE_BLINK_MIN_TIME + ll_frand(EYE_BLINK_MAX_TIME - EYE_BLINK_MIN_TIME);
				mEyeBlinkTimer.reset();
			}
		}
	}

	BOOL has_eye_target = FALSE;
	LLVector3* targetPos = (LLVector3*)mCharacter->getAnimationData("LookAtPoint");

	if (targetPos)
	{
		LLVector3		skyward(0.f, 0.f, 1.f);
		LLVector3		left;
		LLVector3		up;

		eye_look_at = *targetPos;
		has_eye_target = TRUE;
		F32 lookAtDistance = eye_look_at.normVec();

		left.setVec(skyward % eye_look_at);
		up.setVec(eye_look_at % left);

		target_eye_rot = LLQuaternion(eye_look_at, left, up);
		// convert target rotation to head-local coordinates
//.........这里部分代码省略.........

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

// Takes a line defined by "point_a" and "point_b" and determines the closest (to point_a) 
// point where the the line intersects an object or the land surface.  Stores the results
// in "intersection" and "intersection_normal" and returns a scalar value that represents
// the normalized distance along the line from "point_a" to "intersection".
//
// Currently assumes point_a and point_b only differ in z-direction, 
// but it may eventually become more general.
F32 LLWorld::resolveStepHeightGlobal(const LLVOAvatar* avatarp, const LLVector3d &point_a, const LLVector3d &point_b, 
							   LLVector3d &intersection, LLVector3 &intersection_normal,
							   LLViewerObject **viewerObjectPtr)
{
	// initialize return value to null
	if (viewerObjectPtr)
	{
		*viewerObjectPtr = NULL;
	}

	LLViewerRegion *regionp = getRegionFromPosGlobal(point_a);
	if (!regionp)
	{
		// We're outside the world 
		intersection = 0.5f * (point_a + point_b);
		intersection_normal.setVec(0.0f, 0.0f, 1.0f);
		return 0.5f;
	}
	
	// calculate the length of the segment
	F32 segment_length = (F32)((point_a - point_b).length());
	if (0.0f == segment_length)
	{
		intersection = point_a;
		intersection_normal.setVec(0.0f, 0.0f, 1.0f);
		return segment_length;
	}

	// get land height	
	// Note: we assume that the line is parallel to z-axis here
	LLVector3d land_intersection = point_a;
	F32 normalized_land_distance;

	land_intersection.mdV[VZ] = regionp->getLand().resolveHeightGlobal(point_a);
	normalized_land_distance = (F32)(point_a.mdV[VZ] - land_intersection.mdV[VZ]) / segment_length;
	intersection = land_intersection;
	intersection_normal = resolveLandNormalGlobal(land_intersection);

	if (avatarp && !avatarp->mFootPlane.isExactlyClear())
	{
		LLVector3 foot_plane_normal(avatarp->mFootPlane.mV);
		LLVector3 start_pt = avatarp->getRegion()->getPosRegionFromGlobal(point_a);
		// added 0.05 meters to compensate for error in foot plane reported by Havok
		F32 norm_dist_from_plane = ((start_pt * foot_plane_normal) - avatarp->mFootPlane.mV[VW]) + 0.05f;
		norm_dist_from_plane = llclamp(norm_dist_from_plane / segment_length, 0.f, 1.f);
		if (norm_dist_from_plane < normalized_land_distance)
		{
			// collided with object before land
			normalized_land_distance = norm_dist_from_plane;
			intersection = point_a;
			intersection.mdV[VZ] -= norm_dist_from_plane * segment_length;
			intersection_normal = foot_plane_normal;
		}
		else
		{
			intersection = land_intersection;
			intersection_normal = resolveLandNormalGlobal(land_intersection);
		}
	}

	return normalized_land_distance;
}

//---------------------------------------------------------------------------------------------------------
void LLFollowCam::update()
{
	//####################################################################################
	// update Focus
	//####################################################################################
	LLVector3 offsetSubjectPosition = mSubjectPosition + (mFocusOffset * mSubjectRotation);

	LLVector3 simulated_pos_agent = gAgent.getPosAgentFromGlobal(mSimulatedPositionGlobal);
	LLVector3 vectorFromCameraToSubject = offsetSubjectPosition - simulated_pos_agent;
	F32 distanceFromCameraToSubject = vectorFromCameraToSubject.magVec();

	LLVector3 whereFocusWantsToBe = mFocus;
	LLVector3 focus_pt_agent = gAgent.getPosAgentFromGlobal(mSimulatedFocusGlobal);
	if ( mFocusLocked ) // if focus is locked, only relative focus needs to be updated
	{
		mRelativeFocus = (focus_pt_agent - mSubjectPosition) * ~mSubjectRotation;
	}
	else
	{
		LLVector3 focusOffset = offsetSubjectPosition - focus_pt_agent;
		F32 focusOffsetDistance = focusOffset.magVec();

		LLVector3 focusOffsetDirection = focusOffset / focusOffsetDistance;
		whereFocusWantsToBe = focus_pt_agent + 
			(focusOffsetDirection * (focusOffsetDistance - mFocusThreshold));
		if ( focusOffsetDistance > mFocusThreshold )
		{
			// this version normalizes focus threshold by distance 
			// so that the effect is not changed with distance
			/*
			F32 focusThresholdNormalizedByDistance = distanceFromCameraToSubject * mFocusThreshold;
			if ( focusOffsetDistance > focusThresholdNormalizedByDistance )
			{
				LLVector3 focusOffsetDirection = focusOffset / focusOffsetDistance;
				F32 force = focusOffsetDistance - focusThresholdNormalizedByDistance;
			*/

			F32 focusLagLerp = LLCriticalDamp::getInterpolant( mFocusLag );
			focus_pt_agent = lerp( focus_pt_agent, whereFocusWantsToBe, focusLagLerp );
			mSimulatedFocusGlobal = gAgent.getPosGlobalFromAgent(focus_pt_agent);
		}
		mRelativeFocus = lerp(mRelativeFocus, (focus_pt_agent - mSubjectPosition) * ~mSubjectRotation, LLCriticalDamp::getInterpolant(0.05f));
	}// if focus is not locked ---------------------------------------------


	LLVector3 whereCameraPositionWantsToBe = gAgent.getPosAgentFromGlobal(mSimulatedPositionGlobal);
	if (  mPositionLocked )
	{
		mRelativePos = (whereCameraPositionWantsToBe - mSubjectPosition) * ~mSubjectRotation;
	}
	else
	{
		//####################################################################################
		// update Position
		//####################################################################################
		//-------------------------------------------------------------------------
		// I determine the horizontal vector from the camera to the subject
		//-------------------------------------------------------------------------
		LLVector3 horizontalVectorFromCameraToSubject = vectorFromCameraToSubject;
		horizontalVectorFromCameraToSubject.mV[VZ] = 0.0f;

		//---------------------------------------------------------
		// Now I determine the horizontal distance
		//---------------------------------------------------------
		F32 horizontalDistanceFromCameraToSubject = horizontalVectorFromCameraToSubject.magVec();  

		//---------------------------------------------------------
		// Then I get the (normalized) horizontal direction...
		//---------------------------------------------------------
		LLVector3 horizontalDirectionFromCameraToSubject;	
		if ( horizontalDistanceFromCameraToSubject < DISTANCE_EPSILON )
		{
			// make sure we still have a normalized vector if distance is really small 
			// (this case is rare and fleeting)
			horizontalDirectionFromCameraToSubject = LLVector3::z_axis;
		}
		else
		{
			// I'm not using the "normalize" method, because I can just divide by horizontalDistanceFromCameraToSubject
			horizontalDirectionFromCameraToSubject = horizontalVectorFromCameraToSubject / horizontalDistanceFromCameraToSubject;
		}

		//------------------------------------------------------------------------------------------------------------
		// Here is where I determine an offset relative to subject position in oder to set the ideal position. 
		//------------------------------------------------------------------------------------------------------------
		if ( mPitchSineAndCosineNeedToBeUpdated )
		{
			calculatePitchSineAndCosine();
			mPitchSineAndCosineNeedToBeUpdated = false;
		}

		LLVector3 positionOffsetFromSubject;
		positionOffsetFromSubject.setVec
			( 
				horizontalDirectionFromCameraToSubject.mV[ VX ] * mPitchCos,
				horizontalDirectionFromCameraToSubject.mV[ VY ] * mPitchCos,
				-mPitchSin
			);

//.........这里部分代码省略.........

//-----------------------------------------------------------------------------
// LLWalkAdjustMotion::onUpdate()
//-----------------------------------------------------------------------------
BOOL LLWalkAdjustMotion::onUpdate(F32 time, U8* joint_mask)
{
	LLVector3 footCorrection;
	LLVector3 vel = mCharacter->getCharacterVelocity() * mCharacter->getTimeDilation();
	F32 deltaTime = llclamp(time - mLastTime, 0.f, MAX_TIME_DELTA);
	mLastTime = time;

	LLQuaternion inv_rotation = ~mPelvisJoint->getWorldRotation();

	// get speed and normalize velocity vector
	LLVector3 ang_vel = mCharacter->getCharacterAngularVelocity() * mCharacter->getTimeDilation();
	F32 speed = llmin(vel.normVec(), MAX_WALK_PLAYBACK_SPEED);
	mAvgSpeed = lerp(mAvgSpeed, speed, LLCriticalDamp::getInterpolant(0.2f));

	// calculate facing vector in pelvis-local space 
	// (either straight forward or back, depending on velocity)
	LLVector3 localVel = vel * inv_rotation;
	if (localVel.mV[VX] > 0.f)
	{
		mRelativeDir = 1.f;
	}
	else if (localVel.mV[VX] < 0.f)
	{
		mRelativeDir = -1.f;
	}

	// calculate world-space foot drift
	LLVector3 leftFootDelta;
	LLVector3 leftFootWorldPosition = mLeftAnkleJoint->getWorldPosition();
	LLVector3d leftFootGlobalPosition = mCharacter->getPosGlobalFromAgent(leftFootWorldPosition);
	leftFootDelta.setVec(mLastLeftAnklePos - leftFootGlobalPosition);
	mLastLeftAnklePos = leftFootGlobalPosition;

	LLVector3 rightFootDelta;
	LLVector3 rightFootWorldPosition = mRightAnkleJoint->getWorldPosition();
	LLVector3d rightFootGlobalPosition = mCharacter->getPosGlobalFromAgent(rightFootWorldPosition);
	rightFootDelta.setVec(mLastRightAnklePos - rightFootGlobalPosition);
	mLastRightAnklePos = rightFootGlobalPosition;

	// find foot drift along velocity vector
	if (mAvgSpeed > 0.1)
	{
		// walking/running
		F32 leftFootDriftAmt = leftFootDelta * vel;
		F32 rightFootDriftAmt = rightFootDelta * vel;

		if (rightFootDriftAmt > leftFootDriftAmt)
		{
			footCorrection = rightFootDelta;
		} else
		{
			footCorrection = leftFootDelta;
		}
	}
	else
	{
		mAvgSpeed = ang_vel.magVec() * mAnkleOffset;
		mRelativeDir = 1.f;

		// standing/turning
		// find the lower foot
		if (leftFootWorldPosition.mV[VZ] < rightFootWorldPosition.mV[VZ])
		{
			// pivot on left foot
			footCorrection = leftFootDelta;
		}
		else
		{
			// pivot on right foot
			footCorrection = rightFootDelta;
		}
	}

	// rotate into avatar coordinates
	footCorrection = footCorrection * inv_rotation;

	// calculate ideal pelvis offset so that foot is glued to ground and damp towards it
	// the amount of foot slippage this frame + the offset applied last frame
	mPelvisOffset = mPelvisState->getPosition() + lerp(LLVector3::zero, footCorrection, LLCriticalDamp::getInterpolant(0.2f));

	// pelvis drift (along walk direction)
	mAvgCorrection = lerp(mAvgCorrection, footCorrection.mV[VX] * mRelativeDir, LLCriticalDamp::getInterpolant(0.1f));

	// calculate average velocity of foot slippage
	F32 footSlipVelocity = (deltaTime != 0.f) ? (-mAvgCorrection / deltaTime) : 0.f;

	F32 newSpeedAdjust = 0.f;
	
	// modulate speed by dot products of facing and velocity
	// so that if we are moving sideways, we slow down the animation
	// and if we're moving backward, we walk backward

	F32 directional_factor = localVel.mV[VX] * mRelativeDir;
	if (speed > 0.1f)
	{
		// calculate ratio of desired foot velocity to detected foot velocity
		newSpeedAdjust = llclamp(footSlipVelocity - mAvgSpeed * (1.f - directional_factor), 
//.........这里部分代码省略.........

//.........这里部分代码省略.........
        temp_vector = ray_to_cyl % cyl_axis;
        dist_to_closest_point = - (temp_vector * shortest_direction);
        temp_vector = shortest_direction % cyl_axis;
        temp_vector.normVec();
        half_chord_length = (F32) fabs( sqrt(cyl_radius*cyl_radius - shortest_distance * shortest_distance) /
                                        (ray_direction * temp_vector) );

        out = dist_to_closest_point + half_chord_length;	// dist to exiting point
        if (out < 0.0f)
        {
            // cylinder is behind the ray, so we return FALSE
            return FALSE;
        }

        in = dist_to_closest_point - half_chord_length;		// dist to entering point
        if (in < 0.0f)
        {
            // ray_point is inside the cylinder
            // so we store the exiting intersection
            intersection = ray_point + out * ray_direction;
            shortest_distance = out;
        }
        else
        {
            // ray hit cylinder from outside
            // so we store the entering intersection
            intersection = ray_point + in * ray_direction;
            shortest_distance = in;
        }

        // calculate the normal at intersection
        if (0.0f == cyl_radius)
        {
            intersection_normal.setVec(0.0f, 0.0f, 0.0f);
        }
        else
        {
            temp_vector = intersection - cyl_bottom;
            intersection_normal = temp_vector - (temp_vector * cyl_axis) * cyl_axis;
            intersection_normal.normVec();
        }

        // check for intersection with end caps
        // calculate intersection of ray and top plane
        if (line_plane(ray_point, ray_direction, cyl_top, -cyl_axis, temp_vector))	// NOTE side-effect: changing temp_vector
        {
            shortest_distance = (temp_vector - ray_point).magVec();
            if ( (ray_direction * cyl_axis) > 0.0f)
            {
                // ray potentially enters the cylinder at top
                if (shortest_distance > out)
                {
                    // ray missed the finite cylinder
                    return FALSE;
                }
                if (shortest_distance > in)
                {
                    // ray intersects cylinder at top plane
                    intersection = temp_vector;
                    intersection_normal = -cyl_axis;
                    return TRUE;
                }
            }
            else
            {
                // ray potentially exits the cylinder at top

//.........这里部分代码省略.........
			{
				z = - cap_nudge;
				r0 = 0.0;
			}
			else if (i == (slices - 1))
			{
				z = 1.f + cap_nudge;//((i - 2) * z_inc) + cap_nudge;
				r0 = 0.0;
			}
			else  
			{
				z = (i - 1) * z_inc;
				r0 = base_radius + (top_radius - base_radius)*z;
			}

			for (j = 0; j < slices; j++)
			{
				if (slices - 1 == j)
				{
					angle = 0.f;
				}
				else
				{
					angle =  j*angle_inc;
				}
			
				nangle = angle;
				
				x1 = cos(angle * DEG_TO_RAD);
				y1 = sin(angle * DEG_TO_RAD);
				LLVector2 tc;
				// This isn't totally accurate.  Should compute based on slope as well.
				start_radius = r0 * (1.f + 1.2f*fabs(z - 0.66f*height)/height);
				nvec.setVec(	cos(nangle * DEG_TO_RAD)*start_radius*nvec_scale, 
								sin(nangle * DEG_TO_RAD)*start_radius*nvec_scale, 
								z*nvec_scalez); 
				// First and last slice at 0 radius (to bring in top/bottom of structure)
				radius = start_radius + turbulence3((F32*)&nvec.mV, (F32)fractal_depth)*noise_scale;

				if (slices - 1 == j)
				{
					// Not 0.5 for slight slop factor to avoid edges on leaves
					tc = LLVector2(0.490f, (1.f - z/2.f)*tex_z_repeat);
				}
				else
				{
					tc = LLVector2((angle/360.f)*0.5f, (1.f - z/2.f)*tex_z_repeat);
				}

				*(vertices++) =		LLVector3(x1*radius, y1*radius, z);
				*(normals++) =		LLVector3(x1, y1, 0.f);
				*(tex_coords++) = tc;
				vertex_count++;
			}
		}

		for (i = 0; i < (slices - 1); i++)
		{
			for (j = 0; j < (slices - 1); j++)
			{
				S32 x1_offset = j+1;
				if ((j+1) == slices)
				{
					x1_offset = 0;
				}
				// Generate the matching quads

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 = LLVector3(
		gSavedSettings.getF32("EmeraldBuildPrefs_Xsize"),
		gSavedSettings.getF32("EmeraldBuildPrefs_Ysize"),
		gSavedSettings.getF32("EmeraldBuildPrefs_Zsize"));
	
	U8				material = LL_MCODE_WOOD;
	if(gSavedSettings.getString("EmeraldBuildPrefs_Material")== "Stone") material = LL_MCODE_STONE;
	if(gSavedSettings.getString("EmeraldBuildPrefs_Material")== "Metal") material = LL_MCODE_METAL;
	if(gSavedSettings.getString("EmeraldBuildPrefs_Material")== "Wood") material = LL_MCODE_WOOD;
	if(gSavedSettings.getString("EmeraldBuildPrefs_Material")== "Flesh") material = LL_MCODE_FLESH;
	if(gSavedSettings.getString("EmeraldBuildPrefs_Material")== "Rubber") material = LL_MCODE_RUBBER;
	if(gSavedSettings.getString("EmeraldBuildPrefs_Material")== "Plastic") material = LL_MCODE_PLASTIC;
		

	

	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)
	{
		gAudiop->triggerSound( LLUUID(gSavedSettings.getString("UISndObjectCreate")),
							   gAgent.getID(), 1.0f, LLAudioEngine::AUDIO_TYPE_UI);
	}

	gMessageSystem->newMessageFast(_PREHASH_ObjectAdd);
	gMessageSystem->nextBlockFast(_PREHASH_AgentData);
	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());
//.........这里部分代码省略.........

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 );
//.........这里部分代码省略.........