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

73
Demos/SliderConstraintDemo/SliderConstraintDemo.cpp
View File

@@ -16,6 +16,9 @@ subject to the following restrictions:
/*
Added by Roman Ponomarev (rponom@gmail.com)
April 04, 2008
Added support for ODE sover
April 24, 2008
*/
//-----------------------------------------------------------------------------
@@ -35,12 +38,19 @@ April 04, 2008
//-----------------------------------------------------------------------------
#define SLIDER_DEMO_USE_ODE_SOLVER 0
#define SLIDER_DEMO_USE_6DOF 0
#define CUBE_HALF_EXTENTS 1.f
//-----------------------------------------------------------------------------
// A couple of sliders
static btSliderConstraint *spSlider1, *spSlider2;
#if SLIDER_DEMO_USE_6DOF
static btGeneric6DofConstraint *spSlider1, *spSlider2;
#else
static btSliderConstraint *spSlider1, *spSlider2;
#endif
//-----------------------------------------------------------------------------
@@ -87,6 +97,13 @@ static void draw_axes(const btRigidBody& rb, const btTransform& frame)
//-----------------------------------------------------------------------------
#if SLIDER_DEMO_USE_6DOF
static void drawSlider(btGeneric6DofConstraint* pSlider)
{
draw_axes(pSlider->getRigidBodyA(), pSlider->getFrameOffsetA());
draw_axes(pSlider->getRigidBodyB(), pSlider->getFrameOffsetB());
} // drawSlider()
#else
static void drawSlider(btSliderConstraint* pSlider)
{
draw_axes(pSlider->getRigidBodyA(), pSlider->getFrameOffsetA());
@@ -111,6 +128,7 @@ static void drawSlider(btSliderConstraint* pSlider)
glVertex3d(to.getX(), to.getY(), to.getZ());
glEnd();
} // drawSlider()
#endif
//-----------------------------------------------------------------------------
@@ -124,7 +142,13 @@ void SliderConstraintDemo::initPhysics()
btVector3 worldMin(-1000,-1000,-1000);
btVector3 worldMax(1000,1000,1000);
m_overlappingPairCache = new btAxisSweep3(worldMin,worldMax);
#if SLIDER_DEMO_USE_ODE_SOLVER
m_constraintSolver = new btOdeQuickstepConstraintSolver();
#else
m_constraintSolver = new btSequentialImpulseConstraintSolver();
#endif
btDiscreteDynamicsWorld* wp = new btDiscreteDynamicsWorld(m_dispatcher,m_overlappingPairCache,m_constraintSolver,m_collisionConfiguration);
// wp->getSolverInfo().m_numIterations = 20; // default is 10
m_dynamicsWorld = wp;
@@ -162,11 +186,25 @@ void SliderConstraintDemo::initPhysics()
frameInA = btTransform::getIdentity();
frameInB = btTransform::getIdentity();
#if SLIDER_DEMO_USE_6DOF
spSlider1 = new btGeneric6DofConstraint(*pRbA1, *pRbB1, frameInA, frameInB, true);
btVector3 lowerSliderLimit = btVector3(-20,0,0);
btVector3 hiSliderLimit = btVector3(-10,0,0);
// btVector3 lowerSliderLimit = btVector3(-20,-5,-5);
// btVector3 hiSliderLimit = btVector3(-10,5,5);
spSlider1->setLinearLowerLimit(lowerSliderLimit);
spSlider1->setLinearUpperLimit(hiSliderLimit);
spSlider1->setAngularLowerLimit(btVector3(0,0,0));
spSlider1->setAngularUpperLimit(btVector3(0,0,0));
#else
spSlider1 = new btSliderConstraint(*pRbA1, *pRbB1, frameInA, frameInB, true);
spSlider1->setLowerLinLimit(-15.0F);
spSlider1->setUpperLinLimit(-5.0F);
// spSlider1->setLowerLinLimit(-10.0F);
// spSlider1->setUpperLinLimit(-10.0F);
spSlider1->setLowerAngLimit(-SIMD_PI / 3.0F);
spSlider1->setUpperAngLimit( SIMD_PI / 3.0F);
#endif
m_dynamicsWorld->addConstraint(spSlider1, true);
@@ -183,17 +221,44 @@ void SliderConstraintDemo::initPhysics()
pRbB2->setActivationState(DISABLE_DEACTIVATION);
// create slider constraint between A2 and B2 and add it to world
#if SLIDER_DEMO_USE_6DOF
spSlider2 = new btGeneric6DofConstraint(*pRbA2, *pRbB2, frameInA, frameInB, true);
spSlider2->setLinearLowerLimit(lowerSliderLimit);
spSlider2->setLinearUpperLimit(hiSliderLimit);
spSlider2->setAngularLowerLimit(btVector3(0,0,0));
spSlider2->setAngularUpperLimit(btVector3(0,0,0));
#else
spSlider2 = new btSliderConstraint(*pRbA2, *pRbB2, frameInA, frameInB, true);
spSlider2->setLowerLinLimit(-15.0F);
spSlider2->setLowerLinLimit(-25.0F);
spSlider2->setUpperLinLimit(-5.0F);
spSlider2->setLowerAngLimit(-SIMD_PI / 3.0F);
spSlider2->setUpperAngLimit( SIMD_PI / 3.0F);
spSlider2->setLowerAngLimit(SIMD_PI / 2.0F);
spSlider2->setUpperAngLimit(-SIMD_PI / 2.0F);
// add motors
spSlider2->setPoweredLinMotor(true);
spSlider2->setMaxLinMotorForce(0.1);
spSlider2->setTargetLinMotorVelocity(5.0);
spSlider2->setPoweredAngMotor(true);
// spSlider2->setMaxAngMotorForce(0.01);
spSlider2->setMaxAngMotorForce(10.0);
spSlider2->setTargetAngMotorVelocity(1.0);
// change default damping and restitution
spSlider2->setDampingDirLin(0.005F);
spSlider2->setRestitutionLimLin(1.1F);
// various ODE tests
// spSlider2->setDampingLimLin(0.1F); // linear bounce factor for ODE == 1.0 - DampingLimLin;
// spSlider2->setDampingLimAng(0.1F); // angular bounce factor for ODE == 1.0 - DampingLimAng;
// spSlider2->setSoftnessOrthoAng(0.1);
// spSlider2->setSoftnessOrthoLin(0.1);
// spSlider2->setSoftnessLimLin(0.1);
// spSlider2->setSoftnessLimAng(0.1);
#endif
m_dynamicsWorld->addConstraint(spSlider2, true);
} // SliderConstraintDemo::initPhysics()
//-----------------------------------------------------------------------------

