package rlbot.vector;

import rlbot.api.GameData;

public class Vector3 {

public final double x;

public final double y;

public final double z;

public Vector3(double x, double y, double z) {

this.x = x;

this.y = y;

this.z = z;

}

public static Vector3 fromProto(GameData.Vector3 vec) {

// Invert the X value so that the axes make more sense.

return new Vector3(-vec.getX(), vec.getY(), vec.getZ());

}

public Vector3() {

this(0, 0, 0);

}

public Vector3 plus(Vector3 other) {

return new Vector3(x + other.x, y + other.y, z + other.z);

}

public Vector3 minus(Vector3 other) {

return new Vector3(x - other.x, y - other.y, z - other.z);

}

public Vector3 scaled(double scale) {

return new Vector3(x * scale, y * scale, z * scale);

}

/**

* If magnitude is negative, we will return a vector facing the opposite direction.

*/

public Vector3 scaledToMagnitude(double magnitude) {

if (isZero()) {

throw new IllegalStateException("Cannot scale up a vector with length zero!");

}

double scaleRequired = magnitude / magnitude();

return scaled(scaleRequired);

}

public double distance(Vector3 other) {

double xDiff = x - other.x;

double yDiff = y - other.y;

double zDiff = z - other.z;

return Math.sqrt(xDiff * xDiff + yDiff * yDiff + zDiff * zDiff);

}

public double magnitude() {

return Math.sqrt(magnitudeSquared());

}

public double magnitudeSquared() {

return x * x + y * y + z * z;

}

public Vector3 normalized() {

if (isZero()) {

throw new IllegalStateException("Cannot normalize a vector with length zero!");

}

return this.scaled(1 / magnitude());

}

public double dotProduct(Vector3 other) {

return x * other.x + y * other.y + z * other.z;

}

public boolean isZero() {

return x == 0 && y == 0 && z == 0;

}

public Vector2 flatten() {

return new Vector2(x, y);

}

public double angle(Vector3 v) {

double mag2 = magnitudeSquared();

double vmag2 = v.magnitudeSquared();

double dot = dotProduct(v);

return Math.acos(dot / Math.sqrt(mag2 * vmag2));

}

public Vector3 crossProduct(Vector3 v) {

double tx = y * v.z - z * v.y;

double ty = z * v.x - x * v.z;

double tz = x * v.y - y * v.x;

return new Vector3(tx, ty, tz);

}

}