diff options
Diffstat (limited to 'servers/physics')
-rw-r--r-- | servers/physics/body_pair_sw.cpp | 12 | ||||
-rw-r--r-- | servers/physics/body_sw.h | 14 | ||||
-rw-r--r-- | servers/physics/collision_object_sw.cpp | 18 | ||||
-rw-r--r-- | servers/physics/collision_object_sw.h | 2 | ||||
-rw-r--r-- | servers/physics/collision_solver_sat.cpp | 69 | ||||
-rw-r--r-- | servers/physics/physics_server_sw.cpp | 45 | ||||
-rw-r--r-- | servers/physics/physics_server_sw.h | 71 | ||||
-rw-r--r-- | servers/physics/space_sw.h | 2 |
8 files changed, 211 insertions, 22 deletions
diff --git a/servers/physics/body_pair_sw.cpp b/servers/physics/body_pair_sw.cpp index 2a6a9e08ae..0ce38e4486 100644 --- a/servers/physics/body_pair_sw.cpp +++ b/servers/physics/body_pair_sw.cpp @@ -211,6 +211,14 @@ bool BodyPairSW::_test_ccd(real_t p_step, BodySW *p_A, int p_shape_A, const Tran return true; } +real_t combine_bounce(BodySW *A, BodySW *B) { + return CLAMP(A->get_bounce() + B->get_bounce(), 0, 1); +} + +real_t combine_friction(BodySW *A, BodySW *B) { + return ABS(MIN(A->get_friction(), B->get_friction())); +} + bool BodyPairSW::setup(real_t p_step) { //cannot collide @@ -331,7 +339,7 @@ bool BodyPairSW::setup(real_t p_step) { c.acc_bias_impulse = 0; c.acc_bias_impulse_center_of_mass = 0; - c.bounce = MAX(A->get_bounce(), B->get_bounce()); + c.bounce = combine_bounce(A, B); if (c.bounce) { Vector3 crA = A->get_angular_velocity().cross(c.rA); @@ -421,7 +429,7 @@ void BodyPairSW::solve(real_t p_step) { //friction impulse - real_t friction = A->get_friction() * B->get_friction(); + real_t friction = combine_friction(A, B); Vector3 lvA = A->get_linear_velocity() + A->get_angular_velocity().cross(c.rA); Vector3 lvB = B->get_linear_velocity() + B->get_angular_velocity().cross(c.rB); diff --git a/servers/physics/body_sw.h b/servers/physics/body_sw.h index fd2ab16b84..9d7b147fd6 100644 --- a/servers/physics/body_sw.h +++ b/servers/physics/body_sw.h @@ -157,7 +157,7 @@ public: _FORCE_INLINE_ void add_area(AreaSW *p_area) { int index = areas.find(AreaCMP(p_area)); if (index > -1) { - areas[index].refCount += 1; + areas.write[index].refCount += 1; } else { areas.ordered_insert(AreaCMP(p_area)); } @@ -166,7 +166,7 @@ public: _FORCE_INLINE_ void remove_area(AreaSW *p_area) { int index = areas.find(AreaCMP(p_area)); if (index > -1) { - areas[index].refCount -= 1; + areas.write[index].refCount -= 1; if (areas[index].refCount < 1) areas.remove(index); } @@ -217,6 +217,10 @@ public: _FORCE_INLINE_ const Vector3 &get_biased_linear_velocity() const { return biased_linear_velocity; } _FORCE_INLINE_ const Vector3 &get_biased_angular_velocity() const { return biased_angular_velocity; } + _FORCE_INLINE_ void apply_central_impulse(const Vector3 &p_j) { + linear_velocity += p_j * _inv_mass; + } + _FORCE_INLINE_ void apply_impulse(const Vector3 &p_pos, const Vector3 &p_j) { linear_velocity += p_j * _inv_mass; @@ -344,7 +348,7 @@ void BodySW::add_contact(const Vector3 &p_local_pos, const Vector3 &p_local_norm if (c_max == 0) return; - Contact *c = &contacts[0]; + Contact *c = contacts.ptrw(); int idx = -1; @@ -413,6 +417,7 @@ public: virtual void add_central_force(const Vector3 &p_force) { body->add_central_force(p_force); } virtual void add_force(const Vector3 &p_force, const Vector3 &p_pos) { body->add_force(p_force, p_pos); } virtual void add_torque(const Vector3 &p_torque) { body->add_torque(p_torque); } + virtual void apply_central_impulse(const Vector3 &p_j) { body->apply_central_impulse(p_j); } virtual void apply_impulse(const Vector3 &p_pos, const Vector3 &p_j) { body->apply_impulse(p_pos, p_j); } virtual void apply_torque_impulse(const Vector3 &p_j) { body->apply_torque_impulse(p_j); } @@ -429,6 +434,9 @@ public: ERR_FAIL_INDEX_V(p_contact_idx, body->contact_count, Vector3()); return body->contacts[p_contact_idx].local_normal; } + virtual float get_contact_impulse(int p_contact_idx) const { + return 0.0f; // Only implemented for bullet + } virtual int get_contact_local_shape(int p_contact_idx) const { ERR_FAIL_INDEX_V(p_contact_idx, body->contact_count, -1); return body->contacts[p_contact_idx].local_shape; diff --git a/servers/physics/collision_object_sw.cpp b/servers/physics/collision_object_sw.cpp index f7a58a9cf2..09f72ff39b 100644 --- a/servers/physics/collision_object_sw.cpp +++ b/servers/physics/collision_object_sw.cpp @@ -53,7 +53,7 @@ void CollisionObjectSW::set_shape(int p_index, ShapeSW *p_shape) { ERR_FAIL_INDEX(p_index, shapes.size()); shapes[p_index].shape->remove_owner(this); - shapes[p_index].shape = p_shape; + shapes.write[p_index].shape = p_shape; p_shape->add_owner(this); if (!pending_shape_update_list.in_list()) { @@ -66,8 +66,8 @@ void CollisionObjectSW::set_shape_transform(int p_index, const Transform &p_tran ERR_FAIL_INDEX(p_index, shapes.size()); - shapes[p_index].xform = p_transform; - shapes[p_index].xform_inv = p_transform.affine_inverse(); + shapes.write[p_index].xform = p_transform; + shapes.write[p_index].xform_inv = p_transform.affine_inverse(); if (!pending_shape_update_list.in_list()) { PhysicsServerSW::singleton->pending_shape_update_list.add(&pending_shape_update_list); } @@ -97,7 +97,7 @@ void CollisionObjectSW::remove_shape(int p_index) { continue; //should never get here with a null owner space->get_broadphase()->remove(shapes[i].bpid); - shapes[i].bpid = 0; + shapes.write[i].bpid = 0; } shapes[p_index].shape->remove_owner(this); shapes.remove(p_index); @@ -117,7 +117,7 @@ void CollisionObjectSW::_set_static(bool p_static) { if (!space) return; for (int i = 0; i < get_shape_count(); i++) { - Shape &s = shapes[i]; + const Shape &s = shapes[i]; if (s.bpid > 0) { space->get_broadphase()->set_static(s.bpid, _static); } @@ -128,7 +128,7 @@ void CollisionObjectSW::_unregister_shapes() { for (int i = 0; i < shapes.size(); i++) { - Shape &s = shapes[i]; + Shape &s = shapes.write[i]; if (s.bpid > 0) { space->get_broadphase()->remove(s.bpid); s.bpid = 0; @@ -143,7 +143,7 @@ void CollisionObjectSW::_update_shapes() { for (int i = 0; i < shapes.size(); i++) { - Shape &s = shapes[i]; + Shape &s = shapes.write[i]; if (s.bpid == 0) { s.bpid = space->get_broadphase()->create(this, i); space->get_broadphase()->set_static(s.bpid, _static); @@ -170,7 +170,7 @@ void CollisionObjectSW::_update_shapes_with_motion(const Vector3 &p_motion) { for (int i = 0; i < shapes.size(); i++) { - Shape &s = shapes[i]; + Shape &s = shapes.write[i]; if (s.bpid == 0) { s.bpid = space->get_broadphase()->create(this, i); space->get_broadphase()->set_static(s.bpid, _static); @@ -195,7 +195,7 @@ void CollisionObjectSW::_set_space(SpaceSW *p_space) { for (int i = 0; i < shapes.size(); i++) { - Shape &s = shapes[i]; + Shape &s = shapes.write[i]; if (s.bpid) { space->get_broadphase()->remove(s.bpid); s.bpid = 0; diff --git a/servers/physics/collision_object_sw.h b/servers/physics/collision_object_sw.h index dee28bb6df..b6430b38dc 100644 --- a/servers/physics/collision_object_sw.h +++ b/servers/physics/collision_object_sw.h @@ -135,7 +135,7 @@ public: _FORCE_INLINE_ void set_ray_pickable(bool p_enable) { ray_pickable = p_enable; } _FORCE_INLINE_ bool is_ray_pickable() const { return ray_pickable; } - _FORCE_INLINE_ void set_shape_as_disabled(int p_idx, bool p_enable) { shapes[p_idx].disabled = p_enable; } + _FORCE_INLINE_ void set_shape_as_disabled(int p_idx, bool p_enable) { shapes.write[p_idx].disabled = p_enable; } _FORCE_INLINE_ bool is_shape_set_as_disabled(int p_idx) const { return shapes[p_idx].disabled; } _FORCE_INLINE_ void set_collision_layer(uint32_t p_layer) { collision_layer = p_layer; } diff --git a/servers/physics/collision_solver_sat.cpp b/servers/physics/collision_solver_sat.cpp index e587485fcb..b059c20c95 100644 --- a/servers/physics/collision_solver_sat.cpp +++ b/servers/physics/collision_solver_sat.cpp @@ -217,8 +217,6 @@ static void _generate_contacts_face_face(const Vector3 *p_points_A, int p_point_ // generate contacts //Plane plane_A(p_points_A[0],p_points_A[1],p_points_A[2]); - int added = 0; - for (int i = 0; i < clipbuf_len; i++) { real_t d = plane_B.distance_to(clipbuf_src[i]); @@ -233,7 +231,6 @@ static void _generate_contacts_face_face(const Vector3 *p_points_A, int p_point_ continue; p_callback->call(clipbuf_src[i], closest_B); - added++; } } @@ -351,7 +348,9 @@ public: //use the smallest depth - min_B = -min_B; + if (min_B < 0.0) { // could be +0.0, we don't want it to become -0.0 + min_B = -min_B; + } if (max_B < min_B) { if (max_B < best_depth) { @@ -561,6 +560,12 @@ static void _collision_sphere_capsule(const ShapeSW *p_a, const Transform &p_tra } template <bool withMargin> +static void _collision_sphere_cylinder(const ShapeSW *p_a, const Transform &p_transform_a, const ShapeSW *p_b, const Transform &p_transform_b, _CollectorCallback *p_collector, real_t p_margin_a, real_t p_margin_b) { + + return; +} + +template <bool withMargin> static void _collision_sphere_convex_polygon(const ShapeSW *p_a, const Transform &p_transform_a, const ShapeSW *p_b, const Transform &p_transform_b, _CollectorCallback *p_collector, real_t p_margin_a, real_t p_margin_b) { const SphereShapeSW *sphere_A = static_cast<const SphereShapeSW *>(p_a); @@ -852,6 +857,12 @@ static void _collision_box_capsule(const ShapeSW *p_a, const Transform &p_transf } template <bool withMargin> +static void _collision_box_cylinder(const ShapeSW *p_a, const Transform &p_transform_a, const ShapeSW *p_b, const Transform &p_transform_b, _CollectorCallback *p_collector, real_t p_margin_a, real_t p_margin_b) { + + return; +} + +template <bool withMargin> static void _collision_box_convex_polygon(const ShapeSW *p_a, const Transform &p_transform_a, const ShapeSW *p_b, const Transform &p_transform_b, _CollectorCallback *p_collector, real_t p_margin_a, real_t p_margin_b) { const BoxShapeSW *box_A = static_cast<const BoxShapeSW *>(p_a); @@ -1128,6 +1139,12 @@ static void _collision_capsule_capsule(const ShapeSW *p_a, const Transform &p_tr } template <bool withMargin> +static void _collision_capsule_cylinder(const ShapeSW *p_a, const Transform &p_transform_a, const ShapeSW *p_b, const Transform &p_transform_b, _CollectorCallback *p_collector, real_t p_margin_a, real_t p_margin_b) { + + return; +} + +template <bool withMargin> static void _collision_capsule_convex_polygon(const ShapeSW *p_a, const Transform &p_transform_a, const ShapeSW *p_b, const Transform &p_transform_b, _CollectorCallback *p_collector, real_t p_margin_a, real_t p_margin_b) { const CapsuleShapeSW *capsule_A = static_cast<const CapsuleShapeSW *>(p_a); @@ -1248,6 +1265,24 @@ static void _collision_capsule_face(const ShapeSW *p_a, const Transform &p_trans } template <bool withMargin> +static void _collision_cylinder_cylinder(const ShapeSW *p_a, const Transform &p_transform_a, const ShapeSW *p_b, const Transform &p_transform_b, _CollectorCallback *p_collector, real_t p_margin_a, real_t p_margin_b) { + + return; +} + +template <bool withMargin> +static void _collision_cylinder_convex_polygon(const ShapeSW *p_a, const Transform &p_transform_a, const ShapeSW *p_b, const Transform &p_transform_b, _CollectorCallback *p_collector, real_t p_margin_a, real_t p_margin_b) { + + return; +} + +template <bool withMargin> +static void _collision_cylinder_face(const ShapeSW *p_a, const Transform &p_transform_a, const ShapeSW *p_b, const Transform &p_transform_b, _CollectorCallback *p_collector, real_t p_margin_a, real_t p_margin_b) { + + return; +} + +template <bool withMargin> static void _collision_convex_polygon_convex_polygon(const ShapeSW *p_a, const Transform &p_transform_a, const ShapeSW *p_b, const Transform &p_transform_b, _CollectorCallback *p_collector, real_t p_margin_a, real_t p_margin_b) { const ConvexPolygonShapeSW *convex_polygon_A = static_cast<const ConvexPolygonShapeSW *>(p_a); @@ -1476,59 +1511,81 @@ bool sat_calculate_penetration(const ShapeSW *p_shape_A, const Transform &p_tran ERR_FAIL_COND_V(type_B == PhysicsServer::SHAPE_RAY, false); ERR_FAIL_COND_V(p_shape_B->is_concave(), false); - static const CollisionFunc collision_table[5][5] = { + static const CollisionFunc collision_table[6][6] = { { _collision_sphere_sphere<false>, _collision_sphere_box<false>, _collision_sphere_capsule<false>, + _collision_sphere_cylinder<false>, _collision_sphere_convex_polygon<false>, _collision_sphere_face<false> }, { 0, _collision_box_box<false>, _collision_box_capsule<false>, + _collision_box_cylinder<false>, _collision_box_convex_polygon<false>, _collision_box_face<false> }, { 0, 0, _collision_capsule_capsule<false>, + _collision_capsule_cylinder<false>, _collision_capsule_convex_polygon<false>, _collision_capsule_face<false> }, { 0, 0, 0, + _collision_cylinder_cylinder<false>, + _collision_cylinder_convex_polygon<false>, + _collision_cylinder_face<false> }, + { 0, + 0, + 0, + 0, _collision_convex_polygon_convex_polygon<false>, _collision_convex_polygon_face<false> }, { 0, 0, 0, 0, + 0, 0 }, }; - static const CollisionFunc collision_table_margin[5][5] = { + static const CollisionFunc collision_table_margin[6][6] = { { _collision_sphere_sphere<true>, _collision_sphere_box<true>, _collision_sphere_capsule<true>, + _collision_sphere_cylinder<true>, _collision_sphere_convex_polygon<true>, _collision_sphere_face<true> }, { 0, _collision_box_box<true>, _collision_box_capsule<true>, + _collision_box_cylinder<true>, _collision_box_convex_polygon<true>, _collision_box_face<true> }, { 0, 0, _collision_capsule_capsule<true>, + _collision_capsule_cylinder<true>, _collision_capsule_convex_polygon<true>, _collision_capsule_face<true> }, { 0, 0, 0, + _collision_cylinder_cylinder<true>, + _collision_cylinder_convex_polygon<true>, + _collision_cylinder_face<true> }, + { 0, + 0, + 0, + 0, _collision_convex_polygon_convex_polygon<true>, _collision_convex_polygon_face<true> }, { 0, 0, 0, 0, + 0, 0 }, }; diff --git a/servers/physics/physics_server_sw.cpp b/servers/physics/physics_server_sw.cpp index f2dbb635f8..3a32c46a9b 100644 --- a/servers/physics/physics_server_sw.cpp +++ b/servers/physics/physics_server_sw.cpp @@ -65,6 +65,11 @@ RID PhysicsServerSW::shape_create(ShapeType p_shape) { shape = memnew(CapsuleShapeSW); } break; + case SHAPE_CYLINDER: { + + ERR_EXPLAIN("CylinderShape is not supported in GodotPhysics. Please switch to Bullet in the Project Settings."); + ERR_FAIL_V(RID()); + } break; case SHAPE_CONVEX_POLYGON: { shape = memnew(ConvexPolygonShapeSW); @@ -758,6 +763,40 @@ Vector3 PhysicsServerSW::body_get_applied_torque(RID p_body) const { return body->get_applied_torque(); }; +void PhysicsServerSW::body_add_central_force(RID p_body, const Vector3 &p_force) { + BodySW *body = body_owner.get(p_body); + ERR_FAIL_COND(!body); + + body->add_central_force(p_force); + body->wakeup(); +} + +void PhysicsServerSW::body_add_force(RID p_body, const Vector3 &p_force, const Vector3 &p_pos) { + BodySW *body = body_owner.get(p_body); + ERR_FAIL_COND(!body); + + body->add_force(p_force, p_pos); + body->wakeup(); +}; + +void PhysicsServerSW::body_add_torque(RID p_body, const Vector3 &p_torque) { + BodySW *body = body_owner.get(p_body); + ERR_FAIL_COND(!body); + + body->add_torque(p_torque); + body->wakeup(); +}; + +void PhysicsServerSW::body_apply_central_impulse(RID p_body, const Vector3 &p_impulse) { + BodySW *body = body_owner.get(p_body); + ERR_FAIL_COND(!body); + + _update_shapes(); + + body->apply_central_impulse(p_impulse); + body->wakeup(); +} + void PhysicsServerSW::body_apply_impulse(RID p_body, const Vector3 &p_pos, const Vector3 &p_impulse) { BodySW *body = body_owner.get(p_body); @@ -1362,6 +1401,8 @@ void PhysicsServerSW::init() { void PhysicsServerSW::step(real_t p_step) { +#ifndef _3D_DISABLED + if (!active) return; @@ -1382,6 +1423,7 @@ void PhysicsServerSW::step(real_t p_step) { active_objects += E->get()->get_active_objects(); collision_pairs += E->get()->get_collision_pairs(); } +#endif } void PhysicsServerSW::sync(){ @@ -1390,6 +1432,8 @@ void PhysicsServerSW::sync(){ void PhysicsServerSW::flush_queries() { +#ifndef _3D_DISABLED + if (!active) return; @@ -1436,6 +1480,7 @@ void PhysicsServerSW::flush_queries() { ScriptDebugger::get_singleton()->add_profiling_frame_data("physics", values); } +#endif }; void PhysicsServerSW::finish() { diff --git a/servers/physics/physics_server_sw.h b/servers/physics/physics_server_sw.h index 3f56ba26d0..1c5754124d 100644 --- a/servers/physics/physics_server_sw.h +++ b/servers/physics/physics_server_sw.h @@ -200,6 +200,11 @@ public: virtual void body_set_applied_torque(RID p_body, const Vector3 &p_torque); virtual Vector3 body_get_applied_torque(RID p_body) const; + virtual void body_add_central_force(RID p_body, const Vector3 &p_force); + virtual void body_add_force(RID p_body, const Vector3 &p_force, const Vector3 &p_pos); + virtual void body_add_torque(RID p_body, const Vector3 &p_torque); + + virtual void body_apply_central_impulse(RID p_body, const Vector3 &p_impulse); virtual void body_apply_impulse(RID p_body, const Vector3 &p_pos, const Vector3 &p_impulse); virtual void body_apply_torque_impulse(RID p_body, const Vector3 &p_impulse); virtual void body_set_axis_velocity(RID p_body, const Vector3 &p_axis_velocity); @@ -230,6 +235,72 @@ public: // this function only works on physics process, errors and returns null otherwise virtual PhysicsDirectBodyState *body_get_direct_state(RID p_body); + /* SOFT BODY */ + + virtual RID soft_body_create(bool p_init_sleeping = false) { return RID(); } + + virtual void soft_body_update_visual_server(RID p_body, class SoftBodyVisualServerHandler *p_visual_server_handler) {} + + virtual void soft_body_set_space(RID p_body, RID p_space) {} + virtual RID soft_body_get_space(RID p_body) const { return RID(); } + + virtual void soft_body_set_collision_layer(RID p_body, uint32_t p_layer) {} + virtual uint32_t soft_body_get_collision_layer(RID p_body) const { return 0; } + + virtual void soft_body_set_collision_mask(RID p_body, uint32_t p_mask) {} + virtual uint32_t soft_body_get_collision_mask(RID p_body) const { return 0; } + + virtual void soft_body_add_collision_exception(RID p_body, RID p_body_b) {} + virtual void soft_body_remove_collision_exception(RID p_body, RID p_body_b) {} + virtual void soft_body_get_collision_exceptions(RID p_body, List<RID> *p_exceptions) {} + + virtual void soft_body_set_state(RID p_body, BodyState p_state, const Variant &p_variant) {} + virtual Variant soft_body_get_state(RID p_body, BodyState p_state) const { return Variant(); } + + virtual void soft_body_set_transform(RID p_body, const Transform &p_transform) {} + virtual Vector3 soft_body_get_vertex_position(RID p_body, int vertex_index) const { return Vector3(); } + + virtual void soft_body_set_ray_pickable(RID p_body, bool p_enable) {} + virtual bool soft_body_is_ray_pickable(RID p_body) const { return false; } + + virtual void soft_body_set_simulation_precision(RID p_body, int p_simulation_precision) {} + virtual int soft_body_get_simulation_precision(RID p_body) { return 0; } + + virtual void soft_body_set_total_mass(RID p_body, real_t p_total_mass) {} + virtual real_t soft_body_get_total_mass(RID p_body) { return 0.; } + + virtual void soft_body_set_linear_stiffness(RID p_body, real_t p_stiffness) {} + virtual real_t soft_body_get_linear_stiffness(RID p_body) { return 0.; } + + virtual void soft_body_set_areaAngular_stiffness(RID p_body, real_t p_stiffness) {} + virtual real_t soft_body_get_areaAngular_stiffness(RID p_body) { return 0.; } + + virtual void soft_body_set_volume_stiffness(RID p_body, real_t p_stiffness) {} + virtual real_t soft_body_get_volume_stiffness(RID p_body) { return 0.; } + + virtual void soft_body_set_pressure_coefficient(RID p_body, real_t p_pressure_coefficient) {} + virtual real_t soft_body_get_pressure_coefficient(RID p_body) { return 0.; } + + virtual void soft_body_set_pose_matching_coefficient(RID p_body, real_t p_pose_matching_coefficient) {} + virtual real_t soft_body_get_pose_matching_coefficient(RID p_body) { return 0.; } + + virtual void soft_body_set_damping_coefficient(RID p_body, real_t p_damping_coefficient) {} + virtual real_t soft_body_get_damping_coefficient(RID p_body) { return 0.; } + + virtual void soft_body_set_drag_coefficient(RID p_body, real_t p_drag_coefficient) {} + virtual real_t soft_body_get_drag_coefficient(RID p_body) { return 0.; } + + virtual void soft_body_set_mesh(RID p_body, const REF &p_mesh) {} + + virtual void soft_body_move_point(RID p_body, int p_point_index, const Vector3 &p_global_position) {} + virtual Vector3 soft_body_get_point_global_position(RID p_body, int p_point_index) { return Vector3(); } + + virtual Vector3 soft_body_get_point_offset(RID p_body, int p_point_index) const { return Vector3(); } + + virtual void soft_body_remove_all_pinned_points(RID p_body) {} + virtual void soft_body_pin_point(RID p_body, int p_point_index, bool p_pin) {} + virtual bool soft_body_is_point_pinned(RID p_body, int p_point_index) { return 0; } + /* JOINT API */ virtual RID joint_create_pin(RID p_body_A, const Vector3 &p_local_A, RID p_body_B, const Vector3 &p_local_B); diff --git a/servers/physics/space_sw.h b/servers/physics/space_sw.h index 2452d6a187..4d864e9a51 100644 --- a/servers/physics/space_sw.h +++ b/servers/physics/space_sw.h @@ -186,7 +186,7 @@ public: void set_debug_contacts(int p_amount) { contact_debug.resize(p_amount); } _FORCE_INLINE_ bool is_debugging_contacts() const { return !contact_debug.empty(); } _FORCE_INLINE_ void add_debug_contact(const Vector3 &p_contact) { - if (contact_debug_count < contact_debug.size()) contact_debug[contact_debug_count++] = p_contact; + if (contact_debug_count < contact_debug.size()) contact_debug.write[contact_debug_count++] = p_contact; } _FORCE_INLINE_ Vector<Vector3> get_debug_contacts() { return contact_debug; } _FORCE_INLINE_ int get_debug_contact_count() { return contact_debug_count; } |