summaryrefslogtreecommitdiff
path: root/servers/physics_3d/joints
diff options
context:
space:
mode:
Diffstat (limited to 'servers/physics_3d/joints')
-rw-r--r--servers/physics_3d/joints/cone_twist_joint_3d_sw.cpp29
-rw-r--r--servers/physics_3d/joints/cone_twist_joint_3d_sw.h6
-rw-r--r--servers/physics_3d/joints/generic_6dof_joint_3d_sw.cpp25
-rw-r--r--servers/physics_3d/joints/generic_6dof_joint_3d_sw.h9
-rw-r--r--servers/physics_3d/joints/hinge_joint_3d_sw.cpp37
-rw-r--r--servers/physics_3d/joints/hinge_joint_3d_sw.h6
-rw-r--r--servers/physics_3d/joints/pin_joint_3d_sw.cpp13
-rw-r--r--servers/physics_3d/joints/pin_joint_3d_sw.h6
-rw-r--r--servers/physics_3d/joints/slider_joint_3d_sw.cpp45
-rw-r--r--servers/physics_3d/joints/slider_joint_3d_sw.h6
10 files changed, 129 insertions, 53 deletions
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 167f797bfe..e9efddf165 100644
--- a/servers/physics_3d/joints/cone_twist_joint_3d_sw.cpp
+++ b/servers/physics_3d/joints/cone_twist_joint_3d_sw.cpp
@@ -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);
+ }
}
}
}
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 4e4d4e7f0c..b871ea50db 100644
--- a/servers/physics_3d/joints/cone_twist_joint_3d_sw.h
+++ b/servers/physics_3d/joints/cone_twist_joint_3d_sw.h
@@ -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);
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 a86e8b4e76..7c504764a7 100644
--- a/servers/physics_3d/joints/generic_6dof_joint_3d_sw.cpp
+++ b/servers/physics_3d/joints/generic_6dof_joint_3d_sw.cpp
@@ -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);
}
}
}
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 d61a033231..8af76cefc2 100644
--- a/servers/physics_3d/joints/generic_6dof_joint_3d_sw.h
+++ b/servers/physics_3d/joints/generic_6dof_joint_3d_sw.h
@@ -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
/*!
diff --git a/servers/physics_3d/joints/hinge_joint_3d_sw.cpp b/servers/physics_3d/joints/hinge_joint_3d_sw.cpp
index 90b82f4680..bb8858c28a 100644
--- a/servers/physics_3d/joints/hinge_joint_3d_sw.cpp
+++ b/servers/physics_3d/joints/hinge_joint_3d_sw.cpp
@@ -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);
+ }
}
}
}
diff --git a/servers/physics_3d/joints/hinge_joint_3d_sw.h b/servers/physics_3d/joints/hinge_joint_3d_sw.h
index b6117aa0bc..2100f5de44 100644
--- a/servers/physics_3d/joints/hinge_joint_3d_sw.h
+++ b/servers/physics_3d/joints/hinge_joint_3d_sw.h
@@ -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();
diff --git a/servers/physics_3d/joints/pin_joint_3d_sw.cpp b/servers/physics_3d/joints/pin_joint_3d_sw.cpp
index 75d87992d1..8eb84d1c2f 100644
--- a/servers/physics_3d/joints/pin_joint_3d_sw.cpp
+++ b/servers/physics_3d/joints/pin_joint_3d_sw.cpp
@@ -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;
}
diff --git a/servers/physics_3d/joints/pin_joint_3d_sw.h b/servers/physics_3d/joints/pin_joint_3d_sw.h
index 1875983527..3d91452850 100644
--- a/servers/physics_3d/joints/pin_joint_3d_sw.h
+++ b/servers/physics_3d/joints/pin_joint_3d_sw.h
@@ -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;
diff --git a/servers/physics_3d/joints/slider_joint_3d_sw.cpp b/servers/physics_3d/joints/slider_joint_3d_sw.cpp
index 2e1ee8e770..8bd1951311 100644
--- a/servers/physics_3d/joints/slider_joint_3d_sw.cpp
+++ b/servers/physics_3d/joints/slider_joint_3d_sw.cpp
@@ -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()
diff --git a/servers/physics_3d/joints/slider_joint_3d_sw.h b/servers/physics_3d/joints/slider_joint_3d_sw.h
index f52f6ace27..ef5891d0f9 100644
--- a/servers/physics_3d/joints/slider_joint_3d_sw.h
+++ b/servers/physics_3d/joints/slider_joint_3d_sw.h
@@ -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
generated by cgit v1.2.3 (git 2.25.1) at 2024-11-25 10:06:40 +0000