/**
* work out the perpendicular distance between me and the supplied line segment
*
* @param lineStart start point of the line
* @param lineEnd end point of the line
* @return perpendicular distance off track.
*/
protected WorldDistance perpendicularDistanceBetween(
final WorldLocation lineStart, final WorldLocation lineEnd) {
final Point2D pStart = new Point2D.Double(lineStart.getLong(), lineStart.getLat());
final Point2D pEnd = new Point2D.Double(lineEnd.getLong(), lineEnd.getLat());
final Point2D tgt = new Point2D.Double(this.getLong(), this.getLat());
final double res = distanceToSegment(pStart, pEnd, tgt);
final WorldDistance distance = new WorldDistance(res, WorldDistance.DEGS);
// Note: we were using an algorithm to calculate the dist to a point on a
// continuous line, not
// a line segment. The above code class the correct algorithm.
// // sort out known angles
// double thetaOne = lineEnd.bearingFrom(lineStart);
// double thetaTwo = Math.PI - lineStart.bearingFrom(this);
// double thetaThree = thetaOne + thetaTwo;
//
// // and the single known distance
// double rangeToP1 = lineStart.rangeFrom(this);
//
// // now do our trig.
// double sinThetaThree = Math.abs(Math.sin(thetaThree));
// WorldDistance distance = new WorldDistance(rangeToP1 * sinThetaThree,
// WorldDistance.DEGS);
// sorted.
return distance;
}
/**
* note that addToMe changes this object
*
* @param delta the offset to add to this point
*/
public void addToMe(final WorldVector delta) {
// check we have our model
if (_model == null) {
_model = new MWC.Algorithms.EarthModels.FlatEarth();
}
// do the calculation with our model
final WorldLocation res = _model.add(this, delta);
// update ourselves to the result
setLat(res.getLat());
setLong(res.getLong());
setDepth(res.getDepth());
}
public final void testConstructor() {
final WorldLocation v1 = new WorldLocation(12.225, 12.275, 12.5);
final WorldLocation v2 = new WorldLocation(12, 13.5, 0, 'N', 12, 16.5, 0, 'E', 12.5);
final WorldLocation v3 = new WorldLocation(12, 13, 30, 'N', 12, 16, 30, 'E', 12.5);
final WorldLocation v4 = new WorldLocation(w5);
assertTrue("v1 (d,d,d)", v1.equals(w5));
assertTrue("v2 (i,d,c,i,d,c,d)", v2.equals(w5));
assertTrue("v3 (i,i,d,c,i,i,d,c,d)", v3.equals(w5));
assertTrue("v4 (WorldLoc)", v4.equals(w5));
}
public final void setXXX() {
final double dep = w4.getDepth();
final double dLat = w4.getLat();
final double dLong = w4.getLong();
w1.setDepth(dep);
w1.setLat(dLat);
w1.setLong(dLong);
assertTrue(w1.equals(w4));
}
public final void testPerpDistanceFrom() {
WorldLocation.setModel(new CompletelyFlatEarth());
final WorldLocation me = new WorldLocation(7, 0, 0);
final WorldLocation p1 = new WorldLocation(4, 4, 0);
WorldLocation p2 = new WorldLocation(12, 4, 0);
WorldDistance res = me.perpendicularDistanceBetween(p1, p2);
assertEquals("off-track error is correct", 4.0, res.getValueIn(WorldDistance.DEGS), 0.001);
p2 = new WorldLocation(9, 2, 0);
res = me.perpendicularDistanceBetween(p1, p2);
assertEquals("off-track error is correct", 2.5997, res.getValueIn(WorldDistance.DEGS), 0.001);
p2 = new WorldLocation(-4, -4, 0);
res = me.perpendicularDistanceBetween(p1, p2);
assertEquals("off-track error is correct", 4.9497, res.getValueIn(WorldDistance.DEGS), 0.001);
res = me.rangeFrom(p1, p2);
assertEquals(
"off-track error is correct (using range from operator)",
4.9497,
res.getValueIn(WorldDistance.DEGS),
0.001);
}
/**
* create a new point that is this point rotated by set radians about the supplied axis
*
* @param pOrigin centre of rotation
* @param brg angle rotated through (radians)
* @return new location
*/
public final WorldLocation rotatePoint(final WorldLocation pOrigin, final double brg) {
final double resLong =
pOrigin.getLong()
+ (Math.cos((brg)) * (this.getLong() - pOrigin.getLong())
- Math.sin(brg) * (this.getLat() - pOrigin.getLat()));
final double resLat =
pOrigin.getLat()
+ (Math.sin((brg)) * (this.getLong() - pOrigin.getLong())
+ Math.cos(brg) * (this.getLat() - pOrigin.getLat()));
final WorldLocation res = new WorldLocation(resLat, resLong, 0d);
return res;
}
public final void testGetLong() {
assertEquals(w1.getLong(), 12.4, 0d);
}
public final void testGetLat() {
assertEquals(w1.getLat(), 12.3, 0d);
}
public final void testGetDepth() {
assertEquals(w1.getDepth(), 12.5, 0d);
}
public final void testBearingFrom() {
final double brg = w4.bearingFrom(w1);
assertTrue(brg == 0d);
}
public final void testAddToMe() {
w1.addToMe(wv1);
assertTrue(w1.equals(w4));
}
public final void testAdd() {
final WorldLocation ww = w1.add(wv1);
assertTrue(w4.equals(ww));
}
/** put our keywords into the XML description */
protected static String swapKeywords(
final DetectionEvent detection,
final Status currentLocation,
final String weapon,
final TargetType theTarget) {
// amend string template to include available parameters
final Float brg_degs = detection.getBearing();
final WorldDistance rng = detection.getRange();
// take a copy of the string
String working = new String(weapon);
// swap the bearing
if (brg_degs != null) {
final String brg_val = "" + brg_degs.floatValue();
working = replaceAll(working, "$BRG$", brg_val);
}
// swap the range
if (rng != null) {
final String rng_val = "" + rng.getValueIn(WorldDistance.YARDS);
working = replaceAll(working, "$RNG$", rng_val);
}
// insert the location of the target
if (brg_degs != null) {
final float brg_val = brg_degs.floatValue();
// do we know range?
if (rng != null) {
// yes, compute target location
final WorldVector newVector =
new WorldVector(
MWC.Algorithms.Conversions.Degs2Rads(brg_val),
rng.getValueIn(WorldDistance.DEGS),
0);
final WorldLocation newLoc = currentLocation.getLocation().add(newVector);
// produce strings from this location
final String theDepth = "" + newLoc.getDepth();
final String theLat = "" + newLoc.getLat();
final String theLong = "" + newLoc.getLong();
// put these strings into the new behaviour
working = replaceAll(working, "$TGT_DEPTH$", theDepth);
working = replaceAll(working, "$TGT_LAT$", theLat);
working = replaceAll(working, "$TGT_LONG$", theLong);
} else {
// no, send the weapon down a bearing for XXXX yds
// compute target location
final double TGT_RANGE = 5000;
final WorldVector newVector =
new WorldVector(
MWC.Algorithms.Conversions.Degs2Rads(brg_val),
MWC.Algorithms.Conversions.Yds2Degs(TGT_RANGE),
0);
final WorldLocation newLoc = currentLocation.getLocation().add(newVector);
// produce strings from this location
final String theDepth = "" + newLoc.getDepth();
final String theLat = "" + newLoc.getLat();
final String theLong = "" + newLoc.getLong();
// put these strings into the new behaviour
working = replaceAll(working, "$TGT_DEPTH$", theDepth);
working = replaceAll(working, "$TGT_LAT$", theLat);
working = replaceAll(working, "$TGT_LONG$", theLong);
}
}
if (theTarget != null) {
// output the XML header stuff
// output the plot
final java.io.StringWriter newString = new StringWriter();
// final com.sun.xml.tree.XmlDocument doc = new com.sun.xml.tree.XmlDocument();
DocumentBuilderFactory factory = DocumentBuilderFactory.newInstance();
Document doc = null;
try {
DocumentBuilder builder = factory.newDocumentBuilder();
doc = builder.newDocument();
final org.w3c.dom.Element type =
ASSET.Util.XML.Decisions.Util.TargetTypeHandler.getElement(theTarget, doc);
doc.appendChild(type);
doc.setNodeValue(type.getTagName());
// doc.changeNodeOwner(type);
// doc.setSystemId("ASSET XML Version 1.0");
// Use a Transformer for output
TransformerFactory tFactory = TransformerFactory.newInstance();
Transformer transformer = tFactory.newTransformer();
DOMSource source = new DOMSource(doc);
StreamResult result = new StreamResult(newString);
transformer.transform(source, result);
} catch (ParserConfigurationException e) {
e.printStackTrace(); // To change body of catch statement use Options | File Templates.
} catch (DOMException e) {
e.printStackTrace(); // To change body of catch statement use Options | File Templates.
} catch (TransformerFactoryConfigurationError transformerFactoryConfigurationError) {
transformerFactoryConfigurationError
.printStackTrace(); // To change body of catch statement use Options | File Templates.
} catch (TransformerException e) {
e.printStackTrace(); // To change body of catch statement use Options | File Templates.
}
// // ok, we should be done now
// try
// {
// doc.write(newString);
// }
// catch(java.io.IOException e)
// {
// e.printStackTrace();
// }
// try to extract the <target type portion
if (newString != null) {
final String val = newString.toString();
final String startIdentifier = "<TargetType";
final String endIdentifier = "</TargetType";
final int start = val.indexOf(startIdentifier);
final int end = val.lastIndexOf(endIdentifier);
final String detail = val.substring(start, end + endIdentifier.length() + 1);
// lastly, replace the string
working = replaceAll(working, "<TargetType/>", detail);
}
}
return working;
}
public final void setSubtract() {
final WorldVector wvv = w4.subtract(w1);
assertTrue(wvv.equals(wv1));
}
public final void testRangeFrom() {
final double rng = w4.rangeFrom(w1);
assertEquals(rng, 1.0, 0.0001d);
}
public void testDecimalConstructor() {
// test the secs component
WorldLocation worldLoc = new WorldLocation(22, 0, 45, 'N', 22, 0, 1.45, 'W', 0);
assertEquals("right lat", 22.0125, worldLoc.getLat(), 0.001);
assertEquals("right long", -22.000402, worldLoc.getLong(), 0.00001);
// now the mins component
worldLoc = new WorldLocation(22, 0.5, 00, 'N', 14, 0.5, 0, 'W', 0);
assertEquals("right lat", 22.008, worldLoc.getLat(), 0.01);
assertEquals("right long", -14.008333, worldLoc.getLong(), 0.00001);
// now the degs component
worldLoc = new WorldLocation(22.5, 0, 00, 'N', 14.5, 0, 0, 'W', 0);
assertEquals("right lat", 22.5, worldLoc.getLat(), 0.01);
assertEquals("right long", -14.5, worldLoc.getLong(), 0.00001);
//
// NOW LET'S REVERSE THE HEMISPHERE
// test the secs component
worldLoc = new WorldLocation(22, 0, 45, 'S', 22, 0, 1.45, 'E', 0);
assertEquals("right lat", -22.0125, worldLoc.getLat(), 0.001);
assertEquals("right long", 22.000402, worldLoc.getLong(), 0.00001);
// now the mins component
worldLoc = new WorldLocation(22, 0.5, 00, 'S', 14, 0.5, 0, 'E', 0);
assertEquals("right lat", -22.008, worldLoc.getLat(), 0.01);
assertEquals("right long", 14.008333, worldLoc.getLong(), 0.00001);
// now the degs component
worldLoc = new WorldLocation(22.5, 0, 00, 'S', 14.5, 0, 0, 'E', 0);
assertEquals("right lat", -22.5, worldLoc.getLat(), 0.01);
assertEquals("right long", 14.5, worldLoc.getLong(), 0.00001);
}
// TODO FIX-TEST
public void NtestLaunch() {
final double rngToHim = 5000;
final double crseToHim = 45;
final WorldLocation origin = new WorldLocation(0, 0, 0);
final WorldLocation hisLoc =
origin.add(
new WorldVector(
MWC.Algorithms.Conversions.Degs2Rads(crseToHim),
MWC.Algorithms.Conversions.Yds2Degs(rngToHim),
0));
final Status blueStat = new Status(13, 0);
blueStat.setLocation(origin);
blueStat.setCourse(0);
blueStat.setSpeed(new WorldSpeed(4, WorldSpeed.M_sec));
final Status redStat = new Status(blueStat);
redStat.setLocation(hisLoc);
// setup a couple of targets
final ASSET.Models.Vessels.SSN ssn = new ASSET.Models.Vessels.SSN(1);
ssn.setName("ssn");
ssn.setMovementChars(
SSMovementCharacteristics.generateDebug("scrap", 1, 1, 0, 20, 1, 300, 1, 1, 10, 100));
ssn.setCategory(
new ASSET.Participants.Category(
Category.Force.BLUE, Category.Environment.SUBSURFACE, Category.Type.SUBMARINE));
ssn.setStatus(blueStat);
final ASSET.Models.Sensor.Initial.OpticSensor periscope =
new ASSET.Models.Sensor.Initial.OpticSensor(12);
ssn.addSensor(periscope);
RadiatedCharacteristics rc = new RadiatedCharacteristics();
rc.add(EnvironmentType.VISUAL, new Optic(12, new WorldDistance(12, WorldDistance.METRES)));
ssn.setRadiatedChars(rc);
final ASSET.Models.Vessels.Surface su = new ASSET.Models.Vessels.Surface(4);
su.setName("su");
su.setMovementChars(
SurfaceMovementCharacteristics.generateDebug("scrap", 1, 1, 0.001, 14, 1, 299));
su.setCategory(
new ASSET.Participants.Category(
Category.Force.RED, Category.Environment.SURFACE, Category.Type.CARRIER));
su.setStatus(redStat);
rc = new RadiatedCharacteristics();
rc.add(EnvironmentType.VISUAL, new Optic(12, new WorldDistance(12, WorldDistance.METRES)));
su.setRadiatedChars(rc);
/** create the scenario */
final CoreScenario scenario = new CoreScenario();
scenario.setScenarioStepTime(10000);
scenario.setTime(0);
scenario.setName("Testing weapon launch");
// get somebody to listen to the tracks
final ASSET.Scenario.Observers.Recording.DebriefReplayObserver debrief_writer =
new ASSET.Scenario.Observers.Recording.DebriefReplayObserver(
"�test_reports", null, true, "test observer", true);
debrief_writer.setup(scenario);
// DON'T BOTHER RECORDING JUST YET!
debrief_writer.setActive(false);
/** now the behaviours */
final Trail trailRed = new Trail(new WorldDistance(2000, WorldDistance.YARDS));
final TargetType getRed = new TargetType();
getRed.addTargetType(Category.Force.RED);
trailRed.setTargetType(getRed);
final LaunchWeapon launchWeapon = new LaunchWeapon();
launchWeapon.setTargetType(getRed);
launchWeapon.setLaunchRange(new WorldDistance(4000, WorldDistance.YARDS));
launchWeapon.setCoolOffTime(new Duration(Duration.DAYS, 3)); // 24 hr cool off time
launchWeapon.setLaunchType(launchBehaviour);
final Waterfall blueWaterfall = new Waterfall();
blueWaterfall.insertAtFoot(trailRed);
blueWaterfall.insertAtHead(launchWeapon);
ssn.setDecisionModel(blueWaterfall);
scenario.addParticipant(1, ssn);
scenario.addParticipant(4, su);
// move forward a while
boolean found = false;
double lastRange = -1;
double thisRange = -1;
int counter = 0;
while (!found && counter <= 100000) {
scenario.step();
final WorldVector sep =
ssn.getStatus().getLocation().subtract(su.getStatus().getLocation());
thisRange = MWC.Algorithms.Conversions.Degs2Yds(sep.getRange());
if (lastRange == -1) {
lastRange = thisRange;
}
if (ssn.getActivity().equals(LaunchWeapon.LAUNCH_MESSAGE)) {
found = true;
System.out.println("LAUNCHED");
} else {
lastRange = thisRange;
}
// just do "MAD" check to ensure we don't go on for ever
counter++;
}
// check we didn't time out
assertTrue(" we just ran to end of loop (no fire)", counter < 100000);
// check we launched at the right point
final double launchYds = launchWeapon.getLaunchRange().getValueIn(WorldDistance.YARDS);
assertTrue("didn't fire too soon", lastRange > launchYds);
assertTrue(
"didn't fire too late, got:" + launchYds + " but wanted:" + thisRange,
thisRange <= launchYds);
// so, we've launched the weapon - see how many participants are in the scenario
assertEquals("New participant created", 3, scenario.getListOfParticipants().length);
// find the weapon
// final ParticipantType torpedo =
// scenario.getThisParticipant(scenario.getListOfParticipants()[0].intValue());
// check if the torpedo gets to the target
while (scenario.getListOfParticipants().length == 3) {
scenario.step();
}
// check that the torpedo destroys the SU, and they are both removed
assertEquals("surface participant destroyed", 1, scenario.getListOfParticipants().length);
// move a bit further just to give us some more blue track
for (int ii = 0; ii < 20; ii++) {
scenario.step();
}
}
public final void testEquals() {
assertTrue(w1.equals(w2));
assertTrue(!w1.equals(w5));
}
public final void testCopy() {
final WorldLocation w3 = new WorldLocation(0, 0, 0);
w3.copy(w2);
assertTrue(w2.equals(w3));
}
/**
* this method is called by the 'Create' function, and it fills in the buoys into the correct
* pattern
*/
protected final void addBuoys(Debrief.Wrappers.BuoyPatternWrapper pattern) {
WorldLocation origin = getKingpin();
// note that as we calculate the LH angle
double lh_orient_rads = MWC.Algorithms.Conversions.Degs2Rads(_orientation2);
double rh_orient_rads = MWC.Algorithms.Conversions.Degs2Rads(_orientation1);
double spacing_degs = MWC.Algorithms.Conversions.Nm2Degs(_spacing);
// how many bouys in each leg?
// an even number means we don't have one at the tip, which becomes a special case
int num_buoys = getNumberOfBuoys().intValue();
boolean even_num = false;
if ((num_buoys % 2) == 0) {
even_num = true;
}
// sort out how many there are in each leg
int num_in_leg = num_buoys / 2;
// sort out the direction
double this_orient = rh_orient_rads;
int buoy_counter = 0;
// remember that we are looking at the first buoy
boolean first_buoy = true;
// do the first leg
for (int i = 0; i < num_in_leg; i++) {
// create the new symbol
Debrief.Wrappers.LabelWrapper lw =
new Debrief.Wrappers.LabelWrapper("W" + (buoy_counter + 1), origin, java.awt.Color.red);
buoy_counter++;
// get the parent to do the formatting
this.formatSymbol(lw, pattern);
// if this is the first buoy, mark it as the kingping
if (first_buoy) {
lw.setSymbolType("Kingpin");
first_buoy = false;
}
// create the step to use to get to the next buoy
WorldVector thisStep = new MWC.GenericData.WorldVector(this_orient, spacing_degs, 0.0);
// place buoy
origin = origin.add(thisStep);
}
// if we have an even number, we need to move forward 1/2 distance for one more step before we
// change direction
if (even_num) {
// calculate the size of this small step
double reverse_rh_rads = MWC.Algorithms.Conversions.Degs2Rads(_orientation1 + 180.0);
WorldVector short_hop = new WorldVector(reverse_rh_rads, spacing_degs / 2, 0.0);
// move the origin forward
origin = origin.add(short_hop);
// calculate the size of this small step
short_hop = new WorldVector(lh_orient_rads, spacing_degs / 2, 0.0);
// move the origin forward
origin = origin.add(short_hop);
} else {
// drop a buoy at the current point
Debrief.Wrappers.LabelWrapper lw =
new Debrief.Wrappers.LabelWrapper("W" + (buoy_counter + 1), origin, java.awt.Color.red);
buoy_counter++;
// move to the correct location for the next point
WorldVector short_hop = new WorldVector(lh_orient_rads, spacing_degs, 0.0);
// move the origin forward
origin = origin.add(short_hop);
// get the parent to do the formatting
this.formatSymbol(lw, pattern);
}
// now travel back down the reverse side
// sort out the direction
this_orient = lh_orient_rads;
// do the first leg
for (int i = 0; i < num_in_leg; i++) {
// create the new symbol
Debrief.Wrappers.LabelWrapper lw =
new Debrief.Wrappers.LabelWrapper("W" + (buoy_counter + 1), origin, java.awt.Color.red);
buoy_counter++;
// get the parent to do the formatting
this.formatSymbol(lw, pattern);
// create the step to use to get to the next buoy
WorldVector thisStep = new MWC.GenericData.WorldVector(this_orient, spacing_degs, 0.0);
// start moving down the return leg
// move buoy
origin = origin.add(thisStep);
}
}
/**
* decide the course of action to take, or return null to no be used
*
* @param status the current status of the participant
* @param detections the current list of detections for this participant
* @param time the time this decision was made
*/
public DemandedStatus decide(
final Status status,
ASSET.Models.Movement.MovementCharacteristics chars,
DemandedStatus demStatus,
final DetectionList detections,
final ASSET.Scenario.ScenarioActivityMonitor monitor,
final long time) {
// produce a steady-state demanded course - so we continue
// what we're doing during the weapons launch
DemandedStatus res = null;
// clear the activity flag
String activity = "Not in trail";
// is it time to fire another yet?
if ((_lastLaunch == -1)
|| (time > _lastLaunch + _coolOffTime.getValueIn(Duration.MILLISECONDS))) {
// do we have any detections?
if (detections != null) {
// get bearing to first detection
final int len = detections.size();
if (len > 0) {
for (int i = 0; i < len; i++) {
final ASSET.Models.Detection.DetectionEvent de = detections.getDetection(i);
final Float brg = de.getBearing();
if (brg != null) {
// do we have a target type?
if (_myTarget != null) {
// is this of our target type
final ASSET.Participants.Category thisTarget = de.getTargetType();
if (_myTarget.matches(thisTarget)) {
// do we have range?
if (de.getRange() != null) {
// work out distance from us to the target, not from the sensor to the target
WorldLocation sensorLocation = de.getSensorLocation();
// Work out the estimated target location
WorldVector sensorToTarget =
new WorldVector(
MWC.Algorithms.Conversions.Degs2Rads(de.getBearing().doubleValue()),
de.getRange().getValueIn(WorldDistance.DEGS),
0);
WorldLocation targetLocation = sensorLocation.add(sensorToTarget);
// how are are we from the target location
WorldVector meToTarget = status.getLocation().subtract(targetLocation);
double yds_to_target =
MWC.Algorithms.Conversions.Degs2Yds(meToTarget.getRange());
double brg_to_target_degs =
MWC.Algorithms.Conversions.Rads2Degs(meToTarget.getBearing());
// is it within range?
if (yds_to_target < _launchRange.getValueIn(WorldDistance.YARDS)) {
// continue in steady state
res = new SimpleDemandedStatus(time, status);
// remember the launch time
_lastLaunch = time;
// start the launch steps
launchWeapon(de, monitor, status, brg_to_target_degs, time);
activity = LaunchWeapon.LAUNCH_MESSAGE;
// ok, drop out, we don't need to launch any more weapons
return res;
}
}
}
}
} // if we know the bearing
} // looping through the detections
} // if we have any detections
} // if the detections object was received
} // whether it's time to launch another
else {
//
}
super.setLastActivity(activity);
// always return null, since we continue in steady state
return res;
}
