Cynopolis/SLAM-Sim
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

Added angleDiff() function to vector to get the difference in angle from one vector to another.

Browse Source
This commit is contained in:
Quinn
2023-04-06 10:54:53 -05:00
parent 7438d0f64e
commit a06415f371
2 changed files with 19 additions and 0 deletions
Download Patch File Download Diff File Expand all files Collapse all files

6
src/Vector.java
View File

@@ -1,3 +1,4 @@
import static java.lang.Math.acos;
import static java.lang.Math.sqrt;
public class Vector {
@@ -65,4 +66,9 @@ public class Vector {
float mag = this.mag();
return new Vector(x / mag, y / mag, z / mag);
}
float angleDiff(Vector other){
float dot = this.dot(other);
return (float)acos(dot / (this.mag() * other.mag()));
}
}

13
tests/VectorTest.java
View File

@@ -81,4 +81,17 @@ class VectorTest{
assertFloatEquals((float)-216.2, cross.z, (float)0.1);
}
@Test
public void testAngleDiff(){
Vector v1 = new Vector(1, 0);
Vector v2 = new Vector(0, 1);
Vector v3 = new Vector(-1, 0);
assertFloatEquals(v1.angleDiff(v1), 0);
assertFloatEquals(v1.angleDiff(v2), (float) toRadians(90));
assertFloatEquals(v1.angleDiff(v3), (float) toRadians(180));
assertFloatEquals(v1.angleDiff(v1.mul(100)), 0);
assertFloatEquals(v1.angleDiff(v2.mul(100)), (float) toRadians(90));
}
}