add joint/constraint solver support (using CPU, Bullet 2.x solver), in combination with GPU contact solver
This commit is contained in:
@@ -18,7 +18,7 @@ subject to the following restrictions:
|
||||
//#define COMPUTE_IMPULSE_DENOM 1
|
||||
//It is not necessary (redundant) to refresh contact manifolds, this refresh has been moved to the collision algorithms.
|
||||
|
||||
#define DISABLE_JOINTS
|
||||
//#define DISABLE_JOINTS
|
||||
|
||||
#include "b3PgsJacobiSolver.h"
|
||||
#include "Bullet3Common/b3MinMax.h"
|
||||
@@ -154,7 +154,7 @@ b3PgsJacobiSolver::~b3PgsJacobiSolver()
|
||||
{
|
||||
}
|
||||
|
||||
void b3PgsJacobiSolver::solveContacts(int numBodies, b3RigidBodyCL* bodies, btInertiaCL* inertias, int numContacts, b3Contact4* contacts)
|
||||
void b3PgsJacobiSolver::solveContacts(int numBodies, b3RigidBodyCL* bodies, btInertiaCL* inertias, int numContacts, b3Contact4* contacts, int numConstraints, b3TypedConstraint** constraints)
|
||||
{
|
||||
b3ContactSolverInfo infoGlobal;
|
||||
infoGlobal.m_splitImpulse = false;
|
||||
@@ -168,7 +168,7 @@ void b3PgsJacobiSolver::solveContacts(int numBodies, b3RigidBodyCL* bodies, btIn
|
||||
//if ((infoGlobal.m_solverMode & SOLVER_USE_2_FRICTION_DIRECTIONS) && (infoGlobal.m_solverMode & SOLVER_DISABLE_VELOCITY_DEPENDENT_FRICTION_DIRECTION))
|
||||
|
||||
|
||||
solveGroup(bodies,inertias,numBodies,contacts,numContacts,0,0,infoGlobal);
|
||||
solveGroup(bodies,inertias,numBodies,contacts,numContacts,constraints,numConstraints,infoGlobal);
|
||||
|
||||
if (!numContacts)
|
||||
return;
|
||||
@@ -1088,6 +1088,21 @@ b3Scalar b3PgsJacobiSolver::solveGroupCacheFriendlySetup(b3RigidBodyCL* bodies,
|
||||
|
||||
int totalBodies = 0;
|
||||
|
||||
for (int i=0;i<numConstraints;i++)
|
||||
{
|
||||
int bodyIndexA = constraints[i]->getRigidBodyA();
|
||||
int bodyIndexB = constraints[i]->getRigidBodyB();
|
||||
if (m_usePgs)
|
||||
{
|
||||
m_bodyCount[bodyIndexA]=-1;
|
||||
m_bodyCount[bodyIndexB]=-1;
|
||||
} else
|
||||
{
|
||||
//didn't implement joints with Jacobi version yet
|
||||
btAssert(0);
|
||||
}
|
||||
|
||||
}
|
||||
for (int i=0;i<numManifolds;i++)
|
||||
{
|
||||
int bodyIndexA = manifoldPtr[i].getBodyA();
|
||||
@@ -1181,11 +1196,13 @@ b3Scalar b3PgsJacobiSolver::solveGroupCacheFriendlySetup(b3RigidBodyCL* bodies,
|
||||
b3SolverConstraint* currentConstraintRow = &m_tmpSolverNonContactConstraintPool[currentRow];
|
||||
b3TypedConstraint* constraint = constraints[i];
|
||||
|
||||
btRigidBody& rbA = constraint->getRigidBodyA();
|
||||
btRigidBody& rbB = constraint->getRigidBodyB();
|
||||
b3RigidBodyCL& rbA = bodies[ constraint->getRigidBodyA()];
|
||||
//btRigidBody& rbA = constraint->getRigidBodyA();
|
||||
// btRigidBody& rbB = constraint->getRigidBodyB();
|
||||
b3RigidBodyCL& rbB = bodies[ constraint->getRigidBodyB()];
|
||||
|
||||
int solverBodyIdA = getOrInitSolverBody(rbA,);
|
||||
int solverBodyIdB = getOrInitSolverBody(rbB);
|
||||
int solverBodyIdA = getOrInitSolverBody(constraint->getRigidBodyA(),bodies,inertias);
|
||||
int solverBodyIdB = getOrInitSolverBody(constraint->getRigidBodyB(),bodies,inertias);
|
||||
|
||||
b3SolverBody* bodyAPtr = &m_tmpSolverBodyPool[solverBodyIdA];
|
||||
b3SolverBody* bodyBPtr = &m_tmpSolverBodyPool[solverBodyIdB];
|
||||
@@ -1238,7 +1255,7 @@ b3Scalar b3PgsJacobiSolver::solveGroupCacheFriendlySetup(b3RigidBodyCL* bodies,
|
||||
info2.m_lowerLimit = ¤tConstraintRow->m_lowerLimit;
|
||||
info2.m_upperLimit = ¤tConstraintRow->m_upperLimit;
|
||||
info2.m_numIterations = infoGlobal.m_numIterations;
|
||||
constraints[i]->getInfo2(&info2);
|
||||
constraints[i]->getInfo2(&info2,bodies);
|
||||
|
||||
///finalize the constraint setup
|
||||
for ( j=0;j<info1.m_numConstraintRows;j++)
|
||||
@@ -1256,21 +1273,27 @@ b3Scalar b3PgsJacobiSolver::solveGroupCacheFriendlySetup(b3RigidBodyCL* bodies,
|
||||
}
|
||||
|
||||
solverConstraint.m_originalContactPoint = constraint;
|
||||
|
||||
b3Matrix3x3& invInertiaWorldA= inertias[constraint->getRigidBodyA()].m_invInertiaWorld;
|
||||
{
|
||||
|
||||
{
|
||||
//btVector3 angularFactorA(1,1,1);
|
||||
const b3Vector3& ftorqueAxis1 = solverConstraint.m_relpos1CrossNormal;
|
||||
solverConstraint.m_angularComponentA = constraint->getRigidBodyA().getInvInertiaTensorWorld()*ftorqueAxis1*constraint->getRigidBodyA().getAngularFactor();
|
||||
solverConstraint.m_angularComponentA = invInertiaWorldA*ftorqueAxis1;//*angularFactorA;
|
||||
}
|
||||
|
||||
b3Matrix3x3& invInertiaWorldB= inertias[constraint->getRigidBodyB()].m_invInertiaWorld;
|
||||
{
|
||||
|
||||
const b3Vector3& ftorqueAxis2 = solverConstraint.m_relpos2CrossNormal;
|
||||
solverConstraint.m_angularComponentB = constraint->getRigidBodyB().getInvInertiaTensorWorld()*ftorqueAxis2*constraint->getRigidBodyB().getAngularFactor();
|
||||
solverConstraint.m_angularComponentB = invInertiaWorldB*ftorqueAxis2;//*constraint->getRigidBodyB().getAngularFactor();
|
||||
}
|
||||
|
||||
{
|
||||
b3Vector3 iMJlA = solverConstraint.m_contactNormal*rbA.getInvMass();
|
||||
b3Vector3 iMJaA = rbA.getInvInertiaTensorWorld()*solverConstraint.m_relpos1CrossNormal;
|
||||
b3Vector3 iMJaA = invInertiaWorldA*solverConstraint.m_relpos1CrossNormal;
|
||||
b3Vector3 iMJlB = solverConstraint.m_contactNormal*rbB.getInvMass();//sign of normal?
|
||||
b3Vector3 iMJaB = rbB.getInvInertiaTensorWorld()*solverConstraint.m_relpos2CrossNormal;
|
||||
b3Vector3 iMJaB = invInertiaWorldB*solverConstraint.m_relpos2CrossNormal;
|
||||
|
||||
b3Scalar sum = iMJlA.dot(solverConstraint.m_contactNormal);
|
||||
sum += iMJaA.dot(solverConstraint.m_relpos1CrossNormal);
|
||||
@@ -1286,8 +1309,8 @@ b3Scalar b3PgsJacobiSolver::solveGroupCacheFriendlySetup(b3RigidBodyCL* bodies,
|
||||
///todo: add force/torque accelerators
|
||||
{
|
||||
b3Scalar rel_vel;
|
||||
b3Scalar vel1Dotn = solverConstraint.m_contactNormal.dot(rbA.getLinearVelocity()) + solverConstraint.m_relpos1CrossNormal.dot(rbA.getAngularVelocity());
|
||||
b3Scalar vel2Dotn = -solverConstraint.m_contactNormal.dot(rbB.getLinearVelocity()) + solverConstraint.m_relpos2CrossNormal.dot(rbB.getAngularVelocity());
|
||||
b3Scalar vel1Dotn = solverConstraint.m_contactNormal.dot(rbA.m_linVel) + solverConstraint.m_relpos1CrossNormal.dot(rbA.m_angVel);
|
||||
b3Scalar vel2Dotn = -solverConstraint.m_contactNormal.dot(rbB.m_linVel) + solverConstraint.m_relpos2CrossNormal.dot(rbB.m_angVel);
|
||||
|
||||
rel_vel = vel1Dotn+vel2Dotn;
|
||||
|
||||
@@ -1406,20 +1429,6 @@ b3Scalar b3PgsJacobiSolver::solveSingleIteration(int iteration,b3TypedConstraint
|
||||
|
||||
if (iteration< infoGlobal.m_numIterations)
|
||||
{
|
||||
#ifndef DISABLE_JOINTS
|
||||
for (int j=0;j<numConstraints;j++)
|
||||
{
|
||||
if (constraints[j]->isEnabled())
|
||||
{
|
||||
int bodyAid = getOrInitSolverBody(constraints[j]->getRigidBodyA());
|
||||
int bodyBid = getOrInitSolverBody(constraints[j]->getRigidBodyB());
|
||||
b3SolverBody& bodyA = m_tmpSolverBodyPool[bodyAid];
|
||||
b3SolverBody& bodyB = m_tmpSolverBodyPool[bodyBid];
|
||||
constraints[j]->solveConstraintObsolete(bodyA,bodyB,infoGlobal.m_timeStep);
|
||||
}
|
||||
}
|
||||
#endif
|
||||
|
||||
|
||||
///solve all contact constraints using SIMD, if available
|
||||
if (infoGlobal.m_solverMode & SOLVER_INTERLEAVE_CONTACT_AND_FRICTION_CONSTRAINTS)
|
||||
@@ -1539,19 +1548,6 @@ b3Scalar b3PgsJacobiSolver::solveSingleIteration(int iteration,b3TypedConstraint
|
||||
|
||||
if (iteration< infoGlobal.m_numIterations)
|
||||
{
|
||||
#ifndef DISABLE_JOINTS
|
||||
for (int j=0;j<numConstraints;j++)
|
||||
{
|
||||
if (constraints[j]->isEnabled())
|
||||
{
|
||||
int bodyAid = getOrInitSolverBody(constraints[j]->getRigidBodyA());
|
||||
int bodyBid = getOrInitSolverBody(constraints[j]->getRigidBodyB());
|
||||
b3SolverBody& bodyA = m_tmpSolverBodyPool[bodyAid];
|
||||
b3SolverBody& bodyB = m_tmpSolverBodyPool[bodyBid];
|
||||
constraints[j]->solveConstraintObsolete(bodyA,bodyB,infoGlobal.m_timeStep);
|
||||
}
|
||||
}
|
||||
#endif //DISABLE_JOINTS
|
||||
|
||||
///solve all contact constraints
|
||||
int numPoolConstraints = m_tmpSolverContactConstraintPool.size();
|
||||
|
||||
@@ -116,8 +116,9 @@ public:
|
||||
b3PgsJacobiSolver();
|
||||
virtual ~b3PgsJacobiSolver();
|
||||
|
||||
void solveContacts(int numBodies, b3RigidBodyCL* bodies, btInertiaCL* inertias, int numContacts, b3Contact4* contacts);
|
||||
|
||||
// void solveContacts(int numBodies, b3RigidBodyCL* bodies, btInertiaCL* inertias, int numContacts, b3Contact4* contacts);
|
||||
void solveContacts(int numBodies, b3RigidBodyCL* bodies, btInertiaCL* inertias, int numContacts, b3Contact4* contacts, int numConstraints, b3TypedConstraint** constraints);
|
||||
|
||||
b3Scalar solveGroup(b3RigidBodyCL* bodies,btInertiaCL* inertias,int numBodies,b3Contact4* manifoldPtr, int numManifolds,b3TypedConstraint** constraints,int numConstraints,const b3ContactSolverInfo& infoGlobal);
|
||||
|
||||
///clear internal cached data and reset random seed
|
||||
|
||||
225
src/Bullet3Dynamics/ConstraintSolver/b3Point2PointConstraint.cpp
Normal file
225
src/Bullet3Dynamics/ConstraintSolver/b3Point2PointConstraint.cpp
Normal file
@@ -0,0 +1,225 @@
|
||||
/*
|
||||
Bullet Continuous Collision Detection and Physics Library
|
||||
Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/
|
||||
|
||||
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 "b3Point2PointConstraint.h"
|
||||
#include "Bullet3Collision/NarrowPhaseCollision/b3RigidBodyCL.h"
|
||||
|
||||
#include <new>
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
b3Point2PointConstraint::b3Point2PointConstraint(int rbA,int rbB, const b3Vector3& pivotInA,const b3Vector3& pivotInB)
|
||||
:b3TypedConstraint(POINT2POINT_CONSTRAINT_TYPE,rbA,rbB),m_pivotInA(pivotInA),m_pivotInB(pivotInB),
|
||||
m_flags(0),
|
||||
m_useSolveConstraintObsolete(false)
|
||||
{
|
||||
|
||||
}
|
||||
|
||||
/*
|
||||
b3Point2PointConstraint::b3Point2PointConstraint(int rbA,const b3Vector3& pivotInA)
|
||||
:b3TypedConstraint(POINT2POINT_CONSTRAINT_TYPE,rbA),m_pivotInA(pivotInA),m_pivotInB(rbA.getCenterOfMassTransform()(pivotInA)),
|
||||
m_flags(0),
|
||||
m_useSolveConstraintObsolete(false)
|
||||
{
|
||||
|
||||
}
|
||||
*/
|
||||
|
||||
void b3Point2PointConstraint::buildJacobian()
|
||||
{
|
||||
|
||||
///we need it for both methods
|
||||
{
|
||||
m_appliedImpulse = b3Scalar(0.);
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
void b3Point2PointConstraint::getInfo1 (btConstraintInfo1* info)
|
||||
{
|
||||
getInfo1NonVirtual(info);
|
||||
}
|
||||
|
||||
void b3Point2PointConstraint::getInfo1NonVirtual (btConstraintInfo1* info)
|
||||
{
|
||||
if (m_useSolveConstraintObsolete)
|
||||
{
|
||||
info->m_numConstraintRows = 0;
|
||||
info->nub = 0;
|
||||
} else
|
||||
{
|
||||
info->m_numConstraintRows = 3;
|
||||
info->nub = 3;
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
|
||||
|
||||
void b3Point2PointConstraint::getInfo2 (btConstraintInfo2* info, const b3RigidBodyCL* bodies)
|
||||
{
|
||||
b3Transform trA;
|
||||
trA.setIdentity();
|
||||
trA.setOrigin(bodies[m_rbA].m_pos);
|
||||
trA.setRotation(bodies[m_rbB].m_quat);
|
||||
|
||||
b3Transform trB;
|
||||
trB.setIdentity();
|
||||
trB.setOrigin(bodies[m_rbB].m_pos);
|
||||
trB.setRotation(bodies[m_rbB].m_quat);
|
||||
|
||||
getInfo2NonVirtual(info, trA,trB);
|
||||
}
|
||||
|
||||
void b3Point2PointConstraint::getInfo2NonVirtual (btConstraintInfo2* info, const b3Transform& body0_trans, const b3Transform& body1_trans)
|
||||
{
|
||||
btAssert(!m_useSolveConstraintObsolete);
|
||||
|
||||
//retrieve matrices
|
||||
|
||||
// anchor points in global coordinates with respect to body PORs.
|
||||
|
||||
// set jacobian
|
||||
info->m_J1linearAxis[0] = 1;
|
||||
info->m_J1linearAxis[info->rowskip+1] = 1;
|
||||
info->m_J1linearAxis[2*info->rowskip+2] = 1;
|
||||
|
||||
b3Vector3 a1 = body0_trans.getBasis()*getPivotInA();
|
||||
{
|
||||
b3Vector3* angular0 = (b3Vector3*)(info->m_J1angularAxis);
|
||||
b3Vector3* angular1 = (b3Vector3*)(info->m_J1angularAxis+info->rowskip);
|
||||
b3Vector3* angular2 = (b3Vector3*)(info->m_J1angularAxis+2*info->rowskip);
|
||||
b3Vector3 a1neg = -a1;
|
||||
a1neg.getSkewSymmetricMatrix(angular0,angular1,angular2);
|
||||
}
|
||||
|
||||
if (info->m_J2linearAxis)
|
||||
{
|
||||
info->m_J2linearAxis[0] = -1;
|
||||
info->m_J2linearAxis[info->rowskip+1] = -1;
|
||||
info->m_J2linearAxis[2*info->rowskip+2] = -1;
|
||||
}
|
||||
|
||||
b3Vector3 a2 = body1_trans.getBasis()*getPivotInB();
|
||||
|
||||
{
|
||||
// b3Vector3 a2n = -a2;
|
||||
b3Vector3* angular0 = (b3Vector3*)(info->m_J2angularAxis);
|
||||
b3Vector3* angular1 = (b3Vector3*)(info->m_J2angularAxis+info->rowskip);
|
||||
b3Vector3* angular2 = (b3Vector3*)(info->m_J2angularAxis+2*info->rowskip);
|
||||
a2.getSkewSymmetricMatrix(angular0,angular1,angular2);
|
||||
}
|
||||
|
||||
|
||||
|
||||
// set right hand side
|
||||
b3Scalar currERP = (m_flags & BT_P2P_FLAGS_ERP) ? m_erp : info->erp;
|
||||
b3Scalar k = info->fps * currERP;
|
||||
int j;
|
||||
for (j=0; j<3; j++)
|
||||
{
|
||||
info->m_constraintError[j*info->rowskip] = k * (a2[j] + body1_trans.getOrigin()[j] - a1[j] - body0_trans.getOrigin()[j]);
|
||||
//printf("info->m_constraintError[%d]=%f\n",j,info->m_constraintError[j]);
|
||||
}
|
||||
if(m_flags & BT_P2P_FLAGS_CFM)
|
||||
{
|
||||
for (j=0; j<3; j++)
|
||||
{
|
||||
info->cfm[j*info->rowskip] = m_cfm;
|
||||
}
|
||||
}
|
||||
|
||||
b3Scalar impulseClamp = m_setting.m_impulseClamp;//
|
||||
for (j=0; j<3; j++)
|
||||
{
|
||||
if (m_setting.m_impulseClamp > 0)
|
||||
{
|
||||
info->m_lowerLimit[j*info->rowskip] = -impulseClamp;
|
||||
info->m_upperLimit[j*info->rowskip] = impulseClamp;
|
||||
}
|
||||
}
|
||||
info->m_damping = m_setting.m_damping;
|
||||
|
||||
}
|
||||
|
||||
|
||||
|
||||
void b3Point2PointConstraint::updateRHS(b3Scalar timeStep)
|
||||
{
|
||||
(void)timeStep;
|
||||
|
||||
}
|
||||
|
||||
///override the default global value of a parameter (such as ERP or CFM), optionally provide the axis (0..5).
|
||||
///If no axis is provided, it uses the default axis for this constraint.
|
||||
void b3Point2PointConstraint::setParam(int num, b3Scalar value, int axis)
|
||||
{
|
||||
if(axis != -1)
|
||||
{
|
||||
btAssertConstrParams(0);
|
||||
}
|
||||
else
|
||||
{
|
||||
switch(num)
|
||||
{
|
||||
case BT_CONSTRAINT_ERP :
|
||||
case BT_CONSTRAINT_STOP_ERP :
|
||||
m_erp = value;
|
||||
m_flags |= BT_P2P_FLAGS_ERP;
|
||||
break;
|
||||
case BT_CONSTRAINT_CFM :
|
||||
case BT_CONSTRAINT_STOP_CFM :
|
||||
m_cfm = value;
|
||||
m_flags |= BT_P2P_FLAGS_CFM;
|
||||
break;
|
||||
default:
|
||||
btAssertConstrParams(0);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
///return the local value of parameter
|
||||
b3Scalar b3Point2PointConstraint::getParam(int num, int axis) const
|
||||
{
|
||||
b3Scalar retVal(SIMD_INFINITY);
|
||||
if(axis != -1)
|
||||
{
|
||||
btAssertConstrParams(0);
|
||||
}
|
||||
else
|
||||
{
|
||||
switch(num)
|
||||
{
|
||||
case BT_CONSTRAINT_ERP :
|
||||
case BT_CONSTRAINT_STOP_ERP :
|
||||
btAssertConstrParams(m_flags & BT_P2P_FLAGS_ERP);
|
||||
retVal = m_erp;
|
||||
break;
|
||||
case BT_CONSTRAINT_CFM :
|
||||
case BT_CONSTRAINT_STOP_CFM :
|
||||
btAssertConstrParams(m_flags & BT_P2P_FLAGS_CFM);
|
||||
retVal = m_cfm;
|
||||
break;
|
||||
default:
|
||||
btAssertConstrParams(0);
|
||||
}
|
||||
}
|
||||
return retVal;
|
||||
}
|
||||
|
||||
163
src/Bullet3Dynamics/ConstraintSolver/b3Point2PointConstraint.h
Normal file
163
src/Bullet3Dynamics/ConstraintSolver/b3Point2PointConstraint.h
Normal file
@@ -0,0 +1,163 @@
|
||||
/*
|
||||
Bullet Continuous Collision Detection and Physics Library
|
||||
Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/
|
||||
|
||||
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_POINT2POINTCONSTRAINT_H
|
||||
#define BT_POINT2POINTCONSTRAINT_H
|
||||
|
||||
#include "Bullet3Common/b3Vector3.h"
|
||||
//#include "b3JacobianEntry.h"
|
||||
#include "b3TypedConstraint.h"
|
||||
|
||||
class btRigidBody;
|
||||
|
||||
|
||||
#ifdef BT_USE_DOUBLE_PRECISION
|
||||
#define btPoint2PointConstraintData btPoint2PointConstraintDoubleData
|
||||
#define btPoint2PointConstraintDataName "btPoint2PointConstraintDoubleData"
|
||||
#else
|
||||
#define btPoint2PointConstraintData btPoint2PointConstraintFloatData
|
||||
#define btPoint2PointConstraintDataName "btPoint2PointConstraintFloatData"
|
||||
#endif //BT_USE_DOUBLE_PRECISION
|
||||
|
||||
struct btConstraintSetting
|
||||
{
|
||||
btConstraintSetting() :
|
||||
m_tau(b3Scalar(0.3)),
|
||||
m_damping(b3Scalar(1.)),
|
||||
m_impulseClamp(b3Scalar(0.))
|
||||
{
|
||||
}
|
||||
b3Scalar m_tau;
|
||||
b3Scalar m_damping;
|
||||
b3Scalar m_impulseClamp;
|
||||
};
|
||||
|
||||
enum btPoint2PointFlags
|
||||
{
|
||||
BT_P2P_FLAGS_ERP = 1,
|
||||
BT_P2P_FLAGS_CFM = 2
|
||||
};
|
||||
|
||||
/// point to point constraint between two rigidbodies each with a pivotpoint that descibes the 'ballsocket' location in local space
|
||||
ATTRIBUTE_ALIGNED16(class) b3Point2PointConstraint : public b3TypedConstraint
|
||||
{
|
||||
#ifdef IN_PARALLELL_SOLVER
|
||||
public:
|
||||
#endif
|
||||
|
||||
b3Vector3 m_pivotInA;
|
||||
b3Vector3 m_pivotInB;
|
||||
|
||||
int m_flags;
|
||||
b3Scalar m_erp;
|
||||
b3Scalar m_cfm;
|
||||
|
||||
public:
|
||||
|
||||
BT_DECLARE_ALIGNED_ALLOCATOR();
|
||||
|
||||
///for backwards compatibility during the transition to 'getInfo/getInfo2'
|
||||
bool m_useSolveConstraintObsolete;
|
||||
|
||||
btConstraintSetting m_setting;
|
||||
|
||||
b3Point2PointConstraint(int rbA,int rbB, const b3Vector3& pivotInA,const b3Vector3& pivotInB);
|
||||
|
||||
b3Point2PointConstraint(int rbA,const b3Vector3& pivotInA);
|
||||
|
||||
|
||||
virtual void buildJacobian();
|
||||
|
||||
virtual void getInfo1 (btConstraintInfo1* info);
|
||||
|
||||
void getInfo1NonVirtual (btConstraintInfo1* info);
|
||||
|
||||
virtual void getInfo2 (btConstraintInfo2* info, const b3RigidBodyCL* bodies);
|
||||
|
||||
void getInfo2NonVirtual (btConstraintInfo2* info, const b3Transform& body0_trans, const b3Transform& body1_trans);
|
||||
|
||||
void updateRHS(b3Scalar timeStep);
|
||||
|
||||
void setPivotA(const b3Vector3& pivotA)
|
||||
{
|
||||
m_pivotInA = pivotA;
|
||||
}
|
||||
|
||||
void setPivotB(const b3Vector3& pivotB)
|
||||
{
|
||||
m_pivotInB = pivotB;
|
||||
}
|
||||
|
||||
const b3Vector3& getPivotInA() const
|
||||
{
|
||||
return m_pivotInA;
|
||||
}
|
||||
|
||||
const b3Vector3& getPivotInB() const
|
||||
{
|
||||
return m_pivotInB;
|
||||
}
|
||||
|
||||
///override the default global value of a parameter (such as ERP or CFM), optionally provide the axis (0..5).
|
||||
///If no axis is provided, it uses the default axis for this constraint.
|
||||
virtual void setParam(int num, b3Scalar value, int axis = -1);
|
||||
///return the local value of parameter
|
||||
virtual b3Scalar getParam(int num, int axis = -1) const;
|
||||
|
||||
// virtual int calculateSerializeBufferSize() const;
|
||||
|
||||
///fills the dataBuffer and returns the struct name (and 0 on failure)
|
||||
// virtual const char* serialize(void* dataBuffer, btSerializer* serializer) const;
|
||||
|
||||
|
||||
};
|
||||
|
||||
///do not change those serialization structures, it requires an updated sBulletDNAstr/sBulletDNAstr64
|
||||
struct btPoint2PointConstraintFloatData
|
||||
{
|
||||
btTypedConstraintData m_typeConstraintData;
|
||||
btVector3FloatData m_pivotInA;
|
||||
btVector3FloatData m_pivotInB;
|
||||
};
|
||||
|
||||
///do not change those serialization structures, it requires an updated sBulletDNAstr/sBulletDNAstr64
|
||||
struct btPoint2PointConstraintDoubleData
|
||||
{
|
||||
btTypedConstraintData m_typeConstraintData;
|
||||
btVector3DoubleData m_pivotInA;
|
||||
btVector3DoubleData m_pivotInB;
|
||||
};
|
||||
|
||||
/*
|
||||
SIMD_FORCE_INLINE int b3Point2PointConstraint::calculateSerializeBufferSize() const
|
||||
{
|
||||
return sizeof(btPoint2PointConstraintData);
|
||||
|
||||
}
|
||||
|
||||
///fills the dataBuffer and returns the struct name (and 0 on failure)
|
||||
SIMD_FORCE_INLINE const char* b3Point2PointConstraint::serialize(void* dataBuffer, btSerializer* serializer) const
|
||||
{
|
||||
btPoint2PointConstraintData* p2pData = (btPoint2PointConstraintData*)dataBuffer;
|
||||
|
||||
b3TypedConstraint::serialize(&p2pData->m_typeConstraintData,serializer);
|
||||
m_pivotInA.serialize(p2pData->m_pivotInA);
|
||||
m_pivotInB.serialize(p2pData->m_pivotInB);
|
||||
|
||||
return btPoint2PointConstraintDataName;
|
||||
}
|
||||
*/
|
||||
|
||||
#endif //BT_POINT2POINTCONSTRAINT_H
|
||||
@@ -259,7 +259,7 @@ ATTRIBUTE_ALIGNED64 (struct) b3SolverBody
|
||||
|
||||
void writebackVelocity()
|
||||
{
|
||||
if (m_originalBody)
|
||||
//if (m_originalBody>=0)
|
||||
{
|
||||
m_linearVelocity +=m_deltaLinearVelocity;
|
||||
m_angularVelocity += m_deltaAngularVelocity;
|
||||
|
||||
@@ -60,6 +60,8 @@ ATTRIBUTE_ALIGNED16(struct) btJointFeedback
|
||||
b3Vector3 m_appliedTorqueBodyB;
|
||||
};
|
||||
|
||||
struct b3RigidBodyCL;
|
||||
|
||||
|
||||
///TypedConstraint is the baseclass for Bullet constraints and vehicles
|
||||
ATTRIBUTE_ALIGNED16(class) b3TypedConstraint : public btTypedObject
|
||||
@@ -171,7 +173,7 @@ public:
|
||||
virtual void getInfo1 (btConstraintInfo1* info)=0;
|
||||
|
||||
///internal method used by the constraint solver, don't use them directly
|
||||
virtual void getInfo2 (btConstraintInfo2* info)=0;
|
||||
virtual void getInfo2 (btConstraintInfo2* info, const b3RigidBodyCL* bodies)=0;
|
||||
|
||||
///internal method used by the constraint solver, don't use them directly
|
||||
void internalSetAppliedImpulse(b3Scalar appliedImpulse)
|
||||
@@ -321,10 +323,10 @@ public:
|
||||
///return the local value of parameter
|
||||
virtual b3Scalar getParam(int num, int axis = -1) const = 0;
|
||||
|
||||
virtual int calculateSerializeBufferSize() const;
|
||||
// virtual int calculateSerializeBufferSize() const;
|
||||
|
||||
///fills the dataBuffer and returns the struct name (and 0 on failure)
|
||||
virtual const char* serialize(void* dataBuffer, btSerializer* serializer) const;
|
||||
//virtual const char* serialize(void* dataBuffer, btSerializer* serializer) const;
|
||||
|
||||
};
|
||||
|
||||
@@ -377,11 +379,11 @@ struct btTypedConstraintData
|
||||
|
||||
};
|
||||
|
||||
SIMD_FORCE_INLINE int b3TypedConstraint::calculateSerializeBufferSize() const
|
||||
/*SIMD_FORCE_INLINE int b3TypedConstraint::calculateSerializeBufferSize() const
|
||||
{
|
||||
return sizeof(btTypedConstraintData);
|
||||
}
|
||||
|
||||
*/
|
||||
|
||||
|
||||
class btAngularLimit
|
||||
|
||||
Reference in New Issue
Block a user