C++ Quat类代码示例

void Quat2Matrix(const Quat& q, Matrix& m)
{
	double length2 = q.length2();
	if (fabs(length2) <= std::numeric_limits<double>::min()) {
		m._mat[0][0] = 0.0;
		m._mat[1][0] = 0.0;
		m._mat[2][0] = 0.0;

		m._mat[0][1] = 0.0;
		m._mat[1][1] = 0.0;
		m._mat[2][1] = 0.0;

		m._mat[0][2] = 0.0;
		m._mat[1][2] = 0.0;
		m._mat[2][2] = 0.0;
	} else {
		double rlength2;
		// normalize quat if required.
		// We can avoid the expensive sqrt in this case since all 'coefficients' below are products of two q components.
		// That is a square of a square root, so it is possible to avoid that
		if (length2 != 1.0) {
			rlength2 = 2.0/length2;
		} else {
			rlength2 = 2.0;
		}

		// Source: Gamasutra, Rotating Objects Using Quaternions
		//
		//http://www.gamasutra.com/features/19980703/quaternions_01.htm

		double wx, wy, wz, xx, yy, yz, xy, xz, zz, x2, y2, z2;

		// calculate coefficients
		x2 = rlength2 * QX;
		y2 = rlength2 * QY;
		z2 = rlength2 * QZ;

		xx = QX * x2;
		xy = QX * y2;
		xz = QX * z2;

		yy = QY * y2;
		yz = QY * z2;
		zz = QZ * z2;

		wx = QW * x2;
		wy = QW * y2;
		wz = QW * z2;

		// Note. Gamasutra gets the matrix assignments inverted, resulting
		// in left-handed rotations, which is contrary to OpenGL and OSG's
		// methodology. The matrix assignment has been altered in the next
		// few lines of code to do the right thing.
		// Don Burns - Oct 13, 2001
		m._mat[0][0] = 1.0 - (yy + zz);
		m._mat[1][0] = xy - wz;
		m._mat[2][0] = xz + wy;


		m._mat[0][1] = xy + wz;
		m._mat[1][1] = 1.0 - (xx + zz);
		m._mat[2][1] = yz - wx;

		m._mat[0][2] = xz - wy;
		m._mat[1][2] = yz + wx;
		m._mat[2][2] = 1.0 - (xx + yy);
	}
}

Quat Quat::operator*( const Quat& other ) const
{
    return Quat(
               W() * other.W() - X() * other.X() - Y() * other.Y() - Z() * other.Z(),
               W() * other.X() + X() * other.W() + Y() * other.Z() - Z() * other.Y(),
               W() * other.Y() + Y() * other.W() + Z() * other.X() - X() * other.Z(),
               W() * other.Z() + Z() * other.W() + X() * other.Y() - Y() * other.X()
           );
}

INode* CollisionImport::CreateRigidBody(bhkRigidBodyRef body, INode *parent, Matrix3& tm)
{
	INode *rbody = NULL;
	if (body == NULL)
		return rbody;

	OblivionLayer lyr = body->GetLayer();
	//body->GetLayerCopy(lyr);
	MotionSystem msys = body->GetMotionSystem();
	MotionQuality qtype = body->GetQualityType();
	float mass = body->GetMass();
	float lindamp = body->GetLinearDamping();
	float angdamp = body->GetAngularDamping();
	float frict = body->GetFriction();
	float resti = body->GetRestitution();
	float maxlinvel = body->GetMaxLinearVelocity();
	float maxangvel = body->GetMaxAngularVelocity();
	float pendepth = body->GetPenetrationDepth();
	Vector4 center = body->GetCenter();

	SimpleObject2* listObj = (SimpleObject2*)ni.gi->CreateInstance(HELPER_CLASS_ID, BHKLISTOBJECT_CLASS_ID);
	if (listObj != NULL) 
	{
		bool isTransform = false;
		if (bhkRigidBodyInterface *irb = (bhkRigidBodyInterface *)listObj->GetInterface(BHKRIGIDBODYINTERFACE_DESC))
		{
			irb->SetLayer(lyr, 0);
			//irb->SetLayerCopy(lyr, 0);
			irb->SetMotionSystem(msys, 0);
			irb->SetQualityType(qtype, 0);
			irb->SetMass(mass, 0);
			irb->SetLinearDamping(lindamp, 0);
			irb->SetAngularDamping(angdamp, 0);
			irb->SetFriction(frict, 0);
			irb->SetRestitution(resti, 0);
			irb->SetMaxLinearVelocity(maxlinvel, 0);
			irb->SetMaxAngularVelocity(maxangvel, 0);
			irb->SetPenetrationDepth(pendepth, 0);
			//irb->SetCenter(center);

			isTransform = (body->IsDerivedType(bhkRigidBodyT::TYPE));
			irb->SetEnableTransform(isTransform ? TRUE : FALSE, 0);
		}
		TSTR clsName;
		listObj->GetClassName(clsName);
		if (INode *n = ni.CreateImportNode(clsName, listObj, parent)) {
			Point3 pos = TOPOINT3(body->GetTranslation()* ni.bhkScaleFactor);
			Quat q = TOQUAT(body->GetRotation(), true);
			PosRotScaleNode(n, pos, q, 1.0f, prsDefault);
			rbody = n;
			if (isTransform) {
				Matrix3 qm(true);
				q.MakeMatrix(qm);
				qm.Translate(pos);
				tm *= qm;
				//tm *= TransMatrix(pos);
			}
		}
	}

	//npSetCollision(node, true);
	return rbody;
}

