diff options
Diffstat (limited to 'extern/bullet2/BulletDynamics/ConstraintSolver')
29 files changed, 0 insertions, 9278 deletions
diff --git a/extern/bullet2/BulletDynamics/ConstraintSolver/btConeTwistConstraint.cpp b/extern/bullet2/BulletDynamics/ConstraintSolver/btConeTwistConstraint.cpp deleted file mode 100644 index bf77c495404..00000000000 --- a/extern/bullet2/BulletDynamics/ConstraintSolver/btConeTwistConstraint.cpp +++ /dev/null @@ -1,1117 +0,0 @@ -/* -Bullet Continuous Collision Detection and Physics Library -btConeTwistConstraint is Copyright (c) 2007 Starbreeze Studios - -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. - -Written by: Marcus Hennix -*/ - - -#include "btConeTwistConstraint.h" -#include "BulletDynamics/Dynamics/btRigidBody.h" -#include "LinearMath/btTransformUtil.h" -#include "LinearMath/btMinMax.h" -#include <new> - - - -//#define CONETWIST_USE_OBSOLETE_SOLVER true -#define CONETWIST_USE_OBSOLETE_SOLVER false -#define CONETWIST_DEF_FIX_THRESH btScalar(.05f) - - -SIMD_FORCE_INLINE btScalar computeAngularImpulseDenominator(const btVector3& axis, const btMatrix3x3& invInertiaWorld) -{ - btVector3 vec = axis * invInertiaWorld; - return axis.dot(vec); -} - - - - -btConeTwistConstraint::btConeTwistConstraint(btRigidBody& rbA,btRigidBody& rbB, - const btTransform& rbAFrame,const btTransform& rbBFrame) - :btTypedConstraint(CONETWIST_CONSTRAINT_TYPE, rbA,rbB),m_rbAFrame(rbAFrame),m_rbBFrame(rbBFrame), - m_angularOnly(false), - m_useSolveConstraintObsolete(CONETWIST_USE_OBSOLETE_SOLVER) -{ - init(); -} - -btConeTwistConstraint::btConeTwistConstraint(btRigidBody& rbA,const btTransform& rbAFrame) - :btTypedConstraint(CONETWIST_CONSTRAINT_TYPE,rbA),m_rbAFrame(rbAFrame), - m_angularOnly(false), - m_useSolveConstraintObsolete(CONETWIST_USE_OBSOLETE_SOLVER) -{ - m_rbBFrame = m_rbAFrame; - init(); -} - - -void btConeTwistConstraint::init() -{ - m_angularOnly = false; - m_solveTwistLimit = false; - m_solveSwingLimit = false; - m_bMotorEnabled = false; - m_maxMotorImpulse = btScalar(-1); - - setLimit(btScalar(BT_LARGE_FLOAT), btScalar(BT_LARGE_FLOAT), btScalar(BT_LARGE_FLOAT)); - m_damping = btScalar(0.01); - m_fixThresh = CONETWIST_DEF_FIX_THRESH; - m_flags = 0; - m_linCFM = btScalar(0.f); - m_linERP = btScalar(0.7f); - m_angCFM = btScalar(0.f); -} - - -void btConeTwistConstraint::getInfo1 (btConstraintInfo1* info) -{ - if (m_useSolveConstraintObsolete) - { - info->m_numConstraintRows = 0; - info->nub = 0; - } - else - { - info->m_numConstraintRows = 3; - info->nub = 3; - calcAngleInfo2(m_rbA.getCenterOfMassTransform(),m_rbB.getCenterOfMassTransform(),m_rbA.getInvInertiaTensorWorld(),m_rbB.getInvInertiaTensorWorld()); - if(m_solveSwingLimit) - { - info->m_numConstraintRows++; - info->nub--; - if((m_swingSpan1 < m_fixThresh) && (m_swingSpan2 < m_fixThresh)) - { - info->m_numConstraintRows++; - info->nub--; - } - } - if(m_solveTwistLimit) - { - info->m_numConstraintRows++; - info->nub--; - } - } -} - -void btConeTwistConstraint::getInfo1NonVirtual (btConstraintInfo1* info) -{ - //always reserve 6 rows: object transform is not available on SPU - info->m_numConstraintRows = 6; - info->nub = 0; - -} - - -void btConeTwistConstraint::getInfo2 (btConstraintInfo2* info) -{ - getInfo2NonVirtual(info,m_rbA.getCenterOfMassTransform(),m_rbB.getCenterOfMassTransform(),m_rbA.getInvInertiaTensorWorld(),m_rbB.getInvInertiaTensorWorld()); -} - -void btConeTwistConstraint::getInfo2NonVirtual (btConstraintInfo2* info,const btTransform& transA,const btTransform& transB,const btMatrix3x3& invInertiaWorldA,const btMatrix3x3& invInertiaWorldB) -{ - calcAngleInfo2(transA,transB,invInertiaWorldA,invInertiaWorldB); - - btAssert(!m_useSolveConstraintObsolete); - // set jacobian - info->m_J1linearAxis[0] = 1; - info->m_J1linearAxis[info->rowskip+1] = 1; - info->m_J1linearAxis[2*info->rowskip+2] = 1; - btVector3 a1 = transA.getBasis() * m_rbAFrame.getOrigin(); - { - btVector3* angular0 = (btVector3*)(info->m_J1angularAxis); - btVector3* angular1 = (btVector3*)(info->m_J1angularAxis+info->rowskip); - btVector3* angular2 = (btVector3*)(info->m_J1angularAxis+2*info->rowskip); - btVector3 a1neg = -a1; - a1neg.getSkewSymmetricMatrix(angular0,angular1,angular2); - } - btVector3 a2 = transB.getBasis() * m_rbBFrame.getOrigin(); - { - btVector3* angular0 = (btVector3*)(info->m_J2angularAxis); - btVector3* angular1 = (btVector3*)(info->m_J2angularAxis+info->rowskip); - btVector3* angular2 = (btVector3*)(info->m_J2angularAxis+2*info->rowskip); - a2.getSkewSymmetricMatrix(angular0,angular1,angular2); - } - // set right hand side - btScalar linERP = (m_flags & BT_CONETWIST_FLAGS_LIN_ERP) ? m_linERP : info->erp; - btScalar k = info->fps * linERP; - int j; - for (j=0; j<3; j++) - { - info->m_constraintError[j*info->rowskip] = k * (a2[j] + transB.getOrigin()[j] - a1[j] - transA.getOrigin()[j]); - info->m_lowerLimit[j*info->rowskip] = -SIMD_INFINITY; - info->m_upperLimit[j*info->rowskip] = SIMD_INFINITY; - if(m_flags & BT_CONETWIST_FLAGS_LIN_CFM) - { - info->cfm[j*info->rowskip] = m_linCFM; - } - } - int row = 3; - int srow = row * info->rowskip; - btVector3 ax1; - // angular limits - if(m_solveSwingLimit) - { - btScalar *J1 = info->m_J1angularAxis; - btScalar *J2 = info->m_J2angularAxis; - if((m_swingSpan1 < m_fixThresh) && (m_swingSpan2 < m_fixThresh)) - { - btTransform trA = transA*m_rbAFrame; - btVector3 p = trA.getBasis().getColumn(1); - btVector3 q = trA.getBasis().getColumn(2); - int srow1 = srow + info->rowskip; - J1[srow+0] = p[0]; - J1[srow+1] = p[1]; - J1[srow+2] = p[2]; - J1[srow1+0] = q[0]; - J1[srow1+1] = q[1]; - J1[srow1+2] = q[2]; - J2[srow+0] = -p[0]; - J2[srow+1] = -p[1]; - J2[srow+2] = -p[2]; - J2[srow1+0] = -q[0]; - J2[srow1+1] = -q[1]; - J2[srow1+2] = -q[2]; - btScalar fact = info->fps * m_relaxationFactor; - info->m_constraintError[srow] = fact * m_swingAxis.dot(p); - info->m_constraintError[srow1] = fact * m_swingAxis.dot(q); - info->m_lowerLimit[srow] = -SIMD_INFINITY; - info->m_upperLimit[srow] = SIMD_INFINITY; - info->m_lowerLimit[srow1] = -SIMD_INFINITY; - info->m_upperLimit[srow1] = SIMD_INFINITY; - srow = srow1 + info->rowskip; - } - else - { - ax1 = m_swingAxis * m_relaxationFactor * m_relaxationFactor; - J1[srow+0] = ax1[0]; - J1[srow+1] = ax1[1]; - J1[srow+2] = ax1[2]; - J2[srow+0] = -ax1[0]; - J2[srow+1] = -ax1[1]; - J2[srow+2] = -ax1[2]; - btScalar k = info->fps * m_biasFactor; - - info->m_constraintError[srow] = k * m_swingCorrection; - if(m_flags & BT_CONETWIST_FLAGS_ANG_CFM) - { - info->cfm[srow] = m_angCFM; - } - // m_swingCorrection is always positive or 0 - info->m_lowerLimit[srow] = 0; - info->m_upperLimit[srow] = SIMD_INFINITY; - srow += info->rowskip; - } - } - if(m_solveTwistLimit) - { - ax1 = m_twistAxis * m_relaxationFactor * m_relaxationFactor; - btScalar *J1 = info->m_J1angularAxis; - btScalar *J2 = info->m_J2angularAxis; - J1[srow+0] = ax1[0]; - J1[srow+1] = ax1[1]; - J1[srow+2] = ax1[2]; - J2[srow+0] = -ax1[0]; - J2[srow+1] = -ax1[1]; - J2[srow+2] = -ax1[2]; - btScalar k = info->fps * m_biasFactor; - info->m_constraintError[srow] = k * m_twistCorrection; - if(m_flags & BT_CONETWIST_FLAGS_ANG_CFM) - { - info->cfm[srow] = m_angCFM; - } - if(m_twistSpan > 0.0f) - { - - if(m_twistCorrection > 0.0f) - { - info->m_lowerLimit[srow] = 0; - info->m_upperLimit[srow] = SIMD_INFINITY; - } - else - { - info->m_lowerLimit[srow] = -SIMD_INFINITY; - info->m_upperLimit[srow] = 0; - } - } - else - { - info->m_lowerLimit[srow] = -SIMD_INFINITY; - info->m_upperLimit[srow] = SIMD_INFINITY; - } - srow += info->rowskip; - } -} - - - -void btConeTwistConstraint::buildJacobian() -{ - if (m_useSolveConstraintObsolete) - { - m_appliedImpulse = btScalar(0.); - m_accTwistLimitImpulse = btScalar(0.); - m_accSwingLimitImpulse = btScalar(0.); - m_accMotorImpulse = btVector3(0.,0.,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()); - } - } - - calcAngleInfo2(m_rbA.getCenterOfMassTransform(),m_rbB.getCenterOfMassTransform(),m_rbA.getInvInertiaTensorWorld(),m_rbB.getInvInertiaTensorWorld()); - } -} - - - -void btConeTwistConstraint::solveConstraintObsolete(btRigidBody& bodyA,btRigidBody& bodyB,btScalar timeStep) -{ - #ifndef __SPU__ - 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; - bodyA.internalGetVelocityInLocalPointObsolete(rel_pos1,vel1); - btVector3 vel2; - bodyB.internalGetVelocityInLocalPointObsolete(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 ftorqueAxis1 = rel_pos1.cross(normal); - btVector3 ftorqueAxis2 = rel_pos2.cross(normal); - bodyA.internalApplyImpulse(normal*m_rbA.getInvMass(), m_rbA.getInvInertiaTensorWorld()*ftorqueAxis1,impulse); - bodyB.internalApplyImpulse(normal*m_rbB.getInvMass(), m_rbB.getInvInertiaTensorWorld()*ftorqueAxis2,-impulse); - - } - } - - // apply motor - if (m_bMotorEnabled) - { - // compute current and predicted transforms - btTransform trACur = m_rbA.getCenterOfMassTransform(); - btTransform trBCur = m_rbB.getCenterOfMassTransform(); - btVector3 omegaA; bodyA.internalGetAngularVelocity(omegaA); - btVector3 omegaB; bodyB.internalGetAngularVelocity(omegaB); - btTransform trAPred; trAPred.setIdentity(); - btVector3 zerovec(0,0,0); - btTransformUtil::integrateTransform( - trACur, zerovec, omegaA, timeStep, trAPred); - btTransform trBPred; trBPred.setIdentity(); - btTransformUtil::integrateTransform( - trBCur, zerovec, omegaB, timeStep, trBPred); - - // compute desired transforms in world - btTransform trPose(m_qTarget); - btTransform trABDes = m_rbBFrame * trPose * m_rbAFrame.inverse(); - btTransform trADes = trBPred * trABDes; - btTransform trBDes = trAPred * trABDes.inverse(); - - // compute desired omegas in world - btVector3 omegaADes, omegaBDes; - - btTransformUtil::calculateVelocity(trACur, trADes, timeStep, zerovec, omegaADes); - btTransformUtil::calculateVelocity(trBCur, trBDes, timeStep, zerovec, omegaBDes); - - // compute delta omegas - btVector3 dOmegaA = omegaADes - omegaA; - btVector3 dOmegaB = omegaBDes - omegaB; - - // compute weighted avg axis of dOmega (weighting based on inertias) - btVector3 axisA, axisB; - btScalar kAxisAInv = 0, kAxisBInv = 0; - - if (dOmegaA.length2() > SIMD_EPSILON) - { - axisA = dOmegaA.normalized(); - kAxisAInv = getRigidBodyA().computeAngularImpulseDenominator(axisA); - } - - if (dOmegaB.length2() > SIMD_EPSILON) - { - axisB = dOmegaB.normalized(); - kAxisBInv = getRigidBodyB().computeAngularImpulseDenominator(axisB); - } - - btVector3 avgAxis = kAxisAInv * axisA + kAxisBInv * axisB; - - static bool bDoTorque = true; - if (bDoTorque && avgAxis.length2() > SIMD_EPSILON) - { - avgAxis.normalize(); - kAxisAInv = getRigidBodyA().computeAngularImpulseDenominator(avgAxis); - kAxisBInv = getRigidBodyB().computeAngularImpulseDenominator(avgAxis); - btScalar kInvCombined = kAxisAInv + kAxisBInv; - - btVector3 impulse = (kAxisAInv * dOmegaA - kAxisBInv * dOmegaB) / - (kInvCombined * kInvCombined); - - if (m_maxMotorImpulse >= 0) - { - btScalar fMaxImpulse = m_maxMotorImpulse; - if (m_bNormalizedMotorStrength) - fMaxImpulse = fMaxImpulse/kAxisAInv; - - btVector3 newUnclampedAccImpulse = m_accMotorImpulse + impulse; - btScalar newUnclampedMag = newUnclampedAccImpulse.length(); - if (newUnclampedMag > fMaxImpulse) - { - newUnclampedAccImpulse.normalize(); - newUnclampedAccImpulse *= fMaxImpulse; - impulse = newUnclampedAccImpulse - m_accMotorImpulse; - } - m_accMotorImpulse += impulse; - } - - btScalar impulseMag = impulse.length(); - btVector3 impulseAxis = impulse / impulseMag; - - bodyA.internalApplyImpulse(btVector3(0,0,0), m_rbA.getInvInertiaTensorWorld()*impulseAxis, impulseMag); - bodyB.internalApplyImpulse(btVector3(0,0,0), m_rbB.getInvInertiaTensorWorld()*impulseAxis, -impulseMag); - - } - } - else if (m_damping > SIMD_EPSILON) // no motor: do a little damping - { - btVector3 angVelA; bodyA.internalGetAngularVelocity(angVelA); - btVector3 angVelB; bodyB.internalGetAngularVelocity(angVelB); - btVector3 relVel = angVelB - angVelA; - if (relVel.length2() > SIMD_EPSILON) - { - btVector3 relVelAxis = relVel.normalized(); - btScalar m_kDamping = btScalar(1.) / - (getRigidBodyA().computeAngularImpulseDenominator(relVelAxis) + - getRigidBodyB().computeAngularImpulseDenominator(relVelAxis)); - btVector3 impulse = m_damping * m_kDamping * relVel; - - btScalar impulseMag = impulse.length(); - btVector3 impulseAxis = impulse / impulseMag; - bodyA.internalApplyImpulse(btVector3(0,0,0), m_rbA.getInvInertiaTensorWorld()*impulseAxis, impulseMag); - bodyB.internalApplyImpulse(btVector3(0,0,0), m_rbB.getInvInertiaTensorWorld()*impulseAxis, -impulseMag); - } - } - - // joint limits - { - ///solve angular part - btVector3 angVelA; - bodyA.internalGetAngularVelocity(angVelA); - btVector3 angVelB; - bodyB.internalGetAngularVelocity(angVelB); - - // solve swing limit - if (m_solveSwingLimit) - { - btScalar amplitude = m_swingLimitRatio * m_swingCorrection*m_biasFactor/timeStep; - btScalar relSwingVel = (angVelB - angVelA).dot(m_swingAxis); - if (relSwingVel > 0) - amplitude += m_swingLimitRatio * relSwingVel * m_relaxationFactor; - btScalar impulseMag = amplitude * m_kSwing; - - // Clamp the accumulated impulse - btScalar temp = m_accSwingLimitImpulse; - m_accSwingLimitImpulse = btMax(m_accSwingLimitImpulse + impulseMag, btScalar(0.0) ); - impulseMag = m_accSwingLimitImpulse - temp; - - btVector3 impulse = m_swingAxis * impulseMag; - - // don't let cone response affect twist - // (this can happen since body A's twist doesn't match body B's AND we use an elliptical cone limit) - { - btVector3 impulseTwistCouple = impulse.dot(m_twistAxisA) * m_twistAxisA; - btVector3 impulseNoTwistCouple = impulse - impulseTwistCouple; - impulse = impulseNoTwistCouple; - } - - impulseMag = impulse.length(); - btVector3 noTwistSwingAxis = impulse / impulseMag; - - bodyA.internalApplyImpulse(btVector3(0,0,0), m_rbA.getInvInertiaTensorWorld()*noTwistSwingAxis, impulseMag); - bodyB.internalApplyImpulse(btVector3(0,0,0), m_rbB.getInvInertiaTensorWorld()*noTwistSwingAxis, -impulseMag); - } - - - // solve twist limit - if (m_solveTwistLimit) - { - btScalar amplitude = m_twistLimitRatio * m_twistCorrection*m_biasFactor/timeStep; - btScalar relTwistVel = (angVelB - angVelA).dot( m_twistAxis ); - if (relTwistVel > 0) // only damp when moving towards limit (m_twistAxis flipping is important) - amplitude += m_twistLimitRatio * relTwistVel * m_relaxationFactor; - btScalar impulseMag = amplitude * m_kTwist; - - // Clamp the accumulated impulse - btScalar temp = m_accTwistLimitImpulse; - m_accTwistLimitImpulse = btMax(m_accTwistLimitImpulse + impulseMag, btScalar(0.0) ); - impulseMag = m_accTwistLimitImpulse - temp; - - btVector3 impulse = m_twistAxis * impulseMag; - - bodyA.internalApplyImpulse(btVector3(0,0,0), m_rbA.getInvInertiaTensorWorld()*m_twistAxis,impulseMag); - bodyB.internalApplyImpulse(btVector3(0,0,0), m_rbB.getInvInertiaTensorWorld()*m_twistAxis,-impulseMag); - } - } - } -#else -btAssert(0); -#endif //__SPU__ -} - - - - -void btConeTwistConstraint::updateRHS(btScalar timeStep) -{ - (void)timeStep; - -} - - -#ifndef __SPU__ -void btConeTwistConstraint::calcAngleInfo() -{ - m_swingCorrection = btScalar(0.); - m_twistLimitSign = btScalar(0.); - m_solveTwistLimit = false; - m_solveSwingLimit = false; - - btVector3 b1Axis1,b1Axis2,b1Axis3; - btVector3 b2Axis1,b2Axis2; - - b1Axis1 = getRigidBodyA().getCenterOfMassTransform().getBasis() * this->m_rbAFrame.getBasis().getColumn(0); - b2Axis1 = getRigidBodyB().getCenterOfMassTransform().getBasis() * this->m_rbBFrame.getBasis().getColumn(0); - - btScalar swing1=btScalar(0.),swing2 = btScalar(0.); - - btScalar swx=btScalar(0.),swy = btScalar(0.); - btScalar thresh = btScalar(10.); - btScalar fact; - - // Get Frame into world space - if (m_swingSpan1 >= btScalar(0.05f)) - { - b1Axis2 = getRigidBodyA().getCenterOfMassTransform().getBasis() * this->m_rbAFrame.getBasis().getColumn(1); - swx = b2Axis1.dot(b1Axis1); - swy = b2Axis1.dot(b1Axis2); - swing1 = btAtan2Fast(swy, swx); - fact = (swy*swy + swx*swx) * thresh * thresh; - fact = fact / (fact + btScalar(1.0)); - swing1 *= fact; - } - - if (m_swingSpan2 >= btScalar(0.05f)) - { - b1Axis3 = getRigidBodyA().getCenterOfMassTransform().getBasis() * this->m_rbAFrame.getBasis().getColumn(2); - swx = b2Axis1.dot(b1Axis1); - swy = b2Axis1.dot(b1Axis3); - swing2 = btAtan2Fast(swy, swx); - fact = (swy*swy + swx*swx) * thresh * thresh; - fact = fact / (fact + btScalar(1.0)); - swing2 *= fact; - } - - btScalar RMaxAngle1Sq = 1.0f / (m_swingSpan1*m_swingSpan1); - btScalar RMaxAngle2Sq = 1.0f / (m_swingSpan2*m_swingSpan2); - btScalar EllipseAngle = btFabs(swing1*swing1)* RMaxAngle1Sq + btFabs(swing2*swing2) * RMaxAngle2Sq; - - if (EllipseAngle > 1.0f) - { - m_swingCorrection = EllipseAngle-1.0f; - m_solveSwingLimit = true; - // Calculate necessary axis & factors - m_swingAxis = b2Axis1.cross(b1Axis2* b2Axis1.dot(b1Axis2) + b1Axis3* b2Axis1.dot(b1Axis3)); - m_swingAxis.normalize(); - btScalar swingAxisSign = (b2Axis1.dot(b1Axis1) >= 0.0f) ? 1.0f : -1.0f; - m_swingAxis *= swingAxisSign; - } - - // Twist limits - if (m_twistSpan >= btScalar(0.)) - { - btVector3 b2Axis2 = getRigidBodyB().getCenterOfMassTransform().getBasis() * this->m_rbBFrame.getBasis().getColumn(1); - btQuaternion rotationArc = shortestArcQuat(b2Axis1,b1Axis1); - btVector3 TwistRef = quatRotate(rotationArc,b2Axis2); - btScalar twist = btAtan2Fast( TwistRef.dot(b1Axis3), TwistRef.dot(b1Axis2) ); - m_twistAngle = twist; - -// btScalar lockedFreeFactor = (m_twistSpan > btScalar(0.05f)) ? m_limitSoftness : btScalar(0.); - btScalar lockedFreeFactor = (m_twistSpan > btScalar(0.05f)) ? btScalar(1.0f) : btScalar(0.); - if (twist <= -m_twistSpan*lockedFreeFactor) - { - m_twistCorrection = -(twist + m_twistSpan); - m_solveTwistLimit = true; - m_twistAxis = (b2Axis1 + b1Axis1) * 0.5f; - m_twistAxis.normalize(); - m_twistAxis *= -1.0f; - } - else if (twist > m_twistSpan*lockedFreeFactor) - { - m_twistCorrection = (twist - m_twistSpan); - m_solveTwistLimit = true; - m_twistAxis = (b2Axis1 + b1Axis1) * 0.5f; - m_twistAxis.normalize(); - } - } -} -#endif //__SPU__ - -static btVector3 vTwist(1,0,0); // twist axis in constraint's space - - - -void btConeTwistConstraint::calcAngleInfo2(const btTransform& transA, const btTransform& transB, const btMatrix3x3& invInertiaWorldA,const btMatrix3x3& invInertiaWorldB) -{ - m_swingCorrection = btScalar(0.); - m_twistLimitSign = btScalar(0.); - m_solveTwistLimit = false; - m_solveSwingLimit = false; - // compute rotation of A wrt B (in constraint space) - if (m_bMotorEnabled && (!m_useSolveConstraintObsolete)) - { // it is assumed that setMotorTarget() was alredy called - // and motor target m_qTarget is within constraint limits - // TODO : split rotation to pure swing and pure twist - // compute desired transforms in world - btTransform trPose(m_qTarget); - btTransform trA = transA * m_rbAFrame; - btTransform trB = transB * m_rbBFrame; - btTransform trDeltaAB = trB * trPose * trA.inverse(); - btQuaternion qDeltaAB = trDeltaAB.getRotation(); - btVector3 swingAxis = btVector3(qDeltaAB.x(), qDeltaAB.y(), qDeltaAB.z()); - m_swingAxis = swingAxis; - m_swingAxis.normalize(); - m_swingCorrection = qDeltaAB.getAngle(); - if(!btFuzzyZero(m_swingCorrection)) - { - m_solveSwingLimit = true; - } - return; - } - - - { - // compute rotation of A wrt B (in constraint space) - btQuaternion qA = transA.getRotation() * m_rbAFrame.getRotation(); - btQuaternion qB = transB.getRotation() * m_rbBFrame.getRotation(); - btQuaternion qAB = qB.inverse() * qA; - // split rotation into cone and twist - // (all this is done from B's perspective. Maybe I should be averaging axes...) - btVector3 vConeNoTwist = quatRotate(qAB, vTwist); vConeNoTwist.normalize(); - btQuaternion qABCone = shortestArcQuat(vTwist, vConeNoTwist); qABCone.normalize(); - btQuaternion qABTwist = qABCone.inverse() * qAB; qABTwist.normalize(); - - if (m_swingSpan1 >= m_fixThresh && m_swingSpan2 >= m_fixThresh) - { - btScalar swingAngle, swingLimit = 0; btVector3 swingAxis; - computeConeLimitInfo(qABCone, swingAngle, swingAxis, swingLimit); - - if (swingAngle > swingLimit * m_limitSoftness) - { - m_solveSwingLimit = true; - - // compute limit ratio: 0->1, where - // 0 == beginning of soft limit - // 1 == hard/real limit - m_swingLimitRatio = 1.f; - if (swingAngle < swingLimit && m_limitSoftness < 1.f - SIMD_EPSILON) - { - m_swingLimitRatio = (swingAngle - swingLimit * m_limitSoftness)/ - (swingLimit - swingLimit * m_limitSoftness); - } - - // swing correction tries to get back to soft limit - m_swingCorrection = swingAngle - (swingLimit * m_limitSoftness); - - // adjustment of swing axis (based on ellipse normal) - adjustSwingAxisToUseEllipseNormal(swingAxis); - - // Calculate necessary axis & factors - m_swingAxis = quatRotate(qB, -swingAxis); - - m_twistAxisA.setValue(0,0,0); - - m_kSwing = btScalar(1.) / - (computeAngularImpulseDenominator(m_swingAxis,invInertiaWorldA) + - computeAngularImpulseDenominator(m_swingAxis,invInertiaWorldB)); - } - } - else - { - // you haven't set any limits; - // or you're trying to set at least one of the swing limits too small. (if so, do you really want a conetwist constraint?) - // anyway, we have either hinge or fixed joint - btVector3 ivA = transA.getBasis() * m_rbAFrame.getBasis().getColumn(0); - btVector3 jvA = transA.getBasis() * m_rbAFrame.getBasis().getColumn(1); - btVector3 kvA = transA.getBasis() * m_rbAFrame.getBasis().getColumn(2); - btVector3 ivB = transB.getBasis() * m_rbBFrame.getBasis().getColumn(0); - btVector3 target; - btScalar x = ivB.dot(ivA); - btScalar y = ivB.dot(jvA); - btScalar z = ivB.dot(kvA); - if((m_swingSpan1 < m_fixThresh) && (m_swingSpan2 < m_fixThresh)) - { // fixed. We'll need to add one more row to constraint - if((!btFuzzyZero(y)) || (!(btFuzzyZero(z)))) - { - m_solveSwingLimit = true; - m_swingAxis = -ivB.cross(ivA); - } - } - else - { - if(m_swingSpan1 < m_fixThresh) - { // hinge around Y axis - if(!(btFuzzyZero(y))) - { - m_solveSwingLimit = true; - if(m_swingSpan2 >= m_fixThresh) - { - y = btScalar(0.f); - btScalar span2 = btAtan2(z, x); - if(span2 > m_swingSpan2) - { - x = btCos(m_swingSpan2); - z = btSin(m_swingSpan2); - } - else if(span2 < -m_swingSpan2) - { - x = btCos(m_swingSpan2); - z = -btSin(m_swingSpan2); - } - } - } - } - else - { // hinge around Z axis - if(!btFuzzyZero(z)) - { - m_solveSwingLimit = true; - if(m_swingSpan1 >= m_fixThresh) - { - z = btScalar(0.f); - btScalar span1 = btAtan2(y, x); - if(span1 > m_swingSpan1) - { - x = btCos(m_swingSpan1); - y = btSin(m_swingSpan1); - } - else if(span1 < -m_swingSpan1) - { - x = btCos(m_swingSpan1); - y = -btSin(m_swingSpan1); - } - } - } - } - target[0] = x * ivA[0] + y * jvA[0] + z * kvA[0]; - target[1] = x * ivA[1] + y * jvA[1] + z * kvA[1]; - target[2] = x * ivA[2] + y * jvA[2] + z * kvA[2]; - target.normalize(); - m_swingAxis = -ivB.cross(target); - m_swingCorrection = m_swingAxis.length(); - m_swingAxis.normalize(); - } - } - - if (m_twistSpan >= btScalar(0.f)) - { - btVector3 twistAxis; - computeTwistLimitInfo(qABTwist, m_twistAngle, twistAxis); - - if (m_twistAngle > m_twistSpan*m_limitSoftness) - { - m_solveTwistLimit = true; - - m_twistLimitRatio = 1.f; - if (m_twistAngle < m_twistSpan && m_limitSoftness < 1.f - SIMD_EPSILON) - { - m_twistLimitRatio = (m_twistAngle - m_twistSpan * m_limitSoftness)/ - (m_twistSpan - m_twistSpan * m_limitSoftness); - } - - // twist correction tries to get back to soft limit - m_twistCorrection = m_twistAngle - (m_twistSpan * m_limitSoftness); - - m_twistAxis = quatRotate(qB, -twistAxis); - - m_kTwist = btScalar(1.) / - (computeAngularImpulseDenominator(m_twistAxis,invInertiaWorldA) + - computeAngularImpulseDenominator(m_twistAxis,invInertiaWorldB)); - } - - if (m_solveSwingLimit) - m_twistAxisA = quatRotate(qA, -twistAxis); - } - else - { - m_twistAngle = btScalar(0.f); - } - } -} - - - -// given a cone rotation in constraint space, (pre: twist must already be removed) -// this method computes its corresponding swing angle and axis. -// more interestingly, it computes the cone/swing limit (angle) for this cone "pose". -void btConeTwistConstraint::computeConeLimitInfo(const btQuaternion& qCone, - btScalar& swingAngle, // out - btVector3& vSwingAxis, // out - btScalar& swingLimit) // out -{ - swingAngle = qCone.getAngle(); - if (swingAngle > SIMD_EPSILON) - { - vSwingAxis = btVector3(qCone.x(), qCone.y(), qCone.z()); - vSwingAxis.normalize(); - if (fabs(vSwingAxis.x()) > SIMD_EPSILON) - { - // non-zero twist?! this should never happen. - int wtf = 0; wtf = wtf; - } - - // Compute limit for given swing. tricky: - // Given a swing axis, we're looking for the intersection with the bounding cone ellipse. - // (Since we're dealing with angles, this ellipse is embedded on the surface of a sphere.) - - // For starters, compute the direction from center to surface of ellipse. - // This is just the perpendicular (ie. rotate 2D vector by PI/2) of the swing axis. - // (vSwingAxis is the cone rotation (in z,y); change vars and rotate to (x,y) coords.) - btScalar xEllipse = vSwingAxis.y(); - btScalar yEllipse = -vSwingAxis.z(); - - // Now, we use the slope of the vector (using x/yEllipse) and find the length - // of the line that intersects the ellipse: - // x^2 y^2 - // --- + --- = 1, where a and b are semi-major axes 2 and 1 respectively (ie. the limits) - // a^2 b^2 - // Do the math and it should be clear. - - swingLimit = m_swingSpan1; // if xEllipse == 0, we have a pure vSwingAxis.z rotation: just use swingspan1 - if (fabs(xEllipse) > SIMD_EPSILON) - { - btScalar surfaceSlope2 = (yEllipse*yEllipse)/(xEllipse*xEllipse); - btScalar norm = 1 / (m_swingSpan2 * m_swingSpan2); - norm += surfaceSlope2 / (m_swingSpan1 * m_swingSpan1); - btScalar swingLimit2 = (1 + surfaceSlope2) / norm; - swingLimit = sqrt(swingLimit2); - } - - // test! - /*swingLimit = m_swingSpan2; - if (fabs(vSwingAxis.z()) > SIMD_EPSILON) - { - btScalar mag_2 = m_swingSpan1*m_swingSpan1 + m_swingSpan2*m_swingSpan2; - btScalar sinphi = m_swingSpan2 / sqrt(mag_2); - btScalar phi = asin(sinphi); - btScalar theta = atan2(fabs(vSwingAxis.y()),fabs(vSwingAxis.z())); - btScalar alpha = 3.14159f - theta - phi; - btScalar sinalpha = sin(alpha); - swingLimit = m_swingSpan1 * sinphi/sinalpha; - }*/ - } - else if (swingAngle < 0) - { - // this should never happen! - int wtf = 0; wtf = wtf; - } -} - -btVector3 btConeTwistConstraint::GetPointForAngle(btScalar fAngleInRadians, btScalar fLength) const -{ - // compute x/y in ellipse using cone angle (0 -> 2*PI along surface of cone) - btScalar xEllipse = btCos(fAngleInRadians); - btScalar yEllipse = btSin(fAngleInRadians); - - // Use the slope of the vector (using x/yEllipse) and find the length - // of the line that intersects the ellipse: - // x^2 y^2 - // --- + --- = 1, where a and b are semi-major axes 2 and 1 respectively (ie. the limits) - // a^2 b^2 - // Do the math and it should be clear. - - float swingLimit = m_swingSpan1; // if xEllipse == 0, just use axis b (1) - if (fabs(xEllipse) > SIMD_EPSILON) - { - btScalar surfaceSlope2 = (yEllipse*yEllipse)/(xEllipse*xEllipse); - btScalar norm = 1 / (m_swingSpan2 * m_swingSpan2); - norm += surfaceSlope2 / (m_swingSpan1 * m_swingSpan1); - btScalar swingLimit2 = (1 + surfaceSlope2) / norm; - swingLimit = sqrt(swingLimit2); - } - - // convert into point in constraint space: - // note: twist is x-axis, swing 1 and 2 are along the z and y axes respectively - btVector3 vSwingAxis(0, xEllipse, -yEllipse); - btQuaternion qSwing(vSwingAxis, swingLimit); - btVector3 vPointInConstraintSpace(fLength,0,0); - return quatRotate(qSwing, vPointInConstraintSpace); -} - -// given a twist rotation in constraint space, (pre: cone must already be removed) -// this method computes its corresponding angle and axis. -void btConeTwistConstraint::computeTwistLimitInfo(const btQuaternion& qTwist, - btScalar& twistAngle, // out - btVector3& vTwistAxis) // out -{ - btQuaternion qMinTwist = qTwist; - twistAngle = qTwist.getAngle(); - - if (twistAngle > SIMD_PI) // long way around. flip quat and recalculate. - { - qMinTwist = operator-(qTwist); - twistAngle = qMinTwist.getAngle(); - } - if (twistAngle < 0) - { - // this should never happen - int wtf = 0; wtf = wtf; - } - - vTwistAxis = btVector3(qMinTwist.x(), qMinTwist.y(), qMinTwist.z()); - if (twistAngle > SIMD_EPSILON) - vTwistAxis.normalize(); -} - - -void btConeTwistConstraint::adjustSwingAxisToUseEllipseNormal(btVector3& vSwingAxis) const -{ - // the swing axis is computed as the "twist-free" cone rotation, - // but the cone limit is not circular, but elliptical (if swingspan1 != swingspan2). - // so, if we're outside the limits, the closest way back inside the cone isn't - // along the vector back to the center. better (and more stable) to use the ellipse normal. - - // convert swing axis to direction from center to surface of ellipse - // (ie. rotate 2D vector by PI/2) - btScalar y = -vSwingAxis.z(); - btScalar z = vSwingAxis.y(); - - // do the math... - if (fabs(z) > SIMD_EPSILON) // avoid division by 0. and we don't need an update if z == 0. - { - // compute gradient/normal of ellipse surface at current "point" - btScalar grad = y/z; - grad *= m_swingSpan2 / m_swingSpan1; - - // adjust y/z to represent normal at point (instead of vector to point) - if (y > 0) - y = fabs(grad * z); - else - y = -fabs(grad * z); - - // convert ellipse direction back to swing axis - vSwingAxis.setZ(-y); - vSwingAxis.setY( z); - vSwingAxis.normalize(); - } -} - - - -void btConeTwistConstraint::setMotorTarget(const btQuaternion &q) -{ - btTransform trACur = m_rbA.getCenterOfMassTransform(); - btTransform trBCur = m_rbB.getCenterOfMassTransform(); - btTransform trABCur = trBCur.inverse() * trACur; - btQuaternion qABCur = trABCur.getRotation(); - btTransform trConstraintCur = (trBCur * m_rbBFrame).inverse() * (trACur * m_rbAFrame); - btQuaternion qConstraintCur = trConstraintCur.getRotation(); - - btQuaternion qConstraint = m_rbBFrame.getRotation().inverse() * q * m_rbAFrame.getRotation(); - setMotorTargetInConstraintSpace(qConstraint); -} - - -void btConeTwistConstraint::setMotorTargetInConstraintSpace(const btQuaternion &q) -{ - m_qTarget = q; - - // clamp motor target to within limits - { - btScalar softness = 1.f;//m_limitSoftness; - - // split into twist and cone - btVector3 vTwisted = quatRotate(m_qTarget, vTwist); - btQuaternion qTargetCone = shortestArcQuat(vTwist, vTwisted); qTargetCone.normalize(); - btQuaternion qTargetTwist = qTargetCone.inverse() * m_qTarget; qTargetTwist.normalize(); - - // clamp cone - if (m_swingSpan1 >= btScalar(0.05f) && m_swingSpan2 >= btScalar(0.05f)) - { - btScalar swingAngle, swingLimit; btVector3 swingAxis; - computeConeLimitInfo(qTargetCone, swingAngle, swingAxis, swingLimit); - - if (fabs(swingAngle) > SIMD_EPSILON) - { - if (swingAngle > swingLimit*softness) - swingAngle = swingLimit*softness; - else if (swingAngle < -swingLimit*softness) - swingAngle = -swingLimit*softness; - qTargetCone = btQuaternion(swingAxis, swingAngle); - } - } - - // clamp twist - if (m_twistSpan >= btScalar(0.05f)) - { - btScalar twistAngle; btVector3 twistAxis; - computeTwistLimitInfo(qTargetTwist, twistAngle, twistAxis); - - if (fabs(twistAngle) > SIMD_EPSILON) - { - // eddy todo: limitSoftness used here??? - if (twistAngle > m_twistSpan*softness) - twistAngle = m_twistSpan*softness; - else if (twistAngle < -m_twistSpan*softness) - twistAngle = -m_twistSpan*softness; - qTargetTwist = btQuaternion(twistAxis, twistAngle); - } - } - - m_qTarget = qTargetCone * qTargetTwist; - } -} - -///override the default global value of a parameter (such as ERP or CFM), optionally provide the axis (0..5). -///If no axis is provided, it uses the default axis for this constraint. -void btConeTwistConstraint::setParam(int num, btScalar value, int axis) -{ - switch(num) - { - case BT_CONSTRAINT_ERP : - case BT_CONSTRAINT_STOP_ERP : - if((axis >= 0) && (axis < 3)) - { - m_linERP = value; - m_flags |= BT_CONETWIST_FLAGS_LIN_ERP; - } - else - { - m_biasFactor = value; - } - break; - case BT_CONSTRAINT_CFM : - case BT_CONSTRAINT_STOP_CFM : - if((axis >= 0) && (axis < 3)) - { - m_linCFM = value; - m_flags |= BT_CONETWIST_FLAGS_LIN_CFM; - } - else - { - m_angCFM = value; - m_flags |= BT_CONETWIST_FLAGS_ANG_CFM; - } - break; - default: - btAssertConstrParams(0); - break; - } -} - -///return the local value of parameter -btScalar btConeTwistConstraint::getParam(int num, int axis) const -{ - btScalar retVal = 0; - switch(num) - { - case BT_CONSTRAINT_ERP : - case BT_CONSTRAINT_STOP_ERP : - if((axis >= 0) && (axis < 3)) - { - btAssertConstrParams(m_flags & BT_CONETWIST_FLAGS_LIN_ERP); - retVal = m_linERP; - } - else if((axis >= 3) && (axis < 6)) - { - retVal = m_biasFactor; - } - else - { - btAssertConstrParams(0); - } - break; - case BT_CONSTRAINT_CFM : - case BT_CONSTRAINT_STOP_CFM : - if((axis >= 0) && (axis < 3)) - { - btAssertConstrParams(m_flags & BT_CONETWIST_FLAGS_LIN_CFM); - retVal = m_linCFM; - } - else if((axis >= 3) && (axis < 6)) - { - btAssertConstrParams(m_flags & BT_CONETWIST_FLAGS_ANG_CFM); - retVal = m_angCFM; - } - else - { - btAssertConstrParams(0); - } - break; - default : - btAssertConstrParams(0); - } - return retVal; -} - - - diff --git a/extern/bullet2/BulletDynamics/ConstraintSolver/btConeTwistConstraint.h b/extern/bullet2/BulletDynamics/ConstraintSolver/btConeTwistConstraint.h deleted file mode 100644 index f310d474e7a..00000000000 --- a/extern/bullet2/BulletDynamics/ConstraintSolver/btConeTwistConstraint.h +++ /dev/null @@ -1,332 +0,0 @@ -/* -Bullet Continuous Collision Detection and Physics Library -btConeTwistConstraint is Copyright (c) 2007 Starbreeze Studios - -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. - -Written by: Marcus Hennix -*/ - - - -/* -Overview: - -btConeTwistConstraint can be used to simulate ragdoll joints (upper arm, leg etc). -It is a fixed translation, 3 degree-of-freedom (DOF) rotational "joint". -It divides the 3 rotational DOFs into swing (movement within a cone) and twist. -Swing is divided into swing1 and swing2 which can have different limits, giving an elliptical shape. -(Note: the cone's base isn't flat, so this ellipse is "embedded" on the surface of a sphere.) - -In the contraint's frame of reference: -twist is along the x-axis, -and swing 1 and 2 are along the z and y axes respectively. -*/ - - - -#ifndef CONETWISTCONSTRAINT_H -#define CONETWISTCONSTRAINT_H - -#include "LinearMath/btVector3.h" -#include "btJacobianEntry.h" -#include "btTypedConstraint.h" - -class btRigidBody; - -enum btConeTwistFlags -{ - BT_CONETWIST_FLAGS_LIN_CFM = 1, - BT_CONETWIST_FLAGS_LIN_ERP = 2, - BT_CONETWIST_FLAGS_ANG_CFM = 4 -}; - -///btConeTwistConstraint can be used to simulate ragdoll joints (upper arm, leg etc) -class btConeTwistConstraint : public btTypedConstraint -{ -#ifdef IN_PARALLELL_SOLVER -public: -#endif - btJacobianEntry m_jac[3]; //3 orthogonal linear constraints - - btTransform m_rbAFrame; - btTransform m_rbBFrame; - - btScalar m_limitSoftness; - btScalar m_biasFactor; - btScalar m_relaxationFactor; - - btScalar m_damping; - - btScalar m_swingSpan1; - btScalar m_swingSpan2; - btScalar m_twistSpan; - - btScalar m_fixThresh; - - btVector3 m_swingAxis; - btVector3 m_twistAxis; - - btScalar m_kSwing; - btScalar m_kTwist; - - btScalar m_twistLimitSign; - btScalar m_swingCorrection; - btScalar m_twistCorrection; - - btScalar m_twistAngle; - - btScalar m_accSwingLimitImpulse; - btScalar m_accTwistLimitImpulse; - - bool m_angularOnly; - bool m_solveTwistLimit; - bool m_solveSwingLimit; - - bool m_useSolveConstraintObsolete; - - // not yet used... - btScalar m_swingLimitRatio; - btScalar m_twistLimitRatio; - btVector3 m_twistAxisA; - - // motor - bool m_bMotorEnabled; - bool m_bNormalizedMotorStrength; - btQuaternion m_qTarget; - btScalar m_maxMotorImpulse; - btVector3 m_accMotorImpulse; - - // parameters - int m_flags; - btScalar m_linCFM; - btScalar m_linERP; - btScalar m_angCFM; - -protected: - - void init(); - - void computeConeLimitInfo(const btQuaternion& qCone, // in - btScalar& swingAngle, btVector3& vSwingAxis, btScalar& swingLimit); // all outs - - void computeTwistLimitInfo(const btQuaternion& qTwist, // in - btScalar& twistAngle, btVector3& vTwistAxis); // all outs - - void adjustSwingAxisToUseEllipseNormal(btVector3& vSwingAxis) const; - - -public: - - btConeTwistConstraint(btRigidBody& rbA,btRigidBody& rbB,const btTransform& rbAFrame, const btTransform& rbBFrame); - - btConeTwistConstraint(btRigidBody& rbA,const btTransform& rbAFrame); - - virtual void buildJacobian(); - - virtual void getInfo1 (btConstraintInfo1* info); - - void getInfo1NonVirtual(btConstraintInfo1* info); - - virtual void getInfo2 (btConstraintInfo2* info); - - void getInfo2NonVirtual(btConstraintInfo2* info,const btTransform& transA,const btTransform& transB,const btMatrix3x3& invInertiaWorldA,const btMatrix3x3& invInertiaWorldB); - - virtual void solveConstraintObsolete(btRigidBody& bodyA,btRigidBody& bodyB,btScalar timeStep); - - void updateRHS(btScalar timeStep); - - const btRigidBody& getRigidBodyA() const - { - return m_rbA; - } - const btRigidBody& getRigidBodyB() const - { - return m_rbB; - } - - void setAngularOnly(bool angularOnly) - { - m_angularOnly = angularOnly; - } - - void setLimit(int limitIndex,btScalar limitValue) - { - switch (limitIndex) - { - case 3: - { - m_twistSpan = limitValue; - break; - } - case 4: - { - m_swingSpan2 = limitValue; - break; - } - case 5: - { - m_swingSpan1 = limitValue; - break; - } - default: - { - } - }; - } - - // setLimit(), a few notes: - // _softness: - // 0->1, recommend ~0.8->1. - // describes % of limits where movement is free. - // beyond this softness %, the limit is gradually enforced until the "hard" (1.0) limit is reached. - // _biasFactor: - // 0->1?, recommend 0.3 +/-0.3 or so. - // strength with which constraint resists zeroth order (angular, not angular velocity) limit violation. - // __relaxationFactor: - // 0->1, recommend to stay near 1. - // the lower the value, the less the constraint will fight velocities which violate the angular limits. - void setLimit(btScalar _swingSpan1,btScalar _swingSpan2,btScalar _twistSpan, btScalar _softness = 1.f, btScalar _biasFactor = 0.3f, btScalar _relaxationFactor = 1.0f) - { - m_swingSpan1 = _swingSpan1; - m_swingSpan2 = _swingSpan2; - m_twistSpan = _twistSpan; - - m_limitSoftness = _softness; - m_biasFactor = _biasFactor; - m_relaxationFactor = _relaxationFactor; - } - - const btTransform& getAFrame() { return m_rbAFrame; }; - const btTransform& getBFrame() { return m_rbBFrame; }; - - inline int getSolveTwistLimit() - { - return m_solveTwistLimit; - } - - inline int getSolveSwingLimit() - { - return m_solveTwistLimit; - } - - inline btScalar getTwistLimitSign() - { - return m_twistLimitSign; - } - - void calcAngleInfo(); - void calcAngleInfo2(const btTransform& transA, const btTransform& transB,const btMatrix3x3& invInertiaWorldA,const btMatrix3x3& invInertiaWorldB); - - inline btScalar getSwingSpan1() - { - return m_swingSpan1; - } - inline btScalar getSwingSpan2() - { - return m_swingSpan2; - } - inline btScalar getTwistSpan() - { - return m_twistSpan; - } - inline btScalar getTwistAngle() - { - return m_twistAngle; - } - bool isPastSwingLimit() { return m_solveSwingLimit; } - - - void setDamping(btScalar damping) { m_damping = damping; } - - void enableMotor(bool b) { m_bMotorEnabled = b; } - void setMaxMotorImpulse(btScalar maxMotorImpulse) { m_maxMotorImpulse = maxMotorImpulse; m_bNormalizedMotorStrength = false; } - void setMaxMotorImpulseNormalized(btScalar maxMotorImpulse) { m_maxMotorImpulse = maxMotorImpulse; m_bNormalizedMotorStrength = true; } - - btScalar getFixThresh() { return m_fixThresh; } - void setFixThresh(btScalar fixThresh) { m_fixThresh = fixThresh; } - - // setMotorTarget: - // q: the desired rotation of bodyA wrt bodyB. - // note: if q violates the joint limits, the internal target is clamped to avoid conflicting impulses (very bad for stability) - // note: don't forget to enableMotor() - void setMotorTarget(const btQuaternion &q); - - // same as above, but q is the desired rotation of frameA wrt frameB in constraint space - void setMotorTargetInConstraintSpace(const btQuaternion &q); - - btVector3 GetPointForAngle(btScalar fAngleInRadians, btScalar fLength) const; - - ///override the default global value of a parameter (such as ERP or CFM), optionally provide the axis (0..5). - ///If no axis is provided, it uses the default axis for this constraint. - virtual void setParam(int num, btScalar value, int axis = -1); - ///return the local value of parameter - virtual btScalar getParam(int num, int axis = -1) const; - - virtual int calculateSerializeBufferSize() const; - - ///fills the dataBuffer and returns the struct name (and 0 on failure) - virtual const char* serialize(void* dataBuffer, btSerializer* serializer) const; - -}; - -///do not change those serialization structures, it requires an updated sBulletDNAstr/sBulletDNAstr64 -struct btConeTwistConstraintData -{ - btTypedConstraintData m_typeConstraintData; - btTransformFloatData m_rbAFrame; - btTransformFloatData m_rbBFrame; - - //limits - float m_swingSpan1; - float m_swingSpan2; - float m_twistSpan; - float m_limitSoftness; - float m_biasFactor; - float m_relaxationFactor; - - float m_damping; - - char m_pad[4]; - -}; - - - -SIMD_FORCE_INLINE int btConeTwistConstraint::calculateSerializeBufferSize() const -{ - return sizeof(btConeTwistConstraintData); - -} - - - ///fills the dataBuffer and returns the struct name (and 0 on failure) -SIMD_FORCE_INLINE const char* btConeTwistConstraint::serialize(void* dataBuffer, btSerializer* serializer) const -{ - btConeTwistConstraintData* cone = (btConeTwistConstraintData*) dataBuffer; - btTypedConstraint::serialize(&cone->m_typeConstraintData,serializer); - - m_rbAFrame.serializeFloat(cone->m_rbAFrame); - m_rbBFrame.serializeFloat(cone->m_rbBFrame); - - cone->m_swingSpan1 = float(m_swingSpan1); - cone->m_swingSpan2 = float(m_swingSpan2); - cone->m_twistSpan = float(m_twistSpan); - cone->m_limitSoftness = float(m_limitSoftness); - cone->m_biasFactor = float(m_biasFactor); - cone->m_relaxationFactor = float(m_relaxationFactor); - cone->m_damping = float(m_damping); - - return "btConeTwistConstraintData"; -} - - -#endif //CONETWISTCONSTRAINT_H diff --git a/extern/bullet2/BulletDynamics/ConstraintSolver/btConstraintSolver.h b/extern/bullet2/BulletDynamics/ConstraintSolver/btConstraintSolver.h deleted file mode 100644 index 7a8e9c1953d..00000000000 --- a/extern/bullet2/BulletDynamics/ConstraintSolver/btConstraintSolver.h +++ /dev/null @@ -1,52 +0,0 @@ -/* -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. -*/ - -#ifndef CONSTRAINT_SOLVER_H -#define CONSTRAINT_SOLVER_H - -#include "LinearMath/btScalar.h" - -class btPersistentManifold; -class btRigidBody; -class btCollisionObject; -class btTypedConstraint; -struct btContactSolverInfo; -struct btBroadphaseProxy; -class btIDebugDraw; -class btStackAlloc; -class btDispatcher; -/// btConstraintSolver provides solver interface -class btConstraintSolver -{ - -public: - - virtual ~btConstraintSolver() {} - - virtual void prepareSolve (int /* numBodies */, int /* numManifolds */) {;} - - ///solve a group of constraints - virtual btScalar solveGroup(btCollisionObject** bodies,int numBodies,btPersistentManifold** manifold,int numManifolds,btTypedConstraint** constraints,int numConstraints, const btContactSolverInfo& info,class btIDebugDraw* debugDrawer, btStackAlloc* stackAlloc,btDispatcher* dispatcher) = 0; - - virtual void allSolved (const btContactSolverInfo& /* info */,class btIDebugDraw* /* debugDrawer */, btStackAlloc* /* stackAlloc */) {;} - - ///clear internal cached data and reset random seed - virtual void reset() = 0; -}; - - - - -#endif //CONSTRAINT_SOLVER_H diff --git a/extern/bullet2/BulletDynamics/ConstraintSolver/btContactConstraint.cpp b/extern/bullet2/BulletDynamics/ConstraintSolver/btContactConstraint.cpp deleted file mode 100644 index d97096d9f26..00000000000 --- a/extern/bullet2/BulletDynamics/ConstraintSolver/btContactConstraint.cpp +++ /dev/null @@ -1,134 +0,0 @@ -/* -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 "btContactConstraint.h" -#include "BulletDynamics/Dynamics/btRigidBody.h" -#include "LinearMath/btVector3.h" -#include "btJacobianEntry.h" -#include "btContactSolverInfo.h" -#include "LinearMath/btMinMax.h" -#include "BulletCollision/NarrowPhaseCollision/btManifoldPoint.h" - - - -btContactConstraint::btContactConstraint(btPersistentManifold* contactManifold,btRigidBody& rbA,btRigidBody& rbB) -:btTypedConstraint(CONTACT_CONSTRAINT_TYPE,rbA,rbB), - m_contactManifold(*contactManifold) -{ - -} - -btContactConstraint::~btContactConstraint() -{ - -} - -void btContactConstraint::setContactManifold(btPersistentManifold* contactManifold) -{ - m_contactManifold = *contactManifold; -} - -void btContactConstraint::getInfo1 (btConstraintInfo1* info) -{ - -} - -void btContactConstraint::getInfo2 (btConstraintInfo2* info) -{ - -} - -void btContactConstraint::buildJacobian() -{ - -} - - - - - -#include "btContactConstraint.h" -#include "BulletDynamics/Dynamics/btRigidBody.h" -#include "LinearMath/btVector3.h" -#include "btJacobianEntry.h" -#include "btContactSolverInfo.h" -#include "LinearMath/btMinMax.h" -#include "BulletCollision/NarrowPhaseCollision/btManifoldPoint.h" - -#define ASSERT2 btAssert - -#define USE_INTERNAL_APPLY_IMPULSE 1 - - -//bilateral constraint between two dynamic objects -void resolveSingleBilateral(btRigidBody& body1, const btVector3& pos1, - btRigidBody& body2, const btVector3& pos2, - btScalar distance, const btVector3& normal,btScalar& impulse ,btScalar timeStep) -{ - (void)timeStep; - (void)distance; - - - btScalar normalLenSqr = normal.length2(); - ASSERT2(btFabs(normalLenSqr) < btScalar(1.1)); - if (normalLenSqr > btScalar(1.1)) - { - impulse = btScalar(0.); - return; - } - btVector3 rel_pos1 = pos1 - body1.getCenterOfMassPosition(); - btVector3 rel_pos2 = pos2 - body2.getCenterOfMassPosition(); - //this jacobian entry could be re-used for all iterations - - btVector3 vel1 = body1.getVelocityInLocalPoint(rel_pos1); - btVector3 vel2 = body2.getVelocityInLocalPoint(rel_pos2); - btVector3 vel = vel1 - vel2; - - - btJacobianEntry jac(body1.getCenterOfMassTransform().getBasis().transpose(), - body2.getCenterOfMassTransform().getBasis().transpose(), - rel_pos1,rel_pos2,normal,body1.getInvInertiaDiagLocal(),body1.getInvMass(), - body2.getInvInertiaDiagLocal(),body2.getInvMass()); - - btScalar jacDiagAB = jac.getDiagonal(); - btScalar jacDiagABInv = btScalar(1.) / jacDiagAB; - - btScalar rel_vel = jac.getRelativeVelocity( - body1.getLinearVelocity(), - body1.getCenterOfMassTransform().getBasis().transpose() * body1.getAngularVelocity(), - body2.getLinearVelocity(), - body2.getCenterOfMassTransform().getBasis().transpose() * body2.getAngularVelocity()); - btScalar a; - a=jacDiagABInv; - - - rel_vel = normal.dot(vel); - - //todo: move this into proper structure - btScalar contactDamping = btScalar(0.2); - -#ifdef ONLY_USE_LINEAR_MASS - btScalar massTerm = btScalar(1.) / (body1.getInvMass() + body2.getInvMass()); - impulse = - contactDamping * rel_vel * massTerm; -#else - btScalar velocityImpulse = -contactDamping * rel_vel * jacDiagABInv; - impulse = velocityImpulse; -#endif -} - - - - diff --git a/extern/bullet2/BulletDynamics/ConstraintSolver/btContactConstraint.h b/extern/bullet2/BulletDynamics/ConstraintSolver/btContactConstraint.h deleted file mode 100644 index 63c1a417bc1..00000000000 --- a/extern/bullet2/BulletDynamics/ConstraintSolver/btContactConstraint.h +++ /dev/null @@ -1,68 +0,0 @@ -/* -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. -*/ - -#ifndef CONTACT_CONSTRAINT_H -#define CONTACT_CONSTRAINT_H - -#include "LinearMath/btVector3.h" -#include "btJacobianEntry.h" -#include "btTypedConstraint.h" -#include "BulletCollision/NarrowPhaseCollision/btPersistentManifold.h" - -///btContactConstraint can be automatically created to solve contact constraints using the unified btTypedConstraint interface -ATTRIBUTE_ALIGNED16(class) btContactConstraint : public btTypedConstraint -{ -protected: - - btPersistentManifold m_contactManifold; - -public: - - - btContactConstraint(btPersistentManifold* contactManifold,btRigidBody& rbA,btRigidBody& rbB); - - void setContactManifold(btPersistentManifold* contactManifold); - - btPersistentManifold* getContactManifold() - { - return &m_contactManifold; - } - - const btPersistentManifold* getContactManifold() const - { - return &m_contactManifold; - } - - virtual ~btContactConstraint(); - - virtual void getInfo1 (btConstraintInfo1* info); - - virtual void getInfo2 (btConstraintInfo2* info); - - ///obsolete methods - virtual void buildJacobian(); - - -}; - - -///resolveSingleBilateral is an obsolete methods used for vehicle friction between two dynamic objects -void resolveSingleBilateral(btRigidBody& body1, const btVector3& pos1, - btRigidBody& body2, const btVector3& pos2, - btScalar distance, const btVector3& normal,btScalar& impulse ,btScalar timeStep); - - - -#endif //CONTACT_CONSTRAINT_H diff --git a/extern/bullet2/BulletDynamics/ConstraintSolver/btContactSolverInfo.h b/extern/bullet2/BulletDynamics/ConstraintSolver/btContactSolverInfo.h deleted file mode 100644 index 1025da42f5e..00000000000 --- a/extern/bullet2/BulletDynamics/ConstraintSolver/btContactSolverInfo.h +++ /dev/null @@ -1,87 +0,0 @@ -/* -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. -*/ - -#ifndef CONTACT_SOLVER_INFO -#define CONTACT_SOLVER_INFO - -enum btSolverMode -{ - SOLVER_RANDMIZE_ORDER = 1, - SOLVER_FRICTION_SEPARATE = 2, - SOLVER_USE_WARMSTARTING = 4, - SOLVER_USE_FRICTION_WARMSTARTING = 8, - SOLVER_USE_2_FRICTION_DIRECTIONS = 16, - SOLVER_ENABLE_FRICTION_DIRECTION_CACHING = 32, - SOLVER_DISABLE_VELOCITY_DEPENDENT_FRICTION_DIRECTION = 64, - SOLVER_CACHE_FRIENDLY = 128, - SOLVER_SIMD = 256, //enabled for Windows, the solver innerloop is branchless SIMD, 40% faster than FPU/scalar version - SOLVER_CUDA = 512 //will be open sourced during Game Developers Conference 2009. Much faster. -}; - -struct btContactSolverInfoData -{ - - - btScalar m_tau; - btScalar m_damping; - btScalar m_friction; - btScalar m_timeStep; - btScalar m_restitution; - int m_numIterations; - btScalar m_maxErrorReduction; - btScalar m_sor; - btScalar m_erp;//used as Baumgarte factor - btScalar m_erp2;//used in Split Impulse - btScalar m_globalCfm;//constraint force mixing - int m_splitImpulse; - btScalar m_splitImpulsePenetrationThreshold; - btScalar m_linearSlop; - btScalar m_warmstartingFactor; - - int m_solverMode; - int m_restingContactRestitutionThreshold; - int m_minimumSolverBatchSize; - - -}; - -struct btContactSolverInfo : public btContactSolverInfoData -{ - - - - inline btContactSolverInfo() - { - m_tau = btScalar(0.6); - m_damping = btScalar(1.0); - m_friction = btScalar(0.3); - m_restitution = btScalar(0.); - m_maxErrorReduction = btScalar(20.); - m_numIterations = 10; - m_erp = btScalar(0.2); - m_erp2 = btScalar(0.1); - m_globalCfm = btScalar(0.); - m_sor = btScalar(1.); - m_splitImpulse = false; - m_splitImpulsePenetrationThreshold = -0.02f; - m_linearSlop = btScalar(0.0); - m_warmstartingFactor=btScalar(0.85); - m_solverMode = SOLVER_USE_WARMSTARTING | SOLVER_SIMD;// | SOLVER_RANDMIZE_ORDER; - m_restingContactRestitutionThreshold = 2;//resting contact lifetime threshold to disable restitution - m_minimumSolverBatchSize = 128; //try to combine islands until the amount of constraints reaches this limit - } -}; - -#endif //CONTACT_SOLVER_INFO diff --git a/extern/bullet2/BulletDynamics/ConstraintSolver/btGeneric6DofConstraint.cpp b/extern/bullet2/BulletDynamics/ConstraintSolver/btGeneric6DofConstraint.cpp deleted file mode 100644 index a970d706284..00000000000 --- a/extern/bullet2/BulletDynamics/ConstraintSolver/btGeneric6DofConstraint.cpp +++ /dev/null @@ -1,1012 +0,0 @@ -/* -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. -*/ -/* -2007-09-09 -Refactored by Francisco Le?n -email: projectileman@yahoo.com -http://gimpact.sf.net -*/ - -#include "btGeneric6DofConstraint.h" -#include "BulletDynamics/Dynamics/btRigidBody.h" -#include "LinearMath/btTransformUtil.h" -#include "LinearMath/btTransformUtil.h" -#include <new> - - - -#define D6_USE_OBSOLETE_METHOD false -#define D6_USE_FRAME_OFFSET true - - - - - - -btGeneric6DofConstraint::btGeneric6DofConstraint(btRigidBody& rbA, btRigidBody& rbB, const btTransform& frameInA, const btTransform& frameInB, bool useLinearReferenceFrameA) -: btTypedConstraint(D6_CONSTRAINT_TYPE, rbA, rbB) -, m_frameInA(frameInA) -, m_frameInB(frameInB), -m_useLinearReferenceFrameA(useLinearReferenceFrameA), -m_useOffsetForConstraintFrame(D6_USE_FRAME_OFFSET), -m_flags(0), -m_useSolveConstraintObsolete(D6_USE_OBSOLETE_METHOD) -{ - calculateTransforms(); -} - - - -btGeneric6DofConstraint::btGeneric6DofConstraint(btRigidBody& rbB, const btTransform& frameInB, bool useLinearReferenceFrameB) - : btTypedConstraint(D6_CONSTRAINT_TYPE, getFixedBody(), rbB), - m_frameInB(frameInB), - m_useLinearReferenceFrameA(useLinearReferenceFrameB), - m_useOffsetForConstraintFrame(D6_USE_FRAME_OFFSET), - m_flags(0), - m_useSolveConstraintObsolete(false) -{ - ///not providing rigidbody A means implicitly using worldspace for body A - m_frameInA = rbB.getCenterOfMassTransform() * m_frameInB; - calculateTransforms(); -} - - - - -#define GENERIC_D6_DISABLE_WARMSTARTING 1 - - - -btScalar btGetMatrixElem(const btMatrix3x3& mat, int index); -btScalar btGetMatrixElem(const btMatrix3x3& mat, int index) -{ - int i = index%3; - int j = index/3; - return mat[i][j]; -} - - - -///MatrixToEulerXYZ from http://www.geometrictools.com/LibFoundation/Mathematics/Wm4Matrix3.inl.html -bool matrixToEulerXYZ(const btMatrix3x3& mat,btVector3& xyz); -bool matrixToEulerXYZ(const btMatrix3x3& mat,btVector3& xyz) -{ - // // rot = cy*cz -cy*sz sy - // // cz*sx*sy+cx*sz cx*cz-sx*sy*sz -cy*sx - // // -cx*cz*sy+sx*sz cz*sx+cx*sy*sz cx*cy - // - - btScalar fi = btGetMatrixElem(mat,2); - if (fi < btScalar(1.0f)) - { - if (fi > btScalar(-1.0f)) - { - xyz[0] = btAtan2(-btGetMatrixElem(mat,5),btGetMatrixElem(mat,8)); - xyz[1] = btAsin(btGetMatrixElem(mat,2)); - xyz[2] = btAtan2(-btGetMatrixElem(mat,1),btGetMatrixElem(mat,0)); - return true; - } - else - { - // WARNING. Not unique. XA - ZA = -atan2(r10,r11) - xyz[0] = -btAtan2(btGetMatrixElem(mat,3),btGetMatrixElem(mat,4)); - xyz[1] = -SIMD_HALF_PI; - xyz[2] = btScalar(0.0); - return false; - } - } - else - { - // WARNING. Not unique. XAngle + ZAngle = atan2(r10,r11) - xyz[0] = btAtan2(btGetMatrixElem(mat,3),btGetMatrixElem(mat,4)); - xyz[1] = SIMD_HALF_PI; - xyz[2] = 0.0; - } - return false; -} - -//////////////////////////// btRotationalLimitMotor //////////////////////////////////// - -int btRotationalLimitMotor::testLimitValue(btScalar test_value) -{ - if(m_loLimit>m_hiLimit) - { - m_currentLimit = 0;//Free from violation - return 0; - } - if (test_value < m_loLimit) - { - m_currentLimit = 1;//low limit violation - m_currentLimitError = test_value - m_loLimit; - return 1; - } - else if (test_value> m_hiLimit) - { - m_currentLimit = 2;//High limit violation - m_currentLimitError = test_value - m_hiLimit; - return 2; - }; - - m_currentLimit = 0;//Free from violation - return 0; - -} - - - -btScalar btRotationalLimitMotor::solveAngularLimits( - btScalar timeStep,btVector3& axis,btScalar jacDiagABInv, - btRigidBody * body0, btRigidBody * body1 ) -{ - if (needApplyTorques()==false) return 0.0f; - - btScalar target_velocity = m_targetVelocity; - btScalar maxMotorForce = m_maxMotorForce; - - //current error correction - if (m_currentLimit!=0) - { - target_velocity = -m_stopERP*m_currentLimitError/(timeStep); - maxMotorForce = m_maxLimitForce; - } - - maxMotorForce *= timeStep; - - // current velocity difference - - btVector3 angVelA; - body0->internalGetAngularVelocity(angVelA); - btVector3 angVelB; - body1->internalGetAngularVelocity(angVelB); - - btVector3 vel_diff; - vel_diff = angVelA-angVelB; - - - - btScalar rel_vel = axis.dot(vel_diff); - - // correction velocity - btScalar motor_relvel = m_limitSoftness*(target_velocity - m_damping*rel_vel); - - - if ( motor_relvel < SIMD_EPSILON && motor_relvel > -SIMD_EPSILON ) - { - return 0.0f;//no need for applying force - } - - - // correction impulse - btScalar unclippedMotorImpulse = (1+m_bounce)*motor_relvel*jacDiagABInv; - - // clip correction impulse - btScalar clippedMotorImpulse; - - ///@todo: should clip against accumulated impulse - if (unclippedMotorImpulse>0.0f) - { - clippedMotorImpulse = unclippedMotorImpulse > maxMotorForce? maxMotorForce: unclippedMotorImpulse; - } - else - { - clippedMotorImpulse = unclippedMotorImpulse < -maxMotorForce ? -maxMotorForce: unclippedMotorImpulse; - } - - - // sort with accumulated impulses - btScalar lo = btScalar(-BT_LARGE_FLOAT); - btScalar hi = btScalar(BT_LARGE_FLOAT); - - btScalar oldaccumImpulse = m_accumulatedImpulse; - btScalar sum = oldaccumImpulse + clippedMotorImpulse; - m_accumulatedImpulse = sum > hi ? btScalar(0.) : sum < lo ? btScalar(0.) : sum; - - clippedMotorImpulse = m_accumulatedImpulse - oldaccumImpulse; - - btVector3 motorImp = clippedMotorImpulse * axis; - - //body0->applyTorqueImpulse(motorImp); - //body1->applyTorqueImpulse(-motorImp); - - body0->internalApplyImpulse(btVector3(0,0,0), body0->getInvInertiaTensorWorld()*axis,clippedMotorImpulse); - body1->internalApplyImpulse(btVector3(0,0,0), body1->getInvInertiaTensorWorld()*axis,-clippedMotorImpulse); - - - return clippedMotorImpulse; - - -} - -//////////////////////////// End btRotationalLimitMotor //////////////////////////////////// - - - - -//////////////////////////// btTranslationalLimitMotor //////////////////////////////////// - - -int btTranslationalLimitMotor::testLimitValue(int limitIndex, btScalar test_value) -{ - btScalar loLimit = m_lowerLimit[limitIndex]; - btScalar hiLimit = m_upperLimit[limitIndex]; - if(loLimit > hiLimit) - { - m_currentLimit[limitIndex] = 0;//Free from violation - m_currentLimitError[limitIndex] = btScalar(0.f); - return 0; - } - - if (test_value < loLimit) - { - m_currentLimit[limitIndex] = 2;//low limit violation - m_currentLimitError[limitIndex] = test_value - loLimit; - return 2; - } - else if (test_value> hiLimit) - { - m_currentLimit[limitIndex] = 1;//High limit violation - m_currentLimitError[limitIndex] = test_value - hiLimit; - return 1; - }; - - m_currentLimit[limitIndex] = 0;//Free from violation - m_currentLimitError[limitIndex] = btScalar(0.f); - return 0; -} - - - -btScalar btTranslationalLimitMotor::solveLinearAxis( - btScalar timeStep, - btScalar jacDiagABInv, - btRigidBody& body1,const btVector3 &pointInA, - btRigidBody& body2,const btVector3 &pointInB, - int limit_index, - const btVector3 & axis_normal_on_a, - const btVector3 & anchorPos) -{ - - ///find relative velocity - // btVector3 rel_pos1 = pointInA - body1.getCenterOfMassPosition(); - // btVector3 rel_pos2 = pointInB - body2.getCenterOfMassPosition(); - btVector3 rel_pos1 = anchorPos - body1.getCenterOfMassPosition(); - btVector3 rel_pos2 = anchorPos - body2.getCenterOfMassPosition(); - - btVector3 vel1; - body1.internalGetVelocityInLocalPointObsolete(rel_pos1,vel1); - btVector3 vel2; - body2.internalGetVelocityInLocalPointObsolete(rel_pos2,vel2); - btVector3 vel = vel1 - vel2; - - btScalar rel_vel = axis_normal_on_a.dot(vel); - - - - /// apply displacement correction - - //positional error (zeroth order error) - btScalar depth = -(pointInA - pointInB).dot(axis_normal_on_a); - btScalar lo = btScalar(-BT_LARGE_FLOAT); - btScalar hi = btScalar(BT_LARGE_FLOAT); - - btScalar minLimit = m_lowerLimit[limit_index]; - btScalar maxLimit = m_upperLimit[limit_index]; - - //handle the limits - if (minLimit < maxLimit) - { - { - if (depth > maxLimit) - { - depth -= maxLimit; - lo = btScalar(0.); - - } - else - { - if (depth < minLimit) - { - depth -= minLimit; - hi = btScalar(0.); - } - else - { - return 0.0f; - } - } - } - } - - btScalar normalImpulse= m_limitSoftness*(m_restitution*depth/timeStep - m_damping*rel_vel) * jacDiagABInv; - - - - - btScalar oldNormalImpulse = m_accumulatedImpulse[limit_index]; - btScalar sum = oldNormalImpulse + normalImpulse; - m_accumulatedImpulse[limit_index] = sum > hi ? btScalar(0.) : sum < lo ? btScalar(0.) : sum; - normalImpulse = m_accumulatedImpulse[limit_index] - oldNormalImpulse; - - btVector3 impulse_vector = axis_normal_on_a * normalImpulse; - //body1.applyImpulse( impulse_vector, rel_pos1); - //body2.applyImpulse(-impulse_vector, rel_pos2); - - btVector3 ftorqueAxis1 = rel_pos1.cross(axis_normal_on_a); - btVector3 ftorqueAxis2 = rel_pos2.cross(axis_normal_on_a); - body1.internalApplyImpulse(axis_normal_on_a*body1.getInvMass(), body1.getInvInertiaTensorWorld()*ftorqueAxis1,normalImpulse); - body2.internalApplyImpulse(axis_normal_on_a*body2.getInvMass(), body2.getInvInertiaTensorWorld()*ftorqueAxis2,-normalImpulse); - - - - - return normalImpulse; -} - -//////////////////////////// btTranslationalLimitMotor //////////////////////////////////// - -void btGeneric6DofConstraint::calculateAngleInfo() -{ - btMatrix3x3 relative_frame = m_calculatedTransformA.getBasis().inverse()*m_calculatedTransformB.getBasis(); - matrixToEulerXYZ(relative_frame,m_calculatedAxisAngleDiff); - // in euler angle mode we do not actually constrain the angular velocity - // along the axes axis[0] and axis[2] (although we do use axis[1]) : - // - // to get constrain w2-w1 along ...not - // ------ --------------------- ------ - // d(angle[0])/dt = 0 ax[1] x ax[2] ax[0] - // d(angle[1])/dt = 0 ax[1] - // d(angle[2])/dt = 0 ax[0] x ax[1] ax[2] - // - // constraining w2-w1 along an axis 'a' means that a'*(w2-w1)=0. - // to prove the result for angle[0], write the expression for angle[0] from - // GetInfo1 then take the derivative. to prove this for angle[2] it is - // easier to take the euler rate expression for d(angle[2])/dt with respect - // to the components of w and set that to 0. - btVector3 axis0 = m_calculatedTransformB.getBasis().getColumn(0); - btVector3 axis2 = m_calculatedTransformA.getBasis().getColumn(2); - - m_calculatedAxis[1] = axis2.cross(axis0); - m_calculatedAxis[0] = m_calculatedAxis[1].cross(axis2); - m_calculatedAxis[2] = axis0.cross(m_calculatedAxis[1]); - - m_calculatedAxis[0].normalize(); - m_calculatedAxis[1].normalize(); - m_calculatedAxis[2].normalize(); - -} - -void btGeneric6DofConstraint::calculateTransforms() -{ - calculateTransforms(m_rbA.getCenterOfMassTransform(),m_rbB.getCenterOfMassTransform()); -} - -void btGeneric6DofConstraint::calculateTransforms(const btTransform& transA,const btTransform& transB) -{ - m_calculatedTransformA = transA * m_frameInA; - m_calculatedTransformB = transB * m_frameInB; - calculateLinearInfo(); - calculateAngleInfo(); - if(m_useOffsetForConstraintFrame) - { // get weight factors depending on masses - btScalar miA = getRigidBodyA().getInvMass(); - btScalar miB = getRigidBodyB().getInvMass(); - m_hasStaticBody = (miA < SIMD_EPSILON) || (miB < SIMD_EPSILON); - btScalar miS = miA + miB; - if(miS > btScalar(0.f)) - { - m_factA = miB / miS; - } - else - { - m_factA = btScalar(0.5f); - } - m_factB = btScalar(1.0f) - m_factA; - } -} - - - -void btGeneric6DofConstraint::buildLinearJacobian( - btJacobianEntry & jacLinear,const btVector3 & normalWorld, - const btVector3 & pivotAInW,const btVector3 & pivotBInW) -{ - new (&jacLinear) btJacobianEntry( - m_rbA.getCenterOfMassTransform().getBasis().transpose(), - m_rbB.getCenterOfMassTransform().getBasis().transpose(), - pivotAInW - m_rbA.getCenterOfMassPosition(), - pivotBInW - m_rbB.getCenterOfMassPosition(), - normalWorld, - m_rbA.getInvInertiaDiagLocal(), - m_rbA.getInvMass(), - m_rbB.getInvInertiaDiagLocal(), - m_rbB.getInvMass()); -} - - - -void btGeneric6DofConstraint::buildAngularJacobian( - btJacobianEntry & jacAngular,const btVector3 & jointAxisW) -{ - new (&jacAngular) btJacobianEntry(jointAxisW, - m_rbA.getCenterOfMassTransform().getBasis().transpose(), - m_rbB.getCenterOfMassTransform().getBasis().transpose(), - m_rbA.getInvInertiaDiagLocal(), - m_rbB.getInvInertiaDiagLocal()); - -} - - - -bool btGeneric6DofConstraint::testAngularLimitMotor(int axis_index) -{ - btScalar angle = m_calculatedAxisAngleDiff[axis_index]; - angle = btAdjustAngleToLimits(angle, m_angularLimits[axis_index].m_loLimit, m_angularLimits[axis_index].m_hiLimit); - m_angularLimits[axis_index].m_currentPosition = angle; - //test limits - m_angularLimits[axis_index].testLimitValue(angle); - return m_angularLimits[axis_index].needApplyTorques(); -} - - - -void btGeneric6DofConstraint::buildJacobian() -{ -#ifndef __SPU__ - if (m_useSolveConstraintObsolete) - { - - // Clear accumulated impulses for the next simulation step - m_linearLimits.m_accumulatedImpulse.setValue(btScalar(0.), btScalar(0.), btScalar(0.)); - int i; - for(i = 0; i < 3; i++) - { - m_angularLimits[i].m_accumulatedImpulse = btScalar(0.); - } - //calculates transform - calculateTransforms(m_rbA.getCenterOfMassTransform(),m_rbB.getCenterOfMassTransform()); - - // const btVector3& pivotAInW = m_calculatedTransformA.getOrigin(); - // const btVector3& pivotBInW = m_calculatedTransformB.getOrigin(); - calcAnchorPos(); - btVector3 pivotAInW = m_AnchorPos; - btVector3 pivotBInW = m_AnchorPos; - - // not used here - // btVector3 rel_pos1 = pivotAInW - m_rbA.getCenterOfMassPosition(); - // btVector3 rel_pos2 = pivotBInW - m_rbB.getCenterOfMassPosition(); - - btVector3 normalWorld; - //linear part - for (i=0;i<3;i++) - { - if (m_linearLimits.isLimited(i)) - { - if (m_useLinearReferenceFrameA) - normalWorld = m_calculatedTransformA.getBasis().getColumn(i); - else - normalWorld = m_calculatedTransformB.getBasis().getColumn(i); - - buildLinearJacobian( - m_jacLinear[i],normalWorld , - pivotAInW,pivotBInW); - - } - } - - // angular part - for (i=0;i<3;i++) - { - //calculates error angle - if (testAngularLimitMotor(i)) - { - normalWorld = this->getAxis(i); - // Create angular atom - buildAngularJacobian(m_jacAng[i],normalWorld); - } - } - - } -#endif //__SPU__ - -} - - -void btGeneric6DofConstraint::getInfo1 (btConstraintInfo1* info) -{ - if (m_useSolveConstraintObsolete) - { - info->m_numConstraintRows = 0; - info->nub = 0; - } else - { - //prepare constraint - calculateTransforms(m_rbA.getCenterOfMassTransform(),m_rbB.getCenterOfMassTransform()); - info->m_numConstraintRows = 0; - info->nub = 6; - int i; - //test linear limits - for(i = 0; i < 3; i++) - { - if(m_linearLimits.needApplyForce(i)) - { - info->m_numConstraintRows++; - info->nub--; - } - } - //test angular limits - for (i=0;i<3 ;i++ ) - { - if(testAngularLimitMotor(i)) - { - info->m_numConstraintRows++; - info->nub--; - } - } - } -} - -void btGeneric6DofConstraint::getInfo1NonVirtual (btConstraintInfo1* info) -{ - if (m_useSolveConstraintObsolete) - { - info->m_numConstraintRows = 0; - info->nub = 0; - } else - { - //pre-allocate all 6 - info->m_numConstraintRows = 6; - info->nub = 0; - } -} - - -void btGeneric6DofConstraint::getInfo2 (btConstraintInfo2* info) -{ - getInfo2NonVirtual(info,m_rbA.getCenterOfMassTransform(),m_rbB.getCenterOfMassTransform(), m_rbA.getLinearVelocity(),m_rbB.getLinearVelocity(),m_rbA.getAngularVelocity(), m_rbB.getAngularVelocity()); -} - -void btGeneric6DofConstraint::getInfo2NonVirtual (btConstraintInfo2* info, const btTransform& transA,const btTransform& transB,const btVector3& linVelA,const btVector3& linVelB,const btVector3& angVelA,const btVector3& angVelB) -{ - btAssert(!m_useSolveConstraintObsolete); - //prepare constraint - calculateTransforms(transA,transB); - if(m_useOffsetForConstraintFrame) - { // for stability better to solve angular limits first - int row = setAngularLimits(info, 0,transA,transB,linVelA,linVelB,angVelA,angVelB); - setLinearLimits(info, row, transA,transB,linVelA,linVelB,angVelA,angVelB); - } - else - { // leave old version for compatibility - int row = setLinearLimits(info, 0, transA,transB,linVelA,linVelB,angVelA,angVelB); - setAngularLimits(info, row,transA,transB,linVelA,linVelB,angVelA,angVelB); - } -} - - - -int btGeneric6DofConstraint::setLinearLimits(btConstraintInfo2* info, int row, const btTransform& transA,const btTransform& transB,const btVector3& linVelA,const btVector3& linVelB,const btVector3& angVelA,const btVector3& angVelB) -{ -// int row = 0; - //solve linear limits - btRotationalLimitMotor limot; - for (int i=0;i<3 ;i++ ) - { - if(m_linearLimits.needApplyForce(i)) - { // re-use rotational motor code - limot.m_bounce = btScalar(0.f); - limot.m_currentLimit = m_linearLimits.m_currentLimit[i]; - limot.m_currentPosition = m_linearLimits.m_currentLinearDiff[i]; - limot.m_currentLimitError = m_linearLimits.m_currentLimitError[i]; - limot.m_damping = m_linearLimits.m_damping; - limot.m_enableMotor = m_linearLimits.m_enableMotor[i]; - limot.m_hiLimit = m_linearLimits.m_upperLimit[i]; - limot.m_limitSoftness = m_linearLimits.m_limitSoftness; - limot.m_loLimit = m_linearLimits.m_lowerLimit[i]; - limot.m_maxLimitForce = btScalar(0.f); - limot.m_maxMotorForce = m_linearLimits.m_maxMotorForce[i]; - limot.m_targetVelocity = m_linearLimits.m_targetVelocity[i]; - btVector3 axis = m_calculatedTransformA.getBasis().getColumn(i); - int flags = m_flags >> (i * BT_6DOF_FLAGS_AXIS_SHIFT); - limot.m_normalCFM = (flags & BT_6DOF_FLAGS_CFM_NORM) ? m_linearLimits.m_normalCFM[i] : info->cfm[0]; - limot.m_stopCFM = (flags & BT_6DOF_FLAGS_CFM_STOP) ? m_linearLimits.m_stopCFM[i] : info->cfm[0]; - limot.m_stopERP = (flags & BT_6DOF_FLAGS_ERP_STOP) ? m_linearLimits.m_stopERP[i] : info->erp; - if(m_useOffsetForConstraintFrame) - { - int indx1 = (i + 1) % 3; - int indx2 = (i + 2) % 3; - int rotAllowed = 1; // rotations around orthos to current axis - if(m_angularLimits[indx1].m_currentLimit && m_angularLimits[indx2].m_currentLimit) - { - rotAllowed = 0; - } - row += get_limit_motor_info2(&limot, transA,transB,linVelA,linVelB,angVelA,angVelB, info, row, axis, 0, rotAllowed); - } - else - { - row += get_limit_motor_info2(&limot, transA,transB,linVelA,linVelB,angVelA,angVelB, info, row, axis, 0); - } - } - } - return row; -} - - - -int btGeneric6DofConstraint::setAngularLimits(btConstraintInfo2 *info, int row_offset, const btTransform& transA,const btTransform& transB,const btVector3& linVelA,const btVector3& linVelB,const btVector3& angVelA,const btVector3& angVelB) -{ - btGeneric6DofConstraint * d6constraint = this; - int row = row_offset; - //solve angular limits - for (int i=0;i<3 ;i++ ) - { - if(d6constraint->getRotationalLimitMotor(i)->needApplyTorques()) - { - btVector3 axis = d6constraint->getAxis(i); - int flags = m_flags >> ((i + 3) * BT_6DOF_FLAGS_AXIS_SHIFT); - if(!(flags & BT_6DOF_FLAGS_CFM_NORM)) - { - m_angularLimits[i].m_normalCFM = info->cfm[0]; - } - if(!(flags & BT_6DOF_FLAGS_CFM_STOP)) - { - m_angularLimits[i].m_stopCFM = info->cfm[0]; - } - if(!(flags & BT_6DOF_FLAGS_ERP_STOP)) - { - m_angularLimits[i].m_stopERP = info->erp; - } - row += get_limit_motor_info2(d6constraint->getRotationalLimitMotor(i), - transA,transB,linVelA,linVelB,angVelA,angVelB, info,row,axis,1); - } - } - - return row; -} - - - - -void btGeneric6DofConstraint::updateRHS(btScalar timeStep) -{ - (void)timeStep; - -} - - - -btVector3 btGeneric6DofConstraint::getAxis(int axis_index) const -{ - return m_calculatedAxis[axis_index]; -} - - -btScalar btGeneric6DofConstraint::getRelativePivotPosition(int axisIndex) const -{ - return m_calculatedLinearDiff[axisIndex]; -} - - -btScalar btGeneric6DofConstraint::getAngle(int axisIndex) const -{ - return m_calculatedAxisAngleDiff[axisIndex]; -} - - - -void btGeneric6DofConstraint::calcAnchorPos(void) -{ - btScalar imA = m_rbA.getInvMass(); - btScalar imB = m_rbB.getInvMass(); - btScalar weight; - if(imB == btScalar(0.0)) - { - weight = btScalar(1.0); - } - else - { - weight = imA / (imA + imB); - } - const btVector3& pA = m_calculatedTransformA.getOrigin(); - const btVector3& pB = m_calculatedTransformB.getOrigin(); - m_AnchorPos = pA * weight + pB * (btScalar(1.0) - weight); - return; -} - - - -void btGeneric6DofConstraint::calculateLinearInfo() -{ - m_calculatedLinearDiff = m_calculatedTransformB.getOrigin() - m_calculatedTransformA.getOrigin(); - m_calculatedLinearDiff = m_calculatedTransformA.getBasis().inverse() * m_calculatedLinearDiff; - for(int i = 0; i < 3; i++) - { - m_linearLimits.m_currentLinearDiff[i] = m_calculatedLinearDiff[i]; - m_linearLimits.testLimitValue(i, m_calculatedLinearDiff[i]); - } -} - - - -int btGeneric6DofConstraint::get_limit_motor_info2( - btRotationalLimitMotor * limot, - const btTransform& transA,const btTransform& transB,const btVector3& linVelA,const btVector3& linVelB,const btVector3& angVelA,const btVector3& angVelB, - btConstraintInfo2 *info, int row, btVector3& ax1, int rotational,int rotAllowed) -{ - int srow = row * info->rowskip; - int powered = limot->m_enableMotor; - int limit = limot->m_currentLimit; - if (powered || limit) - { // if the joint is powered, or has joint limits, add in the extra row - btScalar *J1 = rotational ? info->m_J1angularAxis : info->m_J1linearAxis; - btScalar *J2 = rotational ? info->m_J2angularAxis : 0; - J1[srow+0] = ax1[0]; - J1[srow+1] = ax1[1]; - J1[srow+2] = ax1[2]; - if(rotational) - { - J2[srow+0] = -ax1[0]; - J2[srow+1] = -ax1[1]; - J2[srow+2] = -ax1[2]; - } - if((!rotational)) - { - if (m_useOffsetForConstraintFrame) - { - btVector3 tmpA, tmpB, relA, relB; - // get vector from bodyB to frameB in WCS - relB = m_calculatedTransformB.getOrigin() - transB.getOrigin(); - // get its projection to constraint axis - btVector3 projB = ax1 * relB.dot(ax1); - // get vector directed from bodyB to constraint axis (and orthogonal to it) - btVector3 orthoB = relB - projB; - // same for bodyA - relA = m_calculatedTransformA.getOrigin() - transA.getOrigin(); - btVector3 projA = ax1 * relA.dot(ax1); - btVector3 orthoA = relA - projA; - // get desired offset between frames A and B along constraint axis - btScalar desiredOffs = limot->m_currentPosition - limot->m_currentLimitError; - // desired vector from projection of center of bodyA to projection of center of bodyB to constraint axis - btVector3 totalDist = projA + ax1 * desiredOffs - projB; - // get offset vectors relA and relB - relA = orthoA + totalDist * m_factA; - relB = orthoB - totalDist * m_factB; - tmpA = relA.cross(ax1); - tmpB = relB.cross(ax1); - if(m_hasStaticBody && (!rotAllowed)) - { - tmpA *= m_factA; - tmpB *= m_factB; - } - int i; - for (i=0; i<3; i++) info->m_J1angularAxis[srow+i] = tmpA[i]; - for (i=0; i<3; i++) info->m_J2angularAxis[srow+i] = -tmpB[i]; - } else - { - btVector3 ltd; // Linear Torque Decoupling vector - btVector3 c = m_calculatedTransformB.getOrigin() - transA.getOrigin(); - ltd = c.cross(ax1); - info->m_J1angularAxis[srow+0] = ltd[0]; - info->m_J1angularAxis[srow+1] = ltd[1]; - info->m_J1angularAxis[srow+2] = ltd[2]; - - c = m_calculatedTransformB.getOrigin() - transB.getOrigin(); - ltd = -c.cross(ax1); - info->m_J2angularAxis[srow+0] = ltd[0]; - info->m_J2angularAxis[srow+1] = ltd[1]; - info->m_J2angularAxis[srow+2] = ltd[2]; - } - } - // if we're limited low and high simultaneously, the joint motor is - // ineffective - if (limit && (limot->m_loLimit == limot->m_hiLimit)) powered = 0; - info->m_constraintError[srow] = btScalar(0.f); - if (powered) - { - info->cfm[srow] = limot->m_normalCFM; - if(!limit) - { - btScalar tag_vel = rotational ? limot->m_targetVelocity : -limot->m_targetVelocity; - - btScalar mot_fact = getMotorFactor( limot->m_currentPosition, - limot->m_loLimit, - limot->m_hiLimit, - tag_vel, - info->fps * limot->m_stopERP); - info->m_constraintError[srow] += mot_fact * limot->m_targetVelocity; - info->m_lowerLimit[srow] = -limot->m_maxMotorForce; - info->m_upperLimit[srow] = limot->m_maxMotorForce; - } - } - if(limit) - { - btScalar k = info->fps * limot->m_stopERP; - if(!rotational) - { - info->m_constraintError[srow] += k * limot->m_currentLimitError; - } - else - { - info->m_constraintError[srow] += -k * limot->m_currentLimitError; - } - info->cfm[srow] = limot->m_stopCFM; - if (limot->m_loLimit == limot->m_hiLimit) - { // limited low and high simultaneously - info->m_lowerLimit[srow] = -SIMD_INFINITY; - info->m_upperLimit[srow] = SIMD_INFINITY; - } - else - { - if (limit == 1) - { - info->m_lowerLimit[srow] = 0; - info->m_upperLimit[srow] = SIMD_INFINITY; - } - else - { - info->m_lowerLimit[srow] = -SIMD_INFINITY; - info->m_upperLimit[srow] = 0; - } - // deal with bounce - if (limot->m_bounce > 0) - { - // calculate joint velocity - btScalar vel; - if (rotational) - { - vel = angVelA.dot(ax1); -//make sure that if no body -> angVelB == zero vec -// if (body1) - vel -= angVelB.dot(ax1); - } - else - { - vel = linVelA.dot(ax1); -//make sure that if no body -> angVelB == zero vec -// if (body1) - vel -= linVelB.dot(ax1); - } - // only apply bounce if the velocity is incoming, and if the - // resulting c[] exceeds what we already have. - if (limit == 1) - { - if (vel < 0) - { - btScalar newc = -limot->m_bounce* vel; - if (newc > info->m_constraintError[srow]) - info->m_constraintError[srow] = newc; - } - } - else - { - if (vel > 0) - { - btScalar newc = -limot->m_bounce * vel; - if (newc < info->m_constraintError[srow]) - info->m_constraintError[srow] = newc; - } - } - } - } - } - return 1; - } - else return 0; -} - - - - - - - ///override the default global value of a parameter (such as ERP or CFM), optionally provide the axis (0..5). - ///If no axis is provided, it uses the default axis for this constraint. -void btGeneric6DofConstraint::setParam(int num, btScalar value, int axis) -{ - if((axis >= 0) && (axis < 3)) - { - switch(num) - { - case BT_CONSTRAINT_STOP_ERP : - m_linearLimits.m_stopERP[axis] = value; - m_flags |= BT_6DOF_FLAGS_ERP_STOP << (axis * BT_6DOF_FLAGS_AXIS_SHIFT); - break; - case BT_CONSTRAINT_STOP_CFM : - m_linearLimits.m_stopCFM[axis] = value; - m_flags |= BT_6DOF_FLAGS_CFM_STOP << (axis * BT_6DOF_FLAGS_AXIS_SHIFT); - break; - case BT_CONSTRAINT_CFM : - m_linearLimits.m_normalCFM[axis] = value; - m_flags |= BT_6DOF_FLAGS_CFM_NORM << (axis * BT_6DOF_FLAGS_AXIS_SHIFT); - break; - default : - btAssertConstrParams(0); - } - } - else if((axis >=3) && (axis < 6)) - { - switch(num) - { - case BT_CONSTRAINT_STOP_ERP : - m_angularLimits[axis - 3].m_stopERP = value; - m_flags |= BT_6DOF_FLAGS_ERP_STOP << (axis * BT_6DOF_FLAGS_AXIS_SHIFT); - break; - case BT_CONSTRAINT_STOP_CFM : - m_angularLimits[axis - 3].m_stopCFM = value; - m_flags |= BT_6DOF_FLAGS_CFM_STOP << (axis * BT_6DOF_FLAGS_AXIS_SHIFT); - break; - case BT_CONSTRAINT_CFM : - m_angularLimits[axis - 3].m_normalCFM = value; - m_flags |= BT_6DOF_FLAGS_CFM_NORM << (axis * BT_6DOF_FLAGS_AXIS_SHIFT); - break; - default : - btAssertConstrParams(0); - } - } - else - { - btAssertConstrParams(0); - } -} - - ///return the local value of parameter -btScalar btGeneric6DofConstraint::getParam(int num, int axis) const -{ - btScalar retVal = 0; - if((axis >= 0) && (axis < 3)) - { - switch(num) - { - case BT_CONSTRAINT_STOP_ERP : - btAssertConstrParams(m_flags & (BT_6DOF_FLAGS_ERP_STOP << (axis * BT_6DOF_FLAGS_AXIS_SHIFT))); - retVal = m_linearLimits.m_stopERP[axis]; - break; - case BT_CONSTRAINT_STOP_CFM : - btAssertConstrParams(m_flags & (BT_6DOF_FLAGS_CFM_STOP << (axis * BT_6DOF_FLAGS_AXIS_SHIFT))); - retVal = m_linearLimits.m_stopCFM[axis]; - break; - case BT_CONSTRAINT_CFM : - btAssertConstrParams(m_flags & (BT_6DOF_FLAGS_CFM_NORM << (axis * BT_6DOF_FLAGS_AXIS_SHIFT))); - retVal = m_linearLimits.m_normalCFM[axis]; - break; - default : - btAssertConstrParams(0); - } - } - else if((axis >=3) && (axis < 6)) - { - switch(num) - { - case BT_CONSTRAINT_STOP_ERP : - btAssertConstrParams(m_flags & (BT_6DOF_FLAGS_ERP_STOP << (axis * BT_6DOF_FLAGS_AXIS_SHIFT))); - retVal = m_angularLimits[axis - 3].m_stopERP; - break; - case BT_CONSTRAINT_STOP_CFM : - btAssertConstrParams(m_flags & (BT_6DOF_FLAGS_CFM_STOP << (axis * BT_6DOF_FLAGS_AXIS_SHIFT))); - retVal = m_angularLimits[axis - 3].m_stopCFM; - break; - case BT_CONSTRAINT_CFM : - btAssertConstrParams(m_flags & (BT_6DOF_FLAGS_CFM_NORM << (axis * BT_6DOF_FLAGS_AXIS_SHIFT))); - retVal = m_angularLimits[axis - 3].m_normalCFM; - break; - default : - btAssertConstrParams(0); - } - } - else - { - btAssertConstrParams(0); - } - return retVal; -} diff --git a/extern/bullet2/BulletDynamics/ConstraintSolver/btGeneric6DofConstraint.h b/extern/bullet2/BulletDynamics/ConstraintSolver/btGeneric6DofConstraint.h deleted file mode 100644 index 2653d26dc3f..00000000000 --- a/extern/bullet2/BulletDynamics/ConstraintSolver/btGeneric6DofConstraint.h +++ /dev/null @@ -1,588 +0,0 @@ -/* -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. -*/ - -/// 2009 March: btGeneric6DofConstraint refactored by Roman Ponomarev -/// Added support for generic constraint solver through getInfo1/getInfo2 methods - -/* -2007-09-09 -btGeneric6DofConstraint Refactored by Francisco Le?n -email: projectileman@yahoo.com -http://gimpact.sf.net -*/ - - -#ifndef GENERIC_6DOF_CONSTRAINT_H -#define GENERIC_6DOF_CONSTRAINT_H - -#include "LinearMath/btVector3.h" -#include "btJacobianEntry.h" -#include "btTypedConstraint.h" - -class btRigidBody; - - - - -//! Rotation Limit structure for generic joints -class btRotationalLimitMotor -{ -public: - //! limit_parameters - //!@{ - btScalar m_loLimit;//!< joint limit - btScalar m_hiLimit;//!< joint limit - btScalar m_targetVelocity;//!< target motor velocity - btScalar m_maxMotorForce;//!< max force on motor - btScalar m_maxLimitForce;//!< max force on limit - btScalar m_damping;//!< Damping. - btScalar m_limitSoftness;//! Relaxation factor - btScalar m_normalCFM;//!< Constraint force mixing factor - btScalar m_stopERP;//!< Error tolerance factor when joint is at limit - btScalar m_stopCFM;//!< Constraint force mixing factor when joint is at limit - btScalar m_bounce;//!< restitution factor - bool m_enableMotor; - - //!@} - - //! temp_variables - //!@{ - btScalar m_currentLimitError;//! How much is violated this limit - btScalar m_currentPosition; //! current value of angle - int m_currentLimit;//!< 0=free, 1=at lo limit, 2=at hi limit - btScalar m_accumulatedImpulse; - //!@} - - btRotationalLimitMotor() - { - m_accumulatedImpulse = 0.f; - m_targetVelocity = 0; - m_maxMotorForce = 0.1f; - m_maxLimitForce = 300.0f; - m_loLimit = 1.0f; - m_hiLimit = -1.0f; - m_normalCFM = 0.f; - m_stopERP = 0.2f; - m_stopCFM = 0.f; - m_bounce = 0.0f; - m_damping = 1.0f; - m_limitSoftness = 0.5f; - m_currentLimit = 0; - m_currentLimitError = 0; - m_enableMotor = false; - } - - btRotationalLimitMotor(const btRotationalLimitMotor & limot) - { - m_targetVelocity = limot.m_targetVelocity; - m_maxMotorForce = limot.m_maxMotorForce; - m_limitSoftness = limot.m_limitSoftness; - m_loLimit = limot.m_loLimit; - m_hiLimit = limot.m_hiLimit; - m_normalCFM = limot.m_normalCFM; - m_stopERP = limot.m_stopERP; - m_stopCFM = limot.m_stopCFM; - m_bounce = limot.m_bounce; - m_currentLimit = limot.m_currentLimit; - m_currentLimitError = limot.m_currentLimitError; - m_enableMotor = limot.m_enableMotor; - } - - - - //! Is limited - bool isLimited() - { - if(m_loLimit > m_hiLimit) return false; - return true; - } - - //! Need apply correction - bool needApplyTorques() - { - if(m_currentLimit == 0 && m_enableMotor == false) return false; - return true; - } - - //! calculates error - /*! - calculates m_currentLimit and m_currentLimitError. - */ - int testLimitValue(btScalar test_value); - - //! apply the correction impulses for two bodies - btScalar solveAngularLimits(btScalar timeStep,btVector3& axis, btScalar jacDiagABInv,btRigidBody * body0, btRigidBody * body1); - -}; - - - -class btTranslationalLimitMotor -{ -public: - btVector3 m_lowerLimit;//!< the constraint lower limits - btVector3 m_upperLimit;//!< the constraint upper limits - btVector3 m_accumulatedImpulse; - //! Linear_Limit_parameters - //!@{ - btScalar m_limitSoftness;//!< Softness for linear limit - btScalar m_damping;//!< Damping for linear limit - btScalar m_restitution;//! Bounce parameter for linear limit - btVector3 m_normalCFM;//!< Constraint force mixing factor - btVector3 m_stopERP;//!< Error tolerance factor when joint is at limit - btVector3 m_stopCFM;//!< Constraint force mixing factor when joint is at limit - //!@} - bool m_enableMotor[3]; - btVector3 m_targetVelocity;//!< target motor velocity - btVector3 m_maxMotorForce;//!< max force on motor - btVector3 m_currentLimitError;//! How much is violated this limit - btVector3 m_currentLinearDiff;//! Current relative offset of constraint frames - int m_currentLimit[3];//!< 0=free, 1=at lower limit, 2=at upper limit - - btTranslationalLimitMotor() - { - m_lowerLimit.setValue(0.f,0.f,0.f); - m_upperLimit.setValue(0.f,0.f,0.f); - m_accumulatedImpulse.setValue(0.f,0.f,0.f); - m_normalCFM.setValue(0.f, 0.f, 0.f); - m_stopERP.setValue(0.2f, 0.2f, 0.2f); - m_stopCFM.setValue(0.f, 0.f, 0.f); - - m_limitSoftness = 0.7f; - m_damping = btScalar(1.0f); - m_restitution = btScalar(0.5f); - for(int i=0; i < 3; i++) - { - m_enableMotor[i] = false; - m_targetVelocity[i] = btScalar(0.f); - m_maxMotorForce[i] = btScalar(0.f); - } - } - - btTranslationalLimitMotor(const btTranslationalLimitMotor & other ) - { - m_lowerLimit = other.m_lowerLimit; - m_upperLimit = other.m_upperLimit; - m_accumulatedImpulse = other.m_accumulatedImpulse; - - m_limitSoftness = other.m_limitSoftness ; - m_damping = other.m_damping; - m_restitution = other.m_restitution; - m_normalCFM = other.m_normalCFM; - m_stopERP = other.m_stopERP; - m_stopCFM = other.m_stopCFM; - - for(int i=0; i < 3; i++) - { - m_enableMotor[i] = other.m_enableMotor[i]; - m_targetVelocity[i] = other.m_targetVelocity[i]; - m_maxMotorForce[i] = other.m_maxMotorForce[i]; - } - } - - //! Test limit - /*! - - free means upper < lower, - - locked means upper == lower - - limited means upper > lower - - limitIndex: first 3 are linear, next 3 are angular - */ - inline bool isLimited(int limitIndex) - { - return (m_upperLimit[limitIndex] >= m_lowerLimit[limitIndex]); - } - inline bool needApplyForce(int limitIndex) - { - if(m_currentLimit[limitIndex] == 0 && m_enableMotor[limitIndex] == false) return false; - return true; - } - int testLimitValue(int limitIndex, btScalar test_value); - - - btScalar solveLinearAxis( - btScalar timeStep, - btScalar jacDiagABInv, - btRigidBody& body1,const btVector3 &pointInA, - btRigidBody& body2,const btVector3 &pointInB, - int limit_index, - const btVector3 & axis_normal_on_a, - const btVector3 & anchorPos); - - -}; - -enum bt6DofFlags -{ - BT_6DOF_FLAGS_CFM_NORM = 1, - BT_6DOF_FLAGS_CFM_STOP = 2, - BT_6DOF_FLAGS_ERP_STOP = 4 -}; -#define BT_6DOF_FLAGS_AXIS_SHIFT 3 // bits per axis - - -/// btGeneric6DofConstraint between two rigidbodies each with a pivotpoint that descibes the axis location in local space -/*! -btGeneric6DofConstraint can leave any of the 6 degree of freedom 'free' or 'locked'. -currently this limit supports rotational motors<br> -<ul> -<li> For Linear limits, use btGeneric6DofConstraint.setLinearUpperLimit, btGeneric6DofConstraint.setLinearLowerLimit. You can set the parameters with the btTranslationalLimitMotor structure accsesible through the btGeneric6DofConstraint.getTranslationalLimitMotor method. -At this moment translational motors are not supported. May be in the future. </li> - -<li> For Angular limits, use the btRotationalLimitMotor structure for configuring the limit. -This is accessible through btGeneric6DofConstraint.getLimitMotor method, -This brings support for limit parameters and motors. </li> - -<li> Angulars limits have these possible ranges: -<table border=1 > -<tr> - <td><b>AXIS</b></td> - <td><b>MIN ANGLE</b></td> - <td><b>MAX ANGLE</b></td> -</tr><tr> - <td>X</td> - <td>-PI</td> - <td>PI</td> -</tr><tr> - <td>Y</td> - <td>-PI/2</td> - <td>PI/2</td> -</tr><tr> - <td>Z</td> - <td>-PI</td> - <td>PI</td> -</tr> -</table> -</li> -</ul> - -*/ -class btGeneric6DofConstraint : public btTypedConstraint -{ -protected: - - //! relative_frames - //!@{ - btTransform m_frameInA;//!< the constraint space w.r.t body A - btTransform m_frameInB;//!< the constraint space w.r.t body B - //!@} - - //! Jacobians - //!@{ - btJacobianEntry m_jacLinear[3];//!< 3 orthogonal linear constraints - btJacobianEntry m_jacAng[3];//!< 3 orthogonal angular constraints - //!@} - - //! Linear_Limit_parameters - //!@{ - btTranslationalLimitMotor m_linearLimits; - //!@} - - - //! hinge_parameters - //!@{ - btRotationalLimitMotor m_angularLimits[3]; - //!@} - - -protected: - //! temporal variables - //!@{ - btScalar m_timeStep; - btTransform m_calculatedTransformA; - btTransform m_calculatedTransformB; - btVector3 m_calculatedAxisAngleDiff; - btVector3 m_calculatedAxis[3]; - btVector3 m_calculatedLinearDiff; - btScalar m_factA; - btScalar m_factB; - bool m_hasStaticBody; - - btVector3 m_AnchorPos; // point betwen pivots of bodies A and B to solve linear axes - - bool m_useLinearReferenceFrameA; - bool m_useOffsetForConstraintFrame; - - int m_flags; - - //!@} - - btGeneric6DofConstraint& operator=(btGeneric6DofConstraint& other) - { - btAssert(0); - (void) other; - return *this; - } - - - int setAngularLimits(btConstraintInfo2 *info, int row_offset,const btTransform& transA,const btTransform& transB,const btVector3& linVelA,const btVector3& linVelB,const btVector3& angVelA,const btVector3& angVelB); - - int setLinearLimits(btConstraintInfo2 *info, int row, const btTransform& transA,const btTransform& transB,const btVector3& linVelA,const btVector3& linVelB,const btVector3& angVelA,const btVector3& angVelB); - - void buildLinearJacobian( - btJacobianEntry & jacLinear,const btVector3 & normalWorld, - const btVector3 & pivotAInW,const btVector3 & pivotBInW); - - void buildAngularJacobian(btJacobianEntry & jacAngular,const btVector3 & jointAxisW); - - // tests linear limits - void calculateLinearInfo(); - - //! calcs the euler angles between the two bodies. - void calculateAngleInfo(); - - - -public: - - ///for backwards compatibility during the transition to 'getInfo/getInfo2' - bool m_useSolveConstraintObsolete; - - btGeneric6DofConstraint(btRigidBody& rbA, btRigidBody& rbB, const btTransform& frameInA, const btTransform& frameInB ,bool useLinearReferenceFrameA); - btGeneric6DofConstraint(btRigidBody& rbB, const btTransform& frameInB, bool useLinearReferenceFrameB); - - //! Calcs global transform of the offsets - /*! - Calcs the global transform for the joint offset for body A an B, and also calcs the agle differences between the bodies. - \sa btGeneric6DofConstraint.getCalculatedTransformA , btGeneric6DofConstraint.getCalculatedTransformB, btGeneric6DofConstraint.calculateAngleInfo - */ - void calculateTransforms(const btTransform& transA,const btTransform& transB); - - void calculateTransforms(); - - //! Gets the global transform of the offset for body A - /*! - \sa btGeneric6DofConstraint.getFrameOffsetA, btGeneric6DofConstraint.getFrameOffsetB, btGeneric6DofConstraint.calculateAngleInfo. - */ - const btTransform & getCalculatedTransformA() const - { - return m_calculatedTransformA; - } - - //! Gets the global transform of the offset for body B - /*! - \sa btGeneric6DofConstraint.getFrameOffsetA, btGeneric6DofConstraint.getFrameOffsetB, btGeneric6DofConstraint.calculateAngleInfo. - */ - const btTransform & getCalculatedTransformB() const - { - return m_calculatedTransformB; - } - - const btTransform & getFrameOffsetA() const - { - return m_frameInA; - } - - const btTransform & getFrameOffsetB() const - { - return m_frameInB; - } - - - btTransform & getFrameOffsetA() - { - return m_frameInA; - } - - btTransform & getFrameOffsetB() - { - return m_frameInB; - } - - - //! performs Jacobian calculation, and also calculates angle differences and axis - virtual void buildJacobian(); - - virtual void getInfo1 (btConstraintInfo1* info); - - void getInfo1NonVirtual (btConstraintInfo1* info); - - virtual void getInfo2 (btConstraintInfo2* info); - - void getInfo2NonVirtual (btConstraintInfo2* info,const btTransform& transA,const btTransform& transB,const btVector3& linVelA,const btVector3& linVelB,const btVector3& angVelA,const btVector3& angVelB); - - - void updateRHS(btScalar timeStep); - - //! Get the rotation axis in global coordinates - /*! - \pre btGeneric6DofConstraint.buildJacobian must be called previously. - */ - btVector3 getAxis(int axis_index) const; - - //! Get the relative Euler angle - /*! - \pre btGeneric6DofConstraint::calculateTransforms() must be called previously. - */ - btScalar getAngle(int axis_index) const; - - //! Get the relative position of the constraint pivot - /*! - \pre btGeneric6DofConstraint::calculateTransforms() must be called previously. - */ - btScalar getRelativePivotPosition(int axis_index) const; - - - //! Test angular limit. - /*! - Calculates angular correction and returns true if limit needs to be corrected. - \pre btGeneric6DofConstraint::calculateTransforms() must be called previously. - */ - bool testAngularLimitMotor(int axis_index); - - void setLinearLowerLimit(const btVector3& linearLower) - { - m_linearLimits.m_lowerLimit = linearLower; - } - - void setLinearUpperLimit(const btVector3& linearUpper) - { - m_linearLimits.m_upperLimit = linearUpper; - } - - void setAngularLowerLimit(const btVector3& angularLower) - { - for(int i = 0; i < 3; i++) - m_angularLimits[i].m_loLimit = btNormalizeAngle(angularLower[i]); - } - - void setAngularUpperLimit(const btVector3& angularUpper) - { - for(int i = 0; i < 3; i++) - m_angularLimits[i].m_hiLimit = btNormalizeAngle(angularUpper[i]); - } - - //! Retrieves the angular limit informacion - btRotationalLimitMotor * getRotationalLimitMotor(int index) - { - return &m_angularLimits[index]; - } - - //! Retrieves the limit informacion - btTranslationalLimitMotor * getTranslationalLimitMotor() - { - return &m_linearLimits; - } - - //first 3 are linear, next 3 are angular - void setLimit(int axis, btScalar lo, btScalar hi) - { - if(axis<3) - { - m_linearLimits.m_lowerLimit[axis] = lo; - m_linearLimits.m_upperLimit[axis] = hi; - } - else - { - lo = btNormalizeAngle(lo); - hi = btNormalizeAngle(hi); - m_angularLimits[axis-3].m_loLimit = lo; - m_angularLimits[axis-3].m_hiLimit = hi; - } - } - - //! Test limit - /*! - - free means upper < lower, - - locked means upper == lower - - limited means upper > lower - - limitIndex: first 3 are linear, next 3 are angular - */ - bool isLimited(int limitIndex) - { - if(limitIndex<3) - { - return m_linearLimits.isLimited(limitIndex); - - } - return m_angularLimits[limitIndex-3].isLimited(); - } - - virtual void calcAnchorPos(void); // overridable - - int get_limit_motor_info2( btRotationalLimitMotor * limot, - const btTransform& transA,const btTransform& transB,const btVector3& linVelA,const btVector3& linVelB,const btVector3& angVelA,const btVector3& angVelB, - btConstraintInfo2 *info, int row, btVector3& ax1, int rotational, int rotAllowed = false); - - // access for UseFrameOffset - bool getUseFrameOffset() { return m_useOffsetForConstraintFrame; } - void setUseFrameOffset(bool frameOffsetOnOff) { m_useOffsetForConstraintFrame = frameOffsetOnOff; } - - ///override the default global value of a parameter (such as ERP or CFM), optionally provide the axis (0..5). - ///If no axis is provided, it uses the default axis for this constraint. - virtual void setParam(int num, btScalar value, int axis = -1); - ///return the local value of parameter - virtual btScalar getParam(int num, int axis = -1) const; - - virtual int calculateSerializeBufferSize() const; - - ///fills the dataBuffer and returns the struct name (and 0 on failure) - virtual const char* serialize(void* dataBuffer, btSerializer* serializer) const; - - -}; - -///do not change those serialization structures, it requires an updated sBulletDNAstr/sBulletDNAstr64 -struct btGeneric6DofConstraintData -{ - btTypedConstraintData m_typeConstraintData; - btTransformFloatData m_rbAFrame; // constraint axii. Assumes z is hinge axis. - btTransformFloatData m_rbBFrame; - - btVector3FloatData m_linearUpperLimit; - btVector3FloatData m_linearLowerLimit; - - btVector3FloatData m_angularUpperLimit; - btVector3FloatData m_angularLowerLimit; - - int m_useLinearReferenceFrameA; - int m_useOffsetForConstraintFrame; -}; - -SIMD_FORCE_INLINE int btGeneric6DofConstraint::calculateSerializeBufferSize() const -{ - return sizeof(btGeneric6DofConstraintData); -} - - ///fills the dataBuffer and returns the struct name (and 0 on failure) -SIMD_FORCE_INLINE const char* btGeneric6DofConstraint::serialize(void* dataBuffer, btSerializer* serializer) const -{ - - btGeneric6DofConstraintData* dof = (btGeneric6DofConstraintData*)dataBuffer; - btTypedConstraint::serialize(&dof->m_typeConstraintData,serializer); - - m_frameInA.serializeFloat(dof->m_rbAFrame); - m_frameInB.serializeFloat(dof->m_rbBFrame); - - - int i; - for (i=0;i<3;i++) - { - dof->m_angularLowerLimit.m_floats[i] = float(m_angularLimits[i].m_loLimit); - dof->m_angularUpperLimit.m_floats[i] = float(m_angularLimits[i].m_hiLimit); - dof->m_linearLowerLimit.m_floats[i] = float(m_linearLimits.m_lowerLimit[i]); - dof->m_linearUpperLimit.m_floats[i] = float(m_linearLimits.m_upperLimit[i]); - } - - dof->m_useLinearReferenceFrameA = m_useLinearReferenceFrameA? 1 : 0; - dof->m_useOffsetForConstraintFrame = m_useOffsetForConstraintFrame ? 1 : 0; - - return "btGeneric6DofConstraintData"; -} - - - - - -#endif //GENERIC_6DOF_CONSTRAINT_H diff --git a/extern/bullet2/BulletDynamics/ConstraintSolver/btGeneric6DofSpringConstraint.cpp b/extern/bullet2/BulletDynamics/ConstraintSolver/btGeneric6DofSpringConstraint.cpp deleted file mode 100644 index 3fa7de4ddb8..00000000000 --- a/extern/bullet2/BulletDynamics/ConstraintSolver/btGeneric6DofSpringConstraint.cpp +++ /dev/null @@ -1,146 +0,0 @@ -/* -Bullet Continuous Collision Detection and Physics Library, http://bulletphysics.org -Copyright (C) 2006, 2007 Sony Computer Entertainment Inc. - -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 "btGeneric6DofSpringConstraint.h" -#include "BulletDynamics/Dynamics/btRigidBody.h" -#include "LinearMath/btTransformUtil.h" - - -btGeneric6DofSpringConstraint::btGeneric6DofSpringConstraint(btRigidBody& rbA, btRigidBody& rbB, const btTransform& frameInA, const btTransform& frameInB ,bool useLinearReferenceFrameA) - : btGeneric6DofConstraint(rbA, rbB, frameInA, frameInB, useLinearReferenceFrameA) -{ - for(int i = 0; i < 6; i++) - { - m_springEnabled[i] = false; - m_equilibriumPoint[i] = btScalar(0.f); - m_springStiffness[i] = btScalar(0.f); - m_springDamping[i] = btScalar(1.f); - } -} - - -void btGeneric6DofSpringConstraint::enableSpring(int index, bool onOff) -{ - btAssert((index >= 0) && (index < 6)); - m_springEnabled[index] = onOff; - if(index < 3) - { - m_linearLimits.m_enableMotor[index] = onOff; - } - else - { - m_angularLimits[index - 3].m_enableMotor = onOff; - } -} - - - -void btGeneric6DofSpringConstraint::setStiffness(int index, btScalar stiffness) -{ - btAssert((index >= 0) && (index < 6)); - m_springStiffness[index] = stiffness; -} - - -void btGeneric6DofSpringConstraint::setDamping(int index, btScalar damping) -{ - btAssert((index >= 0) && (index < 6)); - m_springDamping[index] = damping; -} - - -void btGeneric6DofSpringConstraint::setEquilibriumPoint() -{ - calculateTransforms(); - int i; - - for( i = 0; i < 3; i++) - { - m_equilibriumPoint[i] = m_calculatedLinearDiff[i]; - } - for(i = 0; i < 3; i++) - { - m_equilibriumPoint[i + 3] = m_calculatedAxisAngleDiff[i]; - } -} - - - -void btGeneric6DofSpringConstraint::setEquilibriumPoint(int index) -{ - btAssert((index >= 0) && (index < 6)); - calculateTransforms(); - if(index < 3) - { - m_equilibriumPoint[index] = m_calculatedLinearDiff[index]; - } - else - { - m_equilibriumPoint[index] = m_calculatedAxisAngleDiff[index - 3]; - } -} - - - -void btGeneric6DofSpringConstraint::internalUpdateSprings(btConstraintInfo2* info) -{ - // it is assumed that calculateTransforms() have been called before this call - int i; - btVector3 relVel = m_rbB.getLinearVelocity() - m_rbA.getLinearVelocity(); - for(i = 0; i < 3; i++) - { - if(m_springEnabled[i]) - { - // get current position of constraint - btScalar currPos = m_calculatedLinearDiff[i]; - // calculate difference - btScalar delta = currPos - m_equilibriumPoint[i]; - // spring force is (delta * m_stiffness) according to Hooke's Law - btScalar force = delta * m_springStiffness[i]; - btScalar velFactor = info->fps * m_springDamping[i] / btScalar(info->m_numIterations); - m_linearLimits.m_targetVelocity[i] = velFactor * force; - m_linearLimits.m_maxMotorForce[i] = btFabs(force) / info->fps; - } - } - for(i = 0; i < 3; i++) - { - if(m_springEnabled[i + 3]) - { - // get current position of constraint - btScalar currPos = m_calculatedAxisAngleDiff[i]; - // calculate difference - btScalar delta = currPos - m_equilibriumPoint[i+3]; - // spring force is (-delta * m_stiffness) according to Hooke's Law - btScalar force = -delta * m_springStiffness[i+3]; - btScalar velFactor = info->fps * m_springDamping[i+3] / btScalar(info->m_numIterations); - m_angularLimits[i].m_targetVelocity = velFactor * force; - m_angularLimits[i].m_maxMotorForce = btFabs(force) / info->fps; - } - } -} - - -void btGeneric6DofSpringConstraint::getInfo2(btConstraintInfo2* info) -{ - // this will be called by constraint solver at the constraint setup stage - // set current motor parameters - internalUpdateSprings(info); - // do the rest of job for constraint setup - btGeneric6DofConstraint::getInfo2(info); -} - - - - diff --git a/extern/bullet2/BulletDynamics/ConstraintSolver/btGeneric6DofSpringConstraint.h b/extern/bullet2/BulletDynamics/ConstraintSolver/btGeneric6DofSpringConstraint.h deleted file mode 100644 index e0c1fc9ae39..00000000000 --- a/extern/bullet2/BulletDynamics/ConstraintSolver/btGeneric6DofSpringConstraint.h +++ /dev/null @@ -1,54 +0,0 @@ -/* -Bullet Continuous Collision Detection and Physics Library, http://bulletphysics.org -Copyright (C) 2006, 2007 Sony Computer Entertainment Inc. - -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. -*/ - -#ifndef GENERIC_6DOF_SPRING_CONSTRAINT_H -#define GENERIC_6DOF_SPRING_CONSTRAINT_H - - -#include "LinearMath/btVector3.h" -#include "btTypedConstraint.h" -#include "btGeneric6DofConstraint.h" - - -/// Generic 6 DOF constraint that allows to set spring motors to any translational and rotational DOF - -/// DOF index used in enableSpring() and setStiffness() means: -/// 0 : translation X -/// 1 : translation Y -/// 2 : translation Z -/// 3 : rotation X (3rd Euler rotational around new position of X axis, range [-PI+epsilon, PI-epsilon] ) -/// 4 : rotation Y (2nd Euler rotational around new position of Y axis, range [-PI/2+epsilon, PI/2-epsilon] ) -/// 5 : rotation Z (1st Euler rotational around Z axis, range [-PI+epsilon, PI-epsilon] ) - -class btGeneric6DofSpringConstraint : public btGeneric6DofConstraint -{ -protected: - bool m_springEnabled[6]; - btScalar m_equilibriumPoint[6]; - btScalar m_springStiffness[6]; - btScalar m_springDamping[6]; // between 0 and 1 (1 == no damping) - void internalUpdateSprings(btConstraintInfo2* info); -public: - btGeneric6DofSpringConstraint(btRigidBody& rbA, btRigidBody& rbB, const btTransform& frameInA, const btTransform& frameInB ,bool useLinearReferenceFrameA); - void enableSpring(int index, bool onOff); - void setStiffness(int index, btScalar stiffness); - void setDamping(int index, btScalar damping); - void setEquilibriumPoint(); // set the current constraint position/orientation as an equilibrium point for all DOF - void setEquilibriumPoint(int index); // set the current constraint position/orientation as an equilibrium point for given DOF - virtual void getInfo2 (btConstraintInfo2* info); -}; - -#endif // GENERIC_6DOF_SPRING_CONSTRAINT_H - diff --git a/extern/bullet2/BulletDynamics/ConstraintSolver/btHinge2Constraint.cpp b/extern/bullet2/BulletDynamics/ConstraintSolver/btHinge2Constraint.cpp deleted file mode 100644 index 29123d526b4..00000000000 --- a/extern/bullet2/BulletDynamics/ConstraintSolver/btHinge2Constraint.cpp +++ /dev/null @@ -1,66 +0,0 @@ -/* -Bullet Continuous Collision Detection and Physics Library, http://bulletphysics.org -Copyright (C) 2006, 2007 Sony Computer Entertainment Inc. - -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 "btHinge2Constraint.h" -#include "BulletDynamics/Dynamics/btRigidBody.h" -#include "LinearMath/btTransformUtil.h" - - - -// constructor -// anchor, axis1 and axis2 are in world coordinate system -// axis1 must be orthogonal to axis2 -btHinge2Constraint::btHinge2Constraint(btRigidBody& rbA, btRigidBody& rbB, btVector3& anchor, btVector3& axis1, btVector3& axis2) -: btGeneric6DofSpringConstraint(rbA, rbB, btTransform::getIdentity(), btTransform::getIdentity(), true), - m_anchor(anchor), - m_axis1(axis1), - m_axis2(axis2) -{ - // build frame basis - // 6DOF constraint uses Euler angles and to define limits - // it is assumed that rotational order is : - // Z - first, allowed limits are (-PI,PI); - // new position of Y - second (allowed limits are (-PI/2 + epsilon, PI/2 - epsilon), where epsilon is a small positive number - // used to prevent constraint from instability on poles; - // new position of X, allowed limits are (-PI,PI); - // So to simulate ODE Universal joint we should use parent axis as Z, child axis as Y and limit all other DOFs - // Build the frame in world coordinate system first - btVector3 zAxis = axis1.normalize(); - btVector3 xAxis = axis2.normalize(); - btVector3 yAxis = zAxis.cross(xAxis); // we want right coordinate system - btTransform frameInW; - frameInW.setIdentity(); - frameInW.getBasis().setValue( xAxis[0], yAxis[0], zAxis[0], - xAxis[1], yAxis[1], zAxis[1], - xAxis[2], yAxis[2], zAxis[2]); - frameInW.setOrigin(anchor); - // now get constraint frame in local coordinate systems - m_frameInA = rbA.getCenterOfMassTransform().inverse() * frameInW; - m_frameInB = rbB.getCenterOfMassTransform().inverse() * frameInW; - // sei limits - setLinearLowerLimit(btVector3(0.f, 0.f, -1.f)); - setLinearUpperLimit(btVector3(0.f, 0.f, 1.f)); - // like front wheels of a car - setAngularLowerLimit(btVector3(1.f, 0.f, -SIMD_HALF_PI * 0.5f)); - setAngularUpperLimit(btVector3(-1.f, 0.f, SIMD_HALF_PI * 0.5f)); - // enable suspension - enableSpring(2, true); - setStiffness(2, SIMD_PI * SIMD_PI * 4.f); // period 1 sec for 1 kilogramm weel :-) - setDamping(2, 0.01f); - setEquilibriumPoint(); -} - diff --git a/extern/bullet2/BulletDynamics/ConstraintSolver/btHinge2Constraint.h b/extern/bullet2/BulletDynamics/ConstraintSolver/btHinge2Constraint.h deleted file mode 100644 index 15fd4a014cc..00000000000 --- a/extern/bullet2/BulletDynamics/ConstraintSolver/btHinge2Constraint.h +++ /dev/null @@ -1,58 +0,0 @@ -/* -Bullet Continuous Collision Detection and Physics Library, http://bulletphysics.org -Copyright (C) 2006, 2007 Sony Computer Entertainment Inc. - -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. -*/ - -#ifndef HINGE2_CONSTRAINT_H -#define HINGE2_CONSTRAINT_H - - - -#include "LinearMath/btVector3.h" -#include "btTypedConstraint.h" -#include "btGeneric6DofSpringConstraint.h" - - - -// Constraint similar to ODE Hinge2 Joint -// has 3 degrees of frredom: -// 2 rotational degrees of freedom, similar to Euler rotations around Z (axis 1) and X (axis 2) -// 1 translational (along axis Z) with suspension spring - -class btHinge2Constraint : public btGeneric6DofSpringConstraint -{ -protected: - btVector3 m_anchor; - btVector3 m_axis1; - btVector3 m_axis2; -public: - // constructor - // anchor, axis1 and axis2 are in world coordinate system - // axis1 must be orthogonal to axis2 - btHinge2Constraint(btRigidBody& rbA, btRigidBody& rbB, btVector3& anchor, btVector3& axis1, btVector3& axis2); - // access - const btVector3& getAnchor() { return m_calculatedTransformA.getOrigin(); } - const btVector3& getAnchor2() { return m_calculatedTransformB.getOrigin(); } - const btVector3& getAxis1() { return m_axis1; } - const btVector3& getAxis2() { return m_axis2; } - btScalar getAngle1() { return getAngle(2); } - btScalar getAngle2() { return getAngle(0); } - // limits - void setUpperLimit(btScalar ang1max) { setAngularUpperLimit(btVector3(-1.f, 0.f, ang1max)); } - void setLowerLimit(btScalar ang1min) { setAngularLowerLimit(btVector3( 1.f, 0.f, ang1min)); } -}; - - - -#endif // HINGE2_CONSTRAINT_H - diff --git a/extern/bullet2/BulletDynamics/ConstraintSolver/btHingeConstraint.cpp b/extern/bullet2/BulletDynamics/ConstraintSolver/btHingeConstraint.cpp deleted file mode 100644 index 2bfa55a2933..00000000000 --- a/extern/bullet2/BulletDynamics/ConstraintSolver/btHingeConstraint.cpp +++ /dev/null @@ -1,992 +0,0 @@ -/* -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 "btHingeConstraint.h" -#include "BulletDynamics/Dynamics/btRigidBody.h" -#include "LinearMath/btTransformUtil.h" -#include "LinearMath/btMinMax.h" -#include <new> -#include "btSolverBody.h" - - - -//#define HINGE_USE_OBSOLETE_SOLVER false -#define HINGE_USE_OBSOLETE_SOLVER false - -#define HINGE_USE_FRAME_OFFSET true - -#ifndef __SPU__ - - - - - -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_useOffsetForConstraintFrame(HINGE_USE_FRAME_OFFSET), - m_useReferenceFrameA(useReferenceFrameA), - m_flags(0) -{ - 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_useOffsetForConstraintFrame(HINGE_USE_FRAME_OFFSET), -m_useReferenceFrameA(useReferenceFrameA), -m_flags(0) -{ - - // 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_useOffsetForConstraintFrame(HINGE_USE_FRAME_OFFSET), -m_useReferenceFrameA(useReferenceFrameA), -m_flags(0) -{ - //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_useOffsetForConstraintFrame(HINGE_USE_FRAME_OFFSET), -m_useReferenceFrameA(useReferenceFrameA), -m_flags(0) -{ - ///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); - - 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)); - - } -} - - -#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) -{ - if(m_useOffsetForConstraintFrame) - { - getInfo2InternalUsingFrameOffset(info, m_rbA.getCenterOfMassTransform(),m_rbB.getCenterOfMassTransform(),m_rbA.getAngularVelocity(),m_rbB.getAngularVelocity()); - } - else - { - 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); - btScalar currERP = (m_flags & BT_HINGE_FLAGS_ERP_STOP) ? m_stopERP : info->erp; - if(powered) - { - if(m_flags & BT_HINGE_FLAGS_CFM_NORM) - { - info->cfm[srow] = m_normalCFM; - } - btScalar mot_fact = getMotorFactor(m_hingeAngle, lostop, histop, m_motorTargetVelocity, info->fps * currERP); - 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 * currERP; - info->m_constraintError[srow] += k * limit_err; - if(m_flags & BT_HINGE_FLAGS_CFM_STOP) - { - info->cfm[srow] = m_stopCFM; - } - 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)); - btScalar angle = btAtan2(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; -} - - - -void btHingeConstraint::getInfo2InternalUsingFrameOffset(btConstraintInfo2* info, const btTransform& transA,const btTransform& transB,const btVector3& angVelA,const btVector3& angVelB) -{ - btAssert(!m_useSolveConstraintObsolete); - int i, s = 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 1 - // difference between frames in WCS - btVector3 ofs = trB.getOrigin() - trA.getOrigin(); - // now get weight factors depending on masses - btScalar miA = getRigidBodyA().getInvMass(); - btScalar miB = getRigidBodyB().getInvMass(); - bool hasStaticBody = (miA < SIMD_EPSILON) || (miB < SIMD_EPSILON); - btScalar miS = miA + miB; - btScalar factA, factB; - if(miS > btScalar(0.f)) - { - factA = miB / miS; - } - else - { - factA = btScalar(0.5f); - } - factB = btScalar(1.0f) - factA; - // get the desired direction of hinge axis - // as weighted sum of Z-orthos of frameA and frameB in WCS - btVector3 ax1A = trA.getBasis().getColumn(2); - btVector3 ax1B = trB.getBasis().getColumn(2); - btVector3 ax1 = ax1A * factA + ax1B * factB; - ax1.normalize(); - // fill first 3 rows - // we want: velA + wA x relA == velB + wB x relB - btTransform bodyA_trans = transA; - btTransform bodyB_trans = transB; - int s0 = 0; - int s1 = s; - int s2 = s * 2; - int nrow = 2; // last filled row - btVector3 tmpA, tmpB, relA, relB, p, q; - // get vector from bodyB to frameB in WCS - relB = trB.getOrigin() - bodyB_trans.getOrigin(); - // get its projection to hinge axis - btVector3 projB = ax1 * relB.dot(ax1); - // get vector directed from bodyB to hinge axis (and orthogonal to it) - btVector3 orthoB = relB - projB; - // same for bodyA - relA = trA.getOrigin() - bodyA_trans.getOrigin(); - btVector3 projA = ax1 * relA.dot(ax1); - btVector3 orthoA = relA - projA; - btVector3 totalDist = projA - projB; - // get offset vectors relA and relB - relA = orthoA + totalDist * factA; - relB = orthoB - totalDist * factB; - // now choose average ortho to hinge axis - p = orthoB * factA + orthoA * factB; - btScalar len2 = p.length2(); - if(len2 > SIMD_EPSILON) - { - p /= btSqrt(len2); - } - else - { - p = trA.getBasis().getColumn(1); - } - // make one more ortho - q = ax1.cross(p); - // fill three rows - tmpA = relA.cross(p); - tmpB = relB.cross(p); - for (i=0; i<3; i++) info->m_J1angularAxis[s0+i] = tmpA[i]; - for (i=0; i<3; i++) info->m_J2angularAxis[s0+i] = -tmpB[i]; - tmpA = relA.cross(q); - tmpB = relB.cross(q); - if(hasStaticBody && getSolveLimit()) - { // to make constraint between static and dynamic objects more rigid - // remove wA (or wB) from equation if angular limit is hit - tmpB *= factB; - tmpA *= factA; - } - for (i=0; i<3; i++) info->m_J1angularAxis[s1+i] = tmpA[i]; - for (i=0; i<3; i++) info->m_J2angularAxis[s1+i] = -tmpB[i]; - tmpA = relA.cross(ax1); - tmpB = relB.cross(ax1); - if(hasStaticBody) - { // to make constraint between static and dynamic objects more rigid - // remove wA (or wB) from equation - tmpB *= factB; - tmpA *= factA; - } - for (i=0; i<3; i++) info->m_J1angularAxis[s2+i] = tmpA[i]; - for (i=0; i<3; i++) info->m_J2angularAxis[s2+i] = -tmpB[i]; - - for (i=0; i<3; i++) info->m_J1linearAxis[s0+i] = p[i]; - for (i=0; i<3; i++) info->m_J1linearAxis[s1+i] = q[i]; - for (i=0; i<3; i++) info->m_J1linearAxis[s2+i] = ax1[i]; - // compute three elements of right hand side - btScalar k = info->fps * info->erp; - btScalar rhs = k * p.dot(ofs); - info->m_constraintError[s0] = rhs; - rhs = k * q.dot(ofs); - info->m_constraintError[s1] = rhs; - rhs = k * ax1.dot(ofs); - info->m_constraintError[s2] = rhs; - // 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. - int s3 = 3 * s; - int s4 = 4 * s; - 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 ax1A,ax1B are the unit length hinge axes as computed from bodyA and - // bodyB, 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. - k = info->fps * info->erp; - btVector3 u = ax1A.cross(ax1B); - info->m_constraintError[s3] = k * u.dot(p); - info->m_constraintError[s4] = k * u.dot(q); -#endif - // check angular limits - 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); - btScalar currERP = (m_flags & BT_HINGE_FLAGS_ERP_STOP) ? m_stopERP : info->erp; - if(powered) - { - if(m_flags & BT_HINGE_FLAGS_CFM_NORM) - { - info->cfm[srow] = m_normalCFM; - } - btScalar mot_fact = getMotorFactor(m_hingeAngle, lostop, histop, m_motorTargetVelocity, info->fps * currERP); - 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 * currERP; - info->m_constraintError[srow] += k * limit_err; - if(m_flags & BT_HINGE_FLAGS_CFM_STOP) - { - info->cfm[srow] = m_stopCFM; - } - 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 -} - - -///override the default global value of a parameter (such as ERP or CFM), optionally provide the axis (0..5). -///If no axis is provided, it uses the default axis for this constraint. -void btHingeConstraint::setParam(int num, btScalar value, int axis) -{ - if((axis == -1) || (axis == 5)) - { - switch(num) - { - case BT_CONSTRAINT_STOP_ERP : - m_stopERP = value; - m_flags |= BT_HINGE_FLAGS_ERP_STOP; - break; - case BT_CONSTRAINT_STOP_CFM : - m_stopCFM = value; - m_flags |= BT_HINGE_FLAGS_CFM_STOP; - break; - case BT_CONSTRAINT_CFM : - m_normalCFM = value; - m_flags |= BT_HINGE_FLAGS_CFM_NORM; - break; - default : - btAssertConstrParams(0); - } - } - else - { - btAssertConstrParams(0); - } -} - -///return the local value of parameter -btScalar btHingeConstraint::getParam(int num, int axis) const -{ - btScalar retVal = 0; - if((axis == -1) || (axis == 5)) - { - switch(num) - { - case BT_CONSTRAINT_STOP_ERP : - btAssertConstrParams(m_flags & BT_HINGE_FLAGS_ERP_STOP); - retVal = m_stopERP; - break; - case BT_CONSTRAINT_STOP_CFM : - btAssertConstrParams(m_flags & BT_HINGE_FLAGS_CFM_STOP); - retVal = m_stopCFM; - break; - case BT_CONSTRAINT_CFM : - btAssertConstrParams(m_flags & BT_HINGE_FLAGS_CFM_NORM); - retVal = m_normalCFM; - break; - default : - btAssertConstrParams(0); - } - } - else - { - btAssertConstrParams(0); - } - return retVal; -} - - diff --git a/extern/bullet2/BulletDynamics/ConstraintSolver/btHingeConstraint.h b/extern/bullet2/BulletDynamics/ConstraintSolver/btHingeConstraint.h deleted file mode 100644 index a65f3638ed5..00000000000 --- a/extern/bullet2/BulletDynamics/ConstraintSolver/btHingeConstraint.h +++ /dev/null @@ -1,332 +0,0 @@ -/* -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. -*/ - -/* Hinge Constraint by Dirk Gregorius. Limits added by Marcus Hennix at Starbreeze Studios */ - -#ifndef HINGECONSTRAINT_H -#define HINGECONSTRAINT_H - -#include "LinearMath/btVector3.h" -#include "btJacobianEntry.h" -#include "btTypedConstraint.h" - -class btRigidBody; - -#ifdef BT_USE_DOUBLE_PRECISION -#define btHingeConstraintData btHingeConstraintDoubleData -#define btHingeConstraintDataName "btHingeConstraintDoubleData" -#else -#define btHingeConstraintData btHingeConstraintFloatData -#define btHingeConstraintDataName "btHingeConstraintFloatData" -#endif //BT_USE_DOUBLE_PRECISION - - -enum btHingeFlags -{ - BT_HINGE_FLAGS_CFM_STOP = 1, - BT_HINGE_FLAGS_ERP_STOP = 2, - BT_HINGE_FLAGS_CFM_NORM = 4 -}; - - -/// hinge constraint between two rigidbodies each with a pivotpoint that descibes the axis location in local space -/// axis defines the orientation of the hinge axis -ATTRIBUTE_ALIGNED16(class) btHingeConstraint : public btTypedConstraint -{ -#ifdef IN_PARALLELL_SOLVER -public: -#endif - btJacobianEntry m_jac[3]; //3 orthogonal linear constraints - btJacobianEntry m_jacAng[3]; //2 orthogonal angular constraints+ 1 for limit/motor - - btTransform m_rbAFrame; // constraint axii. Assumes z is hinge axis. - btTransform m_rbBFrame; - - btScalar m_motorTargetVelocity; - btScalar m_maxMotorImpulse; - - btScalar m_limitSoftness; - btScalar m_biasFactor; - btScalar m_relaxationFactor; - - btScalar m_lowerLimit; - btScalar m_upperLimit; - - btScalar m_kHinge; - - btScalar m_limitSign; - btScalar m_correction; - - btScalar m_accLimitImpulse; - btScalar m_hingeAngle; - btScalar m_referenceSign; - - bool m_angularOnly; - bool m_enableAngularMotor; - bool m_solveLimit; - bool m_useSolveConstraintObsolete; - bool m_useOffsetForConstraintFrame; - bool m_useReferenceFrameA; - - btScalar m_accMotorImpulse; - - int m_flags; - btScalar m_normalCFM; - btScalar m_stopCFM; - btScalar m_stopERP; - - -public: - - btHingeConstraint(btRigidBody& rbA,btRigidBody& rbB, const btVector3& pivotInA,const btVector3& pivotInB, btVector3& axisInA,btVector3& axisInB, bool useReferenceFrameA = false); - - btHingeConstraint(btRigidBody& rbA,const btVector3& pivotInA,btVector3& axisInA, bool useReferenceFrameA = false); - - btHingeConstraint(btRigidBody& rbA,btRigidBody& rbB, const btTransform& rbAFrame, const btTransform& rbBFrame, bool useReferenceFrameA = false); - - btHingeConstraint(btRigidBody& rbA,const btTransform& rbAFrame, bool useReferenceFrameA = false); - - - virtual void buildJacobian(); - - virtual void getInfo1 (btConstraintInfo1* info); - - void getInfo1NonVirtual(btConstraintInfo1* info); - - virtual void getInfo2 (btConstraintInfo2* info); - - void getInfo2NonVirtual(btConstraintInfo2* info,const btTransform& transA,const btTransform& transB,const btVector3& angVelA,const btVector3& angVelB); - - void getInfo2Internal(btConstraintInfo2* info,const btTransform& transA,const btTransform& transB,const btVector3& angVelA,const btVector3& angVelB); - void getInfo2InternalUsingFrameOffset(btConstraintInfo2* info,const btTransform& transA,const btTransform& transB,const btVector3& angVelA,const btVector3& angVelB); - - - void updateRHS(btScalar timeStep); - - const btRigidBody& getRigidBodyA() const - { - return m_rbA; - } - const btRigidBody& getRigidBodyB() const - { - return m_rbB; - } - - btRigidBody& getRigidBodyA() - { - return m_rbA; - } - - btRigidBody& getRigidBodyB() - { - return m_rbB; - } - - void setAngularOnly(bool angularOnly) - { - m_angularOnly = angularOnly; - } - - void enableAngularMotor(bool enableMotor,btScalar targetVelocity,btScalar maxMotorImpulse) - { - m_enableAngularMotor = enableMotor; - m_motorTargetVelocity = targetVelocity; - m_maxMotorImpulse = maxMotorImpulse; - } - - // extra motor API, including ability to set a target rotation (as opposed to angular velocity) - // note: setMotorTarget sets angular velocity under the hood, so you must call it every tick to - // maintain a given angular target. - void enableMotor(bool enableMotor) { m_enableAngularMotor = enableMotor; } - void setMaxMotorImpulse(btScalar maxMotorImpulse) { m_maxMotorImpulse = maxMotorImpulse; } - void setMotorTarget(const btQuaternion& qAinB, btScalar dt); // qAinB is rotation of body A wrt body B. - void setMotorTarget(btScalar targetAngle, btScalar dt); - - - void setLimit(btScalar low,btScalar high,btScalar _softness = 0.9f, btScalar _biasFactor = 0.3f, btScalar _relaxationFactor = 1.0f) - { - m_lowerLimit = btNormalizeAngle(low); - m_upperLimit = btNormalizeAngle(high); - - m_limitSoftness = _softness; - m_biasFactor = _biasFactor; - m_relaxationFactor = _relaxationFactor; - - } - - void setAxis(btVector3& axisInA) - { - btVector3 rbAxisA1, rbAxisA2; - btPlaneSpace1(axisInA, rbAxisA1, rbAxisA2); - btVector3 pivotInA = m_rbAFrame.getOrigin(); -// 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 = m_rbA.getCenterOfMassTransform().getBasis() * axisInA; - - btQuaternion rotationArc = shortestArcQuat(axisInA,axisInB); - btVector3 rbAxisB1 = quatRotate(rotationArc,rbAxisA1); - btVector3 rbAxisB2 = axisInB.cross(rbAxisB1); - - - m_rbBFrame.getOrigin() = m_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() ); - } - - btScalar getLowerLimit() const - { - return m_lowerLimit; - } - - btScalar getUpperLimit() const - { - return m_upperLimit; - } - - - btScalar getHingeAngle(); - - btScalar getHingeAngle(const btTransform& transA,const btTransform& transB); - - void testLimit(const btTransform& transA,const btTransform& transB); - - - const btTransform& getAFrame() const { return m_rbAFrame; }; - const btTransform& getBFrame() const { return m_rbBFrame; }; - - btTransform& getAFrame() { return m_rbAFrame; }; - btTransform& getBFrame() { return m_rbBFrame; }; - - inline int getSolveLimit() - { - return m_solveLimit; - } - - inline btScalar getLimitSign() - { - return m_limitSign; - } - - inline bool getAngularOnly() - { - return m_angularOnly; - } - inline bool getEnableAngularMotor() - { - return m_enableAngularMotor; - } - inline btScalar getMotorTargetVelosity() - { - return m_motorTargetVelocity; - } - inline btScalar getMaxMotorImpulse() - { - return m_maxMotorImpulse; - } - // access for UseFrameOffset - bool getUseFrameOffset() { return m_useOffsetForConstraintFrame; } - void setUseFrameOffset(bool frameOffsetOnOff) { m_useOffsetForConstraintFrame = frameOffsetOnOff; } - - - ///override the default global value of a parameter (such as ERP or CFM), optionally provide the axis (0..5). - ///If no axis is provided, it uses the default axis for this constraint. - virtual void setParam(int num, btScalar value, int axis = -1); - ///return the local value of parameter - virtual btScalar getParam(int num, int axis = -1) const; - - virtual int calculateSerializeBufferSize() const; - - ///fills the dataBuffer and returns the struct name (and 0 on failure) - virtual const char* serialize(void* dataBuffer, btSerializer* serializer) const; - - -}; - -///do not change those serialization structures, it requires an updated sBulletDNAstr/sBulletDNAstr64 -struct btHingeConstraintDoubleData -{ - btTypedConstraintData m_typeConstraintData; - btTransformDoubleData m_rbAFrame; // constraint axii. Assumes z is hinge axis. - btTransformDoubleData m_rbBFrame; - int m_useReferenceFrameA; - int m_angularOnly; - int m_enableAngularMotor; - float m_motorTargetVelocity; - float m_maxMotorImpulse; - - float m_lowerLimit; - float m_upperLimit; - float m_limitSoftness; - float m_biasFactor; - float m_relaxationFactor; - -}; -///do not change those serialization structures, it requires an updated sBulletDNAstr/sBulletDNAstr64 -struct btHingeConstraintFloatData -{ - btTypedConstraintData m_typeConstraintData; - btTransformFloatData m_rbAFrame; // constraint axii. Assumes z is hinge axis. - btTransformFloatData m_rbBFrame; - int m_useReferenceFrameA; - int m_angularOnly; - - int m_enableAngularMotor; - float m_motorTargetVelocity; - float m_maxMotorImpulse; - - float m_lowerLimit; - float m_upperLimit; - float m_limitSoftness; - float m_biasFactor; - float m_relaxationFactor; - -}; - - - -SIMD_FORCE_INLINE int btHingeConstraint::calculateSerializeBufferSize() const -{ - return sizeof(btHingeConstraintData); -} - - ///fills the dataBuffer and returns the struct name (and 0 on failure) -SIMD_FORCE_INLINE const char* btHingeConstraint::serialize(void* dataBuffer, btSerializer* serializer) const -{ - btHingeConstraintData* hingeData = (btHingeConstraintData*)dataBuffer; - btTypedConstraint::serialize(&hingeData->m_typeConstraintData,serializer); - - m_rbAFrame.serialize(hingeData->m_rbAFrame); - m_rbBFrame.serialize(hingeData->m_rbBFrame); - - hingeData->m_angularOnly = m_angularOnly; - hingeData->m_enableAngularMotor = m_enableAngularMotor; - hingeData->m_maxMotorImpulse = float(m_maxMotorImpulse); - hingeData->m_motorTargetVelocity = float(m_motorTargetVelocity); - hingeData->m_useReferenceFrameA = m_useReferenceFrameA; - - hingeData->m_lowerLimit = float(m_lowerLimit); - hingeData->m_upperLimit = float(m_upperLimit); - hingeData->m_limitSoftness = float(m_limitSoftness); - hingeData->m_biasFactor = float(m_biasFactor); - hingeData->m_relaxationFactor = float(m_relaxationFactor); - - return btHingeConstraintDataName; -} - -#endif //HINGECONSTRAINT_H diff --git a/extern/bullet2/BulletDynamics/ConstraintSolver/btJacobianEntry.h b/extern/bullet2/BulletDynamics/ConstraintSolver/btJacobianEntry.h deleted file mode 100644 index 22a8af66b8e..00000000000 --- a/extern/bullet2/BulletDynamics/ConstraintSolver/btJacobianEntry.h +++ /dev/null @@ -1,156 +0,0 @@ -/* -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. -*/ - -#ifndef JACOBIAN_ENTRY_H -#define JACOBIAN_ENTRY_H - -#include "LinearMath/btVector3.h" -#include "BulletDynamics/Dynamics/btRigidBody.h" - - -//notes: -// Another memory optimization would be to store m_1MinvJt in the remaining 3 w components -// which makes the btJacobianEntry memory layout 16 bytes -// if you only are interested in angular part, just feed massInvA and massInvB zero - -/// Jacobian entry is an abstraction that allows to describe constraints -/// it can be used in combination with a constraint solver -/// Can be used to relate the effect of an impulse to the constraint error -ATTRIBUTE_ALIGNED16(class) btJacobianEntry -{ -public: - btJacobianEntry() {}; - //constraint between two different rigidbodies - btJacobianEntry( - const btMatrix3x3& world2A, - const btMatrix3x3& world2B, - const btVector3& rel_pos1,const btVector3& rel_pos2, - const btVector3& jointAxis, - const btVector3& inertiaInvA, - const btScalar massInvA, - const btVector3& inertiaInvB, - const btScalar massInvB) - :m_linearJointAxis(jointAxis) - { - m_aJ = world2A*(rel_pos1.cross(m_linearJointAxis)); - m_bJ = world2B*(rel_pos2.cross(-m_linearJointAxis)); - m_0MinvJt = inertiaInvA * m_aJ; - m_1MinvJt = inertiaInvB * m_bJ; - m_Adiag = massInvA + m_0MinvJt.dot(m_aJ) + massInvB + m_1MinvJt.dot(m_bJ); - - btAssert(m_Adiag > btScalar(0.0)); - } - - //angular constraint between two different rigidbodies - btJacobianEntry(const btVector3& jointAxis, - const btMatrix3x3& world2A, - const btMatrix3x3& world2B, - const btVector3& inertiaInvA, - const btVector3& inertiaInvB) - :m_linearJointAxis(btVector3(btScalar(0.),btScalar(0.),btScalar(0.))) - { - m_aJ= world2A*jointAxis; - m_bJ = world2B*-jointAxis; - m_0MinvJt = inertiaInvA * m_aJ; - m_1MinvJt = inertiaInvB * m_bJ; - m_Adiag = m_0MinvJt.dot(m_aJ) + m_1MinvJt.dot(m_bJ); - - btAssert(m_Adiag > btScalar(0.0)); - } - - //angular constraint between two different rigidbodies - btJacobianEntry(const btVector3& axisInA, - const btVector3& axisInB, - const btVector3& inertiaInvA, - const btVector3& inertiaInvB) - : m_linearJointAxis(btVector3(btScalar(0.),btScalar(0.),btScalar(0.))) - , m_aJ(axisInA) - , m_bJ(-axisInB) - { - m_0MinvJt = inertiaInvA * m_aJ; - m_1MinvJt = inertiaInvB * m_bJ; - m_Adiag = m_0MinvJt.dot(m_aJ) + m_1MinvJt.dot(m_bJ); - - btAssert(m_Adiag > btScalar(0.0)); - } - - //constraint on one rigidbody - btJacobianEntry( - const btMatrix3x3& world2A, - const btVector3& rel_pos1,const btVector3& rel_pos2, - const btVector3& jointAxis, - const btVector3& inertiaInvA, - const btScalar massInvA) - :m_linearJointAxis(jointAxis) - { - m_aJ= world2A*(rel_pos1.cross(jointAxis)); - m_bJ = world2A*(rel_pos2.cross(-jointAxis)); - m_0MinvJt = inertiaInvA * m_aJ; - m_1MinvJt = btVector3(btScalar(0.),btScalar(0.),btScalar(0.)); - m_Adiag = massInvA + m_0MinvJt.dot(m_aJ); - - btAssert(m_Adiag > btScalar(0.0)); - } - - btScalar getDiagonal() const { return m_Adiag; } - - // for two constraints on the same rigidbody (for example vehicle friction) - btScalar getNonDiagonal(const btJacobianEntry& jacB, const btScalar massInvA) const - { - const btJacobianEntry& jacA = *this; - btScalar lin = massInvA * jacA.m_linearJointAxis.dot(jacB.m_linearJointAxis); - btScalar ang = jacA.m_0MinvJt.dot(jacB.m_aJ); - return lin + ang; - } - - - - // for two constraints on sharing two same rigidbodies (for example two contact points between two rigidbodies) - btScalar getNonDiagonal(const btJacobianEntry& jacB,const btScalar massInvA,const btScalar massInvB) const - { - const btJacobianEntry& jacA = *this; - btVector3 lin = jacA.m_linearJointAxis * jacB.m_linearJointAxis; - btVector3 ang0 = jacA.m_0MinvJt * jacB.m_aJ; - btVector3 ang1 = jacA.m_1MinvJt * jacB.m_bJ; - btVector3 lin0 = massInvA * lin ; - btVector3 lin1 = massInvB * lin; - btVector3 sum = ang0+ang1+lin0+lin1; - return sum[0]+sum[1]+sum[2]; - } - - btScalar getRelativeVelocity(const btVector3& linvelA,const btVector3& angvelA,const btVector3& linvelB,const btVector3& angvelB) - { - btVector3 linrel = linvelA - linvelB; - btVector3 angvela = angvelA * m_aJ; - btVector3 angvelb = angvelB * m_bJ; - linrel *= m_linearJointAxis; - angvela += angvelb; - angvela += linrel; - btScalar rel_vel2 = angvela[0]+angvela[1]+angvela[2]; - return rel_vel2 + SIMD_EPSILON; - } -//private: - - btVector3 m_linearJointAxis; - btVector3 m_aJ; - btVector3 m_bJ; - btVector3 m_0MinvJt; - btVector3 m_1MinvJt; - //Optimization: can be stored in the w/last component of one of the vectors - btScalar m_Adiag; - -}; - -#endif //JACOBIAN_ENTRY_H diff --git a/extern/bullet2/BulletDynamics/ConstraintSolver/btPoint2PointConstraint.cpp b/extern/bullet2/BulletDynamics/ConstraintSolver/btPoint2PointConstraint.cpp deleted file mode 100644 index 50ad71ab505..00000000000 --- a/extern/bullet2/BulletDynamics/ConstraintSolver/btPoint2PointConstraint.cpp +++ /dev/null @@ -1,229 +0,0 @@ -/* -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 "btPoint2PointConstraint.h" -#include "BulletDynamics/Dynamics/btRigidBody.h" -#include <new> - - - - - -btPoint2PointConstraint::btPoint2PointConstraint(btRigidBody& rbA,btRigidBody& rbB, const btVector3& pivotInA,const btVector3& pivotInB) -:btTypedConstraint(POINT2POINT_CONSTRAINT_TYPE,rbA,rbB),m_pivotInA(pivotInA),m_pivotInB(pivotInB), -m_flags(0), -m_useSolveConstraintObsolete(false) -{ - -} - - -btPoint2PointConstraint::btPoint2PointConstraint(btRigidBody& rbA,const btVector3& pivotInA) -:btTypedConstraint(POINT2POINT_CONSTRAINT_TYPE,rbA),m_pivotInA(pivotInA),m_pivotInB(rbA.getCenterOfMassTransform()(pivotInA)), -m_flags(0), -m_useSolveConstraintObsolete(false) -{ - -} - -void btPoint2PointConstraint::buildJacobian() -{ - - ///we need it for both methods - { - m_appliedImpulse = btScalar(0.); - - btVector3 normal(0,0,0); - - for (int i=0;i<3;i++) - { - normal[i] = 1; - new (&m_jac[i]) btJacobianEntry( - m_rbA.getCenterOfMassTransform().getBasis().transpose(), - m_rbB.getCenterOfMassTransform().getBasis().transpose(), - m_rbA.getCenterOfMassTransform()*m_pivotInA - m_rbA.getCenterOfMassPosition(), - m_rbB.getCenterOfMassTransform()*m_pivotInB - m_rbB.getCenterOfMassPosition(), - normal, - m_rbA.getInvInertiaDiagLocal(), - m_rbA.getInvMass(), - m_rbB.getInvInertiaDiagLocal(), - m_rbB.getInvMass()); - normal[i] = 0; - } - } - - -} - -void btPoint2PointConstraint::getInfo1 (btConstraintInfo1* info) -{ - getInfo1NonVirtual(info); -} - -void btPoint2PointConstraint::getInfo1NonVirtual (btConstraintInfo1* info) -{ - if (m_useSolveConstraintObsolete) - { - info->m_numConstraintRows = 0; - info->nub = 0; - } else - { - info->m_numConstraintRows = 3; - info->nub = 3; - } -} - - - - -void btPoint2PointConstraint::getInfo2 (btConstraintInfo2* info) -{ - getInfo2NonVirtual(info, m_rbA.getCenterOfMassTransform(),m_rbB.getCenterOfMassTransform()); -} - -void btPoint2PointConstraint::getInfo2NonVirtual (btConstraintInfo2* info, const btTransform& body0_trans, const btTransform& body1_trans) -{ - btAssert(!m_useSolveConstraintObsolete); - - //retrieve matrices - - // anchor points in global coordinates with respect to body PORs. - - // set jacobian - info->m_J1linearAxis[0] = 1; - info->m_J1linearAxis[info->rowskip+1] = 1; - info->m_J1linearAxis[2*info->rowskip+2] = 1; - - btVector3 a1 = body0_trans.getBasis()*getPivotInA(); - { - btVector3* angular0 = (btVector3*)(info->m_J1angularAxis); - btVector3* angular1 = (btVector3*)(info->m_J1angularAxis+info->rowskip); - btVector3* angular2 = (btVector3*)(info->m_J1angularAxis+2*info->rowskip); - btVector3 a1neg = -a1; - a1neg.getSkewSymmetricMatrix(angular0,angular1,angular2); - } - - /*info->m_J2linearAxis[0] = -1; - info->m_J2linearAxis[s+1] = -1; - info->m_J2linearAxis[2*s+2] = -1; - */ - - btVector3 a2 = body1_trans.getBasis()*getPivotInB(); - - { - btVector3 a2n = -a2; - btVector3* angular0 = (btVector3*)(info->m_J2angularAxis); - btVector3* angular1 = (btVector3*)(info->m_J2angularAxis+info->rowskip); - btVector3* angular2 = (btVector3*)(info->m_J2angularAxis+2*info->rowskip); - a2.getSkewSymmetricMatrix(angular0,angular1,angular2); - } - - - - // set right hand side - btScalar currERP = (m_flags & BT_P2P_FLAGS_ERP) ? m_erp : info->erp; - btScalar k = info->fps * currERP; - int j; - for (j=0; j<3; j++) - { - info->m_constraintError[j*info->rowskip] = k * (a2[j] + body1_trans.getOrigin()[j] - a1[j] - body0_trans.getOrigin()[j]); - //printf("info->m_constraintError[%d]=%f\n",j,info->m_constraintError[j]); - } - if(m_flags & BT_P2P_FLAGS_CFM) - { - for (j=0; j<3; j++) - { - info->cfm[j*info->rowskip] = m_cfm; - } - } - - btScalar impulseClamp = m_setting.m_impulseClamp;// - for (j=0; j<3; j++) - { - if (m_setting.m_impulseClamp > 0) - { - info->m_lowerLimit[j*info->rowskip] = -impulseClamp; - info->m_upperLimit[j*info->rowskip] = impulseClamp; - } - } - -} - - - -void btPoint2PointConstraint::updateRHS(btScalar timeStep) -{ - (void)timeStep; - -} - -///override the default global value of a parameter (such as ERP or CFM), optionally provide the axis (0..5). -///If no axis is provided, it uses the default axis for this constraint. -void btPoint2PointConstraint::setParam(int num, btScalar value, int axis) -{ - if(axis != -1) - { - btAssertConstrParams(0); - } - else - { - switch(num) - { - case BT_CONSTRAINT_ERP : - case BT_CONSTRAINT_STOP_ERP : - m_erp = value; - m_flags |= BT_P2P_FLAGS_ERP; - break; - case BT_CONSTRAINT_CFM : - case BT_CONSTRAINT_STOP_CFM : - m_cfm = value; - m_flags |= BT_P2P_FLAGS_CFM; - break; - default: - btAssertConstrParams(0); - } - } -} - -///return the local value of parameter -btScalar btPoint2PointConstraint::getParam(int num, int axis) const -{ - btScalar retVal(SIMD_INFINITY); - if(axis != -1) - { - btAssertConstrParams(0); - } - else - { - switch(num) - { - case BT_CONSTRAINT_ERP : - case BT_CONSTRAINT_STOP_ERP : - btAssertConstrParams(m_flags & BT_P2P_FLAGS_ERP); - retVal = m_erp; - break; - case BT_CONSTRAINT_CFM : - case BT_CONSTRAINT_STOP_CFM : - btAssertConstrParams(m_flags & BT_P2P_FLAGS_CFM); - retVal = m_cfm; - break; - default: - btAssertConstrParams(0); - } - } - return retVal; -} - diff --git a/extern/bullet2/BulletDynamics/ConstraintSolver/btPoint2PointConstraint.h b/extern/bullet2/BulletDynamics/ConstraintSolver/btPoint2PointConstraint.h deleted file mode 100644 index b589ee68254..00000000000 --- a/extern/bullet2/BulletDynamics/ConstraintSolver/btPoint2PointConstraint.h +++ /dev/null @@ -1,161 +0,0 @@ -/* -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. -*/ - -#ifndef POINT2POINTCONSTRAINT_H -#define POINT2POINTCONSTRAINT_H - -#include "LinearMath/btVector3.h" -#include "btJacobianEntry.h" -#include "btTypedConstraint.h" - -class btRigidBody; - - -#ifdef BT_USE_DOUBLE_PRECISION -#define btPoint2PointConstraintData btPoint2PointConstraintDoubleData -#define btPoint2PointConstraintDataName "btPoint2PointConstraintDoubleData" -#else -#define btPoint2PointConstraintData btPoint2PointConstraintFloatData -#define btPoint2PointConstraintDataName "btPoint2PointConstraintFloatData" -#endif //BT_USE_DOUBLE_PRECISION - -struct btConstraintSetting -{ - btConstraintSetting() : - m_tau(btScalar(0.3)), - m_damping(btScalar(1.)), - m_impulseClamp(btScalar(0.)) - { - } - btScalar m_tau; - btScalar m_damping; - btScalar m_impulseClamp; -}; - -enum btPoint2PointFlags -{ - BT_P2P_FLAGS_ERP = 1, - BT_P2P_FLAGS_CFM = 2 -}; - -/// point to point constraint between two rigidbodies each with a pivotpoint that descibes the 'ballsocket' location in local space -ATTRIBUTE_ALIGNED16(class) btPoint2PointConstraint : public btTypedConstraint -{ -#ifdef IN_PARALLELL_SOLVER -public: -#endif - btJacobianEntry m_jac[3]; //3 orthogonal linear constraints - - btVector3 m_pivotInA; - btVector3 m_pivotInB; - - int m_flags; - btScalar m_erp; - btScalar m_cfm; - -public: - - ///for backwards compatibility during the transition to 'getInfo/getInfo2' - bool m_useSolveConstraintObsolete; - - btConstraintSetting m_setting; - - btPoint2PointConstraint(btRigidBody& rbA,btRigidBody& rbB, const btVector3& pivotInA,const btVector3& pivotInB); - - btPoint2PointConstraint(btRigidBody& rbA,const btVector3& pivotInA); - - - virtual void buildJacobian(); - - virtual void getInfo1 (btConstraintInfo1* info); - - void getInfo1NonVirtual (btConstraintInfo1* info); - - virtual void getInfo2 (btConstraintInfo2* info); - - void getInfo2NonVirtual (btConstraintInfo2* info, const btTransform& body0_trans, const btTransform& body1_trans); - - void updateRHS(btScalar timeStep); - - void setPivotA(const btVector3& pivotA) - { - m_pivotInA = pivotA; - } - - void setPivotB(const btVector3& pivotB) - { - m_pivotInB = pivotB; - } - - const btVector3& getPivotInA() const - { - return m_pivotInA; - } - - const btVector3& getPivotInB() const - { - return m_pivotInB; - } - - ///override the default global value of a parameter (such as ERP or CFM), optionally provide the axis (0..5). - ///If no axis is provided, it uses the default axis for this constraint. - virtual void setParam(int num, btScalar value, int axis = -1); - ///return the local value of parameter - virtual btScalar getParam(int num, int axis = -1) const; - - virtual int calculateSerializeBufferSize() const; - - ///fills the dataBuffer and returns the struct name (and 0 on failure) - virtual const char* serialize(void* dataBuffer, btSerializer* serializer) const; - - -}; - -///do not change those serialization structures, it requires an updated sBulletDNAstr/sBulletDNAstr64 -struct btPoint2PointConstraintFloatData -{ - btTypedConstraintData m_typeConstraintData; - btVector3FloatData m_pivotInA; - btVector3FloatData m_pivotInB; -}; - -///do not change those serialization structures, it requires an updated sBulletDNAstr/sBulletDNAstr64 -struct btPoint2PointConstraintDoubleData -{ - btTypedConstraintData m_typeConstraintData; - btVector3DoubleData m_pivotInA; - btVector3DoubleData m_pivotInB; -}; - - -SIMD_FORCE_INLINE int btPoint2PointConstraint::calculateSerializeBufferSize() const -{ - return sizeof(btPoint2PointConstraintData); - -} - - ///fills the dataBuffer and returns the struct name (and 0 on failure) -SIMD_FORCE_INLINE const char* btPoint2PointConstraint::serialize(void* dataBuffer, btSerializer* serializer) const -{ - btPoint2PointConstraintData* p2pData = (btPoint2PointConstraintData*)dataBuffer; - - btTypedConstraint::serialize(&p2pData->m_typeConstraintData,serializer); - m_pivotInA.serialize(p2pData->m_pivotInA); - m_pivotInB.serialize(p2pData->m_pivotInB); - - return btPoint2PointConstraintDataName; -} - -#endif //POINT2POINTCONSTRAINT_H diff --git a/extern/bullet2/BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolver.cpp b/extern/bullet2/BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolver.cpp deleted file mode 100644 index 4e1048823c0..00000000000 --- a/extern/bullet2/BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolver.cpp +++ /dev/null @@ -1,1174 +0,0 @@ -/* -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. -*/ - -//#define COMPUTE_IMPULSE_DENOM 1 -//It is not necessary (redundant) to refresh contact manifolds, this refresh has been moved to the collision algorithms. - -#include "btSequentialImpulseConstraintSolver.h" -#include "BulletCollision/NarrowPhaseCollision/btPersistentManifold.h" -#include "BulletDynamics/Dynamics/btRigidBody.h" -#include "btContactConstraint.h" -#include "btSolve2LinearConstraint.h" -#include "btContactSolverInfo.h" -#include "LinearMath/btIDebugDraw.h" -#include "btJacobianEntry.h" -#include "LinearMath/btMinMax.h" -#include "BulletDynamics/ConstraintSolver/btTypedConstraint.h" -#include <new> -#include "LinearMath/btStackAlloc.h" -#include "LinearMath/btQuickprof.h" -#include "btSolverBody.h" -#include "btSolverConstraint.h" -#include "LinearMath/btAlignedObjectArray.h" -#include <string.h> //for memset - -int gNumSplitImpulseRecoveries = 0; - -btSequentialImpulseConstraintSolver::btSequentialImpulseConstraintSolver() -:m_btSeed2(0) -{ - -} - -btSequentialImpulseConstraintSolver::~btSequentialImpulseConstraintSolver() -{ -} - -#ifdef USE_SIMD -#include <emmintrin.h> -#define vec_splat(x, e) _mm_shuffle_ps(x, x, _MM_SHUFFLE(e,e,e,e)) -static inline __m128 _vmathVfDot3( __m128 vec0, __m128 vec1 ) -{ - __m128 result = _mm_mul_ps( vec0, vec1); - return _mm_add_ps( vec_splat( result, 0 ), _mm_add_ps( vec_splat( result, 1 ), vec_splat( result, 2 ) ) ); -} -#endif//USE_SIMD - -// Project Gauss Seidel or the equivalent Sequential Impulse -void btSequentialImpulseConstraintSolver::resolveSingleConstraintRowGenericSIMD(btRigidBody& body1,btRigidBody& body2,const btSolverConstraint& c) -{ -#ifdef USE_SIMD - __m128 cpAppliedImp = _mm_set1_ps(c.m_appliedImpulse); - __m128 lowerLimit1 = _mm_set1_ps(c.m_lowerLimit); - __m128 upperLimit1 = _mm_set1_ps(c.m_upperLimit); - __m128 deltaImpulse = _mm_sub_ps(_mm_set1_ps(c.m_rhs), _mm_mul_ps(_mm_set1_ps(c.m_appliedImpulse),_mm_set1_ps(c.m_cfm))); - __m128 deltaVel1Dotn = _mm_add_ps(_vmathVfDot3(c.m_contactNormal.mVec128,body1.internalGetDeltaLinearVelocity().mVec128), _vmathVfDot3(c.m_relpos1CrossNormal.mVec128,body1.internalGetDeltaAngularVelocity().mVec128)); - __m128 deltaVel2Dotn = _mm_sub_ps(_vmathVfDot3(c.m_relpos2CrossNormal.mVec128,body2.internalGetDeltaAngularVelocity().mVec128),_vmathVfDot3((c.m_contactNormal).mVec128,body2.internalGetDeltaLinearVelocity().mVec128)); - deltaImpulse = _mm_sub_ps(deltaImpulse,_mm_mul_ps(deltaVel1Dotn,_mm_set1_ps(c.m_jacDiagABInv))); - deltaImpulse = _mm_sub_ps(deltaImpulse,_mm_mul_ps(deltaVel2Dotn,_mm_set1_ps(c.m_jacDiagABInv))); - btSimdScalar sum = _mm_add_ps(cpAppliedImp,deltaImpulse); - btSimdScalar resultLowerLess,resultUpperLess; - resultLowerLess = _mm_cmplt_ps(sum,lowerLimit1); - resultUpperLess = _mm_cmplt_ps(sum,upperLimit1); - __m128 lowMinApplied = _mm_sub_ps(lowerLimit1,cpAppliedImp); - deltaImpulse = _mm_or_ps( _mm_and_ps(resultLowerLess, lowMinApplied), _mm_andnot_ps(resultLowerLess, deltaImpulse) ); - c.m_appliedImpulse = _mm_or_ps( _mm_and_ps(resultLowerLess, lowerLimit1), _mm_andnot_ps(resultLowerLess, sum) ); - __m128 upperMinApplied = _mm_sub_ps(upperLimit1,cpAppliedImp); - deltaImpulse = _mm_or_ps( _mm_and_ps(resultUpperLess, deltaImpulse), _mm_andnot_ps(resultUpperLess, upperMinApplied) ); - c.m_appliedImpulse = _mm_or_ps( _mm_and_ps(resultUpperLess, c.m_appliedImpulse), _mm_andnot_ps(resultUpperLess, upperLimit1) ); - __m128 linearComponentA = _mm_mul_ps(c.m_contactNormal.mVec128,body1.internalGetInvMass().mVec128); - __m128 linearComponentB = _mm_mul_ps((c.m_contactNormal).mVec128,body2.internalGetInvMass().mVec128); - __m128 impulseMagnitude = deltaImpulse; - body1.internalGetDeltaLinearVelocity().mVec128 = _mm_add_ps(body1.internalGetDeltaLinearVelocity().mVec128,_mm_mul_ps(linearComponentA,impulseMagnitude)); - body1.internalGetDeltaAngularVelocity().mVec128 = _mm_add_ps(body1.internalGetDeltaAngularVelocity().mVec128 ,_mm_mul_ps(c.m_angularComponentA.mVec128,impulseMagnitude)); - body2.internalGetDeltaLinearVelocity().mVec128 = _mm_sub_ps(body2.internalGetDeltaLinearVelocity().mVec128,_mm_mul_ps(linearComponentB,impulseMagnitude)); - body2.internalGetDeltaAngularVelocity().mVec128 = _mm_add_ps(body2.internalGetDeltaAngularVelocity().mVec128 ,_mm_mul_ps(c.m_angularComponentB.mVec128,impulseMagnitude)); -#else - resolveSingleConstraintRowGeneric(body1,body2,c); -#endif -} - -// Project Gauss Seidel or the equivalent Sequential Impulse - void btSequentialImpulseConstraintSolver::resolveSingleConstraintRowGeneric(btRigidBody& body1,btRigidBody& body2,const btSolverConstraint& c) -{ - btScalar deltaImpulse = c.m_rhs-btScalar(c.m_appliedImpulse)*c.m_cfm; - const btScalar deltaVel1Dotn = c.m_contactNormal.dot(body1.internalGetDeltaLinearVelocity()) + c.m_relpos1CrossNormal.dot(body1.internalGetDeltaAngularVelocity()); - const btScalar deltaVel2Dotn = -c.m_contactNormal.dot(body2.internalGetDeltaLinearVelocity()) + c.m_relpos2CrossNormal.dot(body2.internalGetDeltaAngularVelocity()); - -// const btScalar delta_rel_vel = deltaVel1Dotn-deltaVel2Dotn; - deltaImpulse -= deltaVel1Dotn*c.m_jacDiagABInv; - deltaImpulse -= deltaVel2Dotn*c.m_jacDiagABInv; - - const btScalar sum = btScalar(c.m_appliedImpulse) + deltaImpulse; - if (sum < c.m_lowerLimit) - { - deltaImpulse = c.m_lowerLimit-c.m_appliedImpulse; - c.m_appliedImpulse = c.m_lowerLimit; - } - else if (sum > c.m_upperLimit) - { - deltaImpulse = c.m_upperLimit-c.m_appliedImpulse; - c.m_appliedImpulse = c.m_upperLimit; - } - else - { - c.m_appliedImpulse = sum; - } - body1.internalApplyImpulse(c.m_contactNormal*body1.internalGetInvMass(),c.m_angularComponentA,deltaImpulse); - body2.internalApplyImpulse(-c.m_contactNormal*body2.internalGetInvMass(),c.m_angularComponentB,deltaImpulse); -} - - void btSequentialImpulseConstraintSolver::resolveSingleConstraintRowLowerLimitSIMD(btRigidBody& body1,btRigidBody& body2,const btSolverConstraint& c) -{ -#ifdef USE_SIMD - __m128 cpAppliedImp = _mm_set1_ps(c.m_appliedImpulse); - __m128 lowerLimit1 = _mm_set1_ps(c.m_lowerLimit); - __m128 upperLimit1 = _mm_set1_ps(c.m_upperLimit); - __m128 deltaImpulse = _mm_sub_ps(_mm_set1_ps(c.m_rhs), _mm_mul_ps(_mm_set1_ps(c.m_appliedImpulse),_mm_set1_ps(c.m_cfm))); - __m128 deltaVel1Dotn = _mm_add_ps(_vmathVfDot3(c.m_contactNormal.mVec128,body1.internalGetDeltaLinearVelocity().mVec128), _vmathVfDot3(c.m_relpos1CrossNormal.mVec128,body1.internalGetDeltaAngularVelocity().mVec128)); - __m128 deltaVel2Dotn = _mm_sub_ps(_vmathVfDot3(c.m_relpos2CrossNormal.mVec128,body2.internalGetDeltaAngularVelocity().mVec128),_vmathVfDot3((c.m_contactNormal).mVec128,body2.internalGetDeltaLinearVelocity().mVec128)); - deltaImpulse = _mm_sub_ps(deltaImpulse,_mm_mul_ps(deltaVel1Dotn,_mm_set1_ps(c.m_jacDiagABInv))); - deltaImpulse = _mm_sub_ps(deltaImpulse,_mm_mul_ps(deltaVel2Dotn,_mm_set1_ps(c.m_jacDiagABInv))); - btSimdScalar sum = _mm_add_ps(cpAppliedImp,deltaImpulse); - btSimdScalar resultLowerLess,resultUpperLess; - resultLowerLess = _mm_cmplt_ps(sum,lowerLimit1); - resultUpperLess = _mm_cmplt_ps(sum,upperLimit1); - __m128 lowMinApplied = _mm_sub_ps(lowerLimit1,cpAppliedImp); - deltaImpulse = _mm_or_ps( _mm_and_ps(resultLowerLess, lowMinApplied), _mm_andnot_ps(resultLowerLess, deltaImpulse) ); - c.m_appliedImpulse = _mm_or_ps( _mm_and_ps(resultLowerLess, lowerLimit1), _mm_andnot_ps(resultLowerLess, sum) ); - __m128 linearComponentA = _mm_mul_ps(c.m_contactNormal.mVec128,body1.internalGetInvMass().mVec128); - __m128 linearComponentB = _mm_mul_ps((c.m_contactNormal).mVec128,body2.internalGetInvMass().mVec128); - __m128 impulseMagnitude = deltaImpulse; - body1.internalGetDeltaLinearVelocity().mVec128 = _mm_add_ps(body1.internalGetDeltaLinearVelocity().mVec128,_mm_mul_ps(linearComponentA,impulseMagnitude)); - body1.internalGetDeltaAngularVelocity().mVec128 = _mm_add_ps(body1.internalGetDeltaAngularVelocity().mVec128 ,_mm_mul_ps(c.m_angularComponentA.mVec128,impulseMagnitude)); - body2.internalGetDeltaLinearVelocity().mVec128 = _mm_sub_ps(body2.internalGetDeltaLinearVelocity().mVec128,_mm_mul_ps(linearComponentB,impulseMagnitude)); - body2.internalGetDeltaAngularVelocity().mVec128 = _mm_add_ps(body2.internalGetDeltaAngularVelocity().mVec128 ,_mm_mul_ps(c.m_angularComponentB.mVec128,impulseMagnitude)); -#else - resolveSingleConstraintRowLowerLimit(body1,body2,c); -#endif -} - -// Project Gauss Seidel or the equivalent Sequential Impulse - void btSequentialImpulseConstraintSolver::resolveSingleConstraintRowLowerLimit(btRigidBody& body1,btRigidBody& body2,const btSolverConstraint& c) -{ - btScalar deltaImpulse = c.m_rhs-btScalar(c.m_appliedImpulse)*c.m_cfm; - const btScalar deltaVel1Dotn = c.m_contactNormal.dot(body1.internalGetDeltaLinearVelocity()) + c.m_relpos1CrossNormal.dot(body1.internalGetDeltaAngularVelocity()); - const btScalar deltaVel2Dotn = -c.m_contactNormal.dot(body2.internalGetDeltaLinearVelocity()) + c.m_relpos2CrossNormal.dot(body2.internalGetDeltaAngularVelocity()); - - deltaImpulse -= deltaVel1Dotn*c.m_jacDiagABInv; - deltaImpulse -= deltaVel2Dotn*c.m_jacDiagABInv; - const btScalar sum = btScalar(c.m_appliedImpulse) + deltaImpulse; - if (sum < c.m_lowerLimit) - { - deltaImpulse = c.m_lowerLimit-c.m_appliedImpulse; - c.m_appliedImpulse = c.m_lowerLimit; - } - else - { - c.m_appliedImpulse = sum; - } - body1.internalApplyImpulse(c.m_contactNormal*body1.internalGetInvMass(),c.m_angularComponentA,deltaImpulse); - body2.internalApplyImpulse(-c.m_contactNormal*body2.internalGetInvMass(),c.m_angularComponentB,deltaImpulse); -} - - -void btSequentialImpulseConstraintSolver::resolveSplitPenetrationImpulseCacheFriendly( - btRigidBody& body1, - btRigidBody& body2, - const btSolverConstraint& c) -{ - if (c.m_rhsPenetration) - { - gNumSplitImpulseRecoveries++; - btScalar deltaImpulse = c.m_rhsPenetration-btScalar(c.m_appliedPushImpulse)*c.m_cfm; - const btScalar deltaVel1Dotn = c.m_contactNormal.dot(body1.internalGetPushVelocity()) + c.m_relpos1CrossNormal.dot(body1.internalGetTurnVelocity()); - const btScalar deltaVel2Dotn = -c.m_contactNormal.dot(body2.internalGetPushVelocity()) + c.m_relpos2CrossNormal.dot(body2.internalGetTurnVelocity()); - - deltaImpulse -= deltaVel1Dotn*c.m_jacDiagABInv; - deltaImpulse -= deltaVel2Dotn*c.m_jacDiagABInv; - const btScalar sum = btScalar(c.m_appliedPushImpulse) + deltaImpulse; - if (sum < c.m_lowerLimit) - { - deltaImpulse = c.m_lowerLimit-c.m_appliedPushImpulse; - c.m_appliedPushImpulse = c.m_lowerLimit; - } - else - { - c.m_appliedPushImpulse = sum; - } - body1.internalApplyPushImpulse(c.m_contactNormal*body1.internalGetInvMass(),c.m_angularComponentA,deltaImpulse); - body2.internalApplyPushImpulse(-c.m_contactNormal*body2.internalGetInvMass(),c.m_angularComponentB,deltaImpulse); - } -} - - void btSequentialImpulseConstraintSolver::resolveSplitPenetrationSIMD(btRigidBody& body1,btRigidBody& body2,const btSolverConstraint& c) -{ -#ifdef USE_SIMD - if (!c.m_rhsPenetration) - return; - - gNumSplitImpulseRecoveries++; - - __m128 cpAppliedImp = _mm_set1_ps(c.m_appliedPushImpulse); - __m128 lowerLimit1 = _mm_set1_ps(c.m_lowerLimit); - __m128 upperLimit1 = _mm_set1_ps(c.m_upperLimit); - __m128 deltaImpulse = _mm_sub_ps(_mm_set1_ps(c.m_rhsPenetration), _mm_mul_ps(_mm_set1_ps(c.m_appliedPushImpulse),_mm_set1_ps(c.m_cfm))); - __m128 deltaVel1Dotn = _mm_add_ps(_vmathVfDot3(c.m_contactNormal.mVec128,body1.internalGetPushVelocity().mVec128), _vmathVfDot3(c.m_relpos1CrossNormal.mVec128,body1.internalGetTurnVelocity().mVec128)); - __m128 deltaVel2Dotn = _mm_sub_ps(_vmathVfDot3(c.m_relpos2CrossNormal.mVec128,body2.internalGetTurnVelocity().mVec128),_vmathVfDot3((c.m_contactNormal).mVec128,body2.internalGetPushVelocity().mVec128)); - deltaImpulse = _mm_sub_ps(deltaImpulse,_mm_mul_ps(deltaVel1Dotn,_mm_set1_ps(c.m_jacDiagABInv))); - deltaImpulse = _mm_sub_ps(deltaImpulse,_mm_mul_ps(deltaVel2Dotn,_mm_set1_ps(c.m_jacDiagABInv))); - btSimdScalar sum = _mm_add_ps(cpAppliedImp,deltaImpulse); - btSimdScalar resultLowerLess,resultUpperLess; - resultLowerLess = _mm_cmplt_ps(sum,lowerLimit1); - resultUpperLess = _mm_cmplt_ps(sum,upperLimit1); - __m128 lowMinApplied = _mm_sub_ps(lowerLimit1,cpAppliedImp); - deltaImpulse = _mm_or_ps( _mm_and_ps(resultLowerLess, lowMinApplied), _mm_andnot_ps(resultLowerLess, deltaImpulse) ); - c.m_appliedImpulse = _mm_or_ps( _mm_and_ps(resultLowerLess, lowerLimit1), _mm_andnot_ps(resultLowerLess, sum) ); - __m128 linearComponentA = _mm_mul_ps(c.m_contactNormal.mVec128,body1.internalGetInvMass().mVec128); - __m128 linearComponentB = _mm_mul_ps((c.m_contactNormal).mVec128,body2.internalGetInvMass().mVec128); - __m128 impulseMagnitude = deltaImpulse; - body1.internalGetPushVelocity().mVec128 = _mm_add_ps(body1.internalGetPushVelocity().mVec128,_mm_mul_ps(linearComponentA,impulseMagnitude)); - body1.internalGetTurnVelocity().mVec128 = _mm_add_ps(body1.internalGetTurnVelocity().mVec128 ,_mm_mul_ps(c.m_angularComponentA.mVec128,impulseMagnitude)); - body2.internalGetPushVelocity().mVec128 = _mm_sub_ps(body2.internalGetPushVelocity().mVec128,_mm_mul_ps(linearComponentB,impulseMagnitude)); - body2.internalGetTurnVelocity().mVec128 = _mm_add_ps(body2.internalGetTurnVelocity().mVec128 ,_mm_mul_ps(c.m_angularComponentB.mVec128,impulseMagnitude)); -#else - resolveSplitPenetrationImpulseCacheFriendly(body1,body2,c); -#endif -} - - - -unsigned long btSequentialImpulseConstraintSolver::btRand2() -{ - m_btSeed2 = (1664525L*m_btSeed2 + 1013904223L) & 0xffffffff; - return m_btSeed2; -} - - - -//See ODE: adam's all-int straightforward(?) dRandInt (0..n-1) -int btSequentialImpulseConstraintSolver::btRandInt2 (int n) -{ - // seems good; xor-fold and modulus - const unsigned long un = static_cast<unsigned long>(n); - unsigned long r = btRand2(); - - // note: probably more aggressive than it needs to be -- might be - // able to get away without one or two of the innermost branches. - if (un <= 0x00010000UL) { - r ^= (r >> 16); - if (un <= 0x00000100UL) { - r ^= (r >> 8); - if (un <= 0x00000010UL) { - r ^= (r >> 4); - if (un <= 0x00000004UL) { - r ^= (r >> 2); - if (un <= 0x00000002UL) { - r ^= (r >> 1); - } - } - } - } - } - - return (int) (r % un); -} - - -#if 0 -void btSequentialImpulseConstraintSolver::initSolverBody(btSolverBody* solverBody, btCollisionObject* collisionObject) -{ - btRigidBody* rb = collisionObject? btRigidBody::upcast(collisionObject) : 0; - - solverBody->internalGetDeltaLinearVelocity().setValue(0.f,0.f,0.f); - solverBody->internalGetDeltaAngularVelocity().setValue(0.f,0.f,0.f); - solverBody->internalGetPushVelocity().setValue(0.f,0.f,0.f); - solverBody->internalGetTurnVelocity().setValue(0.f,0.f,0.f); - - if (rb) - { - solverBody->internalGetInvMass() = btVector3(rb->getInvMass(),rb->getInvMass(),rb->getInvMass())*rb->getLinearFactor(); - solverBody->m_originalBody = rb; - solverBody->m_angularFactor = rb->getAngularFactor(); - } else - { - solverBody->internalGetInvMass().setValue(0,0,0); - solverBody->m_originalBody = 0; - solverBody->m_angularFactor.setValue(1,1,1); - } -} -#endif - - - - - -btScalar btSequentialImpulseConstraintSolver::restitutionCurve(btScalar rel_vel, btScalar restitution) -{ - btScalar rest = restitution * -rel_vel; - return rest; -} - - - -void applyAnisotropicFriction(btCollisionObject* colObj,btVector3& frictionDirection); -void applyAnisotropicFriction(btCollisionObject* colObj,btVector3& frictionDirection) -{ - if (colObj && colObj->hasAnisotropicFriction()) - { - // transform to local coordinates - btVector3 loc_lateral = frictionDirection * colObj->getWorldTransform().getBasis(); - const btVector3& friction_scaling = colObj->getAnisotropicFriction(); - //apply anisotropic friction - loc_lateral *= friction_scaling; - // ... and transform it back to global coordinates - frictionDirection = colObj->getWorldTransform().getBasis() * loc_lateral; - } -} - - -void btSequentialImpulseConstraintSolver::setupFrictionConstraint(btSolverConstraint& solverConstraint, const btVector3& normalAxis,btRigidBody* solverBodyA,btRigidBody* solverBodyB,btManifoldPoint& cp,const btVector3& rel_pos1,const btVector3& rel_pos2,btCollisionObject* colObj0,btCollisionObject* colObj1, btScalar relaxation, btScalar desiredVelocity, btScalar cfmSlip) -{ - - - btRigidBody* body0=btRigidBody::upcast(colObj0); - btRigidBody* body1=btRigidBody::upcast(colObj1); - - solverConstraint.m_contactNormal = normalAxis; - - solverConstraint.m_solverBodyA = body0 ? body0 : &getFixedBody(); - solverConstraint.m_solverBodyB = body1 ? body1 : &getFixedBody(); - - solverConstraint.m_friction = cp.m_combinedFriction; - solverConstraint.m_originalContactPoint = 0; - - solverConstraint.m_appliedImpulse = 0.f; - solverConstraint.m_appliedPushImpulse = 0.f; - - { - btVector3 ftorqueAxis1 = rel_pos1.cross(solverConstraint.m_contactNormal); - solverConstraint.m_relpos1CrossNormal = ftorqueAxis1; - solverConstraint.m_angularComponentA = body0 ? body0->getInvInertiaTensorWorld()*ftorqueAxis1*body0->getAngularFactor() : btVector3(0,0,0); - } - { - btVector3 ftorqueAxis1 = rel_pos2.cross(-solverConstraint.m_contactNormal); - solverConstraint.m_relpos2CrossNormal = ftorqueAxis1; - solverConstraint.m_angularComponentB = body1 ? body1->getInvInertiaTensorWorld()*ftorqueAxis1*body1->getAngularFactor() : btVector3(0,0,0); - } - -#ifdef COMPUTE_IMPULSE_DENOM - btScalar denom0 = rb0->computeImpulseDenominator(pos1,solverConstraint.m_contactNormal); - btScalar denom1 = rb1->computeImpulseDenominator(pos2,solverConstraint.m_contactNormal); -#else - btVector3 vec; - btScalar denom0 = 0.f; - btScalar denom1 = 0.f; - if (body0) - { - vec = ( solverConstraint.m_angularComponentA).cross(rel_pos1); - denom0 = body0->getInvMass() + normalAxis.dot(vec); - } - if (body1) - { - vec = ( -solverConstraint.m_angularComponentB).cross(rel_pos2); - denom1 = body1->getInvMass() + normalAxis.dot(vec); - } - - -#endif //COMPUTE_IMPULSE_DENOM - btScalar denom = relaxation/(denom0+denom1); - solverConstraint.m_jacDiagABInv = denom; - -#ifdef _USE_JACOBIAN - solverConstraint.m_jac = btJacobianEntry ( - rel_pos1,rel_pos2,solverConstraint.m_contactNormal, - body0->getInvInertiaDiagLocal(), - body0->getInvMass(), - body1->getInvInertiaDiagLocal(), - body1->getInvMass()); -#endif //_USE_JACOBIAN - - - { - btScalar rel_vel; - btScalar vel1Dotn = solverConstraint.m_contactNormal.dot(body0?body0->getLinearVelocity():btVector3(0,0,0)) - + solverConstraint.m_relpos1CrossNormal.dot(body0?body0->getAngularVelocity():btVector3(0,0,0)); - btScalar vel2Dotn = -solverConstraint.m_contactNormal.dot(body1?body1->getLinearVelocity():btVector3(0,0,0)) - + solverConstraint.m_relpos2CrossNormal.dot(body1?body1->getAngularVelocity():btVector3(0,0,0)); - - rel_vel = vel1Dotn+vel2Dotn; - -// btScalar positionalError = 0.f; - - btSimdScalar velocityError = desiredVelocity - rel_vel; - btSimdScalar velocityImpulse = velocityError * btSimdScalar(solverConstraint.m_jacDiagABInv); - solverConstraint.m_rhs = velocityImpulse; - solverConstraint.m_cfm = cfmSlip; - solverConstraint.m_lowerLimit = 0; - solverConstraint.m_upperLimit = 1e10f; - } -} - - - -btSolverConstraint& btSequentialImpulseConstraintSolver::addFrictionConstraint(const btVector3& normalAxis,btRigidBody* solverBodyA,btRigidBody* solverBodyB,int frictionIndex,btManifoldPoint& cp,const btVector3& rel_pos1,const btVector3& rel_pos2,btCollisionObject* colObj0,btCollisionObject* colObj1, btScalar relaxation, btScalar desiredVelocity, btScalar cfmSlip) -{ - btSolverConstraint& solverConstraint = m_tmpSolverContactFrictionConstraintPool.expandNonInitializing(); - solverConstraint.m_frictionIndex = frictionIndex; - setupFrictionConstraint(solverConstraint, normalAxis, solverBodyA, solverBodyB, cp, rel_pos1, rel_pos2, - colObj0, colObj1, relaxation, desiredVelocity, cfmSlip); - return solverConstraint; -} - -int btSequentialImpulseConstraintSolver::getOrInitSolverBody(btCollisionObject& body) -{ -#if 0 - int solverBodyIdA = -1; - - if (body.getCompanionId() >= 0) - { - //body has already been converted - solverBodyIdA = body.getCompanionId(); - } else - { - btRigidBody* rb = btRigidBody::upcast(&body); - if (rb && rb->getInvMass()) - { - solverBodyIdA = m_tmpSolverBodyPool.size(); - btSolverBody& solverBody = m_tmpSolverBodyPool.expand(); - initSolverBody(&solverBody,&body); - body.setCompanionId(solverBodyIdA); - } else - { - return 0;//assume first one is a fixed solver body - } - } - return solverBodyIdA; -#endif - return 0; -} -#include <stdio.h> - - -void btSequentialImpulseConstraintSolver::setupContactConstraint(btSolverConstraint& solverConstraint, - btCollisionObject* colObj0, btCollisionObject* colObj1, - btManifoldPoint& cp, const btContactSolverInfo& infoGlobal, - btVector3& vel, btScalar& rel_vel, btScalar& relaxation, - btVector3& rel_pos1, btVector3& rel_pos2) -{ - btRigidBody* rb0 = btRigidBody::upcast(colObj0); - btRigidBody* rb1 = btRigidBody::upcast(colObj1); - - const btVector3& pos1 = cp.getPositionWorldOnA(); - const btVector3& pos2 = cp.getPositionWorldOnB(); - -// btVector3 rel_pos1 = pos1 - colObj0->getWorldTransform().getOrigin(); -// btVector3 rel_pos2 = pos2 - colObj1->getWorldTransform().getOrigin(); - rel_pos1 = pos1 - colObj0->getWorldTransform().getOrigin(); - rel_pos2 = pos2 - colObj1->getWorldTransform().getOrigin(); - - relaxation = 1.f; - - btVector3 torqueAxis0 = rel_pos1.cross(cp.m_normalWorldOnB); - solverConstraint.m_angularComponentA = rb0 ? rb0->getInvInertiaTensorWorld()*torqueAxis0*rb0->getAngularFactor() : btVector3(0,0,0); - btVector3 torqueAxis1 = rel_pos2.cross(cp.m_normalWorldOnB); - solverConstraint.m_angularComponentB = rb1 ? rb1->getInvInertiaTensorWorld()*-torqueAxis1*rb1->getAngularFactor() : btVector3(0,0,0); - - { -#ifdef COMPUTE_IMPULSE_DENOM - btScalar denom0 = rb0->computeImpulseDenominator(pos1,cp.m_normalWorldOnB); - btScalar denom1 = rb1->computeImpulseDenominator(pos2,cp.m_normalWorldOnB); -#else - btVector3 vec; - btScalar denom0 = 0.f; - btScalar denom1 = 0.f; - if (rb0) - { - vec = ( solverConstraint.m_angularComponentA).cross(rel_pos1); - denom0 = rb0->getInvMass() + cp.m_normalWorldOnB.dot(vec); - } - if (rb1) - { - vec = ( -solverConstraint.m_angularComponentB).cross(rel_pos2); - denom1 = rb1->getInvMass() + cp.m_normalWorldOnB.dot(vec); - } -#endif //COMPUTE_IMPULSE_DENOM - - btScalar denom = relaxation/(denom0+denom1); - solverConstraint.m_jacDiagABInv = denom; - } - - solverConstraint.m_contactNormal = cp.m_normalWorldOnB; - solverConstraint.m_relpos1CrossNormal = rel_pos1.cross(cp.m_normalWorldOnB); - solverConstraint.m_relpos2CrossNormal = rel_pos2.cross(-cp.m_normalWorldOnB); - - - - - btVector3 vel1 = rb0 ? rb0->getVelocityInLocalPoint(rel_pos1) : btVector3(0,0,0); - btVector3 vel2 = rb1 ? rb1->getVelocityInLocalPoint(rel_pos2) : btVector3(0,0,0); - vel = vel1 - vel2; - rel_vel = cp.m_normalWorldOnB.dot(vel); - - btScalar penetration = cp.getDistance()+infoGlobal.m_linearSlop; - - - solverConstraint.m_friction = cp.m_combinedFriction; - - btScalar restitution = 0.f; - - if (cp.m_lifeTime>infoGlobal.m_restingContactRestitutionThreshold) - { - restitution = 0.f; - } else - { - restitution = restitutionCurve(rel_vel, cp.m_combinedRestitution); - if (restitution <= btScalar(0.)) - { - restitution = 0.f; - }; - } - - - ///warm starting (or zero if disabled) - if (infoGlobal.m_solverMode & SOLVER_USE_WARMSTARTING) - { - solverConstraint.m_appliedImpulse = cp.m_appliedImpulse * infoGlobal.m_warmstartingFactor; - if (rb0) - rb0->internalApplyImpulse(solverConstraint.m_contactNormal*rb0->getInvMass()*rb0->getLinearFactor(),solverConstraint.m_angularComponentA,solverConstraint.m_appliedImpulse); - if (rb1) - rb1->internalApplyImpulse(solverConstraint.m_contactNormal*rb1->getInvMass()*rb1->getLinearFactor(),-solverConstraint.m_angularComponentB,-solverConstraint.m_appliedImpulse); - } else - { - solverConstraint.m_appliedImpulse = 0.f; - } - - solverConstraint.m_appliedPushImpulse = 0.f; - - { - btScalar rel_vel; - btScalar vel1Dotn = solverConstraint.m_contactNormal.dot(rb0?rb0->getLinearVelocity():btVector3(0,0,0)) - + solverConstraint.m_relpos1CrossNormal.dot(rb0?rb0->getAngularVelocity():btVector3(0,0,0)); - btScalar vel2Dotn = -solverConstraint.m_contactNormal.dot(rb1?rb1->getLinearVelocity():btVector3(0,0,0)) - + solverConstraint.m_relpos2CrossNormal.dot(rb1?rb1->getAngularVelocity():btVector3(0,0,0)); - - rel_vel = vel1Dotn+vel2Dotn; - - btScalar positionalError = 0.f; - positionalError = -penetration * infoGlobal.m_erp/infoGlobal.m_timeStep; - btScalar velocityError = restitution - rel_vel;// * damping; - btScalar penetrationImpulse = positionalError*solverConstraint.m_jacDiagABInv; - btScalar velocityImpulse = velocityError *solverConstraint.m_jacDiagABInv; - if (!infoGlobal.m_splitImpulse || (penetration > infoGlobal.m_splitImpulsePenetrationThreshold)) - { - //combine position and velocity into rhs - solverConstraint.m_rhs = penetrationImpulse+velocityImpulse; - solverConstraint.m_rhsPenetration = 0.f; - } else - { - //split position and velocity into rhs and m_rhsPenetration - solverConstraint.m_rhs = velocityImpulse; - solverConstraint.m_rhsPenetration = penetrationImpulse; - } - solverConstraint.m_cfm = 0.f; - solverConstraint.m_lowerLimit = 0; - solverConstraint.m_upperLimit = 1e10f; - } - - - - -} - - - -void btSequentialImpulseConstraintSolver::setFrictionConstraintImpulse( btSolverConstraint& solverConstraint, - btRigidBody* rb0, btRigidBody* rb1, - btManifoldPoint& cp, const btContactSolverInfo& infoGlobal) -{ - if (infoGlobal.m_solverMode & SOLVER_USE_FRICTION_WARMSTARTING) - { - { - btSolverConstraint& frictionConstraint1 = m_tmpSolverContactFrictionConstraintPool[solverConstraint.m_frictionIndex]; - if (infoGlobal.m_solverMode & SOLVER_USE_WARMSTARTING) - { - frictionConstraint1.m_appliedImpulse = cp.m_appliedImpulseLateral1 * infoGlobal.m_warmstartingFactor; - if (rb0) - rb0->internalApplyImpulse(frictionConstraint1.m_contactNormal*rb0->getInvMass()*rb0->getLinearFactor(),frictionConstraint1.m_angularComponentA,frictionConstraint1.m_appliedImpulse); - if (rb1) - rb1->internalApplyImpulse(frictionConstraint1.m_contactNormal*rb1->getInvMass()*rb1->getLinearFactor(),-frictionConstraint1.m_angularComponentB,-frictionConstraint1.m_appliedImpulse); - } else - { - frictionConstraint1.m_appliedImpulse = 0.f; - } - } - - if ((infoGlobal.m_solverMode & SOLVER_USE_2_FRICTION_DIRECTIONS)) - { - btSolverConstraint& frictionConstraint2 = m_tmpSolverContactFrictionConstraintPool[solverConstraint.m_frictionIndex+1]; - if (infoGlobal.m_solverMode & SOLVER_USE_WARMSTARTING) - { - frictionConstraint2.m_appliedImpulse = cp.m_appliedImpulseLateral2 * infoGlobal.m_warmstartingFactor; - if (rb0) - rb0->internalApplyImpulse(frictionConstraint2.m_contactNormal*rb0->getInvMass(),frictionConstraint2.m_angularComponentA,frictionConstraint2.m_appliedImpulse); - if (rb1) - rb1->internalApplyImpulse(frictionConstraint2.m_contactNormal*rb1->getInvMass(),-frictionConstraint2.m_angularComponentB,-frictionConstraint2.m_appliedImpulse); - } else - { - frictionConstraint2.m_appliedImpulse = 0.f; - } - } - } else - { - btSolverConstraint& frictionConstraint1 = m_tmpSolverContactFrictionConstraintPool[solverConstraint.m_frictionIndex]; - frictionConstraint1.m_appliedImpulse = 0.f; - if ((infoGlobal.m_solverMode & SOLVER_USE_2_FRICTION_DIRECTIONS)) - { - btSolverConstraint& frictionConstraint2 = m_tmpSolverContactFrictionConstraintPool[solverConstraint.m_frictionIndex+1]; - frictionConstraint2.m_appliedImpulse = 0.f; - } - } -} - - - - -void btSequentialImpulseConstraintSolver::convertContact(btPersistentManifold* manifold,const btContactSolverInfo& infoGlobal) -{ - btCollisionObject* colObj0=0,*colObj1=0; - - colObj0 = (btCollisionObject*)manifold->getBody0(); - colObj1 = (btCollisionObject*)manifold->getBody1(); - - - btRigidBody* solverBodyA = btRigidBody::upcast(colObj0); - btRigidBody* solverBodyB = btRigidBody::upcast(colObj1); - - ///avoid collision response between two static objects - if ((!solverBodyA || !solverBodyA->getInvMass()) && (!solverBodyB || !solverBodyB->getInvMass())) - return; - - for (int j=0;j<manifold->getNumContacts();j++) - { - - btManifoldPoint& cp = manifold->getContactPoint(j); - - if (cp.getDistance() <= manifold->getContactProcessingThreshold()) - { - btVector3 rel_pos1; - btVector3 rel_pos2; - btScalar relaxation; - btScalar rel_vel; - btVector3 vel; - - int frictionIndex = m_tmpSolverContactConstraintPool.size(); - btSolverConstraint& solverConstraint = m_tmpSolverContactConstraintPool.expandNonInitializing(); - btRigidBody* rb0 = btRigidBody::upcast(colObj0); - btRigidBody* rb1 = btRigidBody::upcast(colObj1); - solverConstraint.m_solverBodyA = rb0? rb0 : &getFixedBody(); - solverConstraint.m_solverBodyB = rb1? rb1 : &getFixedBody(); - solverConstraint.m_originalContactPoint = &cp; - - setupContactConstraint(solverConstraint, colObj0, colObj1, cp, infoGlobal, vel, rel_vel, relaxation, rel_pos1, rel_pos2); - -// const btVector3& pos1 = cp.getPositionWorldOnA(); -// const btVector3& pos2 = cp.getPositionWorldOnB(); - - /////setup the friction constraints - - solverConstraint.m_frictionIndex = m_tmpSolverContactFrictionConstraintPool.size(); - - if (!(infoGlobal.m_solverMode & SOLVER_ENABLE_FRICTION_DIRECTION_CACHING) || !cp.m_lateralFrictionInitialized) - { - cp.m_lateralFrictionDir1 = vel - cp.m_normalWorldOnB * rel_vel; - btScalar lat_rel_vel = cp.m_lateralFrictionDir1.length2(); - if (!(infoGlobal.m_solverMode & SOLVER_DISABLE_VELOCITY_DEPENDENT_FRICTION_DIRECTION) && lat_rel_vel > SIMD_EPSILON) - { - cp.m_lateralFrictionDir1 /= btSqrt(lat_rel_vel); - if((infoGlobal.m_solverMode & SOLVER_USE_2_FRICTION_DIRECTIONS)) - { - cp.m_lateralFrictionDir2 = cp.m_lateralFrictionDir1.cross(cp.m_normalWorldOnB); - cp.m_lateralFrictionDir2.normalize();//?? - applyAnisotropicFriction(colObj0,cp.m_lateralFrictionDir2); - applyAnisotropicFriction(colObj1,cp.m_lateralFrictionDir2); - addFrictionConstraint(cp.m_lateralFrictionDir2,solverBodyA,solverBodyB,frictionIndex,cp,rel_pos1,rel_pos2,colObj0,colObj1, relaxation); - } - - applyAnisotropicFriction(colObj0,cp.m_lateralFrictionDir1); - applyAnisotropicFriction(colObj1,cp.m_lateralFrictionDir1); - addFrictionConstraint(cp.m_lateralFrictionDir1,solverBodyA,solverBodyB,frictionIndex,cp,rel_pos1,rel_pos2,colObj0,colObj1, relaxation); - cp.m_lateralFrictionInitialized = true; - } else - { - //re-calculate friction direction every frame, todo: check if this is really needed - btPlaneSpace1(cp.m_normalWorldOnB,cp.m_lateralFrictionDir1,cp.m_lateralFrictionDir2); - if ((infoGlobal.m_solverMode & SOLVER_USE_2_FRICTION_DIRECTIONS)) - { - applyAnisotropicFriction(colObj0,cp.m_lateralFrictionDir2); - applyAnisotropicFriction(colObj1,cp.m_lateralFrictionDir2); - addFrictionConstraint(cp.m_lateralFrictionDir2,solverBodyA,solverBodyB,frictionIndex,cp,rel_pos1,rel_pos2,colObj0,colObj1, relaxation); - } - - applyAnisotropicFriction(colObj0,cp.m_lateralFrictionDir1); - applyAnisotropicFriction(colObj1,cp.m_lateralFrictionDir1); - addFrictionConstraint(cp.m_lateralFrictionDir1,solverBodyA,solverBodyB,frictionIndex,cp,rel_pos1,rel_pos2,colObj0,colObj1, relaxation); - - cp.m_lateralFrictionInitialized = true; - } - - } else - { - addFrictionConstraint(cp.m_lateralFrictionDir1,solverBodyA,solverBodyB,frictionIndex,cp,rel_pos1,rel_pos2,colObj0,colObj1, relaxation,cp.m_contactMotion1, cp.m_contactCFM1); - if ((infoGlobal.m_solverMode & SOLVER_USE_2_FRICTION_DIRECTIONS)) - addFrictionConstraint(cp.m_lateralFrictionDir2,solverBodyA,solverBodyB,frictionIndex,cp,rel_pos1,rel_pos2,colObj0,colObj1, relaxation, cp.m_contactMotion2, cp.m_contactCFM2); - } - - setFrictionConstraintImpulse( solverConstraint, rb0, rb1, cp, infoGlobal); - - } - } -} - - -btScalar btSequentialImpulseConstraintSolver::solveGroupCacheFriendlySetup(btCollisionObject** /*bodies */,int /*numBodies */,btPersistentManifold** manifoldPtr, int numManifolds,btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& infoGlobal,btIDebugDraw* debugDrawer,btStackAlloc* stackAlloc) -{ - BT_PROFILE("solveGroupCacheFriendlySetup"); - (void)stackAlloc; - (void)debugDrawer; - - - if (!(numConstraints + numManifolds)) - { - // printf("empty\n"); - return 0.f; - } - - if (1) - { - int j; - for (j=0;j<numConstraints;j++) - { - btTypedConstraint* constraint = constraints[j]; - constraint->buildJacobian(); - } - } - //btRigidBody* rb0=0,*rb1=0; - - //if (1) - { - { - - int totalNumRows = 0; - int i; - - m_tmpConstraintSizesPool.resize(numConstraints); - //calculate the total number of contraint rows - for (i=0;i<numConstraints;i++) - { - btTypedConstraint::btConstraintInfo1& info1 = m_tmpConstraintSizesPool[i]; - constraints[i]->getInfo1(&info1); - totalNumRows += info1.m_numConstraintRows; - } - m_tmpSolverNonContactConstraintPool.resize(totalNumRows); - - - ///setup the btSolverConstraints - int currentRow = 0; - - for (i=0;i<numConstraints;i++) - { - const btTypedConstraint::btConstraintInfo1& info1 = m_tmpConstraintSizesPool[i]; - - if (info1.m_numConstraintRows) - { - btAssert(currentRow<totalNumRows); - - btSolverConstraint* currentConstraintRow = &m_tmpSolverNonContactConstraintPool[currentRow]; - btTypedConstraint* constraint = constraints[i]; - - - - btRigidBody& rbA = constraint->getRigidBodyA(); - btRigidBody& rbB = constraint->getRigidBodyB(); - - - int j; - for ( j=0;j<info1.m_numConstraintRows;j++) - { - memset(¤tConstraintRow[j],0,sizeof(btSolverConstraint)); - currentConstraintRow[j].m_lowerLimit = -FLT_MAX; - currentConstraintRow[j].m_upperLimit = FLT_MAX; - currentConstraintRow[j].m_appliedImpulse = 0.f; - currentConstraintRow[j].m_appliedPushImpulse = 0.f; - currentConstraintRow[j].m_solverBodyA = &rbA; - currentConstraintRow[j].m_solverBodyB = &rbB; - } - - rbA.internalGetDeltaLinearVelocity().setValue(0.f,0.f,0.f); - rbA.internalGetDeltaAngularVelocity().setValue(0.f,0.f,0.f); - rbB.internalGetDeltaLinearVelocity().setValue(0.f,0.f,0.f); - rbB.internalGetDeltaAngularVelocity().setValue(0.f,0.f,0.f); - - - - btTypedConstraint::btConstraintInfo2 info2; - info2.fps = 1.f/infoGlobal.m_timeStep; - info2.erp = infoGlobal.m_erp; - info2.m_J1linearAxis = currentConstraintRow->m_contactNormal; - info2.m_J1angularAxis = currentConstraintRow->m_relpos1CrossNormal; - info2.m_J2linearAxis = 0; - info2.m_J2angularAxis = currentConstraintRow->m_relpos2CrossNormal; - info2.rowskip = sizeof(btSolverConstraint)/sizeof(btScalar);//check this - ///the size of btSolverConstraint needs be a multiple of btScalar - btAssert(info2.rowskip*sizeof(btScalar)== sizeof(btSolverConstraint)); - info2.m_constraintError = ¤tConstraintRow->m_rhs; - currentConstraintRow->m_cfm = infoGlobal.m_globalCfm; - info2.cfm = ¤tConstraintRow->m_cfm; - info2.m_lowerLimit = ¤tConstraintRow->m_lowerLimit; - info2.m_upperLimit = ¤tConstraintRow->m_upperLimit; - info2.m_numIterations = infoGlobal.m_numIterations; - constraints[i]->getInfo2(&info2); - - ///finalize the constraint setup - for ( j=0;j<info1.m_numConstraintRows;j++) - { - btSolverConstraint& solverConstraint = currentConstraintRow[j]; - solverConstraint.m_originalContactPoint = constraint; - - { - const btVector3& ftorqueAxis1 = solverConstraint.m_relpos1CrossNormal; - solverConstraint.m_angularComponentA = constraint->getRigidBodyA().getInvInertiaTensorWorld()*ftorqueAxis1*constraint->getRigidBodyA().getAngularFactor(); - } - { - const btVector3& ftorqueAxis2 = solverConstraint.m_relpos2CrossNormal; - solverConstraint.m_angularComponentB = constraint->getRigidBodyB().getInvInertiaTensorWorld()*ftorqueAxis2*constraint->getRigidBodyB().getAngularFactor(); - } - - { - btVector3 iMJlA = solverConstraint.m_contactNormal*rbA.getInvMass(); - btVector3 iMJaA = rbA.getInvInertiaTensorWorld()*solverConstraint.m_relpos1CrossNormal; - btVector3 iMJlB = solverConstraint.m_contactNormal*rbB.getInvMass();//sign of normal? - btVector3 iMJaB = rbB.getInvInertiaTensorWorld()*solverConstraint.m_relpos2CrossNormal; - - btScalar sum = iMJlA.dot(solverConstraint.m_contactNormal); - sum += iMJaA.dot(solverConstraint.m_relpos1CrossNormal); - sum += iMJlB.dot(solverConstraint.m_contactNormal); - sum += iMJaB.dot(solverConstraint.m_relpos2CrossNormal); - - solverConstraint.m_jacDiagABInv = btScalar(1.)/sum; - } - - - ///fix rhs - ///todo: add force/torque accelerators - { - btScalar rel_vel; - btScalar vel1Dotn = solverConstraint.m_contactNormal.dot(rbA.getLinearVelocity()) + solverConstraint.m_relpos1CrossNormal.dot(rbA.getAngularVelocity()); - btScalar vel2Dotn = -solverConstraint.m_contactNormal.dot(rbB.getLinearVelocity()) + solverConstraint.m_relpos2CrossNormal.dot(rbB.getAngularVelocity()); - - rel_vel = vel1Dotn+vel2Dotn; - - btScalar restitution = 0.f; - btScalar positionalError = solverConstraint.m_rhs;//already filled in by getConstraintInfo2 - btScalar velocityError = restitution - rel_vel;// * damping; - btScalar penetrationImpulse = positionalError*solverConstraint.m_jacDiagABInv; - btScalar velocityImpulse = velocityError *solverConstraint.m_jacDiagABInv; - solverConstraint.m_rhs = penetrationImpulse+velocityImpulse; - solverConstraint.m_appliedImpulse = 0.f; - - } - } - } - currentRow+=m_tmpConstraintSizesPool[i].m_numConstraintRows; - } - } - - { - int i; - btPersistentManifold* manifold = 0; -// btCollisionObject* colObj0=0,*colObj1=0; - - - for (i=0;i<numManifolds;i++) - { - manifold = manifoldPtr[i]; - convertContact(manifold,infoGlobal); - } - } - } - - btContactSolverInfo info = infoGlobal; - - - - int numConstraintPool = m_tmpSolverContactConstraintPool.size(); - int numFrictionPool = m_tmpSolverContactFrictionConstraintPool.size(); - - ///@todo: use stack allocator for such temporarily memory, same for solver bodies/constraints - m_orderTmpConstraintPool.resize(numConstraintPool); - m_orderFrictionConstraintPool.resize(numFrictionPool); - { - int i; - for (i=0;i<numConstraintPool;i++) - { - m_orderTmpConstraintPool[i] = i; - } - for (i=0;i<numFrictionPool;i++) - { - m_orderFrictionConstraintPool[i] = i; - } - } - - return 0.f; - -} - -btScalar btSequentialImpulseConstraintSolver::solveSingleIteration(int iteration, btCollisionObject** /*bodies */,int /*numBodies*/,btPersistentManifold** /*manifoldPtr*/, int /*numManifolds*/,btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& infoGlobal,btIDebugDraw* /*debugDrawer*/,btStackAlloc* /*stackAlloc*/) -{ - - int numConstraintPool = m_tmpSolverContactConstraintPool.size(); - int numFrictionPool = m_tmpSolverContactFrictionConstraintPool.size(); - - int j; - - if (infoGlobal.m_solverMode & SOLVER_RANDMIZE_ORDER) - { - if ((iteration & 7) == 0) { - for (j=0; j<numConstraintPool; ++j) { - int tmp = m_orderTmpConstraintPool[j]; - int swapi = btRandInt2(j+1); - m_orderTmpConstraintPool[j] = m_orderTmpConstraintPool[swapi]; - m_orderTmpConstraintPool[swapi] = tmp; - } - - for (j=0; j<numFrictionPool; ++j) { - int tmp = m_orderFrictionConstraintPool[j]; - int swapi = btRandInt2(j+1); - m_orderFrictionConstraintPool[j] = m_orderFrictionConstraintPool[swapi]; - m_orderFrictionConstraintPool[swapi] = tmp; - } - } - } - - if (infoGlobal.m_solverMode & SOLVER_SIMD) - { - ///solve all joint constraints, using SIMD, if available - for (j=0;j<m_tmpSolverNonContactConstraintPool.size();j++) - { - btSolverConstraint& constraint = m_tmpSolverNonContactConstraintPool[j]; - resolveSingleConstraintRowGenericSIMD(*constraint.m_solverBodyA,*constraint.m_solverBodyB,constraint); - } - - for (j=0;j<numConstraints;j++) - { - constraints[j]->solveConstraintObsolete(constraints[j]->getRigidBodyA(),constraints[j]->getRigidBodyB(),infoGlobal.m_timeStep); - } - - ///solve all contact constraints using SIMD, if available - int numPoolConstraints = m_tmpSolverContactConstraintPool.size(); - for (j=0;j<numPoolConstraints;j++) - { - const btSolverConstraint& solveManifold = m_tmpSolverContactConstraintPool[m_orderTmpConstraintPool[j]]; - resolveSingleConstraintRowLowerLimitSIMD(*solveManifold.m_solverBodyA,*solveManifold.m_solverBodyB,solveManifold); - - } - ///solve all friction constraints, using SIMD, if available - int numFrictionPoolConstraints = m_tmpSolverContactFrictionConstraintPool.size(); - for (j=0;j<numFrictionPoolConstraints;j++) - { - btSolverConstraint& solveManifold = m_tmpSolverContactFrictionConstraintPool[m_orderFrictionConstraintPool[j]]; - btScalar totalImpulse = m_tmpSolverContactConstraintPool[solveManifold.m_frictionIndex].m_appliedImpulse; - - if (totalImpulse>btScalar(0)) - { - solveManifold.m_lowerLimit = -(solveManifold.m_friction*totalImpulse); - solveManifold.m_upperLimit = solveManifold.m_friction*totalImpulse; - - resolveSingleConstraintRowGenericSIMD(*solveManifold.m_solverBodyA, *solveManifold.m_solverBodyB,solveManifold); - } - } - } else - { - - ///solve all joint constraints - for (j=0;j<m_tmpSolverNonContactConstraintPool.size();j++) - { - btSolverConstraint& constraint = m_tmpSolverNonContactConstraintPool[j]; - resolveSingleConstraintRowGeneric(*constraint.m_solverBodyA,*constraint.m_solverBodyB,constraint); - } - - for (j=0;j<numConstraints;j++) - { - constraints[j]->solveConstraintObsolete(constraints[j]->getRigidBodyA(),constraints[j]->getRigidBodyB(),infoGlobal.m_timeStep); - } - ///solve all contact constraints - int numPoolConstraints = m_tmpSolverContactConstraintPool.size(); - for (j=0;j<numPoolConstraints;j++) - { - const btSolverConstraint& solveManifold = m_tmpSolverContactConstraintPool[m_orderTmpConstraintPool[j]]; - resolveSingleConstraintRowLowerLimit(*solveManifold.m_solverBodyA,*solveManifold.m_solverBodyB,solveManifold); - } - ///solve all friction constraints - int numFrictionPoolConstraints = m_tmpSolverContactFrictionConstraintPool.size(); - for (j=0;j<numFrictionPoolConstraints;j++) - { - btSolverConstraint& solveManifold = m_tmpSolverContactFrictionConstraintPool[m_orderFrictionConstraintPool[j]]; - btScalar totalImpulse = m_tmpSolverContactConstraintPool[solveManifold.m_frictionIndex].m_appliedImpulse; - - if (totalImpulse>btScalar(0)) - { - solveManifold.m_lowerLimit = -(solveManifold.m_friction*totalImpulse); - solveManifold.m_upperLimit = solveManifold.m_friction*totalImpulse; - - resolveSingleConstraintRowGeneric(*solveManifold.m_solverBodyA,*solveManifold.m_solverBodyB,solveManifold); - } - } - } - return 0.f; -} - - -void btSequentialImpulseConstraintSolver::solveGroupCacheFriendlySplitImpulseIterations(btCollisionObject** bodies,int numBodies,btPersistentManifold** manifoldPtr, int numManifolds,btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& infoGlobal,btIDebugDraw* debugDrawer,btStackAlloc* stackAlloc) -{ - int iteration; - if (infoGlobal.m_splitImpulse) - { - if (infoGlobal.m_solverMode & SOLVER_SIMD) - { - for ( iteration = 0;iteration<infoGlobal.m_numIterations;iteration++) - { - { - int numPoolConstraints = m_tmpSolverContactConstraintPool.size(); - int j; - for (j=0;j<numPoolConstraints;j++) - { - const btSolverConstraint& solveManifold = m_tmpSolverContactConstraintPool[m_orderTmpConstraintPool[j]]; - - resolveSplitPenetrationSIMD(*solveManifold.m_solverBodyA,*solveManifold.m_solverBodyB,solveManifold); - } - } - } - } - else - { - for ( iteration = 0;iteration<infoGlobal.m_numIterations;iteration++) - { - { - int numPoolConstraints = m_tmpSolverContactConstraintPool.size(); - int j; - for (j=0;j<numPoolConstraints;j++) - { - const btSolverConstraint& solveManifold = m_tmpSolverContactConstraintPool[m_orderTmpConstraintPool[j]]; - - resolveSplitPenetrationImpulseCacheFriendly(*solveManifold.m_solverBodyA,*solveManifold.m_solverBodyB,solveManifold); - } - } - } - } - } -} - -btScalar btSequentialImpulseConstraintSolver::solveGroupCacheFriendlyIterations(btCollisionObject** bodies ,int numBodies,btPersistentManifold** manifoldPtr, int numManifolds,btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& infoGlobal,btIDebugDraw* debugDrawer,btStackAlloc* stackAlloc) -{ - BT_PROFILE("solveGroupCacheFriendlyIterations"); - - - //should traverse the contacts random order... - int iteration; - { - for ( iteration = 0;iteration<infoGlobal.m_numIterations;iteration++) - { - solveSingleIteration(iteration, bodies ,numBodies,manifoldPtr, numManifolds,constraints,numConstraints,infoGlobal,debugDrawer,stackAlloc); - } - - solveGroupCacheFriendlySplitImpulseIterations(bodies ,numBodies,manifoldPtr, numManifolds,constraints,numConstraints,infoGlobal,debugDrawer,stackAlloc); - } - return 0.f; -} - -btScalar btSequentialImpulseConstraintSolver::solveGroupCacheFriendlyFinish(btCollisionObject** bodies ,int numBodies,btPersistentManifold** /*manifoldPtr*/, int /*numManifolds*/,btTypedConstraint** /*constraints*/,int /* numConstraints*/,const btContactSolverInfo& infoGlobal,btIDebugDraw* /*debugDrawer*/,btStackAlloc* /*stackAlloc*/) -{ - int numPoolConstraints = m_tmpSolverContactConstraintPool.size(); - int i,j; - - for (j=0;j<numPoolConstraints;j++) - { - - const btSolverConstraint& solveManifold = m_tmpSolverContactConstraintPool[j]; - btManifoldPoint* pt = (btManifoldPoint*) solveManifold.m_originalContactPoint; - btAssert(pt); - pt->m_appliedImpulse = solveManifold.m_appliedImpulse; - if (infoGlobal.m_solverMode & SOLVER_USE_FRICTION_WARMSTARTING) - { - pt->m_appliedImpulseLateral1 = m_tmpSolverContactFrictionConstraintPool[solveManifold.m_frictionIndex].m_appliedImpulse; - pt->m_appliedImpulseLateral2 = m_tmpSolverContactFrictionConstraintPool[solveManifold.m_frictionIndex+1].m_appliedImpulse; - } - - //do a callback here? - } - - numPoolConstraints = m_tmpSolverNonContactConstraintPool.size(); - for (j=0;j<numPoolConstraints;j++) - { - const btSolverConstraint& solverConstr = m_tmpSolverNonContactConstraintPool[j]; - btTypedConstraint* constr = (btTypedConstraint*)solverConstr.m_originalContactPoint; - btScalar sum = constr->internalGetAppliedImpulse(); - sum += solverConstr.m_appliedImpulse; - constr->internalSetAppliedImpulse(sum); - } - - - if (infoGlobal.m_splitImpulse) - { - for ( i=0;i<numBodies;i++) - { - btRigidBody* body = btRigidBody::upcast(bodies[i]); - if (body) - body->internalWritebackVelocity(infoGlobal.m_timeStep); - } - } else - { - for ( i=0;i<numBodies;i++) - { - btRigidBody* body = btRigidBody::upcast(bodies[i]); - if (body) - body->internalWritebackVelocity(); - } - } - - - m_tmpSolverContactConstraintPool.resize(0); - m_tmpSolverNonContactConstraintPool.resize(0); - m_tmpSolverContactFrictionConstraintPool.resize(0); - - return 0.f; -} - - - -/// btSequentialImpulseConstraintSolver Sequentially applies impulses -btScalar btSequentialImpulseConstraintSolver::solveGroup(btCollisionObject** bodies,int numBodies,btPersistentManifold** manifoldPtr, int numManifolds,btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& infoGlobal,btIDebugDraw* debugDrawer,btStackAlloc* stackAlloc,btDispatcher* /*dispatcher*/) -{ - - BT_PROFILE("solveGroup"); - //you need to provide at least some bodies - btAssert(bodies); - btAssert(numBodies); - - solveGroupCacheFriendlySetup( bodies, numBodies, manifoldPtr, numManifolds,constraints, numConstraints,infoGlobal,debugDrawer, stackAlloc); - - solveGroupCacheFriendlyIterations(bodies, numBodies, manifoldPtr, numManifolds,constraints, numConstraints,infoGlobal,debugDrawer, stackAlloc); - - solveGroupCacheFriendlyFinish(bodies, numBodies, manifoldPtr, numManifolds,constraints, numConstraints,infoGlobal,debugDrawer, stackAlloc); - - return 0.f; -} - -void btSequentialImpulseConstraintSolver::reset() -{ - m_btSeed2 = 0; -} - - diff --git a/extern/bullet2/BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolver.h b/extern/bullet2/BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolver.h deleted file mode 100644 index a26fbad787b..00000000000 --- a/extern/bullet2/BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolver.h +++ /dev/null @@ -1,132 +0,0 @@ -/* -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. -*/ - -#ifndef SEQUENTIAL_IMPULSE_CONSTRAINT_SOLVER_H -#define SEQUENTIAL_IMPULSE_CONSTRAINT_SOLVER_H - -#include "btConstraintSolver.h" -class btIDebugDraw; -#include "btContactConstraint.h" -#include "btSolverBody.h" -#include "btSolverConstraint.h" -#include "btTypedConstraint.h" -#include "BulletCollision/NarrowPhaseCollision/btManifoldPoint.h" - -///The btSequentialImpulseConstraintSolver is a fast SIMD implementation of the Projected Gauss Seidel (iterative LCP) method. -class btSequentialImpulseConstraintSolver : public btConstraintSolver -{ -protected: - - btConstraintArray m_tmpSolverContactConstraintPool; - btConstraintArray m_tmpSolverNonContactConstraintPool; - btConstraintArray m_tmpSolverContactFrictionConstraintPool; - btAlignedObjectArray<int> m_orderTmpConstraintPool; - btAlignedObjectArray<int> m_orderFrictionConstraintPool; - btAlignedObjectArray<btTypedConstraint::btConstraintInfo1> m_tmpConstraintSizesPool; - - void setupFrictionConstraint( btSolverConstraint& solverConstraint, const btVector3& normalAxis,btRigidBody* solverBodyA,btRigidBody* solverBodyIdB, - btManifoldPoint& cp,const btVector3& rel_pos1,const btVector3& rel_pos2, - btCollisionObject* colObj0,btCollisionObject* colObj1, btScalar relaxation, - btScalar desiredVelocity=0., btScalar cfmSlip=0.); - - btSolverConstraint& addFrictionConstraint(const btVector3& normalAxis,btRigidBody* solverBodyA,btRigidBody* solverBodyB,int frictionIndex,btManifoldPoint& cp,const btVector3& rel_pos1,const btVector3& rel_pos2,btCollisionObject* colObj0,btCollisionObject* colObj1, btScalar relaxation, btScalar desiredVelocity=0., btScalar cfmSlip=0.); - - void setupContactConstraint(btSolverConstraint& solverConstraint, btCollisionObject* colObj0, btCollisionObject* colObj1, btManifoldPoint& cp, - const btContactSolverInfo& infoGlobal, btVector3& vel, btScalar& rel_vel, btScalar& relaxation, - btVector3& rel_pos1, btVector3& rel_pos2); - - void setFrictionConstraintImpulse( btSolverConstraint& solverConstraint, btRigidBody* rb0, btRigidBody* rb1, - btManifoldPoint& cp, const btContactSolverInfo& infoGlobal); - - ///m_btSeed2 is used for re-arranging the constraint rows. improves convergence/quality of friction - unsigned long m_btSeed2; - -// void initSolverBody(btSolverBody* solverBody, btCollisionObject* collisionObject); - btScalar restitutionCurve(btScalar rel_vel, btScalar restitution); - - void convertContact(btPersistentManifold* manifold,const btContactSolverInfo& infoGlobal); - - - void resolveSplitPenetrationSIMD( - btRigidBody& body1, - btRigidBody& body2, - const btSolverConstraint& contactConstraint); - - void resolveSplitPenetrationImpulseCacheFriendly( - btRigidBody& body1, - btRigidBody& body2, - const btSolverConstraint& contactConstraint); - - //internal method - int getOrInitSolverBody(btCollisionObject& body); - - void resolveSingleConstraintRowGeneric(btRigidBody& body1,btRigidBody& body2,const btSolverConstraint& contactConstraint); - - void resolveSingleConstraintRowGenericSIMD(btRigidBody& body1,btRigidBody& body2,const btSolverConstraint& contactConstraint); - - void resolveSingleConstraintRowLowerLimit(btRigidBody& body1,btRigidBody& body2,const btSolverConstraint& contactConstraint); - - void resolveSingleConstraintRowLowerLimitSIMD(btRigidBody& body1,btRigidBody& body2,const btSolverConstraint& contactConstraint); - -protected: - static btRigidBody& getFixedBody() - { - static btRigidBody s_fixed(0, 0,0); - s_fixed.setMassProps(btScalar(0.),btVector3(btScalar(0.),btScalar(0.),btScalar(0.))); - return s_fixed; - } - virtual void solveGroupCacheFriendlySplitImpulseIterations(btCollisionObject** bodies,int numBodies,btPersistentManifold** manifoldPtr, int numManifolds,btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& infoGlobal,btIDebugDraw* debugDrawer,btStackAlloc* stackAlloc); - virtual btScalar solveGroupCacheFriendlyFinish(btCollisionObject** bodies ,int numBodies,btPersistentManifold** manifoldPtr, int numManifolds,btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& infoGlobal,btIDebugDraw* debugDrawer,btStackAlloc* stackAlloc); - btScalar solveSingleIteration(int iteration, btCollisionObject** bodies ,int numBodies,btPersistentManifold** manifoldPtr, int numManifolds,btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& infoGlobal,btIDebugDraw* debugDrawer,btStackAlloc* stackAlloc); - - virtual btScalar solveGroupCacheFriendlySetup(btCollisionObject** bodies,int numBodies,btPersistentManifold** manifoldPtr, int numManifolds,btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& infoGlobal,btIDebugDraw* debugDrawer,btStackAlloc* stackAlloc); - virtual btScalar solveGroupCacheFriendlyIterations(btCollisionObject** bodies,int numBodies,btPersistentManifold** manifoldPtr, int numManifolds,btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& infoGlobal,btIDebugDraw* debugDrawer,btStackAlloc* stackAlloc); - - -public: - - - btSequentialImpulseConstraintSolver(); - virtual ~btSequentialImpulseConstraintSolver(); - - virtual btScalar solveGroup(btCollisionObject** bodies,int numBodies,btPersistentManifold** manifold,int numManifolds,btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& info, btIDebugDraw* debugDrawer, btStackAlloc* stackAlloc,btDispatcher* dispatcher); - - - - ///clear internal cached data and reset random seed - virtual void reset(); - - unsigned long btRand2(); - - int btRandInt2 (int n); - - void setRandSeed(unsigned long seed) - { - m_btSeed2 = seed; - } - unsigned long getRandSeed() const - { - return m_btSeed2; - } - -}; - -#ifndef BT_PREFER_SIMD -typedef btSequentialImpulseConstraintSolver btSequentialImpulseConstraintSolverPrefered; -#endif - - -#endif //SEQUENTIAL_IMPULSE_CONSTRAINT_SOLVER_H - diff --git a/extern/bullet2/BulletDynamics/ConstraintSolver/btSliderConstraint.cpp b/extern/bullet2/BulletDynamics/ConstraintSolver/btSliderConstraint.cpp deleted file mode 100644 index b69f46da1b4..00000000000 --- a/extern/bullet2/BulletDynamics/ConstraintSolver/btSliderConstraint.cpp +++ /dev/null @@ -1,857 +0,0 @@ -/* -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. -*/ - -/* -Added by Roman Ponomarev (rponom@gmail.com) -April 04, 2008 -*/ - - - -#include "btSliderConstraint.h" -#include "BulletDynamics/Dynamics/btRigidBody.h" -#include "LinearMath/btTransformUtil.h" -#include <new> - -#define USE_OFFSET_FOR_CONSTANT_FRAME true - -void btSliderConstraint::initParams() -{ - m_lowerLinLimit = btScalar(1.0); - m_upperLinLimit = btScalar(-1.0); - m_lowerAngLimit = btScalar(0.); - m_upperAngLimit = btScalar(0.); - m_softnessDirLin = SLIDER_CONSTRAINT_DEF_SOFTNESS; - m_restitutionDirLin = SLIDER_CONSTRAINT_DEF_RESTITUTION; - m_dampingDirLin = btScalar(0.); - m_cfmDirLin = SLIDER_CONSTRAINT_DEF_CFM; - m_softnessDirAng = SLIDER_CONSTRAINT_DEF_SOFTNESS; - m_restitutionDirAng = SLIDER_CONSTRAINT_DEF_RESTITUTION; - m_dampingDirAng = btScalar(0.); - m_cfmDirAng = SLIDER_CONSTRAINT_DEF_CFM; - m_softnessOrthoLin = SLIDER_CONSTRAINT_DEF_SOFTNESS; - m_restitutionOrthoLin = SLIDER_CONSTRAINT_DEF_RESTITUTION; - m_dampingOrthoLin = SLIDER_CONSTRAINT_DEF_DAMPING; - m_cfmOrthoLin = SLIDER_CONSTRAINT_DEF_CFM; - m_softnessOrthoAng = SLIDER_CONSTRAINT_DEF_SOFTNESS; - m_restitutionOrthoAng = SLIDER_CONSTRAINT_DEF_RESTITUTION; - m_dampingOrthoAng = SLIDER_CONSTRAINT_DEF_DAMPING; - m_cfmOrthoAng = SLIDER_CONSTRAINT_DEF_CFM; - m_softnessLimLin = SLIDER_CONSTRAINT_DEF_SOFTNESS; - m_restitutionLimLin = SLIDER_CONSTRAINT_DEF_RESTITUTION; - m_dampingLimLin = SLIDER_CONSTRAINT_DEF_DAMPING; - m_cfmLimLin = SLIDER_CONSTRAINT_DEF_CFM; - m_softnessLimAng = SLIDER_CONSTRAINT_DEF_SOFTNESS; - m_restitutionLimAng = SLIDER_CONSTRAINT_DEF_RESTITUTION; - m_dampingLimAng = SLIDER_CONSTRAINT_DEF_DAMPING; - m_cfmLimAng = SLIDER_CONSTRAINT_DEF_CFM; - - m_poweredLinMotor = false; - m_targetLinMotorVelocity = btScalar(0.); - m_maxLinMotorForce = btScalar(0.); - m_accumulatedLinMotorImpulse = btScalar(0.0); - - m_poweredAngMotor = false; - m_targetAngMotorVelocity = btScalar(0.); - m_maxAngMotorForce = btScalar(0.); - m_accumulatedAngMotorImpulse = btScalar(0.0); - - m_flags = 0; - m_flags = 0; - - m_useOffsetForConstraintFrame = USE_OFFSET_FOR_CONSTANT_FRAME; - - calculateTransforms(m_rbA.getCenterOfMassTransform(),m_rbB.getCenterOfMassTransform()); -} - - - - - -btSliderConstraint::btSliderConstraint(btRigidBody& rbA, btRigidBody& rbB, const btTransform& frameInA, const btTransform& frameInB, bool useLinearReferenceFrameA) - : btTypedConstraint(SLIDER_CONSTRAINT_TYPE, rbA, rbB), - m_useSolveConstraintObsolete(false), - m_frameInA(frameInA), - m_frameInB(frameInB), - m_useLinearReferenceFrameA(useLinearReferenceFrameA) -{ - initParams(); -} - - - -btSliderConstraint::btSliderConstraint(btRigidBody& rbB, const btTransform& frameInB, bool useLinearReferenceFrameA) - : btTypedConstraint(SLIDER_CONSTRAINT_TYPE, getFixedBody(), rbB), - m_useSolveConstraintObsolete(false), - m_frameInB(frameInB), - m_useLinearReferenceFrameA(useLinearReferenceFrameA) -{ - ///not providing rigidbody A means implicitly using worldspace for body A - m_frameInA = rbB.getCenterOfMassTransform() * m_frameInB; -// m_frameInA.getOrigin() = m_rbA.getCenterOfMassTransform()(m_frameInA.getOrigin()); - - initParams(); -} - - - - - - -void btSliderConstraint::getInfo1(btConstraintInfo1* info) -{ - if (m_useSolveConstraintObsolete) - { - info->m_numConstraintRows = 0; - info->nub = 0; - } - else - { - info->m_numConstraintRows = 4; // Fixed 2 linear + 2 angular - info->nub = 2; - //prepare constraint - calculateTransforms(m_rbA.getCenterOfMassTransform(),m_rbB.getCenterOfMassTransform()); - testAngLimits(); - testLinLimits(); - if(getSolveLinLimit() || getPoweredLinMotor()) - { - info->m_numConstraintRows++; // limit 3rd linear as well - info->nub--; - } - if(getSolveAngLimit() || getPoweredAngMotor()) - { - info->m_numConstraintRows++; // limit 3rd angular as well - info->nub--; - } - } -} - -void btSliderConstraint::getInfo1NonVirtual(btConstraintInfo1* info) -{ - - info->m_numConstraintRows = 6; // Fixed 2 linear + 2 angular + 1 limit (even if not used) - info->nub = 0; -} - -void btSliderConstraint::getInfo2(btConstraintInfo2* info) -{ - getInfo2NonVirtual(info,m_rbA.getCenterOfMassTransform(),m_rbB.getCenterOfMassTransform(), m_rbA.getLinearVelocity(),m_rbB.getLinearVelocity(), m_rbA.getInvMass(),m_rbB.getInvMass()); -} - - - - - - - -void btSliderConstraint::calculateTransforms(const btTransform& transA,const btTransform& transB) -{ - if(m_useLinearReferenceFrameA || (!m_useSolveConstraintObsolete)) - { - m_calculatedTransformA = transA * m_frameInA; - m_calculatedTransformB = transB * m_frameInB; - } - else - { - m_calculatedTransformA = transB * m_frameInB; - m_calculatedTransformB = transA * m_frameInA; - } - m_realPivotAInW = m_calculatedTransformA.getOrigin(); - m_realPivotBInW = m_calculatedTransformB.getOrigin(); - m_sliderAxis = m_calculatedTransformA.getBasis().getColumn(0); // along X - if(m_useLinearReferenceFrameA || m_useSolveConstraintObsolete) - { - m_delta = m_realPivotBInW - m_realPivotAInW; - } - else - { - m_delta = m_realPivotAInW - m_realPivotBInW; - } - m_projPivotInW = m_realPivotAInW + m_sliderAxis.dot(m_delta) * m_sliderAxis; - btVector3 normalWorld; - int i; - //linear part - for(i = 0; i < 3; i++) - { - normalWorld = m_calculatedTransformA.getBasis().getColumn(i); - m_depth[i] = m_delta.dot(normalWorld); - } -} - - - -void btSliderConstraint::testLinLimits(void) -{ - m_solveLinLim = false; - m_linPos = m_depth[0]; - if(m_lowerLinLimit <= m_upperLinLimit) - { - if(m_depth[0] > m_upperLinLimit) - { - m_depth[0] -= m_upperLinLimit; - m_solveLinLim = true; - } - else if(m_depth[0] < m_lowerLinLimit) - { - m_depth[0] -= m_lowerLinLimit; - m_solveLinLim = true; - } - else - { - m_depth[0] = btScalar(0.); - } - } - else - { - m_depth[0] = btScalar(0.); - } -} - - - -void btSliderConstraint::testAngLimits(void) -{ - m_angDepth = btScalar(0.); - m_solveAngLim = false; - if(m_lowerAngLimit <= m_upperAngLimit) - { - const btVector3 axisA0 = m_calculatedTransformA.getBasis().getColumn(1); - const btVector3 axisA1 = m_calculatedTransformA.getBasis().getColumn(2); - const btVector3 axisB0 = m_calculatedTransformB.getBasis().getColumn(1); -// btScalar rot = btAtan2Fast(axisB0.dot(axisA1), axisB0.dot(axisA0)); - btScalar rot = btAtan2(axisB0.dot(axisA1), axisB0.dot(axisA0)); - rot = btAdjustAngleToLimits(rot, m_lowerAngLimit, m_upperAngLimit); - m_angPos = rot; - if(rot < m_lowerAngLimit) - { - m_angDepth = rot - m_lowerAngLimit; - m_solveAngLim = true; - } - else if(rot > m_upperAngLimit) - { - m_angDepth = rot - m_upperAngLimit; - m_solveAngLim = true; - } - } -} - -btVector3 btSliderConstraint::getAncorInA(void) -{ - btVector3 ancorInA; - ancorInA = m_realPivotAInW + (m_lowerLinLimit + m_upperLinLimit) * btScalar(0.5) * m_sliderAxis; - ancorInA = m_rbA.getCenterOfMassTransform().inverse() * ancorInA; - return ancorInA; -} - - - -btVector3 btSliderConstraint::getAncorInB(void) -{ - btVector3 ancorInB; - ancorInB = m_frameInB.getOrigin(); - return ancorInB; -} - - -void btSliderConstraint::getInfo2NonVirtual(btConstraintInfo2* info, const btTransform& transA,const btTransform& transB, const btVector3& linVelA,const btVector3& linVelB, btScalar rbAinvMass,btScalar rbBinvMass ) -{ - const btTransform& trA = getCalculatedTransformA(); - const btTransform& trB = getCalculatedTransformB(); - - btAssert(!m_useSolveConstraintObsolete); - int i, s = info->rowskip; - - btScalar signFact = m_useLinearReferenceFrameA ? btScalar(1.0f) : btScalar(-1.0f); - - // difference between frames in WCS - btVector3 ofs = trB.getOrigin() - trA.getOrigin(); - // now get weight factors depending on masses - btScalar miA = rbAinvMass; - btScalar miB = rbBinvMass; - bool hasStaticBody = (miA < SIMD_EPSILON) || (miB < SIMD_EPSILON); - btScalar miS = miA + miB; - btScalar factA, factB; - if(miS > btScalar(0.f)) - { - factA = miB / miS; - } - else - { - factA = btScalar(0.5f); - } - factB = btScalar(1.0f) - factA; - btVector3 ax1, p, q; - btVector3 ax1A = trA.getBasis().getColumn(0); - btVector3 ax1B = trB.getBasis().getColumn(0); - if(m_useOffsetForConstraintFrame) - { - // get the desired direction of slider axis - // as weighted sum of X-orthos of frameA and frameB in WCS - ax1 = ax1A * factA + ax1B * factB; - ax1.normalize(); - // construct two orthos to slider axis - btPlaneSpace1 (ax1, p, q); - } - else - { // old way - use frameA - ax1 = trA.getBasis().getColumn(0); - // get 2 orthos to slider axis (Y, Z) - p = trA.getBasis().getColumn(1); - q = trA.getBasis().getColumn(2); - } - // make rotations around these orthos equal - // the slider axis should be the only unconstrained - // rotational axis, the angular velocity of the two bodies perpendicular to - // the slider 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 slider axis, and w1 and w2 - // are the angular velocity vectors of the two bodies. - info->m_J1angularAxis[0] = p[0]; - info->m_J1angularAxis[1] = p[1]; - info->m_J1angularAxis[2] = p[2]; - info->m_J1angularAxis[s+0] = q[0]; - info->m_J1angularAxis[s+1] = q[1]; - info->m_J1angularAxis[s+2] = q[2]; - - info->m_J2angularAxis[0] = -p[0]; - info->m_J2angularAxis[1] = -p[1]; - info->m_J2angularAxis[2] = -p[2]; - info->m_J2angularAxis[s+0] = -q[0]; - info->m_J2angularAxis[s+1] = -q[1]; - info->m_J2angularAxis[s+2] = -q[2]; - // compute the right hand side of the constraint equation. set relative - // body velocities along p and q to bring the slider back into alignment. - // if ax1A,ax1B are the unit length slider axes as computed from bodyA and - // bodyB, 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. -// btScalar k = info->fps * info->erp * getSoftnessOrthoAng(); - btScalar currERP = (m_flags & BT_SLIDER_FLAGS_ERP_ORTANG) ? m_softnessOrthoAng : m_softnessOrthoAng * info->erp; - btScalar k = info->fps * currERP; - - btVector3 u = ax1A.cross(ax1B); - info->m_constraintError[0] = k * u.dot(p); - info->m_constraintError[s] = k * u.dot(q); - if(m_flags & BT_SLIDER_FLAGS_CFM_ORTANG) - { - info->cfm[0] = m_cfmOrthoAng; - info->cfm[s] = m_cfmOrthoAng; - } - - int nrow = 1; // last filled row - int srow; - btScalar limit_err; - int limit; - int powered; - - // next two rows. - // we want: velA + wA x relA == velB + wB x relB ... but this would - // result in three equations, so we project along two orthos to the slider axis - - btTransform bodyA_trans = transA; - btTransform bodyB_trans = transB; - nrow++; - int s2 = nrow * s; - nrow++; - int s3 = nrow * s; - btVector3 tmpA(0,0,0), tmpB(0,0,0), relA(0,0,0), relB(0,0,0), c(0,0,0); - if(m_useOffsetForConstraintFrame) - { - // get vector from bodyB to frameB in WCS - relB = trB.getOrigin() - bodyB_trans.getOrigin(); - // get its projection to slider axis - btVector3 projB = ax1 * relB.dot(ax1); - // get vector directed from bodyB to slider axis (and orthogonal to it) - btVector3 orthoB = relB - projB; - // same for bodyA - relA = trA.getOrigin() - bodyA_trans.getOrigin(); - btVector3 projA = ax1 * relA.dot(ax1); - btVector3 orthoA = relA - projA; - // get desired offset between frames A and B along slider axis - btScalar sliderOffs = m_linPos - m_depth[0]; - // desired vector from projection of center of bodyA to projection of center of bodyB to slider axis - btVector3 totalDist = projA + ax1 * sliderOffs - projB; - // get offset vectors relA and relB - relA = orthoA + totalDist * factA; - relB = orthoB - totalDist * factB; - // now choose average ortho to slider axis - p = orthoB * factA + orthoA * factB; - btScalar len2 = p.length2(); - if(len2 > SIMD_EPSILON) - { - p /= btSqrt(len2); - } - else - { - p = trA.getBasis().getColumn(1); - } - // make one more ortho - q = ax1.cross(p); - // fill two rows - tmpA = relA.cross(p); - tmpB = relB.cross(p); - for (i=0; i<3; i++) info->m_J1angularAxis[s2+i] = tmpA[i]; - for (i=0; i<3; i++) info->m_J2angularAxis[s2+i] = -tmpB[i]; - tmpA = relA.cross(q); - tmpB = relB.cross(q); - if(hasStaticBody && getSolveAngLimit()) - { // to make constraint between static and dynamic objects more rigid - // remove wA (or wB) from equation if angular limit is hit - tmpB *= factB; - tmpA *= factA; - } - for (i=0; i<3; i++) info->m_J1angularAxis[s3+i] = tmpA[i]; - for (i=0; i<3; i++) info->m_J2angularAxis[s3+i] = -tmpB[i]; - for (i=0; i<3; i++) info->m_J1linearAxis[s2+i] = p[i]; - for (i=0; i<3; i++) info->m_J1linearAxis[s3+i] = q[i]; - } - else - { // old way - maybe incorrect if bodies are not on the slider axis - // see discussion "Bug in slider constraint" http://bulletphysics.org/Bullet/phpBB3/viewtopic.php?f=9&t=4024&start=0 - c = bodyB_trans.getOrigin() - bodyA_trans.getOrigin(); - btVector3 tmp = c.cross(p); - for (i=0; i<3; i++) info->m_J1angularAxis[s2+i] = factA*tmp[i]; - for (i=0; i<3; i++) info->m_J2angularAxis[s2+i] = factB*tmp[i]; - tmp = c.cross(q); - for (i=0; i<3; i++) info->m_J1angularAxis[s3+i] = factA*tmp[i]; - for (i=0; i<3; i++) info->m_J2angularAxis[s3+i] = factB*tmp[i]; - - for (i=0; i<3; i++) info->m_J1linearAxis[s2+i] = p[i]; - for (i=0; i<3; i++) info->m_J1linearAxis[s3+i] = q[i]; - } - // compute two elements of right hand side - - // k = info->fps * info->erp * getSoftnessOrthoLin(); - currERP = (m_flags & BT_SLIDER_FLAGS_ERP_ORTLIN) ? m_softnessOrthoLin : m_softnessOrthoLin * info->erp; - k = info->fps * currERP; - - btScalar rhs = k * p.dot(ofs); - info->m_constraintError[s2] = rhs; - rhs = k * q.dot(ofs); - info->m_constraintError[s3] = rhs; - if(m_flags & BT_SLIDER_FLAGS_CFM_ORTLIN) - { - info->cfm[s2] = m_cfmOrthoLin; - info->cfm[s3] = m_cfmOrthoLin; - } - - - // check linear limits - limit_err = btScalar(0.0); - limit = 0; - if(getSolveLinLimit()) - { - limit_err = getLinDepth() * signFact; - limit = (limit_err > btScalar(0.0)) ? 2 : 1; - } - powered = 0; - if(getPoweredLinMotor()) - { - powered = 1; - } - // if the slider has joint limits or motor, add in the extra row - if (limit || powered) - { - nrow++; - srow = nrow * info->rowskip; - info->m_J1linearAxis[srow+0] = ax1[0]; - info->m_J1linearAxis[srow+1] = ax1[1]; - info->m_J1linearAxis[srow+2] = ax1[2]; - // linear torque decoupling step: - // - // we have to be careful that the linear constraint forces (+/- ax1) applied to the two bodies - // do not create a torque couple. in other words, the points that the - // constraint force is applied at must lie along the same ax1 axis. - // a torque couple will result in limited slider-jointed free - // bodies from gaining angular momentum. - if(m_useOffsetForConstraintFrame) - { - // this is needed only when bodyA and bodyB are both dynamic. - if(!hasStaticBody) - { - tmpA = relA.cross(ax1); - tmpB = relB.cross(ax1); - info->m_J1angularAxis[srow+0] = tmpA[0]; - info->m_J1angularAxis[srow+1] = tmpA[1]; - info->m_J1angularAxis[srow+2] = tmpA[2]; - info->m_J2angularAxis[srow+0] = -tmpB[0]; - info->m_J2angularAxis[srow+1] = -tmpB[1]; - info->m_J2angularAxis[srow+2] = -tmpB[2]; - } - } - else - { // The old way. May be incorrect if bodies are not on the slider axis - btVector3 ltd; // Linear Torque Decoupling vector (a torque) - ltd = c.cross(ax1); - info->m_J1angularAxis[srow+0] = factA*ltd[0]; - info->m_J1angularAxis[srow+1] = factA*ltd[1]; - info->m_J1angularAxis[srow+2] = factA*ltd[2]; - info->m_J2angularAxis[srow+0] = factB*ltd[0]; - info->m_J2angularAxis[srow+1] = factB*ltd[1]; - info->m_J2angularAxis[srow+2] = factB*ltd[2]; - } - // right-hand part - btScalar lostop = getLowerLinLimit(); - btScalar histop = getUpperLinLimit(); - if(limit && (lostop == histop)) - { // the joint motor is ineffective - powered = 0; - } - info->m_constraintError[srow] = 0.; - info->m_lowerLimit[srow] = 0.; - info->m_upperLimit[srow] = 0.; - currERP = (m_flags & BT_SLIDER_FLAGS_ERP_LIMLIN) ? m_softnessLimLin : info->erp; - if(powered) - { - if(m_flags & BT_SLIDER_FLAGS_CFM_DIRLIN) - { - info->cfm[srow] = m_cfmDirLin; - } - btScalar tag_vel = getTargetLinMotorVelocity(); - btScalar mot_fact = getMotorFactor(m_linPos, m_lowerLinLimit, m_upperLinLimit, tag_vel, info->fps * currERP); - info->m_constraintError[srow] -= signFact * mot_fact * getTargetLinMotorVelocity(); - info->m_lowerLimit[srow] += -getMaxLinMotorForce() * info->fps; - info->m_upperLimit[srow] += getMaxLinMotorForce() * info->fps; - } - if(limit) - { - k = info->fps * currERP; - info->m_constraintError[srow] += k * limit_err; - if(m_flags & BT_SLIDER_FLAGS_CFM_LIMLIN) - { - info->cfm[srow] = m_cfmLimLin; - } - 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] = -SIMD_INFINITY; - info->m_upperLimit[srow] = 0; - } - else - { // high limit - info->m_lowerLimit[srow] = 0; - info->m_upperLimit[srow] = SIMD_INFINITY; - } - // bounce (we'll use slider parameter abs(1.0 - m_dampingLimLin) for that) - btScalar bounce = btFabs(btScalar(1.0) - getDampingLimLin()); - if(bounce > btScalar(0.0)) - { - btScalar vel = linVelA.dot(ax1); - vel -= linVelB.dot(ax1); - vel *= signFact; - // 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] *= getSoftnessLimLin(); - } // if(limit) - } // if linear limit - // check angular limits - limit_err = btScalar(0.0); - limit = 0; - if(getSolveAngLimit()) - { - limit_err = getAngDepth(); - limit = (limit_err > btScalar(0.0)) ? 1 : 2; - } - // if the slider has joint limits, add in the extra row - powered = 0; - if(getPoweredAngMotor()) - { - 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 = getLowerAngLimit(); - btScalar histop = getUpperAngLimit(); - if(limit && (lostop == histop)) - { // the joint motor is ineffective - powered = 0; - } - currERP = (m_flags & BT_SLIDER_FLAGS_ERP_LIMANG) ? m_softnessLimAng : info->erp; - if(powered) - { - if(m_flags & BT_SLIDER_FLAGS_CFM_DIRANG) - { - info->cfm[srow] = m_cfmDirAng; - } - btScalar mot_fact = getMotorFactor(m_angPos, m_lowerAngLimit, m_upperAngLimit, getTargetAngMotorVelocity(), info->fps * currERP); - info->m_constraintError[srow] = mot_fact * getTargetAngMotorVelocity(); - info->m_lowerLimit[srow] = -getMaxAngMotorForce() * info->fps; - info->m_upperLimit[srow] = getMaxAngMotorForce() * info->fps; - } - if(limit) - { - k = info->fps * currERP; - info->m_constraintError[srow] += k * limit_err; - if(m_flags & BT_SLIDER_FLAGS_CFM_LIMANG) - { - info->cfm[srow] = m_cfmLimAng; - } - 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 = btFabs(btScalar(1.0) - getDampingLimAng()); - if(bounce > btScalar(0.0)) - { - btScalar vel = m_rbA.getAngularVelocity().dot(ax1); - vel -= m_rbB.getAngularVelocity().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] *= getSoftnessLimAng(); - } // if(limit) - } // if angular limit or powered -} - - -///override the default global value of a parameter (such as ERP or CFM), optionally provide the axis (0..5). -///If no axis is provided, it uses the default axis for this constraint. -void btSliderConstraint::setParam(int num, btScalar value, int axis) -{ - switch(num) - { - case BT_CONSTRAINT_STOP_ERP : - if(axis < 1) - { - m_softnessLimLin = value; - m_flags |= BT_SLIDER_FLAGS_ERP_LIMLIN; - } - else if(axis < 3) - { - m_softnessOrthoLin = value; - m_flags |= BT_SLIDER_FLAGS_ERP_ORTLIN; - } - else if(axis == 3) - { - m_softnessLimAng = value; - m_flags |= BT_SLIDER_FLAGS_ERP_LIMANG; - } - else if(axis < 6) - { - m_softnessOrthoAng = value; - m_flags |= BT_SLIDER_FLAGS_ERP_ORTANG; - } - else - { - btAssertConstrParams(0); - } - break; - case BT_CONSTRAINT_CFM : - if(axis < 1) - { - m_cfmDirLin = value; - m_flags |= BT_SLIDER_FLAGS_CFM_DIRLIN; - } - else if(axis == 3) - { - m_cfmDirAng = value; - m_flags |= BT_SLIDER_FLAGS_CFM_DIRANG; - } - else - { - btAssertConstrParams(0); - } - break; - case BT_CONSTRAINT_STOP_CFM : - if(axis < 1) - { - m_cfmLimLin = value; - m_flags |= BT_SLIDER_FLAGS_CFM_LIMLIN; - } - else if(axis < 3) - { - m_cfmOrthoLin = value; - m_flags |= BT_SLIDER_FLAGS_CFM_ORTLIN; - } - else if(axis == 3) - { - m_cfmLimAng = value; - m_flags |= BT_SLIDER_FLAGS_CFM_LIMANG; - } - else if(axis < 6) - { - m_cfmOrthoAng = value; - m_flags |= BT_SLIDER_FLAGS_CFM_ORTANG; - } - else - { - btAssertConstrParams(0); - } - break; - } -} - -///return the local value of parameter -btScalar btSliderConstraint::getParam(int num, int axis) const -{ - btScalar retVal(SIMD_INFINITY); - switch(num) - { - case BT_CONSTRAINT_STOP_ERP : - if(axis < 1) - { - btAssertConstrParams(m_flags & BT_SLIDER_FLAGS_ERP_LIMLIN); - retVal = m_softnessLimLin; - } - else if(axis < 3) - { - btAssertConstrParams(m_flags & BT_SLIDER_FLAGS_ERP_ORTLIN); - retVal = m_softnessOrthoLin; - } - else if(axis == 3) - { - btAssertConstrParams(m_flags & BT_SLIDER_FLAGS_ERP_LIMANG); - retVal = m_softnessLimAng; - } - else if(axis < 6) - { - btAssertConstrParams(m_flags & BT_SLIDER_FLAGS_ERP_ORTANG); - retVal = m_softnessOrthoAng; - } - else - { - btAssertConstrParams(0); - } - break; - case BT_CONSTRAINT_CFM : - if(axis < 1) - { - btAssertConstrParams(m_flags & BT_SLIDER_FLAGS_CFM_DIRLIN); - retVal = m_cfmDirLin; - } - else if(axis == 3) - { - btAssertConstrParams(m_flags & BT_SLIDER_FLAGS_CFM_DIRANG); - retVal = m_cfmDirAng; - } - else - { - btAssertConstrParams(0); - } - break; - case BT_CONSTRAINT_STOP_CFM : - if(axis < 1) - { - btAssertConstrParams(m_flags & BT_SLIDER_FLAGS_CFM_LIMLIN); - retVal = m_cfmLimLin; - } - else if(axis < 3) - { - btAssertConstrParams(m_flags & BT_SLIDER_FLAGS_CFM_ORTLIN); - retVal = m_cfmOrthoLin; - } - else if(axis == 3) - { - btAssertConstrParams(m_flags & BT_SLIDER_FLAGS_CFM_LIMANG); - retVal = m_cfmLimAng; - } - else if(axis < 6) - { - btAssertConstrParams(m_flags & BT_SLIDER_FLAGS_CFM_ORTANG); - retVal = m_cfmOrthoAng; - } - else - { - btAssertConstrParams(0); - } - break; - } - return retVal; -} - - - diff --git a/extern/bullet2/BulletDynamics/ConstraintSolver/btSliderConstraint.h b/extern/bullet2/BulletDynamics/ConstraintSolver/btSliderConstraint.h deleted file mode 100644 index 7d2a5022753..00000000000 --- a/extern/bullet2/BulletDynamics/ConstraintSolver/btSliderConstraint.h +++ /dev/null @@ -1,321 +0,0 @@ -/* -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. -*/ - -/* -Added by Roman Ponomarev (rponom@gmail.com) -April 04, 2008 - -TODO: - - add clamping od accumulated impulse to improve stability - - add conversion for ODE constraint solver -*/ - -#ifndef SLIDER_CONSTRAINT_H -#define SLIDER_CONSTRAINT_H - - - -#include "LinearMath/btVector3.h" -#include "btJacobianEntry.h" -#include "btTypedConstraint.h" - - - -class btRigidBody; - - - -#define SLIDER_CONSTRAINT_DEF_SOFTNESS (btScalar(1.0)) -#define SLIDER_CONSTRAINT_DEF_DAMPING (btScalar(1.0)) -#define SLIDER_CONSTRAINT_DEF_RESTITUTION (btScalar(0.7)) -#define SLIDER_CONSTRAINT_DEF_CFM (btScalar(0.f)) - - -enum btSliderFlags -{ - BT_SLIDER_FLAGS_CFM_DIRLIN = (1 << 0), - BT_SLIDER_FLAGS_ERP_DIRLIN = (1 << 1), - BT_SLIDER_FLAGS_CFM_DIRANG = (1 << 2), - BT_SLIDER_FLAGS_ERP_DIRANG = (1 << 3), - BT_SLIDER_FLAGS_CFM_ORTLIN = (1 << 4), - BT_SLIDER_FLAGS_ERP_ORTLIN = (1 << 5), - BT_SLIDER_FLAGS_CFM_ORTANG = (1 << 6), - BT_SLIDER_FLAGS_ERP_ORTANG = (1 << 7), - BT_SLIDER_FLAGS_CFM_LIMLIN = (1 << 8), - BT_SLIDER_FLAGS_ERP_LIMLIN = (1 << 9), - BT_SLIDER_FLAGS_CFM_LIMANG = (1 << 10), - BT_SLIDER_FLAGS_ERP_LIMANG = (1 << 11) -}; - - -class btSliderConstraint : public btTypedConstraint -{ -protected: - ///for backwards compatibility during the transition to 'getInfo/getInfo2' - bool m_useSolveConstraintObsolete; - bool m_useOffsetForConstraintFrame; - btTransform m_frameInA; - btTransform m_frameInB; - // use frameA fo define limits, if true - bool m_useLinearReferenceFrameA; - // linear limits - btScalar m_lowerLinLimit; - btScalar m_upperLinLimit; - // angular limits - btScalar m_lowerAngLimit; - btScalar m_upperAngLimit; - // softness, restitution and damping for different cases - // DirLin - moving inside linear limits - // LimLin - hitting linear limit - // DirAng - moving inside angular limits - // LimAng - hitting angular limit - // OrthoLin, OrthoAng - against constraint axis - btScalar m_softnessDirLin; - btScalar m_restitutionDirLin; - btScalar m_dampingDirLin; - btScalar m_cfmDirLin; - - btScalar m_softnessDirAng; - btScalar m_restitutionDirAng; - btScalar m_dampingDirAng; - btScalar m_cfmDirAng; - - btScalar m_softnessLimLin; - btScalar m_restitutionLimLin; - btScalar m_dampingLimLin; - btScalar m_cfmLimLin; - - btScalar m_softnessLimAng; - btScalar m_restitutionLimAng; - btScalar m_dampingLimAng; - btScalar m_cfmLimAng; - - btScalar m_softnessOrthoLin; - btScalar m_restitutionOrthoLin; - btScalar m_dampingOrthoLin; - btScalar m_cfmOrthoLin; - - btScalar m_softnessOrthoAng; - btScalar m_restitutionOrthoAng; - btScalar m_dampingOrthoAng; - btScalar m_cfmOrthoAng; - - // for interlal use - bool m_solveLinLim; - bool m_solveAngLim; - - int m_flags; - - btJacobianEntry m_jacLin[3]; - btScalar m_jacLinDiagABInv[3]; - - btJacobianEntry m_jacAng[3]; - - btScalar m_timeStep; - btTransform m_calculatedTransformA; - btTransform m_calculatedTransformB; - - btVector3 m_sliderAxis; - btVector3 m_realPivotAInW; - btVector3 m_realPivotBInW; - btVector3 m_projPivotInW; - btVector3 m_delta; - btVector3 m_depth; - btVector3 m_relPosA; - btVector3 m_relPosB; - - btScalar m_linPos; - btScalar m_angPos; - - btScalar m_angDepth; - btScalar m_kAngle; - - bool m_poweredLinMotor; - btScalar m_targetLinMotorVelocity; - btScalar m_maxLinMotorForce; - btScalar m_accumulatedLinMotorImpulse; - - bool m_poweredAngMotor; - btScalar m_targetAngMotorVelocity; - btScalar m_maxAngMotorForce; - btScalar m_accumulatedAngMotorImpulse; - - //------------------------ - void initParams(); -public: - // constructors - btSliderConstraint(btRigidBody& rbA, btRigidBody& rbB, const btTransform& frameInA, const btTransform& frameInB ,bool useLinearReferenceFrameA); - btSliderConstraint(btRigidBody& rbB, const btTransform& frameInB, bool useLinearReferenceFrameA); - - // overrides - - virtual void getInfo1 (btConstraintInfo1* info); - - void getInfo1NonVirtual(btConstraintInfo1* info); - - virtual void getInfo2 (btConstraintInfo2* info); - - void getInfo2NonVirtual(btConstraintInfo2* info, const btTransform& transA, const btTransform& transB,const btVector3& linVelA,const btVector3& linVelB, btScalar rbAinvMass,btScalar rbBinvMass); - - - // access - const btRigidBody& getRigidBodyA() const { return m_rbA; } - const btRigidBody& getRigidBodyB() const { return m_rbB; } - const btTransform & getCalculatedTransformA() const { return m_calculatedTransformA; } - const btTransform & getCalculatedTransformB() const { return m_calculatedTransformB; } - const btTransform & getFrameOffsetA() const { return m_frameInA; } - const btTransform & getFrameOffsetB() const { return m_frameInB; } - btTransform & getFrameOffsetA() { return m_frameInA; } - btTransform & getFrameOffsetB() { return m_frameInB; } - btScalar getLowerLinLimit() { return m_lowerLinLimit; } - void setLowerLinLimit(btScalar lowerLimit) { m_lowerLinLimit = lowerLimit; } - btScalar getUpperLinLimit() { return m_upperLinLimit; } - void setUpperLinLimit(btScalar upperLimit) { m_upperLinLimit = upperLimit; } - btScalar getLowerAngLimit() { return m_lowerAngLimit; } - void setLowerAngLimit(btScalar lowerLimit) { m_lowerAngLimit = btNormalizeAngle(lowerLimit); } - btScalar getUpperAngLimit() { return m_upperAngLimit; } - void setUpperAngLimit(btScalar upperLimit) { m_upperAngLimit = btNormalizeAngle(upperLimit); } - bool getUseLinearReferenceFrameA() { return m_useLinearReferenceFrameA; } - btScalar getSoftnessDirLin() { return m_softnessDirLin; } - btScalar getRestitutionDirLin() { return m_restitutionDirLin; } - btScalar getDampingDirLin() { return m_dampingDirLin ; } - btScalar getSoftnessDirAng() { return m_softnessDirAng; } - btScalar getRestitutionDirAng() { return m_restitutionDirAng; } - btScalar getDampingDirAng() { return m_dampingDirAng; } - btScalar getSoftnessLimLin() { return m_softnessLimLin; } - btScalar getRestitutionLimLin() { return m_restitutionLimLin; } - btScalar getDampingLimLin() { return m_dampingLimLin; } - btScalar getSoftnessLimAng() { return m_softnessLimAng; } - btScalar getRestitutionLimAng() { return m_restitutionLimAng; } - btScalar getDampingLimAng() { return m_dampingLimAng; } - btScalar getSoftnessOrthoLin() { return m_softnessOrthoLin; } - btScalar getRestitutionOrthoLin() { return m_restitutionOrthoLin; } - btScalar getDampingOrthoLin() { return m_dampingOrthoLin; } - btScalar getSoftnessOrthoAng() { return m_softnessOrthoAng; } - btScalar getRestitutionOrthoAng() { return m_restitutionOrthoAng; } - btScalar getDampingOrthoAng() { return m_dampingOrthoAng; } - void setSoftnessDirLin(btScalar softnessDirLin) { m_softnessDirLin = softnessDirLin; } - void setRestitutionDirLin(btScalar restitutionDirLin) { m_restitutionDirLin = restitutionDirLin; } - void setDampingDirLin(btScalar dampingDirLin) { m_dampingDirLin = dampingDirLin; } - void setSoftnessDirAng(btScalar softnessDirAng) { m_softnessDirAng = softnessDirAng; } - void setRestitutionDirAng(btScalar restitutionDirAng) { m_restitutionDirAng = restitutionDirAng; } - void setDampingDirAng(btScalar dampingDirAng) { m_dampingDirAng = dampingDirAng; } - void setSoftnessLimLin(btScalar softnessLimLin) { m_softnessLimLin = softnessLimLin; } - void setRestitutionLimLin(btScalar restitutionLimLin) { m_restitutionLimLin = restitutionLimLin; } - void setDampingLimLin(btScalar dampingLimLin) { m_dampingLimLin = dampingLimLin; } - void setSoftnessLimAng(btScalar softnessLimAng) { m_softnessLimAng = softnessLimAng; } - void setRestitutionLimAng(btScalar restitutionLimAng) { m_restitutionLimAng = restitutionLimAng; } - void setDampingLimAng(btScalar dampingLimAng) { m_dampingLimAng = dampingLimAng; } - void setSoftnessOrthoLin(btScalar softnessOrthoLin) { m_softnessOrthoLin = softnessOrthoLin; } - void setRestitutionOrthoLin(btScalar restitutionOrthoLin) { m_restitutionOrthoLin = restitutionOrthoLin; } - void setDampingOrthoLin(btScalar dampingOrthoLin) { m_dampingOrthoLin = dampingOrthoLin; } - void setSoftnessOrthoAng(btScalar softnessOrthoAng) { m_softnessOrthoAng = softnessOrthoAng; } - void setRestitutionOrthoAng(btScalar restitutionOrthoAng) { m_restitutionOrthoAng = restitutionOrthoAng; } - void setDampingOrthoAng(btScalar dampingOrthoAng) { m_dampingOrthoAng = dampingOrthoAng; } - void setPoweredLinMotor(bool onOff) { m_poweredLinMotor = onOff; } - bool getPoweredLinMotor() { return m_poweredLinMotor; } - void setTargetLinMotorVelocity(btScalar targetLinMotorVelocity) { m_targetLinMotorVelocity = targetLinMotorVelocity; } - btScalar getTargetLinMotorVelocity() { return m_targetLinMotorVelocity; } - void setMaxLinMotorForce(btScalar maxLinMotorForce) { m_maxLinMotorForce = maxLinMotorForce; } - btScalar getMaxLinMotorForce() { return m_maxLinMotorForce; } - void setPoweredAngMotor(bool onOff) { m_poweredAngMotor = onOff; } - bool getPoweredAngMotor() { return m_poweredAngMotor; } - void setTargetAngMotorVelocity(btScalar targetAngMotorVelocity) { m_targetAngMotorVelocity = targetAngMotorVelocity; } - btScalar getTargetAngMotorVelocity() { return m_targetAngMotorVelocity; } - void setMaxAngMotorForce(btScalar maxAngMotorForce) { m_maxAngMotorForce = maxAngMotorForce; } - btScalar getMaxAngMotorForce() { return m_maxAngMotorForce; } - btScalar getLinearPos() { return m_linPos; } - - - // access for ODE solver - bool getSolveLinLimit() { return m_solveLinLim; } - btScalar getLinDepth() { return m_depth[0]; } - bool getSolveAngLimit() { return m_solveAngLim; } - btScalar getAngDepth() { return m_angDepth; } - // shared code used by ODE solver - void calculateTransforms(const btTransform& transA,const btTransform& transB); - void testLinLimits(); - void testAngLimits(); - // access for PE Solver - btVector3 getAncorInA(); - btVector3 getAncorInB(); - // access for UseFrameOffset - bool getUseFrameOffset() { return m_useOffsetForConstraintFrame; } - void setUseFrameOffset(bool frameOffsetOnOff) { m_useOffsetForConstraintFrame = frameOffsetOnOff; } - - ///override the default global value of a parameter (such as ERP or CFM), optionally provide the axis (0..5). - ///If no axis is provided, it uses the default axis for this constraint. - virtual void setParam(int num, btScalar value, int axis = -1); - ///return the local value of parameter - virtual btScalar getParam(int num, int axis = -1) const; - - virtual int calculateSerializeBufferSize() const; - - ///fills the dataBuffer and returns the struct name (and 0 on failure) - virtual const char* serialize(void* dataBuffer, btSerializer* serializer) const; - - -}; - -///do not change those serialization structures, it requires an updated sBulletDNAstr/sBulletDNAstr64 -struct btSliderConstraintData -{ - btTypedConstraintData m_typeConstraintData; - btTransformFloatData m_rbAFrame; // constraint axii. Assumes z is hinge axis. - btTransformFloatData m_rbBFrame; - - float m_linearUpperLimit; - float m_linearLowerLimit; - - float m_angularUpperLimit; - float m_angularLowerLimit; - - int m_useLinearReferenceFrameA; - int m_useOffsetForConstraintFrame; - -}; - - -SIMD_FORCE_INLINE int btSliderConstraint::calculateSerializeBufferSize() const -{ - return sizeof(btSliderConstraintData); -} - - ///fills the dataBuffer and returns the struct name (and 0 on failure) -SIMD_FORCE_INLINE const char* btSliderConstraint::serialize(void* dataBuffer, btSerializer* serializer) const -{ - - btSliderConstraintData* sliderData = (btSliderConstraintData*) dataBuffer; - btTypedConstraint::serialize(&sliderData->m_typeConstraintData,serializer); - - m_frameInA.serializeFloat(sliderData->m_rbAFrame); - m_frameInB.serializeFloat(sliderData->m_rbBFrame); - - sliderData->m_linearUpperLimit = float(m_upperLinLimit); - sliderData->m_linearLowerLimit = float(m_lowerLinLimit); - - sliderData->m_angularUpperLimit = float(m_upperAngLimit); - sliderData->m_angularLowerLimit = float(m_lowerAngLimit); - - sliderData->m_useLinearReferenceFrameA = m_useLinearReferenceFrameA; - sliderData->m_useOffsetForConstraintFrame = m_useOffsetForConstraintFrame; - - return "btSliderConstraintData"; -} - - - -#endif //SLIDER_CONSTRAINT_H - diff --git a/extern/bullet2/BulletDynamics/ConstraintSolver/btSolve2LinearConstraint.cpp b/extern/bullet2/BulletDynamics/ConstraintSolver/btSolve2LinearConstraint.cpp deleted file mode 100644 index 0c7dbd668bb..00000000000 --- a/extern/bullet2/BulletDynamics/ConstraintSolver/btSolve2LinearConstraint.cpp +++ /dev/null @@ -1,255 +0,0 @@ -/* -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 "btSolve2LinearConstraint.h" - -#include "BulletDynamics/Dynamics/btRigidBody.h" -#include "LinearMath/btVector3.h" -#include "btJacobianEntry.h" - - -void btSolve2LinearConstraint::resolveUnilateralPairConstraint( - btRigidBody* body1, - btRigidBody* body2, - - const btMatrix3x3& world2A, - const btMatrix3x3& world2B, - - const btVector3& invInertiaADiag, - const btScalar invMassA, - const btVector3& linvelA,const btVector3& angvelA, - const btVector3& rel_posA1, - const btVector3& invInertiaBDiag, - const btScalar invMassB, - const btVector3& linvelB,const btVector3& angvelB, - const btVector3& rel_posA2, - - btScalar depthA, const btVector3& normalA, - const btVector3& rel_posB1,const btVector3& rel_posB2, - btScalar depthB, const btVector3& normalB, - btScalar& imp0,btScalar& imp1) -{ - (void)linvelA; - (void)linvelB; - (void)angvelB; - (void)angvelA; - - - - imp0 = btScalar(0.); - imp1 = btScalar(0.); - - btScalar len = btFabs(normalA.length()) - btScalar(1.); - if (btFabs(len) >= SIMD_EPSILON) - return; - - btAssert(len < SIMD_EPSILON); - - - //this jacobian entry could be re-used for all iterations - btJacobianEntry jacA(world2A,world2B,rel_posA1,rel_posA2,normalA,invInertiaADiag,invMassA, - invInertiaBDiag,invMassB); - btJacobianEntry jacB(world2A,world2B,rel_posB1,rel_posB2,normalB,invInertiaADiag,invMassA, - invInertiaBDiag,invMassB); - - //const btScalar vel0 = jacA.getRelativeVelocity(linvelA,angvelA,linvelB,angvelB); - //const btScalar vel1 = jacB.getRelativeVelocity(linvelA,angvelA,linvelB,angvelB); - - const btScalar vel0 = normalA.dot(body1->getVelocityInLocalPoint(rel_posA1)-body2->getVelocityInLocalPoint(rel_posA1)); - const btScalar vel1 = normalB.dot(body1->getVelocityInLocalPoint(rel_posB1)-body2->getVelocityInLocalPoint(rel_posB1)); - -// btScalar penetrationImpulse = (depth*contactTau*timeCorrection) * massTerm;//jacDiagABInv - btScalar massTerm = btScalar(1.) / (invMassA + invMassB); - - - // calculate rhs (or error) terms - const btScalar dv0 = depthA * m_tau * massTerm - vel0 * m_damping; - const btScalar dv1 = depthB * m_tau * massTerm - vel1 * m_damping; - - - // dC/dv * dv = -C - - // jacobian * impulse = -error - // - - //impulse = jacobianInverse * -error - - // inverting 2x2 symmetric system (offdiagonal are equal!) - // - - - btScalar nonDiag = jacA.getNonDiagonal(jacB,invMassA,invMassB); - btScalar invDet = btScalar(1.0) / (jacA.getDiagonal() * jacB.getDiagonal() - nonDiag * nonDiag ); - - //imp0 = dv0 * jacA.getDiagonal() * invDet + dv1 * -nonDiag * invDet; - //imp1 = dv1 * jacB.getDiagonal() * invDet + dv0 * - nonDiag * invDet; - - imp0 = dv0 * jacA.getDiagonal() * invDet + dv1 * -nonDiag * invDet; - imp1 = dv1 * jacB.getDiagonal() * invDet + dv0 * - nonDiag * invDet; - - //[a b] [d -c] - //[c d] inverse = (1 / determinant) * [-b a] where determinant is (ad - bc) - - //[jA nD] * [imp0] = [dv0] - //[nD jB] [imp1] [dv1] - -} - - - -void btSolve2LinearConstraint::resolveBilateralPairConstraint( - btRigidBody* body1, - btRigidBody* body2, - const btMatrix3x3& world2A, - const btMatrix3x3& world2B, - - const btVector3& invInertiaADiag, - const btScalar invMassA, - const btVector3& linvelA,const btVector3& angvelA, - const btVector3& rel_posA1, - const btVector3& invInertiaBDiag, - const btScalar invMassB, - const btVector3& linvelB,const btVector3& angvelB, - const btVector3& rel_posA2, - - btScalar depthA, const btVector3& normalA, - const btVector3& rel_posB1,const btVector3& rel_posB2, - btScalar depthB, const btVector3& normalB, - btScalar& imp0,btScalar& imp1) -{ - - (void)linvelA; - (void)linvelB; - (void)angvelA; - (void)angvelB; - - - - imp0 = btScalar(0.); - imp1 = btScalar(0.); - - btScalar len = btFabs(normalA.length()) - btScalar(1.); - if (btFabs(len) >= SIMD_EPSILON) - return; - - btAssert(len < SIMD_EPSILON); - - - //this jacobian entry could be re-used for all iterations - btJacobianEntry jacA(world2A,world2B,rel_posA1,rel_posA2,normalA,invInertiaADiag,invMassA, - invInertiaBDiag,invMassB); - btJacobianEntry jacB(world2A,world2B,rel_posB1,rel_posB2,normalB,invInertiaADiag,invMassA, - invInertiaBDiag,invMassB); - - //const btScalar vel0 = jacA.getRelativeVelocity(linvelA,angvelA,linvelB,angvelB); - //const btScalar vel1 = jacB.getRelativeVelocity(linvelA,angvelA,linvelB,angvelB); - - const btScalar vel0 = normalA.dot(body1->getVelocityInLocalPoint(rel_posA1)-body2->getVelocityInLocalPoint(rel_posA1)); - const btScalar vel1 = normalB.dot(body1->getVelocityInLocalPoint(rel_posB1)-body2->getVelocityInLocalPoint(rel_posB1)); - - // calculate rhs (or error) terms - const btScalar dv0 = depthA * m_tau - vel0 * m_damping; - const btScalar dv1 = depthB * m_tau - vel1 * m_damping; - - // dC/dv * dv = -C - - // jacobian * impulse = -error - // - - //impulse = jacobianInverse * -error - - // inverting 2x2 symmetric system (offdiagonal are equal!) - // - - - btScalar nonDiag = jacA.getNonDiagonal(jacB,invMassA,invMassB); - btScalar invDet = btScalar(1.0) / (jacA.getDiagonal() * jacB.getDiagonal() - nonDiag * nonDiag ); - - //imp0 = dv0 * jacA.getDiagonal() * invDet + dv1 * -nonDiag * invDet; - //imp1 = dv1 * jacB.getDiagonal() * invDet + dv0 * - nonDiag * invDet; - - imp0 = dv0 * jacA.getDiagonal() * invDet + dv1 * -nonDiag * invDet; - imp1 = dv1 * jacB.getDiagonal() * invDet + dv0 * - nonDiag * invDet; - - //[a b] [d -c] - //[c d] inverse = (1 / determinant) * [-b a] where determinant is (ad - bc) - - //[jA nD] * [imp0] = [dv0] - //[nD jB] [imp1] [dv1] - - if ( imp0 > btScalar(0.0)) - { - if ( imp1 > btScalar(0.0) ) - { - //both positive - } - else - { - imp1 = btScalar(0.); - - // now imp0>0 imp1<0 - imp0 = dv0 / jacA.getDiagonal(); - if ( imp0 > btScalar(0.0) ) - { - } else - { - imp0 = btScalar(0.); - } - } - } - else - { - imp0 = btScalar(0.); - - imp1 = dv1 / jacB.getDiagonal(); - if ( imp1 <= btScalar(0.0) ) - { - imp1 = btScalar(0.); - // now imp0>0 imp1<0 - imp0 = dv0 / jacA.getDiagonal(); - if ( imp0 > btScalar(0.0) ) - { - } else - { - imp0 = btScalar(0.); - } - } else - { - } - } -} - - -/* -void btSolve2LinearConstraint::resolveAngularConstraint( const btMatrix3x3& invInertiaAWS, - const btScalar invMassA, - const btVector3& linvelA,const btVector3& angvelA, - const btVector3& rel_posA1, - const btMatrix3x3& invInertiaBWS, - const btScalar invMassB, - const btVector3& linvelB,const btVector3& angvelB, - const btVector3& rel_posA2, - - btScalar depthA, const btVector3& normalA, - const btVector3& rel_posB1,const btVector3& rel_posB2, - btScalar depthB, const btVector3& normalB, - btScalar& imp0,btScalar& imp1) -{ - -} -*/ - diff --git a/extern/bullet2/BulletDynamics/ConstraintSolver/btSolve2LinearConstraint.h b/extern/bullet2/BulletDynamics/ConstraintSolver/btSolve2LinearConstraint.h deleted file mode 100644 index 057d3fac827..00000000000 --- a/extern/bullet2/BulletDynamics/ConstraintSolver/btSolve2LinearConstraint.h +++ /dev/null @@ -1,107 +0,0 @@ -/* -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. -*/ - -#ifndef SOLVE_2LINEAR_CONSTRAINT_H -#define SOLVE_2LINEAR_CONSTRAINT_H - -#include "LinearMath/btMatrix3x3.h" -#include "LinearMath/btVector3.h" - - -class btRigidBody; - - - -/// constraint class used for lateral tyre friction. -class btSolve2LinearConstraint -{ - btScalar m_tau; - btScalar m_damping; - -public: - - btSolve2LinearConstraint(btScalar tau,btScalar damping) - { - m_tau = tau; - m_damping = damping; - } - // - // solve unilateral constraint (equality, direct method) - // - void resolveUnilateralPairConstraint( - btRigidBody* body0, - btRigidBody* body1, - - const btMatrix3x3& world2A, - const btMatrix3x3& world2B, - - const btVector3& invInertiaADiag, - const btScalar invMassA, - const btVector3& linvelA,const btVector3& angvelA, - const btVector3& rel_posA1, - const btVector3& invInertiaBDiag, - const btScalar invMassB, - const btVector3& linvelB,const btVector3& angvelB, - const btVector3& rel_posA2, - - btScalar depthA, const btVector3& normalA, - const btVector3& rel_posB1,const btVector3& rel_posB2, - btScalar depthB, const btVector3& normalB, - btScalar& imp0,btScalar& imp1); - - - // - // solving 2x2 lcp problem (inequality, direct solution ) - // - void resolveBilateralPairConstraint( - btRigidBody* body0, - btRigidBody* body1, - const btMatrix3x3& world2A, - const btMatrix3x3& world2B, - - const btVector3& invInertiaADiag, - const btScalar invMassA, - const btVector3& linvelA,const btVector3& angvelA, - const btVector3& rel_posA1, - const btVector3& invInertiaBDiag, - const btScalar invMassB, - const btVector3& linvelB,const btVector3& angvelB, - const btVector3& rel_posA2, - - btScalar depthA, const btVector3& normalA, - const btVector3& rel_posB1,const btVector3& rel_posB2, - btScalar depthB, const btVector3& normalB, - btScalar& imp0,btScalar& imp1); - -/* - void resolveAngularConstraint( const btMatrix3x3& invInertiaAWS, - const btScalar invMassA, - const btVector3& linvelA,const btVector3& angvelA, - const btVector3& rel_posA1, - const btMatrix3x3& invInertiaBWS, - const btScalar invMassB, - const btVector3& linvelB,const btVector3& angvelB, - const btVector3& rel_posA2, - - btScalar depthA, const btVector3& normalA, - const btVector3& rel_posB1,const btVector3& rel_posB2, - btScalar depthB, const btVector3& normalB, - btScalar& imp0,btScalar& imp1); - -*/ - -}; - -#endif //SOLVE_2LINEAR_CONSTRAINT_H diff --git a/extern/bullet2/BulletDynamics/ConstraintSolver/btSolverBody.h b/extern/bullet2/BulletDynamics/ConstraintSolver/btSolverBody.h deleted file mode 100644 index 8de515812ee..00000000000 --- a/extern/bullet2/BulletDynamics/ConstraintSolver/btSolverBody.h +++ /dev/null @@ -1,191 +0,0 @@ -/* -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. -*/ - -#ifndef BT_SOLVER_BODY_H -#define BT_SOLVER_BODY_H - -class btRigidBody; -#include "LinearMath/btVector3.h" -#include "LinearMath/btMatrix3x3.h" -#include "BulletDynamics/Dynamics/btRigidBody.h" -#include "LinearMath/btAlignedAllocator.h" -#include "LinearMath/btTransformUtil.h" - -///Until we get other contributions, only use SIMD on Windows, when using Visual Studio 2008 or later, and not double precision -#ifdef BT_USE_SSE -#define USE_SIMD 1 -#endif // - - -#ifdef USE_SIMD - -struct btSimdScalar -{ - SIMD_FORCE_INLINE btSimdScalar() - { - - } - - SIMD_FORCE_INLINE btSimdScalar(float fl) - :m_vec128 (_mm_set1_ps(fl)) - { - } - - SIMD_FORCE_INLINE btSimdScalar(__m128 v128) - :m_vec128(v128) - { - } - union - { - __m128 m_vec128; - float m_floats[4]; - int m_ints[4]; - btScalar m_unusedPadding; - }; - SIMD_FORCE_INLINE __m128 get128() - { - return m_vec128; - } - - SIMD_FORCE_INLINE const __m128 get128() const - { - return m_vec128; - } - - SIMD_FORCE_INLINE void set128(__m128 v128) - { - m_vec128 = v128; - } - - SIMD_FORCE_INLINE operator __m128() - { - return m_vec128; - } - SIMD_FORCE_INLINE operator const __m128() const - { - return m_vec128; - } - - SIMD_FORCE_INLINE operator float() const - { - return m_floats[0]; - } - -}; - -///@brief Return the elementwise product of two btSimdScalar -SIMD_FORCE_INLINE btSimdScalar -operator*(const btSimdScalar& v1, const btSimdScalar& v2) -{ - return btSimdScalar(_mm_mul_ps(v1.get128(),v2.get128())); -} - -///@brief Return the elementwise product of two btSimdScalar -SIMD_FORCE_INLINE btSimdScalar -operator+(const btSimdScalar& v1, const btSimdScalar& v2) -{ - return btSimdScalar(_mm_add_ps(v1.get128(),v2.get128())); -} - - -#else -#define btSimdScalar btScalar -#endif - -///The btSolverBody is an internal datastructure for the constraint solver. Only necessary data is packed to increase cache coherence/performance. -ATTRIBUTE_ALIGNED64 (struct) btSolverBodyObsolete -{ - BT_DECLARE_ALIGNED_ALLOCATOR(); - btVector3 m_deltaLinearVelocity; - btVector3 m_deltaAngularVelocity; - btVector3 m_angularFactor; - btVector3 m_invMass; - btRigidBody* m_originalBody; - btVector3 m_pushVelocity; - btVector3 m_turnVelocity; - - - SIMD_FORCE_INLINE void getVelocityInLocalPointObsolete(const btVector3& rel_pos, btVector3& velocity ) const - { - if (m_originalBody) - velocity = m_originalBody->getLinearVelocity()+m_deltaLinearVelocity + (m_originalBody->getAngularVelocity()+m_deltaAngularVelocity).cross(rel_pos); - else - velocity.setValue(0,0,0); - } - - SIMD_FORCE_INLINE void getAngularVelocity(btVector3& angVel) const - { - if (m_originalBody) - angVel = m_originalBody->getAngularVelocity()+m_deltaAngularVelocity; - else - angVel.setValue(0,0,0); - } - - - //Optimization for the iterative solver: avoid calculating constant terms involving inertia, normal, relative position - SIMD_FORCE_INLINE void applyImpulse(const btVector3& linearComponent, const btVector3& angularComponent,const btScalar impulseMagnitude) - { - //if (m_invMass) - { - m_deltaLinearVelocity += linearComponent*impulseMagnitude; - m_deltaAngularVelocity += angularComponent*(impulseMagnitude*m_angularFactor); - } - } - - SIMD_FORCE_INLINE void internalApplyPushImpulse(const btVector3& linearComponent, const btVector3& angularComponent,btScalar impulseMagnitude) - { - if (m_originalBody) - { - m_pushVelocity += linearComponent*impulseMagnitude; - m_turnVelocity += angularComponent*(impulseMagnitude*m_angularFactor); - } - } - - void writebackVelocity() - { - if (m_originalBody) - { - m_originalBody->setLinearVelocity(m_originalBody->getLinearVelocity()+ m_deltaLinearVelocity); - m_originalBody->setAngularVelocity(m_originalBody->getAngularVelocity()+m_deltaAngularVelocity); - - //m_originalBody->setCompanionId(-1); - } - } - - - void writebackVelocity(btScalar timeStep) - { - (void) timeStep; - if (m_originalBody) - { - m_originalBody->setLinearVelocity(m_originalBody->getLinearVelocity()+ m_deltaLinearVelocity); - m_originalBody->setAngularVelocity(m_originalBody->getAngularVelocity()+m_deltaAngularVelocity); - - //correct the position/orientation based on push/turn recovery - btTransform newTransform; - btTransformUtil::integrateTransform(m_originalBody->getWorldTransform(),m_pushVelocity,m_turnVelocity,timeStep,newTransform); - m_originalBody->setWorldTransform(newTransform); - - //m_originalBody->setCompanionId(-1); - } - } - - - -}; - -#endif //BT_SOLVER_BODY_H - - diff --git a/extern/bullet2/BulletDynamics/ConstraintSolver/btSolverConstraint.h b/extern/bullet2/BulletDynamics/ConstraintSolver/btSolverConstraint.h deleted file mode 100644 index 929cf6d3ee3..00000000000 --- a/extern/bullet2/BulletDynamics/ConstraintSolver/btSolverConstraint.h +++ /dev/null @@ -1,96 +0,0 @@ -/* -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. -*/ - -#ifndef BT_SOLVER_CONSTRAINT_H -#define BT_SOLVER_CONSTRAINT_H - -class btRigidBody; -#include "LinearMath/btVector3.h" -#include "LinearMath/btMatrix3x3.h" -#include "btJacobianEntry.h" - -//#define NO_FRICTION_TANGENTIALS 1 -#include "btSolverBody.h" - - -///1D constraint along a normal axis between bodyA and bodyB. It can be combined to solve contact and friction constraints. -ATTRIBUTE_ALIGNED64 (struct) btSolverConstraint -{ - BT_DECLARE_ALIGNED_ALLOCATOR(); - - btVector3 m_relpos1CrossNormal; - btVector3 m_contactNormal; - - btVector3 m_relpos2CrossNormal; - //btVector3 m_contactNormal2;//usually m_contactNormal2 == -m_contactNormal - - btVector3 m_angularComponentA; - btVector3 m_angularComponentB; - - mutable btSimdScalar m_appliedPushImpulse; - mutable btSimdScalar m_appliedImpulse; - - - btScalar m_friction; - btScalar m_jacDiagABInv; - union - { - int m_numConsecutiveRowsPerKernel; - btScalar m_unusedPadding0; - }; - - union - { - int m_frictionIndex; - btScalar m_unusedPadding1; - }; - union - { - btRigidBody* m_solverBodyA; - btScalar m_unusedPadding2; - }; - union - { - btRigidBody* m_solverBodyB; - btScalar m_unusedPadding3; - }; - - union - { - void* m_originalContactPoint; - btScalar m_unusedPadding4; - }; - - btScalar m_rhs; - btScalar m_cfm; - btScalar m_lowerLimit; - btScalar m_upperLimit; - - btScalar m_rhsPenetration; - - enum btSolverConstraintType - { - BT_SOLVER_CONTACT_1D = 0, - BT_SOLVER_FRICTION_1D - }; -}; - -typedef btAlignedObjectArray<btSolverConstraint> btConstraintArray; - - -#endif //BT_SOLVER_CONSTRAINT_H - - - diff --git a/extern/bullet2/BulletDynamics/ConstraintSolver/btTypedConstraint.cpp b/extern/bullet2/BulletDynamics/ConstraintSolver/btTypedConstraint.cpp deleted file mode 100644 index e7c94b19308..00000000000 --- a/extern/bullet2/BulletDynamics/ConstraintSolver/btTypedConstraint.cpp +++ /dev/null @@ -1,136 +0,0 @@ -/* -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 "btTypedConstraint.h" -#include "BulletDynamics/Dynamics/btRigidBody.h" -#include "LinearMath/btSerializer.h" - - -#define DEFAULT_DEBUGDRAW_SIZE btScalar(0.3f) - -btTypedConstraint::btTypedConstraint(btTypedConstraintType type, btRigidBody& rbA) -:btTypedObject(type), -m_userConstraintType(-1), -m_userConstraintId(-1), -m_needsFeedback(false), -m_rbA(rbA), -m_rbB(getFixedBody()), -m_appliedImpulse(btScalar(0.)), -m_dbgDrawSize(DEFAULT_DEBUGDRAW_SIZE) -{ -} - - -btTypedConstraint::btTypedConstraint(btTypedConstraintType type, btRigidBody& rbA,btRigidBody& rbB) -:btTypedObject(type), -m_userConstraintType(-1), -m_userConstraintId(-1), -m_needsFeedback(false), -m_rbA(rbA), -m_rbB(rbB), -m_appliedImpulse(btScalar(0.)), -m_dbgDrawSize(DEFAULT_DEBUGDRAW_SIZE) -{ -} - - - - -btScalar btTypedConstraint::getMotorFactor(btScalar pos, btScalar lowLim, btScalar uppLim, btScalar vel, btScalar timeFact) -{ - if(lowLim > uppLim) - { - return btScalar(1.0f); - } - else if(lowLim == uppLim) - { - return btScalar(0.0f); - } - btScalar lim_fact = btScalar(1.0f); - btScalar delta_max = vel / timeFact; - if(delta_max < btScalar(0.0f)) - { - if((pos >= lowLim) && (pos < (lowLim - delta_max))) - { - lim_fact = (lowLim - pos) / delta_max; - } - else if(pos < lowLim) - { - lim_fact = btScalar(0.0f); - } - else - { - lim_fact = btScalar(1.0f); - } - } - else if(delta_max > btScalar(0.0f)) - { - if((pos <= uppLim) && (pos > (uppLim - delta_max))) - { - lim_fact = (uppLim - pos) / delta_max; - } - else if(pos > uppLim) - { - lim_fact = btScalar(0.0f); - } - else - { - lim_fact = btScalar(1.0f); - } - } - else - { - lim_fact = btScalar(0.0f); - } - return lim_fact; -} - -///fills the dataBuffer and returns the struct name (and 0 on failure) -const char* btTypedConstraint::serialize(void* dataBuffer, btSerializer* serializer) const -{ - btTypedConstraintData* tcd = (btTypedConstraintData*) dataBuffer; - - tcd->m_rbA = (btRigidBodyData*)serializer->getUniquePointer(&m_rbA); - tcd->m_rbB = (btRigidBodyData*)serializer->getUniquePointer(&m_rbB); - char* name = (char*) serializer->findNameForPointer(this); - tcd->m_name = (char*)serializer->getUniquePointer(name); - if (tcd->m_name) - { - serializer->serializeName(name); - } - - tcd->m_objectType = m_objectType; - tcd->m_needsFeedback = m_needsFeedback; - tcd->m_userConstraintId =m_userConstraintId; - tcd->m_userConstraintType =m_userConstraintType; - - tcd->m_appliedImpulse = float(m_appliedImpulse); - tcd->m_dbgDrawSize = float(m_dbgDrawSize ); - - tcd->m_disableCollisionsBetweenLinkedBodies = false; - - int i; - for (i=0;i<m_rbA.getNumConstraintRefs();i++) - if (m_rbA.getConstraintRef(i) == this) - tcd->m_disableCollisionsBetweenLinkedBodies = true; - for (i=0;i<m_rbB.getNumConstraintRefs();i++) - if (m_rbB.getConstraintRef(i) == this) - tcd->m_disableCollisionsBetweenLinkedBodies = true; - - return "btTypedConstraintData"; -} - - diff --git a/extern/bullet2/BulletDynamics/ConstraintSolver/btTypedConstraint.h b/extern/bullet2/BulletDynamics/ConstraintSolver/btTypedConstraint.h deleted file mode 100644 index b24dc4a40ed..00000000000 --- a/extern/bullet2/BulletDynamics/ConstraintSolver/btTypedConstraint.h +++ /dev/null @@ -1,302 +0,0 @@ -/* -Bullet Continuous Collision Detection and Physics Library -Copyright (c) 2003-2010 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. -*/ - -#ifndef TYPED_CONSTRAINT_H -#define TYPED_CONSTRAINT_H - -class btRigidBody; -#include "LinearMath/btScalar.h" -#include "btSolverConstraint.h" -#include "BulletCollision/NarrowPhaseCollision/btPersistentManifold.h" - -class btSerializer; - -enum btTypedConstraintType -{ - POINT2POINT_CONSTRAINT_TYPE=MAX_CONTACT_MANIFOLD_TYPE+1, - HINGE_CONSTRAINT_TYPE, - CONETWIST_CONSTRAINT_TYPE, - D6_CONSTRAINT_TYPE, - SLIDER_CONSTRAINT_TYPE, - CONTACT_CONSTRAINT_TYPE -}; - - -enum btConstraintParams -{ - BT_CONSTRAINT_ERP=1, - BT_CONSTRAINT_STOP_ERP, - BT_CONSTRAINT_CFM, - BT_CONSTRAINT_STOP_CFM -}; - -#if 1 - #define btAssertConstrParams(_par) btAssert(_par) -#else - #define btAssertConstrParams(_par) -#endif - - -///TypedConstraint is the baseclass for Bullet constraints and vehicles -class btTypedConstraint : public btTypedObject -{ - int m_userConstraintType; - int m_userConstraintId; - bool m_needsFeedback; - - btTypedConstraint& operator=(btTypedConstraint& other) - { - btAssert(0); - (void) other; - return *this; - } - -protected: - btRigidBody& m_rbA; - btRigidBody& m_rbB; - btScalar m_appliedImpulse; - btScalar m_dbgDrawSize; - - ///internal method used by the constraint solver, don't use them directly - btScalar getMotorFactor(btScalar pos, btScalar lowLim, btScalar uppLim, btScalar vel, btScalar timeFact); - - static btRigidBody& getFixedBody() - { - static btRigidBody s_fixed(0, 0,0); - s_fixed.setMassProps(btScalar(0.),btVector3(btScalar(0.),btScalar(0.),btScalar(0.))); - return s_fixed; - } - - -public: - - virtual ~btTypedConstraint() {}; - btTypedConstraint(btTypedConstraintType type, btRigidBody& rbA); - btTypedConstraint(btTypedConstraintType type, btRigidBody& rbA,btRigidBody& rbB); - - struct btConstraintInfo1 { - int m_numConstraintRows,nub; - }; - - struct btConstraintInfo2 { - // integrator parameters: frames per second (1/stepsize), default error - // reduction parameter (0..1). - btScalar fps,erp; - - // for the first and second body, pointers to two (linear and angular) - // n*3 jacobian sub matrices, stored by rows. these matrices will have - // been initialized to 0 on entry. if the second body is zero then the - // J2xx pointers may be 0. - btScalar *m_J1linearAxis,*m_J1angularAxis,*m_J2linearAxis,*m_J2angularAxis; - - // elements to jump from one row to the next in J's - int rowskip; - - // right hand sides of the equation J*v = c + cfm * lambda. cfm is the - // "constraint force mixing" vector. c is set to zero on entry, cfm is - // set to a constant value (typically very small or zero) value on entry. - btScalar *m_constraintError,*cfm; - - // lo and hi limits for variables (set to -/+ infinity on entry). - btScalar *m_lowerLimit,*m_upperLimit; - - // findex vector for variables. see the LCP solver interface for a - // description of what this does. this is set to -1 on entry. - // note that the returned indexes are relative to the first index of - // the constraint. - int *findex; - // number of solver iterations - int m_numIterations; - }; - - ///internal method used by the constraint solver, don't use them directly - virtual void buildJacobian() {}; - - ///internal method used by the constraint solver, don't use them directly - virtual void setupSolverConstraint(btConstraintArray& ca, int solverBodyA,int solverBodyB, btScalar timeStep) - { - (void)ca; - (void)solverBodyA; - (void)solverBodyB; - (void)timeStep; - } - - ///internal method used by the constraint solver, don't use them directly - virtual void getInfo1 (btConstraintInfo1* info)=0; - - ///internal method used by the constraint solver, don't use them directly - virtual void getInfo2 (btConstraintInfo2* info)=0; - - ///internal method used by the constraint solver, don't use them directly - void internalSetAppliedImpulse(btScalar appliedImpulse) - { - m_appliedImpulse = appliedImpulse; - } - ///internal method used by the constraint solver, don't use them directly - btScalar internalGetAppliedImpulse() - { - return m_appliedImpulse; - } - - ///internal method used by the constraint solver, don't use them directly - virtual void solveConstraintObsolete(btRigidBody& bodyA,btRigidBody& bodyB,btScalar timeStep) {}; - - - const btRigidBody& getRigidBodyA() const - { - return m_rbA; - } - const btRigidBody& getRigidBodyB() const - { - return m_rbB; - } - - btRigidBody& getRigidBodyA() - { - return m_rbA; - } - btRigidBody& getRigidBodyB() - { - return m_rbB; - } - - int getUserConstraintType() const - { - return m_userConstraintType ; - } - - void setUserConstraintType(int userConstraintType) - { - m_userConstraintType = userConstraintType; - }; - - void setUserConstraintId(int uid) - { - m_userConstraintId = uid; - } - - int getUserConstraintId() const - { - return m_userConstraintId; - } - - int getUid() const - { - return m_userConstraintId; - } - - bool needsFeedback() const - { - return m_needsFeedback; - } - - ///enableFeedback will allow to read the applied linear and angular impulse - ///use getAppliedImpulse, getAppliedLinearImpulse and getAppliedAngularImpulse to read feedback information - void enableFeedback(bool needsFeedback) - { - m_needsFeedback = needsFeedback; - } - - ///getAppliedImpulse is an estimated total applied impulse. - ///This feedback could be used to determine breaking constraints or playing sounds. - btScalar getAppliedImpulse() const - { - btAssert(m_needsFeedback); - return m_appliedImpulse; - } - - btTypedConstraintType getConstraintType () const - { - return btTypedConstraintType(m_objectType); - } - - void setDbgDrawSize(btScalar dbgDrawSize) - { - m_dbgDrawSize = dbgDrawSize; - } - btScalar getDbgDrawSize() - { - return m_dbgDrawSize; - } - - ///override the default global value of a parameter (such as ERP or CFM), optionally provide the axis (0..5). - ///If no axis is provided, it uses the default axis for this constraint. - virtual void setParam(int num, btScalar value, int axis = -1) = 0; - - ///return the local value of parameter - virtual btScalar getParam(int num, int axis = -1) const = 0; - - virtual int calculateSerializeBufferSize() const; - - ///fills the dataBuffer and returns the struct name (and 0 on failure) - virtual const char* serialize(void* dataBuffer, btSerializer* serializer) const; - -}; - -// returns angle in range [-SIMD_2_PI, SIMD_2_PI], closest to one of the limits -// all arguments should be normalized angles (i.e. in range [-SIMD_PI, SIMD_PI]) -SIMD_FORCE_INLINE btScalar btAdjustAngleToLimits(btScalar angleInRadians, btScalar angleLowerLimitInRadians, btScalar angleUpperLimitInRadians) -{ - if(angleLowerLimitInRadians >= angleUpperLimitInRadians) - { - return angleInRadians; - } - else if(angleInRadians < angleLowerLimitInRadians) - { - btScalar diffLo = btNormalizeAngle(angleLowerLimitInRadians - angleInRadians); // this is positive - btScalar diffHi = btFabs(btNormalizeAngle(angleUpperLimitInRadians - angleInRadians)); - return (diffLo < diffHi) ? angleInRadians : (angleInRadians + SIMD_2_PI); - } - else if(angleInRadians > angleUpperLimitInRadians) - { - btScalar diffHi = btNormalizeAngle(angleInRadians - angleUpperLimitInRadians); // this is positive - btScalar diffLo = btFabs(btNormalizeAngle(angleInRadians - angleLowerLimitInRadians)); - return (diffLo < diffHi) ? (angleInRadians - SIMD_2_PI) : angleInRadians; - } - else - { - return angleInRadians; - } -} - -///do not change those serialization structures, it requires an updated sBulletDNAstr/sBulletDNAstr64 -struct btTypedConstraintData -{ - btRigidBodyData *m_rbA; - btRigidBodyData *m_rbB; - char *m_name; - - int m_objectType; - int m_userConstraintType; - int m_userConstraintId; - int m_needsFeedback; - - float m_appliedImpulse; - float m_dbgDrawSize; - - int m_disableCollisionsBetweenLinkedBodies; - char m_pad4[4]; - -}; - -SIMD_FORCE_INLINE int btTypedConstraint::calculateSerializeBufferSize() const -{ - return sizeof(btTypedConstraintData); -} - - - - -#endif //TYPED_CONSTRAINT_H diff --git a/extern/bullet2/BulletDynamics/ConstraintSolver/btUniversalConstraint.cpp b/extern/bullet2/BulletDynamics/ConstraintSolver/btUniversalConstraint.cpp deleted file mode 100644 index 3a4c2afa642..00000000000 --- a/extern/bullet2/BulletDynamics/ConstraintSolver/btUniversalConstraint.cpp +++ /dev/null @@ -1,63 +0,0 @@ -/* -Bullet Continuous Collision Detection and Physics Library, http://bulletphysics.org -Copyright (C) 2006, 2007 Sony Computer Entertainment Inc. - -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 "btUniversalConstraint.h" -#include "BulletDynamics/Dynamics/btRigidBody.h" -#include "LinearMath/btTransformUtil.h" - - - -#define UNIV_EPS btScalar(0.01f) - - -// constructor -// anchor, axis1 and axis2 are in world coordinate system -// axis1 must be orthogonal to axis2 -btUniversalConstraint::btUniversalConstraint(btRigidBody& rbA, btRigidBody& rbB, btVector3& anchor, btVector3& axis1, btVector3& axis2) -: btGeneric6DofConstraint(rbA, rbB, btTransform::getIdentity(), btTransform::getIdentity(), true), - m_anchor(anchor), - m_axis1(axis1), - m_axis2(axis2) -{ - // build frame basis - // 6DOF constraint uses Euler angles and to define limits - // it is assumed that rotational order is : - // Z - first, allowed limits are (-PI,PI); - // new position of Y - second (allowed limits are (-PI/2 + epsilon, PI/2 - epsilon), where epsilon is a small positive number - // used to prevent constraint from instability on poles; - // new position of X, allowed limits are (-PI,PI); - // So to simulate ODE Universal joint we should use parent axis as Z, child axis as Y and limit all other DOFs - // Build the frame in world coordinate system first - btVector3 zAxis = axis1.normalize(); - btVector3 yAxis = axis2.normalize(); - btVector3 xAxis = yAxis.cross(zAxis); // we want right coordinate system - btTransform frameInW; - frameInW.setIdentity(); - frameInW.getBasis().setValue( xAxis[0], yAxis[0], zAxis[0], - xAxis[1], yAxis[1], zAxis[1], - xAxis[2], yAxis[2], zAxis[2]); - frameInW.setOrigin(anchor); - // now get constraint frame in local coordinate systems - m_frameInA = rbA.getCenterOfMassTransform().inverse() * frameInW; - m_frameInB = rbB.getCenterOfMassTransform().inverse() * frameInW; - // sei limits - setLinearLowerLimit(btVector3(0., 0., 0.)); - setLinearUpperLimit(btVector3(0., 0., 0.)); - setAngularLowerLimit(btVector3(0.f, -SIMD_HALF_PI + UNIV_EPS, -SIMD_PI + UNIV_EPS)); - setAngularUpperLimit(btVector3(0.f, SIMD_HALF_PI - UNIV_EPS, SIMD_PI - UNIV_EPS)); -} - diff --git a/extern/bullet2/BulletDynamics/ConstraintSolver/btUniversalConstraint.h b/extern/bullet2/BulletDynamics/ConstraintSolver/btUniversalConstraint.h deleted file mode 100644 index 4e64a7d7e0e..00000000000 --- a/extern/bullet2/BulletDynamics/ConstraintSolver/btUniversalConstraint.h +++ /dev/null @@ -1,60 +0,0 @@ -/* -Bullet Continuous Collision Detection and Physics Library, http://bulletphysics.org -Copyright (C) 2006, 2007 Sony Computer Entertainment Inc. - -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. -*/ - -#ifndef UNIVERSAL_CONSTRAINT_H -#define UNIVERSAL_CONSTRAINT_H - - - -#include "LinearMath/btVector3.h" -#include "btTypedConstraint.h" -#include "btGeneric6DofConstraint.h" - - - -/// Constraint similar to ODE Universal Joint -/// has 2 rotatioonal degrees of freedom, similar to Euler rotations around Z (axis 1) -/// and Y (axis 2) -/// Description from ODE manual : -/// "Given axis 1 on body 1, and axis 2 on body 2 that is perpendicular to axis 1, it keeps them perpendicular. -/// In other words, rotation of the two bodies about the direction perpendicular to the two axes will be equal." - -class btUniversalConstraint : public btGeneric6DofConstraint -{ -protected: - btVector3 m_anchor; - btVector3 m_axis1; - btVector3 m_axis2; -public: - // constructor - // anchor, axis1 and axis2 are in world coordinate system - // axis1 must be orthogonal to axis2 - btUniversalConstraint(btRigidBody& rbA, btRigidBody& rbB, btVector3& anchor, btVector3& axis1, btVector3& axis2); - // access - const btVector3& getAnchor() { return m_calculatedTransformA.getOrigin(); } - const btVector3& getAnchor2() { return m_calculatedTransformB.getOrigin(); } - const btVector3& getAxis1() { return m_axis1; } - const btVector3& getAxis2() { return m_axis2; } - btScalar getAngle1() { return getAngle(2); } - btScalar getAngle2() { return getAngle(1); } - // limits - void setUpperLimit(btScalar ang1max, btScalar ang2max) { setAngularUpperLimit(btVector3(0.f, ang1max, ang2max)); } - void setLowerLimit(btScalar ang1min, btScalar ang2min) { setAngularLowerLimit(btVector3(0.f, ang1min, ang2min)); } -}; - - - -#endif // UNIVERSAL_CONSTRAINT_H - |