PhysicsRemoteFPSAgentController.cs

// Copyright Allen Institute for Artificial Intelligence 2017

using System;
using System.Collections;
using System.Collections.Generic;
using System.Linq;
using UnityEngine;

namespace UnityStandardAssets.Characters.FirstPerson {
[RequireComponent(typeof(CharacterController))]
public class PhysicsRemoteFPSAgentController : BaseFPSAgentController {

    // Used to expand agent capsule in collision checks for open/close actions
    protected const float ExpandAgentCapsuleBy = 0.1f;
    protected float serverActionMoveMagnitude;

    // Use this for initialization
    public override void Start() { base.Start(); }

    // forceVisible is true to activate, false to deactivate

    public float WhatIsAgentsMaxVisibleDistance() { return maxVisibleDistance; }

    public SimObjPhysics WhatAmIHolding() { return ItemInHand; }

    // get all sim objets of action.type, then sets their temperature decay
    // timers to value

    private void LateUpdate() {
        // make sure this happens in late update so all physics related checks
        // are done ahead of time this is also mostly for in editor, the array
        // of visible sim objects is found via server actions using
        // VisibleSimObjs(action), so be aware of that

#if UNITY_EDITOR || UNITY_WEBGL
        if (this.agentState == AgentState.ActionComplete) {
            ServerAction action = new ServerAction();
            VisibleSimObjPhysics = VisibleSimObjs(action); // GetAllVisibleSimObjPhysics(m_Camera,
                                                           // maxVisibleDistance);
        }

#endif
    }

    public override ObjectMetadata[] generateObjectMetadata() { return base.generateObjectMetadata(); }

    public override MetadataWrapper generateMetadataWrapper() { return base.generateMetadataWrapper(); }

    public override ObjectMetadata ObjectMetadataFromSimObjPhysics(SimObjPhysics simObj, bool isVisible) { return base.ObjectMetadataFromSimObjPhysics(simObj, isVisible); }

    // change the radius of the agent's capsule on the char controller
    // component, and the capsule collider component

// EDITOR DEBUG SCRIPTS:
//////////////////////////////////////////////////////////////////////
#if UNITY_EDITOR

    // return ID of closest CanPickup object by distance
    public string ObjectIdOfClosestVisibleObject() {
        string objectID = null;

        foreach (SimObjPhysics o in VisibleSimObjPhysics) {
            if (o.PrimaryProperty == SimObjPrimaryProperty.CanPickup) {
                objectID = o.ObjectID;
                break;
            }
        }

        return objectID;
    }

    public string ObjectIdOfClosestPickupableOrMoveableObject() {
        string objectID = null;

        foreach (SimObjPhysics o in VisibleSimObjPhysics) {
            if (o.PrimaryProperty == SimObjPrimaryProperty.CanPickup || o.PrimaryProperty == SimObjPrimaryProperty.Moveable) {
                objectID = o.ObjectID;
                break;
            }
        }

        return objectID;
    }

    // return ID of closest CanOpen or CanOpen_Fridge object by distance

    public string ObjectIdOfClosestReceptacleObject() {
        string objectID = null;

        foreach (SimObjPhysics o in VisibleSimObjPhysics) {
            if (o.DoesThisObjectHaveThisSecondaryProperty(SimObjSecondaryProperty.Receptacle)) {
                objectID = o.ObjectID;
                break;
            }
        }
        return objectID;
    }
#endif
    /////////////////////////////////////////////////////////
    // return a reference to a SimObj that is Visible (in the
    // VisibleSimObjPhysics array) and matches the passed in objectID
    public GameObject FindObjectInVisibleSimObjPhysics(string objectID, SimObjPhysics[] visibleSimObjects = null) {
        GameObject target = null;
        foreach (SimObjPhysics o in visibleSimObjects == null ? VisibleSimObjPhysics : visibleSimObjects) {
            if (o.objectID == objectID) {
                target = o.gameObject;
            }
        }
        return target;
    }

    // use this to check if any given Vector3 coordinate is within the agent's
    // viewport and also not obstructed
    public bool CheckIfPointIsInViewport(Vector3 point) {
        Vector3 viewPoint = m_Camera.WorldToViewportPoint(point);

        // increasing these grants a very slight flexibiltiy in object placement
        // if the agent is below or above a receptacle
        float ViewPointRangeHigh = 1.0f;
        float ViewPointRangeLow = -1.0f;

        if (viewPoint.z > 0                                                         //&& viewPoint.z < maxDistance * DownwardViewDistance //is in
                                                                                    // front of camera and within
                                                                                    // range of visibility sphere
            && viewPoint.x < ViewPointRangeHigh && viewPoint.x > ViewPointRangeLow  // within x bounds of viewport
            && viewPoint.y < ViewPointRangeHigh && viewPoint.y > ViewPointRangeLow) // within y bounds of viewport
        {
            RaycastHit hit;

            updateAllAgentCollidersForVisibilityCheck(false);

            if (Physics.Raycast(m_Camera.transform.position, point - m_Camera.transform.position, out hit, (1 << 8) | (1 << 10))) {
                updateAllAgentCollidersForVisibilityCheck(true);
                // this should be set to true for object placement flexibility
                return true;
            }
        }
        updateAllAgentCollidersForVisibilityCheck(true);
        return false;
    }

    // checks if a float is a multiple of 0.1f
    private bool CheckIfFloatIsMultipleOfOneTenth(float f) {
        if (((decimal)f % 0.1M == 0) == false)
            return false;

        else
            return true;
    }

    public override void LookDown(ServerAction action) {
        if (action.degrees < 0) {
            errorMessage = "LookDown action requires positive degree value. Invalid value used: " + action.degrees;
            actionFinished(false);
            return;
        }

        if (!CheckIfFloatIsMultipleOfOneTenth(action.degrees)) {
            errorMessage = "LookDown action requires degree value to be a multiple of 0.1f";
            actionFinished(false);
            return;
        }

        // default degree increment to 30
        if (action.degrees == 0) {
            action.degrees = 30f;
        }

        // force the degree increment to the nearest tenths place
        // this is to prevent too small of a degree increment change that could
        // cause float imprecision
        action.degrees = Mathf.Round(action.degrees * 10.0f) / 10.0f;

        if (!checkForUpDownAngleLimit("down", action.degrees)) {
            errorMessage = "can't look down beyond " + maxDownwardLookAngle + " degrees below the forward horizon";
            errorCode = ServerActionErrorCode.LookDownCantExceedMin;
            actionFinished(false);
            return;
        }

        if (CheckIfAgentCanRotate("down", action.degrees)) {

            // only default hand if not manually Interacting with things
            if (!action.manualInteract)
                DefaultAgentHand();

            base.LookDown(action);
            return;
        }

        else {
            errorMessage = "a held item: " + ItemInHand.objectID + " will collide with something if agent rotates down " + action.degrees + " degrees";
            actionFinished(false);
        }
    }

    public override void LookUp(ServerAction action) {

        if (action.degrees < 0) {
            errorMessage = "LookUp action requires positive degree value. Invalid value used: " + action.degrees;
            actionFinished(false);
            return;
        }

        if (!CheckIfFloatIsMultipleOfOneTenth(action.degrees)) {
            errorMessage = "LookUp action requires degree value to be a multiple of 0.1f";
            actionFinished(false);
            return;
        }

        // default degree increment to 30
        if (action.degrees == 0) {
            action.degrees = 30f;
        }

        // force the degree increment to the nearest tenths place
        // this is to prevent too small of a degree increment change that could
        // cause float imprecision
        action.degrees = Mathf.Round(action.degrees * 10.0f) / 10.0f;

        if (!checkForUpDownAngleLimit("up", action.degrees)) {
            errorMessage = "can't look up beyond " + maxUpwardLookAngle + " degrees above the forward horizon";
            errorCode = ServerActionErrorCode.LookDownCantExceedMin;
            actionFinished(false);
            return;
        }

        if (CheckIfAgentCanRotate("up", action.degrees)) {
            // only default hand if not manually Interacting with things
            if (!action.manualInteract)
                DefaultAgentHand();

            base.LookUp(action);
        }

        else {
            errorMessage = "a held item: " + ItemInHand.objectID + " will collide with something if agent rotates up " + action.degrees + " degrees";
            actionFinished(false);
        }
    }

    public override void RotateRight(ServerAction action) {
        // if controlCommand.degrees is default (0), rotate by the default
        // rotation amount set on initialize
        if (action.degrees == 0f)
            action.degrees = rotateStepDegrees;

        if (CheckIfAgentCanRotate("right", action.degrees) || action.forceAction) {
            // only default hand if not manually Interacting with things
            if (!action.manualInteract) {
                DefaultAgentHand();
            }

            base.RotateRight(action);
        }

        else {
            errorMessage = "a held item: " + ItemInHand.transform.name + " with something if agent rotates Right " + action.degrees + " degrees";
            actionFinished(false);
        }
    }

    public override void RotateLeft(ServerAction action) {
        // if controlCommand.degrees is default (0), rotate by the default
        // rotation amount set on initialize
        if (action.degrees == 0f)
            action.degrees = rotateStepDegrees;

        if (CheckIfAgentCanRotate("left", action.degrees) || action.forceAction) {
            // only default hand if not manually Interacting with things
            if (!action.manualInteract)
                DefaultAgentHand();

            base.RotateLeft(action);
        }

        else {
            errorMessage = "a held item: " + ItemInHand.transform.name + " with something if agent rotates Left " + action.degrees + " degrees";
            actionFinished(false);
        }
    }

    private bool checkArcForCollisions(Vector3[] corners, Vector3 origin, float degrees, string dir) {
        bool result = true;

        // generate arc points in the positive y axis rotation
        foreach (Vector3 v in corners) {
            Vector3[] pointsOnArc = GenerateArcPoints(v, origin, degrees, dir);

            // raycast from first point in pointsOnArc, stepwise to the last
            // point. If any collisions are hit, immediately return
            for (int i = 0; i < pointsOnArc.Length; i++) {
                // debug draw spheres to show path of arc
                //  GameObject Sphere =
                //  GameObject.CreatePrimitive(PrimitiveType.Sphere);
                //  Sphere.transform.position = pointsOnArc[i];
                //  Sphere.transform.localScale = new Vector3(0.02f, 0.02f,
                //  0.02f); Sphere.GetComponent<SphereCollider>().enabled =
                //  false;

                RaycastHit hit;

                // do linecasts from the first point, sequentially, to the last
                if (i < pointsOnArc.Length - 1) {
                    // Debug.DrawLine(pointsOnArc[i], pointsOnArc[i+1],
                    // Color.magenta, 50f);

                    if (Physics.Linecast(pointsOnArc[i], pointsOnArc[i + 1], out hit, 1 << 8 | 1 << 10, QueryTriggerInteraction.Ignore)) {
                        if (hit.transform.GetComponent<SimObjPhysics>()) {
                            // if we hit the item in our hand, skip
                            if (hit.transform.GetComponent<SimObjPhysics>().transform == ItemInHand.transform)
                                continue;
                        }

                        if (hit.transform == this.transform) {
                            // don't worry about clipping the object into this
                            // agent
                            continue;
                        }

                        result = false;
                        break;
                    }
                }
            }
        }

        return result;
    }

