summaryrefslogtreecommitdiff
path: root/thirdparty/bullet/BulletDynamics/ConstraintSolver/btGeneric6DofConstraint.cpp
diff options
context:
space:
mode:
Diffstat (limited to 'thirdparty/bullet/BulletDynamics/ConstraintSolver/btGeneric6DofConstraint.cpp')
-rw-r--r--thirdparty/bullet/BulletDynamics/ConstraintSolver/btGeneric6DofConstraint.cpp1063
1 files changed, 1063 insertions, 0 deletions
diff --git a/thirdparty/bullet/BulletDynamics/ConstraintSolver/btGeneric6DofConstraint.cpp b/thirdparty/bullet/BulletDynamics/ConstraintSolver/btGeneric6DofConstraint.cpp
new file mode 100644
index 0000000000..fa17254ec3
--- /dev/null
+++ b/thirdparty/bullet/BulletDynamics/ConstraintSolver/btGeneric6DofConstraint.cpp
@@ -0,0 +1,1063 @@
+/*
+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.
+*/
+/*
+2007-09-09
+Refactored by Francisco Le?n
+email: projectileman@yahoo.com
+http://gimpact.sf.net
+*/
+
+#include "btGeneric6DofConstraint.h"
+#include "BulletDynamics/Dynamics/btRigidBody.h"
+#include "LinearMath/btTransformUtil.h"
+#include "LinearMath/btTransformUtil.h"
+#include <new>
+
+
+
+#define D6_USE_OBSOLETE_METHOD false
+#define D6_USE_FRAME_OFFSET 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();
+}
+
+
+
+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();
+}
+
+
+
+
+#define GENERIC_D6_DISABLE_WARMSTARTING 1
+
+
+
+btScalar btGetMatrixElem(const btMatrix3x3& mat, int index);
+btScalar btGetMatrixElem(const btMatrix3x3& mat, int index)
+{
+ int i = index%3;
+ int j = index/3;
+ return mat[i][j];
+}
+
+
+
+///MatrixToEulerXYZ from http://www.geometrictools.com/LibFoundation/Mathematics/Wm4Matrix3.inl.html
+bool matrixToEulerXYZ(const btMatrix3x3& mat,btVector3& xyz);
+bool matrixToEulerXYZ(const btMatrix3x3& mat,btVector3& xyz)
+{
+ // // rot = cy*cz -cy*sz sy
+ // // cz*sx*sy+cx*sz cx*cz-sx*sy*sz -cy*sx
+ // // -cx*cz*sy+sx*sz cz*sx+cx*sy*sz cx*cy
+ //
+
+ btScalar fi = btGetMatrixElem(mat,2);
+ if (fi < btScalar(1.0f))
+ {
+ if (fi > btScalar(-1.0f))
+ {
+ xyz[0] = btAtan2(-btGetMatrixElem(mat,5),btGetMatrixElem(mat,8));
+ xyz[1] = btAsin(btGetMatrixElem(mat,2));
+ xyz[2] = btAtan2(-btGetMatrixElem(mat,1),btGetMatrixElem(mat,0));
+ return true;
+ }
+ else
+ {
+ // WARNING. Not unique. XA - ZA = -atan2(r10,r11)
+ xyz[0] = -btAtan2(btGetMatrixElem(mat,3),btGetMatrixElem(mat,4));
+ xyz[1] = -SIMD_HALF_PI;
+ xyz[2] = btScalar(0.0);
+ return false;
+ }
+ }
+ else
+ {
+ // WARNING. Not unique. XAngle + ZAngle = atan2(r10,r11)
+ xyz[0] = btAtan2(btGetMatrixElem(mat,3),btGetMatrixElem(mat,4));
+ xyz[1] = SIMD_HALF_PI;
+ xyz[2] = 0.0;
+ }
+ return false;
+}
+
+//////////////////////////// btRotationalLimitMotor ////////////////////////////////////
+
+int btRotationalLimitMotor::testLimitValue(btScalar test_value)
+{
+ if(m_loLimit>m_hiLimit)
+ {
+ m_currentLimit = 0;//Free from violation
+ return 0;
+ }
+ if (test_value < m_loLimit)
+ {
+ m_currentLimit = 1;//low limit violation
+ m_currentLimitError = test_value - m_loLimit;
+ if(m_currentLimitError>SIMD_PI)
+ m_currentLimitError-=SIMD_2_PI;
+ else if(m_currentLimitError<-SIMD_PI)
+ m_currentLimitError+=SIMD_2_PI;
+ return 1;
+ }
+ else if (test_value> m_hiLimit)
+ {
+ m_currentLimit = 2;//High limit violation
+ m_currentLimitError = test_value - m_hiLimit;
+ if(m_currentLimitError>SIMD_PI)
+ m_currentLimitError-=SIMD_2_PI;
+ else if(m_currentLimitError<-SIMD_PI)
+ m_currentLimitError+=SIMD_2_PI;
+ return 2;
+ };
+
+ m_currentLimit = 0;//Free from violation
+ return 0;
+
+}
+
+
+
+btScalar btRotationalLimitMotor::solveAngularLimits(
+ btScalar timeStep,btVector3& axis,btScalar jacDiagABInv,
+ btRigidBody * body0, btRigidBody * body1 )
+{
+ if (needApplyTorques()==false) return 0.0f;
+
+ btScalar target_velocity = m_targetVelocity;
+ btScalar maxMotorForce = m_maxMotorForce;
+
+ //current error correction
+ if (m_currentLimit!=0)
+ {
+ target_velocity = -m_stopERP*m_currentLimitError/(timeStep);
+ maxMotorForce = m_maxLimitForce;
+ }
+
+ maxMotorForce *= timeStep;
+
+ // current velocity difference
+
+ btVector3 angVelA = body0->getAngularVelocity();
+ btVector3 angVelB = body1->getAngularVelocity();
+
+ btVector3 vel_diff;
+ vel_diff = angVelA-angVelB;
+
+
+
+ btScalar rel_vel = axis.dot(vel_diff);
+
+ // correction velocity
+ btScalar motor_relvel = m_limitSoftness*(target_velocity - m_damping*rel_vel);
+
+
+ if ( motor_relvel < SIMD_EPSILON && motor_relvel > -SIMD_EPSILON )
+ {
+ return 0.0f;//no need for applying force
+ }
+
+
+ // correction impulse
+ btScalar unclippedMotorImpulse = (1+m_bounce)*motor_relvel*jacDiagABInv;
+
+ // clip correction impulse
+ btScalar clippedMotorImpulse;
+
+ ///@todo: should clip against accumulated impulse
+ if (unclippedMotorImpulse>0.0f)
+ {
+ clippedMotorImpulse = unclippedMotorImpulse > maxMotorForce? maxMotorForce: unclippedMotorImpulse;
+ }
+ else
+ {
+ clippedMotorImpulse = unclippedMotorImpulse < -maxMotorForce ? -maxMotorForce: unclippedMotorImpulse;
+ }
+
+
+ // sort with accumulated impulses
+ btScalar lo = btScalar(-BT_LARGE_FLOAT);
+ btScalar hi = btScalar(BT_LARGE_FLOAT);
+
+ btScalar oldaccumImpulse = m_accumulatedImpulse;
+ btScalar sum = oldaccumImpulse + clippedMotorImpulse;
+ m_accumulatedImpulse = sum > hi ? btScalar(0.) : sum < lo ? btScalar(0.) : sum;
+
+ clippedMotorImpulse = m_accumulatedImpulse - oldaccumImpulse;
+
+ btVector3 motorImp = clippedMotorImpulse * axis;
+
+ body0->applyTorqueImpulse(motorImp);
+ body1->applyTorqueImpulse(-motorImp);
+
+ return clippedMotorImpulse;
+
+
+}
+
+//////////////////////////// End btRotationalLimitMotor ////////////////////////////////////
+
+
+
+
+//////////////////////////// btTranslationalLimitMotor ////////////////////////////////////
+
+
+int btTranslationalLimitMotor::testLimitValue(int limitIndex, btScalar test_value)
+{
+ btScalar loLimit = m_lowerLimit[limitIndex];
+ btScalar hiLimit = m_upperLimit[limitIndex];
+ if(loLimit > hiLimit)
+ {
+ m_currentLimit[limitIndex] = 0;//Free from violation
+ m_currentLimitError[limitIndex] = btScalar(0.f);
+ return 0;
+ }
+
+ if (test_value < loLimit)
+ {
+ m_currentLimit[limitIndex] = 2;//low limit violation
+ m_currentLimitError[limitIndex] = test_value - loLimit;
+ return 2;
+ }
+ else if (test_value> hiLimit)
+ {
+ m_currentLimit[limitIndex] = 1;//High limit violation
+ m_currentLimitError[limitIndex] = test_value - hiLimit;
+ return 1;
+ };
+
+ m_currentLimit[limitIndex] = 0;//Free from violation
+ m_currentLimitError[limitIndex] = btScalar(0.f);
+ return 0;
+}
+
+
+
+btScalar btTranslationalLimitMotor::solveLinearAxis(
+ btScalar timeStep,
+ btScalar jacDiagABInv,
+ btRigidBody& body1,const btVector3 &pointInA,
+ btRigidBody& body2,const btVector3 &pointInB,
+ int limit_index,
+ const btVector3 & axis_normal_on_a,
+ const btVector3 & anchorPos)
+{
+
+ ///find relative velocity
+ // btVector3 rel_pos1 = pointInA - body1.getCenterOfMassPosition();
+ // btVector3 rel_pos2 = pointInB - body2.getCenterOfMassPosition();
+ btVector3 rel_pos1 = anchorPos - body1.getCenterOfMassPosition();
+ btVector3 rel_pos2 = anchorPos - body2.getCenterOfMassPosition();
+
+ btVector3 vel1 = body1.getVelocityInLocalPoint(rel_pos1);
+ btVector3 vel2 = body2.getVelocityInLocalPoint(rel_pos2);
+ btVector3 vel = vel1 - vel2;
+
+ btScalar rel_vel = axis_normal_on_a.dot(vel);
+
+
+
+ /// apply displacement correction
+
+ //positional error (zeroth order error)
+ btScalar depth = -(pointInA - pointInB).dot(axis_normal_on_a);
+ btScalar lo = btScalar(-BT_LARGE_FLOAT);
+ btScalar hi = btScalar(BT_LARGE_FLOAT);
+
+ btScalar minLimit = m_lowerLimit[limit_index];
+ btScalar maxLimit = m_upperLimit[limit_index];
+
+ //handle the limits
+ if (minLimit < maxLimit)
+ {
+ {
+ if (depth > maxLimit)
+ {
+ depth -= maxLimit;
+ lo = btScalar(0.);
+
+ }
+ else
+ {
+ if (depth < minLimit)
+ {
+ depth -= minLimit;
+ hi = btScalar(0.);
+ }
+ else
+ {
+ return 0.0f;
+ }
+ }
+ }
+ }
+
+ btScalar normalImpulse= m_limitSoftness*(m_restitution*depth/timeStep - m_damping*rel_vel) * jacDiagABInv;
+
+
+
+
+ btScalar oldNormalImpulse = m_accumulatedImpulse[limit_index];
+ btScalar sum = oldNormalImpulse + normalImpulse;
+ m_accumulatedImpulse[limit_index] = sum > hi ? btScalar(0.) : sum < lo ? btScalar(0.) : sum;
+ normalImpulse = m_accumulatedImpulse[limit_index] - oldNormalImpulse;
+
+ btVector3 impulse_vector = axis_normal_on_a * normalImpulse;
+ body1.applyImpulse( impulse_vector, rel_pos1);
+ body2.applyImpulse(-impulse_vector, rel_pos2);
+
+
+
+ return normalImpulse;
+}
+
+//////////////////////////// btTranslationalLimitMotor ////////////////////////////////////
+
+void btGeneric6DofConstraint::calculateAngleInfo()
+{
+ btMatrix3x3 relative_frame = m_calculatedTransformA.getBasis().inverse()*m_calculatedTransformB.getBasis();
+ matrixToEulerXYZ(relative_frame,m_calculatedAxisAngleDiff);
+ // in euler angle mode we do not actually constrain the angular velocity
+ // along the axes axis[0] and axis[2] (although we do use axis[1]) :
+ //
+ // to get constrain w2-w1 along ...not
+ // ------ --------------------- ------
+ // d(angle[0])/dt = 0 ax[1] x ax[2] ax[0]
+ // d(angle[1])/dt = 0 ax[1]
+ // d(angle[2])/dt = 0 ax[0] x ax[1] ax[2]
+ //
+ // constraining w2-w1 along an axis 'a' means that a'*(w2-w1)=0.
+ // to prove the result for angle[0], write the expression for angle[0] from
+ // GetInfo1 then take the derivative. to prove this for angle[2] it is
+ // easier to take the euler rate expression for d(angle[2])/dt with respect
+ // to the components of w and set that to 0.
+ btVector3 axis0 = m_calculatedTransformB.getBasis().getColumn(0);
+ btVector3 axis2 = m_calculatedTransformA.getBasis().getColumn(2);
+
+ m_calculatedAxis[1] = axis2.cross(axis0);
+ m_calculatedAxis[0] = m_calculatedAxis[1].cross(axis2);
+ m_calculatedAxis[2] = axis0.cross(m_calculatedAxis[1]);
+
+ m_calculatedAxis[0].normalize();
+ m_calculatedAxis[1].normalize();
+ m_calculatedAxis[2].normalize();
+
+}
+
+void btGeneric6DofConstraint::calculateTransforms()
+{
+ calculateTransforms(m_rbA.getCenterOfMassTransform(),m_rbB.getCenterOfMassTransform());
+}
+
+void btGeneric6DofConstraint::calculateTransforms(const btTransform& transA,const btTransform& transB)
+{
+ m_calculatedTransformA = transA * m_frameInA;
+ m_calculatedTransformB = transB * m_frameInB;
+ calculateLinearInfo();
+ calculateAngleInfo();
+ if(m_useOffsetForConstraintFrame)
+ { // get weight factors depending on masses
+ btScalar miA = getRigidBodyA().getInvMass();
+ btScalar miB = getRigidBodyB().getInvMass();
+ m_hasStaticBody = (miA < SIMD_EPSILON) || (miB < SIMD_EPSILON);
+ btScalar miS = miA + miB;
+ if(miS > btScalar(0.f))
+ {
+ m_factA = miB / miS;
+ }
+ else
+ {
+ m_factA = btScalar(0.5f);
+ }
+ m_factB = btScalar(1.0f) - m_factA;
+ }
+}
+
+
+
+void btGeneric6DofConstraint::buildLinearJacobian(
+ btJacobianEntry & jacLinear,const btVector3 & normalWorld,
+ const btVector3 & pivotAInW,const btVector3 & pivotBInW)
+{
+ new (&jacLinear) btJacobianEntry(
+ m_rbA.getCenterOfMassTransform().getBasis().transpose(),
+ m_rbB.getCenterOfMassTransform().getBasis().transpose(),
+ pivotAInW - m_rbA.getCenterOfMassPosition(),
+ pivotBInW - m_rbB.getCenterOfMassPosition(),
+ normalWorld,
+ m_rbA.getInvInertiaDiagLocal(),
+ m_rbA.getInvMass(),
+ m_rbB.getInvInertiaDiagLocal(),
+ m_rbB.getInvMass());
+}
+
+
+
+void btGeneric6DofConstraint::buildAngularJacobian(
+ btJacobianEntry & jacAngular,const btVector3 & jointAxisW)
+{
+ new (&jacAngular) btJacobianEntry(jointAxisW,
+ m_rbA.getCenterOfMassTransform().getBasis().transpose(),
+ m_rbB.getCenterOfMassTransform().getBasis().transpose(),
+ m_rbA.getInvInertiaDiagLocal(),
+ m_rbB.getInvInertiaDiagLocal());
+
+}
+
+
+
+bool btGeneric6DofConstraint::testAngularLimitMotor(int axis_index)
+{
+ btScalar angle = m_calculatedAxisAngleDiff[axis_index];
+ angle = btAdjustAngleToLimits(angle, m_angularLimits[axis_index].m_loLimit, m_angularLimits[axis_index].m_hiLimit);
+ m_angularLimits[axis_index].m_currentPosition = angle;
+ //test limits
+ m_angularLimits[axis_index].testLimitValue(angle);
+ return m_angularLimits[axis_index].needApplyTorques();
+}
+
+
+
+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 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 btGeneric6DofConstraint::getInfo1NonVirtual (btConstraintInfo1* info)
+{
+ if (m_useSolveConstraintObsolete)
+ {
+ info->m_numConstraintRows = 0;
+ info->nub = 0;
+ } else
+ {
+ //pre-allocate all 6
+ info->m_numConstraintRows = 6;
+ info->nub = 0;
+ }
+}
+
+
+void btGeneric6DofConstraint::getInfo2 (btConstraintInfo2* info)
+{
+ btAssert(!m_useSolveConstraintObsolete);
+
+ const btTransform& transA = m_rbA.getCenterOfMassTransform();
+ const btTransform& transB = m_rbB.getCenterOfMassTransform();
+ const btVector3& linVelA = m_rbA.getLinearVelocity();
+ const btVector3& linVelB = m_rbB.getLinearVelocity();
+ const btVector3& angVelA = m_rbA.getAngularVelocity();
+ const btVector3& angVelB = m_rbB.getAngularVelocity();
+
+ 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);
+ }
+}
+
+
+
+int btGeneric6DofConstraint::setLinearLimits(btConstraintInfo2* info, int row, const btTransform& transA,const btTransform& transB,const btVector3& linVelA,const btVector3& linVelB,const btVector3& angVelA,const btVector3& angVelB)
+{
+// int row = 0;
+ //solve linear limits
+ btRotationalLimitMotor limot;
+ for (int i=0;i<3 ;i++ )
+ {
+ if(m_linearLimits.needApplyForce(i))
+ { // re-use rotational motor code
+ limot.m_bounce = btScalar(0.f);
+ limot.m_currentLimit = m_linearLimits.m_currentLimit[i];
+ limot.m_currentPosition = m_linearLimits.m_currentLinearDiff[i];
+ limot.m_currentLimitError = m_linearLimits.m_currentLimitError[i];
+ limot.m_damping = m_linearLimits.m_damping;
+ limot.m_enableMotor = m_linearLimits.m_enableMotor[i];
+ limot.m_hiLimit = m_linearLimits.m_upperLimit[i];
+ limot.m_limitSoftness = m_linearLimits.m_limitSoftness;
+ limot.m_loLimit = m_linearLimits.m_lowerLimit[i];
+ limot.m_maxLimitForce = btScalar(0.f);
+ limot.m_maxMotorForce = m_linearLimits.m_maxMotorForce[i];
+ limot.m_targetVelocity = m_linearLimits.m_targetVelocity[i];
+ btVector3 axis = m_calculatedTransformA.getBasis().getColumn(i);
+ int flags = m_flags >> (i * BT_6DOF_FLAGS_AXIS_SHIFT);
+ limot.m_normalCFM = (flags & BT_6DOF_FLAGS_CFM_NORM) ? m_linearLimits.m_normalCFM[i] : info->cfm[0];
+ limot.m_stopCFM = (flags & BT_6DOF_FLAGS_CFM_STOP) ? m_linearLimits.m_stopCFM[i] : info->cfm[0];
+ limot.m_stopERP = (flags & BT_6DOF_FLAGS_ERP_STOP) ? m_linearLimits.m_stopERP[i] : info->erp;
+ if(m_useOffsetForConstraintFrame)
+ {
+ int indx1 = (i + 1) % 3;
+ int indx2 = (i + 2) % 3;
+ int rotAllowed = 1; // rotations around orthos to current axis
+ if(m_angularLimits[indx1].m_currentLimit && m_angularLimits[indx2].m_currentLimit)
+ {
+ rotAllowed = 0;
+ }
+ row += get_limit_motor_info2(&limot, transA,transB,linVelA,linVelB,angVelA,angVelB, info, row, axis, 0, rotAllowed);
+ }
+ else
+ {
+ row += get_limit_motor_info2(&limot, transA,transB,linVelA,linVelB,angVelA,angVelB, info, row, axis, 0);
+ }
+ }
+ }
+ return row;
+}
+
+
+
+int btGeneric6DofConstraint::setAngularLimits(btConstraintInfo2 *info, int row_offset, const btTransform& transA,const btTransform& transB,const btVector3& linVelA,const btVector3& linVelB,const btVector3& angVelA,const btVector3& angVelB)
+{
+ btGeneric6DofConstraint * d6constraint = this;
+ int row = row_offset;
+ //solve angular limits
+ for (int i=0;i<3 ;i++ )
+ {
+ if(d6constraint->getRotationalLimitMotor(i)->needApplyTorques())
+ {
+ btVector3 axis = d6constraint->getAxis(i);
+ int flags = m_flags >> ((i + 3) * BT_6DOF_FLAGS_AXIS_SHIFT);
+ if(!(flags & BT_6DOF_FLAGS_CFM_NORM))
+ {
+ m_angularLimits[i].m_normalCFM = info->cfm[0];
+ }
+ if(!(flags & BT_6DOF_FLAGS_CFM_STOP))
+ {
+ m_angularLimits[i].m_stopCFM = info->cfm[0];
+ }
+ if(!(flags & BT_6DOF_FLAGS_ERP_STOP))
+ {
+ m_angularLimits[i].m_stopERP = info->erp;
+ }
+ row += get_limit_motor_info2(d6constraint->getRotationalLimitMotor(i),
+ transA,transB,linVelA,linVelB,angVelA,angVelB, info,row,axis,1);
+ }
+ }
+
+ return row;
+}
+
+
+
+
+void btGeneric6DofConstraint::updateRHS(btScalar timeStep)
+{
+ (void)timeStep;
+
+}
+
+
+void btGeneric6DofConstraint::setFrames(const btTransform& frameA, const btTransform& frameB)
+{
+ m_frameInA = frameA;
+ m_frameInB = frameB;
+ buildJacobian();
+ calculateTransforms();
+}
+
+
+
+btVector3 btGeneric6DofConstraint::getAxis(int axis_index) const
+{
+ return m_calculatedAxis[axis_index];
+}
+
+
+btScalar btGeneric6DofConstraint::getRelativePivotPosition(int axisIndex) const
+{
+ return m_calculatedLinearDiff[axisIndex];
+}
+
+
+btScalar btGeneric6DofConstraint::getAngle(int axisIndex) const
+{
+ return m_calculatedAxisAngleDiff[axisIndex];
+}
+
+
+
+void btGeneric6DofConstraint::calcAnchorPos(void)
+{
+ btScalar imA = m_rbA.getInvMass();
+ btScalar imB = m_rbB.getInvMass();
+ btScalar weight;
+ if(imB == btScalar(0.0))
+ {
+ weight = btScalar(1.0);
+ }
+ else
+ {
+ weight = imA / (imA + imB);
+ }
+ const btVector3& pA = m_calculatedTransformA.getOrigin();
+ const btVector3& pB = m_calculatedTransformB.getOrigin();
+ m_AnchorPos = pA * weight + pB * (btScalar(1.0) - weight);
+ return;
+}
+
+
+
+void btGeneric6DofConstraint::calculateLinearInfo()
+{
+ m_calculatedLinearDiff = m_calculatedTransformB.getOrigin() - m_calculatedTransformA.getOrigin();
+ m_calculatedLinearDiff = m_calculatedTransformA.getBasis().inverse() * m_calculatedLinearDiff;
+ for(int i = 0; i < 3; i++)
+ {
+ m_linearLimits.m_currentLinearDiff[i] = m_calculatedLinearDiff[i];
+ m_linearLimits.testLimitValue(i, m_calculatedLinearDiff[i]);
+ }
+}
+
+
+
+int btGeneric6DofConstraint::get_limit_motor_info2(
+ btRotationalLimitMotor * limot,
+ const btTransform& transA,const btTransform& transB,const btVector3& linVelA,const btVector3& linVelB,const btVector3& angVelA,const btVector3& angVelB,
+ btConstraintInfo2 *info, int row, btVector3& ax1, int rotational,int rotAllowed)
+{
+ int srow = row * info->rowskip;
+ bool powered = limot->m_enableMotor;
+ int limit = limot->m_currentLimit;
+ 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 : info->m_J2linearAxis;
+ J1[srow+0] = ax1[0];
+ J1[srow+1] = ax1[1];
+ J1[srow+2] = ax1[2];
+
+ J2[srow+0] = -ax1[0];
+ J2[srow+1] = -ax1[1];
+ J2[srow+2] = -ax1[2];
+
+ if((!rotational))
+ {
+ if (m_useOffsetForConstraintFrame)
+ {
+ btVector3 tmpA, tmpB, relA, relB;
+ // get vector from bodyB to frameB in WCS
+ relB = m_calculatedTransformB.getOrigin() - transB.getOrigin();
+ // get its projection to constraint axis
+ btVector3 projB = ax1 * relB.dot(ax1);
+ // get vector directed from bodyB to constraint axis (and orthogonal to it)
+ btVector3 orthoB = relB - projB;
+ // same for bodyA
+ relA = m_calculatedTransformA.getOrigin() - transA.getOrigin();
+ btVector3 projA = ax1 * relA.dot(ax1);
+ btVector3 orthoA = relA - projA;
+ // get desired offset between frames A and B along constraint axis
+ btScalar desiredOffs = limot->m_currentPosition - limot->m_currentLimitError;
+ // desired vector from projection of center of bodyA to projection of center of bodyB to constraint axis
+ btVector3 totalDist = projA + ax1 * desiredOffs - projB;
+ // get offset vectors relA and relB
+ relA = orthoA + totalDist * m_factA;
+ relB = orthoB - totalDist * m_factB;
+ tmpA = relA.cross(ax1);
+ tmpB = relB.cross(ax1);
+ if(m_hasStaticBody && (!rotAllowed))
+ {
+ tmpA *= m_factA;
+ tmpB *= m_factB;
+ }
+ int i;
+ for (i=0; i<3; i++) info->m_J1angularAxis[srow+i] = tmpA[i];
+ for (i=0; i<3; i++) info->m_J2angularAxis[srow+i] = -tmpB[i];
+ } else
+ {
+ btVector3 ltd; // Linear Torque Decoupling vector
+ btVector3 c = m_calculatedTransformB.getOrigin() - transA.getOrigin();
+ ltd = c.cross(ax1);
+ info->m_J1angularAxis[srow+0] = ltd[0];
+ info->m_J1angularAxis[srow+1] = ltd[1];
+ info->m_J1angularAxis[srow+2] = ltd[2];
+
+ c = m_calculatedTransformB.getOrigin() - transB.getOrigin();
+ ltd = -c.cross(ax1);
+ info->m_J2angularAxis[srow+0] = ltd[0];
+ info->m_J2angularAxis[srow+1] = ltd[1];
+ info->m_J2angularAxis[srow+2] = ltd[2];
+ }
+ }
+ // if we're limited low and high simultaneously, the joint motor is
+ // ineffective
+ if (limit && (limot->m_loLimit == limot->m_hiLimit)) powered = false;
+ info->m_constraintError[srow] = btScalar(0.f);
+ if (powered)
+ {
+ info->cfm[srow] = limot->m_normalCFM;
+ if(!limit)
+ {
+ btScalar tag_vel = rotational ? limot->m_targetVelocity : -limot->m_targetVelocity;
+
+ btScalar mot_fact = getMotorFactor( limot->m_currentPosition,
+ limot->m_loLimit,
+ limot->m_hiLimit,
+ tag_vel,
+ info->fps * limot->m_stopERP);
+ info->m_constraintError[srow] += mot_fact * limot->m_targetVelocity;
+ info->m_lowerLimit[srow] = -limot->m_maxMotorForce;
+ info->m_upperLimit[srow] = limot->m_maxMotorForce;
+ }
+ }
+ if(limit)
+ {
+ btScalar k = info->fps * limot->m_stopERP;
+ if(!rotational)
+ {
+ info->m_constraintError[srow] += k * limot->m_currentLimitError;
+ }
+ else
+ {
+ info->m_constraintError[srow] += -k * limot->m_currentLimitError;
+ }
+ info->cfm[srow] = limot->m_stopCFM;
+ if (limot->m_loLimit == limot->m_hiLimit)
+ { // limited low and high simultaneously
+ info->m_lowerLimit[srow] = -SIMD_INFINITY;
+ info->m_upperLimit[srow] = SIMD_INFINITY;
+ }
+ else
+ {
+ if (limit == 1)
+ {
+ info->m_lowerLimit[srow] = 0;
+ info->m_upperLimit[srow] = SIMD_INFINITY;
+ }
+ else
+ {
+ info->m_lowerLimit[srow] = -SIMD_INFINITY;
+ info->m_upperLimit[srow] = 0;
+ }
+ // deal with bounce
+ if (limot->m_bounce > 0)
+ {
+ // calculate joint velocity
+ btScalar vel;
+ if (rotational)
+ {
+ vel = angVelA.dot(ax1);
+//make sure that if no body -> angVelB == zero vec
+// if (body1)
+ vel -= angVelB.dot(ax1);
+ }
+ else
+ {
+ vel = linVelA.dot(ax1);
+//make sure that if no body -> angVelB == zero vec
+// if (body1)
+ vel -= linVelB.dot(ax1);
+ }
+ // only apply bounce if the velocity is incoming, and if the
+ // resulting c[] exceeds what we already have.
+ if (limit == 1)
+ {
+ if (vel < 0)
+ {
+ btScalar newc = -limot->m_bounce* vel;
+ if (newc > info->m_constraintError[srow])
+ info->m_constraintError[srow] = newc;
+ }
+ }
+ else
+ {
+ if (vel > 0)
+ {
+ btScalar newc = -limot->m_bounce * vel;
+ if (newc < info->m_constraintError[srow])
+ info->m_constraintError[srow] = newc;
+ }
+ }
+ }
+ }
+ }
+ return 1;
+ }
+ else return 0;
+}
+
+
+
+
+
+
+ ///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 btGeneric6DofConstraint::setParam(int num, btScalar value, int axis)
+{
+ if((axis >= 0) && (axis < 3))
+ {
+ switch(num)
+ {
+ case BT_CONSTRAINT_STOP_ERP :
+ m_linearLimits.m_stopERP[axis] = value;
+ m_flags |= BT_6DOF_FLAGS_ERP_STOP << (axis * BT_6DOF_FLAGS_AXIS_SHIFT);
+ break;
+ case BT_CONSTRAINT_STOP_CFM :
+ m_linearLimits.m_stopCFM[axis] = value;
+ m_flags |= BT_6DOF_FLAGS_CFM_STOP << (axis * BT_6DOF_FLAGS_AXIS_SHIFT);
+ break;
+ case BT_CONSTRAINT_CFM :
+ m_linearLimits.m_normalCFM[axis] = value;
+ m_flags |= BT_6DOF_FLAGS_CFM_NORM << (axis * BT_6DOF_FLAGS_AXIS_SHIFT);
+ break;
+ default :
+ btAssertConstrParams(0);
+ }
+ }
+ else if((axis >=3) && (axis < 6))
+ {
+ switch(num)
+ {
+ case BT_CONSTRAINT_STOP_ERP :
+ m_angularLimits[axis - 3].m_stopERP = value;
+ m_flags |= BT_6DOF_FLAGS_ERP_STOP << (axis * BT_6DOF_FLAGS_AXIS_SHIFT);
+ break;
+ case BT_CONSTRAINT_STOP_CFM :
+ m_angularLimits[axis - 3].m_stopCFM = value;
+ m_flags |= BT_6DOF_FLAGS_CFM_STOP << (axis * BT_6DOF_FLAGS_AXIS_SHIFT);
+ break;
+ case BT_CONSTRAINT_CFM :
+ m_angularLimits[axis - 3].m_normalCFM = value;
+ m_flags |= BT_6DOF_FLAGS_CFM_NORM << (axis * BT_6DOF_FLAGS_AXIS_SHIFT);
+ break;
+ default :
+ btAssertConstrParams(0);
+ }
+ }
+ else
+ {
+ btAssertConstrParams(0);
+ }
+}
+
+ ///return the local value of parameter
+btScalar btGeneric6DofConstraint::getParam(int num, int axis) const
+{
+ btScalar retVal = 0;
+ if((axis >= 0) && (axis < 3))
+ {
+ switch(num)
+ {
+ case BT_CONSTRAINT_STOP_ERP :
+ btAssertConstrParams(m_flags & (BT_6DOF_FLAGS_ERP_STOP << (axis * BT_6DOF_FLAGS_AXIS_SHIFT)));
+ retVal = m_linearLimits.m_stopERP[axis];
+ break;
+ case BT_CONSTRAINT_STOP_CFM :
+ btAssertConstrParams(m_flags & (BT_6DOF_FLAGS_CFM_STOP << (axis * BT_6DOF_FLAGS_AXIS_SHIFT)));
+ retVal = m_linearLimits.m_stopCFM[axis];
+ break;
+ case BT_CONSTRAINT_CFM :
+ btAssertConstrParams(m_flags & (BT_6DOF_FLAGS_CFM_NORM << (axis * BT_6DOF_FLAGS_AXIS_SHIFT)));
+ retVal = m_linearLimits.m_normalCFM[axis];
+ break;
+ default :
+ btAssertConstrParams(0);
+ }
+ }
+ else if((axis >=3) && (axis < 6))
+ {
+ switch(num)
+ {
+ case BT_CONSTRAINT_STOP_ERP :
+ btAssertConstrParams(m_flags & (BT_6DOF_FLAGS_ERP_STOP << (axis * BT_6DOF_FLAGS_AXIS_SHIFT)));
+ retVal = m_angularLimits[axis - 3].m_stopERP;
+ break;
+ case BT_CONSTRAINT_STOP_CFM :
+ btAssertConstrParams(m_flags & (BT_6DOF_FLAGS_CFM_STOP << (axis * BT_6DOF_FLAGS_AXIS_SHIFT)));
+ retVal = m_angularLimits[axis - 3].m_stopCFM;
+ break;
+ case BT_CONSTRAINT_CFM :
+ btAssertConstrParams(m_flags & (BT_6DOF_FLAGS_CFM_NORM << (axis * BT_6DOF_FLAGS_AXIS_SHIFT)));
+ retVal = m_angularLimits[axis - 3].m_normalCFM;
+ break;
+ default :
+ btAssertConstrParams(0);
+ }
+ }
+ else
+ {
+ btAssertConstrParams(0);
+ }
+ return retVal;
+}
+
+
+
+void btGeneric6DofConstraint::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();
+}
generated by cgit v1.2.3 (git 2.25.1) at 2024-12-20 01:30:30 +0000