Bart/bullet3
1
0
Fork 0
Code Issues Pull Requests Releases Wiki Activity

Added linear and angular motors for the slider constraint

Browse Source
This commit is contained in:
rponom
2008-05-23 22:52:51 +00:00
parent a1578accac
commit 6141a55f09
9 changed files with 839 additions and 131 deletions
Download Patch File Download Diff File Expand all files Collapse all files

44
src/BulletDynamics/ConstraintSolver/btGeneric6DofConstraint.cpp
View File

@@ -14,7 +14,7 @@ subject to the following restrictions:
*/
/*
2007-09-09
Refactored by Francisco Le<EFBFBD>n
Refactored by Francisco Le?n
email: projectileman@yahoo.com
http://gimpact.sf.net
*/
@@ -199,12 +199,15 @@ btScalar btTranslationalLimitMotor::solveLinearAxis(
btRigidBody& body1,const btVector3 &pointInA,
btRigidBody& body2,const btVector3 &pointInB,
int limit_index,
const btVector3 & axis_normal_on_a)
const btVector3 & axis_normal_on_a,
const btVector3 & anchorPos)
{
///find relative velocity
btVector3 rel_pos1 = pointInA - body1.getCenterOfMassPosition();
btVector3 rel_pos2 = pointInB - body2.getCenterOfMassPosition();
// btVector3 rel_pos1 = pointInA - body1.getCenterOfMassPosition();
// btVector3 rel_pos2 = pointInB - body2.getCenterOfMassPosition();
btVector3 rel_pos1 = anchorPos - body1.getCenterOfMassPosition();
btVector3 rel_pos2 = anchorPos - body2.getCenterOfMassPosition();
btVector3 vel1 = body1.getVelocityInLocalPoint(rel_pos1);
btVector3 vel2 = body2.getVelocityInLocalPoint(rel_pos2);
@@ -390,12 +393,15 @@ void btGeneric6DofConstraint::buildJacobian()
//calculates transform
calculateTransforms();
const btVector3& pivotAInW = m_calculatedTransformA.getOrigin();
const btVector3& pivotBInW = m_calculatedTransformB.getOrigin();
// const btVector3& pivotAInW = m_calculatedTransformA.getOrigin();
// const btVector3& pivotBInW = m_calculatedTransformB.getOrigin();
calcAnchorPos();
btVector3 pivotAInW = m_AnchorPos;
btVector3 pivotBInW = m_AnchorPos;
btVector3 rel_pos1 = pivotAInW - m_rbA.getCenterOfMassPosition();
btVector3 rel_pos2 = pivotBInW - m_rbB.getCenterOfMassPosition();
// not used here
// btVector3 rel_pos1 = pivotAInW - m_rbA.getCenterOfMassPosition();
// btVector3 rel_pos2 = pivotBInW - m_rbB.getCenterOfMassPosition();
btVector3 normalWorld;
//linear part
@@ -462,7 +468,7 @@ void btGeneric6DofConstraint::solveConstraint(btScalar timeStep)
jacDiagABInv,
m_rbA,pointInA,
m_rbB,pointInB,
i,linear_axis);
i,linear_axis, m_AnchorPos);
}
}
@@ -501,4 +507,22 @@ btScalar btGeneric6DofConstraint::getAngle(int axis_index) const
return m_calculatedAxisAngleDiff[axis_index];
}
void btGeneric6DofConstraint::calcAnchorPos(void)
{
btScalar imA = m_rbA.getInvMass();
btScalar imB = m_rbB.getInvMass();
btScalar weight;
if(imB == btScalar(0.0))
{
weight = btScalar(1.0);
}
else
{
weight = imA / (imA + imB);
}
const btVector3& pA = m_calculatedTransformA.getOrigin();
const btVector3& pB = m_calculatedTransformB.getOrigin();
m_AnchorPos = pA * weight + pB * (btScalar(1.0) - weight);
return;
} // btGeneric6DofConstraint::calcAnchorPos()

8
src/BulletDynamics/ConstraintSolver/btGeneric6DofConstraint.h
View File

