add selfTest, multiple utilities, manual (partial) deep copy of espdu

examples/src/OpenDis7Examples/PduTrack.java

@@ -50,9 +50,13 @@ public class PduTrack
 {   
     private String      descriptor = new String();
     private ArrayList<Pdu> pduList = new ArrayList<>();
+    private EntityStatePdu initialEspdu;
+    private EntityStatePdu  latestEspdu;
+    private Vector3Double  initialLocation;
+    private Vector3Double   latestLocation;
     
     private ArrayList<Vector3Double> waypointsList = new ArrayList<>();
-    private ArrayList<EulerAngles>      anglesList = new ArrayList<>();
+    private ArrayList<EulerAngles> eulerAnglesList = new ArrayList<>();
     /** timelineList in seconds */
     private ArrayList<Float>       timelineList = new ArrayList<>();
     
@@ -92,6 +96,34 @@ public class PduTrack
         return this;
     }
 
+    /**
+     * @return the initialLocation
+     */
+    public Vector3Double getInitialLocation() {
+        if (initialLocation == null)
+        {
+            System.out.println (TRACE_PREFIX + "getInitialLocation() not found, isTrackEmpty()=" + isTrackEmpty() + ", returning 0 0 0");
+            return new Vector3Double();
+        }
+        return initialLocation;
+    }
+    /**
+     * @return the latestLocation
+     */
+    public Vector3Double getLatestLocation() {
+        if (latestLocation == null)
+        {
+            System.out.println (TRACE_PREFIX + "getLatestLocation() not found, isTrackEmpty()=" + isTrackEmpty() + ", returning 0 0 0");
+            return new Vector3Double();
+        }
+        return latestLocation;
+    }
+    /**
+     * @return the pduList
+     */
+    public ArrayList<Pdu> getPduList() {
+        return pduList;
+    }
     /**
      * @return the waypointsList
      */
@@ -99,10 +131,10 @@ public class PduTrack
         return waypointsList;
     }
     /**
-     * @return the anglesList
+     * @return the eulerAnglesList
      */
-    public ArrayList<EulerAngles> getAnglesList() {
-        return anglesList;
+    public ArrayList<EulerAngles> getEulerAnglesList() {
+        return eulerAnglesList;
     }
     /**
      * Time in seconds corresponding to each PDU
@@ -118,7 +150,26 @@ public class PduTrack
      */
     public PduTrack addPdu(Pdu newPdu)
     {
-        pduList.add(newPdu);
+        if (newPdu.getPduType() == DisPduType.ENTITY_STATE)
+        {
+            EntityStatePdu deepCopyEspdu = new EntityStatePdu();
+            deepCopyEspdu.setEntityID         (((EntityStatePdu)newPdu).getEntityID());
+            deepCopyEspdu.setForceId          (((EntityStatePdu)newPdu).getForceId());
+            deepCopyEspdu.setEntityType       (((EntityStatePdu)newPdu).getEntityType());
+            deepCopyEspdu.setMarking          (((EntityStatePdu)newPdu).getMarking());
+            deepCopyEspdu.setEntityLocation   (((EntityStatePdu)newPdu).getEntityLocation());
+            deepCopyEspdu.setEntityOrientation(((EntityStatePdu)newPdu).getEntityOrientation());
+            pduList.add(deepCopyEspdu);
+            
+            if (initialLocation == null)
+            {
+                initialEspdu    = deepCopyEspdu; // must save object since pduList might contain more than ESPDUs
+                initialLocation = deepCopyEspdu.getEntityLocation();
+            }
+            latestEspdu         = deepCopyEspdu; // must save object since pduList might contain more than ESPDUs
+            latestLocation      = deepCopyEspdu.getEntityLocation();
+        }
+        else pduList.add(newPdu); // TODO copy() - careful, must be a new object and not a reference
         return this;
     }
     /**
@@ -127,10 +178,14 @@ public class PduTrack
      */
     public PduTrack clearPduLists()
     {
-              pduList.clear();
-        waypointsList.clear();
-           anglesList.clear();
-        timelineList.clear();
+           getPduList().clear();
+          waypointsList.clear();
+        eulerAnglesList.clear();
+           timelineList.clear();
+           initialEspdu = null;
+            latestEspdu = null;
+        initialLocation = null;
+         latestLocation = null;
         return this;
     }
 
@@ -198,6 +253,14 @@ public class PduTrack
         // https://stackoverflow.com/questions/16252269/how-to-sort-an-arraylist
         return this;
     }
