From 5bbc5b5076ee0137aae9158c1fd075dfc2572fd5 Mon Sep 17 00:00:00 2001 From: Nathan Townshend Date: Thu, 23 Oct 2025 16:53:27 +0800 Subject: [PATCH 1/8] Implements EntityStateStore's getUpdatedSince --- .../openlvc/disco/application/EntityStateStore.java | 10 ++++++++-- 1 file changed, 8 insertions(+), 2 deletions(-) diff --git a/codebase/src/java/disco/org/openlvc/disco/application/EntityStateStore.java b/codebase/src/java/disco/org/openlvc/disco/application/EntityStateStore.java index 6fc192f6..4dc4b599 100644 --- a/codebase/src/java/disco/org/openlvc/disco/application/EntityStateStore.java +++ b/codebase/src/java/disco/org/openlvc/disco/application/EntityStateStore.java @@ -127,6 +127,10 @@ public EntityStatePdu getEntityState( String marking ) return byMarking.get( marking ); } + /** + * @param id + * @return the last {@link EntityStatePdu} for the entity, or `null` if not stored + */ public EntityStatePdu getEntityState( EntityId id ) { return byId.get( id ); @@ -149,7 +153,9 @@ public Set getAllMarkings() */ public Set getEntityStatesUpdatedSince( long time ) { - return null; + return this.byId.values().parallelStream() + .filter( espdu -> espdu.getLocalTimestamp() >= time ) + .collect( Collectors.toSet() ); } //////////////////////////////////////////////////////////////////////////////////////////// @@ -159,7 +165,7 @@ public Set getEntityStatesUpdatedSince( long time ) * Get the set of Entity States whose location is within the radius of the specified entity's * location. If none are close, an empty set is returned. * - * @param location The entity we want to find other entities in proximity to + * @param entity The entity we want to find other entities in proximity to * @param radiusMeters Limit of how far a entity can be from the given entity * @return Set of all entities within the given radius of the given entity */ From dae75d81d758da6785e33ba64c1f9d2ffc23e3dd Mon Sep 17 00:00:00 2001 From: Nathan Townshend Date: Thu, 23 Oct 2025 16:52:55 +0800 Subject: [PATCH 2/8] Updates utils maths classes (Quaternion, Vec3) Adds a number of methods to the Quaternion and Vec3 utility classes. Notably adds support for Quaternion -> EulerAngles conversion, rotating Vec3s by Quaternions, and introduces tests for Quaternion functionality. --- .../openlvc/disco/pdu/record/EulerAngles.java | 19 +- .../openlvc/disco/utils/CoordinateUtils.java | 2 +- .../disco/utils/FloatingPointUtils.java | 18 ++ .../disco/org/openlvc/disco/utils/Mat3x3.java | 85 ++++++- .../org/openlvc/disco/utils/Quaternion.java | 165 ++++++++++-- .../disco/org/openlvc/disco/utils/Vec3.java | 115 ++++++++- .../openlvc/disco/utils/QuaternionTest.java | 235 ++++++++++++++++++ 7 files changed, 608 insertions(+), 31 deletions(-) create mode 100644 codebase/src/java/test/org/openlvc/disco/utils/QuaternionTest.java diff --git a/codebase/src/java/disco/org/openlvc/disco/pdu/record/EulerAngles.java b/codebase/src/java/disco/org/openlvc/disco/pdu/record/EulerAngles.java index 8a4c21c6..317e980e 100644 --- a/codebase/src/java/disco/org/openlvc/disco/pdu/record/EulerAngles.java +++ b/codebase/src/java/disco/org/openlvc/disco/pdu/record/EulerAngles.java @@ -35,7 +35,15 @@ public class EulerAngles implements IPduComponent, Cloneable //---------------------------------------------------------- // STATIC VARIABLES //---------------------------------------------------------- + public static float PSI_MIN = -(float)Math.PI; + public static float PSI_MAX = (float)Math.PI; + public static float THETA_MIN = -(float)(Math.PI / 2); + public static float THETA_MAX = (float)(Math.PI / 2); + + public static float PHI_MIN = -(float)Math.PI; + public static float PHI_MAX = (float)Math.PI; + //---------------------------------------------------------- // INSTANCE VARIABLES //---------------------------------------------------------- @@ -70,9 +78,10 @@ public boolean equals( Object other ) if( other instanceof EulerAngles ) { EulerAngles otherAngle = (EulerAngles)other; - if( FloatingPointUtils.floatEqual(otherAngle.psi,this.psi) && + // psi and phi are continuous (yaw, roll), but theta is discontinuous (pitch) + if( FloatingPointUtils.floatRadEqual(otherAngle.psi,this.psi) && FloatingPointUtils.floatEqual(otherAngle.theta,this.theta) && - FloatingPointUtils.floatEqual(otherAngle.phi,this.phi) ) + FloatingPointUtils.floatRadEqual(otherAngle.phi,this.phi) ) { return true; } @@ -87,6 +96,12 @@ public EulerAngles clone() return new EulerAngles( psi, theta, phi ); } + @Override + public String toString() + { + return "psi=%f, theta=%f, phi=%f".formatted( this.psi, this.theta, this.phi ); + } + //////////////////////////////////////////////////////////////////////////////////////////// /// IPduComponent Methods //////////////////////////////////////////////////////////////// //////////////////////////////////////////////////////////////////////////////////////////// diff --git a/codebase/src/java/disco/org/openlvc/disco/utils/CoordinateUtils.java b/codebase/src/java/disco/org/openlvc/disco/utils/CoordinateUtils.java index ffb590db..c0e0e552 100644 --- a/codebase/src/java/disco/org/openlvc/disco/utils/CoordinateUtils.java +++ b/codebase/src/java/disco/org/openlvc/disco/utils/CoordinateUtils.java @@ -119,7 +119,7 @@ public static Quaternion getNorthFacingQuat( WorldCoordinate ecef ) Vec3 orthogonal = northVec.cross( worldUpVec ).normalize(); // eastward/rightward vector Mat3x3 mat3x3 = new Mat3x3( orthogonal, northVec, worldUpVec ); - return mat3x3.toQuaterion(); + return mat3x3.toQuaternion(); } public static Vec3 getDirectionToNorth( Vec3 ecef ) diff --git a/codebase/src/java/disco/org/openlvc/disco/utils/FloatingPointUtils.java b/codebase/src/java/disco/org/openlvc/disco/utils/FloatingPointUtils.java index b5608e26..e225c0c4 100644 --- a/codebase/src/java/disco/org/openlvc/disco/utils/FloatingPointUtils.java +++ b/codebase/src/java/disco/org/openlvc/disco/utils/FloatingPointUtils.java @@ -73,5 +73,23 @@ public static boolean floatEqual( float f1, float f2 ) float absDiff = Math.abs( f1 - f2 ); return absDiff < FP_EQUALITY_THRESHOLD; } + + /** + * Returns whether the two specified floating precision radian values are equal. The two + * values will be considered equal if the absolute difference between the values is within a + * predefined threshold, accounting for wrapping. + * + * @param f1 The first float to compare + * @param f2 The second float to compare + * + * @return true if the two values are equal, otherwise false + */ + public static boolean floatRadEqual( float f1, float f2 ) + { + float absDiff = Math.abs( f1 - f2 ); + if( absDiff > Math.PI ) + absDiff = Math.abs( absDiff - (float)(Math.PI * 2) ); + return absDiff < FP_EQUALITY_THRESHOLD; + } } diff --git a/codebase/src/java/disco/org/openlvc/disco/utils/Mat3x3.java b/codebase/src/java/disco/org/openlvc/disco/utils/Mat3x3.java index 489f4332..1d893c85 100644 --- a/codebase/src/java/disco/org/openlvc/disco/utils/Mat3x3.java +++ b/codebase/src/java/disco/org/openlvc/disco/utils/Mat3x3.java @@ -17,6 +17,8 @@ */ package org.openlvc.disco.utils; +import org.openlvc.disco.DiscoException; + public class Mat3x3 { //---------------------------------------------------------- @@ -38,6 +40,7 @@ public Mat3x3() this( new Vec3(), new Vec3(), new Vec3() ); } + // vectors appear to be the columns of the matrix public Mat3x3( Vec3 a, Vec3 b, Vec3 c ) { this.a = a; @@ -55,7 +58,7 @@ public Mat3x3( Mat3x3 original ) //---------------------------------------------------------- // INSTANCE METHODS //---------------------------------------------------------- - public Quaternion toQuaterion() + public Quaternion toQuaternion() { // based heavily on https://github.com/g-truc/glm/blob/master/glm/gtc/quaternion.inl double m00 = this.a.x, m10 = this.b.x, m20 = this.c.x, @@ -124,8 +127,88 @@ public Quaternion toQuaterion() //////////////////////////////////////////////////////////////////////////////////////////// /////////////////////////////// Accessor and Mutator Methods /////////////////////////////// //////////////////////////////////////////////////////////////////////////////////////////// + /** + * Computes the transpose of this matrix. + * + * @return the transpose matrix + */ + public Mat3x3 transpose() + { + return new Mat3x3( new Vec3(this.a.x, this.b.x, this.c.x), + new Vec3(this.a.y, this.b.y, this.c.y), + new Vec3(this.a.z, this.b.z, this.c.z) ); + } + + /** + * Scales this {@link Mat3x3} by the given value. + * + * WARNING: modifies this instance + * + * @param rhs the scaling factor + * @return this instance, after scaling + */ + public Mat3x3 multiply( double rhs ) + { + this.a.multiply( rhs ); + this.b.multiply( rhs ); + this.c.multiply( rhs ); + return this; + } + + /** + * Obtains the matrix product of this {@link Mat3x3} and the given {@link Vec3}. + * + * @param rhs the vector to multiply by + * @return the matrix product + */ + public Vec3 multiply( Vec3 rhs ) + { + Mat3x3 T = this.transpose(); + return new Vec3( T.a.dot(rhs), T.b.dot(rhs), T.c.dot(rhs) ); + } + + /** + * Obtains the matrix product of this {@link Mat3x3} and another. + * + * @param rhs the matrix to multiply by + * @return the matrix product + */ + public Mat3x3 multiply( Mat3x3 rhs ) + { + Mat3x3 T = this.transpose(); + return new Mat3x3( new Vec3(T.a.dot(rhs.a), T.b.dot(rhs.a), T.c.dot(rhs.a)), + new Vec3(T.a.dot(rhs.b), T.b.dot(rhs.b), T.c.dot(rhs.b)), + new Vec3(T.a.dot(rhs.c), T.b.dot(rhs.c), T.c.dot(rhs.c)) ); + } + + /** + * Adds the given {@link Mat3x3} to this {@link Mat3x3}. + * + * WARNING: modifies this instance + * + * @param rhs the matrix to add + * @return this instance, after modification + */ + public Mat3x3 add( Mat3x3 rhs ) + { + this.a.add( rhs.a ); + this.b.add( rhs.b ); + this.c.add( rhs.c ); + return this; + } //---------------------------------------------------------- // STATIC METHODS //---------------------------------------------------------- + /** + * Generates a new {@link Mat3x3} Identity matrix instance. + * + * @return an 3x3 identity matrix + */ + public static Mat3x3 Identity() + { + return new Mat3x3( new Vec3(1, 0, 0), + new Vec3(0, 1, 0), + new Vec3(0, 0, 1) ); + } } \ No newline at end of file diff --git a/codebase/src/java/disco/org/openlvc/disco/utils/Quaternion.java b/codebase/src/java/disco/org/openlvc/disco/utils/Quaternion.java index c101fe80..9444049d 100644 --- a/codebase/src/java/disco/org/openlvc/disco/utils/Quaternion.java +++ b/codebase/src/java/disco/org/openlvc/disco/utils/Quaternion.java @@ -24,7 +24,6 @@ public class Quaternion //---------------------------------------------------------- // STATIC VARIABLES //---------------------------------------------------------- - private static final float _180DEGREES = (float)Math.PI; //---------------------------------------------------------- // INSTANCE VARIABLES @@ -49,18 +48,91 @@ public Quaternion( double w, double x, double y, double z ) this.y = y; this.z = z; } + + public Quaternion( Quaternion q ) + { + this( q.w, q.x, q.y, q.z ); + } //---------------------------------------------------------- // INSTANCE METHODS //---------------------------------------------------------- - public Quaternion multiply( Quaternion q ) + @Override + public boolean equals( Object other ) + { + if( this == other ) + return true; + + if( !(other instanceof Quaternion otherQuat) ) + return false; + + return FloatingPointUtils.doubleEqual( otherQuat.w, this.w ) && + FloatingPointUtils.doubleEqual( otherQuat.x, this.x ) && + FloatingPointUtils.doubleEqual( otherQuat.y, this.y ) && + FloatingPointUtils.doubleEqual( otherQuat.z, this.z ); + } + + /** + * Indicates whether the rotation represented by some other object is "equal to" this one. + *

