mirror of
https://github.com/TorqueGameEngines/Torque3D.git
synced 2026-04-24 13:55:34 +00:00
Bullet 2.82 update
This commit is contained in:
rextimmy
parent
d0a64026b0
commit
416c50690e
146 changed files with 12202 additions and 1422 deletions
|
|
@ -53,6 +53,7 @@ btConeTwistConstraint::btConeTwistConstraint(btRigidBody& rbA,const btTransform&
|
|||
m_useSolveConstraintObsolete(CONETWIST_USE_OBSOLETE_SOLVER)
|
||||
{
|
||||
m_rbBFrame = m_rbAFrame;
|
||||
m_rbBFrame.setOrigin(btVector3(0., 0., 0.));
|
||||
init();
|
||||
}
|
||||
|
||||
|
|
@ -136,6 +137,9 @@ void btConeTwistConstraint::getInfo2NonVirtual (btConstraintInfo2* info,const bt
|
|||
btVector3 a1neg = -a1;
|
||||
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 = transB.getBasis() * m_rbBFrame.getOrigin();
|
||||
{
|
||||
btVector3* angular0 = (btVector3*)(info->m_J2angularAxis);
|
||||
|
|
@ -725,7 +729,8 @@ void btConeTwistConstraint::calcAngleInfo2(const btTransform& transA, const btTr
|
|||
{
|
||||
if(m_swingSpan1 < m_fixThresh)
|
||||
{ // hinge around Y axis
|
||||
if(!(btFuzzyZero(y)))
|
||||
// if(!(btFuzzyZero(y)))
|
||||
if((!(btFuzzyZero(x))) || (!(btFuzzyZero(z))))
|
||||
{
|
||||
m_solveSwingLimit = true;
|
||||
if(m_swingSpan2 >= m_fixThresh)
|
||||
|
|
@ -747,7 +752,8 @@ void btConeTwistConstraint::calcAngleInfo2(const btTransform& transA, const btTr
|
|||
}
|
||||
else
|
||||
{ // hinge around Z axis
|
||||
if(!btFuzzyZero(z))
|
||||
// if(!btFuzzyZero(z))
|
||||
if((!(btFuzzyZero(x))) || (!(btFuzzyZero(y))))
|
||||
{
|
||||
m_solveSwingLimit = true;
|
||||
if(m_swingSpan1 >= m_fixThresh)
|
||||
|
|
|
|||
|
|
@ -40,6 +40,15 @@ and swing 1 and 2 are along the z and y axes respectively.
|
|||
#include "btJacobianEntry.h"
|
||||
#include "btTypedConstraint.h"
|
||||
|
||||
#ifdef BT_USE_DOUBLE_PRECISION
|
||||
#define btConeTwistConstraintData2 btConeTwistConstraintDoubleData
|
||||
#define btConeTwistConstraintDataName "btConeTwistConstraintDoubleData"
|
||||
#else
|
||||
#define btConeTwistConstraintData2 btConeTwistConstraintData
|
||||
#define btConeTwistConstraintDataName "btConeTwistConstraintData"
|
||||
#endif //BT_USE_DOUBLE_PRECISION
|
||||
|
||||
|
||||
class btRigidBody;
|
||||
|
||||
enum btConeTwistFlags
|
||||
|
|
@ -295,7 +304,30 @@ public:
|
|||
|
||||
};
|
||||
|
||||
///do not change those serialization structures, it requires an updated sBulletDNAstr/sBulletDNAstr64
|
||||
|
||||
|
||||
struct btConeTwistConstraintDoubleData
|
||||
{
|
||||
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;
|
||||
|
||||
|
||||
|
||||
};
|
||||
|
||||
#ifdef BT_BACKWARDS_COMPATIBLE_SERIALIZATION
|
||||
///this structure is not used, except for loading pre-2.82 .bullet files
|
||||
struct btConeTwistConstraintData
|
||||
{
|
||||
btTypedConstraintData m_typeConstraintData;
|
||||
|
|
@ -315,12 +347,12 @@ struct btConeTwistConstraintData
|
|||
char m_pad[4];
|
||||
|
||||
};
|
||||
|
||||
|
||||
#endif //BT_BACKWARDS_COMPATIBLE_SERIALIZATION
|
||||
//
|
||||
|
||||
SIMD_FORCE_INLINE int btConeTwistConstraint::calculateSerializeBufferSize() const
|
||||
{
|
||||
return sizeof(btConeTwistConstraintData);
|
||||
return sizeof(btConeTwistConstraintData2);
|
||||
|
||||
}
|
||||
|
||||
|
|
@ -328,21 +360,21 @@ SIMD_FORCE_INLINE int btConeTwistConstraint::calculateSerializeBufferSize() cons
|
|||
///fills the dataBuffer and returns the struct name (and 0 on failure)
|
||||
SIMD_FORCE_INLINE const char* btConeTwistConstraint::serialize(void* dataBuffer, btSerializer* serializer) const
|
||||
{
|
||||
btConeTwistConstraintData* cone = (btConeTwistConstraintData*) dataBuffer;
|
||||
btConeTwistConstraintData2* cone = (btConeTwistConstraintData2*) dataBuffer;
|
||||
btTypedConstraint::serialize(&cone->m_typeConstraintData,serializer);
|
||||
|
||||
m_rbAFrame.serializeFloat(cone->m_rbAFrame);
|
||||
m_rbBFrame.serializeFloat(cone->m_rbBFrame);
|
||||
m_rbAFrame.serialize(cone->m_rbAFrame);
|
||||
m_rbBFrame.serialize(cone->m_rbBFrame);
|
||||
|
||||
cone->m_swingSpan1 = float(m_swingSpan1);
|
||||
cone->m_swingSpan2 = float(m_swingSpan2);
|
||||
cone->m_twistSpan = float(m_twistSpan);
|
||||
cone->m_limitSoftness = float(m_limitSoftness);
|
||||
cone->m_biasFactor = float(m_biasFactor);
|
||||
cone->m_relaxationFactor = float(m_relaxationFactor);
|
||||
cone->m_damping = float(m_damping);
|
||||
cone->m_swingSpan1 = m_swingSpan1;
|
||||
cone->m_swingSpan2 = m_swingSpan2;
|
||||
cone->m_twistSpan = m_twistSpan;
|
||||
cone->m_limitSoftness = m_limitSoftness;
|
||||
cone->m_biasFactor = m_biasFactor;
|
||||
cone->m_relaxationFactor = m_relaxationFactor;
|
||||
cone->m_damping = m_damping;
|
||||
|
||||
return "btConeTwistConstraintData";
|
||||
return btConeTwistConstraintDataName;
|
||||
}
|
||||
|
||||
|
||||
|
|
|
|||
|
|
@ -28,6 +28,14 @@ class btIDebugDraw;
|
|||
class btStackAlloc;
|
||||
class btDispatcher;
|
||||
/// btConstraintSolver provides solver interface
|
||||
|
||||
|
||||
enum btConstraintSolverType
|
||||
{
|
||||
BT_SEQUENTIAL_IMPULSE_SOLVER=1,
|
||||
BT_MLCP_SOLVER=2
|
||||
};
|
||||
|
||||
class btConstraintSolver
|
||||
{
|
||||
|
||||
|
|
@ -38,12 +46,16 @@ public:
|
|||
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, btStackAlloc* stackAlloc,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 */, btStackAlloc* /* stackAlloc */) {;}
|
||||
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;
|
||||
|
||||
|
||||
};
|
||||
|
||||
|
||||
|
|
|
|||
|
|
@ -0,0 +1,129 @@
|
|||
/*
|
||||
Bullet Continuous Collision Detection and Physics Library
|
||||
Copyright (c) 2013 Erwin Coumans http://bulletphysics.org
|
||||
|
||||
This software is provided 'as-is', without any express or implied warranty.
|
||||
In no event will the authors be held liable for any damages arising from the use of this software.
|
||||
Permission is granted to anyone to use this software for any purpose,
|
||||
including commercial applications, and to alter it and redistribute it freely,
|
||||
subject to the following restrictions:
|
||||
|
||||
1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
|
||||
2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
|
||||
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)
|
||||
:btTypedConstraint(FIXED_CONSTRAINT_TYPE,rbA,rbB)
|
||||
{
|
||||
m_pivotInA = frameInA.getOrigin();
|
||||
m_pivotInB = frameInB.getOrigin();
|
||||
m_relTargetAB = frameInA.getRotation()*frameInB.getRotation().inverse();
|
||||
|
||||
}
|
||||
|
||||
btFixedConstraint::~btFixedConstraint ()
|
||||
{
|
||||
}
|
||||
|
||||
|
||||
void btFixedConstraint::getInfo1 (btConstraintInfo1* info)
|
||||
{
|
||||
info->m_numConstraintRows = 6;
|
||||
info->nub = 6;
|
||||
}
|
||||
|
||||
void btFixedConstraint::getInfo2 (btConstraintInfo2* info)
|
||||
{
|
||||
//fix the 3 linear degrees of freedom
|
||||
|
||||
|
||||
const btVector3& worldPosA = m_rbA.getCenterOfMassTransform().getOrigin();
|
||||
const btMatrix3x3& worldOrnA = m_rbA.getCenterOfMassTransform().getBasis();
|
||||
const btVector3& worldPosB= m_rbB.getCenterOfMassTransform().getOrigin();
|
||||
const btMatrix3x3& worldOrnB = m_rbB.getCenterOfMassTransform().getBasis();
|
||||
|
||||
|
||||
info->m_J1linearAxis[0] = 1;
|
||||
info->m_J1linearAxis[info->rowskip+1] = 1;
|
||||
info->m_J1linearAxis[2*info->rowskip+2] = 1;
|
||||
|
||||
btVector3 a1 = worldOrnA*m_pivotInA;
|
||||
{
|
||||
btVector3* angular0 = (btVector3*)(info->m_J1angularAxis);
|
||||
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);
|
||||
}
|
||||
|
||||
if (info->m_J2linearAxis)
|
||||
{
|
||||
info->m_J2linearAxis[0] = -1;
|
||||
info->m_J2linearAxis[info->rowskip+1] = -1;
|
||||
info->m_J2linearAxis[2*info->rowskip+2] = -1;
|
||||
}
|
||||
|
||||
btVector3 a2 = worldOrnB*m_pivotInB;
|
||||
|
||||
{
|
||||
// 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);
|
||||
}
|
||||
|
||||
// set right hand side for the linear dofs
|
||||
btScalar k = info->fps * info->erp;
|
||||
|
||||
btVector3 linearError = k*(a2+worldPosB-a1-worldPosA);
|
||||
int j;
|
||||
for (j=0; j<3; j++)
|
||||
{
|
||||
|
||||
|
||||
|
||||
info->m_constraintError[j*info->rowskip] = linearError[j];
|
||||
//printf("info->m_constraintError[%d]=%f\n",j,info->m_constraintError[j]);
|
||||
}
|
||||
|
||||
//fix the 3 angular degrees of freedom
|
||||
|
||||
int start_row = 3;
|
||||
int s = info->rowskip;
|
||||
int start_index = start_row * s;
|
||||
|
||||
// 3 rows to make body rotations equal
|
||||
info->m_J1angularAxis[start_index] = 1;
|
||||
info->m_J1angularAxis[start_index + s + 1] = 1;
|
||||
info->m_J1angularAxis[start_index + s*2+2] = 1;
|
||||
if ( info->m_J2angularAxis)
|
||||
{
|
||||
info->m_J2angularAxis[start_index] = -1;
|
||||
info->m_J2angularAxis[start_index + s+1] = -1;
|
||||
info->m_J2angularAxis[start_index + s*2+2] = -1;
|
||||
}
|
||||
|
||||
// set right hand side for the angular dofs
|
||||
|
||||
btVector3 diff;
|
||||
btScalar angle;
|
||||
btMatrix3x3 mrelCur = worldOrnA *worldOrnB.inverse();
|
||||
btQuaternion qrelCur;
|
||||
mrelCur.getRotation(qrelCur);
|
||||
btTransformUtil::calculateDiffAxisAngleQuaternion(m_relTargetAB,qrelCur,diff,angle);
|
||||
diff*=-angle;
|
||||
for (j=0; j<3; j++)
|
||||
{
|
||||
info->m_constraintError[(3+j)*info->rowskip] = k * diff[j];
|
||||
}
|
||||
|
||||
}
|
||||
|
|
@ -0,0 +1,49 @@
|
|||
/*
|
||||
Bullet Continuous Collision Detection and Physics Library
|
||||
Copyright (c) 2013 Erwin Coumans http://bulletphysics.org
|
||||
|
||||
This software is provided 'as-is', without any express or implied warranty.
|
||||
In no event will the authors be held liable for any damages arising from the use of this software.
|
||||
Permission is granted to anyone to use this software for any purpose,
|
||||
including commercial applications, and to alter it and redistribute it freely,
|
||||
subject to the following restrictions:
|
||||
|
||||
1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
|
||||
2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
|
||||
3. This notice may not be removed or altered from any source distribution.
|
||||
*/
|
||||
|
||||
#ifndef BT_FIXED_CONSTRAINT_H
|
||||
#define BT_FIXED_CONSTRAINT_H
|
||||
|
||||
#include "btTypedConstraint.h"
|
||||
|
||||
ATTRIBUTE_ALIGNED16(class) btFixedConstraint : public btTypedConstraint
|
||||
{
|
||||
btVector3 m_pivotInA;
|
||||
btVector3 m_pivotInB;
|
||||
btQuaternion m_relTargetAB;
|
||||
|
||||
public:
|
||||
btFixedConstraint(btRigidBody& rbA,btRigidBody& rbB, const btTransform& frameInA,const btTransform& frameInB);
|
||||
|
||||
virtual ~btFixedConstraint();
|
||||
|
||||
|
||||
virtual void getInfo1 (btConstraintInfo1* info);
|
||||
|
||||
virtual void getInfo2 (btConstraintInfo2* info);
|
||||
|
||||
virtual void setParam(int num, btScalar value, int axis = -1)
|
||||
{
|
||||
btAssert(0);
|
||||
}
|
||||
virtual btScalar getParam(int num, int axis = -1) const
|
||||
{
|
||||
btAssert(0);
|
||||
return 0.f;
|
||||
}
|
||||
|
||||
};
|
||||
|
||||
#endif //BT_FIXED_CONSTRAINT_H
|
||||
|
|
@ -19,6 +19,18 @@ subject to the following restrictions:
|
|||
#define BT_GEAR_CONSTRAINT_H
|
||||
|
||||
#include "BulletDynamics/ConstraintSolver/btTypedConstraint.h"
|
||||
|
||||
|
||||
#ifdef BT_USE_DOUBLE_PRECISION
|
||||
#define btGearConstraintData btGearConstraintDoubleData
|
||||
#define btGearConstraintDataName "btGearConstraintDoubleData"
|
||||
#else
|
||||
#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
|
||||
|
|
@ -39,18 +51,102 @@ public:
|
|||
///internal method used by the constraint solver, don't use them directly
|
||||
virtual void getInfo2 (btConstraintInfo2* info);
|
||||
|
||||
void setAxisA(btVector3& axisA)
|
||||
{
|
||||
m_axisInA = axisA;
|
||||
}
|
||||
void setAxisB(btVector3& axisB)
|
||||
{
|
||||
m_axisInB = axisB;
|
||||
}
|
||||
void setRatio(btScalar ratio)
|
||||
{
|
||||
m_ratio = ratio;
|
||||
}
|
||||
const btVector3& getAxisA() const
|
||||
{
|
||||
return m_axisInA;
|
||||
}
|
||||
const btVector3& getAxisB() const
|
||||
{
|
||||
return m_axisInB;
|
||||
}
|
||||
btScalar getRatio() const
|
||||
{
|
||||
return m_ratio;
|
||||
}
|
||||
|
||||
|
||||
virtual void setParam(int num, btScalar value, int axis = -1)
|
||||
{
|
||||
(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;
|
||||
btAssert(0);
|
||||
return 0.f;
|
||||
}
|
||||
|
||||
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;
|
||||
};
|
||||
|
||||
|
||||
|
||||
|
||||
///do not change those serialization structures, it requires an updated sBulletDNAstr/sBulletDNAstr64
|
||||
struct btGearConstraintFloatData
|
||||
{
|
||||
btTypedConstraintFloatData m_typeConstraintData;
|
||||
|
||||
btVector3FloatData m_axisInA;
|
||||
btVector3FloatData m_axisInB;
|
||||
|
||||
float m_ratio;
|
||||
char m_padding[4];
|
||||
};
|
||||
|
||||
struct btGearConstraintDoubleData
|
||||
{
|
||||
btTypedConstraintDoubleData m_typeConstraintData;
|
||||
|
||||
btVector3DoubleData m_axisInA;
|
||||
btVector3DoubleData m_axisInB;
|
||||
|
||||
double m_ratio;
|
||||
};
|
||||
|
||||
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
|
||||
{
|
||||
btGearConstraintData* gear = (btGearConstraintData*)dataBuffer;
|
||||
btTypedConstraint::serialize(&gear->m_typeConstraintData,serializer);
|
||||
|
||||
m_axisInA.serialize( gear->m_axisInA );
|
||||
m_axisInB.serialize( gear->m_axisInB );
|
||||
|
||||
gear->m_ratio = m_ratio;
|
||||
|
||||
return btGearConstraintDataName;
|
||||
}
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
#endif //BT_GEAR_CONSTRAINT_H
|
||||
|
|
|
|||
|
|
@ -781,17 +781,16 @@ int btGeneric6DofConstraint::get_limit_motor_info2(
|
|||
if (powered || limit)
|
||||
{ // if the joint is powered, or has joint limits, add in the extra row
|
||||
btScalar *J1 = rotational ? info->m_J1angularAxis : info->m_J1linearAxis;
|
||||
btScalar *J2 = rotational ? info->m_J2angularAxis : 0;
|
||||
btScalar *J2 = rotational ? info->m_J2angularAxis : info->m_J2linearAxis;
|
||||
J1[srow+0] = ax1[0];
|
||||
J1[srow+1] = ax1[1];
|
||||
J1[srow+2] = ax1[2];
|
||||
if(rotational)
|
||||
{
|
||||
J2[srow+0] = -ax1[0];
|
||||
J2[srow+1] = -ax1[1];
|
||||
J2[srow+2] = -ax1[2];
|
||||
}
|
||||
if((!rotational))
|
||||
|
||||
J2[srow+0] = -ax1[0];
|
||||
J2[srow+1] = -ax1[1];
|
||||
J2[srow+2] = -ax1[2];
|
||||
|
||||
if((!rotational))
|
||||
{
|
||||
if (m_useOffsetForConstraintFrame)
|
||||
{
|
||||
|
|
|
|||
|
|
@ -35,6 +35,14 @@ class btRigidBody;
|
|||
|
||||
|
||||
|
||||
#ifdef BT_USE_DOUBLE_PRECISION
|
||||
#define btGeneric6DofConstraintData2 btGeneric6DofConstraintDoubleData2
|
||||
#define btGeneric6DofConstraintDataName "btGeneric6DofConstraintDoubleData2"
|
||||
#else
|
||||
#define btGeneric6DofConstraintData2 btGeneric6DofConstraintData
|
||||
#define btGeneric6DofConstraintDataName "btGeneric6DofConstraintData"
|
||||
#endif //BT_USE_DOUBLE_PRECISION
|
||||
|
||||
|
||||
//! Rotation Limit structure for generic joints
|
||||
class btRotationalLimitMotor
|
||||
|
|
@ -561,7 +569,7 @@ public:
|
|||
|
||||
};
|
||||
|
||||
///do not change those serialization structures, it requires an updated sBulletDNAstr/sBulletDNAstr64
|
||||
|
||||
struct btGeneric6DofConstraintData
|
||||
{
|
||||
btTypedConstraintData m_typeConstraintData;
|
||||
|
|
@ -578,35 +586,51 @@ struct btGeneric6DofConstraintData
|
|||
int m_useOffsetForConstraintFrame;
|
||||
};
|
||||
|
||||
struct btGeneric6DofConstraintDoubleData2
|
||||
{
|
||||
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;
|
||||
int m_useOffsetForConstraintFrame;
|
||||
};
|
||||
|
||||
SIMD_FORCE_INLINE int btGeneric6DofConstraint::calculateSerializeBufferSize() const
|
||||
{
|
||||
return sizeof(btGeneric6DofConstraintData);
|
||||
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
|
||||
{
|
||||
|
||||
btGeneric6DofConstraintData* dof = (btGeneric6DofConstraintData*)dataBuffer;
|
||||
btGeneric6DofConstraintData2* dof = (btGeneric6DofConstraintData2*)dataBuffer;
|
||||
btTypedConstraint::serialize(&dof->m_typeConstraintData,serializer);
|
||||
|
||||
m_frameInA.serializeFloat(dof->m_rbAFrame);
|
||||
m_frameInB.serializeFloat(dof->m_rbBFrame);
|
||||
m_frameInA.serialize(dof->m_rbAFrame);
|
||||
m_frameInB.serialize(dof->m_rbBFrame);
|
||||
|
||||
|
||||
int i;
|
||||
for (i=0;i<3;i++)
|
||||
{
|
||||
dof->m_angularLowerLimit.m_floats[i] = float(m_angularLimits[i].m_loLimit);
|
||||
dof->m_angularUpperLimit.m_floats[i] = float(m_angularLimits[i].m_hiLimit);
|
||||
dof->m_linearLowerLimit.m_floats[i] = float(m_linearLimits.m_lowerLimit[i]);
|
||||
dof->m_linearUpperLimit.m_floats[i] = float(m_linearLimits.m_upperLimit[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_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_useOffsetForConstraintFrame = m_useOffsetForConstraintFrame ? 1 : 0;
|
||||
|
||||
return "btGeneric6DofConstraintData";
|
||||
return btGeneric6DofConstraintDataName;
|
||||
}
|
||||
|
||||
|
||||
|
|
|
|||
|
|
@ -21,6 +21,15 @@ subject to the following restrictions:
|
|||
#include "btTypedConstraint.h"
|
||||
#include "btGeneric6DofConstraint.h"
|
||||
|
||||
#ifdef BT_USE_DOUBLE_PRECISION
|
||||
#define btGeneric6DofSpringConstraintData2 btGeneric6DofSpringConstraintDoubleData2
|
||||
#define btGeneric6DofSpringConstraintDataName "btGeneric6DofSpringConstraintDoubleData2"
|
||||
#else
|
||||
#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
|
||||
|
||||
|
|
@ -65,7 +74,6 @@ public:
|
|||
};
|
||||
|
||||
|
||||
///do not change those serialization structures, it requires an updated sBulletDNAstr/sBulletDNAstr64
|
||||
struct btGeneric6DofSpringConstraintData
|
||||
{
|
||||
btGeneric6DofConstraintData m_6dofData;
|
||||
|
|
@ -76,26 +84,37 @@ struct btGeneric6DofSpringConstraintData
|
|||
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];
|
||||
};
|
||||
|
||||
|
||||
SIMD_FORCE_INLINE int btGeneric6DofSpringConstraint::calculateSerializeBufferSize() const
|
||||
{
|
||||
return sizeof(btGeneric6DofSpringConstraintData);
|
||||
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
|
||||
{
|
||||
btGeneric6DofSpringConstraintData* dof = (btGeneric6DofSpringConstraintData*)dataBuffer;
|
||||
btGeneric6DofSpringConstraintData2* dof = (btGeneric6DofSpringConstraintData2*)dataBuffer;
|
||||
btGeneric6DofConstraint::serialize(&dof->m_6dofData,serializer);
|
||||
|
||||
int i;
|
||||
for (i=0;i<6;i++)
|
||||
{
|
||||
dof->m_equilibriumPoint[i] = (float)m_equilibriumPoint[i];
|
||||
dof->m_springDamping[i] = (float)m_springDamping[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_springStiffness[i] = (float)m_springStiffness[i];
|
||||
dof->m_springStiffness[i] = m_springStiffness[i];
|
||||
}
|
||||
return "btGeneric6DofSpringConstraintData";
|
||||
return btGeneric6DofSpringConstraintDataName;
|
||||
}
|
||||
|
||||
#endif // BT_GENERIC_6DOF_SPRING_CONSTRAINT_H
|
||||
|
|
|
|||
|
|
@ -369,6 +369,10 @@ void btHingeConstraint::getInfo2Internal(btConstraintInfo2* info, const btTransf
|
|||
info->m_J1angularAxis[i*skip+1]=0;
|
||||
info->m_J1angularAxis[i*skip+2]=0;
|
||||
|
||||
info->m_J2linearAxis[i*skip]=0;
|
||||
info->m_J2linearAxis[i*skip+1]=0;
|
||||
info->m_J2linearAxis[i*skip+2]=0;
|
||||
|
||||
info->m_J2angularAxis[i*skip]=0;
|
||||
info->m_J2angularAxis[i*skip+1]=0;
|
||||
info->m_J2angularAxis[i*skip+2]=0;
|
||||
|
|
@ -384,6 +388,10 @@ void btHingeConstraint::getInfo2Internal(btConstraintInfo2* info, const btTransf
|
|||
info->m_J1linearAxis[0] = 1;
|
||||
info->m_J1linearAxis[skip + 1] = 1;
|
||||
info->m_J1linearAxis[2 * skip + 2] = 1;
|
||||
|
||||
info->m_J2linearAxis[0] = -1;
|
||||
info->m_J2linearAxis[skip + 1] = -1;
|
||||
info->m_J2linearAxis[2 * skip + 2] = -1;
|
||||
}
|
||||
|
||||
|
||||
|
|
@ -797,7 +805,11 @@ void btHingeConstraint::getInfo2InternalUsingFrameOffset(btConstraintInfo2* info
|
|||
for (i=0; i<3; i++) info->m_J1linearAxis[s0+i] = p[i];
|
||||
for (i=0; i<3; i++) info->m_J1linearAxis[s1+i] = q[i];
|
||||
for (i=0; i<3; i++) info->m_J1linearAxis[s2+i] = ax1[i];
|
||||
|
||||
|
||||
for (i=0; i<3; i++) info->m_J2linearAxis[s0+i] = -p[i];
|
||||
for (i=0; i<3; i++) info->m_J2linearAxis[s1+i] = -q[i];
|
||||
for (i=0; i<3; i++) info->m_J2linearAxis[s2+i] = -ax1[i];
|
||||
|
||||
// compute three elements of right hand side
|
||||
|
||||
btScalar rhs = k * p.dot(ofs);
|
||||
|
|
|
|||
|
|
@ -28,8 +28,8 @@ subject to the following restrictions:
|
|||
class btRigidBody;
|
||||
|
||||
#ifdef BT_USE_DOUBLE_PRECISION
|
||||
#define btHingeConstraintData btHingeConstraintDoubleData
|
||||
#define btHingeConstraintDataName "btHingeConstraintDoubleData"
|
||||
#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"
|
||||
|
|
@ -302,7 +302,10 @@ public:
|
|||
|
||||
};
|
||||
|
||||
///do not change those serialization structures, it requires an updated sBulletDNAstr/sBulletDNAstr64
|
||||
|
||||
//only for backward compatibility
|
||||
#ifdef BT_BACKWARDS_COMPATIBLE_SERIALIZATION
|
||||
///this structure is not used, except for loading pre-2.82 .bullet files
|
||||
struct btHingeConstraintDoubleData
|
||||
{
|
||||
btTypedConstraintData m_typeConstraintData;
|
||||
|
|
@ -321,7 +324,9 @@ struct btHingeConstraintDoubleData
|
|||
float m_relaxationFactor;
|
||||
|
||||
};
|
||||
///do not change those serialization structures, it requires an updated sBulletDNAstr/sBulletDNAstr64
|
||||
#endif //BT_BACKWARDS_COMPATIBLE_SERIALIZATION
|
||||
|
||||
|
||||
struct btHingeConstraintFloatData
|
||||
{
|
||||
btTypedConstraintData m_typeConstraintData;
|
||||
|
|
@ -344,6 +349,30 @@ struct btHingeConstraintFloatData
|
|||
|
||||
|
||||
|
||||
///do not change those serialization structures, it requires an updated sBulletDNAstr/sBulletDNAstr64
|
||||
struct btHingeConstraintDoubleData2
|
||||
{
|
||||
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];
|
||||
|
||||
};
|
||||
|
||||
|
||||
|
||||
|
||||
SIMD_FORCE_INLINE int btHingeConstraint::calculateSerializeBufferSize() const
|
||||
{
|
||||
return sizeof(btHingeConstraintData);
|
||||
|
|
|
|||
|
|
@ -116,10 +116,9 @@ void btPoint2PointConstraint::getInfo2NonVirtual (btConstraintInfo2* info, const
|
|||
a1neg.getSkewSymmetricMatrix(angular0,angular1,angular2);
|
||||
}
|
||||
|
||||
/*info->m_J2linearAxis[0] = -1;
|
||||
info->m_J2linearAxis[s+1] = -1;
|
||||
info->m_J2linearAxis[2*s+2] = -1;
|
||||
*/
|
||||
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();
|
||||
|
||||
|
|
|
|||
|
|
@ -24,10 +24,10 @@ class btRigidBody;
|
|||
|
||||
|
||||
#ifdef BT_USE_DOUBLE_PRECISION
|
||||
#define btPoint2PointConstraintData btPoint2PointConstraintDoubleData
|
||||
#define btPoint2PointConstraintDataName "btPoint2PointConstraintDoubleData"
|
||||
#define btPoint2PointConstraintData2 btPoint2PointConstraintDoubleData2
|
||||
#define btPoint2PointConstraintDataName "btPoint2PointConstraintDoubleData2"
|
||||
#else
|
||||
#define btPoint2PointConstraintData btPoint2PointConstraintFloatData
|
||||
#define btPoint2PointConstraintData2 btPoint2PointConstraintFloatData
|
||||
#define btPoint2PointConstraintDataName "btPoint2PointConstraintFloatData"
|
||||
#endif //BT_USE_DOUBLE_PRECISION
|
||||
|
||||
|
|
@ -133,6 +133,17 @@ struct btPoint2PointConstraintFloatData
|
|||
btVector3FloatData m_pivotInB;
|
||||
};
|
||||
|
||||
///do not change those serialization structures, it requires an updated sBulletDNAstr/sBulletDNAstr64
|
||||
struct btPoint2PointConstraintDoubleData2
|
||||
{
|
||||
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
|
||||
{
|
||||
|
|
@ -140,18 +151,19 @@ struct btPoint2PointConstraintDoubleData
|
|||
btVector3DoubleData m_pivotInA;
|
||||
btVector3DoubleData m_pivotInB;
|
||||
};
|
||||
#endif //BT_BACKWARDS_COMPATIBLE_SERIALIZATION
|
||||
|
||||
|
||||
SIMD_FORCE_INLINE int btPoint2PointConstraint::calculateSerializeBufferSize() const
|
||||
{
|
||||
return sizeof(btPoint2PointConstraintData);
|
||||
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
|
||||
{
|
||||
btPoint2PointConstraintData* p2pData = (btPoint2PointConstraintData*)dataBuffer;
|
||||
btPoint2PointConstraintData2* p2pData = (btPoint2PointConstraintData2*)dataBuffer;
|
||||
|
||||
btTypedConstraint::serialize(&p2pData->m_typeConstraintData,serializer);
|
||||
m_pivotInA.serialize(p2pData->m_pivotInA);
|
||||
|
|
|
|||
|
|
@ -14,6 +14,8 @@ subject to the following restrictions:
|
|||
*/
|
||||
|
||||
//#define COMPUTE_IMPULSE_DENOM 1
|
||||
//#define BT_ADDITIONAL_DEBUG
|
||||
|
||||
//It is not necessary (redundant) to refresh contact manifolds, this refresh has been moved to the collision algorithms.
|
||||
|
||||
#include "btSequentialImpulseConstraintSolver.h"
|
||||
|
|
@ -63,8 +65,8 @@ void btSequentialImpulseConstraintSolver::resolveSingleConstraintRowGenericSIMD(
|
|||
__m128 lowerLimit1 = _mm_set1_ps(c.m_lowerLimit);
|
||||
__m128 upperLimit1 = _mm_set1_ps(c.m_upperLimit);
|
||||
__m128 deltaImpulse = _mm_sub_ps(_mm_set1_ps(c.m_rhs), _mm_mul_ps(_mm_set1_ps(c.m_appliedImpulse),_mm_set1_ps(c.m_cfm)));
|
||||
__m128 deltaVel1Dotn = _mm_add_ps(btSimdDot3(c.m_contactNormal.mVec128,body1.internalGetDeltaLinearVelocity().mVec128), btSimdDot3(c.m_relpos1CrossNormal.mVec128,body1.internalGetDeltaAngularVelocity().mVec128));
|
||||
__m128 deltaVel2Dotn = _mm_sub_ps(btSimdDot3(c.m_relpos2CrossNormal.mVec128,body2.internalGetDeltaAngularVelocity().mVec128),btSimdDot3((c.m_contactNormal).mVec128,body2.internalGetDeltaLinearVelocity().mVec128));
|
||||
__m128 deltaVel1Dotn = _mm_add_ps(btSimdDot3(c.m_contactNormal1.mVec128,body1.internalGetDeltaLinearVelocity().mVec128), btSimdDot3(c.m_relpos1CrossNormal.mVec128,body1.internalGetDeltaAngularVelocity().mVec128));
|
||||
__m128 deltaVel2Dotn = _mm_add_ps(btSimdDot3(c.m_contactNormal2.mVec128,body2.internalGetDeltaLinearVelocity().mVec128), btSimdDot3(c.m_relpos2CrossNormal.mVec128,body2.internalGetDeltaAngularVelocity().mVec128));
|
||||
deltaImpulse = _mm_sub_ps(deltaImpulse,_mm_mul_ps(deltaVel1Dotn,_mm_set1_ps(c.m_jacDiagABInv)));
|
||||
deltaImpulse = _mm_sub_ps(deltaImpulse,_mm_mul_ps(deltaVel2Dotn,_mm_set1_ps(c.m_jacDiagABInv)));
|
||||
btSimdScalar sum = _mm_add_ps(cpAppliedImp,deltaImpulse);
|
||||
|
|
@ -77,12 +79,12 @@ void btSequentialImpulseConstraintSolver::resolveSingleConstraintRowGenericSIMD(
|
|||
__m128 upperMinApplied = _mm_sub_ps(upperLimit1,cpAppliedImp);
|
||||
deltaImpulse = _mm_or_ps( _mm_and_ps(resultUpperLess, deltaImpulse), _mm_andnot_ps(resultUpperLess, upperMinApplied) );
|
||||
c.m_appliedImpulse = _mm_or_ps( _mm_and_ps(resultUpperLess, c.m_appliedImpulse), _mm_andnot_ps(resultUpperLess, upperLimit1) );
|
||||
__m128 linearComponentA = _mm_mul_ps(c.m_contactNormal.mVec128,body1.internalGetInvMass().mVec128);
|
||||
__m128 linearComponentB = _mm_mul_ps((c.m_contactNormal).mVec128,body2.internalGetInvMass().mVec128);
|
||||
__m128 linearComponentA = _mm_mul_ps(c.m_contactNormal1.mVec128,body1.internalGetInvMass().mVec128);
|
||||
__m128 linearComponentB = _mm_mul_ps((c.m_contactNormal2).mVec128,body2.internalGetInvMass().mVec128);
|
||||
__m128 impulseMagnitude = deltaImpulse;
|
||||
body1.internalGetDeltaLinearVelocity().mVec128 = _mm_add_ps(body1.internalGetDeltaLinearVelocity().mVec128,_mm_mul_ps(linearComponentA,impulseMagnitude));
|
||||
body1.internalGetDeltaAngularVelocity().mVec128 = _mm_add_ps(body1.internalGetDeltaAngularVelocity().mVec128 ,_mm_mul_ps(c.m_angularComponentA.mVec128,impulseMagnitude));
|
||||
body2.internalGetDeltaLinearVelocity().mVec128 = _mm_sub_ps(body2.internalGetDeltaLinearVelocity().mVec128,_mm_mul_ps(linearComponentB,impulseMagnitude));
|
||||
body2.internalGetDeltaLinearVelocity().mVec128 = _mm_add_ps(body2.internalGetDeltaLinearVelocity().mVec128,_mm_mul_ps(linearComponentB,impulseMagnitude));
|
||||
body2.internalGetDeltaAngularVelocity().mVec128 = _mm_add_ps(body2.internalGetDeltaAngularVelocity().mVec128 ,_mm_mul_ps(c.m_angularComponentB.mVec128,impulseMagnitude));
|
||||
#else
|
||||
resolveSingleConstraintRowGeneric(body1,body2,c);
|
||||
|
|
@ -93,8 +95,8 @@ void btSequentialImpulseConstraintSolver::resolveSingleConstraintRowGenericSIMD(
|
|||
void btSequentialImpulseConstraintSolver::resolveSingleConstraintRowGeneric(btSolverBody& body1,btSolverBody& body2,const btSolverConstraint& c)
|
||||
{
|
||||
btScalar deltaImpulse = c.m_rhs-btScalar(c.m_appliedImpulse)*c.m_cfm;
|
||||
const btScalar deltaVel1Dotn = c.m_contactNormal.dot(body1.internalGetDeltaLinearVelocity()) + c.m_relpos1CrossNormal.dot(body1.internalGetDeltaAngularVelocity());
|
||||
const btScalar deltaVel2Dotn = -c.m_contactNormal.dot(body2.internalGetDeltaLinearVelocity()) + c.m_relpos2CrossNormal.dot(body2.internalGetDeltaAngularVelocity());
|
||||
const btScalar deltaVel1Dotn = c.m_contactNormal1.dot(body1.internalGetDeltaLinearVelocity()) + c.m_relpos1CrossNormal.dot(body1.internalGetDeltaAngularVelocity());
|
||||
const btScalar deltaVel2Dotn = c.m_contactNormal2.dot(body2.internalGetDeltaLinearVelocity()) + c.m_relpos2CrossNormal.dot(body2.internalGetDeltaAngularVelocity());
|
||||
|
||||
// const btScalar delta_rel_vel = deltaVel1Dotn-deltaVel2Dotn;
|
||||
deltaImpulse -= deltaVel1Dotn*c.m_jacDiagABInv;
|
||||
|
|
@ -116,8 +118,8 @@ void btSequentialImpulseConstraintSolver::resolveSingleConstraintRowGenericSIMD(
|
|||
c.m_appliedImpulse = sum;
|
||||
}
|
||||
|
||||
body1.internalApplyImpulse(c.m_contactNormal*body1.internalGetInvMass(),c.m_angularComponentA,deltaImpulse);
|
||||
body2.internalApplyImpulse(-c.m_contactNormal*body2.internalGetInvMass(),c.m_angularComponentB,deltaImpulse);
|
||||
body1.internalApplyImpulse(c.m_contactNormal1*body1.internalGetInvMass(),c.m_angularComponentA,deltaImpulse);
|
||||
body2.internalApplyImpulse(c.m_contactNormal2*body2.internalGetInvMass(),c.m_angularComponentB,deltaImpulse);
|
||||
}
|
||||
|
||||
void btSequentialImpulseConstraintSolver::resolveSingleConstraintRowLowerLimitSIMD(btSolverBody& body1,btSolverBody& body2,const btSolverConstraint& c)
|
||||
|
|
@ -127,8 +129,8 @@ void btSequentialImpulseConstraintSolver::resolveSingleConstraintRowGenericSIMD(
|
|||
__m128 lowerLimit1 = _mm_set1_ps(c.m_lowerLimit);
|
||||
__m128 upperLimit1 = _mm_set1_ps(c.m_upperLimit);
|
||||
__m128 deltaImpulse = _mm_sub_ps(_mm_set1_ps(c.m_rhs), _mm_mul_ps(_mm_set1_ps(c.m_appliedImpulse),_mm_set1_ps(c.m_cfm)));
|
||||
__m128 deltaVel1Dotn = _mm_add_ps(btSimdDot3(c.m_contactNormal.mVec128,body1.internalGetDeltaLinearVelocity().mVec128), btSimdDot3(c.m_relpos1CrossNormal.mVec128,body1.internalGetDeltaAngularVelocity().mVec128));
|
||||
__m128 deltaVel2Dotn = _mm_sub_ps(btSimdDot3(c.m_relpos2CrossNormal.mVec128,body2.internalGetDeltaAngularVelocity().mVec128),btSimdDot3((c.m_contactNormal).mVec128,body2.internalGetDeltaLinearVelocity().mVec128));
|
||||
__m128 deltaVel1Dotn = _mm_add_ps(btSimdDot3(c.m_contactNormal1.mVec128,body1.internalGetDeltaLinearVelocity().mVec128), btSimdDot3(c.m_relpos1CrossNormal.mVec128,body1.internalGetDeltaAngularVelocity().mVec128));
|
||||
__m128 deltaVel2Dotn = _mm_add_ps(btSimdDot3(c.m_contactNormal2.mVec128,body2.internalGetDeltaLinearVelocity().mVec128), btSimdDot3(c.m_relpos2CrossNormal.mVec128,body2.internalGetDeltaAngularVelocity().mVec128));
|
||||
deltaImpulse = _mm_sub_ps(deltaImpulse,_mm_mul_ps(deltaVel1Dotn,_mm_set1_ps(c.m_jacDiagABInv)));
|
||||
deltaImpulse = _mm_sub_ps(deltaImpulse,_mm_mul_ps(deltaVel2Dotn,_mm_set1_ps(c.m_jacDiagABInv)));
|
||||
btSimdScalar sum = _mm_add_ps(cpAppliedImp,deltaImpulse);
|
||||
|
|
@ -138,24 +140,24 @@ void btSequentialImpulseConstraintSolver::resolveSingleConstraintRowGenericSIMD(
|
|||
__m128 lowMinApplied = _mm_sub_ps(lowerLimit1,cpAppliedImp);
|
||||
deltaImpulse = _mm_or_ps( _mm_and_ps(resultLowerLess, lowMinApplied), _mm_andnot_ps(resultLowerLess, deltaImpulse) );
|
||||
c.m_appliedImpulse = _mm_or_ps( _mm_and_ps(resultLowerLess, lowerLimit1), _mm_andnot_ps(resultLowerLess, sum) );
|
||||
__m128 linearComponentA = _mm_mul_ps(c.m_contactNormal.mVec128,body1.internalGetInvMass().mVec128);
|
||||
__m128 linearComponentB = _mm_mul_ps((c.m_contactNormal).mVec128,body2.internalGetInvMass().mVec128);
|
||||
__m128 linearComponentA = _mm_mul_ps(c.m_contactNormal1.mVec128,body1.internalGetInvMass().mVec128);
|
||||
__m128 linearComponentB = _mm_mul_ps(c.m_contactNormal2.mVec128,body2.internalGetInvMass().mVec128);
|
||||
__m128 impulseMagnitude = deltaImpulse;
|
||||
body1.internalGetDeltaLinearVelocity().mVec128 = _mm_add_ps(body1.internalGetDeltaLinearVelocity().mVec128,_mm_mul_ps(linearComponentA,impulseMagnitude));
|
||||
body1.internalGetDeltaAngularVelocity().mVec128 = _mm_add_ps(body1.internalGetDeltaAngularVelocity().mVec128 ,_mm_mul_ps(c.m_angularComponentA.mVec128,impulseMagnitude));
|
||||
body2.internalGetDeltaLinearVelocity().mVec128 = _mm_sub_ps(body2.internalGetDeltaLinearVelocity().mVec128,_mm_mul_ps(linearComponentB,impulseMagnitude));
|
||||
body2.internalGetDeltaLinearVelocity().mVec128 = _mm_add_ps(body2.internalGetDeltaLinearVelocity().mVec128,_mm_mul_ps(linearComponentB,impulseMagnitude));
|
||||
body2.internalGetDeltaAngularVelocity().mVec128 = _mm_add_ps(body2.internalGetDeltaAngularVelocity().mVec128 ,_mm_mul_ps(c.m_angularComponentB.mVec128,impulseMagnitude));
|
||||
#else
|
||||
resolveSingleConstraintRowLowerLimit(body1,body2,c);
|
||||
#endif
|
||||
}
|
||||
|
||||
// Project Gauss Seidel or the equivalent Sequential Impulse
|
||||
// Projected Gauss Seidel or the equivalent Sequential Impulse
|
||||
void btSequentialImpulseConstraintSolver::resolveSingleConstraintRowLowerLimit(btSolverBody& body1,btSolverBody& body2,const btSolverConstraint& c)
|
||||
{
|
||||
btScalar deltaImpulse = c.m_rhs-btScalar(c.m_appliedImpulse)*c.m_cfm;
|
||||
const btScalar deltaVel1Dotn = c.m_contactNormal.dot(body1.internalGetDeltaLinearVelocity()) + c.m_relpos1CrossNormal.dot(body1.internalGetDeltaAngularVelocity());
|
||||
const btScalar deltaVel2Dotn = -c.m_contactNormal.dot(body2.internalGetDeltaLinearVelocity()) + c.m_relpos2CrossNormal.dot(body2.internalGetDeltaAngularVelocity());
|
||||
const btScalar deltaVel1Dotn = c.m_contactNormal1.dot(body1.internalGetDeltaLinearVelocity()) + c.m_relpos1CrossNormal.dot(body1.internalGetDeltaAngularVelocity());
|
||||
const btScalar deltaVel2Dotn = c.m_contactNormal2.dot(body2.internalGetDeltaLinearVelocity()) + c.m_relpos2CrossNormal.dot(body2.internalGetDeltaAngularVelocity());
|
||||
|
||||
deltaImpulse -= deltaVel1Dotn*c.m_jacDiagABInv;
|
||||
deltaImpulse -= deltaVel2Dotn*c.m_jacDiagABInv;
|
||||
|
|
@ -169,8 +171,8 @@ void btSequentialImpulseConstraintSolver::resolveSingleConstraintRowGenericSIMD(
|
|||
{
|
||||
c.m_appliedImpulse = sum;
|
||||
}
|
||||
body1.internalApplyImpulse(c.m_contactNormal*body1.internalGetInvMass(),c.m_angularComponentA,deltaImpulse);
|
||||
body2.internalApplyImpulse(-c.m_contactNormal*body2.internalGetInvMass(),c.m_angularComponentB,deltaImpulse);
|
||||
body1.internalApplyImpulse(c.m_contactNormal1*body1.internalGetInvMass(),c.m_angularComponentA,deltaImpulse);
|
||||
body2.internalApplyImpulse(c.m_contactNormal2*body2.internalGetInvMass(),c.m_angularComponentB,deltaImpulse);
|
||||
}
|
||||
|
||||
|
||||
|
|
@ -183,8 +185,8 @@ void btSequentialImpulseConstraintSolver::resolveSplitPenetrationImpulseCacheFri
|
|||
{
|
||||
gNumSplitImpulseRecoveries++;
|
||||
btScalar deltaImpulse = c.m_rhsPenetration-btScalar(c.m_appliedPushImpulse)*c.m_cfm;
|
||||
const btScalar deltaVel1Dotn = c.m_contactNormal.dot(body1.internalGetPushVelocity()) + c.m_relpos1CrossNormal.dot(body1.internalGetTurnVelocity());
|
||||
const btScalar deltaVel2Dotn = -c.m_contactNormal.dot(body2.internalGetPushVelocity()) + c.m_relpos2CrossNormal.dot(body2.internalGetTurnVelocity());
|
||||
const btScalar deltaVel1Dotn = c.m_contactNormal1.dot(body1.internalGetPushVelocity()) + c.m_relpos1CrossNormal.dot(body1.internalGetTurnVelocity());
|
||||
const btScalar deltaVel2Dotn = c.m_contactNormal2.dot(body2.internalGetPushVelocity()) + c.m_relpos2CrossNormal.dot(body2.internalGetTurnVelocity());
|
||||
|
||||
deltaImpulse -= deltaVel1Dotn*c.m_jacDiagABInv;
|
||||
deltaImpulse -= deltaVel2Dotn*c.m_jacDiagABInv;
|
||||
|
|
@ -198,8 +200,8 @@ void btSequentialImpulseConstraintSolver::resolveSplitPenetrationImpulseCacheFri
|
|||
{
|
||||
c.m_appliedPushImpulse = sum;
|
||||
}
|
||||
body1.internalApplyPushImpulse(c.m_contactNormal*body1.internalGetInvMass(),c.m_angularComponentA,deltaImpulse);
|
||||
body2.internalApplyPushImpulse(-c.m_contactNormal*body2.internalGetInvMass(),c.m_angularComponentB,deltaImpulse);
|
||||
body1.internalApplyPushImpulse(c.m_contactNormal1*body1.internalGetInvMass(),c.m_angularComponentA,deltaImpulse);
|
||||
body2.internalApplyPushImpulse(c.m_contactNormal2*body2.internalGetInvMass(),c.m_angularComponentB,deltaImpulse);
|
||||
}
|
||||
}
|
||||
|
||||
|
|
@ -215,8 +217,8 @@ void btSequentialImpulseConstraintSolver::resolveSplitPenetrationImpulseCacheFri
|
|||
__m128 lowerLimit1 = _mm_set1_ps(c.m_lowerLimit);
|
||||
__m128 upperLimit1 = _mm_set1_ps(c.m_upperLimit);
|
||||
__m128 deltaImpulse = _mm_sub_ps(_mm_set1_ps(c.m_rhsPenetration), _mm_mul_ps(_mm_set1_ps(c.m_appliedPushImpulse),_mm_set1_ps(c.m_cfm)));
|
||||
__m128 deltaVel1Dotn = _mm_add_ps(btSimdDot3(c.m_contactNormal.mVec128,body1.internalGetPushVelocity().mVec128), btSimdDot3(c.m_relpos1CrossNormal.mVec128,body1.internalGetTurnVelocity().mVec128));
|
||||
__m128 deltaVel2Dotn = _mm_sub_ps(btSimdDot3(c.m_relpos2CrossNormal.mVec128,body2.internalGetTurnVelocity().mVec128),btSimdDot3((c.m_contactNormal).mVec128,body2.internalGetPushVelocity().mVec128));
|
||||
__m128 deltaVel1Dotn = _mm_add_ps(btSimdDot3(c.m_contactNormal1.mVec128,body1.internalGetPushVelocity().mVec128), btSimdDot3(c.m_relpos1CrossNormal.mVec128,body1.internalGetTurnVelocity().mVec128));
|
||||
__m128 deltaVel2Dotn = _mm_add_ps(btSimdDot3(c.m_contactNormal2.mVec128,body2.internalGetPushVelocity().mVec128), btSimdDot3(c.m_relpos2CrossNormal.mVec128,body2.internalGetTurnVelocity().mVec128));
|
||||
deltaImpulse = _mm_sub_ps(deltaImpulse,_mm_mul_ps(deltaVel1Dotn,_mm_set1_ps(c.m_jacDiagABInv)));
|
||||
deltaImpulse = _mm_sub_ps(deltaImpulse,_mm_mul_ps(deltaVel2Dotn,_mm_set1_ps(c.m_jacDiagABInv)));
|
||||
btSimdScalar sum = _mm_add_ps(cpAppliedImp,deltaImpulse);
|
||||
|
|
@ -226,12 +228,12 @@ void btSequentialImpulseConstraintSolver::resolveSplitPenetrationImpulseCacheFri
|
|||
__m128 lowMinApplied = _mm_sub_ps(lowerLimit1,cpAppliedImp);
|
||||
deltaImpulse = _mm_or_ps( _mm_and_ps(resultLowerLess, lowMinApplied), _mm_andnot_ps(resultLowerLess, deltaImpulse) );
|
||||
c.m_appliedPushImpulse = _mm_or_ps( _mm_and_ps(resultLowerLess, lowerLimit1), _mm_andnot_ps(resultLowerLess, sum) );
|
||||
__m128 linearComponentA = _mm_mul_ps(c.m_contactNormal.mVec128,body1.internalGetInvMass().mVec128);
|
||||
__m128 linearComponentB = _mm_mul_ps((c.m_contactNormal).mVec128,body2.internalGetInvMass().mVec128);
|
||||
__m128 linearComponentA = _mm_mul_ps(c.m_contactNormal1.mVec128,body1.internalGetInvMass().mVec128);
|
||||
__m128 linearComponentB = _mm_mul_ps(c.m_contactNormal2.mVec128,body2.internalGetInvMass().mVec128);
|
||||
__m128 impulseMagnitude = deltaImpulse;
|
||||
body1.internalGetPushVelocity().mVec128 = _mm_add_ps(body1.internalGetPushVelocity().mVec128,_mm_mul_ps(linearComponentA,impulseMagnitude));
|
||||
body1.internalGetTurnVelocity().mVec128 = _mm_add_ps(body1.internalGetTurnVelocity().mVec128 ,_mm_mul_ps(c.m_angularComponentA.mVec128,impulseMagnitude));
|
||||
body2.internalGetPushVelocity().mVec128 = _mm_sub_ps(body2.internalGetPushVelocity().mVec128,_mm_mul_ps(linearComponentB,impulseMagnitude));
|
||||
body2.internalGetPushVelocity().mVec128 = _mm_add_ps(body2.internalGetPushVelocity().mVec128,_mm_mul_ps(linearComponentB,impulseMagnitude));
|
||||
body2.internalGetTurnVelocity().mVec128 = _mm_add_ps(body2.internalGetTurnVelocity().mVec128 ,_mm_mul_ps(c.m_angularComponentB.mVec128,impulseMagnitude));
|
||||
#else
|
||||
resolveSplitPenetrationImpulseCacheFriendly(body1,body2,c);
|
||||
|
|
@ -278,7 +280,7 @@ int btSequentialImpulseConstraintSolver::btRandInt2 (int n)
|
|||
|
||||
|
||||
|
||||
void btSequentialImpulseConstraintSolver::initSolverBody(btSolverBody* solverBody, btCollisionObject* collisionObject)
|
||||
void btSequentialImpulseConstraintSolver::initSolverBody(btSolverBody* solverBody, btCollisionObject* collisionObject, btScalar timeStep)
|
||||
{
|
||||
|
||||
btRigidBody* rb = collisionObject? btRigidBody::upcast(collisionObject) : 0;
|
||||
|
|
@ -297,6 +299,9 @@ void btSequentialImpulseConstraintSolver::initSolverBody(btSolverBody* solverBod
|
|||
solverBody->m_linearFactor = rb->getLinearFactor();
|
||||
solverBody->m_linearVelocity = rb->getLinearVelocity();
|
||||
solverBody->m_angularVelocity = rb->getAngularVelocity();
|
||||
solverBody->m_externalForceImpulse = rb->getTotalForce()*rb->getInvMass()*timeStep;
|
||||
solverBody->m_externalTorqueImpulse = rb->getTotalTorque()*rb->getInvInertiaTensorWorld()*timeStep ;
|
||||
|
||||
} else
|
||||
{
|
||||
solverBody->m_worldTransform.setIdentity();
|
||||
|
|
@ -306,6 +311,8 @@ void btSequentialImpulseConstraintSolver::initSolverBody(btSolverBody* solverBod
|
|||
solverBody->m_linearFactor.setValue(1,1,1);
|
||||
solverBody->m_linearVelocity.setValue(0,0,0);
|
||||
solverBody->m_angularVelocity.setValue(0,0,0);
|
||||
solverBody->m_externalForceImpulse.setValue(0,0,0);
|
||||
solverBody->m_externalTorqueImpulse.setValue(0,0,0);
|
||||
}
|
||||
|
||||
|
||||
|
|
@ -324,8 +331,7 @@ btScalar btSequentialImpulseConstraintSolver::restitutionCurve(btScalar rel_vel,
|
|||
|
||||
|
||||
|
||||
static void applyAnisotropicFriction(btCollisionObject* colObj,btVector3& frictionDirection, int frictionMode);
|
||||
static void applyAnisotropicFriction(btCollisionObject* colObj,btVector3& frictionDirection, int frictionMode)
|
||||
void btSequentialImpulseConstraintSolver::applyAnisotropicFriction(btCollisionObject* colObj,btVector3& frictionDirection, int frictionMode)
|
||||
{
|
||||
|
||||
|
||||
|
|
@ -349,7 +355,6 @@ void btSequentialImpulseConstraintSolver::setupFrictionConstraint(btSolverConstr
|
|||
{
|
||||
|
||||
|
||||
solverConstraint.m_contactNormal = normalAxis;
|
||||
btSolverBody& solverBodyA = m_tmpSolverBodyPool[solverBodyIdA];
|
||||
btSolverBody& solverBodyB = m_tmpSolverBodyPool[solverBodyIdB];
|
||||
|
||||
|
|
@ -365,15 +370,30 @@ void btSequentialImpulseConstraintSolver::setupFrictionConstraint(btSolverConstr
|
|||
solverConstraint.m_appliedImpulse = 0.f;
|
||||
solverConstraint.m_appliedPushImpulse = 0.f;
|
||||
|
||||
if (body0)
|
||||
{
|
||||
btVector3 ftorqueAxis1 = rel_pos1.cross(solverConstraint.m_contactNormal);
|
||||
solverConstraint.m_contactNormal1 = normalAxis;
|
||||
btVector3 ftorqueAxis1 = rel_pos1.cross(solverConstraint.m_contactNormal1);
|
||||
solverConstraint.m_relpos1CrossNormal = ftorqueAxis1;
|
||||
solverConstraint.m_angularComponentA = body0 ? body0->getInvInertiaTensorWorld()*ftorqueAxis1*body0->getAngularFactor() : btVector3(0,0,0);
|
||||
}
|
||||
solverConstraint.m_angularComponentA = body0->getInvInertiaTensorWorld()*ftorqueAxis1*body0->getAngularFactor();
|
||||
}else
|
||||
{
|
||||
btVector3 ftorqueAxis1 = rel_pos2.cross(-solverConstraint.m_contactNormal);
|
||||
solverConstraint.m_contactNormal1.setZero();
|
||||
solverConstraint.m_relpos1CrossNormal.setZero();
|
||||
solverConstraint.m_angularComponentA .setZero();
|
||||
}
|
||||
|
||||
if (body1)
|
||||
{
|
||||
solverConstraint.m_contactNormal2 = -normalAxis;
|
||||
btVector3 ftorqueAxis1 = rel_pos2.cross(solverConstraint.m_contactNormal2);
|
||||
solverConstraint.m_relpos2CrossNormal = ftorqueAxis1;
|
||||
solverConstraint.m_angularComponentB = body1 ? body1->getInvInertiaTensorWorld()*ftorqueAxis1*body1->getAngularFactor() : btVector3(0,0,0);
|
||||
solverConstraint.m_angularComponentB = body1->getInvInertiaTensorWorld()*ftorqueAxis1*body1->getAngularFactor();
|
||||
} else
|
||||
{
|
||||
solverConstraint.m_contactNormal2.setZero();
|
||||
solverConstraint.m_relpos2CrossNormal.setZero();
|
||||
solverConstraint.m_angularComponentB.setZero();
|
||||
}
|
||||
|
||||
{
|
||||
|
|
@ -398,9 +418,9 @@ void btSequentialImpulseConstraintSolver::setupFrictionConstraint(btSolverConstr
|
|||
|
||||
|
||||
btScalar rel_vel;
|
||||
btScalar vel1Dotn = solverConstraint.m_contactNormal.dot(body0?solverBodyA.m_linearVelocity:btVector3(0,0,0))
|
||||
btScalar vel1Dotn = solverConstraint.m_contactNormal1.dot(body0?solverBodyA.m_linearVelocity+solverBodyA.m_externalForceImpulse:btVector3(0,0,0))
|
||||
+ solverConstraint.m_relpos1CrossNormal.dot(body0?solverBodyA.m_angularVelocity:btVector3(0,0,0));
|
||||
btScalar vel2Dotn = -solverConstraint.m_contactNormal.dot(body1?solverBodyB.m_linearVelocity:btVector3(0,0,0))
|
||||
btScalar vel2Dotn = solverConstraint.m_contactNormal2.dot(body1?solverBodyB.m_linearVelocity+solverBodyB.m_externalForceImpulse:btVector3(0,0,0))
|
||||
+ solverConstraint.m_relpos2CrossNormal.dot(body1?solverBodyB.m_angularVelocity:btVector3(0,0,0));
|
||||
|
||||
rel_vel = vel1Dotn+vel2Dotn;
|
||||
|
|
@ -411,8 +431,8 @@ void btSequentialImpulseConstraintSolver::setupFrictionConstraint(btSolverConstr
|
|||
btSimdScalar velocityImpulse = velocityError * btSimdScalar(solverConstraint.m_jacDiagABInv);
|
||||
solverConstraint.m_rhs = velocityImpulse;
|
||||
solverConstraint.m_cfm = cfmSlip;
|
||||
solverConstraint.m_lowerLimit = 0;
|
||||
solverConstraint.m_upperLimit = 1e10f;
|
||||
solverConstraint.m_lowerLimit = -solverConstraint.m_friction;
|
||||
solverConstraint.m_upperLimit = solverConstraint.m_friction;
|
||||
|
||||
}
|
||||
}
|
||||
|
|
@ -436,7 +456,8 @@ void btSequentialImpulseConstraintSolver::setupRollingFrictionConstraint( btSolv
|
|||
btVector3 normalAxis(0,0,0);
|
||||
|
||||
|
||||
solverConstraint.m_contactNormal = normalAxis;
|
||||
solverConstraint.m_contactNormal1 = normalAxis;
|
||||
solverConstraint.m_contactNormal2 = -normalAxis;
|
||||
btSolverBody& solverBodyA = m_tmpSolverBodyPool[solverBodyIdA];
|
||||
btSolverBody& solverBodyB = m_tmpSolverBodyPool[solverBodyIdB];
|
||||
|
||||
|
|
@ -477,9 +498,9 @@ void btSequentialImpulseConstraintSolver::setupRollingFrictionConstraint( btSolv
|
|||
|
||||
|
||||
btScalar rel_vel;
|
||||
btScalar vel1Dotn = solverConstraint.m_contactNormal.dot(body0?solverBodyA.m_linearVelocity:btVector3(0,0,0))
|
||||
btScalar vel1Dotn = solverConstraint.m_contactNormal1.dot(body0?solverBodyA.m_linearVelocity+solverBodyA.m_externalForceImpulse:btVector3(0,0,0))
|
||||
+ solverConstraint.m_relpos1CrossNormal.dot(body0?solverBodyA.m_angularVelocity:btVector3(0,0,0));
|
||||
btScalar vel2Dotn = -solverConstraint.m_contactNormal.dot(body1?solverBodyB.m_linearVelocity:btVector3(0,0,0))
|
||||
btScalar vel2Dotn = solverConstraint.m_contactNormal2.dot(body1?solverBodyB.m_linearVelocity+solverBodyB.m_externalForceImpulse:btVector3(0,0,0))
|
||||
+ solverConstraint.m_relpos2CrossNormal.dot(body1?solverBodyB.m_angularVelocity:btVector3(0,0,0));
|
||||
|
||||
rel_vel = vel1Dotn+vel2Dotn;
|
||||
|
|
@ -490,8 +511,8 @@ void btSequentialImpulseConstraintSolver::setupRollingFrictionConstraint( btSolv
|
|||
btSimdScalar velocityImpulse = velocityError * btSimdScalar(solverConstraint.m_jacDiagABInv);
|
||||
solverConstraint.m_rhs = velocityImpulse;
|
||||
solverConstraint.m_cfm = cfmSlip;
|
||||
solverConstraint.m_lowerLimit = 0;
|
||||
solverConstraint.m_upperLimit = 1e10f;
|
||||
solverConstraint.m_lowerLimit = -solverConstraint.m_friction;
|
||||
solverConstraint.m_upperLimit = solverConstraint.m_friction;
|
||||
|
||||
}
|
||||
}
|
||||
|
|
@ -513,7 +534,7 @@ btSolverConstraint& btSequentialImpulseConstraintSolver::addRollingFrictionConst
|
|||
}
|
||||
|
||||
|
||||
int btSequentialImpulseConstraintSolver::getOrInitSolverBody(btCollisionObject& body)
|
||||
int btSequentialImpulseConstraintSolver::getOrInitSolverBody(btCollisionObject& body,btScalar timeStep)
|
||||
{
|
||||
|
||||
int solverBodyIdA = -1;
|
||||
|
|
@ -531,11 +552,19 @@ int btSequentialImpulseConstraintSolver::getOrInitSolverBody(btCollisionObject&
|
|||
{
|
||||
solverBodyIdA = m_tmpSolverBodyPool.size();
|
||||
btSolverBody& solverBody = m_tmpSolverBodyPool.expand();
|
||||
initSolverBody(&solverBody,&body);
|
||||
initSolverBody(&solverBody,&body,timeStep);
|
||||
body.setCompanionId(solverBodyIdA);
|
||||
} else
|
||||
{
|
||||
return 0;//assume first one is a fixed solver body
|
||||
|
||||
if (m_fixedBodyId<0)
|
||||
{
|
||||
m_fixedBodyId = m_tmpSolverBodyPool.size();
|
||||
btSolverBody& fixedBody = m_tmpSolverBodyPool.expand();
|
||||
initSolverBody(&fixedBody,0,timeStep);
|
||||
}
|
||||
return m_fixedBodyId;
|
||||
// return 0;//assume first one is a fixed solver body
|
||||
}
|
||||
}
|
||||
|
||||
|
|
@ -548,8 +577,8 @@ int btSequentialImpulseConstraintSolver::getOrInitSolverBody(btCollisionObject&
|
|||
void btSequentialImpulseConstraintSolver::setupContactConstraint(btSolverConstraint& solverConstraint,
|
||||
int solverBodyIdA, int solverBodyIdB,
|
||||
btManifoldPoint& cp, const btContactSolverInfo& infoGlobal,
|
||||
btVector3& vel, btScalar& rel_vel, btScalar& relaxation,
|
||||
btVector3& rel_pos1, btVector3& rel_pos2)
|
||||
btScalar& relaxation,
|
||||
const btVector3& rel_pos1, const btVector3& rel_pos2)
|
||||
{
|
||||
|
||||
const btVector3& pos1 = cp.getPositionWorldOnA();
|
||||
|
|
@ -563,8 +592,8 @@ void btSequentialImpulseConstraintSolver::setupContactConstraint(btSolverConstra
|
|||
|
||||
// btVector3 rel_pos1 = pos1 - colObj0->getWorldTransform().getOrigin();
|
||||
// btVector3 rel_pos2 = pos2 - colObj1->getWorldTransform().getOrigin();
|
||||
rel_pos1 = pos1 - bodyA->getWorldTransform().getOrigin();
|
||||
rel_pos2 = pos2 - bodyB->getWorldTransform().getOrigin();
|
||||
//rel_pos1 = pos1 - bodyA->getWorldTransform().getOrigin();
|
||||
//rel_pos2 = pos2 - bodyB->getWorldTransform().getOrigin();
|
||||
|
||||
relaxation = 1.f;
|
||||
|
||||
|
|
@ -597,9 +626,24 @@ void btSequentialImpulseConstraintSolver::setupContactConstraint(btSolverConstra
|
|||
solverConstraint.m_jacDiagABInv = denom;
|
||||
}
|
||||
|
||||
solverConstraint.m_contactNormal = cp.m_normalWorldOnB;
|
||||
solverConstraint.m_relpos1CrossNormal = torqueAxis0;
|
||||
solverConstraint.m_relpos2CrossNormal = -torqueAxis1;
|
||||
if (rb0)
|
||||
{
|
||||
solverConstraint.m_contactNormal1 = cp.m_normalWorldOnB;
|
||||
solverConstraint.m_relpos1CrossNormal = torqueAxis0;
|
||||
} else
|
||||
{
|
||||
solverConstraint.m_contactNormal1.setZero();
|
||||
solverConstraint.m_relpos1CrossNormal.setZero();
|
||||
}
|
||||
if (rb1)
|
||||
{
|
||||
solverConstraint.m_contactNormal2 = -cp.m_normalWorldOnB;
|
||||
solverConstraint.m_relpos2CrossNormal = -torqueAxis1;
|
||||
}else
|
||||
{
|
||||
solverConstraint.m_contactNormal2.setZero();
|
||||
solverConstraint.m_relpos2CrossNormal.setZero();
|
||||
}
|
||||
|
||||
btScalar restitution = 0.f;
|
||||
btScalar penetration = cp.getDistance()+infoGlobal.m_linearSlop;
|
||||
|
|
@ -611,8 +655,8 @@ void btSequentialImpulseConstraintSolver::setupContactConstraint(btSolverConstra
|
|||
vel2 = rb1? rb1->getVelocityInLocalPoint(rel_pos2) : btVector3(0,0,0);
|
||||
|
||||
// btVector3 vel2 = rb1 ? rb1->getVelocityInLocalPoint(rel_pos2) : btVector3(0,0,0);
|
||||
vel = vel1 - vel2;
|
||||
rel_vel = cp.m_normalWorldOnB.dot(vel);
|
||||
btVector3 vel = vel1 - vel2;
|
||||
btScalar rel_vel = cp.m_normalWorldOnB.dot(vel);
|
||||
|
||||
|
||||
|
||||
|
|
@ -632,9 +676,9 @@ void btSequentialImpulseConstraintSolver::setupContactConstraint(btSolverConstra
|
|||
{
|
||||
solverConstraint.m_appliedImpulse = cp.m_appliedImpulse * infoGlobal.m_warmstartingFactor;
|
||||
if (rb0)
|
||||
bodyA->internalApplyImpulse(solverConstraint.m_contactNormal*bodyA->internalGetInvMass()*rb0->getLinearFactor(),solverConstraint.m_angularComponentA,solverConstraint.m_appliedImpulse);
|
||||
bodyA->internalApplyImpulse(solverConstraint.m_contactNormal1*bodyA->internalGetInvMass()*rb0->getLinearFactor(),solverConstraint.m_angularComponentA,solverConstraint.m_appliedImpulse);
|
||||
if (rb1)
|
||||
bodyB->internalApplyImpulse(solverConstraint.m_contactNormal*bodyB->internalGetInvMass()*rb1->getLinearFactor(),-solverConstraint.m_angularComponentB,-(btScalar)solverConstraint.m_appliedImpulse);
|
||||
bodyB->internalApplyImpulse(-solverConstraint.m_contactNormal2*bodyB->internalGetInvMass()*rb1->getLinearFactor(),-solverConstraint.m_angularComponentB,-(btScalar)solverConstraint.m_appliedImpulse);
|
||||
} else
|
||||
{
|
||||
solverConstraint.m_appliedImpulse = 0.f;
|
||||
|
|
@ -643,10 +687,17 @@ void btSequentialImpulseConstraintSolver::setupContactConstraint(btSolverConstra
|
|||
solverConstraint.m_appliedPushImpulse = 0.f;
|
||||
|
||||
{
|
||||
btScalar vel1Dotn = solverConstraint.m_contactNormal.dot(rb0?bodyA->m_linearVelocity:btVector3(0,0,0))
|
||||
+ solverConstraint.m_relpos1CrossNormal.dot(rb0?bodyA->m_angularVelocity:btVector3(0,0,0));
|
||||
btScalar vel2Dotn = -solverConstraint.m_contactNormal.dot(rb1?bodyB->m_linearVelocity:btVector3(0,0,0))
|
||||
+ solverConstraint.m_relpos2CrossNormal.dot(rb1?bodyB->m_angularVelocity:btVector3(0,0,0));
|
||||
|
||||
btVector3 externalForceImpulseA = bodyA->m_originalBody ? bodyA->m_externalForceImpulse: btVector3(0,0,0);
|
||||
btVector3 externalTorqueImpulseA = bodyA->m_originalBody ? bodyA->m_externalTorqueImpulse: btVector3(0,0,0);
|
||||
btVector3 externalForceImpulseB = bodyB->m_originalBody ? bodyB->m_externalForceImpulse: btVector3(0,0,0);
|
||||
btVector3 externalTorqueImpulseB = bodyB->m_originalBody ?bodyB->m_externalTorqueImpulse : btVector3(0,0,0);
|
||||
|
||||
|
||||
btScalar vel1Dotn = solverConstraint.m_contactNormal1.dot(bodyA->m_linearVelocity+externalForceImpulseA)
|
||||
+ solverConstraint.m_relpos1CrossNormal.dot(bodyA->m_angularVelocity+externalTorqueImpulseA);
|
||||
btScalar vel2Dotn = solverConstraint.m_contactNormal2.dot(bodyB->m_linearVelocity+externalForceImpulseB)
|
||||
+ solverConstraint.m_relpos2CrossNormal.dot(bodyB->m_angularVelocity+externalTorqueImpulseB);
|
||||
btScalar rel_vel = vel1Dotn+vel2Dotn;
|
||||
|
||||
btScalar positionalError = 0.f;
|
||||
|
|
@ -675,7 +726,7 @@ void btSequentialImpulseConstraintSolver::setupContactConstraint(btSolverConstra
|
|||
if (!infoGlobal.m_splitImpulse || (penetration > infoGlobal.m_splitImpulsePenetrationThreshold))
|
||||
{
|
||||
//combine position and velocity into rhs
|
||||
solverConstraint.m_rhs = penetrationImpulse+velocityImpulse;
|
||||
solverConstraint.m_rhs = penetrationImpulse+velocityImpulse;//-solverConstraint.m_contactNormal1.dot(bodyA->m_externalForce*bodyA->m_invMass-bodyB->m_externalForce/bodyB->m_invMass)*solverConstraint.m_jacDiagABInv;
|
||||
solverConstraint.m_rhsPenetration = 0.f;
|
||||
|
||||
} else
|
||||
|
|
@ -713,9 +764,9 @@ void btSequentialImpulseConstraintSolver::setFrictionConstraintImpulse( btSolver
|
|||
{
|
||||
frictionConstraint1.m_appliedImpulse = cp.m_appliedImpulseLateral1 * infoGlobal.m_warmstartingFactor;
|
||||
if (rb0)
|
||||
bodyA->internalApplyImpulse(frictionConstraint1.m_contactNormal*rb0->getInvMass()*rb0->getLinearFactor(),frictionConstraint1.m_angularComponentA,frictionConstraint1.m_appliedImpulse);
|
||||
bodyA->internalApplyImpulse(frictionConstraint1.m_contactNormal1*rb0->getInvMass()*rb0->getLinearFactor(),frictionConstraint1.m_angularComponentA,frictionConstraint1.m_appliedImpulse);
|
||||
if (rb1)
|
||||
bodyB->internalApplyImpulse(frictionConstraint1.m_contactNormal*rb1->getInvMass()*rb1->getLinearFactor(),-frictionConstraint1.m_angularComponentB,-(btScalar)frictionConstraint1.m_appliedImpulse);
|
||||
bodyB->internalApplyImpulse(-frictionConstraint1.m_contactNormal2*rb1->getInvMass()*rb1->getLinearFactor(),-frictionConstraint1.m_angularComponentB,-(btScalar)frictionConstraint1.m_appliedImpulse);
|
||||
} else
|
||||
{
|
||||
frictionConstraint1.m_appliedImpulse = 0.f;
|
||||
|
|
@ -729,9 +780,9 @@ void btSequentialImpulseConstraintSolver::setFrictionConstraintImpulse( btSolver
|
|||
{
|
||||
frictionConstraint2.m_appliedImpulse = cp.m_appliedImpulseLateral2 * infoGlobal.m_warmstartingFactor;
|
||||
if (rb0)
|
||||
bodyA->internalApplyImpulse(frictionConstraint2.m_contactNormal*rb0->getInvMass(),frictionConstraint2.m_angularComponentA,frictionConstraint2.m_appliedImpulse);
|
||||
bodyA->internalApplyImpulse(frictionConstraint2.m_contactNormal1*rb0->getInvMass(),frictionConstraint2.m_angularComponentA,frictionConstraint2.m_appliedImpulse);
|
||||
if (rb1)
|
||||
bodyB->internalApplyImpulse(frictionConstraint2.m_contactNormal*rb1->getInvMass(),-frictionConstraint2.m_angularComponentB,-(btScalar)frictionConstraint2.m_appliedImpulse);
|
||||
bodyB->internalApplyImpulse(-frictionConstraint2.m_contactNormal2*rb1->getInvMass(),-frictionConstraint2.m_angularComponentB,-(btScalar)frictionConstraint2.m_appliedImpulse);
|
||||
} else
|
||||
{
|
||||
frictionConstraint2.m_appliedImpulse = 0.f;
|
||||
|
|
@ -749,8 +800,8 @@ void btSequentialImpulseConstraintSolver::convertContact(btPersistentManifold* m
|
|||
colObj0 = (btCollisionObject*)manifold->getBody0();
|
||||
colObj1 = (btCollisionObject*)manifold->getBody1();
|
||||
|
||||
int solverBodyIdA = getOrInitSolverBody(*colObj0);
|
||||
int solverBodyIdB = getOrInitSolverBody(*colObj1);
|
||||
int solverBodyIdA = getOrInitSolverBody(*colObj0,infoGlobal.m_timeStep);
|
||||
int solverBodyIdB = getOrInitSolverBody(*colObj1,infoGlobal.m_timeStep);
|
||||
|
||||
// btRigidBody* bodyA = btRigidBody::upcast(colObj0);
|
||||
// btRigidBody* bodyB = btRigidBody::upcast(colObj1);
|
||||
|
|
@ -761,7 +812,7 @@ void btSequentialImpulseConstraintSolver::convertContact(btPersistentManifold* m
|
|||
|
||||
|
||||
///avoid collision response between two static objects
|
||||
if (!solverBodyA || (!solverBodyA->m_originalBody && (!solverBodyB || !solverBodyB->m_originalBody)))
|
||||
if (!solverBodyA || (solverBodyA->m_invMass.isZero() && (!solverBodyB || solverBodyB->m_invMass.isZero())))
|
||||
return;
|
||||
|
||||
int rollingFriction=1;
|
||||
|
|
@ -775,19 +826,35 @@ void btSequentialImpulseConstraintSolver::convertContact(btPersistentManifold* m
|
|||
btVector3 rel_pos1;
|
||||
btVector3 rel_pos2;
|
||||
btScalar relaxation;
|
||||
btScalar rel_vel;
|
||||
btVector3 vel;
|
||||
|
||||
|
||||
int frictionIndex = m_tmpSolverContactConstraintPool.size();
|
||||
btSolverConstraint& solverConstraint = m_tmpSolverContactConstraintPool.expandNonInitializing();
|
||||
// btRigidBody* rb0 = btRigidBody::upcast(colObj0);
|
||||
// btRigidBody* rb1 = btRigidBody::upcast(colObj1);
|
||||
btRigidBody* rb0 = btRigidBody::upcast(colObj0);
|
||||
btRigidBody* rb1 = btRigidBody::upcast(colObj1);
|
||||
solverConstraint.m_solverBodyIdA = solverBodyIdA;
|
||||
solverConstraint.m_solverBodyIdB = solverBodyIdB;
|
||||
|
||||
solverConstraint.m_originalContactPoint = &cp;
|
||||
|
||||
setupContactConstraint(solverConstraint, solverBodyIdA, solverBodyIdB, cp, infoGlobal, vel, rel_vel, relaxation, rel_pos1, rel_pos2);
|
||||
const btVector3& pos1 = cp.getPositionWorldOnA();
|
||||
const btVector3& pos2 = cp.getPositionWorldOnB();
|
||||
|
||||
rel_pos1 = pos1 - colObj0->getWorldTransform().getOrigin();
|
||||
rel_pos2 = pos2 - colObj1->getWorldTransform().getOrigin();
|
||||
|
||||
btVector3 vel1;// = rb0 ? rb0->getVelocityInLocalPoint(rel_pos1) : btVector3(0,0,0);
|
||||
btVector3 vel2;// = rb1 ? rb1->getVelocityInLocalPoint(rel_pos2) : btVector3(0,0,0);
|
||||
|
||||
solverBodyA->getVelocityInLocalPointNoDelta(rel_pos1,vel1);
|
||||
solverBodyB->getVelocityInLocalPointNoDelta(rel_pos2,vel2 );
|
||||
|
||||
btVector3 vel = vel1 - vel2;
|
||||
btScalar rel_vel = cp.m_normalWorldOnB.dot(vel);
|
||||
|
||||
setupContactConstraint(solverConstraint, solverBodyIdA, solverBodyIdB, cp, infoGlobal, relaxation, rel_pos1, rel_pos2);
|
||||
|
||||
|
||||
|
||||
// const btVector3& pos1 = cp.getPositionWorldOnA();
|
||||
// const btVector3& pos2 = cp.getPositionWorldOnB();
|
||||
|
|
@ -796,9 +863,11 @@ void btSequentialImpulseConstraintSolver::convertContact(btPersistentManifold* m
|
|||
|
||||
solverConstraint.m_frictionIndex = m_tmpSolverContactFrictionConstraintPool.size();
|
||||
|
||||
btVector3 angVelA,angVelB;
|
||||
solverBodyA->getAngularVelocity(angVelA);
|
||||
solverBodyB->getAngularVelocity(angVelB);
|
||||
btVector3 angVelA(0,0,0),angVelB(0,0,0);
|
||||
if (rb0)
|
||||
angVelA = rb0->getAngularVelocity();
|
||||
if (rb1)
|
||||
angVelB = rb1->getAngularVelocity();
|
||||
btVector3 relAngVel = angVelB-angVelA;
|
||||
|
||||
if ((cp.m_combinedRollingFriction>0.f) && (rollingFriction>0))
|
||||
|
|
@ -852,6 +921,10 @@ void btSequentialImpulseConstraintSolver::convertContact(btPersistentManifold* m
|
|||
if (!(infoGlobal.m_solverMode & SOLVER_DISABLE_VELOCITY_DEPENDENT_FRICTION_DIRECTION) && lat_rel_vel > SIMD_EPSILON)
|
||||
{
|
||||
cp.m_lateralFrictionDir1 *= 1.f/btSqrt(lat_rel_vel);
|
||||
applyAnisotropicFriction(colObj0,cp.m_lateralFrictionDir1,btCollisionObject::CF_ANISOTROPIC_FRICTION);
|
||||
applyAnisotropicFriction(colObj1,cp.m_lateralFrictionDir1,btCollisionObject::CF_ANISOTROPIC_FRICTION);
|
||||
addFrictionConstraint(cp.m_lateralFrictionDir1,solverBodyIdA,solverBodyIdB,frictionIndex,cp,rel_pos1,rel_pos2,colObj0,colObj1, relaxation);
|
||||
|
||||
if((infoGlobal.m_solverMode & SOLVER_USE_2_FRICTION_DIRECTIONS))
|
||||
{
|
||||
cp.m_lateralFrictionDir2 = cp.m_lateralFrictionDir1.cross(cp.m_normalWorldOnB);
|
||||
|
|
@ -859,17 +932,16 @@ void btSequentialImpulseConstraintSolver::convertContact(btPersistentManifold* m
|
|||
applyAnisotropicFriction(colObj0,cp.m_lateralFrictionDir2,btCollisionObject::CF_ANISOTROPIC_FRICTION);
|
||||
applyAnisotropicFriction(colObj1,cp.m_lateralFrictionDir2,btCollisionObject::CF_ANISOTROPIC_FRICTION);
|
||||
addFrictionConstraint(cp.m_lateralFrictionDir2,solverBodyIdA,solverBodyIdB,frictionIndex,cp,rel_pos1,rel_pos2,colObj0,colObj1, relaxation);
|
||||
|
||||
}
|
||||
|
||||
applyAnisotropicFriction(colObj0,cp.m_lateralFrictionDir1,btCollisionObject::CF_ANISOTROPIC_FRICTION);
|
||||
applyAnisotropicFriction(colObj1,cp.m_lateralFrictionDir1,btCollisionObject::CF_ANISOTROPIC_FRICTION);
|
||||
addFrictionConstraint(cp.m_lateralFrictionDir1,solverBodyIdA,solverBodyIdB,frictionIndex,cp,rel_pos1,rel_pos2,colObj0,colObj1, relaxation);
|
||||
|
||||
} else
|
||||
{
|
||||
btPlaneSpace1(cp.m_normalWorldOnB,cp.m_lateralFrictionDir1,cp.m_lateralFrictionDir2);
|
||||
|
||||
applyAnisotropicFriction(colObj0,cp.m_lateralFrictionDir1,btCollisionObject::CF_ANISOTROPIC_FRICTION);
|
||||
applyAnisotropicFriction(colObj1,cp.m_lateralFrictionDir1,btCollisionObject::CF_ANISOTROPIC_FRICTION);
|
||||
addFrictionConstraint(cp.m_lateralFrictionDir1,solverBodyIdA,solverBodyIdB,frictionIndex,cp,rel_pos1,rel_pos2,colObj0,colObj1, relaxation);
|
||||
|
||||
if ((infoGlobal.m_solverMode & SOLVER_USE_2_FRICTION_DIRECTIONS))
|
||||
{
|
||||
applyAnisotropicFriction(colObj0,cp.m_lateralFrictionDir2,btCollisionObject::CF_ANISOTROPIC_FRICTION);
|
||||
|
|
@ -877,9 +949,6 @@ void btSequentialImpulseConstraintSolver::convertContact(btPersistentManifold* m
|
|||
addFrictionConstraint(cp.m_lateralFrictionDir2,solverBodyIdA,solverBodyIdB,frictionIndex,cp,rel_pos1,rel_pos2,colObj0,colObj1, relaxation);
|
||||
}
|
||||
|
||||
applyAnisotropicFriction(colObj0,cp.m_lateralFrictionDir1,btCollisionObject::CF_ANISOTROPIC_FRICTION);
|
||||
applyAnisotropicFriction(colObj1,cp.m_lateralFrictionDir1,btCollisionObject::CF_ANISOTROPIC_FRICTION);
|
||||
addFrictionConstraint(cp.m_lateralFrictionDir1,solverBodyIdA,solverBodyIdB,frictionIndex,cp,rel_pos1,rel_pos2,colObj0,colObj1, relaxation);
|
||||
|
||||
if ((infoGlobal.m_solverMode & SOLVER_USE_2_FRICTION_DIRECTIONS) && (infoGlobal.m_solverMode & SOLVER_DISABLE_VELOCITY_DEPENDENT_FRICTION_DIRECTION))
|
||||
{
|
||||
|
|
@ -894,8 +963,8 @@ void btSequentialImpulseConstraintSolver::convertContact(btPersistentManifold* m
|
|||
if ((infoGlobal.m_solverMode & SOLVER_USE_2_FRICTION_DIRECTIONS))
|
||||
addFrictionConstraint(cp.m_lateralFrictionDir2,solverBodyIdA,solverBodyIdB,frictionIndex,cp,rel_pos1,rel_pos2,colObj0,colObj1, relaxation, cp.m_contactMotion2, cp.m_contactCFM2);
|
||||
|
||||
setFrictionConstraintImpulse( solverConstraint, solverBodyIdA, solverBodyIdB, cp, infoGlobal);
|
||||
}
|
||||
setFrictionConstraintImpulse( solverConstraint, solverBodyIdA, solverBodyIdB, cp, infoGlobal);
|
||||
|
||||
|
||||
|
||||
|
|
@ -904,15 +973,29 @@ void btSequentialImpulseConstraintSolver::convertContact(btPersistentManifold* m
|
|||
}
|
||||
}
|
||||
|
||||
btScalar btSequentialImpulseConstraintSolver::solveGroupCacheFriendlySetup(btCollisionObject** bodies, int numBodies, btPersistentManifold** manifoldPtr, int numManifolds,btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& infoGlobal,btIDebugDraw* debugDrawer,btStackAlloc* stackAlloc)
|
||||
void btSequentialImpulseConstraintSolver::convertContacts(btPersistentManifold** manifoldPtr,int numManifolds, const btContactSolverInfo& infoGlobal)
|
||||
{
|
||||
int i;
|
||||
btPersistentManifold* manifold = 0;
|
||||
// btCollisionObject* colObj0=0,*colObj1=0;
|
||||
|
||||
|
||||
for (i=0;i<numManifolds;i++)
|
||||
{
|
||||
manifold = manifoldPtr[i];
|
||||
convertContact(manifold,infoGlobal);
|
||||
}
|
||||
}
|
||||
|
||||
btScalar btSequentialImpulseConstraintSolver::solveGroupCacheFriendlySetup(btCollisionObject** bodies, int numBodies, btPersistentManifold** manifoldPtr, int numManifolds,btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& infoGlobal,btIDebugDraw* debugDrawer)
|
||||
{
|
||||
m_fixedBodyId = -1;
|
||||
BT_PROFILE("solveGroupCacheFriendlySetup");
|
||||
(void)stackAlloc;
|
||||
(void)debugDrawer;
|
||||
|
||||
m_maxOverrideNumSolverIterations = 0;
|
||||
|
||||
#ifdef BT_DEBUG
|
||||
#ifdef BT_ADDITIONAL_DEBUG
|
||||
//make sure that dynamic bodies exist for all (enabled) constraints
|
||||
for (int i=0;i<numConstraints;i++)
|
||||
{
|
||||
|
|
@ -979,7 +1062,7 @@ btScalar btSequentialImpulseConstraintSolver::solveGroupCacheFriendlySetup(btCol
|
|||
btAssert(found);
|
||||
}
|
||||
}
|
||||
#endif //BT_DEBUG
|
||||
#endif //BT_ADDITIONAL_DEBUG
|
||||
|
||||
|
||||
for (int i = 0; i < numBodies; i++)
|
||||
|
|
@ -991,14 +1074,15 @@ btScalar btSequentialImpulseConstraintSolver::solveGroupCacheFriendlySetup(btCol
|
|||
m_tmpSolverBodyPool.reserve(numBodies+1);
|
||||
m_tmpSolverBodyPool.resize(0);
|
||||
|
||||
btSolverBody& fixedBody = m_tmpSolverBodyPool.expand();
|
||||
initSolverBody(&fixedBody,0);
|
||||
//btSolverBody& fixedBody = m_tmpSolverBodyPool.expand();
|
||||
//initSolverBody(&fixedBody,0);
|
||||
|
||||
//convert all bodies
|
||||
|
||||
for (int i=0;i<numBodies;i++)
|
||||
{
|
||||
int bodyId = getOrInitSolverBody(*bodies[i]);
|
||||
int bodyId = getOrInitSolverBody(*bodies[i],infoGlobal.m_timeStep);
|
||||
|
||||
btRigidBody* body = btRigidBody::upcast(bodies[i]);
|
||||
if (body && body->getInvMass())
|
||||
{
|
||||
|
|
@ -1007,9 +1091,8 @@ btScalar btSequentialImpulseConstraintSolver::solveGroupCacheFriendlySetup(btCol
|
|||
if (body->getFlags()&BT_ENABLE_GYROPSCOPIC_FORCE)
|
||||
{
|
||||
gyroForce = body->computeGyroscopicForce(infoGlobal.m_maxGyroscopicForce);
|
||||
solverBody.m_externalTorqueImpulse -= gyroForce*body->getInvInertiaTensorWorld()*infoGlobal.m_timeStep;
|
||||
}
|
||||
solverBody.m_linearVelocity += body->getTotalForce()*body->getInvMass()*infoGlobal.m_timeStep;
|
||||
solverBody.m_angularVelocity += (body->getTotalTorque()-gyroForce)*body->getInvInertiaTensorWorld()*infoGlobal.m_timeStep;
|
||||
}
|
||||
}
|
||||
|
||||
|
|
@ -1079,8 +1162,8 @@ btScalar btSequentialImpulseConstraintSolver::solveGroupCacheFriendlySetup(btCol
|
|||
btRigidBody& rbA = constraint->getRigidBodyA();
|
||||
btRigidBody& rbB = constraint->getRigidBodyB();
|
||||
|
||||
int solverBodyIdA = getOrInitSolverBody(rbA);
|
||||
int solverBodyIdB = getOrInitSolverBody(rbB);
|
||||
int solverBodyIdA = getOrInitSolverBody(rbA,infoGlobal.m_timeStep);
|
||||
int solverBodyIdB = getOrInitSolverBody(rbB,infoGlobal.m_timeStep);
|
||||
|
||||
btSolverBody* bodyAPtr = &m_tmpSolverBodyPool[solverBodyIdA];
|
||||
btSolverBody* bodyBPtr = &m_tmpSolverBodyPool[solverBodyIdB];
|
||||
|
|
@ -1119,9 +1202,9 @@ btScalar btSequentialImpulseConstraintSolver::solveGroupCacheFriendlySetup(btCol
|
|||
btTypedConstraint::btConstraintInfo2 info2;
|
||||
info2.fps = 1.f/infoGlobal.m_timeStep;
|
||||
info2.erp = infoGlobal.m_erp;
|
||||
info2.m_J1linearAxis = currentConstraintRow->m_contactNormal;
|
||||
info2.m_J1linearAxis = currentConstraintRow->m_contactNormal1;
|
||||
info2.m_J1angularAxis = currentConstraintRow->m_relpos1CrossNormal;
|
||||
info2.m_J2linearAxis = 0;
|
||||
info2.m_J2linearAxis = currentConstraintRow->m_contactNormal2;
|
||||
info2.m_J2angularAxis = currentConstraintRow->m_relpos2CrossNormal;
|
||||
info2.rowskip = sizeof(btSolverConstraint)/sizeof(btScalar);//check this
|
||||
///the size of btSolverConstraint needs be a multiple of btScalar
|
||||
|
|
@ -1162,14 +1245,14 @@ btScalar btSequentialImpulseConstraintSolver::solveGroupCacheFriendlySetup(btCol
|
|||
}
|
||||
|
||||
{
|
||||
btVector3 iMJlA = solverConstraint.m_contactNormal*rbA.getInvMass();
|
||||
btVector3 iMJlA = solverConstraint.m_contactNormal1*rbA.getInvMass();
|
||||
btVector3 iMJaA = rbA.getInvInertiaTensorWorld()*solverConstraint.m_relpos1CrossNormal;
|
||||
btVector3 iMJlB = solverConstraint.m_contactNormal*rbB.getInvMass();//sign of normal?
|
||||
btVector3 iMJlB = solverConstraint.m_contactNormal2*rbB.getInvMass();//sign of normal?
|
||||
btVector3 iMJaB = rbB.getInvInertiaTensorWorld()*solverConstraint.m_relpos2CrossNormal;
|
||||
|
||||
btScalar sum = iMJlA.dot(solverConstraint.m_contactNormal);
|
||||
btScalar sum = iMJlA.dot(solverConstraint.m_contactNormal1);
|
||||
sum += iMJaA.dot(solverConstraint.m_relpos1CrossNormal);
|
||||
sum += iMJlB.dot(solverConstraint.m_contactNormal);
|
||||
sum += iMJlB.dot(solverConstraint.m_contactNormal2);
|
||||
sum += iMJaB.dot(solverConstraint.m_relpos2CrossNormal);
|
||||
btScalar fsum = btFabs(sum);
|
||||
btAssert(fsum > SIMD_EPSILON);
|
||||
|
|
@ -1177,15 +1260,22 @@ btScalar btSequentialImpulseConstraintSolver::solveGroupCacheFriendlySetup(btCol
|
|||
}
|
||||
|
||||
|
||||
///fix rhs
|
||||
///todo: add force/torque accelerators
|
||||
|
||||
{
|
||||
btScalar rel_vel;
|
||||
btScalar vel1Dotn = solverConstraint.m_contactNormal.dot(rbA.getLinearVelocity()) + solverConstraint.m_relpos1CrossNormal.dot(rbA.getAngularVelocity());
|
||||
btScalar vel2Dotn = -solverConstraint.m_contactNormal.dot(rbB.getLinearVelocity()) + solverConstraint.m_relpos2CrossNormal.dot(rbB.getAngularVelocity());
|
||||
btVector3 externalForceImpulseA = bodyAPtr->m_originalBody ? bodyAPtr->m_externalForceImpulse : btVector3(0,0,0);
|
||||
btVector3 externalTorqueImpulseA = bodyAPtr->m_originalBody ? bodyAPtr->m_externalTorqueImpulse : btVector3(0,0,0);
|
||||
|
||||
btVector3 externalForceImpulseB = bodyBPtr->m_originalBody ? bodyBPtr->m_externalForceImpulse : btVector3(0,0,0);
|
||||
btVector3 externalTorqueImpulseB = bodyBPtr->m_originalBody ?bodyBPtr->m_externalTorqueImpulse : btVector3(0,0,0);
|
||||
|
||||
btScalar vel1Dotn = solverConstraint.m_contactNormal1.dot(rbA.getLinearVelocity()+externalForceImpulseA)
|
||||
+ solverConstraint.m_relpos1CrossNormal.dot(rbA.getAngularVelocity()+externalTorqueImpulseA);
|
||||
|
||||
btScalar vel2Dotn = solverConstraint.m_contactNormal2.dot(rbB.getLinearVelocity()+externalForceImpulseB)
|
||||
+ solverConstraint.m_relpos2CrossNormal.dot(rbB.getAngularVelocity()+externalTorqueImpulseB);
|
||||
|
||||
rel_vel = vel1Dotn+vel2Dotn;
|
||||
|
||||
btScalar restitution = 0.f;
|
||||
btScalar positionalError = solverConstraint.m_rhs;//already filled in by getConstraintInfo2
|
||||
btScalar velocityError = restitution - rel_vel * info2.m_damping;
|
||||
|
|
@ -1194,6 +1284,7 @@ btScalar btSequentialImpulseConstraintSolver::solveGroupCacheFriendlySetup(btCol
|
|||
solverConstraint.m_rhs = penetrationImpulse+velocityImpulse;
|
||||
solverConstraint.m_appliedImpulse = 0.f;
|
||||
|
||||
|
||||
}
|
||||
}
|
||||
}
|
||||
|
|
@ -1201,18 +1292,8 @@ btScalar btSequentialImpulseConstraintSolver::solveGroupCacheFriendlySetup(btCol
|
|||
}
|
||||
}
|
||||
|
||||
{
|
||||
int i;
|
||||
btPersistentManifold* manifold = 0;
|
||||
// btCollisionObject* colObj0=0,*colObj1=0;
|
||||
convertContacts(manifoldPtr,numManifolds,infoGlobal);
|
||||
|
||||
|
||||
for (i=0;i<numManifolds;i++)
|
||||
{
|
||||
manifold = manifoldPtr[i];
|
||||
convertContact(manifold,infoGlobal);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
// btContactSolverInfo info = infoGlobal;
|
||||
|
|
@ -1251,7 +1332,7 @@ btScalar btSequentialImpulseConstraintSolver::solveGroupCacheFriendlySetup(btCol
|
|||
}
|
||||
|
||||
|
||||
btScalar btSequentialImpulseConstraintSolver::solveSingleIteration(int iteration, btCollisionObject** /*bodies */,int /*numBodies*/,btPersistentManifold** /*manifoldPtr*/, int /*numManifolds*/,btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& infoGlobal,btIDebugDraw* /*debugDrawer*/,btStackAlloc* /*stackAlloc*/)
|
||||
btScalar btSequentialImpulseConstraintSolver::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();
|
||||
|
|
@ -1304,14 +1385,14 @@ btScalar btSequentialImpulseConstraintSolver::solveSingleIteration(int iteration
|
|||
{
|
||||
for (int j=0;j<numConstraints;j++)
|
||||
{
|
||||
if (constraints[j]->isEnabled())
|
||||
{
|
||||
int bodyAid = getOrInitSolverBody(constraints[j]->getRigidBodyA());
|
||||
int bodyBid = getOrInitSolverBody(constraints[j]->getRigidBodyB());
|
||||
btSolverBody& bodyA = m_tmpSolverBodyPool[bodyAid];
|
||||
btSolverBody& bodyB = m_tmpSolverBodyPool[bodyBid];
|
||||
constraints[j]->solveConstraintObsolete(bodyA,bodyB,infoGlobal.m_timeStep);
|
||||
}
|
||||
if (constraints[j]->isEnabled())
|
||||
{
|
||||
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);
|
||||
}
|
||||
}
|
||||
|
||||
///solve all contact constraints using SIMD, if available
|
||||
|
|
@ -1371,7 +1452,8 @@ btScalar btSequentialImpulseConstraintSolver::solveSingleIteration(int iteration
|
|||
for (j=0;j<numPoolConstraints;j++)
|
||||
{
|
||||
const btSolverConstraint& solveManifold = m_tmpSolverContactConstraintPool[m_orderTmpConstraintPool[j]];
|
||||
resolveSingleConstraintRowLowerLimitSIMD(m_tmpSolverBodyPool[solveManifold.m_solverBodyIdA],m_tmpSolverBodyPool[solveManifold.m_solverBodyIdB],solveManifold);
|
||||
//resolveSingleConstraintRowLowerLimitSIMD(m_tmpSolverBodyPool[solveManifold.m_solverBodyIdA],m_tmpSolverBodyPool[solveManifold.m_solverBodyIdB],solveManifold);
|
||||
resolveSingleConstraintRowLowerLimit(m_tmpSolverBodyPool[solveManifold.m_solverBodyIdA],m_tmpSolverBodyPool[solveManifold.m_solverBodyIdB],solveManifold);
|
||||
|
||||
}
|
||||
|
||||
|
|
@ -1390,7 +1472,8 @@ btScalar btSequentialImpulseConstraintSolver::solveSingleIteration(int iteration
|
|||
solveManifold.m_lowerLimit = -(solveManifold.m_friction*totalImpulse);
|
||||
solveManifold.m_upperLimit = solveManifold.m_friction*totalImpulse;
|
||||
|
||||
resolveSingleConstraintRowGenericSIMD(m_tmpSolverBodyPool[solveManifold.m_solverBodyIdA],m_tmpSolverBodyPool[solveManifold.m_solverBodyIdB],solveManifold);
|
||||
//resolveSingleConstraintRowGenericSIMD(m_tmpSolverBodyPool[solveManifold.m_solverBodyIdA],m_tmpSolverBodyPool[solveManifold.m_solverBodyIdB],solveManifold);
|
||||
resolveSingleConstraintRowGeneric(m_tmpSolverBodyPool[solveManifold.m_solverBodyIdA],m_tmpSolverBodyPool[solveManifold.m_solverBodyIdB],solveManifold);
|
||||
}
|
||||
}
|
||||
|
||||
|
|
@ -1432,14 +1515,14 @@ btScalar btSequentialImpulseConstraintSolver::solveSingleIteration(int iteration
|
|||
{
|
||||
for (int j=0;j<numConstraints;j++)
|
||||
{
|
||||
if (constraints[j]->isEnabled())
|
||||
{
|
||||
int bodyAid = getOrInitSolverBody(constraints[j]->getRigidBodyA());
|
||||
int bodyBid = getOrInitSolverBody(constraints[j]->getRigidBodyB());
|
||||
btSolverBody& bodyA = m_tmpSolverBodyPool[bodyAid];
|
||||
btSolverBody& bodyB = m_tmpSolverBodyPool[bodyBid];
|
||||
constraints[j]->solveConstraintObsolete(bodyA,bodyB,infoGlobal.m_timeStep);
|
||||
}
|
||||
if (constraints[j]->isEnabled())
|
||||
{
|
||||
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);
|
||||
}
|
||||
}
|
||||
///solve all contact constraints
|
||||
int numPoolConstraints = m_tmpSolverContactConstraintPool.size();
|
||||
|
|
@ -1487,7 +1570,7 @@ btScalar btSequentialImpulseConstraintSolver::solveSingleIteration(int iteration
|
|||
}
|
||||
|
||||
|
||||
void btSequentialImpulseConstraintSolver::solveGroupCacheFriendlySplitImpulseIterations(btCollisionObject** bodies,int numBodies,btPersistentManifold** manifoldPtr, int numManifolds,btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& infoGlobal,btIDebugDraw* debugDrawer,btStackAlloc* stackAlloc)
|
||||
void btSequentialImpulseConstraintSolver::solveGroupCacheFriendlySplitImpulseIterations(btCollisionObject** bodies,int numBodies,btPersistentManifold** manifoldPtr, int numManifolds,btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& infoGlobal,btIDebugDraw* debugDrawer)
|
||||
{
|
||||
int iteration;
|
||||
if (infoGlobal.m_splitImpulse)
|
||||
|
|
@ -1527,20 +1610,20 @@ void btSequentialImpulseConstraintSolver::solveGroupCacheFriendlySplitImpulseIte
|
|||
}
|
||||
}
|
||||
|
||||
btScalar btSequentialImpulseConstraintSolver::solveGroupCacheFriendlyIterations(btCollisionObject** bodies ,int numBodies,btPersistentManifold** manifoldPtr, int numManifolds,btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& infoGlobal,btIDebugDraw* debugDrawer,btStackAlloc* stackAlloc)
|
||||
btScalar btSequentialImpulseConstraintSolver::solveGroupCacheFriendlyIterations(btCollisionObject** bodies ,int numBodies,btPersistentManifold** manifoldPtr, int numManifolds,btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& infoGlobal,btIDebugDraw* debugDrawer)
|
||||
{
|
||||
BT_PROFILE("solveGroupCacheFriendlyIterations");
|
||||
|
||||
{
|
||||
///this is a special step to resolve penetrations (just for contacts)
|
||||
solveGroupCacheFriendlySplitImpulseIterations(bodies ,numBodies,manifoldPtr, numManifolds,constraints,numConstraints,infoGlobal,debugDrawer,stackAlloc);
|
||||
solveGroupCacheFriendlySplitImpulseIterations(bodies ,numBodies,manifoldPtr, numManifolds,constraints,numConstraints,infoGlobal,debugDrawer);
|
||||
|
||||
int maxIterations = m_maxOverrideNumSolverIterations > infoGlobal.m_numIterations? m_maxOverrideNumSolverIterations : infoGlobal.m_numIterations;
|
||||
|
||||
for ( int iteration = 0 ; iteration< maxIterations ; iteration++)
|
||||
//for ( int iteration = maxIterations-1 ; iteration >= 0;iteration--)
|
||||
{
|
||||
solveSingleIteration(iteration, bodies ,numBodies,manifoldPtr, numManifolds,constraints,numConstraints,infoGlobal,debugDrawer,stackAlloc);
|
||||
solveSingleIteration(iteration, bodies ,numBodies,manifoldPtr, numManifolds,constraints,numConstraints,infoGlobal,debugDrawer);
|
||||
}
|
||||
|
||||
}
|
||||
|
|
@ -1580,10 +1663,10 @@ btScalar btSequentialImpulseConstraintSolver::solveGroupCacheFriendlyFinish(btCo
|
|||
btJointFeedback* fb = constr->getJointFeedback();
|
||||
if (fb)
|
||||
{
|
||||
fb->m_appliedForceBodyA += solverConstr.m_contactNormal*solverConstr.m_appliedImpulse*constr->getRigidBodyA().getLinearFactor()/infoGlobal.m_timeStep;
|
||||
fb->m_appliedForceBodyB += -solverConstr.m_contactNormal*solverConstr.m_appliedImpulse*constr->getRigidBodyB().getLinearFactor()/infoGlobal.m_timeStep;
|
||||
fb->m_appliedForceBodyA += solverConstr.m_contactNormal1*solverConstr.m_appliedImpulse*constr->getRigidBodyA().getLinearFactor()/infoGlobal.m_timeStep;
|
||||
fb->m_appliedForceBodyB += solverConstr.m_contactNormal2*solverConstr.m_appliedImpulse*constr->getRigidBodyB().getLinearFactor()/infoGlobal.m_timeStep;
|
||||
fb->m_appliedTorqueBodyA += solverConstr.m_relpos1CrossNormal* constr->getRigidBodyA().getAngularFactor()*solverConstr.m_appliedImpulse/infoGlobal.m_timeStep;
|
||||
fb->m_appliedTorqueBodyB += -solverConstr.m_relpos1CrossNormal* constr->getRigidBodyB().getAngularFactor()*solverConstr.m_appliedImpulse/infoGlobal.m_timeStep;
|
||||
fb->m_appliedTorqueBodyB += solverConstr.m_relpos2CrossNormal* constr->getRigidBodyB().getAngularFactor()*solverConstr.m_appliedImpulse/infoGlobal.m_timeStep; /*RGM ???? */
|
||||
|
||||
}
|
||||
|
||||
|
|
@ -1605,9 +1688,15 @@ btScalar btSequentialImpulseConstraintSolver::solveGroupCacheFriendlyFinish(btCo
|
|||
m_tmpSolverBodyPool[i].writebackVelocityAndTransform(infoGlobal.m_timeStep, infoGlobal.m_splitImpulseTurnErp);
|
||||
else
|
||||
m_tmpSolverBodyPool[i].writebackVelocity();
|
||||
|
||||
m_tmpSolverBodyPool[i].m_originalBody->setLinearVelocity(
|
||||
m_tmpSolverBodyPool[i].m_linearVelocity+
|
||||
m_tmpSolverBodyPool[i].m_externalForceImpulse);
|
||||
|
||||
m_tmpSolverBodyPool[i].m_originalBody->setAngularVelocity(
|
||||
m_tmpSolverBodyPool[i].m_angularVelocity+
|
||||
m_tmpSolverBodyPool[i].m_externalTorqueImpulse);
|
||||
|
||||
m_tmpSolverBodyPool[i].m_originalBody->setLinearVelocity(m_tmpSolverBodyPool[i].m_linearVelocity);
|
||||
m_tmpSolverBodyPool[i].m_originalBody->setAngularVelocity(m_tmpSolverBodyPool[i].m_angularVelocity);
|
||||
if (infoGlobal.m_splitImpulse)
|
||||
m_tmpSolverBodyPool[i].m_originalBody->setWorldTransform(m_tmpSolverBodyPool[i].m_worldTransform);
|
||||
|
||||
|
|
@ -1627,15 +1716,15 @@ btScalar btSequentialImpulseConstraintSolver::solveGroupCacheFriendlyFinish(btCo
|
|||
|
||||
|
||||
/// btSequentialImpulseConstraintSolver Sequentially applies impulses
|
||||
btScalar btSequentialImpulseConstraintSolver::solveGroup(btCollisionObject** bodies,int numBodies,btPersistentManifold** manifoldPtr, int numManifolds,btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& infoGlobal,btIDebugDraw* debugDrawer,btStackAlloc* stackAlloc,btDispatcher* /*dispatcher*/)
|
||||
btScalar btSequentialImpulseConstraintSolver::solveGroup(btCollisionObject** bodies,int numBodies,btPersistentManifold** manifoldPtr, int numManifolds,btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& infoGlobal,btIDebugDraw* debugDrawer,btDispatcher* /*dispatcher*/)
|
||||
{
|
||||
|
||||
BT_PROFILE("solveGroup");
|
||||
//you need to provide at least some bodies
|
||||
|
||||
solveGroupCacheFriendlySetup( bodies, numBodies, manifoldPtr, numManifolds,constraints, numConstraints,infoGlobal,debugDrawer, stackAlloc);
|
||||
solveGroupCacheFriendlySetup( bodies, numBodies, manifoldPtr, numManifolds,constraints, numConstraints,infoGlobal,debugDrawer);
|
||||
|
||||
solveGroupCacheFriendlyIterations(bodies, numBodies, manifoldPtr, numManifolds,constraints, numConstraints,infoGlobal,debugDrawer, stackAlloc);
|
||||
solveGroupCacheFriendlyIterations(bodies, numBodies, manifoldPtr, numManifolds,constraints, numConstraints,infoGlobal,debugDrawer);
|
||||
|
||||
solveGroupCacheFriendlyFinish(bodies, numBodies, infoGlobal);
|
||||
|
||||
|
|
|
|||
|
|
@ -18,7 +18,6 @@ subject to the following restrictions:
|
|||
|
||||
class btIDebugDraw;
|
||||
class btPersistentManifold;
|
||||
class btStackAlloc;
|
||||
class btDispatcher;
|
||||
class btCollisionObject;
|
||||
#include "BulletDynamics/ConstraintSolver/btTypedConstraint.h"
|
||||
|
|
@ -43,7 +42,7 @@ protected:
|
|||
btAlignedObjectArray<int> m_orderFrictionConstraintPool;
|
||||
btAlignedObjectArray<btTypedConstraint::btConstraintInfo1> m_tmpConstraintSizesPool;
|
||||
int m_maxOverrideNumSolverIterations;
|
||||
|
||||
int m_fixedBodyId;
|
||||
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,
|
||||
|
|
@ -57,10 +56,11 @@ protected:
|
|||
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, btScalar desiredVelocity=0., btScalar cfmSlip=0.);
|
||||
btSolverConstraint& addRollingFrictionConstraint(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, btScalar desiredVelocity=0, btScalar cfmSlip=0.f);
|
||||
|
||||
|
||||
|
||||
void setupContactConstraint(btSolverConstraint& solverConstraint, int solverBodyIdA, int solverBodyIdB, btManifoldPoint& cp,
|
||||
const btContactSolverInfo& infoGlobal, btVector3& vel, btScalar& rel_vel, btScalar& relaxation,
|
||||
btVector3& rel_pos1, btVector3& rel_pos2);
|
||||
const btContactSolverInfo& infoGlobal,btScalar& relaxation, const btVector3& rel_pos1, const btVector3& rel_pos2);
|
||||
|
||||
static void applyAnisotropicFriction(btCollisionObject* colObj,btVector3& frictionDirection, int frictionMode);
|
||||
|
||||
void setFrictionConstraintImpulse( btSolverConstraint& solverConstraint, int solverBodyIdA,int solverBodyIdB,
|
||||
btManifoldPoint& cp, const btContactSolverInfo& infoGlobal);
|
||||
|
|
@ -71,6 +71,8 @@ protected:
|
|||
|
||||
btScalar restitutionCurve(btScalar rel_vel, btScalar restitution);
|
||||
|
||||
virtual void convertContacts(btPersistentManifold** manifoldPtr, int numManifolds, const btContactSolverInfo& infoGlobal);
|
||||
|
||||
void convertContact(btPersistentManifold* manifold,const btContactSolverInfo& infoGlobal);
|
||||
|
||||
|
||||
|
|
@ -83,8 +85,8 @@ protected:
|
|||
const btSolverConstraint& contactConstraint);
|
||||
|
||||
//internal method
|
||||
int getOrInitSolverBody(btCollisionObject& body);
|
||||
void initSolverBody(btSolverBody* solverBody, btCollisionObject* collisionObject);
|
||||
int getOrInitSolverBody(btCollisionObject& body,btScalar timeStep);
|
||||
void initSolverBody(btSolverBody* solverBody, btCollisionObject* collisionObject, btScalar timeStep);
|
||||
|
||||
void resolveSingleConstraintRowGeneric(btSolverBody& bodyA,btSolverBody& bodyB,const btSolverConstraint& contactConstraint);
|
||||
|
||||
|
|
@ -97,12 +99,12 @@ protected:
|
|||
protected:
|
||||
|
||||
|
||||
virtual void solveGroupCacheFriendlySplitImpulseIterations(btCollisionObject** bodies,int numBodies,btPersistentManifold** manifoldPtr, int numManifolds,btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& infoGlobal,btIDebugDraw* debugDrawer,btStackAlloc* stackAlloc);
|
||||
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);
|
||||
btScalar solveSingleIteration(int iteration, btCollisionObject** bodies ,int numBodies,btPersistentManifold** manifoldPtr, int numManifolds,btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& infoGlobal,btIDebugDraw* debugDrawer,btStackAlloc* stackAlloc);
|
||||
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,btStackAlloc* stackAlloc);
|
||||
virtual btScalar solveGroupCacheFriendlyIterations(btCollisionObject** bodies,int numBodies,btPersistentManifold** manifoldPtr, int numManifolds,btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& infoGlobal,btIDebugDraw* debugDrawer,btStackAlloc* stackAlloc);
|
||||
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:
|
||||
|
|
@ -112,7 +114,7 @@ public:
|
|||
btSequentialImpulseConstraintSolver();
|
||||
virtual ~btSequentialImpulseConstraintSolver();
|
||||
|
||||
virtual btScalar solveGroup(btCollisionObject** bodies,int numBodies,btPersistentManifold** manifold,int numManifolds,btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& info, btIDebugDraw* debugDrawer, btStackAlloc* stackAlloc,btDispatcher* dispatcher);
|
||||
virtual btScalar solveGroup(btCollisionObject** bodies,int numBodies,btPersistentManifold** manifold,int numManifolds,btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& info, btIDebugDraw* debugDrawer,btDispatcher* dispatcher);
|
||||
|
||||
|
||||
|
||||
|
|
@ -132,6 +134,11 @@ public:
|
|||
return m_btSeed2;
|
||||
}
|
||||
|
||||
|
||||
virtual btConstraintSolverType getSolverType() const
|
||||
{
|
||||
return BT_SEQUENTIAL_IMPULSE_SOLVER;
|
||||
}
|
||||
};
|
||||
|
||||
|
||||
|
|
|
|||
|
|
@ -426,6 +426,8 @@ void btSliderConstraint::getInfo2NonVirtual(btConstraintInfo2* info, const btTra
|
|||
for (i=0; i<3; i++) info->m_J2angularAxis[s3+i] = -tmpB[i];
|
||||
for (i=0; i<3; i++) info->m_J1linearAxis[s2+i] = p[i];
|
||||
for (i=0; i<3; i++) info->m_J1linearAxis[s3+i] = q[i];
|
||||
for (i=0; i<3; i++) info->m_J2linearAxis[s2+i] = -p[i];
|
||||
for (i=0; i<3; i++) info->m_J2linearAxis[s3+i] = -q[i];
|
||||
}
|
||||
else
|
||||
{ // old way - maybe incorrect if bodies are not on the slider axis
|
||||
|
|
@ -440,6 +442,8 @@ void btSliderConstraint::getInfo2NonVirtual(btConstraintInfo2* info, const btTra
|
|||
|
||||
for (i=0; i<3; i++) info->m_J1linearAxis[s2+i] = p[i];
|
||||
for (i=0; i<3; i++) info->m_J1linearAxis[s3+i] = q[i];
|
||||
for (i=0; i<3; i++) info->m_J2linearAxis[s2+i] = -p[i];
|
||||
for (i=0; i<3; i++) info->m_J2linearAxis[s3+i] = -q[i];
|
||||
}
|
||||
// compute two elements of right hand side
|
||||
|
||||
|
|
@ -479,6 +483,9 @@ void btSliderConstraint::getInfo2NonVirtual(btConstraintInfo2* info, const btTra
|
|||
info->m_J1linearAxis[srow+0] = ax1[0];
|
||||
info->m_J1linearAxis[srow+1] = ax1[1];
|
||||
info->m_J1linearAxis[srow+2] = ax1[2];
|
||||
info->m_J2linearAxis[srow+0] = -ax1[0];
|
||||
info->m_J2linearAxis[srow+1] = -ax1[1];
|
||||
info->m_J2linearAxis[srow+2] = -ax1[2];
|
||||
// linear torque decoupling step:
|
||||
//
|
||||
// we have to be careful that the linear constraint forces (+/- ax1) applied to the two bodies
|
||||
|
|
|
|||
|
|
@ -25,7 +25,13 @@ TODO:
|
|||
#ifndef BT_SLIDER_CONSTRAINT_H
|
||||
#define BT_SLIDER_CONSTRAINT_H
|
||||
|
||||
|
||||
#ifdef BT_USE_DOUBLE_PRECISION
|
||||
#define btSliderConstraintData2 btSliderConstraintDoubleData
|
||||
#define btSliderConstraintDataName "btSliderConstraintDoubleData"
|
||||
#else
|
||||
#define btSliderConstraintData2 btSliderConstraintData
|
||||
#define btSliderConstraintDataName "btSliderConstraintData"
|
||||
#endif //BT_USE_DOUBLE_PRECISION
|
||||
|
||||
#include "LinearMath/btVector3.h"
|
||||
#include "btJacobianEntry.h"
|
||||
|
|
@ -283,7 +289,10 @@ public:
|
|||
|
||||
};
|
||||
|
||||
|
||||
///do not change those serialization structures, it requires an updated sBulletDNAstr/sBulletDNAstr64
|
||||
|
||||
|
||||
struct btSliderConstraintData
|
||||
{
|
||||
btTypedConstraintData m_typeConstraintData;
|
||||
|
|
@ -302,31 +311,48 @@ struct btSliderConstraintData
|
|||
};
|
||||
|
||||
|
||||
struct btSliderConstraintDoubleData
|
||||
{
|
||||
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;
|
||||
|
||||
int m_useLinearReferenceFrameA;
|
||||
int m_useOffsetForConstraintFrame;
|
||||
|
||||
};
|
||||
|
||||
SIMD_FORCE_INLINE int btSliderConstraint::calculateSerializeBufferSize() const
|
||||
{
|
||||
return sizeof(btSliderConstraintData);
|
||||
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
|
||||
{
|
||||
|
||||
btSliderConstraintData* sliderData = (btSliderConstraintData*) dataBuffer;
|
||||
btSliderConstraintData2* sliderData = (btSliderConstraintData2*) dataBuffer;
|
||||
btTypedConstraint::serialize(&sliderData->m_typeConstraintData,serializer);
|
||||
|
||||
m_frameInA.serializeFloat(sliderData->m_rbAFrame);
|
||||
m_frameInB.serializeFloat(sliderData->m_rbBFrame);
|
||||
m_frameInA.serialize(sliderData->m_rbAFrame);
|
||||
m_frameInB.serialize(sliderData->m_rbBFrame);
|
||||
|
||||
sliderData->m_linearUpperLimit = float(m_upperLinLimit);
|
||||
sliderData->m_linearLowerLimit = float(m_lowerLinLimit);
|
||||
sliderData->m_linearUpperLimit = m_upperLinLimit;
|
||||
sliderData->m_linearLowerLimit = m_lowerLinLimit;
|
||||
|
||||
sliderData->m_angularUpperLimit = float(m_upperAngLimit);
|
||||
sliderData->m_angularLowerLimit = float(m_lowerAngLimit);
|
||||
sliderData->m_angularUpperLimit = m_upperAngLimit;
|
||||
sliderData->m_angularLowerLimit = m_lowerAngLimit;
|
||||
|
||||
sliderData->m_useLinearReferenceFrameA = m_useLinearReferenceFrameA;
|
||||
sliderData->m_useOffsetForConstraintFrame = m_useOffsetForConstraintFrame;
|
||||
|
||||
return "btSliderConstraintData";
|
||||
return btSliderConstraintDataName;
|
||||
}
|
||||
|
||||
|
||||
|
|
|
|||
|
|
@ -105,7 +105,7 @@ operator+(const btSimdScalar& v1, const btSimdScalar& v2)
|
|||
#endif
|
||||
|
||||
///The btSolverBody is an internal datastructure for the constraint solver. Only necessary data is packed to increase cache coherence/performance.
|
||||
ATTRIBUTE_ALIGNED64 (struct) btSolverBody
|
||||
ATTRIBUTE_ALIGNED16 (struct) btSolverBody
|
||||
{
|
||||
BT_DECLARE_ALIGNED_ALLOCATOR();
|
||||
btTransform m_worldTransform;
|
||||
|
|
@ -118,6 +118,8 @@ ATTRIBUTE_ALIGNED64 (struct) btSolverBody
|
|||
btVector3 m_turnVelocity;
|
||||
btVector3 m_linearVelocity;
|
||||
btVector3 m_angularVelocity;
|
||||
btVector3 m_externalForceImpulse;
|
||||
btVector3 m_externalTorqueImpulse;
|
||||
|
||||
btRigidBody* m_originalBody;
|
||||
void setWorldTransform(const btTransform& worldTransform)
|
||||
|
|
@ -130,6 +132,17 @@ ATTRIBUTE_ALIGNED64 (struct) btSolverBody
|
|||
return m_worldTransform;
|
||||
}
|
||||
|
||||
|
||||
|
||||
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);
|
||||
else
|
||||
velocity.setValue(0,0,0);
|
||||
}
|
||||
|
||||
|
||||
SIMD_FORCE_INLINE void getVelocityInLocalPointObsolete(const btVector3& rel_pos, btVector3& velocity ) const
|
||||
{
|
||||
if (m_originalBody)
|
||||
|
|
|
|||
|
|
@ -32,10 +32,10 @@ ATTRIBUTE_ALIGNED16 (struct) btSolverConstraint
|
|||
BT_DECLARE_ALIGNED_ALLOCATOR();
|
||||
|
||||
btVector3 m_relpos1CrossNormal;
|
||||
btVector3 m_contactNormal;
|
||||
btVector3 m_contactNormal1;
|
||||
|
||||
btVector3 m_relpos2CrossNormal;
|
||||
//btVector3 m_contactNormal2;//usually m_contactNormal2 == -m_contactNormal
|
||||
btVector3 m_contactNormal2; //usually m_contactNormal2 == -m_contactNormal1, but not always
|
||||
|
||||
btVector3 m_angularComponentA;
|
||||
btVector3 m_angularComponentB;
|
||||
|
|
@ -55,6 +55,7 @@ ATTRIBUTE_ALIGNED16 (struct) btSolverConstraint
|
|||
{
|
||||
void* m_originalContactPoint;
|
||||
btScalar m_unusedPadding4;
|
||||
int m_numRowsForNonContactConstraint;
|
||||
};
|
||||
|
||||
int m_overrideNumSolverIterations;
|
||||
|
|
|
|||
|
|
@ -109,7 +109,7 @@ btScalar btTypedConstraint::getMotorFactor(btScalar pos, btScalar lowLim, btScal
|
|||
///fills the dataBuffer and returns the struct name (and 0 on failure)
|
||||
const char* btTypedConstraint::serialize(void* dataBuffer, btSerializer* serializer) const
|
||||
{
|
||||
btTypedConstraintData* tcd = (btTypedConstraintData*) dataBuffer;
|
||||
btTypedConstraintData2* tcd = (btTypedConstraintData2*) dataBuffer;
|
||||
|
||||
tcd->m_rbA = (btRigidBodyData*)serializer->getUniquePointer(&m_rbA);
|
||||
tcd->m_rbB = (btRigidBodyData*)serializer->getUniquePointer(&m_rbB);
|
||||
|
|
@ -123,14 +123,14 @@ const char* btTypedConstraint::serialize(void* dataBuffer, btSerializer* seriali
|
|||
tcd->m_objectType = m_objectType;
|
||||
tcd->m_needsFeedback = m_needsFeedback;
|
||||
tcd->m_overrideNumSolverIterations = m_overrideNumSolverIterations;
|
||||
tcd->m_breakingImpulseThreshold = float(m_breakingImpulseThreshold);
|
||||
tcd->m_breakingImpulseThreshold = m_breakingImpulseThreshold;
|
||||
tcd->m_isEnabled = m_isEnabled? 1: 0;
|
||||
|
||||
tcd->m_userConstraintId =m_userConstraintId;
|
||||
tcd->m_userConstraintType =m_userConstraintType;
|
||||
|
||||
tcd->m_appliedImpulse = float(m_appliedImpulse);
|
||||
tcd->m_dbgDrawSize = float(m_dbgDrawSize );
|
||||
tcd->m_appliedImpulse = m_appliedImpulse;
|
||||
tcd->m_dbgDrawSize = m_dbgDrawSize;
|
||||
|
||||
tcd->m_disableCollisionsBetweenLinkedBodies = false;
|
||||
|
||||
|
|
@ -142,7 +142,7 @@ const char* btTypedConstraint::serialize(void* dataBuffer, btSerializer* seriali
|
|||
if (m_rbB.getConstraintRef(i) == this)
|
||||
tcd->m_disableCollisionsBetweenLinkedBodies = true;
|
||||
|
||||
return "btTypedConstraintData";
|
||||
return btTypedConstraintDataName;
|
||||
}
|
||||
|
||||
btRigidBody& btTypedConstraint::getFixedBody()
|
||||
|
|
|
|||
|
|
@ -21,6 +21,15 @@ subject to the following restrictions:
|
|||
#include "btSolverConstraint.h"
|
||||
#include "BulletDynamics/Dynamics/btRigidBody.h"
|
||||
|
||||
#ifdef BT_USE_DOUBLE_PRECISION
|
||||
#define btTypedConstraintData2 btTypedConstraintDoubleData
|
||||
#define btTypedConstraintDataName "btTypedConstraintDoubleData"
|
||||
#else
|
||||
#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
|
||||
|
|
@ -34,6 +43,7 @@ enum btTypedConstraintType
|
|||
CONTACT_CONSTRAINT_TYPE,
|
||||
D6_SPRING_CONSTRAINT_TYPE,
|
||||
GEAR_CONSTRAINT_TYPE,
|
||||
FIXED_CONSTRAINT_TYPE,
|
||||
MAX_CONSTRAINT_TYPE
|
||||
};
|
||||
|
||||
|
|
@ -356,6 +366,33 @@ SIMD_FORCE_INLINE btScalar btAdjustAngleToLimits(btScalar angleInRadians, btScal
|
|||
}
|
||||
|
||||
///do not change those serialization structures, it requires an updated sBulletDNAstr/sBulletDNAstr64
|
||||
struct btTypedConstraintFloatData
|
||||
{
|
||||
btRigidBodyFloatData *m_rbA;
|
||||
btRigidBodyFloatData *m_rbB;
|
||||
char *m_name;
|
||||
|
||||
int m_objectType;
|
||||
int m_userConstraintType;
|
||||
int m_userConstraintId;
|
||||
int m_needsFeedback;
|
||||
|
||||
float m_appliedImpulse;
|
||||
float m_dbgDrawSize;
|
||||
|
||||
int m_disableCollisionsBetweenLinkedBodies;
|
||||
int m_overrideNumSolverIterations;
|
||||
|
||||
float m_breakingImpulseThreshold;
|
||||
int m_isEnabled;
|
||||
|
||||
};
|
||||
|
||||
///do not change those serialization structures, it requires an updated sBulletDNAstr/sBulletDNAstr64
|
||||
|
||||
#define BT_BACKWARDS_COMPATIBLE_SERIALIZATION
|
||||
#ifdef BT_BACKWARDS_COMPATIBLE_SERIALIZATION
|
||||
///this structure is not used, except for loading pre-2.82 .bullet files
|
||||
struct btTypedConstraintData
|
||||
{
|
||||
btRigidBodyData *m_rbA;
|
||||
|
|
@ -377,10 +414,35 @@ struct btTypedConstraintData
|
|||
int m_isEnabled;
|
||||
|
||||
};
|
||||
#endif //BACKWARDS_COMPATIBLE
|
||||
|
||||
struct btTypedConstraintDoubleData
|
||||
{
|
||||
btRigidBodyDoubleData *m_rbA;
|
||||
btRigidBodyDoubleData *m_rbB;
|
||||
char *m_name;
|
||||
|
||||
int m_objectType;
|
||||
int m_userConstraintType;
|
||||
int m_userConstraintId;
|
||||
int m_needsFeedback;
|
||||
|
||||
double m_appliedImpulse;
|
||||
double m_dbgDrawSize;
|
||||
|
||||
int m_disableCollisionsBetweenLinkedBodies;
|
||||
int m_overrideNumSolverIterations;
|
||||
|
||||
double m_breakingImpulseThreshold;
|
||||
int m_isEnabled;
|
||||
char padding[4];
|
||||
|
||||
};
|
||||
|
||||
|
||||
SIMD_FORCE_INLINE int btTypedConstraint::calculateSerializeBufferSize() const
|
||||
{
|
||||
return sizeof(btTypedConstraintData);
|
||||
return sizeof(btTypedConstraintData2);
|
||||
}
|
||||
|
||||
|
||||
|
|
|
|||
Loading…
Add table
Add a link
Reference in a new issue