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.3f) 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 109 ///fills the dataBuffer and returns the struct name (and 0 on failure) 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 } 223
