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
}
