// 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 OBB::getInterval(const OBB& box, const Vec3& axis, float &min, float &max)const
{
Vec3 corners[8];
box.getCorners(corners);
float value;
min = max = projectPoint(axis, corners[0]);
for(int i = 1; i < 8; i++)
{
value = projectPoint(axis, corners[i]);
min = MIN(min, value);
max = MAX(max, value);
}
}
void const SphereCollider::render()
{
glLoadIdentity();
getGameObject()->getTransform()->apply();
glColor4f(1,0,0,0.7);
glEnable(GL_BLEND);
glBlendFunc(GL_SRC_ALPHA, GL_ONE_MINUS_SRC_ALPHA);
glEnable(GL_POINT_SMOOTH);
glPointSize(2.0 * radius);
glBegin(GL_POINTS);
glVertex3f(center.x,center.y,-1);
glEnd();
point* points = sweep();
OBB* swept = new OBB(points);
swept->render();
delete swept;
delete points;
}
OBB Frustum::MinimalEnclosingOBB(float expandGuardband) const
{
assume(IsFinite());
assume(front.IsNormalized());
assume(up.IsNormalized());
OBB obb;
obb.pos = pos + (nearPlaneDistance + farPlaneDistance) * 0.5f * front;
obb.axis[1] = up;
obb.axis[2] = -front;
obb.axis[0] = Cross(obb.axis[1], obb.axis[2]);
obb.r = float3::zero;
for(int i = 0; i < 8; ++i)
obb.Enclose(CornerPoint(i));
// Expand the generated OBB very slightly to avoid numerical issues when
// testing whether this Frustum actually is contained inside the generated OBB.
obb.r.x += expandGuardband;
obb.r.y += expandGuardband;
obb.r.z += expandGuardband;
return obb;
}
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 BasicPrimitiveTests::IntersectingRayAgainstOBB(const Ray & ray, const OBB & obb, float & rtn_t)
{
Eigen::Matrix3f obb_rotation;
obb.GetRotationMatrix(obb_rotation);
Eigen::Vector3f ray_d_obb = obb_rotation * ray.GetDirection();
Eigen::Vector3f ray_o_obb = obb_rotation * (ray.GetOrigin() - obb.GetCenter());
rtn_t = 0.0f;
float tmax = FLT_MAX;
Eigen::Vector3f aabb_min = -obb.GetExtents();
Eigen::Vector3f aabb_max = obb.GetExtents();
for (int i = 0; i < 3; ++i)
{
if (abs(ray_d_obb[i]) < EPSILON)
{
//Ray is parallel to slab. Not hit if origin not within slab.
if (ray_o_obb[i] < aabb_min[i] || ray_o_obb[i] > aabb_max[i])
{
return false;
}
}
else
{
float one_over_direction = 1.0f / ray_d_obb[i];
float t1 = (aabb_min[i] - ray_o_obb[i]) * one_over_direction;
float t2 = (aabb_max[i] - ray_o_obb[i]) * one_over_direction;
if (t1 > t2) Swap(t1, t2);
if (t1 > rtn_t) rtn_t = t1;
if (t2 > tmax) tmax = t2;
if (rtn_t > tmax) return false;
}
}
return true;
}
OBB AABB::Transform(const Quat &transform) const
{
OBB obb;
obb.SetFrom(*this, transform);
return obb;
}
OBB AABB::Transform(const float4x4 &transform) const
{
OBB obb;
obb.SetFrom(*this, transform);
return obb;
}
void AABB::Init(const OBB& shp) { Max = vec3(shp.XSize(), shp.YSize(), shp.ZSize()); Min = -Max;}
void Sphere::Enclose(const OBB &obb)
{
///@todo This might not be very optimal at all. Perhaps better to enclose the farthest point first.
for(int i = 0; i < 8; ++i)
Enclose(obb.CornerPoint(i));
}
bool Polygon::Intersects(const OBB &obb) const
{
return obb.Intersects(*this);
}
float float3::Distance(const OBB &rhs) const { return rhs.Distance(*this); }
float Sphere::Distance(const OBB &obb) const
{
return obb.Distance(*this);
}
OBB RigidBox::GetOBBWorldSpace()
{
OBB obb;
obb.ObjectTransform(m_obb, m_worldMat);
return obb;
}
//------------------------------------------------------------------------
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();
}
void CExactPositioningTrigger::Update( float frameTime, Vec3 userPos, Quat userOrient, bool allowTriggering )
{
if (m_state == eS_Invalid)
return;
CRY_ASSERT(m_pos.IsValid());
CRY_ASSERT(m_userPos.IsValid());
CRY_ASSERT(m_orient.IsValid());
CRY_ASSERT(m_userOrient.IsValid());
CRY_ASSERT(m_posSize.IsValid());
CRY_ASSERT(NumberValid(m_cosOrientTolerance));
CRY_ASSERT(NumberValid(frameTime));
CRY_ASSERT(userPos.IsValid());
CRY_ASSERT(userOrient.IsValid());
m_userPos = userPos;
m_userOrient = userOrient;
if (m_state == eS_Initializing)
m_state = eS_Before;
Plane threshold;
threshold.SetPlane( m_orient.GetColumn1(), m_pos );
if (threshold.DistFromPlane(userPos) >= 0.0f)
{
if (m_sideTime < 0.0f)
m_sideTime = 0.0f;
else
m_sideTime += frameTime;
}
else
{
if (m_sideTime > 0.0f)
m_sideTime = 0.0f;
else
m_sideTime -= frameTime;
}
Vec3 curDir = userOrient.GetColumn1();
Vec3 wantDir = m_orient.GetColumn1();
if (m_state == eS_Before)
{
OBB triggerBox;
triggerBox.SetOBB( Matrix33(m_orient), m_posSize+Vec3(0.5f,0.5f,0), ZERO );
if (Overlap::Point_OBB(m_userPos, m_pos, triggerBox))
m_state = eS_Optimizing;
}
if ((m_state == eS_Optimizing) && allowTriggering)
{
#ifdef INCLUDE_EXACTPOS_DEBUGGING
bool debug = (CAnimationGraphCVars::Get().m_debugExactPos != 0);
CPersistantDebug* pPD = CCryAction::GetCryAction()->GetPersistantDebug();
#endif
Vec3 bump(0.0f, 0.0f, 0.1f);
Vec3 posDistanceError = m_userPos - m_pos;
if ( posDistanceError.z > -1.0f && posDistanceError.z < 1.0f )
posDistanceError.z = 0;
Vec3 orientFwd = m_orient.GetColumn1(); orientFwd.z = 0.0f; orientFwd.Normalize();
Vec3 rotAnimMovementWanted = orientFwd * m_animMovementLength;
Vec3 userFwd = m_userOrient.GetColumn1(); userFwd.z = 0.0f; userFwd.Normalize();
Vec3 rotAnimMovementUser = userFwd * m_animMovementLength;
float cosRotError = orientFwd.Dot( userFwd );
float rotError = CLAMP(m_cosOrientTolerance - cosRotError, 0.0f, 1.0f);
//Vec3 rotDistanceError = rotAnimMovementUser - rotAnimMovementWanted;
float fwdDistance = fabsf(orientFwd.Dot( posDistanceError ));
float sideDistance = max( 0.0f, sqrtf( MAX(0,posDistanceError.GetLengthSquared2D() - sqr(fwdDistance)) ) - m_width );
float deltaFwd = m_oldFwdDir < fwdDistance ? fwdDistance - m_oldFwdDir : 0.0f;
m_oldFwdDir = fwdDistance;
fwdDistance += deltaFwd * 0.5f;
deltaFwd = max(0.1f, deltaFwd);
f32 distanceError = sqrtf(sqr(fwdDistance) + sqr(sideDistance)); // posDistanceError.len() * m_distanceErrorFactor;
f32 temp = 1.0f-sqr(1.0f-rotError*rotError);
temp = max(temp,0.0f); //never do a sqrtf with a negative value
f32 orientError = sqrtf(temp) * m_animMovementLength; // rotDistanceError.len();
f32 totalDistanceError = distanceError + orientError;
if (((m_distanceError * 1.05f) < distanceError) && ((m_orientError * 1.05f) < orientError) && (totalDistanceError < deltaFwd) ||
(totalDistanceError < deltaFwd*0.5f))
{ // found local minimum in distance error, force triggering.
m_state = eS_Triggered;
m_oldFwdDir = 0.0f;
#ifdef INCLUDE_EXACTPOS_DEBUGGING
if (debug)
{
pPD->Begin("AnimationTrigger LocalMinima Triggered", false);
pPD->AddPlanarDisc(m_pos + bump, 0.0f, m_distanceError, ColorF(0,1,0,0.5), 10.0f);
}
#endif
}
else
{
m_distanceError = m_distanceError > distanceError ? distanceError : m_distanceError * 0.999f; // should timeout in ~2 secs. on 50 FPS
m_orientError = m_orientError > orientError ? orientError : m_orientError - 0.0001f;
#ifdef INCLUDE_EXACTPOS_DEBUGGING
if (debug)
{
pPD->Begin("AnimationTrigger LocalMinima Optimizing", true);
pPD->AddPlanarDisc(m_pos + bump, 0.0f, m_distanceError, ColorF(1,1,0,0.5), 10.0f);
}
#endif
}
#ifdef INCLUDE_EXACTPOS_DEBUGGING
if (debug)
{
pPD->AddLine(m_userPos + bump, m_pos + bump, ColorF(1,0,0,1), 10.0f);
pPD->AddLine(m_userPos + rotAnimMovementUser + bump, m_pos + rotAnimMovementWanted + bump, ColorF(1,0,0,1), 10.0f);
pPD->AddLine(m_pos + bump, m_pos + rotAnimMovementWanted + bump, ColorF(1,0.5,0,1), 10.0f);
pPD->AddLine(m_userPos + bump, m_pos + rotAnimMovementUser + bump, ColorF(1,0.5,0,1), 10.0f);
}
#endif
}
CRY_ASSERT(m_pos.IsValid());
CRY_ASSERT(m_userPos.IsValid());
CRY_ASSERT(m_orient.IsValid());
CRY_ASSERT(m_userOrient.IsValid());
CRY_ASSERT(m_posSize.IsValid());
CRY_ASSERT(NumberValid(m_cosOrientTolerance));
}
bool Plane::Intersects(const OBB &obb) const
{
return obb.Intersects(*this);
}
bool const OBB::overlaps(OBB& other)
{
if((overlapsOneWay(other)==true)&&(other.overlapsOneWay(*this)))
return true;
return false;
}
void AABB::Enclose(const OBB &obb)
{
for(int i = 0; i < 8; ++i)
Enclose(obb.CornerPoint(i));
}
bool AABB::Contains(const OBB &obb) const
{
return Contains(obb.MinimalEnclosingAABB());
}
bool AABB::Intersects(const OBB &obb) const
{
return obb.Intersects(*this);
}
bool Triangle::Intersects(const OBB &obb) const
{
return obb.Intersects(*this);
}
bool Sphere::Intersects(const OBB &obb, vec *closestPointOnOBB) const
{
return obb.Intersects(*this, closestPointOnOBB);
}
bool Ray::Intersects(const OBB &obb, float *dNear, float *dFar) const
{
return obb.Intersects(*this, dNear, dFar);
}
void GraphicsWorld::DebugDrawOBB(const OBB &obb, const Color &clr, bool depthTest)
{
for(int i = 0; i < 12; ++i)
DebugDrawLineSegment(obb.Edge(i), clr, depthTest);
}
bool Capsule::Intersects(const OBB &obb) const
{
return Intersects(obb.ToPolyhedron());
}
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;
}
bool Frustum::collision(const CollisionObject * pCollisionObject, bool calculateNormal)
{
calculateNormal;
if(pCollisionObject->getCollisionType()==CollisionType_Sphere)
{
Sphere * pSphere = (Sphere*) pCollisionObject;
float x = pSphere->getX();
float y = pSphere->getY();
float z = pSphere->getZ();
float r = pSphere->getR();
for(int i=0;i<6;i++)
{
if(mPlanes[i].a * x + mPlanes[i].b * y + mPlanes[i].c * z + mPlanes[i].d <= -r)
{
return false;
}
}
return true;
}
else if(pCollisionObject->getCollisionType()==CollisionType_AABB)
{
AABB * pAABB = (AABB*) pCollisionObject;
for(unsigned int j=0;j<6;j++)
{
unsigned int cornersInside = 8;
for(unsigned int i=0;i<8;i++)
{
D3DXVECTOR3 corner = pAABB->getCorner(i);
/*
if(mPlanes[j].a * corner.x + mPlanes[j].b * corner.y + mPlanes[j].c * corner.z + mPlanes[j].d <= 0)
{
cornersInside--;
}
*/
if(D3DXPlaneDotNormal(&mPlanes[j], &corner) + mPlanes[j].d <= 0)
{
cornersInside--;
}
}
if(cornersInside==0)
{
return false;
}
}
return true;
}
if(pCollisionObject->getCollisionType()==CollisionType_OBB)
{
OBB * pOBB = (OBB*)pCollisionObject;
for(unsigned int j=0;j<6;j++)
{
unsigned int cornersInside = 8;
for(unsigned int i=0; i<8; i++)
{
D3DXVECTOR3 corner = pOBB->getCorner(i);
if(D3DXPlaneDotNormal(&mPlanes[j], &corner) + mPlanes[j].d <= 0)
{
cornersInside--;
}
}
if(cornersInside==0)
{
return false;
}
}
return true;
}
return false;
}
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 Line::Intersects(const OBB &obb, float &dNear, float &dFar) const
{
return obb.Intersects(*this, dNear, dFar);
}