void JelloMesh::Update(double dt, const World& world, const vec3& externalForces)
{
m_externalForces = externalForces;
CheckForCollisions(m_vparticles, world);
ComputeForces(m_vparticles);
ResolveContacts(m_vparticles);
ResolveCollisions(m_vparticles);
switch (m_integrationType)
{
case EULER: EulerIntegrate(dt); break;
case MIDPOINT: MidPointIntegrate(dt); break;
case RK4: RK4Integrate(dt); break;
}
}
void CRope::RunSimOnSamples()
{
float flDeltaTime = 0.025;
if( m_bInitialDeltaTime )
{
m_bInitialDeltaTime = false;
m_flLastTime = gpGlobals->time;
flDeltaTime = 0;
}
size_t uiIndex = 0;
CRopeSample** ppSampleSource = m_CurrentSys;
CRopeSample** ppSampleTarget = m_TargetSys;
while( true )
{
++uiIndex;
ComputeForces( ppSampleSource );
RK4Integrate( flDeltaTime, ppSampleSource, ppSampleTarget );
m_flLastTime += 0.007;
if( gpGlobals->time <= m_flLastTime )
{
if( ( uiIndex % 2 ) != 0 )
break;
}
std::swap( ppSampleSource, ppSampleTarget );
}
m_flLastTime = gpGlobals->time;
}
void CPhysEnv::Simulate(float DeltaTime, BOOL running)
{
float CurrentTime = 0.0f;
float TargetTime = DeltaTime;
tParticle *tempSys;
int collisionState;
while(CurrentTime < DeltaTime)
{
if (running)
{
ComputeForces(m_CurrentSys);
// IN ORDER TO MAKE THINGS RUN FASTER, I HAVE THIS LITTLE TRICK
// IF THE SYSTEM IS DOING A BINARY SEARCH FOR THE COLLISION POINT,
// I FORCE EULER'S METHOD ON IT. OTHERWISE, LET THE USER CHOOSE.
// THIS DOESN'T SEEM TO EFFECT STABILITY EITHER WAY
if (m_CollisionRootFinding)
{
EulerIntegrate(TargetTime-CurrentTime);
}
else
{
switch (m_IntegratorType)
{
case EULER_INTEGRATOR:
EulerIntegrate(TargetTime-CurrentTime);
break;
case MIDPOINT_INTEGRATOR:
MidPointIntegrate(TargetTime-CurrentTime);
break;
case HEUN_INTEGRATOR:
HeunIntegrate(TargetTime-CurrentTime);
break;
case RK4_INTEGRATOR:
RK4Integrate(TargetTime-CurrentTime);
break;
case RK4_ADAPTIVE_INTEGRATOR:
RK4AdaptiveIntegrate(TargetTime-CurrentTime);
break;
case FEHLBERG:
FehlbergIntegrate(TargetTime-CurrentTime);
break;
}
}
}
collisionState = CheckForCollisions(m_TargetSys);
if(collisionState == PENETRATING)
{
// TELL THE SYSTEM I AM LOOKING FOR A COLLISION SO IT WILL USE EULER
m_CollisionRootFinding = TRUE;
// we simulated too far, so subdivide time and try again
TargetTime = (CurrentTime + TargetTime) / 2.0f;
// blow up if we aren't moving forward each step, which is
// probably caused by interpenetration at the frame start
assert(fabs(TargetTime - CurrentTime) > EPSILON);
}
else
{
// either colliding or clear
if(collisionState == COLLIDING)
{
int Counter = 0;
do
{
ResolveCollisions(m_TargetSys);
Counter++;
} while((CheckForCollisions(m_TargetSys) ==
COLLIDING) && (Counter < 100));
assert(Counter < 100);
m_CollisionRootFinding = FALSE; // FOUND THE COLLISION POINT
}
// we made a successful step, so swap configurations
// to "save" the data for the next step
CurrentTime = TargetTime;
TargetTime = DeltaTime;
// SWAP MY TWO PARTICLE SYSTEM BUFFERS SO I CAN DO IT AGAIN
tempSys = m_CurrentSys;
m_CurrentSys = m_TargetSys;
m_TargetSys = tempSys;
}
}
}
///////////////////////////////////////////////////////////////////////////////
// Function: RK4AdaptiveIntegrate
// Purpose: Calculate new Positions and Velocities given a deltatime
// Arguments: DeltaTime that has passed since last iteration
// Notes: This integrator uses the fourth-order Runge-Kutta method based on step-halving technique
///////////////////////////////////////////////////////////////////////////////
void CPhysEnv::RK4AdaptiveIntegrate( float DeltaTime)
{
float currentX=0;
while(m_IterationNumber!= m_MaxIterationsCount)
{
m_IterationNumber++;
// Your Code Here
m_RelativeError=0;
currentX += m_Step;
//First Call the RK4 using the whole step
RK4Integrate(currentX);
currentX-=m_Step;
tParticle* firstPrediction = new tParticle[m_ParticleCnt];
//tParticle* firsthalf = new tParticle[m_ParticleCnt];
tParticle* secondhalf = new tParticle[m_ParticleCnt];
//tParticle *iterator = m_TargetSys;
//Copy the Target System into one of the temp systems.
/*
for(int i=0;i<m_ParticleCnt;i++)
firstPrediction[i] = m_TargetSys[i];
*/
Copy(m_TargetSys,firstPrediction);
currentX += m_Step/2.0;
//Then we make two calls with half the step
RK4Integrate(currentX);
currentX -= m_Step/2.0;
currentX += m_Step;
//Copy the Target System into one of the temp systems.
/*
for(int i=0;i<m_ParticleCnt;i++)
m_CurrentSys[i] = m_TargetSys[i];
*/
Copy(m_TargetSys,m_CurrentSys);
RK4Integrate(currentX);
//Copy the Target System into one of the temp systems.
Copy(m_TargetSys,secondhalf);
/*
for(int i=0;i<m_ParticleCnt;i++)
secondhalf[i] = m_TargetSys[i];
*/
//Then Calculate the error
tParticle* correction = new tParticle[m_ParticleCnt];
//Copy the Target System into one of the temp systems.
for(int i=0;i<m_ParticleCnt;i++)
{
correction[i].pos.x = (secondhalf[i].pos.x - firstPrediction[i].pos.x)/15;
correction[i].pos.y = (secondhalf[i].pos.y - firstPrediction[i].pos.y)/15;
correction[i].pos.z = (secondhalf[i].pos.z - firstPrediction[i].pos.z)/15;
m_RelativeError+= fabs(correction[i].pos.x+ correction[i].pos.y + correction[i].pos.z);
secondhalf[i].pos.x += correction[i].pos.x;
secondhalf[i].pos.y += correction[i].pos.y;
secondhalf[i].pos.z += correction[i].pos.z;
}
m_RelativeError/=m_ParticleCnt;
/*
for(int i=0;i<m_ParticleCnt;i++)
m_CurrentSys[i] = secondhalf[i] ;
*/
Copy(secondhalf,m_CurrentSys);
//Check the relative error
if(m_RelativeError>EPSILON)
//Since We have a high error so it is more logical that we want to lower the step size
m_Step = m_Step * pow ((double) fabs (EPSILON / m_RelativeError), 0.25);
else
{
/*
for(int i=0;i<m_ParticleCnt;i++)
m_TargetSys[i] = secondhalf[i] ;*/
Copy(secondhalf,m_TargetSys);
break;
}
currentX+=m_Step;
if(currentX>=DeltaTime)
{
/*
for(int i=0;i<m_ParticleCnt;i++)
m_TargetSys[i] = secondhalf[i] ;*/
Copy(secondhalf,m_TargetSys);
break;
}
delete[] secondhalf;
delete[] firstPrediction;
}
if(m_RelativeError<ECON)
//So we will need to increase the step size
m_Step = m_Step * pow ((double) fabs (EPSILON / m_RelativeError), 0.2);
m_IterationNumber=0;
}
