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