void WorldLinearCastMultithreadedDemo::displayRootCdPoint( hkpWorld* world, const hkpRootCdPoint& cp ) { displayRootCdBody( world, cp.m_rootCollidableB, cp.m_shapeKeyB ); // Display contact point HK_DISPLAY_ARROW( cp.m_contact.getPosition(), cp.m_contact.getNormal(), hkColor::YELLOW ); }
// // contactProcessCallback // void MyCollisionListener::contactProcessCallback( hkpContactProcessEvent& event ) { hkpProcessCollisionData& result = *event.m_collisionData; int size = result.getNumContactPoints(); for (int i = 0; i < size; i++ ) { hkpProcessCdPoint& cp = result.m_contactPoints[i]; { ContactPointInfo* info = reinterpret_cast<ContactPointInfo*>( event.m_contactPointProperties[i]->getUserData() ); if ( (info) && (m_reportLevel >= hkDemoEnvironment::REPORT_INFO) ) { int contactId = cp.m_contactPointId; hkprintf("Contact userId=%i processed. Impulse %f. Contact Point Id=%i\n", info->m_uniqueId, event.m_contactPointProperties[i]->getImpulseApplied(), contactId ); } } // draw the contact points and normals in white { const hkVector4& start = result.m_contactPoints[i].m_contact.getPosition(); hkVector4 normal = result.m_contactPoints[i].m_contact.getNormal(); // For ease of display only, we'll always draw the normal "up" (it points from entity 'B' // to entity 'A', but the order of A,B is arbitrary) so that we can see it. Thus, if it's // pointing "down", flip its direction (and scale), only for display. normal.mul4(5.0f * normal(1)); HK_DISPLAY_ARROW(start, normal, hkColor::WHITE); } } }
void OutOfWorldRaycastDemo::doLinearCast(hkpRigidBody* rb1, hkpRigidBody* rb2) { hkVector4 to, from, midpoint; to = rb1->getPosition(); from = rb2->getPosition(); midpoint.setInterpolate4(to, from, .5f); // try a linear cast hkpLinearCastInput lci; hkpAllCdPointCollector collector; lci.m_to = to; m_world->linearCast(rb2->getCollidable(), lci, collector); int color; color = collector.hasHit() ? 0xFF00FF00 : 0xFFFF0000; const hkArray<hkpRootCdPoint> &hits = collector.getHits(); // check for duplicates // HVK-3126 for( int i = 0; i < hits.getSize(); ++i ) { for( int j = i+1; j < hits.getSize(); ++j ) { HK_ON_DEBUG(const hkpRootCdPoint& pi = hits[i]); HK_ON_DEBUG(const hkpRootCdPoint& pj = hits[j]); HK_ASSERT(0, (pi.m_shapeKeyA != pj.m_shapeKeyA) || (pi.m_shapeKeyB != pj.m_shapeKeyB) ); } } hkVector4 dir; dir.setSub4(to, midpoint); HK_DISPLAY_ARROW(midpoint,dir, color); }
hkDemo::Result MotorActionDemo::stepDemo() { // reverse direction every 300 steps // (this is really just a code coverage thing for the hkpMotorAction // get/set methods, as it serves no other purpose than to make the // top fall over. Aww!) if( 0 == ++m_elapsedSteps % 300 && m_motorAction->isActive() ) { m_motorAction->setSpinRate( -(const_cast<const hkpMotorAction*>(m_motorAction)->getSpinRate()) ); m_motorAction->setGain( const_cast<const hkpMotorAction*>(m_motorAction)->getGain() ); m_motorAction->setAxis( const_cast<const hkpMotorAction*>(m_motorAction)->getAxis() ); } // display the motor axis { hkVector4 motorAxisStart, motorAxisEnd; motorAxisStart = static_cast<hkpRigidBody*>(m_motorAction->getEntity())->getPosition(); motorAxisEnd.setRotatedDir( static_cast<hkpRigidBody*>(m_motorAction->getEntity())->getRotation(), m_motorAction->getAxis() ); HK_DISPLAY_ARROW( motorAxisStart, motorAxisEnd, hkColor::YELLOW ); } // and step the world return hkDefaultPhysicsDemo::stepDemo(); }
hkDemo::Result PrevailingWindDemo::stepDemo() { hkVector4 worldPoint( 0.0f, 8.0f, 0.0f); hkVector4 windAtWorldPoint; { m_wind->getWindVector( worldPoint, windAtWorldPoint ); } HK_DISPLAY_ARROW( worldPoint, windAtWorldPoint, hkColor::BLUE); return hkDefaultPhysicsDemo::stepDemo(); }
hkDemo::Result WorldRaycastDemo::stepDemo() { m_world->lock(); m_time += m_timestep; // The start point of the ray remains fixed in world space with the destination point of the // ray being varied in both the X and Y directions. This is achieved with simple // sine and cosine functions calls using the current time as the varying parameter: hkReal xDir = 12.0f * hkMath::sin(m_time * 0.3f); hkReal yDir = 12.0f * hkMath::cos(m_time * 0.3f); // The start and end points are both specified in World Space as we are using the world castRay() method // to fire the ray. hkpWorldRayCastInput input; input.m_from.set(0.0f, 0.0f, 15.0f); input.m_to.set( xDir, yDir, -15.0f); hkpClosestRayHitCollector output; m_world->castRay(input, output ); // To visualise the raycast we make use of a macro defined in "hkDebugDisplay.h" called HK_DISPLAY_LINE. // The macro takes three parameters: a start point, an end point and the line color. // If a hit is found we display a RED line from the raycast start point to the point of intersection and mark that // point with a small RED cross. The intersection point is calculated using: startWorld + (result.m_mindist * endWorld). // // If no hit is found we simply display a GREY line between the raycast start and end points. if( output.hasHit() ) { hkVector4 intersectionPointWorld; intersectionPointWorld.setInterpolate4( input.m_from, input.m_to, output.getHit().m_hitFraction ); HK_DISPLAY_LINE( input.m_from, intersectionPointWorld, hkColor::RED); HK_DISPLAY_ARROW( intersectionPointWorld, output.getHit().m_normal, hkColor::CYAN); } else { // Otherwise draw as GREY HK_DISPLAY_LINE(input.m_from, input.m_to, hkColor::rgbFromChars(200, 200, 200)); } m_world->unlock(); return hkDefaultPhysicsDemo::stepDemo(); }
void ShapeQueryDemo::displayRayHit( hkpWorld* world, const hkpWorldRayCastInput& in, const hkpWorldRayCastOutput& out ) { // Display hit shape // This demo has a hardcoded knowledge of the shape hierarchy. It could be more generalized // by using the hkpShapeContainer interface. int key = out.m_shapeKeys[0]; for(int i = 0; out.m_shapeKeys[i] != HK_INVALID_SHAPE_KEY; ++i ) { key = out.m_shapeKeys[i]; } displayRootCdBody( world, out.m_rootCollidable, key ); // Display contact point { hkVector4 pos; pos.setInterpolate4( in.m_from, in.m_to, out.m_hitFraction ); HK_DISPLAY_ARROW( pos, out.m_normal, hkColor::YELLOW); } }
void OptimizedWorldRaycastDemo::displayHit( const hkpWorldRayCastInput& input, hkpClosestRayHitCollector& collector, int color) const { // To visualize the raycast we make use of a macro defined in "hkDebugDisplay.h" called HK_DISPLAY_LINE. // The macro takes three parameters: a start point, an end point and the line colour. // If a hit is found we display a RED line from the raycast start point to the point of intersection and mark that // point with a small RED cross. The intersection point is calculated using: startWorld + (result.m_mindist * endWorld). // // If no hit is found we simply display a GREY line between the raycast start and end points. if( collector.hasHit() ) { hkVector4 intersectionPointWorld; intersectionPointWorld.setInterpolate4( input.m_from, input.m_to, collector.getHit().m_hitFraction ); HK_DISPLAY_LINE( input.m_from, intersectionPointWorld, color); HK_DISPLAY_ARROW( intersectionPointWorld, collector.getHit().m_normal, color); } else { // Otherwise draw full length HK_DISPLAY_LINE(input.m_from, input.m_to, color); } }
void OutOfWorldRaycastDemo::doRaycast(hkpRigidBody* rb1, hkpRigidBody* rb2) { hkVector4 to, from, midpoint; to = rb1->getPosition(); from = rb2->getPosition(); midpoint.setInterpolate4(to, from, .5f); hkpWorldRayCastInput input; input.m_from = from; input.m_to = to; hkpClosestRayHitCollector output; m_world->castRay(input, output ); int color; color = output.hasHit() ? 0xFF00FF00 : 0xFFFF0000; hkVector4 dir; dir.setSub4(to, midpoint); HK_DISPLAY_ARROW(midpoint,dir, color); }
hkDemo::Result WorldRayCastMultithreadedDemo::stepDemo() { // const WorldRayCastMultithreadedDemoVariant& variant = g_WorldRayCastMultithreadedDemoVariants[m_variantId]; m_time += m_timestep; // The start point of the ray remains fixed in world space with the destination point of the // ray being varied in both the X and Y directions. This is achieved with simple // sine and cosine functions calls using the current time as the varying parameter: hkReal xDir = 12.0f * hkMath::sin(m_time * 0.3f); hkReal yDir = 12.0f * hkMath::cos(m_time * 0.3f); // For this demo an array of size 1 is sufficient. hkArray<hkpWorldRayCastOutput> resultArray(1); const int numCommands = 1; hkArray<hkpWorldRayCastCommand> commands(numCommands); { hkpWorldRayCastCommand& command = commands[0]; command.m_rayInput.m_from . set(0.0f, 0.0f, 15.0f); command.m_rayInput.m_to . set( xDir, yDir, -15.0f); command.m_rayInput.m_enableShapeCollectionFilter = false; command.m_rayInput.m_filterInfo = 0; command.m_results = resultArray.begin(); command.m_resultsCapacity = 1; command.m_numResultsOut = 0; } // // create the job header // hkpCollisionQueryJobHeader* jobHeader; { jobHeader = hkAllocateChunk<hkpCollisionQueryJobHeader>(1, HK_MEMORY_CLASS_DEMO); } // // setup hkpWorldRayCastJob // m_world->markForRead(); hkpWorldRayCastJob worldRayCastJob(m_world->getCollisionInput(), jobHeader, commands.begin(), numCommands, m_world->m_broadPhase, &m_semaphore); m_world->unmarkForRead(); // // Put the job on the queue, kick-off the PPU/SPU threads and wait for everything to finish. // { m_world->lockReadOnly(); // // Put the raycast job on the job queue. // worldRayCastJob.setRunsOnSpuOrPpu(); m_jobQueue->addJob( worldRayCastJob, hkJobQueue::JOB_LOW_PRIORITY ); m_jobThreadPool->processAllJobs( m_jobQueue ); m_jobThreadPool->waitForCompletion(); // // Wait for the one single job we started to finish. // m_semaphore.acquire(); m_world->unlockReadOnly(); } // // Display results. // if( commands[0].m_numResultsOut > 0 ) { hkVector4 intersectionPointWorld; intersectionPointWorld.setInterpolate4( commands[0].m_rayInput.m_from, commands[0].m_rayInput.m_to, commands[0].m_results->m_hitFraction ); HK_DISPLAY_LINE( commands[0].m_rayInput.m_from, intersectionPointWorld, hkColor::RED); HK_DISPLAY_ARROW( intersectionPointWorld, commands[0].m_results->m_normal, hkColor::CYAN); } else { // Otherwise draw as GREY HK_DISPLAY_LINE(commands[0].m_rayInput.m_from, commands[0].m_rayInput.m_to, hkColor::rgbFromChars(200, 200, 200)); } // // Free temporarily allocated memory. // hkDeallocateChunk(jobHeader, 1, HK_MEMORY_CLASS_DEMO); return hkDefaultPhysicsDemo::stepDemo(); }
hkDemo::Result DestructibleWallsDemo::stepDemo() { m_world->lock(); { const hkgPad* pad = m_env->m_gamePad; // Cannon control + drawing // check to see if the user has pressed one of the control keys: int x = ((pad->getButtonState() & HKG_PAD_DPAD_LEFT) != 0)? -1:0; x = ((pad->getButtonState() & HKG_PAD_DPAD_RIGHT) != 0)? 1:x; int y = ((pad->getButtonState() & HKG_PAD_DPAD_DOWN) != 0)? -1:0; y = ((pad->getButtonState() & HKG_PAD_DPAD_UP) != 0)? 1:y; m_shootingDirX += x * 0.015f; m_shootingDirY += y * 0.015f; hkVector4 canonStart( m_centerOfScene ); canonStart(2) += m_options.m_WallsWidth*2.0f; hkVector4 canonDir( m_shootingDirX, m_shootingDirY, -1.0f ); canonDir.normalize3(); // display the canon direction { hkpWorldRayCastInput in; in.m_from = canonStart; in.m_to.setAddMul4( in.m_from, canonDir, 100.0f ); in.m_filterInfo = 0; hkpWorldRayCastOutput out; m_world->castRay( in , out ); hkVector4 hit; hit.setInterpolate4( in.m_from, in.m_to, out.m_hitFraction ); HK_DISPLAY_LINE( in.m_from, hit, 0x600000ff ); if ( out.hasHit() ) { HK_DISPLAY_ARROW( hit, out.m_normal, 0x60ff00ff ); } HK_DISPLAY_ARROW( canonStart, canonDir, hkColor::CYAN ); } // Shooting bullets { hkBool shooting = false; if ( pad->wasButtonPressed(HKG_PAD_BUTTON_1)!= 0 ) { shooting = true; } if ( pad->isButtonPressed(HKG_PAD_BUTTON_2)!= 0) { if ( m_gunCounter-- < 0 ) { shooting = true; m_gunCounter = 5; } } if ( shooting ) { hkpMassProperties result; hkpInertiaTensorComputer::computeSphereVolumeMassProperties(m_options.m_cannonBallRadius, m_options.m_cannonBallMass, result); hkpSphereShape* sphereShape = new hkpSphereShape(m_options.m_cannonBallRadius); hkVector4 spherePos(-20.0f, 0.0f + m_options.m_cannonBallRadius, 200.0f); hkpRigidBodyCinfo sphereInfo; sphereInfo.m_mass = result.m_mass; sphereInfo.m_centerOfMass = result.m_centerOfMass; sphereInfo.m_inertiaTensor = result.m_inertiaTensor; sphereInfo.m_shape = sphereShape; sphereInfo.m_motionType = hkpMotion::MOTION_BOX_INERTIA; sphereInfo.m_position = canonStart; sphereInfo.m_qualityType = HK_COLLIDABLE_QUALITY_BULLET; sphereInfo.m_linearVelocity.setMul4( 100.0f, canonDir ); hkpRigidBody* bullet = new hkpRigidBody( sphereInfo ); sphereInfo.m_shape->removeReference(); m_world->addEntity( bullet ); bullet->removeReference(); } } } // release the world m_world->unlock(); // step demo hkDemo::Result res = hkDefaultPhysicsDemo::stepDemo(); //HK_TIMER_BEGIN_LIST( "Update Walls", "Update Walls"/*HK_NULL*/); HK_TIMER_BEGIN( "Update Walls", "update walls"/*HK_NULL*/); if(m_collisionDetectionType == PARALLEL) { HK_ASSERT2(0x7274e9ec, m_fractureUtility!=HK_NULL ,"The parallel simulation wasn't set!!"); // and update walls m_fractureUtility->Update(); } HK_TIMER_END(); HK_TIMER_BEGIN("DebugDisplay", HK_NULL); // DEBUG DISPLAY if(m_fractureUtility->getSimulation() && m_options.m_showDebugDisplay) { // applied impulses for(int i=0; i<m_fractureUtility->getSimulation()->debugImpulses.getSize(); i+=2) { hkVector4 start( m_fractureUtility->getSimulation()->debugImpulses[i] ); hkVector4 end( m_fractureUtility->getSimulation()->debugImpulses[i+1] ); end.mul4( 0.01f ); HK_DISPLAY_ARROW(start, end , 0x00000000); } if(m_fractureUtility->getSimulation()->debugImpulses.getSize() > 100 ) m_fractureUtility->getSimulation()->debugImpulses.clear(); // bricks positions in parallel simulation hkArray<hkVector4> positions; m_fractureUtility->getSimulation()->getAllBricksPositions( positions ); for(int i=0; i<positions.getSize(); ++i) { drawBrick(positions[i] , m_brickHalfExtents); } positions.clear(); // edges of union find for(int i=0; i<m_fractureUtility->getSimulation()->debugEdges.getSize(); i+=2) { HK_DISPLAY_LINE(m_fractureUtility->getSimulation()->debugEdges[i], m_fractureUtility->getSimulation()->debugEdges[i+1] , 0x00000000); } } HK_TIMER_END(); if(m_collisionDetectionType == PARALLEL) { hkArray< hkVector4 > planes; m_fractureUtility->getSimulation()->getAllDebugfracturePlanes(planes); for(int i=0; i< planes.getSize()-2; ++i) { if(planes[i].length3()!=0) { hkVector4 offset(.0f, .5f, .0f/*.5f, .5f, .5f*/); HK_DISPLAY_PLANE(planes[i], offset, 0.5f, 0xffffffff); } } if(!planes.isEmpty() && planes[planes.getSize()-1].length3()!=0) { hkVector4 offset(.0f, .5f, .0f/*.5f, .5f, .5f*/); HK_DISPLAY_PLANE(planes[planes.getSize()-2], offset, 0.5f, 0x00000000); HK_DISPLAY_PLANE(planes[planes.getSize()-1], offset, 0.5f, 0xffff0000); } hkArray<hkVector4> cpts; hkArray<hkVector4> impulses; m_fractureUtility->getSimulation()->getAllDebugImpulsesAndContactPoints(impulses, cpts); for(int i=0; i< cpts.getSize(); ++i) { HK_DISPLAY_ARROW(cpts[i], impulses[i] , 0x00000000); } } return res; }
hkDemo::Result AsymetricCharacterRbDemo::stepDemo() { // Update actual time m_time += m_timestep; hkQuaternion orient; { m_world->lock(); hkReal posX = 0.f; hkReal posY = 0.f; { float deltaAngle = 0.f; CharacterUtils::getUserInputForCharacter(m_env, deltaAngle, posX, posY); m_currentAngle += deltaAngle; orient.setAxisAngle(UP, m_currentAngle); } hkpCharacterInput input; hkpCharacterOutput output; { input.m_inputLR = posX; input.m_inputUD = posY; input.m_wantJump = m_env->m_window->getMouse().wasButtonPressed(HKG_MOUSE_LEFT_BUTTON) || m_env->m_gamePad->wasButtonPressed(HKG_PAD_BUTTON_1); input.m_atLadder = false; input.m_up = UP; input.m_forward.set(1,0,0); input.m_forward.setRotatedDir( orient, input.m_forward ); hkStepInfo stepInfo; stepInfo.m_deltaTime = m_timestep; stepInfo.m_invDeltaTime = 1.0f/m_timestep; stepInfo.m_endTime = m_time; input.m_stepInfo = stepInfo; input.m_characterGravity.set(0,-16,0); input.m_velocity = m_characterRigidBody->getLinearVelocity(); input.m_position = m_characterRigidBody->getPosition(); hkpSurfaceInfo ground; m_characterRigidBody->checkSupport( stepInfo, ground); // Avoid accidental state changes (Smooth movement on stairs) // During transition supported->unsupported continue to return N-frames hkpSurfaceInfo data from previous supported state { // Number of frames to skip (continue with previous hkpSurfaceInfo data) const int skipFramesInAir = 5; if (input.m_wantJump) { m_framesInAir = skipFramesInAir; } if ( ground.m_supportedState != hkpSurfaceInfo::SUPPORTED ) { if (m_framesInAir < skipFramesInAir) { input.m_isSupported = true; input.m_surfaceNormal = m_previousGround->m_surfaceNormal; input.m_surfaceVelocity = m_previousGround->m_surfaceVelocity; input.m_surfaceMotionType = m_previousGround->m_surfaceMotionType; } else { input.m_isSupported = false; input.m_surfaceNormal = ground.m_surfaceNormal; input.m_surfaceVelocity = ground.m_surfaceVelocity; input.m_surfaceMotionType = ground.m_surfaceMotionType; } m_framesInAir++; } else { input.m_isSupported = true; input.m_surfaceNormal = ground.m_surfaceNormal; input.m_surfaceVelocity = ground.m_surfaceVelocity; input.m_surfaceMotionType = ground.m_surfaceMotionType; m_previousGround->set(ground); // Reset old number of frames if (m_framesInAir > skipFramesInAir) { m_framesInAir = 0; } } } } // Apply the character state machine { HK_TIMER_BEGIN( "update character state", HK_NULL ); m_characterContext->update(input, output); HK_TIMER_END(); } // Apply the player character controller { HK_TIMER_BEGIN( "simulate character", HK_NULL ); m_characterRigidBody->setLinearVelocity(output.m_velocity, m_timestep); HK_TIMER_END(); } // Rotate the character { hkVector4 offset; offset.set(1,0,0); offset.setRotatedDir( orient , offset); hkRotation rotation; hkVector4& col0 = rotation.getColumn(0); hkVector4& col1 = rotation.getColumn(1); hkVector4& col2 = rotation.getColumn(2); // Smoothed surface normal hkVector4 surfaceNorm; surfaceNorm = input.m_isSupported ? input.m_surfaceNormal : (hkVector4)UP; m_rigidBodyNormal.addMul4( 0.05f, surfaceNorm ); m_rigidBodyNormal.normalize3(); col1 = m_rigidBodyNormal; col2.setCross( col1, offset); col2.normalize3(); col0.setCross( col1, col2 ); #ifdef HK_DEBUG HK_DISPLAY_ARROW(m_characterRigidBody->getPosition(), col0, 0xffff00ff); HK_DISPLAY_ARROW(m_characterRigidBody->getPosition(), col1, 0xff00ffff); HK_DISPLAY_ARROW(m_characterRigidBody->getPosition(), col2, 0xff0000ff); #endif // Forward orientation controller reorientCharacter( rotation ); } // Display states infos { // classical controller state hkpCharacterStateType state = m_characterContext->getState(); char * stateStr; switch (state) { case HK_CHARACTER_ON_GROUND: stateStr = "On Ground"; break; case HK_CHARACTER_JUMPING: stateStr = "Jumping"; break; case HK_CHARACTER_IN_AIR: stateStr = "In Air"; break; case HK_CHARACTER_CLIMBING: stateStr = "Climbing"; break; default: stateStr = "Other"; break; } char buffer[255]; hkString::snprintf(buffer, 255, "State: %s ", stateStr); m_env->m_textDisplay->outputText(buffer, 20, 450, 0xffffffff); m_world->unlock(); } // Step the world { hkDefaultPhysicsDemo::stepDemo(); } m_world->lock(); // Camera Handling { const hkReal height = 1.25f; hkVector4 forward; forward.set(1,0,0); forward.setRotatedDir( orient, forward ); hkVector4 from, to; to = m_characterRigidBody->getRigidBody()->getPosition(); hkVector4 dir; dir.setMul4( height, UP ); dir.addMul4( -4.0f, forward); from.setAdd4(to, dir); setupDefaultCameras(m_env, from, to, UP, 1.0f); } m_world->unlock(); } return hkDemo::DEMO_OK; }
hkDemo::Result WorldLinearCastMultithreadingApiDemo::stepDemo() { // const WorldLinearCastMultithreadingApiDemoVariant& variant = g_WorldLinearCastMultithreadingApiDemoVariants[m_variantId]; // // Advance time (used for calculating the rotating linear cast path). // m_time += m_timestep; // // Setup the output array where the resulting collision points will be returned. // hkpRootCdPoint* bpCollisionPoints = hkAllocateChunk<hkpRootCdPoint>(1, HK_MEMORY_CLASS_DEMO); hkpRootCdPoint* kdCollisionPoints = hkAllocateChunk<hkpRootCdPoint>(1, HK_MEMORY_CLASS_DEMO); // // Setup the hkpWorldLinearCastCommand bpCommand. // hkpWorldLinearCastCommand* bpCommand; hkpWorldLinearCastCommand* kdCommand; { bpCommand = hkAllocateChunk<hkpWorldLinearCastCommand>(1, HK_MEMORY_CLASS_DEMO); kdCommand = hkAllocateChunk<hkpWorldLinearCastCommand>(1, HK_MEMORY_CLASS_DEMO); // Init input data. { bpCommand->m_input.m_to.setZero4(); bpCommand->m_input.m_to(0) = (CAST_CIRCLE_RADIUS * hkMath::sin(m_time * 1.0f)); bpCommand->m_input.m_to(2) = (CAST_CIRCLE_RADIUS * hkMath::cos(m_time * 1.0f)); bpCommand->m_input.m_maxExtraPenetration = HK_REAL_EPSILON; bpCommand->m_input.m_startPointTolerance = HK_REAL_EPSILON; // hkpWorldObject::getCollidable() needs a read-lock on the object m_castBody->markForRead(); bpCommand->m_collidable = m_castBody->getCollidable(); m_castBody->unmarkForRead(); } // Init output data. { bpCommand->m_results = bpCollisionPoints; bpCommand->m_resultsCapacity = 1; bpCommand->m_numResultsOut = 0; } // Copy the data for the kd-tree command *kdCommand = *bpCommand; // Different results ptr and numResultsOut, but everything else is the same; kdCommand->m_results = kdCollisionPoints; } // // create the job header // hkpCollisionQueryJobHeader* bpJobHeader; hkpRayCastQueryJobHeader* kdJobHeader; { bpJobHeader = hkAllocateChunk<hkpCollisionQueryJobHeader>(1, HK_MEMORY_CLASS_DEMO); kdJobHeader = hkAllocateChunk<hkpRayCastQueryJobHeader>(1, HK_MEMORY_CLASS_DEMO); } // // Setup the jobs. // hkSemaphoreBusyWait* semaphore = new hkSemaphoreBusyWait(0, 1000); // must be allocated on heap, for SPU to work m_world->markForRead(); hkpWorldLinearCastJob worldLinearCastJob( m_world->getCollisionInput(), bpJobHeader, bpCommand, 1, m_world->m_broadPhase, semaphore); m_world->unmarkForRead(); m_world->markForWrite(); m_world->m_kdTreeManager->updateFromWorld(m_jobQueue, m_jobThreadPool); hkpKdTreeLinearCastJob kdTreeLinearCastJob(m_world->getCollisionInput(), kdJobHeader, kdCommand, 1, semaphore); m_world->unmarkForWrite(); { // The jobs support multiple trees, but we only have one in the world right now kdTreeLinearCastJob.m_numTrees = 1; kdTreeLinearCastJob.m_trees[0] = m_world->m_kdTreeManager->getTree(); kdTreeLinearCastJob.setRunsOnSpuOrPpu(); } // // Put the job on the queue, kick-off the PPU/SPU threads and wait for everything to finish. // { m_world->lockReadOnly(); worldLinearCastJob.setRunsOnSpuOrPpu(); m_jobQueue->addJob( worldLinearCastJob, hkJobQueue::JOB_LOW_PRIORITY ); m_jobThreadPool->processAllJobs( m_jobQueue ); m_jobThreadPool->waitForCompletion(); // // Wait for the one single job we started to finish. // semaphore->acquire(); m_world->unlockReadOnly(); } // // Now do the same for the kd-tree job // { m_jobQueue->addJob(kdTreeLinearCastJob, hkJobQueue::JOB_HIGH_PRIORITY ); m_jobThreadPool->processAllJobs( m_jobQueue ); m_jobThreadPool->waitForCompletion(); semaphore->acquire(); } delete semaphore; // // Display results. // { hkVector4 from = bpCommand->m_collidable->getTransform().getTranslation(); hkVector4 to = bpCommand->m_input.m_to; hkVector4 path; path.setSub4(to, from); if ( bpCommand->m_numResultsOut > 0 ) { for ( int r = 0; r < bpCommand->m_numResultsOut; r++) { const hkContactPoint& contactPoint = bpCommand->m_results[r].m_contact; hkReal fraction = contactPoint.getDistance(); // Calculate the position on the linear cast's path where the first collidable hit the second collidable. hkVector4 pointOnPath; { pointOnPath.setAddMul4( from, path, fraction ); } // Draw the linear cast line from startpoint to hitpoint. HK_DISPLAY_LINE( from, pointOnPath, hkColor::RED ); // Draw the collision normal. HK_DISPLAY_ARROW( pointOnPath, contactPoint.getNormal(), hkColor::BLUE ); // Draw the linear cast line from hitpoint to endpoint. HK_DISPLAY_LINE( pointOnPath, to, hkColor::BLACK ); // // Display body's position at 'time of collision'. // { hkTransform castTransform = bpCommand->m_collidable->getTransform(); castTransform.setTranslation(pointOnPath); m_env->m_displayHandler->updateGeometry(castTransform, 1, 0); } } } else { // Display the whole linear cast line. HK_DISPLAY_LINE( from, to, hkColor::BLACK ); // Display the casted collidable at cast's endpoint. { hkTransform castTransform = bpCommand->m_collidable->getTransform(); m_env->m_displayHandler->updateGeometry(castTransform, 1, 0); } } } // // Free temporarily allocated memory. // hkDeallocateChunk(bpJobHeader, 1, HK_MEMORY_CLASS_DEMO); hkDeallocateChunk(bpCommand, 1, HK_MEMORY_CLASS_DEMO); hkDeallocateChunk(kdCommand, 1, HK_MEMORY_CLASS_DEMO); hkDeallocateChunk(bpCollisionPoints, 1, HK_MEMORY_CLASS_DEMO); hkDeallocateChunk(kdCollisionPoints, 1, HK_MEMORY_CLASS_DEMO); return hkDefaultPhysicsDemo::stepDemo(); }
hkDemo::Result PlanetGravityDemo::stepDemo() { // Update lighting { // Update the light source to be at the camera float position[3]; m_cameraPosition.store3( position ); m_flashLight->setPosition( position ); // Update the light direction to be pointing toward the character controller rigid body hkVector4 directionVector; directionVector.setSub4( m_cameraPosition, m_characterRigidBody->getPosition() ); directionVector.mul4( -1.0f ); directionVector.normalize3(); float direction[3]; directionVector.store3( direction ); m_flashLight->setDirection( direction ); } // Detach the camera from the character when P is pressed. if( m_env->m_window->getKeyboard().wasKeyPressed('P') ) { m_detachedCamera = !m_detachedCamera; } // Update turrets for( int i = 0; i < m_turrets.getSize(); i++ ) { Turret& turret = m_turrets[i]; turret.cooldown -= m_timestep; // Make the turret spin turret.hinge->setMotorTargetAngle( turret.hinge->getMotorTargetAngle() + ( m_timestep / 5.f ) ); // Bail out if the turret is "hot" if( turret.cooldown > 0.0f ) { continue; } // Generate a curved raycast and shoot the ray // This has to be done every time-step as it's in world-space { const hkReal radius = 14.8f; hkRotation rot; hkVector4 offset; hkVector4 turretDown; rot.set( turret.turretRigidBody->getRotation() ); offset = turret.turretRigidBody->getPosition(); turretDown.setMul4( -1.0f, rot.getColumn(2) ); hkpLinearParametricCurve myCurve; // Move the ray's source a little up so it's coming from the center of the barrel hkTransform localTransform( hkQuaternion::getIdentity(), hkVector4( 0.0f, 0.0f, 0.7f ) ); // Create a curve of 20 points for( int j = 0; j < 20; j++ ) { hkReal angle = HK_REAL_PI * static_cast<hkReal>(j) / 15.0f; hkVector4 newPoint( radius * 2.0f * sin( angle ), 0.0f, radius * 2.0f * cos( angle ) ); newPoint.setTransformedPos( localTransform, newPoint ); newPoint.setTransformedPos( turret.turretRigidBody->getTransform(), newPoint ); newPoint.addMul4( radius * 2.0f, turretDown ); myCurve.addPoint( newPoint ); } // We only need the closest hit (as our lasers can't pass through objects) // so hkpClosestRayHitCollector is used. hkpClosestRayHitCollector raycastOutput; hkReal t = castCurvedRay( raycastOutput, myCurve, 20 ); // Apply a large force to the closest rb we hit, along the tangent at the colliding point if( raycastOutput.hasHit() ) { hkpRigidBody* hitRb = hkGetRigidBody( raycastOutput.getHit().m_rootCollidable ); if( hitRb->getMotionType() != hkpMotion::MOTION_FIXED ) { hkVector4 tangent; myCurve.getTangent( t, tangent ); tangent.mul4( 15000.0f ); applyScaledLinearImpulse( hitRb, tangent ); turret.cooldown = 3.0f; } } } } m_world->markForWrite(); // Update the character context m_characterRigidBody->m_up = m_worldUp; hkReal posX = 0.0f; hkReal posY = 0.0f; if( !m_detachedCamera ) { float deltaAngle; CharacterUtils::getUserInputForCharacter( m_env, deltaAngle, posX, posY ); if( ( ( hkMath::fabs( posX ) < HK_REAL_MAX ) && ( hkMath::fabs( posY ) < HK_REAL_MAX ) ) && ( posX || posY ) ) { // find new orientation in local space hkVector4 newForward( -posY, 0.0f, -posX ); hkVector4 absoluteForward( 1.0f, 0.0f, 0.0f ); hkReal characterAngle = hkMath::acos( absoluteForward.dot3( newForward ) ); // Calculate cross product to get sign of rotation. hkVector4 crossProduct; crossProduct.setCross( absoluteForward, newForward ); if( crossProduct(1) < 0.0f ) { characterAngle *= -1.0f; } // Rotate the character's rigid body to face in the direction it's moving hkRotation newRotation; newRotation.setAxisAngle( m_worldUp, characterAngle ); m_characterForward.setRotatedDir( newRotation, m_cameraForward ); m_characterForward.normalize3(); } // Rotate the camera's forward vector based on world up vector and mouse movement if( deltaAngle != 0.0f && m_characterRigidBody->getRigidBody()->getRotation().hasValidAxis() ) { hkRotation newRotation; newRotation.setAxisAngle( m_worldUp, deltaAngle ); m_cameraForward.setRotatedDir( newRotation, m_cameraForward ); m_cameraForward.normalize3(); } } HK_TIMER_BEGIN( "set character state", HK_NULL ); hkpCharacterInput input; hkpCharacterOutput output; { input.m_atLadder = false; input.m_inputLR = posX; input.m_inputUD = posY; if( m_detachedCamera ) { input.m_wantJump = false; } else { input.m_wantJump = m_env->m_window->getMouse().wasButtonPressed( HKG_MOUSE_LEFT_BUTTON ) || m_env->m_gamePad->wasButtonPressed( HKG_PAD_BUTTON_1 ); } // Check that we have a valid rotation. Probably won't for the first couple of frames. if( !( m_characterRigidBody->getRigidBody()->getRotation().hasValidAxis() ) ) { input.m_up = hkVector4( 0.0f, 0.0f, 1.0f ); input.m_forward = m_cameraForward; } else { input.m_up = m_worldUp; // Recalculate m_forward so it's perpendicular to m_worldUp hkVector4 newRot; newRot.setCross( m_cameraForward, m_worldUp ); m_cameraForward.setCross( m_worldUp, newRot ); // Display character's current heading hkRotation characterRotation; characterRotation.set( m_characterRigidBody->getRigidBody()->getRotation() ); HK_DISPLAY_ARROW( m_characterRigidBody->getPosition(), characterRotation.getColumn(0), hkColor::LIMEGREEN ); input.m_forward = m_cameraForward; } hkStepInfo stepInfo; stepInfo.m_deltaTime = m_timestep; stepInfo.m_invDeltaTime = 1.0f / m_timestep; input.m_stepInfo = stepInfo; input.m_characterGravity.setMul4( -20.0f, m_worldUp ); input.m_velocity = m_characterRigidBody->getRigidBody()->getLinearVelocity(); input.m_position = m_characterRigidBody->getRigidBody()->getPosition(); { hkpSurfaceInfo ground; m_characterRigidBody->checkSupport( stepInfo, ground ); // Avoid accidental state changes (Smooth movement on stairs) // During transition supported->unsupported continue to return N-frames hkpSurfaceInfo data from previous supported state { // Number of frames to skip (continue with previous hkpSurfaceInfo data) const int skipFramesInAir = 6; if( input.m_wantJump ) { m_framesInAir = skipFramesInAir; } hkpSurfaceInfo* currInfo; if( ground.m_supportedState != hkpSurfaceInfo::SUPPORTED ) { if( m_framesInAir < skipFramesInAir ) { input.m_isSupported = true; currInfo = m_previousGround; } else { input.m_isSupported = false; currInfo = &ground; } m_framesInAir++; } else { input.m_isSupported = true; currInfo = &ground; m_previousGround->set( ground ); // reset old number of frames if( m_framesInAir > skipFramesInAir ) { m_framesInAir = 0; } } input.m_surfaceNormal = currInfo->m_surfaceNormal; input.m_surfaceVelocity = currInfo->m_surfaceVelocity; input.m_surfaceMotionType = currInfo->m_surfaceMotionType; } } HK_TIMER_END(); } // Apply the character state machine { HK_TIMER_BEGIN( "update character state", HK_NULL ); m_characterContext->update( input, output ); HK_TIMER_END(); } //Apply the player character controller { HK_TIMER_BEGIN( "simulate character", HK_NULL ); // Set output velocity from state machine into character rigid body m_characterRigidBody->setLinearVelocity( output.m_velocity, m_timestep ); HK_TIMER_END(); m_world->unmarkForWrite(); } // Rotate the character { hkRotation newOrientation; newOrientation.getColumn(0) = m_characterForward; newOrientation.getColumn(1) = m_worldUp; newOrientation.getColumn(2).setCross( newOrientation.getColumn(0), newOrientation.getColumn(1) ); newOrientation.renormalize(); reorientCharacter( newOrientation ); } // Step the world hkDefaultPhysicsDemo::stepDemo(); // Display state { hkpCharacterStateType state = m_characterContext->getState(); char* stateStr; switch( state ) { case HK_CHARACTER_ON_GROUND: { stateStr = "On Ground"; break; } case HK_CHARACTER_JUMPING: { stateStr = "Jumping"; break; } case HK_CHARACTER_IN_AIR: { stateStr = "In Air"; break; } default: { stateStr = "Other"; break; } } char buffer[255]; hkString::snprintf( buffer, 255, "State : %s", stateStr ); m_env->m_textDisplay->outputText( buffer, 20.f, 270.f, 0xffffffff ); } // // Handle crouching (only for capsule) // if( !m_detachedCamera ) { m_world->markForWrite(); hkBool wantCrouch = ( m_env->m_window->getMouse().getButtonState() & HKG_MOUSE_RIGHT_BUTTON ) || ( m_env->m_gamePad->getButtonState() & HKG_PAD_BUTTON_2 ); hkBool isCrouching = ( m_characterRigidBody->getRigidBody()->getCollidable()->getShape() == m_crouchShape ); // We want to stand if( isCrouching && !wantCrouch ) { m_characterRigidBody->getRigidBody()->setShape( m_standShape ); } // We want to crouch else if( !isCrouching && wantCrouch ) { m_characterRigidBody->getRigidBody()->setShape( m_crouchShape ); } m_world->unmarkForWrite(); } // Transparent camera handling if( !m_detachedCamera ) { m_world->markForWrite(); handleCamera(); m_world->unmarkForWrite(); } return hkDemo::DEMO_OK; }
hkDemo::Result BackfacesCulledRayHitCollectorDemo::stepDemo() { m_world->lock(); m_world->markForRead(); // Cast a ray using the BackfacesCulledRayHitCollector { hkpWorldRayCastInput culledInput; culledInput.m_from = hkVector4(-4.0f, 0.5f, 1.0f); culledInput.m_to = hkVector4(4.0f, 0.5f, 1.0f); hkVector4 culledRayDirection; culledRayDirection.setSub4(culledInput.m_to, culledInput.m_from); HK_DISPLAY_ARROW(culledInput.m_from, culledRayDirection, hkColor::YELLOW); BackfacesCulledRayHitCollector culledOutput(culledRayDirection); m_world->castRay(culledInput, culledOutput); for (int i=0; i < culledOutput.getHits().getSize(); i++) { showRaycastResults(culledOutput.getHits()[i], hkColor::ORANGE); hkVector4 normalPosition; normalPosition.setInterpolate4(culledInput.m_from, culledInput.m_to, culledOutput.getHits()[i].m_hitFraction); HK_DISPLAY_ARROW(normalPosition, culledOutput.getHits()[i].m_normal, hkColor::RED); } } // Cast a ray using the regular hkpAllRayHitCollector { hkpWorldRayCastInput normalInput; normalInput.m_from = hkVector4(-4.0f, -0.6f, 1.0f); normalInput.m_to = hkVector4(4.0f, -0.6f, 1.0f); hkVector4 normalRayDirection; normalRayDirection.setSub4(normalInput.m_to, normalInput.m_from); HK_DISPLAY_ARROW(normalInput.m_from, normalRayDirection, hkColor::BLUE); hkpAllRayHitCollector normalOutput; m_world->castRay(normalInput, normalOutput); for (int i=0; i < normalOutput.getHits().getSize(); i++) { showRaycastResults(normalOutput.getHits()[i], hkColor::CYAN); hkVector4 normalPosition; normalPosition.setInterpolate4(normalInput.m_from, normalInput.m_to, normalOutput.getHits()[i].m_hitFraction); HK_DISPLAY_ARROW(normalPosition, normalOutput.getHits()[i].m_normal, hkColor::GREEN); } } m_world->unmarkForRead(); m_world->markForWrite(); { // const hkpShapeContainer* c = m_hemisphereRb->getCollidable()->getShape()->getContainer(); // for (hkpShapeKey k = c->getFirstKey(); k != HK_INVALID_SHAPE_KEY; k = c->getNextKey(k)) // { // hkpShapeContainer::ShapeBuffer buffer; // // hkcout << hkGetShapeTypeName(m_hemisphereRb->getCollidable()->getShape()->getType()) << hkendl; //buffer; // const hkpTriangleShape* triangle = reinterpret_cast<const hkpTriangleShape*>(c->getChildShape(k, buffer)); // // hkVector4 triangleNormal; // hkpTriangleUtil::calcNormal(triangleNormal, // triangle->getVertex(0), triangle->getVertex(1), triangle->getVertex(2)); // triangleNormal.normalize3(); // // hkVector4 trianglePos; // triangle->getCentre(trianglePos); // trianglePos.setTransformedPos(m_hemisphereRb->getTransform(), trianglePos); // // HK_DISPLAY_ARROW(trianglePos, triangleNormal, hkColor::BLUE); // } } m_world->unmarkForWrite(); m_world->unlock(); return hkDefaultPhysicsDemo::stepDemo(); }
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); } } } }
// // Do some random raycasts into the world // Both the kd-tree and the (deprecated) hkp3AxisSweep versions are used for comparison // void KdTreeVsBroadphaseDemo::doRaycasts() { const int numRays = 100; hkLocalArray<hkpWorldRayCastInput> inputs( numRays ); inputs.setSize(numRays); // Need fixed-size collector arrays to make sure the constructors get called hkpWorldRayCastOutput iterativeCollectors[numRays]; hkpClosestRayHitCollector worldCollectors[numRays]; for (int i=0; i < numRays; i++) { hkVector4 start, end; start.set( hkMath::randRange(-m_worldSizeX, m_worldSizeX), hkMath::randRange(-m_worldSizeY, m_worldSizeY), hkMath::randRange(-m_worldSizeZ, m_worldSizeZ)); end.set( hkMath::randRange(-m_worldSizeX, m_worldSizeX), hkMath::randRange(-m_worldSizeY, m_worldSizeY), hkMath::randRange(-m_worldSizeZ, m_worldSizeZ)); // Flatten out the rays in one component - this triggers a special case in the raycasting code { end(i%3) = start(i%3); } inputs[i].m_from = start; inputs[i].m_to = end; inputs[i].m_filterInfo = 0; } // // Raycast using the world's kd-tree // HK_TIMER_BEGIN("kdTreeRaycast", HK_NULL); // Check that the tree isn't dirty HK_ASSERT(0x3fe8daf1, m_world->m_kdTreeManager->isUpToDate()); for (int i=0; i < numRays; i++) { m_world->castRay(inputs[i], iterativeCollectors[i]); } HK_TIMER_END(); HK_TIMER_BEGIN("worldRc", 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 < numRays; i++) { m_world->castRay(inputs[i], worldCollectors[i]); } } HK_TIMER_END(); // Check that the results agree, and draw the results { for (int i=0; i<numRays; i++) { HK_ASSERT(0x0, iterativeCollectors[i].hasHit() == worldCollectors[i].hasHit()); HK_ASSERT(0x0, hkMath::equal(iterativeCollectors[i].m_hitFraction, worldCollectors[i].m_earlyOutHitFraction)); if (iterativeCollectors[i].hasHit()) { hkVector4 hitpoint; hitpoint.setInterpolate4(inputs[i].m_from, inputs[i].m_to, iterativeCollectors[i].m_hitFraction); HK_DISPLAY_STAR(hitpoint, .1f, hkColor::RED); HK_DISPLAY_ARROW(hitpoint, iterativeCollectors[i].m_normal, hkColor::CYAN); HK_DISPLAY_LINE(inputs[i].m_from, hitpoint, hkColor::BLUE); } else { HK_DISPLAY_LINE(inputs[i].m_from, inputs[i].m_to, hkColor::WHITE); } } } }