*
*
*
*| 30 September * | Don Brutzman and Duane Davis * | Final changes to comply with X3D 3.0 specification changes * |
| 21 July 2003 * | Don McGregor and Don Brutzman * | Numerous changes to comply with X3D specification changes * |
| 14 April 2002 * | Don Brutzman * | revised networkMode enumerations: "standAlone" | "networkReader" | "networkWriter" * and corresponding Booleans isStandAlone, isNetworkReader, isNetworkWriter * |
| 25 January 2002 * | Don Brutzman * | added networkMode/set_networkMode (local | master | remote), plus Booleans isLocal, isMaster, isRemote * |
| 1 February 2000 * | Olivier Doucy, Don Brutzman * | added timestamp as an eventOut * |
| 3 September 99 * | Don Brutzman * | added rtpEnabled boolean flag * |
| 19 June 99 * | Don Brutzman * | Removed articulationParameterValue15 to stay within VRML browser conformance limit of 25 eventOuts * |
| 1 June 99 * | Don Brutzman, Quay Jones * | Added isDetonated and detonateTime * |
| 18 May 99 * | Don Brutzman, Quay Jones * | Changed fired to fired1 and fired2 in VRML scene to enable primary and secondary weapons. * |
| 6 Apr 99 * | Don Brutzman * | Adding ArticulatedParameter eventOuts, shutdown, various thread/trace fixes. * Ignore received PDUs when sending. Changed MFVec3f LineOfFirePointArray code to * SFVec3f's munitionStartPoint & munitionEndPoint for run-time reliability. * |
| 20 Feb 99, 14 Mar 99 * | Don Brutzman, Don McGregor, Bill Bohman * | Added isCollided, isActive, fired, multicastRelayHost, multicastRelayPort. * Also added FirePdu geometry to show munitions. * Incorporated entityDispatcher.checkForTrafficAndFallBackToTunnel (...) * for automatic unicast connectivity when multicast is not locally available. * |
| 6 Feb 99 * | Don Brutzman * | Move VRML EspduTransformPROTO to same directory as dis package
* * Fix write capability * |
| 02 Dec 98 * | Scott Heller * | Added smoothing. See * SmoothingDesign.pdf * |
| 25 Oct 98 * | Don Brutzman * | Javadoc updates for jdk1.2b4
* * Restructure classes, move EspduTransform to dis package * |
| 12 Feb 98 * | Don Brutzman * | changes for documentation templates + complements in documentation * |
| 5 Feb 98 * | Don McGregor * | changes for multiple entities, using EntityDispatcher * |
| 1 Aug 97 * | Don Brutzman * | New * |
* *
* *
* * DIS VRML * * -Z entity up Y entity up * ^ ^ * | | * | | * | | * +------> X +-------> X nose of entity body * / / * / / * / / * Y Z * right-hand side * of entity body * * Rotation angle axes (right-hand rule applies): * * DIS VRML Angle of rotation * * Roll X X phi * Pitch Y Z theta * Yaw Z -Y psi ** *
* *
static or all entities look the same.
*/
private SFString marking = new SFString ("initialize");
/**
* field hooks to state nodes
*/
private SFNode transformNode, markingNode, networkModeNode, munitionPointNode,
readWriteIntervalNode, siteIdNode, entityIdNode, applicationIdNode,
addressNode, multicastAddressNode, portNode, multicastPortNode;
private Node transformNodeAccess, markingNodeAccess, networkModeNodeAccess, munitionPointNodeAccess,
readWriteIntervalNodeAccess, siteIdNodeAccess, entityIdNodeAccess, applicationIdNodeAccess,
addressNodeAccess, multicastAddressNodeAccess, portNodeAccess, multicastPortNodeAccess;
/**
* field with value ( "standAlone" | "networkReader" | "networkWriter" )
*/
private SFString networkMode = new SFString ();
/**
* eventIn with value ( "standAlone" | "networkReader" | "networkWriter" )
*/
private SFString set_networkMode = new SFString ();
/**
* eventOut, with values true or false.
*/
private SFBool isStandAlone = new SFBool (false);
/**
* eventOut, with values true or false.
*/
private SFBool isNetworkWriter = new SFBool (false);
/**
* eventOut, with values true or false.
*/
private SFBool isNetworkReader = new SFBool (false);
/**
* internal state: reading?
*/
private boolean readMode = false;
/**
* internal state: writing?
*/
private boolean writeMode = false;
/**
* field, how often to look for DIS PDUs. Units are seconds in VRML, converted to msec in Java in initialize ().
* readInterval=0 means no reading. Longs are used because VRML SFFloat units are seconds.
*/
private long readInterval;
/**
* field, how often to write DIS PDUs. Units are seconds in VRML, converted to msec in Java in initialize ().
* writeInterval=0 means no writing (which is the default). Longs are used because VRML SFFloat units are seconds.
*/
private long writeInterval;
/**
* field, multicast address, or "unicast"
*/
private SFString address;
/**
* field, port to listen on
*/
private SFInt32 port;
/**
* field, relay host name or IP address (used only if no multicast heard)
*/
private SFString multicastRelayHost;
/**
* field, relay host port to connect to (used only if no multicast heard)
*/
private SFInt32 multicastRelayPort;
/**
* field, are RTP headers expected? Only set by VRML scene at initialization (because it is a field)
* but can be reset at runtime if the opposite situation is encountered.
*/
private SFBool rtpHeaderExpected;
/**
* eventOut, are RTP headers being heard on the wire?
*/
private SFBool isRtpHeaderHeard = new SFBool (false);
/**
* field, entityID triplet: site ID
*/
private SFInt32 siteID;
/**
* field, entityID triplet: unique app ID at that site
*/
private SFInt32 applicationID;
/**
* field, entity ID triplet: ID within that app
*/
private SFInt32 entityID;
/**
* EspduTransformPROTO VRML scene exposedField
*/
private SFVec3f translation;
/**
* EspduTransformPROTO VRML scene exposedField
*/
private SFRotation rotation;
/**
* field: enable Java trace statements to console
*/
private SFBool traceJava;
/**
* eventOut: recent active ESPDUs heard
*/
private SFBool isActive = new SFBool (false);
/**
* eventOut: Collision PDU heard
*/
private SFBool isCollided = new SFBool (false);
/**
* eventOut: time of Collision
*/
private SFTime collideTime;
/**
* eventOut: Detonation PDU heard
*/
private SFBool isDetonated = new SFBool (false);
/**
* eventOut: time of detonation
*/
private SFTime detonateTime;
/**
* eventOut: have we fired a Fire (weapon) PDU for primary (major) weapon?
*/
private SFBool fired1 = new SFBool (false);
/**
* eventOut: have we fired a Fire (weapon) PDU for secondary (minor) weapon?
*/
private SFBool fired2 = new SFBool (false);
/**
* eventOut: when did we fire a Fire (weapon) PDU?
*/
private SFTime firedTime;
/** variables relating to position & orientation*/
private SFVec3f position;
private SFRotation orientation;
private int pduCount = 0;
private Quaternion quaternion = null;
private float rot[] = null;
private float eulers[] = null;
private EntityID entityIDObject = null; // unique triplet that identifies entity
private Vector receivedPDUsList = new Vector(); // holds PDUs that have come in
private boolean registered = false; // true=>has been placed in dispatcher hash table
private boolean entityDispatcherThreadStarted = false;
protected Browser browser = null; // VRML browser
private int pduVectorSize;
protected ProtocolDataUnit pdu = null;
protected final int MAX_ARTICULATION_PARAMETERS = 15;
protected SFInt32 articulationParameterCount; // read from network->VRML only, no write from VRML->network yet
protected SFFloat articulationParameterValue0;
protected SFFloat articulationParameterValue1;
protected SFFloat articulationParameterValue2;
protected SFFloat articulationParameterValue3;
protected SFFloat articulationParameterValue4;
protected SFFloat articulationParameterValue5;
protected SFFloat articulationParameterValue6;
protected SFFloat articulationParameterValue7;
protected SFFloat articulationParameterValue8;
protected SFFloat articulationParameterValue9;
protected SFFloat articulationParameterValue10;
protected SFFloat articulationParameterValue11;
protected SFFloat articulationParameterValue12;
protected SFFloat articulationParameterValue13;
protected SFFloat articulationParameterValue14;
private int previousChangeIndicator0 = -2;
private int previousChangeIndicator1 = -2;
private int previousChangeIndicator2 = -2;
private int previousChangeIndicator3 = -2;
private int previousChangeIndicator4 = -2;
private int previousChangeIndicator5 = -2;
private int previousChangeIndicator6 = -2;
private int previousChangeIndicator7 = -2;
private int previousChangeIndicator8 = -2;
private int previousChangeIndicator9 = -2;
private int previousChangeIndicator10 = -2;
private int previousChangeIndicator11 = -2;
private int previousChangeIndicator12 = -2;
private int previousChangeIndicator13 = -2;
private int previousChangeIndicator14 = -2;
private int changeIndicator0 = -1;
private int changeIndicator1 = -1;
private int changeIndicator2 = -1;
private int changeIndicator3 = -1;
private int changeIndicator4 = -1;
private int changeIndicator5 = -1;
private int changeIndicator6 = -1;
private int changeIndicator7 = -1;
private int changeIndicator8 = -1;
private int changeIndicator9 = -1;
private int changeIndicator10 = -1;
private int changeIndicator11 = -1;
private int changeIndicator12 = -1;
private int changeIndicator13 = -1;
private int changeIndicator14 = -1;
protected EntityStatePdu espdu = new EntityStatePdu ();
private boolean espduFirstHeard = true;
/**
* added by Scott Heller 21 Nov 98 for smoothing
*/
private EntityStatePdu previousEspdu = null;
private float prev_dt = 0.0f;
private RunningAverage averageUpdateTime = new RunningAverage(5);
private RunningAverage aveSecPerFrame = new RunningAverage(5);
private float[] positionArray = new float[3];
private final int ROLL = 2, PITCH = 1, YAW = 0;
private boolean DoNotSmooth = false;
private mil.navy.nps.dis.Timer secSinceLastPdu = new mil.navy.nps.dis.Timer(); // seconds
private mil.navy.nps.dis.Timer secPerFrame = new mil.navy.nps.dis.Timer(); // seconds per browser frame
private boolean SMOOTH_LINVELOCITY = false; // should velocity be smoothed?
/**
* what portion of an averageupdateTime should we smooth for? For example 0.5 will cause the
* the smoothing algorithm to reaquire the most recent DR track with in half the
* PDU averageUpdateTime.
*/
protected final float SMOOTH_LIMIT = 1.0f; // controls how fast the smoothed update
// SMOOTH_LIMIT * aveSecPerFrame = how fast.
// 1.0f seems to be the only pleasing value.
protected final float DR_LIMIT = 5.0f; // seconds, max time to dead reckon without updates
/**
* are the DR and smooth close enough to stop smoothing?
*/
private boolean closeEnough = false;
private float initPos[] = {0f, 5f, 0f}; // x, y, z
private float initPosDt[] = {0f, 0f, 0f}; // aka: velocity
private float initAng[] = {0f, 0f, 0f}; // heading, pitch, roll
private float initAngDt[] = {0f, 0f, 0f}; // aka: angular rates
private Quaternion tmpQuat = null;
private float pos[] = null; // x, y, z
private float posDt[] = null; // aka: velocity
private float ang[] = null; // heading, pitch, roll
private float angDt[] = null; // aka: angular rates
private float drPos[] = null; // x, y, z
private float drPosDt[] = null; // aka: velocity
private float drAng[] = null; // heading, pitch, roll
private float drAngDt[] = null; // aka: angular rates
/**
* munitionStartPoint is node's field (hook to VRML scene) obtained from FirePdu matching launch point of munition.
*/
protected SFVec3f munitionStartPoint = new SFVec3f (0, 0, 0);
/**
* munitionEndPoint is node's field (hook to VRML scene) computed from FirePdu matching detonation point of munition.
*/
protected SFVec3f munitionEndPoint = new SFVec3f (0, 0, 0);
/** local count of firePdu's */
private int firePduCount = 0;
/**
* Timers. Note that in JDK 1.4 and later there is a "Timer" class that we
* need to differentiate ourselves from. However, Cortona is only using
* JDK 1.3 at best, so you shouldn't be compiling this with 1.4 anyway.
*/
protected mil.navy.nps.dis.Timer localFireTimer1 = new mil.navy.nps.dis.Timer ();
protected mil.navy.nps.dis.Timer localFireTimer2 = new mil.navy.nps.dis.Timer ();
// Static variables
protected static EntityDispatcher entityDispatcher = null; // sends/receives PDUs
protected static Thread entityDispatcherThread;
private static Boolean guard = new Boolean(true); // guard for entityDispatcher
/**
* Constructor
*/
public EspduTransform()
{
// do nothing
}
/**
* Turn Smoothing on or off. Returns the new state of DoNotSmooth.
*/
public boolean toggleSmoothing()
{
if (DoNotSmooth) DoNotSmooth = false;
else DoNotSmooth = true;
return DoNotSmooth;
}
/**
* Get Smoothing flag.
*/
public boolean getDoNotSmooth()
{
trace ("getDoNotSmooth = " + DoNotSmooth);
return DoNotSmooth;
}
/**
* Set Smoothing flag.
*/
public void setDoNotSmooth(boolean pDoNotSmooth)
{
DoNotSmooth = pDoNotSmooth;
debug ("setDoNotSmooth = " + DoNotSmooth);
return;
}
/**
* When DEBUG is true, console text messages
* trace the internals of script operation.
*/
protected boolean DEBUG = false; // diagnostic messages on (true) or off (false)
/**
* Access DEBUG trace variable.
*/
public boolean getDEBUG ()
{
debug ("getDEBUG " + DEBUG);
return DEBUG;
}
/**
* Modify DEBUG trace variable.
*/
public void setDEBUG (boolean pDEBUG)
{
DEBUG = pDEBUG;
traceJava.setValue (pDEBUG);
trace ("setDEBUG " + pDEBUG); // send confirming VRML console trace message
}
/**
* Debugging output routine. Pass in a string, and it gets printed out on the console.
* You can pass in strings such as "foo " + bar.getName().
* Text output appears in the Java Console (CosmoPlayer browser)
* or in the VRML console (WorldView browser).
*/
protected void debug (String pDiagnostic)
{
if (DEBUG) System.out.println(" [EspduTransform] " + marking + " " + pDiagnostic);
System.out.flush ();
}
/**
* Guaranteed trace output routine. Pass in a string, and it gets printed out on the console.
* You can pass in strings such as "foo " + bar.getName().
* Text output appears in the Java Console (CosmoPlayer browser)
* or in the VRML console (WorldView browser).
*
* Can't be static or all entities look the same.
*/
protected void trace (String pDiagnostic)
{
System.out.println(" [EspduTransform] " + marking + " " + pDiagnostic);
System.out.flush ();
}
/**
* Simple name for the ESPDU is the marking field.
*/
public String getName () { return "EspduTransform " + marking; }
/**
* Gets references of necessary nodes of VRML file, and runs the threads that
* controls the scene
*/
public void initialize()
{
try
{
DEBUG=true;
trace ("begin initialize () ...");
// Get the VRML Marking exposedField-placeholder Anchor node, where marking is text about the entity
markingNode = (SFNode) getField ("markingNode");
markingNodeAccess = (Node)(markingNode.getValue());
marking = (SFString) markingNodeAccess.getExposedField ("description");
espdu.setMarking (marking.getValue());
debug ("marking = " + marking ); // expected marking is prefixed in output statements
// Initialize some orientation variables
quaternion = new Quaternion ();
orientation = new SFRotation (0, 0, 0, 0);
eulers = new float[3];
rot = new float[4];
// Retrieve various values from the fields of the node, so we can later work
// with them.
// Turn tracing on or off, depending on the value retrieved from parent Script node
traceJava = (SFBool) getField ("traceJava");
setDEBUG (traceJava.getValue());
debug ("traceJava.getValue()=" + traceJava.getValue());
browser = getBrowser ();
if (DEBUG)
{
debug ("browser.getName () = " + browser.getName ());
debug ("browser.getVersion () = " + browser.getVersion ());
}
// Sanity check: ensure dis library visible, otherwise report problem:
try
{
Class.forName ("mil.navy.nps.dis.EntityStatePdu");
debug ("mil.navy.nps.dis.EntityStatePdu class found");
}
catch (ClassNotFoundException e)
{
trace ("mil.navy.nps.dis.EntityStatePdu class not found, which probably");
trace("indicates that the dis library is not on the classpath. You should");
trace("probably add dis-java-vrml.jar to the classpath.");
trace (" " + e);
}
// Get the VRML transform node, which we modify to change the object in the scene
transformNode = (SFNode) getField ("transformNode");
transformNodeAccess = (Node)(transformNode.getValue());
// Translation & rotation
translation = (SFVec3f) transformNodeAccess.getExposedField ("translation");
rotation = (SFRotation) transformNodeAccess.getExposedField ("rotation");
geoCoords_changed = (SFString) getEventOut("geoCoords_changed");// experimental
// Entity ID values
siteIdNode = (SFNode) getField ("siteIdNode");
siteIdNodeAccess = (Node)(siteIdNode.getValue());
siteID = (SFInt32) siteIdNodeAccess.getExposedField ("whichChoice");
applicationIdNode = (SFNode) getField ("applicationIdNode");
applicationIdNodeAccess = (Node)(applicationIdNode.getValue());
applicationID = (SFInt32) applicationIdNodeAccess.getExposedField ("whichChoice");
entityIdNode = (SFNode) getField ("entityIdNode");
entityIdNodeAccess = (Node)(entityIdNode.getValue());
entityID = (SFInt32) entityIdNodeAccess.getExposedField ("whichChoice");
// siteID = (SFInt32)getField("siteID");
// applicationID = (SFInt32)getField("applicationID");
// entityID = (SFInt32)getField("entityID");
// Set the entity ID of the espdu to match the entity passed in
espdu.setEntityID ((short)siteID.getValue(), (short) applicationID.getValue(), (short) entityID.getValue());
trace ("siteID, applicationID, entityID = " + siteID + " " + applicationID + " " + entityID);
// Networking parameters.
addressNode = (SFNode) getField ("addressNode");
addressNodeAccess = (Node)(addressNode.getValue());
address = (SFString) addressNodeAccess.getExposedField ("description");
multicastAddressNode = (SFNode) getField ("multicastAddressNode");
multicastAddressNodeAccess = (Node)(multicastAddressNode.getValue());
multicastRelayHost = (SFString) multicastAddressNodeAccess.getExposedField ("description");
portNode = (SFNode) getField ("portNode");
portNodeAccess = (Node)(portNode.getValue());
port = (SFInt32) portNodeAccess.getExposedField ("whichChoice");
multicastPortNode = (SFNode) getField ("multicastPortNode");
multicastPortNodeAccess = (Node)(multicastPortNode.getValue());
multicastRelayPort = (SFInt32) multicastPortNodeAccess.getExposedField ("whichChoice");
// address = (SFString) getField("address"); // network address
// port = (SFInt32) getField("port");
// multicastRelayHost = (SFString) getField ("multicastRelayHost");
// multicastRelayPort = (SFInt32) getField ("multicastRelayPort");
trace ("address, port = " + address + ", " + port);
trace ("multicastRelayHost, multicastRelayPort = " + multicastRelayHost + ", " + multicastRelayPort);
// Get the readInterval/writeInterval exposedField-placeholder TimeSensor node
readWriteIntervalNode = (SFNode) getField ("readWriteIntervalNode");
readWriteIntervalNodeAccess = (Node)(readWriteIntervalNode.getValue());
// * l000.0 is conversion to msec, then cast float to long
readInterval = (long) (((SFTime) readWriteIntervalNodeAccess.getExposedField ("startTime")).getValue() * 1000.0);
writeInterval = (long) (((SFTime) readWriteIntervalNodeAccess.getExposedField ("stopTime" )).getValue() * 1000.0);
// The frequency of read and write updates:
trace ("readInterval=" + readInterval + ", writeInterval=" + writeInterval);
// readInterval = (long) (((SFTime) getField ("readInterval")).getValue() * 1000.0);
// writeInterval = (long) (((SFTime) getField ("writeInterval")).getValue() * 1000.0);
// Get the network mode exposedField-placeholder Anchor node
networkModeNode = (SFNode) getField ("networkModeNode");
networkModeNodeAccess = (Node)(networkModeNode.getValue());
networkMode = (SFString) networkModeNodeAccess.getExposedField ("description");
debug ("networkMode=" + networkMode);
// Do not set eventOuts during initialization!!
// rtp (real time protocol) headers may be included at the start of dis
// packets. This hints us to look for them.
timestamp = (SFTime) getEventOut ("timestamp");
isActive = (SFBool) getEventOut ("isActive");
rtpHeaderExpected = (SFBool) getField ("rtpHeaderExpected");
isRtpHeaderHeard = (SFBool) getEventOut ("isRtpHeaderHeard");
// Retrieve various fields
isCollided = (SFBool) getEventOut ("isCollided");
isDetonated = (SFBool) getEventOut ("isDetonated");
// fired1 = (SFBool) getEventOut ("fired1");
// fired2 = (SFBool) getEventOut ("fired2");
collideTime = (SFTime) getEventOut ("collideTime");
detonateTime = (SFTime) getEventOut ("detonateTime");
firedTime = (SFTime) getEventOut ("firedTime");
// Do not set eventOuts during initialization!!
debug ("getting LineOfFirePointArray eventOut");
// Get the munitionPointNode exposedField-placeholder Transform node
munitionPointNode = (SFNode) getField ("munitionPointNode");
munitionPointNodeAccess = (Node)(munitionPointNode.getValue());
munitionStartPoint = (SFVec3f) munitionPointNodeAccess.getExposedField ("translation");
munitionEndPoint = (SFVec3f) munitionPointNodeAccess.getExposedField ("center");
debug ("munitionStartPoint=" + munitionStartPoint + ", munitionEndPoint=" + munitionEndPoint);
// munitionStartPoint = (SFVec3f) getEventOut ("munitionStartPoint"); // instantiate
// munitionEndPoint = (SFVec3f) getEventOut ("munitionEndPoint"); // instantiate
/*
// Retrieve articulation parameters
debug ("Initialize articulation parameters...");
articulationParameterCount = (SFInt32) getEventOut ("articulationParameterCount");
articulationParameterValue0 = (SFFloat) getEventOut ("articulationParameterValue0");
articulationParameterValue1 = (SFFloat) getEventOut ("articulationParameterValue1");
articulationParameterValue2 = (SFFloat) getEventOut ("articulationParameterValue2");
articulationParameterValue3 = (SFFloat) getEventOut ("articulationParameterValue3");
articulationParameterValue4 = (SFFloat) getEventOut ("articulationParameterValue4");
articulationParameterValue5 = (SFFloat) getEventOut ("articulationParameterValue5");
articulationParameterValue6 = (SFFloat) getEventOut ("articulationParameterValue6");
articulationParameterValue7 = (SFFloat) getEventOut ("articulationParameterValue7");
articulationParameterValue8 = (SFFloat) getEventOut ("articulationParameterValue8");
articulationParameterValue9 = (SFFloat) getEventOut ("articulationParameterValue9");
articulationParameterValue10 = (SFFloat) getEventOut ("articulationParameterValue10");
articulationParameterValue11 = (SFFloat) getEventOut ("articulationParameterValue11");
articulationParameterValue12 = (SFFloat) getEventOut ("articulationParameterValue12");
articulationParameterValue13 = (SFFloat) getEventOut ("articulationParameterValue13");
articulationParameterValue14 = (SFFloat) getEventOut ("articulationParameterValue14");
*/
// use half of readInterval (or half of writeInterval) as recommended sleepInterval
// for entityDispatcher.
if(readInterval > 0l)
{
if (writeInterval > 0l)
{
trace ("both readInterval & writeInterval > 0, " +
"resetting writeInterval = 0");
}
writeInterval = 0l; // ensure zero, not negative
debug ("readInterval = " + readInterval + " msec");
}
else if (writeInterval > 0l)
{
readInterval = 0l; // ensure zero, not negative
espdu.setRtpHeaderEnabled (rtpHeaderExpected.getValue());
trace ("writeInterval = " + writeInterval + " msec, " +
"rtpHeaderExpected = " + rtpHeaderExpected);
}
else
{
trace ("read & write intervals both <= 0, " +
"setting readInterval = 1000 msec, writeInterval = 0");
readInterval = 1000l;
writeInterval = 0l; // ensure zero, not negative
}
// Set up smoothing & dead reckoning
// hi-fi vars
pos = new float[3]; // x, y, z
posDt = new float[3]; // aka: velocity
ang = new float[3]; // heading, pitch, roll
angDt = new float[3]; // aka: angular rates
// lo-fi vars
drPos = new float[3]; // x, y, z
drPosDt = new float[3]; // aka: velocity
drAng = new float[3]; // heading, pitch, roll
drAngDt = new float[3]; // aka: angular rates
// these are used for hpr->axis/angle conversions
tmpQuat = new Quaternion();
rot = new float[4]; // axis/angle
// now set init vars
System.arraycopy(initPos, 0, pos, 0, 3);
System.arraycopy(initPosDt, 0, posDt, 0, 3);
System.arraycopy(initAng, 0, ang, 0, 3);
System.arraycopy(initAngDt, 0, angDt, 0, 3);
// now convert hpr (ang) to axis/angle (rot)
tmpQuat.setEulers(ang);
tmpQuat.getAxisAngle(rot);
setDoNotSmooth (true); // turn smoothing off while diagnosing position jitter. unverified!
// Set up the entity dispatcher. This uses the singleton pattern; we
// should have just one copy of EntityDispatcher instantiated, which
// the getEntityDispatcher class enforces.
debug ("initialize: entityDispatcher instantiation:");
if (address.getValue().equalsIgnoreCase ("unicast") || // Unicast
address.getValue().equalsIgnoreCase ("localhost"))
{
entityDispatcher = EntityDispatcher.getEntityDispatcher (null, port.getValue());
}
else // multicast
{
entityDispatcher = EntityDispatcher.getEntityDispatcher (address.getValue(), port.getValue());
}
debug ("initialize: entityDispatcher instantiation complete");
debug ("turn on tracing in entityDispatcher if tracing turned on by VRML scene: " + traceJava.getValue());
if (traceJava.getValue()) entityDispatcher.setDEBUG (traceJava.getValue());
if(readInterval > 0l)
{
debug ("invoke entityDispatcher.adviseSleepInterval (" + readInterval/2 + ")");
entityDispatcher.adviseSleepInterval (readInterval/2);
}
else if (writeInterval > 0l)
{
debug ("invoke entityDispatcher.adviseSleepInterval (" + writeInterval/2 + ")");
entityDispatcher.adviseSleepInterval (writeInterval/2);
}
// Register ourselves with the entity dispatcher. We register by
// our entity ID.
entityIDObject = new EntityID((short)siteID.getValue(),
(short)applicationID.getValue(),
(short)entityID.getValue());
debug ("entityIDObject=" + entityIDObject);
debug ("Register ourselves using entityDispatcher.addListener to receive PDUs");
entityDispatcher.addListener(this, entityIDObject);
}
catch(Exception e)
{
e.printStackTrace();
System.out.println(e);
}
debug ("initialize () complete in EspduTransform");
}
/**
* receivePDU () is called by the entity dispatcher when a PDU for the node
* arrives from the network. The PDU is added to the list of PDUs that have already arrived,
* and is later sent up to the 3D scene graph when processEvent() is called by the VRML scene.
*/
public void receivePDU (ProtocolDataUnit pPDU)
{
debug ("receivePDU got a PDU from EntityDispatcher");
if (writeInterval > 0)
{
debug ("receivePDU () ignored since we are sending PDUs with this EntityID");
return;
}
synchronized(receivedPDUsList)
{
receivedPDUsList.addElement(pPDU);
}
return;
}
/**
* gets the entity ID, which is the unique triplet that identifies the entity,
* consisting of (site, application, entity) ID numbers. This is set in the VRML node, and
* retrieved for use here.
*/
public EntityID getEntityIDObject()
{
return entityIDObject;
}
/**
* resets the entity ID triplet = (site, application, entity) ID numbers.
*/
public void setEntityIDObject(EntityID pEIO)
{
entityIDObject = pEIO;
}
/**
* This is the main method, called by the VRML scene with periodicity
* readInterval (or writeInterval).
*/
public void processEvent(Event event)
{
ProtocolDataUnit nextPdu;
FirePdu firePdu;
DetonationPdu detonationPdu;
CollisionPdu collisionPdu;
float x, y, z, roll, pitch, yaw;
int espduIndex;
boolean standAloneMode = false;
try { // catchAllException to diagnose run-time writer errors while VRML browser continues
debug ("processEvent() start processing PDUs in EspduTransform...");
nextPdu = (ProtocolDataUnit) new EntityStatePdu ();
firePdu = new FirePdu ();
detonationPdu = new DetonationPdu ();
collisionPdu = new CollisionPdu ();
// debug ("start garbage collecting gc()...");
System.gc ();
// debug ("done garbage collecting gc()");
// do gc first for minimum latency between received PDUs and rendering later
// I'm not convinced this is useful, but what the hell. This suggests that
// now would be a good time to GC, but does not actually require it.
// debug ("begin check networkMode=" + networkMode + ", standAloneMode=" + standAloneMode);
// compareToIgnoreCase will crash older JVMs!!!
if ((new String (networkMode.getValue())).compareTo ("standAlone") == 0)
{
if (standAloneMode == false)
{
standAloneMode = true;
debug ("*** networkMode=standAlone, not processing any network events!");
}
// occurs before thread start, so shouldn't have any thread/buffer issues
return;
}
// debug ("check networkMode complete.");
// thread-start block - - - - - - - - - - - - - - - - - - - - -
if (entityDispatcherThreadStarted == false)
{
debug ("entityDispatcher new Thread...");
entityDispatcherThread = new Thread(entityDispatcher,
"DebuggingEntityDispatcher-" + ClassUtilities.nextSerialNum());
debug ("entityDispatcher thread starting...");
entityDispatcherThread.start();
entityDispatcherThreadStarted = true; // keep this after start() call!
debug (entityDispatcherThread.getName() + " thread started, startEntityDispatcherThread() complete");
}
// debug ("entityDispatcherThreadStarted = " + entityDispatcherThreadStarted);
if (entityDispatcherThreadStarted == false)
{
// only used if entityDispatcher not threaded (& thus start-ed) !
entityDispatcher.singleReadLoop ();
}
// write block - - - - - - - - - - - - - - - - - - - - - - - -
if (writeInterval > 0) // *** articulation parameters not yet included here....
{
debug ("writeInterval=" + writeInterval);
x = translation.getX ();
y = translation.getY ();
z = translation.getZ ();
geoCoords_changed.setValue (new Float(x).toString() + " " + new Float(y).toString() + " " + new Float(z).toString());
espdu.setEntityLocationX (x);
espdu.setEntityLocationY (y);
espdu.setEntityLocationZ (z);
espdu.setEntityLinearVelocityX (0.0f);
espdu.setEntityLinearVelocityY (0.0f);
espdu.setEntityLinearVelocityZ (0.0f);
espdu.setEntityLinearAcceleration (0.0f, 0.0f, 0.0f);
orientation = (SFRotation) transformNodeAccess.getExposedField ("rotation"); // get
orientation.getValue (rot);
quaternion.setAxisAngle (rot);
quaternion.getEulers (eulers);
// note Kent's quaternion code has irregular ordering of Euler angles
// which (by whatever method :) accomplishes the angle transformation
// desired... (needs to be verified using NPS AUV)
yaw = -eulers [0];
roll = eulers [1];
pitch = eulers [2];
// set roll, pitch, and yaw in the PDU
espdu.setEntityOrientationPsi (roll);
espdu.setEntityOrientationTheta (pitch);
espdu.setEntityOrientationPhi (yaw);
debug ("phi/theta/psi = " + roll * 180.0 / Math.PI + ", "
+ pitch * 180.0 / Math.PI + ", " + yaw * 180.0 / Math.PI + " degrees");
debug ("espdu marking = " + espdu.getMarking() ); // expected marking is prefixed in output statements
debug ("espdu " + espdu.getEntityID().toString());
entityDispatcher.sendPdu (espdu, address.getValue(), port.getValue());
debug ("entityDispatcher.sendPdu (...) complete");
}
// read block - - - - - - - - - - - - - - - - - - - - - - - -
else synchronized (receivedPDUsList)
{
debug ("process read events, readInterval=" + readInterval);
/*
* Since this code is called every time the browser wishes to redraw the
* screen we can use this as a data point to figure out the time per frame
*/
aveSecPerFrame.addDataPoint( secPerFrame.getDuration() );
secPerFrame.reset();
/*
* Dead reckon: calculate VRML position/orientation from DIS position/orientation
*/
if (secSinceLastPdu.getDuration() > 5.0f) previousEspdu = null;
if ((secSinceLastPdu.getDuration() > 0.01f) && (previousEspdu != null))
{
DRandSmooth();
position = new SFVec3f (positionArray[0], positionArray[1], positionArray[2]);
translation.setValue (position); // set VRML scene value
rotation.setValue (orientation); // set VRML scene value
// debug("position = (" + positionArray[0] + ", " + positionArray[1] + ", " + positionArray[2] + ")");
debug("position = " + position);
debug("rotation = " + orientation );
x = translation.getX ();
y = translation.getY ();
z = translation.getZ ();
geoCoords_changed.setValue (new Float(x).toString() + " " + new Float(y).toString() + " " + new Float(z).toString());
debug("geoCoords_changed = " + geoCoords_changed);
}
// put timers on reset events
if ((isActive.getValue() == true) && (secSinceLastPdu.getDuration() > 15.0f))
{
isActive.setValue (false);
trace ("no ESPDU heard for 15 seconds, isActive=false!");
}
// if ( isCollided.getValue ()) isCollided.setValue (false); // reset scene
if (fired1.getValue ())
debug ("fired1.getValue ()=" + (new Boolean(fired1.getValue ())).toString() +
", localFireTimer1.getDuration()=" + localFireTimer1.toString());
if (fired2.getValue ())
debug ("fired2.getValue ()=" + (new Boolean(fired2.getValue ())).toString() +
", localFireTimer2.getDuration()=" + localFireTimer2.toString());
if ((fired1.getValue () == true) && (localFireTimer1.getDuration() >= 5.0f)) // seconds
{
trace ("fired1 timeout: " + localFireTimer1.getDuration() + "sec");
fired1.setValue(false); // notify scene done (but currently causes another explosion)
debug ("fired1.setValue(false);");
munitionStartPoint.setValue (0.0f, 0.0f, 0.0f);
munitionEndPoint.setValue (0.0f, 0.0f, 0.0f);
debug ("munitionStartPoint, munitionEndPoint reset to (0 0 0)");
}
if ((fired2.getValue () == true) && (localFireTimer2.getDuration() >= 5.0f)) // seconds
{
trace ("fired2 timeout: " + localFireTimer2.getDuration() + "sec");
fired2.setValue(false); // notify scene done (but currently causes another explosion)
debug ("fired2.setValue(false);");
munitionStartPoint.setValue (0.0f, 0.0f, 0.0f);
munitionEndPoint.setValue (0.0f, 0.0f, 0.0f);
debug ("munitionStartPoint, munitionEndPoint reset to (0 0 0)");
}
pduVectorSize = receivedPDUsList.size ();
pduCount += pduVectorSize;
if (pduVectorSize < 0)
{
trace ("error! pduVectorSize < 0, received 0 PDUs [" + pduCount + " total]");
return;
}
else if (pduVectorSize == 0)
{
debug ("received 0 PDUs [" + pduCount + " total]");
return;
}
else
{
debug ("received " + pduVectorSize + " PDUs [" + pduCount + " total]");
}
// process all received PDUs
// find the most recently timestamped ESPDU, process Collision and Fire
espduIndex = -1;
for (int i = 0; i < pduVectorSize; i++)
{
nextPdu = (ProtocolDataUnit) receivedPDUsList.elementAt (i);
// debug (" :) got nextPdu");
// debug (" ((ProtocolDataUnit) receivedPDUsList.elementAt(i)).getPduType()=" + ((ProtocolDataUnit) receivedPDUsList.elementAt (i)).getPduType());
// debug (" ((ProtocolDataUnit) receivedPDUsList.elementAt(i)).getPduType().shortValue()=" + ((ProtocolDataUnit) receivedPDUsList.elementAt (i)).getPduType().shortValue());
if (nextPdu.getRtpHeaderEnabled() != rtpHeaderExpected.getValue())
{
debug ("RTP headers " + nextPdu.getRtpHeaderEnabled() + " rather than " +
rtpHeaderExpected + " as expected!");
isRtpHeaderHeard.setValue(nextPdu.getRtpHeaderEnabled()); // send eventOut
rtpHeaderExpected.setValue(nextPdu.getRtpHeaderEnabled()); // remember
debug ("unexpected RTP header status, rtpHeader = " + nextPdu.getRtpHeaderEnabled());
}
if (nextPdu.getPduType().shortValue() == PduTypeField.ENTITYSTATE)
{
if (espduIndex == -1) // first espdu found
{
espduIndex = i;
pdu = (ProtocolDataUnit) receivedPDUsList.elementAt (i);
debug ("-- ESPDU of interest is now element " + i + " of receivedPDUsList");
}
else if (pdu.getTimestamp().longValue() < nextPdu.getTimestamp().longValue())
{
espduIndex = i;
pdu = (ProtocolDataUnit) receivedPDUsList.elementAt (i);
debug ("-- ESPDU of interest is now element " + i + " of receivedPDUsList");
}
else debug ("-- ESPDU at element " + i + " of receivedPDUsList is less recent than pdu, ignored");
}
else if (nextPdu.getPduType().shortValue() == PduTypeField.FIRE) // ** need intersection check? **
{
debug (">> FirePdu detected...");
firePdu = (FirePdu) receivedPDUsList.elementAt (i);
// make decision based on munition, fired1 or fired2 ***
//*******Quay's decision code**************
if ((int)WarheadField.HEDARTS == firePdu.getBurstDescriptor().getWarhead().intValue())
{
trace ("firePdu: fired2, found WarheadField.HEDARTS == " + WarheadField.HEDARTS);
fired2.setValue(true);
localFireTimer2.reset();
}
else{
fired1.setValue(true);
localFireTimer1.reset();
} // notify scene
//*******ends Quay's changes*******
// notify scene of time, but use local clock so explosion sounds
firedTime.setValue (firePdu.getVRMLTimestamp()); // event.getTimeStamp() didn't work :(
float fireRange = firePdu.getRange();
if (fireRange <= 10.0f)
{
debug ("fireRange = " + fireRange + ", reset to 10m for visibility");
fireRange = 10.0f; // meters
}
trace ("fired! fireRange=" + fireRange);
debug ("firedTime=" + firedTime.getValue());
// include conversion from DIS-to-VRML world coordinates (VRML Y gets DIS -Z, VRML Z gets DIS Y)
// need to add time of flight & munition handling to these calculations
float launchX, launchY, launchZ, launchVelocityX, launchVelocityY, launchVelocityZ,
targetX, targetY, targetZ = 0.0f;
if (firePdu.getLocationInWorldCoordinate() != null)
{
launchX = (new Double ((firePdu.getLocationInWorldCoordinate()).getX()).floatValue()); // VRML coordinate system
launchY = -(new Double ((firePdu.getLocationInWorldCoordinate()).getZ()).floatValue());
launchZ = (new Double ((firePdu.getLocationInWorldCoordinate()).getY()).floatValue());
// need proper calculations for actual aim point ***
// include conversion from DIS-to-VRML world coordinates (VRML Y gets DIS -Z, VRML Z gets DIS Y)
launchVelocityX = firePdu.getVelocity().getX(); // VRML coordinate system
launchVelocityY = - firePdu.getVelocity().getZ();
launchVelocityZ = firePdu.getVelocity().getY();
double horizontalVelocity = Math.sqrt (launchVelocityX*launchVelocityX + launchVelocityZ*launchVelocityZ);
if (horizontalVelocity == 0.0) horizontalVelocity = 0.0001; // avoid divide by zero exception
double psi = Math.atan2(launchVelocityX, launchVelocityZ); // VRML coordinate system, horizontal plane
double theta = Math.atan (launchVelocityY / horizontalVelocity); // VRML coordinate system, vertical plane
debug ("(VRML launchVelocityX=" + launchVelocityX + ", launchVelocityY=" + launchVelocityY + ")");
debug (" launchVelocityZ=" + launchVelocityZ + ", theta =" + theta * Math.PI / 180.0 + " degrees, psi = "
+ psi * Math.PI / 180.0 + " degrees)");
targetX = launchX + (float) (fireRange * Math.cos (theta) * Math.sin(psi));
targetY = launchY + (float) (fireRange * Math.sin (theta));
targetZ = launchZ + (float) (fireRange * Math.cos (theta) * Math.cos(psi));
debug ("calculations complete, setting (crash-prone) munitionStartPoint, munitionEndPoint ...");
munitionStartPoint.setValue (launchX, launchY, launchZ); // crash!!
munitionEndPoint.setValue (targetX, targetY, targetZ); // crash!!
debug ("munitionStartPoint set to (" + launchX + ", " + launchY + ", " + launchZ + ")");
debug (" munitionEndPoint set to (" + targetX + ", " + targetY + ", " + targetZ + ")");
}
else trace ("*** error, firePdu.getLocationInWorldCoordinate() == null");
}
else if (nextPdu.getPduType().shortValue() == PduTypeField.DETONATION)
{
trace ("<< DetonationPdu detected...");
detonationPdu = (DetonationPdu) receivedPDUsList.elementAt (i);
isDetonated.setValue (true); // notify scene
detonateTime.setValue (new SFTime (detonationPdu.getVRMLTimestamp())); // notify scene
trace ("isDetonated=true! timeStamp " + detonateTime.getValue());
}
else if (nextPdu.getPduType().shortValue() == PduTypeField.COLLISION)
{
trace ("<< CollisionPdu detected...");
collisionPdu = (CollisionPdu) receivedPDUsList.elementAt (i);
isCollided.setValue (true); // notify scene
collideTime.setValue (new SFTime (collisionPdu.getVRMLTimestamp())); // notify scene
trace ("isCollided=true! timeStamp " + collideTime.getValue());
}
else // unusable PDU type
{
trace ("not a usable ESPDU, element " + i + " of receivedPDUsList is type " +
nextPdu.getPduType() + " = " +
PduTypeField.toString (nextPdu.getPduType().intValue()));
// trace ("see dis-java-vrml/docs/dis-java-vrml/mil/navy/nps/disEnumerations/PduTypeField.html");
}
}
if (espduIndex == -1) // no ESPDUs received
{
debug ("have no ESPDUs");
}
else // process the most current ESPDU which is 'pdu'
{
debug ("have one or more ESPDUs");
if (isActive.getValue() == false)
{
isActive.setValue (true);
trace ("espdu(s) received, isActive=true! RTP headers heard: " + nextPdu.getRtpHeaderEnabled());
// DEBUG=true;
}
prev_dt = secSinceLastPdu.getDuration() - secPerFrame.getDuration();
// allow smoothing until the DR and smoothing results are closeEnough.
closeEnough = false;
// this is how long the espdu has been used (i.e. time between pdu updates)
averageUpdateTime.addDataPoint( secSinceLastPdu.getDuration() );
if (pdu == null) trace ("pdu lost scope!!");
espdu = (EntityStatePdu) pdu;
timestamp.setValue(new SFTime (espdu.getVRMLTimestamp())); // send eventOut
// save the utilized previousEspdu for smoothing.
// need partial deep copy to get DR values and eliminate multiple reference count
previousEspdu = new EntityStatePdu ();
previousEspdu.setDeadReckoningAlgorithm (espdu.getDeadReckoningAlgorithm ());
previousEspdu.setDeadReckoningParameters (espdu.getDeadReckoningParameters ());
previousEspdu.setEntityAngularVelocity (espdu.getEntityAngularVelocity ());
previousEspdu.setEntityLinearAcceleration (espdu.getEntityLinearAcceleration ());
previousEspdu.setEntityLinearVelocity (espdu.getEntityLinearVelocity ());
previousEspdu.setEntityLocation (espdu.getEntityLocation ());
previousEspdu.setEntityOrientation (espdu.getEntityOrientation ());
secSinceLastPdu.reset(); // reset local dead-reckoning clock since new pdu is now in use
// browser bug trap, espdu location at origin is unlikely
if ((espdu.getEntityLocationX() == 0.0) &&
(espdu.getEntityLocationY() == 0.0) &&
(espdu.getEntityLocationZ() == 0.0))
trace ("warning! suspicious ESPDU location <0, 0, 0> received");
else debug ("ESPDU-at-origin bug trap complete.");
/*
int APCount = espdu.articulationParameterCount().intValue();
if (APCount != articulationParameterCount.getValue())
articulationParameterCount.setValue(APCount); // send eventOut
if (APCount > MAX_ARTICULATION_PARAMETERS)
{
trace ( APCount +
" articulation parameters received, maximum supported is " +
MAX_ARTICULATION_PARAMETERS);
}
if (APCount > 0) changeIndicator0 = espdu.getArticulationParameterAt(0).getChangeIndicator().intValue();
if (APCount > 1) changeIndicator1 = espdu.getArticulationParameterAt(1).getChangeIndicator().intValue();
if (APCount > 2) changeIndicator2 = espdu.getArticulationParameterAt(2).getChangeIndicator().intValue();
if (APCount > 3) changeIndicator3 = espdu.getArticulationParameterAt(3).getChangeIndicator().intValue();
if (APCount > 4) changeIndicator4 = espdu.getArticulationParameterAt(4).getChangeIndicator().intValue();
if (APCount > 5) changeIndicator5 = espdu.getArticulationParameterAt(5).getChangeIndicator().intValue();
if (APCount > 6) changeIndicator6 = espdu.getArticulationParameterAt(6).getChangeIndicator().intValue();
if (APCount > 7) changeIndicator7 = espdu.getArticulationParameterAt(7).getChangeIndicator().intValue();
if (APCount > 8) changeIndicator8 = espdu.getArticulationParameterAt(8).getChangeIndicator().intValue();
if (APCount > 9) changeIndicator9 = espdu.getArticulationParameterAt(9).getChangeIndicator().intValue();
if (APCount >10) changeIndicator10 = espdu.getArticulationParameterAt(10).getChangeIndicator().intValue();
if (APCount >11) changeIndicator11 = espdu.getArticulationParameterAt(11).getChangeIndicator().intValue();
if (APCount >12) changeIndicator12 = espdu.getArticulationParameterAt(12).getChangeIndicator().intValue();
if (APCount >13) changeIndicator13 = espdu.getArticulationParameterAt(13).getChangeIndicator().intValue();
if (APCount >14) changeIndicator14 = espdu.getArticulationParameterAt(14).getChangeIndicator().intValue();
// for articulationParameterValues, note that doubles are passed over the wire,
// but they must be downcasted to floats so VRML can accept them :(
if (APCount > 0)
{
// the following if statements check for validity and then whether a change has occurred from prior value,
// prior to accessing ArticulationParameter member values and then sending (via assignment) eventOuts
if ((APCount > 0) && ( changeIndicator0 != previousChangeIndicator0)) articulationParameterValue0.setValue ((float)espdu.getArticulationParameterAt(0).getParameterValue());
if ((APCount > 1) && ( changeIndicator1 != previousChangeIndicator1)) articulationParameterValue1.setValue ((float)espdu.getArticulationParameterAt(1).getParameterValue());
if ((APCount > 2) && ( changeIndicator2 != previousChangeIndicator2)) articulationParameterValue2.setValue ((float)espdu.getArticulationParameterAt(2).getParameterValue());
if ((APCount > 3) && ( changeIndicator3 != previousChangeIndicator3)) articulationParameterValue3.setValue ((float)espdu.getArticulationParameterAt(3).getParameterValue());
if ((APCount > 4) && ( changeIndicator4 != previousChangeIndicator4)) articulationParameterValue4.setValue ((float)espdu.getArticulationParameterAt(4).getParameterValue());
if ((APCount > 5) && ( changeIndicator5 != previousChangeIndicator5)) articulationParameterValue5.setValue ((float)espdu.getArticulationParameterAt(5).getParameterValue());
if ((APCount > 6) && ( changeIndicator6 != previousChangeIndicator6)) articulationParameterValue6.setValue ((float)espdu.getArticulationParameterAt(6).getParameterValue());
if ((APCount > 7) && ( changeIndicator7 != previousChangeIndicator7)) articulationParameterValue7.setValue ((float)espdu.getArticulationParameterAt(7).getParameterValue());
if ((APCount > 8) && ( changeIndicator8 != previousChangeIndicator8)) articulationParameterValue8.setValue ((float)espdu.getArticulationParameterAt(8).getParameterValue());
if ((APCount > 9) && ( changeIndicator9 != previousChangeIndicator9)) articulationParameterValue9.setValue ((float)espdu.getArticulationParameterAt(9).getParameterValue());
if ((APCount >10) && (changeIndicator10 != previousChangeIndicator10)) articulationParameterValue10.setValue ((float)espdu.getArticulationParameterAt(10).getParameterValue());
if ((APCount >11) && (changeIndicator11 != previousChangeIndicator11)) articulationParameterValue11.setValue ((float)espdu.getArticulationParameterAt(11).getParameterValue());
if ((APCount >12) && (changeIndicator12 != previousChangeIndicator12)) articulationParameterValue12.setValue ((float)espdu.getArticulationParameterAt(12).getParameterValue());
if ((APCount >13) && (changeIndicator13 != previousChangeIndicator13)) articulationParameterValue13.setValue ((float)espdu.getArticulationParameterAt(13).getParameterValue());
if ((APCount >14) && (changeIndicator13 != previousChangeIndicator14)) articulationParameterValue14.setValue ((float)espdu.getArticulationParameterAt(14).getParameterValue());
}
previousChangeIndicator0 = changeIndicator0;
previousChangeIndicator1 = changeIndicator1;
previousChangeIndicator2 = changeIndicator2;
previousChangeIndicator3 = changeIndicator3;
previousChangeIndicator4 = changeIndicator4;
previousChangeIndicator5 = changeIndicator5;
previousChangeIndicator6 = changeIndicator6;
previousChangeIndicator7 = changeIndicator7;
previousChangeIndicator8 = changeIndicator8;
previousChangeIndicator9 = changeIndicator9;
previousChangeIndicator10 = changeIndicator10;
previousChangeIndicator11 = changeIndicator11;
previousChangeIndicator12 = changeIndicator12;
previousChangeIndicator13 = changeIndicator13;
previousChangeIndicator14 = changeIndicator14;
*/
if (espduFirstHeard == true)
{
espduFirstHeard = false;
trace ("espduFirstHeard, marking = " + espdu.getMarking ());
}
// crash!! if ((espduFirstHeard == true) && espdu.getMarking().compareToIgnoreCase (marking.getValue()) != 0)
// {
// trace ("warning! VRML scene marking (" + marking + ") differs from ESPDU marking (" + ")" );
// espduFirstHeard = false;
// }
}
debug ("starting receivedPDUsList.removeAllElements ()...");
receivedPDUsList.removeAllElements (); // garbage collection will reclaim this Vector
debug ("finished receivedPDUsList.removeAllElements ()");
pdu = null; nextPdu = null; firePdu = null; collisionPdu = null; // dereference list PDUs
debug ("receivedPDUsList.removeAllElements (); isEmpty() = " + receivedPDUsList.isEmpty ());
if (!receivedPDUsList.isEmpty ()) trace ("receivedPDUsList.removeAllElements (); failed!");
} // end synchronized (receivedPDUsList)
}
catch (Exception catchAllException)
{
trace ("*** processEvent exception: " + catchAllException);
catchAllException.printStackTrace();
// setDEBUG (true);
// can't re-throw (catchAllException); since not supported by class vrml.node.Script
if (!receivedPDUsList.isEmpty ())
{
trace ("*** Exception occurred before clearing receivedPDUsList, retrying...");
debug ("starting receivedPDUsList.removeAllElements ()...");
receivedPDUsList.removeAllElements (); // garbage collection will reclaim this Vector
debug ("finished receivedPDUsList.removeAllElements ()");
pdu = null; nextPdu = null; firePdu = null; collisionPdu = null; // dereference list PDUs
debug ("receivedPDUsList.removeAllElements (); isEmpty() = " + receivedPDUsList.isEmpty ());
if (!receivedPDUsList.isEmpty ()) trace ("receivedPDUsList.removeAllElements (); failed!");
}
}
debug ("finished processEvent");
return;
}
/** Smooths data
*/
private float[] smooth3Floats( float[] drCurrent, float[] drPrevUpdate,
float currentTime, float averageUpdateTime )
{
float[] result = new float[3];
// the ratio of how long since the most recent update time to the averageUpdateTime
float factor = currentTime / averageUpdateTime;
result[0] = drPrevUpdate[0] + ( drCurrent[0] - drPrevUpdate[0] ) * factor;
result[1] = drPrevUpdate[1] + ( drCurrent[1] - drPrevUpdate[1] ) * factor;
result[2] = drPrevUpdate[2] + ( drCurrent[2] - drPrevUpdate[2] ) * factor;
return result;
}
/**
* Calculate the deadreckon position of a EntityStatePDU
*/
private float[] DRPosition ( EntityStatePdu espdu, float dt )
{
float[] dRposition = new float[3];
if( dt < DR_LIMIT )
{
dRposition[0] = (float) ( espdu.getEntityLocationX() +
dt * espdu.getEntityLinearVelocityX() +
dt*dt * espdu.getEntityLinearAccelerationX());
dRposition[1] = (float) (-espdu.getEntityLocationZ() -
dt * espdu.getEntityLinearVelocityZ() -
dt*dt * espdu.getEntityLinearAccelerationZ());
dRposition[2] = (float) ( espdu.getEntityLocationY() +
dt * espdu.getEntityLinearVelocityY() +
dt*dt * espdu.getEntityLinearAccelerationY());
} else
{
dRposition[0] = (float) ( espdu.getEntityLocationX());
dRposition[1] = (float) (-espdu.getEntityLocationZ());
dRposition[2] = (float) ( espdu.getEntityLocationY());
}
return dRposition;
}
/*
* Calculate the deadreckon orientation of a EntityStatePDU
*/
private float[] DROrientation ( EntityStatePdu espdu, float dt )
{
float[] dRorientation = new float[3];
float yaw, pitch, roll;
if ( dt < DR_LIMIT ) // no angular acceleration in ESPDU
{
roll = espdu.getEntityOrientationPhi() + dt * espdu.getEntityAngularVelocityX();
pitch = espdu.getEntityOrientationTheta() + dt * espdu.getEntityAngularVelocityY();
yaw = espdu.getEntityOrientationPsi() + dt * espdu.getEntityAngularVelocityZ();
} else
{
roll = espdu.getEntityOrientationPhi();
pitch = espdu.getEntityOrientationTheta();
yaw = espdu.getEntityOrientationPsi();
}
// note Kent's quaternion code has irregular ordering of Euler angles
// which (by whatever method :) accomplishes the angle transformation
// desired... (results verified using NPS AUV)
dRorientation[0] = -yaw;
dRorientation[1] = roll;
dRorientation[2] = pitch;
return dRorientation;
}// end DROrientation()
/**
* Sum of the squares of the diffs between two float arrays.
*/
private float SqrDeltaFloats( float[] first, float[] second )
{
float sumOfSqrs = 0.0f;
for( int idx = 0; idx < first.length; idx++ )
{
sumOfSqrs += (first[idx] - second[idx]) * (first[idx] - second[idx]);
}
return sumOfSqrs;
}
/**
* Dead reckon and smooth if appropriate.
*/
private void DRandSmooth()
{
if (espdu == null) // trap error condition
{
trace ("DRandSmooth: espdu == null !");
return;
}
if (previousEspdu == null) // trap error condition
{
trace ("DRandSmooth: previousEspdu == null !");
return;
}
float aveUpdateTime = averageUpdateTime.average();
float stopSmoothTime = aveUpdateTime * SMOOTH_LIMIT;
float updateTime = secSinceLastPdu.getDuration();
// calculate the DR Position and Orientation
float[] tempPositionArray = DRPosition( espdu, updateTime );
float[] goalOrientation = DROrientation( espdu, updateTime );
float[] currOrientation; // only used when smoothing.
float[] smoothedLinearVel;
if( DoNotSmooth || closeEnough || updateTime > aveUpdateTime )
{ // just DR since we have already rejoined the new DR track.
positionArray = tempPositionArray;
eulers = goalOrientation;
} else
{
// DR and smooth position
if( SMOOTH_LINVELOCITY )
{
smoothedLinearVel = smoothLinVelocity (espdu, previousEspdu, updateTime, aveUpdateTime);
tempPositionArray = DRPosition (espdu, smoothedLinearVel, updateTime);
positionArray = smooth3Floats (tempPositionArray,
DRPosition (previousEspdu,smoothedLinearVel, prev_dt + updateTime),
updateTime, aveUpdateTime * SMOOTH_LIMIT);
}
else
{
positionArray = smooth3Floats (tempPositionArray,
DRPosition (previousEspdu, prev_dt + updateTime),
updateTime, aveUpdateTime * SMOOTH_LIMIT);
}
currOrientation = DROrientation (previousEspdu, prev_dt + updateTime);
goalOrientation = fixEulers( goalOrientation , currOrientation );
// goalOrientation should now contain values such that the entity will
// head the short way around the circle.
eulers = smooth3Floats( goalOrientation, currOrientation,
updateTime, aveUpdateTime * SMOOTH_LIMIT );
if( SqrDeltaFloats (tempPositionArray, positionArray ) < 0.02 &&
SqrDeltaFloats (tempPositionArray, positionArray ) < 0.02 &&
SqrDeltaFloats (goalOrientation, currOrientation ) < 0.02 )
{
// stop smoothing. Only use DR calculations until next pdu arrives.
closeEnough = true;
}
}
quaternion.setEulers (eulers);
quaternion.getAxisAngle (rot);
orientation.setValue (rot);
} // END: Dead reckon and smooth
private float normalize(float input_angle)
{
float angle = input_angle;
float twoPI = (float)Math.PI * 2.0f;
while( angle < 0.0f )
{
angle += twoPI;
}
while( angle >= twoPI )
{
angle -= twoPI;
}
return angle;
}// end normalize
private float[] normalize( float[] inputAngles )
{
float[] normal = new float[inputAngles.length];
for( int idx = 0; idx < inputAngles.length; idx++ )
{
normal[idx] = normalize2( inputAngles[idx] );
}
return normal;
} //end normalize float array => ( 0, 2PI ]
private float[] normalize2( float[] inputAngles )
{
float[] normal = new float[inputAngles.length];
for( int idx = 0; idx < inputAngles.length; idx++ )
{
normal[idx] = normalize2( inputAngles[idx] );
}
return normal;
} //end normalize2 float array => ( -PI, PI ]
private float normalize2( float input_angle )
{
float angle = input_angle;
float twoPI = (float)Math.PI * 2.0f;
while( angle > Math.PI )
{
angle -= twoPI;
}
while(angle <= -Math.PI )
{
angle += twoPI;
}
return angle;
}// end normalize2 => ( -PI, PI ]
// returns: the magnitude and direction of the angle change.
private float[] fixEulers( float[] goalOrientation, float[] currOrientation )
{
float[] goalOrien = goalOrientation;
for( int idx = 0; idx < goalOrientation.length; idx++ )
{
goalOrien[idx] = currOrientation[idx] +
normalize2( normalize2( goalOrien[idx] ) - normalize2( currOrientation[idx] ) );
}
return goalOrien;
}
/**
* Calculate the dead reckoning position of a EntityStatePDU
*/
private float[] DRPosition ( EntityStatePdu espdu, float[] smoothedVelocity, float dt )
{
float[] dRposition = new float[3];
if( dt < DR_LIMIT )
{
dRposition[0] = (float) ( espdu.getEntityLocationX() + dt * smoothedVelocity[0]);
dRposition[1] = (float) (-espdu.getEntityLocationZ() - dt * smoothedVelocity[1]);
dRposition[2] = (float) ( espdu.getEntityLocationY() + dt * smoothedVelocity[2]);
}
else
{
dRposition[0] = (float) ( espdu.getEntityLocationX());
dRposition[1] = (float) (-espdu.getEntityLocationZ());
dRposition[2] = (float) ( espdu.getEntityLocationY());
}
return dRposition;
}
private float[] smoothLinVelocity( EntityStatePdu espdu, EntityStatePdu previousEspdu,
float currentTime, float averageUpdateTime )
{
float[] ELV = new float[3];
float[] prevELV = new float[3];
ELV[0] = espdu.getEntityLinearVelocityX();
ELV[1] = espdu.getEntityLinearVelocityZ();
ELV[2] = espdu.getEntityLinearVelocityY();
prevELV[0] = previousEspdu.getEntityLinearVelocityX();
prevELV[1] = previousEspdu.getEntityLinearVelocityZ();
prevELV[2] = previousEspdu.getEntityLinearVelocityY();
return smooth3Floats( ELV, prevELV, currentTime, averageUpdateTime );
}// end smoothVelocity
private float[] smoothAngVelocity( EntityStatePdu espdu, EntityStatePdu previousEspdu,
float currentTime, float averageUpdateTime )
{
float[] EAV = new float[3];
float[] prevEAV = new float[3];
EAV[0] = espdu.getEntityAngularVelocityX();
EAV[1] = espdu.getEntityAngularVelocityY();
EAV[2] = espdu.getEntityAngularVelocityZ();
prevEAV[0] = previousEspdu.getEntityAngularVelocityX();
prevEAV[1] = previousEspdu.getEntityAngularVelocityY();
prevEAV[2] = previousEspdu.getEntityAngularVelocityZ();
return smooth3Floats( EAV, prevEAV, currentTime, averageUpdateTime );
} // end smoothVelocity
} //end class