package demo.helicopter; import java.text.*; // for class DecimalFormat import java.util.*; // for class Vector import mil.navy.nps.dis.*; // for class EntityStatePdu import mil.navy.nps.testing.*; // for MulticastPduSender import mil.navy.nps.util.*; import mil.navy.nps.disEnumerations.*; import java.awt.event.AdjustmentEvent; import javax.swing.JScrollBar; import java.lang.Math.*; public class AgentTankActionInterpreter extends Thread { private float pduSendInterval, // time delay between pdus speed; // not velocity, no directional information private DecimalFormat df; // pretty print screen output private EntityStatePdu teamEspdu; // Pdu to send entity state to VRML scene private EntityStatePdu espdu; // Pdu to send entity state to VRML scene private FirePdu fpdu; // Pdu to send fire info to VRML scene private CollisionPdu copdu; private BehaviorStreamBufferUDP behaviorStreamBuffer; // tie in to the network private DetonationPdu dpdu; private CreateEntityPdu cepdu; private CommentPdu cpdu; private RemoveEntityPdu repdu; private DatumSpecification datum; private VariableDatum variableDatum; private String geometryURL; private byte geometryURLbyteArray[]; private long geometryURLlongArray[]; private String ipAddress; private int portNumber; private int id; private boolean isCollided; private AgentTankControlPanel mypanel; private boolean turretLeft; private boolean turretRight; private boolean raiseGun; private boolean lowerGun; private double turretAngle; //in radians private double elevationAngle; //in radians private ArticulationParameter ApTurretRotation; private ArticulationParameter ApMainGunElevation; private boolean red; private boolean driving; private static CollisionDetectionTerrainReader terrain; private static boolean terrainAlreadyRead = false; private double oldHeading; private double newHeading; private double oldGunElevation; private boolean first; // first time thru the run cycle for panel updates //Agent variables below: //HashMap to hold the sensed PDU's that are received in the //collectSensedPdus() method private Hashtable sensedPdus; private Integer enemyFlag; private Integer friendlyFlag; private EntityStatePdu homeBaseEspdu; private boolean RedTeamMember; private boolean offensive = false; private boolean defensive = false; private boolean flagCaptured = false; private double maxProtectRange; private double minProtectRange; private double sensorRange; private Vector targetList; private float gunAccuracy; public static final int OFFENSIVE = 0; public static final int DEFENSIVE = 1; //============================================================================== // Constructors & Start //============================================================================== public AgentTankActionInterpreter( String ipAddr, int portNum, int timeToLive, short siteNum, short appNum, short idNum, String marking, boolean rtpHeaderEnabled, AgentTankControlPanel panel) { speed = 0.0f; // initially helo on ground with no fwd airspeed pduSendInterval = 0.5f; // approx 300 ms between pdu sends df = new DecimalFormat("###.00"); cepdu = new CreateEntityPdu(); // instantiate an cepdu for announcing the // creation of an entity cepdu.setRtpHeaderEnabled(rtpHeaderEnabled); cpdu = new CommentPdu(); // instantiate an cpdu, used to store the URL // address cpdu.setRtpHeaderEnabled(rtpHeaderEnabled); repdu = new RemoveEntityPdu(); // instantiate a repdu, //used to remove an entity 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... geometryURL = new String("http://www.web3D.org/WorkingGroups/vrtp/dis-java-vrml/demo/helicopter/RenegadeTank.wrl"); // place URL address here teamEspdu = new EntityStatePdu(); teamEspdu.setRtpHeaderEnabled(rtpHeaderEnabled); espdu = new EntityStatePdu(); // instantiate an espdu, used as a data structure espdu.setRtpHeaderEnabled(rtpHeaderEnabled); fpdu = new FirePdu(); // instantiate a fire Pdu, used as data structure fpdu.setRtpHeaderEnabled(rtpHeaderEnabled); copdu = new CollisionPdu(); // instantiate a collision Pdu for later use copdu.setRtpHeaderEnabled(rtpHeaderEnabled); dpdu = new DetonationPdu(); dpdu.setRtpHeaderEnabled(rtpHeaderEnabled); sensedPdus = new Hashtable(); espdu.setMarking (marking); driving = true; id = idNum; // instantiate a fire Pdu, used as data structure if (id < 30) { red = false; } else { red = true; } 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; teamEspdu.setEntityID( RedentityID[0], // set entity id triplet into RedentityID[1], RedentityID[2]); espdu.setEntityLocationX(-10f); //mls espdu.setEntityLocationY(1990f + id%35 *50); espdu.setEntityOrientationPsi(-6.7f+(float)Math.PI); // mls espdu.setEntityLocationZ(36.3f); // mls } else // create entity id triplet for espdu & fire pdu { short[] BlueentityID = new short[3]; BlueentityID[0] = 0; BlueentityID[1] = 1; BlueentityID[2] = 99; teamEspdu.setEntityID( BlueentityID[0], // set entity id triplet into BlueentityID[1], BlueentityID[2]); espdu.setEntityLocationX(-5010f); // mls espdu.setEntityLocationY(1990f + id%27 *50); espdu.setEntityOrientationPsi(-6.7f); // mls espdu.setEntityLocationZ(40.5f); // mls } // mls short[] entityID = new short[3]; entityID[0] = siteNum; entityID[1] = appNum; entityID[2] = (short)id; espdu.setEntityID( entityID[0], // set entity id triplet into entityID[1], entityID[2]); fpdu.setFiringEntityID(espdu.getEntityID()); // the espdu and fire pdu copdu.setIssuingEntityID(espdu.getEntityID()); // the espdu and fire pdu geometryURLbyteArray = geometryURL.getBytes(); variableDatum.setVariableDatumID(1); variableDatum.setVariableDatumLength(120); for (int counter = 0; counter < geometryURLbyteArray.length ; counter++) { geometryURLlongArray[counter] = (long) geometryURLbyteArray[counter]; } 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 // instantiate a BehaviorStreamBuffer to handle the network interface, // instantiate with the multicast IP address and port EntityID temp = espdu.getEntityID(); turretAngle = 1.57; oldGunElevation = elevationAngle = 0.0; oldHeading = newHeading = espdu.getEntityOrientationPsi(); first = true; ApTurretRotation = new ArticulationParameter(); ApMainGunElevation = new ArticulationParameter(); ApTurretRotation.setParameterTypeDesignator(ParameterTypeDesignatorField.ARTICULATEDPART); // 0 ApMainGunElevation.setParameterTypeDesignator(ParameterTypeDesignatorField.ARTICULATEDPART); ApTurretRotation.setChangeIndicator(0); // sequentially increment with each change in value ApMainGunElevation.setChangeIndicator(0); ApTurretRotation.setArticulationAttachmentID (0); // attached to main entity, not multiply articulated ApMainGunElevation.setArticulationAttachmentID(1); // attached to turret (which we are calling #1) ApTurretRotation.setParameterType (4096 + 15); // Primary turret number 1 + Rotation ApMainGunElevation.setParameterType(4416 + 13); // Primary gun number 1 + Elevation ApTurretRotation.setParameterValue(turretAngle); // double, radians ApMainGunElevation.setParameterValue(elevationAngle); // double, radians espdu.addArticulationParameter(ApTurretRotation); espdu.addArticulationParameter(ApMainGunElevation); ///////////////////////////////////////////////////////////////////////////////////////////////////// //agent thing //this identifies this entity as an agent to the referee espdu.setForceID(1); mypanel = panel; isCollided = false; ipAddress = ipAddr; portNumber = portNum; behaviorStreamBuffer = new BehaviorStreamBufferUDP (ipAddress, portNumber); behaviorStreamBuffer.setTimeToLive (timeToLive); Thread pduReader = new Thread(behaviorStreamBuffer); pduReader.start(); // System.out.println("sending CreateEntity PDU..."); behaviorStreamBuffer.sendPdu(cepdu.getExemplar(), ipAddress, portNumber); // System.out.println("sending Comment PDU with URL..."); behaviorStreamBuffer.sendPdu(cpdu.getExemplar(), ipAddress, portNumber); // read in Fort Irwin Terrain file if (!terrainAlreadyRead) { terrain = new CollisionDetectionTerrainReader(); terrain.parseFile(); terrainAlreadyRead = true; } turretLeft = false; turretRight = false; raiseGun = false; lowerGun = false; //******************** added agent stuff ********************* homeBaseEspdu = new EntityStatePdu();//(EntityStatePdu)espdu.clone(); //determine which team I'm on and then set the enemy flag as the //puppy I'm after int myIDNumber = espdu.getEntityID().getEntityID().intValue(); if (myIDNumber < 30)//I'm a red team tank { RedTeamMember = true; enemyFlag = new Integer(99); friendlyFlag = new Integer(98); homeBaseEspdu.setEntityLocationX(-5000); // set starting location into homeBaseEspdu.setEntityLocationY(2000); // espdu, all other initial homeBaseEspdu.setEntityLocationZ(39.8); // quantities are set to zero } //when the espdu is constructed else //I'm a blue team tamk //in the dis-java-vrml code { RedTeamMember = false; enemyFlag = new Integer(98); friendlyFlag = new Integer(99); homeBaseEspdu.setEntityLocationX(0);// set starting location into espdu, homeBaseEspdu.setEntityLocationY(2000); // all other initial quantities homeBaseEspdu.setEntityLocationZ(35.8); // are set to zero when the } // espdu is constructed in the // dis-java-vrml code offensive = true; defensive = false; flagCaptured = false; minProtectRange = 50.0d; maxProtectRange = 250.0d; sensorRange = 1750.0d; targetList = new Vector(); gunAccuracy = 0.5f; } // end constructor // start sending espdus, called from TestFrame.java // calls flying which is an infinite espdu sending loop public void run() {startDriving();} //============================================================================== // Properties //============================================================================== public void setIsCollided (boolean setValue) {isCollided = setValue;} public boolean getIsCollided() {return isCollided;} public void setDriving(boolean setValue) {driving = setValue;} public boolean getDriving() {return driving;} public void setSpeed(float spd) { speed = .5144f * spd; // convert from knots to meters/second espdu.setEntityLinearVelocityX((float)(speed * Math.cos(espdu.getEntityOrientationPsi()))); espdu.setEntityLinearVelocityY((float)(speed * Math.sin(espdu.getEntityOrientationPsi()))); } // end setSpeed() public double getSpeed() {return speed;} public void setTurnAngle(float turnAngle) { espdu.setEntityAngularVelocityZ((float)(.1058 * (turnAngle * 3.1415 / 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() public EntityStatePdu getEspdu() {return espdu;} //============================================================================== // Utility Methods //============================================================================== // this method loops indefinitely sending espdus every 300 ms or so. This loop // runs on itsown thread public void startDriving() { while(driving) // loops until mother thread exits thus killing this thread { // simple kinematics to update X, Y, Z location in world coordinates double rotatedPitchX; double rotatedPitchY; double rotatedPitchZ; double rotatedRollX; double rotatedRollY; double rotatedRollZ; double turnAngle = espdu.getEntityOrientationPsi() + espdu.getEntityAngularVelocityZ() * pduSendInterval; rotatedRollY = rotatedPitchX = Math.cos(turnAngle); rotatedRollX = rotatedPitchY = Math.sin(turnAngle); rotatedRollX *= -1; float newXLocation =(float) (espdu.getEntityLocationX() + espdu.getEntityLinearVelocityX() * pduSendInterval); espdu.setEntityLocationX(newXLocation); float newYLocation= (float)(espdu.getEntityLocationY() + espdu.getEntityLinearVelocityY() * pduSendInterval); espdu.setEntityLocationY(newYLocation); // get normal and height at current terrain position double [] normalHeight = new double [4]; 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]; float thetaAngle = (float)Math.atan(-1*rotatedPitchZ); float phiAngle = (float)Math.atan(rotatedRollZ); // negative number offset = m above local terrain espdu.setEntityLocationZ(normalHeight[3] - 2.5); // update linear velocities espdu.setEntityLinearVelocityX((float)(speed * Math.cos(espdu.getEntityOrientationPsi()))); espdu.setEntityLinearVelocityY((float)(speed * Math.sin(espdu.getEntityOrientationPsi()))); // update orientations // update orientations espdu.setEntityOrientationTheta(thetaAngle); espdu.setEntityOrientationPhi(phiAngle); espdu.setEntityOrientationPsi(espdu.getEntityOrientationPsi() + espdu.getEntityAngularVelocityZ() * pduSendInterval); checkForTurretTurn(); checkForGunElevate(); checkForGroundCollision(); checkForReload(); // for display in the tank control panel newHeading = espdu.getEntityOrientationPsi(); if( Math.abs((oldGunElevation - elevationAngle) * 180 / 3.1416) >= 1.0 || Math.abs((oldHeading - newHeading) * 180 / 3.1416) >= 1.0 ) { mypanel.updateHeading( newHeading ); mypanel.updateGunElev( elevationAngle ); oldHeading = newHeading; oldGunElevation = elevationAngle; } if (first) { mypanel.updateHeading( newHeading ); mypanel.updateGunElev( elevationAngle ); first = false; } // send espdu to the specified multicast address espdu.makeTimestampCurrent(); behaviorStreamBuffer.sendPdu(espdu, ipAddress, portNumber); collectSensorPdus(); try { Thread.sleep(450); // put thread to sleep for 450 ms, thus when } // end try // 50ms) is added the pdu send interval will catch(InterruptedException ite) { ite.printStackTrace(); } // end catch runAgent(); } // end while } // end startDriving() public void setOffensive() { offensive = true; defensive = false; String tempTitle = new String("Agent Tank Control Panel " + getFrameHeaderInfo()); mypanel.setTitle(tempTitle); espdu.setForceID(1); } public void setDefensive() { offensive = false; defensive = true; String tempTitle = new String("Agent Tank Control Panel " + getFrameHeaderInfo()); mypanel.setTitle(tempTitle); espdu.setForceID(2); } public boolean getOffensive( ) { return offensive; } public boolean getDefensive( ) { return defensive; } public void runAgent() { //develop target list and find flag pdus double range = 0.0d; double lastRange = 0.0d; double minRange = 6000.0d; EntityStatePdu closestEnemyEspdu = null; EntityStatePdu enemyFlagEspdu = null; EntityStatePdu friendlyFlagEspdu = null; Enumeration enum = sensedPdus.keys(); while( enum.hasMoreElements() ) { Integer tempKey = (Integer)enum.nextElement(); if( tempKey.equals(enemyFlag) ) { enemyFlagEspdu = (EntityStatePdu)sensedPdus.get( enemyFlag ); } if( tempKey.equals(friendlyFlag) ) { friendlyFlagEspdu = (EntityStatePdu)sensedPdus.get( friendlyFlag ); } // develop list of all enemy units within sensor range EntityStatePdu tempEspdu = (EntityStatePdu)sensedPdus.get(tempKey); range = determineRange( espdu, tempEspdu); if( range < sensorRange ) { if( RedTeamMember ) // I'm a red team member { //sensedPdu is a blue team member, thus an enemy if ( (tempKey.intValue() >= 30) && (tempKey.intValue() < 98) ) { targetList.addElement(tempEspdu); if( range < minRange ) { minRange = range; closestEnemyEspdu = tempEspdu; } } } else // I'm a blue team member { //sensedPdu is a red team member, thus an enemy if (tempKey.intValue() < 30) { targetList.addElement(tempEspdu); if( range < minRange ) { minRange = range; closestEnemyEspdu = tempEspdu; } } } }// end if in sensor range }// end while //DRIVE TO ENEMY FLAG AND ENGAGE ENEMY UNITS IF WITHIN WEAPONS RANGE// if( offensive && enemyFlagEspdu != null ) { // System.out.println("running Offensive Tank Agent"); getFlag( enemyFlagEspdu ); } //RETURN TO BE NEAR OWN FLAG AND ENGAGE ENEMY UNITS IF WITHIN WEAPONS RANGE// if( defensive && friendlyFlagEspdu != null ) { // System.out.println("running Defensive Tank Agent"); protectFlag( friendlyFlagEspdu ); } // System.out.println("Target list has " + targetList.size() + " elements."); //ALWAYS ENGAGE ENEMY if(closestEnemyEspdu != null ) { // System.out.println("engaging enemy unit"); engageEnemy( closestEnemyEspdu ); } else { if( ( turretAngle >= -0.01d) || (turretAngle <= 0.01d) ) { resetTurret(); } } targetList.clear(); }// end runAgent public void getFlag(EntityStatePdu enemyFlagEspdu) { int angle; double horizontalRange; double xCarrier = espdu.getEntityLocationX(); double yCarrier = espdu.getEntityLocationY(); double xFlag = enemyFlagEspdu.getEntityLocationX(); double yFlag = enemyFlagEspdu.getEntityLocationY(); // Set agent speed to max speed speed = 60; mypanel.speedScroll.setValue((int)speed); horizontalRange = Math.sqrt((xCarrier-xFlag)*(xCarrier-xFlag) + (yCarrier-yFlag)*(yCarrier-yFlag)); //System.out.println("horizontalRange = " + horizontalRange); if(flagCaptured) { if(horizontalRange > 100) { flagCaptured = false; } } // Check to see if flag is captured if( !flagCaptured ) { if( horizontalRange <= 100 ) { flagCaptured = true; // System.out.println("flagCaptured = true"); } } if( !flagCaptured ) // go get it! { driveToHere( enemyFlagEspdu ); } else { driveToHere( homeBaseEspdu ); // and win! } } public void protectFlag(EntityStatePdu friendlyFlagEspdu) { int angle; double horizontalRange; // Set agent speed to max speed speed = 60; mypanel.speedScroll.setValue((int)speed); // Calculate range to own flag horizontalRange = determineRange( espdu, friendlyFlagEspdu ); // Ensure that range is less than maxProtectRange if( horizontalRange >= maxProtectRange ) // return closer to own flag { driveToHere( friendlyFlagEspdu ); } } // same as "shoot this( EntityStatePdu)" public void engageEnemy( EntityStatePdu enemyUnitEspdu ) { boolean gunAzmuthGood = false; boolean gunElevationGood = false; boolean readyToFire = false; //train main gun azimuth & elevation int targetInterceptAngle = -interceptAngle( espdu, enemyUnitEspdu ); int relativeGunAngle = relativeTurretAngle(); int gunAzmuthError = ((relativeGunAngle) - (targetInterceptAngle)); double range; double deltaZ; double targetElevationAngle; double gunElevationError; // train gun azmuth if( (gunAzmuthError >= 2) && !turretLeft ) { leftTurretTurn(); } if( (gunAzmuthError <= -2) && !turretRight ) { rightTurretTurn(); } if( (gunAzmuthError == 0) )// stop turret turning when on target { if( turretLeft ) { leftTurretTurn(); } if( turretRight ) { rightTurretTurn(); } gunAzmuthGood = true; } // train gun elevation range = determineRange( espdu, enemyUnitEspdu ); deltaZ = espdu.getEntityLocationZ() - enemyUnitEspdu.getEntityLocationZ(); targetElevationAngle = Math.asin( deltaZ / range ); gunElevationError = ( (elevationAngle) - (targetElevationAngle)); if( (gunElevationError > 0.01d ) && !lowerGun ) { depressGun(); } if( (gunElevationError < -0.01d ) && !raiseGun ) { elevateGun(); } // stop elevating or depressing gun when on target if( (gunElevationError >= -0.1d) && (gunElevationError <= 0.1d) ) { if( lowerGun ) { lowerGun = false; } if( raiseGun ) { raiseGun = false; } gunElevationGood = true; } //shoot when main gun azimuth & elevation within tollerance if( gunAzmuthGood && gunElevationGood ) { sendMainFirePdu(); } } public void resetTurret() // aims turret forward { //train main gun azimuth & elevation to 0 int relativeGunAngle = relativeTurretAngle(); if( relativeGunAngle >= 1 ) { leftTurretTurn(); } if( relativeGunAngle <= -1 ) { rightTurretTurn(); } if( (relativeGunAngle == 0) )// stop turret turning when on target { turretLeft = false; turretRight = false; } } public void driveToHere( EntityStatePdu here ) { int angle = interceptAngle( espdu, here ); float turnAngle = -((float)angle * 0.4f); setTurnAngle(turnAngle); mypanel.turnTextField.setText((new Integer((int)turnAngle)).toString()); mypanel.turnScroll.setValue((int)turnAngle); } public int interceptAngle( EntityStatePdu me, EntityStatePdu target ) { //calculation variables int myX = (int)me.getEntityLocationX(); int myY = (int)me.getEntityLocationY(); int targetX = (int)target.getEntityLocationX(); int targetY = (int)target.getEntityLocationY(); //this gives the angle (0-90) between the two points double a = Math.abs(myX - targetX); double b = Math.abs(myY - targetY); double c = Math.sqrt((a * a) + (b * b)); double theta = Math.toDegrees(Math.acos(a / c)); int heading; int interceptAngle; int targetBearing = 0; //used to get the heading of the "me" object double mypsi = me.getEntityOrientationPsi(); // determine my heading if ( mypsi < 0 ) { heading = (int)( 360 + ((( mypsi * 180 ) / Math.PI ) % 360 )); } else { heading = (int)((( mypsi * 180 ) / Math.PI ) % 360 ); } // determine target bearing with relation to quadrants // North West quadrant if( (( myX == targetX ) && ( myY > targetY )) || (( myX < targetX ) && ( myY > targetY )) || (( myX < targetX ) && ( myY == targetY )) ) { theta = 360 - theta; }//end if // North East quadrant else if(( myX < targetX ) && ( myY < targetY )) { theta += 0 ; } // South East quadrant else if( (( myX == targetX ) && ( myY < targetY )) || (( myX > targetX ) && ( myY < targetY )) || (( myX > targetX ) && ( myY == targetY )) ) { theta = 180 - theta; } // South West quadrant else if(( myX > targetX ) && ( myY > targetY )) { theta = 180 + theta; } // determine intercept angle interceptAngle = heading - (int)theta; //check to see if there is a turn greater than 180 degrees if (interceptAngle > 180) { interceptAngle -= 360; } if (interceptAngle < -180) { interceptAngle += 360; } return interceptAngle; // returns an angle between -180 and 180 in degrees } //returns the relative turret angle between -180 and 180 with 0 straight ahead public int relativeTurretAngle() { double turretAngleRadians = turretAngle; int turretAngleDegrees; int trueAngle; // determine my heading if( turretAngleRadians < 0 ) { turretAngleDegrees = (int)( 360 + ((( turretAngleRadians * 180 ) / Math.PI ) % 360 )); } else { turretAngleDegrees = (int)((( turretAngleRadians * 180 ) / Math.PI ) % 360 ); } //check to see if there is a turn greater than 180 degrees if (turretAngleDegrees > 180) { turretAngleDegrees -= 360; } if (turretAngleDegrees < -180) { turretAngleDegrees += 360; } // correct for the fact that turretAngle of 0 radians is relative 270 degrees // from tank heading trueAngle = turretAngleDegrees - 90; if( trueAngle < -180 ) { trueAngle = 360 + trueAngle; } if( trueAngle > 180 ) { trueAngle = 360 - trueAngle; } return trueAngle; }// end relativeTurretAngle public void checkForGroundCollision() { //tank doesn't check for collision with ground, it's on the ground!!! } // end checkforGroundCollision public void stopRun() { behaviorStreamBuffer.shutdown (); } public void checkForGunElevate() { if (raiseGun==true) { if ((elevationAngle + .065) > 0.80) { elevationAngle = 0.800; } else { elevationAngle = elevationAngle + 0.065; } ApMainGunElevation.setParameterValue(elevationAngle); // double, radians }//end if else if (lowerGun == true) { if ((elevationAngle - .065) < -0.20) { elevationAngle = -0.200; } else { elevationAngle = elevationAngle - 0.065; } ApMainGunElevation.setParameterValue (elevationAngle); // double, radians }//end else if } // end checkforGunElevate public void checkForTurretTurn() { if (turretLeft==true) { //turret turn rate = 0.1058 radians/time unit turretAngle = turretAngle - 0.1058; ApTurretRotation.setParameterValue( turretAngle ); // double, radians } else if (turretRight == true) { turretAngle = turretAngle + 0.1058; ApTurretRotation.setParameterValue( turretAngle ); // double, radians }//end else if } // end checkforTurretTurn public void checkForReload() { double xreload; double yreload; double xNought = espdu.getEntityLocationX(); // This is the entities double yNought = espdu.getEntityLocationY(); if (red == true) { xreload = 0; // mls yreload = 2000; } else { xreload = -5000; // mls yreload = 2000; } double distanceFromHome = Math.sqrt((xreload-xNought)*(xreload-xNought) + (yreload-yNought)*(yreload-yNought)); if (distanceFromHome < 100) { mypanel.resetAmmo(); } }//end check for reload public void selfDestruct() { 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 mypanel.reset(); reset(); // System.out.println("sending RemoveEntity PDU..."); // send notification to remove entity behaviorStreamBuffer.sendPdu(repdu.getExemplar(), ipAddress, portNumber); }//end selfDestruct public String getFrameHeaderInfo() { EntityID temp = espdu.getEntityID(); String tactics = new String(" "); if( offensive ) tactics = new String( " Offensive " ); if( defensive ) tactics = new String( " Defensive " ); return new String(espdu.getMarking () + tactics + " R71-630" + temp.getEntityID() + " (" + temp.getSiteID() + "/" + temp.getApplicationID() + "/" + temp.getEntityID() + ")"); } // end getFrameheaderInfo //============================================================================== // Button Methods - These methods are all called by TestFrame.java //============================================================================== public void reset() { if (red == true) { espdu.setEntityLocationX(-10f); // mls espdu.setEntityLocationY(1990f + id%35*50); espdu.setEntityOrientationPsi(-6.7f+(float)Math.PI); // mls espdu.setEntityLocationZ(36.3f); // mls } else { espdu.setEntityLocationX(-5010f); // mls espdu.setEntityLocationY(1990f + id%27*50); espdu.setEntityOrientationPsi(-6.7f); // mls espdu.setEntityLocationZ(40.5f); // mls } // mls 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; turretLeft = false; turretRight = false; raiseGun = false; lowerGun = false; turretAngle = 1.57; //in radians elevationAngle = 0.0; ApTurretRotation.setParameterValue(turretAngle); ApMainGunElevation.setParameterValue(elevationAngle); // double, radians isCollided = false; } // end reset() public void sendFirePdu() // coax { // round velocity = 1000 m/s fpdu.getBurstDescriptor().setWarhead(1630); fpdu.setLocationInWorldCoordinate((float)(espdu.getEntityLocationX()+ 0.0f), espdu.getEntityLocationY(), (float)(espdu.getEntityLocationZ()- 0.5f)); fpdu.setVelocity((float)(espdu.getEntityLinearVelocityX() + 1000 * Math.cos(espdu.getEntityOrientationPsi() + turretAngle - Math.PI/2.0)), (float)(espdu.getEntityLinearVelocityY() + 1000 * Math.sin(espdu.getEntityOrientationPsi() + turretAngle - Math.PI/2.0)), (float)(espdu.getEntityLinearVelocityZ() - 1000 * Math.sin(espdu.getEntityOrientationTheta() + elevationAngle))); // note turret elevationAngle is in VRML coordinate system, // and so is opposite to Theta fpdu.makeTimestampCurrent(); /* System.out.println("here is the data " + (float)(espdu.getEntityLinearVelocityX() + 1000 * Math.cos(espdu.getEntityOrientationPsi() + turretAngle - Math.PI/2.0))+ " " + (float)(espdu.getEntityLinearVelocityY() + 1000 * Math.sin(espdu.getEntityOrientationPsi() + turretAngle - Math.PI/2.0)) + " " + (float)(espdu.getEntityLinearVelocityZ() - 1000 * Math.sin(espdu.getEntityOrientationTheta() + elevationAngle))); */ fpdu.setRange(3000.0f); espdu.makeTimestampCurrent(); //send the fire pdu behaviorStreamBuffer.sendPdu(fpdu, ipAddress, portNumber); //send a current espdu behaviorStreamBuffer.sendPdu(espdu, ipAddress, portNumber); } // end sendFirePdu() public void sendMainFirePdu() { // System.out.println("Enter sendmainFire!!!*******"); // round velocity = 1000 m/s fpdu.getBurstDescriptor().setWarhead(1500); fpdu.setLocationInWorldCoordinate((float)(espdu.getEntityLocationX()+ 0.0f), espdu.getEntityLocationY(), (float)(espdu.getEntityLocationZ()- 0.5f)); fpdu.setVelocity((float)(espdu.getEntityLinearVelocityX() + 1000 * Math.cos(espdu.getEntityOrientationPsi() + turretAngle - Math.PI/2.0)), (float)(espdu.getEntityLinearVelocityY() + 1000 * Math.sin(espdu.getEntityOrientationPsi() + turretAngle - Math.PI/2.0)), (float)(espdu.getEntityLinearVelocityZ() - 1000 * Math.sin(espdu.getEntityOrientationTheta() + elevationAngle))); // note elevationAngle is in VRML coordinate system, so must be negated fpdu.makeTimestampCurrent(); /* System.out.println("here is the data " + (float)(espdu.getEntityLinearVelocityX() + 1000 * Math.cos(espdu.getEntityOrientationPsi() + turretAngle - Math.PI/2.0))+ " " + (float)(espdu.getEntityLinearVelocityY() + 1000 * Math.sin(espdu.getEntityOrientationPsi() + turretAngle - Math.PI/2.0)) + " " + (float)(espdu.getEntityLinearVelocityZ() - 1000 * Math.sin(espdu.getEntityOrientationTheta() + elevationAngle))); */ fpdu.setRange(3000.0f); espdu.makeTimestampCurrent(); //send the fire pdu behaviorStreamBuffer.sendPdu(fpdu, ipAddress, portNumber); //send a current espdu behaviorStreamBuffer.sendPdu(espdu, ipAddress, portNumber); } // end sendMainFirePdu() public void brake() //stop the tank { 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; } // end public brake() public void leftTurretTurn() { turretRight = false; if (turretLeft == true) { turretLeft = false; } else { turretLeft = true; } } // end leftpedalTurn() public void rightTurretTurn() { turretLeft = false; if (turretRight == true) { turretRight = false; } else { turretRight = true; } } // end rightPedalTurn() public void elevateGun() { lowerGun = false; if (raiseGun == true) { raiseGun = false; } else { raiseGun = true; } } // end elevateGun() public void depressGun() { raiseGun = false; if (lowerGun == true) { lowerGun = false; } else { lowerGun = true; } } // end depressGun() //============================================================================== // Kill Determination Methods //============================================================================== public void collectSensorPdus() { Vector pdus = behaviorStreamBuffer.receivedPdus(); 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); EntityStatePdu tempPDU = (EntityStatePdu)tempPdu; Integer espduID = new Integer(tempPDU.getEntityID().getEntityID().intValue()); //this adds the received PDUs into the HashMap using the espduID as //the Key value. Therefore only the most receint PDU is left in the //HashMap for the agent to look at in the sensed environment //check range and ID, if within sensor range or ID is 98 or 99 (flags) //then put it into the perceived environment (sensedPdus) sensedPdus.put(espduID, tempPdu); } // end if if (pduType.shortValue() == PduTypeField.DATA) { Vector receivedData = new Vector(); DataPdu dataPdu = (DataPdu)tempPdu; EntityID sender = dataPdu.getOriginatingEntityID(); Integer senderID = new Integer(sender.getEntityID().intValue()); // if this senderID is the same as me if(senderID.intValue() == espdu.getEntityID().getEntityID().intValue()) { // decode dataPdu and implement action DatumSpecification datum1 = dataPdu.getDatumInformation(); for( int kx = 0; kx < datum1.getFixedDatumCount(); kx++ ) { FixedDatum fixedDatum3 = datum1.fixedDatumAt(kx); Integer datumInteger = new Integer( fixedDatum3.getValue() ); receivedData.addElement(datumInteger); }// end for interpretProtocol( receivedData ); }// end if } //end else if } // end for } // end collect FirePdus public void interpretProtocol( Vector receivedData ) { // Determine protocol and set booleans to take the appropriate action int protocol = ((Integer)receivedData.firstElement()).intValue(); switch( protocol ) { case 0: int tacticalAction = ((Integer)receivedData.elementAt(1)).intValue(); if( tacticalAction == OFFENSIVE ) setOffensive(); if( tacticalAction == DEFENSIVE ) setDefensive(); break; // ADD ADDITIONAL PROTOCOL DEFINITIONS HERE default: break; // ignor this pdu, unknown actionProtocol }// end switch }// end processDataPdu() /** * 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 tank at launch. * 5. Get X, Y, Z location of this tank at launch. * 6. Calculate the distance between this tank and the launch point. * 7. Calculate the relative velocity of the fire with respect to this tank. * 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. */ 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 = espdu.getEntityLinearVelocityX(); double myVelocityAtLaunchY = espdu.getEntityLinearVelocityY(); double myVelocityAtLaunchZ = espdu.getEntityLinearVelocityZ(); 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) ); //End terrain hit control if(CPA <= HIT_RADIUS) //Terrain hit control { 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); // put thread to sleep for 250 ms, thus when // time to complete the loop (hopefully about } // end try // 50ms) is added the pdu send interval will catch(InterruptedException ite) { ite.printStackTrace(); } // end catch mypanel.reset(); reset(); }//end if }// end checkFirePdus() public void checkCollision(EntityStatePdu entityPdu ) { if( entityPdu.getEntityID().equals( espdu.getEntityID()) || entityPdu.getEntityID().getEntityID().intValue() == 99 || entityPdu.getEntityID().getEntityID().intValue() == 98) { // do nothing (don't collide with yourself or the flags) } else { // check for obstacle collision double[] entityMinusMe = new double[3]; entityMinusMe[0] = entityPdu.getEntityLocationX() - espdu.getEntityLocationX(); entityMinusMe[1] = entityPdu.getEntityLocationY() - espdu.getEntityLocationY(); entityMinusMe[2] = entityPdu.getEntityLocationZ() - espdu.getEntityLocationZ(); double distBtwn = entityMinusMe[0]*entityMinusMe[0] + entityMinusMe[1]*entityMinusMe[1] + entityMinusMe[2]*entityMinusMe[2]; // w/in 8 meters of each other if (distBtwn <= 256.0) { // System.out.println("Stopping Tank"); // System.out.println(entityPdu.getEntityID()); } //endif }//endif } //end checkCollision public double determineRange( EntityStatePdu firstEspu, EntityStatePdu secondEspdu ) { double range; double firstX = firstEspu.getEntityLocationX(); double firstY = firstEspu.getEntityLocationY(); double firstZ = firstEspu.getEntityLocationZ(); double secondX = secondEspdu.getEntityLocationX(); double secondY = secondEspdu.getEntityLocationY(); double secondZ = secondEspdu.getEntityLocationZ(); range = Math.sqrt((firstX-secondX)*(firstX-secondX) + (firstY-secondY)*(firstY-secondY) + (firstZ-secondZ)*(firstZ-secondZ)); return range; } } // end class AgentTankActionInterpreter