MAC0318/HistogramFilter/src/DiscretePoint.java

import lejos.geom.Rectangle;
import lejos.robotics.navigation.Pose;

/*
 * Representa um ponto do quadrado da matriz.
 * Criado somente qunado necessário.
 */
public class DiscretePoint {
	ProbabilityMatriz m;
	int x, y, z;
	public DiscretePoint(ProbabilityMatriz m, int x, int y, int z) {
		this.m = m;
		this.x =x;
		this.y = y;
		this.z = z;
	}
	
	public void set (double v) {
		m.set(v, x, y, z);
	} 
	
	public double get () {
		return m.get(x, y, z);
	}
	
	public Rectangle bounds () {
		float a = (float) (m.bounds.getMinX()+x*m.ssize);
		float b = (float) (m.bounds.getMinY()+y*m.ssize);
		return new Rectangle(a, b, (float)m.ssize, (float)m.ssize);
	}
	
	/* menor angulo em graus do intervalo */
	public double minAng () {
		double ang = 360.0/m.alphad*z+270;
		return ang;
	}
	
	/* maior angulo em graus do intervalo */
	public double maxAng () {
		double ang = 360.0/m.alphad*(z+1)+270;
		return ang;
	}
	
	public double midAng () {
		return (maxAng()+minAng())/2.0;
	}
	
	public Pose pose() {
		return new Pose(
				(float) bounds().getCenterX(),
				(float) bounds().getCenterY(),
				(float) midAng()
		);
	}

}