Code-style consistency improvement:
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
File diff suppressed because it is too large
Load Diff
@@ -21,46 +21,42 @@ subject to the following restrictions:
|
||||
#include "BulletDynamics/ConstraintSolver/btSolverBody.h"
|
||||
#include "BulletDynamics/ConstraintSolver/btSolverConstraint.h"
|
||||
|
||||
|
||||
class btIDebugDraw;
|
||||
|
||||
struct btBatchedConstraints
|
||||
{
|
||||
enum BatchingMethod
|
||||
{
|
||||
BATCHING_METHOD_SPATIAL_GRID_2D,
|
||||
BATCHING_METHOD_SPATIAL_GRID_3D,
|
||||
BATCHING_METHOD_COUNT
|
||||
};
|
||||
struct Range
|
||||
{
|
||||
int begin;
|
||||
int end;
|
||||
enum BatchingMethod
|
||||
{
|
||||
BATCHING_METHOD_SPATIAL_GRID_2D,
|
||||
BATCHING_METHOD_SPATIAL_GRID_3D,
|
||||
BATCHING_METHOD_COUNT
|
||||
};
|
||||
struct Range
|
||||
{
|
||||
int begin;
|
||||
int end;
|
||||
|
||||
Range() : begin( 0 ), end( 0 ) {}
|
||||
Range( int _beg, int _end ) : begin( _beg ), end( _end ) {}
|
||||
};
|
||||
Range() : begin(0), end(0) {}
|
||||
Range(int _beg, int _end) : begin(_beg), end(_end) {}
|
||||
};
|
||||
|
||||
btAlignedObjectArray<int> m_constraintIndices;
|
||||
btAlignedObjectArray<Range> m_batches; // each batch is a range of indices in the m_constraintIndices array
|
||||
btAlignedObjectArray<Range> m_phases; // each phase is range of indices in the m_batches array
|
||||
btAlignedObjectArray<char> m_phaseGrainSize; // max grain size for each phase
|
||||
btAlignedObjectArray<int> m_phaseOrder; // phases can be done in any order, so we can randomize the order here
|
||||
btIDebugDraw* m_debugDrawer;
|
||||
btAlignedObjectArray<int> m_constraintIndices;
|
||||
btAlignedObjectArray<Range> m_batches; // each batch is a range of indices in the m_constraintIndices array
|
||||
btAlignedObjectArray<Range> m_phases; // each phase is range of indices in the m_batches array
|
||||
btAlignedObjectArray<char> m_phaseGrainSize; // max grain size for each phase
|
||||
btAlignedObjectArray<int> m_phaseOrder; // phases can be done in any order, so we can randomize the order here
|
||||
btIDebugDraw* m_debugDrawer;
|
||||
|
||||
static bool s_debugDrawBatches;
|
||||
static bool s_debugDrawBatches;
|
||||
|
||||
btBatchedConstraints() {m_debugDrawer=NULL;}
|
||||
void setup( btConstraintArray* constraints,
|
||||
const btAlignedObjectArray<btSolverBody>& bodies,
|
||||
BatchingMethod batchingMethod,
|
||||
int minBatchSize,
|
||||
int maxBatchSize,
|
||||
btAlignedObjectArray<char>* scratchMemory
|
||||
);
|
||||
bool validate( btConstraintArray* constraints, const btAlignedObjectArray<btSolverBody>& bodies ) const;
|
||||
btBatchedConstraints() { m_debugDrawer = NULL; }
|
||||
void setup(btConstraintArray* constraints,
|
||||
const btAlignedObjectArray<btSolverBody>& bodies,
|
||||
BatchingMethod batchingMethod,
|
||||
int minBatchSize,
|
||||
int maxBatchSize,
|
||||
btAlignedObjectArray<char>* scratchMemory);
|
||||
bool validate(btConstraintArray* constraints, const btAlignedObjectArray<btSolverBody>& bodies) const;
|
||||
};
|
||||
|
||||
|
||||
#endif // BT_BATCHED_CONSTRAINTS_H
|
||||
|
||||
#endif // BT_BATCHED_CONSTRAINTS_H
|
||||
|
||||
File diff suppressed because it is too large
Load Diff
@@ -15,8 +15,6 @@ subject to the following restrictions:
|
||||
Written by: Marcus Hennix
|
||||
*/
|
||||
|
||||
|
||||
|
||||
/*
|
||||
Overview:
|
||||
|
||||
@@ -31,8 +29,6 @@ twist is along the x-axis,
|
||||
and swing 1 and 2 are along the z and y axes respectively.
|
||||
*/
|
||||
|
||||
|
||||
|
||||
#ifndef BT_CONETWISTCONSTRAINT_H
|
||||
#define BT_CONETWISTCONSTRAINT_H
|
||||
|
||||
@@ -41,13 +37,12 @@ and swing 1 and 2 are along the z and y axes respectively.
|
||||
#include "btTypedConstraint.h"
|
||||
|
||||
#ifdef BT_USE_DOUBLE_PRECISION
|
||||
#define btConeTwistConstraintData2 btConeTwistConstraintDoubleData
|
||||
#define btConeTwistConstraintDataName "btConeTwistConstraintDoubleData"
|
||||
#define btConeTwistConstraintData2 btConeTwistConstraintDoubleData
|
||||
#define btConeTwistConstraintDataName "btConeTwistConstraintDoubleData"
|
||||
#else
|
||||
#define btConeTwistConstraintData2 btConeTwistConstraintData
|
||||
#define btConeTwistConstraintDataName "btConeTwistConstraintData"
|
||||
#endif //BT_USE_DOUBLE_PRECISION
|
||||
|
||||
#define btConeTwistConstraintData2 btConeTwistConstraintData
|
||||
#define btConeTwistConstraintDataName "btConeTwistConstraintData"
|
||||
#endif //BT_USE_DOUBLE_PRECISION
|
||||
|
||||
class btRigidBody;
|
||||
|
||||
@@ -59,103 +54,99 @@ enum btConeTwistFlags
|
||||
};
|
||||
|
||||
///btConeTwistConstraint can be used to simulate ragdoll joints (upper arm, leg etc)
|
||||
ATTRIBUTE_ALIGNED16(class) btConeTwistConstraint : public btTypedConstraint
|
||||
ATTRIBUTE_ALIGNED16(class)
|
||||
btConeTwistConstraint : public btTypedConstraint
|
||||
{
|
||||
#ifdef IN_PARALLELL_SOLVER
|
||||
public:
|
||||
#endif
|
||||
btJacobianEntry m_jac[3]; //3 orthogonal linear constraints
|
||||
btJacobianEntry m_jac[3]; //3 orthogonal linear constraints
|
||||
|
||||
btTransform m_rbAFrame;
|
||||
btTransform m_rbAFrame;
|
||||
btTransform m_rbBFrame;
|
||||
|
||||
btScalar m_limitSoftness;
|
||||
btScalar m_biasFactor;
|
||||
btScalar m_relaxationFactor;
|
||||
btScalar m_limitSoftness;
|
||||
btScalar m_biasFactor;
|
||||
btScalar m_relaxationFactor;
|
||||
|
||||
btScalar m_damping;
|
||||
btScalar m_damping;
|
||||
|
||||
btScalar m_swingSpan1;
|
||||
btScalar m_swingSpan2;
|
||||
btScalar m_twistSpan;
|
||||
btScalar m_swingSpan1;
|
||||
btScalar m_swingSpan2;
|
||||
btScalar m_twistSpan;
|
||||
|
||||
btScalar m_fixThresh;
|
||||
btScalar m_fixThresh;
|
||||
|
||||
btVector3 m_swingAxis;
|
||||
btVector3 m_twistAxis;
|
||||
btVector3 m_swingAxis;
|
||||
btVector3 m_twistAxis;
|
||||
|
||||
btScalar m_kSwing;
|
||||
btScalar m_kTwist;
|
||||
btScalar m_kSwing;
|
||||
btScalar m_kTwist;
|
||||
|
||||
btScalar m_twistLimitSign;
|
||||
btScalar m_swingCorrection;
|
||||
btScalar m_twistCorrection;
|
||||
btScalar m_twistLimitSign;
|
||||
btScalar m_swingCorrection;
|
||||
btScalar m_twistCorrection;
|
||||
|
||||
btScalar m_twistAngle;
|
||||
btScalar m_twistAngle;
|
||||
|
||||
btScalar m_accSwingLimitImpulse;
|
||||
btScalar m_accTwistLimitImpulse;
|
||||
btScalar m_accSwingLimitImpulse;
|
||||
btScalar m_accTwistLimitImpulse;
|
||||
|
||||
bool m_angularOnly;
|
||||
bool m_solveTwistLimit;
|
||||
bool m_solveSwingLimit;
|
||||
bool m_angularOnly;
|
||||
bool m_solveTwistLimit;
|
||||
bool m_solveSwingLimit;
|
||||
|
||||
bool m_useSolveConstraintObsolete;
|
||||
bool m_useSolveConstraintObsolete;
|
||||
|
||||
// not yet used...
|
||||
btScalar m_swingLimitRatio;
|
||||
btScalar m_twistLimitRatio;
|
||||
btVector3 m_twistAxisA;
|
||||
btScalar m_swingLimitRatio;
|
||||
btScalar m_twistLimitRatio;
|
||||
btVector3 m_twistAxisA;
|
||||
|
||||
// motor
|
||||
bool m_bMotorEnabled;
|
||||
bool m_bNormalizedMotorStrength;
|
||||
bool m_bMotorEnabled;
|
||||
bool m_bNormalizedMotorStrength;
|
||||
btQuaternion m_qTarget;
|
||||
btScalar m_maxMotorImpulse;
|
||||
btVector3 m_accMotorImpulse;
|
||||
|
||||
// parameters
|
||||
int m_flags;
|
||||
btScalar m_linCFM;
|
||||
btScalar m_linERP;
|
||||
btScalar m_angCFM;
|
||||
|
||||
protected:
|
||||
btScalar m_maxMotorImpulse;
|
||||
btVector3 m_accMotorImpulse;
|
||||
|
||||
// parameters
|
||||
int m_flags;
|
||||
btScalar m_linCFM;
|
||||
btScalar m_linERP;
|
||||
btScalar m_angCFM;
|
||||
|
||||
protected:
|
||||
void init();
|
||||
|
||||
void computeConeLimitInfo(const btQuaternion& qCone, // in
|
||||
btScalar& swingAngle, btVector3& vSwingAxis, btScalar& swingLimit); // all outs
|
||||
void computeConeLimitInfo(const btQuaternion& qCone, // in
|
||||
btScalar& swingAngle, btVector3& vSwingAxis, btScalar& swingLimit); // all outs
|
||||
|
||||
void computeTwistLimitInfo(const btQuaternion& qTwist, // in
|
||||
btScalar& twistAngle, btVector3& vTwistAxis); // all outs
|
||||
|
||||
void adjustSwingAxisToUseEllipseNormal(btVector3& vSwingAxis) const;
|
||||
void computeTwistLimitInfo(const btQuaternion& qTwist, // in
|
||||
btScalar& twistAngle, btVector3& vTwistAxis); // all outs
|
||||
|
||||
void adjustSwingAxisToUseEllipseNormal(btVector3 & vSwingAxis) const;
|
||||
|
||||
public:
|
||||
|
||||
BT_DECLARE_ALIGNED_ALLOCATOR();
|
||||
|
||||
btConeTwistConstraint(btRigidBody& rbA,btRigidBody& rbB,const btTransform& rbAFrame, const btTransform& rbBFrame);
|
||||
|
||||
btConeTwistConstraint(btRigidBody& rbA,const btTransform& rbAFrame);
|
||||
btConeTwistConstraint(btRigidBody & rbA, btRigidBody & rbB, const btTransform& rbAFrame, const btTransform& rbBFrame);
|
||||
|
||||
virtual void buildJacobian();
|
||||
btConeTwistConstraint(btRigidBody & rbA, const btTransform& rbAFrame);
|
||||
|
||||
virtual void getInfo1 (btConstraintInfo1* info);
|
||||
virtual void buildJacobian();
|
||||
|
||||
void getInfo1NonVirtual(btConstraintInfo1* info);
|
||||
|
||||
virtual void getInfo2 (btConstraintInfo2* info);
|
||||
|
||||
void getInfo2NonVirtual(btConstraintInfo2* info,const btTransform& transA,const btTransform& transB,const btMatrix3x3& invInertiaWorldA,const btMatrix3x3& invInertiaWorldB);
|
||||
virtual void getInfo1(btConstraintInfo1 * info);
|
||||
|
||||
virtual void solveConstraintObsolete(btSolverBody& bodyA,btSolverBody& bodyB,btScalar timeStep);
|
||||
void getInfo1NonVirtual(btConstraintInfo1 * info);
|
||||
|
||||
|
||||
void updateRHS(btScalar timeStep);
|
||||
virtual void getInfo2(btConstraintInfo2 * info);
|
||||
|
||||
void getInfo2NonVirtual(btConstraintInfo2 * info, const btTransform& transA, const btTransform& transB, const btMatrix3x3& invInertiaWorldA, const btMatrix3x3& invInertiaWorldB);
|
||||
|
||||
virtual void solveConstraintObsolete(btSolverBody & bodyA, btSolverBody & bodyB, btScalar timeStep);
|
||||
|
||||
void updateRHS(btScalar timeStep);
|
||||
|
||||
const btRigidBody& getRigidBodyA() const
|
||||
{
|
||||
@@ -166,64 +157,64 @@ public:
|
||||
return m_rbB;
|
||||
}
|
||||
|
||||
void setAngularOnly(bool angularOnly)
|
||||
void setAngularOnly(bool angularOnly)
|
||||
{
|
||||
m_angularOnly = angularOnly;
|
||||
}
|
||||
|
||||
bool getAngularOnly() const
|
||||
|
||||
bool getAngularOnly() const
|
||||
{
|
||||
return m_angularOnly;
|
||||
return m_angularOnly;
|
||||
}
|
||||
|
||||
void setLimit(int limitIndex,btScalar limitValue)
|
||||
void setLimit(int limitIndex, btScalar limitValue)
|
||||
{
|
||||
switch (limitIndex)
|
||||
{
|
||||
case 3:
|
||||
case 3:
|
||||
{
|
||||
m_twistSpan = limitValue;
|
||||
break;
|
||||
}
|
||||
case 4:
|
||||
case 4:
|
||||
{
|
||||
m_swingSpan2 = limitValue;
|
||||
break;
|
||||
}
|
||||
case 5:
|
||||
case 5:
|
||||
{
|
||||
m_swingSpan1 = limitValue;
|
||||
break;
|
||||
}
|
||||
default:
|
||||
default:
|
||||
{
|
||||
}
|
||||
};
|
||||
}
|
||||
|
||||
btScalar getLimit(int limitIndex) const
|
||||
btScalar getLimit(int limitIndex) const
|
||||
{
|
||||
switch (limitIndex)
|
||||
{
|
||||
case 3:
|
||||
case 3:
|
||||
{
|
||||
return m_twistSpan;
|
||||
break;
|
||||
}
|
||||
case 4:
|
||||
case 4:
|
||||
{
|
||||
return m_swingSpan2;
|
||||
break;
|
||||
}
|
||||
case 5:
|
||||
case 5:
|
||||
{
|
||||
return m_swingSpan1;
|
||||
break;
|
||||
}
|
||||
default:
|
||||
default:
|
||||
{
|
||||
btAssert(0 && "Invalid limitIndex specified for btConeTwistConstraint");
|
||||
return 0.0;
|
||||
btAssert(0 && "Invalid limitIndex specified for btConeTwistConstraint");
|
||||
return 0.0;
|
||||
}
|
||||
};
|
||||
}
|
||||
@@ -239,18 +230,18 @@ public:
|
||||
// __relaxationFactor:
|
||||
// 0->1, recommend to stay near 1.
|
||||
// the lower the value, the less the constraint will fight velocities which violate the angular limits.
|
||||
void setLimit(btScalar _swingSpan1,btScalar _swingSpan2,btScalar _twistSpan, btScalar _softness = 1.f, btScalar _biasFactor = 0.3f, btScalar _relaxationFactor = 1.0f)
|
||||
void setLimit(btScalar _swingSpan1, btScalar _swingSpan2, btScalar _twistSpan, btScalar _softness = 1.f, btScalar _biasFactor = 0.3f, btScalar _relaxationFactor = 1.0f)
|
||||
{
|
||||
m_swingSpan1 = _swingSpan1;
|
||||
m_swingSpan2 = _swingSpan2;
|
||||
m_twistSpan = _twistSpan;
|
||||
m_twistSpan = _twistSpan;
|
||||
|
||||
m_limitSoftness = _softness;
|
||||
m_limitSoftness = _softness;
|
||||
m_biasFactor = _biasFactor;
|
||||
m_relaxationFactor = _relaxationFactor;
|
||||
}
|
||||
|
||||
const btTransform& getAFrame() const { return m_rbAFrame; };
|
||||
const btTransform& getAFrame() const { return m_rbAFrame; };
|
||||
const btTransform& getBFrame() const { return m_rbBFrame; };
|
||||
|
||||
inline int getSolveTwistLimit()
|
||||
@@ -269,7 +260,7 @@ public:
|
||||
}
|
||||
|
||||
void calcAngleInfo();
|
||||
void calcAngleInfo2(const btTransform& transA, const btTransform& transB,const btMatrix3x3& invInertiaWorldA,const btMatrix3x3& invInertiaWorldB);
|
||||
void calcAngleInfo2(const btTransform& transA, const btTransform& transB, const btMatrix3x3& invInertiaWorldA, const btMatrix3x3& invInertiaWorldB);
|
||||
|
||||
inline btScalar getSwingSpan1() const
|
||||
{
|
||||
@@ -308,8 +299,16 @@ public:
|
||||
bool isMotorEnabled() const { return m_bMotorEnabled; }
|
||||
btScalar getMaxMotorImpulse() const { return m_maxMotorImpulse; }
|
||||
bool isMaxMotorImpulseNormalized() const { return m_bNormalizedMotorStrength; }
|
||||
void setMaxMotorImpulse(btScalar maxMotorImpulse) { m_maxMotorImpulse = maxMotorImpulse; m_bNormalizedMotorStrength = false; }
|
||||
void setMaxMotorImpulseNormalized(btScalar maxMotorImpulse) { m_maxMotorImpulse = maxMotorImpulse; m_bNormalizedMotorStrength = true; }
|
||||
void setMaxMotorImpulse(btScalar maxMotorImpulse)
|
||||
{
|
||||
m_maxMotorImpulse = maxMotorImpulse;
|
||||
m_bNormalizedMotorStrength = false;
|
||||
}
|
||||
void setMaxMotorImpulseNormalized(btScalar maxMotorImpulse)
|
||||
{
|
||||
m_maxMotorImpulse = maxMotorImpulse;
|
||||
m_bNormalizedMotorStrength = true;
|
||||
}
|
||||
|
||||
btScalar getFixThresh() { return m_fixThresh; }
|
||||
void setFixThresh(btScalar fixThresh) { m_fixThresh = fixThresh; }
|
||||
@@ -318,17 +317,17 @@ public:
|
||||
// q: the desired rotation of bodyA wrt bodyB.
|
||||
// note: if q violates the joint limits, the internal target is clamped to avoid conflicting impulses (very bad for stability)
|
||||
// note: don't forget to enableMotor()
|
||||
void setMotorTarget(const btQuaternion &q);
|
||||
void setMotorTarget(const btQuaternion& q);
|
||||
const btQuaternion& getMotorTarget() const { return m_qTarget; }
|
||||
|
||||
// same as above, but q is the desired rotation of frameA wrt frameB in constraint space
|
||||
void setMotorTargetInConstraintSpace(const btQuaternion &q);
|
||||
void setMotorTargetInConstraintSpace(const btQuaternion& q);
|
||||
|
||||
btVector3 GetPointForAngle(btScalar fAngleInRadians, btScalar fLength) const;
|
||||
|
||||
///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, btScalar value, int axis = -1);
|
||||
virtual void setParam(int num, btScalar value, int axis = -1);
|
||||
|
||||
virtual void setFrames(const btTransform& frameA, const btTransform& frameB);
|
||||
|
||||
@@ -342,84 +341,74 @@ public:
|
||||
return m_rbBFrame;
|
||||
}
|
||||
|
||||
|
||||
///return the local value of parameter
|
||||
virtual btScalar getParam(int num, int axis = -1) const;
|
||||
virtual btScalar getParam(int num, int axis = -1) const;
|
||||
|
||||
int getFlags() const
|
||||
{
|
||||
return m_flags;
|
||||
}
|
||||
|
||||
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, btSerializer* serializer) const;
|
||||
|
||||
virtual const char* serialize(void* dataBuffer, btSerializer* serializer) const;
|
||||
};
|
||||
|
||||
|
||||
|
||||
struct btConeTwistConstraintDoubleData
|
||||
struct btConeTwistConstraintDoubleData
|
||||
{
|
||||
btTypedConstraintDoubleData m_typeConstraintData;
|
||||
btTypedConstraintDoubleData m_typeConstraintData;
|
||||
btTransformDoubleData m_rbAFrame;
|
||||
btTransformDoubleData m_rbBFrame;
|
||||
|
||||
//limits
|
||||
double m_swingSpan1;
|
||||
double m_swingSpan2;
|
||||
double m_twistSpan;
|
||||
double m_limitSoftness;
|
||||
double m_biasFactor;
|
||||
double m_relaxationFactor;
|
||||
|
||||
double m_damping;
|
||||
|
||||
|
||||
double m_swingSpan1;
|
||||
double m_swingSpan2;
|
||||
double m_twistSpan;
|
||||
double m_limitSoftness;
|
||||
double m_biasFactor;
|
||||
double m_relaxationFactor;
|
||||
|
||||
double m_damping;
|
||||
};
|
||||
|
||||
#ifdef BT_BACKWARDS_COMPATIBLE_SERIALIZATION
|
||||
///this structure is not used, except for loading pre-2.82 .bullet files
|
||||
struct btConeTwistConstraintData
|
||||
struct btConeTwistConstraintData
|
||||
{
|
||||
btTypedConstraintData m_typeConstraintData;
|
||||
btTypedConstraintData m_typeConstraintData;
|
||||
btTransformFloatData m_rbAFrame;
|
||||
btTransformFloatData m_rbBFrame;
|
||||
|
||||
//limits
|
||||
float m_swingSpan1;
|
||||
float m_swingSpan2;
|
||||
float m_twistSpan;
|
||||
float m_limitSoftness;
|
||||
float m_biasFactor;
|
||||
float m_relaxationFactor;
|
||||
float m_swingSpan1;
|
||||
float m_swingSpan2;
|
||||
float m_twistSpan;
|
||||
float m_limitSoftness;
|
||||
float m_biasFactor;
|
||||
float m_relaxationFactor;
|
||||
|
||||
float m_damping;
|
||||
|
||||
float m_damping;
|
||||
|
||||
char m_pad[4];
|
||||
|
||||
};
|
||||
#endif //BT_BACKWARDS_COMPATIBLE_SERIALIZATION
|
||||
#endif //BT_BACKWARDS_COMPATIBLE_SERIALIZATION
|
||||
//
|
||||
|
||||
SIMD_FORCE_INLINE int btConeTwistConstraint::calculateSerializeBufferSize() const
|
||||
SIMD_FORCE_INLINE int btConeTwistConstraint::calculateSerializeBufferSize() const
|
||||
{
|
||||
return sizeof(btConeTwistConstraintData2);
|
||||
|
||||
}
|
||||
|
||||
|
||||
///fills the dataBuffer and returns the struct name (and 0 on failure)
|
||||
SIMD_FORCE_INLINE const char* btConeTwistConstraint::serialize(void* dataBuffer, btSerializer* serializer) const
|
||||
///fills the dataBuffer and returns the struct name (and 0 on failure)
|
||||
SIMD_FORCE_INLINE const char* btConeTwistConstraint::serialize(void* dataBuffer, btSerializer* serializer) const
|
||||
{
|
||||
btConeTwistConstraintData2* cone = (btConeTwistConstraintData2*) dataBuffer;
|
||||
btTypedConstraint::serialize(&cone->m_typeConstraintData,serializer);
|
||||
btConeTwistConstraintData2* cone = (btConeTwistConstraintData2*)dataBuffer;
|
||||
btTypedConstraint::serialize(&cone->m_typeConstraintData, serializer);
|
||||
|
||||
m_rbAFrame.serialize(cone->m_rbAFrame);
|
||||
m_rbBFrame.serialize(cone->m_rbBFrame);
|
||||
|
||||
|
||||
cone->m_swingSpan1 = m_swingSpan1;
|
||||
cone->m_swingSpan2 = m_swingSpan2;
|
||||
cone->m_twistSpan = m_twistSpan;
|
||||
@@ -431,5 +420,4 @@ SIMD_FORCE_INLINE const char* btConeTwistConstraint::serialize(void* dataBuffer,
|
||||
return btConeTwistConstraintDataName;
|
||||
}
|
||||
|
||||
|
||||
#endif //BT_CONETWISTCONSTRAINT_H
|
||||
#endif //BT_CONETWISTCONSTRAINT_H
|
||||
|
||||
@@ -26,41 +26,33 @@ struct btContactSolverInfo;
|
||||
struct btBroadphaseProxy;
|
||||
class btIDebugDraw;
|
||||
class btStackAlloc;
|
||||
class btDispatcher;
|
||||
class btDispatcher;
|
||||
/// btConstraintSolver provides solver interface
|
||||
|
||||
|
||||
enum btConstraintSolverType
|
||||
{
|
||||
BT_SEQUENTIAL_IMPULSE_SOLVER=1,
|
||||
BT_MLCP_SOLVER=2,
|
||||
BT_NNCG_SOLVER=4,
|
||||
BT_MULTIBODY_SOLVER=8,
|
||||
BT_SEQUENTIAL_IMPULSE_SOLVER = 1,
|
||||
BT_MLCP_SOLVER = 2,
|
||||
BT_NNCG_SOLVER = 4,
|
||||
BT_MULTIBODY_SOLVER = 8,
|
||||
};
|
||||
|
||||
class btConstraintSolver
|
||||
{
|
||||
|
||||
public:
|
||||
|
||||
virtual ~btConstraintSolver() {}
|
||||
|
||||
virtual void prepareSolve (int /* numBodies */, int /* numManifolds */) {;}
|
||||
|
||||
virtual void prepareSolve(int /* numBodies */, int /* numManifolds */) { ; }
|
||||
|
||||
///solve a group of constraints
|
||||
virtual btScalar solveGroup(btCollisionObject** bodies,int numBodies,btPersistentManifold** manifold,int numManifolds,btTypedConstraint** constraints,int numConstraints, const btContactSolverInfo& info,class btIDebugDraw* debugDrawer,btDispatcher* dispatcher) = 0;
|
||||
virtual btScalar solveGroup(btCollisionObject** bodies, int numBodies, btPersistentManifold** manifold, int numManifolds, btTypedConstraint** constraints, int numConstraints, const btContactSolverInfo& info, class btIDebugDraw* debugDrawer, btDispatcher* dispatcher) = 0;
|
||||
|
||||
virtual void allSolved (const btContactSolverInfo& /* info */,class btIDebugDraw* /* debugDrawer */) {;}
|
||||
virtual void allSolved(const btContactSolverInfo& /* info */, class btIDebugDraw* /* debugDrawer */) { ; }
|
||||
|
||||
///clear internal cached data and reset random seed
|
||||
virtual void reset() = 0;
|
||||
|
||||
virtual btConstraintSolverType getSolverType() const=0;
|
||||
|
||||
virtual void reset() = 0;
|
||||
|
||||
virtual btConstraintSolverType getSolverType() const = 0;
|
||||
};
|
||||
|
||||
|
||||
|
||||
|
||||
#endif //BT_CONSTRAINT_SOLVER_H
|
||||
#endif //BT_CONSTRAINT_SOLVER_H
|
||||
|
||||
@@ -13,7 +13,6 @@ subject to the following restrictions:
|
||||
3. This notice may not be removed or altered from any source distribution.
|
||||
*/
|
||||
|
||||
|
||||
#include "btContactConstraint.h"
|
||||
#include "BulletDynamics/Dynamics/btRigidBody.h"
|
||||
#include "LinearMath/btVector3.h"
|
||||
@@ -22,44 +21,33 @@ subject to the following restrictions:
|
||||
#include "LinearMath/btMinMax.h"
|
||||
#include "BulletCollision/NarrowPhaseCollision/btManifoldPoint.h"
|
||||
|
||||
|
||||
|
||||
btContactConstraint::btContactConstraint(btPersistentManifold* contactManifold,btRigidBody& rbA,btRigidBody& rbB)
|
||||
:btTypedConstraint(CONTACT_CONSTRAINT_TYPE,rbA,rbB),
|
||||
m_contactManifold(*contactManifold)
|
||||
btContactConstraint::btContactConstraint(btPersistentManifold* contactManifold, btRigidBody& rbA, btRigidBody& rbB)
|
||||
: btTypedConstraint(CONTACT_CONSTRAINT_TYPE, rbA, rbB),
|
||||
m_contactManifold(*contactManifold)
|
||||
{
|
||||
|
||||
}
|
||||
|
||||
btContactConstraint::~btContactConstraint()
|
||||
{
|
||||
|
||||
}
|
||||
|
||||
void btContactConstraint::setContactManifold(btPersistentManifold* contactManifold)
|
||||
void btContactConstraint::setContactManifold(btPersistentManifold* contactManifold)
|
||||
{
|
||||
m_contactManifold = *contactManifold;
|
||||
}
|
||||
|
||||
void btContactConstraint::getInfo1 (btConstraintInfo1* info)
|
||||
void btContactConstraint::getInfo1(btConstraintInfo1* info)
|
||||
{
|
||||
|
||||
}
|
||||
|
||||
void btContactConstraint::getInfo2 (btConstraintInfo2* info)
|
||||
void btContactConstraint::getInfo2(btConstraintInfo2* info)
|
||||
{
|
||||
|
||||
}
|
||||
|
||||
void btContactConstraint::buildJacobian()
|
||||
void btContactConstraint::buildJacobian()
|
||||
{
|
||||
|
||||
}
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
#include "btContactConstraint.h"
|
||||
#include "BulletDynamics/Dynamics/btRigidBody.h"
|
||||
#include "LinearMath/btVector3.h"
|
||||
@@ -68,64 +56,59 @@ void btContactConstraint::buildJacobian()
|
||||
#include "LinearMath/btMinMax.h"
|
||||
#include "BulletCollision/NarrowPhaseCollision/btManifoldPoint.h"
|
||||
|
||||
|
||||
|
||||
//response between two dynamic objects without friction and no restitution, assuming 0 penetration depth
|
||||
btScalar resolveSingleCollision(
|
||||
btRigidBody* body1,
|
||||
btCollisionObject* colObj2,
|
||||
const btVector3& contactPositionWorld,
|
||||
const btVector3& contactNormalOnB,
|
||||
const btContactSolverInfo& solverInfo,
|
||||
btScalar distance)
|
||||
btRigidBody* body1,
|
||||
btCollisionObject* colObj2,
|
||||
const btVector3& contactPositionWorld,
|
||||
const btVector3& contactNormalOnB,
|
||||
const btContactSolverInfo& solverInfo,
|
||||
btScalar distance)
|
||||
{
|
||||
btRigidBody* body2 = btRigidBody::upcast(colObj2);
|
||||
|
||||
|
||||
const btVector3& normal = contactNormalOnB;
|
||||
|
||||
btVector3 rel_pos1 = contactPositionWorld - body1->getWorldTransform().getOrigin();
|
||||
btVector3 rel_pos2 = contactPositionWorld - colObj2->getWorldTransform().getOrigin();
|
||||
|
||||
btVector3 vel1 = body1->getVelocityInLocalPoint(rel_pos1);
|
||||
btVector3 vel2 = body2? body2->getVelocityInLocalPoint(rel_pos2) : btVector3(0,0,0);
|
||||
btVector3 vel = vel1 - vel2;
|
||||
btScalar rel_vel;
|
||||
rel_vel = normal.dot(vel);
|
||||
|
||||
btScalar combinedRestitution = 0.f;
|
||||
btScalar restitution = combinedRestitution* -rel_vel;
|
||||
const btVector3& normal = contactNormalOnB;
|
||||
|
||||
btScalar positionalError = solverInfo.m_erp *-distance /solverInfo.m_timeStep ;
|
||||
btScalar velocityError = -(1.0f + restitution) * rel_vel;// * damping;
|
||||
btScalar denom0 = body1->computeImpulseDenominator(contactPositionWorld,normal);
|
||||
btScalar denom1 = body2? body2->computeImpulseDenominator(contactPositionWorld,normal) : 0.f;
|
||||
btVector3 rel_pos1 = contactPositionWorld - body1->getWorldTransform().getOrigin();
|
||||
btVector3 rel_pos2 = contactPositionWorld - colObj2->getWorldTransform().getOrigin();
|
||||
|
||||
btVector3 vel1 = body1->getVelocityInLocalPoint(rel_pos1);
|
||||
btVector3 vel2 = body2 ? body2->getVelocityInLocalPoint(rel_pos2) : btVector3(0, 0, 0);
|
||||
btVector3 vel = vel1 - vel2;
|
||||
btScalar rel_vel;
|
||||
rel_vel = normal.dot(vel);
|
||||
|
||||
btScalar combinedRestitution = 0.f;
|
||||
btScalar restitution = combinedRestitution * -rel_vel;
|
||||
|
||||
btScalar positionalError = solverInfo.m_erp * -distance / solverInfo.m_timeStep;
|
||||
btScalar velocityError = -(1.0f + restitution) * rel_vel; // * damping;
|
||||
btScalar denom0 = body1->computeImpulseDenominator(contactPositionWorld, normal);
|
||||
btScalar denom1 = body2 ? body2->computeImpulseDenominator(contactPositionWorld, normal) : 0.f;
|
||||
btScalar relaxation = 1.f;
|
||||
btScalar jacDiagABInv = relaxation/(denom0+denom1);
|
||||
btScalar jacDiagABInv = relaxation / (denom0 + denom1);
|
||||
|
||||
btScalar penetrationImpulse = positionalError * jacDiagABInv;
|
||||
btScalar velocityImpulse = velocityError * jacDiagABInv;
|
||||
btScalar penetrationImpulse = positionalError * jacDiagABInv;
|
||||
btScalar velocityImpulse = velocityError * jacDiagABInv;
|
||||
|
||||
btScalar normalImpulse = penetrationImpulse+velocityImpulse;
|
||||
normalImpulse = 0.f > normalImpulse ? 0.f: normalImpulse;
|
||||
btScalar normalImpulse = penetrationImpulse + velocityImpulse;
|
||||
normalImpulse = 0.f > normalImpulse ? 0.f : normalImpulse;
|
||||
|
||||
body1->applyImpulse(normal*(normalImpulse), rel_pos1);
|
||||
if (body2)
|
||||
body2->applyImpulse(-normal*(normalImpulse), rel_pos2);
|
||||
|
||||
return normalImpulse;
|
||||
body1->applyImpulse(normal * (normalImpulse), rel_pos1);
|
||||
if (body2)
|
||||
body2->applyImpulse(-normal * (normalImpulse), rel_pos2);
|
||||
|
||||
return normalImpulse;
|
||||
}
|
||||
|
||||
|
||||
//bilateral constraint between two dynamic objects
|
||||
void resolveSingleBilateral(btRigidBody& body1, const btVector3& pos1,
|
||||
btRigidBody& body2, const btVector3& pos2,
|
||||
btScalar distance, const btVector3& normal,btScalar& impulse ,btScalar timeStep)
|
||||
btRigidBody& body2, const btVector3& pos2,
|
||||
btScalar distance, const btVector3& normal, btScalar& impulse, btScalar timeStep)
|
||||
{
|
||||
(void)timeStep;
|
||||
(void)distance;
|
||||
|
||||
|
||||
btScalar normalLenSqr = normal.length2();
|
||||
btAssert(btFabs(normalLenSqr) < btScalar(1.1));
|
||||
if (normalLenSqr > btScalar(1.1))
|
||||
@@ -133,45 +116,38 @@ void resolveSingleBilateral(btRigidBody& body1, const btVector3& pos1,
|
||||
impulse = btScalar(0.);
|
||||
return;
|
||||
}
|
||||
btVector3 rel_pos1 = pos1 - body1.getCenterOfMassPosition();
|
||||
btVector3 rel_pos1 = pos1 - body1.getCenterOfMassPosition();
|
||||
btVector3 rel_pos2 = pos2 - body2.getCenterOfMassPosition();
|
||||
//this jacobian entry could be re-used for all iterations
|
||||
|
||||
|
||||
btVector3 vel1 = body1.getVelocityInLocalPoint(rel_pos1);
|
||||
btVector3 vel2 = body2.getVelocityInLocalPoint(rel_pos2);
|
||||
btVector3 vel = vel1 - vel2;
|
||||
|
||||
|
||||
btJacobianEntry jac(body1.getCenterOfMassTransform().getBasis().transpose(),
|
||||
body2.getCenterOfMassTransform().getBasis().transpose(),
|
||||
rel_pos1,rel_pos2,normal,body1.getInvInertiaDiagLocal(),body1.getInvMass(),
|
||||
body2.getInvInertiaDiagLocal(),body2.getInvMass());
|
||||
btJacobianEntry jac(body1.getCenterOfMassTransform().getBasis().transpose(),
|
||||
body2.getCenterOfMassTransform().getBasis().transpose(),
|
||||
rel_pos1, rel_pos2, normal, body1.getInvInertiaDiagLocal(), body1.getInvMass(),
|
||||
body2.getInvInertiaDiagLocal(), body2.getInvMass());
|
||||
|
||||
btScalar jacDiagAB = jac.getDiagonal();
|
||||
btScalar jacDiagABInv = btScalar(1.) / jacDiagAB;
|
||||
|
||||
btScalar rel_vel = jac.getRelativeVelocity(
|
||||
|
||||
btScalar rel_vel = jac.getRelativeVelocity(
|
||||
body1.getLinearVelocity(),
|
||||
body1.getCenterOfMassTransform().getBasis().transpose() * body1.getAngularVelocity(),
|
||||
body2.getLinearVelocity(),
|
||||
body2.getCenterOfMassTransform().getBasis().transpose() * body2.getAngularVelocity());
|
||||
|
||||
|
||||
body2.getCenterOfMassTransform().getBasis().transpose() * body2.getAngularVelocity());
|
||||
|
||||
rel_vel = normal.dot(vel);
|
||||
|
||||
|
||||
//todo: move this into proper structure
|
||||
btScalar contactDamping = btScalar(0.2);
|
||||
|
||||
#ifdef ONLY_USE_LINEAR_MASS
|
||||
btScalar massTerm = btScalar(1.) / (body1.getInvMass() + body2.getInvMass());
|
||||
impulse = - contactDamping * rel_vel * massTerm;
|
||||
#else
|
||||
impulse = -contactDamping * rel_vel * massTerm;
|
||||
#else
|
||||
btScalar velocityImpulse = -contactDamping * rel_vel * jacDiagABInv;
|
||||
impulse = velocityImpulse;
|
||||
#endif
|
||||
}
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
@@ -22,20 +22,17 @@ subject to the following restrictions:
|
||||
#include "BulletCollision/NarrowPhaseCollision/btPersistentManifold.h"
|
||||
|
||||
///btContactConstraint can be automatically created to solve contact constraints using the unified btTypedConstraint interface
|
||||
ATTRIBUTE_ALIGNED16(class) btContactConstraint : public btTypedConstraint
|
||||
ATTRIBUTE_ALIGNED16(class)
|
||||
btContactConstraint : public btTypedConstraint
|
||||
{
|
||||
protected:
|
||||
|
||||
btPersistentManifold m_contactManifold;
|
||||
|
||||
protected:
|
||||
|
||||
|
||||
btContactConstraint(btPersistentManifold* contactManifold,btRigidBody& rbA,btRigidBody& rbB);
|
||||
btContactConstraint(btPersistentManifold * contactManifold, btRigidBody & rbA, btRigidBody & rbB);
|
||||
|
||||
public:
|
||||
|
||||
void setContactManifold(btPersistentManifold* contactManifold);
|
||||
void setContactManifold(btPersistentManifold * contactManifold);
|
||||
|
||||
btPersistentManifold* getContactManifold()
|
||||
{
|
||||
@@ -49,25 +46,20 @@ public:
|
||||
|
||||
virtual ~btContactConstraint();
|
||||
|
||||
virtual void getInfo1 (btConstraintInfo1* info);
|
||||
virtual void getInfo1(btConstraintInfo1 * info);
|
||||
|
||||
virtual void getInfo2 (btConstraintInfo2* info);
|
||||
virtual void getInfo2(btConstraintInfo2 * info);
|
||||
|
||||
///obsolete methods
|
||||
virtual void buildJacobian();
|
||||
|
||||
|
||||
virtual void buildJacobian();
|
||||
};
|
||||
|
||||
///very basic collision resolution without friction
|
||||
btScalar resolveSingleCollision(btRigidBody* body1, class btCollisionObject* colObj2, const btVector3& contactPositionWorld,const btVector3& contactNormalOnB, const struct btContactSolverInfo& solverInfo,btScalar distance);
|
||||
|
||||
btScalar resolveSingleCollision(btRigidBody* body1, class btCollisionObject* colObj2, const btVector3& contactPositionWorld, const btVector3& contactNormalOnB, const struct btContactSolverInfo& solverInfo, btScalar distance);
|
||||
|
||||
///resolveSingleBilateral is an obsolete methods used for vehicle friction between two dynamic objects
|
||||
void resolveSingleBilateral(btRigidBody& body1, const btVector3& pos1,
|
||||
btRigidBody& body2, const btVector3& pos2,
|
||||
btScalar distance, const btVector3& normal,btScalar& impulse ,btScalar timeStep);
|
||||
btRigidBody& body2, const btVector3& pos2,
|
||||
btScalar distance, const btVector3& normal, btScalar& impulse, btScalar timeStep);
|
||||
|
||||
|
||||
|
||||
#endif //BT_CONTACT_CONSTRAINT_H
|
||||
#endif //BT_CONTACT_CONSTRAINT_H
|
||||
|
||||
@@ -18,7 +18,7 @@ subject to the following restrictions:
|
||||
|
||||
#include "LinearMath/btScalar.h"
|
||||
|
||||
enum btSolverMode
|
||||
enum btSolverMode
|
||||
{
|
||||
SOLVER_RANDMIZE_ORDER = 1,
|
||||
SOLVER_FRICTION_SEPARATE = 2,
|
||||
@@ -35,134 +35,125 @@ enum btSolverMode
|
||||
|
||||
struct btContactSolverInfoData
|
||||
{
|
||||
|
||||
btScalar m_tau;
|
||||
btScalar m_damping; //global non-contact constraint damping, can be locally overridden by constraints during 'getInfo2'.
|
||||
btScalar m_friction;
|
||||
btScalar m_timeStep;
|
||||
btScalar m_restitution;
|
||||
int m_numIterations;
|
||||
btScalar m_maxErrorReduction;
|
||||
btScalar m_sor; //successive over-relaxation term
|
||||
btScalar m_erp; //error reduction for non-contact constraints
|
||||
btScalar m_erp2; //error reduction for contact constraints
|
||||
btScalar m_globalCfm; //constraint force mixing for contacts and non-contacts
|
||||
btScalar m_frictionERP; //error reduction for friction constraints
|
||||
btScalar m_frictionCFM; //constraint force mixing for friction constraints
|
||||
|
||||
btScalar m_tau;
|
||||
btScalar m_damping;//global non-contact constraint damping, can be locally overridden by constraints during 'getInfo2'.
|
||||
btScalar m_friction;
|
||||
btScalar m_timeStep;
|
||||
btScalar m_restitution;
|
||||
int m_numIterations;
|
||||
btScalar m_maxErrorReduction;
|
||||
btScalar m_sor;//successive over-relaxation term
|
||||
btScalar m_erp;//error reduction for non-contact constraints
|
||||
btScalar m_erp2;//error reduction for contact constraints
|
||||
btScalar m_globalCfm;//constraint force mixing for contacts and non-contacts
|
||||
btScalar m_frictionERP;//error reduction for friction constraints
|
||||
btScalar m_frictionCFM;//constraint force mixing for friction constraints
|
||||
|
||||
int m_splitImpulse;
|
||||
btScalar m_splitImpulsePenetrationThreshold;
|
||||
btScalar m_splitImpulseTurnErp;
|
||||
btScalar m_linearSlop;
|
||||
btScalar m_warmstartingFactor;
|
||||
|
||||
int m_solverMode;
|
||||
int m_restingContactRestitutionThreshold;
|
||||
int m_minimumSolverBatchSize;
|
||||
btScalar m_maxGyroscopicForce;
|
||||
btScalar m_singleAxisRollingFrictionThreshold;
|
||||
btScalar m_leastSquaresResidualThreshold;
|
||||
btScalar m_restitutionVelocityThreshold;
|
||||
int m_splitImpulse;
|
||||
btScalar m_splitImpulsePenetrationThreshold;
|
||||
btScalar m_splitImpulseTurnErp;
|
||||
btScalar m_linearSlop;
|
||||
btScalar m_warmstartingFactor;
|
||||
|
||||
int m_solverMode;
|
||||
int m_restingContactRestitutionThreshold;
|
||||
int m_minimumSolverBatchSize;
|
||||
btScalar m_maxGyroscopicForce;
|
||||
btScalar m_singleAxisRollingFrictionThreshold;
|
||||
btScalar m_leastSquaresResidualThreshold;
|
||||
btScalar m_restitutionVelocityThreshold;
|
||||
};
|
||||
|
||||
struct btContactSolverInfo : public btContactSolverInfoData
|
||||
{
|
||||
|
||||
|
||||
|
||||
inline btContactSolverInfo()
|
||||
{
|
||||
m_tau = btScalar(0.6);
|
||||
m_damping = btScalar(1.0);
|
||||
m_friction = btScalar(0.3);
|
||||
m_timeStep = btScalar(1.f/60.f);
|
||||
m_timeStep = btScalar(1.f / 60.f);
|
||||
m_restitution = btScalar(0.);
|
||||
m_maxErrorReduction = btScalar(20.);
|
||||
m_numIterations = 10;
|
||||
m_erp = btScalar(0.2);
|
||||
m_erp2 = btScalar(0.2);
|
||||
m_globalCfm = btScalar(0.);
|
||||
m_frictionERP = btScalar(0.2);//positional friction 'anchors' are disabled by default
|
||||
m_frictionERP = btScalar(0.2); //positional friction 'anchors' are disabled by default
|
||||
m_frictionCFM = btScalar(0.);
|
||||
m_sor = btScalar(1.);
|
||||
m_splitImpulse = true;
|
||||
m_splitImpulsePenetrationThreshold = -.04f;
|
||||
m_splitImpulseTurnErp = 0.1f;
|
||||
m_linearSlop = btScalar(0.0);
|
||||
m_warmstartingFactor=btScalar(0.85);
|
||||
m_warmstartingFactor = btScalar(0.85);
|
||||
//m_solverMode = SOLVER_USE_WARMSTARTING | SOLVER_SIMD | SOLVER_DISABLE_VELOCITY_DEPENDENT_FRICTION_DIRECTION|SOLVER_USE_2_FRICTION_DIRECTIONS|SOLVER_ENABLE_FRICTION_DIRECTION_CACHING;// | SOLVER_RANDMIZE_ORDER;
|
||||
m_solverMode = SOLVER_USE_WARMSTARTING | SOLVER_SIMD;// | 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; ///it is only used for 'explicit' version of gyroscopic force
|
||||
m_singleAxisRollingFrictionThreshold = 1e30f;///if the velocity is above this threshold, it will use a single constraint row (axis), otherwise 3 rows.
|
||||
m_solverMode = SOLVER_USE_WARMSTARTING | SOLVER_SIMD; // | 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; ///it is only used for 'explicit' version of gyroscopic force
|
||||
m_singleAxisRollingFrictionThreshold = 1e30f; ///if the velocity is above this threshold, it will use a single constraint row (axis), otherwise 3 rows.
|
||||
m_leastSquaresResidualThreshold = 0.f;
|
||||
m_restitutionVelocityThreshold = 0.2f;//if the relative velocity is below this threshold, there is zero restitution
|
||||
m_restitutionVelocityThreshold = 0.2f; //if the relative velocity is below this threshold, there is zero restitution
|
||||
}
|
||||
};
|
||||
|
||||
///do not change those serialization structures, it requires an updated sBulletDNAstr/sBulletDNAstr64
|
||||
struct btContactSolverInfoDoubleData
|
||||
{
|
||||
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;///it is only used for 'explicit' version of gyroscopic force
|
||||
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; ///it is only used for 'explicit' version of gyroscopic force
|
||||
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 btContactSolverInfoFloatData
|
||||
{
|
||||
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 //BT_CONTACT_SOLVER_INFO
|
||||
#endif //BT_CONTACT_SOLVER_INFO
|
||||
|
||||
@@ -13,25 +13,20 @@ subject to the following restrictions:
|
||||
3. This notice may not be removed or altered from any source distribution.
|
||||
*/
|
||||
|
||||
|
||||
#include "btFixedConstraint.h"
|
||||
#include "BulletDynamics/Dynamics/btRigidBody.h"
|
||||
#include "LinearMath/btTransformUtil.h"
|
||||
#include <new>
|
||||
|
||||
|
||||
btFixedConstraint::btFixedConstraint(btRigidBody& rbA,btRigidBody& rbB, const btTransform& frameInA,const btTransform& frameInB)
|
||||
:btGeneric6DofSpring2Constraint(rbA,rbB,frameInA,frameInB)
|
||||
btFixedConstraint::btFixedConstraint(btRigidBody& rbA, btRigidBody& rbB, const btTransform& frameInA, const btTransform& frameInB)
|
||||
: btGeneric6DofSpring2Constraint(rbA, rbB, frameInA, frameInB)
|
||||
{
|
||||
setAngularLowerLimit(btVector3(0,0,0));
|
||||
setAngularUpperLimit(btVector3(0,0,0));
|
||||
setLinearLowerLimit(btVector3(0,0,0));
|
||||
setLinearUpperLimit(btVector3(0,0,0));
|
||||
setAngularLowerLimit(btVector3(0, 0, 0));
|
||||
setAngularUpperLimit(btVector3(0, 0, 0));
|
||||
setLinearLowerLimit(btVector3(0, 0, 0));
|
||||
setLinearUpperLimit(btVector3(0, 0, 0));
|
||||
}
|
||||
|
||||
|
||||
|
||||
|
||||
btFixedConstraint::~btFixedConstraint ()
|
||||
btFixedConstraint::~btFixedConstraint()
|
||||
{
|
||||
}
|
||||
|
||||
@@ -18,16 +18,13 @@ subject to the following restrictions:
|
||||
|
||||
#include "btGeneric6DofSpring2Constraint.h"
|
||||
|
||||
|
||||
ATTRIBUTE_ALIGNED16(class) btFixedConstraint : public btGeneric6DofSpring2Constraint
|
||||
ATTRIBUTE_ALIGNED16(class)
|
||||
btFixedConstraint : public btGeneric6DofSpring2Constraint
|
||||
{
|
||||
|
||||
public:
|
||||
btFixedConstraint(btRigidBody& rbA,btRigidBody& rbB, const btTransform& frameInA,const btTransform& frameInB);
|
||||
btFixedConstraint(btRigidBody & rbA, btRigidBody & rbB, const btTransform& frameInA, const btTransform& frameInB);
|
||||
|
||||
|
||||
virtual ~btFixedConstraint();
|
||||
|
||||
};
|
||||
|
||||
#endif //BT_FIXED_CONSTRAINT_H
|
||||
#endif //BT_FIXED_CONSTRAINT_H
|
||||
|
||||
@@ -17,38 +17,36 @@ subject to the following restrictions:
|
||||
|
||||
#include "btGearConstraint.h"
|
||||
|
||||
btGearConstraint::btGearConstraint(btRigidBody& rbA, btRigidBody& rbB, const btVector3& axisInA,const btVector3& axisInB, btScalar ratio)
|
||||
:btTypedConstraint(GEAR_CONSTRAINT_TYPE,rbA,rbB),
|
||||
m_axisInA(axisInA),
|
||||
m_axisInB(axisInB),
|
||||
m_ratio(ratio)
|
||||
btGearConstraint::btGearConstraint(btRigidBody& rbA, btRigidBody& rbB, const btVector3& axisInA, const btVector3& axisInB, btScalar ratio)
|
||||
: btTypedConstraint(GEAR_CONSTRAINT_TYPE, rbA, rbB),
|
||||
m_axisInA(axisInA),
|
||||
m_axisInB(axisInB),
|
||||
m_ratio(ratio)
|
||||
{
|
||||
}
|
||||
|
||||
btGearConstraint::~btGearConstraint ()
|
||||
btGearConstraint::~btGearConstraint()
|
||||
{
|
||||
}
|
||||
|
||||
void btGearConstraint::getInfo1 (btConstraintInfo1* info)
|
||||
void btGearConstraint::getInfo1(btConstraintInfo1* info)
|
||||
{
|
||||
info->m_numConstraintRows = 1;
|
||||
info->nub = 1;
|
||||
}
|
||||
|
||||
void btGearConstraint::getInfo2 (btConstraintInfo2* info)
|
||||
void btGearConstraint::getInfo2(btConstraintInfo2* info)
|
||||
{
|
||||
btVector3 globalAxisA, globalAxisB;
|
||||
|
||||
globalAxisA = m_rbA.getWorldTransform().getBasis()*this->m_axisInA;
|
||||
globalAxisB = m_rbB.getWorldTransform().getBasis()*this->m_axisInB;
|
||||
globalAxisA = m_rbA.getWorldTransform().getBasis() * this->m_axisInA;
|
||||
globalAxisB = m_rbB.getWorldTransform().getBasis() * this->m_axisInB;
|
||||
|
||||
info->m_J1angularAxis[0] = globalAxisA[0];
|
||||
info->m_J1angularAxis[1] = globalAxisA[1];
|
||||
info->m_J1angularAxis[2] = globalAxisA[2];
|
||||
|
||||
info->m_J2angularAxis[0] = m_ratio*globalAxisB[0];
|
||||
info->m_J2angularAxis[1] = m_ratio*globalAxisB[1];
|
||||
info->m_J2angularAxis[2] = m_ratio*globalAxisB[2];
|
||||
|
||||
info->m_J2angularAxis[0] = m_ratio * globalAxisB[0];
|
||||
info->m_J2angularAxis[1] = m_ratio * globalAxisB[1];
|
||||
info->m_J2angularAxis[2] = m_ratio * globalAxisB[2];
|
||||
}
|
||||
|
||||
|
||||
@@ -13,45 +13,40 @@ subject to the following restrictions:
|
||||
3. This notice may not be removed or altered from any source distribution.
|
||||
*/
|
||||
|
||||
|
||||
|
||||
#ifndef BT_GEAR_CONSTRAINT_H
|
||||
#define BT_GEAR_CONSTRAINT_H
|
||||
|
||||
#include "BulletDynamics/ConstraintSolver/btTypedConstraint.h"
|
||||
|
||||
|
||||
#ifdef BT_USE_DOUBLE_PRECISION
|
||||
#define btGearConstraintData btGearConstraintDoubleData
|
||||
#define btGearConstraintDataName "btGearConstraintDoubleData"
|
||||
#define btGearConstraintData btGearConstraintDoubleData
|
||||
#define btGearConstraintDataName "btGearConstraintDoubleData"
|
||||
#else
|
||||
#define btGearConstraintData btGearConstraintFloatData
|
||||
#define btGearConstraintDataName "btGearConstraintFloatData"
|
||||
#endif //BT_USE_DOUBLE_PRECISION
|
||||
|
||||
|
||||
#define btGearConstraintData btGearConstraintFloatData
|
||||
#define btGearConstraintDataName "btGearConstraintFloatData"
|
||||
#endif //BT_USE_DOUBLE_PRECISION
|
||||
|
||||
///The btGeatConstraint will couple the angular velocity for two bodies around given local axis and ratio.
|
||||
///See Bullet/Demos/ConstraintDemo for an example use.
|
||||
class btGearConstraint : public btTypedConstraint
|
||||
{
|
||||
protected:
|
||||
btVector3 m_axisInA;
|
||||
btVector3 m_axisInB;
|
||||
bool m_useFrameA;
|
||||
btScalar m_ratio;
|
||||
btVector3 m_axisInA;
|
||||
btVector3 m_axisInB;
|
||||
bool m_useFrameA;
|
||||
btScalar m_ratio;
|
||||
|
||||
public:
|
||||
btGearConstraint(btRigidBody& rbA, btRigidBody& rbB, const btVector3& axisInA,const btVector3& axisInB, btScalar ratio=1.f);
|
||||
virtual ~btGearConstraint ();
|
||||
btGearConstraint(btRigidBody& rbA, btRigidBody& rbB, const btVector3& axisInA, const btVector3& axisInB, btScalar ratio = 1.f);
|
||||
virtual ~btGearConstraint();
|
||||
|
||||
///internal method used by the constraint solver, don't use them directly
|
||||
virtual void getInfo1 (btConstraintInfo1* info);
|
||||
virtual void getInfo1(btConstraintInfo1* info);
|
||||
|
||||
///internal method used by the constraint solver, don't use them directly
|
||||
virtual void getInfo2 (btConstraintInfo2* info);
|
||||
virtual void getInfo2(btConstraintInfo2* info);
|
||||
|
||||
void setAxisA(btVector3& axisA)
|
||||
void setAxisA(btVector3& axisA)
|
||||
{
|
||||
m_axisInA = axisA;
|
||||
}
|
||||
@@ -76,68 +71,64 @@ public:
|
||||
return m_ratio;
|
||||
}
|
||||
|
||||
|
||||
virtual void setParam(int num, btScalar value, int axis = -1)
|
||||
virtual void setParam(int num, btScalar value, int axis = -1)
|
||||
{
|
||||
(void) num;
|
||||
(void) value;
|
||||
(void) axis;
|
||||
(void)num;
|
||||
(void)value;
|
||||
(void)axis;
|
||||
btAssert(0);
|
||||
}
|
||||
|
||||
///return the local value of parameter
|
||||
virtual btScalar getParam(int num, int axis = -1) const
|
||||
{
|
||||
(void) num;
|
||||
(void) axis;
|
||||
virtual btScalar getParam(int num, int axis = -1) const
|
||||
{
|
||||
(void)num;
|
||||
(void)axis;
|
||||
btAssert(0);
|
||||
return 0.f;
|
||||
}
|
||||
|
||||
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, btSerializer* serializer) const;
|
||||
virtual const char* serialize(void* dataBuffer, btSerializer* serializer) const;
|
||||
};
|
||||
|
||||
|
||||
|
||||
|
||||
///do not change those serialization structures, it requires an updated sBulletDNAstr/sBulletDNAstr64
|
||||
struct btGearConstraintFloatData
|
||||
{
|
||||
btTypedConstraintFloatData m_typeConstraintData;
|
||||
btTypedConstraintFloatData m_typeConstraintData;
|
||||
|
||||
btVector3FloatData m_axisInA;
|
||||
btVector3FloatData m_axisInB;
|
||||
btVector3FloatData m_axisInA;
|
||||
btVector3FloatData m_axisInB;
|
||||
|
||||
float m_ratio;
|
||||
char m_padding[4];
|
||||
float m_ratio;
|
||||
char m_padding[4];
|
||||
};
|
||||
|
||||
struct btGearConstraintDoubleData
|
||||
{
|
||||
btTypedConstraintDoubleData m_typeConstraintData;
|
||||
btTypedConstraintDoubleData m_typeConstraintData;
|
||||
|
||||
btVector3DoubleData m_axisInA;
|
||||
btVector3DoubleData m_axisInB;
|
||||
btVector3DoubleData m_axisInA;
|
||||
btVector3DoubleData m_axisInB;
|
||||
|
||||
double m_ratio;
|
||||
double m_ratio;
|
||||
};
|
||||
|
||||
SIMD_FORCE_INLINE int btGearConstraint::calculateSerializeBufferSize() const
|
||||
SIMD_FORCE_INLINE int btGearConstraint::calculateSerializeBufferSize() const
|
||||
{
|
||||
return sizeof(btGearConstraintData);
|
||||
}
|
||||
|
||||
///fills the dataBuffer and returns the struct name (and 0 on failure)
|
||||
SIMD_FORCE_INLINE const char* btGearConstraint::serialize(void* dataBuffer, btSerializer* serializer) const
|
||||
///fills the dataBuffer and returns the struct name (and 0 on failure)
|
||||
SIMD_FORCE_INLINE const char* btGearConstraint::serialize(void* dataBuffer, btSerializer* serializer) const
|
||||
{
|
||||
btGearConstraintData* gear = (btGearConstraintData*)dataBuffer;
|
||||
btTypedConstraint::serialize(&gear->m_typeConstraintData,serializer);
|
||||
btTypedConstraint::serialize(&gear->m_typeConstraintData, serializer);
|
||||
|
||||
m_axisInA.serialize( gear->m_axisInA );
|
||||
m_axisInB.serialize( gear->m_axisInB );
|
||||
m_axisInA.serialize(gear->m_axisInA);
|
||||
m_axisInB.serialize(gear->m_axisInB);
|
||||
|
||||
gear->m_ratio = m_ratio;
|
||||
|
||||
@@ -152,9 +143,4 @@ SIMD_FORCE_INLINE const char* btGearConstraint::serialize(void* dataBuffer, btSe
|
||||
return btGearConstraintDataName;
|
||||
}
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
#endif //BT_GEAR_CONSTRAINT_H
|
||||
#endif //BT_GEAR_CONSTRAINT_H
|
||||
|
||||
File diff suppressed because it is too large
Load Diff
@@ -23,7 +23,6 @@ email: projectileman@yahoo.com
|
||||
http://gimpact.sf.net
|
||||
*/
|
||||
|
||||
|
||||
#ifndef BT_GENERIC_6DOF_CONSTRAINT_H
|
||||
#define BT_GENERIC_6DOF_CONSTRAINT_H
|
||||
|
||||
@@ -33,96 +32,91 @@ http://gimpact.sf.net
|
||||
|
||||
class btRigidBody;
|
||||
|
||||
|
||||
|
||||
#ifdef BT_USE_DOUBLE_PRECISION
|
||||
#define btGeneric6DofConstraintData2 btGeneric6DofConstraintDoubleData2
|
||||
#define btGeneric6DofConstraintDataName "btGeneric6DofConstraintDoubleData2"
|
||||
#define btGeneric6DofConstraintData2 btGeneric6DofConstraintDoubleData2
|
||||
#define btGeneric6DofConstraintDataName "btGeneric6DofConstraintDoubleData2"
|
||||
#else
|
||||
#define btGeneric6DofConstraintData2 btGeneric6DofConstraintData
|
||||
#define btGeneric6DofConstraintDataName "btGeneric6DofConstraintData"
|
||||
#endif //BT_USE_DOUBLE_PRECISION
|
||||
|
||||
#define btGeneric6DofConstraintData2 btGeneric6DofConstraintData
|
||||
#define btGeneric6DofConstraintDataName "btGeneric6DofConstraintData"
|
||||
#endif //BT_USE_DOUBLE_PRECISION
|
||||
|
||||
//! Rotation Limit structure for generic joints
|
||||
class btRotationalLimitMotor
|
||||
{
|
||||
public:
|
||||
//! limit_parameters
|
||||
//!@{
|
||||
btScalar m_loLimit;//!< joint limit
|
||||
btScalar m_hiLimit;//!< joint limit
|
||||
btScalar m_targetVelocity;//!< target motor velocity
|
||||
btScalar m_maxMotorForce;//!< max force on motor
|
||||
btScalar m_maxLimitForce;//!< max force on limit
|
||||
btScalar m_damping;//!< Damping.
|
||||
btScalar m_limitSoftness;//! Relaxation factor
|
||||
btScalar m_normalCFM;//!< Constraint force mixing factor
|
||||
btScalar m_stopERP;//!< Error tolerance factor when joint is at limit
|
||||
btScalar m_stopCFM;//!< Constraint force mixing factor when joint is at limit
|
||||
btScalar m_bounce;//!< restitution factor
|
||||
bool m_enableMotor;
|
||||
//! limit_parameters
|
||||
//!@{
|
||||
btScalar m_loLimit; //!< joint limit
|
||||
btScalar m_hiLimit; //!< joint limit
|
||||
btScalar m_targetVelocity; //!< target motor velocity
|
||||
btScalar m_maxMotorForce; //!< max force on motor
|
||||
btScalar m_maxLimitForce; //!< max force on limit
|
||||
btScalar m_damping; //!< Damping.
|
||||
btScalar m_limitSoftness; //! Relaxation factor
|
||||
btScalar m_normalCFM; //!< Constraint force mixing factor
|
||||
btScalar m_stopERP; //!< Error tolerance factor when joint is at limit
|
||||
btScalar m_stopCFM; //!< Constraint force mixing factor when joint is at limit
|
||||
btScalar m_bounce; //!< restitution factor
|
||||
bool m_enableMotor;
|
||||
|
||||
//!@}
|
||||
//!@}
|
||||
|
||||
//! temp_variables
|
||||
//!@{
|
||||
btScalar m_currentLimitError;//! How much is violated this limit
|
||||
btScalar m_currentPosition; //! current value of angle
|
||||
int m_currentLimit;//!< 0=free, 1=at lo limit, 2=at hi limit
|
||||
btScalar m_accumulatedImpulse;
|
||||
//!@}
|
||||
//! temp_variables
|
||||
//!@{
|
||||
btScalar m_currentLimitError; //! How much is violated this limit
|
||||
btScalar m_currentPosition; //! current value of angle
|
||||
int m_currentLimit; //!< 0=free, 1=at lo limit, 2=at hi limit
|
||||
btScalar m_accumulatedImpulse;
|
||||
//!@}
|
||||
|
||||
btRotationalLimitMotor()
|
||||
{
|
||||
m_accumulatedImpulse = 0.f;
|
||||
m_targetVelocity = 0;
|
||||
m_maxMotorForce = 6.0f;
|
||||
m_maxLimitForce = 300.0f;
|
||||
m_loLimit = 1.0f;
|
||||
m_hiLimit = -1.0f;
|
||||
btRotationalLimitMotor()
|
||||
{
|
||||
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;
|
||||
}
|
||||
|
||||
btRotationalLimitMotor(const btRotationalLimitMotor & 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;
|
||||
btRotationalLimitMotor(const btRotationalLimitMotor& 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() const
|
||||
{
|
||||
if(m_loLimit > m_hiLimit) return false;
|
||||
return true;
|
||||
}
|
||||
bool isLimited() const
|
||||
{
|
||||
if (m_loLimit > m_hiLimit) return false;
|
||||
return true;
|
||||
}
|
||||
|
||||
//! Need apply correction
|
||||
bool needApplyTorques() const
|
||||
{
|
||||
if(m_currentLimit == 0 && m_enableMotor == false) return false;
|
||||
return true;
|
||||
}
|
||||
bool needApplyTorques() const
|
||||
{
|
||||
if (m_currentLimit == 0 && m_enableMotor == false) return false;
|
||||
return true;
|
||||
}
|
||||
|
||||
//! calculates error
|
||||
/*!
|
||||
@@ -131,104 +125,98 @@ public:
|
||||
int testLimitValue(btScalar test_value);
|
||||
|
||||
//! apply the correction impulses for two bodies
|
||||
btScalar solveAngularLimits(btScalar timeStep,btVector3& axis, btScalar jacDiagABInv,btRigidBody * body0, btRigidBody * body1);
|
||||
|
||||
btScalar solveAngularLimits(btScalar timeStep, btVector3& axis, btScalar jacDiagABInv, btRigidBody* body0, btRigidBody* body1);
|
||||
};
|
||||
|
||||
|
||||
|
||||
class btTranslationalLimitMotor
|
||||
{
|
||||
public:
|
||||
btVector3 m_lowerLimit;//!< the constraint lower limits
|
||||
btVector3 m_upperLimit;//!< the constraint upper limits
|
||||
btVector3 m_accumulatedImpulse;
|
||||
//! Linear_Limit_parameters
|
||||
//!@{
|
||||
btScalar m_limitSoftness;//!< Softness for linear limit
|
||||
btScalar m_damping;//!< Damping for linear limit
|
||||
btScalar m_restitution;//! Bounce parameter for linear limit
|
||||
btVector3 m_normalCFM;//!< Constraint force mixing factor
|
||||
btVector3 m_stopERP;//!< Error tolerance factor when joint is at limit
|
||||
btVector3 m_stopCFM;//!< Constraint force mixing factor when joint is at limit
|
||||
//!@}
|
||||
bool m_enableMotor[3];
|
||||
btVector3 m_targetVelocity;//!< target motor velocity
|
||||
btVector3 m_maxMotorForce;//!< max force on motor
|
||||
btVector3 m_currentLimitError;//! How much is violated this limit
|
||||
btVector3 m_currentLinearDiff;//! Current relative offset of constraint frames
|
||||
int m_currentLimit[3];//!< 0=free, 1=at lower limit, 2=at upper limit
|
||||
btVector3 m_lowerLimit; //!< the constraint lower limits
|
||||
btVector3 m_upperLimit; //!< the constraint upper limits
|
||||
btVector3 m_accumulatedImpulse;
|
||||
//! Linear_Limit_parameters
|
||||
//!@{
|
||||
btScalar m_limitSoftness; //!< Softness for linear limit
|
||||
btScalar m_damping; //!< Damping for linear limit
|
||||
btScalar m_restitution; //! Bounce parameter for linear limit
|
||||
btVector3 m_normalCFM; //!< Constraint force mixing factor
|
||||
btVector3 m_stopERP; //!< Error tolerance factor when joint is at limit
|
||||
btVector3 m_stopCFM; //!< Constraint force mixing factor when joint is at limit
|
||||
//!@}
|
||||
bool m_enableMotor[3];
|
||||
btVector3 m_targetVelocity; //!< target motor velocity
|
||||
btVector3 m_maxMotorForce; //!< max force on motor
|
||||
btVector3 m_currentLimitError; //! How much is violated this limit
|
||||
btVector3 m_currentLinearDiff; //! Current relative offset of constraint frames
|
||||
int m_currentLimit[3]; //!< 0=free, 1=at lower limit, 2=at upper limit
|
||||
|
||||
btTranslationalLimitMotor()
|
||||
{
|
||||
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);
|
||||
btTranslationalLimitMotor()
|
||||
{
|
||||
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 = btScalar(1.0f);
|
||||
m_restitution = btScalar(0.5f);
|
||||
for(int i=0; i < 3; i++)
|
||||
m_limitSoftness = 0.7f;
|
||||
m_damping = btScalar(1.0f);
|
||||
m_restitution = btScalar(0.5f);
|
||||
for (int i = 0; i < 3; i++)
|
||||
{
|
||||
m_enableMotor[i] = false;
|
||||
m_targetVelocity[i] = btScalar(0.f);
|
||||
m_maxMotorForce[i] = btScalar(0.f);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
btTranslationalLimitMotor(const btTranslationalLimitMotor & other )
|
||||
{
|
||||
m_lowerLimit = other.m_lowerLimit;
|
||||
m_upperLimit = other.m_upperLimit;
|
||||
m_accumulatedImpulse = other.m_accumulatedImpulse;
|
||||
btTranslationalLimitMotor(const btTranslationalLimitMotor& 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) const
|
||||
{
|
||||
return (m_upperLimit[limitIndex] >= m_lowerLimit[limitIndex]);
|
||||
}
|
||||
inline bool needApplyForce(int limitIndex) const
|
||||
{
|
||||
if(m_currentLimit[limitIndex] == 0 && m_enableMotor[limitIndex] == false) return false;
|
||||
return true;
|
||||
}
|
||||
inline bool isLimited(int limitIndex) const
|
||||
{
|
||||
return (m_upperLimit[limitIndex] >= m_lowerLimit[limitIndex]);
|
||||
}
|
||||
inline bool needApplyForce(int limitIndex) const
|
||||
{
|
||||
if (m_currentLimit[limitIndex] == 0 && m_enableMotor[limitIndex] == false) return false;
|
||||
return true;
|
||||
}
|
||||
int testLimitValue(int limitIndex, btScalar test_value);
|
||||
|
||||
|
||||
btScalar solveLinearAxis(
|
||||
btScalar timeStep,
|
||||
btScalar jacDiagABInv,
|
||||
btRigidBody& body1,const btVector3 &pointInA,
|
||||
btRigidBody& body2,const btVector3 &pointInB,
|
||||
int limit_index,
|
||||
const btVector3 & axis_normal_on_a,
|
||||
const btVector3 & anchorPos);
|
||||
|
||||
|
||||
btScalar solveLinearAxis(
|
||||
btScalar timeStep,
|
||||
btScalar jacDiagABInv,
|
||||
btRigidBody& body1, const btVector3& pointInA,
|
||||
btRigidBody& body2, const btVector3& pointInB,
|
||||
int limit_index,
|
||||
const btVector3& axis_normal_on_a,
|
||||
const btVector3& anchorPos);
|
||||
};
|
||||
|
||||
enum bt6DofFlags
|
||||
@@ -237,8 +225,7 @@ enum bt6DofFlags
|
||||
BT_6DOF_FLAGS_CFM_STOP = 2,
|
||||
BT_6DOF_FLAGS_ERP_STOP = 4
|
||||
};
|
||||
#define BT_6DOF_FLAGS_AXIS_SHIFT 3 // bits per axis
|
||||
|
||||
#define BT_6DOF_FLAGS_AXIS_SHIFT 3 // bits per axis
|
||||
|
||||
/// btGeneric6DofConstraint between two rigidbodies each with a pivotpoint that descibes the axis location in local space
|
||||
/*!
|
||||
@@ -276,254 +263,245 @@ This brings support for limit parameters and motors. </li>
|
||||
</ul>
|
||||
|
||||
*/
|
||||
ATTRIBUTE_ALIGNED16(class) btGeneric6DofConstraint : public btTypedConstraint
|
||||
ATTRIBUTE_ALIGNED16(class)
|
||||
btGeneric6DofConstraint : public btTypedConstraint
|
||||
{
|
||||
protected:
|
||||
|
||||
//! relative_frames
|
||||
//!@{
|
||||
btTransform m_frameInA;//!< the constraint space w.r.t body A
|
||||
btTransform m_frameInB;//!< the constraint space w.r.t body B
|
||||
//!@}
|
||||
|
||||
//! Jacobians
|
||||
//!@{
|
||||
btJacobianEntry m_jacLinear[3];//!< 3 orthogonal linear constraints
|
||||
btJacobianEntry m_jacAng[3];//!< 3 orthogonal angular constraints
|
||||
//!@}
|
||||
|
||||
//! Linear_Limit_parameters
|
||||
//!@{
|
||||
btTranslationalLimitMotor m_linearLimits;
|
||||
//!@}
|
||||
|
||||
|
||||
//! hinge_parameters
|
||||
//!@{
|
||||
btRotationalLimitMotor m_angularLimits[3];
|
||||
//!@{
|
||||
btTransform m_frameInA; //!< the constraint space w.r.t body A
|
||||
btTransform m_frameInB; //!< the constraint space w.r.t body B
|
||||
//!@}
|
||||
|
||||
//! Jacobians
|
||||
//!@{
|
||||
btJacobianEntry m_jacLinear[3]; //!< 3 orthogonal linear constraints
|
||||
btJacobianEntry m_jacAng[3]; //!< 3 orthogonal angular constraints
|
||||
//!@}
|
||||
|
||||
//! Linear_Limit_parameters
|
||||
//!@{
|
||||
btTranslationalLimitMotor m_linearLimits;
|
||||
//!@}
|
||||
|
||||
//! hinge_parameters
|
||||
//!@{
|
||||
btRotationalLimitMotor m_angularLimits[3];
|
||||
//!@}
|
||||
|
||||
protected:
|
||||
//! temporal variables
|
||||
//!@{
|
||||
btScalar m_timeStep;
|
||||
btTransform m_calculatedTransformA;
|
||||
btTransform m_calculatedTransformB;
|
||||
btVector3 m_calculatedAxisAngleDiff;
|
||||
btVector3 m_calculatedAxis[3];
|
||||
btVector3 m_calculatedLinearDiff;
|
||||
btScalar m_factA;
|
||||
btScalar m_factB;
|
||||
bool m_hasStaticBody;
|
||||
|
||||
btVector3 m_AnchorPos; // point betwen pivots of bodies A and B to solve linear axes
|
||||
//! temporal variables
|
||||
//!@{
|
||||
btScalar m_timeStep;
|
||||
btTransform m_calculatedTransformA;
|
||||
btTransform m_calculatedTransformB;
|
||||
btVector3 m_calculatedAxisAngleDiff;
|
||||
btVector3 m_calculatedAxis[3];
|
||||
btVector3 m_calculatedLinearDiff;
|
||||
btScalar m_factA;
|
||||
btScalar m_factB;
|
||||
bool m_hasStaticBody;
|
||||
|
||||
bool m_useLinearReferenceFrameA;
|
||||
bool m_useOffsetForConstraintFrame;
|
||||
|
||||
int m_flags;
|
||||
btVector3 m_AnchorPos; // point betwen pivots of bodies A and B to solve linear axes
|
||||
|
||||
//!@}
|
||||
bool m_useLinearReferenceFrameA;
|
||||
bool m_useOffsetForConstraintFrame;
|
||||
|
||||
btGeneric6DofConstraint& operator=(btGeneric6DofConstraint& other)
|
||||
{
|
||||
btAssert(0);
|
||||
(void) other;
|
||||
return *this;
|
||||
}
|
||||
int m_flags;
|
||||
|
||||
//!@}
|
||||
|
||||
int setAngularLimits(btConstraintInfo2 *info, int row_offset,const btTransform& transA,const btTransform& transB,const btVector3& linVelA,const btVector3& linVelB,const btVector3& angVelA,const btVector3& angVelB);
|
||||
btGeneric6DofConstraint& operator=(btGeneric6DofConstraint& other)
|
||||
{
|
||||
btAssert(0);
|
||||
(void)other;
|
||||
return *this;
|
||||
}
|
||||
|
||||
int setLinearLimits(btConstraintInfo2 *info, int row, const btTransform& transA,const btTransform& transB,const btVector3& linVelA,const btVector3& linVelB,const btVector3& angVelA,const btVector3& angVelB);
|
||||
int setAngularLimits(btConstraintInfo2 * info, int row_offset, const btTransform& transA, const btTransform& transB, const btVector3& linVelA, const btVector3& linVelB, const btVector3& angVelA, const btVector3& angVelB);
|
||||
|
||||
void buildLinearJacobian(
|
||||
btJacobianEntry & jacLinear,const btVector3 & normalWorld,
|
||||
const btVector3 & pivotAInW,const btVector3 & pivotBInW);
|
||||
int setLinearLimits(btConstraintInfo2 * info, int row, const btTransform& transA, const btTransform& transB, const btVector3& linVelA, const btVector3& linVelB, const btVector3& angVelA, const btVector3& angVelB);
|
||||
|
||||
void buildAngularJacobian(btJacobianEntry & jacAngular,const btVector3 & jointAxisW);
|
||||
void buildLinearJacobian(
|
||||
btJacobianEntry & jacLinear, const btVector3& normalWorld,
|
||||
const btVector3& pivotAInW, const btVector3& pivotBInW);
|
||||
|
||||
void buildAngularJacobian(btJacobianEntry & jacAngular, const btVector3& jointAxisW);
|
||||
|
||||
// tests linear limits
|
||||
void calculateLinearInfo();
|
||||
|
||||
//! calcs the euler angles between the two bodies.
|
||||
void calculateAngleInfo();
|
||||
|
||||
|
||||
void calculateAngleInfo();
|
||||
|
||||
public:
|
||||
|
||||
BT_DECLARE_ALIGNED_ALLOCATOR();
|
||||
|
||||
///for backwards compatibility during the transition to 'getInfo/getInfo2'
|
||||
bool m_useSolveConstraintObsolete;
|
||||
|
||||
btGeneric6DofConstraint(btRigidBody& rbA, btRigidBody& rbB, const btTransform& frameInA, const btTransform& frameInB ,bool useLinearReferenceFrameA);
|
||||
btGeneric6DofConstraint(btRigidBody& rbB, const btTransform& frameInB, bool useLinearReferenceFrameB);
|
||||
|
||||
///for backwards compatibility during the transition to 'getInfo/getInfo2'
|
||||
bool m_useSolveConstraintObsolete;
|
||||
|
||||
btGeneric6DofConstraint(btRigidBody & rbA, btRigidBody & rbB, const btTransform& frameInA, const btTransform& frameInB, bool useLinearReferenceFrameA);
|
||||
btGeneric6DofConstraint(btRigidBody & rbB, const btTransform& frameInB, bool useLinearReferenceFrameB);
|
||||
|
||||
//! 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 btGeneric6DofConstraint.getCalculatedTransformA , btGeneric6DofConstraint.getCalculatedTransformB, btGeneric6DofConstraint.calculateAngleInfo
|
||||
*/
|
||||
void calculateTransforms(const btTransform& transA,const btTransform& transB);
|
||||
void calculateTransforms(const btTransform& transA, const btTransform& transB);
|
||||
|
||||
void calculateTransforms();
|
||||
|
||||
//! Gets the global transform of the offset for body A
|
||||
/*!
|
||||
/*!
|
||||
\sa btGeneric6DofConstraint.getFrameOffsetA, btGeneric6DofConstraint.getFrameOffsetB, btGeneric6DofConstraint.calculateAngleInfo.
|
||||
*/
|
||||
const btTransform & getCalculatedTransformA() const
|
||||
{
|
||||
return m_calculatedTransformA;
|
||||
}
|
||||
const btTransform& 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 btGeneric6DofConstraint.getFrameOffsetA, btGeneric6DofConstraint.getFrameOffsetB, btGeneric6DofConstraint.calculateAngleInfo.
|
||||
*/
|
||||
const btTransform & getCalculatedTransformB() const
|
||||
{
|
||||
return m_calculatedTransformB;
|
||||
}
|
||||
const btTransform& getCalculatedTransformB() const
|
||||
{
|
||||
return m_calculatedTransformB;
|
||||
}
|
||||
|
||||
const btTransform & getFrameOffsetA() const
|
||||
{
|
||||
return m_frameInA;
|
||||
}
|
||||
const btTransform& getFrameOffsetA() const
|
||||
{
|
||||
return m_frameInA;
|
||||
}
|
||||
|
||||
const btTransform & getFrameOffsetB() const
|
||||
{
|
||||
return m_frameInB;
|
||||
}
|
||||
const btTransform& getFrameOffsetB() const
|
||||
{
|
||||
return m_frameInB;
|
||||
}
|
||||
|
||||
btTransform& getFrameOffsetA()
|
||||
{
|
||||
return m_frameInA;
|
||||
}
|
||||
|
||||
btTransform & getFrameOffsetA()
|
||||
{
|
||||
return m_frameInA;
|
||||
}
|
||||
|
||||
btTransform & getFrameOffsetB()
|
||||
{
|
||||
return m_frameInB;
|
||||
}
|
||||
|
||||
btTransform& getFrameOffsetB()
|
||||
{
|
||||
return m_frameInB;
|
||||
}
|
||||
|
||||
//! performs Jacobian calculation, and also calculates angle differences and axis
|
||||
virtual void buildJacobian();
|
||||
virtual void buildJacobian();
|
||||
|
||||
virtual void getInfo1 (btConstraintInfo1* info);
|
||||
virtual void getInfo1(btConstraintInfo1 * info);
|
||||
|
||||
void getInfo1NonVirtual (btConstraintInfo1* info);
|
||||
void getInfo1NonVirtual(btConstraintInfo1 * info);
|
||||
|
||||
virtual void getInfo2 (btConstraintInfo2* info);
|
||||
virtual void getInfo2(btConstraintInfo2 * info);
|
||||
|
||||
void getInfo2NonVirtual (btConstraintInfo2* info,const btTransform& transA,const btTransform& transB,const btVector3& linVelA,const btVector3& linVelB,const btVector3& angVelA,const btVector3& angVelB);
|
||||
void getInfo2NonVirtual(btConstraintInfo2 * info, const btTransform& transA, const btTransform& transB, const btVector3& linVelA, const btVector3& linVelB, const btVector3& angVelA, const btVector3& angVelB);
|
||||
|
||||
|
||||
void updateRHS(btScalar timeStep);
|
||||
void updateRHS(btScalar timeStep);
|
||||
|
||||
//! Get the rotation axis in global coordinates
|
||||
/*!
|
||||
\pre btGeneric6DofConstraint.buildJacobian must be called previously.
|
||||
*/
|
||||
btVector3 getAxis(int axis_index) const;
|
||||
btVector3 getAxis(int axis_index) const;
|
||||
|
||||
//! Get the relative Euler angle
|
||||
/*!
|
||||
//! Get the relative Euler angle
|
||||
/*!
|
||||
\pre btGeneric6DofConstraint::calculateTransforms() must be called previously.
|
||||
*/
|
||||
btScalar getAngle(int axis_index) const;
|
||||
btScalar getAngle(int axis_index) const;
|
||||
|
||||
//! Get the relative position of the constraint pivot
|
||||
/*!
|
||||
/*!
|
||||
\pre btGeneric6DofConstraint::calculateTransforms() must be called previously.
|
||||
*/
|
||||
btScalar getRelativePivotPosition(int axis_index) const;
|
||||
|
||||
void setFrames(const btTransform & frameA, const btTransform & frameB);
|
||||
void setFrames(const btTransform& frameA, const btTransform& frameB);
|
||||
|
||||
//! Test angular limit.
|
||||
/*!
|
||||
Calculates angular correction and returns true if limit needs to be corrected.
|
||||
\pre btGeneric6DofConstraint::calculateTransforms() must be called previously.
|
||||
*/
|
||||
bool testAngularLimitMotor(int axis_index);
|
||||
bool testAngularLimitMotor(int axis_index);
|
||||
|
||||
void setLinearLowerLimit(const btVector3& linearLower)
|
||||
{
|
||||
m_linearLimits.m_lowerLimit = linearLower;
|
||||
}
|
||||
void setLinearLowerLimit(const btVector3& linearLower)
|
||||
{
|
||||
m_linearLimits.m_lowerLimit = linearLower;
|
||||
}
|
||||
|
||||
void getLinearLowerLimit(btVector3& linearLower) const
|
||||
void getLinearLowerLimit(btVector3 & linearLower) const
|
||||
{
|
||||
linearLower = m_linearLimits.m_lowerLimit;
|
||||
}
|
||||
|
||||
void setLinearUpperLimit(const btVector3& linearUpper)
|
||||
void setLinearUpperLimit(const btVector3& linearUpper)
|
||||
{
|
||||
m_linearLimits.m_upperLimit = linearUpper;
|
||||
}
|
||||
|
||||
void getLinearUpperLimit(btVector3& linearUpper) const
|
||||
void getLinearUpperLimit(btVector3 & linearUpper) const
|
||||
{
|
||||
linearUpper = m_linearLimits.m_upperLimit;
|
||||
}
|
||||
|
||||
void setAngularLowerLimit(const btVector3& angularLower)
|
||||
{
|
||||
for(int i = 0; i < 3; i++)
|
||||
m_angularLimits[i].m_loLimit = btNormalizeAngle(angularLower[i]);
|
||||
}
|
||||
|
||||
void getAngularLowerLimit(btVector3& angularLower) const
|
||||
void setAngularLowerLimit(const btVector3& angularLower)
|
||||
{
|
||||
for(int i = 0; i < 3; i++)
|
||||
for (int i = 0; i < 3; i++)
|
||||
m_angularLimits[i].m_loLimit = btNormalizeAngle(angularLower[i]);
|
||||
}
|
||||
|
||||
void getAngularLowerLimit(btVector3 & angularLower) const
|
||||
{
|
||||
for (int i = 0; i < 3; i++)
|
||||
angularLower[i] = m_angularLimits[i].m_loLimit;
|
||||
}
|
||||
|
||||
void setAngularUpperLimit(const btVector3& angularUpper)
|
||||
{
|
||||
for(int i = 0; i < 3; i++)
|
||||
m_angularLimits[i].m_hiLimit = btNormalizeAngle(angularUpper[i]);
|
||||
}
|
||||
|
||||
void getAngularUpperLimit(btVector3& angularUpper) const
|
||||
void setAngularUpperLimit(const btVector3& angularUpper)
|
||||
{
|
||||
for(int i = 0; i < 3; i++)
|
||||
for (int i = 0; i < 3; i++)
|
||||
m_angularLimits[i].m_hiLimit = btNormalizeAngle(angularUpper[i]);
|
||||
}
|
||||
|
||||
void getAngularUpperLimit(btVector3 & angularUpper) const
|
||||
{
|
||||
for (int i = 0; i < 3; i++)
|
||||
angularUpper[i] = m_angularLimits[i].m_hiLimit;
|
||||
}
|
||||
|
||||
//! Retrieves the angular limit informacion
|
||||
btRotationalLimitMotor * getRotationalLimitMotor(int index)
|
||||
{
|
||||
return &m_angularLimits[index];
|
||||
}
|
||||
btRotationalLimitMotor* getRotationalLimitMotor(int index)
|
||||
{
|
||||
return &m_angularLimits[index];
|
||||
}
|
||||
|
||||
//! Retrieves the limit informacion
|
||||
btTranslationalLimitMotor * getTranslationalLimitMotor()
|
||||
{
|
||||
return &m_linearLimits;
|
||||
}
|
||||
//! Retrieves the limit informacion
|
||||
btTranslationalLimitMotor* getTranslationalLimitMotor()
|
||||
{
|
||||
return &m_linearLimits;
|
||||
}
|
||||
|
||||
//first 3 are linear, next 3 are angular
|
||||
void setLimit(int axis, btScalar lo, btScalar 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, btScalar lo, btScalar hi)
|
||||
{
|
||||
if (axis < 3)
|
||||
{
|
||||
m_linearLimits.m_lowerLimit[axis] = lo;
|
||||
m_linearLimits.m_upperLimit[axis] = hi;
|
||||
}
|
||||
else
|
||||
{
|
||||
lo = btNormalizeAngle(lo);
|
||||
hi = btNormalizeAngle(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
|
||||
/*!
|
||||
@@ -532,116 +510,106 @@ public:
|
||||
- limited means upper > lower
|
||||
- limitIndex: first 3 are linear, next 3 are angular
|
||||
*/
|
||||
bool isLimited(int limitIndex) const
|
||||
{
|
||||
if(limitIndex<3)
|
||||
{
|
||||
bool isLimited(int limitIndex) const
|
||||
{
|
||||
if (limitIndex < 3)
|
||||
{
|
||||
return m_linearLimits.isLimited(limitIndex);
|
||||
}
|
||||
return m_angularLimits[limitIndex - 3].isLimited();
|
||||
}
|
||||
|
||||
}
|
||||
return m_angularLimits[limitIndex-3].isLimited();
|
||||
}
|
||||
virtual void calcAnchorPos(void); // overridable
|
||||
|
||||
virtual void calcAnchorPos(void); // overridable
|
||||
|
||||
int get_limit_motor_info2( btRotationalLimitMotor * limot,
|
||||
const btTransform& transA,const btTransform& transB,const btVector3& linVelA,const btVector3& linVelB,const btVector3& angVelA,const btVector3& angVelB,
|
||||
btConstraintInfo2 *info, int row, btVector3& ax1, int rotational, int rotAllowed = false);
|
||||
int get_limit_motor_info2(btRotationalLimitMotor * limot,
|
||||
const btTransform& transA, const btTransform& transB, const btVector3& linVelA, const btVector3& linVelB, const btVector3& angVelA, const btVector3& angVelB,
|
||||
btConstraintInfo2* info, int row, btVector3& ax1, int rotational, int rotAllowed = false);
|
||||
|
||||
// access for UseFrameOffset
|
||||
bool getUseFrameOffset() const { return m_useOffsetForConstraintFrame; }
|
||||
void setUseFrameOffset(bool frameOffsetOnOff) { m_useOffsetForConstraintFrame = frameOffsetOnOff; }
|
||||
|
||||
|
||||
bool getUseLinearReferenceFrameA() const { return m_useLinearReferenceFrameA; }
|
||||
void setUseLinearReferenceFrameA(bool linearReferenceFrameA) { m_useLinearReferenceFrameA = linearReferenceFrameA; }
|
||||
|
||||
///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, btScalar value, int axis = -1);
|
||||
virtual void setParam(int num, btScalar value, int axis = -1);
|
||||
///return the local value of parameter
|
||||
virtual btScalar getParam(int num, int axis = -1) const;
|
||||
virtual btScalar getParam(int num, int axis = -1) const;
|
||||
|
||||
void setAxis( const btVector3& axis1, const btVector3& axis2);
|
||||
void setAxis(const btVector3& axis1, const btVector3& axis2);
|
||||
|
||||
virtual int getFlags() const
|
||||
{
|
||||
return m_flags;
|
||||
virtual int getFlags() const
|
||||
{
|
||||
return m_flags;
|
||||
}
|
||||
|
||||
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, btSerializer* serializer) const;
|
||||
|
||||
|
||||
virtual const char* serialize(void* dataBuffer, btSerializer* serializer) const;
|
||||
};
|
||||
|
||||
|
||||
struct btGeneric6DofConstraintData
|
||||
{
|
||||
btTypedConstraintData m_typeConstraintData;
|
||||
btTransformFloatData m_rbAFrame; // constraint axii. Assumes z is hinge axis.
|
||||
btTypedConstraintData m_typeConstraintData;
|
||||
btTransformFloatData m_rbAFrame; // constraint axii. Assumes z is hinge axis.
|
||||
btTransformFloatData m_rbBFrame;
|
||||
|
||||
btVector3FloatData m_linearUpperLimit;
|
||||
btVector3FloatData m_linearLowerLimit;
|
||||
|
||||
btVector3FloatData m_angularUpperLimit;
|
||||
btVector3FloatData m_angularLowerLimit;
|
||||
|
||||
int m_useLinearReferenceFrameA;
|
||||
btVector3FloatData m_linearUpperLimit;
|
||||
btVector3FloatData m_linearLowerLimit;
|
||||
|
||||
btVector3FloatData m_angularUpperLimit;
|
||||
btVector3FloatData m_angularLowerLimit;
|
||||
|
||||
int m_useLinearReferenceFrameA;
|
||||
int m_useOffsetForConstraintFrame;
|
||||
};
|
||||
|
||||
struct btGeneric6DofConstraintDoubleData2
|
||||
{
|
||||
btTypedConstraintDoubleData m_typeConstraintData;
|
||||
btTransformDoubleData m_rbAFrame; // constraint axii. Assumes z is hinge axis.
|
||||
btTypedConstraintDoubleData m_typeConstraintData;
|
||||
btTransformDoubleData m_rbAFrame; // constraint axii. Assumes z is hinge axis.
|
||||
btTransformDoubleData m_rbBFrame;
|
||||
|
||||
btVector3DoubleData m_linearUpperLimit;
|
||||
btVector3DoubleData m_linearLowerLimit;
|
||||
|
||||
btVector3DoubleData m_angularUpperLimit;
|
||||
btVector3DoubleData m_angularLowerLimit;
|
||||
|
||||
int m_useLinearReferenceFrameA;
|
||||
btVector3DoubleData m_linearUpperLimit;
|
||||
btVector3DoubleData m_linearLowerLimit;
|
||||
|
||||
btVector3DoubleData m_angularUpperLimit;
|
||||
btVector3DoubleData m_angularLowerLimit;
|
||||
|
||||
int m_useLinearReferenceFrameA;
|
||||
int m_useOffsetForConstraintFrame;
|
||||
};
|
||||
|
||||
SIMD_FORCE_INLINE int btGeneric6DofConstraint::calculateSerializeBufferSize() const
|
||||
SIMD_FORCE_INLINE int btGeneric6DofConstraint::calculateSerializeBufferSize() const
|
||||
{
|
||||
return sizeof(btGeneric6DofConstraintData2);
|
||||
}
|
||||
|
||||
///fills the dataBuffer and returns the struct name (and 0 on failure)
|
||||
SIMD_FORCE_INLINE const char* btGeneric6DofConstraint::serialize(void* dataBuffer, btSerializer* serializer) const
|
||||
///fills the dataBuffer and returns the struct name (and 0 on failure)
|
||||
SIMD_FORCE_INLINE const char* btGeneric6DofConstraint::serialize(void* dataBuffer, btSerializer* serializer) const
|
||||
{
|
||||
|
||||
btGeneric6DofConstraintData2* dof = (btGeneric6DofConstraintData2*)dataBuffer;
|
||||
btTypedConstraint::serialize(&dof->m_typeConstraintData,serializer);
|
||||
btTypedConstraint::serialize(&dof->m_typeConstraintData, serializer);
|
||||
|
||||
m_frameInA.serialize(dof->m_rbAFrame);
|
||||
m_frameInB.serialize(dof->m_rbBFrame);
|
||||
|
||||
|
||||
int i;
|
||||
for (i=0;i<3;i++)
|
||||
for (i = 0; i < 3; i++)
|
||||
{
|
||||
dof->m_angularLowerLimit.m_floats[i] = m_angularLimits[i].m_loLimit;
|
||||
dof->m_angularUpperLimit.m_floats[i] = m_angularLimits[i].m_hiLimit;
|
||||
dof->m_angularLowerLimit.m_floats[i] = m_angularLimits[i].m_loLimit;
|
||||
dof->m_angularUpperLimit.m_floats[i] = m_angularLimits[i].m_hiLimit;
|
||||
dof->m_linearLowerLimit.m_floats[i] = m_linearLimits.m_lowerLimit[i];
|
||||
dof->m_linearUpperLimit.m_floats[i] = m_linearLimits.m_upperLimit[i];
|
||||
}
|
||||
|
||||
dof->m_useLinearReferenceFrameA = m_useLinearReferenceFrameA? 1 : 0;
|
||||
|
||||
dof->m_useLinearReferenceFrameA = m_useLinearReferenceFrameA ? 1 : 0;
|
||||
dof->m_useOffsetForConstraintFrame = m_useOffsetForConstraintFrame ? 1 : 0;
|
||||
|
||||
return btGeneric6DofConstraintDataName;
|
||||
}
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
#endif //BT_GENERIC_6DOF_CONSTRAINT_H
|
||||
#endif //BT_GENERIC_6DOF_CONSTRAINT_H
|
||||
|
||||
File diff suppressed because it is too large
Load Diff
@@ -37,7 +37,6 @@ email: projectileman@yahoo.com
|
||||
http://gimpact.sf.net
|
||||
*/
|
||||
|
||||
|
||||
#ifndef BT_GENERIC_6DOF_CONSTRAINT2_H
|
||||
#define BT_GENERIC_6DOF_CONSTRAINT2_H
|
||||
|
||||
@@ -47,18 +46,17 @@ http://gimpact.sf.net
|
||||
|
||||
class btRigidBody;
|
||||
|
||||
|
||||
#ifdef BT_USE_DOUBLE_PRECISION
|
||||
#define btGeneric6DofSpring2ConstraintData2 btGeneric6DofSpring2ConstraintDoubleData2
|
||||
#define btGeneric6DofSpring2ConstraintDataName "btGeneric6DofSpring2ConstraintDoubleData2"
|
||||
#define btGeneric6DofSpring2ConstraintData2 btGeneric6DofSpring2ConstraintDoubleData2
|
||||
#define btGeneric6DofSpring2ConstraintDataName "btGeneric6DofSpring2ConstraintDoubleData2"
|
||||
#else
|
||||
#define btGeneric6DofSpring2ConstraintData2 btGeneric6DofSpring2ConstraintData
|
||||
#define btGeneric6DofSpring2ConstraintDataName "btGeneric6DofSpring2ConstraintData"
|
||||
#endif //BT_USE_DOUBLE_PRECISION
|
||||
#define btGeneric6DofSpring2ConstraintData2 btGeneric6DofSpring2ConstraintData
|
||||
#define btGeneric6DofSpring2ConstraintDataName "btGeneric6DofSpring2ConstraintData"
|
||||
#endif //BT_USE_DOUBLE_PRECISION
|
||||
|
||||
enum RotateOrder
|
||||
{
|
||||
RO_XYZ=0,
|
||||
RO_XYZ = 0,
|
||||
RO_XZY,
|
||||
RO_YXZ,
|
||||
RO_YZX,
|
||||
@@ -69,9 +67,9 @@ enum RotateOrder
|
||||
class btRotationalLimitMotor2
|
||||
{
|
||||
public:
|
||||
// upper < lower means free
|
||||
// upper == lower means locked
|
||||
// upper > lower means limited
|
||||
// upper < lower means free
|
||||
// upper == lower means locked
|
||||
// upper > lower means limited
|
||||
btScalar m_loLimit;
|
||||
btScalar m_hiLimit;
|
||||
btScalar m_bounce;
|
||||
@@ -79,95 +77,92 @@ public:
|
||||
btScalar m_stopCFM;
|
||||
btScalar m_motorERP;
|
||||
btScalar m_motorCFM;
|
||||
bool m_enableMotor;
|
||||
bool m_enableMotor;
|
||||
btScalar m_targetVelocity;
|
||||
btScalar m_maxMotorForce;
|
||||
bool m_servoMotor;
|
||||
bool m_servoMotor;
|
||||
btScalar m_servoTarget;
|
||||
bool m_enableSpring;
|
||||
bool m_enableSpring;
|
||||
btScalar m_springStiffness;
|
||||
bool m_springStiffnessLimited;
|
||||
bool m_springStiffnessLimited;
|
||||
btScalar m_springDamping;
|
||||
bool m_springDampingLimited;
|
||||
bool m_springDampingLimited;
|
||||
btScalar m_equilibriumPoint;
|
||||
|
||||
btScalar m_currentLimitError;
|
||||
btScalar m_currentLimitErrorHi;
|
||||
btScalar m_currentPosition;
|
||||
int m_currentLimit;
|
||||
int m_currentLimit;
|
||||
|
||||
btRotationalLimitMotor2()
|
||||
{
|
||||
m_loLimit = 1.0f;
|
||||
m_hiLimit = -1.0f;
|
||||
m_bounce = 0.0f;
|
||||
m_stopERP = 0.2f;
|
||||
m_stopCFM = 0.f;
|
||||
m_motorERP = 0.9f;
|
||||
m_motorCFM = 0.f;
|
||||
m_enableMotor = false;
|
||||
m_targetVelocity = 0;
|
||||
m_maxMotorForce = 6.0f;
|
||||
m_servoMotor = false;
|
||||
m_servoTarget = 0;
|
||||
m_enableSpring = false;
|
||||
m_springStiffness = 0;
|
||||
m_loLimit = 1.0f;
|
||||
m_hiLimit = -1.0f;
|
||||
m_bounce = 0.0f;
|
||||
m_stopERP = 0.2f;
|
||||
m_stopCFM = 0.f;
|
||||
m_motorERP = 0.9f;
|
||||
m_motorCFM = 0.f;
|
||||
m_enableMotor = false;
|
||||
m_targetVelocity = 0;
|
||||
m_maxMotorForce = 6.0f;
|
||||
m_servoMotor = false;
|
||||
m_servoTarget = 0;
|
||||
m_enableSpring = false;
|
||||
m_springStiffness = 0;
|
||||
m_springStiffnessLimited = false;
|
||||
m_springDamping = 0;
|
||||
m_springDampingLimited = false;
|
||||
m_equilibriumPoint = 0;
|
||||
m_springDamping = 0;
|
||||
m_springDampingLimited = false;
|
||||
m_equilibriumPoint = 0;
|
||||
|
||||
m_currentLimitError = 0;
|
||||
m_currentLimitError = 0;
|
||||
m_currentLimitErrorHi = 0;
|
||||
m_currentPosition = 0;
|
||||
m_currentLimit = 0;
|
||||
m_currentPosition = 0;
|
||||
m_currentLimit = 0;
|
||||
}
|
||||
|
||||
btRotationalLimitMotor2(const btRotationalLimitMotor2 & limot)
|
||||
btRotationalLimitMotor2(const btRotationalLimitMotor2& limot)
|
||||
{
|
||||
m_loLimit = limot.m_loLimit;
|
||||
m_hiLimit = limot.m_hiLimit;
|
||||
m_bounce = limot.m_bounce;
|
||||
m_stopERP = limot.m_stopERP;
|
||||
m_stopCFM = limot.m_stopCFM;
|
||||
m_motorERP = limot.m_motorERP;
|
||||
m_motorCFM = limot.m_motorCFM;
|
||||
m_enableMotor = limot.m_enableMotor;
|
||||
m_targetVelocity = limot.m_targetVelocity;
|
||||
m_maxMotorForce = limot.m_maxMotorForce;
|
||||
m_servoMotor = limot.m_servoMotor;
|
||||
m_servoTarget = limot.m_servoTarget;
|
||||
m_enableSpring = limot.m_enableSpring;
|
||||
m_springStiffness = limot.m_springStiffness;
|
||||
m_loLimit = limot.m_loLimit;
|
||||
m_hiLimit = limot.m_hiLimit;
|
||||
m_bounce = limot.m_bounce;
|
||||
m_stopERP = limot.m_stopERP;
|
||||
m_stopCFM = limot.m_stopCFM;
|
||||
m_motorERP = limot.m_motorERP;
|
||||
m_motorCFM = limot.m_motorCFM;
|
||||
m_enableMotor = limot.m_enableMotor;
|
||||
m_targetVelocity = limot.m_targetVelocity;
|
||||
m_maxMotorForce = limot.m_maxMotorForce;
|
||||
m_servoMotor = limot.m_servoMotor;
|
||||
m_servoTarget = limot.m_servoTarget;
|
||||
m_enableSpring = limot.m_enableSpring;
|
||||
m_springStiffness = limot.m_springStiffness;
|
||||
m_springStiffnessLimited = limot.m_springStiffnessLimited;
|
||||
m_springDamping = limot.m_springDamping;
|
||||
m_springDampingLimited = limot.m_springDampingLimited;
|
||||
m_equilibriumPoint = limot.m_equilibriumPoint;
|
||||
m_springDamping = limot.m_springDamping;
|
||||
m_springDampingLimited = limot.m_springDampingLimited;
|
||||
m_equilibriumPoint = limot.m_equilibriumPoint;
|
||||
|
||||
m_currentLimitError = limot.m_currentLimitError;
|
||||
m_currentLimitError = limot.m_currentLimitError;
|
||||
m_currentLimitErrorHi = limot.m_currentLimitErrorHi;
|
||||
m_currentPosition = limot.m_currentPosition;
|
||||
m_currentLimit = limot.m_currentLimit;
|
||||
m_currentPosition = limot.m_currentPosition;
|
||||
m_currentLimit = limot.m_currentLimit;
|
||||
}
|
||||
|
||||
|
||||
bool isLimited()
|
||||
{
|
||||
if(m_loLimit > m_hiLimit) return false;
|
||||
if (m_loLimit > m_hiLimit) return false;
|
||||
return true;
|
||||
}
|
||||
|
||||
void testLimitValue(btScalar test_value);
|
||||
};
|
||||
|
||||
|
||||
|
||||
class btTranslationalLimitMotor2
|
||||
{
|
||||
public:
|
||||
// upper < lower means free
|
||||
// upper == lower means locked
|
||||
// upper > lower means limited
|
||||
// upper < lower means free
|
||||
// upper == lower means locked
|
||||
// upper > lower means limited
|
||||
btVector3 m_lowerLimit;
|
||||
btVector3 m_upperLimit;
|
||||
btVector3 m_bounce;
|
||||
@@ -175,14 +170,14 @@ public:
|
||||
btVector3 m_stopCFM;
|
||||
btVector3 m_motorERP;
|
||||
btVector3 m_motorCFM;
|
||||
bool m_enableMotor[3];
|
||||
bool m_servoMotor[3];
|
||||
bool m_enableSpring[3];
|
||||
bool m_enableMotor[3];
|
||||
bool m_servoMotor[3];
|
||||
bool m_enableSpring[3];
|
||||
btVector3 m_servoTarget;
|
||||
btVector3 m_springStiffness;
|
||||
bool m_springStiffnessLimited[3];
|
||||
bool m_springStiffnessLimited[3];
|
||||
btVector3 m_springDamping;
|
||||
bool m_springDampingLimited[3];
|
||||
bool m_springDampingLimited[3];
|
||||
btVector3 m_equilibriumPoint;
|
||||
btVector3 m_targetVelocity;
|
||||
btVector3 m_maxMotorForce;
|
||||
@@ -190,69 +185,69 @@ public:
|
||||
btVector3 m_currentLimitError;
|
||||
btVector3 m_currentLimitErrorHi;
|
||||
btVector3 m_currentLinearDiff;
|
||||
int m_currentLimit[3];
|
||||
int m_currentLimit[3];
|
||||
|
||||
btTranslationalLimitMotor2()
|
||||
{
|
||||
m_lowerLimit .setValue(0.f , 0.f , 0.f );
|
||||
m_upperLimit .setValue(0.f , 0.f , 0.f );
|
||||
m_bounce .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_motorERP .setValue(0.9f, 0.9f, 0.9f);
|
||||
m_motorCFM .setValue(0.f , 0.f , 0.f );
|
||||
m_lowerLimit.setValue(0.f, 0.f, 0.f);
|
||||
m_upperLimit.setValue(0.f, 0.f, 0.f);
|
||||
m_bounce.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_motorERP.setValue(0.9f, 0.9f, 0.9f);
|
||||
m_motorCFM.setValue(0.f, 0.f, 0.f);
|
||||
|
||||
m_currentLimitError .setValue(0.f , 0.f , 0.f );
|
||||
m_currentLimitErrorHi.setValue(0.f , 0.f , 0.f );
|
||||
m_currentLinearDiff .setValue(0.f , 0.f , 0.f );
|
||||
m_currentLimitError.setValue(0.f, 0.f, 0.f);
|
||||
m_currentLimitErrorHi.setValue(0.f, 0.f, 0.f);
|
||||
m_currentLinearDiff.setValue(0.f, 0.f, 0.f);
|
||||
|
||||
for(int i=0; i < 3; i++)
|
||||
for (int i = 0; i < 3; i++)
|
||||
{
|
||||
m_enableMotor[i] = false;
|
||||
m_servoMotor[i] = false;
|
||||
m_enableSpring[i] = false;
|
||||
m_servoTarget[i] = btScalar(0.f);
|
||||
m_springStiffness[i] = btScalar(0.f);
|
||||
m_enableMotor[i] = false;
|
||||
m_servoMotor[i] = false;
|
||||
m_enableSpring[i] = false;
|
||||
m_servoTarget[i] = btScalar(0.f);
|
||||
m_springStiffness[i] = btScalar(0.f);
|
||||
m_springStiffnessLimited[i] = false;
|
||||
m_springDamping[i] = btScalar(0.f);
|
||||
m_springDampingLimited[i] = false;
|
||||
m_equilibriumPoint[i] = btScalar(0.f);
|
||||
m_targetVelocity[i] = btScalar(0.f);
|
||||
m_maxMotorForce[i] = btScalar(0.f);
|
||||
|
||||
m_currentLimit[i] = 0;
|
||||
m_springDamping[i] = btScalar(0.f);
|
||||
m_springDampingLimited[i] = false;
|
||||
m_equilibriumPoint[i] = btScalar(0.f);
|
||||
m_targetVelocity[i] = btScalar(0.f);
|
||||
m_maxMotorForce[i] = btScalar(0.f);
|
||||
|
||||
m_currentLimit[i] = 0;
|
||||
}
|
||||
}
|
||||
|
||||
btTranslationalLimitMotor2(const btTranslationalLimitMotor2 & other )
|
||||
btTranslationalLimitMotor2(const btTranslationalLimitMotor2& other)
|
||||
{
|
||||
m_lowerLimit = other.m_lowerLimit;
|
||||
m_upperLimit = other.m_upperLimit;
|
||||
m_bounce = other.m_bounce;
|
||||
m_stopERP = other.m_stopERP;
|
||||
m_stopCFM = other.m_stopCFM;
|
||||
m_motorERP = other.m_motorERP;
|
||||
m_motorCFM = other.m_motorCFM;
|
||||
|
||||
m_currentLimitError = other.m_currentLimitError;
|
||||
m_lowerLimit = other.m_lowerLimit;
|
||||
m_upperLimit = other.m_upperLimit;
|
||||
m_bounce = other.m_bounce;
|
||||
m_stopERP = other.m_stopERP;
|
||||
m_stopCFM = other.m_stopCFM;
|
||||
m_motorERP = other.m_motorERP;
|
||||
m_motorCFM = other.m_motorCFM;
|
||||
|
||||
m_currentLimitError = other.m_currentLimitError;
|
||||
m_currentLimitErrorHi = other.m_currentLimitErrorHi;
|
||||
m_currentLinearDiff = other.m_currentLinearDiff;
|
||||
m_currentLinearDiff = other.m_currentLinearDiff;
|
||||
|
||||
for(int i=0; i < 3; i++)
|
||||
for (int i = 0; i < 3; i++)
|
||||
{
|
||||
m_enableMotor[i] = other.m_enableMotor[i];
|
||||
m_servoMotor[i] = other.m_servoMotor[i];
|
||||
m_enableSpring[i] = other.m_enableSpring[i];
|
||||
m_servoTarget[i] = other.m_servoTarget[i];
|
||||
m_springStiffness[i] = other.m_springStiffness[i];
|
||||
m_enableMotor[i] = other.m_enableMotor[i];
|
||||
m_servoMotor[i] = other.m_servoMotor[i];
|
||||
m_enableSpring[i] = other.m_enableSpring[i];
|
||||
m_servoTarget[i] = other.m_servoTarget[i];
|
||||
m_springStiffness[i] = other.m_springStiffness[i];
|
||||
m_springStiffnessLimited[i] = other.m_springStiffnessLimited[i];
|
||||
m_springDamping[i] = other.m_springDamping[i];
|
||||
m_springDampingLimited[i] = other.m_springDampingLimited[i];
|
||||
m_equilibriumPoint[i] = other.m_equilibriumPoint[i];
|
||||
m_targetVelocity[i] = other.m_targetVelocity[i];
|
||||
m_maxMotorForce[i] = other.m_maxMotorForce[i];
|
||||
m_springDamping[i] = other.m_springDamping[i];
|
||||
m_springDampingLimited[i] = other.m_springDampingLimited[i];
|
||||
m_equilibriumPoint[i] = other.m_equilibriumPoint[i];
|
||||
m_targetVelocity[i] = other.m_targetVelocity[i];
|
||||
m_maxMotorForce[i] = other.m_maxMotorForce[i];
|
||||
|
||||
m_currentLimit[i] = other.m_currentLimit[i];
|
||||
m_currentLimit[i] = other.m_currentLimit[i];
|
||||
}
|
||||
}
|
||||
|
||||
@@ -271,13 +266,12 @@ enum bt6DofFlags2
|
||||
BT_6DOF_FLAGS_CFM_MOTO2 = 4,
|
||||
BT_6DOF_FLAGS_ERP_MOTO2 = 8
|
||||
};
|
||||
#define BT_6DOF_FLAGS_AXIS_SHIFT2 4 // bits per axis
|
||||
#define BT_6DOF_FLAGS_AXIS_SHIFT2 4 // bits per axis
|
||||
|
||||
|
||||
ATTRIBUTE_ALIGNED16(class) btGeneric6DofSpring2Constraint : public btTypedConstraint
|
||||
ATTRIBUTE_ALIGNED16(class)
|
||||
btGeneric6DofSpring2Constraint : public btTypedConstraint
|
||||
{
|
||||
protected:
|
||||
|
||||
btTransform m_frameInA;
|
||||
btTransform m_frameInB;
|
||||
|
||||
@@ -290,45 +284,43 @@ protected:
|
||||
RotateOrder m_rotateOrder;
|
||||
|
||||
protected:
|
||||
btTransform m_calculatedTransformA;
|
||||
btTransform m_calculatedTransformB;
|
||||
btVector3 m_calculatedAxisAngleDiff;
|
||||
btVector3 m_calculatedAxis[3];
|
||||
btVector3 m_calculatedLinearDiff;
|
||||
btScalar m_factA;
|
||||
btScalar m_factB;
|
||||
bool m_hasStaticBody;
|
||||
int m_flags;
|
||||
|
||||
btTransform m_calculatedTransformA;
|
||||
btTransform m_calculatedTransformB;
|
||||
btVector3 m_calculatedAxisAngleDiff;
|
||||
btVector3 m_calculatedAxis[3];
|
||||
btVector3 m_calculatedLinearDiff;
|
||||
btScalar m_factA;
|
||||
btScalar m_factB;
|
||||
bool m_hasStaticBody;
|
||||
int m_flags;
|
||||
|
||||
btGeneric6DofSpring2Constraint& operator=(btGeneric6DofSpring2Constraint&)
|
||||
btGeneric6DofSpring2Constraint& operator=(btGeneric6DofSpring2Constraint&)
|
||||
{
|
||||
btAssert(0);
|
||||
return *this;
|
||||
}
|
||||
|
||||
int setAngularLimits(btConstraintInfo2 *info, int row_offset,const btTransform& transA,const btTransform& transB,const btVector3& linVelA,const btVector3& linVelB,const btVector3& angVelA,const btVector3& angVelB);
|
||||
int setLinearLimits(btConstraintInfo2 *info, int row, const btTransform& transA,const btTransform& transB,const btVector3& linVelA,const btVector3& linVelB,const btVector3& angVelA,const btVector3& angVelB);
|
||||
int setAngularLimits(btConstraintInfo2 * info, int row_offset, const btTransform& transA, const btTransform& transB, const btVector3& linVelA, const btVector3& linVelB, const btVector3& angVelA, const btVector3& angVelB);
|
||||
int setLinearLimits(btConstraintInfo2 * info, int row, const btTransform& transA, const btTransform& transB, const btVector3& linVelA, const btVector3& linVelB, const btVector3& angVelA, const btVector3& angVelB);
|
||||
|
||||
void calculateLinearInfo();
|
||||
void calculateAngleInfo();
|
||||
void testAngularLimitMotor(int axis_index);
|
||||
|
||||
void calculateJacobi(btRotationalLimitMotor2* limot, const btTransform& transA,const btTransform& transB, btConstraintInfo2* info, int srow, btVector3& ax1, int rotational, int rotAllowed);
|
||||
int get_limit_motor_info2(btRotationalLimitMotor2* limot,
|
||||
const btTransform& transA,const btTransform& transB,const btVector3& linVelA,const btVector3& linVelB,const btVector3& angVelA,const btVector3& angVelB,
|
||||
btConstraintInfo2* info, int row, btVector3& ax1, int rotational, int rotAllowed = false);
|
||||
void calculateJacobi(btRotationalLimitMotor2 * limot, const btTransform& transA, const btTransform& transB, btConstraintInfo2* info, int srow, btVector3& ax1, int rotational, int rotAllowed);
|
||||
int get_limit_motor_info2(btRotationalLimitMotor2 * limot,
|
||||
const btTransform& transA, const btTransform& transB, const btVector3& linVelA, const btVector3& linVelB, const btVector3& angVelA, const btVector3& angVelB,
|
||||
btConstraintInfo2* info, int row, btVector3& ax1, int rotational, int rotAllowed = false);
|
||||
|
||||
public:
|
||||
|
||||
BT_DECLARE_ALIGNED_ALLOCATOR();
|
||||
|
||||
btGeneric6DofSpring2Constraint(btRigidBody& rbA, btRigidBody& rbB, const btTransform& frameInA, const btTransform& frameInB, RotateOrder rotOrder = RO_XYZ);
|
||||
btGeneric6DofSpring2Constraint(btRigidBody& rbB, const btTransform& frameInB, RotateOrder rotOrder = RO_XYZ);
|
||||
btGeneric6DofSpring2Constraint(btRigidBody & rbA, btRigidBody & rbB, const btTransform& frameInA, const btTransform& frameInB, RotateOrder rotOrder = RO_XYZ);
|
||||
btGeneric6DofSpring2Constraint(btRigidBody & rbB, const btTransform& frameInB, RotateOrder rotOrder = RO_XYZ);
|
||||
|
||||
virtual void buildJacobian() {}
|
||||
virtual void getInfo1 (btConstraintInfo1* info);
|
||||
virtual void getInfo2 (btConstraintInfo2* info);
|
||||
virtual void getInfo1(btConstraintInfo1 * info);
|
||||
virtual void getInfo2(btConstraintInfo2 * info);
|
||||
virtual int calculateSerializeBufferSize() const;
|
||||
virtual const char* serialize(void* dataBuffer, btSerializer* serializer) const;
|
||||
|
||||
@@ -336,19 +328,19 @@ public:
|
||||
btTranslationalLimitMotor2* getTranslationalLimitMotor() { return &m_linearLimits; }
|
||||
|
||||
// Calculates the global transform for the joint offset for body A an B, and also calculates the angle differences between the bodies.
|
||||
void calculateTransforms(const btTransform& transA,const btTransform& transB);
|
||||
void calculateTransforms(const btTransform& transA, const btTransform& transB);
|
||||
void calculateTransforms();
|
||||
|
||||
// Gets the global transform of the offset for body A
|
||||
const btTransform & getCalculatedTransformA() const { return m_calculatedTransformA; }
|
||||
const btTransform& getCalculatedTransformA() const { return m_calculatedTransformA; }
|
||||
// Gets the global transform of the offset for body B
|
||||
const btTransform & getCalculatedTransformB() const { return m_calculatedTransformB; }
|
||||
const btTransform& getCalculatedTransformB() const { return m_calculatedTransformB; }
|
||||
|
||||
const btTransform & getFrameOffsetA() const { return m_frameInA; }
|
||||
const btTransform & getFrameOffsetB() const { return m_frameInB; }
|
||||
const btTransform& getFrameOffsetA() const { return m_frameInA; }
|
||||
const btTransform& getFrameOffsetB() const { return m_frameInB; }
|
||||
|
||||
btTransform & getFrameOffsetA() { return m_frameInA; }
|
||||
btTransform & getFrameOffsetB() { return m_frameInB; }
|
||||
btTransform& getFrameOffsetA() { return m_frameInA; }
|
||||
btTransform& getFrameOffsetB() { return m_frameInB; }
|
||||
|
||||
// Get the rotation axis in global coordinates ( btGeneric6DofSpring2Constraint::calculateTransforms() must be called previously )
|
||||
btVector3 getAxis(int axis_index) const { return m_calculatedAxis[axis_index]; }
|
||||
@@ -359,58 +351,58 @@ public:
|
||||
// Get the relative position of the constraint pivot ( btGeneric6DofSpring2Constraint::calculateTransforms() must be called previously )
|
||||
btScalar getRelativePivotPosition(int axis_index) const { return m_calculatedLinearDiff[axis_index]; }
|
||||
|
||||
void setFrames(const btTransform & frameA, const btTransform & frameB);
|
||||
void setFrames(const btTransform& frameA, const btTransform& frameB);
|
||||
|
||||
void setLinearLowerLimit(const btVector3& linearLower) { m_linearLimits.m_lowerLimit = linearLower; }
|
||||
void getLinearLowerLimit(btVector3& linearLower) { linearLower = m_linearLimits.m_lowerLimit; }
|
||||
void getLinearLowerLimit(btVector3 & linearLower) { linearLower = m_linearLimits.m_lowerLimit; }
|
||||
void setLinearUpperLimit(const btVector3& linearUpper) { m_linearLimits.m_upperLimit = linearUpper; }
|
||||
void getLinearUpperLimit(btVector3& linearUpper) { linearUpper = m_linearLimits.m_upperLimit; }
|
||||
void getLinearUpperLimit(btVector3 & linearUpper) { linearUpper = m_linearLimits.m_upperLimit; }
|
||||
|
||||
void setAngularLowerLimit(const btVector3& angularLower)
|
||||
{
|
||||
for(int i = 0; i < 3; i++)
|
||||
for (int i = 0; i < 3; i++)
|
||||
m_angularLimits[i].m_loLimit = btNormalizeAngle(angularLower[i]);
|
||||
}
|
||||
|
||||
void setAngularLowerLimitReversed(const btVector3& angularLower)
|
||||
{
|
||||
for(int i = 0; i < 3; i++)
|
||||
for (int i = 0; i < 3; i++)
|
||||
m_angularLimits[i].m_hiLimit = btNormalizeAngle(-angularLower[i]);
|
||||
}
|
||||
|
||||
void getAngularLowerLimit(btVector3& angularLower)
|
||||
void getAngularLowerLimit(btVector3 & angularLower)
|
||||
{
|
||||
for(int i = 0; i < 3; i++)
|
||||
for (int i = 0; i < 3; i++)
|
||||
angularLower[i] = m_angularLimits[i].m_loLimit;
|
||||
}
|
||||
|
||||
void getAngularLowerLimitReversed(btVector3& angularLower)
|
||||
void getAngularLowerLimitReversed(btVector3 & angularLower)
|
||||
{
|
||||
for(int i = 0; i < 3; i++)
|
||||
for (int i = 0; i < 3; i++)
|
||||
angularLower[i] = -m_angularLimits[i].m_hiLimit;
|
||||
}
|
||||
|
||||
void setAngularUpperLimit(const btVector3& angularUpper)
|
||||
{
|
||||
for(int i = 0; i < 3; i++)
|
||||
for (int i = 0; i < 3; i++)
|
||||
m_angularLimits[i].m_hiLimit = btNormalizeAngle(angularUpper[i]);
|
||||
}
|
||||
|
||||
void setAngularUpperLimitReversed(const btVector3& angularUpper)
|
||||
{
|
||||
for(int i = 0; i < 3; i++)
|
||||
for (int i = 0; i < 3; i++)
|
||||
m_angularLimits[i].m_loLimit = btNormalizeAngle(-angularUpper[i]);
|
||||
}
|
||||
|
||||
void getAngularUpperLimit(btVector3& angularUpper)
|
||||
void getAngularUpperLimit(btVector3 & angularUpper)
|
||||
{
|
||||
for(int i = 0; i < 3; i++)
|
||||
for (int i = 0; i < 3; i++)
|
||||
angularUpper[i] = m_angularLimits[i].m_hiLimit;
|
||||
}
|
||||
|
||||
void getAngularUpperLimitReversed(btVector3& angularUpper)
|
||||
void getAngularUpperLimitReversed(btVector3 & angularUpper)
|
||||
{
|
||||
for(int i = 0; i < 3; i++)
|
||||
for (int i = 0; i < 3; i++)
|
||||
angularUpper[i] = -m_angularLimits[i].m_loLimit;
|
||||
}
|
||||
|
||||
@@ -418,7 +410,7 @@ public:
|
||||
|
||||
void setLimit(int axis, btScalar lo, btScalar hi)
|
||||
{
|
||||
if(axis<3)
|
||||
if (axis < 3)
|
||||
{
|
||||
m_linearLimits.m_lowerLimit[axis] = lo;
|
||||
m_linearLimits.m_upperLimit[axis] = hi;
|
||||
@@ -427,14 +419,14 @@ public:
|
||||
{
|
||||
lo = btNormalizeAngle(lo);
|
||||
hi = btNormalizeAngle(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;
|
||||
}
|
||||
}
|
||||
|
||||
void setLimitReversed(int axis, btScalar lo, btScalar hi)
|
||||
{
|
||||
if(axis<3)
|
||||
if (axis < 3)
|
||||
{
|
||||
m_linearLimits.m_lowerLimit[axis] = lo;
|
||||
m_linearLimits.m_upperLimit[axis] = hi;
|
||||
@@ -443,54 +435,53 @@ public:
|
||||
{
|
||||
lo = btNormalizeAngle(lo);
|
||||
hi = btNormalizeAngle(hi);
|
||||
m_angularLimits[axis-3].m_hiLimit = -lo;
|
||||
m_angularLimits[axis-3].m_loLimit = -hi;
|
||||
m_angularLimits[axis - 3].m_hiLimit = -lo;
|
||||
m_angularLimits[axis - 3].m_loLimit = -hi;
|
||||
}
|
||||
}
|
||||
|
||||
bool isLimited(int limitIndex)
|
||||
{
|
||||
if(limitIndex<3)
|
||||
if (limitIndex < 3)
|
||||
{
|
||||
return m_linearLimits.isLimited(limitIndex);
|
||||
}
|
||||
return m_angularLimits[limitIndex-3].isLimited();
|
||||
return m_angularLimits[limitIndex - 3].isLimited();
|
||||
}
|
||||
|
||||
void setRotationOrder(RotateOrder order) { m_rotateOrder = order; }
|
||||
RotateOrder getRotationOrder() { return m_rotateOrder; }
|
||||
|
||||
void setAxis( const btVector3& axis1, const btVector3& axis2);
|
||||
void setAxis(const btVector3& axis1, const btVector3& axis2);
|
||||
|
||||
void setBounce(int index, btScalar bounce);
|
||||
|
||||
void enableMotor(int index, bool onOff);
|
||||
void setServo(int index, bool onOff); // set the type of the motor (servo or not) (the motor has to be turned on for servo also)
|
||||
void setServo(int index, bool onOff); // set the type of the motor (servo or not) (the motor has to be turned on for servo also)
|
||||
void setTargetVelocity(int index, btScalar velocity);
|
||||
void setServoTarget(int index, btScalar target);
|
||||
void setMaxMotorForce(int index, btScalar force);
|
||||
|
||||
void enableSpring(int index, bool onOff);
|
||||
void setStiffness(int index, btScalar stiffness, bool limitIfNeeded = true); // if limitIfNeeded is true the system will automatically limit the stiffness in necessary situations where otherwise the spring would move unrealistically too widely
|
||||
void setDamping(int index, btScalar damping, bool limitIfNeeded = true); // if limitIfNeeded is true the system will automatically limit the damping in necessary situations where otherwise the spring would blow up
|
||||
void setEquilibriumPoint(); // set the current constraint position/orientation as an equilibrium point for all DOF
|
||||
void setEquilibriumPoint(int index); // set the current constraint position/orientation as an equilibrium point for given DOF
|
||||
void setStiffness(int index, btScalar stiffness, bool limitIfNeeded = true); // if limitIfNeeded is true the system will automatically limit the stiffness in necessary situations where otherwise the spring would move unrealistically too widely
|
||||
void setDamping(int index, btScalar damping, bool limitIfNeeded = true); // if limitIfNeeded is true the system will automatically limit the damping in necessary situations where otherwise the spring would blow up
|
||||
void setEquilibriumPoint(); // set the current constraint position/orientation as an equilibrium point for all DOF
|
||||
void setEquilibriumPoint(int index); // set the current constraint position/orientation as an equilibrium point for given DOF
|
||||
void setEquilibriumPoint(int index, btScalar val);
|
||||
|
||||
//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, btScalar value, int axis = -1);
|
||||
virtual btScalar getParam(int num, int axis = -1) const;
|
||||
|
||||
static btScalar btGetMatrixElem(const btMatrix3x3& mat, int index);
|
||||
static bool matrixToEulerXYZ(const btMatrix3x3& mat,btVector3& xyz);
|
||||
static bool matrixToEulerXZY(const btMatrix3x3& mat,btVector3& xyz);
|
||||
static bool matrixToEulerYXZ(const btMatrix3x3& mat,btVector3& xyz);
|
||||
static bool matrixToEulerYZX(const btMatrix3x3& mat,btVector3& xyz);
|
||||
static bool matrixToEulerZXY(const btMatrix3x3& mat,btVector3& xyz);
|
||||
static bool matrixToEulerZYX(const btMatrix3x3& mat,btVector3& xyz);
|
||||
};
|
||||
|
||||
static btScalar btGetMatrixElem(const btMatrix3x3& mat, int index);
|
||||
static bool matrixToEulerXYZ(const btMatrix3x3& mat, btVector3& xyz);
|
||||
static bool matrixToEulerXZY(const btMatrix3x3& mat, btVector3& xyz);
|
||||
static bool matrixToEulerYXZ(const btMatrix3x3& mat, btVector3& xyz);
|
||||
static bool matrixToEulerYZX(const btMatrix3x3& mat, btVector3& xyz);
|
||||
static bool matrixToEulerZXY(const btMatrix3x3& mat, btVector3& xyz);
|
||||
static bool matrixToEulerZYX(const btMatrix3x3& mat, btVector3& xyz);
|
||||
};
|
||||
|
||||
struct btGeneric6DofSpring2ConstraintData
|
||||
{
|
||||
@@ -511,12 +502,12 @@ struct btGeneric6DofSpring2ConstraintData
|
||||
btVector3FloatData m_linearSpringStiffness;
|
||||
btVector3FloatData m_linearSpringDamping;
|
||||
btVector3FloatData m_linearEquilibriumPoint;
|
||||
char m_linearEnableMotor[4];
|
||||
char m_linearServoMotor[4];
|
||||
char m_linearEnableSpring[4];
|
||||
char m_linearSpringStiffnessLimited[4];
|
||||
char m_linearSpringDampingLimited[4];
|
||||
char m_padding1[4];
|
||||
char m_linearEnableMotor[4];
|
||||
char m_linearServoMotor[4];
|
||||
char m_linearEnableSpring[4];
|
||||
char m_linearSpringStiffnessLimited[4];
|
||||
char m_linearSpringDampingLimited[4];
|
||||
char m_padding1[4];
|
||||
|
||||
btVector3FloatData m_angularUpperLimit;
|
||||
btVector3FloatData m_angularLowerLimit;
|
||||
@@ -531,13 +522,13 @@ struct btGeneric6DofSpring2ConstraintData
|
||||
btVector3FloatData m_angularSpringStiffness;
|
||||
btVector3FloatData m_angularSpringDamping;
|
||||
btVector3FloatData m_angularEquilibriumPoint;
|
||||
char m_angularEnableMotor[4];
|
||||
char m_angularServoMotor[4];
|
||||
char m_angularEnableSpring[4];
|
||||
char m_angularSpringStiffnessLimited[4];
|
||||
char m_angularSpringDampingLimited[4];
|
||||
char m_angularEnableMotor[4];
|
||||
char m_angularServoMotor[4];
|
||||
char m_angularEnableSpring[4];
|
||||
char m_angularSpringStiffnessLimited[4];
|
||||
char m_angularSpringDampingLimited[4];
|
||||
|
||||
int m_rotateOrder;
|
||||
int m_rotateOrder;
|
||||
};
|
||||
|
||||
struct btGeneric6DofSpring2ConstraintDoubleData2
|
||||
@@ -559,12 +550,12 @@ struct btGeneric6DofSpring2ConstraintDoubleData2
|
||||
btVector3DoubleData m_linearSpringStiffness;
|
||||
btVector3DoubleData m_linearSpringDamping;
|
||||
btVector3DoubleData m_linearEquilibriumPoint;
|
||||
char m_linearEnableMotor[4];
|
||||
char m_linearServoMotor[4];
|
||||
char m_linearEnableSpring[4];
|
||||
char m_linearSpringStiffnessLimited[4];
|
||||
char m_linearSpringDampingLimited[4];
|
||||
char m_padding1[4];
|
||||
char m_linearEnableMotor[4];
|
||||
char m_linearServoMotor[4];
|
||||
char m_linearEnableSpring[4];
|
||||
char m_linearSpringStiffnessLimited[4];
|
||||
char m_linearSpringDampingLimited[4];
|
||||
char m_padding1[4];
|
||||
|
||||
btVector3DoubleData m_angularUpperLimit;
|
||||
btVector3DoubleData m_angularLowerLimit;
|
||||
@@ -579,13 +570,13 @@ struct btGeneric6DofSpring2ConstraintDoubleData2
|
||||
btVector3DoubleData m_angularSpringStiffness;
|
||||
btVector3DoubleData m_angularSpringDamping;
|
||||
btVector3DoubleData m_angularEquilibriumPoint;
|
||||
char m_angularEnableMotor[4];
|
||||
char m_angularServoMotor[4];
|
||||
char m_angularEnableSpring[4];
|
||||
char m_angularSpringStiffnessLimited[4];
|
||||
char m_angularSpringDampingLimited[4];
|
||||
char m_angularEnableMotor[4];
|
||||
char m_angularServoMotor[4];
|
||||
char m_angularEnableSpring[4];
|
||||
char m_angularSpringStiffnessLimited[4];
|
||||
char m_angularSpringDampingLimited[4];
|
||||
|
||||
int m_rotateOrder;
|
||||
int m_rotateOrder;
|
||||
};
|
||||
|
||||
SIMD_FORCE_INLINE int btGeneric6DofSpring2Constraint::calculateSerializeBufferSize() const
|
||||
@@ -596,84 +587,80 @@ SIMD_FORCE_INLINE int btGeneric6DofSpring2Constraint::calculateSerializeBufferSi
|
||||
SIMD_FORCE_INLINE const char* btGeneric6DofSpring2Constraint::serialize(void* dataBuffer, btSerializer* serializer) const
|
||||
{
|
||||
btGeneric6DofSpring2ConstraintData2* dof = (btGeneric6DofSpring2ConstraintData2*)dataBuffer;
|
||||
btTypedConstraint::serialize(&dof->m_typeConstraintData,serializer);
|
||||
btTypedConstraint::serialize(&dof->m_typeConstraintData, serializer);
|
||||
|
||||
m_frameInA.serialize(dof->m_rbAFrame);
|
||||
m_frameInB.serialize(dof->m_rbBFrame);
|
||||
|
||||
int i;
|
||||
for (i=0;i<3;i++)
|
||||
for (i = 0; i < 3; i++)
|
||||
{
|
||||
dof->m_angularLowerLimit.m_floats[i] = m_angularLimits[i].m_loLimit;
|
||||
dof->m_angularUpperLimit.m_floats[i] = m_angularLimits[i].m_hiLimit;
|
||||
dof->m_angularBounce.m_floats[i] = m_angularLimits[i].m_bounce;
|
||||
dof->m_angularStopERP.m_floats[i] = m_angularLimits[i].m_stopERP;
|
||||
dof->m_angularStopCFM.m_floats[i] = m_angularLimits[i].m_stopCFM;
|
||||
dof->m_angularMotorERP.m_floats[i] = m_angularLimits[i].m_motorERP;
|
||||
dof->m_angularMotorCFM.m_floats[i] = m_angularLimits[i].m_motorCFM;
|
||||
dof->m_angularTargetVelocity.m_floats[i] = m_angularLimits[i].m_targetVelocity;
|
||||
dof->m_angularMaxMotorForce.m_floats[i] = m_angularLimits[i].m_maxMotorForce;
|
||||
dof->m_angularServoTarget.m_floats[i] = m_angularLimits[i].m_servoTarget;
|
||||
dof->m_angularSpringStiffness.m_floats[i] = m_angularLimits[i].m_springStiffness;
|
||||
dof->m_angularSpringDamping.m_floats[i] = m_angularLimits[i].m_springDamping;
|
||||
dof->m_angularLowerLimit.m_floats[i] = m_angularLimits[i].m_loLimit;
|
||||
dof->m_angularUpperLimit.m_floats[i] = m_angularLimits[i].m_hiLimit;
|
||||
dof->m_angularBounce.m_floats[i] = m_angularLimits[i].m_bounce;
|
||||
dof->m_angularStopERP.m_floats[i] = m_angularLimits[i].m_stopERP;
|
||||
dof->m_angularStopCFM.m_floats[i] = m_angularLimits[i].m_stopCFM;
|
||||
dof->m_angularMotorERP.m_floats[i] = m_angularLimits[i].m_motorERP;
|
||||
dof->m_angularMotorCFM.m_floats[i] = m_angularLimits[i].m_motorCFM;
|
||||
dof->m_angularTargetVelocity.m_floats[i] = m_angularLimits[i].m_targetVelocity;
|
||||
dof->m_angularMaxMotorForce.m_floats[i] = m_angularLimits[i].m_maxMotorForce;
|
||||
dof->m_angularServoTarget.m_floats[i] = m_angularLimits[i].m_servoTarget;
|
||||
dof->m_angularSpringStiffness.m_floats[i] = m_angularLimits[i].m_springStiffness;
|
||||
dof->m_angularSpringDamping.m_floats[i] = m_angularLimits[i].m_springDamping;
|
||||
dof->m_angularEquilibriumPoint.m_floats[i] = m_angularLimits[i].m_equilibriumPoint;
|
||||
}
|
||||
dof->m_angularLowerLimit.m_floats[3] = 0;
|
||||
dof->m_angularUpperLimit.m_floats[3] = 0;
|
||||
dof->m_angularBounce.m_floats[3] = 0;
|
||||
dof->m_angularStopERP.m_floats[3] = 0;
|
||||
dof->m_angularStopCFM.m_floats[3] = 0;
|
||||
dof->m_angularMotorERP.m_floats[3] = 0;
|
||||
dof->m_angularMotorCFM.m_floats[3] = 0;
|
||||
dof->m_angularTargetVelocity.m_floats[3] = 0;
|
||||
dof->m_angularMaxMotorForce.m_floats[3] = 0;
|
||||
dof->m_angularServoTarget.m_floats[3] = 0;
|
||||
dof->m_angularSpringStiffness.m_floats[3] = 0;
|
||||
dof->m_angularSpringDamping.m_floats[3] = 0;
|
||||
dof->m_angularLowerLimit.m_floats[3] = 0;
|
||||
dof->m_angularUpperLimit.m_floats[3] = 0;
|
||||
dof->m_angularBounce.m_floats[3] = 0;
|
||||
dof->m_angularStopERP.m_floats[3] = 0;
|
||||
dof->m_angularStopCFM.m_floats[3] = 0;
|
||||
dof->m_angularMotorERP.m_floats[3] = 0;
|
||||
dof->m_angularMotorCFM.m_floats[3] = 0;
|
||||
dof->m_angularTargetVelocity.m_floats[3] = 0;
|
||||
dof->m_angularMaxMotorForce.m_floats[3] = 0;
|
||||
dof->m_angularServoTarget.m_floats[3] = 0;
|
||||
dof->m_angularSpringStiffness.m_floats[3] = 0;
|
||||
dof->m_angularSpringDamping.m_floats[3] = 0;
|
||||
dof->m_angularEquilibriumPoint.m_floats[3] = 0;
|
||||
for (i=0;i<4;i++)
|
||||
for (i = 0; i < 4; i++)
|
||||
{
|
||||
dof->m_angularEnableMotor[i] = i < 3 ? ( m_angularLimits[i].m_enableMotor ? 1 : 0 ) : 0;
|
||||
dof->m_angularServoMotor[i] = i < 3 ? ( m_angularLimits[i].m_servoMotor ? 1 : 0 ) : 0;
|
||||
dof->m_angularEnableSpring[i] = i < 3 ? ( m_angularLimits[i].m_enableSpring ? 1 : 0 ) : 0;
|
||||
dof->m_angularSpringStiffnessLimited[i] = i < 3 ? ( m_angularLimits[i].m_springStiffnessLimited ? 1 : 0 ) : 0;
|
||||
dof->m_angularSpringDampingLimited[i] = i < 3 ? ( m_angularLimits[i].m_springDampingLimited ? 1 : 0 ) : 0;
|
||||
dof->m_angularEnableMotor[i] = i < 3 ? (m_angularLimits[i].m_enableMotor ? 1 : 0) : 0;
|
||||
dof->m_angularServoMotor[i] = i < 3 ? (m_angularLimits[i].m_servoMotor ? 1 : 0) : 0;
|
||||
dof->m_angularEnableSpring[i] = i < 3 ? (m_angularLimits[i].m_enableSpring ? 1 : 0) : 0;
|
||||
dof->m_angularSpringStiffnessLimited[i] = i < 3 ? (m_angularLimits[i].m_springStiffnessLimited ? 1 : 0) : 0;
|
||||
dof->m_angularSpringDampingLimited[i] = i < 3 ? (m_angularLimits[i].m_springDampingLimited ? 1 : 0) : 0;
|
||||
}
|
||||
|
||||
m_linearLimits.m_lowerLimit.serialize( dof->m_linearLowerLimit );
|
||||
m_linearLimits.m_upperLimit.serialize( dof->m_linearUpperLimit );
|
||||
m_linearLimits.m_bounce.serialize( dof->m_linearBounce );
|
||||
m_linearLimits.m_stopERP.serialize( dof->m_linearStopERP );
|
||||
m_linearLimits.m_stopCFM.serialize( dof->m_linearStopCFM );
|
||||
m_linearLimits.m_motorERP.serialize( dof->m_linearMotorERP );
|
||||
m_linearLimits.m_motorCFM.serialize( dof->m_linearMotorCFM );
|
||||
m_linearLimits.m_targetVelocity.serialize( dof->m_linearTargetVelocity );
|
||||
m_linearLimits.m_maxMotorForce.serialize( dof->m_linearMaxMotorForce );
|
||||
m_linearLimits.m_servoTarget.serialize( dof->m_linearServoTarget );
|
||||
m_linearLimits.m_springStiffness.serialize( dof->m_linearSpringStiffness );
|
||||
m_linearLimits.m_springDamping.serialize( dof->m_linearSpringDamping );
|
||||
m_linearLimits.m_equilibriumPoint.serialize( dof->m_linearEquilibriumPoint );
|
||||
for (i=0;i<4;i++)
|
||||
m_linearLimits.m_lowerLimit.serialize(dof->m_linearLowerLimit);
|
||||
m_linearLimits.m_upperLimit.serialize(dof->m_linearUpperLimit);
|
||||
m_linearLimits.m_bounce.serialize(dof->m_linearBounce);
|
||||
m_linearLimits.m_stopERP.serialize(dof->m_linearStopERP);
|
||||
m_linearLimits.m_stopCFM.serialize(dof->m_linearStopCFM);
|
||||
m_linearLimits.m_motorERP.serialize(dof->m_linearMotorERP);
|
||||
m_linearLimits.m_motorCFM.serialize(dof->m_linearMotorCFM);
|
||||
m_linearLimits.m_targetVelocity.serialize(dof->m_linearTargetVelocity);
|
||||
m_linearLimits.m_maxMotorForce.serialize(dof->m_linearMaxMotorForce);
|
||||
m_linearLimits.m_servoTarget.serialize(dof->m_linearServoTarget);
|
||||
m_linearLimits.m_springStiffness.serialize(dof->m_linearSpringStiffness);
|
||||
m_linearLimits.m_springDamping.serialize(dof->m_linearSpringDamping);
|
||||
m_linearLimits.m_equilibriumPoint.serialize(dof->m_linearEquilibriumPoint);
|
||||
for (i = 0; i < 4; i++)
|
||||
{
|
||||
dof->m_linearEnableMotor[i] = i < 3 ? ( m_linearLimits.m_enableMotor[i] ? 1 : 0 ) : 0;
|
||||
dof->m_linearServoMotor[i] = i < 3 ? ( m_linearLimits.m_servoMotor[i] ? 1 : 0 ) : 0;
|
||||
dof->m_linearEnableSpring[i] = i < 3 ? ( m_linearLimits.m_enableSpring[i] ? 1 : 0 ) : 0;
|
||||
dof->m_linearSpringStiffnessLimited[i] = i < 3 ? ( m_linearLimits.m_springStiffnessLimited[i] ? 1 : 0 ) : 0;
|
||||
dof->m_linearSpringDampingLimited[i] = i < 3 ? ( m_linearLimits.m_springDampingLimited[i] ? 1 : 0 ) : 0;
|
||||
dof->m_linearEnableMotor[i] = i < 3 ? (m_linearLimits.m_enableMotor[i] ? 1 : 0) : 0;
|
||||
dof->m_linearServoMotor[i] = i < 3 ? (m_linearLimits.m_servoMotor[i] ? 1 : 0) : 0;
|
||||
dof->m_linearEnableSpring[i] = i < 3 ? (m_linearLimits.m_enableSpring[i] ? 1 : 0) : 0;
|
||||
dof->m_linearSpringStiffnessLimited[i] = i < 3 ? (m_linearLimits.m_springStiffnessLimited[i] ? 1 : 0) : 0;
|
||||
dof->m_linearSpringDampingLimited[i] = i < 3 ? (m_linearLimits.m_springDampingLimited[i] ? 1 : 0) : 0;
|
||||
}
|
||||
|
||||
dof->m_rotateOrder = m_rotateOrder;
|
||||
|
||||
dof->m_padding1[0] = 0;
|
||||
dof->m_padding1[1] = 0;
|
||||
dof->m_padding1[2] = 0;
|
||||
dof->m_padding1[3] = 0;
|
||||
|
||||
dof->m_padding1[0] = 0;
|
||||
dof->m_padding1[1] = 0;
|
||||
dof->m_padding1[2] = 0;
|
||||
dof->m_padding1[3] = 0;
|
||||
|
||||
return btGeneric6DofSpring2ConstraintDataName;
|
||||
}
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
#endif //BT_GENERIC_6DOF_CONSTRAINT_H
|
||||
#endif //BT_GENERIC_6DOF_CONSTRAINT_H
|
||||
|
||||
@@ -17,26 +17,23 @@ subject to the following restrictions:
|
||||
#include "BulletDynamics/Dynamics/btRigidBody.h"
|
||||
#include "LinearMath/btTransformUtil.h"
|
||||
|
||||
|
||||
btGeneric6DofSpringConstraint::btGeneric6DofSpringConstraint(btRigidBody& rbA, btRigidBody& rbB, const btTransform& frameInA, const btTransform& frameInB ,bool useLinearReferenceFrameA)
|
||||
btGeneric6DofSpringConstraint::btGeneric6DofSpringConstraint(btRigidBody& rbA, btRigidBody& rbB, const btTransform& frameInA, const btTransform& frameInB, bool useLinearReferenceFrameA)
|
||||
: btGeneric6DofConstraint(rbA, rbB, frameInA, frameInB, useLinearReferenceFrameA)
|
||||
{
|
||||
init();
|
||||
init();
|
||||
}
|
||||
|
||||
|
||||
btGeneric6DofSpringConstraint::btGeneric6DofSpringConstraint(btRigidBody& rbB, const btTransform& frameInB, bool useLinearReferenceFrameB)
|
||||
: btGeneric6DofConstraint(rbB, frameInB, useLinearReferenceFrameB)
|
||||
: btGeneric6DofConstraint(rbB, frameInB, useLinearReferenceFrameB)
|
||||
{
|
||||
init();
|
||||
init();
|
||||
}
|
||||
|
||||
|
||||
void btGeneric6DofSpringConstraint::init()
|
||||
{
|
||||
m_objectType = D6_SPRING_CONSTRAINT_TYPE;
|
||||
|
||||
for(int i = 0; i < 6; i++)
|
||||
for (int i = 0; i < 6; i++)
|
||||
{
|
||||
m_springEnabled[i] = false;
|
||||
m_equilibriumPoint[i] = btScalar(0.f);
|
||||
@@ -45,12 +42,11 @@ void btGeneric6DofSpringConstraint::init()
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
void btGeneric6DofSpringConstraint::enableSpring(int index, bool onOff)
|
||||
{
|
||||
btAssert((index >= 0) && (index < 6));
|
||||
m_springEnabled[index] = onOff;
|
||||
if(index < 3)
|
||||
if (index < 3)
|
||||
{
|
||||
m_linearLimits.m_enableMotor[index] = onOff;
|
||||
}
|
||||
@@ -60,44 +56,38 @@ void btGeneric6DofSpringConstraint::enableSpring(int index, bool onOff)
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
|
||||
void btGeneric6DofSpringConstraint::setStiffness(int index, btScalar stiffness)
|
||||
{
|
||||
btAssert((index >= 0) && (index < 6));
|
||||
m_springStiffness[index] = stiffness;
|
||||
}
|
||||
|
||||
|
||||
void btGeneric6DofSpringConstraint::setDamping(int index, btScalar damping)
|
||||
{
|
||||
btAssert((index >= 0) && (index < 6));
|
||||
m_springDamping[index] = damping;
|
||||
}
|
||||
|
||||
|
||||
void btGeneric6DofSpringConstraint::setEquilibriumPoint()
|
||||
{
|
||||
calculateTransforms();
|
||||
int i;
|
||||
|
||||
for( i = 0; i < 3; i++)
|
||||
for (i = 0; i < 3; i++)
|
||||
{
|
||||
m_equilibriumPoint[i] = m_calculatedLinearDiff[i];
|
||||
}
|
||||
for(i = 0; i < 3; i++)
|
||||
for (i = 0; i < 3; i++)
|
||||
{
|
||||
m_equilibriumPoint[i + 3] = m_calculatedAxisAngleDiff[i];
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
|
||||
void btGeneric6DofSpringConstraint::setEquilibriumPoint(int index)
|
||||
{
|
||||
btAssert((index >= 0) && (index < 6));
|
||||
calculateTransforms();
|
||||
if(index < 3)
|
||||
if (index < 3)
|
||||
{
|
||||
m_equilibriumPoint[index] = m_calculatedLinearDiff[index];
|
||||
}
|
||||
@@ -113,15 +103,14 @@ void btGeneric6DofSpringConstraint::setEquilibriumPoint(int index, btScalar val)
|
||||
m_equilibriumPoint[index] = val;
|
||||
}
|
||||
|
||||
|
||||
void btGeneric6DofSpringConstraint::internalUpdateSprings(btConstraintInfo2* info)
|
||||
{
|
||||
// it is assumed that calculateTransforms() have been called before this call
|
||||
int i;
|
||||
//btVector3 relVel = m_rbB.getLinearVelocity() - m_rbA.getLinearVelocity();
|
||||
for(i = 0; i < 3; i++)
|
||||
for (i = 0; i < 3; i++)
|
||||
{
|
||||
if(m_springEnabled[i])
|
||||
if (m_springEnabled[i])
|
||||
{
|
||||
// get current position of constraint
|
||||
btScalar currPos = m_calculatedLinearDiff[i];
|
||||
@@ -130,28 +119,27 @@ void btGeneric6DofSpringConstraint::internalUpdateSprings(btConstraintInfo2* inf
|
||||
// spring force is (delta * m_stiffness) according to Hooke's Law
|
||||
btScalar force = delta * m_springStiffness[i];
|
||||
btScalar velFactor = info->fps * m_springDamping[i] / btScalar(info->m_numIterations);
|
||||
m_linearLimits.m_targetVelocity[i] = velFactor * force;
|
||||
m_linearLimits.m_maxMotorForce[i] = btFabs(force);
|
||||
m_linearLimits.m_targetVelocity[i] = velFactor * force;
|
||||
m_linearLimits.m_maxMotorForce[i] = btFabs(force);
|
||||
}
|
||||
}
|
||||
for(i = 0; i < 3; i++)
|
||||
for (i = 0; i < 3; i++)
|
||||
{
|
||||
if(m_springEnabled[i + 3])
|
||||
if (m_springEnabled[i + 3])
|
||||
{
|
||||
// get current position of constraint
|
||||
btScalar currPos = m_calculatedAxisAngleDiff[i];
|
||||
// calculate difference
|
||||
btScalar delta = currPos - m_equilibriumPoint[i+3];
|
||||
btScalar delta = currPos - m_equilibriumPoint[i + 3];
|
||||
// spring force is (-delta * m_stiffness) according to Hooke's Law
|
||||
btScalar force = -delta * m_springStiffness[i+3];
|
||||
btScalar velFactor = info->fps * m_springDamping[i+3] / btScalar(info->m_numIterations);
|
||||
btScalar force = -delta * m_springStiffness[i + 3];
|
||||
btScalar velFactor = info->fps * m_springDamping[i + 3] / btScalar(info->m_numIterations);
|
||||
m_angularLimits[i].m_targetVelocity = velFactor * force;
|
||||
m_angularLimits[i].m_maxMotorForce = btFabs(force);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
void btGeneric6DofSpringConstraint::getInfo2(btConstraintInfo2* info)
|
||||
{
|
||||
// this will be called by constraint solver at the constraint setup stage
|
||||
@@ -161,25 +149,21 @@ void btGeneric6DofSpringConstraint::getInfo2(btConstraintInfo2* info)
|
||||
btGeneric6DofConstraint::getInfo2(info);
|
||||
}
|
||||
|
||||
|
||||
void btGeneric6DofSpringConstraint::setAxis(const btVector3& axis1,const btVector3& axis2)
|
||||
void btGeneric6DofSpringConstraint::setAxis(const btVector3& axis1, const btVector3& axis2)
|
||||
{
|
||||
btVector3 zAxis = axis1.normalized();
|
||||
btVector3 yAxis = axis2.normalized();
|
||||
btVector3 xAxis = yAxis.cross(zAxis); // we want right coordinate system
|
||||
btVector3 xAxis = yAxis.cross(zAxis); // we want right coordinate system
|
||||
|
||||
btTransform 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 = m_rbA.getCenterOfMassTransform().inverse() * frameInW;
|
||||
m_frameInB = m_rbB.getCenterOfMassTransform().inverse() * frameInW;
|
||||
|
||||
calculateTransforms();
|
||||
calculateTransforms();
|
||||
}
|
||||
|
||||
|
||||
|
||||
|
||||
@@ -16,20 +16,17 @@ subject to the following restrictions:
|
||||
#ifndef BT_GENERIC_6DOF_SPRING_CONSTRAINT_H
|
||||
#define BT_GENERIC_6DOF_SPRING_CONSTRAINT_H
|
||||
|
||||
|
||||
#include "LinearMath/btVector3.h"
|
||||
#include "btTypedConstraint.h"
|
||||
#include "btGeneric6DofConstraint.h"
|
||||
|
||||
#ifdef BT_USE_DOUBLE_PRECISION
|
||||
#define btGeneric6DofSpringConstraintData2 btGeneric6DofSpringConstraintDoubleData2
|
||||
#define btGeneric6DofSpringConstraintDataName "btGeneric6DofSpringConstraintDoubleData2"
|
||||
#define btGeneric6DofSpringConstraintData2 btGeneric6DofSpringConstraintDoubleData2
|
||||
#define btGeneric6DofSpringConstraintDataName "btGeneric6DofSpringConstraintDoubleData2"
|
||||
#else
|
||||
#define btGeneric6DofSpringConstraintData2 btGeneric6DofSpringConstraintData
|
||||
#define btGeneric6DofSpringConstraintDataName "btGeneric6DofSpringConstraintData"
|
||||
#endif //BT_USE_DOUBLE_PRECISION
|
||||
|
||||
|
||||
#define btGeneric6DofSpringConstraintData2 btGeneric6DofSpringConstraintData
|
||||
#define btGeneric6DofSpringConstraintDataName "btGeneric6DofSpringConstraintData"
|
||||
#endif //BT_USE_DOUBLE_PRECISION
|
||||
|
||||
/// Generic 6 DOF constraint that allows to set spring motors to any translational and rotational DOF
|
||||
|
||||
@@ -41,101 +38,98 @@ subject to the following restrictions:
|
||||
/// 4 : rotation Y (2nd Euler rotational around new position of Y axis, range [-PI/2+epsilon, PI/2-epsilon] )
|
||||
/// 5 : rotation Z (1st Euler rotational around Z axis, range [-PI+epsilon, PI-epsilon] )
|
||||
|
||||
ATTRIBUTE_ALIGNED16(class) btGeneric6DofSpringConstraint : public btGeneric6DofConstraint
|
||||
ATTRIBUTE_ALIGNED16(class)
|
||||
btGeneric6DofSpringConstraint : public btGeneric6DofConstraint
|
||||
{
|
||||
protected:
|
||||
bool m_springEnabled[6];
|
||||
btScalar m_equilibriumPoint[6];
|
||||
btScalar m_springStiffness[6];
|
||||
btScalar m_springDamping[6]; // between 0 and 1 (1 == no damping)
|
||||
bool m_springEnabled[6];
|
||||
btScalar m_equilibriumPoint[6];
|
||||
btScalar m_springStiffness[6];
|
||||
btScalar m_springDamping[6]; // between 0 and 1 (1 == no damping)
|
||||
void init();
|
||||
void internalUpdateSprings(btConstraintInfo2* info);
|
||||
public:
|
||||
|
||||
void internalUpdateSprings(btConstraintInfo2 * info);
|
||||
|
||||
public:
|
||||
BT_DECLARE_ALIGNED_ALLOCATOR();
|
||||
|
||||
btGeneric6DofSpringConstraint(btRigidBody& rbA, btRigidBody& rbB, const btTransform& frameInA, const btTransform& frameInB ,bool useLinearReferenceFrameA);
|
||||
btGeneric6DofSpringConstraint(btRigidBody& rbB, const btTransform& frameInB, bool useLinearReferenceFrameB);
|
||||
|
||||
btGeneric6DofSpringConstraint(btRigidBody & rbA, btRigidBody & rbB, const btTransform& frameInA, const btTransform& frameInB, bool useLinearReferenceFrameA);
|
||||
btGeneric6DofSpringConstraint(btRigidBody & rbB, const btTransform& frameInB, bool useLinearReferenceFrameB);
|
||||
void enableSpring(int index, bool onOff);
|
||||
void setStiffness(int index, btScalar stiffness);
|
||||
void setDamping(int index, btScalar damping);
|
||||
void setEquilibriumPoint(); // set the current constraint position/orientation as an equilibrium point for all DOF
|
||||
void setEquilibriumPoint(); // set the current constraint position/orientation as an equilibrium point for all DOF
|
||||
void setEquilibriumPoint(int index); // set the current constraint position/orientation as an equilibrium point for given DOF
|
||||
void setEquilibriumPoint(int index, btScalar val);
|
||||
|
||||
bool isSpringEnabled(int index) const
|
||||
{
|
||||
return m_springEnabled[index];
|
||||
return m_springEnabled[index];
|
||||
}
|
||||
|
||||
btScalar getStiffness(int index) const
|
||||
{
|
||||
return m_springStiffness[index];
|
||||
return m_springStiffness[index];
|
||||
}
|
||||
|
||||
btScalar getDamping(int index) const
|
||||
{
|
||||
return m_springDamping[index];
|
||||
return m_springDamping[index];
|
||||
}
|
||||
|
||||
btScalar getEquilibriumPoint(int index) const
|
||||
{
|
||||
return m_equilibriumPoint[index];
|
||||
return m_equilibriumPoint[index];
|
||||
}
|
||||
|
||||
virtual void setAxis( const btVector3& axis1, const btVector3& axis2);
|
||||
virtual void setAxis(const btVector3& axis1, const btVector3& axis2);
|
||||
|
||||
virtual void getInfo2 (btConstraintInfo2* info);
|
||||
virtual void getInfo2(btConstraintInfo2 * info);
|
||||
|
||||
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, btSerializer* serializer) const;
|
||||
|
||||
virtual const char* serialize(void* dataBuffer, btSerializer* serializer) const;
|
||||
};
|
||||
|
||||
|
||||
struct btGeneric6DofSpringConstraintData
|
||||
{
|
||||
btGeneric6DofConstraintData m_6dofData;
|
||||
|
||||
int m_springEnabled[6];
|
||||
float m_equilibriumPoint[6];
|
||||
float m_springStiffness[6];
|
||||
float m_springDamping[6];
|
||||
btGeneric6DofConstraintData m_6dofData;
|
||||
|
||||
int m_springEnabled[6];
|
||||
float m_equilibriumPoint[6];
|
||||
float m_springStiffness[6];
|
||||
float m_springDamping[6];
|
||||
};
|
||||
|
||||
struct btGeneric6DofSpringConstraintDoubleData2
|
||||
{
|
||||
btGeneric6DofConstraintDoubleData2 m_6dofData;
|
||||
|
||||
int m_springEnabled[6];
|
||||
double m_equilibriumPoint[6];
|
||||
double m_springStiffness[6];
|
||||
double m_springDamping[6];
|
||||
btGeneric6DofConstraintDoubleData2 m_6dofData;
|
||||
|
||||
int m_springEnabled[6];
|
||||
double m_equilibriumPoint[6];
|
||||
double m_springStiffness[6];
|
||||
double m_springDamping[6];
|
||||
};
|
||||
|
||||
|
||||
SIMD_FORCE_INLINE int btGeneric6DofSpringConstraint::calculateSerializeBufferSize() const
|
||||
SIMD_FORCE_INLINE int btGeneric6DofSpringConstraint::calculateSerializeBufferSize() const
|
||||
{
|
||||
return sizeof(btGeneric6DofSpringConstraintData2);
|
||||
}
|
||||
|
||||
///fills the dataBuffer and returns the struct name (and 0 on failure)
|
||||
SIMD_FORCE_INLINE const char* btGeneric6DofSpringConstraint::serialize(void* dataBuffer, btSerializer* serializer) const
|
||||
///fills the dataBuffer and returns the struct name (and 0 on failure)
|
||||
SIMD_FORCE_INLINE const char* btGeneric6DofSpringConstraint::serialize(void* dataBuffer, btSerializer* serializer) const
|
||||
{
|
||||
btGeneric6DofSpringConstraintData2* dof = (btGeneric6DofSpringConstraintData2*)dataBuffer;
|
||||
btGeneric6DofConstraint::serialize(&dof->m_6dofData,serializer);
|
||||
btGeneric6DofConstraint::serialize(&dof->m_6dofData, serializer);
|
||||
|
||||
int i;
|
||||
for (i=0;i<6;i++)
|
||||
for (i = 0; i < 6; i++)
|
||||
{
|
||||
dof->m_equilibriumPoint[i] = m_equilibriumPoint[i];
|
||||
dof->m_springDamping[i] = m_springDamping[i];
|
||||
dof->m_springEnabled[i] = m_springEnabled[i]? 1 : 0;
|
||||
dof->m_springEnabled[i] = m_springEnabled[i] ? 1 : 0;
|
||||
dof->m_springStiffness[i] = m_springStiffness[i];
|
||||
}
|
||||
return btGeneric6DofSpringConstraintDataName;
|
||||
}
|
||||
|
||||
#endif // BT_GENERIC_6DOF_SPRING_CONSTRAINT_H
|
||||
|
||||
#endif // BT_GENERIC_6DOF_SPRING_CONSTRAINT_H
|
||||
|
||||
@@ -13,54 +13,49 @@ subject to the following restrictions:
|
||||
3. This notice may not be removed or altered from any source distribution.
|
||||
*/
|
||||
|
||||
|
||||
|
||||
#include "btHinge2Constraint.h"
|
||||
#include "BulletDynamics/Dynamics/btRigidBody.h"
|
||||
#include "LinearMath/btTransformUtil.h"
|
||||
|
||||
|
||||
|
||||
// constructor
|
||||
// anchor, axis1 and axis2 are in world coordinate system
|
||||
// axis1 must be orthogonal to axis2
|
||||
btHinge2Constraint::btHinge2Constraint(btRigidBody& rbA, btRigidBody& rbB, btVector3& anchor, btVector3& axis1, btVector3& axis2)
|
||||
: btGeneric6DofSpring2Constraint(rbA, rbB, btTransform::getIdentity(), btTransform::getIdentity(),RO_XYZ),
|
||||
m_anchor(anchor),
|
||||
m_axis1(axis1),
|
||||
m_axis2(axis2)
|
||||
: btGeneric6DofSpring2Constraint(rbA, rbB, btTransform::getIdentity(), btTransform::getIdentity(), RO_XYZ),
|
||||
m_anchor(anchor),
|
||||
m_axis1(axis1),
|
||||
m_axis2(axis2)
|
||||
{
|
||||
// build frame basis
|
||||
// 6DOF constraint uses Euler angles and to define limits
|
||||
// it is assumed that rotational order is :
|
||||
// Z - first, allowed limits are (-PI,PI);
|
||||
// new position of Y - second (allowed limits are (-PI/2 + epsilon, PI/2 - epsilon), where epsilon is a small positive number
|
||||
// new position of Y - second (allowed limits are (-PI/2 + epsilon, PI/2 - epsilon), where epsilon is a small positive number
|
||||
// used to prevent constraint from instability on poles;
|
||||
// new position of X, allowed limits are (-PI,PI);
|
||||
// So to simulate ODE Universal joint we should use parent axis as Z, child axis as Y and limit all other DOFs
|
||||
// Build the frame in world coordinate system first
|
||||
btVector3 zAxis = axis1.normalize();
|
||||
btVector3 xAxis = axis2.normalize();
|
||||
btVector3 yAxis = zAxis.cross(xAxis); // we want right coordinate system
|
||||
btVector3 yAxis = zAxis.cross(xAxis); // we want right coordinate system
|
||||
btTransform 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]);
|
||||
frameInW.setOrigin(anchor);
|
||||
// now get constraint frame in local coordinate systems
|
||||
m_frameInA = rbA.getCenterOfMassTransform().inverse() * frameInW;
|
||||
m_frameInB = rbB.getCenterOfMassTransform().inverse() * frameInW;
|
||||
// sei limits
|
||||
setLinearLowerLimit(btVector3(0.f, 0.f, -1.f));
|
||||
setLinearUpperLimit(btVector3(0.f, 0.f, 1.f));
|
||||
setLinearUpperLimit(btVector3(0.f, 0.f, 1.f));
|
||||
// like front wheels of a car
|
||||
setAngularLowerLimit(btVector3(1.f, 0.f, -SIMD_HALF_PI * 0.5f));
|
||||
setAngularUpperLimit(btVector3(-1.f, 0.f, SIMD_HALF_PI * 0.5f));
|
||||
setAngularLowerLimit(btVector3(1.f, 0.f, -SIMD_HALF_PI * 0.5f));
|
||||
setAngularUpperLimit(btVector3(-1.f, 0.f, SIMD_HALF_PI * 0.5f));
|
||||
// enable suspension
|
||||
enableSpring(2, true);
|
||||
setStiffness(2, SIMD_PI * SIMD_PI * 4.f);
|
||||
setDamping(2, 0.01f);
|
||||
setEquilibriumPoint();
|
||||
}
|
||||
|
||||
|
||||
@@ -16,32 +16,30 @@ subject to the following restrictions:
|
||||
#ifndef BT_HINGE2_CONSTRAINT_H
|
||||
#define BT_HINGE2_CONSTRAINT_H
|
||||
|
||||
|
||||
|
||||
#include "LinearMath/btVector3.h"
|
||||
#include "btTypedConstraint.h"
|
||||
#include "btGeneric6DofSpring2Constraint.h"
|
||||
|
||||
|
||||
|
||||
// Constraint similar to ODE Hinge2 Joint
|
||||
// has 3 degrees of frredom:
|
||||
// 2 rotational degrees of freedom, similar to Euler rotations around Z (axis 1) and X (axis 2)
|
||||
// 1 translational (along axis Z) with suspension spring
|
||||
|
||||
ATTRIBUTE_ALIGNED16(class) btHinge2Constraint : public btGeneric6DofSpring2Constraint
|
||||
ATTRIBUTE_ALIGNED16(class)
|
||||
btHinge2Constraint : public btGeneric6DofSpring2Constraint
|
||||
{
|
||||
protected:
|
||||
btVector3 m_anchor;
|
||||
btVector3 m_axis1;
|
||||
btVector3 m_axis2;
|
||||
btVector3 m_anchor;
|
||||
btVector3 m_axis1;
|
||||
btVector3 m_axis2;
|
||||
|
||||
public:
|
||||
BT_DECLARE_ALIGNED_ALLOCATOR();
|
||||
|
||||
BT_DECLARE_ALIGNED_ALLOCATOR();
|
||||
|
||||
// constructor
|
||||
// anchor, axis1 and axis2 are in world coordinate system
|
||||
// axis1 must be orthogonal to axis2
|
||||
btHinge2Constraint(btRigidBody& rbA, btRigidBody& rbB, btVector3& anchor, btVector3& axis1, btVector3& axis2);
|
||||
btHinge2Constraint(btRigidBody & rbA, btRigidBody & rbB, btVector3 & anchor, btVector3 & axis1, btVector3 & axis2);
|
||||
// access
|
||||
const btVector3& getAnchor() { return m_calculatedTransformA.getOrigin(); }
|
||||
const btVector3& getAnchor2() { return m_calculatedTransformB.getOrigin(); }
|
||||
@@ -51,10 +49,7 @@ public:
|
||||
btScalar getAngle2() { return getAngle(0); }
|
||||
// limits
|
||||
void setUpperLimit(btScalar ang1max) { setAngularUpperLimit(btVector3(-1.f, 0.f, ang1max)); }
|
||||
void setLowerLimit(btScalar ang1min) { setAngularLowerLimit(btVector3( 1.f, 0.f, ang1min)); }
|
||||
void setLowerLimit(btScalar ang1min) { setAngularLowerLimit(btVector3(1.f, 0.f, ang1min)); }
|
||||
};
|
||||
|
||||
|
||||
|
||||
#endif // BT_HINGE2_CONSTRAINT_H
|
||||
|
||||
#endif // BT_HINGE2_CONSTRAINT_H
|
||||
|
||||
File diff suppressed because it is too large
Load Diff
@@ -20,7 +20,6 @@ subject to the following restrictions:
|
||||
|
||||
#define _BT_USE_CENTER_LIMIT_ 1
|
||||
|
||||
|
||||
#include "LinearMath/btVector3.h"
|
||||
#include "btJacobianEntry.h"
|
||||
#include "btTypedConstraint.h"
|
||||
@@ -28,14 +27,12 @@ subject to the following restrictions:
|
||||
class btRigidBody;
|
||||
|
||||
#ifdef BT_USE_DOUBLE_PRECISION
|
||||
#define btHingeConstraintData btHingeConstraintDoubleData2 //rename to 2 for backwards compatibility, so we can still load the 'btHingeConstraintDoubleData' version
|
||||
#define btHingeConstraintDataName "btHingeConstraintDoubleData2"
|
||||
#define btHingeConstraintData btHingeConstraintDoubleData2 //rename to 2 for backwards compatibility, so we can still load the 'btHingeConstraintDoubleData' version
|
||||
#define btHingeConstraintDataName "btHingeConstraintDoubleData2"
|
||||
#else
|
||||
#define btHingeConstraintData btHingeConstraintFloatData
|
||||
#define btHingeConstraintDataName "btHingeConstraintFloatData"
|
||||
#endif //BT_USE_DOUBLE_PRECISION
|
||||
|
||||
|
||||
#define btHingeConstraintData btHingeConstraintFloatData
|
||||
#define btHingeConstraintDataName "btHingeConstraintFloatData"
|
||||
#endif //BT_USE_DOUBLE_PRECISION
|
||||
|
||||
enum btHingeFlags
|
||||
{
|
||||
@@ -45,89 +42,83 @@ enum btHingeFlags
|
||||
BT_HINGE_FLAGS_ERP_NORM = 8
|
||||
};
|
||||
|
||||
|
||||
/// hinge constraint between two rigidbodies each with a pivotpoint that descibes the axis location in local space
|
||||
/// axis defines the orientation of the hinge axis
|
||||
ATTRIBUTE_ALIGNED16(class) btHingeConstraint : public btTypedConstraint
|
||||
ATTRIBUTE_ALIGNED16(class)
|
||||
btHingeConstraint : public btTypedConstraint
|
||||
{
|
||||
#ifdef IN_PARALLELL_SOLVER
|
||||
public:
|
||||
#endif
|
||||
btJacobianEntry m_jac[3]; //3 orthogonal linear constraints
|
||||
btJacobianEntry m_jacAng[3]; //2 orthogonal angular constraints+ 1 for limit/motor
|
||||
btJacobianEntry m_jac[3]; //3 orthogonal linear constraints
|
||||
btJacobianEntry m_jacAng[3]; //2 orthogonal angular constraints+ 1 for limit/motor
|
||||
|
||||
btTransform m_rbAFrame; // constraint axii. Assumes z is hinge axis.
|
||||
btTransform m_rbAFrame; // constraint axii. Assumes z is hinge axis.
|
||||
btTransform m_rbBFrame;
|
||||
|
||||
btScalar m_motorTargetVelocity;
|
||||
btScalar m_maxMotorImpulse;
|
||||
btScalar m_motorTargetVelocity;
|
||||
btScalar m_maxMotorImpulse;
|
||||
|
||||
|
||||
#ifdef _BT_USE_CENTER_LIMIT_
|
||||
btAngularLimit m_limit;
|
||||
#ifdef _BT_USE_CENTER_LIMIT_
|
||||
btAngularLimit m_limit;
|
||||
#else
|
||||
btScalar m_lowerLimit;
|
||||
btScalar m_upperLimit;
|
||||
btScalar m_limitSign;
|
||||
btScalar m_correction;
|
||||
btScalar m_lowerLimit;
|
||||
btScalar m_upperLimit;
|
||||
btScalar m_limitSign;
|
||||
btScalar m_correction;
|
||||
|
||||
btScalar m_limitSoftness;
|
||||
btScalar m_biasFactor;
|
||||
btScalar m_relaxationFactor;
|
||||
btScalar m_limitSoftness;
|
||||
btScalar m_biasFactor;
|
||||
btScalar m_relaxationFactor;
|
||||
|
||||
bool m_solveLimit;
|
||||
bool m_solveLimit;
|
||||
#endif
|
||||
|
||||
btScalar m_kHinge;
|
||||
btScalar m_kHinge;
|
||||
|
||||
btScalar m_accLimitImpulse;
|
||||
btScalar m_hingeAngle;
|
||||
btScalar m_referenceSign;
|
||||
|
||||
btScalar m_accLimitImpulse;
|
||||
btScalar m_hingeAngle;
|
||||
btScalar m_referenceSign;
|
||||
bool m_angularOnly;
|
||||
bool m_enableAngularMotor;
|
||||
bool m_useSolveConstraintObsolete;
|
||||
bool m_useOffsetForConstraintFrame;
|
||||
bool m_useReferenceFrameA;
|
||||
|
||||
bool m_angularOnly;
|
||||
bool m_enableAngularMotor;
|
||||
bool m_useSolveConstraintObsolete;
|
||||
bool m_useOffsetForConstraintFrame;
|
||||
bool m_useReferenceFrameA;
|
||||
btScalar m_accMotorImpulse;
|
||||
|
||||
btScalar m_accMotorImpulse;
|
||||
int m_flags;
|
||||
btScalar m_normalCFM;
|
||||
btScalar m_normalERP;
|
||||
btScalar m_stopCFM;
|
||||
btScalar m_stopERP;
|
||||
|
||||
int m_flags;
|
||||
btScalar m_normalCFM;
|
||||
btScalar m_normalERP;
|
||||
btScalar m_stopCFM;
|
||||
btScalar m_stopERP;
|
||||
|
||||
|
||||
public:
|
||||
|
||||
BT_DECLARE_ALIGNED_ALLOCATOR();
|
||||
|
||||
btHingeConstraint(btRigidBody& rbA,btRigidBody& rbB, const btVector3& pivotInA,const btVector3& pivotInB, const btVector3& axisInA,const btVector3& axisInB, bool useReferenceFrameA = false);
|
||||
|
||||
btHingeConstraint(btRigidBody& rbA,const btVector3& pivotInA,const btVector3& axisInA, bool useReferenceFrameA = false);
|
||||
|
||||
btHingeConstraint(btRigidBody& rbA,btRigidBody& rbB, const btTransform& rbAFrame, const btTransform& rbBFrame, bool useReferenceFrameA = false);
|
||||
btHingeConstraint(btRigidBody & rbA, btRigidBody & rbB, const btVector3& pivotInA, const btVector3& pivotInB, const btVector3& axisInA, const btVector3& axisInB, bool useReferenceFrameA = false);
|
||||
|
||||
btHingeConstraint(btRigidBody& rbA,const btTransform& rbAFrame, bool useReferenceFrameA = false);
|
||||
btHingeConstraint(btRigidBody & rbA, const btVector3& pivotInA, const btVector3& axisInA, bool useReferenceFrameA = false);
|
||||
|
||||
btHingeConstraint(btRigidBody & rbA, btRigidBody & rbB, const btTransform& rbAFrame, const btTransform& rbBFrame, bool useReferenceFrameA = false);
|
||||
|
||||
virtual void buildJacobian();
|
||||
btHingeConstraint(btRigidBody & rbA, const btTransform& rbAFrame, bool useReferenceFrameA = false);
|
||||
|
||||
virtual void getInfo1 (btConstraintInfo1* info);
|
||||
virtual void buildJacobian();
|
||||
|
||||
void getInfo1NonVirtual(btConstraintInfo1* info);
|
||||
virtual void getInfo1(btConstraintInfo1 * info);
|
||||
|
||||
virtual void getInfo2 (btConstraintInfo2* info);
|
||||
void getInfo1NonVirtual(btConstraintInfo1 * info);
|
||||
|
||||
void getInfo2NonVirtual(btConstraintInfo2* info,const btTransform& transA,const btTransform& transB,const btVector3& angVelA,const btVector3& angVelB);
|
||||
virtual void getInfo2(btConstraintInfo2 * info);
|
||||
|
||||
void getInfo2Internal(btConstraintInfo2* info,const btTransform& transA,const btTransform& transB,const btVector3& angVelA,const btVector3& angVelB);
|
||||
void getInfo2InternalUsingFrameOffset(btConstraintInfo2* info,const btTransform& transA,const btTransform& transB,const btVector3& angVelA,const btVector3& angVelB);
|
||||
|
||||
void getInfo2NonVirtual(btConstraintInfo2 * info, const btTransform& transA, const btTransform& transB, const btVector3& angVelA, const btVector3& angVelB);
|
||||
|
||||
void updateRHS(btScalar timeStep);
|
||||
void getInfo2Internal(btConstraintInfo2 * info, const btTransform& transA, const btTransform& transB, const btVector3& angVelA, const btVector3& angVelB);
|
||||
void getInfo2InternalUsingFrameOffset(btConstraintInfo2 * info, const btTransform& transA, const btTransform& transB, const btVector3& angVelA, const btVector3& angVelB);
|
||||
|
||||
void updateRHS(btScalar timeStep);
|
||||
|
||||
const btRigidBody& getRigidBodyA() const
|
||||
{
|
||||
@@ -138,19 +129,19 @@ public:
|
||||
return m_rbB;
|
||||
}
|
||||
|
||||
btRigidBody& getRigidBodyA()
|
||||
{
|
||||
return m_rbA;
|
||||
}
|
||||
btRigidBody& getRigidBodyA()
|
||||
{
|
||||
return m_rbA;
|
||||
}
|
||||
|
||||
btRigidBody& getRigidBodyB()
|
||||
{
|
||||
return m_rbB;
|
||||
btRigidBody& getRigidBodyB()
|
||||
{
|
||||
return m_rbB;
|
||||
}
|
||||
|
||||
btTransform& getFrameOffsetA()
|
||||
{
|
||||
return m_rbAFrame;
|
||||
return m_rbAFrame;
|
||||
}
|
||||
|
||||
btTransform& getFrameOffsetB()
|
||||
@@ -159,15 +150,15 @@ public:
|
||||
}
|
||||
|
||||
void setFrames(const btTransform& frameA, const btTransform& frameB);
|
||||
|
||||
void setAngularOnly(bool angularOnly)
|
||||
|
||||
void setAngularOnly(bool angularOnly)
|
||||
{
|
||||
m_angularOnly = angularOnly;
|
||||
}
|
||||
|
||||
void enableAngularMotor(bool enableMotor,btScalar targetVelocity,btScalar maxMotorImpulse)
|
||||
void enableAngularMotor(bool enableMotor, btScalar targetVelocity, btScalar maxMotorImpulse)
|
||||
{
|
||||
m_enableAngularMotor = enableMotor;
|
||||
m_enableAngularMotor = enableMotor;
|
||||
m_motorTargetVelocity = targetVelocity;
|
||||
m_maxMotorImpulse = maxMotorImpulse;
|
||||
}
|
||||
@@ -175,29 +166,28 @@ public:
|
||||
// extra motor API, including ability to set a target rotation (as opposed to angular velocity)
|
||||
// note: setMotorTarget sets angular velocity under the hood, so you must call it every tick to
|
||||
// maintain a given angular target.
|
||||
void enableMotor(bool enableMotor) { m_enableAngularMotor = enableMotor; }
|
||||
void enableMotor(bool enableMotor) { m_enableAngularMotor = enableMotor; }
|
||||
void setMaxMotorImpulse(btScalar maxMotorImpulse) { m_maxMotorImpulse = maxMotorImpulse; }
|
||||
void setMotorTargetVelocity(btScalar motorTargetVelocity) { m_motorTargetVelocity = motorTargetVelocity; }
|
||||
void setMotorTarget(const btQuaternion& qAinB, btScalar dt); // qAinB is rotation of body A wrt body B.
|
||||
void setMotorTarget(const btQuaternion& qAinB, btScalar dt); // qAinB is rotation of body A wrt body B.
|
||||
void setMotorTarget(btScalar targetAngle, btScalar dt);
|
||||
|
||||
|
||||
void setLimit(btScalar low,btScalar high,btScalar _softness = 0.9f, btScalar _biasFactor = 0.3f, btScalar _relaxationFactor = 1.0f)
|
||||
void setLimit(btScalar low, btScalar high, btScalar _softness = 0.9f, btScalar _biasFactor = 0.3f, btScalar _relaxationFactor = 1.0f)
|
||||
{
|
||||
#ifdef _BT_USE_CENTER_LIMIT_
|
||||
#ifdef _BT_USE_CENTER_LIMIT_
|
||||
m_limit.set(low, high, _softness, _biasFactor, _relaxationFactor);
|
||||
#else
|
||||
m_lowerLimit = btNormalizeAngle(low);
|
||||
m_upperLimit = btNormalizeAngle(high);
|
||||
m_limitSoftness = _softness;
|
||||
m_limitSoftness = _softness;
|
||||
m_biasFactor = _biasFactor;
|
||||
m_relaxationFactor = _relaxationFactor;
|
||||
#endif
|
||||
}
|
||||
|
||||
|
||||
btScalar getLimitSoftness() const
|
||||
{
|
||||
#ifdef _BT_USE_CENTER_LIMIT_
|
||||
#ifdef _BT_USE_CENTER_LIMIT_
|
||||
return m_limit.getSoftness();
|
||||
#else
|
||||
return m_limitSoftness;
|
||||
@@ -206,7 +196,7 @@ public:
|
||||
|
||||
btScalar getLimitBiasFactor() const
|
||||
{
|
||||
#ifdef _BT_USE_CENTER_LIMIT_
|
||||
#ifdef _BT_USE_CENTER_LIMIT_
|
||||
return m_limit.getBiasFactor();
|
||||
#else
|
||||
return m_biasFactor;
|
||||
@@ -215,112 +205,110 @@ public:
|
||||
|
||||
btScalar getLimitRelaxationFactor() const
|
||||
{
|
||||
#ifdef _BT_USE_CENTER_LIMIT_
|
||||
#ifdef _BT_USE_CENTER_LIMIT_
|
||||
return m_limit.getRelaxationFactor();
|
||||
#else
|
||||
return m_relaxationFactor;
|
||||
#endif
|
||||
}
|
||||
|
||||
void setAxis(btVector3& axisInA)
|
||||
void setAxis(btVector3 & axisInA)
|
||||
{
|
||||
btVector3 rbAxisA1, rbAxisA2;
|
||||
btPlaneSpace1(axisInA, rbAxisA1, rbAxisA2);
|
||||
btVector3 pivotInA = m_rbAFrame.getOrigin();
|
||||
// m_rbAFrame.getOrigin() = pivotInA;
|
||||
m_rbAFrame.getBasis().setValue( rbAxisA1.getX(),rbAxisA2.getX(),axisInA.getX(),
|
||||
rbAxisA1.getY(),rbAxisA2.getY(),axisInA.getY(),
|
||||
rbAxisA1.getZ(),rbAxisA2.getZ(),axisInA.getZ() );
|
||||
// m_rbAFrame.getOrigin() = pivotInA;
|
||||
m_rbAFrame.getBasis().setValue(rbAxisA1.getX(), rbAxisA2.getX(), axisInA.getX(),
|
||||
rbAxisA1.getY(), rbAxisA2.getY(), axisInA.getY(),
|
||||
rbAxisA1.getZ(), rbAxisA2.getZ(), axisInA.getZ());
|
||||
|
||||
btVector3 axisInB = m_rbA.getCenterOfMassTransform().getBasis() * axisInA;
|
||||
|
||||
btQuaternion rotationArc = shortestArcQuat(axisInA,axisInB);
|
||||
btVector3 rbAxisB1 = quatRotate(rotationArc,rbAxisA1);
|
||||
btQuaternion rotationArc = shortestArcQuat(axisInA, axisInB);
|
||||
btVector3 rbAxisB1 = quatRotate(rotationArc, rbAxisA1);
|
||||
btVector3 rbAxisB2 = axisInB.cross(rbAxisB1);
|
||||
|
||||
m_rbBFrame.getOrigin() = m_rbB.getCenterOfMassTransform().inverse()(m_rbA.getCenterOfMassTransform()(pivotInA));
|
||||
|
||||
m_rbBFrame.getBasis().setValue( rbAxisB1.getX(),rbAxisB2.getX(),axisInB.getX(),
|
||||
rbAxisB1.getY(),rbAxisB2.getY(),axisInB.getY(),
|
||||
rbAxisB1.getZ(),rbAxisB2.getZ(),axisInB.getZ() );
|
||||
m_rbBFrame.getBasis().setValue(rbAxisB1.getX(), rbAxisB2.getX(), axisInB.getX(),
|
||||
rbAxisB1.getY(), rbAxisB2.getY(), axisInB.getY(),
|
||||
rbAxisB1.getZ(), rbAxisB2.getZ(), axisInB.getZ());
|
||||
m_rbBFrame.getBasis() = m_rbB.getCenterOfMassTransform().getBasis().inverse() * m_rbBFrame.getBasis();
|
||||
|
||||
}
|
||||
|
||||
bool hasLimit() const {
|
||||
#ifdef _BT_USE_CENTER_LIMIT_
|
||||
return m_limit.getHalfRange() > 0;
|
||||
#else
|
||||
return m_lowerLimit <= m_upperLimit;
|
||||
#endif
|
||||
}
|
||||
|
||||
btScalar getLowerLimit() const
|
||||
bool hasLimit() const
|
||||
{
|
||||
#ifdef _BT_USE_CENTER_LIMIT_
|
||||
return m_limit.getLow();
|
||||
#ifdef _BT_USE_CENTER_LIMIT_
|
||||
return m_limit.getHalfRange() > 0;
|
||||
#else
|
||||
return m_lowerLimit;
|
||||
return m_lowerLimit <= m_upperLimit;
|
||||
#endif
|
||||
}
|
||||
|
||||
btScalar getUpperLimit() const
|
||||
btScalar getLowerLimit() const
|
||||
{
|
||||
#ifdef _BT_USE_CENTER_LIMIT_
|
||||
return m_limit.getHigh();
|
||||
#else
|
||||
return m_upperLimit;
|
||||
#ifdef _BT_USE_CENTER_LIMIT_
|
||||
return m_limit.getLow();
|
||||
#else
|
||||
return m_lowerLimit;
|
||||
#endif
|
||||
}
|
||||
|
||||
btScalar getUpperLimit() const
|
||||
{
|
||||
#ifdef _BT_USE_CENTER_LIMIT_
|
||||
return m_limit.getHigh();
|
||||
#else
|
||||
return m_upperLimit;
|
||||
#endif
|
||||
}
|
||||
|
||||
///The getHingeAngle gives the hinge angle in range [-PI,PI]
|
||||
btScalar getHingeAngle();
|
||||
|
||||
btScalar getHingeAngle(const btTransform& transA,const btTransform& transB);
|
||||
btScalar getHingeAngle(const btTransform& transA, const btTransform& transB);
|
||||
|
||||
void testLimit(const btTransform& transA,const btTransform& transB);
|
||||
void testLimit(const btTransform& transA, const btTransform& transB);
|
||||
|
||||
|
||||
const btTransform& getAFrame() const { return m_rbAFrame; };
|
||||
const btTransform& getAFrame() const { return m_rbAFrame; };
|
||||
const btTransform& getBFrame() const { return m_rbBFrame; };
|
||||
|
||||
btTransform& getAFrame() { return m_rbAFrame; };
|
||||
btTransform& getAFrame() { return m_rbAFrame; };
|
||||
btTransform& getBFrame() { return m_rbBFrame; };
|
||||
|
||||
inline int getSolveLimit()
|
||||
{
|
||||
#ifdef _BT_USE_CENTER_LIMIT_
|
||||
return m_limit.isLimit();
|
||||
#ifdef _BT_USE_CENTER_LIMIT_
|
||||
return m_limit.isLimit();
|
||||
#else
|
||||
return m_solveLimit;
|
||||
return m_solveLimit;
|
||||
#endif
|
||||
}
|
||||
|
||||
inline btScalar getLimitSign()
|
||||
{
|
||||
#ifdef _BT_USE_CENTER_LIMIT_
|
||||
return m_limit.getSign();
|
||||
#ifdef _BT_USE_CENTER_LIMIT_
|
||||
return m_limit.getSign();
|
||||
#else
|
||||
return m_limitSign;
|
||||
#endif
|
||||
}
|
||||
|
||||
inline bool getAngularOnly()
|
||||
{
|
||||
return m_angularOnly;
|
||||
inline bool getAngularOnly()
|
||||
{
|
||||
return m_angularOnly;
|
||||
}
|
||||
inline bool getEnableAngularMotor()
|
||||
{
|
||||
return m_enableAngularMotor;
|
||||
inline bool getEnableAngularMotor()
|
||||
{
|
||||
return m_enableAngularMotor;
|
||||
}
|
||||
inline btScalar getMotorTargetVelocity()
|
||||
{
|
||||
return m_motorTargetVelocity;
|
||||
inline btScalar getMotorTargetVelocity()
|
||||
{
|
||||
return m_motorTargetVelocity;
|
||||
}
|
||||
inline btScalar getMaxMotorImpulse()
|
||||
{
|
||||
return m_maxMotorImpulse;
|
||||
inline btScalar getMaxMotorImpulse()
|
||||
{
|
||||
return m_maxMotorImpulse;
|
||||
}
|
||||
// access for UseFrameOffset
|
||||
bool getUseFrameOffset() { return m_useOffsetForConstraintFrame; }
|
||||
@@ -329,143 +317,132 @@ public:
|
||||
bool getUseReferenceFrameA() const { return m_useReferenceFrameA; }
|
||||
void setUseReferenceFrameA(bool useReferenceFrameA) { m_useReferenceFrameA = useReferenceFrameA; }
|
||||
|
||||
///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, btScalar value, int axis = -1);
|
||||
virtual void setParam(int num, btScalar value, int axis = -1);
|
||||
///return the local value of parameter
|
||||
virtual btScalar getParam(int num, int axis = -1) const;
|
||||
|
||||
virtual int getFlags() const
|
||||
virtual btScalar getParam(int num, int axis = -1) const;
|
||||
|
||||
virtual int getFlags() const
|
||||
{
|
||||
return m_flags;
|
||||
return m_flags;
|
||||
}
|
||||
|
||||
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, btSerializer* serializer) const;
|
||||
|
||||
|
||||
virtual const char* serialize(void* dataBuffer, btSerializer* serializer) const;
|
||||
};
|
||||
|
||||
|
||||
//only for backward compatibility
|
||||
#ifdef BT_BACKWARDS_COMPATIBLE_SERIALIZATION
|
||||
///this structure is not used, except for loading pre-2.82 .bullet files
|
||||
struct btHingeConstraintDoubleData
|
||||
struct btHingeConstraintDoubleData
|
||||
{
|
||||
btTypedConstraintData m_typeConstraintData;
|
||||
btTransformDoubleData m_rbAFrame; // constraint axii. Assumes z is hinge axis.
|
||||
btTypedConstraintData m_typeConstraintData;
|
||||
btTransformDoubleData m_rbAFrame; // constraint axii. Assumes z is hinge axis.
|
||||
btTransformDoubleData m_rbBFrame;
|
||||
int m_useReferenceFrameA;
|
||||
int m_angularOnly;
|
||||
int m_enableAngularMotor;
|
||||
float m_motorTargetVelocity;
|
||||
float m_maxMotorImpulse;
|
||||
|
||||
float m_lowerLimit;
|
||||
float m_upperLimit;
|
||||
float m_limitSoftness;
|
||||
float m_biasFactor;
|
||||
float m_relaxationFactor;
|
||||
int m_useReferenceFrameA;
|
||||
int m_angularOnly;
|
||||
int m_enableAngularMotor;
|
||||
float m_motorTargetVelocity;
|
||||
float m_maxMotorImpulse;
|
||||
|
||||
float m_lowerLimit;
|
||||
float m_upperLimit;
|
||||
float m_limitSoftness;
|
||||
float m_biasFactor;
|
||||
float m_relaxationFactor;
|
||||
};
|
||||
#endif //BT_BACKWARDS_COMPATIBLE_SERIALIZATION
|
||||
#endif //BT_BACKWARDS_COMPATIBLE_SERIALIZATION
|
||||
|
||||
///The getAccumulatedHingeAngle returns the accumulated hinge angle, taking rotation across the -PI/PI boundary into account
|
||||
ATTRIBUTE_ALIGNED16(class) btHingeAccumulatedAngleConstraint : public btHingeConstraint
|
||||
ATTRIBUTE_ALIGNED16(class)
|
||||
btHingeAccumulatedAngleConstraint : public btHingeConstraint
|
||||
{
|
||||
protected:
|
||||
btScalar m_accumulatedAngle;
|
||||
btScalar m_accumulatedAngle;
|
||||
|
||||
public:
|
||||
|
||||
BT_DECLARE_ALIGNED_ALLOCATOR();
|
||||
|
||||
btHingeAccumulatedAngleConstraint(btRigidBody& rbA,btRigidBody& rbB, const btVector3& pivotInA,const btVector3& pivotInB, const btVector3& axisInA,const btVector3& axisInB, bool useReferenceFrameA = false)
|
||||
:btHingeConstraint(rbA,rbB,pivotInA,pivotInB, axisInA,axisInB, useReferenceFrameA )
|
||||
|
||||
btHingeAccumulatedAngleConstraint(btRigidBody & rbA, btRigidBody & rbB, const btVector3& pivotInA, const btVector3& pivotInB, const btVector3& axisInA, const btVector3& axisInB, bool useReferenceFrameA = false)
|
||||
: btHingeConstraint(rbA, rbB, pivotInA, pivotInB, axisInA, axisInB, useReferenceFrameA)
|
||||
{
|
||||
m_accumulatedAngle=getHingeAngle();
|
||||
m_accumulatedAngle = getHingeAngle();
|
||||
}
|
||||
|
||||
btHingeAccumulatedAngleConstraint(btRigidBody& rbA,const btVector3& pivotInA,const btVector3& axisInA, bool useReferenceFrameA = false)
|
||||
:btHingeConstraint(rbA,pivotInA,axisInA, useReferenceFrameA)
|
||||
btHingeAccumulatedAngleConstraint(btRigidBody & rbA, const btVector3& pivotInA, const btVector3& axisInA, bool useReferenceFrameA = false)
|
||||
: btHingeConstraint(rbA, pivotInA, axisInA, useReferenceFrameA)
|
||||
{
|
||||
m_accumulatedAngle=getHingeAngle();
|
||||
}
|
||||
|
||||
btHingeAccumulatedAngleConstraint(btRigidBody& rbA,btRigidBody& rbB, const btTransform& rbAFrame, const btTransform& rbBFrame, bool useReferenceFrameA = false)
|
||||
:btHingeConstraint(rbA,rbB, rbAFrame, rbBFrame, useReferenceFrameA )
|
||||
{
|
||||
m_accumulatedAngle=getHingeAngle();
|
||||
m_accumulatedAngle = getHingeAngle();
|
||||
}
|
||||
|
||||
btHingeAccumulatedAngleConstraint(btRigidBody& rbA,const btTransform& rbAFrame, bool useReferenceFrameA = false)
|
||||
:btHingeConstraint(rbA,rbAFrame, useReferenceFrameA )
|
||||
btHingeAccumulatedAngleConstraint(btRigidBody & rbA, btRigidBody & rbB, const btTransform& rbAFrame, const btTransform& rbBFrame, bool useReferenceFrameA = false)
|
||||
: btHingeConstraint(rbA, rbB, rbAFrame, rbBFrame, useReferenceFrameA)
|
||||
{
|
||||
m_accumulatedAngle=getHingeAngle();
|
||||
m_accumulatedAngle = getHingeAngle();
|
||||
}
|
||||
|
||||
btHingeAccumulatedAngleConstraint(btRigidBody & rbA, const btTransform& rbAFrame, bool useReferenceFrameA = false)
|
||||
: btHingeConstraint(rbA, rbAFrame, useReferenceFrameA)
|
||||
{
|
||||
m_accumulatedAngle = getHingeAngle();
|
||||
}
|
||||
btScalar getAccumulatedHingeAngle();
|
||||
void setAccumulatedHingeAngle(btScalar accAngle);
|
||||
virtual void getInfo1 (btConstraintInfo1* info);
|
||||
|
||||
void setAccumulatedHingeAngle(btScalar accAngle);
|
||||
virtual void getInfo1(btConstraintInfo1 * info);
|
||||
};
|
||||
|
||||
struct btHingeConstraintFloatData
|
||||
struct btHingeConstraintFloatData
|
||||
{
|
||||
btTypedConstraintData m_typeConstraintData;
|
||||
btTransformFloatData m_rbAFrame; // constraint axii. Assumes z is hinge axis.
|
||||
btTypedConstraintData m_typeConstraintData;
|
||||
btTransformFloatData m_rbAFrame; // constraint axii. Assumes z is hinge axis.
|
||||
btTransformFloatData m_rbBFrame;
|
||||
int m_useReferenceFrameA;
|
||||
int m_angularOnly;
|
||||
|
||||
int m_enableAngularMotor;
|
||||
float m_motorTargetVelocity;
|
||||
float m_maxMotorImpulse;
|
||||
int m_useReferenceFrameA;
|
||||
int m_angularOnly;
|
||||
|
||||
float m_lowerLimit;
|
||||
float m_upperLimit;
|
||||
float m_limitSoftness;
|
||||
float m_biasFactor;
|
||||
float m_relaxationFactor;
|
||||
int m_enableAngularMotor;
|
||||
float m_motorTargetVelocity;
|
||||
float m_maxMotorImpulse;
|
||||
|
||||
float m_lowerLimit;
|
||||
float m_upperLimit;
|
||||
float m_limitSoftness;
|
||||
float m_biasFactor;
|
||||
float m_relaxationFactor;
|
||||
};
|
||||
|
||||
|
||||
|
||||
///do not change those serialization structures, it requires an updated sBulletDNAstr/sBulletDNAstr64
|
||||
struct btHingeConstraintDoubleData2
|
||||
struct btHingeConstraintDoubleData2
|
||||
{
|
||||
btTypedConstraintDoubleData m_typeConstraintData;
|
||||
btTransformDoubleData m_rbAFrame; // constraint axii. Assumes z is hinge axis.
|
||||
btTypedConstraintDoubleData m_typeConstraintData;
|
||||
btTransformDoubleData m_rbAFrame; // constraint axii. Assumes z is hinge axis.
|
||||
btTransformDoubleData m_rbBFrame;
|
||||
int m_useReferenceFrameA;
|
||||
int m_angularOnly;
|
||||
int m_enableAngularMotor;
|
||||
double m_motorTargetVelocity;
|
||||
double m_maxMotorImpulse;
|
||||
|
||||
double m_lowerLimit;
|
||||
double m_upperLimit;
|
||||
double m_limitSoftness;
|
||||
double m_biasFactor;
|
||||
double m_relaxationFactor;
|
||||
char m_padding1[4];
|
||||
int m_useReferenceFrameA;
|
||||
int m_angularOnly;
|
||||
int m_enableAngularMotor;
|
||||
double m_motorTargetVelocity;
|
||||
double m_maxMotorImpulse;
|
||||
|
||||
double m_lowerLimit;
|
||||
double m_upperLimit;
|
||||
double m_limitSoftness;
|
||||
double m_biasFactor;
|
||||
double m_relaxationFactor;
|
||||
char m_padding1[4];
|
||||
};
|
||||
|
||||
|
||||
|
||||
|
||||
SIMD_FORCE_INLINE int btHingeConstraint::calculateSerializeBufferSize() const
|
||||
SIMD_FORCE_INLINE int btHingeConstraint::calculateSerializeBufferSize() const
|
||||
{
|
||||
return sizeof(btHingeConstraintData);
|
||||
}
|
||||
|
||||
///fills the dataBuffer and returns the struct name (and 0 on failure)
|
||||
SIMD_FORCE_INLINE const char* btHingeConstraint::serialize(void* dataBuffer, btSerializer* serializer) const
|
||||
///fills the dataBuffer and returns the struct name (and 0 on failure)
|
||||
SIMD_FORCE_INLINE const char* btHingeConstraint::serialize(void* dataBuffer, btSerializer* serializer) const
|
||||
{
|
||||
btHingeConstraintData* hingeData = (btHingeConstraintData*)dataBuffer;
|
||||
btTypedConstraint::serialize(&hingeData->m_typeConstraintData,serializer);
|
||||
btTypedConstraint::serialize(&hingeData->m_typeConstraintData, serializer);
|
||||
|
||||
m_rbAFrame.serialize(hingeData->m_rbAFrame);
|
||||
m_rbBFrame.serialize(hingeData->m_rbBFrame);
|
||||
@@ -475,7 +452,7 @@ SIMD_FORCE_INLINE const char* btHingeConstraint::serialize(void* dataBuffer, btS
|
||||
hingeData->m_maxMotorImpulse = float(m_maxMotorImpulse);
|
||||
hingeData->m_motorTargetVelocity = float(m_motorTargetVelocity);
|
||||
hingeData->m_useReferenceFrameA = m_useReferenceFrameA;
|
||||
#ifdef _BT_USE_CENTER_LIMIT_
|
||||
#ifdef _BT_USE_CENTER_LIMIT_
|
||||
hingeData->m_lowerLimit = float(m_limit.getLow());
|
||||
hingeData->m_upperLimit = float(m_limit.getHigh());
|
||||
hingeData->m_limitSoftness = float(m_limit.getSoftness());
|
||||
@@ -500,4 +477,4 @@ SIMD_FORCE_INLINE const char* btHingeConstraint::serialize(void* dataBuffer, btS
|
||||
return btHingeConstraintDataName;
|
||||
}
|
||||
|
||||
#endif //BT_HINGECONSTRAINT_H
|
||||
#endif //BT_HINGECONSTRAINT_H
|
||||
|
||||
@@ -18,7 +18,6 @@ subject to the following restrictions:
|
||||
|
||||
#include "LinearMath/btMatrix3x3.h"
|
||||
|
||||
|
||||
//notes:
|
||||
// Another memory optimization would be to store m_1MinvJt in the remaining 3 w components
|
||||
// which makes the btJacobianEntry 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
|
||||
ATTRIBUTE_ALIGNED16(class) btJacobianEntry
|
||||
ATTRIBUTE_ALIGNED16(class)
|
||||
btJacobianEntry
|
||||
{
|
||||
public:
|
||||
btJacobianEntry() {};
|
||||
btJacobianEntry(){};
|
||||
//constraint between two different rigidbodies
|
||||
btJacobianEntry(
|
||||
const btMatrix3x3& world2A,
|
||||
const btMatrix3x3& world2B,
|
||||
const btVector3& rel_pos1,const btVector3& rel_pos2,
|
||||
const btVector3& rel_pos1, const btVector3& rel_pos2,
|
||||
const btVector3& jointAxis,
|
||||
const btVector3& inertiaInvA,
|
||||
const btVector3& inertiaInvA,
|
||||
const btScalar massInvA,
|
||||
const btVector3& inertiaInvB,
|
||||
const btScalar 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
|
||||
btJacobianEntry(const btVector3& jointAxis,
|
||||
const btMatrix3x3& world2A,
|
||||
const btMatrix3x3& world2B,
|
||||
const btVector3& inertiaInvA,
|
||||
const btVector3& inertiaInvB)
|
||||
:m_linearJointAxis(btVector3(btScalar(0.),btScalar(0.),btScalar(0.)))
|
||||
const btMatrix3x3& world2A,
|
||||
const btMatrix3x3& world2B,
|
||||
const btVector3& inertiaInvA,
|
||||
const btVector3& inertiaInvB)
|
||||
: m_linearJointAxis(btVector3(btScalar(0.), btScalar(0.), btScalar(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);
|
||||
|
||||
btAssert(m_Adiag > btScalar(0.0));
|
||||
}
|
||||
|
||||
//angular constraint between two different rigidbodies
|
||||
btJacobianEntry(const btVector3& axisInA,
|
||||
const btVector3& axisInB,
|
||||
const btVector3& inertiaInvA,
|
||||
const btVector3& inertiaInvB)
|
||||
: m_linearJointAxis(btVector3(btScalar(0.),btScalar(0.),btScalar(0.)))
|
||||
, m_aJ(axisInA)
|
||||
, m_bJ(-axisInB)
|
||||
const btVector3& axisInB,
|
||||
const btVector3& inertiaInvA,
|
||||
const btVector3& inertiaInvB)
|
||||
: m_linearJointAxis(btVector3(btScalar(0.), btScalar(0.), btScalar(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);
|
||||
|
||||
btAssert(m_Adiag > btScalar(0.0));
|
||||
}
|
||||
@@ -88,25 +86,25 @@ public:
|
||||
//constraint on one rigidbody
|
||||
btJacobianEntry(
|
||||
const btMatrix3x3& world2A,
|
||||
const btVector3& rel_pos1,const btVector3& rel_pos2,
|
||||
const btVector3& rel_pos1, const btVector3& rel_pos2,
|
||||
const btVector3& jointAxis,
|
||||
const btVector3& inertiaInvA,
|
||||
const btVector3& inertiaInvA,
|
||||
const btScalar 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 = btVector3(btScalar(0.),btScalar(0.),btScalar(0.));
|
||||
m_aJ = world2A * (rel_pos1.cross(jointAxis));
|
||||
m_bJ = world2A * (rel_pos2.cross(-jointAxis));
|
||||
m_0MinvJt = inertiaInvA * m_aJ;
|
||||
m_1MinvJt = btVector3(btScalar(0.), btScalar(0.), btScalar(0.));
|
||||
m_Adiag = massInvA + m_0MinvJt.dot(m_aJ);
|
||||
|
||||
btAssert(m_Adiag > btScalar(0.0));
|
||||
}
|
||||
|
||||
btScalar getDiagonal() const { return m_Adiag; }
|
||||
btScalar getDiagonal() const { return m_Adiag; }
|
||||
|
||||
// for two constraints on the same rigidbody (for example vehicle friction)
|
||||
btScalar getNonDiagonal(const btJacobianEntry& jacB, const btScalar massInvA) const
|
||||
btScalar getNonDiagonal(const btJacobianEntry& jacB, const btScalar massInvA) const
|
||||
{
|
||||
const btJacobianEntry& jacA = *this;
|
||||
btScalar 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)
|
||||
btScalar getNonDiagonal(const btJacobianEntry& jacB,const btScalar massInvA,const btScalar massInvB) const
|
||||
btScalar getNonDiagonal(const btJacobianEntry& jacB, const btScalar massInvA, const btScalar massInvB) const
|
||||
{
|
||||
const btJacobianEntry& jacA = *this;
|
||||
btVector3 lin = jacA.m_linearJointAxis * jacB.m_linearJointAxis;
|
||||
btVector3 ang0 = jacA.m_0MinvJt * jacB.m_aJ;
|
||||
btVector3 ang1 = jacA.m_1MinvJt * jacB.m_bJ;
|
||||
btVector3 lin0 = massInvA * lin ;
|
||||
btVector3 lin0 = massInvA * lin;
|
||||
btVector3 lin1 = massInvB * lin;
|
||||
btVector3 sum = ang0+ang1+lin0+lin1;
|
||||
return sum[0]+sum[1]+sum[2];
|
||||
btVector3 sum = ang0 + ang1 + lin0 + lin1;
|
||||
return sum[0] + sum[1] + sum[2];
|
||||
}
|
||||
|
||||
btScalar getRelativeVelocity(const btVector3& linvelA,const btVector3& angvelA,const btVector3& linvelB,const btVector3& angvelB)
|
||||
btScalar getRelativeVelocity(const btVector3& linvelA, const btVector3& angvelA, const btVector3& linvelB, const btVector3& angvelB)
|
||||
{
|
||||
btVector3 linrel = linvelA - linvelB;
|
||||
btVector3 angvela = angvelA * m_aJ;
|
||||
btVector3 angvelb = angvelB * m_bJ;
|
||||
btVector3 angvela = angvelA * m_aJ;
|
||||
btVector3 angvelb = angvelB * m_bJ;
|
||||
linrel *= m_linearJointAxis;
|
||||
angvela += angvelb;
|
||||
angvela += linrel;
|
||||
btScalar rel_vel2 = angvela[0]+angvela[1]+angvela[2];
|
||||
btScalar rel_vel2 = angvela[0] + angvela[1] + angvela[2];
|
||||
return rel_vel2 + SIMD_EPSILON;
|
||||
}
|
||||
//private:
|
||||
//private:
|
||||
|
||||
btVector3 m_linearJointAxis;
|
||||
btVector3 m_aJ;
|
||||
btVector3 m_bJ;
|
||||
btVector3 m_0MinvJt;
|
||||
btVector3 m_1MinvJt;
|
||||
btVector3 m_linearJointAxis;
|
||||
btVector3 m_aJ;
|
||||
btVector3 m_bJ;
|
||||
btVector3 m_0MinvJt;
|
||||
btVector3 m_1MinvJt;
|
||||
//Optimization: can be stored in the w/last component of one of the vectors
|
||||
btScalar m_Adiag;
|
||||
|
||||
btScalar m_Adiag;
|
||||
};
|
||||
|
||||
#endif //BT_JACOBIAN_ENTRY_H
|
||||
#endif //BT_JACOBIAN_ENTRY_H
|
||||
|
||||
@@ -15,14 +15,9 @@ subject to the following restrictions:
|
||||
|
||||
#include "btNNCGConstraintSolver.h"
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
btScalar btNNCGConstraintSolver::solveGroupCacheFriendlySetup(btCollisionObject** bodies,int numBodies,btPersistentManifold** manifoldPtr, int numManifolds,btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& infoGlobal,btIDebugDraw* debugDrawer)
|
||||
btScalar btNNCGConstraintSolver::solveGroupCacheFriendlySetup(btCollisionObject** bodies, int numBodies, btPersistentManifold** manifoldPtr, int numManifolds, btTypedConstraint** constraints, int numConstraints, const btContactSolverInfo& infoGlobal, btIDebugDraw* debugDrawer)
|
||||
{
|
||||
btScalar val = btSequentialImpulseConstraintSolver::solveGroupCacheFriendlySetup( bodies,numBodies,manifoldPtr, numManifolds, constraints,numConstraints,infoGlobal,debugDrawer);
|
||||
btScalar val = btSequentialImpulseConstraintSolver::solveGroupCacheFriendlySetup(bodies, numBodies, manifoldPtr, numManifolds, constraints, numConstraints, infoGlobal, debugDrawer);
|
||||
|
||||
m_pNC.resizeNoInitialize(m_tmpSolverNonContactConstraintPool.size());
|
||||
m_pC.resizeNoInitialize(m_tmpSolverContactConstraintPool.size());
|
||||
@@ -37,38 +32,39 @@ btScalar btNNCGConstraintSolver::solveGroupCacheFriendlySetup(btCollisionObject*
|
||||
return val;
|
||||
}
|
||||
|
||||
btScalar btNNCGConstraintSolver::solveSingleIteration(int iteration, btCollisionObject** /*bodies */,int /*numBodies*/,btPersistentManifold** /*manifoldPtr*/, int /*numManifolds*/,btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& infoGlobal,btIDebugDraw* /*debugDrawer*/)
|
||||
btScalar btNNCGConstraintSolver::solveSingleIteration(int iteration, btCollisionObject** /*bodies */, int /*numBodies*/, btPersistentManifold** /*manifoldPtr*/, int /*numManifolds*/, btTypedConstraint** constraints, int numConstraints, const btContactSolverInfo& infoGlobal, btIDebugDraw* /*debugDrawer*/)
|
||||
{
|
||||
|
||||
int numNonContactPool = m_tmpSolverNonContactConstraintPool.size();
|
||||
int numConstraintPool = m_tmpSolverContactConstraintPool.size();
|
||||
int numFrictionPool = m_tmpSolverContactFrictionConstraintPool.size();
|
||||
|
||||
if (infoGlobal.m_solverMode & SOLVER_RANDMIZE_ORDER)
|
||||
{
|
||||
if (1) // uncomment this for a bit less random ((iteration & 7) == 0)
|
||||
if (1) // uncomment this for a bit less random ((iteration & 7) == 0)
|
||||
{
|
||||
|
||||
for (int j=0; j<numNonContactPool; ++j) {
|
||||
for (int j = 0; j < numNonContactPool; ++j)
|
||||
{
|
||||
int tmp = m_orderNonContactConstraintPool[j];
|
||||
int swapi = btRandInt2(j+1);
|
||||
int swapi = btRandInt2(j + 1);
|
||||
m_orderNonContactConstraintPool[j] = m_orderNonContactConstraintPool[swapi];
|
||||
m_orderNonContactConstraintPool[swapi] = tmp;
|
||||
}
|
||||
|
||||
//contact/friction constraints are not solved more than
|
||||
if (iteration< infoGlobal.m_numIterations)
|
||||
//contact/friction constraints are not solved more than
|
||||
if (iteration < infoGlobal.m_numIterations)
|
||||
{
|
||||
for (int j=0; j<numConstraintPool; ++j) {
|
||||
for (int j = 0; j < numConstraintPool; ++j)
|
||||
{
|
||||
int tmp = m_orderTmpConstraintPool[j];
|
||||
int swapi = btRandInt2(j+1);
|
||||
int swapi = btRandInt2(j + 1);
|
||||
m_orderTmpConstraintPool[j] = m_orderTmpConstraintPool[swapi];
|
||||
m_orderTmpConstraintPool[swapi] = tmp;
|
||||
}
|
||||
|
||||
for (int j=0; j<numFrictionPool; ++j) {
|
||||
for (int j = 0; j < numFrictionPool; ++j)
|
||||
{
|
||||
int tmp = m_orderFrictionConstraintPool[j];
|
||||
int swapi = btRandInt2(j+1);
|
||||
int swapi = btRandInt2(j + 1);
|
||||
m_orderFrictionConstraintPool[j] = m_orderFrictionConstraintPool[swapi];
|
||||
m_orderFrictionConstraintPool[swapi] = tmp;
|
||||
}
|
||||
@@ -76,39 +72,40 @@ btScalar btNNCGConstraintSolver::solveSingleIteration(int iteration, btCollision
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
btScalar deltaflengthsqr = 0;
|
||||
{
|
||||
for (int j=0;j<m_tmpSolverNonContactConstraintPool.size();j++)
|
||||
for (int j = 0; j < m_tmpSolverNonContactConstraintPool.size(); j++)
|
||||
{
|
||||
btSolverConstraint& constraint = m_tmpSolverNonContactConstraintPool[m_orderNonContactConstraintPool[j]];
|
||||
if (iteration < constraint.m_overrideNumSolverIterations)
|
||||
if (iteration < constraint.m_overrideNumSolverIterations)
|
||||
{
|
||||
btScalar deltaf = resolveSingleConstraintRowGeneric(m_tmpSolverBodyPool[constraint.m_solverBodyIdA],m_tmpSolverBodyPool[constraint.m_solverBodyIdB],constraint);
|
||||
btScalar deltaf = resolveSingleConstraintRowGeneric(m_tmpSolverBodyPool[constraint.m_solverBodyIdA], m_tmpSolverBodyPool[constraint.m_solverBodyIdB], constraint);
|
||||
m_deltafNC[j] = deltaf;
|
||||
deltaflengthsqr += deltaf * deltaf;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
if (m_onlyForNoneContact)
|
||||
if (m_onlyForNoneContact)
|
||||
{
|
||||
if (iteration==0)
|
||||
if (iteration == 0)
|
||||
{
|
||||
for (int j = 0; j < m_tmpSolverNonContactConstraintPool.size(); j++) m_pNC[j] = m_deltafNC[j];
|
||||
}
|
||||
else
|
||||
{
|
||||
for (int j=0;j<m_tmpSolverNonContactConstraintPool.size();j++) m_pNC[j] = m_deltafNC[j];
|
||||
} else {
|
||||
// deltaflengthsqrprev can be 0 only if the solver solved the problem exactly in the previous iteration. In this case we should have quit, but mainly for debug reason with this 'hack' it is now allowed to continue the calculation
|
||||
btScalar beta = m_deltafLengthSqrPrev>0 ? deltaflengthsqr / m_deltafLengthSqrPrev : 2;
|
||||
if (beta>1)
|
||||
btScalar beta = m_deltafLengthSqrPrev > 0 ? deltaflengthsqr / m_deltafLengthSqrPrev : 2;
|
||||
if (beta > 1)
|
||||
{
|
||||
for (int j=0;j<m_tmpSolverNonContactConstraintPool.size();j++) m_pNC[j] = 0;
|
||||
} else
|
||||
for (int j = 0; j < m_tmpSolverNonContactConstraintPool.size(); j++) m_pNC[j] = 0;
|
||||
}
|
||||
else
|
||||
{
|
||||
for (int j=0;j<m_tmpSolverNonContactConstraintPool.size();j++)
|
||||
for (int j = 0; j < m_tmpSolverNonContactConstraintPool.size(); j++)
|
||||
{
|
||||
btSolverConstraint& constraint = m_tmpSolverNonContactConstraintPool[m_orderNonContactConstraintPool[j]];
|
||||
if (iteration < constraint.m_overrideNumSolverIterations)
|
||||
if (iteration < constraint.m_overrideNumSolverIterations)
|
||||
{
|
||||
btScalar additionaldeltaimpulse = beta * m_pNC[j];
|
||||
constraint.m_appliedImpulse = btScalar(constraint.m_appliedImpulse) + additionaldeltaimpulse;
|
||||
@@ -116,8 +113,8 @@ btScalar btNNCGConstraintSolver::solveSingleIteration(int iteration, btCollision
|
||||
btSolverBody& body1 = m_tmpSolverBodyPool[constraint.m_solverBodyIdA];
|
||||
btSolverBody& body2 = m_tmpSolverBodyPool[constraint.m_solverBodyIdB];
|
||||
const btSolverConstraint& c = constraint;
|
||||
body1.internalApplyImpulse(c.m_contactNormal1*body1.internalGetInvMass(),c.m_angularComponentA,additionaldeltaimpulse);
|
||||
body2.internalApplyImpulse(c.m_contactNormal2*body2.internalGetInvMass(),c.m_angularComponentB,additionaldeltaimpulse);
|
||||
body1.internalApplyImpulse(c.m_contactNormal1 * body1.internalGetInvMass(), c.m_angularComponentA, additionaldeltaimpulse);
|
||||
body2.internalApplyImpulse(c.m_contactNormal2 * body2.internalGetInvMass(), c.m_angularComponentB, additionaldeltaimpulse);
|
||||
}
|
||||
}
|
||||
}
|
||||
@@ -125,21 +122,18 @@ btScalar btNNCGConstraintSolver::solveSingleIteration(int iteration, btCollision
|
||||
m_deltafLengthSqrPrev = deltaflengthsqr;
|
||||
}
|
||||
|
||||
|
||||
|
||||
{
|
||||
|
||||
if (iteration< infoGlobal.m_numIterations)
|
||||
if (iteration < infoGlobal.m_numIterations)
|
||||
{
|
||||
for (int j=0;j<numConstraints;j++)
|
||||
for (int j = 0; j < numConstraints; j++)
|
||||
{
|
||||
if (constraints[j]->isEnabled())
|
||||
{
|
||||
int bodyAid = getOrInitSolverBody(constraints[j]->getRigidBodyA(),infoGlobal.m_timeStep);
|
||||
int bodyBid = getOrInitSolverBody(constraints[j]->getRigidBodyB(),infoGlobal.m_timeStep);
|
||||
int bodyAid = getOrInitSolverBody(constraints[j]->getRigidBodyA(), infoGlobal.m_timeStep);
|
||||
int bodyBid = getOrInitSolverBody(constraints[j]->getRigidBodyB(), infoGlobal.m_timeStep);
|
||||
btSolverBody& bodyA = m_tmpSolverBodyPool[bodyAid];
|
||||
btSolverBody& bodyB = m_tmpSolverBodyPool[bodyBid];
|
||||
constraints[j]->solveConstraintObsolete(bodyA,bodyB,infoGlobal.m_timeStep);
|
||||
constraints[j]->solveConstraintObsolete(bodyA, bodyB, infoGlobal.m_timeStep);
|
||||
}
|
||||
}
|
||||
|
||||
@@ -147,203 +141,206 @@ btScalar btNNCGConstraintSolver::solveSingleIteration(int iteration, btCollision
|
||||
if (infoGlobal.m_solverMode & SOLVER_INTERLEAVE_CONTACT_AND_FRICTION_CONSTRAINTS)
|
||||
{
|
||||
int numPoolConstraints = m_tmpSolverContactConstraintPool.size();
|
||||
int multiplier = (infoGlobal.m_solverMode & SOLVER_USE_2_FRICTION_DIRECTIONS)? 2 : 1;
|
||||
int multiplier = (infoGlobal.m_solverMode & SOLVER_USE_2_FRICTION_DIRECTIONS) ? 2 : 1;
|
||||
|
||||
for (int c=0;c<numPoolConstraints;c++)
|
||||
for (int c = 0; c < numPoolConstraints; c++)
|
||||
{
|
||||
btScalar totalImpulse =0;
|
||||
btScalar totalImpulse = 0;
|
||||
|
||||
{
|
||||
const btSolverConstraint& solveManifold = m_tmpSolverContactConstraintPool[m_orderTmpConstraintPool[c]];
|
||||
btScalar deltaf = resolveSingleConstraintRowLowerLimit(m_tmpSolverBodyPool[solveManifold.m_solverBodyIdA],m_tmpSolverBodyPool[solveManifold.m_solverBodyIdB],solveManifold);
|
||||
btScalar deltaf = resolveSingleConstraintRowLowerLimit(m_tmpSolverBodyPool[solveManifold.m_solverBodyIdA], m_tmpSolverBodyPool[solveManifold.m_solverBodyIdB], solveManifold);
|
||||
m_deltafC[c] = deltaf;
|
||||
deltaflengthsqr += deltaf*deltaf;
|
||||
deltaflengthsqr += deltaf * deltaf;
|
||||
totalImpulse = solveManifold.m_appliedImpulse;
|
||||
}
|
||||
bool applyFriction = true;
|
||||
if (applyFriction)
|
||||
{
|
||||
{
|
||||
btSolverConstraint& solveManifold = m_tmpSolverContactFrictionConstraintPool[m_orderFrictionConstraintPool[c * multiplier]];
|
||||
|
||||
btSolverConstraint& solveManifold = m_tmpSolverContactFrictionConstraintPool[m_orderFrictionConstraintPool[c*multiplier]];
|
||||
|
||||
if (totalImpulse>btScalar(0))
|
||||
if (totalImpulse > btScalar(0))
|
||||
{
|
||||
solveManifold.m_lowerLimit = -(solveManifold.m_friction*totalImpulse);
|
||||
solveManifold.m_upperLimit = solveManifold.m_friction*totalImpulse;
|
||||
btScalar deltaf = resolveSingleConstraintRowGeneric(m_tmpSolverBodyPool[solveManifold.m_solverBodyIdA],m_tmpSolverBodyPool[solveManifold.m_solverBodyIdB],solveManifold);
|
||||
m_deltafCF[c*multiplier] = deltaf;
|
||||
deltaflengthsqr += deltaf*deltaf;
|
||||
} else {
|
||||
m_deltafCF[c*multiplier] = 0;
|
||||
solveManifold.m_lowerLimit = -(solveManifold.m_friction * totalImpulse);
|
||||
solveManifold.m_upperLimit = solveManifold.m_friction * totalImpulse;
|
||||
btScalar deltaf = resolveSingleConstraintRowGeneric(m_tmpSolverBodyPool[solveManifold.m_solverBodyIdA], m_tmpSolverBodyPool[solveManifold.m_solverBodyIdB], solveManifold);
|
||||
m_deltafCF[c * multiplier] = deltaf;
|
||||
deltaflengthsqr += deltaf * deltaf;
|
||||
}
|
||||
else
|
||||
{
|
||||
m_deltafCF[c * multiplier] = 0;
|
||||
}
|
||||
}
|
||||
|
||||
if (infoGlobal.m_solverMode & SOLVER_USE_2_FRICTION_DIRECTIONS)
|
||||
{
|
||||
btSolverConstraint& solveManifold = m_tmpSolverContactFrictionConstraintPool[m_orderFrictionConstraintPool[c * multiplier + 1]];
|
||||
|
||||
btSolverConstraint& solveManifold = m_tmpSolverContactFrictionConstraintPool[m_orderFrictionConstraintPool[c*multiplier+1]];
|
||||
|
||||
if (totalImpulse>btScalar(0))
|
||||
if (totalImpulse > btScalar(0))
|
||||
{
|
||||
solveManifold.m_lowerLimit = -(solveManifold.m_friction*totalImpulse);
|
||||
solveManifold.m_upperLimit = solveManifold.m_friction*totalImpulse;
|
||||
btScalar deltaf = resolveSingleConstraintRowGeneric(m_tmpSolverBodyPool[solveManifold.m_solverBodyIdA],m_tmpSolverBodyPool[solveManifold.m_solverBodyIdB],solveManifold);
|
||||
m_deltafCF[c*multiplier+1] = deltaf;
|
||||
deltaflengthsqr += deltaf*deltaf;
|
||||
} else {
|
||||
m_deltafCF[c*multiplier+1] = 0;
|
||||
solveManifold.m_lowerLimit = -(solveManifold.m_friction * totalImpulse);
|
||||
solveManifold.m_upperLimit = solveManifold.m_friction * totalImpulse;
|
||||
btScalar deltaf = resolveSingleConstraintRowGeneric(m_tmpSolverBodyPool[solveManifold.m_solverBodyIdA], m_tmpSolverBodyPool[solveManifold.m_solverBodyIdB], solveManifold);
|
||||
m_deltafCF[c * multiplier + 1] = deltaf;
|
||||
deltaflengthsqr += deltaf * deltaf;
|
||||
}
|
||||
else
|
||||
{
|
||||
m_deltafCF[c * multiplier + 1] = 0;
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
}
|
||||
else//SOLVER_INTERLEAVE_CONTACT_AND_FRICTION_CONSTRAINTS
|
||||
else //SOLVER_INTERLEAVE_CONTACT_AND_FRICTION_CONSTRAINTS
|
||||
{
|
||||
//solve the friction constraints after all contact constraints, don't interleave them
|
||||
int numPoolConstraints = m_tmpSolverContactConstraintPool.size();
|
||||
int j;
|
||||
|
||||
for (j=0;j<numPoolConstraints;j++)
|
||||
for (j = 0; j < numPoolConstraints; j++)
|
||||
{
|
||||
const btSolverConstraint& solveManifold = m_tmpSolverContactConstraintPool[m_orderTmpConstraintPool[j]];
|
||||
btScalar deltaf = resolveSingleConstraintRowLowerLimit(m_tmpSolverBodyPool[solveManifold.m_solverBodyIdA],m_tmpSolverBodyPool[solveManifold.m_solverBodyIdB],solveManifold);
|
||||
btScalar deltaf = resolveSingleConstraintRowLowerLimit(m_tmpSolverBodyPool[solveManifold.m_solverBodyIdA], m_tmpSolverBodyPool[solveManifold.m_solverBodyIdB], solveManifold);
|
||||
m_deltafC[j] = deltaf;
|
||||
deltaflengthsqr += deltaf*deltaf;
|
||||
deltaflengthsqr += deltaf * deltaf;
|
||||
}
|
||||
|
||||
|
||||
|
||||
///solve all friction constraints
|
||||
|
||||
int numFrictionPoolConstraints = m_tmpSolverContactFrictionConstraintPool.size();
|
||||
for (j=0;j<numFrictionPoolConstraints;j++)
|
||||
for (j = 0; j < numFrictionPoolConstraints; j++)
|
||||
{
|
||||
btSolverConstraint& solveManifold = m_tmpSolverContactFrictionConstraintPool[m_orderFrictionConstraintPool[j]];
|
||||
btScalar totalImpulse = m_tmpSolverContactConstraintPool[solveManifold.m_frictionIndex].m_appliedImpulse;
|
||||
|
||||
if (totalImpulse>btScalar(0))
|
||||
if (totalImpulse > btScalar(0))
|
||||
{
|
||||
solveManifold.m_lowerLimit = -(solveManifold.m_friction*totalImpulse);
|
||||
solveManifold.m_upperLimit = solveManifold.m_friction*totalImpulse;
|
||||
solveManifold.m_lowerLimit = -(solveManifold.m_friction * totalImpulse);
|
||||
solveManifold.m_upperLimit = solveManifold.m_friction * totalImpulse;
|
||||
|
||||
btScalar deltaf = resolveSingleConstraintRowGeneric(m_tmpSolverBodyPool[solveManifold.m_solverBodyIdA],m_tmpSolverBodyPool[solveManifold.m_solverBodyIdB],solveManifold);
|
||||
btScalar deltaf = resolveSingleConstraintRowGeneric(m_tmpSolverBodyPool[solveManifold.m_solverBodyIdA], m_tmpSolverBodyPool[solveManifold.m_solverBodyIdB], solveManifold);
|
||||
m_deltafCF[j] = deltaf;
|
||||
deltaflengthsqr += deltaf*deltaf;
|
||||
} else {
|
||||
deltaflengthsqr += deltaf * deltaf;
|
||||
}
|
||||
else
|
||||
{
|
||||
m_deltafCF[j] = 0;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
{
|
||||
{
|
||||
int numRollingFrictionPoolConstraints = m_tmpSolverContactRollingFrictionConstraintPool.size();
|
||||
for (int j=0;j<numRollingFrictionPoolConstraints;j++)
|
||||
for (int j = 0; j < numRollingFrictionPoolConstraints; j++)
|
||||
{
|
||||
|
||||
btSolverConstraint& rollingFrictionConstraint = m_tmpSolverContactRollingFrictionConstraintPool[j];
|
||||
btScalar totalImpulse = m_tmpSolverContactConstraintPool[rollingFrictionConstraint.m_frictionIndex].m_appliedImpulse;
|
||||
if (totalImpulse>btScalar(0))
|
||||
if (totalImpulse > btScalar(0))
|
||||
{
|
||||
btScalar rollingFrictionMagnitude = rollingFrictionConstraint.m_friction*totalImpulse;
|
||||
if (rollingFrictionMagnitude>rollingFrictionConstraint.m_friction)
|
||||
btScalar rollingFrictionMagnitude = rollingFrictionConstraint.m_friction * totalImpulse;
|
||||
if (rollingFrictionMagnitude > rollingFrictionConstraint.m_friction)
|
||||
rollingFrictionMagnitude = rollingFrictionConstraint.m_friction;
|
||||
|
||||
rollingFrictionConstraint.m_lowerLimit = -rollingFrictionMagnitude;
|
||||
rollingFrictionConstraint.m_upperLimit = rollingFrictionMagnitude;
|
||||
|
||||
btScalar deltaf = resolveSingleConstraintRowGeneric(m_tmpSolverBodyPool[rollingFrictionConstraint.m_solverBodyIdA],m_tmpSolverBodyPool[rollingFrictionConstraint.m_solverBodyIdB],rollingFrictionConstraint);
|
||||
btScalar deltaf = resolveSingleConstraintRowGeneric(m_tmpSolverBodyPool[rollingFrictionConstraint.m_solverBodyIdA], m_tmpSolverBodyPool[rollingFrictionConstraint.m_solverBodyIdB], rollingFrictionConstraint);
|
||||
m_deltafCRF[j] = deltaf;
|
||||
deltaflengthsqr += deltaf*deltaf;
|
||||
} else {
|
||||
deltaflengthsqr += deltaf * deltaf;
|
||||
}
|
||||
else
|
||||
{
|
||||
m_deltafCRF[j] = 0;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
|
||||
}
|
||||
|
||||
|
||||
|
||||
|
||||
if (!m_onlyForNoneContact)
|
||||
if (!m_onlyForNoneContact)
|
||||
{
|
||||
if (iteration==0)
|
||||
if (iteration == 0)
|
||||
{
|
||||
for (int j=0;j<m_tmpSolverNonContactConstraintPool.size();j++) m_pNC[j] = m_deltafNC[j];
|
||||
for (int j=0;j<m_tmpSolverContactConstraintPool.size();j++) m_pC[j] = m_deltafC[j];
|
||||
for (int j=0;j<m_tmpSolverContactFrictionConstraintPool.size();j++) m_pCF[j] = m_deltafCF[j];
|
||||
for (int j=0;j<m_tmpSolverContactRollingFrictionConstraintPool.size();j++) m_pCRF[j] = m_deltafCRF[j];
|
||||
} else
|
||||
for (int j = 0; j < m_tmpSolverNonContactConstraintPool.size(); j++) m_pNC[j] = m_deltafNC[j];
|
||||
for (int j = 0; j < m_tmpSolverContactConstraintPool.size(); j++) m_pC[j] = m_deltafC[j];
|
||||
for (int j = 0; j < m_tmpSolverContactFrictionConstraintPool.size(); j++) m_pCF[j] = m_deltafCF[j];
|
||||
for (int j = 0; j < m_tmpSolverContactRollingFrictionConstraintPool.size(); j++) m_pCRF[j] = m_deltafCRF[j];
|
||||
}
|
||||
else
|
||||
{
|
||||
// deltaflengthsqrprev can be 0 only if the solver solved the problem exactly in the previous iteration. In this case we should have quit, but mainly for debug reason with this 'hack' it is now allowed to continue the calculation
|
||||
btScalar beta = m_deltafLengthSqrPrev>0 ? deltaflengthsqr / m_deltafLengthSqrPrev : 2;
|
||||
if (beta>1) {
|
||||
for (int j=0;j<m_tmpSolverNonContactConstraintPool.size();j++) m_pNC[j] = 0;
|
||||
for (int j=0;j<m_tmpSolverContactConstraintPool.size();j++) m_pC[j] = 0;
|
||||
for (int j=0;j<m_tmpSolverContactFrictionConstraintPool.size();j++) m_pCF[j] = 0;
|
||||
for (int j=0;j<m_tmpSolverContactRollingFrictionConstraintPool.size();j++) m_pCRF[j] = 0;
|
||||
} else {
|
||||
for (int j=0;j<m_tmpSolverNonContactConstraintPool.size();j++)
|
||||
btScalar beta = m_deltafLengthSqrPrev > 0 ? deltaflengthsqr / m_deltafLengthSqrPrev : 2;
|
||||
if (beta > 1)
|
||||
{
|
||||
for (int j = 0; j < m_tmpSolverNonContactConstraintPool.size(); j++) m_pNC[j] = 0;
|
||||
for (int j = 0; j < m_tmpSolverContactConstraintPool.size(); j++) m_pC[j] = 0;
|
||||
for (int j = 0; j < m_tmpSolverContactFrictionConstraintPool.size(); j++) m_pCF[j] = 0;
|
||||
for (int j = 0; j < m_tmpSolverContactRollingFrictionConstraintPool.size(); j++) m_pCRF[j] = 0;
|
||||
}
|
||||
else
|
||||
{
|
||||
for (int j = 0; j < m_tmpSolverNonContactConstraintPool.size(); j++)
|
||||
{
|
||||
btSolverConstraint& constraint = m_tmpSolverNonContactConstraintPool[m_orderNonContactConstraintPool[j]];
|
||||
if (iteration < constraint.m_overrideNumSolverIterations) {
|
||||
if (iteration < constraint.m_overrideNumSolverIterations)
|
||||
{
|
||||
btScalar additionaldeltaimpulse = beta * m_pNC[j];
|
||||
constraint.m_appliedImpulse = btScalar(constraint.m_appliedImpulse) + additionaldeltaimpulse;
|
||||
m_pNC[j] = beta * m_pNC[j] + m_deltafNC[j];
|
||||
btSolverBody& body1 = m_tmpSolverBodyPool[constraint.m_solverBodyIdA];
|
||||
btSolverBody& body2 = m_tmpSolverBodyPool[constraint.m_solverBodyIdB];
|
||||
const btSolverConstraint& c = constraint;
|
||||
body1.internalApplyImpulse(c.m_contactNormal1*body1.internalGetInvMass(),c.m_angularComponentA,additionaldeltaimpulse);
|
||||
body2.internalApplyImpulse(c.m_contactNormal2*body2.internalGetInvMass(),c.m_angularComponentB,additionaldeltaimpulse);
|
||||
body1.internalApplyImpulse(c.m_contactNormal1 * body1.internalGetInvMass(), c.m_angularComponentA, additionaldeltaimpulse);
|
||||
body2.internalApplyImpulse(c.m_contactNormal2 * body2.internalGetInvMass(), c.m_angularComponentB, additionaldeltaimpulse);
|
||||
}
|
||||
}
|
||||
for (int j=0;j<m_tmpSolverContactConstraintPool.size();j++)
|
||||
for (int j = 0; j < m_tmpSolverContactConstraintPool.size(); j++)
|
||||
{
|
||||
btSolverConstraint& constraint = m_tmpSolverContactConstraintPool[m_orderTmpConstraintPool[j]];
|
||||
if (iteration< infoGlobal.m_numIterations) {
|
||||
if (iteration < infoGlobal.m_numIterations)
|
||||
{
|
||||
btScalar additionaldeltaimpulse = beta * m_pC[j];
|
||||
constraint.m_appliedImpulse = btScalar(constraint.m_appliedImpulse) + additionaldeltaimpulse;
|
||||
m_pC[j] = beta * m_pC[j] + m_deltafC[j];
|
||||
btSolverBody& body1 = m_tmpSolverBodyPool[constraint.m_solverBodyIdA];
|
||||
btSolverBody& body2 = m_tmpSolverBodyPool[constraint.m_solverBodyIdB];
|
||||
const btSolverConstraint& c = constraint;
|
||||
body1.internalApplyImpulse(c.m_contactNormal1*body1.internalGetInvMass(),c.m_angularComponentA,additionaldeltaimpulse);
|
||||
body2.internalApplyImpulse(c.m_contactNormal2*body2.internalGetInvMass(),c.m_angularComponentB,additionaldeltaimpulse);
|
||||
body1.internalApplyImpulse(c.m_contactNormal1 * body1.internalGetInvMass(), c.m_angularComponentA, additionaldeltaimpulse);
|
||||
body2.internalApplyImpulse(c.m_contactNormal2 * body2.internalGetInvMass(), c.m_angularComponentB, additionaldeltaimpulse);
|
||||
}
|
||||
}
|
||||
for (int j=0;j<m_tmpSolverContactFrictionConstraintPool.size();j++)
|
||||
for (int j = 0; j < m_tmpSolverContactFrictionConstraintPool.size(); j++)
|
||||
{
|
||||
btSolverConstraint& constraint = m_tmpSolverContactFrictionConstraintPool[m_orderFrictionConstraintPool[j]];
|
||||
if (iteration< infoGlobal.m_numIterations) {
|
||||
if (iteration < infoGlobal.m_numIterations)
|
||||
{
|
||||
btScalar additionaldeltaimpulse = beta * m_pCF[j];
|
||||
constraint.m_appliedImpulse = btScalar(constraint.m_appliedImpulse) + additionaldeltaimpulse;
|
||||
m_pCF[j] = beta * m_pCF[j] + m_deltafCF[j];
|
||||
btSolverBody& body1 = m_tmpSolverBodyPool[constraint.m_solverBodyIdA];
|
||||
btSolverBody& body2 = m_tmpSolverBodyPool[constraint.m_solverBodyIdB];
|
||||
const btSolverConstraint& c = constraint;
|
||||
body1.internalApplyImpulse(c.m_contactNormal1*body1.internalGetInvMass(),c.m_angularComponentA,additionaldeltaimpulse);
|
||||
body2.internalApplyImpulse(c.m_contactNormal2*body2.internalGetInvMass(),c.m_angularComponentB,additionaldeltaimpulse);
|
||||
body1.internalApplyImpulse(c.m_contactNormal1 * body1.internalGetInvMass(), c.m_angularComponentA, additionaldeltaimpulse);
|
||||
body2.internalApplyImpulse(c.m_contactNormal2 * body2.internalGetInvMass(), c.m_angularComponentB, additionaldeltaimpulse);
|
||||
}
|
||||
}
|
||||
{
|
||||
for (int j=0;j<m_tmpSolverContactRollingFrictionConstraintPool.size();j++)
|
||||
for (int j = 0; j < m_tmpSolverContactRollingFrictionConstraintPool.size(); j++)
|
||||
{
|
||||
btSolverConstraint& constraint = m_tmpSolverContactRollingFrictionConstraintPool[j];
|
||||
if (iteration< infoGlobal.m_numIterations) {
|
||||
if (iteration < infoGlobal.m_numIterations)
|
||||
{
|
||||
btScalar additionaldeltaimpulse = beta * m_pCRF[j];
|
||||
constraint.m_appliedImpulse = btScalar(constraint.m_appliedImpulse) + additionaldeltaimpulse;
|
||||
m_pCRF[j] = beta * m_pCRF[j] + m_deltafCRF[j];
|
||||
btSolverBody& body1 = m_tmpSolverBodyPool[constraint.m_solverBodyIdA];
|
||||
btSolverBody& body2 = m_tmpSolverBodyPool[constraint.m_solverBodyIdB];
|
||||
const btSolverConstraint& c = constraint;
|
||||
body1.internalApplyImpulse(c.m_contactNormal1*body1.internalGetInvMass(),c.m_angularComponentA,additionaldeltaimpulse);
|
||||
body2.internalApplyImpulse(c.m_contactNormal2*body2.internalGetInvMass(),c.m_angularComponentB,additionaldeltaimpulse);
|
||||
body1.internalApplyImpulse(c.m_contactNormal1 * body1.internalGetInvMass(), c.m_angularComponentA, additionaldeltaimpulse);
|
||||
body2.internalApplyImpulse(c.m_contactNormal2 * body2.internalGetInvMass(), c.m_angularComponentB, additionaldeltaimpulse);
|
||||
}
|
||||
}
|
||||
}
|
||||
@@ -355,7 +352,7 @@ btScalar btNNCGConstraintSolver::solveSingleIteration(int iteration, btCollision
|
||||
return deltaflengthsqr;
|
||||
}
|
||||
|
||||
btScalar btNNCGConstraintSolver::solveGroupCacheFriendlyFinish(btCollisionObject** bodies,int numBodies,const btContactSolverInfo& infoGlobal)
|
||||
btScalar btNNCGConstraintSolver::solveGroupCacheFriendlyFinish(btCollisionObject** bodies, int numBodies, const btContactSolverInfo& infoGlobal)
|
||||
{
|
||||
m_pNC.resizeNoInitialize(0);
|
||||
m_pC.resizeNoInitialize(0);
|
||||
@@ -369,6 +366,3 @@ btScalar btNNCGConstraintSolver::solveGroupCacheFriendlyFinish(btCollisionObject
|
||||
|
||||
return btSequentialImpulseConstraintSolver::solveGroupCacheFriendlyFinish(bodies, numBodies, infoGlobal);
|
||||
}
|
||||
|
||||
|
||||
|
||||
|
||||
@@ -18,33 +18,30 @@ subject to the following restrictions:
|
||||
|
||||
#include "btSequentialImpulseConstraintSolver.h"
|
||||
|
||||
ATTRIBUTE_ALIGNED16(class) btNNCGConstraintSolver : public btSequentialImpulseConstraintSolver
|
||||
ATTRIBUTE_ALIGNED16(class)
|
||||
btNNCGConstraintSolver : public btSequentialImpulseConstraintSolver
|
||||
{
|
||||
protected:
|
||||
|
||||
btScalar m_deltafLengthSqrPrev;
|
||||
|
||||
btAlignedObjectArray<btScalar> m_pNC; // p for None Contact constraints
|
||||
btAlignedObjectArray<btScalar> m_pC; // p for Contact constraints
|
||||
btAlignedObjectArray<btScalar> m_pCF; // p for ContactFriction constraints
|
||||
btAlignedObjectArray<btScalar> m_pCRF; // p for ContactRollingFriction constraints
|
||||
btAlignedObjectArray<btScalar> m_pNC; // p for None Contact constraints
|
||||
btAlignedObjectArray<btScalar> m_pC; // p for Contact constraints
|
||||
btAlignedObjectArray<btScalar> m_pCF; // p for ContactFriction constraints
|
||||
btAlignedObjectArray<btScalar> m_pCRF; // p for ContactRollingFriction constraints
|
||||
|
||||
//These are recalculated in every iterations. We just keep these to prevent reallocation in each iteration.
|
||||
btAlignedObjectArray<btScalar> m_deltafNC; // deltaf for NoneContact constraints
|
||||
btAlignedObjectArray<btScalar> m_deltafC; // deltaf for Contact constraints
|
||||
btAlignedObjectArray<btScalar> m_deltafCF; // deltaf for ContactFriction constraints
|
||||
btAlignedObjectArray<btScalar> m_deltafCRF; // deltaf for ContactRollingFriction constraints
|
||||
btAlignedObjectArray<btScalar> m_deltafNC; // deltaf for NoneContact constraints
|
||||
btAlignedObjectArray<btScalar> m_deltafC; // deltaf for Contact constraints
|
||||
btAlignedObjectArray<btScalar> m_deltafCF; // deltaf for ContactFriction constraints
|
||||
btAlignedObjectArray<btScalar> m_deltafCRF; // deltaf for ContactRollingFriction constraints
|
||||
|
||||
|
||||
protected:
|
||||
virtual btScalar solveGroupCacheFriendlyFinish(btCollisionObject * *bodies, int numBodies, const btContactSolverInfo& infoGlobal);
|
||||
virtual btScalar solveSingleIteration(int iteration, btCollisionObject** bodies, int numBodies, btPersistentManifold** manifoldPtr, int numManifolds, btTypedConstraint** constraints, int numConstraints, const btContactSolverInfo& infoGlobal, btIDebugDraw* debugDrawer);
|
||||
|
||||
virtual btScalar solveGroupCacheFriendlyFinish(btCollisionObject** bodies,int numBodies,const btContactSolverInfo& infoGlobal);
|
||||
virtual btScalar solveSingleIteration(int iteration, btCollisionObject** bodies ,int numBodies,btPersistentManifold** manifoldPtr, int numManifolds,btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& infoGlobal,btIDebugDraw* debugDrawer);
|
||||
|
||||
virtual btScalar solveGroupCacheFriendlySetup(btCollisionObject** bodies,int numBodies,btPersistentManifold** manifoldPtr, int numManifolds,btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& infoGlobal,btIDebugDraw* debugDrawer);
|
||||
virtual btScalar solveGroupCacheFriendlySetup(btCollisionObject * *bodies, int numBodies, btPersistentManifold** manifoldPtr, int numManifolds, btTypedConstraint** constraints, int numConstraints, const btContactSolverInfo& infoGlobal, btIDebugDraw* debugDrawer);
|
||||
|
||||
public:
|
||||
|
||||
BT_DECLARE_ALIGNED_ALLOCATOR();
|
||||
|
||||
btNNCGConstraintSolver() : btSequentialImpulseConstraintSolver(), m_onlyForNoneContact(false) {}
|
||||
@@ -57,8 +54,4 @@ public:
|
||||
bool m_onlyForNoneContact;
|
||||
};
|
||||
|
||||
|
||||
|
||||
|
||||
#endif //BT_NNCG_CONSTRAINT_SOLVER_H
|
||||
|
||||
#endif //BT_NNCG_CONSTRAINT_SOLVER_H
|
||||
|
||||
@@ -13,217 +13,193 @@ subject to the following restrictions:
|
||||
3. This notice may not be removed or altered from any source distribution.
|
||||
*/
|
||||
|
||||
|
||||
#include "btPoint2PointConstraint.h"
|
||||
#include "BulletDynamics/Dynamics/btRigidBody.h"
|
||||
#include <new>
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
btPoint2PointConstraint::btPoint2PointConstraint(btRigidBody& rbA,btRigidBody& rbB, const btVector3& pivotInA,const btVector3& pivotInB)
|
||||
:btTypedConstraint(POINT2POINT_CONSTRAINT_TYPE,rbA,rbB),m_pivotInA(pivotInA),m_pivotInB(pivotInB),
|
||||
m_flags(0),
|
||||
m_useSolveConstraintObsolete(false)
|
||||
btPoint2PointConstraint::btPoint2PointConstraint(btRigidBody& rbA, btRigidBody& rbB, const btVector3& pivotInA, const btVector3& pivotInB)
|
||||
: btTypedConstraint(POINT2POINT_CONSTRAINT_TYPE, rbA, rbB), m_pivotInA(pivotInA), m_pivotInB(pivotInB), m_flags(0), m_useSolveConstraintObsolete(false)
|
||||
{
|
||||
|
||||
}
|
||||
|
||||
|
||||
btPoint2PointConstraint::btPoint2PointConstraint(btRigidBody& rbA,const btVector3& pivotInA)
|
||||
:btTypedConstraint(POINT2POINT_CONSTRAINT_TYPE,rbA),m_pivotInA(pivotInA),m_pivotInB(rbA.getCenterOfMassTransform()(pivotInA)),
|
||||
m_flags(0),
|
||||
m_useSolveConstraintObsolete(false)
|
||||
btPoint2PointConstraint::btPoint2PointConstraint(btRigidBody& rbA, const btVector3& pivotInA)
|
||||
: btTypedConstraint(POINT2POINT_CONSTRAINT_TYPE, rbA), m_pivotInA(pivotInA), m_pivotInB(rbA.getCenterOfMassTransform()(pivotInA)), m_flags(0), m_useSolveConstraintObsolete(false)
|
||||
{
|
||||
|
||||
}
|
||||
|
||||
void btPoint2PointConstraint::buildJacobian()
|
||||
void btPoint2PointConstraint::buildJacobian()
|
||||
{
|
||||
|
||||
///we need it for both methods
|
||||
{
|
||||
m_appliedImpulse = btScalar(0.);
|
||||
|
||||
btVector3 normal(0,0,0);
|
||||
btVector3 normal(0, 0, 0);
|
||||
|
||||
for (int i=0;i<3;i++)
|
||||
for (int i = 0; i < 3; i++)
|
||||
{
|
||||
normal[i] = 1;
|
||||
new (&m_jac[i]) btJacobianEntry(
|
||||
m_rbA.getCenterOfMassTransform().getBasis().transpose(),
|
||||
m_rbB.getCenterOfMassTransform().getBasis().transpose(),
|
||||
m_rbA.getCenterOfMassTransform()*m_pivotInA - m_rbA.getCenterOfMassPosition(),
|
||||
m_rbB.getCenterOfMassTransform()*m_pivotInB - m_rbB.getCenterOfMassPosition(),
|
||||
normal,
|
||||
m_rbA.getInvInertiaDiagLocal(),
|
||||
m_rbA.getInvMass(),
|
||||
m_rbB.getInvInertiaDiagLocal(),
|
||||
m_rbB.getInvMass());
|
||||
normal[i] = 0;
|
||||
m_rbA.getCenterOfMassTransform().getBasis().transpose(),
|
||||
m_rbB.getCenterOfMassTransform().getBasis().transpose(),
|
||||
m_rbA.getCenterOfMassTransform() * m_pivotInA - m_rbA.getCenterOfMassPosition(),
|
||||
m_rbB.getCenterOfMassTransform() * m_pivotInB - m_rbB.getCenterOfMassPosition(),
|
||||
normal,
|
||||
m_rbA.getInvInertiaDiagLocal(),
|
||||
m_rbA.getInvMass(),
|
||||
m_rbB.getInvInertiaDiagLocal(),
|
||||
m_rbB.getInvMass());
|
||||
normal[i] = 0;
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
}
|
||||
|
||||
void btPoint2PointConstraint::getInfo1 (btConstraintInfo1* info)
|
||||
void btPoint2PointConstraint::getInfo1(btConstraintInfo1* info)
|
||||
{
|
||||
getInfo1NonVirtual(info);
|
||||
}
|
||||
|
||||
void btPoint2PointConstraint::getInfo1NonVirtual (btConstraintInfo1* info)
|
||||
void btPoint2PointConstraint::getInfo1NonVirtual(btConstraintInfo1* info)
|
||||
{
|
||||
if (m_useSolveConstraintObsolete)
|
||||
{
|
||||
info->m_numConstraintRows = 0;
|
||||
info->nub = 0;
|
||||
} else
|
||||
}
|
||||
else
|
||||
{
|
||||
info->m_numConstraintRows = 3;
|
||||
info->nub = 3;
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
|
||||
|
||||
void btPoint2PointConstraint::getInfo2 (btConstraintInfo2* info)
|
||||
void btPoint2PointConstraint::getInfo2(btConstraintInfo2* info)
|
||||
{
|
||||
getInfo2NonVirtual(info, m_rbA.getCenterOfMassTransform(),m_rbB.getCenterOfMassTransform());
|
||||
getInfo2NonVirtual(info, m_rbA.getCenterOfMassTransform(), m_rbB.getCenterOfMassTransform());
|
||||
}
|
||||
|
||||
void btPoint2PointConstraint::getInfo2NonVirtual (btConstraintInfo2* info, const btTransform& body0_trans, const btTransform& body1_trans)
|
||||
void btPoint2PointConstraint::getInfo2NonVirtual(btConstraintInfo2* info, const btTransform& body0_trans, const btTransform& body1_trans)
|
||||
{
|
||||
btAssert(!m_useSolveConstraintObsolete);
|
||||
|
||||
//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;
|
||||
|
||||
btVector3 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;
|
||||
|
||||
btVector3 a1 = body0_trans.getBasis() * getPivotInA();
|
||||
{
|
||||
btVector3* angular0 = (btVector3*)(info->m_J1angularAxis);
|
||||
btVector3* angular1 = (btVector3*)(info->m_J1angularAxis+info->rowskip);
|
||||
btVector3* angular2 = (btVector3*)(info->m_J1angularAxis+2*info->rowskip);
|
||||
btVector3* angular1 = (btVector3*)(info->m_J1angularAxis + info->rowskip);
|
||||
btVector3* angular2 = (btVector3*)(info->m_J1angularAxis + 2 * info->rowskip);
|
||||
btVector3 a1neg = -a1;
|
||||
a1neg.getSkewSymmetricMatrix(angular0,angular1,angular2);
|
||||
a1neg.getSkewSymmetricMatrix(angular0, angular1, angular2);
|
||||
}
|
||||
|
||||
|
||||
info->m_J2linearAxis[0] = -1;
|
||||
info->m_J2linearAxis[info->rowskip+1] = -1;
|
||||
info->m_J2linearAxis[2*info->rowskip+2] = -1;
|
||||
|
||||
btVector3 a2 = body1_trans.getBasis()*getPivotInB();
|
||||
|
||||
info->m_J2linearAxis[info->rowskip + 1] = -1;
|
||||
info->m_J2linearAxis[2 * info->rowskip + 2] = -1;
|
||||
|
||||
btVector3 a2 = body1_trans.getBasis() * getPivotInB();
|
||||
|
||||
{
|
||||
// btVector3 a2n = -a2;
|
||||
// btVector3 a2n = -a2;
|
||||
btVector3* angular0 = (btVector3*)(info->m_J2angularAxis);
|
||||
btVector3* angular1 = (btVector3*)(info->m_J2angularAxis+info->rowskip);
|
||||
btVector3* angular2 = (btVector3*)(info->m_J2angularAxis+2*info->rowskip);
|
||||
a2.getSkewSymmetricMatrix(angular0,angular1,angular2);
|
||||
btVector3* angular1 = (btVector3*)(info->m_J2angularAxis + info->rowskip);
|
||||
btVector3* angular2 = (btVector3*)(info->m_J2angularAxis + 2 * info->rowskip);
|
||||
a2.getSkewSymmetricMatrix(angular0, angular1, angular2);
|
||||
}
|
||||
|
||||
|
||||
|
||||
// set right hand side
|
||||
// set right hand side
|
||||
btScalar currERP = (m_flags & BT_P2P_FLAGS_ERP) ? m_erp : info->erp;
|
||||
btScalar 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 & BT_P2P_FLAGS_CFM)
|
||||
btScalar 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 & BT_P2P_FLAGS_CFM)
|
||||
{
|
||||
for (j = 0; j < 3; j++)
|
||||
{
|
||||
info->cfm[j*info->rowskip] = m_cfm;
|
||||
info->cfm[j * info->rowskip] = m_cfm;
|
||||
}
|
||||
}
|
||||
|
||||
btScalar impulseClamp = m_setting.m_impulseClamp;//
|
||||
for (j=0; j<3; j++)
|
||||
{
|
||||
btScalar 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 btPoint2PointConstraint::updateRHS(btScalar timeStep)
|
||||
void btPoint2PointConstraint::updateRHS(btScalar 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 btPoint2PointConstraint::setParam(int num, btScalar value, int axis)
|
||||
{
|
||||
if(axis != -1)
|
||||
if (axis != -1)
|
||||
{
|
||||
btAssertConstrParams(0);
|
||||
}
|
||||
else
|
||||
{
|
||||
switch(num)
|
||||
switch (num)
|
||||
{
|
||||
case BT_CONSTRAINT_ERP :
|
||||
case BT_CONSTRAINT_STOP_ERP :
|
||||
m_erp = value;
|
||||
case BT_CONSTRAINT_ERP:
|
||||
case BT_CONSTRAINT_STOP_ERP:
|
||||
m_erp = value;
|
||||
m_flags |= BT_P2P_FLAGS_ERP;
|
||||
break;
|
||||
case BT_CONSTRAINT_CFM :
|
||||
case BT_CONSTRAINT_STOP_CFM :
|
||||
m_cfm = value;
|
||||
case BT_CONSTRAINT_CFM:
|
||||
case BT_CONSTRAINT_STOP_CFM:
|
||||
m_cfm = value;
|
||||
m_flags |= BT_P2P_FLAGS_CFM;
|
||||
break;
|
||||
default:
|
||||
default:
|
||||
btAssertConstrParams(0);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
///return the local value of parameter
|
||||
btScalar btPoint2PointConstraint::getParam(int num, int axis) const
|
||||
btScalar btPoint2PointConstraint::getParam(int num, int axis) const
|
||||
{
|
||||
btScalar retVal(SIMD_INFINITY);
|
||||
if(axis != -1)
|
||||
if (axis != -1)
|
||||
{
|
||||
btAssertConstrParams(0);
|
||||
}
|
||||
else
|
||||
{
|
||||
switch(num)
|
||||
switch (num)
|
||||
{
|
||||
case BT_CONSTRAINT_ERP :
|
||||
case BT_CONSTRAINT_STOP_ERP :
|
||||
case BT_CONSTRAINT_ERP:
|
||||
case BT_CONSTRAINT_STOP_ERP:
|
||||
btAssertConstrParams(m_flags & BT_P2P_FLAGS_ERP);
|
||||
retVal = m_erp;
|
||||
retVal = m_erp;
|
||||
break;
|
||||
case BT_CONSTRAINT_CFM :
|
||||
case BT_CONSTRAINT_STOP_CFM :
|
||||
case BT_CONSTRAINT_CFM:
|
||||
case BT_CONSTRAINT_STOP_CFM:
|
||||
btAssertConstrParams(m_flags & BT_P2P_FLAGS_CFM);
|
||||
retVal = m_cfm;
|
||||
retVal = m_cfm;
|
||||
break;
|
||||
default:
|
||||
default:
|
||||
btAssertConstrParams(0);
|
||||
}
|
||||
}
|
||||
return retVal;
|
||||
}
|
||||
|
||||
|
||||
@@ -22,26 +22,24 @@ subject to the following restrictions:
|
||||
|
||||
class btRigidBody;
|
||||
|
||||
|
||||
#ifdef BT_USE_DOUBLE_PRECISION
|
||||
#define btPoint2PointConstraintData2 btPoint2PointConstraintDoubleData2
|
||||
#define btPoint2PointConstraintDataName "btPoint2PointConstraintDoubleData2"
|
||||
#define btPoint2PointConstraintData2 btPoint2PointConstraintDoubleData2
|
||||
#define btPoint2PointConstraintDataName "btPoint2PointConstraintDoubleData2"
|
||||
#else
|
||||
#define btPoint2PointConstraintData2 btPoint2PointConstraintFloatData
|
||||
#define btPoint2PointConstraintDataName "btPoint2PointConstraintFloatData"
|
||||
#endif //BT_USE_DOUBLE_PRECISION
|
||||
#define btPoint2PointConstraintData2 btPoint2PointConstraintFloatData
|
||||
#define btPoint2PointConstraintDataName "btPoint2PointConstraintFloatData"
|
||||
#endif //BT_USE_DOUBLE_PRECISION
|
||||
|
||||
struct btConstraintSetting
|
||||
struct btConstraintSetting
|
||||
{
|
||||
btConstraintSetting() :
|
||||
m_tau(btScalar(0.3)),
|
||||
m_damping(btScalar(1.)),
|
||||
m_impulseClamp(btScalar(0.))
|
||||
btConstraintSetting() : m_tau(btScalar(0.3)),
|
||||
m_damping(btScalar(1.)),
|
||||
m_impulseClamp(btScalar(0.))
|
||||
{
|
||||
}
|
||||
btScalar m_tau;
|
||||
btScalar m_damping;
|
||||
btScalar m_impulseClamp;
|
||||
btScalar m_tau;
|
||||
btScalar m_damping;
|
||||
btScalar m_impulseClamp;
|
||||
};
|
||||
|
||||
enum btPoint2PointFlags
|
||||
@@ -51,52 +49,51 @@ enum btPoint2PointFlags
|
||||
};
|
||||
|
||||
/// point to point constraint between two rigidbodies each with a pivotpoint that descibes the 'ballsocket' location in local space
|
||||
ATTRIBUTE_ALIGNED16(class) btPoint2PointConstraint : public btTypedConstraint
|
||||
ATTRIBUTE_ALIGNED16(class)
|
||||
btPoint2PointConstraint : public btTypedConstraint
|
||||
{
|
||||
#ifdef IN_PARALLELL_SOLVER
|
||||
public:
|
||||
#endif
|
||||
btJacobianEntry m_jac[3]; //3 orthogonal linear constraints
|
||||
|
||||
btVector3 m_pivotInA;
|
||||
btVector3 m_pivotInB;
|
||||
|
||||
int m_flags;
|
||||
btScalar m_erp;
|
||||
btScalar m_cfm;
|
||||
|
||||
public:
|
||||
btJacobianEntry m_jac[3]; //3 orthogonal linear constraints
|
||||
|
||||
btVector3 m_pivotInA;
|
||||
btVector3 m_pivotInB;
|
||||
|
||||
int m_flags;
|
||||
btScalar m_erp;
|
||||
btScalar m_cfm;
|
||||
|
||||
public:
|
||||
BT_DECLARE_ALIGNED_ALLOCATOR();
|
||||
|
||||
///for backwards compatibility during the transition to 'getInfo/getInfo2'
|
||||
bool m_useSolveConstraintObsolete;
|
||||
bool m_useSolveConstraintObsolete;
|
||||
|
||||
btConstraintSetting m_setting;
|
||||
btConstraintSetting m_setting;
|
||||
|
||||
btPoint2PointConstraint(btRigidBody& rbA,btRigidBody& rbB, const btVector3& pivotInA,const btVector3& pivotInB);
|
||||
btPoint2PointConstraint(btRigidBody & rbA, btRigidBody & rbB, const btVector3& pivotInA, const btVector3& pivotInB);
|
||||
|
||||
btPoint2PointConstraint(btRigidBody& rbA,const btVector3& pivotInA);
|
||||
btPoint2PointConstraint(btRigidBody & rbA, const btVector3& pivotInA);
|
||||
|
||||
virtual void buildJacobian();
|
||||
|
||||
virtual void buildJacobian();
|
||||
virtual void getInfo1(btConstraintInfo1 * info);
|
||||
|
||||
virtual void getInfo1 (btConstraintInfo1* info);
|
||||
void getInfo1NonVirtual(btConstraintInfo1 * info);
|
||||
|
||||
void getInfo1NonVirtual (btConstraintInfo1* info);
|
||||
virtual void getInfo2(btConstraintInfo2 * info);
|
||||
|
||||
virtual void getInfo2 (btConstraintInfo2* info);
|
||||
void getInfo2NonVirtual(btConstraintInfo2 * info, const btTransform& body0_trans, const btTransform& body1_trans);
|
||||
|
||||
void getInfo2NonVirtual (btConstraintInfo2* info, const btTransform& body0_trans, const btTransform& body1_trans);
|
||||
void updateRHS(btScalar timeStep);
|
||||
|
||||
void updateRHS(btScalar timeStep);
|
||||
|
||||
void setPivotA(const btVector3& pivotA)
|
||||
void setPivotA(const btVector3& pivotA)
|
||||
{
|
||||
m_pivotInA = pivotA;
|
||||
}
|
||||
|
||||
void setPivotB(const btVector3& pivotB)
|
||||
void setPivotB(const btVector3& pivotB)
|
||||
{
|
||||
m_pivotInB = pivotB;
|
||||
}
|
||||
@@ -111,70 +108,66 @@ 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, btScalar value, int axis = -1);
|
||||
virtual void setParam(int num, btScalar value, int axis = -1);
|
||||
///return the local value of parameter
|
||||
virtual btScalar getParam(int num, int axis = -1) const;
|
||||
|
||||
virtual int getFlags() const
|
||||
{
|
||||
return m_flags;
|
||||
}
|
||||
virtual btScalar getParam(int num, int axis = -1) const;
|
||||
|
||||
virtual int calculateSerializeBufferSize() const;
|
||||
virtual int getFlags() const
|
||||
{
|
||||
return m_flags;
|
||||
}
|
||||
|
||||
virtual int calculateSerializeBufferSize() const;
|
||||
|
||||
///fills the dataBuffer and returns the struct name (and 0 on failure)
|
||||
virtual const char* serialize(void* dataBuffer, btSerializer* serializer) const;
|
||||
|
||||
|
||||
virtual const char* serialize(void* dataBuffer, btSerializer* serializer) const;
|
||||
};
|
||||
|
||||
///do not change those serialization structures, it requires an updated sBulletDNAstr/sBulletDNAstr64
|
||||
struct btPoint2PointConstraintFloatData
|
||||
struct btPoint2PointConstraintFloatData
|
||||
{
|
||||
btTypedConstraintData m_typeConstraintData;
|
||||
btVector3FloatData m_pivotInA;
|
||||
btVector3FloatData m_pivotInB;
|
||||
btTypedConstraintData m_typeConstraintData;
|
||||
btVector3FloatData m_pivotInA;
|
||||
btVector3FloatData m_pivotInB;
|
||||
};
|
||||
|
||||
///do not change those serialization structures, it requires an updated sBulletDNAstr/sBulletDNAstr64
|
||||
struct btPoint2PointConstraintDoubleData2
|
||||
struct btPoint2PointConstraintDoubleData2
|
||||
{
|
||||
btTypedConstraintDoubleData m_typeConstraintData;
|
||||
btVector3DoubleData m_pivotInA;
|
||||
btVector3DoubleData m_pivotInB;
|
||||
btTypedConstraintDoubleData m_typeConstraintData;
|
||||
btVector3DoubleData m_pivotInA;
|
||||
btVector3DoubleData m_pivotInB;
|
||||
};
|
||||
|
||||
#ifdef BT_BACKWARDS_COMPATIBLE_SERIALIZATION
|
||||
///do not change those serialization structures, it requires an updated sBulletDNAstr/sBulletDNAstr64
|
||||
///this structure is not used, except for loading pre-2.82 .bullet files
|
||||
///do not change those serialization structures, it requires an updated sBulletDNAstr/sBulletDNAstr64
|
||||
struct btPoint2PointConstraintDoubleData
|
||||
struct btPoint2PointConstraintDoubleData
|
||||
{
|
||||
btTypedConstraintData m_typeConstraintData;
|
||||
btVector3DoubleData m_pivotInA;
|
||||
btVector3DoubleData m_pivotInB;
|
||||
btTypedConstraintData m_typeConstraintData;
|
||||
btVector3DoubleData m_pivotInA;
|
||||
btVector3DoubleData m_pivotInB;
|
||||
};
|
||||
#endif //BT_BACKWARDS_COMPATIBLE_SERIALIZATION
|
||||
#endif //BT_BACKWARDS_COMPATIBLE_SERIALIZATION
|
||||
|
||||
|
||||
SIMD_FORCE_INLINE int btPoint2PointConstraint::calculateSerializeBufferSize() const
|
||||
SIMD_FORCE_INLINE int btPoint2PointConstraint::calculateSerializeBufferSize() const
|
||||
{
|
||||
return sizeof(btPoint2PointConstraintData2);
|
||||
|
||||
}
|
||||
|
||||
///fills the dataBuffer and returns the struct name (and 0 on failure)
|
||||
SIMD_FORCE_INLINE const char* btPoint2PointConstraint::serialize(void* dataBuffer, btSerializer* serializer) const
|
||||
///fills the dataBuffer and returns the struct name (and 0 on failure)
|
||||
SIMD_FORCE_INLINE const char* btPoint2PointConstraint::serialize(void* dataBuffer, btSerializer* serializer) const
|
||||
{
|
||||
btPoint2PointConstraintData2* p2pData = (btPoint2PointConstraintData2*)dataBuffer;
|
||||
|
||||
btTypedConstraint::serialize(&p2pData->m_typeConstraintData,serializer);
|
||||
btTypedConstraint::serialize(&p2pData->m_typeConstraintData, serializer);
|
||||
m_pivotInA.serialize(p2pData->m_pivotInA);
|
||||
m_pivotInB.serialize(p2pData->m_pivotInB);
|
||||
|
||||
return btPoint2PointConstraintDataName;
|
||||
}
|
||||
|
||||
#endif //BT_POINT2POINTCONSTRAINT_H
|
||||
#endif //BT_POINT2POINTCONSTRAINT_H
|
||||
|
||||
File diff suppressed because it is too large
Load Diff
@@ -27,147 +27,142 @@ class btCollisionObject;
|
||||
#include "BulletCollision/NarrowPhaseCollision/btManifoldPoint.h"
|
||||
#include "BulletDynamics/ConstraintSolver/btConstraintSolver.h"
|
||||
|
||||
typedef btScalar(*btSingleConstraintRowSolver)(btSolverBody&, btSolverBody&, const btSolverConstraint&);
|
||||
typedef btScalar (*btSingleConstraintRowSolver)(btSolverBody&, btSolverBody&, const btSolverConstraint&);
|
||||
|
||||
///The btSequentialImpulseConstraintSolver is a fast SIMD implementation of the Projected Gauss Seidel (iterative LCP) method.
|
||||
ATTRIBUTE_ALIGNED16(class) btSequentialImpulseConstraintSolver : public btConstraintSolver
|
||||
ATTRIBUTE_ALIGNED16(class)
|
||||
btSequentialImpulseConstraintSolver : public btConstraintSolver
|
||||
{
|
||||
protected:
|
||||
btAlignedObjectArray<btSolverBody> m_tmpSolverBodyPool;
|
||||
btConstraintArray m_tmpSolverContactConstraintPool;
|
||||
btConstraintArray m_tmpSolverNonContactConstraintPool;
|
||||
btConstraintArray m_tmpSolverContactFrictionConstraintPool;
|
||||
btConstraintArray m_tmpSolverContactRollingFrictionConstraintPool;
|
||||
btAlignedObjectArray<btSolverBody> m_tmpSolverBodyPool;
|
||||
btConstraintArray m_tmpSolverContactConstraintPool;
|
||||
btConstraintArray m_tmpSolverNonContactConstraintPool;
|
||||
btConstraintArray m_tmpSolverContactFrictionConstraintPool;
|
||||
btConstraintArray m_tmpSolverContactRollingFrictionConstraintPool;
|
||||
|
||||
btAlignedObjectArray<int> m_orderTmpConstraintPool;
|
||||
btAlignedObjectArray<int> m_orderNonContactConstraintPool;
|
||||
btAlignedObjectArray<int> m_orderFrictionConstraintPool;
|
||||
btAlignedObjectArray<int> m_orderTmpConstraintPool;
|
||||
btAlignedObjectArray<int> m_orderNonContactConstraintPool;
|
||||
btAlignedObjectArray<int> m_orderFrictionConstraintPool;
|
||||
btAlignedObjectArray<btTypedConstraint::btConstraintInfo1> m_tmpConstraintSizesPool;
|
||||
int m_maxOverrideNumSolverIterations;
|
||||
int m_maxOverrideNumSolverIterations;
|
||||
int m_fixedBodyId;
|
||||
// When running solvers on multiple threads, a race condition exists for Kinematic objects that
|
||||
// participate in more than one solver.
|
||||
// The getOrInitSolverBody() function writes the companionId of each body (storing the index of the solver body
|
||||
// for the current solver). For normal dynamic bodies it isn't an issue because they can only be in one island
|
||||
// (and therefore one thread) at a time. But kinematic bodies can be in multiple islands at once.
|
||||
// To avoid this race condition, this solver does not write the companionId, instead it stores the solver body
|
||||
// index in this solver-local table, indexed by the uniqueId of the body.
|
||||
btAlignedObjectArray<int> m_kinematicBodyUniqueIdToSolverBodyTable; // only used for multithreading
|
||||
// When running solvers on multiple threads, a race condition exists for Kinematic objects that
|
||||
// participate in more than one solver.
|
||||
// The getOrInitSolverBody() function writes the companionId of each body (storing the index of the solver body
|
||||
// for the current solver). For normal dynamic bodies it isn't an issue because they can only be in one island
|
||||
// (and therefore one thread) at a time. But kinematic bodies can be in multiple islands at once.
|
||||
// To avoid this race condition, this solver does not write the companionId, instead it stores the solver body
|
||||
// index in this solver-local table, indexed by the uniqueId of the body.
|
||||
btAlignedObjectArray<int> m_kinematicBodyUniqueIdToSolverBodyTable; // only used for multithreading
|
||||
|
||||
btSingleConstraintRowSolver m_resolveSingleConstraintRowGeneric;
|
||||
btSingleConstraintRowSolver m_resolveSingleConstraintRowLowerLimit;
|
||||
btSingleConstraintRowSolver m_resolveSplitPenetrationImpulse;
|
||||
int m_cachedSolverMode; // used to check if SOLVER_SIMD flag has been changed
|
||||
void setupSolverFunctions( bool useSimd );
|
||||
btSingleConstraintRowSolver m_resolveSplitPenetrationImpulse;
|
||||
int m_cachedSolverMode; // used to check if SOLVER_SIMD flag has been changed
|
||||
void setupSolverFunctions(bool useSimd);
|
||||
|
||||
btScalar m_leastSquaresResidual;
|
||||
btScalar m_leastSquaresResidual;
|
||||
|
||||
void setupFrictionConstraint( btSolverConstraint& solverConstraint, const btVector3& normalAxis,int solverBodyIdA,int solverBodyIdB,
|
||||
btManifoldPoint& cp,const btVector3& rel_pos1,const btVector3& rel_pos2,
|
||||
btCollisionObject* colObj0,btCollisionObject* colObj1, btScalar relaxation,
|
||||
const btContactSolverInfo& infoGlobal,
|
||||
btScalar desiredVelocity=0., btScalar cfmSlip=0.);
|
||||
void setupFrictionConstraint(btSolverConstraint & solverConstraint, const btVector3& normalAxis, int solverBodyIdA, int solverBodyIdB,
|
||||
btManifoldPoint& cp, const btVector3& rel_pos1, const btVector3& rel_pos2,
|
||||
btCollisionObject* colObj0, btCollisionObject* colObj1, btScalar relaxation,
|
||||
const btContactSolverInfo& infoGlobal,
|
||||
btScalar desiredVelocity = 0., btScalar cfmSlip = 0.);
|
||||
|
||||
void setupTorsionalFrictionConstraint( btSolverConstraint& solverConstraint, const btVector3& normalAxis,int solverBodyIdA,int solverBodyIdB,
|
||||
btManifoldPoint& cp,btScalar combinedTorsionalFriction, const btVector3& rel_pos1,const btVector3& rel_pos2,
|
||||
btCollisionObject* colObj0,btCollisionObject* colObj1, btScalar relaxation,
|
||||
btScalar desiredVelocity=0., btScalar cfmSlip=0.);
|
||||
void setupTorsionalFrictionConstraint(btSolverConstraint & solverConstraint, const btVector3& normalAxis, int solverBodyIdA, int solverBodyIdB,
|
||||
btManifoldPoint& cp, btScalar combinedTorsionalFriction, const btVector3& rel_pos1, const btVector3& rel_pos2,
|
||||
btCollisionObject* colObj0, btCollisionObject* colObj1, btScalar relaxation,
|
||||
btScalar desiredVelocity = 0., btScalar cfmSlip = 0.);
|
||||
|
||||
btSolverConstraint& addFrictionConstraint(const btVector3& normalAxis,int solverBodyIdA,int solverBodyIdB,int frictionIndex,btManifoldPoint& cp,const btVector3& rel_pos1,const btVector3& rel_pos2,btCollisionObject* colObj0,btCollisionObject* colObj1, btScalar relaxation, const btContactSolverInfo& infoGlobal, btScalar desiredVelocity=0., btScalar cfmSlip=0.);
|
||||
btSolverConstraint& addTorsionalFrictionConstraint(const btVector3& normalAxis,int solverBodyIdA,int solverBodyIdB,int frictionIndex,btManifoldPoint& cp,btScalar torsionalFriction, const btVector3& rel_pos1,const btVector3& rel_pos2,btCollisionObject* colObj0,btCollisionObject* colObj1, btScalar relaxation, btScalar desiredVelocity=0, btScalar cfmSlip=0.f);
|
||||
btSolverConstraint& addFrictionConstraint(const btVector3& normalAxis, int solverBodyIdA, int solverBodyIdB, int frictionIndex, btManifoldPoint& cp, const btVector3& rel_pos1, const btVector3& rel_pos2, btCollisionObject* colObj0, btCollisionObject* colObj1, btScalar relaxation, const btContactSolverInfo& infoGlobal, btScalar desiredVelocity = 0., btScalar cfmSlip = 0.);
|
||||
btSolverConstraint& addTorsionalFrictionConstraint(const btVector3& normalAxis, int solverBodyIdA, int solverBodyIdB, int frictionIndex, btManifoldPoint& cp, btScalar torsionalFriction, const btVector3& rel_pos1, const btVector3& rel_pos2, btCollisionObject* colObj0, btCollisionObject* colObj1, btScalar relaxation, btScalar desiredVelocity = 0, btScalar cfmSlip = 0.f);
|
||||
|
||||
void setupContactConstraint(btSolverConstraint & solverConstraint, int solverBodyIdA, int solverBodyIdB, btManifoldPoint& cp,
|
||||
const btContactSolverInfo& infoGlobal, btScalar& relaxation, const btVector3& rel_pos1, const btVector3& rel_pos2);
|
||||
|
||||
void setupContactConstraint(btSolverConstraint& solverConstraint, int solverBodyIdA, int solverBodyIdB, btManifoldPoint& cp,
|
||||
const btContactSolverInfo& infoGlobal,btScalar& relaxation, const btVector3& rel_pos1, const btVector3& rel_pos2);
|
||||
static void applyAnisotropicFriction(btCollisionObject * colObj, btVector3 & frictionDirection, int frictionMode);
|
||||
|
||||
static void applyAnisotropicFriction(btCollisionObject* colObj,btVector3& frictionDirection, int frictionMode);
|
||||
|
||||
void setFrictionConstraintImpulse( btSolverConstraint& solverConstraint, int solverBodyIdA,int solverBodyIdB,
|
||||
btManifoldPoint& cp, const btContactSolverInfo& infoGlobal);
|
||||
void setFrictionConstraintImpulse(btSolverConstraint & solverConstraint, int solverBodyIdA, int solverBodyIdB,
|
||||
btManifoldPoint& cp, const btContactSolverInfo& infoGlobal);
|
||||
|
||||
///m_btSeed2 is used for re-arranging the constraint rows. improves convergence/quality of friction
|
||||
unsigned long m_btSeed2;
|
||||
|
||||
unsigned long m_btSeed2;
|
||||
|
||||
btScalar restitutionCurve(btScalar rel_vel, btScalar restitution, btScalar velocityThreshold);
|
||||
|
||||
virtual void convertContacts(btPersistentManifold** manifoldPtr, int numManifolds, const btContactSolverInfo& infoGlobal);
|
||||
virtual void convertContacts(btPersistentManifold * *manifoldPtr, int numManifolds, const btContactSolverInfo& infoGlobal);
|
||||
|
||||
void convertContact(btPersistentManifold* manifold,const btContactSolverInfo& infoGlobal);
|
||||
void convertContact(btPersistentManifold * manifold, const btContactSolverInfo& infoGlobal);
|
||||
|
||||
virtual void convertJoints(btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& infoGlobal);
|
||||
void convertJoint(btSolverConstraint* currentConstraintRow, btTypedConstraint* constraint, const btTypedConstraint::btConstraintInfo1& info1, int solverBodyIdA, int solverBodyIdB, const btContactSolverInfo& infoGlobal);
|
||||
virtual void convertJoints(btTypedConstraint * *constraints, int numConstraints, const btContactSolverInfo& infoGlobal);
|
||||
void convertJoint(btSolverConstraint * currentConstraintRow, btTypedConstraint * constraint, const btTypedConstraint::btConstraintInfo1& info1, int solverBodyIdA, int solverBodyIdB, const btContactSolverInfo& infoGlobal);
|
||||
|
||||
virtual void convertBodies(btCollisionObject** bodies, int numBodies, const btContactSolverInfo& infoGlobal);
|
||||
virtual void convertBodies(btCollisionObject * *bodies, int numBodies, const btContactSolverInfo& infoGlobal);
|
||||
|
||||
btScalar resolveSplitPenetrationSIMD(btSolverBody& bodyA,btSolverBody& bodyB, const btSolverConstraint& contactConstraint)
|
||||
{
|
||||
return m_resolveSplitPenetrationImpulse( bodyA, bodyB, contactConstraint );
|
||||
}
|
||||
btScalar resolveSplitPenetrationSIMD(btSolverBody & bodyA, btSolverBody & bodyB, const btSolverConstraint& contactConstraint)
|
||||
{
|
||||
return m_resolveSplitPenetrationImpulse(bodyA, bodyB, contactConstraint);
|
||||
}
|
||||
|
||||
btScalar resolveSplitPenetrationImpulseCacheFriendly(btSolverBody& bodyA,btSolverBody& bodyB, const btSolverConstraint& contactConstraint)
|
||||
{
|
||||
return m_resolveSplitPenetrationImpulse( bodyA, bodyB, contactConstraint );
|
||||
}
|
||||
btScalar resolveSplitPenetrationImpulseCacheFriendly(btSolverBody & bodyA, btSolverBody & bodyB, const btSolverConstraint& contactConstraint)
|
||||
{
|
||||
return m_resolveSplitPenetrationImpulse(bodyA, bodyB, contactConstraint);
|
||||
}
|
||||
|
||||
//internal method
|
||||
int getOrInitSolverBody(btCollisionObject& body,btScalar timeStep);
|
||||
void initSolverBody(btSolverBody* solverBody, btCollisionObject* collisionObject, btScalar timeStep);
|
||||
int getOrInitSolverBody(btCollisionObject & body, btScalar timeStep);
|
||||
void initSolverBody(btSolverBody * solverBody, btCollisionObject * collisionObject, btScalar timeStep);
|
||||
|
||||
btScalar resolveSingleConstraintRowGeneric(btSolverBody& bodyA,btSolverBody& bodyB,const btSolverConstraint& contactConstraint);
|
||||
btScalar resolveSingleConstraintRowGenericSIMD(btSolverBody& bodyA,btSolverBody& bodyB,const btSolverConstraint& contactConstraint);
|
||||
btScalar resolveSingleConstraintRowLowerLimit(btSolverBody& bodyA,btSolverBody& bodyB,const btSolverConstraint& contactConstraint);
|
||||
btScalar resolveSingleConstraintRowLowerLimitSIMD(btSolverBody& bodyA,btSolverBody& bodyB,const btSolverConstraint& contactConstraint);
|
||||
btScalar resolveSplitPenetrationImpulse(btSolverBody& bodyA,btSolverBody& bodyB, const btSolverConstraint& contactConstraint)
|
||||
{
|
||||
return m_resolveSplitPenetrationImpulse( bodyA, bodyB, contactConstraint );
|
||||
}
|
||||
btScalar resolveSingleConstraintRowGeneric(btSolverBody & bodyA, btSolverBody & bodyB, const btSolverConstraint& contactConstraint);
|
||||
btScalar resolveSingleConstraintRowGenericSIMD(btSolverBody & bodyA, btSolverBody & bodyB, const btSolverConstraint& contactConstraint);
|
||||
btScalar resolveSingleConstraintRowLowerLimit(btSolverBody & bodyA, btSolverBody & bodyB, const btSolverConstraint& contactConstraint);
|
||||
btScalar resolveSingleConstraintRowLowerLimitSIMD(btSolverBody & bodyA, btSolverBody & bodyB, const btSolverConstraint& contactConstraint);
|
||||
btScalar resolveSplitPenetrationImpulse(btSolverBody & bodyA, btSolverBody & bodyB, const btSolverConstraint& contactConstraint)
|
||||
{
|
||||
return m_resolveSplitPenetrationImpulse(bodyA, bodyB, contactConstraint);
|
||||
}
|
||||
|
||||
protected:
|
||||
void writeBackContacts(int iBegin, int iEnd, const btContactSolverInfo& infoGlobal);
|
||||
void writeBackJoints(int iBegin, int iEnd, const btContactSolverInfo& infoGlobal);
|
||||
void writeBackBodies(int iBegin, int iEnd, const btContactSolverInfo& infoGlobal);
|
||||
virtual void solveGroupCacheFriendlySplitImpulseIterations(btCollisionObject * *bodies, int numBodies, btPersistentManifold** manifoldPtr, int numManifolds, btTypedConstraint** constraints, int numConstraints, const btContactSolverInfo& infoGlobal, btIDebugDraw* debugDrawer);
|
||||
virtual btScalar solveGroupCacheFriendlyFinish(btCollisionObject * *bodies, int numBodies, const btContactSolverInfo& infoGlobal);
|
||||
virtual btScalar solveSingleIteration(int iteration, btCollisionObject** bodies, int numBodies, btPersistentManifold** manifoldPtr, int numManifolds, btTypedConstraint** constraints, int numConstraints, const btContactSolverInfo& infoGlobal, btIDebugDraw* debugDrawer);
|
||||
|
||||
void writeBackContacts(int iBegin, int iEnd, const btContactSolverInfo& infoGlobal);
|
||||
void writeBackJoints(int iBegin, int iEnd, const btContactSolverInfo& infoGlobal);
|
||||
void writeBackBodies(int iBegin, int iEnd, const btContactSolverInfo& infoGlobal);
|
||||
virtual void solveGroupCacheFriendlySplitImpulseIterations(btCollisionObject** bodies,int numBodies,btPersistentManifold** manifoldPtr, int numManifolds,btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& infoGlobal,btIDebugDraw* debugDrawer);
|
||||
virtual btScalar solveGroupCacheFriendlyFinish(btCollisionObject** bodies,int numBodies,const btContactSolverInfo& infoGlobal);
|
||||
virtual btScalar solveSingleIteration(int iteration, btCollisionObject** bodies ,int numBodies,btPersistentManifold** manifoldPtr, int numManifolds,btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& infoGlobal,btIDebugDraw* debugDrawer);
|
||||
|
||||
virtual btScalar solveGroupCacheFriendlySetup(btCollisionObject** bodies,int numBodies,btPersistentManifold** manifoldPtr, int numManifolds,btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& infoGlobal,btIDebugDraw* debugDrawer);
|
||||
virtual btScalar solveGroupCacheFriendlyIterations(btCollisionObject** bodies,int numBodies,btPersistentManifold** manifoldPtr, int numManifolds,btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& infoGlobal,btIDebugDraw* debugDrawer);
|
||||
|
||||
virtual btScalar solveGroupCacheFriendlySetup(btCollisionObject * *bodies, int numBodies, btPersistentManifold** manifoldPtr, int numManifolds, btTypedConstraint** constraints, int numConstraints, const btContactSolverInfo& infoGlobal, btIDebugDraw* debugDrawer);
|
||||
virtual btScalar solveGroupCacheFriendlyIterations(btCollisionObject * *bodies, int numBodies, btPersistentManifold** manifoldPtr, int numManifolds, btTypedConstraint** constraints, int numConstraints, const btContactSolverInfo& infoGlobal, btIDebugDraw* debugDrawer);
|
||||
|
||||
public:
|
||||
|
||||
BT_DECLARE_ALIGNED_ALLOCATOR();
|
||||
|
||||
btSequentialImpulseConstraintSolver();
|
||||
virtual ~btSequentialImpulseConstraintSolver();
|
||||
|
||||
virtual btScalar solveGroup(btCollisionObject** bodies,int numBodies,btPersistentManifold** manifold,int numManifolds,btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& info, btIDebugDraw* debugDrawer,btDispatcher* dispatcher);
|
||||
virtual btScalar solveGroup(btCollisionObject * *bodies, int numBodies, btPersistentManifold** manifold, int numManifolds, btTypedConstraint** constraints, int numConstraints, const btContactSolverInfo& info, btIDebugDraw* debugDrawer, btDispatcher* dispatcher);
|
||||
|
||||
///clear internal cached data and reset random seed
|
||||
virtual void reset();
|
||||
virtual void reset();
|
||||
|
||||
unsigned long btRand2();
|
||||
|
||||
int btRandInt2 (int n);
|
||||
int btRandInt2(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;
|
||||
}
|
||||
|
||||
|
||||
virtual btConstraintSolverType getSolverType() const
|
||||
virtual btConstraintSolverType getSolverType() const
|
||||
{
|
||||
return BT_SEQUENTIAL_IMPULSE_SOLVER;
|
||||
}
|
||||
|
||||
btSingleConstraintRowSolver getActiveConstraintRowSolverGeneric()
|
||||
btSingleConstraintRowSolver getActiveConstraintRowSolverGeneric()
|
||||
{
|
||||
return m_resolveSingleConstraintRowGeneric;
|
||||
}
|
||||
@@ -175,7 +170,7 @@ public:
|
||||
{
|
||||
m_resolveSingleConstraintRowGeneric = rowSolver;
|
||||
}
|
||||
btSingleConstraintRowSolver getActiveConstraintRowSolverLowerLimit()
|
||||
btSingleConstraintRowSolver getActiveConstraintRowSolverLowerLimit()
|
||||
{
|
||||
return m_resolveSingleConstraintRowLowerLimit;
|
||||
}
|
||||
@@ -185,18 +180,14 @@ public:
|
||||
}
|
||||
|
||||
///Various implementations of solving a single constraint row using a generic equality constraint, using scalar reference, SSE2 or SSE4
|
||||
btSingleConstraintRowSolver getScalarConstraintRowSolverGeneric();
|
||||
btSingleConstraintRowSolver getSSE2ConstraintRowSolverGeneric();
|
||||
btSingleConstraintRowSolver getSSE4_1ConstraintRowSolverGeneric();
|
||||
btSingleConstraintRowSolver getScalarConstraintRowSolverGeneric();
|
||||
btSingleConstraintRowSolver getSSE2ConstraintRowSolverGeneric();
|
||||
btSingleConstraintRowSolver getSSE4_1ConstraintRowSolverGeneric();
|
||||
|
||||
///Various implementations of solving a single constraint row using an inequality (lower limit) constraint, using scalar reference, SSE2 or SSE4
|
||||
btSingleConstraintRowSolver getScalarConstraintRowSolverLowerLimit();
|
||||
btSingleConstraintRowSolver getSSE2ConstraintRowSolverLowerLimit();
|
||||
btSingleConstraintRowSolver getSSE4_1ConstraintRowSolverLowerLimit();
|
||||
btSingleConstraintRowSolver getScalarConstraintRowSolverLowerLimit();
|
||||
btSingleConstraintRowSolver getSSE2ConstraintRowSolverLowerLimit();
|
||||
btSingleConstraintRowSolver getSSE4_1ConstraintRowSolverLowerLimit();
|
||||
};
|
||||
|
||||
|
||||
|
||||
|
||||
#endif //BT_SEQUENTIAL_IMPULSE_CONSTRAINT_SOLVER_H
|
||||
|
||||
#endif //BT_SEQUENTIAL_IMPULSE_CONSTRAINT_SOLVER_H
|
||||
|
||||
File diff suppressed because it is too large
Load Diff
@@ -53,102 +53,98 @@ subject to the following restrictions:
|
||||
/// because floating point addition is not associative due to rounding errors.
|
||||
/// The task scheduler can and should ensure that the result of any parallelSum operation is deterministic.
|
||||
///
|
||||
ATTRIBUTE_ALIGNED16(class) btSequentialImpulseConstraintSolverMt : public btSequentialImpulseConstraintSolver
|
||||
ATTRIBUTE_ALIGNED16(class)
|
||||
btSequentialImpulseConstraintSolverMt : public btSequentialImpulseConstraintSolver
|
||||
{
|
||||
public:
|
||||
virtual void solveGroupCacheFriendlySplitImpulseIterations(btCollisionObject** bodies,int numBodies,btPersistentManifold** manifoldPtr, int numManifolds,btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& infoGlobal,btIDebugDraw* debugDrawer) BT_OVERRIDE;
|
||||
virtual btScalar solveSingleIteration(int iteration, btCollisionObject** bodies ,int numBodies,btPersistentManifold** manifoldPtr, int numManifolds,btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& infoGlobal,btIDebugDraw* debugDrawer) BT_OVERRIDE;
|
||||
virtual btScalar solveGroupCacheFriendlySetup(btCollisionObject** bodies,int numBodies,btPersistentManifold** manifoldPtr, int numManifolds,btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& infoGlobal,btIDebugDraw* debugDrawer) BT_OVERRIDE;
|
||||
virtual btScalar solveGroupCacheFriendlyFinish(btCollisionObject** bodies, int numBodies, const btContactSolverInfo& infoGlobal) BT_OVERRIDE;
|
||||
virtual void solveGroupCacheFriendlySplitImpulseIterations(btCollisionObject * *bodies, int numBodies, btPersistentManifold** manifoldPtr, int numManifolds, btTypedConstraint** constraints, int numConstraints, const btContactSolverInfo& infoGlobal, btIDebugDraw* debugDrawer) BT_OVERRIDE;
|
||||
virtual btScalar solveSingleIteration(int iteration, btCollisionObject** bodies, int numBodies, btPersistentManifold** manifoldPtr, int numManifolds, btTypedConstraint** constraints, int numConstraints, const btContactSolverInfo& infoGlobal, btIDebugDraw* debugDrawer) BT_OVERRIDE;
|
||||
virtual btScalar solveGroupCacheFriendlySetup(btCollisionObject * *bodies, int numBodies, btPersistentManifold** manifoldPtr, int numManifolds, btTypedConstraint** constraints, int numConstraints, const btContactSolverInfo& infoGlobal, btIDebugDraw* debugDrawer) BT_OVERRIDE;
|
||||
virtual btScalar solveGroupCacheFriendlyFinish(btCollisionObject * *bodies, int numBodies, const btContactSolverInfo& infoGlobal) BT_OVERRIDE;
|
||||
|
||||
// temp struct used to collect info from persistent manifolds into a cache-friendly struct using multiple threads
|
||||
struct btContactManifoldCachedInfo
|
||||
{
|
||||
static const int MAX_NUM_CONTACT_POINTS = 4;
|
||||
// temp struct used to collect info from persistent manifolds into a cache-friendly struct using multiple threads
|
||||
struct btContactManifoldCachedInfo
|
||||
{
|
||||
static const int MAX_NUM_CONTACT_POINTS = 4;
|
||||
|
||||
int numTouchingContacts;
|
||||
int solverBodyIds[ 2 ];
|
||||
int contactIndex;
|
||||
int rollingFrictionIndex;
|
||||
bool contactHasRollingFriction[ MAX_NUM_CONTACT_POINTS ];
|
||||
btManifoldPoint* contactPoints[ MAX_NUM_CONTACT_POINTS ];
|
||||
};
|
||||
// temp struct used for setting up joint constraints in parallel
|
||||
struct JointParams
|
||||
{
|
||||
int m_solverConstraint;
|
||||
int m_solverBodyA;
|
||||
int m_solverBodyB;
|
||||
};
|
||||
void internalInitMultipleJoints(btTypedConstraint** constraints, int iBegin, int iEnd);
|
||||
void internalConvertMultipleJoints( const btAlignedObjectArray<JointParams>& jointParamsArray, btTypedConstraint** constraints, int iBegin, int iEnd, const btContactSolverInfo& infoGlobal );
|
||||
int numTouchingContacts;
|
||||
int solverBodyIds[2];
|
||||
int contactIndex;
|
||||
int rollingFrictionIndex;
|
||||
bool contactHasRollingFriction[MAX_NUM_CONTACT_POINTS];
|
||||
btManifoldPoint* contactPoints[MAX_NUM_CONTACT_POINTS];
|
||||
};
|
||||
// temp struct used for setting up joint constraints in parallel
|
||||
struct JointParams
|
||||
{
|
||||
int m_solverConstraint;
|
||||
int m_solverBodyA;
|
||||
int m_solverBodyB;
|
||||
};
|
||||
void internalInitMultipleJoints(btTypedConstraint * *constraints, int iBegin, int iEnd);
|
||||
void internalConvertMultipleJoints(const btAlignedObjectArray<JointParams>& jointParamsArray, btTypedConstraint** constraints, int iBegin, int iEnd, const btContactSolverInfo& infoGlobal);
|
||||
|
||||
// parameters to control batching
|
||||
static bool s_allowNestedParallelForLoops; // whether to allow nested parallel operations
|
||||
static int s_minimumContactManifoldsForBatching; // don't even try to batch if fewer manifolds than this
|
||||
static btBatchedConstraints::BatchingMethod s_contactBatchingMethod;
|
||||
static btBatchedConstraints::BatchingMethod s_jointBatchingMethod;
|
||||
static int s_minBatchSize; // desired number of constraints per batch
|
||||
static int s_maxBatchSize;
|
||||
// parameters to control batching
|
||||
static bool s_allowNestedParallelForLoops; // whether to allow nested parallel operations
|
||||
static int s_minimumContactManifoldsForBatching; // don't even try to batch if fewer manifolds than this
|
||||
static btBatchedConstraints::BatchingMethod s_contactBatchingMethod;
|
||||
static btBatchedConstraints::BatchingMethod s_jointBatchingMethod;
|
||||
static int s_minBatchSize; // desired number of constraints per batch
|
||||
static int s_maxBatchSize;
|
||||
|
||||
protected:
|
||||
static const int CACHE_LINE_SIZE = 64;
|
||||
static const int CACHE_LINE_SIZE = 64;
|
||||
|
||||
btBatchedConstraints m_batchedContactConstraints;
|
||||
btBatchedConstraints m_batchedJointConstraints;
|
||||
int m_numFrictionDirections;
|
||||
bool m_useBatching;
|
||||
bool m_useObsoleteJointConstraints;
|
||||
btAlignedObjectArray<btContactManifoldCachedInfo> m_manifoldCachedInfoArray;
|
||||
btAlignedObjectArray<int> m_rollingFrictionIndexTable; // lookup table mapping contact index to rolling friction index
|
||||
btSpinMutex m_bodySolverArrayMutex;
|
||||
char m_antiFalseSharingPadding[CACHE_LINE_SIZE]; // padding to keep mutexes in separate cachelines
|
||||
btSpinMutex m_kinematicBodyUniqueIdToSolverBodyTableMutex;
|
||||
btAlignedObjectArray<char> m_scratchMemory;
|
||||
btBatchedConstraints m_batchedContactConstraints;
|
||||
btBatchedConstraints m_batchedJointConstraints;
|
||||
int m_numFrictionDirections;
|
||||
bool m_useBatching;
|
||||
bool m_useObsoleteJointConstraints;
|
||||
btAlignedObjectArray<btContactManifoldCachedInfo> m_manifoldCachedInfoArray;
|
||||
btAlignedObjectArray<int> m_rollingFrictionIndexTable; // lookup table mapping contact index to rolling friction index
|
||||
btSpinMutex m_bodySolverArrayMutex;
|
||||
char m_antiFalseSharingPadding[CACHE_LINE_SIZE]; // padding to keep mutexes in separate cachelines
|
||||
btSpinMutex m_kinematicBodyUniqueIdToSolverBodyTableMutex;
|
||||
btAlignedObjectArray<char> m_scratchMemory;
|
||||
|
||||
virtual void randomizeConstraintOrdering( int iteration, int numIterations );
|
||||
virtual btScalar resolveAllJointConstraints( int iteration );
|
||||
virtual btScalar resolveAllContactConstraints();
|
||||
virtual btScalar resolveAllContactFrictionConstraints();
|
||||
virtual btScalar resolveAllContactConstraintsInterleaved();
|
||||
virtual btScalar resolveAllRollingFrictionConstraints();
|
||||
virtual void randomizeConstraintOrdering(int iteration, int numIterations);
|
||||
virtual btScalar resolveAllJointConstraints(int iteration);
|
||||
virtual btScalar resolveAllContactConstraints();
|
||||
virtual btScalar resolveAllContactFrictionConstraints();
|
||||
virtual btScalar resolveAllContactConstraintsInterleaved();
|
||||
virtual btScalar resolveAllRollingFrictionConstraints();
|
||||
|
||||
virtual void setupBatchedContactConstraints();
|
||||
virtual void setupBatchedJointConstraints();
|
||||
virtual void convertJoints(btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& infoGlobal) BT_OVERRIDE;
|
||||
virtual void convertContacts(btPersistentManifold** manifoldPtr, int numManifolds, const btContactSolverInfo& infoGlobal) BT_OVERRIDE;
|
||||
virtual void convertBodies(btCollisionObject** bodies, int numBodies, const btContactSolverInfo& infoGlobal) BT_OVERRIDE;
|
||||
virtual void setupBatchedContactConstraints();
|
||||
virtual void setupBatchedJointConstraints();
|
||||
virtual void convertJoints(btTypedConstraint * *constraints, int numConstraints, const btContactSolverInfo& infoGlobal) BT_OVERRIDE;
|
||||
virtual void convertContacts(btPersistentManifold * *manifoldPtr, int numManifolds, const btContactSolverInfo& infoGlobal) BT_OVERRIDE;
|
||||
virtual void convertBodies(btCollisionObject * *bodies, int numBodies, const btContactSolverInfo& infoGlobal) BT_OVERRIDE;
|
||||
|
||||
int getOrInitSolverBodyThreadsafe(btCollisionObject& body, btScalar timeStep);
|
||||
void allocAllContactConstraints(btPersistentManifold** manifoldPtr, int numManifolds, const btContactSolverInfo& infoGlobal);
|
||||
void setupAllContactConstraints(const btContactSolverInfo& infoGlobal);
|
||||
void randomizeBatchedConstraintOrdering( btBatchedConstraints* batchedConstraints );
|
||||
int getOrInitSolverBodyThreadsafe(btCollisionObject & body, btScalar timeStep);
|
||||
void allocAllContactConstraints(btPersistentManifold * *manifoldPtr, int numManifolds, const btContactSolverInfo& infoGlobal);
|
||||
void setupAllContactConstraints(const btContactSolverInfo& infoGlobal);
|
||||
void randomizeBatchedConstraintOrdering(btBatchedConstraints * batchedConstraints);
|
||||
|
||||
public:
|
||||
|
||||
BT_DECLARE_ALIGNED_ALLOCATOR();
|
||||
|
||||
btSequentialImpulseConstraintSolverMt();
|
||||
virtual ~btSequentialImpulseConstraintSolverMt();
|
||||
|
||||
btScalar resolveMultipleJointConstraints( const btAlignedObjectArray<int>& consIndices, int batchBegin, int batchEnd, int iteration );
|
||||
btScalar resolveMultipleContactConstraints( const btAlignedObjectArray<int>& consIndices, int batchBegin, int batchEnd );
|
||||
btScalar resolveMultipleContactSplitPenetrationImpulseConstraints( const btAlignedObjectArray<int>& consIndices, int batchBegin, int batchEnd );
|
||||
btScalar resolveMultipleContactFrictionConstraints( const btAlignedObjectArray<int>& consIndices, int batchBegin, int batchEnd );
|
||||
btScalar resolveMultipleContactRollingFrictionConstraints( const btAlignedObjectArray<int>& consIndices, int batchBegin, int batchEnd );
|
||||
btScalar resolveMultipleContactConstraintsInterleaved( const btAlignedObjectArray<int>& contactIndices, int batchBegin, int batchEnd );
|
||||
btScalar resolveMultipleJointConstraints(const btAlignedObjectArray<int>& consIndices, int batchBegin, int batchEnd, int iteration);
|
||||
btScalar resolveMultipleContactConstraints(const btAlignedObjectArray<int>& consIndices, int batchBegin, int batchEnd);
|
||||
btScalar resolveMultipleContactSplitPenetrationImpulseConstraints(const btAlignedObjectArray<int>& consIndices, int batchBegin, int batchEnd);
|
||||
btScalar resolveMultipleContactFrictionConstraints(const btAlignedObjectArray<int>& consIndices, int batchBegin, int batchEnd);
|
||||
btScalar resolveMultipleContactRollingFrictionConstraints(const btAlignedObjectArray<int>& consIndices, int batchBegin, int batchEnd);
|
||||
btScalar resolveMultipleContactConstraintsInterleaved(const btAlignedObjectArray<int>& contactIndices, int batchBegin, int batchEnd);
|
||||
|
||||
void internalCollectContactManifoldCachedInfo(btContactManifoldCachedInfo* cachedInfoArray, btPersistentManifold** manifoldPtr, int numManifolds, const btContactSolverInfo& infoGlobal);
|
||||
void internalAllocContactConstraints(const btContactManifoldCachedInfo* cachedInfoArray, int numManifolds);
|
||||
void internalSetupContactConstraints(int iContactConstraint, const btContactSolverInfo& infoGlobal);
|
||||
void internalConvertBodies(btCollisionObject** bodies, int iBegin, int iEnd, const btContactSolverInfo& infoGlobal);
|
||||
void internalWriteBackContacts(int iBegin, int iEnd, const btContactSolverInfo& infoGlobal);
|
||||
void internalWriteBackJoints(int iBegin, int iEnd, const btContactSolverInfo& infoGlobal);
|
||||
void internalWriteBackBodies(int iBegin, int iEnd, const btContactSolverInfo& infoGlobal);
|
||||
void internalCollectContactManifoldCachedInfo(btContactManifoldCachedInfo * cachedInfoArray, btPersistentManifold * *manifoldPtr, int numManifolds, const btContactSolverInfo& infoGlobal);
|
||||
void internalAllocContactConstraints(const btContactManifoldCachedInfo* cachedInfoArray, int numManifolds);
|
||||
void internalSetupContactConstraints(int iContactConstraint, const btContactSolverInfo& infoGlobal);
|
||||
void internalConvertBodies(btCollisionObject * *bodies, int iBegin, int iEnd, const btContactSolverInfo& infoGlobal);
|
||||
void internalWriteBackContacts(int iBegin, int iEnd, const btContactSolverInfo& infoGlobal);
|
||||
void internalWriteBackJoints(int iBegin, int iEnd, const btContactSolverInfo& infoGlobal);
|
||||
void internalWriteBackBodies(int iBegin, int iEnd, const btContactSolverInfo& infoGlobal);
|
||||
};
|
||||
|
||||
|
||||
|
||||
|
||||
#endif //BT_SEQUENTIAL_IMPULSE_CONSTRAINT_SOLVER_MT_H
|
||||
|
||||
#endif //BT_SEQUENTIAL_IMPULSE_CONSTRAINT_SOLVER_MT_H
|
||||
|
||||
File diff suppressed because it is too large
Load Diff
@@ -25,31 +25,26 @@ TODO:
|
||||
#ifndef BT_SLIDER_CONSTRAINT_H
|
||||
#define BT_SLIDER_CONSTRAINT_H
|
||||
|
||||
#include "LinearMath/btScalar.h"//for BT_USE_DOUBLE_PRECISION
|
||||
#include "LinearMath/btScalar.h" //for BT_USE_DOUBLE_PRECISION
|
||||
|
||||
#ifdef BT_USE_DOUBLE_PRECISION
|
||||
#define btSliderConstraintData2 btSliderConstraintDoubleData
|
||||
#define btSliderConstraintDataName "btSliderConstraintDoubleData"
|
||||
#define btSliderConstraintData2 btSliderConstraintDoubleData
|
||||
#define btSliderConstraintDataName "btSliderConstraintDoubleData"
|
||||
#else
|
||||
#define btSliderConstraintData2 btSliderConstraintData
|
||||
#define btSliderConstraintDataName "btSliderConstraintData"
|
||||
#endif //BT_USE_DOUBLE_PRECISION
|
||||
#define btSliderConstraintData2 btSliderConstraintData
|
||||
#define btSliderConstraintDataName "btSliderConstraintData"
|
||||
#endif //BT_USE_DOUBLE_PRECISION
|
||||
|
||||
#include "LinearMath/btVector3.h"
|
||||
#include "btJacobianEntry.h"
|
||||
#include "btTypedConstraint.h"
|
||||
|
||||
|
||||
|
||||
class btRigidBody;
|
||||
|
||||
|
||||
|
||||
#define SLIDER_CONSTRAINT_DEF_SOFTNESS (btScalar(1.0))
|
||||
#define SLIDER_CONSTRAINT_DEF_DAMPING (btScalar(1.0))
|
||||
#define SLIDER_CONSTRAINT_DEF_RESTITUTION (btScalar(0.7))
|
||||
#define SLIDER_CONSTRAINT_DEF_CFM (btScalar(0.f))
|
||||
|
||||
#define SLIDER_CONSTRAINT_DEF_SOFTNESS (btScalar(1.0))
|
||||
#define SLIDER_CONSTRAINT_DEF_DAMPING (btScalar(1.0))
|
||||
#define SLIDER_CONSTRAINT_DEF_RESTITUTION (btScalar(0.7))
|
||||
#define SLIDER_CONSTRAINT_DEF_CFM (btScalar(0.f))
|
||||
|
||||
enum btSliderFlags
|
||||
{
|
||||
@@ -67,15 +62,15 @@ enum btSliderFlags
|
||||
BT_SLIDER_FLAGS_ERP_LIMANG = (1 << 11)
|
||||
};
|
||||
|
||||
|
||||
ATTRIBUTE_ALIGNED16(class) btSliderConstraint : public btTypedConstraint
|
||||
ATTRIBUTE_ALIGNED16(class)
|
||||
btSliderConstraint : public btTypedConstraint
|
||||
{
|
||||
protected:
|
||||
///for backwards compatibility during the transition to 'getInfo/getInfo2'
|
||||
bool m_useSolveConstraintObsolete;
|
||||
bool m_useOffsetForConstraintFrame;
|
||||
btTransform m_frameInA;
|
||||
btTransform m_frameInB;
|
||||
bool m_useSolveConstraintObsolete;
|
||||
bool m_useOffsetForConstraintFrame;
|
||||
btTransform m_frameInA;
|
||||
btTransform m_frameInB;
|
||||
// use frameA fo define limits, if true
|
||||
bool m_useLinearReferenceFrameA;
|
||||
// linear limits
|
||||
@@ -119,21 +114,21 @@ protected:
|
||||
btScalar m_restitutionOrthoAng;
|
||||
btScalar m_dampingOrthoAng;
|
||||
btScalar m_cfmOrthoAng;
|
||||
|
||||
|
||||
// for interlal use
|
||||
bool m_solveLinLim;
|
||||
bool m_solveAngLim;
|
||||
|
||||
int m_flags;
|
||||
|
||||
btJacobianEntry m_jacLin[3];
|
||||
btScalar m_jacLinDiagABInv[3];
|
||||
btJacobianEntry m_jacLin[3];
|
||||
btScalar m_jacLinDiagABInv[3];
|
||||
|
||||
btJacobianEntry m_jacAng[3];
|
||||
btJacobianEntry m_jacAng[3];
|
||||
|
||||
btScalar m_timeStep;
|
||||
btTransform m_calculatedTransformA;
|
||||
btTransform m_calculatedTransformB;
|
||||
btTransform m_calculatedTransformA;
|
||||
btTransform m_calculatedTransformB;
|
||||
|
||||
btVector3 m_sliderAxis;
|
||||
btVector3 m_realPivotAInW;
|
||||
@@ -150,57 +145,57 @@ protected:
|
||||
btScalar m_angDepth;
|
||||
btScalar m_kAngle;
|
||||
|
||||
bool m_poweredLinMotor;
|
||||
btScalar m_targetLinMotorVelocity;
|
||||
btScalar m_maxLinMotorForce;
|
||||
btScalar m_accumulatedLinMotorImpulse;
|
||||
|
||||
bool m_poweredAngMotor;
|
||||
btScalar m_targetAngMotorVelocity;
|
||||
btScalar m_maxAngMotorForce;
|
||||
btScalar m_accumulatedAngMotorImpulse;
|
||||
bool m_poweredLinMotor;
|
||||
btScalar m_targetLinMotorVelocity;
|
||||
btScalar m_maxLinMotorForce;
|
||||
btScalar m_accumulatedLinMotorImpulse;
|
||||
|
||||
//------------------------
|
||||
bool m_poweredAngMotor;
|
||||
btScalar m_targetAngMotorVelocity;
|
||||
btScalar m_maxAngMotorForce;
|
||||
btScalar m_accumulatedAngMotorImpulse;
|
||||
|
||||
//------------------------
|
||||
void initParams();
|
||||
|
||||
public:
|
||||
BT_DECLARE_ALIGNED_ALLOCATOR();
|
||||
|
||||
|
||||
// constructors
|
||||
btSliderConstraint(btRigidBody& rbA, btRigidBody& rbB, const btTransform& frameInA, const btTransform& frameInB ,bool useLinearReferenceFrameA);
|
||||
btSliderConstraint(btRigidBody& rbB, const btTransform& frameInB, bool useLinearReferenceFrameA);
|
||||
btSliderConstraint(btRigidBody & rbA, btRigidBody & rbB, const btTransform& frameInA, const btTransform& frameInB, bool useLinearReferenceFrameA);
|
||||
btSliderConstraint(btRigidBody & rbB, const btTransform& frameInB, bool useLinearReferenceFrameA);
|
||||
|
||||
// overrides
|
||||
|
||||
virtual void getInfo1 (btConstraintInfo1* info);
|
||||
virtual void getInfo1(btConstraintInfo1 * info);
|
||||
|
||||
void getInfo1NonVirtual(btConstraintInfo1* info);
|
||||
|
||||
virtual void getInfo2 (btConstraintInfo2* info);
|
||||
void getInfo1NonVirtual(btConstraintInfo1 * info);
|
||||
|
||||
void getInfo2NonVirtual(btConstraintInfo2* info, const btTransform& transA, const btTransform& transB,const btVector3& linVelA,const btVector3& linVelB, btScalar rbAinvMass,btScalar rbBinvMass);
|
||||
virtual void getInfo2(btConstraintInfo2 * info);
|
||||
|
||||
void getInfo2NonVirtual(btConstraintInfo2 * info, const btTransform& transA, const btTransform& transB, const btVector3& linVelA, const btVector3& linVelB, btScalar rbAinvMass, btScalar rbBinvMass);
|
||||
|
||||
// access
|
||||
const btRigidBody& getRigidBodyA() const { return m_rbA; }
|
||||
const btRigidBody& getRigidBodyB() const { return m_rbB; }
|
||||
const btTransform & getCalculatedTransformA() const { return m_calculatedTransformA; }
|
||||
const btTransform & getCalculatedTransformB() const { return m_calculatedTransformB; }
|
||||
const btTransform & getFrameOffsetA() const { return m_frameInA; }
|
||||
const btTransform & getFrameOffsetB() const { return m_frameInB; }
|
||||
btTransform & getFrameOffsetA() { return m_frameInA; }
|
||||
btTransform & getFrameOffsetB() { return m_frameInB; }
|
||||
btScalar getLowerLinLimit() { return m_lowerLinLimit; }
|
||||
void setLowerLinLimit(btScalar lowerLimit) { m_lowerLinLimit = lowerLimit; }
|
||||
btScalar getUpperLinLimit() { return m_upperLinLimit; }
|
||||
void setUpperLinLimit(btScalar upperLimit) { m_upperLinLimit = upperLimit; }
|
||||
btScalar getLowerAngLimit() { return m_lowerAngLimit; }
|
||||
void setLowerAngLimit(btScalar lowerLimit) { m_lowerAngLimit = btNormalizeAngle(lowerLimit); }
|
||||
btScalar getUpperAngLimit() { return m_upperAngLimit; }
|
||||
void setUpperAngLimit(btScalar upperLimit) { m_upperAngLimit = btNormalizeAngle(upperLimit); }
|
||||
const btRigidBody& getRigidBodyA() const { return m_rbA; }
|
||||
const btRigidBody& getRigidBodyB() const { return m_rbB; }
|
||||
const btTransform& getCalculatedTransformA() const { return m_calculatedTransformA; }
|
||||
const btTransform& getCalculatedTransformB() const { return m_calculatedTransformB; }
|
||||
const btTransform& getFrameOffsetA() const { return m_frameInA; }
|
||||
const btTransform& getFrameOffsetB() const { return m_frameInB; }
|
||||
btTransform& getFrameOffsetA() { return m_frameInA; }
|
||||
btTransform& getFrameOffsetB() { return m_frameInB; }
|
||||
btScalar getLowerLinLimit() { return m_lowerLinLimit; }
|
||||
void setLowerLinLimit(btScalar lowerLimit) { m_lowerLinLimit = lowerLimit; }
|
||||
btScalar getUpperLinLimit() { return m_upperLinLimit; }
|
||||
void setUpperLinLimit(btScalar upperLimit) { m_upperLinLimit = upperLimit; }
|
||||
btScalar getLowerAngLimit() { return m_lowerAngLimit; }
|
||||
void setLowerAngLimit(btScalar lowerLimit) { m_lowerAngLimit = btNormalizeAngle(lowerLimit); }
|
||||
btScalar getUpperAngLimit() { return m_upperAngLimit; }
|
||||
void setUpperAngLimit(btScalar upperLimit) { m_upperAngLimit = btNormalizeAngle(upperLimit); }
|
||||
bool getUseLinearReferenceFrameA() { return m_useLinearReferenceFrameA; }
|
||||
btScalar getSoftnessDirLin() { return m_softnessDirLin; }
|
||||
btScalar getRestitutionDirLin() { return m_restitutionDirLin; }
|
||||
btScalar getDampingDirLin() { return m_dampingDirLin ; }
|
||||
btScalar getDampingDirLin() { return m_dampingDirLin; }
|
||||
btScalar getSoftnessDirAng() { return m_softnessDirAng; }
|
||||
btScalar getRestitutionDirAng() { return m_restitutionDirAng; }
|
||||
btScalar getDampingDirAng() { return m_dampingDirAng; }
|
||||
@@ -249,8 +244,6 @@ public:
|
||||
|
||||
btScalar getLinearPos() const { return m_linPos; }
|
||||
btScalar getAngularPos() const { return m_angPos; }
|
||||
|
||||
|
||||
|
||||
// access for ODE solver
|
||||
bool getSolveLinLimit() { return m_solveLinLim; }
|
||||
@@ -258,9 +251,9 @@ public:
|
||||
bool getSolveAngLimit() { return m_solveAngLim; }
|
||||
btScalar getAngDepth() { return m_angDepth; }
|
||||
// shared code used by ODE solver
|
||||
void calculateTransforms(const btTransform& transA,const btTransform& transB);
|
||||
void testLinLimits();
|
||||
void testAngLimits();
|
||||
void calculateTransforms(const btTransform& transA, const btTransform& transB);
|
||||
void testLinLimits();
|
||||
void testAngLimits();
|
||||
// access for PE Solver
|
||||
btVector3 getAncorInA();
|
||||
btVector3 getAncorInB();
|
||||
@@ -268,84 +261,75 @@ public:
|
||||
bool getUseFrameOffset() { return m_useOffsetForConstraintFrame; }
|
||||
void setUseFrameOffset(bool frameOffsetOnOff) { m_useOffsetForConstraintFrame = frameOffsetOnOff; }
|
||||
|
||||
void setFrames(const btTransform& frameA, const btTransform& frameB)
|
||||
{
|
||||
m_frameInA=frameA;
|
||||
m_frameInB=frameB;
|
||||
calculateTransforms(m_rbA.getCenterOfMassTransform(),m_rbB.getCenterOfMassTransform());
|
||||
void setFrames(const btTransform& frameA, const btTransform& frameB)
|
||||
{
|
||||
m_frameInA = frameA;
|
||||
m_frameInB = frameB;
|
||||
calculateTransforms(m_rbA.getCenterOfMassTransform(), m_rbB.getCenterOfMassTransform());
|
||||
buildJacobian();
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
///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, btScalar value, int axis = -1);
|
||||
virtual void setParam(int num, btScalar value, int axis = -1);
|
||||
///return the local value of parameter
|
||||
virtual btScalar getParam(int num, int axis = -1) const;
|
||||
|
||||
virtual int getFlags() const
|
||||
{
|
||||
virtual btScalar getParam(int num, int axis = -1) const;
|
||||
|
||||
virtual int getFlags() const
|
||||
{
|
||||
return m_flags;
|
||||
}
|
||||
|
||||
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, btSerializer* serializer) const;
|
||||
|
||||
|
||||
virtual const char* serialize(void* dataBuffer, btSerializer* serializer) const;
|
||||
};
|
||||
|
||||
|
||||
///do not change those serialization structures, it requires an updated sBulletDNAstr/sBulletDNAstr64
|
||||
|
||||
|
||||
struct btSliderConstraintData
|
||||
{
|
||||
btTypedConstraintData m_typeConstraintData;
|
||||
btTransformFloatData m_rbAFrame; // constraint axii. Assumes z is hinge axis.
|
||||
btTypedConstraintData m_typeConstraintData;
|
||||
btTransformFloatData m_rbAFrame; // constraint axii. Assumes z is hinge axis.
|
||||
btTransformFloatData m_rbBFrame;
|
||||
|
||||
float m_linearUpperLimit;
|
||||
float m_linearLowerLimit;
|
||||
|
||||
float m_angularUpperLimit;
|
||||
float m_angularLowerLimit;
|
||||
float m_linearUpperLimit;
|
||||
float m_linearLowerLimit;
|
||||
|
||||
int m_useLinearReferenceFrameA;
|
||||
float m_angularUpperLimit;
|
||||
float m_angularLowerLimit;
|
||||
|
||||
int m_useLinearReferenceFrameA;
|
||||
int m_useOffsetForConstraintFrame;
|
||||
|
||||
};
|
||||
|
||||
|
||||
struct btSliderConstraintDoubleData
|
||||
{
|
||||
btTypedConstraintDoubleData m_typeConstraintData;
|
||||
btTransformDoubleData m_rbAFrame; // constraint axii. Assumes z is hinge axis.
|
||||
btTypedConstraintDoubleData m_typeConstraintData;
|
||||
btTransformDoubleData m_rbAFrame; // constraint axii. Assumes z is hinge axis.
|
||||
btTransformDoubleData m_rbBFrame;
|
||||
|
||||
double m_linearUpperLimit;
|
||||
double m_linearLowerLimit;
|
||||
|
||||
double m_angularUpperLimit;
|
||||
double m_angularLowerLimit;
|
||||
double m_linearUpperLimit;
|
||||
double m_linearLowerLimit;
|
||||
|
||||
int m_useLinearReferenceFrameA;
|
||||
double m_angularUpperLimit;
|
||||
double m_angularLowerLimit;
|
||||
|
||||
int m_useLinearReferenceFrameA;
|
||||
int m_useOffsetForConstraintFrame;
|
||||
|
||||
};
|
||||
|
||||
SIMD_FORCE_INLINE int btSliderConstraint::calculateSerializeBufferSize() const
|
||||
SIMD_FORCE_INLINE int btSliderConstraint::calculateSerializeBufferSize() const
|
||||
{
|
||||
return sizeof(btSliderConstraintData2);
|
||||
}
|
||||
|
||||
///fills the dataBuffer and returns the struct name (and 0 on failure)
|
||||
SIMD_FORCE_INLINE const char* btSliderConstraint::serialize(void* dataBuffer, btSerializer* serializer) const
|
||||
///fills the dataBuffer and returns the struct name (and 0 on failure)
|
||||
SIMD_FORCE_INLINE const char* btSliderConstraint::serialize(void* dataBuffer, btSerializer* serializer) const
|
||||
{
|
||||
|
||||
btSliderConstraintData2* sliderData = (btSliderConstraintData2*) dataBuffer;
|
||||
btTypedConstraint::serialize(&sliderData->m_typeConstraintData,serializer);
|
||||
btSliderConstraintData2* sliderData = (btSliderConstraintData2*)dataBuffer;
|
||||
btTypedConstraint::serialize(&sliderData->m_typeConstraintData, serializer);
|
||||
|
||||
m_frameInA.serialize(sliderData->m_rbAFrame);
|
||||
m_frameInB.serialize(sliderData->m_rbBFrame);
|
||||
@@ -362,7 +346,4 @@ SIMD_FORCE_INLINE const char* btSliderConstraint::serialize(void* dataBuffer, bt
|
||||
return btSliderConstraintDataName;
|
||||
}
|
||||
|
||||
|
||||
|
||||
#endif //BT_SLIDER_CONSTRAINT_H
|
||||
|
||||
#endif //BT_SLIDER_CONSTRAINT_H
|
||||
|
||||
@@ -13,43 +13,38 @@ subject to the following restrictions:
|
||||
3. This notice may not be removed or altered from any source distribution.
|
||||
*/
|
||||
|
||||
|
||||
|
||||
#include "btSolve2LinearConstraint.h"
|
||||
|
||||
#include "BulletDynamics/Dynamics/btRigidBody.h"
|
||||
#include "LinearMath/btVector3.h"
|
||||
#include "btJacobianEntry.h"
|
||||
|
||||
|
||||
void btSolve2LinearConstraint::resolveUnilateralPairConstraint(
|
||||
btRigidBody* body1,
|
||||
btRigidBody* body2,
|
||||
btRigidBody* body1,
|
||||
btRigidBody* body2,
|
||||
|
||||
const btMatrix3x3& world2A,
|
||||
const btMatrix3x3& world2B,
|
||||
|
||||
const btVector3& invInertiaADiag,
|
||||
const btScalar invMassA,
|
||||
const btVector3& linvelA,const btVector3& angvelA,
|
||||
const btVector3& rel_posA1,
|
||||
const btVector3& invInertiaBDiag,
|
||||
const btScalar invMassB,
|
||||
const btVector3& linvelB,const btVector3& angvelB,
|
||||
const btVector3& rel_posA2,
|
||||
const btMatrix3x3& world2A,
|
||||
const btMatrix3x3& world2B,
|
||||
|
||||
btScalar depthA, const btVector3& normalA,
|
||||
const btVector3& rel_posB1,const btVector3& rel_posB2,
|
||||
btScalar depthB, const btVector3& normalB,
|
||||
btScalar& imp0,btScalar& imp1)
|
||||
const btVector3& invInertiaADiag,
|
||||
const btScalar invMassA,
|
||||
const btVector3& linvelA, const btVector3& angvelA,
|
||||
const btVector3& rel_posA1,
|
||||
const btVector3& invInertiaBDiag,
|
||||
const btScalar invMassB,
|
||||
const btVector3& linvelB, const btVector3& angvelB,
|
||||
const btVector3& rel_posA2,
|
||||
|
||||
btScalar depthA, const btVector3& normalA,
|
||||
const btVector3& rel_posB1, const btVector3& rel_posB2,
|
||||
btScalar depthB, const btVector3& normalB,
|
||||
btScalar& imp0, btScalar& imp1)
|
||||
{
|
||||
(void)linvelA;
|
||||
(void)linvelB;
|
||||
(void)angvelB;
|
||||
(void)angvelA;
|
||||
|
||||
|
||||
|
||||
imp0 = btScalar(0.);
|
||||
imp1 = btScalar(0.);
|
||||
|
||||
@@ -59,86 +54,76 @@ void btSolve2LinearConstraint::resolveUnilateralPairConstraint(
|
||||
|
||||
btAssert(len < SIMD_EPSILON);
|
||||
|
||||
|
||||
//this jacobian entry could be re-used for all iterations
|
||||
btJacobianEntry jacA(world2A,world2B,rel_posA1,rel_posA2,normalA,invInertiaADiag,invMassA,
|
||||
invInertiaBDiag,invMassB);
|
||||
btJacobianEntry jacB(world2A,world2B,rel_posB1,rel_posB2,normalB,invInertiaADiag,invMassA,
|
||||
invInertiaBDiag,invMassB);
|
||||
|
||||
btJacobianEntry jacA(world2A, world2B, rel_posA1, rel_posA2, normalA, invInertiaADiag, invMassA,
|
||||
invInertiaBDiag, invMassB);
|
||||
btJacobianEntry jacB(world2A, world2B, rel_posB1, rel_posB2, normalB, invInertiaADiag, invMassA,
|
||||
invInertiaBDiag, invMassB);
|
||||
|
||||
//const btScalar vel0 = jacA.getRelativeVelocity(linvelA,angvelA,linvelB,angvelB);
|
||||
//const btScalar vel1 = jacB.getRelativeVelocity(linvelA,angvelA,linvelB,angvelB);
|
||||
|
||||
const btScalar vel0 = normalA.dot(body1->getVelocityInLocalPoint(rel_posA1)-body2->getVelocityInLocalPoint(rel_posA1));
|
||||
const btScalar vel1 = normalB.dot(body1->getVelocityInLocalPoint(rel_posB1)-body2->getVelocityInLocalPoint(rel_posB1));
|
||||
const btScalar vel0 = normalA.dot(body1->getVelocityInLocalPoint(rel_posA1) - body2->getVelocityInLocalPoint(rel_posA1));
|
||||
const btScalar vel1 = normalB.dot(body1->getVelocityInLocalPoint(rel_posB1) - body2->getVelocityInLocalPoint(rel_posB1));
|
||||
|
||||
// btScalar penetrationImpulse = (depth*contactTau*timeCorrection) * massTerm;//jacDiagABInv
|
||||
// btScalar penetrationImpulse = (depth*contactTau*timeCorrection) * massTerm;//jacDiagABInv
|
||||
btScalar massTerm = btScalar(1.) / (invMassA + invMassB);
|
||||
|
||||
|
||||
// calculate rhs (or error) terms
|
||||
const btScalar dv0 = depthA * m_tau * massTerm - vel0 * m_damping;
|
||||
const btScalar dv1 = depthB * m_tau * massTerm - vel1 * m_damping;
|
||||
|
||||
const btScalar dv0 = depthA * m_tau * massTerm - vel0 * m_damping;
|
||||
const btScalar dv1 = depthB * m_tau * massTerm - vel1 * m_damping;
|
||||
|
||||
// dC/dv * dv = -C
|
||||
|
||||
|
||||
// jacobian * impulse = -error
|
||||
//
|
||||
|
||||
//impulse = jacobianInverse * -error
|
||||
|
||||
// inverting 2x2 symmetric system (offdiagonal are equal!)
|
||||
//
|
||||
//
|
||||
|
||||
btScalar nonDiag = jacA.getNonDiagonal(jacB, invMassA, invMassB);
|
||||
btScalar invDet = btScalar(1.0) / (jacA.getDiagonal() * jacB.getDiagonal() - nonDiag * nonDiag);
|
||||
|
||||
btScalar nonDiag = jacA.getNonDiagonal(jacB,invMassA,invMassB);
|
||||
btScalar invDet = btScalar(1.0) / (jacA.getDiagonal() * jacB.getDiagonal() - nonDiag * nonDiag );
|
||||
|
||||
//imp0 = dv0 * jacA.getDiagonal() * invDet + dv1 * -nonDiag * invDet;
|
||||
//imp1 = dv1 * jacB.getDiagonal() * invDet + dv0 * - nonDiag * invDet;
|
||||
|
||||
imp0 = dv0 * jacA.getDiagonal() * invDet + dv1 * -nonDiag * invDet;
|
||||
imp1 = dv1 * jacB.getDiagonal() * invDet + dv0 * - nonDiag * invDet;
|
||||
imp1 = dv1 * jacB.getDiagonal() * invDet + dv0 * -nonDiag * invDet;
|
||||
|
||||
//[a b] [d -c]
|
||||
//[c d] inverse = (1 / determinant) * [-b a] where determinant is (ad - bc)
|
||||
|
||||
//[jA nD] * [imp0] = [dv0]
|
||||
//[nD jB] [imp1] [dv1]
|
||||
|
||||
}
|
||||
|
||||
|
||||
|
||||
void btSolve2LinearConstraint::resolveBilateralPairConstraint(
|
||||
btRigidBody* body1,
|
||||
btRigidBody* body2,
|
||||
const btMatrix3x3& world2A,
|
||||
const btMatrix3x3& world2B,
|
||||
|
||||
const btVector3& invInertiaADiag,
|
||||
const btScalar invMassA,
|
||||
const btVector3& linvelA,const btVector3& angvelA,
|
||||
const btVector3& rel_posA1,
|
||||
const btVector3& invInertiaBDiag,
|
||||
const btScalar invMassB,
|
||||
const btVector3& linvelB,const btVector3& angvelB,
|
||||
const btVector3& rel_posA2,
|
||||
btRigidBody* body1,
|
||||
btRigidBody* body2,
|
||||
const btMatrix3x3& world2A,
|
||||
const btMatrix3x3& world2B,
|
||||
|
||||
btScalar depthA, const btVector3& normalA,
|
||||
const btVector3& rel_posB1,const btVector3& rel_posB2,
|
||||
btScalar depthB, const btVector3& normalB,
|
||||
btScalar& imp0,btScalar& imp1)
|
||||
const btVector3& invInertiaADiag,
|
||||
const btScalar invMassA,
|
||||
const btVector3& linvelA, const btVector3& angvelA,
|
||||
const btVector3& rel_posA1,
|
||||
const btVector3& invInertiaBDiag,
|
||||
const btScalar invMassB,
|
||||
const btVector3& linvelB, const btVector3& angvelB,
|
||||
const btVector3& rel_posA2,
|
||||
|
||||
btScalar depthA, const btVector3& normalA,
|
||||
const btVector3& rel_posB1, const btVector3& rel_posB2,
|
||||
btScalar depthB, const btVector3& normalB,
|
||||
btScalar& imp0, btScalar& imp1)
|
||||
{
|
||||
|
||||
(void)linvelA;
|
||||
(void)linvelB;
|
||||
(void)angvelA;
|
||||
(void)angvelB;
|
||||
|
||||
|
||||
|
||||
imp0 = btScalar(0.);
|
||||
imp1 = btScalar(0.);
|
||||
|
||||
@@ -148,42 +133,40 @@ void btSolve2LinearConstraint::resolveBilateralPairConstraint(
|
||||
|
||||
btAssert(len < SIMD_EPSILON);
|
||||
|
||||
|
||||
//this jacobian entry could be re-used for all iterations
|
||||
btJacobianEntry jacA(world2A,world2B,rel_posA1,rel_posA2,normalA,invInertiaADiag,invMassA,
|
||||
invInertiaBDiag,invMassB);
|
||||
btJacobianEntry jacB(world2A,world2B,rel_posB1,rel_posB2,normalB,invInertiaADiag,invMassA,
|
||||
invInertiaBDiag,invMassB);
|
||||
|
||||
btJacobianEntry jacA(world2A, world2B, rel_posA1, rel_posA2, normalA, invInertiaADiag, invMassA,
|
||||
invInertiaBDiag, invMassB);
|
||||
btJacobianEntry jacB(world2A, world2B, rel_posB1, rel_posB2, normalB, invInertiaADiag, invMassA,
|
||||
invInertiaBDiag, invMassB);
|
||||
|
||||
//const btScalar vel0 = jacA.getRelativeVelocity(linvelA,angvelA,linvelB,angvelB);
|
||||
//const btScalar vel1 = jacB.getRelativeVelocity(linvelA,angvelA,linvelB,angvelB);
|
||||
|
||||
const btScalar vel0 = normalA.dot(body1->getVelocityInLocalPoint(rel_posA1)-body2->getVelocityInLocalPoint(rel_posA1));
|
||||
const btScalar vel1 = normalB.dot(body1->getVelocityInLocalPoint(rel_posB1)-body2->getVelocityInLocalPoint(rel_posB1));
|
||||
const btScalar vel0 = normalA.dot(body1->getVelocityInLocalPoint(rel_posA1) - body2->getVelocityInLocalPoint(rel_posA1));
|
||||
const btScalar vel1 = normalB.dot(body1->getVelocityInLocalPoint(rel_posB1) - body2->getVelocityInLocalPoint(rel_posB1));
|
||||
|
||||
// calculate rhs (or error) terms
|
||||
const btScalar dv0 = depthA * m_tau - vel0 * m_damping;
|
||||
const btScalar dv1 = depthB * m_tau - vel1 * m_damping;
|
||||
const btScalar dv0 = depthA * m_tau - vel0 * m_damping;
|
||||
const btScalar dv1 = depthB * m_tau - vel1 * m_damping;
|
||||
|
||||
// dC/dv * dv = -C
|
||||
|
||||
|
||||
// jacobian * impulse = -error
|
||||
//
|
||||
|
||||
//impulse = jacobianInverse * -error
|
||||
|
||||
// inverting 2x2 symmetric system (offdiagonal are equal!)
|
||||
//
|
||||
//
|
||||
|
||||
btScalar nonDiag = jacA.getNonDiagonal(jacB, invMassA, invMassB);
|
||||
btScalar invDet = btScalar(1.0) / (jacA.getDiagonal() * jacB.getDiagonal() - nonDiag * nonDiag);
|
||||
|
||||
btScalar nonDiag = jacA.getNonDiagonal(jacB,invMassA,invMassB);
|
||||
btScalar invDet = btScalar(1.0) / (jacA.getDiagonal() * jacB.getDiagonal() - nonDiag * nonDiag );
|
||||
|
||||
//imp0 = dv0 * jacA.getDiagonal() * invDet + dv1 * -nonDiag * invDet;
|
||||
//imp1 = dv1 * jacB.getDiagonal() * invDet + dv0 * - nonDiag * invDet;
|
||||
|
||||
imp0 = dv0 * jacA.getDiagonal() * invDet + dv1 * -nonDiag * invDet;
|
||||
imp1 = dv1 * jacB.getDiagonal() * invDet + dv0 * - nonDiag * invDet;
|
||||
imp1 = dv1 * jacB.getDiagonal() * invDet + dv0 * -nonDiag * invDet;
|
||||
|
||||
//[a b] [d -c]
|
||||
//[c d] inverse = (1 / determinant) * [-b a] where determinant is (ad - bc)
|
||||
@@ -191,9 +174,9 @@ void btSolve2LinearConstraint::resolveBilateralPairConstraint(
|
||||
//[jA nD] * [imp0] = [dv0]
|
||||
//[nD jB] [imp1] [dv1]
|
||||
|
||||
if ( imp0 > btScalar(0.0))
|
||||
if (imp0 > btScalar(0.0))
|
||||
{
|
||||
if ( imp1 > btScalar(0.0) )
|
||||
if (imp1 > btScalar(0.0))
|
||||
{
|
||||
//both positive
|
||||
}
|
||||
@@ -203,9 +186,10 @@ void btSolve2LinearConstraint::resolveBilateralPairConstraint(
|
||||
|
||||
// now imp0>0 imp1<0
|
||||
imp0 = dv0 / jacA.getDiagonal();
|
||||
if ( imp0 > btScalar(0.0) )
|
||||
if (imp0 > btScalar(0.0))
|
||||
{
|
||||
} else
|
||||
}
|
||||
else
|
||||
{
|
||||
imp0 = btScalar(0.);
|
||||
}
|
||||
@@ -216,24 +200,25 @@ void btSolve2LinearConstraint::resolveBilateralPairConstraint(
|
||||
imp0 = btScalar(0.);
|
||||
|
||||
imp1 = dv1 / jacB.getDiagonal();
|
||||
if ( imp1 <= btScalar(0.0) )
|
||||
if (imp1 <= btScalar(0.0))
|
||||
{
|
||||
imp1 = btScalar(0.);
|
||||
// now imp0>0 imp1<0
|
||||
imp0 = dv0 / jacA.getDiagonal();
|
||||
if ( imp0 > btScalar(0.0) )
|
||||
if (imp0 > btScalar(0.0))
|
||||
{
|
||||
} else
|
||||
}
|
||||
else
|
||||
{
|
||||
imp0 = btScalar(0.);
|
||||
}
|
||||
} else
|
||||
}
|
||||
else
|
||||
{
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
/*
|
||||
void btSolve2LinearConstraint::resolveAngularConstraint( const btMatrix3x3& invInertiaAWS,
|
||||
const btScalar invMassA,
|
||||
@@ -252,4 +237,3 @@ void btSolve2LinearConstraint::resolveAngularConstraint( const btMatrix3x3& invI
|
||||
|
||||
}
|
||||
*/
|
||||
|
||||
|
||||
@@ -19,20 +19,16 @@ subject to the following restrictions:
|
||||
#include "LinearMath/btMatrix3x3.h"
|
||||
#include "LinearMath/btVector3.h"
|
||||
|
||||
|
||||
class btRigidBody;
|
||||
|
||||
|
||||
|
||||
/// constraint class used for lateral tyre friction.
|
||||
class btSolve2LinearConstraint
|
||||
class btSolve2LinearConstraint
|
||||
{
|
||||
btScalar m_tau;
|
||||
btScalar m_damping;
|
||||
btScalar m_tau;
|
||||
btScalar m_damping;
|
||||
|
||||
public:
|
||||
|
||||
btSolve2LinearConstraint(btScalar tau,btScalar damping)
|
||||
btSolve2LinearConstraint(btScalar tau, btScalar damping)
|
||||
{
|
||||
m_tau = tau;
|
||||
m_damping = damping;
|
||||
@@ -40,52 +36,51 @@ public:
|
||||
//
|
||||
// solve unilateral constraint (equality, direct method)
|
||||
//
|
||||
void resolveUnilateralPairConstraint(
|
||||
btRigidBody* body0,
|
||||
void resolveUnilateralPairConstraint(
|
||||
btRigidBody* body0,
|
||||
btRigidBody* body1,
|
||||
|
||||
const btMatrix3x3& world2A,
|
||||
const btMatrix3x3& world2B,
|
||||
|
||||
const btVector3& invInertiaADiag,
|
||||
const btScalar invMassA,
|
||||
const btVector3& linvelA,const btVector3& angvelA,
|
||||
const btVector3& rel_posA1,
|
||||
const btVector3& invInertiaBDiag,
|
||||
const btScalar invMassB,
|
||||
const btVector3& linvelB,const btVector3& angvelB,
|
||||
const btVector3& rel_posA2,
|
||||
const btMatrix3x3& world2B,
|
||||
|
||||
btScalar depthA, const btVector3& normalA,
|
||||
const btVector3& rel_posB1,const btVector3& rel_posB2,
|
||||
btScalar depthB, const btVector3& normalB,
|
||||
btScalar& imp0,btScalar& imp1);
|
||||
const btVector3& invInertiaADiag,
|
||||
const btScalar invMassA,
|
||||
const btVector3& linvelA, const btVector3& angvelA,
|
||||
const btVector3& rel_posA1,
|
||||
const btVector3& invInertiaBDiag,
|
||||
const btScalar invMassB,
|
||||
const btVector3& linvelB, const btVector3& angvelB,
|
||||
const btVector3& rel_posA2,
|
||||
|
||||
btScalar depthA, const btVector3& normalA,
|
||||
const btVector3& rel_posB1, const btVector3& rel_posB2,
|
||||
btScalar depthB, const btVector3& normalB,
|
||||
btScalar& imp0, btScalar& imp1);
|
||||
|
||||
//
|
||||
// solving 2x2 lcp problem (inequality, direct solution )
|
||||
//
|
||||
void resolveBilateralPairConstraint(
|
||||
btRigidBody* body0,
|
||||
btRigidBody* body1,
|
||||
btRigidBody* body0,
|
||||
btRigidBody* body1,
|
||||
const btMatrix3x3& world2A,
|
||||
const btMatrix3x3& world2B,
|
||||
|
||||
const btVector3& invInertiaADiag,
|
||||
const btScalar invMassA,
|
||||
const btVector3& linvelA,const btVector3& angvelA,
|
||||
const btVector3& rel_posA1,
|
||||
const btVector3& invInertiaBDiag,
|
||||
const btScalar invMassB,
|
||||
const btVector3& linvelB,const btVector3& angvelB,
|
||||
const btVector3& rel_posA2,
|
||||
const btMatrix3x3& world2B,
|
||||
|
||||
btScalar depthA, const btVector3& normalA,
|
||||
const btVector3& rel_posB1,const btVector3& rel_posB2,
|
||||
btScalar depthB, const btVector3& normalB,
|
||||
btScalar& imp0,btScalar& imp1);
|
||||
const btVector3& invInertiaADiag,
|
||||
const btScalar invMassA,
|
||||
const btVector3& linvelA, const btVector3& angvelA,
|
||||
const btVector3& rel_posA1,
|
||||
const btVector3& invInertiaBDiag,
|
||||
const btScalar invMassB,
|
||||
const btVector3& linvelB, const btVector3& angvelB,
|
||||
const btVector3& rel_posA2,
|
||||
|
||||
/*
|
||||
btScalar depthA, const btVector3& normalA,
|
||||
const btVector3& rel_posB1, const btVector3& rel_posB2,
|
||||
btScalar depthB, const btVector3& normalB,
|
||||
btScalar& imp0, btScalar& imp1);
|
||||
|
||||
/*
|
||||
void resolveAngularConstraint( const btMatrix3x3& invInertiaAWS,
|
||||
const btScalar invMassA,
|
||||
const btVector3& linvelA,const btVector3& angvelA,
|
||||
@@ -101,7 +96,6 @@ public:
|
||||
btScalar& imp0,btScalar& imp1);
|
||||
|
||||
*/
|
||||
|
||||
};
|
||||
|
||||
#endif //BT_SOLVE_2LINEAR_CONSTRAINT_H
|
||||
#endif //BT_SOLVE_2LINEAR_CONSTRAINT_H
|
||||
|
||||
@@ -16,7 +16,7 @@ subject to the following restrictions:
|
||||
#ifndef BT_SOLVER_BODY_H
|
||||
#define BT_SOLVER_BODY_H
|
||||
|
||||
class btRigidBody;
|
||||
class btRigidBody;
|
||||
#include "LinearMath/btVector3.h"
|
||||
#include "LinearMath/btMatrix3x3.h"
|
||||
|
||||
@@ -26,103 +26,99 @@ class btRigidBody;
|
||||
///Until we get other contributions, only use SIMD on Windows, when using Visual Studio 2008 or later, and not double precision
|
||||
#ifdef BT_USE_SSE
|
||||
#define USE_SIMD 1
|
||||
#endif //
|
||||
|
||||
#endif //
|
||||
|
||||
#ifdef USE_SIMD
|
||||
|
||||
struct btSimdScalar
|
||||
struct btSimdScalar
|
||||
{
|
||||
SIMD_FORCE_INLINE btSimdScalar()
|
||||
{
|
||||
|
||||
}
|
||||
|
||||
SIMD_FORCE_INLINE btSimdScalar(float fl)
|
||||
:m_vec128 (_mm_set1_ps(fl))
|
||||
SIMD_FORCE_INLINE btSimdScalar()
|
||||
{
|
||||
}
|
||||
|
||||
SIMD_FORCE_INLINE btSimdScalar(__m128 v128)
|
||||
:m_vec128(v128)
|
||||
SIMD_FORCE_INLINE btSimdScalar(float fl)
|
||||
: m_vec128(_mm_set1_ps(fl))
|
||||
{
|
||||
}
|
||||
union
|
||||
|
||||
SIMD_FORCE_INLINE btSimdScalar(__m128 v128)
|
||||
: m_vec128(v128)
|
||||
{
|
||||
__m128 m_vec128;
|
||||
float m_floats[4];
|
||||
int m_ints[4];
|
||||
btScalar m_unusedPadding;
|
||||
}
|
||||
union {
|
||||
__m128 m_vec128;
|
||||
float m_floats[4];
|
||||
int m_ints[4];
|
||||
btScalar m_unusedPadding;
|
||||
};
|
||||
SIMD_FORCE_INLINE __m128 get128()
|
||||
SIMD_FORCE_INLINE __m128 get128()
|
||||
{
|
||||
return m_vec128;
|
||||
}
|
||||
|
||||
SIMD_FORCE_INLINE const __m128 get128() const
|
||||
SIMD_FORCE_INLINE const __m128 get128() const
|
||||
{
|
||||
return m_vec128;
|
||||
}
|
||||
|
||||
SIMD_FORCE_INLINE void set128(__m128 v128)
|
||||
SIMD_FORCE_INLINE void set128(__m128 v128)
|
||||
{
|
||||
m_vec128 = v128;
|
||||
}
|
||||
|
||||
SIMD_FORCE_INLINE operator __m128()
|
||||
{
|
||||
return m_vec128;
|
||||
SIMD_FORCE_INLINE operator __m128()
|
||||
{
|
||||
return m_vec128;
|
||||
}
|
||||
SIMD_FORCE_INLINE operator const __m128() const
|
||||
{
|
||||
return m_vec128;
|
||||
}
|
||||
|
||||
SIMD_FORCE_INLINE operator float() const
|
||||
{
|
||||
return m_floats[0];
|
||||
SIMD_FORCE_INLINE operator const __m128() const
|
||||
{
|
||||
return m_vec128;
|
||||
}
|
||||
|
||||
SIMD_FORCE_INLINE operator float() const
|
||||
{
|
||||
return m_floats[0];
|
||||
}
|
||||
};
|
||||
|
||||
///@brief Return the elementwise product of two btSimdScalar
|
||||
SIMD_FORCE_INLINE btSimdScalar
|
||||
operator*(const btSimdScalar& v1, const btSimdScalar& v2)
|
||||
SIMD_FORCE_INLINE btSimdScalar
|
||||
operator*(const btSimdScalar& v1, const btSimdScalar& v2)
|
||||
{
|
||||
return btSimdScalar(_mm_mul_ps(v1.get128(),v2.get128()));
|
||||
return btSimdScalar(_mm_mul_ps(v1.get128(), v2.get128()));
|
||||
}
|
||||
|
||||
///@brief Return the elementwise product of two btSimdScalar
|
||||
SIMD_FORCE_INLINE btSimdScalar
|
||||
operator+(const btSimdScalar& v1, const btSimdScalar& v2)
|
||||
SIMD_FORCE_INLINE btSimdScalar
|
||||
operator+(const btSimdScalar& v1, const btSimdScalar& v2)
|
||||
{
|
||||
return btSimdScalar(_mm_add_ps(v1.get128(),v2.get128()));
|
||||
return btSimdScalar(_mm_add_ps(v1.get128(), v2.get128()));
|
||||
}
|
||||
|
||||
|
||||
#else
|
||||
#define btSimdScalar btScalar
|
||||
#endif
|
||||
|
||||
///The btSolverBody is an internal datastructure for the constraint solver. Only necessary data is packed to increase cache coherence/performance.
|
||||
ATTRIBUTE_ALIGNED16 (struct) btSolverBody
|
||||
ATTRIBUTE_ALIGNED16(struct)
|
||||
btSolverBody
|
||||
{
|
||||
BT_DECLARE_ALIGNED_ALLOCATOR();
|
||||
btTransform m_worldTransform;
|
||||
btVector3 m_deltaLinearVelocity;
|
||||
btVector3 m_deltaAngularVelocity;
|
||||
btVector3 m_angularFactor;
|
||||
btVector3 m_linearFactor;
|
||||
btVector3 m_invMass;
|
||||
btVector3 m_pushVelocity;
|
||||
btVector3 m_turnVelocity;
|
||||
btVector3 m_linearVelocity;
|
||||
btVector3 m_angularVelocity;
|
||||
btVector3 m_externalForceImpulse;
|
||||
btVector3 m_externalTorqueImpulse;
|
||||
btTransform m_worldTransform;
|
||||
btVector3 m_deltaLinearVelocity;
|
||||
btVector3 m_deltaAngularVelocity;
|
||||
btVector3 m_angularFactor;
|
||||
btVector3 m_linearFactor;
|
||||
btVector3 m_invMass;
|
||||
btVector3 m_pushVelocity;
|
||||
btVector3 m_turnVelocity;
|
||||
btVector3 m_linearVelocity;
|
||||
btVector3 m_angularVelocity;
|
||||
btVector3 m_externalForceImpulse;
|
||||
btVector3 m_externalTorqueImpulse;
|
||||
|
||||
btRigidBody* m_originalBody;
|
||||
void setWorldTransform(const btTransform& worldTransform)
|
||||
btRigidBody* m_originalBody;
|
||||
void setWorldTransform(const btTransform& worldTransform)
|
||||
{
|
||||
m_worldTransform = worldTransform;
|
||||
}
|
||||
@@ -131,56 +127,50 @@ ATTRIBUTE_ALIGNED16 (struct) btSolverBody
|
||||
{
|
||||
return m_worldTransform;
|
||||
}
|
||||
|
||||
|
||||
|
||||
SIMD_FORCE_INLINE void getVelocityInLocalPointNoDelta(const btVector3& rel_pos, btVector3& velocity ) const
|
||||
SIMD_FORCE_INLINE void getVelocityInLocalPointNoDelta(const btVector3& rel_pos, btVector3& velocity) const
|
||||
{
|
||||
if (m_originalBody)
|
||||
velocity = m_linearVelocity + m_externalForceImpulse + (m_angularVelocity+m_externalTorqueImpulse).cross(rel_pos);
|
||||
velocity = m_linearVelocity + m_externalForceImpulse + (m_angularVelocity + m_externalTorqueImpulse).cross(rel_pos);
|
||||
else
|
||||
velocity.setValue(0,0,0);
|
||||
velocity.setValue(0, 0, 0);
|
||||
}
|
||||
|
||||
|
||||
SIMD_FORCE_INLINE void getVelocityInLocalPointObsolete(const btVector3& rel_pos, btVector3& velocity ) const
|
||||
SIMD_FORCE_INLINE void getVelocityInLocalPointObsolete(const btVector3& rel_pos, btVector3& 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);
|
||||
}
|
||||
|
||||
SIMD_FORCE_INLINE void getAngularVelocity(btVector3& angVel) const
|
||||
SIMD_FORCE_INLINE void getAngularVelocity(btVector3 & 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
|
||||
SIMD_FORCE_INLINE void applyImpulse(const btVector3& linearComponent, const btVector3& angularComponent,const btScalar impulseMagnitude)
|
||||
SIMD_FORCE_INLINE void applyImpulse(const btVector3& linearComponent, const btVector3& angularComponent, const btScalar 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);
|
||||
}
|
||||
}
|
||||
|
||||
SIMD_FORCE_INLINE void internalApplyPushImpulse(const btVector3& linearComponent, const btVector3& angularComponent,btScalar impulseMagnitude)
|
||||
SIMD_FORCE_INLINE void internalApplyPushImpulse(const btVector3& linearComponent, const btVector3& angularComponent, btScalar 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 btVector3& getDeltaLinearVelocity() const
|
||||
{
|
||||
return m_deltaLinearVelocity;
|
||||
@@ -191,20 +181,19 @@ ATTRIBUTE_ALIGNED16 (struct) btSolverBody
|
||||
return m_deltaAngularVelocity;
|
||||
}
|
||||
|
||||
const btVector3& getPushVelocity() const
|
||||
const btVector3& getPushVelocity() const
|
||||
{
|
||||
return m_pushVelocity;
|
||||
}
|
||||
|
||||
const btVector3& getTurnVelocity() const
|
||||
const btVector3& getTurnVelocity() const
|
||||
{
|
||||
return m_turnVelocity;
|
||||
}
|
||||
|
||||
|
||||
////////////////////////////////////////////////
|
||||
///some internal methods, don't use them
|
||||
|
||||
|
||||
btVector3& internalGetDeltaLinearVelocity()
|
||||
{
|
||||
return m_deltaLinearVelocity;
|
||||
@@ -229,7 +218,7 @@ ATTRIBUTE_ALIGNED16 (struct) btSolverBody
|
||||
{
|
||||
m_invMass = invMass;
|
||||
}
|
||||
|
||||
|
||||
btVector3& internalGetPushVelocity()
|
||||
{
|
||||
return m_pushVelocity;
|
||||
@@ -240,67 +229,57 @@ ATTRIBUTE_ALIGNED16 (struct) btSolverBody
|
||||
return m_turnVelocity;
|
||||
}
|
||||
|
||||
SIMD_FORCE_INLINE void internalGetVelocityInLocalPointObsolete(const btVector3& rel_pos, btVector3& velocity ) const
|
||||
SIMD_FORCE_INLINE void internalGetVelocityInLocalPointObsolete(const btVector3& rel_pos, btVector3& 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);
|
||||
}
|
||||
|
||||
SIMD_FORCE_INLINE void internalGetAngularVelocity(btVector3& angVel) const
|
||||
SIMD_FORCE_INLINE void internalGetAngularVelocity(btVector3 & 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
|
||||
SIMD_FORCE_INLINE void internalApplyImpulse(const btVector3& linearComponent, const btVector3& angularComponent,const btScalar impulseMagnitude)
|
||||
SIMD_FORCE_INLINE void internalApplyImpulse(const btVector3& linearComponent, const btVector3& angularComponent, const btScalar impulseMagnitude)
|
||||
{
|
||||
if (m_originalBody)
|
||||
{
|
||||
m_deltaLinearVelocity += linearComponent*impulseMagnitude*m_linearFactor;
|
||||
m_deltaAngularVelocity += angularComponent*(impulseMagnitude*m_angularFactor);
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
|
||||
|
||||
void writebackVelocity()
|
||||
{
|
||||
if (m_originalBody)
|
||||
{
|
||||
m_linearVelocity +=m_deltaLinearVelocity;
|
||||
m_angularVelocity += m_deltaAngularVelocity;
|
||||
|
||||
//m_originalBody->setCompanionId(-1);
|
||||
m_deltaLinearVelocity += linearComponent * impulseMagnitude * m_linearFactor;
|
||||
m_deltaAngularVelocity += angularComponent * (impulseMagnitude * m_angularFactor);
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
void writebackVelocityAndTransform(btScalar timeStep, btScalar splitImpulseTurnErp)
|
||||
void writebackVelocity()
|
||||
{
|
||||
(void) timeStep;
|
||||
if (m_originalBody)
|
||||
{
|
||||
m_linearVelocity += m_deltaLinearVelocity;
|
||||
m_angularVelocity += m_deltaAngularVelocity;
|
||||
|
||||
|
||||
//m_originalBody->setCompanionId(-1);
|
||||
}
|
||||
}
|
||||
|
||||
void writebackVelocityAndTransform(btScalar timeStep, btScalar splitImpulseTurnErp)
|
||||
{
|
||||
(void)timeStep;
|
||||
if (m_originalBody)
|
||||
{
|
||||
m_linearVelocity += m_deltaLinearVelocity;
|
||||
m_angularVelocity += m_deltaAngularVelocity;
|
||||
|
||||
//correct the position/orientation based on push/turn recovery
|
||||
btTransform 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)
|
||||
{
|
||||
// btQuaternion orn = m_worldTransform.getRotation();
|
||||
btTransformUtil::integrateTransform(m_worldTransform,m_pushVelocity,m_turnVelocity*splitImpulseTurnErp,timeStep,newTransform);
|
||||
// btQuaternion orn = m_worldTransform.getRotation();
|
||||
btTransformUtil::integrateTransform(m_worldTransform, m_pushVelocity, m_turnVelocity * splitImpulseTurnErp, timeStep, newTransform);
|
||||
m_worldTransform = newTransform;
|
||||
}
|
||||
//m_worldTransform.setRotation(orn);
|
||||
//m_originalBody->setCompanionId(-1);
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
|
||||
};
|
||||
|
||||
#endif //BT_SOLVER_BODY_H
|
||||
|
||||
|
||||
#endif //BT_SOLVER_BODY_H
|
||||
|
||||
@@ -16,7 +16,7 @@ subject to the following restrictions:
|
||||
#ifndef BT_SOLVER_CONSTRAINT_H
|
||||
#define BT_SOLVER_CONSTRAINT_H
|
||||
|
||||
class btRigidBody;
|
||||
class btRigidBody;
|
||||
#include "LinearMath/btVector3.h"
|
||||
#include "LinearMath/btMatrix3x3.h"
|
||||
#include "btJacobianEntry.h"
|
||||
@@ -25,56 +25,50 @@ class btRigidBody;
|
||||
//#define NO_FRICTION_TANGENTIALS 1
|
||||
#include "btSolverBody.h"
|
||||
|
||||
|
||||
///1D constraint along a normal axis between bodyA and bodyB. It can be combined to solve contact and friction constraints.
|
||||
ATTRIBUTE_ALIGNED16 (struct) btSolverConstraint
|
||||
ATTRIBUTE_ALIGNED16(struct)
|
||||
btSolverConstraint
|
||||
{
|
||||
BT_DECLARE_ALIGNED_ALLOCATOR();
|
||||
|
||||
btVector3 m_relpos1CrossNormal;
|
||||
btVector3 m_contactNormal1;
|
||||
btVector3 m_relpos1CrossNormal;
|
||||
btVector3 m_contactNormal1;
|
||||
|
||||
btVector3 m_relpos2CrossNormal;
|
||||
btVector3 m_contactNormal2; //usually m_contactNormal2 == -m_contactNormal1, but not always
|
||||
btVector3 m_relpos2CrossNormal;
|
||||
btVector3 m_contactNormal2; //usually m_contactNormal2 == -m_contactNormal1, but not always
|
||||
|
||||
btVector3 m_angularComponentA;
|
||||
btVector3 m_angularComponentB;
|
||||
|
||||
mutable btSimdScalar m_appliedPushImpulse;
|
||||
mutable btSimdScalar m_appliedImpulse;
|
||||
btVector3 m_angularComponentA;
|
||||
btVector3 m_angularComponentB;
|
||||
|
||||
btScalar m_friction;
|
||||
btScalar m_jacDiagABInv;
|
||||
btScalar m_rhs;
|
||||
btScalar m_cfm;
|
||||
|
||||
btScalar m_lowerLimit;
|
||||
btScalar m_upperLimit;
|
||||
btScalar m_rhsPenetration;
|
||||
union
|
||||
{
|
||||
void* m_originalContactPoint;
|
||||
btScalar m_unusedPadding4;
|
||||
int m_numRowsForNonContactConstraint;
|
||||
mutable btSimdScalar m_appliedPushImpulse;
|
||||
mutable btSimdScalar m_appliedImpulse;
|
||||
|
||||
btScalar m_friction;
|
||||
btScalar m_jacDiagABInv;
|
||||
btScalar m_rhs;
|
||||
btScalar m_cfm;
|
||||
|
||||
btScalar m_lowerLimit;
|
||||
btScalar m_upperLimit;
|
||||
btScalar m_rhsPenetration;
|
||||
union {
|
||||
void* m_originalContactPoint;
|
||||
btScalar m_unusedPadding4;
|
||||
int m_numRowsForNonContactConstraint;
|
||||
};
|
||||
|
||||
int m_overrideNumSolverIterations;
|
||||
int m_frictionIndex;
|
||||
int m_overrideNumSolverIterations;
|
||||
int m_frictionIndex;
|
||||
int m_solverBodyIdA;
|
||||
int m_solverBodyIdB;
|
||||
|
||||
|
||||
enum btSolverConstraintType
|
||||
enum btSolverConstraintType
|
||||
{
|
||||
BT_SOLVER_CONTACT_1D = 0,
|
||||
BT_SOLVER_FRICTION_1D
|
||||
};
|
||||
};
|
||||
|
||||
typedef btAlignedObjectArray<btSolverConstraint> btConstraintArray;
|
||||
|
||||
|
||||
#endif //BT_SOLVER_CONSTRAINT_H
|
||||
|
||||
|
||||
typedef btAlignedObjectArray<btSolverConstraint> btConstraintArray;
|
||||
|
||||
#endif //BT_SOLVER_CONSTRAINT_H
|
||||
|
||||
@@ -13,69 +13,63 @@ subject to the following restrictions:
|
||||
3. This notice may not be removed or altered from any source distribution.
|
||||
*/
|
||||
|
||||
|
||||
#include "btTypedConstraint.h"
|
||||
#include "BulletDynamics/Dynamics/btRigidBody.h"
|
||||
#include "LinearMath/btSerializer.h"
|
||||
|
||||
|
||||
#define DEFAULT_DEBUGDRAW_SIZE btScalar(0.05f)
|
||||
|
||||
btTypedConstraint::btTypedConstraint(btTypedConstraintType type, btRigidBody& rbA)
|
||||
:btTypedObject(type),
|
||||
m_userConstraintType(-1),
|
||||
m_userConstraintPtr((void*)-1),
|
||||
m_breakingImpulseThreshold(SIMD_INFINITY),
|
||||
m_isEnabled(true),
|
||||
m_needsFeedback(false),
|
||||
m_overrideNumSolverIterations(-1),
|
||||
m_rbA(rbA),
|
||||
m_rbB(getFixedBody()),
|
||||
m_appliedImpulse(btScalar(0.)),
|
||||
m_dbgDrawSize(DEFAULT_DEBUGDRAW_SIZE),
|
||||
m_jointFeedback(0)
|
||||
: btTypedObject(type),
|
||||
m_userConstraintType(-1),
|
||||
m_userConstraintPtr((void*)-1),
|
||||
m_breakingImpulseThreshold(SIMD_INFINITY),
|
||||
m_isEnabled(true),
|
||||
m_needsFeedback(false),
|
||||
m_overrideNumSolverIterations(-1),
|
||||
m_rbA(rbA),
|
||||
m_rbB(getFixedBody()),
|
||||
m_appliedImpulse(btScalar(0.)),
|
||||
m_dbgDrawSize(DEFAULT_DEBUGDRAW_SIZE),
|
||||
m_jointFeedback(0)
|
||||
{
|
||||
}
|
||||
|
||||
|
||||
btTypedConstraint::btTypedConstraint(btTypedConstraintType type, btRigidBody& rbA,btRigidBody& rbB)
|
||||
:btTypedObject(type),
|
||||
m_userConstraintType(-1),
|
||||
m_userConstraintPtr((void*)-1),
|
||||
m_breakingImpulseThreshold(SIMD_INFINITY),
|
||||
m_isEnabled(true),
|
||||
m_needsFeedback(false),
|
||||
m_overrideNumSolverIterations(-1),
|
||||
m_rbA(rbA),
|
||||
m_rbB(rbB),
|
||||
m_appliedImpulse(btScalar(0.)),
|
||||
m_dbgDrawSize(DEFAULT_DEBUGDRAW_SIZE),
|
||||
m_jointFeedback(0)
|
||||
btTypedConstraint::btTypedConstraint(btTypedConstraintType type, btRigidBody& rbA, btRigidBody& rbB)
|
||||
: btTypedObject(type),
|
||||
m_userConstraintType(-1),
|
||||
m_userConstraintPtr((void*)-1),
|
||||
m_breakingImpulseThreshold(SIMD_INFINITY),
|
||||
m_isEnabled(true),
|
||||
m_needsFeedback(false),
|
||||
m_overrideNumSolverIterations(-1),
|
||||
m_rbA(rbA),
|
||||
m_rbB(rbB),
|
||||
m_appliedImpulse(btScalar(0.)),
|
||||
m_dbgDrawSize(DEFAULT_DEBUGDRAW_SIZE),
|
||||
m_jointFeedback(0)
|
||||
{
|
||||
}
|
||||
|
||||
|
||||
|
||||
|
||||
btScalar btTypedConstraint::getMotorFactor(btScalar pos, btScalar lowLim, btScalar uppLim, btScalar vel, btScalar timeFact)
|
||||
{
|
||||
if(lowLim > uppLim)
|
||||
if (lowLim > uppLim)
|
||||
{
|
||||
return btScalar(1.0f);
|
||||
}
|
||||
else if(lowLim == uppLim)
|
||||
else if (lowLim == uppLim)
|
||||
{
|
||||
return btScalar(0.0f);
|
||||
}
|
||||
btScalar lim_fact = btScalar(1.0f);
|
||||
btScalar delta_max = vel / timeFact;
|
||||
if(delta_max < btScalar(0.0f))
|
||||
if (delta_max < btScalar(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 = btScalar(0.0f);
|
||||
}
|
||||
@@ -84,13 +78,13 @@ btScalar btTypedConstraint::getMotorFactor(btScalar pos, btScalar lowLim, btScal
|
||||
lim_fact = btScalar(1.0f);
|
||||
}
|
||||
}
|
||||
else if(delta_max > btScalar(0.0f))
|
||||
else if (delta_max > btScalar(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 = btScalar(0.0f);
|
||||
}
|
||||
@@ -101,19 +95,19 @@ btScalar btTypedConstraint::getMotorFactor(btScalar pos, btScalar lowLim, btScal
|
||||
}
|
||||
else
|
||||
{
|
||||
lim_fact = btScalar(0.0f);
|
||||
lim_fact = btScalar(0.0f);
|
||||
}
|
||||
return lim_fact;
|
||||
}
|
||||
|
||||
///fills the dataBuffer and returns the struct name (and 0 on failure)
|
||||
const char* btTypedConstraint::serialize(void* dataBuffer, btSerializer* serializer) const
|
||||
const char* btTypedConstraint::serialize(void* dataBuffer, btSerializer* serializer) const
|
||||
{
|
||||
btTypedConstraintData2* tcd = (btTypedConstraintData2*) dataBuffer;
|
||||
btTypedConstraintData2* tcd = (btTypedConstraintData2*)dataBuffer;
|
||||
|
||||
tcd->m_rbA = (btRigidBodyData*)serializer->getUniquePointer(&m_rbA);
|
||||
tcd->m_rbB = (btRigidBodyData*)serializer->getUniquePointer(&m_rbB);
|
||||
char* name = (char*) serializer->findNameForPointer(this);
|
||||
char* name = (char*)serializer->findNameForPointer(this);
|
||||
tcd->m_name = (char*)serializer->getUniquePointer(name);
|
||||
if (tcd->m_name)
|
||||
{
|
||||
@@ -124,10 +118,10 @@ const char* btTypedConstraint::serialize(void* dataBuffer, btSerializer* seriali
|
||||
tcd->m_needsFeedback = m_needsFeedback;
|
||||
tcd->m_overrideNumSolverIterations = m_overrideNumSolverIterations;
|
||||
tcd->m_breakingImpulseThreshold = m_breakingImpulseThreshold;
|
||||
tcd->m_isEnabled = m_isEnabled? 1: 0;
|
||||
|
||||
tcd->m_userConstraintId =m_userConstraintId;
|
||||
tcd->m_userConstraintType =m_userConstraintType;
|
||||
tcd->m_isEnabled = m_isEnabled ? 1 : 0;
|
||||
|
||||
tcd->m_userConstraintId = m_userConstraintId;
|
||||
tcd->m_userConstraintType = m_userConstraintType;
|
||||
|
||||
tcd->m_appliedImpulse = m_appliedImpulse;
|
||||
tcd->m_dbgDrawSize = m_dbgDrawSize;
|
||||
@@ -135,10 +129,10 @@ const char* btTypedConstraint::serialize(void* dataBuffer, btSerializer* seriali
|
||||
tcd->m_disableCollisionsBetweenLinkedBodies = false;
|
||||
|
||||
int i;
|
||||
for (i=0;i<m_rbA.getNumConstraintRefs();i++)
|
||||
for (i = 0; i < m_rbA.getNumConstraintRefs(); i++)
|
||||
if (m_rbA.getConstraintRef(i) == this)
|
||||
tcd->m_disableCollisionsBetweenLinkedBodies = true;
|
||||
for (i=0;i<m_rbB.getNumConstraintRefs();i++)
|
||||
for (i = 0; i < m_rbB.getNumConstraintRefs(); i++)
|
||||
if (m_rbB.getConstraintRef(i) == this)
|
||||
tcd->m_disableCollisionsBetweenLinkedBodies = true;
|
||||
|
||||
@@ -147,17 +141,16 @@ const char* btTypedConstraint::serialize(void* dataBuffer, btSerializer* seriali
|
||||
|
||||
btRigidBody& btTypedConstraint::getFixedBody()
|
||||
{
|
||||
static btRigidBody s_fixed(0, 0,0);
|
||||
s_fixed.setMassProps(btScalar(0.),btVector3(btScalar(0.),btScalar(0.),btScalar(0.)));
|
||||
static btRigidBody s_fixed(0, 0, 0);
|
||||
s_fixed.setMassProps(btScalar(0.), btVector3(btScalar(0.), btScalar(0.), btScalar(0.)));
|
||||
return s_fixed;
|
||||
}
|
||||
|
||||
|
||||
void btAngularLimit::set(btScalar low, btScalar high, btScalar _softness, btScalar _biasFactor, btScalar _relaxationFactor)
|
||||
{
|
||||
m_halfRange = (high - low) / 2.0f;
|
||||
m_center = btNormalizeAngle(low + m_halfRange);
|
||||
m_softness = _softness;
|
||||
m_softness = _softness;
|
||||
m_biasFactor = _biasFactor;
|
||||
m_relaxationFactor = _relaxationFactor;
|
||||
}
|
||||
@@ -174,7 +167,7 @@ void btAngularLimit::test(const btScalar 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)
|
||||
@@ -186,7 +179,6 @@ void btAngularLimit::test(const btScalar angle)
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
btScalar btAngularLimit::getError() const
|
||||
{
|
||||
return m_correction * m_sign;
|
||||
|
||||
@@ -16,26 +16,24 @@ subject to the following restrictions:
|
||||
#ifndef BT_TYPED_CONSTRAINT_H
|
||||
#define BT_TYPED_CONSTRAINT_H
|
||||
|
||||
|
||||
#include "LinearMath/btScalar.h"
|
||||
#include "btSolverConstraint.h"
|
||||
#include "BulletDynamics/Dynamics/btRigidBody.h"
|
||||
|
||||
#ifdef BT_USE_DOUBLE_PRECISION
|
||||
#define btTypedConstraintData2 btTypedConstraintDoubleData
|
||||
#define btTypedConstraintDataName "btTypedConstraintDoubleData"
|
||||
#define btTypedConstraintData2 btTypedConstraintDoubleData
|
||||
#define btTypedConstraintDataName "btTypedConstraintDoubleData"
|
||||
#else
|
||||
#define btTypedConstraintData2 btTypedConstraintFloatData
|
||||
#define btTypedConstraintDataName "btTypedConstraintFloatData"
|
||||
#endif //BT_USE_DOUBLE_PRECISION
|
||||
|
||||
#define btTypedConstraintData2 btTypedConstraintFloatData
|
||||
#define btTypedConstraintDataName "btTypedConstraintFloatData"
|
||||
#endif //BT_USE_DOUBLE_PRECISION
|
||||
|
||||
class btSerializer;
|
||||
|
||||
//Don't change any of the existing enum values, so add enum types at the end for serialization compatibility
|
||||
enum btTypedConstraintType
|
||||
{
|
||||
POINT2POINT_CONSTRAINT_TYPE=3,
|
||||
POINT2POINT_CONSTRAINT_TYPE = 3,
|
||||
HINGE_CONSTRAINT_TYPE,
|
||||
CONETWIST_CONSTRAINT_TYPE,
|
||||
D6_CONSTRAINT_TYPE,
|
||||
@@ -48,91 +46,88 @@ enum btTypedConstraintType
|
||||
MAX_CONSTRAINT_TYPE
|
||||
};
|
||||
|
||||
|
||||
enum btConstraintParams
|
||||
{
|
||||
BT_CONSTRAINT_ERP=1,
|
||||
BT_CONSTRAINT_ERP = 1,
|
||||
BT_CONSTRAINT_STOP_ERP,
|
||||
BT_CONSTRAINT_CFM,
|
||||
BT_CONSTRAINT_STOP_CFM
|
||||
};
|
||||
|
||||
#if 1
|
||||
#define btAssertConstrParams(_par) btAssert(_par)
|
||||
#define btAssertConstrParams(_par) btAssert(_par)
|
||||
#else
|
||||
#define btAssertConstrParams(_par)
|
||||
#define btAssertConstrParams(_par)
|
||||
#endif
|
||||
|
||||
|
||||
ATTRIBUTE_ALIGNED16(struct) btJointFeedback
|
||||
ATTRIBUTE_ALIGNED16(struct)
|
||||
btJointFeedback
|
||||
{
|
||||
BT_DECLARE_ALIGNED_ALLOCATOR();
|
||||
btVector3 m_appliedForceBodyA;
|
||||
btVector3 m_appliedTorqueBodyA;
|
||||
btVector3 m_appliedForceBodyB;
|
||||
btVector3 m_appliedTorqueBodyB;
|
||||
btVector3 m_appliedForceBodyA;
|
||||
btVector3 m_appliedTorqueBodyA;
|
||||
btVector3 m_appliedForceBodyB;
|
||||
btVector3 m_appliedTorqueBodyB;
|
||||
};
|
||||
|
||||
|
||||
///TypedConstraint is the baseclass for Bullet constraints and vehicles
|
||||
ATTRIBUTE_ALIGNED16(class) btTypedConstraint : public btTypedObject
|
||||
ATTRIBUTE_ALIGNED16(class)
|
||||
btTypedConstraint : public btTypedObject
|
||||
{
|
||||
int m_userConstraintType;
|
||||
int m_userConstraintType;
|
||||
|
||||
union
|
||||
{
|
||||
int m_userConstraintId;
|
||||
union {
|
||||
int m_userConstraintId;
|
||||
void* m_userConstraintPtr;
|
||||
};
|
||||
|
||||
btScalar m_breakingImpulseThreshold;
|
||||
bool m_isEnabled;
|
||||
bool m_needsFeedback;
|
||||
int m_overrideNumSolverIterations;
|
||||
btScalar m_breakingImpulseThreshold;
|
||||
bool m_isEnabled;
|
||||
bool m_needsFeedback;
|
||||
int m_overrideNumSolverIterations;
|
||||
|
||||
|
||||
btTypedConstraint& operator=(btTypedConstraint& other)
|
||||
btTypedConstraint& operator=(btTypedConstraint& other)
|
||||
{
|
||||
btAssert(0);
|
||||
(void) other;
|
||||
(void)other;
|
||||
return *this;
|
||||
}
|
||||
|
||||
protected:
|
||||
btRigidBody& m_rbA;
|
||||
btRigidBody& m_rbB;
|
||||
btScalar m_appliedImpulse;
|
||||
btScalar m_dbgDrawSize;
|
||||
btJointFeedback* m_jointFeedback;
|
||||
btRigidBody& m_rbA;
|
||||
btRigidBody& m_rbB;
|
||||
btScalar m_appliedImpulse;
|
||||
btScalar m_dbgDrawSize;
|
||||
btJointFeedback* m_jointFeedback;
|
||||
|
||||
///internal method used by the constraint solver, don't use them directly
|
||||
btScalar getMotorFactor(btScalar pos, btScalar lowLim, btScalar uppLim, btScalar vel, btScalar timeFact);
|
||||
|
||||
|
||||
public:
|
||||
|
||||
BT_DECLARE_ALIGNED_ALLOCATOR();
|
||||
|
||||
virtual ~btTypedConstraint() {};
|
||||
btTypedConstraint(btTypedConstraintType type, btRigidBody& rbA);
|
||||
btTypedConstraint(btTypedConstraintType type, btRigidBody& rbA,btRigidBody& rbB);
|
||||
virtual ~btTypedConstraint(){};
|
||||
btTypedConstraint(btTypedConstraintType type, btRigidBody & rbA);
|
||||
btTypedConstraint(btTypedConstraintType type, btRigidBody & rbA, btRigidBody & rbB);
|
||||
|
||||
struct btConstraintInfo1 {
|
||||
int m_numConstraintRows,nub;
|
||||
struct btConstraintInfo1
|
||||
{
|
||||
int m_numConstraintRows, nub;
|
||||
};
|
||||
|
||||
static btRigidBody& getFixedBody();
|
||||
|
||||
struct btConstraintInfo2 {
|
||||
struct btConstraintInfo2
|
||||
{
|
||||
// integrator parameters: frames per second (1/stepsize), default error
|
||||
// reduction parameter (0..1).
|
||||
btScalar fps,erp;
|
||||
btScalar 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.
|
||||
btScalar *m_J1linearAxis,*m_J1angularAxis,*m_J2linearAxis,*m_J2angularAxis;
|
||||
btScalar *m_J1linearAxis, *m_J1angularAxis, *m_J2linearAxis, *m_J2angularAxis;
|
||||
|
||||
// elements to jump from one row to the next in J's
|
||||
int rowskip;
|
||||
@@ -140,19 +135,19 @@ 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.
|
||||
btScalar *m_constraintError,*cfm;
|
||||
btScalar *m_constraintError, *cfm;
|
||||
|
||||
// lo and hi limits for variables (set to -/+ infinity on entry).
|
||||
btScalar *m_lowerLimit,*m_upperLimit;
|
||||
btScalar *m_lowerLimit, *m_upperLimit;
|
||||
|
||||
// number of solver iterations
|
||||
int m_numIterations;
|
||||
|
||||
//damping of the velocity
|
||||
btScalar m_damping;
|
||||
btScalar m_damping;
|
||||
};
|
||||
|
||||
int getOverrideNumSolverIterations() const
|
||||
int getOverrideNumSolverIterations() const
|
||||
{
|
||||
return m_overrideNumSolverIterations;
|
||||
}
|
||||
@@ -165,60 +160,57 @@ public:
|
||||
}
|
||||
|
||||
///internal method used by the constraint solver, don't use them directly
|
||||
virtual void buildJacobian() {};
|
||||
virtual void buildJacobian(){};
|
||||
|
||||
///internal method used by the constraint solver, don't use them directly
|
||||
virtual void setupSolverConstraint(btConstraintArray& ca, int solverBodyA,int solverBodyB, btScalar timeStep)
|
||||
virtual void setupSolverConstraint(btConstraintArray & ca, int solverBodyA, int solverBodyB, btScalar 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 (btConstraintInfo1* info)=0;
|
||||
|
||||
///internal method used by the constraint solver, don't use them directly
|
||||
virtual void getInfo2 (btConstraintInfo2* info)=0;
|
||||
virtual void getInfo1(btConstraintInfo1 * info) = 0;
|
||||
|
||||
///internal method used by the constraint solver, don't use them directly
|
||||
void internalSetAppliedImpulse(btScalar appliedImpulse)
|
||||
virtual void getInfo2(btConstraintInfo2 * info) = 0;
|
||||
|
||||
///internal method used by the constraint solver, don't use them directly
|
||||
void internalSetAppliedImpulse(btScalar appliedImpulse)
|
||||
{
|
||||
m_appliedImpulse = appliedImpulse;
|
||||
}
|
||||
///internal method used by the constraint solver, don't use them directly
|
||||
btScalar internalGetAppliedImpulse()
|
||||
btScalar internalGetAppliedImpulse()
|
||||
{
|
||||
return m_appliedImpulse;
|
||||
}
|
||||
|
||||
|
||||
btScalar getBreakingImpulseThreshold() const
|
||||
btScalar getBreakingImpulseThreshold() const
|
||||
{
|
||||
return m_breakingImpulseThreshold;
|
||||
return m_breakingImpulseThreshold;
|
||||
}
|
||||
|
||||
void setBreakingImpulseThreshold(btScalar threshold)
|
||||
void setBreakingImpulseThreshold(btScalar 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(btSolverBody& /*bodyA*/,btSolverBody& /*bodyB*/,btScalar /*timeStep*/) {};
|
||||
virtual void solveConstraintObsolete(btSolverBody& /*bodyA*/, btSolverBody& /*bodyB*/, btScalar /*timeStep*/){};
|
||||
|
||||
|
||||
const btRigidBody& getRigidBodyA() const
|
||||
{
|
||||
return m_rbA;
|
||||
@@ -228,7 +220,7 @@ public:
|
||||
return m_rbB;
|
||||
}
|
||||
|
||||
btRigidBody& getRigidBodyA()
|
||||
btRigidBody& getRigidBodyA()
|
||||
{
|
||||
return m_rbA;
|
||||
}
|
||||
@@ -239,15 +231,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;
|
||||
}
|
||||
@@ -257,17 +249,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(btJointFeedback* jointFeedback)
|
||||
void setJointFeedback(btJointFeedback * jointFeedback)
|
||||
{
|
||||
m_jointFeedback = jointFeedback;
|
||||
}
|
||||
@@ -282,37 +274,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.
|
||||
btScalar getAppliedImpulse() const
|
||||
btScalar getAppliedImpulse() const
|
||||
{
|
||||
btAssert(m_needsFeedback);
|
||||
return m_appliedImpulse;
|
||||
}
|
||||
|
||||
btTypedConstraintType getConstraintType () const
|
||||
btTypedConstraintType getConstraintType() const
|
||||
{
|
||||
return btTypedConstraintType(m_objectType);
|
||||
}
|
||||
|
||||
|
||||
void setDbgDrawSize(btScalar dbgDrawSize)
|
||||
{
|
||||
m_dbgDrawSize = dbgDrawSize;
|
||||
@@ -322,35 +313,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, btScalar value, int axis = -1) = 0;
|
||||
virtual void setParam(int num, btScalar value, int axis = -1) = 0;
|
||||
|
||||
///return the local value of parameter
|
||||
virtual btScalar getParam(int num, int axis = -1) const = 0;
|
||||
|
||||
virtual int calculateSerializeBufferSize() const;
|
||||
virtual btScalar 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, btSerializer* serializer) const;
|
||||
|
||||
virtual const char* serialize(void* dataBuffer, btSerializer* serializer) const;
|
||||
};
|
||||
|
||||
// returns angle in range [-SIMD_2_PI, SIMD_2_PI], closest to one of the limits
|
||||
// returns angle in range [-SIMD_2_PI, SIMD_2_PI], closest to one of the limits
|
||||
// all arguments should be normalized angles (i.e. in range [-SIMD_PI, SIMD_PI])
|
||||
SIMD_FORCE_INLINE btScalar btAdjustAngleToLimits(btScalar angleInRadians, btScalar angleLowerLimitInRadians, btScalar angleUpperLimitInRadians)
|
||||
{
|
||||
if(angleLowerLimitInRadians >= angleUpperLimitInRadians)
|
||||
if (angleLowerLimitInRadians >= angleUpperLimitInRadians)
|
||||
{
|
||||
return angleInRadians;
|
||||
}
|
||||
else if(angleInRadians < angleLowerLimitInRadians)
|
||||
else if (angleInRadians < angleLowerLimitInRadians)
|
||||
{
|
||||
btScalar diffLo = btFabs(btNormalizeAngle(angleLowerLimitInRadians - angleInRadians));
|
||||
btScalar diffHi = btFabs(btNormalizeAngle(angleUpperLimitInRadians - angleInRadians));
|
||||
return (diffLo < diffHi) ? angleInRadians : (angleInRadians + SIMD_2_PI);
|
||||
}
|
||||
else if(angleInRadians > angleUpperLimitInRadians)
|
||||
else if (angleInRadians > angleUpperLimitInRadians)
|
||||
{
|
||||
btScalar diffHi = btFabs(btNormalizeAngle(angleInRadians - angleUpperLimitInRadians));
|
||||
btScalar diffLo = btFabs(btNormalizeAngle(angleInRadians - angleLowerLimitInRadians));
|
||||
@@ -362,6 +352,8 @@ SIMD_FORCE_INLINE btScalar btAdjustAngleToLimits(btScalar angleInRadians, btScal
|
||||
}
|
||||
}
|
||||
|
||||
// clang-format off
|
||||
|
||||
///do not change those serialization structures, it requires an updated sBulletDNAstr/sBulletDNAstr64
|
||||
struct btTypedConstraintFloatData
|
||||
{
|
||||
@@ -385,6 +377,8 @@ struct btTypedConstraintFloatData
|
||||
|
||||
};
|
||||
|
||||
|
||||
|
||||
///do not change those serialization structures, it requires an updated sBulletDNAstr/sBulletDNAstr64
|
||||
|
||||
#define BT_BACKWARDS_COMPATIBLE_SERIALIZATION
|
||||
@@ -436,18 +430,17 @@ struct btTypedConstraintDoubleData
|
||||
|
||||
};
|
||||
|
||||
// clang-format on
|
||||
|
||||
SIMD_FORCE_INLINE int btTypedConstraint::calculateSerializeBufferSize() const
|
||||
SIMD_FORCE_INLINE int btTypedConstraint::calculateSerializeBufferSize() const
|
||||
{
|
||||
return sizeof(btTypedConstraintData2);
|
||||
}
|
||||
|
||||
|
||||
|
||||
class btAngularLimit
|
||||
{
|
||||
private:
|
||||
btScalar
|
||||
btScalar
|
||||
m_center,
|
||||
m_halfRange,
|
||||
m_softness,
|
||||
@@ -462,15 +455,16 @@ private:
|
||||
public:
|
||||
/// Default constructor initializes limit as inactive, allowing free constraint movement
|
||||
btAngularLimit()
|
||||
: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.
|
||||
@@ -499,13 +493,13 @@ public:
|
||||
return m_relaxationFactor;
|
||||
}
|
||||
|
||||
/// Returns correction value evaluated when test() was invoked
|
||||
/// Returns correction value evaluated when test() was invoked
|
||||
inline btScalar getCorrection() const
|
||||
{
|
||||
return m_correction;
|
||||
}
|
||||
|
||||
/// Returns sign value evaluated when test() was invoked
|
||||
/// Returns sign value evaluated when test() was invoked
|
||||
inline btScalar getSign() const
|
||||
{
|
||||
return m_sign;
|
||||
@@ -533,9 +527,6 @@ public:
|
||||
btScalar getLow() const;
|
||||
|
||||
btScalar getHigh() const;
|
||||
|
||||
};
|
||||
|
||||
|
||||
|
||||
#endif //BT_TYPED_CONSTRAINT_H
|
||||
#endif //BT_TYPED_CONSTRAINT_H
|
||||
|
||||
@@ -13,43 +13,38 @@ subject to the following restrictions:
|
||||
3. This notice may not be removed or altered from any source distribution.
|
||||
*/
|
||||
|
||||
|
||||
|
||||
#include "btUniversalConstraint.h"
|
||||
#include "BulletDynamics/Dynamics/btRigidBody.h"
|
||||
#include "LinearMath/btTransformUtil.h"
|
||||
|
||||
|
||||
|
||||
#define UNIV_EPS btScalar(0.01f)
|
||||
|
||||
|
||||
// constructor
|
||||
// anchor, axis1 and axis2 are in world coordinate system
|
||||
// axis1 must be orthogonal to axis2
|
||||
btUniversalConstraint::btUniversalConstraint(btRigidBody& rbA, btRigidBody& rbB, const btVector3& anchor, const btVector3& axis1, const btVector3& axis2)
|
||||
: btGeneric6DofConstraint(rbA, rbB, btTransform::getIdentity(), btTransform::getIdentity(), true),
|
||||
m_anchor(anchor),
|
||||
m_axis1(axis1),
|
||||
m_axis2(axis2)
|
||||
: btGeneric6DofConstraint(rbA, rbB, btTransform::getIdentity(), btTransform::getIdentity(), true),
|
||||
m_anchor(anchor),
|
||||
m_axis1(axis1),
|
||||
m_axis2(axis2)
|
||||
{
|
||||
// build frame basis
|
||||
// 6DOF constraint uses Euler angles and to define limits
|
||||
// it is assumed that rotational order is :
|
||||
// Z - first, allowed limits are (-PI,PI);
|
||||
// new position of Y - second (allowed limits are (-PI/2 + epsilon, PI/2 - epsilon), where epsilon is a small positive number
|
||||
// new position of Y - second (allowed limits are (-PI/2 + epsilon, PI/2 - epsilon), where epsilon is a small positive number
|
||||
// used to prevent constraint from instability on poles;
|
||||
// new position of X, allowed limits are (-PI,PI);
|
||||
// So to simulate ODE Universal joint we should use parent axis as Z, child axis as Y and limit all other DOFs
|
||||
// Build the frame in world coordinate system first
|
||||
btVector3 zAxis = m_axis1.normalize();
|
||||
btVector3 yAxis = m_axis2.normalize();
|
||||
btVector3 xAxis = yAxis.cross(zAxis); // we want right coordinate system
|
||||
btVector3 xAxis = yAxis.cross(zAxis); // we want right coordinate system
|
||||
btTransform 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]);
|
||||
frameInW.setOrigin(anchor);
|
||||
// now get constraint frame in local coordinate systems
|
||||
m_frameInA = rbA.getCenterOfMassTransform().inverse() * frameInW;
|
||||
@@ -58,30 +53,28 @@ btUniversalConstraint::btUniversalConstraint(btRigidBody& rbA, btRigidBody& rbB,
|
||||
setLinearLowerLimit(btVector3(0., 0., 0.));
|
||||
setLinearUpperLimit(btVector3(0., 0., 0.));
|
||||
setAngularLowerLimit(btVector3(0.f, -SIMD_HALF_PI + UNIV_EPS, -SIMD_PI + UNIV_EPS));
|
||||
setAngularUpperLimit(btVector3(0.f, SIMD_HALF_PI - UNIV_EPS, SIMD_PI - UNIV_EPS));
|
||||
setAngularUpperLimit(btVector3(0.f, SIMD_HALF_PI - UNIV_EPS, SIMD_PI - UNIV_EPS));
|
||||
}
|
||||
|
||||
void btUniversalConstraint::setAxis(const btVector3& axis1,const btVector3& axis2)
|
||||
void btUniversalConstraint::setAxis(const btVector3& axis1, const btVector3& axis2)
|
||||
{
|
||||
m_axis1 = axis1;
|
||||
m_axis2 = axis2;
|
||||
m_axis1 = axis1;
|
||||
m_axis2 = axis2;
|
||||
|
||||
btVector3 zAxis = axis1.normalized();
|
||||
btVector3 yAxis = axis2.normalized();
|
||||
btVector3 xAxis = yAxis.cross(zAxis); // we want right coordinate system
|
||||
btVector3 xAxis = yAxis.cross(zAxis); // we want right coordinate system
|
||||
|
||||
btTransform 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]);
|
||||
frameInW.setOrigin(m_anchor);
|
||||
|
||||
// now get constraint frame in local coordinate systems
|
||||
m_frameInA = m_rbA.getCenterOfMassTransform().inverse() * frameInW;
|
||||
m_frameInB = m_rbB.getCenterOfMassTransform().inverse() * frameInW;
|
||||
|
||||
calculateTransforms();
|
||||
calculateTransforms();
|
||||
}
|
||||
|
||||
|
||||
|
||||
@@ -16,35 +16,32 @@ subject to the following restrictions:
|
||||
#ifndef BT_UNIVERSAL_CONSTRAINT_H
|
||||
#define BT_UNIVERSAL_CONSTRAINT_H
|
||||
|
||||
|
||||
|
||||
#include "LinearMath/btVector3.h"
|
||||
#include "btTypedConstraint.h"
|
||||
#include "btGeneric6DofConstraint.h"
|
||||
|
||||
|
||||
|
||||
/// Constraint similar to ODE Universal Joint
|
||||
/// has 2 rotatioonal degrees of freedom, similar to Euler rotations around Z (axis 1)
|
||||
/// and Y (axis 2)
|
||||
/// Description from ODE manual :
|
||||
/// "Given axis 1 on body 1, and axis 2 on body 2 that is perpendicular to axis 1, it keeps them perpendicular.
|
||||
/// Description from ODE manual :
|
||||
/// "Given axis 1 on body 1, and axis 2 on body 2 that is perpendicular to axis 1, it keeps them perpendicular.
|
||||
/// In other words, rotation of the two bodies about the direction perpendicular to the two axes will be equal."
|
||||
|
||||
ATTRIBUTE_ALIGNED16(class) btUniversalConstraint : public btGeneric6DofConstraint
|
||||
ATTRIBUTE_ALIGNED16(class)
|
||||
btUniversalConstraint : public btGeneric6DofConstraint
|
||||
{
|
||||
protected:
|
||||
btVector3 m_anchor;
|
||||
btVector3 m_axis1;
|
||||
btVector3 m_axis2;
|
||||
btVector3 m_anchor;
|
||||
btVector3 m_axis1;
|
||||
btVector3 m_axis2;
|
||||
|
||||
public:
|
||||
|
||||
BT_DECLARE_ALIGNED_ALLOCATOR();
|
||||
|
||||
|
||||
// constructor
|
||||
// anchor, axis1 and axis2 are in world coordinate system
|
||||
// axis1 must be orthogonal to axis2
|
||||
btUniversalConstraint(btRigidBody& rbA, btRigidBody& rbB, const btVector3& anchor, const btVector3& axis1, const btVector3& axis2);
|
||||
btUniversalConstraint(btRigidBody & rbA, btRigidBody & rbB, const btVector3& anchor, const btVector3& axis1, const btVector3& axis2);
|
||||
// access
|
||||
const btVector3& getAnchor() { return m_calculatedTransformA.getOrigin(); }
|
||||
const btVector3& getAnchor2() { return m_calculatedTransformB.getOrigin(); }
|
||||
@@ -56,10 +53,7 @@ public:
|
||||
void setUpperLimit(btScalar ang1max, btScalar ang2max) { setAngularUpperLimit(btVector3(0.f, ang1max, ang2max)); }
|
||||
void setLowerLimit(btScalar ang1min, btScalar ang2min) { setAngularLowerLimit(btVector3(0.f, ang1min, ang2min)); }
|
||||
|
||||
void setAxis( const btVector3& axis1, const btVector3& axis2);
|
||||
void setAxis(const btVector3& axis1, const btVector3& axis2);
|
||||
};
|
||||
|
||||
|
||||
|
||||
#endif // BT_UNIVERSAL_CONSTRAINT_H
|
||||
|
||||
#endif // BT_UNIVERSAL_CONSTRAINT_H
|
||||
|
||||
Reference in New Issue
Block a user