void World::simulate(float dt)
{
mAllocator.clearFrame();
int vIterNum = 50;
int pIterNum = 10;
mColManager.preocss( dt );
for( RigidBodyList::iterator iter = mRigidBodies.begin() ,itEnd = mRigidBodies.end();
iter != itEnd ; ++iter )
{
RigidBody* body = *iter;
body->saveState();
if ( body->getMotionType() != BodyMotion::eStatic )
{
if ( body->getMotionType() == BodyMotion::eDynamic )
body->addLinearImpulse( body->mMass * mGrivaty * dt );
body->applyImpulse();
body->mLinearVel *= 1.0 / ( 1 + body->mLinearDamping );
}
}
ContactManifold** sortedContact = new ( mAllocator ) ContactManifold* [ mColManager.mMainifolds.size() ];
int numMainfold = mColManager.mMainifolds.size();
int idxStatic = mColManager.mMainifolds.size() - 1;
int idxNormal = 0;
for( int i = 0 ; i < mColManager.mMainifolds.size() ; ++i )
{
ContactManifold& cm = *mColManager.mMainifolds[i];
RigidBody* bodyA = static_cast< RigidBody* >( cm.mContect.object[0] );
RigidBody* bodyB = static_cast< RigidBody* >( cm.mContect.object[1] );
if ( bodyA->getMotionType() == BodyMotion::eStatic ||
bodyB->getMotionType() == BodyMotion::eStatic )
{
sortedContact[idxStatic--] = &cm;
}
else
{
sortedContact[idxNormal++] = &cm;
}
}
for( int i = 0 ; i < numMainfold ; ++i )
{
ContactManifold& cm = *sortedContact[i];
Contact& c = cm.mContect;
Vec2f cp = 0.5 * ( c.pos[0] + c.pos[1] );
RigidBody* bodyA = static_cast< RigidBody* >( c.object[0] );
RigidBody* bodyB = static_cast< RigidBody* >( c.object[1] );
Vec2f vA = bodyA->getVelFromWorldPos( cp );
Vec2f vB = bodyB->getVelFromWorldPos( cp );
float vrel = c.normal.dot( vB - vA );
float relectParam = 1.0f;
cm.velParam = 0;
if ( vrel < -1 )
{
cm.velParam = -relectParam * vrel;
}
//cm.impulse = 0;
////warm start
Vec2f rA = cp - bodyA->mPosCenter;
Vec2f rB = cp - bodyB->mPosCenter;
Vec2f dp = cm.impulse * c.normal;
bodyA->mLinearVel -= dp * bodyA->mInvMass;
//bodyA->mAngularVel -= rA.cross( dp ) * bodyA->mInvI;
bodyB->mLinearVel += dp * bodyB->mInvMass;
//bodyB->mAngularVel += rB.cross( dp ) * bodyB->mInvI;
}
if ( numMainfold != 0 )
jumpDebug();
//std::sort( sortedContact.begin() , sortedContact.end() , DepthSort() );
for( int nIter = 0 ; nIter < vIterNum ; ++nIter )
{
for( int i = 0 ; i < numMainfold ; ++i )
{
ContactManifold& cm = *sortedContact[i];
Contact& c = cm.mContect;
RigidBody* bodyA = static_cast< RigidBody* >( c.object[0] );
RigidBody* bodyB = static_cast< RigidBody* >( c.object[1] );
Vec2f cp = 0.5 * ( c.pos[0] + c.pos[1] );
Vec2f vA = bodyA->getVelFromWorldPos( cp );
Vec2f vB = bodyB->getVelFromWorldPos( cp );
Vec2f rA = cp - bodyA->mPosCenter;
Vec2f rB = cp - bodyB->mPosCenter;
Vec2f vrel = vB - vA;
float vn = vrel.dot( c.normal );
float nrA = rA.cross( c.normal );
float nrB = rB.cross( c.normal );
float invMass = 0;
invMass += bodyA->mInvMass + bodyA->mInvI * nrA * nrA;
invMass += bodyB->mInvMass + bodyB->mInvI * nrB * nrB;
float impulse = -( vn - cm.velParam ) / invMass;
float newImpulse = Math::Max( cm.impulse + impulse , 0.0f );
impulse = newImpulse - cm.impulse;
bodyA->mLinearVel -= impulse * c.normal * bodyA->mInvMass;
//bodyA->mAngularVel -= impulse * nrA * bodyA->mInvI;
bodyB->mLinearVel += impulse * c.normal * bodyB->mInvMass;
//bodyB->mAngularVel += impulse * nrB * bodyB->mInvI;
cm.impulse = newImpulse;
float fa = impulse * nrA * bodyA->mInvI;
float fb = impulse * nrB * bodyB->mInvI;
if ( fa != 0 || fb !=0 )
{
int i = 1;
}
if ( numMainfold != 0 )
jumpDebug();
}
}
for( RigidBodyList::iterator iter = mRigidBodies.begin() ,itEnd = mRigidBodies.end();
iter != itEnd ; ++iter )
{
RigidBody* body = *iter;
body->intergedTramsform( dt );
//body->mAngularVel = 0;
}
for( int nIter = 0 ; nIter < pIterNum ; ++nIter )
{
float const kValueB = 0.8f;
float const kMaxDepth = 2.f;
float const kSlopValue = 0.0001f;
float maxDepth = 0.0;
for( int i = 0 ; i < numMainfold ; ++i )
{
ContactManifold& cm = *sortedContact[i];
Contact& c = cm.mContect;
RigidBody* bodyA = static_cast< RigidBody* >( c.object[0] );
RigidBody* bodyB = static_cast< RigidBody* >( c.object[1] );
if ( bodyB->getMotionType() != BodyMotion::eDynamic &&
bodyB->getMotionType() != BodyMotion::eDynamic )
continue;
Vec2f cpA = bodyA->mXForm.mul( c.posLocal[0] );
Vec2f cpB = bodyB->mXForm.mul( c.posLocal[1] );
//TODO: normal change need concerned
Vec2f normal = c.normal;
float depth = normal.dot( cpA - cpB ) + 0.001;
if ( depth <= 0 )
continue;
Vec2f cp = 0.5 * ( cpA + cpB );
Vec2f rA = cp - bodyA->mPosCenter;
Vec2f rB = cp - bodyB->mPosCenter;
float nrA = rA.cross( normal );
float nrB = rB.cross( normal );
float invMass = 0;
invMass += bodyA->mInvMass + bodyA->mInvI * nrA * nrA;
invMass += bodyB->mInvMass + bodyB->mInvI * nrB * nrB;
float offDepth = Math::Clamp ( ( depth - kSlopValue ) , 0 , kMaxDepth );
float impulse = ( invMass > 0 ) ? kValueB * offDepth / invMass : 0;
if ( impulse > 0 )
{
bodyA->mXForm.translate( -impulse * normal * bodyA->mInvMass );
bodyA->mRotationAngle += -impulse * nrA * bodyA->mInvI;
bodyA->synTransform();
bodyB->mXForm.translate( impulse * normal * bodyB->mInvMass );
bodyB->mRotationAngle += impulse * nrB * bodyB->mInvI;
bodyB->synTransform();
}
{
Vec2f cpA = bodyA->mXForm.mul( c.posLocal[0] );
Vec2f cpB = bodyB->mXForm.mul( c.posLocal[1] );
//TODO: normal change need concerned
float depth2 = normal.dot( cpA - cpB );
float dp = depth - depth2;
::Msg( "dp = %f " , dp );
int i = 1;
}
}
if ( maxDepth < 3 * kMaxDepth )
break;
}
if ( numMainfold != 0 )
jumpDebug();
}
