238 lines
11 KiB
C++
238 lines
11 KiB
C++
/**************************************************************************/
|
|
/* test_generic_6dof_joint_3d.cpp */
|
|
/**************************************************************************/
|
|
/* This file is part of: */
|
|
/* GODOT ENGINE */
|
|
/* https://godotengine.org */
|
|
/**************************************************************************/
|
|
/* Copyright (c) 2014-present Godot Engine contributors (see AUTHORS.md). */
|
|
/* Copyright (c) 2007-2014 Juan Linietsky, Ariel Manzur. */
|
|
/* */
|
|
/* Permission is hereby granted, free of charge, to any person obtaining */
|
|
/* a copy of this software and associated documentation files (the */
|
|
/* "Software"), to deal in the Software without restriction, including */
|
|
/* without limitation the rights to use, copy, modify, merge, publish, */
|
|
/* distribute, sublicense, and/or sell copies of the Software, and to */
|
|
/* permit persons to whom the Software is furnished to do so, subject to */
|
|
/* the following conditions: */
|
|
/* */
|
|
/* The above copyright notice and this permission notice shall be */
|
|
/* included in all copies or substantial portions of the Software. */
|
|
/* */
|
|
/* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, */
|
|
/* EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF */
|
|
/* MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. */
|
|
/* IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY */
|
|
/* CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, */
|
|
/* TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE */
|
|
/* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. */
|
|
/**************************************************************************/
|
|
|
|
#include "tests/test_macros.h"
|
|
|
|
TEST_FORCE_LINK(test_generic_6dof_joint_3d)
|
|
|
|
#ifndef PHYSICS_3D_DISABLED
|
|
|
|
#include "scene/3d/physics/joints/generic_6dof_joint_3d.h"
|
|
#include "servers/physics_3d/physics_server_3d.h"
|
|
|
|
#include <cfloat>
|
|
|
|
namespace TestGeneric6DOFJoint3D {
|
|
|
|
TEST_CASE("[SceneTree][Generic6DOFJoint3D] Quaternion angular target is normalized and stored") {
|
|
Generic6DOFJoint3D *joint = memnew(Generic6DOFJoint3D);
|
|
|
|
const Quaternion target_rotation(Vector3(0, 1, 0), 0.5);
|
|
joint->set_angular_target_rotation(target_rotation * 2.0);
|
|
|
|
CHECK(joint->get_angular_target_rotation().is_normalized());
|
|
CHECK(joint->get_angular_target_rotation().is_equal_approx(target_rotation));
|
|
CHECK(joint->has_target_rotation());
|
|
|
|
memdelete(joint);
|
|
}
|
|
|
|
TEST_CASE("[SceneTree][Generic6DOFJoint3D] Euler equilibrium setter clears stored quaternion target") {
|
|
Generic6DOFJoint3D *joint = memnew(Generic6DOFJoint3D);
|
|
|
|
joint->set_angular_target_rotation(Quaternion(Vector3(0, 0, 1), 0.25));
|
|
CHECK(joint->has_target_rotation());
|
|
|
|
joint->set_param_x(Generic6DOFJoint3D::PARAM_ANGULAR_SPRING_EQUILIBRIUM_POINT, 0.1);
|
|
CHECK_FALSE(joint->has_target_rotation());
|
|
|
|
memdelete(joint);
|
|
}
|
|
|
|
TEST_CASE("[SceneTree][Generic6DOFJoint3D] clear_angular_target_rotation resets state") {
|
|
Generic6DOFJoint3D *joint = memnew(Generic6DOFJoint3D);
|
|
|
|
joint->set_angular_target_rotation(Quaternion(Vector3(1, 0, 0), 0.5));
|
|
CHECK(joint->has_target_rotation());
|
|
|
|
joint->clear_angular_target_rotation();
|
|
CHECK_FALSE(joint->has_target_rotation());
|
|
ERR_PRINT_OFF;
|
|
CHECK(joint->get_angular_target_rotation().is_equal_approx(Quaternion()));
|
|
ERR_PRINT_ON;
|
|
|
|
memdelete(joint);
|
|
}
|
|
|
|
TEST_CASE("[SceneTree][Generic6DOFJoint3D] Getter returns identity before configuration when no explicit target is set") {
|
|
Generic6DOFJoint3D *joint = memnew(Generic6DOFJoint3D);
|
|
|
|
joint->set_param_x(Generic6DOFJoint3D::PARAM_ANGULAR_SPRING_EQUILIBRIUM_POINT, 0.3);
|
|
joint->set_param_y(Generic6DOFJoint3D::PARAM_ANGULAR_SPRING_EQUILIBRIUM_POINT, -0.2);
|
|
joint->set_param_z(Generic6DOFJoint3D::PARAM_ANGULAR_SPRING_EQUILIBRIUM_POINT, 0.4);
|
|
|
|
CHECK_FALSE(joint->has_target_rotation());
|
|
ERR_PRINT_OFF;
|
|
CHECK(joint->get_angular_target_rotation().is_equal_approx(Quaternion()));
|
|
ERR_PRINT_ON;
|
|
|
|
memdelete(joint);
|
|
}
|
|
|
|
TEST_CASE("[SceneTree][Generic6DOFJoint3D] Explicit quaternion target shadows Euler equilibrium derivation") {
|
|
Generic6DOFJoint3D *joint = memnew(Generic6DOFJoint3D);
|
|
|
|
joint->set_param_x(Generic6DOFJoint3D::PARAM_ANGULAR_SPRING_EQUILIBRIUM_POINT, 0.3);
|
|
const Quaternion explicit_target(Vector3(0, 1, 0), 0.7);
|
|
joint->set_angular_target_rotation(explicit_target);
|
|
|
|
CHECK(joint->has_target_rotation());
|
|
CHECK(joint->get_angular_target_rotation().is_equal_approx(explicit_target));
|
|
|
|
memdelete(joint);
|
|
}
|
|
|
|
static RID make_configured_generic_6dof_joint(PhysicsServer3D *p_server, RID &r_body_a, RID &r_body_b) {
|
|
r_body_a = p_server->body_create();
|
|
r_body_b = p_server->body_create();
|
|
|
|
const RID joint = p_server->joint_create();
|
|
p_server->joint_make_generic_6dof(joint, r_body_a, Transform3D(), r_body_b, Transform3D());
|
|
return joint;
|
|
}
|
|
|
|
TEST_CASE("[SceneTree][Generic6DOFJoint3D] Drive force and torque limits params round-trip") {
|
|
Generic6DOFJoint3D *joint = memnew(Generic6DOFJoint3D);
|
|
|
|
joint->set_param_x(Generic6DOFJoint3D::PARAM_LINEAR_DRIVE_FORCE_LIMIT, 123.0);
|
|
joint->set_param_x(Generic6DOFJoint3D::PARAM_ANGULAR_DRIVE_TORQUE_LIMIT, 45.0);
|
|
|
|
CHECK(joint->get_param_x(Generic6DOFJoint3D::PARAM_LINEAR_DRIVE_FORCE_LIMIT) == 123.0);
|
|
CHECK(joint->get_param_x(Generic6DOFJoint3D::PARAM_ANGULAR_DRIVE_TORQUE_LIMIT) == 45.0);
|
|
|
|
memdelete(joint);
|
|
}
|
|
|
|
TEST_CASE("[SceneTree][Generic6DOFJoint3D] Drive force and torque limits defaults are unlimited") {
|
|
Generic6DOFJoint3D *joint = memnew(Generic6DOFJoint3D);
|
|
|
|
CHECK(joint->get_param_x(Generic6DOFJoint3D::PARAM_LINEAR_DRIVE_FORCE_LIMIT) == FLT_MAX);
|
|
CHECK(joint->get_param_y(Generic6DOFJoint3D::PARAM_LINEAR_DRIVE_FORCE_LIMIT) == FLT_MAX);
|
|
CHECK(joint->get_param_z(Generic6DOFJoint3D::PARAM_LINEAR_DRIVE_FORCE_LIMIT) == FLT_MAX);
|
|
|
|
CHECK(joint->get_param_x(Generic6DOFJoint3D::PARAM_ANGULAR_DRIVE_TORQUE_LIMIT) == FLT_MAX);
|
|
CHECK(joint->get_param_y(Generic6DOFJoint3D::PARAM_ANGULAR_DRIVE_TORQUE_LIMIT) == FLT_MAX);
|
|
CHECK(joint->get_param_z(Generic6DOFJoint3D::PARAM_ANGULAR_DRIVE_TORQUE_LIMIT) == FLT_MAX);
|
|
|
|
memdelete(joint);
|
|
}
|
|
|
|
TEST_CASE("[SceneTree][PhysicsServer3D][Generic6DOFJoint3D] Drive force and torque limits params round-trip per axis") {
|
|
PhysicsServer3D *physics_server = PhysicsServer3D::get_singleton();
|
|
REQUIRE(physics_server != nullptr);
|
|
|
|
RID body_a;
|
|
RID body_b;
|
|
RID joint = make_configured_generic_6dof_joint(physics_server, body_a, body_b);
|
|
|
|
constexpr Vector3::Axis axes[] = { Vector3::AXIS_X, Vector3::AXIS_Y, Vector3::AXIS_Z };
|
|
constexpr real_t linear_values[] = { 12.0, 34.0, 56.0 };
|
|
constexpr real_t angular_values[] = { 7.0, 8.0, 9.0 };
|
|
|
|
for (int i = 0; i < 3; i++) {
|
|
physics_server->generic_6dof_joint_set_param(joint, axes[i], PS3DE::G6DOF_JOINT_LINEAR_DRIVE_FORCE_LIMIT, linear_values[i]);
|
|
physics_server->generic_6dof_joint_set_param(joint, axes[i], PS3DE::G6DOF_JOINT_ANGULAR_DRIVE_TORQUE_LIMIT, angular_values[i]);
|
|
}
|
|
|
|
for (int i = 0; i < 3; i++) {
|
|
CHECK_EQ(physics_server->generic_6dof_joint_get_param(joint, axes[i], PS3DE::G6DOF_JOINT_LINEAR_DRIVE_FORCE_LIMIT), doctest::Approx(linear_values[i]));
|
|
CHECK_EQ(physics_server->generic_6dof_joint_get_param(joint, axes[i], PS3DE::G6DOF_JOINT_ANGULAR_DRIVE_TORQUE_LIMIT), doctest::Approx(angular_values[i]));
|
|
}
|
|
|
|
physics_server->free_rid(joint);
|
|
physics_server->free_rid(body_a);
|
|
physics_server->free_rid(body_b);
|
|
}
|
|
|
|
TEST_CASE("[SceneTree][PhysicsServer3D][Generic6DOFJoint3D] Drive force and torque limits defaults are unlimited") {
|
|
PhysicsServer3D *physics_server = PhysicsServer3D::get_singleton();
|
|
REQUIRE(physics_server != nullptr);
|
|
|
|
RID body_a;
|
|
RID body_b;
|
|
RID joint = make_configured_generic_6dof_joint(physics_server, body_a, body_b);
|
|
|
|
CHECK_EQ(physics_server->generic_6dof_joint_get_param(joint, Vector3::AXIS_X, PS3DE::G6DOF_JOINT_LINEAR_DRIVE_FORCE_LIMIT), FLT_MAX);
|
|
CHECK_EQ(physics_server->generic_6dof_joint_get_param(joint, Vector3::AXIS_Y, PS3DE::G6DOF_JOINT_LINEAR_DRIVE_FORCE_LIMIT), FLT_MAX);
|
|
CHECK_EQ(physics_server->generic_6dof_joint_get_param(joint, Vector3::AXIS_Z, PS3DE::G6DOF_JOINT_LINEAR_DRIVE_FORCE_LIMIT), FLT_MAX);
|
|
|
|
CHECK_EQ(physics_server->generic_6dof_joint_get_param(joint, Vector3::AXIS_X, PS3DE::G6DOF_JOINT_ANGULAR_DRIVE_TORQUE_LIMIT), FLT_MAX);
|
|
CHECK_EQ(physics_server->generic_6dof_joint_get_param(joint, Vector3::AXIS_Y, PS3DE::G6DOF_JOINT_ANGULAR_DRIVE_TORQUE_LIMIT), FLT_MAX);
|
|
CHECK_EQ(physics_server->generic_6dof_joint_get_param(joint, Vector3::AXIS_Z, PS3DE::G6DOF_JOINT_ANGULAR_DRIVE_TORQUE_LIMIT), FLT_MAX);
|
|
|
|
physics_server->free_rid(joint);
|
|
physics_server->free_rid(body_a);
|
|
physics_server->free_rid(body_b);
|
|
}
|
|
|
|
TEST_CASE("[SceneTree][Generic6DOFJoint3D] Legacy motor and DRIVE limits are stored independently") {
|
|
Generic6DOFJoint3D *joint = memnew(Generic6DOFJoint3D);
|
|
|
|
// Set legacy motor limit, then set DRIVE limit to a finite value.
|
|
ERR_PRINT_OFF;
|
|
joint->set_param_x(Generic6DOFJoint3D::PARAM_LINEAR_MOTOR_FORCE_LIMIT, 50.0);
|
|
ERR_PRINT_ON;
|
|
joint->set_param_x(Generic6DOFJoint3D::PARAM_LINEAR_DRIVE_FORCE_LIMIT, 80.0);
|
|
|
|
CHECK_EQ(joint->get_param_x(Generic6DOFJoint3D::PARAM_LINEAR_MOTOR_FORCE_LIMIT), doctest::Approx(50.0));
|
|
CHECK_EQ(joint->get_param_x(Generic6DOFJoint3D::PARAM_LINEAR_DRIVE_FORCE_LIMIT), doctest::Approx(80.0));
|
|
|
|
memdelete(joint);
|
|
}
|
|
|
|
TEST_CASE("[SceneTree][PhysicsServer3D][Generic6DOFJoint3D] DRIVE FLT_MAX after legacy motor round-trips through storage") {
|
|
PhysicsServer3D *physics_server = PhysicsServer3D::get_singleton();
|
|
REQUIRE(physics_server != nullptr);
|
|
|
|
RID body_a;
|
|
RID body_b;
|
|
RID joint = make_configured_generic_6dof_joint(physics_server, body_a, body_b);
|
|
|
|
// Legacy motor limit set first, then explicit DRIVE FLT_MAX
|
|
physics_server->generic_6dof_joint_set_param(joint, Vector3::AXIS_X, PS3DE::G6DOF_JOINT_ANGULAR_MOTOR_FORCE_LIMIT, 50.0);
|
|
physics_server->generic_6dof_joint_set_param(joint, Vector3::AXIS_X, PS3DE::G6DOF_JOINT_ANGULAR_DRIVE_TORQUE_LIMIT, FLT_MAX);
|
|
|
|
CHECK_EQ(physics_server->generic_6dof_joint_get_param(joint, Vector3::AXIS_X, PS3DE::G6DOF_JOINT_ANGULAR_DRIVE_TORQUE_LIMIT), FLT_MAX);
|
|
|
|
physics_server->free_rid(joint);
|
|
physics_server->free_rid(body_a);
|
|
physics_server->free_rid(body_b);
|
|
}
|
|
|
|
TEST_CASE("[SceneTree][Generic6DOFJoint3D] Default joint configuration does not emit MOTOR_FORCE_LIMIT deprecation warning") {
|
|
// Instancing a node should be silent
|
|
Generic6DOFJoint3D *joint = memnew(Generic6DOFJoint3D);
|
|
CHECK(joint != nullptr);
|
|
memdelete(joint);
|
|
}
|
|
|
|
} // namespace TestGeneric6DOFJoint3D
|
|
|
|
#endif // PHYSICS_3D_DISABLED
|