defaultmotionmodel.java

一个基于PlaceLab的室内和室外的智能导航系统

package org.placelab.particlefilter.beacon;

import org.placelab.particlefilter.MotionModel;
import org.placelab.particlefilter.Particle;
import org.placelab.util.NumUtil;


public class DefaultMotionModel implements MotionModel {
	

	public void move(Particle p, long elapsedMillis) {
		PositionWithMotionParticle particle = (PositionWithMotionParticle) p;
		double twoPI=2.0*Math.PI;
		
		if (elapsedMillis <= 0) {
			return;
		}
		double accel=newAccel(elapsedMillis);
		particle.setVelocity(particle.getVelocity() + accel);
		if (particle.getVelocity() < 0) {
			particle.setVelocity(-1*particle.getVelocity());
		}
		
		double turnInRadians=newTurn(elapsedMillis);
		double direction = particle.getDirection() + turnInRadians;
		if (direction < 0.0)  {
			direction += twoPI;
		}
		if (direction >= twoPI) {
			direction -= twoPI;
		}
		particle.setDirection(direction);
		
//		System.out.println("elapsed is " + elapsedMillis);

		particle.moveBy(Math.cos(particle.getDirection()) * particle.getVelocity() * (elapsedMillis/1000.0),
				Math.sin(particle.getDirection()) * particle.getVelocity() * (elapsedMillis/1000.0));
	}
	
	private double newAccel(long elapsedMillis) {
		long foo = elapsedMillis % 1000;
		double d=NumUtil.rand.nextDouble()*10;
		d-=5.0;
		return d;
	}
	
	private double newTurn(long elapsedMillis) {
		long foo = elapsedMillis % 1000;
		double d=NumUtil.rand.nextDouble()*2.0*Math.PI;
		if (d>Math.PI) {
			return 0.0;
		}
		double piOverTwo=Math.PI/2.0;
		d-=piOverTwo;
		return d;
	}



}