//
// Test function to make sure that the hits from the kd-tree are identical to the ones from the broadphase.
//
static hkBool compareHitArrays(const hkAabb& aabb, const hkPrimitiveId* kdHits, int numKdHits, const hkArray<hkpBroadPhaseHandlePair>& sapHits)
{
hkLocalArray<const hkpCollidable*> sapCollidables(sapHits.getSize());
for (int i=0; i<sapHits.getSize(); i++)
{
const hkpTypedBroadPhaseHandle* tp = static_cast<const hkpTypedBroadPhaseHandle*>( sapHits[i].m_b );
const hkpCollidable* collB = static_cast<hkpCollidable*>(tp->getOwner());
// Make sure no spurious hits from the broadphase
hkAabb colAabb;
hkpRigidBody* rb = hkGetRigidBody(collB);
if (rb)
{
collB->getShape()->getAabb(rb->getTransform(), 0.0f, colAabb);
hkBool aabbOverlap = colAabb.overlaps(aabb);
if (aabbOverlap)
sapCollidables.pushBack(collB);
}
}
// Make sure that the contents of the arrays at the same
// It would be faster to sort and do a string compare, but this is just as easy.
hkBool isOk = true;
for (int i=0; i<numKdHits; i++)
{
const hkpCollidable* kdCollidable = reinterpret_cast<const hkpCollidable*> (kdHits[i]);
int idx = sapCollidables.indexOf(kdCollidable);
if(idx < 0)
{
// There's a hit in the kd-tree's list but not the 3AxisSweep's list
// It's possible that 3AxisSweep skipped something. So get the AABB of the body in the kd-tree list.
hkAabb colAabb;
hkpRigidBody* rb = hkGetRigidBody(kdCollidable);
kdCollidable->getShape()->getAabb(rb->getTransform(), rb->getWorld()->getCollisionInput()->getTolerance(), colAabb);
hkBool aabbOverlap = colAabb.overlaps(aabb);
if (!aabbOverlap)
{
// Something in the list doesn't overlap with the query's aabb
isOk = false;
}
continue;
}
sapCollidables.removeAt(idx);
}
// If we made it this far, they must agree.
return isOk && sapCollidables.isEmpty();
}
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);
}
}
void LaunchPadListener::contactPointAddedCallback(hkpContactPointAddedEvent& event)
{
// Grab the body to launch
hkpRigidBody* collidedRb = hkGetRigidBody( event.m_bodyA->getRootCollidable() );
// If m_forceMagnitude is zero then launch toward a target
if( m_forceMagnitude == 0.0f )
{
collidedRb->setMaxLinearVelocity( 300.0f );
hkVector4 impulse;
impulse.setSub4( m_targetPosition, collidedRb->getPosition() );
impulse.mul4( 225.0f );
PlanetGravityDemo::applyScaledLinearImpulse( collidedRb, impulse );
}
// Just apply an impulse along the contact normal
else
{
hkVector4 impulse( event.m_contactPoint->getNormal() );
impulse.mul4( m_forceMagnitude * 225.0f );
collidedRb->setMaxLinearVelocity( 300.0f );
PlanetGravityDemo::applyScaledLinearImpulse( collidedRb, impulse );
}
event.m_status = HK_CONTACT_POINT_REJECT;
}
void contactPointRemovedCallback(const hkpRootCdPoint& point)
{
hkpRigidBody* body = hkGetRigidBody(point.m_rootCollidableB);
if ( body->hasProperty(HK_OBJECT_IS_LADDER) )
{
m_atLadder = false;
}
}
// hkpPhantom interface implementation
virtual void phantomEnterEvent( const hkpCollidable* collidableA, const hkpCollidable* collidableB, const hkpCollisionInput& env )
{
// the color can only be changed once the entity has been added to the world
hkpRigidBody* owner = hkGetRigidBody(collidableB);
// the "Collidables" here are "faked" so it's necessary to get the owner first in order
// to get the "real" collidable!
HK_SET_OBJECT_COLOR((hkUlong)owner->getCollidable(), hkColor::rgbFromChars(255, 0, 0));
//HK_SET_OBJECT_COLOR((int)&collidableB, hkColor::rgbFromChars(255, 0, 0));
}
// Ladder handling code goes here
void contactPointAddedCallback(const hkpRootCdPoint& point)
{
hkpRigidBody* body = hkGetRigidBody(point.m_rootCollidableB);
if ( body->hasProperty(HK_OBJECT_IS_LADDER) )
{
m_atLadder = true;
m_ladderNorm = point.m_contact.getNormal();
body->getPointVelocity(point.m_contact.getPosition(), m_ladderVelocity);
}
}
hkpRigidBody* CollisionFilterViewerUtil::getBodyUnderMousePointer(const hkDemoEnvironment* env, const hkpWorld* world)
{
hkpWorldRayCastInput input;
hkpWorldRayCastOutput output;
hkVector4 intersectionPointWorld;
castRayIntoWorldAtMousePointer(env, world, input, output);
if(output.hasHit())
{
hkpRigidBody* rb = hkGetRigidBody(output.m_rootCollidable);
return rb;
}
return HK_NULL;
}
void PortalPhantom::removeOverlappingCollidable( hkpCollidable* handle )
{
hkpRigidBody* rb = hkGetRigidBody(handle);
//if hovercraft send envent to checkpoint
if ( (rb != HK_NULL) && HavokEntityType::isEntityType(rb,HavokEntityType::CHARACTER ) ) {
HavokEntity * ch = reinterpret_cast<HavokEntity*>(rb->getUserData());
Entity * e = ch->getEntity();
if ( e->getCategory() == Hovercraft::CATEGORY ){
mPortal->onLeave(dynamic_cast<Hovercraft*>(e));
}
}
hkpAabbPhantom::removeOverlappingCollidable( handle );
}
// hkpPhantom interface implementation
virtual void phantomLeaveEvent( const hkpCollidable* collidableA, const hkpCollidable* collidableB )
{
// the color can only be changed once the entity has been added to the world
hkpRigidBody* owner = hkGetRigidBody(collidableB);
// the "Collidables" here are "faked" so it's necessary to get the owner first in order
// to get the "real" collidable!
HK_SET_OBJECT_COLOR((hkUlong)owner->getCollidable(), hkColor::rgbFromChars(0, 255, 0));
// If moving out AND falling down, apply impulse to fire it towards "wall"
if(owner->getLinearVelocity()(1) < 0.0f)
{
hkVector4 impulse(-50.0f, 220.0f, 0.0f);
owner->applyLinearImpulseAsCriticalOperation(impulse);
}
}
void SlidingWorldDemo::removeBodiesLeavingBroadphaseFromSimulation(const hkArray<hkpBroadPhaseHandle*>& objectsEnteringBroadphaseBorder)
{
for (int i = 0; i < objectsEnteringBroadphaseBorder.getSize(); i++)
{
hkpBroadPhaseHandle* handle = objectsEnteringBroadphaseBorder[i];
hkpCollidable* collidable = static_cast<hkpCollidable*>(static_cast<hkpTypedBroadPhaseHandle*>(handle)->getOwner());
// It might be a rigid body, or a phantom, so check both possibilities
hkpRigidBody* rigidBody = hkGetRigidBody(collidable);
if(rigidBody)
{
m_world->removeEntity( rigidBody );
}
hkpPhantom* phantom = hkGetPhantom(collidable);
if(phantom)
{
m_world->removePhantom( phantom );
}
}
}
hkDemo::Result PlanetGravityDemo::stepDemo()
{
// Update lighting
{
// Update the light source to be at the camera
float position[3];
m_cameraPosition.store3( position );
m_flashLight->setPosition( position );
// Update the light direction to be pointing toward the character controller rigid body
hkVector4 directionVector;
directionVector.setSub4( m_cameraPosition, m_characterRigidBody->getPosition() );
directionVector.mul4( -1.0f );
directionVector.normalize3();
float direction[3];
directionVector.store3( direction );
m_flashLight->setDirection( direction );
}
// Detach the camera from the character when P is pressed.
if( m_env->m_window->getKeyboard().wasKeyPressed('P') )
{
m_detachedCamera = !m_detachedCamera;
}
// Update turrets
for( int i = 0; i < m_turrets.getSize(); i++ )
{
Turret& turret = m_turrets[i];
turret.cooldown -= m_timestep;
// Make the turret spin
turret.hinge->setMotorTargetAngle( turret.hinge->getMotorTargetAngle() + ( m_timestep / 5.f ) );
// Bail out if the turret is "hot"
if( turret.cooldown > 0.0f )
{
continue;
}
// Generate a curved raycast and shoot the ray
// This has to be done every time-step as it's in world-space
{
const hkReal radius = 14.8f;
hkRotation rot;
hkVector4 offset;
hkVector4 turretDown;
rot.set( turret.turretRigidBody->getRotation() );
offset = turret.turretRigidBody->getPosition();
turretDown.setMul4( -1.0f, rot.getColumn(2) );
hkpLinearParametricCurve myCurve;
// Move the ray's source a little up so it's coming from the center of the barrel
hkTransform localTransform( hkQuaternion::getIdentity(), hkVector4( 0.0f, 0.0f, 0.7f ) );
// Create a curve of 20 points
for( int j = 0; j < 20; j++ )
{
hkReal angle = HK_REAL_PI * static_cast<hkReal>(j) / 15.0f;
hkVector4 newPoint( radius * 2.0f * sin( angle ),
0.0f,
radius * 2.0f * cos( angle ) );
newPoint.setTransformedPos( localTransform, newPoint );
newPoint.setTransformedPos( turret.turretRigidBody->getTransform(), newPoint );
newPoint.addMul4( radius * 2.0f, turretDown );
myCurve.addPoint( newPoint );
}
// We only need the closest hit (as our lasers can't pass through objects)
// so hkpClosestRayHitCollector is used.
hkpClosestRayHitCollector raycastOutput;
hkReal t = castCurvedRay( raycastOutput, myCurve, 20 );
// Apply a large force to the closest rb we hit, along the tangent at the colliding point
if( raycastOutput.hasHit() )
{
hkpRigidBody* hitRb = hkGetRigidBody( raycastOutput.getHit().m_rootCollidable );
if( hitRb->getMotionType() != hkpMotion::MOTION_FIXED )
{
hkVector4 tangent;
myCurve.getTangent( t, tangent );
tangent.mul4( 15000.0f );
applyScaledLinearImpulse( hitRb, tangent );
turret.cooldown = 3.0f;
}
}
}
}
m_world->markForWrite();
// Update the character context
m_characterRigidBody->m_up = m_worldUp;
hkReal posX = 0.0f;
hkReal posY = 0.0f;
if( !m_detachedCamera )
{
float deltaAngle;
CharacterUtils::getUserInputForCharacter( m_env, deltaAngle, posX, posY );
if( ( ( hkMath::fabs( posX ) < HK_REAL_MAX ) && ( hkMath::fabs( posY ) < HK_REAL_MAX ) ) && ( posX || posY ) )
{
// find new orientation in local space
hkVector4 newForward( -posY, 0.0f, -posX );
hkVector4 absoluteForward( 1.0f, 0.0f, 0.0f );
hkReal characterAngle = hkMath::acos( absoluteForward.dot3( newForward ) );
// Calculate cross product to get sign of rotation.
hkVector4 crossProduct;
crossProduct.setCross( absoluteForward, newForward );
if( crossProduct(1) < 0.0f )
{
characterAngle *= -1.0f;
}
// Rotate the character's rigid body to face in the direction it's moving
hkRotation newRotation;
newRotation.setAxisAngle( m_worldUp, characterAngle );
m_characterForward.setRotatedDir( newRotation, m_cameraForward );
m_characterForward.normalize3();
}
// Rotate the camera's forward vector based on world up vector and mouse movement
if( deltaAngle != 0.0f && m_characterRigidBody->getRigidBody()->getRotation().hasValidAxis() )
{
hkRotation newRotation;
newRotation.setAxisAngle( m_worldUp, deltaAngle );
m_cameraForward.setRotatedDir( newRotation, m_cameraForward );
m_cameraForward.normalize3();
}
}
HK_TIMER_BEGIN( "set character state", HK_NULL );
hkpCharacterInput input;
hkpCharacterOutput output;
{
input.m_atLadder = false;
input.m_inputLR = posX;
input.m_inputUD = posY;
if( m_detachedCamera )
{
input.m_wantJump = false;
}
else
{
input.m_wantJump = m_env->m_window->getMouse().wasButtonPressed( HKG_MOUSE_LEFT_BUTTON )
|| m_env->m_gamePad->wasButtonPressed( HKG_PAD_BUTTON_1 );
}
// Check that we have a valid rotation. Probably won't for the first couple of frames.
if( !( m_characterRigidBody->getRigidBody()->getRotation().hasValidAxis() ) )
{
input.m_up = hkVector4( 0.0f, 0.0f, 1.0f );
input.m_forward = m_cameraForward;
}
else
{
input.m_up = m_worldUp;
// Recalculate m_forward so it's perpendicular to m_worldUp
hkVector4 newRot;
newRot.setCross( m_cameraForward, m_worldUp );
m_cameraForward.setCross( m_worldUp, newRot );
// Display character's current heading
hkRotation characterRotation;
characterRotation.set( m_characterRigidBody->getRigidBody()->getRotation() );
HK_DISPLAY_ARROW( m_characterRigidBody->getPosition(), characterRotation.getColumn(0), hkColor::LIMEGREEN );
input.m_forward = m_cameraForward;
}
hkStepInfo stepInfo;
stepInfo.m_deltaTime = m_timestep;
stepInfo.m_invDeltaTime = 1.0f / m_timestep;
input.m_stepInfo = stepInfo;
input.m_characterGravity.setMul4( -20.0f, m_worldUp );
input.m_velocity = m_characterRigidBody->getRigidBody()->getLinearVelocity();
input.m_position = m_characterRigidBody->getRigidBody()->getPosition();
{
hkpSurfaceInfo ground;
m_characterRigidBody->checkSupport( stepInfo, ground );
// Avoid accidental state changes (Smooth movement on stairs)
// During transition supported->unsupported continue to return N-frames hkpSurfaceInfo data from previous supported state
{
// Number of frames to skip (continue with previous hkpSurfaceInfo data)
const int skipFramesInAir = 6;
if( input.m_wantJump )
{
m_framesInAir = skipFramesInAir;
}
hkpSurfaceInfo* currInfo;
if( ground.m_supportedState != hkpSurfaceInfo::SUPPORTED )
{
if( m_framesInAir < skipFramesInAir )
{
input.m_isSupported = true;
currInfo = m_previousGround;
}
else
{
input.m_isSupported = false;
currInfo = &ground;
}
m_framesInAir++;
}
else
{
input.m_isSupported = true;
currInfo = &ground;
m_previousGround->set( ground );
// reset old number of frames
if( m_framesInAir > skipFramesInAir )
{
m_framesInAir = 0;
}
}
input.m_surfaceNormal = currInfo->m_surfaceNormal;
input.m_surfaceVelocity = currInfo->m_surfaceVelocity;
input.m_surfaceMotionType = currInfo->m_surfaceMotionType;
}
}
HK_TIMER_END();
}
// Apply the character state machine
{
HK_TIMER_BEGIN( "update character state", HK_NULL );
m_characterContext->update( input, output );
HK_TIMER_END();
}
//Apply the player character controller
{
HK_TIMER_BEGIN( "simulate character", HK_NULL );
// Set output velocity from state machine into character rigid body
m_characterRigidBody->setLinearVelocity( output.m_velocity, m_timestep );
HK_TIMER_END();
m_world->unmarkForWrite();
}
// Rotate the character
{
hkRotation newOrientation;
newOrientation.getColumn(0) = m_characterForward;
newOrientation.getColumn(1) = m_worldUp;
newOrientation.getColumn(2).setCross( newOrientation.getColumn(0), newOrientation.getColumn(1) );
newOrientation.renormalize();
reorientCharacter( newOrientation );
}
// Step the world
hkDefaultPhysicsDemo::stepDemo();
// Display state
{
hkpCharacterStateType state = m_characterContext->getState();
char* stateStr;
switch( state )
{
case HK_CHARACTER_ON_GROUND:
{
stateStr = "On Ground";
break;
}
case HK_CHARACTER_JUMPING:
{
stateStr = "Jumping";
break;
}
case HK_CHARACTER_IN_AIR:
{
stateStr = "In Air";
break;
}
default:
{
stateStr = "Other";
break;
}
}
char buffer[255];
hkString::snprintf( buffer, 255, "State : %s", stateStr );
m_env->m_textDisplay->outputText( buffer, 20.f, 270.f, 0xffffffff );
}
//
// Handle crouching (only for capsule)
//
if( !m_detachedCamera )
{
m_world->markForWrite();
hkBool wantCrouch = ( m_env->m_window->getMouse().getButtonState() & HKG_MOUSE_RIGHT_BUTTON )
|| ( m_env->m_gamePad->getButtonState() & HKG_PAD_BUTTON_2 );
hkBool isCrouching = ( m_characterRigidBody->getRigidBody()->getCollidable()->getShape() == m_crouchShape );
// We want to stand
if( isCrouching && !wantCrouch )
{
m_characterRigidBody->getRigidBody()->setShape( m_standShape );
}
// We want to crouch
else if( !isCrouching && wantCrouch )
{
m_characterRigidBody->getRigidBody()->setShape( m_crouchShape );
}
m_world->unmarkForWrite();
}
// Transparent camera handling
if( !m_detachedCamera )
{
m_world->markForWrite();
handleCamera();
m_world->unmarkForWrite();
}
return hkDemo::DEMO_OK;
}
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);
}
}
}
}
hkBool hkDefaultPhysicsDemo::objectPicked( const hkgDisplayObject* displayObject, const hkVector4& worldPosition, int geomIndex )
{
HK_ASSERT(0x65b2643b, m_env->m_displayHandler);
hkgDisplayHandler* dhandler = m_env->m_displayHandler;
if( displayObject && m_world )
{
hkUlong id = 0;
void* userPtr = HK_NULL;
if (displayObject->getStatusFlags() & HKG_DISPLAY_OBJECT_INSTANCED)
{
id = ((hkgInstancedDisplayObject*)displayObject)->getUserDataFromIndex( geomIndex );
}
else
{
id = dhandler->getDisplayObjectId( displayObject );
userPtr = displayObject->getUserPointer();
}
// HACK! We know the id is actually the address of the Collidable
if ( (id > 0) || (userPtr != HK_NULL) )
{
hkpRigidBody* rb = HK_NULL;
// Check if doLink->m_id is a valid address.
if (id > 0)
{
if ( (id & 0x03) == 0x03) // 0x1 == swept transform from, 0x2 = swept transform to, 0x3 = convex radius (ok to pick)
{
id &= ~0x03;
}
if (id % 4 != 0 || id < 64)
{
return false;
}
hkpCollidable* col = reinterpret_cast<hkpCollidable*>( id );
if( col->getOwner() )
{
rb = hkGetRigidBody(col);
}
}
else
{
rb = reinterpret_cast<hkpRigidBody*>( userPtr ); // true in our demos that use assets
}
if( rb && !rb->isFixed() )
{
hkpWorld* mouseWorld = rb->getWorld();
if (mouseWorld)
{
hkVector4 positionAinA;
positionAinA.setTransformedInversePos( rb->getTransform(), worldPosition );
const hkReal springDamping = 0.5f;
const hkReal springElasticity = 0.3f;
const hkReal objectDamping = 0.95f;
mouseWorld->lock();
m_mouseSpring = new hkpMouseSpringAction( positionAinA, worldPosition, springDamping, springElasticity, objectDamping, rb, &m_mouseSpringAppliedCallbacks );
m_mouseSpring->setMaxRelativeForce(m_mouseSpringMaxRelativeForce);
mouseWorld->addAction( m_mouseSpring );
mouseWorld->unlock();
}
return true;
}
}
}
return false;
}
void AabbQueryDemo::queryAabbMT()
{
HK_TIMER_BEGIN_LIST("queryAabbMT", "setup");
hkAabb aabb;
// Grab a body from the world, and compute it's AABB + some padding
const hkpCollidable* queryCollidable;
{
queryCollidable = m_collidables[m_collIdx];
hkpRigidBody* rb = hkGetRigidBody(queryCollidable);
queryCollidable->getShape()->getAabb(rb->getTransform(), 20.0f, aabb);
}
hkArray<hkpKdTreeAabbCommand> commands;
commands.setSize(numQueries);
//
// For performance timings, we query the same AABB multiple times and overwrite the results.
// When using this, make sure to use different output arrays!
//
hkArray<hkPrimitiveId> output;
output.setSize(50);
//
// Set up the commands for the job
//
for (int i=0; i<commands.getSize(); i++)
{
hkpKdTreeAabbCommand& command = commands[i];
command.m_aabb = aabb;
command.m_results = output.begin();
command.m_resultsCapacity = output.getSize();
command.m_numResultsOut = 0;
}
//
// Setup the job
//
hkArray<hkpRayCastQueryJobHeader> header(1);
hkpKdTreeAabbJob aabbJob(header.begin(), commands.begin(), commands.getSize(), &m_semaphore);
aabbJob.m_numTrees = 1;
aabbJob.m_trees[0] = m_world->m_kdTreeManager->getTree();
m_jobQueue->addJob( *reinterpret_cast<hkJobQueue::JobQueueEntry*>(&aabbJob), hkJobQueue::JOB_HIGH_PRIORITY );
HK_TIMER_SPLIT_LIST("process");
m_jobThreadPool->processAllJobs( m_jobQueue );
m_jobThreadPool->waitForCompletion();
m_semaphore.acquire();
HK_TIMER_END_LIST();
//
// Run the same query on the broadphase for comparison purposes
//
hkArray<hkpBroadPhaseHandlePair> sapHits;
{
HK_TIME_CODE_BLOCK("BroadphaseQueryAabb", HK_NULL);
for (int i=0; i<numQueries; i++)
{
sapHits.clear();
m_world->getBroadPhase()->querySingleAabb( aabb, sapHits );
}
}
//
// Check results and draw
//
for (int i=0; i<commands.getSize(); i++)
{
hkpKdTreeAabbCommand& command = commands[i];
hkBool jobOk = compareHitArrays(command.m_aabb, command.m_results, command.m_numResultsOut, sapHits);
for (int j=0; j<command.m_numResultsOut; j++)
{
HK_SET_OBJECT_COLOR(command.m_results[j], hkColor::YELLOW);
}
HK_SET_OBJECT_COLOR((hkUlong)queryCollidable, hkColor::LIME);
if( !jobOk )
{
m_env->m_textDisplay->outputText("MT Hit lits differed!", 20, 250, (hkUint32) hkColor::RED);
}
}
}
void AabbQueryDemo::queryAabbST()
{
HK_TIME_CODE_BLOCK("queryAabbST", HK_NULL);
hkAabb aabb;
// Grab a body from the world, and compute it's AABB + some padding
const hkpCollidable* queryCollidable;
{
queryCollidable = m_collidables[m_collIdx];
hkpRigidBody* rb = hkGetRigidBody(queryCollidable);
queryCollidable->getShape()->getAabb(rb->getTransform(), 20.0f, aabb);
}
//
// For performance timings, we query the same AABB multiple times and overwrite the results.
// When using this, make sure to use different output arrays!
//
hkArray<hkPrimitiveId> kdhitsRecurs, kdhitsIter;
hkArray<hkpBroadPhaseHandlePair> sapHits;
{
HK_TIME_CODE_BLOCK("KdQueryRecursiveST", HK_NULL);
for (int i=0; i<numQueries; i++)
{
kdhitsRecurs.clear();
hkKdTreeAabbQueryUtils::queryAabbRecursive(m_world->m_kdTreeManager->getTree(), aabb, kdhitsRecurs);
}
}
{
HK_TIME_CODE_BLOCK("KdQueryIterativeST", HK_NULL);
for (int i=0; i<numQueries; i++)
{
kdhitsIter.clear();
hkKdTreeAabbQueryUtils::queryAabbIterative(m_world->m_kdTreeManager->getTree(), aabb, kdhitsIter);
}
}
// Visualize the results
HK_DISPLAY_BOUNDING_BOX(aabb, hkColor::YELLOW);
for (int i=0; i<kdhitsIter.getSize(); i++)
{
HK_SET_OBJECT_COLOR(kdhitsIter[i], hkColor::YELLOW);
}
HK_SET_OBJECT_COLOR((hkUlong)queryCollidable, hkColor::RED);
// Call the corresponding hkp3AxisSweep method for comparison
{
HK_TIME_CODE_BLOCK("BroadphaseQueryAabb", HK_NULL);
for (int i=0; i<numQueries; i++)
{
sapHits.clear();
m_world->getBroadPhase()->querySingleAabb( aabb, sapHits );
}
}
{
hkBool iterOk = compareHitArrays(aabb, kdhitsIter.begin(), kdhitsIter.getSize(), sapHits);
hkBool recursOk = compareHitArrays(aabb, kdhitsRecurs.begin(), kdhitsRecurs.getSize(), sapHits);
if( !(iterOk && recursOk) )
{
m_env->m_textDisplay->outputText("ST Hit lits differed!", 20, 150, (hkUint32) hkColor::RED);
}
}
}