//------------------------------------------------------------------------
void CMelee::Update(float frameTime, uint32 frameId)
{
	FUNCTION_PROFILER( GetISystem(), PROFILE_GAME );

	bool remote = false;
	bool doMelee = false;
	bool requireUpdate = false;

	Vec3 dirToTarget(ZERO);
	if(m_pMeleeAction && s_meleeSnapTargetId && g_pGameCVars->pl_melee.mp_melee_system_camera_lock_and_turn)
	{
		m_attackTime += frameTime;

		CActor* pOwner = m_pWeapon->GetOwnerActor();
		IEntity* pTarget = gEnv->pEntitySystem->GetEntity(s_meleeSnapTargetId);
		if(pOwner && pTarget)
		{
			Vec3 targetPos = pTarget->GetWorldPos();

			if(s_bMeleeSnapTargetCrouched)
			{
				targetPos.z -= g_pGameCVars->pl_melee.mp_melee_system_camera_lock_crouch_height_offset;
			}

			dirToTarget = targetPos - pOwner->GetEntity()->GetWorldPos();
			dirToTarget.Normalize();

			static const float smooth_time = 0.1f;
			SmoothCD(m_attackTurnAmount, m_attackTurnAmountSmoothRate, frameTime, 1.f, smooth_time);
			Quat newViewRotation;
			newViewRotation.SetSlerp(pOwner->GetViewRotation(), Quat::CreateRotationVDir(dirToTarget), m_attackTurnAmount);
			Ang3 newAngles(newViewRotation);
			newAngles.y = 0.f; //No head tilting
			newViewRotation = Quat(newAngles);
			pOwner->SetViewRotation( newViewRotation );
		}

		if(m_attackTime >= g_pGameCVars->pl_melee.mp_melee_system_camera_lock_time && pOwner && pOwner->IsClient())
		{
			pOwner->GetActorParams().viewLimits.ClearViewLimit(SViewLimitParams::eVLS_Item);
		}
	}

	if (m_attacking)
	{
		MeleeDebugLog ("CMelee<%p> Update while attacking: m_attacked=%d, delay=%f", this, m_attacked, m_delayTimer);

		requireUpdate = true;
		if (m_delayTimer>0.0f)
		{
			RequestAlignmentToNearestTarget();
			m_delayTimer-=frameTime;
			if (m_delayTimer<=0.0f)
			{
				m_delayTimer=0.0f;
			}
		}
		else if (m_netAttacking)
		{
			remote = true;
			doMelee = true;
			m_attacking = false;
			m_slideKick = false;
			m_netAttacking = false;

			m_pWeapon->SetBusy(false);
		}	
		else if (!m_attacked)
		{
			doMelee = true;
			m_attacked = true;
		}

		if ( !m_collisionHelper.IsBlocked() && doMelee)
		{
			if (CActor *pActor = m_pWeapon->GetOwnerActor())
			{
				if (IMovementController * pMC = pActor->GetMovementController())
				{
					SMovementState info;
					pMC->GetMovementState(info);

					if(!dirToTarget.IsZeroFast())
					{
						PerformMelee(info.weaponPosition, dirToTarget, remote); //We know where we will be facing at the point of impact - using our current fire direction is not accurate enough
					}
					else
					{
						PerformMelee(info.weaponPosition, info.fireDirection, remote);
					}
				}
			}
		}

		if( (m_useMeleeWeaponDelay <= 0.0f && m_useMeleeWeaponDelay > -1.0f) )
		{
			m_useMeleeWeaponDelay = -1.0f;

			// Switch to the MELEE WEAPON.
//.........这里部分代码省略.........

void Ray::Transform(const Quat &transform)
{
	pos = transform.Transform(pos);
	dir = transform.Transform(dir);
}

bool ConeTwistJointSW::setup(real_t p_timestep) {
	m_appliedImpulse = real_t(0.);

	//set bias, sign, clear accumulator
	m_swingCorrection = real_t(0.);
	m_twistLimitSign = real_t(0.);
	m_solveTwistLimit = false;
	m_solveSwingLimit = false;
	m_accTwistLimitImpulse = real_t(0.);
	m_accSwingLimitImpulse = real_t(0.);

	if (!m_angularOnly) {
		Vector3 pivotAInW = A->get_transform().xform(m_rbAFrame.origin);
		Vector3 pivotBInW = B->get_transform().xform(m_rbBFrame.origin);
		Vector3 relPos = pivotBInW - pivotAInW;

		Vector3 normal[3];
		if (relPos.length_squared() > CMP_EPSILON) {
			normal[0] = relPos.normalized();
		} else {
			normal[0] = Vector3(real_t(1.0), 0, 0);
		}

		plane_space(normal[0], normal[1], normal[2]);

		for (int i = 0; i < 3; i++) {
			memnew_placement(&m_jac[i], JacobianEntrySW(
												A->get_principal_inertia_axes().transposed(),
												B->get_principal_inertia_axes().transposed(),
												pivotAInW - A->get_transform().origin - A->get_center_of_mass(),
												pivotBInW - B->get_transform().origin - B->get_center_of_mass(),
												normal[i],
												A->get_inv_inertia(),
												A->get_inv_mass(),
												B->get_inv_inertia(),
												B->get_inv_mass()));
		}
	}

	Vector3 b1Axis1, b1Axis2, b1Axis3;
	Vector3 b2Axis1, b2Axis2;

	b1Axis1 = A->get_transform().basis.xform(this->m_rbAFrame.basis.get_axis(0));
	b2Axis1 = B->get_transform().basis.xform(this->m_rbBFrame.basis.get_axis(0));

	real_t swing1 = real_t(0.), swing2 = real_t(0.);

	real_t swx = real_t(0.), swy = real_t(0.);
	real_t thresh = real_t(10.);
	real_t fact;

	// Get Frame into world space
	if (m_swingSpan1 >= real_t(0.05f)) {
		b1Axis2 = A->get_transform().basis.xform(this->m_rbAFrame.basis.get_axis(1));
		//swing1  = btAtan2Fast( b2Axis1.dot(b1Axis2),b2Axis1.dot(b1Axis1) );
		swx = b2Axis1.dot(b1Axis1);
		swy = b2Axis1.dot(b1Axis2);
		swing1 = atan2fast(swy, swx);
		fact = (swy * swy + swx * swx) * thresh * thresh;
		fact = fact / (fact + real_t(1.0));
		swing1 *= fact;
	}

	if (m_swingSpan2 >= real_t(0.05f)) {
		b1Axis3 = A->get_transform().basis.xform(this->m_rbAFrame.basis.get_axis(2));
		//swing2 = btAtan2Fast( b2Axis1.dot(b1Axis3),b2Axis1.dot(b1Axis1) );
		swx = b2Axis1.dot(b1Axis1);
		swy = b2Axis1.dot(b1Axis3);
		swing2 = atan2fast(swy, swx);
		fact = (swy * swy + swx * swx) * thresh * thresh;
		fact = fact / (fact + real_t(1.0));
		swing2 *= fact;
	}

	real_t RMaxAngle1Sq = 1.0f / (m_swingSpan1 * m_swingSpan1);
	real_t RMaxAngle2Sq = 1.0f / (m_swingSpan2 * m_swingSpan2);
	real_t EllipseAngle = Math::abs(swing1 * swing1) * RMaxAngle1Sq + Math::abs(swing2 * swing2) * RMaxAngle2Sq;

	if (EllipseAngle > 1.0f) {
		m_swingCorrection = EllipseAngle - 1.0f;
		m_solveSwingLimit = true;

		// Calculate necessary axis & factors
		m_swingAxis = b2Axis1.cross(b1Axis2 * b2Axis1.dot(b1Axis2) + b1Axis3 * b2Axis1.dot(b1Axis3));
		m_swingAxis.normalize();

		real_t swingAxisSign = (b2Axis1.dot(b1Axis1) >= 0.0f) ? 1.0f : -1.0f;
		m_swingAxis *= swingAxisSign;

		m_kSwing = real_t(1.) / (A->compute_angular_impulse_denominator(m_swingAxis) +
										B->compute_angular_impulse_denominator(m_swingAxis));
	}

	// Twist limits
	if (m_twistSpan >= real_t(0.)) {
		Vector3 b2Axis2 = B->get_transform().basis.xform(this->m_rbBFrame.basis.get_axis(1));
		Quat rotationArc = Quat(b2Axis1, b1Axis1);
		Vector3 TwistRef = rotationArc.xform(b2Axis2);
		real_t twist = atan2fast(TwistRef.dot(b1Axis3), TwistRef.dot(b1Axis2));

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

void CCameraFlight::UpdateFlight(SViewParams &viewParams)
{
	if(m_eMovementMode != eCFM_FREE_FLIGHT &&
		(m_fFlightProgress >= 1.0f || m_eMovementMode == eCFM_NONE || m_cameraCourse.size() < 3))
	{
		//update free fly point while not in free fly
		m_freeFlyPoint.m_vCamPos = viewParams.position;
		m_freeFlyPoint.m_vCamLookAt = viewParams.position + Vec3Constants<float>::fVec3_OneY;
		m_eState = eCFS_NONE;
		//nothing else to do
		return;
	}

	m_eState = eCFS_RUNNING;

	//update ref pos
	if(m_pRefEnt)
		m_vRefPos = m_pRefEnt->GetWorldPos();
	//if refPos2 is set, find middle
	if(m_vRefPos2.len2() > 0.0f)
		m_vRefPos = (m_vRefPos + m_vRefPos2) * 0.5f;

	//find target
	SCameraFlightPoint targetPoint = SCameraFlightPoint();

	switch(m_eMovementMode)
	{
	case eCFM_FREE_FLIGHT:
		targetPoint = m_freeFlyPoint;
		break;
	case eCFM_SPLINE:
		targetPoint = GetSplinePoint(m_fFlightProgress);
		break;
	case eCFM_LINE:
		targetPoint = GetTrackPoint(m_fFlightProgress);
		break;
	default:
		break;
	}

	//compute new dir/pos
	m_vLookingDirection = targetPoint.m_vCamLookAt - targetPoint.m_vCamPos;
	if(m_bUseRefDir)
	{
		m_vLookingDirection = m_vRefDir;
		m_qFadeOrientation = Quat::CreateRotationVDir(m_vLookingDirection, 0.0f);
	}
	m_vLookingDirection.NormalizeSafe();
	Quat qTempDirection = Quat::CreateRotationVDir(m_vLookingDirection, 0.0f);
	Vec3 vTempPos = targetPoint.m_vCamPos;

	bool bFading = false;

	//compute fading
	if(m_eMovementMode != eCFM_FREE_FLIGHT)
	{
		if(m_fFlightProgress > m_fFadeOutTime && (m_eFadeMode == eCFFM_OUT || m_eFadeMode == eCFFM_INOUT))
		{
			//fade position
			m_fFadeProgress = InterpolateTo(m_fFadeProgress, 1.0f, m_fFadeTime);
			m_vTargetFadePos = vTempPos * (1.0f - m_fFadeProgress) + viewParams.position * m_fFadeProgress;
			//fade orientation
			qTempDirection = Quat_tpl<float>::CreateNlerp(m_qFadeOrientation, viewParams.rotation, m_fFadeProgress);
			if(m_fFadeProgress < 0.998f)
			{
				bFading = true;
				m_eState = eCFS_FADE_OUT;
			}
		}
		else if(m_fFlightProgress < m_fFadeInTime && (m_eFadeMode == eCFFM_IN || m_eFadeMode == eCFFM_INOUT))
		{
			//fade position
			m_fFadeProgress = InterpolateTo(m_fFadeProgress, 1.0f, m_fFadeTime);
			m_vTargetFadePos = viewParams.position * (1.0f - m_fFadeProgress) + vTempPos * m_fFadeProgress;
			//fade orientation
			qTempDirection = Quat_tpl<float>::CreateNlerp(viewParams.rotation, qTempDirection, m_fFadeProgress);
			if(m_fFadeProgress < 0.998f)
			{
				bFading = true;
				m_eState = eCFS_FADE_IN;
			}
		}
		else
		{
			m_vTargetFadePos = vTempPos;
			//m_vTargetFadeLookAt = targetPoint.m_vCamLookAt;
			m_qFadeOrientation = qTempDirection;
			m_fFadeProgress = 0.0f;
			m_eState = eCFS_RUNNING;
		}
	}
	else
	{
		m_vTargetFadePos = vTempPos;
	}

	//update dir
	m_vLookingDirection = qTempDirection.GetColumn1();

	//raycast to prevent clipping during flight
//.........这里部分代码省略.........

Quat MUST_USE_RESULT Quat::Slerp(const Quat &q2, float t) const
{
	assume(0.f <= t && t <= 1.f);
	assume(IsNormalized());
	assume(q2.IsNormalized());

#if defined(MATH_AUTOMATIC_SSE) && defined(MATH_SSE)
	simd4f angle = dot4_ps(q, q2.q); // <q, q2.q>
	simd4f neg = cmplt_ps(angle, zero_ps()); // angle < 0?
	neg = and_ps(neg, set1_ps_hex(0x80000000)); // Convert 0/0xFFFFFFFF mask to a 0x/0x80000000 mask.
//	neg = s4i_to_s4f(_mm_slli_epi32(s4f_to_s4i(neg), 31)); // A SSE2-esque way to achieve the above would be this, but this seems to clock slower (12.04 clocks vs 11.97 clocks)
	angle = xor_ps(angle, neg); // if angle was negative, make it positive.
	simd4f one = set1_ps(1.f);
	angle = min_ps(angle, one); // If user passed t > 1 or t < -1, clamp the range.

	// Compute a fast polynomial approximation to arccos(angle).
	// arccos(x): (-0.69813170079773212f * x * x - 0.87266462599716477f) * x + 1.5707963267948966f;
	angle = madd_ps(msub_ps(mul_ps(set1_ps(-0.69813170079773212f), angle), angle, set1_ps(0.87266462599716477f)), angle, set1_ps(1.5707963267948966f));

	// Shuffle an appropriate vector from 't' and 'angle' for computing two sines in one go.
	simd4f T = _mm_set_ss(t); // (.., t)
	simd4f oneSubT = sub_ps(one, T); // (.., 1-t)
	T = _mm_movelh_ps(T, oneSubT); // (.., 1-t, .., t)
	angle = mul_ps(angle, T); // (.., (1-t)*angle, .., t*angle)

	// Compute a fast polynomial approximation to sin(t*angle) and sin((1-t)*angle).
	// Here could use "angle = sin_ps(angle);" for precision, but favor speed instead with the following polynomial expansion:
	// sin(x): ((5.64311797634681035370e-03 * x * x - 1.55271410633428644799e-01) * x * x + 9.87862135574673806965e-01) * x
	simd4f angle2 = mul_ps(angle, angle);
	angle = mul_ps(angle, madd_ps(madd_ps(angle2, set1_ps(5.64311797634681035370e-03f), set1_ps(-1.55271410633428644799e-01f)), angle2, set1_ps(9.87862135574673806965e-01f)));

	// Compute the final lerp factors a and b to scale q and q2.
	simd4f a = zzzz_ps(angle);
	simd4f b = xxxx_ps(angle);
	a = xor_ps(a, neg);
	a = mul_ps(q, a);
	a = madd_ps(q2, b, a);

	// The lerp above generates an unnormalized quaternion which needs to be renormalized.
	return mul_ps(a, rsqrt_ps(dot4_ps(a, a)));
#else
	float angle = this->Dot(q2);
	float sign = 1.f; // Multiply by a sign of +/-1 to guarantee we rotate the shorter arc.
	if (angle < 0.f)
	{
		angle = -angle;
		sign = -1.f;
	}

	float a;
	float b;
	if (angle < 0.999) // perform spherical linear interpolation.
	{
		// angle = Acos(angle); // After this, angle is in the range pi/2 -> 0 as the original angle variable ranged from 0 -> 1.
		angle = (-0.69813170079773212f * angle * angle - 0.87266462599716477f) * angle + 1.5707963267948966f;

		float ta = t*angle;
#ifdef MATH_USE_SINCOS_LOOKUPTABLE
		// If Sin() is based on a lookup table, prefer that over polynomial approximation.
		a = Sin(angle - ta);
		b = Sin(ta);
#else
		// Not using a lookup table, manually compute the two sines by using a very rough approximation.
		float ta2 = ta*ta;
		b = ((5.64311797634681035370e-03f * ta2 - 1.55271410633428644799e-01f) * ta2 + 9.87862135574673806965e-01f) * ta;
		a = angle - ta;
		float a2 = a*a;
		a = ((5.64311797634681035370e-03f * a2 - 1.55271410633428644799e-01f) * a2 + 9.87862135574673806965e-01f) * a;
#endif
	}
	else // If angle is close to taking the denominator to zero, resort to linear interpolation (and normalization).
	{
		a = 1.f - t;
		b = t;
	}
	// Lerp and renormalize.
	return (*this * (a * sign) + q2 * b).Normalized();
#endif
}

		void							TMCtrl::update(REAL dt)
		{
			if(!_node.valid())
				return;

			Z_ASSERT( find(_node.get_unsafe()->getCtrlSet().getArray().begin(), _node.get_unsafe()->getCtrlSet().getArray().end(), this) != _node.get_unsafe()->getCtrlSet().getArray().end());

			_anim_ctrl.advance(dt);

			LOD::TRANSITION transition = LOD::isForceTransition() ? LOD::getForcedTransition() : _anim_ctrl.getTransition();
			LOD::INTERPOLATION interpolation = LOD::isForceInterpolation() ? LOD::getForcedInterpolation() : getInterpolation();

			if(interpolation == LOD::INTERPOLATION_LINEAR)
			{
				switch(transition)
				{
				case LOD::TRANSITION_NONE:
					{
						AnimCtrl::Clip& stage = _anim_ctrl.current_stage();
						if(getPosAnimData())
							_node.get_unsafe()->setPos(_pos_eval.sample(stage.getTime()));
						if(getRotAnimData())
							_node.get_unsafe()->setRot(_rot_eval.sample(stage.getTime()));
						if(getScaleAnimData())
							_node.get_unsafe()->setScale(_scale_eval.sample(stage.getTime()));
					}
					break;
				case LOD::TRANSITION_BLEND_CAPTURED:
					{
						AnimCtrl::Clip& stage = _anim_ctrl.current_stage();
						if(getPosAnimData())
							_node.get_unsafe()->setPos(_pos_eval.sample(stage.getTime()).lerp(_pos_captured, 1 - stage.getWeight()));
						if(getRotAnimData())
							_node.get_unsafe()->setRot(_rot_eval.sample(stage.getTime()).slerp(_rot_captured, 1 - stage.getWeight()));
						if(getScaleAnimData())
							_node.get_unsafe()->setScale(_scale_eval.sample(stage.getTime()).lerp(_scale_captured, 1 - stage.getWeight()));
					}
					break;
				case LOD::TRANSITION_MULTISTAGE_BLEND:
					{
						Vec3 pos;
						Quat rot;
						Vec3 scale;
						bool enable_pos = !!getPosAnimData();
						bool enable_rot = !!getRotAnimData();
						bool enable_scale = !!getScaleAnimData();
						if(true)
						{
							size_t c = _anim_ctrl.getStageCount();
							AnimCtrl::Clip* stages = _anim_ctrl.getStages();
							for(size_t i = 0; i < c; ++i)
							{
								const AnimCtrl::Clip& stage = stages[i];
								if(i == 0)
								{
									if(enable_pos)
										pos = _pos_eval.sample(stage.getTime());
									if(enable_rot)
										rot = _rot_eval.sample(stage.getTime());
									if(enable_scale)
										scale = _scale_eval.sample(stage.getTime());
									continue;
								}
								if(enable_pos)
									pos = pos.lerp(_pos_eval.sample(stage.getTime()), stage.getWeight());
								if(enable_rot)
									rot = rot.slerp(_rot_eval.sample(stage.getTime()), stage.getWeight());
								if(enable_scale)
									scale = scale.lerp(_scale_eval.sample(stage.getTime()), stage.getWeight());
							}
							if(enable_pos)
								_node.get_unsafe()->setPos(pos);
							if(enable_rot)
								_node.get_unsafe()->setRot(rot);
							if(enable_scale)
								_node.get_unsafe()->setScale(scale);
						}
					}
					break;
				}
			}
			else
			{
				switch(transition)
				{
				case LOD::TRANSITION_NONE:
					{
						AnimCtrl::Clip& stage = _anim_ctrl.current_stage();
						if(getPosAnimData())
							_node.get_unsafe()->setPos(_pos_eval.eval_step(stage.getTime()));
						if(getRotAnimData())
							_node.get_unsafe()->setRot(_rot_eval.eval_step(stage.getTime()));
						if(getScaleAnimData())
							_node.get_unsafe()->setScale(_scale_eval.eval_step(stage.getTime()));
					}
					break;
				case LOD::TRANSITION_BLEND_CAPTURED:
					{
						AnimCtrl::Clip& stage = _anim_ctrl.current_stage();
						if(getPosAnimData())
//.........这里部分代码省略.........

void OrientConstRotation::Update(TimeValue t){
	Interval iv = FOREVER;
	ivalid = FOREVER;
	
	int ct = pblock->Count(orientation_target_list);
	int ctf = pblock->Count(orientation_target_weight);

	float total_orient_target_weight_prev = 0.0f, total_orient_target_weight_current = 0.0f;
	float orient_targ_Wt_current = 0.0f;
	Quat quat_prev, quat_current, lCurRot;
	INode *orient_target_prev= NULL, *orient_target_current= NULL;
	Matrix3 targetTM(1);
	Point3 trans, scaleP;

	quat_prev.Identity();
	quat_current.Identity();
	lCurRot.Identity();

		if (ct == 1){
			pblock->GetValue(orientation_target_list, t, orient_target_prev, iv, 0);
			pblock->GetValue(orientation_target_weight, t, orient_targ_Wt_current, iv, 0);
				ivalid &= iv;

				if (orient_target_prev == NULL){
					targetTM.IdentityMatrix();
				}
				else {
					targetTM = orient_target_prev->GetNodeTM(t, &ivalid);
				}

				if (IsLocal() && orient_target_prev != NULL) {
					targetTM = targetTM * Inverse(orient_target_prev->GetParentTM(t));
				}
				AffineParts comps;		// Requires header decomp.h
				decomp_affine(targetTM, &comps);
				quat_current = comps.q;
				quat_current.Normalize();
				quat_current.MakeClosest(quat_prev);

				lCurRot = quat_current;

				quat_prev = lCurRot;
				total_orient_target_weight_prev += orient_targ_Wt_current;
//			}
		}
		else if (ct > 1){

			pblock->GetValue(orientation_target_list, t, orient_target_prev, iv, 0);
			pblock->GetValue(orientation_target_weight, t, orient_targ_Wt_current, iv, 0);
//			if (orient_target_prev != NULL)
//			{
				ivalid &= iv;

				if (orient_target_prev == NULL){
					targetTM.IdentityMatrix();
				}
				else {
					targetTM = orient_target_prev->GetNodeTM(t, &ivalid);
				}

				if (IsLocal() && orient_target_prev != NULL) {
					targetTM = targetTM * Inverse(orient_target_prev->GetParentTM(t));
				}
				AffineParts comps;		// Requires header decomp.h
				decomp_affine(targetTM, &comps);
				quat_current = comps.q;
				quat_current.Normalize();
				quat_current.MakeClosest(quat_prev);

				lCurRot = quat_current;

				quat_prev = lCurRot;
				total_orient_target_weight_prev += orient_targ_Wt_current;
//			}

			for (int i = 0; i < ct -1; i++) {
//				ct = pblock->Count(orientation_target_list);
				pblock->GetValue(orientation_target_list, t, orient_target_current, iv, i+1);
				pblock->GetValue(orientation_target_weight, t, orient_targ_Wt_current, iv, i+1);
//				if (orient_target_current != NULL){
					ivalid &= iv;

					if (orient_target_current == NULL){
						targetTM.IdentityMatrix();
					}
					else {
						targetTM = orient_target_current->GetNodeTM(t, &ivalid);
					}

//					Matrix3 targetTM = orient_target_current->GetNodeTM(t, &ivalid);
					if (IsLocal() && orient_target_current != NULL) {
						targetTM = targetTM * Inverse(orient_target_current->GetParentTM(t));
					}
					AffineParts comps;		// Requires header decomp.h
					decomp_affine(targetTM, &comps);
					quat_current = comps.q;
					quat_current.Normalize();
					quat_current.MakeClosest(quat_prev);

					float slerp_wt = 0.0f;
//.........这里部分代码省略.........

	void PS_Billboard::_updateBuffer()
	{
		mRenderOp.primCount = mParent->GetParticleCount() * 2;
		if (mRenderOp.primCount == 0)
			return ;

		Camera * pCamera = World::Instance()->GetCurrentRenderContext()->GetCamera();
		const Quat & worldQ = mParent->GetParent()->GetWorldRotation();
		Mat4 worldTM;
		
		if (mParent->IsScaleAble())
			worldTM = mParent->GetParent()->GetWorldTM();
		else
			worldTM.MakeTransform(mParent->GetParent()->GetWorldPosition(), worldQ, Float3(1, 1, 1));

		pCamera->GetWorldRotation().ToAxis(mCamXAxis, mCamYAxis, mCamZAxis);
		mCamPos = pCamera->GetWorldPosition();

		mCommonDir = mParent->GetCommonDirection();
		mCommonUp = mParent->GetCommonUpVector();

		if (mParent->GetBillboardType() == PS_BillboardType::PERPENDICULAR ||
			mParent->GetBillboardType() == PS_BillboardType::PERPENDICULAR_COMMON)
		{
			mCommonDir.TransformQ(worldQ);
			mCommonUp.TransformQ(worldQ);
		}

		float width, height, asecpt = 1;
		Float3 xAxis, yAxis;
		Float3 pos, dir;
		const Float2 & center = mParent->GetBillboardCenter();

		if (mParent->IsKeepAspect())
			asecpt = mParent->_getTexture()->_getAscept();
		
		PS_Vertex * v = (PS_Vertex *)mRenderOp.vertexBuffers[0]->Lock(eLockFlag::WRITE);
		for (int i = 0; i < mParent->GetParticleCount(); ++i)
		{
			const Particle * p = mParent->GetParticle(i);

			height = Max(0.0f, p->Size.y);
			if (!mParent->IsKeepAspect())
				width = Max(0.0f, p->Size.x);
			else
				width = height * asecpt;

			pos = p->Position;
			dir = p->Direction;

			if (mParent->IsLocalSpace())
			{
				pos.TransformA(worldTM);
				dir.TransformQ(mParent->GetParent()->GetWorldRotation());
			}

			_getBillboardXYAxis(xAxis, yAxis, pos, dir);

			if (p->Rotation.x != 0)
			{
				Quat q;
				q.FromAxis(Float3::Cross(xAxis, yAxis), Math::DegreeToRadian(p->Rotation.x));

				xAxis.TransformQ(q);
				yAxis.TransformQ(q);
			}

			xAxis *= width, yAxis *= height;

			v->position = pos - xAxis * center.x + yAxis * center.y;
			v->color = p->Color;
			v->uv.x = p->UVRect.x1, v->uv.y = p->UVRect.y1;
			++v;

			v->position = pos + xAxis * (1 - center.x) + yAxis * center.y;
			v->color = p->Color;
			v->uv.x = p->UVRect.x2, v->uv.y = p->UVRect.y1;
			++v;

			v->position = pos - xAxis * center.x - yAxis * (1 - center.y);
			v->color = p->Color;
			v->uv.x = p->UVRect.x1, v->uv.y = p->UVRect.y2;
			++v;

			v->position = pos + xAxis * (1 - center.x) - yAxis * (1 - center.y);
			v->color = p->Color;
			v->uv.x = p->UVRect.x2, v->uv.y = p->UVRect.y2;
			++v;
		}
		mRenderOp.vertexBuffers[0]->Unlock();
	}

//------------------------------------------------------------------
void CLam::UpdateFPLaser(float frameTime, CItem* parent)
{
	Vec3 lamPos, dir;
	
	if (m_laserActivated)
		AdjustLaserFPDirection(parent,dir,lamPos);
	else
	{
		// Lam Light
		lamPos = parent->GetSlotHelperPos(eIGS_FirstPerson,m_laserHelperFP.c_str(),true);
		Quat   lamRot = Quat(parent->GetSlotHelperRotation(eIGS_FirstPerson,m_laserHelperFP.c_str(),true));
		dir = lamRot.GetColumn1();
	}

//	float  len = m_lamparams.laser_range[eIGS_FirstPerson];

	dir.Normalize();

	const float nearClipPlaneLimit = 10.0f;

	Vec3 hitPos(0,0,0);
	float laserLength = 0.0f;
	float dotScale = 1.0f;
	{
		IPhysicalEntity* pSkipEntity = NULL;
		if(parent->GetOwner())
			pSkipEntity = parent->GetOwner()->GetPhysics();

		const int objects = ent_all;
		const int flags = (geom_colltype_ray << rwi_colltype_bit) | rwi_colltype_any | (10 & rwi_pierceability_mask) | (geom_colltype14 << rwi_colltype_bit);

		ray_hit hit;	
		if (gEnv->pPhysicalWorld->RayWorldIntersection(lamPos, dir*m_lamparams.laser_range[eIGS_FirstPerson], objects,
			flags, &hit, 1, &pSkipEntity, pSkipEntity?1:0))
		{

			//Clamp distance below near clip plane limits, if not dot will be overdrawn during rasterization
			if(hit.dist>nearClipPlaneLimit)
			{
				laserLength = nearClipPlaneLimit;
				m_lastLaserHitPt = lamPos + (nearClipPlaneLimit*dir);
			}
			else
			{
				laserLength = hit.dist;
				m_lastLaserHitPt = hit.pt;
			}
			m_lastLaserHitSolid = true;
			if(parent->GetOwnerActor() && parent->GetOwnerActor()->GetActorParams())
				dotScale *= max(0.3f,parent->GetOwnerActor()->GetActorParams()->viewFoVScale);

		}
		else
		{
			m_lastLaserHitSolid = false;
			m_lastLaserHitPt = lamPos - (dir*3.0f);
			laserLength = 3.0f;
		}
		hitPos = m_lastLaserHitPt;
		if(g_pGameCVars->i_debug_projectiles!=0)
		 gEnv->pRenderer->GetIRenderAuxGeom()->DrawSphere(hitPos, 0.2f, ColorB(255,0,0));
	}

	if (m_laserActivated && m_dotEffectSlot >= 0)
	{
		Matrix34 worldMatrix = GetEntity()->GetWorldTM();

		if(laserLength<=0.7f)
			hitPos = lamPos+(0.7f*dir);

		CWeapon* pWep = static_cast<CWeapon*>(parent->GetIWeapon());
		if(pWep && pWep->IsWeaponLowered())
		{
			hitPos = lamPos+(2.0f*dir);
			laserLength = 2.0f;
		}

		if(laserLength<=2.0f)
			dotScale *= min(1.0f,(0.35f + ((laserLength-0.7f)*0.5f)));

		IEntity* pDotEntity = m_pEntitySystem->GetEntity(m_pLaserEntityId);
		if(pDotEntity)
		{
			Matrix34 finalMatrix = Matrix34::CreateTranslationMat(hitPos-(0.2f*dir));
			pDotEntity->SetWorldTM(finalMatrix);
			Matrix34 localScale = Matrix34::CreateIdentity();
			localScale.SetScale(Vec3(dotScale,dotScale,dotScale));
			pDotEntity->SetSlotLocalTM(m_dotEffectSlot,localScale);
		}
	}

	if (m_laserActivated || m_lightActivated)
	{
		float laserAIRange = m_laserActivated ? laserLength : 0.0f;
		float lightAIRange = m_lightActivated ? min(laserLength, m_lamparams.light_range[eIGS_FirstPerson] * 1.5f) : 0.0f;
		UpdateAILightAndLaser(lamPos, dir, lightAIRange, m_lamparams.light_fov[eIGS_FirstPerson], laserAIRange);
	}

}

void CUIHUD3D::UpdateView(const SViewParams &viewParams)
{
	CActor* pLocalPlayer = (CActor*)g_pGame->GetIGameFramework()->GetClientActor();
	
	if (gEnv->IsEditor() && !gEnv->IsEditing() && !m_pHUDRootEntity)
		SpawnHudEntities();

	// When you die we destroy the HUD, so this will make sure it re-spawns when you respawned
	if (!gEnv->IsEditor() && !m_pHUDRootEntity)
		SpawnHudEntities();
	
	const CUICVars* pCVars = g_pGame->GetUI()->GetCVars();
	if (m_pHUDRootEntity && pLocalPlayer)
	{
		if(pCVars->hud_detach)
			return;

		const QuatT& cameraTran = pLocalPlayer->GetCameraTran();
		const QuatT& hudTran = pLocalPlayer->GetHUDTran();

		const Quat& cameraRotation = cameraTran.q;
		const Quat& hudRotation = hudTran.q;
		Quat deltaHudRotation = cameraRotation.GetInverted() * hudRotation;

		if(pCVars->hud_bobHud > 0.0f && !pLocalPlayer->IsDead())
		{
			deltaHudRotation.w *= (float)__fres(pCVars->hud_bobHud);

			// Avoid divide by 0  
			if (!(fabs(deltaHudRotation.w) < FLT_EPSILON)) // IsValid() doesn't catch it
			{
				deltaHudRotation.Normalize();
			}
		}
		else
		{
			deltaHudRotation.SetIdentity();
		}

		// In general use the player position + viewparams override
		Vec3 viewPosition = pLocalPlayer->GetEntity()->GetPos() + viewParams.position;
		Quat clientRotation = viewParams.rotation * deltaHudRotation;
		
		// Override special cases: Third person should use camera-oriented HUD instead of HUD-bone oriented
		// Sliding should use the Bone instead of the viewParams solution
		if(pLocalPlayer->IsThirdPerson() || pLocalPlayer->GetLinkedVehicle() != NULL)
		{
			viewPosition = viewParams.position;
		}
		else
		{
			CPlayer* player = (CPlayer*)pLocalPlayer;
			if(player && player->IsSliding())
			{
				viewPosition = pLocalPlayer->GetEntity()->GetWorldTM().TransformPoint(hudTran.t);
			}
		}
		//

		const Vec3 forward(clientRotation.GetColumn1());
		const Vec3 up(clientRotation.GetColumn2());
		const Vec3 right(-(up % forward));

		const float distance = pCVars->hud_cameraOverride ? pCVars->hud_cameraDistance : m_fHudDist;
		const float offset = pCVars->hud_cameraOverride ? pCVars->hud_cameraOffsetZ  : m_fHudZOffset;

		float offsetScale = 1.0f;
		if(viewParams.fov > 0.0f)
		{
			offsetScale = (pCVars->hud_cgf_positionScaleFOV * __fres(viewParams.fov )) + distance;
		}
		
		// Allow overscanBorders to control safe zones
		Vec2 overscanBorders = ZERO;
		if(gEnv->pRenderer)
		{	
			gEnv->pRenderer->EF_Query(EFQ_OverscanBorders, overscanBorders);
		}
		const float viewDepth = 1.0f + (pCVars->hud_overscanBorder_depthScale * overscanBorders.y);
		viewPosition += forward*viewDepth*offsetScale;

		viewPosition += (up * offset);
		viewPosition += (right * pCVars->hud_cgf_positionRightScale);
		const Vec3 posVec(viewPosition + (forward * 0.001f));

		static const Quat rot90Deg = Quat::CreateRotationXYZ( Ang3(gf_PI * 0.5f, 0, 0) );
		const Quat rotation = clientRotation * rot90Deg; // rotate 90 degrees around X-Axis

		m_pHUDRootEntity->SetPosRotScale(posVec, rotation, m_pHUDRootEntity->GetScale(), ENTITY_XFORM_NO_SEND_TO_ENTITY_SYSTEM);
	}

	if (gEnv->pRenderer && m_pHUDRootEntity && pCVars->hud_debug3dpos > 0)
	{
		gEnv->pRenderer->GetIRenderAuxGeom()->DrawSphere(m_pHUDRootEntity->GetWorldPos(), 0.2f, ColorB(255,0,0));
		const int children = m_pHUDRootEntity->GetChildCount();
		for (int i = 0; i < children && pCVars->hud_debug3dpos > 1; ++i)
		{
			IEntity* pChild = m_pHUDRootEntity->GetChild(i);
			gEnv->pRenderer->GetIRenderAuxGeom()->DrawSphere(pChild->GetWorldPos(), 0.1f, ColorB(255,255,0));
		}
//.........这里部分代码省略.........

//--------------------------------------------------------------------------------------------------
// Name: SpawnParticlesOnSkeleton
// Desc: Spawn particles on Skeleton
//--------------------------------------------------------------------------------------------------
void CGameEffect::SpawnParticlesOnSkeleton(IEntity* pEntity, IParticleEmitter* pParticleEmitter, uint32 numParticles,float maxHeightScale) const
{
	if((pEntity) && (numParticles>0) && (pParticleEmitter) && (maxHeightScale>0.0f))
	{
		ICharacterInstance* pCharacter = pEntity->GetCharacter(0);
		if(pCharacter)
		{
			IDefaultSkeleton& rIDefaultSkeleton = pCharacter->GetIDefaultSkeleton();
			ISkeletonPose* pPose = pCharacter->GetISkeletonPose();
			if(pPose)
			{
				Vec3 animPos;
				Quat animRot;

				IActor* pActor = gEnv->pGame->GetIGameFramework()->GetIActorSystem()->GetActor(pEntity->GetId());
				if(pActor) // First try to get animation data
				{
					QuatT animLoc = pActor->GetAnimatedCharacter()->GetAnimLocation();
					animPos = animLoc.t;
					animRot = animLoc.q;
				}
				else // If no actor, then use entity data
				{
					animPos = pEntity->GetWorldPos();
					animRot = pEntity->GetWorldRotation();
				}

				animRot.Invert();

				AABB bbox;
				pEntity->GetLocalBounds(bbox);
				float bbHeight = bbox.max.z - bbox.min.z;
				// Avoid division by 0
				if(bbHeight == 0)
				{
					bbHeight = 0.0001f;
				}

				const uint32 numJoints = rIDefaultSkeleton.GetJointCount();

				for (uint32 i = 0; i < numParticles; ++i)
				{
					int id = cry_random(0U, numJoints - 1);
					int parentId = rIDefaultSkeleton.GetJointParentIDByID(id);

					if(parentId>0)
					{
						QuatT boneQuat = pPose->GetAbsJointByID(id);
						QuatT parentBoneQuat= pPose->GetAbsJointByID(parentId);
						float lerpScale = cry_random(0.0f, 1.0f);

						QuatTS loc(IDENTITY);
						loc.t = LERP(boneQuat.t,parentBoneQuat.t,lerpScale);

						float heightScale = ((loc.t.z - bbox.min.z) / bbHeight);
						if(heightScale < maxHeightScale)
						{
							loc.t = loc.t * animRot;
							loc.t = loc.t + animPos;

							pParticleEmitter->EmitParticle(NULL, NULL, &loc);
						}
					}
				}
			}
		}
	}
}//-------------------------------------------------------------------------------------------------