    // for use with each of the 8 corners of a picked up object's bounding box -
    // returns an array of Vector3 points along the arc of the rotation for a
    // given starting point given a starting Vector3, rotate about an origin
    // point for a total given angle. maxIncrementAngle is the maximum value of
    // the increment between points on the arc. if leftOrRight is true - rotate
    // around Y (rotate left/right), false - rotate around X (look up/down)
    private Vector3[] GenerateArcPoints(Vector3 startingPoint, Vector3 origin, float angle, string dir) {
        float incrementAngle = angle / 10f;    // divide the total amount we are rotating by 10 to get
                                               // 10 points on the arc
        Vector3[] arcPoints = new Vector3[11]; // we just always want 10 points in addition to our
                                               // starting corner position (11 total) to check
                                               // against per corner
        float currentIncrementAngle;

        if (dir == "left") // Yawing left (Rotating across XZ plane around Y-pivot)
        {
            for (int i = 0; i < arcPoints.Length; i++) {
                currentIncrementAngle = i * -incrementAngle;
                // move the rotPoint to the current corner's position
                rotPoint.transform.position = startingPoint;
                // rotate the rotPoint around the origin the current increment's
                // angle, relative to the correct axis
                rotPoint.transform.RotateAround(origin, transform.up, currentIncrementAngle);
                // set the current arcPoint's vector3 to the rotated point
                arcPoints[i] = rotPoint.transform.position;
                // arcPoints[i] = RotatePointAroundPivot(startingPoint, origin,
                // new Vector3(0, currentIncrementAngle, 0));
            }
        }

        if (dir == "right") // Yawing right (Rotating across XZ plane around Y-pivot)
        {
            for (int i = 0; i < arcPoints.Length; i++) {
                currentIncrementAngle = i * incrementAngle;
                // move the rotPoint to the current corner's position
                rotPoint.transform.position = startingPoint;
                // rotate the rotPoint around the origin the current increment's
                // angle, relative to the correct axis
                rotPoint.transform.RotateAround(origin, transform.up, currentIncrementAngle);
                // set the current arcPoint's vector3 to the rotated point
                arcPoints[i] = rotPoint.transform.position;
                // arcPoints[i] = RotatePointAroundPivot(startingPoint, origin,
                // new Vector3(0, currentIncrementAngle, 0));
            }
        }

        else if (dir == "up") // Pitching up(Rotating across YZ plane around X-pivot)
        {
            for (int i = 0; i < arcPoints.Length; i++) {
                // reverse the increment angle because of the right handedness
                // orientation of the local x-axis
                currentIncrementAngle = i * -incrementAngle;
                // move the rotPoint to the current corner's position
                rotPoint.transform.position = startingPoint;
                // rotate the rotPoint around the origin the current increment's
                // angle, relative to the correct axis
                rotPoint.transform.RotateAround(origin, transform.right, currentIncrementAngle);
                // set the current arcPoint's vector3 to the rotated point
                arcPoints[i] = rotPoint.transform.position;
                // arcPoints[i] = RotatePointAroundPivot(startingPoint, origin,
                // new Vector3(0, currentIncrementAngle, 0));
            }
        }

        else if (dir == "down") // Pitching down (Rotating across YZ plane
                                // around X-pivot)
        {
            for (int i = 0; i < arcPoints.Length; i++) {
                // reverse the increment angle because of the right handedness
                // orientation of the local x-axis
                currentIncrementAngle = i * incrementAngle;
                // move the rotPoint to the current corner's position
                rotPoint.transform.position = startingPoint;
                // rotate the rotPoint around the origin the current increment's
                // angle, relative to the correct axis
                rotPoint.transform.RotateAround(origin, transform.right, currentIncrementAngle);
                // set the current arcPoint's vector3 to the rotated point
                arcPoints[i] = rotPoint.transform.position;
                // arcPoints[i] = RotatePointAroundPivot(startingPoint, origin,
                // new Vector3(0, currentIncrementAngle, 0));
            }
        }

        return arcPoints;
    }

    // checks if agent is clear to rotate left/right/up/down some number of
    // degrees while holding an object
    public bool CheckIfAgentCanRotate(string direction, float degrees) {

        // If the Item the agent is holding isn't active, it's geometry won't
        // interfere with looking Note for code review: Is there a case where
        // this game object's parent is inactive, but this object is active (but
        // treated as inactive)?
        if (ItemInHand == null || !ItemInHand.gameObject.activeInHierarchy) {
            // Debug.Log("Look check passed: nothing in Agent Hand to prevent
            // Angle change");
            return true;
        }

        bool result = true;

        BoxCollider bb = ItemInHand.boundingBoxCollider;

        // get world coordinates of object in hand's bounding box corners
        Vector3[] corners = UtilityFunctions.CornerCoordinatesOfBoxColliderToWorld(bb);

        // ok now we have each corner, let's rotate them the specified direction
        if (direction == "right" || direction == "left") {
            result = checkArcForCollisions(corners, m_CharacterController.transform.position, degrees, direction);
        }

        else if (direction == "up" || direction == "down") {
            result = checkArcForCollisions(corners, m_Camera.transform.position, degrees, direction);
        }
        // no checks flagged anything, good to go, return true i guess
        return result;
    }

    private bool checkForUpDownAngleLimit(string direction, float degrees) {
        bool result = true;
        // check the angle between the agent's forward vector and the proposed
        // rotation vector if it exceeds the min/max based on if we are rotating
        // up or down, return false

        // first move the rotPoint to the camera
        rotPoint.transform.position = m_Camera.transform.position;
        // zero out the rotation first
        rotPoint.transform.rotation = m_Camera.transform.rotation;

        if (direction == "down") {
            rotPoint.Rotate(new Vector3(degrees, 0, 0));
            // note: maxDownwardLookAngle is negative because SignedAngle()
            // returns a... signed angle... so even though the input is
            // LookDown(degrees) with degrees being positive, it still needs to
            // check against this negatively signed direction.
            if (Mathf.Round(Vector3.SignedAngle(rotPoint.transform.forward, m_CharacterController.transform.forward, m_CharacterController.transform.right) * 10.0f) / 10.0f <
                -maxDownwardLookAngle) {
                result = false;
            }
        }

        if (direction == "up") {
            rotPoint.Rotate(new Vector3(-degrees, 0, 0));
            if (Mathf.Round(Vector3.SignedAngle(rotPoint.transform.forward, m_CharacterController.transform.forward, m_CharacterController.transform.right) * 10.0f) / 10.0f >
                maxUpwardLookAngle) {
                result = false;
            }
        }
        return result;
    }

    protected bool moveObject(SimObjPhysics sop, Vector3 targetPosition, bool snapToGrid = false, HashSet<Transform> ignoreCollisionWithTransforms = null) {
        Vector3 lastPosition = sop.transform.position;

        if (snapToGrid) {
            float mult = 1.0f / gridSize;
            float gridX = Convert.ToSingle(Math.Round(targetPosition.x * mult) / mult);
            float gridZ = Convert.ToSingle(Math.Round(targetPosition.z * mult) / mult);
            targetPosition = new Vector3(gridX, targetPosition.y, gridZ);
        }

        Vector3 dir = targetPosition - sop.transform.position;
        RaycastHit[] sweepResults =
            UtilityFunctions.CastAllPrimitiveColliders(sop.gameObject, targetPosition - sop.transform.position, dir.magnitude, 1 << 8 | 1 << 10, QueryTriggerInteraction.Ignore);

        if (sweepResults.Length > 0) {
            foreach (RaycastHit hit in sweepResults) {
                if (ignoreCollisionWithTransforms == null || !ignoreCollisionWithTransforms.Contains(hit.transform)) {
                    errorMessage = hit.transform.name + " is in the way of moving " + sop.ObjectID;
                    return false;
                }
            }
        }
        sop.transform.position = targetPosition;
        return true;
    }

    public override void TeleportFull(ServerAction action) {
        targetTeleport = new Vector3(action.x, action.y, action.z);

        if (action.forceAction) {
            DefaultAgentHand();
            transform.position = targetTeleport;
            transform.rotation = Quaternion.Euler(new Vector3(0.0f, action.rotation.y, 0.0f));
            if (action.standing) {
                m_Camera.transform.localPosition = standingLocalCameraPosition;
            } else {
                m_Camera.transform.localPosition = crouchingLocalCameraPosition;
            }
            m_Camera.transform.localEulerAngles = new Vector3(action.horizon, 0.0f, 0.0f);
        } else {
            if (!agentManager.SceneBounds.Contains(targetTeleport)) {
                errorMessage = "Teleport target out of scene bounds.";
                actionFinished(false);
                return;
            }

            Vector3 oldPosition = transform.position;
            Quaternion oldRotation = transform.rotation;
            Vector3 oldLocalHandPosition = new Vector3();
            Quaternion oldLocalHandRotation = new Quaternion();
            if (ItemInHand != null) {
                oldLocalHandPosition = ItemInHand.transform.localPosition;
                oldLocalHandRotation = ItemInHand.transform.localRotation;
            }
            Vector3 oldCameraLocalEulerAngle = m_Camera.transform.localEulerAngles;
            Vector3 oldCameraLocalPosition = m_Camera.transform.localPosition;

            DefaultAgentHand();
            transform.position = targetTeleport;

            // apply gravity after teleport so we aren't floating in the air
            Vector3 m = new Vector3();
            m.y = Physics.gravity.y * this.m_GravityMultiplier;
            m_CharacterController.Move(m);

            transform.rotation = Quaternion.Euler(new Vector3(0.0f, action.rotation.y, 0.0f));
            if (action.standing) {
                m_Camera.transform.localPosition = standingLocalCameraPosition;
            } else {
                m_Camera.transform.localPosition = crouchingLocalCameraPosition;
            }
            m_Camera.transform.localEulerAngles = new Vector3(action.horizon, 0.0f, 0.0f);

            bool agentCollides = isAgentCapsuleColliding(collidersToIgnoreDuringMovement);
            bool handObjectCollides = isHandObjectColliding(true);

            if (agentCollides) {
                errorMessage = "Cannot teleport due to agent collision.";
                Debug.Log(errorMessage);
            } else if (handObjectCollides) {
                errorMessage = "Cannot teleport due to hand object collision.";
                Debug.Log(errorMessage);
            }

            if (agentCollides || handObjectCollides) {
                if (ItemInHand != null) {
                    ItemInHand.transform.localPosition = oldLocalHandPosition;
                    ItemInHand.transform.localRotation = oldLocalHandRotation;
                }
                transform.position = oldPosition;
                transform.rotation = oldRotation;
                m_Camera.transform.localPosition = oldCameraLocalPosition;
                m_Camera.transform.localEulerAngles = oldCameraLocalEulerAngle;
                actionFinished(false);
                return;
            }
        }

        Vector3 v = new Vector3();
        v.y = Physics.gravity.y * this.m_GravityMultiplier;
        m_CharacterController.Move(v);

        snapAgentToGrid();
        actionFinished(true);
    }

    protected Vector3 FindClosestPoint(Vector3 position, SimObjPhysics target) {
        bool isNull = true;
        Vector3 bestClosestPoint = new Vector3();
        float bestDistance = 0;
        foreach (Collider collider in target.MyColliders) {
            Vector3 closestPoint = collider.ClosestPoint(position);
            float distance = Vector3.Distance(position, closestPoint);
            if (isNull || distance < bestDistance) {
                isNull = false;
                bestClosestPoint = closestPoint;
                bestDistance = distance;
            }
        }
        return bestClosestPoint;
    }