142
Demos/SliderConstraintDemo/SliderConstraintDemo.h
View File

@@ -1,71 +1,71 @@
/*
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.
*/
//-----------------------------------------------------------------------------
#ifndef SLIDER_CONSTRAINT_DEMO_H
#define SLIDER_CONSTRAINT_DEMO_H
//-----------------------------------------------------------------------------
#include "btBulletDynamicsCommon.h"
#include "BulletDynamics/ConstraintSolver/btSliderConstraint.h"
#include "DemoApplication.h"
//-----------------------------------------------------------------------------
/// SliderConstraintDemo shows how to create a slider constraint
class SliderConstraintDemo : public DemoApplication
{
//keep track of variables to delete memory at the end
btAlignedObjectArray<btCollisionShape*> m_collisionShapes;
class btBroadphaseInterface* m_overlappingPairCache;
class btCollisionDispatcher* m_dispatcher;
class btConstraintSolver* m_constraintSolver;
class btDefaultCollisionConfiguration* m_collisionConfiguration;
public:
virtual ~SliderConstraintDemo();
void initPhysics();
void initModel();
void drawSliders();
void drawSliderConstraint(btSliderConstraint* constraint);
virtual void clientMoveAndDisplay();
virtual void displayCallback();
static DemoApplication* Create()
{
SliderConstraintDemo* demo = new SliderConstraintDemo();
demo->myinit();
demo->initPhysics();
return demo;
}
};
#endif //SLIDER_CONSTRAINT_DEMO_H
/*
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.
*/
//-----------------------------------------------------------------------------
#ifndef SLIDER_CONSTRAINT_DEMO_H
#define SLIDER_CONSTRAINT_DEMO_H
//-----------------------------------------------------------------------------
#include "btBulletDynamicsCommon.h"
#include "BulletDynamics/ConstraintSolver/btSliderConstraint.h"
#include "DemoApplication.h"
//-----------------------------------------------------------------------------
/// SliderConstraintDemo shows how to create a slider constraint
class SliderConstraintDemo : public DemoApplication
{
//keep track of variables to delete memory at the end
btAlignedObjectArray<btCollisionShape*> m_collisionShapes;
class btBroadphaseInterface* m_overlappingPairCache;
class btCollisionDispatcher* m_dispatcher;
class btConstraintSolver* m_constraintSolver;
class btDefaultCollisionConfiguration* m_collisionConfiguration;
public:
virtual ~SliderConstraintDemo();
void initPhysics();
void initModel();
void drawSliders();
void drawSliderConstraint(btSliderConstraint* constraint);
virtual void clientMoveAndDisplay();
virtual void displayCallback();
static DemoApplication* Create()
{
SliderConstraintDemo* demo = new SliderConstraintDemo();
demo->myinit();
demo->initPhysics();
return demo;
}
};
#endif //SLIDER_CONSTRAINT_DEMO_H

2
src/BulletCollision/BroadphaseCollision/btDbvt.h
View File

@@ -25,7 +25,7 @@ subject to the following restrictions:
//
#ifdef WIN32
#define DBVT_USE_TEMPLATE
//#define DBVT_USE_TEMPLATE
#endif
#ifdef DBVT_USE_TEMPLATE

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);
};
//-----------------------------------------------------------------------------