d58081ce37
virtual void btTypedConstraint::setParam(int num, btScalar value, int axis = -1) = 0; virtual btScalar btTypedConstraint::getParam(int num, int axis = -1) const = 0; Parameter can be BT_CONSTRAINT_ERP,BT_CONSTRAINT_STOP_ERP,BT_CONSTRAINT_CFM,BT_CONSTRAINT_STOP_CFM Axis is 0 .. 5, first 3 for linear degrees of freedom, last 3 for angular. If no axis is specified it will take the 'default' degree of freedom. For a btHingeConstraint this would be the hinge axis (5)
1450 lines
44 KiB
C++
Executable File
1450 lines
44 KiB
C++
Executable File
/*
|
|
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_useLinearReferenceFrameA = USE_OFFSET_FOR_CONSTANT_FRAME;
|
|
m_flags = 0;
|
|
|
|
calculateTransforms(m_rbA.getCenterOfMassTransform(),m_rbB.getCenterOfMassTransform());
|
|
}
|
|
|
|
|
|
|
|
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)
|
|
{
|
|
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 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::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());
|
|
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)
|
|
{
|
|
if(m_useOffsetForConstraintFrame)
|
|
{
|
|
getInfo2NonVirtualUsingFrameOffset(info,m_rbA.getCenterOfMassTransform(),m_rbB.getCenterOfMassTransform(), m_rbA.getLinearVelocity(),m_rbB.getLinearVelocity(), m_rbA.getInvMass(),m_rbB.getInvMass());
|
|
}
|
|
else
|
|
{
|
|
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();
|
|
btScalar currERP = (m_flags & BT_SLIDER_FLAGS_ERP_ORTANG) ? m_softnessOrthoAng : m_softnessOrthoAng * info->erp;
|
|
btScalar k = info->fps * currERP;
|
|
|
|
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);
|
|
if(m_flags & BT_SLIDER_FLAGS_CFM_ORTANG)
|
|
{
|
|
info->cfm[0] = m_cfmOrthoAng;
|
|
info->cfm[s] = m_cfmOrthoAng;
|
|
}
|
|
// 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();
|
|
currERP = (m_flags & BT_SLIDER_FLAGS_ERP_ORTLIN) ? m_softnessOrthoLin : m_softnessOrthoLin * info->erp;
|
|
k = info->fps * currERP;
|
|
|
|
info->m_constraintError[s2] = k * p.dot(ofs);
|
|
info->m_constraintError[s3] = k * q.dot(ofs);
|
|
if(m_flags & BT_SLIDER_FLAGS_CFM_ORTLIN)
|
|
{
|
|
info->cfm[s2] = m_cfmOrthoLin;
|
|
info->cfm[s3] = m_cfmOrthoLin;
|
|
}
|
|
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.;
|
|
currERP = (m_flags & BT_SLIDER_FLAGS_ERP_LIMLIN) ? m_softnessLimLin : info->erp;
|
|
if(powered)
|
|
{
|
|
if(m_flags & BT_SLIDER_FLAGS_CFM_DIRLIN)
|
|
{
|
|
info->cfm[nrow] = 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] += 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 * 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[nrow] = 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[nrow] = 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
|
|
}
|
|
|
|
|
|
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));
|
|
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::getInfo2NonVirtualUsingFrameOffset(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;
|
|
// get the desired direction of slider axis
|
|
// as weighted sum of X-orthos of frameA and frameB in WCS
|
|
btVector3 ax1A = trA.getBasis().getColumn(0);
|
|
btVector3 ax1B = trB.getBasis().getColumn(0);
|
|
btVector3 ax1 = ax1A * factA + ax1B * factB;
|
|
ax1.normalize();
|
|
// construct two orthos to slider axis
|
|
btVector3 p, q;
|
|
btPlaneSpace1 (ax1, p, q);
|
|
// 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, tmpB, relA, relB;
|
|
// 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];
|
|
// 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.
|
|
// 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];
|
|
}
|
|
// 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[nrow] = 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[nrow] = m_cfmDirAng;
|
|
}
|
|
info->cfm[srow] = btScalar(0.0);
|
|
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[nrow] = 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;
|
|
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;
|
|
}
|
|
|
|
|
|
|