    public void ApplyForceObject(ServerAction action) {

        GameObject player = this.gameObject;

        if (action.forceAction) {
            action.forceVisible = true;
        }

        SimObjPhysics target = physicsSceneManager.ObjectIdToSimObjPhysics[action.objectId];

        VisibilityBools visibilityBools = objectIsCurrentlyVisible(target, maxVisibleDistance);
        if (Vector3.Distance(transform.position, FindClosestPoint(transform.position, target)) < maxVisibleDistance && !ObjectIsVisibleSimObjOrRoomStructure(target)) {
            if (!visibilityBools.inViewportAndVisible && !visibilityBools.notInViewportButNotObstructed) {
                errorMessage = "Target " + action.objectId + " is within distance of interaction but obstructed.";
                Debug.Log(errorMessage);
                this.lastActionStatus = Enum.GetName(typeof(ActionStatus), ActionStatus.NOT_VISIBLE);
                actionFinished(false);
                return;
            }

            if (!visibilityBools.inViewportAndVisible && visibilityBools.notInViewportButNotObstructed) {
                errorMessage = "Target " + action.objectId + " is within distance of interaction but not visible.";
                Debug.Log(errorMessage);
                Debug.Log(string.Format("Agent - X rotation: {0} - Y rotation {1}.", m_Camera.transform.eulerAngles.x, transform.eulerAngles.y));
                this.lastActionStatus = Enum.GetName(typeof(ActionStatus), ActionStatus.NOT_VISIBLE);
                actionFinished(false);
                return;
            }
        }

        if (!ObjectIsVisibleSimObjOrRoomStructure(target)) {
            errorMessage = action.objectId + " is out of reach.";
            this.lastActionStatus = Enum.GetName(typeof(ActionStatus), ActionStatus.OUT_OF_REACH);
            actionFinished(false);
            return;
        }

        if (target.GetComponent<StructureObject>() && Vector3.Distance(transform.position, FindClosestPoint(transform.position, target)) < maxVisibleDistance) {
            errorMessage = action.objectId + " is not moveable.";
            this.lastActionStatus = Enum.GetName(typeof(ActionStatus), ActionStatus.NOT_MOVEABLE);
            actionFinished(false);
            return;
        }

        if (target == null || !target.GetComponent<SimObjPhysics>()) {
            if (Vector3.Distance(transform.position, FindClosestPoint(transform.position, target)) < maxVisibleDistance) {
                errorMessage = "Target must be SimObjPhysics!";
                Debug.Log(errorMessage);
                this.lastActionStatus = Enum.GetName(typeof(ActionStatus), ActionStatus.NOT_INTERACTABLE);
                actionFinished(false);
                return;
            }
        }

        bool canbepushed = false;
        if (target.PrimaryProperty == SimObjPrimaryProperty.CanPickup || target.PrimaryProperty == SimObjPrimaryProperty.Moveable)
            canbepushed = true;

        if (!canbepushed) {
            errorMessage = "Target Sim Object cannot be moved. It's primary property must be Pickupable or Moveable";
            this.lastActionStatus = Enum.GetName(typeof(ActionStatus), ActionStatus.NOT_MOVEABLE);
            actionFinished(false);
            return;
        }

        target.myRigidbody.isKinematic = false;

        ServerAction apply = new ServerAction();
        apply.moveMagnitude = action.moveMagnitude;

        Vector3 dir = Vector3.zero;

        if (action.z == 1) {
            dir = gameObject.transform.forward;
        }

        if (action.z == -1) {
            dir = -gameObject.transform.forward;
        }
        apply.x = dir.x;
        apply.y = dir.y;
        apply.z = dir.z;

        if (action.action == "TorqueObject") {
            this.lastActionStatus = Enum.GetName(typeof(ActionStatus), ActionStatus.SUCCESSFUL);
            actionFinished(true);
            target.ApplyTorque(action);
        } else if (action.action == "RotateObject") {
            bool succesfulRotation = target.ApplyRotation(action);
            this.lastActionStatus = Enum.GetName(typeof(ActionStatus), succesfulRotation ? ActionStatus.SUCCESSFUL : ActionStatus.OBSTRUCTED);
            actionFinished(succesfulRotation);
        } else if (action.action == "MoveObject") {
            bool succesfullMovement = target.ApplyMovement(action);
            this.lastActionStatus = Enum.GetName(typeof(ActionStatus), succesfullMovement ? ActionStatus.SUCCESSFUL : ActionStatus.OBSTRUCTED);
            actionFinished(succesfullMovement);
        } else {
            this.lastActionStatus = Enum.GetName(typeof(ActionStatus), ActionStatus.SUCCESSFUL);
            actionFinished(true);
            sopApplyForce(apply, target);
        }
    }

    // pause physics autosimulation! Automatic physics simulation can be resumed
    // using the UnpausePhysicsAutoSim() action. additionally, auto simulation
    // will automatically resume from the LateUpdate() check on AgentManager.cs
    // - if the scene has come to rest, physics autosimulation will resume
    protected void sopApplyForce(ServerAction action, SimObjPhysics sop, float length) {
        // apply force, return action finished immediately
        if (physicsSceneManager.physicsSimulationPaused) {
            sop.ApplyForce(action);
            if (length >= 0.00001f) {
                WhatDidITouch feedback = new WhatDidITouch() { didHandTouchSomething = true, objectId = sop.objectID, armsLength = length };
                actionFinished(true, feedback);
            }

            // why is this here?
            else {
                actionFinished(true);
            }
        }

        // if physics is automatically being simulated, use coroutine rather
        // than returning actionFinished immediately
        else {
            sop.ApplyForce(action);
            StartCoroutine(checkIfObjectHasStoppedMoving(sop, length));
        }
    }

    // wrapping the SimObjPhysics.ApplyForce function since lots of things use
    // it....
    protected void sopApplyForce(ServerAction action, SimObjPhysics sop) { sopApplyForce(action, sop, 0.0f); }

    // used to check if an specified sim object has come to rest
    // set useTimeout bool to use a faster time out
    private IEnumerator checkIfObjectHasStoppedMoving(SimObjPhysics sop, float length, bool useTimeout = false) {
        // yield for the physics update to make sure this yield is consistent
        // regardless of framerate
        yield return new WaitForFixedUpdate();

        float startTime = Time.time;
        float waitTime = TimeToWaitForObjectsToComeToRest;

        if (useTimeout) {
            waitTime = 1.0f;
        }

        if (sop != null) {
            Rigidbody rb = sop.myRigidbody;
            bool stoppedMoving = false;

            while (Time.time - startTime < waitTime) {
                if (sop == null)
                    break;

                float currentVelocity = Math.Abs(rb.angularVelocity.sqrMagnitude + rb.velocity.sqrMagnitude);
                float accel = (currentVelocity - sop.lastVelocity) / Time.fixedDeltaTime;

                // ok the accel is basically zero, so it has stopped moving
                if (Mathf.Abs(accel) <= 0.001f) {
                    // force the rb to stop moving just to be safe
                    rb.velocity = Vector3.zero;
                    rb.angularVelocity = Vector3.zero;
                    rb.Sleep();
                    stoppedMoving = true;
                    break;
                }

                else
                    yield return new WaitForFixedUpdate();
            }

            // so we never stopped moving and we are using the timeout
            if (!stoppedMoving && useTimeout) {
                errorMessage = "object couldn't come to rest";
                actionFinished(false);
                yield break;
            }

            // return to metadatawrapper.actionReturn if an object was touched
            // during this interaction
            if (length != 0.0f) {
                WhatDidITouch feedback = new WhatDidITouch() { didHandTouchSomething = true, objectId = sop.objectID, armsLength = length };
                // force objec to stop moving
                rb.velocity = Vector3.zero;
                rb.angularVelocity = Vector3.zero;
                rb.Sleep();

                actionFinished(true, feedback);
            }

            // if passed in length is 0, don't return feedback cause not all
            // actions need that
            else {
                DefaultAgentHand();
                actionFinished(true, "object settled after: " + (Time.time - startTime));
            }
        }

        else {
            errorMessage = "null reference sim obj in checkIfObjectHasStoppedMoving call";
            actionFinished(false);
        }
    }

    // Sweeptest to see if the object Agent is holding will prohibit movement

    // for use with TouchThenApplyForce feedback return
    public struct WhatDidITouch {
        public bool didHandTouchSomething; // did the hand touch something or
                                           // did it hit nothing?
        public string objectId;            // id of object touched, if it is a sim object
        public float armsLength;           // the amount the hand moved from it's starting
                                           // position to hit the object touched
    }

    // checks if the target position in space is within the agent's current
    // viewport
    public bool CheckIfTargetPositionIsInViewportRange(Vector3 targetPosition) {
        // now check if the target position is within bounds of the Agent's
        // forward (z) view
        Vector3 tmp = m_Camera.transform.position;

        if (Vector3.Distance(tmp, targetPosition) > maxVisibleDistance) // + 0.3)
        {
            errorMessage = "The target position is outside the agent's max visible distance.";
            return false;
        }

        // now make sure that the targetPosition is within the Agent's x/y view,
        // restricted by camera
        Vector3 vp = m_Camera.WorldToViewportPoint(targetPosition);
        if (vp.z < 0 || vp.x > 1.0f || vp.y < 0.0f || vp.y > 1.0f || vp.y < 0.0f) {
            errorMessage = "The target position is outside the viewport.";
            return false;
        }

        return true;
    }

    // moves hand to the x, y, z coordinate, not constrained by any axis, if
    // within range

    // same as PlaceObjectAtPoint(ServerAction action) but without a server
    // action
    public bool placeObjectAtPoint(SimObjPhysics t, Vector3 position) {
        SimObjPhysics target = null;
        // find the object in the scene, disregard visibility
        foreach (SimObjPhysics sop in VisibleSimObjs(true)) {
            if (sop.objectID == t.objectID) {
                target = sop;
            }
        }

        if (target == null) {
            return false;
        }

        // make sure point we are moving the object to is valid
        if (!agentManager.sceneBounds.Contains(position)) {
            return false;
        }

        // ok let's get the distance from the simObj to the bottom most part of
        // its colliders
        Vector3 targetNegY = target.transform.position + new Vector3(0, -1, 0);
        BoxCollider b = target.boundingBoxCollider;

        b.enabled = true;
        Vector3 bottomPoint = b.ClosestPoint(targetNegY);
        b.enabled = false;

        float distFromSopToBottomPoint = Vector3.Distance(bottomPoint, target.transform.position);

        float offset = distFromSopToBottomPoint;

        // final position to place on surface
        Vector3 finalPos = GetSurfacePointBelowPosition(position) + new Vector3(0, offset, 0);

        // check spawn area, if its clear, then place object at finalPos
        InstantiatePrefabTest ipt = physicsSceneManager.GetComponent<InstantiatePrefabTest>();
        if (ipt.CheckSpawnArea(target, finalPos, target.transform.rotation, false)) {
            target.transform.position = finalPos;
            return true;
        }

        return false;
    }
    // uncomment this to debug draw valid points
    // private List<Vector3> validpointlist = new List<Vector3>();

    // return a bunch of vector3 points above a target receptacle
    // if forceVisible = true, return points regardless of where receptacle is
    // if forceVisible = false, only return points that are also within view of
    // the Agent camera

    // same as GetSpawnCoordinatesAboveReceptacle(Server Action) but takes a sim
    // obj phys instead returns a list of vector3 coordinates above a
    // receptacle. These coordinates will make up a grid above the receptacle
    public List<Vector3> getSpawnCoordinatesAboveReceptacle(SimObjPhysics t) {
        SimObjPhysics target = t;
        // ok now get spawn points from target
        List<Vector3> targetPoints = new List<Vector3>();
        targetPoints = target.FindMySpawnPointsFromTopOfTriggerBox();
        return targetPoints;
    }

    // instantiate a target circle, and then place it in a
    // "SpawnOnlyOUtsideReceptacle" that is also within camera view If fails,
    // return actionFinished(false) and despawn target circle

    public virtual void PutObject(ServerAction action) { PlaceHeldObject(action); }

