diff --git a/Demos/SliderConstraintDemo/SliderConstraintDemo.cpp b/Demos/SliderConstraintDemo/SliderConstraintDemo.cpp index b60a6e80c..834d43f7f 100755 --- a/Demos/SliderConstraintDemo/SliderConstraintDemo.cpp +++ b/Demos/SliderConstraintDemo/SliderConstraintDemo.cpp @@ -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() //----------------------------------------------------------------------------- diff --git a/Demos/SliderConstraintDemo/SliderConstraintDemo.h b/Demos/SliderConstraintDemo/SliderConstraintDemo.h index 9b582da51..7987a93b9 100755 --- a/Demos/SliderConstraintDemo/SliderConstraintDemo.h +++ b/Demos/SliderConstraintDemo/SliderConstraintDemo.h @@ -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 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 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 + diff --git a/src/BulletCollision/BroadphaseCollision/btDbvt.h b/src/BulletCollision/BroadphaseCollision/btDbvt.h index 1eee2cb24..456ed540b 100644 --- a/src/BulletCollision/BroadphaseCollision/btDbvt.h +++ b/src/BulletCollision/BroadphaseCollision/btDbvt.h @@ -25,7 +25,7 @@ subject to the following restrictions: // #ifdef WIN32 -#define DBVT_USE_TEMPLATE +//#define DBVT_USE_TEMPLATE #endif #ifdef DBVT_USE_TEMPLATE diff --git a/src/BulletDynamics/ConstraintSolver/btGeneric6DofConstraint.cpp b/src/BulletDynamics/ConstraintSolver/btGeneric6DofConstraint.cpp index c9678baa2..077b326d1 100644 --- a/src/BulletDynamics/ConstraintSolver/btGeneric6DofConstraint.cpp +++ b/src/BulletDynamics/ConstraintSolver/btGeneric6DofConstraint.cpp @@ -14,7 +14,7 @@ subject to the following restrictions: */ /* 2007-09-09 -Refactored by Francisco Leó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() diff --git a/src/BulletDynamics/ConstraintSolver/btGeneric6DofConstraint.h b/src/BulletDynamics/ConstraintSolver/btGeneric6DofConstraint.h index e4683b91b..f0718d2d4 100644 --- a/src/BulletDynamics/ConstraintSolver/btGeneric6DofConstraint.h +++ b/src/BulletDynamics/ConstraintSolver/btGeneric6DofConstraint.h @@ -14,7 +14,7 @@ subject to the following restrictions: */ /* 2007-09-09 -btGeneric6DofConstraint Refactored by Francisco Leó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 }; diff --git a/src/BulletDynamics/ConstraintSolver/btOdeTypedJoint.cpp b/src/BulletDynamics/ConstraintSolver/btOdeTypedJoint.cpp index e61cb9bb3..022e6a8ed 100644 --- a/src/BulletDynamics/ConstraintSolver/btOdeTypedJoint.cpp +++ b/src/BulletDynamics/ConstraintSolver/btOdeTypedJoint.cpp @@ -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() + +//---------------------------------------------------------------------------------- +//---------------------------------------------------------------------------------- + + + + diff --git a/src/BulletDynamics/ConstraintSolver/btOdeTypedJoint.h b/src/BulletDynamics/ConstraintSolver/btOdeTypedJoint.h index 429bd7813..ae3e078b6 100644 --- a/src/BulletDynamics/ConstraintSolver/btOdeTypedJoint.h +++ b/src/BulletDynamics/ConstraintSolver/btOdeTypedJoint.h @@ -14,7 +14,7 @@ subject to the following restrictions: */ /* 2007-09-09 -Added support for typed joints by Francisco Leó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(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 + + diff --git a/src/BulletDynamics/ConstraintSolver/btSliderConstraint.cpp b/src/BulletDynamics/ConstraintSolver/btSliderConstraint.cpp index 6a326e264..ce67a9440 100755 --- a/src/BulletDynamics/ConstraintSolver/btSliderConstraint.cpp +++ b/src/BulletDynamics/ConstraintSolver/btSliderConstraint.cpp @@ -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() + + +//----------------------------------------------------------------------------- + + diff --git a/src/BulletDynamics/ConstraintSolver/btSliderConstraint.h b/src/BulletDynamics/ConstraintSolver/btSliderConstraint.h index a078e6266..905c5d8ad 100755 --- a/src/BulletDynamics/ConstraintSolver/btSliderConstraint.h +++ b/src/BulletDynamics/ConstraintSolver/btSliderConstraint.h @@ -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); }; //-----------------------------------------------------------------------------