Bart/bullet3
1
0
Fork 0
Code Issues Pull Requests Releases Wiki Activity

Code-style consistency improvement:

Browse Source 
Apply clang-format-all.sh using the _clang-format file through all the cpp/.h files.
make sure not to apply it to certain serialization structures, since some parser expects the * as part of the name, instead of type.
This commit contains no other changes aside from adding and applying clang-format-all.sh
This commit is contained in:
erwincoumans
2018-09-23 14:17:31 -07:00
parent b73b05e9fb
commit ab8f16961e
1773 changed files with 1081087 additions and 474249 deletions
Download Patch File Download Diff File Expand all files Collapse all files

158
src/Bullet3Dynamics/ConstraintSolver/b3ContactSolverInfo.h
View File

@@ -18,7 +18,7 @@ subject to the following restrictions:
#include "Bullet3Common/b3Scalar.h"
enum b3SolverMode
enum b3SolverMode
{
B3_SOLVER_RANDMIZE_ORDER = 1,
B3_SOLVER_FRICTION_SEPARATE = 2,
@@ -34,45 +34,38 @@ enum b3SolverMode
struct b3ContactSolverInfoData
{
b3Scalar m_tau;
b3Scalar m_damping;//global non-contact constraint damping, can be locally overridden by constraints during 'getInfo2'.
b3Scalar m_friction;
b3Scalar m_timeStep;
b3Scalar m_restitution;
int m_numIterations;
b3Scalar m_maxErrorReduction;
b3Scalar m_sor;
b3Scalar m_erp;//used as Baumgarte factor
b3Scalar m_erp2;//used in Split Impulse
b3Scalar m_globalCfm;//constraint force mixing
int m_splitImpulse;
b3Scalar m_splitImpulsePenetrationThreshold;
b3Scalar m_splitImpulseTurnErp;
b3Scalar m_linearSlop;
b3Scalar m_warmstartingFactor;
int m_solverMode;
int m_restingContactRestitutionThreshold;
int m_minimumSolverBatchSize;
b3Scalar m_maxGyroscopicForce;
b3Scalar m_singleAxisRollingFrictionThreshold;
b3Scalar m_tau;
b3Scalar m_damping; //global non-contact constraint damping, can be locally overridden by constraints during 'getInfo2'.
b3Scalar m_friction;
b3Scalar m_timeStep;
b3Scalar m_restitution;
int m_numIterations;
b3Scalar m_maxErrorReduction;
b3Scalar m_sor;
b3Scalar m_erp; //used as Baumgarte factor
b3Scalar m_erp2; //used in Split Impulse
b3Scalar m_globalCfm; //constraint force mixing
int m_splitImpulse;
b3Scalar m_splitImpulsePenetrationThreshold;
b3Scalar m_splitImpulseTurnErp;
b3Scalar m_linearSlop;
b3Scalar m_warmstartingFactor;
int m_solverMode;
int m_restingContactRestitutionThreshold;
int m_minimumSolverBatchSize;
b3Scalar m_maxGyroscopicForce;
b3Scalar m_singleAxisRollingFrictionThreshold;
};
struct b3ContactSolverInfo : public b3ContactSolverInfoData
{
inline b3ContactSolverInfo()
{
m_tau = b3Scalar(0.6);
m_damping = b3Scalar(1.0);
m_friction = b3Scalar(0.3);
m_timeStep = b3Scalar(1.f/60.f);
m_timeStep = b3Scalar(1.f / 60.f);
m_restitution = b3Scalar(0.);
m_maxErrorReduction = b3Scalar(20.);
m_numIterations = 10;
@@ -84,76 +77,73 @@ struct b3ContactSolverInfo : public b3ContactSolverInfoData
m_splitImpulsePenetrationThreshold = -.04f;
m_splitImpulseTurnErp = 0.1f;
m_linearSlop = b3Scalar(0.0);
m_warmstartingFactor=b3Scalar(0.85);
m_warmstartingFactor = b3Scalar(0.85);
//m_solverMode = B3_SOLVER_USE_WARMSTARTING | B3_SOLVER_SIMD | B3_SOLVER_DISABLE_VELOCITY_DEPENDENT_FRICTION_DIRECTION|B3_SOLVER_USE_2_FRICTION_DIRECTIONS|B3_SOLVER_ENABLE_FRICTION_DIRECTION_CACHING;// | B3_SOLVER_RANDMIZE_ORDER;
m_solverMode = B3_SOLVER_USE_WARMSTARTING | B3_SOLVER_SIMD;// | B3_SOLVER_RANDMIZE_ORDER;
m_restingContactRestitutionThreshold = 2;//unused as of 2.81
m_minimumSolverBatchSize = 128; //try to combine islands until the amount of constraints reaches this limit
m_maxGyroscopicForce = 100.f; ///only used to clamp forces for bodies that have their B3_ENABLE_GYROPSCOPIC_FORCE flag set (using b3RigidBody::setFlag)
m_singleAxisRollingFrictionThreshold = 1e30f;///if the velocity is above this threshold, it will use a single constraint row (axis), otherwise 3 rows.
m_solverMode = B3_SOLVER_USE_WARMSTARTING | B3_SOLVER_SIMD; // | B3_SOLVER_RANDMIZE_ORDER;
m_restingContactRestitutionThreshold = 2; //unused as of 2.81
m_minimumSolverBatchSize = 128; //try to combine islands until the amount of constraints reaches this limit
m_maxGyroscopicForce = 100.f; ///only used to clamp forces for bodies that have their B3_ENABLE_GYROPSCOPIC_FORCE flag set (using b3RigidBody::setFlag)
m_singleAxisRollingFrictionThreshold = 1e30f; ///if the velocity is above this threshold, it will use a single constraint row (axis), otherwise 3 rows.
}
};
///do not change those serialization structures, it requires an updated sBulletDNAstr/sBulletDNAstr64
struct b3ContactSolverInfoDoubleData
{
double m_tau;
double m_damping;//global non-contact constraint damping, can be locally overridden by constraints during 'getInfo2'.
double m_friction;
double m_timeStep;
double m_restitution;
double m_maxErrorReduction;
double m_sor;
double m_erp;//used as Baumgarte factor
double m_erp2;//used in Split Impulse
double m_globalCfm;//constraint force mixing
double m_splitImpulsePenetrationThreshold;
double m_splitImpulseTurnErp;
double m_linearSlop;
double m_warmstartingFactor;
double m_maxGyroscopicForce;
double m_singleAxisRollingFrictionThreshold;
int m_numIterations;
int m_solverMode;
int m_restingContactRestitutionThreshold;
int m_minimumSolverBatchSize;
int m_splitImpulse;
char m_padding[4];
double m_tau;
double m_damping; //global non-contact constraint damping, can be locally overridden by constraints during 'getInfo2'.
double m_friction;
double m_timeStep;
double m_restitution;
double m_maxErrorReduction;
double m_sor;
double m_erp; //used as Baumgarte factor
double m_erp2; //used in Split Impulse
double m_globalCfm; //constraint force mixing
double m_splitImpulsePenetrationThreshold;
double m_splitImpulseTurnErp;
double m_linearSlop;
double m_warmstartingFactor;
double m_maxGyroscopicForce;
double m_singleAxisRollingFrictionThreshold;
int m_numIterations;
int m_solverMode;
int m_restingContactRestitutionThreshold;
int m_minimumSolverBatchSize;
int m_splitImpulse;
char m_padding[4];
};
///do not change those serialization structures, it requires an updated sBulletDNAstr/sBulletDNAstr64
struct b3ContactSolverInfoFloatData
{
float m_tau;
float m_damping;//global non-contact constraint damping, can be locally overridden by constraints during 'getInfo2'.
float m_friction;
float m_timeStep;
float m_tau;
float m_damping; //global non-contact constraint damping, can be locally overridden by constraints during 'getInfo2'.
float m_friction;
float m_timeStep;
float m_restitution;
float m_maxErrorReduction;
float m_sor;
float m_erp;//used as Baumgarte factor
float m_restitution;
float m_maxErrorReduction;
float m_sor;
float m_erp; //used as Baumgarte factor
float m_erp2;//used in Split Impulse
float m_globalCfm;//constraint force mixing
float m_splitImpulsePenetrationThreshold;
float m_splitImpulseTurnErp;
float m_erp2; //used in Split Impulse
float m_globalCfm; //constraint force mixing
float m_splitImpulsePenetrationThreshold;
float m_splitImpulseTurnErp;
float m_linearSlop;
float m_warmstartingFactor;
float m_maxGyroscopicForce;
float m_singleAxisRollingFrictionThreshold;
float m_linearSlop;
float m_warmstartingFactor;
float m_maxGyroscopicForce;
float m_singleAxisRollingFrictionThreshold;
int m_numIterations;
int m_solverMode;
int m_restingContactRestitutionThreshold;
int m_minimumSolverBatchSize;
int m_numIterations;
int m_solverMode;
int m_restingContactRestitutionThreshold;
int m_minimumSolverBatchSize;
int m_splitImpulse;
char m_padding[4];
int m_splitImpulse;
char m_padding[4];
};
#endif //B3_CONTACT_SOLVER_INFO
#endif //B3_CONTACT_SOLVER_INFO

103
src/Bullet3Dynamics/ConstraintSolver/b3FixedConstraint.cpp
View File

@@ -4,105 +4,100 @@
#include "Bullet3Common/b3TransformUtil.h"
#include <new>
b3FixedConstraint::b3FixedConstraint(int rbA,int rbB, const b3Transform& frameInA,const b3Transform& frameInB)
:b3TypedConstraint(B3_FIXED_CONSTRAINT_TYPE,rbA,rbB)
b3FixedConstraint::b3FixedConstraint(int rbA, int rbB, const b3Transform& frameInA, const b3Transform& frameInB)
: b3TypedConstraint(B3_FIXED_CONSTRAINT_TYPE, rbA, rbB)
{
m_pivotInA = frameInA.getOrigin();
m_pivotInB = frameInB.getOrigin();
m_relTargetAB = frameInA.getRotation()*frameInB.getRotation().inverse();
m_relTargetAB = frameInA.getRotation() * frameInB.getRotation().inverse();
}
b3FixedConstraint::~b3FixedConstraint ()
b3FixedConstraint::~b3FixedConstraint()
{
}
void b3FixedConstraint::getInfo1 (b3ConstraintInfo1* info,const b3RigidBodyData* bodies)
void b3FixedConstraint::getInfo1(b3ConstraintInfo1* info, const b3RigidBodyData* bodies)
{
info->m_numConstraintRows = 6;
info->nub = 6;
}
void b3FixedConstraint::getInfo2 (b3ConstraintInfo2* info, const b3RigidBodyData* bodies)
void b3FixedConstraint::getInfo2(b3ConstraintInfo2* info, const b3RigidBodyData* bodies)
{
//fix the 3 linear degrees of freedom
const b3Vector3& worldPosA = bodies[m_rbA].m_pos;
const b3Quaternion& worldOrnA = bodies[m_rbA].m_quat;
const b3Vector3& worldPosB= bodies[m_rbB].m_pos;
const b3Vector3& worldPosB = bodies[m_rbB].m_pos;
const b3Quaternion& worldOrnB = bodies[m_rbB].m_quat;
info->m_J1linearAxis[0] = 1;
info->m_J1linearAxis[info->rowskip+1] = 1;
info->m_J1linearAxis[2*info->rowskip+2] = 1;
info->m_J1linearAxis[info->rowskip + 1] = 1;
info->m_J1linearAxis[2 * info->rowskip + 2] = 1;
b3Vector3 a1 = b3QuatRotate(worldOrnA,m_pivotInA);
b3Vector3 a1 = b3QuatRotate(worldOrnA, m_pivotInA);
{
b3Vector3* angular0 = (b3Vector3*)(info->m_J1angularAxis);
b3Vector3* angular1 = (b3Vector3*)(info->m_J1angularAxis+info->rowskip);
b3Vector3* angular2 = (b3Vector3*)(info->m_J1angularAxis+2*info->rowskip);
b3Vector3* angular1 = (b3Vector3*)(info->m_J1angularAxis + info->rowskip);
b3Vector3* angular2 = (b3Vector3*)(info->m_J1angularAxis + 2 * info->rowskip);
b3Vector3 a1neg = -a1;
a1neg.getSkewSymmetricMatrix(angular0,angular1,angular2);
a1neg.getSkewSymmetricMatrix(angular0, angular1, angular2);
}
if (info->m_J2linearAxis)
{
info->m_J2linearAxis[0] = -1;
info->m_J2linearAxis[info->rowskip+1] = -1;
info->m_J2linearAxis[2*info->rowskip+2] = -1;
info->m_J2linearAxis[info->rowskip + 1] = -1;
info->m_J2linearAxis[2 * info->rowskip + 2] = -1;
}
b3Vector3 a2 = b3QuatRotate(worldOrnB,m_pivotInB);
b3Vector3 a2 = b3QuatRotate(worldOrnB, m_pivotInB);
{
// b3Vector3 a2n = -a2;
// b3Vector3 a2n = -a2;
b3Vector3* angular0 = (b3Vector3*)(info->m_J2angularAxis);
b3Vector3* angular1 = (b3Vector3*)(info->m_J2angularAxis+info->rowskip);
b3Vector3* angular2 = (b3Vector3*)(info->m_J2angularAxis+2*info->rowskip);
a2.getSkewSymmetricMatrix(angular0,angular1,angular2);
b3Vector3* angular1 = (b3Vector3*)(info->m_J2angularAxis + info->rowskip);
b3Vector3* angular2 = (b3Vector3*)(info->m_J2angularAxis + 2 * info->rowskip);
a2.getSkewSymmetricMatrix(angular0, angular1, angular2);
}
// set right hand side for the linear dofs
// set right hand side for the linear dofs
b3Scalar k = info->fps * info->erp;
b3Vector3 linearError = k*(a2+worldPosB-a1-worldPosA);
int j;
for (j=0; j<3; j++)
{
info->m_constraintError[j*info->rowskip] = linearError[j];
b3Vector3 linearError = k * (a2 + worldPosB - a1 - worldPosA);
int j;
for (j = 0; j < 3; j++)
{
info->m_constraintError[j * info->rowskip] = linearError[j];
//printf("info->m_constraintError[%d]=%f\n",j,info->m_constraintError[j]);
}
}
//fix the 3 angular degrees of freedom
//fix the 3 angular degrees of freedom
int start_row = 3;
int s = info->rowskip;
int start_index = start_row * s;
int start_index = start_row * s;
// 3 rows to make body rotations equal
// 3 rows to make body rotations equal
info->m_J1angularAxis[start_index] = 1;
info->m_J1angularAxis[start_index + s + 1] = 1;
info->m_J1angularAxis[start_index + s*2+2] = 1;
if ( info->m_J2angularAxis)
{
info->m_J2angularAxis[start_index] = -1;
info->m_J2angularAxis[start_index + s+1] = -1;
info->m_J2angularAxis[start_index + s*2+2] = -1;
}
info->m_J1angularAxis[start_index + s + 1] = 1;
info->m_J1angularAxis[start_index + s * 2 + 2] = 1;
if (info->m_J2angularAxis)
{
info->m_J2angularAxis[start_index] = -1;
info->m_J2angularAxis[start_index + s + 1] = -1;
info->m_J2angularAxis[start_index + s * 2 + 2] = -1;
}
// set right hand side for the angular dofs
// set right hand side for the angular dofs
b3Vector3 diff;
b3Scalar angle;
b3Quaternion qrelCur = worldOrnA *worldOrnB.inverse();
b3TransformUtil::calculateDiffAxisAngleQuaternion(m_relTargetAB,qrelCur,diff,angle);
diff*=-angle;
for (j=0; j<3; j++)
{
info->m_constraintError[(3+j)*info->rowskip] = k * diff[j];
}
b3Quaternion qrelCur = worldOrnA * worldOrnB.inverse();
b3TransformUtil::calculateDiffAxisAngleQuaternion(m_relTargetAB, qrelCur, diff, angle);
diff *= -angle;
for (j = 0; j < 3; j++)
{
info->m_constraintError[(3 + j) * info->rowskip] = k * diff[j];
}
}

19
src/Bullet3Dynamics/ConstraintSolver/b3FixedConstraint.h
View File

@@ -4,32 +4,31 @@
#include "b3TypedConstraint.h"
B3_ATTRIBUTE_ALIGNED16(class) b3FixedConstraint : public b3TypedConstraint
B3_ATTRIBUTE_ALIGNED16(class)
b3FixedConstraint : public b3TypedConstraint
{
b3Vector3 m_pivotInA;
b3Vector3 m_pivotInB;
b3Quaternion m_relTargetAB;
public:
b3FixedConstraint(int rbA,int rbB, const b3Transform& frameInA,const b3Transform& frameInB);
b3FixedConstraint(int rbA, int rbB, const b3Transform& frameInA, const b3Transform& frameInB);
virtual ~b3FixedConstraint();
virtual void getInfo1 (b3ConstraintInfo1* info,const b3RigidBodyData* bodies);
virtual void getInfo1(b3ConstraintInfo1 * info, const b3RigidBodyData* bodies);
virtual void getInfo2 (b3ConstraintInfo2* info, const b3RigidBodyData* bodies);
virtual void getInfo2(b3ConstraintInfo2 * info, const b3RigidBodyData* bodies);
virtual void setParam(int num, b3Scalar value, int axis = -1)
virtual void setParam(int num, b3Scalar value, int axis = -1)
{
b3Assert(0);
}
virtual b3Scalar getParam(int num, int axis = -1) const
virtual b3Scalar getParam(int num, int axis = -1) const
{
b3Assert(0);
return 0.f;
}
};
#endif //B3_FIXED_CONSTRAINT_H
#endif //B3_FIXED_CONSTRAINT_H

576
src/Bullet3Dynamics/ConstraintSolver/b3Generic6DofConstraint.cpp
View File

@@ -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);
}

567
src/Bullet3Dynamics/ConstraintSolver/b3Generic6DofConstraint.h
View File

