Sara/behaviour-tree-test
1
0
Fork 0
Code Issues Pull requests Projects Releases 2 Packages Wiki Activity Actions

Godot Physics collisions and solver processed on threads

Browse source 
Use ThreadWorkPool to process physics step tasks in multiple threads. Collisions are all processed in parallel and solving impulses is
processed in parallel for rigid body islands.

Additional changes:
- Proper islands for soft bodies linked to active bodies
- All moving areas are on separate islands (can be parallelized)
- Fix inconsistencies with body islands (Kinematic bodies could link
bodies together or not depending on the processing order)
- Completely prevent static bodies to be active (it could cause islands
to be wrongly created and cause dangerous multi-threading operations as
well as inconsistencies in created islands)
- Apply impulses only on dynamic bodies to avoid unsafe multi-threaded
operations (static bodies can be on multiple islands)
- Removed inverted iterations when populating body islands, it's now
faster in regular order (maybe after fixing inconsistencies)
This commit is contained in:
PouleyKetchoupp 2021-04-19 18:38:11 -07:00
parent 639b02f454
commit 448c41a3e4
31 changed files with 894 additions and 466 deletions
Download patch file Download diff file Expand all files Collapse all files

29
servers/physics_3d/joints/cone_twist_joint_3d_sw.cpp
View file

@ -109,7 +109,10 @@ ConeTwistJoint3DSW::ConeTwistJoint3DSW(Body3DSW *rbA, Body3DSW *rbB, const Trans
}
bool ConeTwistJoint3DSW::setup(real_t p_timestep) {
if ((A->get_mode() <= PhysicsServer3D::BODY_MODE_KINEMATIC) && (B->get_mode() <= PhysicsServer3D::BODY_MODE_KINEMATIC)) {
dynamic_A = (A->get_mode() > PhysicsServer3D::BODY_MODE_KINEMATIC);
dynamic_B = (B->get_mode() > PhysicsServer3D::BODY_MODE_KINEMATIC);
if (!dynamic_A && !dynamic_B) {
return false;
}
@ -265,8 +268,12 @@ void ConeTwistJoint3DSW::solve(real_t p_timestep) {
real_t impulse = depth * tau / p_timestep * jacDiagABInv - rel_vel * jacDiagABInv;
m_appliedImpulse += impulse;
Vector3 impulse_vector = normal * impulse;
A->apply_impulse(impulse_vector, pivotAInW - A->get_transform().origin);
B->apply_impulse(-impulse_vector, pivotBInW - B->get_transform().origin);
if (dynamic_A) {
A->apply_impulse(impulse_vector, pivotAInW - A->get_transform().origin);
}
if (dynamic_B) {
B->apply_impulse(-impulse_vector, pivotBInW - B->get_transform().origin);
}
}
}
@ -287,8 +294,12 @@ void ConeTwistJoint3DSW::solve(real_t p_timestep) {
Vector3 impulse = m_swingAxis * impulseMag;
A->apply_torque_impulse(impulse);
B->apply_torque_impulse(-impulse);
if (dynamic_A) {
A->apply_torque_impulse(impulse);
}
if (dynamic_B) {
B->apply_torque_impulse(-impulse);
}
}
// solve twist limit
@ -303,8 +314,12 @@ void ConeTwistJoint3DSW::solve(real_t p_timestep) {
Vector3 impulse = m_twistAxis * impulseMag;
A->apply_torque_impulse(impulse);
B->apply_torque_impulse(-impulse);
if (dynamic_A) {
A->apply_torque_impulse(impulse);
}
if (dynamic_B) {
B->apply_torque_impulse(-impulse);
}
}
}
}

6
servers/physics_3d/joints/cone_twist_joint_3d_sw.h
View file

@ -102,10 +102,10 @@ public:
bool m_solveSwingLimit;
public:
virtual PhysicsServer3D::JointType get_type() const { return PhysicsServer3D::JOINT_TYPE_CONE_TWIST; }
virtual PhysicsServer3D::JointType get_type() const override { return PhysicsServer3D::JOINT_TYPE_CONE_TWIST; }
virtual bool setup(real_t p_timestep);
virtual void solve(real_t p_timestep);
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);

