diff options
Diffstat (limited to 'servers/physics_3d')
19 files changed, 195 insertions, 190 deletions
diff --git a/servers/physics_3d/gjk_epa.cpp b/servers/physics_3d/gjk_epa.cpp index 23c8079538..ba362740b2 100644 --- a/servers/physics_3d/gjk_epa.cpp +++ b/servers/physics_3d/gjk_epa.cpp @@ -113,7 +113,7 @@ struct MinkowskiDiff { real_t margin_A = 0.0; real_t margin_B = 0.0; - Vector3 (*get_support)(const GodotShape3D*, const Vector3&, real_t); + Vector3 (*get_support)(const GodotShape3D*, const Vector3&, real_t) = nullptr; void Initialize(const GodotShape3D* shape0, const Transform3D& wtrs0, const real_t margin0, const GodotShape3D* shape1, const Transform3D& wtrs1, const real_t margin1) { @@ -191,13 +191,13 @@ struct GJK /* Fields */ tShape m_shape; Vector3 m_ray; - real_t m_distance; + real_t m_distance = 0.0f; sSimplex m_simplices[2]; sSV m_store[4]; sSV* m_free[4]; - U m_nfree; - U m_current; - sSimplex* m_simplex; + U m_nfree = 0; + U m_current = 0; + sSimplex* m_simplex = nullptr; eStatus::_ m_status; /* Methods */ GJK() @@ -548,12 +548,12 @@ struct GJK struct sFace { Vector3 n; - real_t d; + real_t d = 0.0f; sSV* c[3]; sFace* f[3]; sFace* l[2]; U1 e[3]; - U1 pass; + U1 pass = 0; }; struct sList { @@ -583,10 +583,10 @@ struct GJK eStatus::_ m_status; GJK::sSimplex m_result; Vector3 m_normal; - real_t m_depth; + real_t m_depth = 0.0f; sSV m_sv_store[EPA_MAX_VERTICES]; sFace m_fc_store[EPA_MAX_FACES]; - U m_nextsv; + U m_nextsv = 0; sList m_hull; sList m_stock; /* Methods */ diff --git a/servers/physics_3d/godot_area_3d.cpp b/servers/physics_3d/godot_area_3d.cpp index e7df23d0d8..e2ad765d62 100644 --- a/servers/physics_3d/godot_area_3d.cpp +++ b/servers/physics_3d/godot_area_3d.cpp @@ -254,22 +254,24 @@ void GodotArea3D::call_queries() { resptr[i] = &res[i]; } - for (Map<BodyKey, BodyState>::Element *E = monitored_bodies.front(); E;) { - if (E->get().state == 0) { // Nothing happened - Map<BodyKey, BodyState>::Element *next = E->next(); - monitored_bodies.erase(E); + for (HashMap<BodyKey, BodyState, BodyKey>::Iterator E = monitored_bodies.begin(); E;) { + if (E->value.state == 0) { // Nothing happened + HashMap<BodyKey, BodyState, BodyKey>::Iterator next = E; + ++next; + monitored_bodies.remove(E); E = next; continue; } - res[0] = E->get().state > 0 ? PhysicsServer3D::AREA_BODY_ADDED : PhysicsServer3D::AREA_BODY_REMOVED; - res[1] = E->key().rid; - res[2] = E->key().instance_id; - res[3] = E->key().body_shape; - res[4] = E->key().area_shape; + res[0] = E->value.state > 0 ? PhysicsServer3D::AREA_BODY_ADDED : PhysicsServer3D::AREA_BODY_REMOVED; + res[1] = E->key.rid; + res[2] = E->key.instance_id; + res[3] = E->key.body_shape; + res[4] = E->key.area_shape; - Map<BodyKey, BodyState>::Element *next = E->next(); - monitored_bodies.erase(E); + HashMap<BodyKey, BodyState, BodyKey>::Iterator next = E; + ++next; + monitored_bodies.remove(E); E = next; Callable::CallError ce; @@ -290,22 +292,24 @@ void GodotArea3D::call_queries() { resptr[i] = &res[i]; } - for (Map<BodyKey, BodyState>::Element *E = monitored_areas.front(); E;) { - if (E->get().state == 0) { // Nothing happened - Map<BodyKey, BodyState>::Element *next = E->next(); - monitored_areas.erase(E); + for (HashMap<BodyKey, BodyState, BodyKey>::Iterator E = monitored_areas.begin(); E;) { + if (E->value.state == 0) { // Nothing happened + HashMap<BodyKey, BodyState, BodyKey>::Iterator next = E; + ++next; + monitored_areas.remove(E); E = next; continue; } - res[0] = E->get().state > 0 ? PhysicsServer3D::AREA_BODY_ADDED : PhysicsServer3D::AREA_BODY_REMOVED; - res[1] = E->key().rid; - res[2] = E->key().instance_id; - res[3] = E->key().body_shape; - res[4] = E->key().area_shape; + res[0] = E->value.state > 0 ? PhysicsServer3D::AREA_BODY_ADDED : PhysicsServer3D::AREA_BODY_REMOVED; + res[1] = E->key.rid; + res[2] = E->key.instance_id; + res[3] = E->key.body_shape; + res[4] = E->key.area_shape; - Map<BodyKey, BodyState>::Element *next = E->next(); - monitored_areas.erase(E); + HashMap<BodyKey, BodyState, BodyKey>::Iterator next = E; + ++next; + monitored_areas.remove(E); E = next; Callable::CallError ce; diff --git a/servers/physics_3d/godot_area_3d.h b/servers/physics_3d/godot_area_3d.h index d15f8ec0a6..51b435eb00 100644 --- a/servers/physics_3d/godot_area_3d.h +++ b/servers/physics_3d/godot_area_3d.h @@ -72,16 +72,15 @@ class GodotArea3D : public GodotCollisionObject3D { uint32_t body_shape = 0; uint32_t area_shape = 0; - _FORCE_INLINE_ bool operator<(const BodyKey &p_key) const { - if (rid == p_key.rid) { - if (body_shape == p_key.body_shape) { - return area_shape < p_key.area_shape; - } else { - return body_shape < p_key.body_shape; - } - } else { - return rid < p_key.rid; - } + static uint32_t hash(const BodyKey &p_key) { + uint32_t h = hash_one_uint64(p_key.rid.get_id()); + h = hash_murmur3_one_64(p_key.instance_id, h); + h = hash_murmur3_one_32(p_key.area_shape, h); + return hash_fmix32(hash_murmur3_one_32(p_key.body_shape, h)); + } + + _FORCE_INLINE_ bool operator==(const BodyKey &p_key) const { + return rid == p_key.rid && instance_id == p_key.instance_id && body_shape == p_key.body_shape && area_shape == p_key.area_shape; } _FORCE_INLINE_ BodyKey() {} @@ -96,11 +95,11 @@ class GodotArea3D : public GodotCollisionObject3D { _FORCE_INLINE_ void dec() { state--; } }; - Map<BodyKey, BodyState> monitored_soft_bodies; - Map<BodyKey, BodyState> monitored_bodies; - Map<BodyKey, BodyState> monitored_areas; + HashMap<BodyKey, BodyState, BodyKey> monitored_soft_bodies; + HashMap<BodyKey, BodyState, BodyKey> monitored_bodies; + HashMap<BodyKey, BodyState, BodyKey> monitored_areas; - Set<GodotConstraint3D *> constraints; + HashSet<GodotConstraint3D *> constraints; virtual void _shapes_changed() override; void _queue_monitor_update(); @@ -164,7 +163,7 @@ public: _FORCE_INLINE_ void add_constraint(GodotConstraint3D *p_constraint) { constraints.insert(p_constraint); } _FORCE_INLINE_ void remove_constraint(GodotConstraint3D *p_constraint) { constraints.erase(p_constraint); } - _FORCE_INLINE_ const Set<GodotConstraint3D *> &get_constraints() const { return constraints; } + _FORCE_INLINE_ const HashSet<GodotConstraint3D *> &get_constraints() const { return constraints; } _FORCE_INLINE_ void clear_constraints() { constraints.clear(); } void set_monitorable(bool p_monitorable); diff --git a/servers/physics_3d/godot_body_3d.h b/servers/physics_3d/godot_body_3d.h index c0c847d920..93bd5a0071 100644 --- a/servers/physics_3d/godot_body_3d.h +++ b/servers/physics_3d/godot_body_3d.h @@ -112,7 +112,7 @@ class GodotBody3D : public GodotCollisionObject3D { virtual void _shapes_changed() override; Transform3D new_transform; - Map<GodotConstraint3D *, int> constraint_map; + HashMap<GodotConstraint3D *, int> constraint_map; Vector<AreaCMP> areas; @@ -196,7 +196,7 @@ public: _FORCE_INLINE_ void add_constraint(GodotConstraint3D *p_constraint, int p_pos) { constraint_map[p_constraint] = p_pos; } _FORCE_INLINE_ void remove_constraint(GodotConstraint3D *p_constraint) { constraint_map.erase(p_constraint); } - const Map<GodotConstraint3D *, int> &get_constraint_map() const { return constraint_map; } + const HashMap<GodotConstraint3D *, int> &get_constraint_map() const { return constraint_map; } _FORCE_INLINE_ void clear_constraint_map() { constraint_map.clear(); } _FORCE_INLINE_ void set_omit_force_integration(bool p_omit_force_integration) { omit_force_integration = p_omit_force_integration; } diff --git a/servers/physics_3d/godot_collision_solver_3d.cpp b/servers/physics_3d/godot_collision_solver_3d.cpp index 0adfabef78..b2d3e4d876 100644 --- a/servers/physics_3d/godot_collision_solver_3d.cpp +++ b/servers/physics_3d/godot_collision_solver_3d.cpp @@ -29,6 +29,7 @@ /*************************************************************************/ #include "godot_collision_solver_3d.h" + #include "godot_collision_solver_3d_sat.h" #include "godot_soft_body_3d.h" @@ -93,7 +94,7 @@ bool GodotCollisionSolver3D::solve_separation_ray(const GodotShape3D *p_shape_A, const GodotSeparationRayShape3D *ray = static_cast<const GodotSeparationRayShape3D *>(p_shape_A); Vector3 from = p_transform_A.origin; - Vector3 to = from + p_transform_A.basis.get_axis(2) * (ray->get_length() + p_margin); + Vector3 to = from + p_transform_A.basis.get_column(2) * (ray->get_length() + p_margin); Vector3 support_A = to; Transform3D ai = p_transform_B.affine_inverse(); @@ -251,7 +252,7 @@ bool GodotCollisionSolver3D::solve_soft_body(const GodotShape3D *p_shape_A, cons // Calculate AABB for internal concave shape query (in local space). AABB local_aabb; for (int i = 0; i < 3; i++) { - Vector3 axis(p_transform_A.basis.get_axis(i)); + Vector3 axis(p_transform_A.basis.get_column(i)); real_t axis_scale = 1.0 / axis.length(); real_t smin = soft_body_aabb.position[i]; @@ -276,19 +277,20 @@ bool GodotCollisionSolver3D::solve_soft_body(const GodotShape3D *p_shape_A, cons } struct _ConcaveCollisionInfo { - const Transform3D *transform_A; - const GodotShape3D *shape_A; - const Transform3D *transform_B; - GodotCollisionSolver3D::CallbackResult result_callback; - void *userdata; - bool swap_result; - bool collided; - int aabb_tests; - int collisions; - bool tested; - real_t margin_A; - real_t margin_B; - Vector3 close_A, close_B; + const Transform3D *transform_A = nullptr; + const GodotShape3D *shape_A = nullptr; + const Transform3D *transform_B = nullptr; + GodotCollisionSolver3D::CallbackResult result_callback = nullptr; + void *userdata = nullptr; + bool swap_result = false; + bool collided = false; + int aabb_tests = 0; + int collisions = 0; + bool tested = false; + real_t margin_A = 0.0f; + real_t margin_B = 0.0f; + Vector3 close_A; + Vector3 close_B; }; bool GodotCollisionSolver3D::concave_callback(void *p_userdata, GodotShape3D *p_convex) { @@ -331,7 +333,7 @@ bool GodotCollisionSolver3D::solve_concave(const GodotShape3D *p_shape_A, const AABB local_aabb; for (int i = 0; i < 3; i++) { - Vector3 axis(p_transform_B.basis.get_axis(i)); + Vector3 axis(p_transform_B.basis.get_column(i)); real_t axis_scale = 1.0 / axis.length(); axis *= axis_scale; @@ -540,7 +542,7 @@ bool GodotCollisionSolver3D::solve_distance(const GodotShape3D *p_shape_A, const AABB local_aabb; for (int i = 0; i < 3; i++) { - Vector3 axis(p_transform_B.basis.get_axis(i)); + Vector3 axis(p_transform_B.basis.get_column(i)); real_t axis_scale = ((real_t)1.0) / axis.length(); axis *= axis_scale; diff --git a/servers/physics_3d/godot_collision_solver_3d_sat.cpp b/servers/physics_3d/godot_collision_solver_3d_sat.cpp index ca429040f5..20e9300778 100644 --- a/servers/physics_3d/godot_collision_solver_3d_sat.cpp +++ b/servers/physics_3d/godot_collision_solver_3d_sat.cpp @@ -68,7 +68,7 @@ *************************************************************************/ struct _CollectorCallback { - GodotCollisionSolver3D::CallbackResult callback; + GodotCollisionSolver3D::CallbackResult callback = nullptr; void *userdata = nullptr; bool swap = false; bool collided = false; @@ -792,7 +792,7 @@ static void _collision_sphere_box(const GodotShape3D *p_a, const Transform3D &p_ // test faces for (int i = 0; i < 3; i++) { - Vector3 axis = p_transform_b.basis.get_axis(i).normalized(); + Vector3 axis = p_transform_b.basis.get_column(i).normalized(); if (!separator.test_axis(axis)) { return; @@ -819,7 +819,7 @@ static void _collision_sphere_box(const GodotShape3D *p_a, const Transform3D &p_ // test edges for (int i = 0; i < 3; i++) { - Vector3 axis = point_axis.cross(p_transform_b.basis.get_axis(i)).cross(p_transform_b.basis.get_axis(i)).normalized(); + Vector3 axis = point_axis.cross(p_transform_b.basis.get_column(i)).cross(p_transform_b.basis.get_column(i)).normalized(); if (!separator.test_axis(axis)) { return; @@ -842,7 +842,7 @@ static void _collision_sphere_capsule(const GodotShape3D *p_a, const Transform3D //capsule sphere 1, sphere - Vector3 capsule_axis = p_transform_b.basis.get_axis(1) * (capsule_B->get_height() * 0.5 - capsule_B->get_radius()); + Vector3 capsule_axis = p_transform_b.basis.get_column(1) * (capsule_B->get_height() * 0.5 - capsule_B->get_radius()); Vector3 capsule_ball_1 = p_transform_b.origin + capsule_axis; @@ -883,7 +883,7 @@ static void _collision_sphere_cylinder(const GodotShape3D *p_a, const Transform3 } // Cylinder B end caps. - Vector3 cylinder_B_axis = p_transform_b.basis.get_axis(1).normalized(); + Vector3 cylinder_B_axis = p_transform_b.basis.get_column(1).normalized(); if (!separator.test_axis(cylinder_B_axis)) { return; } @@ -897,8 +897,8 @@ static void _collision_sphere_cylinder(const GodotShape3D *p_a, const Transform3 // Closest point to cylinder caps. const Vector3 &sphere_center = p_transform_a.origin; - Vector3 cyl_axis = p_transform_b.basis.get_axis(1); - Vector3 cap_axis = p_transform_b.basis.get_axis(0); + Vector3 cyl_axis = p_transform_b.basis.get_column(1); + Vector3 cap_axis = p_transform_b.basis.get_column(0); real_t height_scale = cyl_axis.length(); real_t cap_dist = cylinder_B->get_height() * 0.5 * height_scale; cyl_axis /= height_scale; @@ -1063,7 +1063,7 @@ static void _collision_box_box(const GodotShape3D *p_a, const Transform3D &p_tra // test faces of A for (int i = 0; i < 3; i++) { - Vector3 axis = p_transform_a.basis.get_axis(i).normalized(); + Vector3 axis = p_transform_a.basis.get_column(i).normalized(); if (!separator.test_axis(axis)) { return; @@ -1073,7 +1073,7 @@ static void _collision_box_box(const GodotShape3D *p_a, const Transform3D &p_tra // test faces of B for (int i = 0; i < 3; i++) { - Vector3 axis = p_transform_b.basis.get_axis(i).normalized(); + Vector3 axis = p_transform_b.basis.get_column(i).normalized(); if (!separator.test_axis(axis)) { return; @@ -1083,7 +1083,7 @@ static void _collision_box_box(const GodotShape3D *p_a, const Transform3D &p_tra // test combined edges for (int i = 0; i < 3; i++) { for (int j = 0; j < 3; j++) { - Vector3 axis = p_transform_a.basis.get_axis(i).cross(p_transform_b.basis.get_axis(j)); + Vector3 axis = p_transform_a.basis.get_column(i).cross(p_transform_b.basis.get_column(j)); if (Math::is_zero_approx(axis.length_squared())) { continue; @@ -1129,14 +1129,14 @@ static void _collision_box_box(const GodotShape3D *p_a, const Transform3D &p_tra for (int i = 0; i < 3; i++) { //a ->b - Vector3 axis_a = p_transform_a.basis.get_axis(i); + Vector3 axis_a = p_transform_a.basis.get_column(i); if (!separator.test_axis(axis_ab.cross(axis_a).cross(axis_a).normalized())) { return; } //b ->a - Vector3 axis_b = p_transform_b.basis.get_axis(i); + Vector3 axis_b = p_transform_b.basis.get_column(i); if (!separator.test_axis(axis_ab.cross(axis_b).cross(axis_b).normalized())) { return; @@ -1160,20 +1160,20 @@ static void _collision_box_capsule(const GodotShape3D *p_a, const Transform3D &p // faces of A for (int i = 0; i < 3; i++) { - Vector3 axis = p_transform_a.basis.get_axis(i).normalized(); + Vector3 axis = p_transform_a.basis.get_column(i).normalized(); if (!separator.test_axis(axis)) { return; } } - Vector3 cyl_axis = p_transform_b.basis.get_axis(1).normalized(); + Vector3 cyl_axis = p_transform_b.basis.get_column(1).normalized(); // edges of A, capsule cylinder for (int i = 0; i < 3; i++) { // cylinder - Vector3 box_axis = p_transform_a.basis.get_axis(i); + Vector3 box_axis = p_transform_a.basis.get_column(i); Vector3 axis = box_axis.cross(cyl_axis); if (Math::is_zero_approx(axis.length_squared())) { continue; @@ -1196,7 +1196,7 @@ static void _collision_box_capsule(const GodotShape3D *p_a, const Transform3D &p he.z *= (k * 2 - 1); Vector3 point = p_transform_a.origin; for (int l = 0; l < 3; l++) { - point += p_transform_a.basis.get_axis(l) * he[l]; + point += p_transform_a.basis.get_column(l) * he[l]; } //Vector3 axis = (point - cyl_axis * cyl_axis.dot(point)).normalized(); @@ -1212,7 +1212,7 @@ static void _collision_box_capsule(const GodotShape3D *p_a, const Transform3D &p // capsule balls, edges of A for (int i = 0; i < 2; i++) { - Vector3 capsule_axis = p_transform_b.basis.get_axis(1) * (capsule_B->get_height() * 0.5 - capsule_B->get_radius()); + Vector3 capsule_axis = p_transform_b.basis.get_column(1) * (capsule_B->get_height() * 0.5 - capsule_B->get_radius()); Vector3 sphere_pos = p_transform_b.origin + ((i == 0) ? capsule_axis : -capsule_axis); @@ -1234,7 +1234,7 @@ static void _collision_box_capsule(const GodotShape3D *p_a, const Transform3D &p // test edges of A for (int j = 0; j < 3; j++) { - Vector3 axis = point_axis.cross(p_transform_a.basis.get_axis(j)).cross(p_transform_a.basis.get_axis(j)).normalized(); + Vector3 axis = point_axis.cross(p_transform_a.basis.get_column(j)).cross(p_transform_a.basis.get_column(j)).normalized(); if (!separator.test_axis(axis)) { return; @@ -1258,14 +1258,14 @@ static void _collision_box_cylinder(const GodotShape3D *p_a, const Transform3D & // Faces of A. for (int i = 0; i < 3; i++) { - Vector3 axis = p_transform_a.basis.get_axis(i).normalized(); + Vector3 axis = p_transform_a.basis.get_column(i).normalized(); if (!separator.test_axis(axis)) { return; } } - Vector3 cyl_axis = p_transform_b.basis.get_axis(1).normalized(); + Vector3 cyl_axis = p_transform_b.basis.get_column(1).normalized(); // Cylinder end caps. { @@ -1276,7 +1276,7 @@ static void _collision_box_cylinder(const GodotShape3D *p_a, const Transform3D & // Edges of A, cylinder lateral surface. for (int i = 0; i < 3; i++) { - Vector3 box_axis = p_transform_a.basis.get_axis(i); + Vector3 box_axis = p_transform_a.basis.get_column(i); Vector3 axis = box_axis.cross(cyl_axis); if (Math::is_zero_approx(axis.length_squared())) { continue; @@ -1300,7 +1300,7 @@ static void _collision_box_cylinder(const GodotShape3D *p_a, const Transform3D & Vector3 &point = vertices_A[i * 2 * 2 + j * 2 + k]; point = p_transform_a.origin; for (int l = 0; l < 3; l++) { - point += p_transform_a.basis.get_axis(l) * extent[l]; + point += p_transform_a.basis.get_column(l) * extent[l]; } } } @@ -1380,7 +1380,7 @@ static void _collision_box_convex_polygon(const GodotShape3D *p_a, const Transfo // faces of A for (int i = 0; i < 3; i++) { - Vector3 axis = p_transform_a.basis.get_axis(i).normalized(); + Vector3 axis = p_transform_a.basis.get_column(i).normalized(); if (!separator.test_axis(axis)) { return; @@ -1401,7 +1401,7 @@ static void _collision_box_convex_polygon(const GodotShape3D *p_a, const Transfo // A<->B edges for (int i = 0; i < 3; i++) { - Vector3 e1 = p_transform_a.basis.get_axis(i); + Vector3 e1 = p_transform_a.basis.get_column(i); for (int j = 0; j < edge_count; j++) { Vector3 e2 = p_transform_b.basis.xform(vertices[edges[j].a]) - p_transform_b.basis.xform(vertices[edges[j].b]); @@ -1438,7 +1438,7 @@ static void _collision_box_convex_polygon(const GodotShape3D *p_a, const Transfo for (int i = 0; i < 3; i++) { //a ->b - Vector3 axis_a = p_transform_a.basis.get_axis(i); + Vector3 axis_a = p_transform_a.basis.get_column(i); if (!separator.test_axis(axis_ab.cross(axis_a).cross(axis_a).normalized())) { return; @@ -1456,7 +1456,7 @@ static void _collision_box_convex_polygon(const GodotShape3D *p_a, const Transfo he.z *= (k * 2 - 1); Vector3 point = p_transform_a.origin; for (int l = 0; l < 3; l++) { - point += p_transform_a.basis.get_axis(l) * he[l]; + point += p_transform_a.basis.get_column(l) * he[l]; } for (int e = 0; e < edge_count; e++) { @@ -1497,7 +1497,7 @@ static void _collision_box_face(const GodotShape3D *p_a, const Transform3D &p_tr // faces of A for (int i = 0; i < 3; i++) { - Vector3 axis = p_transform_a.basis.get_axis(i).normalized(); + Vector3 axis = p_transform_a.basis.get_column(i).normalized(); if (axis.dot(normal) < 0.0) { axis *= -1.0; } @@ -1513,7 +1513,7 @@ static void _collision_box_face(const GodotShape3D *p_a, const Transform3D &p_tr Vector3 e = vertex[i] - vertex[(i + 1) % 3]; for (int j = 0; j < 3; j++) { - Vector3 axis = e.cross(p_transform_a.basis.get_axis(j)).normalized(); + Vector3 axis = e.cross(p_transform_a.basis.get_column(j)).normalized(); if (axis.dot(normal) < 0.0) { axis *= -1.0; } @@ -1550,7 +1550,7 @@ static void _collision_box_face(const GodotShape3D *p_a, const Transform3D &p_tr for (int i = 0; i < 3; i++) { //a ->b - Vector3 axis_a = p_transform_a.basis.get_axis(i); + Vector3 axis_a = p_transform_a.basis.get_column(i); Vector3 axis = axis_ab.cross(axis_a).cross(axis_a).normalized(); if (axis.dot(normal) < 0.0) { @@ -1573,7 +1573,7 @@ static void _collision_box_face(const GodotShape3D *p_a, const Transform3D &p_tr he.z *= (k * 2 - 1); Vector3 point = p_transform_a.origin; for (int l = 0; l < 3; l++) { - point += p_transform_a.basis.get_axis(l) * he[l]; + point += p_transform_a.basis.get_column(l) * he[l]; } for (int e = 0; e < 3; e++) { @@ -1623,8 +1623,8 @@ static void _collision_capsule_capsule(const GodotShape3D *p_a, const Transform3 // some values - Vector3 capsule_A_axis = p_transform_a.basis.get_axis(1) * (capsule_A->get_height() * 0.5 - capsule_A->get_radius()); - Vector3 capsule_B_axis = p_transform_b.basis.get_axis(1) * (capsule_B->get_height() * 0.5 - capsule_B->get_radius()); + Vector3 capsule_A_axis = p_transform_a.basis.get_column(1) * (capsule_A->get_height() * 0.5 - capsule_A->get_radius()); + Vector3 capsule_B_axis = p_transform_b.basis.get_column(1) * (capsule_B->get_height() * 0.5 - capsule_B->get_radius()); Vector3 capsule_A_ball_1 = p_transform_a.origin + capsule_A_axis; Vector3 capsule_A_ball_2 = p_transform_a.origin - capsule_A_axis; @@ -1686,14 +1686,14 @@ static void _collision_capsule_cylinder(const GodotShape3D *p_a, const Transform } // Cylinder B end caps. - Vector3 cylinder_B_axis = p_transform_b.basis.get_axis(1).normalized(); + Vector3 cylinder_B_axis = p_transform_b.basis.get_column(1).normalized(); if (!separator.test_axis(cylinder_B_axis)) { return; } // Cylinder edge against capsule balls. - Vector3 capsule_A_axis = p_transform_a.basis.get_axis(1); + Vector3 capsule_A_axis = p_transform_a.basis.get_column(1); Vector3 capsule_A_ball_1 = p_transform_a.origin + capsule_A_axis * (capsule_A->get_height() * 0.5 - capsule_A->get_radius()); Vector3 capsule_A_ball_2 = p_transform_a.origin - capsule_A_axis * (capsule_A->get_height() * 0.5 - capsule_A->get_radius()); @@ -1772,7 +1772,7 @@ static void _collision_capsule_convex_polygon(const GodotShape3D *p_a, const Tra for (int i = 0; i < edge_count; i++) { // cylinder Vector3 edge_axis = p_transform_b.basis.xform(vertices[edges[i].a]) - p_transform_b.basis.xform(vertices[edges[i].b]); - Vector3 axis = edge_axis.cross(p_transform_a.basis.get_axis(1)).normalized(); + Vector3 axis = edge_axis.cross(p_transform_a.basis.get_column(1)).normalized(); if (!separator.test_axis(axis)) { return; @@ -1784,7 +1784,7 @@ static void _collision_capsule_convex_polygon(const GodotShape3D *p_a, const Tra for (int i = 0; i < 2; i++) { // edges of B, capsule cylinder - Vector3 capsule_axis = p_transform_a.basis.get_axis(1) * (capsule_A->get_height() * 0.5 - capsule_A->get_radius()); + Vector3 capsule_axis = p_transform_a.basis.get_column(1) * (capsule_A->get_height() * 0.5 - capsule_A->get_radius()); Vector3 sphere_pos = p_transform_a.origin + ((i == 0) ? capsule_axis : -capsule_axis); @@ -1824,7 +1824,7 @@ static void _collision_capsule_face(const GodotShape3D *p_a, const Transform3D & // edges of B, capsule cylinder - Vector3 capsule_axis = p_transform_a.basis.get_axis(1) * (capsule_A->get_height() * 0.5 - capsule_A->get_radius()); + Vector3 capsule_axis = p_transform_a.basis.get_column(1) * (capsule_A->get_height() * 0.5 - capsule_A->get_radius()); for (int i = 0; i < 3; i++) { // edge-cylinder @@ -1895,8 +1895,8 @@ static void _collision_cylinder_cylinder(const GodotShape3D *p_a, const Transfor SeparatorAxisTest<GodotCylinderShape3D, GodotCylinderShape3D, withMargin> separator(cylinder_A, p_transform_a, cylinder_B, p_transform_b, p_collector, p_margin_a, p_margin_b); - Vector3 cylinder_A_axis = p_transform_a.basis.get_axis(1); - Vector3 cylinder_B_axis = p_transform_b.basis.get_axis(1); + Vector3 cylinder_A_axis = p_transform_a.basis.get_column(1); + Vector3 cylinder_B_axis = p_transform_b.basis.get_column(1); if (!separator.test_previous_axis()) { return; @@ -1983,7 +1983,7 @@ static void _collision_cylinder_face(const GodotShape3D *p_a, const Transform3D return; } - Vector3 cyl_axis = p_transform_a.basis.get_axis(1).normalized(); + Vector3 cyl_axis = p_transform_a.basis.get_column(1).normalized(); if (cyl_axis.dot(normal) < 0.0) { cyl_axis *= -1.0; } diff --git a/servers/physics_3d/godot_physics_server_3d.cpp b/servers/physics_3d/godot_physics_server_3d.cpp index e5107be74b..b735283ebe 100644 --- a/servers/physics_3d/godot_physics_server_3d.cpp +++ b/servers/physics_3d/godot_physics_server_3d.cpp @@ -1531,7 +1531,7 @@ void GodotPhysicsServer3D::free(RID p_rid) { GodotShape3D *shape = shape_owner.get_or_null(p_rid); while (shape->get_owners().size()) { - GodotShapeOwner3D *so = shape->get_owners().front()->key(); + GodotShapeOwner3D *so = shape->get_owners().begin()->key; so->remove_shape(shape); } @@ -1570,7 +1570,7 @@ void GodotPhysicsServer3D::free(RID p_rid) { GodotSpace3D *space = space_owner.get_or_null(p_rid); while (space->get_objects().size()) { - GodotCollisionObject3D *co = static_cast<GodotCollisionObject3D *>(space->get_objects().front()->get()); + GodotCollisionObject3D *co = static_cast<GodotCollisionObject3D *>(*space->get_objects().begin()); co->set_space(nullptr); } @@ -1611,11 +1611,11 @@ void GodotPhysicsServer3D::step(real_t p_step) { island_count = 0; active_objects = 0; collision_pairs = 0; - for (Set<const GodotSpace3D *>::Element *E = active_spaces.front(); E; E = E->next()) { - stepper->step(const_cast<GodotSpace3D *>(E->get()), p_step); - island_count += E->get()->get_island_count(); - active_objects += E->get()->get_active_objects(); - collision_pairs += E->get()->get_collision_pairs(); + for (const GodotSpace3D *E : active_spaces) { + stepper->step(const_cast<GodotSpace3D *>(E), p_step); + island_count += E->get_island_count(); + active_objects += E->get_active_objects(); + collision_pairs += E->get_collision_pairs(); } #endif } @@ -1635,8 +1635,8 @@ void GodotPhysicsServer3D::flush_queries() { uint64_t time_beg = OS::get_singleton()->get_ticks_usec(); - for (Set<const GodotSpace3D *>::Element *E = active_spaces.front(); E; E = E->next()) { - GodotSpace3D *space = const_cast<GodotSpace3D *>(E->get()); + for (const GodotSpace3D *E : active_spaces) { + GodotSpace3D *space = const_cast<GodotSpace3D *>(E); space->call_queries(); } @@ -1656,9 +1656,9 @@ void GodotPhysicsServer3D::flush_queries() { total_time[i] = 0; } - for (Set<const GodotSpace3D *>::Element *E = active_spaces.front(); E; E = E->next()) { + for (const GodotSpace3D *E : active_spaces) { for (int i = 0; i < GodotSpace3D::ELAPSED_TIME_MAX; i++) { - total_time[i] += E->get()->get_elapsed_time(GodotSpace3D::ElapsedTime(i)); + total_time[i] += E->get_elapsed_time(GodotSpace3D::ElapsedTime(i)); } } @@ -1671,7 +1671,7 @@ void GodotPhysicsServer3D::flush_queries() { values.push_back("flush_queries"); values.push_back(USEC_TO_SEC(OS::get_singleton()->get_ticks_usec() - time_beg)); - values.push_front("physics"); + values.push_front("physics_3d"); EngineDebugger::profiler_add_frame_data("servers", values); } #endif diff --git a/servers/physics_3d/godot_physics_server_3d.h b/servers/physics_3d/godot_physics_server_3d.h index d2078a912c..1d57451925 100644 --- a/servers/physics_3d/godot_physics_server_3d.h +++ b/servers/physics_3d/godot_physics_server_3d.h @@ -54,7 +54,7 @@ class GodotPhysicsServer3D : public PhysicsServer3D { bool flushing_queries = false; GodotStep3D *stepper = nullptr; - Set<const GodotSpace3D *> active_spaces; + HashSet<const GodotSpace3D *> active_spaces; mutable RID_PtrOwner<GodotShape3D, true> shape_owner; mutable RID_PtrOwner<GodotSpace3D, true> space_owner; diff --git a/servers/physics_3d/godot_shape_3d.cpp b/servers/physics_3d/godot_shape_3d.cpp index 21595c9612..5e310670a5 100644 --- a/servers/physics_3d/godot_shape_3d.cpp +++ b/servers/physics_3d/godot_shape_3d.cpp @@ -76,20 +76,20 @@ Vector3 GodotShape3D::get_support(const Vector3 &p_normal) const { } void GodotShape3D::add_owner(GodotShapeOwner3D *p_owner) { - Map<GodotShapeOwner3D *, int>::Element *E = owners.find(p_owner); + HashMap<GodotShapeOwner3D *, int>::Iterator E = owners.find(p_owner); if (E) { - E->get()++; + E->value++; } else { owners[p_owner] = 1; } } void GodotShape3D::remove_owner(GodotShapeOwner3D *p_owner) { - Map<GodotShapeOwner3D *, int>::Element *E = owners.find(p_owner); + HashMap<GodotShapeOwner3D *, int>::Iterator E = owners.find(p_owner); ERR_FAIL_COND(!E); - E->get()--; - if (E->get() == 0) { - owners.erase(E); + E->value--; + if (E->value == 0) { + owners.remove(E); } } @@ -97,7 +97,7 @@ bool GodotShape3D::is_owner(GodotShapeOwner3D *p_owner) const { return owners.has(p_owner); } -const Map<GodotShapeOwner3D *, int> &GodotShape3D::get_owners() const { +const HashMap<GodotShapeOwner3D *, int> &GodotShape3D::get_owners() const { return owners; } @@ -662,7 +662,7 @@ GodotCapsuleShape3D::GodotCapsuleShape3D() {} /********** CYLINDER *************/ void GodotCylinderShape3D::project_range(const Vector3 &p_normal, const Transform3D &p_transform, real_t &r_min, real_t &r_max) const { - Vector3 cylinder_axis = p_transform.basis.get_axis(1).normalized(); + Vector3 cylinder_axis = p_transform.basis.get_column(1).normalized(); real_t axis_dot = cylinder_axis.dot(p_normal); Vector3 local_normal = p_transform.basis.xform_inv(p_normal); @@ -1438,7 +1438,7 @@ Vector3 GodotConcavePolygonShape3D::get_moment_of_inertia(real_t p_mass) const { struct _Volume_BVH_Element { AABB aabb; Vector3 center; - int face_index; + int face_index = 0; }; struct _Volume_BVH_CompareX { @@ -1461,10 +1461,10 @@ struct _Volume_BVH_CompareZ { struct _Volume_BVH { AABB aabb; - _Volume_BVH *left; - _Volume_BVH *right; + _Volume_BVH *left = nullptr; + _Volume_BVH *right = nullptr; - int face_index; + int face_index = 0; }; _Volume_BVH *_volume_build_bvh(_Volume_BVH_Element *p_elements, int p_size, int &count) { diff --git a/servers/physics_3d/godot_shape_3d.h b/servers/physics_3d/godot_shape_3d.h index 9d171c3928..1fc8f7c711 100644 --- a/servers/physics_3d/godot_shape_3d.h +++ b/servers/physics_3d/godot_shape_3d.h @@ -51,7 +51,7 @@ class GodotShape3D { bool configured = false; real_t custom_bias = 0.0; - Map<GodotShapeOwner3D *, int> owners; + HashMap<GodotShapeOwner3D *, int> owners; protected: void configure(const AABB &p_aabb); @@ -93,7 +93,7 @@ public: void add_owner(GodotShapeOwner3D *p_owner); void remove_owner(GodotShapeOwner3D *p_owner); bool is_owner(GodotShapeOwner3D *p_owner) const; - const Map<GodotShapeOwner3D *, int> &get_owners() const; + const HashMap<GodotShapeOwner3D *, int> &get_owners() const; GodotShape3D() {} virtual ~GodotShape3D(); diff --git a/servers/physics_3d/godot_soft_body_3d.cpp b/servers/physics_3d/godot_soft_body_3d.cpp index be4b41292d..173843072a 100644 --- a/servers/physics_3d/godot_soft_body_3d.cpp +++ b/servers/physics_3d/godot_soft_body_3d.cpp @@ -33,7 +33,7 @@ #include "godot_space_3d.h" #include "core/math/geometry_3d.h" -#include "core/templates/map.h" +#include "core/templates/rb_map.h" #include "servers/rendering_server.h" // Based on Bullet soft body. @@ -429,33 +429,33 @@ uint32_t GodotSoftBody3D::get_node_count() const { } real_t GodotSoftBody3D::get_node_inv_mass(uint32_t p_node_index) const { - ERR_FAIL_COND_V(p_node_index >= nodes.size(), 0.0); + ERR_FAIL_UNSIGNED_INDEX_V(p_node_index, nodes.size(), 0.0); return nodes[p_node_index].im; } Vector3 GodotSoftBody3D::get_node_position(uint32_t p_node_index) const { - ERR_FAIL_COND_V(p_node_index >= nodes.size(), Vector3()); + ERR_FAIL_UNSIGNED_INDEX_V(p_node_index, nodes.size(), Vector3()); return nodes[p_node_index].x; } Vector3 GodotSoftBody3D::get_node_velocity(uint32_t p_node_index) const { - ERR_FAIL_COND_V(p_node_index >= nodes.size(), Vector3()); + ERR_FAIL_UNSIGNED_INDEX_V(p_node_index, nodes.size(), Vector3()); return nodes[p_node_index].v; } Vector3 GodotSoftBody3D::get_node_biased_velocity(uint32_t p_node_index) const { - ERR_FAIL_COND_V(p_node_index >= nodes.size(), Vector3()); + ERR_FAIL_UNSIGNED_INDEX_V(p_node_index, nodes.size(), Vector3()); return nodes[p_node_index].bv; } void GodotSoftBody3D::apply_node_impulse(uint32_t p_node_index, const Vector3 &p_impulse) { - ERR_FAIL_COND(p_node_index >= nodes.size()); + ERR_FAIL_UNSIGNED_INDEX(p_node_index, nodes.size()); Node &node = nodes[p_node_index]; node.v += p_impulse * node.im; } void GodotSoftBody3D::apply_node_bias_impulse(uint32_t p_node_index, const Vector3 &p_impulse) { - ERR_FAIL_COND(p_node_index >= nodes.size()); + ERR_FAIL_UNSIGNED_INDEX(p_node_index, nodes.size()); Node &node = nodes[p_node_index]; node.bv += p_impulse * node.im; } @@ -465,7 +465,7 @@ uint32_t GodotSoftBody3D::get_face_count() const { } void GodotSoftBody3D::get_face_points(uint32_t p_face_index, Vector3 &r_point_1, Vector3 &r_point_2, Vector3 &r_point_3) const { - ERR_FAIL_COND(p_face_index >= faces.size()); + ERR_FAIL_UNSIGNED_INDEX(p_face_index, faces.size()); const Face &face = faces[p_face_index]; r_point_1 = face.n[0]->x; r_point_2 = face.n[1]->x; @@ -473,7 +473,7 @@ void GodotSoftBody3D::get_face_points(uint32_t p_face_index, Vector3 &r_point_1, } Vector3 GodotSoftBody3D::get_face_normal(uint32_t p_face_index) const { - ERR_FAIL_COND_V(p_face_index >= faces.size(), Vector3()); + ERR_FAIL_UNSIGNED_INDEX_V(p_face_index, faces.size(), Vector3()); return faces[p_face_index].normal; } @@ -494,7 +494,7 @@ bool GodotSoftBody3D::create_from_trimesh(const Vector<int> &p_indices, const Ve // Process vertices. { uint32_t vertex_count = 0; - Map<Vector3, uint32_t> unique_vertices; + HashMap<Vector3, uint32_t> unique_vertices; vertices.resize(visual_vertex_count); map_visual_to_physics.resize(visual_vertex_count); @@ -502,11 +502,11 @@ bool GodotSoftBody3D::create_from_trimesh(const Vector<int> &p_indices, const Ve for (int visual_vertex_index = 0; visual_vertex_index < visual_vertex_count; ++visual_vertex_index) { const Vector3 &vertex = p_vertices[visual_vertex_index]; - Map<Vector3, uint32_t>::Element *e = unique_vertices.find(vertex); + HashMap<Vector3, uint32_t>::Iterator e = unique_vertices.find(vertex); uint32_t vertex_id; if (e) { // Already existing. - vertex_id = e->value(); + vertex_id = e->value; } else { // Create new one. vertex_id = vertex_count++; diff --git a/servers/physics_3d/godot_soft_body_3d.h b/servers/physics_3d/godot_soft_body_3d.h index 96f63e5819..86f73c366b 100644 --- a/servers/physics_3d/godot_soft_body_3d.h +++ b/servers/physics_3d/godot_soft_body_3d.h @@ -37,8 +37,8 @@ #include "core/math/aabb.h" #include "core/math/dynamic_bvh.h" #include "core/math/vector3.h" +#include "core/templates/hash_set.h" #include "core/templates/local_vector.h" -#include "core/templates/set.h" #include "core/templates/vset.h" class GodotConstraint3D; @@ -103,7 +103,7 @@ class GodotSoftBody3D : public GodotCollisionObject3D { SelfList<GodotSoftBody3D> active_list; - Set<GodotConstraint3D *> constraints; + HashSet<GodotConstraint3D *> constraints; Vector<AreaCMP> areas; @@ -123,7 +123,7 @@ public: _FORCE_INLINE_ void add_constraint(GodotConstraint3D *p_constraint) { constraints.insert(p_constraint); } _FORCE_INLINE_ void remove_constraint(GodotConstraint3D *p_constraint) { constraints.erase(p_constraint); } - _FORCE_INLINE_ const Set<GodotConstraint3D *> &get_constraints() const { return constraints; } + _FORCE_INLINE_ const HashSet<GodotConstraint3D *> &get_constraints() const { return constraints; } _FORCE_INLINE_ void clear_constraints() { constraints.clear(); } _FORCE_INLINE_ void add_exception(const RID &p_exception) { exceptions.insert(p_exception); } diff --git a/servers/physics_3d/godot_space_3d.cpp b/servers/physics_3d/godot_space_3d.cpp index 0b84520c39..533d7605ce 100644 --- a/servers/physics_3d/godot_space_3d.cpp +++ b/servers/physics_3d/godot_space_3d.cpp @@ -906,7 +906,7 @@ bool GodotSpace3D::test_body_motion(GodotBody3D *p_body, const PhysicsServer3D:: } bool collided = false; - if (recovered || (safe < 1)) { + if ((p_parameters.recovery_as_collision && recovered) || (safe < 1)) { if (safe >= 1) { best_shape = -1; //no best shape with cast, reset to -1 } @@ -1096,7 +1096,7 @@ void GodotSpace3D::remove_object(GodotCollisionObject3D *p_object) { objects.erase(p_object); } -const Set<GodotCollisionObject3D *> &GodotSpace3D::get_objects() const { +const HashSet<GodotCollisionObject3D *> &GodotSpace3D::get_objects() const { return objects; } diff --git a/servers/physics_3d/godot_space_3d.h b/servers/physics_3d/godot_space_3d.h index 6308ede9a9..df7315e96d 100644 --- a/servers/physics_3d/godot_space_3d.h +++ b/servers/physics_3d/godot_space_3d.h @@ -89,7 +89,7 @@ private: static void *_broadphase_pair(GodotCollisionObject3D *A, int p_subindex_A, GodotCollisionObject3D *B, int p_subindex_B, void *p_self); static void _broadphase_unpair(GodotCollisionObject3D *A, int p_subindex_A, GodotCollisionObject3D *B, int p_subindex_B, void *p_data, void *p_self); - Set<GodotCollisionObject3D *> objects; + HashSet<GodotCollisionObject3D *> objects; GodotArea3D *area = nullptr; @@ -158,7 +158,7 @@ public: void add_object(GodotCollisionObject3D *p_object); void remove_object(GodotCollisionObject3D *p_object); - const Set<GodotCollisionObject3D *> &get_objects() const; + const HashSet<GodotCollisionObject3D *> &get_objects() const; _FORCE_INLINE_ int get_solver_iterations() const { return solver_iterations; } _FORCE_INLINE_ real_t get_contact_recycle_radius() const { return contact_recycle_radius; } diff --git a/servers/physics_3d/godot_step_3d.cpp b/servers/physics_3d/godot_step_3d.cpp index 204adae450..99656d01a0 100644 --- a/servers/physics_3d/godot_step_3d.cpp +++ b/servers/physics_3d/godot_step_3d.cpp @@ -87,8 +87,8 @@ void GodotStep3D::_populate_island(GodotBody3D *p_body, LocalVector<GodotBody3D void GodotStep3D::_populate_island_soft_body(GodotSoftBody3D *p_soft_body, LocalVector<GodotBody3D *> &p_body_island, LocalVector<GodotConstraint3D *> &p_constraint_island) { p_soft_body->set_island_step(_step); - for (Set<GodotConstraint3D *>::Element *E = p_soft_body->get_constraints().front(); E; E = E->next()) { - GodotConstraint3D *constraint = const_cast<GodotConstraint3D *>(E->get()); + for (const GodotConstraint3D *E : p_soft_body->get_constraints()) { + GodotConstraint3D *constraint = const_cast<GodotConstraint3D *>(E); if (constraint->get_island_step() == _step) { continue; // Already processed. } @@ -236,8 +236,8 @@ void GodotStep3D::step(GodotSpace3D *p_space, real_t p_delta) { const SelfList<GodotArea3D>::List &aml = p_space->get_moved_area_list(); while (aml.first()) { - for (const Set<GodotConstraint3D *>::Element *E = aml.first()->self()->get_constraints().front(); E; E = E->next()) { - GodotConstraint3D *constraint = E->get(); + for (GodotConstraint3D *E : aml.first()->self()->get_constraints()) { + GodotConstraint3D *constraint = E; if (constraint->get_island_step() == _step) { continue; } diff --git a/servers/physics_3d/joints/godot_cone_twist_joint_3d.cpp b/servers/physics_3d/joints/godot_cone_twist_joint_3d.cpp index dc4858ff9a..28cc064a1e 100644 --- a/servers/physics_3d/joints/godot_cone_twist_joint_3d.cpp +++ b/servers/physics_3d/joints/godot_cone_twist_joint_3d.cpp @@ -147,8 +147,8 @@ bool GodotConeTwistJoint3D::setup(real_t p_timestep) { Vector3 b1Axis1, b1Axis2, b1Axis3; Vector3 b2Axis1, b2Axis2; - b1Axis1 = A->get_transform().basis.xform(this->m_rbAFrame.basis.get_axis(0)); - b2Axis1 = B->get_transform().basis.xform(this->m_rbBFrame.basis.get_axis(0)); + b1Axis1 = A->get_transform().basis.xform(this->m_rbAFrame.basis.get_column(0)); + b2Axis1 = B->get_transform().basis.xform(this->m_rbBFrame.basis.get_column(0)); real_t swing1 = real_t(0.), swing2 = real_t(0.); @@ -158,7 +158,7 @@ bool GodotConeTwistJoint3D::setup(real_t p_timestep) { // Get Frame into world space if (m_swingSpan1 >= real_t(0.05f)) { - b1Axis2 = A->get_transform().basis.xform(this->m_rbAFrame.basis.get_axis(1)); + b1Axis2 = A->get_transform().basis.xform(this->m_rbAFrame.basis.get_column(1)); //swing1 = btAtan2Fast( b2Axis1.dot(b1Axis2),b2Axis1.dot(b1Axis1) ); swx = b2Axis1.dot(b1Axis1); swy = b2Axis1.dot(b1Axis2); @@ -169,7 +169,7 @@ bool GodotConeTwistJoint3D::setup(real_t p_timestep) { } if (m_swingSpan2 >= real_t(0.05f)) { - b1Axis3 = A->get_transform().basis.xform(this->m_rbAFrame.basis.get_axis(2)); + b1Axis3 = A->get_transform().basis.xform(this->m_rbAFrame.basis.get_column(2)); //swing2 = btAtan2Fast( b2Axis1.dot(b1Axis3),b2Axis1.dot(b1Axis1) ); swx = b2Axis1.dot(b1Axis1); swy = b2Axis1.dot(b1Axis3); @@ -199,7 +199,7 @@ bool GodotConeTwistJoint3D::setup(real_t p_timestep) { // Twist limits if (m_twistSpan >= real_t(0.)) { - Vector3 b2Axis22 = B->get_transform().basis.xform(this->m_rbBFrame.basis.get_axis(1)); + Vector3 b2Axis22 = B->get_transform().basis.xform(this->m_rbBFrame.basis.get_column(1)); Quaternion rotationArc = Quaternion(b2Axis1, b1Axis1); Vector3 TwistRef = rotationArc.xform(b2Axis22); real_t twist = atan2fast(TwistRef.dot(b1Axis3), TwistRef.dot(b1Axis2)); diff --git a/servers/physics_3d/joints/godot_generic_6dof_joint_3d.cpp b/servers/physics_3d/joints/godot_generic_6dof_joint_3d.cpp index 41e9bf06bb..e0fa940104 100644 --- a/servers/physics_3d/joints/godot_generic_6dof_joint_3d.cpp +++ b/servers/physics_3d/joints/godot_generic_6dof_joint_3d.cpp @@ -249,8 +249,8 @@ void GodotGeneric6DOFJoint3D::calculateAngleInfo() { // easier to take the euler rate expression for d(angle[2])/dt with respect // to the components of w and set that to 0. - Vector3 axis0 = m_calculatedTransformB.basis.get_axis(0); - Vector3 axis2 = m_calculatedTransformA.basis.get_axis(2); + Vector3 axis0 = m_calculatedTransformB.basis.get_column(0); + Vector3 axis2 = m_calculatedTransformA.basis.get_column(2); m_calculatedAxis[1] = axis2.cross(axis0); m_calculatedAxis[0] = m_calculatedAxis[1].cross(axis2); @@ -345,9 +345,9 @@ bool GodotGeneric6DOFJoint3D::setup(real_t p_timestep) { for (i = 0; i < 3; i++) { if (m_linearLimits.enable_limit[i] && m_linearLimits.isLimited(i)) { if (m_useLinearReferenceFrameA) { - normalWorld = m_calculatedTransformA.basis.get_axis(i); + normalWorld = m_calculatedTransformA.basis.get_column(i); } else { - normalWorld = m_calculatedTransformB.basis.get_axis(i); + normalWorld = m_calculatedTransformB.basis.get_column(i); } buildLinearJacobian( @@ -388,9 +388,9 @@ void GodotGeneric6DOFJoint3D::solve(real_t p_timestep) { jacDiagABInv = real_t(1.) / m_jacLinear[i].getDiagonal(); if (m_useLinearReferenceFrameA) { - linear_axis = m_calculatedTransformA.basis.get_axis(i); + linear_axis = m_calculatedTransformA.basis.get_column(i); } else { - linear_axis = m_calculatedTransformB.basis.get_axis(i); + linear_axis = m_calculatedTransformB.basis.get_column(i); } m_linearLimits.solveLinearAxis( diff --git a/servers/physics_3d/joints/godot_hinge_joint_3d.cpp b/servers/physics_3d/joints/godot_hinge_joint_3d.cpp index 1c4d5dec23..01c59395c8 100644 --- a/servers/physics_3d/joints/godot_hinge_joint_3d.cpp +++ b/servers/physics_3d/joints/godot_hinge_joint_3d.cpp @@ -92,16 +92,16 @@ GodotHingeJoint3D::GodotHingeJoint3D(GodotBody3D *rbA, GodotBody3D *rbB, const V m_rbAFrame.origin = pivotInA; // since no frame is given, assume this to be zero angle and just pick rb transform axis - Vector3 rbAxisA1 = rbA->get_transform().basis.get_axis(0); + Vector3 rbAxisA1 = rbA->get_transform().basis.get_column(0); Vector3 rbAxisA2; real_t projection = axisInA.dot(rbAxisA1); if (projection >= 1.0f - CMP_EPSILON) { - rbAxisA1 = -rbA->get_transform().basis.get_axis(2); - rbAxisA2 = rbA->get_transform().basis.get_axis(1); + rbAxisA1 = -rbA->get_transform().basis.get_column(2); + rbAxisA2 = rbA->get_transform().basis.get_column(1); } else if (projection <= -1.0f + CMP_EPSILON) { - rbAxisA1 = rbA->get_transform().basis.get_axis(2); - rbAxisA2 = rbA->get_transform().basis.get_axis(1); + rbAxisA1 = rbA->get_transform().basis.get_column(2); + rbAxisA2 = rbA->get_transform().basis.get_column(1); } else { rbAxisA2 = axisInA.cross(rbAxisA1); rbAxisA1 = rbAxisA2.cross(axisInA); @@ -171,11 +171,11 @@ bool GodotHingeJoint3D::setup(real_t p_step) { Vector3 jointAxis0local; Vector3 jointAxis1local; - plane_space(m_rbAFrame.basis.get_axis(2), jointAxis0local, jointAxis1local); + plane_space(m_rbAFrame.basis.get_column(2), jointAxis0local, jointAxis1local); Vector3 jointAxis0 = A->get_transform().basis.xform(jointAxis0local); Vector3 jointAxis1 = A->get_transform().basis.xform(jointAxis1local); - Vector3 hingeAxisWorld = A->get_transform().basis.xform(m_rbAFrame.basis.get_axis(2)); + Vector3 hingeAxisWorld = A->get_transform().basis.xform(m_rbAFrame.basis.get_column(2)); memnew_placement( &m_jacAng[0], @@ -226,7 +226,7 @@ bool GodotHingeJoint3D::setup(real_t p_step) { } //Compute K = J*W*J' for hinge axis - Vector3 axisA = A->get_transform().basis.xform(m_rbAFrame.basis.get_axis(2)); + Vector3 axisA = A->get_transform().basis.xform(m_rbAFrame.basis.get_column(2)); m_kHinge = 1.0f / (A->compute_angular_impulse_denominator(axisA) + B->compute_angular_impulse_denominator(axisA)); return true; @@ -271,8 +271,8 @@ void GodotHingeJoint3D::solve(real_t p_step) { ///solve angular part // get axes in world space - Vector3 axisA = A->get_transform().basis.xform(m_rbAFrame.basis.get_axis(2)); - Vector3 axisB = B->get_transform().basis.xform(m_rbBFrame.basis.get_axis(2)); + Vector3 axisA = A->get_transform().basis.xform(m_rbAFrame.basis.get_column(2)); + Vector3 axisB = B->get_transform().basis.xform(m_rbBFrame.basis.get_column(2)); const Vector3 &angVelA = A->get_angular_velocity(); const Vector3 &angVelB = B->get_angular_velocity(); @@ -384,9 +384,9 @@ static _FORCE_INLINE_ real_t atan2fast(real_t y, real_t x) { } real_t GodotHingeJoint3D::get_hinge_angle() { - const Vector3 refAxis0 = A->get_transform().basis.xform(m_rbAFrame.basis.get_axis(0)); - const Vector3 refAxis1 = A->get_transform().basis.xform(m_rbAFrame.basis.get_axis(1)); - const Vector3 swingAxis = B->get_transform().basis.xform(m_rbBFrame.basis.get_axis(1)); + const Vector3 refAxis0 = A->get_transform().basis.xform(m_rbAFrame.basis.get_column(0)); + const Vector3 refAxis1 = A->get_transform().basis.xform(m_rbAFrame.basis.get_column(1)); + const Vector3 swingAxis = B->get_transform().basis.xform(m_rbBFrame.basis.get_column(1)); return atan2fast(swingAxis.dot(refAxis0), swingAxis.dot(refAxis1)); } diff --git a/servers/physics_3d/joints/godot_slider_joint_3d.cpp b/servers/physics_3d/joints/godot_slider_joint_3d.cpp index f175421304..4539be21e3 100644 --- a/servers/physics_3d/joints/godot_slider_joint_3d.cpp +++ b/servers/physics_3d/joints/godot_slider_joint_3d.cpp @@ -102,7 +102,7 @@ bool GodotSliderJoint3D::setup(real_t p_step) { m_calculatedTransformB = B->get_transform() * m_frameInB; m_realPivotAInW = m_calculatedTransformA.origin; m_realPivotBInW = m_calculatedTransformB.origin; - m_sliderAxis = m_calculatedTransformA.basis.get_axis(0); // along X + m_sliderAxis = m_calculatedTransformA.basis.get_column(0); // along X m_delta = m_realPivotBInW - m_realPivotAInW; m_projPivotInW = m_realPivotAInW + m_sliderAxis.dot(m_delta) * m_sliderAxis; m_relPosA = m_projPivotInW - A->get_transform().origin; @@ -111,7 +111,7 @@ bool GodotSliderJoint3D::setup(real_t p_step) { int i; //linear part for (i = 0; i < 3; i++) { - normalWorld = m_calculatedTransformA.basis.get_axis(i); + normalWorld = m_calculatedTransformA.basis.get_column(i); memnew_placement( &m_jacLin[i], GodotJacobianEntry3D( @@ -130,7 +130,7 @@ bool GodotSliderJoint3D::setup(real_t p_step) { testLinLimits(); // angular part for (i = 0; i < 3; i++) { - normalWorld = m_calculatedTransformA.basis.get_axis(i); + normalWorld = m_calculatedTransformA.basis.get_column(i); memnew_placement( &m_jacAng[i], GodotJacobianEntry3D( @@ -141,7 +141,7 @@ bool GodotSliderJoint3D::setup(real_t p_step) { B->get_inv_inertia())); } testAngLimits(); - Vector3 axisA = m_calculatedTransformA.basis.get_axis(0); + Vector3 axisA = m_calculatedTransformA.basis.get_column(0); m_kAngle = real_t(1.0) / (A->compute_angular_impulse_denominator(axisA) + B->compute_angular_impulse_denominator(axisA)); // clear accumulator for motors m_accumulatedLinMotorImpulse = real_t(0.0); @@ -206,8 +206,8 @@ void GodotSliderJoint3D::solve(real_t p_step) { } // angular // get axes in world space - Vector3 axisA = m_calculatedTransformA.basis.get_axis(0); - Vector3 axisB = m_calculatedTransformB.basis.get_axis(0); + Vector3 axisA = m_calculatedTransformA.basis.get_column(0); + Vector3 axisB = m_calculatedTransformB.basis.get_column(0); const Vector3 &angVelA = A->get_angular_velocity(); const Vector3 &angVelB = B->get_angular_velocity(); @@ -297,14 +297,14 @@ void GodotSliderJoint3D::calculateTransforms() { m_calculatedTransformB = B->get_transform() * m_frameInB; m_realPivotAInW = m_calculatedTransformA.origin; m_realPivotBInW = m_calculatedTransformB.origin; - m_sliderAxis = m_calculatedTransformA.basis.get_axis(0); // along X + m_sliderAxis = m_calculatedTransformA.basis.get_column(0); // along X m_delta = m_realPivotBInW - m_realPivotAInW; m_projPivotInW = m_realPivotAInW + m_sliderAxis.dot(m_delta) * m_sliderAxis; Vector3 normalWorld; int i; //linear part for (i = 0; i < 3; i++) { - normalWorld = m_calculatedTransformA.basis.get_axis(i); + normalWorld = m_calculatedTransformA.basis.get_column(i); m_depth[i] = m_delta.dot(normalWorld); } } @@ -335,9 +335,9 @@ void GodotSliderJoint3D::testAngLimits() { m_angDepth = real_t(0.); m_solveAngLim = false; if (m_lowerAngLimit <= m_upperAngLimit) { - const Vector3 axisA0 = m_calculatedTransformA.basis.get_axis(1); - const Vector3 axisA1 = m_calculatedTransformA.basis.get_axis(2); - const Vector3 axisB0 = m_calculatedTransformB.basis.get_axis(1); + const Vector3 axisA0 = m_calculatedTransformA.basis.get_column(1); + const Vector3 axisA1 = m_calculatedTransformA.basis.get_column(2); + const Vector3 axisB0 = m_calculatedTransformB.basis.get_column(1); real_t rot = atan2fast(axisB0.dot(axisA1), axisB0.dot(axisA0)); if (rot < m_lowerAngLimit) { m_angDepth = rot - m_lowerAngLimit; |