C++ OBB::SetOBBfromAABB方法代码示例

bool CVTOLVehicleManager::TestIsInVTOL( const SVTOLInfo& info, const AABB& aabb ) const
{
	OBB obb;
	obb.SetOBBfromAABB(info.location.q, info.localBounds);
	obb.h.x *= g_pGameCVars->g_VTOLInsideBoundsScaleX;
	obb.h.y *= g_pGameCVars->g_VTOLInsideBoundsScaleY;
	obb.h.z *= g_pGameCVars->g_VTOLInsideBoundsScaleZ;
	obb.h.z += g_pGameCVars->g_VTOLInsideBoundsOffsetZ;
	return Overlap::AABB_OBB(aabb, info.location.t, obb);
}

// Calculates the desired position for the physics box, so that its center will be superimposed with AABB center of provided entity.
// Also adjusts upwards to avoid any obvious floor clipping.  Returns desired Position for entity.
Vec3 CIntersectionAssistanceUnit::CalculateTargetAdjustPoint(const IEntity* pEntity, const Matrix34 &wMat, const Vec3& vStartingPos) const
{
    // (if present + desired) adjust physbox center to that of owner Ent - to make sure centers of PhysBox + focal ent superimposed
    // at the desired position
    const IEntity* pFocalEnt = gEnv->pEntitySystem->GetEntity(m_focalEntityId);
    if(pFocalEnt)
        {
            OBB focalOBB;
            AABB focalAABB;

            // Compensate for actor/non actor entities that require different paths :(
            if(CPlayer* pPlayer = static_cast<CPlayer*>(g_pGame->GetIGameFramework()->GetIActorSystem()->GetActor(m_focalEntityId)))
                {
                    EStance playerStance = pPlayer->GetStance();
                    focalAABB = pPlayer->GetStanceInfo(playerStance)->GetStanceBounds();
                }
            else
                {
                    pFocalEnt->GetLocalBounds(focalAABB);
                }
            focalOBB.SetOBBfromAABB(Quat(IDENTITY), focalAABB);

            // shift to match focus ent Center (taking into account crouch etc if player).
            float fVerticalAdjust = focalOBB.h.z;

            // Additionally.. if the new test pos *would* immediately penetrate the floor (assumption based on any part of the volume being < player AABB z min value)
            // shift it up.
            float fFloorPenetrationAdjust = 0.0f;
            AABB wEntABB;
            pEntity->GetLocalBounds(wEntABB);
            wEntABB.SetTransformedAABB(wMat,wEntABB);
            float fFloorClearance =  focalOBB.h.z - (wEntABB.GetSize().z * 0.5f);

            fFloorPenetrationAdjust += (0.0f - min(fFloorClearance, 0.0f));

            // Apply floor clearance + Vertical adjust
            Vec3 desiredPos = wMat.GetTranslation() + Vec3(0.0f,0.0f,(fFloorPenetrationAdjust) * kFloorAdjustConstant);
            desiredPos += (fVerticalAdjust * pFocalEnt->GetWorldTM().GetColumn2() * kFloorAdjustConstant);
            return desiredPos;
        }

    return wMat.GetTranslation();
}

void CBoundingContainer::HideContainedItems()
{
	IEntity* pEntity = GetEntity();
	if (pEntity)
	{
		// Proximity query all entities in area
		AABB aabbBounds(m_vBoundingMin, m_vBoundingMax);

		OBB	obbBounds;
		Matrix34	worldTM = pEntity->GetWorldTM();
		obbBounds.SetOBBfromAABB(Matrix33(worldTM), aabbBounds);
		aabbBounds.Reset();
		aabbBounds.SetAABBfromOBB(pEntity->GetWorldPos(), obbBounds);

		SEntityProximityQuery query;
		query.box = aabbBounds;
		gEnv->pEntitySystem->QueryProximity(query);
		const int iQueryCount = query.nCount;
		for (int i = 0; i < iQueryCount; ++i)
		{
			IEntity* pQueryEntity = query.pEntities[i];
			if (pQueryEntity)
			{
				const EntityId queryEntityId = pQueryEntity->GetId();
				
				if (!stl::find(m_hiddenEntities, queryEntityId)) // Only if not already hidden
				{
					// Make sure entity type should be hidden and entity pos is also inside, not just an intersection
					if (ShouldHide(queryEntityId, pQueryEntity) && aabbBounds.IsContainPoint(pQueryEntity->GetWorldPos()))
					{
						pQueryEntity->Hide(true);
						m_hiddenEntities.push_back(pQueryEntity->GetId());
					}
				}
			}
		}
	}
}

