From c49927b4ed18edee0b7be92fb1a61c61b8c62653 Mon Sep 17 00:00:00 2001 From: "erwin.coumans" Date: Wed, 17 Nov 2010 23:55:39 +0000 Subject: [PATCH] Use cylinder inertia tensor, instead of box, by default. See Issue 427 Expose some internal data for constraints, Thanks to Francois Sugny, see Issue 420 --- Demos/OpenGL/GL_DialogDynamicsWorld.cpp | 6 +- .../CollisionShapes/btCylinderShape.cpp | 61 ++++++++++++++++++- .../btConeTwistConstraint.cpp | 8 +++ .../ConstraintSolver/btConeTwistConstraint.h | 11 ++++ .../btGeneric6DofConstraint.cpp | 9 +++ .../btGeneric6DofConstraint.h | 23 +++++++ .../ConstraintSolver/btHingeConstraint.cpp | 8 ++- .../ConstraintSolver/btHingeConstraint.h | 17 +++++- 8 files changed, 133 insertions(+), 10 deletions(-) diff --git a/Demos/OpenGL/GL_DialogDynamicsWorld.cpp b/Demos/OpenGL/GL_DialogDynamicsWorld.cpp index 3f049fc43..0fa5b46f7 100644 --- a/Demos/OpenGL/GL_DialogDynamicsWorld.cpp +++ b/Demos/OpenGL/GL_DialogDynamicsWorld.cpp @@ -229,7 +229,7 @@ GL_SliderControl* GL_DialogDynamicsWorld::createSlider(GL_DialogWindow* dialog, int sliderX = dialog->getDialogHorPos() - m_screenWidth/2 + dialog->getDialogWidth()/2; // int sliderY = dialog->getDialogVertPos() + m_screenHeight/2 + dialog->getDialogHeight()/2 + dialog->getNumControls()*20; int sliderY = dialog->getDialogVertPos() - m_screenHeight/2 + dialog->getDialogHeight()/2 + dialog->getNumControls()*20; - trans.setOrigin(btVector3(sliderX, sliderY,-0.2f)); + trans.setOrigin(btVector3((btScalar)sliderX, (btScalar)sliderY,(btScalar)-0.2f)); body->setWorldTransform(trans); //body->setDamping(0.999,0.99); @@ -251,7 +251,7 @@ GL_SliderControl* GL_DialogDynamicsWorld::createSlider(GL_DialogWindow* dialog, frameInA.setIdentity(); int offsX = -dialog->getDialogWidth()/2 + 16; int offsY = -dialog->getDialogHeight()/2 + dialog->getNumControls()*20 + 36; - btVector3 offset(btVector3(offsX, offsY, 0.2f)); + btVector3 offset(btVector3((btScalar)offsX, (btScalar)offsY, (btScalar)0.2f)); frameInA.setOrigin(offset); @@ -309,7 +309,7 @@ GL_ToggleControl* GL_DialogDynamicsWorld::createToggle(GL_DialogWindow* dialog, int toggleX = dialog->getDialogHorPos() - m_screenWidth/2 + dialog->getDialogWidth()/2; // int toggleY = dialog->getDialogVertPos() + m_screenHeight/2 + dialog->getDialogHeight()/2 + dialog->getNumControls()*20; int toggleY = dialog->getDialogVertPos() - m_screenHeight/2 + dialog->getDialogHeight()/2 + dialog->getNumControls()*20; - trans.setOrigin(btVector3(toggleX, toggleY, -0.2f)); + trans.setOrigin(btVector3((btScalar)toggleX, (btScalar)toggleY,(btScalar) -0.2f)); body->setWorldTransform(trans); body->setDamping(0.999f,0.99f); diff --git a/src/BulletCollision/CollisionShapes/btCylinderShape.cpp b/src/BulletCollision/CollisionShapes/btCylinderShape.cpp index 268809304..c2e534b0b 100644 --- a/src/BulletCollision/CollisionShapes/btCylinderShape.cpp +++ b/src/BulletCollision/CollisionShapes/btCylinderShape.cpp @@ -47,7 +47,64 @@ void btCylinderShape::getAabb(const btTransform& t,btVector3& aabbMin,btVector3& void btCylinderShape::calculateLocalInertia(btScalar mass,btVector3& inertia) const { - //approximation of box shape, todo: implement cylinder shape inertia before people notice ;-) + +//Until Bullet 2.77 a box approximation was used, so uncomment this if you need backwards compatibility +//#define USE_BOX_INERTIA_APPROXIMATION 1 +#ifndef USE_BOX_INERTIA_APPROXIMATION + + /* + cylinder is defined as following: + * + * - principle axis aligned along y by default, radius in x, z-value not used + * - for btCylinderShapeX: principle axis aligned along x, radius in y direction, z-value not used + * - for btCylinderShapeZ: principle axis aligned along z, radius in x direction, y-value not used + * + */ + + btScalar radius2; // square of cylinder radius + btScalar height2; // square of cylinder height + btVector3 halfExtents = getHalfExtentsWithMargin(); // get cylinder dimension + btScalar div12 = mass / 12.f; + btScalar div4 = mass / 4.f; + btScalar div2 = mass / 2.f; + int idxRadius, idxHeight; + + switch (m_upAxis) // get indices of radius and height of cylinder + { + case 0: // cylinder is aligned along x + idxRadius = 1; + idxHeight = 0; + break; + case 2: // cylinder is aligned along z + idxRadius = 0; + idxHeight = 2; + break; + default: // cylinder is aligned along y + idxRadius = 0; + idxHeight = 1; + } + + // calculate squares + radius2 = halfExtents[idxRadius] * halfExtents[idxRadius]; + height2 = btScalar(4.) * halfExtents[idxHeight] * halfExtents[idxHeight]; + + // calculate tensor terms + btScalar t1 = div12 * height2 + div4 * radius2; + btScalar t2 = div2 * radius2; + + switch (m_upAxis) // set diagonal elements of inertia tensor + { + case 0: // cylinder is aligned along x + inertia.setValue(t2,t1,t1); + break; + case 2: // cylinder is aligned along z + inertia.setValue(t1,t1,t2); + break; + default: // cylinder is aligned along y + inertia.setValue(t1,t2,t1); + } +#else //USE_BOX_INERTIA_APPROXIMATION + //approximation of box shape btVector3 halfExtents = getHalfExtentsWithMargin(); btScalar lx=btScalar(2.)*(halfExtents.x()); @@ -57,7 +114,7 @@ void btCylinderShape::calculateLocalInertia(btScalar mass,btVector3& inertia) co inertia.setValue(mass/(btScalar(12.0)) * (ly*ly + lz*lz), mass/(btScalar(12.0)) * (lx*lx + lz*lz), mass/(btScalar(12.0)) * (lx*lx + ly*ly)); - +#endif //USE_BOX_INERTIA_APPROXIMATION } diff --git a/src/BulletDynamics/ConstraintSolver/btConeTwistConstraint.cpp b/src/BulletDynamics/ConstraintSolver/btConeTwistConstraint.cpp index bf77c4954..ee66193f0 100644 --- a/src/BulletDynamics/ConstraintSolver/btConeTwistConstraint.cpp +++ b/src/BulletDynamics/ConstraintSolver/btConeTwistConstraint.cpp @@ -1114,4 +1114,12 @@ btScalar btConeTwistConstraint::getParam(int num, int axis) const } +void btConeTwistConstraint::setFrames(const btTransform & frameA, const btTransform & frameB) +{ + m_rbAFrame = frameA; + m_rbBFrame = frameB; + buildJacobian(); + //calculateTransforms(); +} + diff --git a/src/BulletDynamics/ConstraintSolver/btConeTwistConstraint.h b/src/BulletDynamics/ConstraintSolver/btConeTwistConstraint.h index f310d474e..2bf25443f 100644 --- a/src/BulletDynamics/ConstraintSolver/btConeTwistConstraint.h +++ b/src/BulletDynamics/ConstraintSolver/btConeTwistConstraint.h @@ -144,6 +144,16 @@ public: void updateRHS(btScalar timeStep); + const btTransform & getFrameOffsetA() const + { + return m_rbAFrame; + } + + const btTransform & getFrameOffsetB() const + { + return m_rbBFrame; + } + const btRigidBody& getRigidBodyA() const { return m_rbA; @@ -244,6 +254,7 @@ public: } bool isPastSwingLimit() { return m_solveSwingLimit; } + void setFrames(const btTransform & frameA, const btTransform & frameB); void setDamping(btScalar damping) { m_damping = damping; } diff --git a/src/BulletDynamics/ConstraintSolver/btGeneric6DofConstraint.cpp b/src/BulletDynamics/ConstraintSolver/btGeneric6DofConstraint.cpp index 70c484001..fe52995af 100644 --- a/src/BulletDynamics/ConstraintSolver/btGeneric6DofConstraint.cpp +++ b/src/BulletDynamics/ConstraintSolver/btGeneric6DofConstraint.cpp @@ -713,6 +713,15 @@ void btGeneric6DofConstraint::updateRHS(btScalar timeStep) } +void btGeneric6DofConstraint::setFrames(const btTransform & frameA, const btTransform & frameB) +{ + m_frameInA = frameA; + m_frameInB = frameB; + buildJacobian(); + calculateTransforms(); +} + + btVector3 btGeneric6DofConstraint::getAxis(int axis_index) const { diff --git a/src/BulletDynamics/ConstraintSolver/btGeneric6DofConstraint.h b/src/BulletDynamics/ConstraintSolver/btGeneric6DofConstraint.h index 2653d26dc..4d35aaa91 100644 --- a/src/BulletDynamics/ConstraintSolver/btGeneric6DofConstraint.h +++ b/src/BulletDynamics/ConstraintSolver/btGeneric6DofConstraint.h @@ -433,6 +433,7 @@ public: */ btScalar getRelativePivotPosition(int axis_index) const; + void setFrames(const btTransform & frameA, const btTransform & frameB); //! Test angular limit. /*! @@ -446,23 +447,45 @@ public: m_linearLimits.m_lowerLimit = linearLower; } + void getLinearLowerLimit(btVector3& linearLower) + { + linearLower = m_linearLimits.m_lowerLimit; + } + void setLinearUpperLimit(const btVector3& linearUpper) { m_linearLimits.m_upperLimit = linearUpper; } + void getLinearUpperLimit(btVector3& linearUpper) + { + linearUpper = m_linearLimits.m_upperLimit; + } + void setAngularLowerLimit(const btVector3& angularLower) { for(int i = 0; i < 3; i++) m_angularLimits[i].m_loLimit = btNormalizeAngle(angularLower[i]); } + void getAngularLowerLimit(btVector3& angularLower) + { + for(int i = 0; i < 3; i++) + angularLower[i] = m_angularLimits[i].m_loLimit; + } + void setAngularUpperLimit(const btVector3& angularUpper) { for(int i = 0; i < 3; i++) m_angularLimits[i].m_hiLimit = btNormalizeAngle(angularUpper[i]); } + void getAngularUpperLimit(btVector3& angularUpper) + { + for(int i = 0; i < 3; i++) + angularUpper[i] = m_angularLimits[i].m_hiLimit; + } + //! Retrieves the angular limit informacion btRotationalLimitMotor * getRotationalLimitMotor(int index) { diff --git a/src/BulletDynamics/ConstraintSolver/btHingeConstraint.cpp b/src/BulletDynamics/ConstraintSolver/btHingeConstraint.cpp index 9ea428bbf..bc874f75b 100644 --- a/src/BulletDynamics/ConstraintSolver/btHingeConstraint.cpp +++ b/src/BulletDynamics/ConstraintSolver/btHingeConstraint.cpp @@ -550,8 +550,12 @@ void btHingeConstraint::getInfo2Internal(btConstraintInfo2* info, const btTransf } - - +void btHingeConstraint::setFrames(const btTransform & frameA, const btTransform & frameB) +{ + m_rbAFrame = frameA; + m_rbBFrame = frameB; + buildJacobian(); +} void btHingeConstraint::updateRHS(btScalar timeStep) diff --git a/src/BulletDynamics/ConstraintSolver/btHingeConstraint.h b/src/BulletDynamics/ConstraintSolver/btHingeConstraint.h index 4680c42c9..c38b64045 100644 --- a/src/BulletDynamics/ConstraintSolver/btHingeConstraint.h +++ b/src/BulletDynamics/ConstraintSolver/btHingeConstraint.h @@ -132,7 +132,19 @@ public: btRigidBody& getRigidBodyB() { return m_rbB; - } + } + + btTransform & getFrameOffsetA() + { + return m_rbAFrame; + } + + btTransform & getFrameOffsetB() + { + return m_rbBFrame; + } + + void setFrames(const btTransform & frameA, const btTransform & frameB); void setAngularOnly(bool angularOnly) { @@ -182,8 +194,7 @@ public: btVector3 rbAxisB1 = quatRotate(rotationArc,rbAxisA1); btVector3 rbAxisB2 = axisInB.cross(rbAxisB1); - - m_rbBFrame.getOrigin() = m_rbA.getCenterOfMassTransform()(pivotInA); + m_rbBFrame.getOrigin() = m_rbB.getCenterOfMassTransform().inverse()(m_rbA.getCenterOfMassTransform()(pivotInA)); m_rbBFrame.getBasis().setValue( rbAxisB1.getX(),rbAxisB2.getX(),axisInB.getX(), rbAxisB1.getY(),rbAxisB2.getY(),axisInB.getY(), rbAxisB1.getZ(),rbAxisB2.getZ(),axisInB.getZ() );