@@ -23,7 +23,6 @@ email: projectileman@yahoo.com
http://gimpact.sf.net
*/
#ifndef B3_GENERIC_6DOF_CONSTRAINT_H
#define B3_GENERIC_6DOF_CONSTRAINT_H
@@ -33,88 +32,83 @@ http://gimpact.sf.net
struct b3RigidBodyData;
//! Rotation Limit structure for generic joints
class b3RotationalLimitMotor
{
public:
//! limit_parameters
//!@{
b3Scalar m_loLimit;//!< joint limit
b3Scalar m_hiLimit;//!< joint limit
b3Scalar m_targetVelocity;//!< target motor velocity
b3Scalar m_maxMotorForce;//!< max force on motor
b3Scalar m_maxLimitForce;//!< max force on limit
b3Scalar m_damping;//!< Damping.
b3Scalar m_limitSoftness;//! Relaxation factor
b3Scalar m_normalCFM;//!< Constraint force mixing factor
b3Scalar m_stopERP;//!< Error tolerance factor when joint is at limit
b3Scalar m_stopCFM;//!< Constraint force mixing factor when joint is at limit
b3Scalar m_bounce;//!< restitution factor
bool m_enableMotor;
//! limit_parameters
//!@{
b3Scalar m_loLimit; //!< joint limit
b3Scalar m_hiLimit; //!< joint limit
b3Scalar m_targetVelocity; //!< target motor velocity
b3Scalar m_maxMotorForce; //!< max force on motor
b3Scalar m_maxLimitForce; //!< max force on limit
b3Scalar m_damping; //!< Damping.
b3Scalar m_limitSoftness; //! Relaxation factor
b3Scalar m_normalCFM; //!< Constraint force mixing factor
b3Scalar m_stopERP; //!< Error tolerance factor when joint is at limit
b3Scalar m_stopCFM; //!< Constraint force mixing factor when joint is at limit
b3Scalar m_bounce; //!< restitution factor
bool m_enableMotor;
//!@}
//!@}
//! temp_variables
//!@{
b3Scalar m_currentLimitError;//! How much is violated this limit
b3Scalar m_currentPosition; //! current value of angle
int m_currentLimit;//!< 0=free, 1=at lo limit, 2=at hi limit
b3Scalar m_accumulatedImpulse;
//!@}
//! temp_variables
//!@{
b3Scalar m_currentLimitError; //! How much is violated this limit
b3Scalar m_currentPosition; //! current value of angle
int m_currentLimit; //!< 0=free, 1=at lo limit, 2=at hi limit
b3Scalar m_accumulatedImpulse;
//!@}
b3RotationalLimitMotor()
{
m_accumulatedImpulse = 0.f;
m_targetVelocity = 0;
m_maxMotorForce = 6.0f;
m_maxLimitForce = 300.0f;
m_loLimit = 1.0f;
m_hiLimit = -1.0f;
b3RotationalLimitMotor()
{
m_accumulatedImpulse = 0.f;
m_targetVelocity = 0;
m_maxMotorForce = 6.0f;
m_maxLimitForce = 300.0f;
m_loLimit = 1.0f;
m_hiLimit = -1.0f;
m_normalCFM = 0.f;
m_stopERP = 0.2f;
m_stopCFM = 0.f;
m_bounce = 0.0f;
m_damping = 1.0f;
m_limitSoftness = 0.5f;
m_currentLimit = 0;
m_currentLimitError = 0;
m_enableMotor = false;
}
m_bounce = 0.0f;
m_damping = 1.0f;
m_limitSoftness = 0.5f;
m_currentLimit = 0;
m_currentLimitError = 0;
m_enableMotor = false;
}
b3RotationalLimitMotor(const b3RotationalLimitMotor & 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;
b3RotationalLimitMotor(const b3RotationalLimitMotor& 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_normalCFM = limot.m_normalCFM;
m_stopERP = limot.m_stopERP;
m_stopCFM = limot.m_stopCFM;
m_bounce = limot.m_bounce;
m_currentLimit = limot.m_currentLimit;
m_currentLimitError = limot.m_currentLimitError;
m_enableMotor = limot.m_enableMotor;
}
m_stopCFM = limot.m_stopCFM;
m_bounce = limot.m_bounce;
m_currentLimit = limot.m_currentLimit;
m_currentLimitError = limot.m_currentLimitError;
m_enableMotor = limot.m_enableMotor;
}
//! Is limited
bool isLimited()
{
if(m_loLimit > m_hiLimit) return false;
return true;
}
bool isLimited()
{
if (m_loLimit > m_hiLimit) return false;
return true;
}
//! Need apply correction
bool needApplyTorques()
{
if(m_currentLimit == 0 && m_enableMotor == false) return false;
return true;
}
bool needApplyTorques()
{
if (m_currentLimit == 0 && m_enableMotor == false) return false;
return true;
}
//! calculates error
/*!
@@ -123,104 +117,98 @@ public:
int testLimitValue(b3Scalar test_value);
//! apply the correction impulses for two bodies
b3Scalar solveAngularLimits(b3Scalar timeStep,b3Vector3& axis, b3Scalar jacDiagABInv,b3RigidBodyData * body0, b3RigidBodyData * body1);
b3Scalar solveAngularLimits(b3Scalar timeStep, b3Vector3& axis, b3Scalar jacDiagABInv, b3RigidBodyData* body0, b3RigidBodyData* body1);
};
class b3TranslationalLimitMotor
{
public:
b3Vector3 m_lowerLimit;//!< the constraint lower limits
b3Vector3 m_upperLimit;//!< the constraint upper limits
b3Vector3 m_accumulatedImpulse;
//! Linear_Limit_parameters
//!@{
b3Vector3 m_normalCFM;//!< Constraint force mixing factor
b3Vector3 m_stopERP;//!< Error tolerance factor when joint is at limit
b3Vector3 m_stopCFM;//!< Constraint force mixing factor when joint is at limit
b3Vector3 m_targetVelocity;//!< target motor velocity
b3Vector3 m_maxMotorForce;//!< max force on motor
b3Vector3 m_currentLimitError;//! How much is violated this limit
b3Vector3 m_currentLinearDiff;//! Current relative offset of constraint frames
b3Scalar m_limitSoftness;//!< Softness for linear limit
b3Scalar m_damping;//!< Damping for linear limit
b3Scalar m_restitution;//! Bounce parameter for linear limit
b3Vector3 m_lowerLimit; //!< the constraint lower limits
b3Vector3 m_upperLimit; //!< the constraint upper limits
b3Vector3 m_accumulatedImpulse;
//! Linear_Limit_parameters
//!@{
b3Vector3 m_normalCFM; //!< Constraint force mixing factor
b3Vector3 m_stopERP; //!< Error tolerance factor when joint is at limit
b3Vector3 m_stopCFM; //!< Constraint force mixing factor when joint is at limit
b3Vector3 m_targetVelocity; //!< target motor velocity
b3Vector3 m_maxMotorForce; //!< max force on motor
b3Vector3 m_currentLimitError; //! How much is violated this limit
b3Vector3 m_currentLinearDiff; //! Current relative offset of constraint frames
b3Scalar m_limitSoftness; //!< Softness for linear limit
b3Scalar m_damping; //!< Damping for linear limit
b3Scalar m_restitution; //! Bounce parameter for linear limit
//!@}
bool m_enableMotor[3];
int m_currentLimit[3];//!< 0=free, 1=at lower limit, 2=at upper limit
bool m_enableMotor[3];
int m_currentLimit[3]; //!< 0=free, 1=at lower limit, 2=at upper limit
b3TranslationalLimitMotor()
{
m_lowerLimit.setValue(0.f,0.f,0.f);
m_upperLimit.setValue(0.f,0.f,0.f);
m_accumulatedImpulse.setValue(0.f,0.f,0.f);
b3TranslationalLimitMotor()
{
m_lowerLimit.setValue(0.f, 0.f, 0.f);
m_upperLimit.setValue(0.f, 0.f, 0.f);
m_accumulatedImpulse.setValue(0.f, 0.f, 0.f);
m_normalCFM.setValue(0.f, 0.f, 0.f);
m_stopERP.setValue(0.2f, 0.2f, 0.2f);
m_stopCFM.setValue(0.f, 0.f, 0.f);
m_limitSoftness = 0.7f;
m_damping = b3Scalar(1.0f);
m_restitution = b3Scalar(0.5f);
for(int i=0; i < 3; i++)
m_limitSoftness = 0.7f;
m_damping = b3Scalar(1.0f);
m_restitution = b3Scalar(0.5f);
for (int i = 0; i < 3; i++)
{
m_enableMotor[i] = false;
m_targetVelocity[i] = b3Scalar(0.f);
m_maxMotorForce[i] = b3Scalar(0.f);
}
}
}
b3TranslationalLimitMotor(const b3TranslationalLimitMotor & other )
{
m_lowerLimit = other.m_lowerLimit;
m_upperLimit = other.m_upperLimit;
m_accumulatedImpulse = other.m_accumulatedImpulse;
b3TranslationalLimitMotor(const b3TranslationalLimitMotor& other)
{
m_lowerLimit = other.m_lowerLimit;
m_upperLimit = other.m_upperLimit;
m_accumulatedImpulse = other.m_accumulatedImpulse;
m_limitSoftness = other.m_limitSoftness ;
m_damping = other.m_damping;
m_restitution = other.m_restitution;
m_limitSoftness = other.m_limitSoftness;
m_damping = other.m_damping;
m_restitution = other.m_restitution;
m_normalCFM = other.m_normalCFM;
m_stopERP = other.m_stopERP;
m_stopCFM = other.m_stopCFM;
for(int i=0; i < 3; i++)
for (int i = 0; i < 3; i++)
{
m_enableMotor[i] = other.m_enableMotor[i];
m_targetVelocity[i] = other.m_targetVelocity[i];
m_maxMotorForce[i] = other.m_maxMotorForce[i];
}
}
}
//! Test limit
//! Test limit
/*!
- free means upper < lower,
- locked means upper == lower
- limited means upper > lower
- limitIndex: first 3 are linear, next 3 are angular
*/
inline bool isLimited(int limitIndex)
{
return (m_upperLimit[limitIndex] >= m_lowerLimit[limitIndex]);
}
inline bool needApplyForce(int limitIndex)
{
if(m_currentLimit[limitIndex] == 0 && m_enableMotor[limitIndex] == false) return false;
return true;
}
inline bool isLimited(int limitIndex)
{
return (m_upperLimit[limitIndex] >= m_lowerLimit[limitIndex]);
}
inline bool needApplyForce(int limitIndex)
{
if (m_currentLimit[limitIndex] == 0 && m_enableMotor[limitIndex] == false) return false;
return true;
}
int testLimitValue(int limitIndex, b3Scalar test_value);
b3Scalar solveLinearAxis(
b3Scalar timeStep,
b3Scalar jacDiagABInv,
b3RigidBodyData& body1,const b3Vector3 &pointInA,
b3RigidBodyData& body2,const b3Vector3 &pointInB,
int limit_index,
const b3Vector3 & axis_normal_on_a,
const b3Vector3 & anchorPos);
b3Scalar solveLinearAxis(
b3Scalar timeStep,
b3Scalar jacDiagABInv,
b3RigidBodyData& body1, const b3Vector3& pointInA,
b3RigidBodyData& body2, const b3Vector3& pointInB,
int limit_index,
const b3Vector3& axis_normal_on_a,
const b3Vector3& anchorPos);
};
enum b36DofFlags
@@ -229,8 +217,7 @@ enum b36DofFlags
B3_6DOF_FLAGS_CFM_STOP = 2,
B3_6DOF_FLAGS_ERP_STOP = 4
};
#define B3_6DOF_FLAGS_AXIS_SHIFT 3 // bits per axis
#define B3_6DOF_FLAGS_AXIS_SHIFT 3 // bits per axis
/// b3Generic6DofConstraint between two rigidbodies each with a pivotpoint that descibes the axis location in local space
/*!
@@ -268,240 +255,229 @@ This brings support for limit parameters and motors. </li>
</ul>
*/
B3_ATTRIBUTE_ALIGNED16(class) b3Generic6DofConstraint : public b3TypedConstraint
B3_ATTRIBUTE_ALIGNED16(class)
b3Generic6DofConstraint : public b3TypedConstraint
{
protected:
//! relative_frames
//!@{
b3Transform m_frameInA;//!< the constraint space w.r.t body A
b3Transform m_frameInB;//!< the constraint space w.r.t body B
//!@}
//! Jacobians
//!@{
// b3JacobianEntry m_jacLinear[3];//!< 3 orthogonal linear constraints
// b3JacobianEntry m_jacAng[3];//!< 3 orthogonal angular constraints
//!@}
//! Linear_Limit_parameters
//!@{
b3TranslationalLimitMotor m_linearLimits;
//!@}
//! hinge_parameters
//!@{
b3RotationalLimitMotor m_angularLimits[3];
//!@{
b3Transform m_frameInA; //!< the constraint space w.r.t body A
b3Transform m_frameInB; //!< the constraint space w.r.t body B
//!@}
//! Jacobians
//!@{
// b3JacobianEntry m_jacLinear[3];//!< 3 orthogonal linear constraints
// b3JacobianEntry m_jacAng[3];//!< 3 orthogonal angular constraints
//!@}
//! Linear_Limit_parameters
//!@{
b3TranslationalLimitMotor m_linearLimits;
//!@}
//! hinge_parameters
//!@{
b3RotationalLimitMotor m_angularLimits[3];
//!@}
protected:
//! temporal variables
//!@{
b3Transform m_calculatedTransformA;
b3Transform m_calculatedTransformB;
b3Vector3 m_calculatedAxisAngleDiff;
b3Vector3 m_calculatedAxis[3];
b3Vector3 m_calculatedLinearDiff;
b3Scalar m_timeStep;
b3Scalar m_factA;
b3Scalar m_factB;
bool m_hasStaticBody;
b3Vector3 m_AnchorPos; // point betwen pivots of bodies A and B to solve linear axes
//! temporal variables
//!@{
b3Transform m_calculatedTransformA;
b3Transform m_calculatedTransformB;
b3Vector3 m_calculatedAxisAngleDiff;
b3Vector3 m_calculatedAxis[3];
b3Vector3 m_calculatedLinearDiff;
b3Scalar m_timeStep;
b3Scalar m_factA;
b3Scalar m_factB;
bool m_hasStaticBody;
bool m_useLinearReferenceFrameA;
bool m_useOffsetForConstraintFrame;
int m_flags;
b3Vector3 m_AnchorPos; // point betwen pivots of bodies A and B to solve linear axes
//!@}
bool m_useLinearReferenceFrameA;
bool m_useOffsetForConstraintFrame;
b3Generic6DofConstraint& operator=(b3Generic6DofConstraint& other)
{
b3Assert(0);
(void) other;
return *this;
}
int m_flags;
//!@}
int 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& operator=(b3Generic6DofConstraint& other)
{
b3Assert(0);
(void)other;
return *this;
}
int setLinearLimits(b3ConstraintInfo2 *info, int row, const b3Transform& transA,const b3Transform& transB,const b3Vector3& linVelA,const b3Vector3& linVelB,const b3Vector3& angVelA,const b3Vector3& angVelB);
int 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 setLinearLimits(b3ConstraintInfo2 * info, int row, const b3Transform& transA, const b3Transform& transB, const b3Vector3& linVelA, const b3Vector3& linVelB, const b3Vector3& angVelA, const b3Vector3& angVelB);
// tests linear limits
void calculateLinearInfo();
//! calcs the euler angles between the two bodies.
void calculateAngleInfo();
void calculateAngleInfo();
public:
B3_DECLARE_ALIGNED_ALLOCATOR();
b3Generic6DofConstraint(int rbA, int rbB, const b3Transform& frameInA, const b3Transform& frameInB ,bool useLinearReferenceFrameA,const b3RigidBodyData* bodies);
b3Generic6DofConstraint(int rbA, int rbB, const b3Transform& frameInA, const b3Transform& frameInB, bool useLinearReferenceFrameA, const b3RigidBodyData* bodies);
//! 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 b3Generic6DofConstraint.getCalculatedTransformA , b3Generic6DofConstraint.getCalculatedTransformB, b3Generic6DofConstraint.calculateAngleInfo
*/
void calculateTransforms(const b3Transform& transA,const b3Transform& transB,const b3RigidBodyData* bodies);
void calculateTransforms(const b3Transform& transA, const b3Transform& transB, const b3RigidBodyData* bodies);
void calculateTransforms(const b3RigidBodyData* bodies);
//! Gets the global transform of the offset for body A
/*!
/*!
\sa b3Generic6DofConstraint.getFrameOffsetA, b3Generic6DofConstraint.getFrameOffsetB, b3Generic6DofConstraint.calculateAngleInfo.
*/
const b3Transform & getCalculatedTransformA() const
{
return m_calculatedTransformA;
}
const b3Transform& getCalculatedTransformA() const
{
return m_calculatedTransformA;
}
//! Gets the global transform of the offset for body B
/*!
//! Gets the global transform of the offset for body B
/*!
\sa b3Generic6DofConstraint.getFrameOffsetA, b3Generic6DofConstraint.getFrameOffsetB, b3Generic6DofConstraint.calculateAngleInfo.
*/
const b3Transform & getCalculatedTransformB() const
{
return m_calculatedTransformB;
}
const b3Transform& getCalculatedTransformB() const
{
return m_calculatedTransformB;
}
const b3Transform & getFrameOffsetA() const
{
return m_frameInA;
}
const b3Transform& getFrameOffsetA() const
{
return m_frameInA;
}
const b3Transform & getFrameOffsetB() const
{
return m_frameInB;
}
const b3Transform& getFrameOffsetB() const
{
return m_frameInB;
}
b3Transform& getFrameOffsetA()
{
return m_frameInA;
}
b3Transform & getFrameOffsetA()
{
return m_frameInA;
}
b3Transform& getFrameOffsetB()
{
return m_frameInB;
}
b3Transform & getFrameOffsetB()
{
return m_frameInB;
}
virtual void getInfo1(b3ConstraintInfo1 * info, const b3RigidBodyData* bodies);
void getInfo1NonVirtual(b3ConstraintInfo1 * info, const b3RigidBodyData* bodies);
virtual void getInfo2(b3ConstraintInfo2 * info, const b3RigidBodyData* bodies);
virtual void getInfo1 (b3ConstraintInfo1* info,const b3RigidBodyData* bodies);
void 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 getInfo1NonVirtual (b3ConstraintInfo1* info,const b3RigidBodyData* bodies);
virtual void getInfo2 (b3ConstraintInfo2* info,const b3RigidBodyData* bodies);
void 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 updateRHS(b3Scalar timeStep);
void updateRHS(b3Scalar timeStep);
//! Get the rotation axis in global coordinates
b3Vector3 getAxis(int axis_index) const;
b3Vector3 getAxis(int axis_index) const;
//! Get the relative Euler angle
/*!
//! Get the relative Euler angle
/*!
\pre b3Generic6DofConstraint::calculateTransforms() must be called previously.
*/
b3Scalar getAngle(int axis_index) const;
b3Scalar getAngle(int axis_index) const;
//! Get the relative position of the constraint pivot
/*!
/*!
\pre b3Generic6DofConstraint::calculateTransforms() must be called previously.
*/
b3Scalar getRelativePivotPosition(int axis_index) const;
void setFrames(const b3Transform & frameA, const b3Transform & frameB, const b3RigidBodyData* bodies);
void setFrames(const b3Transform& frameA, const b3Transform& frameB, const b3RigidBodyData* bodies);
//! Test angular limit.
/*!
Calculates angular correction and returns true if limit needs to be corrected.
\pre b3Generic6DofConstraint::calculateTransforms() must be called previously.
*/
bool testAngularLimitMotor(int axis_index);
bool testAngularLimitMotor(int axis_index);
void setLinearLowerLimit(const b3Vector3& linearLower)
{
m_linearLimits.m_lowerLimit = linearLower;
}
void setLinearLowerLimit(const b3Vector3& linearLower)
{
m_linearLimits.m_lowerLimit = linearLower;
}
void getLinearLowerLimit(b3Vector3& linearLower)
void getLinearLowerLimit(b3Vector3 & linearLower)
{
linearLower = m_linearLimits.m_lowerLimit;
}
void setLinearUpperLimit(const b3Vector3& linearUpper)
void setLinearUpperLimit(const b3Vector3& linearUpper)
{
m_linearLimits.m_upperLimit = linearUpper;
}
void getLinearUpperLimit(b3Vector3& linearUpper)
void getLinearUpperLimit(b3Vector3 & linearUpper)
{
linearUpper = m_linearLimits.m_upperLimit;
}
void setAngularLowerLimit(const b3Vector3& angularLower)
{
for(int i = 0; i < 3; i++)
m_angularLimits[i].m_loLimit = b3NormalizeAngle(angularLower[i]);
}
void getAngularLowerLimit(b3Vector3& angularLower)
void setAngularLowerLimit(const b3Vector3& angularLower)
{
for(int i = 0; i < 3; i++)
for (int i = 0; i < 3; i++)
m_angularLimits[i].m_loLimit = b3NormalizeAngle(angularLower[i]);
}
void getAngularLowerLimit(b3Vector3 & angularLower)
{
for (int i = 0; i < 3; i++)
angularLower[i] = m_angularLimits[i].m_loLimit;
}
void setAngularUpperLimit(const b3Vector3& angularUpper)
{
for(int i = 0; i < 3; i++)
m_angularLimits[i].m_hiLimit = b3NormalizeAngle(angularUpper[i]);
}
void getAngularUpperLimit(b3Vector3& angularUpper)
void setAngularUpperLimit(const b3Vector3& angularUpper)
{
for(int i = 0; i < 3; i++)
for (int i = 0; i < 3; i++)
m_angularLimits[i].m_hiLimit = b3NormalizeAngle(angularUpper[i]);
}
void getAngularUpperLimit(b3Vector3 & angularUpper)
{
for (int i = 0; i < 3; i++)
angularUpper[i] = m_angularLimits[i].m_hiLimit;
}
//! Retrieves the angular limit informacion
b3RotationalLimitMotor * getRotationalLimitMotor(int index)
{
return &m_angularLimits[index];
}
b3RotationalLimitMotor* getRotationalLimitMotor(int index)
{
return &m_angularLimits[index];
}
//! Retrieves the limit informacion
b3TranslationalLimitMotor * getTranslationalLimitMotor()
{
return &m_linearLimits;
}
//! Retrieves the limit informacion
b3TranslationalLimitMotor* getTranslationalLimitMotor()
{
return &m_linearLimits;
}
//first 3 are linear, next 3 are angular
void setLimit(int axis, b3Scalar lo, b3Scalar hi)
{
if(axis<3)
{
m_linearLimits.m_lowerLimit[axis] = lo;
m_linearLimits.m_upperLimit[axis] = hi;
}
else
{
//first 3 are linear, next 3 are angular
void setLimit(int axis, b3Scalar lo, b3Scalar hi)
{
if (axis < 3)
{
m_linearLimits.m_lowerLimit[axis] = lo;
m_linearLimits.m_upperLimit[axis] = hi;
}
else
{
lo = b3NormalizeAngle(lo);
hi = b3NormalizeAngle(hi);
m_angularLimits[axis-3].m_loLimit = lo;
m_angularLimits[axis-3].m_hiLimit = hi;
}
}
m_angularLimits[axis - 3].m_loLimit = lo;
m_angularLimits[axis - 3].m_hiLimit = hi;
}
}
//! Test limit
/*!
@@ -510,41 +486,32 @@ public:
- limited means upper > lower
- limitIndex: first 3 are linear, next 3 are angular
*/
bool isLimited(int limitIndex)
{
if(limitIndex<3)
{
bool isLimited(int limitIndex)
{
if (limitIndex < 3)
{
return m_linearLimits.isLimited(limitIndex);
}
return m_angularLimits[limitIndex - 3].isLimited();
}
}
return m_angularLimits[limitIndex-3].isLimited();
}
virtual void calcAnchorPos(const b3RigidBodyData* bodies); // overridable
virtual void calcAnchorPos(const b3RigidBodyData* bodies); // overridable
int 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 = false);
int 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 = false);
// access for UseFrameOffset
bool getUseFrameOffset() { return m_useOffsetForConstraintFrame; }
void setUseFrameOffset(bool frameOffsetOnOff) { m_useOffsetForConstraintFrame = frameOffsetOnOff; }
///override the default global value of a parameter (such as ERP or CFM), optionally provide the axis (0..5).
///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.
virtual void setParam(int num, b3Scalar value, int axis = -1);
virtual void setParam(int num, b3Scalar value, int axis = -1);
///return the local value of parameter
virtual b3Scalar getParam(int num, int axis = -1) const;
virtual b3Scalar getParam(int num, int axis = -1) const;
void setAxis( const b3Vector3& axis1, const b3Vector3& axis2,const b3RigidBodyData* bodies);
void setAxis(const b3Vector3& axis1, const b3Vector3& axis2, const b3RigidBodyData* bodies);
};
#endif //B3_GENERIC_6DOF_CONSTRAINT_H
#endif //B3_GENERIC_6DOF_CONSTRAINT_H

