void b3Generic6DofConstraint::getInfo1 (b3ConstraintInfo1* info,const b3RigidBodyData* bodies)
{
//prepare constraint
calculateTransforms(getCenterOfMassTransform(bodies[m_rbA]),getCenterOfMassTransform(bodies[m_rbB]),bodies);
info->m_numConstraintRows = 0;
info->nub = 6;
int i;
//test linear limits
for(i = 0; i < 3; i++)
{
if(m_linearLimits.needApplyForce(i))
{
info->m_numConstraintRows++;
info->nub--;
}
}
//test angular limits
for (i=0;i<3 ;i++ )
{
if(testAngularLimitMotor(i))
{
info->m_numConstraintRows++;
info->nub--;
}
}
// printf("info->m_numConstraintRows=%d\n",info->m_numConstraintRows);
}
void btGeneric6DofConstraint::getInfo1 (btConstraintInfo1* info)
{
if (m_useSolveConstraintObsolete)
{
info->m_numConstraintRows = 0;
info->nub = 0;
} else
{
//prepare constraint
calculateTransforms(m_rbA.getCenterOfMassTransform(),m_rbB.getCenterOfMassTransform());
info->m_numConstraintRows = 0;
info->nub = 6;
int i;
//test linear limits
for(i = 0; i < 3; i++)
{
if(m_linearLimits.needApplyForce(i))
{
info->m_numConstraintRows++;
info->nub--;
}
}
//test angular limits
for (i=0;i<3 ;i++ )
{
if(testAngularLimitMotor(i))
{
info->m_numConstraintRows++;
info->nub--;
}
}
}
}
void btSliderConstraint::getInfo1(btConstraintInfo1* info)
{
if (m_useSolveConstraintObsolete)
{
info->m_numConstraintRows = 0;
info->nub = 0;
}
else
{
info->m_numConstraintRows = 4; // Fixed 2 linear + 2 angular
info->nub = 2;
//prepare constraint
calculateTransforms();
testLinLimits();
if(getSolveLinLimit() || getPoweredLinMotor())
{
info->m_numConstraintRows++; // limit 3rd linear as well
info->nub--;
}
testAngLimits();
if(getSolveAngLimit() || getPoweredAngMotor())
{
info->m_numConstraintRows++; // limit 3rd angular as well
info->nub--;
}
}
}
void btGeneric6DofSpring2Constraint::getInfo1 (btConstraintInfo1* info)
{
//prepare constraint
calculateTransforms(m_rbA.getCenterOfMassTransform(),m_rbB.getCenterOfMassTransform());
info->m_numConstraintRows = 0;
info->nub = 0;
int i;
//test linear limits
for(i = 0; i < 3; i++)
{
if (m_linearLimits.m_currentLimit[i]==4) info->m_numConstraintRows += 2;
else if (m_linearLimits.m_currentLimit[i]!=0) info->m_numConstraintRows += 1;
if (m_linearLimits.m_enableMotor[i] ) info->m_numConstraintRows += 1;
if (m_linearLimits.m_enableSpring[i]) info->m_numConstraintRows += 1;
}
//test angular limits
for (i=0;i<3 ;i++ )
{
testAngularLimitMotor(i);
if (m_angularLimits[i].m_currentLimit==4) info->m_numConstraintRows += 2;
else if (m_angularLimits[i].m_currentLimit!=0) info->m_numConstraintRows += 1;
if (m_angularLimits[i].m_enableMotor ) info->m_numConstraintRows += 1;
if (m_angularLimits[i].m_enableSpring) info->m_numConstraintRows += 1;
}
}
void btGeneric6DofConstraint::buildJacobian()
{
#ifndef __SPU__
if (m_useSolveConstraintObsolete)
{
// Clear accumulated impulses for the next simulation step
m_linearLimits.m_accumulatedImpulse.setValue(btScalar(0.), btScalar(0.), btScalar(0.));
int i;
for(i = 0; i < 3; i++)
{
m_angularLimits[i].m_accumulatedImpulse = btScalar(0.);
}
//calculates transform
calculateTransforms(m_rbA.getCenterOfMassTransform(),m_rbB.getCenterOfMassTransform());
// const btVector3& pivotAInW = m_calculatedTransformA.getOrigin();
// const btVector3& pivotBInW = m_calculatedTransformB.getOrigin();
calcAnchorPos();
btVector3 pivotAInW = m_AnchorPos;
btVector3 pivotBInW = m_AnchorPos;
// not used here
// btVector3 rel_pos1 = pivotAInW - m_rbA.getCenterOfMassPosition();
// btVector3 rel_pos2 = pivotBInW - m_rbB.getCenterOfMassPosition();
btVector3 normalWorld;
//linear part
for (i=0;i<3;i++)
{
if (m_linearLimits.isLimited(i))
{
if (m_useLinearReferenceFrameA)
normalWorld = m_calculatedTransformA.getBasis().getColumn(i);
else
normalWorld = m_calculatedTransformB.getBasis().getColumn(i);
buildLinearJacobian(
m_jacLinear[i],normalWorld ,
pivotAInW,pivotBInW);
}
}
// angular part
for (i=0;i<3;i++)
{
//calculates error angle
if (testAngularLimitMotor(i))
{
normalWorld = this->getAxis(i);
// Create angular atom
buildAngularJacobian(m_jacAng[i],normalWorld);
}
}
}
#endif //__SPU__
}
void b3Generic6DofConstraint::calculateTransforms(const b3RigidBodyData* bodies)
{
b3Transform transA;
b3Transform transB;
transA = getCenterOfMassTransform(bodies[m_rbA]);
transB = getCenterOfMassTransform(bodies[m_rbB]);
calculateTransforms(transA,transB,bodies);
}
btGeneric6DofSpring2Constraint::btGeneric6DofSpring2Constraint(btRigidBody& rbA, btRigidBody& rbB, const btTransform& frameInA, const btTransform& frameInB, RotateOrder rotOrder)
: btTypedConstraint(D6_SPRING_2_CONSTRAINT_TYPE, rbA, rbB)
, m_frameInA(frameInA)
, m_frameInB(frameInB)
, m_rotateOrder(rotOrder)
, m_flags(0)
{
calculateTransforms();
}
QgsCoordinateTransformPrivate::QgsCoordinateTransformPrivate( const QgsCoordinateReferenceSystem &source,
const QgsCoordinateReferenceSystem &destination,
const QgsCoordinateTransformContext &context )
: mSourceCRS( source )
, mDestCRS( destination )
{
setFinder();
calculateTransforms( context );
}
b3Generic6DofConstraint::b3Generic6DofConstraint(int rbA,int rbB, const b3Transform& frameInA, const b3Transform& frameInB, bool useLinearReferenceFrameA, const b3RigidBodyData* bodies)
: b3TypedConstraint(B3_D6_CONSTRAINT_TYPE, rbA, rbB)
, m_frameInA(frameInA)
, m_frameInB(frameInB),
m_useLinearReferenceFrameA(useLinearReferenceFrameA),
m_useOffsetForConstraintFrame(D6_USE_FRAME_OFFSET),
m_flags(0)
{
calculateTransforms(bodies);
}
btGeneric6DofSpring2Constraint::btGeneric6DofSpring2Constraint(btRigidBody& rbB, const btTransform& frameInB, RotateOrder rotOrder)
: btTypedConstraint(D6_SPRING_2_CONSTRAINT_TYPE, getFixedBody(), rbB)
, m_frameInB(frameInB)
, m_rotateOrder(rotOrder)
, m_flags(0)
{
///not providing rigidbody A means implicitly using worldspace for body A
m_frameInA = rbB.getCenterOfMassTransform() * m_frameInB;
calculateTransforms();
}
bool Generic6DOFJointSW::setup(float p_step) {
// Clear accumulated impulses for the next simulation step
m_linearLimits.m_accumulatedImpulse=Vector3(real_t(0.), real_t(0.), real_t(0.));
int i;
for(i = 0; i < 3; i++)
{
m_angularLimits[i].m_accumulatedImpulse = real_t(0.);
}
//calculates transform
calculateTransforms();
// const Vector3& pivotAInW = m_calculatedTransformA.origin;
// const Vector3& pivotBInW = m_calculatedTransformB.origin;
calcAnchorPos();
Vector3 pivotAInW = m_AnchorPos;
Vector3 pivotBInW = m_AnchorPos;
// not used here
// Vector3 rel_pos1 = pivotAInW - A->get_transform().origin;
// Vector3 rel_pos2 = pivotBInW - B->get_transform().origin;
Vector3 normalWorld;
//linear part
for (i=0;i<3;i++)
{
if (m_linearLimits.enable_limit[i] && m_linearLimits.isLimited(i))
{
if (m_useLinearReferenceFrameA)
normalWorld = m_calculatedTransformA.basis.get_axis(i);
else
normalWorld = m_calculatedTransformB.basis.get_axis(i);
buildLinearJacobian(
m_jacLinear[i],normalWorld ,
pivotAInW,pivotBInW);
}
}
// angular part
for (i=0;i<3;i++)
{
//calculates error angle
if (m_angularLimits[i].m_enableLimit && testAngularLimitMotor(i))
{
normalWorld = this->getAxis(i);
// Create angular atom
buildAngularJacobian(m_jacAng[i],normalWorld);
}
}
return true;
}
btGeneric6DofConstraint::btGeneric6DofConstraint(btRigidBody& rbA, btRigidBody& rbB, const btTransform& frameInA, const btTransform& frameInB, bool useLinearReferenceFrameA)
: btTypedConstraint(D6_CONSTRAINT_TYPE, rbA, rbB)
, m_frameInA(frameInA)
, m_frameInB(frameInB),
m_useLinearReferenceFrameA(useLinearReferenceFrameA),
m_useOffsetForConstraintFrame(D6_USE_FRAME_OFFSET),
m_flags(0),
m_useSolveConstraintObsolete(D6_USE_OBSOLETE_METHOD)
{
calculateTransforms();
}
void btGeneric6DofSpringConstraint::setEquilibriumPoint()
{
calculateTransforms();
for(int i = 0; i < 3; i++)
{
m_equilibriumPoint[i] = m_calculatedLinearDiff[i];
}
for(int i = 0; i < 3; i++)
{
m_equilibriumPoint[i + 3] = m_calculatedAxisAngleDiff[i];
}
}
btGeneric6DofConstraint::btGeneric6DofConstraint(btRigidBody& rbB, const btTransform& frameInB, bool useLinearReferenceFrameB)
: btTypedConstraint(D6_CONSTRAINT_TYPE, getFixedBody(), rbB),
m_frameInB(frameInB),
m_useLinearReferenceFrameA(useLinearReferenceFrameB),
m_useOffsetForConstraintFrame(D6_USE_FRAME_OFFSET),
m_flags(0),
m_useSolveConstraintObsolete(false)
{
///not providing rigidbody A means implicitly using worldspace for body A
m_frameInA = rbB.getCenterOfMassTransform() * m_frameInB;
calculateTransforms();
}
void btGeneric6DofSpringConstraint::setEquilibriumPoint(int index)
{
btAssert((index >= 0) && (index < 6));
calculateTransforms();
if(index < 3)
{
m_equilibriumPoint[index] = m_calculatedLinearDiff[index];
}
else
{
m_equilibriumPoint[index + 3] = m_calculatedAxisAngleDiff[index];
}
}
void btSliderConstraint::initParams()
{
m_lowerLinLimit = btScalar(1.0);
m_upperLinLimit = btScalar(-1.0);
m_lowerAngLimit = btScalar(0.);
m_upperAngLimit = btScalar(0.);
m_softnessDirLin = SLIDER_CONSTRAINT_DEF_SOFTNESS;
m_restitutionDirLin = SLIDER_CONSTRAINT_DEF_RESTITUTION;
m_dampingDirLin = btScalar(0.);
m_cfmDirLin = SLIDER_CONSTRAINT_DEF_CFM;
m_softnessDirAng = SLIDER_CONSTRAINT_DEF_SOFTNESS;
m_restitutionDirAng = SLIDER_CONSTRAINT_DEF_RESTITUTION;
m_dampingDirAng = btScalar(0.);
m_cfmDirAng = SLIDER_CONSTRAINT_DEF_CFM;
m_softnessOrthoLin = SLIDER_CONSTRAINT_DEF_SOFTNESS;
m_restitutionOrthoLin = SLIDER_CONSTRAINT_DEF_RESTITUTION;
m_dampingOrthoLin = SLIDER_CONSTRAINT_DEF_DAMPING;
m_cfmOrthoLin = SLIDER_CONSTRAINT_DEF_CFM;
m_softnessOrthoAng = SLIDER_CONSTRAINT_DEF_SOFTNESS;
m_restitutionOrthoAng = SLIDER_CONSTRAINT_DEF_RESTITUTION;
m_dampingOrthoAng = SLIDER_CONSTRAINT_DEF_DAMPING;
m_cfmOrthoAng = SLIDER_CONSTRAINT_DEF_CFM;
m_softnessLimLin = SLIDER_CONSTRAINT_DEF_SOFTNESS;
m_restitutionLimLin = SLIDER_CONSTRAINT_DEF_RESTITUTION;
m_dampingLimLin = SLIDER_CONSTRAINT_DEF_DAMPING;
m_cfmLimLin = SLIDER_CONSTRAINT_DEF_CFM;
m_softnessLimAng = SLIDER_CONSTRAINT_DEF_SOFTNESS;
m_restitutionLimAng = SLIDER_CONSTRAINT_DEF_RESTITUTION;
m_dampingLimAng = SLIDER_CONSTRAINT_DEF_DAMPING;
m_cfmLimAng = SLIDER_CONSTRAINT_DEF_CFM;
m_poweredLinMotor = false;
m_targetLinMotorVelocity = btScalar(0.);
m_maxLinMotorForce = btScalar(0.);
m_accumulatedLinMotorImpulse = btScalar(0.0);
m_poweredAngMotor = false;
m_targetAngMotorVelocity = btScalar(0.);
m_maxAngMotorForce = btScalar(0.);
m_accumulatedAngMotorImpulse = btScalar(0.0);
m_flags = 0;
m_flags = 0;
m_useOffsetForConstraintFrame = USE_OFFSET_FOR_CONSTANT_FRAME;
calculateTransforms(m_rbA.getCenterOfMassTransform(),m_rbB.getCenterOfMassTransform());
}
void btGeneric6DofSpringConstraint::setAxis(const btVector3& axis1, const btVector3& axis2)
{
btVector3 zAxis = axis1.normalized();
btVector3 yAxis = axis2.normalized();
btVector3 xAxis = yAxis.cross(zAxis); // we want right coordinate system
btTransform frameInW;
frameInW.setIdentity();
frameInW.getBasis().setValue(xAxis[0], yAxis[0], zAxis[0],
xAxis[1], yAxis[1], zAxis[1],
xAxis[2], yAxis[2], zAxis[2]);
// now get constraint frame in local coordinate systems
m_frameInA = m_rbA.getCenterOfMassTransform().inverse() * frameInW;
m_frameInB = m_rbB.getCenterOfMassTransform().inverse() * frameInW;
calculateTransforms();
}
void b3Generic6DofConstraint::getInfo2NonVirtual (b3ConstraintInfo2* info, const b3Transform& transA,const b3Transform& transB,const b3Vector3& linVelA,const b3Vector3& linVelB,const b3Vector3& angVelA,const b3Vector3& angVelB,const b3RigidBodyData* bodies)
{
//prepare constraint
calculateTransforms(transA,transB,bodies);
int i;
for (i=0;i<3 ;i++ )
{
testAngularLimitMotor(i);
}
if(m_useOffsetForConstraintFrame)
{ // for stability better to solve angular limits first
int row = setAngularLimits(info, 0,transA,transB,linVelA,linVelB,angVelA,angVelB);
setLinearLimits(info, row, transA,transB,linVelA,linVelB,angVelA,angVelB);
}
else
{ // leave old version for compatibility
int row = setLinearLimits(info, 0, transA,transB,linVelA,linVelB,angVelA,angVelB);
setAngularLimits(info, row,transA,transB,linVelA,linVelB,angVelA,angVelB);
}
}
void btGeneric6DofConstraint::getInfo2NonVirtual(btConstraintInfo2* info, const btTransform& transA, const btTransform& transB, const btVector3& linVelA, const btVector3& linVelB, const btVector3& angVelA, const btVector3& angVelB)
{
btAssert(!m_useSolveConstraintObsolete);
//prepare constraint
calculateTransforms(transA, transB);
int i;
for (i = 0; i < 3; i++)
{
testAngularLimitMotor(i);
}
if (m_useOffsetForConstraintFrame)
{ // for stability better to solve angular limits first
int row = setAngularLimits(info, 0, transA, transB, linVelA, linVelB, angVelA, angVelB);
setLinearLimits(info, row, transA, transB, linVelA, linVelB, angVelA, angVelB);
}
else
{ // leave old version for compatibility
int row = setLinearLimits(info, 0, transA, transB, linVelA, linVelB, angVelA, angVelB);
setAngularLimits(info, row, transA, transB, linVelA, linVelB, angVelA, angVelB);
}
}
void btGeneric6DofConstraint::calculateTransforms()
{
calculateTransforms(m_rbA.getCenterOfMassTransform(),m_rbB.getCenterOfMassTransform());
}