+ * Returns {@code true} for this quaternion {@code q} iff {@code q} or {@code -q} gives + * {@code true} with {@link #equals(Object)} and the obj argument. + * + * @param other the reference object with which to compare. + * @return {@code true} if the rotation of this object is the same as the obj argument; + * {@code false} otherwise. + * + * @see #equals(Object) + */ + public boolean equalsRotation( Object other ) + { + return this.equals( other ) || this.inverseSign().equals( other ); + } + + @Override + public String toString() + { + return "Quaternion[w=%f, x=%f, y=%f, z=%f]".formatted( this.w, this.x, this.y, this.z ); + } + + /** + * Returns the Hamilton product of this quaternion right-multiplied by the given quaternion. + * + * @return {@code this * rhs} + */ + public Quaternion multiply( Quaternion rhs ) { - double w = this.w * q.w - this.x * q.x - this.y * q.y - this.z * q.z; - double x = this.w * q.x + this.x * q.w + this.y * q.z - this.z * q.y; - double y = this.w * q.y - this.x * q.z + this.y * q.w + this.z * q.x; - double z = this.w * q.z + this.x * q.y - this.y * q.x + this.z * q.w; + double w = this.w * rhs.w - this.x * rhs.x - this.y * rhs.y - this.z * rhs.z; + double x = this.w * rhs.x + this.x * rhs.w + this.y * rhs.z - this.z * rhs.y; + double y = this.w * rhs.y - this.x * rhs.z + this.y * rhs.w + this.z * rhs.x; + double z = this.w * rhs.z + this.x * rhs.y - this.y * rhs.x + this.z * rhs.w; return new Quaternion( w, x, y, z ); - } + } + + /** + * Returns a new quaternion that is the conjugate of the current quaternion. For unit + * quaternions this is also the Hamilton product inverse; quaternion {@code q}, returns + * {@code q* = q^-1} such that {@code q^-1 * (q * v * q^-1) * q = v} (the inverse of a unit + * quaternion is both the left and right inverse). + * + * @return A new unit {@link Quaternion} evaluating to {@code q*} + */ + public Quaternion conjugate() + { + return new Quaternion( w, -x, -y, -z ); + } + + /** + * Returns a new quaternion with opposite sign; for quaternion {@code q}, returns {@code -q}. + * For quaternions representing a rotation in 3d space, these two quaternions represent + * equivalent rotations. + * + * @return A new {@link Quaternion} evaluating to {@code -q} + */ + public Quaternion inverseSign() + { + return new Quaternion( -this.w, -this.x, -this.y, -this.z ); + } /** * Convert this Quaternion to an Euler which follows the DIS PDU orientation conventions. @@ -72,21 +144,49 @@ public Quaternion multiply( Quaternion q ) * * @return an {@link EulerAngles} instance suitable for use in a DIS PDU for setting entity * orientation + * + * @see #fromPduEulerAngles(EulerAngles) */ public EulerAngles toPduEulerAngles() { - double sqw = w * w; - double sqx = x * x; - double sqy = y * y; - double sqz = z * z; - - double yaw = Math.atan2( 2.0 * (x * y + z * w), (sqx - sqy - sqz + sqw) ); - double roll = Math.atan2( 2.0 * (y * z + x * w), (-sqx - sqy + sqz + sqw) ); - double pitch = Math.asin( -2.0 * (x * z - y * w) / (sqx + sqy + sqz + sqw) ); + double pitchRatio = -2.0 * (x * z - y * w); + + double divA = w * w - y * y; + double divB = x * x - z * z; + + double yaw = Math.atan2( 2.0 * (x * y + z * w), divA + divB ); + double pitch; + double roll = Math.atan2( 2.0 * (y * z + x * w), divA - divB ); + + // handle pitch singularities + if( Math.abs(pitchRatio) > 0.998 ) // > ~86.4 degrees + { + // yaw and roll are aligned - use only yaw + yaw = yaw - roll; + pitch = pitchRatio > 0d ? EulerAngles.THETA_MAX + : EulerAngles.THETA_MIN; + roll = 0; + + // wrap yaw + if( yaw >= EulerAngles.PHI_MAX ) yaw -= EulerAngles.PHI_MAX - EulerAngles.PHI_MIN; + if( yaw < EulerAngles.PHI_MIN ) yaw += EulerAngles.PHI_MAX - EulerAngles.PHI_MIN; + } + else + { + // regular pitch + pitch = Math.asin( pitchRatio ); + } - return new EulerAngles( (float)yaw, (float)pitch, (float)(roll-_180DEGREES)); + return new EulerAngles( (float)yaw, (float)pitch, (float)roll ); } - + + //////////////////////////////////////////////////////////////////////////////////////////// + /// Accessor and Mutator Methods ///////////////////////////////////////////////////////// + //////////////////////////////////////////////////////////////////////////////////////////// + + //---------------------------------------------------------- + // STATIC METHODS + //---------------------------------------------------------- /** * Create a Quaternion from an Euler which follows the X=PITCH, Y=ROLL, Z=YAW orientation conventions. * @@ -116,13 +216,30 @@ public static Quaternion fromEulerRad( Vec3 eulerAngleRad ) cx * sy * cz + sx * cy * sz, // y cx * cy * sz - sx * sy * cz // z ); - } + } - //////////////////////////////////////////////////////////////////////////////////////////// - /// Accessor and Mutator Methods ///////////////////////////////////////////////////////// - //////////////////////////////////////////////////////////////////////////////////////////// + /** + * Create a Quaternion from an Euler which follows the DIS PDU orientation conventions. + * + * @see #toPduEulerAngles() + */ + public static Quaternion fromPduEulerAngles( EulerAngles eulerAngles ) + { + double halfPsiRad = eulerAngles.getPsi() / 2.0; + double halfThetaRad = eulerAngles.getTheta() / 2.0; + double halfPhiRad = eulerAngles.getPhi() / 2.0; - //---------------------------------------------------------- - // STATIC METHODS - //---------------------------------------------------------- + double cs = Math.cos( halfPsiRad ); + double ct = Math.cos( halfThetaRad ); + double cp = Math.cos( halfPhiRad ); + double ss = Math.sin( halfPsiRad ); + double st = Math.sin( halfThetaRad ); + double sp = Math.sin( halfPhiRad ); + + return new Quaternion( cs * ct * cp + ss * st * sp, // w + cs * ct * sp - ss * st * cp, // x + cs * st * cp + ss * ct * sp, // y + ss * ct * cp - cs * st * sp // z + ); + } } diff --git a/codebase/src/java/disco/org/openlvc/disco/utils/Vec3.java b/codebase/src/java/disco/org/openlvc/disco/utils/Vec3.java index b221411c..7eef783f 100644 --- a/codebase/src/java/disco/org/openlvc/disco/utils/Vec3.java +++ b/codebase/src/java/disco/org/openlvc/disco/utils/Vec3.java @@ -62,6 +62,26 @@ public Vec3( WorldCoordinate ecef ) //---------------------------------------------------------- // INSTANCE METHODS //---------------------------------------------------------- + @Override + public boolean equals( Object other ) + { + if( this == other ) + return true; + + if( !(other instanceof Vec3 otherVec3) ) + return false; + + return FloatingPointUtils.doubleEqual( otherVec3.x, this.x ) && + FloatingPointUtils.doubleEqual( otherVec3.y, this.y ) && + FloatingPointUtils.doubleEqual( otherVec3.z, this.z ); + } + + @Override + public String toString() + { + return "Vec3[x=%f, y=%f, z=%f]".formatted( this.x, this.y, this.z ); + } + /** * Obtain the length of this {@link Vec3}, as measured from the origin * @@ -86,7 +106,63 @@ public double sqrdLength() return (this.x * this.x) + (this.y * this.y) + (this.z * this.z); - } + } + + /** + * Obtains the distance between the end of this {@link Vec3} and another. Equivalent to the + * distance between two points if both vectors represent offset from the origin. + * + * @return the distance between the two {@link Vec3}s ({@code |v1 - v2|}). + */ + public double distance( Vec3 rhs ) + { + return new Vec3( this ).subtract( rhs ).length(); + } + + /** + * Adds the given {@link Vec3} to this {@link Vec3} + * + * WARNING: modifies this instance + * + * @return this instance, after modification + */ + public Vec3 add( Vec3 rhs ) + { + this.x += rhs.x; + this.y += rhs.y; + this.z += rhs.z; + return this; + } + + /** + * Subtracts the given {@link Vec3} to this {@link Vec3} + * + * WARNING: modifies this instance + * + * @return this instance, after modification + */ + public Vec3 subtract( Vec3 rhs ) + { + this.x -= rhs.x; + this.y -= rhs.y; + this.z -= rhs.z; + return this; + } + + /** + * Scale this {@link Vec3} by the given value + * + * WARNING: modifies this instance + * + * @return this instance, which has been scaled + */ + public Vec3 multiply( double rhs ) + { + this.x *= rhs; + this.y *= rhs; + this.z *= rhs; + return this; + } /** * Divide this {@link Vec3} by the given value @@ -97,11 +173,22 @@ public double sqrdLength() */ public Vec3 divide( double rhs ) { - this.x /= rhs; + this.x /= rhs; this.y /= rhs; this.z /= rhs; return this; } + + /** + * Obtain the dot product of this {@link Vec3} and another + * + * @param v the other {@link Vec3} + * @return the dot product + */ + public double dot( Vec3 v ) + { + return this.x * v.x + this.y * v.y + this.z * v.z; + } /** * Obtain the cross product of this {@link Vec3} and another @@ -109,12 +196,20 @@ public Vec3 divide( double rhs ) * @param v the other {@link Vec3} * @return the cross product */ - public Vec3 cross(Vec3 v) + public Vec3 cross( Vec3 v ) { return new Vec3( this.y * v.z - v.y * this.z, this.z * v.x - v.z * this.x, this.x * v.y - v.x * this.y ); } + + public Mat3x3 outer( Vec3 v ) + { + // each vector input is a column + return new Mat3x3( new Vec3(this).multiply(v.x), + new Vec3(this).multiply(v.y), + new Vec3(this).multiply(v.z) ); + } /** * Normalizes this {@link Vec3} @@ -128,6 +223,20 @@ public Vec3 normalize() double vecLength = this.length(); return divide( vecLength ); } + + /** + * Computes this {@link Vec3} rotated by the given {@link Quaternion} through conjugation. + * Assumes {@code q} is a unit quaternion. + * + * @return a new {@link Vec3} representing this vector after rotation + */ + public Vec3 rotate( Quaternion q ) + { + Quaternion v_prime = new Quaternion( 0, this.x, this.y, this.z ); + Quaternion conjugate_result = q.multiply( v_prime.multiply(q.conjugate()) ); + // conjugate_result.w should be 0 + return new Vec3( conjugate_result.x, conjugate_result.y, conjugate_result.z ); + } //////////////////////////////////////////////////////////////////////////////////////////// /// Accessor and Mutator Methods ///////////////////////////////////////////////////////// diff --git a/codebase/src/java/test/org/openlvc/disco/utils/QuaternionTest.java b/codebase/src/java/test/org/openlvc/disco/utils/QuaternionTest.java new file mode 100644 index 00000000..51e2fe31 --- /dev/null +++ b/codebase/src/java/test/org/openlvc/disco/utils/QuaternionTest.java @@ -0,0 +1,235 @@ +/* + * Copyright 2025 Open LVC Project. + * + * This file is part of Open LVC Disco. + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ +package org.openlvc.disco.utils; + +import java.util.ArrayList; +import java.util.Iterator; +import java.util.Random; + +import org.openlvc.disco.pdu.record.EulerAngles; +import org.testng.Assert; +import org.testng.annotations.AfterClass; +import org.testng.annotations.AfterMethod; +import org.testng.annotations.BeforeClass; +import org.testng.annotations.BeforeMethod; +import org.testng.annotations.DataProvider; +import org.testng.annotations.Test; + +@Test(groups={"utils"}) +public class QuaternionTest +{ + //---------------------------------------------------------- + // STATIC VARIABLES + //---------------------------------------------------------- + + //---------------------------------------------------------- + // INSTANCE VARIABLES + //---------------------------------------------------------- + + //---------------------------------------------------------- + // CONSTRUCTORS + //---------------------------------------------------------- + + //---------------------------------------------------------- + // INSTANCE METHODS + //---------------------------------------------------------- + + /////////////////////////////////////////////////////////////////////////////////// + /// Test Class Setup/Tear Down ////////////////////////////////////////////////// + /////////////////////////////////////////////////////////////////////////////////// + @BeforeClass(alwaysRun=true) + public void beforeClass() + { + } + + @BeforeMethod(alwaysRun=true) + public void beforeMethod() + { + } + + @AfterMethod(alwaysRun=true) + public void afterMethod() + { + } + + @AfterClass(alwaysRun=true) + public void afterClass() + { + } + + /////////////////////////////////////////////////////////////////////////////////// + /// Testing Helpers ///////////////////////////////////////////////////////////// + /////////////////////////////////////////////////////////////////////////////////// + private void assertEulerAngleInBounds( EulerAngles orientation ) + { + Assert.assertTrue( orientation.getPsi() <= EulerAngles.PSI_MAX ); + Assert.assertTrue( EulerAngles.PSI_MIN <= orientation.getPsi() ); + + Assert.assertTrue( orientation.getTheta() <= EulerAngles.THETA_MAX ); + Assert.assertTrue( EulerAngles.THETA_MIN <= orientation.getTheta() ); + + Assert.assertTrue( orientation.getPhi() <= EulerAngles.PHI_MAX ); + Assert.assertTrue( EulerAngles.PHI_MIN <= orientation.getPhi() ); + } + + private void testQuaternionEulerAngleConversion( EulerAngles testOrientation, EulerAngles expectedOrientation ) + { + // double check our test values are in bounds + assertEulerAngleInBounds( testOrientation ); + assertEulerAngleInBounds( expectedOrientation ); + + // convert to a quaternion, and then back + Quaternion q = Quaternion.fromPduEulerAngles( testOrientation ); + EulerAngles orientation = q.toPduEulerAngles(); + + // check the output is in bounds + assertEulerAngleInBounds( orientation ); + + // check the output is what we expect + Assert.assertEquals( orientation, expectedOrientation ); + } + + private void testQuaternionEulerAngleConversion( EulerAngles testOrientation ) + { + testQuaternionEulerAngleConversion( testOrientation, testOrientation ); + } + + /////////////////////////////////////////////////////////////////////////////////// + /// Testing Methods ///////////////////////////////////////////////////////////// + /////////////////////////////////////////////////////////////////////////////////// + // TODO test with known quaternion output? + + @Test(dataProvider="eulerAngleBounds") + public void testQuaternionEulerAnglesBounds( EulerAngles testOrientation ) + { + testQuaternionEulerAngleConversion( testOrientation ); + } + + @Test(dataProvider="orientations_random100") + public void testQuaternionEulerAnglesRandom( EulerAngles testOrientation ) + { + testQuaternionEulerAngleConversion( testOrientation ); + } + + @Test(dataProvider="eulerAngleSingularityBounds") + public void testQuaternionEulerAnglesSingularityBounds( EulerAngles testOrientation ) + { + // figure out the correct yaw + EulerAngles expectedOrientation = testOrientation.clone(); + float yaw = expectedOrientation.getPsi() - expectedOrientation.getPhi(); + if( yaw >= EulerAngles.PSI_MAX ) yaw -= EulerAngles.PSI_MAX - EulerAngles.PSI_MIN; + if( yaw < EulerAngles.PSI_MIN ) yaw += EulerAngles.PSI_MAX - EulerAngles.PSI_MIN; + expectedOrientation.setPsi( yaw ); + expectedOrientation.setPhi( 0 ); + + testQuaternionEulerAngleConversion( testOrientation, expectedOrientation ); + } + + @Test(dataProvider="orientations_random100") + public void testQuaternionEulerAnglesSingularitiesRandom( EulerAngles testOrientation ) + { + // we get a random non-singularity orientation as input, set theta so we have a singularity + testOrientation.setTheta( testOrientation.getTheta() > 0f ? EulerAngles.THETA_MAX + : EulerAngles.THETA_MIN ); + + testQuaternionEulerAnglesSingularityBounds( testOrientation ); + } + + //---------------------------------------------------------- + // STATIC METHODS + //---------------------------------------------------------- + + /////////////////////////////////////////////////////////////////////////////////// + /// Data Providers ////////////////////////////////////////////////////////////// + /////////////////////////////////////////////////////////////////////////////////// + @DataProvider(name="eulerAngleBounds",parallel=true) + public static Iterator bounds() + { + ArrayList orientations = new ArrayList<>( 7 * 3 * 7 ); + + // tests with: [ MIN, NEAR MIN, NEGATIVE, 0, POSITIVE, NEAR MAX, MAX ] + // '-1.3' and '1.1' chosen as random positive/negative values + for( float psi : new float[]{ EulerAngles.PSI_MIN, EulerAngles.PSI_MIN + 0.01f, -1.3f, 0, + 1.1f, EulerAngles.PSI_MAX - 0.01f, EulerAngles.PSI_MAX } ) + { + for( float theta : new float[]{ -1.3f, 0, 1.1f } ) // test singularities separately + { + for( float phi : new float[]{ EulerAngles.PHI_MIN, EulerAngles.PHI_MIN + 0.01f, + -1.3f, 0, 1.1f, EulerAngles.PHI_MAX - 0.01f, + EulerAngles.PHI_MAX } ) + { + orientations.add( new EulerAngles( psi, theta, phi ) ); + } + } + } + + return orientations.iterator(); + } + + @DataProvider(name="eulerAngleSingularityBounds",parallel=true) + public static Iterator boundsWithSingularities() + { + ArrayList orientations = new ArrayList<>( 7 * 2 * 7 ); + + // tests with: [ MIN, NEAR MIN, NEGATIVE, 0, POSITIVE, NEAR MAX, MAX ] + // '-1.3' and '1.1' chosen as random positive/negative values + for( float psi : new float[]{ EulerAngles.PSI_MIN, EulerAngles.PSI_MIN + 0.01f, -1.3f, 0, + 1.1f, EulerAngles.PSI_MAX - 0.01f, EulerAngles.PSI_MAX } ) + { + for( float theta : new float[]{ EulerAngles.THETA_MIN, EulerAngles.THETA_MAX } ) // only test singularities + { + for( float phi : new float[]{ EulerAngles.PHI_MIN, EulerAngles.PHI_MIN + 0.01f, + -1.3f, 0, 1.1f, EulerAngles.PHI_MAX - 0.01f, + EulerAngles.PHI_MAX } ) + { + orientations.add( new EulerAngles( psi, theta, phi ) ); + } + } + } + + return orientations.iterator(); + } + + @DataProvider(name="orientations_random100",parallel=true) + public static Iterator randomOrientations() + { + return new Iterator() { + Random rand = new Random(); + int i = 0; + + @Override + public boolean hasNext() + { + return i < 100; + } + + @Override + public EulerAngles next() + { + i++; + + float psi = rand.nextFloat( EulerAngles.PSI_MIN, EulerAngles.PSI_MAX ); + float theta = rand.nextFloat( EulerAngles.THETA_MIN * (86.3f / 90), + EulerAngles.THETA_MAX * (86.3f / 90) ); // prevent singularities + float phi = rand.nextFloat( EulerAngles.PHI_MIN, EulerAngles.PHI_MAX ); + + return new EulerAngles( psi, theta, phi ); + } + }; + } +} From a37abb4714e948ed35ca2f2879340cad6265dd53 Mon Sep 17 00:00:00 2001 From: Nathan Townshend Date: Wed, 5 Nov 2025 16:08:53 +0800 Subject: [PATCH 3/8] Loads DisApplication config before services DisApplication services and helpers used to be loaded before the config for the DisApplication was loaded. If they relied on config options then they would only be initialised with the defaults, even if the config used to initialise the DisApplication had been modified. This commit reorders the DisApplication constructor to load the config first before starting the services and helpers. --- .../org/openlvc/disco/application/DisApplication.java | 9 ++++----- 1 file changed, 4 insertions(+), 5 deletions(-) diff --git a/codebase/src/java/disco/org/openlvc/disco/application/DisApplication.java b/codebase/src/java/disco/org/openlvc/disco/application/DisApplication.java index 0bc0921b..e51a5d0b 100644 --- a/codebase/src/java/disco/org/openlvc/disco/application/DisApplication.java +++ b/codebase/src/java/disco/org/openlvc/disco/application/DisApplication.java @@ -72,9 +72,9 @@ public class DisApplication //---------------------------------------------------------- // CONSTRUCTORS //---------------------------------------------------------- - public DisApplication() + public DisApplication( DiscoConfiguration configuration ) { - this.configuration = new DiscoConfiguration(); + this.configuration = configuration; this.opscenter = null; // set in start() // State Management Services and Helpers @@ -88,10 +88,9 @@ public DisApplication() this.pduBus.subscribe( new ApplicationBusErrorReporter() ); } - public DisApplication( DiscoConfiguration configuration ) + public DisApplication() { - this(); - this.configuration = configuration; + this( new DiscoConfiguration() ); } //---------------------------------------------------------- From 020e1023cbf05abc524999b89ceb4f648e0d1a92 Mon Sep 17 00:00:00 2001 From: Nathan Townshend Date: Thu, 23 Oct 2025 18:40:06 +0800 Subject: [PATCH 4/8] Adds dead-reckoning support Modifies EntityStateStore to provide dead-reckoning models for entities, providing support for querying the entity's estimated position. See: #93 --- .../disco/application/EntityStateStore.java | 34 +- .../openlvc/disco/application/PduStore.java | 5 +- .../application/pdu/DrEntityStatePdu.java | 333 +++++++++++++++ .../disco/application/utils/DrmState.java | 183 ++++++++ .../configuration/DiscoConfiguration.java | 15 + .../disco/pdu/entity/EntityStatePdu.java | 20 + .../pdu/field/DeadReckoningAlgorithm.java | 358 +++++++++++++++- .../pdu/record/AngularVelocityVector.java | 6 + .../openlvc/disco/pdu/record/EulerAngles.java | 2 +- .../disco/pdu/record/VectorRecord.java | 6 + .../disco/pdu/record/WorldCoordinate.java | 6 + .../org/openlvc/disco/utils/LruCache.java | 73 ++++ .../application/EntityStateStoreTest.java | 93 ++++- .../application/pdu/DrEntityStatePduTest.java | 394 ++++++++++++++++++ .../pdu/field/DeadReckoningAlgorithmTest.java | 286 +++++++++++++ 15 files changed, 1789 insertions(+), 25 deletions(-) create mode 100644 codebase/src/java/disco/org/openlvc/disco/application/pdu/DrEntityStatePdu.java create mode 100644 codebase/src/java/disco/org/openlvc/disco/application/utils/DrmState.java create mode 100644 codebase/src/java/disco/org/openlvc/disco/utils/LruCache.java create mode 100644 codebase/src/java/test/org/openlvc/disco/application/pdu/DrEntityStatePduTest.java create mode 100644 codebase/src/java/test/org/openlvc/disco/pdu/field/DeadReckoningAlgorithmTest.java diff --git a/codebase/src/java/disco/org/openlvc/disco/application/EntityStateStore.java b/codebase/src/java/disco/org/openlvc/disco/application/EntityStateStore.java index 4dc4b599..301d5faa 100644 --- a/codebase/src/java/disco/org/openlvc/disco/application/EntityStateStore.java +++ b/codebase/src/java/disco/org/openlvc/disco/application/EntityStateStore.java @@ -22,9 +22,11 @@ import java.util.concurrent.ConcurrentHashMap; import java.util.concurrent.ConcurrentMap; import java.util.concurrent.atomic.AtomicInteger; +import java.util.function.BooleanSupplier; import java.util.stream.Collectors; import org.openlvc.disco.DiscoException; +import org.openlvc.disco.application.pdu.DrEntityStatePdu; import org.openlvc.disco.pdu.entity.EntityStatePdu; import org.openlvc.disco.pdu.record.EntityId; import org.openlvc.disco.pdu.record.WorldCoordinate; @@ -42,8 +44,10 @@ public class EntityStateStore implements IDeleteReaperManaged //---------------------------------------------------------- // INSTANCE VARIABLES //---------------------------------------------------------- - private ConcurrentMap byId; - private ConcurrentMap byMarking; + private ConcurrentMap byId; + private ConcurrentMap byMarking; + + private BooleanSupplier isDrEnabled; //---------------------------------------------------------- // CONSTRUCTORS @@ -52,6 +56,8 @@ protected EntityStateStore( PduStore store ) { this.byId = new ConcurrentHashMap<>(); this.byMarking = new ConcurrentHashMap<>(); + + this.isDrEnabled = store.app.getConfiguration()::getDeadReckoningEnabled; } //---------------------------------------------------------- @@ -63,13 +69,17 @@ protected EntityStateStore( PduStore store ) //////////////////////////////////////////////////////////////////////////////////////////// protected void receivePdu( EntityStatePdu pdu ) { + // wrap the PDU to provide dead-reckoning support + DrEntityStatePdu wrappedPdu = this.isDrEnabled.getAsBoolean() ? new DrEntityStatePdu( pdu ) + : DrEntityStatePdu.makeWithoutDr( pdu ); + // bang the entity into the ID indexed store - EntityStatePdu existing = byId.put( pdu.getEntityID(), pdu ); + DrEntityStatePdu existing = byId.put( wrappedPdu.getEntityID(), wrappedPdu ); // if we are discovering this entity for first time, store in marking indexed store as well if( existing == null ) { - EntityStatePdu existingMarking = byMarking.put( pdu.getMarking(), pdu ); + DrEntityStatePdu existingMarking = byMarking.put( wrappedPdu.getMarking(), wrappedPdu ); // If there is already an entity against this marking with a different id, then // we assume that it has gone stale in favor of the one that we have just received. @@ -81,11 +91,11 @@ protected void receivePdu( EntityStatePdu pdu ) byId.remove( existingMarking.getEntityID(), existingMarking ); } - else if( existing.getMarking().equals(pdu.getMarking()) == false ) + else if( existing.getMarking().equals(wrappedPdu.getMarking()) == false ) { // marking has changed, need to update the marking indexed store byMarking.remove( existing.getMarking() ); - byMarking.put( pdu.getMarking(), pdu ); + byMarking.put( wrappedPdu.getMarking(), wrappedPdu ); } } @@ -119,7 +129,7 @@ public boolean hasEntityState( EntityId id ) return byId.containsKey( id ); } - public EntityStatePdu getEntityState( String marking ) + public DrEntityStatePdu getEntityState( String marking ) { if( marking.length() > 11 ) throw new DiscoException( "DIS markings limited to 11 characters: [%s] too long", marking ); @@ -129,9 +139,9 @@ public EntityStatePdu getEntityState( String marking ) /** * @param id - * @return the last {@link EntityStatePdu} for the entity, or `null` if not stored + * @return the last {@link DrEntityStatePdu} for the entity, or `null` if not stored */ - public EntityStatePdu getEntityState( EntityId id ) + public DrEntityStatePdu getEntityState( EntityId id ) { return byId.get( id ); } @@ -151,7 +161,7 @@ public Set getAllMarkings() * @param time The oldest time a PDU can have been updated to be returned * @return The set of all PDUs that have been updated since the given time, which may be empty */ - public Set getEntityStatesUpdatedSince( long time ) + public Set getEntityStatesUpdatedSince( long time ) { return this.byId.values().parallelStream() .filter( espdu -> espdu.getLocalTimestamp() >= time ) @@ -169,7 +179,7 @@ public Set getEntityStatesUpdatedSince( long time ) * @param radiusMeters Limit of how far a entity can be from the given entity * @return Set of all entities within the given radius of the given entity */ - public Set getEntityStatesNear( EntityStatePdu entity, int radiusMeters ) + public Set getEntityStatesNear( EntityStatePdu entity, int radiusMeters ) { return getEntityStatesNear( entity.getLocation(), radiusMeters ); } @@ -182,7 +192,7 @@ public Set getEntityStatesNear( EntityStatePdu entity, int radiu * @param radiusMeters Limit of how far a entity can be from the location * @return Set of all entities within the given radius of the given location */ - public Set getEntityStatesNear( WorldCoordinate location, int radiusMeters ) + public Set getEntityStatesNear( WorldCoordinate location, int radiusMeters ) { return byId.values().parallelStream() .filter( other -> WorldCoordinate.getStraightLineDistanceBetween(location, other.getLocation()) < radiusMeters ) diff --git a/codebase/src/java/disco/org/openlvc/disco/application/PduStore.java b/codebase/src/java/disco/org/openlvc/disco/application/PduStore.java index 32250626..b5021bc6 100644 --- a/codebase/src/java/disco/org/openlvc/disco/application/PduStore.java +++ b/codebase/src/java/disco/org/openlvc/disco/application/PduStore.java @@ -36,6 +36,8 @@ public class PduStore //---------------------------------------------------------- // INSTANCE VARIABLES //---------------------------------------------------------- + protected DisApplication app; + private EntityStateStore entityStore; private TransmitterStore transmitterStore; private EmitterStore emitterStore; @@ -45,6 +47,8 @@ public class PduStore //---------------------------------------------------------- protected PduStore( DisApplication app ) { + this.app = app; + this.entityStore = new EntityStateStore( this ); this.transmitterStore = new TransmitterStore( this ); // depends on EntityStore this.emitterStore = new EmitterStore( this ); // depends on EntityStore @@ -57,7 +61,6 @@ protected PduStore( DisApplication app ) //---------------------------------------------------------- // INSTANCE METHODS //---------------------------------------------------------- - protected void pduReceived( PDU pdu ) { switch( pdu.getType() ) diff --git a/codebase/src/java/disco/org/openlvc/disco/application/pdu/DrEntityStatePdu.java b/codebase/src/java/disco/org/openlvc/disco/application/pdu/DrEntityStatePdu.java new file mode 100644 index 00000000..a47401df --- /dev/null +++ b/codebase/src/java/disco/org/openlvc/disco/application/pdu/DrEntityStatePdu.java @@ -0,0 +1,333 @@ +/* + * Copyright 2025 Open LVC Project. + * + * This file is part of Open LVC Disco. + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ +package org.openlvc.disco.application.pdu; + +import java.util.Map; + +import org.openlvc.disco.application.utils.DrmState; +import org.openlvc.disco.pdu.entity.EntityStatePdu; +import org.openlvc.disco.pdu.field.DeadReckoningAlgorithm; +import org.openlvc.disco.pdu.record.EulerAngles; +import org.openlvc.disco.pdu.record.VectorRecord; +import org.openlvc.disco.pdu.record.WorldCoordinate; +import org.openlvc.disco.utils.LruCache; + +/** + * A logical extension of {@link EntityStatePdu} to include dead-reckoning, with methods for + * querying the state of the entity at a future timestamp.
+ *
+ * When queries are made for dead-reckoned information at a particular timestamp, stores the + * computed state in an internal LRU Cache. + */ +public class DrEntityStatePdu extends EntityStatePdu { + //---------------------------------------------------------- + // STATIC VARIABLES + //---------------------------------------------------------- + + //---------------------------------------------------------- + // INSTANCE VARIABLES + //---------------------------------------------------------- + private boolean drEnabled; + private final DrmState initialDrmState; + private Map drmStateCache; // access must be synchronized + private OutdatedTimestampBehavior outdatedTimestampBehavior; + + //---------------------------------------------------------- + // CONSTRUCTORS + //---------------------------------------------------------- + public DrEntityStatePdu( EntityStatePdu pdu, int cacheCapacity ) + { + super( pdu ); + + this.drEnabled = true; + + this.initialDrmState = new DrmState( pdu.getLocation(), + pdu.getLinearVelocity(), + pdu.getDeadReckoningParams().getEntityLinearAcceleration(), + pdu.getOrientation(), + pdu.getDeadReckoningParams().getEntityAngularVelocity() ); + + this.drmStateCache = new LruCache<>( cacheCapacity ); + + this.outdatedTimestampBehavior = OutdatedTimestampBehavior.ERROR; + } + + /** + * Uses a capacity of 3 as the default for the DRM State LRU Cache. + */ + public DrEntityStatePdu( EntityStatePdu pdu ) + { + this( pdu, 3 ); + } + + //---------------------------------------------------------- + // INSTANCE METHODS + //---------------------------------------------------------- + protected DrmState getInitialDrmState() + { + return this.initialDrmState; + } + + protected DrmState getDrmStateAtLocalTime( DeadReckoningAlgorithm algorithm, long localTimestamp ) throws IllegalArgumentException + { + if( this.getLocalTimestamp() > localTimestamp ) + { + // outdated dead-reckoning request + // might happen if a new EntityStatePDU comes in before another packet is finished being processed + switch( outdatedTimestampBehavior ) + { + case USE_CURRENT_STATE: + localTimestamp = this.getLocalTimestamp(); + break; + + default: + case ERROR: + throw new IllegalArgumentException( "Timestamp in dead-reckoning query too old. Expected %s or newer, got %s.".formatted( this.getLocalTimestamp(), + localTimestamp ) ); + } + } + + if( !this.drEnabled || this.isFrozen() || this.getLocalTimestamp() == localTimestamp ) + return this.getInitialDrmState(); + + CacheKey cacheKey = new CacheKey( algorithm, localTimestamp ); + + synchronized( this.drmStateCache ) + { + DrmState state = this.drmStateCache.get( cacheKey ); + if( state != null ) + return state; + + // hold access to the cache until we have written to it to prevent calculating the same state twice + + double dt_ms = localTimestamp - this.getLocalTimestamp(); + double dt = dt_ms / 1000.0; + + DrmState initialState = this.getInitialDrmState(); + if( algorithm.getReferenceFrame() != this.getDeadReckoningAlgorithm().getReferenceFrame() ) + { + // handle conversion between body and world coords if the algorithms don't match coord systems + switch( algorithm.getReferenceFrame() ) // what frame are we switching _to_ + { + case WorldCoordinates: + // Body -> World + initialState = initialState.asWorldCoords(); + break; + + case BodyCoordinates: + // World -> Body + initialState = initialState.asBodyCoords(); + break; + + default: + case Other: + // TODO warn? + break; + } + } + + state = algorithm.computeStateAfter( initialState, dt ); + this.drmStateCache.put( cacheKey, state ); + return state; + } + } + + //////////////////////////////////////////////////////////////////////////////////////////// + /////////////////////////////// Accessor and Mutator Methods /////////////////////////////// + //////////////////////////////////////////////////////////////////////////////////////////// + /** + * Sets the behavior when performing a dead-reckoning request with a timestamp older than the + * {@link EntityStatePdu}. + * + * @param behavior the new {@link OutdatedTimestampBehavior} to use when processing a + * dead-reckoning request with an outdated timestamp + */ + public void setOutdatedTimestampBehavior( OutdatedTimestampBehavior behavior ) + { + this.outdatedTimestampBehavior = behavior; + } + + /** + * Gets the current {@link OutdatedTimestampBehavior} used when processing a dead-reckoning + * request with an outdated timestamp + * + * @return the current {@link OutdatedTimestampBehavior} + */ + public OutdatedTimestampBehavior getOutdatedTimestampBehavior() + { + return this.outdatedTimestampBehavior; + } + + /** + * Returns the dead-reckoning algorithm specified for use by the {@link EntityStatePdu}. + * + * @return the {@link DeadReckoningAlgorithm} for this {@link DrEntityStatePdu} + */ + public DeadReckoningAlgorithm getDeadReckoningAlgorithm() + { + return this.getDeadReckoningParams().getDeadReckoningAlgorithm(); + } + + /** + * Gets the position of the entity at the given local timestamp, extrapolated using the + * dead-reckoning algorithm of this entity. + *

+ * Uses the internally cached value for the state at this timestamp, if available, rather than + * recomputing the state. + * + * @param localTimestamp target timestamp, in ms since epoch. + * + * @return the position of the entity, as a {@link WorldCoordinate} + * @throws IllegalArgumentException if the given timestamp is older than the {@link EntityStatePdu} + * @see #setOutdatedTimestampBehavior(OutdatedTimestampBehavior) + */ + public WorldCoordinate getDrLocation( long localTimestamp ) throws IllegalArgumentException + { + return this.getDrLocation( this.getDeadReckoningAlgorithm(), localTimestamp ); + } + + /** + * Gets the position of the entity at the given local timestamp, extrapolated using the + * specified dead-reckoning algorithm. + *

+ * Uses the internally cached value for the state at this timestamp (with this algorithm), if + * available, rather than recomputing the state. + * + * @param algorithm the dead-reckoning algorithm to use + * @param localTimestamp target timestamp, in ms since epoch. + * + * @return the position of the entity, as a {@link WorldCoordinate} + * @throws IllegalArgumentException if the given timestamp is older than the {@link EntityStatePdu} + * @see #setOutdatedTimestampBehavior(OutdatedTimestampBehavior) + */ + public WorldCoordinate getDrLocation( DeadReckoningAlgorithm algorithm, long localTimestamp ) throws IllegalArgumentException + { + return this.getDrmStateAtLocalTime( algorithm, localTimestamp ).getLocation(); + } + + /** + * Gets the velocity of the entity at the given local timestamp, extrapolated using the + * dead-reckoning algorithm of this entity. + *

+ * Uses the internally cached value for the state at this timestamp, if available, rather than + * recomputing the state. + * + * @return the velocity of the entity, as a {@link VectorRecord} + * @throws IllegalArgumentException if the given timestamp is older than the {@link EntityStatePdu} + * @see #setOutdatedTimestampBehavior(OutdatedTimestampBehavior) + */ + public VectorRecord getDrLinearVelocity( long localTimestamp ) throws IllegalArgumentException + { + return this.getDrLinearVelocity( this.getDeadReckoningAlgorithm(), localTimestamp ); + } + + /** + * Gets the velocity of the entity at the given local timestamp, extrapolated using the + * specified dead-reckoning algorithm. + *

+ * Uses the internally cached value for the state at this timestamp (with this algorithm), if + * available, rather than recomputing the state. + * + * @param algorithm the dead-reckoning algorithm to use + * @param localTimestamp target timestamp, in ms since epoch. + * + * @return the velocity of the entity, as a {@link VectorRecord} + * @throws IllegalArgumentException if the given timestamp is older than the {@link EntityStatePdu} + * @see #setOutdatedTimestampBehavior(OutdatedTimestampBehavior) + */ + public VectorRecord getDrLinearVelocity( DeadReckoningAlgorithm algorithm, long localTimestamp ) throws IllegalArgumentException + { + return this.getDrmStateAtLocalTime( algorithm, localTimestamp ).getLinearVelocity(); + } + + /** + * Gets the orientation of the entity at the given local timestamp, extrapolated using the + * dead-reckoning algorithm of this entity. + *

+ * Uses the internally cached value for the state at this timestamp, if available, rather than + * recomputing the state. + * + * @return the orientation of the entity, as {@link EulerAngles} + * @throws IllegalArgumentException if the given timestamp is older than the {@link EntityStatePdu} + * @see #setOutdatedTimestampBehavior(OutdatedTimestampBehavior) + */ + public EulerAngles getDrOrientation( long localTimestamp ) throws IllegalArgumentException + { + return this.getDrOrientation( this.getDeadReckoningAlgorithm(), localTimestamp ); + } + + /** + * Gets the orientation of the entity at the given local timestamp, extrapolated using the + * specified dead-reckoning algorithm. + *

+ * Uses the internally cached value for the state at this timestamp (with this algorithm), if + * available, rather than recomputing the state. + * + * @param algorithm the dead-reckoning algorithm to use + * @param localTimestamp target timestamp, in ms since epoch. + * + * @return the orientation of the entity, as a {@link EulerAngles} + * @throws IllegalArgumentException if the given timestamp is older than the {@link EntityStatePdu} + * @see #setOutdatedTimestampBehavior(OutdatedTimestampBehavior) + */ + public EulerAngles getDrOrientation( DeadReckoningAlgorithm algorithm, long localTimestamp ) throws IllegalArgumentException + { + return this.getDrmStateAtLocalTime( algorithm, localTimestamp ).getOrientation(); + } + + //---------------------------------------------------------- + // STATIC METHODS + //---------------------------------------------------------- + /** + * A record of each value that acts as a key to the dead-reckoning cache. + */ + private record CacheKey( DeadReckoningAlgorithm algorithm, long localTimestamp ) + { + } + + /** + * Creates an {@link DrEntityStatePdu} instance with dead-reckoning calculations disabled + * (behaves as if the dead-reckoning algorithm is {@link DeadReckoningAlgorithm#Static}). + */ + public static DrEntityStatePdu makeWithoutDr( EntityStatePdu pdu ) + { + DrEntityStatePdu wrappedPdu = new DrEntityStatePdu( pdu, -1 ); + wrappedPdu.drEnabled = false; + wrappedPdu.drmStateCache = null; + return wrappedPdu; + } + + /** + * The behavior to follow when a dead-reckoning request is made with an outdated timestamp + * (older than the current latest timestamp). + */ + public enum OutdatedTimestampBehavior + { + /** + * Throw an {@link IllegalArgumentException}. + */ + ERROR, + + /** + * Use the entity's last reported state (the state in this EntityStatePdu).
+ * Unless entity state has been manually stored elsewhere, this will usually be the oldest + * state known for the entity that is more recent than the attempted timestamp. + */ + USE_CURRENT_STATE; + } +} diff --git a/codebase/src/java/disco/org/openlvc/disco/application/utils/DrmState.java b/codebase/src/java/disco/org/openlvc/disco/application/utils/DrmState.java new file mode 100644 index 00000000..23b3499a --- /dev/null +++ b/codebase/src/java/disco/org/openlvc/disco/application/utils/DrmState.java @@ -0,0 +1,183 @@ +/* + * Copyright 2025 Open LVC Project. + * + * This file is part of Open LVC Disco. + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ +package org.openlvc.disco.application.utils; + +import java.util.Objects; + +import org.openlvc.disco.DiscoException; +import org.openlvc.disco.pdu.record.AngularVelocityVector; +import org.openlvc.disco.pdu.record.EulerAngles; +import org.openlvc.disco.pdu.record.VectorRecord; +import org.openlvc.disco.pdu.record.WorldCoordinate; +import org.openlvc.disco.utils.Quaternion; +import org.openlvc.disco.utils.Vec3; + +/** + * Represents the state of an entity at a point in time in a {@link DeadReckoningModel}.
+ *
+ * The reference frame of the {@link #velocity} and {@link #acceleration} vectors can vary, and + * uses the coordinate system of the model that produced this state.
+ *
+ * Complete list of reference frames: + *

    + *
  • {@link #position}: world coordinate system (see also: {@link WorldCoordinate})
    • + *
  • {@link #velocity}: varies depending on DR model
    • + *
  • {@link #acceleration}: varies depending on DR model
    • + *
  • {@link #orientation}: world coordinate system (see also: {@link EulerAngles})
    • + *
  • {@link #angularVelocity}: entity coordinate system (see also: + * {@link AngularVelocityVector})
    • + *
+ */ +public record DrmState( Vec3 position, + Vec3 velocity, + Vec3 acceleration, + Quaternion orientation, + Vec3 angularVelocity ) +{ + //---------------------------------------------------------- + // STATIC VARIABLES + //---------------------------------------------------------- + + //---------------------------------------------------------- + // CONSTRUCTORS + //---------------------------------------------------------- + public DrmState( WorldCoordinate location, VectorRecord linearVelocity, VectorRecord linearAcceleration, EulerAngles orientation, AngularVelocityVector angularVelocity ) + { + this( new Vec3(location), + new Vec3(linearVelocity.getFirstComponent(), + linearVelocity.getSecondComponent(), + linearVelocity.getThirdComponent()), + new Vec3(linearAcceleration.getFirstComponent(), + linearAcceleration.getSecondComponent(), + linearAcceleration.getThirdComponent()), + Quaternion.fromPduEulerAngles(orientation), + new Vec3(angularVelocity.getRateAboutXAxis(), + angularVelocity.getRateAboutYAxis(), + angularVelocity.getRateAboutZAxis()) ); + } + + //---------------------------------------------------------- + // INSTANCE METHODS + //---------------------------------------------------------- + @Override + public boolean equals( Object other ) + { + if( this == other ) + return true; + + if( !(other instanceof DrmState otherDrmState) ) + return false; + + return Objects.equals( otherDrmState.position(), this.position() ) && + Objects.equals( otherDrmState.velocity(), this.velocity() ) && + Objects.equals( otherDrmState.acceleration(), this.acceleration() ) && + this.orientation() != null && + this.orientation().equalsRotation( otherDrmState.orientation() ) && + Objects.equals( otherDrmState.angularVelocity(), this.angularVelocity() ); + } + + /** + * Returns a {@link DrmState} with coordinate-system-sensitive fields using body coordinates. + * Assumes the current coordinate system (for the respective fields) is world coordinate. + * + * @return the object state, using body coordinates + */ + public DrmState asBodyCoords() + { + return new DrmState( this.position(), + this.velocity().rotate(this.orientation()), + this.acceleration().rotate(this.orientation()), + this.orientation(), + this.angularVelocity() ); + } + + /** + * Returns a {@link DrmState} with coordinate-system-sensitive fields using world coordinates. + * Assumes the current coordinate system (for the respective fields) is body coordinate. + * + * @return the object state, using world coordinates + */ + public DrmState asWorldCoords() + { + return new DrmState( this.position(), + this.velocity().rotate(this.orientation().conjugate()), + this.acceleration().rotate(this.orientation().conjugate()), + this.orientation(), + this.angularVelocity() ); + } + + //////////////////////////////////////////////////////////////////////////////////////////// + /////////////////////////////// Accessor Methods /////////////////////////////////////////// + //////////////////////////////////////////////////////////////////////////////////////////// + /** + * Gets the position of the represented entity in DIS PDU format. + * + * @return the position of the entity, as a {@link WorldCoordinate} + */ + public WorldCoordinate getLocation() + { + return new WorldCoordinate( this.position().x, this.position().y, this.position().z ); + } + + /** + * Gets the velocity of the represented entity in DIS PDU format. + * Uses the coordinate system of the model that produced this state. + * + * @return the velocity of the entity, as a {@link VectorRecord} + */ + public VectorRecord getLinearVelocity() + { + return new VectorRecord( (float)this.velocity().x, (float)this.velocity().y, (float)this.velocity().z ); + } + + /** + * Gets the acceleration of the represented entity in DIS PDU format. + * Uses the coordinate system of the model that produced this state. + * + * @return the acceleration of the entity, as a {@link VectorRecord} + */ + public VectorRecord getLinearAcceleration() + { + return new VectorRecord( (float)this.acceleration().x, (float)this.acceleration().y, (float)this.acceleration().z ); + } + + /** + * Gets the orientation of the represented entity in DIS PDU format. + * + * @return the orientation of the entity, as {@link EulerAngles} + */ + public EulerAngles getOrientation() + { + return this.orientation().toPduEulerAngles(); + } + + /** + * Gets the angular velocity of the represented entity in DIS PDU format (Body Coordinate + * System). + * + * @return the angular velocity of the entity, as an {@link AngularVelocityVector} + */ + public AngularVelocityVector getAngularVelocity() + { + return new AngularVelocityVector( (float)this.angularVelocity().x, (float)this.angularVelocity().y, (float)this.angularVelocity().z ); + } + + //---------------------------------------------------------- + // STATIC METHODS + //---------------------------------------------------------- +} diff --git a/codebase/src/java/disco/org/openlvc/disco/configuration/DiscoConfiguration.java b/codebase/src/java/disco/org/openlvc/disco/configuration/DiscoConfiguration.java index 96728442..96708980 100644 --- a/codebase/src/java/disco/org/openlvc/disco/configuration/DiscoConfiguration.java +++ b/codebase/src/java/disco/org/openlvc/disco/configuration/DiscoConfiguration.java @@ -44,6 +44,8 @@ public class DiscoConfiguration public static final String PROP_PDU_SENDER = "disco.pdu.sender"; public static final String PROP_PDU_RECEIVER = "disco.pdu.receiver"; + + public static final String PROP_DR_ENABLED = "disco.app.deadreckoning.enabled"; // bool, default true //---------------------------------------------------------- // INSTANCE VARIABLES @@ -210,6 +212,19 @@ public RprConfiguration getRprConfiguration() { return this.rprConfiguration; } + + //////////////////////////////////////////////////////////////////////////////////////////// + /// Application Properties /////////////////////////////////////////////////////////////// + //////////////////////////////////////////////////////////////////////////////////////////// + public boolean getDeadReckoningEnabled() + { + return this.isProperty( PROP_DR_ENABLED, true ); + } + + public void setDeadReckoningEnabled( boolean enabled ) + { + properties.setProperty( PROP_DR_ENABLED, ""+enabled ); + } protected String getProperty( String key, String defaultValue ) diff --git a/codebase/src/java/disco/org/openlvc/disco/pdu/entity/EntityStatePdu.java b/codebase/src/java/disco/org/openlvc/disco/pdu/entity/EntityStatePdu.java index f8087a26..38814e59 100644 --- a/codebase/src/java/disco/org/openlvc/disco/pdu/entity/EntityStatePdu.java +++ b/codebase/src/java/disco/org/openlvc/disco/pdu/entity/EntityStatePdu.java @@ -81,6 +81,26 @@ public EntityStatePdu() this.capabilities = new EntityCapabilities(); this.articulationParameters = new ArrayList(); } + + protected EntityStatePdu( EntityStatePdu pdu ) + { + super( PduType.EntityState ); + + this.localTimestamp = pdu.localTimestamp; + + this.entityID = pdu.entityID; + this.forceID = pdu.forceID; + this.entityType = pdu.entityType; + this.alternativeEntityType = pdu.alternativeEntityType; + this.linearVelocity = pdu.linearVelocity; + this.location = pdu.location; + this.orientation = pdu.orientation; + this.appearance = pdu.appearance; + this.deadReckoningParams = pdu.deadReckoningParams; + this.marking = pdu.marking; + this.capabilities = pdu.capabilities; + this.articulationParameters = pdu.articulationParameters; + } //---------------------------------------------------------- // INSTANCE METHODS diff --git a/codebase/src/java/disco/org/openlvc/disco/pdu/field/DeadReckoningAlgorithm.java b/codebase/src/java/disco/org/openlvc/disco/pdu/field/DeadReckoningAlgorithm.java index a257b636..a96a9115 100644 --- a/codebase/src/java/disco/org/openlvc/disco/pdu/field/DeadReckoningAlgorithm.java +++ b/codebase/src/java/disco/org/openlvc/disco/pdu/field/DeadReckoningAlgorithm.java @@ -17,9 +17,17 @@ */ package org.openlvc.disco.pdu.field; +import java.util.Optional; + +import org.openlvc.disco.DiscoException; +import org.openlvc.disco.application.utils.DrmState; import org.openlvc.disco.configuration.DiscoConfiguration; import org.openlvc.disco.configuration.Flag; import org.openlvc.disco.pdu.DisSizes; +import org.openlvc.disco.pdu.record.WorldCoordinate; +import org.openlvc.disco.utils.Mat3x3; +import org.openlvc.disco.utils.Quaternion; +import org.openlvc.disco.utils.Vec3; public enum DeadReckoningAlgorithm { @@ -58,6 +66,88 @@ public short value() return this.value; } + @Override + public String toString() + { + switch( this.value ) + { + case 1: return "Static"; + case 2: return "FPW"; + case 3: return "RPW"; + case 4: return "RVW"; + case 5: return "FVW"; + case 6: return "FPB"; + case 7: return "RPB"; + case 8: return "RVB"; + case 9: return "FVB"; + default: // drop through + } + + // Missing + if( DiscoConfiguration.isSet(Flag.Strict) ) + throw new IllegalArgumentException( value+" not a valid Dead Reckoning Algorithm" ); + else + return "Other"; + } + + public ReferenceFrame getReferenceFrame() + { + switch( this ) + { + case Static: + case FPW: + case FVW: + case RPW: + case RVW: + return ReferenceFrame.WorldCoordinates; + + case FPB: + case FVB: + case RPB: + case RVB: + return ReferenceFrame.BodyCoordinates; + + default: // drop through + } + + // Ill-defined or unknown + if( DiscoConfiguration.isSet(Flag.Strict) ) + throw new DiscoException( "Unknown reference frame for dead-reckoning algorithm: %s (%d)".formatted(this, + this.value()) ); + return ReferenceFrame.Other; + } + + /** + * Returns the state of the dead-reckoning model when extrapolated for the given duration + * using this dead-reckoning algorithm. + * + * @param initialState the initial state to apply the model to + * @param dt time to extrapolate forwards, in s + * @return a {@link DrmState} with the state of the model after the given duration + * + * @see #computeFixedStateAfter(DeadReckoningAlgorithm, DrmState, double) + */ + public DrmState computeStateAfter( DrmState initialState, double dt ) + { + if( this == Static ) + return initialState; + + switch( this.getReferenceFrame() ) + { + case WorldCoordinates: + return DeadReckoningAlgorithm.computeFixedStateAfter( this, initialState, dt ); + + case BodyCoordinates: + return DeadReckoningAlgorithm.computeRotatingStateAfter( this, initialState, dt ); + + default: + case Other: + // fallback to static + // TODO warn? + return initialState; + } + } + //---------------------------------------------------------- // STATIC METHODS //---------------------------------------------------------- @@ -70,15 +160,15 @@ public static DeadReckoningAlgorithm fromValue( short value ) { switch( value ) { - case 2: return FPW; - case 4: return RVW; - case 1: return Static; - case 8: return RVB; - case 3: return RPW; - case 5: return FVW; - case 6: return FPB; - case 7: return RPB; - case 9: return FVB; + case 2: return FPW; + case 4: return RVW; + case 1: return Static; + case 8: return RVB; + case 3: return RPW; + case 5: return FVW; + case 6: return FPB; + case 7: return RPB; + case 9: return FVB; default: // drop through } @@ -89,4 +179,254 @@ public static DeadReckoningAlgorithm fromValue( short value ) return Other; } + // q_DR in the spec + private static Quaternion makeRotationQuaternion( Vec3 angularVelocity, double dt ) + { + Vec3 rotAxis = new Vec3( angularVelocity ).normalize(); + + double halfBeta = (angularVelocity.length() * dt) / 2.0; + double sinHalfBeta = Math.sin( halfBeta ); + + double w = Math.cos( halfBeta ); + double x = rotAxis.x * sinHalfBeta; + double y = rotAxis.y * sinHalfBeta; + double z = rotAxis.z * sinHalfBeta; + + return new Quaternion( w, x, y, z ); + } + + /** + * Returns the state of the dead-reckoning model when extrapolated for the given duration, + * using the specified fixed/world reference (World Coordinate) dead-reckoning algorithm. + * + * @param algorithm MUST be one of FPW, FVW, RPW, or RVW + * @param initialState the initial state to apply the model to + * @param dt time to extrapolate forwards, in s + * @return a {@link DrmState} with the state of the model after the given duration + * + * @see #computeRotatingStateAfter(DeadReckoningAlgorithm, DrmState, double) + */ + private static DrmState computeFixedStateAfter( DeadReckoningAlgorithm algorithm, DrmState initialState, double dt ) + { + // use the old copies if we don't make any changes + Optional position = Optional.empty(); + Optional velocity = Optional.empty(); + Optional orientation = Optional.empty(); + + // effects due to velocity + switch( algorithm ) + { + // currently all models + case FPW: + case FVW: + case RPW: + case RVW: + // displacement + Vec3 disp = new Vec3( initialState.velocity() ); + disp.multiply( dt ); + + if( position.isEmpty() ) + position = Optional.of( new Vec3(initialState.position()) ); + position.get().add( disp ); + break; + + default: + break; + } + + // effects due to acceleration + switch( algorithm ) + { + // 'V'-type models + case FVW: + case RVW: + // change in velocity + Vec3 dv = new Vec3( initialState.acceleration() ); + dv.multiply( dt ); + + if( velocity.isEmpty() ) + velocity = Optional.of( new Vec3( initialState.velocity() ) ); + velocity.get().add( dv ); + + // displacement + Vec3 disp = new Vec3( dv ); + disp.multiply( dt / 2.0 ); + + if( position.isEmpty() ) + position = Optional.of( new Vec3( initialState.position() ) ); + position.get().add( disp ); + break; + + default: + break; + } + + // effects due to angular velocity + switch( algorithm ) + { + // 'R'-type models + case RPW: + case RVW: + // rotation + Quaternion rotationQuaternion = DeadReckoningAlgorithm.makeRotationQuaternion( initialState.angularVelocity(), dt ); + orientation = Optional.of( orientation.orElseGet(initialState::orientation).multiply(rotationQuaternion) ); + break; + + default: + break; + } + + return new DrmState( position.orElse(initialState.position()), + velocity.orElse(initialState.velocity()), + new Vec3(initialState.acceleration()), + orientation.orElse(initialState.orientation()), + new Vec3(initialState.angularVelocity()) ); + } + + /** + * Returns the state of the dead-reckoning model when extrapolated for the given duration, + * using the specified rotating/body reference (Body Coordinate) dead-reckoning algorithm. + * + * @param algorithm MUST be one of FPB, FVB, RPB, or RVB + * @param initialState the initial state to apply the model to + * @param dt time to extrapolate forwards, in s + * @return a {@link DrmState} with the state of the model after the given duration + * + * @see #computeFixedStateAfter(DeadReckoningAlgorithm, DrmState, double) + */ + private static DrmState computeRotatingStateAfter( DeadReckoningAlgorithm algorithm, DrmState initialState, double dt ) + { + Optional position = Optional.empty(); + Optional orientation = Optional.empty(); + + // precompute values shared across calculations + + // V_b; same as v_b + Vec3 V_b = initialState.velocity(); + + Vec3 w = initialState.angularVelocity(); + + double w_mag = w.length(); + double w_mag2 = w.sqrdLength(); + double w_mag3 = w_mag * w_mag2; + + double wdt = w_mag * dt; + + // vectors are columns + Mat3x3 skew = new Mat3x3( new Vec3(0, w.z, -w.y), + new Vec3(-w.z, 0, w.x), + new Vec3(w.y, -w.x, 0) ); + Mat3x3 wwT = w.outer( w ); + + double sin_wdt = Math.sin( wdt ); + double cos_wdt = Math.cos( wdt ); + + // effects due to velocity + switch( algorithm ) + { + // currently all models + case FPB: + case FVB: + case RPB: + case RVB: + // R_1 + Mat3x3 R_1 = new Mat3x3( wwT ).multiply( (wdt - sin_wdt) / w_mag3 ); + R_1.add( Mat3x3.Identity().multiply(sin_wdt / w_mag) ); + R_1.add( new Mat3x3(skew).multiply((1 - cos_wdt) / w_mag2) ); + + // displacement + Vec3 disp = R_1.multiply( V_b ).rotate( initialState.orientation() ); + + if( position.isEmpty() ) + position = Optional.of( new Vec3(initialState.position()) ); + position.get().add( disp ); + break; + + default: + break; + } + + // effects due to acceleration + switch( algorithm ) + { + // 'V'-type models + case FVB: + case RVB: + double w_mag4 = w_mag2 * w_mag2; + + // R_2 + double cpwdtsm1 = cos_wdt + wdt*sin_wdt - 1; + Mat3x3 R_2 = new Mat3x3( wwT ).multiply( (0.5*w_mag2*dt*dt - cpwdtsm1) / w_mag4 ); + R_2.add( Mat3x3.Identity().multiply(cpwdtsm1 / w_mag2) ); + R_2.add( new Mat3x3(skew).multiply((sin_wdt - wdt*cos_wdt) / w_mag3) ); + + // A_b; time derivative of V_b, computed as a_b - skew * V_b + Vec3 A_b = new Vec3( initialState.acceleration() ); + A_b.subtract( new Mat3x3(skew).multiply(V_b) ); + + // TODO compute new velocity if needed + + // displacement + Vec3 disp = R_2.multiply( A_b ).rotate( initialState.orientation() ); + + if( position.isEmpty() ) + position = Optional.of( new Vec3( initialState.position() ) ); + position.get().add( disp ); + break; + + default: + break; + } + + // effects due to angular velocity + switch( algorithm ) + { + // 'R'-type models + case RPB: + case RVB: + // rotation + Quaternion rotationQuaternion = DeadReckoningAlgorithm.makeRotationQuaternion( initialState.angularVelocity(), dt ); + orientation = Optional.of( orientation.orElseGet(initialState::orientation).multiply(rotationQuaternion) ); + break; + + default: + break; + } + + return new DrmState( position.orElse(initialState.position()), + new Vec3(initialState.velocity()), + new Vec3(initialState.acceleration()), + orientation.orElse(initialState.orientation()), + new Vec3(initialState.angularVelocity()) ); + } + + /** + * The reference frame of a dead-reckoning algorithm - either world or body coordinates. + */ + public enum ReferenceFrame + { + Other, + + /** + * The 'World coordinate system' as defined in the DIS spec (1.6.3.1, IEEE 1278.1-2012), a + * NIMA TR 8350.2 and WGS 84 based right-handed geocentric Cartesian coordinate system. + * + * See {@link WorldCoordinate} for more information. + */ + WorldCoordinates, + + /** + * The 'Entity coordinate system' as defined in the DIS spec (1.6.3.2, IEEE 1278.1-2012), + * a right-handed Cartesian coordinate system centred and oriented on the associated + * entity, with a distance of 1 unit corresponding to 1m.
+ *
+ * Axes (see Figure 2 of IEEE 1278.1-2012 for a graphical depiction): + *
    + *
  • {@code x}: positive is to the front of the entity
    • + *
  • {@code y}: positive is to the right of the entity
    • + *
  • {@code z}: positive is below the entity
    • + *
+ */ + BodyCoordinates; + } } diff --git a/codebase/src/java/disco/org/openlvc/disco/pdu/record/AngularVelocityVector.java b/codebase/src/java/disco/org/openlvc/disco/pdu/record/AngularVelocityVector.java index a31cae76..8e6e7160 100644 --- a/codebase/src/java/disco/org/openlvc/disco/pdu/record/AngularVelocityVector.java +++ b/codebase/src/java/disco/org/openlvc/disco/pdu/record/AngularVelocityVector.java @@ -85,6 +85,12 @@ public boolean equals( Object object ) return equal; } + + @Override + public String toString() + { + return "AngularVelocityVector[x=%f, y=%f, z=%f]".formatted( this.rateAboutXAxis, this.rateAboutYAxis, this.rateAboutZAxis ); + } @Override public AngularVelocityVector clone() diff --git a/codebase/src/java/disco/org/openlvc/disco/pdu/record/EulerAngles.java b/codebase/src/java/disco/org/openlvc/disco/pdu/record/EulerAngles.java index 317e980e..db46e7a7 100644 --- a/codebase/src/java/disco/org/openlvc/disco/pdu/record/EulerAngles.java +++ b/codebase/src/java/disco/org/openlvc/disco/pdu/record/EulerAngles.java @@ -99,7 +99,7 @@ public EulerAngles clone() @Override public String toString() { - return "psi=%f, theta=%f, phi=%f".formatted( this.psi, this.theta, this.phi ); + return "EulerAngles[psi=%f, theta=%f, phi=%f]".formatted( this.psi, this.theta, this.phi ); } //////////////////////////////////////////////////////////////////////////////////////////// diff --git a/codebase/src/java/disco/org/openlvc/disco/pdu/record/VectorRecord.java b/codebase/src/java/disco/org/openlvc/disco/pdu/record/VectorRecord.java index 5915031f..6fc5adae 100644 --- a/codebase/src/java/disco/org/openlvc/disco/pdu/record/VectorRecord.java +++ b/codebase/src/java/disco/org/openlvc/disco/pdu/record/VectorRecord.java @@ -81,6 +81,12 @@ public boolean equals( Object other ) return false; } + @Override + public String toString() + { + return "VectorRecord[%f, %f, %f]".formatted( this.firstComponent, this.secondComponent, this.thirdComponent ); + } + @Override public int hashCode() { diff --git a/codebase/src/java/disco/org/openlvc/disco/pdu/record/WorldCoordinate.java b/codebase/src/java/disco/org/openlvc/disco/pdu/record/WorldCoordinate.java index f5324ae2..e7709bad 100644 --- a/codebase/src/java/disco/org/openlvc/disco/pdu/record/WorldCoordinate.java +++ b/codebase/src/java/disco/org/openlvc/disco/pdu/record/WorldCoordinate.java @@ -101,6 +101,12 @@ public boolean equals( Object other ) return false; } + + @Override + public String toString() + { + return "WorldCoordinate[x=%f, y=%f, z=%f]".formatted( this.x, this.y, this.z ); + } /** * {@inheritDoc} diff --git a/codebase/src/java/disco/org/openlvc/disco/utils/LruCache.java b/codebase/src/java/disco/org/openlvc/disco/utils/LruCache.java new file mode 100644 index 00000000..0c444809 --- /dev/null +++ b/codebase/src/java/disco/org/openlvc/disco/utils/LruCache.java @@ -0,0 +1,73 @@ +/* + * Copyright 2025 Open LVC Project. + * + * This file is part of Open LVC Disco. + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ +package org.openlvc.disco.utils; + +import java.util.LinkedHashMap; +import java.util.Map; + +// based on (not thread safe) implementation in https://stackoverflow.com/questions/40239485/concurrent-lru-cache-implementation + +/** + * An implementation of a Least Recently Used Cache (LRU Cache). + * Not thread safe; synchronization should be performed on the map when accessed. + */ +public class LruCache extends LinkedHashMap +{ + //---------------------------------------------------------- + // STATIC VARIABLES + //---------------------------------------------------------- + @java.io.Serial + private static final long serialVersionUID = 6228500846571341824L; + + //---------------------------------------------------------- + // INSTANCE VARIABLES + //---------------------------------------------------------- + private final int capacity; + + //---------------------------------------------------------- + // CONSTRUCTORS + //---------------------------------------------------------- + public LruCache( int capacity ) + { + // load factor of 1.0 so it doesn't auto resize + super( capacity + 1, 1.0f, true ); + this.capacity = capacity; + } + + //---------------------------------------------------------- + // INSTANCE METHODS + //---------------------------------------------------------- + + //////////////////////////////////////////////////////////////////////////////////////////// + /////////////////////////////// Accessor and Mutator Methods /////////////////////////////// + //////////////////////////////////////////////////////////////////////////////////////////// + public int getCapacity() + { + return this.capacity; + } + + @Override + protected boolean removeEldestEntry( final Map.Entry eldest ) + { + return super.size() > capacity; + } + + //---------------------------------------------------------- + // STATIC METHODS + //---------------------------------------------------------- +} diff --git a/codebase/src/java/test/org/openlvc/disco/application/EntityStateStoreTest.java b/codebase/src/java/test/org/openlvc/disco/application/EntityStateStoreTest.java index 3ace53ca..90ab45b4 100644 --- a/codebase/src/java/test/org/openlvc/disco/application/EntityStateStoreTest.java +++ b/codebase/src/java/test/org/openlvc/disco/application/EntityStateStoreTest.java @@ -21,11 +21,17 @@ import org.openlvc.disco.AbstractTest; import org.openlvc.disco.OpsCenter; +import org.openlvc.disco.application.pdu.DrEntityStatePdu; +import org.openlvc.disco.application.utils.DrmState; import org.openlvc.disco.configuration.DiscoConfiguration; import org.openlvc.disco.pdu.entity.EntityStatePdu; +import org.openlvc.disco.pdu.field.DeadReckoningAlgorithm; +import org.openlvc.disco.pdu.record.AngularVelocityVector; +import org.openlvc.disco.pdu.record.DeadReckoningParameter; import org.openlvc.disco.pdu.record.EntityId; import org.openlvc.disco.pdu.record.EntityType; import org.openlvc.disco.pdu.record.EulerAngles; +import org.openlvc.disco.pdu.record.VectorRecord; import org.openlvc.disco.pdu.record.WorldCoordinate; import org.testng.Assert; import org.testng.annotations.AfterClass; @@ -34,7 +40,7 @@ import org.testng.annotations.BeforeMethod; import org.testng.annotations.Test; -@Test(groups={"application"}) +@Test(groups={"application","entityStateStore"}) public class EntityStateStoreTest extends AbstractTest { //---------------------------------------------------------- @@ -122,7 +128,8 @@ public void testEntityStatePduStore() Assert.assertEquals( entityStore.size(), 1 ); // Check the entity values - EntityStatePdu received = entityStore.getEntityState("PhatEntity"); + DrEntityStatePdu received = entityStore.getEntityState("PhatEntity"); + Assert.assertTrue( received instanceof EntityStatePdu ); Assert.assertEquals( received.getEntityID(), entityId ); Assert.assertEquals( received.getEntityType(), entityType ); Assert.assertEquals( received.getLocation(), location ); @@ -157,6 +164,88 @@ public void testEntityStatePduStore() Assert.assertTrue( markings.contains("Ph@tEntity") ); } + @Test(dependsOnMethods={"testEntityStatePduStore"}) + public void testDrDisabled() + { + // create the "local" (dead-reckoning disabled) DisApplication we'll be testing + DiscoConfiguration drDisabledConfig = new DiscoConfiguration(); // need to also apply any changes that are made to localConfig + drDisabledConfig.setDeadReckoningEnabled( false ); + DisApplication drDisabledApp = new DisApplication( drDisabledConfig ); + drDisabledApp.start(); + + try + { + // setup the model + EntityStatePdu source = new EntityStatePdu(); + DeadReckoningParameter deadReckoningParams = new DeadReckoningParameter(); + + EntityId entityId = new EntityId( 12, 13, 14 ); + long localTimestamp = 10; + DeadReckoningAlgorithm deadReckoningAlgorithm = DeadReckoningAlgorithm.RVW; + + WorldCoordinate location = new WorldCoordinate( 41.0, 43.0, 47.0 ); + VectorRecord velocity = new VectorRecord( 10, 20, 30 ); + VectorRecord acceleration = new VectorRecord( 3, 5, 7 ); + + EulerAngles orientation = new EulerAngles( 0.0f, 0.1f, 0.0f ); + AngularVelocityVector angularVelocity = new AngularVelocityVector( 0.00f, 0.3f, 0.00f ); + + DrmState expectedInitialState = new DrmState( location, velocity, acceleration, orientation, angularVelocity ); + + source.setEntityID( entityId ); + source.setLocalTimestamp( localTimestamp ); + deadReckoningParams.setDeadReckoningAlgorithm( deadReckoningAlgorithm ); + + source.setLocation( location ); + source.setLinearVelocity( velocity ); + deadReckoningParams.setEntityLinearAcceleration( acceleration ); + + source.setOrientation( orientation ); + deadReckoningParams.setEntityAngularVelocity( angularVelocity ); + + source.setDeadReckoningParams( deadReckoningParams ); + + // check the stores + EntityStateStore drDisabledEntityStore = drDisabledApp.getPduStore().getEntityStore(); + EntityStateStore drEnabledEntityStore = local.getPduStore().getEntityStore(); + Assert.assertEquals( drDisabledEntityStore.size(), 0 ); + Assert.assertEquals( drEnabledEntityStore.size(), 0 ); + + // send it + remote.send( source ); + + // wait for the stores to pick the change up + super.waitFor( () -> drDisabledEntityStore.size() > 0, 100 ); + super.waitFor( () -> drEnabledEntityStore.size() > 0, 100 ); + + // get the DrEntityStatePdus + DrEntityStatePdu drDisabledPdu = drDisabledEntityStore.getEntityState( entityId ); + DrEntityStatePdu drEnabledPdu = drEnabledEntityStore.getEntityState( entityId ); + Assert.assertNotNull( drDisabledPdu ); + Assert.assertNotNull( drEnabledPdu ); + + // overwrite timestamp + drDisabledPdu.setLocalTimestamp( localTimestamp ); + drEnabledPdu.setLocalTimestamp( localTimestamp ); + + // check the state after 3s when enabled + long t3s = localTimestamp + 3000; + Assert.assertEquals( drEnabledPdu.getDrLocation(t3s), new WorldCoordinate(84.5, 125.5, 168.5) ); + Assert.assertEquals( drEnabledPdu.getDrLinearVelocity(t3s), new VectorRecord(19.0f, 35.0f, 51.0f) ); + Assert.assertEquals( drEnabledPdu.getDrOrientation(t3s), new EulerAngles(0.0f, 1.0f, 0.0f) ); + + // check the state after 3s when disabled + Assert.assertEquals( drDisabledPdu.getDrLocation(t3s), expectedInitialState.getLocation() ); + Assert.assertEquals( drDisabledPdu.getDrLinearVelocity(t3s), expectedInitialState.getLinearVelocity() ); + Assert.assertEquals( drDisabledPdu.getDrOrientation(t3s), expectedInitialState.getOrientation() ); + } + finally + { + // clean up + drDisabledApp.stop(); + } + } + //---------------------------------------------------------- // STATIC METHODS //---------------------------------------------------------- diff --git a/codebase/src/java/test/org/openlvc/disco/application/pdu/DrEntityStatePduTest.java b/codebase/src/java/test/org/openlvc/disco/application/pdu/DrEntityStatePduTest.java new file mode 100644 index 00000000..44720295 --- /dev/null +++ b/codebase/src/java/test/org/openlvc/disco/application/pdu/DrEntityStatePduTest.java @@ -0,0 +1,394 @@ +/* + * Copyright 2025 Open LVC Project. + * + * This file is part of Open LVC Disco. + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ +package org.openlvc.disco.application.pdu; + +import org.openlvc.disco.application.pdu.DrEntityStatePdu.OutdatedTimestampBehavior; +import org.openlvc.disco.application.utils.DrmState; +import org.openlvc.disco.pdu.entity.EntityStatePdu; +import org.openlvc.disco.pdu.field.DeadReckoningAlgorithm; +import org.openlvc.disco.pdu.record.AngularVelocityVector; +import org.openlvc.disco.pdu.record.DeadReckoningParameter; +import org.openlvc.disco.pdu.record.EulerAngles; +import org.openlvc.disco.pdu.record.VectorRecord; +import org.openlvc.disco.pdu.record.WorldCoordinate; +import org.testng.Assert; +import org.testng.annotations.AfterClass; +import org.testng.annotations.AfterMethod; +import org.testng.annotations.BeforeClass; +import org.testng.annotations.BeforeMethod; +import org.testng.annotations.Test; + +@Test(groups={"deadreckoning"}) +public class DrEntityStatePduTest +{ + //---------------------------------------------------------- + // STATIC VARIABLES + //---------------------------------------------------------- + + //---------------------------------------------------------- + // INSTANCE VARIABLES + //---------------------------------------------------------- + + //---------------------------------------------------------- + // CONSTRUCTORS + //---------------------------------------------------------- + + //---------------------------------------------------------- + // INSTANCE METHODS + //---------------------------------------------------------- + + /////////////////////////////////////////////////////////////////////////////////// + /// Test Class Setup/Tear Down ////////////////////////////////////////////////// + /////////////////////////////////////////////////////////////////////////////////// + @BeforeClass(alwaysRun = true) + public void beforeClass() + { + } + + @BeforeMethod(alwaysRun = true) + public void beforeMethod() + { + } + + @AfterMethod(alwaysRun = true) + public void afterMethod() + { + } + + @AfterClass(alwaysRun = true) + public void afterClass() + { + } + + /////////////////////////////////////////////////////////////////////////////////// + /// Testing Methods ///////////////////////////////////////////////////////////// + /////////////////////////////////////////////////////////////////////////////////// + @Test + public void testDrEntityRVW() + { + // setup the model + EntityStatePdu entityPdu = new EntityStatePdu(); + DeadReckoningParameter deadReckoningParams = new DeadReckoningParameter(); + + long localTimestamp = 10; + DeadReckoningAlgorithm deadReckoningAlgorithm = DeadReckoningAlgorithm.RVW; + + WorldCoordinate location = new WorldCoordinate( 41.0, 43.0, 47.0 ); + VectorRecord velocity = new VectorRecord( 10, 20, 30 ); + VectorRecord acceleration = new VectorRecord( 3, 5, 7 ); + + EulerAngles orientation = new EulerAngles( 0.0f, 0.1f, 0.0f ); + AngularVelocityVector angularVelocity = new AngularVelocityVector( 0.00f, 0.3f, 0.00f ); + + DrmState expectedInitialState = new DrmState( location, velocity, acceleration, orientation, angularVelocity ); + + entityPdu.setLocalTimestamp( localTimestamp ); + deadReckoningParams.setDeadReckoningAlgorithm( deadReckoningAlgorithm ); + + entityPdu.setLocation( location ); + entityPdu.setLinearVelocity( velocity ); + deadReckoningParams.setEntityLinearAcceleration( acceleration ); + + entityPdu.setOrientation( orientation ); + deadReckoningParams.setEntityAngularVelocity( angularVelocity ); + + entityPdu.setDeadReckoningParams( deadReckoningParams ); + + DrEntityStatePdu drEntityPdu = new DrEntityStatePdu( entityPdu, 2 ); + + // check the initial values are what we set + Assert.assertEquals( drEntityPdu.getDeadReckoningAlgorithm(), deadReckoningAlgorithm ); + Assert.assertEquals( drEntityPdu.getLocalTimestamp(), localTimestamp ); + Assert.assertEquals( drEntityPdu.getInitialDrmState(), expectedInitialState ); + + // check the state after 1s + DrmState drmState1s = drEntityPdu.getDrmStateAtLocalTime( drEntityPdu.getDeadReckoningAlgorithm(), localTimestamp + 1000 ); + Assert.assertEquals( drmState1s.getLocation(), new WorldCoordinate(52.5, 65.5, 80.5) ); + Assert.assertEquals( drmState1s.getLinearVelocity(), new VectorRecord(13, 25, 37) ); + Assert.assertEquals( drmState1s.getLinearAcceleration(), acceleration ); + Assert.assertEquals( drmState1s.getOrientation(), new EulerAngles(0.0f, 0.4f, 0.0f) ); + Assert.assertEquals( drmState1s.getAngularVelocity(), angularVelocity ); + + // check the state after 3s + DrmState drmState3s = drEntityPdu.getDrmStateAtLocalTime( drEntityPdu.getDeadReckoningAlgorithm(), localTimestamp + 3000 ); + Assert.assertEquals( drmState3s.getLocation(), new WorldCoordinate(84.5, 125.5, 168.5) ); + Assert.assertEquals( drmState3s.getLinearVelocity(), new VectorRecord(19.0f, 35.0f, 51.0f) ); + Assert.assertEquals( drmState3s.getLinearAcceleration(), acceleration ); + Assert.assertEquals( drmState3s.getOrientation(), new EulerAngles(0.0f, 1.0f, 0.0f) ); + Assert.assertEquals( drmState3s.getAngularVelocity(), angularVelocity ); + } + + @Test(dependsOnMethods={"testDrEntityRVW"}) + public void testDrEntityCaching() + { + // setup the model + EntityStatePdu entityPdu = new EntityStatePdu(); + DeadReckoningParameter deadReckoningParams = new DeadReckoningParameter(); + + long localTimestamp = 10; + DeadReckoningAlgorithm deadReckoningAlgorithm = DeadReckoningAlgorithm.RVW; + + WorldCoordinate location = new WorldCoordinate( 41.0, 43.0, 47.0 ); + VectorRecord velocity = new VectorRecord( 10, 20, 30 ); + VectorRecord acceleration = new VectorRecord( 3, 5, 7 ); + + EulerAngles orientation = new EulerAngles( 0.0f, -0.3f, 0.0f ); + AngularVelocityVector angularVelocity = new AngularVelocityVector( 0.00f, 0.3f, 0.00f ); + + entityPdu.setLocalTimestamp( localTimestamp ); + deadReckoningParams.setDeadReckoningAlgorithm( deadReckoningAlgorithm ); + + entityPdu.setLocation( location ); + entityPdu.setLinearVelocity( velocity ); + deadReckoningParams.setEntityLinearAcceleration( acceleration ); + + entityPdu.setOrientation( orientation ); + deadReckoningParams.setEntityAngularVelocity( angularVelocity ); + + entityPdu.setDeadReckoningParams( deadReckoningParams ); + + DrEntityStatePdu drEntityPdu = new DrEntityStatePdu( entityPdu, 2 ); + + // check the state after 1s + long t1s = localTimestamp + 1000; + DrmState drmState1s = drEntityPdu.getDrmStateAtLocalTime( drEntityPdu.getDeadReckoningAlgorithm(), t1s ); + Assert.assertEquals( drmState1s.getLocation(), new WorldCoordinate(52.5, 65.5, 80.5) ); + Assert.assertEquals( drmState1s.getLinearVelocity(), new VectorRecord(13, 25, 37) ); + Assert.assertEquals( drmState1s.getLinearAcceleration(), acceleration ); + Assert.assertEquals( drmState1s.getOrientation(), new EulerAngles(0.0f, 0.0f, 0.0f) ); + Assert.assertEquals( drmState1s.getAngularVelocity(), angularVelocity ); + + // check the state after 3s + long t3s = localTimestamp + 3000; + DrmState drmState3s = drEntityPdu.getDrmStateAtLocalTime( drEntityPdu.getDeadReckoningAlgorithm(), t3s ); + Assert.assertEquals( drmState3s.getLocation(), new WorldCoordinate(84.5, 125.5, 168.5) ); + Assert.assertEquals( drmState3s.getLinearVelocity(), new VectorRecord(19.0f, 35.0f, 51.0f) ); + Assert.assertEquals( drmState3s.getLinearAcceleration(), acceleration ); + Assert.assertEquals( drmState3s.getOrientation(), new EulerAngles(0.0f, 0.6f, 0.0f) ); + Assert.assertEquals( drmState3s.getAngularVelocity(), angularVelocity ); + + // check we get the same objects when fetching those times again (cache hits) + DrmState drmState1s_2 = drEntityPdu.getDrmStateAtLocalTime( drEntityPdu.getDeadReckoningAlgorithm(), t1s ); + Assert.assertTrue( drmState1s_2 == drmState1s ); + DrmState drmState3s_2 = drEntityPdu.getDrmStateAtLocalTime( drEntityPdu.getDeadReckoningAlgorithm(), t3s ); + Assert.assertTrue( drmState3s_2 == drmState3s ); + + // check the state after 5s + long t5s = localTimestamp + 5000; + DrmState drmState5s = drEntityPdu.getDrmStateAtLocalTime( drEntityPdu.getDeadReckoningAlgorithm(), t5s ); + Assert.assertEquals( drmState5s.getLocation(), new WorldCoordinate(128.5, 205.5, 284.5) ); + Assert.assertEquals( drmState5s.getLinearVelocity(), new VectorRecord(25, 45, 65) ); + Assert.assertEquals( drmState5s.getLinearAcceleration(), acceleration ); + Assert.assertEquals( drmState5s.getOrientation(), new EulerAngles(0.0f, 1.2f, 0.0f) ); + Assert.assertEquals( drmState5s.getAngularVelocity(), angularVelocity ); + + // check we get a cache miss when querying at 1s + DrmState drmState1s_3 = drEntityPdu.getDrmStateAtLocalTime( drEntityPdu.getDeadReckoningAlgorithm(), t1s ); + Assert.assertFalse( drmState1s_3 == drmState1s ); + // should still have the same values though + Assert.assertEquals( drmState1s_3, drmState1s ); + } + + @Test + public void testNoAlgorithm() + { + // create the test entity + EntityStatePdu entityPdu = new EntityStatePdu(); + DrEntityStatePdu drEntityPdu = new DrEntityStatePdu( entityPdu ); + + // verify the selected DRM is a Static model + Assert.assertEquals( drEntityPdu.getDeadReckoningAlgorithm(), DeadReckoningAlgorithm.Static ); + } + + @Test(dependsOnMethods={"testDrEntityRVW"}) + public void testOverrideNoAlgorithm() + { + // setup the model + EntityStatePdu entityPdu = new EntityStatePdu(); + DeadReckoningParameter deadReckoningParams = new DeadReckoningParameter(); + + long localTimestamp = 10; + DeadReckoningAlgorithm deadReckoningAlgorithm = DeadReckoningAlgorithm.RVW; + + WorldCoordinate location = new WorldCoordinate( 41.0, 43.0, 47.0 ); + VectorRecord velocity = new VectorRecord( 10, 20, 30 ); + VectorRecord acceleration = new VectorRecord( 3, 5, 7 ); + + EulerAngles orientation = new EulerAngles( 0.0f, 0.1f, 0.0f ); + AngularVelocityVector angularVelocity = new AngularVelocityVector( 0.00f, 0.3f, 0.00f ); + + DrmState expectedInitialState = new DrmState( location, velocity, acceleration, orientation, angularVelocity ); + + entityPdu.setLocalTimestamp( localTimestamp ); + deadReckoningParams.setDeadReckoningAlgorithm( deadReckoningAlgorithm ); + + entityPdu.setLocation( location ); + entityPdu.setLinearVelocity( velocity ); + deadReckoningParams.setEntityLinearAcceleration( acceleration ); + + entityPdu.setOrientation( orientation ); + deadReckoningParams.setEntityAngularVelocity( angularVelocity ); + + entityPdu.setDeadReckoningParams( deadReckoningParams ); + + DrEntityStatePdu drEntityPdu = new DrEntityStatePdu( entityPdu, 2 ); + + // check the initial values are what we set + Assert.assertEquals( drEntityPdu.getDeadReckoningAlgorithm(), deadReckoningAlgorithm ); + Assert.assertEquals( drEntityPdu.getLocalTimestamp(), localTimestamp ); + Assert.assertEquals( drEntityPdu.getInitialDrmState(), expectedInitialState ); + + // check the state after 1s + DrmState drmState1s = drEntityPdu.getDrmStateAtLocalTime( DeadReckoningAlgorithm.Static, localTimestamp + 1000 ); + Assert.assertEquals( drmState1s, expectedInitialState ); + + // check the state after 3s + DrmState drmState3s = drEntityPdu.getDrmStateAtLocalTime( DeadReckoningAlgorithm.Static, localTimestamp + 3000 ); + Assert.assertEquals( drmState3s, expectedInitialState ); + } + + @Test(dependsOnMethods={"testDrEntityRVW"}) + public void testDrDisabled() + { + // setup the model + EntityStatePdu entityPdu = new EntityStatePdu(); + DeadReckoningParameter deadReckoningParams = new DeadReckoningParameter(); + + long localTimestamp = 10; + DeadReckoningAlgorithm deadReckoningAlgorithm = DeadReckoningAlgorithm.RVW; + + WorldCoordinate location = new WorldCoordinate( 41.0, 43.0, 47.0 ); + VectorRecord velocity = new VectorRecord( 10, 20, 30 ); + VectorRecord acceleration = new VectorRecord( 3, 5, 7 ); + + EulerAngles orientation = new EulerAngles( 0.0f, 0.1f, 0.0f ); + AngularVelocityVector angularVelocity = new AngularVelocityVector( 0.00f, 0.3f, 0.00f ); + + DrmState expectedInitialState = new DrmState( location, velocity, acceleration, orientation, angularVelocity ); + + entityPdu.setLocalTimestamp( localTimestamp ); + deadReckoningParams.setDeadReckoningAlgorithm( deadReckoningAlgorithm ); + + entityPdu.setLocation( location ); + entityPdu.setLinearVelocity( velocity ); + deadReckoningParams.setEntityLinearAcceleration( acceleration ); + + entityPdu.setOrientation( orientation ); + deadReckoningParams.setEntityAngularVelocity( angularVelocity ); + + entityPdu.setDeadReckoningParams( deadReckoningParams ); + + DrEntityStatePdu drEntityPdu = DrEntityStatePdu.makeWithoutDr( entityPdu ); + + // check the state after 3s + DrmState drmState3s = drEntityPdu.getDrmStateAtLocalTime( drEntityPdu.getDeadReckoningAlgorithm(), localTimestamp + 3000 ); + Assert.assertEquals( drmState3s, expectedInitialState ); + } + + @Test(dependsOnMethods={"testDrEntityRVW"}) + public void testFrozenEntity() + { + // setup the model + EntityStatePdu entityPdu = new EntityStatePdu(); + DeadReckoningParameter deadReckoningParams = new DeadReckoningParameter(); + + long localTimestamp = 10; + DeadReckoningAlgorithm deadReckoningAlgorithm = DeadReckoningAlgorithm.RVW; + + WorldCoordinate location = new WorldCoordinate( 41.0, 43.0, 47.0 ); + VectorRecord velocity = new VectorRecord( 10, 20, 30 ); + VectorRecord acceleration = new VectorRecord( 3, 5, 7 ); + + EulerAngles orientation = new EulerAngles( 0.0f, 0.1f, 0.0f ); + AngularVelocityVector angularVelocity = new AngularVelocityVector( 0.00f, 0.3f, 0.00f ); + + DrmState expectedInitialState = new DrmState( location, velocity, acceleration, orientation, angularVelocity ); + + entityPdu.setLocalTimestamp( localTimestamp ); + deadReckoningParams.setDeadReckoningAlgorithm( deadReckoningAlgorithm ); + + entityPdu.setLocation( location ); + entityPdu.setLinearVelocity( velocity ); + deadReckoningParams.setEntityLinearAcceleration( acceleration ); + + entityPdu.setOrientation( orientation ); + deadReckoningParams.setEntityAngularVelocity( angularVelocity ); + + entityPdu.setDeadReckoningParams( deadReckoningParams ); + + DrEntityStatePdu drEntityPdu = new DrEntityStatePdu( entityPdu ); + + // check the state after 3s + DrmState drmState3s = drEntityPdu.getDrmStateAtLocalTime( drEntityPdu.getDeadReckoningAlgorithm(), localTimestamp + 3000 ); + Assert.assertEquals( drmState3s.getLocation(), new WorldCoordinate(84.5, 125.5, 168.5) ); + Assert.assertEquals( drmState3s.getLinearVelocity(), new VectorRecord(19.0f, 35.0f, 51.0f) ); + Assert.assertEquals( drmState3s.getLinearAcceleration(), acceleration ); + Assert.assertEquals( drmState3s.getOrientation(), new EulerAngles(0.0f, 1.0f, 0.0f) ); + Assert.assertEquals( drmState3s.getAngularVelocity(), angularVelocity ); + + drEntityPdu.setFrozen( true ); + + // check the state after 3s when frozen + DrmState drmState3sFrozen = drEntityPdu.getDrmStateAtLocalTime( drEntityPdu.getDeadReckoningAlgorithm(), localTimestamp + 3000 ); + Assert.assertEquals( drmState3sFrozen, expectedInitialState ); + } + + @Test + public void testOutdatedTimestampBehavior() + { + EntityStatePdu entityPdu = new EntityStatePdu(); + DeadReckoningParameter deadReckoningParams = new DeadReckoningParameter(); + + long localTimestamp = 10; + WorldCoordinate location = new WorldCoordinate( 41.0, 43.0, 47.0 ); + VectorRecord velocity = new VectorRecord( 10, 20, 30 ); + DeadReckoningAlgorithm deadReckoningAlgorithm = DeadReckoningAlgorithm.FPW; // velocity only + + entityPdu.setLocalTimestamp( localTimestamp ); + entityPdu.setLocation( location ); + entityPdu.setLinearVelocity( velocity ); + + deadReckoningParams.setDeadReckoningAlgorithm( deadReckoningAlgorithm ); + entityPdu.setDeadReckoningParams( deadReckoningParams ); + + DrEntityStatePdu drEntityPdu = new DrEntityStatePdu( entityPdu ); + + // verify the DR model is setup correctly + Assert.assertEquals( drEntityPdu.getDrLocation(localTimestamp), location ); + Assert.assertEquals( drEntityPdu.getDrLocation(localTimestamp + 5000), new WorldCoordinate(91, 143, 197) ); + + // default behavior should be to error + long oldTimestamp = localTimestamp - 5; + Assert.assertEquals( drEntityPdu.getOutdatedTimestampBehavior(), OutdatedTimestampBehavior.ERROR ); + Assert.assertThrows( IllegalArgumentException.class, () -> drEntityPdu.getDrLocation(oldTimestamp) ); + + // we should be able to make it use the latest instead + drEntityPdu.setOutdatedTimestampBehavior( OutdatedTimestampBehavior.USE_CURRENT_STATE ); + Assert.assertEquals( drEntityPdu.getOutdatedTimestampBehavior(), OutdatedTimestampBehavior.USE_CURRENT_STATE ); + Assert.assertEquals( drEntityPdu.getDrLocation(oldTimestamp), location ); + + // and we should be able to make it go back to erroring + drEntityPdu.setOutdatedTimestampBehavior( OutdatedTimestampBehavior.ERROR ); + Assert.assertEquals( drEntityPdu.getOutdatedTimestampBehavior(), OutdatedTimestampBehavior.ERROR ); + Assert.assertThrows( IllegalArgumentException.class, () -> drEntityPdu.getDrLocation(oldTimestamp) ); + } + + //---------------------------------------------------------- + // STATIC METHODS + //---------------------------------------------------------- +} diff --git a/codebase/src/java/test/org/openlvc/disco/pdu/field/DeadReckoningAlgorithmTest.java b/codebase/src/java/test/org/openlvc/disco/pdu/field/DeadReckoningAlgorithmTest.java new file mode 100644 index 00000000..5ffd8319 --- /dev/null +++ b/codebase/src/java/test/org/openlvc/disco/pdu/field/DeadReckoningAlgorithmTest.java @@ -0,0 +1,286 @@ +/* + * Copyright 2025 Open LVC Project. + * + * This file is part of Open LVC Disco. + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ +package org.openlvc.disco.pdu.field; + +import java.util.Arrays; +import java.util.Iterator; +import java.util.Random; + +import org.openlvc.disco.application.utils.DrmState; +import org.openlvc.disco.pdu.record.AngularVelocityVector; +import org.openlvc.disco.pdu.record.EulerAngles; +import org.openlvc.disco.pdu.record.VectorRecord; +import org.openlvc.disco.pdu.record.WorldCoordinate; +import org.openlvc.disco.utils.Quaternion; +import org.openlvc.disco.utils.Vec3; +import org.testng.Assert; +import org.testng.annotations.AfterClass; +import org.testng.annotations.AfterMethod; +import org.testng.annotations.BeforeClass; +import org.testng.annotations.BeforeMethod; +import org.testng.annotations.DataProvider; +import org.testng.annotations.Test; +import org.testng.internal.EclipseInterface; + +@Test(groups={"deadreckoning"}) +public class DeadReckoningAlgorithmTest +{ + //---------------------------------------------------------- + // STATIC VARIABLES + //---------------------------------------------------------- + + //---------------------------------------------------------- + // INSTANCE VARIABLES + //---------------------------------------------------------- + + //---------------------------------------------------------- + // CONSTRUCTORS + //---------------------------------------------------------- + + //---------------------------------------------------------- + // INSTANCE METHODS + //---------------------------------------------------------- + + /////////////////////////////////////////////////////////////////////////////////// + /// Test Class Setup/Tear Down ////////////////////////////////////////////////// + /////////////////////////////////////////////////////////////////////////////////// + @BeforeClass(alwaysRun = true) + public void beforeClass() + { + } + + @BeforeMethod(alwaysRun = true) + public void beforeMethod() + { + } + + @AfterMethod(alwaysRun = true) + public void afterMethod() + { + } + + @AfterClass(alwaysRun = true) + public void afterClass() + { + } + + /////////////////////////////////////////////////////////////////////////////////// + /// Testing Methods ///////////////////////////////////////////////////////////// + /////////////////////////////////////////////////////////////////////////////////// + @Test + public void testRVW() + { + // set up the initial state + WorldCoordinate location = new WorldCoordinate( 41.0, 43.0, 47.0 ); + VectorRecord velocity = new VectorRecord( 10, 20, 30 ); + VectorRecord acceleration = new VectorRecord( 3, 5, 7 ); + + EulerAngles orientation = new EulerAngles( 0.0f, 0.1f, 0.0f ); + AngularVelocityVector angularVelocity = new AngularVelocityVector( 0.00f, 0.3f, 0.00f ); + + DrmState initialState = new DrmState( location, velocity, acceleration, orientation, angularVelocity ); + + // check the state after 3s + DrmState newState = DeadReckoningAlgorithm.RVW.computeStateAfter( initialState, 3 ); + Assert.assertEquals( newState.getLocation(), new WorldCoordinate(84.5, 125.5, 168.5) ); + Assert.assertEquals( newState.getLinearVelocity(), new VectorRecord(19.0f, 35.0f, 51.0f) ); + Assert.assertEquals( newState.getLinearAcceleration(), acceleration ); + Assert.assertEquals( newState.getOrientation(), new EulerAngles(0.0f, 1.0f, 0.0f) ); + Assert.assertEquals( newState.getAngularVelocity(), angularVelocity ); + } + + @Test(dataProvider="axisRotations") + public void testRVWRotation( RotationTestSet rotTest ) + { + // set up the initial state + WorldCoordinate location = new WorldCoordinate( 41.0, 43.0, 47.0 ); + VectorRecord velocity = new VectorRecord( 0, 0, 0 ); + VectorRecord acceleration = new VectorRecord( 0, 0, 0 ); + + DrmState initialState = new DrmState( location, velocity, acceleration, rotTest.initialOrientation, rotTest.angularVelocity ); + + // check the state after 1s + DrmState dtState1s = DeadReckoningAlgorithm.RVW.computeStateAfter( initialState, 1.0 ); + Assert.assertEquals( dtState1s, new DrmState(location, velocity, acceleration, rotTest.outOrientation1s, rotTest.angularVelocity) ); + + // check the state after 3s + DrmState dtState3s = DeadReckoningAlgorithm.RVW.computeStateAfter( initialState, 3.0 ); + Assert.assertEquals( dtState3s, new DrmState(location, velocity, acceleration, rotTest.outOrientation3s, rotTest.angularVelocity) ); + } + + @Test + public void testRVBCircularBasic() + { + testRVBCircular( new CircularTestParams(11, 3, new WorldCoordinate(5, 13, 17), new EulerAngles()) ); + // lap 1.5: pos approx <5, 7, 17>, orientation psi approx +/- PI (i.e. 2k*PI +/- PI) + } + + @Test(dataProvider="circleParams_random100") + public void testRVBCircularRandom( CircularTestParams testParams ) + { + testRVBCircular( testParams ); + } + + private void testRVBCircular( CircularTestParams testParams ) + { + // circular motion parameters + double period = testParams.period, radius = testParams.radius; // seconds, metres + + double w = Math.TAU / period; // angular velocity w.r.t. circle centre - also angular velocity of entity (in -z, for left-hand turn) + double v = w * radius; // forwards velocity (in +x) + double a = v * w; // perpendicular acceleration (in -y, for left-hand turn) + + // set up the initial state + WorldCoordinate location = testParams.location; + VectorRecord velocity = new VectorRecord( (float)v, 0, 0 ); + VectorRecord acceleration = new VectorRecord( 0, (float)-a, 0 ); + + EulerAngles orientation = testParams.orientation; + AngularVelocityVector angularVelocity = new AngularVelocityVector( 0, 0, (float)-w ); + + DrmState initialState = new DrmState( location, velocity, acceleration, orientation, angularVelocity ); + + final double errTolerance = 0.0001; // tolerate 0.01% error + + // check the state after 1 lap - should be (approx) back at the start - tests show this has an error usually not exceeding 10^-6 (~0.0001% error) + DrmState newState1x = DeadReckoningAlgorithm.RVB.computeStateAfter( initialState, period ); + Assert.assertEquals( newState1x.position().distance(initialState.position()) / radius, // error + 0, + errTolerance, + failFormat(newState1x.position(), initialState.position()) ); + + Assert.assertEquals( newState1x.getOrientation(), orientation ); + Assert.assertEquals( newState1x.velocity(), initialState.velocity() ); + Assert.assertEquals( newState1x.acceleration(), initialState.acceleration() ); + Assert.assertEquals( newState1x.angularVelocity(), initialState.angularVelocity() ); + + // check the state after 1.5 laps - should be (approx) offset by diameter and rotated halfway around the axis of the circle + DrmState newState1_5x = DeadReckoningAlgorithm.RVB.computeStateAfter( initialState, period * 1.5 ); + Vec3 halfLapPosition = new Vec3( location ).add( new Vec3( 0, -2 * radius, 0 ).rotate( Quaternion.fromPduEulerAngles(orientation) ) ); + Assert.assertEquals( newState1_5x.position().distance( halfLapPosition ) / radius, // error + 0, + errTolerance, + failFormat(newState1_5x.position(), halfLapPosition) ); + + Quaternion halfLapOrientation = Quaternion.fromPduEulerAngles( orientation ).multiply( Quaternion.fromPduEulerAngles(new EulerAngles((float)Math.PI, 0, 0)) ); + Assert.assertEquals( newState1_5x.getOrientation(), halfLapOrientation.toPduEulerAngles() ); + Assert.assertEquals( newState1_5x.velocity(), initialState.velocity() ); + Assert.assertEquals( newState1_5x.acceleration(), initialState.acceleration() ); + Assert.assertEquals( newState1_5x.angularVelocity(), initialState.angularVelocity() ); + + // check the state after 5 laps - should be (approx) back at the start + DrmState newState5x = DeadReckoningAlgorithm.RVB.computeStateAfter( initialState, period * 5 ); + Assert.assertEquals( newState5x.position().distance(initialState.position()) / radius, // error + 0, + errTolerance, + failFormat(newState5x.position(), initialState.position()) ); + + // error has accumulated larger than the float helper equality bounds - manually set the tolerance + EulerAngles newOrientation5x = newState5x.getOrientation(); + Assert.assertEquals( newOrientation5x.getPsi(), orientation.getPsi(), errTolerance * 3 ); + Assert.assertEquals( newOrientation5x.getTheta(), orientation.getTheta(), errTolerance * 1.5 ); + Assert.assertEquals( newOrientation5x.getPhi(), orientation.getPhi(), errTolerance * 3 ); + + Assert.assertEquals( newState5x.velocity(), initialState.velocity() ); + Assert.assertEquals( newState5x.acceleration(), initialState.acceleration() ); + Assert.assertEquals( newState5x.angularVelocity(), initialState.angularVelocity() ); + } + + //---------------------------------------------------------- + // STATIC METHODS + //---------------------------------------------------------- + private static String failFormat( Object actual, Object expected ) + { + return "values: " + EclipseInterface.ASSERT_EQUAL_LEFT + expected + EclipseInterface.ASSERT_MIDDLE + actual + EclipseInterface.ASSERT_RIGHT + ", test: "; + } + + private static record RotationTestSet( EulerAngles initialOrientation, + AngularVelocityVector angularVelocity, + EulerAngles outOrientation1s, + EulerAngles outOrientation3s ) + { + } + + private static record CircularTestParams( double period, + double radius, + WorldCoordinate location, + EulerAngles orientation ) + { + } + + /////////////////////////////////////////////////////////////////////////////////// + /// Data Providers ////////////////////////////////////////////////////////////// + /////////////////////////////////////////////////////////////////////////////////// + @DataProvider(name="axisRotations",parallel=true) + public static Iterator bounds() + { + return Arrays.asList( new RotationTestSet( new EulerAngles( 0.0f, 0.0f, 0.1f ), + new AngularVelocityVector( 0.3f, 0.0f, 0.0f ), + new EulerAngles( 0.0f, 0.0f, 0.4f ), + new EulerAngles( 0.0f, 0.0f, 1.0f ) ), + + new RotationTestSet( new EulerAngles( 0.0f, 0.1f, 0.0f ), + new AngularVelocityVector( 0.0f, 0.3f, 0.0f ), + new EulerAngles( 0.0f, 0.4f, 0.0f ), + new EulerAngles( 0.0f, 1.0f, 0.0f ) ), + + new RotationTestSet( new EulerAngles( 0.1f, 0.0f, 0.0f ), + new AngularVelocityVector( 0.0f, 0.0f, 0.3f ), + new EulerAngles( 0.4f, 0.0f, 0.0f ), + new EulerAngles( 1.0f, 0.0f, 0.0f ) ) ).iterator(); + } + + @DataProvider(name="circleParams_random100",parallel=true) + public static Iterator circleParams() + { + return new Iterator() { + // arbitrary axis bounds + static final float SPATIAL_AXIS_MIN = -10 * 1000; // 10km + static final float SPATIAL_AXIS_MAX = 1000 * 1000; // 1000km + + final Random rand = new Random(); + int i = 0; + + @Override + public boolean hasNext() + { + return i < 100; + } + + @Override + public CircularTestParams next() + { + i++; + + double period = rand.nextDouble( 0.1, 7200 ); // 0.1s - 2h + double radius = rand.nextDouble( 0.1, 100000 ); // 10cm - 100km + + float x = rand.nextFloat( SPATIAL_AXIS_MIN, SPATIAL_AXIS_MAX ); + float y = rand.nextFloat( SPATIAL_AXIS_MIN, SPATIAL_AXIS_MAX ); + float z = rand.nextFloat( SPATIAL_AXIS_MIN, SPATIAL_AXIS_MAX ); + + float psi = rand.nextFloat( EulerAngles.PSI_MIN, EulerAngles.PSI_MAX ); + float theta = rand.nextFloat( EulerAngles.THETA_MIN * (86.3f / 90), + EulerAngles.THETA_MAX * (86.3f / 90) ); // prevent singularities + float phi = rand.nextFloat( EulerAngles.PHI_MIN, EulerAngles.PHI_MAX ); + + return new CircularTestParams( period, radius, new WorldCoordinate(x, y, z), new EulerAngles(psi, theta, phi) ); + } + }; + } +} From ac603a04ca46eb11f14e3ee08b7613409dfdf728 Mon Sep 17 00:00:00 2001 From: Nathan Townshend Date: Fri, 7 Nov 2025 15:01:44 +0800 Subject: [PATCH 5/8] FIXUP - Formatting changes --- .../disco/application/utils/DrmState.java | 12 +++++------ .../pdu/field/DeadReckoningAlgorithm.java | 6 +++--- .../application/pdu/DrEntityStatePduTest.java | 21 ++++++++++++------- .../pdu/field/DeadReckoningAlgorithmTest.java | 13 +++++++++--- 4 files changed, 32 insertions(+), 20 deletions(-) diff --git a/codebase/src/java/disco/org/openlvc/disco/application/utils/DrmState.java b/codebase/src/java/disco/org/openlvc/disco/application/utils/DrmState.java index 23b3499a..11c0e2b4 100644 --- a/codebase/src/java/disco/org/openlvc/disco/application/utils/DrmState.java +++ b/codebase/src/java/disco/org/openlvc/disco/application/utils/DrmState.java @@ -19,7 +19,6 @@ import java.util.Objects; -import org.openlvc.disco.DiscoException; import org.openlvc.disco.pdu.record.AngularVelocityVector; import org.openlvc.disco.pdu.record.EulerAngles; import org.openlvc.disco.pdu.record.VectorRecord; @@ -28,19 +27,18 @@ import org.openlvc.disco.utils.Vec3; /** - * Represents the state of an entity at a point in time in a {@link DeadReckoningModel}.
- *
+ * Represents the state of an entity at a point in time in a {@link DeadReckoningModel}. + *

