Blender  V3.3
btPoint2PointConstraint.cpp
Go to the documentation of this file.
1 /*
2 Bullet Continuous Collision Detection and Physics Library
3 Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/
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 
18 #include <new>
19 
22 {
23 }
24 
26  : btTypedConstraint(POINT2POINT_CONSTRAINT_TYPE, rbA), m_pivotInA(pivotInA), m_pivotInB(rbA.getCenterOfMassTransform()(pivotInA)), m_flags(0), m_useSolveConstraintObsolete(false)
27 {
28 }
29 
31 {
33  {
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(),
46  normal,
48  m_rbA.getInvMass(),
50  m_rbB.getInvMass());
51  normal[i] = 0;
52  }
53  }
54 }
55 
57 {
58  getInfo1NonVirtual(info);
59 }
60 
62 {
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 
76 {
78 }
79 
80 void btPoint2PointConstraint::getInfo2NonVirtual(btConstraintInfo2* info, const btTransform& body0_trans, const btTransform& body1_trans)
81 {
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  }
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 
146 {
147  (void)timeStep;
148 }
149 
152 void btPoint2PointConstraint::setParam(int num, btScalar value, int axis)
153 {
154  if (axis != -1)
155  {
157  }
158  else
159  {
160  switch (num)
161  {
162  case BT_CONSTRAINT_ERP:
164  m_erp = value;
166  break;
167  case BT_CONSTRAINT_CFM:
169  m_cfm = value;
171  break;
172  default:
174  }
175  }
176 }
177 
179 btScalar btPoint2PointConstraint::getParam(int num, int axis) const
180 {
181  btScalar retVal(SIMD_INFINITY);
182  if (axis != -1)
183  {
185  }
186  else
187  {
188  switch (num)
189  {
190  case BT_CONSTRAINT_ERP:
193  retVal = m_erp;
194  break;
195  case BT_CONSTRAINT_CFM:
198  retVal = m_cfm;
199  break;
200  default:
202  }
203  }
204  return retVal;
205 }
virtual void getInfo2(btConstraintInfo2 *info)
virtual void buildJacobian()
internal method used by the constraint solver, don't use them directly
virtual void setParam(int num, btScalar value, int axis=-1)
void updateRHS(btScalar timeStep)
bool m_useSolveConstraintObsolete
virtual void getInfo1(btConstraintInfo1 *info)
void getInfo2NonVirtual(btConstraintInfo2 *info, const btTransform &transA, const btTransform &transB, const btMatrix3x3 &invInertiaWorldA, const btMatrix3x3 &invInertiaWorldB)
virtual btScalar getParam(int num, int axis=-1) const
return the local value of parameter
int m_flags
void getInfo1NonVirtual(btConstraintInfo1 *info)
btFixedConstraint btRigidBody & rbB
btJacobianEntry
btPoint2PointConstraint(btRigidBody &rbA, btRigidBody &rbB, const btVector3 &pivotInA, const btVector3 &pivotInB)
btVector3 m_pivotInA
const btVector3 & getPivotInA() const
@ BT_P2P_FLAGS_CFM
@ BT_P2P_FLAGS_ERP
btVector3 m_pivotInB
btScalar m_erp
const btVector3 & getPivotInB() const
btScalar m_cfm
btConstraintSetting m_setting
float btScalar
The btScalar type abstracts floating point numbers, to easily switch between double and single floati...
Definition: btScalar.h:314
#define SIMD_INFINITY
Definition: btScalar.h:544
#define btAssert(x)
Definition: btScalar.h:295
btSimdScalar m_appliedImpulse
btTransform
The btTransform class supports rigid transforms with only translation and rotation and no scaling/she...
Definition: btTransform.h:30
btRigidBody & m_rbA
#define btAssertConstrParams(_par)
btTypedConstraint(btTypedConstraintType type, btRigidBody &rbA)
btRigidBody & m_rbB
@ BT_CONSTRAINT_CFM
@ BT_CONSTRAINT_ERP
@ BT_CONSTRAINT_STOP_CFM
@ BT_CONSTRAINT_STOP_ERP
@ POINT2POINT_CONSTRAINT_TYPE
btVector3
btVector3 can be used to represent 3D points and vectors. It has an un-used w component to suit 16-by...
Definition: btVector3.h:82
btScalar getInvMass() const
Definition: btRigidBody.h:263
const btTransform & getCenterOfMassTransform() const
Definition: btRigidBody.h:429
const btVector3 & getCenterOfMassPosition() const
Definition: btRigidBody.h:423
const btVector3 & getInvInertiaDiagLocal() const
Definition: btRigidBody.h:289
SyclQueue void void size_t num_bytes void
IconTextureDrawCall normal
btScalar * m_J2angularAxis
btScalar * m_J1angularAxis
btScalar * m_constraintError