| /trunk/src/BulletDynamics/ConstraintSolver/btHingeConstraint.cpp r2096 | /trunk/src/BulletDynamics/ConstraintSolver/btHingeConstraint.cpp r2138 | ||
| 1 | /* | 1 | /* |
|---|---|---|---|
| 2 | Bullet Continuous Collision Detection and Physics Library | 2 | Bullet Continuous Collision Detection and Physics Library |
| 3 | Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/ | 3 | Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/ |
| 4 | 4 | ||
| 5 | This software is provided 'as-is', without any express or implied warranty. | 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. | 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, | 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, | 8 | including commercial applications, and to alter it and redistribute it freely, |
| 9 | subject to the following restrictions: | 9 | subject to the following restrictions: |
| 10 | 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. | 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. | 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. | 13 | 3. This notice may not be removed or altered from any source distribution. |
| 14 | */ | 14 | */ |
| 15 | 15 | ||
| 16 | 16 | ||
| 17 | #include "btHingeConstraint.h" | 17 | #include "btHingeConstraint.h" |
| 18 | #include "BulletDynamics/Dynamics/btRigidBody.h" | 18 | #include "BulletDynamics/Dynamics/btRigidBody.h" |
| 19 | #include "LinearMath/btTransformUtil.h" | 19 | #include "LinearMath/btTransformUtil.h" |
| 20 | #include "LinearMath/btMinMax.h" | 20 | #include "LinearMath/btMinMax.h" |
| 21 | #include <new> | 21 | #include <new> |
| 22 | #include "btSolverBody.h" | 22 | #include "btSolverBody.h" |
| 23 | 23 | ||
| 24 | 24 | ||
| 25 | 25 | ||
| 26 | //#define HINGE_USE_OBSOLETE_SOLVER false | 26 | //#define HINGE_USE_OBSOLETE_SOLVER false |
| 27 | #define HINGE_USE_OBSOLETE_SOLVER false | 27 | #define HINGE_USE_OBSOLETE_SOLVER false |
| 28 | 28 | ||
| 29 | #define HINGE_USE_FRAME_OFFSET true | 29 | #define HINGE_USE_FRAME_OFFSET true |
| 30 | 30 | ||
| 31 | #ifndef __SPU__ | 31 | #ifndef __SPU__ |
| 32 | 32 | ||
| 33 | 33 | ||
| 34 | 34 | ||
| 35 | 35 | ||
| 36 | 36 | ||
| 37 | btHingeConstraint::btHingeConstraint(btRigidBody& rbA,btRigidBody& rbB, const btVector3& pivotInA,const btVector3& pivotInB, | 37 | btHingeConstraint::btHingeConstraint(btRigidBody& rbA,btRigidBody& rbB, const btVector3& pivotInA,const btVector3& pivotInB, |
| 38 | const btVector3& axisInA,const btVector3& axisInB, bool useReferenceFrameA) | 38 | const btVector3& axisInA,const btVector3& axisInB, bool useReferenceFrameA) |
| 39 | :btTypedConstraint(HINGE_CONSTRAINT_TYPE, rbA,rbB), | 39 | :btTypedConstraint(HINGE_CONSTRAINT_TYPE, rbA,rbB), |
| 40 | m_angularOnly(false), | 40 | m_angularOnly(false), |
| 41 | m_enableAngularMotor(false), | 41 | m_enableAngularMotor(false), |
| 42 | m_useSolveConstraintObsolete(HINGE_USE_OBSOLETE_SOLVER), | 42 | m_useSolveConstraintObsolete(HINGE_USE_OBSOLETE_SOLVER), |
| 43 | m_useOffsetForConstraintFrame(HINGE_USE_FRAME_OFFSET), | 43 | m_useOffsetForConstraintFrame(HINGE_USE_FRAME_OFFSET), |
| 44 | m_useReferenceFrameA(useReferenceFrameA), | 44 | m_useReferenceFrameA(useReferenceFrameA), |
| 45 | m_flags(0) | 45 | m_flags(0) |
| 46 | { | 46 | { |
| 47 | m_rbAFrame.getOrigin() = pivotInA; | 47 | m_rbAFrame.getOrigin() = pivotInA; |
| 48 | 48 | ||
| 49 | // since no frame is given, assume this to be zero angle and just pick rb transform axis | 49 | // since no frame is given, assume this to be zero angle and just pick rb transform axis |
| 50 | btVector3 rbAxisA1 = rbA.getCenterOfMassTransform().getBasis().getColumn(0); | 50 | btVector3 rbAxisA1 = rbA.getCenterOfMassTransform().getBasis().getColumn(0); |
| 51 | 51 | ||
| 52 | btVector3 rbAxisA2; | 52 | btVector3 rbAxisA2; |
| 53 | btScalar projection = axisInA.dot(rbAxisA1); | 53 | btScalar projection = axisInA.dot(rbAxisA1); |
| 54 | if (projection >= 1.0f - SIMD_EPSILON) { | 54 | if (projection >= 1.0f - SIMD_EPSILON) { |
| 55 | rbAxisA1 = -rbA.getCenterOfMassTransform().getBasis().getColumn(2); | 55 | rbAxisA1 = -rbA.getCenterOfMassTransform().getBasis().getColumn(2); |
| 56 | rbAxisA2 = rbA.getCenterOfMassTransform().getBasis().getColumn(1); | 56 | rbAxisA2 = rbA.getCenterOfMassTransform().getBasis().getColumn(1); |
| 57 | } else if (projection <= -1.0f + SIMD_EPSILON) { | 57 | } else if (projection <= -1.0f + SIMD_EPSILON) { |
| 58 | rbAxisA1 = rbA.getCenterOfMassTransform().getBasis().getColumn(2); | 58 | rbAxisA1 = rbA.getCenterOfMassTransform().getBasis().getColumn(2); |
| 59 | rbAxisA2 = rbA.getCenterOfMassTransform().getBasis().getColumn(1); | 59 | rbAxisA2 = rbA.getCenterOfMassTransform().getBasis().getColumn(1); |
| 60 | } else { | 60 | } else { |
| 61 | rbAxisA2 = axisInA.cross(rbAxisA1); | 61 | rbAxisA2 = axisInA.cross(rbAxisA1); |
| 62 | rbAxisA1 = rbAxisA2.cross(axisInA); | 62 | rbAxisA1 = rbAxisA2.cross(axisInA); |
| 63 | } | 63 | } |
| 64 | 64 | ||
| 65 | m_rbAFrame.getBasis().setValue( rbAxisA1.getX(),rbAxisA2.getX(),axisInA.getX(), | 65 | m_rbAFrame.getBasis().setValue( rbAxisA1.getX(),rbAxisA2.getX(),axisInA.getX(), |
| 66 | rbAxisA1.getY(),rbAxisA2.getY(),axisInA.getY(), | 66 | rbAxisA1.getY(),rbAxisA2.getY(),axisInA.getY(), |
| 67 | rbAxisA1.getZ(),rbAxisA2.getZ(),axisInA.getZ() ); | 67 | rbAxisA1.getZ(),rbAxisA2.getZ(),axisInA.getZ() ); |
| 68 | 68 | ||
| 69 | btQuaternion rotationArc = shortestArcQuat(axisInA,axisInB); | 69 | btQuaternion rotationArc = shortestArcQuat(axisInA,axisInB); |
| 70 | btVector3 rbAxisB1 = quatRotate(rotationArc,rbAxisA1); | 70 | btVector3 rbAxisB1 = quatRotate(rotationArc,rbAxisA1); |
| 71 | btVector3 rbAxisB2 = axisInB.cross(rbAxisB1); | 71 | btVector3 rbAxisB2 = axisInB.cross(rbAxisB1); |
| 72 | 72 | ||
| 73 | m_rbBFrame.getOrigin() = pivotInB; | 73 | m_rbBFrame.getOrigin() = pivotInB; |
| 74 | m_rbBFrame.getBasis().setValue( rbAxisB1.getX(),rbAxisB2.getX(),axisInB.getX(), | 74 | m_rbBFrame.getBasis().setValue( rbAxisB1.getX(),rbAxisB2.getX(),axisInB.getX(), |
| 75 | rbAxisB1.getY(),rbAxisB2.getY(),axisInB.getY(), | 75 | rbAxisB1.getY(),rbAxisB2.getY(),axisInB.getY(), |
| 76 | rbAxisB1.getZ(),rbAxisB2.getZ(),axisInB.getZ() ); | 76 | rbAxisB1.getZ(),rbAxisB2.getZ(),axisInB.getZ() ); |
| 77 | 77 | ||
| 78 | //start with free | 78 | //start with free |
| 79 | m_lowerLimit = btScalar(1.0f); | 79 | m_lowerLimit = btScalar(1.0f); |
| 80 | m_upperLimit = btScalar(-1.0f); | 80 | m_upperLimit = btScalar(-1.0f); |
| 81 | m_biasFactor = 0.3f; | 81 | m_biasFactor = 0.3f; |
| 82 | m_relaxationFactor = 1.0f; | 82 | m_relaxationFactor = 1.0f; |
| 83 | m_limitSoftness = 0.9f; | 83 | m_limitSoftness = 0.9f; |
| 84 | m_solveLimit = false; | 84 | m_solveLimit = false; |
| 85 | m_referenceSign = m_useReferenceFrameA ? btScalar(-1.f) : btScalar(1.f); | 85 | m_referenceSign = m_useReferenceFrameA ? btScalar(-1.f) : btScalar(1.f); |
| 86 | } | 86 | } |
| 87 | 87 | ||
| 88 | 88 | ||
| 89 | 89 | ||
| 90 | btHingeConstraint::btHingeConstraint(btRigidBody& rbA,const btVector3& pivotInA,const btVector3& axisInA, bool useReferenceFrameA) | 90 | btHingeConstraint::btHingeConstraint(btRigidBody& rbA,const btVector3& pivotInA,const btVector3& axisInA, bool useReferenceFrameA) |
| 91 | :btTypedConstraint(HINGE_CONSTRAINT_TYPE, rbA), m_angularOnly(false), m_enableAngularMotor(false), | 91 | :btTypedConstraint(HINGE_CONSTRAINT_TYPE, rbA), m_angularOnly(false), m_enableAngularMotor(false), |
| 92 | m_useSolveConstraintObsolete(HINGE_USE_OBSOLETE_SOLVER), | 92 | m_useSolveConstraintObsolete(HINGE_USE_OBSOLETE_SOLVER), |
| 93 | m_useOffsetForConstraintFrame(HINGE_USE_FRAME_OFFSET), | 93 | m_useOffsetForConstraintFrame(HINGE_USE_FRAME_OFFSET), |
| 94 | m_useReferenceFrameA(useReferenceFrameA), | 94 | m_useReferenceFrameA(useReferenceFrameA), |
| 95 | m_flags(0) | 95 | m_flags(0) |
| 96 | { | 96 | { |
| 97 | 97 | ||
| 98 | // since no frame is given, assume this to be zero angle and just pick rb transform axis | 98 | // since no frame is given, assume this to be zero angle and just pick rb transform axis |
| 99 | // fixed axis in worldspace | 99 | // fixed axis in worldspace |
| 100 | btVector3 rbAxisA1, rbAxisA2; | 100 | btVector3 rbAxisA1, rbAxisA2; |
| 101 | btPlaneSpace1(axisInA, rbAxisA1, rbAxisA2); | 101 | btPlaneSpace1(axisInA, rbAxisA1, rbAxisA2); |
| 102 | 102 | ||
| 103 | m_rbAFrame.getOrigin() = pivotInA; | 103 | m_rbAFrame.getOrigin() = pivotInA; |
| 104 | m_rbAFrame.getBasis().setValue( rbAxisA1.getX(),rbAxisA2.getX(),axisInA.getX(), | 104 | m_rbAFrame.getBasis().setValue( rbAxisA1.getX(),rbAxisA2.getX(),axisInA.getX(), |
| 105 | rbAxisA1.getY(),rbAxisA2.getY(),axisInA.getY(), | 105 | rbAxisA1.getY(),rbAxisA2.getY(),axisInA.getY(), |
| 106 | rbAxisA1.getZ(),rbAxisA2.getZ(),axisInA.getZ() ); | 106 | rbAxisA1.getZ(),rbAxisA2.getZ(),axisInA.getZ() ); |
| 107 | 107 | ||
| 108 | btVector3 axisInB = rbA.getCenterOfMassTransform().getBasis() * axisInA; | 108 | btVector3 axisInB = rbA.getCenterOfMassTransform().getBasis() * axisInA; |
| 109 | 109 | ||
| 110 | btQuaternion rotationArc = shortestArcQuat(axisInA,axisInB); | 110 | btQuaternion rotationArc = shortestArcQuat(axisInA,axisInB); |
| 111 | btVector3 rbAxisB1 = quatRotate(rotationArc,rbAxisA1); | 111 | btVector3 rbAxisB1 = quatRotate(rotationArc,rbAxisA1); |
| 112 | btVector3 rbAxisB2 = axisInB.cross(rbAxisB1); | 112 | btVector3 rbAxisB2 = axisInB.cross(rbAxisB1); |
| 113 | 113 | ||
| 114 | 114 | ||
| 115 | m_rbBFrame.getOrigin() = rbA.getCenterOfMassTransform()(pivotInA); | 115 | m_rbBFrame.getOrigin() = rbA.getCenterOfMassTransform()(pivotInA); |
| 116 | m_rbBFrame.getBasis().setValue( rbAxisB1.getX(),rbAxisB2.getX(),axisInB.getX(), | 116 | m_rbBFrame.getBasis().setValue( rbAxisB1.getX(),rbAxisB2.getX(),axisInB.getX(), |
| 117 | rbAxisB1.getY(),rbAxisB2.getY(),axisInB.getY(), | 117 | rbAxisB1.getY(),rbAxisB2.getY(),axisInB.getY(), |
| 118 | rbAxisB1.getZ(),rbAxisB2.getZ(),axisInB.getZ() ); | 118 | rbAxisB1.getZ(),rbAxisB2.getZ(),axisInB.getZ() ); |
| 119 | 119 | ||
| 120 | //start with free | 120 | //start with free |
| 121 | m_lowerLimit = btScalar(1.0f); | 121 | m_lowerLimit = btScalar(1.0f); |
| 122 | m_upperLimit = btScalar(-1.0f); | 122 | m_upperLimit = btScalar(-1.0f); |
| 123 | m_biasFactor = 0.3f; | 123 | m_biasFactor = 0.3f; |
| 124 | m_relaxationFactor = 1.0f; | 124 | m_relaxationFactor = 1.0f; |
| 125 | m_limitSoftness = 0.9f; | 125 | m_limitSoftness = 0.9f; |
| 126 | m_solveLimit = false; | 126 | m_solveLimit = false; |
| 127 | m_referenceSign = m_useReferenceFrameA ? btScalar(-1.f) : btScalar(1.f); | 127 | m_referenceSign = m_useReferenceFrameA ? btScalar(-1.f) : btScalar(1.f); |
| 128 | } | 128 | } |
| 129 | 129 | ||
| 130 | 130 | ||
| 131 | 131 | ||
| 132 | btHingeConstraint::btHingeConstraint(btRigidBody& rbA,btRigidBody& rbB, | 132 | btHingeConstraint::btHingeConstraint(btRigidBody& rbA,btRigidBody& rbB, |
| 133 | const btTransform& rbAFrame, const btTransform& rbBFrame, bool useReferenceFrameA) | 133 | const btTransform& rbAFrame, const btTransform& rbBFrame, bool useReferenceFrameA) |
| 134 | :btTypedConstraint(HINGE_CONSTRAINT_TYPE, rbA,rbB),m_rbAFrame(rbAFrame),m_rbBFrame(rbBFrame), | 134 | :btTypedConstraint(HINGE_CONSTRAINT_TYPE, rbA,rbB),m_rbAFrame(rbAFrame),m_rbBFrame(rbBFrame), |
| 135 | m_angularOnly(false), | 135 | m_angularOnly(false), |
| 136 | m_enableAngularMotor(false), | 136 | m_enableAngularMotor(false), |
| 137 | m_useSolveConstraintObsolete(HINGE_USE_OBSOLETE_SOLVER), | 137 | m_useSolveConstraintObsolete(HINGE_USE_OBSOLETE_SOLVER), |
| 138 | m_useOffsetForConstraintFrame(HINGE_USE_FRAME_OFFSET), | 138 | m_useOffsetForConstraintFrame(HINGE_USE_FRAME_OFFSET), |
| 139 | m_useReferenceFrameA(useReferenceFrameA), | 139 | m_useReferenceFrameA(useReferenceFrameA), |
| 140 | m_flags(0) | 140 | m_flags(0) |
| 141 | { | 141 | { |
| 142 | //start with free | 142 | //start with free |
| 143 | m_lowerLimit = btScalar(1.0f); | 143 | m_lowerLimit = btScalar(1.0f); |
| 144 | m_upperLimit = btScalar(-1.0f); | 144 | m_upperLimit = btScalar(-1.0f); |
| 145 | m_biasFactor = 0.3f; | 145 | m_biasFactor = 0.3f; |
| 146 | m_relaxationFactor = 1.0f; | 146 | m_relaxationFactor = 1.0f; |
| 147 | m_limitSoftness = 0.9f; | 147 | m_limitSoftness = 0.9f; |
| 148 | m_solveLimit = false; | 148 | m_solveLimit = false; |
| 149 | m_referenceSign = m_useReferenceFrameA ? btScalar(-1.f) : btScalar(1.f); | 149 | m_referenceSign = m_useReferenceFrameA ? btScalar(-1.f) : btScalar(1.f); |
| 150 | } | 150 | } |
| 151 | 151 | ||
| 152 | 152 | ||
| 153 | 153 | ||
| 154 | btHingeConstraint::btHingeConstraint(btRigidBody& rbA, const btTransform& rbAFrame, bool useReferenceFrameA) | 154 | btHingeConstraint::btHingeConstraint(btRigidBody& rbA, const btTransform& rbAFrame, bool useReferenceFrameA) |
| 155 | :btTypedConstraint(HINGE_CONSTRAINT_TYPE, rbA),m_rbAFrame(rbAFrame),m_rbBFrame(rbAFrame), | 155 | :btTypedConstraint(HINGE_CONSTRAINT_TYPE, rbA),m_rbAFrame(rbAFrame),m_rbBFrame(rbAFrame), |
| 156 | m_angularOnly(false), | 156 | m_angularOnly(false), |
| 157 | m_enableAngularMotor(false), | 157 | m_enableAngularMotor(false), |
| 158 | m_useSolveConstraintObsolete(HINGE_USE_OBSOLETE_SOLVER), | 158 | m_useSolveConstraintObsolete(HINGE_USE_OBSOLETE_SOLVER), |
| 159 | m_useOffsetForConstraintFrame(HINGE_USE_FRAME_OFFSET), | 159 | m_useOffsetForConstraintFrame(HINGE_USE_FRAME_OFFSET), |
| 160 | m_useReferenceFrameA(useReferenceFrameA), | 160 | m_useReferenceFrameA(useReferenceFrameA), |
| 161 | m_flags(0) | 161 | m_flags(0) |
| 162 | { | 162 | { |
| 163 | ///not providing rigidbody B means implicitly using worldspace for body B | 163 | ///not providing rigidbody B means implicitly using worldspace for body B |
| 164 | 164 | ||
| 165 | m_rbBFrame.getOrigin() = m_rbA.getCenterOfMassTransform()(m_rbAFrame.getOrigin()); | 165 | m_rbBFrame.getOrigin() = m_rbA.getCenterOfMassTransform()(m_rbAFrame.getOrigin()); |
| 166 | 166 | ||
| 167 | //start with free | 167 | //start with free |
| 168 | m_lowerLimit = btScalar(1.0f); | 168 | m_lowerLimit = btScalar(1.0f); |
| 169 | m_upperLimit = btScalar(-1.0f); | 169 | m_upperLimit = btScalar(-1.0f); |
| 170 | m_biasFactor = 0.3f; | 170 | m_biasFactor = 0.3f; |
| 171 | m_relaxationFactor = 1.0f; | 171 | m_relaxationFactor = 1.0f; |
| 172 | m_limitSoftness = 0.9f; | 172 | m_limitSoftness = 0.9f; |
| 173 | m_solveLimit = false; | 173 | m_solveLimit = false; |
| 174 | m_referenceSign = m_useReferenceFrameA ? btScalar(-1.f) : btScalar(1.f); | 174 | m_referenceSign = m_useReferenceFrameA ? btScalar(-1.f) : btScalar(1.f); |
| 175 | } | 175 | } |
| 176 | 176 | ||
| 177 | 177 | ||
| 178 | 178 | ||
| 179 | void btHingeConstraint::buildJacobian() | 179 | void btHingeConstraint::buildJacobian() |
| 180 | { | 180 | { |
| 181 | if (m_useSolveConstraintObsolete) | 181 | if (m_useSolveConstraintObsolete) |
| 182 | { | 182 | { |
| 183 | m_appliedImpulse = btScalar(0.); | 183 | m_appliedImpulse = btScalar(0.); |
| 184 | m_accMotorImpulse = btScalar(0.); | 184 | m_accMotorImpulse = btScalar(0.); |
| 185 | 185 | ||
| 186 | if (!m_angularOnly) | 186 | if (!m_angularOnly) |
| 187 | { | 187 | { |
| 188 | btVector3 pivotAInW = m_rbA.getCenterOfMassTransform()*m_rbAFrame.getOrigin(); | 188 | btVector3 pivotAInW = m_rbA.getCenterOfMassTransform()*m_rbAFrame.getOrigin(); |
| 189 | btVector3 pivotBInW = m_rbB.getCenterOfMassTransform()*m_rbBFrame.getOrigin(); | 189 | btVector3 pivotBInW = m_rbB.getCenterOfMassTransform()*m_rbBFrame.getOrigin(); |
| 190 | btVector3 relPos = pivotBInW - pivotAInW; | 190 | btVector3 relPos = pivotBInW - pivotAInW; |
| 191 | 191 | ||
| 192 | btVector3 normal[3]; | 192 | btVector3 normal[3]; |
| 193 | if (relPos.length2() > SIMD_EPSILON) | 193 | if (relPos.length2() > SIMD_EPSILON) |
| 194 | { | 194 | { |
| 195 | normal[0] = relPos.normalized(); | 195 | normal[0] = relPos.normalized(); |
| 196 | } | 196 | } |
| 197 | else | 197 | else |
| 198 | { | 198 | { |
| 199 | normal[0].setValue(btScalar(1.0),0,0); | 199 | normal[0].setValue(btScalar(1.0),0,0); |
| 200 | } | 200 | } |
| 201 | 201 | ||
| 202 | btPlaneSpace1(normal[0], normal[1], normal[2]); | 202 | btPlaneSpace1(normal[0], normal[1], normal[2]); |
| 203 | 203 | ||
| 204 | for (int i=0;i<3;i++) | 204 | for (int i=0;i<3;i++) |
| 205 | { | 205 | { |
| 206 | new (&m_jac[i]) btJacobianEntry( | 206 | new (&m_jac[i]) btJacobianEntry( |
| 207 | m_rbA.getCenterOfMassTransform().getBasis().transpose(), | 207 | m_rbA.getCenterOfMassTransform().getBasis().transpose(), |
| 208 | m_rbB.getCenterOfMassTransform().getBasis().transpose(), | 208 | m_rbB.getCenterOfMassTransform().getBasis().transpose(), |
| 209 | pivotAInW - m_rbA.getCenterOfMassPosition(), | 209 | pivotAInW - m_rbA.getCenterOfMassPosition(), |
| 210 | pivotBInW - m_rbB.getCenterOfMassPosition(), | 210 | pivotBInW - m_rbB.getCenterOfMassPosition(), |
| 211 | normal[i], | 211 | normal[i], |
| 212 | m_rbA.getInvInertiaDiagLocal(), | 212 | m_rbA.getInvInertiaDiagLocal(), |
| 213 | m_rbA.getInvMass(), | 213 | m_rbA.getInvMass(), |
| 214 | m_rbB.getInvInertiaDiagLocal(), | 214 | m_rbB.getInvInertiaDiagLocal(), |
| 215 | m_rbB.getInvMass()); | 215 | m_rbB.getInvMass()); |
| 216 | } | 216 | } |
| 217 | } | 217 | } |
| 218 | 218 | ||
| 219 | //calculate two perpendicular jointAxis, orthogonal to hingeAxis | 219 | //calculate two perpendicular jointAxis, orthogonal to hingeAxis |
| 220 | //these two jointAxis require equal angular velocities for both bodies | 220 | //these two jointAxis require equal angular velocities for both bodies |
| 221 | 221 | ||
| 222 | //this is unused for now, it's a todo | 222 | //this is unused for now, it's a todo |
| 223 | btVector3 jointAxis0local; | 223 | btVector3 jointAxis0local; |
| 224 | btVector3 jointAxis1local; | 224 | btVector3 jointAxis1local; |
| 225 | 225 | ||
| 226 | btPlaneSpace1(m_rbAFrame.getBasis().getColumn(2),jointAxis0local,jointAxis1local); | 226 | btPlaneSpace1(m_rbAFrame.getBasis().getColumn(2),jointAxis0local,jointAxis1local); |
| 227 | 227 | ||
| 228 | btVector3 jointAxis0 = getRigidBodyA().getCenterOfMassTransform().getBasis() * jointAxis0local; | 228 | btVector3 jointAxis0 = getRigidBodyA().getCenterOfMassTransform().getBasis() * jointAxis0local; |
| 229 | btVector3 jointAxis1 = getRigidBodyA().getCenterOfMassTransform().getBasis() * jointAxis1local; | 229 | btVector3 jointAxis1 = getRigidBodyA().getCenterOfMassTransform().getBasis() * jointAxis1local; |
| 230 | btVector3 hingeAxisWorld = getRigidBodyA().getCenterOfMassTransform().getBasis() * m_rbAFrame.getBasis().getColumn(2); | 230 | btVector3 hingeAxisWorld = getRigidBodyA().getCenterOfMassTransform().getBasis() * m_rbAFrame.getBasis().getColumn(2); |
| 231 | 231 | ||
| 232 | new (&m_jacAng[0]) btJacobianEntry(jointAxis0, | 232 | new (&m_jacAng[0]) btJacobianEntry(jointAxis0, |
| 233 | m_rbA.getCenterOfMassTransform().getBasis().transpose(), | 233 | m_rbA.getCenterOfMassTransform().getBasis().transpose(), |
| 234 | m_rbB.getCenterOfMassTransform().getBasis().transpose(), | 234 | m_rbB.getCenterOfMassTransform().getBasis().transpose(), |
| 235 | m_rbA.getInvInertiaDiagLocal(), | 235 | m_rbA.getInvInertiaDiagLocal(), |
| 236 | m_rbB.getInvInertiaDiagLocal()); | 236 | m_rbB.getInvInertiaDiagLocal()); |
| 237 | 237 | ||
| 238 | new (&m_jacAng[1]) btJacobianEntry(jointAxis1, | 238 | new (&m_jacAng[1]) btJacobianEntry(jointAxis1, |
| 239 | m_rbA.getCenterOfMassTransform().getBasis().transpose(), | 239 | m_rbA.getCenterOfMassTransform().getBasis().transpose(), |
| 240 | m_rbB.getCenterOfMassTransform().getBasis().transpose(), | 240 | m_rbB.getCenterOfMassTransform().getBasis().transpose(), |
| 241 | m_rbA.getInvInertiaDiagLocal(), | 241 | m_rbA.getInvInertiaDiagLocal(), |
| 242 | m_rbB.getInvInertiaDiagLocal()); | 242 | m_rbB.getInvInertiaDiagLocal()); |
| 243 | 243 | ||
| 244 | new (&m_jacAng[2]) btJacobianEntry(hingeAxisWorld, | 244 | new (&m_jacAng[2]) btJacobianEntry(hingeAxisWorld, |
| 245 | m_rbA.getCenterOfMassTransform().getBasis().transpose(), | 245 | m_rbA.getCenterOfMassTransform().getBasis().transpose(), |
| 246 | m_rbB.getCenterOfMassTransform().getBasis().transpose(), | 246 | m_rbB.getCenterOfMassTransform().getBasis().transpose(), |
| 247 | m_rbA.getInvInertiaDiagLocal(), | 247 | m_rbA.getInvInertiaDiagLocal(), |
| 248 | m_rbB.getInvInertiaDiagLocal()); | 248 | m_rbB.getInvInertiaDiagLocal()); |
| 249 | 249 | ||
| 250 | // clear accumulator | 250 | // clear accumulator |
| 251 | m_accLimitImpulse = btScalar(0.); | 251 | m_accLimitImpulse = btScalar(0.); |
| 252 | 252 | ||
| 253 | // test angular limit | 253 | // test angular limit |
| 254 | testLimit(m_rbA.getCenterOfMassTransform(),m_rbB.getCenterOfMassTransform()); | 254 | testLimit(m_rbA.getCenterOfMassTransform(),m_rbB.getCenterOfMassTransform()); |
| 255 | 255 | ||
| 256 | //Compute K = J*W*J' for hinge axis | 256 | //Compute K = J*W*J' for hinge axis |
| 257 | btVector3 axisA = getRigidBodyA().getCenterOfMassTransform().getBasis() * m_rbAFrame.getBasis().getColumn(2); | 257 | btVector3 axisA = getRigidBodyA().getCenterOfMassTransform().getBasis() * m_rbAFrame.getBasis().getColumn(2); |
| 258 | m_kHinge = 1.0f / (getRigidBodyA().computeAngularImpulseDenominator(axisA) + | 258 | m_kHinge = 1.0f / (getRigidBodyA().computeAngularImpulseDenominator(axisA) + |
| 259 | getRigidBodyB().computeAngularImpulseDenominator(axisA)); | 259 | getRigidBodyB().computeAngularImpulseDenominator(axisA)); |
| 260 | 260 | ||
| 261 | } | 261 | } |
| 262 | } | 262 | } |
| 263 | 263 | ||
| 264 | 264 | ||
| 265 | #endif //__SPU__ | 265 | #endif //__SPU__ |
| 266 | 266 | ||
| 267 | 267 | ||
| 268 | void btHingeConstraint::getInfo1(btConstraintInfo1* info) | 268 | void btHingeConstraint::getInfo1(btConstraintInfo1* info) |
| 269 | { | 269 | { |
| 270 | if (m_useSolveConstraintObsolete) | 270 | if (m_useSolveConstraintObsolete) |
| 271 | { | 271 | { |
| 272 | info->m_numConstraintRows = 0; | 272 | info->m_numConstraintRows = 0; |
| 273 | info->nub = 0; | 273 | info->nub = 0; |
| 274 | } | 274 | } |
| 275 | else | 275 | else |
| 276 | { | 276 | { |
| 277 | info->m_numConstraintRows = 5; // Fixed 3 linear + 2 angular | 277 | info->m_numConstraintRows = 5; // Fixed 3 linear + 2 angular |
| 278 | info->nub = 1; | 278 | info->nub = 1; |
| 279 | //always add the row, to avoid computation (data is not available yet) | 279 | //always add the row, to avoid computation (data is not available yet) |
| 280 | //prepare constraint | 280 | //prepare constraint |
| 281 | testLimit(m_rbA.getCenterOfMassTransform(),m_rbB.getCenterOfMassTransform()); | 281 | testLimit(m_rbA.getCenterOfMassTransform(),m_rbB.getCenterOfMassTransform()); |
| 282 | if(getSolveLimit() || getEnableAngularMotor()) | 282 | if(getSolveLimit() || getEnableAngularMotor()) |
| 283 | { | 283 | { |
| 284 | info->m_numConstraintRows++; // limit 3rd anguar as well | 284 | info->m_numConstraintRows++; // limit 3rd anguar as well |
| 285 | info->nub--; | 285 | info->nub--; |
| 286 | } | 286 | } |
| 287 | 287 | ||
| 288 | } | 288 | } |
| 289 | } | 289 | } |
| 290 | 290 | ||
| 291 | void btHingeConstraint::getInfo1NonVirtual(btConstraintInfo1* info) | 291 | void btHingeConstraint::getInfo1NonVirtual(btConstraintInfo1* info) |
| 292 | { | 292 | { |
| 293 | if (m_useSolveConstraintObsolete) | 293 | if (m_useSolveConstraintObsolete) |
| 294 | { | 294 | { |
| 295 | info->m_numConstraintRows = 0; | 295 | info->m_numConstraintRows = 0; |
| 296 | info->nub = 0; | 296 | info->nub = 0; |
| 297 | } | 297 | } |
| 298 | else | 298 | else |
| 299 | { | 299 | { |
| 300 | //always add the 'limit' row, to avoid computation (data is not available yet) | 300 | //always add the 'limit' row, to avoid computation (data is not available yet) |
| 301 | info->m_numConstraintRows = 6; // Fixed 3 linear + 2 angular | 301 | info->m_numConstraintRows = 6; // Fixed 3 linear + 2 angular |
| 302 | info->nub = 0; | 302 | info->nub = 0; |
| 303 | } | 303 | } |
| 304 | } | 304 | } |
| 305 | 305 | ||
| 306 | void btHingeConstraint::getInfo2 (btConstraintInfo2* info) | 306 | void btHingeConstraint::getInfo2 (btConstraintInfo2* info) |
| 307 | { | 307 | { |
| 308 | if(m_useOffsetForConstraintFrame) | 308 | if(m_useOffsetForConstraintFrame) |
| 309 | { | 309 | { |
| 310 | getInfo2InternalUsingFrameOffset(info, m_rbA.getCenterOfMassTransform(),m_rbB.getCenterOfMassTransform(),m_rbA.getAngularVelocity(),m_rbB.getAngularVelocity()); | 310 | getInfo2InternalUsingFrameOffset(info, m_rbA.getCenterOfMassTransform(),m_rbB.getCenterOfMassTransform(),m_rbA.getAngularVelocity(),m_rbB.getAngularVelocity()); |
| 311 | } | 311 | } |
| 312 | else | 312 | else |
| 313 | { | 313 | { |
| 314 | getInfo2Internal(info, m_rbA.getCenterOfMassTransform(),m_rbB.getCenterOfMassTransform(),m_rbA.getAngularVelocity(),m_rbB.getAngularVelocity()); | 314 | getInfo2Internal(info, m_rbA.getCenterOfMassTransform(),m_rbB.getCenterOfMassTransform(),m_rbA.getAngularVelocity(),m_rbB.getAngularVelocity()); |
| 315 | } | 315 | } |
| 316 | } | 316 | } |
| 317 | 317 | ||
| 318 | 318 | ||
| 319 | void btHingeConstraint::getInfo2NonVirtual (btConstraintInfo2* info,const btTransform& transA,const btTransform& transB,const btVector3& angVelA,const btVector3& angVelB) | 319 | void btHingeConstraint::getInfo2NonVirtual (btConstraintInfo2* info,const btTransform& transA,const btTransform& transB,const btVector3& angVelA,const btVector3& angVelB) |
| 320 | { | 320 | { |
| 321 | ///the regular (virtual) implementation getInfo2 already performs 'testLimit' during getInfo1, so we need to do it now | 321 | ///the regular (virtual) implementation getInfo2 already performs 'testLimit' during getInfo1, so we need to do it now |
| 322 | testLimit(transA,transB); | 322 | testLimit(transA,transB); |
| 323 | 323 | ||
| 324 | getInfo2Internal(info,transA,transB,angVelA,angVelB); | 324 | getInfo2Internal(info,transA,transB,angVelA,angVelB); |
| 325 | } | 325 | } |
| 326 | 326 | ||
| 327 | 327 | ||
| 328 | void btHingeConstraint::getInfo2Internal(btConstraintInfo2* info, const btTransform& transA,const btTransform& transB,const btVector3& angVelA,const btVector3& angVelB) | 328 | void btHingeConstraint::getInfo2Internal(btConstraintInfo2* info, const btTransform& transA,const btTransform& transB,const btVector3& angVelA,const btVector3& angVelB) |
| 329 | { | 329 | { |
| 330 | 330 | ||
| 331 | btAssert(!m_useSolveConstraintObsolete); | 331 | btAssert(!m_useSolveConstraintObsolete); |
| 332 | int i, skip = info->rowskip; | 332 | int i, skip = info->rowskip; |
| 333 | // transforms in world space | 333 | // transforms in world space |
| 334 | btTransform trA = transA*m_rbAFrame; | 334 | btTransform trA = transA*m_rbAFrame; |
| 335 | btTransform trB = transB*m_rbBFrame; | 335 | btTransform trB = transB*m_rbBFrame; |
| 336 | // pivot point | 336 | // pivot point |
| 337 | btVector3 pivotAInW = trA.getOrigin(); | 337 | btVector3 pivotAInW = trA.getOrigin(); |
| 338 | btVector3 pivotBInW = trB.getOrigin(); | 338 | btVector3 pivotBInW = trB.getOrigin(); |
| 339 | #if 0 | 339 | #if 0 |
| 340 | if (0) | 340 | if (0) |
| 341 | { | 341 | { |
| 342 | for (i=0;i<6;i++) | 342 | for (i=0;i<6;i++) |
| 343 | { | 343 | { |
| 344 | info->m_J1linearAxis[i*skip]=0; | 344 | info->m_J1linearAxis[i*skip]=0; |
| 345 | info->m_J1linearAxis[i*skip+1]=0; | 345 | info->m_J1linearAxis[i*skip+1]=0; |
| 346 | info->m_J1linearAxis[i*skip+2]=0; | 346 | info->m_J1linearAxis[i*skip+2]=0; |
| 347 | 347 | ||
| 348 | info->m_J1angularAxis[i*skip]=0; | 348 | info->m_J1angularAxis[i*skip]=0; |
| 349 | info->m_J1angularAxis[i*skip+1]=0; | 349 | info->m_J1angularAxis[i*skip+1]=0; |
| 350 | info->m_J1angularAxis[i*skip+2]=0; | 350 | info->m_J1angularAxis[i*skip+2]=0; |
| 351 | 351 | ||
| 352 | info->m_J2angularAxis[i*skip]=0; | 352 | info->m_J2angularAxis[i*skip]=0; |
| 353 | info->m_J2angularAxis[i*skip+1]=0; | 353 | info->m_J2angularAxis[i*skip+1]=0; |
| 354 | info->m_J2angularAxis[i*skip+2]=0; | 354 | info->m_J2angularAxis[i*skip+2]=0; |
| 355 | 355 | ||
| 356 | info->m_constraintError[i*skip]=0.f; | 356 | info->m_constraintError[i*skip]=0.f; |
| 357 | } | 357 | } |
| 358 | } | 358 | } |
| 359 | #endif //#if 0 | 359 | #endif //#if 0 |
| 360 | // linear (all fixed) | 360 | // linear (all fixed) |
| 361 | info->m_J1linearAxis[0] = 1; | 361 | |
| 362 | info->m_J1linearAxis[skip + 1] = 1; | 362 | if (!m_angularOnly) |
| 363 | info->m_J1linearAxis[2 * skip + 2] = 1; | 363 | { |
| 364 | 364 | info->m_J1linearAxis[0] = 1; | |
| 365 | info->m_J1linearAxis[skip + 1] = 1; | ||
| 366 | info->m_J1linearAxis[2 * skip + 2] = 1; | ||
| 367 | } | ||
| 365 | 368 | ||
| 366 | 369 | ||
| 367 | 370 | ||
| 368 | 371 | ||
| 369 | btVector3 a1 = pivotAInW - transA.getOrigin(); | 372 | btVector3 a1 = pivotAInW - transA.getOrigin(); |
| 370 | { | 373 | { |
| 371 | btVector3* angular0 = (btVector3*)(info->m_J1angularAxis); | 374 | btVector3* angular0 = (btVector3*)(info->m_J1angularAxis); |
| 372 | btVector3* angular1 = (btVector3*)(info->m_J1angularAxis + skip); | 375 | btVector3* angular1 = (btVector3*)(info->m_J1angularAxis + skip); |
| 373 | btVector3* angular2 = (btVector3*)(info->m_J1angularAxis + 2 * skip); | 376 | btVector3* angular2 = (btVector3*)(info->m_J1angularAxis + 2 * skip); |
| 374 | btVector3 a1neg = -a1; | 377 | btVector3 a1neg = -a1; |
| 375 | a1neg.getSkewSymmetricMatrix(angular0,angular1,angular2); | 378 | a1neg.getSkewSymmetricMatrix(angular0,angular1,angular2); |
| 376 | } | 379 | } |
| 377 | btVector3 a2 = pivotBInW - transB.getOrigin(); | 380 | btVector3 a2 = pivotBInW - transB.getOrigin(); |
| 378 | { | 381 | { |
| 379 | btVector3* angular0 = (btVector3*)(info->m_J2angularAxis); | 382 | btVector3* angular0 = (btVector3*)(info->m_J2angularAxis); |
| 380 | btVector3* angular1 = (btVector3*)(info->m_J2angularAxis + skip); | 383 | btVector3* angular1 = (btVector3*)(info->m_J2angularAxis + skip); |
| 381 | btVector3* angular2 = (btVector3*)(info->m_J2angularAxis + 2 * skip); | 384 | btVector3* angular2 = (btVector3*)(info->m_J2angularAxis + 2 * skip); |
| 382 | a2.getSkewSymmetricMatrix(angular0,angular1,angular2); | 385 | a2.getSkewSymmetricMatrix(angular0,angular1,angular2); |
| 383 | } | 386 | } |
| 384 | // linear RHS | 387 | // linear RHS |
| 385 | btScalar k = info->fps * info->erp; | 388 | btScalar k = info->fps * info->erp; |
| 386 | for(i = 0; i < 3; i++) | 389 | if (!m_angularOnly) |
| 387 | { | 390 | { |
| 388 | info->m_constraintError[i * skip] = k * (pivotBInW[i] - pivotAInW[i]); | 391 | for(i = 0; i < 3; i++) |
| 389 | } | 392 | { |
| 393 | info->m_constraintError[i * skip] = k * (pivotBInW[i] - pivotAInW[i]); | ||
| 394 | } | ||
| 395 | } | ||
| 390 | // make rotations around X and Y equal | 396 | // make rotations around X and Y equal |
| 391 | // the hinge axis should be the only unconstrained | 397 | // the hinge axis should be the only unconstrained |
| 392 | // rotational axis, the angular velocity of the two bodies perpendicular to | 398 | // rotational axis, the angular velocity of the two bodies perpendicular to |
| 393 | // the hinge axis should be equal. thus the constraint equations are | 399 | // the hinge axis should be equal. thus the constraint equations are |
| 394 | // p*w1 - p*w2 = 0 | 400 | // p*w1 - p*w2 = 0 |
| 395 | // q*w1 - q*w2 = 0 | 401 | // q*w1 - q*w2 = 0 |
| 396 | // where p and q are unit vectors normal to the hinge axis, and w1 and w2 | 402 | // where p and q are unit vectors normal to the hinge axis, and w1 and w2 |
| 397 | // are the angular velocity vectors of the two bodies. | 403 | // are the angular velocity vectors of the two bodies. |
| 398 | // get hinge axis (Z) | 404 | // get hinge axis (Z) |
| 399 | btVector3 ax1 = trA.getBasis().getColumn(2); | 405 | btVector3 ax1 = trA.getBasis().getColumn(2); |
| 400 | // get 2 orthos to hinge axis (X, Y) | 406 | // get 2 orthos to hinge axis (X, Y) |
| 401 | btVector3 p = trA.getBasis().getColumn(0); | 407 | btVector3 p = trA.getBasis().getColumn(0); |
| 402 | btVector3 q = trA.getBasis().getColumn(1); | 408 | btVector3 q = trA.getBasis().getColumn(1); |
| 403 | // set the two hinge angular rows | 409 | // set the two hinge angular rows |
| 404 | int s3 = 3 * info->rowskip; | 410 | int s3 = 3 * info->rowskip; |
| 405 | int s4 = 4 * info->rowskip; | 411 | int s4 = 4 * info->rowskip; |
| 406 | 412 | ||
| 407 | info->m_J1angularAxis[s3 + 0] = p[0]; | 413 | info->m_J1angularAxis[s3 + 0] = p[0]; |
| 408 | info->m_J1angularAxis[s3 + 1] = p[1]; | 414 | info->m_J1angularAxis[s3 + 1] = p[1]; |
| 409 | info->m_J1angularAxis[s3 + 2] = p[2]; | 415 | info->m_J1angularAxis[s3 + 2] = p[2]; |
| 410 | info->m_J1angularAxis[s4 + 0] = q[0]; | 416 | info->m_J1angularAxis[s4 + 0] = q[0]; |
| 411 | info->m_J1angularAxis[s4 + 1] = q[1]; | 417 | info->m_J1angularAxis[s4 + 1] = q[1]; |
| 412 | info->m_J1angularAxis[s4 + 2] = q[2]; | 418 | info->m_J1angularAxis[s4 + 2] = q[2]; |
| 413 | 419 | ||
| 414 | info->m_J2angularAxis[s3 + 0] = -p[0]; | 420 | info->m_J2angularAxis[s3 + 0] = -p[0]; |
| 415 | info->m_J2angularAxis[s3 + 1] = -p[1]; | 421 | info->m_J2angularAxis[s3 + 1] = -p[1]; |
| 416 | info->m_J2angularAxis[s3 + 2] = -p[2]; | 422 | info->m_J2angularAxis[s3 + 2] = -p[2]; |
| 417 | info->m_J2angularAxis[s4 + 0] = -q[0]; | 423 | info->m_J2angularAxis[s4 + 0] = -q[0]; |
| 418 | info->m_J2angularAxis[s4 + 1] = -q[1]; | 424 | info->m_J2angularAxis[s4 + 1] = -q[1]; |
| 419 | info->m_J2angularAxis[s4 + 2] = -q[2]; | 425 | info->m_J2angularAxis[s4 + 2] = -q[2]; |
| 420 | // compute the right hand side of the constraint equation. set relative | 426 | // compute the right hand side of the constraint equation. set relative |
| 421 | // body velocities along p and q to bring the hinge back into alignment. | 427 | // body velocities along p and q to bring the hinge back into alignment. |
| 422 | // if ax1,ax2 are the unit length hinge axes as computed from body1 and | 428 | // if ax1,ax2 are the unit length hinge axes as computed from body1 and |
| 423 | // body2, we need to rotate both bodies along the axis u = (ax1 x ax2). | 429 | // body2, we need to rotate both bodies along the axis u = (ax1 x ax2). |
| 424 | // if `theta' is the angle between ax1 and ax2, we need an angular velocity | 430 | // if `theta' is the angle between ax1 and ax2, we need an angular velocity |
| 425 | // along u to cover angle erp*theta in one step : | 431 | // along u to cover angle erp*theta in one step : |
| 426 | // |angular_velocity| = angle/time = erp*theta / stepsize | 432 | // |angular_velocity| = angle/time = erp*theta / stepsize |
| 427 | // = (erp*fps) * theta | 433 | // = (erp*fps) * theta |
| 428 | // angular_velocity = |angular_velocity| * (ax1 x ax2) / |ax1 x ax2| | 434 | // angular_velocity = |angular_velocity| * (ax1 x ax2) / |ax1 x ax2| |
| 429 | // = (erp*fps) * theta * (ax1 x ax2) / sin(theta) | 435 | // = (erp*fps) * theta * (ax1 x ax2) / sin(theta) |
| 430 | // ...as ax1 and ax2 are unit length. if theta is smallish, | 436 | // ...as ax1 and ax2 are unit length. if theta is smallish, |
| 431 | // theta ~= sin(theta), so | 437 | // theta ~= sin(theta), so |
| 432 | // angular_velocity = (erp*fps) * (ax1 x ax2) | 438 | // angular_velocity = (erp*fps) * (ax1 x ax2) |
| 433 | // ax1 x ax2 is in the plane space of ax1, so we project the angular | 439 | // ax1 x ax2 is in the plane space of ax1, so we project the angular |
| 434 | // velocity to p and q to find the right hand side. | 440 | // velocity to p and q to find the right hand side. |
| 435 | btVector3 ax2 = trB.getBasis().getColumn(2); | 441 | btVector3 ax2 = trB.getBasis().getColumn(2); |
| 436 | btVector3 u = ax1.cross(ax2); | 442 | btVector3 u = ax1.cross(ax2); |
| 437 | info->m_constraintError[s3] = k * u.dot(p); | 443 | info->m_constraintError[s3] = k * u.dot(p); |
| 438 | info->m_constraintError[s4] = k * u.dot(q); | 444 | info->m_constraintError[s4] = k * u.dot(q); |
| 439 | // check angular limits | 445 | // check angular limits |
| 440 | int nrow = 4; // last filled row | 446 | int nrow = 4; // last filled row |
| 441 | int srow; | 447 | int srow; |
| 442 | btScalar limit_err = btScalar(0.0); | 448 | btScalar limit_err = btScalar(0.0); |
| 443 | int limit = 0; | 449 | int limit = 0; |
| 444 | if(getSolveLimit()) | 450 | if(getSolveLimit()) |
| 445 | { | 451 | { |
| 446 | limit_err = m_correction * m_referenceSign; | 452 | limit_err = m_correction * m_referenceSign; |
| 447 | limit = (limit_err > btScalar(0.0)) ? 1 : 2; | 453 | limit = (limit_err > btScalar(0.0)) ? 1 : 2; |
| 448 | } | 454 | } |
| 449 | // if the hinge has joint limits or motor, add in the extra row | 455 | // if the hinge has joint limits or motor, add in the extra row |
| 450 | int powered = 0; | 456 | int powered = 0; |
| 451 | if(getEnableAngularMotor()) | 457 | if(getEnableAngularMotor()) |
| 452 | { | 458 | { |
| 453 | powered = 1; | 459 | powered = 1; |
| 454 | } | 460 | } |
| 455 | if(limit || powered) | 461 | if(limit || powered) |
| 456 | { | 462 | { |
| 457 | nrow++; | 463 | nrow++; |
| 458 | srow = nrow * info->rowskip; | 464 | srow = nrow * info->rowskip; |
| 459 | info->m_J1angularAxis[srow+0] = ax1[0]; | 465 | info->m_J1angularAxis[srow+0] = ax1[0]; |
| 460 | info->m_J1angularAxis[srow+1] = ax1[1]; | 466 | info->m_J1angularAxis[srow+1] = ax1[1]; |
| 461 | info->m_J1angularAxis[srow+2] = ax1[2]; | 467 | info->m_J1angularAxis[srow+2] = ax1[2]; |
| 462 | 468 | ||
| 463 | info->m_J2angularAxis[srow+0] = -ax1[0]; | 469 | info->m_J2angularAxis[srow+0] = -ax1[0]; |
| 464 | info->m_J2angularAxis[srow+1] = -ax1[1]; | 470 | info->m_J2angularAxis[srow+1] = -ax1[1]; |
| 465 | info->m_J2angularAxis[srow+2] = -ax1[2]; | 471 | info->m_J2angularAxis[srow+2] = -ax1[2]; |
| 466 | 472 | ||
| 467 | btScalar lostop = getLowerLimit(); | 473 | btScalar lostop = getLowerLimit(); |
| 468 | btScalar histop = getUpperLimit(); | 474 | btScalar histop = getUpperLimit(); |
| 469 | if(limit && (lostop == histop)) | 475 | if(limit && (lostop == histop)) |
| 470 | { // the joint motor is ineffective | 476 | { // the joint motor is ineffective |
| 471 | powered = 0; | 477 | powered = 0; |
| 472 | } | 478 | } |
| 473 | info->m_constraintError[srow] = btScalar(0.0f); | 479 | info->m_constraintError[srow] = btScalar(0.0f); |
| 474 | btScalar currERP = (m_flags & BT_HINGE_FLAGS_ERP_STOP) ? m_stopERP : info->erp; | 480 | btScalar currERP = (m_flags & BT_HINGE_FLAGS_ERP_STOP) ? m_stopERP : info->erp; |
| 475 | if(powered) | 481 | if(powered) |
| 476 | { | 482 | { |
| 477 | if(m_flags & BT_HINGE_FLAGS_CFM_NORM) | 483 | if(m_flags & BT_HINGE_FLAGS_CFM_NORM) |
| 478 | { | 484 | { |
| 479 | info->cfm[srow] = m_normalCFM; | 485 | info->cfm[srow] = m_normalCFM; |
| 480 | } | 486 | } |
| 481 | btScalar mot_fact = getMotorFactor(m_hingeAngle, lostop, histop, m_motorTargetVelocity, info->fps * currERP); | 487 | btScalar mot_fact = getMotorFactor(m_hingeAngle, lostop, histop, m_motorTargetVelocity, info->fps * currERP); |
| 482 | info->m_constraintError[srow] += mot_fact * m_motorTargetVelocity * m_referenceSign; | 488 | info->m_constraintError[srow] += mot_fact * m_motorTargetVelocity * m_referenceSign; |
| 483 | info->m_lowerLimit[srow] = - m_maxMotorImpulse; | 489 | info->m_lowerLimit[srow] = - m_maxMotorImpulse; |
| 484 | info->m_upperLimit[srow] = m_maxMotorImpulse; | 490 | info->m_upperLimit[srow] = m_maxMotorImpulse; |
| 485 | } | 491 | } |
| 486 | if(limit) | 492 | if(limit) |
| 487 | { | 493 | { |
| 488 | k = info->fps * currERP; | 494 | k = info->fps * currERP; |
| 489 | info->m_constraintError[srow] += k * limit_err; | 495 | info->m_constraintError[srow] += k * limit_err; |
| 490 | if(m_flags & BT_HINGE_FLAGS_CFM_STOP) | 496 | if(m_flags & BT_HINGE_FLAGS_CFM_STOP) |
| 491 | { | 497 | { |
| 492 | info->cfm[srow] = m_stopCFM; | 498 | info->cfm[srow] = m_stopCFM; |
| 493 | } | 499 | } |
| 494 | if(lostop == histop) | 500 | if(lostop == histop) |
| 495 | { | 501 | { |
| 496 | // limited low and high simultaneously | 502 | // limited low and high simultaneously |
| 497 | info->m_lowerLimit[srow] = -SIMD_INFINITY; | 503 | info->m_lowerLimit[srow] = -SIMD_INFINITY; |
| 498 | info->m_upperLimit[srow] = SIMD_INFINITY; | 504 | info->m_upperLimit[srow] = SIMD_INFINITY; |
| 499 | } | 505 | } |
| 500 | else if(limit == 1) | 506 | else if(limit == 1) |
| 501 | { // low limit | 507 | { // low limit |
| 502 | info->m_lowerLimit[srow] = 0; | 508 | info->m_lowerLimit[srow] = 0; |
| 503 | info->m_upperLimit[srow] = SIMD_INFINITY; | 509 | info->m_upperLimit[srow] = SIMD_INFINITY; |
| 504 | } | 510 | } |
| 505 | else | 511 | else |
| 506 | { // high limit | 512 | { // high limit |
| 507 | info->m_lowerLimit[srow] = -SIMD_INFINITY; | 513 | info->m_lowerLimit[srow] = -SIMD_INFINITY; |
| 508 | info->m_upperLimit[srow] = 0; | 514 | info->m_upperLimit[srow] = 0; |
| 509 | } | 515 | } |
| 510 | // bounce (we'll use slider parameter abs(1.0 - m_dampingLimAng) for that) | 516 | // bounce (we'll use slider parameter abs(1.0 - m_dampingLimAng) for that) |
| 511 | btScalar bounce = m_relaxationFactor; | 517 | btScalar bounce = m_relaxationFactor; |
| 512 | if(bounce > btScalar(0.0)) | 518 | if(bounce > btScalar(0.0)) |
| 513 | { | 519 | { |
| 514 | btScalar vel = angVelA.dot(ax1); | 520 | btScalar vel = angVelA.dot(ax1); |
| 515 | vel -= angVelB.dot(ax1); | 521 | vel -= angVelB.dot(ax1); |
| 516 | // only apply bounce if the velocity is incoming, and if the | 522 | // only apply bounce if the velocity is incoming, and if the |
| 517 | // resulting c[] exceeds what we already have. | 523 | // resulting c[] exceeds what we already have. |
| 518 | if(limit == 1) | 524 | if(limit == 1) |
| 519 | { // low limit | 525 | { // low limit |
| 520 | if(vel < 0) | 526 | if(vel < 0) |
| 521 | { | 527 | { |
| 522 | btScalar newc = -bounce * vel; | 528 | btScalar newc = -bounce * vel; |
| 523 | if(newc > info->m_constraintError[srow]) | 529 | if(newc > info->m_constraintError[srow]) |
| 524 | { | 530 | { |
| 525 | info->m_constraintError[srow] = newc; | 531 | info->m_constraintError[srow] = newc; |
| 526 | } | 532 | } |
| 527 | } | 533 | } |
| 528 | } | 534 | } |
| 529 | else | 535 | else |
| 530 | { // high limit - all those computations are reversed | 536 | { // high limit - all those computations are reversed |
| 531 | if(vel > 0) | 537 | if(vel > 0) |
| 532 | { | 538 | { |
| 533 | btScalar newc = -bounce * vel; | 539 | btScalar newc = -bounce * vel; |
| 534 | if(newc < info->m_constraintError[srow]) | 540 | if(newc < info->m_constraintError[srow]) |
| 535 | { | 541 | { |
| 536 | info->m_constraintError[srow] = newc; | 542 | info->m_constraintError[srow] = newc; |
| 537 | } | 543 | } |
| 538 | } | 544 | } |
| 539 | } | 545 | } |
| 540 | } | 546 | } |
| 541 | info->m_constraintError[srow] *= m_biasFactor; | 547 | info->m_constraintError[srow] *= m_biasFactor; |
| 542 | } // if(limit) | 548 | } // if(limit) |
| 543 | } // if angular limit or powered | 549 | } // if angular limit or powered |
| 544 | } | 550 | } |
| 545 | 551 | ||
| 546 | 552 | ||
| 547 | 553 | ||
| 548 | 554 | ||
| 549 | 555 | ||
| 550 | 556 | ||
| 551 | void btHingeConstraint::updateRHS(btScalar timeStep) | 557 | void btHingeConstraint::updateRHS(btScalar timeStep) |
| 552 | { | 558 | { |
| 553 | (void)timeStep; | 559 | (void)timeStep; |
| 554 | 560 | ||
| 555 | } | 561 | } |
| 556 | 562 | ||
| 557 | 563 | ||
| 558 | btScalar btHingeConstraint::getHingeAngle() | 564 | btScalar btHingeConstraint::getHingeAngle() |
| 559 | { | 565 | { |
| 560 | return getHingeAngle(m_rbA.getCenterOfMassTransform(),m_rbB.getCenterOfMassTransform()); | 566 | return getHingeAngle(m_rbA.getCenterOfMassTransform(),m_rbB.getCenterOfMassTransform()); |
| 561 | } | 567 | } |
| 562 | 568 | ||
| 563 | btScalar btHingeConstraint::getHingeAngle(const btTransform& transA,const btTransform& transB) | 569 | btScalar btHingeConstraint::getHingeAngle(const btTransform& transA,const btTransform& transB) |
| 564 | { | 570 | { |
| 565 | const btVector3 refAxis0 = transA.getBasis() * m_rbAFrame.getBasis().getColumn(0); | 571 | const btVector3 refAxis0 = transA.getBasis() * m_rbAFrame.getBasis().getColumn(0); |
| 566 | const btVector3 refAxis1 = transA.getBasis() * m_rbAFrame.getBasis().getColumn(1); | 572 | const btVector3 refAxis1 = transA.getBasis() * m_rbAFrame.getBasis().getColumn(1); |
| 567 | const btVector3 swingAxis = transB.getBasis() * m_rbBFrame.getBasis().getColumn(1); | 573 | const btVector3 swingAxis = transB.getBasis() * m_rbBFrame.getBasis().getColumn(1); |
| 568 | // btScalar angle = btAtan2Fast(swingAxis.dot(refAxis0), swingAxis.dot(refAxis1)); | 574 | // btScalar angle = btAtan2Fast(swingAxis.dot(refAxis0), swingAxis.dot(refAxis1)); |
| 569 | btScalar angle = btAtan2(swingAxis.dot(refAxis0), swingAxis.dot(refAxis1)); | 575 | btScalar angle = btAtan2(swingAxis.dot(refAxis0), swingAxis.dot(refAxis1)); |
| 570 | return m_referenceSign * angle; | 576 | return m_referenceSign * angle; |
| 571 | } | 577 | } |
| 572 | 578 | ||
| 573 | 579 | ||
| 574 | #if 0 | 580 | #if 0 |
| 575 | void btHingeConstraint::testLimit() | 581 | void btHingeConstraint::testLimit() |
| 576 | { | 582 | { |
| 577 | // Compute limit information | 583 | // Compute limit information |
| 578 | m_hingeAngle = getHingeAngle(); | 584 | m_hingeAngle = getHingeAngle(); |
| 579 | m_correction = btScalar(0.); | 585 | m_correction = btScalar(0.); |
| 580 | m_limitSign = btScalar(0.); | 586 | m_limitSign = btScalar(0.); |
| 581 | m_solveLimit = false; | 587 | m_solveLimit = false; |
| 582 | if (m_lowerLimit <= m_upperLimit) | 588 | if (m_lowerLimit <= m_upperLimit) |
| 583 | { | 589 | { |
| 584 | if (m_hingeAngle <= m_lowerLimit) | 590 | if (m_hingeAngle <= m_lowerLimit) |
| 585 | { | 591 | { |
| 586 | m_correction = (m_lowerLimit - m_hingeAngle); | 592 | m_correction = (m_lowerLimit - m_hingeAngle); |
| 587 | m_limitSign = 1.0f; | 593 | m_limitSign = 1.0f; |
| 588 | m_solveLimit = true; | 594 | m_solveLimit = true; |
| 589 | } | 595 | } |
| 590 | else if (m_hingeAngle >= m_upperLimit) | 596 | else if (m_hingeAngle >= m_upperLimit) |
| 591 | { | 597 | { |
| 592 | m_correction = m_upperLimit - m_hingeAngle; | 598 | m_correction = m_upperLimit - m_hingeAngle; |
| 593 | m_limitSign = -1.0f; | 599 | m_limitSign = -1.0f; |
| 594 | m_solveLimit = true; | 600 | m_solveLimit = true; |
| 595 | } | 601 | } |
| 596 | } | 602 | } |
| 597 | return; | 603 | return; |
| 598 | } | 604 | } |
| 599 | #else | 605 | #else |
| 600 | 606 | ||
| 601 | 607 | ||
| 602 | void btHingeConstraint::testLimit(const btTransform& transA,const btTransform& transB) | 608 | void btHingeConstraint::testLimit(const btTransform& transA,const btTransform& transB) |
| 603 | { | 609 | { |
| 604 | // Compute limit information | 610 | // Compute limit information |
| 605 | m_hingeAngle = getHingeAngle(transA,transB); | 611 | m_hingeAngle = getHingeAngle(transA,transB); |
| 606 | m_correction = btScalar(0.); | 612 | m_correction = btScalar(0.); |
| 607 | m_limitSign = btScalar(0.); | 613 | m_limitSign = btScalar(0.); |
| 608 | m_solveLimit = false; | 614 | m_solveLimit = false; |
| 609 | if (m_lowerLimit <= m_upperLimit) | 615 | if (m_lowerLimit <= m_upperLimit) |
| 610 | { | 616 | { |
| 611 | m_hingeAngle = btAdjustAngleToLimits(m_hingeAngle, m_lowerLimit, m_upperLimit); | 617 | m_hingeAngle = btAdjustAngleToLimits(m_hingeAngle, m_lowerLimit, m_upperLimit); |
| 612 | if (m_hingeAngle <= m_lowerLimit) | 618 | if (m_hingeAngle <= m_lowerLimit) |
| 613 | { | 619 | { |
| 614 | m_correction = (m_lowerLimit - m_hingeAngle); | 620 | m_correction = (m_lowerLimit - m_hingeAngle); |
| 615 | m_limitSign = 1.0f; | 621 | m_limitSign = 1.0f; |
| 616 | m_solveLimit = true; | 622 | m_solveLimit = true; |
| 617 | } | 623 | } |
| 618 | else if (m_hingeAngle >= m_upperLimit) | 624 | else if (m_hingeAngle >= m_upperLimit) |
| 619 | { | 625 | { |
| 620 | m_correction = m_upperLimit - m_hingeAngle; | 626 | m_correction = m_upperLimit - m_hingeAngle; |
| 621 | m_limitSign = -1.0f; | 627 | m_limitSign = -1.0f; |
| 622 | m_solveLimit = true; | 628 | m_solveLimit = true; |
| 623 | } | 629 | } |
| 624 | } | 630 | } |
| 625 | return; | 631 | return; |
| 626 | } | 632 | } |
| 627 | #endif | 633 | #endif |
| 628 | 634 | ||
| 629 | static btVector3 vHinge(0, 0, btScalar(1)); | 635 | static btVector3 vHinge(0, 0, btScalar(1)); |
| 630 | 636 | ||
| 631 | void btHingeConstraint::setMotorTarget(const btQuaternion& qAinB, btScalar dt) | 637 | void btHingeConstraint::setMotorTarget(const btQuaternion& qAinB, btScalar dt) |
| 632 | { | 638 | { |
| 633 | // convert target from body to constraint space | 639 | // convert target from body to constraint space |
| 634 | btQuaternion qConstraint = m_rbBFrame.getRotation().inverse() * qAinB * m_rbAFrame.getRotation(); | 640 | btQuaternion qConstraint = m_rbBFrame.getRotation().inverse() * qAinB * m_rbAFrame.getRotation(); |
| 635 | qConstraint.normalize(); | 641 | qConstraint.normalize(); |
| 636 | 642 | ||
| 637 | // extract "pure" hinge component | 643 | // extract "pure" hinge component |
| 638 | btVector3 vNoHinge = quatRotate(qConstraint, vHinge); vNoHinge.normalize(); | 644 | btVector3 vNoHinge = quatRotate(qConstraint, vHinge); vNoHinge.normalize(); |
| 639 | btQuaternion qNoHinge = shortestArcQuat(vHinge, vNoHinge); | 645 | btQuaternion qNoHinge = shortestArcQuat(vHinge, vNoHinge); |
| 640 | btQuaternion qHinge = qNoHinge.inverse() * qConstraint; | 646 | btQuaternion qHinge = qNoHinge.inverse() * qConstraint; |
| 641 | qHinge.normalize(); | 647 | qHinge.normalize(); |
| 642 | 648 | ||
| 643 | // compute angular target, clamped to limits | 649 | // compute angular target, clamped to limits |
| 644 | btScalar targetAngle = qHinge.getAngle(); | 650 | btScalar targetAngle = qHinge.getAngle(); |
| 645 | if (targetAngle > SIMD_PI) // long way around. flip quat and recalculate. | 651 | if (targetAngle > SIMD_PI) // long way around. flip quat and recalculate. |
| 646 | { | 652 | { |
| 647 | qHinge = operator-(qHinge); | 653 | qHinge = operator-(qHinge); |
| 648 | targetAngle = qHinge.getAngle(); | 654 | targetAngle = qHinge.getAngle(); |
| 649 | } | 655 | } |
| 650 | if (qHinge.getZ() < 0) | 656 | if (qHinge.getZ() < 0) |
| 651 | targetAngle = -targetAngle; | 657 | targetAngle = -targetAngle; |
| 652 | 658 | ||
| 653 | setMotorTarget(targetAngle, dt); | 659 | setMotorTarget(targetAngle, dt); |
| 654 | } | 660 | } |
| 655 | 661 | ||
| 656 | void btHingeConstraint::setMotorTarget(btScalar targetAngle, btScalar dt) | 662 | void btHingeConstraint::setMotorTarget(btScalar targetAngle, btScalar dt) |
| 657 | { | 663 | { |
| 658 | if (m_lowerLimit < m_upperLimit) | 664 | if (m_lowerLimit < m_upperLimit) |
| 659 | { | 665 | { |
| 660 | if (targetAngle < m_lowerLimit) | 666 | if (targetAngle < m_lowerLimit) |
| 661 | targetAngle = m_lowerLimit; | 667 | targetAngle = m_lowerLimit; |
| 662 | else if (targetAngle > m_upperLimit) | 668 | else if (targetAngle > m_upperLimit) |
| 663 | targetAngle = m_upperLimit; | 669 | targetAngle = m_upperLimit; |
| 664 | } | 670 | } |
| 665 | 671 | ||
| 666 | // compute angular velocity | 672 | // compute angular velocity |
| 667 | btScalar curAngle = getHingeAngle(m_rbA.getCenterOfMassTransform(),m_rbB.getCenterOfMassTransform()); | 673 | btScalar curAngle = getHingeAngle(m_rbA.getCenterOfMassTransform(),m_rbB.getCenterOfMassTransform()); |
| 668 | btScalar dAngle = targetAngle - curAngle; | 674 | btScalar dAngle = targetAngle - curAngle; |
| 669 | m_motorTargetVelocity = dAngle / dt; | 675 | m_motorTargetVelocity = dAngle / dt; |
| 670 | } | 676 | } |
| 671 | 677 | ||
| 672 | 678 | ||
| 673 | 679 | ||
| 674 | void btHingeConstraint::getInfo2InternalUsingFrameOffset(btConstraintInfo2* info, const btTransform& transA,const btTransform& transB,const btVector3& angVelA,const btVector3& angVelB) | 680 | void btHingeConstraint::getInfo2InternalUsingFrameOffset(btConstraintInfo2* info, const btTransform& transA,const btTransform& transB,const btVector3& angVelA,const btVector3& angVelB) |
| 675 | { | 681 | { |
| 676 | btAssert(!m_useSolveConstraintObsolete); | 682 | btAssert(!m_useSolveConstraintObsolete); |
| 677 | int i, s = info->rowskip; | 683 | int i, s = info->rowskip; |
| 678 | // transforms in world space | 684 | // transforms in world space |
| 679 | btTransform trA = transA*m_rbAFrame; | 685 | btTransform trA = transA*m_rbAFrame; |
| 680 | btTransform trB = transB*m_rbBFrame; | 686 | btTransform trB = transB*m_rbBFrame; |
| 681 | // pivot point | 687 | // pivot point |
| 682 | btVector3 pivotAInW = trA.getOrigin(); | 688 | btVector3 pivotAInW = trA.getOrigin(); |
| 683 | btVector3 pivotBInW = trB.getOrigin(); | 689 | btVector3 pivotBInW = trB.getOrigin(); |
| 684 | #if 1 | 690 | #if 1 |
| 685 | // difference between frames in WCS | 691 | // difference between frames in WCS |
| 686 | btVector3 ofs = trB.getOrigin() - trA.getOrigin(); | 692 | btVector3 ofs = trB.getOrigin() - trA.getOrigin(); |
| 687 | // now get weight factors depending on masses | 693 | // now get weight factors depending on masses |
| 688 | btScalar miA = getRigidBodyA().getInvMass(); | 694 | btScalar miA = getRigidBodyA().getInvMass(); |
| 689 | btScalar miB = getRigidBodyB().getInvMass(); | 695 | btScalar miB = getRigidBodyB().getInvMass(); |
| 690 | bool hasStaticBody = (miA < SIMD_EPSILON) || (miB < SIMD_EPSILON); | 696 | bool hasStaticBody = (miA < SIMD_EPSILON) || (miB < SIMD_EPSILON); |
| 691 | btScalar miS = miA + miB; | 697 | btScalar miS = miA + miB; |
| 692 | btScalar factA, factB; | 698 | btScalar factA, factB; |
| 693 | if(miS > btScalar(0.f)) | 699 | if(miS > btScalar(0.f)) |
| 694 | { | 700 | { |
| 695 | factA = miB / miS; | 701 | factA = miB / miS; |
| 696 | } | 702 | } |
| 697 | else | 703 | else |
| 698 | { | 704 | { |
| 699 | factA = btScalar(0.5f); | 705 | factA = btScalar(0.5f); |
| 700 | } | 706 | } |
| 701 | factB = btScalar(1.0f) - factA; | 707 | factB = btScalar(1.0f) - factA; |
| 702 | // get the desired direction of hinge axis | 708 | // get the desired direction of hinge axis |
| 703 | // as weighted sum of Z-orthos of frameA and frameB in WCS | 709 | // as weighted sum of Z-orthos of frameA and frameB in WCS |
| 704 | btVector3 ax1A = trA.getBasis().getColumn(2); | 710 | btVector3 ax1A = trA.getBasis().getColumn(2); |
| 705 | btVector3 ax1B = trB.getBasis().getColumn(2); | 711 | btVector3 ax1B = trB.getBasis().getColumn(2); |
| 706 | btVector3 ax1 = ax1A * factA + ax1B * factB; | 712 | btVector3 ax1 = ax1A * factA + ax1B * factB; |
| 707 | ax1.normalize(); | 713 | ax1.normalize(); |
| 708 | // fill first 3 rows | 714 | // fill first 3 rows |
| 709 | // we want: velA + wA x relA == velB + wB x relB | 715 | // we want: velA + wA x relA == velB + wB x relB |
| 710 | btTransform bodyA_trans = transA; | 716 | btTransform bodyA_trans = transA; |
| 711 | btTransform bodyB_trans = transB; | 717 | btTransform bodyB_trans = transB; |
| 712 | int s0 = 0; | 718 | int s0 = 0; |
| 713 | int s1 = s; | 719 | int s1 = s; |
| 714 | int s2 = s * 2; | 720 | int s2 = s * 2; |
| 715 | int nrow = 2; // last filled row | 721 | int nrow = 2; // last filled row |
| 716 | btVector3 tmpA, tmpB, relA, relB, p, q; | 722 | btVector3 tmpA, tmpB, relA, relB, p, q; |
| 717 | // get vector from bodyB to frameB in WCS | 723 | // get vector from bodyB to frameB in WCS |
| 718 | relB = trB.getOrigin() - bodyB_trans.getOrigin(); | 724 | relB = trB.getOrigin() - bodyB_trans.getOrigin(); |
| 719 | // get its projection to hinge axis | 725 | // get its projection to hinge axis |
| 720 | btVector3 projB = ax1 * relB.dot(ax1); | 726 | btVector3 projB = ax1 * relB.dot(ax1); |
| 721 | // get vector directed from bodyB to hinge axis (and orthogonal to it) | 727 | // get vector directed from bodyB to hinge axis (and orthogonal to it) |
| 722 | btVector3 orthoB = relB - projB; | 728 | btVector3 orthoB = relB - projB; |
| 723 | // same for bodyA | 729 | // same for bodyA |
| 724 | relA = trA.getOrigin() - bodyA_trans.getOrigin(); | 730 | relA = trA.getOrigin() - bodyA_trans.getOrigin(); |
| 725 | btVector3 projA = ax1 * relA.dot(ax1); | 731 | btVector3 projA = ax1 * relA.dot(ax1); |
| 726 | btVector3 orthoA = relA - projA; | 732 | btVector3 orthoA = relA - projA; |
| 727 | btVector3 totalDist = projA - projB; | 733 | btVector3 totalDist = projA - projB; |
| 728 | // get offset vectors relA and relB | 734 | // get offset vectors relA and relB |
| 729 | relA = orthoA + totalDist * factA; | 735 | relA = orthoA + totalDist * factA; |
| 730 | relB = orthoB - totalDist * factB; | 736 | relB = orthoB - totalDist * factB; |
| 731 | // now choose average ortho to hinge axis | 737 | // now choose average ortho to hinge axis |
| 732 | p = orthoB * factA + orthoA * factB; | 738 | p = orthoB * factA + orthoA * factB; |
| 733 | btScalar len2 = p.length2(); | 739 | btScalar len2 = p.length2(); |
| 734 | if(len2 > SIMD_EPSILON) | 740 | if(len2 > SIMD_EPSILON) |
| 735 | { | 741 | { |
| 736 | p /= btSqrt(len2); | 742 | p /= btSqrt(len2); |
| 737 | } | 743 | } |
| 738 | else | 744 | else |
| 739 | { | 745 | { |
| 740 | p = trA.getBasis().getColumn(1); | 746 | p = trA.getBasis().getColumn(1); |
| 741 | } | 747 | } |
| 742 | // make one more ortho | 748 | // make one more ortho |
| 743 | q = ax1.cross(p); | 749 | q = ax1.cross(p); |
| 744 | // fill three rows | 750 | // fill three rows |
| 745 | tmpA = relA.cross(p); | 751 | tmpA = relA.cross(p); |
| 746 | tmpB = relB.cross(p); | 752 | tmpB = relB.cross(p); |
| 747 | for (i=0; i<3; i++) info->m_J1angularAxis[s0+i] = tmpA[i]; | 753 | for (i=0; i<3; i++) info->m_J1angularAxis[s0+i] = tmpA[i]; |
| 748 | for (i=0; i<3; i++) info->m_J2angularAxis[s0+i] = -tmpB[i]; | 754 | for (i=0; i<3; i++) info->m_J2angularAxis[s0+i] = -tmpB[i]; |
| 749 | tmpA = relA.cross(q); | 755 | tmpA = relA.cross(q); |
| 750 | tmpB = relB.cross(q); | 756 | tmpB = relB.cross(q); |
| 751 | if(hasStaticBody && getSolveLimit()) | 757 | if(hasStaticBody && getSolveLimit()) |
| 752 | { // to make constraint between static and dynamic objects more rigid | 758 | { // to make constraint between static and dynamic objects more rigid |
| 753 | // remove wA (or wB) from equation if angular limit is hit | 759 | // remove wA (or wB) from equation if angular limit is hit |
| 754 | tmpB *= factB; | 760 | tmpB *= factB; |
| 755 | tmpA *= factA; | 761 | tmpA *= factA; |
| 756 | } | 762 | } |
| 757 | for (i=0; i<3; i++) info->m_J1angularAxis[s1+i] = tmpA[i]; | 763 | for (i=0; i<3; i++) info->m_J1angularAxis[s1+i] = tmpA[i]; |
| 758 | for (i=0; i<3; i++) info->m_J2angularAxis[s1+i] = -tmpB[i]; | 764 | for (i=0; i<3; i++) info->m_J2angularAxis[s1+i] = -tmpB[i]; |
| 759 | tmpA = relA.cross(ax1); | 765 | tmpA = relA.cross(ax1); |
| 760 | tmpB = relB.cross(ax1); | 766 | tmpB = relB.cross(ax1); |
| 761 | if(hasStaticBody) | 767 | if(hasStaticBody) |
| 762 | { // to make constraint between static and dynamic objects more rigid | 768 | { // to make constraint between static and dynamic objects more rigid |
| 763 | // remove wA (or wB) from equation | 769 | // remove wA (or wB) from equation |
| 764 | tmpB *= factB; | 770 | tmpB *= factB; |
| 765 | tmpA *= factA; | 771 | tmpA *= factA; |
| 766 | } | 772 | } |
| 767 | for (i=0; i<3; i++) info->m_J1angularAxis[s2+i] = tmpA[i]; | 773 | for (i=0; i<3; i++) info->m_J1angularAxis[s2+i] = tmpA[i]; |
| 768 | for (i=0; i<3; i++) info->m_J2angularAxis[s2+i] = -tmpB[i]; | 774 | for (i=0; i<3; i++) info->m_J2angularAxis[s2+i] = -tmpB[i]; |
| 769 | 775 | ||
| 770 | for (i=0; i<3; i++) info->m_J1linearAxis[s0+i] = p[i]; | 776 | for (i=0; i<3; i++) info->m_J1linearAxis[s0+i] = p[i]; |
| 771 | for (i=0; i<3; i++) info->m_J1linearAxis[s1+i] = q[i]; | 777 | for (i=0; i<3; i++) info->m_J1linearAxis[s1+i] = q[i]; |
| 772 | for (i=0; i<3; i++) info->m_J1linearAxis[s2+i] = ax1[i]; | 778 | for (i=0; i<3; i++) info->m_J1linearAxis[s2+i] = ax1[i]; |
| 773 | // compute three elements of right hand side | 779 | // compute three elements of right hand side |
| 774 | btScalar k = info->fps * info->erp; | 780 | btScalar k = info->fps * info->erp; |
| 775 | btScalar rhs = k * p.dot(ofs); | 781 | btScalar rhs = k * p.dot(ofs); |
| 776 | info->m_constraintError[s0] = rhs; | 782 | info->m_constraintError[s0] = rhs; |
| 777 | rhs = k * q.dot(ofs); | 783 | rhs = k * q.dot(ofs); |
| 778 | info->m_constraintError[s1] = rhs; | 784 | info->m_constraintError[s1] = rhs; |
| 779 | rhs = k * ax1.dot(ofs); | 785 | rhs = k * ax1.dot(ofs); |
| 780 | info->m_constraintError[s2] = rhs; | 786 | info->m_constraintError[s2] = rhs; |
| 781 | // the hinge axis should be the only unconstrained | 787 | // the hinge axis should be the only unconstrained |
| 782 | // rotational axis, the angular velocity of the two bodies perpendicular to | 788 | // rotational axis, the angular velocity of the two bodies perpendicular to |
| 783 | // the hinge axis should be equal. thus the constraint equations are | 789 | // the hinge axis should be equal. thus the constraint equations are |
| 784 | // p*w1 - p*w2 = 0 | 790 | // p*w1 - p*w2 = 0 |
| 785 | // q*w1 - q*w2 = 0 | 791 | // q*w1 - q*w2 = 0 |
| 786 | // where p and q are unit vectors normal to the hinge axis, and w1 and w2 | 792 | // where p and q are unit vectors normal to the hinge axis, and w1 and w2 |
| 787 | // are the angular velocity vectors of the two bodies. | 793 | // are the angular velocity vectors of the two bodies. |
| 788 | int s3 = 3 * s; | 794 | int s3 = 3 * s; |
| 789 | int s4 = 4 * s; | 795 | int s4 = 4 * s; |
| 790 | info->m_J1angularAxis[s3 + 0] = p[0]; | 796 | info->m_J1angularAxis[s3 + 0] = p[0]; |
| 791 | info->m_J1angularAxis[s3 + 1] = p[1]; | 797 | info->m_J1angularAxis[s3 + 1] = p[1]; |
| 792 | info->m_J1angularAxis[s3 + 2] = p[2]; | 798 | info->m_J1angularAxis[s3 + 2] = p[2]; |
| 793 | info->m_J1angularAxis[s4 + 0] = q[0]; | 799 | info->m_J1angularAxis[s4 + 0] = q[0]; |
| 794 | info->m_J1angularAxis[s4 + 1] = q[1]; | 800 | info->m_J1angularAxis[s4 + 1] = q[1]; |
| 795 | info->m_J1angularAxis[s4 + 2] = q[2]; | 801 | info->m_J1angularAxis[s4 + 2] = q[2]; |
| 796 | 802 | ||
| 797 | info->m_J2angularAxis[s3 + 0] = -p[0]; | 803 | info->m_J2angularAxis[s3 + 0] = -p[0]; |
| 798 | info->m_J2angularAxis[s3 + 1] = -p[1]; | 804 | info->m_J2angularAxis[s3 + 1] = -p[1]; |
| 799 | info->m_J2angularAxis[s3 + 2] = -p[2]; | 805 | info->m_J2angularAxis[s3 + 2] = -p[2]; |
| 800 | info->m_J2angularAxis[s4 + 0] = -q[0]; | 806 | info->m_J2angularAxis[s4 + 0] = -q[0]; |
| 801 | info->m_J2angularAxis[s4 + 1] = -q[1]; | 807 | info->m_J2angularAxis[s4 + 1] = -q[1]; |
| 802 | info->m_J2angularAxis[s4 + 2] = -q[2]; | 808 | info->m_J2angularAxis[s4 + 2] = -q[2]; |
| 803 | // compute the right hand side of the constraint equation. set relative | 809 | // compute the right hand side of the constraint equation. set relative |
| 804 | // body velocities along p and q to bring the hinge back into alignment. | 810 | // body velocities along p and q to bring the hinge back into alignment. |
| 805 | // if ax1A,ax1B are the unit length hinge axes as computed from bodyA and | 811 | // if ax1A,ax1B are the unit length hinge axes as computed from bodyA and |
| 806 | // bodyB, we need to rotate both bodies along the axis u = (ax1 x ax2). | 812 | // bodyB, we need to rotate both bodies along the axis u = (ax1 x ax2). |
| 807 | // if "theta" is the angle between ax1 and ax2, we need an angular velocity | 813 | // if "theta" is the angle between ax1 and ax2, we need an angular velocity |
| 808 | // along u to cover angle erp*theta in one step : | 814 | // along u to cover angle erp*theta in one step : |
| 809 | // |angular_velocity| = angle/time = erp*theta / stepsize | 815 | // |angular_velocity| = angle/time = erp*theta / stepsize |
| 810 | // = (erp*fps) * theta | 816 | // = (erp*fps) * theta |
| 811 | // angular_velocity = |angular_velocity| * (ax1 x ax2) / |ax1 x ax2| | 817 | // angular_velocity = |angular_velocity| * (ax1 x ax2) / |ax1 x ax2| |
| 812 | // = (erp*fps) * theta * (ax1 x ax2) / sin(theta) | 818 | // = (erp*fps) * theta * (ax1 x ax2) / sin(theta) |
| 813 | // ...as ax1 and ax2 are unit length. if theta is smallish, | 819 | // ...as ax1 and ax2 are unit length. if theta is smallish, |
| 814 | // theta ~= sin(theta), so | 820 | // theta ~= sin(theta), so |
| 815 | // angular_velocity = (erp*fps) * (ax1 x ax2) | 821 | // angular_velocity = (erp*fps) * (ax1 x ax2) |
| 816 | // ax1 x ax2 is in the plane space of ax1, so we project the angular | 822 | // ax1 x ax2 is in the plane space of ax1, so we project the angular |
| 817 | // velocity to p and q to find the right hand side. | 823 | // velocity to p and q to find the right hand side. |
| 818 | k = info->fps * info->erp; | 824 | k = info->fps * info->erp; |
| 819 | btVector3 u = ax1A.cross(ax1B); | 825 | btVector3 u = ax1A.cross(ax1B); |
| 820 | info->m_constraintError[s3] = k * u.dot(p); | 826 | info->m_constraintError[s3] = k * u.dot(p); |
| 821 | info->m_constraintError[s4] = k * u.dot(q); | 827 | info->m_constraintError[s4] = k * u.dot(q); |
| 822 | #endif | 828 | #endif |
| 823 | // check angular limits | 829 | // check angular limits |
| 824 | nrow = 4; // last filled row | 830 | nrow = 4; // last filled row |
| 825 | int srow; | 831 | int srow; |
| 826 | btScalar limit_err = btScalar(0.0); | 832 | btScalar limit_err = btScalar(0.0); |
| 827 | int limit = 0; | 833 | int limit = 0; |
| 828 | if(getSolveLimit()) | 834 | if(getSolveLimit()) |
| 829 | { | 835 | { |
| 830 | limit_err = m_correction * m_referenceSign; | 836 | limit_err = m_correction * m_referenceSign; |
| 831 | limit = (limit_err > btScalar(0.0)) ? 1 : 2; | 837 | limit = (limit_err > btScalar(0.0)) ? 1 : 2; |
| 832 | } | 838 | } |
| 833 | // if the hinge has joint limits or motor, add in the extra row | 839 | // if the hinge has joint limits or motor, add in the extra row |
| 834 | int powered = 0; | 840 | int powered = 0; |
| 835 | if(getEnableAngularMotor()) | 841 | if(getEnableAngularMotor()) |
| 836 | { | 842 | { |
| 837 | powered = 1; | 843 | powered = 1; |
| 838 | } | 844 | } |
| 839 | if(limit || powered) | 845 | if(limit || powered) |
| 840 | { | 846 | { |
| 841 | nrow++; | 847 | nrow++; |
| 842 | srow = nrow * info->rowskip; | 848 | srow = nrow * info->rowskip; |
| 843 | info->m_J1angularAxis[srow+0] = ax1[0]; | 849 | info->m_J1angularAxis[srow+0] = ax1[0]; |
| 844 | info->m_J1angularAxis[srow+1] = ax1[1]; | 850 | info->m_J1angularAxis[srow+1] = ax1[1]; |
| 845 | info->m_J1angularAxis[srow+2] = ax1[2]; | 851 | info->m_J1angularAxis[srow+2] = ax1[2]; |
| 846 | 852 | ||
| 847 | info->m_J2angularAxis[srow+0] = -ax1[0]; | 853 | info->m_J2angularAxis[srow+0] = -ax1[0]; |
| 848 | info->m_J2angularAxis[srow+1] = -ax1[1]; | 854 | info->m_J2angularAxis[srow+1] = -ax1[1]; |
| 849 | info->m_J2angularAxis[srow+2] = -ax1[2]; | 855 | info->m_J2angularAxis[srow+2] = -ax1[2]; |
| 850 | 856 | ||
| 851 | btScalar lostop = getLowerLimit(); | 857 | btScalar lostop = getLowerLimit(); |
| 852 | btScalar histop = getUpperLimit(); | 858 | btScalar histop = getUpperLimit(); |
| 853 | if(limit && (lostop == histop)) | 859 | if(limit && (lostop == histop)) |
| 854 | { // the joint motor is ineffective | 860 | { // the joint motor is ineffective |
| 855 | powered = 0; | 861 | powered = 0; |
| 856 | } | 862 | } |
| 857 | info->m_constraintError[srow] = btScalar(0.0f); | 863 | info->m_constraintError[srow] = btScalar(0.0f); |
| 858 | btScalar currERP = (m_flags & BT_HINGE_FLAGS_ERP_STOP) ? m_stopERP : info->erp; | 864 | btScalar currERP = (m_flags & BT_HINGE_FLAGS_ERP_STOP) ? m_stopERP : info->erp; |
| 859 | if(powered) | 865 | if(powered) |
| 860 | { | 866 | { |
| 861 | if(m_flags & BT_HINGE_FLAGS_CFM_NORM) | 867 | if(m_flags & BT_HINGE_FLAGS_CFM_NORM) |
| 862 | { | 868 | { |
| 863 | info->cfm[srow] = m_normalCFM; | 869 | info->cfm[srow] = m_normalCFM; |
| 864 | } | 870 | } |
| 865 | btScalar mot_fact = getMotorFactor(m_hingeAngle, lostop, histop, m_motorTargetVelocity, info->fps * currERP); | 871 | btScalar mot_fact = getMotorFactor(m_hingeAngle, lostop, histop, m_motorTargetVelocity, info->fps * currERP); |
| 866 | info->m_constraintError[srow] += mot_fact * m_motorTargetVelocity * m_referenceSign; | 872 | info->m_constraintError[srow] += mot_fact * m_motorTargetVelocity * m_referenceSign; |
| 867 | info->m_lowerLimit[srow] = - m_maxMotorImpulse; | 873 | info->m_lowerLimit[srow] = - m_maxMotorImpulse; |
| 868 | info->m_upperLimit[srow] = m_maxMotorImpulse; | 874 | info->m_upperLimit[srow] = m_maxMotorImpulse; |
| 869 | } | 875 | } |
| 870 | if(limit) | 876 | if(limit) |
| 871 | { | 877 | { |
| 872 | k = info->fps * currERP; | 878 | k = info->fps * currERP; |
| 873 | info->m_constraintError[srow] += k * limit_err; | 879 | info->m_constraintError[srow] += k * limit_err; |
| 874 | if(m_flags & BT_HINGE_FLAGS_CFM_STOP) | 880 | if(m_flags & BT_HINGE_FLAGS_CFM_STOP) |
| 875 | { | 881 | { |
| 876 | info->cfm[srow] = m_stopCFM; | 882 | info->cfm[srow] = m_stopCFM; |
| 877 | } | 883 | } |
| 878 | if(lostop == histop) | 884 | if(lostop == histop) |
| 879 | { | 885 | { |
| 880 | // limited low and high simultaneously | 886 | // limited low and high simultaneously |
| 881 | info->m_lowerLimit[srow] = -SIMD_INFINITY; | 887 | info->m_lowerLimit[srow] = -SIMD_INFINITY; |
| 882 | info->m_upperLimit[srow] = SIMD_INFINITY; | 888 | info->m_upperLimit[srow] = SIMD_INFINITY; |
| 883 | } | 889 | } |
| 884 | else if(limit == 1) | 890 | else if(limit == 1) |
| 885 | { // low limit | 891 | { // low limit |
| 886 | info->m_lowerLimit[srow] = 0; | 892 | info->m_lowerLimit[srow] = 0; |
| 887 | info->m_upperLimit[srow] = SIMD_INFINITY; | 893 | info->m_upperLimit[srow] = SIMD_INFINITY; |
| 888 | } | 894 | } |
| 889 | else | 895 | else |
| 890 | { // high limit | 896 | { // high limit |
| 891 | info->m_lowerLimit[srow] = -SIMD_INFINITY; | 897 | info->m_lowerLimit[srow] = -SIMD_INFINITY; |
| 892 | info->m_upperLimit[srow] = 0; | 898 | info->m_upperLimit[srow] = 0; |
| 893 | } | 899 | } |
| 894 | // bounce (we'll use slider parameter abs(1.0 - m_dampingLimAng) for that) | 900 | // bounce (we'll use slider parameter abs(1.0 - m_dampingLimAng) for that) |
| 895 | btScalar bounce = m_relaxationFactor; | 901 | btScalar bounce = m_relaxationFactor; |
| 896 | if(bounce > btScalar(0.0)) | 902 | if(bounce > btScalar(0.0)) |
| 897 | { | 903 | { |
| 898 | btScalar vel = angVelA.dot(ax1); | 904 | btScalar vel = angVelA.dot(ax1); |
| 899 | vel -= angVelB.dot(ax1); | 905 | vel -= angVelB.dot(ax1); |
| 900 | // only apply bounce if the velocity is incoming, and if the | 906 | // only apply bounce if the velocity is incoming, and if the |
| 901 | // resulting c[] exceeds what we already have. | 907 | // resulting c[] exceeds what we already have. |
| 902 | if(limit == 1) | 908 | if(limit == 1) |
| 903 | { // low limit | 909 | { // low limit |
| 904 | if(vel < 0) | 910 | if(vel < 0) |
| 905 | { | 911 | { |
| 906 | btScalar newc = -bounce * vel; | 912 | btScalar newc = -bounce * vel; |
| 907 | if(newc > info->m_constraintError[srow]) | 913 | if(newc > info->m_constraintError[srow]) |
| 908 | { | 914 | { |
| 909 | info->m_constraintError[srow] = newc; | 915 | info->m_constraintError[srow] = newc; |
| 910 | } | 916 | } |
| 911 | } | 917 | } |
| 912 | } | 918 | } |
| 913 | else | 919 | else |
| 914 | { // high limit - all those computations are reversed | 920 | { // high limit - all those computations are reversed |
| 915 | if(vel > 0) | 921 | if(vel > 0) |
| 916 | { | 922 | { |
| 917 | btScalar newc = -bounce * vel; | 923 | btScalar newc = -bounce * vel; |
| 918 | if(newc < info->m_constraintError[srow]) | 924 | if(newc < info->m_constraintError[srow]) |
| 919 | { | 925 | { |
| 920 | info->m_constraintError[srow] = newc; | 926 | info->m_constraintError[srow] = newc; |
| 921 | } | 927 | } |
| 922 | } | 928 | } |
| 923 | } | 929 | } |
| 924 | } | 930 | } |
| 925 | info->m_constraintError[srow] *= m_biasFactor; | 931 | info->m_constraintError[srow] *= m_biasFactor; |
| 926 | } // if(limit) | 932 | } // if(limit) |
| 927 | } // if angular limit or powered | 933 | } // if angular limit or powered |
| 928 | } | 934 | } |
| 929 | 935 | ||
| 930 | 936 | ||
| 931 | ///override the default global value of a parameter (such as ERP or CFM), optionally provide the axis (0..5). | 937 | ///override the default global value of a parameter (such as ERP or CFM), optionally provide the axis (0..5). |
| 932 | ///If no axis is provided, it uses the default axis for this constraint. | 938 | ///If no axis is provided, it uses the default axis for this constraint. |
| 933 | void btHingeConstraint::setParam(int num, btScalar value, int axis) | 939 | void btHingeConstraint::setParam(int num, btScalar value, int axis) |
| 934 | { | 940 | { |
| 935 | if((axis == -1) || (axis == 5)) | 941 | if((axis == -1) || (axis == 5)) |
| 936 | { | 942 | { |
| 937 | switch(num) | 943 | switch(num) |
| 938 | { | 944 | { |
| 939 | case BT_CONSTRAINT_STOP_ERP : | 945 | case BT_CONSTRAINT_STOP_ERP : |
| 940 | m_stopERP = value; | 946 | m_stopERP = value; |
| 941 | m_flags |= BT_HINGE_FLAGS_ERP_STOP; | 947 | m_flags |= BT_HINGE_FLAGS_ERP_STOP; |
| 942 | break; | 948 | break; |
| 943 | case BT_CONSTRAINT_STOP_CFM : | 949 | case BT_CONSTRAINT_STOP_CFM : |
| 944 | m_stopCFM = value; | 950 | m_stopCFM = value; |
| 945 | m_flags |= BT_HINGE_FLAGS_CFM_STOP; | 951 | m_flags |= BT_HINGE_FLAGS_CFM_STOP; |
| 946 | break; | 952 | break; |
| 947 | case BT_CONSTRAINT_CFM : | 953 | case BT_CONSTRAINT_CFM : |
| 948 | m_normalCFM = value; | 954 | m_normalCFM = value; |
| 949 | m_flags |= BT_HINGE_FLAGS_CFM_NORM; | 955 | m_flags |= BT_HINGE_FLAGS_CFM_NORM; |
| 950 | break; | 956 | break; |
| 951 | default : | 957 | default : |
| 952 | btAssertConstrParams(0); | 958 | btAssertConstrParams(0); |
| 953 | } | 959 | } |
| 954 | } | 960 | } |
| 955 | else | 961 | else |
| 956 | { | 962 | { |
| 957 | btAssertConstrParams(0); | 963 | btAssertConstrParams(0); |
| 958 | } | 964 | } |
| 959 | } | 965 | } |
| 960 | 966 | ||
| 961 | ///return the local value of parameter | 967 | ///return the local value of parameter |
| 962 | btScalar btHingeConstraint::getParam(int num, int axis) const | 968 | btScalar btHingeConstraint::getParam(int num, int axis) const |
| 963 | { | 969 | { |
| 964 | btScalar retVal = 0; | 970 | btScalar retVal = 0; |
| 965 | if((axis == -1) || (axis == 5)) | 971 | if((axis == -1) || (axis == 5)) |
| 966 | { | 972 | { |
| 967 | switch(num) | 973 | switch(num) |
| 968 | { | 974 | { |
| 969 | case BT_CONSTRAINT_STOP_ERP : | 975 | case BT_CONSTRAINT_STOP_ERP : |
| 970 | btAssertConstrParams(m_flags & BT_HINGE_FLAGS_ERP_STOP); | 976 | btAssertConstrParams(m_flags & BT_HINGE_FLAGS_ERP_STOP); |
| 971 | retVal = m_stopERP; | 977 | retVal = m_stopERP; |
| 972 | break; | 978 | break; |
| 973 | case BT_CONSTRAINT_STOP_CFM : | 979 | case BT_CONSTRAINT_STOP_CFM : |
| 974 | btAssertConstrParams(m_flags & BT_HINGE_FLAGS_CFM_STOP); | 980 | btAssertConstrParams(m_flags & BT_HINGE_FLAGS_CFM_STOP); |
| 975 | retVal = m_stopCFM; | 981 | retVal = m_stopCFM; |
| 976 | break; | 982 | break; |
| 977 | case BT_CONSTRAINT_CFM : | 983 | case BT_CONSTRAINT_CFM : |
| 978 | btAssertConstrParams(m_flags & BT_HINGE_FLAGS_CFM_NORM); | 984 | btAssertConstrParams(m_flags & BT_HINGE_FLAGS_CFM_NORM); |
| 979 | retVal = m_normalCFM; | 985 | retVal = m_normalCFM; |
| 980 | break; | 986 | break; |
| 981 | default : | 987 | default : |
| 982 | btAssertConstrParams(0); | 988 | btAssertConstrParams(0); |
| 983 | } | 989 | } |
| 984 | } | 990 | } |
| 985 | else | 991 | else |
| 986 | { | 992 | { |
| 987 | btAssertConstrParams(0); | 993 | btAssertConstrParams(0); |
| 988 | } | 994 | } |
| 989 | return retVal; | 995 | return retVal; |
| 990 | } | 996 | } |
| 991 | 997 | ||
| 992 | 998 | ||
|
|