diff --git a/core/math/transform.h b/core/math/transform.h index f8bd2a4baee..330fc0be22a 100644 --- a/core/math/transform.h +++ b/core/math/transform.h @@ -76,16 +76,24 @@ public: bool operator!=(const Transform &p_transform) const; _FORCE_INLINE_ Vector3 xform(const Vector3 &p_vector) const; - _FORCE_INLINE_ Vector3 xform_inv(const Vector3 &p_vector) const; + _FORCE_INLINE_ AABB xform(const AABB &p_aabb) const; + _FORCE_INLINE_ PoolVector xform(const PoolVector &p_array) const; + // NOTE: These are UNSAFE with non-uniform scaling, and will produce incorrect results. + // They use the transpose. + // For safe inverse transforms, xform by the affine_inverse. + _FORCE_INLINE_ Vector3 xform_inv(const Vector3 &p_vector) const; + _FORCE_INLINE_ AABB xform_inv(const AABB &p_aabb) const; + _FORCE_INLINE_ PoolVector xform_inv(const PoolVector &p_array) const; + + // Safe with non-uniform scaling (uses affine_inverse). _FORCE_INLINE_ Plane xform(const Plane &p_plane) const; _FORCE_INLINE_ Plane xform_inv(const Plane &p_plane) const; - _FORCE_INLINE_ AABB xform(const AABB &p_aabb) const; - _FORCE_INLINE_ AABB xform_inv(const AABB &p_aabb) const; - - _FORCE_INLINE_ PoolVector xform(const PoolVector &p_array) const; - _FORCE_INLINE_ PoolVector xform_inv(const PoolVector &p_array) const; + // These fast versions use precomputed affine inverse, and should be used in bottleneck areas where + // multiple planes are to be transformed. + _FORCE_INLINE_ Plane xform_fast(const Plane &p_plane, const Basis &p_basis_inverse_transpose) const; + static _FORCE_INLINE_ Plane xform_inv_fast(const Plane &p_plane, const Transform &p_inverse, const Basis &p_basis_transpose); void operator*=(const Transform &p_transform); Transform operator*(const Transform &p_transform) const; @@ -118,6 +126,7 @@ _FORCE_INLINE_ Vector3 Transform::xform(const Vector3 &p_vector) const { basis[1].dot(p_vector) + origin.y, basis[2].dot(p_vector) + origin.z); } + _FORCE_INLINE_ Vector3 Transform::xform_inv(const Vector3 &p_vector) const { Vector3 v = p_vector - origin; @@ -127,29 +136,20 @@ _FORCE_INLINE_ Vector3 Transform::xform_inv(const Vector3 &p_vector) const { (basis.elements[0][2] * v.x) + (basis.elements[1][2] * v.y) + (basis.elements[2][2] * v.z)); } +// Neither the plane regular xform or xform_inv are particularly efficient, +// as they do a basis inverse. For xforming a large number +// of planes it is better to pre-calculate the inverse transpose basis once +// and reuse it for each plane, by using the 'fast' version of the functions. _FORCE_INLINE_ Plane Transform::xform(const Plane &p_plane) const { - Vector3 point = p_plane.normal * p_plane.d; - Vector3 point_dir = point + p_plane.normal; - point = xform(point); - point_dir = xform(point_dir); - - Vector3 normal = point_dir - point; - normal.normalize(); - real_t d = normal.dot(point); - - return Plane(normal, d); + Basis b = basis.inverse(); + b.transpose(); + return xform_fast(p_plane, b); } + _FORCE_INLINE_ Plane Transform::xform_inv(const Plane &p_plane) const { - Vector3 point = p_plane.normal * p_plane.d; - Vector3 point_dir = point + p_plane.normal; - point = xform_inv(point); - point_dir = xform_inv(point_dir); - - Vector3 normal = point_dir - point; - normal.normalize(); - real_t d = normal.dot(point); - - return Plane(normal, d); + Transform inv = affine_inverse(); + Basis basis_transpose = basis.transposed(); + return xform_inv_fast(p_plane, inv, basis_transpose); } _FORCE_INLINE_ AABB Transform::xform(const AABB &p_aabb) const { @@ -227,4 +227,37 @@ PoolVector Transform::xform_inv(const PoolVector &p_array) con return array; } +_FORCE_INLINE_ Plane Transform::xform_fast(const Plane &p_plane, const Basis &p_basis_inverse_transpose) const { + // Transform a single point on the plane. + Vector3 point = p_plane.normal * p_plane.d; + point = xform(point); + + // Use inverse transpose for correct normals with non-uniform scaling. + Vector3 normal = p_basis_inverse_transpose.xform(p_plane.normal); + normal.normalize(); + + real_t d = normal.dot(point); + return Plane(normal, d); +} + +_FORCE_INLINE_ Plane Transform::xform_inv_fast(const Plane &p_plane, const Transform &p_inverse, const Basis &p_basis_transpose) { + // Transform a single point on the plane. + Vector3 point = p_plane.normal * p_plane.d; + point = p_inverse.xform(point); + + // Note that instead of precalculating the transpose, an alternative + // would be to use the transpose for the basis transform. + // However that would be less SIMD friendly (requiring a swizzle). + // So the cost is one extra precalced value in the calling code. + // This is probably worth it, as this could be used in bottleneck areas. And + // where it is not a bottleneck, the non-fast method is fine. + + // Use transpose for correct normals with non-uniform scaling. + Vector3 normal = p_basis_transpose.xform(p_plane.normal); + normal.normalize(); + + real_t d = normal.dot(point); + return Plane(normal, d); +} + #endif // TRANSFORM_H diff --git a/main/tests/test_main.cpp b/main/tests/test_main.cpp index 4c7016f0b0d..7a0e23f5bbb 100644 --- a/main/tests/test_main.cpp +++ b/main/tests/test_main.cpp @@ -47,6 +47,7 @@ #include "test_render.h" #include "test_shader_lang.h" #include "test_string.h" +#include "test_transform.h" #include "test_xml_parser.h" const char **tests_get_names() { @@ -54,6 +55,7 @@ const char **tests_get_names() { "string", "math", "basis", + "transform", "physics", "physics_2d", "render", @@ -86,6 +88,10 @@ MainLoop *test_main(String p_test, const List &p_args) { return TestBasis::test(); } + if (p_test == "transform") { + return TestTransform::test(); + } + if (p_test == "physics") { return TestPhysics::test(); } diff --git a/main/tests/test_transform.cpp b/main/tests/test_transform.cpp new file mode 100644 index 00000000000..161885cb6b9 --- /dev/null +++ b/main/tests/test_transform.cpp @@ -0,0 +1,270 @@ +/*************************************************************************/ +/* test_transform.cpp */ +/*************************************************************************/ +/* This file is part of: */ +/* GODOT ENGINE */ +/* https://godotengine.org */ +/*************************************************************************/ +/* Copyright (c) 2007-2021 Juan Linietsky, Ariel Manzur. */ +/* Copyright (c) 2014-2021 Godot Engine contributors (cf. AUTHORS.md). */ +/* */ +/* 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 "test_transform.h" + +#include "core/math/random_number_generator.h" +#include "core/math/transform.h" +#include "core/math/vector3.h" +#include "core/os/os.h" +#include "core/ustring.h" + +// #define GODOT_TEST_TRANSFORM_NON_UNIFORM_SCALE_TESTS_ENABLED + +namespace TestTransform { + +bool test_plane() { + bool pass = true; + + // test non-uniform scaling, forward and inverse + Transform tr; + tr.scale(Vector3(1, 2, 3)); + + Plane p(Vector3(1, 1, 1), Vector3(1, 1, 1).normalized()); + + Plane p2 = tr.xform(p); + Plane p3 = tr.xform_inv(p2); + + if (!p3.normal.is_equal_approx(p.normal)) { + OS::get_singleton()->print("Fail due to Transform::xform(Plane)\n"); + pass = false; + } + + return pass; +} + +bool test_aabb_regular() { + bool pass = true; + + Transform tr; + tr.basis = Basis(Vector3(Math_PI, 0, 0)); + tr.origin = Vector3(1, 2, 3); + + AABB bb(Vector3(1, 1, 1), Vector3(2, 3, 4)); + + // Test forward xform. + AABB bb2 = tr.xform(bb); + AABB bb3 = tr.xform_inv(bb2); + + if (!bb3.position.is_equal_approx(bb.position)) { + OS::get_singleton()->print("Fail due to Transform::xform_inv(AABB) position\n"); + pass = false; + } + + if (!bb3.size.is_equal_approx(bb.size)) { + OS::get_singleton()->print("Fail due to Transform::xform_inv(AABB) size\n"); + pass = false; + } + + if (!pass) { + String string = String("bb2 : ") + String(Variant(bb2)); + OS::get_singleton()->print("\t%ls\n", string.c_str()); + string = String("bb3 : ") + String(Variant(bb3)); + OS::get_singleton()->print("\t%ls\n", string.c_str()); + } + + return pass; +} + +bool test_aabb_non_uniform_scale() { + bool pass = true; + + Transform tr; + tr.scale(Vector3(1, 2, 3)); + + AABB bb(Vector3(1, 1, 1), Vector3(2, 3, 4)); + + // Test forward xform. + AABB bb2 = tr.xform(bb); + + if (!bb2.position.is_equal_approx(Vector3(1, 2, 3))) { + OS::get_singleton()->print("Fail with non-uniform scale due to Transform::xform(AABB) position\n"); + pass = false; + } + + if (!bb2.size.is_equal_approx(Vector3(2, 6, 12))) { + OS::get_singleton()->print("Fail with non-uniform scale due to Transform::xform(AABB) size\n"); + pass = false; + } + + // Now test inverse. + // This will fail if using the transpose and not the affine_inverse. + bb2.position = Vector3(1, 2, 3); + bb2.size = Vector3(2, 6, 12); + + AABB bb3 = tr.xform_inv(bb2); + + if (!bb3.position.is_equal_approx(bb.position)) { + OS::get_singleton()->print("Fail with non-uniform scale due to Transform::xform_inv(AABB) position\n"); + pass = false; + } + + if (!bb3.size.is_equal_approx(bb.size)) { + OS::get_singleton()->print("Fail with non-uniform scale due to Transform::xform_inv(AABB) size\n"); + pass = false; + } + + if (!pass) { + String string = String("bb2 : ") + String(Variant(bb2)); + OS::get_singleton()->print("\t%ls\n", string.c_str()); + string = String("bb3 : ") + String(Variant(bb3)); + OS::get_singleton()->print("\t%ls\n", string.c_str()); + } + + return pass; +} + +bool test_aabb() { + bool pass = true; + if (!test_aabb_regular()) { + pass = false; + } + +#ifdef GODOT_TEST_TRANSFORM_NON_UNIFORM_SCALE_TESTS_ENABLED + if (!test_aabb_non_uniform_scale()) { + pass = false; + } +#endif + + return pass; +} + +bool test_vector3_regular() { + bool pass = true; + + Transform tr; + + RandomNumberGenerator rng; + const real_t range = 1800.0; + const real_t range_rot = Math_PI; + + bool passed_multi = true; + for (int n = 0; n < 1000; n++) { + Vector3 pt_test = Vector3(rng.randf_range(-range, range), rng.randf_range(-range, range), rng.randf_range(-range, range)); + + tr.origin = Vector3(rng.randf_range(-range, range), rng.randf_range(-range, range), rng.randf_range(-range, range)); + tr.basis = Basis(Vector3(rng.randf_range(-range_rot, range_rot), rng.randf_range(-range_rot, range_rot), rng.randf_range(-range_rot, range_rot))); + + Vector3 pt = tr.xform(pt_test); + pt = tr.xform_inv(pt); + + if (!pt.is_equal_approx(pt_test, 0.1)) { + passed_multi = false; + } + } + if (!passed_multi) { + OS::get_singleton()->print("Failed multitest due to Transform::xform and xform_inv(Vector3)\n"); + pass = false; + } + + return pass; +} + +bool test_vector3_non_uniform_scale() { + bool pass = true; + + // Regular scale. + Transform tr; + tr.scale(Vector3(3, 3, 3)); + Vector3 pt(1, 1, 1); + Vector3 res = tr.xform(pt); + + if (!res.is_equal_approx(Vector3(3, 3, 3))) { + OS::get_singleton()->print("Fail with scale due to Transform::xform(Vector3)\n"); + pass = false; + } + + res = tr.xform_inv(res); + if (!res.is_equal_approx(pt)) { + OS::get_singleton()->print("Fail with scale due to Transform::xform_inv(Vector3)\n"); + pass = false; + } + + // Non uniform scale. + tr.scale(Vector3(1, 2, 3)); + res = tr.xform(pt); + + if (!res.is_equal_approx(Vector3(1, 2, 3))) { + OS::get_singleton()->print("Fail with non-uniform scale due to Transform::xform(Vector3)\n"); + pass = false; + } + + pt = Vector3(1, 2, 3); + res = tr.xform_inv(pt); + if (!res.is_equal_approx(Vector3(1, 1, 1))) { + OS::get_singleton()->print("Fail with non-uniform scale due to Transform::xform_inv(Vector3)\n"); + pass = false; + } + + return pass; +} + +bool test_vector3() { + bool pass = true; + if (!test_vector3_regular()) { + pass = false; + } + +#ifdef GODOT_TEST_TRANSFORM_NON_UNIFORM_SCALE_TESTS_ENABLED + if (!test_vector3_non_uniform_scale()) { + pass = false; + } +#endif + + return pass; +} + +MainLoop *test() { + OS::get_singleton()->print("Start Transform checks.\n"); + + bool success = true; + + if (!test_vector3()) { + success = false; + } + + if (!test_plane()) { + success = false; + } + + if (!test_aabb()) { + success = false; + } + + if (success) { + OS::get_singleton()->print("Transform checks passed.\n"); + } else { + OS::get_singleton()->print("Transform checks FAILED.\n"); + } + + return nullptr; +} + +} // namespace TestTransform diff --git a/main/tests/test_transform.h b/main/tests/test_transform.h new file mode 100644 index 00000000000..b90efd77ed0 --- /dev/null +++ b/main/tests/test_transform.h @@ -0,0 +1,40 @@ +/*************************************************************************/ +/* test_transform.h */ +/*************************************************************************/ +/* This file is part of: */ +/* GODOT ENGINE */ +/* https://godotengine.org */ +/*************************************************************************/ +/* Copyright (c) 2007-2021 Juan Linietsky, Ariel Manzur. */ +/* Copyright (c) 2014-2021 Godot Engine contributors (cf. AUTHORS.md). */ +/* */ +/* 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. */ +/*************************************************************************/ + +#ifndef TEST_TRANSFORM_H +#define TEST_TRANSFORM_H + +#include "core/os/main_loop.h" + +namespace TestTransform { +MainLoop *test(); +} + +#endif diff --git a/servers/physics/collision_solver_sat.cpp b/servers/physics/collision_solver_sat.cpp index ea98e4861a9..5e4589a134e 100644 --- a/servers/physics/collision_solver_sat.cpp +++ b/servers/physics/collision_solver_sat.cpp @@ -953,9 +953,12 @@ static void _collision_sphere_convex_polygon(const ShapeSW *p_a, const Transform const Vector3 *vertices = mesh.vertices.ptr(); int vertex_count = mesh.vertices.size(); + // Precalculating this makes the transforms faster. + Basis b_xform_normal = p_transform_b.basis.inverse().transposed(); + // faces of B for (int i = 0; i < face_count; i++) { - Vector3 axis = p_transform_b.xform(faces[i].plane).normal; + Vector3 axis = b_xform_normal.xform(faces[i].plane.normal).normalized(); if (!separator.test_axis(axis)) { return; @@ -1191,13 +1194,14 @@ static void _collision_box_capsule(const ShapeSW *p_a, const Transform &p_transf } // capsule balls, edges of A + Transform transform_a_inverse = p_transform_a.affine_inverse(); for (int i = 0; i < 2; i++) { Vector3 capsule_axis = p_transform_b.basis.get_axis(2) * (capsule_B->get_height() * 0.5); Vector3 sphere_pos = p_transform_b.origin + ((i == 0) ? capsule_axis : -capsule_axis); - Vector3 cnormal = p_transform_a.xform_inv(sphere_pos); + Vector3 cnormal = transform_a_inverse.xform(sphere_pos); Vector3 cpoint = p_transform_a.xform(Vector3( @@ -1368,9 +1372,12 @@ static void _collision_box_convex_polygon(const ShapeSW *p_a, const Transform &p } } + // Precalculating this makes the transforms faster. + Basis b_xform_normal = p_transform_b.basis.inverse().transposed(); + // faces of B for (int i = 0; i < face_count; i++) { - Vector3 axis = p_transform_b.xform(faces[i].plane).normal; + Vector3 axis = b_xform_normal.xform(faces[i].plane.normal).normalized(); if (!separator.test_axis(axis)) { return; @@ -1701,9 +1708,12 @@ static void _collision_capsule_convex_polygon(const ShapeSW *p_a, const Transfor int edge_count = mesh.edges.size(); const Vector3 *vertices = mesh.vertices.ptr(); + // Precalculating this makes the transforms faster. + Basis b_xform_normal = p_transform_b.basis.inverse().transposed(); + // faces of B for (int i = 0; i < face_count; i++) { - Vector3 axis = p_transform_b.xform(faces[i].plane).normal; + Vector3 axis = b_xform_normal.xform(faces[i].plane.normal).normalized(); if (!separator.test_axis(axis)) { return; @@ -1995,20 +2005,24 @@ static void _collision_convex_polygon_convex_polygon(const ShapeSW *p_a, const T const Vector3 *vertices_B = mesh_B.vertices.ptr(); int vertex_count_B = mesh_B.vertices.size(); + // Precalculating this makes the transforms faster. + Basis a_xform_normal = p_transform_a.basis.inverse().transposed(); + // faces of A for (int i = 0; i < face_count_A; i++) { - Vector3 axis = p_transform_a.xform(faces_A[i].plane).normal; - //Vector3 axis = p_transform_a.basis.xform( faces_A[i].plane.normal ).normalized(); + Vector3 axis = a_xform_normal.xform(faces_A[i].plane.normal).normalized(); if (!separator.test_axis(axis)) { return; } } + // Precalculating this makes the transforms faster. + Basis b_xform_normal = p_transform_b.basis.inverse().transposed(); + // faces of B for (int i = 0; i < face_count_B; i++) { - Vector3 axis = p_transform_b.xform(faces_B[i].plane).normal; - //Vector3 axis = p_transform_b.basis.xform( faces_B[i].plane.normal ).normalized(); + Vector3 axis = b_xform_normal.xform(faces_B[i].plane.normal).normalized(); if (!separator.test_axis(axis)) { return;