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 ); } }