diff --git a/unity/Assets/Scripts/AgentManager.cs b/unity/Assets/Scripts/AgentManager.cs index 1b869c9fad..36d2070205 100644 --- a/unity/Assets/Scripts/AgentManager.cs +++ b/unity/Assets/Scripts/AgentManager.cs @@ -320,7 +320,7 @@ public void Initialize(ServerAction action) { : action.dynamicServerAction.agentInitializationParams ); Debug.Log( - $"Initialize of AgentController. lastActionSuccess: {primaryAgent.lastActionSuccess}, errorMessage: {primaryAgent.errorMessage}, actionReturn: {primaryAgent.actionReturn}, agentState: {primaryAgent.agentState}" + $"Initialize of AgentController.lastAction: {primaryAgent.lastAction} lastActionSuccess: {primaryAgent.lastActionSuccess}, errorMessage: {primaryAgent.errorMessage}, actionReturn: {primaryAgent.actionReturn}, agentState: {primaryAgent.agentState}" ); Time.fixedDeltaTime = action.fixedDeltaTime.GetValueOrDefault(Time.fixedDeltaTime); if (action.targetFrameRate > 0) { diff --git a/unity/Assets/Scripts/ArmAgentController.cs b/unity/Assets/Scripts/ArmAgentController.cs index 5e3aea7317..0c317ade63 100644 --- a/unity/Assets/Scripts/ArmAgentController.cs +++ b/unity/Assets/Scripts/ArmAgentController.cs @@ -266,6 +266,86 @@ public virtual IEnumerator RotateAgent( ); } + public override void Teleport( + Vector3? position = null, + Vector3? rotation = null, + float? horizon = null, + bool? standing = null, + bool forceAction = false + ) { + //non-high level agents cannot set standing + if(standing != null) { + errorMessage = "Cannot set standing for stretch agent"; + actionFinishedEmit(success:false, actionReturn:null, errorMessage:errorMessage); + return; + } + + TeleportFull( + position: position, + rotation: rotation, + horizon: horizon, + standing: standing, + forceAction: forceAction + ); + } + + public override void TeleportFull( + Vector3? position = null, + Vector3? rotation = null, + float? horizon = null, + bool? standing = null, + bool forceAction = false + ) { + //non-high level agents cannot set standing + if(standing != null) { + errorMessage = "Cannot set standing for stretch agent"; + actionFinishedEmit(success:false, actionReturn:null, errorMessage:errorMessage); + return; + } + + //cache old values in case there is a failure + Vector3 oldPosition = transform.position; + Quaternion oldRotation = transform.rotation; + Quaternion oldCameraRotation = m_Camera.transform.localRotation; + + try { + base.teleportFull( + position: position, + rotation: rotation, + horizon: horizon, + forceAction: forceAction + ); + + // add arm value cases + if (!forceAction) { + if (isHandObjectColliding(ignoreAgent: true)) { + throw new InvalidOperationException( + "Cannot teleport due to hand object collision." + ); + } + if (Arm != null && Arm.IsArmColliding()) { + throw new InvalidOperationException( + "Mid Level Arm is actively clipping with some geometry in the environment. TeleportFull fails in this position." + ); + } else if (SArm != null && SArm.IsArmColliding()) { + throw new InvalidOperationException( + "Stretch Arm is actively clipping with some geometry in the environment. TeleportFull fails in this position." + ); + } + base.assertTeleportedNearGround(targetPosition: position); + } + + } catch (InvalidOperationException e) { + transform.position = oldPosition; + transform.rotation = oldRotation; + m_Camera.transform.localRotation = oldCameraRotation; + + throw new InvalidOperationException(e.Message); + } + + actionFinished(success: true); + } + /* Rotates the wrist (in a relative fashion) given some input pitch, yaw, and roll offsets. Easiest to see how this works by diff --git a/unity/Assets/Scripts/PhysicsRemoteFPSAgentController.cs b/unity/Assets/Scripts/PhysicsRemoteFPSAgentController.cs index a827d8200a..7daea12c8f 100644 --- a/unity/Assets/Scripts/PhysicsRemoteFPSAgentController.cs +++ b/unity/Assets/Scripts/PhysicsRemoteFPSAgentController.cs @@ -174,6 +174,21 @@ public void SetMassProperties(string objectId, float mass, float drag, float ang } public bool isStanding() { + + //default to not standing if this isn't literally the PhysicsRemoteFPSAgentController + //this means the metadat for isStanding should always be false for all derived classes + if (this.GetType() != typeof(PhysicsRemoteFPSAgentController)) + { + return false; + } + + if(UtilityFunctions.ArePositionsApproximatelyEqual(m_Camera.transform.localPosition, standingLocalCameraPosition) != true && + UtilityFunctions.ArePositionsApproximatelyEqual(m_Camera.transform.localPosition, crouchingLocalCameraPosition) != true) { + throw new InvalidOperationException( + $"Camera position is not equal to standing or crouching position. camera local position: {m_Camera.transform.localPosition}, standing local position: {standingLocalCameraPosition}, crouching local position: {crouchingLocalCameraPosition}" + ); + } + return (m_Camera.transform.localPosition - standingLocalCameraPosition).magnitude < 0.1f; } @@ -1625,23 +1640,6 @@ public void TeleportObjectToFloor(ServerAction action) { ////////////// TELEPORT FULL ////////////// /////////////////////////////////////////// - // [ObsoleteAttribute(message: "This action is deprecated. Call TeleportFull(position, ...) instead.", error: false)] - // public void TeleportFull( - // float x, float y, float z, - // float rotation, - // float horizon, - // bool standing, - // bool forceAction = false - // ) { - // TeleportFull( - // position: new Vector3(x, y, z), - // rotation: new Vector3(0, rotation, 0), - // horizon: horizon, - // standing: standing, - // forceAction: forceAction - // ); - // } - [ObsoleteAttribute( message: "This action is deprecated. Call TeleportFull(position, ...) instead.", error: false @@ -1664,24 +1662,6 @@ public void TeleportFull( ); } - // keep undocumented until float: rotation is added to Stochastic - // public void TeleportFull( - // Vector3 position, - // float rotation, - // float horizon, - // bool standing, - // bool forceAction = false - // ) { - // TeleportFull( - // position: position, - // rotation: new Vector3(0, rotation, 0), - // horizon: horizon, - // standing: standing, - // forceAction: forceAction - // ); - // } - - // has to consider both the arm and standing public virtual void TeleportFull( Vector3? position, Vector3? rotation, @@ -1763,23 +1743,6 @@ public virtual void TeleportFull( //////////////// TELEPORT ///////////////// /////////////////////////////////////////// - // [ObsoleteAttribute(message: "This action is deprecated. Call Teleport(position, ...) instead.", error: false)] - // public void Teleport( - // float x, float y, float z, - // float? rotation = null, - // float? horizon = null, - // bool? standing = null, - // bool forceAction = false - // ) { - // Teleport( - // position: new Vector3(x, y, z), - // rotation: rotation, - // horizon: horizon, - // standing: standing, - // forceAction: forceAction - // ); - // } - [ObsoleteAttribute( message: "This action is deprecated. Call Teleport(position, ...) instead.", error: false @@ -1802,25 +1765,8 @@ public void Teleport( ); } - // keep undocumented until float: rotation is added to Stochastic - // DO NOT add float: rotation to base. - // public void Teleport( - // Vector3? position = null, - // float? rotation = null, - // float? horizon = null, - // bool? standing = null, - // bool forceAction = false - // ) { - // Teleport( - // position: position, - // rotation: rotation == null ? m_Camera.transform.localEulerAngles : new Vector3(0, (float) rotation, 0), - // horizon: horizon, - // standing: standing, - // forceAction: forceAction - // ); - // } - - public void Teleport( + //keeping 'Teleport' for backwards compatibility + public virtual void Teleport( Vector3? position = null, Vector3? rotation = null, float? horizon = null, @@ -1828,10 +1774,10 @@ public void Teleport( bool forceAction = false ) { TeleportFull( - position: position == null ? transform.position : (Vector3)position, - rotation: rotation == null ? transform.eulerAngles : (Vector3)rotation, - horizon: horizon == null ? m_Camera.transform.localEulerAngles.x : (float)horizon, - standing: standing == null ? isStanding() : (bool)standing, + position: position, + rotation: rotation, + horizon: horizon, + standing: standing, forceAction: forceAction ); } diff --git a/unity/Assets/Scripts/StretchAgentController.cs b/unity/Assets/Scripts/StretchAgentController.cs index 0c9f8e4fe8..74e9510ad5 100644 --- a/unity/Assets/Scripts/StretchAgentController.cs +++ b/unity/Assets/Scripts/StretchAgentController.cs @@ -60,10 +60,6 @@ public override ActionFinished InitializeBody(ServerAction initializeAction) { m_Camera.GetComponent().enabled = true; m_Camera.GetComponent().enabled = true; - // set camera stand/crouch local positions for Tall mode - standingLocalCameraPosition = m_Camera.transform.localPosition; - crouchingLocalCameraPosition = m_Camera.transform.localPosition; - // set up main camera parameters m_Camera.fieldOfView = 65f; @@ -115,6 +111,10 @@ out secondaryCameraParams CameraParameters.setCameraParameters(fp_camera_2, secondaryCameraParams); } + // set camera stand/crouch local positions for stretch mode even though they arent used + standingLocalCameraPosition = m_Camera.transform.localPosition; + crouchingLocalCameraPosition = m_Camera.transform.localPosition; + // enable stretch arm component Debug.Log("initializing stretch arm"); StretchArm.SetActive(true); diff --git a/unity/Assets/Scripts/UtilityFunctions.cs b/unity/Assets/Scripts/UtilityFunctions.cs index 18e1e8cbd5..c05a2cba83 100644 --- a/unity/Assets/Scripts/UtilityFunctions.cs +++ b/unity/Assets/Scripts/UtilityFunctions.cs @@ -484,6 +484,13 @@ public static List GetLightPropertiesOfScene() { return allOfTheLights; } + public static bool ArePositionsApproximatelyEqual(Vector3 position1, Vector3 position2) { + // Compare each component (x, y, z) of the two positions to see if they are approximately equal via the epsilon value + return Mathf.Abs(position1.x - position2.x) < Vector3.kEpsilon + && Mathf.Abs(position1.y - position2.y) < Vector3.kEpsilon + && Mathf.Abs(position1.z - position2.z) < Vector3.kEpsilon; + } + #if UNITY_EDITOR public static void debugGetLightPropertiesOfScene(List lights) { diff --git a/unity/Assets/UnitTests/Procedural/Movement/TestProceduralTeleport.cs b/unity/Assets/UnitTests/Procedural/Movement/TestProceduralTeleport.cs index 4f08054e24..d9b367a6ac 100644 --- a/unity/Assets/UnitTests/Procedural/Movement/TestProceduralTeleport.cs +++ b/unity/Assets/UnitTests/Procedural/Movement/TestProceduralTeleport.cs @@ -174,9 +174,7 @@ public IEnumerator TestTeleport() { "action", "TeleportFull" }, { "position", new Vector3(3f, 0.91f, 1.0f) }, //adjusting Y value to be within the error (0.05) of the floor. { "rotation", new Vector3(0f, 180f, 0f) }, - // {"forceAction", true}, { "horizon", -20f }, - { "standing", true } } );