103
src/Bullet3Dynamics/ConstraintSolver/b3JacobianEntry.h
View File

@@ -18,7 +18,6 @@ subject to the following restrictions:
#include "Bullet3Common/b3Matrix3x3.h"
//notes:
// Another memory optimization would be to store m_1MinvJt in the remaining 3 w components
// which makes the b3JacobianEntry memory layout 16 bytes
@@ -27,25 +26,26 @@ subject to the following restrictions:
/// Jacobian entry is an abstraction that allows to describe constraints
/// it can be used in combination with a constraint solver
/// Can be used to relate the effect of an impulse to the constraint error
B3_ATTRIBUTE_ALIGNED16(class) b3JacobianEntry
B3_ATTRIBUTE_ALIGNED16(class)
b3JacobianEntry
{
public:
b3JacobianEntry() {};
b3JacobianEntry(){};
//constraint between two different rigidbodies
b3JacobianEntry(
const b3Matrix3x3& world2A,
const b3Matrix3x3& world2B,
const b3Vector3& rel_pos1,const b3Vector3& rel_pos2,
const b3Vector3& rel_pos1, const b3Vector3& rel_pos2,
const b3Vector3& jointAxis,
const b3Vector3& inertiaInvA,
const b3Vector3& inertiaInvA,
const b3Scalar massInvA,
const b3Vector3& inertiaInvB,
const b3Scalar massInvB)
:m_linearJointAxis(jointAxis)
: m_linearJointAxis(jointAxis)
{
m_aJ = world2A*(rel_pos1.cross(m_linearJointAxis));
m_bJ = world2B*(rel_pos2.cross(-m_linearJointAxis));
m_0MinvJt = inertiaInvA * m_aJ;
m_aJ = world2A * (rel_pos1.cross(m_linearJointAxis));
m_bJ = world2B * (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);
@@ -54,33 +54,31 @@ public:
//angular constraint between two different rigidbodies
b3JacobianEntry(const b3Vector3& jointAxis,
const b3Matrix3x3& world2A,
const b3Matrix3x3& world2B,
const b3Vector3& inertiaInvA,
const b3Vector3& inertiaInvB)
:m_linearJointAxis(b3MakeVector3(b3Scalar(0.),b3Scalar(0.),b3Scalar(0.)))
const b3Matrix3x3& world2A,
const b3Matrix3x3& world2B,
const b3Vector3& inertiaInvA,
const b3Vector3& inertiaInvB)
: m_linearJointAxis(b3MakeVector3(b3Scalar(0.), b3Scalar(0.), b3Scalar(0.)))
{
m_aJ= world2A*jointAxis;
m_bJ = world2B*-jointAxis;
m_0MinvJt = inertiaInvA * m_aJ;
m_aJ = world2A * jointAxis;
m_bJ = world2B * -jointAxis;
m_0MinvJt = inertiaInvA * m_aJ;
m_1MinvJt = inertiaInvB * m_bJ;
m_Adiag = m_0MinvJt.dot(m_aJ) + m_1MinvJt.dot(m_bJ);
m_Adiag = m_0MinvJt.dot(m_aJ) + m_1MinvJt.dot(m_bJ);
b3Assert(m_Adiag > b3Scalar(0.0));
}
//angular constraint between two different rigidbodies
b3JacobianEntry(const b3Vector3& axisInA,
const b3Vector3& axisInB,
const b3Vector3& inertiaInvA,
const b3Vector3& inertiaInvB)
: m_linearJointAxis(b3MakeVector3(b3Scalar(0.),b3Scalar(0.),b3Scalar(0.)))
, m_aJ(axisInA)
, m_bJ(-axisInB)
const b3Vector3& axisInB,
const b3Vector3& inertiaInvA,
const b3Vector3& inertiaInvB)
: m_linearJointAxis(b3MakeVector3(b3Scalar(0.), b3Scalar(0.), b3Scalar(0.))), m_aJ(axisInA), m_bJ(-axisInB)
{
m_0MinvJt = inertiaInvA * m_aJ;
m_0MinvJt = inertiaInvA * m_aJ;
m_1MinvJt = inertiaInvB * m_bJ;
m_Adiag = m_0MinvJt.dot(m_aJ) + m_1MinvJt.dot(m_bJ);
m_Adiag = m_0MinvJt.dot(m_aJ) + m_1MinvJt.dot(m_bJ);
b3Assert(m_Adiag > b3Scalar(0.0));
}
@@ -88,25 +86,25 @@ public:
//constraint on one rigidbody
b3JacobianEntry(
const b3Matrix3x3& world2A,
const b3Vector3& rel_pos1,const b3Vector3& rel_pos2,
const b3Vector3& rel_pos1, const b3Vector3& rel_pos2,
const b3Vector3& jointAxis,
const b3Vector3& inertiaInvA,
const b3Vector3& inertiaInvA,
const b3Scalar massInvA)
:m_linearJointAxis(jointAxis)
: m_linearJointAxis(jointAxis)
{
m_aJ= world2A*(rel_pos1.cross(jointAxis));
m_bJ = world2A*(rel_pos2.cross(-jointAxis));
m_0MinvJt = inertiaInvA * m_aJ;
m_1MinvJt = b3MakeVector3(b3Scalar(0.),b3Scalar(0.),b3Scalar(0.));
m_aJ = world2A * (rel_pos1.cross(jointAxis));
m_bJ = world2A * (rel_pos2.cross(-jointAxis));
m_0MinvJt = inertiaInvA * m_aJ;
m_1MinvJt = b3MakeVector3(b3Scalar(0.), b3Scalar(0.), b3Scalar(0.));
m_Adiag = massInvA + m_0MinvJt.dot(m_aJ);
b3Assert(m_Adiag > b3Scalar(0.0));
}
b3Scalar getDiagonal() const { return m_Adiag; }
b3Scalar getDiagonal() const { return m_Adiag; }
// for two constraints on the same rigidbody (for example vehicle friction)
b3Scalar getNonDiagonal(const b3JacobianEntry& jacB, const b3Scalar massInvA) const
b3Scalar getNonDiagonal(const b3JacobianEntry& jacB, const b3Scalar massInvA) const
{
const b3JacobianEntry& jacA = *this;
b3Scalar lin = massInvA * jacA.m_linearJointAxis.dot(jacB.m_linearJointAxis);
@@ -114,42 +112,39 @@ public:
return lin + ang;
}
// for two constraints on sharing two same rigidbodies (for example two contact points between two rigidbodies)
b3Scalar getNonDiagonal(const b3JacobianEntry& jacB,const b3Scalar massInvA,const b3Scalar massInvB) const
b3Scalar getNonDiagonal(const b3JacobianEntry& jacB, const b3Scalar massInvA, const b3Scalar massInvB) const
{
const b3JacobianEntry& jacA = *this;
b3Vector3 lin = jacA.m_linearJointAxis * jacB.m_linearJointAxis;
b3Vector3 ang0 = jacA.m_0MinvJt * jacB.m_aJ;
b3Vector3 ang1 = jacA.m_1MinvJt * jacB.m_bJ;
b3Vector3 lin0 = massInvA * lin ;
b3Vector3 lin0 = massInvA * lin;
b3Vector3 lin1 = massInvB * lin;
b3Vector3 sum = ang0+ang1+lin0+lin1;
return sum[0]+sum[1]+sum[2];
b3Vector3 sum = ang0 + ang1 + lin0 + lin1;
return sum[0] + sum[1] + sum[2];
}
b3Scalar getRelativeVelocity(const b3Vector3& linvelA,const b3Vector3& angvelA,const b3Vector3& linvelB,const b3Vector3& angvelB)
b3Scalar getRelativeVelocity(const b3Vector3& linvelA, const b3Vector3& angvelA, const b3Vector3& linvelB, const b3Vector3& angvelB)
{
b3Vector3 linrel = linvelA - linvelB;
b3Vector3 angvela = angvelA * m_aJ;
b3Vector3 angvelb = angvelB * m_bJ;
b3Vector3 angvela = angvelA * m_aJ;
b3Vector3 angvelb = angvelB * m_bJ;
linrel *= m_linearJointAxis;
angvela += angvelb;
angvela += linrel;
b3Scalar rel_vel2 = angvela[0]+angvela[1]+angvela[2];
b3Scalar rel_vel2 = angvela[0] + angvela[1] + angvela[2];
return rel_vel2 + B3_EPSILON;
}
//private:
//private:
b3Vector3 m_linearJointAxis;
b3Vector3 m_aJ;
b3Vector3 m_bJ;
b3Vector3 m_0MinvJt;
b3Vector3 m_1MinvJt;
b3Vector3 m_linearJointAxis;
b3Vector3 m_aJ;
b3Vector3 m_bJ;
b3Vector3 m_0MinvJt;
b3Vector3 m_1MinvJt;
//Optimization: can be stored in the w/last component of one of the vectors
b3Scalar m_Adiag;
b3Scalar m_Adiag;
};
#endif //B3_JACOBIAN_ENTRY_H
#endif //B3_JACOBIAN_ENTRY_H

1429
src/Bullet3Dynamics/ConstraintSolver/b3PgsJacobiSolver.cpp
View File

File diff suppressed because it is too large Load Diff

146
src/Bullet3Dynamics/ConstraintSolver/b3PgsJacobiSolver.h
View File

@@ -1,11 +1,9 @@
#ifndef B3_PGS_JACOBI_SOLVER
#define B3_PGS_JACOBI_SOLVER
struct b3Contact4;
struct b3ContactPoint;
class b3Dispatcher;
#include "b3TypedConstraint.h"
@@ -18,132 +16,118 @@ struct b3InertiaData;
class b3PgsJacobiSolver
{
protected:
b3AlignedObjectArray<b3SolverBody> m_tmpSolverBodyPool;
b3ConstraintArray m_tmpSolverContactConstraintPool;
b3ConstraintArray m_tmpSolverNonContactConstraintPool;
b3ConstraintArray m_tmpSolverContactFrictionConstraintPool;
b3ConstraintArray m_tmpSolverContactRollingFrictionConstraintPool;
b3AlignedObjectArray<b3SolverBody> m_tmpSolverBodyPool;
b3ConstraintArray m_tmpSolverContactConstraintPool;
b3ConstraintArray m_tmpSolverNonContactConstraintPool;
b3ConstraintArray m_tmpSolverContactFrictionConstraintPool;
b3ConstraintArray m_tmpSolverContactRollingFrictionConstraintPool;
b3AlignedObjectArray<int> m_orderTmpConstraintPool;
b3AlignedObjectArray<int> m_orderNonContactConstraintPool;
b3AlignedObjectArray<int> m_orderFrictionConstraintPool;
b3AlignedObjectArray<int> m_orderTmpConstraintPool;
b3AlignedObjectArray<int> m_orderNonContactConstraintPool;
b3AlignedObjectArray<int> m_orderFrictionConstraintPool;
b3AlignedObjectArray<b3TypedConstraint::b3ConstraintInfo1> m_tmpConstraintSizesPool;
b3AlignedObjectArray<int> m_bodyCount;
b3AlignedObjectArray<int> m_bodyCountCheck;
b3AlignedObjectArray<b3Vector3> m_deltaLinearVelocities;
b3AlignedObjectArray<b3Vector3> m_deltaAngularVelocities;
bool m_usePgs;
void averageVelocities();
b3AlignedObjectArray<int> m_bodyCount;
b3AlignedObjectArray<int> m_bodyCountCheck;
int m_maxOverrideNumSolverIterations;
b3AlignedObjectArray<b3Vector3> m_deltaLinearVelocities;
b3AlignedObjectArray<b3Vector3> m_deltaAngularVelocities;
int m_numSplitImpulseRecoveries;
bool m_usePgs;
void averageVelocities();
b3Scalar getContactProcessingThreshold(b3Contact4* contact)
int m_maxOverrideNumSolverIterations;
int m_numSplitImpulseRecoveries;
b3Scalar getContactProcessingThreshold(b3Contact4* contact)
{
return 0.02f;
}
void setupFrictionConstraint( b3RigidBodyData* bodies,b3InertiaData* inertias, b3SolverConstraint& solverConstraint, const b3Vector3& normalAxis,int solverBodyIdA,int solverBodyIdB,
b3ContactPoint& cp,const b3Vector3& rel_pos1,const b3Vector3& rel_pos2,
b3RigidBodyData* colObj0,b3RigidBodyData* colObj1, b3Scalar relaxation,
b3Scalar desiredVelocity=0., b3Scalar cfmSlip=0.);
void setupFrictionConstraint(b3RigidBodyData* bodies, b3InertiaData* inertias, b3SolverConstraint& solverConstraint, const b3Vector3& normalAxis, int solverBodyIdA, int solverBodyIdB,
b3ContactPoint& cp, const b3Vector3& rel_pos1, const b3Vector3& rel_pos2,
b3RigidBodyData* colObj0, b3RigidBodyData* colObj1, b3Scalar relaxation,
b3Scalar desiredVelocity = 0., b3Scalar cfmSlip = 0.);
void setupRollingFrictionConstraint(b3RigidBodyData* bodies,b3InertiaData* inertias, b3SolverConstraint& solverConstraint, const b3Vector3& normalAxis,int solverBodyIdA,int solverBodyIdB,
b3ContactPoint& cp,const b3Vector3& rel_pos1,const b3Vector3& rel_pos2,
b3RigidBodyData* colObj0,b3RigidBodyData* colObj1, b3Scalar relaxation,
b3Scalar desiredVelocity=0., b3Scalar cfmSlip=0.);
b3SolverConstraint& addFrictionConstraint(b3RigidBodyData* bodies,b3InertiaData* inertias,const b3Vector3& normalAxis,int solverBodyIdA,int solverBodyIdB,int frictionIndex,b3ContactPoint& cp,const b3Vector3& rel_pos1,const b3Vector3& rel_pos2,b3RigidBodyData* colObj0,b3RigidBodyData* colObj1, b3Scalar relaxation, b3Scalar desiredVelocity=0., b3Scalar cfmSlip=0.);
b3SolverConstraint& addRollingFrictionConstraint(b3RigidBodyData* bodies,b3InertiaData* inertias,const b3Vector3& normalAxis,int solverBodyIdA,int solverBodyIdB,int frictionIndex,b3ContactPoint& cp,const b3Vector3& rel_pos1,const b3Vector3& rel_pos2,b3RigidBodyData* colObj0,b3RigidBodyData* colObj1, b3Scalar relaxation, b3Scalar desiredVelocity=0, b3Scalar cfmSlip=0.f);
void setupRollingFrictionConstraint(b3RigidBodyData* bodies, b3InertiaData* inertias, b3SolverConstraint& solverConstraint, const b3Vector3& normalAxis, int solverBodyIdA, int solverBodyIdB,
b3ContactPoint& cp, const b3Vector3& rel_pos1, const b3Vector3& rel_pos2,
b3RigidBodyData* colObj0, b3RigidBodyData* colObj1, b3Scalar relaxation,
b3Scalar desiredVelocity = 0., b3Scalar cfmSlip = 0.);
b3SolverConstraint& addFrictionConstraint(b3RigidBodyData* bodies, b3InertiaData* inertias, const b3Vector3& normalAxis, int solverBodyIdA, int solverBodyIdB, int frictionIndex, b3ContactPoint& cp, const b3Vector3& rel_pos1, const b3Vector3& rel_pos2, b3RigidBodyData* colObj0, b3RigidBodyData* colObj1, b3Scalar relaxation, b3Scalar desiredVelocity = 0., b3Scalar cfmSlip = 0.);
b3SolverConstraint& addRollingFrictionConstraint(b3RigidBodyData* bodies, b3InertiaData* inertias, const b3Vector3& normalAxis, int solverBodyIdA, int solverBodyIdB, int frictionIndex, b3ContactPoint& cp, const b3Vector3& rel_pos1, const b3Vector3& rel_pos2, b3RigidBodyData* colObj0, b3RigidBodyData* colObj1, b3Scalar relaxation, b3Scalar desiredVelocity = 0, b3Scalar cfmSlip = 0.f);
void setupContactConstraint(b3RigidBodyData* bodies, b3InertiaData* inertias,
b3SolverConstraint& solverConstraint, int solverBodyIdA, int solverBodyIdB, b3ContactPoint& cp,
const b3ContactSolverInfo& infoGlobal, b3Vector3& vel, b3Scalar& rel_vel, b3Scalar& relaxation,
b3SolverConstraint& solverConstraint, int solverBodyIdA, int solverBodyIdB, b3ContactPoint& cp,
const b3ContactSolverInfo& infoGlobal, b3Vector3& vel, b3Scalar& rel_vel, b3Scalar& relaxation,
b3Vector3& rel_pos1, b3Vector3& rel_pos2);
void setFrictionConstraintImpulse( b3RigidBodyData* bodies, b3InertiaData* inertias,b3SolverConstraint& solverConstraint, int solverBodyIdA,int solverBodyIdB,
b3ContactPoint& cp, const b3ContactSolverInfo& infoGlobal);
void setFrictionConstraintImpulse(b3RigidBodyData* bodies, b3InertiaData* inertias, b3SolverConstraint& solverConstraint, int solverBodyIdA, int solverBodyIdB,
b3ContactPoint& cp, const b3ContactSolverInfo& infoGlobal);
///m_btSeed2 is used for re-arranging the constraint rows. improves convergence/quality of friction
unsigned long m_btSeed2;
unsigned long m_btSeed2;
b3Scalar restitutionCurve(b3Scalar rel_vel, b3Scalar restitution);
void convertContact(b3RigidBodyData* bodies, b3InertiaData* inertias,b3Contact4* manifold,const b3ContactSolverInfo& infoGlobal);
void convertContact(b3RigidBodyData* bodies, b3InertiaData* inertias, b3Contact4* manifold, const b3ContactSolverInfo& infoGlobal);
void resolveSplitPenetrationSIMD(
b3SolverBody& bodyA, b3SolverBody& bodyB,
const b3SolverConstraint& contactConstraint);
void resolveSplitPenetrationSIMD(
b3SolverBody& bodyA,b3SolverBody& bodyB,
const b3SolverConstraint& contactConstraint);
void resolveSplitPenetrationImpulseCacheFriendly(
b3SolverBody& bodyA,b3SolverBody& bodyB,
const b3SolverConstraint& contactConstraint);
void resolveSplitPenetrationImpulseCacheFriendly(
b3SolverBody& bodyA, b3SolverBody& bodyB,
const b3SolverConstraint& contactConstraint);
//internal method
int getOrInitSolverBody(int bodyIndex, b3RigidBodyData* bodies,b3InertiaData* inertias);
void initSolverBody(int bodyIndex, b3SolverBody* solverBody, b3RigidBodyData* collisionObject);
int getOrInitSolverBody(int bodyIndex, b3RigidBodyData* bodies, b3InertiaData* inertias);
void initSolverBody(int bodyIndex, b3SolverBody* solverBody, b3RigidBodyData* collisionObject);
void resolveSingleConstraintRowGeneric(b3SolverBody& bodyA,b3SolverBody& bodyB,const b3SolverConstraint& contactConstraint);
void resolveSingleConstraintRowGeneric(b3SolverBody& bodyA, b3SolverBody& bodyB, const b3SolverConstraint& contactConstraint);
void resolveSingleConstraintRowGenericSIMD(b3SolverBody& bodyA, b3SolverBody& bodyB, const b3SolverConstraint& contactConstraint);
void resolveSingleConstraintRowLowerLimit(b3SolverBody& bodyA, b3SolverBody& bodyB, const b3SolverConstraint& contactConstraint);
void resolveSingleConstraintRowLowerLimitSIMD(b3SolverBody& bodyA, b3SolverBody& bodyB, const b3SolverConstraint& contactConstraint);
void resolveSingleConstraintRowGenericSIMD(b3SolverBody& bodyA,b3SolverBody& bodyB,const b3SolverConstraint& contactConstraint);
void resolveSingleConstraintRowLowerLimit(b3SolverBody& bodyA,b3SolverBody& bodyB,const b3SolverConstraint& contactConstraint);
void resolveSingleConstraintRowLowerLimitSIMD(b3SolverBody& bodyA,b3SolverBody& bodyB,const b3SolverConstraint& contactConstraint);
protected:
virtual b3Scalar solveGroupCacheFriendlySetup(b3RigidBodyData* bodies, b3InertiaData* inertias, int numBodies, b3Contact4* manifoldPtr, int numManifolds, b3TypedConstraint** constraints, int numConstraints, const b3ContactSolverInfo& infoGlobal);
virtual b3Scalar solveGroupCacheFriendlySetup(b3RigidBodyData* bodies, b3InertiaData* inertias,int numBodies,b3Contact4* manifoldPtr, int numManifolds,b3TypedConstraint** constraints,int numConstraints,const b3ContactSolverInfo& infoGlobal);
virtual b3Scalar solveGroupCacheFriendlyIterations(b3TypedConstraint** constraints,int numConstraints,const b3ContactSolverInfo& infoGlobal);
virtual void solveGroupCacheFriendlySplitImpulseIterations(b3TypedConstraint** constraints,int numConstraints,const b3ContactSolverInfo& infoGlobal);
b3Scalar solveSingleIteration(int iteration, b3TypedConstraint** constraints,int numConstraints,const b3ContactSolverInfo& infoGlobal);
virtual b3Scalar solveGroupCacheFriendlyFinish(b3RigidBodyData* bodies, b3InertiaData* inertias,int numBodies,const b3ContactSolverInfo& infoGlobal);
virtual b3Scalar solveGroupCacheFriendlyIterations(b3TypedConstraint** constraints, int numConstraints, const b3ContactSolverInfo& infoGlobal);
virtual void solveGroupCacheFriendlySplitImpulseIterations(b3TypedConstraint** constraints, int numConstraints, const b3ContactSolverInfo& infoGlobal);
b3Scalar solveSingleIteration(int iteration, b3TypedConstraint** constraints, int numConstraints, const b3ContactSolverInfo& infoGlobal);
virtual b3Scalar solveGroupCacheFriendlyFinish(b3RigidBodyData* bodies, b3InertiaData* inertias, int numBodies, const b3ContactSolverInfo& infoGlobal);
public:
B3_DECLARE_ALIGNED_ALLOCATOR();
b3PgsJacobiSolver(bool usePgs);
virtual ~b3PgsJacobiSolver();
// void solveContacts(int numBodies, b3RigidBodyData* bodies, b3InertiaData* inertias, int numContacts, b3Contact4* contacts);
void solveContacts(int numBodies, b3RigidBodyData* bodies, b3InertiaData* inertias, int numContacts, b3Contact4* contacts, int numConstraints, b3TypedConstraint** constraints);
// void solveContacts(int numBodies, b3RigidBodyData* bodies, b3InertiaData* inertias, int numContacts, b3Contact4* contacts);
void solveContacts(int numBodies, b3RigidBodyData* bodies, b3InertiaData* inertias, int numContacts, b3Contact4* contacts, int numConstraints, b3TypedConstraint** constraints);
b3Scalar solveGroup(b3RigidBodyData* bodies,b3InertiaData* inertias,int numBodies,b3Contact4* manifoldPtr, int numManifolds,b3TypedConstraint** constraints,int numConstraints,const b3ContactSolverInfo& infoGlobal);
b3Scalar solveGroup(b3RigidBodyData* bodies, b3InertiaData* inertias, int numBodies, b3Contact4* manifoldPtr, int numManifolds, b3TypedConstraint** constraints, int numConstraints, const b3ContactSolverInfo& infoGlobal);
///clear internal cached data and reset random seed
virtual void reset();
virtual void reset();
unsigned long b3Rand2();
int b3RandInt2 (int n);
int b3RandInt2(int n);
void setRandSeed(unsigned long seed)
void setRandSeed(unsigned long seed)
{
m_btSeed2 = seed;
}
unsigned long getRandSeed() const
unsigned long getRandSeed() const
{
return m_btSeed2;
}
};
#endif //B3_PGS_JACOBI_SOLVER
#endif //B3_PGS_JACOBI_SOLVER