    // if you are holding an object, place it on a valid Receptacle
    // used for placing objects on receptacles without enclosed restrictions
    // (drawers, cabinets, etc) only checks if the object can be placed on top
    // of the target receptacle
    public void PlaceHeldObject(ServerAction action) {
        // check if we are even holding anything
        if (ItemInHand == null) {
            errorMessage = "Can't place an object if Agent isn't holding anything";
            Debug.Log(errorMessage);
            this.lastActionStatus = Enum.GetName(typeof(ActionStatus), ActionStatus.NOT_HELD);
            actionFinished(false);
            return;
        } else {
            // Make sure object ID given is actually the item in hand
            if (!ItemInHand.transform.name.Equals(action.objectId)) {
                errorMessage = "Object ID " + action.objectId + " is not the object currently being held.";
                Debug.Log(errorMessage);
                this.lastActionStatus = Enum.GetName(typeof(ActionStatus), ActionStatus.NOT_HELD);
                actionFinished(false);
                return;
            }
        }

        if (!physicsSceneManager.ObjectIdToSimObjPhysics.ContainsKey(action.receptacleObjectId)) {
            errorMessage = "Object ID " + action.receptacleObjectId + " appears to be invalid.";
            Debug.Log(errorMessage);
            this.lastActionStatus = Enum.GetName(typeof(ActionStatus), ActionStatus.NOT_OBJECT);
            actionFinished(false);
            return;
        }

        SimObjPhysics targetReceptacle = physicsSceneManager.ObjectIdToSimObjPhysics[action.receptacleObjectId];

        // if receptacle can open, check that it's open before placing. Can't
        // place objects in something that is closed!

        if (targetReceptacle.DoesThisObjectHaveThisSecondaryProperty(SimObjSecondaryProperty.CanOpen)) {
            CanOpen_Object canOpenObject = targetReceptacle.GetComponent<CanOpen_Object>();
            if (canOpenObject != null && !canOpenObject.isOpen) {
                errorMessage = "Target openable Receptacle is CLOSED, placement is obstructed until target is opened!";
                Debug.Log(errorMessage);
                this.lastActionStatus = Enum.GetName(typeof(ActionStatus), ActionStatus.OBSTRUCTED);
                actionFinished(false);
                return;
            }
        }

        VisibilityBools visibilityBools = objectIsCurrentlyVisible(targetReceptacle, maxVisibleDistance);
        if (Vector3.Distance(transform.position, FindClosestPoint(transform.position, targetReceptacle)) < maxVisibleDistance &&
            !ObjectIsVisibleSimObjOrRoomStructure(targetReceptacle)) {
            if (!visibilityBools.inViewportAndVisible && !visibilityBools.notInViewportButNotObstructed) {
                errorMessage = "Target " + action.objectId + " is within distance of interaction but obstructed.";
                Debug.Log(errorMessage);
                this.lastActionStatus = Enum.GetName(typeof(ActionStatus), ActionStatus.NOT_VISIBLE);
                actionFinished(false);
                return;
            }

            if (!visibilityBools.inViewportAndVisible && visibilityBools.notInViewportButNotObstructed) {
                errorMessage = "Target " + action.objectId + " is within distance of interaction but not visible.";
                Debug.Log(errorMessage);
                Debug.Log(string.Format("Agent - X rotation: {0} - Y rotation {1}.", m_Camera.transform.eulerAngles.x, transform.eulerAngles.y));
                this.lastActionStatus = Enum.GetName(typeof(ActionStatus), ActionStatus.NOT_VISIBLE);
                actionFinished(false);
                return;
            }
        }

        if (!ObjectIsVisibleSimObjOrRoomStructure(targetReceptacle)) {
            errorMessage = action.objectId + " is out of reach.";
            this.lastActionStatus = Enum.GetName(typeof(ActionStatus), ActionStatus.OUT_OF_REACH);
            actionFinished(false);
            return;
        }

        if (targetReceptacle == null || !targetReceptacle.GetComponent<SimObjPhysics>()) {
            errorMessage = action.receptacleObjectId + " is not interactable.";
            this.lastActionStatus = Enum.GetName(typeof(ActionStatus), ActionStatus.NOT_INTERACTABLE);
            actionFinished(false);
            return;
        }

        if (!targetReceptacle.DoesThisObjectHaveThisSecondaryProperty(SimObjSecondaryProperty.Receptacle) || targetReceptacle.GetComponent<StructureObject>()) {
            errorMessage = "This target object is NOT a receptacle!";
            Debug.Log(errorMessage);
            this.lastActionStatus = Enum.GetName(typeof(ActionStatus), ActionStatus.NOT_RECEPTACLE);
            actionFinished(false);
            return;
        }

        // if this receptacle only receives specific objects, check that the
        // ItemInHand is compatible and check if the receptacle is currently
        // full with another valid object or not
        if (targetReceptacle.DoesThisObjectHaveThisSecondaryProperty(SimObjSecondaryProperty.ObjectSpecificReceptacle)) {
            ObjectSpecificReceptacle osr = targetReceptacle.GetComponent<ObjectSpecificReceptacle>();
            if (osr.HasSpecificType(ItemInHand.ObjType) && !osr.isFull()) {

                ItemInHand.transform.position = osr.attachPoint.position;
                ItemInHand.transform.SetParent(osr.attachPoint.transform);
                ItemInHand.transform.localRotation = Quaternion.identity;
                ItemInHand.myRigidbody.isKinematic = true;
                ItemInHand.isInAgentHand = false; // remove in agent hand flag
                ItemInHand.gameObject.layer = 8;  // SimObjVisible
                // MCS ADDED BLOCK
                ItemInHand.MyColliders.ToList().ForEach((collider) => {
                    collider.gameObject.layer = 8; // SimObjVisible
                });
                ItemInHand = null;
                DefaultAgentHand();
                this.lastActionStatus = Enum.GetName(typeof(ActionStatus), ActionStatus.SUCCESSFUL);
                actionFinished(true);
                return;
            } else {

                if (osr.attachPoint.transform.childCount > 0 || osr.isFull()) {
                    errorMessage = targetReceptacle.name + " is full right now";
                } else {
                    errorMessage = ItemInHand.name + " is not a valid Object Type to be placed in " + targetReceptacle.name;
                }

                this.lastActionStatus = Enum.GetName(typeof(ActionStatus), ActionStatus.FAILED);
                actionFinished(false);
                return;
            }
        }

        SimObjPhysics handSOP = ItemInHand;

        // MCS CHANGE NEXT LINE
        if (!action.forceAction && handSOP.ObjType != SimObjType.IgnoreType) {
            // check if the item we are holding can even be placed in the
            // action.UniqueID target at all
            foreach (KeyValuePair<SimObjType, List<SimObjType>> res in ReceptacleRestrictions.PlacementRestrictions) {
                // find the Object Type in the PlacementRestrictions dictionary
                if (res.Key == handSOP.ObjType) {
                    if (!res.Value.Contains(targetReceptacle.ObjType)) {
                        errorMessage = ItemInHand.name + " cannot be placed in " + targetReceptacle.transform.name;
                        Debug.Log(errorMessage);
                        this.lastActionStatus = Enum.GetName(typeof(ActionStatus), ActionStatus.FAILED);
                        actionFinished(false);
                        return;
                    }
                }
            }
        }

        bool onlyPointsCloseToAgent = true;

        // if the target is something like a pot or bowl on a table, return all
        // valid points instead of ONLY visible points since the Agent can't see
        // the bottom of the receptacle if it's placed too high on a table
        if (ReceptacleRestrictions.ReturnAllPoints.Contains(targetReceptacle.ObjType)) {
            onlyPointsCloseToAgent = false;
        }

        bool placeUpright = false;
        // check if the object should be forced to only check upright placement
        // angles (this prevents things like Pots being placed sideways)
        if (ReceptacleRestrictions.AlwaysPlaceUpright.Contains(handSOP.ObjType)) {
            placeUpright = true;
        }

        // ok we are holding something, time to try and place it
        InstantiatePrefabTest script = physicsSceneManager.GetComponent<InstantiatePrefabTest>();
        // set degreeIncrement to 90 for placing held objects to check for
        // vertical angles
        List<ReceptacleSpawnPoint> spawnPoints = targetReceptacle.ReturnMySpawnPoints(onlyPointsCloseToAgent);
        if (action.randomSeed != 0) {
            List<ReceptacleSpawnPoint> randomizedSpawnPoints = new List<ReceptacleSpawnPoint>();
            float maxDistance = action.z;
            if (maxDistance == 0.0f) {
                maxDistance = maxVisibleDistance;
            }
            foreach (ReceptacleSpawnPoint sp in spawnPoints) {
                Vector3 tmp = new Vector3(transform.position.x, sp.Point.y, transform.position.z);
                if (Vector3.Distance(sp.Point, tmp) < maxDistance) {
                    randomizedSpawnPoints.Add(sp);
                }
            }
            randomizedSpawnPoints.Shuffle_(action.randomSeed);
            spawnPoints = randomizedSpawnPoints;
        }

        // MCS CHANGE START
        // Remove the parent from the held object so that the parent's
        // properties (like scale) don't affect our calculations.
        Transform previousParent = handSOP.transform.parent;
        handSOP.transform.parent = null;
        // MCS CHANGE END

        if (script.PlaceObjectReceptacle(spawnPoints, ItemInHand, action.placeStationary, -1, 90, placeUpright)) {
            ItemInHand.gameObject.layer = 8; // SimObjVisible
                                             // MCS ADDED BLOCK
            ItemInHand.MyColliders.ToList().ForEach((collider) => {
                collider.gameObject.layer = 8; // SimObjVisible
            });
            ItemInHand = null;
            DefaultAgentHand();

            // MCS CHANGE START
            if (!action.placeStationary) {
                // Reset isKinematic because it was set to true when the object
                // was picked up.
                handSOP.GetComponentInChildren<Rigidbody>().isKinematic = false;
            }
            // MCS CHANGE END
            this.lastActionStatus = Enum.GetName(typeof(ActionStatus), ActionStatus.SUCCESSFUL);
            actionFinished(true);
        } else {
            // MCS CHANGE NEXT LINE
            handSOP.transform.parent = previousParent;

            errorMessage = "No valid positions to place object found";
            this.lastActionStatus = Enum.GetName(typeof(ActionStatus), ActionStatus.OBSTRUCTED);
            actionFinished(false);
            return;
        }
    }

    // used for all actions that need a sim object target
    // instead of objectId, use screen coordinates to raycast toward potential
    // targets will set the target object by reference if raycast is succesful
    public bool ScreenToWorldTarget(float x, float y, ref SimObjPhysics target, bool requireWithinViewportRange) {
        // float x = action.x;
        y = 1.0f - y; // reverse the y so that the origin (0, 0) can be passed
                      // in as the top left of the screen
        // cast ray from screen coordinate into world space. If it hits an
        // object
        Ray ray = m_Camera.ViewportPointToRay(new Vector3(x, y, 0.0f));
        RaycastHit hit;
        // if something was touched, actionFinished(true) always
        if (Physics.Raycast(ray, out hit, Mathf.Infinity, 1 << 0 | 1 << 8 | 1 << 10, QueryTriggerInteraction.Ignore)) {
            if (hit.transform.GetComponent<SimObjPhysics>()) {
                // wait! First check if the point hit is withing visibility
                // bounds (camera viewport, max distance etc) this should
                // basically only happen if the handDistance value is too big
                if (requireWithinViewportRange && !CheckIfTargetPositionIsInViewportRange(hit.point)) {
                    errorMessage = "target sim object at screen coordinate: (" + x + ", " + y + ") is not within the viewport";
                    return false;
                }

                // it is within viewport, so we are good, assign as target
                target = hit.transform.GetComponent<SimObjPhysics>();
            }
        }

        // try again, this time cast for placeable surface for things like
        // countertops or interior of cabinets
        if (target == null) {
            if (Physics.Raycast(ray, out hit, Mathf.Infinity, 1 << 11, QueryTriggerInteraction.Ignore)) {
                if (hit.transform.GetComponentInParent<SimObjPhysics>()) {
                    // wait! First check if the point hit is withing visibility
                    // bounds (camera viewport, max distance etc) this should
                    // basically only happen if the handDistance value is too
                    // big
                    if (requireWithinViewportRange && !CheckIfTargetPositionIsInViewportRange(hit.point)) {
                        errorMessage = "target sim object at screen coordinate: (" + x + ", " + y + ") is not within the viewport";
                        return false;
                    }

                    // it is within viewport, so we are good, assign as target
                    target = hit.transform.GetComponentInParent<SimObjPhysics>();
                }
            }
        }

        // force update objects to be visible/interactable correctly
        VisibleSimObjs(false);
        return true;
    }

