From 4cf0cb0ea0069d34ab6bc315e893237e406b1581 Mon Sep 17 00:00:00 2001
From: terry-norbraten <tnorb@comcast.net>
Date: Mon, 19 Jul 2021 17:34:13 -0700
Subject: [PATCH] [Terry N.] use enum to state which timestamp style to use for
a PDU. Resurrect an old math package required for Xj3D BasicESPDUTransform
---
build.xml | 3 +-
.../dis7/examples/ThreadedNetExample.java | 2 +-
.../nps/moves/dis7/utilities/PduFactory.java | 70 +-
src/edu/nps/moves/legacy/math/Matrix3f.java | 267 ++++++
src/edu/nps/moves/legacy/math/Matrix4f.java | 284 +++++++
src/edu/nps/moves/legacy/math/Quaternion.java | 775 ++++++++++++++++++
.../nps/moves/legacy/math/Quaternion2.java | 422 ++++++++++
src/edu/nps/moves/legacy/math/README.TXT | 11 +
src/edu/nps/moves/legacy/math/Vec3f.java | 287 +++++++
src/edu/nps/moves/legacy/math/Vec4f.java | 290 +++++++
src/edu/nps/moves/legacy/math/package.html | 16 +
.../nps/moves/dis7/AllPduRoundTripTest.java | 2 +-
test/edu/nps/moves/dis7/PduFactoryTest.java | 2 +-
13 files changed, 2408 insertions(+), 23 deletions(-)
create mode 100644 src/edu/nps/moves/legacy/math/Matrix3f.java
create mode 100644 src/edu/nps/moves/legacy/math/Matrix4f.java
create mode 100644 src/edu/nps/moves/legacy/math/Quaternion.java
create mode 100644 src/edu/nps/moves/legacy/math/Quaternion2.java
create mode 100644 src/edu/nps/moves/legacy/math/README.TXT
create mode 100644 src/edu/nps/moves/legacy/math/Vec3f.java
create mode 100644 src/edu/nps/moves/legacy/math/Vec4f.java
create mode 100644 src/edu/nps/moves/legacy/math/package.html
diff --git a/build.xml b/build.xml
index baf0ce0e9d..90efc74917 100644
--- a/build.xml
+++ b/build.xml
@@ -361,9 +361,10 @@
update="true"
zip64Mode="always">
<fileset dir="build/classes" defaultexcludes="yes">
+ <include name="**/edu/nps/moves/legacy/math/*"/>
<include name="**/edu/nps/moves/dis7/pdus/*"/>
- <include name="**/edu/nps/moves/dis7/utilities/**/*"/>
<include name="**/edu/nps/moves/spatial/*"/>
+ <include name="**/edu/nps/moves/dis7/utilities/**/*"/>
<exclude name="**/edu/nps/moves/dis7/entities/*"/>
<exclude name="**/edu/nps/moves/dis7/enumerations/*"/>
<exclude name="**/edu/nps/moves/dis7/jammers/*"/>
diff --git a/src/edu/nps/moves/dis7/examples/ThreadedNetExample.java b/src/edu/nps/moves/dis7/examples/ThreadedNetExample.java
index ce3c553d76..0764f43d08 100644
--- a/src/edu/nps/moves/dis7/examples/ThreadedNetExample.java
+++ b/src/edu/nps/moves/dis7/examples/ThreadedNetExample.java
@@ -35,7 +35,7 @@ public class ThreadedNetExample
// Use PduFactory to make pdus, default country = Deutschland, exercise, site, app, absolute timestamps
- PduFactory factory = new PduFactory(Country.GERMANY_DEU, (byte) 1, (short) 2, (short) 3, true);
+ PduFactory factory = new PduFactory(Country.GERMANY_DEU, (byte) 1, (short) 2, (short) 3, PduFactory.TimestampStyle.IEEE_ABSOLUTE);
// Make and send 3 pdus with no delay between, testing threaded receiver performance
netif.send(factory.makeEntityStatePdu());
diff --git a/src/edu/nps/moves/dis7/utilities/PduFactory.java b/src/edu/nps/moves/dis7/utilities/PduFactory.java
index ccae6e443f..cc132764f8 100644
--- a/src/edu/nps/moves/dis7/utilities/PduFactory.java
+++ b/src/edu/nps/moves/dis7/utilities/PduFactory.java
@@ -33,6 +33,8 @@ import java.util.stream.Stream;
*/
public class PduFactory
{
+ public enum TimestampStyle {IEEE_ABSOLUTE, IEEE_RELATIVE, NPS, UNIX};
+
private Country country = Country.UNITED_STATES_OF_AMERICA_USA;
private byte defaultExerciseId = 1;
private short defaultSiteId = 2;
@@ -42,14 +44,21 @@ public class PduFactory
private Method getTime;
private boolean useFastPdu = false;
+
+ /** We can marshal the PDU with a timestamp set to any of several styles.
+ * Remember, you MUST set a timestamp. DIS will regard multiple packets sent
+ * with the same timestamp as duplicates and may discard them.
+ */
+ private TimestampStyle timestampStyle = TimestampStyle.IEEE_ABSOLUTE;
/**
- * Create a PduFactory using defaults for country (USA), exerciseId (2), application (3) and absolute timestamps.
+ * Create a PduFactory using defaults for country (USA), exerciseId (2),
+ * application (3) and absolute timestamps.
*/
public PduFactory()
{
this.disTime = new DisTime();
- getTimeStampMethod();
+ setTimeStampStyle(timestampStyle);
}
/**
@@ -58,34 +67,57 @@ public class PduFactory
* @param exerciseId used in standard PDU header
* @param siteId used in standard PDU header
* @param applicationId used in standard PDU header
- * @param useAbsoluteTimestamp boolean to specify absolute time stamps (IEEE Std 1278.1-2012, 4.6)
+ * @param timestampStyle enum to specify time stamp style (IEEE Std 1278.1-2012, 4.6)
* @see edu.nps.moves.dis7.pdus.EntityType
* @see edu.nps.moves.dis7.pdus.RadioType
*/
- public PduFactory(Country country, byte exerciseId, short siteId, short applicationId, boolean useAbsoluteTimestamp)
+ public PduFactory(Country country, byte exerciseId, short siteId, short applicationId, TimestampStyle timestampStyle)
{
- this.disTime = new DisTime();
+ this();
this.country = country;
this.defaultExerciseId = exerciseId;
this.defaultSiteId = siteId;
this.defaultAppId = applicationId;
-
- this.useAbsoluteTimestamp = useAbsoluteTimestamp;
- getTimeStampMethod();
+
+ setTimeStampStyle(timestampStyle);
+ }
+
+ /** Set one of four styles. IEEE_ABSOLUTE, IEEE_RELATIVE, NPS, or UNIX
+ *
+ * @param ts the timestamp style to set for this PDU
+ */
+ public final void setTimeStampStyle(TimestampStyle ts) {
+ timestampStyle = ts;
+ setTimeStampMethod();
}
- private void getTimeStampMethod()
- {
- try {
- if (useAbsoluteTimestamp)
- getTime = DisTime.class.getDeclaredMethod("getDisAbsoluteTimestamp", new Class<?>[0]);
- else
- getTime = DisTime.class.getDeclaredMethod("getDisRelativeTimestamp", new Class<?>[0]);
- }
- catch (NoSuchMethodException ex) {
- throw new RuntimeException(ex);
+ private void setTimeStampMethod() {
+ try {
+ switch (timestampStyle) {
+ case IEEE_ABSOLUTE:
+ getTime = DisTime.class.getDeclaredMethod("getDisAbsoluteTimestamp", new Class<?>[0]);
+ break;
+
+ case IEEE_RELATIVE:
+ getTime = DisTime.class.getDeclaredMethod("getDisRelativeTimestamp", new Class<?>[0]);
+ break;
+
+ case NPS:
+ getTime = DisTime.class.getDeclaredMethod("getNpsTimestamp", new Class<?>[0]);
+ break;
+
+ case UNIX:
+ getTime = DisTime.class.getDeclaredMethod("getUnixTimestamp", new Class<?>[0]);
+ break;
+
+ default:
+ getTime = DisTime.class.getDeclaredMethod("getDisAbsoluteTimestamp", new Class<?>[0]);
+ break;
+ }
+ } catch (NoSuchMethodException ex) {
+ throw new RuntimeException(ex);
+ }
}
- }
/**
* Use the default or fast method to create EntityState pdus from input byte streams.
diff --git a/src/edu/nps/moves/legacy/math/Matrix3f.java b/src/edu/nps/moves/legacy/math/Matrix3f.java
new file mode 100644
index 0000000000..c27ee550e3
--- /dev/null
+++ b/src/edu/nps/moves/legacy/math/Matrix3f.java
@@ -0,0 +1,267 @@
+/*
+Copyright (c) 1995-2021 held by the author(s). All rights reserved.
+
+Redistribution and use in source and binary forms, with or without
+modification, are permitted provided that the following conditions
+are met:
+
+ * Redistributions of source code must retain the above copyright
+ notice, this list of conditions and the following disclaimer.
+ * Redistributions in binary form must reproduce the above copyright
+ notice, this list of conditions and the following disclaimer
+ in the documentation and/or other materials provided with the
+ distribution.
+ * Neither the names of the Naval Postgraduate School (NPS)
+ Modeling Virtual Environments and Simulation (MOVES) Institute
+ (https://www.nps.edu and https://my.nps.edu/web/moves)
+ nor the names of its contributors may be used to endorse or
+ promote products derived from this software without specific
+ prior written permission.
+
+THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+"AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+POSSIBILITY OF SUCH DAMAGE.
+*/
+package edu.nps.moves.legacy.math;
+
+// Standard classes
+// none
+
+/**
+ * EXECUTIVE SUMMARY
+ * Module Name: Matrix3f.java
+ * Description: Definition of the Matrix3f class
+ * @author Kent A. Watsen, http://www.mbay.net/~watsen
+ */
+public class Matrix3f
+ {
+ private
+ float m[][];
+
+ public Matrix3f()
+ {
+ m = new float[3][3];
+ makeNull();
+ }
+
+ public Matrix3f(float mat[][])
+ {
+ m = new float[3][3];
+ setMat(mat);
+ }
+
+ public Matrix3f(Matrix3f mat)
+ {
+ m = new float[3][3];
+ setMat(mat);
+ }
+
+ public Matrix3f(Quaternion quat)
+ {
+ setQuat(quat);
+ }
+
+ public Matrix3f(float hpr[])
+ {
+ m = new float[3][3];
+ setEulers(hpr);
+ }
+
+ public Matrix3f(float heading, float pitch, float roll)
+ {
+ m = new float[3][3];
+ setEulers(heading, pitch, roll);
+ }
+
+ public void print()
+ {
+ System.out.println("m = " + m[0][0] + ", " + m[0][1] + ", " + m[0][2]
+ + m[1][0] + ", " + m[1][1] + ", " + m[1][2]
+ + m[2][0] + ", " + m[2][1] + ", " + m[2][2]);
+ }
+
+ public void setMatValue(int row, int col, float val)
+ {
+ if (row < 0 || row > 3 || col < 0 || col > 3)
+ return;
+ m[row][col] = val;
+ }
+
+ public float getMatValue(int row, int col)
+ {
+ if (row < 0 || row > 3 || col < 0 || col > 3)
+ return 0.0f;
+ return m[row][col];
+ }
+
+ public void setMat(float mat[][])
+ {
+ m[0][0] = mat[0][0];
+ m[0][1] = mat[0][1];
+ m[0][2] = mat[0][2];
+ m[1][0] = mat[1][0];
+ m[1][1] = mat[1][1];
+ m[1][2] = mat[1][2];
+ m[2][0] = mat[2][0];
+ m[2][1] = mat[2][1];
+ m[2][2] = mat[2][2];
+ }
+
+ public void getMat(float mat[][])
+ {
+ mat[0][0] = m[0][0];
+ mat[0][1] = m[0][1];
+ mat[0][2] = m[0][2];
+ mat[1][0] = m[1][0];
+ mat[1][1] = m[1][1];
+ mat[1][2] = m[1][2];
+ mat[2][0] = m[2][0];
+ mat[2][1] = m[2][1];
+ mat[2][2] = m[2][2];
+ }
+
+ public void setMat(Matrix3f mat)
+ {
+ float mat2[][] = new float[3][3];
+ mat.getMat(mat2);
+ setMat(mat2);
+ }
+
+ public void getMat(Matrix3f mat)
+ {
+ float mat2[][] = new float[3][3];
+ getMat(mat2);
+ mat.setMat(mat2);
+ }
+
+ public void setQuat(Quaternion quat)
+ {
+ quat.getMat3(m);
+ }
+
+ public void getQuat(Quaternion quat)
+ {
+ quat.setMat3(m);
+ }
+
+ public void setEulers(float hpr[])
+ {
+ setEulers(hpr[0], hpr[1], hpr[2]);
+ }
+
+ public void getEulers(float hpr[]) // self factored - tested ok
+ {
+ float cosh, sinh, cosp, sinp, cosr, sinr;
+
+ sinp = -m[1][2];
+ cosp = (float)Math.sqrt(1.0f - sinp*sinp);
+
+ if ( (float)Math.abs(cosp) > 0.0001f )
+ {
+ cosh = m[2][2] / cosp;
+ sinh = m[0][2] / cosp;
+ cosr = m[2][1] / cosp;
+ sinr = m[1][0] / cosp;
+ }
+ else
+ {
+ cosh = 1.0f;
+ sinh = 0.0f;
+ cosr = m[0][0];
+ sinr = -m[0][1];
+ }
+
+ hpr[0] = (float)Math.atan2(sinh, cosh);
+ hpr[1] = (float)Math.atan2(sinp, cosp);
+ hpr[2] = (float)Math.atan2(sinr, cosr);
+ }
+
+ public void setEulers(float h, float p, float r) // Vince p.26 - tested ok
+ {
+ float cosh, sinh, cosp, sinp, cosr, sinr;
+
+ cosh = (float)Math.cos(h);
+ sinh = (float)Math.sin(h);
+ cosp = (float)Math.cos(p);
+ sinp = (float)Math.sin(p);
+ cosr = (float)Math.cos(r);
+ sinr = (float)Math.sin(r);
+
+ m[0][0] = + cosh * cosr + sinh * sinp * sinr;
+ m[0][1] = - cosh * sinr + sinh * sinp * cosr;
+ m[0][2] = + sinh * cosp;
+ m[1][0] = + cosp * sinr;
+ m[1][1] = + sinh * sinr + cosh * sinp * cosr;
+ m[1][2] = - sinp;
+ m[2][0] = - sinh * cosr + cosh * sinp * sinr;
+ m[2][1] = + cosp * cosr;
+ m[2][2] = + cosh * cosp;
+ }
+
+ public void getEulers(float h[], float p[], float r[])
+ {
+ float hpr[] = new float[3];
+ getEulers(hpr);
+ h[0] = hpr[0];
+ p[0] = hpr[1];
+ r[0] = hpr[2];
+ }
+
+ public void makeNull()
+ {
+ m[0][0] = 0f;
+ m[0][1] = 0f;
+ m[0][2] = 0f;
+ m[1][0] = 0f;
+ m[1][1] = 0f;
+ m[1][2] = 0f;
+ m[2][0] = 0f;
+ m[2][1] = 0f;
+ m[2][2] = 0f;
+ }
+
+ public void makeIdent()
+ {
+ m[0][0] = 1f;
+ m[0][1] = 0f;
+ m[0][2] = 0f;
+ m[1][0] = 0f;
+ m[1][1] = 1f;
+ m[1][2] = 0f;
+ m[2][0] = 0f;
+ m[2][1] = 0f;
+ m[2][2] = 1f;
+ }
+
+ public void xform(Vec3f vec) // math_utils - tested ok
+ {
+ float v[] = new float[3];
+
+ vec.get(v);
+ vec.set(0, v[0]*m[0][0] + v[1]*m[0][1] + v[2]*m[0][2]);
+ vec.set(1, v[0]*m[1][0] + v[1]*m[1][1] + v[2]*m[1][2]);
+ vec.set(2, v[0]*m[2][0] + v[1]*m[2][1] + v[2]*m[2][2]);
+ }
+
+ public void xform(float v[]) // math_utils - tested ok
+ {
+ float tmp_v[] = new float[3];
+
+ tmp_v[0] = v[0]*m[0][0] + v[1]*m[0][1] + v[2]*m[0][2];
+ tmp_v[1] = v[0]*m[1][0] + v[1]*m[1][1] + v[2]*m[1][2];
+ tmp_v[2] = v[0]*m[2][0] + v[1]*m[2][1] + v[2]*m[2][2];
+ v[0] = tmp_v[0];
+ v[1] = tmp_v[1];
+ v[2] = tmp_v[2];
+ }
+
+ }
diff --git a/src/edu/nps/moves/legacy/math/Matrix4f.java b/src/edu/nps/moves/legacy/math/Matrix4f.java
new file mode 100644
index 0000000000..4ebc565f47
--- /dev/null
+++ b/src/edu/nps/moves/legacy/math/Matrix4f.java
@@ -0,0 +1,284 @@
+/*
+Copyright (c) 1995-2021 held by the author(s). All rights reserved.
+
+Redistribution and use in source and binary forms, with or without
+modification, are permitted provided that the following conditions
+are met:
+
+ * Redistributions of source code must retain the above copyright
+ notice, this list of conditions and the following disclaimer.
+ * Redistributions in binary form must reproduce the above copyright
+ notice, this list of conditions and the following disclaimer
+ in the documentation and/or other materials provided with the
+ distribution.
+ * Neither the names of the Naval Postgraduate School (NPS)
+ Modeling Virtual Environments and Simulation (MOVES) Institute
+ (https://www.nps.edu and https://my.nps.edu/web/moves)
+ nor the names of its contributors may be used to endorse or
+ promote products derived from this software without specific
+ prior written permission.
+
+THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+"AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+POSSIBILITY OF SUCH DAMAGE.
+*/
+package edu.nps.moves.legacy.math;
+
+/** EXECUTIVE SUMMARY
+ * Module Name: Matrix4f.java
+ * Description: Definition of the Matrix4f class
+ * @author Kent A. Watsen, http://www.mbay.net/~watsen
+ */
+public class Matrix4f
+ {
+ private
+ float m[][];
+
+ public Matrix4f()
+ {
+ m = new float[4][4];
+ makeNull();
+ }
+
+ public Matrix4f(float mat[][])
+ {
+ m = new float[4][4];
+ setMat(mat);
+ }
+
+ public Matrix4f(Matrix4f mat)
+ {
+ m = new float[4][4];
+ setMat(mat);
+ }
+
+ public Matrix4f(Quaternion quat)
+ {
+ m = new float[4][4];
+ setQuat(quat);
+ }
+
+ public Matrix4f(float hpr[])
+ {
+ m = new float[4][4];
+ setEulers(hpr);
+ }
+
+ public Matrix4f(float heading, float pitch, float roll)
+ {
+ m = new float[4][4];
+ setEulers(heading, pitch, roll);
+ }
+
+ public void print()
+ {
+ System.out.println("m = " + m[0][0] + ", " + m[0][1] + ", " + m[0][2] + ", " + m[0][3]
+ + m[1][0] + ", " + m[1][1] + ", " + m[1][2] + ", " + m[1][3]
+ + m[2][0] + ", " + m[2][1] + ", " + m[2][2] + ", " + m[2][3]
+ + m[3][0] + ", " + m[3][1] + ", " + m[3][2] + ", " + m[3][3]);
+ }
+
+ public void setMatValue(int row, int col, float val)
+ {
+ if (row < 0 || row > 4 || col < 0 || col > 4)
+ return;
+ m[row][col] = val;
+ }
+
+ public float getMatValue(int row, int col)
+ {
+ if (row < 0 || row > 4 || col < 0 || col > 4)
+ return 0.0f;
+ return m[row][col];
+ }
+
+ public void setMat(float mat[][])
+ {
+ m[0][0] = mat[0][0];
+ m[0][1] = mat[0][1];
+ m[0][2] = mat[0][2];
+ m[0][3] = mat[0][3];
+ m[1][0] = mat[1][0];
+ m[1][1] = mat[1][1];
+ m[1][2] = mat[1][2];
+ m[1][3] = mat[1][3];
+ m[2][0] = mat[2][0];
+ m[2][1] = mat[2][1];
+ m[2][2] = mat[2][2];
+ m[2][3] = mat[2][3];
+ m[3][0] = mat[3][0];
+ m[3][1] = mat[3][1];
+ m[3][2] = mat[3][2];
+ m[3][3] = mat[3][3];
+ }
+
+ public void getMat(float mat[][])
+ {
+ mat[0][0] = m[0][0];
+ mat[0][1] = m[0][1];
+ mat[0][2] = m[0][2];
+ mat[0][3] = m[0][3];
+ mat[1][0] = m[1][0];
+ mat[1][1] = m[1][1];
+ mat[1][2] = m[1][2];
+ mat[1][3] = m[1][3];
+ mat[2][0] = m[2][0];
+ mat[2][1] = m[2][1];
+ mat[2][2] = m[2][2];
+ mat[2][3] = m[2][3];
+ mat[3][0] = m[3][0];
+ mat[3][1] = m[3][1];
+ mat[3][2] = m[3][2];
+ mat[3][3] = m[3][3];
+ }
+
+ public void setMat(Matrix4f mat)
+ {
+ float mat2[][] = new float[4][4];
+ mat.getMat(mat2);
+ setMat(mat2);
+ }
+
+ public void getMat(Matrix4f mat)
+ {
+ float mat2[][] = new float[4][4];
+ getMat(mat2);
+ mat.setMat(mat2);
+ }
+
+ public void setQuat(Quaternion quat)
+ {
+ quat.getMat4(m);
+ }
+
+ public void getQuat(Quaternion quat)
+ {
+ quat.setMat4(m);
+ }
+
+ public void setEulers(float hpr[])
+ {
+ setEulers(hpr[0], hpr[1], hpr[2]);
+ }
+
+ public void getEulers(float hpr[])
+ {
+ // no answer yet
+ }
+
+ public void setEulers(float h, float p, float r) // Shoemake
+ {
+ float cosh, sinh, cosp, sinp, cosr, sinr;
+
+ cosh = (float)Math.cos(h);
+ sinh = (float)Math.sin(h);
+ cosp = (float)Math.cos(p);
+ sinp = (float)Math.sin(p);
+ cosr = (float)Math.cos(r);
+ sinr = (float)Math.sin(r);
+
+ m[0][0] = cosh * cosp;
+ m[0][1] = cosh * sinp * sinr - sinh * cosr;
+ m[0][2] = cosh * sinp * cosr + sinh * sinr;
+ m[0][3] = 0.0f;
+
+ m[1][0] = sinh * cosp;
+ m[1][1] = cosh * cosr + sinh * sinp * sinr;
+ m[1][2] = sinh * sinp * cosr - cosh * sinr;
+ m[1][3] = 0.0f;
+
+ m[2][0] = -sinp;
+ m[2][1] = cosp * sinr;
+ m[2][2] = cosp * cosr;
+ m[2][3] = 0.0f;
+
+ m[3][0] = 0.0f;
+ m[3][1] = 0.0f;
+ m[3][2] = 0.0f;
+ m[3][3] = 1.0f;
+ }
+
+ public void getEulers(float h[], float p[], float r[])
+ {
+ float hpr[] = new float[3];
+ getEulers(hpr);
+ h[0] = hpr[0];
+ p[0] = hpr[1];
+ r[0] = hpr[2];
+ }
+
+ public void makeNull()
+ {
+ m[0][0] = 0f;
+ m[0][1] = 0f;
+ m[0][2] = 0f;
+ m[0][3] = 0f;
+ m[1][0] = 0f;
+ m[1][1] = 0f;
+ m[1][2] = 0f;
+ m[1][3] = 0f;
+ m[2][0] = 0f;
+ m[2][1] = 0f;
+ m[2][2] = 0f;
+ m[2][3] = 0f;
+ m[3][0] = 0f;
+ m[3][1] = 0f;
+ m[3][2] = 0f;
+ m[3][3] = 0f;
+ }
+
+ public void makeIdent()
+ {
+ m[0][0] = 1f;
+ m[0][1] = 0f;
+ m[0][2] = 0f;
+ m[0][3] = 0f;
+ m[1][0] = 0f;
+ m[1][1] = 1f;
+ m[1][2] = 0f;
+ m[1][3] = 0f;
+ m[2][0] = 0f;
+ m[2][1] = 0f;
+ m[2][2] = 1f;
+ m[2][3] = 0f;
+ m[3][0] = 0f;
+ m[3][1] = 0f;
+ m[3][2] = 0f;
+ m[3][3] = 1f;
+ }
+
+ public void xform(Vec4f vec) // math_utils
+ {
+ float v[] = new float[4];
+
+ vec.get(v);
+ vec.set(0, v[0]*m[0][0] + v[1]*m[0][1] + v[2]*m[0][2] + v[3]*m[0][3]);
+ vec.set(1, v[0]*m[1][0] + v[1]*m[1][1] + v[2]*m[1][2] + v[3]*m[1][3]);
+ vec.set(2, v[0]*m[2][0] + v[1]*m[2][1] + v[2]*m[2][2] + v[3]*m[2][3]);
+ vec.set(3, v[0]*m[3][0] + v[1]*m[3][1] + v[2]*m[3][2] + v[3]*m[3][3]);
+ }
+
+ public void xform(float v[]) // math_utils
+ {
+ float tmp_v[] = new float[4];
+
+ tmp_v[0] = v[0]*m[0][0] + v[1]*m[0][1] + v[2]*m[0][2] + v[3]*m[0][3];
+ tmp_v[1] = v[0]*m[1][0] + v[1]*m[1][1] + v[2]*m[1][2] + v[3]*m[1][3];
+ tmp_v[2] = v[0]*m[2][0] + v[1]*m[2][1] + v[2]*m[2][2] + v[3]*m[2][3];
+ tmp_v[2] = v[0]*m[3][0] + v[1]*m[3][1] + v[2]*m[3][2] + v[3]*m[3][3];
+ v[0] = tmp_v[0];
+ v[1] = tmp_v[1];
+ v[2] = tmp_v[2];
+ v[3] = tmp_v[3];
+ }
+
+ }
diff --git a/src/edu/nps/moves/legacy/math/Quaternion.java b/src/edu/nps/moves/legacy/math/Quaternion.java
new file mode 100644
index 0000000000..09d825cb37
--- /dev/null
+++ b/src/edu/nps/moves/legacy/math/Quaternion.java
@@ -0,0 +1,775 @@
+/*
+Copyright (c) 1995-2009 held by the author(s). All rights reserved.
+
+Redistribution and use in source and binary forms, with or without
+modification, are permitted provided that the following conditions
+are met:
+
+ * Redistributions of source code must retain the above copyright
+ notice, this list of conditions and the following disclaimer.
+ * Redistributions in binary form must reproduce the above copyright
+ notice, this list of conditions and the following disclaimer
+ in the documentation and/or other materials provided with the
+ distribution.
+ * Neither the names of the Naval Postgraduate School (NPS)
+ Modeling Virtual Environments and Simulation (MOVES) Institute
+ (http://www.nps.edu and http://www.movesinstitute.org)
+ nor the names of its contributors may be used to endorse or
+ promote products derived from this software without specific
+ prior written permission.
+
+THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+"AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+POSSIBILITY OF SUCH DAMAGE.
+*/
+package edu.nps.moves.legacy.math;
+
+/**
+ * EXECUTIVE SUMMARY
+ * Module Name: Quaternion.java
+ * Description: Definition of the Quaternion class
+ * @author Kent A. Watsen, http://www.mbay.net/~watsen
+ */
+public class Quaternion {
+
+ private float q[];
+
+ public Quaternion() {
+ q = new float[4];
+ makeIdent();
+ }
+
+ public Quaternion(float axis[], float angle) // radians
+ {
+ q = new float[4];
+ setAxisAngle(axis, angle);
+ }
+
+ public Quaternion(Vec3f axis, float angle) // radians
+ {
+ q = new float[4];
+ setAxisAngle(axis, angle);
+ }
+
+ public Quaternion(Matrix3f mat) {
+ q = new float[4];
+ setMat3(mat);
+ }
+
+ public Quaternion(Matrix4f mat) {
+ q = new float[4];
+ setMat4(mat);
+ }
+
+ public Quaternion(Quaternion quat) {
+ q = new float[4];
+ setQuat(quat);
+ }
+
+ public Quaternion(float vec1[], float vec2[]) {
+ q = new float[4];
+ makeFromVecs(vec1, vec2);
+ }
+
+ public Quaternion(Vec3f vec1, Vec3f vec2) {
+ q = new float[4];
+ makeFromVecs(vec1, vec2);
+ }
+
+ public void print() {
+ System.out.println("q = " + q[0] + ", " + q[1] + ", " + q[2] + ", " + q[3]);
+ }
+
+ public void setVec(float i, float j, float k) {
+ q[0] = i;
+ q[1] = j;
+ q[2] = k;
+ q[3] = 0.0f;
+ }
+
+ public void getVec(float i[], float j[], float k[]) {
+ i[0] = q[0];
+ j[0] = q[1];
+ k[0] = q[2];
+ }
+
+ public void setVec(float vec[]) {
+ q[0] = vec[0];
+ q[1] = vec[1];
+ q[2] = vec[2];
+ q[3] = 0.0f;
+ }
+
+ public void getVec(float vec[]) {
+ vec[0] = q[0];
+ vec[1] = q[1];
+ vec[2] = q[2];
+ }
+
+ public void setVec(Vec3f vec) {
+ q[0] = vec.get(0);
+ q[1] = vec.get(1);
+ q[2] = vec.get(2);
+ q[3] = 0.0f;
+ }
+
+ public void getVec(Vec3f vec) {
+ vec.set(q[0], q[1], q[2]);
+ }
+
+ public void setAxisAngle(float axis_angle[]) // radians - tested ok
+ {
+ float sin;
+ float length_sqr;
+ float one_over_length;
+
+ if (axis_angle[3] == 0.0f) {
+ makeIdent();
+ }
+
+ length_sqr = axis_angle[0] * axis_angle[0] + axis_angle[1] * axis_angle[1] + axis_angle[2] * axis_angle[2];
+ if (length_sqr < 0.0001) {
+ makeIdent();
+ }
+
+ one_over_length = 1.0f / (float) Math.sqrt(length_sqr);
+ axis_angle[0] = axis_angle[0] * one_over_length;
+ axis_angle[1] = axis_angle[1] * one_over_length;
+ axis_angle[2] = axis_angle[2] * one_over_length;
+
+ sin = (float) Math.sin(axis_angle[3] * 0.5f);
+ q[0] = axis_angle[0] * sin;
+ q[1] = axis_angle[1] * sin;
+ q[2] = axis_angle[2] * sin;
+ q[3] = (float) Math.cos(axis_angle[3] * 0.5f);
+ }
+
+ public void getAxisAngle(float axis_angle[]) // radians - tested ok
+ {
+ float one_over_sin;
+
+ axis_angle[3] = 2.0f * (float) Math.acos(q[3]);
+ if (Math.abs(axis_angle[3]) < 0.0001f) {
+ axis_angle[0] = 0.0f; // Doesn't this lose
+ axis_angle[1] = 1.0f; // axis info?
+ axis_angle[2] = 0.0f;
+ } else {
+ one_over_sin = 1.0f / (float) Math.sin(axis_angle[3] * 0.5f);
+ axis_angle[0] = q[0] * one_over_sin;
+ axis_angle[1] = q[1] * one_over_sin;
+ axis_angle[2] = q[2] * one_over_sin;
+ }
+ }
+
+ public void setAxisAngle(Vec4f axis_angle) // radians
+ {
+ float aa[] = new float[4];
+ axis_angle.get(aa);
+ setAxisAngle(aa);
+ }
+
+ public void getAxisAngle(Vec4f axis_angle) // radians
+ {
+ float aa[] = new float[4];
+ getAxisAngle(aa);
+ axis_angle.set(aa);
+ }
+
+ public void setAxisAngle(float axis[], float angle) // radians
+ {
+ float aa[] = new float[4];
+ aa[0] = axis[0];
+ aa[1] = axis[1];
+ aa[2] = axis[2];
+ aa[3] = angle;
+ setAxisAngle(aa);
+ }
+
+ public void getAxisAngle(float axis[], float angle[]) // radians
+ {
+ float aa[] = new float[4];
+ getAxisAngle(aa);
+ axis[0] = aa[0];
+ axis[1] = aa[1];
+ axis[2] = aa[2];
+ angle[0] = aa[3];
+ }
+
+ public void setAxisAngle(Vec3f axis, float angle) // radians
+ {
+ float aa[] = new float[4];
+ aa[0] = axis.get(0);
+ aa[1] = axis.get(1);
+ aa[2] = axis.get(2);
+ aa[3] = angle;
+ setAxisAngle(aa);
+ }
+
+ public void getAxisAngle(Vec3f axis, float angle[]) // radians
+ {
+ float aa[] = new float[4];
+ getAxisAngle(aa);
+ axis.set(aa[0], aa[1], aa[2]);
+ angle[0] = aa[3];
+ }
+
+ public void setAxisAngle(float i, float j, float k, float angle) // radians
+ {
+ float aa[] = new float[4];
+ aa[0] = i;
+ aa[1] = j;
+ aa[2] = k;
+ aa[3] = angle;
+ setAxisAngle(aa);
+ }
+
+ public void getAxisAngle(float i[], float j[], float k[], float angle[]) // radians
+ {
+ float aa[] = new float[4];
+ getAxisAngle(aa);
+ i[0] = aa[0];
+ j[0] = aa[1];
+ k[0] = aa[2];
+ angle[0] = aa[3];
+ }
+
+ public void setEulers(float hpr[]) // radians - tested ok
+ {
+ float sin_h = (float) Math.sin(hpr[0] * 0.5f);
+ float cos_h = (float) Math.cos(hpr[0] * 0.5f);
+ float sin_p = (float) Math.sin(hpr[1] * 0.5f);
+ float cos_p = (float) Math.cos(hpr[1] * 0.5f);
+ float sin_r = (float) Math.sin(hpr[2] * 0.5f);
+ float cos_r = (float) Math.cos(hpr[2] * 0.5f);
+
+ q[0] = cos_h * sin_p * cos_r + sin_h * cos_p * sin_r;
+ q[1] = sin_h * cos_p * cos_r - cos_h * sin_p * sin_r;
+ q[2] = cos_h * cos_p * sin_r - sin_h * sin_p * cos_r;
+ q[3] = cos_h * cos_p * cos_r + sin_h * sin_p * sin_r;
+ }
+
+ public void getEulers(float hpr[]) // radians - tested ok
+ {
+ Matrix3f m = new Matrix3f();
+
+ getMat3(m);
+ m.getEulers(hpr);
+ }
+
+ public void setEulers(float h, float p, float r) // radians; Vince, pg 201
+ {
+ float hpr[] = new float[3];
+ hpr[0] = h;
+ hpr[1] = p;
+ hpr[2] = r;
+ setEulers(hpr);
+ }
+
+ public void getEulers(float h[], float p[], float r[]) {
+ float hpr[] = new float[3];
+ getEulers(hpr);
+ h[0] = hpr[0];
+ p[0] = hpr[1];
+ r[0] = hpr[2];
+ }
+
+ public void setMat3(float mat[][]) // Chrenshaw, pg 16 (he transposes m!)
+ {
+ float tr, s;
+
+ tr = mat[0][0] + mat[1][1] + mat[2][2] + 1;
+ if (tr > 0.0625f) {
+ s = (float) Math.sqrt(tr);
+ q[3] = s * 0.5f;
+ s = 0.5f / s;
+
+ q[0] = (mat[1][2] - mat[2][1]) * s;
+ q[1] = (mat[2][0] - mat[0][2]) * s;
+ q[2] = (mat[0][1] - mat[1][0]) * s;
+ return;
+ }
+
+ tr = -mat[0][0] - mat[1][1] + mat[2][2] + 1;
+ if (tr > 0.0625f) {
+ s = (float) Math.sqrt(tr);
+ q[2] = s * 0.5f;
+ s = 0.5f / s;
+
+ q[0] = (mat[2][0] - mat[0][2]) * s;
+ q[1] = (mat[2][1] + mat[1][2]) * s;
+ q[3] = (mat[0][1] - mat[1][0]) * s;
+ return;
+ }
+
+ tr = -mat[0][0] + mat[1][1] - mat[2][2] + 1.0f;
+ if (tr > 0.0625f) {
+ s = (float) Math.sqrt(tr);
+ q[1] = s * 0.5f;
+ s = 0.5f / s;
+
+ q[0] = (mat[1][0] + mat[0][1]) * s;
+ q[2] = (mat[2][1] + mat[1][2]) * s;
+ q[3] = (mat[2][0] - mat[0][2]) * s;
+ return;
+ }
+
+ tr = mat[0][0] - mat[1][1] - mat[2][2] + 1.0f;
+ if (tr > 0.0625f) {
+ s = (float) Math.sqrt(tr);
+ q[0] = s * 0.5f;
+ s = 0.5f / s;
+
+ q[1] = (mat[1][0] + mat[0][1]) * s;
+ q[2] = (mat[2][0] - mat[0][2]) * s;
+ q[3] = (mat[1][2] - mat[2][1]) * s;
+ return;
+ }
+ }
+
+ public void getMat3(float mat[][]) // Vince, pg 202; WATT, pg 362 - tested ok
+ {
+ float two_over_length_sqr;
+ float xs, ys, zs;
+ float wx, wy, wz;
+ float xx, xy, xz;
+ float yy, yz, zz;
+
+ two_over_length_sqr = 2.0f / length_sqr();
+ xs = q[0] * two_over_length_sqr;
+ ys = q[1] * two_over_length_sqr;
+ zs = q[2] * two_over_length_sqr;
+
+ wx = q[3] * xs;
+ wy = q[3] * ys;
+ wz = q[3] * zs;
+ xx = q[0] * xs;
+ xy = q[0] * ys;
+ xz = q[0] * zs;
+ yy = q[1] * ys;
+ yz = q[1] * zs;
+ zz = q[2] * zs;
+
+ mat[0][0] = 1.0f - yy - zz;
+ mat[0][1] = xy - wz;
+ mat[0][2] = xz + wy;
+ mat[1][0] = xy + wz;
+ mat[1][1] = 1.0f - xx - zz;
+ mat[1][2] = yz - wx;
+ mat[2][0] = xz - wy;
+ mat[2][1] = yz + wx;
+ mat[2][2] = 1.0f - xx - yy;
+ }
+
+ public void setMat3(Matrix3f mat) {
+ float m[][] = new float[3][3];
+ mat.getMat(m);
+ setMat3(m);
+ }
+
+ public void getMat3(Matrix3f mat) {
+ float m[][] = new float[3][3];
+ getMat3(m);
+ mat.setMat(m);
+ }
+
+ public void setMat4(float mat[][]) {
+ setMat3(mat); // It doesn't know the difference
+ }
+
+ public void getMat4(float mat[][]) {
+ getMat3(mat); // It doesn't know the difference
+ mat[0][3] = 0.0f;
+ mat[1][3] = 0.0f;
+ mat[2][3] = 0.0f;
+ mat[3][0] = 0.0f;
+ mat[3][1] = 0.0f;
+ mat[3][2] = 0.0f;
+ mat[3][3] = 1.0f;
+ }
+
+ public void setMat4(Matrix4f mat) {
+ float m[][] = new float[4][4];
+ mat.getMat(m);
+ setMat4(m);
+ }
+
+ public void getMat4(Matrix4f mat) {
+ float m[][] = new float[4][4];
+ getMat4(m);
+ mat.setMat(m);
+ }
+
+ public void setQuat(float quat[]) {
+ q[0] = quat[0];
+ q[1] = quat[1];
+ q[2] = quat[2];
+ q[3] = quat[3];
+ }
+
+ public void getQuat(float quat[]) {
+ quat[0] = q[0];
+ quat[1] = q[1];
+ quat[2] = q[2];
+ quat[3] = q[3];
+ }
+
+ public void setQuat(Quaternion quat) {
+ float quat2[] = new float[4];
+ quat.getQuat(quat2);
+ setQuat(quat2);
+ }
+
+ public void getQuat(Quaternion quat) {
+ float quat2[] = new float[4];
+ getQuat(quat2);
+ quat.setQuat(quat2);
+ }
+
+ public void setQuat(float i, float j, float k, float w) {
+ q[0] = i;
+ q[1] = j;
+ q[2] = k;
+ q[3] = w;
+ }
+
+ public void getQuat(float i[], float j[], float k[], float w[]) {
+ i[0] = q[0];
+ j[0] = q[1];
+ k[0] = q[2];
+ w[0] = q[3];
+ }
+
+ public void setQuatValue(int index, float value) {
+ if (index < 0 || index > 3) {
+ return;
+ }
+ q[index] = value;
+ }
+
+ public float getQuatValue(int index) {
+ if (index < 0 || index > 3) {
+ return 0.0f;
+ }
+ return q[index];
+ }
+
+ public void makeIdent() {
+ q[0] = 0.0f;
+ q[1] = 0.0f;
+ q[2] = 0.0f;
+ q[3] = 1.0f;
+ }
+
+ public float length() {
+ return (float) Math.sqrt(q[0] * q[0] + q[1] * q[1] + q[2] * q[2] + q[3] * q[3]);
+ }
+
+ public float length_sqr() {
+ return q[0] * q[0] + q[1] * q[1] + q[2] * q[2] + q[3] * q[3];
+ }
+
+ public void normalize() // Crenshaw, pg 20
+ {
+ float one_over_length = 1.0f / length();
+ q[0] = q[0] * one_over_length;
+ q[1] = q[1] * one_over_length;
+ q[2] = q[2] * one_over_length;
+ q[3] = q[3] * one_over_length;
+ }
+
+ public void normalize(Quaternion quat) // Crenshaw, pg 20
+ {
+ float one_over_length = 1.0f / quat.length();
+ q[0] = quat.getQuatValue(0) * one_over_length;
+ q[1] = quat.getQuatValue(1) * one_over_length;
+ q[2] = quat.getQuatValue(2) * one_over_length;
+ q[3] = quat.getQuatValue(3) * one_over_length;
+ }
+
+ public void conjugate() // Gem1, pg 501
+ {
+ q[0] = -q[0];
+ q[1] = -q[1];
+ q[2] = -q[2];
+ q[3] = q[3];
+ }
+
+ public void conjugate(Quaternion quat) // Gem1, pg 501
+ {
+ q[0] = -quat.getQuatValue(0);
+ q[1] = -quat.getQuatValue(1);
+ q[2] = -quat.getQuatValue(2);
+ q[3] = quat.getQuatValue(3);
+ }
+
+ public void invert() // Vince 197; Gem1 pg 501 - tested ok
+ {
+ float one_over_length_sqr;
+ one_over_length_sqr = 1.0f / length_sqr();
+ q[0] = -q[0] * one_over_length_sqr;
+ q[1] = -q[1] * one_over_length_sqr;
+ q[2] = -q[2] * one_over_length_sqr;
+ q[3] = q[3] * one_over_length_sqr;
+ }
+
+ public void invert(Quaternion quat) // Vince 197; Gem1 pg 501 - tested ok
+ {
+ float one_over_length_sqr;
+ one_over_length_sqr = 1.0f / quat.length_sqr();
+ q[0] = -quat.getQuatValue(0) * one_over_length_sqr;
+ q[1] = -quat.getQuatValue(1) * one_over_length_sqr;
+ q[2] = -quat.getQuatValue(2) * one_over_length_sqr;
+ q[3] = quat.getQuatValue(3) * one_over_length_sqr;
+ }
+
+ public void add(Quaternion quat) // Vince 197
+ {
+ q[0] = q[0] + quat.getQuatValue(0);
+ q[1] = q[1] + quat.getQuatValue(1);
+ q[2] = q[2] + quat.getQuatValue(2);
+ q[3] = q[3] + quat.getQuatValue(3);
+ }
+
+ public void add(Quaternion quat1, Quaternion quat2) // Vince 197
+ {
+ q[0] = quat1.getQuatValue(0) + quat2.getQuatValue(0);
+ q[1] = quat1.getQuatValue(1) + quat2.getQuatValue(1);
+ q[2] = quat1.getQuatValue(2) + quat2.getQuatValue(2);
+ q[3] = quat1.getQuatValue(3) + quat2.getQuatValue(3);
+ }
+
+ public void sub(Quaternion quat) // Vince 197
+ {
+ q[0] = q[0] - quat.getQuatValue(0);
+ q[1] = q[1] - quat.getQuatValue(1);
+ q[2] = q[2] - quat.getQuatValue(2);
+ q[3] = q[3] - quat.getQuatValue(3);
+ }
+
+ public void sub(Quaternion quat1, Quaternion quat2) // Vince 197
+ {
+ q[0] = quat1.getQuatValue(0) - quat2.getQuatValue(0);
+ q[1] = quat1.getQuatValue(1) - quat2.getQuatValue(1);
+ q[2] = quat1.getQuatValue(2) - quat2.getQuatValue(2);
+ q[3] = quat1.getQuatValue(3) - quat2.getQuatValue(3);
+ }
+
+ public void preMult(Quaternion quat1) // NPS wrong?; Crenshaw pg 20
+ {
+ float tmp_q[] = new float[4];
+ float q1[] = new float[4];
+
+ quat1.getQuat(q1);
+ tmp_q[0] = q1[3] * q[0] - q1[2] * q[1] + q1[1] * q[2] + q1[0] * q[3];
+ tmp_q[1] = q1[2] * q[0] + q1[3] * q[1] - q1[0] * q[2] + q1[1] * q[3];
+ tmp_q[2] = -q1[1] * q[0] + q1[0] * q[1] + q1[3] * q[2] + q1[2] * q[3];
+ tmp_q[3] = -q1[0] * q[0] - q1[1] * q[1] - q1[2] * q[2] + q1[3] * q[3];
+ setQuat(tmp_q);
+ }
+
+ public void postMult(Quaternion quat2) // NPS wrong?; Crenshaw pg 20
+ {
+ float tmp_q[] = new float[4];
+ float q2[] = new float[4];
+
+ quat2.getQuat(q2);
+ tmp_q[0] = q[3] * q2[0] - q[2] * q2[1] + q[1] * q2[2] + q[0] * q2[3];
+ tmp_q[1] = q[2] * q2[0] + q[3] * q2[1] - q[0] * q2[2] + q[1] * q2[3];
+ tmp_q[2] = -q[1] * q2[0] + q[0] * q2[1] + q[3] * q2[2] + q[2] * q2[3];
+ tmp_q[3] = -q[0] * q2[0] - q[1] * q2[1] - q[2] * q2[2] + q[3] * q2[3];
+ setQuat(tmp_q);
+ }
+
+ public void mult(Quaternion quat1, Quaternion quat2) // NPS wrong?; Crenshaw pg 20 - tested ok
+ {
+ float q1[] = new float[4];
+ float q2[] = new float[4];
+
+ quat1.getQuat(q1);
+ quat2.getQuat(q2);
+ q[0] = q1[3] * q2[0] - q1[2] * q2[1] + q1[1] * q2[2] + q1[0] * q2[3];
+ q[1] = q1[2] * q2[0] + q1[3] * q2[1] - q1[0] * q2[2] + q1[1] * q2[3];
+ q[2] = -q1[1] * q2[0] + q1[0] * q2[1] + q1[3] * q2[2] + q1[2] * q2[3];
+ q[3] = -q1[0] * q2[0] - q1[1] * q2[1] - q1[2] * q2[2] + q1[3] * q2[3];
+ }
+
+ public void makeFromVecs(float i1, float j1, float k1, float i2, float j2, float k2) {
+ Vec3f tmp1 = new Vec3f(i1, j1, k1);
+ Vec3f tmp2 = new Vec3f(i2, j2, k2);
+ makeFromVecs(tmp1, tmp2);
+ }
+
+ public void makeFromVecs(float vec1[], float vec2[]) {
+ Vec3f tmp1 = new Vec3f(vec1);
+ Vec3f tmp2 = new Vec3f(vec2);
+ makeFromVecs(tmp1, tmp2);
+ }
+
+ public void makeFromVecs(Vec3f vec1, Vec3f vec2) // modified Ken Shoemake - tested ok
+ {
+ Vec3f tmp1 = new Vec3f();
+ Vec3f tmp2 = new Vec3f();
+ Vec3f axis = new Vec3f();
+ float dot;
+ float angle;
+ float abs_angle;
+
+ tmp1.normalize(vec1);
+ tmp2.normalize(vec2);
+
+ dot = Vec3f.dot(tmp1, tmp2);
+ angle = (float) Math.acos(dot); // -PI < angle < +PI
+ abs_angle = Math.abs(angle); // 0 <= angle < PI
+
+ if (abs_angle < 0.0001f) // vec's parallel
+ {
+ makeIdent();
+ return;
+ }
+
+ if (abs_angle > ((float) Math.PI) - 0.0001f) // vec's oppose
+ {
+ // Can't use cross product for axis. Find arbitrary axis.
+ // First try cross with X-axis, else just use Z-axis.
+ axis.set(1.0f, 0.0f, 0.0f);
+ if (axis.dot(tmp1) < 0.1f) {
+ axis.cross(tmp1);
+ axis.normalize();
+ } else {
+ axis.set(0.0f, 0.0f, 1.0f);
+ }
+ setAxisAngle(axis, angle);
+ return;
+ }
+
+ // normal case
+ axis.cross(tmp1, tmp2);
+ axis.normalize();
+ setAxisAngle(axis, angle);
+ }
+
+ public void xform(Vec3f vec) // - tested ok
+ {
+ Quaternion vecQuat = new Quaternion();
+ Quaternion tmpQuat = new Quaternion();
+
+ vecQuat.setVec(vec);
+ tmpQuat.invert(this);
+
+ tmpQuat.mult(vecQuat, tmpQuat);
+ tmpQuat.mult(this, tmpQuat);
+
+ tmpQuat.getVec(vec);
+ }
+
+ public void xform(float v[]) // - tested ok
+ {
+ Quaternion vecQuat = new Quaternion();
+ Quaternion tmpQuat = new Quaternion();
+
+ vecQuat.setVec(v);
+ tmpQuat.invert(this);
+
+ tmpQuat.mult(vecQuat, tmpQuat);
+ tmpQuat.mult(this, tmpQuat);
+
+ tmpQuat.getVec(v);
+ }
+
+ public void slerp(Quaternion quat1, Quaternion quat2, float alpha, int spin) // - tested ok
+ { // 0<=alpha<=1, spin = {..., -1, 0, 1, ...}
+ float q1[] = new float[4];
+ float q2[] = new float[4];
+ float beta; // complementary interp parameter
+ float theta; // angle between A and B
+ float sin_t, cos_t; // sine, cosine of theta
+ float phi; // theta plus spins
+ int bflip; // use negation of B?
+
+ quat1.getQuat(q1);
+ quat2.getQuat(q2);
+
+
+ // cosine theta = dot product of A and B
+ cos_t = q1[0] * q2[0] + q1[1] * q2[1] + q1[2] * q2[2] + q1[3] * q2[3];
+
+ // if B is on opposite hemisphere from A, use -B instead
+ if (cos_t < 0.0f) {
+ cos_t = -cos_t;
+ bflip = 1; // true
+ } else {
+ bflip = 0; // false
+ }
+
+ // if B is (within precision limits) the same as A,
+ // just linear interpolate between A and B.
+ // Can't do spins, since we don't know what direction to spin.
+
+ if (1.0f - cos_t < 0.0001f) {
+ beta = 1.0f - alpha;
+ } else // normal case
+ {
+ theta = (float) Math.acos(cos_t);
+ phi = theta + spin * (float) Math.PI;
+ sin_t = (float) Math.sin(theta);
+ beta = (float) Math.sin(theta - alpha * phi) / sin_t;
+ alpha = (float) Math.sin(alpha * phi) / sin_t;
+ }
+
+ if (bflip == 1) {
+ alpha = -alpha;
+ }
+
+ // interpolate
+ q[0] = beta * q1[0] + alpha * q2[0];
+ q[1] = beta * q1[1] + alpha * q2[1];
+ q[2] = beta * q1[2] + alpha * q2[2];
+ q[3] = beta * q1[3] + alpha * q2[3];
+ }
+}
+/* Alternative...may have less accuracy.
+public void setMat(float mat[][]) // Watt, pg 363; Crenshaw, pg 15
+{
+float tr, s;
+int i, j, k;
+
+tr = m[0][0] + m[1][1] + m[2][2];
+if (tr > 0.0f)
+{
+s = (float)Math.sqrt(tr + 1.0f);
+r[3] = s*0.5f;
+s = 0.5f/s;
+
+r[0] = (m[1][2] - m[2][1])*s;
+r[1] = (m[2][0] - m[0][2])*s;
+r[2] = (m[0][1] - m[1][0])*s;
+}
+else
+{
+i = 0;
+if (m[1][1] > m[i][i])
+i = 1;
+if (m[2][2] > m[i][i])
+i = 2;
+j = (i+1)%3;
+k = (j+1)%3;
+
+s = (float)Math.sqrt( (m[i][i] - (m[j][j]+m[k][k])) + 1.0f);
+
+r[i] = s*0.5f;
+s = 0.5f/s;
+r[3] = (m[j][k] - m[k][j])*s;
+r[j] = (m[i][j] + m[j][i])*s;
+r[k] = (m[i][k] + m[k][i])*s;
+}
+}
+ */
\ No newline at end of file
diff --git a/src/edu/nps/moves/legacy/math/Quaternion2.java b/src/edu/nps/moves/legacy/math/Quaternion2.java
new file mode 100644
index 0000000000..bdb45195f0
--- /dev/null
+++ b/src/edu/nps/moves/legacy/math/Quaternion2.java
@@ -0,0 +1,422 @@
+/*Copyright (c) 1995-2021 held by the author(s). All rights reserved.
+
+Redistribution and use in source and binary forms, with or without
+modification, are permitted provided that the following conditions
+are met:
+
+ * Redistributions of source code must retain the above copyright
+ notice, this list of conditions and the following disclaimer.
+ * Redistributions in binary form must reproduce the above copyright
+ notice, this list of conditions and the following disclaimer
+ in the documentation and/or other materials provided with the
+ distribution.
+ * Neither the names of the Naval Postgraduate School (NPS)
+ Modeling Virtual Environments and Simulation (MOVES) Institute
+ (https://www.nps.edu and https://my.nps.edu/web/moves)
+ nor the names of its contributors may be used to endorse or
+ promote products derived from this software without specific
+ prior written permission.
+
+THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+"AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+POSSIBILITY OF SUCH DAMAGE.
+*/
+package edu.nps.moves.legacy.math;
+
+/**
+ * The Quaternion2 class executes quaternion operations.
+ *
+ * @author Ildeniz Duman
+ * @version 1.0 13 Dec 98
+ *
+ * DIS VRML - OpenGl
+ * +---------->x ^y
+ * /| |
+ * / | |
+ * / | ======> |
+ * y | +-------------->x
+ * z /
+ * /
+ * /
+ * z
+ */
+public class Quaternion2 {
+
+ /**
+ * Elements of a quaternion.
+ */
+ private double w,x,y,z;
+
+ /**
+ * Name of the quaternion.
+ */
+ private String name;
+
+/**
+* Default constructor
+* @param myName Name of the quaternion
+* @param ww w value of the quaternion
+* @param xx x value of the quaternion
+* @param yy y value of the quaternion
+* @param zz z value of the quaternion
+*/
+public Quaternion2 (String myName, double ww, double xx,double yy, double zz){
+ name = new String (myName);
+ w=ww;
+ x=xx;
+ y=yy;
+ z=zz;
+}
+
+
+/**
+* Default constructor
+* @param myName Name of the quaternion
+*/
+public Quaternion2 (String myName){
+ name = new String (myName);
+ w=1.0f;
+ x=0.0f;
+ y=0.0f;
+ z=0.0f;
+}
+
+
+/**
+* Default constructor
+*/
+public Quaternion2 (){
+ name = new String ("Quaternion2");
+ w=1.0f;
+ x=0.0f;
+ y=0.0f;
+ z=0.0f;
+}
+
+
+/**
+* Sets the quaternion values
+* @param ww w value of the quaternion
+* @param xx x value of the quaternion
+* @param yy y value of the quaternion
+* @param zz z value of the quaternion
+*/
+public void setQuaternion2 (double ww, double xx, double yy, double zz)
+{
+ w=ww;
+ x=xx;
+ y=yy;
+ z=zz;
+ return;
+}// end setQuaternion2()
+
+/**
+* Returns the w value of quaternion.
+*/
+public double getW(){return w;}
+/**
+* Returns the x value of quaternion.
+*/
+public double getX(){return x;}
+/**
+* Returns the y value of quaternion.
+*/
+public double getY(){return y;}
+/**
+* Returns the z value of quaternion.
+*/
+public double getZ(){return z;}
+
+
+/**
+* Multiplies two quaternions
+* @param QUAT second quaternion
+* @return new Quaternion2
+*/
+public Quaternion2 multiply(final Quaternion2 QUAT)
+{
+ Quaternion2 dest = new Quaternion2 ("Quaternion2 Product");
+ dest.w = QUAT.w * w - QUAT.x * x - QUAT.y * y - QUAT.z * z;
+ dest.x = QUAT.w * x + QUAT.x * w - QUAT.y * z + QUAT.z * y;
+ dest.y = QUAT.w * y + QUAT.y * w - QUAT.z * x + QUAT.x * z;
+ dest.z = QUAT.w * z + QUAT.z * w - QUAT.x * y + QUAT.y * x;
+
+ return (dest);
+}//end multiply()
+
+
+/**
+* Multiplies this quaternion with a scalar
+* @param NUM is a scalar
+* @return modified Quaternion2
+*/
+public Quaternion2 multiply(final double NUM)
+{
+ w=NUM*w;
+ x=NUM*x;
+ y=NUM*y;
+ z=NUM*z;
+ return (this);
+}//end multiply()
+
+
+/**
+* Adds two quaternions
+* @param QUAT is a quaternion to add
+* @return new Quaternion2
+*/
+public Quaternion2 add(final Quaternion2 QUAT)
+{
+ Quaternion2 addTo = new Quaternion2 ("addition");
+ addTo.w = w + QUAT.w;
+ addTo.x = x + QUAT.x;
+ addTo.y = y + QUAT.y;
+ addTo.z = z + QUAT.z;
+
+ return (addTo);
+}// end add()
+
+
+/**
+* Substracts two quaternions
+* @param QUAT is a quaternion to substract
+* @return new Quaternion2
+*/
+public Quaternion2 substract(final Quaternion2 QUAT)
+{
+ Quaternion2 s = new Quaternion2 ("substraction");
+ s.w = w - QUAT.w;
+ s.x = x - QUAT.x;
+ s.y = y - QUAT.y;
+ s.z = z - QUAT.z;
+
+ return (s);
+}// end substract()
+
+
+/**
+* Finds the inverse (conjugate) of a quaternion
+* @return new Quaternion2
+*/
+public Quaternion2 invert()
+{
+ Quaternion2 temp = new Quaternion2 ("Conjugate");
+ temp.w = w;
+ temp.x = -x;
+ temp.y = -y;
+ temp.z = -z;
+ return (temp);
+}//end invert()
+
+
+/**
+* Rotates a vector by quaternions
+* @param QUAT4 a vector in quaternion form
+* @return Rotated vector in quaternion form
+*/
+public Quaternion2 rotate(final Quaternion2 QUAT4)
+{
+ Quaternion2 temp = new Quaternion2 ("Rotation");
+ temp = this.multiply(QUAT4.multiply(this.invert()));
+ return (temp);
+}//end rotate()
+
+
+/** Converts to body coordinates
+* @param QUAT a vector in quaternion form to convert to body coordinates
+* @return Converted vector in quaternion form
+*/
+public Quaternion2 toBody(final Quaternion2 QUAT)
+{
+ Quaternion2 temp = new Quaternion2 ("to Body");
+ temp = (this.invert()).multiply(QUAT.multiply(this));
+ return (temp);
+}//end toBody()
+
+
+/** Finds the dot product of two quaternions
+* @param QUAT5 a quaternion
+* @return Dot product value
+*/
+public double dotProduct(final Quaternion2 QUAT5)
+{
+ double dotproduct;
+ dotproduct = (w * QUAT5.w) + (x * QUAT5.x) + (y * QUAT5.y) + (z * QUAT5.z);
+ return (dotproduct);
+} //end dotProduct()
+
+
+/** Normalizes the quaternion
+* modifies the current quaternion
+*/
+public void normalize()
+{
+ double sq = Math.sqrt(dotProduct(this));
+ x /= sq;
+ y /= sq;
+ z /= sq;
+ w /= sq;
+ return;
+}//end normalize()
+
+
+/** Calculates the axis angles of quaternion for drawing,
+* results must be used in glRotatef() by using get() functions
+* @return Result in quaternion form
+*/
+public Quaternion2 toAxisAngles()
+{
+ Quaternion2 axisAngle = new Quaternion2 ("To Axis Angle ");
+
+ double scalarNumber,temp;
+ temp = Math.acos(w) * 2.0;
+ scalarNumber = Math.sin(temp / 2.0);
+ axisAngle.x = x / scalarNumber;
+ // aligning the axes
+ axisAngle.y = -1 * z / scalarNumber;
+ axisAngle.z = y / scalarNumber;
+ axisAngle.w = (temp * (360 / 2.0)) / Math.PI;
+
+ return (axisAngle);
+}//end toAxisAngles()
+
+
+/** Calculates the quaternion value of three rotations
+* Current object is a 3D vector with rotation angles in quaternion form
+@return new Quaternion2
+*/
+public Quaternion2 toQuaternion2()
+{
+ //current object is a 3D vector with rotation angles at x,y,z
+ double radX,radY,radZ,tempx,tempy,tempz;
+ double cosx,cosy,cosz,sinx,siny,sinz,cosc,coss,sinc,sins;
+
+ Quaternion2 quat = new Quaternion2 ("Euler To Quaternion2");
+
+ // convert angles to radians
+ radX = (x * Math.PI) / (360.0 / 2.0);
+ radY = (y * Math.PI) / (360.0 / 2.0);
+ radZ = (z * Math.PI) / (360.0 / 2.0);
+ // half angles
+ tempx = radX * 0.5;
+ tempy = radY * 0.5;
+ tempz = radZ * 0.5;
+ cosx = Math.cos(tempx);
+ cosy = Math.cos(tempy);
+ cosz = Math.cos(tempz);
+ sinx = Math.sin(tempx);
+ siny = Math.sin(tempy);
+ sinz = Math.sin(tempz);
+
+ cosc = cosx * cosz;
+ coss = cosx * sinz;
+ sinc = sinx * cosz;
+ sins = sinx * sinz;
+
+ quat.x = (cosy * sinc) - (siny * coss);
+ quat.y = (cosy * sins) + (siny * cosc);
+ quat.z = (cosy * coss) - (siny * sinc);
+ quat.w = (cosy * cosc) + (siny * sins);
+
+ quat.normalize();
+ return(quat);
+}// end toQuaternion2
+
+
+/** Converts a quaternion into Euler angles
+* Warning : This conversion is inherently ill-defined
+* @return new Quaternion2
+*/
+public Quaternion2 toEulerAngles()
+{
+ Quaternion2 euler = new Quaternion2 ("Quat to euler");
+ double sint,cost,sinv,cosv,sinf,cosf,ex,ey,ez;
+
+ sint = (2*w*y)-(2*x*z);
+ cost = Math.sqrt(1- Math.pow(sint,2));
+ if (cost != 0.0){
+ sinv = ((2*y*z)+(2*w*x))/cost;
+ cosv = (1-(2*x*x)-(2*y*y))/cost;
+ sinf = (1-(2*x*x)-(2*y*y))/cost;
+ cosf = (1-(2*y*y)-(2*z*z))/cost;
+ }else {
+ sinv = (2*w*x)-(2*y*z);
+ cosv = 1-(2*x*x)-(2*z*z);
+ sinf = 0.0;
+ cosf = 1.0;
+ }
+
+ ex = Math.atan2(sinv,cosv);
+ ey = Math.atan2(sint,cost);
+ ez = Math.atan2(sinf,cosf); //range -pi +pi
+
+ ex *= 180.0/Math.PI;
+ ey *= 180.0/Math.PI;
+ ez *= 180.0/Math.PI;
+
+ euler.setQuaternion2(0.0, ex,ey,ez);
+ return (euler);
+}//end toEulerAngles()
+
+
+/** Overrides the toString method
+* @return String object
+*/
+public String toString()
+{
+ return ("[ "+name+" w="+String.valueOf(w)+" x="+String.valueOf(x)+" y="+
+ String.valueOf(y)+" z="+String.valueOf(z)+" ]");
+} // end toString()
+
+
+/** Converts quaternion's values into float
+* @return Float []
+*/
+public float[] toFloat()
+{
+ float myFloat [] = new float[4];
+ myFloat[0] = (float)w;
+ myFloat[1] = (float)x;
+ myFloat[2] = (float)y;
+ myFloat[3] = (float)z;
+ return myFloat;
+} // end toFloat()
+
+
+/** Main method for testing
+*/
+public static void main(String ags[]){
+ Quaternion2 one=new Quaternion2 ("one");
+ Quaternion2 two=new Quaternion2 ("two",5, 1, 2, 3);
+ System.out.println(two);
+ float f[]=new float[4];
+ f=two.toFloat();
+ System.out.println("floats"+" "+f[0]+" "+f[1]+" "+f[2]+" "+f[3]);
+
+ System.out.println(one + "\n"+two);
+ two.normalize();
+
+ System.out.println(two);
+
+ System.out.println(one);
+ System.out.println(one.add(two));
+ Quaternion2 q=new Quaternion2();
+ System.out.println(q);
+ q=one.multiply(two);
+ q.normalize();
+ System.out.println(q);
+
+ q=two.rotate(one);
+ System.out.println(one + "\n "+two+"\n " + q);
+} // end main()
+
+} // end of file Quaternion2.java
diff --git a/src/edu/nps/moves/legacy/math/README.TXT b/src/edu/nps/moves/legacy/math/README.TXT
new file mode 100644
index 0000000000..9de78e4047
--- /dev/null
+++ b/src/edu/nps/moves/legacy/math/README.TXT
@@ -0,0 +1,11 @@
+
+Math Library README
+
+This is a 3-D math library by Kent Watsen, http://www.mbay.net/~watsen
+
+This package contains a variety of 3D classes such as Quaternions, etc.
+It may form the basis for a standard 3D math library for Java.
+
+Although there is a vecmath.jar from the Java3D library, the classes in
+this package provide convenience methods for performing rotations of
+matricies representing 3D entities.
diff --git a/src/edu/nps/moves/legacy/math/Vec3f.java b/src/edu/nps/moves/legacy/math/Vec3f.java
new file mode 100644
index 0000000000..9188317b23
--- /dev/null
+++ b/src/edu/nps/moves/legacy/math/Vec3f.java
@@ -0,0 +1,287 @@
+/*
+Copyright (c) 1995-2021 held by the author(s). All rights reserved.
+
+Redistribution and use in source and binary forms, with or without
+modification, are permitted provided that the following conditions
+are met:
+
+ * Redistributions of source code must retain the above copyright
+ notice, this list of conditions and the following disclaimer.
+ * Redistributions in binary form must reproduce the above copyright
+ notice, this list of conditions and the following disclaimer
+ in the documentation and/or other materials provided with the
+ distribution.
+ * Neither the names of the Naval Postgraduate School (NPS)
+ Modeling Virtual Environments and Simulation (MOVES) Institute
+ (https://www.nps.edu and https://my.nps.edu/web/moves)
+ nor the names of its contributors may be used to endorse or
+ promote products derived from this software without specific
+ prior written permission.
+
+THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+"AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+POSSIBILITY OF SUCH DAMAGE.
+*/
+package edu.nps.moves.legacy.math;
+
+/** EXECUTIVE SUMMARY
+ * Module Name: Vec3f.java
+ * Description: Definition of the Vec3f class
+ * @author Kent A. Watsen, http://www.mbay.net/~watsen
+ */
+public class Vec3f
+ {
+ private
+ float v[];
+
+ public Vec3f()
+ {
+ v = new float[3];
+ makeNull();
+ }
+
+ public Vec3f(float v0, float v1, float v2)
+ {
+ v = new float[3];
+ set(v0, v1, v2);
+ }
+
+ public Vec3f(float vec[])
+ {
+ v = new float[3];
+ set(vec);
+ }
+
+ public Vec3f(Vec3f vec)
+ {
+ v = new float[3];
+ set(vec);
+ }
+
+ public void print()
+ {
+ System.out.println("v = " + v[0] + ", " + v[1] + ", " + v[2]);
+ }
+
+ public void set(float v0, float v1, float v2)
+ {
+ v[0] = v0;
+ v[1] = v1;
+ v[2] = v2;
+ }
+
+ public void get(float v0[], float v1[], float v2[])
+ {
+ v0[0] = v[0];
+ v1[0] = v[1];
+ v2[0] = v[2];
+ }
+
+ public void set(int index, float val)
+ {
+ v[index] = val;
+ }
+
+ public float get(int index)
+ {
+ return v[index];
+ }
+
+ public void set(float vec[])
+ {
+ v[0] = vec[0];
+ v[1] = vec[1];
+ v[2] = vec[2];
+ }
+
+ public void get(float vec[])
+ {
+ vec[0] = v[0];
+ vec[1] = v[1];
+ vec[2] = v[2];
+ }
+
+ public void set(Vec3f vec)
+ {
+ v[0] = vec.get(0);
+ v[1] = vec.get(1);
+ v[2] = vec.get(2);
+ }
+
+ public void get(Vec3f vec)
+ {
+ vec.set(0,v[0]);
+ vec.set(1,v[1]);
+ vec.set(2,v[2]);
+ }
+
+ public void makeNull()
+ {
+ v[0] = 0f;
+ v[1] = 0f;
+ v[2] = 0f;
+ }
+
+ public void negate()
+ {
+ v[0] = -v[0];
+ v[1] = -v[1];
+ v[2] = -v[2];
+ }
+
+ public void negate(Vec3f vec)
+ {
+ v[0] = -vec.get(0);
+ v[1] = -vec.get(1);
+ v[2] = -vec.get(2);
+ }
+
+ public void add(Vec3f vec)
+ {
+ v[0] = v[0] + vec.get(0);
+ v[1] = v[1] + vec.get(1);
+ v[2] = v[2] + vec.get(2);
+ }
+
+ public void add(Vec3f vec1, Vec3f vec2)
+ {
+ v[0] = vec1.get(0) + vec2.get(0);
+ v[1] = vec1.get(1) + vec2.get(1);
+ v[2] = vec1.get(2) + vec2.get(2);
+ }
+
+ public void sub(Vec3f vec)
+ {
+ v[0] = v[0] - vec.get(0);
+ v[1] = v[1] - vec.get(1);
+ v[2] = v[2] - vec.get(2);
+ }
+
+ public void sub(Vec3f vec1, Vec3f vec2)
+ {
+ v[0] = vec1.get(0) - vec2.get(0);
+ v[1] = vec1.get(1) - vec2.get(1);
+ v[2] = vec1.get(2) - vec2.get(2);
+ }
+
+ public void scale(float s)
+ {
+ v[0] = s * v[0];
+ v[1] = s * v[1];
+ v[2] = s * v[2];
+ }
+
+ public void scale(float s, Vec3f vec)
+ {
+ v[0] = s * vec.get(0);
+ v[1] = s * vec.get(1);
+ v[2] = s * vec.get(2);
+ }
+
+ public float length()
+ {
+ return (float)Math.sqrt(v[0]*v[0] + v[1]*v[1] + v[2]*v[2]);
+ }
+
+ public float length_sqr()
+ {
+ return v[0]*v[0] + v[1]*v[1] + v[2]*v[2];
+ }
+
+ public void normalize()
+ {
+ float len_sqr;
+ float one_over_length;
+
+ len_sqr = length_sqr();
+ if (len_sqr > 0.0001f)
+ {
+ one_over_length = 1.0f/(float)Math.sqrt(len_sqr);
+ }
+ else
+ {
+ one_over_length = 0.0f;
+ }
+ v[0] = v[0] * one_over_length;
+ v[1] = v[1] * one_over_length;
+ v[2] = v[2] * one_over_length;
+ }
+
+ public void normalize(Vec3f vec)
+ {
+ float len_sqr;
+ float one_over_length;
+
+ len_sqr = vec.length_sqr();
+ if (len_sqr > 0.0001f)
+ {
+ one_over_length = 1.0f/(float)Math.sqrt(len_sqr);
+ }
+ else
+ {
+ one_over_length = 0.0f;
+ }
+ v[0] = vec.get(0) * one_over_length;
+ v[1] = vec.get(1) * one_over_length;
+ v[2] = vec.get(2) * one_over_length;
+ }
+
+ public float dot(Vec3f vec)
+ {
+ return v[0]*vec.get(0) + v[1]*vec.get(1) + v[2]*vec.get(2);
+ }
+
+ static public float dot(Vec3f vec1, Vec3f vec2)
+ {
+ return vec1.get(0)*vec2.get(0) + vec1.get(1)*vec2.get(1) + vec1.get(2)*vec2.get(2);
+ }
+
+ public void cross(Vec3f vec)
+ {
+ float tmp_float[] = new float[3];
+ tmp_float[0] = v[1]*vec.get(2) - v[2]*vec.get(1);
+ tmp_float[1] = v[2]*vec.get(0) - v[0]*vec.get(2);
+ tmp_float[2] = v[0]*vec.get(1) - v[1]*vec.get(0);
+ set(tmp_float);
+ }
+
+ public void cross(Vec3f vec1, Vec3f vec2)
+ {
+ v[0] = vec1.get(1)*vec2.get(2) - vec1.get(2)*vec2.get(1);
+ v[1] = vec1.get(2)*vec2.get(0) - vec1.get(0)*vec2.get(2);
+ v[2] = vec1.get(0)*vec2.get(1) - vec1.get(1)*vec2.get(0);
+ }
+
+ public void xform(Matrix3f mat) // math_utils
+ {
+ float tmp_v[] = new float[3];
+ float m[][] = new float[3][3];
+
+ mat.getMat(m);
+ tmp_v[0] = v[0]*m[0][0] + v[1]*m[0][1] + v[2]*m[0][2];
+ tmp_v[1] = v[0]*m[1][0] + v[1]*m[1][1] + v[2]*m[1][2];
+ tmp_v[2] = v[0]*m[2][0] + v[1]*m[2][1] + v[2]*m[2][2];
+ set(tmp_v);
+ }
+
+ public void xform(Matrix3f mat, Vec3f vec) // math_utils
+ {
+ float tmp_v[] = new float[3];
+ float m[][] = new float[3][3];
+
+ vec.get(tmp_v);
+ mat.getMat(m);
+ v[0] = tmp_v[0]*m[0][0] + tmp_v[1]*m[0][1] + tmp_v[2]*m[0][2];
+ v[1] = tmp_v[0]*m[1][0] + tmp_v[1]*m[1][1] + tmp_v[2]*m[1][2];
+ v[2] = tmp_v[0]*m[2][0] + tmp_v[1]*m[2][1] + tmp_v[2]*m[2][2];
+ }
+}
diff --git a/src/edu/nps/moves/legacy/math/Vec4f.java b/src/edu/nps/moves/legacy/math/Vec4f.java
new file mode 100644
index 0000000000..85227ecdb8
--- /dev/null
+++ b/src/edu/nps/moves/legacy/math/Vec4f.java
@@ -0,0 +1,290 @@
+/*
+Copyright (c) 1995-2021 held by the author(s). All rights reserved.
+
+Redistribution and use in source and binary forms, with or without
+modification, are permitted provided that the following conditions
+are met:
+
+ * Redistributions of source code must retain the above copyright
+ notice, this list of conditions and the following disclaimer.
+ * Redistributions in binary form must reproduce the above copyright
+ notice, this list of conditions and the following disclaimer
+ in the documentation and/or other materials provided with the
+ distribution.
+ * Neither the names of the Naval Postgraduate School (NPS)
+ Modeling Virtual Environments and Simulation (MOVES) Institute
+ (https://www.nps.edu and https://my.nps.edu/web/moves)
+ nor the names of its contributors may be used to endorse or
+ promote products derived from this software without specific
+ prior written permission.
+
+THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+"AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+POSSIBILITY OF SUCH DAMAGE.
+*/
+package edu.nps.moves.legacy.math;
+
+/** EXECUTIVE SUMMARY
+ * Module Name: Vec4f.java
+ * Description: Definition of the Vec4f class
+ * Author: Kent A. Watsen, http://www.mbay.net/~watsen
+ */
+public class Vec4f
+ {
+ private
+ float v[];
+
+ public Vec4f()
+ {
+ v = new float[4];
+ makeNull();
+ }
+
+ public Vec4f(float v0, float v1, float v2, float v3)
+ {
+ v = new float[4];
+ set(v0, v1, v2, v3);
+ }
+
+ public Vec4f(float vec[])
+ {
+ v = new float[4];
+ set(vec);
+ }
+
+ public Vec4f(Vec4f vec)
+ {
+ v = new float[4];
+ set(vec);
+ }
+
+ public void print()
+ {
+ System.out.println("v = " + v[0] + ", " + v[1] + ", " + v[2] + ", " + v[3]);
+ }
+
+ public void set(float v0, float v1, float v2, float v3)
+ {
+ v[0] = v0;
+ v[1] = v1;
+ v[2] = v2;
+ v[3] = v3;
+ }
+
+ public void get(float v0[], float v1[], float v2[], float v3[])
+ {
+ v0[0] = v[0];
+ v1[0] = v[1];
+ v2[0] = v[2];
+ v3[0] = v[3];
+ }
+
+ public void set(int index, float val)
+ {
+ if (index < 0 || index > 3)
+ return;
+ v[index] = val;
+ }
+
+ public float get(int index)
+ {
+ if (index < 0 || index > 3)
+ return 0.0f;
+ return v[index];
+ }
+
+ public void set(float vec[])
+ {
+ v[0] = vec[0];
+ v[1] = vec[1];
+ v[2] = vec[2];
+ v[3] = vec[3];
+ }
+
+ public void get(float vec[])
+ {
+ vec[0] = v[0];
+ vec[1] = v[1];
+ vec[2] = v[2];
+ vec[3] = v[3];
+ }
+
+ public void set(Vec4f vec)
+ {
+ v[0] = vec.get(0);
+ v[1] = vec.get(1);
+ v[2] = vec.get(2);
+ v[3] = vec.get(3);
+ }
+
+ public void get(Vec4f vec)
+ {
+ vec.set(v[0], v[1], v[2], v[3]);
+ }
+
+ public void makeNull()
+ {
+ v[0] = 0f;
+ v[1] = 0f;
+ v[2] = 0f;
+ }
+
+ public void negate()
+ {
+ v[0] = -v[0];
+ v[1] = -v[1];
+ v[2] = -v[2];
+ v[3] = -v[3];
+ }
+
+ public void negate(Vec4f vec)
+ {
+ v[0] = -vec.get(0);
+ v[1] = -vec.get(1);
+ v[2] = -vec.get(2);
+ v[3] = -vec.get(3);
+ }
+
+ public void add(Vec4f vec)
+ {
+ v[0] = v[0] + vec.get(0);
+ v[1] = v[1] + vec.get(1);
+ v[2] = v[2] + vec.get(2);
+ v[3] = v[3] + vec.get(3);
+ }
+
+ public void add(Vec4f vec1, Vec4f vec2)
+ {
+ v[0] = vec1.get(0) + vec2.get(0);
+ v[1] = vec1.get(1) + vec2.get(1);
+ v[2] = vec1.get(2) + vec2.get(2);
+ v[3] = vec1.get(3) + vec2.get(3);
+ }
+
+ public void sub(Vec4f vec)
+ {
+ v[0] = v[0] - vec.get(0);
+ v[1] = v[1] - vec.get(1);
+ v[2] = v[2] - vec.get(2);
+ v[3] = v[3] - vec.get(3);
+ }
+
+ public void sub(Vec4f vec1, Vec4f vec2)
+ {
+ v[0] = vec1.get(0) - vec2.get(0);
+ v[1] = vec1.get(1) - vec2.get(1);
+ v[2] = vec1.get(2) - vec2.get(2);
+ v[3] = vec1.get(3) - vec2.get(3);
+ }
+
+ public void scale(float s)
+ {
+ v[0] = s * v[0];
+ v[1] = s * v[1];
+ v[2] = s * v[2];
+ v[3] = s * v[3];
+ }
+
+ public void scale(float s, Vec4f vec)
+ {
+ v[0] = s * vec.get(0);
+ v[1] = s * vec.get(1);
+ v[2] = s * vec.get(2);
+ v[3] = s * vec.get(3);
+ }
+
+ public float length()
+ {
+ return (float)Math.sqrt(v[0]*v[0] + v[1]*v[1] + v[2]*v[2] + v[3]*v[3]);
+ }
+
+ public float length_sqr()
+ {
+ return v[0]*v[0] + v[1]*v[1] + v[2]*v[2] + v[3]*v[3];
+ }
+
+ public void normalize()
+ {
+ float len_sqr;
+ float one_over_length;
+
+ len_sqr = length_sqr();
+ if (len_sqr > 0.0001f)
+ {
+ one_over_length = 1.0f/(float)Math.sqrt(len_sqr);
+ }
+ else
+ {
+ one_over_length = 0.0f;
+ }
+ v[0] = v[0] * one_over_length;
+ v[1] = v[1] * one_over_length;
+ v[2] = v[2] * one_over_length;
+ v[3] = v[3] * one_over_length;
+ }
+
+ public void normalize(Vec4f vec)
+ {
+ float len_sqr;
+ float one_over_length;
+
+ len_sqr = vec.length_sqr();
+ if (len_sqr > 0.0001f)
+ {
+ one_over_length = 1.0f/(float)Math.sqrt(len_sqr);
+ }
+ else
+ {
+ one_over_length = 0.0f;
+ }
+ v[0] = vec.get(0) * one_over_length;
+ v[1] = vec.get(1) * one_over_length;
+ v[2] = vec.get(2) * one_over_length;
+ v[3] = vec.get(3) * one_over_length;
+ }
+
+ public float dot(Vec4f vec)
+ {
+ return v[0]*vec.get(0) + v[1]*vec.get(1) + v[2]*vec.get(2) + v[3]*vec.get(3);
+ }
+
+ static public float dot(Vec4f vec1, Vec4f vec2)
+ {
+ return vec1.get(0)*vec2.get(0) + vec1.get(1)*vec2.get(1) + vec1.get(2)*vec2.get(2) + vec1.get(3)*vec2.get(3);
+ }
+
+ public void xform(Matrix4f mat) // math_utils
+ {
+ float tmp_v[] = new float[4];
+ float m[][] = new float[4][4];
+
+ mat.getMat(m);
+ tmp_v[0] = v[0]*m[0][0] + v[1]*m[0][1] + v[2]*m[0][2] + v[3]*m[0][3];
+ tmp_v[1] = v[0]*m[1][0] + v[1]*m[1][1] + v[2]*m[1][2] + v[3]*m[1][3];
+ tmp_v[2] = v[0]*m[2][0] + v[1]*m[2][1] + v[2]*m[2][2] + v[3]*m[2][3];
+ tmp_v[3] = v[0]*m[3][0] + v[1]*m[3][1] + v[2]*m[3][2] + v[3]*m[3][3];
+ set(tmp_v);
+ }
+
+ public void xform(Matrix4f mat, Vec4f vec) // math_utils
+ {
+ float tmp_v[] = new float[4];
+ float m[][] = new float[4][4];
+
+ vec.get(tmp_v);
+ mat.getMat(m);
+ v[0] = tmp_v[0]*m[0][0] + tmp_v[1]*m[0][1] + tmp_v[2]*m[0][2] + tmp_v[3]*m[0][3];
+ v[1] = tmp_v[0]*m[1][0] + tmp_v[1]*m[1][1] + tmp_v[2]*m[1][2] + tmp_v[3]*m[1][3];
+ v[2] = tmp_v[0]*m[2][0] + tmp_v[1]*m[2][1] + tmp_v[2]*m[2][2] + tmp_v[3]*m[2][3];
+ v[3] = tmp_v[0]*m[3][0] + tmp_v[1]*m[3][1] + tmp_v[2]*m[3][2] + tmp_v[3]*m[3][3];
+ }
+ }
diff --git a/src/edu/nps/moves/legacy/math/package.html b/src/edu/nps/moves/legacy/math/package.html
new file mode 100644
index 0000000000..2e58b93688
--- /dev/null
+++ b/src/edu/nps/moves/legacy/math/package.html
@@ -0,0 +1,16 @@
+<HTML>
+<head>
+<title>edu.nps.moves.legacy.math package</title>
+</head>
+
+<!-- Used by Javadoc documentation generation -->
+
+<body>
+
+Contains several useful math-related classes.
+These may be removed in a future build if we begin to use Java3D-related math classes.
+<P>
+Additional information appears in the
+<A HREF="README.TXT" target="_top">README</A>.
+</body>
+</HTML>
diff --git a/test/edu/nps/moves/dis7/AllPduRoundTripTest.java b/test/edu/nps/moves/dis7/AllPduRoundTripTest.java
index d14cab3963..31f8866d0c 100644
--- a/test/edu/nps/moves/dis7/AllPduRoundTripTest.java
+++ b/test/edu/nps/moves/dis7/AllPduRoundTripTest.java
@@ -73,7 +73,7 @@ public class AllPduRoundTripTest
try {
setupSenderRecorder();
- pduFactory = new PduFactory(Country.PHILIPPINES_PHL, (byte) 11, (byte) 22, (short) 33, true);
+ pduFactory = new PduFactory(Country.PHILIPPINES_PHL, (byte) 11, (byte) 22, (short) 33, PduFactory.TimestampStyle.IEEE_ABSOLUTE);
pdusSent.add(pduFactory.makeAcknowledgePdu());
pdusSent.add(pduFactory.makeAcknowledgeReliablePdu());
diff --git a/test/edu/nps/moves/dis7/PduFactoryTest.java b/test/edu/nps/moves/dis7/PduFactoryTest.java
index bac9dfc286..ae1fb6493b 100644
--- a/test/edu/nps/moves/dis7/PduFactoryTest.java
+++ b/test/edu/nps/moves/dis7/PduFactoryTest.java
@@ -44,7 +44,7 @@ public class PduFactoryTest
Throwable ex = null;
try {
//Arrays.stream(PduFactory.class.getDeclaredMethods()).forEach(m->System.out.println(m.getName()));
- PduFactory fact = new PduFactory(Country.PHILIPPINES_PHL, (byte) 11, (byte) 22, (short) 33, true);
+ PduFactory fact = new PduFactory(Country.PHILIPPINES_PHL, (byte) 11, (byte) 22, (short) 33, PduFactory.TimestampStyle.IEEE_ABSOLUTE);
fact.makeAcknowledgePdu();
fact.makeAcknowledgeReliablePdu();
--
GitLab