summaryrefslogtreecommitdiff
path: root/servers/physics/joints/generic_6dof_joint_sw.cpp
diff options
context:
space:
mode:
Diffstat (limited to 'servers/physics/joints/generic_6dof_joint_sw.cpp')
-rw-r--r--servers/physics/joints/generic_6dof_joint_sw.cpp563
1 files changed, 239 insertions, 324 deletions
diff --git a/servers/physics/joints/generic_6dof_joint_sw.cpp b/servers/physics/joints/generic_6dof_joint_sw.cpp
index bd07f3122a..1e07bc73fb 100644
--- a/servers/physics/joints/generic_6dof_joint_sw.cpp
+++ b/servers/physics/joints/generic_6dof_joint_sw.cpp
@@ -34,118 +34,90 @@ See corresponding header file for licensing info.
#include "generic_6dof_joint_sw.h"
-
-
#define GENERIC_D6_DISABLE_WARMSTARTING 1
-
//////////////////////////// G6DOFRotationalLimitMotorSW ////////////////////////////////////
-
-int G6DOFRotationalLimitMotorSW::testLimitValue(real_t test_value)
-{
- if(m_loLimit>m_hiLimit)
- {
- m_currentLimit = 0;//Free from violation
+int G6DOFRotationalLimitMotorSW::testLimitValue(real_t test_value) {
+ if (m_loLimit > m_hiLimit) {
+ m_currentLimit = 0; //Free from violation
return 0;
}
- if (test_value < m_loLimit)
- {
- m_currentLimit = 1;//low limit violation
- m_currentLimitError = test_value - m_loLimit;
+ if (test_value < m_loLimit) {
+ m_currentLimit = 1; //low limit violation
+ m_currentLimitError = test_value - m_loLimit;
return 1;
- }
- else if (test_value> m_hiLimit)
- {
- m_currentLimit = 2;//High limit violation
+ } else if (test_value > m_hiLimit) {
+ m_currentLimit = 2; //High limit violation
m_currentLimitError = test_value - m_hiLimit;
return 2;
};
- m_currentLimit = 0;//Free from violation
+ m_currentLimit = 0; //Free from violation
return 0;
-
}
-
real_t G6DOFRotationalLimitMotorSW::solveAngularLimits(
- real_t timeStep,Vector3& axis,real_t jacDiagABInv,
- BodySW * body0, BodySW * body1)
-{
- if (needApplyTorques()==false) return 0.0f;
+ real_t timeStep, Vector3 &axis, real_t jacDiagABInv,
+ BodySW *body0, BodySW *body1) {
+ if (needApplyTorques() == false) return 0.0f;
- real_t target_velocity = m_targetVelocity;
- real_t maxMotorForce = m_maxMotorForce;
+ real_t target_velocity = m_targetVelocity;
+ real_t maxMotorForce = m_maxMotorForce;
//current error correction
- if (m_currentLimit!=0)
- {
- target_velocity = -m_ERP*m_currentLimitError/(timeStep);
- maxMotorForce = m_maxLimitForce;
- }
-
- maxMotorForce *= timeStep;
-
- // current velocity difference
- Vector3 vel_diff = body0->get_angular_velocity();
- if (body1)
- {
- vel_diff -= body1->get_angular_velocity();
- }
+ if (m_currentLimit != 0) {
+ target_velocity = -m_ERP * m_currentLimitError / (timeStep);
+ maxMotorForce = m_maxLimitForce;
+ }
+ maxMotorForce *= timeStep;
+ // current velocity difference
+ Vector3 vel_diff = body0->get_angular_velocity();
+ if (body1) {
+ vel_diff -= body1->get_angular_velocity();
+ }
- real_t rel_vel = axis.dot(vel_diff);
+ real_t rel_vel = axis.dot(vel_diff);
// correction velocity
- real_t motor_relvel = m_limitSoftness*(target_velocity - m_damping*rel_vel);
-
-
- if ( motor_relvel < CMP_EPSILON && motor_relvel > -CMP_EPSILON )
- {
- return 0.0f;//no need for applying force
- }
+ real_t motor_relvel = m_limitSoftness * (target_velocity - m_damping * rel_vel);
+ if (motor_relvel < CMP_EPSILON && motor_relvel > -CMP_EPSILON) {
+ return 0.0f; //no need for applying force
+ }
// correction impulse
- real_t unclippedMotorImpulse = (1+m_bounce)*motor_relvel*jacDiagABInv;
+ real_t unclippedMotorImpulse = (1 + m_bounce) * motor_relvel * jacDiagABInv;
// clip correction impulse
- real_t clippedMotorImpulse;
-
- ///@todo: should clip against accumulated impulse
- if (unclippedMotorImpulse>0.0f)
- {
- clippedMotorImpulse = unclippedMotorImpulse > maxMotorForce? maxMotorForce: unclippedMotorImpulse;
- }
- else
- {
- clippedMotorImpulse = unclippedMotorImpulse < -maxMotorForce ? -maxMotorForce: unclippedMotorImpulse;
- }
+ real_t clippedMotorImpulse;
+ ///@todo: should clip against accumulated impulse
+ if (unclippedMotorImpulse > 0.0f) {
+ clippedMotorImpulse = unclippedMotorImpulse > maxMotorForce ? maxMotorForce : unclippedMotorImpulse;
+ } else {
+ clippedMotorImpulse = unclippedMotorImpulse < -maxMotorForce ? -maxMotorForce : unclippedMotorImpulse;
+ }
// sort with accumulated impulses
- real_t lo = real_t(-1e30);
- real_t hi = real_t(1e30);
-
- real_t oldaccumImpulse = m_accumulatedImpulse;
- real_t sum = oldaccumImpulse + clippedMotorImpulse;
- m_accumulatedImpulse = sum > hi ? real_t(0.) : sum < lo ? real_t(0.) : sum;
-
- clippedMotorImpulse = m_accumulatedImpulse - oldaccumImpulse;
+ real_t lo = real_t(-1e30);
+ real_t hi = real_t(1e30);
+ real_t oldaccumImpulse = m_accumulatedImpulse;
+ real_t sum = oldaccumImpulse + clippedMotorImpulse;
+ m_accumulatedImpulse = sum > hi ? real_t(0.) : sum < lo ? real_t(0.) : sum;
+ clippedMotorImpulse = m_accumulatedImpulse - oldaccumImpulse;
- Vector3 motorImp = clippedMotorImpulse * axis;
-
-
- body0->apply_torque_impulse(motorImp);
- if (body1) body1->apply_torque_impulse(-motorImp);
-
- return clippedMotorImpulse;
+ Vector3 motorImp = clippedMotorImpulse * axis;
+ body0->apply_torque_impulse(motorImp);
+ if (body1) body1->apply_torque_impulse(-motorImp);
+ return clippedMotorImpulse;
}
//////////////////////////// End G6DOFRotationalLimitMotorSW ////////////////////////////////////
@@ -153,120 +125,96 @@ real_t G6DOFRotationalLimitMotorSW::solveAngularLimits(
//////////////////////////// G6DOFTranslationalLimitMotorSW ////////////////////////////////////
real_t G6DOFTranslationalLimitMotorSW::solveLinearAxis(
real_t timeStep,
- real_t jacDiagABInv,
- BodySW* body1,const Vector3 &pointInA,
- BodySW* body2,const Vector3 &pointInB,
- int limit_index,
- const Vector3 & axis_normal_on_a,
- const Vector3 & anchorPos)
-{
-
-///find relative velocity
-// Vector3 rel_pos1 = pointInA - body1->get_transform().origin;
-// Vector3 rel_pos2 = pointInB - body2->get_transform().origin;
- Vector3 rel_pos1 = anchorPos - body1->get_transform().origin;
- Vector3 rel_pos2 = anchorPos - body2->get_transform().origin;
+ real_t jacDiagABInv,
+ BodySW *body1, const Vector3 &pointInA,
+ BodySW *body2, const Vector3 &pointInB,
+ int limit_index,
+ const Vector3 &axis_normal_on_a,
+ const Vector3 &anchorPos) {
- Vector3 vel1 = body1->get_velocity_in_local_point(rel_pos1);
- Vector3 vel2 = body2->get_velocity_in_local_point(rel_pos2);
- Vector3 vel = vel1 - vel2;
+ ///find relative velocity
+ // Vector3 rel_pos1 = pointInA - body1->get_transform().origin;
+ // Vector3 rel_pos2 = pointInB - body2->get_transform().origin;
+ Vector3 rel_pos1 = anchorPos - body1->get_transform().origin;
+ Vector3 rel_pos2 = anchorPos - body2->get_transform().origin;
- real_t rel_vel = axis_normal_on_a.dot(vel);
+ Vector3 vel1 = body1->get_velocity_in_local_point(rel_pos1);
+ Vector3 vel2 = body2->get_velocity_in_local_point(rel_pos2);
+ Vector3 vel = vel1 - vel2;
+ real_t rel_vel = axis_normal_on_a.dot(vel);
+ /// apply displacement correction
-/// apply displacement correction
+ //positional error (zeroth order error)
+ real_t depth = -(pointInA - pointInB).dot(axis_normal_on_a);
+ real_t lo = real_t(-1e30);
+ real_t hi = real_t(1e30);
-//positional error (zeroth order error)
- real_t depth = -(pointInA - pointInB).dot(axis_normal_on_a);
- real_t lo = real_t(-1e30);
- real_t hi = real_t(1e30);
+ real_t minLimit = m_lowerLimit[limit_index];
+ real_t maxLimit = m_upperLimit[limit_index];
- real_t minLimit = m_lowerLimit[limit_index];
- real_t maxLimit = m_upperLimit[limit_index];
-
- //handle the limits
- if (minLimit < maxLimit)
- {
- {
- if (depth > maxLimit)
- {
- depth -= maxLimit;
- lo = real_t(0.);
-
- }
- else
- {
- if (depth < minLimit)
- {
- depth -= minLimit;
- hi = real_t(0.);
- }
- else
+ //handle the limits
+ if (minLimit < maxLimit) {
{
- return 0.0f;
+ if (depth > maxLimit) {
+ depth -= maxLimit;
+ lo = real_t(0.);
+
+ } else {
+ if (depth < minLimit) {
+ depth -= minLimit;
+ hi = real_t(0.);
+ } else {
+ return 0.0f;
+ }
+ }
}
- }
}
- }
- real_t normalImpulse= m_limitSoftness[limit_index]*(m_restitution[limit_index]*depth/timeStep - m_damping[limit_index]*rel_vel) * jacDiagABInv;
+ real_t normalImpulse = m_limitSoftness[limit_index] * (m_restitution[limit_index] * depth / timeStep - m_damping[limit_index] * rel_vel) * jacDiagABInv;
+ real_t oldNormalImpulse = m_accumulatedImpulse[limit_index];
+ real_t sum = oldNormalImpulse + normalImpulse;
+ m_accumulatedImpulse[limit_index] = sum > hi ? real_t(0.) : sum < lo ? real_t(0.) : sum;
+ normalImpulse = m_accumulatedImpulse[limit_index] - oldNormalImpulse;
-
-
- real_t oldNormalImpulse = m_accumulatedImpulse[limit_index];
- real_t sum = oldNormalImpulse + normalImpulse;
- m_accumulatedImpulse[limit_index] = sum > hi ? real_t(0.) : sum < lo ? real_t(0.) : sum;
- normalImpulse = m_accumulatedImpulse[limit_index] - oldNormalImpulse;
-
- Vector3 impulse_vector = axis_normal_on_a * normalImpulse;
- body1->apply_impulse( rel_pos1, impulse_vector);
- body2->apply_impulse( rel_pos2, -impulse_vector);
- return normalImpulse;
+ Vector3 impulse_vector = axis_normal_on_a * normalImpulse;
+ body1->apply_impulse(rel_pos1, impulse_vector);
+ body2->apply_impulse(rel_pos2, -impulse_vector);
+ return normalImpulse;
}
//////////////////////////// G6DOFTranslationalLimitMotorSW ////////////////////////////////////
-
-Generic6DOFJointSW::Generic6DOFJointSW(BodySW* rbA, BodySW* rbB, const Transform& frameInA, const Transform& frameInB, bool useLinearReferenceFrameA)
- : JointSW(_arr,2)
- , m_frameInA(frameInA)
- , m_frameInB(frameInB),
- m_useLinearReferenceFrameA(useLinearReferenceFrameA)
-{
- A=rbA;
- B=rbB;
- A->add_constraint(this,0);
- B->add_constraint(this,1);
+Generic6DOFJointSW::Generic6DOFJointSW(BodySW *rbA, BodySW *rbB, const Transform &frameInA, const Transform &frameInB, bool useLinearReferenceFrameA)
+ : JointSW(_arr, 2), m_frameInA(frameInA), m_frameInB(frameInB),
+ m_useLinearReferenceFrameA(useLinearReferenceFrameA) {
+ A = rbA;
+ B = rbB;
+ A->add_constraint(this, 0);
+ B->add_constraint(this, 1);
}
-
-
-
-
-void Generic6DOFJointSW::calculateAngleInfo()
-{
- Basis relative_frame = m_calculatedTransformA.basis.inverse()*m_calculatedTransformB.basis;
+void Generic6DOFJointSW::calculateAngleInfo() {
+ Basis relative_frame = m_calculatedTransformA.basis.inverse() * m_calculatedTransformB.basis;
m_calculatedAxisAngleDiff = relative_frame.get_euler();
-
-
// in euler angle mode we do not actually constrain the angular velocity
- // along the axes axis[0] and axis[2] (although we do use axis[1]) :
- //
- // to get constrain w2-w1 along ...not
- // ------ --------------------- ------
- // d(angle[0])/dt = 0 ax[1] x ax[2] ax[0]
- // d(angle[1])/dt = 0 ax[1]
- // d(angle[2])/dt = 0 ax[0] x ax[1] ax[2]
- //
- // constraining w2-w1 along an axis 'a' means that a'*(w2-w1)=0.
- // to prove the result for angle[0], write the expression for angle[0] from
- // GetInfo1 then take the derivative. to prove this for angle[2] it is
- // easier to take the euler rate expression for d(angle[2])/dt with respect
- // to the components of w and set that to 0.
+ // along the axes axis[0] and axis[2] (although we do use axis[1]) :
+ //
+ // to get constrain w2-w1 along ...not
+ // ------ --------------------- ------
+ // d(angle[0])/dt = 0 ax[1] x ax[2] ax[0]
+ // d(angle[1])/dt = 0 ax[1]
+ // d(angle[2])/dt = 0 ax[0] x ax[1] ax[2]
+ //
+ // constraining w2-w1 along an axis 'a' means that a'*(w2-w1)=0.
+ // to prove the result for angle[0], write the expression for angle[0] from
+ // GetInfo1 then take the derivative. to prove this for angle[2] it is
+ // easier to take the euler rate expression for d(angle[2])/dt with respect
+ // to the components of w and set that to 0.
Vector3 axis0 = m_calculatedTransformB.basis.get_axis(0);
Vector3 axis2 = m_calculatedTransformA.basis.get_axis(2);
@@ -275,7 +223,6 @@ void Generic6DOFJointSW::calculateAngleInfo()
m_calculatedAxis[0] = m_calculatedAxis[1].cross(axis2);
m_calculatedAxis[2] = axis0.cross(m_calculatedAxis[1]);
-
/*
if(m_debugDrawer)
{
@@ -288,280 +235,250 @@ void Generic6DOFJointSW::calculateAngleInfo()
m_debugDrawer->reportErrorWarning(buff);
}
*/
-
}
-void Generic6DOFJointSW::calculateTransforms()
-{
- m_calculatedTransformA = A->get_transform() * m_frameInA;
- m_calculatedTransformB = B->get_transform() * m_frameInB;
+void Generic6DOFJointSW::calculateTransforms() {
+ m_calculatedTransformA = A->get_transform() * m_frameInA;
+ m_calculatedTransformB = B->get_transform() * m_frameInB;
- calculateAngleInfo();
+ calculateAngleInfo();
}
-
void Generic6DOFJointSW::buildLinearJacobian(
- JacobianEntrySW & jacLinear,const Vector3 & normalWorld,
- const Vector3 & pivotAInW,const Vector3 & pivotBInW)
-{
- memnew_placement(&jacLinear, JacobianEntrySW(
- A->get_principal_inertia_axes().transposed(),
- B->get_principal_inertia_axes().transposed(),
- pivotAInW - A->get_transform().origin - A->get_center_of_mass(),
- pivotBInW - B->get_transform().origin - B->get_center_of_mass(),
- normalWorld,
- A->get_inv_inertia(),
- A->get_inv_mass(),
- B->get_inv_inertia(),
- B->get_inv_mass()));
-
+ JacobianEntrySW &jacLinear, const Vector3 &normalWorld,
+ const Vector3 &pivotAInW, const Vector3 &pivotBInW) {
+ memnew_placement(&jacLinear, JacobianEntrySW(
+ A->get_principal_inertia_axes().transposed(),
+ B->get_principal_inertia_axes().transposed(),
+ pivotAInW - A->get_transform().origin - A->get_center_of_mass(),
+ pivotBInW - B->get_transform().origin - B->get_center_of_mass(),
+ normalWorld,
+ A->get_inv_inertia(),
+ A->get_inv_mass(),
+ B->get_inv_inertia(),
+ B->get_inv_mass()));
}
void Generic6DOFJointSW::buildAngularJacobian(
- JacobianEntrySW & jacAngular,const Vector3 & jointAxisW)
-{
- memnew_placement(&jacAngular, JacobianEntrySW(jointAxisW,
- A->get_principal_inertia_axes().transposed(),
- B->get_principal_inertia_axes().transposed(),
- A->get_inv_inertia(),
- B->get_inv_inertia()));
-
+ JacobianEntrySW &jacAngular, const Vector3 &jointAxisW) {
+ memnew_placement(&jacAngular, JacobianEntrySW(jointAxisW,
+ A->get_principal_inertia_axes().transposed(),
+ B->get_principal_inertia_axes().transposed(),
+ A->get_inv_inertia(),
+ B->get_inv_inertia()));
}
-bool Generic6DOFJointSW::testAngularLimitMotor(int axis_index)
-{
- real_t angle = m_calculatedAxisAngleDiff[axis_index];
+bool Generic6DOFJointSW::testAngularLimitMotor(int axis_index) {
+ real_t angle = m_calculatedAxisAngleDiff[axis_index];
- //test limits
- m_angularLimits[axis_index].testLimitValue(angle);
- return m_angularLimits[axis_index].needApplyTorques();
+ //test limits
+ m_angularLimits[axis_index].testLimitValue(angle);
+ return m_angularLimits[axis_index].needApplyTorques();
}
bool Generic6DOFJointSW::setup(real_t p_step) {
// Clear accumulated impulses for the next simulation step
- m_linearLimits.m_accumulatedImpulse=Vector3(real_t(0.), real_t(0.), real_t(0.));
- int i;
- for(i = 0; i < 3; i++)
- {
- m_angularLimits[i].m_accumulatedImpulse = real_t(0.);
- }
- //calculates transform
- calculateTransforms();
-
-// const Vector3& pivotAInW = m_calculatedTransformA.origin;
-// const Vector3& pivotBInW = m_calculatedTransformB.origin;
+ 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;
+ // not used here
+ // Vector3 rel_pos1 = pivotAInW - A->get_transform().origin;
+ // Vector3 rel_pos2 = pivotBInW - B->get_transform().origin;
- Vector3 normalWorld;
- //linear part
- for (i=0;i<3;i++)
- {
- if (m_linearLimits.enable_limit[i] && m_linearLimits.isLimited(i))
- {
+ Vector3 normalWorld;
+ //linear part
+ for (i = 0; i < 3; i++) {
+ if (m_linearLimits.enable_limit[i] && m_linearLimits.isLimited(i)) {
if (m_useLinearReferenceFrameA)
- normalWorld = m_calculatedTransformA.basis.get_axis(i);
+ normalWorld = m_calculatedTransformA.basis.get_axis(i);
else
- normalWorld = m_calculatedTransformB.basis.get_axis(i);
-
- buildLinearJacobian(
- m_jacLinear[i],normalWorld ,
- pivotAInW,pivotBInW);
+ normalWorld = m_calculatedTransformB.basis.get_axis(i);
+ buildLinearJacobian(
+ m_jacLinear[i], normalWorld,
+ pivotAInW, pivotBInW);
+ }
}
- }
- // angular part
- for (i=0;i<3;i++)
- {
- //calculates error angle
- if (m_angularLimits[i].m_enableLimit && testAngularLimitMotor(i))
- {
- normalWorld = this->getAxis(i);
- // Create angular atom
- buildAngularJacobian(m_jacAng[i],normalWorld);
+ // angular part
+ for (i = 0; i < 3; i++) {
+ //calculates error angle
+ if (m_angularLimits[i].m_enableLimit && testAngularLimitMotor(i)) {
+ normalWorld = this->getAxis(i);
+ // Create angular atom
+ buildAngularJacobian(m_jacAng[i], normalWorld);
+ }
}
- }
return true;
}
+void Generic6DOFJointSW::solve(real_t timeStep) {
+ m_timeStep = timeStep;
-void Generic6DOFJointSW::solve(real_t timeStep)
-{
- m_timeStep = timeStep;
-
- //calculateTransforms();
+ //calculateTransforms();
- int i;
+ int i;
- // linear
+ // linear
- Vector3 pointInA = m_calculatedTransformA.origin;
+ Vector3 pointInA = m_calculatedTransformA.origin;
Vector3 pointInB = m_calculatedTransformB.origin;
real_t jacDiagABInv;
Vector3 linear_axis;
- for (i=0;i<3;i++)
- {
- if (m_linearLimits.enable_limit[i] && m_linearLimits.isLimited(i))
- {
- jacDiagABInv = real_t(1.) / m_jacLinear[i].getDiagonal();
+ for (i = 0; i < 3; i++) {
+ if (m_linearLimits.enable_limit[i] && m_linearLimits.isLimited(i)) {
+ jacDiagABInv = real_t(1.) / m_jacLinear[i].getDiagonal();
if (m_useLinearReferenceFrameA)
- linear_axis = m_calculatedTransformA.basis.get_axis(i);
+ linear_axis = m_calculatedTransformA.basis.get_axis(i);
else
- linear_axis = m_calculatedTransformB.basis.get_axis(i);
-
- m_linearLimits.solveLinearAxis(
- m_timeStep,
- jacDiagABInv,
- A,pointInA,
- B,pointInB,
- i,linear_axis, m_AnchorPos);
-
+ linear_axis = m_calculatedTransformB.basis.get_axis(i);
+
+ m_linearLimits.solveLinearAxis(
+ m_timeStep,
+ jacDiagABInv,
+ A, pointInA,
+ B, pointInB,
+ i, linear_axis, m_AnchorPos);
+ }
}
- }
-
- // angular
- Vector3 angular_axis;
- real_t angularJacDiagABInv;
- for (i=0;i<3;i++)
- {
- if (m_angularLimits[i].m_enableLimit && m_angularLimits[i].needApplyTorques())
- {
+
+ // angular
+ Vector3 angular_axis;
+ real_t angularJacDiagABInv;
+ for (i = 0; i < 3; i++) {
+ if (m_angularLimits[i].m_enableLimit && m_angularLimits[i].needApplyTorques()) {
// get axis
angular_axis = getAxis(i);
angularJacDiagABInv = real_t(1.) / m_jacAng[i].getDiagonal();
- m_angularLimits[i].solveAngularLimits(m_timeStep,angular_axis,angularJacDiagABInv, A,B);
+ m_angularLimits[i].solveAngularLimits(m_timeStep, angular_axis, angularJacDiagABInv, A, B);
+ }
}
- }
}
-void Generic6DOFJointSW::updateRHS(real_t timeStep)
-{
- (void)timeStep;
-
+void Generic6DOFJointSW::updateRHS(real_t timeStep) {
+ (void)timeStep;
}
-Vector3 Generic6DOFJointSW::getAxis(int axis_index) const
-{
- return m_calculatedAxis[axis_index];
+Vector3 Generic6DOFJointSW::getAxis(int axis_index) const {
+ return m_calculatedAxis[axis_index];
}
-real_t Generic6DOFJointSW::getAngle(int axis_index) const
-{
- return m_calculatedAxisAngleDiff[axis_index];
+real_t Generic6DOFJointSW::getAngle(int axis_index) const {
+ return m_calculatedAxisAngleDiff[axis_index];
}
-void Generic6DOFJointSW::calcAnchorPos(void)
-{
+void Generic6DOFJointSW::calcAnchorPos(void) {
real_t imA = A->get_inv_mass();
real_t imB = B->get_inv_mass();
real_t weight;
- if(imB == real_t(0.0))
- {
+ if (imB == real_t(0.0)) {
weight = real_t(1.0);
- }
- else
- {
+ } else {
weight = imA / (imA + imB);
}
- const Vector3& pA = m_calculatedTransformA.origin;
- const Vector3& pB = m_calculatedTransformB.origin;
+ const Vector3 &pA = m_calculatedTransformA.origin;
+ const Vector3 &pB = m_calculatedTransformB.origin;
m_AnchorPos = pA * weight + pB * (real_t(1.0) - weight);
return;
} // Generic6DOFJointSW::calcAnchorPos()
+void Generic6DOFJointSW::set_param(Vector3::Axis p_axis, PhysicsServer::G6DOFJointAxisParam p_param, real_t p_value) {
-void Generic6DOFJointSW::set_param(Vector3::Axis p_axis,PhysicsServer::G6DOFJointAxisParam p_param, real_t p_value) {
-
- ERR_FAIL_INDEX(p_axis,3);
- switch(p_param) {
+ ERR_FAIL_INDEX(p_axis, 3);
+ switch (p_param) {
case PhysicsServer::G6DOF_JOINT_LINEAR_LOWER_LIMIT: {
- m_linearLimits.m_lowerLimit[p_axis]=p_value;
+ m_linearLimits.m_lowerLimit[p_axis] = p_value;
} break;
case PhysicsServer::G6DOF_JOINT_LINEAR_UPPER_LIMIT: {
- m_linearLimits.m_upperLimit[p_axis]=p_value;
+ m_linearLimits.m_upperLimit[p_axis] = p_value;
} break;
case PhysicsServer::G6DOF_JOINT_LINEAR_LIMIT_SOFTNESS: {
- m_linearLimits.m_limitSoftness[p_axis]=p_value;
+ m_linearLimits.m_limitSoftness[p_axis] = p_value;
} break;
case PhysicsServer::G6DOF_JOINT_LINEAR_RESTITUTION: {
- m_linearLimits.m_restitution[p_axis]=p_value;
+ m_linearLimits.m_restitution[p_axis] = p_value;
} break;
case PhysicsServer::G6DOF_JOINT_LINEAR_DAMPING: {
- m_linearLimits.m_damping[p_axis]=p_value;
+ m_linearLimits.m_damping[p_axis] = p_value;
} break;
case PhysicsServer::G6DOF_JOINT_ANGULAR_LOWER_LIMIT: {
- m_angularLimits[p_axis].m_loLimit=p_value;
+ m_angularLimits[p_axis].m_loLimit = p_value;
} break;
case PhysicsServer::G6DOF_JOINT_ANGULAR_UPPER_LIMIT: {
- m_angularLimits[p_axis].m_hiLimit=p_value;
+ m_angularLimits[p_axis].m_hiLimit = p_value;
} break;
case PhysicsServer::G6DOF_JOINT_ANGULAR_LIMIT_SOFTNESS: {
- m_angularLimits[p_axis].m_limitSoftness=p_value;
+ m_angularLimits[p_axis].m_limitSoftness = p_value;
} break;
case PhysicsServer::G6DOF_JOINT_ANGULAR_DAMPING: {
- m_angularLimits[p_axis].m_damping=p_value;
+ m_angularLimits[p_axis].m_damping = p_value;
} break;
case PhysicsServer::G6DOF_JOINT_ANGULAR_RESTITUTION: {
- m_angularLimits[p_axis].m_bounce=p_value;
+ m_angularLimits[p_axis].m_bounce = p_value;
} break;
case PhysicsServer::G6DOF_JOINT_ANGULAR_FORCE_LIMIT: {
- m_angularLimits[p_axis].m_maxLimitForce=p_value;
+ m_angularLimits[p_axis].m_maxLimitForce = p_value;
} break;
case PhysicsServer::G6DOF_JOINT_ANGULAR_ERP: {
- m_angularLimits[p_axis].m_ERP=p_value;
+ m_angularLimits[p_axis].m_ERP = p_value;
} break;
case PhysicsServer::G6DOF_JOINT_ANGULAR_MOTOR_TARGET_VELOCITY: {
- m_angularLimits[p_axis].m_targetVelocity=p_value;
+ m_angularLimits[p_axis].m_targetVelocity = p_value;
} break;
case PhysicsServer::G6DOF_JOINT_ANGULAR_MOTOR_FORCE_LIMIT: {
- m_angularLimits[p_axis].m_maxLimitForce=p_value;
+ m_angularLimits[p_axis].m_maxLimitForce = p_value;
} break;
}
}
-real_t Generic6DOFJointSW::get_param(Vector3::Axis p_axis,PhysicsServer::G6DOFJointAxisParam p_param) const{
- ERR_FAIL_INDEX_V(p_axis,3,0);
- switch(p_param) {
+real_t Generic6DOFJointSW::get_param(Vector3::Axis p_axis, PhysicsServer::G6DOFJointAxisParam p_param) const {
+ ERR_FAIL_INDEX_V(p_axis, 3, 0);
+ switch (p_param) {
case PhysicsServer::G6DOF_JOINT_LINEAR_LOWER_LIMIT: {
return m_linearLimits.m_lowerLimit[p_axis];
@@ -635,31 +552,29 @@ real_t Generic6DOFJointSW::get_param(Vector3::Axis p_axis,PhysicsServer::G6DOFJo
return 0;
}
-void Generic6DOFJointSW::set_flag(Vector3::Axis p_axis,PhysicsServer::G6DOFJointAxisFlag p_flag, bool p_value){
+void Generic6DOFJointSW::set_flag(Vector3::Axis p_axis, PhysicsServer::G6DOFJointAxisFlag p_flag, bool p_value) {
- ERR_FAIL_INDEX(p_axis,3);
+ ERR_FAIL_INDEX(p_axis, 3);
- switch(p_flag) {
+ switch (p_flag) {
case PhysicsServer::G6DOF_JOINT_FLAG_ENABLE_LINEAR_LIMIT: {
- m_linearLimits.enable_limit[p_axis]=p_value;
+ m_linearLimits.enable_limit[p_axis] = p_value;
} break;
case PhysicsServer::G6DOF_JOINT_FLAG_ENABLE_ANGULAR_LIMIT: {
- m_angularLimits[p_axis].m_enableLimit=p_value;
+ m_angularLimits[p_axis].m_enableLimit = p_value;
} break;
case PhysicsServer::G6DOF_JOINT_FLAG_ENABLE_MOTOR: {
- m_angularLimits[p_axis].m_enableMotor=p_value;
+ m_angularLimits[p_axis].m_enableMotor = p_value;
} break;
}
-
-
}
-bool Generic6DOFJointSW::get_flag(Vector3::Axis p_axis,PhysicsServer::G6DOFJointAxisFlag p_flag) const{
+bool Generic6DOFJointSW::get_flag(Vector3::Axis p_axis, PhysicsServer::G6DOFJointAxisFlag p_flag) const {
- ERR_FAIL_INDEX_V(p_axis,3,0);
- switch(p_flag) {
+ ERR_FAIL_INDEX_V(p_axis, 3, 0);
+ switch (p_flag) {
case PhysicsServer::G6DOF_JOINT_FLAG_ENABLE_LINEAR_LIMIT: {
return m_linearLimits.enable_limit[p_axis];
generated by cgit v1.2.3 (git 2.25.1) at 2024-11-22 05:19:37 +0000