    public virtual void PickupObject(ServerAction action) // use serveraction objectid
    {
        if (!physicsSceneManager.ObjectIdToSimObjPhysics.ContainsKey(action.objectId)) {
            errorMessage = "Object ID " + action.objectId + " appears to be invalid.";
            Debug.Log(errorMessage);
            this.lastActionStatus = Enum.GetName(typeof(ActionStatus), ActionStatus.NOT_OBJECT);
            actionFinished(false);
            return;
        }

        SimObjPhysics target = physicsSceneManager.ObjectIdToSimObjPhysics[action.objectId];
        if (target == null) {
            errorMessage = action.objectId + " is not interactable.";
            this.lastActionStatus = Enum.GetName(typeof(ActionStatus), ActionStatus.NOT_OBJECT);
            actionFinished(false);
            return;
        }

        if (IsHandDefault == false) {
            errorMessage = "Reset Hand to default position before attempting to Pick Up objects";
            this.lastActionStatus = Enum.GetName(typeof(ActionStatus), ActionStatus.FAILED);
            actionFinished(false);
            return;
        }

        VisibilityBools visibilityBools = objectIsCurrentlyVisible(target, maxVisibleDistance);
        if (Vector3.Distance(transform.position, FindClosestPoint(transform.position, target)) < maxVisibleDistance && !ObjectIsVisibleSimObjOrRoomStructure(target)) {
            if (!visibilityBools.inViewportAndVisible && !visibilityBools.notInViewportButNotObstructed) {
                errorMessage = "Target " + action.objectId + " is within distance of interaction but obstructed.";
                Debug.Log(errorMessage);
                this.lastActionStatus = Enum.GetName(typeof(ActionStatus), ActionStatus.NOT_VISIBLE);
                actionFinished(false);
                return;
            }

            if (!visibilityBools.inViewportAndVisible && visibilityBools.notInViewportButNotObstructed) {
                errorMessage = "Target " + action.objectId + " is within distance of interaction but not visible.";
                Debug.Log(errorMessage);
                Debug.Log(string.Format("Agent - X rotation: {0} - Y rotation {1}.", m_Camera.transform.eulerAngles.x, transform.eulerAngles.y));
                this.lastActionStatus = Enum.GetName(typeof(ActionStatus), ActionStatus.NOT_VISIBLE);
                actionFinished(false);
                return;
            }
        }

        if (!ObjectIsVisibleSimObjOrRoomStructure(target)) {
            errorMessage = action.objectId + " is out of reach.";
            this.lastActionStatus = Enum.GetName(typeof(ActionStatus), ActionStatus.OUT_OF_REACH);
            actionFinished(false);
            return;
        }

        if (target.GetComponent<StructureObject>() && Vector3.Distance(transform.position, FindClosestPoint(transform.position, target)) < maxVisibleDistance) {
            errorMessage = action.objectId + " is not interactable.";
            this.lastActionStatus = Enum.GetName(typeof(ActionStatus), ActionStatus.NOT_PICKUPABLE);
            actionFinished(false);
            return;
        }

        if (!target.GetComponent<SimObjPhysics>()) {
            errorMessage = action.objectId + " is not interactable.";
            this.lastActionStatus = Enum.GetName(typeof(ActionStatus), ActionStatus.NOT_INTERACTABLE);
            actionFinished(false);
            return;
        }

        if (target.PrimaryProperty != SimObjPrimaryProperty.CanPickup) {
            errorMessage = action.objectId + " must have the property CanPickup to be picked up.";
            this.lastActionStatus = Enum.GetName(typeof(ActionStatus), ActionStatus.NOT_PICKUPABLE);
            actionFinished(false);
            return;
        }

        // MCS Addition
        // if pickup action is being abstracted, don't teleport target to hand
        // instead move hand to target and allow for immediate manipulation from
        // where the object was
        if (tryPickupTarget(target, action, action.manualInteract)) {
            // we have succesfully picked up something!
            SimObjPhysics sop = target.GetComponent<SimObjPhysics>();
            sop.isInAgentHand = true;
            sop.transform.localEulerAngles = Vector3.zero;
            sop.myRigidbody.isKinematic = false;
            UpdateAgentObjectAssociations(sop);
            this.lastActionStatus = Enum.GetName(typeof(ActionStatus), ActionStatus.SUCCESSFUL);
            actionFinished(true, target.ObjectID);
            return;
        }

        else {
            errorMessage = "Picking up object would cause it to collide and clip into something!";
            actionFinished(false);
            return;
        }
    }

    public virtual void UpdateAgentObjectAssociations(SimObjPhysics sop) {}

    public bool tryPickupTarget(SimObjPhysics target, ServerAction action, bool manualInteract = false) {
        // save all initial values in case we need to reset on action fail
        Vector3 savedPos = target.transform.position;
        Quaternion savedRot = target.transform.rotation;
        Transform savedParent = target.transform.parent;

        // oh also save kinematic values in case we need to reset
        Rigidbody rb = target.GetComponent<Rigidbody>();
        bool wasKinematic = rb.isKinematic;

        // in preparation for object being held, force collision detection to
        // discrete and make sure kinematic = true
        rb.collisionDetectionMode = CollisionDetectionMode.Discrete;
        rb.isKinematic = true;

        // run this to pickup any contained objects if object is a receptacle
        // if the target is rotated too much, don't try to pick up any contained
        // objects since they would fall out
        if (Vector3.Angle(target.transform.up, Vector3.up) < 60) {
            PickupContainedObjects(target);
        }

        if (!manualInteract) {
            // by default, abstract agent hand pickup so that object teleports
            // to hand and changes orientation to match agent

            // agent's hand is in default position in front of camera, teleport
            // object into agent's hand
            target.transform.position = AgentHand.transform.position;
            // target.transform.rotation = AgentHand.transform.rotation; - keep
            // this line if we ever want to change the pickup position to be
            // constant relative to the Agent Hand and Agent Camera rather than
            // aligned by world axis
        } else {
            // in manualInteract mode, move the hand to the object, and require
            // agent hand manipulation to move object around or move closer to
            // agent

            AgentHand.transform.position = target.transform.position;
            // don't rotate target at all as we are moving the hand to the
            // object in manualInteract = True mode
        }

        // MCS ADDED NEXT LINE
        Quaternion previousRotation = target.transform.localRotation;
        target.transform.SetParent(AgentHand.transform);
        target.transform.localRotation = previousRotation;
        ItemInHand = target;
        ItemInHand.gameObject.layer = 9; // SimObjInvisible
        // MCS ADDED BLOCK
        ItemInHand.MyColliders.ToList().ForEach((collider) => {
            collider.gameObject.layer = 9; // SimObjInvisible
        });

        // we have succesfully picked up something!
        // target.GetComponent<SimObjPhysics>().isInAgentHand = true;
        // this.lastActionStatus = Enum.GetName(typeof(ActionStatus),
        // ActionStatus.SUCCESSFUL); actionFinished(true, target.ObjectID);
        return true;
    }

    // make sure not to pick up any sliced objects because those should remain
    // uninteractable i they have been sliced
    public void PickupContainedObjects(SimObjPhysics target) {
        // Removed
        // !target.DoesThisObjectHaveThisSecondaryProperty(SimObjSecondaryProperty.Stacking
        // check because it causes items in bowl not being picked up.
        if (target.DoesThisObjectHaveThisSecondaryProperty(SimObjSecondaryProperty.Receptacle)) {
            foreach (SimObjPhysics sop in target.SimObjectsContainedByReceptacle) {
                // for every object that is contained by this object...first
                // make sure it's pickupable so we don't like, grab a Chair if
                // it happened to be in the receptacle box or something turn off
                // the colliders (so contained object doesn't block movement),
                // leaving Trigger Colliders active (this is important to
                // maintain visibility!)
                if (sop.PrimaryProperty == SimObjPrimaryProperty.CanPickup) {
                    // wait! check if this object is sliceable and is sliced, if
                    // so SKIP!
                    if (sop.DoesThisObjectHaveThisSecondaryProperty(SimObjSecondaryProperty.CanBeSliced)) {
                        // if this object is sliced, don't pick it up because it
                        // is effectively disabled
                        // if (sop.GetComponent<SliceObject>().IsSliced()) {
                        //     target.RemoveFromContainedObjectReferences(sop);
                        //     break;
                        // }
                    }

                    if (sop.transform.Find("Colliders") != null) {
                        sop.transform.Find("Colliders").gameObject.SetActive(false);
                    }
                    sop.MyColliders.ToList().ForEach((collider) => { collider.enabled = false; });
                    Rigidbody soprb = sop.GetComponent<Rigidbody>();
                    soprb.collisionDetectionMode = CollisionDetectionMode.Discrete;
                    soprb.isKinematic = true;
                    sop.transform.SetParent(target.transform);

                    // used to reference objects in the receptacle that is being
                    // picked up without having to search through all children
                    target.AddToContainedObjectReferences(sop);

                    target.GetComponent<SimObjPhysics>().isInAgentHand = true; // agent hand flag
                }
            }
        }
    }

    public void DropContainedObjects(SimObjPhysics target) {
        if (target.DoesThisObjectHaveThisSecondaryProperty(SimObjSecondaryProperty.Receptacle)) {
            GameObject topObject = GameObject.Find("Objects");

            foreach (SimObjPhysics sop in target.ContainedObjectReferences) {
                // for every object that is contained by this object...
                // turn off the colliders, leaving Trigger Colliders active
                // (this is important to maintain visibility!)
                if (sop.transform.Find("Colliders") != null) {
                    sop.transform.Find("Colliders").gameObject.SetActive(true);
                }
                sop.MyColliders.ToList().ForEach((collider) => { collider.enabled = true; });
                Rigidbody rb = sop.GetComponent<Rigidbody>();

                rb.isKinematic = false;
                rb.collisionDetectionMode = CollisionDetectionMode.ContinuousSpeculative;

                sop.isInAgentHand = false; // agent hand flag
                sop.transform.SetParent(topObject.transform);
            }

            target.ClearContainedObjectReferences();
        }
    }

    public void DropContainedObjectsStationary(SimObjPhysics target) {
        if (target.DoesThisObjectHaveThisSecondaryProperty(SimObjSecondaryProperty.Receptacle)) {

            foreach (SimObjPhysics sop in target.ContainedObjectReferences) {
                // for every object that is contained by this object...
                // turn off the colliders, leaving Trigger Colliders active
                // (this is important to maintain visibility!)
                if (sop.transform.Find("Colliders") != null) {
                    sop.transform.Find("Colliders").gameObject.SetActive(true);
                }
                sop.MyColliders.ToList().ForEach((collider) => { collider.enabled = true; });
                sop.isInAgentHand = false; // agent hand flag
            }
            target.ClearContainedObjectReferences();
        }
    }

    private IEnumerator checkDropHandObjectActionFast(SimObjPhysics currentHandSimObj) {
        if (currentHandSimObj != null) {
            Rigidbody rb = currentHandSimObj.GetComponentInChildren<Rigidbody>();
            Physics.autoSimulation = false;
            // MCS TODO (?) Should this line be changed to: yield return new WaitForFixedUpdate();
            yield return null;

            for (int i = 0; i < 100; i++) {
                Physics.Simulate(0.04f);
#if UNITY_EDITOR
                // MCS TODO (?) Should this line be changed to: yield return new WaitForFixedUpdate();
                yield return null;
#endif
                if (Math.Abs(rb.angularVelocity.sqrMagnitude + rb.velocity.sqrMagnitude) < 0.00001) {
                    break;
                }
            }
            Physics.autoSimulation = true;
        }

        DefaultAgentHand();
        actionFinished(true);
    }

    public virtual bool DropHandObject(ServerAction action) {
        // make sure something is actually in our hands
        if (ItemInHand != null) {

            if (!physicsSceneManager.ObjectIdToSimObjPhysics.ContainsKey(action.objectId)) {
                errorMessage = "Object ID appears to be invalid.";
                Debug.Log(errorMessage);
                this.lastActionStatus = Enum.GetName(typeof(ActionStatus), ActionStatus.NOT_OBJECT);
                actionFinished(false);
                return false;
            }

            if (!ItemInHand.transform.name.Equals(action.objectId)) {
                errorMessage = "Object ID " + action.objectId + " is not the object currently being held.";
                Debug.Log(errorMessage);
                this.lastActionStatus = Enum.GetName(typeof(ActionStatus), ActionStatus.NOT_HELD);
                actionFinished(false);
                return false;
            }

            // we do need this to check if the item is currently colliding with
            // the agent, otherwise dropping an object while it is inside the
            // agent will cause it to shoot out weirdly
            if (!action.forceAction && isHandObjectColliding(false)) {
                errorMessage = ItemInHand.transform.name + " can't be dropped. It must be clear of all other collision first, including the Agent";
                Debug.Log(errorMessage);
                this.lastActionStatus = Enum.GetName(typeof(ActionStatus), ActionStatus.OBSTRUCTED);
                actionFinished(false);
                return false;
            } else {
                Rigidbody rb = ItemInHand.myRigidbody;
                rb.isKinematic = false;
                rb.constraints = RigidbodyConstraints.None;
                rb.useGravity = true;

                // change collision detection mode while falling so that obejcts
                // don't phase through colliders. this is reset to discrete on
                // SimObjPhysics.cs's update
                rb.collisionDetectionMode = CollisionDetectionMode.ContinuousSpeculative;

                GameObject topObject = GameObject.Find("Objects");
                if (topObject != null) {
                    ItemInHand.transform.parent = topObject.transform;
                } else {
                    ItemInHand.transform.parent = null;
                }

                // Add some random rotational momentum to the dropped object to
                // make things less deterministic.
                // TODO: Need a parameter to control how much randomness we
                // introduce.
                rb.angularVelocity = Vector3.zero;

                DropContainedObjects(ItemInHand);

                this.lastActionStatus = Enum.GetName(typeof(ActionStatus), ActionStatus.SUCCESSFUL);

                // if physics simulation has been paused by the
                // PausePhysicsAutoSim() action, don't do any coroutine checks
                if (!physicsSceneManager.physicsSimulationPaused) {
                    // this is true by default
                    if (action.autoSimulation) {
                        StartCoroutine(checkIfObjectHasStoppedMoving(ItemInHand, 0));
                    }

                    else {
                        StartCoroutine(checkDropHandObjectActionFast(ItemInHand));
                    }
                }

                else
                    actionFinished(true);

                ItemInHand.isInAgentHand = false;
                ItemInHand.gameObject.layer = 8; // SimObjVisible
                // MCS ADDED BLOCK
                ItemInHand.MyColliders.ToList().ForEach((collider) => {
                    collider.gameObject.layer = 8; // SimObjVisible
                });
                ItemInHand = null;
                return true;
            }
        } else {
            errorMessage = "nothing in hand to drop!";
            Debug.Log(errorMessage);
            this.lastActionStatus = Enum.GetName(typeof(ActionStatus), ActionStatus.NOT_HELD);
            actionFinished(false);
            return false;
        }
    }

