Пример #1
0
void KoulesSimulator::elasticCollision(unsigned int i, unsigned int j)
{
    double *a = &qcur_[ind(i)], *b = &qcur_[ind(j)];
    double d[2] = { b[0] - a[0], b[1] - a[1] };
    normalize2(d);
    double va = dot2(a + 2, d), vb = dot2(b + 2, d);
    if (va - vb <= 0.)
        return;
    double ma = si_->getStateSpace()->as<KoulesStateSpace>()->getMass(i);
    double mb = si_->getStateSpace()->as<KoulesStateSpace>()->getMass(j);
    double vap = (va * (ma - mb) + 2. * mb * vb) / (ma + mb);
    double vbp = (vb * (mb - ma) + 2. * ma * va) / (ma + mb);
    double amag = vap - va, bmag = vbp - vb;
    if (std::abs(amag) < eps)
        amag = amag < 0. ? -eps : eps;
    if (std::abs(bmag) < eps)
        bmag = bmag < 0. ? -eps : eps;
#ifndef NDEBUG
    double px = ma * a[2] + mb * b[2], py = ma * a[3] + mb * b[3];
    double k = ma * (a[2] * a[2] + a[3] * a[3]) + mb * (b[2] * b[2] + b[3] * b[3]);
#endif
    vadd2(a + 2, amag, d);
    vadd2(b + 2, bmag, d);

#ifndef NDEBUG
    // preservation of momemtum
    assert(std::abs(ma * a[2] + mb * b[2] - px) < 1e-6);
    assert(std::abs(ma * a[3] + mb * b[3] - py) < 1e-6);
    // preservation of kinetic energy
    assert(std::abs(ma * (a[2] * a[2] + a[3] * a[3]) + mb * (b[2] * b[2] + b[3] * b[3]) - k) < 1e-6);
#endif
}
Пример #2
0
void kalman_2D_prediction(kalman_filter_2D_t *kalman, vector_2_t control)
{
    kalman->state =     vadd2( 	mvmul2(kalman->system_model, kalman->state),
                                mvmul2(kalman->control_model, control));

    kalman->covariance= madd2(  mmul2( 	mmul2(	kalman->system_model,
                                        kalman->covariance),
                                        trans2(kalman->system_model)),
                                kalman->noise_prediction);
}
Пример #3
0
void sl_entity_aabb( sl_box *result, const sl_entity *q )
{
	v2 min_p1, max_p1;
	v2 min_p2, max_p2, offset;
	m22 m2;

	min_p1 = vec2( -1.0f, -1.0f );
	max_p1 = vec2( 1.0f,  1.0f );
	m2 = mtruncate42( &q->world_matrix );

	min_p2 = vmulm2( &m2, min_p1 );
	max_p2 = vmulm2( &m2, max_p1 );

	offset.x = q->world_matrix.A[ 12 ];
	offset.y = q->world_matrix.A[ 13 ];

	min_p1 = vadd2( min_p2, offset );
	max_p1 = vadd2( max_p2, offset );

	result->min_p = vmin2( min_p1, max_p1 );
	result->max_p = vmax2( min_p1, max_p1 );
}
Пример #4
0
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);
}
Пример #5
0
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;
}