25
servers/physics_3d/joints/generic_6dof_joint_3d_sw.cpp
View file

@ -82,7 +82,7 @@ int G6DOFRotationalLimitMotor3DSW::testLimitValue(real_t test_value) {
real_t G6DOFRotationalLimitMotor3DSW::solveAngularLimits(
real_t timeStep, Vector3 &axis, real_t jacDiagABInv,
Body3DSW *body0, Body3DSW *body1) {
Body3DSW *body0, Body3DSW *body1, bool p_body0_dynamic, bool p_body1_dynamic) {
if (!needApplyTorques()) {
return 0.0f;
}
@ -138,8 +138,10 @@ real_t G6DOFRotationalLimitMotor3DSW::solveAngularLimits(
Vector3 motorImp = clippedMotorImpulse * axis;
body0->apply_torque_impulse(motorImp);
if (body1) {
if (p_body0_dynamic) {
body0->apply_torque_impulse(motorImp);
}
if (body1 && p_body1_dynamic) {
body1->apply_torque_impulse(-motorImp);
}
@ -154,6 +156,7 @@ real_t G6DOFTranslationalLimitMotor3DSW::solveLinearAxis(
real_t jacDiagABInv,
Body3DSW *body1, const Vector3 &pointInA,
Body3DSW *body2, const Vector3 &pointInB,
bool p_body1_dynamic, bool p_body2_dynamic,
int limit_index,
const Vector3 &axis_normal_on_a,
const Vector3 &anchorPos) {
@ -205,8 +208,12 @@ real_t G6DOFTranslationalLimitMotor3DSW::solveLinearAxis(
normalImpulse = m_accumulatedImpulse[limit_index] - oldNormalImpulse;
Vector3 impulse_vector = axis_normal_on_a * normalImpulse;
body1->apply_impulse(impulse_vector, rel_pos1);
body2->apply_impulse(-impulse_vector, rel_pos2);
if (p_body1_dynamic) {
body1->apply_impulse(impulse_vector, rel_pos1);
}
if (p_body2_dynamic) {
body2->apply_impulse(-impulse_vector, rel_pos2);
}
return normalImpulse;
}
@ -303,7 +310,10 @@ bool Generic6DOFJoint3DSW::testAngularLimitMotor(int axis_index) {
}
bool Generic6DOFJoint3DSW::setup(real_t p_timestep) {
if ((A->get_mode() <= PhysicsServer3D::BODY_MODE_KINEMATIC) && (B->get_mode() <= PhysicsServer3D::BODY_MODE_KINEMATIC)) {
dynamic_A = (A->get_mode() > PhysicsServer3D::BODY_MODE_KINEMATIC);
dynamic_B = (B->get_mode() > PhysicsServer3D::BODY_MODE_KINEMATIC);
if (!dynamic_A && !dynamic_B) {
return false;
}
@ -384,6 +394,7 @@ void Generic6DOFJoint3DSW::solve(real_t p_timestep) {
jacDiagABInv,
A, pointInA,
B, pointInB,
dynamic_A, dynamic_B,
i, linear_axis, m_AnchorPos);
}
}
@ -398,7 +409,7 @@ void Generic6DOFJoint3DSW::solve(real_t p_timestep) {
angularJacDiagABInv = real_t(1.) / m_jacAng[i].getDiagonal();
m_angularLimits[i].solveAngularLimits(m_timeStep, angular_axis, angularJacDiagABInv, A, B);
m_angularLimits[i].solveAngularLimits(m_timeStep, angular_axis, angularJacDiagABInv, A, B, dynamic_A, dynamic_B);
}
}
}

9
servers/physics_3d/joints/generic_6dof_joint_3d_sw.h
View file

@ -120,7 +120,7 @@ public:
int testLimitValue(real_t test_value);
//! apply the correction impulses for two bodies
real_t solveAngularLimits(real_t timeStep, Vector3 &axis, real_t jacDiagABInv, Body3DSW *body0, Body3DSW *body1);
real_t solveAngularLimits(real_t timeStep, Vector3 &axis, real_t jacDiagABInv, Body3DSW *body0, Body3DSW *body1, bool p_body0_dynamic, bool p_body1_dynamic);
};
class G6DOFTranslationalLimitMotor3DSW {
@ -166,6 +166,7 @@ public:
real_t jacDiagABInv,
Body3DSW *body1, const Vector3 &pointInA,
Body3DSW *body2, const Vector3 &pointInB,
bool p_body1_dynamic, bool p_body2_dynamic,
int limit_index,
const Vector3 &axis_normal_on_a,
const Vector3 &anchorPos);
@ -234,10 +235,10 @@ protected:
public:
Generic6DOFJoint3DSW(Body3DSW *rbA, Body3DSW *rbB, const Transform &frameInA, const Transform &frameInB, bool useLinearReferenceFrameA);
virtual PhysicsServer3D::JointType get_type() const { return PhysicsServer3D::JOINT_TYPE_6DOF; }
virtual PhysicsServer3D::JointType get_type() const override { return PhysicsServer3D::JOINT_TYPE_6DOF; }
virtual bool setup(real_t p_timestep);
virtual void solve(real_t p_timestep);
virtual bool setup(real_t p_step) override;
virtual void solve(real_t p_step) override;
//! Calcs global transform of the offsets
/*!

37
servers/physics_3d/joints/hinge_joint_3d_sw.cpp
View file

@ -155,7 +155,10 @@ HingeJoint3DSW::HingeJoint3DSW(Body3DSW *rbA, Body3DSW *rbB, const Vector3 &pivo
}
bool HingeJoint3DSW::setup(real_t p_step) {
if ((A->get_mode() <= PhysicsServer3D::BODY_MODE_KINEMATIC) && (B->get_mode() <= PhysicsServer3D::BODY_MODE_KINEMATIC)) {
dynamic_A = (A->get_mode() > PhysicsServer3D::BODY_MODE_KINEMATIC);
dynamic_B = (B->get_mode() > PhysicsServer3D::BODY_MODE_KINEMATIC);
if (!dynamic_A && !dynamic_B) {
return false;
}
@ -279,8 +282,12 @@ void HingeJoint3DSW::solve(real_t p_step) {
real_t impulse = depth * tau / p_step * jacDiagABInv - rel_vel * jacDiagABInv;
m_appliedImpulse += impulse;
Vector3 impulse_vector = normal * impulse;
A->apply_impulse(impulse_vector, pivotAInW - A->get_transform().origin);
B->apply_impulse(-impulse_vector, pivotBInW - B->get_transform().origin);
if (dynamic_A) {
A->apply_impulse(impulse_vector, pivotAInW - A->get_transform().origin);
}
if (dynamic_B) {
B->apply_impulse(-impulse_vector, pivotBInW - B->get_transform().origin);
}
}
}
@ -322,8 +329,12 @@ void HingeJoint3DSW::solve(real_t p_step) {
angularError *= (real_t(1.) / denom2) * relaxation;
}
A->apply_torque_impulse(-velrelOrthog + angularError);
B->apply_torque_impulse(velrelOrthog - angularError);
if (dynamic_A) {
A->apply_torque_impulse(-velrelOrthog + angularError);
}
if (dynamic_B) {
B->apply_torque_impulse(velrelOrthog - angularError);
}
// solve limit
if (m_solveLimit) {
@ -337,8 +348,12 @@ void HingeJoint3DSW::solve(real_t p_step) {
impulseMag = m_accLimitImpulse - temp;
Vector3 impulse = axisA * impulseMag * m_limitSign;
A->apply_torque_impulse(impulse);
B->apply_torque_impulse(-impulse);
if (dynamic_A) {
A->apply_torque_impulse(impulse);
}
if (dynamic_B) {
B->apply_torque_impulse(-impulse);
}
}
}
@ -359,8 +374,12 @@ void HingeJoint3DSW::solve(real_t p_step) {
clippedMotorImpulse = clippedMotorImpulse < -m_maxMotorImpulse ? -m_maxMotorImpulse : clippedMotorImpulse;
Vector3 motorImp = clippedMotorImpulse * axisA;
A->apply_torque_impulse(motorImp + angularLimit);
B->apply_torque_impulse(-motorImp - angularLimit);
if (dynamic_A) {
A->apply_torque_impulse(motorImp + angularLimit);
}
if (dynamic_B) {
B->apply_torque_impulse(-motorImp - angularLimit);
}
}
}
}

6
servers/physics_3d/joints/hinge_joint_3d_sw.h
View file

@ -96,10 +96,10 @@ class HingeJoint3DSW : public Joint3DSW {
real_t m_appliedImpulse;
public:
virtual PhysicsServer3D::JointType get_type() const { return PhysicsServer3D::JOINT_TYPE_HINGE; }
virtual PhysicsServer3D::JointType get_type() const override { return PhysicsServer3D::JOINT_TYPE_HINGE; }
virtual bool setup(real_t p_step);
virtual void solve(real_t p_step);
virtual bool setup(real_t p_step) override;
virtual void solve(real_t p_step) override;
real_t get_hinge_angle();

13
servers/physics_3d/joints/pin_joint_3d_sw.cpp
View file

@ -50,7 +50,10 @@ subject to the following restrictions:
#include "pin_joint_3d_sw.h"
bool PinJoint3DSW::setup(real_t p_step) {
if ((A->get_mode() <= PhysicsServer3D::BODY_MODE_KINEMATIC) && (B->get_mode() <= PhysicsServer3D::BODY_MODE_KINEMATIC)) {
dynamic_A = (A->get_mode() > PhysicsServer3D::BODY_MODE_KINEMATIC);
dynamic_B = (B->get_mode() > PhysicsServer3D::BODY_MODE_KINEMATIC);
if (!dynamic_A && !dynamic_B) {
return false;
}
@ -123,8 +126,12 @@ void PinJoint3DSW::solve(real_t p_step) {
m_appliedImpulse += impulse;
Vector3 impulse_vector = normal * impulse;
A->apply_impulse(impulse_vector, pivotAInW - A->get_transform().origin);
B->apply_impulse(-impulse_vector, pivotBInW - B->get_transform().origin);
if (dynamic_A) {
A->apply_impulse(impulse_vector, pivotAInW - A->get_transform().origin);
}
if (dynamic_B) {
B->apply_impulse(-impulse_vector, pivotBInW - B->get_transform().origin);
}
normal[i] = 0;
}

6
servers/physics_3d/joints/pin_joint_3d_sw.h
View file

@ -74,10 +74,10 @@ class PinJoint3DSW : public Joint3DSW {
Vector3 m_pivotInB;
public:
virtual PhysicsServer3D::JointType get_type() const { return PhysicsServer3D::JOINT_TYPE_PIN; }
virtual PhysicsServer3D::JointType get_type() const override { return PhysicsServer3D::JOINT_TYPE_PIN; }
virtual bool setup(real_t p_step);
virtual void solve(real_t p_step);
virtual bool setup(real_t p_step) override;
virtual void solve(real_t p_step) override;
void set_param(PhysicsServer3D::PinJointParam p_param, real_t p_value);
real_t get_param(PhysicsServer3D::PinJointParam p_param) const;

45
servers/physics_3d/joints/slider_joint_3d_sw.cpp
View file

@ -127,7 +127,10 @@ SliderJoint3DSW::SliderJoint3DSW(Body3DSW *rbA, Body3DSW *rbB, const Transform &
//-----------------------------------------------------------------------------
bool SliderJoint3DSW::setup(real_t p_step) {
if ((A->get_mode() <= PhysicsServer3D::BODY_MODE_KINEMATIC) && (B->get_mode() <= PhysicsServer3D::BODY_MODE_KINEMATIC)) {
dynamic_A = (A->get_mode() > PhysicsServer3D::BODY_MODE_KINEMATIC);
dynamic_B = (B->get_mode() > PhysicsServer3D::BODY_MODE_KINEMATIC);
if (!dynamic_A && !dynamic_B) {
return false;
}
@ -200,8 +203,12 @@ void SliderJoint3DSW::solve(real_t p_step) {
// calcutate and apply impulse
real_t normalImpulse = softness * (restitution * depth / p_step - damping * rel_vel) * m_jacLinDiagABInv[i];
Vector3 impulse_vector = normal * normalImpulse;
A->apply_impulse(impulse_vector, m_relPosA);
B->apply_impulse(-impulse_vector, m_relPosB);
if (dynamic_A) {
A->apply_impulse(impulse_vector, m_relPosA);
}
if (dynamic_B) {
B->apply_impulse(-impulse_vector, m_relPosB);
}
if (m_poweredLinMotor && (!i)) { // apply linear motor
if (m_accumulatedLinMotorImpulse < m_maxLinMotorForce) {
real_t desiredMotorVel = m_targetLinMotorVelocity;
@ -221,8 +228,12 @@ void SliderJoint3DSW::solve(real_t p_step) {
m_accumulatedLinMotorImpulse = new_acc;
// apply clamped impulse
impulse_vector = normal * normalImpulse;
A->apply_impulse(impulse_vector, m_relPosA);
B->apply_impulse(-impulse_vector, m_relPosB);
if (dynamic_A) {
A->apply_impulse(impulse_vector, m_relPosA);
}
if (dynamic_B) {
B->apply_impulse(-impulse_vector, m_relPosB);
}
}
}
}
@ -256,8 +267,12 @@ void SliderJoint3DSW::solve(real_t p_step) {
angularError *= (real_t(1.) / denom2) * m_restitutionOrthoAng * m_softnessOrthoAng;
}
// apply impulse
A->apply_torque_impulse(-velrelOrthog + angularError);
B->apply_torque_impulse(velrelOrthog - angularError);
if (dynamic_A) {
A->apply_torque_impulse(-velrelOrthog + angularError);
}
if (dynamic_B) {
B->apply_torque_impulse(velrelOrthog - angularError);
}
real_t impulseMag;
//solve angular limits
if (m_solveAngLim) {
@ -268,8 +283,12 @@ void SliderJoint3DSW::solve(real_t p_step) {
impulseMag *= m_kAngle * m_softnessDirAng;
}
Vector3 impulse = axisA * impulseMag;
A->apply_torque_impulse(impulse);
B->apply_torque_impulse(-impulse);
if (dynamic_A) {
A->apply_torque_impulse(impulse);
}
if (dynamic_B) {
B->apply_torque_impulse(-impulse);
}
//apply angular motor
if (m_poweredAngMotor) {
if (m_accumulatedAngMotorImpulse < m_maxAngMotorForce) {
@ -294,8 +313,12 @@ void SliderJoint3DSW::solve(real_t p_step) {
m_accumulatedAngMotorImpulse = new_acc;
// apply clamped impulse
Vector3 motorImp = angImpulse * axisA;
A->apply_torque_impulse(motorImp);
B->apply_torque_impulse(-motorImp);
if (dynamic_A) {
A->apply_torque_impulse(motorImp);
}
if (dynamic_B) {
B->apply_torque_impulse(-motorImp);
}
}
}
} // SliderJointSW::solveConstraint()

6
servers/physics_3d/joints/slider_joint_3d_sw.h
View file

@ -240,10 +240,10 @@ public:
void set_param(PhysicsServer3D::SliderJointParam p_param, real_t p_value);
real_t get_param(PhysicsServer3D::SliderJointParam p_param) const;
bool setup(real_t p_step);
void solve(real_t p_step);
virtual bool setup(real_t p_step) override;
virtual void solve(real_t p_step) override;
virtual PhysicsServer3D::JointType get_type() const { return PhysicsServer3D::JOINT_TYPE_SLIDER; }
virtual PhysicsServer3D::JointType get_type() const override { return PhysicsServer3D::JOINT_TYPE_SLIDER; }
};
#endif // SLIDER_JOINT_SW_H