MAC0318/Sonar/src/config/Models.java

package config;

import java.util.ArrayList;

import lejos.robotics.mapping.LineMap;
import lejos.robotics.navigation.Pose;

public class Models {
	private LineMap map;

	public Models(LineMap map) {
		this.map = map;
	}

	public double expectedSonarRead(Pose p, double angle) {
		/**************** MODELO DO SONAR *******************/
		Pose tmppose = new Pose(p.getX(), p.getY(), p.getHeading());
		float mindist = Float.POSITIVE_INFINITY;
		int cone = 0;	
		for (int angulo=-cone/2; angulo <= cone/2; angulo++) {
			tmppose.setHeading((float) (p.getHeading() - angulo + angle));
			float dist = map.range(tmppose);
			if (dist > 0 && dist < mindist) 
				mindist = dist;
		} 
		return mindist;
	}
	
	/* retorna a probabilidade do robo estar em (ap, aang) sendo que ele saiu de (bp, bang) 
	 * com um movimento de rotacao de ang graus.*/
	public double rotateProbability (Pose pa, double aang, Pose pb, double bang, double ang) {
		return 0;
	}
	
	/* retorna a probabilidade do robo estar em (ap, aang) sendo que ele saiu de (bp, bang) 
	 * com sendo que ele foi move centimetros para frente.*/
	public double moveProbability (Pose pa, double aang, Pose pb, double bang, double move) {
		return 0;
	}
	
	/* retorna a probabilidade dele estar em (p, ang) sendo que realizou a leitura do
	 *  conjunto x de medidas */
	public double readProbability (Pose p, double angle, ArrayList<Integer> x) {
		return 0;
	}
}