bool CIntersectionAssistanceUnit::GetHighestScoringLastKnownGoodPosition( const QuatT& baseOrientation, QuatT& outQuat ) const
{
    bool bFlippedIsBest = false;

    if(!m_lastKnownGoodPositions.empty())
        {
            // Higher is better
            float fBestScore = 0.0f;
            int bestIndex = -1;
            Vec3 vBaseUpDir = baseOrientation.q.GetColumn2().GetNormalized();
            for(uint8 i = 0; i < m_lastKnownGoodPositions.size(); ++i)
                {
                    const QuatT& qLastKnownGood = m_lastKnownGoodPositions[i];
                    if(IsPositionWithinAcceptedLimits(qLastKnownGood.t, baseOrientation.t, kDistanceTolerance))
                        {
                            // Generate [0.0f,1.0f] score for distance
                            const Vec3 distVec = (qLastKnownGood.t - baseOrientation.t);

                            const float length = max(distVec.GetLengthFast(),0.0001f);
                            const float distanceScore = max(1.0f - (length * kInverseDistanceTolerance) * kDistanceWeight, 0.0f);

                            Vec3 vUpDir		 = qLastKnownGood.q.GetColumn2();

                            const float regularOrientationScore = vBaseUpDir.Dot(vUpDir);
                            const float flippedOrientationScore = vBaseUpDir.Dot(-vUpDir);

                            float orientationScore = max(regularOrientationScore, flippedOrientationScore);
                            orientationScore *= kOrientationWeight;

                            const float fCandidateScore = distanceScore + orientationScore;

#ifndef _RELEASE
                            if(g_pGameCVars->pl_pickAndThrow.intersectionAssistDebugEnabled == 2)
                                {
                                    CryWatch("[INDEX(%d)] : D[%.3f] O[%.3f] T[%.3f] (%s)", i, distanceScore, orientationScore, fCandidateScore, flippedOrientationScore > regularOrientationScore ? "*F*" : "R");
                                }
#endif //#ifndef _RELEASE

                            if(fCandidateScore > fBestScore)
                                {
                                    bestIndex	 = i;
                                    fBestScore = fCandidateScore;
                                    bFlippedIsBest = (flippedOrientationScore > regularOrientationScore);
                                }
                        }
                }

            if(bestIndex >= 0)
                {
                    outQuat = m_lastKnownGoodPositions[bestIndex];
                    if(bFlippedIsBest)
                        {
                            Matrix34 wMat(outQuat);
                            Vec3 vFlippedUpDir = -outQuat.q.GetColumn2().GetNormalized();
                            Vec3 vForwardDir	 = outQuat.q.GetColumn1().GetNormalized();
                            Vec3 vSideDir			 = -outQuat.q.GetColumn0().GetNormalized();
                            Matrix34 wFlippedMat;
                            wFlippedMat = Matrix34::CreateFromVectors(vSideDir, vForwardDir, vFlippedUpDir, wMat.GetTranslation());
                            outQuat = QuatT(wFlippedMat);

                            // Adjust pos (rotating around OOBB centre effectively)
                            const IEntity* pSubjectEntity = gEnv->pEntitySystem->GetEntity(m_subjectEntityId);
                            if(pSubjectEntity)
                                {
                                    AABB entAABB;
                                    OBB  entOBB;
                                    pSubjectEntity->GetLocalBounds(entAABB);
                                    entOBB.SetOBBfromAABB(Quat(IDENTITY), entAABB);

                                    Vec3 Centre = wMat.TransformPoint(entOBB.c);
                                    Vec3 toCentre = Centre - outQuat.t;
                                    outQuat.t += (toCentre * 2.0f);
                                }
                        }

#ifndef _RELEASE
                    if(g_pGameCVars->pl_pickAndThrow.intersectionAssistDebugEnabled == 2)
                        {
                            m_currentBestIndex = bestIndex;
                            CryWatch("[BEST INDEX] : %d", bestIndex);
                        }
#endif // ifndef _RELEASE

                    return true;
                }
        }

#ifndef _RELEASE
    m_currentBestIndex = -1;
#endif // ifndef _RELEASE

    return false;
}

