diff --git a/src/main/java/rlbotexample/input/ModernBallPrediction.java b/src/main/java/rlbotexample/input/ModernBallPrediction.java new file mode 100644 index 0000000..460de45 --- /dev/null +++ b/src/main/java/rlbotexample/input/ModernBallPrediction.java @@ -0,0 +1,218 @@ +package rlbotexample.input; + +import rlbot.cppinterop.RLBotDll; +import rlbot.cppinterop.RLBotInterfaceException; +import rlbot.flat.BallPrediction; +import rlbot.flat.Physics; +import rlbot.flat.PredictionSlice; +import rlbotexample.input.ball.BallData; + +import java.util.ArrayList; +import java.util.Collections; +import java.util.List; +import java.util.Optional; +import java.util.stream.Collectors; + +public class ModernBallPrediction { + + public final int tickRate; // close to 60 or 120 + public final float tickFrequency; // close to 1/60 or 1/120 + public final List frames; + + private ModernBallPrediction(List frames, float tickFrequency) { + if (frames == null) + frames = new ArrayList<>(); + float latencyCompensation = RLConstants.gameLatencyCompensation; + if (latencyCompensation > 0) { + List newFrames = new ArrayList<>(frames.size()); + for (ModernPredictionFrame frame : frames) { + if (frame.relativeTime < latencyCompensation) + continue; + + frame.adjustForLatencyCompensation(latencyCompensation); + newFrames.add(frame); + } + frames = newFrames; + } + frames = Collections.unmodifiableList(frames); + + this.frames = frames; + this.tickFrequency = tickFrequency; + this.tickRate = Math.round(1 / tickFrequency); + } + + public static ModernBallPrediction from(List frames, float tickFrequency) { + return new ModernBallPrediction(frames, tickFrequency); + } + + public static ModernBallPrediction empty() { + return new ModernBallPrediction(null, RLConstants.tickFrequency); + } + + private static ModernBallPrediction from(BallPrediction ballPrediction) { + assert ballPrediction.slicesLength() > 0 : "RLBot Ball Prediction has no frames"; + + List frames = new ArrayList<>(ballPrediction.slicesLength()); + + float startTime = ballPrediction.slices(0).gameSeconds(); + float lastTime = startTime; + float averageDt = 0; + + for (int i = 0; i < ballPrediction.slicesLength(); i++) { + PredictionSlice slice = ballPrediction.slices(i); + frames.add(new ModernPredictionFrame(slice.gameSeconds() - startTime, slice)); + + averageDt += slice.gameSeconds() - lastTime; + lastTime = slice.gameSeconds(); + } + + averageDt /= ballPrediction.slicesLength(); + + return new ModernBallPrediction(frames, averageDt); + } + + public static ModernBallPrediction get() { + return get(BallPredictionType.RLBOT); + } + + public static ModernBallPrediction get(BallPredictionType ballPredictionType) { + switch (ballPredictionType) { + case RLBOT: + try { + return ModernBallPrediction.from(RLBotDll.getBallPrediction()); + } catch (RLBotInterfaceException e) { + System.err.println("Could not get RLBot ball Prediction!"); + throw new RuntimeException(e); + } + } + + throw new IllegalStateException("Ball Prediction Type '" + ballPredictionType.name() + "' not recognized"); + } + + /*public void draw(AdvancedRenderer renderer, Color color, float length) { + if (this.frames.size() == 0) + return; + if (length <= 0) + length = this.relativeTimeOfLastFrame(); + + float time = 0; + float lastAbsTime = this.frames.get(0).absoluteTime; + Vector3 lastPos = this.frames.get(0).ballData.position; + while (time < length) { + Optional frame = this.getFrameAfterRelativeTime(time); + if (!frame.isPresent()) + break; + + if (Math.floor(lastAbsTime) < Math.floor(frame.get().absoluteTime)) { + renderer.drawLine3d(color.brighter(), frame.get().ballData.position, frame.get().ballData.position.add(0, 0, 50)); + } + lastAbsTime = frame.get().absoluteTime; + time = frame.get().relativeTime; + ImmutableBallData ball = frame.get().ballData; + + if (ball.makeMutable().isInAnyGoal()) { + renderer.drawCentered3dCube(color.brighter().brighter(), ball.position, 50); + renderer.drawString3d(String.format("Goal! (%.1f)", time), Color.WHITE, ball.position.add(0, 0, 150), 1, 1); + renderer.drawLine3d(color, lastPos, ball.position); + break; + } + + if (lastPos.distance(ball.position) < 50) + continue; + renderer.drawLine3d(color, lastPos, ball.position); + lastPos = ball.position; + } + }*/ + + public float relativeTimeOfLastFrame() { + if (this.frames.size() == 0) + return -1; + return this.frames.get(this.frames.size() - 1).relativeTime; + } + + public ModernBallPrediction trim(float relativeStartTime, float relativeEndTime) { + if (relativeEndTime < relativeStartTime) + throw new IllegalArgumentException("Relative end time smaller than relative start time"); + if (relativeEndTime - relativeStartTime == 0) + return ModernBallPrediction.empty(); + + return new ModernBallPrediction(this.getFramesBetweenRelative(relativeStartTime, relativeEndTime), this.tickFrequency); + } + + public Optional getFrameAtRelativeTime(float relativeTime) { + return this.frames + .stream() + .filter((f) -> f.relativeTime >= relativeTime) + .findFirst(); + } + + public Optional getFrameAfterRelativeTime(float relativeTime) { + return this.frames + .stream() + .filter((f) -> f.relativeTime > relativeTime) + .findFirst(); + } + + public Optional getFrameAtAbsoluteTime(float absolute) { + return this.frames + .stream() + .filter((f) -> f.absoluteTime >= absolute) + .findFirst(); + } + + public List getFramesBeforeRelative(float relativeTime) { + return this.frames + .stream() + .filter((f) -> f.relativeTime < relativeTime) + .collect(Collectors.toList()); + } + + public ModernBallPrediction getBeforeRelative(float relativeTime) { + return new ModernBallPrediction(getFramesBeforeRelative(relativeTime), this.tickFrequency); + } + + public List getFramesAfterRelative(float relativeTime) { + return this.frames + .stream() + .filter((f) -> f.relativeTime > relativeTime) + .collect(Collectors.toList()); + } + + public List getFramesBetweenRelative(float start, float end) { + return this.frames + .stream() + .filter((f) -> f.relativeTime > start && f.relativeTime < end) + .collect(Collectors.toList()); + } + + public boolean hasFrames() { + return this.frames.size() > 0; + } + + enum BallPredictionType { + RLBOT + } + + public static class ModernPredictionFrame { + public final float absoluteTime; + public final BallData ballData; + public float relativeTime; + + public ModernPredictionFrame(float absoluteTime, float relativeTime, BallData ballData) { + this.absoluteTime = absoluteTime; + this.relativeTime = relativeTime; + this.ballData = ballData; + } + + public ModernPredictionFrame(float relativeTime, PredictionSlice predictionSlice) { + this.relativeTime = relativeTime; + this.absoluteTime = predictionSlice.gameSeconds(); + Physics physics = predictionSlice.physics(); + this.ballData = new BallData(physics); + } + + public void adjustForLatencyCompensation(float offset) { + relativeTime -= offset; + } + } +} diff --git a/src/main/java/rlbotexample/input/RLConstants.java b/src/main/java/rlbotexample/input/RLConstants.java new file mode 100644 index 0000000..7a5a162 --- /dev/null +++ b/src/main/java/rlbotexample/input/RLConstants.java @@ -0,0 +1,43 @@ +package rlbotexample.input; + +import rlbotexample.vector.Vector2; +import rlbotexample.vector.Vector3; + +public class RLConstants { + + public static final float carElevation = 17.01f; + + public static final float goalDistance = 5120f; // Distance from center to goal + public static final float goalCenterToPost = 892.755f; + public static final float goalHeight = 642.775f; + + public static final float arenaHeight = 2044; + public static final float arenaWidth = 8192; + public static final float arenaHalfWidth = arenaWidth / 2; + public static final float arenaLength = 10240; + public static final float arenaHalfLength = arenaLength / 2; + + public static Vector3 gravity = new Vector3(0, 0, -650); + + public static float gameLatencyCompensation = 0.f; + + public static int tickRate = 120; + public static float tickFrequency = 1f / tickRate; + + public static int simulationTickRate = 60; + public static float simulationTickFrequency = 1f / simulationTickRate; + + public static boolean isPosNearWall(Vector2 pos, float tolerance) { + float x = (float) Math.abs(pos.x) + tolerance; + float y = (float) Math.abs(pos.y); + + if (x >= 2700 - tolerance && x <= 3850 + tolerance) { + // https://www.wolframalpha.com/input/?i=Line+between+%283850%2C+3850%29+%282700%2C+4950%29 + float val = 173250f / 23f - (22f * x) / 23f; + if (val <= y + tolerance) + return true; + } + + return x >= 3820 || y + tolerance >= 4920; + } +} diff --git a/src/main/java/rlbotexample/input/ball/BallData.java b/src/main/java/rlbotexample/input/ball/BallData.java index 8dffab4..26a58b0 100644 --- a/src/main/java/rlbotexample/input/ball/BallData.java +++ b/src/main/java/rlbotexample/input/ball/BallData.java @@ -2,6 +2,7 @@ import rlbot.flat.BallInfo; +import rlbot.flat.Physics; import rlbotexample.vector.Vector3; /** @@ -24,4 +25,12 @@ public BallData(final BallInfo ball) { this.hasBeenTouched = ball.latestTouch() != null; this.latestTouch = this.hasBeenTouched ? new BallTouch(ball.latestTouch()) : null; } + + public BallData(final Physics ballPhysics) { + this.position = new Vector3(ballPhysics.location()); + this.velocity = new Vector3(ballPhysics.velocity()); + this.spin = new Vector3(ballPhysics.angularVelocity()); + this.hasBeenTouched = false; + this.latestTouch = null; + } } diff --git a/src/main/java/rlbotexample/input/car/CarData.java b/src/main/java/rlbotexample/input/car/CarData.java index 1023e73..b613dc1 100644 --- a/src/main/java/rlbotexample/input/car/CarData.java +++ b/src/main/java/rlbotexample/input/car/CarData.java @@ -1,6 +1,8 @@ package rlbotexample.input.car; +import rlbot.flat.Rotator; +import rlbotexample.vector.Matrix3x3; import rlbotexample.vector.Vector3; /** @@ -20,6 +22,8 @@ public class CarData { /** The orientation of the car */ public final CarOrientation orientation; + public final Matrix3x3 matOrientation; + /** Boost ranges from 0 to 100 */ public final double boost; @@ -52,5 +56,7 @@ public CarData(rlbot.flat.PlayerInfo playerInfo, float elapsedSeconds) { this.team = playerInfo.team(); this.hasWheelContact = playerInfo.hasWheelContact(); this.elapsedSeconds = elapsedSeconds; + Rotator r = playerInfo.physics().rotation(); + this.matOrientation = Matrix3x3.eulerToRotation(new Vector3(r.pitch(), r.yaw(), r.roll())); } } diff --git a/src/main/java/rlbotexample/util/Tuple.java b/src/main/java/rlbotexample/util/Tuple.java new file mode 100644 index 0000000..85960c6 --- /dev/null +++ b/src/main/java/rlbotexample/util/Tuple.java @@ -0,0 +1,45 @@ +package rlbotexample.util; + +import java.io.Serializable; +import java.util.Objects; + +public class Tuple implements Serializable { + + private K key; + private V value; + + public Tuple(K key, V value) { + this.key = key; + this.value = value; + } + + public K getKey() { + return key; + } + + public V getValue() { + return value; + } + + @Override + public String toString() { + return key + "=" + value; + } + + @Override + public int hashCode() { + return key.hashCode() * 13 + (value == null ? 0 : value.hashCode()); + } + + @Override + public boolean equals(Object o) { + if (this == o) return true; + if (o instanceof Tuple) { + Tuple pair = (Tuple) o; + if (!Objects.equals(key, pair.key)) return false; + if (!Objects.equals(value, pair.value)) return false; + return true; + } + return false; + } +} \ No newline at end of file diff --git a/src/main/java/rlbotexample/vector/Matrix3x3.java b/src/main/java/rlbotexample/vector/Matrix3x3.java new file mode 100644 index 0000000..8e7ede2 --- /dev/null +++ b/src/main/java/rlbotexample/vector/Matrix3x3.java @@ -0,0 +1,358 @@ +package rlbotexample.vector; + +import java.util.Arrays; + +public class Matrix3x3 { + private final float[] data = new float[9]; + + public Matrix3x3() { + Arrays.fill(data, 0); + } + + public Matrix3x3(Matrix3x3 other) { + System.arraycopy(other.data, 0, this.data, 0, data.length); + } + + + public static Matrix3x3 identity() { + Matrix3x3 mat = new Matrix3x3(); + mat.assign(0, 0, 1); + mat.assign(1, 1, 1); + mat.assign(2, 2, 1); + return mat; + } + + + public static Matrix3x3 antiSym( Vector3 w) { + // http://mathworld.wolfram.com/AntisymmetricMatrix.html + + Matrix3x3 mat = new Matrix3x3(); + mat.assign(0, 1, -w.z); + mat.assign(1, 0, w.z); + + mat.assign(2, 0, -w.y); + mat.assign(0, 2, w.y); + + mat.assign(1, 2, -w.x); + mat.assign(2, 1, w.x); + return mat; + } + + + public static Matrix3x3 R3_basis(Vector3 n) { + float sign = n.z >= 0f ? 1f : -1f; + float a = -1f / (sign + n.z); + float b = n.x * n.y * a; + + Matrix3x3 mat = new Matrix3x3(); + + mat.assign(0, 0, 1f + sign * n.x * n.x * a); + mat.assign(0, 1, b); + mat.assign(0, 2, n.x); + + mat.assign(1, 0, sign * b); + mat.assign(1, 1, sign + n.y * n.y * a); + mat.assign(1, 2, n.y); + + mat.assign(2, 0, -sign * n.x); + mat.assign(2, 1, -n.y); + mat.assign(2, 2, n.z); + + return mat; + } + + + public static Matrix3x3 axisToRotation(Vector3 omega) { + float norm_omega = (float) omega.magnitude(); + + if (Math.abs(norm_omega) == 0) + norm_omega = 1.1755e-38f; + { + Vector3 u = omega.div(norm_omega); + + float c = (float) Math.cos(norm_omega); + float s = (float) Math.sin(norm_omega); + + Matrix3x3 mat = new Matrix3x3(); + + mat.assign(0, 0, u.get(0) * u.get(0) * (1.0f - c) + c); + mat.assign(0, 1, u.get(0) * u.get(1) * (1.0f - c) - u.get(2) * s); + mat.assign(0, 2, u.get(0) * u.get(2) * (1.0f - c) + u.get(1) * s); + + mat.assign(1, 0, u.get(1) * u.get(0) * (1.0f - c) + u.get(2) * s); + mat.assign(1, 1, u.get(1) * u.get(1) * (1.0f - c) + c); + mat.assign(1, 2, u.get(1) * u.get(2) * (1.0f - c) - u.get(0) * s); + + mat.assign(2, 0, u.get(2) * u.get(0) * (1.0f - c) - u.get(1) * s); + mat.assign(2, 1, u.get(2) * u.get(1) * (1.0f - c) + u.get(0) * s); + mat.assign(2, 2, u.get(2) * u.get(2) * (1.0f - c) + c); + + return mat; + } + } + + + public static Matrix3x3 eulerToRotation(Vector3 pyr) { + Matrix3x3 mat = new Matrix3x3(); + float CP = (float) Math.cos(pyr.x); + float SP = (float) Math.sin(pyr.x); + float CY = (float) Math.cos(pyr.y); + float SY = (float) Math.sin(pyr.y); + float CR = (float) Math.cos(pyr.z); + float SR = (float) Math.sin(pyr.z); + + mat.assign(0, 0, CP * CY); + mat.assign(0, 0, CP * CY); + mat.assign(1, 0, CP * SY); + mat.assign(2, 0, SP); + + mat.assign(0, 1, CY * SP * SR - CR * SY); + mat.assign(1, 1, SY * SP * SR + CR * CY); + mat.assign(2, 1, -CP * SR); + + mat.assign(0, 2, -CR * CY * SP - SR * SY); + mat.assign(1, 2, -CR * SY * SP + SR * CY); + mat.assign(2, 2, CP * CR); + + return mat; + } + + + public static Matrix3x3 from( Vector3 forward, Vector3 up, Vector3 left) { + Matrix3x3 mat = new Matrix3x3(); + + mat.assign(0, 0, forward.x); + mat.assign(1, 0, forward.y); + mat.assign(2, 0, forward.z); + + mat.assign(0, 1, left.x); + mat.assign(1, 1, left.y); + mat.assign(2, 1, left.z); + + mat.assign(0, 2, up.x); + mat.assign(1, 2, up.y); + mat.assign(2, 2, up.z); + + return mat; + } + + + public static Matrix3x3 lookAt(Vector3 direction, Vector3 up) { + if (up == null) + up = new Vector3(0, 0, 1); + + Vector3 f = direction.normalized(); + Vector3 u = f.crossProduct(up.crossProduct(f)).normalized(); + Vector3 l = u.crossProduct(f).normalized(); + + Matrix3x3 mat = new Matrix3x3(); + mat.assign(0, 0, f.x); + mat.assign(0, 1, l.x); + mat.assign(0, 2, u.x); + + mat.assign(1, 0, f.y); + mat.assign(1, 1, l.y); + mat.assign(1, 2, u.y); + + mat.assign(2, 0, f.z); + mat.assign(2, 1, l.z); + mat.assign(2, 2, u.z); + return mat; + } + + + public static Matrix3x3 roofTo(Vector3 up, Vector3 generalDirection) { + Vector3 f = new Vector3(); + + if (generalDirection != null) { + // https://stackoverflow.com/a/9605695 + + double dist = generalDirection.dot(up); + Vector3 projected = generalDirection.sub(up.mul(dist)); + f = projected.normalized(); + } + + if (f.isZero()) + f = new Vector3(0, 0, -1); + Vector3 u = f.crossProduct(up.crossProduct(f)).normalized(); + Vector3 l = u.crossProduct(f).normalized(); + + Matrix3x3 mat = new Matrix3x3(); + mat.assign(0, 0, f.x); + mat.assign(0, 1, l.x); + mat.assign(0, 2, u.x); + + mat.assign(1, 0, f.y); + mat.assign(1, 1, l.y); + mat.assign(1, 2, u.y); + + mat.assign(2, 0, f.z); + mat.assign(2, 1, l.z); + mat.assign(2, 2, u.z); + return mat; + } + + public float[][] getFloatMatrix() { + return new float[][]{ + {get(0, 0), get(0, 1), get(0, 2)}, + {get(1, 0), get(1, 1), get(1, 2)}, + {get(2, 0), get(2, 1), get(2, 2)} + }; + } + + public Vector3 toEuler() { + return new Vector3( + (float) Math.atan2(this.get(2, 0), new Vector2(this.get(0, 0), this.get(1, 0)).magnitude()), + (float) Math.atan2(this.get(1, 0), this.get(0, 0)), + (float) Math.atan2(-this.get(2, 1), this.get(2, 2)) + ); + } + + public Vector3 up() { + return new Vector3(this.get(0, 2), this.get(1, 2), this.get(2, 2)); + } + + public Vector3 forward() { + return new Vector3(this.get(0, 0), this.get(1, 0), this.get(2, 0)); + } + + public Vector3 right() { + return new Vector3(this.get(0, 1), this.get(1, 1), this.get(2, 1)); + } + + public void assign(int row, int column, float value) { + this.data[row + column * 3] = value; + } + + public float get(int row, int column) { + return this.data[row + column * 3]; + } + + public float det() { + return + +this.get(0, 0) * this.get(1, 1) * this.get(2, 2) + + this.get(0, 1) * this.get(1, 2) * this.get(2, 0) + + this.get(0, 2) * this.get(1, 0) * this.get(2, 1) + - this.get(0, 0) * this.get(1, 2) * this.get(2, 1) + - this.get(0, 1) * this.get(1, 0) * this.get(2, 2) + - this.get(0, 2) * this.get(1, 1) * this.get(2, 0); + } + + public float tr() { + float sum = 0; + for (int i = 0; i < 3; i++) + sum += get(i, i); + return sum; + } + + public Matrix3x3 invert() { + Matrix3x3 invA = new Matrix3x3(); + + float inv_detA = 1.0f / this.det(); + + invA.assign(0, 0, (this.get(1, 1) * this.get(2, 2) - this.get(1, 2) * this.get(2, 1)) * inv_detA); + invA.assign(0, 1, (this.get(0, 2) * this.get(2, 1) - this.get(0, 1) * this.get(2, 2)) * inv_detA); + invA.assign(0, 2, (this.get(0, 1) * this.get(1, 2) - this.get(0, 2) * this.get(1, 1)) * inv_detA); + invA.assign(1, 0, (this.get(1, 2) * this.get(2, 0) - this.get(1, 0) * this.get(2, 2)) * inv_detA); + invA.assign(1, 1, (this.get(0, 0) * this.get(2, 2) - this.get(0, 2) * this.get(2, 0)) * inv_detA); + invA.assign(1, 2, (this.get(0, 2) * this.get(1, 0) - this.get(0, 0) * this.get(1, 2)) * inv_detA); + invA.assign(2, 0, (this.get(1, 0) * this.get(2, 1) - this.get(1, 1) * this.get(2, 0)) * inv_detA); + invA.assign(2, 1, (this.get(0, 1) * this.get(2, 0) - this.get(0, 0) * this.get(2, 1)) * inv_detA); + invA.assign(2, 2, (this.get(0, 0) * this.get(1, 1) - this.get(0, 1) * this.get(1, 0)) * inv_detA); + + return invA; + } + + public Matrix3x3 div(float denominator) { + Matrix3x3 B = new Matrix3x3(); + for (int i = 0; i < 3; i++) { + for (int j = 0; j < 3; j++) { + B.assign(i, j, this.get(i, j) / denominator); + } + } + return B; + } + + public Matrix3x3 elementwiseMul(float denominator) { + Matrix3x3 B = new Matrix3x3(); + for (int i = 0; i < 3; i++) { + for (int j = 0; j < 3; j++) { + B.assign(i, j, this.get(i, j) * denominator); + } + } + return B; + } + + public Matrix3x3 add(Matrix3x3 other) { + Matrix3x3 B = new Matrix3x3(); + for (int i = 0; i < 3; i++) { + for (int j = 0; j < 3; j++) { + B.assign(i, j, this.get(i, j) + other.get(i, j)); + } + } + return B; + } + + public Matrix3x3 sub(Matrix3x3 other) { + Matrix3x3 B = new Matrix3x3(); + for (int i = 0; i < 3; i++) { + for (int j = 0; j < 3; j++) { + B.assign(i, j, this.get(i, j) - other.get(i, j)); + } + } + return B; + } + + public Matrix3x3 transpose() { + Matrix3x3 other = new Matrix3x3(); + + for (int r = 0; r < 3; r++) { + for (int c = 0; c < 3; c++) { + other.assign(r, c, get(c, r)); + } + } + + return other; + } + + public Vector3 dot(Vector3 other) { + float[] vecArr = new float[3]; + for (int i = 0; i < 3; i++) { + vecArr[i] = 0; + for (int j = 0; j < 3; j++) { + vecArr[i] += get(i, j) * other.get(j); + } + } + return new Vector3(vecArr[0], vecArr[1], vecArr[2]); + } + + public Matrix3x3 matrixMul(Matrix3x3 other) { + Matrix3x3 C = new Matrix3x3(); + + for (int i = 0; i < 3; i++) { + for (int j = 0; j < 3; j++) { + C.assign(i, j, 0.0f); + for (int k = 0; k < 3; k++) { + C.assign(i, j, (C.get(i, j) + this.get(i, k) * other.get(k, j))); + } + } + } + + return C; + } + + public float angle( Matrix3x3 other) { + return (float) Math.acos(0.5f * (this.matrixMul(other.transpose()).tr() - 1.0f)); + } + + @Override + public String toString() { + float[][] dat = getFloatMatrix(); + final StringBuilder sb = new StringBuilder("Matrix3x3[\n"); + sb.append(Arrays.toString(dat[0])).append(",\n"); + sb.append(Arrays.toString(dat[1])).append(",\n"); + sb.append(Arrays.toString(dat[2])).append("\n"); + sb.append(']'); + return sb.toString(); + } +} diff --git a/src/main/java/rlbotexample/vector/Vector2.java b/src/main/java/rlbotexample/vector/Vector2.java index dad1633..aad497b 100644 --- a/src/main/java/rlbotexample/vector/Vector2.java +++ b/src/main/java/rlbotexample/vector/Vector2.java @@ -8,12 +8,12 @@ */ public class Vector2 { - public final double x; - public final double y; + public final float x; + public final float y; public Vector2(double x, double y) { - this.x = x; - this.y = y; + this.x = (float) x; + this.y = (float) y; } public Vector2 plus(Vector2 other) { diff --git a/src/main/java/rlbotexample/vector/Vector3.java b/src/main/java/rlbotexample/vector/Vector3.java index 506ab21..2478ac3 100644 --- a/src/main/java/rlbotexample/vector/Vector3.java +++ b/src/main/java/rlbotexample/vector/Vector3.java @@ -40,6 +40,72 @@ public Vector3 scaled(double scale) { return new Vector3(x * scale, y * scale, z * scale); } + public double dot(Vector3 other) { + return x * other.x + y * other.y + z * other.z; + } + + public Vector3 add(Vector3 other) { + return new Vector3(x + other.x, y + other.y, z + other.z); + } + + public Vector3 add(float other) { + return new Vector3(x + other, y + other, z + other); + } + + public Vector3 add(Vector2 other, float z) { + return new Vector3(x + other.x, y + other.y, this.z + z); + } + + public Vector3 add(float x, float y, float z) { + return new Vector3(this.x + x, this.y + y, this.z + z); + } + + public Vector3 sub(Vector3 other) { + return new Vector3(x - other.x, y - other.y, z - other.z); + } + + public Vector3 sub(float xS, float yS, float zS) { + return new Vector3(x - xS, y - yS, z - zS); + } + + public Vector3 sub(float other) { + return new Vector3(x - other, y - other, z - other); + } + + public Vector3 mul(double scale) { + return new Vector3(x * scale, y * scale, z * scale); + } + + public Vector3 mul(Vector3 other) { + return new Vector3(x * other.x, y * other.y, z * other.z); + } + + public Vector3 mul(float xS, float yS, float zS) { + return new Vector3(x * xS, y * yS, z * zS); + } + + + public Vector3 dot(Matrix3x3 o) { + float[] vA = new float[3]; + for (int i = 0; i < 3; i++) { + vA[i] = 0; + for (int j = 0; j < 3; j++) { + vA[i] += this.get(j) * o.get(j, i); + } + } + return new Vector3(vA[0], vA[1], vA[2]); + } + + public float get(int index) { + if (index == 0) + return x; + if (index == 1) + return y; + if (index == 2) + return z; + return 0; + } + /** * If magnitude is negative, we will return a vector facing the opposite direction. */ @@ -51,6 +117,10 @@ public Vector3 scaledToMagnitude(double magnitude) { return scaled(scaleRequired); } + public Vector3 div(double scale) { + return new Vector3(x / scale, y / scale, z / scale); + } + public double distance(Vector3 other) { double xDiff = x - other.x; double yDiff = y - other.y; @@ -73,11 +143,6 @@ public Vector3 normalized() { } 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; } @@ -89,7 +154,7 @@ public Vector2 flatten() { public double angle(Vector3 v) { double mag2 = magnitudeSquared(); double vmag2 = v.magnitudeSquared(); - double dot = dotProduct(v); + double dot = dot(v); return Math.acos(dot / Math.sqrt(mag2 * vmag2)); }