diff options
author | Sergej Reich <sergej.reich@googlemail.com> | 2013-10-25 07:43:20 +0400 |
---|---|---|
committer | Sergej Reich <sergej.reich@googlemail.com> | 2013-10-25 07:43:20 +0400 |
commit | 472a021aca3c0a6278151f7abee4e0970b026e3f (patch) | |
tree | 3456ce084e33450cc21c68beca1bd407801bf256 /extern/bullet2/src/BulletDynamics/ConstraintSolver/btFixedConstraint.cpp | |
parent | 4514eeaa8e335a86de0bda68aa4814c0bafad7eb (diff) |
bullet: Update to version 2.82 (bullet revision 2705)
Remove patch that has been applied upstream.
Fixes several bugs.
Diffstat (limited to 'extern/bullet2/src/BulletDynamics/ConstraintSolver/btFixedConstraint.cpp')
-rw-r--r-- | extern/bullet2/src/BulletDynamics/ConstraintSolver/btFixedConstraint.cpp | 129 |
1 files changed, 129 insertions, 0 deletions
diff --git a/extern/bullet2/src/BulletDynamics/ConstraintSolver/btFixedConstraint.cpp b/extern/bullet2/src/BulletDynamics/ConstraintSolver/btFixedConstraint.cpp new file mode 100644 index 00000000000..f93a3280f35 --- /dev/null +++ b/extern/bullet2/src/BulletDynamics/ConstraintSolver/btFixedConstraint.cpp @@ -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]; + } + +}
\ No newline at end of file |