o3de/AutomatedTesting/Gem/PythonTests/Physics/tests/joints/Joints_HingeNoLimitsConstrained.py

"""
Copyright (c) Contributors to the Open 3D Engine Project.
For complete copyright and license terms please see the LICENSE at the root of this distribution.

SPDX-License-Identifier: Apache-2.0 OR MIT
"""

# Test case ID : C18243585
# Test Case Title : Check that hinge joint allows no limit constraints on 2 bodies


# fmt: off
class Tests:
    enter_game_mode                 = ("Entered game mode",                         "Failed to enter game mode")
    exit_game_mode                  = ("Exited game mode",                          "Couldn't exit game mode")
    lead_found                      = ("Found lead",                                "Did not find lead")
    follower_found                  = ("Found follower",                            "Did not find follower")
    check_lead_position             = ("Lead stays still",                          "Lead moved")
    check_follower_position         = ("Follower moved higher than lead, and swinged over it",        "Follower did not move higher than lead, or swing over it")
# fmt: on


def Joints_HingeNoLimitsConstrained():
    """
    Summary: Check that hinge joint allows no limit constraints on 2 bodies

    Level Description:
    lead - Starts above follower entity
    follower - Starts below lead entity. Constrained to lead entity with a hinge joint. Starts with initial velocity of (5, 1, 0).
    forceRegion - Contains suction and damping force to hold follower position when it enters the region that will pass the test.

    Expected Behavior: 
    Lead entity remains still.
    Follower entity's Z position exceeds lead entity's Z position and swings past above the lead entity.
    The hinge joint constraint is not limited, the follower can swing to a position higher than the lead, and over it

    Test Steps:
    1) Open Level
    2) Enter Game Mode
    3) Create and Validate Entities
    4) Wait for several seconds
    5) Check to see if lead and follower behaved as expected
    6) Exit Game Mode
    7) Close Editor

    Note:
    - This test file must be called from the Open 3D Engine Editor command terminal
    - Any passed and failed tests are written to the Editor.log file.
            Parsing the file or running a log_monitor are required to observe the test results.

    :return: None
    """
    import os
    import sys
    import math
    from editor_python_test_tools.utils import Report
    from editor_python_test_tools.utils import TestHelper as helper

    import azlmbr.legacy.general as general
    import azlmbr.bus
    import azlmbr.math as azmath

    import JointsHelper
    from JointsHelper import JointEntity

    # Constants
    FLOAT_EPSILON = 0.2

    # Helper Entity class
    class Entity(JointEntity):
        def criticalEntityFound(self): # Override function to use local Test dictionary
            Report.critical_result(Tests.__dict__[self.name + "_found"], self.id.isValid())

    # Main Script
    helper.init_idle()

    # 1) Open Level
    helper.open_level("Physics", "Joints_HingeNoLimitsConstrained")

    # 2) Enter Game Mode
    helper.enter_game_mode(Tests.enter_game_mode)

    # 3) Create and Validate Entities
    lead = Entity("lead")
    follower = Entity("follower")
    Report.info_vector3(lead.position, "lead initial position:")
    Report.info_vector3(follower.position, "follower initial position:")
    leadInitialPosition = lead.position

    # 4) Wait for the follower to move above and over the lead or Timeout
    normalizedStartPos = JointsHelper.getRelativeVector(lead.position, follower.position)
    normalizedStartPos = normalizedStartPos.GetNormalizedSafe()

    class WaitCondition:
        ANGLE_CHECKPOINT_1 = math.radians(90)
        ANGLE_CHECKPOINT_2 = math.radians(200)

        angleAchieved = 0.0
        followerMovedAbove90Deg = False #this is expected to be true to pass the test
        followerMovedAbove200Deg = False #this is expected to be true to pass the test

        jointNormal = azmath.Vector3(0.0, -1.0, 0.0) # the joint rotates around the y axis
        def checkConditionMet(self):
            #calculate the current follower-lead vector
            normalVec = JointsHelper.getRelativeVector(lead.position, follower.position)
            normalVec = normalVec.GetNormalizedSafe()

            #triple product and get the angle
            tripleProduct = normalizedStartPos.dot(normalVec.cross(self.jointNormal))
            currentAngle = math.acos(normalizedStartPos.Dot(normalVec))
            if tripleProduct < 0:
                currentAngle = (2*math.pi) - currentAngle

            #if the angle is now less then last time, it is no longer rising, so end the test.
            if currentAngle < self.angleAchieved:
                return True
            
            #once we're passed the final check point, end the test
            if currentAngle > self.ANGLE_CHECKPOINT_2:
                self.followerMovedAbove200Deg = True
                return True
            
            self.angleAchieved = currentAngle
            self.followerMovedAbove90Deg = currentAngle > self.ANGLE_CHECKPOINT_1
            return False

        def isFollowerPositionCorrect(self):
            return self.followerMovedAbove90Deg and self.followerMovedAbove200Deg

    waitCondition = WaitCondition()

    MAX_WAIT_TIME = 10.0 #seconds
    conditionMet = helper.wait_for_condition(lambda: waitCondition.checkConditionMet(), MAX_WAIT_TIME)

    # 5) Check to see if lead and follower behaved as expected
    Report.info_vector3(lead.position, "lead position after test run:")
    Report.info_vector3(follower.position, "follower position after test run:")

    leadPositionDelta = lead.position.Subtract(leadInitialPosition)
    leadRemainedStill = JointsHelper.vector3SmallerThanScalar(leadPositionDelta, FLOAT_EPSILON)
    Report.critical_result(Tests.check_lead_position, leadRemainedStill)

    Report.critical_result(Tests.check_follower_position, conditionMet and waitCondition.isFollowerPositionCorrect())

    # 6) Exit Game Mode
    helper.exit_game_mode(Tests.exit_game_mode)

if __name__ == "__main__":
    from editor_python_test_tools.utils import Report
    Report.start_test(Joints_HingeNoLimitsConstrained)