| /* | |
| 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 "BulletDynamics/ConstraintSolver/btSliderConstraint.h" | |
| #include "BulletDynamics/Dynamics/btRigidBody.h" | |
| #include "LinearMath/btTransformUtil.h" | |
| #include <new> | |
| 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_softnessDirAng = SLIDER_CONSTRAINT_DEF_SOFTNESS; | |
| m_restitutionDirAng = SLIDER_CONSTRAINT_DEF_RESTITUTION; | |
| m_dampingDirAng = btScalar(0.); | |
| m_softnessOrthoLin = SLIDER_CONSTRAINT_DEF_SOFTNESS; | |
| m_restitutionOrthoLin = SLIDER_CONSTRAINT_DEF_RESTITUTION; | |
| m_dampingOrthoLin = SLIDER_CONSTRAINT_DEF_DAMPING; | |
| m_softnessOrthoAng = SLIDER_CONSTRAINT_DEF_SOFTNESS; | |
| m_restitutionOrthoAng = SLIDER_CONSTRAINT_DEF_RESTITUTION; | |
| m_dampingOrthoAng = SLIDER_CONSTRAINT_DEF_DAMPING; | |
| m_softnessLimLin = SLIDER_CONSTRAINT_DEF_SOFTNESS; | |
| m_restitutionLimLin = SLIDER_CONSTRAINT_DEF_RESTITUTION; | |
| m_dampingLimLin = SLIDER_CONSTRAINT_DEF_DAMPING; | |
| m_softnessLimAng = SLIDER_CONSTRAINT_DEF_SOFTNESS; | |
| m_restitutionLimAng = SLIDER_CONSTRAINT_DEF_RESTITUTION; | |
| m_dampingLimAng = SLIDER_CONSTRAINT_DEF_DAMPING; | |
| 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); | |
| } | |
| btSliderConstraint::btSliderConstraint() | |
| :btTypedConstraint(SLIDER_CONSTRAINT_TYPE), | |
| m_useSolveConstraintObsolete(false), | |
| m_useLinearReferenceFrameA(true) | |
| { | |
| initParams(); | |
| } | |
| 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(); | |
| } | |
| static btRigidBody s_fixed(0, 0, 0); | |
| btSliderConstraint::btSliderConstraint(btRigidBody& rbB, const btTransform& frameInB, bool useLinearReferenceFrameB) | |
| : btTypedConstraint(SLIDER_CONSTRAINT_TYPE, s_fixed, rbB), | |
| m_useSolveConstraintObsolete(false), | |
| m_frameInB(frameInB), | |
| m_useLinearReferenceFrameA(useLinearReferenceFrameB) | |
| { | |
| ///not providing rigidbody B means implicitly using worldspace for body B | |
| // m_frameInA.getOrigin() = m_rbA.getCenterOfMassTransform()(m_frameInA.getOrigin()); | |
| initParams(); | |
| } | |
| void btSliderConstraint::buildJacobian() | |
| { | |
| if (!m_useSolveConstraintObsolete) | |
| { | |
| return; | |
| } | |
| if(m_useLinearReferenceFrameA) | |
| { | |
| buildJacobianInt(m_rbA, m_rbB, m_frameInA, m_frameInB); | |
| } | |
| else | |
| { | |
| buildJacobianInt(m_rbB, m_rbA, m_frameInB, m_frameInA); | |
| } | |
| } | |
| void btSliderConstraint::buildJacobianInt(btRigidBody& rbA, btRigidBody& rbB, const btTransform& frameInA, const btTransform& frameInB) | |
| { | |
| #ifndef __SPU__ | |
| //calculate transforms | |
| m_calculatedTransformA = rbA.getCenterOfMassTransform() * frameInA; | |
| m_calculatedTransformB = rbB.getCenterOfMassTransform() * frameInB; | |
| m_realPivotAInW = m_calculatedTransformA.getOrigin(); | |
| m_realPivotBInW = m_calculatedTransformB.getOrigin(); | |
| m_sliderAxis = m_calculatedTransformA.getBasis().getColumn(0); // along X | |
| m_delta = m_realPivotBInW - m_realPivotAInW; | |
| m_projPivotInW = m_realPivotAInW + m_sliderAxis.dot(m_delta) * m_sliderAxis; | |
| m_relPosA = m_projPivotInW - rbA.getCenterOfMassPosition(); | |
| m_relPosB = m_realPivotBInW - rbB.getCenterOfMassPosition(); | |
| btVector3 normalWorld; | |
| int i; | |
| //linear part | |
| for(i = 0; i < 3; i++) | |
| { | |
| normalWorld = m_calculatedTransformA.getBasis().getColumn(i); | |
| new (&m_jacLin[i]) btJacobianEntry( | |
| rbA.getCenterOfMassTransform().getBasis().transpose(), | |
| rbB.getCenterOfMassTransform().getBasis().transpose(), | |
| m_relPosA, | |
| m_relPosB, | |
| normalWorld, | |
| rbA.getInvInertiaDiagLocal(), | |
| rbA.getInvMass(), | |
| rbB.getInvInertiaDiagLocal(), | |
| rbB.getInvMass() | |
| ); | |
| m_jacLinDiagABInv[i] = btScalar(1.) / m_jacLin[i].getDiagonal(); | |
| m_depth[i] = m_delta.dot(normalWorld); | |
| } | |
| testLinLimits(); | |
| // angular part | |
| for(i = 0; i < 3; i++) | |
| { | |
| normalWorld = m_calculatedTransformA.getBasis().getColumn(i); | |
| new (&m_jacAng[i]) btJacobianEntry( | |
| normalWorld, | |
| rbA.getCenterOfMassTransform().getBasis().transpose(), | |
| rbB.getCenterOfMassTransform().getBasis().transpose(), | |
| rbA.getInvInertiaDiagLocal(), | |
| rbB.getInvInertiaDiagLocal() | |
| ); | |
| } | |
| testAngLimits(); | |
| btVector3 axisA = m_calculatedTransformA.getBasis().getColumn(0); | |
| m_kAngle = btScalar(1.0 )/ (rbA.computeAngularImpulseDenominator(axisA) + rbB.computeAngularImpulseDenominator(axisA)); | |
| // clear accumulator for motors | |
| m_accumulatedLinMotorImpulse = btScalar(0.0); | |
| m_accumulatedAngMotorImpulse = btScalar(0.0); | |
| #endif //__SPU__ | |
| } | |
| 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()); | |
| testLinLimits(); | |
| if(getSolveLinLimit() || getPoweredLinMotor()) | |
| { | |
| info->m_numConstraintRows++; // limit 3rd linear as well | |
| info->nub--; | |
| } | |
| testAngLimits(); | |
| 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::getInfo2NonVirtual(btConstraintInfo2* info, const btTransform& transA,const btTransform& transB, const btVector3& linVelA,const btVector3& linVelB, btScalar rbAinvMass,btScalar rbBinvMass ) | |
| { | |
| //prepare constraint | |
| calculateTransforms(transA,transB); | |
| testLinLimits(); | |
| testAngLimits(); | |
| 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); | |
| // make rotations around Y and Z 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. | |
| // get slider axis (X) | |
| btVector3 ax1 = trA.getBasis().getColumn(0); | |
| // get 2 orthos to slider axis (Y, Z) | |
| btVector3 p = trA.getBasis().getColumn(1); | |
| btVector3 q = trA.getBasis().getColumn(2); | |
| // set the two slider rows | |
| 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 ax1,ax2 are the unit length slider 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. | |
| btScalar k = info->fps * info->erp * getSoftnessOrthoAng(); | |
| btVector3 ax2 = trB.getBasis().getColumn(0); | |
| btVector3 u = ax1.cross(ax2); | |
| info->m_constraintError[0] = k * u.dot(p); | |
| info->m_constraintError[s] = k * u.dot(q); | |
| // pull out pos and R for both bodies. also get the connection | |
| // vector c = pos2-pos1. | |
| // next two rows. we want: vel2 = vel1 + w1 x c ... but this would | |
| // result in three equations, so we project along the planespace vectors | |
| // so that sliding along the slider axis is disregarded. for symmetry we | |
| // also consider rotation around center of mass of two bodies (factA and factB). | |
| btTransform bodyA_trans = transA; | |
| btTransform bodyB_trans = transB; | |
| int s2 = 2 * s, s3 = 3 * s; | |
| btVector3 c; | |
| btScalar miA = rbAinvMass; | |
| btScalar miB = rbBinvMass; | |
| btScalar miS = miA + miB; | |
| btScalar factA, factB; | |
| if(miS > btScalar(0.f)) | |
| { | |
| factA = miB / miS; | |
| } | |
| else | |
| { | |
| factA = btScalar(0.5f); | |
| } | |
| if(factA > 0.99f) factA = 0.99f; | |
| if(factA < 0.01f) factA = 0.01f; | |
| factB = btScalar(1.0f) - factA; | |
| 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. we want to align the offset | |
| // point (in body 2's frame) with the center of body 1. | |
| btVector3 ofs; // offset point in global coordinates | |
| ofs = trB.getOrigin() - trA.getOrigin(); | |
| k = info->fps * info->erp * getSoftnessOrthoLin(); | |
| info->m_constraintError[s2] = k * p.dot(ofs); | |
| info->m_constraintError[s3] = k * q.dot(ofs); | |
| int nrow = 3; // last filled row | |
| int srow; | |
| // check linear limits linear | |
| btScalar limit_err = btScalar(0.0); | |
| int limit = 0; | |
| if(getSolveLinLimit()) | |
| { | |
| limit_err = getLinDepth() * signFact; | |
| limit = (limit_err > btScalar(0.0)) ? 2 : 1; | |
| } | |
| int 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. | |
| // the solution used here is to apply the constraint forces at the center of mass of the two bodies | |
| btVector3 ltd; // Linear Torque Decoupling vector (a torque) | |
| // c = btScalar(0.5) * c; | |
| 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.; | |
| if(powered) | |
| { | |
| info->cfm[nrow] = btScalar(0.0); | |
| btScalar tag_vel = getTargetLinMotorVelocity(); | |
| btScalar mot_fact = getMotorFactor(m_linPos, m_lowerLinLimit, m_upperLinLimit, tag_vel, info->fps * info->erp); | |
| // info->m_constraintError[srow] += mot_fact * getTargetLinMotorVelocity(); | |
| 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 * info->erp; | |
| info->m_constraintError[srow] += k * limit_err; | |
| info->cfm[srow] = btScalar(0.0); // stop_cfm; | |
| 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; | |
| } | |
| if(powered) | |
| { | |
| info->cfm[srow] = btScalar(0.0); | |
| btScalar mot_fact = getMotorFactor(m_angPos, m_lowerAngLimit, m_upperAngLimit, getTargetAngMotorVelocity(), info->fps * info->erp); | |
| 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 * info->erp; | |
| info->m_constraintError[srow] += k * limit_err; | |
| info->cfm[srow] = btScalar(0.0); // stop_cfm; | |
| 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 | |
| } | |
| void btSliderConstraint::solveConstraintObsolete(btSolverBody& bodyA,btSolverBody& bodyB,btScalar timeStep) | |
| { | |
| if (m_useSolveConstraintObsolete) | |
| { | |
| m_timeStep = timeStep; | |
| if(m_useLinearReferenceFrameA) | |
| { | |
| solveConstraintInt(m_rbA,bodyA, m_rbB,bodyB); | |
| } | |
| else | |
| { | |
| solveConstraintInt(m_rbB,bodyB, m_rbA,bodyA); | |
| } | |
| } | |
| } | |
| void btSliderConstraint::solveConstraintInt(btRigidBody& rbA, btSolverBody& bodyA,btRigidBody& rbB, btSolverBody& bodyB) | |
| { | |
| #ifndef __SPU__ | |
| int i; | |
| // linear | |
| btVector3 velA; | |
| bodyA.getVelocityInLocalPointObsolete(m_relPosA,velA); | |
| btVector3 velB; | |
| bodyB.getVelocityInLocalPointObsolete(m_relPosB,velB); | |
| btVector3 vel = velA - velB; | |
| for(i = 0; i < 3; i++) | |
| { | |
| const btVector3& normal = m_jacLin[i].m_linearJointAxis; | |
| btScalar rel_vel = normal.dot(vel); | |
| // calculate positional error | |
| btScalar depth = m_depth[i]; | |
| // get parameters | |
| btScalar softness = (i) ? m_softnessOrthoLin : (m_solveLinLim ? m_softnessLimLin : m_softnessDirLin); | |
| btScalar restitution = (i) ? m_restitutionOrthoLin : (m_solveLinLim ? m_restitutionLimLin : m_restitutionDirLin); | |
| btScalar damping = (i) ? m_dampingOrthoLin : (m_solveLinLim ? m_dampingLimLin : m_dampingDirLin); | |
| // calcutate and apply impulse | |
| btScalar normalImpulse = softness * (restitution * depth / m_timeStep - damping * rel_vel) * m_jacLinDiagABInv[i]; | |
| btVector3 impulse_vector = normal * normalImpulse; | |
| //rbA.applyImpulse( impulse_vector, m_relPosA); | |
| //rbB.applyImpulse(-impulse_vector, m_relPosB); | |
| { | |
| btVector3 ftorqueAxis1 = m_relPosA.cross(normal); | |
| btVector3 ftorqueAxis2 = m_relPosB.cross(normal); | |
| bodyA.applyImpulse(normal*rbA.getInvMass(), rbA.getInvInertiaTensorWorld()*ftorqueAxis1,normalImpulse); | |
| bodyB.applyImpulse(normal*rbB.getInvMass(), rbB.getInvInertiaTensorWorld()*ftorqueAxis2,-normalImpulse); | |
| } | |
| if(m_poweredLinMotor && (!i)) | |
| { // apply linear motor | |
| if(m_accumulatedLinMotorImpulse < m_maxLinMotorForce) | |
| { | |
| btScalar desiredMotorVel = m_targetLinMotorVelocity; | |
| btScalar motor_relvel = desiredMotorVel + rel_vel; | |
| normalImpulse = -motor_relvel * m_jacLinDiagABInv[i]; | |
| // clamp accumulated impulse | |
| btScalar new_acc = m_accumulatedLinMotorImpulse + btFabs(normalImpulse); | |
| if(new_acc > m_maxLinMotorForce) | |
| { | |
| new_acc = m_maxLinMotorForce; | |
| } | |
| btScalar del = new_acc - m_accumulatedLinMotorImpulse; | |
| if(normalImpulse < btScalar(0.0)) | |
| { | |
| normalImpulse = -del; | |
| } | |
| else | |
| { | |
| normalImpulse = del; | |
| } | |
| m_accumulatedLinMotorImpulse = new_acc; | |
| // apply clamped impulse | |
| impulse_vector = normal * normalImpulse; | |
| //rbA.applyImpulse( impulse_vector, m_relPosA); | |
| //rbB.applyImpulse(-impulse_vector, m_relPosB); | |
| { | |
| btVector3 ftorqueAxis1 = m_relPosA.cross(normal); | |
| btVector3 ftorqueAxis2 = m_relPosB.cross(normal); | |
| bodyA.applyImpulse(normal*rbA.getInvMass(), rbA.getInvInertiaTensorWorld()*ftorqueAxis1,normalImpulse); | |
| bodyB.applyImpulse(normal*rbB.getInvMass(), rbB.getInvInertiaTensorWorld()*ftorqueAxis2,-normalImpulse); | |
| } | |
| } | |
| } | |
| } | |
| // angular | |
| // get axes in world space | |
| btVector3 axisA = m_calculatedTransformA.getBasis().getColumn(0); | |
| btVector3 axisB = m_calculatedTransformB.getBasis().getColumn(0); | |
| btVector3 angVelA; | |
| bodyA.getAngularVelocity(angVelA); | |
| btVector3 angVelB; | |
| bodyB.getAngularVelocity(angVelB); | |
| btVector3 angVelAroundAxisA = axisA * axisA.dot(angVelA); | |
| btVector3 angVelAroundAxisB = axisB * axisB.dot(angVelB); | |
| btVector3 angAorthog = angVelA - angVelAroundAxisA; | |
| btVector3 angBorthog = angVelB - angVelAroundAxisB; | |
| btVector3 velrelOrthog = angAorthog-angBorthog; | |
| //solve orthogonal angular velocity correction | |
| btScalar len = velrelOrthog.length(); | |
| btScalar orthorImpulseMag = 0.f; | |
| if (len > btScalar(0.00001)) | |
| { | |
| btVector3 normal = velrelOrthog.normalized(); | |
| btScalar denom = rbA.computeAngularImpulseDenominator(normal) + rbB.computeAngularImpulseDenominator(normal); | |
| //velrelOrthog *= (btScalar(1.)/denom) * m_dampingOrthoAng * m_softnessOrthoAng; | |
| orthorImpulseMag = (btScalar(1.)/denom) * m_dampingOrthoAng * m_softnessOrthoAng; | |
| } | |
| //solve angular positional correction | |
| btVector3 angularError = axisA.cross(axisB) *(btScalar(1.)/m_timeStep); | |
| btVector3 angularAxis = angularError; | |
| btScalar angularImpulseMag = 0; | |
| btScalar len2 = angularError.length(); | |
| if (len2>btScalar(0.00001)) | |
| { | |
| btVector3 normal2 = angularError.normalized(); | |
| btScalar denom2 = rbA.computeAngularImpulseDenominator(normal2) + rbB.computeAngularImpulseDenominator(normal2); | |
| angularImpulseMag = (btScalar(1.)/denom2) * m_restitutionOrthoAng * m_softnessOrthoAng; | |
| angularError *= angularImpulseMag; | |
| } | |
| // apply impulse | |
| //rbA.applyTorqueImpulse(-velrelOrthog+angularError); | |
| //rbB.applyTorqueImpulse(velrelOrthog-angularError); | |
| bodyA.applyImpulse(btVector3(0,0,0), rbA.getInvInertiaTensorWorld()*velrelOrthog,-orthorImpulseMag); | |
| bodyB.applyImpulse(btVector3(0,0,0), rbB.getInvInertiaTensorWorld()*velrelOrthog,orthorImpulseMag); | |
| bodyA.applyImpulse(btVector3(0,0,0), rbA.getInvInertiaTensorWorld()*angularAxis,angularImpulseMag); | |
| bodyB.applyImpulse(btVector3(0,0,0), rbB.getInvInertiaTensorWorld()*angularAxis,-angularImpulseMag); | |
| btScalar impulseMag; | |
| //solve angular limits | |
| if(m_solveAngLim) | |
| { | |
| impulseMag = (angVelB - angVelA).dot(axisA) * m_dampingLimAng + m_angDepth * m_restitutionLimAng / m_timeStep; | |
| impulseMag *= m_kAngle * m_softnessLimAng; | |
| } | |
| else | |
| { | |
| impulseMag = (angVelB - angVelA).dot(axisA) * m_dampingDirAng + m_angDepth * m_restitutionDirAng / m_timeStep; | |
| impulseMag *= m_kAngle * m_softnessDirAng; | |
| } | |
| btVector3 impulse = axisA * impulseMag; | |
| //rbA.applyTorqueImpulse(impulse); | |
| //rbB.applyTorqueImpulse(-impulse); | |
| bodyA.applyImpulse(btVector3(0,0,0), rbA.getInvInertiaTensorWorld()*axisA,impulseMag); | |
| bodyB.applyImpulse(btVector3(0,0,0), rbB.getInvInertiaTensorWorld()*axisA,-impulseMag); | |
| //apply angular motor | |
| if(m_poweredAngMotor) | |
| { | |
| if(m_accumulatedAngMotorImpulse < m_maxAngMotorForce) | |
| { | |
| btVector3 velrel = angVelAroundAxisA - angVelAroundAxisB; | |
| btScalar projRelVel = velrel.dot(axisA); | |
| btScalar desiredMotorVel = m_targetAngMotorVelocity; | |
| btScalar motor_relvel = desiredMotorVel - projRelVel; | |
| btScalar angImpulse = m_kAngle * motor_relvel; | |
| // clamp accumulated impulse | |
| btScalar new_acc = m_accumulatedAngMotorImpulse + btFabs(angImpulse); | |
| if(new_acc > m_maxAngMotorForce) | |
| { | |
| new_acc = m_maxAngMotorForce; | |
| } | |
| btScalar del = new_acc - m_accumulatedAngMotorImpulse; | |
| if(angImpulse < btScalar(0.0)) | |
| { | |
| angImpulse = -del; | |
| } | |
| else | |
| { | |
| angImpulse = del; | |
| } | |
| m_accumulatedAngMotorImpulse = new_acc; | |
| // apply clamped impulse | |
| btVector3 motorImp = angImpulse * axisA; | |
| //rbA.applyTorqueImpulse(motorImp); | |
| //rbB.applyTorqueImpulse(-motorImp); | |
| bodyA.applyImpulse(btVector3(0,0,0), rbA.getInvInertiaTensorWorld()*axisA,angImpulse); | |
| bodyB.applyImpulse(btVector3(0,0,0), rbB.getInvInertiaTensorWorld()*axisA,-angImpulse); | |
| } | |
| } | |
| #endif //__SPU__ | |
| } | |
| 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)); | |
| 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; | |
| } |