aboutsummaryrefslogtreecommitdiff
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.cpp232
-rw-r--r--servers/physics/joints/cone_twist_joint_sw.h74
-rw-r--r--servers/physics/joints/generic_6dof_joint_sw.cpp557
-rw-r--r--servers/physics/joints/generic_6dof_joint_sw.h485
-rw-r--r--servers/physics/joints/hinge_joint_sw.cpp297
-rw-r--r--servers/physics/joints/hinge_joint_sw.h46
-rw-r--r--servers/physics/joints/jacobian_entry_sw.h123
-rw-r--r--servers/physics/joints/pin_joint_sw.cpp89
-rw-r--r--servers/physics/joints/pin_joint_sw.h29
-rw-r--r--servers/physics/joints/slider_joint_sw.cpp301
-rw-r--r--servers/physics/joints/slider_joint_sw.h87
11 files changed, 1003 insertions, 1317 deletions
diff --git a/servers/physics/joints/cone_twist_joint_sw.cpp b/servers/physics/joints/cone_twist_joint_sw.cpp
index d78bcbd16..8cab81de2 100644
--- a/servers/physics/joints/cone_twist_joint_sw.cpp
+++ b/servers/physics/joints/cone_twist_joint_sw.cpp
@@ -34,29 +34,26 @@ See corresponding header file for licensing info.
#include "cone_twist_joint_sw.h"
-static void plane_space(const Vector3& n, Vector3& p, Vector3& q) {
+static void plane_space(const Vector3 &n, Vector3 &p, Vector3 &q) {
- if (Math::abs(n.z) > 0.707106781186547524400844362) {
- // choose p in y-z plane
- real_t a = n[1]*n[1] + n[2]*n[2];
- real_t k = 1.0/Math::sqrt(a);
- p=Vector3(0,-n[2]*k,n[1]*k);
- // set q = n x p
- q=Vector3(a*k,-n[0]*p[2],n[0]*p[1]);
- }
- else {
- // choose p in x-y plane
- real_t a = n.x*n.x + n.y*n.y;
- real_t k = 1.0/Math::sqrt(a);
- p=Vector3(-n.y*k,n.x*k,0);
- // set q = n x p
- q=Vector3(-n.z*p.y,n.z*p.x,a*k);
- }
+ if (Math::abs(n.z) > 0.707106781186547524400844362) {
+ // choose p in y-z plane
+ real_t a = n[1] * n[1] + n[2] * n[2];
+ real_t k = 1.0 / Math::sqrt(a);
+ p = Vector3(0, -n[2] * k, n[1] * k);
+ // set q = n x p
+ q = Vector3(a * k, -n[0] * p[2], n[0] * p[1]);
+ } else {
+ // choose p in x-y plane
+ real_t a = n.x * n.x + n.y * n.y;
+ real_t k = 1.0 / Math::sqrt(a);
+ p = Vector3(-n.y * k, n.x * k, 0);
+ // set q = n x p
+ q = Vector3(-n.z * p.y, n.z * p.x, a * k);
+ }
}
-
-static _FORCE_INLINE_ real_t atan2fast(real_t y, real_t x)
-{
+static _FORCE_INLINE_ real_t atan2fast(real_t y, real_t x) {
real_t coeff_1 = Math_PI / 4.0f;
real_t coeff_2 = 3.0f * coeff_1;
real_t abs_y = Math::abs(y);
@@ -71,32 +68,31 @@ static _FORCE_INLINE_ real_t atan2fast(real_t y, real_t x)
return (y < 0.0f) ? -angle : angle;
}
-ConeTwistJointSW::ConeTwistJointSW(BodySW* rbA,BodySW* rbB,const Transform& rbAFrame, const Transform& rbBFrame) : JointSW(_arr,2) {
+ConeTwistJointSW::ConeTwistJointSW(BodySW *rbA, BodySW *rbB, const Transform &rbAFrame, const Transform &rbBFrame)
+ : JointSW(_arr, 2) {
- A=rbA;
- B=rbB;
+ A = rbA;
+ B = rbB;
+ m_rbAFrame = rbAFrame;
+ m_rbBFrame = rbBFrame;
- m_rbAFrame=rbAFrame;
- m_rbBFrame=rbBFrame;
-
- m_swingSpan1 = Math_PI/4.0;
- m_swingSpan2 = Math_PI/4.0;
- m_twistSpan = Math_PI*2;
+ m_swingSpan1 = Math_PI / 4.0;
+ m_swingSpan2 = Math_PI / 4.0;
+ m_twistSpan = Math_PI * 2;
m_biasFactor = 0.3f;
m_relaxationFactor = 1.0f;
m_solveTwistLimit = false;
m_solveSwingLimit = false;
- A->add_constraint(this,0);
- B->add_constraint(this,1);
+ A->add_constraint(this, 0);
+ B->add_constraint(this, 1);
- m_appliedImpulse=0;
+ m_appliedImpulse = 0;
}
-
-bool ConeTwistJointSW::setup(real_t p_step) {
+bool ConeTwistJointSW::setup(real_t p_step) {
m_appliedImpulse = real_t(0.);
//set bias, sign, clear accumulator
@@ -107,109 +103,97 @@ bool ConeTwistJointSW::setup(real_t p_step) {
m_accTwistLimitImpulse = real_t(0.);
m_accSwingLimitImpulse = real_t(0.);
- if (!m_angularOnly)
- {
+ if (!m_angularOnly) {
Vector3 pivotAInW = A->get_transform().xform(m_rbAFrame.origin);
Vector3 pivotBInW = B->get_transform().xform(m_rbBFrame.origin);
Vector3 relPos = pivotBInW - pivotAInW;
Vector3 normal[3];
- if (relPos.length_squared() > CMP_EPSILON)
- {
+ if (relPos.length_squared() > CMP_EPSILON) {
normal[0] = relPos.normalized();
- }
- else
- {
- normal[0]=Vector3(real_t(1.0),0,0);
+ } else {
+ normal[0] = Vector3(real_t(1.0), 0, 0);
}
plane_space(normal[0], normal[1], normal[2]);
- for (int i=0;i<3;i++)
- {
+ for (int i = 0; i < 3; i++) {
memnew_placement(&m_jac[i], JacobianEntrySW(
- A->get_principal_inertia_axes().transposed(),
- B->get_principal_inertia_axes().transposed(),
- pivotAInW - A->get_transform().origin - A->get_center_of_mass(),
- pivotBInW - B->get_transform().origin - B->get_center_of_mass(),
- normal[i],
- A->get_inv_inertia(),
- A->get_inv_mass(),
- B->get_inv_inertia(),
- B->get_inv_mass()));
+ A->get_principal_inertia_axes().transposed(),
+ B->get_principal_inertia_axes().transposed(),
+ pivotAInW - A->get_transform().origin - A->get_center_of_mass(),
+ pivotBInW - B->get_transform().origin - B->get_center_of_mass(),
+ normal[i],
+ A->get_inv_inertia(),
+ A->get_inv_mass(),
+ B->get_inv_inertia(),
+ B->get_inv_mass()));
}
}
- Vector3 b1Axis1,b1Axis2,b1Axis3;
- Vector3 b2Axis1,b2Axis2;
+ Vector3 b1Axis1, b1Axis2, b1Axis3;
+ Vector3 b2Axis1, b2Axis2;
- b1Axis1 = A->get_transform().basis.xform( this->m_rbAFrame.basis.get_axis(0) );
- b2Axis1 = B->get_transform().basis.xform( this->m_rbBFrame.basis.get_axis(0) );
+ b1Axis1 = A->get_transform().basis.xform(this->m_rbAFrame.basis.get_axis(0));
+ b2Axis1 = B->get_transform().basis.xform(this->m_rbBFrame.basis.get_axis(0));
- real_t swing1=real_t(0.),swing2 = real_t(0.);
+ real_t swing1 = real_t(0.), swing2 = real_t(0.);
- real_t swx=real_t(0.),swy = real_t(0.);
+ real_t swx = real_t(0.), swy = real_t(0.);
real_t thresh = real_t(10.);
real_t fact;
// Get Frame into world space
- if (m_swingSpan1 >= real_t(0.05f))
- {
- b1Axis2 = A->get_transform().basis.xform( this->m_rbAFrame.basis.get_axis(1) );
+ if (m_swingSpan1 >= real_t(0.05f)) {
+ b1Axis2 = A->get_transform().basis.xform(this->m_rbAFrame.basis.get_axis(1));
//swing1 = btAtan2Fast( b2Axis1.dot(b1Axis2),b2Axis1.dot(b1Axis1) );
swx = b2Axis1.dot(b1Axis1);
swy = b2Axis1.dot(b1Axis2);
- swing1 = atan2fast(swy, swx);
- fact = (swy*swy + swx*swx) * thresh * thresh;
+ swing1 = atan2fast(swy, swx);
+ fact = (swy * swy + swx * swx) * thresh * thresh;
fact = fact / (fact + real_t(1.0));
swing1 *= fact;
-
}
- if (m_swingSpan2 >= real_t(0.05f))
- {
- b1Axis3 = A->get_transform().basis.xform( this->m_rbAFrame.basis.get_axis(2) );
+ if (m_swingSpan2 >= real_t(0.05f)) {
+ b1Axis3 = A->get_transform().basis.xform(this->m_rbAFrame.basis.get_axis(2));
//swing2 = btAtan2Fast( b2Axis1.dot(b1Axis3),b2Axis1.dot(b1Axis1) );
swx = b2Axis1.dot(b1Axis1);
swy = b2Axis1.dot(b1Axis3);
- swing2 = atan2fast(swy, swx);
- fact = (swy*swy + swx*swx) * thresh * thresh;
+ swing2 = atan2fast(swy, swx);
+ fact = (swy * swy + swx * swx) * thresh * thresh;
fact = fact / (fact + real_t(1.0));
swing2 *= fact;
}
- real_t RMaxAngle1Sq = 1.0f / (m_swingSpan1*m_swingSpan1);
- real_t RMaxAngle2Sq = 1.0f / (m_swingSpan2*m_swingSpan2);
- real_t EllipseAngle = Math::abs(swing1*swing1)* RMaxAngle1Sq + Math::abs(swing2*swing2) * RMaxAngle2Sq;
+ real_t RMaxAngle1Sq = 1.0f / (m_swingSpan1 * m_swingSpan1);
+ real_t RMaxAngle2Sq = 1.0f / (m_swingSpan2 * m_swingSpan2);
+ real_t EllipseAngle = Math::abs(swing1 * swing1) * RMaxAngle1Sq + Math::abs(swing2 * swing2) * RMaxAngle2Sq;
- if (EllipseAngle > 1.0f)
- {
- m_swingCorrection = EllipseAngle-1.0f;
+ if (EllipseAngle > 1.0f) {
+ m_swingCorrection = EllipseAngle - 1.0f;
m_solveSwingLimit = true;
// Calculate necessary axis & factors
- m_swingAxis = b2Axis1.cross(b1Axis2* b2Axis1.dot(b1Axis2) + b1Axis3* b2Axis1.dot(b1Axis3));
+ m_swingAxis = b2Axis1.cross(b1Axis2 * b2Axis1.dot(b1Axis2) + b1Axis3 * b2Axis1.dot(b1Axis3));
m_swingAxis.normalize();
real_t swingAxisSign = (b2Axis1.dot(b1Axis1) >= 0.0f) ? 1.0f : -1.0f;
m_swingAxis *= swingAxisSign;
- m_kSwing = real_t(1.) / (A->compute_angular_impulse_denominator(m_swingAxis) +
- B->compute_angular_impulse_denominator(m_swingAxis));
-
+ m_kSwing = real_t(1.) / (A->compute_angular_impulse_denominator(m_swingAxis) +
+ B->compute_angular_impulse_denominator(m_swingAxis));
}
// Twist limits
- if (m_twistSpan >= real_t(0.))
- {
- Vector3 b2Axis2 = B->get_transform().basis.xform( this->m_rbBFrame.basis.get_axis(1) );
- Quat rotationArc = Quat(b2Axis1,b1Axis1);
+ if (m_twistSpan >= real_t(0.)) {
+ Vector3 b2Axis2 = B->get_transform().basis.xform(this->m_rbBFrame.basis.get_axis(1));
+ Quat rotationArc = Quat(b2Axis1, b1Axis1);
Vector3 TwistRef = rotationArc.xform(b2Axis2);
- real_t twist = atan2fast( TwistRef.dot(b1Axis3), TwistRef.dot(b1Axis2) );
+ real_t twist = atan2fast(TwistRef.dot(b1Axis3), TwistRef.dot(b1Axis2));
real_t lockedFreeFactor = (m_twistSpan > real_t(0.05f)) ? m_limitSoftness : real_t(0.);
- if (twist <= -m_twistSpan*lockedFreeFactor)
- {
+ if (twist <= -m_twistSpan * lockedFreeFactor) {
m_twistCorrection = -(twist + m_twistSpan);
m_solveTwistLimit = true;
@@ -218,28 +202,24 @@ bool ConeTwistJointSW::setup(real_t p_step) {
m_twistAxis *= -1.0f;
m_kTwist = real_t(1.) / (A->compute_angular_impulse_denominator(m_twistAxis) +
- B->compute_angular_impulse_denominator(m_twistAxis));
-
- } else
- if (twist > m_twistSpan*lockedFreeFactor)
- {
- m_twistCorrection = (twist - m_twistSpan);
- m_solveTwistLimit = true;
+ B->compute_angular_impulse_denominator(m_twistAxis));
- m_twistAxis = (b2Axis1 + b1Axis1) * 0.5f;
- m_twistAxis.normalize();
+ } else if (twist > m_twistSpan * lockedFreeFactor) {
+ m_twistCorrection = (twist - m_twistSpan);
+ m_solveTwistLimit = true;
- m_kTwist = real_t(1.) / (A->compute_angular_impulse_denominator(m_twistAxis) +
- B->compute_angular_impulse_denominator(m_twistAxis));
+ m_twistAxis = (b2Axis1 + b1Axis1) * 0.5f;
+ m_twistAxis.normalize();
- }
+ m_kTwist = real_t(1.) / (A->compute_angular_impulse_denominator(m_twistAxis) +
+ B->compute_angular_impulse_denominator(m_twistAxis));
+ }
}
return true;
}
-void ConeTwistJointSW::solve(real_t timeStep)
-{
+void ConeTwistJointSW::solve(real_t timeStep) {
Vector3 pivotAInW = A->get_transform().xform(m_rbAFrame.origin);
Vector3 pivotBInW = B->get_transform().xform(m_rbBFrame.origin);
@@ -247,8 +227,7 @@ void ConeTwistJointSW::solve(real_t timeStep)
real_t tau = real_t(0.3);
//linear part
- if (!m_angularOnly)
- {
+ if (!m_angularOnly) {
Vector3 rel_pos1 = pivotAInW - A->get_transform().origin;
Vector3 rel_pos2 = pivotBInW - B->get_transform().origin;
@@ -256,16 +235,15 @@ void ConeTwistJointSW::solve(real_t timeStep)
Vector3 vel2 = B->get_velocity_in_local_point(rel_pos2);
Vector3 vel = vel1 - vel2;
- for (int i=0;i<3;i++)
- {
- const Vector3& normal = m_jac[i].m_linearJointAxis;
+ for (int i = 0; i < 3; i++) {
+ const Vector3 &normal = m_jac[i].m_linearJointAxis;
real_t jacDiagABInv = real_t(1.) / m_jac[i].getDiagonal();
real_t rel_vel;
rel_vel = normal.dot(vel);
//positional error (zeroth order error)
real_t depth = -(pivotAInW - pivotBInW).dot(normal); //this is the error projected on the normal
- real_t impulse = depth*tau/timeStep * jacDiagABInv - rel_vel * jacDiagABInv;
+ real_t impulse = depth * tau / timeStep * jacDiagABInv - rel_vel * jacDiagABInv;
m_appliedImpulse += impulse;
Vector3 impulse_vector = normal * impulse;
A->apply_impulse(pivotAInW - A->get_transform().origin, impulse_vector);
@@ -275,79 +253,73 @@ void ConeTwistJointSW::solve(real_t timeStep)
{
///solve angular part
- const Vector3& angVelA = A->get_angular_velocity();
- const Vector3& angVelB = B->get_angular_velocity();
+ const Vector3 &angVelA = A->get_angular_velocity();
+ const Vector3 &angVelB = B->get_angular_velocity();
// solve swing limit
- if (m_solveSwingLimit)
- {
- real_t amplitude = ((angVelB - angVelA).dot( m_swingAxis )*m_relaxationFactor*m_relaxationFactor + m_swingCorrection*(real_t(1.)/timeStep)*m_biasFactor);
+ if (m_solveSwingLimit) {
+ real_t amplitude = ((angVelB - angVelA).dot(m_swingAxis) * m_relaxationFactor * m_relaxationFactor + m_swingCorrection * (real_t(1.) / timeStep) * m_biasFactor);
real_t impulseMag = amplitude * m_kSwing;
// Clamp the accumulated impulse
real_t temp = m_accSwingLimitImpulse;
- m_accSwingLimitImpulse = MAX(m_accSwingLimitImpulse + impulseMag, real_t(0.0) );
+ m_accSwingLimitImpulse = MAX(m_accSwingLimitImpulse + impulseMag, real_t(0.0));
impulseMag = m_accSwingLimitImpulse - temp;
Vector3 impulse = m_swingAxis * impulseMag;
A->apply_torque_impulse(impulse);
B->apply_torque_impulse(-impulse);
-
}
// solve twist limit
- if (m_solveTwistLimit)
- {
- real_t amplitude = ((angVelB - angVelA).dot( m_twistAxis )*m_relaxationFactor*m_relaxationFactor + m_twistCorrection*(real_t(1.)/timeStep)*m_biasFactor );
+ if (m_solveTwistLimit) {
+ real_t amplitude = ((angVelB - angVelA).dot(m_twistAxis) * m_relaxationFactor * m_relaxationFactor + m_twistCorrection * (real_t(1.) / timeStep) * m_biasFactor);
real_t impulseMag = amplitude * m_kTwist;
// Clamp the accumulated impulse
real_t temp = m_accTwistLimitImpulse;
- m_accTwistLimitImpulse = MAX(m_accTwistLimitImpulse + impulseMag, real_t(0.0) );
+ m_accTwistLimitImpulse = MAX(m_accTwistLimitImpulse + impulseMag, real_t(0.0));
impulseMag = m_accTwistLimitImpulse - temp;
Vector3 impulse = m_twistAxis * impulseMag;
A->apply_torque_impulse(impulse);
B->apply_torque_impulse(-impulse);
-
}
-
}
-
}
void ConeTwistJointSW::set_param(PhysicsServer::ConeTwistJointParam p_param, real_t p_value) {
- switch(p_param) {
+ switch (p_param) {
case PhysicsServer::CONE_TWIST_JOINT_SWING_SPAN: {
- m_swingSpan1=p_value;
- m_swingSpan2=p_value;
+ m_swingSpan1 = p_value;
+ m_swingSpan2 = p_value;
} break;
case PhysicsServer::CONE_TWIST_JOINT_TWIST_SPAN: {
- m_twistSpan=p_value;
+ m_twistSpan = p_value;
} break;
case PhysicsServer::CONE_TWIST_JOINT_BIAS: {
- m_biasFactor=p_value;
+ m_biasFactor = p_value;
} break;
case PhysicsServer::CONE_TWIST_JOINT_SOFTNESS: {
- m_limitSoftness=p_value;
+ m_limitSoftness = p_value;
} break;
case PhysicsServer::CONE_TWIST_JOINT_RELAXATION: {
- m_relaxationFactor=p_value;
+ m_relaxationFactor = p_value;
} break;
}
}
-real_t ConeTwistJointSW::get_param(PhysicsServer::ConeTwistJointParam p_param) const{
+real_t ConeTwistJointSW::get_param(PhysicsServer::ConeTwistJointParam p_param) const {
- switch(p_param) {
+ switch (p_param) {
case PhysicsServer::CONE_TWIST_JOINT_SWING_SPAN: {
return m_swingSpan1;
diff --git a/servers/physics/joints/cone_twist_joint_sw.h b/servers/physics/joints/cone_twist_joint_sw.h
index eb7a5cd1b..c122c2225 100644
--- a/servers/physics/joints/cone_twist_joint_sw.h
+++ b/servers/physics/joints/cone_twist_joint_sw.h
@@ -51,14 +51,11 @@ Written by: Marcus Hennix
#ifndef CONE_TWIST_JOINT_SW_H
#define CONE_TWIST_JOINT_SW_H
-#include "servers/physics/joints_sw.h"
#include "servers/physics/joints/jacobian_entry_sw.h"
-
-
+#include "servers/physics/joints_sw.h"
///ConeTwistJointSW can be used to simulate ragdoll joints (upper arm, leg etc)
-class ConeTwistJointSW : public JointSW
-{
+class ConeTwistJointSW : public JointSW {
#ifdef IN_PARALLELL_SOLVER
public:
#endif
@@ -72,86 +69,73 @@ public:
BodySW *_arr[2];
};
- JacobianEntrySW m_jac[3]; //3 orthogonal linear constraints
-
+ JacobianEntrySW m_jac[3]; //3 orthogonal linear constraints
real_t m_appliedImpulse;
Transform m_rbAFrame;
Transform m_rbBFrame;
- real_t m_limitSoftness;
- real_t m_biasFactor;
- real_t m_relaxationFactor;
-
- real_t m_swingSpan1;
- real_t m_swingSpan2;
- real_t m_twistSpan;
+ real_t m_limitSoftness;
+ real_t m_biasFactor;
+ real_t m_relaxationFactor;
- Vector3 m_swingAxis;
- Vector3 m_twistAxis;
+ real_t m_swingSpan1;
+ real_t m_swingSpan2;
+ real_t m_twistSpan;
- real_t m_kSwing;
- real_t m_kTwist;
+ Vector3 m_swingAxis;
+ Vector3 m_twistAxis;
- real_t m_twistLimitSign;
- real_t m_swingCorrection;
- real_t m_twistCorrection;
+ real_t m_kSwing;
+ real_t m_kTwist;
- real_t m_accSwingLimitImpulse;
- real_t m_accTwistLimitImpulse;
+ real_t m_twistLimitSign;
+ real_t m_swingCorrection;
+ real_t m_twistCorrection;
- bool m_angularOnly;
- bool m_solveTwistLimit;
- bool m_solveSwingLimit;
+ real_t m_accSwingLimitImpulse;
+ real_t m_accTwistLimitImpulse;
+ bool m_angularOnly;
+ bool m_solveTwistLimit;
+ bool m_solveSwingLimit;
public:
-
virtual PhysicsServer::JointType get_type() const { return PhysicsServer::JOINT_CONE_TWIST; }
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);
-
+ ConeTwistJointSW(BodySW *rbA, BodySW *rbB, const Transform &rbAFrame, const Transform &rbBFrame);
- void setAngularOnly(bool angularOnly)
- {
+ void setAngularOnly(bool angularOnly) {
m_angularOnly = angularOnly;
}
- void setLimit(real_t _swingSpan1,real_t _swingSpan2,real_t _twistSpan, real_t _softness = 0.8f, real_t _biasFactor = 0.3f, real_t _relaxationFactor = 1.0f)
- {
+ void setLimit(real_t _swingSpan1, real_t _swingSpan2, real_t _twistSpan, real_t _softness = 0.8f, real_t _biasFactor = 0.3f, real_t _relaxationFactor = 1.0f) {
m_swingSpan1 = _swingSpan1;
m_swingSpan2 = _swingSpan2;
- m_twistSpan = _twistSpan;
+ m_twistSpan = _twistSpan;
- m_limitSoftness = _softness;
+ m_limitSoftness = _softness;
m_biasFactor = _biasFactor;
m_relaxationFactor = _relaxationFactor;
}
- inline int getSolveTwistLimit()
- {
+ inline int getSolveTwistLimit() {
return m_solveTwistLimit;
}
- inline int getSolveSwingLimit()
- {
+ inline int getSolveSwingLimit() {
return m_solveTwistLimit;
}
- inline real_t getTwistLimitSign()
- {
+ inline real_t getTwistLimitSign() {
return m_twistLimitSign;
}
void set_param(PhysicsServer::ConeTwistJointParam p_param, real_t p_value);
real_t get_param(PhysicsServer::ConeTwistJointParam p_param) const;
-
-
};
-
-
#endif // CONE_TWIST_JOINT_SW_H
diff --git a/servers/physics/joints/generic_6dof_joint_sw.cpp b/servers/physics/joints/generic_6dof_joint_sw.cpp
index bd07f3122..1e07bc73f 100644
--- a/servers/physics/joints/generic_6dof_joint_sw.cpp
+++ b/servers/physics/joints/generic_6dof_joint_sw.cpp
@@ -34,118 +34,90 @@ See corresponding header file for licensing info.
#include "generic_6dof_joint_sw.h"
-
-
#define GENERIC_D6_DISABLE_WARMSTARTING 1
-
//////////////////////////// G6DOFRotationalLimitMotorSW ////////////////////////////////////
-
-int G6DOFRotationalLimitMotorSW::testLimitValue(real_t test_value)
-{
- if(m_loLimit>m_hiLimit)
- {
- m_currentLimit = 0;//Free from violation
+int G6DOFRotationalLimitMotorSW::testLimitValue(real_t test_value) {
+ if (m_loLimit > m_hiLimit) {
+ m_currentLimit = 0; //Free from violation
return 0;
}
- if (test_value < m_loLimit)
- {
- m_currentLimit = 1;//low limit violation
- m_currentLimitError = test_value - m_loLimit;
+ if (test_value < m_loLimit) {
+ m_currentLimit = 1; //low limit violation
+ m_currentLimitError = test_value - m_loLimit;
return 1;
- }
- else if (test_value> m_hiLimit)
- {
- m_currentLimit = 2;//High limit violation
+ } else if (test_value > m_hiLimit) {
+ m_currentLimit = 2; //High limit violation
m_currentLimitError = test_value - m_hiLimit;
return 2;
};
- m_currentLimit = 0;//Free from violation
+ m_currentLimit = 0; //Free from violation
return 0;
-
}
-
real_t G6DOFRotationalLimitMotorSW::solveAngularLimits(
- real_t timeStep,Vector3& axis,real_t jacDiagABInv,
- BodySW * body0, BodySW * body1)
-{
- if (needApplyTorques()==false) return 0.0f;
+ real_t timeStep, Vector3 &axis, real_t jacDiagABInv,
+ BodySW *body0, BodySW *body1) {
+ if (needApplyTorques() == false) return 0.0f;
- real_t target_velocity = m_targetVelocity;
- real_t maxMotorForce = m_maxMotorForce;
+ real_t target_velocity = m_targetVelocity;
+ real_t maxMotorForce = m_maxMotorForce;
//current error correction
- if (m_currentLimit!=0)
- {
- target_velocity = -m_ERP*m_currentLimitError/(timeStep);
- maxMotorForce = m_maxLimitForce;
- }
-
- maxMotorForce *= timeStep;
-
- // current velocity difference
- Vector3 vel_diff = body0->get_angular_velocity();
- if (body1)
- {
- vel_diff -= body1->get_angular_velocity();
- }
+ if (m_currentLimit != 0) {
+ target_velocity = -m_ERP * m_currentLimitError / (timeStep);
+ maxMotorForce = m_maxLimitForce;
+ }
+ maxMotorForce *= timeStep;
+ // current velocity difference
+ Vector3 vel_diff = body0->get_angular_velocity();
+ if (body1) {
+ vel_diff -= body1->get_angular_velocity();
+ }
- real_t rel_vel = axis.dot(vel_diff);
+ real_t rel_vel = axis.dot(vel_diff);
// correction velocity
- real_t motor_relvel = m_limitSoftness*(target_velocity - m_damping*rel_vel);
-
-
- if ( motor_relvel < CMP_EPSILON && motor_relvel > -CMP_EPSILON )
- {
- return 0.0f;//no need for applying force
- }
+ real_t motor_relvel = m_limitSoftness * (target_velocity - m_damping * rel_vel);
+ if (motor_relvel < CMP_EPSILON && motor_relvel > -CMP_EPSILON) {
+ return 0.0f; //no need for applying force
+ }
// correction impulse
- real_t unclippedMotorImpulse = (1+m_bounce)*motor_relvel*jacDiagABInv;
+ real_t unclippedMotorImpulse = (1 + m_bounce) * motor_relvel * jacDiagABInv;
// clip correction impulse
- real_t clippedMotorImpulse;
-
- ///@todo: should clip against accumulated impulse
- if (unclippedMotorImpulse>0.0f)
- {
- clippedMotorImpulse = unclippedMotorImpulse > maxMotorForce? maxMotorForce: unclippedMotorImpulse;
- }
- else
- {
- clippedMotorImpulse = unclippedMotorImpulse < -maxMotorForce ? -maxMotorForce: unclippedMotorImpulse;
- }
+ real_t clippedMotorImpulse;
+ ///@todo: should clip against accumulated impulse
+ if (unclippedMotorImpulse > 0.0f) {
+ clippedMotorImpulse = unclippedMotorImpulse > maxMotorForce ? maxMotorForce : unclippedMotorImpulse;
+ } else {
+ clippedMotorImpulse = unclippedMotorImpulse < -maxMotorForce ? -maxMotorForce : unclippedMotorImpulse;
+ }
// sort with accumulated impulses
- real_t lo = real_t(-1e30);
- real_t hi = real_t(1e30);
-
- real_t oldaccumImpulse = m_accumulatedImpulse;
- real_t sum = oldaccumImpulse + clippedMotorImpulse;
- m_accumulatedImpulse = sum > hi ? real_t(0.) : sum < lo ? real_t(0.) : sum;
-
- clippedMotorImpulse = m_accumulatedImpulse - oldaccumImpulse;
+ real_t lo = real_t(-1e30);
+ real_t hi = real_t(1e30);
+ real_t oldaccumImpulse = m_accumulatedImpulse;
+ real_t sum = oldaccumImpulse + clippedMotorImpulse;
+ m_accumulatedImpulse = sum > hi ? real_t(0.) : sum < lo ? real_t(0.) : sum;
+ clippedMotorImpulse = m_accumulatedImpulse - oldaccumImpulse;
- Vector3 motorImp = clippedMotorImpulse * axis;
-
-
- body0->apply_torque_impulse(motorImp);
- if (body1) body1->apply_torque_impulse(-motorImp);
-
- return clippedMotorImpulse;
+ Vector3 motorImp = clippedMotorImpulse * axis;
+ body0->apply_torque_impulse(motorImp);
+ if (body1) body1->apply_torque_impulse(-motorImp);
+ return clippedMotorImpulse;
}
//////////////////////////// End G6DOFRotationalLimitMotorSW ////////////////////////////////////
@@ -153,120 +125,96 @@ real_t G6DOFRotationalLimitMotorSW::solveAngularLimits(
//////////////////////////// G6DOFTranslationalLimitMotorSW ////////////////////////////////////
real_t G6DOFTranslationalLimitMotorSW::solveLinearAxis(
real_t timeStep,
- real_t jacDiagABInv,
- BodySW* body1,const Vector3 &pointInA,
- BodySW* body2,const Vector3 &pointInB,
- int limit_index,
- const Vector3 & axis_normal_on_a,
- const Vector3 & anchorPos)
-{
-
-///find relative velocity
-// Vector3 rel_pos1 = pointInA - body1->get_transform().origin;
-// Vector3 rel_pos2 = pointInB - body2->get_transform().origin;
- Vector3 rel_pos1 = anchorPos - body1->get_transform().origin;
- Vector3 rel_pos2 = anchorPos - body2->get_transform().origin;
+ real_t jacDiagABInv,
+ BodySW *body1, const Vector3 &pointInA,
+ BodySW *body2, const Vector3 &pointInB,
+ int limit_index,
+ const Vector3 &axis_normal_on_a,
+ const Vector3 &anchorPos) {
- Vector3 vel1 = body1->get_velocity_in_local_point(rel_pos1);
- Vector3 vel2 = body2->get_velocity_in_local_point(rel_pos2);
- Vector3 vel = vel1 - vel2;
+ ///find relative velocity
+ // Vector3 rel_pos1 = pointInA - body1->get_transform().origin;
+ // Vector3 rel_pos2 = pointInB - body2->get_transform().origin;
+ Vector3 rel_pos1 = anchorPos - body1->get_transform().origin;
+ Vector3 rel_pos2 = anchorPos - body2->get_transform().origin;
- real_t rel_vel = axis_normal_on_a.dot(vel);
+ Vector3 vel1 = body1->get_velocity_in_local_point(rel_pos1);
+ Vector3 vel2 = body2->get_velocity_in_local_point(rel_pos2);
+ Vector3 vel = vel1 - vel2;
+ real_t rel_vel = axis_normal_on_a.dot(vel);
+ /// apply displacement correction
-/// apply displacement correction
+ //positional error (zeroth order error)
+ real_t depth = -(pointInA - pointInB).dot(axis_normal_on_a);
+ real_t lo = real_t(-1e30);
+ real_t hi = real_t(1e30);
-//positional error (zeroth order error)
- real_t depth = -(pointInA - pointInB).dot(axis_normal_on_a);
- real_t lo = real_t(-1e30);
- real_t hi = real_t(1e30);
+ real_t minLimit = m_lowerLimit[limit_index];
+ real_t maxLimit = m_upperLimit[limit_index];
- real_t minLimit = m_lowerLimit[limit_index];
- real_t maxLimit = m_upperLimit[limit_index];
-
- //handle the limits
- if (minLimit < maxLimit)
- {
- {
- if (depth > maxLimit)
- {
- depth -= maxLimit;
- lo = real_t(0.);
-
- }
- else
- {
- if (depth < minLimit)
- {
- depth -= minLimit;
- hi = real_t(0.);
- }
- else
+ //handle the limits
+ if (minLimit < maxLimit) {
{
- return 0.0f;
+ if (depth > maxLimit) {
+ depth -= maxLimit;
+ lo = real_t(0.);
+
+ } else {
+ if (depth < minLimit) {
+ depth -= minLimit;
+ hi = real_t(0.);
+ } else {
+ return 0.0f;
+ }
+ }
}
- }
}
- }
-
- real_t normalImpulse= m_limitSoftness[limit_index]*(m_restitution[limit_index]*depth/timeStep - m_damping[limit_index]*rel_vel) * jacDiagABInv;
-
+ real_t normalImpulse = m_limitSoftness[limit_index] * (m_restitution[limit_index] * depth / timeStep - m_damping[limit_index] * rel_vel) * jacDiagABInv;
+ real_t oldNormalImpulse = m_accumulatedImpulse[limit_index];
+ real_t sum = oldNormalImpulse + normalImpulse;
+ m_accumulatedImpulse[limit_index] = sum > hi ? real_t(0.) : sum < lo ? real_t(0.) : sum;
+ normalImpulse = m_accumulatedImpulse[limit_index] - oldNormalImpulse;
- real_t oldNormalImpulse = m_accumulatedImpulse[limit_index];
- real_t sum = oldNormalImpulse + normalImpulse;
- m_accumulatedImpulse[limit_index] = sum > hi ? real_t(0.) : sum < lo ? real_t(0.) : sum;
- normalImpulse = m_accumulatedImpulse[limit_index] - oldNormalImpulse;
-
- Vector3 impulse_vector = axis_normal_on_a * normalImpulse;
- body1->apply_impulse( rel_pos1, impulse_vector);
- body2->apply_impulse( rel_pos2, -impulse_vector);
- return normalImpulse;
+ Vector3 impulse_vector = axis_normal_on_a * normalImpulse;
+ body1->apply_impulse(rel_pos1, impulse_vector);
+ body2->apply_impulse(rel_pos2, -impulse_vector);
+ return normalImpulse;
}
//////////////////////////// G6DOFTranslationalLimitMotorSW ////////////////////////////////////
-
-Generic6DOFJointSW::Generic6DOFJointSW(BodySW* rbA, BodySW* rbB, const Transform& frameInA, const Transform& frameInB, bool useLinearReferenceFrameA)
- : JointSW(_arr,2)
- , m_frameInA(frameInA)
- , m_frameInB(frameInB),
- m_useLinearReferenceFrameA(useLinearReferenceFrameA)
-{
- A=rbA;
- B=rbB;
- A->add_constraint(this,0);
- B->add_constraint(this,1);
+Generic6DOFJointSW::Generic6DOFJointSW(BodySW *rbA, BodySW *rbB, const Transform &frameInA, const Transform &frameInB, bool useLinearReferenceFrameA)
+ : JointSW(_arr, 2), m_frameInA(frameInA), m_frameInB(frameInB),
+ m_useLinearReferenceFrameA(useLinearReferenceFrameA) {
+ A = rbA;
+ B = rbB;
+ A->add_constraint(this, 0);
+ B->add_constraint(this, 1);
}
-
-
-
-
-void Generic6DOFJointSW::calculateAngleInfo()
-{
- Basis relative_frame = m_calculatedTransformA.basis.inverse()*m_calculatedTransformB.basis;
+void Generic6DOFJointSW::calculateAngleInfo() {
+ Basis relative_frame = m_calculatedTransformA.basis.inverse() * m_calculatedTransformB.basis;
m_calculatedAxisAngleDiff = relative_frame.get_euler();
-
-
// in euler angle mode we do not actually constrain the angular velocity
- // along the axes axis[0] and axis[2] (although we do use axis[1]) :
- //
- // to get constrain w2-w1 along ...not
- // ------ --------------------- ------
- // d(angle[0])/dt = 0 ax[1] x ax[2] ax[0]
- // d(angle[1])/dt = 0 ax[1]
- // d(angle[2])/dt = 0 ax[0] x ax[1] ax[2]
- //
- // constraining w2-w1 along an axis 'a' means that a'*(w2-w1)=0.
- // to prove the result for angle[0], write the expression for angle[0] from
- // GetInfo1 then take the derivative. to prove this for angle[2] it is
- // easier to take the euler rate expression for d(angle[2])/dt with respect
- // to the components of w and set that to 0.
+ // along the axes axis[0] and axis[2] (although we do use axis[1]) :
+ //
+ // to get constrain w2-w1 along ...not
+ // ------ --------------------- ------
+ // d(angle[0])/dt = 0 ax[1] x ax[2] ax[0]
+ // d(angle[1])/dt = 0 ax[1]
+ // d(angle[2])/dt = 0 ax[0] x ax[1] ax[2]
+ //
+ // constraining w2-w1 along an axis 'a' means that a'*(w2-w1)=0.
+ // to prove the result for angle[0], write the expression for angle[0] from
+ // GetInfo1 then take the derivative. to prove this for angle[2] it is
+ // easier to take the euler rate expression for d(angle[2])/dt with respect
+ // to the components of w and set that to 0.
Vector3 axis0 = m_calculatedTransformB.basis.get_axis(0);
Vector3 axis2 = m_calculatedTransformA.basis.get_axis(2);
@@ -275,7 +223,6 @@ void Generic6DOFJointSW::calculateAngleInfo()
m_calculatedAxis[0] = m_calculatedAxis[1].cross(axis2);
m_calculatedAxis[2] = axis0.cross(m_calculatedAxis[1]);
-
/*
if(m_debugDrawer)
{
@@ -288,280 +235,250 @@ void Generic6DOFJointSW::calculateAngleInfo()
m_debugDrawer->reportErrorWarning(buff);
}
*/
-
}
-void Generic6DOFJointSW::calculateTransforms()
-{
- m_calculatedTransformA = A->get_transform() * m_frameInA;
- m_calculatedTransformB = B->get_transform() * m_frameInB;
+void Generic6DOFJointSW::calculateTransforms() {
+ m_calculatedTransformA = A->get_transform() * m_frameInA;
+ m_calculatedTransformB = B->get_transform() * m_frameInB;
- calculateAngleInfo();
+ calculateAngleInfo();
}
-
void Generic6DOFJointSW::buildLinearJacobian(
- JacobianEntrySW & jacLinear,const Vector3 & normalWorld,
- const Vector3 & pivotAInW,const Vector3 & pivotBInW)
-{
- memnew_placement(&jacLinear, JacobianEntrySW(
- A->get_principal_inertia_axes().transposed(),
- B->get_principal_inertia_axes().transposed(),
- pivotAInW - A->get_transform().origin - A->get_center_of_mass(),
- pivotBInW - B->get_transform().origin - B->get_center_of_mass(),
- normalWorld,
- A->get_inv_inertia(),
- A->get_inv_mass(),
- B->get_inv_inertia(),
- B->get_inv_mass()));
-
+ JacobianEntrySW &jacLinear, const Vector3 &normalWorld,
+ const Vector3 &pivotAInW, const Vector3 &pivotBInW) {
+ memnew_placement(&jacLinear, JacobianEntrySW(
+ A->get_principal_inertia_axes().transposed(),
+ B->get_principal_inertia_axes().transposed(),
+ pivotAInW - A->get_transform().origin - A->get_center_of_mass(),
+ pivotBInW - B->get_transform().origin - B->get_center_of_mass(),
+ normalWorld,
+ A->get_inv_inertia(),
+ A->get_inv_mass(),
+ B->get_inv_inertia(),
+ B->get_inv_mass()));
}
void Generic6DOFJointSW::buildAngularJacobian(
- JacobianEntrySW & jacAngular,const Vector3 & jointAxisW)
-{
- memnew_placement(&jacAngular, JacobianEntrySW(jointAxisW,
- A->get_principal_inertia_axes().transposed(),
- B->get_principal_inertia_axes().transposed(),
- A->get_inv_inertia(),
- B->get_inv_inertia()));
-
+ JacobianEntrySW &jacAngular, const Vector3 &jointAxisW) {
+ memnew_placement(&jacAngular, JacobianEntrySW(jointAxisW,
+ A->get_principal_inertia_axes().transposed(),
+ B->get_principal_inertia_axes().transposed(),
+ A->get_inv_inertia(),
+ B->get_inv_inertia()));
}
-bool Generic6DOFJointSW::testAngularLimitMotor(int axis_index)
-{
- real_t angle = m_calculatedAxisAngleDiff[axis_index];
+bool Generic6DOFJointSW::testAngularLimitMotor(int axis_index) {
+ real_t angle = m_calculatedAxisAngleDiff[axis_index];
- //test limits
- m_angularLimits[axis_index].testLimitValue(angle);
- return m_angularLimits[axis_index].needApplyTorques();
+ //test limits
+ m_angularLimits[axis_index].testLimitValue(angle);
+ return m_angularLimits[axis_index].needApplyTorques();
}
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.));
- int i;
- for(i = 0; i < 3; i++)
- {
- m_angularLimits[i].m_accumulatedImpulse = real_t(0.);
- }
- //calculates transform
- calculateTransforms();
+ m_linearLimits.m_accumulatedImpulse = Vector3(real_t(0.), real_t(0.), real_t(0.));
+ int i;
+ for (i = 0; i < 3; i++) {
+ m_angularLimits[i].m_accumulatedImpulse = real_t(0.);
+ }
+ //calculates transform
+ calculateTransforms();
-// const Vector3& pivotAInW = m_calculatedTransformA.origin;
-// const Vector3& pivotBInW = m_calculatedTransformB.origin;
+ // const Vector3& pivotAInW = m_calculatedTransformA.origin;
+ // const Vector3& pivotBInW = m_calculatedTransformB.origin;
calcAnchorPos();
Vector3 pivotAInW = m_AnchorPos;
Vector3 pivotBInW = m_AnchorPos;
-// not used here
-// Vector3 rel_pos1 = pivotAInW - A->get_transform().origin;
-// Vector3 rel_pos2 = pivotBInW - B->get_transform().origin;
+ // not used here
+ // Vector3 rel_pos1 = pivotAInW - A->get_transform().origin;
+ // Vector3 rel_pos2 = pivotBInW - B->get_transform().origin;
- Vector3 normalWorld;
- //linear part
- for (i=0;i<3;i++)
- {
- if (m_linearLimits.enable_limit[i] && m_linearLimits.isLimited(i))
- {
+ Vector3 normalWorld;
+ //linear part
+ for (i = 0; i < 3; i++) {
+ if (m_linearLimits.enable_limit[i] && m_linearLimits.isLimited(i)) {
if (m_useLinearReferenceFrameA)
- normalWorld = m_calculatedTransformA.basis.get_axis(i);
+ normalWorld = m_calculatedTransformA.basis.get_axis(i);
else
- normalWorld = m_calculatedTransformB.basis.get_axis(i);
-
- buildLinearJacobian(
- m_jacLinear[i],normalWorld ,
- pivotAInW,pivotBInW);
+ normalWorld = m_calculatedTransformB.basis.get_axis(i);
+ buildLinearJacobian(
+ m_jacLinear[i], normalWorld,
+ pivotAInW, pivotBInW);
+ }
}
- }
- // angular part
- for (i=0;i<3;i++)
- {
- //calculates error angle
- if (m_angularLimits[i].m_enableLimit && testAngularLimitMotor(i))
- {
- normalWorld = this->getAxis(i);
- // Create angular atom
- buildAngularJacobian(m_jacAng[i],normalWorld);
+ // angular part
+ for (i = 0; i < 3; i++) {
+ //calculates error angle
+ if (m_angularLimits[i].m_enableLimit && testAngularLimitMotor(i)) {
+ normalWorld = this->getAxis(i);
+ // Create angular atom
+ buildAngularJacobian(m_jacAng[i], normalWorld);
+ }
}
- }
return true;
}
+void Generic6DOFJointSW::solve(real_t timeStep) {
+ m_timeStep = timeStep;
-void Generic6DOFJointSW::solve(real_t timeStep)
-{
- m_timeStep = timeStep;
-
- //calculateTransforms();
+ //calculateTransforms();
- int i;
+ int i;
- // linear
+ // linear
- Vector3 pointInA = m_calculatedTransformA.origin;
+ Vector3 pointInA = m_calculatedTransformA.origin;
Vector3 pointInB = m_calculatedTransformB.origin;
real_t jacDiagABInv;
Vector3 linear_axis;
- for (i=0;i<3;i++)
- {
- if (m_linearLimits.enable_limit[i] && m_linearLimits.isLimited(i))
- {
- jacDiagABInv = real_t(1.) / m_jacLinear[i].getDiagonal();
+ for (i = 0; i < 3; i++) {
+ if (m_linearLimits.enable_limit[i] && m_linearLimits.isLimited(i)) {
+ jacDiagABInv = real_t(1.) / m_jacLinear[i].getDiagonal();
if (m_useLinearReferenceFrameA)
- linear_axis = m_calculatedTransformA.basis.get_axis(i);
+ linear_axis = m_calculatedTransformA.basis.get_axis(i);
else
- linear_axis = m_calculatedTransformB.basis.get_axis(i);
-
- m_linearLimits.solveLinearAxis(
- m_timeStep,
- jacDiagABInv,
- A,pointInA,
- B,pointInB,
- i,linear_axis, m_AnchorPos);
+ linear_axis = m_calculatedTransformB.basis.get_axis(i);
+ m_linearLimits.solveLinearAxis(
+ m_timeStep,
+ jacDiagABInv,
+ A, pointInA,
+ B, pointInB,
+ i, linear_axis, m_AnchorPos);
+ }
}
- }
- // angular
- Vector3 angular_axis;
- real_t angularJacDiagABInv;
- for (i=0;i<3;i++)
- {
- if (m_angularLimits[i].m_enableLimit && m_angularLimits[i].needApplyTorques())
- {
+ // angular
+ Vector3 angular_axis;
+ real_t angularJacDiagABInv;
+ for (i = 0; i < 3; i++) {
+ if (m_angularLimits[i].m_enableLimit && m_angularLimits[i].needApplyTorques()) {
// get axis
angular_axis = getAxis(i);
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);
+ }
}
- }
}
-void Generic6DOFJointSW::updateRHS(real_t timeStep)
-{
- (void)timeStep;
-
+void Generic6DOFJointSW::updateRHS(real_t timeStep) {
+ (void)timeStep;
}
-Vector3 Generic6DOFJointSW::getAxis(int axis_index) const
-{
- return m_calculatedAxis[axis_index];
+Vector3 Generic6DOFJointSW::getAxis(int axis_index) const {
+ return m_calculatedAxis[axis_index];
}
-real_t Generic6DOFJointSW::getAngle(int axis_index) const
-{
- return m_calculatedAxisAngleDiff[axis_index];
+real_t Generic6DOFJointSW::getAngle(int axis_index) const {
+ return m_calculatedAxisAngleDiff[axis_index];
}
-void Generic6DOFJointSW::calcAnchorPos(void)
-{
+void Generic6DOFJointSW::calcAnchorPos(void) {
real_t imA = A->get_inv_mass();
real_t imB = B->get_inv_mass();
real_t weight;
- if(imB == real_t(0.0))
- {
+ if (imB == real_t(0.0)) {
weight = real_t(1.0);
- }
- else
- {
+ } else {
weight = imA / (imA + imB);
}
- const Vector3& pA = m_calculatedTransformA.origin;
- const Vector3& pB = m_calculatedTransformB.origin;
+ const Vector3 &pA = m_calculatedTransformA.origin;
+ const Vector3 &pB = m_calculatedTransformB.origin;
m_AnchorPos = pA * weight + pB * (real_t(1.0) - weight);
return;
} // Generic6DOFJointSW::calcAnchorPos()
+void Generic6DOFJointSW::set_param(Vector3::Axis p_axis, PhysicsServer::G6DOFJointAxisParam p_param, real_t 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) {
+ ERR_FAIL_INDEX(p_axis, 3);
+ switch (p_param) {
case PhysicsServer::G6DOF_JOINT_LINEAR_LOWER_LIMIT: {
- m_linearLimits.m_lowerLimit[p_axis]=p_value;
+ m_linearLimits.m_lowerLimit[p_axis] = p_value;
} break;
case PhysicsServer::G6DOF_JOINT_LINEAR_UPPER_LIMIT: {
- m_linearLimits.m_upperLimit[p_axis]=p_value;
+ m_linearLimits.m_upperLimit[p_axis] = p_value;
} break;
case PhysicsServer::G6DOF_JOINT_LINEAR_LIMIT_SOFTNESS: {
- m_linearLimits.m_limitSoftness[p_axis]=p_value;
+ m_linearLimits.m_limitSoftness[p_axis] = p_value;
} break;
case PhysicsServer::G6DOF_JOINT_LINEAR_RESTITUTION: {
- m_linearLimits.m_restitution[p_axis]=p_value;
+ m_linearLimits.m_restitution[p_axis] = p_value;
} break;
case PhysicsServer::G6DOF_JOINT_LINEAR_DAMPING: {
- m_linearLimits.m_damping[p_axis]=p_value;
+ m_linearLimits.m_damping[p_axis] = p_value;
} break;
case PhysicsServer::G6DOF_JOINT_ANGULAR_LOWER_LIMIT: {
- m_angularLimits[p_axis].m_loLimit=p_value;
+ m_angularLimits[p_axis].m_loLimit = p_value;
} break;
case PhysicsServer::G6DOF_JOINT_ANGULAR_UPPER_LIMIT: {
- m_angularLimits[p_axis].m_hiLimit=p_value;
+ m_angularLimits[p_axis].m_hiLimit = p_value;
} break;
case PhysicsServer::G6DOF_JOINT_ANGULAR_LIMIT_SOFTNESS: {
- m_angularLimits[p_axis].m_limitSoftness=p_value;
+ m_angularLimits[p_axis].m_limitSoftness = p_value;
} break;
case PhysicsServer::G6DOF_JOINT_ANGULAR_DAMPING: {
- m_angularLimits[p_axis].m_damping=p_value;
+ m_angularLimits[p_axis].m_damping = p_value;
} break;
case PhysicsServer::G6DOF_JOINT_ANGULAR_RESTITUTION: {
- m_angularLimits[p_axis].m_bounce=p_value;
+ m_angularLimits[p_axis].m_bounce = p_value;
} break;
case PhysicsServer::G6DOF_JOINT_ANGULAR_FORCE_LIMIT: {
- m_angularLimits[p_axis].m_maxLimitForce=p_value;
+ m_angularLimits[p_axis].m_maxLimitForce = p_value;
} break;
case PhysicsServer::G6DOF_JOINT_ANGULAR_ERP: {
- m_angularLimits[p_axis].m_ERP=p_value;
+ m_angularLimits[p_axis].m_ERP = p_value;
} break;
case PhysicsServer::G6DOF_JOINT_ANGULAR_MOTOR_TARGET_VELOCITY: {
- m_angularLimits[p_axis].m_targetVelocity=p_value;
+ m_angularLimits[p_axis].m_targetVelocity = p_value;
} break;
case PhysicsServer::G6DOF_JOINT_ANGULAR_MOTOR_FORCE_LIMIT: {
- m_angularLimits[p_axis].m_maxLimitForce=p_value;
+ m_angularLimits[p_axis].m_maxLimitForce = p_value;
} break;
}
}
-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) {
+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: {
return m_linearLimits.m_lowerLimit[p_axis];
@@ -635,31 +552,29 @@ real_t Generic6DOFJointSW::get_param(Vector3::Axis p_axis,PhysicsServer::G6DOFJo
return 0;
}
-void Generic6DOFJointSW::set_flag(Vector3::Axis p_axis,PhysicsServer::G6DOFJointAxisFlag p_flag, bool p_value){
+void Generic6DOFJointSW::set_flag(Vector3::Axis p_axis, PhysicsServer::G6DOFJointAxisFlag p_flag, bool p_value) {
- ERR_FAIL_INDEX(p_axis,3);
+ ERR_FAIL_INDEX(p_axis, 3);
- switch(p_flag) {
+ switch (p_flag) {
case PhysicsServer::G6DOF_JOINT_FLAG_ENABLE_LINEAR_LIMIT: {
- m_linearLimits.enable_limit[p_axis]=p_value;
+ m_linearLimits.enable_limit[p_axis] = p_value;
} break;
case PhysicsServer::G6DOF_JOINT_FLAG_ENABLE_ANGULAR_LIMIT: {
- m_angularLimits[p_axis].m_enableLimit=p_value;
+ m_angularLimits[p_axis].m_enableLimit = p_value;
} break;
case PhysicsServer::G6DOF_JOINT_FLAG_ENABLE_MOTOR: {
- m_angularLimits[p_axis].m_enableMotor=p_value;
+ m_angularLimits[p_axis].m_enableMotor = p_value;
} break;
}
-
-
}
-bool Generic6DOFJointSW::get_flag(Vector3::Axis p_axis,PhysicsServer::G6DOFJointAxisFlag p_flag) const{
+bool Generic6DOFJointSW::get_flag(Vector3::Axis p_axis, PhysicsServer::G6DOFJointAxisFlag p_flag) const {
- ERR_FAIL_INDEX_V(p_axis,3,0);
- switch(p_flag) {
+ ERR_FAIL_INDEX_V(p_axis, 3, 0);
+ switch (p_flag) {
case PhysicsServer::G6DOF_JOINT_FLAG_ENABLE_LINEAR_LIMIT: {
return m_linearLimits.enable_limit[p_axis];
diff --git a/servers/physics/joints/generic_6dof_joint_sw.h b/servers/physics/joints/generic_6dof_joint_sw.h
index 207ae85a4..87245c6ff 100644
--- a/servers/physics/joints/generic_6dof_joint_sw.h
+++ b/servers/physics/joints/generic_6dof_joint_sw.h
@@ -34,9 +34,8 @@ Adapted to Godot from the Bullet library.
#ifndef GENERIC_6DOF_JOINT_SW_H
#define GENERIC_6DOF_JOINT_SW_H
-#include "servers/physics/joints_sw.h"
#include "servers/physics/joints/jacobian_entry_sw.h"
-
+#include "servers/physics/joints_sw.h"
/*
Bullet Continuous Collision Detection and Physics Library
@@ -53,7 +52,6 @@ subject to the following restrictions:
3. This notice may not be removed or altered from any source distribution.
*/
-
/*
2007-09-09
Generic6DOFJointSW Refactored by Francisco Le?n
@@ -61,80 +59,73 @@ email: projectileman@yahoo.com
http://gimpact.sf.net
*/
-
//! Rotation Limit structure for generic joints
class G6DOFRotationalLimitMotorSW {
public:
- //! limit_parameters
- //!@{
- real_t m_loLimit;//!< joint limit
- real_t m_hiLimit;//!< joint limit
- real_t m_targetVelocity;//!< target motor velocity
- real_t m_maxMotorForce;//!< max force on motor
- real_t m_maxLimitForce;//!< max force on limit
- real_t m_damping;//!< Damping.
- real_t m_limitSoftness;//! Relaxation factor
- real_t m_ERP;//!< Error tolerance factor when joint is at limit
- real_t m_bounce;//!< restitution factor
- bool m_enableMotor;
- bool m_enableLimit;
+ //! limit_parameters
+ //!@{
+ real_t m_loLimit; //!< joint limit
+ real_t m_hiLimit; //!< joint limit
+ real_t m_targetVelocity; //!< target motor velocity
+ real_t m_maxMotorForce; //!< max force on motor
+ real_t m_maxLimitForce; //!< max force on limit
+ real_t m_damping; //!< Damping.
+ real_t m_limitSoftness; //! Relaxation factor
+ real_t m_ERP; //!< Error tolerance factor when joint is at limit
+ real_t m_bounce; //!< restitution factor
+ bool m_enableMotor;
+ bool m_enableLimit;
- //!@}
-
- //! temp_variables
- //!@{
- 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;
- //!@}
-
- G6DOFRotationalLimitMotorSW()
- {
- m_accumulatedImpulse = 0.f;
- m_targetVelocity = 0;
- m_maxMotorForce = 0.1f;
- m_maxLimitForce = 300.0f;
- m_loLimit = -1e30;
- m_hiLimit = 1e30;
- m_ERP = 0.5f;
- m_bounce = 0.0f;
- m_damping = 1.0f;
- m_limitSoftness = 0.5f;
- m_currentLimit = 0;
- m_currentLimitError = 0;
- m_enableMotor = false;
- m_enableLimit=false;
- }
+ //!@}
- G6DOFRotationalLimitMotorSW(const G6DOFRotationalLimitMotorSW & limot)
- {
- m_targetVelocity = limot.m_targetVelocity;
- m_maxMotorForce = limot.m_maxMotorForce;
- m_limitSoftness = limot.m_limitSoftness;
- m_loLimit = limot.m_loLimit;
- m_hiLimit = limot.m_hiLimit;
- m_ERP = limot.m_ERP;
- m_bounce = limot.m_bounce;
- m_currentLimit = limot.m_currentLimit;
- m_currentLimitError = limot.m_currentLimitError;
- m_enableMotor = limot.m_enableMotor;
- }
+ //! temp_variables
+ //!@{
+ 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;
+ //!@}
+ G6DOFRotationalLimitMotorSW() {
+ m_accumulatedImpulse = 0.f;
+ m_targetVelocity = 0;
+ m_maxMotorForce = 0.1f;
+ m_maxLimitForce = 300.0f;
+ m_loLimit = -1e30;
+ m_hiLimit = 1e30;
+ m_ERP = 0.5f;
+ m_bounce = 0.0f;
+ m_damping = 1.0f;
+ m_limitSoftness = 0.5f;
+ m_currentLimit = 0;
+ m_currentLimitError = 0;
+ m_enableMotor = false;
+ m_enableLimit = false;
+ }
+ G6DOFRotationalLimitMotorSW(const G6DOFRotationalLimitMotorSW &limot) {
+ m_targetVelocity = limot.m_targetVelocity;
+ m_maxMotorForce = limot.m_maxMotorForce;
+ m_limitSoftness = limot.m_limitSoftness;
+ m_loLimit = limot.m_loLimit;
+ m_hiLimit = limot.m_hiLimit;
+ m_ERP = limot.m_ERP;
+ m_bounce = limot.m_bounce;
+ m_currentLimit = limot.m_currentLimit;
+ m_currentLimitError = limot.m_currentLimitError;
+ m_enableMotor = limot.m_enableMotor;
+ }
//! Is limited
- bool isLimited()
- {
- if(m_loLimit>=m_hiLimit) return false;
- return true;
- }
+ bool isLimited() {
+ if (m_loLimit >= m_hiLimit) return false;
+ return true;
+ }
//! Need apply correction
- bool needApplyTorques()
- {
- if(m_currentLimit == 0 && m_enableMotor == false) return false;
- return true;
- }
+ bool needApplyTorques() {
+ if (m_currentLimit == 0 && m_enableMotor == false) return false;
+ return true;
+ }
//! calculates error
/*!
@@ -143,84 +134,69 @@ 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,BodySW * body0, BodySW * body1);
-
-
+ real_t solveAngularLimits(real_t timeStep, Vector3 &axis, real_t jacDiagABInv, BodySW *body0, BodySW *body1);
};
-
-
-class G6DOFTranslationalLimitMotorSW
-{
+class G6DOFTranslationalLimitMotorSW {
public:
- Vector3 m_lowerLimit;//!< the constraint lower limits
- Vector3 m_upperLimit;//!< the constraint upper limits
- Vector3 m_accumulatedImpulse;
- //! Linear_Limit_parameters
- //!@{
- Vector3 m_limitSoftness;//!< Softness for linear limit
- Vector3 m_damping;//!< Damping for linear limit
- Vector3 m_restitution;//! Bounce parameter for linear limit
- //!@}
- bool enable_limit[3];
+ Vector3 m_lowerLimit; //!< the constraint lower limits
+ Vector3 m_upperLimit; //!< the constraint upper limits
+ Vector3 m_accumulatedImpulse;
+ //! Linear_Limit_parameters
+ //!@{
+ Vector3 m_limitSoftness; //!< Softness for linear limit
+ Vector3 m_damping; //!< Damping for linear limit
+ Vector3 m_restitution; //! Bounce parameter for linear limit
+ //!@}
+ bool enable_limit[3];
- G6DOFTranslationalLimitMotorSW()
- {
- m_lowerLimit=Vector3(0.f,0.f,0.f);
- m_upperLimit=Vector3(0.f,0.f,0.f);
- m_accumulatedImpulse=Vector3(0.f,0.f,0.f);
+ G6DOFTranslationalLimitMotorSW() {
+ m_lowerLimit = Vector3(0.f, 0.f, 0.f);
+ m_upperLimit = Vector3(0.f, 0.f, 0.f);
+ m_accumulatedImpulse = Vector3(0.f, 0.f, 0.f);
- m_limitSoftness = Vector3(1,1,1)*0.7f;
- m_damping = Vector3(1,1,1)*real_t(1.0f);
- m_restitution = Vector3(1,1,1)*real_t(0.5f);
+ m_limitSoftness = Vector3(1, 1, 1) * 0.7f;
+ m_damping = Vector3(1, 1, 1) * real_t(1.0f);
+ m_restitution = Vector3(1, 1, 1) * real_t(0.5f);
- enable_limit[0]=true;
- enable_limit[1]=true;
- enable_limit[2]=true;
- }
+ enable_limit[0] = true;
+ enable_limit[1] = true;
+ enable_limit[2] = true;
+ }
- G6DOFTranslationalLimitMotorSW(const G6DOFTranslationalLimitMotorSW & other )
- {
- m_lowerLimit = other.m_lowerLimit;
- m_upperLimit = other.m_upperLimit;
- m_accumulatedImpulse = other.m_accumulatedImpulse;
+ G6DOFTranslationalLimitMotorSW(const G6DOFTranslationalLimitMotorSW &other) {
+ m_lowerLimit = other.m_lowerLimit;
+ m_upperLimit = other.m_upperLimit;
+ m_accumulatedImpulse = other.m_accumulatedImpulse;
- m_limitSoftness = other.m_limitSoftness ;
- m_damping = other.m_damping;
- m_restitution = other.m_restitution;
- }
+ m_limitSoftness = other.m_limitSoftness;
+ m_damping = other.m_damping;
+ m_restitution = other.m_restitution;
+ }
- //! Test limit
+ //! Test limit
/*!
- free means upper < lower,
- locked means upper == lower
- limited means upper > lower
- limitIndex: first 3 are linear, next 3 are angular
*/
- inline bool isLimited(int limitIndex)
- {
- return (m_upperLimit[limitIndex] >= m_lowerLimit[limitIndex]);
- }
-
-
- real_t solveLinearAxis(
- real_t timeStep,
- real_t jacDiagABInv,
- BodySW* body1,const Vector3 &pointInA,
- BodySW* body2,const Vector3 &pointInB,
- int limit_index,
- const Vector3 & axis_normal_on_a,
- const Vector3 & anchorPos);
-
+ inline bool isLimited(int limitIndex) {
+ return (m_upperLimit[limitIndex] >= m_lowerLimit[limitIndex]);
+ }
+ real_t solveLinearAxis(
+ real_t timeStep,
+ real_t jacDiagABInv,
+ BodySW *body1, const Vector3 &pointInA,
+ BodySW *body2, const Vector3 &pointInB,
+ int limit_index,
+ const Vector3 &axis_normal_on_a,
+ const Vector3 &anchorPos);
};
-
-class Generic6DOFJointSW : public JointSW
-{
+class Generic6DOFJointSW : public JointSW {
protected:
-
-
union {
struct {
BodySW *A;
@@ -231,195 +207,167 @@ 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
- //!@}
+ //!@{
+ Transform m_frameInA; //!< the constraint space w.r.t body A
+ Transform m_frameInB; //!< the constraint space w.r.t body B
+ //!@}
- //! Jacobians
- //!@{
- JacobianEntrySW m_jacLinear[3];//!< 3 orthogonal linear constraints
- JacobianEntrySW m_jacAng[3];//!< 3 orthogonal angular constraints
- //!@}
+ //! Jacobians
+ //!@{
+ JacobianEntrySW m_jacLinear[3]; //!< 3 orthogonal linear constraints
+ JacobianEntrySW m_jacAng[3]; //!< 3 orthogonal angular constraints
+ //!@}
//! Linear_Limit_parameters
- //!@{
- G6DOFTranslationalLimitMotorSW m_linearLimits;
- //!@}
-
-
- //! hinge_parameters
- //!@{
- G6DOFRotationalLimitMotorSW m_angularLimits[3];
+ //!@{
+ G6DOFTranslationalLimitMotorSW m_linearLimits;
//!@}
+ //! hinge_parameters
+ //!@{
+ G6DOFRotationalLimitMotorSW m_angularLimits[3];
+ //!@}
protected:
- //! temporal variables
- //!@{
- real_t m_timeStep;
- Transform m_calculatedTransformA;
- Transform m_calculatedTransformB;
- Vector3 m_calculatedAxisAngleDiff;
- Vector3 m_calculatedAxis[3];
+ //! temporal variables
+ //!@{
+ real_t m_timeStep;
+ Transform m_calculatedTransformA;
+ Transform m_calculatedTransformB;
+ Vector3 m_calculatedAxisAngleDiff;
+ Vector3 m_calculatedAxis[3];
Vector3 m_AnchorPos; // point betwen pivots of bodies A and B to solve linear axes
- bool m_useLinearReferenceFrameA;
-
- //!@}
-
- Generic6DOFJointSW& operator=(Generic6DOFJointSW& other)
- {
- ERR_PRINT("pito");
- (void) other;
- return *this;
- }
-
+ bool m_useLinearReferenceFrameA;
+ //!@}
- void buildLinearJacobian(
- JacobianEntrySW & jacLinear,const Vector3 & normalWorld,
- const Vector3 & pivotAInW,const Vector3 & pivotBInW);
+ Generic6DOFJointSW &operator=(Generic6DOFJointSW &other) {
+ ERR_PRINT("pito");
+ (void)other;
+ return *this;
+ }
- void buildAngularJacobian(JacobianEntrySW & jacAngular,const Vector3 & jointAxisW);
+ void buildLinearJacobian(
+ JacobianEntrySW &jacLinear, const Vector3 &normalWorld,
+ const Vector3 &pivotAInW, const Vector3 &pivotBInW);
+ void buildAngularJacobian(JacobianEntrySW &jacAngular, const Vector3 &jointAxisW);
//! calcs the euler angles between the two bodies.
- void calculateAngleInfo();
-
-
+ void calculateAngleInfo();
public:
- Generic6DOFJointSW(BodySW* rbA, BodySW* rbB, const Transform& frameInA, const Transform& frameInB ,bool useLinearReferenceFrameA);
-
- virtual PhysicsServer::JointType get_type() const { return PhysicsServer::JOINT_6DOF; }
+ Generic6DOFJointSW(BodySW *rbA, BodySW *rbB, const Transform &frameInA, const Transform &frameInB, bool useLinearReferenceFrameA);
- virtual bool setup(real_t p_step);
- virtual void solve(real_t p_step);
+ virtual PhysicsServer::JointType get_type() const { return PhysicsServer::JOINT_6DOF; }
+ virtual bool setup(real_t p_step);
+ virtual void solve(real_t p_step);
//! Calcs global transform of the offsets
/*!
Calcs the global transform for the joint offset for body A an B, and also calcs the agle differences between the bodies.
\sa Generic6DOFJointSW.getCalculatedTransformA , Generic6DOFJointSW.getCalculatedTransformB, Generic6DOFJointSW.calculateAngleInfo
*/
- void calculateTransforms();
+ void calculateTransforms();
//! Gets the global transform of the offset for body A
- /*!
+ /*!
\sa Generic6DOFJointSW.getFrameOffsetA, Generic6DOFJointSW.getFrameOffsetB, Generic6DOFJointSW.calculateAngleInfo.
*/
- const Transform & getCalculatedTransformA() const
- {
- return m_calculatedTransformA;
- }
+ const Transform &getCalculatedTransformA() const {
+ return m_calculatedTransformA;
+ }
- //! Gets the global transform of the offset for body B
- /*!
+ //! Gets the global transform of the offset for body B
+ /*!
\sa Generic6DOFJointSW.getFrameOffsetA, Generic6DOFJointSW.getFrameOffsetB, Generic6DOFJointSW.calculateAngleInfo.
*/
- const Transform & getCalculatedTransformB() const
- {
- return m_calculatedTransformB;
- }
-
- const Transform & getFrameOffsetA() const
- {
- return m_frameInA;
- }
-
- const Transform & getFrameOffsetB() const
- {
- return m_frameInB;
- }
+ const Transform &getCalculatedTransformB() const {
+ return m_calculatedTransformB;
+ }
+ const Transform &getFrameOffsetA() const {
+ return m_frameInA;
+ }
- Transform & getFrameOffsetA()
- {
- return m_frameInA;
- }
+ const Transform &getFrameOffsetB() const {
+ return m_frameInB;
+ }
- Transform & getFrameOffsetB()
- {
- return m_frameInB;
- }
+ Transform &getFrameOffsetA() {
+ return m_frameInA;
+ }
+ Transform &getFrameOffsetB() {
+ return m_frameInB;
+ }
//! performs Jacobian calculation, and also calculates angle differences and axis
-
- void updateRHS(real_t timeStep);
+ void updateRHS(real_t timeStep);
//! Get the rotation axis in global coordinates
/*!
\pre Generic6DOFJointSW.buildJacobian must be called previously.
*/
- Vector3 getAxis(int axis_index) const;
+ Vector3 getAxis(int axis_index) const;
- //! Get the relative Euler angle
- /*!
+ //! Get the relative Euler angle
+ /*!
\pre Generic6DOFJointSW.buildJacobian must be called previously.
*/
- real_t getAngle(int axis_index) const;
+ real_t getAngle(int axis_index) const;
//! Test angular limit.
/*!
Calculates angular correction and returns true if limit needs to be corrected.
\pre Generic6DOFJointSW.buildJacobian must be called previously.
*/
- bool testAngularLimitMotor(int axis_index);
+ bool testAngularLimitMotor(int axis_index);
- void setLinearLowerLimit(const Vector3& linearLower)
- {
- m_linearLimits.m_lowerLimit = linearLower;
- }
+ void setLinearLowerLimit(const Vector3 &linearLower) {
+ m_linearLimits.m_lowerLimit = linearLower;
+ }
- void setLinearUpperLimit(const Vector3& linearUpper)
- {
- m_linearLimits.m_upperLimit = linearUpper;
- }
+ void setLinearUpperLimit(const Vector3 &linearUpper) {
+ m_linearLimits.m_upperLimit = linearUpper;
+ }
- void setAngularLowerLimit(const Vector3& angularLower)
- {
- m_angularLimits[0].m_loLimit = angularLower.x;
- m_angularLimits[1].m_loLimit = angularLower.y;
- m_angularLimits[2].m_loLimit = angularLower.z;
- }
+ void setAngularLowerLimit(const Vector3 &angularLower) {
+ m_angularLimits[0].m_loLimit = angularLower.x;
+ m_angularLimits[1].m_loLimit = angularLower.y;
+ m_angularLimits[2].m_loLimit = angularLower.z;
+ }
- void setAngularUpperLimit(const Vector3& angularUpper)
- {
- m_angularLimits[0].m_hiLimit = angularUpper.x;
- m_angularLimits[1].m_hiLimit = angularUpper.y;
- m_angularLimits[2].m_hiLimit = angularUpper.z;
- }
+ void setAngularUpperLimit(const Vector3 &angularUpper) {
+ m_angularLimits[0].m_hiLimit = angularUpper.x;
+ m_angularLimits[1].m_hiLimit = angularUpper.y;
+ m_angularLimits[2].m_hiLimit = angularUpper.z;
+ }
//! Retrieves the angular limit informacion
- G6DOFRotationalLimitMotorSW * getRotationalLimitMotor(int index)
- {
- return &m_angularLimits[index];
- }
-
- //! Retrieves the limit informacion
- G6DOFTranslationalLimitMotorSW * getTranslationalLimitMotor()
- {
- return &m_linearLimits;
- }
+ G6DOFRotationalLimitMotorSW *getRotationalLimitMotor(int index) {
+ return &m_angularLimits[index];
+ }
- //first 3 are linear, next 3 are angular
- void setLimit(int axis, real_t lo, real_t hi)
- {
- if(axis<3)
- {
- m_linearLimits.m_lowerLimit[axis] = lo;
- m_linearLimits.m_upperLimit[axis] = hi;
+ //! Retrieves the limit informacion
+ G6DOFTranslationalLimitMotorSW *getTranslationalLimitMotor() {
+ return &m_linearLimits;
}
- else
- {
- m_angularLimits[axis-3].m_loLimit = lo;
- m_angularLimits[axis-3].m_hiLimit = hi;
+
+ //first 3 are linear, next 3 are angular
+ void setLimit(int axis, real_t lo, real_t hi) {
+ if (axis < 3) {
+ m_linearLimits.m_lowerLimit[axis] = lo;
+ m_linearLimits.m_upperLimit[axis] = hi;
+ } else {
+ m_angularLimits[axis - 3].m_loLimit = lo;
+ m_angularLimits[axis - 3].m_hiLimit = hi;
+ }
}
- }
//! Test limit
/*!
@@ -428,34 +376,27 @@ public:
- limited means upper > lower
- limitIndex: first 3 are linear, next 3 are angular
*/
- bool isLimited(int limitIndex)
- {
- if(limitIndex<3)
- {
+ bool isLimited(int limitIndex) {
+ if (limitIndex < 3) {
return m_linearLimits.isLimited(limitIndex);
-
+ }
+ return m_angularLimits[limitIndex - 3].isLimited();
}
- return m_angularLimits[limitIndex-3].isLimited();
- }
- const BodySW* getRigidBodyA() const
- {
- return A;
- }
- const BodySW* getRigidBodyB() const
- {
- return B;
- }
+ const BodySW *getRigidBodyA() const {
+ return A;
+ }
+ const BodySW *getRigidBodyB() const {
+ return B;
+ }
virtual void calcAnchorPos(void); // overridable
- 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;
+ 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;
};
-
#endif // GENERIC_6DOF_JOINT_SW_H
diff --git a/servers/physics/joints/hinge_joint_sw.cpp b/servers/physics/joints/hinge_joint_sw.cpp
index 9617eb879..eaa57af87 100644
--- a/servers/physics/joints/hinge_joint_sw.cpp
+++ b/servers/physics/joints/hinge_joint_sw.cpp
@@ -34,65 +34,63 @@ See corresponding header file for licensing info.
#include "hinge_joint_sw.h"
-static void plane_space(const Vector3& n, Vector3& p, Vector3& q) {
+static void plane_space(const Vector3 &n, Vector3 &p, Vector3 &q) {
- if (Math::abs(n.z) > 0.707106781186547524400844362) {
- // choose p in y-z plane
- real_t a = n[1]*n[1] + n[2]*n[2];
- real_t k = 1.0/Math::sqrt(a);
- p=Vector3(0,-n[2]*k,n[1]*k);
- // set q = n x p
- q=Vector3(a*k,-n[0]*p[2],n[0]*p[1]);
- }
- else {
- // choose p in x-y plane
- real_t a = n.x*n.x + n.y*n.y;
- real_t k = 1.0/Math::sqrt(a);
- p=Vector3(-n.y*k,n.x*k,0);
- // set q = n x p
- q=Vector3(-n.z*p.y,n.z*p.x,a*k);
- }
+ if (Math::abs(n.z) > 0.707106781186547524400844362) {
+ // choose p in y-z plane
+ real_t a = n[1] * n[1] + n[2] * n[2];
+ real_t k = 1.0 / Math::sqrt(a);
+ p = Vector3(0, -n[2] * k, n[1] * k);
+ // set q = n x p
+ q = Vector3(a * k, -n[0] * p[2], n[0] * p[1]);
+ } else {
+ // choose p in x-y plane
+ real_t a = n.x * n.x + n.y * n.y;
+ real_t k = 1.0 / Math::sqrt(a);
+ p = Vector3(-n.y * k, n.x * k, 0);
+ // set q = n x p
+ q = Vector3(-n.z * p.y, n.z * p.x, a * k);
+ }
}
-HingeJointSW::HingeJointSW(BodySW* rbA,BodySW* rbB, const Transform& frameA, const Transform& frameB) : JointSW(_arr,2) {
+HingeJointSW::HingeJointSW(BodySW *rbA, BodySW *rbB, const Transform &frameA, const Transform &frameB)
+ : JointSW(_arr, 2) {
- A=rbA;
- B=rbB;
+ A = rbA;
+ B = rbB;
- m_rbAFrame=frameA;
- m_rbBFrame=frameB;
+ m_rbAFrame = frameA;
+ m_rbBFrame = frameB;
// flip axis
m_rbBFrame.basis[0][2] *= real_t(-1.);
m_rbBFrame.basis[1][2] *= real_t(-1.);
m_rbBFrame.basis[2][2] *= real_t(-1.);
-
//start with free
m_lowerLimit = Math_PI;
m_upperLimit = -Math_PI;
-
m_useLimit = false;
m_biasFactor = 0.3f;
m_relaxationFactor = 1.0f;
m_limitSoftness = 0.9f;
m_solveLimit = false;
- tau=0.3;
-
- m_angularOnly=false;
- m_enableAngularMotor=false;
+ tau = 0.3;
- A->add_constraint(this,0);
- B->add_constraint(this,1);
+ m_angularOnly = false;
+ m_enableAngularMotor = false;
+ A->add_constraint(this, 0);
+ B->add_constraint(this, 1);
}
-HingeJointSW::HingeJointSW(BodySW* rbA,BodySW* rbB, const Vector3& pivotInA,const Vector3& pivotInB,
- const Vector3& axisInA,const Vector3& axisInB) : JointSW(_arr,2) {
+HingeJointSW::HingeJointSW(BodySW *rbA, BodySW *rbB, const Vector3 &pivotInA, const Vector3 &pivotInB,
+ const Vector3 &axisInA, const Vector3 &axisInB)
+ : JointSW(_arr, 2) {
- A=rbA;
- B=rbB;
+ A = rbA;
+ B = rbB;
m_rbAFrame.origin = pivotInA;
@@ -112,76 +110,67 @@ HingeJointSW::HingeJointSW(BodySW* rbA,BodySW* rbB, const Vector3& pivotInA,cons
rbAxisA1 = rbAxisA2.cross(axisInA);
}
- m_rbAFrame.basis=Basis( rbAxisA1.x,rbAxisA2.x,axisInA.x,
- rbAxisA1.y,rbAxisA2.y,axisInA.y,
- rbAxisA1.z,rbAxisA2.z,axisInA.z );
+ m_rbAFrame.basis = Basis(rbAxisA1.x, rbAxisA2.x, axisInA.x,
+ rbAxisA1.y, rbAxisA2.y, axisInA.y,
+ rbAxisA1.z, rbAxisA2.z, axisInA.z);
- Quat rotationArc = Quat(axisInA,axisInB);
- Vector3 rbAxisB1 = rotationArc.xform(rbAxisA1);
- Vector3 rbAxisB2 = axisInB.cross(rbAxisB1);
+ Quat rotationArc = Quat(axisInA, axisInB);
+ Vector3 rbAxisB1 = rotationArc.xform(rbAxisA1);
+ Vector3 rbAxisB2 = axisInB.cross(rbAxisB1);
m_rbBFrame.origin = pivotInB;
- m_rbBFrame.basis=Basis( rbAxisB1.x,rbAxisB2.x,-axisInB.x,
- rbAxisB1.y,rbAxisB2.y,-axisInB.y,
- rbAxisB1.z,rbAxisB2.z,-axisInB.z );
+ m_rbBFrame.basis = Basis(rbAxisB1.x, rbAxisB2.x, -axisInB.x,
+ rbAxisB1.y, rbAxisB2.y, -axisInB.y,
+ rbAxisB1.z, rbAxisB2.z, -axisInB.z);
//start with free
m_lowerLimit = Math_PI;
m_upperLimit = -Math_PI;
-
m_useLimit = false;
m_biasFactor = 0.3f;
m_relaxationFactor = 1.0f;
m_limitSoftness = 0.9f;
m_solveLimit = false;
- tau=0.3;
-
- m_angularOnly=false;
- m_enableAngularMotor=false;
+ tau = 0.3;
- A->add_constraint(this,0);
- B->add_constraint(this,1);
+ m_angularOnly = false;
+ m_enableAngularMotor = false;
+ A->add_constraint(this, 0);
+ B->add_constraint(this, 1);
}
-
-
bool HingeJointSW::setup(real_t p_step) {
m_appliedImpulse = real_t(0.);
- if (!m_angularOnly)
- {
+ if (!m_angularOnly) {
Vector3 pivotAInW = A->get_transform().xform(m_rbAFrame.origin);
Vector3 pivotBInW = B->get_transform().xform(m_rbBFrame.origin);
Vector3 relPos = pivotBInW - pivotAInW;
Vector3 normal[3];
- if (relPos.length_squared() > CMP_EPSILON)
- {
+ if (relPos.length_squared() > CMP_EPSILON) {
normal[0] = relPos.normalized();
- }
- else
- {
- normal[0]=Vector3(real_t(1.0),0,0);
+ } else {
+ normal[0] = Vector3(real_t(1.0), 0, 0);
}
plane_space(normal[0], normal[1], normal[2]);
- for (int i=0;i<3;i++)
- {
+ for (int i = 0; i < 3; i++) {
memnew_placement(&m_jac[i], JacobianEntrySW(
- A->get_principal_inertia_axes().transposed(),
- B->get_principal_inertia_axes().transposed(),
- pivotAInW - A->get_transform().origin - A->get_center_of_mass(),
- pivotBInW - B->get_transform().origin - B->get_center_of_mass(),
- normal[i],
- A->get_inv_inertia(),
- A->get_inv_mass(),
- B->get_inv_inertia(),
- B->get_inv_mass()) );
+ A->get_principal_inertia_axes().transposed(),
+ B->get_principal_inertia_axes().transposed(),
+ pivotAInW - A->get_transform().origin - A->get_center_of_mass(),
+ pivotBInW - B->get_transform().origin - B->get_center_of_mass(),
+ normal[i],
+ A->get_inv_inertia(),
+ A->get_inv_mass(),
+ B->get_inv_inertia(),
+ B->get_inv_mass()));
}
}
@@ -192,31 +181,30 @@ bool HingeJointSW::setup(real_t p_step) {
Vector3 jointAxis0local;
Vector3 jointAxis1local;
- plane_space(m_rbAFrame.basis.get_axis(2),jointAxis0local,jointAxis1local);
-
- A->get_transform().basis.xform( m_rbAFrame.basis.get_axis(2) );
- Vector3 jointAxis0 = A->get_transform().basis.xform( jointAxis0local );
- Vector3 jointAxis1 = A->get_transform().basis.xform( jointAxis1local );
- Vector3 hingeAxisWorld = A->get_transform().basis.xform( m_rbAFrame.basis.get_axis(2) );
+ plane_space(m_rbAFrame.basis.get_axis(2), jointAxis0local, jointAxis1local);
- memnew_placement(&m_jacAng[0], JacobianEntrySW(jointAxis0,
- A->get_principal_inertia_axes().transposed(),
- B->get_principal_inertia_axes().transposed(),
- A->get_inv_inertia(),
- B->get_inv_inertia()));
+ A->get_transform().basis.xform(m_rbAFrame.basis.get_axis(2));
+ Vector3 jointAxis0 = A->get_transform().basis.xform(jointAxis0local);
+ Vector3 jointAxis1 = A->get_transform().basis.xform(jointAxis1local);
+ Vector3 hingeAxisWorld = A->get_transform().basis.xform(m_rbAFrame.basis.get_axis(2));
- memnew_placement(&m_jacAng[1], JacobianEntrySW(jointAxis1,
- A->get_principal_inertia_axes().transposed(),
- B->get_principal_inertia_axes().transposed(),
- A->get_inv_inertia(),
- B->get_inv_inertia()));
+ memnew_placement(&m_jacAng[0], JacobianEntrySW(jointAxis0,
+ A->get_principal_inertia_axes().transposed(),
+ B->get_principal_inertia_axes().transposed(),
+ A->get_inv_inertia(),
+ B->get_inv_inertia()));
- memnew_placement(&m_jacAng[2], JacobianEntrySW(hingeAxisWorld,
- A->get_principal_inertia_axes().transposed(),
- B->get_principal_inertia_axes().transposed(),
- A->get_inv_inertia(),
- B->get_inv_inertia()));
+ memnew_placement(&m_jacAng[1], JacobianEntrySW(jointAxis1,
+ A->get_principal_inertia_axes().transposed(),
+ B->get_principal_inertia_axes().transposed(),
+ A->get_inv_inertia(),
+ B->get_inv_inertia()));
+ memnew_placement(&m_jacAng[2], JacobianEntrySW(hingeAxisWorld,
+ A->get_principal_inertia_axes().transposed(),
+ B->get_principal_inertia_axes().transposed(),
+ A->get_inv_inertia(),
+ B->get_inv_inertia()));
// Compute limit information
real_t hingeAngle = get_hinge_angle();
@@ -228,26 +216,21 @@ bool HingeJointSW::setup(real_t p_step) {
m_solveLimit = false;
m_accLimitImpulse = real_t(0.);
-
-
/*if (m_useLimit) {
print_line("low: "+rtos(m_lowerLimit));
print_line("hi: "+rtos(m_upperLimit));
}*/
//if (m_lowerLimit < m_upperLimit)
- if (m_useLimit && m_lowerLimit <= m_upperLimit)
- {
+ if (m_useLimit && m_lowerLimit <= m_upperLimit) {
//if (hingeAngle <= m_lowerLimit*m_limitSoftness)
- if (hingeAngle <= m_lowerLimit)
- {
+ if (hingeAngle <= m_lowerLimit) {
m_correction = (m_lowerLimit - hingeAngle);
m_limitSign = 1.0f;
m_solveLimit = true;
}
//else if (hingeAngle >= m_upperLimit*m_limitSoftness)
- else if (hingeAngle >= m_upperLimit)
- {
+ else if (hingeAngle >= m_upperLimit) {
m_correction = m_upperLimit - hingeAngle;
m_limitSign = -1.0f;
m_solveLimit = true;
@@ -255,9 +238,9 @@ bool HingeJointSW::setup(real_t p_step) {
}
//Compute K = J*W*J' for hinge axis
- Vector3 axisA = A->get_transform().basis.xform( m_rbAFrame.basis.get_axis(2) );
- m_kHinge = 1.0f / (A->compute_angular_impulse_denominator(axisA) +
- B->compute_angular_impulse_denominator(axisA));
+ Vector3 axisA = A->get_transform().basis.xform(m_rbAFrame.basis.get_axis(2));
+ m_kHinge = 1.0f / (A->compute_angular_impulse_denominator(axisA) +
+ B->compute_angular_impulse_denominator(axisA));
return true;
}
@@ -270,8 +253,7 @@ void HingeJointSW::solve(real_t p_step) {
//real_t tau = real_t(0.3);
//linear part
- if (!m_angularOnly)
- {
+ if (!m_angularOnly) {
Vector3 rel_pos1 = pivotAInW - A->get_transform().origin;
Vector3 rel_pos2 = pivotBInW - B->get_transform().origin;
@@ -279,80 +261,74 @@ void HingeJointSW::solve(real_t p_step) {
Vector3 vel2 = B->get_velocity_in_local_point(rel_pos2);
Vector3 vel = vel1 - vel2;
- for (int i=0;i<3;i++)
- {
- const Vector3& normal = m_jac[i].m_linearJointAxis;
+ for (int i = 0; i < 3; i++) {
+ const Vector3 &normal = m_jac[i].m_linearJointAxis;
real_t jacDiagABInv = real_t(1.) / m_jac[i].getDiagonal();
real_t rel_vel;
rel_vel = normal.dot(vel);
//positional error (zeroth order error)
real_t depth = -(pivotAInW - pivotBInW).dot(normal); //this is the error projected on the normal
- real_t impulse = depth*tau/p_step * jacDiagABInv - rel_vel * jacDiagABInv;
+ real_t impulse = depth * tau / p_step * jacDiagABInv - rel_vel * jacDiagABInv;
m_appliedImpulse += impulse;
Vector3 impulse_vector = normal * impulse;
- A->apply_impulse(pivotAInW - A->get_transform().origin,impulse_vector);
- B->apply_impulse(pivotBInW - B->get_transform().origin,-impulse_vector);
+ A->apply_impulse(pivotAInW - A->get_transform().origin, impulse_vector);
+ B->apply_impulse(pivotBInW - B->get_transform().origin, -impulse_vector);
}
}
-
{
///solve angular part
// get axes in world space
- Vector3 axisA = A->get_transform().basis.xform( m_rbAFrame.basis.get_axis(2) );
- Vector3 axisB = B->get_transform().basis.xform( m_rbBFrame.basis.get_axis(2) );
+ Vector3 axisA = A->get_transform().basis.xform(m_rbAFrame.basis.get_axis(2));
+ Vector3 axisB = B->get_transform().basis.xform(m_rbBFrame.basis.get_axis(2));
- const Vector3& angVelA = A->get_angular_velocity();
- const Vector3& angVelB = B->get_angular_velocity();
+ const Vector3 &angVelA = A->get_angular_velocity();
+ const Vector3 &angVelB = B->get_angular_velocity();
Vector3 angVelAroundHingeAxisA = axisA * axisA.dot(angVelA);
Vector3 angVelAroundHingeAxisB = axisB * axisB.dot(angVelB);
Vector3 angAorthog = angVelA - angVelAroundHingeAxisA;
Vector3 angBorthog = angVelB - angVelAroundHingeAxisB;
- Vector3 velrelOrthog = angAorthog-angBorthog;
+ Vector3 velrelOrthog = angAorthog - angBorthog;
{
//solve orthogonal angular velocity correction
real_t relaxation = real_t(1.);
real_t len = velrelOrthog.length();
- if (len > real_t(0.00001))
- {
+ if (len > real_t(0.00001)) {
Vector3 normal = velrelOrthog.normalized();
real_t denom = A->compute_angular_impulse_denominator(normal) +
- B->compute_angular_impulse_denominator(normal);
+ B->compute_angular_impulse_denominator(normal);
// scale for mass and relaxation
- velrelOrthog *= (real_t(1.)/denom) * m_relaxationFactor;
+ velrelOrthog *= (real_t(1.) / denom) * m_relaxationFactor;
}
//solve angular positional correction
- Vector3 angularError = -axisA.cross(axisB) *(real_t(1.)/p_step);
+ Vector3 angularError = -axisA.cross(axisB) * (real_t(1.) / p_step);
real_t len2 = angularError.length();
- if (len2>real_t(0.00001))
- {
+ if (len2 > real_t(0.00001)) {
Vector3 normal2 = angularError.normalized();
real_t denom2 = A->compute_angular_impulse_denominator(normal2) +
- B->compute_angular_impulse_denominator(normal2);
- angularError *= (real_t(1.)/denom2) * relaxation;
+ B->compute_angular_impulse_denominator(normal2);
+ angularError *= (real_t(1.) / denom2) * relaxation;
}
- A->apply_torque_impulse(-velrelOrthog+angularError);
- B->apply_torque_impulse(velrelOrthog-angularError);
+ A->apply_torque_impulse(-velrelOrthog + angularError);
+ B->apply_torque_impulse(velrelOrthog - angularError);
// solve limit
- if (m_solveLimit)
- {
- real_t amplitude = ( (angVelB - angVelA).dot( axisA )*m_relaxationFactor + m_correction* (real_t(1.)/p_step)*m_biasFactor ) * m_limitSign;
+ if (m_solveLimit) {
+ real_t amplitude = ((angVelB - angVelA).dot(axisA) * m_relaxationFactor + m_correction * (real_t(1.) / p_step) * m_biasFactor) * m_limitSign;
real_t impulseMag = amplitude * m_kHinge;
// Clamp the accumulated impulse
real_t temp = m_accLimitImpulse;
- m_accLimitImpulse = MAX(m_accLimitImpulse + impulseMag, real_t(0) );
+ m_accLimitImpulse = MAX(m_accLimitImpulse + impulseMag, real_t(0));
impulseMag = m_accLimitImpulse - temp;
-
Vector3 impulse = axisA * impulseMag * m_limitSign;
A->apply_torque_impulse(impulse);
B->apply_torque_impulse(-impulse);
@@ -360,10 +336,9 @@ void HingeJointSW::solve(real_t p_step) {
}
//apply motor
- if (m_enableAngularMotor)
- {
+ if (m_enableAngularMotor) {
//todo: add limits too
- Vector3 angularLimit(0,0,0);
+ Vector3 angularLimit(0, 0, 0);
Vector3 velrel = angVelAroundHingeAxisA - angVelAroundHingeAxisB;
real_t projRelVel = velrel.dot(axisA);
@@ -377,12 +352,10 @@ void HingeJointSW::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);
-
+ A->apply_torque_impulse(motorImp + angularLimit);
+ B->apply_torque_impulse(-motorImp - angularLimit);
}
}
-
}
/*
void HingeJointSW::updateRHS(real_t timeStep)
@@ -392,8 +365,7 @@ void HingeJointSW::updateRHS(real_t timeStep)
}
*/
-static _FORCE_INLINE_ real_t atan2fast(real_t y, real_t x)
-{
+static _FORCE_INLINE_ real_t atan2fast(real_t y, real_t x) {
real_t coeff_1 = Math_PI / 4.0f;
real_t coeff_2 = 3.0f * coeff_1;
real_t abs_y = Math::abs(y);
@@ -408,33 +380,30 @@ static _FORCE_INLINE_ real_t atan2fast(real_t y, real_t x)
return (y < 0.0f) ? -angle : angle;
}
-
real_t HingeJointSW::get_hinge_angle() {
- const Vector3 refAxis0 = A->get_transform().basis.xform( m_rbAFrame.basis.get_axis(0) );
- const Vector3 refAxis1 = A->get_transform().basis.xform( m_rbAFrame.basis.get_axis(1) );
- const Vector3 swingAxis = B->get_transform().basis.xform( m_rbBFrame.basis.get_axis(1) );
+ const Vector3 refAxis0 = A->get_transform().basis.xform(m_rbAFrame.basis.get_axis(0));
+ const Vector3 refAxis1 = A->get_transform().basis.xform(m_rbAFrame.basis.get_axis(1));
+ const Vector3 swingAxis = B->get_transform().basis.xform(m_rbBFrame.basis.get_axis(1));
- return atan2fast( swingAxis.dot(refAxis0), swingAxis.dot(refAxis1) );
+ return atan2fast(swingAxis.dot(refAxis0), swingAxis.dot(refAxis1));
}
-
void HingeJointSW::set_param(PhysicsServer::HingeJointParam p_param, real_t p_value) {
switch (p_param) {
- case PhysicsServer::HINGE_JOINT_BIAS: tau=p_value; break;
- case PhysicsServer::HINGE_JOINT_LIMIT_UPPER: m_upperLimit=p_value; break;
- case PhysicsServer::HINGE_JOINT_LIMIT_LOWER: m_lowerLimit=p_value; break;
- case PhysicsServer::HINGE_JOINT_LIMIT_BIAS: m_biasFactor=p_value; break;
- case PhysicsServer::HINGE_JOINT_LIMIT_SOFTNESS: m_limitSoftness=p_value; break;
- case PhysicsServer::HINGE_JOINT_LIMIT_RELAXATION: m_relaxationFactor=p_value; break;
- case PhysicsServer::HINGE_JOINT_MOTOR_TARGET_VELOCITY: m_motorTargetVelocity=p_value; break;
- case PhysicsServer::HINGE_JOINT_MOTOR_MAX_IMPULSE: m_maxMotorImpulse=p_value; break;
-
+ case PhysicsServer::HINGE_JOINT_BIAS: tau = p_value; break;
+ case PhysicsServer::HINGE_JOINT_LIMIT_UPPER: m_upperLimit = p_value; break;
+ case PhysicsServer::HINGE_JOINT_LIMIT_LOWER: m_lowerLimit = p_value; break;
+ case PhysicsServer::HINGE_JOINT_LIMIT_BIAS: m_biasFactor = p_value; break;
+ case PhysicsServer::HINGE_JOINT_LIMIT_SOFTNESS: m_limitSoftness = p_value; break;
+ case PhysicsServer::HINGE_JOINT_LIMIT_RELAXATION: m_relaxationFactor = p_value; break;
+ case PhysicsServer::HINGE_JOINT_MOTOR_TARGET_VELOCITY: m_motorTargetVelocity = p_value; break;
+ case PhysicsServer::HINGE_JOINT_MOTOR_MAX_IMPULSE: m_maxMotorImpulse = p_value; break;
}
}
-real_t HingeJointSW::get_param(PhysicsServer::HingeJointParam p_param) const{
+real_t HingeJointSW::get_param(PhysicsServer::HingeJointParam p_param) const {
switch (p_param) {
@@ -446,25 +415,23 @@ real_t HingeJointSW::get_param(PhysicsServer::HingeJointParam p_param) const{
case PhysicsServer::HINGE_JOINT_LIMIT_RELAXATION: return m_relaxationFactor;
case PhysicsServer::HINGE_JOINT_MOTOR_TARGET_VELOCITY: return m_motorTargetVelocity;
case PhysicsServer::HINGE_JOINT_MOTOR_MAX_IMPULSE: return m_maxMotorImpulse;
-
}
return 0;
}
-void HingeJointSW::set_flag(PhysicsServer::HingeJointFlag p_flag, bool p_value){
+void HingeJointSW::set_flag(PhysicsServer::HingeJointFlag p_flag, bool p_value) {
switch (p_flag) {
- case PhysicsServer::HINGE_JOINT_FLAG_USE_LIMIT: m_useLimit=p_value; break;
- case PhysicsServer::HINGE_JOINT_FLAG_ENABLE_MOTOR: m_enableAngularMotor=p_value; break;
+ case PhysicsServer::HINGE_JOINT_FLAG_USE_LIMIT: m_useLimit = p_value; break;
+ case PhysicsServer::HINGE_JOINT_FLAG_ENABLE_MOTOR: m_enableAngularMotor = p_value; break;
}
-
}
-bool HingeJointSW::get_flag(PhysicsServer::HingeJointFlag p_flag) const{
+bool HingeJointSW::get_flag(PhysicsServer::HingeJointFlag p_flag) const {
switch (p_flag) {
case PhysicsServer::HINGE_JOINT_FLAG_USE_LIMIT: return m_useLimit;
- case PhysicsServer::HINGE_JOINT_FLAG_ENABLE_MOTOR:return m_enableAngularMotor;
+ case PhysicsServer::HINGE_JOINT_FLAG_ENABLE_MOTOR: return m_enableAngularMotor;
}
return false;
diff --git a/servers/physics/joints/hinge_joint_sw.h b/servers/physics/joints/hinge_joint_sw.h
index 8469fd1ca..013d9afdb 100644
--- a/servers/physics/joints/hinge_joint_sw.h
+++ b/servers/physics/joints/hinge_joint_sw.h
@@ -34,9 +34,8 @@ Adapted to Godot from the Bullet library.
#ifndef HINGE_JOINT_SW_H
#define HINGE_JOINT_SW_H
-#include "servers/physics/joints_sw.h"
#include "servers/physics/joints/jacobian_entry_sw.h"
-
+#include "servers/physics/joints_sw.h"
/*
Bullet Continuous Collision Detection and Physics Library
@@ -53,8 +52,6 @@ subject to the following restrictions:
3. This notice may not be removed or altered from any source distribution.
*/
-
-
class HingeJointSW : public JointSW {
union {
@@ -66,41 +63,39 @@ class HingeJointSW : public JointSW {
BodySW *_arr[2];
};
- JacobianEntrySW m_jac[3]; //3 orthogonal linear constraints
- JacobianEntrySW m_jacAng[3]; //2 orthogonal angular constraints+ 1 for limit/motor
+ JacobianEntrySW m_jac[3]; //3 orthogonal linear constraints
+ JacobianEntrySW m_jacAng[3]; //2 orthogonal angular constraints+ 1 for limit/motor
Transform m_rbAFrame; // constraint axii. Assumes z is hinge axis.
Transform m_rbBFrame;
- real_t m_motorTargetVelocity;
- real_t m_maxMotorImpulse;
+ real_t m_motorTargetVelocity;
+ real_t m_maxMotorImpulse;
- real_t m_limitSoftness;
- real_t m_biasFactor;
- real_t m_relaxationFactor;
+ real_t m_limitSoftness;
+ real_t m_biasFactor;
+ real_t m_relaxationFactor;
- real_t m_lowerLimit;
- real_t m_upperLimit;
+ real_t m_lowerLimit;
+ real_t m_upperLimit;
- real_t m_kHinge;
+ real_t m_kHinge;
- real_t m_limitSign;
- real_t m_correction;
+ real_t m_limitSign;
+ real_t m_correction;
- real_t m_accLimitImpulse;
+ real_t m_accLimitImpulse;
real_t tau;
- bool m_useLimit;
- bool m_angularOnly;
- bool m_enableAngularMotor;
- bool m_solveLimit;
+ bool m_useLimit;
+ bool m_angularOnly;
+ bool m_enableAngularMotor;
+ bool m_solveLimit;
real_t m_appliedImpulse;
-
public:
-
virtual PhysicsServer::JointType get_type() const { return PhysicsServer::JOINT_HINGE; }
virtual bool setup(real_t p_step);
@@ -114,9 +109,8 @@ public:
void set_flag(PhysicsServer::HingeJointFlag p_flag, bool p_value);
bool get_flag(PhysicsServer::HingeJointFlag p_flag) const;
- HingeJointSW(BodySW* rbA,BodySW* rbB, const Transform& frameA, const Transform& frameB);
- HingeJointSW(BodySW* rbA,BodySW* rbB, const Vector3& pivotInA,const Vector3& pivotInB, const Vector3& axisInA,const Vector3& axisInB);
-
+ HingeJointSW(BodySW *rbA, BodySW *rbB, const Transform &frameA, const Transform &frameB);
+ HingeJointSW(BodySW *rbA, BodySW *rbB, const Vector3 &pivotInA, const Vector3 &pivotInB, const Vector3 &axisInA, const Vector3 &axisInB);
};
#endif // HINGE_JOINT_SW_H
diff --git a/servers/physics/joints/jacobian_entry_sw.h b/servers/physics/joints/jacobian_entry_sw.h
index cd85162ba..b0b31ed79 100644
--- a/servers/physics/joints/jacobian_entry_sw.h
+++ b/servers/physics/joints/jacobian_entry_sw.h
@@ -53,22 +53,21 @@ subject to the following restrictions:
class JacobianEntrySW {
public:
- JacobianEntrySW() {};
+ JacobianEntrySW(){};
//constraint between two different rigidbodies
JacobianEntrySW(
- const Basis& world2A,
- const Basis& world2B,
- const Vector3& rel_pos1,const Vector3& rel_pos2,
- const Vector3& jointAxis,
- const Vector3& inertiaInvA,
- const real_t massInvA,
- const Vector3& inertiaInvB,
- const real_t massInvB)
- :m_linearJointAxis(jointAxis)
- {
+ const Basis &world2A,
+ const Basis &world2B,
+ const Vector3 &rel_pos1, const Vector3 &rel_pos2,
+ const Vector3 &jointAxis,
+ const Vector3 &inertiaInvA,
+ const real_t massInvA,
+ const Vector3 &inertiaInvB,
+ const real_t massInvB)
+ : m_linearJointAxis(jointAxis) {
m_aJ = world2A.xform(rel_pos1.cross(m_linearJointAxis));
m_bJ = world2B.xform(rel_pos2.cross(-m_linearJointAxis));
- m_0MinvJt = inertiaInvA * m_aJ;
+ m_0MinvJt = inertiaInvA * m_aJ;
m_1MinvJt = inertiaInvB * m_bJ;
m_Adiag = massInvA + m_0MinvJt.dot(m_aJ) + massInvB + m_1MinvJt.dot(m_bJ);
@@ -76,104 +75,92 @@ public:
}
//angular constraint between two different rigidbodies
- JacobianEntrySW(const Vector3& jointAxis,
- const Basis& world2A,
- const Basis& world2B,
- const Vector3& inertiaInvA,
- const Vector3& inertiaInvB)
- :m_linearJointAxis(Vector3(real_t(0.),real_t(0.),real_t(0.)))
- {
- m_aJ= world2A.xform(jointAxis);
+ JacobianEntrySW(const Vector3 &jointAxis,
+ const Basis &world2A,
+ const Basis &world2B,
+ const Vector3 &inertiaInvA,
+ const Vector3 &inertiaInvB)
+ : m_linearJointAxis(Vector3(real_t(0.), real_t(0.), real_t(0.))) {
+ m_aJ = world2A.xform(jointAxis);
m_bJ = world2B.xform(-jointAxis);
- m_0MinvJt = inertiaInvA * m_aJ;
+ m_0MinvJt = inertiaInvA * m_aJ;
m_1MinvJt = inertiaInvB * m_bJ;
- m_Adiag = m_0MinvJt.dot(m_aJ) + m_1MinvJt.dot(m_bJ);
+ m_Adiag = m_0MinvJt.dot(m_aJ) + m_1MinvJt.dot(m_bJ);
ERR_FAIL_COND(m_Adiag <= real_t(0.0));
}
//angular constraint between two different rigidbodies
- JacobianEntrySW(const Vector3& axisInA,
- const Vector3& axisInB,
- const Vector3& inertiaInvA,
- const Vector3& inertiaInvB)
- : m_linearJointAxis(Vector3(real_t(0.),real_t(0.),real_t(0.)))
- , m_aJ(axisInA)
- , m_bJ(-axisInB)
- {
- m_0MinvJt = inertiaInvA * m_aJ;
+ JacobianEntrySW(const Vector3 &axisInA,
+ const Vector3 &axisInB,
+ const Vector3 &inertiaInvA,
+ const Vector3 &inertiaInvB)
+ : m_linearJointAxis(Vector3(real_t(0.), real_t(0.), real_t(0.))), m_aJ(axisInA), m_bJ(-axisInB) {
+ m_0MinvJt = inertiaInvA * m_aJ;
m_1MinvJt = inertiaInvB * m_bJ;
- m_Adiag = m_0MinvJt.dot(m_aJ) + m_1MinvJt.dot(m_bJ);
+ m_Adiag = m_0MinvJt.dot(m_aJ) + m_1MinvJt.dot(m_bJ);
ERR_FAIL_COND(m_Adiag <= real_t(0.0));
}
//constraint on one rigidbody
JacobianEntrySW(
- const Basis& world2A,
- const Vector3& rel_pos1,const Vector3& rel_pos2,
- const Vector3& jointAxis,
- const Vector3& inertiaInvA,
- const real_t massInvA)
- :m_linearJointAxis(jointAxis)
- {
- m_aJ= world2A.xform(rel_pos1.cross(jointAxis));
+ const Basis &world2A,
+ const Vector3 &rel_pos1, const Vector3 &rel_pos2,
+ const Vector3 &jointAxis,
+ const Vector3 &inertiaInvA,
+ const real_t massInvA)
+ : m_linearJointAxis(jointAxis) {
+ m_aJ = world2A.xform(rel_pos1.cross(jointAxis));
m_bJ = world2A.xform(rel_pos2.cross(-jointAxis));
- m_0MinvJt = inertiaInvA * m_aJ;
- m_1MinvJt = Vector3(real_t(0.),real_t(0.),real_t(0.));
+ m_0MinvJt = inertiaInvA * m_aJ;
+ m_1MinvJt = Vector3(real_t(0.), real_t(0.), real_t(0.));
m_Adiag = massInvA + m_0MinvJt.dot(m_aJ);
ERR_FAIL_COND(m_Adiag <= real_t(0.0));
}
- real_t getDiagonal() const { return m_Adiag; }
+ real_t getDiagonal() const { return m_Adiag; }
// for two constraints on the same rigidbody (for example vehicle friction)
- real_t getNonDiagonal(const JacobianEntrySW& jacB, const real_t massInvA) const
- {
- const JacobianEntrySW& jacA = *this;
+ real_t getNonDiagonal(const JacobianEntrySW &jacB, const real_t massInvA) const {
+ const JacobianEntrySW &jacA = *this;
real_t lin = massInvA * jacA.m_linearJointAxis.dot(jacB.m_linearJointAxis);
real_t ang = jacA.m_0MinvJt.dot(jacB.m_aJ);
return lin + ang;
}
-
-
// for two constraints on sharing two same rigidbodies (for example two contact points between two rigidbodies)
- real_t getNonDiagonal(const JacobianEntrySW& jacB,const real_t massInvA,const real_t massInvB) const
- {
- const JacobianEntrySW& jacA = *this;
+ real_t getNonDiagonal(const JacobianEntrySW &jacB, const real_t massInvA, const real_t massInvB) const {
+ const JacobianEntrySW &jacA = *this;
Vector3 lin = jacA.m_linearJointAxis * jacB.m_linearJointAxis;
Vector3 ang0 = jacA.m_0MinvJt * jacB.m_aJ;
Vector3 ang1 = jacA.m_1MinvJt * jacB.m_bJ;
- Vector3 lin0 = massInvA * lin ;
+ Vector3 lin0 = massInvA * lin;
Vector3 lin1 = massInvB * lin;
- Vector3 sum = ang0+ang1+lin0+lin1;
- return sum[0]+sum[1]+sum[2];
+ Vector3 sum = ang0 + ang1 + lin0 + lin1;
+ return sum[0] + sum[1] + sum[2];
}
- real_t getRelativeVelocity(const Vector3& linvelA,const Vector3& angvelA,const Vector3& linvelB,const Vector3& angvelB)
- {
+ real_t getRelativeVelocity(const Vector3 &linvelA, const Vector3 &angvelA, const Vector3 &linvelB, const Vector3 &angvelB) {
Vector3 linrel = linvelA - linvelB;
- Vector3 angvela = angvelA * m_aJ;
- Vector3 angvelb = angvelB * m_bJ;
+ Vector3 angvela = angvelA * m_aJ;
+ Vector3 angvelb = angvelB * m_bJ;
linrel *= m_linearJointAxis;
angvela += angvelb;
angvela += linrel;
- real_t rel_vel2 = angvela[0]+angvela[1]+angvela[2];
+ real_t rel_vel2 = angvela[0] + angvela[1] + angvela[2];
return rel_vel2 + CMP_EPSILON;
}
-//private:
+ //private:
- Vector3 m_linearJointAxis;
- Vector3 m_aJ;
- Vector3 m_bJ;
- Vector3 m_0MinvJt;
- Vector3 m_1MinvJt;
+ Vector3 m_linearJointAxis;
+ Vector3 m_aJ;
+ Vector3 m_bJ;
+ Vector3 m_0MinvJt;
+ Vector3 m_1MinvJt;
//Optimization: can be stored in the w/last component of one of the vectors
- real_t m_Adiag;
-
+ real_t m_Adiag;
};
-
#endif // JACOBIAN_ENTRY_SW_H
diff --git a/servers/physics/joints/pin_joint_sw.cpp b/servers/physics/joints/pin_joint_sw.cpp
index b545ae630..e01514f4b 100644
--- a/servers/physics/joints/pin_joint_sw.cpp
+++ b/servers/physics/joints/pin_joint_sw.cpp
@@ -38,41 +38,37 @@ bool PinJointSW::setup(real_t p_step) {
m_appliedImpulse = real_t(0.);
- Vector3 normal(0,0,0);
+ Vector3 normal(0, 0, 0);
- for (int i=0;i<3;i++)
- {
+ for (int i = 0; i < 3; i++) {
normal[i] = 1;
- memnew_placement(&m_jac[i],JacobianEntrySW(
- A->get_principal_inertia_axes().transposed(),
- B->get_principal_inertia_axes().transposed(),
- A->get_transform().xform(m_pivotInA) - A->get_transform().origin - A->get_center_of_mass(),
- B->get_transform().xform(m_pivotInB) - B->get_transform().origin - B->get_center_of_mass(),
- normal,
- A->get_inv_inertia(),
- A->get_inv_mass(),
- B->get_inv_inertia(),
- B->get_inv_mass()));
+ memnew_placement(&m_jac[i], JacobianEntrySW(
+ A->get_principal_inertia_axes().transposed(),
+ B->get_principal_inertia_axes().transposed(),
+ A->get_transform().xform(m_pivotInA) - A->get_transform().origin - A->get_center_of_mass(),
+ B->get_transform().xform(m_pivotInB) - B->get_transform().origin - B->get_center_of_mass(),
+ normal,
+ A->get_inv_inertia(),
+ A->get_inv_mass(),
+ B->get_inv_inertia(),
+ B->get_inv_mass()));
normal[i] = 0;
}
return true;
}
-void PinJointSW::solve(real_t 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);
-
- Vector3 normal(0,0,0);
-
+ Vector3 normal(0, 0, 0);
//Vector3 angvelA = A->get_transform().origin.getBasis().transpose() * A->getAngularVelocity();
//Vector3 angvelB = B->get_transform().origin.getBasis().transpose() * B->getAngularVelocity();
- for (int i=0;i<3;i++)
- {
+ for (int i = 0; i < 3; i++) {
normal[i] = 1;
real_t jacDiagABInv = real_t(1.) / m_jac[i].getDiagonal();
@@ -87,7 +83,7 @@ void PinJointSW::solve(real_t p_step){
real_t rel_vel;
rel_vel = normal.dot(vel);
- /*
+ /*
//velocity error (first order error)
real_t rel_vel = m_jac[i].getRelativeVelocity(A->getLinearVelocity(),angvelA,
B->getLinearVelocity(),angvelB);
@@ -96,38 +92,37 @@ void PinJointSW::solve(real_t p_step){
//positional error (zeroth order error)
real_t depth = -(pivotAInW - pivotBInW).dot(normal); //this is the error projected on the normal
- real_t impulse = depth*m_tau/p_step * jacDiagABInv - m_damping * rel_vel * jacDiagABInv;
+ real_t impulse = depth * m_tau / p_step * jacDiagABInv - m_damping * rel_vel * jacDiagABInv;
real_t impulseClamp = m_impulseClamp;
- if (impulseClamp > 0)
- {
+ if (impulseClamp > 0) {
if (impulse < -impulseClamp)
impulse = -impulseClamp;
if (impulse > impulseClamp)
impulse = impulseClamp;
}
- m_appliedImpulse+=impulse;
+ m_appliedImpulse += impulse;
Vector3 impulse_vector = normal * impulse;
- A->apply_impulse(pivotAInW - A->get_transform().origin,impulse_vector);
- B->apply_impulse(pivotBInW - B->get_transform().origin,-impulse_vector);
+ A->apply_impulse(pivotAInW - A->get_transform().origin, impulse_vector);
+ B->apply_impulse(pivotBInW - B->get_transform().origin, -impulse_vector);
normal[i] = 0;
}
}
-void PinJointSW::set_param(PhysicsServer::PinJointParam p_param,real_t 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;
- case PhysicsServer::PIN_JOINT_DAMPING: m_damping=p_value; break;
- case PhysicsServer::PIN_JOINT_IMPULSE_CLAMP: m_impulseClamp=p_value; break;
+ switch (p_param) {
+ case PhysicsServer::PIN_JOINT_BIAS: m_tau = p_value; break;
+ case PhysicsServer::PIN_JOINT_DAMPING: m_damping = p_value; break;
+ case PhysicsServer::PIN_JOINT_IMPULSE_CLAMP: m_impulseClamp = p_value; break;
}
}
-real_t PinJointSW::get_param(PhysicsServer::PinJointParam p_param) const{
+real_t PinJointSW::get_param(PhysicsServer::PinJointParam p_param) const {
- switch(p_param) {
+ switch (p_param) {
case PhysicsServer::PIN_JOINT_BIAS: return m_tau;
case PhysicsServer::PIN_JOINT_DAMPING: return m_damping;
case PhysicsServer::PIN_JOINT_IMPULSE_CLAMP: return m_impulseClamp;
@@ -136,26 +131,22 @@ real_t PinJointSW::get_param(PhysicsServer::PinJointParam p_param) const{
return 0;
}
-PinJointSW::PinJointSW(BodySW* p_body_a,const Vector3& p_pos_a,BodySW* p_body_b,const Vector3& p_pos_b) : JointSW(_arr,2) {
-
- A=p_body_a;
- B=p_body_b;
- m_pivotInA=p_pos_a;
- m_pivotInB=p_pos_b;
-
- m_tau=0.3;
- m_damping=1;
- m_impulseClamp=0;
- m_appliedImpulse=0;
+PinJointSW::PinJointSW(BodySW *p_body_a, const Vector3 &p_pos_a, BodySW *p_body_b, const Vector3 &p_pos_b)
+ : JointSW(_arr, 2) {
- A->add_constraint(this,0);
- B->add_constraint(this,1);
+ A = p_body_a;
+ B = p_body_b;
+ m_pivotInA = p_pos_a;
+ m_pivotInB = p_pos_b;
+ m_tau = 0.3;
+ m_damping = 1;
+ m_impulseClamp = 0;
+ m_appliedImpulse = 0;
+ A->add_constraint(this, 0);
+ B->add_constraint(this, 1);
}
PinJointSW::~PinJointSW() {
-
-
-
}
diff --git a/servers/physics/joints/pin_joint_sw.h b/servers/physics/joints/pin_joint_sw.h
index b72b21219..9500d4b46 100644
--- a/servers/physics/joints/pin_joint_sw.h
+++ b/servers/physics/joints/pin_joint_sw.h
@@ -34,8 +34,8 @@ Adapted to Godot from the Bullet library.
#ifndef PIN_JOINT_SW_H
#define PIN_JOINT_SW_H
-#include "servers/physics/joints_sw.h"
#include "servers/physics/joints/jacobian_entry_sw.h"
+#include "servers/physics/joints_sw.h"
/*
Bullet Continuous Collision Detection and Physics Library
@@ -52,7 +52,6 @@ subject to the following restrictions:
3. This notice may not be removed or altered from any source distribution.
*/
-
class PinJointSW : public JointSW {
union {
@@ -64,37 +63,33 @@ class PinJointSW : public JointSW {
BodySW *_arr[2];
};
+ real_t m_tau; //bias
+ real_t m_damping;
+ real_t m_impulseClamp;
+ real_t m_appliedImpulse;
- real_t m_tau; //bias
- real_t m_damping;
- real_t m_impulseClamp;
- real_t m_appliedImpulse;
-
- JacobianEntrySW m_jac[3]; //3 orthogonal linear constraints
+ JacobianEntrySW m_jac[3]; //3 orthogonal linear constraints
- Vector3 m_pivotInA;
- Vector3 m_pivotInB;
+ Vector3 m_pivotInA;
+ Vector3 m_pivotInB;
public:
-
virtual PhysicsServer::JointType get_type() const { return PhysicsServer::JOINT_PIN; }
virtual bool setup(real_t p_step);
virtual void solve(real_t p_step);
- void set_param(PhysicsServer::PinJointParam p_param,real_t p_value);
+ 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; }
+ void set_pos_A(const Vector3 &p_pos) { m_pivotInA = p_pos; }
+ void set_pos_B(const Vector3 &p_pos) { m_pivotInB = p_pos; }
Vector3 get_pos_A() { return m_pivotInB; }
Vector3 get_pos_B() { return m_pivotInA; }
- PinJointSW(BodySW* p_body_a,const Vector3& p_pos_a,BodySW* p_body_b,const Vector3& p_pos_b);
+ PinJointSW(BodySW *p_body_a, const Vector3 &p_pos_a, BodySW *p_body_b, const Vector3 &p_pos_b);
~PinJointSW();
};
-
-
#endif // PIN_JOINT_SW_H
diff --git a/servers/physics/joints/slider_joint_sw.cpp b/servers/physics/joints/slider_joint_sw.cpp
index fc728ed0b..b8a6c1eca 100644
--- a/servers/physics/joints/slider_joint_sw.cpp
+++ b/servers/physics/joints/slider_joint_sw.cpp
@@ -36,8 +36,7 @@ See corresponding header file for licensing info.
//-----------------------------------------------------------------------------
-static _FORCE_INLINE_ real_t atan2fast(real_t y, real_t x)
-{
+static _FORCE_INLINE_ real_t atan2fast(real_t y, real_t x) {
real_t coeff_1 = Math_PI / 4.0f;
real_t coeff_2 = 3.0f * coeff_1;
real_t abs_y = Math::abs(y);
@@ -52,13 +51,11 @@ static _FORCE_INLINE_ real_t atan2fast(real_t y, real_t x)
return (y < 0.0f) ? -angle : angle;
}
-
-void SliderJointSW::initParams()
-{
- m_lowerLinLimit = real_t(1.0);
- m_upperLinLimit = real_t(-1.0);
- m_lowerAngLimit = real_t(0.);
- m_upperAngLimit = real_t(0.);
+void SliderJointSW::initParams() {
+ m_lowerLinLimit = real_t(1.0);
+ m_upperLinLimit = real_t(-1.0);
+ m_lowerAngLimit = real_t(0.);
+ m_upperAngLimit = real_t(0.);
m_softnessDirLin = SLIDER_CONSTRAINT_DEF_SOFTNESS;
m_restitutionDirLin = SLIDER_CONSTRAINT_DEF_RESTITUTION;
m_dampingDirLin = real_t(0.);
@@ -84,40 +81,35 @@ void SliderJointSW::initParams()
m_accumulatedLinMotorImpulse = real_t(0.0);
m_poweredAngMotor = false;
- m_targetAngMotorVelocity = real_t(0.);
- m_maxAngMotorForce = real_t(0.);
+ m_targetAngMotorVelocity = real_t(0.);
+ m_maxAngMotorForce = real_t(0.);
m_accumulatedAngMotorImpulse = real_t(0.0);
} // SliderJointSW::initParams()
//-----------------------------------------------------------------------------
-
//-----------------------------------------------------------------------------
-SliderJointSW::SliderJointSW(BodySW* rbA, BodySW* rbB, const Transform& frameInA, const Transform& frameInB)
- : JointSW(_arr,2)
- , m_frameInA(frameInA)
- , m_frameInB(frameInB)
-{
+SliderJointSW::SliderJointSW(BodySW *rbA, BodySW *rbB, const Transform &frameInA, const Transform &frameInB)
+ : JointSW(_arr, 2), m_frameInA(frameInA), m_frameInB(frameInB) {
- A=rbA;
- B=rbB;
+ A = rbA;
+ B = rbB;
- A->add_constraint(this,0);
- B->add_constraint(this,1);
+ A->add_constraint(this, 0);
+ B->add_constraint(this, 1);
initParams();
} // SliderJointSW::SliderJointSW()
//-----------------------------------------------------------------------------
-bool SliderJointSW::setup(real_t p_step)
-{
+bool SliderJointSW::setup(real_t p_step) {
//calculate transforms
- m_calculatedTransformA = A->get_transform() * m_frameInA;
- m_calculatedTransformB = B->get_transform() * m_frameInB;
+ m_calculatedTransformA = A->get_transform() * m_frameInA;
+ m_calculatedTransformB = B->get_transform() * m_frameInB;
m_realPivotAInW = m_calculatedTransformA.origin;
m_realPivotBInW = m_calculatedTransformB.origin;
m_sliderAxis = m_calculatedTransformA.basis.get_axis(0); // along X
@@ -125,42 +117,38 @@ bool SliderJointSW::setup(real_t p_step)
m_projPivotInW = m_realPivotAInW + m_sliderAxis.dot(m_delta) * m_sliderAxis;
m_relPosA = m_projPivotInW - A->get_transform().origin;
m_relPosB = m_realPivotBInW - B->get_transform().origin;
- Vector3 normalWorld;
- int i;
- //linear part
- for(i = 0; i < 3; i++)
- {
+ Vector3 normalWorld;
+ int i;
+ //linear part
+ for (i = 0; i < 3; i++) {
normalWorld = m_calculatedTransformA.basis.get_axis(i);
memnew_placement(&m_jacLin[i], JacobianEntrySW(
- A->get_principal_inertia_axes().transposed(),
- B->get_principal_inertia_axes().transposed(),
- m_relPosA - A->get_center_of_mass(),
- m_relPosB - B->get_center_of_mass(),
- normalWorld,
- A->get_inv_inertia(),
- A->get_inv_mass(),
- B->get_inv_inertia(),
- B->get_inv_mass()
- ));
+ A->get_principal_inertia_axes().transposed(),
+ B->get_principal_inertia_axes().transposed(),
+ m_relPosA - A->get_center_of_mass(),
+ m_relPosB - B->get_center_of_mass(),
+ normalWorld,
+ A->get_inv_inertia(),
+ A->get_inv_mass(),
+ B->get_inv_inertia(),
+ B->get_inv_mass()));
m_jacLinDiagABInv[i] = real_t(1.) / m_jacLin[i].getDiagonal();
m_depth[i] = m_delta.dot(normalWorld);
- }
+ }
testLinLimits();
- // angular part
- for(i = 0; i < 3; i++)
- {
+ // angular part
+ for (i = 0; i < 3; i++) {
normalWorld = m_calculatedTransformA.basis.get_axis(i);
- memnew_placement(&m_jacAng[i], JacobianEntrySW(
- normalWorld,
- A->get_principal_inertia_axes().transposed(),
- B->get_principal_inertia_axes().transposed(),
- A->get_inv_inertia(),
- B->get_inv_inertia()
- ));
+ memnew_placement(&m_jacAng[i], JacobianEntrySW(
+ normalWorld,
+ A->get_principal_inertia_axes().transposed(),
+ B->get_principal_inertia_axes().transposed(),
+ A->get_inv_inertia(),
+ B->get_inv_inertia()));
}
testAngLimits();
Vector3 axisA = m_calculatedTransformA.basis.get_axis(0);
- m_kAngle = real_t(1.0 )/ (A->compute_angular_impulse_denominator(axisA) + B->compute_angular_impulse_denominator(axisA));
+ m_kAngle = real_t(1.0) / (A->compute_angular_impulse_denominator(axisA) + B->compute_angular_impulse_denominator(axisA));
// clear accumulator for motors
m_accumulatedLinMotorImpulse = real_t(0.0);
m_accumulatedAngMotorImpulse = real_t(0.0);
@@ -172,14 +160,13 @@ bool SliderJointSW::setup(real_t p_step)
void SliderJointSW::solve(real_t p_step) {
- int i;
- // linear
- Vector3 velA = A->get_velocity_in_local_point(m_relPosA);
- Vector3 velB = B->get_velocity_in_local_point(m_relPosB);
- Vector3 vel = velA - velB;
- for(i = 0; i < 3; i++)
- {
- const Vector3& normal = m_jacLin[i].m_linearJointAxis;
+ int i;
+ // linear
+ Vector3 velA = A->get_velocity_in_local_point(m_relPosA);
+ Vector3 velB = B->get_velocity_in_local_point(m_relPosB);
+ Vector3 vel = velA - velB;
+ for (i = 0; i < 3; i++) {
+ const Vector3 &normal = m_jacLin[i].m_linearJointAxis;
real_t rel_vel = normal.dot(vel);
// calculate positional error
real_t depth = m_depth[i];
@@ -190,81 +177,70 @@ void SliderJointSW::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( m_relPosA, impulse_vector);
- B->apply_impulse(m_relPosB,-impulse_vector);
- if(m_poweredLinMotor && (!i))
- { // apply linear motor
- if(m_accumulatedLinMotorImpulse < m_maxLinMotorForce)
- {
+ A->apply_impulse(m_relPosA, impulse_vector);
+ B->apply_impulse(m_relPosB, -impulse_vector);
+ if (m_poweredLinMotor && (!i)) { // apply linear motor
+ if (m_accumulatedLinMotorImpulse < m_maxLinMotorForce) {
real_t desiredMotorVel = m_targetLinMotorVelocity;
real_t motor_relvel = desiredMotorVel + rel_vel;
normalImpulse = -motor_relvel * m_jacLinDiagABInv[i];
// clamp accumulated impulse
real_t new_acc = m_accumulatedLinMotorImpulse + Math::abs(normalImpulse);
- if(new_acc > m_maxLinMotorForce)
- {
+ if (new_acc > m_maxLinMotorForce) {
new_acc = m_maxLinMotorForce;
}
- real_t del = new_acc - m_accumulatedLinMotorImpulse;
- if(normalImpulse < real_t(0.0))
- {
+ real_t del = new_acc - m_accumulatedLinMotorImpulse;
+ if (normalImpulse < real_t(0.0)) {
normalImpulse = -del;
- }
- else
- {
+ } else {
normalImpulse = del;
}
m_accumulatedLinMotorImpulse = new_acc;
// apply clamped impulse
impulse_vector = normal * normalImpulse;
- A->apply_impulse( m_relPosA, impulse_vector);
- B->apply_impulse( m_relPosB,-impulse_vector);
+ A->apply_impulse(m_relPosA, impulse_vector);
+ B->apply_impulse(m_relPosB, -impulse_vector);
}
}
- }
+ }
// angular
// get axes in world space
- Vector3 axisA = m_calculatedTransformA.basis.get_axis(0);
- Vector3 axisB = m_calculatedTransformB.basis.get_axis(0);
+ Vector3 axisA = m_calculatedTransformA.basis.get_axis(0);
+ Vector3 axisB = m_calculatedTransformB.basis.get_axis(0);
- const Vector3& angVelA = A->get_angular_velocity();
- const Vector3& angVelB = B->get_angular_velocity();
+ const Vector3 &angVelA = A->get_angular_velocity();
+ const Vector3 &angVelB = B->get_angular_velocity();
Vector3 angVelAroundAxisA = axisA * axisA.dot(angVelA);
Vector3 angVelAroundAxisB = axisB * axisB.dot(angVelB);
Vector3 angAorthog = angVelA - angVelAroundAxisA;
Vector3 angBorthog = angVelB - angVelAroundAxisB;
- Vector3 velrelOrthog = angAorthog-angBorthog;
+ Vector3 velrelOrthog = angAorthog - angBorthog;
//solve orthogonal angular velocity correction
real_t len = velrelOrthog.length();
- if (len > real_t(0.00001))
- {
+ if (len > real_t(0.00001)) {
Vector3 normal = velrelOrthog.normalized();
real_t denom = A->compute_angular_impulse_denominator(normal) + B->compute_angular_impulse_denominator(normal);
- velrelOrthog *= (real_t(1.)/denom) * m_dampingOrthoAng * m_softnessOrthoAng;
+ velrelOrthog *= (real_t(1.) / denom) * m_dampingOrthoAng * m_softnessOrthoAng;
}
//solve angular positional correction
- Vector3 angularError = axisA.cross(axisB) *(real_t(1.)/p_step);
+ Vector3 angularError = axisA.cross(axisB) * (real_t(1.) / p_step);
real_t len2 = angularError.length();
- if (len2>real_t(0.00001))
- {
+ if (len2 > real_t(0.00001)) {
Vector3 normal2 = angularError.normalized();
real_t denom2 = A->compute_angular_impulse_denominator(normal2) + B->compute_angular_impulse_denominator(normal2);
- angularError *= (real_t(1.)/denom2) * m_restitutionOrthoAng * m_softnessOrthoAng;
+ angularError *= (real_t(1.) / denom2) * m_restitutionOrthoAng * m_softnessOrthoAng;
}
// apply impulse
- A->apply_torque_impulse(-velrelOrthog+angularError);
- B->apply_torque_impulse(velrelOrthog-angularError);
+ A->apply_torque_impulse(-velrelOrthog + angularError);
+ B->apply_torque_impulse(velrelOrthog - angularError);
real_t impulseMag;
//solve angular limits
- if(m_solveAngLim)
- {
+ if (m_solveAngLim) {
impulseMag = (angVelB - angVelA).dot(axisA) * m_dampingLimAng + m_angDepth * m_restitutionLimAng / p_step;
impulseMag *= m_kAngle * m_softnessLimAng;
- }
- else
- {
+ } else {
impulseMag = (angVelB - angVelA).dot(axisA) * m_dampingDirAng + m_angDepth * m_restitutionDirAng / p_step;
impulseMag *= m_kAngle * m_softnessDirAng;
}
@@ -272,10 +248,8 @@ void SliderJointSW::solve(real_t p_step) {
A->apply_torque_impulse(impulse);
B->apply_torque_impulse(-impulse);
//apply angular motor
- if(m_poweredAngMotor)
- {
- if(m_accumulatedAngMotorImpulse < m_maxAngMotorForce)
- {
+ if (m_poweredAngMotor) {
+ if (m_accumulatedAngMotorImpulse < m_maxAngMotorForce) {
Vector3 velrel = angVelAroundAxisA - angVelAroundAxisB;
real_t projRelVel = velrel.dot(axisA);
@@ -285,17 +259,13 @@ void SliderJointSW::solve(real_t p_step) {
real_t angImpulse = m_kAngle * motor_relvel;
// clamp accumulated impulse
real_t new_acc = m_accumulatedAngMotorImpulse + Math::abs(angImpulse);
- if(new_acc > m_maxAngMotorForce)
- {
+ if (new_acc > m_maxAngMotorForce) {
new_acc = m_maxAngMotorForce;
}
- real_t del = new_acc - m_accumulatedAngMotorImpulse;
- if(angImpulse < real_t(0.0))
- {
+ real_t del = new_acc - m_accumulatedAngMotorImpulse;
+ if (angImpulse < real_t(0.0)) {
angImpulse = -del;
- }
- else
- {
+ } else {
angImpulse = del;
}
m_accumulatedAngMotorImpulse = new_acc;
@@ -311,96 +281,75 @@ void SliderJointSW::solve(real_t p_step) {
//-----------------------------------------------------------------------------
-void SliderJointSW::calculateTransforms(void){
- m_calculatedTransformA = A->get_transform() * m_frameInA ;
+void SliderJointSW::calculateTransforms(void) {
+ m_calculatedTransformA = A->get_transform() * m_frameInA;
m_calculatedTransformB = B->get_transform() * m_frameInB;
m_realPivotAInW = m_calculatedTransformA.origin;
m_realPivotBInW = m_calculatedTransformB.origin;
m_sliderAxis = m_calculatedTransformA.basis.get_axis(0); // along X
m_delta = m_realPivotBInW - m_realPivotAInW;
m_projPivotInW = m_realPivotAInW + m_sliderAxis.dot(m_delta) * m_sliderAxis;
- Vector3 normalWorld;
- int i;
- //linear part
- for(i = 0; i < 3; i++)
- {
+ Vector3 normalWorld;
+ int i;
+ //linear part
+ for (i = 0; i < 3; i++) {
normalWorld = m_calculatedTransformA.basis.get_axis(i);
m_depth[i] = m_delta.dot(normalWorld);
- }
+ }
} // SliderJointSW::calculateTransforms()
//-----------------------------------------------------------------------------
-void SliderJointSW::testLinLimits(void)
-{
+void SliderJointSW::testLinLimits(void) {
m_solveLinLim = false;
m_linPos = m_depth[0];
- if(m_lowerLinLimit <= m_upperLinLimit)
- {
- if(m_depth[0] > m_upperLinLimit)
- {
+ if (m_lowerLinLimit <= m_upperLinLimit) {
+ if (m_depth[0] > m_upperLinLimit) {
m_depth[0] -= m_upperLinLimit;
m_solveLinLim = true;
- }
- else if(m_depth[0] < m_lowerLinLimit)
- {
+ } else if (m_depth[0] < m_lowerLinLimit) {
m_depth[0] -= m_lowerLinLimit;
m_solveLinLim = true;
- }
- else
- {
+ } else {
m_depth[0] = real_t(0.);
}
- }
- else
- {
+ } else {
m_depth[0] = real_t(0.);
}
} // SliderJointSW::testLinLimits()
//-----------------------------------------------------------------------------
-
-void SliderJointSW::testAngLimits(void)
-{
+void SliderJointSW::testAngLimits(void) {
m_angDepth = real_t(0.);
m_solveAngLim = false;
- if(m_lowerAngLimit <= m_upperAngLimit)
- {
+ if (m_lowerAngLimit <= m_upperAngLimit) {
const Vector3 axisA0 = m_calculatedTransformA.basis.get_axis(1);
const Vector3 axisA1 = m_calculatedTransformA.basis.get_axis(2);
const Vector3 axisB0 = m_calculatedTransformB.basis.get_axis(1);
real_t rot = atan2fast(axisB0.dot(axisA1), axisB0.dot(axisA0));
- if(rot < m_lowerAngLimit)
- {
+ if (rot < m_lowerAngLimit) {
m_angDepth = rot - m_lowerAngLimit;
m_solveAngLim = true;
- }
- else if(rot > m_upperAngLimit)
- {
+ } else if (rot > m_upperAngLimit) {
m_angDepth = rot - m_upperAngLimit;
m_solveAngLim = true;
}
}
} // SliderJointSW::testAngLimits()
-
//-----------------------------------------------------------------------------
-
-
-Vector3 SliderJointSW::getAncorInA(void)
-{
+Vector3 SliderJointSW::getAncorInA(void) {
Vector3 ancorInA;
ancorInA = m_realPivotAInW + (m_lowerLinLimit + m_upperLinLimit) * real_t(0.5) * m_sliderAxis;
- ancorInA = A->get_transform().inverse().xform( ancorInA );
+ ancorInA = A->get_transform().inverse().xform(ancorInA);
return ancorInA;
} // SliderJointSW::getAncorInA()
//-----------------------------------------------------------------------------
-Vector3 SliderJointSW::getAncorInB(void)
-{
+Vector3 SliderJointSW::getAncorInB(void) {
Vector3 ancorInB;
ancorInB = m_frameInB.origin;
return ancorInB;
@@ -408,38 +357,36 @@ Vector3 SliderJointSW::getAncorInB(void)
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;
- case PhysicsServer::SLIDER_JOINT_LINEAR_LIMIT_LOWER: m_lowerLinLimit=p_value; break;
- case PhysicsServer::SLIDER_JOINT_LINEAR_LIMIT_SOFTNESS: m_softnessLimLin=p_value; break;
- case PhysicsServer::SLIDER_JOINT_LINEAR_LIMIT_RESTITUTION: m_restitutionLimLin=p_value; break;
- case PhysicsServer::SLIDER_JOINT_LINEAR_LIMIT_DAMPING: m_dampingLimLin=p_value; break;
- case PhysicsServer::SLIDER_JOINT_LINEAR_MOTION_SOFTNESS: m_softnessDirLin=p_value; break;
- case PhysicsServer::SLIDER_JOINT_LINEAR_MOTION_RESTITUTION: m_restitutionDirLin=p_value; break;
- case PhysicsServer::SLIDER_JOINT_LINEAR_MOTION_DAMPING: m_dampingDirLin=p_value; break;
- case PhysicsServer::SLIDER_JOINT_LINEAR_ORTHOGONAL_SOFTNESS: m_softnessOrthoLin=p_value; break;
- case PhysicsServer::SLIDER_JOINT_LINEAR_ORTHOGONAL_RESTITUTION: m_restitutionOrthoLin=p_value; break;
- case PhysicsServer::SLIDER_JOINT_LINEAR_ORTHOGONAL_DAMPING: m_dampingOrthoLin=p_value; break;
-
- case PhysicsServer::SLIDER_JOINT_ANGULAR_LIMIT_UPPER: m_upperAngLimit=p_value; break;
- case PhysicsServer::SLIDER_JOINT_ANGULAR_LIMIT_LOWER: m_lowerAngLimit=p_value; break;
- case PhysicsServer::SLIDER_JOINT_ANGULAR_LIMIT_SOFTNESS: m_softnessLimAng=p_value; break;
- case PhysicsServer::SLIDER_JOINT_ANGULAR_LIMIT_RESTITUTION: m_restitutionLimAng=p_value; break;
- case PhysicsServer::SLIDER_JOINT_ANGULAR_LIMIT_DAMPING: m_dampingLimAng=p_value; break;
- case PhysicsServer::SLIDER_JOINT_ANGULAR_MOTION_SOFTNESS: m_softnessDirAng=p_value; break;
- case PhysicsServer::SLIDER_JOINT_ANGULAR_MOTION_RESTITUTION: m_restitutionDirAng=p_value; break;
- case PhysicsServer::SLIDER_JOINT_ANGULAR_MOTION_DAMPING: m_dampingDirAng=p_value; break;
- case PhysicsServer::SLIDER_JOINT_ANGULAR_ORTHOGONAL_SOFTNESS: m_softnessOrthoAng=p_value; break;
- case PhysicsServer::SLIDER_JOINT_ANGULAR_ORTHOGONAL_RESTITUTION: m_restitutionOrthoAng=p_value; break;
- case PhysicsServer::SLIDER_JOINT_ANGULAR_ORTHOGONAL_DAMPING: m_dampingOrthoAng=p_value; break;
+ switch (p_param) {
+ case PhysicsServer::SLIDER_JOINT_LINEAR_LIMIT_UPPER: m_upperLinLimit = p_value; break;
+ case PhysicsServer::SLIDER_JOINT_LINEAR_LIMIT_LOWER: m_lowerLinLimit = p_value; break;
+ case PhysicsServer::SLIDER_JOINT_LINEAR_LIMIT_SOFTNESS: m_softnessLimLin = p_value; break;
+ case PhysicsServer::SLIDER_JOINT_LINEAR_LIMIT_RESTITUTION: m_restitutionLimLin = p_value; break;
+ case PhysicsServer::SLIDER_JOINT_LINEAR_LIMIT_DAMPING: m_dampingLimLin = p_value; break;
+ case PhysicsServer::SLIDER_JOINT_LINEAR_MOTION_SOFTNESS: m_softnessDirLin = p_value; break;
+ case PhysicsServer::SLIDER_JOINT_LINEAR_MOTION_RESTITUTION: m_restitutionDirLin = p_value; break;
+ case PhysicsServer::SLIDER_JOINT_LINEAR_MOTION_DAMPING: m_dampingDirLin = p_value; break;
+ case PhysicsServer::SLIDER_JOINT_LINEAR_ORTHOGONAL_SOFTNESS: m_softnessOrthoLin = p_value; break;
+ case PhysicsServer::SLIDER_JOINT_LINEAR_ORTHOGONAL_RESTITUTION: m_restitutionOrthoLin = p_value; break;
+ case PhysicsServer::SLIDER_JOINT_LINEAR_ORTHOGONAL_DAMPING: m_dampingOrthoLin = p_value; break;
+ case PhysicsServer::SLIDER_JOINT_ANGULAR_LIMIT_UPPER: m_upperAngLimit = p_value; break;
+ case PhysicsServer::SLIDER_JOINT_ANGULAR_LIMIT_LOWER: m_lowerAngLimit = p_value; break;
+ case PhysicsServer::SLIDER_JOINT_ANGULAR_LIMIT_SOFTNESS: m_softnessLimAng = p_value; break;
+ case PhysicsServer::SLIDER_JOINT_ANGULAR_LIMIT_RESTITUTION: m_restitutionLimAng = p_value; break;
+ case PhysicsServer::SLIDER_JOINT_ANGULAR_LIMIT_DAMPING: m_dampingLimAng = p_value; break;
+ case PhysicsServer::SLIDER_JOINT_ANGULAR_MOTION_SOFTNESS: m_softnessDirAng = p_value; break;
+ case PhysicsServer::SLIDER_JOINT_ANGULAR_MOTION_RESTITUTION: m_restitutionDirAng = p_value; break;
+ case PhysicsServer::SLIDER_JOINT_ANGULAR_MOTION_DAMPING: m_dampingDirAng = p_value; break;
+ case PhysicsServer::SLIDER_JOINT_ANGULAR_ORTHOGONAL_SOFTNESS: m_softnessOrthoAng = p_value; break;
+ case PhysicsServer::SLIDER_JOINT_ANGULAR_ORTHOGONAL_RESTITUTION: m_restitutionOrthoAng = p_value; break;
+ case PhysicsServer::SLIDER_JOINT_ANGULAR_ORTHOGONAL_DAMPING: m_dampingOrthoAng = p_value; break;
}
-
}
real_t SliderJointSW::get_param(PhysicsServer::SliderJointParam p_param) const {
- switch(p_param) {
+ switch (p_param) {
case PhysicsServer::SLIDER_JOINT_LINEAR_LIMIT_UPPER: return m_upperLinLimit;
case PhysicsServer::SLIDER_JOINT_LINEAR_LIMIT_LOWER: return m_lowerLinLimit;
case PhysicsServer::SLIDER_JOINT_LINEAR_LIMIT_SOFTNESS: return m_softnessLimLin;
@@ -463,11 +410,7 @@ real_t SliderJointSW::get_param(PhysicsServer::SliderJointParam p_param) const {
case PhysicsServer::SLIDER_JOINT_ANGULAR_ORTHOGONAL_SOFTNESS: return m_softnessOrthoAng;
case PhysicsServer::SLIDER_JOINT_ANGULAR_ORTHOGONAL_RESTITUTION: return m_restitutionOrthoAng;
case PhysicsServer::SLIDER_JOINT_ANGULAR_ORTHOGONAL_DAMPING: return m_dampingOrthoAng;
-
}
return 0;
-
}
-
-
diff --git a/servers/physics/joints/slider_joint_sw.h b/servers/physics/joints/slider_joint_sw.h
index d01038df5..faf36bfe9 100644
--- a/servers/physics/joints/slider_joint_sw.h
+++ b/servers/physics/joints/slider_joint_sw.h
@@ -34,9 +34,8 @@ Adapted to Godot from the Bullet library.
#ifndef SLIDER_JOINT_SW_H
#define SLIDER_JOINT_SW_H
-#include "servers/physics/joints_sw.h"
#include "servers/physics/joints/jacobian_entry_sw.h"
-
+#include "servers/physics/joints_sw.h"
/*
Bullet Continuous Collision Detection and Physics Library
@@ -59,15 +58,14 @@ April 04, 2008
*/
-#define SLIDER_CONSTRAINT_DEF_SOFTNESS (real_t(1.0))
-#define SLIDER_CONSTRAINT_DEF_DAMPING (real_t(1.0))
-#define SLIDER_CONSTRAINT_DEF_RESTITUTION (real_t(0.7))
+#define SLIDER_CONSTRAINT_DEF_SOFTNESS (real_t(1.0))
+#define SLIDER_CONSTRAINT_DEF_DAMPING (real_t(1.0))
+#define SLIDER_CONSTRAINT_DEF_RESTITUTION (real_t(0.7))
//-----------------------------------------------------------------------------
class SliderJointSW : public JointSW {
protected:
-
union {
struct {
BodySW *A;
@@ -77,8 +75,8 @@ protected:
BodySW *_arr[2];
};
- Transform m_frameInA;
- Transform m_frameInB;
+ Transform m_frameInA;
+ Transform m_frameInB;
// linear limits
real_t m_lowerLinLimit;
@@ -115,14 +113,14 @@ protected:
bool m_solveLinLim;
bool m_solveAngLim;
- JacobianEntrySW m_jacLin[3];
- real_t m_jacLinDiagABInv[3];
+ JacobianEntrySW m_jacLin[3];
+ real_t m_jacLinDiagABInv[3];
- JacobianEntrySW m_jacAng[3];
+ JacobianEntrySW m_jacAng[3];
real_t m_timeStep;
- Transform m_calculatedTransformA;
- Transform m_calculatedTransformB;
+ Transform m_calculatedTransformA;
+ Transform m_calculatedTransformB;
Vector3 m_sliderAxis;
Vector3 m_realPivotAInW;
@@ -138,45 +136,46 @@ protected:
real_t m_angDepth;
real_t m_kAngle;
- bool m_poweredLinMotor;
- real_t m_targetLinMotorVelocity;
- real_t m_maxLinMotorForce;
- real_t m_accumulatedLinMotorImpulse;
+ bool m_poweredLinMotor;
+ real_t m_targetLinMotorVelocity;
+ real_t m_maxLinMotorForce;
+ real_t m_accumulatedLinMotorImpulse;
- bool m_poweredAngMotor;
- real_t m_targetAngMotorVelocity;
- real_t m_maxAngMotorForce;
- real_t m_accumulatedAngMotorImpulse;
+ bool m_poweredAngMotor;
+ real_t m_targetAngMotorVelocity;
+ real_t m_maxAngMotorForce;
+ real_t m_accumulatedAngMotorImpulse;
//------------------------
void initParams();
+
public:
// constructors
- SliderJointSW(BodySW* rbA, BodySW* rbB, const Transform& frameInA, const Transform& frameInB);
- //SliderJointSW();
+ SliderJointSW(BodySW *rbA, BodySW *rbB, const Transform &frameInA, const Transform &frameInB);
+ //SliderJointSW();
// overrides
// access
- const BodySW* getRigidBodyA() const { return A; }
- const BodySW* 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; }
- real_t getLowerLinLimit() { return m_lowerLinLimit; }
- void setLowerLinLimit(real_t lowerLimit) { m_lowerLinLimit = lowerLimit; }
- real_t getUpperLinLimit() { return m_upperLinLimit; }
- void setUpperLinLimit(real_t upperLimit) { m_upperLinLimit = upperLimit; }
- real_t getLowerAngLimit() { return m_lowerAngLimit; }
- void setLowerAngLimit(real_t lowerLimit) { m_lowerAngLimit = lowerLimit; }
- real_t getUpperAngLimit() { return m_upperAngLimit; }
- void setUpperAngLimit(real_t upperLimit) { m_upperAngLimit = upperLimit; }
+ const BodySW *getRigidBodyA() const { return A; }
+ const BodySW *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; }
+ real_t getLowerLinLimit() { return m_lowerLinLimit; }
+ void setLowerLinLimit(real_t lowerLimit) { m_lowerLinLimit = lowerLimit; }
+ real_t getUpperLinLimit() { return m_upperLinLimit; }
+ void setUpperLinLimit(real_t upperLimit) { m_upperLinLimit = upperLimit; }
+ real_t getLowerAngLimit() { return m_lowerAngLimit; }
+ void setLowerAngLimit(real_t lowerLimit) { m_lowerAngLimit = lowerLimit; }
+ real_t getUpperAngLimit() { return m_upperAngLimit; }
+ void setUpperAngLimit(real_t upperLimit) { m_upperAngLimit = upperLimit; }
real_t getSoftnessDirLin() { return m_softnessDirLin; }
real_t getRestitutionDirLin() { return m_restitutionDirLin; }
- real_t getDampingDirLin() { return m_dampingDirLin ; }
+ real_t getDampingDirLin() { return m_dampingDirLin; }
real_t getSoftnessDirAng() { return m_softnessDirAng; }
real_t getRestitutionDirAng() { return m_restitutionDirAng; }
real_t getDampingDirAng() { return m_dampingDirAng; }
@@ -230,9 +229,9 @@ public:
bool getSolveAngLimit() { return m_solveAngLim; }
real_t getAngDepth() { return m_angDepth; }
// shared code used by ODE solver
- void calculateTransforms(void);
- void testLinLimits(void);
- void testAngLimits(void);
+ void calculateTransforms(void);
+ void testLinLimits(void);
+ void testAngLimits(void);
// access for PE Solver
Vector3 getAncorInA(void);
Vector3 getAncorInB(void);
@@ -244,8 +243,6 @@ public:
void solve(real_t p_step);
virtual PhysicsServer::JointType get_type() const { return PhysicsServer::JOINT_SLIDER; }
-
};
-
#endif // SLIDER_JOINT_SW_H
generated by cgit v1.2.3-70-g09d2 (git 2.52.0) at 2025-12-19 12:00:25 +0000