C++ Matrix34::OrthonormalizeFast方法代码示例

//------------------------------------------------------------------------
void CGunTurret::UpdatePhysics()
{
	int slots[] = {eIGS_Aux0,eIGS_Aux1,eIGS_ThirdPerson};
	int slots_num = 3;

	if(m_destroyed)
	{
		slots[1] = eIGS_Destroyed;
		slots_num = 2;
	}

	if(IEntity *pParent = GetEntity()->GetParent())
	{
		IPhysicalEntity *pParentPhysics = pParent->GetPhysics();
		IEntityPhysicalProxy *pPhysicsProxy = (IEntityPhysicalProxy *)GetEntity()->GetProxy(ENTITY_PROXY_PHYSICS);

		if(pParentPhysics && pPhysicsProxy)
		{
			Matrix34 localTM = GetEntity()->GetLocalTM();
			localTM.OrthonormalizeFast();
			Quat localQ = Quat(localTM);

			for(int i=0; i<slots_num; ++i)
			{
				pe_params_part params;

				params.partid = slots[i] + pPhysicsProxy->GetPartId0();

				const Matrix34 &slotTM = GetEntity()->GetSlotLocalTM(slots[i], false);

				params.pos = slotTM.GetTranslation();
				params.q = Quat(slotTM);

				params.pos = localTM * params.pos;
				params.q = localQ * params.q;
				pParentPhysics->SetParams(&params);
			}
		}
	}
	else
	{
		if(IPhysicalEntity *pPhysics = GetEntity()->GetPhysics())
		{
			for(int i=0; i<slots_num; ++i)
			{
				pe_params_part params;
				params.partid = slots[i];

				Matrix34 slotTM = GetEntity()->GetSlotLocalTM(slots[i], false);
				params.pMtx3x4 = &slotTM;

				pPhysics->SetParams(&params);
			}
		}
	}
}

void CVehicleSeatActionRotateTurret::MaintainPartRotationWorldSpace(EVehicleTurretRotationType eType)
{
	CVehiclePartBase* pPart   = m_rotations[eType].m_pPart;
	IVehiclePart*     pParent = pPart->GetParent();
	IActor*           pActor  = m_pSeat->GetPassengerActor();

	bool remote     = m_pSeat->GetCurrentTransition() == IVehicleSeat::eVT_RemoteUsage;
	bool worldSpace = m_rotations[eType].m_worldSpace && VehicleCVars().v_independentMountedGuns != 0;

	if (worldSpace && pParent && pActor && pActor->IsClient() && !remote)
	{
		// we want to keep the old worldspace rotation
		// therefore we're updating the local transform from it
		// NB: there is no need to clamp here, its done later

		Matrix34 localTM = pParent->GetWorldTM().GetInverted() * Matrix34(m_rotations[eType].m_prevWorldQuat);
		localTM.OrthonormalizeFast(); // precision issue

		const Matrix34 &baseTM = pPart->GetLocalBaseTM();

		if (!Matrix34::IsEquivalent(baseTM,localTM))
		{
			Ang3 anglesCurr(baseTM);
			Ang3 angles(localTM);

			if (eType == eVTRT_Pitch)
			{
				angles.y = anglesCurr.y;
				angles.z = anglesCurr.z;
			}
			else if (eType == eVTRT_Yaw)
			{
				angles.x = anglesCurr.x;
				angles.y = anglesCurr.y;
			}

			localTM.SetRotationXYZ(angles);
			localTM.SetTranslation(baseTM.GetTranslation());
			pPart->SetLocalBaseTM(localTM);

			m_pSeat->ChangedNetworkState(CVehicle::ASPECT_PART_MATRIX);
		}

#if ENABLE_VEHICLE_DEBUG
		if (VehicleCVars().v_debugdraw == eVDB_Parts)
		{
			float color[] = {1,1,1,1};
			Ang3  a(localTM), aBase(baseTM);
			gEnv->pRenderer->Draw2dLabel(200,200,1.4f,color,false,"localAng: %.1f (real: %.1f)", RAD2DEG(a.z), RAD2DEG(aBase.z));
		}
#endif
	}
}

