void PlayerWalkAction::applyAction(const hkStepInfo& stepInfo)
{
HK_TIME_CODE_BLOCK("PlayerWalkAction::applyAction", HK_NULL);
hkpRigidBody* rb = static_cast<hkpRigidBody*>(m_entity);
hkQuaternion rotation;
rotation.setAxisAngle(m_axis, m_walkSpeed * stepInfo.m_deltaTime);
hkVector4 nextPos; nextPos.setRotatedDir(rotation, rb->getPosition());
hkpKeyFrameUtility::applyHardKeyFrame(nextPos, rb->getRotation(), stepInfo.m_invDeltaTime, rb);
}
void DoorSpring::applyAction(const hkStepInfo& stepInfo)
{
HK_TIME_CODE_BLOCK("DoorSpring::applyAction", HK_NULL);
float angle = hkpLimitedHingeConstraintData::getRuntime(m_hinge->getRuntime())->getCurrentPos();
if(hkMath::fabs(angle) < 0.001f )
{
return;
}
hkpRigidBody* rb = static_cast<hkpRigidBody*>(m_entity);
hkVector4 torque = m_axis;
m_angVel = rb->getAngularVelocity().dot3(torque);
m_opening = (m_angVel * angle) < 0.0f;
torque.mul4( (angle * m_strength) - (m_angVel * m_damping ) );
rb->applyTorque(stepInfo.m_deltaTime, torque);
m_torque = torque.length3();
}
void AabbQueryDemo::queryAabbMT()
{
HK_TIMER_BEGIN_LIST("queryAabbMT", "setup");
hkAabb aabb;
// Grab a body from the world, and compute it's AABB + some padding
const hkpCollidable* queryCollidable;
{
queryCollidable = m_collidables[m_collIdx];
hkpRigidBody* rb = hkGetRigidBody(queryCollidable);
queryCollidable->getShape()->getAabb(rb->getTransform(), 20.0f, aabb);
}
hkArray<hkpKdTreeAabbCommand> commands;
commands.setSize(numQueries);
//
// For performance timings, we query the same AABB multiple times and overwrite the results.
// When using this, make sure to use different output arrays!
//
hkArray<hkPrimitiveId> output;
output.setSize(50);
//
// Set up the commands for the job
//
for (int i=0; i<commands.getSize(); i++)
{
hkpKdTreeAabbCommand& command = commands[i];
command.m_aabb = aabb;
command.m_results = output.begin();
command.m_resultsCapacity = output.getSize();
command.m_numResultsOut = 0;
}
//
// Setup the job
//
hkArray<hkpRayCastQueryJobHeader> header(1);
hkpKdTreeAabbJob aabbJob(header.begin(), commands.begin(), commands.getSize(), &m_semaphore);
aabbJob.m_numTrees = 1;
aabbJob.m_trees[0] = m_world->m_kdTreeManager->getTree();
m_jobQueue->addJob( *reinterpret_cast<hkJobQueue::JobQueueEntry*>(&aabbJob), hkJobQueue::JOB_HIGH_PRIORITY );
HK_TIMER_SPLIT_LIST("process");
m_jobThreadPool->processAllJobs( m_jobQueue );
m_jobThreadPool->waitForCompletion();
m_semaphore.acquire();
HK_TIMER_END_LIST();
//
// Run the same query on the broadphase for comparison purposes
//
hkArray<hkpBroadPhaseHandlePair> sapHits;
{
HK_TIME_CODE_BLOCK("BroadphaseQueryAabb", HK_NULL);
for (int i=0; i<numQueries; i++)
{
sapHits.clear();
m_world->getBroadPhase()->querySingleAabb( aabb, sapHits );
}
}
//
// Check results and draw
//
for (int i=0; i<commands.getSize(); i++)
{
hkpKdTreeAabbCommand& command = commands[i];
hkBool jobOk = compareHitArrays(command.m_aabb, command.m_results, command.m_numResultsOut, sapHits);
for (int j=0; j<command.m_numResultsOut; j++)
{
HK_SET_OBJECT_COLOR(command.m_results[j], hkColor::YELLOW);
}
HK_SET_OBJECT_COLOR((hkUlong)queryCollidable, hkColor::LIME);
if( !jobOk )
{
m_env->m_textDisplay->outputText("MT Hit lits differed!", 20, 250, (hkUint32) hkColor::RED);
}
}
}
void KdTreeVsBroadphaseDemo::doLinearCasts()
{
const int numCasts = 100;
hkObjectArray<hkpClosestCdPointCollector> worldCollectors(numCasts);
hkObjectArray<hkpClosestCdPointCollector> treeCollectors(numCasts);
hkArray<hkpLinearCastInput> lcInput (numCasts);
hkArray<const hkpCollidable*> castCollidables(numCasts);
for (int i=0; i < numCasts; i++)
{
//hkprintf("random seed = %d\n", m_rand.getCurrent());
castCollidables[i] = m_collidables[ m_rand.getRand32() % m_collidables.getSize() ];
hkVector4 start, end;
start = hkGetRigidBody(castCollidables[i])->getPosition();
hkVector4 worldSize(m_worldSizeX, m_worldSizeY, m_worldSizeZ);
m_rand.getRandomVector11(end);
end.mul4(worldSize);
// Flatten out the rays in one component - this triggers a special case in the raycasting code
{
end(i%3) = start(i%3);
}
lcInput[i].m_to = end;
lcInput[i].m_maxExtraPenetration = HK_REAL_EPSILON;
lcInput[i].m_startPointTolerance = HK_REAL_EPSILON;
worldCollectors[i].reset();
treeCollectors[i].reset();
}
{
HK_ASSERT(0x3fe8daf1, m_world->m_kdTreeManager->isUpToDate());
HK_TIME_CODE_BLOCK("kdtreeLinearCast", HK_NULL);
for (int i=0; i < numCasts; i++)
{
m_world->linearCast(castCollidables[i], lcInput[i], treeCollectors[i] );
}
}
{
HK_TIME_CODE_BLOCK("worldLinearCast", HK_NULL);
//
// Mark the world's kd-tree as dirty, forcing raycasting to go through the old (slow) hkp3AxisSweep algorithm
// You should NOT usually be doing this.
//
m_world->markKdTreeDirty();
for (int i=0; i < numCasts; i++)
{
m_world->linearCast(castCollidables[i], lcInput[i], worldCollectors[i] );
}
}
// Check that the results agree, and draw the results
{
for (int i=0; i<numCasts; i++)
{
HK_ASSERT(0x0, worldCollectors[i].hasHit() == treeCollectors[i].hasHit() );
if (worldCollectors[i].hasHit())
{
hkReal tolerance = m_world->getCollisionInput()->m_config->m_iterativeLinearCastEarlyOutDistance;
hkBool hitFractionsEqual = hkMath::equal(worldCollectors[i].getEarlyOutDistance(), treeCollectors[i].getEarlyOutDistance(), 2.0f*tolerance);
hkBool hitCollidablesEqual = worldCollectors[i].getHit().m_rootCollidableB == treeCollectors[i].getHit().m_rootCollidableB ;
HK_ASSERT(0x0, hitFractionsEqual || hitCollidablesEqual );
}
hkVector4 start = hkGetRigidBody(castCollidables[i])->getPosition();
if (treeCollectors[i].hasHit())
{
hkVector4 hitpoint;
hitpoint.setInterpolate4(start, lcInput[i].m_to, treeCollectors[i].getEarlyOutDistance() );
HK_DISPLAY_STAR(hitpoint, .1f, hkColor::RED);
HK_DISPLAY_ARROW(hitpoint, treeCollectors[i].getHit().m_contact.getNormal(), hkColor::CYAN);
HK_DISPLAY_LINE(start, hitpoint, hkColor::BLUE);
}
else
{
HK_DISPLAY_LINE(start, lcInput[i].m_to, hkColor::WHITE);
}
}
}
}
void AabbQueryDemo::queryAabbST()
{
HK_TIME_CODE_BLOCK("queryAabbST", HK_NULL);
hkAabb aabb;
// Grab a body from the world, and compute it's AABB + some padding
const hkpCollidable* queryCollidable;
{
queryCollidable = m_collidables[m_collIdx];
hkpRigidBody* rb = hkGetRigidBody(queryCollidable);
queryCollidable->getShape()->getAabb(rb->getTransform(), 20.0f, aabb);
}
//
// For performance timings, we query the same AABB multiple times and overwrite the results.
// When using this, make sure to use different output arrays!
//
hkArray<hkPrimitiveId> kdhitsRecurs, kdhitsIter;
hkArray<hkpBroadPhaseHandlePair> sapHits;
{
HK_TIME_CODE_BLOCK("KdQueryRecursiveST", HK_NULL);
for (int i=0; i<numQueries; i++)
{
kdhitsRecurs.clear();
hkKdTreeAabbQueryUtils::queryAabbRecursive(m_world->m_kdTreeManager->getTree(), aabb, kdhitsRecurs);
}
}
{
HK_TIME_CODE_BLOCK("KdQueryIterativeST", HK_NULL);
for (int i=0; i<numQueries; i++)
{
kdhitsIter.clear();
hkKdTreeAabbQueryUtils::queryAabbIterative(m_world->m_kdTreeManager->getTree(), aabb, kdhitsIter);
}
}
// Visualize the results
HK_DISPLAY_BOUNDING_BOX(aabb, hkColor::YELLOW);
for (int i=0; i<kdhitsIter.getSize(); i++)
{
HK_SET_OBJECT_COLOR(kdhitsIter[i], hkColor::YELLOW);
}
HK_SET_OBJECT_COLOR((hkUlong)queryCollidable, hkColor::RED);
// Call the corresponding hkp3AxisSweep method for comparison
{
HK_TIME_CODE_BLOCK("BroadphaseQueryAabb", HK_NULL);
for (int i=0; i<numQueries; i++)
{
sapHits.clear();
m_world->getBroadPhase()->querySingleAabb( aabb, sapHits );
}
}
{
hkBool iterOk = compareHitArrays(aabb, kdhitsIter.begin(), kdhitsIter.getSize(), sapHits);
hkBool recursOk = compareHitArrays(aabb, kdhitsRecurs.begin(), kdhitsRecurs.getSize(), sapHits);
if( !(iterOk && recursOk) )
{
m_env->m_textDisplay->outputText("ST Hit lits differed!", 20, 150, (hkUint32) hkColor::RED);
}
}
}
