void BackfacesCulledRayHitCollectorDemo::showRaycastResults(const hkpWorldRayCastOutput& output, int triangleColor) { // Find the leaf shape hkpShapeContainer::ShapeBuffer buffer; const hkpShape* shape = output.m_rootCollidable->getShape(); for(int keyIndex = 0; shape != HK_NULL; ++keyIndex ) { if(shape->getType() == HK_SHAPE_MOPP) { keyIndex++; } // go to the next level const hkpShapeContainer* container = shape->getContainer(); if (container) { shape = container->getChildShape(output.m_shapeKeys[keyIndex], buffer); } else { break; } } // Draw the leaf shape's outline if it's a triangle. if (shape->getType() == HK_SHAPE_TRIANGLE) { const hkpTriangleShape* triangle = static_cast<const hkpTriangleShape*>( shape ); hkArray<hkVector4> transformedVertices(3); transformedVertices[0].setTransformedPos(hkGetRigidBody(output.m_rootCollidable)->getTransform(), triangle->getVertex(0)); transformedVertices[1].setTransformedPos(hkGetRigidBody(output.m_rootCollidable)->getTransform(), triangle->getVertex(1)); transformedVertices[2].setTransformedPos(hkGetRigidBody(output.m_rootCollidable)->getTransform(), triangle->getVertex(2)); HK_DISPLAY_LINE(transformedVertices[0], transformedVertices[1], triangleColor); HK_DISPLAY_LINE(transformedVertices[2], transformedVertices[1], triangleColor); HK_DISPLAY_LINE(transformedVertices[0], transformedVertices[2], triangleColor); // hkVector4 triangleNormal; // hkpTriangleUtil::calcNormal(triangleNormal, // transformedVertices[0], transformedVertices[1], transformedVertices[2]); // triangleNormal.normalize3(); // // hkVector4 trianglePos; // triangle->getCentre(trianglePos); // trianglePos.setTransformedPos(hkGetRigidBody(output.m_rootCollidable)->getTransform(), trianglePos); // // HK_DISPLAY_ARROW(trianglePos, triangleNormal, hkColor::ORANGE); } }
// This function casts a set of small rays along a curved path. // Leaving numSamplePoints as 0 will use however many points defined originally in the curve. hkReal PlanetGravityDemo::castCurvedRay( hkpClosestRayHitCollector& output, const hkpLinearParametricCurve& curve, int numSamplePoints ) { hkReal t_min, t_max; t_min = curve.getStart(); t_max = curve.getEnd(); if( numSamplePoints == 0 ) { numSamplePoints = curve.m_points.getSize(); } // Sample the curve and do a cast at each sample point for( int i = 0; i < numSamplePoints - 1; i++ ) { hkpWorldRayCastInput ray; hkReal difference = ( t_max - t_min ); hkReal parametricValue1 = t_min + ( difference * ( i / static_cast<hkReal>( numSamplePoints ) ) ); hkReal parametricValue2 = t_min + ( difference * ( (i + 1.0f) / static_cast<hkReal>( numSamplePoints ) ) ); curve.getPoint( parametricValue1, ray.m_from ); curve.getPoint( parametricValue2, ray.m_to ); HK_DISPLAY_LINE( ray.m_from, ray.m_to, hkColor::RED ); m_world->castRay( ray, output ); if( output.hasHit() ) { return parametricValue1 + ( ( parametricValue2 - parametricValue1 ) * output.getHit().m_hitFraction ); } output.reset(); } return -1.0f; }
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 DestructibleWallsDemo::drawBrick( const hkVector4& position, const hkVector4& halfsize) const { hkVector4 posA=position; hkVector4 posC=position; posA.sub4( halfsize ); posC.add4( halfsize ); hkVector4 posB=posA; posB(0) = posC(0); hkVector4 posD=posA; posD(1) = posC(1); posA(2) = posB(2) = posC(2)= posD(2) = .0f; HK_DISPLAY_LINE(posA, posB , 0xffffffff); HK_DISPLAY_LINE(posB, posC , 0xffffffff); HK_DISPLAY_LINE(posC, posD , 0xffffffff); HK_DISPLAY_LINE(posD, posA , 0xffffffff); HK_DISPLAY_LINE(posA, posC , 0xffffffff); }
void WorldLinearCastMultithreadedDemo::displayRootCdBody( hkpWorld* world, const hkpCollidable* collidable, hkpShapeKey key) { hkpShapeCollection::ShapeBuffer shapeBuffer; // Check, whether we have a triangle from the landscape or a single object if ( key == HK_INVALID_SHAPE_KEY ) { // now we have a single object as we are not part of a hkpShapeCollection HK_SET_OBJECT_COLOR((hkUlong)collidable, hkColor::rgbFromChars(160,160,160)); } else { // now we need to get our triangle. // Attention: The next lines make certain assumptions about how the landscape is constructed: // 1. The landscape is a single hkMoppShape with wraps a hkpShapeCollection, which // produces only triangles. // 2. All hkShapeCollections are wrapped with a hkpBvTreeShape HK_ASSERT(0x3e93321f, world->getCollisionDispatcher()->hasAlternateType( collidable->getShape()->getType(), HK_SHAPE_BV_TREE ) ); const hkpBvTreeShape* bvTreeShape = static_cast<const hkpBvTreeShape*>(collidable->getShape()); hkpShapeKey triangleId = key; const hkpShape* childShape = bvTreeShape->getContainer()->getChildShape( triangleId, shapeBuffer ); HK_ASSERT(0x23f67112, childShape->getType() == HK_SHAPE_TRIANGLE ); const hkpTriangleShape* triangle = static_cast<const hkpTriangleShape*>( childShape ); hkVector4 vertex[3]; vertex[0].setTransformedPos(collidable->getTransform(), triangle->getVertex(0)); vertex[1].setTransformedPos(collidable->getTransform(), triangle->getVertex(1)); vertex[2].setTransformedPos(collidable->getTransform(), triangle->getVertex(2)); // Draw the border of the triangle HK_DISPLAY_LINE(vertex[0], vertex[1], hkColor::WHITE); HK_DISPLAY_LINE(vertex[2], vertex[1], hkColor::WHITE); HK_DISPLAY_LINE(vertex[0], vertex[2], hkColor::WHITE); if ( (const void*)childShape != (const void*)shapeBuffer && shapeBuffer[10] == 0 ) { HK_ASSERT(0x20b8765d, 0); } } }
void MyCollisionListener::contactPointAddedCallback( hkpContactPointAddedEvent& event ) { // // draw the contact point as a little red star // { const hkVector4& start = event.m_contactPoint->getPosition(); for ( int i = 0; i < 20; i++ ) { hkVector4 dir( hkMath::sin( i * 1.0f ), hkMath::cos( i * 1.0f ), hkMath::sin(i * 5.0f ) ); dir.setMul4(0.3f, dir); hkVector4 end; end.setAdd4(start, dir); HK_DISPLAY_LINE(start, end, hkColor::RED); } } // // collect all information in our own data structure // as the havok memory manager is really really fast, // allocating lots of small structures is acceptable // if ( event.m_contactPointProperties->getUserData() == HK_NULL ) { ContactPointInfo* info = new ContactPointInfo; info->m_uniqueId = m_uniqueIdCounter++; event.m_contactPointProperties->setUserData( reinterpret_cast<hkUlong>(info) ); // // printf some information // if (m_reportLevel >= hkDemoEnvironment::REPORT_INFO) { if ( event.isToi() ) { hkprintf("Toi userId=%i created\n", info->m_uniqueId ); } else { int cpId = event.asManifoldEvent().m_contactPointId; hkprintf("Contact Point userId=%i created: contactId=%i\n", info->m_uniqueId, cpId ); } } } // By setting the ProcessContactCallbackDelay to 0 we will receive callbacks for // any collisions processed for this body every frame (simulation step), i.e. the delay between // any such callbacks is 0 frames. // If you wish to only be notified every N frames simply set the delay to be N-1. // The default is 65536, i.e. (for practical purpose) once for the first collision only, until // the bodies separate to outside the collision tolerance. event.m_callbackFiredFrom->setProcessContactCallbackDelay(0); }
void VehicleDisplayUtils::updateTyremarks( hkReal timestep, hkpVehicleInstance* vehicle ) { // Update the skidmarks. Note: as the vehicle is integrated, we msut add our // current velocity to our skidmarks to compensate for the missing integration // of the contact point information. vehicle->m_tyreMarks->updateTyremarksInfo( timestep, vehicle ); for (int wheel = 0; wheel < vehicle->m_data->m_numWheels; wheel++ ) { const hkpTyremarksWheel &tyremarks_wheel = *vehicle->m_tyreMarks->m_tyremarksWheel[wheel]; const int num_points = tyremarks_wheel.m_tyremarkPoints.getSize(); const hkVector4* startl = &tyremarks_wheel.getTyremarkPoint(0).m_pointLeft; const hkVector4* startr = &tyremarks_wheel.getTyremarkPoint(0).m_pointRight; for (int p_it=1; p_it < num_points; p_it++) { const hkpTyremarkPoint* tyremark_point = &tyremarks_wheel.getTyremarkPoint(p_it); const hkVector4* endl = &tyremark_point->m_pointLeft; const hkVector4* endr = &tyremark_point->m_pointRight; hkReal alpha = (*startl)(3); if ( alpha ) { // Note: alpha display does not work with the havok graphics engine but // the next lines should be ok. To trim the alpha values you can use // m_minSkidmarkEnergy and m_maxSkidmarkEnergy. const long color = int( alpha ) <<24; HK_DISPLAY_LINE( *startl, *endl, color); HK_DISPLAY_LINE( *startr, *endr, color); } startl = endl; startr = endr; } } }
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); } }
// // This collector draws a collered line from the last hit to the new hit. // Nicely, the broadphase raycaster will sort all the objects, so our ray // will hit in the correct order. However relying on the broadphase sorting // does not work, if big objects, like landscape are involved. // virtual void addRayHit( const hkpCdBody& cdBody, const hkpShapeRayCastCollectorOutput& hitInfo ) { // // Display the current hit from the last intersection point // hkVector4 lastHit; lastHit.setInterpolate4( m_ray.m_from, m_ray.m_to, m_lastHitFraction ); hkVector4 hitPoint; hitPoint.setInterpolate4( m_ray.m_from, m_ray.m_to, hitInfo.m_hitFraction ); m_lastHitFraction = hitInfo.m_hitFraction; hkpWorldObject *object = static_cast<hkpWorldObject*>( cdBody.getRootCollidable()->getOwner() ); HK_DISPLAY_LINE( lastHit, hitPoint, m_lastColor); m_lastColor = object->getProperty( UserRayHitCollectorDemo::COLOR_PROPERTY_ID ).getInt(); }
// Raycast from the bottom (Y is up) to make sure that raycasting against the scaled mopp works void MoppInstancingDemo::checkRayCasts(const hkpMoppBvTreeShape* mopp, const hkTransform& t) { hkAabb aabb; mopp->getAabb(hkTransform::getIdentity(), 1.0f, aabb); hkVector4 extents; extents.setSub4(aabb.m_max, aabb.m_min); const int numX=10, numZ = 10; const hkReal deltaX = extents(0) / hkReal(numX-1); const hkReal deltaZ = extents(2) / hkReal(numZ-1); for (int x=0; x<numX; x++) { for (int z=0; z<numZ; z++) { hkpShapeRayCastInput input; hkpShapeRayCastOutput output; input.m_from(0) = aabb.m_min(0) + hkReal(x)*deltaX; input.m_from(1) = aabb.m_min(1) ; input.m_from(2) = aabb.m_min(2) + hkReal(z)*deltaZ; input.m_to = input.m_from; input.m_to(1) = aabb.m_max(1); input.m_rayShapeCollectionFilter = HK_NULL; mopp->castRay(input, output); hkVector4 worldFrom, worldTo; worldFrom.setTransformedPos(t, input.m_from); worldTo.setTransformedPos(t, input.m_to); if (output.hasHit()) { hkVector4 hitpoint; hitpoint.setInterpolate4(worldFrom, worldTo, output.m_hitFraction); HK_DISPLAY_LINE(worldFrom, hitpoint, hkColor::GREEN); } // Check that the naive raycast gives the same results hkpShapeRayCastOutput testOutput; mopp->getShapeCollection()->castRay(input, testOutput); HK_ASSERT(0x793171a3, hkMath::equal(testOutput.m_hitFraction, output.m_hitFraction) ); if ( !hkMath::equal(testOutput.m_hitFraction, 1.0f) ) { HK_ASSERT(0x793171a3, testOutput.m_normal.equals3(output.m_normal) ); } } } }
void SlidingWorldDemo::drawAabb(const hkVector4& m_minExtent, const hkVector4& m_maxExtent, int color) { hkArray<hkVector4> lines; lines.setSize(24); lines[0].set(m_minExtent(0),m_minExtent(1),m_minExtent(2)); lines[1].set(m_minExtent(0),m_maxExtent(1),m_minExtent(2)); lines[2].set(m_minExtent(0),m_minExtent(1),m_minExtent(2)); lines[3].set(m_minExtent(0),m_minExtent(1),m_maxExtent(2)); lines[4].set(m_minExtent(0),m_minExtent(1),m_minExtent(2)); lines[5].set(m_maxExtent(0),m_minExtent(1),m_minExtent(2)); lines[6].set(m_maxExtent(0),m_maxExtent(1),m_maxExtent(2)); lines[7].set(m_maxExtent(0),m_maxExtent(1),m_minExtent(2)); lines[8].set(m_maxExtent(0),m_maxExtent(1),m_maxExtent(2)); lines[9].set(m_minExtent(0),m_maxExtent(1),m_maxExtent(2)); lines[10].set(m_maxExtent(0),m_maxExtent(1),m_maxExtent(2)); lines[11].set(m_maxExtent(0),m_minExtent(1),m_maxExtent(2)); lines[12].set(m_minExtent(0),m_maxExtent(1),m_minExtent(2)); lines[13].set(m_maxExtent(0),m_maxExtent(1),m_minExtent(2)); lines[14].set(m_minExtent(0),m_maxExtent(1),m_minExtent(2)); lines[15].set(m_minExtent(0),m_maxExtent(1),m_maxExtent(2)); lines[16].set(m_maxExtent(0),m_maxExtent(1),m_minExtent(2)); lines[17].set(m_maxExtent(0),m_minExtent(1),m_minExtent(2)); lines[18].set(m_minExtent(0),m_maxExtent(1),m_maxExtent(2)); lines[19].set(m_minExtent(0),m_minExtent(1),m_maxExtent(2)); lines[20].set(m_minExtent(0),m_minExtent(1),m_maxExtent(2)); lines[21].set(m_maxExtent(0),m_minExtent(1),m_maxExtent(2)); lines[22].set(m_maxExtent(0),m_minExtent(1),m_maxExtent(2)); lines[23].set(m_maxExtent(0),m_minExtent(1),m_minExtent(2)); for (int i=0; i<24; i+=2) { HK_DISPLAY_LINE(lines[i], lines[i+1], color); } }
// Display the constraint, and update this constrained axis (which is specified in World space). // If we update the axis to always pass through the pivots of the two bodies it will act // correctly no matter how the bodies are oriented. hkDemo::Result ConstraintKitDemo::stepDemo() { // // Work out pivots in World space // hkVector4 pivotAW, pivotBW; { hkVector4 pivotA = *(m_constraintData->getParameters(m_pivotAIndex)); hkVector4 pivotB = *(m_constraintData->getParameters(m_pivotBIndex)); hkpRigidBody* ra = (hkpRigidBody*) m_constraint->getEntityA(); hkpRigidBody* rb = (hkpRigidBody*) m_constraint->getEntityB(); pivotAW.setTransformedPos(ra->getTransform(), pivotA); pivotBW.setTransformedPos(rb->getTransform(), pivotB); } hkVector4 newAxis; newAxis.setSub4(pivotAW, pivotBW); hkReal length = newAxis.length3(); // Only update axis if length non-zero (otherwise we can't normalize it correctly) if(length > 1e-3f) { newAxis.normalize3(); m_constraintData->setParameters(m_axisIndex, 1, &newAxis); } // Set colour as RED if outside limits (constraint forces applied) // or as YELLOW if inside (constraint has no effect, rope is "slack"). unsigned int colour = (length > 5.0f) ? hkColor::RED : hkColor::YELLOW; HK_DISPLAY_LINE(pivotAW, pivotBW, colour); return hkDefaultPhysicsDemo::stepDemo(); }
hkDemo::Result MotionExtractionDemo::stepDemo() { static hkReal val = 0.0f; hkReal stickX = m_env->m_gamePad->getStickPosX(1); val += (stickX - val) * 0.3f; hkReal leftWeight = (val < 0) ? -val : 0; hkReal rightWeight = (val > 0) ? val : 0; m_control[1]->setMasterWeight( leftWeight ); m_control[2]->setMasterWeight( rightWeight ); // Grab accumulated motion { hkQsTransform deltaMotion; deltaMotion.setIdentity(); m_skeletonInstance->getDeltaReferenceFrame( m_timestep, deltaMotion); m_currentMotion.setMulEq(deltaMotion); } // Show the extracted motion for the next 2 seconds const hkReal freq = 60; hkVector4 lp = m_currentMotion.getTranslation(); for (hkReal tm = 0.0f; tm < 2.0f; tm += 1.0f / freq) { hkQsTransform deltaMotion; deltaMotion.setIdentity(); m_skeletonInstance->getDeltaReferenceFrame(tm, deltaMotion); hkVector4 p; p.setTransformedPos(m_currentMotion, deltaMotion.getTranslation()); HK_DISPLAY_LINE( lp, p, 0xffff0f0f); lp = p; } // Advance the active animations m_skeletonInstance->stepDeltaTime( m_timestep ); hkaPose pose (m_skeleton); // Sample the active animations and combine into a single pose m_skeletonInstance->sampleAndCombineAnimations( pose.accessUnsyncedPoseLocalSpace().begin(), pose.getFloatSlotValues().begin() ); AnimationUtils::drawPose( pose, hkQsTransform::getIdentity() ); // Skin { // Work with the pose in Model space const hkArray<hkQsTransform>& poseInWorld = pose.getSyncedPoseModelSpace(); // Construct the composite world transform const int boneCount = m_skeleton->m_numBones; hkLocalArray<hkTransform> compositeWorldInverse( boneCount ); compositeWorldInverse.setSize( boneCount ); // Convert accumlated info to graphics matrix hkTransform graphicsTransform; m_currentMotion.copyToTransform(graphicsTransform); // Skin the meshes for (int i=0; i < m_numSkinBindings; i++) { // assumes either a straight map (null map) or a single one (1 palette) hkInt16* usedBones = m_skinBindings[i]->m_mappings? m_skinBindings[i]->m_mappings[0].m_mapping : HK_NULL; int numUsedBones = usedBones? m_skinBindings[i]->m_mappings[0].m_numMapping : boneCount; // Multiply through by the bind pose inverse world inverse matrices for (int p=0; p < numUsedBones; p++) { int boneIndex = usedBones? usedBones[p] : p; compositeWorldInverse[p].setMul( poseInWorld[ boneIndex ], m_skinBindings[i]->m_boneFromSkinMeshTransforms[ boneIndex ] ); } AnimationUtils::skinMesh( *m_skinBindings[i]->m_mesh, graphicsTransform, compositeWorldInverse.begin(), *m_env->m_sceneConverter ); } } return hkDemo::DEMO_OK; }
// doReach() is always called, with reachOn=true when we want to do reaching, reachOn=false otherwise // This is because is internally handles the easing in and out. It is called once for each arm void Gdc2005Demo::doReach (hkBool reachOn, WhichArm whichArm, hkaPose& thePose) { const hkVector4 upWS(0,1,0); const hkInt16 shoulderIdx = m_shoulderIdx[whichArm]; const hkInt16 elbowIdx = m_elbowIdx[whichArm]; const hkInt16 handIdx = m_handIdx[whichArm]; hkQsTransform modelFromReference = thePose.getBoneModelSpace(m_reachReferenceBoneIdx); // Remove all local translations from the reference bone, use only its orientation modelFromReference.m_translation.set(0.0f, 0.0f, 0.0f); hkQsTransform worldFromReference; worldFromReference.setMul(m_currentTransform, modelFromReference); // Look for a place to reach hkBool found = false; hkVector4 pointWS; hkVector4 normalWS; // Search for closest point, unless we are switching off reaching if (reachOn) { // The position of our sensor: hkVector4 radarPosWS; radarPosWS.setTransformedPos(worldFromReference, m_radarLocationRS[whichArm]); // Display it if (m_options.m_Display.m_showIkInfo) { HK_DISPLAY_STAR( radarPosWS, 0.1f, 0xffffffff); } // Use hkpWorld::getClosestPoints() to look for candidates // Use a sphere of 0.05m radius located at the radar point hkTransform sphereTransform; sphereTransform.setIdentity(); sphereTransform.setTranslation(radarPosWS); hkpSphereShape sphere(0.05f); // Use the ray cast filter layer, so it only landscape and movable environment is detected hkpCollidable collidable( &sphere, &sphereTransform ); collidable.setCollisionFilterInfo(hkpGroupFilter::calcFilterInfo(LAYER_RAYCAST, 0)); // Collect all possible candidates // Use the same filter we use for the world (it already knows which layers collide with which layers) hkpAllCdPointCollector collector; hkpProcessCollisionInput input =*m_world->getCollisionInput(); input.m_tolerance = 0.4f; input.m_filter = m_world->getCollisionFilter(); m_world->getClosestPoints( &collidable, input, collector); // Sort all the candidates (closest first) so the first good one is the best one collector.sortHits(); hkArray<hkpRootCdPoint>& allHits = collector.getHits(); // Now, we have a list of closest points (closest objects). Ray cast against them to find out if they // have a surface we can lay the hand on. for (int hitId = 0; hitId < allHits.getSize(); hitId++) { hkContactPoint& contactPoint = allHits[hitId].m_contact; hkVector4 closest = contactPoint.getPosition(); // Construct a unit vector that points from the radar towards the detected point in WS // This is the "away" vector hkVector4 awayWS; { awayWS.setSub4(closest, radarPosWS); awayWS.addMul4(-awayWS.dot3(upWS), upWS); const hkReal length = awayWS.length3(); if (length>1e-5f) { awayWS.mul4(1.0f/length); } else { const hkVector4 forwardMS(0.0f,0.0f,1.0f); awayWS.setRotatedDir(m_currentTransform.getRotation(), forwardMS); } } // The closest point is usually at the edge. Move it a little away (10cm) closest.addMul4(0.10f, awayWS); // Do a raycast above it. We reuse the raycast interface object that we use for the foot placement const hkReal upDistance = 0.3f; const hkReal downDistance = 0.3f; const hkReal totalLength = upDistance + downDistance; hkVector4 startWS; startWS.setAddMul4(closest,upWS, upDistance); hkVector4 endWS; endWS.setAddMul4(closest,upWS, -downDistance); hkReal hitFraction; const hkBool properHit = m_raycastInterface->castRay(startWS, endWS, hitFraction, normalWS); if (m_options.m_Display.m_showIkInfo) { HK_DISPLAY_LINE( startWS, endWS, 0x00f8f8f8 ); } // Nothing found, check the next candidate if (!properHit) { continue; } // The surface is not (roughly) horizontal, check the next candidate if (normalWS.dot3(upWS)<0.8f) { continue; } // Good, we have a good candidate. Check where the landing point is pointWS.setAddMul4(startWS, upWS, -totalLength * hitFraction); // If we are dealing with a box (the big crates), move the point closer since to account for the radius { const bool isABox = allHits[hitId].m_rootCollidableB->getShape()->getType() == HK_SHAPE_CONVEX_TRANSLATE; // Box : move it back closer if (isABox) { pointWS.addMul4(-0.1f, awayWS); } } // Finally, reject points too far away (the arm won't reach) hkVector4 vDistance;vDistance.setSub4(pointWS, radarPosWS); const hkReal distanceSqr = vDistance.lengthSquared3(); if (distanceSqr > 0.16f) { continue; } // If we reached here, we have a found a place where to lay our hand, don't look for more candidates found = true; break; } // Did we find a place to put the hand on? if (found) { if (m_options.m_Display.m_showIkInfo) { HK_DISPLAY_STAR(pointWS, 0.5f, 0xffffff00); } // Move the point a little up to account for the width of the hand pointWS.addMul4(0.03f, upWS); // Interpolate the point and the normal with the previous ones, unless it's the first time of course if (m_reachWeight[whichArm]>0.0f) { // Use the gain specified by the user const hkReal moveGain = m_options.m_IK.m_handIkMoveGain; pointWS.setInterpolate4(m_previousReachPointWS[whichArm], pointWS, moveGain); normalWS.setInterpolate4(m_previousNormalWS[whichArm], normalWS, moveGain); normalWS.normalize3(); } // Store them to interpolate the next frame m_previousReachPointWS[whichArm] = pointWS; m_previousNormalWS[whichArm] = normalWS; } } // Do gain on the weight for the reaching if (found) { // If reach is on : go towards 1.0f (ease in), otherwise towards 0.0f (ease off) const hkReal desiredWeight = (reachOn) ? 1.0f : 0.0f; const hkReal diffWeight = desiredWeight - m_reachWeight[whichArm]; const hkReal reachGain = m_options.m_IK.m_handIkReachGain; m_reachWeight[whichArm] += diffWeight * reachGain; } else { // If we didn't find anything, go towards 0.0f (ease off) const hkReal leaveGain = m_options.m_IK.m_handIkLeaveGain; m_reachWeight[whichArm] *= (1.0f - leaveGain); pointWS = m_previousReachPointWS[whichArm]; normalWS = m_previousNormalWS[whichArm]; } // If weight 0 or very small, switch off completely and do nothing if (m_reachWeight[whichArm] < 0.01f) { m_reachWeight[whichArm] = 0.0f; } else { // Fix the position, use the Two Joints IK solver { hkVector4 pointMS; pointMS.setTransformedInversePos(m_currentTransform, pointWS); hkaTwoJointsIkSolver::Setup setup; setup.m_firstJointIdx =shoulderIdx; setup.m_secondJointIdx = elbowIdx; setup.m_endBoneIdx = handIdx; setup.m_hingeAxisLS = m_elbowAxis[whichArm]; setup.m_firstJointIkGain = m_reachWeight[whichArm]; setup.m_secondJointIkGain = m_reachWeight[whichArm]; setup.m_endJointIkGain = m_reachWeight[whichArm]; setup.m_endTargetMS = pointMS; hkaTwoJointsIkSolver::solve(setup, thePose); } // Now, rotate the wrist, so the palm follows the surface { const hkVector4 upHandLocalSpace (0,1,0); // In this rig the hand Y axis points up (when the palm faces down) const hkQuaternion currentHandRotation = thePose.getBoneModelSpace(handIdx).getRotation(); hkVector4 currentUpMS; currentUpMS.setRotatedDir(currentHandRotation, upHandLocalSpace); hkVector4 desiredUpMS; desiredUpMS.setRotatedInverseDir(m_currentTransform.getRotation(), normalWS); { hkVector4 currentUpWS; currentUpWS.setRotatedDir(m_currentTransform.getRotation(), currentUpMS); hkVector4 desiredUpWS; desiredUpWS.setRotatedDir(m_currentTransform.getRotation(), desiredUpMS); } // Calculate the shortest rotation that matches both axis hkQuaternion extraRotation; extraRotation.setIdentity(); { // Look for the shortest rotation that would bring the foot to the right orientation // We scale that rotation (its angle) by the weight of the reach IK const hkReal dotProd = currentUpMS.dot3(desiredUpMS); if ( (dotProd - 1.0f) < - HK_REAL_EPSILON) { const hkReal rotationAngle = hkMath::acos(dotProd); hkVector4 rotationAxis; rotationAxis.setCross(currentUpMS, desiredUpMS); if (rotationAxis.length3()>1e-14f) { rotationAxis.normalize3(); extraRotation.setAxisAngle(rotationAxis, rotationAngle*m_reachWeight[whichArm]); } } } // Apply the extra rotation hkQuaternion newRotationMS; newRotationMS.setMul(extraRotation, currentHandRotation); newRotationMS.normalize(); // Set the new rotation to the hand in the pose thePose.accessBoneModelSpace(handIdx, hkaPose::PROPAGATE).setRotation(newRotationMS); } } }
// // 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); } } } }
void displayFinalRay( ) { hkVector4 lastHit; lastHit.setInterpolate4( m_ray.m_from, m_ray.m_to, m_lastHitFraction ); HK_DISPLAY_LINE( lastHit, m_ray.m_to, m_lastColor); }
void ShapeQueryDemo::worldLinearCast( hkpWorld* world, hkReal time, hkArray<QueryObject>& queryObjects ) { // For this cast we use a hkpClosestCdPointCollector to gather the results: hkpClosestCdPointCollector collector[10]; // Since we're not too concerned with perfect accuracy, we can set the early out // distance to give the algorithm a chance to exit more quickly: world->getCollisionInput()->m_config->m_iterativeLinearCastEarlyOutDistance = 0.1f; // Cast direction hkVector4 displacement( 0.0f, -30.0f, 0.0f ); // Perform all queries in one go (do not interleave with examining the results, // as this is bad for code and data cache. // The core loop is very similar to the one we described for the world ray cast, // with several casts being performed forming a circular pattern. The only // real difference this time is that we get the shape to cast (and the 'from' // position) from our collection of 'QueryObjects'. { for ( int i = 0; i < queryObjects.getSize(); i++ ) { QueryObject& qo = queryObjects[i]; // Create a new position: all objects move in a circle hkReal t = time + HK_REAL_PI * 2 * i / queryObjects.getSize(); hkVector4 pos( hkMath::sin( t ) * 10.0f, 10.0f, hkMath::cos( t ) * 10.0f ); // Set our position in our motion state qo.m_transform->setTranslation(pos); // // Query for intersecting objects // for (int k = 0; k < NUM_ITER ; k++ ) { MY_TIMER_BEGIN(i); { // Input hkpLinearCastInput input; input.m_to.setAdd4( pos, displacement ); // Output hkpClosestCdPointCollector& coll = collector[i]; coll.reset(); // Call the engine world->linearCast( qo.m_collidable, input, coll ); } MY_TIMER_END(); } } } // // Batch examine the results // // All that remains to do now is to display the results and update the position of our 'QueryObjects' (QO). We use the updateGeometry(...) // method from hkDebugDisplay to do the QO updates and once again we draw everything in GREEN: { for ( int i = 0; i < queryObjects.getSize(); i++ ) { QueryObject& qo = queryObjects[i]; // update our display if ( collector[i].hasHit() ) { // move our position to the hit and draw a line along the cast direction hkVector4& pos = qo.m_transform->getTranslation(); hkVector4 to; to.setAdd4( pos, displacement ); hkVector4 newPos; newPos.setInterpolate4( pos, to, collector[i].getHitContact().getDistance() ); HK_DISPLAY_LINE(pos, newPos, hkColor::GREEN); // Update our QO qo.m_transform->setTranslation( newPos ); hkDebugDisplay::getInstance().updateGeometry( qo.m_collidable->getTransform(), (hkUlong)qo.m_collidable, 0); // call a function to display the details displayRootCdPoint( world, collector[i].getHit() ); } } } }
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 ShapeQueryDemo::worldRayCast( hkpWorld* world, hkReal time, hkBool useCollector ) { const int N_RAY_CASTS = 10; hkpWorldRayCastInput rayInputs[N_RAY_CASTS]; hkpWorldRayCastOutput rayOutputs[N_RAY_CASTS]; hkpClosestRayHitCollector rayCollector[ N_RAY_CASTS ]; // We may also choose whether or not we wish to use a collector or output structure to // gather the results of the query (hkpWorld::castRay has two overloaded methods). hkVector4 displacement( 0.0f, -30.0f, 0.0f ); // Perform all queries in one go (do not interleave with examining the results, // as this is bad for code and data cache. { for ( int i = 0; i < N_RAY_CASTS; i++ ) { hkpWorldRayCastInput& in = rayInputs[i]; // create a new position: all objects move in a circle hkReal t = time + (HK_REAL_PI * 2 * i) / N_RAY_CASTS; hkVector4 pos( hkMath::sin( t ) * 8.0f, 10.0f, hkMath::cos( t ) * 8.0f ); // Set our position in our input structure in.m_from = pos; in.m_to.setAdd4( pos, displacement ); // // Query for intersecting objects. monitor the timing. // for (int k = 0; k < NUM_ITER ; k++ ) { HK_TIMER_BEGIN("raycast", HK_NULL); if ( useCollector ) { hkpClosestRayHitCollector& collector = rayCollector[i]; collector.reset(); world->castRay( in, collector ); } else { hkpWorldRayCastOutput& out = rayOutputs[i]; out.reset(); world->castRay( in, out ); } HK_TIMER_END( ); } } } // Once we've cast our rays all that remains to do is display them! // We can do this by using both the input structure (to find the // start position) and the output structure (which in the case // of using collectors uses the collector's internal // hkpWorldRayCastOutput structure) to find the end point. // We plot N_RAY_CAST casts, one for each iteration, in a GREEN colour. { for ( int i = 0; i < N_RAY_CASTS; i++ ) { hkpWorldRayCastInput& in = rayInputs[i]; const hkpWorldRayCastOutput* out; if ( useCollector ) { out = &rayCollector[i].getHit(); } else { out = &rayOutputs[i]; } // Display the ray { hkVector4 hitPoint; hitPoint.setInterpolate4( in.m_from, in.m_to, out->m_hitFraction ); HK_DISPLAY_LINE(in.m_from, hitPoint, hkColor::GREEN); } // Display the result if ( out->hasHit() ) { displayRayHit( world, in, *out ); } } } }
hkDemo::Result WorldLinearCastMultithreadedDemo::stepDemo() { if (m_jobThreadPool->getNumThreads() == 0) { HK_WARN(0x34561f23, "This demo does not run with only one thread"); return DEMO_STOP; } // const WorldLinearCastMultithreadedDemoVariant& variant = g_WorldLinearCastMultithreadedDemoVariants[m_variantId]; m_world->lock(); // Reset all object colors { for (int i = 0; i < m_rocksOnTheFloor.getSize(); i++) { HK_SET_OBJECT_COLOR((hkUlong)m_rocksOnTheFloor[i]->getCollidable(), hkColor::rgbFromChars(70,70,70)); } } // For this cast we use a hkpClosestCdPointCollector to gather the results: hkpClosestCdPointCollector collectors[10]; // Since we're not too concerned with perfect accuracy, we can set the early out // distance to give the algorithm a chance to exit more quickly: m_world->getCollisionInput()->m_config->m_iterativeLinearCastEarlyOutDistance = 0.1f; m_world->unlock(); // Cast direction & length. hkVector4 castVector( 0.0f, -30.0f, 0.0f ); int numQueryObjects = m_queryObjects.getSize(); // // Setup the output array where the resulting collision points will be returned. // hkpRootCdPoint* collisionPoints = hkAllocateChunk<hkpRootCdPoint>(numQueryObjects, HK_MEMORY_CLASS_DEMO); // // Setup commands: one command for each query object. // hkArray<hkpWorldLinearCastCommand> commands; { for ( int i = 0; i < numQueryObjects; i++ ) { QueryObject& queryObject = m_queryObjects[i]; // // Let QueryObjects move in circles. // hkVector4 position; { hkReal t = m_time + HK_REAL_PI * 2 * i / m_queryObjects.getSize(); position.set( hkMath::sin( t ) * 10.0f, 10.0f, hkMath::cos( t ) * 10.0f ); } queryObject.m_transform->setTranslation(position); hkpWorldLinearCastCommand& command = commands.expandOne(); { // Init input data. { command.m_input.m_to . setAdd4( position, castVector ); command.m_input.m_maxExtraPenetration = HK_REAL_EPSILON; command.m_input.m_startPointTolerance = HK_REAL_EPSILON; command.m_collidable = queryObject.m_collidable; } // Init output data. { command.m_results = &collisionPoints[i]; command.m_resultsCapacity = 1; command.m_numResultsOut = 0; } } } } // // Create the job header. // hkpCollisionQueryJobHeader* jobHeader; { jobHeader = hkAllocateChunk<hkpCollisionQueryJobHeader>(1, HK_MEMORY_CLASS_DEMO); } // // Setup hkpWorldLinearCastJob. // m_world->markForRead(); hkpWorldLinearCastJob worldLinearCastJob(m_world->getCollisionInput(), jobHeader, commands.begin(), commands.getSize(), 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. // 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. // m_semaphore.acquire(); m_world->unlockReadOnly(); } // // Display results. // { m_world->lock(); { for (int i = 0; i < commands.getSize(); i++) { QueryObject& queryObject = m_queryObjects[i]; hkpWorldLinearCastCommand& command = commands[i]; if ( command.m_numResultsOut > 0 ) { // move our position to the hit and draw a line along the cast direction hkVector4& pos = queryObject.m_transform->getTranslation(); hkVector4 to; to.setAdd4( pos, castVector ); hkVector4 newPos; newPos.setInterpolate4( pos, to, command.m_results->m_contact.getDistance() ); HK_DISPLAY_LINE(pos, newPos, hkColor::GREEN); // Update our QO queryObject.m_transform->setTranslation( newPos ); hkDebugDisplay::getInstance().updateGeometry( queryObject.m_collidable->getTransform(), (hkUlong)queryObject.m_collidable, 0); // call a function to display the details displayRootCdPoint( m_world, *command.m_results ); } else { // only draw a line along the cast direction hkVector4& pos = queryObject.m_transform->getTranslation(); hkVector4 to; to.setAdd4( pos, castVector ); HK_DISPLAY_LINE(pos, to, hkColor::GREEN); // Update our QO hkVector4 nirvana(10000.0f, 10000.0f, 10000.0f); queryObject.m_transform->setTranslation( nirvana ); hkDebugDisplay::getInstance().updateGeometry( queryObject.m_collidable->getTransform(), (hkUlong)queryObject.m_collidable, 0); } } } m_world->unlock(); } // // Free temporarily allocated memory. // hkDeallocateChunk( jobHeader, 1, HK_MEMORY_CLASS_DEMO ); hkDeallocateChunk(collisionPoints, numQueryObjects, HK_MEMORY_CLASS_DEMO); m_time += 0.005f; return hkDefaultPhysicsDemo::stepDemo(); }
static void phantomLinearCast( ShapeQueryDemo* demo, hkArray<T*>& phantoms, hkReal offset ) { hkpClosestCdPointCollector collector[10]; // We are not really interested in the perfect accuracy, so we give the // algorithm a chance for early outs demo->m_world->getCollisionInput()->m_config->m_iterativeLinearCastEarlyOutDistance = 0.1f; // Cast direction hkVector4 displacement( 0.0f, -30.0f, 0.0f ); // Perform all queries in one go (do not interleave with examining the results, // as this is bad for code and data cache. { for ( int i = 0; i < phantoms.getSize(); i++ ) { // create a new position: all objects move in a circle hkReal t = demo->m_time + HK_REAL_PI * 2 * i / phantoms.getSize() + offset; hkVector4 pos( hkMath::sin( t ) * 12.0f, 10.0f, hkMath::cos( t ) * 10.0f ); // // Query for intersecting objects // for (int k = 0; k < NUM_ITER ; k++ ) { MY_TIMER_BEGIN(i); { // input hkpLinearCastInput input; input.m_to.setAdd4( pos, displacement ); // output hkpClosestCdPointCollector& coll = collector[i]; coll.reset(); // Call the engine phantoms[i]->setPositionAndLinearCast( pos, input, coll, HK_NULL ); } MY_TIMER_END(); } } } // As usual for phantoms we need only concern ourselves with updating the display with the results of the // linear cast. In this case we draw everything in ORANGE. { for ( int i = 0; i < phantoms.getSize(); i++ ) { // update our display if ( collector[i].hasHit() ) { // move our position to the hit and draw a line along the cast direction const hkVector4& pos = phantoms[i]->getTransform().getTranslation(); hkVector4 to; to.setAdd4( pos, displacement ); hkVector4 newPos; newPos.setInterpolate4( pos, to, collector[i].getHitContact().getDistance() ); HK_DISPLAY_LINE(pos, newPos, hkColor::rgbFromChars( 240, 200, 0, 200 )); phantoms[i]->setPosition( newPos ); // call a function to display the details demo->displayRootCdPoint( demo->m_world, collector[i].getHit() ); } } } }
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; }
void ShapeQueryDemo::phantomRayCast( hkpWorld* world, hkReal time, hkArray<hkpAabbPhantom*>& phantoms, hkBool useCollector ) { const int N_RAY_CASTS = 10; hkpWorldRayCastInput rayInputs[N_RAY_CASTS]; hkpWorldRayCastOutput rayOutputs[N_RAY_CASTS]; hkpClosestRayHitCollector rayCollector[ N_RAY_CASTS ]; hkVector4 displacement( 0.0f, -30.0f, 0.0f ); // Perform all queries in one go (do not interleave with examining the results, // as this is bad for code and data cache. { for ( int i = 0; i < N_RAY_CASTS; i++ ) { hkpWorldRayCastInput& in = rayInputs[i]; // create a new position: all objects move in a circle hkReal t = time + (HK_REAL_PI * 2 * i) / N_RAY_CASTS; hkVector4 pos( hkMath::sin( t ) * 12.0f, 10.0f, hkMath::cos( t ) * 12.0f ); // Set our position in our input structure in.m_from = pos; in.m_to.setAdd4( pos, displacement ); // // Query for intersecting objects // for (int k = 0; k < NUM_ITER ; k++ ) { HK_TIMER_BEGIN("raycast", HK_NULL); // Move our phantom so it encapsulates our ray hkpAabbPhantom* phantom = phantoms[i]; { hkAabb aabb; aabb.m_min.setMin4( in.m_to, in.m_from ); aabb.m_max.setMax4( in.m_to, in.m_from ); phantom->setAabb( aabb ); } if ( useCollector ) { hkpClosestRayHitCollector& collector = rayCollector[i]; collector.reset(); phantom->castRay( in, collector ); } else { hkpWorldRayCastOutput& out = rayOutputs[i]; out.reset(); phantom->castRay( in, out ); } HK_TIMER_END( ); } } } // // Batch display the results // NOTE that phantom displays have been forced off for this variant. // { for ( int i = 0; i < N_RAY_CASTS; i++ ) { hkpWorldRayCastInput& in = rayInputs[i]; const hkpWorldRayCastOutput* out; if ( useCollector ) { out = &rayCollector[i].getHit(); hkVector4 off(.1f,0,0); in.m_from.add4( off ); in.m_to.add4( off ); } else { out = &rayOutputs[i]; } // Display the ray { hkVector4 hitPoint; hitPoint.setInterpolate4( in.m_from, in.m_to, out->m_hitFraction ); HK_DISPLAY_LINE(in.m_from, hitPoint, hkColor::rgbFromChars( 240, 200, 0, 200 )); } // Display the result if ( out->hasHit() ) { displayRayHit( world, in, *out ); } } } }
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 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(); }