[1963] | 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 "btHingeConstraint.h" |
---|
| 18 | #include "BulletDynamics/Dynamics/btRigidBody.h" |
---|
| 19 | #include "LinearMath/btTransformUtil.h" |
---|
| 20 | #include "LinearMath/btMinMax.h" |
---|
| 21 | #include <new> |
---|
[2882] | 22 | #include "btSolverBody.h" |
---|
[1963] | 23 | |
---|
[2882] | 24 | //----------------------------------------------------------------------------- |
---|
[1963] | 25 | |
---|
[2882] | 26 | #define HINGE_USE_OBSOLETE_SOLVER false |
---|
| 27 | |
---|
| 28 | //----------------------------------------------------------------------------- |
---|
| 29 | |
---|
| 30 | |
---|
[1963] | 31 | btHingeConstraint::btHingeConstraint() |
---|
| 32 | : btTypedConstraint (HINGE_CONSTRAINT_TYPE), |
---|
[2882] | 33 | m_enableAngularMotor(false), |
---|
| 34 | m_useSolveConstraintObsolete(HINGE_USE_OBSOLETE_SOLVER), |
---|
| 35 | m_useReferenceFrameA(false) |
---|
[1963] | 36 | { |
---|
[2882] | 37 | m_referenceSign = m_useReferenceFrameA ? btScalar(-1.f) : btScalar(1.f); |
---|
[1963] | 38 | } |
---|
| 39 | |
---|
[2882] | 40 | //----------------------------------------------------------------------------- |
---|
| 41 | |
---|
[1963] | 42 | btHingeConstraint::btHingeConstraint(btRigidBody& rbA,btRigidBody& rbB, const btVector3& pivotInA,const btVector3& pivotInB, |
---|
[2882] | 43 | btVector3& axisInA,btVector3& axisInB, bool useReferenceFrameA) |
---|
[1963] | 44 | :btTypedConstraint(HINGE_CONSTRAINT_TYPE, rbA,rbB), |
---|
| 45 | m_angularOnly(false), |
---|
[2882] | 46 | m_enableAngularMotor(false), |
---|
| 47 | m_useSolveConstraintObsolete(HINGE_USE_OBSOLETE_SOLVER), |
---|
| 48 | m_useReferenceFrameA(useReferenceFrameA) |
---|
[1963] | 49 | { |
---|
| 50 | m_rbAFrame.getOrigin() = pivotInA; |
---|
| 51 | |
---|
| 52 | // since no frame is given, assume this to be zero angle and just pick rb transform axis |
---|
| 53 | btVector3 rbAxisA1 = rbA.getCenterOfMassTransform().getBasis().getColumn(0); |
---|
| 54 | |
---|
| 55 | btVector3 rbAxisA2; |
---|
| 56 | btScalar projection = axisInA.dot(rbAxisA1); |
---|
| 57 | if (projection >= 1.0f - SIMD_EPSILON) { |
---|
| 58 | rbAxisA1 = -rbA.getCenterOfMassTransform().getBasis().getColumn(2); |
---|
| 59 | rbAxisA2 = rbA.getCenterOfMassTransform().getBasis().getColumn(1); |
---|
| 60 | } else if (projection <= -1.0f + SIMD_EPSILON) { |
---|
| 61 | rbAxisA1 = rbA.getCenterOfMassTransform().getBasis().getColumn(2); |
---|
| 62 | rbAxisA2 = rbA.getCenterOfMassTransform().getBasis().getColumn(1); |
---|
| 63 | } else { |
---|
| 64 | rbAxisA2 = axisInA.cross(rbAxisA1); |
---|
| 65 | rbAxisA1 = rbAxisA2.cross(axisInA); |
---|
| 66 | } |
---|
| 67 | |
---|
| 68 | m_rbAFrame.getBasis().setValue( rbAxisA1.getX(),rbAxisA2.getX(),axisInA.getX(), |
---|
| 69 | rbAxisA1.getY(),rbAxisA2.getY(),axisInA.getY(), |
---|
| 70 | rbAxisA1.getZ(),rbAxisA2.getZ(),axisInA.getZ() ); |
---|
| 71 | |
---|
| 72 | btQuaternion rotationArc = shortestArcQuat(axisInA,axisInB); |
---|
| 73 | btVector3 rbAxisB1 = quatRotate(rotationArc,rbAxisA1); |
---|
| 74 | btVector3 rbAxisB2 = axisInB.cross(rbAxisB1); |
---|
| 75 | |
---|
| 76 | m_rbBFrame.getOrigin() = pivotInB; |
---|
[2882] | 77 | m_rbBFrame.getBasis().setValue( rbAxisB1.getX(),rbAxisB2.getX(),axisInB.getX(), |
---|
| 78 | rbAxisB1.getY(),rbAxisB2.getY(),axisInB.getY(), |
---|
| 79 | rbAxisB1.getZ(),rbAxisB2.getZ(),axisInB.getZ() ); |
---|
[1963] | 80 | |
---|
| 81 | //start with free |
---|
| 82 | m_lowerLimit = btScalar(1e30); |
---|
| 83 | m_upperLimit = btScalar(-1e30); |
---|
| 84 | m_biasFactor = 0.3f; |
---|
| 85 | m_relaxationFactor = 1.0f; |
---|
| 86 | m_limitSoftness = 0.9f; |
---|
| 87 | m_solveLimit = false; |
---|
[2882] | 88 | m_referenceSign = m_useReferenceFrameA ? btScalar(-1.f) : btScalar(1.f); |
---|
[1963] | 89 | } |
---|
| 90 | |
---|
[2882] | 91 | //----------------------------------------------------------------------------- |
---|
[1963] | 92 | |
---|
[2882] | 93 | btHingeConstraint::btHingeConstraint(btRigidBody& rbA,const btVector3& pivotInA,btVector3& axisInA, bool useReferenceFrameA) |
---|
| 94 | :btTypedConstraint(HINGE_CONSTRAINT_TYPE, rbA), m_angularOnly(false), m_enableAngularMotor(false), |
---|
| 95 | m_useSolveConstraintObsolete(HINGE_USE_OBSOLETE_SOLVER), |
---|
| 96 | m_useReferenceFrameA(useReferenceFrameA) |
---|
[1963] | 97 | { |
---|
| 98 | |
---|
| 99 | // since no frame is given, assume this to be zero angle and just pick rb transform axis |
---|
| 100 | // fixed axis in worldspace |
---|
| 101 | btVector3 rbAxisA1, rbAxisA2; |
---|
| 102 | btPlaneSpace1(axisInA, rbAxisA1, rbAxisA2); |
---|
| 103 | |
---|
| 104 | m_rbAFrame.getOrigin() = pivotInA; |
---|
| 105 | m_rbAFrame.getBasis().setValue( rbAxisA1.getX(),rbAxisA2.getX(),axisInA.getX(), |
---|
| 106 | rbAxisA1.getY(),rbAxisA2.getY(),axisInA.getY(), |
---|
| 107 | rbAxisA1.getZ(),rbAxisA2.getZ(),axisInA.getZ() ); |
---|
| 108 | |
---|
[2882] | 109 | btVector3 axisInB = rbA.getCenterOfMassTransform().getBasis() * axisInA; |
---|
[1963] | 110 | |
---|
| 111 | btQuaternion rotationArc = shortestArcQuat(axisInA,axisInB); |
---|
| 112 | btVector3 rbAxisB1 = quatRotate(rotationArc,rbAxisA1); |
---|
| 113 | btVector3 rbAxisB2 = axisInB.cross(rbAxisB1); |
---|
| 114 | |
---|
| 115 | |
---|
| 116 | m_rbBFrame.getOrigin() = rbA.getCenterOfMassTransform()(pivotInA); |
---|
| 117 | m_rbBFrame.getBasis().setValue( rbAxisB1.getX(),rbAxisB2.getX(),axisInB.getX(), |
---|
| 118 | rbAxisB1.getY(),rbAxisB2.getY(),axisInB.getY(), |
---|
| 119 | rbAxisB1.getZ(),rbAxisB2.getZ(),axisInB.getZ() ); |
---|
| 120 | |
---|
| 121 | //start with free |
---|
| 122 | m_lowerLimit = btScalar(1e30); |
---|
| 123 | m_upperLimit = btScalar(-1e30); |
---|
| 124 | m_biasFactor = 0.3f; |
---|
| 125 | m_relaxationFactor = 1.0f; |
---|
| 126 | m_limitSoftness = 0.9f; |
---|
| 127 | m_solveLimit = false; |
---|
[2882] | 128 | m_referenceSign = m_useReferenceFrameA ? btScalar(-1.f) : btScalar(1.f); |
---|
[1963] | 129 | } |
---|
| 130 | |
---|
[2882] | 131 | //----------------------------------------------------------------------------- |
---|
| 132 | |
---|
[1963] | 133 | btHingeConstraint::btHingeConstraint(btRigidBody& rbA,btRigidBody& rbB, |
---|
[2882] | 134 | const btTransform& rbAFrame, const btTransform& rbBFrame, bool useReferenceFrameA) |
---|
[1963] | 135 | :btTypedConstraint(HINGE_CONSTRAINT_TYPE, rbA,rbB),m_rbAFrame(rbAFrame),m_rbBFrame(rbBFrame), |
---|
| 136 | m_angularOnly(false), |
---|
[2882] | 137 | m_enableAngularMotor(false), |
---|
| 138 | m_useSolveConstraintObsolete(HINGE_USE_OBSOLETE_SOLVER), |
---|
| 139 | m_useReferenceFrameA(useReferenceFrameA) |
---|
[1963] | 140 | { |
---|
| 141 | //start with free |
---|
| 142 | m_lowerLimit = btScalar(1e30); |
---|
| 143 | m_upperLimit = btScalar(-1e30); |
---|
| 144 | m_biasFactor = 0.3f; |
---|
| 145 | m_relaxationFactor = 1.0f; |
---|
| 146 | m_limitSoftness = 0.9f; |
---|
| 147 | m_solveLimit = false; |
---|
[2882] | 148 | m_referenceSign = m_useReferenceFrameA ? btScalar(-1.f) : btScalar(1.f); |
---|
[1963] | 149 | } |
---|
| 150 | |
---|
[2882] | 151 | //----------------------------------------------------------------------------- |
---|
[1963] | 152 | |
---|
[2882] | 153 | btHingeConstraint::btHingeConstraint(btRigidBody& rbA, const btTransform& rbAFrame, bool useReferenceFrameA) |
---|
[1963] | 154 | :btTypedConstraint(HINGE_CONSTRAINT_TYPE, rbA),m_rbAFrame(rbAFrame),m_rbBFrame(rbAFrame), |
---|
| 155 | m_angularOnly(false), |
---|
[2882] | 156 | m_enableAngularMotor(false), |
---|
| 157 | m_useSolveConstraintObsolete(HINGE_USE_OBSOLETE_SOLVER), |
---|
| 158 | m_useReferenceFrameA(useReferenceFrameA) |
---|
[1963] | 159 | { |
---|
| 160 | ///not providing rigidbody B means implicitly using worldspace for body B |
---|
| 161 | |
---|
| 162 | m_rbBFrame.getOrigin() = m_rbA.getCenterOfMassTransform()(m_rbAFrame.getOrigin()); |
---|
| 163 | |
---|
| 164 | //start with free |
---|
| 165 | m_lowerLimit = btScalar(1e30); |
---|
| 166 | m_upperLimit = btScalar(-1e30); |
---|
| 167 | m_biasFactor = 0.3f; |
---|
| 168 | m_relaxationFactor = 1.0f; |
---|
| 169 | m_limitSoftness = 0.9f; |
---|
| 170 | m_solveLimit = false; |
---|
[2882] | 171 | m_referenceSign = m_useReferenceFrameA ? btScalar(-1.f) : btScalar(1.f); |
---|
[1963] | 172 | } |
---|
| 173 | |
---|
[2882] | 174 | //----------------------------------------------------------------------------- |
---|
| 175 | |
---|
[1963] | 176 | void btHingeConstraint::buildJacobian() |
---|
| 177 | { |
---|
[2882] | 178 | if (m_useSolveConstraintObsolete) |
---|
[1963] | 179 | { |
---|
[2882] | 180 | m_appliedImpulse = btScalar(0.); |
---|
[1963] | 181 | |
---|
[2882] | 182 | if (!m_angularOnly) |
---|
[1963] | 183 | { |
---|
[2882] | 184 | btVector3 pivotAInW = m_rbA.getCenterOfMassTransform()*m_rbAFrame.getOrigin(); |
---|
| 185 | btVector3 pivotBInW = m_rbB.getCenterOfMassTransform()*m_rbBFrame.getOrigin(); |
---|
| 186 | btVector3 relPos = pivotBInW - pivotAInW; |
---|
[1963] | 187 | |
---|
[2882] | 188 | btVector3 normal[3]; |
---|
| 189 | if (relPos.length2() > SIMD_EPSILON) |
---|
| 190 | { |
---|
| 191 | normal[0] = relPos.normalized(); |
---|
| 192 | } |
---|
| 193 | else |
---|
| 194 | { |
---|
| 195 | normal[0].setValue(btScalar(1.0),0,0); |
---|
| 196 | } |
---|
[1963] | 197 | |
---|
[2882] | 198 | btPlaneSpace1(normal[0], normal[1], normal[2]); |
---|
| 199 | |
---|
| 200 | for (int i=0;i<3;i++) |
---|
| 201 | { |
---|
| 202 | new (&m_jac[i]) btJacobianEntry( |
---|
[1963] | 203 | m_rbA.getCenterOfMassTransform().getBasis().transpose(), |
---|
| 204 | m_rbB.getCenterOfMassTransform().getBasis().transpose(), |
---|
| 205 | pivotAInW - m_rbA.getCenterOfMassPosition(), |
---|
| 206 | pivotBInW - m_rbB.getCenterOfMassPosition(), |
---|
| 207 | normal[i], |
---|
| 208 | m_rbA.getInvInertiaDiagLocal(), |
---|
| 209 | m_rbA.getInvMass(), |
---|
| 210 | m_rbB.getInvInertiaDiagLocal(), |
---|
| 211 | m_rbB.getInvMass()); |
---|
[2882] | 212 | } |
---|
[1963] | 213 | } |
---|
| 214 | |
---|
[2882] | 215 | //calculate two perpendicular jointAxis, orthogonal to hingeAxis |
---|
| 216 | //these two jointAxis require equal angular velocities for both bodies |
---|
[1963] | 217 | |
---|
[2882] | 218 | //this is unused for now, it's a todo |
---|
| 219 | btVector3 jointAxis0local; |
---|
| 220 | btVector3 jointAxis1local; |
---|
[1963] | 221 | |
---|
[2882] | 222 | btPlaneSpace1(m_rbAFrame.getBasis().getColumn(2),jointAxis0local,jointAxis1local); |
---|
[1963] | 223 | |
---|
[2882] | 224 | getRigidBodyA().getCenterOfMassTransform().getBasis() * m_rbAFrame.getBasis().getColumn(2); |
---|
| 225 | btVector3 jointAxis0 = getRigidBodyA().getCenterOfMassTransform().getBasis() * jointAxis0local; |
---|
| 226 | btVector3 jointAxis1 = getRigidBodyA().getCenterOfMassTransform().getBasis() * jointAxis1local; |
---|
| 227 | btVector3 hingeAxisWorld = getRigidBodyA().getCenterOfMassTransform().getBasis() * m_rbAFrame.getBasis().getColumn(2); |
---|
| 228 | |
---|
| 229 | new (&m_jacAng[0]) btJacobianEntry(jointAxis0, |
---|
| 230 | m_rbA.getCenterOfMassTransform().getBasis().transpose(), |
---|
| 231 | m_rbB.getCenterOfMassTransform().getBasis().transpose(), |
---|
| 232 | m_rbA.getInvInertiaDiagLocal(), |
---|
| 233 | m_rbB.getInvInertiaDiagLocal()); |
---|
[1963] | 234 | |
---|
[2882] | 235 | new (&m_jacAng[1]) btJacobianEntry(jointAxis1, |
---|
| 236 | m_rbA.getCenterOfMassTransform().getBasis().transpose(), |
---|
| 237 | m_rbB.getCenterOfMassTransform().getBasis().transpose(), |
---|
| 238 | m_rbA.getInvInertiaDiagLocal(), |
---|
| 239 | m_rbB.getInvInertiaDiagLocal()); |
---|
[1963] | 240 | |
---|
[2882] | 241 | new (&m_jacAng[2]) btJacobianEntry(hingeAxisWorld, |
---|
| 242 | m_rbA.getCenterOfMassTransform().getBasis().transpose(), |
---|
| 243 | m_rbB.getCenterOfMassTransform().getBasis().transpose(), |
---|
| 244 | m_rbA.getInvInertiaDiagLocal(), |
---|
| 245 | m_rbB.getInvInertiaDiagLocal()); |
---|
[1963] | 246 | |
---|
[2882] | 247 | // clear accumulator |
---|
| 248 | m_accLimitImpulse = btScalar(0.); |
---|
[1963] | 249 | |
---|
[2882] | 250 | // test angular limit |
---|
| 251 | testLimit(); |
---|
[1963] | 252 | |
---|
[2882] | 253 | //Compute K = J*W*J' for hinge axis |
---|
| 254 | btVector3 axisA = getRigidBodyA().getCenterOfMassTransform().getBasis() * m_rbAFrame.getBasis().getColumn(2); |
---|
| 255 | m_kHinge = 1.0f / (getRigidBodyA().computeAngularImpulseDenominator(axisA) + |
---|
| 256 | getRigidBodyB().computeAngularImpulseDenominator(axisA)); |
---|
| 257 | |
---|
| 258 | } |
---|
| 259 | } |
---|
| 260 | |
---|
| 261 | //----------------------------------------------------------------------------- |
---|
| 262 | |
---|
| 263 | void btHingeConstraint::getInfo1(btConstraintInfo1* info) |
---|
| 264 | { |
---|
| 265 | if (m_useSolveConstraintObsolete) |
---|
[1963] | 266 | { |
---|
[2882] | 267 | info->m_numConstraintRows = 0; |
---|
| 268 | info->nub = 0; |
---|
| 269 | } |
---|
| 270 | else |
---|
| 271 | { |
---|
| 272 | info->m_numConstraintRows = 5; // Fixed 3 linear + 2 angular |
---|
| 273 | info->nub = 1; |
---|
| 274 | //prepare constraint |
---|
| 275 | testLimit(); |
---|
| 276 | if(getSolveLimit() || getEnableAngularMotor()) |
---|
[1963] | 277 | { |
---|
[2882] | 278 | info->m_numConstraintRows++; // limit 3rd anguar as well |
---|
| 279 | info->nub--; |
---|
[1963] | 280 | } |
---|
| 281 | } |
---|
[2882] | 282 | } // btHingeConstraint::getInfo1 () |
---|
[1963] | 283 | |
---|
[2882] | 284 | //----------------------------------------------------------------------------- |
---|
[1963] | 285 | |
---|
[2882] | 286 | void btHingeConstraint::getInfo2 (btConstraintInfo2* info) |
---|
| 287 | { |
---|
| 288 | btAssert(!m_useSolveConstraintObsolete); |
---|
| 289 | int i, s = info->rowskip; |
---|
| 290 | // transforms in world space |
---|
| 291 | btTransform trA = m_rbA.getCenterOfMassTransform()*m_rbAFrame; |
---|
| 292 | btTransform trB = m_rbB.getCenterOfMassTransform()*m_rbBFrame; |
---|
| 293 | // pivot point |
---|
| 294 | btVector3 pivotAInW = trA.getOrigin(); |
---|
| 295 | btVector3 pivotBInW = trB.getOrigin(); |
---|
| 296 | // linear (all fixed) |
---|
| 297 | info->m_J1linearAxis[0] = 1; |
---|
| 298 | info->m_J1linearAxis[s + 1] = 1; |
---|
| 299 | info->m_J1linearAxis[2 * s + 2] = 1; |
---|
| 300 | btVector3 a1 = pivotAInW - m_rbA.getCenterOfMassTransform().getOrigin(); |
---|
| 301 | { |
---|
| 302 | btVector3* angular0 = (btVector3*)(info->m_J1angularAxis); |
---|
| 303 | btVector3* angular1 = (btVector3*)(info->m_J1angularAxis + s); |
---|
| 304 | btVector3* angular2 = (btVector3*)(info->m_J1angularAxis + 2 * s); |
---|
| 305 | btVector3 a1neg = -a1; |
---|
| 306 | a1neg.getSkewSymmetricMatrix(angular0,angular1,angular2); |
---|
| 307 | } |
---|
| 308 | btVector3 a2 = pivotBInW - m_rbB.getCenterOfMassTransform().getOrigin(); |
---|
| 309 | { |
---|
| 310 | btVector3* angular0 = (btVector3*)(info->m_J2angularAxis); |
---|
| 311 | btVector3* angular1 = (btVector3*)(info->m_J2angularAxis + s); |
---|
| 312 | btVector3* angular2 = (btVector3*)(info->m_J2angularAxis + 2 * s); |
---|
| 313 | a2.getSkewSymmetricMatrix(angular0,angular1,angular2); |
---|
| 314 | } |
---|
| 315 | // linear RHS |
---|
| 316 | btScalar k = info->fps * info->erp; |
---|
| 317 | for(i = 0; i < 3; i++) |
---|
| 318 | { |
---|
| 319 | info->m_constraintError[i * s] = k * (pivotBInW[i] - pivotAInW[i]); |
---|
| 320 | } |
---|
| 321 | // make rotations around X and Y equal |
---|
| 322 | // the hinge axis should be the only unconstrained |
---|
| 323 | // rotational axis, the angular velocity of the two bodies perpendicular to |
---|
| 324 | // the hinge axis should be equal. thus the constraint equations are |
---|
| 325 | // p*w1 - p*w2 = 0 |
---|
| 326 | // q*w1 - q*w2 = 0 |
---|
| 327 | // where p and q are unit vectors normal to the hinge axis, and w1 and w2 |
---|
| 328 | // are the angular velocity vectors of the two bodies. |
---|
| 329 | // get hinge axis (Z) |
---|
| 330 | btVector3 ax1 = trA.getBasis().getColumn(2); |
---|
| 331 | // get 2 orthos to hinge axis (X, Y) |
---|
| 332 | btVector3 p = trA.getBasis().getColumn(0); |
---|
| 333 | btVector3 q = trA.getBasis().getColumn(1); |
---|
| 334 | // set the two hinge angular rows |
---|
| 335 | int s3 = 3 * info->rowskip; |
---|
| 336 | int s4 = 4 * info->rowskip; |
---|
| 337 | |
---|
| 338 | info->m_J1angularAxis[s3 + 0] = p[0]; |
---|
| 339 | info->m_J1angularAxis[s3 + 1] = p[1]; |
---|
| 340 | info->m_J1angularAxis[s3 + 2] = p[2]; |
---|
| 341 | info->m_J1angularAxis[s4 + 0] = q[0]; |
---|
| 342 | info->m_J1angularAxis[s4 + 1] = q[1]; |
---|
| 343 | info->m_J1angularAxis[s4 + 2] = q[2]; |
---|
| 344 | |
---|
| 345 | info->m_J2angularAxis[s3 + 0] = -p[0]; |
---|
| 346 | info->m_J2angularAxis[s3 + 1] = -p[1]; |
---|
| 347 | info->m_J2angularAxis[s3 + 2] = -p[2]; |
---|
| 348 | info->m_J2angularAxis[s4 + 0] = -q[0]; |
---|
| 349 | info->m_J2angularAxis[s4 + 1] = -q[1]; |
---|
| 350 | info->m_J2angularAxis[s4 + 2] = -q[2]; |
---|
| 351 | // compute the right hand side of the constraint equation. set relative |
---|
| 352 | // body velocities along p and q to bring the hinge back into alignment. |
---|
| 353 | // if ax1,ax2 are the unit length hinge axes as computed from body1 and |
---|
| 354 | // body2, we need to rotate both bodies along the axis u = (ax1 x ax2). |
---|
| 355 | // if `theta' is the angle between ax1 and ax2, we need an angular velocity |
---|
| 356 | // along u to cover angle erp*theta in one step : |
---|
| 357 | // |angular_velocity| = angle/time = erp*theta / stepsize |
---|
| 358 | // = (erp*fps) * theta |
---|
| 359 | // angular_velocity = |angular_velocity| * (ax1 x ax2) / |ax1 x ax2| |
---|
| 360 | // = (erp*fps) * theta * (ax1 x ax2) / sin(theta) |
---|
| 361 | // ...as ax1 and ax2 are unit length. if theta is smallish, |
---|
| 362 | // theta ~= sin(theta), so |
---|
| 363 | // angular_velocity = (erp*fps) * (ax1 x ax2) |
---|
| 364 | // ax1 x ax2 is in the plane space of ax1, so we project the angular |
---|
| 365 | // velocity to p and q to find the right hand side. |
---|
| 366 | btVector3 ax2 = trB.getBasis().getColumn(2); |
---|
| 367 | btVector3 u = ax1.cross(ax2); |
---|
| 368 | info->m_constraintError[s3] = k * u.dot(p); |
---|
| 369 | info->m_constraintError[s4] = k * u.dot(q); |
---|
| 370 | // check angular limits |
---|
| 371 | int nrow = 4; // last filled row |
---|
| 372 | int srow; |
---|
| 373 | btScalar limit_err = btScalar(0.0); |
---|
| 374 | int limit = 0; |
---|
| 375 | if(getSolveLimit()) |
---|
| 376 | { |
---|
| 377 | limit_err = m_correction * m_referenceSign; |
---|
| 378 | limit = (limit_err > btScalar(0.0)) ? 1 : 2; |
---|
| 379 | } |
---|
| 380 | // if the hinge has joint limits or motor, add in the extra row |
---|
| 381 | int powered = 0; |
---|
| 382 | if(getEnableAngularMotor()) |
---|
| 383 | { |
---|
| 384 | powered = 1; |
---|
| 385 | } |
---|
| 386 | if(limit || powered) |
---|
| 387 | { |
---|
| 388 | nrow++; |
---|
| 389 | srow = nrow * info->rowskip; |
---|
| 390 | info->m_J1angularAxis[srow+0] = ax1[0]; |
---|
| 391 | info->m_J1angularAxis[srow+1] = ax1[1]; |
---|
| 392 | info->m_J1angularAxis[srow+2] = ax1[2]; |
---|
| 393 | |
---|
| 394 | info->m_J2angularAxis[srow+0] = -ax1[0]; |
---|
| 395 | info->m_J2angularAxis[srow+1] = -ax1[1]; |
---|
| 396 | info->m_J2angularAxis[srow+2] = -ax1[2]; |
---|
| 397 | |
---|
| 398 | btScalar lostop = getLowerLimit(); |
---|
| 399 | btScalar histop = getUpperLimit(); |
---|
| 400 | if(limit && (lostop == histop)) |
---|
| 401 | { // the joint motor is ineffective |
---|
| 402 | powered = 0; |
---|
| 403 | } |
---|
| 404 | info->m_constraintError[srow] = btScalar(0.0f); |
---|
| 405 | if(powered) |
---|
| 406 | { |
---|
| 407 | info->cfm[srow] = btScalar(0.0); |
---|
| 408 | btScalar mot_fact = getMotorFactor(m_hingeAngle, lostop, histop, m_motorTargetVelocity, info->fps * info->erp); |
---|
| 409 | info->m_constraintError[srow] += mot_fact * m_motorTargetVelocity * m_referenceSign; |
---|
| 410 | info->m_lowerLimit[srow] = - m_maxMotorImpulse; |
---|
| 411 | info->m_upperLimit[srow] = m_maxMotorImpulse; |
---|
| 412 | } |
---|
| 413 | if(limit) |
---|
| 414 | { |
---|
| 415 | k = info->fps * info->erp; |
---|
| 416 | info->m_constraintError[srow] += k * limit_err; |
---|
| 417 | info->cfm[srow] = btScalar(0.0); |
---|
| 418 | if(lostop == histop) |
---|
| 419 | { |
---|
| 420 | // limited low and high simultaneously |
---|
| 421 | info->m_lowerLimit[srow] = -SIMD_INFINITY; |
---|
| 422 | info->m_upperLimit[srow] = SIMD_INFINITY; |
---|
| 423 | } |
---|
| 424 | else if(limit == 1) |
---|
| 425 | { // low limit |
---|
| 426 | info->m_lowerLimit[srow] = 0; |
---|
| 427 | info->m_upperLimit[srow] = SIMD_INFINITY; |
---|
| 428 | } |
---|
| 429 | else |
---|
| 430 | { // high limit |
---|
| 431 | info->m_lowerLimit[srow] = -SIMD_INFINITY; |
---|
| 432 | info->m_upperLimit[srow] = 0; |
---|
| 433 | } |
---|
| 434 | // bounce (we'll use slider parameter abs(1.0 - m_dampingLimAng) for that) |
---|
| 435 | btScalar bounce = m_relaxationFactor; |
---|
| 436 | if(bounce > btScalar(0.0)) |
---|
| 437 | { |
---|
| 438 | btScalar vel = m_rbA.getAngularVelocity().dot(ax1); |
---|
| 439 | vel -= m_rbB.getAngularVelocity().dot(ax1); |
---|
| 440 | // only apply bounce if the velocity is incoming, and if the |
---|
| 441 | // resulting c[] exceeds what we already have. |
---|
| 442 | if(limit == 1) |
---|
| 443 | { // low limit |
---|
| 444 | if(vel < 0) |
---|
| 445 | { |
---|
| 446 | btScalar newc = -bounce * vel; |
---|
| 447 | if(newc > info->m_constraintError[srow]) |
---|
| 448 | { |
---|
| 449 | info->m_constraintError[srow] = newc; |
---|
| 450 | } |
---|
| 451 | } |
---|
| 452 | } |
---|
| 453 | else |
---|
| 454 | { // high limit - all those computations are reversed |
---|
| 455 | if(vel > 0) |
---|
| 456 | { |
---|
| 457 | btScalar newc = -bounce * vel; |
---|
| 458 | if(newc < info->m_constraintError[srow]) |
---|
| 459 | { |
---|
| 460 | info->m_constraintError[srow] = newc; |
---|
| 461 | } |
---|
| 462 | } |
---|
| 463 | } |
---|
| 464 | } |
---|
| 465 | info->m_constraintError[srow] *= m_biasFactor; |
---|
| 466 | } // if(limit) |
---|
| 467 | } // if angular limit or powered |
---|
[1963] | 468 | } |
---|
| 469 | |
---|
[2882] | 470 | //----------------------------------------------------------------------------- |
---|
| 471 | |
---|
| 472 | void btHingeConstraint::solveConstraintObsolete(btSolverBody& bodyA,btSolverBody& bodyB,btScalar timeStep) |
---|
[1963] | 473 | { |
---|
| 474 | |
---|
[2882] | 475 | ///for backwards compatibility during the transition to 'getInfo/getInfo2' |
---|
| 476 | if (m_useSolveConstraintObsolete) |
---|
| 477 | { |
---|
[1963] | 478 | |
---|
[2882] | 479 | btVector3 pivotAInW = m_rbA.getCenterOfMassTransform()*m_rbAFrame.getOrigin(); |
---|
| 480 | btVector3 pivotBInW = m_rbB.getCenterOfMassTransform()*m_rbBFrame.getOrigin(); |
---|
[1963] | 481 | |
---|
[2882] | 482 | btScalar tau = btScalar(0.3); |
---|
[1963] | 483 | |
---|
[2882] | 484 | //linear part |
---|
| 485 | if (!m_angularOnly) |
---|
| 486 | { |
---|
| 487 | btVector3 rel_pos1 = pivotAInW - m_rbA.getCenterOfMassPosition(); |
---|
| 488 | btVector3 rel_pos2 = pivotBInW - m_rbB.getCenterOfMassPosition(); |
---|
[1963] | 489 | |
---|
[2882] | 490 | btVector3 vel1,vel2; |
---|
| 491 | bodyA.getVelocityInLocalPointObsolete(rel_pos1,vel1); |
---|
| 492 | bodyB.getVelocityInLocalPointObsolete(rel_pos2,vel2); |
---|
| 493 | btVector3 vel = vel1 - vel2; |
---|
[1963] | 494 | |
---|
[2882] | 495 | for (int i=0;i<3;i++) |
---|
| 496 | { |
---|
| 497 | const btVector3& normal = m_jac[i].m_linearJointAxis; |
---|
| 498 | btScalar jacDiagABInv = btScalar(1.) / m_jac[i].getDiagonal(); |
---|
| 499 | |
---|
| 500 | btScalar rel_vel; |
---|
| 501 | rel_vel = normal.dot(vel); |
---|
| 502 | //positional error (zeroth order error) |
---|
| 503 | btScalar depth = -(pivotAInW - pivotBInW).dot(normal); //this is the error projected on the normal |
---|
| 504 | btScalar impulse = depth*tau/timeStep * jacDiagABInv - rel_vel * jacDiagABInv; |
---|
| 505 | m_appliedImpulse += impulse; |
---|
| 506 | btVector3 impulse_vector = normal * impulse; |
---|
| 507 | btVector3 ftorqueAxis1 = rel_pos1.cross(normal); |
---|
| 508 | btVector3 ftorqueAxis2 = rel_pos2.cross(normal); |
---|
| 509 | bodyA.applyImpulse(normal*m_rbA.getInvMass(), m_rbA.getInvInertiaTensorWorld()*ftorqueAxis1,impulse); |
---|
| 510 | bodyB.applyImpulse(normal*m_rbB.getInvMass(), m_rbB.getInvInertiaTensorWorld()*ftorqueAxis2,-impulse); |
---|
| 511 | } |
---|
[1963] | 512 | } |
---|
| 513 | |
---|
[2882] | 514 | |
---|
| 515 | { |
---|
| 516 | ///solve angular part |
---|
[1963] | 517 | |
---|
[2882] | 518 | // get axes in world space |
---|
| 519 | btVector3 axisA = getRigidBodyA().getCenterOfMassTransform().getBasis() * m_rbAFrame.getBasis().getColumn(2); |
---|
| 520 | btVector3 axisB = getRigidBodyB().getCenterOfMassTransform().getBasis() * m_rbBFrame.getBasis().getColumn(2); |
---|
[1963] | 521 | |
---|
[2882] | 522 | btVector3 angVelA; |
---|
| 523 | bodyA.getAngularVelocity(angVelA); |
---|
| 524 | btVector3 angVelB; |
---|
| 525 | bodyB.getAngularVelocity(angVelB); |
---|
[1963] | 526 | |
---|
[2882] | 527 | btVector3 angVelAroundHingeAxisA = axisA * axisA.dot(angVelA); |
---|
| 528 | btVector3 angVelAroundHingeAxisB = axisB * axisB.dot(angVelB); |
---|
[1963] | 529 | |
---|
[2882] | 530 | btVector3 angAorthog = angVelA - angVelAroundHingeAxisA; |
---|
| 531 | btVector3 angBorthog = angVelB - angVelAroundHingeAxisB; |
---|
| 532 | btVector3 velrelOrthog = angAorthog-angBorthog; |
---|
[1963] | 533 | { |
---|
[2882] | 534 | |
---|
[1963] | 535 | |
---|
[2882] | 536 | //solve orthogonal angular velocity correction |
---|
| 537 | btScalar relaxation = btScalar(1.); |
---|
| 538 | btScalar len = velrelOrthog.length(); |
---|
| 539 | if (len > btScalar(0.00001)) |
---|
| 540 | { |
---|
| 541 | btVector3 normal = velrelOrthog.normalized(); |
---|
| 542 | btScalar denom = getRigidBodyA().computeAngularImpulseDenominator(normal) + |
---|
| 543 | getRigidBodyB().computeAngularImpulseDenominator(normal); |
---|
| 544 | // scale for mass and relaxation |
---|
| 545 | //velrelOrthog *= (btScalar(1.)/denom) * m_relaxationFactor; |
---|
[1963] | 546 | |
---|
[2882] | 547 | bodyA.applyImpulse(btVector3(0,0,0), m_rbA.getInvInertiaTensorWorld()*velrelOrthog,-(btScalar(1.)/denom)); |
---|
| 548 | bodyB.applyImpulse(btVector3(0,0,0), m_rbB.getInvInertiaTensorWorld()*velrelOrthog,(btScalar(1.)/denom)); |
---|
[1963] | 549 | |
---|
[2882] | 550 | } |
---|
[1963] | 551 | |
---|
[2882] | 552 | //solve angular positional correction |
---|
| 553 | btVector3 angularError = axisA.cross(axisB) *(btScalar(1.)/timeStep); |
---|
| 554 | btScalar len2 = angularError.length(); |
---|
| 555 | if (len2>btScalar(0.00001)) |
---|
| 556 | { |
---|
| 557 | btVector3 normal2 = angularError.normalized(); |
---|
| 558 | btScalar denom2 = getRigidBodyA().computeAngularImpulseDenominator(normal2) + |
---|
| 559 | getRigidBodyB().computeAngularImpulseDenominator(normal2); |
---|
| 560 | //angularError *= (btScalar(1.)/denom2) * relaxation; |
---|
| 561 | |
---|
| 562 | bodyA.applyImpulse(btVector3(0,0,0), m_rbA.getInvInertiaTensorWorld()*angularError,(btScalar(1.)/denom2)); |
---|
| 563 | bodyB.applyImpulse(btVector3(0,0,0), m_rbB.getInvInertiaTensorWorld()*angularError,-(btScalar(1.)/denom2)); |
---|
[1963] | 564 | |
---|
[2882] | 565 | } |
---|
| 566 | |
---|
| 567 | |
---|
[1963] | 568 | |
---|
| 569 | |
---|
[2882] | 570 | |
---|
| 571 | // solve limit |
---|
| 572 | if (m_solveLimit) |
---|
| 573 | { |
---|
| 574 | btScalar amplitude = ( (angVelB - angVelA).dot( axisA )*m_relaxationFactor + m_correction* (btScalar(1.)/timeStep)*m_biasFactor ) * m_limitSign; |
---|
| 575 | |
---|
| 576 | btScalar impulseMag = amplitude * m_kHinge; |
---|
| 577 | |
---|
| 578 | // Clamp the accumulated impulse |
---|
| 579 | btScalar temp = m_accLimitImpulse; |
---|
| 580 | m_accLimitImpulse = btMax(m_accLimitImpulse + impulseMag, btScalar(0) ); |
---|
| 581 | impulseMag = m_accLimitImpulse - temp; |
---|
| 582 | |
---|
| 583 | |
---|
| 584 | |
---|
| 585 | bodyA.applyImpulse(btVector3(0,0,0), m_rbA.getInvInertiaTensorWorld()*axisA,impulseMag * m_limitSign); |
---|
| 586 | bodyB.applyImpulse(btVector3(0,0,0), m_rbB.getInvInertiaTensorWorld()*axisA,-(impulseMag * m_limitSign)); |
---|
| 587 | |
---|
| 588 | } |
---|
[1963] | 589 | } |
---|
| 590 | |
---|
[2882] | 591 | //apply motor |
---|
| 592 | if (m_enableAngularMotor) |
---|
| 593 | { |
---|
| 594 | //todo: add limits too |
---|
| 595 | btVector3 angularLimit(0,0,0); |
---|
[1963] | 596 | |
---|
[2882] | 597 | btVector3 velrel = angVelAroundHingeAxisA - angVelAroundHingeAxisB; |
---|
| 598 | btScalar projRelVel = velrel.dot(axisA); |
---|
[1963] | 599 | |
---|
[2882] | 600 | btScalar desiredMotorVel = m_motorTargetVelocity; |
---|
| 601 | btScalar motor_relvel = desiredMotorVel - projRelVel; |
---|
[1963] | 602 | |
---|
[2882] | 603 | btScalar unclippedMotorImpulse = m_kHinge * motor_relvel;; |
---|
| 604 | //todo: should clip against accumulated impulse |
---|
| 605 | btScalar clippedMotorImpulse = unclippedMotorImpulse > m_maxMotorImpulse ? m_maxMotorImpulse : unclippedMotorImpulse; |
---|
| 606 | clippedMotorImpulse = clippedMotorImpulse < -m_maxMotorImpulse ? -m_maxMotorImpulse : clippedMotorImpulse; |
---|
| 607 | btVector3 motorImp = clippedMotorImpulse * axisA; |
---|
[1963] | 608 | |
---|
[2882] | 609 | bodyA.applyImpulse(btVector3(0,0,0), m_rbA.getInvInertiaTensorWorld()*axisA,clippedMotorImpulse); |
---|
| 610 | bodyB.applyImpulse(btVector3(0,0,0), m_rbB.getInvInertiaTensorWorld()*axisA,-clippedMotorImpulse); |
---|
| 611 | |
---|
| 612 | } |
---|
[1963] | 613 | } |
---|
| 614 | } |
---|
| 615 | |
---|
| 616 | } |
---|
| 617 | |
---|
[2882] | 618 | //----------------------------------------------------------------------------- |
---|
| 619 | |
---|
[1963] | 620 | void btHingeConstraint::updateRHS(btScalar timeStep) |
---|
| 621 | { |
---|
| 622 | (void)timeStep; |
---|
| 623 | |
---|
| 624 | } |
---|
| 625 | |
---|
[2882] | 626 | //----------------------------------------------------------------------------- |
---|
| 627 | |
---|
[1963] | 628 | btScalar btHingeConstraint::getHingeAngle() |
---|
| 629 | { |
---|
| 630 | const btVector3 refAxis0 = getRigidBodyA().getCenterOfMassTransform().getBasis() * m_rbAFrame.getBasis().getColumn(0); |
---|
| 631 | const btVector3 refAxis1 = getRigidBodyA().getCenterOfMassTransform().getBasis() * m_rbAFrame.getBasis().getColumn(1); |
---|
| 632 | const btVector3 swingAxis = getRigidBodyB().getCenterOfMassTransform().getBasis() * m_rbBFrame.getBasis().getColumn(1); |
---|
[2882] | 633 | btScalar angle = btAtan2Fast(swingAxis.dot(refAxis0), swingAxis.dot(refAxis1)); |
---|
| 634 | return m_referenceSign * angle; |
---|
[1963] | 635 | } |
---|
| 636 | |
---|
[2882] | 637 | //----------------------------------------------------------------------------- |
---|
| 638 | |
---|
| 639 | void btHingeConstraint::testLimit() |
---|
| 640 | { |
---|
| 641 | // Compute limit information |
---|
| 642 | m_hingeAngle = getHingeAngle(); |
---|
| 643 | m_correction = btScalar(0.); |
---|
| 644 | m_limitSign = btScalar(0.); |
---|
| 645 | m_solveLimit = false; |
---|
| 646 | if (m_lowerLimit <= m_upperLimit) |
---|
| 647 | { |
---|
| 648 | if (m_hingeAngle <= m_lowerLimit) |
---|
| 649 | { |
---|
| 650 | m_correction = (m_lowerLimit - m_hingeAngle); |
---|
| 651 | m_limitSign = 1.0f; |
---|
| 652 | m_solveLimit = true; |
---|
| 653 | } |
---|
| 654 | else if (m_hingeAngle >= m_upperLimit) |
---|
| 655 | { |
---|
| 656 | m_correction = m_upperLimit - m_hingeAngle; |
---|
| 657 | m_limitSign = -1.0f; |
---|
| 658 | m_solveLimit = true; |
---|
| 659 | } |
---|
| 660 | } |
---|
| 661 | return; |
---|
| 662 | } // btHingeConstraint::testLimit() |
---|
| 663 | |
---|
| 664 | //----------------------------------------------------------------------------- |
---|
| 665 | //----------------------------------------------------------------------------- |
---|
| 666 | //----------------------------------------------------------------------------- |
---|