153
src/Bullet3Dynamics/ConstraintSolver/b3Point2PointConstraint.cpp
View File

@@ -13,21 +13,14 @@ subject to the following restrictions:
3. This notice may not be removed or altered from any source distribution.
*/
#include "b3Point2PointConstraint.h"
#include "Bullet3Collision/NarrowPhaseCollision/shared/b3RigidBodyData.h"
#include <new>
b3Point2PointConstraint::b3Point2PointConstraint(int rbA,int rbB, const b3Vector3& pivotInA,const b3Vector3& pivotInB)
:b3TypedConstraint(B3_POINT2POINT_CONSTRAINT_TYPE,rbA,rbB),m_pivotInA(pivotInA),m_pivotInB(pivotInB),
m_flags(0)
b3Point2PointConstraint::b3Point2PointConstraint(int rbA, int rbB, const b3Vector3& pivotInA, const b3Vector3& pivotInB)
: b3TypedConstraint(B3_POINT2POINT_CONSTRAINT_TYPE, rbA, rbB), m_pivotInA(pivotInA), m_pivotInB(pivotInB), m_flags(0)
{
}
/*
@@ -40,22 +33,18 @@ m_useSolveConstraintObsolete(false)
}
*/
void b3Point2PointConstraint::getInfo1 (b3ConstraintInfo1* info,const b3RigidBodyData* bodies)
void b3Point2PointConstraint::getInfo1(b3ConstraintInfo1* info, const b3RigidBodyData* bodies)
{
getInfo1NonVirtual(info,bodies);
getInfo1NonVirtual(info, bodies);
}
void b3Point2PointConstraint::getInfo1NonVirtual (b3ConstraintInfo1* info,const b3RigidBodyData* bodies)
void b3Point2PointConstraint::getInfo1NonVirtual(b3ConstraintInfo1* info, const b3RigidBodyData* bodies)
{
info->m_numConstraintRows = 3;
info->nub = 3;
info->m_numConstraintRows = 3;
info->nub = 3;
}
void b3Point2PointConstraint::getInfo2 (b3ConstraintInfo2* info, const b3RigidBodyData* bodies)
void b3Point2PointConstraint::getInfo2(b3ConstraintInfo2* info, const b3RigidBodyData* bodies)
{
b3Transform trA;
trA.setIdentity();
@@ -67,143 +56,135 @@ void b3Point2PointConstraint::getInfo2 (b3ConstraintInfo2* info, const b3RigidBo
trB.setOrigin(bodies[m_rbB].m_pos);
trB.setRotation(bodies[m_rbB].m_quat);
getInfo2NonVirtual(info, trA,trB);
getInfo2NonVirtual(info, trA, trB);
}
void b3Point2PointConstraint::getInfo2NonVirtual (b3ConstraintInfo2* info, const b3Transform& body0_trans, const b3Transform& body1_trans)
void b3Point2PointConstraint::getInfo2NonVirtual(b3ConstraintInfo2* info, const b3Transform& body0_trans, const b3Transform& body1_trans)
{
//retrieve matrices
//retrieve matrices
// anchor points in global coordinates with respect to body PORs.
// set jacobian
info->m_J1linearAxis[0] = 1;
info->m_J1linearAxis[info->rowskip+1] = 1;
info->m_J1linearAxis[2*info->rowskip+2] = 1;
b3Vector3 a1 = body0_trans.getBasis()*getPivotInA();
// set jacobian
info->m_J1linearAxis[0] = 1;
info->m_J1linearAxis[info->rowskip + 1] = 1;
info->m_J1linearAxis[2 * info->rowskip + 2] = 1;
b3Vector3 a1 = body0_trans.getBasis() * getPivotInA();
//b3Vector3 a1a = b3QuatRotate(body0_trans.getRotation(),getPivotInA());
{
b3Vector3* angular0 = (b3Vector3*)(info->m_J1angularAxis);
b3Vector3* angular1 = (b3Vector3*)(info->m_J1angularAxis+info->rowskip);
b3Vector3* angular2 = (b3Vector3*)(info->m_J1angularAxis+2*info->rowskip);
b3Vector3* angular1 = (b3Vector3*)(info->m_J1angularAxis + info->rowskip);
b3Vector3* angular2 = (b3Vector3*)(info->m_J1angularAxis + 2 * info->rowskip);
b3Vector3 a1neg = -a1;
a1neg.getSkewSymmetricMatrix(angular0,angular1,angular2);
a1neg.getSkewSymmetricMatrix(angular0, angular1, angular2);
}
if (info->m_J2linearAxis)
{
info->m_J2linearAxis[0] = -1;
info->m_J2linearAxis[info->rowskip+1] = -1;
info->m_J2linearAxis[2*info->rowskip+2] = -1;
info->m_J2linearAxis[info->rowskip + 1] = -1;
info->m_J2linearAxis[2 * info->rowskip + 2] = -1;
}
b3Vector3 a2 = body1_trans.getBasis()*getPivotInB();
b3Vector3 a2 = body1_trans.getBasis() * getPivotInB();
{
// b3Vector3 a2n = -a2;
// b3Vector3 a2n = -a2;
b3Vector3* angular0 = (b3Vector3*)(info->m_J2angularAxis);
b3Vector3* angular1 = (b3Vector3*)(info->m_J2angularAxis+info->rowskip);
b3Vector3* angular2 = (b3Vector3*)(info->m_J2angularAxis+2*info->rowskip);
a2.getSkewSymmetricMatrix(angular0,angular1,angular2);
b3Vector3* angular1 = (b3Vector3*)(info->m_J2angularAxis + info->rowskip);
b3Vector3* angular2 = (b3Vector3*)(info->m_J2angularAxis + 2 * info->rowskip);
a2.getSkewSymmetricMatrix(angular0, angular1, angular2);
}
// set right hand side
// set right hand side
b3Scalar currERP = (m_flags & B3_P2P_FLAGS_ERP) ? m_erp : info->erp;
b3Scalar k = info->fps * currERP;
int j;
for (j=0; j<3; j++)
{
info->m_constraintError[j*info->rowskip] = k * (a2[j] + body1_trans.getOrigin()[j] - a1[j] - body0_trans.getOrigin()[j]);
//printf("info->m_constraintError[%d]=%f\n",j,info->m_constraintError[j]);
}
if(m_flags & B3_P2P_FLAGS_CFM)
b3Scalar k = info->fps * currERP;
int j;
for (j = 0; j < 3; j++)
{
for (j=0; j<3; j++)
info->m_constraintError[j * info->rowskip] = k * (a2[j] + body1_trans.getOrigin()[j] - a1[j] - body0_trans.getOrigin()[j]);
//printf("info->m_constraintError[%d]=%f\n",j,info->m_constraintError[j]);
}
if (m_flags & B3_P2P_FLAGS_CFM)
{
for (j = 0; j < 3; j++)
{
info->cfm[j*info->rowskip] = m_cfm;
info->cfm[j * info->rowskip] = m_cfm;
}
}
b3Scalar impulseClamp = m_setting.m_impulseClamp;//
for (j=0; j<3; j++)
{
b3Scalar impulseClamp = m_setting.m_impulseClamp; //
for (j = 0; j < 3; j++)
{
if (m_setting.m_impulseClamp > 0)
{
info->m_lowerLimit[j*info->rowskip] = -impulseClamp;
info->m_upperLimit[j*info->rowskip] = impulseClamp;
info->m_lowerLimit[j * info->rowskip] = -impulseClamp;
info->m_upperLimit[j * info->rowskip] = impulseClamp;
}
}
info->m_damping = m_setting.m_damping;
}
void b3Point2PointConstraint::updateRHS(b3Scalar timeStep)
void b3Point2PointConstraint::updateRHS(b3Scalar timeStep)
{
(void)timeStep;
}
///override the default global value of a parameter (such as ERP or CFM), optionally provide the axis (0..5).
///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 b3Point2PointConstraint::setParam(int num, b3Scalar value, int axis)
{
if(axis != -1)
if (axis != -1)
{
b3AssertConstrParams(0);
}
else
{
switch(num)
switch (num)
{
case B3_CONSTRAINT_ERP :
case B3_CONSTRAINT_STOP_ERP :
m_erp = value;
case B3_CONSTRAINT_ERP:
case B3_CONSTRAINT_STOP_ERP:
m_erp = value;
m_flags |= B3_P2P_FLAGS_ERP;
break;
case B3_CONSTRAINT_CFM :
case B3_CONSTRAINT_STOP_CFM :
m_cfm = value;
case B3_CONSTRAINT_CFM:
case B3_CONSTRAINT_STOP_CFM:
m_cfm = value;
m_flags |= B3_P2P_FLAGS_CFM;
break;
default:
default:
b3AssertConstrParams(0);
}
}
}
///return the local value of parameter
b3Scalar b3Point2PointConstraint::getParam(int num, int axis) const
b3Scalar b3Point2PointConstraint::getParam(int num, int axis) const
{
b3Scalar retVal(B3_INFINITY);
if(axis != -1)
if (axis != -1)
{
b3AssertConstrParams(0);
}
else
{
switch(num)
switch (num)
{
case B3_CONSTRAINT_ERP :
case B3_CONSTRAINT_STOP_ERP :
case B3_CONSTRAINT_ERP:
case B3_CONSTRAINT_STOP_ERP:
b3AssertConstrParams(m_flags & B3_P2P_FLAGS_ERP);
retVal = m_erp;
retVal = m_erp;
break;
case B3_CONSTRAINT_CFM :
case B3_CONSTRAINT_STOP_CFM :
case B3_CONSTRAINT_CFM:
case B3_CONSTRAINT_STOP_CFM:
b3AssertConstrParams(m_flags & B3_P2P_FLAGS_CFM);
retVal = m_cfm;
retVal = m_cfm;
break;
default:
default:
b3AssertConstrParams(0);
}
}
return retVal;
}

96
src/Bullet3Dynamics/ConstraintSolver/b3Point2PointConstraint.h
View File

@@ -22,26 +22,24 @@ subject to the following restrictions:
class b3RigidBody;
#ifdef B3_USE_DOUBLE_PRECISION
#define b3Point2PointConstraintData b3Point2PointConstraintDoubleData
#define b3Point2PointConstraintDataName "b3Point2PointConstraintDoubleData"
#define b3Point2PointConstraintData b3Point2PointConstraintDoubleData
#define b3Point2PointConstraintDataName "b3Point2PointConstraintDoubleData"
#else
#define b3Point2PointConstraintData b3Point2PointConstraintFloatData
#define b3Point2PointConstraintDataName "b3Point2PointConstraintFloatData"
#endif //B3_USE_DOUBLE_PRECISION
#define b3Point2PointConstraintData b3Point2PointConstraintFloatData
#define b3Point2PointConstraintDataName "b3Point2PointConstraintFloatData"
#endif //B3_USE_DOUBLE_PRECISION
struct b3ConstraintSetting
struct b3ConstraintSetting
{
b3ConstraintSetting() :
m_tau(b3Scalar(0.3)),
m_damping(b3Scalar(1.)),
m_impulseClamp(b3Scalar(0.))
b3ConstraintSetting() : m_tau(b3Scalar(0.3)),
m_damping(b3Scalar(1.)),
m_impulseClamp(b3Scalar(0.))
{
}
b3Scalar m_tau;
b3Scalar m_damping;
b3Scalar m_impulseClamp;
b3Scalar m_tau;
b3Scalar m_damping;
b3Scalar m_impulseClamp;
};
enum b3Point2PointFlags
@@ -51,47 +49,45 @@ enum b3Point2PointFlags
};
/// point to point constraint between two rigidbodies each with a pivotpoint that descibes the 'ballsocket' location in local space
B3_ATTRIBUTE_ALIGNED16(class) b3Point2PointConstraint : public b3TypedConstraint
B3_ATTRIBUTE_ALIGNED16(class)
b3Point2PointConstraint : public b3TypedConstraint
{
#ifdef IN_PARALLELL_SOLVER
public:
#endif
b3Vector3 m_pivotInA;
b3Vector3 m_pivotInB;
int m_flags;
b3Scalar m_erp;
b3Scalar m_cfm;
public:
b3Vector3 m_pivotInA;
b3Vector3 m_pivotInB;
int m_flags;
b3Scalar m_erp;
b3Scalar m_cfm;
public:
B3_DECLARE_ALIGNED_ALLOCATOR();
b3ConstraintSetting m_setting;
b3ConstraintSetting m_setting;
b3Point2PointConstraint(int rbA,int rbB, const b3Vector3& pivotInA,const b3Vector3& pivotInB);
b3Point2PointConstraint(int rbA, int rbB, const b3Vector3& pivotInA, const b3Vector3& pivotInB);
//b3Point2PointConstraint(int rbA,const b3Vector3& pivotInA);
virtual void getInfo1(b3ConstraintInfo1 * info, const b3RigidBodyData* bodies);
void getInfo1NonVirtual(b3ConstraintInfo1 * info, const b3RigidBodyData* bodies);
virtual void getInfo1 (b3ConstraintInfo1* info,const b3RigidBodyData* bodies);
virtual void getInfo2(b3ConstraintInfo2 * info, const b3RigidBodyData* bodies);
void getInfo1NonVirtual (b3ConstraintInfo1* info,const b3RigidBodyData* bodies);
void getInfo2NonVirtual(b3ConstraintInfo2 * info, const b3Transform& body0_trans, const b3Transform& body1_trans);
virtual void getInfo2 (b3ConstraintInfo2* info, const b3RigidBodyData* bodies);
void updateRHS(b3Scalar timeStep);
void getInfo2NonVirtual (b3ConstraintInfo2* info, const b3Transform& body0_trans, const b3Transform& body1_trans);
void updateRHS(b3Scalar timeStep);
void setPivotA(const b3Vector3& pivotA)
void setPivotA(const b3Vector3& pivotA)
{
m_pivotInA = pivotA;
}
void setPivotB(const b3Vector3& pivotB)
void setPivotB(const b3Vector3& pivotB)
{
m_pivotInB = pivotB;
}
@@ -106,34 +102,32 @@ public:
return m_pivotInB;
}
///override the default global value of a parameter (such as ERP or CFM), optionally provide the axis (0..5).
///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.
virtual void setParam(int num, b3Scalar value, int axis = -1);
virtual void setParam(int num, b3Scalar value, int axis = -1);
///return the local value of parameter
virtual b3Scalar getParam(int num, int axis = -1) const;
virtual b3Scalar getParam(int num, int axis = -1) const;
// virtual int calculateSerializeBufferSize() const;
// virtual int calculateSerializeBufferSize() const;
///fills the dataBuffer and returns the struct name (and 0 on failure)
// virtual const char* serialize(void* dataBuffer, b3Serializer* serializer) const;
// virtual const char* serialize(void* dataBuffer, b3Serializer* serializer) const;
};
///do not change those serialization structures, it requires an updated sBulletDNAstr/sBulletDNAstr64
struct b3Point2PointConstraintFloatData
struct b3Point2PointConstraintFloatData
{
b3TypedConstraintData m_typeConstraintData;
b3Vector3FloatData m_pivotInA;
b3Vector3FloatData m_pivotInB;
b3TypedConstraintData m_typeConstraintData;
b3Vector3FloatData m_pivotInA;
b3Vector3FloatData m_pivotInB;
};
///do not change those serialization structures, it requires an updated sBulletDNAstr/sBulletDNAstr64
struct b3Point2PointConstraintDoubleData
struct b3Point2PointConstraintDoubleData
{
b3TypedConstraintData m_typeConstraintData;
b3Vector3DoubleData m_pivotInA;
b3Vector3DoubleData m_pivotInB;
b3TypedConstraintData m_typeConstraintData;
b3Vector3DoubleData m_pivotInA;
b3Vector3DoubleData m_pivotInB;
};
/*
@@ -156,4 +150,4 @@ B3_FORCE_INLINE const char* b3Point2PointConstraint::serialize(void* dataBuffer,
}
*/
#endif //B3_POINT2POINTCONSTRAINT_H
#endif //B3_POINT2POINTCONSTRAINT_H

