diff options
Diffstat (limited to 'thirdparty/bullet/src/BulletDynamics/ConstraintSolver/btPoint2PointConstraint.cpp')
| -rw-r--r-- | thirdparty/bullet/src/BulletDynamics/ConstraintSolver/btPoint2PointConstraint.cpp | 229 |
1 files changed, 0 insertions, 229 deletions
diff --git a/thirdparty/bullet/src/BulletDynamics/ConstraintSolver/btPoint2PointConstraint.cpp b/thirdparty/bullet/src/BulletDynamics/ConstraintSolver/btPoint2PointConstraint.cpp deleted file mode 100644 index 3c0430b903..0000000000 --- a/thirdparty/bullet/src/BulletDynamics/ConstraintSolver/btPoint2PointConstraint.cpp +++ /dev/null @@ -1,229 +0,0 @@ -/* -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 "btPoint2PointConstraint.h" -#include "BulletDynamics/Dynamics/btRigidBody.h" -#include <new> - - - - - -btPoint2PointConstraint::btPoint2PointConstraint(btRigidBody& rbA,btRigidBody& rbB, const btVector3& pivotInA,const btVector3& pivotInB) -:btTypedConstraint(POINT2POINT_CONSTRAINT_TYPE,rbA,rbB),m_pivotInA(pivotInA),m_pivotInB(pivotInB), -m_flags(0), -m_useSolveConstraintObsolete(false) -{ - -} - - -btPoint2PointConstraint::btPoint2PointConstraint(btRigidBody& rbA,const btVector3& pivotInA) -:btTypedConstraint(POINT2POINT_CONSTRAINT_TYPE,rbA),m_pivotInA(pivotInA),m_pivotInB(rbA.getCenterOfMassTransform()(pivotInA)), -m_flags(0), -m_useSolveConstraintObsolete(false) -{ - -} - -void btPoint2PointConstraint::buildJacobian() -{ - - ///we need it for both methods - { - m_appliedImpulse = btScalar(0.); - - btVector3 normal(0,0,0); - - for (int i=0;i<3;i++) - { - normal[i] = 1; - new (&m_jac[i]) btJacobianEntry( - m_rbA.getCenterOfMassTransform().getBasis().transpose(), - m_rbB.getCenterOfMassTransform().getBasis().transpose(), - m_rbA.getCenterOfMassTransform()*m_pivotInA - m_rbA.getCenterOfMassPosition(), - m_rbB.getCenterOfMassTransform()*m_pivotInB - m_rbB.getCenterOfMassPosition(), - normal, - m_rbA.getInvInertiaDiagLocal(), - m_rbA.getInvMass(), - m_rbB.getInvInertiaDiagLocal(), - m_rbB.getInvMass()); - normal[i] = 0; - } - } - - -} - -void btPoint2PointConstraint::getInfo1 (btConstraintInfo1* info) -{ - getInfo1NonVirtual(info); -} - -void btPoint2PointConstraint::getInfo1NonVirtual (btConstraintInfo1* info) -{ - if (m_useSolveConstraintObsolete) - { - info->m_numConstraintRows = 0; - info->nub = 0; - } else - { - info->m_numConstraintRows = 3; - info->nub = 3; - } -} - - - - -void btPoint2PointConstraint::getInfo2 (btConstraintInfo2* info) -{ - getInfo2NonVirtual(info, m_rbA.getCenterOfMassTransform(),m_rbB.getCenterOfMassTransform()); -} - -void btPoint2PointConstraint::getInfo2NonVirtual (btConstraintInfo2* info, const btTransform& body0_trans, const btTransform& 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; - - btVector3 a1 = body0_trans.getBasis()*getPivotInA(); - { - 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); - } - - info->m_J2linearAxis[0] = -1; - info->m_J2linearAxis[info->rowskip+1] = -1; - info->m_J2linearAxis[2*info->rowskip+2] = -1; - - btVector3 a2 = body1_trans.getBasis()*getPivotInB(); - - { - // 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 - btScalar currERP = (m_flags & BT_P2P_FLAGS_ERP) ? m_erp : info->erp; - btScalar 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; - } - } - - btScalar 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 btPoint2PointConstraint::updateRHS(btScalar 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 btPoint2PointConstraint::setParam(int num, btScalar 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 -btScalar btPoint2PointConstraint::getParam(int num, int axis) const -{ - btScalar 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; -} - |