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