189
src/Bullet3Dynamics/ConstraintSolver/b3SolverBody.h
View File

@@ -16,7 +16,6 @@ subject to the following restrictions:
#ifndef B3_SOLVER_BODY_H
#define B3_SOLVER_BODY_H
#include "Bullet3Common/b3Vector3.h"
#include "Bullet3Common/b3Matrix3x3.h"
@@ -26,110 +25,104 @@ subject to the following restrictions:
///Until we get other contributions, only use SIMD on Windows, when using Visual Studio 2008 or later, and not double precision
#ifdef B3_USE_SSE
#define USE_SIMD 1
#endif //
#endif //
#ifdef USE_SIMD
struct b3SimdScalar
struct b3SimdScalar
{
B3_FORCE_INLINE b3SimdScalar()
{
}
B3_FORCE_INLINE b3SimdScalar(float fl)
:m_vec128 (_mm_set1_ps(fl))
B3_FORCE_INLINE b3SimdScalar()
{
}
B3_FORCE_INLINE b3SimdScalar(__m128 v128)
:m_vec128(v128)
B3_FORCE_INLINE b3SimdScalar(float fl)
: m_vec128(_mm_set1_ps(fl))
{
}
union
B3_FORCE_INLINE b3SimdScalar(__m128 v128)
: m_vec128(v128)
{
__m128 m_vec128;
float m_floats[4];
float x,y,z,w;
int m_ints[4];
b3Scalar m_unusedPadding;
}
union {
__m128 m_vec128;
float m_floats[4];
float x, y, z, w;
int m_ints[4];
b3Scalar m_unusedPadding;
};
B3_FORCE_INLINE __m128 get128()
B3_FORCE_INLINE __m128 get128()
{
return m_vec128;
}
B3_FORCE_INLINE const __m128 get128() const
B3_FORCE_INLINE const __m128 get128() const
{
return m_vec128;
}
B3_FORCE_INLINE void set128(__m128 v128)
B3_FORCE_INLINE void set128(__m128 v128)
{
m_vec128 = v128;
}
B3_FORCE_INLINE operator __m128()
{
return m_vec128;
B3_FORCE_INLINE operator __m128()
{
return m_vec128;
}
B3_FORCE_INLINE operator const __m128() const
{
return m_vec128;
}
B3_FORCE_INLINE operator float() const
{
return m_floats[0];
B3_FORCE_INLINE operator const __m128() const
{
return m_vec128;
}
B3_FORCE_INLINE operator float() const
{
return m_floats[0];
}
};
///@brief Return the elementwise product of two b3SimdScalar
B3_FORCE_INLINE b3SimdScalar
operator*(const b3SimdScalar& v1, const b3SimdScalar& v2)
B3_FORCE_INLINE b3SimdScalar
operator*(const b3SimdScalar& v1, const b3SimdScalar& v2)
{
return b3SimdScalar(_mm_mul_ps(v1.get128(),v2.get128()));
return b3SimdScalar(_mm_mul_ps(v1.get128(), v2.get128()));
}
///@brief Return the elementwise product of two b3SimdScalar
B3_FORCE_INLINE b3SimdScalar
operator+(const b3SimdScalar& v1, const b3SimdScalar& v2)
B3_FORCE_INLINE b3SimdScalar
operator+(const b3SimdScalar& v1, const b3SimdScalar& v2)
{
return b3SimdScalar(_mm_add_ps(v1.get128(),v2.get128()));
return b3SimdScalar(_mm_add_ps(v1.get128(), v2.get128()));
}
#else
#define b3SimdScalar b3Scalar
#endif
///The b3SolverBody is an internal datastructure for the constraint solver. Only necessary data is packed to increase cache coherence/performance.
B3_ATTRIBUTE_ALIGNED16 (struct) b3SolverBody
B3_ATTRIBUTE_ALIGNED16(struct)
b3SolverBody
{
B3_DECLARE_ALIGNED_ALLOCATOR();
b3Transform m_worldTransform;
b3Vector3 m_deltaLinearVelocity;
b3Vector3 m_deltaAngularVelocity;
b3Vector3 m_angularFactor;
b3Vector3 m_linearFactor;
b3Vector3 m_invMass;
b3Vector3 m_pushVelocity;
b3Vector3 m_turnVelocity;
b3Vector3 m_linearVelocity;
b3Vector3 m_angularVelocity;
b3Transform m_worldTransform;
b3Vector3 m_deltaLinearVelocity;
b3Vector3 m_deltaAngularVelocity;
b3Vector3 m_angularFactor;
b3Vector3 m_linearFactor;
b3Vector3 m_invMass;
b3Vector3 m_pushVelocity;
b3Vector3 m_turnVelocity;
b3Vector3 m_linearVelocity;
b3Vector3 m_angularVelocity;
union
{
void* m_originalBody;
int m_originalBodyIndex;
union {
void* m_originalBody;
int m_originalBodyIndex;
};
int padding[3];
void setWorldTransform(const b3Transform& worldTransform)
void setWorldTransform(const b3Transform& worldTransform)
{
m_worldTransform = worldTransform;
}
@@ -138,45 +131,42 @@ B3_ATTRIBUTE_ALIGNED16 (struct) b3SolverBody
{
return m_worldTransform;
}
B3_FORCE_INLINE void getVelocityInLocalPointObsolete(const b3Vector3& rel_pos, b3Vector3& velocity ) const
B3_FORCE_INLINE void getVelocityInLocalPointObsolete(const b3Vector3& rel_pos, b3Vector3& velocity) const
{
if (m_originalBody)
velocity = m_linearVelocity+m_deltaLinearVelocity + (m_angularVelocity+m_deltaAngularVelocity).cross(rel_pos);
velocity = m_linearVelocity + m_deltaLinearVelocity + (m_angularVelocity + m_deltaAngularVelocity).cross(rel_pos);
else
velocity.setValue(0,0,0);
velocity.setValue(0, 0, 0);
}
B3_FORCE_INLINE void getAngularVelocity(b3Vector3& angVel) const
B3_FORCE_INLINE void getAngularVelocity(b3Vector3 & angVel) const
{
if (m_originalBody)
angVel =m_angularVelocity+m_deltaAngularVelocity;
angVel = m_angularVelocity + m_deltaAngularVelocity;
else
angVel.setValue(0,0,0);
angVel.setValue(0, 0, 0);
}
//Optimization for the iterative solver: avoid calculating constant terms involving inertia, normal, relative position
B3_FORCE_INLINE void applyImpulse(const b3Vector3& linearComponent, const b3Vector3& angularComponent,const b3Scalar impulseMagnitude)
B3_FORCE_INLINE void applyImpulse(const b3Vector3& linearComponent, const b3Vector3& angularComponent, const b3Scalar impulseMagnitude)
{
if (m_originalBody)
{
m_deltaLinearVelocity += linearComponent*impulseMagnitude*m_linearFactor;
m_deltaAngularVelocity += angularComponent*(impulseMagnitude*m_angularFactor);
m_deltaLinearVelocity += linearComponent * impulseMagnitude * m_linearFactor;
m_deltaAngularVelocity += angularComponent * (impulseMagnitude * m_angularFactor);
}
}
B3_FORCE_INLINE void internalApplyPushImpulse(const b3Vector3& linearComponent, const b3Vector3& angularComponent,b3Scalar impulseMagnitude)
B3_FORCE_INLINE void internalApplyPushImpulse(const b3Vector3& linearComponent, const b3Vector3& angularComponent, b3Scalar impulseMagnitude)
{
if (m_originalBody)
{
m_pushVelocity += linearComponent*impulseMagnitude*m_linearFactor;
m_turnVelocity += angularComponent*(impulseMagnitude*m_angularFactor);
m_pushVelocity += linearComponent * impulseMagnitude * m_linearFactor;
m_turnVelocity += angularComponent * (impulseMagnitude * m_angularFactor);
}
}
const b3Vector3& getDeltaLinearVelocity() const
{
return m_deltaLinearVelocity;
@@ -187,20 +177,19 @@ B3_ATTRIBUTE_ALIGNED16 (struct) b3SolverBody
return m_deltaAngularVelocity;
}
const b3Vector3& getPushVelocity() const
const b3Vector3& getPushVelocity() const
{
return m_pushVelocity;
}
const b3Vector3& getTurnVelocity() const
const b3Vector3& getTurnVelocity() const
{
return m_turnVelocity;
}
////////////////////////////////////////////////
///some internal methods, don't use them
b3Vector3& internalGetDeltaLinearVelocity()
{
return m_deltaLinearVelocity;
@@ -225,7 +214,7 @@ B3_ATTRIBUTE_ALIGNED16 (struct) b3SolverBody
{
m_invMass = invMass;
}
b3Vector3& internalGetPushVelocity()
{
return m_pushVelocity;
@@ -236,67 +225,57 @@ B3_ATTRIBUTE_ALIGNED16 (struct) b3SolverBody
return m_turnVelocity;
}
B3_FORCE_INLINE void internalGetVelocityInLocalPointObsolete(const b3Vector3& rel_pos, b3Vector3& velocity ) const
B3_FORCE_INLINE void internalGetVelocityInLocalPointObsolete(const b3Vector3& rel_pos, b3Vector3& velocity) const
{
velocity = m_linearVelocity+m_deltaLinearVelocity + (m_angularVelocity+m_deltaAngularVelocity).cross(rel_pos);
velocity = m_linearVelocity + m_deltaLinearVelocity + (m_angularVelocity + m_deltaAngularVelocity).cross(rel_pos);
}
B3_FORCE_INLINE void internalGetAngularVelocity(b3Vector3& angVel) const
B3_FORCE_INLINE void internalGetAngularVelocity(b3Vector3 & angVel) const
{
angVel = m_angularVelocity+m_deltaAngularVelocity;
angVel = m_angularVelocity + m_deltaAngularVelocity;
}
//Optimization for the iterative solver: avoid calculating constant terms involving inertia, normal, relative position
B3_FORCE_INLINE void internalApplyImpulse(const b3Vector3& linearComponent, const b3Vector3& angularComponent,const b3Scalar impulseMagnitude)
B3_FORCE_INLINE void internalApplyImpulse(const b3Vector3& linearComponent, const b3Vector3& angularComponent, const b3Scalar impulseMagnitude)
{
//if (m_originalBody)
{
m_deltaLinearVelocity += linearComponent*impulseMagnitude*m_linearFactor;
m_deltaAngularVelocity += angularComponent*(impulseMagnitude*m_angularFactor);
m_deltaLinearVelocity += linearComponent * impulseMagnitude * m_linearFactor;
m_deltaAngularVelocity += angularComponent * (impulseMagnitude * m_angularFactor);
}
}
void writebackVelocity()
void writebackVelocity()
{
//if (m_originalBody>=0)
{
m_linearVelocity +=m_deltaLinearVelocity;
m_linearVelocity += m_deltaLinearVelocity;
m_angularVelocity += m_deltaAngularVelocity;
//m_originalBody->setCompanionId(-1);
}
}
void writebackVelocityAndTransform(b3Scalar timeStep, b3Scalar splitImpulseTurnErp)
void writebackVelocityAndTransform(b3Scalar timeStep, b3Scalar splitImpulseTurnErp)
{
(void) timeStep;
(void)timeStep;
if (m_originalBody)
{
m_linearVelocity += m_deltaLinearVelocity;
m_angularVelocity += m_deltaAngularVelocity;
//correct the position/orientation based on push/turn recovery
b3Transform newTransform;
if (m_pushVelocity[0]!=0.f || m_pushVelocity[1]!=0 || m_pushVelocity[2]!=0 || m_turnVelocity[0]!=0.f || m_turnVelocity[1]!=0 || m_turnVelocity[2]!=0)
if (m_pushVelocity[0] != 0.f || m_pushVelocity[1] != 0 || m_pushVelocity[2] != 0 || m_turnVelocity[0] != 0.f || m_turnVelocity[1] != 0 || m_turnVelocity[2] != 0)
{
// b3Quaternion orn = m_worldTransform.getRotation();
b3TransformUtil::integrateTransform(m_worldTransform,m_pushVelocity,m_turnVelocity*splitImpulseTurnErp,timeStep,newTransform);
// b3Quaternion orn = m_worldTransform.getRotation();
b3TransformUtil::integrateTransform(m_worldTransform, m_pushVelocity, m_turnVelocity * splitImpulseTurnErp, timeStep, newTransform);
m_worldTransform = newTransform;
}
//m_worldTransform.setRotation(orn);
//m_originalBody->setCompanionId(-1);
}
}
};
#endif //B3_SOLVER_BODY_H
#endif //B3_SOLVER_BODY_H

59
src/Bullet3Dynamics/ConstraintSolver/b3SolverConstraint.h
View File

@@ -16,7 +16,6 @@ subject to the following restrictions:
#ifndef B3_SOLVER_CONSTRAINT_H
#define B3_SOLVER_CONSTRAINT_H
#include "Bullet3Common/b3Vector3.h"
#include "Bullet3Common/b3Matrix3x3.h"
//#include "b3JacobianEntry.h"
@@ -25,56 +24,50 @@ subject to the following restrictions:
//#define NO_FRICTION_TANGENTIALS 1
#include "b3SolverBody.h"
///1D constraint along a normal axis between bodyA and bodyB. It can be combined to solve contact and friction constraints.
B3_ATTRIBUTE_ALIGNED16 (struct) b3SolverConstraint
B3_ATTRIBUTE_ALIGNED16(struct)
b3SolverConstraint
{
B3_DECLARE_ALIGNED_ALLOCATOR();
b3Vector3 m_relpos1CrossNormal;
b3Vector3 m_contactNormal;
b3Vector3 m_relpos1CrossNormal;
b3Vector3 m_contactNormal;
b3Vector3 m_relpos2CrossNormal;
b3Vector3 m_relpos2CrossNormal;
//b3Vector3 m_contactNormal2;//usually m_contactNormal2 == -m_contactNormal
b3Vector3 m_angularComponentA;
b3Vector3 m_angularComponentB;
mutable b3SimdScalar m_appliedPushImpulse;
mutable b3SimdScalar m_appliedImpulse;
b3Vector3 m_angularComponentA;
b3Vector3 m_angularComponentB;
mutable b3SimdScalar m_appliedPushImpulse;
mutable b3SimdScalar m_appliedImpulse;
int m_padding1;
int m_padding2;
b3Scalar m_friction;
b3Scalar m_jacDiagABInv;
b3Scalar m_rhs;
b3Scalar m_cfm;
b3Scalar m_lowerLimit;
b3Scalar m_upperLimit;
b3Scalar m_rhsPenetration;
union
{
void* m_originalContactPoint;
b3Scalar m_unusedPadding4;
b3Scalar m_friction;
b3Scalar m_jacDiagABInv;
b3Scalar m_rhs;
b3Scalar m_cfm;
b3Scalar m_lowerLimit;
b3Scalar m_upperLimit;
b3Scalar m_rhsPenetration;
union {
void* m_originalContactPoint;
b3Scalar m_unusedPadding4;
};
int m_overrideNumSolverIterations;
int m_frictionIndex;
int m_overrideNumSolverIterations;
int m_frictionIndex;
int m_solverBodyIdA;
int m_solverBodyIdB;
enum b3SolverConstraintType
enum b3SolverConstraintType
{
B3_SOLVER_CONTACT_1D = 0,
B3_SOLVER_FRICTION_1D
};
};
typedef b3AlignedObjectArray<b3SolverConstraint> b3ConstraintArray;
#endif //B3_SOLVER_CONSTRAINT_H
typedef b3AlignedObjectArray<b3SolverConstraint> b3ConstraintArray;
#endif //B3_SOLVER_CONSTRAINT_H

58
src/Bullet3Dynamics/ConstraintSolver/b3TypedConstraint.cpp
View File

@@ -13,53 +13,46 @@ subject to the following restrictions:
3. This notice may not be removed or altered from any source distribution.
*/
#include "b3TypedConstraint.h"
//#include "Bullet3Common/b3Serializer.h"
#define B3_DEFAULT_DEBUGDRAW_SIZE b3Scalar(0.3f)
b3TypedConstraint::b3TypedConstraint(b3TypedConstraintType type, int rbA,int rbB)
:b3TypedObject(type),
m_userConstraintType(-1),
m_userConstraintPtr((void*)-1),
m_breakingImpulseThreshold(B3_INFINITY),
m_isEnabled(true),
m_needsFeedback(false),
m_overrideNumSolverIterations(-1),
m_rbA(rbA),
m_rbB(rbB),
m_appliedImpulse(b3Scalar(0.)),
m_dbgDrawSize(B3_DEFAULT_DEBUGDRAW_SIZE),
m_jointFeedback(0)
b3TypedConstraint::b3TypedConstraint(b3TypedConstraintType type, int rbA, int rbB)
: b3TypedObject(type),
m_userConstraintType(-1),
m_userConstraintPtr((void*)-1),
m_breakingImpulseThreshold(B3_INFINITY),
m_isEnabled(true),
m_needsFeedback(false),
m_overrideNumSolverIterations(-1),
m_rbA(rbA),
m_rbB(rbB),
m_appliedImpulse(b3Scalar(0.)),
m_dbgDrawSize(B3_DEFAULT_DEBUGDRAW_SIZE),
m_jointFeedback(0)
{
}
b3Scalar b3TypedConstraint::getMotorFactor(b3Scalar pos, b3Scalar lowLim, b3Scalar uppLim, b3Scalar vel, b3Scalar timeFact)
{
if(lowLim > uppLim)
if (lowLim > uppLim)
{
return b3Scalar(1.0f);
}
else if(lowLim == uppLim)
else if (lowLim == uppLim)
{
return b3Scalar(0.0f);
}
b3Scalar lim_fact = b3Scalar(1.0f);
b3Scalar delta_max = vel / timeFact;
if(delta_max < b3Scalar(0.0f))
if (delta_max < b3Scalar(0.0f))
{
if((pos >= lowLim) && (pos < (lowLim - delta_max)))
if ((pos >= lowLim) && (pos < (lowLim - delta_max)))
{
lim_fact = (lowLim - pos) / delta_max;
}
else if(pos < lowLim)
else if (pos < lowLim)
{
lim_fact = b3Scalar(0.0f);
}
@@ -68,13 +61,13 @@ b3Scalar b3TypedConstraint::getMotorFactor(b3Scalar pos, b3Scalar lowLim, b3Scal
lim_fact = b3Scalar(1.0f);
}
}
else if(delta_max > b3Scalar(0.0f))
else if (delta_max > b3Scalar(0.0f))
{
if((pos <= uppLim) && (pos > (uppLim - delta_max)))
if ((pos <= uppLim) && (pos > (uppLim - delta_max)))
{
lim_fact = (uppLim - pos) / delta_max;
}
else if(pos > uppLim)
else if (pos > uppLim)
{
lim_fact = b3Scalar(0.0f);
}
@@ -85,18 +78,16 @@ b3Scalar b3TypedConstraint::getMotorFactor(b3Scalar pos, b3Scalar lowLim, b3Scal
}
else
{
lim_fact = b3Scalar(0.0f);
lim_fact = b3Scalar(0.0f);
}
return lim_fact;
}
void b3AngularLimit::set(b3Scalar low, b3Scalar high, b3Scalar _softness, b3Scalar _biasFactor, b3Scalar _relaxationFactor)
{
m_halfRange = (high - low) / 2.0f;
m_center = b3NormalizeAngle(low + m_halfRange);
m_softness = _softness;
m_softness = _softness;
m_biasFactor = _biasFactor;
m_relaxationFactor = _relaxationFactor;
}
@@ -113,7 +104,7 @@ void b3AngularLimit::test(const b3Scalar angle)
if (deviation < -m_halfRange)
{
m_solveLimit = true;
m_correction = - (deviation + m_halfRange);
m_correction = -(deviation + m_halfRange);
m_sign = +1.0f;
}
else if (deviation > m_halfRange)
@@ -125,7 +116,6 @@ void b3AngularLimit::test(const b3Scalar angle)
}
}
b3Scalar b3AngularLimit::getError() const
{
return m_correction * m_sign;

212
src/Bullet3Dynamics/ConstraintSolver/b3TypedConstraint.h
View File

@@ -16,7 +16,6 @@ subject to the following restrictions:
#ifndef B3_TYPED_CONSTRAINT_H
#define B3_TYPED_CONSTRAINT_H
#include "Bullet3Common/b3Scalar.h"
#include "b3SolverConstraint.h"
@@ -25,7 +24,7 @@ class b3Serializer;
//Don't change any of the existing enum values, so add enum types at the end for serialization compatibility
enum b3TypedConstraintType
{
B3_POINT2POINT_CONSTRAINT_TYPE=3,
B3_POINT2POINT_CONSTRAINT_TYPE = 3,
B3_HINGE_CONSTRAINT_TYPE,
B3_CONETWIST_CONSTRAINT_TYPE,
B3_D6_CONSTRAINT_TYPE,
@@ -37,92 +36,86 @@ enum b3TypedConstraintType
B3_MAX_CONSTRAINT_TYPE
};
enum b3ConstraintParams
{
B3_CONSTRAINT_ERP=1,
B3_CONSTRAINT_ERP = 1,
B3_CONSTRAINT_STOP_ERP,
B3_CONSTRAINT_CFM,
B3_CONSTRAINT_STOP_CFM
};
#if 1
#define b3AssertConstrParams(_par) b3Assert(_par)
#define b3AssertConstrParams(_par) b3Assert(_par)
#else
#define b3AssertConstrParams(_par)
#define b3AssertConstrParams(_par)
#endif
B3_ATTRIBUTE_ALIGNED16(struct) b3JointFeedback
B3_ATTRIBUTE_ALIGNED16(struct)
b3JointFeedback
{
b3Vector3 m_appliedForceBodyA;
b3Vector3 m_appliedTorqueBodyA;
b3Vector3 m_appliedForceBodyB;
b3Vector3 m_appliedTorqueBodyB;
b3Vector3 m_appliedForceBodyA;
b3Vector3 m_appliedTorqueBodyA;
b3Vector3 m_appliedForceBodyB;
b3Vector3 m_appliedTorqueBodyB;
};
struct b3RigidBodyData;
///TypedConstraint is the baseclass for Bullet constraints and vehicles
B3_ATTRIBUTE_ALIGNED16(class) b3TypedConstraint : public b3TypedObject
B3_ATTRIBUTE_ALIGNED16(class)
b3TypedConstraint : public b3TypedObject
{
int m_userConstraintType;
int m_userConstraintType;
union
{
int m_userConstraintId;
union {
int m_userConstraintId;
void* m_userConstraintPtr;
};
b3Scalar m_breakingImpulseThreshold;
bool m_isEnabled;
bool m_needsFeedback;
int m_overrideNumSolverIterations;
b3Scalar m_breakingImpulseThreshold;
bool m_isEnabled;
bool m_needsFeedback;
int m_overrideNumSolverIterations;
b3TypedConstraint& operator=(b3TypedConstraint& other)
b3TypedConstraint& operator=(b3TypedConstraint& other)
{
b3Assert(0);
(void) other;
(void)other;
return *this;
}
protected:
int m_rbA;
int m_rbB;
b3Scalar m_appliedImpulse;
b3Scalar m_dbgDrawSize;
b3JointFeedback* m_jointFeedback;
int m_rbA;
int m_rbB;
b3Scalar m_appliedImpulse;
b3Scalar m_dbgDrawSize;
b3JointFeedback* m_jointFeedback;
///internal method used by the constraint solver, don't use them directly
b3Scalar getMotorFactor(b3Scalar pos, b3Scalar lowLim, b3Scalar uppLim, b3Scalar vel, b3Scalar timeFact);
public:
B3_DECLARE_ALIGNED_ALLOCATOR();
virtual ~b3TypedConstraint() {};
b3TypedConstraint(b3TypedConstraintType type, int bodyA,int bodyB);
virtual ~b3TypedConstraint(){};
b3TypedConstraint(b3TypedConstraintType type, int bodyA, int bodyB);
struct b3ConstraintInfo1 {
int m_numConstraintRows,nub;
struct b3ConstraintInfo1
{
int m_numConstraintRows, nub;
};
struct b3ConstraintInfo2 {
struct b3ConstraintInfo2
{
// integrator parameters: frames per second (1/stepsize), default error
// reduction parameter (0..1).
b3Scalar fps,erp;
b3Scalar fps, erp;
// for the first and second body, pointers to two (linear and angular)
// n*3 jacobian sub matrices, stored by rows. these matrices will have
// been initialized to 0 on entry. if the second body is zero then the
// J2xx pointers may be 0.
b3Scalar *m_J1linearAxis,*m_J1angularAxis,*m_J2linearAxis,*m_J2angularAxis;
b3Scalar *m_J1linearAxis, *m_J1angularAxis, *m_J2linearAxis, *m_J2angularAxis;
// elements to jump from one row to the next in J's
int rowskip;
@@ -130,24 +123,24 @@ public:
// right hand sides of the equation J*v = c + cfm * lambda. cfm is the
// "constraint force mixing" vector. c is set to zero on entry, cfm is
// set to a constant value (typically very small or zero) value on entry.
b3Scalar *m_constraintError,*cfm;
b3Scalar *m_constraintError, *cfm;
// lo and hi limits for variables (set to -/+ infinity on entry).
b3Scalar *m_lowerLimit,*m_upperLimit;
b3Scalar *m_lowerLimit, *m_upperLimit;
// findex vector for variables. see the LCP solver interface for a
// description of what this does. this is set to -1 on entry.
// note that the returned indexes are relative to the first index of
// the constraint.
int *findex;
int* findex;
// number of solver iterations
int m_numIterations;
//damping of the velocity
b3Scalar m_damping;
b3Scalar m_damping;
};
int getOverrideNumSolverIterations() const
int getOverrideNumSolverIterations() const
{
return m_overrideNumSolverIterations;
}
@@ -159,59 +152,55 @@ public:
m_overrideNumSolverIterations = overideNumIterations;
}
///internal method used by the constraint solver, don't use them directly
virtual void setupSolverConstraint(b3ConstraintArray& ca, int solverBodyA,int solverBodyB, b3Scalar timeStep)
virtual void setupSolverConstraint(b3ConstraintArray & ca, int solverBodyA, int solverBodyB, b3Scalar timeStep)
{
(void)ca;
(void)solverBodyA;
(void)solverBodyB;
(void)timeStep;
(void)ca;
(void)solverBodyA;
(void)solverBodyB;
(void)timeStep;
}
///internal method used by the constraint solver, don't use them directly
virtual void getInfo1 (b3ConstraintInfo1* info,const b3RigidBodyData* bodies)=0;
///internal method used by the constraint solver, don't use them directly
virtual void getInfo2 (b3ConstraintInfo2* info, const b3RigidBodyData* bodies)=0;
virtual void getInfo1(b3ConstraintInfo1 * info, const b3RigidBodyData* bodies) = 0;
///internal method used by the constraint solver, don't use them directly
void internalSetAppliedImpulse(b3Scalar appliedImpulse)
virtual void getInfo2(b3ConstraintInfo2 * info, const b3RigidBodyData* bodies) = 0;
///internal method used by the constraint solver, don't use them directly
void internalSetAppliedImpulse(b3Scalar appliedImpulse)
{
m_appliedImpulse = appliedImpulse;
}
///internal method used by the constraint solver, don't use them directly
b3Scalar internalGetAppliedImpulse()
b3Scalar internalGetAppliedImpulse()
{
return m_appliedImpulse;
}
b3Scalar getBreakingImpulseThreshold() const
b3Scalar getBreakingImpulseThreshold() const
{
return m_breakingImpulseThreshold;
return m_breakingImpulseThreshold;
}
void setBreakingImpulseThreshold(b3Scalar threshold)
void setBreakingImpulseThreshold(b3Scalar threshold)
{
m_breakingImpulseThreshold = threshold;
}
bool isEnabled() const
bool isEnabled() const
{
return m_isEnabled;
}
void setEnabled(bool enabled)
void setEnabled(bool enabled)
{
m_isEnabled=enabled;
m_isEnabled = enabled;
}
///internal method used by the constraint solver, don't use them directly
virtual void solveConstraintObsolete(b3SolverBody& /*bodyA*/,b3SolverBody& /*bodyB*/,b3Scalar /*timeStep*/) {};
virtual void solveConstraintObsolete(b3SolverBody& /*bodyA*/, b3SolverBody& /*bodyB*/, b3Scalar /*timeStep*/){};
int getRigidBodyA() const
{
return m_rbA;
@@ -221,8 +210,7 @@ public:
return m_rbB;
}
int getRigidBodyA()
int getRigidBodyA()
{
return m_rbA;
}
@@ -233,15 +221,15 @@ public:
int getUserConstraintType() const
{
return m_userConstraintType ;
return m_userConstraintType;
}
void setUserConstraintType(int userConstraintType)
void setUserConstraintType(int userConstraintType)
{
m_userConstraintType = userConstraintType;
};
void setUserConstraintId(int uid)
void setUserConstraintId(int uid)
{
m_userConstraintId = uid;
}
@@ -251,17 +239,17 @@ public:
return m_userConstraintId;
}
void setUserConstraintPtr(void* ptr)
void setUserConstraintPtr(void* ptr)
{
m_userConstraintPtr = ptr;
}
void* getUserConstraintPtr()
void* getUserConstraintPtr()
{
return m_userConstraintPtr;
}
void setJointFeedback(b3JointFeedback* jointFeedback)
void setJointFeedback(b3JointFeedback * jointFeedback)
{
m_jointFeedback = jointFeedback;
}
@@ -276,37 +264,36 @@ public:
return m_jointFeedback;
}
int getUid() const
{
return m_userConstraintId;
}
return m_userConstraintId;
}
bool needsFeedback() const
bool needsFeedback() const
{
return m_needsFeedback;
}
///enableFeedback will allow to read the applied linear and angular impulse
///use getAppliedImpulse, getAppliedLinearImpulse and getAppliedAngularImpulse to read feedback information
void enableFeedback(bool needsFeedback)
void enableFeedback(bool needsFeedback)
{
m_needsFeedback = needsFeedback;
}
///getAppliedImpulse is an estimated total applied impulse.
///getAppliedImpulse is an estimated total applied impulse.
///This feedback could be used to determine breaking constraints or playing sounds.
b3Scalar getAppliedImpulse() const
b3Scalar getAppliedImpulse() const
{
b3Assert(m_needsFeedback);
return m_appliedImpulse;
}
b3TypedConstraintType getConstraintType () const
b3TypedConstraintType getConstraintType() const
{
return b3TypedConstraintType(m_objectType);
}
void setDbgDrawSize(b3Scalar dbgDrawSize)
{
m_dbgDrawSize = dbgDrawSize;
@@ -316,35 +303,34 @@ public:
return m_dbgDrawSize;
}
///override the default global value of a parameter (such as ERP or CFM), optionally provide the axis (0..5).
///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.
virtual void setParam(int num, b3Scalar value, int axis = -1) = 0;
virtual void setParam(int num, b3Scalar value, int axis = -1) = 0;
///return the local value of parameter
virtual b3Scalar getParam(int num, int axis = -1) const = 0;
// virtual int calculateSerializeBufferSize() const;
virtual b3Scalar getParam(int num, int axis = -1) const = 0;
// virtual int calculateSerializeBufferSize() const;
///fills the dataBuffer and returns the struct name (and 0 on failure)
//virtual const char* serialize(void* dataBuffer, b3Serializer* serializer) const;
};
// returns angle in range [-B3_2_PI, B3_2_PI], closest to one of the limits
// returns angle in range [-B3_2_PI, B3_2_PI], closest to one of the limits
// all arguments should be normalized angles (i.e. in range [-B3_PI, B3_PI])
B3_FORCE_INLINE b3Scalar b3AdjustAngleToLimits(b3Scalar angleInRadians, b3Scalar angleLowerLimitInRadians, b3Scalar angleUpperLimitInRadians)
{
if(angleLowerLimitInRadians >= angleUpperLimitInRadians)
if (angleLowerLimitInRadians >= angleUpperLimitInRadians)
{
return angleInRadians;
}
else if(angleInRadians < angleLowerLimitInRadians)
else if (angleInRadians < angleLowerLimitInRadians)
{
b3Scalar diffLo = b3Fabs(b3NormalizeAngle(angleLowerLimitInRadians - angleInRadians));
b3Scalar diffHi = b3Fabs(b3NormalizeAngle(angleUpperLimitInRadians - angleInRadians));
return (diffLo < diffHi) ? angleInRadians : (angleInRadians + B3_2_PI);
}
else if(angleInRadians > angleUpperLimitInRadians)
else if (angleInRadians > angleUpperLimitInRadians)
{
b3Scalar diffHi = b3Fabs(b3NormalizeAngle(angleInRadians - angleUpperLimitInRadians));
b3Scalar diffLo = b3Fabs(b3NormalizeAngle(angleInRadians - angleLowerLimitInRadians));
@@ -356,6 +342,7 @@ B3_FORCE_INLINE b3Scalar b3AdjustAngleToLimits(b3Scalar angleInRadians, b3Scalar
}
}
// clang-format off
///do not change those serialization structures, it requires an updated sBulletDNAstr/sBulletDNAstr64
struct b3TypedConstraintData
{
@@ -379,17 +366,18 @@ struct b3TypedConstraintData
};
// clang-format on
/*B3_FORCE_INLINE int b3TypedConstraint::calculateSerializeBufferSize() const
{
return sizeof(b3TypedConstraintData);
}
*/
class b3AngularLimit
{
private:
b3Scalar
b3Scalar
m_center,
m_halfRange,
m_softness,
@@ -404,15 +392,16 @@ private:
public:
/// Default constructor initializes limit as inactive, allowing free constraint movement
b3AngularLimit()
:m_center(0.0f),
m_halfRange(-1.0f),
m_softness(0.9f),
m_biasFactor(0.3f),
m_relaxationFactor(1.0f),
m_correction(0.0f),
m_sign(0.0f),
m_solveLimit(false)
{}
: m_center(0.0f),
m_halfRange(-1.0f),
m_softness(0.9f),
m_biasFactor(0.3f),
m_relaxationFactor(1.0f),
m_correction(0.0f),
m_sign(0.0f),
m_solveLimit(false)
{
}
/// Sets all limit's parameters.
/// When low > high limit becomes inactive.
@@ -441,13 +430,13 @@ public:
return m_relaxationFactor;
}
/// Returns correction value evaluated when test() was invoked
/// Returns correction value evaluated when test() was invoked
inline b3Scalar getCorrection() const
{
return m_correction;
}
/// Returns sign value evaluated when test() was invoked
/// Returns sign value evaluated when test() was invoked
inline b3Scalar getSign() const
{
return m_sign;
@@ -475,9 +464,6 @@ public:
b3Scalar getLow() const;
b3Scalar getHigh() const;
};
#endif //B3_TYPED_CONSTRAINT_H
#endif //B3_TYPED_CONSTRAINT_H

313
src/Bullet3Dynamics/b3CpuRigidBodyPipeline.cpp
View File

@@ -11,7 +11,6 @@
#include "Bullet3Dynamics/shared/b3ContactConstraint4.h"
#include "Bullet3Dynamics/shared/b3Inertia.h"
struct b3CpuRigidBodyPipelineInternalData
{
b3AlignedObjectArray<b3RigidBodyData> m_rigidBodies;
@@ -22,7 +21,6 @@ struct b3CpuRigidBodyPipelineInternalData
b3CpuNarrowPhase* m_np;
b3Config m_config;
};
b3CpuRigidBodyPipeline::b3CpuRigidBodyPipeline(class b3CpuNarrowPhase* narrowphase, struct b3DynamicBvhBroadphase* broadphaseDbvt, const b3Config& config)
{
@@ -39,49 +37,43 @@ b3CpuRigidBodyPipeline::~b3CpuRigidBodyPipeline()
void b3CpuRigidBodyPipeline::updateAabbWorldSpace()
{
for (int i=0;i<this->getNumBodies();i++)
for (int i = 0; i < this->getNumBodies(); i++)
{
b3RigidBodyData* body = &m_data->m_rigidBodies[i];
b3Float4 position = body->m_pos;
b3Quat orientation = body->m_quat;
b3Quat orientation = body->m_quat;
int collidableIndex = body->m_collidableIdx;
b3Collidable& collidable = m_data->m_np->getCollidableCpu(collidableIndex);
int shapeIndex = collidable.m_shapeIndex;
if (shapeIndex>=0)
{
if (shapeIndex >= 0)
{
b3Aabb localAabb = m_data->m_np->getLocalSpaceAabb(shapeIndex);
b3Aabb& worldAabb = m_data->m_aabbWorldSpace[i];
float margin=0.f;
b3TransformAabb2(localAabb.m_minVec,localAabb.m_maxVec,margin,position,orientation,&worldAabb.m_minVec,&worldAabb.m_maxVec);
m_data->m_bp->setAabb(i,worldAabb.m_minVec,worldAabb.m_maxVec,0);
float margin = 0.f;
b3TransformAabb2(localAabb.m_minVec, localAabb.m_maxVec, margin, position, orientation, &worldAabb.m_minVec, &worldAabb.m_maxVec);
m_data->m_bp->setAabb(i, worldAabb.m_minVec, worldAabb.m_maxVec, 0);
}
}
}
void b3CpuRigidBodyPipeline::computeOverlappingPairs()
void b3CpuRigidBodyPipeline::computeOverlappingPairs()
{
int numPairs = m_data->m_bp->getOverlappingPairCache()->getNumOverlappingPairs();
m_data->m_bp->calculateOverlappingPairs();
numPairs = m_data->m_bp->getOverlappingPairCache()->getNumOverlappingPairs();
printf("numPairs=%d\n",numPairs);
printf("numPairs=%d\n", numPairs);
}
void b3CpuRigidBodyPipeline::computeContactPoints()
{
b3AlignedObjectArray<b3Int4>& pairs = m_data->m_bp->getOverlappingPairCache()->getOverlappingPairArray();
m_data->m_np->computeContacts(pairs,m_data->m_aabbWorldSpace, m_data->m_rigidBodies);
m_data->m_np->computeContacts(pairs, m_data->m_aabbWorldSpace, m_data->m_rigidBodies);
}
void b3CpuRigidBodyPipeline::stepSimulation(float deltaTime)
void b3CpuRigidBodyPipeline::stepSimulation(float deltaTime)
{
//update world space aabb's
updateAabbWorldSpace();
@@ -92,73 +84,71 @@ void b3CpuRigidBodyPipeline::stepSimulation(float deltaTime)
computeContactPoints();
//solve contacts
//update transforms
integrate(deltaTime);
}
static inline float b3CalcRelVel(const b3Vector3& l0, const b3Vector3& l1, const b3Vector3& a0, const b3Vector3& a1,
const b3Vector3& linVel0, const b3Vector3& angVel0, const b3Vector3& linVel1, const b3Vector3& angVel1)
static inline float b3CalcRelVel(const b3Vector3& l0, const b3Vector3& l1, const b3Vector3& a0, const b3Vector3& a1,
const b3Vector3& linVel0, const b3Vector3& angVel0, const b3Vector3& linVel1, const b3Vector3& angVel1)
{
return b3Dot(l0, linVel0) + b3Dot(a0, angVel0) + b3Dot(l1, linVel1) + b3Dot(a1, angVel1);
}
static inline void b3SetLinearAndAngular(const b3Vector3& n, const b3Vector3& r0, const b3Vector3& r1,
b3Vector3& linear, b3Vector3& angular0, b3Vector3& angular1)
static inline void b3SetLinearAndAngular(const b3Vector3& n, const b3Vector3& r0, const b3Vector3& r1,
b3Vector3& linear, b3Vector3& angular0, b3Vector3& angular1)
{
linear = -n;
angular0 = -b3Cross(r0, n);
angular1 = b3Cross(r1, n);
}
static inline void b3SolveContact(b3ContactConstraint4& cs,
const b3Vector3& posA, b3Vector3& linVelA, b3Vector3& angVelA, float invMassA, const b3Matrix3x3& invInertiaA,
const b3Vector3& posB, b3Vector3& linVelB, b3Vector3& angVelB, float invMassB, const b3Matrix3x3& invInertiaB,
float maxRambdaDt[4], float minRambdaDt[4])
static inline void b3SolveContact(b3ContactConstraint4& cs,
const b3Vector3& posA, b3Vector3& linVelA, b3Vector3& angVelA, float invMassA, const b3Matrix3x3& invInertiaA,
const b3Vector3& posB, b3Vector3& linVelB, b3Vector3& angVelB, float invMassB, const b3Matrix3x3& invInertiaB,
float maxRambdaDt[4], float minRambdaDt[4])
{
b3Vector3 dLinVelA;
dLinVelA.setZero();
b3Vector3 dAngVelA;
dAngVelA.setZero();
b3Vector3 dLinVelB;
dLinVelB.setZero();
b3Vector3 dAngVelB;
dAngVelB.setZero();
b3Vector3 dLinVelA; dLinVelA.setZero();
b3Vector3 dAngVelA; dAngVelA.setZero();
b3Vector3 dLinVelB; dLinVelB.setZero();
b3Vector3 dAngVelB; dAngVelB.setZero();
for(int ic=0; ic<4; ic++)
for (int ic = 0; ic < 4; ic++)
{
// dont necessary because this makes change to 0
if( cs.m_jacCoeffInv[ic] == 0.f ) continue;
if (cs.m_jacCoeffInv[ic] == 0.f) continue;
{
b3Vector3 angular0, angular1, linear;
b3Vector3 r0 = cs.m_worldPos[ic] - (b3Vector3&)posA;
b3Vector3 r1 = cs.m_worldPos[ic] - (b3Vector3&)posB;
b3SetLinearAndAngular( (const b3Vector3 &)-cs.m_linear, (const b3Vector3 &)r0, (const b3Vector3 &)r1, linear, angular0, angular1 );
b3SetLinearAndAngular((const b3Vector3&)-cs.m_linear, (const b3Vector3&)r0, (const b3Vector3&)r1, linear, angular0, angular1);
float rambdaDt = b3CalcRelVel((const b3Vector3 &)cs.m_linear,(const b3Vector3 &) -cs.m_linear, angular0, angular1,
linVelA, angVelA, linVelB, angVelB ) + cs.m_b[ic];
float rambdaDt = b3CalcRelVel((const b3Vector3&)cs.m_linear, (const b3Vector3&)-cs.m_linear, angular0, angular1,
linVelA, angVelA, linVelB, angVelB) +
cs.m_b[ic];
rambdaDt *= cs.m_jacCoeffInv[ic];
{
float prevSum = cs.m_appliedRambdaDt[ic];
float updated = prevSum;
updated += rambdaDt;
updated = b3Max( updated, minRambdaDt[ic] );
updated = b3Min( updated, maxRambdaDt[ic] );
updated = b3Max(updated, minRambdaDt[ic]);
updated = b3Min(updated, maxRambdaDt[ic]);
rambdaDt = updated - prevSum;
cs.m_appliedRambdaDt[ic] = updated;
}
b3Vector3 linImp0 = invMassA*linear*rambdaDt;
b3Vector3 linImp1 = invMassB*(-linear)*rambdaDt;
b3Vector3 angImp0 = (invInertiaA* angular0)*rambdaDt;
b3Vector3 angImp1 = (invInertiaB* angular1)*rambdaDt;
b3Vector3 linImp0 = invMassA * linear * rambdaDt;
b3Vector3 linImp1 = invMassB * (-linear) * rambdaDt;
b3Vector3 angImp0 = (invInertiaA * angular0) * rambdaDt;
b3Vector3 angImp1 = (invInertiaB * angular1) * rambdaDt;
#ifdef _WIN32
b3Assert(_finite(linImp0.getX()));
b3Assert(_finite(linImp0.getX()));
b3Assert(_finite(linImp1.getX()));
#endif
{
@@ -169,53 +159,46 @@ static inline void b3SolveContact(b3ContactConstraint4& cs,
}
}
}
}
static inline void b3SolveFriction(b3ContactConstraint4& cs,
const b3Vector3& posA, b3Vector3& linVelA, b3Vector3& angVelA, float invMassA, const b3Matrix3x3& invInertiaA,
const b3Vector3& posB, b3Vector3& linVelB, b3Vector3& angVelB, float invMassB, const b3Matrix3x3& invInertiaB,
float maxRambdaDt[4], float minRambdaDt[4])
static inline void b3SolveFriction(b3ContactConstraint4& cs,
const b3Vector3& posA, b3Vector3& linVelA, b3Vector3& angVelA, float invMassA, const b3Matrix3x3& invInertiaA,
const b3Vector3& posB, b3Vector3& linVelB, b3Vector3& angVelB, float invMassB, const b3Matrix3x3& invInertiaB,
float maxRambdaDt[4], float minRambdaDt[4])
{
if( cs.m_fJacCoeffInv[0] == 0 && cs.m_fJacCoeffInv[0] == 0 ) return;
if (cs.m_fJacCoeffInv[0] == 0 && cs.m_fJacCoeffInv[0] == 0) return;
const b3Vector3& center = (const b3Vector3&)cs.m_center;
b3Vector3 n = -(const b3Vector3&)cs.m_linear;
b3Vector3 tangent[2];
b3PlaneSpace1 (n, tangent[0],tangent[1]);
b3PlaneSpace1(n, tangent[0], tangent[1]);
b3Vector3 angular0, angular1, linear;
b3Vector3 r0 = center - posA;
b3Vector3 r1 = center - posB;
for(int i=0; i<2; i++)
for (int i = 0; i < 2; i++)
{
b3SetLinearAndAngular( tangent[i], r0, r1, linear, angular0, angular1 );
b3SetLinearAndAngular(tangent[i], r0, r1, linear, angular0, angular1);
float rambdaDt = b3CalcRelVel(linear, -linear, angular0, angular1,
linVelA, angVelA, linVelB, angVelB );
linVelA, angVelA, linVelB, angVelB);
rambdaDt *= cs.m_fJacCoeffInv[i];
{
float prevSum = cs.m_fAppliedRambdaDt[i];
float updated = prevSum;
updated += rambdaDt;
updated = b3Max( updated, minRambdaDt[i] );
updated = b3Min( updated, maxRambdaDt[i] );
rambdaDt = updated - prevSum;
cs.m_fAppliedRambdaDt[i] = updated;
}
{
float prevSum = cs.m_fAppliedRambdaDt[i];
float updated = prevSum;
updated += rambdaDt;
updated = b3Max(updated, minRambdaDt[i]);
updated = b3Min(updated, maxRambdaDt[i]);
rambdaDt = updated - prevSum;
cs.m_fAppliedRambdaDt[i] = updated;
}
b3Vector3 linImp0 = invMassA*linear*rambdaDt;
b3Vector3 linImp1 = invMassB*(-linear)*rambdaDt;
b3Vector3 angImp0 = (invInertiaA* angular0)*rambdaDt;
b3Vector3 angImp1 = (invInertiaB* angular1)*rambdaDt;
b3Vector3 linImp0 = invMassA * linear * rambdaDt;
b3Vector3 linImp1 = invMassB * (-linear) * rambdaDt;
b3Vector3 angImp0 = (invInertiaA * angular0) * rambdaDt;
b3Vector3 angImp1 = (invInertiaB * angular1) * rambdaDt;
#ifdef _WIN32
b3Assert(_finite(linImp0.getX()));
b3Assert(_finite(linImp1.getX()));
@@ -226,57 +209,45 @@ static inline void b3SolveFriction(b3ContactConstraint4& cs,
angVelB += angImp1;
}
{ // angular damping for point constraint
b3Vector3 ab = ( posB - posA ).normalized();
b3Vector3 ac = ( center - posA ).normalized();
if( b3Dot( ab, ac ) > 0.95f || (invMassA == 0.f || invMassB == 0.f))
{ // angular damping for point constraint
b3Vector3 ab = (posB - posA).normalized();
b3Vector3 ac = (center - posA).normalized();
if (b3Dot(ab, ac) > 0.95f || (invMassA == 0.f || invMassB == 0.f))
{
float angNA = b3Dot( n, angVelA );
float angNB = b3Dot( n, angVelB );
float angNA = b3Dot(n, angVelA);
float angNB = b3Dot(n, angVelB);
angVelA -= (angNA*0.1f)*n;
angVelB -= (angNB*0.1f)*n;
angVelA -= (angNA * 0.1f) * n;
angVelB -= (angNB * 0.1f) * n;
}
}
}
struct b3SolveTask// : public ThreadPool::Task
struct b3SolveTask // : public ThreadPool::Task
{
b3SolveTask(b3AlignedObjectArray<b3RigidBodyData>& bodies,
b3AlignedObjectArray<b3Inertia>& shapes,
b3SolveTask(b3AlignedObjectArray<b3RigidBodyData>& bodies,
b3AlignedObjectArray<b3Inertia>& shapes,
b3AlignedObjectArray<b3ContactConstraint4>& constraints,
int start, int nConstraints,
int maxNumBatches,
b3AlignedObjectArray<int>* wgUsedBodies, int curWgidx
)
: m_bodies( bodies ), m_shapes( shapes ), m_constraints( constraints ),
m_wgUsedBodies(wgUsedBodies),m_curWgidx(curWgidx),
m_start( start ),
m_nConstraints( nConstraints ),
m_solveFriction( true ),
m_maxNumBatches(maxNumBatches)
{}
b3AlignedObjectArray<int>* wgUsedBodies, int curWgidx)
: m_bodies(bodies), m_shapes(shapes), m_constraints(constraints), m_wgUsedBodies(wgUsedBodies), m_curWgidx(curWgidx), m_start(start), m_nConstraints(nConstraints), m_solveFriction(true), m_maxNumBatches(maxNumBatches)
{
}
unsigned short int getType(){ return 0; }
unsigned short int getType() { return 0; }
void run(int tIdx)
{
b3AlignedObjectArray<int> usedBodies;
//printf("run..............\n");
for (int bb=0;bb<m_maxNumBatches;bb++)
for (int bb = 0; bb < m_maxNumBatches; bb++)
{
usedBodies.resize(0);
for(int ic=m_nConstraints-1; ic>=0; ic--)
for (int ic = m_nConstraints - 1; ic >= 0; ic--)
//for(int ic=0; ic<m_nConstraints; ic++)
{
int i = m_start + ic;
if (m_constraints[i].m_batchIdx != bb)
continue;
@@ -298,87 +269,80 @@ m_start( start ),
//printf("ic(b)=%d, localBatch=%d\n",ic,localBatch);
}
#endif
if (aIdx==10)
if (aIdx == 10)
{
//printf("ic(a)=%d, localBatch=%d\n",ic,localBatch);
}
if (usedBodies.size()<(aIdx+1))
if (usedBodies.size() < (aIdx + 1))
{
usedBodies.resize(aIdx+1,0);
usedBodies.resize(aIdx + 1, 0);
}
if (usedBodies.size()<(bIdx+1))
if (usedBodies.size() < (bIdx + 1))
{
usedBodies.resize(bIdx+1,0);
usedBodies.resize(bIdx + 1, 0);
}
if (bodyA.m_invMass)
{
b3Assert(usedBodies[aIdx]==0);
b3Assert(usedBodies[aIdx] == 0);
usedBodies[aIdx]++;
}
if (bodyB.m_invMass)
{
b3Assert(usedBodies[bIdx]==0);
b3Assert(usedBodies[bIdx] == 0);
usedBodies[bIdx]++;
}
if( !m_solveFriction )
if (!m_solveFriction)
{
float maxRambdaDt[4] = {FLT_MAX,FLT_MAX,FLT_MAX,FLT_MAX};
float minRambdaDt[4] = {0.f,0.f,0.f,0.f};
b3SolveContact( m_constraints[i], (b3Vector3&)bodyA.m_pos, (b3Vector3&)bodyA.m_linVel, (b3Vector3&)bodyA.m_angVel, bodyA.m_invMass, (const b3Matrix3x3 &)m_shapes[aIdx].m_invInertiaWorld,
(b3Vector3&)bodyB.m_pos, (b3Vector3&)bodyB.m_linVel, (b3Vector3&)bodyB.m_angVel, bodyB.m_invMass, (const b3Matrix3x3 &)m_shapes[bIdx].m_invInertiaWorld,
maxRambdaDt, minRambdaDt );
float maxRambdaDt[4] = {FLT_MAX, FLT_MAX, FLT_MAX, FLT_MAX};
float minRambdaDt[4] = {0.f, 0.f, 0.f, 0.f};
b3SolveContact(m_constraints[i], (b3Vector3&)bodyA.m_pos, (b3Vector3&)bodyA.m_linVel, (b3Vector3&)bodyA.m_angVel, bodyA.m_invMass, (const b3Matrix3x3&)m_shapes[aIdx].m_invInertiaWorld,
(b3Vector3&)bodyB.m_pos, (b3Vector3&)bodyB.m_linVel, (b3Vector3&)bodyB.m_angVel, bodyB.m_invMass, (const b3Matrix3x3&)m_shapes[bIdx].m_invInertiaWorld,
maxRambdaDt, minRambdaDt);
}
else
{
float maxRambdaDt[4] = {FLT_MAX,FLT_MAX,FLT_MAX,FLT_MAX};
float minRambdaDt[4] = {0.f,0.f,0.f,0.f};
float maxRambdaDt[4] = {FLT_MAX, FLT_MAX, FLT_MAX, FLT_MAX};
float minRambdaDt[4] = {0.f, 0.f, 0.f, 0.f};
float sum = 0;
for(int j=0; j<4; j++)
for (int j = 0; j < 4; j++)
{
sum +=m_constraints[i].m_appliedRambdaDt[j];
sum += m_constraints[i].m_appliedRambdaDt[j];
}
frictionCoeff = 0.7f;
for(int j=0; j<4; j++)
for (int j = 0; j < 4; j++)
{
maxRambdaDt[j] = frictionCoeff*sum;
maxRambdaDt[j] = frictionCoeff * sum;
minRambdaDt[j] = -maxRambdaDt[j];
}
b3SolveFriction( m_constraints[i], (b3Vector3&)bodyA.m_pos, (b3Vector3&)bodyA.m_linVel, (b3Vector3&)bodyA.m_angVel, bodyA.m_invMass,(const b3Matrix3x3 &) m_shapes[aIdx].m_invInertiaWorld,
(b3Vector3&)bodyB.m_pos, (b3Vector3&)bodyB.m_linVel, (b3Vector3&)bodyB.m_angVel, bodyB.m_invMass,(const b3Matrix3x3 &) m_shapes[bIdx].m_invInertiaWorld,
maxRambdaDt, minRambdaDt );
b3SolveFriction(m_constraints[i], (b3Vector3&)bodyA.m_pos, (b3Vector3&)bodyA.m_linVel, (b3Vector3&)bodyA.m_angVel, bodyA.m_invMass, (const b3Matrix3x3&)m_shapes[aIdx].m_invInertiaWorld,
(b3Vector3&)bodyB.m_pos, (b3Vector3&)bodyB.m_linVel, (b3Vector3&)bodyB.m_angVel, bodyB.m_invMass, (const b3Matrix3x3&)m_shapes[bIdx].m_invInertiaWorld,
maxRambdaDt, minRambdaDt);
}
}
if (m_wgUsedBodies)
{
if (m_wgUsedBodies[m_curWgidx].size()<usedBodies.size())
if (m_wgUsedBodies[m_curWgidx].size() < usedBodies.size())
{
m_wgUsedBodies[m_curWgidx].resize(usedBodies.size());
}
for (int i=0;i<usedBodies.size();i++)
for (int i = 0; i < usedBodies.size(); i++)
{
if (usedBodies[i])
{
//printf("cell %d uses body %d\n", m_curWgidx,i);
m_wgUsedBodies[m_curWgidx][i]=1;
m_wgUsedBodies[m_curWgidx][i] = 1;
}
}
}
}
}
b3AlignedObjectArray<b3RigidBodyData>& m_bodies;
@@ -397,24 +361,22 @@ void b3CpuRigidBodyPipeline::solveContactConstraints()
int m_nIterations = 4;
b3AlignedObjectArray<b3ContactConstraint4> contactConstraints;
// const b3AlignedObjectArray<b3Contact4Data>& contacts = m_data->m_np->getContacts();
// const b3AlignedObjectArray<b3Contact4Data>& contacts = m_data->m_np->getContacts();
int n = contactConstraints.size();
//convert contacts...
int maxNumBatches = 250;
for(int iter=0; iter<m_nIterations; iter++)
for (int iter = 0; iter < m_nIterations; iter++)
{
b3SolveTask task( m_data->m_rigidBodies, m_data->m_inertias, contactConstraints, 0, n ,maxNumBatches,0,0);
b3SolveTask task(m_data->m_rigidBodies, m_data->m_inertias, contactConstraints, 0, n, maxNumBatches, 0, 0);
task.m_solveFriction = false;
task.run(0);
}
for(int iter=0; iter<m_nIterations; iter++)
for (int iter = 0; iter < m_nIterations; iter++)
{
b3SolveTask task( m_data->m_rigidBodies, m_data->m_inertias, contactConstraints, 0, n ,maxNumBatches,0,0);
b3SolveTask task(m_data->m_rigidBodies, m_data->m_inertias, contactConstraints, 0, n, maxNumBatches, 0, 0);
task.m_solveFriction = true;
task.run(0);
}
@@ -422,53 +384,51 @@ void b3CpuRigidBodyPipeline::solveContactConstraints()
void b3CpuRigidBodyPipeline::integrate(float deltaTime)
{
float angDamping=0.f;
b3Vector3 gravityAcceleration=b3MakeVector3(0,-9,0);
float angDamping = 0.f;
b3Vector3 gravityAcceleration = b3MakeVector3(0, -9, 0);
//integrate transforms (external forces/gravity should be moved into constraint solver)
for (int i=0;i<m_data->m_rigidBodies.size();i++)
for (int i = 0; i < m_data->m_rigidBodies.size(); i++)
{
b3IntegrateTransform(&m_data->m_rigidBodies[i],deltaTime,angDamping,gravityAcceleration);
b3IntegrateTransform(&m_data->m_rigidBodies[i], deltaTime, angDamping, gravityAcceleration);
}
}
int b3CpuRigidBodyPipeline::registerPhysicsInstance(float mass, const float* position, const float* orientation, int collidableIndex, int userData)
int b3CpuRigidBodyPipeline::registerPhysicsInstance(float mass, const float* position, const float* orientation, int collidableIndex, int userData)
{
b3RigidBodyData body;
int bodyIndex = m_data->m_rigidBodies.size();
body.m_invMass = mass ? 1.f/mass : 0.f;
body.m_angVel.setValue(0,0,0);
body.m_invMass = mass ? 1.f / mass : 0.f;
body.m_angVel.setValue(0, 0, 0);
body.m_collidableIdx = collidableIndex;
body.m_frictionCoeff = 0.3f;
body.m_linVel.setValue(0,0,0);
body.m_pos.setValue(position[0],position[1],position[2]);
body.m_quat.setValue(orientation[0],orientation[1],orientation[2],orientation[3]);
body.m_linVel.setValue(0, 0, 0);
body.m_pos.setValue(position[0], position[1], position[2]);
body.m_quat.setValue(orientation[0], orientation[1], orientation[2], orientation[3]);
body.m_restituitionCoeff = 0.f;
m_data->m_rigidBodies.push_back(body);
if (collidableIndex>=0)
if (collidableIndex >= 0)
{
b3Aabb& worldAabb = m_data->m_aabbWorldSpace.expand();
b3Aabb localAabb = m_data->m_np->getLocalSpaceAabb(collidableIndex);
b3Vector3 localAabbMin=b3MakeVector3(localAabb.m_min[0],localAabb.m_min[1],localAabb.m_min[2]);
b3Vector3 localAabbMax=b3MakeVector3(localAabb.m_max[0],localAabb.m_max[1],localAabb.m_max[2]);
b3Vector3 localAabbMin = b3MakeVector3(localAabb.m_min[0], localAabb.m_min[1], localAabb.m_min[2]);
b3Vector3 localAabbMax = b3MakeVector3(localAabb.m_max[0], localAabb.m_max[1], localAabb.m_max[2]);
b3Scalar margin = 0.01f;
b3Transform t;
t.setIdentity();
t.setOrigin(b3MakeVector3(position[0],position[1],position[2]));
t.setRotation(b3Quaternion(orientation[0],orientation[1],orientation[2],orientation[3]));
b3TransformAabb(localAabbMin,localAabbMax, margin,t,worldAabb.m_minVec,worldAabb.m_maxVec);
t.setOrigin(b3MakeVector3(position[0], position[1], position[2]));
t.setRotation(b3Quaternion(orientation[0], orientation[1], orientation[2], orientation[3]));
b3TransformAabb(localAabbMin, localAabbMax, margin, t, worldAabb.m_minVec, worldAabb.m_maxVec);
m_data->m_bp->createProxy(worldAabb.m_minVec,worldAabb.m_maxVec,bodyIndex,0,1,1);
// b3Vector3 aabbMin,aabbMax;
// m_data->m_bp->getAabb(bodyIndex,aabbMin,aabbMax);
} else
m_data->m_bp->createProxy(worldAabb.m_minVec, worldAabb.m_maxVec, bodyIndex, 0, 1, 1);
// b3Vector3 aabbMin,aabbMax;
// m_data->m_bp->getAabb(bodyIndex,aabbMin,aabbMax);
}
else
{
b3Error("registerPhysicsInstance using invalid collidableIndex\n");
}
@@ -476,13 +436,12 @@ int b3CpuRigidBodyPipeline::registerPhysicsInstance(float mass, const float* po
return bodyIndex;
}
const struct b3RigidBodyData* b3CpuRigidBodyPipeline::getBodyBuffer() const
{
return m_data->m_rigidBodies.size() ? &m_data->m_rigidBodies[0] : 0;
}
int b3CpuRigidBodyPipeline::getNumBodies() const
int b3CpuRigidBodyPipeline::getNumBodies() const
{
return m_data->m_rigidBodies.size();
}

45
src/Bullet3Dynamics/b3CpuRigidBodyPipeline.h
View File

@@ -16,52 +16,47 @@ subject to the following restrictions:
#ifndef B3_CPU_RIGIDBODY_PIPELINE_H
#define B3_CPU_RIGIDBODY_PIPELINE_H
#include "Bullet3Common/b3AlignedObjectArray.h"
#include "Bullet3Collision/NarrowPhaseCollision/b3RaycastInfo.h"
class b3CpuRigidBodyPipeline
{
protected:
struct b3CpuRigidBodyPipelineInternalData* m_data;
struct b3CpuRigidBodyPipelineInternalData* m_data;
int allocateCollidable();
public:
b3CpuRigidBodyPipeline(class b3CpuNarrowPhase* narrowphase, struct b3DynamicBvhBroadphase* broadphaseDbvt, const struct b3Config& config);
virtual ~b3CpuRigidBodyPipeline();
virtual void stepSimulation(float deltaTime);
virtual void integrate(float timeStep);
virtual void updateAabbWorldSpace();
virtual void computeOverlappingPairs();
virtual void computeContactPoints();
virtual void solveContactConstraints();
virtual void stepSimulation(float deltaTime);
virtual void integrate(float timeStep);
virtual void updateAabbWorldSpace();
virtual void computeOverlappingPairs();
virtual void computeContactPoints();
virtual void solveContactConstraints();
int registerConvexPolyhedron(class b3ConvexUtility* convex);
int registerConvexPolyhedron(class b3ConvexUtility* convex);
int registerPhysicsInstance(float mass, const float* position, const float* orientation, int collisionShapeIndex, int userData);
void writeAllInstancesToGpu();
void copyConstraintsToHost();
void setGravity(const float* grav);
void reset();
int createPoint2PointConstraint(int bodyA, int bodyB, const float* pivotInA, const float* pivotInB,float breakingThreshold);
int registerPhysicsInstance(float mass, const float* position, const float* orientation, int collisionShapeIndex, int userData);
void writeAllInstancesToGpu();
void copyConstraintsToHost();
void setGravity(const float* grav);
void reset();
int createPoint2PointConstraint(int bodyA, int bodyB, const float* pivotInA, const float* pivotInB, float breakingThreshold);
int createFixedConstraint(int bodyA, int bodyB, const float* pivotInA, const float* pivotInB, const float* relTargetAB, float breakingThreshold);
void removeConstraintByUid(int uid);
void addConstraint(class b3TypedConstraint* constraint);
void removeConstraint(b3TypedConstraint* constraint);
void addConstraint(class b3TypedConstraint* constraint);
void removeConstraint(b3TypedConstraint* constraint);
void castRays(const b3AlignedObjectArray<b3RayInfo>& rays, b3AlignedObjectArray<b3RayHit>& hitResults);
void castRays(const b3AlignedObjectArray<b3RayInfo>& rays, b3AlignedObjectArray<b3RayHit>& hitResults);
const struct b3RigidBodyData* getBodyBuffer() const;
int getNumBodies() const;
int getNumBodies() const;
};
#endif //B3_CPU_RIGIDBODY_PIPELINE_H
#endif //B3_CPU_RIGIDBODY_PIPELINE_H

19
src/Bullet3Dynamics/shared/b3ContactConstraint4.h
View File

@@ -5,30 +5,27 @@
typedef struct b3ContactConstraint4 b3ContactConstraint4_t;
struct b3ContactConstraint4
{
b3Float4 m_linear;//normal?
b3Float4 m_linear; //normal?
b3Float4 m_worldPos[4];
b3Float4 m_center; // friction
b3Float4 m_center; // friction
float m_jacCoeffInv[4];
float m_b[4];
float m_appliedRambdaDt[4];
float m_fJacCoeffInv[2]; // friction
float m_fAppliedRambdaDt[2]; // friction
float m_fJacCoeffInv[2]; // friction
float m_fAppliedRambdaDt[2]; // friction
unsigned int m_bodyA;
unsigned int m_bodyB;
int m_batchIdx;
int m_batchIdx;
unsigned int m_paddings;
};
//inline void setFrictionCoeff(float value) { m_linear[3] = value; }
inline float b3GetFrictionCoeff(b3ContactConstraint4_t* constraint)
inline float b3GetFrictionCoeff(b3ContactConstraint4_t* constraint)
{
return constraint->m_linear.w;
return constraint->m_linear.w;
}
#endif //B3_CONTACT_CONSTRAINT5_H
#endif //B3_CONTACT_CONSTRAINT5_H

133
src/Bullet3Dynamics/shared/b3ConvertConstraint4.h
View File

@@ -4,89 +4,84 @@
#include "Bullet3Dynamics/shared/b3ContactConstraint4.h"
#include "Bullet3Collision/NarrowPhaseCollision/shared/b3RigidBodyData.h"
void b3PlaneSpace1 (b3Float4ConstArg n, b3Float4* p, b3Float4* q);
void b3PlaneSpace1 (b3Float4ConstArg n, b3Float4* p, b3Float4* q)
void b3PlaneSpace1(b3Float4ConstArg n, b3Float4* p, b3Float4* q);
void b3PlaneSpace1(b3Float4ConstArg n, b3Float4* p, b3Float4* q)
{
if (b3Fabs(n.z) > 0.70710678f) {
// choose p in y-z plane
float a = n.y*n.y + n.z*n.z;
float k = 1.f/sqrt(a);
p[0].x = 0;
p[0].y = -n.z*k;
p[0].z = n.y*k;
// set q = n x p
q[0].x = a*k;
q[0].y = -n.x*p[0].z;
q[0].z = n.x*p[0].y;
}
else {
// choose p in x-y plane
float a = n.x*n.x + n.y*n.y;
float k = 1.f/sqrt(a);
p[0].x = -n.y*k;
p[0].y = n.x*k;
p[0].z = 0;
// set q = n x p
q[0].x = -n.z*p[0].y;
q[0].y = n.z*p[0].x;
q[0].z = a*k;
}
if (b3Fabs(n.z) > 0.70710678f)
{
// choose p in y-z plane
float a = n.y * n.y + n.z * n.z;
float k = 1.f / sqrt(a);
p[0].x = 0;
p[0].y = -n.z * k;
p[0].z = n.y * k;
// set q = n x p
q[0].x = a * k;
q[0].y = -n.x * p[0].z;
q[0].z = n.x * p[0].y;
}
else
{
// choose p in x-y plane
float a = n.x * n.x + n.y * n.y;
float k = 1.f / sqrt(a);
p[0].x = -n.y * k;
p[0].y = n.x * k;
p[0].z = 0;
// set q = n x p
q[0].x = -n.z * p[0].y;
q[0].y = n.z * p[0].x;
q[0].z = a * k;
}
}
void setLinearAndAngular( b3Float4ConstArg n, b3Float4ConstArg r0, b3Float4ConstArg r1, b3Float4* linear, b3Float4* angular0, b3Float4* angular1)
void setLinearAndAngular(b3Float4ConstArg n, b3Float4ConstArg r0, b3Float4ConstArg r1, b3Float4* linear, b3Float4* angular0, b3Float4* angular1)
{
*linear = b3MakeFloat4(n.x,n.y,n.z,0.f);
*linear = b3MakeFloat4(n.x, n.y, n.z, 0.f);
*angular0 = b3Cross3(r0, n);
*angular1 = -b3Cross3(r1, n);
}
float calcRelVel( b3Float4ConstArg l0, b3Float4ConstArg l1, b3Float4ConstArg a0, b3Float4ConstArg a1, b3Float4ConstArg linVel0,
b3Float4ConstArg angVel0, b3Float4ConstArg linVel1, b3Float4ConstArg angVel1 )
float calcRelVel(b3Float4ConstArg l0, b3Float4ConstArg l1, b3Float4ConstArg a0, b3Float4ConstArg a1, b3Float4ConstArg linVel0,
b3Float4ConstArg angVel0, b3Float4ConstArg linVel1, b3Float4ConstArg angVel1)
{
return b3Dot3F4(l0, linVel0) + b3Dot3F4(a0, angVel0) + b3Dot3F4(l1, linVel1) + b3Dot3F4(a1, angVel1);
}
float calcJacCoeff(b3Float4ConstArg linear0, b3Float4ConstArg linear1, b3Float4ConstArg angular0, b3Float4ConstArg angular1,
float invMass0, const b3Mat3x3* invInertia0, float invMass1, const b3Mat3x3* invInertia1)
float invMass0, const b3Mat3x3* invInertia0, float invMass1, const b3Mat3x3* invInertia1)
{
// linear0,1 are normlized
float jmj0 = invMass0;//b3Dot3F4(linear0, linear0)*invMass0;
float jmj1 = b3Dot3F4(mtMul3(angular0,*invInertia0), angular0);
float jmj2 = invMass1;//b3Dot3F4(linear1, linear1)*invMass1;
float jmj3 = b3Dot3F4(mtMul3(angular1,*invInertia1), angular1);
return -1.f/(jmj0+jmj1+jmj2+jmj3);
float jmj0 = invMass0; //b3Dot3F4(linear0, linear0)*invMass0;
float jmj1 = b3Dot3F4(mtMul3(angular0, *invInertia0), angular0);
float jmj2 = invMass1; //b3Dot3F4(linear1, linear1)*invMass1;
float jmj3 = b3Dot3F4(mtMul3(angular1, *invInertia1), angular1);
return -1.f / (jmj0 + jmj1 + jmj2 + jmj3);
}
void setConstraint4( b3Float4ConstArg posA, b3Float4ConstArg linVelA, b3Float4ConstArg angVelA, float invMassA, b3Mat3x3ConstArg invInertiaA,
b3Float4ConstArg posB, b3Float4ConstArg linVelB, b3Float4ConstArg angVelB, float invMassB, b3Mat3x3ConstArg invInertiaB,
__global struct b3Contact4Data* src, float dt, float positionDrift, float positionConstraintCoeff,
b3ContactConstraint4_t* dstC )
void setConstraint4(b3Float4ConstArg posA, b3Float4ConstArg linVelA, b3Float4ConstArg angVelA, float invMassA, b3Mat3x3ConstArg invInertiaA,
b3Float4ConstArg posB, b3Float4ConstArg linVelB, b3Float4ConstArg angVelB, float invMassB, b3Mat3x3ConstArg invInertiaB,
__global struct b3Contact4Data* src, float dt, float positionDrift, float positionConstraintCoeff,
b3ContactConstraint4_t* dstC)
{
dstC->m_bodyA = abs(src->m_bodyAPtrAndSignBit);
dstC->m_bodyB = abs(src->m_bodyBPtrAndSignBit);
float dtInv = 1.f/dt;
for(int ic=0; ic<4; ic++)
float dtInv = 1.f / dt;
for (int ic = 0; ic < 4; ic++)
{
dstC->m_appliedRambdaDt[ic] = 0.f;
}
dstC->m_fJacCoeffInv[0] = dstC->m_fJacCoeffInv[1] = 0.f;
dstC->m_linear = src->m_worldNormalOnB;
dstC->m_linear.w = 0.7f ;//src->getFrictionCoeff() );
for(int ic=0; ic<4; ic++)
dstC->m_linear.w = 0.7f; //src->getFrictionCoeff() );
for (int ic = 0; ic < 4; ic++)
{
b3Float4 r0 = src->m_worldPosB[ic] - posA;
b3Float4 r1 = src->m_worldPosB[ic] - posB;
if( ic >= src->m_worldNormalOnB.w )//npoints
if (ic >= src->m_worldNormalOnB.w) //npoints
{
dstC->m_jacCoeffInv[ic] = 0.f;
continue;
@@ -98,56 +93,56 @@ void setConstraint4( b3Float4ConstArg posA, b3Float4ConstArg linVelA, b3Float4Co
setLinearAndAngular(src->m_worldNormalOnB, r0, r1, &linear, &angular0, &angular1);
dstC->m_jacCoeffInv[ic] = calcJacCoeff(linear, -linear, angular0, angular1,
invMassA, &invInertiaA, invMassB, &invInertiaB );
invMassA, &invInertiaA, invMassB, &invInertiaB);
relVelN = calcRelVel(linear, -linear, angular0, angular1,
linVelA, angVelA, linVelB, angVelB);
linVelA, angVelA, linVelB, angVelB);
float e = 0.f;//src->getRestituitionCoeff();
if( relVelN*relVelN < 0.004f ) e = 0.f;
float e = 0.f; //src->getRestituitionCoeff();
if (relVelN * relVelN < 0.004f) e = 0.f;
dstC->m_b[ic] = e*relVelN;
dstC->m_b[ic] = e * relVelN;
//float penetration = src->m_worldPosB[ic].w;
dstC->m_b[ic] += (src->m_worldPosB[ic].w + positionDrift)*positionConstraintCoeff*dtInv;
dstC->m_b[ic] += (src->m_worldPosB[ic].w + positionDrift) * positionConstraintCoeff * dtInv;
dstC->m_appliedRambdaDt[ic] = 0.f;
}
}
if( src->m_worldNormalOnB.w > 0 )//npoints
{ // prepare friction
b3Float4 center = b3MakeFloat4(0.f,0.f,0.f,0.f);
for(int i=0; i<src->m_worldNormalOnB.w; i++)
if (src->m_worldNormalOnB.w > 0) //npoints
{ // prepare friction
b3Float4 center = b3MakeFloat4(0.f, 0.f, 0.f, 0.f);
for (int i = 0; i < src->m_worldNormalOnB.w; i++)
center += src->m_worldPosB[i];
center /= (float)src->m_worldNormalOnB.w;
b3Float4 tangent[2];
b3PlaneSpace1(src->m_worldNormalOnB,&tangent[0],&tangent[1]);
b3PlaneSpace1(src->m_worldNormalOnB, &tangent[0], &tangent[1]);
b3Float4 r[2];
r[0] = center - posA;
r[1] = center - posB;
for(int i=0; i<2; i++)
for (int i = 0; i < 2; i++)
{
b3Float4 linear, angular0, angular1;
setLinearAndAngular(tangent[i], r[0], r[1], &linear, &angular0, &angular1);
dstC->m_fJacCoeffInv[i] = calcJacCoeff(linear, -linear, angular0, angular1,
invMassA, &invInertiaA, invMassB, &invInertiaB );
invMassA, &invInertiaA, invMassB, &invInertiaB);
dstC->m_fAppliedRambdaDt[i] = 0.f;
}
dstC->m_center = center;
}
for(int i=0; i<4; i++)
for (int i = 0; i < 4; i++)
{
if( i<src->m_worldNormalOnB.w )
if (i < src->m_worldNormalOnB.w)
{
dstC->m_worldPos[i] = src->m_worldPosB[i];
}
else
{
dstC->m_worldPos[i] = b3MakeFloat4(0.f,0.f,0.f,0.f);
dstC->m_worldPos[i] = b3MakeFloat4(0.f, 0.f, 0.f, 0.f);
}
}
}

3
src/Bullet3Dynamics/shared/b3Inertia.h
View File

@@ -11,5 +11,4 @@ struct b3Inertia
b3Mat3x3 m_initInvInertia;
};
#endif //B3_INERTIA_H
#endif //B3_INERTIA_H

53
src/Bullet3Dynamics/shared/b3IntegrateTransforms.h
View File

@@ -2,11 +2,8 @@
#include "Bullet3Collision/NarrowPhaseCollision/shared/b3RigidBodyData.h"
inline void integrateSingleTransform( __global b3RigidBodyData_t* bodies,int nodeID, float timeStep, float angularDamping, b3Float4ConstArg gravityAcceleration)
inline void integrateSingleTransform(__global b3RigidBodyData_t* bodies, int nodeID, float timeStep, float angularDamping, b3Float4ConstArg gravityAcceleration)
{
if (bodies[nodeID].m_invMass != 0.f)
{
float BT_GPU_ANGULAR_MOTION_THRESHOLD = (0.25f * 3.14159254f);
@@ -18,27 +15,27 @@ inline void integrateSingleTransform( __global b3RigidBodyData_t* bodies,int nod
bodies[nodeID].m_angVel.x *= angularDamping;
bodies[nodeID].m_angVel.y *= angularDamping;
bodies[nodeID].m_angVel.z *= angularDamping;
b3Float4 angvel = bodies[nodeID].m_angVel;
float fAngle = b3Sqrt(b3Dot3F4(angvel, angvel));
//limit the angular motion
if(fAngle*timeStep > BT_GPU_ANGULAR_MOTION_THRESHOLD)
if (fAngle * timeStep > BT_GPU_ANGULAR_MOTION_THRESHOLD)
{
fAngle = BT_GPU_ANGULAR_MOTION_THRESHOLD / timeStep;
}
if(fAngle < 0.001f)
if (fAngle < 0.001f)
{
// use Taylor's expansions of sync function
axis = angvel * (0.5f*timeStep-(timeStep*timeStep*timeStep)*0.020833333333f * fAngle * fAngle);
axis = angvel * (0.5f * timeStep - (timeStep * timeStep * timeStep) * 0.020833333333f * fAngle * fAngle);
}
else
{
// sync(fAngle) = sin(c*fAngle)/t
axis = angvel * ( b3Sin(0.5f * fAngle * timeStep) / fAngle);
axis = angvel * (b3Sin(0.5f * fAngle * timeStep) / fAngle);
}
b3Quat dorn;
dorn.x = axis.x;
dorn.y = axis.y;
@@ -47,23 +44,21 @@ inline void integrateSingleTransform( __global b3RigidBodyData_t* bodies,int nod
b3Quat orn0 = bodies[nodeID].m_quat;
b3Quat predictedOrn = b3QuatMul(dorn, orn0);
predictedOrn = b3QuatNormalized(predictedOrn);
bodies[nodeID].m_quat=predictedOrn;
bodies[nodeID].m_quat = predictedOrn;
}
//linear velocity
bodies[nodeID].m_pos += bodies[nodeID].m_linVel * timeStep;
//linear velocity
bodies[nodeID].m_pos += bodies[nodeID].m_linVel * timeStep;
//apply gravity
bodies[nodeID].m_linVel += gravityAcceleration * timeStep;
}
}
inline void b3IntegrateTransform( __global b3RigidBodyData_t* body, float timeStep, float angularDamping, b3Float4ConstArg gravityAcceleration)
inline void b3IntegrateTransform(__global b3RigidBodyData_t* body, float timeStep, float angularDamping, b3Float4ConstArg gravityAcceleration)
{
float BT_GPU_ANGULAR_MOTION_THRESHOLD = (0.25f * 3.14159254f);
if( (body->m_invMass != 0.f))
if ((body->m_invMass != 0.f))
{
//angular velocity
{
@@ -72,23 +67,23 @@ inline void b3IntegrateTransform( __global b3RigidBodyData_t* body, float timeSt
body->m_angVel.x *= angularDamping;
body->m_angVel.y *= angularDamping;
body->m_angVel.z *= angularDamping;
b3Float4 angvel = body->m_angVel;
float fAngle = b3Sqrt(b3Dot3F4(angvel, angvel));
//limit the angular motion
if(fAngle*timeStep > BT_GPU_ANGULAR_MOTION_THRESHOLD)
if (fAngle * timeStep > BT_GPU_ANGULAR_MOTION_THRESHOLD)
{
fAngle = BT_GPU_ANGULAR_MOTION_THRESHOLD / timeStep;
}
if(fAngle < 0.001f)
if (fAngle < 0.001f)
{
// use Taylor's expansions of sync function
axis = angvel * (0.5f*timeStep-(timeStep*timeStep*timeStep)*0.020833333333f * fAngle * fAngle);
axis = angvel * (0.5f * timeStep - (timeStep * timeStep * timeStep) * 0.020833333333f * fAngle * fAngle);
}
else
{
// sync(fAngle) = sin(c*fAngle)/t
axis = angvel * ( b3Sin(0.5f * fAngle * timeStep) / fAngle);
axis = angvel * (b3Sin(0.5f * fAngle * timeStep) / fAngle);
}
b3Quat dorn;
dorn.x = axis.x;
@@ -99,15 +94,13 @@ inline void b3IntegrateTransform( __global b3RigidBodyData_t* body, float timeSt
b3Quat predictedOrn = b3QuatMul(dorn, orn0);
predictedOrn = b3QuatNormalized(predictedOrn);
body->m_quat=predictedOrn;
body->m_quat = predictedOrn;
}
//apply gravity
body->m_linVel += gravityAcceleration * timeStep;
//linear velocity
body->m_pos += body->m_linVel * timeStep;
//linear velocity
body->m_pos += body->m_linVel * timeStep;
}
}