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 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));
}