| /* | |
| Bullet Continuous Collision Detection and Physics Library | |
| Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/ | |
| This software is provided 'as-is', without any express or implied warranty. | |
| In no event will the authors be held liable for any damages arising from the use of this software. | |
| Permission is granted to anyone to use this software for any purpose, | |
| including commercial applications, and to alter it and redistribute it freely, | |
| subject to the following restrictions: | |
| 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. | |
| 2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. | |
| 3. This notice may not be removed or altered from any source distribution. | |
| */ | |
| #include "BulletDynamics/ConstraintSolver/btHingeConstraint.h" | |
| #include "BulletDynamics/Dynamics/btRigidBody.h" | |
| #include "LinearMath/btTransformUtil.h" | |
| #include "LinearMath/btMinMax.h" | |
| #include <new> | |
| #include "BulletDynamics/ConstraintSolver/btSolverBody.h" | |
| #define HINGE_USE_OBSOLETE_SOLVER false | |
| #ifndef __SPU__ | |
| btHingeConstraint::btHingeConstraint() | |
| : btTypedConstraint (HINGE_CONSTRAINT_TYPE), | |
| m_enableAngularMotor(false), | |
| m_useSolveConstraintObsolete(HINGE_USE_OBSOLETE_SOLVER), | |
| m_useReferenceFrameA(false) | |
| { | |
| m_referenceSign = m_useReferenceFrameA ? btScalar(-1.f) : btScalar(1.f); | |
| } | |
| btHingeConstraint::btHingeConstraint(btRigidBody& rbA,btRigidBody& rbB, const btVector3& pivotInA,const btVector3& pivotInB, | |
| btVector3& axisInA,btVector3& axisInB, bool useReferenceFrameA) | |
| :btTypedConstraint(HINGE_CONSTRAINT_TYPE, rbA,rbB), | |
| m_angularOnly(false), | |
| m_enableAngularMotor(false), | |
| m_useSolveConstraintObsolete(HINGE_USE_OBSOLETE_SOLVER), | |
| m_useReferenceFrameA(useReferenceFrameA) | |
| { | |
| m_rbAFrame.getOrigin() = pivotInA; | |
| // since no frame is given, assume this to be zero angle and just pick rb transform axis | |
| btVector3 rbAxisA1 = rbA.getCenterOfMassTransform().getBasis().getColumn(0); | |
| btVector3 rbAxisA2; | |
| btScalar projection = axisInA.dot(rbAxisA1); | |
| if (projection >= 1.0f - SIMD_EPSILON) { | |
| rbAxisA1 = -rbA.getCenterOfMassTransform().getBasis().getColumn(2); | |
| rbAxisA2 = rbA.getCenterOfMassTransform().getBasis().getColumn(1); | |
| } else if (projection <= -1.0f + SIMD_EPSILON) { | |
| rbAxisA1 = rbA.getCenterOfMassTransform().getBasis().getColumn(2); | |
| rbAxisA2 = rbA.getCenterOfMassTransform().getBasis().getColumn(1); | |
| } else { | |
| rbAxisA2 = axisInA.cross(rbAxisA1); | |
| rbAxisA1 = rbAxisA2.cross(axisInA); | |
| } | |
| m_rbAFrame.getBasis().setValue( rbAxisA1.getX(),rbAxisA2.getX(),axisInA.getX(), | |
| rbAxisA1.getY(),rbAxisA2.getY(),axisInA.getY(), | |
| rbAxisA1.getZ(),rbAxisA2.getZ(),axisInA.getZ() ); | |
| btQuaternion rotationArc = shortestArcQuat(axisInA,axisInB); | |
| btVector3 rbAxisB1 = quatRotate(rotationArc,rbAxisA1); | |
| btVector3 rbAxisB2 = axisInB.cross(rbAxisB1); | |
| m_rbBFrame.getOrigin() = pivotInB; | |
| m_rbBFrame.getBasis().setValue( rbAxisB1.getX(),rbAxisB2.getX(),axisInB.getX(), | |
| rbAxisB1.getY(),rbAxisB2.getY(),axisInB.getY(), | |
| rbAxisB1.getZ(),rbAxisB2.getZ(),axisInB.getZ() ); | |
| //start with free | |
| m_lowerLimit = btScalar(1.0f); | |
| m_upperLimit = btScalar(-1.0f); | |
| m_biasFactor = 0.3f; | |
| m_relaxationFactor = 1.0f; | |
| m_limitSoftness = 0.9f; | |
| m_solveLimit = false; | |
| m_referenceSign = m_useReferenceFrameA ? btScalar(-1.f) : btScalar(1.f); | |
| } | |
| btHingeConstraint::btHingeConstraint(btRigidBody& rbA,const btVector3& pivotInA,btVector3& axisInA, bool useReferenceFrameA) | |
| :btTypedConstraint(HINGE_CONSTRAINT_TYPE, rbA), m_angularOnly(false), m_enableAngularMotor(false), | |
| m_useSolveConstraintObsolete(HINGE_USE_OBSOLETE_SOLVER), | |
| m_useReferenceFrameA(useReferenceFrameA) | |
| { | |
| // since no frame is given, assume this to be zero angle and just pick rb transform axis | |
| // fixed axis in worldspace | |
| btVector3 rbAxisA1, rbAxisA2; | |
| btPlaneSpace1(axisInA, rbAxisA1, rbAxisA2); | |
| m_rbAFrame.getOrigin() = pivotInA; | |
| m_rbAFrame.getBasis().setValue( rbAxisA1.getX(),rbAxisA2.getX(),axisInA.getX(), | |
| rbAxisA1.getY(),rbAxisA2.getY(),axisInA.getY(), | |
| rbAxisA1.getZ(),rbAxisA2.getZ(),axisInA.getZ() ); | |
| btVector3 axisInB = rbA.getCenterOfMassTransform().getBasis() * axisInA; | |
| btQuaternion rotationArc = shortestArcQuat(axisInA,axisInB); | |
| btVector3 rbAxisB1 = quatRotate(rotationArc,rbAxisA1); | |
| btVector3 rbAxisB2 = axisInB.cross(rbAxisB1); | |
| m_rbBFrame.getOrigin() = rbA.getCenterOfMassTransform()(pivotInA); | |
| m_rbBFrame.getBasis().setValue( rbAxisB1.getX(),rbAxisB2.getX(),axisInB.getX(), | |
| rbAxisB1.getY(),rbAxisB2.getY(),axisInB.getY(), | |
| rbAxisB1.getZ(),rbAxisB2.getZ(),axisInB.getZ() ); | |
| //start with free | |
| m_lowerLimit = btScalar(1.0f); | |
| m_upperLimit = btScalar(-1.0f); | |
| m_biasFactor = 0.3f; | |
| m_relaxationFactor = 1.0f; | |
| m_limitSoftness = 0.9f; | |
| m_solveLimit = false; | |
| m_referenceSign = m_useReferenceFrameA ? btScalar(-1.f) : btScalar(1.f); | |
| } | |
| btHingeConstraint::btHingeConstraint(btRigidBody& rbA,btRigidBody& rbB, | |
| const btTransform& rbAFrame, const btTransform& rbBFrame, bool useReferenceFrameA) | |
| :btTypedConstraint(HINGE_CONSTRAINT_TYPE, rbA,rbB),m_rbAFrame(rbAFrame),m_rbBFrame(rbBFrame), | |
| m_angularOnly(false), | |
| m_enableAngularMotor(false), | |
| m_useSolveConstraintObsolete(HINGE_USE_OBSOLETE_SOLVER), | |
| m_useReferenceFrameA(useReferenceFrameA) | |
| { | |
| //start with free | |
| m_lowerLimit = btScalar(1.0f); | |
| m_upperLimit = btScalar(-1.0f); | |
| m_biasFactor = 0.3f; | |
| m_relaxationFactor = 1.0f; | |
| m_limitSoftness = 0.9f; | |
| m_solveLimit = false; | |
| m_referenceSign = m_useReferenceFrameA ? btScalar(-1.f) : btScalar(1.f); | |
| } | |
| btHingeConstraint::btHingeConstraint(btRigidBody& rbA, const btTransform& rbAFrame, bool useReferenceFrameA) | |
| :btTypedConstraint(HINGE_CONSTRAINT_TYPE, rbA),m_rbAFrame(rbAFrame),m_rbBFrame(rbAFrame), | |
| m_angularOnly(false), | |
| m_enableAngularMotor(false), | |
| m_useSolveConstraintObsolete(HINGE_USE_OBSOLETE_SOLVER), | |
| m_useReferenceFrameA(useReferenceFrameA) | |
| { | |
| ///not providing rigidbody B means implicitly using worldspace for body B | |
| m_rbBFrame.getOrigin() = m_rbA.getCenterOfMassTransform()(m_rbAFrame.getOrigin()); | |
| //start with free | |
| m_lowerLimit = btScalar(1.0f); | |
| m_upperLimit = btScalar(-1.0f); | |
| m_biasFactor = 0.3f; | |
| m_relaxationFactor = 1.0f; | |
| m_limitSoftness = 0.9f; | |
| m_solveLimit = false; | |
| m_referenceSign = m_useReferenceFrameA ? btScalar(-1.f) : btScalar(1.f); | |
| } | |
| void btHingeConstraint::buildJacobian() | |
| { | |
| if (m_useSolveConstraintObsolete) | |
| { | |
| m_appliedImpulse = btScalar(0.); | |
| m_accMotorImpulse = btScalar(0.); | |
| if (!m_angularOnly) | |
| { | |
| btVector3 pivotAInW = m_rbA.getCenterOfMassTransform()*m_rbAFrame.getOrigin(); | |
| btVector3 pivotBInW = m_rbB.getCenterOfMassTransform()*m_rbBFrame.getOrigin(); | |
| btVector3 relPos = pivotBInW - pivotAInW; | |
| btVector3 normal[3]; | |
| if (relPos.length2() > SIMD_EPSILON) | |
| { | |
| normal[0] = relPos.normalized(); | |
| } | |
| else | |
| { | |
| normal[0].setValue(btScalar(1.0),0,0); | |
| } | |
| btPlaneSpace1(normal[0], normal[1], normal[2]); | |
| for (int i=0;i<3;i++) | |
| { | |
| new (&m_jac[i]) btJacobianEntry( | |
| m_rbA.getCenterOfMassTransform().getBasis().transpose(), | |
| m_rbB.getCenterOfMassTransform().getBasis().transpose(), | |
| pivotAInW - m_rbA.getCenterOfMassPosition(), | |
| pivotBInW - m_rbB.getCenterOfMassPosition(), | |
| normal[i], | |
| m_rbA.getInvInertiaDiagLocal(), | |
| m_rbA.getInvMass(), | |
| m_rbB.getInvInertiaDiagLocal(), | |
| m_rbB.getInvMass()); | |
| } | |
| } | |
| //calculate two perpendicular jointAxis, orthogonal to hingeAxis | |
| //these two jointAxis require equal angular velocities for both bodies | |
| //this is unused for now, it's a todo | |
| btVector3 jointAxis0local; | |
| btVector3 jointAxis1local; | |
| btPlaneSpace1(m_rbAFrame.getBasis().getColumn(2),jointAxis0local,jointAxis1local); | |
| getRigidBodyA().getCenterOfMassTransform().getBasis() * m_rbAFrame.getBasis().getColumn(2); | |
| btVector3 jointAxis0 = getRigidBodyA().getCenterOfMassTransform().getBasis() * jointAxis0local; | |
| btVector3 jointAxis1 = getRigidBodyA().getCenterOfMassTransform().getBasis() * jointAxis1local; | |
| btVector3 hingeAxisWorld = getRigidBodyA().getCenterOfMassTransform().getBasis() * m_rbAFrame.getBasis().getColumn(2); | |
| new (&m_jacAng[0]) btJacobianEntry(jointAxis0, | |
| m_rbA.getCenterOfMassTransform().getBasis().transpose(), | |
| m_rbB.getCenterOfMassTransform().getBasis().transpose(), | |
| m_rbA.getInvInertiaDiagLocal(), | |
| m_rbB.getInvInertiaDiagLocal()); | |
| new (&m_jacAng[1]) btJacobianEntry(jointAxis1, | |
| m_rbA.getCenterOfMassTransform().getBasis().transpose(), | |
| m_rbB.getCenterOfMassTransform().getBasis().transpose(), | |
| m_rbA.getInvInertiaDiagLocal(), | |
| m_rbB.getInvInertiaDiagLocal()); | |
| new (&m_jacAng[2]) btJacobianEntry(hingeAxisWorld, | |
| m_rbA.getCenterOfMassTransform().getBasis().transpose(), | |
| m_rbB.getCenterOfMassTransform().getBasis().transpose(), | |
| m_rbA.getInvInertiaDiagLocal(), | |
| m_rbB.getInvInertiaDiagLocal()); | |
| // clear accumulator | |
| m_accLimitImpulse = btScalar(0.); | |
| // test angular limit | |
| testLimit(m_rbA.getCenterOfMassTransform(),m_rbB.getCenterOfMassTransform()); | |
| //Compute K = J*W*J' for hinge axis | |
| btVector3 axisA = getRigidBodyA().getCenterOfMassTransform().getBasis() * m_rbAFrame.getBasis().getColumn(2); | |
| m_kHinge = 1.0f / (getRigidBodyA().computeAngularImpulseDenominator(axisA) + | |
| getRigidBodyB().computeAngularImpulseDenominator(axisA)); | |
| } | |
| } | |
| void btHingeConstraint::solveConstraintObsolete(btSolverBody& bodyA,btSolverBody& bodyB,btScalar timeStep) | |
| { | |
| ///for backwards compatibility during the transition to 'getInfo/getInfo2' | |
| if (m_useSolveConstraintObsolete) | |
| { | |
| btVector3 pivotAInW = m_rbA.getCenterOfMassTransform()*m_rbAFrame.getOrigin(); | |
| btVector3 pivotBInW = m_rbB.getCenterOfMassTransform()*m_rbBFrame.getOrigin(); | |
| btScalar tau = btScalar(0.3); | |
| //linear part | |
| if (!m_angularOnly) | |
| { | |
| btVector3 rel_pos1 = pivotAInW - m_rbA.getCenterOfMassPosition(); | |
| btVector3 rel_pos2 = pivotBInW - m_rbB.getCenterOfMassPosition(); | |
| btVector3 vel1,vel2; | |
| bodyA.getVelocityInLocalPointObsolete(rel_pos1,vel1); | |
| bodyB.getVelocityInLocalPointObsolete(rel_pos2,vel2); | |
| btVector3 vel = vel1 - vel2; | |
| for (int i=0;i<3;i++) | |
| { | |
| const btVector3& normal = m_jac[i].m_linearJointAxis; | |
| btScalar jacDiagABInv = btScalar(1.) / m_jac[i].getDiagonal(); | |
| btScalar rel_vel; | |
| rel_vel = normal.dot(vel); | |
| //positional error (zeroth order error) | |
| btScalar depth = -(pivotAInW - pivotBInW).dot(normal); //this is the error projected on the normal | |
| btScalar impulse = depth*tau/timeStep * jacDiagABInv - rel_vel * jacDiagABInv; | |
| m_appliedImpulse += impulse; | |
| btVector3 impulse_vector = normal * impulse; | |
| btVector3 ftorqueAxis1 = rel_pos1.cross(normal); | |
| btVector3 ftorqueAxis2 = rel_pos2.cross(normal); | |
| bodyA.applyImpulse(normal*m_rbA.getInvMass(), m_rbA.getInvInertiaTensorWorld()*ftorqueAxis1,impulse); | |
| bodyB.applyImpulse(normal*m_rbB.getInvMass(), m_rbB.getInvInertiaTensorWorld()*ftorqueAxis2,-impulse); | |
| } | |
| } | |
| { | |
| ///solve angular part | |
| // get axes in world space | |
| btVector3 axisA = getRigidBodyA().getCenterOfMassTransform().getBasis() * m_rbAFrame.getBasis().getColumn(2); | |
| btVector3 axisB = getRigidBodyB().getCenterOfMassTransform().getBasis() * m_rbBFrame.getBasis().getColumn(2); | |
| btVector3 angVelA; | |
| bodyA.getAngularVelocity(angVelA); | |
| btVector3 angVelB; | |
| bodyB.getAngularVelocity(angVelB); | |
| btVector3 angVelAroundHingeAxisA = axisA * axisA.dot(angVelA); | |
| btVector3 angVelAroundHingeAxisB = axisB * axisB.dot(angVelB); | |
| btVector3 angAorthog = angVelA - angVelAroundHingeAxisA; | |
| btVector3 angBorthog = angVelB - angVelAroundHingeAxisB; | |
| btVector3 velrelOrthog = angAorthog-angBorthog; | |
| { | |
| //solve orthogonal angular velocity correction | |
| //btScalar relaxation = btScalar(1.); | |
| btScalar len = velrelOrthog.length(); | |
| if (len > btScalar(0.00001)) | |
| { | |
| btVector3 normal = velrelOrthog.normalized(); | |
| btScalar denom = getRigidBodyA().computeAngularImpulseDenominator(normal) + | |
| getRigidBodyB().computeAngularImpulseDenominator(normal); | |
| // scale for mass and relaxation | |
| //velrelOrthog *= (btScalar(1.)/denom) * m_relaxationFactor; | |
| bodyA.applyImpulse(btVector3(0,0,0), m_rbA.getInvInertiaTensorWorld()*velrelOrthog,-(btScalar(1.)/denom)); | |
| bodyB.applyImpulse(btVector3(0,0,0), m_rbB.getInvInertiaTensorWorld()*velrelOrthog,(btScalar(1.)/denom)); | |
| } | |
| //solve angular positional correction | |
| btVector3 angularError = axisA.cross(axisB) *(btScalar(1.)/timeStep); | |
| btScalar len2 = angularError.length(); | |
| if (len2>btScalar(0.00001)) | |
| { | |
| btVector3 normal2 = angularError.normalized(); | |
| btScalar denom2 = getRigidBodyA().computeAngularImpulseDenominator(normal2) + | |
| getRigidBodyB().computeAngularImpulseDenominator(normal2); | |
| //angularError *= (btScalar(1.)/denom2) * relaxation; | |
| bodyA.applyImpulse(btVector3(0,0,0), m_rbA.getInvInertiaTensorWorld()*angularError,(btScalar(1.)/denom2)); | |
| bodyB.applyImpulse(btVector3(0,0,0), m_rbB.getInvInertiaTensorWorld()*angularError,-(btScalar(1.)/denom2)); | |
| } | |
| // solve limit | |
| if (m_solveLimit) | |
| { | |
| btScalar amplitude = ( (angVelB - angVelA).dot( axisA )*m_relaxationFactor + m_correction* (btScalar(1.)/timeStep)*m_biasFactor ) * m_limitSign; | |
| btScalar impulseMag = amplitude * m_kHinge; | |
| // Clamp the accumulated impulse | |
| btScalar temp = m_accLimitImpulse; | |
| m_accLimitImpulse = btMax(m_accLimitImpulse + impulseMag, btScalar(0) ); | |
| impulseMag = m_accLimitImpulse - temp; | |
| bodyA.applyImpulse(btVector3(0,0,0), m_rbA.getInvInertiaTensorWorld()*axisA,impulseMag * m_limitSign); | |
| bodyB.applyImpulse(btVector3(0,0,0), m_rbB.getInvInertiaTensorWorld()*axisA,-(impulseMag * m_limitSign)); | |
| } | |
| } | |
| //apply motor | |
| if (m_enableAngularMotor) | |
| { | |
| //todo: add limits too | |
| btVector3 angularLimit(0,0,0); | |
| btVector3 velrel = angVelAroundHingeAxisA - angVelAroundHingeAxisB; | |
| btScalar projRelVel = velrel.dot(axisA); | |
| btScalar desiredMotorVel = m_motorTargetVelocity; | |
| btScalar motor_relvel = desiredMotorVel - projRelVel; | |
| btScalar unclippedMotorImpulse = m_kHinge * motor_relvel;; | |
| // accumulated impulse clipping: | |
| btScalar fMaxImpulse = m_maxMotorImpulse; | |
| btScalar newAccImpulse = m_accMotorImpulse + unclippedMotorImpulse; | |
| btScalar clippedMotorImpulse = unclippedMotorImpulse; | |
| if (newAccImpulse > fMaxImpulse) | |
| { | |
| newAccImpulse = fMaxImpulse; | |
| clippedMotorImpulse = newAccImpulse - m_accMotorImpulse; | |
| } | |
| else if (newAccImpulse < -fMaxImpulse) | |
| { | |
| newAccImpulse = -fMaxImpulse; | |
| clippedMotorImpulse = newAccImpulse - m_accMotorImpulse; | |
| } | |
| m_accMotorImpulse += clippedMotorImpulse; | |
| bodyA.applyImpulse(btVector3(0,0,0), m_rbA.getInvInertiaTensorWorld()*axisA,clippedMotorImpulse); | |
| bodyB.applyImpulse(btVector3(0,0,0), m_rbB.getInvInertiaTensorWorld()*axisA,-clippedMotorImpulse); | |
| } | |
| } | |
| } | |
| } | |
| #endif //__SPU__ | |
| void btHingeConstraint::getInfo1(btConstraintInfo1* info) | |
| { | |
| if (m_useSolveConstraintObsolete) | |
| { | |
| info->m_numConstraintRows = 0; | |
| info->nub = 0; | |
| } | |
| else | |
| { | |
| info->m_numConstraintRows = 5; // Fixed 3 linear + 2 angular | |
| info->nub = 1; | |
| //always add the row, to avoid computation (data is not available yet) | |
| //prepare constraint | |
| testLimit(m_rbA.getCenterOfMassTransform(),m_rbB.getCenterOfMassTransform()); | |
| if(getSolveLimit() || getEnableAngularMotor()) | |
| { | |
| info->m_numConstraintRows++; // limit 3rd anguar as well | |
| info->nub--; | |
| } | |
| } | |
| } | |
| void btHingeConstraint::getInfo1NonVirtual(btConstraintInfo1* info) | |
| { | |
| if (m_useSolveConstraintObsolete) | |
| { | |
| info->m_numConstraintRows = 0; | |
| info->nub = 0; | |
| } | |
| else | |
| { | |
| //always add the 'limit' row, to avoid computation (data is not available yet) | |
| info->m_numConstraintRows = 6; // Fixed 3 linear + 2 angular | |
| info->nub = 0; | |
| } | |
| } | |
| void btHingeConstraint::getInfo2 (btConstraintInfo2* info) | |
| { | |
| getInfo2Internal(info, m_rbA.getCenterOfMassTransform(),m_rbB.getCenterOfMassTransform(),m_rbA.getAngularVelocity(),m_rbB.getAngularVelocity()); | |
| } | |
| void btHingeConstraint::getInfo2NonVirtual (btConstraintInfo2* info,const btTransform& transA,const btTransform& transB,const btVector3& angVelA,const btVector3& angVelB) | |
| { | |
| ///the regular (virtual) implementation getInfo2 already performs 'testLimit' during getInfo1, so we need to do it now | |
| testLimit(transA,transB); | |
| getInfo2Internal(info,transA,transB,angVelA,angVelB); | |
| } | |
| void btHingeConstraint::getInfo2Internal(btConstraintInfo2* info, const btTransform& transA,const btTransform& transB,const btVector3& angVelA,const btVector3& angVelB) | |
| { | |
| btAssert(!m_useSolveConstraintObsolete); | |
| int i, skip = info->rowskip; | |
| // transforms in world space | |
| btTransform trA = transA*m_rbAFrame; | |
| btTransform trB = transB*m_rbBFrame; | |
| // pivot point | |
| btVector3 pivotAInW = trA.getOrigin(); | |
| btVector3 pivotBInW = trB.getOrigin(); | |
| #if 0 | |
| if (0) | |
| { | |
| for (i=0;i<6;i++) | |
| { | |
| info->m_J1linearAxis[i*skip]=0; | |
| info->m_J1linearAxis[i*skip+1]=0; | |
| info->m_J1linearAxis[i*skip+2]=0; | |
| info->m_J1angularAxis[i*skip]=0; | |
| info->m_J1angularAxis[i*skip+1]=0; | |
| info->m_J1angularAxis[i*skip+2]=0; | |
| info->m_J2angularAxis[i*skip]=0; | |
| info->m_J2angularAxis[i*skip+1]=0; | |
| info->m_J2angularAxis[i*skip+2]=0; | |
| info->m_constraintError[i*skip]=0.f; | |
| } | |
| } | |
| #endif //#if 0 | |
| // linear (all fixed) | |
| info->m_J1linearAxis[0] = 1; | |
| info->m_J1linearAxis[skip + 1] = 1; | |
| info->m_J1linearAxis[2 * skip + 2] = 1; | |
| btVector3 a1 = pivotAInW - transA.getOrigin(); | |
| { | |
| btVector3* angular0 = (btVector3*)(info->m_J1angularAxis); | |
| btVector3* angular1 = (btVector3*)(info->m_J1angularAxis + skip); | |
| btVector3* angular2 = (btVector3*)(info->m_J1angularAxis + 2 * skip); | |
| btVector3 a1neg = -a1; | |
| a1neg.getSkewSymmetricMatrix(angular0,angular1,angular2); | |
| } | |
| btVector3 a2 = pivotBInW - transB.getOrigin(); | |
| { | |
| btVector3* angular0 = (btVector3*)(info->m_J2angularAxis); | |
| btVector3* angular1 = (btVector3*)(info->m_J2angularAxis + skip); | |
| btVector3* angular2 = (btVector3*)(info->m_J2angularAxis + 2 * skip); | |
| a2.getSkewSymmetricMatrix(angular0,angular1,angular2); | |
| } | |
| // linear RHS | |
| btScalar k = info->fps * info->erp; | |
| for(i = 0; i < 3; i++) | |
| { | |
| info->m_constraintError[i * skip] = k * (pivotBInW[i] - pivotAInW[i]); | |
| } | |
| // make rotations around X and Y equal | |
| // the hinge axis should be the only unconstrained | |
| // rotational axis, the angular velocity of the two bodies perpendicular to | |
| // the hinge axis should be equal. thus the constraint equations are | |
| // p*w1 - p*w2 = 0 | |
| // q*w1 - q*w2 = 0 | |
| // where p and q are unit vectors normal to the hinge axis, and w1 and w2 | |
| // are the angular velocity vectors of the two bodies. | |
| // get hinge axis (Z) | |
| btVector3 ax1 = trA.getBasis().getColumn(2); | |
| // get 2 orthos to hinge axis (X, Y) | |
| btVector3 p = trA.getBasis().getColumn(0); | |
| btVector3 q = trA.getBasis().getColumn(1); | |
| // set the two hinge angular rows | |
| int s3 = 3 * info->rowskip; | |
| int s4 = 4 * info->rowskip; | |
| info->m_J1angularAxis[s3 + 0] = p[0]; | |
| info->m_J1angularAxis[s3 + 1] = p[1]; | |
| info->m_J1angularAxis[s3 + 2] = p[2]; | |
| info->m_J1angularAxis[s4 + 0] = q[0]; | |
| info->m_J1angularAxis[s4 + 1] = q[1]; | |
| info->m_J1angularAxis[s4 + 2] = q[2]; | |
| info->m_J2angularAxis[s3 + 0] = -p[0]; | |
| info->m_J2angularAxis[s3 + 1] = -p[1]; | |
| info->m_J2angularAxis[s3 + 2] = -p[2]; | |
| info->m_J2angularAxis[s4 + 0] = -q[0]; | |
| info->m_J2angularAxis[s4 + 1] = -q[1]; | |
| info->m_J2angularAxis[s4 + 2] = -q[2]; | |
| // compute the right hand side of the constraint equation. set relative | |
| // body velocities along p and q to bring the hinge back into alignment. | |
| // if ax1,ax2 are the unit length hinge axes as computed from body1 and | |
| // body2, we need to rotate both bodies along the axis u = (ax1 x ax2). | |
| // if `theta' is the angle between ax1 and ax2, we need an angular velocity | |
| // along u to cover angle erp*theta in one step : | |
| // |angular_velocity| = angle/time = erp*theta / stepsize | |
| // = (erp*fps) * theta | |
| // angular_velocity = |angular_velocity| * (ax1 x ax2) / |ax1 x ax2| | |
| // = (erp*fps) * theta * (ax1 x ax2) / sin(theta) | |
| // ...as ax1 and ax2 are unit length. if theta is smallish, | |
| // theta ~= sin(theta), so | |
| // angular_velocity = (erp*fps) * (ax1 x ax2) | |
| // ax1 x ax2 is in the plane space of ax1, so we project the angular | |
| // velocity to p and q to find the right hand side. | |
| btVector3 ax2 = trB.getBasis().getColumn(2); | |
| btVector3 u = ax1.cross(ax2); | |
| info->m_constraintError[s3] = k * u.dot(p); | |
| info->m_constraintError[s4] = k * u.dot(q); | |
| // check angular limits | |
| int nrow = 4; // last filled row | |
| int srow; | |
| btScalar limit_err = btScalar(0.0); | |
| int limit = 0; | |
| if(getSolveLimit()) | |
| { | |
| limit_err = m_correction * m_referenceSign; | |
| limit = (limit_err > btScalar(0.0)) ? 1 : 2; | |
| } | |
| // if the hinge has joint limits or motor, add in the extra row | |
| int powered = 0; | |
| if(getEnableAngularMotor()) | |
| { | |
| powered = 1; | |
| } | |
| if(limit || powered) | |
| { | |
| nrow++; | |
| srow = nrow * info->rowskip; | |
| info->m_J1angularAxis[srow+0] = ax1[0]; | |
| info->m_J1angularAxis[srow+1] = ax1[1]; | |
| info->m_J1angularAxis[srow+2] = ax1[2]; | |
| info->m_J2angularAxis[srow+0] = -ax1[0]; | |
| info->m_J2angularAxis[srow+1] = -ax1[1]; | |
| info->m_J2angularAxis[srow+2] = -ax1[2]; | |
| btScalar lostop = getLowerLimit(); | |
| btScalar histop = getUpperLimit(); | |
| if(limit && (lostop == histop)) | |
| { // the joint motor is ineffective | |
| powered = 0; | |
| } | |
| info->m_constraintError[srow] = btScalar(0.0f); | |
| if(powered) | |
| { | |
| info->cfm[srow] = btScalar(0.0); | |
| btScalar mot_fact = getMotorFactor(m_hingeAngle, lostop, histop, m_motorTargetVelocity, info->fps * info->erp); | |
| info->m_constraintError[srow] += mot_fact * m_motorTargetVelocity * m_referenceSign; | |
| info->m_lowerLimit[srow] = - m_maxMotorImpulse; | |
| info->m_upperLimit[srow] = m_maxMotorImpulse; | |
| } | |
| if(limit) | |
| { | |
| k = info->fps * info->erp; | |
| info->m_constraintError[srow] += k * limit_err; | |
| info->cfm[srow] = btScalar(0.0); | |
| if(lostop == histop) | |
| { | |
| // limited low and high simultaneously | |
| info->m_lowerLimit[srow] = -SIMD_INFINITY; | |
| info->m_upperLimit[srow] = SIMD_INFINITY; | |
| } | |
| else if(limit == 1) | |
| { // low limit | |
| info->m_lowerLimit[srow] = 0; | |
| info->m_upperLimit[srow] = SIMD_INFINITY; | |
| } | |
| else | |
| { // high limit | |
| info->m_lowerLimit[srow] = -SIMD_INFINITY; | |
| info->m_upperLimit[srow] = 0; | |
| } | |
| // bounce (we'll use slider parameter abs(1.0 - m_dampingLimAng) for that) | |
| btScalar bounce = m_relaxationFactor; | |
| if(bounce > btScalar(0.0)) | |
| { | |
| btScalar vel = angVelA.dot(ax1); | |
| vel -= angVelB.dot(ax1); | |
| // only apply bounce if the velocity is incoming, and if the | |
| // resulting c[] exceeds what we already have. | |
| if(limit == 1) | |
| { // low limit | |
| if(vel < 0) | |
| { | |
| btScalar newc = -bounce * vel; | |
| if(newc > info->m_constraintError[srow]) | |
| { | |
| info->m_constraintError[srow] = newc; | |
| } | |
| } | |
| } | |
| else | |
| { // high limit - all those computations are reversed | |
| if(vel > 0) | |
| { | |
| btScalar newc = -bounce * vel; | |
| if(newc < info->m_constraintError[srow]) | |
| { | |
| info->m_constraintError[srow] = newc; | |
| } | |
| } | |
| } | |
| } | |
| info->m_constraintError[srow] *= m_biasFactor; | |
| } // if(limit) | |
| } // if angular limit or powered | |
| } | |
| void btHingeConstraint::updateRHS(btScalar timeStep) | |
| { | |
| (void)timeStep; | |
| } | |
| btScalar btHingeConstraint::getHingeAngle() | |
| { | |
| return getHingeAngle(m_rbA.getCenterOfMassTransform(),m_rbB.getCenterOfMassTransform()); | |
| } | |
| btScalar btHingeConstraint::getHingeAngle(const btTransform& transA,const btTransform& transB) | |
| { | |
| const btVector3 refAxis0 = transA.getBasis() * m_rbAFrame.getBasis().getColumn(0); | |
| const btVector3 refAxis1 = transA.getBasis() * m_rbAFrame.getBasis().getColumn(1); | |
| const btVector3 swingAxis = transB.getBasis() * m_rbBFrame.getBasis().getColumn(1); | |
| btScalar angle = btAtan2Fast(swingAxis.dot(refAxis0), swingAxis.dot(refAxis1)); | |
| return m_referenceSign * angle; | |
| } | |
| #if 0 | |
| void btHingeConstraint::testLimit() | |
| { | |
| // Compute limit information | |
| m_hingeAngle = getHingeAngle(); | |
| m_correction = btScalar(0.); | |
| m_limitSign = btScalar(0.); | |
| m_solveLimit = false; | |
| if (m_lowerLimit <= m_upperLimit) | |
| { | |
| if (m_hingeAngle <= m_lowerLimit) | |
| { | |
| m_correction = (m_lowerLimit - m_hingeAngle); | |
| m_limitSign = 1.0f; | |
| m_solveLimit = true; | |
| } | |
| else if (m_hingeAngle >= m_upperLimit) | |
| { | |
| m_correction = m_upperLimit - m_hingeAngle; | |
| m_limitSign = -1.0f; | |
| m_solveLimit = true; | |
| } | |
| } | |
| return; | |
| } | |
| #else | |
| void btHingeConstraint::testLimit(const btTransform& transA,const btTransform& transB) | |
| { | |
| // Compute limit information | |
| m_hingeAngle = getHingeAngle(transA,transB); | |
| m_correction = btScalar(0.); | |
| m_limitSign = btScalar(0.); | |
| m_solveLimit = false; | |
| if (m_lowerLimit <= m_upperLimit) | |
| { | |
| m_hingeAngle = btAdjustAngleToLimits(m_hingeAngle, m_lowerLimit, m_upperLimit); | |
| if (m_hingeAngle <= m_lowerLimit) | |
| { | |
| m_correction = (m_lowerLimit - m_hingeAngle); | |
| m_limitSign = 1.0f; | |
| m_solveLimit = true; | |
| } | |
| else if (m_hingeAngle >= m_upperLimit) | |
| { | |
| m_correction = m_upperLimit - m_hingeAngle; | |
| m_limitSign = -1.0f; | |
| m_solveLimit = true; | |
| } | |
| } | |
| return; | |
| } | |
| #endif | |
| static btVector3 vHinge(0, 0, btScalar(1)); | |
| void btHingeConstraint::setMotorTarget(const btQuaternion& qAinB, btScalar dt) | |
| { | |
| // convert target from body to constraint space | |
| btQuaternion qConstraint = m_rbBFrame.getRotation().inverse() * qAinB * m_rbAFrame.getRotation(); | |
| qConstraint.normalize(); | |
| // extract "pure" hinge component | |
| btVector3 vNoHinge = quatRotate(qConstraint, vHinge); vNoHinge.normalize(); | |
| btQuaternion qNoHinge = shortestArcQuat(vHinge, vNoHinge); | |
| btQuaternion qHinge = qNoHinge.inverse() * qConstraint; | |
| qHinge.normalize(); | |
| // compute angular target, clamped to limits | |
| btScalar targetAngle = qHinge.getAngle(); | |
| if (targetAngle > SIMD_PI) // long way around. flip quat and recalculate. | |
| { | |
| qHinge = operator-(qHinge); | |
| targetAngle = qHinge.getAngle(); | |
| } | |
| if (qHinge.getZ() < 0) | |
| targetAngle = -targetAngle; | |
| setMotorTarget(targetAngle, dt); | |
| } | |
| void btHingeConstraint::setMotorTarget(btScalar targetAngle, btScalar dt) | |
| { | |
| if (m_lowerLimit < m_upperLimit) | |
| { | |
| if (targetAngle < m_lowerLimit) | |
| targetAngle = m_lowerLimit; | |
| else if (targetAngle > m_upperLimit) | |
| targetAngle = m_upperLimit; | |
| } | |
| // compute angular velocity | |
| btScalar curAngle = getHingeAngle(m_rbA.getCenterOfMassTransform(),m_rbB.getCenterOfMassTransform()); | |
| btScalar dAngle = targetAngle - curAngle; | |
| m_motorTargetVelocity = dAngle / dt; | |
| } | |