//------------------------------------------------------------------------
void CView::Update(float frameTime,bool isActive)
{
	//FIXME:some cameras may need to be updated always
	if (!isActive)
		return;

	CGameObject* pLinkedTo = GetLinkedGameObject();
	if (pLinkedTo && !pLinkedTo->CanUpdateView())
		pLinkedTo = nullptr;
	IEntity* pEntity = pLinkedTo ? 0 : GetLinkedEntity();

	if (pLinkedTo || pEntity)
	{
		m_viewParams.SaveLast();

		CCamera* pSysCam = &m_pSystem->GetViewCamera();

		//process screen shaking
		ProcessShaking(frameTime);

		//FIXME:to let the updateView implementation use the correct shakeVector
		m_viewParams.currentShakeShift = m_viewParams.rotation * m_viewParams.currentShakeShift;

		m_viewParams.frameTime = frameTime;
		//update view position/rotation
		if (pLinkedTo)
		{
			pLinkedTo->UpdateView(m_viewParams);
			if (!m_viewParams.position.IsValid())
			{
				m_viewParams.position = m_viewParams.GetPositionLast();
				CRY_ASSERT_MESSAGE(0, "Camera position is invalid, reverting to old position");
			}
			if (!m_viewParams.rotation.IsValid())
			{
				m_viewParams.rotation = m_viewParams.GetRotationLast();
				CRY_ASSERT_MESSAGE(0, "Camera rotation is invalid, reverting to old rotation");
			}
		}
		else
		{
			Matrix34 mat = pEntity->GetWorldTM();
			mat.OrthonormalizeFast();
			m_viewParams.position = mat.GetTranslation();
			m_viewParams.rotation = Quat(mat);
		}

		ApplyFrameAdditiveAngles(m_viewParams.rotation);

		if (pLinkedTo)
		{
			pLinkedTo->PostUpdateView(m_viewParams);
		}

		const float fNearZ = gEnv->pGame->GetIGameFramework()->GetIViewSystem()->GetDefaultZNear();

		//see if the view have to use a custom near clipping plane
		const float nearPlane = (m_viewParams.nearplane >= 0.01f) ? (m_viewParams.nearplane) : fNearZ;
		const float farPlane  = gEnv->p3DEngine->GetMaxViewDistance();
		float       fov       = (m_viewParams.fov < 0.001f) ? DEFAULT_FOV : m_viewParams.fov;

		// [VR] specific
		// Modify FOV based on the HDM device configuration
		IHmdManager* pHmdManager         = gEnv->pSystem->GetHmdManager();
		IHmdDevice*  pHmdDevice          = nullptr;
		bool         bHmdTrackingEnabled = false;
		if (pHmdManager)
		{
			pHmdDevice = pHmdManager->GetHmdDevice();
			if (pHmdDevice)
			{
				const HmdTrackingState &sensorState = pHmdDevice->GetLocalTrackingState();
				if (sensorState.CheckStatusFlags(eHmdStatus_IsUsable))
				{
					bHmdTrackingEnabled = true;
				}
			}
		}

		if (pHmdManager->IsStereoSetupOk())
		{
			const IHmdDevice*       pDev        = pHmdManager->GetHmdDevice();
			const HmdTrackingState &sensorState = pDev->GetLocalTrackingState();
			if (sensorState.CheckStatusFlags(eHmdStatus_IsUsable))
			{
				float arf_notUsed;
				pDev->GetCameraSetupInfo(fov, arf_notUsed);
			}
		}

		m_camera.SetFrustum(pSysCam->GetViewSurfaceX(), pSysCam->GetViewSurfaceZ(), fov, nearPlane, farPlane, pSysCam->GetPixelAspectRatio());

		//TODO: (14, 06, 2010, "the player view should always get updated, this due to the hud being visable, without shocking, in cutscenes - todo is to see if we can optimise this code");
		IActor* pActor = gEnv->pGame->GetIGameFramework()->GetClientActor();
		if (pActor)
		{
			CGameObject* const linkToObj = static_cast<CGameObject*>(pActor->GetEntity()->GetProxy(ENTITY_PROXY_USER));

			if (linkToObj && linkToObj != pLinkedTo)
//.........这里部分代码省略.........

//------------------------------------------------------------------------
void CView::Update(float frameTime,bool isActive)
{
	//FIXME:some cameras may need to be updated always
	if (!isActive)
		return;

	CGameObject * pLinkedTo = GetLinkedGameObject();
	if (pLinkedTo && !pLinkedTo->CanUpdateView())
	  pLinkedTo = NULL;
	IEntity* pEntity = pLinkedTo ? 0 : GetLinkedEntity();

	if (pLinkedTo || pEntity)
	{
		m_viewParams.SaveLast();

		CCamera *pSysCam = &m_pSystem->GetViewCamera();

		//process screen shaking
		ProcessShaking(frameTime);
		
		//FIXME:to let the updateView implementation use the correct shakeVector
		m_viewParams.currentShakeShift = m_viewParams.rotation * m_viewParams.currentShakeShift;
		
    m_viewParams.frameTime=frameTime;
		//update view position/rotation
		if (pLinkedTo)
		{
			pLinkedTo->UpdateView(m_viewParams);
			if (!m_viewParams.position.IsValid())
			{
				m_viewParams.position = m_viewParams.GetPositionLast();
				CRY_ASSERT_MESSAGE(0, "Camera position is invalid, reverting to old position");
			}
			if (!m_viewParams.rotation.IsValid())
			{
				m_viewParams.rotation = m_viewParams.GetRotationLast();
				CRY_ASSERT_MESSAGE(0, "Camera rotation is invalid, reverting to old rotation");
			}
		}
		else
		{
			Matrix34 mat = pEntity->GetWorldTM();
			mat.OrthonormalizeFast();
			m_viewParams.position = mat.GetTranslation();
			m_viewParams.rotation = Quat(mat);
		}

		ApplyFrameAdditiveAngles(m_viewParams.rotation);

    if (pLinkedTo)
    {
      pLinkedTo->PostUpdateView(m_viewParams);
    }

		float fNearZ  = gEnv->pGame->GetIGameFramework()->GetIViewSystem()->GetDefaultZNear();
		
		//see if the view have to use a custom near clipping plane
		float nearPlane = (m_viewParams.nearplane > 0.01f)?(m_viewParams.nearplane):(fNearZ/*pSysCam->GetNearPlane()*/);
		float farPlane = gEnv->p3DEngine->GetMaxViewDistance();
		float fov = m_viewParams.fov < 0.001 ? DEFAULT_FOV : m_viewParams.fov;
		
		m_camera.SetFrustum(pSysCam->GetViewSurfaceX(),pSysCam->GetViewSurfaceZ(),fov,nearPlane,farPlane, pSysCam->GetPixelAspectRatio());

    //TODO: (14, 06, 2010, "the player view should always get updated, this due to the hud being visable, without shocking, in cutscenes - todo is to see if we can optimise this code");
    IActor * pActor = gEnv->pGame->GetIGameFramework()->GetClientActor();
    if (pActor)
    {
      CGameObject * linkToObj = (CGameObject*)pActor->GetEntity()->GetProxy( ENTITY_PROXY_USER );
      if (linkToObj && linkToObj != pLinkedTo)
      {
        linkToObj->PostUpdateView(m_viewParams);
      }
    }

		//apply shake & set the view matrix
		m_viewParams.rotation *= m_viewParams.currentShakeQuat;
		m_viewParams.rotation.NormalizeSafe();
		m_viewParams.position += m_viewParams.currentShakeShift;

		// Camera space Rendering calculations on Entity
		if(pLinkedTo)
		{
			IEntity* pLinkedToEntity = pLinkedTo->GetEntity();
			if(pLinkedToEntity)
			{
				const int slotIndex = 0;
				uint32 entityFlags = pLinkedToEntity->GetSlotFlags(slotIndex);
				if(entityFlags & ENTITY_SLOT_RENDER_NEAREST)
				{
					// Get camera pos relative to entity
					const Vec3 cameraLocalPos = m_viewParams.position;

					// Set entity's camera space position
					const Vec3 cameraSpacePos(-cameraLocalPos * m_viewParams.rotation);
					pLinkedToEntity->SetSlotCameraSpacePos(slotIndex,cameraSpacePos);

					// Add world pos onto camera local pos
					m_viewParams.position = pLinkedToEntity->GetWorldPos() + cameraLocalPos;
				}
//.........这里部分代码省略.........