/**
 * A general purpose 3D Vector class.  Based on Dan Shiffman's Vector3D
 */

public class Vector3D {
	private float x;
	private float y;
	private float z;
	
	Vector3D(float x_, float y_, float z_) {
		x = x_; 
		y = y_; 
		z = z_;
	}
	
	Vector3D(float x_, float y_) {
		x = x_; 
		y = y_; 
		z = 0f;
	}
	
	Vector3D() {
		x = 0f; 
		y = 0f; 
		z = 0f;
	}
	
	float x() {
		return x;
	}
	
	float y() {
		return y;
	}
	
	float z() {
		return z;
	}
	
	void setX(float x_) {
		x = x_;
	}
	
	void setY(float y_) {
		y = y_;
	}
	
	void setZ(float z_) {
		z = z_;
	}
	
	void setXYZ(float x_, float y_, float z_) {
		x = x_;
		y = y_;
		z = z_;
	}
	
	void setXYZ(Vector3D v) {
		x = v.x();
		y = v.y();
		z = v.z();
	}
	public float magnitude() {
		return (float) Math.sqrt(x*x + y*y + z*z);
	}
	
	public Vector3D copy() {
		return new Vector3D(x,y,z);
	}
	
	public static Vector3D copy(Vector3D v) {
		return new Vector3D(v.x(), v.y(),v.z());
	}
	
	public Vector3D add(Vector3D v) {
		x += v.x;
		y += v.y;
		z += v.z;
		return this;
	}
	
	public Vector3D sub(Vector3D v) {
		x -= v.x;
		y -= v.y;
		z -= v.z;
		return this;
	}
	
	public Vector3D mult(float n) {
		x *= n;
		y *= n;
		z *= n;
		return this;
	}
	
	public Vector3D div(float n) { 
		x /= n;
		y /= n;
		z /= n;
		return this;
	}
	
	/**
	 * make the vector length equal 1
	 */
	public Vector3D normalize() {
		float m = magnitude();
		if (m > 0) {
			div(m);
		}
		return this;
	}
	
	public void limit(float max) {
		if (magnitude() > max) {
			normalize();
			mult(max);
		}
	}
	
	public float heading2D() {
		float angle = (float) Math.atan2(-y, x);
		return -1*angle;
	}
	
	public static Vector3D add(Vector3D a, Vector3D b) {
		return new Vector3D(a.x()+b.x(), a.y()+b.y(), a.z()+b.z());
	}
	
	public static Vector3D sub(Vector3D a, Vector3D b) {
		return new Vector3D(a.x()-b.x(), a.y()-b.y(), a.z()-b.z());
	}
	
	public static Vector3D div(Vector3D a, float n) {
		return new Vector3D(a.x()/n, a.y()/n, a.z()/n);
	}
	
	public static Vector3D mult(Vector3D a, float n) {
		return new Vector3D(a.x()*n, a.y()*n, a.z()*n);
	}
	
	public static float distance (Vector3D v1, Vector3D v2) {
		float dx = v1.x() - v2.x();
		float dy = v1.y() - v2.y();
		float dz = v1.z() - v2.z();
		return (float) Math.sqrt(dx*dx + dy*dy + dz*dz);
	}

	public static Vector3D avg(Vector3D v1, Vector3D v2, Vector3D v3) {
		Vector3D center = new Vector3D();
		center.add(v1).add(v2).add(v3);
		return center.div(3);
	}
	//==========================================================

    // returns the normal vector of the plane defined by the three points
    public static Vector3D getNormal(Vector3D a, Vector3D b, Vector3D c)
    {
    	Vector3D BminusA = sub(b,a);
    	Vector3D CminusA = sub(c,a);
        return crossProduct(BminusA,CminusA).normalize();
    }

    // returns a x b
    public static Vector3D crossProduct(Vector3D a, Vector3D b)
    {
        return new Vector3D(a.y*b.z - a.z*b.y,  a.z*b.x - a.x*b.z,  a.x*b.y - a.y*b.x);
    }

	public String toString()
	{
		return "<x=" + x + ",y=" + y + ",z=" + z +">";
	}
}