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.*; public class HumanActionInterpreterSingle extends Thread { private float pduSendInterval, // time delay between pdus speed; // not velocity, no directional information private DecimalFormat df; // pretty print screen output 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 networkInterface; // tie in to the network private DetonationPdu dpdu; private String ipAddress; private int portNumber; private int id; private boolean isCollided; private HumanControlPanel mypanel; private boolean turretLeft; private boolean turretRight; private boolean raiseGun; private boolean lowerGun; private double behaviorCode; private double elevationAngle; //in radians private ArticulationParameter ApBehavior; private ArticulationParameter ApCycleTime; private boolean red; private boolean driving; private CollisionDetectionTerrainReader terrain; /** * Meters above local terrain for entity's geometric center. */ public double verticalOffset = 0.0; /** * Behavior code. */ public final double NOTHING = 0.0; /** * Behavior code. */ public final double STAND = 1.0; /** * Behavior code. */ public final double WALK = 2.0; /** * Behavior code. */ public final double RUN = 3.0; /** * Behavior code. */ public final double JUMP = 4.0; /** * Behavior code. */ public final double KNEEL = 5.0; //============================================================================================= // Constructors & Start //============================================================================================= //==================================================================== public HumanActionInterpreterSingle( String ipAddr, int portNum, int timeToLive, short siteNum, short appNum, short idNum, String marking, boolean rtpHeaderEnabled, HumanControlPanel panel) { speed = 0.0f; // initially helo on ground with no fwd airspeed pduSendInterval = 0.5f; // approx 500 ms between pdu sends df = new DecimalFormat("###.00"); espdu = new EntityStatePdu(); // instantiate an espdu, used as a data structure espdu.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); driving = true; id = idNum; // instantiate a fire Pdu, used as data structure if (id < 30){ red = false;} else { red = true;} 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 - verticalOffset); // mls } // end if else { espdu.setEntityLocationX(-5010f); // mls espdu.setEntityLocationY(1990f + id%27 *50); espdu.setEntityOrientationPsi(-6.7f); // mls espdu.setEntityLocationZ(40.5f - verticalOffset); // mls } // end else short[] entityID = new short[3]; entityID[0] = siteNum; entityID[1] = appNum; entityID[2] = (short)id; espdu.setEntityID(entityID[0], entityID[1], entityID[2]); // set entity id triplet into fpdu.setFiringEntityID(espdu.getEntityID()); // the espdu and fie pdu copdu.setIssuingEntityID(espdu.getEntityID()); // the espdu and fie pdu // instantiate a BehaviorStreamBuffer to handle the network interface, instantiate with // the multicast IP address and port EntityID temp = espdu.getEntityID(); // elevationAngle = 0.0; ApBehavior = new ArticulationParameter(); // ApMainGunElevation = new ArticulationParameter(); ApBehavior.setParameterTypeDesignator(ParameterTypeDesignatorField.ARTICULATEDPART); // 0 // ApMainGunElevation.setParameterTypeDesignator(ParameterTypeDesignatorField.ARTICULATEDPART); ApBehavior.setChangeIndicator(0); // sequentially increment with each change in value // ApMainGunElevation.setChangeIndicator(0); ApBehavior.setArticulationAttachmentID (0); // attached to main entity, not multiply articulated // ApMainGunElevation.setArticulationAttachmentID(1); // attached to turret (which we are calling #1) ApBehavior.setParameterType (4096 + 15); // Primary turret number 1 + Rotation // ApMainGunElevation.setParameterType(4416 + 13); // Primary gun number 1 + Elevation behaviorCode = NOTHING; ApBehavior.setParameterValue( behaviorCode ); // // ApMainGunElevation.setParameterValue( 0 ); // double, radians espdu.addArticulationParameter(ApBehavior); // espdu.addArticulationParameter(ApMainGunElevation); mypanel = panel; isCollided = false; ipAddress = ipAddr; portNumber = portNum; System.out.println ("marking=" + marking + ", timeToLive=" + timeToLive); espdu.setMarking (marking); networkInterface = new BehaviorStreamBufferUDP (ipAddress, portNumber); networkInterface.setTimeToLive (timeToLive); Thread pduReader = new Thread(networkInterface); pduReader.start(); // read in Fort Irwin Terrain file terrain = new CollisionDetectionTerrainReader(); terrain.parseFile(); } // end constructor public void run() {startDriving();} // start sending espdus, called from TestFrame.java // calls flying which is an infinite espdu sending loop //============================================================================================= // 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()))); ///////Add behavior settings here////////////////////////////////////////////////////////////////////// if ( speed == 0 ) { behaviorCode = STAND; } else if (( speed > 0 ) && ( speed <= 4 )) { behaviorCode = WALK; } else { behaviorCode = RUN; // need JUMP if just turning... } // end else ApBehavior.setParameterValue (( double ) behaviorCode ); } // 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 its // own 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); System.out.println("** here is the (x,y) ** = " + newXLocation + " " + newYLocation); // System.out.println("** here is the Normal ** = " + // normalHeight[0] + " " + normalHeight[1] + " " + normalHeight[2]); // System.out.println("** here is the Height ** = " + normalHeight[3]); // espdu.setEntityLocationZ(espdu.getEntityLocationZ() + // espdu.getEntityLinearVelocityZ() * pduSendInterval); espdu.setEntityLocationZ(normalHeight[3] - verticalOffset); // negative number offset = m above local terrain // update linear velocities espdu.setEntityLinearVelocityX((float)(speed * Math.cos(espdu.getEntityOrientationPsi()))); espdu.setEntityLinearVelocityY((float)(speed * Math.sin(espdu.getEntityOrientationPsi()))); // update orientations // update orientations // System.out.println("** here is the theta ** = " + thetaAngle); espdu.setEntityOrientationTheta(thetaAngle); // System.out.println("** here is the phi ** = " + phiAngle); espdu.setEntityOrientationPhi(phiAngle); // espdu.setEntityOrientationPhi(espdu.getEntityOrientationPhi() + // espdu.getEntityAngularVelocityX() * pduSendInterval); espdu.setEntityOrientationPsi(espdu.getEntityOrientationPsi() + espdu.getEntityAngularVelocityZ() * pduSendInterval); // espdu.setEntityOrientationTheta(espdu.getEntityOrientationTheta() + // espdu.getEntityAngularVelocityY() * pduSendInterval); checkForTurretTurn(); checkForGunElevate(); checkForGroundCollision(); checkForReload(); // for display in the tank control panel double newHeading = espdu.getEntityOrientationPsi(); mypanel.updateHeading( newHeading ); // mypanel.updateGunElev( elevationAngle ); // send espdu to the specified multicast address espdu.makeTimestampCurrent(); networkInterface.sendPdu(espdu, ipAddress, portNumber); collectFirePdus(); 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 } // end while } // end startDriving() public void checkForGroundCollision(){ //tank doesn't check for collision with ground, it's on the ground!!! } // end checkforGroundCollision public void stopRun(){ networkInterface.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){ // 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 = 2000; yreload = 2000; } else { xreload = 1000; 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(); networkInterface.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(); networkInterface.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 ); networkInterface.sendPdu(espdu, ipAddress, portNumber); isCollided = true; try{ Thread.sleep(4500); } // end try catch(InterruptedException ite){ ite.printStackTrace(); } // end catch mypanel.reset(); reset(); }//end selfDestruct public String getFrameHeaderInfo(){ EntityID temp = espdu.getEntityID(); return new String("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 - verticalOffset); // mls } else { espdu.setEntityLocationX(-5010f); // mls espdu.setEntityLocationY(1990f + id%27*50); espdu.setEntityOrientationPsi(-6.7f); // mls espdu.setEntityLocationZ(40.5f - verticalOffset); // 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; // boolean 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 // much more work needs to be done here (overall) fpdu.setLocationInWorldCoordinate(espdu.getEntityLocationX() + 2.35, espdu.getEntityLocationY() + 0.5, espdu.getEntityLocationZ() + 0.6); 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()))); fpdu.makeTimestampCurrent(); fpdu.setRange(3000.0f); networkInterface.sendPdu(fpdu, ipAddress, portNumber); espdu.makeTimestampCurrent(); networkInterface.sendPdu(espdu, ipAddress, portNumber); try{ Thread.sleep(50); } // end try catch (InterruptedException ie){} } // end sendFirePdu() //public void sendMainFirePdu(){ //System.out.println("Enter sendmainFire!!!*******"); // round velocity = 1000 m/s // 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(); // networkInterface.sendPdu(fpdu, ipAddress, portNumber); //send the fire pdu // networkInterface.sendPdu(espdu, ipAddress, portNumber);//send a current espdu // } // 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 kneel () { behaviorCode = KNEEL; ApBehavior.setParameterValue (( double ) behaviorCode ); } // end kneel 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 collectFirePdus(){ System.out.println("\nEntering collectFirePdus()"); Vector firedPdus = new Vector(); Vector pdus = networkInterface.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 //public void checkFirePdu(FirePdu shot){ // // double proximityOfKillDistance = 25.0;//meters from target that still kills // WorldCoordinate launchLocation = shot.getLocationInWorldCoordinate(); // LinearVelocity projectileVelocity = shot.getVelocity(); // // double xNought = espdu.getEntityLocationX(); // This is the entities // double yNought = espdu.getEntityLocationY(); // current location // double zNought = espdu.getEntityLocationZ(); // // double xLaunch = launchLocation.getX(); // The position the projectile // double yLaunch = launchLocation.getY(); // was launched from // double zLaunch = launchLocation.getZ(); // // double vLaunchX = projectileVelocity.getX(); // The projectile's velocity // double vLaunchY = projectileVelocity.getY(); // at launch // double vLaunchZ = projectileVelocity.getZ(); // // double vLaunchMagnitude = Math.sqrt(vLaunchX*vLaunchX + vLaunchY*vLaunchY); // double vLaunchTheta = Math.atan2 (vLaunchY, vLaunchX); // double targetTheta = Math.atan2 ((yNought-yLaunch), (xNought-xLaunch)); // // // ensure vLaunchTheta, targetTheta positive (0..2pi) since range atan2 is (-pi .. pi) // if (vLaunchTheta < 0.0) vLaunchTheta += 2.0 * Math.PI; // if (targetTheta < 0.0) targetTheta += 2.0 * Math.PI; // // double horizontalLaunchRange = Math.sqrt((xLaunch-xNought)*(xLaunch-xNought) + (yLaunch-yNought)*(yLaunch-yNought)); // // if (horizontalLaunchRange > shot.getRange()) // { // System.out.println("no hit, out of range"); // return; // } // // double bulletFlightTime = horizontalLaunchRange/vLaunchMagnitude; // double bulletAtTargetRangeX = xLaunch + (vLaunchX * bulletFlightTime); // double bulletAtTargetRangeY = yLaunch + (vLaunchY * bulletFlightTime); // if (proximityOfKillDistance*proximityOfKillDistance < // ( (bulletAtTargetRangeX- xNought) * (bulletAtTargetRangeX- xNought) // + (bulletAtTargetRangeY- yNought) * (bulletAtTargetRangeY- yNought))){ // // return; // no hit // } // System.out.println("Ack, You got me partner!!!*******"); // copdu.setCollisionType(new UnsignedByte(CollisionTypeField.INELASTIC)); // copdu.setLinearVelocity(0.0f, 0.0f, 0.0f); // copdu.makeTimestampCurrent(); // networkInterface.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(); // networkInterface.sendPdu(dpdu, ipAddress, portNumber); // // isCollided = true; // try{ // Thread.sleep(4500); // } // end try // catch(InterruptedException ite){ // ite.printStackTrace(); // } // end catch // mypanel.reset(); // reset(); // } // end 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 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 /* boolean isFirePDUCollidedWithTerrain = false; double collisionCheckInterval = 0.1; double currentFirePduLocationX = launchLocationX; double currentFirePduLocationY = launchLocationY; double currentFirePduLocationZ = launchLocationZ; CollisionDetectionTerrainReader terrainAtCurrentFirePduLocation = new CollisionDetectionTerrainReader(); double [] normalHeight = new double [4]; for(float ix = 0; ix <= timeToTravel; ix += collisionCheckInterval) { currentFirePduLocationX += launchVelocityX * collisionCheckInterval; currentFirePduLocationY += launchVelocityY * collisionCheckInterval; currentFirePduLocationZ += launchVelocityZ * collisionCheckInterval; terrainAtCurrentFirePduLocation.getNormalHeight( currentFirePduLocationX, currentFirePduLocationY, normalHeight); if(currentFirePduLocationZ >= normalHeight[3]) {//bullet hits terrain isFirePDUCollidedWithTerrain = true; System.out.println("\nFire hits the terrain\n"); break; }//end if }//end for if (!isFirePDUCollidedWithTerrain){ */ // copdu.setLocation(espdu.getEntityLocation()); copdu.setCollisionType(new UnsignedByte(CollisionTypeField.INELASTIC)); copdu.setLinearVelocity(0.0f, 0.0f, 0.0f); copdu.makeTimestampCurrent(); networkInterface.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(); networkInterface.sendPdu(dpdu, ipAddress, portNumber); isCollided = true; try{ Thread.sleep(4500); // put thread to sleep for 250 ms, thus when //Thread.sleep(200); // 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 if }// end checkFirePdus() public void checkCollision(EntityStatePdu entityPdu ) { if (entityPdu.getEntityID().equals( espdu.getEntityID()) || entityPdu.getEntityID().getEntityID().intValue() == 99 || entityPdu.getEntityID().getEntityID().intValue() == 98){} 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()); // *** mypanel.brake(); } //endif }//endif } //end checkCollision } // end class MotionInterpreter