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.cpp35
-rw-r--r--servers/physics_3d/joints/cone_twist_joint_3d_sw.h12
-rw-r--r--servers/physics_3d/joints/generic_6dof_joint_3d_sw.cpp29
-rw-r--r--servers/physics_3d/joints/generic_6dof_joint_3d_sw.h37
-rw-r--r--servers/physics_3d/joints/hinge_joint_3d_sw.cpp43
-rw-r--r--servers/physics_3d/joints/hinge_joint_3d_sw.h12
-rw-r--r--servers/physics_3d/joints/jacobian_entry_3d_sw.h2
-rw-r--r--servers/physics_3d/joints/pin_joint_3d_sw.cpp15
-rw-r--r--servers/physics_3d/joints/pin_joint_3d_sw.h6
-rw-r--r--servers/physics_3d/joints/slider_joint_3d_sw.cpp49
-rw-r--r--servers/physics_3d/joints/slider_joint_3d_sw.h28
11 files changed, 182 insertions, 86 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 9c4493f4a2..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;
@@ -109,6 +109,13 @@ ConeTwistJoint3DSW::ConeTwistJoint3DSW(Body3DSW *rbA, Body3DSW *rbB, const Trans
}
bool ConeTwistJoint3DSW::setup(real_t p_timestep) {
+ 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;
+ }
+
m_appliedImpulse = real_t(0.);
//set bias, sign, clear accumulator
@@ -204,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));
@@ -261,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);
+ }
}
}
@@ -283,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
@@ -299,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..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;
@@ -102,12 +102,12 @@ 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);
+ 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 13b389251f..56aba24b42 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,14 +208,18 @@ 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;
}
//////////////////////////// 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),
@@ -303,6 +310,13 @@ bool Generic6DOFJoint3DSW::testAngularLimitMotor(int axis_index) {
}
bool Generic6DOFJoint3DSW::setup(real_t p_timestep) {
+ 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;
+ }
+
// Clear accumulated impulses for the next simulation step
m_linearLimits.m_accumulatedImpulse = Vector3(real_t(0.), real_t(0.), real_t(0.));
int i;
@@ -380,6 +394,7 @@ void Generic6DOFJoint3DSW::solve(real_t p_timestep) {
jacDiagABInv,
A, pointInA,
B, pointInB,
+ dynamic_A, dynamic_B,
i, linear_axis, m_AnchorPos);
}
}
@@ -394,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..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,14 +113,14 @@ public:
return (m_enableMotor || m_currentLimit != 0);
}
- //! calculates error
+ //! calculates error
/*!
calculates m_currentLimit and m_currentLimitError.
*/
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);
@@ -184,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
@@ -208,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];
@@ -232,12 +233,12 @@ 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 { 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
/*!
@@ -250,7 +251,7 @@ public:
/*!
\sa Generic6DOFJointSW.getFrameOffsetA, Generic6DOFJointSW.getFrameOffsetB, Generic6DOFJointSW.calculateAngleInfo.
*/
- const Transform &getCalculatedTransformA() const {
+ const Transform3D &getCalculatedTransformA() const {
return m_calculatedTransformA;
}
@@ -258,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;
}
@@ -326,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 2b9f0038b4..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);
@@ -155,6 +155,13 @@ HingeJoint3DSW::HingeJoint3DSW(Body3DSW *rbA, Body3DSW *rbB, const Vector3 &pivo
}
bool HingeJoint3DSW::setup(real_t p_step) {
+ 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;
+ }
+
m_appliedImpulse = real_t(0.);
if (!m_angularOnly) {
@@ -275,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);
+ }
}
}
@@ -318,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) {
@@ -333,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);
+ }
}
}
@@ -355,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..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;
@@ -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();
@@ -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/pin_joint_3d_sw.cpp b/servers/physics_3d/joints/pin_joint_3d_sw.cpp
index 9f708ce151..8eb84d1c2f 100644
--- a/servers/physics_3d/joints/pin_joint_3d_sw.cpp
+++ b/servers/physics_3d/joints/pin_joint_3d_sw.cpp
@@ -50,6 +50,13 @@ subject to the following restrictions:
#include "pin_joint_3d_sw.h"
bool PinJoint3DSW::setup(real_t p_step) {
+ 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;
+ }
+
m_appliedImpulse = real_t(0.);
Vector3 normal(0, 0, 0);
@@ -119,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 0adc471797..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) {
@@ -127,6 +127,13 @@ SliderJoint3DSW::SliderJoint3DSW(Body3DSW *rbA, Body3DSW *rbB, const Transform &
//-----------------------------------------------------------------------------
bool SliderJoint3DSW::setup(real_t p_step) {
+ 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;
+ }
+
//calculate transforms
m_calculatedTransformA = A->get_transform() * m_frameInA;
m_calculatedTransformB = B->get_transform() * m_frameInB;
@@ -196,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;
@@ -217,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);
+ }
}
}
}
@@ -252,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) {
@@ -264,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) {
@@ -290,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..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; }
@@ -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 2025-01-05 22:40:51 +0000