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