@@ -14,7 +14,7 @@ subject to the following restrictions:
*/
/*
2007-09-09
btGeneric6DofConstraint Refactored by Francisco Le<EFBFBD>n
btGeneric6DofConstraint Refactored by Francisco Le?n
email: projectileman@yahoo.com
http://gimpact.sf.net
*/
@@ -171,7 +171,8 @@ public:
btRigidBody& body1,const btVector3 &pointInA,
btRigidBody& body2,const btVector3 &pointInB,
int limit_index,
const btVector3 & axis_normal_on_a);
const btVector3 & axis_normal_on_a,
const btVector3 & anchorPos);
};
@@ -247,6 +248,8 @@ protected:
btVector3 m_calculatedAxisAngleDiff;
btVector3 m_calculatedAxis[3];
btVector3 m_AnchorPos; // point betwen pivots of bodies A and B to solve linear axes
bool m_useLinearReferenceFrameA;
//!@}
@@ -427,6 +430,7 @@ public:
return m_rbB;
}
virtual void calcAnchorPos(void); // overridable
};

431
src/BulletDynamics/ConstraintSolver/btOdeTypedJoint.cpp
View File

@@ -34,6 +34,12 @@ void btOdeTypedJoint::GetInfo1(Info1 *info)
d6joint.GetInfo1(info);
}
break;
case SLIDER_CONSTRAINT_TYPE:
{
OdeSliderJoint sliderjoint(m_constraint,m_index,m_swapBodies,m_body0,m_body1);
sliderjoint.GetInfo1(info);
}
break;
};
}
@@ -54,6 +60,12 @@ void btOdeTypedJoint::GetInfo2(Info2 *info)
d6joint.GetInfo2(info);
}
break;
case SLIDER_CONSTRAINT_TYPE:
{
OdeSliderJoint sliderjoint(m_constraint,m_index,m_swapBodies,m_body0,m_body1);
sliderjoint.GetInfo2(info);
}
break;
};
}
@@ -436,7 +448,9 @@ int OdeD6Joint::setAngularLimits(Info2 *info, int row_offset)
btVector3 axis = d6constraint->getAxis(i);
row += bt_get_limit_motor_info2(
d6constraint->getRotationalLimitMotor(i),
m_body0->m_originalBody,m_body1->m_originalBody,info,row,axis,1);
m_body0->m_originalBody,
m_body1 ? m_body1->m_originalBody : NULL,
info,row,axis,1);
}
}
@@ -449,3 +463,418 @@ void OdeD6Joint::GetInfo2(Info2 *info)
setAngularLimits(info, row);
}
//----------------------------------------------------------------------------------
//----------------------------------------------------------------------------------
//----------------------------------------------------------------------------------
//----------------------------------------------------------------------------------
/*
OdeSliderJoint
Ported from ODE by Roman Ponomarev (rponom@gmail.com)
April 24, 2008
*/
OdeSliderJoint::OdeSliderJoint(
btTypedConstraint * constraint,
int index,bool swap, btOdeSolverBody* body0, btOdeSolverBody* body1):
btOdeTypedJoint(constraint,index,swap,body0,body1)
{
} // OdeSliderJoint::OdeSliderJoint()
//----------------------------------------------------------------------------------
void OdeSliderJoint::GetInfo1(Info1* info)
{
info->nub = 4;
info->m = 4; // Fixed 2 linear + 2 angular
btSliderConstraint * slider = this->getSliderConstraint();
//prepare constraint
slider->calculateTransforms();
slider->testLinLimits();
if(slider->getSolveLinLimit() || slider->getPoweredLinMotor())
{
info->m++; // limit 3rd linear as well
}
slider->testAngLimits();
if(slider->getSolveAngLimit() || slider->getPoweredAngMotor())
{
info->m++; // limit 3rd angular as well
}
} // OdeSliderJoint::GetInfo1()
//----------------------------------------------------------------------------------
void OdeSliderJoint::GetInfo2(Info2 *info)
{
int i, s = info->rowskip;
btSliderConstraint * slider = this->getSliderConstraint();
const btTransform& trA = slider->getCalculatedTransformA();
const btTransform& trB = slider->getCalculatedTransformB();
// 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->J1a[0] = p[0];
info->J1a[1] = p[1];
info->J1a[2] = p[2];
info->J1a[s+0] = q[0];
info->J1a[s+1] = q[1];
info->J1a[s+2] = q[2];
if(m_body1)
{
info->J2a[0] = -p[0];
info->J2a[1] = -p[1];
info->J2a[2] = -p[2];
info->J2a[s+0] = -q[0];
info->J2a[s+1] = -q[1];
info->J2a[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 * slider->getSoftnessOrthoAng();
btVector3 ax2 = trB.getBasis().getColumn(0);
btVector3 u;
if(m_body1)
{
u = ax1.cross(ax2);
}
else
{
u = ax2.cross(ax1);
}
info->c[0] = k * u.dot(p);
info->c[1] = 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 substitute (w1+w2)/2 for w1, as w1 is supposed to equal w2.
btTransform bodyA_trans = m_body0->m_originalBody->getCenterOfMassTransform();
btTransform bodyB_trans;
if(m_body1)
{
bodyB_trans = m_body1->m_originalBody->getCenterOfMassTransform();
}
int s2 = 2 * s, s3 = 3 * s;
btVector3 c;
if(m_body1)
{
c = bodyB_trans.getOrigin() - bodyA_trans.getOrigin();
btVector3 tmp = btScalar(0.5) * c.cross(p);
for (i=0; i<3; i++) info->J1a[s2+i] = tmp[i];
for (i=0; i<3; i++) info->J2a[s2+i] = tmp[i];
tmp = btScalar(0.5) * c.cross(q);
for (i=0; i<3; i++) info->J1a[s3+i] = tmp[i];
for (i=0; i<3; i++) info->J2a[s3+i] = tmp[i];
for (i=0; i<3; i++) info->J2l[s2+i] = -p[i];
for (i=0; i<3; i++) info->J2l[s3+i] = -q[i];
}
for (i=0; i<3; i++) info->J1l[s2+i] = p[i];
for (i=0; i<3; i++) info->J1l[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
if(m_body1)
{
ofs = trB.getOrigin() - trA.getOrigin();
}
else
{
ofs = trA.getOrigin() - trB.getOrigin();
}
k = info->fps * info->erp * slider->getSoftnessOrthoLin();
info->c[2] = k * p.dot(ofs);
info->c[3] = 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(slider->getSolveLinLimit())
{
limit_err = slider->getLinDepth();
if(m_body1)
{
limit = (limit_err > btScalar(0.0)) ? 1 : 2;
}
else
{
limit = (limit_err > btScalar(0.0)) ? 2 : 1;
}
}
int powered = 0;
if(slider->getPoweredLinMotor())
{
powered = 1;
}
// if the slider has joint limits, add in the extra row
if (limit || powered)
{
nrow++;
srow = nrow * info->rowskip;
info->J1l[srow+0] = ax1[0];
info->J1l[srow+1] = ax1[1];
info->J1l[srow+2] = ax1[2];
if(m_body1)
{
info->J2l[srow+0] = -ax1[0];
info->J2l[srow+1] = -ax1[1];
info->J2l[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 point
// halfway between the body centers. there is no penalty (other than an
// extra tiny bit of computation) in doing this adjustment. note that we
// only need to do this if the constraint connects two bodies.
if (m_body1)
{
dVector3 ltd; // Linear Torque Decoupling vector (a torque)
c = btScalar(0.5) * c;
dCROSS (ltd,=,c,ax1);
info->J1a[srow+0] = ltd[0];
info->J1a[srow+1] = ltd[1];
info->J1a[srow+2] = ltd[2];
info->J2a[srow+0] = ltd[0];
info->J2a[srow+1] = ltd[1];
info->J2a[srow+2] = ltd[2];
}
// right-hand part
btScalar lostop = slider->getLowerLinLimit();
btScalar histop = slider->getUpperLinLimit();
if(limit && (lostop == histop))
{ // the joint motor is ineffective
powered = 0;
}
if(powered)
{
info->cfm[nrow] = btScalar(0.0);
if(!limit)
{
info->c[nrow] = slider->getTargetLinMotorVelocity();
info->lo[nrow] = -slider->getMaxLinMotorForce() * info->fps;
info->hi[nrow] = slider->getMaxLinMotorForce() * info->fps;
}
}
if(limit)
{
k = info->fps * info->erp;
if(m_body1)
{
info->c[nrow] = k * limit_err;
}
else
{
info->c[nrow] = - k * limit_err;
}
info->cfm[nrow] = btScalar(0.0); // stop_cfm;
if(lostop == histop)
{
// limited low and high simultaneously
info->lo[nrow] = -SIMD_INFINITY;
info->hi[nrow] = SIMD_INFINITY;
}
else
{
if(limit == 1)
{
// low limit
info->lo[nrow] = 0;
info->hi[nrow] = SIMD_INFINITY;
}
else
{
// high limit
info->lo[nrow] = -SIMD_INFINITY;
info->hi[nrow] = 0;
}
}
// bounce (we'll use slider parameter abs(1.0 - m_dampingLimLin) for that)
btScalar bounce = btFabs(btScalar(1.0) - slider->getDampingLimLin());
if(bounce > btScalar(0.0))
{
btScalar vel = m_body0->m_originalBody->getLinearVelocity().dot(ax1);
if(m_body1)
{
vel -= m_body1->m_originalBody->getLinearVelocity().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->c[nrow]) info->c[nrow] = newc;
}
}
else
{
// high limit - all those computations are reversed
if(vel > 0)
{
btScalar newc = -bounce * vel;
if(newc < info->c[nrow]) info->c[nrow] = newc;
}
}
}
info->c[nrow] *= slider->getSoftnessLimLin();
} // if(limit)
} // if linear limit
// check angular limits
limit_err = btScalar(0.0);
limit = 0;
if(slider->getSolveAngLimit())
{
limit_err = slider->getAngDepth();
if(m_body1)
{
limit = (limit_err > btScalar(0.0)) ? 1 : 2;
}
else
{
limit = (limit_err > btScalar(0.0)) ? 2 : 1;
}
}
// if the slider has joint limits, add in the extra row
powered = 0;
if(slider->getPoweredAngMotor())
{
powered = 1;
}
if(limit || powered)
{
nrow++;
srow = nrow * info->rowskip;
info->J1a[srow+0] = ax1[0];
info->J1a[srow+1] = ax1[1];
info->J1a[srow+2] = ax1[2];
if(m_body1)
{
info->J2a[srow+0] = -ax1[0];
info->J2a[srow+1] = -ax1[1];
info->J2a[srow+2] = -ax1[2];
}
btScalar lostop = slider->getLowerAngLimit();
btScalar histop = slider->getUpperAngLimit();
if(limit && (lostop == histop))
{ // the joint motor is ineffective
powered = 0;
}
if(powered)
{
info->cfm[nrow] = btScalar(0.0);
if(!limit)
{
info->c[nrow] = slider->getTargetAngMotorVelocity();
info->lo[nrow] = -slider->getMaxAngMotorForce() * info->fps;
info->hi[nrow] = slider->getMaxAngMotorForce() * info->fps;
}
}
if(limit)
{
k = info->fps * info->erp;
if (m_body1)
{
info->c[nrow] = k * limit_err;
}
else
{
info->c[nrow] = -k * limit_err;
}
info->cfm[nrow] = btScalar(0.0); // stop_cfm;
if(lostop == histop)
{
// limited low and high simultaneously
info->lo[nrow] = -SIMD_INFINITY;
info->hi[nrow] = SIMD_INFINITY;
}
else
{
if (limit == 1)
{
// low limit
info->lo[nrow] = 0;
info->hi[nrow] = SIMD_INFINITY;
}
else
{
// high limit
info->lo[nrow] = -SIMD_INFINITY;
info->hi[nrow] = 0;
}
}
// bounce (we'll use slider parameter abs(1.0 - m_dampingLimAng) for that)
btScalar bounce = btFabs(btScalar(1.0) - slider->getDampingLimAng());
if(bounce > btScalar(0.0))
{
btScalar vel = m_body0->m_originalBody->getAngularVelocity().dot(ax1);
if(m_body1)
{
vel -= m_body1->m_originalBody->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->c[nrow]) info->c[nrow] = newc;
}
}
else
{
// high limit - all those computations are reversed
if(vel > 0)
{
btScalar newc = -bounce * vel;
if(newc < info->c[nrow]) info->c[nrow] = newc;
}
}
}
info->c[nrow] *= slider->getSoftnessLimAng();
} // if(limit)
} // if angular limit or powered
} // OdeSliderJoint::GetInfo2()
//----------------------------------------------------------------------------------
//----------------------------------------------------------------------------------

33
src/BulletDynamics/ConstraintSolver/btOdeTypedJoint.h
View File

@@ -14,7 +14,7 @@ subject to the following restrictions:
*/
/*
2007-09-09
Added support for typed joints by Francisco Le<EFBFBD>n
Added support for typed joints by Francisco Le?n
email: projectileman@yahoo.com
http://gimpact.sf.net
*/
@@ -25,6 +25,7 @@ http://gimpact.sf.net
#include "btOdeJoint.h"
#include "BulletDynamics/ConstraintSolver/btPoint2PointConstraint.h"
#include "BulletDynamics/ConstraintSolver/btGeneric6DofConstraint.h"
#include "BulletDynamics/ConstraintSolver/btSliderConstraint.h"
struct btOdeSolverBody;
@@ -107,5 +108,35 @@ int bt_get_limit_motor_info2(
btRigidBody * body0, btRigidBody * body1,
btOdeJoint::Info2 *info, int row, btVector3& ax1, int rotational);
/*
OdeSliderJoint
Ported from ODE by Roman Ponomarev (rponom@gmail.com)
April 24, 2008
*/
class OdeSliderJoint : public btOdeTypedJoint
{
protected:
inline btSliderConstraint * getSliderConstraint()
{
return static_cast<btSliderConstraint * >(m_constraint);
}
public:
OdeSliderJoint() {};
OdeSliderJoint(btTypedConstraint* constraint,int index,bool swap, btOdeSolverBody* body0, btOdeSolverBody* body1);
//BU_Joint interface for solver
virtual void GetInfo1(Info1 *info);
virtual void GetInfo2(Info2 *info);
};
#endif //CONTACT_JOINT_H

206
src/BulletDynamics/ConstraintSolver/btSliderConstraint.cpp
View File

@@ -51,6 +51,17 @@ void btSliderConstraint::initParams()
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::initParams()
//-----------------------------------------------------------------------------
@@ -121,28 +132,7 @@ void btSliderConstraint::buildJacobianInt(btRigidBody& rbA, btRigidBody& rbB, co
m_jacLinDiagABInv[i] = btScalar(1.) / m_jacLin[i].getDiagonal();
m_depth[i] = m_delta.dot(normalWorld);
}
m_solveLinLim = false;
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.);
}
testLinLimits();
// angular part
for(i = 0; i < 3; i++)
{
@@ -155,27 +145,12 @@ void btSliderConstraint::buildJacobianInt(btRigidBody& rbA, btRigidBody& rbB, co
rbB.getInvInertiaDiagLocal()
);
}
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));
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;
}
}
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);
} // btSliderConstraint::buildJacobianInt()
//-----------------------------------------------------------------------------
@@ -217,6 +192,35 @@ void btSliderConstraint::solveConstraintInt(btRigidBody& rbA, btRigidBody& rbB)
btVector3 impulse_vector = normal * normalImpulse;
rbA.applyImpulse( impulse_vector, m_relPosA);
rbB.applyImpulse(-impulse_vector, m_relPosB);
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);
}
}
}
// angular
// get axes in world space
@@ -267,7 +271,127 @@ void btSliderConstraint::solveConstraintInt(btRigidBody& rbA, btRigidBody& rbB)
btVector3 impulse = axisA * impulseMag;
rbA.applyTorqueImpulse(impulse);
rbB.applyTorqueImpulse(-impulse);
//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;
m_rbA.applyTorqueImpulse(motorImp);
m_rbB.applyTorqueImpulse(-motorImp);
}
}
} // btSliderConstraint::solveConstraint()
//-----------------------------------------------------------------------------
//-----------------------------------------------------------------------------
void btSliderConstraint::calculateTransforms(void){
if(m_useLinearReferenceFrameA)
{
m_calculatedTransformA = m_rbA.getCenterOfMassTransform() * m_frameInA;
m_calculatedTransformB = m_rbB.getCenterOfMassTransform() * m_frameInB;
}
else
{
m_calculatedTransformA = m_rbB.getCenterOfMassTransform() * m_frameInB;
m_calculatedTransformB = m_rbA.getCenterOfMassTransform() * m_frameInA;
}
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;
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);
}
} // btSliderConstraint::calculateTransforms()
//-----------------------------------------------------------------------------
void btSliderConstraint::testLinLimits(void)
{
m_solveLinLim = false;
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.);
}
} // btSliderConstraint::testLinLimits()
//-----------------------------------------------------------------------------
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));
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;
}
}
} // btSliderConstraint::testAngLimits()
//-----------------------------------------------------------------------------

