summaryrefslogtreecommitdiff
path: root/servers/physics/joints
diff options
context:
space:
mode:
Diffstat (limited to 'servers/physics/joints')
-rw-r--r--servers/physics/joints/SCsub8
-rw-r--r--servers/physics/joints/cone_twist_joint_sw.cpp340
-rw-r--r--servers/physics/joints/cone_twist_joint_sw.h125
-rw-r--r--servers/physics/joints/generic_6dof_joint_sw.cpp691
-rw-r--r--servers/physics/joints/generic_6dof_joint_sw.h428
-rw-r--r--servers/physics/joints/hinge_joint_sw.cpp438
-rw-r--r--servers/physics/joints/hinge_joint_sw.h89
-rw-r--r--servers/physics/joints/jacobian_entry_sw.cpp2
-rw-r--r--servers/physics/joints/jacobian_entry_sw.h146
-rw-r--r--servers/physics/joints/pin_joint_sw.cpp127
-rw-r--r--servers/physics/joints/pin_joint_sw.h67
-rw-r--r--servers/physics/joints/slider_joint_sw.cpp439
-rw-r--r--servers/physics/joints/slider_joint_sw.h218
13 files changed, 3118 insertions, 0 deletions
diff --git a/servers/physics/joints/SCsub b/servers/physics/joints/SCsub
new file mode 100644
index 0000000000..97d6edea21
--- /dev/null
+++ b/servers/physics/joints/SCsub
@@ -0,0 +1,8 @@
+Import('env')
+
+env.add_source_files(env.servers_sources,"*.cpp")
+
+Export('env')
+
+
+
diff --git a/servers/physics/joints/cone_twist_joint_sw.cpp b/servers/physics/joints/cone_twist_joint_sw.cpp
new file mode 100644
index 0000000000..d97d8c599f
--- /dev/null
+++ b/servers/physics/joints/cone_twist_joint_sw.cpp
@@ -0,0 +1,340 @@
+#include "cone_twist_joint_sw.h"
+
+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);
+ }
+}
+
+
+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);
+ real_t angle;
+ if (x >= 0.0f) {
+ real_t r = (x - abs_y) / (x + abs_y);
+ angle = coeff_1 - coeff_1 * r;
+ } else {
+ real_t r = (x + abs_y) / (abs_y - x);
+ angle = coeff_2 - coeff_1 * r;
+ }
+ return (y < 0.0f) ? -angle : angle;
+}
+
+ConeTwistJointSW::ConeTwistJointSW(BodySW* rbA,BodySW* rbB,const Transform& rbAFrame, const Transform& rbBFrame) : JointSW(_arr,2) {
+
+ A=rbA;
+ B=rbB;
+
+
+ m_rbAFrame=rbAFrame;
+ m_rbBFrame=rbBFrame;
+
+ 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);
+
+ m_appliedImpulse=0;
+}
+
+
+bool ConeTwistJointSW::setup(float p_step) {
+ m_appliedImpulse = real_t(0.);
+
+ //set bias, sign, clear accumulator
+ m_swingCorrection = real_t(0.);
+ m_twistLimitSign = real_t(0.);
+ m_solveTwistLimit = false;
+ m_solveSwingLimit = false;
+ m_accTwistLimitImpulse = real_t(0.);
+ m_accSwingLimitImpulse = real_t(0.);
+
+ 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)
+ {
+ normal[0] = relPos.normalized();
+ }
+ 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++)
+ {
+ memnew_placement(&m_jac[i], JacobianEntrySW(
+ A->get_transform().basis.transposed(),
+ B->get_transform().basis.transposed(),
+ pivotAInW - A->get_transform().origin,
+ pivotBInW - B->get_transform().origin,
+ 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;
+
+ 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 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) );
+// 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;
+ 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) );
+// 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;
+ 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;
+
+ 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.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));
+
+ }
+
+ // 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);
+ Vector3 TwistRef = rotationArc.xform(b2Axis2);
+ 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)
+ {
+ m_twistCorrection = -(twist + m_twistSpan);
+ m_solveTwistLimit = true;
+
+ m_twistAxis = (b2Axis1 + b1Axis1) * 0.5f;
+ m_twistAxis.normalize();
+ 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;
+
+ 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)
+{
+
+ Vector3 pivotAInW = A->get_transform().xform(m_rbAFrame.origin);
+ Vector3 pivotBInW = B->get_transform().xform(m_rbBFrame.origin);
+
+ real_t tau = real_t(0.3);
+
+ //linear part
+ if (!m_angularOnly)
+ {
+ Vector3 rel_pos1 = pivotAInW - A->get_transform().origin;
+ Vector3 rel_pos2 = pivotBInW - B->get_transform().origin;
+
+ Vector3 vel1 = A->get_velocity_in_local_point(rel_pos1);
+ 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;
+ 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;
+ 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);
+ }
+ }
+
+ {
+ ///solve angular part
+ 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);
+ 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) );
+ 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 );
+ 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) );
+ 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, float p_value) {
+
+ switch(p_param) {
+ case PhysicsServer::CONE_TWIST_JOINT_SWING_SPAN: {
+
+ m_swingSpan1=p_value;
+ m_swingSpan2=p_value;
+ } break;
+ case PhysicsServer::CONE_TWIST_JOINT_TWIST_SPAN: {
+
+ m_twistSpan=p_value;
+ } break;
+ case PhysicsServer::CONE_TWIST_JOINT_BIAS: {
+
+ m_biasFactor=p_value;
+ } break;
+ case PhysicsServer::CONE_TWIST_JOINT_SOFTNESS: {
+
+ m_limitSoftness=p_value;
+ } break;
+ case PhysicsServer::CONE_TWIST_JOINT_RELAXATION: {
+
+ m_relaxationFactor=p_value;
+ } break;
+ }
+}
+
+float ConeTwistJointSW::get_param(PhysicsServer::ConeTwistJointParam p_param) const{
+
+ switch(p_param) {
+ case PhysicsServer::CONE_TWIST_JOINT_SWING_SPAN: {
+
+ return m_swingSpan1;
+ } break;
+ case PhysicsServer::CONE_TWIST_JOINT_TWIST_SPAN: {
+
+ return m_twistSpan;
+ } break;
+ case PhysicsServer::CONE_TWIST_JOINT_BIAS: {
+
+ return m_biasFactor;
+ } break;
+ case PhysicsServer::CONE_TWIST_JOINT_SOFTNESS: {
+
+ return m_limitSoftness;
+ } break;
+ case PhysicsServer::CONE_TWIST_JOINT_RELAXATION: {
+
+ return m_relaxationFactor;
+ } break;
+ }
+
+ return 0;
+}
diff --git a/servers/physics/joints/cone_twist_joint_sw.h b/servers/physics/joints/cone_twist_joint_sw.h
new file mode 100644
index 0000000000..63502d2036
--- /dev/null
+++ b/servers/physics/joints/cone_twist_joint_sw.h
@@ -0,0 +1,125 @@
+#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"
+
+
+/*
+Bullet Continuous Collision Detection and Physics Library
+ConeTwistJointSW is Copyright (c) 2007 Starbreeze Studios
+
+This software is provided 'as-is', without any express or implied warranty.
+In no event will the authors be held liable for any damages arising from the use of this software.
+Permission is granted to anyone to use this software for any purpose,
+including commercial applications, and to alter it and redistribute it freely,
+subject to the following restrictions:
+
+1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
+2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
+3. This notice may not be removed or altered from any source distribution.
+
+Written by: Marcus Hennix
+*/
+
+
+
+///ConeTwistJointSW can be used to simulate ragdoll joints (upper arm, leg etc)
+class ConeTwistJointSW : public JointSW
+{
+#ifdef IN_PARALLELL_SOLVER
+public:
+#endif
+
+ union {
+ struct {
+ BodySW *A;
+ BodySW *B;
+ };
+
+ BodySW *_arr[2];
+ };
+
+ 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;
+
+ Vector3 m_swingAxis;
+ Vector3 m_twistAxis;
+
+ real_t m_kSwing;
+ real_t m_kTwist;
+
+ real_t m_twistLimitSign;
+ real_t m_swingCorrection;
+ real_t m_twistCorrection;
+
+ 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(float p_step);
+ virtual void solve(float p_step);
+
+ ConeTwistJointSW(BodySW* rbA,BodySW* rbB,const Transform& rbAFrame, const Transform& rbBFrame);
+
+
+ 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)
+ {
+ m_swingSpan1 = _swingSpan1;
+ m_swingSpan2 = _swingSpan2;
+ m_twistSpan = _twistSpan;
+
+ m_limitSoftness = _softness;
+ m_biasFactor = _biasFactor;
+ m_relaxationFactor = _relaxationFactor;
+ }
+
+ inline int getSolveTwistLimit()
+ {
+ return m_solveTwistLimit;
+ }
+
+ inline int getSolveSwingLimit()
+ {
+ return m_solveTwistLimit;
+ }
+
+ inline real_t getTwistLimitSign()
+ {
+ return m_twistLimitSign;
+ }
+
+ void set_param(PhysicsServer::ConeTwistJointParam p_param, float p_value);
+ float 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
new file mode 100644
index 0000000000..3d569df2c9
--- /dev/null
+++ b/servers/physics/joints/generic_6dof_joint_sw.cpp
@@ -0,0 +1,691 @@
+#include "generic_6dof_joint_sw.h"
+
+
+
+#define GENERIC_D6_DISABLE_WARMSTARTING 1
+
+real_t btGetMatrixElem(const Matrix3& mat, int index);
+real_t btGetMatrixElem(const Matrix3& mat, int index)
+{
+ int i = index%3;
+ int j = index/3;
+ return mat[i][j];
+}
+
+///MatrixToEulerXYZ from http://www.geometrictools.com/LibFoundation/Mathematics/Wm4Matrix3.inl.html
+bool matrixToEulerXYZ(const Matrix3& mat,Vector3& xyz);
+bool matrixToEulerXYZ(const Matrix3& mat,Vector3& xyz)
+{
+// // rot = cy*cz -cy*sz sy
+// // cz*sx*sy+cx*sz cx*cz-sx*sy*sz -cy*sx
+// // -cx*cz*sy+sx*sz cz*sx+cx*sy*sz cx*cy
+//
+
+ if (btGetMatrixElem(mat,2) < real_t(1.0))
+ {
+ if (btGetMatrixElem(mat,2) > real_t(-1.0))
+ {
+ xyz[0] = Math::atan2(-btGetMatrixElem(mat,5),btGetMatrixElem(mat,8));
+ xyz[1] = Math::asin(btGetMatrixElem(mat,2));
+ xyz[2] = Math::atan2(-btGetMatrixElem(mat,1),btGetMatrixElem(mat,0));
+ return true;
+ }
+ else
+ {
+ // WARNING. Not unique. XA - ZA = -atan2(r10,r11)
+ xyz[0] = -Math::atan2(btGetMatrixElem(mat,3),btGetMatrixElem(mat,4));
+ xyz[1] = -Math_PI*0.5;
+ xyz[2] = real_t(0.0);
+ return false;
+ }
+ }
+ else
+ {
+ // WARNING. Not unique. XAngle + ZAngle = atan2(r10,r11)
+ xyz[0] = Math::atan2(btGetMatrixElem(mat,3),btGetMatrixElem(mat,4));
+ xyz[1] = Math_PI*0.5;
+ xyz[2] = 0.0;
+
+ }
+
+
+ return false;
+}
+
+
+
+//////////////////////////// G6DOFRotationalLimitMotorSW ////////////////////////////////////
+
+
+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;
+ return 1;
+ }
+ 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
+ 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 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();
+ }
+
+
+
+ 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
+ }
+
+
+ // correction impulse
+ 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;
+ }
+
+
+ // 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;
+
+
+
+ Vector3 motorImp = clippedMotorImpulse * axis;
+
+
+ body0->apply_torque_impulse(motorImp);
+ if (body1) body1->apply_torque_impulse(-motorImp);
+
+ return clippedMotorImpulse;
+
+
+}
+
+//////////////////////////// End G6DOFRotationalLimitMotorSW ////////////////////////////////////
+
+//////////////////////////// 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;
+
+ 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
+
+//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];
+
+ //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
+ {
+ 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 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;
+}
+
+//////////////////////////// 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);
+}
+
+
+
+
+
+void Generic6DOFJointSW::calculateAngleInfo()
+{
+ Matrix3 relative_frame = m_calculatedTransformA.basis.inverse()*m_calculatedTransformB.basis;
+
+ matrixToEulerXYZ(relative_frame,m_calculatedAxisAngleDiff);
+
+
+
+ // 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.
+
+ Vector3 axis0 = m_calculatedTransformB.basis.get_axis(0);
+ Vector3 axis2 = m_calculatedTransformA.basis.get_axis(2);
+
+ m_calculatedAxis[1] = axis2.cross(axis0);
+ m_calculatedAxis[0] = m_calculatedAxis[1].cross(axis2);
+ m_calculatedAxis[2] = axis0.cross(m_calculatedAxis[1]);
+
+
+// if(m_debugDrawer)
+// {
+//
+// char buff[300];
+// sprintf(buff,"\n X: %.2f ; Y: %.2f ; Z: %.2f ",
+// m_calculatedAxisAngleDiff[0],
+// m_calculatedAxisAngleDiff[1],
+// m_calculatedAxisAngleDiff[2]);
+// m_debugDrawer->reportErrorWarning(buff);
+// }
+
+}
+
+void Generic6DOFJointSW::calculateTransforms()
+{
+ m_calculatedTransformA = A->get_transform() * m_frameInA;
+ m_calculatedTransformB = B->get_transform() * m_frameInB;
+
+ calculateAngleInfo();
+}
+
+
+void Generic6DOFJointSW::buildLinearJacobian(
+ JacobianEntrySW & jacLinear,const Vector3 & normalWorld,
+ const Vector3 & pivotAInW,const Vector3 & pivotBInW)
+{
+ memnew_placement(&jacLinear, JacobianEntrySW(
+ A->get_transform().basis.transposed(),
+ B->get_transform().basis.transposed(),
+ pivotAInW - A->get_transform().origin,
+ pivotBInW - B->get_transform().origin,
+ 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_transform().basis.transposed(),
+ B->get_transform().basis.transposed(),
+ A->get_inv_inertia(),
+ B->get_inv_inertia()));
+
+}
+
+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();
+}
+
+bool Generic6DOFJointSW::setup(float 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();
+
+// 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;
+
+ 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);
+ else
+ 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);
+ }
+ }
+
+ return true;
+}
+
+
+void Generic6DOFJointSW::solve(real_t timeStep)
+{
+ m_timeStep = timeStep;
+
+ //calculateTransforms();
+
+ int i;
+
+ // linear
+
+ 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();
+
+ if (m_useLinearReferenceFrameA)
+ 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);
+
+ }
+ }
+
+ // 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);
+ }
+ }
+}
+
+void Generic6DOFJointSW::updateRHS(real_t timeStep)
+{
+ (void)timeStep;
+
+}
+
+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];
+}
+
+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))
+ {
+ weight = real_t(1.0);
+ }
+ else
+ {
+ weight = imA / (imA + imB);
+ }
+ 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, float p_value) {
+
+ ERR_FAIL_INDEX(p_axis,3);
+ switch(p_param) {
+ case PhysicsServer::G6DOF_JOINT_LINEAR_LOWER_LIMIT: {
+
+ m_linearLimits.m_lowerLimit[p_axis]=p_value;
+ } break;
+ case PhysicsServer::G6DOF_JOINT_LINEAR_UPPER_LIMIT: {
+
+ m_linearLimits.m_upperLimit[p_axis]=p_value;
+
+ } break;
+ case PhysicsServer::G6DOF_JOINT_LINEAR_LIMIT_SOFTNESS: {
+
+ m_linearLimits.m_limitSoftness[p_axis]=p_value;
+
+ } break;
+ case PhysicsServer::G6DOF_JOINT_LINEAR_RESTITUTION: {
+
+ m_linearLimits.m_restitution[p_axis]=p_value;
+
+ } break;
+ case PhysicsServer::G6DOF_JOINT_LINEAR_DAMPING: {
+
+ m_linearLimits.m_damping[p_axis]=p_value;
+
+ } break;
+ case PhysicsServer::G6DOF_JOINT_ANGULAR_LOWER_LIMIT: {
+
+ m_angularLimits[p_axis].m_loLimit=p_value;
+
+ } break;
+ case PhysicsServer::G6DOF_JOINT_ANGULAR_UPPER_LIMIT: {
+
+ m_angularLimits[p_axis].m_hiLimit=p_value;
+
+ } break;
+ case PhysicsServer::G6DOF_JOINT_ANGULAR_LIMIT_SOFTNESS: {
+
+ m_angularLimits[p_axis].m_limitSoftness;
+
+ } break;
+ case PhysicsServer::G6DOF_JOINT_ANGULAR_DAMPING: {
+
+ m_angularLimits[p_axis].m_damping=p_value;
+
+ } break;
+ case PhysicsServer::G6DOF_JOINT_ANGULAR_RESTITUTION: {
+
+ m_angularLimits[p_axis].m_bounce=p_value;
+
+ } break;
+ case PhysicsServer::G6DOF_JOINT_ANGULAR_FORCE_LIMIT: {
+
+ m_angularLimits[p_axis].m_maxLimitForce=p_value;
+
+ } break;
+ case PhysicsServer::G6DOF_JOINT_ANGULAR_ERP: {
+
+ 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;
+
+ } break;
+ case PhysicsServer::G6DOF_JOINT_ANGULAR_MOTOR_FORCE_LIMIT: {
+
+ m_angularLimits[p_axis].m_maxLimitForce=p_value;
+
+ } break;
+ }
+}
+
+float 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];
+ } break;
+ case PhysicsServer::G6DOF_JOINT_LINEAR_UPPER_LIMIT: {
+
+ return m_linearLimits.m_upperLimit[p_axis];
+
+ } break;
+ case PhysicsServer::G6DOF_JOINT_LINEAR_LIMIT_SOFTNESS: {
+
+ return m_linearLimits.m_limitSoftness[p_axis];
+
+ } break;
+ case PhysicsServer::G6DOF_JOINT_LINEAR_RESTITUTION: {
+
+ return m_linearLimits.m_restitution[p_axis];
+
+ } break;
+ case PhysicsServer::G6DOF_JOINT_LINEAR_DAMPING: {
+
+ return m_linearLimits.m_damping[p_axis];
+
+ } break;
+ case PhysicsServer::G6DOF_JOINT_ANGULAR_LOWER_LIMIT: {
+
+ return m_angularLimits[p_axis].m_loLimit;
+
+ } break;
+ case PhysicsServer::G6DOF_JOINT_ANGULAR_UPPER_LIMIT: {
+
+ return m_angularLimits[p_axis].m_hiLimit;
+
+ } break;
+ case PhysicsServer::G6DOF_JOINT_ANGULAR_LIMIT_SOFTNESS: {
+
+ return m_angularLimits[p_axis].m_limitSoftness;
+
+ } break;
+ case PhysicsServer::G6DOF_JOINT_ANGULAR_DAMPING: {
+
+ return m_angularLimits[p_axis].m_damping;
+
+ } break;
+ case PhysicsServer::G6DOF_JOINT_ANGULAR_RESTITUTION: {
+
+ return m_angularLimits[p_axis].m_bounce;
+
+ } break;
+ case PhysicsServer::G6DOF_JOINT_ANGULAR_FORCE_LIMIT: {
+
+ return m_angularLimits[p_axis].m_maxLimitForce;
+
+ } break;
+ case PhysicsServer::G6DOF_JOINT_ANGULAR_ERP: {
+
+ return m_angularLimits[p_axis].m_ERP;
+
+ } break;
+ case PhysicsServer::G6DOF_JOINT_ANGULAR_MOTOR_TARGET_VELOCITY: {
+
+ return m_angularLimits[p_axis].m_targetVelocity;
+
+ } break;
+ case PhysicsServer::G6DOF_JOINT_ANGULAR_MOTOR_FORCE_LIMIT: {
+
+ return m_angularLimits[p_axis].m_maxLimitForce;
+
+ } break;
+ }
+ return 0;
+}
+
+void Generic6DOFJointSW::set_flag(Vector3::Axis p_axis,PhysicsServer::G6DOFJointAxisFlag p_flag, bool p_value){
+
+ ERR_FAIL_INDEX(p_axis,3);
+
+ switch(p_flag) {
+ case PhysicsServer::G6DOF_JOINT_FLAG_ENABLE_LINEAR_LIMIT: {
+
+ 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;
+ } break;
+ case PhysicsServer::G6DOF_JOINT_FLAG_ENABLE_MOTOR: {
+
+ m_angularLimits[p_axis].m_enableMotor=p_value;
+ } break;
+ }
+
+
+}
+bool Generic6DOFJointSW::get_flag(Vector3::Axis p_axis,PhysicsServer::G6DOFJointAxisFlag p_flag) const{
+
+ 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];
+ } break;
+ case PhysicsServer::G6DOF_JOINT_FLAG_ENABLE_ANGULAR_LIMIT: {
+
+ return m_angularLimits[p_axis].m_enableLimit;
+ } break;
+ case PhysicsServer::G6DOF_JOINT_FLAG_ENABLE_MOTOR: {
+
+ return m_angularLimits[p_axis].m_enableMotor;
+ } break;
+ }
+
+ return 0;
+}
diff --git a/servers/physics/joints/generic_6dof_joint_sw.h b/servers/physics/joints/generic_6dof_joint_sw.h
new file mode 100644
index 0000000000..7f762e51a2
--- /dev/null
+++ b/servers/physics/joints/generic_6dof_joint_sw.h
@@ -0,0 +1,428 @@
+#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"
+
+
+/*
+Bullet Continuous Collision Detection and Physics Library
+Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/
+
+This software is provided 'as-is', without any express or implied warranty.
+In no event will the authors be held liable for any damages arising from the use of this software.
+Permission is granted to anyone to use this software for any purpose,
+including commercial applications, and to alter it and redistribute it freely,
+subject to the following restrictions:
+
+1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
+2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
+3. This notice may not be removed or altered from any source distribution.
+*/
+
+
+/*
+2007-09-09
+Generic6DOFJointSW Refactored by Francisco Le?n
+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;
+
+ //!@}
+
+ //! 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;
+ }
+
+ //! Need apply correction
+ bool needApplyTorques()
+ {
+ if(m_currentLimit == 0 && m_enableMotor == false) return false;
+ return true;
+ }
+
+ //! calculates error
+ /*!
+ calculates m_currentLimit and m_currentLimitError.
+ */
+ int testLimitValue(real_t test_value);
+
+ //! apply the correction impulses for two bodies
+ real_t solveAngularLimits(real_t timeStep,Vector3& axis, real_t jacDiagABInv,BodySW * body0, BodySW * body1);
+
+
+};
+
+
+
+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];
+
+ 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);
+
+ 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;
+
+ m_limitSoftness = other.m_limitSoftness ;
+ m_damping = other.m_damping;
+ m_restitution = other.m_restitution;
+ }
+
+ //! 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);
+
+
+};
+
+
+class Generic6DOFJointSW : public JointSW
+{
+protected:
+
+
+ union {
+ struct {
+ BodySW *A;
+ BodySW *B;
+ };
+
+ BodySW *_arr[2];
+ };
+
+ //! relative_frames
+ //!@{
+ 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
+ //!@}
+
+ //! Linear_Limit_parameters
+ //!@{
+ 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];
+
+ 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;
+ }
+
+
+
+ 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();
+
+
+
+public:
+ Generic6DOFJointSW(BodySW* rbA, BodySW* rbB, const Transform& frameInA, const Transform& frameInB ,bool useLinearReferenceFrameA);
+
+ virtual PhysicsServer::JointType get_type() const { return PhysicsServer::JOINT_6DOF; }
+
+ virtual bool setup(float p_step);
+ virtual void solve(float 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();
+
+ //! Gets the global transform of the offset for body A
+ /*!
+ \sa Generic6DOFJointSW.getFrameOffsetA, Generic6DOFJointSW.getFrameOffsetB, Generic6DOFJointSW.calculateAngleInfo.
+ */
+ const Transform & getCalculatedTransformA() const
+ {
+ return m_calculatedTransformA;
+ }
+
+ //! 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;
+ }
+
+
+ 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);
+
+ //! Get the rotation axis in global coordinates
+ /*!
+ \pre Generic6DOFJointSW.buildJacobian must be called previously.
+ */
+ Vector3 getAxis(int axis_index) const;
+
+ //! Get the relative Euler angle
+ /*!
+ \pre Generic6DOFJointSW.buildJacobian must be called previously.
+ */
+ 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);
+
+ void setLinearLowerLimit(const Vector3& linearLower)
+ {
+ m_linearLimits.m_lowerLimit = linearLower;
+ }
+
+ 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 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;
+ }
+
+ //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
+ /*!
+ - free means upper < lower,
+ - locked means upper == lower
+ - limited means upper > lower
+ - limitIndex: first 3 are linear, next 3 are angular
+ */
+ bool isLimited(int limitIndex)
+ {
+ if(limitIndex<3)
+ {
+ return m_linearLimits.isLimited(limitIndex);
+
+ }
+ return m_angularLimits[limitIndex-3].isLimited();
+ }
+
+ 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, float p_value);
+ float 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
new file mode 100644
index 0000000000..feaf00290d
--- /dev/null
+++ b/servers/physics/joints/hinge_joint_sw.cpp
@@ -0,0 +1,438 @@
+#include "hinge_joint_sw.h"
+
+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);
+ }
+}
+
+HingeJointSW::HingeJointSW(BodySW* rbA,BodySW* rbB, const Transform& frameA, const Transform& frameB) : JointSW(_arr,2) {
+
+ A=rbA;
+ B=rbB;
+
+ 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;
+
+ 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) {
+
+ A=rbA;
+ B=rbB;
+
+ m_rbAFrame.origin = pivotInA;
+
+ // since no frame is given, assume this to be zero angle and just pick rb transform axis
+ Vector3 rbAxisA1 = rbA->get_transform().basis.get_axis(0);
+
+ Vector3 rbAxisA2;
+ real_t projection = axisInA.dot(rbAxisA1);
+ if (projection >= 1.0f - CMP_EPSILON) {
+ rbAxisA1 = -rbA->get_transform().basis.get_axis(2);
+ rbAxisA2 = rbA->get_transform().basis.get_axis(1);
+ } else if (projection <= -1.0f + CMP_EPSILON) {
+ rbAxisA1 = rbA->get_transform().basis.get_axis(2);
+ rbAxisA2 = rbA->get_transform().basis.get_axis(1);
+ } else {
+ rbAxisA2 = axisInA.cross(rbAxisA1);
+ rbAxisA1 = rbAxisA2.cross(axisInA);
+ }
+
+ m_rbAFrame.basis=Matrix3( 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);
+
+ m_rbBFrame.origin = pivotInB;
+ m_rbBFrame.basis=Matrix3( 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;
+
+ A->add_constraint(this,0);
+ B->add_constraint(this,1);
+
+}
+
+
+
+bool HingeJointSW::setup(float p_step) {
+
+ m_appliedImpulse = real_t(0.);
+
+ 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)
+ {
+ normal[0] = relPos.normalized();
+ }
+ 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++)
+ {
+ memnew_placement(&m_jac[i], JacobianEntrySW(
+ A->get_transform().basis.transposed(),
+ B->get_transform().basis.transposed(),
+ pivotAInW - A->get_transform().origin,
+ pivotBInW - B->get_transform().origin,
+ normal[i],
+ A->get_inv_inertia(),
+ A->get_inv_mass(),
+ B->get_inv_inertia(),
+ B->get_inv_mass()) );
+ }
+ }
+
+ //calculate two perpendicular jointAxis, orthogonal to hingeAxis
+ //these two jointAxis require equal angular velocities for both bodies
+
+ //this is unused for now, it's a todo
+ 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) );
+
+ memnew_placement(&m_jacAng[0], JacobianEntrySW(jointAxis0,
+ A->get_transform().basis.transposed(),
+ B->get_transform().basis.transposed(),
+ A->get_inv_inertia(),
+ B->get_inv_inertia()));
+
+ memnew_placement(&m_jacAng[1], JacobianEntrySW(jointAxis1,
+ A->get_transform().basis.transposed(),
+ B->get_transform().basis.transposed(),
+ A->get_inv_inertia(),
+ B->get_inv_inertia()));
+
+ memnew_placement(&m_jacAng[2], JacobianEntrySW(hingeAxisWorld,
+ A->get_transform().basis.transposed(),
+ B->get_transform().basis.transposed(),
+ A->get_inv_inertia(),
+ B->get_inv_inertia()));
+
+
+ // Compute limit information
+ real_t hingeAngle = get_hinge_angle();
+
+// print_line("angle: "+rtos(hingeAngle));
+ //set bias, sign, clear accumulator
+ m_correction = real_t(0.);
+ m_limitSign = real_t(0.);
+ 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 (hingeAngle <= m_lowerLimit*m_limitSoftness)
+ 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)
+ {
+ m_correction = m_upperLimit - hingeAngle;
+ m_limitSign = -1.0f;
+ m_solveLimit = true;
+ }
+ }
+
+ //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));
+
+ return true;
+}
+
+void HingeJointSW::solve(float p_step) {
+
+ Vector3 pivotAInW = A->get_transform().xform(m_rbAFrame.origin);
+ Vector3 pivotBInW = B->get_transform().xform(m_rbBFrame.origin);
+
+ //real_t tau = real_t(0.3);
+
+ //linear part
+ if (!m_angularOnly)
+ {
+ Vector3 rel_pos1 = pivotAInW - A->get_transform().origin;
+ Vector3 rel_pos2 = pivotBInW - B->get_transform().origin;
+
+ Vector3 vel1 = A->get_velocity_in_local_point(rel_pos1);
+ 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;
+ 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;
+ 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);
+ }
+ }
+
+
+ {
+ ///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) );
+
+ 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;
+ {
+ //solve orthogonal angular velocity correction
+ real_t relaxation = real_t(1.);
+ real_t len = velrelOrthog.length();
+ 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);
+ // scale for mass and relaxation
+ velrelOrthog *= (real_t(1.)/denom) * m_relaxationFactor;
+ }
+
+ //solve angular positional correction
+ Vector3 angularError = -axisA.cross(axisB) *(real_t(1.)/p_step);
+ real_t len2 = angularError.length();
+ 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;
+ }
+
+ 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;
+
+ real_t impulseMag = amplitude * m_kHinge;
+
+ // Clamp the accumulated impulse
+ real_t temp = m_accLimitImpulse;
+ 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);
+ }
+ }
+
+ //apply motor
+ if (m_enableAngularMotor)
+ {
+ //todo: add limits too
+ Vector3 angularLimit(0,0,0);
+
+ Vector3 velrel = angVelAroundHingeAxisA - angVelAroundHingeAxisB;
+ real_t projRelVel = velrel.dot(axisA);
+
+ real_t desiredMotorVel = m_motorTargetVelocity;
+ real_t motor_relvel = desiredMotorVel - projRelVel;
+
+ real_t unclippedMotorImpulse = m_kHinge * motor_relvel;;
+ //todo: should clip against accumulated impulse
+ real_t clippedMotorImpulse = unclippedMotorImpulse > m_maxMotorImpulse ? m_maxMotorImpulse : unclippedMotorImpulse;
+ clippedMotorImpulse = clippedMotorImpulse < -m_maxMotorImpulse ? -m_maxMotorImpulse : clippedMotorImpulse;
+ Vector3 motorImp = clippedMotorImpulse * axisA;
+
+ A->apply_torque_impulse(motorImp+angularLimit);
+ B->apply_torque_impulse(-motorImp-angularLimit);
+
+ }
+ }
+
+}
+/*
+void HingeJointSW::updateRHS(real_t timeStep)
+{
+ (void)timeStep;
+
+}
+*/
+
+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);
+ real_t angle;
+ if (x >= 0.0f) {
+ real_t r = (x - abs_y) / (x + abs_y);
+ angle = coeff_1 - coeff_1 * r;
+ } else {
+ real_t r = (x + abs_y) / (abs_y - x);
+ angle = coeff_2 - coeff_1 * r;
+ }
+ 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) );
+
+ return atan2fast( swingAxis.dot(refAxis0), swingAxis.dot(refAxis1) );
+}
+
+
+void HingeJointSW::set_param(PhysicsServer::HingeJointParam p_param, float 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;
+
+ }
+}
+
+float HingeJointSW::get_param(PhysicsServer::HingeJointParam p_param) const{
+
+ switch (p_param) {
+
+ case PhysicsServer::HINGE_JOINT_BIAS: return tau;
+ case PhysicsServer::HINGE_JOINT_LIMIT_UPPER: return m_upperLimit;
+ case PhysicsServer::HINGE_JOINT_LIMIT_LOWER: return m_lowerLimit;
+ case PhysicsServer::HINGE_JOINT_LIMIT_BIAS: return m_biasFactor;
+ case PhysicsServer::HINGE_JOINT_LIMIT_SOFTNESS: return m_limitSoftness;
+ 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){
+
+ print_line(p_flag+": "+itos(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;
+ }
+
+}
+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;
+ }
+
+ return false;
+}
diff --git a/servers/physics/joints/hinge_joint_sw.h b/servers/physics/joints/hinge_joint_sw.h
new file mode 100644
index 0000000000..4f6cdaf799
--- /dev/null
+++ b/servers/physics/joints/hinge_joint_sw.h
@@ -0,0 +1,89 @@
+#ifndef HINGE_JOINT_SW_H
+#define HINGE_JOINT_SW_H
+
+#include "servers/physics/joints_sw.h"
+#include "servers/physics/joints/jacobian_entry_sw.h"
+
+
+/*
+Bullet Continuous Collision Detection and Physics Library
+Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/
+
+This software is provided 'as-is', without any express or implied warranty.
+In no event will the authors be held liable for any damages arising from the use of this software.
+Permission is granted to anyone to use this software for any purpose,
+including commercial applications, and to alter it and redistribute it freely,
+subject to the following restrictions:
+
+1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
+2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
+3. This notice may not be removed or altered from any source distribution.
+*/
+
+
+
+class HingeJointSW : public JointSW {
+
+ union {
+ struct {
+ BodySW *A;
+ BodySW *B;
+ };
+
+ BodySW *_arr[2];
+ };
+
+ 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_limitSoftness;
+ real_t m_biasFactor;
+ real_t m_relaxationFactor;
+
+ real_t m_lowerLimit;
+ real_t m_upperLimit;
+
+ real_t m_kHinge;
+
+ real_t m_limitSign;
+ real_t m_correction;
+
+ real_t m_accLimitImpulse;
+
+ real_t tau;
+
+ 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(float p_step);
+ virtual void solve(float p_step);
+
+ real_t get_hinge_angle();
+
+ void set_param(PhysicsServer::HingeJointParam p_param, float p_value);
+ float get_param(PhysicsServer::HingeJointParam p_param) const;
+
+ void set_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);
+
+};
+
+#endif // HINGE_JOINT_SW_H
diff --git a/servers/physics/joints/jacobian_entry_sw.cpp b/servers/physics/joints/jacobian_entry_sw.cpp
new file mode 100644
index 0000000000..faa3cf15c4
--- /dev/null
+++ b/servers/physics/joints/jacobian_entry_sw.cpp
@@ -0,0 +1,2 @@
+#include "jacobian_entry_sw.h"
+
diff --git a/servers/physics/joints/jacobian_entry_sw.h b/servers/physics/joints/jacobian_entry_sw.h
new file mode 100644
index 0000000000..16fa034215
--- /dev/null
+++ b/servers/physics/joints/jacobian_entry_sw.h
@@ -0,0 +1,146 @@
+#ifndef JACOBIAN_ENTRY_SW_H
+#define JACOBIAN_ENTRY_SW_H
+
+/*
+Bullet Continuous Collision Detection and Physics Library
+Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/
+
+This software is provided 'as-is', without any express or implied warranty.
+In no event will the authors be held liable for any damages arising from the use of this software.
+Permission is granted to anyone to use this software for any purpose,
+including commercial applications, and to alter it and redistribute it freely,
+subject to the following restrictions:
+
+1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
+2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
+3. This notice may not be removed or altered from any source distribution.
+*/
+
+#include "transform.h"
+
+class JacobianEntrySW {
+public:
+ JacobianEntrySW() {};
+ //constraint between two different rigidbodies
+ JacobianEntrySW(
+ const Matrix3& world2A,
+ const Matrix3& 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_1MinvJt = inertiaInvB * m_bJ;
+ m_Adiag = massInvA + m_0MinvJt.dot(m_aJ) + massInvB + m_1MinvJt.dot(m_bJ);
+
+ ERR_FAIL_COND(m_Adiag <= real_t(0.0));
+ }
+
+ //angular constraint between two different rigidbodies
+ JacobianEntrySW(const Vector3& jointAxis,
+ const Matrix3& world2A,
+ const Matrix3& 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_1MinvJt = inertiaInvB * 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;
+ m_1MinvJt = inertiaInvB * 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 Matrix3& 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_Adiag = massInvA + m_0MinvJt.dot(m_aJ);
+
+ ERR_FAIL_COND(m_Adiag <= real_t(0.0));
+ }
+
+ 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 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;
+ 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 lin1 = massInvB * lin;
+ 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)
+ {
+ Vector3 linrel = linvelA - linvelB;
+ 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];
+ return rel_vel2 + CMP_EPSILON;
+ }
+//private:
+
+ 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;
+
+};
+
+
+#endif // JACOBIAN_ENTRY_SW_H
diff --git a/servers/physics/joints/pin_joint_sw.cpp b/servers/physics/joints/pin_joint_sw.cpp
new file mode 100644
index 0000000000..229863fb7b
--- /dev/null
+++ b/servers/physics/joints/pin_joint_sw.cpp
@@ -0,0 +1,127 @@
+#include "pin_joint_sw.h"
+
+bool PinJointSW::setup(float p_step) {
+
+ m_appliedImpulse = real_t(0.);
+
+ Vector3 normal(0,0,0);
+
+ for (int i=0;i<3;i++)
+ {
+ normal[i] = 1;
+ memnew_placement(&m_jac[i],JacobianEntrySW(
+ A->get_transform().basis.transposed(),
+ B->get_transform().basis.transposed(),
+ A->get_transform().xform(m_pivotInA) - A->get_transform().origin,
+ B->get_transform().xform(m_pivotInB) - B->get_transform().origin,
+ 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(float p_step){
+
+ Vector3 pivotAInW = A->get_transform().xform(m_pivotInA);
+ Vector3 pivotBInW = B->get_transform().xform(m_pivotInB);
+
+
+ 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++)
+ {
+ normal[i] = 1;
+ real_t jacDiagABInv = real_t(1.) / m_jac[i].getDiagonal();
+
+ Vector3 rel_pos1 = pivotAInW - A->get_transform().origin;
+ Vector3 rel_pos2 = pivotBInW - B->get_transform().origin;
+ //this jacobian entry could be re-used for all iterations
+
+ Vector3 vel1 = A->get_velocity_in_local_point(rel_pos1);
+ Vector3 vel2 = B->get_velocity_in_local_point(rel_pos2);
+ Vector3 vel = vel1 - vel2;
+
+ 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);
+ */
+
+ //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 impulseClamp = m_impulseClamp;
+ if (impulseClamp > 0)
+ {
+ if (impulse < -impulseClamp)
+ impulse = -impulseClamp;
+ if (impulse > impulseClamp)
+ impulse = impulseClamp;
+ }
+
+ 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);
+
+ normal[i] = 0;
+ }
+}
+
+void PinJointSW::set_param(PhysicsServer::PinJointParam p_param,float 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;
+ }
+}
+
+float PinJointSW::get_param(PhysicsServer::PinJointParam p_param) const{
+
+ 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;
+ }
+
+ 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;
+
+ 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
new file mode 100644
index 0000000000..dae6e7d5f2
--- /dev/null
+++ b/servers/physics/joints/pin_joint_sw.h
@@ -0,0 +1,67 @@
+#ifndef PIN_JOINT_SW_H
+#define PIN_JOINT_SW_H
+
+#include "servers/physics/joints_sw.h"
+#include "servers/physics/joints/jacobian_entry_sw.h"
+
+/*
+Bullet Continuous Collision Detection and Physics Library
+Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/
+
+This software is provided 'as-is', without any express or implied warranty.
+In no event will the authors be held liable for any damages arising from the use of this software.
+Permission is granted to anyone to use this software for any purpose,
+including commercial applications, and to alter it and redistribute it freely,
+subject to the following restrictions:
+
+1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
+2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
+3. This notice may not be removed or altered from any source distribution.
+*/
+
+
+class PinJointSW : public JointSW {
+
+ union {
+ struct {
+ BodySW *A;
+ BodySW *B;
+ };
+
+ BodySW *_arr[2];
+ };
+
+
+ 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
+
+ Vector3 m_pivotInA;
+ Vector3 m_pivotInB;
+
+public:
+
+ virtual PhysicsServer::JointType get_type() const { return PhysicsServer::JOINT_PIN; }
+
+ virtual bool setup(float p_step);
+ virtual void solve(float p_step);
+
+ void set_param(PhysicsServer::PinJointParam p_param,float p_value);
+ float 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; }
+
+ 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();
+};
+
+
+
+#endif // PIN_JOINT_SW_H
diff --git a/servers/physics/joints/slider_joint_sw.cpp b/servers/physics/joints/slider_joint_sw.cpp
new file mode 100644
index 0000000000..faa6875378
--- /dev/null
+++ b/servers/physics/joints/slider_joint_sw.cpp
@@ -0,0 +1,439 @@
+#include "slider_joint_sw.h"
+
+//-----------------------------------------------------------------------------
+
+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);
+ real_t angle;
+ if (x >= 0.0f) {
+ real_t r = (x - abs_y) / (x + abs_y);
+ angle = coeff_1 - coeff_1 * r;
+ } else {
+ real_t r = (x + abs_y) / (abs_y - x);
+ angle = coeff_2 - coeff_1 * r;
+ }
+ 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.);
+ m_softnessDirLin = SLIDER_CONSTRAINT_DEF_SOFTNESS;
+ m_restitutionDirLin = SLIDER_CONSTRAINT_DEF_RESTITUTION;
+ m_dampingDirLin = real_t(0.);
+ m_softnessDirAng = SLIDER_CONSTRAINT_DEF_SOFTNESS;
+ m_restitutionDirAng = SLIDER_CONSTRAINT_DEF_RESTITUTION;
+ m_dampingDirAng = real_t(0.);
+ m_softnessOrthoLin = SLIDER_CONSTRAINT_DEF_SOFTNESS;
+ m_restitutionOrthoLin = SLIDER_CONSTRAINT_DEF_RESTITUTION;
+ m_dampingOrthoLin = SLIDER_CONSTRAINT_DEF_DAMPING;
+ m_softnessOrthoAng = SLIDER_CONSTRAINT_DEF_SOFTNESS;
+ m_restitutionOrthoAng = SLIDER_CONSTRAINT_DEF_RESTITUTION;
+ m_dampingOrthoAng = SLIDER_CONSTRAINT_DEF_DAMPING;
+ m_softnessLimLin = SLIDER_CONSTRAINT_DEF_SOFTNESS;
+ m_restitutionLimLin = SLIDER_CONSTRAINT_DEF_RESTITUTION;
+ m_dampingLimLin = SLIDER_CONSTRAINT_DEF_DAMPING;
+ m_softnessLimAng = SLIDER_CONSTRAINT_DEF_SOFTNESS;
+ m_restitutionLimAng = SLIDER_CONSTRAINT_DEF_RESTITUTION;
+ m_dampingLimAng = SLIDER_CONSTRAINT_DEF_DAMPING;
+
+ m_poweredLinMotor = false;
+ m_targetLinMotorVelocity = real_t(0.);
+ m_maxLinMotorForce = real_t(0.);
+ m_accumulatedLinMotorImpulse = real_t(0.0);
+
+ m_poweredAngMotor = false;
+ 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)
+{
+
+ A=rbA;
+ B=rbB;
+
+ A->add_constraint(this,0);
+ B->add_constraint(this,1);
+
+ initParams();
+} // SliderJointSW::SliderJointSW()
+
+//-----------------------------------------------------------------------------
+
+bool SliderJointSW::setup(float p_step)
+{
+
+ //calculate transforms
+ 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;
+ 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++)
+ {
+ normalWorld = m_calculatedTransformA.basis.get_axis(i);
+ memnew_placement(&m_jacLin[i], JacobianEntrySW(
+ A->get_transform().basis.transposed(),
+ B->get_transform().basis.transposed(),
+ m_relPosA,
+ m_relPosB,
+ 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++)
+ {
+ normalWorld = m_calculatedTransformA.basis.get_axis(i);
+ memnew_placement(&m_jacAng[i], JacobianEntrySW(
+ normalWorld,
+ A->get_transform().basis.transposed(),
+ B->get_transform().basis.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));
+ // clear accumulator for motors
+ m_accumulatedLinMotorImpulse = real_t(0.0);
+ m_accumulatedAngMotorImpulse = real_t(0.0);
+
+ return true;
+} // SliderJointSW::buildJacobianInt()
+
+//-----------------------------------------------------------------------------
+
+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;
+ real_t rel_vel = normal.dot(vel);
+ // calculate positional error
+ real_t depth = m_depth[i];
+ // get parameters
+ real_t softness = (i) ? m_softnessOrthoLin : (m_solveLinLim ? m_softnessLimLin : m_softnessDirLin);
+ real_t restitution = (i) ? m_restitutionOrthoLin : (m_solveLinLim ? m_restitutionLimLin : m_restitutionDirLin);
+ real_t damping = (i) ? m_dampingOrthoLin : (m_solveLinLim ? m_dampingLimLin : m_dampingDirLin);
+ // 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)
+ {
+ 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)
+ {
+ new_acc = m_maxLinMotorForce;
+ }
+ real_t del = new_acc - m_accumulatedLinMotorImpulse;
+ if(normalImpulse < real_t(0.0))
+ {
+ normalImpulse = -del;
+ }
+ 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);
+ }
+ }
+ }
+ // angular
+ // get axes in world space
+ 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();
+
+ Vector3 angVelAroundAxisA = axisA * axisA.dot(angVelA);
+ Vector3 angVelAroundAxisB = axisB * axisB.dot(angVelB);
+
+ Vector3 angAorthog = angVelA - angVelAroundAxisA;
+ Vector3 angBorthog = angVelB - angVelAroundAxisB;
+ Vector3 velrelOrthog = angAorthog-angBorthog;
+ //solve orthogonal angular velocity correction
+ real_t len = velrelOrthog.length();
+ 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;
+ }
+ //solve angular positional correction
+ Vector3 angularError = axisA.cross(axisB) *(real_t(1.)/p_step);
+ real_t len2 = angularError.length();
+ 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;
+ }
+ // apply impulse
+ A->apply_torque_impulse(-velrelOrthog+angularError);
+ B->apply_torque_impulse(velrelOrthog-angularError);
+ real_t impulseMag;
+ //solve angular limits
+ if(m_solveAngLim)
+ {
+ impulseMag = (angVelB - angVelA).dot(axisA) * m_dampingLimAng + m_angDepth * m_restitutionLimAng / p_step;
+ impulseMag *= m_kAngle * m_softnessLimAng;
+ }
+ else
+ {
+ impulseMag = (angVelB - angVelA).dot(axisA) * m_dampingDirAng + m_angDepth * m_restitutionDirAng / p_step;
+ impulseMag *= m_kAngle * m_softnessDirAng;
+ }
+ Vector3 impulse = axisA * impulseMag;
+ A->apply_torque_impulse(impulse);
+ B->apply_torque_impulse(-impulse);
+ //apply angular motor
+ if(m_poweredAngMotor)
+ {
+ if(m_accumulatedAngMotorImpulse < m_maxAngMotorForce)
+ {
+ Vector3 velrel = angVelAroundAxisA - angVelAroundAxisB;
+ real_t projRelVel = velrel.dot(axisA);
+
+ real_t desiredMotorVel = m_targetAngMotorVelocity;
+ real_t motor_relvel = desiredMotorVel - projRelVel;
+
+ real_t angImpulse = m_kAngle * motor_relvel;
+ // clamp accumulated impulse
+ real_t new_acc = m_accumulatedAngMotorImpulse + Math::abs(angImpulse);
+ if(new_acc > m_maxAngMotorForce)
+ {
+ new_acc = m_maxAngMotorForce;
+ }
+ real_t del = new_acc - m_accumulatedAngMotorImpulse;
+ if(angImpulse < real_t(0.0))
+ {
+ angImpulse = -del;
+ }
+ else
+ {
+ angImpulse = del;
+ }
+ m_accumulatedAngMotorImpulse = new_acc;
+ // apply clamped impulse
+ Vector3 motorImp = angImpulse * axisA;
+ A->apply_torque_impulse(motorImp);
+ B->apply_torque_impulse(-motorImp);
+ }
+ }
+} // SliderJointSW::solveConstraint()
+
+//-----------------------------------------------------------------------------
+
+//-----------------------------------------------------------------------------
+
+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++)
+ {
+ normalWorld = m_calculatedTransformA.basis.get_axis(i);
+ m_depth[i] = m_delta.dot(normalWorld);
+ }
+} // SliderJointSW::calculateTransforms()
+
+//-----------------------------------------------------------------------------
+
+void SliderJointSW::testLinLimits(void)
+{
+ m_solveLinLim = false;
+ m_linPos = m_depth[0];
+ 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)
+ {
+ m_depth[0] -= m_lowerLinLimit;
+ m_solveLinLim = true;
+ }
+ else
+ {
+ m_depth[0] = real_t(0.);
+ }
+ }
+ else
+ {
+ m_depth[0] = real_t(0.);
+ }
+} // SliderJointSW::testLinLimits()
+
+//-----------------------------------------------------------------------------
+
+
+void SliderJointSW::testAngLimits(void)
+{
+ m_angDepth = real_t(0.);
+ m_solveAngLim = false;
+ 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)
+ {
+ m_angDepth = rot - m_lowerAngLimit;
+ m_solveAngLim = true;
+ }
+ else if(rot > m_upperAngLimit)
+ {
+ m_angDepth = rot - m_upperAngLimit;
+ m_solveAngLim = true;
+ }
+ }
+} // SliderJointSW::testAngLimits()
+
+
+//-----------------------------------------------------------------------------
+
+
+
+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 );
+ return ancorInA;
+} // SliderJointSW::getAncorInA()
+
+//-----------------------------------------------------------------------------
+
+Vector3 SliderJointSW::getAncorInB(void)
+{
+ Vector3 ancorInB;
+ ancorInB = m_frameInB.origin;
+ return ancorInB;
+} // SliderJointSW::getAncorInB();
+
+void SliderJointSW::set_param(PhysicsServer::SliderJointParam p_param, float 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;
+
+ }
+
+}
+
+float SliderJointSW::get_param(PhysicsServer::SliderJointParam p_param) const {
+
+ 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;
+ case PhysicsServer::SLIDER_JOINT_LINEAR_LIMIT_RESTITUTION: return m_restitutionLimLin;
+ case PhysicsServer::SLIDER_JOINT_LINEAR_LIMIT_DAMPING: return m_dampingLimLin;
+ case PhysicsServer::SLIDER_JOINT_LINEAR_MOTION_SOFTNESS: return m_softnessDirLin;
+ case PhysicsServer::SLIDER_JOINT_LINEAR_MOTION_RESTITUTION: return m_restitutionDirLin;
+ case PhysicsServer::SLIDER_JOINT_LINEAR_MOTION_DAMPING: return m_dampingDirLin;
+ case PhysicsServer::SLIDER_JOINT_LINEAR_ORTHOGONAL_SOFTNESS: return m_softnessOrthoLin;
+ case PhysicsServer::SLIDER_JOINT_LINEAR_ORTHOGONAL_RESTITUTION: return m_restitutionOrthoLin;
+ case PhysicsServer::SLIDER_JOINT_LINEAR_ORTHOGONAL_DAMPING: return m_dampingOrthoLin;
+
+ case PhysicsServer::SLIDER_JOINT_ANGULAR_LIMIT_UPPER: return m_upperAngLimit;
+ case PhysicsServer::SLIDER_JOINT_ANGULAR_LIMIT_LOWER: return m_lowerAngLimit;
+ case PhysicsServer::SLIDER_JOINT_ANGULAR_LIMIT_SOFTNESS: return m_softnessLimAng;
+ case PhysicsServer::SLIDER_JOINT_ANGULAR_LIMIT_RESTITUTION: return m_restitutionLimAng;
+ case PhysicsServer::SLIDER_JOINT_ANGULAR_LIMIT_DAMPING: return m_dampingLimAng;
+ case PhysicsServer::SLIDER_JOINT_ANGULAR_MOTION_SOFTNESS: return m_softnessDirAng;
+ case PhysicsServer::SLIDER_JOINT_ANGULAR_MOTION_RESTITUTION: return m_restitutionDirAng;
+ case PhysicsServer::SLIDER_JOINT_ANGULAR_MOTION_DAMPING: return m_dampingDirAng;
+ 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
new file mode 100644
index 0000000000..517bb5e6bc
--- /dev/null
+++ b/servers/physics/joints/slider_joint_sw.h
@@ -0,0 +1,218 @@
+#ifndef SLIDER_JOINT_SW_H
+#define SLIDER_JOINT_SW_H
+
+#include "servers/physics/joints_sw.h"
+#include "servers/physics/joints/jacobian_entry_sw.h"
+
+
+/*
+Bullet Continuous Collision Detection and Physics Library
+Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/
+
+This software is provided 'as-is', without any express or implied warranty.
+In no event will the authors be held liable for any damages arising from the use of this software.
+Permission is granted to anyone to use this software for any purpose,
+including commercial applications, and to alter it and redistribute it freely,
+subject to the following restrictions:
+
+1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
+2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
+3. This notice may not be removed or altered from any source distribution.
+*/
+
+/*
+Added by Roman Ponomarev (rponom@gmail.com)
+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))
+
+//-----------------------------------------------------------------------------
+
+class SliderJointSW : public JointSW {
+protected:
+
+ union {
+ struct {
+ BodySW *A;
+ BodySW *B;
+ };
+
+ BodySW *_arr[2];
+ };
+
+ Transform m_frameInA;
+ Transform m_frameInB;
+
+ // linear limits
+ real_t m_lowerLinLimit;
+ real_t m_upperLinLimit;
+ // angular limits
+ real_t m_lowerAngLimit;
+ real_t m_upperAngLimit;
+ // softness, restitution and damping for different cases
+ // DirLin - moving inside linear limits
+ // LimLin - hitting linear limit
+ // DirAng - moving inside angular limits
+ // LimAng - hitting angular limit
+ // OrthoLin, OrthoAng - against constraint axis
+ real_t m_softnessDirLin;
+ real_t m_restitutionDirLin;
+ real_t m_dampingDirLin;
+ real_t m_softnessDirAng;
+ real_t m_restitutionDirAng;
+ real_t m_dampingDirAng;
+ real_t m_softnessLimLin;
+ real_t m_restitutionLimLin;
+ real_t m_dampingLimLin;
+ real_t m_softnessLimAng;
+ real_t m_restitutionLimAng;
+ real_t m_dampingLimAng;
+ real_t m_softnessOrthoLin;
+ real_t m_restitutionOrthoLin;
+ real_t m_dampingOrthoLin;
+ real_t m_softnessOrthoAng;
+ real_t m_restitutionOrthoAng;
+ real_t m_dampingOrthoAng;
+
+ // for interlal use
+ bool m_solveLinLim;
+ bool m_solveAngLim;
+
+ JacobianEntrySW m_jacLin[3];
+ real_t m_jacLinDiagABInv[3];
+
+ JacobianEntrySW m_jacAng[3];
+
+ real_t m_timeStep;
+ Transform m_calculatedTransformA;
+ Transform m_calculatedTransformB;
+
+ Vector3 m_sliderAxis;
+ Vector3 m_realPivotAInW;
+ Vector3 m_realPivotBInW;
+ Vector3 m_projPivotInW;
+ Vector3 m_delta;
+ Vector3 m_depth;
+ Vector3 m_relPosA;
+ Vector3 m_relPosB;
+
+ real_t m_linPos;
+
+ 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_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();
+ // 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; }
+
+ real_t getSoftnessDirLin() { return m_softnessDirLin; }
+ real_t getRestitutionDirLin() { return m_restitutionDirLin; }
+ real_t getDampingDirLin() { return m_dampingDirLin ; }
+ real_t getSoftnessDirAng() { return m_softnessDirAng; }
+ real_t getRestitutionDirAng() { return m_restitutionDirAng; }
+ real_t getDampingDirAng() { return m_dampingDirAng; }
+ real_t getSoftnessLimLin() { return m_softnessLimLin; }
+ real_t getRestitutionLimLin() { return m_restitutionLimLin; }
+ real_t getDampingLimLin() { return m_dampingLimLin; }
+ real_t getSoftnessLimAng() { return m_softnessLimAng; }
+ real_t getRestitutionLimAng() { return m_restitutionLimAng; }
+ real_t getDampingLimAng() { return m_dampingLimAng; }
+ real_t getSoftnessOrthoLin() { return m_softnessOrthoLin; }
+ real_t getRestitutionOrthoLin() { return m_restitutionOrthoLin; }
+ real_t getDampingOrthoLin() { return m_dampingOrthoLin; }
+ real_t getSoftnessOrthoAng() { return m_softnessOrthoAng; }
+ real_t getRestitutionOrthoAng() { return m_restitutionOrthoAng; }
+ real_t getDampingOrthoAng() { return m_dampingOrthoAng; }
+ void setSoftnessDirLin(real_t softnessDirLin) { m_softnessDirLin = softnessDirLin; }
+ void setRestitutionDirLin(real_t restitutionDirLin) { m_restitutionDirLin = restitutionDirLin; }
+ void setDampingDirLin(real_t dampingDirLin) { m_dampingDirLin = dampingDirLin; }
+ void setSoftnessDirAng(real_t softnessDirAng) { m_softnessDirAng = softnessDirAng; }
+ void setRestitutionDirAng(real_t restitutionDirAng) { m_restitutionDirAng = restitutionDirAng; }
+ void setDampingDirAng(real_t dampingDirAng) { m_dampingDirAng = dampingDirAng; }
+ void setSoftnessLimLin(real_t softnessLimLin) { m_softnessLimLin = softnessLimLin; }
+ void setRestitutionLimLin(real_t restitutionLimLin) { m_restitutionLimLin = restitutionLimLin; }
+ void setDampingLimLin(real_t dampingLimLin) { m_dampingLimLin = dampingLimLin; }
+ void setSoftnessLimAng(real_t softnessLimAng) { m_softnessLimAng = softnessLimAng; }
+ void setRestitutionLimAng(real_t restitutionLimAng) { m_restitutionLimAng = restitutionLimAng; }
+ void setDampingLimAng(real_t dampingLimAng) { m_dampingLimAng = dampingLimAng; }
+ void setSoftnessOrthoLin(real_t softnessOrthoLin) { m_softnessOrthoLin = softnessOrthoLin; }
+ void setRestitutionOrthoLin(real_t restitutionOrthoLin) { m_restitutionOrthoLin = restitutionOrthoLin; }
+ void setDampingOrthoLin(real_t dampingOrthoLin) { m_dampingOrthoLin = dampingOrthoLin; }
+ void setSoftnessOrthoAng(real_t softnessOrthoAng) { m_softnessOrthoAng = softnessOrthoAng; }
+ void setRestitutionOrthoAng(real_t restitutionOrthoAng) { m_restitutionOrthoAng = restitutionOrthoAng; }
+ void setDampingOrthoAng(real_t dampingOrthoAng) { m_dampingOrthoAng = dampingOrthoAng; }
+ void setPoweredLinMotor(bool onOff) { m_poweredLinMotor = onOff; }
+ bool getPoweredLinMotor() { return m_poweredLinMotor; }
+ void setTargetLinMotorVelocity(real_t targetLinMotorVelocity) { m_targetLinMotorVelocity = targetLinMotorVelocity; }
+ real_t getTargetLinMotorVelocity() { return m_targetLinMotorVelocity; }
+ void setMaxLinMotorForce(real_t maxLinMotorForce) { m_maxLinMotorForce = maxLinMotorForce; }
+ real_t getMaxLinMotorForce() { return m_maxLinMotorForce; }
+ void setPoweredAngMotor(bool onOff) { m_poweredAngMotor = onOff; }
+ bool getPoweredAngMotor() { return m_poweredAngMotor; }
+ void setTargetAngMotorVelocity(real_t targetAngMotorVelocity) { m_targetAngMotorVelocity = targetAngMotorVelocity; }
+ real_t getTargetAngMotorVelocity() { return m_targetAngMotorVelocity; }
+ void setMaxAngMotorForce(real_t maxAngMotorForce) { m_maxAngMotorForce = maxAngMotorForce; }
+ real_t getMaxAngMotorForce() { return m_maxAngMotorForce; }
+ real_t getLinearPos() { return m_linPos; }
+
+ // access for ODE solver
+ bool getSolveLinLimit() { return m_solveLinLim; }
+ real_t getLinDepth() { return m_depth[0]; }
+ 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);
+ // access for PE Solver
+ Vector3 getAncorInA(void);
+ Vector3 getAncorInB(void);
+
+ void set_param(PhysicsServer::SliderJointParam p_param, float p_value);
+ float get_param(PhysicsServer::SliderJointParam p_param) const;
+
+ bool setup(float p_step);
+ void solve(float p_step);
+
+ virtual PhysicsServer::JointType get_type() const { return PhysicsServer::JOINT_SLIDER; }
+
+};
+
+
+#endif // SLIDER_JOINT_SW_H
generated by cgit v1.2.3 (git 2.25.1) at 2024-11-29 15:23:06 +0000