public void handleUpdateWorldNode(long oid, WorldManagerClient.UpdateWorldNodeMessage wnodeMsg) {
PerceiverData perceiverData = perceiverDataMap.get(oid);
if (perceiverData == null) {
if (Log.loggingDebug)
Log.debug(
"ProximityTracker.handleMessage: ignoring updateWNMsg for oid "
+ oid
+ " because PerceptionData for oid not found");
return;
}
BasicWorldNode bwnode = wnodeMsg.getWorldNode();
if (Log.loggingDebug)
Log.debug(
"ProximityTracker.handleMessage: UpdateWnode for "
+ oid
+ ", loc "
+ bwnode.getLoc()
+ ", dir "
+ bwnode.getDir());
if (perceiverData.wnode != null) {
perceiverData.previousLoc = perceiverData.lastLoc;
perceiverData.wnode.setDirLocOrient(bwnode);
perceiverData.wnode.setInstanceOid(bwnode.getInstanceOid());
perceiverData.lastLoc = perceiverData.wnode.getLoc();
} else
Log.error(
"ProximityTracker.handleMessage: In UpdateWorldNodeMessage for oid "
+ oid
+ ", perceiverData.wnode is null!");
updateEntity(perceiverData);
}
public void handleMessage(Message msg, int flags) {
try {
lock.lock();
if (activated == false) return; // return true;
if (msg.getMsgType() == Behavior.MSG_TYPE_COMMAND) {
Behavior.CommandMessage cmdMsg = (Behavior.CommandMessage) msg;
String command = cmdMsg.getCmd();
// Remove the executor, because anything we do will
// end the current execution.
Engine.getExecutor().remove(this);
if (Log.loggingDebug)
Log.debug(
"BaseBehavior.onMessage: command = "
+ command
+ "; oid = "
+ obj.getOid()
+ "; name "
+ obj.getName());
if (command.equals(MSG_CMD_TYPE_GOTO)) {
GotoCommandMessage gotoMsg = (GotoCommandMessage) msg;
Point destination = gotoMsg.getDestination();
mode = MSG_CMD_TYPE_GOTO;
roamingBehavior = true;
gotoSetup(destination, gotoMsg.getSpeed());
} else if (command.equals(MSG_CMD_TYPE_STOP)) {
followTarget = null;
pathState.clear();
obj.getWorldNode().setDir(new MVVector(0, 0, 0));
obj.updateWorldNode();
mode = MSG_CMD_TYPE_STOP;
// If roamingBehavior is set, that means that we
// used formerly had a roaming behavior, so send
// an ArrivedEventMessage so that the other
// behavior starts up again.
if (roamingBehavior) {
try {
Engine.getAgent().sendBroadcast(new ArrivedEventMessage(obj));
} catch (Exception e) {
Log.error(
"BaseBehavior.onMessage: Error sending ArrivedEventMessage, error was '"
+ e.getMessage()
+ "'");
throw new RuntimeException(e);
}
}
} else if (command.equals(BaseBehavior.MSG_CMD_TYPE_FOLLOW)) {
FollowCommandMessage followMsg = (FollowCommandMessage) msg;
mode = MSG_CMD_TYPE_FOLLOW;
followSetup(followMsg.getTarget(), followMsg.getSpeed());
}
} else if (msg.getMsgType() == WorldManagerClient.MSG_TYPE_MOB_PATH_CORRECTION) {
Engine.getExecutor().remove(this);
interpolatePath();
interpolatingPath = false;
}
// return true;
} finally {
lock.unlock();
}
}
public void addTrackedPerceiver(
Long perceiverOid, InterpolatedWorldNode wnode, Integer reactionRadius) {
lock.lock();
try {
if (perceiverDataMap.containsKey(perceiverOid)) {
// Don't add the object more than once.
Log.error(
"ProximityTracker.addTrackedPerceiver: perceiverOid "
+ perceiverOid
+ " is already in the set of local objects, for ProximityTracker instance "
+ this);
return;
}
PerceiverData perceiverData = new PerceiverData(perceiverOid, reactionRadius, wnode);
perceiverDataMap.put(perceiverOid, perceiverData);
} finally {
lock.unlock();
}
if (Log.loggingDebug)
Log.debug(
"ProximityTracker.addTrackedPerceiver: perceiverOid="
+ perceiverOid
+ " reactionRadius="
+ reactionRadius
+ " instanceOid="
+ instanceOid);
}
protected void performNotification(
long perceiverOid, long perceivedOid, boolean inRadius, boolean wasInRadius) {
if (Log.loggingDebug)
Log.debug(
"ProximityTracker.performNotification: perceiverOid "
+ perceiverOid
+ ", perceivedOid "
+ perceivedOid
+ ", inRadius "
+ inRadius
+ ", wasInRadius "
+ wasInRadius);
if (notifyCallback != null) {
notifyCallback.notifyReactionRadius(perceivedOid, perceiverOid, inRadius, wasInRadius);
notifyCallback.notifyReactionRadius(perceiverOid, perceivedOid, inRadius, wasInRadius);
} else {
ObjectTracker.NotifyReactionRadiusMessage nmsg =
new ObjectTracker.NotifyReactionRadiusMessage(
perceivedOid, perceiverOid, inRadius, wasInRadius);
Engine.getAgent().sendBroadcast(nmsg);
nmsg =
new ObjectTracker.NotifyReactionRadiusMessage(
perceiverOid, perceivedOid, inRadius, wasInRadius);
Engine.getAgent().sendBroadcast(nmsg);
}
}
protected boolean interpolatePath() {
long timeNow = System.currentTimeMillis();
PathLocAndDir locAndDir = pathState.interpolatePath(timeNow);
long oid = obj.getOid();
// if (locAndDir != null) {
// if (Log.loggingDebug) {
// Log.debug("BaseBehavior.interpolatePath: oid = " + oid + "; loc = " +
// locAndDir.getLoc() + "; dir = " + locAndDir.getDir());
// }
// }
// else {
// if (Log.loggingDebug)
// Log.debug("BaseBehavior.interpolatePath: oid = " + oid + "; locAndDir is
// null");
// }
if (locAndDir == null) {
// We have arrived - - turn off interpolation, and cancel that path
interpolatingPath = false;
if (Log.loggingDebug)
Log.debug(
"BaseBehavior.interpolatePath: cancelling path: oid = "
+ oid
+ "; myLoc = "
+ obj.getWorldNode().getLoc());
cancelPathInterpolator(oid);
obj.getWorldNode().setDir(new MVVector(0, 0, 0));
} else {
obj.getWorldNode()
.setPathInterpolatorValues(
timeNow, locAndDir.getDir(), locAndDir.getLoc(), locAndDir.getOrientation());
MobManagerPlugin.getTracker(obj.getInstanceOid()).updateEntity(obj);
}
return interpolatingPath;
}
protected boolean maybeAddPerceivedObject(PerceptionMessage.ObjectNote objectNote) {
ObjectType objType = (ObjectType) objectNote.getObjectType();
long perceivedOid = objectNote.getSubject();
long perceiverOid = objectNote.getTarget();
if (perceivedOid == perceiverOid) return true;
boolean callbackNixedIt = false;
if (remoteObjectFilter != null)
callbackNixedIt = !remoteObjectFilter.objectShouldBeTracked(perceivedOid, objectNote);
if (callbackNixedIt || !(objType.isMob())) {
// if (Log.loggingDebug)
// Log.debug("ProximityTracker.maybeAddPerceivedObject: ignoring oid=" +
// perceivedOid
// + " objType=" + objType
// + " detected by " + perceiverOid
// + ", instanceOid=" + instanceOid);
return false;
}
if (Log.loggingDebug)
Log.debug(
"ProximityTracker.maybeAddPerceivedObject: oid="
+ perceivedOid
+ " objType="
+ objType
+ " detected by "
+ perceiverOid
+ ", instanceOid="
+ instanceOid);
lock.lock();
try {
PerceiverData perceiverData = perceiverDataMap.get(perceiverOid);
if (perceiverData == null) {
Log.error(
"ProximityTracker.maybeAddPerceivedObject: got perception msg with perceived obj oid="
+ perceivedOid
+ " for unknown perceiver="
+ perceiverOid);
return false;
}
perceiverData.perceivedOids.add(perceivedOid);
PerceiverData perceivedData = perceiverDataMap.get(perceivedOid);
if (perceivedData != null) testProximity(perceiverData, perceivedData, true, false);
} finally {
lock.unlock();
}
return true;
}
public void run() {
while (running) {
try {
update();
} catch (MVRuntimeException e) {
Log.exception("ProximityTracker.Updater.run caught MVRuntimeException", e);
} catch (Exception e) {
Log.exception("ProximityTracker.Updater.run caught exception", e);
}
try {
Thread.sleep(1000);
} catch (InterruptedException e) {
Log.warn("Updater: " + e);
e.printStackTrace();
}
}
}
public void removeTrackedPerceiver(Long perceiverOid) {
lock.lock();
try {
// Iterate over perceived objects, removing our
// perceiverOid from their oid sets.
PerceiverData perceiverData = perceiverDataMap.get(perceiverOid);
if (perceiverData != null) {
if (Log.loggingDebug)
Log.debug(
"ProximityTracker.removeTrackedPerceiver: perceiverOid "
+ perceiverOid
+ ", inRangeOids count "
+ perceiverData.inRangeOids.size());
// Iterate over perceived objects, removing our
// perceiverOid from their oid sets.
for (Long perceivedOid : perceiverData.perceivedOids) {
PerceiverData perceivedData = perceiverDataMap.get(perceivedOid);
if (perceivedData != null) {
perceivedData.perceivedOids.remove(perceiverOid);
if (perceivedData.inRangeOids.contains(perceiverOid)) {
perceivedData.inRangeOids.remove(perceiverOid);
performNotification(perceiverOid, perceivedOid, false, true);
}
}
}
perceiverData.perceivedOids.clear();
perceiverData.inRangeOids.clear();
perceiverDataMap.remove(perceiverOid);
} else
Log.warn(
"ProximityTracker.removeTrackedPerceiver: For oid="
+ perceiverOid
+ ", didn't find PerceiverData");
} finally {
lock.unlock();
}
if (Log.loggingDebug)
Log.debug(
"ProximityTracker.removeTrackedPerceiver: oid="
+ perceiverOid
+ " instanceOid="
+ instanceOid);
}
public void gotoSetup(Point dest, int speed) {
// calculate the vector to the destination
destLoc = dest;
mobSpeed = speed;
Point myLoc = obj.getWorldNode().getLoc();
long oid = obj.getOid();
if (Log.loggingDebug)
Log.debug("BaseBehavior.gotoSetup: oid = " + oid + "; myLoc = " + myLoc + "; dest = " + dest);
scheduleMe(
setupPathInterpolator(oid, myLoc, dest, false, obj.getWorldNode().getFollowsTerrain()));
}
public void followUpdate() {
ObjectStub followObj = (ObjectStub) followTarget.getEntity(Namespace.MOB);
Point followLoc = followObj.getWorldNode().getLoc();
InterpolatedWorldNode node = obj.getWorldNode();
Point myLoc = node.getLoc();
long oid = obj.getOid();
float fdist = Point.distanceTo(followLoc, destLoc);
float dist = Point.distanceTo(followLoc, myLoc);
if (Log.loggingDebug)
Log.debug(
"BaseBehavior.followUpdate: oid = "
+ oid
+ "; myLoc = "
+ myLoc
+ "; followLoc = "
+ followLoc
+ "; fdist = "
+ fdist
+ "; dist = "
+ dist);
long msToSleep = (long) 500;
// If the new target location is more than a meter from
// the old one, create a new path.
if (fdist > 1000) {
long msToDest = setupPathInterpolator(oid, myLoc, followLoc, true, node.getFollowsTerrain());
destLoc = followLoc;
msToSleep = msToDest == 0 ? (long) 500 : Math.min((long) 500, msToDest);
}
// Else if we're interpolating, interpolate the current path
else if (interpolatingPath) {
interpolatePath();
if (Log.loggingDebug)
Log.debug(
"baseBehavior.followUpdate: oid = "
+ oid
+ "; interpolated myLoc = "
+ obj.getWorldNode().getLoc());
}
scheduleMe(interpolatingPath ? msToSleep : pathState.pathTimeRemaining());
}
public void run() {
try {
lock.lock();
if (activated == false) {
return;
}
try {
if (mode == MSG_CMD_TYPE_GOTO) {
gotoUpdate();
} else if (mode == MSG_CMD_TYPE_FOLLOW) {
followUpdate();
} else if (mode == MSG_CMD_TYPE_STOP) {
} else {
Log.error("BaseBehavior.run: invalid mode");
}
} catch (Exception e) {
Log.exception("BaseBehavior.run caught exception raised during run for mode = " + mode, e);
throw new RuntimeException(e);
}
} finally {
lock.unlock();
}
}
protected void update() {
Log.debug("Updater.update: in update");
List<Long> perceiverOids = null;
lock.lock();
try {
perceiverOids = new ArrayList<Long>(perceiverDataMap.keySet());
} finally {
lock.unlock();
}
// We loop over the copied perceiverOids causing
// interpolation to happen, and capturing the location in
// the PerceiverData, so we can later do comparisons
// cheaply. Note that underlying map can change while
// we're doing so, so we don't raise errors if it happens.
for (long perceiverOid : perceiverOids) {
PerceiverData perceiverData = perceiverDataMap.get(perceiverOid);
if (perceiverData != null) {
perceiverData.previousLoc = perceiverData.lastLoc;
// long lastInterp = perceiverData.wnode.getLastInterp();
perceiverData.lastLoc = perceiverData.wnode.getLoc();
// if (Log.loggingDebug)
// Log.debug("Updater.update: perceiverOid " + perceiverOid + ",
// previousLoc " + perceiverData.previousLoc +
// ", lastLoc " + perceiverData.lastLoc + ", time since interp
// " + (System.currentTimeMillis() - lastInterp));
}
}
// Now actually do the double loop to check if inRange has
// changed
for (long perceiverOid : perceiverOids) {
PerceiverData perceiverData = perceiverDataMap.get(perceiverOid);
if (perceiverData == null) continue;
// If the perceiver hasn't moved much, no need to
// iterate over it's perceived entities
if (perceiverData.previousLoc != null
&& Point.distanceToSquared(perceiverData.previousLoc, perceiverData.lastLoc) < 100f)
continue;
ArrayList<Long> perceivedOids = new ArrayList<Long>(perceiverData.perceivedOids);
for (long perceivedOid : perceivedOids) {
PerceiverData perceivedData = perceiverDataMap.get(perceivedOid);
if (perceivedData == null) continue;
// Invoke the testProximity method but tell it not
// to interpolate, but instead get its location
// from the PerceptionData.lastLoc members
testProximity(perceiverData, perceivedData, false, false);
}
}
}
public List<Long> getOidsInRadius(long perceiverOid) {
lock.lock();
try {
PerceiverData perceiverData = perceiverDataMap.get(perceiverOid);
if (perceiverData == null) {
Log.error(
"ProximityTracker.getOidsInRadius: perceptionData for oid "
+ perceiverOid
+ " is null");
return new LinkedList<Long>();
} else return new LinkedList<Long>(perceiverData.inRangeOids);
} finally {
lock.unlock();
}
}
protected void removePerceivedObject(long perceiverOid, long perceivedOid) {
lock.lock();
try {
PerceiverData perceiverData = perceiverDataMap.get(perceiverOid);
if (perceiverData == null) {
if (Log.loggingDebug)
Log.debug(
"ProximityTracker.removePerceivedObject: No perceiverData for oid " + perceiverOid);
return;
}
perceiverData.perceivedOids.remove(perceivedOid);
if (perceiverData.inRangeOids.contains(perceivedOid)) {
performNotification(perceiverOid, perceivedOid, true, false);
perceiverData.inRangeOids.remove(perceivedOid);
}
} finally {
lock.unlock();
}
}
protected long setupPathInterpolator(
long oid, Point myLoc, Point dest, boolean follow, boolean followsTerrain) {
long timeNow = System.currentTimeMillis();
WorldManagerClient.MobPathReqMessage reqMsg =
pathState.setupPathInterpolator(timeNow, myLoc, dest, mobSpeed, follow, followsTerrain);
if (reqMsg != null) {
try {
Engine.getAgent().sendBroadcast(reqMsg);
if (Log.loggingDebug)
Log.debug("BaseBehavior.setupPathInterpolator: send MobPathReqMessage " + reqMsg);
} catch (Exception e) {
throw new RuntimeException(e);
}
interpolatingPath = true;
return pathState.pathTimeRemaining();
} else {
interpolatingPath = false;
return 0;
}
}
public void gotoUpdate() {
Point myLoc = obj.getWorldNode().getLoc();
long oid = obj.getOid();
if (interpolatingPath) {
interpolatePath();
if (!interpolatingPath) {
Engine.getAgent().sendBroadcast(new ArrivedEventMessage(obj));
if (Log.loggingDebug)
Log.debug(
"BaseBehavior.gotoUpdate sending ArrivedEventMessage: oid = "
+ oid
+ "; myLoc = "
+ myLoc
+ "; destLoc = "
+ destLoc);
mode = MSG_CMD_TYPE_STOP;
}
}
if (interpolatingPath) scheduleMe(pathState.pathTimeRemaining());
}
public void handlePerception(PerceptionMessage perceptionMessage) {
long targetOid = perceptionMessage.getTarget();
List<PerceptionMessage.ObjectNote> gain = perceptionMessage.getGainObjects();
List<PerceptionMessage.ObjectNote> lost = perceptionMessage.getLostObjects();
if (Log.loggingDebug)
Log.debug(
"ProximityTracker.handlePerception: targetOid + "
+ targetOid
+ ", instanceOid="
+ instanceOid
+ " "
+ ((gain == null) ? 0 : gain.size())
+ " gain and "
+ ((lost == null) ? 0 : lost.size())
+ " lost");
if (gain != null) for (PerceptionMessage.ObjectNote note : gain) maybeAddPerceivedObject(note);
if (lost != null)
for (PerceptionMessage.ObjectNote note : lost)
maybeRemovePerceivedObject(note.getSubject(), note, targetOid);
}