31
src/BulletDynamics/ConstraintSolver/btSliderConstraint.h
View File

@@ -106,6 +106,15 @@ protected:
btScalar m_angDepth;
btScalar m_kAngle;
bool m_poweredLinMotor;
btScalar m_targetLinMotorVelocity;
btScalar m_maxLinMotorForce;
btScalar m_accumulatedLinMotorImpulse;
bool m_poweredAngMotor;
btScalar m_targetAngMotorVelocity;
btScalar m_maxAngMotorForce;
btScalar m_accumulatedAngMotorImpulse;
//------------------------
void initParams();
@@ -170,9 +179,31 @@ public:
void setSoftnessOrthoAng(btScalar softnessOrthoAng) { m_softnessOrthoAng = softnessOrthoAng; }
void setRestitutionOrthoAng(btScalar restitutionOrthoAng) { m_restitutionOrthoAng = restitutionOrthoAng; }
void setDampingOrthoAng(btScalar dampingOrthoAng) { m_dampingOrthoAng = dampingOrthoAng; }
void setPoweredLinMotor(bool onOff) { m_poweredLinMotor = onOff; }
bool getPoweredLinMotor() { return m_poweredLinMotor; }
void setTargetLinMotorVelocity(btScalar targetLinMotorVelocity) { m_targetLinMotorVelocity = targetLinMotorVelocity; }
btScalar getTargetLinMotorVelocity() { return m_targetLinMotorVelocity; }
void setMaxLinMotorForce(btScalar maxLinMotorForce) { m_maxLinMotorForce = maxLinMotorForce; }
btScalar getMaxLinMotorForce() { return m_maxLinMotorForce; }
void setPoweredAngMotor(bool onOff) { m_poweredAngMotor = onOff; }
bool getPoweredAngMotor() { return m_poweredAngMotor; }
void setTargetAngMotorVelocity(btScalar targetAngMotorVelocity) { m_targetAngMotorVelocity = targetAngMotorVelocity; }
btScalar getTargetAngMotorVelocity() { return m_targetAngMotorVelocity; }
void setMaxAngMotorForce(btScalar maxAngMotorForce) { m_maxAngMotorForce = maxAngMotorForce; }
btScalar getMaxAngMotorForce() { return m_maxAngMotorForce; }
// access for ODE solver
bool getSolveLinLimit() { return m_solveLinLim; }
btScalar getLinDepth() { return m_depth[0]; }
bool getSolveAngLimit() { return m_solveAngLim; }
btScalar getAngDepth() { return m_angDepth; }
// internal
void buildJacobianInt(btRigidBody& rbA, btRigidBody& rbB, const btTransform& frameInA, const btTransform& frameInB);
void solveConstraintInt(btRigidBody& rbA, btRigidBody& rbB);
// shared code used by ODE solver
void calculateTransforms(void);
void testLinLimits(void);
void testAngLimits(void);
};
//-----------------------------------------------------------------------------