四元数

Quaternion.java

public class Quaternion implements Comparable<Quaternion>{
	private final double re, i, j, k;
	public static final Quaternion ZERO = new Quaternion(0, 0, 0, 0);
	public static final Quaternion ONE  = new Quaternion(1, 0, 0, 0);
	public static final Quaternion I    = new Quaternion(0, 1, 0, 0);
	
	private Quaternion(double re, double i, double j, double k){
		this.re = re;
		this.i = i;
		this.j = j;
		this.k = k;
	}
	
	public static Quaternion valueOf(double re, double i, double j, double k){
		return new Quaternion(re, i, j, k);
	}
	
	public double re(){ return re;}
	public double i() { return i; }
	public double j() { return j; }
	public double k() { return k; }
	
	
	public Quaternion add(Quaternion q){
		return new Quaternion(re + q.re(),
          							  i  + q.i(),
          							  j  + q.i(), 
          							  k  + q.k());
	}
	
	public Quaternion sub(Quaternion q){
		return new Quaternion(re - q.re(),
          							  i  - q.i(),
          							  j  - q.i(), 
          							  k  - q.k());
	}
	
	public Quaternion div(double m){
		return new Quaternion(re/m, i/m, j/m, k/m);
	}
	
	public Quaternion mult(double m){
		return new Quaternion(re * m, i * m, j * m, k * m);
	}
	
	public Quaternion mult(Quaternion q){
		return new Quaternion(re * q.re() - i * q.i()  - j * q.i()  - k * q.k() ,
    				              re * q.i()  + i * q.re() + j * q.k()  - k * q.j() ,
    				              re * q.j()  - i * q.k()  + j * q.re() + k * q.i() ,
    				              re * q.k()  + i * q.j()  - j * q.i()  + k * q.re());
	}
	
	public double vectorDot(Quaternion q){
		return i * q.i + j * q.j + k * q.k;
	}
	
	public Quaternion vectorCross(Quaternion q){
		return new Quaternion(0,
    				              j*q.k - k*q.j,
    				              k*q.i - i*q.k,
    				              i*q.j - j*q.i);
	}
	
	public Quaternion unit(){
		double norm = norm();
		if(norm == 0) return ZERO;
		return this.div(norm);
	}
	
	public String toString(){
		String iSign = " + ";
		String jSign = " + ";
		String kSign = " + ";
		if(i < 0) iSign = " - ";
		if(j < 0) jSign = " - ";
		if(k < 0) kSign = " - ";
		return re + iSign + i +"i" + jSign + j + "j"+ kSign + k +"k";
	}
	
	public Quaternion conjunction(){
		return new Quaternion(re, -i, -j, -k);
	}
	
	public double norm(){
		return Math.sqrt(re*re + i*i + j*j + k*k);
	}
	
	public Quaternion inverse(){
		return conjunction().div(Math.pow(norm(), 2));
	}
	
	public boolean isZero(){
		return re == 0 && i == 0 && j == 0 && k ==0;
	}
	
	@Override
	public boolean equals(Object o){
		if(!(o instanceof Quaternion)) 
			return false;
		Quaternion q = (Quaternion) o;
		return re == q.re && i == q.i && j == q.j && k == q.k;
	}
	
	@Override
	public int hashCode(){
		int result = 17;
		long l = Double.doubleToLongBits(re);
		result = 31 * result + (int)Math.pow(l, l >>> 32);
		l = Double.doubleToLongBits(i);
		result = 31 * result + (int)Math.pow(l, l >>> 32);
		l = Double.doubleToLongBits(j);
		result = 31 * result + (int)Math.pow(l, l >>> 32);
		l = Double.doubleToLongBits(k);
		result = 31 * result + (int)Math.pow(l, l >>> 32);
		return result;
	}
	
	public int compareTo(Quaternion q){
		double normDiff = norm() - q.norm();
		if(normDiff != 0)
			return (int) normDiff;
		return 0;
	}

}