summaryrefslogtreecommitdiff
path: root/thirdparty/bullet/Bullet3Dynamics/ConstraintSolver/b3Generic6DofConstraint.cpp
diff options
context:
space:
mode:
Diffstat (limited to 'thirdparty/bullet/Bullet3Dynamics/ConstraintSolver/b3Generic6DofConstraint.cpp')
-rw-r--r--thirdparty/bullet/Bullet3Dynamics/ConstraintSolver/b3Generic6DofConstraint.cpp576
1 files changed, 253 insertions, 323 deletions
diff --git a/thirdparty/bullet/Bullet3Dynamics/ConstraintSolver/b3Generic6DofConstraint.cpp b/thirdparty/bullet/Bullet3Dynamics/ConstraintSolver/b3Generic6DofConstraint.cpp
index 3ae2922e58..0d5bb2014b 100644
--- a/thirdparty/bullet/Bullet3Dynamics/ConstraintSolver/b3Generic6DofConstraint.cpp
+++ b/thirdparty/bullet/Bullet3Dynamics/ConstraintSolver/b3Generic6DofConstraint.cpp
@@ -26,69 +26,48 @@ http://gimpact.sf.net
#include "Bullet3Common/b3TransformUtil.h"
#include <new>
-
-
#define D6_USE_OBSOLETE_METHOD false
#define D6_USE_FRAME_OFFSET true
-
-
-
-
-
-b3Generic6DofConstraint::b3Generic6DofConstraint(int rbA,int rbB, const b3Transform& frameInA, const b3Transform& frameInB, bool useLinearReferenceFrameA, const b3RigidBodyData* bodies)
-: b3TypedConstraint(B3_D6_CONSTRAINT_TYPE, rbA, rbB)
-, m_frameInA(frameInA)
-, m_frameInB(frameInB),
-m_useLinearReferenceFrameA(useLinearReferenceFrameA),
-m_useOffsetForConstraintFrame(D6_USE_FRAME_OFFSET),
-m_flags(0)
+b3Generic6DofConstraint::b3Generic6DofConstraint(int rbA, int rbB, const b3Transform& frameInA, const b3Transform& frameInB, bool useLinearReferenceFrameA, const b3RigidBodyData* bodies)
+ : b3TypedConstraint(B3_D6_CONSTRAINT_TYPE, rbA, rbB), m_frameInA(frameInA), m_frameInB(frameInB), m_useLinearReferenceFrameA(useLinearReferenceFrameA), m_useOffsetForConstraintFrame(D6_USE_FRAME_OFFSET), m_flags(0)
{
calculateTransforms(bodies);
}
-
-
-
-
-
#define GENERIC_D6_DISABLE_WARMSTARTING 1
-
-
b3Scalar btGetMatrixElem(const b3Matrix3x3& mat, int index);
b3Scalar btGetMatrixElem(const b3Matrix3x3& mat, int index)
{
- int i = index%3;
- int j = index/3;
+ 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 b3Matrix3x3& mat,b3Vector3& xyz);
-bool matrixToEulerXYZ(const b3Matrix3x3& mat,b3Vector3& xyz)
+bool matrixToEulerXYZ(const b3Matrix3x3& mat, b3Vector3& xyz);
+bool matrixToEulerXYZ(const b3Matrix3x3& mat, b3Vector3& 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
//
- b3Scalar fi = btGetMatrixElem(mat,2);
+ b3Scalar fi = btGetMatrixElem(mat, 2);
if (fi < b3Scalar(1.0f))
{
if (fi > b3Scalar(-1.0f))
{
- xyz[0] = b3Atan2(-btGetMatrixElem(mat,5),btGetMatrixElem(mat,8));
- xyz[1] = b3Asin(btGetMatrixElem(mat,2));
- xyz[2] = b3Atan2(-btGetMatrixElem(mat,1),btGetMatrixElem(mat,0));
+ xyz[0] = b3Atan2(-btGetMatrixElem(mat, 5), btGetMatrixElem(mat, 8));
+ xyz[1] = b3Asin(btGetMatrixElem(mat, 2));
+ xyz[2] = b3Atan2(-btGetMatrixElem(mat, 1), btGetMatrixElem(mat, 0));
return true;
}
else
{
// WARNING. Not unique. XA - ZA = -atan2(r10,r11)
- xyz[0] = -b3Atan2(btGetMatrixElem(mat,3),btGetMatrixElem(mat,4));
+ xyz[0] = -b3Atan2(btGetMatrixElem(mat, 3), btGetMatrixElem(mat, 4));
xyz[1] = -B3_HALF_PI;
xyz[2] = b3Scalar(0.0);
return false;
@@ -97,7 +76,7 @@ bool matrixToEulerXYZ(const b3Matrix3x3& mat,b3Vector3& xyz)
else
{
// WARNING. Not unique. XAngle + ZAngle = atan2(r10,r11)
- xyz[0] = b3Atan2(btGetMatrixElem(mat,3),btGetMatrixElem(mat,4));
+ xyz[0] = b3Atan2(btGetMatrixElem(mat, 3), btGetMatrixElem(mat, 4));
xyz[1] = B3_HALF_PI;
xyz[2] = 0.0;
}
@@ -108,85 +87,75 @@ bool matrixToEulerXYZ(const b3Matrix3x3& mat,b3Vector3& xyz)
int b3RotationalLimitMotor::testLimitValue(b3Scalar test_value)
{
- if(m_loLimit>m_hiLimit)
+ if (m_loLimit > m_hiLimit)
{
- m_currentLimit = 0;//Free from violation
+ 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(m_currentLimitError>B3_PI)
- m_currentLimitError-=B3_2_PI;
- else if(m_currentLimitError<-B3_PI)
- m_currentLimitError+=B3_2_PI;
+ m_currentLimit = 1; //low limit violation
+ m_currentLimitError = test_value - m_loLimit;
+ if (m_currentLimitError > B3_PI)
+ m_currentLimitError -= B3_2_PI;
+ else if (m_currentLimitError < -B3_PI)
+ m_currentLimitError += B3_2_PI;
return 1;
}
- else if (test_value> m_hiLimit)
+ else if (test_value > m_hiLimit)
{
- m_currentLimit = 2;//High limit violation
+ m_currentLimit = 2; //High limit violation
m_currentLimitError = test_value - m_hiLimit;
- if(m_currentLimitError>B3_PI)
- m_currentLimitError-=B3_2_PI;
- else if(m_currentLimitError<-B3_PI)
- m_currentLimitError+=B3_2_PI;
+ if (m_currentLimitError > B3_PI)
+ m_currentLimitError -= B3_2_PI;
+ else if (m_currentLimitError < -B3_PI)
+ m_currentLimitError += B3_2_PI;
return 2;
};
- m_currentLimit = 0;//Free from violation
+ m_currentLimit = 0; //Free from violation
return 0;
-
}
-
-
-
//////////////////////////// End b3RotationalLimitMotor ////////////////////////////////////
-
-
-
//////////////////////////// b3TranslationalLimitMotor ////////////////////////////////////
-
int b3TranslationalLimitMotor::testLimitValue(int limitIndex, b3Scalar test_value)
{
b3Scalar loLimit = m_lowerLimit[limitIndex];
b3Scalar hiLimit = m_upperLimit[limitIndex];
- if(loLimit > hiLimit)
+ if (loLimit > hiLimit)
{
- m_currentLimit[limitIndex] = 0;//Free from violation
+ m_currentLimit[limitIndex] = 0; //Free from violation
m_currentLimitError[limitIndex] = b3Scalar(0.f);
return 0;
}
if (test_value < loLimit)
{
- m_currentLimit[limitIndex] = 2;//low limit violation
- m_currentLimitError[limitIndex] = test_value - loLimit;
+ m_currentLimit[limitIndex] = 2; //low limit violation
+ m_currentLimitError[limitIndex] = test_value - loLimit;
return 2;
}
- else if (test_value> hiLimit)
+ else if (test_value > hiLimit)
{
- m_currentLimit[limitIndex] = 1;//High limit violation
+ m_currentLimit[limitIndex] = 1; //High limit violation
m_currentLimitError[limitIndex] = test_value - hiLimit;
return 1;
};
- m_currentLimit[limitIndex] = 0;//Free from violation
+ m_currentLimit[limitIndex] = 0; //Free from violation
m_currentLimitError[limitIndex] = b3Scalar(0.f);
return 0;
}
-
-
//////////////////////////// b3TranslationalLimitMotor ////////////////////////////////////
void b3Generic6DofConstraint::calculateAngleInfo()
{
- b3Matrix3x3 relative_frame = m_calculatedTransformA.getBasis().inverse()*m_calculatedTransformB.getBasis();
- matrixToEulerXYZ(relative_frame,m_calculatedAxisAngleDiff);
+ b3Matrix3x3 relative_frame = m_calculatedTransformA.getBasis().inverse() * m_calculatedTransformB.getBasis();
+ 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]) :
//
@@ -211,12 +180,11 @@ void b3Generic6DofConstraint::calculateAngleInfo()
m_calculatedAxis[0].normalize();
m_calculatedAxis[1].normalize();
m_calculatedAxis[2].normalize();
-
}
static b3Transform getCenterOfMassTransform(const b3RigidBodyData& body)
{
- b3Transform tr(body.m_quat,body.m_pos);
+ b3Transform tr(body.m_quat, body.m_pos);
return tr;
}
@@ -226,26 +194,26 @@ void b3Generic6DofConstraint::calculateTransforms(const b3RigidBodyData* bodies)
b3Transform transB;
transA = getCenterOfMassTransform(bodies[m_rbA]);
transB = getCenterOfMassTransform(bodies[m_rbB]);
- calculateTransforms(transA,transB,bodies);
+ calculateTransforms(transA, transB, bodies);
}
-void b3Generic6DofConstraint::calculateTransforms(const b3Transform& transA,const b3Transform& transB,const b3RigidBodyData* bodies)
+void b3Generic6DofConstraint::calculateTransforms(const b3Transform& transA, const b3Transform& transB, const b3RigidBodyData* bodies)
{
m_calculatedTransformA = transA * m_frameInA;
m_calculatedTransformB = transB * m_frameInB;
calculateLinearInfo();
calculateAngleInfo();
- if(m_useOffsetForConstraintFrame)
- { // get weight factors depending on masses
+ if (m_useOffsetForConstraintFrame)
+ { // get weight factors depending on masses
b3Scalar miA = bodies[m_rbA].m_invMass;
b3Scalar miB = bodies[m_rbB].m_invMass;
m_hasStaticBody = (miA < B3_EPSILON) || (miB < B3_EPSILON);
b3Scalar miS = miA + miB;
- if(miS > b3Scalar(0.f))
+ if (miS > b3Scalar(0.f))
{
m_factA = miB / miS;
}
- else
+ else
{
m_factA = b3Scalar(0.5f);
}
@@ -253,12 +221,6 @@ void b3Generic6DofConstraint::calculateTransforms(const b3Transform& transA,cons
}
}
-
-
-
-
-
-
bool b3Generic6DofConstraint::testAngularLimitMotor(int axis_index)
{
b3Scalar angle = m_calculatedAxisAngleDiff[axis_index];
@@ -269,48 +231,43 @@ bool b3Generic6DofConstraint::testAngularLimitMotor(int axis_index)
return m_angularLimits[axis_index].needApplyTorques();
}
-
-
-
-void b3Generic6DofConstraint::getInfo1 (b3ConstraintInfo1* info,const b3RigidBodyData* bodies)
+void b3Generic6DofConstraint::getInfo1(b3ConstraintInfo1* info, const b3RigidBodyData* bodies)
{
//prepare constraint
- calculateTransforms(getCenterOfMassTransform(bodies[m_rbA]),getCenterOfMassTransform(bodies[m_rbB]),bodies);
+ calculateTransforms(getCenterOfMassTransform(bodies[m_rbA]), getCenterOfMassTransform(bodies[m_rbB]), bodies);
info->m_numConstraintRows = 0;
info->nub = 6;
int i;
//test linear limits
- for(i = 0; i < 3; i++)
+ for (i = 0; i < 3; i++)
{
- if(m_linearLimits.needApplyForce(i))
+ if (m_linearLimits.needApplyForce(i))
{
info->m_numConstraintRows++;
info->nub--;
}
}
//test angular limits
- for (i=0;i<3 ;i++ )
+ for (i = 0; i < 3; i++)
{
- if(testAngularLimitMotor(i))
+ if (testAngularLimitMotor(i))
{
info->m_numConstraintRows++;
info->nub--;
}
}
-// printf("info->m_numConstraintRows=%d\n",info->m_numConstraintRows);
+ // printf("info->m_numConstraintRows=%d\n",info->m_numConstraintRows);
}
-void b3Generic6DofConstraint::getInfo1NonVirtual (b3ConstraintInfo1* info,const b3RigidBodyData* bodies)
+void b3Generic6DofConstraint::getInfo1NonVirtual(b3ConstraintInfo1* info, const b3RigidBodyData* bodies)
{
//pre-allocate all 6
info->m_numConstraintRows = 6;
info->nub = 0;
}
-
-void b3Generic6DofConstraint::getInfo2 (b3ConstraintInfo2* info,const b3RigidBodyData* bodies)
+void b3Generic6DofConstraint::getInfo2(b3ConstraintInfo2* info, const b3RigidBodyData* bodies)
{
-
b3Transform transA = getCenterOfMassTransform(bodies[m_rbA]);
b3Transform transB = getCenterOfMassTransform(bodies[m_rbB]);
const b3Vector3& linVelA = bodies[m_rbA].m_linVel;
@@ -318,136 +275,124 @@ void b3Generic6DofConstraint::getInfo2 (b3ConstraintInfo2* info,const b3RigidBod
const b3Vector3& angVelA = bodies[m_rbA].m_angVel;
const b3Vector3& angVelB = bodies[m_rbB].m_angVel;
- if(m_useOffsetForConstraintFrame)
- { // for stability better to solve angular limits first
- int row = setAngularLimits(info, 0,transA,transB,linVelA,linVelB,angVelA,angVelB);
- setLinearLimits(info, row, transA,transB,linVelA,linVelB,angVelA,angVelB);
+ if (m_useOffsetForConstraintFrame)
+ { // for stability better to solve angular limits first
+ int row = setAngularLimits(info, 0, transA, transB, linVelA, linVelB, angVelA, angVelB);
+ setLinearLimits(info, row, transA, transB, linVelA, linVelB, angVelA, angVelB);
}
else
- { // leave old version for compatibility
- int row = setLinearLimits(info, 0, transA,transB,linVelA,linVelB,angVelA,angVelB);
- setAngularLimits(info, row,transA,transB,linVelA,linVelB,angVelA,angVelB);
+ { // leave old version for compatibility
+ int row = setLinearLimits(info, 0, transA, transB, linVelA, linVelB, angVelA, angVelB);
+ setAngularLimits(info, row, transA, transB, linVelA, linVelB, angVelA, angVelB);
}
-
}
-
-void b3Generic6DofConstraint::getInfo2NonVirtual (b3ConstraintInfo2* info, const b3Transform& transA,const b3Transform& transB,const b3Vector3& linVelA,const b3Vector3& linVelB,const b3Vector3& angVelA,const b3Vector3& angVelB,const b3RigidBodyData* bodies)
+void b3Generic6DofConstraint::getInfo2NonVirtual(b3ConstraintInfo2* info, const b3Transform& transA, const b3Transform& transB, const b3Vector3& linVelA, const b3Vector3& linVelB, const b3Vector3& angVelA, const b3Vector3& angVelB, const b3RigidBodyData* bodies)
{
-
//prepare constraint
- calculateTransforms(transA,transB,bodies);
+ calculateTransforms(transA, transB, bodies);
int i;
- for (i=0;i<3 ;i++ )
+ for (i = 0; i < 3; i++)
{
testAngularLimitMotor(i);
}
- if(m_useOffsetForConstraintFrame)
- { // for stability better to solve angular limits first
- int row = setAngularLimits(info, 0,transA,transB,linVelA,linVelB,angVelA,angVelB);
- setLinearLimits(info, row, transA,transB,linVelA,linVelB,angVelA,angVelB);
+ if (m_useOffsetForConstraintFrame)
+ { // for stability better to solve angular limits first
+ int row = setAngularLimits(info, 0, transA, transB, linVelA, linVelB, angVelA, angVelB);
+ setLinearLimits(info, row, transA, transB, linVelA, linVelB, angVelA, angVelB);
}
else
- { // leave old version for compatibility
- int row = setLinearLimits(info, 0, transA,transB,linVelA,linVelB,angVelA,angVelB);
- setAngularLimits(info, row,transA,transB,linVelA,linVelB,angVelA,angVelB);
+ { // leave old version for compatibility
+ int row = setLinearLimits(info, 0, transA, transB, linVelA, linVelB, angVelA, angVelB);
+ setAngularLimits(info, row, transA, transB, linVelA, linVelB, angVelA, angVelB);
}
}
-
-
-int b3Generic6DofConstraint::setLinearLimits(b3ConstraintInfo2* info, int row, const b3Transform& transA,const b3Transform& transB,const b3Vector3& linVelA,const b3Vector3& linVelB,const b3Vector3& angVelA,const b3Vector3& angVelB)
+int b3Generic6DofConstraint::setLinearLimits(b3ConstraintInfo2* info, int row, const b3Transform& transA, const b3Transform& transB, const b3Vector3& linVelA, const b3Vector3& linVelB, const b3Vector3& angVelA, const b3Vector3& angVelB)
{
-// int row = 0;
+ // int row = 0;
//solve linear limits
b3RotationalLimitMotor limot;
- for (int i=0;i<3 ;i++ )
+ for (int i = 0; i < 3; i++)
{
- if(m_linearLimits.needApplyForce(i))
- { // re-use rotational motor code
+ if (m_linearLimits.needApplyForce(i))
+ { // re-use rotational motor code
limot.m_bounce = b3Scalar(0.f);
limot.m_currentLimit = m_linearLimits.m_currentLimit[i];
limot.m_currentPosition = m_linearLimits.m_currentLinearDiff[i];
- limot.m_currentLimitError = m_linearLimits.m_currentLimitError[i];
- limot.m_damping = m_linearLimits.m_damping;
- limot.m_enableMotor = m_linearLimits.m_enableMotor[i];
- limot.m_hiLimit = m_linearLimits.m_upperLimit[i];
- limot.m_limitSoftness = m_linearLimits.m_limitSoftness;
- limot.m_loLimit = m_linearLimits.m_lowerLimit[i];
- limot.m_maxLimitForce = b3Scalar(0.f);
- limot.m_maxMotorForce = m_linearLimits.m_maxMotorForce[i];
- limot.m_targetVelocity = m_linearLimits.m_targetVelocity[i];
+ limot.m_currentLimitError = m_linearLimits.m_currentLimitError[i];
+ limot.m_damping = m_linearLimits.m_damping;
+ limot.m_enableMotor = m_linearLimits.m_enableMotor[i];
+ limot.m_hiLimit = m_linearLimits.m_upperLimit[i];
+ limot.m_limitSoftness = m_linearLimits.m_limitSoftness;
+ limot.m_loLimit = m_linearLimits.m_lowerLimit[i];
+ limot.m_maxLimitForce = b3Scalar(0.f);
+ limot.m_maxMotorForce = m_linearLimits.m_maxMotorForce[i];
+ limot.m_targetVelocity = m_linearLimits.m_targetVelocity[i];
b3Vector3 axis = m_calculatedTransformA.getBasis().getColumn(i);
int flags = m_flags >> (i * B3_6DOF_FLAGS_AXIS_SHIFT);
- limot.m_normalCFM = (flags & B3_6DOF_FLAGS_CFM_NORM) ? m_linearLimits.m_normalCFM[i] : info->cfm[0];
- limot.m_stopCFM = (flags & B3_6DOF_FLAGS_CFM_STOP) ? m_linearLimits.m_stopCFM[i] : info->cfm[0];
- limot.m_stopERP = (flags & B3_6DOF_FLAGS_ERP_STOP) ? m_linearLimits.m_stopERP[i] : info->erp;
- if(m_useOffsetForConstraintFrame)
+ limot.m_normalCFM = (flags & B3_6DOF_FLAGS_CFM_NORM) ? m_linearLimits.m_normalCFM[i] : info->cfm[0];
+ limot.m_stopCFM = (flags & B3_6DOF_FLAGS_CFM_STOP) ? m_linearLimits.m_stopCFM[i] : info->cfm[0];
+ limot.m_stopERP = (flags & B3_6DOF_FLAGS_ERP_STOP) ? m_linearLimits.m_stopERP[i] : info->erp;
+ if (m_useOffsetForConstraintFrame)
{
int indx1 = (i + 1) % 3;
int indx2 = (i + 2) % 3;
- int rotAllowed = 1; // rotations around orthos to current axis
- if(m_angularLimits[indx1].m_currentLimit && m_angularLimits[indx2].m_currentLimit)
+ int rotAllowed = 1; // rotations around orthos to current axis
+ if (m_angularLimits[indx1].m_currentLimit && m_angularLimits[indx2].m_currentLimit)
{
rotAllowed = 0;
}
- row += get_limit_motor_info2(&limot, transA,transB,linVelA,linVelB,angVelA,angVelB, info, row, axis, 0, rotAllowed);
+ row += get_limit_motor_info2(&limot, transA, transB, linVelA, linVelB, angVelA, angVelB, info, row, axis, 0, rotAllowed);
}
else
{
- row += get_limit_motor_info2(&limot, transA,transB,linVelA,linVelB,angVelA,angVelB, info, row, axis, 0);
+ row += get_limit_motor_info2(&limot, transA, transB, linVelA, linVelB, angVelA, angVelB, info, row, axis, 0);
}
}
}
return row;
}
-
-
-int b3Generic6DofConstraint::setAngularLimits(b3ConstraintInfo2 *info, int row_offset, const b3Transform& transA,const b3Transform& transB,const b3Vector3& linVelA,const b3Vector3& linVelB,const b3Vector3& angVelA,const b3Vector3& angVelB)
+int b3Generic6DofConstraint::setAngularLimits(b3ConstraintInfo2* info, int row_offset, const b3Transform& transA, const b3Transform& transB, const b3Vector3& linVelA, const b3Vector3& linVelB, const b3Vector3& angVelA, const b3Vector3& angVelB)
{
- b3Generic6DofConstraint * d6constraint = this;
+ b3Generic6DofConstraint* d6constraint = this;
int row = row_offset;
//solve angular limits
- for (int i=0;i<3 ;i++ )
+ for (int i = 0; i < 3; i++)
{
- if(d6constraint->getRotationalLimitMotor(i)->needApplyTorques())
+ if (d6constraint->getRotationalLimitMotor(i)->needApplyTorques())
{
b3Vector3 axis = d6constraint->getAxis(i);
int flags = m_flags >> ((i + 3) * B3_6DOF_FLAGS_AXIS_SHIFT);
- if(!(flags & B3_6DOF_FLAGS_CFM_NORM))
+ if (!(flags & B3_6DOF_FLAGS_CFM_NORM))
{
m_angularLimits[i].m_normalCFM = info->cfm[0];
}
- if(!(flags & B3_6DOF_FLAGS_CFM_STOP))
+ if (!(flags & B3_6DOF_FLAGS_CFM_STOP))
{
m_angularLimits[i].m_stopCFM = info->cfm[0];
}
- if(!(flags & B3_6DOF_FLAGS_ERP_STOP))
+ if (!(flags & B3_6DOF_FLAGS_ERP_STOP))
{
m_angularLimits[i].m_stopERP = info->erp;
}
row += get_limit_motor_info2(d6constraint->getRotationalLimitMotor(i),
- transA,transB,linVelA,linVelB,angVelA,angVelB, info,row,axis,1);
+ transA, transB, linVelA, linVelB, angVelA, angVelB, info, row, axis, 1);
}
}
return row;
}
-
-
-
-void b3Generic6DofConstraint::updateRHS(b3Scalar timeStep)
+void b3Generic6DofConstraint::updateRHS(b3Scalar timeStep)
{
(void)timeStep;
-
}
-
-void b3Generic6DofConstraint::setFrames(const b3Transform& frameA, const b3Transform& frameB,const b3RigidBodyData* bodies)
+void b3Generic6DofConstraint::setFrames(const b3Transform& frameA, const b3Transform& frameB, const b3RigidBodyData* bodies)
{
m_frameInA = frameA;
m_frameInB = frameB;
@@ -455,33 +400,27 @@ void b3Generic6DofConstraint::setFrames(const b3Transform& frameA, const b3Trans
calculateTransforms(bodies);
}
-
-
b3Vector3 b3Generic6DofConstraint::getAxis(int axis_index) const
{
return m_calculatedAxis[axis_index];
}
-
-b3Scalar b3Generic6DofConstraint::getRelativePivotPosition(int axisIndex) const
+b3Scalar b3Generic6DofConstraint::getRelativePivotPosition(int axisIndex) const
{
return m_calculatedLinearDiff[axisIndex];
}
-
b3Scalar b3Generic6DofConstraint::getAngle(int axisIndex) const
{
return m_calculatedAxisAngleDiff[axisIndex];
}
-
-
void b3Generic6DofConstraint::calcAnchorPos(const b3RigidBodyData* bodies)
{
b3Scalar imA = bodies[m_rbA].m_invMass;
b3Scalar imB = bodies[m_rbB].m_invMass;
b3Scalar weight;
- if(imB == b3Scalar(0.0))
+ if (imB == b3Scalar(0.0))
{
weight = b3Scalar(1.0);
}
@@ -495,47 +434,43 @@ void b3Generic6DofConstraint::calcAnchorPos(const b3RigidBodyData* bodies)
return;
}
-
-
void b3Generic6DofConstraint::calculateLinearInfo()
{
m_calculatedLinearDiff = m_calculatedTransformB.getOrigin() - m_calculatedTransformA.getOrigin();
m_calculatedLinearDiff = m_calculatedTransformA.getBasis().inverse() * m_calculatedLinearDiff;
- for(int i = 0; i < 3; i++)
+ for (int i = 0; i < 3; i++)
{
m_linearLimits.m_currentLinearDiff[i] = m_calculatedLinearDiff[i];
m_linearLimits.testLimitValue(i, m_calculatedLinearDiff[i]);
}
}
-
-
int b3Generic6DofConstraint::get_limit_motor_info2(
- b3RotationalLimitMotor * limot,
- const b3Transform& transA,const b3Transform& transB,const b3Vector3& linVelA,const b3Vector3& linVelB,const b3Vector3& angVelA,const b3Vector3& angVelB,
- b3ConstraintInfo2 *info, int row, b3Vector3& ax1, int rotational,int rotAllowed)
+ b3RotationalLimitMotor* limot,
+ const b3Transform& transA, const b3Transform& transB, const b3Vector3& linVelA, const b3Vector3& linVelB, const b3Vector3& angVelA, const b3Vector3& angVelB,
+ b3ConstraintInfo2* info, int row, b3Vector3& ax1, int rotational, int rotAllowed)
{
- int srow = row * info->rowskip;
- bool powered = limot->m_enableMotor;
- int limit = limot->m_currentLimit;
- if (powered || limit)
- { // if the joint is powered, or has joint limits, add in the extra row
- b3Scalar *J1 = rotational ? info->m_J1angularAxis : info->m_J1linearAxis;
- b3Scalar *J2 = rotational ? info->m_J2angularAxis : info->m_J2linearAxis;
+ int srow = row * info->rowskip;
+ bool powered = limot->m_enableMotor;
+ int limit = limot->m_currentLimit;
+ if (powered || limit)
+ { // if the joint is powered, or has joint limits, add in the extra row
+ b3Scalar* J1 = rotational ? info->m_J1angularAxis : info->m_J1linearAxis;
+ b3Scalar* J2 = rotational ? info->m_J2angularAxis : info->m_J2linearAxis;
if (J1)
{
- J1[srow+0] = ax1[0];
- J1[srow+1] = ax1[1];
- J1[srow+2] = ax1[2];
+ J1[srow + 0] = ax1[0];
+ J1[srow + 1] = ax1[1];
+ J1[srow + 2] = ax1[2];
}
if (J2)
{
- J2[srow+0] = -ax1[0];
- J2[srow+1] = -ax1[1];
- J2[srow+2] = -ax1[2];
+ J2[srow + 0] = -ax1[0];
+ J2[srow + 1] = -ax1[1];
+ J2[srow + 2] = -ax1[2];
}
- if((!rotational))
- {
+ if ((!rotational))
+ {
if (m_useOffsetForConstraintFrame)
{
b3Vector3 tmpA, tmpB, relA, relB;
@@ -558,55 +493,56 @@ int b3Generic6DofConstraint::get_limit_motor_info2(
relB = orthoB - totalDist * m_factB;
tmpA = relA.cross(ax1);
tmpB = relB.cross(ax1);
- if(m_hasStaticBody && (!rotAllowed))
+ if (m_hasStaticBody && (!rotAllowed))
{
tmpA *= m_factA;
tmpB *= m_factB;
}
int i;
- for (i=0; i<3; i++) info->m_J1angularAxis[srow+i] = tmpA[i];
- for (i=0; i<3; i++) info->m_J2angularAxis[srow+i] = -tmpB[i];
- } else
+ for (i = 0; i < 3; i++) info->m_J1angularAxis[srow + i] = tmpA[i];
+ for (i = 0; i < 3; i++) info->m_J2angularAxis[srow + i] = -tmpB[i];
+ }
+ else
{
- b3Vector3 ltd; // Linear Torque Decoupling vector
+ b3Vector3 ltd; // Linear Torque Decoupling vector
b3Vector3 c = m_calculatedTransformB.getOrigin() - transA.getOrigin();
ltd = c.cross(ax1);
- info->m_J1angularAxis[srow+0] = ltd[0];
- info->m_J1angularAxis[srow+1] = ltd[1];
- info->m_J1angularAxis[srow+2] = ltd[2];
+ info->m_J1angularAxis[srow + 0] = ltd[0];
+ info->m_J1angularAxis[srow + 1] = ltd[1];
+ info->m_J1angularAxis[srow + 2] = ltd[2];
c = m_calculatedTransformB.getOrigin() - transB.getOrigin();
ltd = -c.cross(ax1);
- info->m_J2angularAxis[srow+0] = ltd[0];
- info->m_J2angularAxis[srow+1] = ltd[1];
- info->m_J2angularAxis[srow+2] = ltd[2];
+ info->m_J2angularAxis[srow + 0] = ltd[0];
+ info->m_J2angularAxis[srow + 1] = ltd[1];
+ info->m_J2angularAxis[srow + 2] = ltd[2];
}
- }
- // if we're limited low and high simultaneously, the joint motor is
- // ineffective
- if (limit && (limot->m_loLimit == limot->m_hiLimit)) powered = false;
- info->m_constraintError[srow] = b3Scalar(0.f);
- if (powered)
- {
+ }
+ // if we're limited low and high simultaneously, the joint motor is
+ // ineffective
+ if (limit && (limot->m_loLimit == limot->m_hiLimit)) powered = false;
+ info->m_constraintError[srow] = b3Scalar(0.f);
+ if (powered)
+ {
info->cfm[srow] = limot->m_normalCFM;
- if(!limit)
- {
+ if (!limit)
+ {
b3Scalar tag_vel = rotational ? limot->m_targetVelocity : -limot->m_targetVelocity;
- b3Scalar mot_fact = getMotorFactor( limot->m_currentPosition,
- limot->m_loLimit,
- limot->m_hiLimit,
- tag_vel,
- info->fps * limot->m_stopERP);
+ b3Scalar mot_fact = getMotorFactor(limot->m_currentPosition,
+ limot->m_loLimit,
+ limot->m_hiLimit,
+ tag_vel,
+ info->fps * limot->m_stopERP);
info->m_constraintError[srow] += mot_fact * limot->m_targetVelocity;
- info->m_lowerLimit[srow] = -limot->m_maxMotorForce / info->fps;
- info->m_upperLimit[srow] = limot->m_maxMotorForce / info->fps;
- }
- }
- if(limit)
- {
- b3Scalar k = info->fps * limot->m_stopERP;
- if(!rotational)
+ info->m_lowerLimit[srow] = -limot->m_maxMotorForce / info->fps;
+ info->m_upperLimit[srow] = limot->m_maxMotorForce / info->fps;
+ }
+ }
+ if (limit)
+ {
+ b3Scalar k = info->fps * limot->m_stopERP;
+ if (!rotational)
{
info->m_constraintError[srow] += k * limot->m_currentLimitError;
}
@@ -615,116 +551,112 @@ int b3Generic6DofConstraint::get_limit_motor_info2(
info->m_constraintError[srow] += -k * limot->m_currentLimitError;
}
info->cfm[srow] = limot->m_stopCFM;
- if (limot->m_loLimit == limot->m_hiLimit)
- { // limited low and high simultaneously
- info->m_lowerLimit[srow] = -B3_INFINITY;
- info->m_upperLimit[srow] = B3_INFINITY;
- }
- else
- {
- if (limit == 1)
- {
- info->m_lowerLimit[srow] = 0;
- info->m_upperLimit[srow] = B3_INFINITY;
- }
- else
- {
- info->m_lowerLimit[srow] = -B3_INFINITY;
- info->m_upperLimit[srow] = 0;
- }
- // deal with bounce
- if (limot->m_bounce > 0)
- {
- // calculate joint velocity
- b3Scalar vel;
- if (rotational)
- {
- vel = angVelA.dot(ax1);
-//make sure that if no body -> angVelB == zero vec
-// if (body1)
- vel -= angVelB.dot(ax1);
- }
- else
- {
- vel = linVelA.dot(ax1);
-//make sure that if no body -> angVelB == zero vec
-// if (body1)
- vel -= linVelB.dot(ax1);
- }
- // only apply bounce if the velocity is incoming, and if the
- // resulting c[] exceeds what we already have.
- if (limit == 1)
- {
- if (vel < 0)
- {
- b3Scalar newc = -limot->m_bounce* vel;
- if (newc > info->m_constraintError[srow])
+ if (limot->m_loLimit == limot->m_hiLimit)
+ { // limited low and high simultaneously
+ info->m_lowerLimit[srow] = -B3_INFINITY;
+ info->m_upperLimit[srow] = B3_INFINITY;
+ }
+ else
+ {
+ if (limit == 1)
+ {
+ info->m_lowerLimit[srow] = 0;
+ info->m_upperLimit[srow] = B3_INFINITY;
+ }
+ else
+ {
+ info->m_lowerLimit[srow] = -B3_INFINITY;
+ info->m_upperLimit[srow] = 0;
+ }
+ // deal with bounce
+ if (limot->m_bounce > 0)
+ {
+ // calculate joint velocity
+ b3Scalar vel;
+ if (rotational)
+ {
+ vel = angVelA.dot(ax1);
+ //make sure that if no body -> angVelB == zero vec
+ // if (body1)
+ vel -= angVelB.dot(ax1);
+ }
+ else
+ {
+ vel = linVelA.dot(ax1);
+ //make sure that if no body -> angVelB == zero vec
+ // if (body1)
+ vel -= linVelB.dot(ax1);
+ }
+ // only apply bounce if the velocity is incoming, and if the
+ // resulting c[] exceeds what we already have.
+ if (limit == 1)
+ {
+ if (vel < 0)
+ {
+ b3Scalar newc = -limot->m_bounce * vel;
+ if (newc > info->m_constraintError[srow])
info->m_constraintError[srow] = newc;
- }
- }
- else
- {
- if (vel > 0)
- {
- b3Scalar newc = -limot->m_bounce * vel;
- if (newc < info->m_constraintError[srow])
+ }
+ }
+ else
+ {
+ if (vel > 0)
+ {
+ b3Scalar newc = -limot->m_bounce * vel;
+ if (newc < info->m_constraintError[srow])
info->m_constraintError[srow] = newc;
- }
- }
- }
- }
- }
- return 1;
- }
- else return 0;
+ }
+ }
+ }
+ }
+ }
+ return 1;
+ }
+ else
+ return 0;
}
-
-
-
-
-
- ///override the default global value of a parameter (such as ERP or CFM), optionally provide the axis (0..5).
- ///If no axis is provided, it uses the default axis for this constraint.
+///override the default global value of a parameter (such as ERP or CFM), optionally provide the axis (0..5).
+///If no axis is provided, it uses the default axis for this constraint.
void b3Generic6DofConstraint::setParam(int num, b3Scalar value, int axis)
{
- if((axis >= 0) && (axis < 3))
+ if ((axis >= 0) && (axis < 3))
{
- switch(num)
+ switch (num)
{
- case B3_CONSTRAINT_STOP_ERP :
+ case B3_CONSTRAINT_STOP_ERP:
m_linearLimits.m_stopERP[axis] = value;
m_flags |= B3_6DOF_FLAGS_ERP_STOP << (axis * B3_6DOF_FLAGS_AXIS_SHIFT);
break;
- case B3_CONSTRAINT_STOP_CFM :
+ case B3_CONSTRAINT_STOP_CFM:
m_linearLimits.m_stopCFM[axis] = value;
m_flags |= B3_6DOF_FLAGS_CFM_STOP << (axis * B3_6DOF_FLAGS_AXIS_SHIFT);
break;
- case B3_CONSTRAINT_CFM :
+ case B3_CONSTRAINT_CFM:
m_linearLimits.m_normalCFM[axis] = value;
m_flags |= B3_6DOF_FLAGS_CFM_NORM << (axis * B3_6DOF_FLAGS_AXIS_SHIFT);
break;
- default :
+ default:
b3AssertConstrParams(0);
}
}
- else if((axis >=3) && (axis < 6))
+ else if ((axis >= 3) && (axis < 6))
{
- switch(num)
+ switch (num)
{
- case B3_CONSTRAINT_STOP_ERP :
+ case B3_CONSTRAINT_STOP_ERP:
m_angularLimits[axis - 3].m_stopERP = value;
m_flags |= B3_6DOF_FLAGS_ERP_STOP << (axis * B3_6DOF_FLAGS_AXIS_SHIFT);
break;
- case B3_CONSTRAINT_STOP_CFM :
+ case B3_CONSTRAINT_STOP_CFM:
m_angularLimits[axis - 3].m_stopCFM = value;
m_flags |= B3_6DOF_FLAGS_CFM_STOP << (axis * B3_6DOF_FLAGS_AXIS_SHIFT);
break;
- case B3_CONSTRAINT_CFM :
+ case B3_CONSTRAINT_CFM:
m_angularLimits[axis - 3].m_normalCFM = value;
m_flags |= B3_6DOF_FLAGS_CFM_NORM << (axis * B3_6DOF_FLAGS_AXIS_SHIFT);
break;
- default :
+ default:
b3AssertConstrParams(0);
}
}
@@ -734,47 +666,47 @@ void b3Generic6DofConstraint::setParam(int num, b3Scalar value, int axis)
}
}
- ///return the local value of parameter
-b3Scalar b3Generic6DofConstraint::getParam(int num, int axis) const
+///return the local value of parameter
+b3Scalar b3Generic6DofConstraint::getParam(int num, int axis) const
{
b3Scalar retVal = 0;
- if((axis >= 0) && (axis < 3))
+ if ((axis >= 0) && (axis < 3))
{
- switch(num)
+ switch (num)
{
- case B3_CONSTRAINT_STOP_ERP :
+ case B3_CONSTRAINT_STOP_ERP:
b3AssertConstrParams(m_flags & (B3_6DOF_FLAGS_ERP_STOP << (axis * B3_6DOF_FLAGS_AXIS_SHIFT)));
retVal = m_linearLimits.m_stopERP[axis];
break;
- case B3_CONSTRAINT_STOP_CFM :
+ case B3_CONSTRAINT_STOP_CFM:
b3AssertConstrParams(m_flags & (B3_6DOF_FLAGS_CFM_STOP << (axis * B3_6DOF_FLAGS_AXIS_SHIFT)));
retVal = m_linearLimits.m_stopCFM[axis];
break;
- case B3_CONSTRAINT_CFM :
+ case B3_CONSTRAINT_CFM:
b3AssertConstrParams(m_flags & (B3_6DOF_FLAGS_CFM_NORM << (axis * B3_6DOF_FLAGS_AXIS_SHIFT)));
retVal = m_linearLimits.m_normalCFM[axis];
break;
- default :
+ default:
b3AssertConstrParams(0);
}
}
- else if((axis >=3) && (axis < 6))
+ else if ((axis >= 3) && (axis < 6))
{
- switch(num)
+ switch (num)
{
- case B3_CONSTRAINT_STOP_ERP :
+ case B3_CONSTRAINT_STOP_ERP:
b3AssertConstrParams(m_flags & (B3_6DOF_FLAGS_ERP_STOP << (axis * B3_6DOF_FLAGS_AXIS_SHIFT)));
retVal = m_angularLimits[axis - 3].m_stopERP;
break;
- case B3_CONSTRAINT_STOP_CFM :
+ case B3_CONSTRAINT_STOP_CFM:
b3AssertConstrParams(m_flags & (B3_6DOF_FLAGS_CFM_STOP << (axis * B3_6DOF_FLAGS_AXIS_SHIFT)));
retVal = m_angularLimits[axis - 3].m_stopCFM;
break;
- case B3_CONSTRAINT_CFM :
+ case B3_CONSTRAINT_CFM:
b3AssertConstrParams(m_flags & (B3_6DOF_FLAGS_CFM_NORM << (axis * B3_6DOF_FLAGS_AXIS_SHIFT)));
retVal = m_angularLimits[axis - 3].m_normalCFM;
break;
- default :
+ default:
b3AssertConstrParams(0);
}
}
@@ -785,23 +717,21 @@ b3Scalar b3Generic6DofConstraint::getParam(int num, int axis) const
return retVal;
}
-
-
-void b3Generic6DofConstraint::setAxis(const b3Vector3& axis1,const b3Vector3& axis2, const b3RigidBodyData* bodies)
+void b3Generic6DofConstraint::setAxis(const b3Vector3& axis1, const b3Vector3& axis2, const b3RigidBodyData* bodies)
{
b3Vector3 zAxis = axis1.normalized();
b3Vector3 yAxis = axis2.normalized();
- b3Vector3 xAxis = yAxis.cross(zAxis); // we want right coordinate system
-
+ b3Vector3 xAxis = yAxis.cross(zAxis); // we want right coordinate system
+
b3Transform frameInW;
frameInW.setIdentity();
- frameInW.getBasis().setValue( xAxis[0], yAxis[0], zAxis[0],
- xAxis[1], yAxis[1], zAxis[1],
- xAxis[2], yAxis[2], zAxis[2]);
-
+ frameInW.getBasis().setValue(xAxis[0], yAxis[0], zAxis[0],
+ xAxis[1], yAxis[1], zAxis[1],
+ xAxis[2], yAxis[2], zAxis[2]);
+
// now get constraint frame in local coordinate systems
m_frameInA = getCenterOfMassTransform(bodies[m_rbA]).inverse() * frameInW;
m_frameInB = getCenterOfMassTransform(bodies[m_rbB]).inverse() * frameInW;
-
+
calculateTransforms(bodies);
}
generated by cgit v1.2.3 (git 2.25.1) at 2024-12-11 23:55:32 +0000