Merge pull request #38 from camejia/master

Woof. I'm about the sickest ever for the last week and a half, but it all seems good to me. Looks like nice stuff, well thought out. It's great that we can start using this finally.
open-dis · May 17, 2017 · a1052cf · a1052cf
2 parents 116733d + f7c2d06
commit a1052cf
Show file tree

Hide file tree

Showing 29 changed files with 2,405 additions and 3,455 deletions.
diff --git a/pom.xml b/pom.xml
@@ -53,6 +53,11 @@
             <version>4.12</version>
             <scope>test</scope>
         </dependency>
+        <dependency>
+            <groupId>org.apache.commons</groupId>
+            <artifactId>commons-math3</artifactId>
+            <version>3.6</version>
+        </dependency>
     </dependencies>
     <build>
         <plugins>

diff --git a/src/main/java/edu/nps/moves/deadreckoning/DIS_DR_FPB_06.java b/src/main/java/edu/nps/moves/deadreckoning/DIS_DR_FPB_06.java
@@ -1,6 +1,6 @@
 package edu.nps.moves.deadreckoning;
 
-import edu.nps.moves.deadreckoning.utils.*;
+import org.apache.commons.math3.geometry.euclidean.threed.Vector3D;
 
 /**
  * 
@@ -17,11 +17,6 @@
  */
 public class DIS_DR_FPB_06 extends DIS_DeadReckoning
 {     
-    Matrix initInv;
-
-    double[] velVec = {entityLinearVelocity_X, entityLinearVelocity_Y, entityLinearVelocity_Z};
-    double[] updated;
-
     /**
      * The driver for a DIS_DR_FPB_06 DR algorithm from the Runnable interface
      * <p>
@@ -31,59 +26,29 @@ public void run()
     {
         try
         {            
-            initInv = Matrix.transpose(initOrien);            
-            //initInv = Matrix.inversMat3x3(initOrien);
-
             while(true)
             {
-                deltaCt++;
                 Thread.sleep(stall);   
-
-                // solve for the new position
-                updated = Matrix.multVec(initInv, makeR1());
-
-                // set the new position...
-                entityLocation_X += updated[0];
-                entityLocation_Y += updated[1];
-                entityLocation_Z += updated[2];
-
+                update();
             }//while(true)  
         }// try
         catch(Exception e)
         {   
             System.out.println(e);     
         }
     }//run()--------------------------------------------------------------------
-
-
-
-    /***************************************************************************
-     * Makes the R1 matrix
-     * @return - the vector R1
-     * @throws java.lang.Exception
-     */
-    private double[] makeR1() throws Exception
-    {
-        Matrix R1 = new Matrix(3); 
-        Matrix ident = new Matrix(3);  
-
-        // common factors
-        double wDelta = wMag * changeDelta * deltaCt;  
-
-        // matrix scalars
-        double wwScale = (wDelta-Math.sin(wDelta)) / (wSq * wMag); 
-        double identScalar = Math.sin(wDelta) / wMag;
-        double skewScale = 1 - (Math.cos(wDelta) / wSq);
-
-        // scaled matrixes
-        Matrix wwTmp = ww.mult(wwScale);
-        Matrix identTmp = ident.mult(identScalar);
-        Matrix skwTmp = skewOmega.mult(skewScale);
-
-        R1 = Matrix.add(wwTmp, identTmp);
-        R1 = Matrix.subtract(R1, skwTmp);  
-
-        return Matrix.multVec(R1, velVec);
-    }//makeR1() throws Exception------------------------------------------------
 
-}
+    void update() {
+        deltaCt++;
+        // solve for the new position
+        Vector3D velVec = new Vector3D(entityLinearVelocity_X, entityLinearVelocity_Y, entityLinearVelocity_Z);
+        // For fixed (non-rotating), R1 = changeDelta * Identity
+        Vector3D updated = initOrien.applyInverseTo(velVec.scalarMultiply(changeDelta));
+
+        // set the new position...
+        entityLocation_X += updated.getX();
+        entityLocation_Y += updated.getY();
+        entityLocation_Z += updated.getZ();
+    }
+
+}
diff --git a/src/main/java/edu/nps/moves/deadreckoning/DIS_DR_FPW_02.java b/src/main/java/edu/nps/moves/deadreckoning/DIS_DR_FPW_02.java
@@ -1,7 +1,5 @@
 package edu.nps.moves.deadreckoning;
 