bool CIntersectionAssistanceUnit::TestForIntersectionAtLocation(const eTestMethod testMethod, const Matrix34& wMat, EntityId testEntityId, EntityId ignoreEnt, QuatT& outAdjustedResult, const bool bCentreOnFocalEnt /* = false */, bool bRenderOnFail /* = true */, const int index /* = -1*/)
{
    // Build an OOBB that surrounds this entity, test for intersection between that and world
    IEntity* pEntity = gEnv->pEntitySystem->GetEntity(testEntityId);
    if(pEntity)
        {
            IPhysicalEntity* pPhysical = pEntity->GetPhysics();
            if(pPhysical)
                {
                    OBB entOBB;
                    AABB entAABB;
                    pEntity->GetLocalBounds(entAABB);
                    entOBB.SetOBBfromAABB(Quat(IDENTITY), entAABB);

                    // Do Primitive world intersection
                    primitives::box physBox;
                    physBox.bOriented = 1;

                    // LSpace
                    physBox.center = entOBB.c;
                    physBox.Basis = entOBB.m33;
                    physBox.size.x = entOBB.h.x;
                    physBox.size.y = entOBB.h.y;
                    physBox.size.z = entOBB.h.z;

                    // WSpace
                    physBox.center					= wMat.TransformPoint(physBox.center);
                    physBox.Basis					  *= Matrix33(wMat).GetInverted();

                    // Optional tweak - We can get away with a little bit of scaling down (if edges are slightly embedded the physics pushes them out easily)
                    physBox.size = physBox.size.scale(kPhysBoxScaleFactor);

                    // adjust
                    Vec3 vAdjustments(0.0f,0.0f,0.0f);
                    if(bCentreOnFocalEnt && m_focalEntityId)
                        {
                            Vec3 vDesiredPos = CalculateTargetAdjustPoint(pEntity, wMat, physBox.center);
                            vAdjustments = (vDesiredPos - physBox.center);
                            physBox.center += vAdjustments;
                        }

                    IEntity* pIgnoreEnt = gEnv->pEntitySystem->GetEntity(ignoreEnt);
                    IPhysicalEntity* pIgnorePhys = pIgnoreEnt ? pIgnoreEnt->GetPhysics() : NULL;

                    // Test
                    if(testMethod == eTM_Immediate
#ifndef _RELEASE
                            || g_pGameCVars->pl_pickAndThrow.intersectionAssistDebugEnabled >= 1
#endif // #ifndef _RELEASE
                      )
                        {
                            geom_contact *contacts;
                            intersection_params params;
                            float numHits = gEnv->pPhysicalWorld->PrimitiveWorldIntersection(primitives::box::type, &physBox, Vec3(ZERO),
                                            ent_static|ent_terrain, &contacts, 0,
                                            3, &params, 0, 0, &pIgnorePhys, pIgnorePhys ? 1 : 0);

                            // Debug
#ifndef _RELEASE
                            if(g_pGameCVars->pl_pickAndThrow.intersectionAssistDebugEnabled)
                                {

                                    const bool bIntersect = numHits <= 0.0f ? false : true;
                                    if(bRenderOnFail || !bIntersect)
                                        {
                                            const ColorB colorPositive = ColorB(16, 96, 16);
                                            const ColorB colorNegative = ColorB(128, 0, 0);
                                            const ColorB colorSelected = ColorB(0,255,0);

                                            if(numHits > 0.0f)
                                                {
                                                    gEnv->pRenderer->GetIRenderAuxGeom()->DrawSphere(contacts->pt, 0.1f, colorPositive);
                                                }

                                            OBB finalOBB;
                                            finalOBB.SetOBB(Matrix33(IDENTITY), physBox.size, Vec3(0.0f,0.0f,0.0f));
                                            Matrix34 drawMat = wMat;
                                            drawMat.AddTranslation(physBox.center - wMat.GetTranslation());
                                            if(index != -1 && index == m_currentBestIndex)
                                                {
                                                    gEnv->pRenderer->GetIRenderAuxGeom()->DrawOBB(finalOBB, drawMat, false, colorSelected, eBBD_Faceted);
                                                }
                                            else
                                                {
                                                    gEnv->pRenderer->GetIRenderAuxGeom()->DrawOBB(finalOBB, drawMat, false, bIntersect ? colorNegative : colorPositive, eBBD_Faceted);
                                                }

                                        }
                                }
#endif //#ifndef RELEASE

                            // If we performed an adjust, make sure we pass out the QuatT representing the FINAL ENTITY POSITION that passed/failed (not the phys box etc)
                            outAdjustedResult.t = wMat.GetTranslation() + vAdjustments;
                            outAdjustedResult.q = Quat(wMat);

#ifndef _RELEASE
                            // allow optional debug drawing of last known good positions by retaining non adjusted position
                            if(g_pGameCVars->pl_pickAndThrow.intersectionAssistDebugEnabled == 1)
                                {
                                    outAdjustedResult.t = wMat.GetTranslation();
//.........这里部分代码省略.........

//.........这里部分代码省略.........
		Limit(offset.z, targetOffsetHeight - 2.f, targetOffsetHeight + 2.f);
		float verticalChange = offset.z - oldOffsetHeight;

		m_lastOffsetBeforeElev = offset;

		// Add up and down camera tilt
		{
			offset.z     -= verticalChange;
			m_rotation.x += dt * m_stickSensitivity.x * m_rotatingAction.x;
			m_rotation.x  = clamp_tpl(m_rotation.x, -maxRotation.x, +maxRotation.x);

			float elevAngleVehicle = m_inheritedElev * yAxis.z;     // yAxis.z == approx elevation angle

			float elevationAngle = m_rotation.x - elevAngleVehicle;

			float sinElev, cosElev;
			sincos_tpl(elevationAngle, &sinElev, &cosElev);
			float horizLen    = sqrtf(offset.GetLengthSquared2D());
			float horizLenNew = horizLen * cosElev - sinElev * offset.z;
			if (horizLen > 1e-4f)
			{
				horizLenNew /= horizLen;
				offset.x    *= horizLenNew;
				offset.y    *= horizLenNew;
				offset.z     = offset.z * cosElev + sinElev * horizLen;
			}
			offset.z += verticalChange;
		}

		if (!offset.IsValid()) offset = m_lastOffset;

		m_position = m_lookAt + offset;

		// Perform world intersection test.
		{
			// Initialise sphere and direction.
			primitives::sphere sphere;

			sphere.center = m_lookAt;
			sphere.r      = g_SteerCameraRadius;

			Vec3 direction = m_position - m_lookAt;

			// Calculate camera bounds.
			AABB localBounds;

			m_pVehicle->GetEntity()->GetLocalBounds(localBounds);

			const float cameraBoundsScale = 0.75f;

			localBounds.min *= cameraBoundsScale;
			localBounds.max *= cameraBoundsScale;

			OBB cameraBounds;

			Matrix34 worldTM = m_pVehicle->GetEntity()->GetWorldTM();

			cameraBounds.SetOBBfromAABB(Matrix33(worldTM), localBounds);

			// Try to find point on edge of camera bounds to begin swept sphere intersection test.
			Vec3 rayBoxIntersect;

			if (Intersect::Ray_OBB(Ray(m_position, -direction), worldTM.GetTranslation(), cameraBounds, rayBoxIntersect) > 0)
			{
				Vec3 temp = m_position - rayBoxIntersect;

				if (direction.Dot(temp) > 0.0f)
				{
					sphere.center = rayBoxIntersect;
					direction     = temp;
				}
			}

			// Perform swept sphere intersection test against world.
			geom_contact* pContact = NULL;

			IPhysicalEntity* pSkipEntities[10];

			float distance = gEnv->pPhysicalWorld->PrimitiveWorldIntersection(sphere.type, &sphere, direction, ent_static | ent_terrain | ent_rigid | ent_sleeping_rigid,
			                                                                  &pContact, 0, (geom_colltype_player << rwi_colltype_bit) | rwi_stop_at_pierceable, 0, 0, 0,
			                                                                  pSkipEntities, m_pVehicle->GetSkipEntities(pSkipEntities, 10));

			if (distance > 0.0f)
			{
				// Sweep intersects world so calculate new offset.
				offset = (sphere.center + (direction.GetNormalizedSafe() * distance)) - m_lookAt;
			}
		}

		Interpolate(m_lastOffset, offset, 10.f, dt);

		m_position = m_lookAt + m_lastOffset;
	}
	else
	{
		CRY_ASSERT_MESSAGE(0, "camera will fail because lookat position is invalid");
	}

	m_rotatingAction.zero();
}