Blender V2.61 - r43446

btPoint2PointConstraint.cpp

Go to the documentation of this file.
00001 /*
00002 Bullet Continuous Collision Detection and Physics Library
00003 Copyright (c) 2003-2006 Erwin Coumans  http://continuousphysics.com/Bullet/
00004 
00005 This software is provided 'as-is', without any express or implied warranty.
00006 In no event will the authors be held liable for any damages arising from the use of this software.
00007 Permission is granted to anyone to use this software for any purpose, 
00008 including commercial applications, and to alter it and redistribute it freely, 
00009 subject to the following restrictions:
00010 
00011 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.
00012 2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
00013 3. This notice may not be removed or altered from any source distribution.
00014 */
00015 
00016 
00017 #include "btPoint2PointConstraint.h"
00018 #include "BulletDynamics/Dynamics/btRigidBody.h"
00019 #include <new>
00020 
00021 
00022 
00023 
00024 
00025 btPoint2PointConstraint::btPoint2PointConstraint(btRigidBody& rbA,btRigidBody& rbB, const btVector3& pivotInA,const btVector3& pivotInB)
00026 :btTypedConstraint(POINT2POINT_CONSTRAINT_TYPE,rbA,rbB),m_pivotInA(pivotInA),m_pivotInB(pivotInB),
00027 m_flags(0),
00028 m_useSolveConstraintObsolete(false)
00029 {
00030 
00031 }
00032 
00033 
00034 btPoint2PointConstraint::btPoint2PointConstraint(btRigidBody& rbA,const btVector3& pivotInA)
00035 :btTypedConstraint(POINT2POINT_CONSTRAINT_TYPE,rbA),m_pivotInA(pivotInA),m_pivotInB(rbA.getCenterOfMassTransform()(pivotInA)),
00036 m_flags(0),
00037 m_useSolveConstraintObsolete(false)
00038 {
00039     
00040 }
00041 
00042 void    btPoint2PointConstraint::buildJacobian()
00043 {
00044 
00046     {
00047         m_appliedImpulse = btScalar(0.);
00048 
00049         btVector3   normal(0,0,0);
00050 
00051         for (int i=0;i<3;i++)
00052         {
00053             normal[i] = 1;
00054             new (&m_jac[i]) btJacobianEntry(
00055             m_rbA.getCenterOfMassTransform().getBasis().transpose(),
00056             m_rbB.getCenterOfMassTransform().getBasis().transpose(),
00057             m_rbA.getCenterOfMassTransform()*m_pivotInA - m_rbA.getCenterOfMassPosition(),
00058             m_rbB.getCenterOfMassTransform()*m_pivotInB - m_rbB.getCenterOfMassPosition(),
00059             normal,
00060             m_rbA.getInvInertiaDiagLocal(),
00061             m_rbA.getInvMass(),
00062             m_rbB.getInvInertiaDiagLocal(),
00063             m_rbB.getInvMass());
00064         normal[i] = 0;
00065         }
00066     }
00067 
00068 
00069 }
00070 
00071 void btPoint2PointConstraint::getInfo1 (btConstraintInfo1* info)
00072 {
00073     getInfo1NonVirtual(info);
00074 }
00075 
00076 void btPoint2PointConstraint::getInfo1NonVirtual (btConstraintInfo1* info)
00077 {
00078     if (m_useSolveConstraintObsolete)
00079     {
00080         info->m_numConstraintRows = 0;
00081         info->nub = 0;
00082     } else
00083     {
00084         info->m_numConstraintRows = 3;
00085         info->nub = 3;
00086     }
00087 }
00088 
00089 
00090 
00091 
00092 void btPoint2PointConstraint::getInfo2 (btConstraintInfo2* info)
00093 {
00094     getInfo2NonVirtual(info, m_rbA.getCenterOfMassTransform(),m_rbB.getCenterOfMassTransform());
00095 }
00096 
00097 void btPoint2PointConstraint::getInfo2NonVirtual (btConstraintInfo2* info, const btTransform& body0_trans, const btTransform& body1_trans)
00098 {
00099     btAssert(!m_useSolveConstraintObsolete);
00100 
00101      //retrieve matrices
00102 
00103     // anchor points in global coordinates with respect to body PORs.
00104    
00105     // set jacobian
00106     info->m_J1linearAxis[0] = 1;
00107     info->m_J1linearAxis[info->rowskip+1] = 1;
00108     info->m_J1linearAxis[2*info->rowskip+2] = 1;
00109 
00110     btVector3 a1 = body0_trans.getBasis()*getPivotInA();
00111     {
00112         btVector3* angular0 = (btVector3*)(info->m_J1angularAxis);
00113         btVector3* angular1 = (btVector3*)(info->m_J1angularAxis+info->rowskip);
00114         btVector3* angular2 = (btVector3*)(info->m_J1angularAxis+2*info->rowskip);
00115         btVector3 a1neg = -a1;
00116         a1neg.getSkewSymmetricMatrix(angular0,angular1,angular2);
00117     }
00118     
00119     /*info->m_J2linearAxis[0] = -1;
00120     info->m_J2linearAxis[s+1] = -1;
00121     info->m_J2linearAxis[2*s+2] = -1;
00122     */
00123     
00124     btVector3 a2 = body1_trans.getBasis()*getPivotInB();
00125    
00126     {
00127         btVector3 a2n = -a2;
00128         btVector3* angular0 = (btVector3*)(info->m_J2angularAxis);
00129         btVector3* angular1 = (btVector3*)(info->m_J2angularAxis+info->rowskip);
00130         btVector3* angular2 = (btVector3*)(info->m_J2angularAxis+2*info->rowskip);
00131         a2.getSkewSymmetricMatrix(angular0,angular1,angular2);
00132     }
00133     
00134 
00135 
00136     // set right hand side
00137     btScalar currERP = (m_flags & BT_P2P_FLAGS_ERP) ? m_erp : info->erp;
00138     btScalar k = info->fps * currERP;
00139     int j;
00140     for (j=0; j<3; j++)
00141     {
00142         info->m_constraintError[j*info->rowskip] = k * (a2[j] + body1_trans.getOrigin()[j] - a1[j] - body0_trans.getOrigin()[j]);
00143         //printf("info->m_constraintError[%d]=%f\n",j,info->m_constraintError[j]);
00144     }
00145     if(m_flags & BT_P2P_FLAGS_CFM)
00146     {
00147         for (j=0; j<3; j++)
00148         {
00149             info->cfm[j*info->rowskip] = m_cfm;
00150         }
00151     }
00152 
00153     btScalar impulseClamp = m_setting.m_impulseClamp;//
00154     for (j=0; j<3; j++)
00155     {
00156         if (m_setting.m_impulseClamp > 0)
00157         {
00158             info->m_lowerLimit[j*info->rowskip] = -impulseClamp;
00159             info->m_upperLimit[j*info->rowskip] = impulseClamp;
00160         }
00161     }
00162     info->m_damping = m_setting.m_damping;
00163     
00164 }
00165 
00166 
00167 
00168 void    btPoint2PointConstraint::updateRHS(btScalar timeStep)
00169 {
00170     (void)timeStep;
00171 
00172 }
00173 
00176 void btPoint2PointConstraint::setParam(int num, btScalar value, int axis)
00177 {
00178     if(axis != -1)
00179     {
00180         btAssertConstrParams(0);
00181     }
00182     else
00183     {
00184         switch(num)
00185         {
00186             case BT_CONSTRAINT_ERP :
00187             case BT_CONSTRAINT_STOP_ERP :
00188                 m_erp = value; 
00189                 m_flags |= BT_P2P_FLAGS_ERP;
00190                 break;
00191             case BT_CONSTRAINT_CFM :
00192             case BT_CONSTRAINT_STOP_CFM :
00193                 m_cfm = value; 
00194                 m_flags |= BT_P2P_FLAGS_CFM;
00195                 break;
00196             default: 
00197                 btAssertConstrParams(0);
00198         }
00199     }
00200 }
00201 
00203 btScalar btPoint2PointConstraint::getParam(int num, int axis) const 
00204 {
00205     btScalar retVal(SIMD_INFINITY);
00206     if(axis != -1)
00207     {
00208         btAssertConstrParams(0);
00209     }
00210     else
00211     {
00212         switch(num)
00213         {
00214             case BT_CONSTRAINT_ERP :
00215             case BT_CONSTRAINT_STOP_ERP :
00216                 btAssertConstrParams(m_flags & BT_P2P_FLAGS_ERP);
00217                 retVal = m_erp; 
00218                 break;
00219             case BT_CONSTRAINT_CFM :
00220             case BT_CONSTRAINT_STOP_CFM :
00221                 btAssertConstrParams(m_flags & BT_P2P_FLAGS_CFM);
00222                 retVal = m_cfm; 
00223                 break;
00224             default: 
00225                 btAssertConstrParams(0);
00226         }
00227     }
00228     return retVal;
00229 }
00230