-import edu.nps.moves.deadreckoning.utils.*;
-
 /**
  *
  * (PRIMARY Methods group) Fixed, Rate of Positon, World || Constant Linear motion
@@ -10,7 +8,7 @@
  */
 public class DIS_DR_FPW_02 extends DIS_DeadReckoning
 {
-    
+
     /***************************************************************************
      * The driver for a DIS_DR_FPW_02 DR algorithm from the Runnable interface
      * <p>
@@ -26,14 +24,17 @@ public void run()
     {
         while(true)
         {
-            deltaCt++;   
             try
             {    Thread.sleep(stall);    }catch (Exception e){}     
-
-            entityLocation_X += entityLinearVelocity_X * changeDelta;
-            entityLocation_Y += entityLinearVelocity_Y * changeDelta;
-            entityLocation_Z += entityLinearVelocity_Z * changeDelta;
+            update();
         }
     }//run()--------------------------------------------------------------------
-
-}
+
+    void update() {
+        deltaCt++;   
+        entityLocation_X += entityLinearVelocity_X * changeDelta;
+        entityLocation_Y += entityLinearVelocity_Y * changeDelta;
+        entityLocation_Z += entityLinearVelocity_Z * changeDelta;
+
+    }
+}
diff --git a/src/main/java/edu/nps/moves/deadreckoning/DIS_DR_FVB_09.java b/src/main/java/edu/nps/moves/deadreckoning/DIS_DR_FVB_09.java
@@ -1,6 +1,6 @@
 package edu.nps.moves.deadreckoning;
 
-import edu.nps.moves.deadreckoning.utils.*;
+import org.apache.commons.math3.geometry.euclidean.threed.Vector3D;
 
 /**
  * (SECONDARY Methods Group) Fixed, rate of velocity, body coordinates || 
@@ -16,13 +16,6 @@
  */
 public class DIS_DR_FVB_09 extends DIS_DeadReckoning
 {
-    Matrix initInv;
-    double[] velVec = {entityLinearVelocity_X, entityLinearVelocity_Y, entityLinearVelocity_Z};
-    double[] angVec = {entityAngularVelocity_X,entityAngularVelocity_Y,entityAngularVelocity_Z};
-
-    double[] updated1 = new double[3]; 
-    double[] updated2 = new double[3]; 
-    double[] updated = new double[3];
 
     /**
      * The driver for a DIS_DR_FVB_09 DR algorithm from the Runnable interface
@@ -33,100 +26,37 @@ public void run()
     {
         try
         {           
-            initInv = Matrix.transpose(initOrien);              
-
             while(true)
             {
-                deltaCt++;
                 Thread.sleep(stall);    
-
-                updated1 = makeR1();
-                updated2 = makeR2();
-
-                updated[0] = updated1[0] + updated2[0];
-                updated[1] = updated1[1] + updated2[1];
-                updated[2] = updated1[2] + updated2[2];                
-
-                updated = Matrix.multVec(initInv, updated);
-
-                entityLocation_X += updated[0];
-                entityLocation_Y += updated[1];
-                entityLocation_Z += updated[2];                         
+                update();
             }//while(true)  
         }// try
         catch(Exception e)
         {   
             System.out.println(e);     
         }
     }//run()--------------------------------------------------------------------
-
-
-
-    /***************************************************************************
-     * Makes the R2 vector
-     * @return - the r2 vector
-     * @throws java.lang.Exception
-     */
-    private double[] makeR2() throws Exception
-    {
-        Matrix R2 = new Matrix(3); 
-        Matrix ident = new Matrix(3);  
-
-        // common factors
-        double wDelta = wMag * changeDelta * deltaCt;  
-
-        // matrix scalars
-        double wwScale = .5*wSq*changeDelta * deltaCt*changeDelta * deltaCt;
-            wwScale -= Math.cos(wDelta);
-            wwScale -= wDelta*Math.sin(wDelta);
-            wwScale++;
-            wwScale /= wSq * wSq;
-
-        double identScalar = Math.cbrt(wDelta) + wDelta*Math.sin(wDelta) - 1;
-            identScalar /= wSq;
-
-        double skewScale = Math.sin(wDelta) - wDelta*Math.cos(wDelta);
-            skewScale /= wSq * wMag;
-
-        // scaled matrixes
-        Matrix wwTmp = ww.mult(wwScale);
-        Matrix identTmp = ident.mult(identScalar);
-        Matrix skwTmp = skewOmega.mult(skewScale);
-
-        R2 = Matrix.add(wwTmp, identTmp);
-        R2 = Matrix.add(R2, skwTmp);  
-
-        return Matrix.multVec(R2, angVec);
-    }//makeR2() throws Exception------------------------------------------------
-
-
-
-    /***************************************************************************
-     * Makes the R1 matrix
-     * @return - the vector R1
-     * @throws java.lang.Exception
-     */
-    private double[] makeR1() throws Exception
-    {
-        Matrix R1 = new Matrix(3); 
-        Matrix ident = new Matrix(3);  
-
-        // common factors
-        double wDelta = wMag * changeDelta * deltaCt;  
-
-        // matrix scalars
-        double wwScale = (wDelta-Math.sin(wDelta)) / (wSq * wMag); 
-        double identScalar = Math.sin(wDelta) / wMag;
-        double skewScale = 1 - (Math.cos(wDelta) / wSq);
-
-        // scaled matrixes
-        Matrix wwTmp = ww.mult(wwScale);
-        Matrix identTmp = ident.mult(identScalar);
-        Matrix skwTmp = skewOmega.mult(skewScale);
-
-        R1 = Matrix.add(wwTmp, identTmp);
-        R1 = Matrix.subtract(R1, skwTmp);  
-
-        return Matrix.multVec(R1, velVec);
-    }//makeR1() throws Exception------------------------------------------------
-}
+
+    void update() {
+        deltaCt++;
+
+        Vector3D velVec = new Vector3D(entityLinearVelocity_X,     entityLinearVelocity_Y,     entityLinearVelocity_Z);
+        Vector3D accVec = new Vector3D(entityLinearAcceleration_X, entityLinearAcceleration_Y, entityLinearAcceleration_Z);
+
+        // add the R1 and R2
+        // Solve for new position 
+        // For fixed (non-rotating), R1 = changeDelta * Identity and R2 = 0.5 * changeDelta^2 * Identity
+        Vector3D updated = initOrien.applyInverseTo(velVec.add(0.5 * changeDelta, accVec).scalarMultiply(changeDelta));
+
+        // Set the new position
+        entityLocation_X += updated.getX();
+        entityLocation_Y += updated.getY();
+        entityLocation_Z += updated.getZ();                    
+
+        entityLinearVelocity_X += entityLinearAcceleration_X * changeDelta;
+        entityLinearVelocity_Y += entityLinearAcceleration_Y * changeDelta;
+        entityLinearVelocity_Z += entityLinearAcceleration_Z * changeDelta;
+    }
+
+}
diff --git a/src/main/java/edu/nps/moves/deadreckoning/DIS_DR_FVW_05.java b/src/main/java/edu/nps/moves/deadreckoning/DIS_DR_FVW_05.java
@@ -1,7 +1,5 @@
 package edu.nps.moves.deadreckoning;
 
