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 "btTypedConstraint.h"
17 #include "BulletDynamics/Dynamics/btRigidBody.h"
18 #include "LinearMath/btSerializer.h"
19
20 #define DEFAULT_DEBUGDRAW_SIZE btScalar(0.05f)
21
btTypedConstraint(btTypedConstraintType type,btRigidBody & rbA)22 btTypedConstraint::btTypedConstraint(btTypedConstraintType type, btRigidBody& rbA)
23 : btTypedObject(type),
24 m_userConstraintType(-1),
25 m_userConstraintPtr((void*)-1),
26 m_breakingImpulseThreshold(SIMD_INFINITY),
27 m_isEnabled(true),
28 m_needsFeedback(false),
29 m_overrideNumSolverIterations(-1),
30 m_rbA(rbA),
31 m_rbB(getFixedBody()),
32 m_appliedImpulse(btScalar(0.)),
33 m_dbgDrawSize(DEFAULT_DEBUGDRAW_SIZE),
34 m_jointFeedback(0)
35 {
36 }
37
btTypedConstraint(btTypedConstraintType type,btRigidBody & rbA,btRigidBody & rbB)38 btTypedConstraint::btTypedConstraint(btTypedConstraintType type, btRigidBody& rbA, btRigidBody& rbB)
39 : btTypedObject(type),
40 m_userConstraintType(-1),
41 m_userConstraintPtr((void*)-1),
42 m_breakingImpulseThreshold(SIMD_INFINITY),
43 m_isEnabled(true),
44 m_needsFeedback(false),
45 m_overrideNumSolverIterations(-1),
46 m_rbA(rbA),
47 m_rbB(rbB),
48 m_appliedImpulse(btScalar(0.)),
49 m_dbgDrawSize(DEFAULT_DEBUGDRAW_SIZE),
50 m_jointFeedback(0)
51 {
52 }
53
getMotorFactor(btScalar pos,btScalar lowLim,btScalar uppLim,btScalar vel,btScalar timeFact)54 btScalar btTypedConstraint::getMotorFactor(btScalar pos, btScalar lowLim, btScalar uppLim, btScalar vel, btScalar timeFact)
55 {
56 if (lowLim > uppLim)
57 {
58 return btScalar(1.0f);
59 }
60 else if (lowLim == uppLim)
61 {
62 return btScalar(0.0f);
63 }
64 btScalar lim_fact = btScalar(1.0f);
65 btScalar delta_max = vel / timeFact;
66 if (delta_max < btScalar(0.0f))
67 {
68 if ((pos >= lowLim) && (pos < (lowLim - delta_max)))
69 {
70 lim_fact = (lowLim - pos) / delta_max;
71 }
72 else if (pos < lowLim)
73 {
74 lim_fact = btScalar(0.0f);
75 }
76 else
77 {
78 lim_fact = btScalar(1.0f);
79 }
80 }
81 else if (delta_max > btScalar(0.0f))
82 {
83 if ((pos <= uppLim) && (pos > (uppLim - delta_max)))
84 {
85 lim_fact = (uppLim - pos) / delta_max;
86 }
87 else if (pos > uppLim)
88 {
89 lim_fact = btScalar(0.0f);
90 }
91 else
92 {
93 lim_fact = btScalar(1.0f);
94 }
95 }
96 else
97 {
98 lim_fact = btScalar(0.0f);
99 }
100 return lim_fact;
101 }
102
103 ///fills the dataBuffer and returns the struct name (and 0 on failure)
serialize(void * dataBuffer,btSerializer * serializer) const104 const char* btTypedConstraint::serialize(void* dataBuffer, btSerializer* serializer) const
105 {
106 btTypedConstraintData2* tcd = (btTypedConstraintData2*)dataBuffer;
107
108 tcd->m_rbA = (btRigidBodyData*)serializer->getUniquePointer(&m_rbA);
109 tcd->m_rbB = (btRigidBodyData*)serializer->getUniquePointer(&m_rbB);
110 char* name = (char*)serializer->findNameForPointer(this);
111 tcd->m_name = (char*)serializer->getUniquePointer(name);
112 if (tcd->m_name)
113 {
114 serializer->serializeName(name);
115 }
116
117 tcd->m_objectType = m_objectType;
118 tcd->m_needsFeedback = m_needsFeedback;
119 tcd->m_overrideNumSolverIterations = m_overrideNumSolverIterations;
120 tcd->m_breakingImpulseThreshold = m_breakingImpulseThreshold;
121 tcd->m_isEnabled = m_isEnabled ? 1 : 0;
122
123 tcd->m_userConstraintId = m_userConstraintId;
124 tcd->m_userConstraintType = m_userConstraintType;
125
126 tcd->m_appliedImpulse = m_appliedImpulse;
127 tcd->m_dbgDrawSize = m_dbgDrawSize;
128
129 tcd->m_disableCollisionsBetweenLinkedBodies = false;
130
131 int i;
132 for (i = 0; i < m_rbA.getNumConstraintRefs(); i++)
133 if (m_rbA.getConstraintRef(i) == this)
134 tcd->m_disableCollisionsBetweenLinkedBodies = true;
135 for (i = 0; i < m_rbB.getNumConstraintRefs(); i++)
136 if (m_rbB.getConstraintRef(i) == this)
137 tcd->m_disableCollisionsBetweenLinkedBodies = true;
138
139 return btTypedConstraintDataName;
140 }
141
getFixedBody()142 btRigidBody& btTypedConstraint::getFixedBody()
143 {
144 static btRigidBody s_fixed(0, 0, 0);
145 s_fixed.setMassProps(btScalar(0.), btVector3(btScalar(0.), btScalar(0.), btScalar(0.)));
146 return s_fixed;
147 }
148
set(btScalar low,btScalar high,btScalar _softness,btScalar _biasFactor,btScalar _relaxationFactor)149 void btAngularLimit::set(btScalar low, btScalar high, btScalar _softness, btScalar _biasFactor, btScalar _relaxationFactor)
150 {
151 m_halfRange = (high - low) / 2.0f;
152 m_center = btNormalizeAngle(low + m_halfRange);
153 m_softness = _softness;
154 m_biasFactor = _biasFactor;
155 m_relaxationFactor = _relaxationFactor;
156 }
157
test(const btScalar angle)158 void btAngularLimit::test(const btScalar angle)
159 {
160 m_correction = 0.0f;
161 m_sign = 0.0f;
162 m_solveLimit = false;
163
164 if (m_halfRange >= 0.0f)
165 {
166 btScalar deviation = btNormalizeAngle(angle - m_center);
167 if (deviation < -m_halfRange)
168 {
169 m_solveLimit = true;
170 m_correction = -(deviation + m_halfRange);
171 m_sign = +1.0f;
172 }
173 else if (deviation > m_halfRange)
174 {
175 m_solveLimit = true;
176 m_correction = m_halfRange - deviation;
177 m_sign = -1.0f;
178 }
179 }
180 }
181
getError() const182 btScalar btAngularLimit::getError() const
183 {
184 return m_correction * m_sign;
185 }
186
fit(btScalar & angle) const187 void btAngularLimit::fit(btScalar& angle) const
188 {
189 if (m_halfRange > 0.0f)
190 {
191 btScalar relativeAngle = btNormalizeAngle(angle - m_center);
192 if (!btEqual(relativeAngle, m_halfRange))
193 {
194 if (relativeAngle > 0.0f)
195 {
196 angle = getHigh();
197 }
198 else
199 {
200 angle = getLow();
201 }
202 }
203 }
204 }
205
getLow() const206 btScalar btAngularLimit::getLow() const
207 {
208 return btNormalizeAngle(m_center - m_halfRange);
209 }
210
getHigh() const211 btScalar btAngularLimit::getHigh() const
212 {
213 return btNormalizeAngle(m_center + m_halfRange);
214 }
215