* The reference frame of the {@link #velocity} and {@link #acceleration} vectors can vary, and - * uses the coordinate system of the model that produced this state.
- *
+ * uses the coordinate system of the model that produced this state. + *

* Complete list of reference frames: *

    *
  • {@link #position}: world coordinate system (see also: {@link WorldCoordinate})
    • *
  • {@link #velocity}: varies depending on DR model
    • *
  • {@link #acceleration}: varies depending on DR model
    • *
  • {@link #orientation}: world coordinate system (see also: {@link EulerAngles})
    • - *
  • {@link #angularVelocity}: entity coordinate system (see also: - * {@link AngularVelocityVector})
    • + *
  • {@link #angularVelocity}: entity coordinate system (see also: {@link AngularVelocityVector})
    • *
*/ public record DrmState( Vec3 position, diff --git a/codebase/src/java/disco/org/openlvc/disco/pdu/field/DeadReckoningAlgorithm.java b/codebase/src/java/disco/org/openlvc/disco/pdu/field/DeadReckoningAlgorithm.java index a96a9115..f4d6248b 100644 --- a/codebase/src/java/disco/org/openlvc/disco/pdu/field/DeadReckoningAlgorithm.java +++ b/codebase/src/java/disco/org/openlvc/disco/pdu/field/DeadReckoningAlgorithm.java @@ -245,7 +245,7 @@ private static DrmState computeFixedStateAfter( DeadReckoningAlgorithm algorithm dv.multiply( dt ); if( velocity.isEmpty() ) - velocity = Optional.of( new Vec3( initialState.velocity() ) ); + velocity = Optional.of( new Vec3(initialState.velocity()) ); velocity.get().add( dv ); // displacement @@ -253,7 +253,7 @@ private static DrmState computeFixedStateAfter( DeadReckoningAlgorithm algorithm disp.multiply( dt / 2.0 ); if( position.isEmpty() ) - position = Optional.of( new Vec3( initialState.position() ) ); + position = Optional.of( new Vec3(initialState.position()) ); position.get().add( disp ); break; @@ -370,7 +370,7 @@ private static DrmState computeRotatingStateAfter( DeadReckoningAlgorithm algori Vec3 disp = R_2.multiply( A_b ).rotate( initialState.orientation() ); if( position.isEmpty() ) - position = Optional.of( new Vec3( initialState.position() ) ); + position = Optional.of( new Vec3(initialState.position()) ); position.get().add( disp ); break; diff --git a/codebase/src/java/test/org/openlvc/disco/application/pdu/DrEntityStatePduTest.java b/codebase/src/java/test/org/openlvc/disco/application/pdu/DrEntityStatePduTest.java index 44720295..bf978721 100644 --- a/codebase/src/java/test/org/openlvc/disco/application/pdu/DrEntityStatePduTest.java +++ b/codebase/src/java/test/org/openlvc/disco/application/pdu/DrEntityStatePduTest.java @@ -117,7 +117,8 @@ public void testDrEntityRVW() Assert.assertEquals( drEntityPdu.getInitialDrmState(), expectedInitialState ); // check the state after 1s - DrmState drmState1s = drEntityPdu.getDrmStateAtLocalTime( drEntityPdu.getDeadReckoningAlgorithm(), localTimestamp + 1000 ); + DrmState drmState1s = drEntityPdu.getDrmStateAtLocalTime( drEntityPdu.getDeadReckoningAlgorithm(), + localTimestamp + 1000 ); Assert.assertEquals( drmState1s.getLocation(), new WorldCoordinate(52.5, 65.5, 80.5) ); Assert.assertEquals( drmState1s.getLinearVelocity(), new VectorRecord(13, 25, 37) ); Assert.assertEquals( drmState1s.getLinearAcceleration(), acceleration ); @@ -125,7 +126,8 @@ public void testDrEntityRVW() Assert.assertEquals( drmState1s.getAngularVelocity(), angularVelocity ); // check the state after 3s - DrmState drmState3s = drEntityPdu.getDrmStateAtLocalTime( drEntityPdu.getDeadReckoningAlgorithm(), localTimestamp + 3000 ); + DrmState drmState3s = drEntityPdu.getDrmStateAtLocalTime( drEntityPdu.getDeadReckoningAlgorithm(), + localTimestamp + 3000 ); Assert.assertEquals( drmState3s.getLocation(), new WorldCoordinate(84.5, 125.5, 168.5) ); Assert.assertEquals( drmState3s.getLinearVelocity(), new VectorRecord(19.0f, 35.0f, 51.0f) ); Assert.assertEquals( drmState3s.getLinearAcceleration(), acceleration ); @@ -254,11 +256,13 @@ public void testOverrideNoAlgorithm() Assert.assertEquals( drEntityPdu.getInitialDrmState(), expectedInitialState ); // check the state after 1s - DrmState drmState1s = drEntityPdu.getDrmStateAtLocalTime( DeadReckoningAlgorithm.Static, localTimestamp + 1000 ); + DrmState drmState1s = drEntityPdu.getDrmStateAtLocalTime( DeadReckoningAlgorithm.Static, + localTimestamp + 1000 ); Assert.assertEquals( drmState1s, expectedInitialState ); // check the state after 3s - DrmState drmState3s = drEntityPdu.getDrmStateAtLocalTime( DeadReckoningAlgorithm.Static, localTimestamp + 3000 ); + DrmState drmState3s = drEntityPdu.getDrmStateAtLocalTime( DeadReckoningAlgorithm.Static, + localTimestamp + 3000 ); Assert.assertEquals( drmState3s, expectedInitialState ); } @@ -296,7 +300,8 @@ public void testDrDisabled() DrEntityStatePdu drEntityPdu = DrEntityStatePdu.makeWithoutDr( entityPdu ); // check the state after 3s - DrmState drmState3s = drEntityPdu.getDrmStateAtLocalTime( drEntityPdu.getDeadReckoningAlgorithm(), localTimestamp + 3000 ); + DrmState drmState3s = drEntityPdu.getDrmStateAtLocalTime( drEntityPdu.getDeadReckoningAlgorithm(), + localTimestamp + 3000 ); Assert.assertEquals( drmState3s, expectedInitialState ); } @@ -334,7 +339,8 @@ public void testFrozenEntity() DrEntityStatePdu drEntityPdu = new DrEntityStatePdu( entityPdu ); // check the state after 3s - DrmState drmState3s = drEntityPdu.getDrmStateAtLocalTime( drEntityPdu.getDeadReckoningAlgorithm(), localTimestamp + 3000 ); + DrmState drmState3s = drEntityPdu.getDrmStateAtLocalTime( drEntityPdu.getDeadReckoningAlgorithm(), + localTimestamp + 3000 ); Assert.assertEquals( drmState3s.getLocation(), new WorldCoordinate(84.5, 125.5, 168.5) ); Assert.assertEquals( drmState3s.getLinearVelocity(), new VectorRecord(19.0f, 35.0f, 51.0f) ); Assert.assertEquals( drmState3s.getLinearAcceleration(), acceleration ); @@ -344,7 +350,8 @@ public void testFrozenEntity() drEntityPdu.setFrozen( true ); // check the state after 3s when frozen - DrmState drmState3sFrozen = drEntityPdu.getDrmStateAtLocalTime( drEntityPdu.getDeadReckoningAlgorithm(), localTimestamp + 3000 ); + DrmState drmState3sFrozen = drEntityPdu.getDrmStateAtLocalTime( drEntityPdu.getDeadReckoningAlgorithm(), + localTimestamp + 3000 ); Assert.assertEquals( drmState3sFrozen, expectedInitialState ); } diff --git a/codebase/src/java/test/org/openlvc/disco/pdu/field/DeadReckoningAlgorithmTest.java b/codebase/src/java/test/org/openlvc/disco/pdu/field/DeadReckoningAlgorithmTest.java index 5ffd8319..6dda0a6e 100644 --- a/codebase/src/java/test/org/openlvc/disco/pdu/field/DeadReckoningAlgorithmTest.java +++ b/codebase/src/java/test/org/openlvc/disco/pdu/field/DeadReckoningAlgorithmTest.java @@ -171,13 +171,14 @@ private void testRVBCircular( CircularTestParams testParams ) // check the state after 1.5 laps - should be (approx) offset by diameter and rotated halfway around the axis of the circle DrmState newState1_5x = DeadReckoningAlgorithm.RVB.computeStateAfter( initialState, period * 1.5 ); - Vec3 halfLapPosition = new Vec3( location ).add( new Vec3( 0, -2 * radius, 0 ).rotate( Quaternion.fromPduEulerAngles(orientation) ) ); + Vec3 halfLapPosition = new Vec3( location ).add( new Vec3(0, -2 * radius, 0).rotate(Quaternion.fromPduEulerAngles(orientation)) ); Assert.assertEquals( newState1_5x.position().distance( halfLapPosition ) / radius, // error 0, errTolerance, failFormat(newState1_5x.position(), halfLapPosition) ); - Quaternion halfLapOrientation = Quaternion.fromPduEulerAngles( orientation ).multiply( Quaternion.fromPduEulerAngles(new EulerAngles((float)Math.PI, 0, 0)) ); + Quaternion halfLapOrientation = Quaternion.fromPduEulerAngles( orientation ) + .multiply( Quaternion.fromPduEulerAngles(new EulerAngles((float)Math.PI, 0, 0)) ); Assert.assertEquals( newState1_5x.getOrientation(), halfLapOrientation.toPduEulerAngles() ); Assert.assertEquals( newState1_5x.velocity(), initialState.velocity() ); Assert.assertEquals( newState1_5x.acceleration(), initialState.acceleration() ); @@ -206,7 +207,13 @@ private void testRVBCircular( CircularTestParams testParams ) //---------------------------------------------------------- private static String failFormat( Object actual, Object expected ) { - return "values: " + EclipseInterface.ASSERT_EQUAL_LEFT + expected + EclipseInterface.ASSERT_MIDDLE + actual + EclipseInterface.ASSERT_RIGHT + ", test: "; + return "values: " + + EclipseInterface.ASSERT_EQUAL_LEFT + + expected + + EclipseInterface.ASSERT_MIDDLE + + actual + + EclipseInterface.ASSERT_RIGHT + + ", test: "; } private static record RotationTestSet( EulerAngles initialOrientation, From 815b092b859bd563074f63ee4880a56cc449c765 Mon Sep 17 00:00:00 2001 From: Nathan Townshend Date: Tue, 25 Nov 2025 18:27:39 +0800 Subject: [PATCH 6/8] FIXUP - Formatting changes and suggested tweaks --- .../disco/application/EntityStateStore.java | 60 ++++++++- .../application/pdu/DrEntityStatePdu.java | 125 +++++++++--------- .../disco/application/utils/DrmState.java | 29 ++-- .../application/pdu/DrEntityStatePduTest.java | 40 +++--- 4 files changed, 161 insertions(+), 93 deletions(-) diff --git a/codebase/src/java/disco/org/openlvc/disco/application/EntityStateStore.java b/codebase/src/java/disco/org/openlvc/disco/application/EntityStateStore.java index 301d5faa..1c77efbb 100644 --- a/codebase/src/java/disco/org/openlvc/disco/application/EntityStateStore.java +++ b/codebase/src/java/disco/org/openlvc/disco/application/EntityStateStore.java @@ -22,12 +22,13 @@ import java.util.concurrent.ConcurrentHashMap; import java.util.concurrent.ConcurrentMap; import java.util.concurrent.atomic.AtomicInteger; -import java.util.function.BooleanSupplier; import java.util.stream.Collectors; import org.openlvc.disco.DiscoException; import org.openlvc.disco.application.pdu.DrEntityStatePdu; +import org.openlvc.disco.application.utils.DrmState; import org.openlvc.disco.pdu.entity.EntityStatePdu; +import org.openlvc.disco.pdu.field.DeadReckoningAlgorithm; import org.openlvc.disco.pdu.record.EntityId; import org.openlvc.disco.pdu.record.WorldCoordinate; @@ -47,22 +48,26 @@ public class EntityStateStore implements IDeleteReaperManaged private ConcurrentMap byId; private ConcurrentMap byMarking; - private BooleanSupplier isDrEnabled; + private PduStore parentStore; //---------------------------------------------------------- // CONSTRUCTORS //---------------------------------------------------------- - protected EntityStateStore( PduStore store ) + protected EntityStateStore( PduStore parentStore ) { this.byId = new ConcurrentHashMap<>(); this.byMarking = new ConcurrentHashMap<>(); - this.isDrEnabled = store.app.getConfiguration()::getDeadReckoningEnabled; + this.parentStore = parentStore; } //---------------------------------------------------------- // INSTANCE METHODS //---------------------------------------------------------- + private boolean isDrEnabled() + { + return this.parentStore.app.getConfiguration().getDeadReckoningEnabled(); + } //////////////////////////////////////////////////////////////////////////////////////////// /// PDU Processing /////////////////////////////////////////////////////////////////////// @@ -70,8 +75,8 @@ protected EntityStateStore( PduStore store ) protected void receivePdu( EntityStatePdu pdu ) { // wrap the PDU to provide dead-reckoning support - DrEntityStatePdu wrappedPdu = this.isDrEnabled.getAsBoolean() ? new DrEntityStatePdu( pdu ) - : DrEntityStatePdu.makeWithoutDr( pdu ); + DrEntityStatePdu wrappedPdu = this.isDrEnabled() ? new DrEntityStatePdu( pdu ) + : new DisabledDrEntityStatePdu( pdu ); // bang the entity into the ID indexed store DrEntityStatePdu existing = byId.put( wrappedPdu.getEntityID(), wrappedPdu ); @@ -241,4 +246,47 @@ public int size() //---------------------------------------------------------- // STATIC METHODS //---------------------------------------------------------- + /** + * A version of {@link DrEntityStatePdu} with dead-reckoning overwritten to behave as though + * the algorithm was static. Used to disable dead-reckoning without actually changing the + * PDU's algorithm. + */ + public static class DisabledDrEntityStatePdu extends DrEntityStatePdu + { + //---------------------------------------------------------- + // STATIC VARIABLES + //---------------------------------------------------------- + + //---------------------------------------------------------- + // INSTANCE VARIABLES + //---------------------------------------------------------- + + //---------------------------------------------------------- + // CONSTRUCTORS + //---------------------------------------------------------- + public DisabledDrEntityStatePdu( EntityStatePdu pdu ) + { + super( pdu ); + } + + //---------------------------------------------------------- + // INSTANCE METHODS + //---------------------------------------------------------- + @Override + protected DrmState getDrmStateAtLocalTime( DeadReckoningAlgorithm algorithm, + long localTimestamp ) + throws IllegalArgumentException + { + // skip calculations and always return the current state + return this.getInitialDrmState(); + } + + //////////////////////////////////////////////////////////////////////////////////////////// + /////////////////////////////// Accessor and Mutator Methods /////////////////////////////// + //////////////////////////////////////////////////////////////////////////////////////////// + + //---------------------------------------------------------- + // STATIC METHODS + //---------------------------------------------------------- + } } diff --git a/codebase/src/java/disco/org/openlvc/disco/application/pdu/DrEntityStatePdu.java b/codebase/src/java/disco/org/openlvc/disco/application/pdu/DrEntityStatePdu.java index a47401df..7899359c 100644 --- a/codebase/src/java/disco/org/openlvc/disco/application/pdu/DrEntityStatePdu.java +++ b/codebase/src/java/disco/org/openlvc/disco/application/pdu/DrEntityStatePdu.java @@ -17,8 +17,6 @@ */ package org.openlvc.disco.application.pdu; -import java.util.Map; - import org.openlvc.disco.application.utils.DrmState; import org.openlvc.disco.pdu.entity.EntityStatePdu; import org.openlvc.disco.pdu.field.DeadReckoningAlgorithm; @@ -34,7 +32,8 @@ * When queries are made for dead-reckoned information at a particular timestamp, stores the * computed state in an internal LRU Cache. */ -public class DrEntityStatePdu extends EntityStatePdu { +public class DrEntityStatePdu extends EntityStatePdu +{ //---------------------------------------------------------- // STATIC VARIABLES //---------------------------------------------------------- @@ -42,37 +41,25 @@ public class DrEntityStatePdu extends EntityStatePdu { //---------------------------------------------------------- // INSTANCE VARIABLES //---------------------------------------------------------- - private boolean drEnabled; private final DrmState initialDrmState; - private Map drmStateCache; // access must be synchronized + private LruCache drmStateCache; // access must be synchronized private OutdatedTimestampBehavior outdatedTimestampBehavior; //---------------------------------------------------------- // CONSTRUCTORS //---------------------------------------------------------- - public DrEntityStatePdu( EntityStatePdu pdu, int cacheCapacity ) + public DrEntityStatePdu( EntityStatePdu pdu ) { super( pdu ); - this.drEnabled = true; - this.initialDrmState = new DrmState( pdu.getLocation(), pdu.getLinearVelocity(), pdu.getDeadReckoningParams().getEntityLinearAcceleration(), pdu.getOrientation(), pdu.getDeadReckoningParams().getEntityAngularVelocity() ); - this.drmStateCache = new LruCache<>( cacheCapacity ); - - this.outdatedTimestampBehavior = OutdatedTimestampBehavior.ERROR; - } - - /** - * Uses a capacity of 3 as the default for the DRM State LRU Cache. - */ - public DrEntityStatePdu( EntityStatePdu pdu ) - { - this( pdu, 3 ); + this.setDrmStateCacheSize( 3 ); + this.setOutdatedTimestampBehavior( OutdatedTimestampBehavior.ERROR ); } //---------------------------------------------------------- @@ -83,12 +70,14 @@ protected DrmState getInitialDrmState() return this.initialDrmState; } - protected DrmState getDrmStateAtLocalTime( DeadReckoningAlgorithm algorithm, long localTimestamp ) throws IllegalArgumentException + protected DrmState getDrmStateAtLocalTime( DeadReckoningAlgorithm algorithm, + long localTimestamp ) + throws IllegalArgumentException { if( this.getLocalTimestamp() > localTimestamp ) { - // outdated dead-reckoning request - // might happen if a new EntityStatePDU comes in before another packet is finished being processed + // outdated dead-reckoning request - might happen if a new EntityStatePDU comes in + // before another packet has finished being processed switch( outdatedTimestampBehavior ) { case USE_CURRENT_STATE: @@ -97,12 +86,11 @@ protected DrmState getDrmStateAtLocalTime( DeadReckoningAlgorithm algorithm, lon default: case ERROR: - throw new IllegalArgumentException( "Timestamp in dead-reckoning query too old. Expected %s or newer, got %s.".formatted( this.getLocalTimestamp(), - localTimestamp ) ); + throw new IllegalArgumentException( "Timestamp in dead-reckoning query too old. Expected %s or newer, got %s.".formatted( this.getLocalTimestamp(), localTimestamp ) ); } } - if( !this.drEnabled || this.isFrozen() || this.getLocalTimestamp() == localTimestamp ) + if( this.isFrozen() || this.getLocalTimestamp() == localTimestamp ) return this.getInitialDrmState(); CacheKey cacheKey = new CacheKey( algorithm, localTimestamp ); @@ -113,15 +101,17 @@ protected DrmState getDrmStateAtLocalTime( DeadReckoningAlgorithm algorithm, lon if( state != null ) return state; - // hold access to the cache until we have written to it to prevent calculating the same state twice + // hold access to the cache until we have written to it to prevent calculating the + // same state twice double dt_ms = localTimestamp - this.getLocalTimestamp(); double dt = dt_ms / 1000.0; DrmState initialState = this.getInitialDrmState(); - if( algorithm.getReferenceFrame() != this.getDeadReckoningAlgorithm().getReferenceFrame() ) + if( algorithm.getReferenceFrame() != this.getDefaultDeadReckoningAlgorithm().getReferenceFrame() ) { - // handle conversion between body and world coords if the algorithms don't match coord systems + // handle conversion between body and world coords if the algorithms don't match + // coord systems switch( algorithm.getReferenceFrame() ) // what frame are we switching _to_ { case WorldCoordinates: @@ -157,14 +147,14 @@ protected DrmState getDrmStateAtLocalTime( DeadReckoningAlgorithm algorithm, lon * @param behavior the new {@link OutdatedTimestampBehavior} to use when processing a * dead-reckoning request with an outdated timestamp */ - public void setOutdatedTimestampBehavior( OutdatedTimestampBehavior behavior ) + public synchronized void setOutdatedTimestampBehavior( OutdatedTimestampBehavior behavior ) { this.outdatedTimestampBehavior = behavior; } /** * Gets the current {@link OutdatedTimestampBehavior} used when processing a dead-reckoning - * request with an outdated timestamp + * request with an outdated timestamp. * * @return the current {@link OutdatedTimestampBehavior} */ @@ -173,12 +163,29 @@ public OutdatedTimestampBehavior getOutdatedTimestampBehavior() return this.outdatedTimestampBehavior; } + /** + * Updates the capacity for the internal LRU cache used for dead-reckoning request results + * from the default capacity of 3. + */ + public synchronized void setDrmStateCacheSize( int capacity ) + { + this.drmStateCache = new LruCache<>( capacity ); + } + + /** + * Gets the current capacity for the internal LRU cache used for dead-reckoning request results. + */ + public synchronized int getDrmStateCacheSize() + { + return this.drmStateCache.getCapacity(); + } + /** * Returns the dead-reckoning algorithm specified for use by the {@link EntityStatePdu}. * * @return the {@link DeadReckoningAlgorithm} for this {@link DrEntityStatePdu} */ - public DeadReckoningAlgorithm getDeadReckoningAlgorithm() + public DeadReckoningAlgorithm getDefaultDeadReckoningAlgorithm() { return this.getDeadReckoningParams().getDeadReckoningAlgorithm(); } @@ -193,17 +200,18 @@ public DeadReckoningAlgorithm getDeadReckoningAlgorithm() * @param localTimestamp target timestamp, in ms since epoch. * * @return the position of the entity, as a {@link WorldCoordinate} - * @throws IllegalArgumentException if the given timestamp is older than the {@link EntityStatePdu} + * @throws IllegalArgumentException if the given timestamp is older than the + * {@link EntityStatePdu} * @see #setOutdatedTimestampBehavior(OutdatedTimestampBehavior) */ public WorldCoordinate getDrLocation( long localTimestamp ) throws IllegalArgumentException { - return this.getDrLocation( this.getDeadReckoningAlgorithm(), localTimestamp ); + return this.getDrLocation( this.getDefaultDeadReckoningAlgorithm(), localTimestamp ); } /** * Gets the position of the entity at the given local timestamp, extrapolated using the - * specified dead-reckoning algorithm. + * specified dead-reckoning algorithm. *

* Uses the internally cached value for the state at this timestamp (with this algorithm), if * available, rather than recomputing the state. @@ -212,33 +220,36 @@ public WorldCoordinate getDrLocation( long localTimestamp ) throws IllegalArgume * @param localTimestamp target timestamp, in ms since epoch. * * @return the position of the entity, as a {@link WorldCoordinate} - * @throws IllegalArgumentException if the given timestamp is older than the {@link EntityStatePdu} + * @throws IllegalArgumentException if the given timestamp is older than the + * {@link EntityStatePdu} * @see #setOutdatedTimestampBehavior(OutdatedTimestampBehavior) */ - public WorldCoordinate getDrLocation( DeadReckoningAlgorithm algorithm, long localTimestamp ) throws IllegalArgumentException + public WorldCoordinate getDrLocation( DeadReckoningAlgorithm algorithm, long localTimestamp ) + throws IllegalArgumentException { return this.getDrmStateAtLocalTime( algorithm, localTimestamp ).getLocation(); } /** * Gets the velocity of the entity at the given local timestamp, extrapolated using the - * dead-reckoning algorithm of this entity. + * dead-reckoning algorithm of this entity. *

* Uses the internally cached value for the state at this timestamp, if available, rather than * recomputing the state. * * @return the velocity of the entity, as a {@link VectorRecord} - * @throws IllegalArgumentException if the given timestamp is older than the {@link EntityStatePdu} + * @throws IllegalArgumentException if the given timestamp is older than the + * {@link EntityStatePdu} * @see #setOutdatedTimestampBehavior(OutdatedTimestampBehavior) */ public VectorRecord getDrLinearVelocity( long localTimestamp ) throws IllegalArgumentException { - return this.getDrLinearVelocity( this.getDeadReckoningAlgorithm(), localTimestamp ); + return this.getDrLinearVelocity( this.getDefaultDeadReckoningAlgorithm(), localTimestamp ); } /** * Gets the velocity of the entity at the given local timestamp, extrapolated using the - * specified dead-reckoning algorithm. + * specified dead-reckoning algorithm. *

* Uses the internally cached value for the state at this timestamp (with this algorithm), if * available, rather than recomputing the state. @@ -247,33 +258,37 @@ public VectorRecord getDrLinearVelocity( long localTimestamp ) throws IllegalArg * @param localTimestamp target timestamp, in ms since epoch. * * @return the velocity of the entity, as a {@link VectorRecord} - * @throws IllegalArgumentException if the given timestamp is older than the {@link EntityStatePdu} + * @throws IllegalArgumentException if the given timestamp is older than the + * {@link EntityStatePdu} * @see #setOutdatedTimestampBehavior(OutdatedTimestampBehavior) */ - public VectorRecord getDrLinearVelocity( DeadReckoningAlgorithm algorithm, long localTimestamp ) throws IllegalArgumentException + public VectorRecord getDrLinearVelocity( DeadReckoningAlgorithm algorithm, + long localTimestamp ) + throws IllegalArgumentException { return this.getDrmStateAtLocalTime( algorithm, localTimestamp ).getLinearVelocity(); } /** * Gets the orientation of the entity at the given local timestamp, extrapolated using the - * dead-reckoning algorithm of this entity. + * dead-reckoning algorithm of this entity. *

* Uses the internally cached value for the state at this timestamp, if available, rather than * recomputing the state. * * @return the orientation of the entity, as {@link EulerAngles} - * @throws IllegalArgumentException if the given timestamp is older than the {@link EntityStatePdu} + * @throws IllegalArgumentException if the given timestamp is older than the + * {@link EntityStatePdu} * @see #setOutdatedTimestampBehavior(OutdatedTimestampBehavior) */ public EulerAngles getDrOrientation( long localTimestamp ) throws IllegalArgumentException { - return this.getDrOrientation( this.getDeadReckoningAlgorithm(), localTimestamp ); + return this.getDrOrientation( this.getDefaultDeadReckoningAlgorithm(), localTimestamp ); } /** * Gets the orientation of the entity at the given local timestamp, extrapolated using the - * specified dead-reckoning algorithm. + * specified dead-reckoning algorithm. *

* Uses the internally cached value for the state at this timestamp (with this algorithm), if * available, rather than recomputing the state. @@ -282,10 +297,12 @@ public EulerAngles getDrOrientation( long localTimestamp ) throws IllegalArgumen * @param localTimestamp target timestamp, in ms since epoch. * * @return the orientation of the entity, as a {@link EulerAngles} - * @throws IllegalArgumentException if the given timestamp is older than the {@link EntityStatePdu} + * @throws IllegalArgumentException if the given timestamp is older than the + * {@link EntityStatePdu} * @see #setOutdatedTimestampBehavior(OutdatedTimestampBehavior) */ - public EulerAngles getDrOrientation( DeadReckoningAlgorithm algorithm, long localTimestamp ) throws IllegalArgumentException + public EulerAngles getDrOrientation( DeadReckoningAlgorithm algorithm, long localTimestamp ) + throws IllegalArgumentException { return this.getDrmStateAtLocalTime( algorithm, localTimestamp ).getOrientation(); } @@ -300,18 +317,6 @@ private record CacheKey( DeadReckoningAlgorithm algorithm, long localTimestamp ) { } - /** - * Creates an {@link DrEntityStatePdu} instance with dead-reckoning calculations disabled - * (behaves as if the dead-reckoning algorithm is {@link DeadReckoningAlgorithm#Static}). - */ - public static DrEntityStatePdu makeWithoutDr( EntityStatePdu pdu ) - { - DrEntityStatePdu wrappedPdu = new DrEntityStatePdu( pdu, -1 ); - wrappedPdu.drEnabled = false; - wrappedPdu.drmStateCache = null; - return wrappedPdu; - } - /** * The behavior to follow when a dead-reckoning request is made with an outdated timestamp * (older than the current latest timestamp). diff --git a/codebase/src/java/disco/org/openlvc/disco/application/utils/DrmState.java b/codebase/src/java/disco/org/openlvc/disco/application/utils/DrmState.java index 11c0e2b4..a9b79614 100644 --- a/codebase/src/java/disco/org/openlvc/disco/application/utils/DrmState.java +++ b/codebase/src/java/disco/org/openlvc/disco/application/utils/DrmState.java @@ -38,7 +38,8 @@ *

  • {@link #velocity}: varies depending on DR model
    • *
  • {@link #acceleration}: varies depending on DR model
    • *
  • {@link #orientation}: world coordinate system (see also: {@link EulerAngles})
    • - *
  • {@link #angularVelocity}: entity coordinate system (see also: {@link AngularVelocityVector})
    • + *
  • {@link #angularVelocity}: entity coordinate system (see also: + * {@link AngularVelocityVector})
    • * */ public record DrmState( Vec3 position, @@ -54,7 +55,11 @@ public record DrmState( Vec3 position, //---------------------------------------------------------- // CONSTRUCTORS //---------------------------------------------------------- - public DrmState( WorldCoordinate location, VectorRecord linearVelocity, VectorRecord linearAcceleration, EulerAngles orientation, AngularVelocityVector angularVelocity ) + public DrmState( WorldCoordinate location, + VectorRecord linearVelocity, + VectorRecord linearAcceleration, + EulerAngles orientation, + AngularVelocityVector angularVelocity ) { this( new Vec3(location), new Vec3(linearVelocity.getFirstComponent(), @@ -133,25 +138,29 @@ public WorldCoordinate getLocation() } /** - * Gets the velocity of the represented entity in DIS PDU format. - * Uses the coordinate system of the model that produced this state. + * Gets the velocity of the represented entity in DIS PDU format. Uses the coordinate system + * of the model that produced this state. * * @return the velocity of the entity, as a {@link VectorRecord} */ public VectorRecord getLinearVelocity() { - return new VectorRecord( (float)this.velocity().x, (float)this.velocity().y, (float)this.velocity().z ); + return new VectorRecord( (float)this.velocity().x, + (float)this.velocity().y, + (float)this.velocity().z ); } /** - * Gets the acceleration of the represented entity in DIS PDU format. - * Uses the coordinate system of the model that produced this state. + * Gets the acceleration of the represented entity in DIS PDU format. Uses the coordinate + * system of the model that produced this state. * * @return the acceleration of the entity, as a {@link VectorRecord} */ public VectorRecord getLinearAcceleration() { - return new VectorRecord( (float)this.acceleration().x, (float)this.acceleration().y, (float)this.acceleration().z ); + return new VectorRecord( (float)this.acceleration().x, + (float)this.acceleration().y, + (float)this.acceleration().z ); } /** @@ -172,7 +181,9 @@ public EulerAngles getOrientation() */ public AngularVelocityVector getAngularVelocity() { - return new AngularVelocityVector( (float)this.angularVelocity().x, (float)this.angularVelocity().y, (float)this.angularVelocity().z ); + return new AngularVelocityVector( (float)this.angularVelocity().x, + (float)this.angularVelocity().y, + (float)this.angularVelocity().z ); } //---------------------------------------------------------- diff --git a/codebase/src/java/test/org/openlvc/disco/application/pdu/DrEntityStatePduTest.java b/codebase/src/java/test/org/openlvc/disco/application/pdu/DrEntityStatePduTest.java index bf978721..b83acd6b 100644 --- a/codebase/src/java/test/org/openlvc/disco/application/pdu/DrEntityStatePduTest.java +++ b/codebase/src/java/test/org/openlvc/disco/application/pdu/DrEntityStatePduTest.java @@ -17,6 +17,7 @@ */ package org.openlvc.disco.application.pdu; +import org.openlvc.disco.application.EntityStateStore.DisabledDrEntityStatePdu; import org.openlvc.disco.application.pdu.DrEntityStatePdu.OutdatedTimestampBehavior; import org.openlvc.disco.application.utils.DrmState; import org.openlvc.disco.pdu.entity.EntityStatePdu; @@ -109,15 +110,16 @@ public void testDrEntityRVW() entityPdu.setDeadReckoningParams( deadReckoningParams ); - DrEntityStatePdu drEntityPdu = new DrEntityStatePdu( entityPdu, 2 ); + DrEntityStatePdu drEntityPdu = new DrEntityStatePdu( entityPdu ); + drEntityPdu.setDrmStateCacheSize( 2 ); // check the initial values are what we set - Assert.assertEquals( drEntityPdu.getDeadReckoningAlgorithm(), deadReckoningAlgorithm ); + Assert.assertEquals( drEntityPdu.getDefaultDeadReckoningAlgorithm(), deadReckoningAlgorithm ); Assert.assertEquals( drEntityPdu.getLocalTimestamp(), localTimestamp ); Assert.assertEquals( drEntityPdu.getInitialDrmState(), expectedInitialState ); // check the state after 1s - DrmState drmState1s = drEntityPdu.getDrmStateAtLocalTime( drEntityPdu.getDeadReckoningAlgorithm(), + DrmState drmState1s = drEntityPdu.getDrmStateAtLocalTime( drEntityPdu.getDefaultDeadReckoningAlgorithm(), localTimestamp + 1000 ); Assert.assertEquals( drmState1s.getLocation(), new WorldCoordinate(52.5, 65.5, 80.5) ); Assert.assertEquals( drmState1s.getLinearVelocity(), new VectorRecord(13, 25, 37) ); @@ -126,7 +128,7 @@ public void testDrEntityRVW() Assert.assertEquals( drmState1s.getAngularVelocity(), angularVelocity ); // check the state after 3s - DrmState drmState3s = drEntityPdu.getDrmStateAtLocalTime( drEntityPdu.getDeadReckoningAlgorithm(), + DrmState drmState3s = drEntityPdu.getDrmStateAtLocalTime( drEntityPdu.getDefaultDeadReckoningAlgorithm(), localTimestamp + 3000 ); Assert.assertEquals( drmState3s.getLocation(), new WorldCoordinate(84.5, 125.5, 168.5) ); Assert.assertEquals( drmState3s.getLinearVelocity(), new VectorRecord(19.0f, 35.0f, 51.0f) ); @@ -164,11 +166,12 @@ public void testDrEntityCaching() entityPdu.setDeadReckoningParams( deadReckoningParams ); - DrEntityStatePdu drEntityPdu = new DrEntityStatePdu( entityPdu, 2 ); + DrEntityStatePdu drEntityPdu = new DrEntityStatePdu( entityPdu ); + drEntityPdu.setDrmStateCacheSize( 2 ); // check the state after 1s long t1s = localTimestamp + 1000; - DrmState drmState1s = drEntityPdu.getDrmStateAtLocalTime( drEntityPdu.getDeadReckoningAlgorithm(), t1s ); + DrmState drmState1s = drEntityPdu.getDrmStateAtLocalTime( drEntityPdu.getDefaultDeadReckoningAlgorithm(), t1s ); Assert.assertEquals( drmState1s.getLocation(), new WorldCoordinate(52.5, 65.5, 80.5) ); Assert.assertEquals( drmState1s.getLinearVelocity(), new VectorRecord(13, 25, 37) ); Assert.assertEquals( drmState1s.getLinearAcceleration(), acceleration ); @@ -177,7 +180,7 @@ public void testDrEntityCaching() // check the state after 3s long t3s = localTimestamp + 3000; - DrmState drmState3s = drEntityPdu.getDrmStateAtLocalTime( drEntityPdu.getDeadReckoningAlgorithm(), t3s ); + DrmState drmState3s = drEntityPdu.getDrmStateAtLocalTime( drEntityPdu.getDefaultDeadReckoningAlgorithm(), t3s ); Assert.assertEquals( drmState3s.getLocation(), new WorldCoordinate(84.5, 125.5, 168.5) ); Assert.assertEquals( drmState3s.getLinearVelocity(), new VectorRecord(19.0f, 35.0f, 51.0f) ); Assert.assertEquals( drmState3s.getLinearAcceleration(), acceleration ); @@ -185,14 +188,14 @@ public void testDrEntityCaching() Assert.assertEquals( drmState3s.getAngularVelocity(), angularVelocity ); // check we get the same objects when fetching those times again (cache hits) - DrmState drmState1s_2 = drEntityPdu.getDrmStateAtLocalTime( drEntityPdu.getDeadReckoningAlgorithm(), t1s ); + DrmState drmState1s_2 = drEntityPdu.getDrmStateAtLocalTime( drEntityPdu.getDefaultDeadReckoningAlgorithm(), t1s ); Assert.assertTrue( drmState1s_2 == drmState1s ); - DrmState drmState3s_2 = drEntityPdu.getDrmStateAtLocalTime( drEntityPdu.getDeadReckoningAlgorithm(), t3s ); + DrmState drmState3s_2 = drEntityPdu.getDrmStateAtLocalTime( drEntityPdu.getDefaultDeadReckoningAlgorithm(), t3s ); Assert.assertTrue( drmState3s_2 == drmState3s ); // check the state after 5s long t5s = localTimestamp + 5000; - DrmState drmState5s = drEntityPdu.getDrmStateAtLocalTime( drEntityPdu.getDeadReckoningAlgorithm(), t5s ); + DrmState drmState5s = drEntityPdu.getDrmStateAtLocalTime( drEntityPdu.getDefaultDeadReckoningAlgorithm(), t5s ); Assert.assertEquals( drmState5s.getLocation(), new WorldCoordinate(128.5, 205.5, 284.5) ); Assert.assertEquals( drmState5s.getLinearVelocity(), new VectorRecord(25, 45, 65) ); Assert.assertEquals( drmState5s.getLinearAcceleration(), acceleration ); @@ -200,7 +203,7 @@ public void testDrEntityCaching() Assert.assertEquals( drmState5s.getAngularVelocity(), angularVelocity ); // check we get a cache miss when querying at 1s - DrmState drmState1s_3 = drEntityPdu.getDrmStateAtLocalTime( drEntityPdu.getDeadReckoningAlgorithm(), t1s ); + DrmState drmState1s_3 = drEntityPdu.getDrmStateAtLocalTime( drEntityPdu.getDefaultDeadReckoningAlgorithm(), t1s ); Assert.assertFalse( drmState1s_3 == drmState1s ); // should still have the same values though Assert.assertEquals( drmState1s_3, drmState1s ); @@ -214,7 +217,7 @@ public void testNoAlgorithm() DrEntityStatePdu drEntityPdu = new DrEntityStatePdu( entityPdu ); // verify the selected DRM is a Static model - Assert.assertEquals( drEntityPdu.getDeadReckoningAlgorithm(), DeadReckoningAlgorithm.Static ); + Assert.assertEquals( drEntityPdu.getDefaultDeadReckoningAlgorithm(), DeadReckoningAlgorithm.Static ); } @Test(dependsOnMethods={"testDrEntityRVW"}) @@ -248,10 +251,11 @@ public void testOverrideNoAlgorithm() entityPdu.setDeadReckoningParams( deadReckoningParams ); - DrEntityStatePdu drEntityPdu = new DrEntityStatePdu( entityPdu, 2 ); + DrEntityStatePdu drEntityPdu = new DrEntityStatePdu( entityPdu ); + drEntityPdu.setDrmStateCacheSize( 2 ); // check the initial values are what we set - Assert.assertEquals( drEntityPdu.getDeadReckoningAlgorithm(), deadReckoningAlgorithm ); + Assert.assertEquals( drEntityPdu.getDefaultDeadReckoningAlgorithm(), deadReckoningAlgorithm ); Assert.assertEquals( drEntityPdu.getLocalTimestamp(), localTimestamp ); Assert.assertEquals( drEntityPdu.getInitialDrmState(), expectedInitialState ); @@ -297,10 +301,10 @@ public void testDrDisabled() entityPdu.setDeadReckoningParams( deadReckoningParams ); - DrEntityStatePdu drEntityPdu = DrEntityStatePdu.makeWithoutDr( entityPdu ); + DrEntityStatePdu drEntityPdu = new DisabledDrEntityStatePdu( entityPdu ); // check the state after 3s - DrmState drmState3s = drEntityPdu.getDrmStateAtLocalTime( drEntityPdu.getDeadReckoningAlgorithm(), + DrmState drmState3s = drEntityPdu.getDrmStateAtLocalTime( drEntityPdu.getDefaultDeadReckoningAlgorithm(), localTimestamp + 3000 ); Assert.assertEquals( drmState3s, expectedInitialState ); } @@ -339,7 +343,7 @@ public void testFrozenEntity() DrEntityStatePdu drEntityPdu = new DrEntityStatePdu( entityPdu ); // check the state after 3s - DrmState drmState3s = drEntityPdu.getDrmStateAtLocalTime( drEntityPdu.getDeadReckoningAlgorithm(), + DrmState drmState3s = drEntityPdu.getDrmStateAtLocalTime( drEntityPdu.getDefaultDeadReckoningAlgorithm(), localTimestamp + 3000 ); Assert.assertEquals( drmState3s.getLocation(), new WorldCoordinate(84.5, 125.5, 168.5) ); Assert.assertEquals( drmState3s.getLinearVelocity(), new VectorRecord(19.0f, 35.0f, 51.0f) ); @@ -350,7 +354,7 @@ public void testFrozenEntity() drEntityPdu.setFrozen( true ); // check the state after 3s when frozen - DrmState drmState3sFrozen = drEntityPdu.getDrmStateAtLocalTime( drEntityPdu.getDeadReckoningAlgorithm(), + DrmState drmState3sFrozen = drEntityPdu.getDrmStateAtLocalTime( drEntityPdu.getDefaultDeadReckoningAlgorithm(), localTimestamp + 3000 ); Assert.assertEquals( drmState3sFrozen, expectedInitialState ); } From 3bb1dbc2bd7f67df8ea91f96f108584c38cf7818 Mon Sep 17 00:00:00 2001 From: Nathan Townshend Date: Tue, 25 Nov 2025 18:32:52 +0800 Subject: [PATCH 7/8] FIXUP - Formatting tweaks in utils --- .../src/java/disco/org/openlvc/disco/utils/Mat3x3.java | 2 -- .../test/org/openlvc/disco/utils/QuaternionTest.java | 9 ++++++--- 2 files changed, 6 insertions(+), 5 deletions(-) diff --git a/codebase/src/java/disco/org/openlvc/disco/utils/Mat3x3.java b/codebase/src/java/disco/org/openlvc/disco/utils/Mat3x3.java index 1d893c85..244cd5cc 100644 --- a/codebase/src/java/disco/org/openlvc/disco/utils/Mat3x3.java +++ b/codebase/src/java/disco/org/openlvc/disco/utils/Mat3x3.java @@ -17,8 +17,6 @@ */ package org.openlvc.disco.utils; -import org.openlvc.disco.DiscoException; - public class Mat3x3 { //---------------------------------------------------------- diff --git a/codebase/src/java/test/org/openlvc/disco/utils/QuaternionTest.java b/codebase/src/java/test/org/openlvc/disco/utils/QuaternionTest.java index 51e2fe31..c250c913 100644 --- a/codebase/src/java/test/org/openlvc/disco/utils/QuaternionTest.java +++ b/codebase/src/java/test/org/openlvc/disco/utils/QuaternionTest.java @@ -87,7 +87,8 @@ private void assertEulerAngleInBounds( EulerAngles orientation ) Assert.assertTrue( EulerAngles.PHI_MIN <= orientation.getPhi() ); } - private void testQuaternionEulerAngleConversion( EulerAngles testOrientation, EulerAngles expectedOrientation ) + private void testQuaternionEulerAngleConversion( EulerAngles testOrientation, + EulerAngles expectedOrientation ) { // double check our test values are in bounds assertEulerAngleInBounds( testOrientation ); @@ -191,7 +192,8 @@ public static Iterator boundsWithSingularities() for( float psi : new float[]{ EulerAngles.PSI_MIN, EulerAngles.PSI_MIN + 0.01f, -1.3f, 0, 1.1f, EulerAngles.PSI_MAX - 0.01f, EulerAngles.PSI_MAX } ) { - for( float theta : new float[]{ EulerAngles.THETA_MIN, EulerAngles.THETA_MAX } ) // only test singularities + // only test singularities + for( float theta : new float[]{ EulerAngles.THETA_MIN, EulerAngles.THETA_MAX } ) { for( float phi : new float[]{ EulerAngles.PHI_MIN, EulerAngles.PHI_MIN + 0.01f, -1.3f, 0, 1.1f, EulerAngles.PHI_MAX - 0.01f, @@ -224,8 +226,9 @@ public EulerAngles next() i++; float psi = rand.nextFloat( EulerAngles.PSI_MIN, EulerAngles.PSI_MAX ); + // prevent singularities by using a maximum theta magnitude of 86.3 degrees float theta = rand.nextFloat( EulerAngles.THETA_MIN * (86.3f / 90), - EulerAngles.THETA_MAX * (86.3f / 90) ); // prevent singularities + EulerAngles.THETA_MAX * (86.3f / 90) ); float phi = rand.nextFloat( EulerAngles.PHI_MIN, EulerAngles.PHI_MAX ); return new EulerAngles( psi, theta, phi ); From 2d54df6b7c7cebfcfe88f5f45b3ada48e7104a47 Mon Sep 17 00:00:00 2001 From: Nathan Townshend Date: Mon, 9 Feb 2026 16:50:39 +0800 Subject: [PATCH 8/8] FIXUP - Formatting changes and satisfying Sonarqube --- .../disco/application/EntityStateStore.java | 96 +++++------ .../application/pdu/DrEntityStatePdu.java | 59 +++---- .../disco/application/utils/DrmState.java | 32 ++-- .../types/variant/SpatialVariantStruct.java | 2 +- .../disco/pdu/emissions/DesignatorPdu.java | 2 +- .../pdu/field/DeadReckoningAlgorithm.java | 160 ++++++++++-------- .../pdu/record/DeadReckoningParameter.java | 2 +- .../openlvc/disco/pdu/record/EulerAngles.java | 59 +++---- .../org/openlvc/disco/utils/LruCache.java | 35 +++- .../disco/org/openlvc/disco/utils/Mat3x3.java | 12 +- .../org/openlvc/disco/utils/Quaternion.java | 31 ++-- .../disco/org/openlvc/disco/utils/Vec3.java | 42 +++-- .../application/EntityStateStoreTest.java | 64 +++---- .../application/pdu/DrEntityStatePduTest.java | 32 ++-- .../pdu/field/DeadReckoningAlgorithmTest.java | 43 ++--- .../openlvc/disco/utils/QuaternionTest.java | 40 +++-- 16 files changed, 383 insertions(+), 328 deletions(-) diff --git a/codebase/src/java/disco/org/openlvc/disco/application/EntityStateStore.java b/codebase/src/java/disco/org/openlvc/disco/application/EntityStateStore.java index 1c77efbb..a339a5de 100644 --- a/codebase/src/java/disco/org/openlvc/disco/application/EntityStateStore.java +++ b/codebase/src/java/disco/org/openlvc/disco/application/EntityStateStore.java @@ -69,9 +69,9 @@ private boolean isDrEnabled() return this.parentStore.app.getConfiguration().getDeadReckoningEnabled(); } - //////////////////////////////////////////////////////////////////////////////////////////// - /// PDU Processing /////////////////////////////////////////////////////////////////////// - //////////////////////////////////////////////////////////////////////////////////////////// + //========================================================================================== + //------------------------------------- PDU Processing ------------------------------------- + //========================================================================================== protected void receivePdu( EntityStatePdu pdu ) { // wrap the PDU to provide dead-reckoning support @@ -80,12 +80,12 @@ protected void receivePdu( EntityStatePdu pdu ) // bang the entity into the ID indexed store DrEntityStatePdu existing = byId.put( wrappedPdu.getEntityID(), wrappedPdu ); - + // if we are discovering this entity for first time, store in marking indexed store as well if( existing == null ) { DrEntityStatePdu existingMarking = byMarking.put( wrappedPdu.getMarking(), wrappedPdu ); - + // If there is already an entity against this marking with a different id, then // we assume that it has gone stale in favor of the one that we have just received. // @@ -94,41 +94,40 @@ protected void receivePdu( EntityStatePdu pdu ) // with different EntityIds, however their marking are the same. if( existingMarking != null ) byId.remove( existingMarking.getEntityID(), existingMarking ); - } - else if( existing.getMarking().equals(wrappedPdu.getMarking()) == false ) - { + else if( !existing.getMarking().equals(wrappedPdu.getMarking()) ) + { // marking has changed, need to update the marking indexed store byMarking.remove( existing.getMarking() ); byMarking.put( wrappedPdu.getMarking(), wrappedPdu ); } } - - //////////////////////////////////////////////////////////////////////////////////////////// - /// Locally Created PDU Tracking Methods ///////////////////////////////////////////////// - //////////////////////////////////////////////////////////////////////////////////////////// + + //========================================================================================== + //-------------------------- Locally Created PDU Tracking Methods -------------------------- + //========================================================================================== //public void addEntityState( EntityStatePdu pdu ) //{ //} - + //public EntityStatePdu removeEntityState( String marking ) //{ // return null; //} - + //public EntityStatePdu removeEntityState( EntityId id ) //{ // return null; //} - - //////////////////////////////////////////////////////////////////////////////////////////// - /// Entity State Query Methods /////////////////////////////////////////////////////////// - //////////////////////////////////////////////////////////////////////////////////////////// + + //========================================================================================== + //------------------------------- Entity State Query Methods ------------------------------- + //========================================================================================== public boolean hasEntityState( String marking ) { return byMarking.containsKey( marking ); } - + public boolean hasEntityState( EntityId id ) { return byId.containsKey( id ); @@ -138,10 +137,10 @@ public DrEntityStatePdu getEntityState( String marking ) { if( marking.length() > 11 ) throw new DiscoException( "DIS markings limited to 11 characters: [%s] too long", marking ); - + return byMarking.get( marking ); } - + /** * @param id * @return the last {@link DrEntityStatePdu} for the entity, or `null` if not stored @@ -150,15 +149,15 @@ public DrEntityStatePdu getEntityState( EntityId id ) { return byId.get( id ); } - - //////////////////////////////////////////////////////////////////////////////////////////// - /// Property Based Query Methods ///////////////////////////////////////////////////////// - //////////////////////////////////////////////////////////////////////////////////////////// + + //========================================================================================== + //------------------------------ Property Based Query Methods ------------------------------ + //========================================================================================== public Set getAllMarkings() { return new HashSet<>( byMarking.keySet() ); } - + /** * Return all the entities that have been updated only AFTER the given timestamp (millis * since the epoch). Note that we use Disco's local timestamp, NOT the DIS timestamp. @@ -172,10 +171,10 @@ public Set getEntityStatesUpdatedSince( long time ) .filter( espdu -> espdu.getLocalTimestamp() >= time ) .collect( Collectors.toSet() ); } - - //////////////////////////////////////////////////////////////////////////////////////////// - /// Location Based Query Methods ///////////////////////////////////////////////////////// - //////////////////////////////////////////////////////////////////////////////////////////// + + //========================================================================================== + //------------------------------ Location Based Query Methods ------------------------------ + //========================================================================================== /** * Get the set of Entity States whose location is within the radius of the specified entity's * location. If none are close, an empty set is returned. @@ -188,7 +187,7 @@ public Set getEntityStatesNear( EntityStatePdu entity, int rad { return getEntityStatesNear( entity.getLocation(), radiusMeters ); } - + /** * Get the set of Entity States whose location is within the specified radius of the specified * location. If none are close, an empty set is returned. @@ -204,9 +203,9 @@ public Set getEntityStatesNear( WorldCoordinate location, int .collect( Collectors.toSet() ); } - //////////////////////////////////////////////////////////////////////////////////////////// - /// Delete Timeout Support Methods /////////////////////////////////////////////////////// - //////////////////////////////////////////////////////////////////////////////////////////// + //========================================================================================== + //----------------------------- Delete Timeout Support Methods ----------------------------- + //========================================================================================== @Override public int removeStaleData( long oldestTimestamp ) { @@ -223,21 +222,20 @@ public int removeStaleData( long oldestTimestamp ) byMarking.remove( espdu.getMarking() ); removed.incrementAndGet(); - }); - + }); + return removed.intValue(); } - - //////////////////////////////////////////////////////////////////////////////////////////// - /// Accessor and Mutator Methods ///////////////////////////////////////////////////////// - //////////////////////////////////////////////////////////////////////////////////////////// + //========================================================================================== + //------------------------------ Accessor and Mutator Methods ------------------------------ + //========================================================================================== public void clear() { this.byId.clear(); this.byMarking.clear(); } - + public int size() { return byMarking.size(); @@ -256,11 +254,11 @@ public static class DisabledDrEntityStatePdu extends DrEntityStatePdu //---------------------------------------------------------- // STATIC VARIABLES //---------------------------------------------------------- - + //---------------------------------------------------------- // INSTANCE VARIABLES //---------------------------------------------------------- - + //---------------------------------------------------------- // CONSTRUCTORS //---------------------------------------------------------- @@ -268,7 +266,7 @@ public DisabledDrEntityStatePdu( EntityStatePdu pdu ) { super( pdu ); } - + //---------------------------------------------------------- // INSTANCE METHODS //---------------------------------------------------------- @@ -280,11 +278,11 @@ protected DrmState getDrmStateAtLocalTime( DeadReckoningAlgorithm algorithm, // skip calculations and always return the current state return this.getInitialDrmState(); } - - //////////////////////////////////////////////////////////////////////////////////////////// - /////////////////////////////// Accessor and Mutator Methods /////////////////////////////// - //////////////////////////////////////////////////////////////////////////////////////////// - + + //========================================================================================== + //------------------------------ Accessor and Mutator Methods ------------------------------ + //========================================================================================== + //---------------------------------------------------------- // STATIC METHODS //---------------------------------------------------------- diff --git a/codebase/src/java/disco/org/openlvc/disco/application/pdu/DrEntityStatePdu.java b/codebase/src/java/disco/org/openlvc/disco/application/pdu/DrEntityStatePdu.java index 7899359c..05e12f1f 100644 --- a/codebase/src/java/disco/org/openlvc/disco/application/pdu/DrEntityStatePdu.java +++ b/codebase/src/java/disco/org/openlvc/disco/application/pdu/DrEntityStatePdu.java @@ -37,14 +37,14 @@ public class DrEntityStatePdu extends EntityStatePdu //---------------------------------------------------------- // STATIC VARIABLES //---------------------------------------------------------- - + //---------------------------------------------------------- // INSTANCE VARIABLES //---------------------------------------------------------- private final DrmState initialDrmState; private LruCache drmStateCache; // access must be synchronized private OutdatedTimestampBehavior outdatedTimestampBehavior; - + //---------------------------------------------------------- // CONSTRUCTORS //---------------------------------------------------------- @@ -76,23 +76,24 @@ protected DrmState getDrmStateAtLocalTime( DeadReckoningAlgorithm algorithm, { if( this.getLocalTimestamp() > localTimestamp ) { - // outdated dead-reckoning request - might happen if a new EntityStatePDU comes in + // Outdated dead-reckoning request - might happen if a new EntityStatePDU comes in // before another packet has finished being processed - switch( outdatedTimestampBehavior ) + switch( this.getOutdatedTimestampBehavior() ) { case USE_CURRENT_STATE: localTimestamp = this.getLocalTimestamp(); break; - default: case ERROR: - throw new IllegalArgumentException( "Timestamp in dead-reckoning query too old. Expected %s or newer, got %s.".formatted( this.getLocalTimestamp(), localTimestamp ) ); + throw new IllegalArgumentException( String.format("Timestamp in dead-reckoning query too old. Expected %s or newer, got %s.", + this.getLocalTimestamp(), + localTimestamp) ); } } if( this.isFrozen() || this.getLocalTimestamp() == localTimestamp ) return this.getInitialDrmState(); - + CacheKey cacheKey = new CacheKey( algorithm, localTimestamp ); synchronized( this.drmStateCache ) @@ -101,34 +102,28 @@ protected DrmState getDrmStateAtLocalTime( DeadReckoningAlgorithm algorithm, if( state != null ) return state; - // hold access to the cache until we have written to it to prevent calculating the + // Hold access to the cache until we have written to it to prevent calculating the // same state twice - double dt_ms = localTimestamp - this.getLocalTimestamp(); - double dt = dt_ms / 1000.0; + double dt = (localTimestamp - this.getLocalTimestamp()) / 1000d; // seconds DrmState initialState = this.getInitialDrmState(); if( algorithm.getReferenceFrame() != this.getDefaultDeadReckoningAlgorithm().getReferenceFrame() ) { - // handle conversion between body and world coords if the algorithms don't match + // Handle conversion between body and world coords if the algorithms don't match // coord systems - switch( algorithm.getReferenceFrame() ) // what frame are we switching _to_ + initialState = switch( algorithm.getReferenceFrame() ) // what frame are we switching _to_ { - case WorldCoordinates: - // Body -> World - initialState = initialState.asWorldCoords(); - break; - - case BodyCoordinates: - // World -> Body - initialState = initialState.asBodyCoords(); - break; - - default: - case Other: - // TODO warn? - break; - } + // Body -> World + case WORLD_COORDINATES -> initialState.asWorldCoords(); + + // World -> Body + case BODY_COORDINATES -> initialState.asBodyCoords(); + + // Unknown + // TODO warn? + case OTHER -> initialState; + }; } state = algorithm.computeStateAfter( initialState, dt ); @@ -137,9 +132,9 @@ protected DrmState getDrmStateAtLocalTime( DeadReckoningAlgorithm algorithm, } } - //////////////////////////////////////////////////////////////////////////////////////////// - /////////////////////////////// Accessor and Mutator Methods /////////////////////////////// - //////////////////////////////////////////////////////////////////////////////////////////// + //========================================================================================== + //------------------------------ Accessor and Mutator Methods ------------------------------ + //========================================================================================== /** * Sets the behavior when performing a dead-reckoning request with a timestamp older than the * {@link EntityStatePdu}. @@ -151,14 +146,14 @@ public synchronized void setOutdatedTimestampBehavior( OutdatedTimestampBehavior { this.outdatedTimestampBehavior = behavior; } - + /** * Gets the current {@link OutdatedTimestampBehavior} used when processing a dead-reckoning * request with an outdated timestamp. * * @return the current {@link OutdatedTimestampBehavior} */ - public OutdatedTimestampBehavior getOutdatedTimestampBehavior() + public synchronized OutdatedTimestampBehavior getOutdatedTimestampBehavior() { return this.outdatedTimestampBehavior; } diff --git a/codebase/src/java/disco/org/openlvc/disco/application/utils/DrmState.java b/codebase/src/java/disco/org/openlvc/disco/application/utils/DrmState.java index a9b79614..04a2018c 100644 --- a/codebase/src/java/disco/org/openlvc/disco/application/utils/DrmState.java +++ b/codebase/src/java/disco/org/openlvc/disco/application/utils/DrmState.java @@ -73,7 +73,7 @@ public DrmState( WorldCoordinate location, angularVelocity.getRateAboutYAxis(), angularVelocity.getRateAboutZAxis()) ); } - + //---------------------------------------------------------- // INSTANCE METHODS //---------------------------------------------------------- @@ -82,16 +82,20 @@ public boolean equals( Object other ) { if( this == other ) return true; - - if( !(other instanceof DrmState otherDrmState) ) + + if( !(other instanceof DrmState(Vec3 otherPosition, + Vec3 otherVelocity, + Vec3 otherAcceleration, + Quaternion otherOrientation, + Vec3 otherAngularVelocity)) ) return false; - return Objects.equals( otherDrmState.position(), this.position() ) && - Objects.equals( otherDrmState.velocity(), this.velocity() ) && - Objects.equals( otherDrmState.acceleration(), this.acceleration() ) && - this.orientation() != null && - this.orientation().equalsRotation( otherDrmState.orientation() ) && - Objects.equals( otherDrmState.angularVelocity(), this.angularVelocity() ); + return Objects.equals( otherPosition, this.position() ) && + Objects.equals( otherVelocity, this.velocity() ) && + Objects.equals( otherAcceleration, this.acceleration() ) && + otherOrientation != null && + otherOrientation.equalsRotation( this.orientation() ) && + Objects.equals( otherAngularVelocity, this.angularVelocity() ); } /** @@ -124,9 +128,9 @@ public DrmState asWorldCoords() this.angularVelocity() ); } - //////////////////////////////////////////////////////////////////////////////////////////// - /////////////////////////////// Accessor Methods /////////////////////////////////////////// - //////////////////////////////////////////////////////////////////////////////////////////// + //========================================================================================== + //------------------------------------ Accessor Methods ------------------------------------ + //========================================================================================== /** * Gets the position of the represented entity in DIS PDU format. * @@ -136,7 +140,7 @@ public WorldCoordinate getLocation() { return new WorldCoordinate( this.position().x, this.position().y, this.position().z ); } - + /** * Gets the velocity of the represented entity in DIS PDU format. Uses the coordinate system * of the model that produced this state. @@ -185,7 +189,7 @@ public AngularVelocityVector getAngularVelocity() (float)this.angularVelocity().y, (float)this.angularVelocity().z ); } - + //---------------------------------------------------------- // STATIC METHODS //---------------------------------------------------------- diff --git a/codebase/src/java/disco/org/openlvc/disco/connection/rpr/types/variant/SpatialVariantStruct.java b/codebase/src/java/disco/org/openlvc/disco/connection/rpr/types/variant/SpatialVariantStruct.java index fe44923b..f9d584a7 100644 --- a/codebase/src/java/disco/org/openlvc/disco/connection/rpr/types/variant/SpatialVariantStruct.java +++ b/codebase/src/java/disco/org/openlvc/disco/connection/rpr/types/variant/SpatialVariantStruct.java @@ -70,7 +70,7 @@ public void setValue( EntityStatePdu pdu ) AbstractSpatialStruct value = null; switch( dr ) { - case Static: value = new SpatialStaticStruct().fromPdu(pdu); break; + case STATIC: value = new SpatialStaticStruct().fromPdu(pdu); break; case FPW: value = new SpatialFPStruct().fromPdu(pdu); break; case FPB: value = new SpatialFPStruct().fromPdu(pdu); break; case RPW: value = new SpatialRPStruct().fromPdu(pdu); break; diff --git a/codebase/src/java/disco/org/openlvc/disco/pdu/emissions/DesignatorPdu.java b/codebase/src/java/disco/org/openlvc/disco/pdu/emissions/DesignatorPdu.java index dccde232..05b95765 100644 --- a/codebase/src/java/disco/org/openlvc/disco/pdu/emissions/DesignatorPdu.java +++ b/codebase/src/java/disco/org/openlvc/disco/pdu/emissions/DesignatorPdu.java @@ -65,7 +65,7 @@ public DesignatorPdu() this.wavelength = 0.0f; this.spotRelativeToDesignatedEntity = new VectorRecord(); this.spotLocation = new WorldCoordinate(); - this.spotDRA = DeadReckoningAlgorithm.Static; + this.spotDRA = DeadReckoningAlgorithm.STATIC; // padding 24-bit this.spotLinearAcceleration = new VectorRecord(); } diff --git a/codebase/src/java/disco/org/openlvc/disco/pdu/field/DeadReckoningAlgorithm.java b/codebase/src/java/disco/org/openlvc/disco/pdu/field/DeadReckoningAlgorithm.java index f4d6248b..b1c1bd60 100644 --- a/codebase/src/java/disco/org/openlvc/disco/pdu/field/DeadReckoningAlgorithm.java +++ b/codebase/src/java/disco/org/openlvc/disco/pdu/field/DeadReckoningAlgorithm.java @@ -34,8 +34,8 @@ public enum DeadReckoningAlgorithm //---------------------------------------------------------- // VALUES //---------------------------------------------------------- - Other ( (short)0 ), - Static( (short)1 ), + OTHER ( (short)0 ), + STATIC( (short)1 ), FPW ( (short)2 ), RPW ( (short)3 ), RVW ( (short)4 ), @@ -69,20 +69,23 @@ public short value() @Override public String toString() { - switch( this.value ) + switch( this ) { - case 1: return "Static"; - case 2: return "FPW"; - case 3: return "RPW"; - case 4: return "RVW"; - case 5: return "FVW"; - case 6: return "FPB"; - case 7: return "RPB"; - case 8: return "RVB"; - case 9: return "FVB"; - default: // drop through + case STATIC: return "Static"; + + case FPW: return "FPW"; + case FVW: return "FVW"; + case RPW: return "RPW"; + case RVW: return "RVW"; + + case FPB: return "FPB"; + case FVB: return "FVB"; + case RPB: return "RPB"; + case RVB: return "RVB"; + + default: // fall through } - + // Missing if( DiscoConfiguration.isSet(Flag.Strict) ) throw new IllegalArgumentException( value+" not a valid Dead Reckoning Algorithm" ); @@ -94,27 +97,21 @@ public ReferenceFrame getReferenceFrame() { switch( this ) { - case Static: - case FPW: - case FVW: - case RPW: - case RVW: - return ReferenceFrame.WorldCoordinates; - - case FPB: - case FVB: - case RPB: - case RVB: - return ReferenceFrame.BodyCoordinates; - - default: // drop through + case STATIC, FPW, FVW, RPW, RVW: + return ReferenceFrame.WORLD_COORDINATES; + + case FPB, FVB, RPB, RVB: + return ReferenceFrame.BODY_COORDINATES; + + case OTHER: // fall through } // Ill-defined or unknown if( DiscoConfiguration.isSet(Flag.Strict) ) - throw new DiscoException( "Unknown reference frame for dead-reckoning algorithm: %s (%d)".formatted(this, - this.value()) ); - return ReferenceFrame.Other; + throw new DiscoException( String.format("Unknown reference frame for dead-reckoning algorithm: %s (%d)", + this, + this.value()) ); + return ReferenceFrame.OTHER; } /** @@ -129,23 +126,23 @@ public ReferenceFrame getReferenceFrame() */ public DrmState computeStateAfter( DrmState initialState, double dt ) { - if( this == Static ) + if( this == STATIC ) return initialState; - + switch( this.getReferenceFrame() ) { - case WorldCoordinates: + case WORLD_COORDINATES: return DeadReckoningAlgorithm.computeFixedStateAfter( this, initialState, dt ); - case BodyCoordinates: + case BODY_COORDINATES: return DeadReckoningAlgorithm.computeRotatingStateAfter( this, initialState, dt ); - default: - case Other: - // fallback to static - // TODO warn? - return initialState; + case OTHER: // fall through } + + // fallback to static + // TODO warn? + return initialState; } //---------------------------------------------------------- @@ -160,23 +157,26 @@ public static DeadReckoningAlgorithm fromValue( short value ) { switch( value ) { + case 1: return STATIC; + case 2: return FPW; - case 4: return RVW; - case 1: return Static; - case 8: return RVB; - case 3: return RPW; case 5: return FVW; + case 3: return RPW; + case 4: return RVW; + case 6: return FPB; - case 7: return RPB; case 9: return FVB; - default: // drop through + case 7: return RPB; + case 8: return RVB; + + default: // fall through } - + // Missing if( DiscoConfiguration.isSet(Flag.Strict) ) throw new IllegalArgumentException( value+" not a valid Dead Reckoning Algorithm" ); else - return Other; + return OTHER; } // q_DR in the spec @@ -193,7 +193,7 @@ private static Quaternion makeRotationQuaternion( Vec3 angularVelocity, double d double z = rotAxis.z * sinHalfBeta; return new Quaternion( w, x, y, z ); - } + } /** * Returns the state of the dead-reckoning model when extrapolated for the given duration, @@ -208,6 +208,17 @@ private static Quaternion makeRotationQuaternion( Vec3 angularVelocity, double d */ private static DrmState computeFixedStateAfter( DeadReckoningAlgorithm algorithm, DrmState initialState, double dt ) { + // * If updating this to add additional algorithms, make + // * sure to update the switch statements below as well + switch( algorithm ) + { + case FPW, FVW, RPW, RVW: + break; + + case STATIC, FPB, FVB, RPB, RVB, OTHER: + throw new IllegalArgumentException( "Invalid Dead Reckoning Algorithm for this computation type" ); + } + // use the old copies if we don't make any changes Optional position = Optional.empty(); Optional velocity = Optional.empty(); @@ -217,10 +228,7 @@ private static DrmState computeFixedStateAfter( DeadReckoningAlgorithm algorithm switch( algorithm ) { // currently all models - case FPW: - case FVW: - case RPW: - case RVW: + case FPW, FVW, RPW, RVW: // displacement Vec3 disp = new Vec3( initialState.velocity() ); disp.multiply( dt ); @@ -238,8 +246,7 @@ private static DrmState computeFixedStateAfter( DeadReckoningAlgorithm algorithm switch( algorithm ) { // 'V'-type models - case FVW: - case RVW: + case FVW, RVW: // change in velocity Vec3 dv = new Vec3( initialState.acceleration() ); dv.multiply( dt ); @@ -265,8 +272,7 @@ private static DrmState computeFixedStateAfter( DeadReckoningAlgorithm algorithm switch( algorithm ) { // 'R'-type models - case RPW: - case RVW: + case RPW, RVW: // rotation Quaternion rotationQuaternion = DeadReckoningAlgorithm.makeRotationQuaternion( initialState.angularVelocity(), dt ); orientation = Optional.of( orientation.orElseGet(initialState::orientation).multiply(rotationQuaternion) ); @@ -296,6 +302,17 @@ private static DrmState computeFixedStateAfter( DeadReckoningAlgorithm algorithm */ private static DrmState computeRotatingStateAfter( DeadReckoningAlgorithm algorithm, DrmState initialState, double dt ) { + // * If updating this to add additional algorithms, make + // * sure to update the switch statements below as well + switch( algorithm ) + { + case FPB, FVB, RPB, RVB: + break; + + case STATIC, FPW, FVW, RPW, RVW, OTHER: + throw new IllegalArgumentException( "Invalid Dead Reckoning Algorithm for this computation type" ); + } + Optional position = Optional.empty(); Optional orientation = Optional.empty(); @@ -325,13 +342,10 @@ private static DrmState computeRotatingStateAfter( DeadReckoningAlgorithm algori switch( algorithm ) { // currently all models - case FPB: - case FVB: - case RPB: - case RVB: + case FPB, FVB, RPB, RVB: // R_1 Mat3x3 R_1 = new Mat3x3( wwT ).multiply( (wdt - sin_wdt) / w_mag3 ); - R_1.add( Mat3x3.Identity().multiply(sin_wdt / w_mag) ); + R_1.add( Mat3x3.identity().multiply(sin_wdt / w_mag) ); R_1.add( new Mat3x3(skew).multiply((1 - cos_wdt) / w_mag2) ); // displacement @@ -350,21 +364,20 @@ private static DrmState computeRotatingStateAfter( DeadReckoningAlgorithm algori switch( algorithm ) { // 'V'-type models - case FVB: - case RVB: + case FVB, RVB: double w_mag4 = w_mag2 * w_mag2; // R_2 double cpwdtsm1 = cos_wdt + wdt*sin_wdt - 1; Mat3x3 R_2 = new Mat3x3( wwT ).multiply( (0.5*w_mag2*dt*dt - cpwdtsm1) / w_mag4 ); - R_2.add( Mat3x3.Identity().multiply(cpwdtsm1 / w_mag2) ); + R_2.add( Mat3x3.identity().multiply(cpwdtsm1 / w_mag2) ); R_2.add( new Mat3x3(skew).multiply((sin_wdt - wdt*cos_wdt) / w_mag3) ); // A_b; time derivative of V_b, computed as a_b - skew * V_b Vec3 A_b = new Vec3( initialState.acceleration() ); A_b.subtract( new Mat3x3(skew).multiply(V_b) ); - - // TODO compute new velocity if needed + + // compute the new velocity here if needed // displacement Vec3 disp = R_2.multiply( A_b ).rotate( initialState.orientation() ); @@ -382,8 +395,7 @@ private static DrmState computeRotatingStateAfter( DeadReckoningAlgorithm algori switch( algorithm ) { // 'R'-type models - case RPB: - case RVB: + case RPB, RVB: // rotation Quaternion rotationQuaternion = DeadReckoningAlgorithm.makeRotationQuaternion( initialState.angularVelocity(), dt ); orientation = Optional.of( orientation.orElseGet(initialState::orientation).multiply(rotationQuaternion) ); @@ -392,7 +404,7 @@ private static DrmState computeRotatingStateAfter( DeadReckoningAlgorithm algori default: break; } - + return new DrmState( position.orElse(initialState.position()), new Vec3(initialState.velocity()), new Vec3(initialState.acceleration()), @@ -405,15 +417,15 @@ private static DrmState computeRotatingStateAfter( DeadReckoningAlgorithm algori */ public enum ReferenceFrame { - Other, - + OTHER, + /** * The 'World coordinate system' as defined in the DIS spec (1.6.3.1, IEEE 1278.1-2012), a * NIMA TR 8350.2 and WGS 84 based right-handed geocentric Cartesian coordinate system. * * See {@link WorldCoordinate} for more information. */ - WorldCoordinates, + WORLD_COORDINATES, /** * The 'Entity coordinate system' as defined in the DIS spec (1.6.3.2, IEEE 1278.1-2012), @@ -427,6 +439,6 @@ public enum ReferenceFrame *
  • {@code z}: positive is below the entity
    • * */ - BodyCoordinates; + BODY_COORDINATES; } } diff --git a/codebase/src/java/disco/org/openlvc/disco/pdu/record/DeadReckoningParameter.java b/codebase/src/java/disco/org/openlvc/disco/pdu/record/DeadReckoningParameter.java index e6a91e39..85427916 100644 --- a/codebase/src/java/disco/org/openlvc/disco/pdu/record/DeadReckoningParameter.java +++ b/codebase/src/java/disco/org/openlvc/disco/pdu/record/DeadReckoningParameter.java @@ -51,7 +51,7 @@ public class DeadReckoningParameter implements IPduComponent, Cloneable //---------------------------------------------------------- public DeadReckoningParameter() { - this( DeadReckoningAlgorithm.Static, + this( DeadReckoningAlgorithm.STATIC, new byte[OTHER_PARAMETERS_ARRAY_SIZE], new VectorRecord(), new AngularVelocityVector() ); diff --git a/codebase/src/java/disco/org/openlvc/disco/pdu/record/EulerAngles.java b/codebase/src/java/disco/org/openlvc/disco/pdu/record/EulerAngles.java index db46e7a7..eea4b8cc 100644 --- a/codebase/src/java/disco/org/openlvc/disco/pdu/record/EulerAngles.java +++ b/codebase/src/java/disco/org/openlvc/disco/pdu/record/EulerAngles.java @@ -18,6 +18,7 @@ package org.openlvc.disco.pdu.record; import java.io.IOException; +import java.util.Objects; import org.openlvc.disco.pdu.DisInputStream; import org.openlvc.disco.pdu.DisOutputStream; @@ -35,15 +36,15 @@ public class EulerAngles implements IPduComponent, Cloneable //---------------------------------------------------------- // STATIC VARIABLES //---------------------------------------------------------- - public static float PSI_MIN = -(float)Math.PI; - public static float PSI_MAX = (float)Math.PI; + public static final float PSI_MIN = -(float)Math.PI; + public static final float PSI_MAX = (float)Math.PI; - public static float THETA_MIN = -(float)(Math.PI / 2); - public static float THETA_MAX = (float)(Math.PI / 2); + public static final float THETA_MIN = -(float)(Math.PI / 2); + public static final float THETA_MAX = (float)(Math.PI / 2); + + public static final float PHI_MIN = -(float)Math.PI; + public static final float PHI_MAX = (float)Math.PI; - public static float PHI_MIN = -(float)Math.PI; - public static float PHI_MAX = (float)Math.PI; - //---------------------------------------------------------- // INSTANCE VARIABLES //---------------------------------------------------------- @@ -58,7 +59,7 @@ public EulerAngles() { this( 0f, 0f, 0f ); } - + public EulerAngles( float psi, float theta, float phi ) { this.psi = psi; @@ -75,19 +76,19 @@ public boolean equals( Object other ) if( this == other ) return true; - if( other instanceof EulerAngles ) - { - EulerAngles otherAngle = (EulerAngles)other; - // psi and phi are continuous (yaw, roll), but theta is discontinuous (pitch) - if( FloatingPointUtils.floatRadEqual(otherAngle.psi,this.psi) && - FloatingPointUtils.floatEqual(otherAngle.theta,this.theta) && - FloatingPointUtils.floatRadEqual(otherAngle.phi,this.phi) ) - { - return true; - } - } - - return false; + if( !(other instanceof EulerAngles otherAngle) ) + return false; + + // psi and phi are continuous (yaw, roll), but theta is discontinuous (pitch) + return FloatingPointUtils.floatRadEqual( otherAngle.psi, this.psi ) && + FloatingPointUtils.floatEqual( otherAngle.theta, this.theta ) && + FloatingPointUtils.floatRadEqual( otherAngle.phi, this.phi ); + } + + @Override + public int hashCode() + { + return Objects.hash( this.psi, this.theta, this.phi ); } @Override @@ -102,9 +103,9 @@ public String toString() return "EulerAngles[psi=%f, theta=%f, phi=%f]".formatted( this.psi, this.theta, this.phi ); } - //////////////////////////////////////////////////////////////////////////////////////////// - /// IPduComponent Methods //////////////////////////////////////////////////////////////// - //////////////////////////////////////////////////////////////////////////////////////////// + //========================================================================================== + //--------------------------------- IPduComponent Methods ---------------------------------- + //========================================================================================== @Override public void from( DisInputStream dis ) throws IOException { @@ -120,16 +121,16 @@ public void to( DisOutputStream dos ) throws IOException dos.writeFloat( theta ); dos.writeFloat( phi ); } - + @Override public int getByteLength() { return DisSizes.FLOAT32_SIZE * 3; } - - //////////////////////////////////////////////////////////////////////////////////////////// - /// Accessor and Mutator Methods ///////////////////////////////////////////////////////// - //////////////////////////////////////////////////////////////////////////////////////////// + + //========================================================================================== + //------------------------------ Accessor and Mutator Methods ------------------------------ + //========================================================================================== public float getPsi() { return psi; diff --git a/codebase/src/java/disco/org/openlvc/disco/utils/LruCache.java b/codebase/src/java/disco/org/openlvc/disco/utils/LruCache.java index 0c444809..8dfbc285 100644 --- a/codebase/src/java/disco/org/openlvc/disco/utils/LruCache.java +++ b/codebase/src/java/disco/org/openlvc/disco/utils/LruCache.java @@ -19,6 +19,7 @@ import java.util.LinkedHashMap; import java.util.Map; +import java.util.Objects; // based on (not thread safe) implementation in https://stackoverflow.com/questions/40239485/concurrent-lru-cache-implementation @@ -33,12 +34,12 @@ public class LruCache extends LinkedHashMap //---------------------------------------------------------- @java.io.Serial private static final long serialVersionUID = 6228500846571341824L; - + //---------------------------------------------------------- // INSTANCE VARIABLES //---------------------------------------------------------- private final int capacity; - + //---------------------------------------------------------- // CONSTRUCTORS //---------------------------------------------------------- @@ -48,14 +49,32 @@ public LruCache( int capacity ) super( capacity + 1, 1.0f, true ); this.capacity = capacity; } - + //---------------------------------------------------------- // INSTANCE METHODS //---------------------------------------------------------- - - //////////////////////////////////////////////////////////////////////////////////////////// - /////////////////////////////// Accessor and Mutator Methods /////////////////////////////// - //////////////////////////////////////////////////////////////////////////////////////////// + + @Override + public boolean equals( Object other ) + { + if( this == other ) + return true; + + if( !(other instanceof LruCache otherLruCache) ) + return false; + + return super.equals( otherLruCache ); + } + + @Override + public int hashCode() + { + return Objects.hash( super.hashCode(), this.capacity ); + } + + //========================================================================================== + //------------------------------ Accessor and Mutator Methods ------------------------------ + //========================================================================================== public int getCapacity() { return this.capacity; @@ -66,7 +85,7 @@ protected boolean removeEldestEntry( final Map.Entry eldest ) { return super.size() > capacity; } - + //---------------------------------------------------------- // STATIC METHODS //---------------------------------------------------------- diff --git a/codebase/src/java/disco/org/openlvc/disco/utils/Mat3x3.java b/codebase/src/java/disco/org/openlvc/disco/utils/Mat3x3.java index 244cd5cc..3ecfa996 100644 --- a/codebase/src/java/disco/org/openlvc/disco/utils/Mat3x3.java +++ b/codebase/src/java/disco/org/openlvc/disco/utils/Mat3x3.java @@ -52,7 +52,7 @@ public Mat3x3( Mat3x3 original ) new Vec3( original.b ), new Vec3( original.c ) ); } - + //---------------------------------------------------------- // INSTANCE METHODS //---------------------------------------------------------- @@ -122,9 +122,9 @@ public Quaternion toQuaternion() return result; } - //////////////////////////////////////////////////////////////////////////////////////////// - /////////////////////////////// Accessor and Mutator Methods /////////////////////////////// - //////////////////////////////////////////////////////////////////////////////////////////// + //========================================================================================== + //------------------------------ Accessor and Mutator Methods ------------------------------ + //========================================================================================== /** * Computes the transpose of this matrix. * @@ -136,7 +136,7 @@ public Mat3x3 transpose() new Vec3(this.a.y, this.b.y, this.c.y), new Vec3(this.a.z, this.b.z, this.c.z) ); } - + /** * Scales this {@link Mat3x3} by the given value. * @@ -203,7 +203,7 @@ public Mat3x3 add( Mat3x3 rhs ) * * @return an 3x3 identity matrix */ - public static Mat3x3 Identity() + public static Mat3x3 identity() { return new Mat3x3( new Vec3(1, 0, 0), new Vec3(0, 1, 0), diff --git a/codebase/src/java/disco/org/openlvc/disco/utils/Quaternion.java b/codebase/src/java/disco/org/openlvc/disco/utils/Quaternion.java index 9444049d..d53fdce8 100644 --- a/codebase/src/java/disco/org/openlvc/disco/utils/Quaternion.java +++ b/codebase/src/java/disco/org/openlvc/disco/utils/Quaternion.java @@ -17,6 +17,8 @@ */ package org.openlvc.disco.utils; +import java.util.Objects; + import org.openlvc.disco.pdu.record.EulerAngles; public class Quaternion @@ -53,7 +55,7 @@ public Quaternion( Quaternion q ) { this( q.w, q.x, q.y, q.z ); } - + //---------------------------------------------------------- // INSTANCE METHODS //---------------------------------------------------------- @@ -62,7 +64,7 @@ public boolean equals( Object other ) { if( this == other ) return true; - + if( !(other instanceof Quaternion otherQuat) ) return false; @@ -72,6 +74,12 @@ public boolean equals( Object other ) FloatingPointUtils.doubleEqual( otherQuat.z, this.z ); } + @Override + public int hashCode() + { + return Objects.hash( this.w, this.x, this.y, this.z ); + } + /** * Indicates whether the rotation represented by some other object is "equal to" this one. *

    @@ -102,11 +110,10 @@ public String toString() */ public Quaternion multiply( Quaternion rhs ) { - double w = this.w * rhs.w - this.x * rhs.x - this.y * rhs.y - this.z * rhs.z; - double x = this.w * rhs.x + this.x * rhs.w + this.y * rhs.z - this.z * rhs.y; - double y = this.w * rhs.y - this.x * rhs.z + this.y * rhs.w + this.z * rhs.x; - double z = this.w * rhs.z + this.x * rhs.y - this.y * rhs.x + this.z * rhs.w; - return new Quaternion( w, x, y, z ); + return new Quaternion( this.w * rhs.w - this.x * rhs.x - this.y * rhs.y - this.z * rhs.z, + this.w * rhs.x + this.x * rhs.w + this.y * rhs.z - this.z * rhs.y, + this.w * rhs.y - this.x * rhs.z + this.y * rhs.w + this.z * rhs.x, + this.w * rhs.z + this.x * rhs.y - this.y * rhs.x + this.z * rhs.w ); } /** @@ -133,7 +140,7 @@ public Quaternion inverseSign() { return new Quaternion( -this.w, -this.x, -this.y, -this.z ); } - + /** * Convert this Quaternion to an Euler which follows the DIS PDU orientation conventions. * @@ -176,13 +183,13 @@ public EulerAngles toPduEulerAngles() // regular pitch pitch = Math.asin( pitchRatio ); } - + return new EulerAngles( (float)yaw, (float)pitch, (float)roll ); } - //////////////////////////////////////////////////////////////////////////////////////////// - /// Accessor and Mutator Methods ///////////////////////////////////////////////////////// - //////////////////////////////////////////////////////////////////////////////////////////// + //========================================================================================== + //------------------------------ Accessor and Mutator Methods ------------------------------ + //========================================================================================== //---------------------------------------------------------- // STATIC METHODS diff --git a/codebase/src/java/disco/org/openlvc/disco/utils/Vec3.java b/codebase/src/java/disco/org/openlvc/disco/utils/Vec3.java index 7eef783f..12f3c318 100644 --- a/codebase/src/java/disco/org/openlvc/disco/utils/Vec3.java +++ b/codebase/src/java/disco/org/openlvc/disco/utils/Vec3.java @@ -17,6 +17,8 @@ */ package org.openlvc.disco.utils; +import java.util.Objects; + import org.openlvc.disco.pdu.record.WorldCoordinate; public class Vec3 @@ -24,7 +26,7 @@ public class Vec3 //---------------------------------------------------------- // STATIC VARIABLES //---------------------------------------------------------- - + //---------------------------------------------------------- // INSTANCE VARIABLES //---------------------------------------------------------- @@ -44,21 +46,21 @@ public Vec3( Vec3 other ) { this( other.x, other.y, other.z ); } - + public Vec3( double x, double y, double z ) { this.x = x; this.y = y; this.z = z; } - + public Vec3( WorldCoordinate ecef ) { this.x = ecef.getX(); this.y = ecef.getY(); this.z = ecef.getZ(); } - + //---------------------------------------------------------- // INSTANCE METHODS //---------------------------------------------------------- @@ -67,7 +69,7 @@ public boolean equals( Object other ) { if( this == other ) return true; - + if( !(other instanceof Vec3 otherVec3) ) return false; @@ -75,7 +77,13 @@ public boolean equals( Object other ) FloatingPointUtils.doubleEqual( otherVec3.y, this.y ) && FloatingPointUtils.doubleEqual( otherVec3.z, this.z ); } - + + @Override + public int hashCode() + { + return Objects.hash( this.x, this.y, this.z ); + } + @Override public String toString() { @@ -163,7 +171,7 @@ public Vec3 multiply( double rhs ) this.z *= rhs; return this; } - + /** * Divide this {@link Vec3} by the given value * @@ -189,7 +197,7 @@ public double dot( Vec3 v ) { return this.x * v.x + this.y * v.y + this.z * v.z; } - + /** * Obtain the cross product of this {@link Vec3} and another * @@ -210,7 +218,7 @@ public Mat3x3 outer( Vec3 v ) new Vec3(this).multiply(v.y), new Vec3(this).multiply(v.z) ); } - + /** * Normalizes this {@link Vec3} * @@ -232,15 +240,15 @@ public Vec3 normalize() */ public Vec3 rotate( Quaternion q ) { - Quaternion v_prime = new Quaternion( 0, this.x, this.y, this.z ); - Quaternion conjugate_result = q.multiply( v_prime.multiply(q.conjugate()) ); - // conjugate_result.w should be 0 - return new Vec3( conjugate_result.x, conjugate_result.y, conjugate_result.z ); + Quaternion vPrime = new Quaternion( 0, this.x, this.y, this.z ); + Quaternion conjugateResult = q.multiply( vPrime.multiply(q.conjugate()) ); + // conjugateResult.w should be 0 + return new Vec3( conjugateResult.x, conjugateResult.y, conjugateResult.z ); } - - //////////////////////////////////////////////////////////////////////////////////////////// - /// Accessor and Mutator Methods ///////////////////////////////////////////////////////// - //////////////////////////////////////////////////////////////////////////////////////////// + + //========================================================================================== + //------------------------------ Accessor and Mutator Methods ------------------------------ + //========================================================================================== //---------------------------------------------------------- // STATIC METHODS diff --git a/codebase/src/java/test/org/openlvc/disco/application/EntityStateStoreTest.java b/codebase/src/java/test/org/openlvc/disco/application/EntityStateStoreTest.java index 90ab45b4..e34258dc 100644 --- a/codebase/src/java/test/org/openlvc/disco/application/EntityStateStoreTest.java +++ b/codebase/src/java/test/org/openlvc/disco/application/EntityStateStoreTest.java @@ -52,7 +52,7 @@ public class EntityStateStoreTest extends AbstractTest //---------------------------------------------------------- private DisApplication local; private OpsCenter remote; - + //---------------------------------------------------------- // CONSTRUCTORS //---------------------------------------------------------- @@ -61,14 +61,14 @@ public class EntityStateStoreTest extends AbstractTest // INSTANCE METHODS //---------------------------------------------------------- - /////////////////////////////////////////////////////////////////////////////////// - /// Test Class Setup/Tear Down ////////////////////////////////////////////////// - /////////////////////////////////////////////////////////////////////////////////// + //========================================================================================== + //------------------------------- Test Class Setup/Tear Down ------------------------------- + //========================================================================================== @BeforeClass(alwaysRun=true) public void beforeClass() { } - + @BeforeMethod(alwaysRun=true) public void beforeMethod() { @@ -79,7 +79,7 @@ public void beforeMethod() // create the OpsCenter that will mimic a "remote" DIS application DiscoConfiguration remoteConfiguration = new DiscoConfiguration(); this.remote = super.newOpsCenter( remoteConfiguration ); - + // open for bizness! this.local.start(); this.remote.open(); @@ -91,28 +91,29 @@ public void afterMethod() this.local.stop(); this.remote.close(); } - + @AfterClass(alwaysRun=true) public void afterClass() { } - /////////////////////////////////////////////////////////////////////////////////// - /// PDU Testing Methods ///////////////////////////////////////////////////////// - /////////////////////////////////////////////////////////////////////////////////// + //========================================================================================== + //---------------------------------- PDU Testing Methods ----------------------------------- + //========================================================================================== @Test public void testEntityStatePduStore() { // 1. Create the entity we want to track as a remote - EntityStatePdu source = new EntityStatePdu(); - EntityId entityId = new EntityId( 12, 13, 14 ); - EntityType entityType = new EntityType( 1, 1, 225, 2, 4, 6, 8 ); - WorldCoordinate location = new WorldCoordinate( 31.9505, 115.8605, 100 ); - EulerAngles orientation = new EulerAngles( 1.0f, 2.0f, 3.0f ); + final EntityStatePdu source = new EntityStatePdu(); + final EntityId entityId = new EntityId( 12, 13, 14 ); + final EntityType entityType = new EntityType( 1, 1, 225, 2, 4, 6, 8 ); + final WorldCoordinate location = new WorldCoordinate( 31.9505, 115.8605, 100 ); + final String marking = "PhatEntity"; + final EulerAngles orientation = new EulerAngles( 1.0f, 2.0f, 3.0f ); source.setEntityID( entityId ); source.setEntityType( entityType ); source.setLocation( location ); - source.setMarking( "PhatEntity" ); + source.setMarking( marking ); source.setOrientation( orientation ); // 2. Send it out @@ -122,46 +123,47 @@ public void testEntityStatePduStore() EntityStateStore entityStore = local.getPduStore().getEntityStore(); // wait for the store to pick the change up - super.waitFor( () -> {return entityStore.size() > 0;}, 100 ); + super.waitFor( () -> entityStore.size() > 0, 100 ); // check to make sure the store has what we want - Assert.assertTrue( entityStore.hasEntityState("PhatEntity") ); + Assert.assertTrue( entityStore.hasEntityState(marking) ); Assert.assertEquals( entityStore.size(), 1 ); // Check the entity values - DrEntityStatePdu received = entityStore.getEntityState("PhatEntity"); + DrEntityStatePdu received = entityStore.getEntityState( marking ); Assert.assertTrue( received instanceof EntityStatePdu ); Assert.assertEquals( received.getEntityID(), entityId ); Assert.assertEquals( received.getEntityType(), entityType ); Assert.assertEquals( received.getLocation(), location ); Assert.assertEquals( received.getOrientation(), orientation ); - + // Check property fetch methods Set markings = entityStore.getAllMarkings(); Assert.assertEquals( markings.size(), 1 ); - Assert.assertTrue( markings.contains("PhatEntity") ); - + Assert.assertTrue( markings.contains(marking) ); + // 4. Update the PDU to change some key settings and make sure that // these are reflected accurately - source.setMarking( "Ph@tEntity" ); + final String modifiedMarking = "Ph@tEntity"; + source.setMarking( modifiedMarking ); remote.send( source ); - super.waitFor( () -> entityStore.hasEntityState("Ph@tEntity") ); - + super.waitFor( () -> entityStore.hasEntityState(modifiedMarking) ); + // we got the update, make sure everything is still cool Assert.assertEquals( entityStore.size(), 1 ); // Check the entity values - received = entityStore.getEntityState("PhatEntity"); + received = entityStore.getEntityState( marking ); Assert.assertNull( received ); - received = entityStore.getEntityState("Ph@tEntity"); + received = entityStore.getEntityState( modifiedMarking ); Assert.assertEquals( received.getEntityID(), entityId ); Assert.assertEquals( received.getEntityType(), entityType ); Assert.assertEquals( received.getLocation(), location ); Assert.assertEquals( received.getOrientation(), orientation ); - + // Check property fetch methods markings = entityStore.getAllMarkings(); Assert.assertEquals( markings.size(), 1 ); - Assert.assertFalse( markings.contains("PhatEntity") ); - Assert.assertTrue( markings.contains("Ph@tEntity") ); + Assert.assertFalse( markings.contains(marking) ); + Assert.assertTrue( markings.contains(modifiedMarking) ); } @Test(dependsOnMethods={"testEntityStatePduStore"}) @@ -178,7 +180,7 @@ public void testDrDisabled() // setup the model EntityStatePdu source = new EntityStatePdu(); DeadReckoningParameter deadReckoningParams = new DeadReckoningParameter(); - + EntityId entityId = new EntityId( 12, 13, 14 ); long localTimestamp = 10; DeadReckoningAlgorithm deadReckoningAlgorithm = DeadReckoningAlgorithm.RVW; diff --git a/codebase/src/java/test/org/openlvc/disco/application/pdu/DrEntityStatePduTest.java b/codebase/src/java/test/org/openlvc/disco/application/pdu/DrEntityStatePduTest.java index b83acd6b..2ae320b0 100644 --- a/codebase/src/java/test/org/openlvc/disco/application/pdu/DrEntityStatePduTest.java +++ b/codebase/src/java/test/org/openlvc/disco/application/pdu/DrEntityStatePduTest.java @@ -53,9 +53,9 @@ public class DrEntityStatePduTest // INSTANCE METHODS //---------------------------------------------------------- - /////////////////////////////////////////////////////////////////////////////////// - /// Test Class Setup/Tear Down ////////////////////////////////////////////////// - /////////////////////////////////////////////////////////////////////////////////// + //========================================================================================== + //------------------------------- Test Class Setup/Teat Down ------------------------------- + //========================================================================================== @BeforeClass(alwaysRun = true) public void beforeClass() { @@ -76,16 +76,16 @@ public void afterClass() { } - /////////////////////////////////////////////////////////////////////////////////// - /// Testing Methods ///////////////////////////////////////////////////////////// - /////////////////////////////////////////////////////////////////////////////////// + //========================================================================================== + //------------------------------------ Testing Methods ------------------------------------- + //========================================================================================== @Test public void testDrEntityRVW() { // setup the model EntityStatePdu entityPdu = new EntityStatePdu(); DeadReckoningParameter deadReckoningParams = new DeadReckoningParameter(); - + long localTimestamp = 10; DeadReckoningAlgorithm deadReckoningAlgorithm = DeadReckoningAlgorithm.RVW; @@ -143,7 +143,7 @@ public void testDrEntityCaching() // setup the model EntityStatePdu entityPdu = new EntityStatePdu(); DeadReckoningParameter deadReckoningParams = new DeadReckoningParameter(); - + long localTimestamp = 10; DeadReckoningAlgorithm deadReckoningAlgorithm = DeadReckoningAlgorithm.RVW; @@ -186,7 +186,7 @@ public void testDrEntityCaching() Assert.assertEquals( drmState3s.getLinearAcceleration(), acceleration ); Assert.assertEquals( drmState3s.getOrientation(), new EulerAngles(0.0f, 0.6f, 0.0f) ); Assert.assertEquals( drmState3s.getAngularVelocity(), angularVelocity ); - + // check we get the same objects when fetching those times again (cache hits) DrmState drmState1s_2 = drEntityPdu.getDrmStateAtLocalTime( drEntityPdu.getDefaultDeadReckoningAlgorithm(), t1s ); Assert.assertTrue( drmState1s_2 == drmState1s ); @@ -217,7 +217,7 @@ public void testNoAlgorithm() DrEntityStatePdu drEntityPdu = new DrEntityStatePdu( entityPdu ); // verify the selected DRM is a Static model - Assert.assertEquals( drEntityPdu.getDefaultDeadReckoningAlgorithm(), DeadReckoningAlgorithm.Static ); + Assert.assertEquals( drEntityPdu.getDefaultDeadReckoningAlgorithm(), DeadReckoningAlgorithm.STATIC ); } @Test(dependsOnMethods={"testDrEntityRVW"}) @@ -226,7 +226,7 @@ public void testOverrideNoAlgorithm() // setup the model EntityStatePdu entityPdu = new EntityStatePdu(); DeadReckoningParameter deadReckoningParams = new DeadReckoningParameter(); - + long localTimestamp = 10; DeadReckoningAlgorithm deadReckoningAlgorithm = DeadReckoningAlgorithm.RVW; @@ -260,12 +260,12 @@ public void testOverrideNoAlgorithm() Assert.assertEquals( drEntityPdu.getInitialDrmState(), expectedInitialState ); // check the state after 1s - DrmState drmState1s = drEntityPdu.getDrmStateAtLocalTime( DeadReckoningAlgorithm.Static, + DrmState drmState1s = drEntityPdu.getDrmStateAtLocalTime( DeadReckoningAlgorithm.STATIC, localTimestamp + 1000 ); Assert.assertEquals( drmState1s, expectedInitialState ); // check the state after 3s - DrmState drmState3s = drEntityPdu.getDrmStateAtLocalTime( DeadReckoningAlgorithm.Static, + DrmState drmState3s = drEntityPdu.getDrmStateAtLocalTime( DeadReckoningAlgorithm.STATIC, localTimestamp + 3000 ); Assert.assertEquals( drmState3s, expectedInitialState ); } @@ -276,7 +276,7 @@ public void testDrDisabled() // setup the model EntityStatePdu entityPdu = new EntityStatePdu(); DeadReckoningParameter deadReckoningParams = new DeadReckoningParameter(); - + long localTimestamp = 10; DeadReckoningAlgorithm deadReckoningAlgorithm = DeadReckoningAlgorithm.RVW; @@ -315,7 +315,7 @@ public void testFrozenEntity() // setup the model EntityStatePdu entityPdu = new EntityStatePdu(); DeadReckoningParameter deadReckoningParams = new DeadReckoningParameter(); - + long localTimestamp = 10; DeadReckoningAlgorithm deadReckoningAlgorithm = DeadReckoningAlgorithm.RVW; @@ -387,7 +387,7 @@ public void testOutdatedTimestampBehavior() long oldTimestamp = localTimestamp - 5; Assert.assertEquals( drEntityPdu.getOutdatedTimestampBehavior(), OutdatedTimestampBehavior.ERROR ); Assert.assertThrows( IllegalArgumentException.class, () -> drEntityPdu.getDrLocation(oldTimestamp) ); - + // we should be able to make it use the latest instead drEntityPdu.setOutdatedTimestampBehavior( OutdatedTimestampBehavior.USE_CURRENT_STATE ); Assert.assertEquals( drEntityPdu.getOutdatedTimestampBehavior(), OutdatedTimestampBehavior.USE_CURRENT_STATE ); diff --git a/codebase/src/java/test/org/openlvc/disco/pdu/field/DeadReckoningAlgorithmTest.java b/codebase/src/java/test/org/openlvc/disco/pdu/field/DeadReckoningAlgorithmTest.java index 6dda0a6e..e88f18e2 100644 --- a/codebase/src/java/test/org/openlvc/disco/pdu/field/DeadReckoningAlgorithmTest.java +++ b/codebase/src/java/test/org/openlvc/disco/pdu/field/DeadReckoningAlgorithmTest.java @@ -19,6 +19,7 @@ import java.util.Arrays; import java.util.Iterator; +import java.util.NoSuchElementException; import java.util.Random; import org.openlvc.disco.application.utils.DrmState; @@ -56,9 +57,9 @@ public class DeadReckoningAlgorithmTest // INSTANCE METHODS //---------------------------------------------------------- - /////////////////////////////////////////////////////////////////////////////////// - /// Test Class Setup/Tear Down ////////////////////////////////////////////////// - /////////////////////////////////////////////////////////////////////////////////// + //========================================================================================== + //------------------------------- Test Class Setup/Tear Down ------------------------------- + //========================================================================================== @BeforeClass(alwaysRun = true) public void beforeClass() { @@ -79,9 +80,9 @@ public void afterClass() { } - /////////////////////////////////////////////////////////////////////////////////// - /// Testing Methods ///////////////////////////////////////////////////////////// - /////////////////////////////////////////////////////////////////////////////////// + //========================================================================================== + //------------------------------------ Testing Methods ------------------------------------- + //========================================================================================== @Test public void testRVW() { @@ -117,7 +118,7 @@ public void testRVWRotation( RotationTestSet rotTest ) // check the state after 1s DrmState dtState1s = DeadReckoningAlgorithm.RVW.computeStateAfter( initialState, 1.0 ); Assert.assertEquals( dtState1s, new DrmState(location, velocity, acceleration, rotTest.outOrientation1s, rotTest.angularVelocity) ); - + // check the state after 3s DrmState dtState3s = DeadReckoningAlgorithm.RVW.computeStateAfter( initialState, 3.0 ); Assert.assertEquals( dtState3s, new DrmState(location, velocity, acceleration, rotTest.outOrientation3s, rotTest.angularVelocity) ); @@ -139,7 +140,8 @@ public void testRVBCircularRandom( CircularTestParams testParams ) private void testRVBCircular( CircularTestParams testParams ) { // circular motion parameters - double period = testParams.period, radius = testParams.radius; // seconds, metres + double period = testParams.period; // seconds + double radius = testParams.radius; // metres double w = Math.TAU / period; // angular velocity w.r.t. circle centre - also angular velocity of entity (in -z, for left-hand turn) double v = w * radius; // forwards velocity (in +x) @@ -183,7 +185,7 @@ private void testRVBCircular( CircularTestParams testParams ) Assert.assertEquals( newState1_5x.velocity(), initialState.velocity() ); Assert.assertEquals( newState1_5x.acceleration(), initialState.acceleration() ); Assert.assertEquals( newState1_5x.angularVelocity(), initialState.angularVelocity() ); - + // check the state after 5 laps - should be (approx) back at the start DrmState newState5x = DeadReckoningAlgorithm.RVB.computeStateAfter( initialState, period * 5 ); Assert.assertEquals( newState5x.position().distance(initialState.position()) / radius, // error @@ -229,10 +231,10 @@ private static record CircularTestParams( double period, EulerAngles orientation ) { } - - /////////////////////////////////////////////////////////////////////////////////// - /// Data Providers ////////////////////////////////////////////////////////////// - /////////////////////////////////////////////////////////////////////////////////// + + //========================================================================================== + //------------------------------------- Data Providers ------------------------------------- + //========================================================================================== @DataProvider(name="axisRotations",parallel=true) public static Iterator bounds() { @@ -257,8 +259,8 @@ public static Iterator circleParams() { return new Iterator() { // arbitrary axis bounds - static final float SPATIAL_AXIS_MIN = -10 * 1000; // 10km - static final float SPATIAL_AXIS_MAX = 1000 * 1000; // 1000km + static final double SPATIAL_AXIS_MIN = -10 * 1000d; // 10km + static final double SPATIAL_AXIS_MAX = 1000 * 1000d; // 1000km final Random rand = new Random(); int i = 0; @@ -270,16 +272,19 @@ public boolean hasNext() } @Override - public CircularTestParams next() + public CircularTestParams next() throws NoSuchElementException { + if( !this.hasNext() ) + throw new NoSuchElementException(); + i++; double period = rand.nextDouble( 0.1, 7200 ); // 0.1s - 2h double radius = rand.nextDouble( 0.1, 100000 ); // 10cm - 100km - float x = rand.nextFloat( SPATIAL_AXIS_MIN, SPATIAL_AXIS_MAX ); - float y = rand.nextFloat( SPATIAL_AXIS_MIN, SPATIAL_AXIS_MAX ); - float z = rand.nextFloat( SPATIAL_AXIS_MIN, SPATIAL_AXIS_MAX ); + double x = rand.nextDouble( SPATIAL_AXIS_MIN, SPATIAL_AXIS_MAX ); + double y = rand.nextDouble( SPATIAL_AXIS_MIN, SPATIAL_AXIS_MAX ); + double z = rand.nextDouble( SPATIAL_AXIS_MIN, SPATIAL_AXIS_MAX ); float psi = rand.nextFloat( EulerAngles.PSI_MIN, EulerAngles.PSI_MAX ); float theta = rand.nextFloat( EulerAngles.THETA_MIN * (86.3f / 90), diff --git a/codebase/src/java/test/org/openlvc/disco/utils/QuaternionTest.java b/codebase/src/java/test/org/openlvc/disco/utils/QuaternionTest.java index c250c913..0b335e35 100644 --- a/codebase/src/java/test/org/openlvc/disco/utils/QuaternionTest.java +++ b/codebase/src/java/test/org/openlvc/disco/utils/QuaternionTest.java @@ -19,6 +19,7 @@ import java.util.ArrayList; import java.util.Iterator; +import java.util.NoSuchElementException; import java.util.Random; import org.openlvc.disco.pdu.record.EulerAngles; @@ -49,14 +50,14 @@ public class QuaternionTest // INSTANCE METHODS //---------------------------------------------------------- - /////////////////////////////////////////////////////////////////////////////////// - /// Test Class Setup/Tear Down ////////////////////////////////////////////////// - /////////////////////////////////////////////////////////////////////////////////// + //========================================================================================== + //------------------------------- Test Class Setup/Tear Down ------------------------------- + //========================================================================================== @BeforeClass(alwaysRun=true) public void beforeClass() { } - + @BeforeMethod(alwaysRun=true) public void beforeMethod() { @@ -66,15 +67,15 @@ public void beforeMethod() public void afterMethod() { } - + @AfterClass(alwaysRun=true) public void afterClass() { } - /////////////////////////////////////////////////////////////////////////////////// - /// Testing Helpers ///////////////////////////////////////////////////////////// - /////////////////////////////////////////////////////////////////////////////////// + //========================================================================================== + //------------------------------------ Testing Helpers ------------------------------------- + //========================================================================================== private void assertEulerAngleInBounds( EulerAngles orientation ) { Assert.assertTrue( orientation.getPsi() <= EulerAngles.PSI_MAX ); @@ -86,7 +87,7 @@ private void assertEulerAngleInBounds( EulerAngles orientation ) Assert.assertTrue( orientation.getPhi() <= EulerAngles.PHI_MAX ); Assert.assertTrue( EulerAngles.PHI_MIN <= orientation.getPhi() ); } - + private void testQuaternionEulerAngleConversion( EulerAngles testOrientation, EulerAngles expectedOrientation ) { @@ -110,11 +111,9 @@ private void testQuaternionEulerAngleConversion( EulerAngles testOrientation ) testQuaternionEulerAngleConversion( testOrientation, testOrientation ); } - /////////////////////////////////////////////////////////////////////////////////// - /// Testing Methods ///////////////////////////////////////////////////////////// - /////////////////////////////////////////////////////////////////////////////////// - // TODO test with known quaternion output? - + //========================================================================================== + //------------------------------------ Testing Methods ------------------------------------- + //========================================================================================== @Test(dataProvider="eulerAngleBounds") public void testQuaternionEulerAnglesBounds( EulerAngles testOrientation ) { @@ -151,13 +150,15 @@ public void testQuaternionEulerAnglesSingularitiesRandom( EulerAngles testOrient testQuaternionEulerAnglesSingularityBounds( testOrientation ); } + // TODO test with known quaternion output? + //---------------------------------------------------------- // STATIC METHODS //---------------------------------------------------------- - /////////////////////////////////////////////////////////////////////////////////// - /// Data Providers ////////////////////////////////////////////////////////////// - /////////////////////////////////////////////////////////////////////////////////// + //========================================================================================== + //------------------------------------- Data Providers ------------------------------------- + //========================================================================================== @DataProvider(name="eulerAngleBounds",parallel=true) public static Iterator bounds() { @@ -221,8 +222,11 @@ public boolean hasNext() } @Override - public EulerAngles next() + public EulerAngles next() throws NoSuchElementException { + if( !this.hasNext() ) + throw new NoSuchElementException(); + i++; float psi = rand.nextFloat( EulerAngles.PSI_MIN, EulerAngles.PSI_MAX );