void kalman_2D_update(kalman_filter_2D_t *kalman, vector_2_t measurement) { vector_2_t innovation = vsub2( measurement, mvmul2(kalman->observation_model, kalman->state)); matrix_2x2_t innovation_covariance = madd2( mmul2( mmul2(kalman->observation_model, kalman->covariance), trans2(kalman->observation_model)), kalman->noise_measurement); matrix_2x2_t kalman_gain = mmul2( mmul2(kalman->covariance, trans2(kalman->observation_model)), inv2(innovation_covariance)); kalman->state = vadd2(kalman->state, mvmul2(kalman_gain, innovation)); kalman->covariance = mmul2( msub2(ident_2x2, mmul2(kalman_gain, kalman->observation_model)), kalman->covariance); }
static void dynamicsMovieRun(struct part *part, struct xyz *averagePositions, struct xyz **pOldPositions, struct xyz **pNewPositions, struct xyz **pPositions, struct xyz *force, int start_frame, int num_frames) { int i; int j; // wware 060110 don't handle Interrupted with the BAIL mechanism for (i = start_frame; i < num_frames && !Interrupted; i++) { if (PrintFrameNums) { printf("%4d ", i); if ((i & 15) == 15) { printf("\n"); } fflush(stdout); } oneDynamicsFrame(part, IterPerFrame, averagePositions, pOldPositions, pNewPositions, pPositions, force); if (PrintPotentialEnergy) { // update velocities, so they can be used to compute kinetic energy - wware 060713 for (j = 0; j < part->num_atoms; j++) { vsub2(part->velocities[j], (*pPositions)[j], (*pOldPositions)[j]); } } writeDynamicsMovieFrame(OutputFile, i, part, averagePositions, i == NumFrames-1, *pPositions); if (DEBUG(D_DYNAMICS_SIMPLE_MOVIE)) { writeSimpleMovieFrame(part, *pNewPositions, force, ""); } } if (PrintFrameNums) { printf("\n"); fflush(stdout); } }
void oneDynamicsFrame(struct part *part, int iters, struct xyz *averagePositions, struct xyz **pOldPositions, struct xyz **pNewPositions, struct xyz **pPositions, struct xyz *force) { int j; int loop; double deltaTframe; struct xyz f; struct xyz *tmp; struct jig *jig; struct xyz *oldPositions = *pOldPositions; struct xyz *newPositions = *pNewPositions; struct xyz *positions = *pPositions; // wware 060109 python exception handling NULLPTR(part); NULLPTR(averagePositions); NULLPTR(oldPositions); NULLPTR(newPositions); NULLPTR(positions); iters = max(iters,1); deltaTframe = 1.0/iters; for (j=0; j<part->num_atoms; j++) { vsetc(averagePositions[j],0.0); } // See http://www.nanoengineer-1.net/mediawiki/index.php?title=Verlet_integration // for a discussion of how dynamics is done in the simulator. // we want: // x(t+dt) = 2x(t) - x(t-dt) + A dt^2 // or: // newPositions = 2 * positions - oldPositions + A dt^2 // wware 060110 don't handle Interrupted with the BAIL mechanism for (loop=0; loop < iters && !Interrupted; loop++) { _last_iteration = loop == iters - 1; Iteration++; // wware 060109 python exception handling updateVanDerWaals(part, NULL, positions); BAIL(); calculateGradient(part, positions, force); BAIL(); /* first, for each atom, find non-accelerated new pos */ /* Atom moved from oldPositions to positions last time, now we move it the same amount from positions to newPositions */ for (j=0; j<part->num_atoms; j++) { // f = positions - oldPositions vsub2(f,positions[j],oldPositions[j]); // newPositions = positions + f // or: // newPositions = 2 * positions - oldPositions vadd2(newPositions[j],positions[j],f); // after this, we will need to add A dt^2 to newPositions } // pre-force jigs for (j=0;j<part->num_jigs;j++) { /* for each jig */ jig = part->jigs[j]; // wware 060109 python exception handling NULLPTR(jig); switch (jig->type) { case LinearMotor: jigLinearMotor(jig, positions, newPositions, force, deltaTframe); break; default: break; } } /* convert forces to accelerations, giving new positions */ //FoundKE = 0.0; /* and add up total KE */ for (j=0; j<part->num_atoms; j++) { // to complete Verlet integration, this needs to do: // newPositions += A dt^2 // // force[] is in pN, mass is in g, Dt in seconds, f in pm vmul2c(f,force[j],part->atoms[j]->inverseMass); // inverseMass = Dt*Dt/mass // XXX: 0.15 probably needs a scaling by Dt // 0.15 = deltaX // keMax = m v^2 / 2 // v^2 = 2 keMax / m // v = deltaX / Dt = sqrt(2 keMax / m) // deltaX = Dt sqrt(2 keMax / m) // We probably don't want to do this, because a large raw // velocity isn't a problem, it's just when that creates a // high force between atoms that it becomes a problem. We // check that elsewhere. //if (!ExcessiveEnergyWarning && vlen(f)>0.15) { // 0.15 is just below H flyaway // WARNING3("Excessive force %.6f in iteration %d on atom %d -- further warnings suppressed", vlen(f), Iteration, j+1); // ExcessiveEnergyWarningThisFrame++; //} vadd(newPositions[j],f); //vsub2(f, newPositions[j], positions[j]); //ff = vdot(f, f); //FoundKE += atom[j].energ * ff; } // Jigs are executed in the following order: motors, // thermostats, grounds, measurements. Motions from each // motor are added together, then thermostats operate on the // motor output. Grounds override anything that moves atoms. // Measurements happen after all things that could affect // positions, including grounds. // motors for (j=0;j<part->num_jigs;j++) { /* for each jig */ jig = part->jigs[j]; if (jig->type == RotaryMotor) { jigMotor(jig, deltaTframe, positions, newPositions, force); } // LinearMotor handled in preforce above } // thermostats for (j=0;j<part->num_jigs;j++) { /* for each jig */ jig = part->jigs[j]; if (jig->type == Thermostat) { jigThermostat(jig, deltaTframe, positions, newPositions); } } // grounds for (j=0;j<part->num_jigs;j++) { /* for each jig */ jig = part->jigs[j]; if (jig->type == Ground) { jigGround(jig, deltaTframe, positions, newPositions, force); } } // measurements for (j=0;j<part->num_jigs;j++) { /* for each jig */ jig = part->jigs[j]; switch (jig->type) { case Thermometer: jigThermometer(jig, deltaTframe, positions, newPositions); break; case DihedralMeter: jigDihedral(jig, newPositions); break; case AngleMeter: jigAngle(jig, newPositions); break; case RadiusMeter: jigRadius(jig, newPositions); break; default: break; } } for (j=0; j<part->num_atoms; j++) { vadd(averagePositions[j],newPositions[j]); } tmp=oldPositions; oldPositions=positions; positions=newPositions; newPositions=tmp; if (ExcessiveEnergyWarningThisFrame > 0) { ExcessiveEnergyWarning = 1; } } for (j=0; j<part->num_atoms; j++) { vmulc(averagePositions[j],deltaTframe); } *pOldPositions = oldPositions; *pNewPositions = newPositions; *pPositions = positions; }
void dynamicsMovie(struct part *part) { struct xyz *averagePositions = (struct xyz *)allocate(sizeof(struct xyz) * part->num_atoms); struct xyz *oldPositions = (struct xyz *)allocate(sizeof(struct xyz) * part->num_atoms); struct xyz *newPositions = (struct xyz *)allocate(sizeof(struct xyz) * part->num_atoms); struct xyz *positions = (struct xyz *)allocate(sizeof(struct xyz) * part->num_atoms); struct xyz *force = (struct xyz *)allocate(sizeof(struct xyz) * part->num_atoms); struct xyz *tmp; int i; #ifndef WIN32 int timefailure = 0; struct timeval start; struct timeval end; double elapsedSeconds; char timebuffer[256]; #endif setThermalVelocities(part, Temperature); for (i = 0; i < part->num_atoms; i++) { vset(positions[i], part->positions[i]); vsub2(oldPositions[i], positions[i], part->velocities[i]); } #ifndef WIN32 // we should probably use times() to get user and system time // instead of wall time, but the clock ticks conversions appear to // be system dependant. if (gettimeofday(&start, NULL)) { timefailure = errno; errno = 0; } #endif if (TimeReversal) { dynamicsMovieRun(part, averagePositions, &oldPositions, &newPositions, &positions, force, 0, NumFrames/2); tmp = newPositions; newPositions = positions; positions = tmp; dynamicsMovieRun(part, averagePositions, &oldPositions, &newPositions, &positions, force, NumFrames/2, NumFrames); } else { dynamicsMovieRun(part, averagePositions, &oldPositions, &newPositions, &positions, force, 0, NumFrames); } #ifndef WIN32 if (gettimeofday(&end, NULL)) { timefailure = errno; } if (timefailure) { errno = timefailure; perror("gettimeofday"); errno = 0; } else { end.tv_sec -= start.tv_sec; end.tv_usec -= start.tv_usec; if (end.tv_usec < 0) { end.tv_sec--; end.tv_usec += 1000000; } elapsedSeconds = (double)end.tv_sec + (double)end.tv_usec / 1e6; write_traceline("# Duration: %s, %f sec/frame, %f sec/iteration\n", formatSeconds(elapsedSeconds, timebuffer), elapsedSeconds / (double)i, elapsedSeconds / (double)(i * IterPerFrame)); } #endif writeOutputTrailer(OutputFile, part, NumFrames); free(averagePositions); free(oldPositions); free(newPositions); free(positions); free(force); }
int occlusiontest(float *x, float *y, float *p, float *a, float *b) { float n1[2], n2[2], n3[2]; float nt[2]; float tmp[3], tmp2[3]; float ta[3], tb[3], tp[3], tx[3], ty[3], ti[3]; int A, B; n1[0] = x[1] - y[1]; n1[1] = - ( x[0] - y[0] ); n2[0] = x[1] - p[1]; n2[1] = - ( x[0] - p[0] ); n3[0] = p[1] - y[1]; n3[1] = - ( p[0] - y[0] ); vsub2(p, x, nt); if(scalarprod2(n1, nt) < 0) { n1[0] = - n1[0]; n1[1] = - n1[1]; } return 1; vsub2(y, p, tmp); if(scalarprod2(n2, tmp) < 0) { n2[0] = - n2[0]; n2[1] = - n2[1]; } vsub2(x, p, nt); if(scalarprod2(n3, nt) < 0) { n3[0] = - n3[0]; n3[1] = - n3[1]; } A = 0; B = 0; vsub2(x, a, nt); if( scalarprod2(n1, nt) < 0) A |= 1; if( scalarprod2(n2, nt) < 0) A |= 2; vsub2(y, a, nt); if( scalarprod2(n3, nt) < 0) A |= 4; vsub2(x, b, nt); if( scalarprod2(n1, nt) < 0) B |= 1; if( scalarprod2(n2, nt) < 0) B |= 2; vsub2(y, b, nt); if( scalarprod2(n3, nt) < 0) B |= 4; if(A & B) return 0; if(A | B) return 1; ta[0] = a[0]; ta[1] = a[1]; ta[2] = 1; tb[0] = b[0]; tb[1] = b[1]; tb[2] = 1; tp[0] = p[0]; tp[1] = p[1]; tp[2] = 1; tx[0] = x[0]; tx[1] = x[1]; tx[2] = 1; ty[0] = y[0]; ty[1] = y[1]; ty[2] = 1; crossprod(ta, tb, tmp); crossprod(tp, tx, tmp2); crossprod(tmp, tmp2, ti); if(ti[2] != 0) { ti[0] /= ti[2]; ti[1] /= ti[2]; } else { fprintf(stderr, "ab is parallel to px!\n"); } vsub2(ti, x, nt); if(scalarprod2(n1, nt) < 0) return 1; crossprod(ta, tb, tmp); crossprod(tp, ty, tmp2); crossprod(tmp, tmp2, ti); if(ti[2] != 0) { ti[0] /= ti[2]; ti[1] /= ti[2]; } else { fprintf(stderr, "ab is parallel to py!\n"); } vsub2(ti, x, nt); if(scalarprod2(n1, nt) < 0) return 1; return 0; }