void SingleStep ()
{
++ stepCount;
timeNow = stepCount * deltaT;
PredictorStep ();
PredictorStepS ();
ComputeForces ();
ComputeForcesDipoleR ();
ComputeForcesDipoleF ();
ComputeDipoleAccel ();
ApplyThermostat ();
CorrectorStep ();
CorrectorStepS ();
AdjustDipole ();
ApplyBoundaryCond ();
EvalProps ();
if (stepCount % stepAdjustTemp == 0) AdjustTemp ();
AccumProps (1);
if (stepCount % stepAvg == 0) {
AccumProps (2);
PrintSummary (stdout);
AccumProps (0);
}
if (stepCount >= stepEquil && (stepCount - stepEquil) % stepRdf == 0)
EvalRdf ();
}
void AdvanceFrame()
{
RebuildGrid();
ComputeForces();
ProcessCollisions();
AdvanceParticles();
#if defined(USE_ImpeneratableWall)
// N.B. The integration of the position can place the particle
// outside the domain. We now make a pass on the perimiter cells
// to account for particle migration beyond domain.
ProcessCollisions2();
#endif
#ifdef ENABLE_STATISTICS
float mean, stddev;
int i;
mean = (float)numParticles/(float)numCells;
stddev = 0.0;
for(i=0; i<numCells; i++) {
stddev += (mean-cnumPars[i])*(mean-cnumPars[i]);
}
stddev = sqrtf(stddev);
std::cout << "Cell statistics: mean=" << mean << " particles, stddev=" << stddev << " particles." << std::endl;
#endif
}
int main(int argc, char *argv[]) {
size_t i;
FILE *statfile;
ReadParameters(argc, argv);
Initialize();
statfile = fopen(outname, "w");
ReadConfiguration();
for(i=0; i<nsteps; i++) { /* a number of single time steps */
ApplyPerBoundaries();
if ((i % liststeps) == 0) {
PutInBox();
MakeList();
}
ComputeForces();
if (microcanon) {
Leapfrog();
}
else {
NoseHoover();
}
if ((i % iosteps) == 0) {
fprintf(statfile, "%ld %e %e %e %e %e\n", i, Ekin, Epot, P, eta, Ekin+Epot);
}
}
fclose(statfile);
WriteConfiguration(final_conf);
return 1;
}
//! Calculate new Positions and Velocities given a deltatime
//! \param DeltaTime that has passed since last iteration
static void RK4Integrate(PHYSICS_BOX_DATA * pbox, float DeltaTime) {
float halfDeltaT, sixthDeltaT;
halfDeltaT = DeltaTime * .5f; // some time values i will need
sixthDeltaT = ( 1.0f / 6 );
PHYSVERT m_TempSys[5][32];
for(long jj = 0; jj < 4; jj++) {
arx_assert(size_t(pbox->nb_physvert) <= ARRAY_SIZE(m_TempSys[jj + 1]));
memcpy(m_TempSys[jj + 1], pbox->vert, sizeof(PHYSVERT) * pbox->nb_physvert);
if(jj == 3) {
halfDeltaT = DeltaTime;
}
for(long kk = 0; kk < pbox->nb_physvert; kk++) {
PHYSVERT * source = &pbox->vert[kk];
PHYSVERT * accum1 = &m_TempSys[jj + 1][kk];
PHYSVERT * target = &m_TempSys[0][kk];
accum1->force = source->force * (source->mass * halfDeltaT);
accum1->velocity = source->velocity * halfDeltaT;
// determine the new velocity for the particle over 1/2 time
target->velocity = source->velocity + accum1->force;
target->mass = source->mass;
// set the new position
target->pos = source->pos + accum1->velocity;
}
ComputeForces(m_TempSys[0], pbox->nb_physvert); // compute the new forces
}
for(long kk = 0; kk < pbox->nb_physvert; kk++) {
PHYSVERT * source = &pbox->vert[kk]; // current state of particle
PHYSVERT * target = &pbox->vert[kk];
PHYSVERT * accum1 = &m_TempSys[1][kk];
PHYSVERT * accum2 = &m_TempSys[2][kk];
PHYSVERT * accum3 = &m_TempSys[3][kk];
PHYSVERT * accum4 = &m_TempSys[4][kk];
// determine the new velocity for the particle using rk4 formula
Vec3f dv = accum1->force + ((accum2->force + accum3->force) * 2.f) + accum4->force;
target->velocity = source->velocity + (dv * sixthDeltaT);
// determine the new position for the particle using rk4 formula
Vec3f dp = accum1->velocity + ((accum2->velocity + accum3->velocity) * 2.f)
+ accum4->velocity;
target->pos = source->pos + (dp * sixthDeltaT * 1.2f);
}
}
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;
}
}
///////////////////////////////////////////////////////////////////////////////
// Function: MidPointIntegrate
// Purpose: Calculate new Positions and Velocities given a deltatime
// Arguments: DeltaTime that has passed since last iteration
// Notes: This integrator uses the Midpoint method
///////////////////////////////////////////////////////////////////////////////
void CPhysEnv::MidPointIntegrate( float DeltaTime)
{
/// Local Variables ///////////////////////////////////////////////////////////
float halfDeltaT;
///////////////////////////////////////////////////////////////////////////////
halfDeltaT = DeltaTime / 2.0f;
// TAKE A HALF STEP AND UPDATE VELOCITY AND POSITION - Find y at half-interval
IntegrateSysOverTime(m_CurrentSys,m_CurrentSys,m_TempSys[0],halfDeltaT);
// COMPUTE FORCES USING THESE NEW POSITIONS AND VELOCITIES - Compute slope at half-interval
ComputeForces(m_TempSys[0]);
// TAKE THE FULL STEP WITH THIS NEW INFORMATION - Now use above slope for full-interval
IntegrateSysOverTime(m_CurrentSys,m_TempSys[0],m_TargetSys,DeltaTime);
}
void SingleStep ()
{
++ stepCount;
timeNow = stepCount * deltaT;
LeapfrogStep (1);
ApplyBoundaryCond ();
ComputeForces ();
LeapfrogStep (2);
//EvalProps ();
//AccumProps (1);
if (stepCount % stepAvg == 0) {
//AccumProps (2);
PrintSummary (stdout);
//PrintVels (stdout);
//AccumProps (0);
}
}
void ParticleDerivative(ParticleSystem p, double *dst)
{
double m;
ComputeForces (p);
// Newton's 2nd law: f=ma
for (unsigned int i = 0; i < p->particles.size(); i++){
m = p->particles[i]->m;
*(dst++) = p->particles[i]->v[0];
*(dst++) = p->particles[i]->v[1];
*(dst++) = p->particles[i]->v[2];
*(dst++) = p->particles[i]->f[0]/m;
*(dst++) = p->particles[i]->f[1]/m;
*(dst++) = p->particles[i]->f[2]/m;
}
}
void CSheetSimulator::EulerStep( float dt )
{
ClearForces();
ComputeForces();
// Update positions and velocities
for ( int i = 0; i < NumParticles(); ++i)
{
m_Particle[i].m_Position += m_Particle[i].m_Velocity * dt;
m_Particle[i].m_Velocity += m_Particle[i].m_Force * dt / m_Particle[i].m_Mass;
assert( _finite( m_Particle[i].m_Velocity.x ) &&
_finite( m_Particle[i].m_Velocity.y) &&
_finite( m_Particle[i].m_Velocity.z) );
// clamp for stability
float lensq = m_Particle[i].m_Velocity.LengthSqr();
if (lensq > 1e6)
{
m_Particle[i].m_Velocity *= 1e3 / sqrt(lensq);
}
}
SatisfyCollisionConstraints();
}
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::FehlbergIntegrate( float deltaTime)
{
float a2=0.2,a3=0.3,a4=0.6,a5=1,a6=0.875,b21=0.2,b31=3.0/40.0,b32=9.0/40.0,b41=0.3,b42=-0.9,
b43=1.2,b51=-11.0/54.0,b52=2.5,b53=-70.0/27.0,b54=35.0/27.0,b61=1631.0/55296.0,b62=175.0/512.0,
b63=575.0/13824.0,b64=44275.0/110592.0,b65=253.0/4096.0,c1=37.0/378.0,c3=250.0/621.0,c4=125.0/594.0,
c6=512.0/1771.0;
float dc1= c1 - 2825.0/27648.9, dc3 = c3 - 18575.0/48384.0, dc4 = c4- 13525.0/55296.0,
dc5= -277.0/14336.0,dc6=c6- 0.25;
tParticle* target, * source,*k1,*k2,*k3,*k4;
/////////////////////////////////////////////////////////////////////////////////
//First Step
//////////////////////////////////////////////////////////////////////////////////
target = m_TempSys[0];
source = m_CurrentSys;
k1 = m_TempSys[1];
for (int loop = 0; loop < m_ParticleCnt; loop++)
{
// DETERMINE THE NEW VELOCITY FOR THE PARTICLE
k1->f.x = deltaTime * source->f.x * source->oneOverM * b21;
k1->f.y = deltaTime * source->f.y * source->oneOverM * b21;
k1->f.z = deltaTime * source->f.z * source->oneOverM * b21;
k1->v.x = deltaTime * source->v.x * b21;
k1->v.y = deltaTime * source->v.y * b21;
k1->v.z = deltaTime * source->v.z * b21;
target->v.x = source->v.x + (k1->f.x);
target->v.y = source->v.y + (k1->f.y);
target->v.z = source->v.z + (k1->f.z);
target->oneOverM = source->oneOverM;
// SET THE NEW POSITION
target->pos.x = source->pos.x + (k1->v.x);
target->pos.y = source->pos.y + (k1->v.y);
target->pos.z = source->pos.z + (k1->v.z);
source++;
target++;
k1++;
}
//Update the forces
ComputeForces(m_TempSys[0]);
/////////////////////////////////////////////////////////////////////////////////
//Second Step
//////////////////////////////////////////////////////////////////////////////////
target = m_TempSys[0];
source = m_CurrentSys;
k1 = m_TempSys[2];
for (int loop = 0; loop < m_ParticleCnt; loop++)
{
// DETERMINE THE NEW VELOCITY FOR THE PARTICLE
k1->f.x = deltaTime * source->f.x * source->oneOverM * b21;
k1->f.y = deltaTime * source->f.y * source->oneOverM * b21;
k1->f.z = deltaTime * source->f.z * source->oneOverM * b21;
k1->v.x = deltaTime * source->v.x * b21;
k1->v.y = deltaTime * source->v.y * b21;
k1->v.z = deltaTime * source->v.z * b21;
target->v.x = source->v.x + (k1->f.x);
target->v.y = source->v.y + (k1->f.y);
target->v.z = source->v.z + (k1->f.z);
target->oneOverM = source->oneOverM;
// SET THE NEW POSITION
target->pos.x = source->pos.x + (k1->v.x);
target->pos.y = source->pos.y + (k1->v.y);
target->pos.z = source->pos.z + (k1->v.z);
source++;
target++;
k1++;
}
//Update the forces
ComputeForces(m_TempSys[0]);
}
///////////////////////////////////////////////////////////////////////////////
// Function: RK4Integrate
// 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
///////////////////////////////////////////////////////////////////////////////
void CPhysEnv::RK4Integrate( float DeltaTime)
{
// Your Code Here
/// Local Variables ///////////////////////////////////////////////////////////
float halfDeltaT;
///////////////////////////////////////////////////////////////////////////////
halfDeltaT = DeltaTime / 2.0f;
// TAKE A HALF STEP AND UPDATE VELOCITY AND POSITION - Find y at half-interval
IntegrateSysOverTime(m_CurrentSys,m_TempSys[1],halfDeltaT);
tParticle* target, * source,*k1,*k2,*k3,*k4;
target = m_TempSys[0];
source = m_CurrentSys;
k1=m_TempSys[1];
///////////////////////////////////////////////////////////////////////////////
for (int i = 0; i < m_ParticleCnt; i++)
{
target->v.x = source->v.x + (k1->f.x);
target->v.y = source->v.y + (k1->f.y);
target->v.z = source->v.z + (k1->f.z);
target->oneOverM = source->oneOverM;
// SET THE NEW POSITION
target->pos.x = source->pos.x + (k1->v.x);
target->pos.y = source->pos.y + (k1->v.y);
target->pos.z = source->pos.z + (k1->v.z);
source++;
target++;
k1++;
}
// COMPUTE FORCES USING THESE NEW POSITIONS AND VELOCITIES - Compute slope at half-interval
ComputeForces(m_TempSys[0]);
// TAKE THE FULL STEP WITH THIS NEW INFORMATION - Now use above slope for full-interval
IntegrateSysOverTime(m_TempSys[0],m_TempSys[2],halfDeltaT);
target = m_TempSys[0];
source = m_CurrentSys;
k1=m_TempSys[2];
///////////////////////////////////////////////////////////////////////////////
for (int i = 0; i < m_ParticleCnt; i++)
{
target->v.x = source->v.x + (k1->f.x);
target->v.y = source->v.y + (k1->f.y);
target->v.z = source->v.z + (k1->f.z);
target->oneOverM = source->oneOverM;
// SET THE NEW POSITION
target->pos.x = source->pos.x + (k1->v.x);
target->pos.y = source->pos.y + (k1->v.y);
target->pos.z = source->pos.z + (k1->v.z);
source++;
target++;
k1++;
}
// COMPUTE FORCES USING THESE NEW POSITIONS AND VELOCITIES - Compute slope at half-interval
ComputeForces(m_TempSys[0]);
IntegrateSysOverTime(m_TempSys[0],m_TempSys[3],DeltaTime);
target = m_TempSys[0];
source = m_CurrentSys;
k1 = m_TempSys[3];
///////////////////////////////////////////////////////////////////////////////
for (int i = 0; i < m_ParticleCnt; i++)
{
target->v.x = source->v.x + (k1->f.x);
target->v.y = source->v.y + (k1->f.y);
target->v.z = source->v.z + (k1->f.z);
target->oneOverM = source->oneOverM;
// SET THE NEW POSITION
target->pos.x = source->pos.x + (k1->v.x);
target->pos.y = source->pos.y + (k1->v.y);
target->pos.z = source->pos.z + (k1->v.z);
source++;
target++;
k1++;
}
ComputeForces(m_TempSys[0]);
IntegrateSysOverTime(m_TempSys[0],m_TempSys[4],DeltaTime);
//From all those temp systems, get the final system
target = m_TargetSys;
source = m_CurrentSys;
k1 = m_TempSys[1];
k2 = m_TempSys[2];
k3 = m_TempSys[3];
k4 = m_TempSys[4];
float sixthDeltaT = 1.0f / 6.0f;
for (int i = 0; i < m_ParticleCnt; i++)
{
target->v.x = source->v.x + ((k1->f.x + ((k2->f.x + k3->f.x) * 2.0f) + k4->f.x) * sixthDeltaT);
target->v.y = source->v.y + ((k1->f.y + ((k2->f.y + k3->f.y) * 2.0f) + k4->f.y) * sixthDeltaT);
target->v.z = source->v.z + ((k1->f.z + ((k2->f.z + k3->f.z) * 2.0f) + k4->f.z) * sixthDeltaT);
// DETERMINE THE NEW POSITION FOR THE PARTICLE USING RK4 FORMULA
target->pos.x = source->pos.x + ((k1->v.x + ((k2->v.x + k3->v.x) * 2.0f) + k4->v.x) * sixthDeltaT);
target->pos.y = source->pos.y + ((k1->v.y + ((k2->v.y + k3->v.y) * 2.0f) + k4->v.y) * sixthDeltaT);
target->pos.z = source->pos.z + ((k1->v.z + ((k2->v.z + k3->v.z) * 2.0f) + k4->v.z) * sixthDeltaT);
source++;
target++;
k1++;
k2++;
k3++;
k4++;
}
}
//main work function for each thread
void * do_thread_work (void * _id) {
int id = (int) _id;
int tstep, i;
cpu_set_t mask;
CPU_ZERO( &mask );
#ifdef WITH_SMT
CPU_SET( id*2, &mask );
#else
CPU_SET( id, &mask );
#endif
sched_setaffinity(0, sizeof(mask), &mask);
for ( tstep=0; tstep< NUMBER_TIMESTEPS; tstep++)
{
barrier_wait (&barrier);
//need to reset these global vars in the beginning of every timestep
//global vars, need to be protected
if (id == 0) {
vir = 0.0;
epot = 0.0;
ekin = 0.0;
vel = 0.0;
count = 0.0;
}
//because we have this barrier here, we don't need one at the end of
//each timestep; this barrier effectively acts as if it was at the end;
//we need it here because we need to perform the initializations above
//at the _beginning_ of each timestep.
//No, you need a barrier at the end, as well, because if you only have one
//here but not at the end, you may reset some of the above global vars
//while a slower thread is still computing stuff in the previous timestep...
//I'm not sure, but we may be able to replace this barrier with just
//asm volatile("mfence" ::: "memory");
barrier_wait (&barrier);
//UpdateCoordinates (NULL, id);
//PARALLEL_EXECUTE( NTHREADS, UpdateCoordinates, NULL );
//barrier_wait (&barrier);
if ( tstep % neighUpdate == 0)
{
//this barrier needed because of the single-threaded code below
barrier_wait (&barrier);
if (id == 0) {
#ifdef PRINT_COORDINATES
PrintCoordinates(numMoles);
#endif
//global var, needs to be protected
ninter = 0;
}
//this barrier needed because of the single-threaded code above
barrier_wait (&barrier);
BuildNeigh (NULL, id);
//PARALLEL_EXECUTE( NTHREADS, BuildNeigh, NULL );
//this barrier needed because of the single-threaded code below
barrier_wait (&barrier);
if (id == 0) {
#ifdef PRINT_INTERACTION_LIST
PrintInteractionList(INPARAMS ninter);
#endif
#ifdef MEASURE
PrintConnectivity();
#endif
}
//we need this here because otherwise fast threads might start
//changing the data that thread 0 is reading above while running
//PrintInteractionList() and PrintConnectivity().
barrier_wait (&barrier);
}
ComputeForces (NULL, id);
//PARALLEL_EXECUTE( NTHREADS, ComputeForces, NULL );
//this barrier disappears with relaxed predicated commits
barrier_wait (&barrier);
//UpdateVelocities (NULL, id);
//PARALLEL_EXECUTE( NTHREADS, UpdateVelocities, NULL );
//this barrier disappears with relaxed predicated commits
//barrier_wait (&barrier);
//ComputeKEVel (NULL, id);
//PARALLEL_EXECUTE( NTHREADS, ComputeKEVel, NULL );
//Mike: consolidated all update functions into 1
Update(NULL, id);
//need a barrier at the end of each timestep
barrier_wait (&barrier);
if (id == 0) {
PrintResults (INPARAMS tstep, (double)ekin, (double)epot, (double)vir,(double)vel,(double)count,numMoles,(int)ninter);
}
barrier_wait (&barrier);
}
return NULL;
}
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;
}
}
}
void JelloMesh::MidPointIntegrate(double dt)
{
// TODO
double halfdt = 0.5 * dt;
ParticleGrid target = m_vparticles; // target is a copy!
ParticleGrid& source = m_vparticles; // source is a ptr!
// Step 1
ParticleGrid accum1 = m_vparticles;
for (int i = 0; i < m_rows+1; i++)
{
for (int j = 0; j < m_cols+1; j++)
{
for (int k = 0; k < m_stacks+1; k++)
{
Particle& s = GetParticle(source, i,j,k);
Particle& k1 = GetParticle(accum1, i,j,k);
k1.force = s.force;
k1.velocity = s.velocity;
Particle& t = GetParticle(target, i,j,k);
t.velocity = s.velocity + halfdt * k1.force * 1/k1.mass;
t.position = s.position + halfdt * k1.velocity;
}
}
}
ComputeForces(target);
// Step 2
ParticleGrid accum2 = m_vparticles;
for (int i = 0; i < m_rows+1; i++)
{
for (int j = 0; j < m_cols+1; j++)
{
for (int k = 0; k < m_stacks+1; k++)
{
Particle& t = GetParticle(target, i,j,k);
Particle& k2 = GetParticle(accum2, i,j,k);
k2.force = t.force;
k2.velocity = t.velocity;
}
}
}
// Put it all together
for (int i = 0; i < m_rows+1; i++)
{
for (int j = 0; j < m_cols+1; j++)
{
for (int k = 0; k < m_stacks+1; k++)
{
Particle& p = GetParticle(m_vparticles, i,j,k);
//Particle& k1 = GetParticle(accum1, i,j,k);
Particle& k2 = GetParticle(accum2, i,j,k);
p.velocity = p.velocity + dt*k2.force/p.mass;
p.position = p.position + dt*k2.velocity;
}
}
}
}
void JelloMesh::RK4Integrate(double dt)
{
double halfdt = 0.5 * dt;
ParticleGrid target = m_vparticles; // target is a copy!
ParticleGrid& source = m_vparticles; // source is a ptr!
// Step 1
ParticleGrid accum1 = m_vparticles;
for (int i = 0; i < m_rows+1; i++)
{
for (int j = 0; j < m_cols+1; j++)
{
for (int k = 0; k < m_stacks+1; k++)
{
Particle& s = GetParticle(source, i,j,k);
Particle& k1 = GetParticle(accum1, i,j,k);
k1.force = s.force;
k1.velocity = s.velocity;
Particle& t = GetParticle(target, i,j,k);
t.velocity = s.velocity + halfdt * k1.force * 1/k1.mass;
t.position = s.position + halfdt * k1.velocity;
}
}
}
ComputeForces(target);
// Step 2
ParticleGrid accum2 = m_vparticles;
for (int i = 0; i < m_rows+1; i++)
{
for (int j = 0; j < m_cols+1; j++)
{
for (int k = 0; k < m_stacks+1; k++)
{
Particle& t = GetParticle(target, i,j,k);
Particle& k2 = GetParticle(accum2, i,j,k);
k2.force = t.force;
k2.velocity = t.velocity;
Particle& s = GetParticle(source, i,j,k);
t.velocity = s.velocity + halfdt * k2.force * 1/k2.mass;
t.position = s.position + halfdt * k2.velocity;
}
}
}
ComputeForces(target);
// Step 3
ParticleGrid accum3 = m_vparticles;
for (int i = 0; i < m_rows+1; i++)
{
for (int j = 0; j < m_cols+1; j++)
{
for (int k = 0; k < m_stacks+1; k++)
{
Particle& t = GetParticle(target, i,j,k);
Particle& k3 = GetParticle(accum3, i,j,k);
k3.force = t.force;
k3.velocity = t.velocity;
Particle& s = GetParticle(source, i,j,k);
t.velocity = s.velocity + dt * k3.force * 1/k3.mass;
t.position = s.position + dt * k3.velocity;
}
}
}
ComputeForces(target);
// Step 4
ParticleGrid accum4 = m_vparticles;
for (int i = 0; i < m_rows+1; i++)
{
for (int j = 0; j < m_cols+1; j++)
{
for (int k = 0; k < m_stacks+1; k++)
{
Particle& t = GetParticle(target, i,j,k);
Particle& k4 = GetParticle(accum4, i,j,k);
k4.force = t.force;
k4.velocity = t.velocity;
}
}
}
// Put it all together
double asixth = 1/6.0;
double athird = 1/3.0;
for (int i = 0; i < m_rows+1; i++)
{
for (int j = 0; j < m_cols+1; j++)
{
for (int k = 0; k < m_stacks+1; k++)
{
Particle& p = GetParticle(m_vparticles, i,j,k);
Particle& k1 = GetParticle(accum1, i,j,k);
Particle& k2 = GetParticle(accum2, i,j,k);
Particle& k3 = GetParticle(accum3, i,j,k);
Particle& k4 = GetParticle(accum4, i,j,k);
p.velocity = p.velocity + dt*(asixth * k1.force +
athird * k2.force + athird * k3.force + asixth * k4.force)*1/p.mass;
p.position = p.position + dt*(asixth * k1.velocity +
athird * k2.velocity + athird * k3.velocity + asixth * k4.velocity);
}
}
}
}
void CRope::RK4Integrate( const float flDeltaTime, CRopeSample** ppSampleSource, CRopeSample** ppSampleTarget )
{
const float flDeltas[ MAX_TEMP_SAMPLES - 1 ] =
{
flDeltaTime * 0.5f,
flDeltaTime * 0.5f,
flDeltaTime * 0.5f,
flDeltaTime
};
{
RopeSampleData* pTemp1 = m_TempSys[ 0 ];
RopeSampleData* pTemp2 = m_TempSys[ 1 ];
for( size_t uiIndex = 0; uiIndex < m_uiNumSamples; ++uiIndex, ++pTemp1, ++pTemp2 )
{
const auto& data = ppSampleSource[ uiIndex ]->GetData();
pTemp2->mForce = data.mMassReciprocal * data.mForce * flDeltas[ 0 ];
pTemp2->mVelocity = data.mVelocity * flDeltas[ 0 ];
pTemp1->mMassReciprocal = data.mMassReciprocal;
pTemp1->mVelocity = data.mVelocity + pTemp2->mForce;
pTemp1->mPosition = data.mPosition + pTemp2->mVelocity;
}
ComputeForces( m_TempSys[ 0 ] );
}
for( size_t uiStep = 2; uiStep < MAX_TEMP_SAMPLES - 1; ++uiStep )
{
RopeSampleData* pTemp1 = m_TempSys[ 0 ];
RopeSampleData* pTemp2 = m_TempSys[ uiStep ];
for( size_t uiIndex = 0; uiIndex < m_uiNumSamples; ++uiIndex, ++pTemp1, ++pTemp2 )
{
const auto& data = ppSampleSource[ uiIndex ]->GetData();
pTemp2->mForce = data.mMassReciprocal * pTemp1->mForce * flDeltas[ uiStep - 1 ];
pTemp2->mVelocity = pTemp1->mVelocity * flDeltas[ uiStep - 1 ];
pTemp1->mMassReciprocal = data.mMassReciprocal;
pTemp1->mVelocity = data.mVelocity + pTemp2->mForce;
pTemp1->mPosition = data.mPosition + pTemp2->mVelocity;
}
ComputeForces( m_TempSys[ 0 ] );
}
{
RopeSampleData* pTemp1 = m_TempSys[ 0 ];
RopeSampleData* pTemp2 = m_TempSys[ 4 ];
for( size_t uiIndex = 0; uiIndex < m_uiNumSamples; ++uiIndex, ++pTemp1, ++pTemp2 )
{
const auto& data = ppSampleSource[ uiIndex ]->GetData();
pTemp2->mForce = data.mMassReciprocal * pTemp1->mForce * flDeltas[ 3 ];
pTemp2->mVelocity = pTemp1->mVelocity * flDeltas[ 3 ];
}
}
RopeSampleData* pTemp1 = m_TempSys[ 1 ];
RopeSampleData* pTemp2 = m_TempSys[ 2 ];
RopeSampleData* pTemp3 = m_TempSys[ 3 ];
RopeSampleData* pTemp4 = m_TempSys[ 4 ];
for( size_t uiIndex = 0; uiIndex < m_uiNumSamples; ++uiIndex, ++pTemp1, ++pTemp2, ++pTemp3, ++pTemp4 )
{
auto pSource = ppSampleSource[ uiIndex ];
auto pTarget = ppSampleTarget[ uiIndex ];
const Vector vecPosChange = 1.0f / 6.0f * ( pTemp1->mVelocity + ( pTemp2->mVelocity + pTemp3->mVelocity ) * 2 + pTemp4->mVelocity );
const Vector vecVelChange = 1.0f / 6.0f * ( pTemp1->mForce + ( pTemp2->mForce + pTemp3->mForce ) * 2 + pTemp4->mForce );
pTarget->GetData().mPosition = pSource->GetData().mPosition + ( vecPosChange );//* flDeltaTime );
pTarget->GetData().mVelocity = pSource->GetData().mVelocity + ( vecVelChange );//* flDeltaTime );
}
}