    // by default will throw in the forward direction relative to the Agent's
    // Camera moveMagnitude, strength of throw, good values for an average throw
    // are around 150-250
    public virtual void ThrowObject(ServerAction action) {
        if (ItemInHand == null) {
            errorMessage = "Nothing in Hand to Throw!";
            Debug.Log(errorMessage);
            actionFinished(false);
            return;
        }

        GameObject go = ItemInHand.gameObject;
        DropHandObject(action);
        if (this.lastActionSuccess) {
            Vector3 dir = m_Camera.transform.forward;
            go.GetComponent<SimObjPhysics>().ApplyForce(dir, action.moveMagnitude);
        }
    }

    // Hide and Seek helper function, makes overlap box at x,z coordinates

    public virtual void CloseObject(ServerAction action) {
        // pass name of object in from action.objectID
        // check if that object is in the viewport
        // also check to make sure that target object is interactable
        if (action.objectId == null) {
            Debug.Log("Object ID cannot be empty for closing object");
            this.lastActionStatus = Enum.GetName(typeof(ActionStatus), ActionStatus.NOT_OBJECT);
            errorMessage = "objectId required for CloseObject";
            actionFinished(false);
            return;
        }

        if (!physicsSceneManager.ObjectIdToSimObjPhysics.ContainsKey(action.objectId)) {
            errorMessage = "Object ID " + action.objectId + " appears to be invalid.";
            Debug.Log(errorMessage);
            this.lastActionStatus = Enum.GetName(typeof(ActionStatus), ActionStatus.NOT_OBJECT);
            actionFinished(false);
            return;
        }

        SimObjPhysics target = physicsSceneManager.ObjectIdToSimObjPhysics[action.objectId];
        if (action.forceAction) {
            action.forceVisible = true;
        }
        // no target object specified, so instead try and use x/y screen
        // coordinates
        if (action.objectId == null) {
            if (!ScreenToWorldTarget(action.x, action.y, ref target, !action.forceAction)) {
                // error message is set insice ScreenToWorldTarget
                actionFinished(false);
                return;
            }
        }

        SimObjPhysics[] visibleObjects = VisibleSimObjs(action);
        VisibilityBools visibilityBools = objectIsCurrentlyVisible(target, maxVisibleDistance);
        if (Vector3.Distance(transform.position, FindClosestPoint(transform.position, target)) < maxVisibleDistance && !ObjectIsVisibleSimObjOrRoomStructure(target)) {
            if (!visibilityBools.inViewportAndVisible && !visibilityBools.notInViewportButNotObstructed) {
                errorMessage = "Target " + action.objectId + " is within distance of interaction but obstructed.";
                Debug.Log(errorMessage);
                this.lastActionStatus = Enum.GetName(typeof(ActionStatus), ActionStatus.NOT_VISIBLE);
                actionFinished(false);
                return;
            }

            if (!visibilityBools.inViewportAndVisible && visibilityBools.notInViewportButNotObstructed) {
                errorMessage = "Target " + action.objectId + " is within distance of interaction but not visible.";
                Debug.Log(errorMessage);
                Debug.Log(string.Format("Agent - X rotation: {0} - Y rotation {1}.", m_Camera.transform.eulerAngles.x, transform.eulerAngles.y));
                this.lastActionStatus = Enum.GetName(typeof(ActionStatus), ActionStatus.NOT_VISIBLE);
                actionFinished(false);
                return;
            }
        }

        if (!ObjectIsVisibleSimObjOrRoomStructure(target)) {
            errorMessage = action.objectId + " is out of reach.";
            this.lastActionStatus = Enum.GetName(typeof(ActionStatus), ActionStatus.OUT_OF_REACH);
            actionFinished(false);
            return;
        }

        if (!target.GetComponent<SimObjPhysics>()) {
            errorMessage = action.objectId + " is not interactable.";
            this.lastActionStatus = Enum.GetName(typeof(ActionStatus), ActionStatus.NOT_INTERACTABLE);
            actionFinished(false);
            return;
        }

        if (target != null && (!target.DoesThisObjectHaveThisSecondaryProperty(SimObjSecondaryProperty.CanOpen) || target.GetComponent<StructureObject>())) {
            errorMessage = "This target object is NOT openable!";
            Debug.Log(errorMessage);
            this.lastActionStatus = Enum.GetName(typeof(ActionStatus), ActionStatus.NOT_OPENABLE);
            actionFinished(false);
            return;
        }

        foreach (SimObjPhysics sop in visibleObjects) {
            if (sop.GetComponent<CanOpen_Object>()) {
                target = sop;
            }
        }

        if (target != null) {
            if (!action.forceAction && target.isInteractable == false) {
                this.lastActionStatus = Enum.GetName(typeof(ActionStatus), ActionStatus.NOT_VISIBLE);
                errorMessage = "object is within distance of interaction but obstructed: " + action.objectId;
                actionFinished(false);
                return;
            }

            if (target.GetComponent<CanOpen_Object>()) {
                CanOpen_Object codd = target.GetComponent<CanOpen_Object>();

                //[REVIEW] original line was using function no longer available
                // float previousOpenPercent = codd.GetOpenPercent();
                float previousOpenPercent = codd.currentOpenPercentage;

                // pass in percentage close if desired
                if (action.moveMagnitude > 0.0f) {
                    // if this fails, invalid percentage given
                    if (!codd.SetOpenPercent(1 - action.moveMagnitude)) {
                        this.lastActionStatus = Enum.GetName(typeof(ActionStatus), ActionStatus.FAILED);
                        errorMessage = "Please give an close percentage between 0.0f and 1.0f";
                        actionFinished(false);
                        return;
                    }
                }

                // if object is open, close it
                if (codd.isOpen) {
                    if (Physics.autoSimulation) {
                        StartCoroutine(InteractAndWait(codd, action.simplifyPhysics, previousOpenPercent));
                        return;
                    } else {
                        OpenOrCloseObject(codd, previousOpenPercent);
                        return;
                    }
                } else {
                    this.lastActionStatus = Enum.GetName(typeof(ActionStatus), ActionStatus.IS_CLOSED_COMPLETELY);
                    errorMessage = "object already closed: " + action.objectId;
                    actionFinished(false);
                    return;
                }
            }
        }
        this.lastActionStatus = Enum.GetName(typeof(ActionStatus), ActionStatus.NOT_OPENABLE);
        errorMessage = "Object " + action.objectId + " is not openable.";
        actionFinished(false);
    }

    protected bool isAgentOnTopOfObject(CanOpen_Object canOpen) {
        // currently only handle openeable objects that can slide out into agent
        // (like drawers)
        if (canOpen.IsMovementTypeSlide()) {
            RaycastHit hit;
            GameObject attachedGameObj = canOpen.GetComponentInParent<SimObjPhysics>().gameObject;

            if (Physics.Raycast(transform.position, Vector3.down, out hit, transform.position.y, 1 << 8, QueryTriggerInteraction.Ignore)) {
                if (hit.rigidbody != null && hit.rigidbody.gameObject.Equals(attachedGameObj)) {
                    return true;
                }
            }
        }

        return false;
    }

    protected void OpenOrCloseObject(CanOpen_Object canOpen, float previousOpenPercent, bool restrictOpenDoors = false, bool restrictOpenObjects = false) {
        if (canOpen.GetComponent<SimObjPhysics>().IsLid || canOpen.separateLid != null) {
            canOpen.InteractWithLid(canOpen.separateLid);
            this.lastActionStatus = Enum.GetName(typeof(ActionStatus), ActionStatus.SUCCESSFUL);
            actionFinished(true);
            lockOtherObjectsInTheScene(canOpen, true, restrictOpenDoors, restrictOpenObjects);
            return;
        }

        List<Collider> collidersDisabled = new List<Collider>();
        foreach (Collider collider in this.GetComponentsInChildren<Collider>()) {
            if (collider.enabled) {
                collidersDisabled.Add(collider);
                collider.enabled = false;
            }
        }

        Dictionary<string, Transform> previousParents = new Dictionary<string, Transform>();

        if (canOpen.GetComponent<SimObjPhysics>().IsReceptacle) {
            foreach (SimObjPhysics item in canOpen.GetComponent<SimObjPhysics>().SimObjectsContainedByReceptacle) {
                previousParents.Add(item.objectID, item.transform.parent);
                item.transform.SetParent(canOpen.GetComponent<SimObjPhysics>().transform);
            }
        }

        bool success;
        canOpen.Interact();

        GameObject openedObject = canOpen.GetComponentInParent<SimObjPhysics>().gameObject;

        if (isAgentCapsuleCollidingWith(openedObject, ExpandAgentCapsuleBy) || isHandObjectCollidingWith(openedObject) || isAgentOnTopOfObject(canOpen)) {
            this.lastActionStatus = Enum.GetName(typeof(ActionStatus), ActionStatus.OBSTRUCTED);
            Debug.Log("Object failed to open/close successfully.");
            success = false;
            canOpen.SetOpenPercent(previousOpenPercent);
            canOpen.Interact();
            RemoveTriggerBy(canOpen);
        } else {
            this.lastActionStatus = Enum.GetName(typeof(ActionStatus), ActionStatus.SUCCESSFUL);
            success = true;
            CheckTriggeredByIndexValid(canOpen);
        }

        foreach (Collider collider in collidersDisabled) {
            collider.enabled = true;
        }

        if (canOpen.GetComponent<SimObjPhysics>().IsReceptacle) {
            foreach (SimObjPhysics item in canOpen.GetComponent<SimObjPhysics>().SimObjectsContainedByReceptacle) {
                if (previousParents.ContainsKey(item.objectID)) {
                    item.transform.SetParent(previousParents[item.objectID]);
                }
            }
        }

        lockOtherObjectsInTheScene(canOpen, success, restrictOpenDoors, restrictOpenObjects);

        actionFinished(success);
    }

    protected void lockOtherObjectsInTheScene(CanOpen_Object canOpen, bool success, bool restrictOpenDoors, bool restrictOpenObjects) {
        bool lockOtherObjects = (canOpen.isDoor && restrictOpenDoors) || restrictOpenObjects;
        if (success && canOpen.isOpen && lockOtherObjects) {
            string openedId = canOpen.GetComponent<SimObjPhysics>().objectID;
            CanOpen_Object[] openableObjs = GameObject.FindObjectsOfType<CanOpen_Object>();
            foreach (CanOpen_Object co in openableObjs) {
                // If restrictOpenDoors is true and restrictOpenObjects is
                // false, skip this object if it's not a door.
                if (!restrictOpenObjects && !co.isDoor) {
                    continue;
                }
                if (openedId != co.GetComponent<SimObjPhysics>().objectID && !co.locked) {
                    co.locked = true;
                }
            }
        }
    }

