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() ); } //---------------------------------------------------------- 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..a339a5de 100644 --- a/codebase/src/java/disco/org/openlvc/disco/application/EntityStateStore.java +++ b/codebase/src/java/disco/org/openlvc/disco/application/EntityStateStore.java @@ -25,7 +25,10 @@ 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; @@ -42,35 +45,47 @@ public class EntityStateStore implements IDeleteReaperManaged //---------------------------------------------------------- // INSTANCE VARIABLES //---------------------------------------------------------- - private ConcurrentMap byId; - private ConcurrentMap byMarking; + private ConcurrentMap byId; + private ConcurrentMap byMarking; + + private PduStore parentStore; //---------------------------------------------------------- // CONSTRUCTORS //---------------------------------------------------------- - protected EntityStateStore( PduStore store ) + protected EntityStateStore( PduStore parentStore ) { this.byId = new ConcurrentHashMap<>(); this.byMarking = new ConcurrentHashMap<>(); + + this.parentStore = parentStore; } //---------------------------------------------------------- // INSTANCE METHODS //---------------------------------------------------------- + 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 + DrEntityStatePdu wrappedPdu = this.isDrEnabled() ? new DrEntityStatePdu( pdu ) + : new DisabledDrEntityStatePdu( 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. // @@ -79,67 +94,70 @@ 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(pdu.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( pdu.getMarking(), pdu ); + 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 ); } - 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 ); - + return byMarking.get( marking ); } - - public EntityStatePdu getEntityState( EntityId id ) + + /** + * @param id + * @return the last {@link DrEntityStatePdu} for the entity, or `null` if not stored + */ + 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. @@ -147,27 +165,29 @@ 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 null; + return this.byId.values().parallelStream() + .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. * - * @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 */ - public Set getEntityStatesNear( EntityStatePdu entity, int radiusMeters ) + public Set getEntityStatesNear( EntityStatePdu entity, int radiusMeters ) { 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. @@ -176,16 +196,16 @@ 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 ) .collect( Collectors.toSet() ); } - //////////////////////////////////////////////////////////////////////////////////////////// - /// Delete Timeout Support Methods /////////////////////////////////////////////////////// - //////////////////////////////////////////////////////////////////////////////////////////// + //========================================================================================== + //----------------------------- Delete Timeout Support Methods ----------------------------- + //========================================================================================== @Override public int removeStaleData( long oldestTimestamp ) { @@ -202,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(); @@ -225,4 +244,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/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..05e12f1f --- /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 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 final DrmState initialDrmState; + private LruCache drmStateCache; // access must be synchronized + private OutdatedTimestampBehavior outdatedTimestampBehavior; + + //---------------------------------------------------------- + // CONSTRUCTORS + //---------------------------------------------------------- + public DrEntityStatePdu( EntityStatePdu pdu ) + { + super( pdu ); + + this.initialDrmState = new DrmState( pdu.getLocation(), + pdu.getLinearVelocity(), + pdu.getDeadReckoningParams().getEntityLinearAcceleration(), + pdu.getOrientation(), + pdu.getDeadReckoningParams().getEntityAngularVelocity() ); + + this.setDrmStateCacheSize( 3 ); + this.setOutdatedTimestampBehavior( OutdatedTimestampBehavior.ERROR ); + } + + //---------------------------------------------------------- + // 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 has finished being processed + switch( this.getOutdatedTimestampBehavior() ) + { + case USE_CURRENT_STATE: + localTimestamp = this.getLocalTimestamp(); + break; + + case ERROR: + 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 ) + { + 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 = (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 + // coord systems + initialState = switch( algorithm.getReferenceFrame() ) // what frame are we switching _to_ + { + // 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 ); + 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 synchronized 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 synchronized 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 getDefaultDeadReckoningAlgorithm() + { + 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.getDefaultDeadReckoningAlgorithm(), 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.getDefaultDeadReckoningAlgorithm(), 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.getDefaultDeadReckoningAlgorithm(), 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 ) + { + } + + /** + * 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..04a2018c --- /dev/null +++ b/codebase/src/java/disco/org/openlvc/disco/application/utils/DrmState.java @@ -0,0 +1,196 @@ +/* + * 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.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(Vec3 otherPosition, + Vec3 otherVelocity, + Vec3 otherAcceleration, + Quaternion otherOrientation, + Vec3 otherAngularVelocity)) ) + return false; + + 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() ); + } + + /** + * 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/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/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..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 @@ -17,17 +17,25 @@ */ 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 { //---------------------------------------------------------- // VALUES //---------------------------------------------------------- - Other ( (short)0 ), - Static( (short)1 ), + OTHER ( (short)0 ), + STATIC( (short)1 ), FPW ( (short)2 ), RPW ( (short)3 ), RVW ( (short)4 ), @@ -58,6 +66,85 @@ public short value() return this.value; } + @Override + public String toString() + { + switch( this ) + { + 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" ); + else + return "Other"; + } + + public ReferenceFrame getReferenceFrame() + { + switch( this ) + { + 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( String.format("Unknown reference frame for dead-reckoning algorithm: %s (%d)", + 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 WORLD_COORDINATES: + return DeadReckoningAlgorithm.computeFixedStateAfter( this, initialState, dt ); + + case BODY_COORDINATES: + return DeadReckoningAlgorithm.computeRotatingStateAfter( this, initialState, dt ); + + case OTHER: // fall through + } + + // fallback to static + // TODO warn? + return initialState; + } + //---------------------------------------------------------- // STATIC METHODS //---------------------------------------------------------- @@ -70,23 +157,288 @@ 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; - default: // drop through + case 1: return STATIC; + + case 2: return FPW; + case 5: return FVW; + case 3: return RPW; + case 4: return RVW; + + case 6: return FPB; + case 9: return FVB; + 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 + 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 ) + { + // * 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(); + Optional orientation = Optional.empty(); + + // effects due to velocity + switch( algorithm ) + { + // currently all models + case FPW, FVW, RPW, 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, 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, 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 ) + { + // * 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(); + + // 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, 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( 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, 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) ); + + // compute the new velocity here 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, 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. + */ + WORLD_COORDINATES, + + /** + * 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
    • + *
