dali-physics/third-party/bullet3/src/Bullet3Dynamics/ConstraintSolver/b3Point2PointConstraint.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 "b3Point2PointConstraint.h"
  17 #include "Bullet3Collision/NarrowPhaseCollision/shared/b3RigidBodyData.h"
  18
  19 #include <new>
  20
  21 b3Point2PointConstraint::b3Point2PointConstraint(int rbA, int rbB, const b3Vector3& pivotInA, const b3Vector3& pivotInB)
  22         : b3TypedConstraint(B3_POINT2POINT_CONSTRAINT_TYPE, rbA, rbB), m_pivotInA(pivotInA), m_pivotInB(pivotInB), m_flags(0)
  23 {
  24 }
  25
  26 /*
  27 b3Point2PointConstraint::b3Point2PointConstraint(int  rbA,const b3Vector3& pivotInA)
  28 :b3TypedConstraint(B3_POINT2POINT_CONSTRAINT_TYPE,rbA),m_pivotInA(pivotInA),m_pivotInB(rbA.getCenterOfMassTransform()(pivotInA)),
  29 m_flags(0),
  30 m_useSolveConstraintObsolete(false)
  31 {
  32
  33 }
  34 */
  35
  36 void b3Point2PointConstraint::getInfo1(b3ConstraintInfo1* info, const b3RigidBodyData* bodies)
  37 {
  38         getInfo1NonVirtual(info, bodies);
  39 }
  40
  41 void b3Point2PointConstraint::getInfo1NonVirtual(b3ConstraintInfo1* info, const b3RigidBodyData* bodies)
  42 {
  43         info->m_numConstraintRows = 3;
  44         info->nub = 3;
  45 }
  46
  47 void b3Point2PointConstraint::getInfo2(b3ConstraintInfo2* info, const b3RigidBodyData* bodies)
  48 {
  49         b3Transform trA;
  50         trA.setIdentity();
  51         trA.setOrigin(bodies[m_rbA].m_pos);
  52         trA.setRotation(bodies[m_rbA].m_quat);
  53
  54         b3Transform trB;
  55         trB.setIdentity();
  56         trB.setOrigin(bodies[m_rbB].m_pos);
  57         trB.setRotation(bodies[m_rbB].m_quat);
  58
  59         getInfo2NonVirtual(info, trA, trB);
  60 }
  61
  62 void b3Point2PointConstraint::getInfo2NonVirtual(b3ConstraintInfo2* info, const b3Transform& body0_trans, const b3Transform& body1_trans)
  63 {
  64         //retrieve matrices
  65
  66         // anchor points in global coordinates with respect to body PORs.
  67
  68         // set jacobian
  69         info->m_J1linearAxis[0] = 1;
  70         info->m_J1linearAxis[info->rowskip + 1] = 1;
  71         info->m_J1linearAxis[2 * info->rowskip + 2] = 1;
  72
  73         b3Vector3 a1 = body0_trans.getBasis() * getPivotInA();
  74         //b3Vector3 a1a = b3QuatRotate(body0_trans.getRotation(),getPivotInA());
  75
  76         {
  77                 b3Vector3* angular0 = (b3Vector3*)(info->m_J1angularAxis);
  78                 b3Vector3* angular1 = (b3Vector3*)(info->m_J1angularAxis + info->rowskip);
  79                 b3Vector3* angular2 = (b3Vector3*)(info->m_J1angularAxis + 2 * info->rowskip);
  80                 b3Vector3 a1neg = -a1;
  81                 a1neg.getSkewSymmetricMatrix(angular0, angular1, angular2);
  82         }
  83
  84         if (info->m_J2linearAxis)
  85         {
  86                 info->m_J2linearAxis[0] = -1;
  87                 info->m_J2linearAxis[info->rowskip + 1] = -1;
  88                 info->m_J2linearAxis[2 * info->rowskip + 2] = -1;
  89         }
  90
  91         b3Vector3 a2 = body1_trans.getBasis() * getPivotInB();
  92
  93         {
  94                 //      b3Vector3 a2n = -a2;
  95                 b3Vector3* angular0 = (b3Vector3*)(info->m_J2angularAxis);
  96                 b3Vector3* angular1 = (b3Vector3*)(info->m_J2angularAxis + info->rowskip);
  97                 b3Vector3* angular2 = (b3Vector3*)(info->m_J2angularAxis + 2 * info->rowskip);
  98                 a2.getSkewSymmetricMatrix(angular0, angular1, angular2);
  99         }
 100
 101         // set right hand side
 102         b3Scalar currERP = (m_flags & B3_P2P_FLAGS_ERP) ? m_erp : info->erp;
 103         b3Scalar k = info->fps * currERP;
 104         int j;
 105         for (j = 0; j < 3; j++)
 106         {
 107                 info->m_constraintError[j * info->rowskip] = k * (a2[j] + body1_trans.getOrigin()[j] - a1[j] - body0_trans.getOrigin()[j]);
 108                 //printf("info->m_constraintError[%d]=%f\n",j,info->m_constraintError[j]);
 109         }
 110         if (m_flags & B3_P2P_FLAGS_CFM)
 111         {
 112                 for (j = 0; j < 3; j++)
 113                 {
 114                         info->cfm[j * info->rowskip] = m_cfm;
 115                 }
 116         }
 117
 118         b3Scalar impulseClamp = m_setting.m_impulseClamp;  //
 119         for (j = 0; j < 3; j++)
 120         {
 121                 if (m_setting.m_impulseClamp > 0)
 122                 {
 123                         info->m_lowerLimit[j * info->rowskip] = -impulseClamp;
 124                         info->m_upperLimit[j * info->rowskip] = impulseClamp;
 125                 }
 126         }
 127         info->m_damping = m_setting.m_damping;
 128 }
 129
 130 void b3Point2PointConstraint::updateRHS(b3Scalar timeStep)
 131 {
 132         (void)timeStep;
 133 }
 134
 135 ///override the default global value of a parameter (such as ERP or CFM), optionally provide the axis (0..5).
 136 ///If no axis is provided, it uses the default axis for this constraint.
 137 void b3Point2PointConstraint::setParam(int num, b3Scalar value, int axis)
 138 {
 139         if (axis != -1)
 140         {
 141                 b3AssertConstrParams(0);
 142         }
 143         else
 144         {
 145                 switch (num)
 146                 {
 147                         case B3_CONSTRAINT_ERP:
 148                         case B3_CONSTRAINT_STOP_ERP:
 149                                 m_erp = value;
 150                                 m_flags |= B3_P2P_FLAGS_ERP;
 151                                 break;
 152                         case B3_CONSTRAINT_CFM:
 153                         case B3_CONSTRAINT_STOP_CFM:
 154                                 m_cfm = value;
 155                                 m_flags |= B3_P2P_FLAGS_CFM;
 156                                 break;
 157                         default:
 158                                 b3AssertConstrParams(0);
 159                 }
 160         }
 161 }
 162
 163 ///return the local value of parameter
 164 b3Scalar b3Point2PointConstraint::getParam(int num, int axis) const
 165 {
 166         b3Scalar retVal(B3_INFINITY);
 167         if (axis != -1)
 168         {
 169                 b3AssertConstrParams(0);
 170         }
 171         else
 172         {
 173                 switch (num)
 174                 {
 175                         case B3_CONSTRAINT_ERP:
 176                         case B3_CONSTRAINT_STOP_ERP:
 177                                 b3AssertConstrParams(m_flags & B3_P2P_FLAGS_ERP);
 178                                 retVal = m_erp;
 179                                 break;
 180                         case B3_CONSTRAINT_CFM:
 181                         case B3_CONSTRAINT_STOP_CFM:
 182                                 b3AssertConstrParams(m_flags & B3_P2P_FLAGS_CFM);
 183                                 retVal = m_cfm;
 184                                 break;
 185                         default:
 186                                 b3AssertConstrParams(0);
 187                 }
 188         }
 189         return retVal;
 190 }