dali-physics/third-party/bullet3/src/BulletDynamics/ConstraintSolver/btPoint2PointConstraint.cpp

   1 /*
   2 Bullet Continuous Collision Detection and Physics Library
   3 Copyright (c) 2003-2006 Erwin Coumans  https://bulletphysics.org
   4
   5 This software is provided 'as-is', without any express or implied warranty.
   6 In no event will the authors be held liable for any damages arising from the use of this software.
   7 Permission is granted to anyone to use this software for any purpose,
   8 including commercial applications, and to alter it and redistribute it freely,
   9 subject to the following restrictions:
  10
  11 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.
  12 2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
  13 3. This notice may not be removed or altered from any source distribution.
  14 */
  15
  16 #include "btPoint2PointConstraint.h"
  17 #include "BulletDynamics/Dynamics/btRigidBody.h"
  18 #include <new>
  19
  20 btPoint2PointConstraint::btPoint2PointConstraint(btRigidBody& rbA, btRigidBody& rbB, const btVector3& pivotInA, const btVector3& pivotInB)
  21         : btTypedConstraint(POINT2POINT_CONSTRAINT_TYPE, rbA, rbB), m_pivotInA(pivotInA), m_pivotInB(pivotInB), m_flags(0), m_useSolveConstraintObsolete(false)
  22 {
  23 }
  24
  25 btPoint2PointConstraint::btPoint2PointConstraint(btRigidBody& rbA, const btVector3& pivotInA)
  26         : btTypedConstraint(POINT2POINT_CONSTRAINT_TYPE, rbA), m_pivotInA(pivotInA), m_pivotInB(rbA.getCenterOfMassTransform()(pivotInA)), m_flags(0), m_useSolveConstraintObsolete(false)
  27 {
  28 }
  29
  30 void btPoint2PointConstraint::buildJacobian()
  31 {
  32         ///we need it for both methods
  33         {
  34                 m_appliedImpulse = btScalar(0.);
  35
  36                 btVector3 normal(0, 0, 0);
  37
  38                 for (int i = 0; i < 3; i++)
  39                 {
  40                         normal[i] = 1;
  41                         new (&m_jac[i]) btJacobianEntry(
  42                                 m_rbA.getCenterOfMassTransform().getBasis().transpose(),
  43                                 m_rbB.getCenterOfMassTransform().getBasis().transpose(),
  44                                 m_rbA.getCenterOfMassTransform() * m_pivotInA - m_rbA.getCenterOfMassPosition(),
  45                                 m_rbB.getCenterOfMassTransform() * m_pivotInB - m_rbB.getCenterOfMassPosition(),
  46                                 normal,
  47                                 m_rbA.getInvInertiaDiagLocal(),
  48                                 m_rbA.getInvMass(),
  49                                 m_rbB.getInvInertiaDiagLocal(),
  50                                 m_rbB.getInvMass());
  51                         normal[i] = 0;
  52                 }
  53         }
  54 }
  55
  56 void btPoint2PointConstraint::getInfo1(btConstraintInfo1* info)
  57 {
  58         getInfo1NonVirtual(info);
  59 }
  60
  61 void btPoint2PointConstraint::getInfo1NonVirtual(btConstraintInfo1* info)
  62 {
  63         if (m_useSolveConstraintObsolete)
  64         {
  65                 info->m_numConstraintRows = 0;
  66                 info->nub = 0;
  67         }
  68         else
  69         {
  70                 info->m_numConstraintRows = 3;
  71                 info->nub = 3;
  72         }
  73 }
  74
  75 void btPoint2PointConstraint::getInfo2(btConstraintInfo2* info)
  76 {
  77         getInfo2NonVirtual(info, m_rbA.getCenterOfMassTransform(), m_rbB.getCenterOfMassTransform());
  78 }
  79
  80 void btPoint2PointConstraint::getInfo2NonVirtual(btConstraintInfo2* info, const btTransform& body0_trans, const btTransform& body1_trans)
  81 {
  82         btAssert(!m_useSolveConstraintObsolete);
  83
  84         //retrieve matrices
  85
  86         // anchor points in global coordinates with respect to body PORs.
  87
  88         // set jacobian
  89         info->m_J1linearAxis[0] = 1;
  90         info->m_J1linearAxis[info->rowskip + 1] = 1;
  91         info->m_J1linearAxis[2 * info->rowskip + 2] = 1;
  92
  93         btVector3 a1 = body0_trans.getBasis() * getPivotInA();
  94         {
  95                 btVector3* angular0 = (btVector3*)(info->m_J1angularAxis);
  96                 btVector3* angular1 = (btVector3*)(info->m_J1angularAxis + info->rowskip);
  97                 btVector3* angular2 = (btVector3*)(info->m_J1angularAxis + 2 * info->rowskip);
  98                 btVector3 a1neg = -a1;
  99                 a1neg.getSkewSymmetricMatrix(angular0, angular1, angular2);
 100         }
 101
 102         info->m_J2linearAxis[0] = -1;
 103         info->m_J2linearAxis[info->rowskip + 1] = -1;
 104         info->m_J2linearAxis[2 * info->rowskip + 2] = -1;
 105
 106         btVector3 a2 = body1_trans.getBasis() * getPivotInB();
 107
 108         {
 109                 //      btVector3 a2n = -a2;
 110                 btVector3* angular0 = (btVector3*)(info->m_J2angularAxis);
 111                 btVector3* angular1 = (btVector3*)(info->m_J2angularAxis + info->rowskip);
 112                 btVector3* angular2 = (btVector3*)(info->m_J2angularAxis + 2 * info->rowskip);
 113                 a2.getSkewSymmetricMatrix(angular0, angular1, angular2);
 114         }
 115
 116         // set right hand side
 117         btScalar currERP = (m_flags & BT_P2P_FLAGS_ERP) ? m_erp : info->erp;
 118         btScalar k = info->fps * currERP;
 119         int j;
 120         for (j = 0; j < 3; j++)
 121         {
 122                 info->m_constraintError[j * info->rowskip] = k * (a2[j] + body1_trans.getOrigin()[j] - a1[j] - body0_trans.getOrigin()[j]);
 123                 //printf("info->m_constraintError[%d]=%f\n",j,info->m_constraintError[j]);
 124         }
 125         if (m_flags & BT_P2P_FLAGS_CFM)
 126         {
 127                 for (j = 0; j < 3; j++)
 128                 {
 129                         info->cfm[j * info->rowskip] = m_cfm;
 130                 }
 131         }
 132
 133         btScalar impulseClamp = m_setting.m_impulseClamp;  //
 134         for (j = 0; j < 3; j++)
 135         {
 136                 if (m_setting.m_impulseClamp > 0)
 137                 {
 138                         info->m_lowerLimit[j * info->rowskip] = -impulseClamp;
 139                         info->m_upperLimit[j * info->rowskip] = impulseClamp;
 140                 }
 141         }
 142         info->m_damping = m_setting.m_damping;
 143 }
 144
 145 void btPoint2PointConstraint::updateRHS(btScalar timeStep)
 146 {
 147         (void)timeStep;
 148 }
 149
 150 ///override the default global value of a parameter (such as ERP or CFM), optionally provide the axis (0..5).
 151 ///If no axis is provided, it uses the default axis for this constraint.
 152 void btPoint2PointConstraint::setParam(int num, btScalar value, int axis)
 153 {
 154         if (axis != -1)
 155         {
 156                 btAssertConstrParams(0);
 157         }
 158         else
 159         {
 160                 switch (num)
 161                 {
 162                         case BT_CONSTRAINT_ERP:
 163                         case BT_CONSTRAINT_STOP_ERP:
 164                                 m_erp = value;
 165                                 m_flags |= BT_P2P_FLAGS_ERP;
 166                                 break;
 167                         case BT_CONSTRAINT_CFM:
 168                         case BT_CONSTRAINT_STOP_CFM:
 169                                 m_cfm = value;
 170                                 m_flags |= BT_P2P_FLAGS_CFM;
 171                                 break;
 172                         default:
 173                                 btAssertConstrParams(0);
 174                 }
 175         }
 176 }
 177
 178 ///return the local value of parameter
 179 btScalar btPoint2PointConstraint::getParam(int num, int axis) const
 180 {
 181         btScalar retVal(SIMD_INFINITY);
 182         if (axis != -1)
 183         {
 184                 btAssertConstrParams(0);
 185         }
 186         else
 187         {
 188                 switch (num)
 189                 {
 190                         case BT_CONSTRAINT_ERP:
 191                         case BT_CONSTRAINT_STOP_ERP:
 192                                 btAssertConstrParams(m_flags & BT_P2P_FLAGS_ERP);
 193                                 retVal = m_erp;
 194                                 break;
 195                         case BT_CONSTRAINT_CFM:
 196                         case BT_CONSTRAINT_STOP_CFM:
 197                                 btAssertConstrParams(m_flags & BT_P2P_FLAGS_CFM);
 198                                 retVal = m_cfm;
 199                                 break;
 200                         default:
 201                                 btAssertConstrParams(0);
 202                 }
 203         }
 204         return retVal;
 205 }