+ */ + BODY_COORDINATES; + } } 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/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 8a4c21c6..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,6 +36,14 @@ public class EulerAngles implements IPduComponent, Cloneable //---------------------------------------------------------- // STATIC VARIABLES //---------------------------------------------------------- + public static final float PSI_MIN = -(float)Math.PI; + public static final float PSI_MAX = (float)Math.PI; + + 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; //---------------------------------------------------------- // INSTANCE VARIABLES @@ -50,7 +59,7 @@ public EulerAngles() { this( 0f, 0f, 0f ); } - + public EulerAngles( float psi, float theta, float phi ) { this.psi = psi; @@ -67,18 +76,19 @@ public boolean equals( Object other ) if( this == other ) return true; - if( other instanceof EulerAngles ) - { - EulerAngles otherAngle = (EulerAngles)other; - if( FloatingPointUtils.floatEqual(otherAngle.psi,this.psi) && - FloatingPointUtils.floatEqual(otherAngle.theta,this.theta) && - FloatingPointUtils.floatEqual(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 @@ -87,9 +97,15 @@ public EulerAngles clone() return new EulerAngles( psi, theta, phi ); } - //////////////////////////////////////////////////////////////////////////////////////////// - /// IPduComponent Methods //////////////////////////////////////////////////////////////// - //////////////////////////////////////////////////////////////////////////////////////////// + @Override + public String toString() + { + return "EulerAngles[psi=%f, theta=%f, phi=%f]".formatted( this.psi, this.theta, this.phi ); + } + + //========================================================================================== + //--------------------------------- IPduComponent Methods ---------------------------------- + //========================================================================================== @Override public void from( DisInputStream dis ) throws IOException { @@ -105,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/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/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/LruCache.java b/codebase/src/java/disco/org/openlvc/disco/utils/LruCache.java new file mode 100644 index 00000000..8dfbc285 --- /dev/null +++ b/codebase/src/java/disco/org/openlvc/disco/utils/LruCache.java @@ -0,0 +1,92 @@ +/* + * 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; +import java.util.Objects; + +// 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 + //---------------------------------------------------------- + + @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; + } + + @Override + 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 489f4332..3ecfa996 100644 --- a/codebase/src/java/disco/org/openlvc/disco/utils/Mat3x3.java +++ b/codebase/src/java/disco/org/openlvc/disco/utils/Mat3x3.java @@ -38,6 +38,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; @@ -51,11 +52,11 @@ public Mat3x3( Mat3x3 original ) new Vec3( original.b ), new Vec3( original.c ) ); } - + //---------------------------------------------------------- // 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, @@ -121,11 +122,91 @@ public Quaternion toQuaterion() return result; } - //////////////////////////////////////////////////////////////////////////////////////////// - /////////////////////////////// Accessor and Mutator Methods /////////////////////////////// - //////////////////////////////////////////////////////////////////////////////////////////// + //========================================================================================== + //------------------------------ 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..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 @@ -24,7 +26,6 @@ public class Quaternion //---------------------------------------------------------- // STATIC VARIABLES //---------------------------------------------------------- - private static final float _180DEGREES = (float)Math.PI; //---------------------------------------------------------- // INSTANCE VARIABLES @@ -49,19 +50,97 @@ 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 ); + } + + @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. + *