-import edu.nps.moves.deadreckoning.utils.*;
-
 /**
  * (PRIMARY Methods group) Fixed, rate of velocity, world coordinates || Linear Motion with 
  * Acceleration but no rotation
@@ -19,13 +17,21 @@ public void run()
     {
         while(true)
         {
-            deltaCt++;   
             try
             {    Thread.sleep(stall);    }catch (Exception e){}     
-
-            entityLocation_X += (entityLinearVelocity_X * changeDelta) + (.5 * entityLinearAcceleration_X * changeDelta * changeDelta);
-            entityLocation_Y += (entityLinearVelocity_Y * changeDelta) + (.5 * entityLinearAcceleration_Y * changeDelta * changeDelta);
-            entityLocation_Z += (entityLinearVelocity_Z * changeDelta) + (.5 * entityLinearAcceleration_Z * changeDelta * changeDelta);
+            update();
         }
     }//run()--------------------------------------------------------------------
+
+    void update() {
+        deltaCt++;   
+
+        entityLocation_X += (entityLinearVelocity_X * changeDelta) + (.5 * entityLinearAcceleration_X * changeDelta * changeDelta);
+        entityLocation_Y += (entityLinearVelocity_Y * changeDelta) + (.5 * entityLinearAcceleration_Y * changeDelta * changeDelta);
+        entityLocation_Z += (entityLinearVelocity_Z * changeDelta) + (.5 * entityLinearAcceleration_Z * changeDelta * changeDelta);
+
+        entityLinearVelocity_X += entityLinearAcceleration_X * changeDelta;
+        entityLinearVelocity_Y += entityLinearAcceleration_Y * changeDelta;
+        entityLinearVelocity_Z += entityLinearAcceleration_Z * changeDelta;
+    }
 }