tarch::la::Vector<DIMENSIONS,double>
peano::integration::partitioncoupling::builtin::tests::PartitionCoupling4MovingSphereTest::
getExpectedVelocity(
const tarch::la::Vector<DIMENSIONS,double> &spherePosition,
const tarch::la::Vector<DIMENSIONS,double> &velocityPosition,
const tarch::la::Vector<DIMENSIONS,double> &force,
const double &radius, const double &sphereMass,const double &dt
) const {
tarch::la::Vector<DIMENSIONS,double> result(0.0);
tarch::la::Vector<3,double> r(0.0);
tarch::la::Vector<3,double> spherePos(0.0);
tarch::la::Vector<3,double> velocityPos(0.0);
tarch::la::Vector<3,double> force3(0.0);
for (int d = 0; d < DIMENSIONS; d++){
force3(d) = force(d);
spherePos(d) = spherePosition(d);
velocityPos(d) = velocityPosition(d);
}
r = peano::integration::partitioncoupling::builtin::crossProduct(velocityPos-spherePos,force3);
r = peano::integration::partitioncoupling::builtin::crossProduct(r,velocityPos-spherePos);
r = dt/(2.0/5.0*sphereMass*radius*radius)*r;
for (int d = 0; d < DIMENSIONS; d++){
result(d) = r(d);
}
return result;
}
hkpRigidBody* DestructibleWallsDemo::createBall(const hkVector4& position, const hkVector4& velocity)
{
const hkReal radius = 1.0f;
const hkReal mass = 100.0f;
hkpMassProperties result;
hkpInertiaTensorComputer::computeSphereVolumeMassProperties(radius, mass, result);
hkpSphereShape* sphereShape = new hkpSphereShape(radius);
hkVector4 spherePos(-20.0f, 0.0f + radius, 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.setAdd4(position, spherePos);
sphereInfo.m_qualityType = HK_COLLIDABLE_QUALITY_BULLET;
hkpRigidBody* sphereBody = new hkpRigidBody(sphereInfo);
sphereBody->setLinearVelocity(velocity);
sphereShape->removeReference();
return sphereBody;
}
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;
}