+ * 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 ) + { + 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 ); + } + + /** + * 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() { - 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; - return new Quaternion( w, x, y, z ); - } - + 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 +151,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) ); - - return new EulerAngles( (float)yaw, (float)pitch, (float)(roll-_180DEGREES)); + 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 ); } - + + //========================================================================================== + //------------------------------ 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 +223,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..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,24 +46,50 @@ 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 //---------------------------------------------------------- + @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 int hashCode() + { + return Objects.hash( this.x, this.y, 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,8 +114,64 @@ 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,25 +181,44 @@ 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 * * @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,10 +231,24 @@ public Vec3 normalize() double vecLength = this.length(); return divide( vecLength ); } - - //////////////////////////////////////////////////////////////////////////////////////////// - /// Accessor and Mutator Methods ///////////////////////////////////////////////////////// - //////////////////////////////////////////////////////////////////////////////////////////// + + /** + * 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 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 ------------------------------ + //========================================================================================== //---------------------------------------------------------- // 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..e34258dc 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 { //---------------------------------------------------------- @@ -46,7 +52,7 @@ public class EntityStateStoreTest extends AbstractTest //---------------------------------------------------------- private DisApplication local; private OpsCenter remote; - + //---------------------------------------------------------- // CONSTRUCTORS //---------------------------------------------------------- @@ -55,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() { @@ -73,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(); @@ -85,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 @@ -116,45 +123,129 @@ 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 - EntityStatePdu 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"}) + 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(); + } } //---------------------------------------------------------- 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..2ae320b0 --- /dev/null +++ b/codebase/src/java/test/org/openlvc/disco/application/pdu/DrEntityStatePduTest.java @@ -0,0 +1,405 @@ +/* + * 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.EntityStateStore.DisabledDrEntityStatePdu; +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/Teat 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 ); + drEntityPdu.setDrmStateCacheSize( 2 ); + + // check the initial values are what we set + 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.getDefaultDeadReckoningAlgorithm(), + 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.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) ); + 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 ); + drEntityPdu.setDrmStateCacheSize( 2 ); + + // check the state after 1s + long t1s = localTimestamp + 1000; + 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 ); + 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.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 ); + 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 ); + 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.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 ); + 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.getDefaultDeadReckoningAlgorithm(), 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.getDefaultDeadReckoningAlgorithm(), 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 ); + drEntityPdu.setDrmStateCacheSize( 2 ); + + // check the initial values are what we set + Assert.assertEquals( drEntityPdu.getDefaultDeadReckoningAlgorithm(), 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 = new DisabledDrEntityStatePdu( entityPdu ); + + // check the state after 3s + DrmState drmState3s = drEntityPdu.getDrmStateAtLocalTime( drEntityPdu.getDefaultDeadReckoningAlgorithm(), + 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.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) ); + 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.getDefaultDeadReckoningAlgorithm(), + 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..e88f18e2 --- /dev/null +++ b/codebase/src/java/test/org/openlvc/disco/pdu/field/DeadReckoningAlgorithmTest.java @@ -0,0 +1,298 @@ +/* + * 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.NoSuchElementException; +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; // 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) + 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 double SPATIAL_AXIS_MIN = -10 * 1000d; // 10km + static final double SPATIAL_AXIS_MAX = 1000 * 1000d; // 1000km + + final Random rand = new Random(); + int i = 0; + + @Override + public boolean hasNext() + { + return i < 100; + } + + @Override + 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 + + 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), + 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) ); + } + }; + } +} 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..0b335e35 --- /dev/null +++ b/codebase/src/java/test/org/openlvc/disco/utils/QuaternionTest.java @@ -0,0 +1,242 @@ +/* + * 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.NoSuchElementException; +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 ------------------------------------- + //========================================================================================== + @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 ); + } + + // TODO test with known quaternion output? + + //---------------------------------------------------------- + // 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 } ) + { + // 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, + 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() throws NoSuchElementException + { + if( !this.hasNext() ) + throw new NoSuchElementException(); + + 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) ); + float phi = rand.nextFloat( EulerAngles.PHI_MIN, EulerAngles.PHI_MAX ); + + return new EulerAngles( psi, theta, phi ); + } + }; + } +}