    protected IEnumerator InteractAndWait(CanOpen_Object coo, bool freezeContained = false, float openPercent = 1.0f, bool restrictOpenDoors = false,
                                          bool restrictOpenObjects = false) {
        bool ignoreAgentInTransition = true;

        List<Collider> collidersDisabled = new List<Collider>();
        if (ignoreAgentInTransition) {
            foreach (Collider c in this.GetComponentsInChildren<Collider>()) {
                if (c.enabled) {
                    collidersDisabled.Add(c);
                    c.enabled = false;
                }
            }
        }

        Dictionary<string, Transform> objectIdToOldParent = null;
        SimObjPhysics target = null;
        if (freezeContained) {
            target = ancestorSimObjPhysics(coo.gameObject);
            objectIdToOldParent = new Dictionary<string, Transform>();
            foreach (string objectId in target.GetAllSimObjectsInReceptacleTriggersByObjectID()) {
                SimObjPhysics toReParent = physicsSceneManager.ObjectIdToSimObjPhysics[objectId];
                objectIdToOldParent[toReParent.ObjectID] = toReParent.transform.parent;
                toReParent.transform.parent = coo.transform;
                toReParent.GetComponent<Rigidbody>().isKinematic = true;
            }
        }

        Dictionary<string, Transform> previousParents = new Dictionary<string, Transform>();

        if (coo.GetComponent<SimObjPhysics>().IsReceptacle) {
            foreach (SimObjPhysics item in coo.GetComponent<SimObjPhysics>().SimObjectsContainedByReceptacle) {
                previousParents.Add(item.objectID, item.transform.parent);
                item.transform.SetParent(coo.GetComponent<SimObjPhysics>().transform);
            }
        }

        bool success = false;
        if (coo != null) {
            coo.Interact();
        }

        success = true;
        // MCS TODO (?) Should this line be changed to: yield return new WaitForFixedUpdate();
        yield return null;

        if (ignoreAgentInTransition) {
            GameObject openedObject = null;
            openedObject = coo.GetComponentInParent<SimObjPhysics>().gameObject;

            if (isAgentCapsuleCollidingWith(openedObject, ExpandAgentCapsuleBy) || isHandObjectCollidingWith(openedObject) || isAgentOnTopOfObject(coo)) {
                Debug.Log("Object failed to open/close successfully.");
                this.lastActionStatus = Enum.GetName(typeof(ActionStatus), ActionStatus.OBSTRUCTED);
                success = false;
                if (coo != null) {
                    //[REVIEW] original line was using function no longer
                    // available
                    // float previousOpenPercent = codd.GetOpenPercent();
                    float previousOpenPercent = coo.currentOpenPercentage;
                    coo.SetOpenPercent(previousOpenPercent);
                    coo.Interact();
                }
                // MCS TODO (?) Should this line be changed to: yield return new WaitForFixedUpdate();
                yield return null;
            }

            foreach (Collider c in collidersDisabled) {
                c.enabled = true;
            }
        }

        if (coo.GetComponent<SimObjPhysics>().IsReceptacle) {
            foreach (SimObjPhysics item in coo.GetComponent<SimObjPhysics>().SimObjectsContainedByReceptacle) {
                if (previousParents.ContainsKey(item.objectID)) {
                    item.transform.SetParent(previousParents[item.objectID]);
                }
            }
            if (freezeContained) {
                foreach (string objectId in objectIdToOldParent.Keys) {
                    SimObjPhysics toReParent = physicsSceneManager.ObjectIdToSimObjPhysics[objectId];
                    toReParent.transform.parent = objectIdToOldParent[toReParent.ObjectID];
                    Rigidbody rb = toReParent.GetComponent<Rigidbody>();
                    rb.velocity = new Vector3(0f, 0f, 0f);
                    rb.angularVelocity = new Vector3(0f, 0f, 0f);
                    rb.isKinematic = false;
                }
            }

            if (!success) {
                errorMessage = "Object failed to open/close successfully.";
            }
        }

        lockOtherObjectsInTheScene(coo, success, restrictOpenDoors, restrictOpenObjects);

        actionFinished(success);
    }

    public bool ObjectIsVisibleSimObjOrRoomStructure(SimObjPhysics target) {
        SimObjPhysics[] visibleSimObjects = VisibleSimObjs(new ServerAction());
        bool isAVisibleSimObject = FindObjectInVisibleSimObjPhysics(target.objectID, visibleSimObjects) != null;
        if (isAVisibleSimObject)
            return true;
        // if its a room wall or floor and using objectId, when within distance
        // never return NOT_VISIBLE because there is only one visibility point
        // return the appropriate non-interactable return status (NOT_MOVEABLE,
        // NOT_OPENABLE)
        return MCSMain.ROOM_STRUCTURES.Contains(target.objectID) && Vector3.Distance(transform.position, FindClosestPoint(transform.position, target)) < maxVisibleDistance;
    }

    public virtual void OpenObject(ServerAction action) {
        // pass name of object in from action.objectID
        // check if that object is in the viewport
        // also check to make sure that target object is interactable
        if (action.objectId == null) {
            this.lastActionStatus = Enum.GetName(typeof(ActionStatus), ActionStatus.NOT_OBJECT);
            errorMessage = "objectId required for OpenObject";
            actionFinished(false);
            return;
        }

        if (!physicsSceneManager.ObjectIdToSimObjPhysics.ContainsKey(action.objectId)) {
            errorMessage = "Object ID " + action.objectId + " appears to be invalid.";
            Debug.Log(errorMessage);
            this.lastActionStatus = Enum.GetName(typeof(ActionStatus), ActionStatus.NOT_OBJECT);
            actionFinished(false);
            return;
        }

        SimObjPhysics target = physicsSceneManager.ObjectIdToSimObjPhysics[action.objectId];
        if (action.forceAction) {
            action.forceVisible = true;
        }
        // no target object specified, so instead try and use x/y screen
        // coordinates
        if (action.objectId == null) {
            if (!ScreenToWorldTarget(action.x, action.y, ref target, !action.forceAction)) {
                // error message is set insice ScreenToWorldTarget
                actionFinished(false);
                return;
            }
        }

        target = physicsSceneManager.ObjectIdToSimObjPhysics[action.objectId];
        SimObjPhysics[] visibleObjects = VisibleSimObjs(action);
        VisibilityBools visibilityBools = objectIsCurrentlyVisible(target, maxVisibleDistance);
        if (Vector3.Distance(transform.position, FindClosestPoint(transform.position, target)) < maxVisibleDistance && !ObjectIsVisibleSimObjOrRoomStructure(target)) {
            if (!visibilityBools.inViewportAndVisible && !visibilityBools.notInViewportButNotObstructed) {
                errorMessage = "Target " + action.objectId + " is within distance of interaction but obstructed.";
                Debug.Log(errorMessage);
                this.lastActionStatus = Enum.GetName(typeof(ActionStatus), ActionStatus.NOT_VISIBLE);
                actionFinished(false);
                return;
            }

            if (!visibilityBools.inViewportAndVisible && visibilityBools.notInViewportButNotObstructed) {
                errorMessage = "Target " + action.objectId + " is within distance of interaction but not visible.";
                Debug.Log(errorMessage);
                Debug.Log(string.Format("Agent - X rotation: {0} - Y rotation {1}.", m_Camera.transform.eulerAngles.x, transform.eulerAngles.y));
                this.lastActionStatus = Enum.GetName(typeof(ActionStatus), ActionStatus.NOT_VISIBLE);
                actionFinished(false);
                return;
            }
        }

        if (!ObjectIsVisibleSimObjOrRoomStructure(target)) {
            errorMessage = action.objectId + " is out of reach.";
            this.lastActionStatus = Enum.GetName(typeof(ActionStatus), ActionStatus.OUT_OF_REACH);
            actionFinished(false);
            return;
        }

        if (!target.GetComponent<SimObjPhysics>()) {
            errorMessage = action.objectId + " is not interactable.";
            this.lastActionStatus = Enum.GetName(typeof(ActionStatus), ActionStatus.NOT_INTERACTABLE);
            actionFinished(false);
            return;
        }

        if (target != null && (!target.DoesThisObjectHaveThisSecondaryProperty(SimObjSecondaryProperty.CanOpen) || target.GetComponent<StructureObject>())) {
            errorMessage = "This target object is NOT openable!";
            Debug.Log(errorMessage);
            this.lastActionStatus = Enum.GetName(typeof(ActionStatus), ActionStatus.NOT_OPENABLE);
            actionFinished(false);
            return;
        }

        foreach (SimObjPhysics sop in visibleObjects) {
            // check for CanOpen drawers, cabinets or CanOpen_Fridge fridge
            // objects
            if (sop.GetComponent<CanOpen_Object>()) {
                target = sop;
            }
        }

        if (target != null) {
            if (!action.forceAction && target.isInteractable == false) {
                this.lastActionStatus = Enum.GetName(typeof(ActionStatus), ActionStatus.NOT_VISIBLE);
                actionFinished(false);
                errorMessage = "object is visible but occluded by something: " + action.objectId;
                return;
            }

            if (!target.GetComponent<CanOpen_Object>()) {
                this.lastActionStatus = Enum.GetName(typeof(ActionStatus), ActionStatus.NOT_OPENABLE);
                errorMessage = "object must be Openable to open";
                actionFinished(false);
                return;
            }

            if (target.GetComponent<CanOpen_Object>()) {
                CanOpen_Object codd = target.GetComponent<CanOpen_Object>();

                //[REVIEW] original line was using function no longer available
                // float previousOpenPercent = codd.GetOpenPercent();
                float previousOpenPercent = codd.currentOpenPercentage;

                // check to make sure object is closed
                if (codd.isOpen && codd.isOpenByPercentage == 1) {
                    this.lastActionStatus = Enum.GetName(typeof(ActionStatus), ActionStatus.IS_OPENED_COMPLETELY);
                    errorMessage = "Object already open and can't be opened again until closed fully";
                    actionFinished(false);
                    return;
                }

                if (codd.WhatReceptaclesMustBeOffToOpen().Contains(target.Type)) {
                    if (target.GetComponent<CanToggleOnOff>().isOn) {
                        // TODO MCS toggleable objects
                        this.lastActionStatus = Enum.GetName(typeof(ActionStatus), ActionStatus.FAILED);
                        errorMessage = "Target must be OFF to open!";
                        actionFinished(false);
                        return;
                    }
                }

                if (codd.locked) {
                    this.lastActionStatus = Enum.GetName(typeof(ActionStatus), ActionStatus.IS_LOCKED);
                    errorMessage = "Object " + action.objectId + " is locked.";
                    actionFinished(false);
                    return;
                }

                // pass in percentage open if desired
                if (action.moveMagnitude > 0.0f) {
                    // if this fails, invalid percentage given
                    if (!codd.SetOpenPercent(action.moveMagnitude)) {
                        this.lastActionStatus = Enum.GetName(typeof(ActionStatus), ActionStatus.FAILED);
                        errorMessage = "Please give an open percentage between 0.0f and 1.0f";
                        actionFinished(false);
                        return;
                    }
                }

                if (Physics.autoSimulation) {
                    StartCoroutine(InteractAndWait(codd, false, previousOpenPercent, action.restrictOpenDoors, action.restrictOpenObjects));
                } else {
                    OpenOrCloseObject(codd, previousOpenPercent, action.restrictOpenDoors, action.restrictOpenObjects);
                }
            }
        } else {
            this.lastActionStatus = Enum.GetName(typeof(ActionStatus), ActionStatus.NOT_OPENABLE);
            errorMessage = "Object " + action.objectId + " is not openable.";
            actionFinished(false);
        }
    }

    // open an object without returning actionFinished since this is used in the
    // setup function

    ///////////////////////////////////
    ///// DATA GENERATION HELPERS /////
    ///////////////////////////////////