+    /**
+     * Determine whether any ESPDUs have been received by this track
+     * @return whether track is empty
+     */
+    public boolean isTrackEmpty()
+    {
+        return (getEspduCount() == 0);
+    }
     /**
      * Compute track duration in timestamp ticks
      * @return duration in timestamp ticks between initial and final ESPDU timestamps in waypointList
@@ -205,9 +268,10 @@ public class PduTrack
     public int getEspduCount()
     {
         int counter = 0;
-        for (Pdu nextPdu : pduList)
+        for (Pdu nextPdu : getPduList())
         {
-            counter += 1;
+            if (nextPdu.getPduType() == DisPduType.ENTITY_STATE)
+               counter += 1;
         }
         return counter;
     }
@@ -222,7 +286,7 @@ public class PduTrack
         int  durationTicks = -1; // used if pduList is empty
         
         // must skip through pduList since non-ESPDU PDUs may be present
-        for (Pdu nextPdu : pduList)
+        for (Pdu nextPdu : getPduList())
         {
             if (nextPdu.getPduType() == DisPduType.ENTITY_STATE)
             {
@@ -233,7 +297,7 @@ public class PduTrack
         }
         if ((initialTime >= 0) && (finalTime >= 0))
              durationTicks = (finalTime - initialTime);
-        if (pduList.isEmpty())
+        if (getPduList().isEmpty())
         {
             System.out.println(TRACE_PREFIX + "getTrackDuration() computed illegal duration=" + durationTicks + " due to empty pdu list");
         }
@@ -265,26 +329,27 @@ public class PduTrack
     {
         // https://stackoverflow.com/questions/6536094/java-arraylist-copy
         
-        getWaypointsList().clear();
-           getAnglesList().clear();
-            float clock = 0.0f;
-        for (Pdu nextPdu : pduList)
+          waypointsList.clear();
+        eulerAnglesList.clear();
+        float clock = 0.0f;
+        for (int i = 0; i < pduList.size(); i++)
         {
+            Pdu nextPdu = pduList.get(i);
             if (nextPdu.getPduType() == DisPduType.ENTITY_STATE)
             {
                 EntityStatePdu espdu = (EntityStatePdu)nextPdu;
-                getWaypointsList().add(espdu.getEntityLocation());
-                   getAnglesList().add(espdu.getEntityOrientation());
+                  waypointsList.add(espdu.getEntityLocation());
+                eulerAnglesList.add(espdu.getEntityOrientation());
                    
-                   if (defaultWaypointInterval > 0)
-                   {
-                       timelineList.add(clock);
-                       clock += defaultWaypointInterval;
-                   }
-                   else
-                   {
-                       timelineList.add(espdu.getTimestamp() * 1.0f); // TODO convert
-                   }
+                if (defaultWaypointInterval > 0)
+                {
+                    timelineList.add(clock);
+                    clock += defaultWaypointInterval;
+                }
+                else
+                {
+                    timelineList.add(espdu.getTimestamp() * 1.0f); // TODO convert
+                }
             }
         }
         return this;
@@ -326,9 +391,9 @@ public class PduTrack
         for (int i = 0; i < waypointsList.size(); i++)
         {
             Vector3Double nextPosition = waypointsList.get(i);
-            sb.append(String.valueOf(nextPosition.getX()) + " " +
-                      String.valueOf(nextPosition.getY()) + " " +
-                      String.valueOf(nextPosition.getZ()));
+            sb.append(String.valueOf(nextPosition.getX())).append(" ")
+              .append(String.valueOf(nextPosition.getY())).append(" ")
+              .append(String.valueOf(nextPosition.getZ()));
             if (i < waypointsList.size() - 1)
                 sb.append(",");
         }
@@ -341,7 +406,7 @@ public class PduTrack
     /**
      * @return the wayPointInterval
      */
-    public float getWayPointInterval() {
+    public float getDefaultWaypointInterval() {
         return defaultWaypointInterval;
     }
 
@@ -349,7 +414,7 @@ public class PduTrack
      * Set uniform waypoint interval (currently for testing)
      * @param newWaypointInterval the wayPointInterval to set, in seconds, must be greater than zero
      * @return same object to permit progressive setters */
-    public PduTrack setWaypointInterval(float newWaypointInterval) {
+    public PduTrack setDefaultWaypointInterval(float newWaypointInterval) {
         if (newWaypointInterval > 0.0)
             this.defaultWaypointInterval = newWaypointInterval;
         else 
@@ -372,10 +437,29 @@ public class PduTrack
         return this;
     }
     
-    /** Self test to check basic operation, invoked by main() */
+    /** Self test to check basic operation, invoked by main()
+     */
     public void selfTest()
     {
-        // TODO
+        System.out.println(TRACE_PREFIX + "selfTest() start...");
+      
+        PduTrack pduTrack = new PduTrack();
+        pduTrack.setDescriptor("PduTrack selfTest()");
+        pduTrack.setDefaultWaypointInterval(1.0f);
+        
+        EntityStatePdu espdu = new EntityStatePdu();
+        espdu.setMarking("PduTrack");
+        for (int i = 0; i < 5; i++)
+        {
+            espdu.setEntityLocation(i, i, i);
+            pduTrack.addPdu(espdu);
+        }
+        
+        System.out.println(TRACE_PREFIX + "getEspduCount()="              + pduTrack.getEspduCount());
+        System.out.println(TRACE_PREFIX + "getDefaultWaypointInterval()=" + pduTrack.getDefaultWaypointInterval());
+        System.out.println(TRACE_PREFIX + "getTrackDurationSeconds()="    + pduTrack.getTrackDurationSeconds());
+        
+        System.out.println(TRACE_PREFIX + "selfTest() complete.");
     }
     
     /**