seedlingattempt/godot
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

Merge pull request #81610 from Ughuuu/add-angle-limits-and-motor-to-pin-joint-2d

Browse Source 
Update PinJoint2D API with angle limits and motor speed
This commit is contained in:
Rémi Verschelde
2023-09-26 13:45:07 +02:00
parent 93cf990cdb 0fcfb07246
commit eefe161549
17 changed files with 361 additions and 9 deletions
Download Patch File Download Diff File Expand all files Collapse all files

115
servers/physics_2d/godot_joints_2d.cpp
View File

@ -155,6 +155,9 @@ bool GodotPinJoint2D::setup(real_t p_step) {
bias = delta * -(get_bias() == 0 ? space->get_constraint_bias() : get_bias()) * (1.0 / p_step);
// Compute max impulse.
jn_max = get_max_force() * p_step;
return true;
}
@ -170,12 +173,41 @@ bool GodotPinJoint2D::pre_solve(real_t p_step) {
if (B && dynamic_B) {
B->apply_impulse(P, rB);
}
// Angle limits joint pre_solve step taken from https://github.com/slembcke/Chipmunk2D/blob/d0239ef4599b3688a5a336373f7d0a68426414ba/src/cpRotaryLimitJoint.c
real_t i_sum_local = A->get_inv_inertia();
if (B) {
i_sum_local += B->get_inv_inertia();
}
i_sum = 1.0 / (i_sum_local);
if (angular_limit_enabled && B) {
Vector2 diff_vector = B->get_transform().get_origin() - A->get_transform().get_origin();
diff_vector = diff_vector.rotated(-initial_angle);
real_t dist = diff_vector.angle();
real_t pdist = 0.0;
if (dist > angular_limit_upper) {
pdist = dist - angular_limit_upper;
} else if (dist < angular_limit_lower) {
pdist = dist - angular_limit_lower;
}
real_t error_bias = Math::pow(1.0 - 0.15, 60.0);
// Calculate bias velocity.
bias_velocity = -CLAMP((-1.0 - Math::pow(error_bias, p_step)) * pdist / p_step, -get_max_bias(), get_max_bias());
// If the bias velocity is 0, the joint is not at a limit.
if (bias_velocity >= -CMP_EPSILON && bias_velocity <= CMP_EPSILON) {
j_acc = 0;
is_joint_at_limit = false;
} else {
is_joint_at_limit = true;
}
} else {
bias_velocity = 0.0;
}
return true;
}
void GodotPinJoint2D::solve(real_t p_step) {
// compute relative velocity
// Compute relative velocity.
Vector2 vA = A->get_linear_velocity() - custom_cross(rA - A->get_center_of_mass(), A->get_angular_velocity());
Vector2 rel_vel;
@ -184,6 +216,33 @@ void GodotPinJoint2D::solve(real_t p_step) {
} else {
rel_vel = -vA;
}
// Angle limits joint solve step taken from https://github.com/slembcke/Chipmunk2D/blob/d0239ef4599b3688a5a336373f7d0a68426414ba/src/cpRotaryLimitJoint.c
if ((angular_limit_enabled || motor_enabled) && B) {
// Compute relative rotational velocity.
real_t wr = B->get_angular_velocity() - A->get_angular_velocity();
// Motor solve part taken from https://github.com/slembcke/Chipmunk2D/blob/d0239ef4599b3688a5a336373f7d0a68426414ba/src/cpSimpleMotor.c
if (motor_enabled) {
wr -= motor_target_velocity;
}
real_t j_max = jn_max;
// Compute normal impulse.
real_t j = -(bias_velocity + wr) * i_sum;
real_t j_old = j_acc;
// Only enable the limits if we have to.
if (angular_limit_enabled && is_joint_at_limit) {
if (bias_velocity < 0.0) {
j_acc = CLAMP(j_old + j, 0.0, j_max);
} else {
j_acc = CLAMP(j_old + j, -j_max, 0.0);
}
} else {
j_acc = CLAMP(j_old + j, -j_max, j_max);
}
j = j_acc - j_old;
A->apply_torque_impulse(-j * A->get_inv_inertia());
B->apply_torque_impulse(j * B->get_inv_inertia());
}
Vector2 impulse = M.basis_xform(bias - rel_vel - Vector2(softness, softness) * P);
@ -198,14 +257,59 @@ void GodotPinJoint2D::solve(real_t p_step) {
}
void GodotPinJoint2D::set_param(PhysicsServer2D::PinJointParam p_param, real_t p_value) {
if (p_param == PhysicsServer2D::PIN_JOINT_SOFTNESS) {
softness = p_value;
switch (p_param) {
case PhysicsServer2D::PIN_JOINT_SOFTNESS: {
softness = p_value;
} break;
case PhysicsServer2D::PIN_JOINT_LIMIT_UPPER: {
angular_limit_upper = p_value;
} break;
case PhysicsServer2D::PIN_JOINT_LIMIT_LOWER: {
angular_limit_lower = p_value;
} break;
case PhysicsServer2D::PIN_JOINT_MOTOR_TARGET_VELOCITY: {
motor_target_velocity = p_value;
} break;
}
}
real_t GodotPinJoint2D::get_param(PhysicsServer2D::PinJointParam p_param) const {
if (p_param == PhysicsServer2D::PIN_JOINT_SOFTNESS) {
return softness;
switch (p_param) {
case PhysicsServer2D::PIN_JOINT_SOFTNESS: {
return softness;
}
case PhysicsServer2D::PIN_JOINT_LIMIT_UPPER: {
return angular_limit_upper;
}
case PhysicsServer2D::PIN_JOINT_LIMIT_LOWER: {
return angular_limit_lower;
}
case PhysicsServer2D::PIN_JOINT_MOTOR_TARGET_VELOCITY: {
return motor_target_velocity;
}
}
ERR_FAIL_V(0);
}
void GodotPinJoint2D::set_flag(PhysicsServer2D::PinJointFlag p_flag, bool p_enabled) {
switch (p_flag) {
case PhysicsServer2D::PIN_JOINT_FLAG_ANGULAR_LIMIT_ENABLED: {
angular_limit_enabled = p_enabled;
} break;
case PhysicsServer2D::PIN_JOINT_FLAG_MOTOR_ENABLED: {
motor_enabled = p_enabled;
} break;
}
}
bool GodotPinJoint2D::get_flag(PhysicsServer2D::PinJointFlag p_flag) const {
switch (p_flag) {
case PhysicsServer2D::PIN_JOINT_FLAG_ANGULAR_LIMIT_ENABLED: {
return angular_limit_enabled;
}
case PhysicsServer2D::PIN_JOINT_FLAG_MOTOR_ENABLED: {
return motor_enabled;
}
}
ERR_FAIL_V(0);
}
@ -220,6 +324,7 @@ GodotPinJoint2D::GodotPinJoint2D(const Vector2 &p_pos, GodotBody2D *p_body_a, Go
p_body_a->add_constraint(this, 0);
if (p_body_b) {
p_body_b->add_constraint(this, 1);
initial_angle = A->get_transform().get_origin().angle_to_point(B->get_transform().get_origin());
}
}

14
servers/physics_2d/godot_joints_2d.h
View File

@ -88,8 +88,19 @@ class GodotPinJoint2D : public GodotJoint2D {
Vector2 anchor_A;
Vector2 anchor_B;
Vector2 bias;
real_t initial_angle = 0.0;
real_t bias_velocity = 0.0;
real_t jn_max = 0.0;
real_t j_acc = 0.0;
real_t i_sum = 0.0;
Vector2 P;
real_t softness = 0.0;
real_t angular_limit_lower = 0.0;
real_t angular_limit_upper = 0.0;
real_t motor_target_velocity = 0.0;
bool is_joint_at_limit = false;
bool motor_enabled = false;
bool angular_limit_enabled = false;
public:
virtual PhysicsServer2D::JointType get_type() const override { return PhysicsServer2D::JOINT_TYPE_PIN; }
@ -101,6 +112,9 @@ public:
void set_param(PhysicsServer2D::PinJointParam p_param, real_t p_value);
real_t get_param(PhysicsServer2D::PinJointParam p_param) const;
void set_flag(PhysicsServer2D::PinJointFlag p_flag, bool p_enabled);
bool get_flag(PhysicsServer2D::PinJointFlag p_flag) const;
GodotPinJoint2D(const Vector2 &p_pos, GodotBody2D *p_body_a, GodotBody2D *p_body_b = nullptr);
};

18
servers/physics_2d/godot_physics_server_2d.cpp
View File

@ -1158,6 +1158,24 @@ void GodotPhysicsServer2D::joint_make_damped_spring(RID p_joint, const Vector2 &
memdelete(prev_joint);
}
void GodotPhysicsServer2D::pin_joint_set_flag(RID p_joint, PinJointFlag p_flag, bool p_enabled) {
GodotJoint2D *joint = joint_owner.get_or_null(p_joint);
ERR_FAIL_NULL(joint);
ERR_FAIL_COND(joint->get_type() != JOINT_TYPE_PIN);
GodotPinJoint2D *pin_joint = static_cast<GodotPinJoint2D *>(joint);
pin_joint->set_flag(p_flag, p_enabled);
}
bool GodotPhysicsServer2D::pin_joint_get_flag(RID p_joint, PinJointFlag p_flag) const {
GodotJoint2D *joint = joint_owner.get_or_null(p_joint);
ERR_FAIL_NULL_V(joint, 0);
ERR_FAIL_COND_V(joint->get_type() != JOINT_TYPE_PIN, 0);
GodotPinJoint2D *pin_joint = static_cast<GodotPinJoint2D *>(joint);
return pin_joint->get_flag(p_flag);
}
void GodotPhysicsServer2D::pin_joint_set_param(RID p_joint, PinJointParam p_param, real_t p_value) {
GodotJoint2D *joint = joint_owner.get_or_null(p_joint);
ERR_FAIL_NULL(joint);

2
servers/physics_2d/godot_physics_server_2d.h
View File

@ -275,6 +275,8 @@ public:
virtual void joint_make_groove(RID p_joint, const Vector2 &p_a_groove1, const Vector2 &p_a_groove2, const Vector2 &p_b_anchor, RID p_body_a, RID p_body_b) override;
virtual void joint_make_damped_spring(RID p_joint, const Vector2 &p_anchor_a, const Vector2 &p_anchor_b, RID p_body_a, RID p_body_b = RID()) override;
virtual void pin_joint_set_flag(RID p_joint, PinJointFlag p_flag, bool p_enabled) override;
virtual bool pin_joint_get_flag(RID p_joint, PinJointFlag p_flag) const override;
virtual void pin_joint_set_param(RID p_joint, PinJointParam p_param, real_t p_value) override;
virtual real_t pin_joint_get_param(RID p_joint, PinJointParam p_param) const override;
virtual void damped_spring_joint_set_param(RID p_joint, DampedSpringParam p_param, real_t p_value) override;