    // this is a combination of objectIsWithinViewport and
    // objectIsCurrentlyVisible, specifically to check if a single sim object is
    // on screen regardless of agent visibility maxDistance DO NOT USE THIS FOR
    // ALL OBJECTS cause it's going to be soooo expensive
    public bool objectIsOnScreen(SimObjPhysics sop) {
        bool result = false;
        if (sop.VisibilityPoints.Length > 0) {
            Transform[] visPoints = sop.VisibilityPoints;
            foreach (Transform point in visPoints) {
                Vector3 viewPoint = m_Camera.WorldToViewportPoint(point.position);
                float ViewPointRangeHigh = 1.0f;
                float ViewPointRangeLow = 0.0f;

                // first make sure the vis point is within the viewport at all
                if (viewPoint.z > 0 && viewPoint.x < ViewPointRangeHigh && viewPoint.x > ViewPointRangeLow && // within x bounds of viewport
                    viewPoint.y < ViewPointRangeHigh && viewPoint.y > ViewPointRangeLow                       // within y bounds of viewport
                ) {
                    // ok so it is within the viewport, not lets do a raycast to
                    // see if we can see the vis point
                    updateAllAgentCollidersForVisibilityCheck(false);
                    // raycast from agentcamera to point, ignore triggers, use
                    // layers 8 and 10
                    RaycastHit hit;

                    if (Physics.Raycast(m_Camera.transform.position, (point.position - m_Camera.transform.position), out hit, Mathf.Infinity, (1 << 8) | (1 << 10))) {
                        if (hit.transform != sop.transform)
                            result = false;

                        else {
                            result = true;
                            break;
                        }
                    }
                }
            }

            updateAllAgentCollidersForVisibilityCheck(true);
            return result;
        }

        else {
#if UNITY_EDITOR
            Debug.Log("Error! Set at least 1 visibility point on SimObjPhysics prefab!");
#endif
        }

        return false;
    }

    public bool objectIsWithinViewport(SimObjPhysics sop) {
        if (sop.VisibilityPoints.Length > 0) {
            Transform[] visPoints = sop.VisibilityPoints;
            foreach (Transform point in visPoints) {
                Vector3 viewPoint = m_Camera.WorldToViewportPoint(point.position);
                float ViewPointRangeHigh = 1.0f;
                float ViewPointRangeLow = 0.0f;

                if (viewPoint.z > 0 && viewPoint.x < ViewPointRangeHigh && viewPoint.x > ViewPointRangeLow && // within x bounds of viewport
                    viewPoint.y < ViewPointRangeHigh && viewPoint.y > ViewPointRangeLow                       // within y bounds of viewport
                ) {
                    return true;
                }
            }
        } else {
#if UNITY_EDITOR
            Debug.Log("Error! Set at least 1 visibility point on SimObjPhysics prefab!");
#endif
        }
        return false;
    }

    public VisibilityBools objectIsCurrentlyVisible(SimObjPhysics sop, float maxDistance) {
        VisibilityBools visibilityBools = new VisibilityBools(false, false);
        if (sop.VisibilityPoints.Length > 0) {
            Transform[] visPoints = sop.VisibilityPoints;
            updateAllAgentCollidersForVisibilityCheck(false);
            foreach (Transform point in visPoints) {
                Vector3 tmp = point.position;
                if (Vector3.Distance(tmp, transform.position) < maxDistance) {
                    VisibilityBools visibilityBoolsDontIgnoreInvisible = CheckIfVisibilityPointInViewport(sop, point, m_Camera, false);
                    VisibilityBools visibilityBoolsIgnoreInvisible = CheckIfVisibilityPointInViewport(sop, point, m_Camera, true);
                    if (visibilityBoolsDontIgnoreInvisible.inViewportAndVisible || visibilityBoolsIgnoreInvisible.inViewportAndVisible) {
                        visibilityBools.inViewportAndVisible = true;
                        return visibilityBools;
                    }

                    if (visibilityBoolsDontIgnoreInvisible.notInViewportButNotObstructed || visibilityBoolsIgnoreInvisible.notInViewportButNotObstructed) {
                        visibilityBools.notInViewportButNotObstructed = true;
                    }
                }
            }
        } else {
#if UNITY_EDITOR
            Debug.Log("Error! Set at least 1 visibility point on SimObjPhysics prefab!");
#endif
        }
        updateAllAgentCollidersForVisibilityCheck(true);
        return visibilityBools;
    }

    // to ignore the agent in this collision check, set ignoreAgent to true
    protected bool isHandObjectColliding(bool ignoreAgent = false, float expandBy = 0.0f) {
        if (ItemInHand == null) {
            return false;
        }
        List<GameObject> ignoreGameObjects = new List<GameObject>();
        // Ignore the agent when determining if the hand object is colliding
        if (ignoreAgent) {
            ignoreGameObjects.Add(this.gameObject);
        }
        return UtilityFunctions.isObjectColliding(ItemInHand.gameObject, ignoreGameObjects, expandBy);
    }

    protected bool isAgentCapsuleCollidingWith(GameObject otherGameObject, float expandBy = 0.0f) {
        int layerMask = 1 << 8;
        foreach (Collider c in PhysicsExtensions.OverlapCapsule(GetComponent<CapsuleCollider>(), layerMask, QueryTriggerInteraction.Ignore, expandBy)) {
            if (hasAncestor(c.transform.gameObject, otherGameObject)) {
                return true;
            }
        }
        return false;
    }

    protected bool isHandObjectCollidingWith(GameObject otherGameObject) {
        if (ItemInHand == null) {
            return false;
        }
        int layerMask = 1 << 8;
        foreach (CapsuleCollider cc in ItemInHand.GetComponentsInChildren<CapsuleCollider>()) {
            foreach (Collider c in PhysicsExtensions.OverlapCapsule(cc, layerMask, QueryTriggerInteraction.Ignore)) {
                if (hasAncestor(c.transform.gameObject, otherGameObject)) {
                    return true;
                }
            }
        }
        foreach (BoxCollider bc in ItemInHand.GetComponentsInChildren<BoxCollider>()) {
            foreach (Collider c in PhysicsExtensions.OverlapBox(bc, layerMask, QueryTriggerInteraction.Ignore)) {
                if (!hasAncestor(c.transform.gameObject, otherGameObject)) {
                    return true;
                }
            }
        }
        foreach (SphereCollider sc in ItemInHand.GetComponentsInChildren<SphereCollider>()) {
            foreach (Collider c in PhysicsExtensions.OverlapSphere(sc, layerMask, QueryTriggerInteraction.Ignore)) {
                if (!hasAncestor(c.transform.gameObject, otherGameObject)) {
                    return true;
                }
            }
        }
        return false;
    }

    // from given position in worldspace, raycast straight down and return a
    // point of any surface hit useful for getting a worldspace coordinate on
    // the floor given any point in space.
    public Vector3 GetSurfacePointBelowPosition(Vector3 position) {
        Vector3 point = Vector3.zero;

        // raycast down from the position like 10m and see if you hit anything.
        // If nothing hit, return the original position and an error message?
        RaycastHit hit;
        if (Physics.Raycast(position, Vector3.down, out hit, 10f, (1 << 8 | 1 << 10), QueryTriggerInteraction.Ignore)) {
            point = hit.point;
            return point;
        }

        // nothing hit, return the original position?
        else {
            return position;
        }
    }

    private bool stringInSomeAncestorName(GameObject go, string[] strs) {
        foreach (string str in strs) {
            if (go.name.Contains(str)) {
                return true;
            }
        }
        if (go.transform.parent != null) {
            return stringInSomeAncestorName(go.transform.parent.gameObject, strs);
        } else {
            return false;
        }
    }

    public virtual void PullObject(ServerAction action) {
        if (!physicsSceneManager.ObjectIdToSimObjPhysics.ContainsKey(action.objectId)) {
            errorMessage = "Object ID appears to be invalid.";
            Debug.Log(errorMessage);
            this.lastActionStatus = Enum.GetName(typeof(ActionStatus), ActionStatus.NOT_OBJECT);
            actionFinished(false);
            return;
        }

        if (ItemInHand != null && action.objectId == ItemInHand.objectID) {
            errorMessage = "Please use Throw for an item in the Agent's Hand";
            Debug.Log(errorMessage);
            this.lastActionStatus = Enum.GetName(typeof(ActionStatus), ActionStatus.FAILED);
            actionFinished(false);
            return;
        }

        action.z = -1;

        // if (action.moveMagnitude == 0f) {
        //     action.moveMagnitude = 200f;
        // }

        ApplyForceObject(action);
    }

    public virtual void PushObject(ServerAction action) {
        if (!physicsSceneManager.ObjectIdToSimObjPhysics.ContainsKey(action.objectId)) {
            errorMessage = "Object ID appears to be invalid.";
            Debug.Log(errorMessage);
            this.lastActionStatus = Enum.GetName(typeof(ActionStatus), ActionStatus.NOT_OBJECT);
            actionFinished(false);
            return;
        }

        if (ItemInHand != null && action.objectId == ItemInHand.objectID) {
            errorMessage = "Please use Throw for an item in the Agent's Hand";
            Debug.Log(errorMessage);
            this.lastActionStatus = Enum.GetName(typeof(ActionStatus), ActionStatus.FAILED);
            actionFinished(false);
            return;
        }

        action.z = 1;

        ApplyForceObject(action);
    }

    public virtual void ToggleObject(ServerAction action, bool toggleOn, bool forceAction) {
        SimObjPhysics target = null;
        if (action.forceAction) {
            action.forceVisible = true;
        }
        // no target object specified, so instead try and use x/y screen
        // coordinates
        if (action.objectId == null) {
            if (!ScreenToWorldTarget(action.x, action.y, ref target, !action.forceAction)) {
                // error message is set insice ScreenToWorldTarget
                actionFinished(false);
                return;
            }
        }

        // an objectId was given, so find that target in the scene if it exists
        else {
            if (!physicsSceneManager.ObjectIdToSimObjPhysics.ContainsKey(action.objectId)) {
                errorMessage = "Object ID appears to be invalid.";
                actionFinished(false);
                return;
            }

            // if object is in the scene and visible, assign it to 'target'
            foreach (SimObjPhysics sop in VisibleSimObjs(action)) {
                target = sop;
            }
        }

        if (!target) {

            // target not found in currently visible objects, report not found
            errorMessage = "object not found: " + action.objectId;
            actionFinished(false);
            return;
        }

        ToggleObject(target, toggleOn, forceAction);
    }

    public bool ToggleObject(SimObjPhysics target, bool toggleOn, bool forceAction) {
        if (!forceAction && target.isInteractable == false) {
            errorMessage = "object is visible but occluded by something: " + target.ObjectID;
            actionFinished(false);
            return false;
        }

        if (target.GetComponent<CanToggleOnOff>()) {
            CanToggleOnOff ctof = target.GetComponent<CanToggleOnOff>();

            if (!ctof.ReturnSelfControlled()) {
                errorMessage = "target object is controlled by another sim object. target object cannot be turned on/off directly";
                actionFinished(false);
                return false;
            }

            // check to make sure object is in other state
            if (ctof.isOn == toggleOn) {
                if (ctof.isOn) {
                    errorMessage = "can't toggle object on if it's already on!";
                } else {
                    errorMessage = "can't toggle object off if it's already off!";
                }

                actionFinished(false);
                return false;
            }
            // check if this object needs to be closed in order to turn on
            if (toggleOn && ctof.ReturnMustBeClosedToTurnOn().Contains(target.Type)) {
                if (target.GetComponent<CanOpen_Object>().isOpen) {
                    errorMessage = "Target must be closed to Toggle On!";
                    actionFinished(false);
                    return false;
                }
            }

            // interact then wait
            StartCoroutine(ToggleAndWait(ctof));
            return true;

        } else {
            errorMessage = "object is not toggleable.";
            actionFinished(false);
            return false;
        }
    }

    protected IEnumerator ToggleAndWait(CanToggleOnOff ctof) {
        if (ctof != null)
            ctof.Toggle();

        bool success = false;

        yield return new WaitUntil(() => (ctof != null && ctof.GetiTweenCount() == 0));
        success = true;

        if (!success) {
            errorMessage = "object could not be toggled on/off succesfully";
        }
        actionFinished(success);
    }

    public virtual void RemoveTriggerBy(CanOpen_Object canOpen) {}
    public virtual void CheckTriggeredByIndexValid(CanOpen_Object canOpen) {}
}
}