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