real_t SliderJointBullet::get_param(PhysicsServer::SliderJointParam p_param) const { switch (p_param) { case PhysicsServer::SLIDER_JOINT_LINEAR_LIMIT_UPPER: return getUpperLinLimit(); case PhysicsServer::SLIDER_JOINT_LINEAR_LIMIT_LOWER: return getLowerLinLimit(); case PhysicsServer::SLIDER_JOINT_LINEAR_LIMIT_SOFTNESS: return getSoftnessLimLin(); case PhysicsServer::SLIDER_JOINT_LINEAR_LIMIT_RESTITUTION: return getRestitutionLimLin(); case PhysicsServer::SLIDER_JOINT_LINEAR_LIMIT_DAMPING: return getDampingLimLin(); case PhysicsServer::SLIDER_JOINT_LINEAR_MOTION_SOFTNESS: return getSoftnessDirLin(); case PhysicsServer::SLIDER_JOINT_LINEAR_MOTION_RESTITUTION: return getRestitutionDirLin(); case PhysicsServer::SLIDER_JOINT_LINEAR_MOTION_DAMPING: return getDampingDirLin(); case PhysicsServer::SLIDER_JOINT_LINEAR_ORTHOGONAL_SOFTNESS: return getSoftnessOrthoLin(); case PhysicsServer::SLIDER_JOINT_LINEAR_ORTHOGONAL_RESTITUTION: return getRestitutionOrthoLin(); case PhysicsServer::SLIDER_JOINT_LINEAR_ORTHOGONAL_DAMPING: return getDampingOrthoLin(); case PhysicsServer::SLIDER_JOINT_ANGULAR_LIMIT_UPPER: return getUpperAngLimit(); case PhysicsServer::SLIDER_JOINT_ANGULAR_LIMIT_LOWER: return getLowerAngLimit(); case PhysicsServer::SLIDER_JOINT_ANGULAR_LIMIT_SOFTNESS: return getSoftnessLimAng(); case PhysicsServer::SLIDER_JOINT_ANGULAR_LIMIT_RESTITUTION: return getRestitutionLimAng(); case PhysicsServer::SLIDER_JOINT_ANGULAR_LIMIT_DAMPING: return getDampingLimAng(); case PhysicsServer::SLIDER_JOINT_ANGULAR_MOTION_SOFTNESS: return getSoftnessDirAng(); case PhysicsServer::SLIDER_JOINT_ANGULAR_MOTION_RESTITUTION: return getRestitutionDirAng(); case PhysicsServer::SLIDER_JOINT_ANGULAR_MOTION_DAMPING: return getDampingDirAng(); case PhysicsServer::SLIDER_JOINT_ANGULAR_ORTHOGONAL_SOFTNESS: return getSoftnessOrthoAng(); case PhysicsServer::SLIDER_JOINT_ANGULAR_ORTHOGONAL_RESTITUTION: return getRestitutionOrthoAng(); case PhysicsServer::SLIDER_JOINT_ANGULAR_ORTHOGONAL_DAMPING: return getDampingOrthoAng(); default: return 0; } }
void btSliderConstraint::getInfo2(btConstraintInfo2* info) { btAssert(!m_useSolveConstraintObsolete); int i, s = info->rowskip; const btTransform& trA = getCalculatedTransformA(); const btTransform& trB = getCalculatedTransformB(); 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 = m_rbA.getCenterOfMassTransform(); btTransform bodyB_trans = m_rbB.getCenterOfMassTransform(); int s2 = 2 * s, s3 = 3 * s; btVector3 c; btScalar miA = m_rbA.getInvMass(); btScalar miB = m_rbB.getInvMass(); 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 = m_rbA.getLinearVelocity().dot(ax1); vel -= m_rbB.getLinearVelocity().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 }