diff options
Diffstat (limited to 'servers/physics_3d')
32 files changed, 271 insertions, 307 deletions
diff --git a/servers/physics_3d/area_3d_sw.cpp b/servers/physics_3d/area_3d_sw.cpp index bb4e0ed752..a9f5c4aec3 100644 --- a/servers/physics_3d/area_3d_sw.cpp +++ b/servers/physics_3d/area_3d_sw.cpp @@ -52,7 +52,7 @@ void Area3DSW::_shapes_changed() { } } -void Area3DSW::set_transform(const Transform &p_transform) { +void Area3DSW::set_transform(const Transform3D &p_transform) { if (!moved_list.in_list() && get_space()) { get_space()->area_add_to_moved_list(&moved_list); } diff --git a/servers/physics_3d/area_3d_sw.h b/servers/physics_3d/area_3d_sw.h index 8a0a1e963b..12f7545c08 100644 --- a/servers/physics_3d/area_3d_sw.h +++ b/servers/physics_3d/area_3d_sw.h @@ -156,7 +156,7 @@ public: void set_monitorable(bool p_monitorable); _FORCE_INLINE_ bool is_monitorable() const { return monitorable; } - void set_transform(const Transform &p_transform); + void set_transform(const Transform3D &p_transform); void set_space(Space3DSW *p_space); diff --git a/servers/physics_3d/body_3d_sw.cpp b/servers/physics_3d/body_3d_sw.cpp index 4357c474e4..ea6064cb4c 100644 --- a/servers/physics_3d/body_3d_sw.cpp +++ b/servers/physics_3d/body_3d_sw.cpp @@ -54,7 +54,7 @@ void Body3DSW::update_inertias() { // Update shapes and motions. switch (mode) { - case PhysicsServer3D::BODY_MODE_RIGID: { + case PhysicsServer3D::BODY_MODE_DYNAMIC: { // Update tensor for all shapes, not the best way but should be somehow OK. (inspired from bullet) real_t total_area = 0; @@ -65,16 +65,18 @@ void Body3DSW::update_inertias() { // We have to recompute the center of mass. center_of_mass_local.zero(); - for (int i = 0; i < get_shape_count(); i++) { - real_t area = get_shape_area(i); + if (total_area != 0.0) { + for (int i = 0; i < get_shape_count(); i++) { + real_t area = get_shape_area(i); - real_t mass = area * this->mass / total_area; + real_t mass = area * this->mass / total_area; - // NOTE: we assume that the shape origin is also its center of mass. - center_of_mass_local += mass * get_shape_transform(i).origin; - } + // NOTE: we assume that the shape origin is also its center of mass. + center_of_mass_local += mass * get_shape_transform(i).origin; + } - center_of_mass_local /= mass; + center_of_mass_local /= mass; + } // Recompute the inertia tensor. Basis inertia_tensor; @@ -86,16 +88,19 @@ void Body3DSW::update_inertias() { continue; } + real_t area = get_shape_area(i); + if (area == 0.0) { + continue; + } + inertia_set = true; const Shape3DSW *shape = get_shape(i); - real_t area = get_shape_area(i); - real_t mass = area * this->mass / total_area; Basis shape_inertia_tensor = shape->get_moment_of_inertia(mass).to_diagonal_matrix(); - Transform shape_transform = get_shape_transform(i); + Transform3D shape_transform = get_shape_transform(i); Basis shape_basis = shape_transform.basis.orthonormalized(); // NOTE: we don't take the scale of collision shapes into account when computing the inertia tensor! @@ -127,7 +132,7 @@ void Body3DSW::update_inertias() { _inv_inertia_tensor.set_zero(); _inv_mass = 0; } break; - case PhysicsServer3D::BODY_MODE_CHARACTER: { + case PhysicsServer3D::BODY_MODE_DYNAMIC_LOCKED: { _inv_inertia_tensor.set_zero(); _inv_mass = 1.0 / mass; @@ -234,13 +239,13 @@ void Body3DSW::set_mode(PhysicsServer3D::BodyMode p_mode) { } } break; - case PhysicsServer3D::BODY_MODE_RIGID: { + case PhysicsServer3D::BODY_MODE_DYNAMIC: { _inv_mass = mass > 0 ? (1.0 / mass) : 0; _set_static(false); set_active(true); } break; - case PhysicsServer3D::BODY_MODE_CHARACTER: { + case PhysicsServer3D::BODY_MODE_DYNAMIC_LOCKED: { _inv_mass = mass > 0 ? (1.0 / mass) : 0; _set_static(false); set_active(true); @@ -281,7 +286,7 @@ void Body3DSW::set_state(PhysicsServer3D::BodyState p_state, const Variant &p_va _set_inv_transform(get_transform().affine_inverse()); wakeup_neighbours(); } else { - Transform t = p_variant; + Transform3D t = p_variant; t.orthonormalize(); new_transform = get_transform(); //used as old to compute motion if (new_transform == t) { @@ -294,24 +299,15 @@ void Body3DSW::set_state(PhysicsServer3D::BodyState p_state, const Variant &p_va } break; case PhysicsServer3D::BODY_STATE_LINEAR_VELOCITY: { - /* - if (mode==PhysicsServer3D::BODY_MODE_STATIC) - break; - */ linear_velocity = p_variant; wakeup(); } break; case PhysicsServer3D::BODY_STATE_ANGULAR_VELOCITY: { - /* - if (mode!=PhysicsServer3D::BODY_MODE_RIGID) - break; - */ angular_velocity = p_variant; wakeup(); } break; case PhysicsServer3D::BODY_STATE_SLEEPING: { - //? if (mode == PhysicsServer3D::BODY_MODE_STATIC || mode == PhysicsServer3D::BODY_MODE_KINEMATIC) { break; } @@ -328,7 +324,7 @@ void Body3DSW::set_state(PhysicsServer3D::BodyState p_state, const Variant &p_va } break; case PhysicsServer3D::BODY_STATE_CAN_SLEEP: { can_sleep = p_variant; - if (mode == PhysicsServer3D::BODY_MODE_RIGID && !active && !can_sleep) { + if (mode == PhysicsServer3D::BODY_MODE_DYNAMIC && !active && !can_sleep) { set_active(true); } @@ -580,7 +576,7 @@ void Body3DSW::integrate_velocities(real_t p_step) { Vector3 total_angular_velocity = angular_velocity + biased_angular_velocity; real_t ang_vel = total_angular_velocity.length(); - Transform transform = get_transform(); + Transform3D transform = get_transform(); if (ang_vel != 0.0) { Vector3 ang_vel_axis = total_angular_velocity / ang_vel; @@ -612,8 +608,8 @@ void Body3DSW::integrate_velocities(real_t p_step) { } /* -void BodySW::simulate_motion(const Transform& p_xform,real_t p_step) { - Transform inv_xform = p_xform.affine_inverse(); +void BodySW::simulate_motion(const Transform3D& p_xform,real_t p_step) { + Transform3D inv_xform = p_xform.affine_inverse(); if (!get_space()) { _set_transform(p_xform); _set_inv_transform(inv_xform); @@ -654,7 +650,7 @@ void Body3DSW::wakeup_neighbours() { continue; } Body3DSW *b = n[i]; - if (b->mode != PhysicsServer3D::BODY_MODE_RIGID) { + if (b->mode != PhysicsServer3D::BODY_MODE_DYNAMIC) { continue; } @@ -688,9 +684,7 @@ void Body3DSW::call_queries() { bool Body3DSW::sleep_test(real_t p_step) { if (mode == PhysicsServer3D::BODY_MODE_STATIC || mode == PhysicsServer3D::BODY_MODE_KINEMATIC) { - return true; // - } else if (mode == PhysicsServer3D::BODY_MODE_CHARACTER) { - return !active; // characters don't sleep unless asked to sleep + return true; } else if (!can_sleep) { return false; } @@ -718,22 +712,16 @@ void Body3DSW::set_force_integration_callback(const Callable &p_callable, const } } -void Body3DSW::set_kinematic_margin(real_t p_margin) { - kinematic_safe_margin = p_margin; -} - Body3DSW::Body3DSW() : CollisionObject3DSW(TYPE_BODY), active_list(this), inertia_update_list(this), direct_state_query_list(this) { - mode = PhysicsServer3D::BODY_MODE_RIGID; + mode = PhysicsServer3D::BODY_MODE_DYNAMIC; active = true; mass = 1; - kinematic_safe_margin = 0.001; - //_inv_inertia=Transform(); _inv_mass = 1; bounce = 0; friction = 1; diff --git a/servers/physics_3d/body_3d_sw.h b/servers/physics_3d/body_3d_sw.h index 9afb8cd56f..0fa31c5037 100644 --- a/servers/physics_3d/body_3d_sw.h +++ b/servers/physics_3d/body_3d_sw.h @@ -55,7 +55,6 @@ class Body3DSW : public CollisionObject3DSW { uint16_t locked_axis = 0; - real_t kinematic_safe_margin; real_t _inv_mass; Vector3 _inv_inertia; // Relative to the principal axes of inertia @@ -93,7 +92,7 @@ class Body3DSW : public CollisionObject3DSW { bool first_time_kinematic; void _update_inertia(); virtual void _shapes_changed(); - Transform new_transform; + Transform3D new_transform; Map<Constraint3DSW *, int> constraint_map; @@ -144,9 +143,6 @@ class Body3DSW : public CollisionObject3DSW { public: void set_force_integration_callback(const Callable &p_callable, const Variant &p_udata = Variant()); - void set_kinematic_margin(real_t p_margin); - _FORCE_INLINE_ real_t get_kinematic_margin() { return kinematic_safe_margin; } - _FORCE_INLINE_ void add_area(Area3DSW *p_area) { int index = areas.find(AreaCMP(p_area)); if (index > -1) { @@ -311,7 +307,7 @@ public: return p_axis.dot(_inv_inertia_tensor.xform_inv(p_axis)); } - //void simulate_motion(const Transform& p_xform,real_t p_step); + //void simulate_motion(const Transform3D& p_xform,real_t p_step); void call_queries(); void wakeup_neighbours(); @@ -390,8 +386,8 @@ public: virtual void set_angular_velocity(const Vector3 &p_velocity) override { body->set_angular_velocity(p_velocity); } virtual Vector3 get_angular_velocity() const override { return body->get_angular_velocity(); } - virtual void set_transform(const Transform &p_transform) override { body->set_state(PhysicsServer3D::BODY_STATE_TRANSFORM, p_transform); } - virtual Transform get_transform() const override { return body->get_transform(); } + virtual void set_transform(const Transform3D &p_transform) override { body->set_state(PhysicsServer3D::BODY_STATE_TRANSFORM, p_transform); } + virtual Transform3D get_transform() const override { return body->get_transform(); } virtual void add_central_force(const Vector3 &p_force) override { body->add_central_force(p_force); } virtual void add_force(const Vector3 &p_force, const Vector3 &p_position = Vector3()) override { diff --git a/servers/physics_3d/body_pair_3d_sw.cpp b/servers/physics_3d/body_pair_3d_sw.cpp index cdb3da665e..aed4815c5e 100644 --- a/servers/physics_3d/body_pair_3d_sw.cpp +++ b/servers/physics_3d/body_pair_3d_sw.cpp @@ -161,7 +161,7 @@ void BodyPair3DSW::validate_contacts() { } } -bool BodyPair3DSW::_test_ccd(real_t p_step, Body3DSW *p_A, int p_shape_A, const Transform &p_xform_A, Body3DSW *p_B, int p_shape_B, const Transform &p_xform_B) { +bool BodyPair3DSW::_test_ccd(real_t p_step, Body3DSW *p_A, int p_shape_A, const Transform3D &p_xform_A, Body3DSW *p_B, int p_shape_B, const Transform3D &p_xform_B) { Vector3 motion = p_A->get_linear_velocity() * p_step; real_t mlen = motion.length(); if (mlen < CMP_EPSILON) { @@ -184,7 +184,7 @@ bool BodyPair3DSW::_test_ccd(real_t p_step, Body3DSW *p_A, int p_shape_A, const Vector3 from = p_xform_A.xform(s); Vector3 to = from + motion; - Transform from_inv = p_xform_B.affine_inverse(); + Transform3D from_inv = p_xform_B.affine_inverse(); Vector3 local_from = from_inv.xform(from - mnormal * mlen * 0.1); //start from a little inside the bounding box Vector3 local_to = from_inv.xform(to); @@ -240,12 +240,12 @@ bool BodyPair3DSW::setup(real_t p_step) { validate_contacts(); const Vector3 &offset_A = A->get_transform().get_origin(); - Transform xform_Au = Transform(A->get_transform().basis, Vector3()); - Transform xform_A = xform_Au * A->get_shape_transform(shape_A); + Transform3D xform_Au = Transform3D(A->get_transform().basis, Vector3()); + Transform3D xform_A = xform_Au * A->get_shape_transform(shape_A); - Transform xform_Bu = B->get_transform(); + Transform3D xform_Bu = B->get_transform(); xform_Bu.origin -= offset_A; - Transform xform_B = xform_Bu * B->get_shape_transform(shape_B); + Transform3D xform_B = xform_Bu * B->get_shape_transform(shape_B); Shape3DSW *shape_A_ptr = A->get_shape(shape_A); Shape3DSW *shape_B_ptr = B->get_shape(shape_B); @@ -571,7 +571,7 @@ void BodySoftBodyPair3DSW::contact_added_callback(const Vector3 &p_point_A, int void BodySoftBodyPair3DSW::validate_contacts() { // Make sure to erase contacts that are no longer valid. - const Transform &transform_A = body->get_transform(); + const Transform3D &transform_A = body->get_transform(); real_t contact_max_separation = space->get_contact_max_separation(); @@ -612,11 +612,11 @@ bool BodySoftBodyPair3DSW::setup(real_t p_step) { return false; } - const Transform &xform_Au = body->get_transform(); - Transform xform_A = xform_Au * body->get_shape_transform(body_shape); + const Transform3D &xform_Au = body->get_transform(); + Transform3D xform_A = xform_Au * body->get_shape_transform(body_shape); - Transform xform_Bu = soft_body->get_transform(); - Transform xform_B = xform_Bu * soft_body->get_shape_transform(0); + Transform3D xform_Bu = soft_body->get_transform(); + Transform3D xform_B = xform_Bu * soft_body->get_shape_transform(0); validate_contacts(); @@ -647,7 +647,7 @@ bool BodySoftBodyPair3DSW::pre_solve(real_t p_step) { bool do_process = false; - const Transform &transform_A = body->get_transform(); + const Transform3D &transform_A = body->get_transform(); uint32_t contact_count = contacts.size(); for (uint32_t contact_index = 0; contact_index < contact_count; ++contact_index) { diff --git a/servers/physics_3d/body_pair_3d_sw.h b/servers/physics_3d/body_pair_3d_sw.h index 3f425ba2d7..976982d1f1 100644 --- a/servers/physics_3d/body_pair_3d_sw.h +++ b/servers/physics_3d/body_pair_3d_sw.h @@ -98,7 +98,7 @@ class BodyPair3DSW : public BodyContact3DSW { void contact_added_callback(const Vector3 &p_point_A, int p_index_A, const Vector3 &p_point_B, int p_index_B); void validate_contacts(); - bool _test_ccd(real_t p_step, Body3DSW *p_A, int p_shape_A, const Transform &p_xform_A, Body3DSW *p_B, int p_shape_B, const Transform &p_xform_B); + bool _test_ccd(real_t p_step, Body3DSW *p_A, int p_shape_A, const Transform3D &p_xform_A, Body3DSW *p_B, int p_shape_B, const Transform3D &p_xform_B); public: virtual bool setup(real_t p_step) override; diff --git a/servers/physics_3d/collision_object_3d_sw.cpp b/servers/physics_3d/collision_object_3d_sw.cpp index 459deb1356..51e2432071 100644 --- a/servers/physics_3d/collision_object_3d_sw.cpp +++ b/servers/physics_3d/collision_object_3d_sw.cpp @@ -32,7 +32,7 @@ #include "servers/physics_3d/physics_server_3d_sw.h" #include "space_3d_sw.h" -void CollisionObject3DSW::add_shape(Shape3DSW *p_shape, const Transform &p_transform, bool p_disabled) { +void CollisionObject3DSW::add_shape(Shape3DSW *p_shape, const Transform3D &p_transform, bool p_disabled) { Shape s; s.shape = p_shape; s.xform = p_transform; @@ -62,7 +62,7 @@ void CollisionObject3DSW::set_shape(int p_index, Shape3DSW *p_shape) { //_shapes_changed(); } -void CollisionObject3DSW::set_shape_transform(int p_index, const Transform &p_transform) { +void CollisionObject3DSW::set_shape_transform(int p_index, const Transform3D &p_transform) { ERR_FAIL_INDEX(p_index, shapes.size()); shapes.write[p_index].xform = p_transform; @@ -149,7 +149,7 @@ void CollisionObject3DSW::_update_shapes() { //not quite correct, should compute the next matrix.. AABB shape_aabb = s.shape->get_aabb(); - Transform xform = transform * s.xform; + Transform3D xform = transform * s.xform; shape_aabb = xform.xform(shape_aabb); shape_aabb.grow_by((s.aabb_cache.size.x + s.aabb_cache.size.y) * 0.5 * 0.05); s.aabb_cache = shape_aabb; @@ -176,7 +176,7 @@ void CollisionObject3DSW::_update_shapes_with_motion(const Vector3 &p_motion) { //not quite correct, should compute the next matrix.. AABB shape_aabb = s.shape->get_aabb(); - Transform xform = transform * s.xform; + Transform3D xform = transform * s.xform; shape_aabb = xform.xform(shape_aabb); shape_aabb.merge_with(AABB(shape_aabb.position + p_motion, shape_aabb.size)); //use motion s.aabb_cache = shape_aabb; diff --git a/servers/physics_3d/collision_object_3d_sw.h b/servers/physics_3d/collision_object_3d_sw.h index 85221b7746..5505fec3da 100644 --- a/servers/physics_3d/collision_object_3d_sw.h +++ b/servers/physics_3d/collision_object_3d_sw.h @@ -60,8 +60,8 @@ private: uint32_t collision_mask; struct Shape { - Transform xform; - Transform xform_inv; + Transform3D xform; + Transform3D xform_inv; BroadPhase3DSW::ID bpid; AABB aabb_cache; //for rayqueries real_t area_cache; @@ -73,8 +73,8 @@ private: Vector<Shape> shapes; Space3DSW *space; - Transform transform; - Transform inv_transform; + Transform3D transform; + Transform3D inv_transform; bool _static; SelfList<CollisionObject3DSW> pending_shape_update_list; @@ -85,7 +85,7 @@ protected: void _update_shapes_with_motion(const Vector3 &p_motion); void _unregister_shapes(); - _FORCE_INLINE_ void _set_transform(const Transform &p_transform, bool p_update_shapes = true) { + _FORCE_INLINE_ void _set_transform(const Transform3D &p_transform, bool p_update_shapes = true) { #ifdef DEBUG_ENABLED ERR_FAIL_COND_MSG(p_transform.origin.length_squared() > MAX_OBJECT_DISTANCE_X2, "Object went too far away (more than '" + itos(MAX_OBJECT_DISTANCE) + "' units from origin)."); @@ -96,7 +96,7 @@ protected: _update_shapes(); } } - _FORCE_INLINE_ void _set_inv_transform(const Transform &p_transform) { inv_transform = p_transform; } + _FORCE_INLINE_ void _set_inv_transform(const Transform3D &p_transform) { inv_transform = p_transform; } void _set_static(bool p_static); virtual void _shapes_changed() = 0; @@ -116,22 +116,22 @@ public: void _shape_changed(); _FORCE_INLINE_ Type get_type() const { return type; } - void add_shape(Shape3DSW *p_shape, const Transform &p_transform = Transform(), bool p_disabled = false); + void add_shape(Shape3DSW *p_shape, const Transform3D &p_transform = Transform3D(), bool p_disabled = false); void set_shape(int p_index, Shape3DSW *p_shape); - void set_shape_transform(int p_index, const Transform &p_transform); + void set_shape_transform(int p_index, const Transform3D &p_transform); _FORCE_INLINE_ int get_shape_count() const { return shapes.size(); } _FORCE_INLINE_ bool is_shape_disabled(int p_index) const { CRASH_BAD_INDEX(p_index, shapes.size()); return shapes[p_index].disabled; } _FORCE_INLINE_ Shape3DSW *get_shape(int p_index) const { return shapes[p_index].shape; } - _FORCE_INLINE_ const Transform &get_shape_transform(int p_index) const { return shapes[p_index].xform; } - _FORCE_INLINE_ const Transform &get_shape_inv_transform(int p_index) const { return shapes[p_index].xform_inv; } + _FORCE_INLINE_ const Transform3D &get_shape_transform(int p_index) const { return shapes[p_index].xform; } + _FORCE_INLINE_ const Transform3D &get_shape_inv_transform(int p_index) const { return shapes[p_index].xform_inv; } _FORCE_INLINE_ const AABB &get_shape_aabb(int p_index) const { return shapes[p_index].aabb_cache; } _FORCE_INLINE_ real_t get_shape_area(int p_index) const { return shapes[p_index].area_cache; } - _FORCE_INLINE_ const Transform &get_transform() const { return transform; } - _FORCE_INLINE_ const Transform &get_inv_transform() const { return inv_transform; } + _FORCE_INLINE_ const Transform3D &get_transform() const { return transform; } + _FORCE_INLINE_ const Transform3D &get_inv_transform() const { return inv_transform; } _FORCE_INLINE_ Space3DSW *get_space() const { return space; } _FORCE_INLINE_ void set_ray_pickable(bool p_enable) { ray_pickable = p_enable; } diff --git a/servers/physics_3d/collision_solver_3d_sat.cpp b/servers/physics_3d/collision_solver_3d_sat.cpp index fcac0587b2..b362f1ff17 100644 --- a/servers/physics_3d/collision_solver_3d_sat.cpp +++ b/servers/physics_3d/collision_solver_3d_sat.cpp @@ -608,8 +608,8 @@ template <class ShapeA, class ShapeB, bool withMargin = false> class SeparatorAxisTest { const ShapeA *shape_A; const ShapeB *shape_B; - const Transform *transform_A; - const Transform *transform_B; + const Transform3D *transform_A; + const Transform3D *transform_B; real_t best_depth; Vector3 best_axis; _CollectorCallback *callback; @@ -750,7 +750,7 @@ public: callback->collided = true; } - _FORCE_INLINE_ SeparatorAxisTest(const ShapeA *p_shape_A, const Transform &p_transform_A, const ShapeB *p_shape_B, const Transform &p_transform_B, _CollectorCallback *p_callback, real_t p_margin_A = 0, real_t p_margin_B = 0) { + _FORCE_INLINE_ SeparatorAxisTest(const ShapeA *p_shape_A, const Transform3D &p_transform_A, const ShapeB *p_shape_B, const Transform3D &p_transform_B, _CollectorCallback *p_callback, real_t p_margin_A = 0, real_t p_margin_B = 0) { best_depth = 1e15; shape_A = p_shape_A; shape_B = p_shape_B; @@ -764,10 +764,10 @@ public: /****** SAT TESTS *******/ -typedef void (*CollisionFunc)(const Shape3DSW *, const Transform &, const Shape3DSW *, const Transform &, _CollectorCallback *p_callback, real_t, real_t); +typedef void (*CollisionFunc)(const Shape3DSW *, const Transform3D &, const Shape3DSW *, const Transform3D &, _CollectorCallback *p_callback, real_t, real_t); template <bool withMargin> -static void _collision_sphere_sphere(const Shape3DSW *p_a, const Transform &p_transform_a, const Shape3DSW *p_b, const Transform &p_transform_b, _CollectorCallback *p_collector, real_t p_margin_a, real_t p_margin_b) { +static void _collision_sphere_sphere(const Shape3DSW *p_a, const Transform3D &p_transform_a, const Shape3DSW *p_b, const Transform3D &p_transform_b, _CollectorCallback *p_collector, real_t p_margin_a, real_t p_margin_b) { const SphereShape3DSW *sphere_A = static_cast<const SphereShape3DSW *>(p_a); const SphereShape3DSW *sphere_B = static_cast<const SphereShape3DSW *>(p_b); @@ -787,7 +787,7 @@ static void _collision_sphere_sphere(const Shape3DSW *p_a, const Transform &p_tr } template <bool withMargin> -static void _collision_sphere_box(const Shape3DSW *p_a, const Transform &p_transform_a, const Shape3DSW *p_b, const Transform &p_transform_b, _CollectorCallback *p_collector, real_t p_margin_a, real_t p_margin_b) { +static void _collision_sphere_box(const Shape3DSW *p_a, const Transform3D &p_transform_a, const Shape3DSW *p_b, const Transform3D &p_transform_b, _CollectorCallback *p_collector, real_t p_margin_a, real_t p_margin_b) { const SphereShape3DSW *sphere_A = static_cast<const SphereShape3DSW *>(p_a); const BoxShape3DSW *box_B = static_cast<const BoxShape3DSW *>(p_b); @@ -838,7 +838,7 @@ static void _collision_sphere_box(const Shape3DSW *p_a, const Transform &p_trans } template <bool withMargin> -static void _collision_sphere_capsule(const Shape3DSW *p_a, const Transform &p_transform_a, const Shape3DSW *p_b, const Transform &p_transform_b, _CollectorCallback *p_collector, real_t p_margin_a, real_t p_margin_b) { +static void _collision_sphere_capsule(const Shape3DSW *p_a, const Transform3D &p_transform_a, const Shape3DSW *p_b, const Transform3D &p_transform_b, _CollectorCallback *p_collector, real_t p_margin_a, real_t p_margin_b) { const SphereShape3DSW *sphere_A = static_cast<const SphereShape3DSW *>(p_a); const CapsuleShape3DSW *capsule_B = static_cast<const CapsuleShape3DSW *>(p_b); @@ -880,7 +880,7 @@ static void _collision_sphere_capsule(const Shape3DSW *p_a, const Transform &p_t } template <bool withMargin> -static void _collision_sphere_cylinder(const Shape3DSW *p_a, const Transform &p_transform_a, const Shape3DSW *p_b, const Transform &p_transform_b, _CollectorCallback *p_collector, real_t p_margin_a, real_t p_margin_b) { +static void _collision_sphere_cylinder(const Shape3DSW *p_a, const Transform3D &p_transform_a, const Shape3DSW *p_b, const Transform3D &p_transform_b, _CollectorCallback *p_collector, real_t p_margin_a, real_t p_margin_b) { const SphereShape3DSW *sphere_A = static_cast<const SphereShape3DSW *>(p_a); const CylinderShape3DSW *cylinder_B = static_cast<const CylinderShape3DSW *>(p_b); @@ -939,7 +939,7 @@ static void _collision_sphere_cylinder(const Shape3DSW *p_a, const Transform &p_ } template <bool withMargin> -static void _collision_sphere_convex_polygon(const Shape3DSW *p_a, const Transform &p_transform_a, const Shape3DSW *p_b, const Transform &p_transform_b, _CollectorCallback *p_collector, real_t p_margin_a, real_t p_margin_b) { +static void _collision_sphere_convex_polygon(const Shape3DSW *p_a, const Transform3D &p_transform_a, const Shape3DSW *p_b, const Transform3D &p_transform_b, _CollectorCallback *p_collector, real_t p_margin_a, real_t p_margin_b) { const SphereShape3DSW *sphere_A = static_cast<const SphereShape3DSW *>(p_a); const ConvexPolygonShape3DSW *convex_polygon_B = static_cast<const ConvexPolygonShape3DSW *>(p_b); @@ -999,7 +999,7 @@ static void _collision_sphere_convex_polygon(const Shape3DSW *p_a, const Transfo } template <bool withMargin> -static void _collision_sphere_face(const Shape3DSW *p_a, const Transform &p_transform_a, const Shape3DSW *p_b, const Transform &p_transform_b, _CollectorCallback *p_collector, real_t p_margin_a, real_t p_margin_b) { +static void _collision_sphere_face(const Shape3DSW *p_a, const Transform3D &p_transform_a, const Shape3DSW *p_b, const Transform3D &p_transform_b, _CollectorCallback *p_collector, real_t p_margin_a, real_t p_margin_b) { const SphereShape3DSW *sphere_A = static_cast<const SphereShape3DSW *>(p_a); const FaceShape3DSW *face_B = static_cast<const FaceShape3DSW *>(p_b); @@ -1044,7 +1044,7 @@ static void _collision_sphere_face(const Shape3DSW *p_a, const Transform &p_tran } template <bool withMargin> -static void _collision_box_box(const Shape3DSW *p_a, const Transform &p_transform_a, const Shape3DSW *p_b, const Transform &p_transform_b, _CollectorCallback *p_collector, real_t p_margin_a, real_t p_margin_b) { +static void _collision_box_box(const Shape3DSW *p_a, const Transform3D &p_transform_a, const Shape3DSW *p_b, const Transform3D &p_transform_b, _CollectorCallback *p_collector, real_t p_margin_a, real_t p_margin_b) { const BoxShape3DSW *box_A = static_cast<const BoxShape3DSW *>(p_a); const BoxShape3DSW *box_B = static_cast<const BoxShape3DSW *>(p_b); @@ -1142,7 +1142,7 @@ static void _collision_box_box(const Shape3DSW *p_a, const Transform &p_transfor } template <bool withMargin> -static void _collision_box_capsule(const Shape3DSW *p_a, const Transform &p_transform_a, const Shape3DSW *p_b, const Transform &p_transform_b, _CollectorCallback *p_collector, real_t p_margin_a, real_t p_margin_b) { +static void _collision_box_capsule(const Shape3DSW *p_a, const Transform3D &p_transform_a, const Shape3DSW *p_b, const Transform3D &p_transform_b, _CollectorCallback *p_collector, real_t p_margin_a, real_t p_margin_b) { const BoxShape3DSW *box_A = static_cast<const BoxShape3DSW *>(p_a); const CapsuleShape3DSW *capsule_B = static_cast<const CapsuleShape3DSW *>(p_b); @@ -1240,7 +1240,7 @@ static void _collision_box_capsule(const Shape3DSW *p_a, const Transform &p_tran } template <bool withMargin> -static void _collision_box_cylinder(const Shape3DSW *p_a, const Transform &p_transform_a, const Shape3DSW *p_b, const Transform &p_transform_b, _CollectorCallback *p_collector, real_t p_margin_a, real_t p_margin_b) { +static void _collision_box_cylinder(const Shape3DSW *p_a, const Transform3D &p_transform_a, const Shape3DSW *p_b, const Transform3D &p_transform_b, _CollectorCallback *p_collector, real_t p_margin_a, real_t p_margin_b) { const BoxShape3DSW *box_A = static_cast<const BoxShape3DSW *>(p_a); const CylinderShape3DSW *cylinder_B = static_cast<const CylinderShape3DSW *>(p_b); @@ -1353,7 +1353,7 @@ static void _collision_box_cylinder(const Shape3DSW *p_a, const Transform &p_tra } template <bool withMargin> -static void _collision_box_convex_polygon(const Shape3DSW *p_a, const Transform &p_transform_a, const Shape3DSW *p_b, const Transform &p_transform_b, _CollectorCallback *p_collector, real_t p_margin_a, real_t p_margin_b) { +static void _collision_box_convex_polygon(const Shape3DSW *p_a, const Transform3D &p_transform_a, const Shape3DSW *p_b, const Transform3D &p_transform_b, _CollectorCallback *p_collector, real_t p_margin_a, real_t p_margin_b) { const BoxShape3DSW *box_A = static_cast<const BoxShape3DSW *>(p_a); const ConvexPolygonShape3DSW *convex_polygon_B = static_cast<const ConvexPolygonShape3DSW *>(p_b); @@ -1468,7 +1468,7 @@ static void _collision_box_convex_polygon(const Shape3DSW *p_a, const Transform } template <bool withMargin> -static void _collision_box_face(const Shape3DSW *p_a, const Transform &p_transform_a, const Shape3DSW *p_b, const Transform &p_transform_b, _CollectorCallback *p_collector, real_t p_margin_a, real_t p_margin_b) { +static void _collision_box_face(const Shape3DSW *p_a, const Transform3D &p_transform_a, const Shape3DSW *p_b, const Transform3D &p_transform_b, _CollectorCallback *p_collector, real_t p_margin_a, real_t p_margin_b) { const BoxShape3DSW *box_A = static_cast<const BoxShape3DSW *>(p_a); const FaceShape3DSW *face_B = static_cast<const FaceShape3DSW *>(p_b); @@ -1591,7 +1591,7 @@ static void _collision_box_face(const Shape3DSW *p_a, const Transform &p_transfo } template <bool withMargin> -static void _collision_capsule_capsule(const Shape3DSW *p_a, const Transform &p_transform_a, const Shape3DSW *p_b, const Transform &p_transform_b, _CollectorCallback *p_collector, real_t p_margin_a, real_t p_margin_b) { +static void _collision_capsule_capsule(const Shape3DSW *p_a, const Transform3D &p_transform_a, const Shape3DSW *p_b, const Transform3D &p_transform_b, _CollectorCallback *p_collector, real_t p_margin_a, real_t p_margin_b) { const CapsuleShape3DSW *capsule_A = static_cast<const CapsuleShape3DSW *>(p_a); const CapsuleShape3DSW *capsule_B = static_cast<const CapsuleShape3DSW *>(p_b); @@ -1655,7 +1655,7 @@ static void _collision_capsule_capsule(const Shape3DSW *p_a, const Transform &p_ } template <bool withMargin> -static void _collision_capsule_cylinder(const Shape3DSW *p_a, const Transform &p_transform_a, const Shape3DSW *p_b, const Transform &p_transform_b, _CollectorCallback *p_collector, real_t p_margin_a, real_t p_margin_b) { +static void _collision_capsule_cylinder(const Shape3DSW *p_a, const Transform3D &p_transform_a, const Shape3DSW *p_b, const Transform3D &p_transform_b, _CollectorCallback *p_collector, real_t p_margin_a, real_t p_margin_b) { const CapsuleShape3DSW *capsule_A = static_cast<const CapsuleShape3DSW *>(p_a); const CylinderShape3DSW *cylinder_B = static_cast<const CylinderShape3DSW *>(p_b); @@ -1717,7 +1717,7 @@ static void _collision_capsule_cylinder(const Shape3DSW *p_a, const Transform &p } template <bool withMargin> -static void _collision_capsule_convex_polygon(const Shape3DSW *p_a, const Transform &p_transform_a, const Shape3DSW *p_b, const Transform &p_transform_b, _CollectorCallback *p_collector, real_t p_margin_a, real_t p_margin_b) { +static void _collision_capsule_convex_polygon(const Shape3DSW *p_a, const Transform3D &p_transform_a, const Shape3DSW *p_b, const Transform3D &p_transform_b, _CollectorCallback *p_collector, real_t p_margin_a, real_t p_margin_b) { const CapsuleShape3DSW *capsule_A = static_cast<const CapsuleShape3DSW *>(p_a); const ConvexPolygonShape3DSW *convex_polygon_B = static_cast<const ConvexPolygonShape3DSW *>(p_b); @@ -1781,7 +1781,7 @@ static void _collision_capsule_convex_polygon(const Shape3DSW *p_a, const Transf } template <bool withMargin> -static void _collision_capsule_face(const Shape3DSW *p_a, const Transform &p_transform_a, const Shape3DSW *p_b, const Transform &p_transform_b, _CollectorCallback *p_collector, real_t p_margin_a, real_t p_margin_b) { +static void _collision_capsule_face(const Shape3DSW *p_a, const Transform3D &p_transform_a, const Shape3DSW *p_b, const Transform3D &p_transform_b, _CollectorCallback *p_collector, real_t p_margin_a, real_t p_margin_b) { const CapsuleShape3DSW *capsule_A = static_cast<const CapsuleShape3DSW *>(p_a); const FaceShape3DSW *face_B = static_cast<const FaceShape3DSW *>(p_b); @@ -1855,7 +1855,7 @@ static void _collision_capsule_face(const Shape3DSW *p_a, const Transform &p_tra } template <bool withMargin> -static void _collision_cylinder_cylinder(const Shape3DSW *p_a, const Transform &p_transform_a, const Shape3DSW *p_b, const Transform &p_transform_b, _CollectorCallback *p_collector, real_t p_margin_a, real_t p_margin_b) { +static void _collision_cylinder_cylinder(const Shape3DSW *p_a, const Transform3D &p_transform_a, const Shape3DSW *p_b, const Transform3D &p_transform_b, _CollectorCallback *p_collector, real_t p_margin_a, real_t p_margin_b) { const CylinderShape3DSW *cylinder_A = static_cast<const CylinderShape3DSW *>(p_a); const CylinderShape3DSW *cylinder_B = static_cast<const CylinderShape3DSW *>(p_b); @@ -1909,7 +1909,7 @@ static void _collision_cylinder_cylinder(const Shape3DSW *p_a, const Transform & } template <bool withMargin> -static void _collision_cylinder_convex_polygon(const Shape3DSW *p_a, const Transform &p_transform_a, const Shape3DSW *p_b, const Transform &p_transform_b, _CollectorCallback *p_collector, real_t p_margin_a, real_t p_margin_b) { +static void _collision_cylinder_convex_polygon(const Shape3DSW *p_a, const Transform3D &p_transform_a, const Shape3DSW *p_b, const Transform3D &p_transform_b, _CollectorCallback *p_collector, real_t p_margin_a, real_t p_margin_b) { const CylinderShape3DSW *cylinder_A = static_cast<const CylinderShape3DSW *>(p_a); const ConvexPolygonShape3DSW *convex_polygon_B = static_cast<const ConvexPolygonShape3DSW *>(p_b); @@ -1926,7 +1926,7 @@ static void _collision_cylinder_convex_polygon(const Shape3DSW *p_a, const Trans } template <bool withMargin> -static void _collision_cylinder_face(const Shape3DSW *p_a, const Transform &p_transform_a, const Shape3DSW *p_b, const Transform &p_transform_b, _CollectorCallback *p_collector, real_t p_margin_a, real_t p_margin_b) { +static void _collision_cylinder_face(const Shape3DSW *p_a, const Transform3D &p_transform_a, const Shape3DSW *p_b, const Transform3D &p_transform_b, _CollectorCallback *p_collector, real_t p_margin_a, real_t p_margin_b) { const CylinderShape3DSW *cylinder_A = static_cast<const CylinderShape3DSW *>(p_a); const FaceShape3DSW *face_B = static_cast<const FaceShape3DSW *>(p_b); @@ -2031,7 +2031,7 @@ static void _collision_cylinder_face(const Shape3DSW *p_a, const Transform &p_tr } template <bool withMargin> -static void _collision_convex_polygon_convex_polygon(const Shape3DSW *p_a, const Transform &p_transform_a, const Shape3DSW *p_b, const Transform &p_transform_b, _CollectorCallback *p_collector, real_t p_margin_a, real_t p_margin_b) { +static void _collision_convex_polygon_convex_polygon(const Shape3DSW *p_a, const Transform3D &p_transform_a, const Shape3DSW *p_b, const Transform3D &p_transform_b, _CollectorCallback *p_collector, real_t p_margin_a, real_t p_margin_b) { const ConvexPolygonShape3DSW *convex_polygon_A = static_cast<const ConvexPolygonShape3DSW *>(p_a); const ConvexPolygonShape3DSW *convex_polygon_B = static_cast<const ConvexPolygonShape3DSW *>(p_b); @@ -2140,7 +2140,7 @@ static void _collision_convex_polygon_convex_polygon(const Shape3DSW *p_a, const } template <bool withMargin> -static void _collision_convex_polygon_face(const Shape3DSW *p_a, const Transform &p_transform_a, const Shape3DSW *p_b, const Transform &p_transform_b, _CollectorCallback *p_collector, real_t p_margin_a, real_t p_margin_b) { +static void _collision_convex_polygon_face(const Shape3DSW *p_a, const Transform3D &p_transform_a, const Shape3DSW *p_b, const Transform3D &p_transform_b, _CollectorCallback *p_collector, real_t p_margin_a, real_t p_margin_b) { const ConvexPolygonShape3DSW *convex_polygon_A = static_cast<const ConvexPolygonShape3DSW *>(p_a); const FaceShape3DSW *face_B = static_cast<const FaceShape3DSW *>(p_b); @@ -2258,7 +2258,7 @@ static void _collision_convex_polygon_face(const Shape3DSW *p_a, const Transform separator.generate_contacts(); } -bool sat_calculate_penetration(const Shape3DSW *p_shape_A, const Transform &p_transform_A, const Shape3DSW *p_shape_B, const Transform &p_transform_B, CollisionSolver3DSW::CallbackResult p_result_callback, void *p_userdata, bool p_swap, Vector3 *r_prev_axis, real_t p_margin_a, real_t p_margin_b) { +bool sat_calculate_penetration(const Shape3DSW *p_shape_A, const Transform3D &p_transform_A, const Shape3DSW *p_shape_B, const Transform3D &p_transform_B, CollisionSolver3DSW::CallbackResult p_result_callback, void *p_userdata, bool p_swap, Vector3 *r_prev_axis, real_t p_margin_a, real_t p_margin_b) { PhysicsServer3D::ShapeType type_A = p_shape_A->get_type(); ERR_FAIL_COND_V(type_A == PhysicsServer3D::SHAPE_PLANE, false); @@ -2358,8 +2358,8 @@ bool sat_calculate_penetration(const Shape3DSW *p_shape_A, const Transform &p_tr const Shape3DSW *A = p_shape_A; const Shape3DSW *B = p_shape_B; - const Transform *transform_A = &p_transform_A; - const Transform *transform_B = &p_transform_B; + const Transform3D *transform_A = &p_transform_A; + const Transform3D *transform_B = &p_transform_B; real_t margin_A = p_margin_a; real_t margin_B = p_margin_b; diff --git a/servers/physics_3d/collision_solver_3d_sat.h b/servers/physics_3d/collision_solver_3d_sat.h index 97454c0b4a..e50da7b101 100644 --- a/servers/physics_3d/collision_solver_3d_sat.h +++ b/servers/physics_3d/collision_solver_3d_sat.h @@ -33,6 +33,6 @@ #include "collision_solver_3d_sw.h" -bool sat_calculate_penetration(const Shape3DSW *p_shape_A, const Transform &p_transform_A, const Shape3DSW *p_shape_B, const Transform &p_transform_B, CollisionSolver3DSW::CallbackResult p_result_callback, void *p_userdata, bool p_swap = false, Vector3 *r_prev_axis = nullptr, real_t p_margin_a = 0, real_t p_margin_b = 0); +bool sat_calculate_penetration(const Shape3DSW *p_shape_A, const Transform3D &p_transform_A, const Shape3DSW *p_shape_B, const Transform3D &p_transform_B, CollisionSolver3DSW::CallbackResult p_result_callback, void *p_userdata, bool p_swap = false, Vector3 *r_prev_axis = nullptr, real_t p_margin_a = 0, real_t p_margin_b = 0); #endif // COLLISION_SOLVER_SAT_H diff --git a/servers/physics_3d/collision_solver_3d_sw.cpp b/servers/physics_3d/collision_solver_3d_sw.cpp index f655c4626c..67330d497e 100644 --- a/servers/physics_3d/collision_solver_3d_sw.cpp +++ b/servers/physics_3d/collision_solver_3d_sw.cpp @@ -37,7 +37,7 @@ #define collision_solver sat_calculate_penetration //#define collision_solver gjk_epa_calculate_penetration -bool CollisionSolver3DSW::solve_static_plane(const Shape3DSW *p_shape_A, const Transform &p_transform_A, const Shape3DSW *p_shape_B, const Transform &p_transform_B, CallbackResult p_result_callback, void *p_userdata, bool p_swap_result) { +bool CollisionSolver3DSW::solve_static_plane(const Shape3DSW *p_shape_A, const Transform3D &p_transform_A, const Shape3DSW *p_shape_B, const Transform3D &p_transform_B, CallbackResult p_result_callback, void *p_userdata, bool p_swap_result) { const PlaneShape3DSW *plane = static_cast<const PlaneShape3DSW *>(p_shape_A); if (p_shape_B->get_type() == PhysicsServer3D::SHAPE_PLANE) { return false; @@ -89,14 +89,14 @@ bool CollisionSolver3DSW::solve_static_plane(const Shape3DSW *p_shape_A, const T return found; } -bool CollisionSolver3DSW::solve_ray(const Shape3DSW *p_shape_A, const Transform &p_transform_A, const Shape3DSW *p_shape_B, const Transform &p_transform_B, CallbackResult p_result_callback, void *p_userdata, bool p_swap_result) { +bool CollisionSolver3DSW::solve_ray(const Shape3DSW *p_shape_A, const Transform3D &p_transform_A, const Shape3DSW *p_shape_B, const Transform3D &p_transform_B, CallbackResult p_result_callback, void *p_userdata, bool p_swap_result) { const RayShape3DSW *ray = static_cast<const RayShape3DSW *>(p_shape_A); Vector3 from = p_transform_A.origin; Vector3 to = from + p_transform_A.basis.get_axis(2) * ray->get_length(); Vector3 support_A = to; - Transform ai = p_transform_B.affine_inverse(); + Transform3D ai = p_transform_B.affine_inverse(); from = ai.xform(from); to = ai.xform(to); @@ -146,8 +146,8 @@ struct _SoftBodyQueryInfo { SoftBody3DSW *soft_body = nullptr; const Shape3DSW *shape_A = nullptr; const Shape3DSW *shape_B = nullptr; - Transform transform_A; - Transform node_transform; + Transform3D transform_A; + Transform3D node_transform; _SoftBodyContactCollisionInfo contact_info; #ifdef DEBUG_ENABLED int node_query_count = 0; @@ -160,7 +160,7 @@ bool CollisionSolver3DSW::soft_body_query_callback(uint32_t p_node_index, void * Vector3 node_position = query_cinfo.soft_body->get_node_position(p_node_index); - Transform transform_B; + Transform3D transform_B; transform_B.origin = query_cinfo.node_transform.xform(node_position); query_cinfo.contact_info.node_index = p_node_index; @@ -201,11 +201,11 @@ void CollisionSolver3DSW::soft_body_concave_callback(void *p_userdata, Shape3DSW #endif } -bool CollisionSolver3DSW::solve_soft_body(const Shape3DSW *p_shape_A, const Transform &p_transform_A, const Shape3DSW *p_shape_B, const Transform &p_transform_B, CallbackResult p_result_callback, void *p_userdata, bool p_swap_result) { +bool CollisionSolver3DSW::solve_soft_body(const Shape3DSW *p_shape_A, const Transform3D &p_transform_A, const Shape3DSW *p_shape_B, const Transform3D &p_transform_B, CallbackResult p_result_callback, void *p_userdata, bool p_swap_result) { const SoftBodyShape3DSW *soft_body_shape_B = static_cast<const SoftBodyShape3DSW *>(p_shape_B); SoftBody3DSW *soft_body = soft_body_shape_B->get_soft_body(); - const Transform &world_to_local = soft_body->get_inv_transform(); + const Transform3D &world_to_local = soft_body->get_inv_transform(); const real_t collision_margin = soft_body->get_collision_margin(); @@ -257,9 +257,9 @@ bool CollisionSolver3DSW::solve_soft_body(const Shape3DSW *p_shape_A, const Tran } struct _ConcaveCollisionInfo { - const Transform *transform_A; + const Transform3D *transform_A; const Shape3DSW *shape_A; - const Transform *transform_B; + const Transform3D *transform_B; CollisionSolver3DSW::CallbackResult result_callback; void *userdata; bool swap_result; @@ -285,7 +285,7 @@ void CollisionSolver3DSW::concave_callback(void *p_userdata, Shape3DSW *p_convex cinfo.collisions++; } -bool CollisionSolver3DSW::solve_concave(const Shape3DSW *p_shape_A, const Transform &p_transform_A, const Shape3DSW *p_shape_B, const Transform &p_transform_B, CallbackResult p_result_callback, void *p_userdata, bool p_swap_result, real_t p_margin_A, real_t p_margin_B) { +bool CollisionSolver3DSW::solve_concave(const Shape3DSW *p_shape_A, const Transform3D &p_transform_A, const Shape3DSW *p_shape_B, const Transform3D &p_transform_B, CallbackResult p_result_callback, void *p_userdata, bool p_swap_result, real_t p_margin_A, real_t p_margin_B) { const ConcaveShape3DSW *concave_B = static_cast<const ConcaveShape3DSW *>(p_shape_B); _ConcaveCollisionInfo cinfo; @@ -302,7 +302,7 @@ bool CollisionSolver3DSW::solve_concave(const Shape3DSW *p_shape_A, const Transf cinfo.aabb_tests = 0; - Transform rel_transform = p_transform_A; + Transform3D rel_transform = p_transform_A; rel_transform.origin -= p_transform_B.origin; //quickly compute a local AABB @@ -329,7 +329,7 @@ bool CollisionSolver3DSW::solve_concave(const Shape3DSW *p_shape_A, const Transf return cinfo.collided; } -bool CollisionSolver3DSW::solve_static(const Shape3DSW *p_shape_A, const Transform &p_transform_A, const Shape3DSW *p_shape_B, const Transform &p_transform_B, CallbackResult p_result_callback, void *p_userdata, Vector3 *r_sep_axis, real_t p_margin_A, real_t p_margin_B) { +bool CollisionSolver3DSW::solve_static(const Shape3DSW *p_shape_A, const Transform3D &p_transform_A, const Shape3DSW *p_shape_B, const Transform3D &p_transform_B, CallbackResult p_result_callback, void *p_userdata, Vector3 *r_sep_axis, real_t p_margin_A, real_t p_margin_B) { PhysicsServer3D::ShapeType type_A = p_shape_A->get_type(); PhysicsServer3D::ShapeType type_B = p_shape_B->get_type(); bool concave_A = p_shape_A->is_concave(); @@ -421,7 +421,7 @@ void CollisionSolver3DSW::concave_distance_callback(void *p_userdata, Shape3DSW cinfo.collisions++; } -bool CollisionSolver3DSW::solve_distance_plane(const Shape3DSW *p_shape_A, const Transform &p_transform_A, const Shape3DSW *p_shape_B, const Transform &p_transform_B, Vector3 &r_point_A, Vector3 &r_point_B) { +bool CollisionSolver3DSW::solve_distance_plane(const Shape3DSW *p_shape_A, const Transform3D &p_transform_A, const Shape3DSW *p_shape_B, const Transform3D &p_transform_B, Vector3 &r_point_A, Vector3 &r_point_B) { const PlaneShape3DSW *plane = static_cast<const PlaneShape3DSW *>(p_shape_A); if (p_shape_B->get_type() == PhysicsServer3D::SHAPE_PLANE) { return false; @@ -473,7 +473,7 @@ bool CollisionSolver3DSW::solve_distance_plane(const Shape3DSW *p_shape_A, const return collided; } -bool CollisionSolver3DSW::solve_distance(const Shape3DSW *p_shape_A, const Transform &p_transform_A, const Shape3DSW *p_shape_B, const Transform &p_transform_B, Vector3 &r_point_A, Vector3 &r_point_B, const AABB &p_concave_hint, Vector3 *r_sep_axis) { +bool CollisionSolver3DSW::solve_distance(const Shape3DSW *p_shape_A, const Transform3D &p_transform_A, const Shape3DSW *p_shape_B, const Transform3D &p_transform_B, Vector3 &r_point_A, Vector3 &r_point_B, const AABB &p_concave_hint, Vector3 *r_sep_axis) { if (p_shape_A->is_concave()) { return false; } @@ -504,7 +504,7 @@ bool CollisionSolver3DSW::solve_distance(const Shape3DSW *p_shape_A, const Trans cinfo.aabb_tests = 0; cinfo.tested = false; - Transform rel_transform = p_transform_A; + Transform3D rel_transform = p_transform_A; rel_transform.origin -= p_transform_B.origin; //quickly compute a local AABB diff --git a/servers/physics_3d/collision_solver_3d_sw.h b/servers/physics_3d/collision_solver_3d_sw.h index 34ac2c6d3f..a5dd7d48eb 100644 --- a/servers/physics_3d/collision_solver_3d_sw.h +++ b/servers/physics_3d/collision_solver_3d_sw.h @@ -42,16 +42,16 @@ private: static void soft_body_contact_callback(const Vector3 &p_point_A, int p_index_A, const Vector3 &p_point_B, int p_index_B, void *p_userdata); static void soft_body_concave_callback(void *p_userdata, Shape3DSW *p_convex); static void concave_callback(void *p_userdata, Shape3DSW *p_convex); - static bool solve_static_plane(const Shape3DSW *p_shape_A, const Transform &p_transform_A, const Shape3DSW *p_shape_B, const Transform &p_transform_B, CallbackResult p_result_callback, void *p_userdata, bool p_swap_result); - static bool solve_ray(const Shape3DSW *p_shape_A, const Transform &p_transform_A, const Shape3DSW *p_shape_B, const Transform &p_transform_B, CallbackResult p_result_callback, void *p_userdata, bool p_swap_result); - static bool solve_soft_body(const Shape3DSW *p_shape_A, const Transform &p_transform_A, const Shape3DSW *p_shape_B, const Transform &p_transform_B, CallbackResult p_result_callback, void *p_userdata, bool p_swap_result); - static bool solve_concave(const Shape3DSW *p_shape_A, const Transform &p_transform_A, const Shape3DSW *p_shape_B, const Transform &p_transform_B, CallbackResult p_result_callback, void *p_userdata, bool p_swap_result, real_t p_margin_A = 0, real_t p_margin_B = 0); + static bool solve_static_plane(const Shape3DSW *p_shape_A, const Transform3D &p_transform_A, const Shape3DSW *p_shape_B, const Transform3D &p_transform_B, CallbackResult p_result_callback, void *p_userdata, bool p_swap_result); + static bool solve_ray(const Shape3DSW *p_shape_A, const Transform3D &p_transform_A, const Shape3DSW *p_shape_B, const Transform3D &p_transform_B, CallbackResult p_result_callback, void *p_userdata, bool p_swap_result); + static bool solve_soft_body(const Shape3DSW *p_shape_A, const Transform3D &p_transform_A, const Shape3DSW *p_shape_B, const Transform3D &p_transform_B, CallbackResult p_result_callback, void *p_userdata, bool p_swap_result); + static bool solve_concave(const Shape3DSW *p_shape_A, const Transform3D &p_transform_A, const Shape3DSW *p_shape_B, const Transform3D &p_transform_B, CallbackResult p_result_callback, void *p_userdata, bool p_swap_result, real_t p_margin_A = 0, real_t p_margin_B = 0); static void concave_distance_callback(void *p_userdata, Shape3DSW *p_convex); - static bool solve_distance_plane(const Shape3DSW *p_shape_A, const Transform &p_transform_A, const Shape3DSW *p_shape_B, const Transform &p_transform_B, Vector3 &r_point_A, Vector3 &r_point_B); + static bool solve_distance_plane(const Shape3DSW *p_shape_A, const Transform3D &p_transform_A, const Shape3DSW *p_shape_B, const Transform3D &p_transform_B, Vector3 &r_point_A, Vector3 &r_point_B); public: - static bool solve_static(const Shape3DSW *p_shape_A, const Transform &p_transform_A, const Shape3DSW *p_shape_B, const Transform &p_transform_B, CallbackResult p_result_callback, void *p_userdata, Vector3 *r_sep_axis = nullptr, real_t p_margin_A = 0, real_t p_margin_B = 0); - static bool solve_distance(const Shape3DSW *p_shape_A, const Transform &p_transform_A, const Shape3DSW *p_shape_B, const Transform &p_transform_B, Vector3 &r_point_A, Vector3 &r_point_B, const AABB &p_concave_hint, Vector3 *r_sep_axis = nullptr); + static bool solve_static(const Shape3DSW *p_shape_A, const Transform3D &p_transform_A, const Shape3DSW *p_shape_B, const Transform3D &p_transform_B, CallbackResult p_result_callback, void *p_userdata, Vector3 *r_sep_axis = nullptr, real_t p_margin_A = 0, real_t p_margin_B = 0); + static bool solve_distance(const Shape3DSW *p_shape_A, const Transform3D &p_transform_A, const Shape3DSW *p_shape_B, const Transform3D &p_transform_B, Vector3 &r_point_A, Vector3 &r_point_B, const AABB &p_concave_hint, Vector3 *r_sep_axis = nullptr); }; #endif // COLLISION_SOLVER__SW_H diff --git a/servers/physics_3d/gjk_epa.cpp b/servers/physics_3d/gjk_epa.cpp index 1a8c7f704f..2df991563d 100644 --- a/servers/physics_3d/gjk_epa.cpp +++ b/servers/physics_3d/gjk_epa.cpp @@ -107,16 +107,16 @@ typedef unsigned char U1; struct MinkowskiDiff { const Shape3DSW* m_shapes[2]; - Transform transform_A; - Transform transform_B; + Transform3D transform_A; + Transform3D transform_B; real_t margin_A = 0.0; real_t margin_B = 0.0; Vector3 (*get_support)(const Shape3DSW*, const Vector3&, real_t); - void Initialize(const Shape3DSW* shape0, const Transform& wtrs0, const real_t margin0, - const Shape3DSW* shape1, const Transform& wtrs1, const real_t margin1) { + void Initialize(const Shape3DSW* shape0, const Transform3D& wtrs0, const real_t margin0, + const Shape3DSW* shape1, const Transform3D& wtrs1, const real_t margin1) { m_shapes[0] = shape0; m_shapes[1] = shape1; transform_A = wtrs0; @@ -862,8 +862,8 @@ struct GJK }; // - static void Initialize( const Shape3DSW* shape0, const Transform& wtrs0, real_t margin0, - const Shape3DSW* shape1, const Transform& wtrs1, real_t margin1, + static void Initialize( const Shape3DSW* shape0, const Transform3D& wtrs0, real_t margin0, + const Shape3DSW* shape1, const Transform3D& wtrs1, real_t margin1, sResults& results, tShape& shape) { @@ -885,10 +885,10 @@ struct GJK // bool Distance( const Shape3DSW* shape0, - const Transform& wtrs0, + const Transform3D& wtrs0, real_t margin0, const Shape3DSW* shape1, - const Transform& wtrs1, + const Transform3D& wtrs1, real_t margin1, const Vector3& guess, sResults& results) @@ -926,10 +926,10 @@ bool Distance( const Shape3DSW* shape0, // bool Penetration( const Shape3DSW* shape0, - const Transform& wtrs0, + const Transform3D& wtrs0, real_t margin0, const Shape3DSW* shape1, - const Transform& wtrs1, + const Transform3D& wtrs1, real_t margin1, const Vector3& guess, sResults& results @@ -993,7 +993,7 @@ bool Penetration( const Shape3DSW* shape0, /* clang-format on */ -bool gjk_epa_calculate_distance(const Shape3DSW *p_shape_A, const Transform &p_transform_A, const Shape3DSW *p_shape_B, const Transform &p_transform_B, Vector3 &r_result_A, Vector3 &r_result_B) { +bool gjk_epa_calculate_distance(const Shape3DSW *p_shape_A, const Transform3D &p_transform_A, const Shape3DSW *p_shape_B, const Transform3D &p_transform_B, Vector3 &r_result_A, Vector3 &r_result_B) { GjkEpa2::sResults res; if (GjkEpa2::Distance(p_shape_A, p_transform_A, 0.0, p_shape_B, p_transform_B, 0.0, p_transform_B.origin - p_transform_A.origin, res)) { @@ -1005,7 +1005,7 @@ bool gjk_epa_calculate_distance(const Shape3DSW *p_shape_A, const Transform &p_t return false; } -bool gjk_epa_calculate_penetration(const Shape3DSW *p_shape_A, const Transform &p_transform_A, const Shape3DSW *p_shape_B, const Transform &p_transform_B, CollisionSolver3DSW::CallbackResult p_result_callback, void *p_userdata, bool p_swap, real_t p_margin_A, real_t p_margin_B) { +bool gjk_epa_calculate_penetration(const Shape3DSW *p_shape_A, const Transform3D &p_transform_A, const Shape3DSW *p_shape_B, const Transform3D &p_transform_B, CollisionSolver3DSW::CallbackResult p_result_callback, void *p_userdata, bool p_swap, real_t p_margin_A, real_t p_margin_B) { GjkEpa2::sResults res; if (GjkEpa2::Penetration(p_shape_A, p_transform_A, p_margin_A, p_shape_B, p_transform_B, p_margin_B, p_transform_B.origin - p_transform_A.origin, res)) { diff --git a/servers/physics_3d/gjk_epa.h b/servers/physics_3d/gjk_epa.h index a7e2e1719e..69e85d2bc0 100644 --- a/servers/physics_3d/gjk_epa.h +++ b/servers/physics_3d/gjk_epa.h @@ -34,7 +34,7 @@ #include "collision_solver_3d_sw.h" #include "shape_3d_sw.h" -bool gjk_epa_calculate_penetration(const Shape3DSW *p_shape_A, const Transform &p_transform_A, const Shape3DSW *p_shape_B, const Transform &p_transform_B, CollisionSolver3DSW::CallbackResult p_result_callback, void *p_userdata, bool p_swap = false, real_t p_margin_A = 0.0, real_t p_margin_B = 0.0); -bool gjk_epa_calculate_distance(const Shape3DSW *p_shape_A, const Transform &p_transform_A, const Shape3DSW *p_shape_B, const Transform &p_transform_B, Vector3 &r_result_A, Vector3 &r_result_B); +bool gjk_epa_calculate_penetration(const Shape3DSW *p_shape_A, const Transform3D &p_transform_A, const Shape3DSW *p_shape_B, const Transform3D &p_transform_B, CollisionSolver3DSW::CallbackResult p_result_callback, void *p_userdata, bool p_swap = false, real_t p_margin_A = 0.0, real_t p_margin_B = 0.0); +bool gjk_epa_calculate_distance(const Shape3DSW *p_shape_A, const Transform3D &p_transform_A, const Shape3DSW *p_shape_B, const Transform3D &p_transform_B, Vector3 &r_result_A, Vector3 &r_result_B); #endif diff --git a/servers/physics_3d/joints/cone_twist_joint_3d_sw.cpp b/servers/physics_3d/joints/cone_twist_joint_3d_sw.cpp index e9efddf165..7315e9c709 100644 --- a/servers/physics_3d/joints/cone_twist_joint_3d_sw.cpp +++ b/servers/physics_3d/joints/cone_twist_joint_3d_sw.cpp @@ -84,7 +84,7 @@ static _FORCE_INLINE_ real_t atan2fast(real_t y, real_t x) { return (y < 0.0f) ? -angle : angle; } -ConeTwistJoint3DSW::ConeTwistJoint3DSW(Body3DSW *rbA, Body3DSW *rbB, const Transform &rbAFrame, const Transform &rbBFrame) : +ConeTwistJoint3DSW::ConeTwistJoint3DSW(Body3DSW *rbA, Body3DSW *rbB, const Transform3D &rbAFrame, const Transform3D &rbBFrame) : Joint3DSW(_arr, 2) { A = rbA; B = rbB; @@ -211,7 +211,7 @@ bool ConeTwistJoint3DSW::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)); - Quat rotationArc = Quat(b2Axis1, b1Axis1); + 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/cone_twist_joint_3d_sw.h b/servers/physics_3d/joints/cone_twist_joint_3d_sw.h index b871ea50db..608847352c 100644 --- a/servers/physics_3d/joints/cone_twist_joint_3d_sw.h +++ b/servers/physics_3d/joints/cone_twist_joint_3d_sw.h @@ -73,8 +73,8 @@ public: JacobianEntry3DSW m_jac[3]; //3 orthogonal linear constraints real_t m_appliedImpulse; - Transform m_rbAFrame; - Transform m_rbBFrame; + Transform3D m_rbAFrame; + Transform3D m_rbBFrame; real_t m_limitSoftness; real_t m_biasFactor; @@ -107,7 +107,7 @@ public: virtual bool setup(real_t p_step) override; virtual void solve(real_t p_step) override; - ConeTwistJoint3DSW(Body3DSW *rbA, Body3DSW *rbB, const Transform &rbAFrame, const Transform &rbBFrame); + ConeTwistJoint3DSW(Body3DSW *rbA, Body3DSW *rbB, const Transform3D &rbAFrame, const Transform3D &rbBFrame); void setAngularOnly(bool angularOnly) { m_angularOnly = angularOnly; diff --git a/servers/physics_3d/joints/generic_6dof_joint_3d_sw.cpp b/servers/physics_3d/joints/generic_6dof_joint_3d_sw.cpp index 7c504764a7..56aba24b42 100644 --- a/servers/physics_3d/joints/generic_6dof_joint_3d_sw.cpp +++ b/servers/physics_3d/joints/generic_6dof_joint_3d_sw.cpp @@ -219,7 +219,7 @@ real_t G6DOFTranslationalLimitMotor3DSW::solveLinearAxis( //////////////////////////// G6DOFTranslationalLimitMotorSW //////////////////////////////////// -Generic6DOFJoint3DSW::Generic6DOFJoint3DSW(Body3DSW *rbA, Body3DSW *rbB, const Transform &frameInA, const Transform &frameInB, bool useLinearReferenceFrameA) : +Generic6DOFJoint3DSW::Generic6DOFJoint3DSW(Body3DSW *rbA, Body3DSW *rbB, const Transform3D &frameInA, const Transform3D &frameInB, bool useLinearReferenceFrameA) : Joint3DSW(_arr, 2), m_frameInA(frameInA), m_frameInB(frameInB), diff --git a/servers/physics_3d/joints/generic_6dof_joint_3d_sw.h b/servers/physics_3d/joints/generic_6dof_joint_3d_sw.h index 8af76cefc2..d46437e782 100644 --- a/servers/physics_3d/joints/generic_6dof_joint_3d_sw.h +++ b/servers/physics_3d/joints/generic_6dof_joint_3d_sw.h @@ -81,7 +81,7 @@ public: //! temp_variables //!@{ - real_t m_currentLimitError; //! How much is violated this limit + real_t m_currentLimitError; //!< How much is violated this limit int m_currentLimit; //!< 0=free, 1=at lo limit, 2=at hi limit real_t m_accumulatedImpulse; //!@} @@ -113,7 +113,7 @@ public: return (m_enableMotor || m_currentLimit != 0); } - //! calculates error + //! calculates error /*! calculates m_currentLimit and m_currentLimitError. */ @@ -185,8 +185,8 @@ protected: //! relative_frames //!@{ - Transform m_frameInA; //!< the constraint space w.r.t body A - Transform m_frameInB; //!< the constraint space w.r.t body B + Transform3D m_frameInA; //!< the constraint space w.r.t body A + Transform3D m_frameInB; //!< the constraint space w.r.t body B //!@} //! Jacobians @@ -209,8 +209,8 @@ protected: //! temporal variables //!@{ real_t m_timeStep; - Transform m_calculatedTransformA; - Transform m_calculatedTransformB; + Transform3D m_calculatedTransformA; + Transform3D m_calculatedTransformB; Vector3 m_calculatedAxisAngleDiff; Vector3 m_calculatedAxis[3]; @@ -233,7 +233,7 @@ protected: void calculateAngleInfo(); public: - Generic6DOFJoint3DSW(Body3DSW *rbA, Body3DSW *rbB, const Transform &frameInA, const Transform &frameInB, bool useLinearReferenceFrameA); + Generic6DOFJoint3DSW(Body3DSW *rbA, Body3DSW *rbB, const Transform3D &frameInA, const Transform3D &frameInB, bool useLinearReferenceFrameA); virtual PhysicsServer3D::JointType get_type() const override { return PhysicsServer3D::JOINT_TYPE_6DOF; } @@ -251,7 +251,7 @@ public: /*! \sa Generic6DOFJointSW.getFrameOffsetA, Generic6DOFJointSW.getFrameOffsetB, Generic6DOFJointSW.calculateAngleInfo. */ - const Transform &getCalculatedTransformA() const { + const Transform3D &getCalculatedTransformA() const { return m_calculatedTransformA; } @@ -259,23 +259,23 @@ public: /*! \sa Generic6DOFJointSW.getFrameOffsetA, Generic6DOFJointSW.getFrameOffsetB, Generic6DOFJointSW.calculateAngleInfo. */ - const Transform &getCalculatedTransformB() const { + const Transform3D &getCalculatedTransformB() const { return m_calculatedTransformB; } - const Transform &getFrameOffsetA() const { + const Transform3D &getFrameOffsetA() const { return m_frameInA; } - const Transform &getFrameOffsetB() const { + const Transform3D &getFrameOffsetB() const { return m_frameInB; } - Transform &getFrameOffsetA() { + Transform3D &getFrameOffsetA() { return m_frameInA; } - Transform &getFrameOffsetB() { + Transform3D &getFrameOffsetB() { return m_frameInB; } @@ -327,7 +327,7 @@ public: return &m_angularLimits[index]; } - //! Retrieves the limit informacion + //! Retrieves the limit informacion G6DOFTranslationalLimitMotor3DSW *getTranslationalLimitMotor() { return &m_linearLimits; } diff --git a/servers/physics_3d/joints/hinge_joint_3d_sw.cpp b/servers/physics_3d/joints/hinge_joint_3d_sw.cpp index bb8858c28a..b928f18231 100644 --- a/servers/physics_3d/joints/hinge_joint_3d_sw.cpp +++ b/servers/physics_3d/joints/hinge_joint_3d_sw.cpp @@ -67,7 +67,7 @@ static void plane_space(const Vector3 &n, Vector3 &p, Vector3 &q) { } } -HingeJoint3DSW::HingeJoint3DSW(Body3DSW *rbA, Body3DSW *rbB, const Transform &frameA, const Transform &frameB) : +HingeJoint3DSW::HingeJoint3DSW(Body3DSW *rbA, Body3DSW *rbB, const Transform3D &frameA, const Transform3D &frameB) : Joint3DSW(_arr, 2) { A = rbA; B = rbB; @@ -126,7 +126,7 @@ HingeJoint3DSW::HingeJoint3DSW(Body3DSW *rbA, Body3DSW *rbB, const Vector3 &pivo rbAxisA1.y, rbAxisA2.y, axisInA.y, rbAxisA1.z, rbAxisA2.z, axisInA.z); - Quat rotationArc = Quat(axisInA, axisInB); + Quaternion rotationArc = Quaternion(axisInA, axisInB); Vector3 rbAxisB1 = rotationArc.xform(rbAxisA1); Vector3 rbAxisB2 = axisInB.cross(rbAxisB1); diff --git a/servers/physics_3d/joints/hinge_joint_3d_sw.h b/servers/physics_3d/joints/hinge_joint_3d_sw.h index 2100f5de44..22eb2f4660 100644 --- a/servers/physics_3d/joints/hinge_joint_3d_sw.h +++ b/servers/physics_3d/joints/hinge_joint_3d_sw.h @@ -66,8 +66,8 @@ class HingeJoint3DSW : public Joint3DSW { JacobianEntry3DSW m_jac[3]; //3 orthogonal linear constraints JacobianEntry3DSW m_jacAng[3]; //2 orthogonal angular constraints+ 1 for limit/motor - Transform m_rbAFrame; // constraint axii. Assumes z is hinge axis. - Transform m_rbBFrame; + Transform3D m_rbAFrame; // constraint axii. Assumes z is hinge axis. + Transform3D m_rbBFrame; real_t m_motorTargetVelocity; real_t m_maxMotorImpulse; @@ -109,7 +109,7 @@ public: void set_flag(PhysicsServer3D::HingeJointFlag p_flag, bool p_value); bool get_flag(PhysicsServer3D::HingeJointFlag p_flag) const; - HingeJoint3DSW(Body3DSW *rbA, Body3DSW *rbB, const Transform &frameA, const Transform &frameB); + HingeJoint3DSW(Body3DSW *rbA, Body3DSW *rbB, const Transform3D &frameA, const Transform3D &frameB); HingeJoint3DSW(Body3DSW *rbA, Body3DSW *rbB, const Vector3 &pivotInA, const Vector3 &pivotInB, const Vector3 &axisInA, const Vector3 &axisInB); }; diff --git a/servers/physics_3d/joints/jacobian_entry_3d_sw.h b/servers/physics_3d/joints/jacobian_entry_3d_sw.h index 2829a5caf7..6afa70c816 100644 --- a/servers/physics_3d/joints/jacobian_entry_3d_sw.h +++ b/servers/physics_3d/joints/jacobian_entry_3d_sw.h @@ -50,7 +50,7 @@ subject to the following restrictions: 3. This notice may not be removed or altered from any source distribution. */ -#include "core/math/transform.h" +#include "core/math/transform_3d.h" class JacobianEntry3DSW { public: diff --git a/servers/physics_3d/joints/slider_joint_3d_sw.cpp b/servers/physics_3d/joints/slider_joint_3d_sw.cpp index 8bd1951311..db9bdb2986 100644 --- a/servers/physics_3d/joints/slider_joint_3d_sw.cpp +++ b/servers/physics_3d/joints/slider_joint_3d_sw.cpp @@ -111,7 +111,7 @@ void SliderJoint3DSW::initParams() { //----------------------------------------------------------------------------- -SliderJoint3DSW::SliderJoint3DSW(Body3DSW *rbA, Body3DSW *rbB, const Transform &frameInA, const Transform &frameInB) : +SliderJoint3DSW::SliderJoint3DSW(Body3DSW *rbA, Body3DSW *rbB, const Transform3D &frameInA, const Transform3D &frameInB) : Joint3DSW(_arr, 2), m_frameInA(frameInA), m_frameInB(frameInB) { diff --git a/servers/physics_3d/joints/slider_joint_3d_sw.h b/servers/physics_3d/joints/slider_joint_3d_sw.h index ef5891d0f9..f357bbd67a 100644 --- a/servers/physics_3d/joints/slider_joint_3d_sw.h +++ b/servers/physics_3d/joints/slider_joint_3d_sw.h @@ -76,8 +76,8 @@ protected: Body3DSW *_arr[2]; }; - Transform m_frameInA; - Transform m_frameInB; + Transform3D m_frameInA; + Transform3D m_frameInB; // linear limits real_t m_lowerLinLimit; @@ -120,8 +120,8 @@ protected: JacobianEntry3DSW m_jacAng[3]; real_t m_timeStep; - Transform m_calculatedTransformA; - Transform m_calculatedTransformB; + Transform3D m_calculatedTransformA; + Transform3D m_calculatedTransformB; Vector3 m_sliderAxis; Vector3 m_realPivotAInW; @@ -152,19 +152,19 @@ protected: public: // constructors - SliderJoint3DSW(Body3DSW *rbA, Body3DSW *rbB, const Transform &frameInA, const Transform &frameInB); + SliderJoint3DSW(Body3DSW *rbA, Body3DSW *rbB, const Transform3D &frameInA, const Transform3D &frameInB); //SliderJointSW(); // overrides // access const Body3DSW *getRigidBodyA() const { return A; } const Body3DSW *getRigidBodyB() const { return B; } - const Transform &getCalculatedTransformA() const { return m_calculatedTransformA; } - const Transform &getCalculatedTransformB() const { return m_calculatedTransformB; } - const Transform &getFrameOffsetA() const { return m_frameInA; } - const Transform &getFrameOffsetB() const { return m_frameInB; } - Transform &getFrameOffsetA() { return m_frameInA; } - Transform &getFrameOffsetB() { return m_frameInB; } + const Transform3D &getCalculatedTransformA() const { return m_calculatedTransformA; } + const Transform3D &getCalculatedTransformB() const { return m_calculatedTransformB; } + const Transform3D &getFrameOffsetA() const { return m_frameInA; } + const Transform3D &getFrameOffsetB() const { return m_frameInB; } + Transform3D &getFrameOffsetA() { return m_frameInA; } + Transform3D &getFrameOffsetB() { return m_frameInB; } real_t getLowerLinLimit() { return m_lowerLinLimit; } void setLowerLinLimit(real_t lowerLimit) { m_lowerLinLimit = lowerLimit; } real_t getUpperLinLimit() { return m_upperLinLimit; } diff --git a/servers/physics_3d/physics_server_3d_sw.cpp b/servers/physics_3d/physics_server_3d_sw.cpp index f3eb1ae48f..7a95a8abc8 100644 --- a/servers/physics_3d/physics_server_3d_sw.cpp +++ b/servers/physics_3d/physics_server_3d_sw.cpp @@ -262,7 +262,7 @@ PhysicsServer3D::AreaSpaceOverrideMode PhysicsServer3DSW::area_get_space_overrid return area->get_space_override_mode(); } -void PhysicsServer3DSW::area_add_shape(RID p_area, RID p_shape, const Transform &p_transform, bool p_disabled) { +void PhysicsServer3DSW::area_add_shape(RID p_area, RID p_shape, const Transform3D &p_transform, bool p_disabled) { Area3DSW *area = area_owner.getornull(p_area); ERR_FAIL_COND(!area); @@ -283,7 +283,7 @@ void PhysicsServer3DSW::area_set_shape(RID p_area, int p_shape_idx, RID p_shape) area->set_shape(p_shape_idx, shape); } -void PhysicsServer3DSW::area_set_shape_transform(RID p_area, int p_shape_idx, const Transform &p_transform) { +void PhysicsServer3DSW::area_set_shape_transform(RID p_area, int p_shape_idx, const Transform3D &p_transform) { Area3DSW *area = area_owner.getornull(p_area); ERR_FAIL_COND(!area); @@ -307,9 +307,9 @@ RID PhysicsServer3DSW::area_get_shape(RID p_area, int p_shape_idx) const { return shape->get_self(); } -Transform PhysicsServer3DSW::area_get_shape_transform(RID p_area, int p_shape_idx) const { +Transform3D PhysicsServer3DSW::area_get_shape_transform(RID p_area, int p_shape_idx) const { Area3DSW *area = area_owner.getornull(p_area); - ERR_FAIL_COND_V(!area, Transform()); + ERR_FAIL_COND_V(!area, Transform3D()); return area->get_shape_transform(p_shape_idx); } @@ -368,7 +368,7 @@ void PhysicsServer3DSW::area_set_param(RID p_area, AreaParameter p_param, const area->set_param(p_param, p_value); }; -void PhysicsServer3DSW::area_set_transform(RID p_area, const Transform &p_transform) { +void PhysicsServer3DSW::area_set_transform(RID p_area, const Transform3D &p_transform) { Area3DSW *area = area_owner.getornull(p_area); ERR_FAIL_COND(!area); area->set_transform(p_transform); @@ -385,9 +385,9 @@ Variant PhysicsServer3DSW::area_get_param(RID p_area, AreaParameter p_param) con return area->get_param(p_param); }; -Transform PhysicsServer3DSW::area_get_transform(RID p_area) const { +Transform3D PhysicsServer3DSW::area_get_transform(RID p_area) const { Area3DSW *area = area_owner.getornull(p_area); - ERR_FAIL_COND_V(!area, Transform()); + ERR_FAIL_COND_V(!area, Transform3D()); return area->get_transform(); }; @@ -487,7 +487,7 @@ PhysicsServer3D::BodyMode PhysicsServer3DSW::body_get_mode(RID p_body) const { return body->get_mode(); }; -void PhysicsServer3DSW::body_add_shape(RID p_body, RID p_shape, const Transform &p_transform, bool p_disabled) { +void PhysicsServer3DSW::body_add_shape(RID p_body, RID p_shape, const Transform3D &p_transform, bool p_disabled) { Body3DSW *body = body_owner.getornull(p_body); ERR_FAIL_COND(!body); @@ -507,8 +507,7 @@ void PhysicsServer3DSW::body_set_shape(RID p_body, int p_shape_idx, RID p_shape) body->set_shape(p_shape_idx, shape); } - -void PhysicsServer3DSW::body_set_shape_transform(RID p_body, int p_shape_idx, const Transform &p_transform) { +void PhysicsServer3DSW::body_set_shape_transform(RID p_body, int p_shape_idx, const Transform3D &p_transform) { Body3DSW *body = body_owner.getornull(p_body); ERR_FAIL_COND(!body); @@ -541,9 +540,9 @@ void PhysicsServer3DSW::body_set_shape_disabled(RID p_body, int p_shape_idx, boo body->set_shape_as_disabled(p_shape_idx, p_disabled); } -Transform PhysicsServer3DSW::body_get_shape_transform(RID p_body, int p_shape_idx) const { +Transform3D PhysicsServer3DSW::body_get_shape_transform(RID p_body, int p_shape_idx) const { Body3DSW *body = body_owner.getornull(p_body); - ERR_FAIL_COND_V(!body, Transform()); + ERR_FAIL_COND_V(!body, Transform3D()); return body->get_shape_transform(p_shape_idx); } @@ -657,19 +656,6 @@ real_t PhysicsServer3DSW::body_get_param(RID p_body, BodyParameter p_param) cons return body->get_param(p_param); }; -void PhysicsServer3DSW::body_set_kinematic_safe_margin(RID p_body, real_t p_margin) { - Body3DSW *body = body_owner.getornull(p_body); - ERR_FAIL_COND(!body); - body->set_kinematic_margin(p_margin); -} - -real_t PhysicsServer3DSW::body_get_kinematic_safe_margin(RID p_body) const { - Body3DSW *body = body_owner.getornull(p_body); - ERR_FAIL_COND_V(!body, 0); - - return body->get_kinematic_margin(); -} - void PhysicsServer3DSW::body_set_state(RID p_body, BodyState p_state, const Variant &p_variant) { Body3DSW *body = body_owner.getornull(p_body); ERR_FAIL_COND(!body); @@ -868,7 +854,7 @@ void PhysicsServer3DSW::body_set_ray_pickable(RID p_body, bool p_enable) { body->set_ray_pickable(p_enable); } -bool PhysicsServer3DSW::body_test_motion(RID p_body, const Transform &p_from, const Vector3 &p_motion, bool p_infinite_inertia, MotionResult *r_result, bool p_exclude_raycast_shapes) { +bool PhysicsServer3DSW::body_test_motion(RID p_body, const Transform3D &p_from, const Vector3 &p_motion, bool p_infinite_inertia, real_t p_margin, MotionResult *r_result, bool p_exclude_raycast_shapes) { Body3DSW *body = body_owner.getornull(p_body); ERR_FAIL_COND_V(!body, false); ERR_FAIL_COND_V(!body->get_space(), false); @@ -876,10 +862,10 @@ bool PhysicsServer3DSW::body_test_motion(RID p_body, const Transform &p_from, co _update_shapes(); - return body->get_space()->test_body_motion(body, p_from, p_motion, p_infinite_inertia, body->get_kinematic_margin(), r_result, p_exclude_raycast_shapes); + return body->get_space()->test_body_motion(body, p_from, p_motion, p_infinite_inertia, p_margin, r_result, p_exclude_raycast_shapes); } -int PhysicsServer3DSW::body_test_ray_separation(RID p_body, const Transform &p_transform, bool p_infinite_inertia, Vector3 &r_recover_motion, SeparationResult *r_results, int p_result_max, real_t p_margin) { +int PhysicsServer3DSW::body_test_ray_separation(RID p_body, const Transform3D &p_transform, bool p_infinite_inertia, Vector3 &r_recover_motion, SeparationResult *r_results, int p_result_max, real_t p_margin) { Body3DSW *body = body_owner.getornull(p_body); ERR_FAIL_COND_V(!body, false); ERR_FAIL_COND_V(!body->get_space(), false); @@ -1010,7 +996,7 @@ Variant PhysicsServer3DSW::soft_body_get_state(RID p_body, BodyState p_state) co return soft_body->get_state(p_state); } -void PhysicsServer3DSW::soft_body_set_transform(RID p_body, const Transform &p_transform) { +void PhysicsServer3DSW::soft_body_set_transform(RID p_body, const Transform3D &p_transform) { SoftBody3DSW *soft_body = soft_body_owner.getornull(p_body); ERR_FAIL_COND(!soft_body); @@ -1253,7 +1239,7 @@ Vector3 PhysicsServer3DSW::pin_joint_get_local_b(RID p_joint) const { return pin_joint->get_position_b(); } -void PhysicsServer3DSW::joint_make_hinge(RID p_joint, RID p_body_A, const Transform &p_frame_A, RID p_body_B, const Transform &p_frame_B) { +void PhysicsServer3DSW::joint_make_hinge(RID p_joint, RID p_body_A, const Transform3D &p_frame_A, RID p_body_B, const Transform3D &p_frame_B) { Body3DSW *body_A = body_owner.getornull(p_body_A); ERR_FAIL_COND(!body_A); @@ -1378,7 +1364,7 @@ PhysicsServer3DSW::JointType PhysicsServer3DSW::joint_get_type(RID p_joint) cons return joint->get_type(); } -void PhysicsServer3DSW::joint_make_slider(RID p_joint, RID p_body_A, const Transform &p_local_frame_A, RID p_body_B, const Transform &p_local_frame_B) { +void PhysicsServer3DSW::joint_make_slider(RID p_joint, RID p_body_A, const Transform3D &p_local_frame_A, RID p_body_B, const Transform3D &p_local_frame_B) { Body3DSW *body_A = body_owner.getornull(p_body_A); ERR_FAIL_COND(!body_A); @@ -1418,7 +1404,7 @@ real_t PhysicsServer3DSW::slider_joint_get_param(RID p_joint, SliderJointParam p return slider_joint->get_param(p_param); } -void PhysicsServer3DSW::joint_make_cone_twist(RID p_joint, RID p_body_A, const Transform &p_local_frame_A, RID p_body_B, const Transform &p_local_frame_B) { +void PhysicsServer3DSW::joint_make_cone_twist(RID p_joint, RID p_body_A, const Transform3D &p_local_frame_A, RID p_body_B, const Transform3D &p_local_frame_B) { Body3DSW *body_A = body_owner.getornull(p_body_A); ERR_FAIL_COND(!body_A); @@ -1458,7 +1444,7 @@ real_t PhysicsServer3DSW::cone_twist_joint_get_param(RID p_joint, ConeTwistJoint return cone_twist_joint->get_param(p_param); } -void PhysicsServer3DSW::joint_make_generic_6dof(RID p_joint, RID p_body_A, const Transform &p_local_frame_A, RID p_body_B, const Transform &p_local_frame_B) { +void PhysicsServer3DSW::joint_make_generic_6dof(RID p_joint, RID p_body_A, const Transform3D &p_local_frame_A, RID p_body_B, const Transform3D &p_local_frame_B) { Body3DSW *body_A = body_owner.getornull(p_body_A); ERR_FAIL_COND(!body_A); diff --git a/servers/physics_3d/physics_server_3d_sw.h b/servers/physics_3d/physics_server_3d_sw.h index 0b42f1d605..57b6385758 100644 --- a/servers/physics_3d/physics_server_3d_sw.h +++ b/servers/physics_3d/physics_server_3d_sw.h @@ -130,13 +130,13 @@ public: virtual void area_set_space(RID p_area, RID p_space) override; virtual RID area_get_space(RID p_area) const override; - virtual void area_add_shape(RID p_area, RID p_shape, const Transform &p_transform = Transform(), bool p_disabled = false) override; + virtual void area_add_shape(RID p_area, RID p_shape, const Transform3D &p_transform = Transform3D(), bool p_disabled = false) override; virtual void area_set_shape(RID p_area, int p_shape_idx, RID p_shape) override; - virtual void area_set_shape_transform(RID p_area, int p_shape_idx, const Transform &p_transform) override; + virtual void area_set_shape_transform(RID p_area, int p_shape_idx, const Transform3D &p_transform) override; virtual int area_get_shape_count(RID p_area) const override; virtual RID area_get_shape(RID p_area, int p_shape_idx) const override; - virtual Transform area_get_shape_transform(RID p_area, int p_shape_idx) const override; + virtual Transform3D area_get_shape_transform(RID p_area, int p_shape_idx) const override; virtual void area_remove_shape(RID p_area, int p_shape_idx) override; virtual void area_clear_shapes(RID p_area) override; @@ -147,10 +147,10 @@ public: virtual ObjectID area_get_object_instance_id(RID p_area) const override; virtual void area_set_param(RID p_area, AreaParameter p_param, const Variant &p_value) override; - virtual void area_set_transform(RID p_area, const Transform &p_transform) override; + virtual void area_set_transform(RID p_area, const Transform3D &p_transform) override; virtual Variant area_get_param(RID p_area, AreaParameter p_param) const override; - virtual Transform area_get_transform(RID p_area) const override; + virtual Transform3D area_get_transform(RID p_area) const override; virtual void area_set_ray_pickable(RID p_area, bool p_enable) override; @@ -173,13 +173,13 @@ public: virtual void body_set_mode(RID p_body, BodyMode p_mode) override; virtual BodyMode body_get_mode(RID p_body) const override; - virtual void body_add_shape(RID p_body, RID p_shape, const Transform &p_transform = Transform(), bool p_disabled = false) override; + virtual void body_add_shape(RID p_body, RID p_shape, const Transform3D &p_transform = Transform3D(), bool p_disabled = false) override; virtual void body_set_shape(RID p_body, int p_shape_idx, RID p_shape) override; - virtual void body_set_shape_transform(RID p_body, int p_shape_idx, const Transform &p_transform) override; + virtual void body_set_shape_transform(RID p_body, int p_shape_idx, const Transform3D &p_transform) override; virtual int body_get_shape_count(RID p_body) const override; virtual RID body_get_shape(RID p_body, int p_shape_idx) const override; - virtual Transform body_get_shape_transform(RID p_body, int p_shape_idx) const override; + virtual Transform3D body_get_shape_transform(RID p_body, int p_shape_idx) const override; virtual void body_set_shape_disabled(RID p_body, int p_shape_idx, bool p_disabled) override; @@ -204,9 +204,6 @@ public: virtual void body_set_param(RID p_body, BodyParameter p_param, real_t p_value) override; virtual real_t body_get_param(RID p_body, BodyParameter p_param) const override; - virtual void body_set_kinematic_safe_margin(RID p_body, real_t p_margin) override; - virtual real_t body_get_kinematic_safe_margin(RID p_body) const override; - virtual void body_set_state(RID p_body, BodyState p_state, const Variant &p_variant) override; virtual Variant body_get_state(RID p_body, BodyState p_state) const override; @@ -245,8 +242,8 @@ public: virtual void body_set_ray_pickable(RID p_body, bool p_enable) override; - virtual bool body_test_motion(RID p_body, const Transform &p_from, const Vector3 &p_motion, bool p_infinite_inertia, MotionResult *r_result = nullptr, bool p_exclude_raycast_shapes = true) override; - virtual int body_test_ray_separation(RID p_body, const Transform &p_transform, bool p_infinite_inertia, Vector3 &r_recover_motion, SeparationResult *r_results, int p_result_max, real_t p_margin = 0.001) override; + virtual bool body_test_motion(RID p_body, const Transform3D &p_from, const Vector3 &p_motion, bool p_infinite_inertia, real_t p_margin = 0.001, MotionResult *r_result = nullptr, bool p_exclude_raycast_shapes = true) override; + virtual int body_test_ray_separation(RID p_body, const Transform3D &p_transform, bool p_infinite_inertia, Vector3 &r_recover_motion, SeparationResult *r_results, int p_result_max, real_t p_margin = 0.001) override; // this function only works on physics process, errors and returns null otherwise virtual PhysicsDirectBodyState3D *body_get_direct_state(RID p_body) override; @@ -273,7 +270,7 @@ public: virtual void soft_body_set_state(RID p_body, BodyState p_state, const Variant &p_variant) override; virtual Variant soft_body_get_state(RID p_body, BodyState p_state) const override; - virtual void soft_body_set_transform(RID p_body, const Transform &p_transform) override; + virtual void soft_body_set_transform(RID p_body, const Transform3D &p_transform) override; virtual void soft_body_set_ray_pickable(RID p_body, bool p_enable) override; @@ -323,7 +320,7 @@ public: virtual void pin_joint_set_local_b(RID p_joint, const Vector3 &p_B) override; virtual Vector3 pin_joint_get_local_b(RID p_joint) const override; - virtual void joint_make_hinge(RID p_joint, RID p_body_A, const Transform &p_frame_A, RID p_body_B, const Transform &p_frame_B) override; + virtual void joint_make_hinge(RID p_joint, RID p_body_A, const Transform3D &p_frame_A, RID p_body_B, const Transform3D &p_frame_B) override; virtual void joint_make_hinge_simple(RID p_joint, RID p_body_A, const Vector3 &p_pivot_A, const Vector3 &p_axis_A, RID p_body_B, const Vector3 &p_pivot_B, const Vector3 &p_axis_B) override; virtual void hinge_joint_set_param(RID p_joint, HingeJointParam p_param, real_t p_value) override; @@ -332,17 +329,17 @@ public: virtual void hinge_joint_set_flag(RID p_joint, HingeJointFlag p_flag, bool p_value) override; virtual bool hinge_joint_get_flag(RID p_joint, HingeJointFlag p_flag) const override; - virtual void joint_make_slider(RID p_joint, RID p_body_A, const Transform &p_local_frame_A, RID p_body_B, const Transform &p_local_frame_B) override; //reference frame is A + virtual void joint_make_slider(RID p_joint, RID p_body_A, const Transform3D &p_local_frame_A, RID p_body_B, const Transform3D &p_local_frame_B) override; //reference frame is A virtual void slider_joint_set_param(RID p_joint, SliderJointParam p_param, real_t p_value) override; virtual real_t slider_joint_get_param(RID p_joint, SliderJointParam p_param) const override; - virtual void joint_make_cone_twist(RID p_joint, RID p_body_A, const Transform &p_local_frame_A, RID p_body_B, const Transform &p_local_frame_B) override; //reference frame is A + virtual void joint_make_cone_twist(RID p_joint, RID p_body_A, const Transform3D &p_local_frame_A, RID p_body_B, const Transform3D &p_local_frame_B) override; //reference frame is A virtual void cone_twist_joint_set_param(RID p_joint, ConeTwistJointParam p_param, real_t p_value) override; virtual real_t cone_twist_joint_get_param(RID p_joint, ConeTwistJointParam p_param) const override; - virtual void joint_make_generic_6dof(RID p_joint, RID p_body_A, const Transform &p_local_frame_A, RID p_body_B, const Transform &p_local_frame_B) override; //reference frame is A + virtual void joint_make_generic_6dof(RID p_joint, RID p_body_A, const Transform3D &p_local_frame_A, RID p_body_B, const Transform3D &p_local_frame_B) override; //reference frame is A virtual void generic_6dof_joint_set_param(RID p_joint, Vector3::Axis, G6DOFJointAxisParam p_param, real_t p_value) override; virtual real_t generic_6dof_joint_get_param(RID p_joint, Vector3::Axis, G6DOFJointAxisParam p_param) const override; diff --git a/servers/physics_3d/physics_server_3d_wrap_mt.h b/servers/physics_3d/physics_server_3d_wrap_mt.h index 69d0fcf3ed..bda2e30dd1 100644 --- a/servers/physics_3d/physics_server_3d_wrap_mt.h +++ b/servers/physics_3d/physics_server_3d_wrap_mt.h @@ -142,14 +142,14 @@ public: FUNC2(area_set_space_override_mode, RID, AreaSpaceOverrideMode); FUNC1RC(AreaSpaceOverrideMode, area_get_space_override_mode, RID); - FUNC4(area_add_shape, RID, RID, const Transform &, bool); + FUNC4(area_add_shape, RID, RID, const Transform3D &, bool); FUNC3(area_set_shape, RID, int, RID); - FUNC3(area_set_shape_transform, RID, int, const Transform &); + FUNC3(area_set_shape_transform, RID, int, const Transform3D &); FUNC3(area_set_shape_disabled, RID, int, bool); FUNC1RC(int, area_get_shape_count, RID); FUNC2RC(RID, area_get_shape, RID, int); - FUNC2RC(Transform, area_get_shape_transform, RID, int); + FUNC2RC(Transform3D, area_get_shape_transform, RID, int); FUNC2(area_remove_shape, RID, int); FUNC1(area_clear_shapes, RID); @@ -157,10 +157,10 @@ public: FUNC1RC(ObjectID, area_get_object_instance_id, RID); FUNC3(area_set_param, RID, AreaParameter, const Variant &); - FUNC2(area_set_transform, RID, const Transform &); + FUNC2(area_set_transform, RID, const Transform3D &); FUNC2RC(Variant, area_get_param, RID, AreaParameter); - FUNC1RC(Transform, area_get_transform, RID); + FUNC1RC(Transform3D, area_get_transform, RID); FUNC2(area_set_collision_mask, RID, uint32_t); FUNC2(area_set_collision_layer, RID, uint32_t); @@ -182,12 +182,12 @@ public: FUNC2(body_set_mode, RID, BodyMode); FUNC1RC(BodyMode, body_get_mode, RID); - FUNC4(body_add_shape, RID, RID, const Transform &, bool); + FUNC4(body_add_shape, RID, RID, const Transform3D &, bool); FUNC3(body_set_shape, RID, int, RID); - FUNC3(body_set_shape_transform, RID, int, const Transform &); + FUNC3(body_set_shape_transform, RID, int, const Transform3D &); FUNC1RC(int, body_get_shape_count, RID); - FUNC2RC(Transform, body_get_shape_transform, RID, int); + FUNC2RC(Transform3D, body_get_shape_transform, RID, int); FUNC2RC(RID, body_get_shape, RID, int); FUNC3(body_set_shape_disabled, RID, int, bool); @@ -213,9 +213,6 @@ public: FUNC3(body_set_param, RID, BodyParameter, real_t); FUNC2RC(real_t, body_get_param, RID, BodyParameter); - FUNC2(body_set_kinematic_safe_margin, RID, real_t); - FUNC1RC(real_t, body_get_kinematic_safe_margin, RID); - FUNC3(body_set_state, RID, BodyState, const Variant &); FUNC2RC(Variant, body_get_state, RID, BodyState); @@ -253,12 +250,12 @@ public: FUNC2(body_set_ray_pickable, RID, bool); - bool body_test_motion(RID p_body, const Transform &p_from, const Vector3 &p_motion, bool p_infinite_inertia, MotionResult *r_result = nullptr, bool p_exclude_raycast_shapes = true) override { + bool body_test_motion(RID p_body, const Transform3D &p_from, const Vector3 &p_motion, bool p_infinite_inertia, real_t p_margin = 0.001, MotionResult *r_result = nullptr, bool p_exclude_raycast_shapes = true) override { ERR_FAIL_COND_V(main_thread != Thread::get_caller_id(), false); - return physics_3d_server->body_test_motion(p_body, p_from, p_motion, p_infinite_inertia, r_result, p_exclude_raycast_shapes); + return physics_3d_server->body_test_motion(p_body, p_from, p_motion, p_infinite_inertia, p_margin, r_result, p_exclude_raycast_shapes); } - int body_test_ray_separation(RID p_body, const Transform &p_transform, bool p_infinite_inertia, Vector3 &r_recover_motion, SeparationResult *r_results, int p_result_max, real_t p_margin = 0.001) override { + int body_test_ray_separation(RID p_body, const Transform3D &p_transform, bool p_infinite_inertia, Vector3 &r_recover_motion, SeparationResult *r_results, int p_result_max, real_t p_margin = 0.001) override { ERR_FAIL_COND_V(main_thread != Thread::get_caller_id(), false); return physics_3d_server->body_test_ray_separation(p_body, p_transform, p_infinite_inertia, r_recover_motion, r_results, p_result_max, p_margin); } @@ -293,7 +290,7 @@ public: FUNC3(soft_body_set_state, RID, BodyState, const Variant &); FUNC2RC(Variant, soft_body_get_state, RID, BodyState); - FUNC2(soft_body_set_transform, RID, const Transform &); + FUNC2(soft_body_set_transform, RID, const Transform3D &); FUNC2(soft_body_set_simulation_precision, RID, int); FUNC1RC(int, soft_body_get_simulation_precision, RID); @@ -341,7 +338,7 @@ public: FUNC2(pin_joint_set_local_b, RID, const Vector3 &) FUNC1RC(Vector3, pin_joint_get_local_b, RID) - FUNC5(joint_make_hinge, RID, RID, const Transform &, RID, const Transform &) + FUNC5(joint_make_hinge, RID, RID, const Transform3D &, RID, const Transform3D &) FUNC7(joint_make_hinge_simple, RID, RID, const Vector3 &, const Vector3 &, RID, const Vector3 &, const Vector3 &) FUNC3(hinge_joint_set_param, RID, HingeJointParam, real_t) @@ -350,17 +347,17 @@ public: FUNC3(hinge_joint_set_flag, RID, HingeJointFlag, bool) FUNC2RC(bool, hinge_joint_get_flag, RID, HingeJointFlag) - FUNC5(joint_make_slider, RID, RID, const Transform &, RID, const Transform &) + FUNC5(joint_make_slider, RID, RID, const Transform3D &, RID, const Transform3D &) FUNC3(slider_joint_set_param, RID, SliderJointParam, real_t) FUNC2RC(real_t, slider_joint_get_param, RID, SliderJointParam) - FUNC5(joint_make_cone_twist, RID, RID, const Transform &, RID, const Transform &) + FUNC5(joint_make_cone_twist, RID, RID, const Transform3D &, RID, const Transform3D &) FUNC3(cone_twist_joint_set_param, RID, ConeTwistJointParam, real_t) FUNC2RC(real_t, cone_twist_joint_get_param, RID, ConeTwistJointParam) - FUNC5(joint_make_generic_6dof, RID, RID, const Transform &, RID, const Transform &) + FUNC5(joint_make_generic_6dof, RID, RID, const Transform3D &, RID, const Transform3D &) FUNC4(generic_6dof_joint_set_param, RID, Vector3::Axis, G6DOFJointAxisParam, real_t) FUNC3RC(real_t, generic_6dof_joint_get_param, RID, Vector3::Axis, G6DOFJointAxisParam) diff --git a/servers/physics_3d/shape_3d_sw.cpp b/servers/physics_3d/shape_3d_sw.cpp index ca7248993f..2ffab0c923 100644 --- a/servers/physics_3d/shape_3d_sw.cpp +++ b/servers/physics_3d/shape_3d_sw.cpp @@ -114,7 +114,7 @@ Plane PlaneShape3DSW::get_plane() const { return plane; } -void PlaneShape3DSW::project_range(const Vector3 &p_normal, const Transform &p_transform, real_t &r_min, real_t &r_max) const { +void PlaneShape3DSW::project_range(const Vector3 &p_normal, const Transform3D &p_transform, real_t &r_min, real_t &r_max) const { // gibberish, a plane is infinity r_min = -1e7; r_max = 1e7; @@ -174,7 +174,7 @@ bool RayShape3DSW::get_slips_on_slope() const { return slips_on_slope; } -void RayShape3DSW::project_range(const Vector3 &p_normal, const Transform &p_transform, real_t &r_min, real_t &r_max) const { +void RayShape3DSW::project_range(const Vector3 &p_normal, const Transform3D &p_transform, real_t &r_min, real_t &r_max) const { // don't think this will be even used r_min = 0; r_max = 1; @@ -255,7 +255,7 @@ real_t SphereShape3DSW::get_radius() const { return radius; } -void SphereShape3DSW::project_range(const Vector3 &p_normal, const Transform &p_transform, real_t &r_min, real_t &r_max) const { +void SphereShape3DSW::project_range(const Vector3 &p_normal, const Transform3D &p_transform, real_t &r_min, real_t &r_max) const { real_t d = p_normal.dot(p_transform.origin); // figure out scale at point @@ -317,7 +317,7 @@ SphereShape3DSW::SphereShape3DSW() { /********** BOX *************/ -void BoxShape3DSW::project_range(const Vector3 &p_normal, const Transform &p_transform, real_t &r_min, real_t &r_max) const { +void BoxShape3DSW::project_range(const Vector3 &p_normal, const Transform3D &p_transform, real_t &r_min, real_t &r_max) const { // no matter the angle, the box is mirrored anyway Vector3 local_normal = p_transform.basis.xform_inv(p_normal); @@ -507,7 +507,7 @@ BoxShape3DSW::BoxShape3DSW() { /********** CAPSULE *************/ -void CapsuleShape3DSW::project_range(const Vector3 &p_normal, const Transform &p_transform, real_t &r_min, real_t &r_max) const { +void CapsuleShape3DSW::project_range(const Vector3 &p_normal, const Transform3D &p_transform, real_t &r_min, real_t &r_max) const { Vector3 n = p_transform.basis.xform_inv(p_normal).normalized(); real_t h = (n.y > 0) ? height : -height; @@ -674,7 +674,7 @@ CapsuleShape3DSW::CapsuleShape3DSW() { /********** CYLINDER *************/ -void CylinderShape3DSW::project_range(const Vector3 &p_normal, const Transform &p_transform, real_t &r_min, real_t &r_max) const { +void CylinderShape3DSW::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(); real_t axis_dot = cylinder_axis.dot(p_normal); @@ -854,7 +854,7 @@ CylinderShape3DSW::CylinderShape3DSW() { /********** CONVEX POLYGON *************/ -void ConvexPolygonShape3DSW::project_range(const Vector3 &p_normal, const Transform &p_transform, real_t &r_min, real_t &r_max) const { +void ConvexPolygonShape3DSW::project_range(const Vector3 &p_normal, const Transform3D &p_transform, real_t &r_min, real_t &r_max) const { int vertex_count = mesh.vertices.size(); if (vertex_count == 0) { return; @@ -1120,7 +1120,7 @@ ConvexPolygonShape3DSW::ConvexPolygonShape3DSW() { /********** FACE POLYGON *************/ -void FaceShape3DSW::project_range(const Vector3 &p_normal, const Transform &p_transform, real_t &r_min, real_t &r_max) const { +void FaceShape3DSW::project_range(const Vector3 &p_normal, const Transform3D &p_transform, real_t &r_min, real_t &r_max) const { for (int i = 0; i < 3; i++) { Vector3 v = p_transform.xform(vertex[i]); real_t d = p_normal.dot(v); @@ -1250,7 +1250,7 @@ Vector<Vector3> ConcavePolygonShape3DSW::get_faces() const { return rfaces; } -void ConcavePolygonShape3DSW::project_range(const Vector3 &p_normal, const Transform &p_transform, real_t &r_min, real_t &r_max) const { +void ConcavePolygonShape3DSW::project_range(const Vector3 &p_normal, const Transform3D &p_transform, real_t &r_min, real_t &r_max) const { int count = vertices.size(); if (count == 0) { r_min = 0; @@ -1647,7 +1647,7 @@ int HeightMapShape3DSW::get_depth() const { return depth; } -void HeightMapShape3DSW::project_range(const Vector3 &p_normal, const Transform &p_transform, real_t &r_min, real_t &r_max) const { +void HeightMapShape3DSW::project_range(const Vector3 &p_normal, const Transform3D &p_transform, real_t &r_min, real_t &r_max) const { //not very useful, but not very used either p_transform.xform(get_aabb()).project_range_in_plane(Plane(p_normal, 0), r_min, r_max); } diff --git a/servers/physics_3d/shape_3d_sw.h b/servers/physics_3d/shape_3d_sw.h index 4d2b6ffbed..bc8bd3e695 100644 --- a/servers/physics_3d/shape_3d_sw.h +++ b/servers/physics_3d/shape_3d_sw.h @@ -86,7 +86,7 @@ public: virtual bool is_concave() const { return false; } - virtual void project_range(const Vector3 &p_normal, const Transform &p_transform, real_t &r_min, real_t &r_max) const = 0; + virtual void project_range(const Vector3 &p_normal, const Transform3D &p_transform, real_t &r_min, real_t &r_max) const = 0; virtual Vector3 get_support(const Vector3 &p_normal) const; virtual void get_supports(const Vector3 &p_normal, int p_max, Vector3 *r_supports, int &r_amount, FeatureType &r_type) const = 0; virtual Vector3 get_closest_point_to(const Vector3 &p_point) const = 0; @@ -130,7 +130,7 @@ public: virtual real_t get_area() const { return Math_INF; } virtual PhysicsServer3D::ShapeType get_type() const { return PhysicsServer3D::SHAPE_PLANE; } - virtual void project_range(const Vector3 &p_normal, const Transform &p_transform, real_t &r_min, real_t &r_max) const; + virtual void project_range(const Vector3 &p_normal, const Transform3D &p_transform, real_t &r_min, real_t &r_max) const; virtual Vector3 get_support(const Vector3 &p_normal) const; virtual void get_supports(const Vector3 &p_normal, int p_max, Vector3 *r_supports, int &r_amount, FeatureType &r_type) const { r_amount = 0; } @@ -157,7 +157,7 @@ public: virtual real_t get_area() const { return 0.0; } virtual PhysicsServer3D::ShapeType get_type() const { return PhysicsServer3D::SHAPE_RAY; } - virtual void project_range(const Vector3 &p_normal, const Transform &p_transform, real_t &r_min, real_t &r_max) const; + virtual void project_range(const Vector3 &p_normal, const Transform3D &p_transform, real_t &r_min, real_t &r_max) const; virtual Vector3 get_support(const Vector3 &p_normal) const; virtual void get_supports(const Vector3 &p_normal, int p_max, Vector3 *r_supports, int &r_amount, FeatureType &r_type) const; @@ -185,7 +185,7 @@ public: virtual PhysicsServer3D::ShapeType get_type() const { return PhysicsServer3D::SHAPE_SPHERE; } - virtual void project_range(const Vector3 &p_normal, const Transform &p_transform, real_t &r_min, real_t &r_max) const; + virtual void project_range(const Vector3 &p_normal, const Transform3D &p_transform, real_t &r_min, real_t &r_max) const; virtual Vector3 get_support(const Vector3 &p_normal) const; virtual void get_supports(const Vector3 &p_normal, int p_max, Vector3 *r_supports, int &r_amount, FeatureType &r_type) const; virtual bool intersect_segment(const Vector3 &p_begin, const Vector3 &p_end, Vector3 &r_result, Vector3 &r_normal) const; @@ -210,7 +210,7 @@ public: virtual PhysicsServer3D::ShapeType get_type() const { return PhysicsServer3D::SHAPE_BOX; } - virtual void project_range(const Vector3 &p_normal, const Transform &p_transform, real_t &r_min, real_t &r_max) const; + virtual void project_range(const Vector3 &p_normal, const Transform3D &p_transform, real_t &r_min, real_t &r_max) const; virtual Vector3 get_support(const Vector3 &p_normal) const; virtual void get_supports(const Vector3 &p_normal, int p_max, Vector3 *r_supports, int &r_amount, FeatureType &r_type) const; virtual bool intersect_segment(const Vector3 &p_begin, const Vector3 &p_end, Vector3 &r_result, Vector3 &r_normal) const; @@ -239,7 +239,7 @@ public: virtual PhysicsServer3D::ShapeType get_type() const { return PhysicsServer3D::SHAPE_CAPSULE; } - virtual void project_range(const Vector3 &p_normal, const Transform &p_transform, real_t &r_min, real_t &r_max) const; + virtual void project_range(const Vector3 &p_normal, const Transform3D &p_transform, real_t &r_min, real_t &r_max) const; virtual Vector3 get_support(const Vector3 &p_normal) const; virtual void get_supports(const Vector3 &p_normal, int p_max, Vector3 *r_supports, int &r_amount, FeatureType &r_type) const; virtual bool intersect_segment(const Vector3 &p_begin, const Vector3 &p_end, Vector3 &r_result, Vector3 &r_normal) const; @@ -268,7 +268,7 @@ public: virtual PhysicsServer3D::ShapeType get_type() const { return PhysicsServer3D::SHAPE_CYLINDER; } - virtual void project_range(const Vector3 &p_normal, const Transform &p_transform, real_t &r_min, real_t &r_max) const; + virtual void project_range(const Vector3 &p_normal, const Transform3D &p_transform, real_t &r_min, real_t &r_max) const; virtual Vector3 get_support(const Vector3 &p_normal) const; virtual void get_supports(const Vector3 &p_normal, int p_max, Vector3 *r_supports, int &r_amount, FeatureType &r_type) const; virtual bool intersect_segment(const Vector3 &p_begin, const Vector3 &p_end, Vector3 &r_result, Vector3 &r_normal) const; @@ -293,7 +293,7 @@ public: virtual PhysicsServer3D::ShapeType get_type() const { return PhysicsServer3D::SHAPE_CONVEX_POLYGON; } - virtual void project_range(const Vector3 &p_normal, const Transform &p_transform, real_t &r_min, real_t &r_max) const; + virtual void project_range(const Vector3 &p_normal, const Transform3D &p_transform, real_t &r_min, real_t &r_max) const; virtual Vector3 get_support(const Vector3 &p_normal) const; virtual void get_supports(const Vector3 &p_normal, int p_max, Vector3 *r_supports, int &r_amount, FeatureType &r_type) const; virtual bool intersect_segment(const Vector3 &p_begin, const Vector3 &p_end, Vector3 &r_result, Vector3 &r_normal) const; @@ -371,7 +371,7 @@ public: virtual PhysicsServer3D::ShapeType get_type() const { return PhysicsServer3D::SHAPE_CONCAVE_POLYGON; } - virtual void project_range(const Vector3 &p_normal, const Transform &p_transform, real_t &r_min, real_t &r_max) const; + virtual void project_range(const Vector3 &p_normal, const Transform3D &p_transform, real_t &r_min, real_t &r_max) const; virtual Vector3 get_support(const Vector3 &p_normal) const; virtual bool intersect_segment(const Vector3 &p_begin, const Vector3 &p_end, Vector3 &r_result, Vector3 &r_normal) const; @@ -415,7 +415,7 @@ public: virtual PhysicsServer3D::ShapeType get_type() const { return PhysicsServer3D::SHAPE_HEIGHTMAP; } - virtual void project_range(const Vector3 &p_normal, const Transform &p_transform, real_t &r_min, real_t &r_max) const; + virtual void project_range(const Vector3 &p_normal, const Transform3D &p_transform, real_t &r_min, real_t &r_max) const; virtual Vector3 get_support(const Vector3 &p_normal) const; virtual bool intersect_segment(const Vector3 &p_begin, const Vector3 &p_end, Vector3 &r_point, Vector3 &r_normal) const; virtual bool intersect_point(const Vector3 &p_point) const; @@ -441,7 +441,7 @@ struct FaceShape3DSW : public Shape3DSW { const Vector3 &get_vertex(int p_idx) const { return vertex[p_idx]; } - void project_range(const Vector3 &p_normal, const Transform &p_transform, real_t &r_min, real_t &r_max) const; + void project_range(const Vector3 &p_normal, const Transform3D &p_transform, real_t &r_min, real_t &r_max) const; Vector3 get_support(const Vector3 &p_normal) const; virtual void get_supports(const Vector3 &p_normal, int p_max, Vector3 *r_supports, int &r_amount, FeatureType &r_type) const; bool intersect_segment(const Vector3 &p_begin, const Vector3 &p_end, Vector3 &r_result, Vector3 &r_normal) const; @@ -462,11 +462,11 @@ struct MotionShape3DSW : public Shape3DSW { virtual PhysicsServer3D::ShapeType get_type() const { return PhysicsServer3D::SHAPE_CONVEX_POLYGON; } - void project_range(const Vector3 &p_normal, const Transform &p_transform, real_t &r_min, real_t &r_max) const { + void project_range(const Vector3 &p_normal, const Transform3D &p_transform, real_t &r_min, real_t &r_max) const { Vector3 cast = p_transform.basis.xform(motion); real_t mina, maxa; real_t minb, maxb; - Transform ofsb = p_transform; + Transform3D ofsb = p_transform; ofsb.origin += cast; shape->project_range(p_normal, p_transform, mina, maxa); shape->project_range(p_normal, ofsb, minb, maxb); diff --git a/servers/physics_3d/soft_body_3d_sw.cpp b/servers/physics_3d/soft_body_3d_sw.cpp index f63a470cbe..63a0fe11ba 100644 --- a/servers/physics_3d/soft_body_3d_sw.cpp +++ b/servers/physics_3d/soft_body_3d_sw.cpp @@ -289,7 +289,7 @@ void SoftBody3DSW::update_link_constants() { } } -void SoftBody3DSW::apply_nodes_transform(const Transform &p_transform) { +void SoftBody3DSW::apply_nodes_transform(const Transform3D &p_transform) { if (soft_mesh.is_null()) { return; } @@ -684,7 +684,7 @@ void SoftBody3DSW::generate_bending_constraints(int p_distance) { // // This function takes in a list of interdependent Links and tries // to maximize the distance between calculation -// of dependent links. This increases the amount of parallelism that can +// of dependent links. This increases the amount of parallelism that can // be exploited by out-of-order instruction processors with large but // (inevitably) finite instruction windows. // diff --git a/servers/physics_3d/soft_body_3d_sw.h b/servers/physics_3d/soft_body_3d_sw.h index 98e554218b..ac8bcbf0b9 100644 --- a/servers/physics_3d/soft_body_3d_sw.h +++ b/servers/physics_3d/soft_body_3d_sw.h @@ -201,7 +201,7 @@ private: void reset_link_rest_lengths(); void update_link_constants(); - void apply_nodes_transform(const Transform &p_transform); + void apply_nodes_transform(const Transform3D &p_transform); void add_velocity(const Vector3 &p_velocity); @@ -231,7 +231,7 @@ public: SoftBody3DSW *get_soft_body() const { return soft_body; } virtual PhysicsServer3D::ShapeType get_type() const { return PhysicsServer3D::SHAPE_SOFT_BODY; } - virtual void project_range(const Vector3 &p_normal, const Transform &p_transform, real_t &r_min, real_t &r_max) const { r_min = r_max = 0.0; } + virtual void project_range(const Vector3 &p_normal, const Transform3D &p_transform, real_t &r_min, real_t &r_max) const { r_min = r_max = 0.0; } virtual Vector3 get_support(const Vector3 &p_normal) const { return Vector3(); } virtual void get_supports(const Vector3 &p_normal, int p_max, Vector3 *r_supports, int &r_amount, FeatureType &r_type) const { r_amount = 0; } diff --git a/servers/physics_3d/space_3d_sw.cpp b/servers/physics_3d/space_3d_sw.cpp index 2df824b320..4392202084 100644 --- a/servers/physics_3d/space_3d_sw.cpp +++ b/servers/physics_3d/space_3d_sw.cpp @@ -59,7 +59,7 @@ int PhysicsDirectSpaceState3DSW::intersect_point(const Vector3 &p_point, ShapeRe int amount = space->broadphase->cull_point(p_point, space->intersection_query_results, Space3DSW::INTERSECTION_QUERY_MAX, space->intersection_query_subindex_results); int cc = 0; - //Transform ai = p_xform.affine_inverse(); + //Transform3D ai = p_xform.affine_inverse(); for (int i = 0; i < amount; i++) { if (cc >= p_result_max) { @@ -79,7 +79,7 @@ int PhysicsDirectSpaceState3DSW::intersect_point(const Vector3 &p_point, ShapeRe const CollisionObject3DSW *col_obj = space->intersection_query_results[i]; int shape_idx = space->intersection_query_subindex_results[i]; - Transform inv_xform = col_obj->get_transform() * col_obj->get_shape_transform(shape_idx); + Transform3D inv_xform = col_obj->get_transform() * col_obj->get_shape_transform(shape_idx); inv_xform.affine_invert(); if (!col_obj->get_shape(shape_idx)->intersect_point(inv_xform.xform(p_point))) { @@ -136,7 +136,7 @@ bool PhysicsDirectSpaceState3DSW::intersect_ray(const Vector3 &p_from, const Vec const CollisionObject3DSW *col_obj = space->intersection_query_results[i]; int shape_idx = space->intersection_query_subindex_results[i]; - Transform inv_xform = col_obj->get_shape_inv_transform(shape_idx) * col_obj->get_inv_transform(); + Transform3D inv_xform = col_obj->get_shape_inv_transform(shape_idx) * col_obj->get_inv_transform(); Vector3 local_from = inv_xform.xform(begin); Vector3 local_to = inv_xform.xform(end); @@ -146,7 +146,7 @@ bool PhysicsDirectSpaceState3DSW::intersect_ray(const Vector3 &p_from, const Vec Vector3 shape_point, shape_normal; if (shape->intersect_segment(local_from, local_to, shape_point, shape_normal)) { - Transform xform = col_obj->get_transform() * col_obj->get_shape_transform(shape_idx); + Transform3D xform = col_obj->get_transform() * col_obj->get_shape_transform(shape_idx); shape_point = xform.xform(shape_point); real_t ld = normal.dot(shape_point); @@ -180,7 +180,7 @@ bool PhysicsDirectSpaceState3DSW::intersect_ray(const Vector3 &p_from, const Vec return true; } -int PhysicsDirectSpaceState3DSW::intersect_shape(const RID &p_shape, const Transform &p_xform, real_t p_margin, ShapeResult *r_results, int p_result_max, const Set<RID> &p_exclude, uint32_t p_collision_mask, bool p_collide_with_bodies, bool p_collide_with_areas) { +int PhysicsDirectSpaceState3DSW::intersect_shape(const RID &p_shape, const Transform3D &p_xform, real_t p_margin, ShapeResult *r_results, int p_result_max, const Set<RID> &p_exclude, uint32_t p_collision_mask, bool p_collide_with_bodies, bool p_collide_with_areas) { if (p_result_max <= 0) { return 0; } @@ -194,7 +194,7 @@ int PhysicsDirectSpaceState3DSW::intersect_shape(const RID &p_shape, const Trans int cc = 0; - //Transform ai = p_xform.affine_inverse(); + //Transform3D ai = p_xform.affine_inverse(); for (int i = 0; i < amount; i++) { if (cc >= p_result_max) { @@ -239,7 +239,7 @@ int PhysicsDirectSpaceState3DSW::intersect_shape(const RID &p_shape, const Trans return cc; } -bool PhysicsDirectSpaceState3DSW::cast_motion(const RID &p_shape, const Transform &p_xform, const Vector3 &p_motion, real_t p_margin, real_t &p_closest_safe, real_t &p_closest_unsafe, const Set<RID> &p_exclude, uint32_t p_collision_mask, bool p_collide_with_bodies, bool p_collide_with_areas, ShapeRestInfo *r_info) { +bool PhysicsDirectSpaceState3DSW::cast_motion(const RID &p_shape, const Transform3D &p_xform, const Vector3 &p_motion, real_t p_margin, real_t &p_closest_safe, real_t &p_closest_unsafe, const Set<RID> &p_exclude, uint32_t p_collision_mask, bool p_collide_with_bodies, bool p_collide_with_areas, ShapeRestInfo *r_info) { Shape3DSW *shape = PhysicsServer3DSW::singletonsw->shape_owner.getornull(p_shape); ERR_FAIL_COND_V(!shape, false); @@ -252,7 +252,7 @@ bool PhysicsDirectSpaceState3DSW::cast_motion(const RID &p_shape, const Transfor real_t best_safe = 1; real_t best_unsafe = 1; - Transform xform_inv = p_xform.affine_inverse(); + Transform3D xform_inv = p_xform.affine_inverse(); MotionShape3DSW mshape; mshape.shape = shape; mshape.motion = xform_inv.basis.xform(p_motion); @@ -280,7 +280,7 @@ bool PhysicsDirectSpaceState3DSW::cast_motion(const RID &p_shape, const Transfor Vector3 point_A, point_B; Vector3 sep_axis = p_motion.normalized(); - Transform col_obj_xform = col_obj->get_transform() * col_obj->get_shape_transform(shape_idx); + Transform3D col_obj_xform = col_obj->get_transform() * col_obj->get_shape_transform(shape_idx); //test initial overlap, does it collide if going all the way? if (CollisionSolver3DSW::solve_distance(&mshape, p_xform, col_obj->get_shape(shape_idx), col_obj_xform, point_A, point_B, aabb, &sep_axis)) { continue; @@ -348,7 +348,7 @@ bool PhysicsDirectSpaceState3DSW::cast_motion(const RID &p_shape, const Transfor return true; } -bool PhysicsDirectSpaceState3DSW::collide_shape(RID p_shape, const Transform &p_shape_xform, real_t p_margin, Vector3 *r_results, int p_result_max, int &r_result_count, const Set<RID> &p_exclude, uint32_t p_collision_mask, bool p_collide_with_bodies, bool p_collide_with_areas) { +bool PhysicsDirectSpaceState3DSW::collide_shape(RID p_shape, const Transform3D &p_shape_xform, real_t p_margin, Vector3 *r_results, int p_result_max, int &r_result_count, const Set<RID> &p_exclude, uint32_t p_collision_mask, bool p_collide_with_bodies, bool p_collide_with_areas) { if (p_result_max <= 0) { return false; } @@ -432,7 +432,7 @@ static void _rest_cbk_result(const Vector3 &p_point_A, int p_index_A, const Vect rd->best_local_shape = rd->local_shape; } -bool PhysicsDirectSpaceState3DSW::rest_info(RID p_shape, const Transform &p_shape_xform, real_t p_margin, ShapeRestInfo *r_info, const Set<RID> &p_exclude, uint32_t p_collision_mask, bool p_collide_with_bodies, bool p_collide_with_areas) { +bool PhysicsDirectSpaceState3DSW::rest_info(RID p_shape, const Transform3D &p_shape_xform, real_t p_margin, ShapeRestInfo *r_info, const Set<RID> &p_exclude, uint32_t p_collision_mask, bool p_collide_with_bodies, bool p_collide_with_areas) { Shape3DSW *shape = PhysicsServer3DSW::singletonsw->shape_owner.getornull(p_shape); ERR_FAIL_COND_V(!shape, 0); @@ -512,7 +512,7 @@ Vector3 PhysicsDirectSpaceState3DSW::get_closest_point_to_object_volume(RID p_ob continue; } - Transform shape_xform = obj->get_transform() * obj->get_shape_transform(i); + Transform3D shape_xform = obj->get_transform() * obj->get_shape_transform(i); Shape3DSW *shape = obj->get_shape(i); Vector3 point = shape->get_closest_point_to(shape_xform.affine_inverse().xform(p_point)); @@ -573,7 +573,7 @@ int Space3DSW::_cull_aabb_for_body(Body3DSW *p_body, const AABB &p_aabb) { return amount; } -int Space3DSW::test_body_ray_separation(Body3DSW *p_body, const Transform &p_transform, bool p_infinite_inertia, Vector3 &r_recover_motion, PhysicsServer3D::SeparationResult *r_results, int p_result_max, real_t p_margin) { +int Space3DSW::test_body_ray_separation(Body3DSW *p_body, const Transform3D &p_transform, bool p_infinite_inertia, Vector3 &r_recover_motion, PhysicsServer3D::SeparationResult *r_results, int p_result_max, real_t p_margin) { AABB body_aabb; bool shapes_found = false; @@ -598,7 +598,7 @@ int Space3DSW::test_body_ray_separation(Body3DSW *p_body, const Transform &p_tra body_aabb = p_transform.xform(p_body->get_inv_transform().xform(body_aabb)); body_aabb = body_aabb.grow(p_margin); - Transform body_transform = p_transform; + Transform3D body_transform = p_transform; for (int i = 0; i < p_result_max; i++) { //reset results @@ -636,7 +636,7 @@ int Space3DSW::test_body_ray_separation(Body3DSW *p_body, const Transform &p_tra continue; } - Transform body_shape_xform = body_transform * p_body->get_shape_transform(j); + Transform3D body_shape_xform = body_transform * p_body->get_shape_transform(j); for (int i = 0; i < amount; i++) { const CollisionObject3DSW *col_obj = intersection_query_results[i]; @@ -724,7 +724,7 @@ int Space3DSW::test_body_ray_separation(Body3DSW *p_body, const Transform &p_tra return rays_found; } -bool Space3DSW::test_body_motion(Body3DSW *p_body, const Transform &p_from, const Vector3 &p_motion, bool p_infinite_inertia, real_t p_margin, PhysicsServer3D::MotionResult *r_result, bool p_exclude_raycast_shapes) { +bool Space3DSW::test_body_motion(Body3DSW *p_body, const Transform3D &p_from, const Vector3 &p_motion, bool p_infinite_inertia, real_t p_margin, PhysicsServer3D::MotionResult *r_result, bool p_exclude_raycast_shapes) { //give me back regular physics engine logic //this is madness //and most people using this function will think @@ -768,7 +768,7 @@ bool Space3DSW::test_body_motion(Body3DSW *p_body, const Transform &p_from, cons real_t motion_length = p_motion.length(); Vector3 motion_normal = p_motion / motion_length; - Transform body_transform = p_from; + Transform3D body_transform = p_from; bool recovered = false; @@ -797,7 +797,7 @@ bool Space3DSW::test_body_motion(Body3DSW *p_body, const Transform &p_from, cons continue; } - Transform body_shape_xform = body_transform * p_body->get_shape_transform(j); + Transform3D body_shape_xform = body_transform * p_body->get_shape_transform(j); Shape3DSW *body_shape = p_body->get_shape(j); if (p_exclude_raycast_shapes && body_shape->get_type() == PhysicsServer3D::SHAPE_RAY) { continue; @@ -868,14 +868,14 @@ bool Space3DSW::test_body_motion(Body3DSW *p_body, const Transform &p_from, cons continue; } - Transform body_shape_xform = body_transform * p_body->get_shape_transform(j); + Transform3D body_shape_xform = body_transform * p_body->get_shape_transform(j); Shape3DSW *body_shape = p_body->get_shape(j); if (p_exclude_raycast_shapes && body_shape->get_type() == PhysicsServer3D::SHAPE_RAY) { continue; } - Transform body_shape_xform_inv = body_shape_xform.affine_inverse(); + Transform3D body_shape_xform_inv = body_shape_xform.affine_inverse(); MotionShape3DSW mshape; mshape.shape = body_shape; mshape.motion = body_shape_xform_inv.basis.xform(p_motion); @@ -893,7 +893,7 @@ bool Space3DSW::test_body_motion(Body3DSW *p_body, const Transform &p_from, cons Vector3 point_A, point_B; Vector3 sep_axis = motion_normal; - Transform col_obj_xform = col_obj->get_transform() * col_obj->get_shape_transform(shape_idx); + Transform3D col_obj_xform = col_obj->get_transform() * col_obj->get_shape_transform(shape_idx); //test initial overlap, does it collide if going all the way? if (CollisionSolver3DSW::solve_distance(&mshape, body_shape_xform, col_obj->get_shape(shape_idx), col_obj_xform, point_A, point_B, motion_aabb, &sep_axis)) { continue; @@ -960,7 +960,7 @@ bool Space3DSW::test_body_motion(Body3DSW *p_body, const Transform &p_from, cons } //it collided, let's get the rest info in unsafe advance - Transform ugt = body_transform; + Transform3D ugt = body_transform; ugt.origin += p_motion * unsafe; _RestCallbackData rcd; @@ -979,7 +979,7 @@ bool Space3DSW::test_body_motion(Body3DSW *p_body, const Transform &p_from, cons continue; } - Transform body_shape_xform = ugt * p_body->get_shape_transform(j); + Transform3D body_shape_xform = ugt * p_body->get_shape_transform(j); Shape3DSW *body_shape = p_body->get_shape(j); if (p_exclude_raycast_shapes && body_shape->get_type() == PhysicsServer3D::SHAPE_RAY) { diff --git a/servers/physics_3d/space_3d_sw.h b/servers/physics_3d/space_3d_sw.h index 3a8f452e54..18e93c90cc 100644 --- a/servers/physics_3d/space_3d_sw.h +++ b/servers/physics_3d/space_3d_sw.h @@ -50,10 +50,10 @@ public: virtual int intersect_point(const Vector3 &p_point, ShapeResult *r_results, int p_result_max, const Set<RID> &p_exclude = Set<RID>(), uint32_t p_collision_mask = 0xFFFFFFFF, bool p_collide_with_bodies = true, bool p_collide_with_areas = false) override; virtual bool intersect_ray(const Vector3 &p_from, const Vector3 &p_to, RayResult &r_result, const Set<RID> &p_exclude = Set<RID>(), uint32_t p_collision_mask = 0xFFFFFFFF, bool p_collide_with_bodies = true, bool p_collide_with_areas = false, bool p_pick_ray = false) override; - virtual int intersect_shape(const RID &p_shape, const Transform &p_xform, real_t p_margin, ShapeResult *r_results, int p_result_max, const Set<RID> &p_exclude = Set<RID>(), uint32_t p_collision_mask = 0xFFFFFFFF, bool p_collide_with_bodies = true, bool p_collide_with_areas = false) override; - virtual bool cast_motion(const RID &p_shape, const Transform &p_xform, const Vector3 &p_motion, real_t p_margin, real_t &p_closest_safe, real_t &p_closest_unsafe, const Set<RID> &p_exclude = Set<RID>(), uint32_t p_collision_mask = 0xFFFFFFFF, bool p_collide_with_bodies = true, bool p_collide_with_areas = false, ShapeRestInfo *r_info = nullptr) override; - virtual bool collide_shape(RID p_shape, const Transform &p_shape_xform, real_t p_margin, Vector3 *r_results, int p_result_max, int &r_result_count, const Set<RID> &p_exclude = Set<RID>(), uint32_t p_collision_mask = 0xFFFFFFFF, bool p_collide_with_bodies = true, bool p_collide_with_areas = false) override; - virtual bool rest_info(RID p_shape, const Transform &p_shape_xform, real_t p_margin, ShapeRestInfo *r_info, const Set<RID> &p_exclude = Set<RID>(), uint32_t p_collision_mask = 0xFFFFFFFF, bool p_collide_with_bodies = true, bool p_collide_with_areas = false) override; + virtual int intersect_shape(const RID &p_shape, const Transform3D &p_xform, real_t p_margin, ShapeResult *r_results, int p_result_max, const Set<RID> &p_exclude = Set<RID>(), uint32_t p_collision_mask = 0xFFFFFFFF, bool p_collide_with_bodies = true, bool p_collide_with_areas = false) override; + virtual bool cast_motion(const RID &p_shape, const Transform3D &p_xform, const Vector3 &p_motion, real_t p_margin, real_t &p_closest_safe, real_t &p_closest_unsafe, const Set<RID> &p_exclude = Set<RID>(), uint32_t p_collision_mask = 0xFFFFFFFF, bool p_collide_with_bodies = true, bool p_collide_with_areas = false, ShapeRestInfo *r_info = nullptr) override; + virtual bool collide_shape(RID p_shape, const Transform3D &p_shape_xform, real_t p_margin, Vector3 *r_results, int p_result_max, int &r_result_count, const Set<RID> &p_exclude = Set<RID>(), uint32_t p_collision_mask = 0xFFFFFFFF, bool p_collide_with_bodies = true, bool p_collide_with_areas = false) override; + virtual bool rest_info(RID p_shape, const Transform3D &p_shape_xform, real_t p_margin, ShapeRestInfo *r_info, const Set<RID> &p_exclude = Set<RID>(), uint32_t p_collision_mask = 0xFFFFFFFF, bool p_collide_with_bodies = true, bool p_collide_with_areas = false) override; virtual Vector3 get_closest_point_to_object_volume(RID p_object, const Vector3 p_point) const override; PhysicsDirectSpaceState3DSW(); @@ -203,8 +203,8 @@ public: void set_elapsed_time(ElapsedTime p_time, uint64_t p_msec) { elapsed_time[p_time] = p_msec; } uint64_t get_elapsed_time(ElapsedTime p_time) const { return elapsed_time[p_time]; } - int test_body_ray_separation(Body3DSW *p_body, const Transform &p_transform, bool p_infinite_inertia, Vector3 &r_recover_motion, PhysicsServer3D::SeparationResult *r_results, int p_result_max, real_t p_margin); - bool test_body_motion(Body3DSW *p_body, const Transform &p_from, const Vector3 &p_motion, bool p_infinite_inertia, real_t p_margin, PhysicsServer3D::MotionResult *r_result, bool p_exclude_raycast_shapes); + int test_body_ray_separation(Body3DSW *p_body, const Transform3D &p_transform, bool p_infinite_inertia, Vector3 &r_recover_motion, PhysicsServer3D::SeparationResult *r_results, int p_result_max, real_t p_margin); + bool test_body_motion(Body3DSW *p_body, const Transform3D &p_from, const Vector3 &p_motion, bool p_infinite_inertia, real_t p_margin, PhysicsServer3D::MotionResult *r_result, bool p_exclude_raycast_shapes); Space3DSW(); ~Space3DSW(); |