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, ¶ms, 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();
}
#endif // #ifndef _RELEASE
return (numHits > 0.0f);
}
else
{
// QUEUE primitive intersection check
outAdjustedResult.t = wMat.GetTranslation() + vAdjustments;
outAdjustedResult.q = Quat(wMat);
CRY_ASSERT(index >= 0);
m_intersectionTester.DoCheck(index,physBox,outAdjustedResult,pIgnorePhys);
return false;
}
}
}
return false;
}
//------------------------------------------------------------------------
void CVehicleViewSteer::Update(float dt)
{
IEntity* pEntity = m_pVehicle->GetEntity();
assert(pEntity);
IVehicleMovement* pVehicleMovement = m_pVehicle->GetMovement();
if (pVehicleMovement == NULL)
return;
IPhysicalEntity* pPhysEntity = pEntity->GetPhysics();
if (!pPhysEntity)
return;
pe_status_dynamics dynStatus;
pPhysEntity->GetStatus(&dynStatus);
SMovementState movementState;
pVehicleMovement->GetMovementState(movementState);
const float pedal = pVehicleMovement->GetEnginePedal();
const float maxSpeed = movementState.maxSpeed;
const Matrix34 &pose = m_pAimPart ? m_pAimPart->GetWorldTM() : pEntity->GetWorldTM();
const Vec3 entityPos = pose.GetColumn3();
const Vec3 xAxis = pose.GetColumn0();
const Vec3 yAxis = pose.GetColumn1();
const Vec3 zAxis = pose.GetColumn2();
const float forwardSpeed = dynStatus.v.dot(yAxis);
const float speedNorm = clamp_tpl(forwardSpeed / maxSpeed, 0.0f, 1.0f);
const Vec3 maxRotation = m_maxRotation + speedNorm * (m_maxRotation2 - m_maxRotation);
CalcLookAt(pose);
if (m_lookAt.IsValid())
{
if (!m_lastOffset.IsValid())
{
m_position = pose * m_localSpaceCameraOffset;
m_lastOffset = m_position - m_lookAt;
m_lastOffsetBeforeElev = m_lastOffset;
}
Vec3 offset = m_lastOffsetBeforeElev;
if (pedal < 0.1f && forwardSpeed < 1.0f)
{
// Going Backwards
m_flags &= ~(eVCam_goingForwards | m_forwardFlags);
m_flags |= m_backwardsFlags;
}
if (offset.dot(yAxis) < 0.8f && forwardSpeed > 1.f)
{
// Going Forwards
m_flags &= ~m_backwardsFlags;
m_flags |= eVCam_goingForwards | m_forwardFlags;
}
float sensitivity = (1.f - speedNorm) * m_stickSensitivity.z + speedNorm * m_stickSensitivity2.z;
float rotate = -m_rotatingAction.z * sensitivity;
rotate = rotate * dt;
if (zAxis.z > 0.1f)
{
// Safe to update curYaw
Vec3 projectedX = xAxis;
projectedX.z = 0.f;
Vec3 projectedY = yAxis;
projectedY.z = 0.f;
const float newYaw = atan2_tpl(offset.dot(projectedX), -(offset.dot(projectedY)));
const float maxChange = DEG2RAD(270.f) * dt;
const float delta = clamp_tpl(newYaw - m_curYaw, -maxChange, +maxChange);
m_curYaw += delta;
}
// Rotation Action
{
if (m_flags & eVCam_rotationClamp)
{
float newYaw = clamp_tpl(m_curYaw + rotate, -maxRotation.z, +maxRotation.z);
rotate = newYaw - m_curYaw;
rotate = clamp_tpl(newYaw - m_curYaw, -fabsf(rotate), +fabsf(rotate));
m_rotation.z += rotate;
}
else
{
m_rotation.z = 0.f;
}
if (speedNorm > 0.1f)
{
float reduce = dt * 1.f;
m_rotation.z = m_rotation.z - reduce * m_rotation.z / (fabsf(m_rotation.z) + reduce);
}
}
// Ang Spring
{
float angSpeedCorrection = dt * dt * m_angSpeedCorrection / (dt * m_angSpeedCorrection + 1.f) * dynStatus.w.z;
if ((m_flags & eVCam_rotationSpring) == 0)
{
m_angReturnSpeed = 0.f;
angSpeedCorrection = 0.f;
}
float difference = m_rotation.z - m_curYaw;
float relax = difference * (m_angReturnSpeed * dt) / ((m_angReturnSpeed * dt) + 1.f);
const float delta = +relax + angSpeedCorrection + rotate;
m_curYaw += delta;
Matrix33 rot = Matrix33::CreateRotationZ(delta);
offset = rot * offset;
// Lerp the spring speed
float angSpeedTarget = m_angReturnSpeed1 + speedNorm * (m_angReturnSpeed2 - m_angReturnSpeed1);
m_angReturnSpeed += (angSpeedTarget - m_angReturnSpeed) * (dt / (dt + 0.3f));
m_angSpeedCorrection += (m_angSpeedCorrection0 - m_angSpeedCorrection) * (dt / (dt + 0.3f));
}
if (!offset.IsValid()) offset = m_lastOffset;
// Velocity influence
Vec3 displacement = -((2.f - speedNorm) * dt) * dynStatus.v;// - yAxis*(0.0f*speedNorm*(yAxis.dot(dynStatus.v))));
float dot = offset.dot(displacement);
if (dot < 0.f)
{
displacement = displacement + offset * -0.1f * (offset.dot(displacement) / offset.GetLengthSquared());
}
offset = offset + displacement;
const float radius0 = fabsf(m_localSpaceCameraOffset.y);
const float minRadius = radius0 * m_radiusMin;
const float maxRadius = radius0 * m_radiusMax;
float radiusXY = sqrtf(sqr(offset.x) + sqr(offset.y));
Vec3 offsetXY = offset;
offsetXY.z = 0.f;
Vec3 accelerationV = (dynStatus.v - m_lastVehVel);
float acceleration = offsetXY.dot(accelerationV) / radiusXY;
m_lastVehVel = dynStatus.v;
m_radiusVel -= acceleration;
m_radius += m_radiusVel * dt - dt * m_radiusVelInfluence * offsetXY.dot(dynStatus.v) / radiusXY;
m_radiusVel *= expf(-dt * m_radiusDampRate);
m_radius += (radius0 - m_radius) * (dt * m_radiusRelaxRate) / (dt * m_radiusRelaxRate + 1.f);
m_radius = clamp_tpl(m_radius, minRadius, maxRadius);
offset = offset * (m_radius / radiusXY);
// Vertical motion
float targetOffsetHeight = m_localSpaceCameraOffset.z * (m_radius / radius0);
float oldOffsetHeight = offset.z;
offset.z += (targetOffsetHeight - offset.z) * (dt / (dt + 0.3f));
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();
}
