public override VMPrimitiveExitCode Execute(VMStackFrame context, VMPrimitiveOperand args)
{
var operand = (VMLookTowardsOperand)args;
//TODO: primitive fails if object calls it
VMAvatar sim = (VMAvatar)context.Caller;
var result = new VMFindLocationResult();
result.Position = new LotTilePos(sim.Position);
switch (operand.Mode)
{
case VMLookTowardsMode.HeadTowardsObject:
return VMPrimitiveExitCode.GOTO_TRUE; //TODO: turning head towards things, with head seek timeout
case VMLookTowardsMode.BodyTowardsCamera:
return VMPrimitiveExitCode.GOTO_TRUE; //does not work in TSO
case VMLookTowardsMode.BodyTowardsStackObj:
result.RadianDirection = (float)GetDirectionTo(sim.Position, context.StackObject.Position);
break;
case VMLookTowardsMode.BodyAwayFromStackObj:
result.RadianDirection = (float)GetDirectionTo(sim.Position, context.StackObject.Position);
result.RadianDirection = (float)((result.RadianDirection + Math.PI) % (Math.PI*2));
break;
}
var pathFinder = context.Thread.PushNewRoutingFrame(context, false); //use the path finder to do the turn animation.
pathFinder.InitRoutes(new List<VMFindLocationResult>() { result });
return VMPrimitiveExitCode.CONTINUE;
}
private void VerifyAndAddLocation(VMEntity obj, Vector2 pos, Vector2 center, SLOTFlags entryFlags, double score, VMContext context, VMEntity caller, float facingDir)
{
//note: verification is not performed if snap target slot is enabled.
var tpos = new LotTilePos((short)Math.Round(pos.X * 16), (short)Math.Round(pos.Y * 16), obj.Position.Level);
if (context.IsOutOfBounds(tpos))
{
return;
}
score -= LotTilePos.Distance(tpos, caller.Position) / 3.0;
if (Slot.SnapTargetSlot < 0 && context.Architecture.RaycastWall(new Point((int)pos.X, (int)pos.Y), new Point(obj.Position.TileX, obj.Position.TileY), obj.Position.Level))
{
SetFail(VMRouteFailCode.WallInWay, null);
return;
}
bool faceAnywhere = false;
if (float.IsNaN(facingDir))
{
var obj3P = obj.Position.ToVector3();
var objP = new Vector2(obj3P.X, obj3P.Y);
switch (Slot.Facing)
{
case SLOTFacing.FaceTowardsObject:
facingDir = (float)GetDirectionTo(pos, objP); break;
case SLOTFacing.FaceAwayFromObject:
facingDir = (float)GetDirectionTo(objP, pos); break;
case SLOTFacing.FaceAnywhere:
faceAnywhere = true;
facingDir = 0.0f; break;
default:
int intDir = (int)Math.Round(Math.Log((double)obj.Direction, 2));
var rotatedF = ((int)Slot.Facing + intDir) % 8;
facingDir = (float)(((int)rotatedF > 4) ? ((double)rotatedF * Math.PI / 4.0) : (((double)rotatedF - 8.0) * Math.PI / 4.0)); break;
}
}
VMFindLocationResult result = new VMFindLocationResult
{
Position = new LotTilePos((short)Math.Round(pos.X * 16), (short)Math.Round(pos.Y * 16), obj.Position.Level),
RadianDirection = facingDir,
FaceAnywhere = faceAnywhere,
RouteEntryFlags = entryFlags
};
var avatarInWay = false;
if (Slot.SnapTargetSlot < 0)
{
var solid = caller.PositionValid(tpos, Direction.NORTH, context, VMPlaceRequestFlags.AcceptSlots);
if (solid.Status != Model.VMPlacementError.Success)
{
if (solid.Object != null)
{
if (solid.Object is VMGameObject)
{
if (Slot.Sitting > 0 && solid.Object.EntryPoints[26].ActionFunction != 0)
{
result.Chair = solid.Object;
}
else
{
SetFail(VMRouteFailCode.DestTileOccupied, solid.Object);
return;
}
}
else
{
avatarInWay = true;
}
}
}
if (context.ObjectQueries.GetObjectsAt(tpos)?.Any(
x => ((VMEntityFlags2)x.GetValue(VMStackObjectVariable.FlagField2) & VMEntityFlags2.ArchitectualDoor) > 0) ?? false)
{
avatarInWay = true; //prefer not standing in front of a door. (todo: merge with above check?)
}
if (result.Chair != null && (Math.Abs(DirectionUtils.Difference(result.Chair.RadianDirection, facingDir)) > Math.PI / 4))
{
return; //not a valid goal.
}
if (result.Chair == null && OnlySit)
{
return;
}
score = score * ((result.Chair != null) ? Slot.Sitting : Slot.Standing);
//if an avatar is in or going to our destination positon, we this spot becomes low priority as getting into it will require a shoo.
if (!avatarInWay)
{
foreach (var avatar in context.ObjectQueries.Avatars)
{
if (avatar == caller)
{
continue;
}
//search for routing frame. is its destination the same as ours?
if (avatar.Thread != null)
{
var intersects = avatar.Thread.Stack.Any(x => x is VMRoutingFrame && ((VMRoutingFrame)x).IntersectsOurDestination(result));
if (intersects)
{
score = Math.Max(double.Epsilon, score);
break;
}
}
}
}
else
{
score = Math.Max(double.Epsilon, score);
}
}
result.Score = score;
Results.Add(result);
}
private bool DoRoomRoute(VMFindLocationResult route)
{
Rooms = new Stack<VMRoomPortal>();
LotTilePos dest;
if (Slot != null)
{
//take destination pos from object. Estimate room closeness using distance to object, not destination.
dest = Target.Position;
}
else
{
if (route != null) dest = route.Position;
else return false; //???
}
var DestRoom = VM.Context.GetRoomAt(dest);
var MyRoom = VM.Context.GetRoomAt(Caller.Position);
IgnoreRooms = (Slot != null && (Slot.Rsflags & SLOTFlags.IgnoreRooms) > 0);
if (DestRoom == MyRoom || IgnoreRooms) return true; //we don't have to do any room finding for this
else
{
//find shortest room traversal to destination. Simple A* pathfind.
//Portals are considered nodes to allow multiple portals between rooms to be considered.
var openSet = new List<VMRoomPortal>(); //we use this like a queue, but we need certain functions for sorted queue that are only provided by list.
var closedSet = new HashSet<VMRoomPortal>();
var gScore = new Dictionary<VMRoomPortal, double>();
var fScore = new Dictionary<VMRoomPortal, double>();
var parents = new Dictionary<VMRoomPortal, VMRoomPortal>();
var StartPortal = new VMRoomPortal(Caller.ObjectID, MyRoom); //consider the sim as a portal to this room (as a starting point)
openSet.Add(StartPortal);
gScore[StartPortal] = 0;
fScore[StartPortal] = GetDist(Caller.Position, dest);
while (openSet.Count != 0)
{
var current = openSet[0];
openSet.RemoveAt(0);
if (current.TargetRoom == DestRoom)
{
//this portal gets us to the room.
while (current != StartPortal) //push previous portals till we get to our first "portal", the sim in its current room (we have already "traversed" this portal)
{
Rooms.Push(current);
current = parents[current];
}
return true;
}
closedSet.Add(current);
var portals = VM.Context.RoomInfo[current.TargetRoom].Portals;
foreach (var portal in portals)
{ //evaluate all neighbor portals
if (IgnoredRooms.Contains(portal) || closedSet.Contains(portal)) continue; //already evaluated, or couldn't get to the portal.
var pos = VM.GetObjectById(portal.ObjectID).Position;
var gFromCurrent = gScore[current] + GetDist(VM.GetObjectById(current.ObjectID).Position, pos);
var newcomer = !openSet.Contains(portal);
if (newcomer || gFromCurrent < gScore[portal])
{
parents[portal] = current; //best parent for now
gScore[portal] = gFromCurrent;
fScore[portal] = gFromCurrent + GetDist(pos, dest);
if (newcomer)
{ //add and move to relevant position
OpenSetSortedInsert(openSet, fScore, portal);
}
else
{ //remove and reinsert to refresh sort
openSet.Remove(portal);
OpenSetSortedInsert(openSet, fScore, portal);
}
}
}
}
return false;
}
}
private VMRouteFailCode AttemptRoute(VMFindLocationResult route)
{
//returns false if there is no room portal route to the destination room.
//if route is not null, we are on a DIRECT route, where either the SLOT has been resolved or a route has already been passed to us.
//resets some variables either way, so that the route can start again.
CurRoute = route;
WalkTo = null; //reset routing state
AttemptedChair = false;
TurnTweak = 0;
return (DoRoomRoute(route)) ? VMRouteFailCode.Success : VMRouteFailCode.NoRoomRoute;
}
public VMPrimitiveExitCode Tick()
{
var avatar = (VMAvatar)Caller;
avatar.Velocity = new Vector3(0, 0, 0);
if (State != VMRoutingFrameState.FAILED && avatar.GetFlag(VMEntityFlags.InteractionCanceled) && avatar.GetPersonData(VMPersonDataVariable.NonInterruptable) == 0)
{
HardFail(VMRouteFailCode.Interrupted, null);
return VMPrimitiveExitCode.CONTINUE;
}
if (WaitTime > 0)
{
if (Velocity > 0) Velocity--;
if (avatar.Animations.Count < 3) StartWalkAnimation();
avatar.Animations[0].Weight = (8 - Velocity) / 8f;
avatar.Animations[1].Weight = (Velocity / 8f) * 0.66f;
avatar.Animations[2].Weight = (Velocity / 8f) * 0.33f;
WaitTime--;
Timeout--;
if (Timeout <= 0)
{
//try again. not sure if we should reset timeout for the new route
SoftFail(VMRouteFailCode.NoPath, null);
if (State != VMRoutingFrameState.FAILED) {
Velocity = 0;
State = VMRoutingFrameState.WALKING;
}
} else return VMPrimitiveExitCode.CONTINUE_NEXT_TICK;
}
if (RoomRouteInvalid && State != VMRoutingFrameState.BEGIN_TURN && State != VMRoutingFrameState.END_TURN)
{
RoomRouteInvalid = false;
IgnoredRooms.Clear();
WalkTo = null; //reset routing state
if (!DoRoomRoute(CurRoute))
{
if (CurRoute != null) SoftFail(VMRouteFailCode.NoRoomRoute, null);
else HardFail(VMRouteFailCode.NoRoomRoute, null);
}
else if (Rooms.Count > 0)
{
State = VMRoutingFrameState.INITIAL;
}
}
switch (State)
{
case VMRoutingFrameState.STAND_FUNC:
if (Thread.LastStackExitCode == VMPrimitiveExitCode.RETURN_TRUE)
{
State = VMRoutingFrameState.INITIAL;
if (avatar.GetPersonData(VMPersonDataVariable.Posture) == 1) avatar.SetPersonData(VMPersonDataVariable.Posture, 0);
}
else
SoftFail(VMRouteFailCode.CantStand, null);
return VMPrimitiveExitCode.CONTINUE;
case VMRoutingFrameState.INITIAL:
case VMRoutingFrameState.ROOM_PORTAL:
//check if the room portal that just finished succeeded.
if (State == VMRoutingFrameState.ROOM_PORTAL) {
if (Thread.LastStackExitCode != VMPrimitiveExitCode.RETURN_TRUE)
{
IgnoredRooms.Add(CurrentPortal);
State = VMRoutingFrameState.INITIAL;
if (!DoRoomRoute(CurRoute))
{
SoftFail(VMRouteFailCode.NoRoomRoute, null); //todo: reattempt room route with portal we tried removed.
return VMPrimitiveExitCode.CONTINUE;
}
}
}
if (Rooms.Count > 0)
{ //push portal function of next portal
CurrentPortal = Rooms.Pop();
var ent = VM.GetObjectById(CurrentPortal.ObjectID);
State = VMRoutingFrameState.ROOM_PORTAL;
if (!PushEntryPoint(15, ent)) //15 is portal function
SoftFail(VMRouteFailCode.NoRoomRoute, null); //could not execute portal function
return VMPrimitiveExitCode.CONTINUE;
}
//if we're here, room route is OK. start routing to a destination.
if (Choices == null)
{
//perform slot parse.
if (Slot == null)
{
HardFail(VMRouteFailCode.Unknown, null);
return VMPrimitiveExitCode.CONTINUE; //this should never happen. If it does, someone has used the routing system incorrectly.
}
var parser = new VMSlotParser(Slot);
Choices = parser.FindAvaliableLocations(Target, VM.Context, avatar);
if (Choices.Count == 0)
{
HardFail(parser.FailCode, parser.Blocker);
return VMPrimitiveExitCode.CONTINUE;
}
else
{
CurRoute = Choices[0];
Choices.RemoveAt(0);
}
}
//do we need to sit in a seat? it should take over.
if (CurRoute.Chair != null)
{
if (!AttemptedChair)
{
AttemptedChair = true;
if (PushEntryPoint(26, CurRoute.Chair)) return VMPrimitiveExitCode.CONTINUE;
else
{
SoftFail(VMRouteFailCode.CantSit, null);
return VMPrimitiveExitCode.CONTINUE;
}
}
else
{
if (Thread.LastStackExitCode == VMPrimitiveExitCode.RETURN_TRUE) return VMPrimitiveExitCode.RETURN_TRUE;
else
{
SoftFail(VMRouteFailCode.CantSit, null);
return VMPrimitiveExitCode.CONTINUE;
}
}
}
//If we are sitting, and the target is not this seat we need to call the stand function on the object we are contained within.
if (avatar.GetPersonData(VMPersonDataVariable.Posture) == 1)
{
State = VMRoutingFrameState.STAND_FUNC;
//push it onto our stack, except now the portal owns our soul! when we are returned to we can evaluate the result and determine if the route failed.
var chair = Caller.Container;
if (chair == null) avatar.SetPersonData(VMPersonDataVariable.Posture, 0); //we're sitting, but are not bound to a chair. Just instantly get up..
else
{
if (!PushEntryPoint(27, chair)) //27 is stand. TODO: set up an enum for these
HardFail(VMRouteFailCode.CantStand, null);
return VMPrimitiveExitCode.CONTINUE;
}
}
//no chair, we just need to walk to the spot. Start the within-room routing.
if (WalkTo == null)
{
if (!AttemptWalk())
{
SoftFail(VMRouteFailCode.NoPath, null);
return VMPrimitiveExitCode.CONTINUE;
}
}
if (State != VMRoutingFrameState.TURN_ONLY) BeginWalk();
return VMPrimitiveExitCode.CONTINUE;
case VMRoutingFrameState.FAILED:
return VMPrimitiveExitCode.RETURN_FALSE;
case VMRoutingFrameState.TURN_ONLY:
return (EndWalk()) ? VMPrimitiveExitCode.RETURN_TRUE : VMPrimitiveExitCode.CONTINUE;
case VMRoutingFrameState.SHOOED:
StartWalkAnimation();
State = VMRoutingFrameState.WALKING;
return VMPrimitiveExitCode.CONTINUE;
case VMRoutingFrameState.BEGIN_TURN:
case VMRoutingFrameState.END_TURN:
if (avatar.CurrentAnimationState.EndReached)
{
if (State == VMRoutingFrameState.BEGIN_TURN)
{
State = VMRoutingFrameState.WALKING;
WalkDirection = TargetDirection;
avatar.RadianDirection = (float)TargetDirection;
StartWalkAnimation();
return VMPrimitiveExitCode.CONTINUE;
}
else
{
if (!CurRoute.FaceAnywhere) avatar.RadianDirection = CurRoute.RadianDirection;
//reset animation, so that we're facing the correct direction afterwards.
avatar.Animations.Clear();
var animation = FSO.Content.Content.Get().AvatarAnimations.Get(avatar.WalkAnimations[3] + ".anim");
var state = new VMAnimationState(animation, false);
state.Loop = true;
avatar.Animations.Add(state);
return VMPrimitiveExitCode.RETURN_TRUE; //we are here!
}
}
else
{
avatar.RadianDirection += TurnTweak; //while we're turning, adjust our direction
return VMPrimitiveExitCode.CONTINUE_NEXT_TICK;
}
case VMRoutingFrameState.WALKING:
if (WalkTo == null)
{
if (!AttemptWalk())
{
SoftFail(VMRouteFailCode.NoPath, null);
}
return VMPrimitiveExitCode.CONTINUE;
}
if (WalkTo.Count == 0 && MoveTotalFrames - MoveFrames <= 28 && CanPortalTurn()) //7+6+5+4...
{
//tail off
if (Velocity <= 0) Velocity = 1;
if (Velocity > 1) Velocity--;
}
else
{
//get started
if (Velocity < 8) Velocity++;
}
avatar.Animations[0].Weight = (8 - Velocity) / 8f;
avatar.Animations[1].Weight = (Velocity / 8f) * 0.66f;
avatar.Animations[2].Weight = (Velocity / 8f) * 0.33f;
MoveFrames += Velocity;
if (MoveFrames >= MoveTotalFrames)
{
MoveFrames = MoveTotalFrames;
//move us to the final spot, then attempt an advance.
}
//normal sims can move 0.05 units in a frame.
if (TurnFrames > 0)
{
avatar.RadianDirection = (float)(TargetDirection + DirectionUtils.Difference(TargetDirection, WalkDirection) * (TurnFrames / 10.0));
TurnFrames--;
}
else avatar.RadianDirection = (float)TargetDirection;
var diff = CurrentWaypoint - PreviousPosition;
diff.x = (short)((diff.x * MoveFrames) / MoveTotalFrames);
diff.y = (short)((diff.y * MoveFrames) / MoveTotalFrames);
var storedDir = avatar.RadianDirection;
var result = Caller.SetPosition(PreviousPosition + diff, Direction.NORTH, VM.Context);
avatar.RadianDirection = storedDir;
if (result.Status != VMPlacementError.Success && result.Status != VMPlacementError.CantBeThroughWall)
{
//route failure, either something changed or we hit an avatar
//on both cases try again, but if we hit an avatar then detect if they have a routing frame and stop us for a bit.
//we stop the first collider because in most cases they're walking across our walk direction and the problem will go away after a second once they're past.
//
//if we need to route around a stopped avatar we add them to our "avatars to consider" set.
bool routeAround = true;
VMRoutingFrame colRoute = null;
if (result.Object != null && result.Object is VMAvatar)
{
var colAvatar = (VMAvatar)result.Object;
var colTopFrame = colAvatar.Thread.Stack.LastOrDefault();
//we already attempted to move around this avatar... if this happens too much give up.
if (AvatarsToConsider.Contains(colAvatar) && --Retries <= 0)
{
SoftFail(VMRouteFailCode.NoPath, avatar);
return VMPrimitiveExitCode.CONTINUE;
}
if (colTopFrame != null && colTopFrame is VMRoutingFrame)
{
colRoute = (VMRoutingFrame)colTopFrame;
routeAround = (colRoute.WaitTime > 0);
}
if (routeAround) AvatarsToConsider.Add(colAvatar);
}
if (result.Object == null || result.Object is VMGameObject)
{
//this should not happen often. An object or other feature has blocked our path due to some change in its position.
//repeated occurances indicate that we are stuck in something.
//todo: is this safe for the robot lot?
if (--Retries <= 0)
{
SoftFail(VMRouteFailCode.NoPath, avatar);
return VMPrimitiveExitCode.CONTINUE;
}
}
if (routeAround)
{
var oldWalk = new LinkedList<Point>(WalkTo);
if (AttemptWalk()) return VMPrimitiveExitCode.CONTINUE;
else
{
//failed to walk around the object
if (result.Object is VMAvatar)
{
WalkTo = oldWalk;
//if they're a person, shoo them away.
//if they're in a routing frame we can push the tree directly onto their stack
//otherwise we push an interaction and just hope they move (todo: does tso do this?)
//DO NOT push tree if:
// - sim is already being shooed. (the parent of the top routing frame is in state "SHOOED" or shoo interaction is present)
// - we are being shooed.
// - sim is waiting on someone they just shooed. (presumably can move out of our way once they finish waiting)
//instead just wait a small duration and try again later.
//cases where we cannot continue moving increase the retry count. if this is greater than RETRY_COUNT then we fail.
//not sure how the original game works.
if (CanShooAvatar((VMAvatar)result.Object))
{
AvatarsToConsider.Remove((VMAvatar)result.Object);
VMEntity callee = VM.Context.CreateObjectInstance(GOTO_GUID, new LotTilePos(result.Object.Position), Direction.NORTH).Objects[0];
if (colRoute != null)
{
colRoute.State = VMRoutingFrameState.SHOOED;
colRoute.WalkTo = null;
colRoute.AvatarsToConsider.Add(avatar); //just to make sure they don't try route through us.
var tree = callee.GetBHAVWithOwner(SHOO_TREE, VM.Context);
result.Object.Thread.ExecuteSubRoutine(colRoute, tree.bhav, tree.owner, new VMSubRoutineOperand());
WalkInterrupt(60);
}
else
{
callee.PushUserInteraction(SHOO_INTERACTION, result.Object, VM.Context);
WalkInterrupt(60);
}
}
else
{
WalkInterrupt(60); //wait for a little while, they're moving out of the way.
}
return VMPrimitiveExitCode.CONTINUE;
}
SoftFail(VMRouteFailCode.DestTileOccupied, result.Object);
return VMPrimitiveExitCode.CONTINUE;
}
}
else
{
WalkInterrupt(30);
return VMPrimitiveExitCode.CONTINUE;
}
}
Caller.VisualPosition = Vector3.Lerp(PreviousPosition.ToVector3(), CurrentWaypoint.ToVector3(), MoveFrames / (float)MoveTotalFrames);
var velocity = Vector3.Lerp(PreviousPosition.ToVector3(), CurrentWaypoint.ToVector3(), Velocity / (float)MoveTotalFrames) - PreviousPosition.ToVector3();
velocity.Z = 0;
avatar.Velocity = velocity;
if (MoveTotalFrames == MoveFrames)
{
MoveTotalFrames = 0;
while (MoveTotalFrames == 0)
{
var remains = AdvanceWaypoint();
if (!remains)
{
MoveTotalFrames = -1;
if (EndWalk()) return VMPrimitiveExitCode.RETURN_TRUE;
}
}
}
return VMPrimitiveExitCode.CONTINUE_NEXT_TICK;
}
return VMPrimitiveExitCode.GOTO_FALSE; //???
}
public override void Load(VMStackFrameMarshal input, VMContext context)
{
base.Load(input, context);
var inR = (VMRoutingFrameMarshal)input;
Rooms = new Stack<VMRoomPortal>();
for (int i=inR.Rooms.Length-1; i>=0; i--) Rooms.Push(inR.Rooms[i]);
CurrentPortal = inR.CurrentPortal;
ParentRoute = GetParentFrame(); //should be able to, since all arrays are generated left to right, including the stacks.
WalkTo = (inR.WalkTo == null)?null:new LinkedList<Point>(inR.WalkTo);
WalkDirection = inR.WalkDirection;
TargetDirection = inR.TargetDirection;
IgnoreRooms = inR.IgnoreRooms;
State = inR.State;
PortalTurns = inR.PortalTurns;
WaitTime = inR.WaitTime;
Timeout = inR.Timeout;
Retries = inR.Retries;
AttemptedChair = inR.AttemptedChair;
TurnTweak = inR.TurnTweak;
TurnFrames = inR.TurnFrames;
MoveTotalFrames = inR.MoveTotalFrames;
MoveFrames = inR.MoveFrames;
Velocity = inR.Velocity;
CallFailureTrees = inR.CallFailureTrees;
IgnoredRooms = new HashSet<VMRoomPortal>(inR.IgnoredRooms);
AvatarsToConsider = new HashSet<VMAvatar>();
foreach (var avatar in inR.AvatarsToConsider)
AvatarsToConsider.Add((VMAvatar)context.VM.GetObjectById(avatar));
PreviousPosition = inR.PreviousPosition;
CurrentWaypoint = inR.CurrentWaypoint;
RoomRouteInvalid = inR.RoomRouteInvalid;
Slot = inR.Slot; //NULLable
Target = context.VM.GetObjectById(inR.Target); //object id
if (inR.Choices != null)
{
Choices = new List<VMFindLocationResult>();
foreach (var c in inR.Choices) Choices.Add(new VMFindLocationResult(c, context));
}
else Choices = null;
CurRoute = (inR.CurRoute == null)?null:new VMFindLocationResult(inR.CurRoute, context); //NULLable
}