summaryrefslogtreecommitdiff
path: root/servers/physics/joints
diff options
context:
space:
mode:
Diffstat (limited to 'servers/physics/joints')
-rw-r--r--servers/physics/joints/cone_twist_joint_sw.cpp6
-rw-r--r--servers/physics/joints/cone_twist_joint_sw.h8
-rw-r--r--servers/physics/joints/generic_6dof_joint_sw.cpp56
-rw-r--r--servers/physics/joints/generic_6dof_joint_sw.h8
-rw-r--r--servers/physics/joints/hinge_joint_sw.cpp8
-rw-r--r--servers/physics/joints/hinge_joint_sw.h8
-rw-r--r--servers/physics/joints/pin_joint_sw.cpp8
-rw-r--r--servers/physics/joints/pin_joint_sw.h8
-rw-r--r--servers/physics/joints/slider_joint_sw.cpp6
-rw-r--r--servers/physics/joints/slider_joint_sw.h8
10 files changed, 38 insertions, 86 deletions
diff --git a/servers/physics/joints/cone_twist_joint_sw.cpp b/servers/physics/joints/cone_twist_joint_sw.cpp
index cbf444f1e8..d78bcbd16d 100644
--- a/servers/physics/joints/cone_twist_joint_sw.cpp
+++ b/servers/physics/joints/cone_twist_joint_sw.cpp
@@ -96,7 +96,7 @@ ConeTwistJointSW::ConeTwistJointSW(BodySW* rbA,BodySW* rbB,const Transform& rbAF
}
-bool ConeTwistJointSW::setup(float p_step) {
+bool ConeTwistJointSW::setup(real_t p_step) {
m_appliedImpulse = real_t(0.);
//set bias, sign, clear accumulator
@@ -318,7 +318,7 @@ void ConeTwistJointSW::solve(real_t timeStep)
}
-void ConeTwistJointSW::set_param(PhysicsServer::ConeTwistJointParam p_param, float p_value) {
+void ConeTwistJointSW::set_param(PhysicsServer::ConeTwistJointParam p_param, real_t p_value) {
switch(p_param) {
case PhysicsServer::CONE_TWIST_JOINT_SWING_SPAN: {
@@ -345,7 +345,7 @@ void ConeTwistJointSW::set_param(PhysicsServer::ConeTwistJointParam p_param, flo
}
}
-float ConeTwistJointSW::get_param(PhysicsServer::ConeTwistJointParam p_param) const{
+real_t ConeTwistJointSW::get_param(PhysicsServer::ConeTwistJointParam p_param) const{
switch(p_param) {
case PhysicsServer::CONE_TWIST_JOINT_SWING_SPAN: {
diff --git a/servers/physics/joints/cone_twist_joint_sw.h b/servers/physics/joints/cone_twist_joint_sw.h
index 0d64d67443..eb7a5cd1b1 100644
--- a/servers/physics/joints/cone_twist_joint_sw.h
+++ b/servers/physics/joints/cone_twist_joint_sw.h
@@ -109,8 +109,8 @@ public:
virtual PhysicsServer::JointType get_type() const { return PhysicsServer::JOINT_CONE_TWIST; }
- virtual bool setup(float p_step);
- virtual void solve(float p_step);
+ virtual bool setup(real_t p_step);
+ virtual void solve(real_t p_step);
ConeTwistJointSW(BodySW* rbA,BodySW* rbB,const Transform& rbAFrame, const Transform& rbBFrame);
@@ -146,8 +146,8 @@ public:
return m_twistLimitSign;
}
- void set_param(PhysicsServer::ConeTwistJointParam p_param, float p_value);
- float get_param(PhysicsServer::ConeTwistJointParam p_param) const;
+ void set_param(PhysicsServer::ConeTwistJointParam p_param, real_t p_value);
+ real_t get_param(PhysicsServer::ConeTwistJointParam p_param) const;
};
diff --git a/servers/physics/joints/generic_6dof_joint_sw.cpp b/servers/physics/joints/generic_6dof_joint_sw.cpp
index 8d3fc4721a..bd07f3122a 100644
--- a/servers/physics/joints/generic_6dof_joint_sw.cpp
+++ b/servers/physics/joints/generic_6dof_joint_sw.cpp
@@ -38,54 +38,6 @@ See corresponding header file for licensing info.
#define GENERIC_D6_DISABLE_WARMSTARTING 1
-real_t btGetMatrixElem(const Basis& mat, int index);
-real_t btGetMatrixElem(const Basis& mat, int index)
-{
- int i = index%3;
- int j = index/3;
- return mat[i][j];
-}
-
-///MatrixToEulerXYZ from http://www.geometrictools.com/LibFoundation/Mathematics/Wm4Matrix3.inl.html
-bool matrixToEulerXYZ(const Basis& mat,Vector3& xyz);
-bool matrixToEulerXYZ(const Basis& mat,Vector3& xyz)
-{
- // rot = cy*cz -cy*sz sy
- // cz*sx*sy+cx*sz cx*cz-sx*sy*sz -cy*sx
- // -cx*cz*sy+sx*sz cz*sx+cx*sy*sz cx*cy
-
- if (btGetMatrixElem(mat,2) < real_t(1.0))
- {
- if (btGetMatrixElem(mat,2) > real_t(-1.0))
- {
- xyz[0] = Math::atan2(-btGetMatrixElem(mat,5),btGetMatrixElem(mat,8));
- xyz[1] = Math::asin(btGetMatrixElem(mat,2));
- xyz[2] = Math::atan2(-btGetMatrixElem(mat,1),btGetMatrixElem(mat,0));
- return true;
- }
- else
- {
- // WARNING. Not unique. XA - ZA = -atan2(r10,r11)
- xyz[0] = -Math::atan2(btGetMatrixElem(mat,3),btGetMatrixElem(mat,4));
- xyz[1] = -Math_PI*0.5;
- xyz[2] = real_t(0.0);
- return false;
- }
- }
- else
- {
- // WARNING. Not unique. XAngle + ZAngle = atan2(r10,r11)
- xyz[0] = Math::atan2(btGetMatrixElem(mat,3),btGetMatrixElem(mat,4));
- xyz[1] = Math_PI*0.5;
- xyz[2] = 0.0;
-
- }
-
-
- return false;
-}
-
-
//////////////////////////// G6DOFRotationalLimitMotorSW ////////////////////////////////////
@@ -297,7 +249,7 @@ void Generic6DOFJointSW::calculateAngleInfo()
{
Basis relative_frame = m_calculatedTransformA.basis.inverse()*m_calculatedTransformB.basis;
- matrixToEulerXYZ(relative_frame,m_calculatedAxisAngleDiff);
+ m_calculatedAxisAngleDiff = relative_frame.get_euler();
@@ -385,7 +337,7 @@ bool Generic6DOFJointSW::testAngularLimitMotor(int axis_index)
return m_angularLimits[axis_index].needApplyTorques();
}
-bool Generic6DOFJointSW::setup(float p_step) {
+bool Generic6DOFJointSW::setup(real_t p_step) {
// Clear accumulated impulses for the next simulation step
m_linearLimits.m_accumulatedImpulse=Vector3(real_t(0.), real_t(0.), real_t(0.));
@@ -531,7 +483,7 @@ void Generic6DOFJointSW::calcAnchorPos(void)
} // Generic6DOFJointSW::calcAnchorPos()
-void Generic6DOFJointSW::set_param(Vector3::Axis p_axis,PhysicsServer::G6DOFJointAxisParam p_param, float p_value) {
+void Generic6DOFJointSW::set_param(Vector3::Axis p_axis,PhysicsServer::G6DOFJointAxisParam p_param, real_t p_value) {
ERR_FAIL_INDEX(p_axis,3);
switch(p_param) {
@@ -607,7 +559,7 @@ void Generic6DOFJointSW::set_param(Vector3::Axis p_axis,PhysicsServer::G6DOFJoin
}
}
-float Generic6DOFJointSW::get_param(Vector3::Axis p_axis,PhysicsServer::G6DOFJointAxisParam p_param) const{
+real_t Generic6DOFJointSW::get_param(Vector3::Axis p_axis,PhysicsServer::G6DOFJointAxisParam p_param) const{
ERR_FAIL_INDEX_V(p_axis,3,0);
switch(p_param) {
case PhysicsServer::G6DOF_JOINT_LINEAR_LOWER_LIMIT: {
diff --git a/servers/physics/joints/generic_6dof_joint_sw.h b/servers/physics/joints/generic_6dof_joint_sw.h
index 4ac727c124..207ae85a45 100644
--- a/servers/physics/joints/generic_6dof_joint_sw.h
+++ b/servers/physics/joints/generic_6dof_joint_sw.h
@@ -295,8 +295,8 @@ public:
virtual PhysicsServer::JointType get_type() const { return PhysicsServer::JOINT_6DOF; }
- virtual bool setup(float p_step);
- virtual void solve(float p_step);
+ virtual bool setup(real_t p_step);
+ virtual void solve(real_t p_step);
//! Calcs global transform of the offsets
@@ -449,8 +449,8 @@ public:
virtual void calcAnchorPos(void); // overridable
- void set_param(Vector3::Axis p_axis,PhysicsServer::G6DOFJointAxisParam p_param, float p_value);
- float get_param(Vector3::Axis p_axis,PhysicsServer::G6DOFJointAxisParam p_param) const;
+ void set_param(Vector3::Axis p_axis,PhysicsServer::G6DOFJointAxisParam p_param, real_t p_value);
+ real_t get_param(Vector3::Axis p_axis,PhysicsServer::G6DOFJointAxisParam p_param) const;
void set_flag(Vector3::Axis p_axis,PhysicsServer::G6DOFJointAxisFlag p_flag, bool p_value);
bool get_flag(Vector3::Axis p_axis,PhysicsServer::G6DOFJointAxisFlag p_flag) const;
diff --git a/servers/physics/joints/hinge_joint_sw.cpp b/servers/physics/joints/hinge_joint_sw.cpp
index d50d5a1a73..9617eb8794 100644
--- a/servers/physics/joints/hinge_joint_sw.cpp
+++ b/servers/physics/joints/hinge_joint_sw.cpp
@@ -148,7 +148,7 @@ HingeJointSW::HingeJointSW(BodySW* rbA,BodySW* rbB, const Vector3& pivotInA,cons
-bool HingeJointSW::setup(float p_step) {
+bool HingeJointSW::setup(real_t p_step) {
m_appliedImpulse = real_t(0.);
@@ -262,7 +262,7 @@ bool HingeJointSW::setup(float p_step) {
return true;
}
-void HingeJointSW::solve(float p_step) {
+void HingeJointSW::solve(real_t p_step) {
Vector3 pivotAInW = A->get_transform().xform(m_rbAFrame.origin);
Vector3 pivotBInW = B->get_transform().xform(m_rbBFrame.origin);
@@ -418,7 +418,7 @@ real_t HingeJointSW::get_hinge_angle() {
}
-void HingeJointSW::set_param(PhysicsServer::HingeJointParam p_param, float p_value) {
+void HingeJointSW::set_param(PhysicsServer::HingeJointParam p_param, real_t p_value) {
switch (p_param) {
@@ -434,7 +434,7 @@ void HingeJointSW::set_param(PhysicsServer::HingeJointParam p_param, float p_val
}
}
-float HingeJointSW::get_param(PhysicsServer::HingeJointParam p_param) const{
+real_t HingeJointSW::get_param(PhysicsServer::HingeJointParam p_param) const{
switch (p_param) {
diff --git a/servers/physics/joints/hinge_joint_sw.h b/servers/physics/joints/hinge_joint_sw.h
index 60203e3cc4..8469fd1ca0 100644
--- a/servers/physics/joints/hinge_joint_sw.h
+++ b/servers/physics/joints/hinge_joint_sw.h
@@ -103,13 +103,13 @@ public:
virtual PhysicsServer::JointType get_type() const { return PhysicsServer::JOINT_HINGE; }
- virtual bool setup(float p_step);
- virtual void solve(float p_step);
+ virtual bool setup(real_t p_step);
+ virtual void solve(real_t p_step);
real_t get_hinge_angle();
- void set_param(PhysicsServer::HingeJointParam p_param, float p_value);
- float get_param(PhysicsServer::HingeJointParam p_param) const;
+ void set_param(PhysicsServer::HingeJointParam p_param, real_t p_value);
+ real_t get_param(PhysicsServer::HingeJointParam p_param) const;
void set_flag(PhysicsServer::HingeJointFlag p_flag, bool p_value);
bool get_flag(PhysicsServer::HingeJointFlag p_flag) const;
diff --git a/servers/physics/joints/pin_joint_sw.cpp b/servers/physics/joints/pin_joint_sw.cpp
index 9c7fe65684..b545ae630b 100644
--- a/servers/physics/joints/pin_joint_sw.cpp
+++ b/servers/physics/joints/pin_joint_sw.cpp
@@ -34,7 +34,7 @@ See corresponding header file for licensing info.
#include "pin_joint_sw.h"
-bool PinJointSW::setup(float p_step) {
+bool PinJointSW::setup(real_t p_step) {
m_appliedImpulse = real_t(0.);
@@ -59,7 +59,7 @@ bool PinJointSW::setup(float p_step) {
return true;
}
-void PinJointSW::solve(float p_step){
+void PinJointSW::solve(real_t p_step){
Vector3 pivotAInW = A->get_transform().xform(m_pivotInA);
Vector3 pivotBInW = B->get_transform().xform(m_pivotInB);
@@ -116,7 +116,7 @@ void PinJointSW::solve(float p_step){
}
}
-void PinJointSW::set_param(PhysicsServer::PinJointParam p_param,float p_value) {
+void PinJointSW::set_param(PhysicsServer::PinJointParam p_param,real_t p_value) {
switch(p_param) {
case PhysicsServer::PIN_JOINT_BIAS: m_tau=p_value; break;
@@ -125,7 +125,7 @@ void PinJointSW::set_param(PhysicsServer::PinJointParam p_param,float p_value) {
}
}
-float PinJointSW::get_param(PhysicsServer::PinJointParam p_param) const{
+real_t PinJointSW::get_param(PhysicsServer::PinJointParam p_param) const{
switch(p_param) {
case PhysicsServer::PIN_JOINT_BIAS: return m_tau;
diff --git a/servers/physics/joints/pin_joint_sw.h b/servers/physics/joints/pin_joint_sw.h
index 6797972496..b72b21219d 100644
--- a/servers/physics/joints/pin_joint_sw.h
+++ b/servers/physics/joints/pin_joint_sw.h
@@ -79,11 +79,11 @@ public:
virtual PhysicsServer::JointType get_type() const { return PhysicsServer::JOINT_PIN; }
- virtual bool setup(float p_step);
- virtual void solve(float p_step);
+ virtual bool setup(real_t p_step);
+ virtual void solve(real_t p_step);
- void set_param(PhysicsServer::PinJointParam p_param,float p_value);
- float get_param(PhysicsServer::PinJointParam p_param) const;
+ void set_param(PhysicsServer::PinJointParam p_param,real_t p_value);
+ real_t get_param(PhysicsServer::PinJointParam p_param) const;
void set_pos_A(const Vector3& p_pos) { m_pivotInA=p_pos; }
void set_pos_B(const Vector3& p_pos) { m_pivotInB=p_pos; }
diff --git a/servers/physics/joints/slider_joint_sw.cpp b/servers/physics/joints/slider_joint_sw.cpp
index bdcae08ca4..fc728ed0ba 100644
--- a/servers/physics/joints/slider_joint_sw.cpp
+++ b/servers/physics/joints/slider_joint_sw.cpp
@@ -112,7 +112,7 @@ SliderJointSW::SliderJointSW(BodySW* rbA, BodySW* rbB, const Transform& frameInA
//-----------------------------------------------------------------------------
-bool SliderJointSW::setup(float p_step)
+bool SliderJointSW::setup(real_t p_step)
{
//calculate transforms
@@ -406,7 +406,7 @@ Vector3 SliderJointSW::getAncorInB(void)
return ancorInB;
} // SliderJointSW::getAncorInB();
-void SliderJointSW::set_param(PhysicsServer::SliderJointParam p_param, float p_value) {
+void SliderJointSW::set_param(PhysicsServer::SliderJointParam p_param, real_t p_value) {
switch(p_param) {
case PhysicsServer::SLIDER_JOINT_LINEAR_LIMIT_UPPER: m_upperLinLimit=p_value; break;
@@ -437,7 +437,7 @@ void SliderJointSW::set_param(PhysicsServer::SliderJointParam p_param, float p_v
}
-float SliderJointSW::get_param(PhysicsServer::SliderJointParam p_param) const {
+real_t SliderJointSW::get_param(PhysicsServer::SliderJointParam p_param) const {
switch(p_param) {
case PhysicsServer::SLIDER_JOINT_LINEAR_LIMIT_UPPER: return m_upperLinLimit;
diff --git a/servers/physics/joints/slider_joint_sw.h b/servers/physics/joints/slider_joint_sw.h
index 4e697e6f64..d01038df59 100644
--- a/servers/physics/joints/slider_joint_sw.h
+++ b/servers/physics/joints/slider_joint_sw.h
@@ -237,11 +237,11 @@ public:
Vector3 getAncorInA(void);
Vector3 getAncorInB(void);
- void set_param(PhysicsServer::SliderJointParam p_param, float p_value);
- float get_param(PhysicsServer::SliderJointParam p_param) const;
+ void set_param(PhysicsServer::SliderJointParam p_param, real_t p_value);
+ real_t get_param(PhysicsServer::SliderJointParam p_param) const;
- bool setup(float p_step);
- void solve(float p_step);
+ bool setup(real_t p_step);
+ void solve(real_t p_step);
virtual PhysicsServer::JointType get_type() const { return PhysicsServer::JOINT_SLIDER; }
generated by cgit v1.2.3 (git 2.25.1) at 2025-02-23 20:55:56 +0000