Bullet Collision Detection & Physics Library
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 
21  : btTypedConstraint(POINT2POINT_CONSTRAINT_TYPE, rbA, rbB), m_pivotInA(pivotInA), m_pivotInB(pivotInB), m_flags(0), m_useSolveConstraintObsolete(false)
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(
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 
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 }
btTypedConstraint::btConstraintInfo2::m_damping
btScalar m_damping
Definition: btTypedConstraint.h:147
BT_CONSTRAINT_CFM
Definition: btTypedConstraint.h:53
btTypedConstraint
TypedConstraint is the baseclass for Bullet constraints and vehicles.
Definition: btTypedConstraint.h:74
btPoint2PointConstraint::getPivotInA
const btVector3 & getPivotInA() const
Definition: btPoint2PointConstraint.h:101
btRigidBody
The btRigidBody is the main class for rigid body objects.
Definition: btRigidBody.h:59
btAssertConstrParams
#define btAssertConstrParams(_par)
Definition: btTypedConstraint.h:58
BT_P2P_FLAGS_ERP
Definition: btPoint2PointConstraint.h:47
btPoint2PointConstraint::m_setting
btConstraintSetting m_setting
Definition: btPoint2PointConstraint.h:73
btScalar
float btScalar
The btScalar type abstracts floating point numbers, to easily switch between double and single floati...
Definition: btScalar.h:314
btJacobianEntry
Jacobian entry is an abstraction that allows to describe constraints it can be used in combination wi...
Definition: btJacobianEntry.h:29
btPoint2PointConstraint::getInfo2
virtual void getInfo2(btConstraintInfo2 *info)
internal method used by the constraint solver, don't use them directly
Definition: btPoint2PointConstraint.cpp:75
btTypedConstraint::btConstraintInfo2::m_J2angularAxis
btScalar * m_J2angularAxis
Definition: btTypedConstraint.h:130
btConstraintSetting::m_impulseClamp
btScalar m_impulseClamp
Definition: btPoint2PointConstraint.h:42
btRigidBody.h
btTypedConstraint::btConstraintInfo2::m_J2linearAxis
btScalar * m_J2linearAxis
Definition: btTypedConstraint.h:130
btPoint2PointConstraint::btPoint2PointConstraint
btPoint2PointConstraint(btRigidBody &rbA, btRigidBody &rbB, const btVector3 &pivotInA, const btVector3 &pivotInB)
Definition: btPoint2PointConstraint.cpp:20
btTypedConstraint::btConstraintInfo1::m_numConstraintRows
int m_numConstraintRows
Definition: btTypedConstraint.h:115
btPoint2PointConstraint::getPivotInB
const btVector3 & getPivotInB() const
Definition: btPoint2PointConstraint.h:106
btRigidBody::getCenterOfMassTransform
const btTransform & getCenterOfMassTransform() const
Definition: btRigidBody.h:394
btPoint2PointConstraint::m_useSolveConstraintObsolete
bool m_useSolveConstraintObsolete
for backwards compatibility during the transition to 'getInfo/getInfo2'
Definition: btPoint2PointConstraint.h:71
BT_CONSTRAINT_STOP_ERP
Definition: btTypedConstraint.h:52
btRigidBody::getInvInertiaDiagLocal
const btVector3 & getInvInertiaDiagLocal() const
Definition: btRigidBody.h:289
btAssert
#define btAssert(x)
Definition: btScalar.h:153
btPoint2PointConstraint::m_flags
int m_flags
Definition: btPoint2PointConstraint.h:63
btTypedConstraint::btConstraintInfo2::erp
btScalar erp
Definition: btTypedConstraint.h:124
btPoint2PointConstraint::setParam
virtual void setParam(int num, btScalar value, int axis=-1)
override the default global value of a parameter (such as ERP or CFM), optionally provide the axis (0...
Definition: btPoint2PointConstraint.cpp:152
btTransform::getBasis
btMatrix3x3 & getBasis()
Return the basis matrix for the rotation.
Definition: btTransform.h:108
btPoint2PointConstraint::getInfo2NonVirtual
void getInfo2NonVirtual(btConstraintInfo2 *info, const btTransform &body0_trans, const btTransform &body1_trans)
Definition: btPoint2PointConstraint.cpp:80
POINT2POINT_CONSTRAINT_TYPE
Definition: btTypedConstraint.h:36
btTypedConstraint::btConstraintInfo2::m_lowerLimit
btScalar * m_lowerLimit
Definition: btTypedConstraint.h:141
btTypedConstraint::btConstraintInfo2::m_constraintError
btScalar * m_constraintError
Definition: btTypedConstraint.h:138
btMatrix3x3::transpose
btMatrix3x3 transpose() const
Return the transpose of the matrix.
Definition: btMatrix3x3.h:1033
BT_CONSTRAINT_STOP_CFM
Definition: btTypedConstraint.h:54
btRigidBody::getInvMass
btScalar getInvMass() const
Definition: btRigidBody.h:263
btTransform
The btTransform class supports rigid transforms with only translation and rotation and no scaling/she...
Definition: btTransform.h:28
SIMD_INFINITY
#define SIMD_INFINITY
Definition: btScalar.h:544
btVector3
btVector3 can be used to represent 3D points and vectors.
Definition: btVector3.h:80
btTypedConstraint::btConstraintInfo1
Definition: btTypedConstraint.h:113
btTransform::getOrigin
btVector3 & getOrigin()
Return the origin vector translation.
Definition: btTransform.h:113
btPoint2PointConstraint::buildJacobian
virtual void buildJacobian()
internal method used by the constraint solver, don't use them directly
Definition: btPoint2PointConstraint.cpp:30
btTypedConstraint::btConstraintInfo2::rowskip
int rowskip
Definition: btTypedConstraint.h:133
btTypedConstraint::m_appliedImpulse
btScalar m_appliedImpulse
Definition: btTypedConstraint.h:99
btTypedConstraint::btConstraintInfo1::nub
int nub
Definition: btTypedConstraint.h:115
BT_CONSTRAINT_ERP
Definition: btTypedConstraint.h:51
btConstraintSetting::m_damping
btScalar m_damping
Definition: btPoint2PointConstraint.h:41
btPoint2PointConstraint::getInfo1
virtual void getInfo1(btConstraintInfo1 *info)
internal method used by the constraint solver, don't use them directly
Definition: btPoint2PointConstraint.cpp:56
btTypedConstraint::btConstraintInfo2::cfm
btScalar * cfm
Definition: btTypedConstraint.h:138
btPoint2PointConstraint::m_cfm
btScalar m_cfm
Definition: btPoint2PointConstraint.h:65
BT_P2P_FLAGS_CFM
Definition: btPoint2PointConstraint.h:48
btPoint2PointConstraint::m_pivotInA
btVector3 m_pivotInA
Definition: btPoint2PointConstraint.h:60
btTypedConstraint::btConstraintInfo2
Definition: btTypedConstraint.h:120
btPoint2PointConstraint::m_jac
btJacobianEntry m_jac[3]
Definition: btPoint2PointConstraint.h:58
btPoint2PointConstraint::getInfo1NonVirtual
void getInfo1NonVirtual(btConstraintInfo1 *info)
Definition: btPoint2PointConstraint.cpp:61
btTypedConstraint::m_rbB
btRigidBody & m_rbB
Definition: btTypedConstraint.h:98
btRigidBody::getCenterOfMassPosition
const btVector3 & getCenterOfMassPosition() const
Definition: btRigidBody.h:388
btTypedConstraint::btConstraintInfo2::fps
btScalar fps
Definition: btTypedConstraint.h:124
btPoint2PointConstraint::m_pivotInB
btVector3 m_pivotInB
Definition: btPoint2PointConstraint.h:61
btTypedConstraint::btConstraintInfo2::m_J1angularAxis
btScalar * m_J1angularAxis
Definition: btTypedConstraint.h:130
btPoint2PointConstraint::m_erp
btScalar m_erp
Definition: btPoint2PointConstraint.h:64
btVector3::getSkewSymmetricMatrix
void getSkewSymmetricMatrix(btVector3 *v0, btVector3 *v1, btVector3 *v2) const
Definition: btVector3.h:648
btTypedConstraint::btConstraintInfo2::m_upperLimit
btScalar * m_upperLimit
Definition: btTypedConstraint.h:141
btPoint2PointConstraint::getParam
virtual btScalar getParam(int num, int axis=-1) const
return the local value of parameter
Definition: btPoint2PointConstraint.cpp:179
btTypedConstraint::m_rbA
btRigidBody & m_rbA
Definition: btTypedConstraint.h:97
btTypedConstraint::btConstraintInfo2::m_J1linearAxis
btScalar * m_J1linearAxis
Definition: btTypedConstraint.h:130
btPoint2PointConstraint::updateRHS
void updateRHS(btScalar timeStep)
Definition: btPoint2PointConstraint.cpp:145
btPoint2PointConstraint.h