///
/// \brief
/// Get a Havok Physics world space vector from a Vision render space vector; taking Vision scaling into account.
///
VHAVOK_IMPEXP static HK_FORCE_INLINE void VisVecToPhysVecWorld(const hkvVec4& visV, hkVector4& physV)
{
physV.load<4, HK_IO_NATIVE_ALIGNED>(&visV.data[0]);
physV.mul(m_cachedVis2PhysScale);
physV.add(*m_cachedWorldPivot);
HK_ASSERT(0xdee884, physV.isOk<4>());
}
void DebugDrawManager::add_quad(const hkVector4& center, float width, float height, uint32_t color, bool bDepth)
{
float x = center.getSimdAt(0);
float y = center.getSimdAt(1);
float z = center.getSimdAt(2);
hkVector4 v0, v1, v2, v3;
v0.set(x-width/2, y, z-height/2);
v1.set(x+width/2, y, z-height/2);
v2.set(x+width/2, y, z+height/2);
v3.set(x-width/2, y, z+height/2);
add_line(v0, v1, color, bDepth);
add_line(v1, v2, color, bDepth);
add_line(v2, v3, color, bDepth);
add_line(v3, v0, color, bDepth);
}
///
/// \brief
/// Get a Havok Physics object space/scalar vector from a Vision vector; taking Vision scaling into account.
///
VHAVOK_IMPEXP static HK_FORCE_INLINE void VisVecToPhysVecLocal(const hkvVec3& visV, hkVector4& physV)
{
physV.load<3, HK_IO_NATIVE_ALIGNED>(&visV.data[0]);
physV.zeroComponent<3>();
physV.mul(m_cachedVis2PhysScale);
HK_ASSERT(0xdee883, physV.isOk<4>());
}
void DemoKeeper::getKeyframePositionAndRotation( hkReal t, hkVector4& pos, hkQuaternion& rot )
{
t *= 2 * HK_REAL_PI * m_speed;
pos.set(m_radius * hkMath::cos(t), 10-2.0f, -m_radius * hkMath::sin(t));
hkVector4 axis(0,1,0);
rot.setAxisAngle(axis, t);
}
// Apply a force which is scaled depending on whether or not it's our character controller.
// The character controller's special gravity implementation means that it needs a greater
// force to get the same effect.
void PlanetGravityDemo::applyScaledLinearImpulse( hkpRigidBody* rb, hkVector4& impulse )
{
if( rb != m_characterRigidBody->getRigidBody() )
{
impulse.mul4( 0.3f );
}
rb->applyLinearImpulse( impulse );
}
///
/// \brief
/// Get a Havok Physics object space/scalar vector from a Vision vector; taking Vision scaling into account.
///
VHAVOK_IMPEXP static HK_FORCE_INLINE void VisVecToPhysVecLocal(const hkvVec3d& visV, hkVector4& physV)
{
/*! Work around, see [SIM-41] */
AlignedArray array(visV);
physV.load<3, HK_IO_NATIVE_ALIGNED>(array.m_array);
physV.zeroComponent<3>();
physV.mul(m_cachedVis2PhysScale);
HK_ASSERT(0xdee883, physV.isOk<4>());
}
bool PlayerUIDialog::GetClosestPointOnNavMeshUnderCursor(hkVector4& point, hkVector4 const& searchPoint)
{
IVGUIContext *const context = GetContext();
VASSERT(context);
hkvVec2 const mousePos = GetCursorPosition(context);
hkvVec3 traceDirOut;
VisRenderContext_cl::GetCurrentContext()->GetTraceDirFromScreenPos(mousePos.x, mousePos.y, traceDirOut, 1.0f);
hkvVec3 const& camPos = VisRenderContext_cl::GetCurrentContext()->GetCamera()->GetPosition();
hkVector4 rayStartHavok, rayEndHavok;
RPG_VisionHavokConversion::VisionToHavokPoint(camPos, rayStartHavok);
RPG_VisionHavokConversion::VisionToHavokPoint(camPos + traceDirOut * 5000.0f, rayEndHavok);
hkaiNavMeshQueryMediator::HitDetails hit;
if(vHavokAiModule::GetInstance()->GetAiWorld()->getDynamicQueryMediator()->castRay(rayStartHavok, rayEndHavok, hit))
{
point.setInterpolate(rayStartHavok, rayEndHavok, hit.m_hitFraction);
return true;
}
else
{
// If the ray missed the nav mesh:
// 1. Find the nav mesh face closest to the search point
// 2. Intersect the ray with the plane of this face
// 3. Find the closest point on the nav mesh to the plane point
hkVector4 dummyPoint;
hkaiPackedKey const searchFaceKey =
vHavokAiModule::GetInstance()->GetAiWorld()->getDynamicQueryMediator()->getClosestPoint(searchPoint, 1.f, dummyPoint);
if(searchFaceKey != HKAI_INVALID_PACKED_KEY)
{
hkVector4 facePlane;
hkaiNavMeshInstance const *meshInstance = vHavokAiModule::GetInstance()->GetAiWorld()->getStreamingCollection()->getInstanceAt(hkaiGetRuntimeIdFromPacked(searchFaceKey));
{
hkaiNavMeshUtils::calcFacePlane(*meshInstance, hkaiGetIndexFromPacked(searchFaceKey), facePlane);
}
hkSimdReal const f = facePlane.dot4xyz1(rayStartHavok);
hkSimdReal const t = facePlane.dot4xyz1(rayEndHavok);
if(f.isGreaterEqualZero() && t.isLessZero())
{
hkVector4 planePoint;
{
hkSimdReal const hitFraction = f / (f - t);
planePoint.setInterpolate(rayStartHavok, rayEndHavok, hitFraction);
}
hkaiPackedKey faceKey = vHavokAiModule::GetInstance()->GetAiWorld()->getDynamicQueryMediator()->getClosestPoint(planePoint, 5.f, point);
return (faceKey != HKAI_INVALID_PACKED_KEY);
}
}
}
return false;
}
// little ik
void SpiderDemo::doLegIk( const SpiderDemo::calcLegMatrizesIn& in, hkVector4& pivotOut, hkRotation& mAOut, hkRotation& mBOut )
{
hkVector4 dir; dir.setSub4( in.m_to, in.m_from );
hkReal tdist = dir.length3();
if ( tdist >= in.m_lenA + in.m_lenB )
{
hkVector4Util::buildOrthonormal( dir, in.m_up, mAOut );
mBOut = mAOut;
dir.normalize3();
pivotOut.setAddMul4( in.m_from, dir, in.m_lenA );
return;
}
dir.normalize3();
hkReal la2 = in.m_lenA*in.m_lenA;
hkReal lb2 = in.m_lenB*in.m_lenB;
hkReal adist = 0.5f * (la2- lb2 + tdist*tdist) / tdist;
hkReal hdist = hkMath::sqrt( la2 - adist*adist );
hkVector4 cup;
{
hkVector4 h; h.setCross( dir, in.m_up );
cup.setCross( h, dir );
cup.normalize3();
}
{
pivotOut.setAddMul4( in.m_from, dir, adist );
pivotOut.addMul4( hdist, cup );
}
{
hkVector4 d; d.setSub4( pivotOut, in.m_from );
d.normalize3();
hkVector4Util::buildOrthonormal( d, in.m_up, mAOut );
}
{
hkVector4 d; d.setSub4( in.m_to, pivotOut );
d.normalize3();
hkVector4Util::buildOrthonormal( d, in.m_up, mBOut );
}
}
void DestructibleWallsDemo::posOnGround(hkpWorld* world, const hkVector4& pos, hkVector4& newPos) const
{
// cast a ray to find exact position
hkpWorldRayCastInput ray;
ray.m_from = pos;
ray.m_to = pos;
ray.m_from(1) += 20.0f;
ray.m_to(1) -= 20.0f;
hkpWorldRayCastOutput rayOutput;
world->castRay(ray,rayOutput);
newPos.setInterpolate4(ray.m_from, ray.m_to, rayOutput.m_hitFraction);
}
void DiskEmissionUtil::samplePosition(hkVector4& position, hkPseudoRandomGenerator* pseudoRandomGenerator) const
{
// Pick a random point on the disk (see http://mathworld.wolfram.com/DiskPointPicking.html)
hkReal radius = m_radius * hkMath::sqrt(pseudoRandomGenerator->getRandReal01());
hkReal theta = pseudoRandomGenerator->getRandReal01() * 2.f * HK_REAL_PI;
hkReal diskX = radius * hkMath::cos(theta);
hkReal diskY = radius * hkMath::sin(theta);
hkReal offsetX = pseudoRandomGenerator->getRandRange(-m_outOfPlaneVariance, m_outOfPlaneVariance);
hkReal offsetY = diskY + pseudoRandomGenerator->getRandRange(-m_inPlaneVariance, m_inPlaneVariance);
hkReal offsetZ = diskX + pseudoRandomGenerator->getRandRange(-m_inPlaneVariance, m_inPlaneVariance);
hkVector4 localOffset(offsetX, offsetY, offsetZ);
m_orthonormalBasis.multiplyVector(localOffset, position);
position.add4(m_position);
}
void DebugDrawManager::add_aabb( const hkVector4& min, const hkVector4& max, uint32_t color, bool bDepth)
{
hkVector4 start, end;
//1.
start.set(min.getSimdAt(0), min.getSimdAt(1), min.getSimdAt(2));
end.set(max.getSimdAt(0), min.getSimdAt(1), min.getSimdAt(2));
add_line(start, end, color, bDepth);
//2.
start.set(max.getSimdAt(0), min.getSimdAt(1), min.getSimdAt(2));
end.set(max.getSimdAt(0), max.getSimdAt(1), min.getSimdAt(2));
add_line(start, end, color, bDepth);
//3.
start.set(max.getSimdAt(0), max.getSimdAt(1), min.getSimdAt(2));
end.set(min.getSimdAt(0), max.getSimdAt(1), min.getSimdAt(2));
add_line(start, end, color, bDepth);
//4.
start.set(min.getSimdAt(0), max.getSimdAt(1), min.getSimdAt(2));
end.set(min.getSimdAt(0), min.getSimdAt(1), min.getSimdAt(2));
add_line(start, end, color, bDepth);
//5.
start.set(min.getSimdAt(0), min.getSimdAt(1), min.getSimdAt(2));
end.set(min.getSimdAt(0), min.getSimdAt(1), max.getSimdAt(2));
add_line(start, end, color, bDepth);
//6.
start.set(max.getSimdAt(0), min.getSimdAt(1), min.getSimdAt(2));
end.set(max.getSimdAt(0), min.getSimdAt(1), max.getSimdAt(2));
add_line(start, end, color, bDepth);
//7.
start.set(max.getSimdAt(0), max.getSimdAt(1), min.getSimdAt(2));
end.set(max.getSimdAt(0), max.getSimdAt(1), max.getSimdAt(2));
add_line(start, end, color, bDepth);
//8.
start.set(min.getSimdAt(0), max.getSimdAt(1), min.getSimdAt(2));
end.set(min.getSimdAt(0), max.getSimdAt(1), max.getSimdAt(2));
add_line(start, end, color, bDepth);
//9.
start.set(min.getSimdAt(0), min.getSimdAt(1), max.getSimdAt(2));
end.set(max.getSimdAt(0), min.getSimdAt(1), max.getSimdAt(2));
add_line(start, end, color, bDepth);
//10.
start.set(max.getSimdAt(0), min.getSimdAt(1), max.getSimdAt(2));
end.set(max.getSimdAt(0), max.getSimdAt(1), max.getSimdAt(2));
add_line(start, end, color, bDepth);
//11.
start.set(max.getSimdAt(0), max.getSimdAt(1), max.getSimdAt(2));
end.set(min.getSimdAt(0), max.getSimdAt(1), max.getSimdAt(2));
add_line(start, end, color, bDepth);
//12.
start.set(min.getSimdAt(0), max.getSimdAt(1), max.getSimdAt(2));
end.set(min.getSimdAt(0), min.getSimdAt(1), max.getSimdAt(2));
add_line(start, end, color, bDepth);
}
static void SetTransformPosition(hkQsTransform& transform, hkVector4& p)
{
if (p.getSimdAt(0) != FloatNegINF) transform.setTranslation(p);
}
void AabbSpawnUtil::getNewSpawnPosition(const hkVector4& aabbDims, hkVector4& positionOut)
{
// Try each volume in turn
while (1)
{
hkVector4 avaliableDiag; avaliableDiag.setSub4( m_spawnVolumes[m_currentSpawnVolume].m_max, m_spawnVolumes[m_currentSpawnVolume].m_min );
avaliableDiag.sub4(aabbDims);
if ( avaliableDiag(0) < 0 || avaliableDiag(2) < 0 )
{
HK_ASSERT2(0, 0, "No spawn space large enough for requested aabb");
return;
}
// Try 100 random positions in the volume
int numTries = m_allowOverlaps ? 1 : 100;
for (int j = 0; j < numTries; ++j)
{
hkVector4 minPos(m_pseudoRandomGenerator.getRandReal01(), m_pseudoRandomGenerator.getRandReal01(), m_pseudoRandomGenerator.getRandReal01() );
if (avaliableDiag(1) < 0)
{
minPos(1) = 0;
}
minPos.mul4(avaliableDiag);
minPos.add4(m_spawnVolumes[m_currentSpawnVolume].m_min);
hkVector4 maxPos; maxPos.setAdd4( minPos, aabbDims );
hkAabb candidate( minPos, maxPos );
bool aabbCollides = false;
if (!m_allowOverlaps)
{
for ( int k = 0; k < m_spawnedAabbs[m_currentSpawnVolume].getSize(); k++ )
{
if ( m_spawnedAabbs[m_currentSpawnVolume][k].overlaps(candidate))
{
aabbCollides = true;
break;
}
}
}
if ( !aabbCollides )
{
m_spawnedAabbs[m_currentSpawnVolume].pushBack(candidate);
hkVector4 position; positionOut.setInterpolate4( candidate.m_min, candidate.m_max, .5f );
// If we allow penetrations, take each spawn volume in turn
if ( m_allowOverlaps )
{
m_currentSpawnVolume++;
if (m_currentSpawnVolume == m_spawnVolumes.getSize())
{
m_currentSpawnVolume = 0;
}
}
return;
}
}
// If we couldn't find a space, try the next volume
m_currentSpawnVolume++;
if (m_currentSpawnVolume == m_spawnVolumes.getSize())
{
m_currentSpawnVolume = 0;
m_allowOverlaps = true;
}
}
}
