aboutsummaryrefslogtreecommitdiff
path: root/servers/physics/joints/generic_6dof_joint_sw.cpp
diff options
context:
space:
mode:
Diffstat (limited to '')
-rw-r--r--servers/physics/joints/generic_6dof_joint_sw.cpp646
1 files changed, 275 insertions, 371 deletions
diff --git a/servers/physics/joints/generic_6dof_joint_sw.cpp b/servers/physics/joints/generic_6dof_joint_sw.cpp
index cd643a853..929bf0f0b 100644
--- a/servers/physics/joints/generic_6dof_joint_sw.cpp
+++ b/servers/physics/joints/generic_6dof_joint_sw.cpp
@@ -34,167 +34,128 @@ See corresponding header file for licensing info.
#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;
+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;
+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
+int G6DOFRotationalLimitMotorSW::testLimitValue(real_t test_value) {
+ if (m_loLimit > m_hiLimit) {
+ m_currentLimit = 0; //Free from violation
return 0;
}
- if (test_value < m_loLimit)
- {
- m_currentLimit = 1;//low limit violation
- m_currentLimitError = test_value - m_loLimit;
+ if (test_value < m_loLimit) {
+ m_currentLimit = 1; //low limit violation
+ m_currentLimitError = test_value - m_loLimit;
return 1;
- }
- else if (test_value> m_hiLimit)
- {
- m_currentLimit = 2;//High limit violation
+ } else if (test_value > m_hiLimit) {
+ m_currentLimit = 2; //High limit violation
m_currentLimitError = test_value - m_hiLimit;
return 2;
};
- m_currentLimit = 0;//Free from violation
+ m_currentLimit = 0; //Free from violation
return 0;
-
}
-
real_t G6DOFRotationalLimitMotorSW::solveAngularLimits(
- real_t timeStep,Vector3& axis,real_t jacDiagABInv,
- BodySW * body0, BodySW * body1)
-{
- if (needApplyTorques()==false) return 0.0f;
+ real_t timeStep, Vector3 &axis, real_t jacDiagABInv,
+ BodySW *body0, BodySW *body1) {
+ if (needApplyTorques() == false) return 0.0f;
- real_t target_velocity = m_targetVelocity;
- real_t maxMotorForce = m_maxMotorForce;
+ real_t target_velocity = m_targetVelocity;
+ real_t maxMotorForce = m_maxMotorForce;
//current error correction
- if (m_currentLimit!=0)
- {
- target_velocity = -m_ERP*m_currentLimitError/(timeStep);
- maxMotorForce = m_maxLimitForce;
- }
-
- maxMotorForce *= timeStep;
-
- // current velocity difference
- Vector3 vel_diff = body0->get_angular_velocity();
- if (body1)
- {
- vel_diff -= body1->get_angular_velocity();
- }
+ if (m_currentLimit != 0) {
+ target_velocity = -m_ERP * m_currentLimitError / (timeStep);
+ maxMotorForce = m_maxLimitForce;
+ }
+ maxMotorForce *= timeStep;
+ // current velocity difference
+ Vector3 vel_diff = body0->get_angular_velocity();
+ if (body1) {
+ vel_diff -= body1->get_angular_velocity();
+ }
- real_t rel_vel = axis.dot(vel_diff);
+ real_t rel_vel = axis.dot(vel_diff);
// correction velocity
- real_t motor_relvel = m_limitSoftness*(target_velocity - m_damping*rel_vel);
-
-
- if ( motor_relvel < CMP_EPSILON && motor_relvel > -CMP_EPSILON )
- {
- return 0.0f;//no need for applying force
- }
+ real_t motor_relvel = m_limitSoftness * (target_velocity - m_damping * rel_vel);
+ if (motor_relvel < CMP_EPSILON && motor_relvel > -CMP_EPSILON) {
+ return 0.0f; //no need for applying force
+ }
// correction impulse
- real_t unclippedMotorImpulse = (1+m_bounce)*motor_relvel*jacDiagABInv;
+ real_t unclippedMotorImpulse = (1 + m_bounce) * motor_relvel * jacDiagABInv;
// clip correction impulse
- real_t clippedMotorImpulse;
-
- ///@todo: should clip against accumulated impulse
- if (unclippedMotorImpulse>0.0f)
- {
- clippedMotorImpulse = unclippedMotorImpulse > maxMotorForce? maxMotorForce: unclippedMotorImpulse;
- }
- else
- {
- clippedMotorImpulse = unclippedMotorImpulse < -maxMotorForce ? -maxMotorForce: unclippedMotorImpulse;
- }
+ real_t clippedMotorImpulse;
+ ///@todo: should clip against accumulated impulse
+ if (unclippedMotorImpulse > 0.0f) {
+ clippedMotorImpulse = unclippedMotorImpulse > maxMotorForce ? maxMotorForce : unclippedMotorImpulse;
+ } else {
+ clippedMotorImpulse = unclippedMotorImpulse < -maxMotorForce ? -maxMotorForce : unclippedMotorImpulse;
+ }
// sort with accumulated impulses
- real_t lo = real_t(-1e30);
- real_t hi = real_t(1e30);
-
- real_t oldaccumImpulse = m_accumulatedImpulse;
- real_t sum = oldaccumImpulse + clippedMotorImpulse;
- m_accumulatedImpulse = sum > hi ? real_t(0.) : sum < lo ? real_t(0.) : sum;
-
- clippedMotorImpulse = m_accumulatedImpulse - oldaccumImpulse;
-
+ real_t lo = real_t(-1e30);
+ real_t hi = real_t(1e30);
+ real_t oldaccumImpulse = m_accumulatedImpulse;
+ real_t sum = oldaccumImpulse + clippedMotorImpulse;
+ m_accumulatedImpulse = sum > hi ? real_t(0.) : sum < lo ? real_t(0.) : sum;
- Vector3 motorImp = clippedMotorImpulse * axis;
+ clippedMotorImpulse = m_accumulatedImpulse - oldaccumImpulse;
+ Vector3 motorImp = clippedMotorImpulse * axis;
- body0->apply_torque_impulse(motorImp);
- if (body1) body1->apply_torque_impulse(-motorImp);
-
- return clippedMotorImpulse;
-
+ body0->apply_torque_impulse(motorImp);
+ if (body1) body1->apply_torque_impulse(-motorImp);
+ return clippedMotorImpulse;
}
//////////////////////////// End G6DOFRotationalLimitMotorSW ////////////////////////////////////
@@ -202,120 +163,96 @@ real_t G6DOFRotationalLimitMotorSW::solveAngularLimits(
//////////////////////////// G6DOFTranslationalLimitMotorSW ////////////////////////////////////
real_t G6DOFTranslationalLimitMotorSW::solveLinearAxis(
real_t timeStep,
- real_t jacDiagABInv,
- BodySW* body1,const Vector3 &pointInA,
- BodySW* body2,const Vector3 &pointInB,
- int limit_index,
- const Vector3 & axis_normal_on_a,
- const Vector3 & anchorPos)
-{
-
-///find relative velocity
-// Vector3 rel_pos1 = pointInA - body1->get_transform().origin;
-// Vector3 rel_pos2 = pointInB - body2->get_transform().origin;
- Vector3 rel_pos1 = anchorPos - body1->get_transform().origin;
- Vector3 rel_pos2 = anchorPos - body2->get_transform().origin;
-
- 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 jacDiagABInv,
+ BodySW *body1, const Vector3 &pointInA,
+ BodySW *body2, const Vector3 &pointInB,
+ int limit_index,
+ const Vector3 &axis_normal_on_a,
+ const Vector3 &anchorPos) {
- real_t rel_vel = axis_normal_on_a.dot(vel);
+ ///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
+ /// apply displacement correction
-//positional error (zeroth order error)
- real_t depth = -(pointInA - pointInB).dot(axis_normal_on_a);
- real_t lo = real_t(-1e30);
- real_t hi = real_t(1e30);
+ //positional error (zeroth order error)
+ real_t depth = -(pointInA - pointInB).dot(axis_normal_on_a);
+ real_t lo = real_t(-1e30);
+ real_t hi = real_t(1e30);
- real_t minLimit = m_lowerLimit[limit_index];
- real_t maxLimit = m_upperLimit[limit_index];
+ real_t minLimit = m_lowerLimit[limit_index];
+ real_t maxLimit = m_upperLimit[limit_index];
- //handle the limits
- if (minLimit < maxLimit)
- {
- {
- if (depth > maxLimit)
- {
- depth -= maxLimit;
- lo = real_t(0.);
-
- }
- else
- {
- if (depth < minLimit)
- {
- depth -= minLimit;
- hi = real_t(0.);
- }
- else
+ //handle the limits
+ if (minLimit < maxLimit) {
{
- return 0.0f;
+ if (depth > maxLimit) {
+ depth -= maxLimit;
+ lo = real_t(0.);
+
+ } else {
+ if (depth < minLimit) {
+ depth -= minLimit;
+ hi = real_t(0.);
+ } else {
+ return 0.0f;
+ }
+ }
}
- }
}
- }
- real_t normalImpulse= m_limitSoftness[limit_index]*(m_restitution[limit_index]*depth/timeStep - m_damping[limit_index]*rel_vel) * jacDiagABInv;
+ real_t normalImpulse = m_limitSoftness[limit_index] * (m_restitution[limit_index] * depth / timeStep - m_damping[limit_index] * rel_vel) * jacDiagABInv;
+ real_t oldNormalImpulse = m_accumulatedImpulse[limit_index];
+ real_t sum = oldNormalImpulse + normalImpulse;
+ m_accumulatedImpulse[limit_index] = sum > hi ? real_t(0.) : sum < lo ? real_t(0.) : sum;
+ normalImpulse = m_accumulatedImpulse[limit_index] - oldNormalImpulse;
-
-
- real_t oldNormalImpulse = m_accumulatedImpulse[limit_index];
- real_t sum = oldNormalImpulse + normalImpulse;
- m_accumulatedImpulse[limit_index] = sum > hi ? real_t(0.) : sum < lo ? real_t(0.) : sum;
- normalImpulse = m_accumulatedImpulse[limit_index] - oldNormalImpulse;
-
- Vector3 impulse_vector = axis_normal_on_a * normalImpulse;
- body1->apply_impulse( rel_pos1, impulse_vector);
- body2->apply_impulse( rel_pos2, -impulse_vector);
- return normalImpulse;
+ Vector3 impulse_vector = axis_normal_on_a * normalImpulse;
+ body1->apply_impulse(rel_pos1, impulse_vector);
+ body2->apply_impulse(rel_pos2, -impulse_vector);
+ return normalImpulse;
}
//////////////////////////// G6DOFTranslationalLimitMotorSW ////////////////////////////////////
-
-Generic6DOFJointSW::Generic6DOFJointSW(BodySW* rbA, BodySW* rbB, const Transform& frameInA, const Transform& frameInB, bool useLinearReferenceFrameA)
- : JointSW(_arr,2)
- , m_frameInA(frameInA)
- , m_frameInB(frameInB),
- m_useLinearReferenceFrameA(useLinearReferenceFrameA)
-{
- A=rbA;
- B=rbB;
- A->add_constraint(this,0);
- B->add_constraint(this,1);
+Generic6DOFJointSW::Generic6DOFJointSW(BodySW *rbA, BodySW *rbB, const Transform &frameInA, const Transform &frameInB, bool useLinearReferenceFrameA)
+ : JointSW(_arr, 2), m_frameInA(frameInA), m_frameInB(frameInB),
+ m_useLinearReferenceFrameA(useLinearReferenceFrameA) {
+ A = rbA;
+ B = rbB;
+ A->add_constraint(this, 0);
+ B->add_constraint(this, 1);
}
+void Generic6DOFJointSW::calculateAngleInfo() {
+ Matrix3 relative_frame = m_calculatedTransformA.basis.inverse() * m_calculatedTransformB.basis;
-
-
-
-void Generic6DOFJointSW::calculateAngleInfo()
-{
- Matrix3 relative_frame = m_calculatedTransformA.basis.inverse()*m_calculatedTransformB.basis;
-
- matrixToEulerXYZ(relative_frame,m_calculatedAxisAngleDiff);
-
-
+ 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.
+ // 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);
@@ -324,291 +261,260 @@ void Generic6DOFJointSW::calculateAngleInfo()
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);
-// }
-
+ // 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;
+void Generic6DOFJointSW::calculateTransforms() {
+ m_calculatedTransformA = A->get_transform() * m_frameInA;
+ m_calculatedTransformB = B->get_transform() * m_frameInB;
- calculateAngleInfo();
+ calculateAngleInfo();
}
-
void Generic6DOFJointSW::buildLinearJacobian(
- JacobianEntrySW & jacLinear,const Vector3 & normalWorld,
- const Vector3 & pivotAInW,const Vector3 & pivotBInW)
-{
- memnew_placement(&jacLinear, JacobianEntrySW(
- A->get_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()));
-
+ 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()));
-
+ 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];
+bool Generic6DOFJointSW::testAngularLimitMotor(int axis_index) {
+ real_t angle = m_calculatedAxisAngleDiff[axis_index];
- //test limits
- m_angularLimits[axis_index].testLimitValue(angle);
- return m_angularLimits[axis_index].needApplyTorques();
+ //test limits
+ m_angularLimits[axis_index].testLimitValue(angle);
+ return m_angularLimits[axis_index].needApplyTorques();
}
bool Generic6DOFJointSW::setup(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();
+ m_linearLimits.m_accumulatedImpulse = Vector3(real_t(0.), real_t(0.), real_t(0.));
+ int i;
+ for (i = 0; i < 3; i++) {
+ m_angularLimits[i].m_accumulatedImpulse = real_t(0.);
+ }
+ //calculates transform
+ calculateTransforms();
-// const Vector3& pivotAInW = m_calculatedTransformA.origin;
-// const Vector3& pivotBInW = m_calculatedTransformB.origin;
+ // const Vector3& pivotAInW = m_calculatedTransformA.origin;
+ // const Vector3& pivotBInW = m_calculatedTransformB.origin;
calcAnchorPos();
Vector3 pivotAInW = m_AnchorPos;
Vector3 pivotBInW = m_AnchorPos;
-// not used here
-// Vector3 rel_pos1 = pivotAInW - A->get_transform().origin;
-// Vector3 rel_pos2 = pivotBInW - B->get_transform().origin;
+ // not used here
+ // Vector3 rel_pos1 = pivotAInW - A->get_transform().origin;
+ // Vector3 rel_pos2 = pivotBInW - B->get_transform().origin;
- Vector3 normalWorld;
- //linear part
- for (i=0;i<3;i++)
- {
- if (m_linearLimits.enable_limit[i] && m_linearLimits.isLimited(i))
- {
+ Vector3 normalWorld;
+ //linear part
+ for (i = 0; i < 3; i++) {
+ if (m_linearLimits.enable_limit[i] && m_linearLimits.isLimited(i)) {
if (m_useLinearReferenceFrameA)
- normalWorld = m_calculatedTransformA.basis.get_axis(i);
+ normalWorld = m_calculatedTransformA.basis.get_axis(i);
else
- normalWorld = m_calculatedTransformB.basis.get_axis(i);
-
- buildLinearJacobian(
- m_jacLinear[i],normalWorld ,
- pivotAInW,pivotBInW);
+ normalWorld = m_calculatedTransformB.basis.get_axis(i);
+ buildLinearJacobian(
+ m_jacLinear[i], normalWorld,
+ pivotAInW, pivotBInW);
+ }
}
- }
- // angular part
- for (i=0;i<3;i++)
- {
- //calculates error angle
- if (m_angularLimits[i].m_enableLimit && testAngularLimitMotor(i))
- {
- normalWorld = this->getAxis(i);
- // Create angular atom
- buildAngularJacobian(m_jacAng[i],normalWorld);
+ // angular part
+ for (i = 0; i < 3; i++) {
+ //calculates error angle
+ if (m_angularLimits[i].m_enableLimit && testAngularLimitMotor(i)) {
+ normalWorld = this->getAxis(i);
+ // Create angular atom
+ buildAngularJacobian(m_jacAng[i], normalWorld);
+ }
}
- }
return true;
}
+void Generic6DOFJointSW::solve(real_t timeStep) {
+ m_timeStep = timeStep;
-void Generic6DOFJointSW::solve(real_t timeStep)
-{
- m_timeStep = timeStep;
-
- //calculateTransforms();
+ //calculateTransforms();
- int i;
+ int i;
- // linear
+ // linear
- Vector3 pointInA = m_calculatedTransformA.origin;
+ Vector3 pointInA = m_calculatedTransformA.origin;
Vector3 pointInB = m_calculatedTransformB.origin;
real_t jacDiagABInv;
Vector3 linear_axis;
- for (i=0;i<3;i++)
- {
- if (m_linearLimits.enable_limit[i] && m_linearLimits.isLimited(i))
- {
- jacDiagABInv = real_t(1.) / m_jacLinear[i].getDiagonal();
+ for (i = 0; i < 3; i++) {
+ if (m_linearLimits.enable_limit[i] && m_linearLimits.isLimited(i)) {
+ jacDiagABInv = real_t(1.) / m_jacLinear[i].getDiagonal();
if (m_useLinearReferenceFrameA)
- linear_axis = m_calculatedTransformA.basis.get_axis(i);
+ linear_axis = m_calculatedTransformA.basis.get_axis(i);
else
- linear_axis = m_calculatedTransformB.basis.get_axis(i);
-
- m_linearLimits.solveLinearAxis(
- m_timeStep,
- jacDiagABInv,
- A,pointInA,
- B,pointInB,
- i,linear_axis, m_AnchorPos);
+ linear_axis = m_calculatedTransformB.basis.get_axis(i);
+ m_linearLimits.solveLinearAxis(
+ m_timeStep,
+ jacDiagABInv,
+ A, pointInA,
+ B, pointInB,
+ i, linear_axis, m_AnchorPos);
+ }
}
- }
- // angular
- Vector3 angular_axis;
- real_t angularJacDiagABInv;
- for (i=0;i<3;i++)
- {
- if (m_angularLimits[i].m_enableLimit && m_angularLimits[i].needApplyTorques())
- {
+ // angular
+ Vector3 angular_axis;
+ real_t angularJacDiagABInv;
+ for (i = 0; i < 3; i++) {
+ if (m_angularLimits[i].m_enableLimit && m_angularLimits[i].needApplyTorques()) {
// get axis
angular_axis = getAxis(i);
angularJacDiagABInv = real_t(1.) / m_jacAng[i].getDiagonal();
- m_angularLimits[i].solveAngularLimits(m_timeStep,angular_axis,angularJacDiagABInv, A,B);
+ m_angularLimits[i].solveAngularLimits(m_timeStep, angular_axis, angularJacDiagABInv, A, B);
+ }
}
- }
}
-void Generic6DOFJointSW::updateRHS(real_t timeStep)
-{
- (void)timeStep;
-
+void Generic6DOFJointSW::updateRHS(real_t timeStep) {
+ (void)timeStep;
}
-Vector3 Generic6DOFJointSW::getAxis(int axis_index) const
-{
- return m_calculatedAxis[axis_index];
+Vector3 Generic6DOFJointSW::getAxis(int axis_index) const {
+ return m_calculatedAxis[axis_index];
}
-real_t Generic6DOFJointSW::getAngle(int axis_index) const
-{
- return m_calculatedAxisAngleDiff[axis_index];
+real_t Generic6DOFJointSW::getAngle(int axis_index) const {
+ return m_calculatedAxisAngleDiff[axis_index];
}
-void Generic6DOFJointSW::calcAnchorPos(void)
-{
+void Generic6DOFJointSW::calcAnchorPos(void) {
real_t imA = A->get_inv_mass();
real_t imB = B->get_inv_mass();
real_t weight;
- if(imB == real_t(0.0))
- {
+ if (imB == real_t(0.0)) {
weight = real_t(1.0);
- }
- else
- {
+ } else {
weight = imA / (imA + imB);
}
- const Vector3& pA = m_calculatedTransformA.origin;
- const Vector3& pB = m_calculatedTransformB.origin;
+ const Vector3 &pA = m_calculatedTransformA.origin;
+ const Vector3 &pB = m_calculatedTransformB.origin;
m_AnchorPos = pA * weight + pB * (real_t(1.0) - weight);
return;
} // Generic6DOFJointSW::calcAnchorPos()
+void Generic6DOFJointSW::set_param(Vector3::Axis p_axis, PhysicsServer::G6DOFJointAxisParam p_param, float p_value) {
-void Generic6DOFJointSW::set_param(Vector3::Axis p_axis,PhysicsServer::G6DOFJointAxisParam p_param, float p_value) {
-
- ERR_FAIL_INDEX(p_axis,3);
- switch(p_param) {
+ ERR_FAIL_INDEX(p_axis, 3);
+ switch (p_param) {
case PhysicsServer::G6DOF_JOINT_LINEAR_LOWER_LIMIT: {
- m_linearLimits.m_lowerLimit[p_axis]=p_value;
+ m_linearLimits.m_lowerLimit[p_axis] = p_value;
} break;
case PhysicsServer::G6DOF_JOINT_LINEAR_UPPER_LIMIT: {
- m_linearLimits.m_upperLimit[p_axis]=p_value;
+ m_linearLimits.m_upperLimit[p_axis] = p_value;
} break;
case PhysicsServer::G6DOF_JOINT_LINEAR_LIMIT_SOFTNESS: {
- m_linearLimits.m_limitSoftness[p_axis]=p_value;
+ m_linearLimits.m_limitSoftness[p_axis] = p_value;
} break;
case PhysicsServer::G6DOF_JOINT_LINEAR_RESTITUTION: {
- m_linearLimits.m_restitution[p_axis]=p_value;
+ m_linearLimits.m_restitution[p_axis] = p_value;
} break;
case PhysicsServer::G6DOF_JOINT_LINEAR_DAMPING: {
- m_linearLimits.m_damping[p_axis]=p_value;
+ m_linearLimits.m_damping[p_axis] = p_value;
} break;
case PhysicsServer::G6DOF_JOINT_ANGULAR_LOWER_LIMIT: {
- m_angularLimits[p_axis].m_loLimit=p_value;
+ m_angularLimits[p_axis].m_loLimit = p_value;
} break;
case PhysicsServer::G6DOF_JOINT_ANGULAR_UPPER_LIMIT: {
- m_angularLimits[p_axis].m_hiLimit=p_value;
+ m_angularLimits[p_axis].m_hiLimit = p_value;
} break;
case PhysicsServer::G6DOF_JOINT_ANGULAR_LIMIT_SOFTNESS: {
- m_angularLimits[p_axis].m_limitSoftness=p_value;
+ m_angularLimits[p_axis].m_limitSoftness = p_value;
} break;
case PhysicsServer::G6DOF_JOINT_ANGULAR_DAMPING: {
- m_angularLimits[p_axis].m_damping=p_value;
+ m_angularLimits[p_axis].m_damping = p_value;
} break;
case PhysicsServer::G6DOF_JOINT_ANGULAR_RESTITUTION: {
- m_angularLimits[p_axis].m_bounce=p_value;
+ m_angularLimits[p_axis].m_bounce = p_value;
} break;
case PhysicsServer::G6DOF_JOINT_ANGULAR_FORCE_LIMIT: {
- m_angularLimits[p_axis].m_maxLimitForce=p_value;
+ m_angularLimits[p_axis].m_maxLimitForce = p_value;
} break;
case PhysicsServer::G6DOF_JOINT_ANGULAR_ERP: {
- m_angularLimits[p_axis].m_ERP=p_value;
+ m_angularLimits[p_axis].m_ERP = p_value;
} break;
case PhysicsServer::G6DOF_JOINT_ANGULAR_MOTOR_TARGET_VELOCITY: {
- m_angularLimits[p_axis].m_targetVelocity=p_value;
+ m_angularLimits[p_axis].m_targetVelocity = p_value;
} break;
case PhysicsServer::G6DOF_JOINT_ANGULAR_MOTOR_FORCE_LIMIT: {
- m_angularLimits[p_axis].m_maxLimitForce=p_value;
+ m_angularLimits[p_axis].m_maxLimitForce = p_value;
} break;
}
}
-float Generic6DOFJointSW::get_param(Vector3::Axis p_axis,PhysicsServer::G6DOFJointAxisParam p_param) const{
- ERR_FAIL_INDEX_V(p_axis,3,0);
- switch(p_param) {
+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];
@@ -682,31 +588,29 @@ float Generic6DOFJointSW::get_param(Vector3::Axis p_axis,PhysicsServer::G6DOFJoi
return 0;
}
-void Generic6DOFJointSW::set_flag(Vector3::Axis p_axis,PhysicsServer::G6DOFJointAxisFlag p_flag, bool p_value){
+void Generic6DOFJointSW::set_flag(Vector3::Axis p_axis, PhysicsServer::G6DOFJointAxisFlag p_flag, bool p_value) {
- ERR_FAIL_INDEX(p_axis,3);
+ ERR_FAIL_INDEX(p_axis, 3);
- switch(p_flag) {
+ switch (p_flag) {
case PhysicsServer::G6DOF_JOINT_FLAG_ENABLE_LINEAR_LIMIT: {
- m_linearLimits.enable_limit[p_axis]=p_value;
+ m_linearLimits.enable_limit[p_axis] = p_value;
} break;
case PhysicsServer::G6DOF_JOINT_FLAG_ENABLE_ANGULAR_LIMIT: {
- m_angularLimits[p_axis].m_enableLimit=p_value;
+ m_angularLimits[p_axis].m_enableLimit = p_value;
} break;
case PhysicsServer::G6DOF_JOINT_FLAG_ENABLE_MOTOR: {
- m_angularLimits[p_axis].m_enableMotor=p_value;
+ m_angularLimits[p_axis].m_enableMotor = p_value;
} break;
}
-
-
}
-bool Generic6DOFJointSW::get_flag(Vector3::Axis p_axis,PhysicsServer::G6DOFJointAxisFlag p_flag) const{
+bool Generic6DOFJointSW::get_flag(Vector3::Axis p_axis, PhysicsServer::G6DOFJointAxisFlag p_flag) const {
- ERR_FAIL_INDEX_V(p_axis,3,0);
- switch(p_flag) {
+ ERR_FAIL_INDEX_V(p_axis, 3, 0);
+ switch (p_flag) {
case PhysicsServer::G6DOF_JOINT_FLAG_ENABLE_LINEAR_LIMIT: {
return m_linearLimits.enable_limit[p_axis];
generated by cgit v1.2.3-70-g09d2 (git 2.52.0) at 2026-01-19 09:45:24 +0000