* Class HumanActionInterpreterTeam * @author - Thomas E. Miller * @version 1.0 * Last Modification 20 September 2000 * The HumanActionInterpreterTeam.java file contains the logic that supports * and interprets user input from the HumanPanel.java. This file also * supports the aggregation and disaggregation of an independent human entity * to a group, to perform coordinated, tactical actions. *
* * DIS VRML * * -Z entity up Y entity up * ^ ^ * | | * | | * | | * +------> X +-------> X nose of entity body ( North ) * / / * / / * / / * Y Z * right-hand side * of entity body ( East ) * * Rotation angle axes (right-hand rule applies): * * DIS VRML Angle of rotation * * Roll X X phi * Pitch Y Z theta * Yaw Z -Y psi **/ public class HumanActionInterpreterTeam extends Thread { /** * pduSendInterval time delay between PDUs in seconds */ private float pduSendInterval; /** * speed represents the magnitude of linear velocity in meters per second */ private float speed; /** * df - provides output format for floating point numbers */ private DecimalFormat df; /** * teamEspdu - data structure to hold team state to the VRML scene */ private EntityStatePdu teamEspdu; /** * espdu - Pdu to send entity state to VRML scene */ private EntityStatePdu espdu; /** * truePositionEspdu - data structure to hold the entity's true position * state */ private EntityStatePdu truePositionEspdu; /** * mountedEspdu - data structure to hold the aggregation entity's state */ private EntityStatePdu mountedEspdu; /** * fpdu - Pdu to send fire info to VRML scene */ private FirePdu fpdu; /** * copdu - PDU to send collision info to the VRML scene */ private CollisionPdu copdu; /** * BehaviorStreamBufferUDP - tie in to the network */ private BehaviorStreamBufferUDP behaviorStreamBuffer; /** * dpdu - PDU to send detonation info to the VRML scene */ private DetonationPdu dpdu; /** * cepdu - PDU to send simulation information, create entity */ private CreateEntityPdu cepdu; /** * cpdu - PDU to send information about the entity */ private CommentPdu cpdu; /** * repdu - PDU to send simulation information, remove entity */ private RemoveEntityPdu repdu; /** * datum */ private DatumSpecification datum; /** * variableDatum */ private VariableDatum variableDatum; /** * geometryURL - URL where geometry is located for the entity */ private String geometryURL; private byte geometryURLbyteArray[]; /** * geometryURLlongArray - - URL where geometry is stored in a long array * for the entity */ private long geometryURLlongArray[]; /** * ipAddress - the IP address the simulation */ private String ipAddress; /** * portNumber - port number for IP multicast */ private int portNumber; /** * id - id marking of the entity */ private int id; /** * isCollided - collision state of the entity */ private boolean isCollided; /** * mypanel - the human panel which feeds user input to this class */ private HumanPanel mypanel; /** * behaviorCode - Ennumeration of the individual behaviors, which are * sent to the VRML scene via articulated parameters */ private double behaviorCode; /** * mountedCode - the mounted state of the entity */ private double mountedCode; /** * ApBehavior - articulated parameter holding the behavior code */ private ArticulationParameter ApBehavior; /** * ApMounted - articulated parameter holding the mounted state */ private ArticulationParameter ApMounted; /** * red - boolean holding the entitiy's team designation */ private boolean red; /** * alive - boolean showing whether or not the entity is active */ private boolean alive; /** * terrain - an instance of the CollisionDetectionTerrainReader class. * Used to determine an entity's height and orientation in the VRML scene. */ private CollisionDetectionTerrainReader terrain; /** * oldHeading - previous heading of the entity */ private double oldHeading; /** * newHeading - the most current heading of the entity */ private double newHeading; /** * first - first time thru the run cycle for panel updates */ private boolean first; // first time thru the run cycle for panel updates /** * distance - generally used for holding the distance from a goal position */ private double distance; /** * angle - an intercept angle to a goal position */ private double angle; /** * loaded - boolean to hold whether or not the entity has loaded on a * helicopter or not */ private boolean loaded = false; /** * exited - boolean to hold whether or not the entity has exited the * helicopter */ private boolean exited = false; /** * firstUnload - first time through the unload method */ private boolean firstUnload = true; /** * verticalOffset - Meters above local terrain for entity's geometric center. */ public double verticalOffset = 0.0; /** * NOTHING. Behavior code - enumeration of the individual behaviors. */ public final double NOTHING = 0.0; /** * STAND. Behavior code. */ public final double STAND = 1.0; /** * WALK. Behavior code. */ public final double WALK = 2.0; /** * RUN. Behavior code. */ public final double RUN = 3.0; /** * JUMP. Behavior code. */ public final double JUMP = 4.0; /** * KNEEL. Behavior code. */ public final double KNEEL = 5.0; /** * MOUNTED. Mounted code. */ public final double MOUNTED = 0.0; /** * UNMOUNTED. Mounted code. */ public final double UNMOUNTED = 1.0; /** * movementOrder - Ennumeration of a group action command */ private int movementOrder; /** * COLUMN. movementOrder. */ public final int COLUMN = 0; /** * LINE. movementOrder. */ public final int LINE = 1; /** * WEDGE. movementOrder. */ public final int WEDGE = 2; /** * MOUNT_HELO. movementOrder. */ public final int MOUNT_HELO = 3; /** * UNMOUNT_HELO. movementOrder. */ public final int UNMOUNT_HELO = 4; /** * HOLD. movementOrder. */ public final int HOLD = 5; /** * NONE. movementOrder. */ public final int NONE = 6; //============================================================================================= // Constructors & Start //=================================================================================== /** * Constructor * @param String ipAddr - a string defing the IP address * @param int portNum - the port number receiving the network packets * @param int timeToLive - Number of routing hops allowed * @param short siteNum - site Number of simualtion * @param short appNum - application number of the simulation * @param short idNum - entiy's id number * @param String marking - entity's text name * @param boolean rtpHeaderEnabled - wheteher or not the real-time header * is enabled or not * @param HumanPanel panel - the human control panel * @param boolean independent - the entity's mounted state * @param BehaviorStreamBufferUDP bsb * @param CollisionDetectionTerrainReader ter - instance of CollisionDetection * TerrainReader for the Fort Irwin terrain database * */ public HumanActionInterpreterTeam ( String ipAddr, int portNum, int timeToLive, short siteNum, short appNum, short idNum, String marking, boolean rtpHeaderEnabled, HumanPanel panel, boolean independent, BehaviorStreamBufferUDP bsb, CollisionDetectionTerrainReader ter ) { behaviorStreamBuffer = bsb; terrain = ter; // don't re-instantiate a BehaviorStreamBuffer if part of TeamPanel!! if (( independent ) || ( behaviorStreamBuffer == null )) { behaviorStreamBuffer = new BehaviorStreamBufferUDP ( ipAddress, portNumber ); behaviorStreamBuffer.setTimeToLive ( timeToLive ); Thread pduReader = new Thread ( behaviorStreamBuffer ); pduReader.start (); } // end if if (independent) { mountedCode = UNMOUNTED; } // end if else { mountedCode = MOUNTED; } // end else speed = 0.0f; // initially human no fwd speed pduSendInterval = 0.5f; // approx 500 ms between pdu sends df = new DecimalFormat ( "###.00" ); // instantiate an cepdu for announcing the creation of an entity cepdu = new CreateEntityPdu (); cepdu.setRtpHeaderEnabled ( rtpHeaderEnabled ); // instantiate an cpdu, used to store the URL address cpdu = new CommentPdu (); cpdu.setRtpHeaderEnabled (rtpHeaderEnabled); // instantiate a repdu, used to remove an entity repdu = new RemoveEntityPdu (); repdu.setRtpHeaderEnabled ( rtpHeaderEnabled ); cepdu.setRequestID (( long ) id ); repdu.setRequestID (( long ) id ); datum = new DatumSpecification (); variableDatum = new VariableDatum (); geometryURLbyteArray = new byte [ 120 ]; geometryURLlongArray = new long [ 120 ]; // note generic geometry convention not yet established, this has // EspduTransform embedded... // place URL address here geometryURL = new String ( "http://www.web3D.org/WorkingGroups/vrtp/" + "dis-java-vrml/demo/helicopter/" + "NancyTeam.wrl" ); // instantiate four espdus, to be used as a data structure teamEspdu = new EntityStatePdu (); teamEspdu.setRtpHeaderEnabled ( rtpHeaderEnabled ); // maintains relative information on this entity if mounted true if not espdu = new EntityStatePdu (); espdu.setRtpHeaderEnabled ( rtpHeaderEnabled ); // maintains true information on this entity truePositionEspdu = new EntityStatePdu (); truePositionEspdu.setRtpHeaderEnabled ( rtpHeaderEnabled ); // espdu for aggregation entity mountedEspdu = new EntityStatePdu (); mountedEspdu.setRtpHeaderEnabled ( rtpHeaderEnabled ); fpdu = new FirePdu (); fpdu.setRtpHeaderEnabled ( rtpHeaderEnabled ); copdu = new CollisionPdu (); copdu.setRtpHeaderEnabled ( rtpHeaderEnabled ); dpdu = new DetonationPdu (); dpdu.setRtpHeaderEnabled ( rtpHeaderEnabled ); System.out.println ( "marking=" + marking + ", timeToLive=" + timeToLive ); espdu.setMarking ( marking ); truePositionEspdu.setMarking ( marking ); id = idNum; if ( id < 30 ) { red = false; } // end if else { red = true; } // end else if ( red == true ) { // create entity id triplet for espdu & fire pdu short [] RedentityID = new short [ 3 ]; RedentityID [ 0 ] = 0; RedentityID [ 1 ] = 1; RedentityID [ 2 ] = 98; // set entity id triplet into teamEspdu teamEspdu.setEntityID ( RedentityID [ 0 ], RedentityID [ 1 ], RedentityID [ 2 ]); if ( id > 400 ) { int newID = id / 10; int tester = (( id % 400 ) % 10 ); if ( tester == 1 ) { espdu.setEntityLocationX ( -10f + 5 ); espdu.setEntityLocationY ( 1990f + newID % 35 * 50 ); } // end if else if ( tester == 2 ) { espdu.setEntityLocationX ( -10f + 10 ); espdu.setEntityLocationY ( 1990f + 5 + newID % 35 * 50 ); } // end else if else if ( tester == 3 ) { espdu.setEntityLocationX ( -10f + 10 ); espdu.setEntityLocationY ( 1990f - 5 + newID % 35 * 50 ); } // end else if else if ( tester == 4 ) { espdu.setEntityLocationX ( -10f + 15 ); espdu.setEntityLocationY ( 1990f + 10 + newID % 35 * 50 ); } // end else if else if ( tester == 5 ) { espdu.setEntityLocationX ( -10f + 15 ); espdu.setEntityLocationY ( 1990f - 10 + newID % 35 * 50 ); } // end else if else {} // end else } // end if else { espdu.setEntityLocationX ( -10f ); espdu.setEntityLocationY ( 1990f + id % 35 * 50 ); } // end else if ( mountedCode == UNMOUNTED ) { espdu.setEntityLocationZ ( 36.3f - verticalOffset ); } // end if else { double [] normalHeight = new double [ 4 ]; terrain.getNormalHeight ( espdu.getEntityLocationX (), espdu.getEntityLocationY (), normalHeight ); espdu.setEntityLocationZ ( normalHeight [ 3 ] - verticalOffset ); } // end else espdu.setEntityOrientationPsi ( -6.7f + ( float ) Math.PI ); } // end if else { // create entity id triplet for espdu & fire pdu short [] BlueentityID = new short [ 3 ]; BlueentityID [ 0 ] = 0; BlueentityID [ 1 ] = 1; BlueentityID [ 2 ] = 99; // set entity id triplet into teamEspdu teamEspdu.setEntityID ( BlueentityID [ 0 ], BlueentityID [ 1 ], BlueentityID [ 2 ]); espdu.setEntityLocationX ( -5010f ); espdu.setEntityLocationY ( 1990f + id % 27 * 50 ); espdu.setEntityOrientationPsi ( -6.7f ); espdu.setEntityLocationZ ( 40.5f - verticalOffset ); } // end else truePositionEspdu.setEntityLocationX ( espdu.getEntityLocationX ()); truePositionEspdu.setEntityLocationY ( espdu.getEntityLocationY ()); truePositionEspdu.setEntityOrientationPsi ( espdu.getEntityOrientationPsi ()); truePositionEspdu.setEntityLocationZ ( espdu.getEntityLocationZ ()); short [] entityID = new short [ 3 ]; entityID [ 0 ] = siteNum; entityID [ 1 ] = appNum; entityID [ 2 ] = ( short ) id; // set entity id triplet into the espdu, fire pdu and collision pdu espdu.setEntityID ( entityID [ 0 ], entityID [ 1 ], entityID [ 2 ]); truePositionEspdu.setEntityID ( entityID [ 0 ], entityID [ 1 ], entityID [ 2 ]); fpdu.setFiringEntityID ( espdu.getEntityID ()); copdu.setIssuingEntityID ( espdu.getEntityID ()); geometryURLbyteArray = geometryURL.getBytes (); variableDatum.setVariableDatumID ( 1 ); variableDatum.setVariableDatumLength ( 120 ); for ( int counter = 0; counter < geometryURLbyteArray.length ; counter++ ) { geometryURLlongArray [ counter ] = ( long ) geometryURLbyteArray [ counter ]; } // end for variableDatum.setVariableDatumValue ( geometryURLlongArray ); // add URL address as variable datum value datum.addVariableDatum ( variableDatum ); cpdu.setDatumSpecification ( datum ); cpdu.setOriginatingEntityID ( espdu.getEntityID ()); cpdu.setReceivingEntityID ( teamEspdu.getEntityID ()); cpdu.setExemplar ( cpdu ); // set cpdu fields cepdu.setOriginatingEntityID ( espdu.getEntityID ()); cepdu.setReceivingEntityID ( teamEspdu.getEntityID ()); cepdu.setExemplar ( cepdu ); // set cepdu fields repdu.setOriginatingEntityID ( espdu.getEntityID ()); repdu.setReceivingEntityID ( teamEspdu.getEntityID ()); repdu.setExemplar ( repdu ); // set repdu fields EntityID temp = espdu.getEntityID (); oldHeading = newHeading = espdu.getEntityOrientationPsi (); first = true; ApBehavior = new ArticulationParameter(); behaviorCode = NOTHING; ApBehavior.setParameterValue( behaviorCode ); // espdu.addArticulationParameter(ApBehavior); truePositionEspdu.addArticulationParameter(ApBehavior); ApMounted = new ArticulationParameter(); ApMounted.setParameterValue( mountedCode ); espdu.addArticulationParameter(ApMounted); truePositionEspdu.addArticulationParameter(ApMounted); mypanel = panel; isCollided = false; ipAddress = ipAddr; portNumber = portNum; if ( mountedCode == MOUNTED ) { short [] mountedEntityID = new short [ 3 ]; mountedEntityID [ 0 ] = siteNum; mountedEntityID [ 1 ] = appNum; mountedEntityID [ 2 ] = ( short ) ( id / 10 ); mountedEspdu.setEntityID ( mountedEntityID [ 0 ], mountedEntityID [ 1 ], mountedEntityID [ 2 ]); collectMountedPdus (); espdu.setEntityLocationX ( espdu.getEntityLocationX () - mountedEspdu.getEntityLocationX ()); espdu.setEntityLocationY ( espdu.getEntityLocationY () - mountedEspdu.getEntityLocationY ()); espdu.setEntityLocationZ ( truePositionEspdu.getEntityLocationZ () - mountedEspdu.getEntityLocationZ ()); } // end if // send cepdu System.out.println ( "sending CreateEntity PDU..." ); behaviorStreamBuffer.sendPdu ( cepdu.getExemplar (), ipAddress, portNumber ); // send cpdu System.out.println ( "sending Comment PDU with URL..." ); behaviorStreamBuffer.sendPdu ( cpdu.getExemplar (), ipAddress, portNumber ); // read in Fort Irwin Terrain file if necessary if ( terrain == null) { terrain = new CollisionDetectionTerrainReader(); terrain.parseFile (); } // end if alive = true; movementOrder = NONE; } // end constructor /** * run - start sending espdus * calls startLiving () which is an infinite espdu sending loop * @param void * @return void */ public void run() { startLiving (); } // end run //============================================================================= // Properties //============================================================================= /** * setIsCollided - sets the isCollided state boolean * @param boolean setValue * @return void */ public void setIsCollided ( boolean setValue ) { isCollided = setValue; } // end setIsCollided /** * getIsCollided - returns the isCollided state boolean * @param void * @return boolean isCollided */ public boolean getIsCollided () { return isCollided; } // end getIsCollided /** * setLiving - makes the entity active * @param boolean setValue * @return void */ public void setLiving ( boolean setValue ) { alive = setValue; } // end setLiving /** * getLiving - returns the active state of the entity * @param void * @return boolean alive */ public boolean getLiving () { return alive; } // end getLiving /** * setSpeed - sets the speed of the entity from user input * Also determines the behavior code based off speed * @param float spd - the entity's speed in knots * @return void */ public void setSpeed ( float spd ) { speed = 0.5144f * spd; // convert from knots to meters/second espdu.setEntityLinearVelocityX (( float ) ( speed * Math.cos ( espdu.getEntityOrientationPsi ()))); espdu.setEntityLinearVelocityY (( float ) ( speed * Math.sin ( espdu.getEntityOrientationPsi ()))); if ( speed == 0 ) { behaviorCode = STAND; } // end if else if (( speed > 0 ) && ( speed <= 2.06f )) { behaviorCode = WALK; } // end else if else { behaviorCode = RUN; } // end else // Jump currently not implemented in the action interpreter ApBehavior.setParameterValue (( double ) behaviorCode ); } // end setSpeed() /** * getSpeed - returns the entity's speed * @param void * @return double speed - the magnitude of the entity's velocity */ public double getSpeed () { return speed; } // end getSpeed /** * setIndependence - sets the mounted state of the entity and provides * the correct true or relative position * @param boolean inSquad * @return double - an articulated parameter for an aggregation entity */ public double setIndependence ( boolean inSquad ) { if ( inSquad ) { collectMountedPdus(); mountedCode = MOUNTED; truePositionEspdu.setEntityLocationX ( espdu.getEntityLocationX ()); truePositionEspdu.setEntityLocationY ( espdu.getEntityLocationY ()); truePositionEspdu.setEntityLocationZ ( espdu.getEntityLocationZ ()); espdu.setEntityLocationX ( espdu.getEntityLocationX () - mountedEspdu.getEntityLocationX ()); espdu.setEntityLocationY ( espdu.getEntityLocationY () - mountedEspdu.getEntityLocationY ()); espdu.setEntityLocationZ ( espdu.getEntityLocationZ () - mountedEspdu.getEntityLocationZ ()); } // end if else { mountedCode = UNMOUNTED; espdu.setEntityLocationX ( truePositionEspdu.getEntityLocationX ()); espdu.setEntityLocationY ( truePositionEspdu.getEntityLocationY ()); espdu.setEntityLocationZ ( truePositionEspdu.getEntityLocationZ ()); mypanel.setFormation ( NONE ); } // end else ApMounted.setParameterValue ( mountedCode ); return ( mountedCode ); } // end setIndependence /** * setTurnAngle - sets the angle of turn of the entity based off * user inputs * @param float turnAngle * @return void */ public void setTurnAngle ( float turnAngle ) { espdu.setEntityAngularVelocityZ (( float ) ( 0.1058 * ( turnAngle * Math.PI / 180.0 ))); espdu.setEntityOrientationPsi ( espdu.getEntityOrientationPsi () + espdu.getEntityAngularVelocityZ () * pduSendInterval ); espdu.setEntityLinearVelocityX (( float ) ( speed * Math.cos ( espdu.getEntityOrientationPsi ()))); espdu.setEntityLinearVelocityY (( float ) ( speed * Math.sin ( espdu.getEntityOrientationPsi ()))); } // end setturnAngle() /** * getEspdu returns the vehicles current entity state protocol data unit * @param void * @return EntityStatePDU */ public EntityStatePdu getEspdu () { return espdu; } // end getEspdu /** * getMountedEspdu returns the aggregation entity's current entity * state protocol data unit * @param void * @return EntityStatePDU */ public EntityStatePdu getMountedEspdu () { return mountedEspdu; } // end getMountedEspdu //============================================================================= // Utility Methods //============================================================================= /** * startLiving loops indefinitely sending espdus every 500 ms or so. * This loop runs on its own thread. Determines the entity's correct * positional and velocity states in either the relative or true * coordinate systems * @param void * @return void */ public void startLiving () { // Algorithm if human is mounted to an aggregation entity: // 1) Move Nancy as if she were independent // 2) Determine new relative vector from nancy to // aggregation element // 3) Move aggregation Element // 4) Add relative vector to new aggreagtion position // loops until mother thread exits thus killing this thread while ( alive ) { // simple kinematics to update X, Y, Z location in // world coordinates double rotatedPitchX; double rotatedPitchY; double rotatedPitchZ; double rotatedRollX; double rotatedRollY; double rotatedRollZ; double [] normalHeight = new double [4]; float thetaAngle; float phiAngle; if ( mountedCode == MOUNTED ) { espdu.setEntityLocationX ( truePositionEspdu.getEntityLocationX ()); espdu.setEntityLocationY ( truePositionEspdu.getEntityLocationY ()); espdu.setEntityOrientationPsi ( truePositionEspdu.getEntityOrientationPsi ()); } // end if if ( movementOrder == COLUMN ) { column (); } // end if if ( movementOrder == LINE ) { line (); } // end if if ( movementOrder == WEDGE ) { wedge (); } // end if if (movementOrder == MOUNT_HELO ) { loadHelo (); } // end if if ( movementOrder == UNMOUNT_HELO ) { unloadHelo (); } // end if if ( movementOrder == HOLD ) { loaded = false; } // end if double turnAngle = espdu.getEntityOrientationPsi() + espdu.getEntityAngularVelocityZ() * pduSendInterval; rotatedRollY = rotatedPitchX = Math.cos ( turnAngle ); rotatedRollX = rotatedPitchY = Math.sin ( turnAngle ); rotatedRollX *= -1; //Determine the new X and Y Positions float newXLocation = ( float ) ( espdu.getEntityLocationX () + espdu.getEntityLinearVelocityX () * pduSendInterval ); espdu.setEntityLocationX ( newXLocation ); float newYLocation = ( float ) ( espdu.getEntityLocationY () + espdu.getEntityLinearVelocityY () * pduSendInterval ); espdu.setEntityLocationY ( newYLocation ); System.out.println ( "** here is the (x,y) for " + id + " ** = " + newXLocation + " " + newYLocation ); espdu.setEntityLinearVelocityX (( float )( speed * Math.cos ( espdu.getEntityOrientationPsi ()))); espdu.setEntityLinearVelocityY (( float )( speed * Math.sin ( espdu.getEntityOrientationPsi ()))); if ( mountedCode == UNMOUNTED ) { // get normal and height at current terrain position terrain.getNormalHeight(( double ) newXLocation, ( double ) newYLocation, normalHeight ); // use eqn of plane to get roll and pitch angles rotatedPitchZ = ( 0.0 - normalHeight [ 0 ] * rotatedPitchX - normalHeight [ 1 ] * rotatedPitchY ) / normalHeight [ 2 ]; rotatedRollZ = ( 0.0 - normalHeight [ 0 ] * rotatedRollX - normalHeight [ 1 ] * rotatedRollY ) / normalHeight [ 2 ]; thetaAngle = ( float ) Math.atan ( -1 * rotatedPitchZ ); phiAngle = ( float ) Math.atan ( rotatedRollZ ); // negative number offset = m above local terrain espdu.setEntityLocationZ( normalHeight [ 3 ] - verticalOffset ); // update orientations espdu.setEntityOrientationTheta ( thetaAngle ); espdu.setEntityOrientationPhi ( phiAngle ); espdu.setEntityOrientationPsi( espdu.getEntityOrientationPsi() + espdu.getEntityAngularVelocityZ() * pduSendInterval ); } // end if if (mountedCode == MOUNTED ) { if ( movementOrder != HOLD ) { espdu.setEntityLocationX ( espdu.getEntityLocationX () - mountedEspdu.getEntityLocationX ()); espdu.setEntityLocationY ( espdu.getEntityLocationY () - mountedEspdu.getEntityLocationY ()); } // end if if ( loaded ) { espdu.setEntityLocationX ( 0.0 ); espdu.setEntityLocationY ( 0.0 ); } // end if collectMountedPdus(); if ( movementOrder == HOLD ) { espdu.setEntityLocationX ( espdu.getEntityLocationX () - mountedEspdu.getEntityLocationX ()); espdu.setEntityLocationY ( espdu.getEntityLocationY () - mountedEspdu.getEntityLocationY ()); } // end if truePositionEspdu.setEntityLocationX ( espdu.getEntityLocationX () + mountedEspdu.getEntityLocationX ()); truePositionEspdu.setEntityLocationY ( espdu.getEntityLocationY () + mountedEspdu.getEntityLocationY ()); if ( loaded ) { espdu.setEntityLocationZ ( - 0.75 ); truePositionEspdu.setEntityLocationZ ( mountedEspdu.getEntityLocationZ () + 0.75 ); } // end if else { // get normal and height at current terrain position terrain.getNormalHeight( truePositionEspdu.getEntityLocationX (), truePositionEspdu.getEntityLocationY (), normalHeight ); // use eqn of plane to get roll and pitch angles rotatedPitchZ = ( 0.0 - normalHeight [ 0 ] * rotatedPitchX - normalHeight [ 1 ] * rotatedPitchY ) / normalHeight [ 2 ]; rotatedRollZ = ( 0.0 - normalHeight [ 0 ] * rotatedRollX - normalHeight [ 1 ] * rotatedRollY ) / normalHeight [ 2 ]; thetaAngle = ( float ) Math.atan ( -1 * rotatedPitchZ ); phiAngle = ( float ) Math.atan ( rotatedRollZ ); // update orientations espdu.setEntityOrientationTheta ( thetaAngle ); espdu.setEntityOrientationPhi ( phiAngle ); espdu.setEntityLocationZ (( normalHeight [ 3 ] - verticalOffset ) - mountedEspdu.getEntityLocationZ ()); truePositionEspdu.setEntityLocationZ ( normalHeight [ 3 ] - verticalOffset ); } // end else espdu.setEntityOrientationPsi ( espdu.getEntityOrientationPsi () + espdu.getEntityAngularVelocityZ () * pduSendInterval ); truePositionEspdu.setEntityOrientationPsi ( espdu.getEntityOrientationPsi ()); if ( movementOrder != HOLD ) { double trueSpeed = Math.sqrt ( Math.pow (( espdu.getEntityLinearVelocityX () + mountedEspdu.getEntityLinearVelocityX ()), 2 ) + Math.pow (( espdu.getEntityLinearVelocityY () + mountedEspdu.getEntityLinearVelocityY ()), 2 )); if ( trueSpeed == 0 ) { behaviorCode = STAND; } // end if else if (( trueSpeed > 0 ) && ( trueSpeed <= 2.06f )) { behaviorCode = WALK; } // end else if else { behaviorCode = RUN; } // end else ApBehavior.setParameterValue (( double ) behaviorCode ); } // end if } // end if checkForReload(); // for display in the human panel newHeading = espdu.getEntityOrientationPsi(); if ( Math.abs (( oldHeading - newHeading ) * 180 / 3.1416 ) >= 1.0 ) { mypanel.updateHeading ( newHeading ); oldHeading = newHeading; } // end if if ( first ) { mypanel.updateHeading( newHeading ); first = false; } // end if // send espdu to the specified multicast address espdu.makeTimestampCurrent(); behaviorStreamBuffer.sendPdu(espdu, ipAddress, portNumber); collectFirePdus(); try { // put thread to sleep for 450 ms, thus when // 50ms) is added the pdu send interval will Thread.sleep ( 450 ); } // end try catch ( InterruptedException ite ) { ite.printStackTrace (); } // end catch } // end while } // end startLiving /** * column - the column rule set * @param void * @return void */ public void column () { double offset = 0; double angleOff = ( Math.PI / 2); if ( id == 401 ) { offset = 2.5; angleOff = Math.PI; } // end if else if ( id == 402 ) { offset = 7.5; angleOff = 0; } // end else if else if ( id == 403 ) { offset = 2.5; angleOff = 0; } // end else if else if ( id == 404 ) { offset = 12.5; angleOff = Math.PI; } // end else if else { offset = 7.5; angleOff = Math.PI; } // end else calculatePosition ( offset, angleOff ); } // end column /** * line - the line rule set * @param void * @return void */ public void line () { double offset = 0; double angleOff = ( Math.PI / 2); if ( id == 401 ) { offset = 5; } // end if else if ( id == 402 ) { offset = 20; } // end else if else if ( id == 403 ) { offset = 15; } // end else if else if ( id == 404 ) { offset = 25; } // end else if else { offset = 10; } // end else calculatePosition ( offset, angleOff ); } // end line /** * wedge - the wedge rule set * @param void * @return void */ public void wedge () { double offset = 0; double angleOff = ( Math.PI / 2); if ( id == 401 ) { offset = 5; angleOff = Math.PI; } // end if else if ( id == 402 ) { offset = 11.2; angleOff = -0.8524 * Math.PI; } // end else if else if ( id == 403 ) { offset = 11.2; angleOff = 0.8524 * Math.PI; } // end else if else if ( id == 404 ) { offset = 18; angleOff = -0.8128 * Math.PI; } // end else if else { offset = 18; angleOff = 0.8128 * Math.PI; } // end else calculatePosition ( offset, angleOff ); } // end wedge /** * loadHelo - the mount helicopter rule set * @param void * @return void */ public void loadHelo () { double interceptAngle; double interceptAngleDeg; float turnAngle; interceptAngle = espdu.getEntityOrientationPsi() - mountedEspdu.getEntityOrientationPsi() + Math.PI / 2; //check to see if there is a turn greater than 180 degrees if ( interceptAngle > Math.PI ) { interceptAngle -= ( Math.PI * 2 ); } // end if if ( interceptAngle < -Math.PI ) { interceptAngle += ( Math.PI * 2 ); } // end if interceptAngleDeg = interceptAngle * ( 180.0 / Math.PI ); turnAngle = - (( float ) interceptAngleDeg * 0.8f ); setTurnAngle ( turnAngle ); mypanel.setTurnRateText ( "" + ( int ) turnAngle ); mypanel.setTurnRateSlider (( int ) turnAngle ); if (( Math.abs ( interceptAngleDeg ) <= 2 ) && ( !loaded )) { double relativeGoalX = ( mountedEspdu.getEntityLocationX () - truePositionEspdu.getEntityLocationX ()); double relativeGoalY = ( mountedEspdu.getEntityLocationY () - truePositionEspdu.getEntityLocationY ()); findMagAndAng ( relativeGoalX, relativeGoalY ); if ( distance <= 1.5 ) { loaded = true; mypanel.setSpeedText ( "" + 0 ); mypanel.setSpeedSlider ( 0 ); } // end if else { espdu.setEntityLinearVelocityX (( float ) ( 1 * 0.5144f * Math.cos ( espdu.getEntityOrientationPsi ()))); espdu.setEntityLinearVelocityY (( float ) ( 1 * 0.5144f * Math.sin ( espdu.getEntityOrientationPsi ()))); turnAngle = - (( float ) interceptAngleDeg * 0.8f ); setTurnAngle ( turnAngle ); mypanel.setTurnRateText ( "" + ( int ) turnAngle ); mypanel.setTurnRateSlider (( int ) turnAngle ); mypanel.setSpeedText ( "" + 1 ); mypanel.setSpeedSlider ( 1 ); } // end else } // end if } // end loadHelo /** * unloadHelo - the unmount helicopter rule set * @param void * @return void */ public void unloadHelo () { loaded = false; if ( firstUnload ) { collectMountedPdus (); espdu.setEntityOrientationPsi ( mountedEspdu.getEntityOrientationPsi ()); firstUnload = false; } // end if else { double offset = 0; double angleOff = ( Math.PI / 2); if ( id == 401 ) { offset = 12; angleOff = 0; } // end if else if ( id == 402 ) { offset = 9.486; angleOff = 0.1024 * Math.PI; } // end else if else if ( id == 403 ) { offset = 9.486; angleOff = -0.1024 * Math.PI; } // end else if else if ( id == 404 ) { offset = 8.485; angleOff = Math.PI / 4; } // end else if else { offset = 8.485; angleOff = -Math.PI / 4; } // end else calculatePosition ( offset, angleOff ); } // end else } // end unloadHelo /** * calculatePosition - calulates the vector from the entity's current position * to its goal position and orientation as determined by its rule set * @param double offset * @param double angleOff * @return void */ public void calculatePosition ( double offset, double angleOff ) { double interceptAngle; double interceptAngleDeg; float turnAngle; double placement = mountedEspdu.getEntityOrientationPsi() + angleOff; double relativeGoalX = ( mountedEspdu.getEntityLocationX () + ( Math.cos ( placement ) * offset )) - espdu.getEntityLocationX (); double relativeGoalY = ( mountedEspdu.getEntityLocationY () + ( Math.sin ( placement ) * offset )) - espdu.getEntityLocationY (); findMagAndAng ( relativeGoalX, relativeGoalY ); if ( distance <= 0.5 ) { espdu.setEntityLinearVelocityX ( 0f ); espdu.setEntityLinearVelocityY ( 0f ); if ( movementOrder == UNMOUNT_HELO ) { interceptAngle = espdu.getEntityOrientationPsi() - ( mountedEspdu.getEntityOrientationPsi() + angleOff ); } // end if else { interceptAngle = espdu.getEntityOrientationPsi() - mountedEspdu.getEntityOrientationPsi(); } // end else //check to see if there is a turn greater than 180 degrees if ( interceptAngle > Math.PI ) { interceptAngle -= ( Math.PI * 2 ); } // end if if ( interceptAngle < -Math.PI ) { interceptAngle += ( Math.PI * 2 ); } // end if interceptAngleDeg = interceptAngle * ( 180.0 / Math.PI ); if (( movementOrder == LINE ) || ( movementOrder == MOUNT_HELO )) { if ( Math.abs ( interceptAngleDeg ) <= 3 ) { mypanel.setInLine ( true ); } // end if } // end if if ( movementOrder == UNMOUNT_HELO ) { if ( Math.abs ( interceptAngleDeg ) <= 3 ) { mypanel.setFormation ( HOLD ); } // end if } // end if turnAngle = - (( float ) interceptAngleDeg * 0.8f ); setTurnAngle ( turnAngle ); mypanel.setTurnRateText ( "" + ( int ) turnAngle ); mypanel.setTurnRateSlider (( int ) turnAngle ); } // end if else { interceptAngle = espdu.getEntityOrientationPsi() - angle; //check to see if there is a turn greater than 180 degrees if ( interceptAngle > Math.PI ) { interceptAngle -= ( Math.PI * 2 ); } // end if if ( interceptAngle < -Math.PI ) { interceptAngle += ( Math.PI * 2 ); } // end if interceptAngleDeg = interceptAngle * ( 180.0 / Math.PI ); turnAngle = - (( float ) interceptAngleDeg * 0.8f ); setTurnAngle ( turnAngle ); mypanel.setTurnRateText (("" + new Integer (( int ) turnAngle )).toString()); mypanel.setTurnRateSlider (( int ) turnAngle ); double possibleSpeed; if (( distance <= 5 ) && ( movementOrder == UNMOUNT_HELO )) { exited = true; mypanel.setExited ( exited ); } // end if if ( distance <= 10.0 ) { possibleSpeed = ( distance * 0.5144 * pduSendInterval ); } // end if else { possibleSpeed = 5; } // end else espdu.setEntityLinearVelocityX (( float ) ( possibleSpeed * 0.5144f * Math.cos ( espdu.getEntityOrientationPsi ()))); espdu.setEntityLinearVelocityY (( float ) ( possibleSpeed * 0.5144f * Math.sin ( espdu.getEntityOrientationPsi ()))); mypanel.setSpeedText (( "" + new Integer ((int) possibleSpeed )).toString()); mypanel.setSpeedSlider (( int ) possibleSpeed ); } // end else } // end calculatePosition /** * stopRun - stops the simulation run for this entity * @param void * @return void */ public void stopRun () { behaviorStreamBuffer.shutdown (); } // end stopRun /** * setMovementOrder - sets the active movement order to initiate group * actions * @param int * @return void */ public void setMovementOrder ( int formationIndicator ) { movementOrder = formationIndicator; if ( movementOrder != MOUNT_HELO ) { loaded = false; } // end if } // end setMovementOrder /** * findMagnitudeAndAngle - finds the distance and angle to a goal position * taking Espdus as arguments * @param EntityStatePdu to * @param EntityStatePdu from * @return void */ public void findMagnitudeAndAngle (EntityStatePdu to, EntityStatePdu from) { double theXComp = to.getEntityLocationX() - from.getEntityLocationX(); double theYComp = to.getEntityLocationY() - from.getEntityLocationY(); distance = ( Math.sqrt((Math.pow(theXComp, 2)) + (Math.pow(theYComp, 2)))); if (( theXComp > 0 ) && ((theYComp >= 0 ) || (theYComp < 0 ))) { angle = Math.atan ( theYComp / theXComp ); } // end if else if (( theXComp == 0 ) && (theYComp < 0 )) { angle = -Math.PI/2; } // end if else else if (( theXComp == 0 ) && (theYComp > 0 )) { angle = Math.PI/2; } // end if else else if (( theXComp < 0 ) && (theYComp == 0 )) { angle = -Math.PI; } // end if else else if (( theXComp < 0 ) && (theYComp < 0 )) { angle = Math.atan ( theYComp / theXComp ) - Math.PI; } // end if else else { angle = Math.atan ( theYComp / theXComp ) + Math.PI; } // end else } // end findMagnitudeAndAngle /** * findMagAndAng - finds the distance and angle to a goal position * taking X and Y component offsets as arguments * @param double theXComp * @param double theXComp * @return void */ public void findMagAndAng (double theXComp, double theYComp) { distance = ( Math.sqrt((Math.pow(theXComp, 2)) + (Math.pow(theYComp, 2)))); if (( theXComp > 0 ) && ((theYComp >= 0 ) || (theYComp < 0 ))) { angle = Math.atan ( theYComp / theXComp ); } // end if else if (( theXComp == 0 ) && (theYComp < 0 )) { angle = -Math.PI/2; } // end if else else if (( theXComp == 0 ) && (theYComp > 0 )) { angle = Math.PI/2; } // end if else else if (( theXComp < 0 ) && (theYComp == 0 )) { angle = -Math.PI; } // end if else else if (( theXComp < 0 ) && (theYComp < 0 )) { angle = Math.atan ( theYComp / theXComp ) - Math.PI; } // end if else else { angle = Math.atan ( theYComp / theXComp ) + Math.PI; } // end else } // end findMagAndAng /** * checkForReload checks for the ability to reload ammunition * @param void * @return void */ public void checkForReload () { double xreload; double yreload = 2000; double xNought; double yNought; if ( mountedCode == MOUNTED ) { xNought = truePositionEspdu.getEntityLocationX(); yNought = truePositionEspdu.getEntityLocationY(); } // end if else { xNought = espdu.getEntityLocationX(); yNought = espdu.getEntityLocationY(); } // end else if ( red == true ) { xreload = 2000; } // end if else { xreload = 1000; } // end else double distanceFromHome = Math.sqrt (( xreload - xNought ) * ( xreload - xNought ) + ( yreload - yNought ) * ( yreload - yNought )); if ( distanceFromHome < 100 ) { mypanel.updateAmmo (); } // end if } // end checkForReload /** * selfDestruct - causes the entity to self destruct * @param void * @return void */ public void selfDestruct () { copdu.setCollisionType(new UnsignedByte(CollisionTypeField.INELASTIC)); copdu.setLinearVelocity(0.0f, 0.0f, 0.0f); copdu.makeTimestampCurrent(); behaviorStreamBuffer.sendPdu(copdu, ipAddress, portNumber); if ( mountedCode == MOUNTED ) { collectMountedPdus (); dpdu.setLocationInWorldCoordinate ( truePositionEspdu.getEntityLocationX (), truePositionEspdu.getEntityLocationY (), truePositionEspdu.getEntityLocationZ ()); } // end if else { dpdu.setLocationInWorldCoordinate ( espdu.getEntityLocationX (), espdu.getEntityLocationY (), espdu.getEntityLocationZ ()); } // end else dpdu.setTargetEntityID ( espdu.getEntityID ()); dpdu.setVelocity ( 0.0f, 0.0f, 0.0f ); dpdu.makeTimestampCurrent (); behaviorStreamBuffer.sendPdu ( dpdu, ipAddress, portNumber ); // send espdu to the specified multicast address espdu.makeTimestampCurrent (); espdu.setEntityLinearVelocityX ( 0.0f ); espdu.setEntityLinearVelocityY ( 0.0f ); espdu.setEntityLinearVelocityZ ( 0.0f ); setTurnAngle ( 20.0f ); // spin! then stop after DR times out ApBehavior.setParameterValue ( NOTHING ); ApMounted.setParameterValue ( MOUNTED ); behaviorStreamBuffer.sendPdu ( espdu, ipAddress, portNumber ); isCollided = true; try { Thread.sleep ( 4500 ); } // end try catch( InterruptedException ite ) { ite.printStackTrace (); } // end catch reset (); } // end selfDestruct /** * getFrameHeaderInfo - provides the frame header information * @param void * @return void */ public String getFrameHeaderInfo () { EntityID temp = espdu.getEntityID (); return new String ( "R71-630" + temp.getEntityID () + " (" + temp.getSiteID () + "/" + temp.getApplicationID () + "/" + temp.getEntityID () + ")" ); } // end getFrameheaderInfo /** * collectMountedPdus - reads the stream buffer to get the aggregation * entity's latest Espdu * @param void * @return void */ public void collectMountedPdus(){ boolean gotIt = false; Vector pdus = behaviorStreamBuffer.receivedPdus(); for ( Enumeration enum = pdus.elements (); enum.hasMoreElements ();) { ProtocolDataUnit tempPdu = ( ProtocolDataUnit ) enum.nextElement (); UnsignedByte pduType = tempPdu.getPduType (); if ( pduType.shortValue () == PduTypeField.ENTITYSTATE ) { EntityID mount = (( EntityStatePdu ) tempPdu ).getEntityID (); if ( mount.equals ( mountedEspdu.getEntityID ())) { mountedEspdu = ( EntityStatePdu ) tempPdu; gotIt = true; } // end if } // end if } // end for // Only executes the first time the method is called and if it does not // retrieve the mountedEspdu if ((( gotIt == false ) && ( alive == false )) || (( gotIt == false ) && ( movementOrder == UNMOUNT_HELO ) && ( firstUnload ))) { try { Thread.sleep ( 25 ); } // end try catch ( InterruptedException ite ) { ite.printStackTrace(); } // end catch collectMountedPdus (); } // end if } // end collectMountedPdus //============================================================================================= // Button Methods - These methods are all called by TestFrame.java //============================================================================================= /** * reset - handles input from the reset button * @param void * @return void */ public void reset () { ApMounted.setParameterValue( mountedCode ); if ( red == true ) { if ( id > 400 ) { int newID = id / 10; int tester = (( id % 400 ) % 10 ); if ( tester == 1 ) { espdu.setEntityLocationX ( -10f + 5 ); // mls espdu.setEntityLocationY ( 1990f + newID % 35 * 50 ); } // end if else if ( tester == 2 ) { espdu.setEntityLocationX ( -10f + 10 ); // mls espdu.setEntityLocationY ( 1990f + 5 + newID % 35 * 50 ); } // end else if else if ( tester == 3 ) { espdu.setEntityLocationX ( -10f + 10 ); // mls espdu.setEntityLocationY ( 1990f - 5 + newID % 35 * 50 ); } // end else if else if ( tester == 4 ) { espdu.setEntityLocationX ( -10f + 15 ); // mls espdu.setEntityLocationY ( 1990f + 10 + newID % 35 * 50 ); } // end else if else if ( tester == 5 ) { espdu.setEntityLocationX ( -10f + 15 ); // mls espdu.setEntityLocationY ( 1990f - 10 + newID % 35 * 50 ); } // end else if else {} // end else } // end if else { espdu.setEntityLocationX ( -10f ); espdu.setEntityLocationY ( 1990f + id % 35 * 50 ); } // end else if ( mountedCode == UNMOUNTED ) { espdu.setEntityLocationZ ( 36.3f - verticalOffset ); } // end if else { double [] normalHeight = new double [ 4 ]; terrain.getNormalHeight ( espdu.getEntityLocationX (), espdu.getEntityLocationY (), normalHeight ); espdu.setEntityLocationZ ( normalHeight [ 3 ] - verticalOffset ); } // end else espdu.setEntityOrientationPsi ( -6.7f + ( float ) Math.PI ); } // end if else { espdu.setEntityLocationX ( -5010f ); espdu.setEntityLocationY ( 1990f + id % 27 * 50 ); espdu.setEntityOrientationPsi ( -6.7f ); espdu.setEntityLocationZ ( 40.5f - verticalOffset ); } // end else if ( mountedCode == MOUNTED ) { truePositionEspdu.setEntityLocationX ( espdu.getEntityLocationX ()); truePositionEspdu.setEntityLocationY ( espdu.getEntityLocationY ()); truePositionEspdu.setEntityOrientationPsi ( espdu.getEntityOrientationPsi( )); truePositionEspdu.setEntityLocationZ ( espdu.getEntityLocationZ ()); collectMountedPdus(); espdu.setEntityLocationX ( espdu.getEntityLocationX () - mountedEspdu.getEntityLocationX ()); espdu.setEntityLocationY ( espdu.getEntityLocationY () - mountedEspdu.getEntityLocationY ()); espdu.setEntityLocationZ ( truePositionEspdu.getEntityLocationZ () - mountedEspdu.getEntityLocationZ ()); } // end if espdu.setEntityOrientationPhi ( 0.0f ); espdu.setEntityOrientationTheta ( 0.0f ); espdu.setEntityLinearVelocityX ( 0.0f ); espdu.setEntityLinearVelocityY ( 0.0f ); espdu.setEntityLinearVelocityZ ( 0.0f ); espdu.setEntityAngularVelocityX ( 0.0f ); espdu.setEntityAngularVelocityY ( 0.0f ); espdu.setEntityAngularVelocityZ ( 0.0f ); speed = 0; isCollided = false; ApBehavior.setParameterValue( NOTHING ); } // end reset() /** * sendFirePDU - send fire PDU information for hit determination * @param void * @return void */ public void sendFirePdu () { if ( mountedCode == MOUNTED ) { espdu.setEntityLocationX ( truePositionEspdu.getEntityLocationX ()); espdu.setEntityLocationY ( truePositionEspdu.getEntityLocationY ()); espdu.setEntityLocationZ ( truePositionEspdu.getEntityLocationY ()); espdu.setEntityOrientationPsi ( truePositionEspdu.getEntityOrientationPsi ()); } // end if // much more work needs to be done here (overall) fpdu.setLocationInWorldCoordinate ( espdu.getEntityLocationX () + 0.5, espdu.getEntityLocationY () + 0.5, espdu.getEntityLocationZ () - 1); if ( mountedCode == MOUNTED ) { fpdu.setVelocity (( float ) (( espdu.getEntityLinearVelocityX () + mountedEspdu.getEntityLinearVelocityX ()) + 1000 * Math.cos ( espdu.getEntityOrientationPsi ())), ( float ) (( espdu.getEntityLinearVelocityY () + mountedEspdu.getEntityLinearVelocityY ()) + 1000 * Math.sin ( espdu.getEntityOrientationPsi ())), ( float ) (( espdu.getEntityLinearVelocityZ () + mountedEspdu.getEntityLinearVelocityZ ()) - 1000 * Math.sin ( espdu.getEntityOrientationTheta ()))); } // end if else { fpdu.setVelocity (( float ) ( espdu.getEntityLinearVelocityX () + 1000 * Math.cos ( espdu.getEntityOrientationPsi ())), ( float ) ( espdu.getEntityLinearVelocityY () + 1000 * Math.sin ( espdu.getEntityOrientationPsi ())), ( float ) ( espdu.getEntityLinearVelocityZ () - 1000 * Math.sin ( espdu.getEntityOrientationTheta ()))); } // end else fpdu.makeTimestampCurrent (); fpdu.setRange ( 3000.0f ); espdu.makeTimestampCurrent (); behaviorStreamBuffer.sendPdu ( fpdu, ipAddress, portNumber ); behaviorStreamBuffer.sendPdu ( espdu, ipAddress, portNumber ); } // end sendFirePdu() /** * brake - stops the Human * @param void * @return void */ public void brake () { espdu.setEntityOrientationPhi ( 0.0f ); espdu.setEntityLinearVelocityX ( 0.0f ); espdu.setEntityLinearVelocityY ( 0.0f ); espdu.setEntityLinearVelocityZ ( 0.0f ); espdu.setEntityAngularVelocityX ( 0.0f ); espdu.setEntityAngularVelocityY ( 0.0f ); espdu.setEntityAngularVelocityZ ( 0.0f ); speed = 0; ApBehavior.setParameterValue ( STAND ); } // end public brake() /** * kneel - Makes the Human kneel * @param void * @return void */ public void kneel () { behaviorCode = KNEEL; ApBehavior.setParameterValue (( double ) behaviorCode ); } // end kneel //============================================================================= // Kill Determination Methods //============================================================================= /** * collectFirePDUs - collects the fire PDUs from the stream buffer * @param void * @return void */ public void collectFirePdus () { System.out.println ( "\nEntering collectFirePdus ()" ); Vector firedPdus = new Vector (); Vector pdus = behaviorStreamBuffer.receivedPdus (); System.out.println ( "\n# of Pdus in the Vector = " + pdus.size ()); for (Enumeration enum = pdus.elements(); enum.hasMoreElements ();) { ProtocolDataUnit tempPdu = ( ProtocolDataUnit ) enum.nextElement (); UnsignedByte pduType = tempPdu.getPduType (); if ( pduType.shortValue () == PduTypeField.FIRE ) { System.out.println ( "Received a fire Pdu" ); EntityID shooter = (( FirePdu ) tempPdu).getFiringEntityID (); if ( !shooter.equals ( espdu.getEntityID ())) { checkFirePdu (( FirePdu ) tempPdu ); LinearVelocity missileVel = (( FirePdu ) tempPdu ).getVelocity (); } // end if } // end if if ( pduType.shortValue () == PduTypeField.ENTITYSTATE ) { checkCollision (( EntityStatePdu ) tempPdu ); } // end if } // end for } // end collect FirePdus /** * checkFirePDUs * Function Works as Follows : * 1. Get X, Y, Z components of launch velocity. * 2. Calculate launch velocity vector. * 3. Get X, Y, Z location of the launch. * 4. Get X, Y, Z velocity componets of this human at launch. * 5. Get X, Y, Z location of this human at launch. * 6. Calculate the distance between this human and the launch point. * 7. Calculate the relative velocity of the fire with respect to this * human. * 8. Find the time to fly for this distance. * 9. Find the actual X, Y, Z location for the fire after "time to fly". * 10. Calculate the distance between this tank and the new location of * the fire after "time * to fly. * 11. If the distance is equal to or less than HIT_RADIUS a hit * has occured. * * @param shot Fire PDU that is being checked. * @return void */ public void checkFirePdu ( FirePdu shot ) { double HIT_RADIUS = 25.0; //?????????? double launchVelocityX = shot.getVelocity ().getX (); double launchVelocityY = shot.getVelocity ().getY (); double launchVelocityZ = shot.getVelocity ().getZ (); // Add this code after the wind is implemented // launchVelocityX += windVelocity.x; // launchVelocityY += windVelocity.x; // launchVelocityZ += windVelocity.x; double launchVelocity = Math.sqrt (( launchVelocityX * launchVelocityX ) + ( launchVelocityY * launchVelocityY ) + ( launchVelocityZ * launchVelocityZ )); double launchLocationX = shot.getLocationInWorldCoordinate ().getX (); double launchLocationY = shot.getLocationInWorldCoordinate ().getY (); double launchLocationZ = shot.getLocationInWorldCoordinate ().getZ (); double myVelocityAtLaunchX; double myVelocityAtLaunchY; double myVelocityAtLaunchZ; if ( mountedCode == MOUNTED ) { espdu.setEntityLocationX ( truePositionEspdu.getEntityLocationX ()); espdu.setEntityLocationY ( truePositionEspdu.getEntityLocationY ()); espdu.setEntityLocationZ ( truePositionEspdu.getEntityLocationY ()); espdu.setEntityOrientationPsi ( truePositionEspdu.getEntityOrientationPsi ()); myVelocityAtLaunchX = espdu.getEntityLinearVelocityX () + mountedEspdu.getEntityLinearVelocityX (); myVelocityAtLaunchY = espdu.getEntityLinearVelocityY () + mountedEspdu.getEntityLinearVelocityY (); myVelocityAtLaunchZ = espdu.getEntityLinearVelocityZ () + mountedEspdu.getEntityLinearVelocityZ (); } // end if else { myVelocityAtLaunchX = espdu.getEntityLinearVelocityX (); myVelocityAtLaunchY = espdu.getEntityLinearVelocityY (); myVelocityAtLaunchZ = espdu.getEntityLinearVelocityZ (); } // end else double myLocationAtLaunchX = espdu.getEntityLocationX (); double myLocationAtLaunchY = espdu.getEntityLocationY (); double myLocationAtLaunchZ = espdu.getEntityLocationZ (); double distanceFromTargetAtLaunch = Math.sqrt (( myLocationAtLaunchX - launchLocationX ) * ( myLocationAtLaunchX - launchLocationX ) + ( myLocationAtLaunchY - launchLocationY ) * ( myLocationAtLaunchY - launchLocationY ) + ( myLocationAtLaunchZ - launchLocationZ ) * ( myLocationAtLaunchZ - launchLocationZ )); double relativeLaunchVelocityX = launchVelocityX - myVelocityAtLaunchX; double relativeLaunchVelocityY = launchVelocityY - myVelocityAtLaunchY; double relativeLaunchVelocityZ = launchVelocityZ - myVelocityAtLaunchZ; double relativeLaunchVelocity = Math.sqrt ( ( relativeLaunchVelocityX * relativeLaunchVelocityX ) + ( relativeLaunchVelocityY * relativeLaunchVelocityY ) + ( relativeLaunchVelocityZ * relativeLaunchVelocityZ )); double timeToTravel = distanceFromTargetAtLaunch / relativeLaunchVelocity; double launchLocationAfterFlyX = launchLocationX + relativeLaunchVelocityX * timeToTravel; double launchLocationAfterFlyY = launchLocationY + relativeLaunchVelocityY * timeToTravel; double launchLocationAfterFlyZ = launchLocationZ + relativeLaunchVelocityZ * timeToTravel; //Closest point of approach double CPA = Math.sqrt (( launchLocationAfterFlyX - myLocationAtLaunchX ) * ( launchLocationAfterFlyX - myLocationAtLaunchX ) + ( launchLocationAfterFlyY - myLocationAtLaunchY ) * ( launchLocationAfterFlyY - myLocationAtLaunchY ) + ( launchLocationAfterFlyZ - myLocationAtLaunchZ ) * ( launchLocationAfterFlyZ - myLocationAtLaunchZ )); if ( CPA <= HIT_RADIUS ) { copdu.setCollisionType ( new UnsignedByte ( CollisionTypeField.INELASTIC )); copdu.setLinearVelocity ( 0.0f, 0.0f, 0.0f ); copdu.makeTimestampCurrent (); behaviorStreamBuffer.sendPdu ( copdu, ipAddress, portNumber ); dpdu.setLocationInWorldCoordinate ( espdu.getEntityLocationX (), espdu.getEntityLocationY (), espdu.getEntityLocationZ ()); dpdu.setTargetEntityID ( espdu.getEntityID ()); dpdu.setVelocity (0.0f, 0.0f, 0.0f ); dpdu.makeTimestampCurrent (); behaviorStreamBuffer.sendPdu ( dpdu, ipAddress, portNumber ); isCollided = true; try { Thread.sleep ( 4500 ); } // end try catch ( InterruptedException ite ) { ite.printStackTrace (); } // end catch reset(); } // end if } // end checkFirePdus() /** * checkCollision - checks to see if two entities geomentry have intersected * @param EntityStatePdu entityPdu * @return void */ public void checkCollision ( EntityStatePdu entityPdu ) { double [] entityMinusMe = new double [ 3 ]; if ( entityPdu.getEntityID ().equals ( espdu.getEntityID ()) || entityPdu.getEntityID ().getEntityID ().intValue () == 99 || entityPdu.getEntityID ().getEntityID ().intValue () == 98 || entityPdu.getEntityID ().getEntityID ().intValue () > 400 ) {} else { if ( mountedCode == MOUNTED ) { espdu.setEntityLocationX ( truePositionEspdu.getEntityLocationX ()); espdu.setEntityLocationY ( truePositionEspdu.getEntityLocationY ()); } // end if entityMinusMe [ 0 ] = entityPdu.getEntityLocationX () - espdu.getEntityLocationX (); entityMinusMe [ 1 ] = entityPdu.getEntityLocationY () - espdu.getEntityLocationY (); entityMinusMe [ 2 ] = entityPdu.getEntityLocationZ () - espdu.getEntityLocationZ (); double distBtwn = Math.pow ( entityMinusMe [ 0 ], 2 ) + Math.pow ( entityMinusMe [ 1 ], 2 ) + Math.pow ( entityMinusMe [ 2 ], 2 ); // w/in 8 meters of each other if ( distBtwn <= 8.0 ) { System.out.println ( "Stopping Human" ); System.out.println ( entityPdu.getEntityID ()); brake (); movementOrder = HOLD; } // end if } // end else } // end checkCollision /** * trace * 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.
* @param String - the debug text to be ouptput
* @return void
*/
protected void trace ( String pDiagnostic ) {
System.out.println ( " EspduTransform " + id + " " + pDiagnostic );
} // end trace
} // end class HumanActionInterpreterTeam
// end file HumanActionInterpreterTeam.java