diff options
author | Nicholas Bishop <nicholasbishop@gmail.com> | 2009-01-05 01:30:44 +0300 |
---|---|---|
committer | Nicholas Bishop <nicholasbishop@gmail.com> | 2009-01-05 01:30:44 +0300 |
commit | 4a815817da6a20e5cbcfbf8624e44e8381f55d73 (patch) | |
tree | 900d11f2761d86c0415d40a0197fdb48371e46f4 /extern/bullet2/src/BulletDynamics | |
parent | 6849238143fbb4ffa8780f6c937196562956c521 (diff) |
Resynch a couple more directories with trunk
Diffstat (limited to 'extern/bullet2/src/BulletDynamics')
4 files changed, 872 insertions, 0 deletions
diff --git a/extern/bullet2/src/BulletDynamics/ConstraintSolver/btSliderConstraint.cpp b/extern/bullet2/src/BulletDynamics/ConstraintSolver/btSliderConstraint.cpp new file mode 100644 index 00000000000..4128f504bf1 --- /dev/null +++ b/extern/bullet2/src/BulletDynamics/ConstraintSolver/btSliderConstraint.cpp @@ -0,0 +1,415 @@ +/* +Bullet Continuous Collision Detection and Physics Library +Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/ + +This software is provided 'as-is', without any express or implied warranty. +In no event will the authors be held liable for any damages arising from the use of this software. +Permission is granted to anyone to use this software for any purpose, +including commercial applications, and to alter it and redistribute it freely, +subject to the following restrictions: + +1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. +2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. +3. This notice may not be removed or altered from any source distribution. +*/ + +/* +Added by Roman Ponomarev (rponom@gmail.com) +April 04, 2008 +*/ + +//----------------------------------------------------------------------------- + +#include "btSliderConstraint.h" +#include "BulletDynamics/Dynamics/btRigidBody.h" +#include "LinearMath/btTransformUtil.h" +#include <new> + +//----------------------------------------------------------------------------- + +void btSliderConstraint::initParams() +{ + m_lowerLinLimit = btScalar(1.0); + m_upperLinLimit = btScalar(-1.0); + m_lowerAngLimit = btScalar(0.); + m_upperAngLimit = btScalar(0.); + m_softnessDirLin = SLIDER_CONSTRAINT_DEF_SOFTNESS; + m_restitutionDirLin = SLIDER_CONSTRAINT_DEF_RESTITUTION; + m_dampingDirLin = btScalar(0.); + m_softnessDirAng = SLIDER_CONSTRAINT_DEF_SOFTNESS; + m_restitutionDirAng = SLIDER_CONSTRAINT_DEF_RESTITUTION; + m_dampingDirAng = btScalar(0.); + m_softnessOrthoLin = SLIDER_CONSTRAINT_DEF_SOFTNESS; + m_restitutionOrthoLin = SLIDER_CONSTRAINT_DEF_RESTITUTION; + m_dampingOrthoLin = SLIDER_CONSTRAINT_DEF_DAMPING; + m_softnessOrthoAng = SLIDER_CONSTRAINT_DEF_SOFTNESS; + m_restitutionOrthoAng = SLIDER_CONSTRAINT_DEF_RESTITUTION; + m_dampingOrthoAng = SLIDER_CONSTRAINT_DEF_DAMPING; + m_softnessLimLin = SLIDER_CONSTRAINT_DEF_SOFTNESS; + m_restitutionLimLin = SLIDER_CONSTRAINT_DEF_RESTITUTION; + m_dampingLimLin = SLIDER_CONSTRAINT_DEF_DAMPING; + 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() + +//----------------------------------------------------------------------------- + +btSliderConstraint::btSliderConstraint() + :btTypedConstraint(SLIDER_CONSTRAINT_TYPE), + m_useLinearReferenceFrameA(true) +{ + initParams(); +} // btSliderConstraint::btSliderConstraint() + +//----------------------------------------------------------------------------- + +btSliderConstraint::btSliderConstraint(btRigidBody& rbA, btRigidBody& rbB, const btTransform& frameInA, const btTransform& frameInB, bool useLinearReferenceFrameA) + : btTypedConstraint(SLIDER_CONSTRAINT_TYPE, rbA, rbB) + , m_frameInA(frameInA) + , m_frameInB(frameInB), + m_useLinearReferenceFrameA(useLinearReferenceFrameA) +{ + initParams(); +} // btSliderConstraint::btSliderConstraint() + +//----------------------------------------------------------------------------- + +void btSliderConstraint::buildJacobian() +{ + if(m_useLinearReferenceFrameA) + { + buildJacobianInt(m_rbA, m_rbB, m_frameInA, m_frameInB); + } + else + { + buildJacobianInt(m_rbB, m_rbA, m_frameInB, m_frameInA); + } +} // btSliderConstraint::buildJacobian() + +//----------------------------------------------------------------------------- + +void btSliderConstraint::buildJacobianInt(btRigidBody& rbA, btRigidBody& rbB, const btTransform& frameInA, const btTransform& frameInB) +{ + //calculate transforms + m_calculatedTransformA = rbA.getCenterOfMassTransform() * frameInA; + m_calculatedTransformB = rbB.getCenterOfMassTransform() * frameInB; + m_realPivotAInW = m_calculatedTransformA.getOrigin(); + m_realPivotBInW = m_calculatedTransformB.getOrigin(); + m_sliderAxis = m_calculatedTransformA.getBasis().getColumn(0); // along X + m_delta = m_realPivotBInW - m_realPivotAInW; + m_projPivotInW = m_realPivotAInW + m_sliderAxis.dot(m_delta) * m_sliderAxis; + m_relPosA = m_projPivotInW - rbA.getCenterOfMassPosition(); + m_relPosB = m_realPivotBInW - rbB.getCenterOfMassPosition(); + btVector3 normalWorld; + int i; + //linear part + for(i = 0; i < 3; i++) + { + normalWorld = m_calculatedTransformA.getBasis().getColumn(i); + new (&m_jacLin[i]) btJacobianEntry( + rbA.getCenterOfMassTransform().getBasis().transpose(), + rbB.getCenterOfMassTransform().getBasis().transpose(), + m_relPosA, + m_relPosB, + normalWorld, + rbA.getInvInertiaDiagLocal(), + rbA.getInvMass(), + rbB.getInvInertiaDiagLocal(), + rbB.getInvMass() + ); + m_jacLinDiagABInv[i] = btScalar(1.) / m_jacLin[i].getDiagonal(); + m_depth[i] = m_delta.dot(normalWorld); + } + testLinLimits(); + // angular part + for(i = 0; i < 3; i++) + { + normalWorld = m_calculatedTransformA.getBasis().getColumn(i); + new (&m_jacAng[i]) btJacobianEntry( + normalWorld, + rbA.getCenterOfMassTransform().getBasis().transpose(), + rbB.getCenterOfMassTransform().getBasis().transpose(), + rbA.getInvInertiaDiagLocal(), + rbB.getInvInertiaDiagLocal() + ); + } + testAngLimits(); + btVector3 axisA = m_calculatedTransformA.getBasis().getColumn(0); + m_kAngle = btScalar(1.0 )/ (rbA.computeAngularImpulseDenominator(axisA) + rbB.computeAngularImpulseDenominator(axisA)); + // clear accumulator for motors + m_accumulatedLinMotorImpulse = btScalar(0.0); + m_accumulatedAngMotorImpulse = btScalar(0.0); +} // btSliderConstraint::buildJacobianInt() + +//----------------------------------------------------------------------------- + +void btSliderConstraint::solveConstraint(btScalar timeStep) +{ + m_timeStep = timeStep; + if(m_useLinearReferenceFrameA) + { + solveConstraintInt(m_rbA, m_rbB); + } + else + { + solveConstraintInt(m_rbB, m_rbA); + } +} // btSliderConstraint::solveConstraint() + +//----------------------------------------------------------------------------- + +void btSliderConstraint::solveConstraintInt(btRigidBody& rbA, btRigidBody& rbB) +{ + int i; + // linear + btVector3 velA = rbA.getVelocityInLocalPoint(m_relPosA); + btVector3 velB = rbB.getVelocityInLocalPoint(m_relPosB); + btVector3 vel = velA - velB; + for(i = 0; i < 3; i++) + { + const btVector3& normal = m_jacLin[i].m_linearJointAxis; + btScalar rel_vel = normal.dot(vel); + // calculate positional error + btScalar depth = m_depth[i]; + // get parameters + btScalar softness = (i) ? m_softnessOrthoLin : (m_solveLinLim ? m_softnessLimLin : m_softnessDirLin); + btScalar restitution = (i) ? m_restitutionOrthoLin : (m_solveLinLim ? m_restitutionLimLin : m_restitutionDirLin); + btScalar damping = (i) ? m_dampingOrthoLin : (m_solveLinLim ? m_dampingLimLin : m_dampingDirLin); + // calcutate and apply impulse + btScalar normalImpulse = softness * (restitution * depth / m_timeStep - damping * rel_vel) * m_jacLinDiagABInv[i]; + btVector3 impulse_vector = normal * normalImpulse; + rbA.applyImpulse( impulse_vector, m_relPosA); + rbB.applyImpulse(-impulse_vector, m_relPosB); + 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 + btVector3 axisA = m_calculatedTransformA.getBasis().getColumn(0); + btVector3 axisB = m_calculatedTransformB.getBasis().getColumn(0); + + const btVector3& angVelA = rbA.getAngularVelocity(); + const btVector3& angVelB = rbB.getAngularVelocity(); + + btVector3 angVelAroundAxisA = axisA * axisA.dot(angVelA); + btVector3 angVelAroundAxisB = axisB * axisB.dot(angVelB); + + btVector3 angAorthog = angVelA - angVelAroundAxisA; + btVector3 angBorthog = angVelB - angVelAroundAxisB; + btVector3 velrelOrthog = angAorthog-angBorthog; + //solve orthogonal angular velocity correction + btScalar len = velrelOrthog.length(); + if (len > btScalar(0.00001)) + { + btVector3 normal = velrelOrthog.normalized(); + btScalar denom = rbA.computeAngularImpulseDenominator(normal) + rbB.computeAngularImpulseDenominator(normal); + velrelOrthog *= (btScalar(1.)/denom) * m_dampingOrthoAng * m_softnessOrthoAng; + } + //solve angular positional correction + btVector3 angularError = axisA.cross(axisB) *(btScalar(1.)/m_timeStep); + btScalar len2 = angularError.length(); + if (len2>btScalar(0.00001)) + { + btVector3 normal2 = angularError.normalized(); + btScalar denom2 = rbA.computeAngularImpulseDenominator(normal2) + rbB.computeAngularImpulseDenominator(normal2); + angularError *= (btScalar(1.)/denom2) * m_restitutionOrthoAng * m_softnessOrthoAng; + } + // apply impulse + rbA.applyTorqueImpulse(-velrelOrthog+angularError); + rbB.applyTorqueImpulse(velrelOrthog-angularError); + btScalar impulseMag; + //solve angular limits + if(m_solveAngLim) + { + impulseMag = (angVelB - angVelA).dot(axisA) * m_dampingLimAng + m_angDepth * m_restitutionLimAng / m_timeStep; + impulseMag *= m_kAngle * m_softnessLimAng; + } + else + { + impulseMag = (angVelB - angVelA).dot(axisA) * m_dampingDirAng + m_angDepth * m_restitutionDirAng / m_timeStep; + impulseMag *= m_kAngle * m_softnessDirAng; + } + btVector3 impulse = axisA * impulseMag; + rbA.applyTorqueImpulse(impulse); + rbB.applyTorqueImpulse(-impulse); + //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; + m_linPos = m_depth[0]; + if(m_lowerLinLimit <= m_upperLinLimit) + { + if(m_depth[0] > m_upperLinLimit) + { + m_depth[0] -= m_upperLinLimit; + m_solveLinLim = true; + } + else if(m_depth[0] < m_lowerLinLimit) + { + m_depth[0] -= m_lowerLinLimit; + m_solveLinLim = true; + } + else + { + m_depth[0] = btScalar(0.); + } + } + else + { + m_depth[0] = btScalar(0.); + } +} // 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() + + +//----------------------------------------------------------------------------- + + + +btVector3 btSliderConstraint::getAncorInA(void) +{ + btVector3 ancorInA; + ancorInA = m_realPivotAInW + (m_lowerLinLimit + m_upperLinLimit) * btScalar(0.5) * m_sliderAxis; + ancorInA = m_rbA.getCenterOfMassTransform().inverse() * ancorInA; + return ancorInA; +} // btSliderConstraint::getAncorInA() + +//----------------------------------------------------------------------------- + +btVector3 btSliderConstraint::getAncorInB(void) +{ + btVector3 ancorInB; + ancorInB = m_frameInB.getOrigin(); + return ancorInB; +} // btSliderConstraint::getAncorInB(); diff --git a/extern/bullet2/src/BulletDynamics/ConstraintSolver/btSliderConstraint.h b/extern/bullet2/src/BulletDynamics/ConstraintSolver/btSliderConstraint.h new file mode 100644 index 00000000000..580dfa1178d --- /dev/null +++ b/extern/bullet2/src/BulletDynamics/ConstraintSolver/btSliderConstraint.h @@ -0,0 +1,218 @@ +/* +Bullet Continuous Collision Detection and Physics Library +Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/ + +This software is provided 'as-is', without any express or implied warranty. +In no event will the authors be held liable for any damages arising from the use of this software. +Permission is granted to anyone to use this software for any purpose, +including commercial applications, and to alter it and redistribute it freely, +subject to the following restrictions: + +1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. +2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. +3. This notice may not be removed or altered from any source distribution. +*/ + +/* +Added by Roman Ponomarev (rponom@gmail.com) +April 04, 2008 + +TODO: + - add clamping od accumulated impulse to improve stability + - add conversion for ODE constraint solver +*/ + +#ifndef SLIDER_CONSTRAINT_H +#define SLIDER_CONSTRAINT_H + +//----------------------------------------------------------------------------- + +#include "LinearMath/btVector3.h" +#include "btJacobianEntry.h" +#include "btTypedConstraint.h" + +//----------------------------------------------------------------------------- + +class btRigidBody; + +//----------------------------------------------------------------------------- + +#define SLIDER_CONSTRAINT_DEF_SOFTNESS (btScalar(1.0)) +#define SLIDER_CONSTRAINT_DEF_DAMPING (btScalar(1.0)) +#define SLIDER_CONSTRAINT_DEF_RESTITUTION (btScalar(0.7)) + +//----------------------------------------------------------------------------- + +class btSliderConstraint : public btTypedConstraint +{ +protected: + btTransform m_frameInA; + btTransform m_frameInB; + // use frameA fo define limits, if true + bool m_useLinearReferenceFrameA; + // linear limits + btScalar m_lowerLinLimit; + btScalar m_upperLinLimit; + // angular limits + btScalar m_lowerAngLimit; + btScalar m_upperAngLimit; + // softness, restitution and damping for different cases + // DirLin - moving inside linear limits + // LimLin - hitting linear limit + // DirAng - moving inside angular limits + // LimAng - hitting angular limit + // OrthoLin, OrthoAng - against constraint axis + btScalar m_softnessDirLin; + btScalar m_restitutionDirLin; + btScalar m_dampingDirLin; + btScalar m_softnessDirAng; + btScalar m_restitutionDirAng; + btScalar m_dampingDirAng; + btScalar m_softnessLimLin; + btScalar m_restitutionLimLin; + btScalar m_dampingLimLin; + btScalar m_softnessLimAng; + btScalar m_restitutionLimAng; + btScalar m_dampingLimAng; + btScalar m_softnessOrthoLin; + btScalar m_restitutionOrthoLin; + btScalar m_dampingOrthoLin; + btScalar m_softnessOrthoAng; + btScalar m_restitutionOrthoAng; + btScalar m_dampingOrthoAng; + + // for interlal use + bool m_solveLinLim; + bool m_solveAngLim; + + btJacobianEntry m_jacLin[3]; + btScalar m_jacLinDiagABInv[3]; + + btJacobianEntry m_jacAng[3]; + + btScalar m_timeStep; + btTransform m_calculatedTransformA; + btTransform m_calculatedTransformB; + + btVector3 m_sliderAxis; + btVector3 m_realPivotAInW; + btVector3 m_realPivotBInW; + btVector3 m_projPivotInW; + btVector3 m_delta; + btVector3 m_depth; + btVector3 m_relPosA; + btVector3 m_relPosB; + + btScalar m_linPos; + + 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(); +public: + // constructors + btSliderConstraint(btRigidBody& rbA, btRigidBody& rbB, const btTransform& frameInA, const btTransform& frameInB ,bool useLinearReferenceFrameA); + btSliderConstraint(); + // overrides + virtual void buildJacobian(); + virtual void solveConstraint(btScalar timeStep); + // access + const btRigidBody& getRigidBodyA() const { return m_rbA; } + const btRigidBody& getRigidBodyB() const { return m_rbB; } + const btTransform & getCalculatedTransformA() const { return m_calculatedTransformA; } + const btTransform & getCalculatedTransformB() const { return m_calculatedTransformB; } + const btTransform & getFrameOffsetA() const { return m_frameInA; } + const btTransform & getFrameOffsetB() const { return m_frameInB; } + btTransform & getFrameOffsetA() { return m_frameInA; } + btTransform & getFrameOffsetB() { return m_frameInB; } + btScalar getLowerLinLimit() { return m_lowerLinLimit; } + void setLowerLinLimit(btScalar lowerLimit) { m_lowerLinLimit = lowerLimit; } + btScalar getUpperLinLimit() { return m_upperLinLimit; } + void setUpperLinLimit(btScalar upperLimit) { m_upperLinLimit = upperLimit; } + btScalar getLowerAngLimit() { return m_lowerAngLimit; } + void setLowerAngLimit(btScalar lowerLimit) { m_lowerAngLimit = lowerLimit; } + btScalar getUpperAngLimit() { return m_upperAngLimit; } + void setUpperAngLimit(btScalar upperLimit) { m_upperAngLimit = upperLimit; } + bool getUseLinearReferenceFrameA() { return m_useLinearReferenceFrameA; } + btScalar getSoftnessDirLin() { return m_softnessDirLin; } + btScalar getRestitutionDirLin() { return m_restitutionDirLin; } + btScalar getDampingDirLin() { return m_dampingDirLin ; } + btScalar getSoftnessDirAng() { return m_softnessDirAng; } + btScalar getRestitutionDirAng() { return m_restitutionDirAng; } + btScalar getDampingDirAng() { return m_dampingDirAng; } + btScalar getSoftnessLimLin() { return m_softnessLimLin; } + btScalar getRestitutionLimLin() { return m_restitutionLimLin; } + btScalar getDampingLimLin() { return m_dampingLimLin; } + btScalar getSoftnessLimAng() { return m_softnessLimAng; } + btScalar getRestitutionLimAng() { return m_restitutionLimAng; } + btScalar getDampingLimAng() { return m_dampingLimAng; } + btScalar getSoftnessOrthoLin() { return m_softnessOrthoLin; } + btScalar getRestitutionOrthoLin() { return m_restitutionOrthoLin; } + btScalar getDampingOrthoLin() { return m_dampingOrthoLin; } + btScalar getSoftnessOrthoAng() { return m_softnessOrthoAng; } + btScalar getRestitutionOrthoAng() { return m_restitutionOrthoAng; } + btScalar getDampingOrthoAng() { return m_dampingOrthoAng; } + void setSoftnessDirLin(btScalar softnessDirLin) { m_softnessDirLin = softnessDirLin; } + void setRestitutionDirLin(btScalar restitutionDirLin) { m_restitutionDirLin = restitutionDirLin; } + void setDampingDirLin(btScalar dampingDirLin) { m_dampingDirLin = dampingDirLin; } + void setSoftnessDirAng(btScalar softnessDirAng) { m_softnessDirAng = softnessDirAng; } + void setRestitutionDirAng(btScalar restitutionDirAng) { m_restitutionDirAng = restitutionDirAng; } + void setDampingDirAng(btScalar dampingDirAng) { m_dampingDirAng = dampingDirAng; } + void setSoftnessLimLin(btScalar softnessLimLin) { m_softnessLimLin = softnessLimLin; } + void setRestitutionLimLin(btScalar restitutionLimLin) { m_restitutionLimLin = restitutionLimLin; } + void setDampingLimLin(btScalar dampingLimLin) { m_dampingLimLin = dampingLimLin; } + void setSoftnessLimAng(btScalar softnessLimAng) { m_softnessLimAng = softnessLimAng; } + void setRestitutionLimAng(btScalar restitutionLimAng) { m_restitutionLimAng = restitutionLimAng; } + void setDampingLimAng(btScalar dampingLimAng) { m_dampingLimAng = dampingLimAng; } + void setSoftnessOrthoLin(btScalar softnessOrthoLin) { m_softnessOrthoLin = softnessOrthoLin; } + void setRestitutionOrthoLin(btScalar restitutionOrthoLin) { m_restitutionOrthoLin = restitutionOrthoLin; } + void setDampingOrthoLin(btScalar dampingOrthoLin) { m_dampingOrthoLin = dampingOrthoLin; } + 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; } + btScalar getLinearPos() { return m_linPos; } + + // 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); + // access for PE Solver + btVector3 getAncorInA(void); + btVector3 getAncorInB(void); +}; + +//----------------------------------------------------------------------------- + +#endif //SLIDER_CONSTRAINT_H + diff --git a/extern/bullet2/src/BulletDynamics/Dynamics/btContinuousDynamicsWorld.cpp b/extern/bullet2/src/BulletDynamics/Dynamics/btContinuousDynamicsWorld.cpp new file mode 100644 index 00000000000..19443adc723 --- /dev/null +++ b/extern/bullet2/src/BulletDynamics/Dynamics/btContinuousDynamicsWorld.cpp @@ -0,0 +1,193 @@ +/* +Bullet Continuous Collision Detection and Physics Library +Copyright (c) 2003-2007 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. +*/ + + +#include "btContinuousDynamicsWorld.h" +#include "LinearMath/btQuickprof.h" + +//collision detection +#include "BulletCollision/CollisionDispatch/btCollisionDispatcher.h" +#include "BulletCollision/BroadphaseCollision/btSimpleBroadphase.h" +#include "BulletCollision/CollisionShapes/btCollisionShape.h" +#include "BulletCollision/CollisionDispatch/btSimulationIslandManager.h" + +//rigidbody & constraints +#include "BulletDynamics/Dynamics/btRigidBody.h" +#include "BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolver.h" +#include "BulletDynamics/ConstraintSolver/btContactSolverInfo.h" +#include "BulletDynamics/ConstraintSolver/btTypedConstraint.h" + + + +#include <stdio.h> + +btContinuousDynamicsWorld::btContinuousDynamicsWorld(btDispatcher* dispatcher,btBroadphaseInterface* pairCache,btConstraintSolver* constraintSolver,btCollisionConfiguration* collisionConfiguration) +:btDiscreteDynamicsWorld(dispatcher,pairCache,constraintSolver,collisionConfiguration) +{ +} + +btContinuousDynamicsWorld::~btContinuousDynamicsWorld() +{ +} + + +void btContinuousDynamicsWorld::internalSingleStepSimulation( btScalar timeStep) +{ + + startProfiling(timeStep); + + + ///update aabbs information + updateAabbs(); + //static int frame=0; +// printf("frame %d\n",frame++); + + ///apply gravity, predict motion + predictUnconstraintMotion(timeStep); + + btDispatcherInfo& dispatchInfo = getDispatchInfo(); + + dispatchInfo.m_timeStep = timeStep; + dispatchInfo.m_stepCount = 0; + dispatchInfo.m_debugDraw = getDebugDrawer(); + + ///perform collision detection + performDiscreteCollisionDetection(); + + calculateSimulationIslands(); + + + getSolverInfo().m_timeStep = timeStep; + + + + ///solve contact and other joint constraints + solveConstraints(getSolverInfo()); + + ///CallbackTriggers(); + calculateTimeOfImpacts(timeStep); + + btScalar toi = dispatchInfo.m_timeOfImpact; +// if (toi < 1.f) +// printf("toi = %f\n",toi); + if (toi < 0.f) + printf("toi = %f\n",toi); + + + ///integrate transforms + integrateTransforms(timeStep * toi); + + ///update vehicle simulation + updateVehicles(timeStep); + + + updateActivationState( timeStep ); + + if(0 != m_internalTickCallback) { + (*m_internalTickCallback)(this, timeStep); + } +} + +void btContinuousDynamicsWorld::calculateTimeOfImpacts(btScalar timeStep) +{ + ///these should be 'temporal' aabbs! + updateTemporalAabbs(timeStep); + + ///'toi' is the global smallest time of impact. However, we just calculate the time of impact for each object individually. + ///so we handle the case moving versus static properly, and we cheat for moving versus moving + btScalar toi = 1.f; + + + btDispatcherInfo& dispatchInfo = getDispatchInfo(); + dispatchInfo.m_timeStep = timeStep; + dispatchInfo.m_timeOfImpact = 1.f; + dispatchInfo.m_stepCount = 0; + dispatchInfo.m_dispatchFunc = btDispatcherInfo::DISPATCH_CONTINUOUS; + + ///calculate time of impact for overlapping pairs + + + btDispatcher* dispatcher = getDispatcher(); + if (dispatcher) + dispatcher->dispatchAllCollisionPairs(m_broadphasePairCache->getOverlappingPairCache(),dispatchInfo,m_dispatcher1); + + toi = dispatchInfo.m_timeOfImpact; + + dispatchInfo.m_dispatchFunc = btDispatcherInfo::DISPATCH_DISCRETE; + +} + +void btContinuousDynamicsWorld::updateTemporalAabbs(btScalar timeStep) +{ + + btVector3 temporalAabbMin,temporalAabbMax; + + for ( int i=0;i<m_collisionObjects.size();i++) + { + btCollisionObject* colObj = m_collisionObjects[i]; + + btRigidBody* body = btRigidBody::upcast(colObj); + if (body) + { + body->getCollisionShape()->getAabb(m_collisionObjects[i]->getWorldTransform(),temporalAabbMin,temporalAabbMax); + const btVector3& linvel = body->getLinearVelocity(); + + //make the AABB temporal + btScalar temporalAabbMaxx = temporalAabbMax.getX(); + btScalar temporalAabbMaxy = temporalAabbMax.getY(); + btScalar temporalAabbMaxz = temporalAabbMax.getZ(); + btScalar temporalAabbMinx = temporalAabbMin.getX(); + btScalar temporalAabbMiny = temporalAabbMin.getY(); + btScalar temporalAabbMinz = temporalAabbMin.getZ(); + + // add linear motion + btVector3 linMotion = linvel*timeStep; + + if (linMotion.x() > 0.f) + temporalAabbMaxx += linMotion.x(); + else + temporalAabbMinx += linMotion.x(); + if (linMotion.y() > 0.f) + temporalAabbMaxy += linMotion.y(); + else + temporalAabbMiny += linMotion.y(); + if (linMotion.z() > 0.f) + temporalAabbMaxz += linMotion.z(); + else + temporalAabbMinz += linMotion.z(); + + //add conservative angular motion + btScalar angularMotion(0);// = angvel.length() * GetAngularMotionDisc() * timeStep; + btVector3 angularMotion3d(angularMotion,angularMotion,angularMotion); + temporalAabbMin = btVector3(temporalAabbMinx,temporalAabbMiny,temporalAabbMinz); + temporalAabbMax = btVector3(temporalAabbMaxx,temporalAabbMaxy,temporalAabbMaxz); + + temporalAabbMin -= angularMotion3d; + temporalAabbMax += angularMotion3d; + + m_broadphasePairCache->setAabb(body->getBroadphaseHandle(),temporalAabbMin,temporalAabbMax,m_dispatcher1); + } + } + + //update aabb (of all moved objects) + + m_broadphasePairCache->calculateOverlappingPairs(m_dispatcher1); + + + +} + + + diff --git a/extern/bullet2/src/BulletDynamics/Dynamics/btContinuousDynamicsWorld.h b/extern/bullet2/src/BulletDynamics/Dynamics/btContinuousDynamicsWorld.h new file mode 100644 index 00000000000..61c8dea03eb --- /dev/null +++ b/extern/bullet2/src/BulletDynamics/Dynamics/btContinuousDynamicsWorld.h @@ -0,0 +1,46 @@ +/* +Bullet Continuous Collision Detection and Physics Library +Copyright (c) 2003-2007 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 BT_CONTINUOUS_DYNAMICS_WORLD_H +#define BT_CONTINUOUS_DYNAMICS_WORLD_H + +#include "btDiscreteDynamicsWorld.h" + +///btContinuousDynamicsWorld adds optional (per object) continuous collision detection for fast moving objects to the btDiscreteDynamicsWorld. +///This copes with fast moving objects that otherwise would tunnel/miss collisions. +///Under construction, don't use yet! Please use btDiscreteDynamicsWorld instead. +class btContinuousDynamicsWorld : public btDiscreteDynamicsWorld +{ + + void updateTemporalAabbs(btScalar timeStep); + + public: + + btContinuousDynamicsWorld(btDispatcher* dispatcher,btBroadphaseInterface* pairCache,btConstraintSolver* constraintSolver,btCollisionConfiguration* collisionConfiguration); + virtual ~btContinuousDynamicsWorld(); + + ///time stepping with calculation of time of impact for selected fast moving objects + virtual void internalSingleStepSimulation( btScalar timeStep); + + virtual void calculateTimeOfImpacts(btScalar timeStep); + + virtual btDynamicsWorldType getWorldType() const + { + return BT_CONTINUOUS_DYNAMICS_WORLD; + } + +}; + +#endif //BT_CONTINUOUS_DYNAMICS_WORLD_H |