Go to the documentation of this file.00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
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
00102
00103
00104
00105
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
00120
00121
00122
00123
00124 btVector3 a2 = body1_trans.getBasis()*getPivotInB();
00125
00126 {
00127
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
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
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