Bullet Collision Detection & Physics Library: btTypedConstraint.cpp Source File

Bullet Collision Detection & Physics Library

btTypedConstraint.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
16
17 #include "btTypedConstraint.h"
18 #include "BulletDynamics/Dynamics/btRigidBody.h"
19 #include "LinearMath/btSerializer.h"
20
21
22 #define DEFAULT_DEBUGDRAW_SIZE btScalar(0.05f)
23
24 btTypedConstraint::btTypedConstraint(btTypedConstraintType type, btRigidBody& rbA)
25 :btTypedObject(type),
26 m_userConstraintType(-1),
27 m_userConstraintPtr((void*)-1),
28 m_breakingImpulseThreshold(SIMD_INFINITY),
29 m_isEnabled(true),
30 m_needsFeedback(false),
31 m_overrideNumSolverIterations(-1),
32 m_rbA(rbA),
33 m_rbB(getFixedBody()),
34 m_appliedImpulse(btScalar(0.)),
35 m_dbgDrawSize(DEFAULT_DEBUGDRAW_SIZE),
36 m_jointFeedback(0)
37 {
38 }
39
40
41 btTypedConstraint::btTypedConstraint(btTypedConstraintType type, btRigidBody& rbA,btRigidBody& rbB)
42 :btTypedObject(type),
43 m_userConstraintType(-1),
44 m_userConstraintPtr((void*)-1),
45 m_breakingImpulseThreshold(SIMD_INFINITY),
46 m_isEnabled(true),
47 m_needsFeedback(false),
48 m_overrideNumSolverIterations(-1),
49 m_rbA(rbA),
50 m_rbB(rbB),
51 m_appliedImpulse(btScalar(0.)),
52 m_dbgDrawSize(DEFAULT_DEBUGDRAW_SIZE),
53 m_jointFeedback(0)
54 {
55 }
56
57
58
59
60 btScalar btTypedConstraint::getMotorFactor(btScalar pos, btScalar lowLim, btScalar uppLim, btScalar vel, btScalar timeFact)
61 {
62 if(lowLim > uppLim)
63 {
64 return btScalar(1.0f);
65 }
66 else if(lowLim == uppLim)
67 {
68 return btScalar(0.0f);
69 }
70 btScalar lim_fact = btScalar(1.0f);
71 btScalar delta_max = vel / timeFact;
72 if(delta_max < btScalar(0.0f))
73 {
74 if((pos >= lowLim) && (pos < (lowLim - delta_max)))
75 {
76 lim_fact = (lowLim - pos) / delta_max;
77 }
78 else if(pos < lowLim)
79 {
80 lim_fact = btScalar(0.0f);
81 }
82 else
83 {
84 lim_fact = btScalar(1.0f);
85 }
86 }
87 else if(delta_max > btScalar(0.0f))
88 {
89 if((pos <= uppLim) && (pos > (uppLim - delta_max)))
90 {
91 lim_fact = (uppLim - pos) / delta_max;
92 }
93 else if(pos > uppLim)
94 {
95 lim_fact = btScalar(0.0f);
96 }
97 else
98 {
99 lim_fact = btScalar(1.0f);
100 }
101 }
102 else
103 {
104 lim_fact = btScalar(0.0f);
105 }
106 return lim_fact;
107 }
108
110 const char* btTypedConstraint::serialize(void* dataBuffer, btSerializer* serializer) const
111 {
112 btTypedConstraintData2* tcd = (btTypedConstraintData2*) dataBuffer;
113
114 tcd->m_rbA = (btRigidBodyData*)serializer->getUniquePointer(&m_rbA);
115 tcd->m_rbB = (btRigidBodyData*)serializer->getUniquePointer(&m_rbB);
116 char* name = (char*) serializer->findNameForPointer(this);
117 tcd->m_name = (char*)serializer->getUniquePointer(name);
118 if (tcd->m_name)
119 {
120 serializer->serializeName(name);
121 }
122
123 tcd->m_objectType = m_objectType;
124 tcd->m_needsFeedback = m_needsFeedback;
125 tcd->m_overrideNumSolverIterations = m_overrideNumSolverIterations;
126 tcd->m_breakingImpulseThreshold = m_breakingImpulseThreshold;
127 tcd->m_isEnabled = m_isEnabled? 1: 0;
128
129 tcd->m_userConstraintId =m_userConstraintId;
130 tcd->m_userConstraintType =m_userConstraintType;
131
132 tcd->m_appliedImpulse = m_appliedImpulse;
133 tcd->m_dbgDrawSize = m_dbgDrawSize;
134
135 tcd->m_disableCollisionsBetweenLinkedBodies = false;
136
137 int i;
138 for (i=0;i<m_rbA.getNumConstraintRefs();i++)
139 if (m_rbA.getConstraintRef(i) == this)
140 tcd->m_disableCollisionsBetweenLinkedBodies = true;
141 for (i=0;i<m_rbB.getNumConstraintRefs();i++)
142 if (m_rbB.getConstraintRef(i) == this)
143 tcd->m_disableCollisionsBetweenLinkedBodies = true;
144
145 return btTypedConstraintDataName;
146 }
147
148 btRigidBody& btTypedConstraint::getFixedBody()
149 {
150 static btRigidBody s_fixed(0, 0,0);
151 s_fixed.setMassProps(btScalar(0.),btVector3(btScalar(0.),btScalar(0.),btScalar(0.)));
152 return s_fixed;
153 }
154
155
156 void btAngularLimit::set(btScalar low, btScalar high, btScalar _softness, btScalar _biasFactor, btScalar _relaxationFactor)
157 {
158 m_halfRange = (high - low) / 2.0f;
159 m_center = btNormalizeAngle(low + m_halfRange);
160 m_softness = _softness;
161 m_biasFactor = _biasFactor;
162 m_relaxationFactor = _relaxationFactor;
163 }
164
165 void btAngularLimit::test(const btScalar angle)
166 {
167 m_correction = 0.0f;
168 m_sign = 0.0f;
169 m_solveLimit = false;
170
171 if (m_halfRange >= 0.0f)
172 {
173 btScalar deviation = btNormalizeAngle(angle - m_center);
174 if (deviation < -m_halfRange)
175 {
176 m_solveLimit = true;
177 m_correction = - (deviation + m_halfRange);
178 m_sign = +1.0f;
179 }
180 else if (deviation > m_halfRange)
181 {
182 m_solveLimit = true;
183 m_correction = m_halfRange - deviation;
184 m_sign = -1.0f;
185 }
186 }
187 }
188
189
190 btScalar btAngularLimit::getError() const
191 {
192 return m_correction * m_sign;
193 }
194
195 void btAngularLimit::fit(btScalar& angle) const
196 {
197 if (m_halfRange > 0.0f)
198 {
199 btScalar relativeAngle = btNormalizeAngle(angle - m_center);
200 if (!btEqual(relativeAngle, m_halfRange))
201 {
202 if (relativeAngle > 0.0f)
203 {
204 angle = getHigh();
205 }
206 else
207 {
208 angle = getLow();
209 }
210 }
211 }
212 }
213
214 btScalar btAngularLimit::getLow() const
215 {
216 return btNormalizeAngle(m_center - m_halfRange);
217 }
218
219 btScalar btAngularLimit::getHigh() const
220 {
221 return btNormalizeAngle(m_center + m_halfRange);
222 }
btTypedConstraint::m_rbA
btRigidBody & m_rbA
Definition: btTypedConstraint.h:102
btSerializer::getUniquePointer
virtual void * getUniquePointer(void *oldPtr)=0
btAngularLimit::m_center
btScalar m_center
Definition: btTypedConstraint.h:451
btEqual
bool btEqual(btScalar a, btScalar eps)
Definition: btScalar.h:552
DEFAULT_DEBUGDRAW_SIZE
#define DEFAULT_DEBUGDRAW_SIZE
Definition: btTypedConstraint.cpp:22
btTypedConstraint::m_overrideNumSolverIterations
int m_overrideNumSolverIterations
Definition: btTypedConstraint.h:91
btRigidBodyData
#define btRigidBodyData
Definition: btRigidBody.h:36
btRigidBody.h
btTypedConstraint::m_needsFeedback
bool m_needsFeedback
Definition: btTypedConstraint.h:90
btSerializer
Definition: btSerializer.h:68
btTypedConstraint.h
btAngularLimit::m_halfRange
btScalar m_halfRange
Definition: btTypedConstraint.h:451
btTypedConstraint::m_breakingImpulseThreshold
btScalar m_breakingImpulseThreshold
Definition: btTypedConstraint.h:88
SIMD_INFINITY
#define SIMD_INFINITY
Definition: btScalar.h:522
btTypedConstraintDataName
#define btTypedConstraintDataName
Definition: btTypedConstraint.h:29
btAngularLimit::set
void set(btScalar low, btScalar high, btScalar _softness=0.9f, btScalar _biasFactor=0.3f, btScalar _relaxationFactor=1.0f)
Sets all limit&#39;s parameters.
Definition: btTypedConstraint.cpp:156
btAngularLimit::m_relaxationFactor
btScalar m_relaxationFactor
Definition: btTypedConstraint.h:451
btAngularLimit::fit
void fit(btScalar &angle) const
Checks given angle against limit.
Definition: btTypedConstraint.cpp:195
btTypedConstraintData2
#define btTypedConstraintData2
Definition: btTypedConstraint.h:28
btAngularLimit::m_correction
btScalar m_correction
Definition: btTypedConstraint.h:451
btRigidBody
The btRigidBody is the main class for rigid body objects.
Definition: btRigidBody.h:62
btTypedConstraintType
btTypedConstraintType
Definition: btTypedConstraint.h:36
btAngularLimit::getError
btScalar getError() const
Returns correction value multiplied by sign value.
Definition: btTypedConstraint.cpp:190
btTypedConstraint::m_isEnabled
bool m_isEnabled
Definition: btTypedConstraint.h:89
btAngularLimit::getLow
btScalar getLow() const
Definition: btTypedConstraint.cpp:214
btTypedConstraint::m_dbgDrawSize
btScalar m_dbgDrawSize
Definition: btTypedConstraint.h:105
btVector3
btVector3 can be used to represent 3D points and vectors.
Definition: btVector3.h:83
btAngularLimit::getHigh
btScalar getHigh() const
Definition: btTypedConstraint.cpp:219
btNormalizeAngle
btScalar btNormalizeAngle(btScalar angleInRadians)
Definition: btScalar.h:759
btTypedConstraint::getFixedBody
static btRigidBody & getFixedBody()
Definition: btTypedConstraint.cpp:148
btRigidBody::getConstraintRef
btTypedConstraint * getConstraintRef(int index)
Definition: btRigidBody.h:518
btRigidBody::setMassProps
void setMassProps(btScalar mass, const btVector3 &inertia)
Definition: btRigidBody.cpp:233
btTypedConstraint::m_rbB
btRigidBody & m_rbB
Definition: btTypedConstraint.h:103
btTypedObject
rudimentary class to provide type info
Definition: btScalar.h:777
btTypedConstraint::serialize
virtual const char * serialize(void *dataBuffer, btSerializer *serializer) const
fills the dataBuffer and returns the struct name (and 0 on failure)
Definition: btTypedConstraint.cpp:110
btAngularLimit::m_biasFactor
btScalar m_biasFactor
Definition: btTypedConstraint.h:451
btSerializer::serializeName
virtual void serializeName(const char *ptr)=0
btTypedConstraint::getMotorFactor
btScalar getMotorFactor(btScalar pos, btScalar lowLim, btScalar uppLim, btScalar vel, btScalar timeFact)
internal method used by the constraint solver, don&#39;t use them directly
Definition: btTypedConstraint.cpp:60
btTypedConstraint::btTypedConstraint
btTypedConstraint(btTypedConstraintType type, btRigidBody &rbA)
Definition: btTypedConstraint.cpp:24
btTypedConstraint::m_appliedImpulse
btScalar m_appliedImpulse
Definition: btTypedConstraint.h:104
btAngularLimit::test
void test(const btScalar angle)
Checks conastaint angle against limit.
Definition: btTypedConstraint.cpp:165
btAngularLimit::m_softness
btScalar m_softness
Definition: btTypedConstraint.h:451
btSerializer::findNameForPointer
virtual const char * findNameForPointer(const void *ptr) const =0
btAngularLimit::m_solveLimit
bool m_solveLimit
Definition: btTypedConstraint.h:460
btAngularLimit::m_sign
btScalar m_sign
Definition: btTypedConstraint.h:451
btTypedConstraint::m_userConstraintType
int m_userConstraintType
Definition: btTypedConstraint.h:80
btScalar
float btScalar
The btScalar type abstracts floating point numbers, to easily switch between double and single floati...
Definition: btScalar.h:292
btTypedObject::m_objectType
int m_objectType
Definition: btScalar.h:783
btSerializer.h
btRigidBody::getNumConstraintRefs
int getNumConstraintRefs() const
Definition: btRigidBody.h:523
btTypedConstraint::m_userConstraintId
int m_userConstraintId
Definition: btTypedConstraint.h:84