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