double nTimeSteps(int n, simflat_t *s, real_t dt) {
extern void printIt(simflat_t *sim,FILE *fp);
int i;
/**
* Standard verlet algorithm:
* 1: advance positions half a timestep using current velocities
* 2: compute forces
* 3: advance velocities (momenta) a full timestep
* 4: advance positions half a timestep to bring in sync with velocities.
**/
/* convert dt to atomic units */
dt = dt * bohr_per_atu_to_A_per_s;
for(i=0; i<n; i++) {
advancePositions(s,0,s->nboxes,(dt/2.0));
computeForce(s);
advanceVelocity(s,0,s->nboxes,dt);
advancePositions(s,0,s->nboxes,(dt/2.0));
}
/* compute force to make consistent */
computeForce(s);
return s->e;
}
/**
* Compute interactions using blocking on holder structures rather
* than full body structures.
*/
void computeHolderBlockedPrefetchInteractions(nbody_holder_t* points)
{
int BLOCK_SIZE = 4096;
for (int step = 0; step < NTIMESTEPS; step++)
{
for (int l = 0; l < INPUT_SIZE; l += BLOCK_SIZE)
{
for (int i = l; i < MIN(l+BLOCK_SIZE, INPUT_SIZE); i++)
points[i].body->ax = points[i].body->ay = points[i].body->az = 0;
for (int k = l; k < INPUT_SIZE; k += BLOCK_SIZE)
{
int maxIndexi = MIN(k + BLOCK_SIZE, INPUT_SIZE);
if (k == l)
{
for (int i = l; i < maxIndexi; i++)
{
int maxIndexj = MIN(k + BLOCK_SIZE, INPUT_SIZE);
for (int j = i+1; j < maxIndexj; j++)
{
__builtin_prefetch(points + (i+PREFETCH_AMOUNT),0,0);
double dist = abs(points[i].x - points[j].x);
if (dist < THRESHOLD)
computeForce(points[i].body, points[j].body);
}
}
}
else
{
for (int i = l; i < maxIndexi; i++)
{
int maxIndexj = MIN(k + BLOCK_SIZE, INPUT_SIZE);
for (int j = k; j < maxIndexj; j++)
{
__builtin_prefetch(points + (i+PREFETCH_AMOUNT),0,0);
double dist = abs(points[i].x - points[j].x);
if (dist < THRESHOLD)
computeForce(points[i].body, points[j].body);
}
}
}
}
for (int i=l; i<MIN(l+BLOCK_SIZE, INPUT_SIZE); i++)
{
updatePosition(points[i].body);
points[i].x = points[i].body->x;
}
}
}
}
/// Advance the simulation time to t+dt using a leap frog method
/// (equivalent to velocity verlet).
///
/// Forces must be computed before calling the integrator the first time.
///
/// - Advance velocities half time step using forces
/// - Advance positions full time step using velocities
/// - Update link cells and exchange remote particles
/// - Compute forces
/// - Update velocities half time step using forces
///
/// This leaves positions, velocities, and forces at t+dt, with the
/// forces ready to perform the half step velocity update at the top of
/// the next call.
///
/// After nSteps the kinetic energy is computed for diagnostic output.
double timestep(SimFlat* s, int nSteps, real_t dt)
{
for (int ii=0; ii<nSteps; ++ii)
{
startTimer(velocityTimer);
advanceVelocity(s, s->boxes->nLocalBoxes, 0.5*dt);
stopTimer(velocityTimer);
startTimer(positionTimer);
advancePosition(s, s->boxes->nLocalBoxes, dt);
stopTimer(positionTimer);
startTimer(redistributeTimer);
redistributeAtoms(s);
stopTimer(redistributeTimer);
startTimer(computeForceTimer);
computeForce(s);
stopTimer(computeForceTimer);
startTimer(velocityTimer);
advanceVelocity(s, s->boxes->nLocalBoxes, 0.5*dt);
stopTimer(velocityTimer);
}
kineticEnergy(s);
return s->ePotential;
}
void
CSFLSGACSegmentor3D< TPixel >
::doGACSegmenation()
{
if (!mp_featureImage)
{
std::cerr<<"feature image not ready. stop.\n";
raise(SIGABRT);
}
/*============================================================
* From the initial mask, generate: 1. SFLS, 2. mp_label and
* 3. mp_phi.
*/
this->initializeSFLS();
//douher::saveAsImage3< double >(mp_phi, "initPhi.nrrd");
for (unsigned int it = 0; it < this->m_numIter; ++it)
{
computeForce();
this->normalizeForce();
this->oneStepLevelSetEvolution();
}
}
//do neighbor search
//compute density
//compute force
//integrate
//scene interaction
//visualization
void particleSystem::LeapfrogIntegrate(float dt){
float halfdt = 0.5f * dt;
particleGrid target = particles;// target is a copy!
particleGrid& source = particles;//source is a ptr!
for (int i=0; i < target.size(); i++){
target[i].pos = source[i].pos + source[i].vel * dt
+ halfdt * dt * source[i].force / source[i].actual_density;
}
//calculate actual density
/*particleGrid nghrs;
for (int i=0; i < target.size(); i++){
if( !checkIfOutOfBoundry(target[i]) ){
nghrs = gridcells.getNeighbors(target[i]);
target[i].actual_density = computeDensity(nghrs, target[i]);
target[i].pressure = target[i].gas_constant * (target[i].actual_density - target[i].rest_density);
}
}
for (int i=0; i < target.size(); i++){
if( !checkIfOutOfBoundry(target[i]) ){
nghrs = gridcells.getNeighbors(target[i]);
target[i].force = computeForce(nghrs, target[i]);
}
}*/
//calculate actual density
for (int i=0; i < target.size(); i++){
target[i].actual_density = computeDensity(target, target[i]);
target[i].pressure = target[i].gas_constant * (target[i].actual_density - target[i].rest_density);
}
for (int i=0; i < target.size(); i++){
target[i].force = computeForce(target, target[i]);
}
glm::vec3 collision_normal = glm::vec3(0.f);
for (int i=0; i < target.size(); i++){
if( CollisionDectection(target[i], collision_normal) ){
glm::vec3 vn = (source[i].vel * collision_normal) * collision_normal;//decompose v along normal
glm::vec3 vt = source[i].vel - vn;
source[i].vel = 0.9f * vt - 0.8f * vn;//flip normal direction speed
}else{
source[i].vel += halfdt * (target[i].force/target[i].actual_density + source[i].force /source[i].actual_density);
source[i].pos = target[i].pos;
source[i].force = target[i].force;
source[i].actual_density = target[i].actual_density;
}
}
//gridcells.refillGrid(source);
}
void updateHaptics(void)
{
// reset clock
simClock.reset();
// main haptic simulation loop
while(simulationRunning)
{
// stop the simulation clock
simClock.stop();
// read the time increment in seconds
double timeInterval = simClock.getCurrentTimeSeconds();
if (timeInterval > 0.001) { timeInterval = 0.001; }
// restart the simulation clock
simClock.reset();
simClock.start();
// read position from haptic device
cVector3d pos;
hapticDevice->getPosition(pos);
pos.mul(workspaceScaleFactor);
device->setPos(pos);
// init temp variable
cVector3d force;
force.zero();
// compute reaction forces
for (int y=0; y<10; y++)
{
for (int x=0; x<10; x++)
{
cVector3d nodePos = nodes[x][y]->m_pos;
cVector3d f = computeForce(pos, deviceRadius, nodePos, radius, stiffness);
cVector3d tmpfrc = cNegate(f);
nodes[x][y]->setExternalForce(tmpfrc);
force.add(f);
}
}
// integrate dynamics
defWorld->updateDynamics(timeInterval);
// scale force
force.mul(deviceForceScale);
// send forces to haptic device
hapticDevice->setForce(force);
}
// exit haptics thread
simulationFinished = true;
}
/// Initialized the main CoMD data stucture, SimFlat, based on command
/// line input from the user. Also performs certain sanity checks on
/// the input to screen out certain non-sensical inputs.
///
/// Simple data members such as the time step dt are initialized
/// directly, substructures such as the potential, the link cells, the
/// atoms, etc., are initialized by calling additional initialization
/// functions (initPotential(), initLinkCells(), initAtoms(), etc.).
/// Initialization order is set by the natural dependencies of the
/// substructure such as the atoms need the link cells so the link cells
/// must be initialized before the atoms.
SimFlat* initSimulation(Command cmd)
{
SimFlat* sim = comdMalloc(sizeof(SimFlat));
sim->nSteps = cmd.nSteps;
sim->printRate = cmd.printRate;
sim->dt = cmd.dt;
sim->domain = NULL;
sim->boxes = NULL;
sim->atoms = NULL;
sim->ePotential = 0.0;
sim->eKinetic = 0.0;
sim->atomExchange = NULL;
sim->pot = initPotential(cmd.doeam, cmd.potDir, cmd.potName, cmd.potType);
real_t latticeConstant = cmd.lat;
if (cmd.lat < 0.0)
latticeConstant = sim->pot->lat;
// ensure input parameters make sense.
sanityChecks(cmd, sim->pot->cutoff, latticeConstant, sim->pot->latticeType);
sim->species = initSpecies(sim->pot);
real3 globalExtent;
globalExtent[0] = cmd.nx * latticeConstant;
globalExtent[1] = cmd.ny * latticeConstant;
globalExtent[2] = cmd.nz * latticeConstant;
sim->domain = initDecomposition(
cmd.xproc, cmd.yproc, cmd.zproc, globalExtent);
sim->boxes = initLinkCells(sim->domain, sim->pot->cutoff);
sim->atoms = initAtoms(sim->boxes);
// create lattice with desired temperature and displacement.
createFccLattice(cmd.nx, cmd.ny, cmd.nz, latticeConstant, sim);
setTemperature(sim, cmd.temperature);
randomDisplacements(sim, cmd.initialDelta);
sim->atomExchange = initAtomHaloExchange(sim->domain, sim->boxes);
// Forces must be computed before we call the time stepper.
startTimer(redistributeTimer);
redistributeAtoms(sim);
stopTimer(redistributeTimer);
startTimer(computeForceTimer);
computeForce(sim);
stopTimer(computeForceTimer);
kineticEnergy(sim);
return sim;
}
//Compute the force in the z direction
double SCFPotentialzforce(double R,double Z, double phi,
double t,
struct potentialArg * potentialArgs)
{
double r;
double theta;
cyl_to_spher(R, Z,&r, &theta);
double dr_dz = Z/r;
double dtheta_dz = -R/(r*r);
double dphi_dz = 0;
double F[3];
computeForce(R, Z, phi, t,potentialArgs, &F[0]) ;
return *(F + 0)*dr_dz + *(F + 1)*dtheta_dz + *(F + 2)*dphi_dz;
}
//Compute the force in the phi direction
double SCFPotentialphiforce(double R,double Z, double phi,
double t,
struct potentialArg * potentialArgs)
{
double r;
double theta;
cyl_to_spher(R, Z, &r, &theta);
double dr_dphi = 0;
double dtheta_dphi = 0;
double dphi_dphi = 1;
double F[3];
computeForce(R, Z, phi, t,potentialArgs, &F) ;
return *(F + 0)*dr_dphi + *(F + 1)*dtheta_dphi + *(F + 2)*dphi_dphi;
}
void computePrefetchInteractions(nbody_t* points)
{
for (int step = 0; step < NTIMESTEPS; step++)
{
for (int i = 0; i < INPUT_SIZE; i++)
{
points[i].ax = points[i].ay = points[i].az = 0;
for (int j = i+1; j < INPUT_SIZE; j++)
{
__builtin_prefetch(points + (i+PREFETCH_AMOUNT),0,0);
double dist = abs(points[i].x - points[j].x);
if (dist < THRESHOLD)
computeForce(&points[i], &points[j]);
}
updatePosition(&points[i]);
}
}
}
/*
* Compute the interactions between the parameter and
* all the holder objects.
*/
void computeHolderInteractions(nbody_holder_t* points)
{
for (int step = 0; step < NTIMESTEPS; step++)
{
for (int i = 0; i < INPUT_SIZE; i++)
{
points[i].body->ax = points[i].body->ay = points[i].body->az = 0;
for (int j = i+1; j < INPUT_SIZE; j++)
{
double dist = abs(points[i].x - points[j].x);
if (dist < THRESHOLD)
computeForce(points[i].body, points[j].body);
}
updatePosition(points[i].body);
points[i].x = points[i].body->x;
}
}
}
void SPHSystem::sphLoop()
{
TIMER[NEIGHBOR].Start();
computeNeighbor();
TIMER[NEIGHBOR].Stop();
TIMER[DENSITY].Start();
computeDensityPressure();
TIMER[DENSITY].Stop();
TIMER[FORCE].Start();
computeForce();
TIMER[FORCE].Stop();
TIMER[INTEGRATION].Start();
integration();
TIMER[INTEGRATION].Stop();
numFrame++;
}
//Compute the force in the R direction
double SCFPotentialRforce(double R,double Z, double phi,
double t,
struct potentialArg * potentialArgs)
{
double r;
double theta;
cyl_to_spher(R, Z, &r, &theta);
// The derivatives
double dr_dR = R/r;
double dtheta_dR = Z/(r*r);
double dphi_dR = 0;
double F[3];
computeForce(R, Z, phi, t,potentialArgs, &F[0]) ;
return *(F + 0)*dr_dR + *(F + 1)*dtheta_dR + *(F + 2)*dphi_dR;
}
/*
* Compute the interactions between the parameter and
* all the holder objects.
*/
void computeHolderPrefetchInteractions(nbody_holder_t* points)
{
for (int step = 0; step < NTIMESTEPS; step++)
{
for (int i = 0; i < INPUT_SIZE; i++)
{
points[i].body->ax = points[i].body->ay = points[i].body->az = 0;
for (int j = i+1; j < INPUT_SIZE; j++)
{
// Prefetch the element that is at index (i + PREFETCH_AMOUNT)
__builtin_prefetch(points + (i+PREFETCH_AMOUNT),0,0);
double dist = abs(points[i].x - points[j].x);
if (dist < THRESHOLD)
computeForce(points[i].body, points[j].body);
}
updatePosition(points[i].body);
points[i].x = points[i].body->x;
}
}
}
void computeSortedPrefetchInteractions(nbody_t* points)
{
// Use qsort to sort the points by x value
for (int step = 0; step < NTIMESTEPS; step++)
{
qsort(points, INPUT_SIZE, sizeof(nbody_t), compareBodies);
for (int i = 0; i < INPUT_SIZE; i++)
{
points[i].ax = points[i].ay = points[i].az = 0;
for (int j = i+1; j < INPUT_SIZE; j++)
{
__builtin_prefetch(points + (i+PREFETCH_AMOUNT),0,0);
double dist = abs(points[i].x - points[j].x);
if (dist < THRESHOLD)
computeForce(&points[i], &points[j]);
else break;
}
updatePosition(&points[i]);
}
}
}
void computeHolderSortedInteractions(nbody_holder_t* points) {
for (int step = 0; step < NTIMESTEPS; step++)
{
// Use qsort to sort the points by x value
qsort(points, INPUT_SIZE, sizeof(nbody_holder_t), compareHolders);
for (int i = 0; i < INPUT_SIZE; i++)
{
points[i].body->ax = points[i].body->ay = points[i].body->az = 0;
for (int j = i+1; j < INPUT_SIZE; j++)
{
double dist = abs(points[i].x - points[j].x);
// Once the distance has reached the threshold we know
// we don't need to compute any further.
if (dist < THRESHOLD)
computeForce(points[i].body, points[j].body);
else break;
}
updatePosition(points[i].body);
points[i].x = points[i].body->x;
}
}
}
int ParticleSimulator::step(double time)
{
Vector pos;
double fg;
for(int i = 0; i < m_particle->m_p.size(); i++){
m_particle->getState(i, pos);
//m_particle->m_p[i].force=VectorObj(0,0,0);
fg = m_gravity * m_particle->m_p[i].mass;
m_particle->m_p[i].force[1] = fg;
}
for(int i = 0; i < m_particle->m_p.size(); i++){
for(int j = 0; j < m_no_spring; j++){
if(m_spring[j].p1!=NULL && m_spring[j].p2!=NULL){
if( m_particle->m_p.size()!=0 && m_spring[j].p1 == &m_particle->m_p[i]){
computeForce(m_spring[j].p2, m_spring[j].p1, j);
m_spring[j].p1->force+=_force;
//animTcl::OutputMessage("force p1: %lf %lf %lf", m_spring[j].p1->force.x(), m_spring[j].p1->force.y(), m_spring[j].p1->force.z());
}
else if( m_spring[j].p2 == &m_particle->m_p[i]){
computeForce(m_spring[j].p1, m_spring[j].p2, j);
// animTcl::OutputMessage("force p2: %lf %lf %lf", m_spring[j].p2->force.x(), m_spring[j].p2->force.y(), m_spring[j].p2->force.z());
m_spring[j].p2->force+=_force;
}
}
}
}
VectorObj Pg(0,-0.5,0);
VectorObj T(1,0,0);
VectorObj N(0,1,0);
for(int i = 0; i < m_particle->m_p.size(); i++){
if((m_particle->m_p[i].pos-Pg).dot(N)<=1 ){
//if collision with ground
m_particle->m_p[i].force = VectorObj(0,0,0);
VectorObj force_n = m_ground.ks*N.dot(Pg-m_particle->m_p[i].pos)*N-
m_ground.kd*m_particle->m_p[i].vel.normalize().dot(N)*N;
//VectorObj force_n = m_ground.ks*N.dot(Pg-m_particle->m_p[i].pos)*N-m_ground.kd*m_particle->m_p[i].vel.dot(N)*N;
//animTcl::OutputMessage("force_n dot N: %lf", force_n.dot(N));
if(force_n.dot(N)>0){
m_particle->m_p[i].force+=force_n;
//vt = |a|cos(theta)*b
VectorObj VN = N.normalize().dot(m_particle->m_p[i].vel)*N.normalize();
VectorObj VT = m_particle->m_p[i].vel - VN;
m_particle->m_p[i].vel = VT-m_ground.kd*VN;
}
else{
m_particle->m_p[i].force-=force_n;
//m_particle->m_p[i].force = -m_particle->m_p[i].force;
}
//animTcl::OutputMessage("force p2: %lf %lf %lf", m_particle->m_p[i].force.x(), m_particle->m_p[i].force.y(), m_particle->m_p[i].force.z());
}
if(m_integration=="euler"){
//forward Euler
VectorObj cur_vel = m_particle->m_p[i].vel;
m_particle->m_p[i].vel += (m_particle->m_p[i].force/m_particle->m_p[i].mass)*m_timestep;
m_particle->m_p[i].pos += cur_vel*m_timestep;
}
//symplectic
else if(m_integration=="symplectic"){
m_particle->m_p[i].vel += (m_particle->m_p[i].force/m_particle->m_p[i].mass)*m_timestep;
m_particle->m_p[i].pos += m_timestep* m_particle->m_p[i].vel;
}
//verlet
else if(m_integration=="verlet"){
m_particle->m_p[i].vel += (m_particle->m_p[i].force/m_particle->m_p[i].mass)*m_timestep;
m_prev_pos = m_particle->m_p[i].pos-m_timestep*m_particle->m_p[i].vel;
m_particle->m_p[i].pos = 2*m_particle->m_p[i].pos-m_prev_pos+pow(m_timestep,2)/m_particle->m_p[i].mass*m_particle->m_p[i].force;
}
else{
animTcl::OutputMessage("Wrong integration method!");
return TCL_ERROR;
}
m_particle->setState(i, pos);
}
return TCL_OK;
}// ParticleSimulator::step
int main() {
int i, j, tstep;
time_t t;
srand((unsigned) time(&t));
struct timeval startTime, endTime;
long long time1, time2, totaltime;
getBasic();
printf("No. of Particles : %d\nNo. of Particles on shell : %d\nShell Radius : %d \n", npos, nsphere, shell_radius);
/////START TOTAL_TIME
//gettimeofday(&startTime, NULL);
double *pos, *shell, *rad;
pos = (double *)malloc(sizeof(double)*(npos+nsphere)*3);
rad = (double *)malloc(sizeof(double)*npos);
shell = pos + 3*npos;
setPosRad(pos, rad);
savePos(pos, rad, 0);
getShell(shell);
//printf("here\n");
//check pos & shell
//printVectors(pos, npos+nsphere, 3);
double L = 2*shell_radius;
int boxdim = shell_radius;
double cutoff2 = (2 * shell_particle_radius) * (2 * shell_particle_radius);
int maxnumpairs = 5000000;
int *pairs = (int *)malloc(sizeof(int)*2*maxnumpairs);
int *finalPairs = (int *)malloc(sizeof(int)*2*maxnumpairs);
double *distances2 = (double *)malloc(sizeof(double)*maxnumpairs);
int numpairs_p;
double *f;
f = (double *)malloc(sizeof(double)*3*npos);
complex *f1, *f2, *f3, *rpy, *lanczos_out;
f1 = (complex *)malloc(sizeof(complex)*npos);
f2 = (complex *)malloc(sizeof(complex)*npos);
f3 = (complex *)malloc(sizeof(complex)*npos);
rpy = (complex *)malloc(sizeof(complex)*(npos*3));
lanczos_out = (complex *)malloc(sizeof(complex)*(npos*3));
double *f_serial;
f_serial = (double *)malloc(sizeof(double)*3*npos);
double error1, error2;
double *standardNormalZ1, *standardNormalZ;
standardNormalZ = (double *)malloc(sizeof(double)*npos*3);
standardNormalZ1 = (double *)malloc(sizeof(double)*npos*3);
lanczos_t *lanczos, *lanczos1;
lanczos = malloc(sizeof(lanczos));
int maxiters = 200;
double *A;
lanczos1 = malloc(sizeof(lanczos));
A = (double *)malloc(sizeof(double)*3*3*npos*npos);
char dir[100] = "outputs/";
//double *Mf;
//Mf = (double *)malloc(sizeof(double)*3*npos);
for(tstep = 0; tstep<tmax; tstep++) {
printf("time step %d started\n", tstep+1);
//gettimeofday(&startTime, NULL);
interactions(npos+nsphere, pos, L, boxdim, cutoff2, distances2, pairs, maxnumpairs, &numpairs_p);
interactionsFilter(&numpairs_p, pairs, finalPairs, rad, pos);
printf("beyond interactions\n");
//printf("Final number of pairs (after filtering) : %d\n", numpairs_p);
computeForce(f, f1, f2, f3, pos, rad, finalPairs, numpairs_p);
//gettimeofday(&endTime, NULL);
//time1 = (endTime.tv_sec-startTime.tv_sec)*1000000 + endTime.tv_usec-startTime.tv_usec;
//printf("For INTERACTIVE : %ld msec\n", time1/1000);
if(CHECKCODE) {
//gettimeofday(&startTime, NULL);
computeForceSerial(f_serial, pos, rad, shell);
//gettimeofday(&endTime, NULL);
//time2 = (endTime.tv_sec-startTime.tv_sec)*1000000 + endTime.tv_usec-startTime.tv_usec;
//printf("For SERIAL : %ld msec\n", time2/1000);
//printVectors(f_serial, npos, 3);
//error1 = relError(f_serial, f, npos, 3);
//printf("Relative Error in computeForce %lf\n", error1);
//error2 = maxError(f_serial, f, npos, 3);
//printf("Max Error in computeForce %lf\n", error2);
}
//char dir[100] = "outputs/";
//printf("Calling computeRPY : \n");
//gettimeofday(&startTime, NULL);
computerpy_(&npos, pos, rad, f1, f2, f3, rpy, dir);
//gettimeofday(&endTime, NULL);
//time2 = (endTime.tv_sec-startTime.tv_sec)*1000000 + endTime.tv_usec-startTime.tv_usec;
//printf("For 5 call FMM: %ld \n", time2/1000);
//gettimeofday(&endTime, NULL);
//time1 = (endTime.tv_sec-startTime.tv_sec)*1000000 + endTime.tv_usec-startTime.tv_usec;
//printf("Total Time taken for RPY with FMM: %ld \n", time1/1000);
//printf("Calling postCorrection : \n");
//gettimeofday(&startTime, NULL);
//postCorrection(npos, pos, rad, numpairs_p, finalPairs, f1, f2, f3, rpy);
getNorm((100000000+(rand()%99999999)), standardNormalZ);
//getNorm((100000000+(rand()%99999999)), standardNormalZ1);
//printVectors(standardNormalZ, 3*npos, 1);
/*
if(CHECKCODE){
for(i=0;i<npos*3;i++)
standardNormalZ1[i] = standardNormalZ[i];
}
*/
//
if(CHECKCODE) {
//create_lanczos (&lanczos1, 1, maxiters, npos*3);
//double *Az;
//Az = (double *)malloc(sizeof(double)*3*npos);
//gettimeofday(&startTime, NULL);
createDiag(A, rad);
//sqrtMatrix(A, standardNormalZ1);
//printVectors(A, 3*npos, 3*npos);
//printVectorsToFile(A, 3*npos);
mobilityMatrix(A, pos, rad);
//printVectorsToFile(A, 3*npos);
//printVectorsToFile(A, npos*3);
//compute_lanczos(lanczos1, 1e-4, 1, standardNormalZ1, 3*npos,
// SERIAL, f1, f2, f3, lanczos_out, pos, rad, numpairs_p, finalPairs, A);
//printf("Z: \n");
//printVectors(standardNormalZ1, npos, 3);
//printVectorsToFile(A, 3*npos);
multiplyMatrix(A, f_serial);
//gettimeofday(&endTime, NULL);
//time1 = (endTime.tv_sec-startTime.tv_sec)*1000000 + endTime.tv_usec-startTime.tv_usec;
//printf("For SERIAL : %ld msec\n", time1/1000);
//printf("Mf: \n");
//printf("M*f : \n");
//printVectors(A, npos, 3);
//printf("\n");
//printf("RPY : \n");
//printVectorsComplex(rpy, 10, 3);
//printf("\n");
//printf("Relative error in RPy and M*f : %lf\n", relErrorRealComplex(A, rpy, npos, 3));
//printf("Maximum error in Rpy and M*f : %lf\n", maxErrorRealComplex(A, rpy, npos, 3));
}
//printf("Adding Random Brownian motion ... \n");
create_lanczos (&lanczos, 1, maxiters, npos*3);
compute_lanczos(lanczos, 1e-4, 1, standardNormalZ, 3*npos,
FMM, f1, f2, f3, lanczos_out, pos, rad, numpairs_p, finalPairs, A);
/////////END TOTAL_TIME
//gettimeofday(&endTime, NULL);
//totaltime = (endTime.tv_sec-startTime.tv_sec)*1000000 + endTime.tv_usec-startTime.tv_usec;
//printf("Total time computing 1 time step (%d particles) : %ld msec\n", npos, totaltime/1000);
//if(CHECKCODE){
// printf("Relative Error in brownian : %lf\n", relError(standardNormalZ, standardNormalZ1, npos, 3));
//}
updatePos(pos, rpy, standardNormalZ);
//updatePosSerial(pos, A, standardNormalZ1);
//printf("new pos: \n");
//printVectors(pos, npos, 3);
savePos(pos, rad, tstep+1);
printf("%d time steps done\n\n", tstep+1);
}
//gettimeofday(&endTime, NULL);
//totaltime = (endTime.tv_sec-startTime.tv_sec)*1000000 + endTime.tv_usec-startTime.tv_usec;
//printf("Total time computing %d time steps (%d particles) : %ld msec\n", tmax, npos, totaltime/1000);
return 0;
}
void updateHaptics(void)
{
// reset simulation clock
simClock.reset();
simClock.start();
// main haptic simulation loop
while(simulationRunning)
{
// read position from haptic device
cVector3d pos;
hapticDevice->getPosition(pos);
pos.mul(workspaceScaleFactor);
device->setPos(pos);
// init temp variable
cVector3d force;
force.zero();
// compute reaction forces
list<cGELMesh*>::iterator i;
// model water level
for(i = defWorld->m_gelMeshes.begin(); i != defWorld->m_gelMeshes.end(); ++i)
{
cGELMesh *nextItem = *i;
if (nextItem->m_useMassParticleModel)
{
int numVertices = nextItem->m_gelVertices.size();
for (int i=0; i<numVertices; i++)
{
cVector3d nodePos = nextItem->m_gelVertices[i].m_massParticle->m_pos;
cVector3d force = cVector3d(-0.002 * nodePos.x, -0.002 * nodePos.y, 0.0);
if (nodePos.z < level)
{
double depth = nodePos.z - level;
force.add(cVector3d(0,0,-100*depth));
}
nextItem->m_gelVertices[i].m_massParticle->setExternalForce(force);
}
}
if (nextItem->m_useSkeletonModel)
{
list<cGELSkeletonNode*>::iterator i;
for(i = nextItem->m_nodes.begin(); i != nextItem->m_nodes.end(); ++i)
{
cGELSkeletonNode* node = *i;
cVector3d nodePos = node->m_pos;
double radius = node->m_radius;
cVector3d force = cVector3d(-0.01 * nodePos.x, -0.01 * (nodePos.y), 0.0);
if ((nodePos.z-radius) < level)
{
double depth = (nodePos.z-radius) - level;
force.add(cVector3d(0,0,-1.0 * depth));
node->m_vel.mul(0.95);
}
node->setExternalForce(force);
}
}
}
// compute haptic feedback
for(i = defWorld->m_gelMeshes.begin(); i != defWorld->m_gelMeshes.end(); ++i)
{
cGELMesh *nextItem = *i;
if (nextItem->m_useMassParticleModel)
{
int numVertices = nextItem->m_gelVertices.size();
for (int i=0; i<numVertices; i++)
{
cVector3d nodePos = nextItem->m_gelVertices[i].m_massParticle->m_pos;
cVector3d f = computeForce(pos, deviceRadius, nodePos, radius, stiffness);
if (f.lengthsq() > 0)
{
cVector3d tmpfrc = cNegate(f);
nextItem->m_gelVertices[i].m_massParticle->setExternalForce(tmpfrc);
}
force.add(cMul(1.0, f));
}
}
if (nextItem->m_useSkeletonModel)
{
list<cGELSkeletonNode*>::iterator i;
for(i = nextItem->m_nodes.begin(); i != nextItem->m_nodes.end(); ++i)
{
cGELSkeletonNode* node = *i;
cVector3d nodePos = node->m_pos;
double radius = node->m_radius;
cVector3d f = computeForce(pos, deviceRadius, nodePos, radius, stiffness);
if (f.lengthsq() > 0)
{
cVector3d tmpfrc = cNegate(f);
node->setExternalForce(tmpfrc);
}
force.add(cMul(4.0, f));
}
}
}
// integrate dynamics
double interval = simClock.stop();
simClock.reset();
simClock.start();
if (interval > 0.001) { interval = 0.001; }
defWorld->updateDynamics(interval);
// scale force
force.mul(0.6 * deviceForceScale);
// water viscosity
if ((pos.z - deviceRadius) < level)
{
// read damping properties of haptic device
cHapticDeviceInfo info = hapticDevice->getSpecifications();
double Kv = 0.8 * info.m_maxLinearDamping;
// read device velocity
cVector3d linearVelocity;
hapticDevice->getLinearVelocity(linearVelocity);
// compute a scale factor [0,1] proportional to percentage
// of tool volume immersed in the water
double val = (level - (pos.z - deviceRadius)) / (2.0 * deviceRadius);
double scale = cClamp(val, 0.1, 1.0);
// compute force
cVector3d forceDamping = cMul(-Kv * scale, linearVelocity);
force.add(forceDamping);
}
// send forces to haptic device
hapticDevice->setForce(force);
}
// exit haptics thread
simulationFinished = true;
}
SimFlat* initSimulation(Command cmd)
{
SimFlat* sim = comdMalloc(sizeof(SimFlat));
sim->nSteps = cmd.nSteps;
sim->printRate = cmd.printRate;
sim->dt = cmd.dt;
sim->domain = NULL;
sim->boxes = NULL;
sim->atoms = NULL;
sim->ePotential = 0.0;
sim->eKinetic = 0.0;
sim->atomExchange = NULL;
sim->pot = initPotential(cmd.doeam, cmd.potDir, cmd.potName, cmd.potType);
real_t latticeConstant = cmd.lat;
if (cmd.lat < 0.0)
latticeConstant = sim->pot->lat;
// ensure input parameters make sense.
sanityChecks(cmd, sim->pot->cutoff, latticeConstant, sim->pot->latticeType);
sim->species = initSpecies(sim->pot);
real3 globalExtent;
globalExtent[0] = cmd.nx * latticeConstant;
globalExtent[1] = cmd.ny * latticeConstant;
globalExtent[2] = cmd.nz * latticeConstant;
sim->domain = initDecomposition(cmd.xproc, cmd.yproc, cmd.zproc, globalExtent);
sim->boxes = initLinkCells(sim->domain, sim->pot->cutoff);
sim->atoms = initAtoms(sim->boxes);
sim->defInfo = initDeformation(sim, cmd.defGrad);
//printf("Got to here\n");
// create lattice with desired temperature and displacement.
createFccLattice(cmd.nx, cmd.ny, cmd.nz, latticeConstant, sim);
setTemperature(sim,0.0);
randomDisplacements(sim, cmd.initialDelta);
sim->atomExchange = initAtomHaloExchange(sim->domain, sim->boxes);
forwardDeformation(sim);
//eamForce(sim);
// Procedure for energy density passing from the macrosolver to CoMD
//setTemperature(sim,((cmd.energy*latticeVolume*cmd.nx*cmd.ny*cmd.nz-sim->ePotential)/sim->atoms->nGlobal)/(kB_eV * 1.5));
//randomDisplacements(sim, cmd.initialDelta);
// Forces must be computed before we call the time stepper.
startTimer(redistributeTimer);
redistributeAtoms(sim);
stopTimer(redistributeTimer);
startTimer(computeForceTimer);
computeForce(sim);
stopTimer(computeForceTimer);
double cohmmEnergy=cmd.energy*sim->defInfo->globalVolume;
double temperatureFromEnergyDensity=((cohmmEnergy-sim->ePotential)/sim->atoms->nGlobal)/(kB_eV*1.5);
setTemperature(sim,temperatureFromEnergyDensity); //uncomment to set temperature according to hmm energy density
//setTemperature(sim,cmd.temperature); //uncomment to directly input temperature
kineticEnergy(sim);
return sim;
}
