initial commit
This commit is contained in:
cirdan
@ -0,0 +1,287 @@
|
||||
/*
|
||||
Bullet Continuous Collision Detection and Physics Library
|
||||
btConeTwistConstraint is Copyright (c) 2007 Starbreeze Studios
|
||||
|
||||
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.
|
||||
|
||||
Written by: Marcus Hennix
|
||||
*/
|
||||
|
||||
|
||||
#include "btConeTwistConstraint.h"
|
||||
#include "BulletDynamics/Dynamics/btRigidBody.h"
|
||||
#include "LinearMath/btTransformUtil.h"
|
||||
#include "LinearMath/btSimdMinMax.h"
|
||||
#include <new>
|
||||
|
||||
btConeTwistConstraint::btConeTwistConstraint()
|
||||
:btTypedConstraint(CONETWIST_CONSTRAINT_TYPE)
|
||||
{
|
||||
}
|
||||
|
||||
|
||||
btConeTwistConstraint::btConeTwistConstraint(btRigidBody& rbA,btRigidBody& rbB,
|
||||
const btTransform& rbAFrame,const btTransform& rbBFrame)
|
||||
:btTypedConstraint(CONETWIST_CONSTRAINT_TYPE, rbA,rbB),m_rbAFrame(rbAFrame),m_rbBFrame(rbBFrame),
|
||||
m_angularOnly(false)
|
||||
{
|
||||
// flip axis for correct angles
|
||||
m_rbBFrame.getBasis()[1][0] *= btScalar(-1.);
|
||||
m_rbBFrame.getBasis()[1][1] *= btScalar(-1.);
|
||||
m_rbBFrame.getBasis()[1][2] *= btScalar(-1.);
|
||||
|
||||
m_swingSpan1 = btScalar(1e30);
|
||||
m_swingSpan2 = btScalar(1e30);
|
||||
m_twistSpan = btScalar(1e30);
|
||||
m_biasFactor = 0.3f;
|
||||
m_relaxationFactor = 1.0f;
|
||||
|
||||
m_solveTwistLimit = false;
|
||||
m_solveSwingLimit = false;
|
||||
|
||||
}
|
||||
|
||||
btConeTwistConstraint::btConeTwistConstraint(btRigidBody& rbA,const btTransform& rbAFrame)
|
||||
:btTypedConstraint(CONETWIST_CONSTRAINT_TYPE,rbA),m_rbAFrame(rbAFrame),
|
||||
m_angularOnly(false)
|
||||
{
|
||||
m_rbBFrame = m_rbAFrame;
|
||||
|
||||
// flip axis for correct angles
|
||||
m_rbBFrame.getBasis()[1][0] *= btScalar(-1.);
|
||||
m_rbBFrame.getBasis()[1][1] *= btScalar(-1.);
|
||||
m_rbBFrame.getBasis()[1][2] *= btScalar(-1.);
|
||||
|
||||
m_rbBFrame.getBasis()[2][0] *= btScalar(-1.);
|
||||
m_rbBFrame.getBasis()[2][1] *= btScalar(-1.);
|
||||
m_rbBFrame.getBasis()[2][2] *= btScalar(-1.);
|
||||
|
||||
m_swingSpan1 = btScalar(1e30);
|
||||
m_swingSpan2 = btScalar(1e30);
|
||||
m_twistSpan = btScalar(1e30);
|
||||
m_biasFactor = 0.3f;
|
||||
m_relaxationFactor = 1.0f;
|
||||
|
||||
m_solveTwistLimit = false;
|
||||
m_solveSwingLimit = false;
|
||||
|
||||
}
|
||||
|
||||
void btConeTwistConstraint::buildJacobian()
|
||||
{
|
||||
m_appliedImpulse = btScalar(0.);
|
||||
|
||||
//set bias, sign, clear accumulator
|
||||
m_swingCorrection = btScalar(0.);
|
||||
m_twistLimitSign = btScalar(0.);
|
||||
m_solveTwistLimit = false;
|
||||
m_solveSwingLimit = false;
|
||||
m_accTwistLimitImpulse = btScalar(0.);
|
||||
m_accSwingLimitImpulse = btScalar(0.);
|
||||
|
||||
if (!m_angularOnly)
|
||||
{
|
||||
btVector3 pivotAInW = m_rbA.getCenterOfMassTransform()*m_rbAFrame.getOrigin();
|
||||
btVector3 pivotBInW = m_rbB.getCenterOfMassTransform()*m_rbBFrame.getOrigin();
|
||||
btVector3 relPos = pivotBInW - pivotAInW;
|
||||
|
||||
btVector3 normal[3];
|
||||
if (relPos.length2() > SIMD_EPSILON)
|
||||
{
|
||||
normal[0] = relPos.normalized();
|
||||
}
|
||||
else
|
||||
{
|
||||
normal[0].setValue(btScalar(1.0),0,0);
|
||||
}
|
||||
|
||||
btPlaneSpace1(normal[0], normal[1], normal[2]);
|
||||
|
||||
for (int i=0;i<3;i++)
|
||||
{
|
||||
new (&m_jac[i]) btJacobianEntry(
|
||||
m_rbA.getCenterOfMassTransform().getBasis().transpose(),
|
||||
m_rbB.getCenterOfMassTransform().getBasis().transpose(),
|
||||
pivotAInW - m_rbA.getCenterOfMassPosition(),
|
||||
pivotBInW - m_rbB.getCenterOfMassPosition(),
|
||||
normal[i],
|
||||
m_rbA.getInvInertiaDiagLocal(),
|
||||
m_rbA.getInvMass(),
|
||||
m_rbB.getInvInertiaDiagLocal(),
|
||||
m_rbB.getInvMass());
|
||||
}
|
||||
}
|
||||
|
||||
btVector3 b1Axis1,b1Axis2,b1Axis3;
|
||||
btVector3 b2Axis1,b2Axis2;
|
||||
|
||||
b1Axis1 = getRigidBodyA().getCenterOfMassTransform().getBasis() * this->m_rbAFrame.getBasis().getColumn(0);
|
||||
b2Axis1 = getRigidBodyB().getCenterOfMassTransform().getBasis() * this->m_rbBFrame.getBasis().getColumn(0);
|
||||
|
||||
btScalar swing1=btScalar(0.),swing2 = btScalar(0.);
|
||||
|
||||
// Get Frame into world space
|
||||
if (m_swingSpan1 >= btScalar(0.05f))
|
||||
{
|
||||
b1Axis2 = getRigidBodyA().getCenterOfMassTransform().getBasis() * this->m_rbAFrame.getBasis().getColumn(1);
|
||||
swing1 = btAtan2Fast( b2Axis1.dot(b1Axis2),b2Axis1.dot(b1Axis1) );
|
||||
}
|
||||
|
||||
if (m_swingSpan2 >= btScalar(0.05f))
|
||||
{
|
||||
b1Axis3 = getRigidBodyA().getCenterOfMassTransform().getBasis() * this->m_rbAFrame.getBasis().getColumn(2);
|
||||
swing2 = btAtan2Fast( b2Axis1.dot(b1Axis3),b2Axis1.dot(b1Axis1) );
|
||||
}
|
||||
|
||||
btScalar RMaxAngle1Sq = 1.0f / (m_swingSpan1*m_swingSpan1);
|
||||
btScalar RMaxAngle2Sq = 1.0f / (m_swingSpan2*m_swingSpan2);
|
||||
btScalar EllipseAngle = btFabs(swing1)* RMaxAngle1Sq + btFabs(swing2) * RMaxAngle2Sq;
|
||||
|
||||
if (EllipseAngle > 1.0f)
|
||||
{
|
||||
m_swingCorrection = EllipseAngle-1.0f;
|
||||
m_solveSwingLimit = true;
|
||||
|
||||
// Calculate necessary axis & factors
|
||||
m_swingAxis = b2Axis1.cross(b1Axis2* b2Axis1.dot(b1Axis2) + b1Axis3* b2Axis1.dot(b1Axis3));
|
||||
m_swingAxis.normalize();
|
||||
|
||||
btScalar swingAxisSign = (b2Axis1.dot(b1Axis1) >= 0.0f) ? 1.0f : -1.0f;
|
||||
m_swingAxis *= swingAxisSign;
|
||||
|
||||
m_kSwing = btScalar(1.) / (getRigidBodyA().computeAngularImpulseDenominator(m_swingAxis) +
|
||||
getRigidBodyB().computeAngularImpulseDenominator(m_swingAxis));
|
||||
|
||||
}
|
||||
|
||||
// Twist limits
|
||||
if (m_twistSpan >= btScalar(0.))
|
||||
{
|
||||
btVector3 b2Axis2 = getRigidBodyB().getCenterOfMassTransform().getBasis() * this->m_rbBFrame.getBasis().getColumn(1);
|
||||
btQuaternion rotationArc = shortestArcQuat(b2Axis1,b1Axis1);
|
||||
btVector3 TwistRef = quatRotate(rotationArc,b2Axis2);
|
||||
btScalar twist = btAtan2Fast( TwistRef.dot(b1Axis3), TwistRef.dot(b1Axis2) );
|
||||
|
||||
btScalar lockedFreeFactor = (m_twistSpan > btScalar(0.05f)) ? m_limitSoftness : btScalar(0.);
|
||||
if (twist <= -m_twistSpan*lockedFreeFactor)
|
||||
{
|
||||
m_twistCorrection = -(twist + m_twistSpan);
|
||||
m_solveTwistLimit = true;
|
||||
|
||||
m_twistAxis = (b2Axis1 + b1Axis1) * 0.5f;
|
||||
m_twistAxis.normalize();
|
||||
m_twistAxis *= -1.0f;
|
||||
|
||||
m_kTwist = btScalar(1.) / (getRigidBodyA().computeAngularImpulseDenominator(m_twistAxis) +
|
||||
getRigidBodyB().computeAngularImpulseDenominator(m_twistAxis));
|
||||
|
||||
} else
|
||||
if (twist > m_twistSpan*lockedFreeFactor)
|
||||
{
|
||||
m_twistCorrection = (twist - m_twistSpan);
|
||||
m_solveTwistLimit = true;
|
||||
|
||||
m_twistAxis = (b2Axis1 + b1Axis1) * 0.5f;
|
||||
m_twistAxis.normalize();
|
||||
|
||||
m_kTwist = btScalar(1.) / (getRigidBodyA().computeAngularImpulseDenominator(m_twistAxis) +
|
||||
getRigidBodyB().computeAngularImpulseDenominator(m_twistAxis));
|
||||
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
void btConeTwistConstraint::solveConstraint(btScalar timeStep)
|
||||
{
|
||||
|
||||
btVector3 pivotAInW = m_rbA.getCenterOfMassTransform()*m_rbAFrame.getOrigin();
|
||||
btVector3 pivotBInW = m_rbB.getCenterOfMassTransform()*m_rbBFrame.getOrigin();
|
||||
|
||||
btScalar tau = btScalar(0.3);
|
||||
btScalar damping = btScalar(1.);
|
||||
|
||||
//linear part
|
||||
if (!m_angularOnly)
|
||||
{
|
||||
btVector3 rel_pos1 = pivotAInW - m_rbA.getCenterOfMassPosition();
|
||||
btVector3 rel_pos2 = pivotBInW - m_rbB.getCenterOfMassPosition();
|
||||
|
||||
btVector3 vel1 = m_rbA.getVelocityInLocalPoint(rel_pos1);
|
||||
btVector3 vel2 = m_rbB.getVelocityInLocalPoint(rel_pos2);
|
||||
btVector3 vel = vel1 - vel2;
|
||||
|
||||
for (int i=0;i<3;i++)
|
||||
{
|
||||
const btVector3& normal = m_jac[i].m_linearJointAxis;
|
||||
btScalar jacDiagABInv = btScalar(1.) / m_jac[i].getDiagonal();
|
||||
|
||||
btScalar rel_vel;
|
||||
rel_vel = normal.dot(vel);
|
||||
//positional error (zeroth order error)
|
||||
btScalar depth = -(pivotAInW - pivotBInW).dot(normal); //this is the error projected on the normal
|
||||
btScalar impulse = depth*tau/timeStep * jacDiagABInv - rel_vel * jacDiagABInv;
|
||||
m_appliedImpulse += impulse;
|
||||
btVector3 impulse_vector = normal * impulse;
|
||||
m_rbA.applyImpulse(impulse_vector, pivotAInW - m_rbA.getCenterOfMassPosition());
|
||||
m_rbB.applyImpulse(-impulse_vector, pivotBInW - m_rbB.getCenterOfMassPosition());
|
||||
}
|
||||
}
|
||||
|
||||
{
|
||||
///solve angular part
|
||||
const btVector3& angVelA = getRigidBodyA().getAngularVelocity();
|
||||
const btVector3& angVelB = getRigidBodyB().getAngularVelocity();
|
||||
|
||||
// solve swing limit
|
||||
if (m_solveSwingLimit)
|
||||
{
|
||||
btScalar amplitude = ((angVelB - angVelA).dot( m_swingAxis )*m_relaxationFactor*m_relaxationFactor + m_swingCorrection*(btScalar(1.)/timeStep)*m_biasFactor);
|
||||
btScalar impulseMag = amplitude * m_kSwing;
|
||||
|
||||
// Clamp the accumulated impulse
|
||||
btScalar temp = m_accSwingLimitImpulse;
|
||||
m_accSwingLimitImpulse = btMax(m_accSwingLimitImpulse + impulseMag, 0.0f );
|
||||
impulseMag = m_accSwingLimitImpulse - temp;
|
||||
|
||||
btVector3 impulse = m_swingAxis * impulseMag;
|
||||
|
||||
m_rbA.applyTorqueImpulse(impulse);
|
||||
m_rbB.applyTorqueImpulse(-impulse);
|
||||
|
||||
}
|
||||
|
||||
// solve twist limit
|
||||
if (m_solveTwistLimit)
|
||||
{
|
||||
btScalar amplitude = ((angVelB - angVelA).dot( m_twistAxis )*m_relaxationFactor*m_relaxationFactor + m_twistCorrection*(btScalar(1.)/timeStep)*m_biasFactor );
|
||||
btScalar impulseMag = amplitude * m_kTwist;
|
||||
|
||||
// Clamp the accumulated impulse
|
||||
btScalar temp = m_accTwistLimitImpulse;
|
||||
m_accTwistLimitImpulse = btMax(m_accTwistLimitImpulse + impulseMag, 0.0f );
|
||||
impulseMag = m_accTwistLimitImpulse - temp;
|
||||
|
||||
btVector3 impulse = m_twistAxis * impulseMag;
|
||||
|
||||
m_rbA.applyTorqueImpulse(impulse);
|
||||
m_rbB.applyTorqueImpulse(-impulse);
|
||||
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
void btConeTwistConstraint::updateRHS(btScalar timeStep)
|
||||
{
|
||||
(void)timeStep;
|
||||
|
||||
}
|
||||
@ -0,0 +1,126 @@
|
||||
/*
|
||||
Bullet Continuous Collision Detection and Physics Library
|
||||
btConeTwistConstraint is Copyright (c) 2007 Starbreeze Studios
|
||||
|
||||
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.
|
||||
|
||||
Written by: Marcus Hennix
|
||||
*/
|
||||
|
||||
|
||||
|
||||
#ifndef CONETWISTCONSTRAINT_H
|
||||
#define CONETWISTCONSTRAINT_H
|
||||
|
||||
#include "../../LinearMath/btVector3.h"
|
||||
#include "btJacobianEntry.h"
|
||||
#include "btTypedConstraint.h"
|
||||
|
||||
class btRigidBody;
|
||||
|
||||
|
||||
///btConeTwistConstraint can be used to simulate ragdoll joints (upper arm, leg etc)
|
||||
class btConeTwistConstraint : public btTypedConstraint
|
||||
{
|
||||
#ifdef IN_PARALLELL_SOLVER
|
||||
public:
|
||||
#endif
|
||||
btJacobianEntry m_jac[3]; //3 orthogonal linear constraints
|
||||
|
||||
btTransform m_rbAFrame;
|
||||
btTransform m_rbBFrame;
|
||||
|
||||
btScalar m_limitSoftness;
|
||||
btScalar m_biasFactor;
|
||||
btScalar m_relaxationFactor;
|
||||
|
||||
btScalar m_swingSpan1;
|
||||
btScalar m_swingSpan2;
|
||||
btScalar m_twistSpan;
|
||||
|
||||
btVector3 m_swingAxis;
|
||||
btVector3 m_twistAxis;
|
||||
|
||||
btScalar m_kSwing;
|
||||
btScalar m_kTwist;
|
||||
|
||||
btScalar m_twistLimitSign;
|
||||
btScalar m_swingCorrection;
|
||||
btScalar m_twistCorrection;
|
||||
|
||||
btScalar m_accSwingLimitImpulse;
|
||||
btScalar m_accTwistLimitImpulse;
|
||||
|
||||
bool m_angularOnly;
|
||||
bool m_solveTwistLimit;
|
||||
bool m_solveSwingLimit;
|
||||
|
||||
|
||||
public:
|
||||
|
||||
btConeTwistConstraint(btRigidBody& rbA,btRigidBody& rbB,const btTransform& rbAFrame, const btTransform& rbBFrame);
|
||||
|
||||
btConeTwistConstraint(btRigidBody& rbA,const btTransform& rbAFrame);
|
||||
|
||||
btConeTwistConstraint();
|
||||
|
||||
virtual void buildJacobian();
|
||||
|
||||
virtual void solveConstraint(btScalar timeStep);
|
||||
|
||||
void updateRHS(btScalar timeStep);
|
||||
|
||||
const btRigidBody& getRigidBodyA() const
|
||||
{
|
||||
return m_rbA;
|
||||
}
|
||||
const btRigidBody& getRigidBodyB() const
|
||||
{
|
||||
return m_rbB;
|
||||
}
|
||||
|
||||
void setAngularOnly(bool angularOnly)
|
||||
{
|
||||
m_angularOnly = angularOnly;
|
||||
}
|
||||
|
||||
void setLimit(btScalar _swingSpan1,btScalar _swingSpan2,btScalar _twistSpan, btScalar _softness = 0.8f, btScalar _biasFactor = 0.3f, btScalar _relaxationFactor = 1.0f)
|
||||
{
|
||||
m_swingSpan1 = _swingSpan1;
|
||||
m_swingSpan2 = _swingSpan2;
|
||||
m_twistSpan = _twistSpan;
|
||||
|
||||
m_limitSoftness = _softness;
|
||||
m_biasFactor = _biasFactor;
|
||||
m_relaxationFactor = _relaxationFactor;
|
||||
}
|
||||
|
||||
const btTransform& getAFrame() { return m_rbAFrame; };
|
||||
const btTransform& getBFrame() { return m_rbBFrame; };
|
||||
|
||||
inline int getSolveTwistLimit()
|
||||
{
|
||||
return m_solveTwistLimit;
|
||||
}
|
||||
|
||||
inline int getSolveSwingLimit()
|
||||
{
|
||||
return m_solveTwistLimit;
|
||||
}
|
||||
|
||||
inline btScalar getTwistLimitSign()
|
||||
{
|
||||
return m_twistLimitSign;
|
||||
}
|
||||
|
||||
};
|
||||
|
||||
#endif //CONETWISTCONSTRAINT_H
|
||||
@ -0,0 +1,52 @@
|
||||
/*
|
||||
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 CONSTRAINT_SOLVER_H
|
||||
#define CONSTRAINT_SOLVER_H
|
||||
|
||||
#include "LinearMath/btScalar.h"
|
||||
|
||||
class btPersistentManifold;
|
||||
class btRigidBody;
|
||||
class btCollisionObject;
|
||||
class btTypedConstraint;
|
||||
struct btContactSolverInfo;
|
||||
struct btBroadphaseProxy;
|
||||
class btIDebugDraw;
|
||||
class btStackAlloc;
|
||||
|
||||
/// btConstraintSolver provides solver interface
|
||||
class btConstraintSolver
|
||||
{
|
||||
|
||||
public:
|
||||
|
||||
virtual ~btConstraintSolver() {}
|
||||
|
||||
virtual void prepareSolve (int numBodies, int numManifolds) {;}
|
||||
|
||||
///solve a group of constraints
|
||||
virtual btScalar solveGroup(btCollisionObject** bodies,int numBodies,btPersistentManifold** manifold,int numManifolds,btTypedConstraint** constraints,int numConstraints, const btContactSolverInfo& info,class btIDebugDraw* debugDrawer, btStackAlloc* stackAlloc) = 0;
|
||||
|
||||
virtual void allSolved (const btContactSolverInfo& info,class btIDebugDraw* debugDrawer, btStackAlloc* stackAlloc) {;}
|
||||
|
||||
///clear internal cached data and reset random seed
|
||||
virtual void reset() = 0;
|
||||
};
|
||||
|
||||
|
||||
|
||||
|
||||
#endif //CONSTRAINT_SOLVER_H
|
||||
@ -0,0 +1,417 @@
|
||||
/*
|
||||
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.
|
||||
*/
|
||||
|
||||
|
||||
#include "btContactConstraint.h"
|
||||
#include "BulletDynamics/Dynamics/btRigidBody.h"
|
||||
#include "LinearMath/btVector3.h"
|
||||
#include "btJacobianEntry.h"
|
||||
#include "btContactSolverInfo.h"
|
||||
#include "LinearMath/btMinMax.h"
|
||||
#include "BulletCollision/NarrowPhaseCollision/btManifoldPoint.h"
|
||||
|
||||
#define ASSERT2 assert
|
||||
|
||||
#define USE_INTERNAL_APPLY_IMPULSE 1
|
||||
|
||||
|
||||
//bilateral constraint between two dynamic objects
|
||||
void resolveSingleBilateral(btRigidBody& body1, const btVector3& pos1,
|
||||
btRigidBody& body2, const btVector3& pos2,
|
||||
btScalar distance, const btVector3& normal,btScalar& impulse ,btScalar timeStep)
|
||||
{
|
||||
(void)timeStep;
|
||||
(void)distance;
|
||||
|
||||
|
||||
btScalar normalLenSqr = normal.length2();
|
||||
ASSERT2(btFabs(normalLenSqr) < btScalar(1.1));
|
||||
if (normalLenSqr > btScalar(1.1))
|
||||
{
|
||||
impulse = btScalar(0.);
|
||||
return;
|
||||
}
|
||||
btVector3 rel_pos1 = pos1 - body1.getCenterOfMassPosition();
|
||||
btVector3 rel_pos2 = pos2 - body2.getCenterOfMassPosition();
|
||||
//this jacobian entry could be re-used for all iterations
|
||||
|
||||
btVector3 vel1 = body1.getVelocityInLocalPoint(rel_pos1);
|
||||
btVector3 vel2 = body2.getVelocityInLocalPoint(rel_pos2);
|
||||
btVector3 vel = vel1 - vel2;
|
||||
|
||||
|
||||
btJacobianEntry jac(body1.getCenterOfMassTransform().getBasis().transpose(),
|
||||
body2.getCenterOfMassTransform().getBasis().transpose(),
|
||||
rel_pos1,rel_pos2,normal,body1.getInvInertiaDiagLocal(),body1.getInvMass(),
|
||||
body2.getInvInertiaDiagLocal(),body2.getInvMass());
|
||||
|
||||
btScalar jacDiagAB = jac.getDiagonal();
|
||||
btScalar jacDiagABInv = btScalar(1.) / jacDiagAB;
|
||||
|
||||
btScalar rel_vel = jac.getRelativeVelocity(
|
||||
body1.getLinearVelocity(),
|
||||
body1.getCenterOfMassTransform().getBasis().transpose() * body1.getAngularVelocity(),
|
||||
body2.getLinearVelocity(),
|
||||
body2.getCenterOfMassTransform().getBasis().transpose() * body2.getAngularVelocity());
|
||||
btScalar a;
|
||||
a=jacDiagABInv;
|
||||
|
||||
|
||||
rel_vel = normal.dot(vel);
|
||||
|
||||
//todo: move this into proper structure
|
||||
btScalar contactDamping = btScalar(0.2);
|
||||
|
||||
#ifdef ONLY_USE_LINEAR_MASS
|
||||
btScalar massTerm = btScalar(1.) / (body1.getInvMass() + body2.getInvMass());
|
||||
impulse = - contactDamping * rel_vel * massTerm;
|
||||
#else
|
||||
btScalar velocityImpulse = -contactDamping * rel_vel * jacDiagABInv;
|
||||
impulse = velocityImpulse;
|
||||
#endif
|
||||
}
|
||||
|
||||
|
||||
|
||||
//response between two dynamic objects with friction
|
||||
btScalar resolveSingleCollision(
|
||||
btRigidBody& body1,
|
||||
btRigidBody& body2,
|
||||
btManifoldPoint& contactPoint,
|
||||
const btContactSolverInfo& solverInfo)
|
||||
{
|
||||
|
||||
const btVector3& pos1_ = contactPoint.getPositionWorldOnA();
|
||||
const btVector3& pos2_ = contactPoint.getPositionWorldOnB();
|
||||
const btVector3& normal = contactPoint.m_normalWorldOnB;
|
||||
|
||||
//constant over all iterations
|
||||
btVector3 rel_pos1 = pos1_ - body1.getCenterOfMassPosition();
|
||||
btVector3 rel_pos2 = pos2_ - body2.getCenterOfMassPosition();
|
||||
|
||||
btVector3 vel1 = body1.getVelocityInLocalPoint(rel_pos1);
|
||||
btVector3 vel2 = body2.getVelocityInLocalPoint(rel_pos2);
|
||||
btVector3 vel = vel1 - vel2;
|
||||
btScalar rel_vel;
|
||||
rel_vel = normal.dot(vel);
|
||||
|
||||
btScalar Kfps = btScalar(1.) / solverInfo.m_timeStep ;
|
||||
|
||||
// btScalar damping = solverInfo.m_damping ;
|
||||
btScalar Kerp = solverInfo.m_erp;
|
||||
btScalar Kcor = Kerp *Kfps;
|
||||
|
||||
btConstraintPersistentData* cpd = (btConstraintPersistentData*) contactPoint.m_userPersistentData;
|
||||
assert(cpd);
|
||||
btScalar distance = cpd->m_penetration;
|
||||
btScalar positionalError = Kcor *-distance;
|
||||
btScalar velocityError = cpd->m_restitution - rel_vel;// * damping;
|
||||
|
||||
btScalar penetrationImpulse = positionalError * cpd->m_jacDiagABInv;
|
||||
|
||||
btScalar velocityImpulse = velocityError * cpd->m_jacDiagABInv;
|
||||
|
||||
btScalar normalImpulse = penetrationImpulse+velocityImpulse;
|
||||
|
||||
// See Erin Catto's GDC 2006 paper: Clamp the accumulated impulse
|
||||
btScalar oldNormalImpulse = cpd->m_appliedImpulse;
|
||||
btScalar sum = oldNormalImpulse + normalImpulse;
|
||||
cpd->m_appliedImpulse = btScalar(0.) > sum ? btScalar(0.): sum;
|
||||
|
||||
normalImpulse = cpd->m_appliedImpulse - oldNormalImpulse;
|
||||
|
||||
#ifdef USE_INTERNAL_APPLY_IMPULSE
|
||||
if (body1.getInvMass())
|
||||
{
|
||||
body1.internalApplyImpulse(contactPoint.m_normalWorldOnB*body1.getInvMass(),cpd->m_angularComponentA,normalImpulse);
|
||||
}
|
||||
if (body2.getInvMass())
|
||||
{
|
||||
body2.internalApplyImpulse(contactPoint.m_normalWorldOnB*body2.getInvMass(),cpd->m_angularComponentB,-normalImpulse);
|
||||
}
|
||||
#else //USE_INTERNAL_APPLY_IMPULSE
|
||||
body1.applyImpulse(normal*(normalImpulse), rel_pos1);
|
||||
body2.applyImpulse(-normal*(normalImpulse), rel_pos2);
|
||||
#endif //USE_INTERNAL_APPLY_IMPULSE
|
||||
|
||||
return normalImpulse;
|
||||
}
|
||||
|
||||
|
||||
btScalar resolveSingleFriction(
|
||||
btRigidBody& body1,
|
||||
btRigidBody& body2,
|
||||
btManifoldPoint& contactPoint,
|
||||
const btContactSolverInfo& solverInfo)
|
||||
{
|
||||
|
||||
(void)solverInfo;
|
||||
|
||||
const btVector3& pos1 = contactPoint.getPositionWorldOnA();
|
||||
const btVector3& pos2 = contactPoint.getPositionWorldOnB();
|
||||
|
||||
btVector3 rel_pos1 = pos1 - body1.getCenterOfMassPosition();
|
||||
btVector3 rel_pos2 = pos2 - body2.getCenterOfMassPosition();
|
||||
|
||||
btConstraintPersistentData* cpd = (btConstraintPersistentData*) contactPoint.m_userPersistentData;
|
||||
assert(cpd);
|
||||
|
||||
btScalar combinedFriction = cpd->m_friction;
|
||||
|
||||
btScalar limit = cpd->m_appliedImpulse * combinedFriction;
|
||||
|
||||
if (cpd->m_appliedImpulse>btScalar(0.))
|
||||
//friction
|
||||
{
|
||||
//apply friction in the 2 tangential directions
|
||||
|
||||
// 1st tangent
|
||||
btVector3 vel1 = body1.getVelocityInLocalPoint(rel_pos1);
|
||||
btVector3 vel2 = body2.getVelocityInLocalPoint(rel_pos2);
|
||||
btVector3 vel = vel1 - vel2;
|
||||
|
||||
btScalar j1,j2;
|
||||
|
||||
{
|
||||
|
||||
btScalar vrel = cpd->m_frictionWorldTangential0.dot(vel);
|
||||
|
||||
// calculate j that moves us to zero relative velocity
|
||||
j1 = -vrel * cpd->m_jacDiagABInvTangent0;
|
||||
btScalar oldTangentImpulse = cpd->m_accumulatedTangentImpulse0;
|
||||
cpd->m_accumulatedTangentImpulse0 = oldTangentImpulse + j1;
|
||||
GEN_set_min(cpd->m_accumulatedTangentImpulse0, limit);
|
||||
GEN_set_max(cpd->m_accumulatedTangentImpulse0, -limit);
|
||||
j1 = cpd->m_accumulatedTangentImpulse0 - oldTangentImpulse;
|
||||
|
||||
}
|
||||
{
|
||||
// 2nd tangent
|
||||
|
||||
btScalar vrel = cpd->m_frictionWorldTangential1.dot(vel);
|
||||
|
||||
// calculate j that moves us to zero relative velocity
|
||||
j2 = -vrel * cpd->m_jacDiagABInvTangent1;
|
||||
btScalar oldTangentImpulse = cpd->m_accumulatedTangentImpulse1;
|
||||
cpd->m_accumulatedTangentImpulse1 = oldTangentImpulse + j2;
|
||||
GEN_set_min(cpd->m_accumulatedTangentImpulse1, limit);
|
||||
GEN_set_max(cpd->m_accumulatedTangentImpulse1, -limit);
|
||||
j2 = cpd->m_accumulatedTangentImpulse1 - oldTangentImpulse;
|
||||
}
|
||||
|
||||
#ifdef USE_INTERNAL_APPLY_IMPULSE
|
||||
if (body1.getInvMass())
|
||||
{
|
||||
body1.internalApplyImpulse(cpd->m_frictionWorldTangential0*body1.getInvMass(),cpd->m_frictionAngularComponent0A,j1);
|
||||
body1.internalApplyImpulse(cpd->m_frictionWorldTangential1*body1.getInvMass(),cpd->m_frictionAngularComponent1A,j2);
|
||||
}
|
||||
if (body2.getInvMass())
|
||||
{
|
||||
body2.internalApplyImpulse(cpd->m_frictionWorldTangential0*body2.getInvMass(),cpd->m_frictionAngularComponent0B,-j1);
|
||||
body2.internalApplyImpulse(cpd->m_frictionWorldTangential1*body2.getInvMass(),cpd->m_frictionAngularComponent1B,-j2);
|
||||
}
|
||||
#else //USE_INTERNAL_APPLY_IMPULSE
|
||||
body1.applyImpulse((j1 * cpd->m_frictionWorldTangential0)+(j2 * cpd->m_frictionWorldTangential1), rel_pos1);
|
||||
body2.applyImpulse((j1 * -cpd->m_frictionWorldTangential0)+(j2 * -cpd->m_frictionWorldTangential1), rel_pos2);
|
||||
#endif //USE_INTERNAL_APPLY_IMPULSE
|
||||
|
||||
|
||||
}
|
||||
return cpd->m_appliedImpulse;
|
||||
}
|
||||
|
||||
|
||||
btScalar resolveSingleFrictionOriginal(
|
||||
btRigidBody& body1,
|
||||
btRigidBody& body2,
|
||||
btManifoldPoint& contactPoint,
|
||||
const btContactSolverInfo& solverInfo)
|
||||
{
|
||||
|
||||
(void)solverInfo;
|
||||
|
||||
const btVector3& pos1 = contactPoint.getPositionWorldOnA();
|
||||
const btVector3& pos2 = contactPoint.getPositionWorldOnB();
|
||||
|
||||
btVector3 rel_pos1 = pos1 - body1.getCenterOfMassPosition();
|
||||
btVector3 rel_pos2 = pos2 - body2.getCenterOfMassPosition();
|
||||
|
||||
btConstraintPersistentData* cpd = (btConstraintPersistentData*) contactPoint.m_userPersistentData;
|
||||
assert(cpd);
|
||||
|
||||
btScalar combinedFriction = cpd->m_friction;
|
||||
|
||||
btScalar limit = cpd->m_appliedImpulse * combinedFriction;
|
||||
//if (contactPoint.m_appliedImpulse>btScalar(0.))
|
||||
//friction
|
||||
{
|
||||
//apply friction in the 2 tangential directions
|
||||
|
||||
{
|
||||
// 1st tangent
|
||||
btVector3 vel1 = body1.getVelocityInLocalPoint(rel_pos1);
|
||||
btVector3 vel2 = body2.getVelocityInLocalPoint(rel_pos2);
|
||||
btVector3 vel = vel1 - vel2;
|
||||
|
||||
btScalar vrel = cpd->m_frictionWorldTangential0.dot(vel);
|
||||
|
||||
// calculate j that moves us to zero relative velocity
|
||||
btScalar j = -vrel * cpd->m_jacDiagABInvTangent0;
|
||||
btScalar total = cpd->m_accumulatedTangentImpulse0 + j;
|
||||
GEN_set_min(total, limit);
|
||||
GEN_set_max(total, -limit);
|
||||
j = total - cpd->m_accumulatedTangentImpulse0;
|
||||
cpd->m_accumulatedTangentImpulse0 = total;
|
||||
body1.applyImpulse(j * cpd->m_frictionWorldTangential0, rel_pos1);
|
||||
body2.applyImpulse(j * -cpd->m_frictionWorldTangential0, rel_pos2);
|
||||
}
|
||||
|
||||
|
||||
{
|
||||
// 2nd tangent
|
||||
btVector3 vel1 = body1.getVelocityInLocalPoint(rel_pos1);
|
||||
btVector3 vel2 = body2.getVelocityInLocalPoint(rel_pos2);
|
||||
btVector3 vel = vel1 - vel2;
|
||||
|
||||
btScalar vrel = cpd->m_frictionWorldTangential1.dot(vel);
|
||||
|
||||
// calculate j that moves us to zero relative velocity
|
||||
btScalar j = -vrel * cpd->m_jacDiagABInvTangent1;
|
||||
btScalar total = cpd->m_accumulatedTangentImpulse1 + j;
|
||||
GEN_set_min(total, limit);
|
||||
GEN_set_max(total, -limit);
|
||||
j = total - cpd->m_accumulatedTangentImpulse1;
|
||||
cpd->m_accumulatedTangentImpulse1 = total;
|
||||
body1.applyImpulse(j * cpd->m_frictionWorldTangential1, rel_pos1);
|
||||
body2.applyImpulse(j * -cpd->m_frictionWorldTangential1, rel_pos2);
|
||||
}
|
||||
}
|
||||
return cpd->m_appliedImpulse;
|
||||
}
|
||||
|
||||
|
||||
//velocity + friction
|
||||
//response between two dynamic objects with friction
|
||||
btScalar resolveSingleCollisionCombined(
|
||||
btRigidBody& body1,
|
||||
btRigidBody& body2,
|
||||
btManifoldPoint& contactPoint,
|
||||
const btContactSolverInfo& solverInfo)
|
||||
{
|
||||
|
||||
const btVector3& pos1 = contactPoint.getPositionWorldOnA();
|
||||
const btVector3& pos2 = contactPoint.getPositionWorldOnB();
|
||||
const btVector3& normal = contactPoint.m_normalWorldOnB;
|
||||
|
||||
btVector3 rel_pos1 = pos1 - body1.getCenterOfMassPosition();
|
||||
btVector3 rel_pos2 = pos2 - body2.getCenterOfMassPosition();
|
||||
|
||||
btVector3 vel1 = body1.getVelocityInLocalPoint(rel_pos1);
|
||||
btVector3 vel2 = body2.getVelocityInLocalPoint(rel_pos2);
|
||||
btVector3 vel = vel1 - vel2;
|
||||
btScalar rel_vel;
|
||||
rel_vel = normal.dot(vel);
|
||||
|
||||
btScalar Kfps = btScalar(1.) / solverInfo.m_timeStep ;
|
||||
|
||||
//btScalar damping = solverInfo.m_damping ;
|
||||
btScalar Kerp = solverInfo.m_erp;
|
||||
btScalar Kcor = Kerp *Kfps;
|
||||
|
||||
btConstraintPersistentData* cpd = (btConstraintPersistentData*) contactPoint.m_userPersistentData;
|
||||
assert(cpd);
|
||||
btScalar distance = cpd->m_penetration;
|
||||
btScalar positionalError = Kcor *-distance;
|
||||
btScalar velocityError = cpd->m_restitution - rel_vel;// * damping;
|
||||
|
||||
btScalar penetrationImpulse = positionalError * cpd->m_jacDiagABInv;
|
||||
|
||||
btScalar velocityImpulse = velocityError * cpd->m_jacDiagABInv;
|
||||
|
||||
btScalar normalImpulse = penetrationImpulse+velocityImpulse;
|
||||
|
||||
// See Erin Catto's GDC 2006 paper: Clamp the accumulated impulse
|
||||
btScalar oldNormalImpulse = cpd->m_appliedImpulse;
|
||||
btScalar sum = oldNormalImpulse + normalImpulse;
|
||||
cpd->m_appliedImpulse = btScalar(0.) > sum ? btScalar(0.): sum;
|
||||
|
||||
normalImpulse = cpd->m_appliedImpulse - oldNormalImpulse;
|
||||
|
||||
|
||||
#ifdef USE_INTERNAL_APPLY_IMPULSE
|
||||
if (body1.getInvMass())
|
||||
{
|
||||
body1.internalApplyImpulse(contactPoint.m_normalWorldOnB*body1.getInvMass(),cpd->m_angularComponentA,normalImpulse);
|
||||
}
|
||||
if (body2.getInvMass())
|
||||
{
|
||||
body2.internalApplyImpulse(contactPoint.m_normalWorldOnB*body2.getInvMass(),cpd->m_angularComponentB,-normalImpulse);
|
||||
}
|
||||
#else //USE_INTERNAL_APPLY_IMPULSE
|
||||
body1.applyImpulse(normal*(normalImpulse), rel_pos1);
|
||||
body2.applyImpulse(-normal*(normalImpulse), rel_pos2);
|
||||
#endif //USE_INTERNAL_APPLY_IMPULSE
|
||||
|
||||
{
|
||||
//friction
|
||||
btVector3 vel1 = body1.getVelocityInLocalPoint(rel_pos1);
|
||||
btVector3 vel2 = body2.getVelocityInLocalPoint(rel_pos2);
|
||||
btVector3 vel = vel1 - vel2;
|
||||
|
||||
rel_vel = normal.dot(vel);
|
||||
|
||||
|
||||
btVector3 lat_vel = vel - normal * rel_vel;
|
||||
btScalar lat_rel_vel = lat_vel.length();
|
||||
|
||||
btScalar combinedFriction = cpd->m_friction;
|
||||
|
||||
if (cpd->m_appliedImpulse > 0)
|
||||
if (lat_rel_vel > SIMD_EPSILON)
|
||||
{
|
||||
lat_vel /= lat_rel_vel;
|
||||
btVector3 temp1 = body1.getInvInertiaTensorWorld() * rel_pos1.cross(lat_vel);
|
||||
btVector3 temp2 = body2.getInvInertiaTensorWorld() * rel_pos2.cross(lat_vel);
|
||||
btScalar friction_impulse = lat_rel_vel /
|
||||
(body1.getInvMass() + body2.getInvMass() + lat_vel.dot(temp1.cross(rel_pos1) + temp2.cross(rel_pos2)));
|
||||
btScalar normal_impulse = cpd->m_appliedImpulse * combinedFriction;
|
||||
|
||||
GEN_set_min(friction_impulse, normal_impulse);
|
||||
GEN_set_max(friction_impulse, -normal_impulse);
|
||||
body1.applyImpulse(lat_vel * -friction_impulse, rel_pos1);
|
||||
body2.applyImpulse(lat_vel * friction_impulse, rel_pos2);
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
|
||||
return normalImpulse;
|
||||
}
|
||||
|
||||
btScalar resolveSingleFrictionEmpty(
|
||||
btRigidBody& body1,
|
||||
btRigidBody& body2,
|
||||
btManifoldPoint& contactPoint,
|
||||
const btContactSolverInfo& solverInfo)
|
||||
{
|
||||
(void)contactPoint;
|
||||
(void)body1;
|
||||
(void)body2;
|
||||
(void)solverInfo;
|
||||
|
||||
|
||||
return btScalar(0.);
|
||||
};
|
||||
|
||||
122
bullet/src/BulletDynamics/ConstraintSolver/btContactConstraint.h
Normal file
122
bullet/src/BulletDynamics/ConstraintSolver/btContactConstraint.h
Normal file
@ -0,0 +1,122 @@
|
||||
/*
|
||||
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 CONTACT_CONSTRAINT_H
|
||||
#define CONTACT_CONSTRAINT_H
|
||||
|
||||
//todo: make into a proper class working with the iterative constraint solver
|
||||
|
||||
class btRigidBody;
|
||||
#include "LinearMath/btVector3.h"
|
||||
#include "LinearMath/btScalar.h"
|
||||
struct btContactSolverInfo;
|
||||
class btManifoldPoint;
|
||||
|
||||
enum {
|
||||
DEFAULT_CONTACT_SOLVER_TYPE=0,
|
||||
CONTACT_SOLVER_TYPE1,
|
||||
CONTACT_SOLVER_TYPE2,
|
||||
USER_CONTACT_SOLVER_TYPE1,
|
||||
MAX_CONTACT_SOLVER_TYPES
|
||||
};
|
||||
|
||||
|
||||
typedef btScalar (*ContactSolverFunc)(btRigidBody& body1,
|
||||
btRigidBody& body2,
|
||||
class btManifoldPoint& contactPoint,
|
||||
const btContactSolverInfo& info);
|
||||
|
||||
///stores some extra information to each contact point. It is not in the contact point, because that want to keep the collision detection independent from the constraint solver.
|
||||
struct btConstraintPersistentData
|
||||
{
|
||||
inline btConstraintPersistentData()
|
||||
:m_appliedImpulse(btScalar(0.)),
|
||||
m_prevAppliedImpulse(btScalar(0.)),
|
||||
m_accumulatedTangentImpulse0(btScalar(0.)),
|
||||
m_accumulatedTangentImpulse1(btScalar(0.)),
|
||||
m_jacDiagABInv(btScalar(0.)),
|
||||
m_persistentLifeTime(0),
|
||||
m_restitution(btScalar(0.)),
|
||||
m_friction(btScalar(0.)),
|
||||
m_penetration(btScalar(0.)),
|
||||
m_contactSolverFunc(0),
|
||||
m_frictionSolverFunc(0)
|
||||
{
|
||||
}
|
||||
|
||||
|
||||
/// total applied impulse during most recent frame
|
||||
btScalar m_appliedImpulse;
|
||||
btScalar m_prevAppliedImpulse;
|
||||
btScalar m_accumulatedTangentImpulse0;
|
||||
btScalar m_accumulatedTangentImpulse1;
|
||||
|
||||
btScalar m_jacDiagABInv;
|
||||
btScalar m_jacDiagABInvTangent0;
|
||||
btScalar m_jacDiagABInvTangent1;
|
||||
int m_persistentLifeTime;
|
||||
btScalar m_restitution;
|
||||
btScalar m_friction;
|
||||
btScalar m_penetration;
|
||||
btVector3 m_frictionWorldTangential0;
|
||||
btVector3 m_frictionWorldTangential1;
|
||||
|
||||
btVector3 m_frictionAngularComponent0A;
|
||||
btVector3 m_frictionAngularComponent0B;
|
||||
btVector3 m_frictionAngularComponent1A;
|
||||
btVector3 m_frictionAngularComponent1B;
|
||||
|
||||
//some data doesn't need to be persistent over frames: todo: clean/reuse this
|
||||
btVector3 m_angularComponentA;
|
||||
btVector3 m_angularComponentB;
|
||||
|
||||
ContactSolverFunc m_contactSolverFunc;
|
||||
ContactSolverFunc m_frictionSolverFunc;
|
||||
|
||||
};
|
||||
|
||||
///bilateral constraint between two dynamic objects
|
||||
///positive distance = separation, negative distance = penetration
|
||||
void resolveSingleBilateral(btRigidBody& body1, const btVector3& pos1,
|
||||
btRigidBody& body2, const btVector3& pos2,
|
||||
btScalar distance, const btVector3& normal,btScalar& impulse ,btScalar timeStep);
|
||||
|
||||
|
||||
///contact constraint resolution:
|
||||
///calculate and apply impulse to satisfy non-penetration and non-negative relative velocity constraint
|
||||
///positive distance = separation, negative distance = penetration
|
||||
btScalar resolveSingleCollision(
|
||||
btRigidBody& body1,
|
||||
btRigidBody& body2,
|
||||
btManifoldPoint& contactPoint,
|
||||
const btContactSolverInfo& info);
|
||||
|
||||
btScalar resolveSingleFriction(
|
||||
btRigidBody& body1,
|
||||
btRigidBody& body2,
|
||||
btManifoldPoint& contactPoint,
|
||||
const btContactSolverInfo& solverInfo
|
||||
);
|
||||
|
||||
|
||||
|
||||
btScalar resolveSingleCollisionCombined(
|
||||
btRigidBody& body1,
|
||||
btRigidBody& body2,
|
||||
btManifoldPoint& contactPoint,
|
||||
const btContactSolverInfo& solverInfo
|
||||
);
|
||||
|
||||
#endif //CONTACT_CONSTRAINT_H
|
||||
@ -0,0 +1,51 @@
|
||||
/*
|
||||
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 CONTACT_SOLVER_INFO
|
||||
#define CONTACT_SOLVER_INFO
|
||||
|
||||
struct btContactSolverInfoData
|
||||
{
|
||||
btScalar m_tau;
|
||||
btScalar m_damping;
|
||||
btScalar m_friction;
|
||||
btScalar m_timeStep;
|
||||
btScalar m_restitution;
|
||||
int m_numIterations;
|
||||
btScalar m_maxErrorReduction;
|
||||
btScalar m_sor;
|
||||
btScalar m_erp;
|
||||
|
||||
};
|
||||
|
||||
struct btContactSolverInfo : public btContactSolverInfoData
|
||||
{
|
||||
|
||||
inline btContactSolverInfo()
|
||||
{
|
||||
m_tau = btScalar(0.6);
|
||||
m_damping = btScalar(1.0);
|
||||
m_friction = btScalar(0.3);
|
||||
m_restitution = btScalar(0.);
|
||||
m_maxErrorReduction = btScalar(20.);
|
||||
m_numIterations = 10;
|
||||
m_erp = btScalar(0.4);
|
||||
m_sor = btScalar(1.3);
|
||||
}
|
||||
|
||||
|
||||
};
|
||||
|
||||
#endif //CONTACT_SOLVER_INFO
|
||||
@ -0,0 +1,390 @@
|
||||
/*
|
||||
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.
|
||||
*/
|
||||
|
||||
|
||||
#include "btGeneric6DofConstraint.h"
|
||||
#include "BulletDynamics/Dynamics/btRigidBody.h"
|
||||
#include "LinearMath/btTransformUtil.h"
|
||||
#include <new>
|
||||
|
||||
static const btScalar kSign[] = { btScalar(1.0), btScalar(-1.0), btScalar(1.0) };
|
||||
static const int kAxisA[] = { 1, 0, 0 };
|
||||
static const int kAxisB[] = { 2, 2, 1 };
|
||||
#define GENERIC_D6_DISABLE_WARMSTARTING 1
|
||||
|
||||
btGeneric6DofConstraint::btGeneric6DofConstraint()
|
||||
:btTypedConstraint(D6_CONSTRAINT_TYPE)
|
||||
{
|
||||
}
|
||||
|
||||
btGeneric6DofConstraint::btGeneric6DofConstraint(btRigidBody& rbA, btRigidBody& rbB, const btTransform& frameInA, const btTransform& frameInB)
|
||||
: btTypedConstraint(D6_CONSTRAINT_TYPE, rbA, rbB)
|
||||
, m_frameInA(frameInA)
|
||||
, m_frameInB(frameInB)
|
||||
{
|
||||
//free means upper < lower,
|
||||
//locked means upper == lower
|
||||
//limited means upper > lower
|
||||
//so start all locked
|
||||
for (int i=0; i<6;++i)
|
||||
{
|
||||
m_lowerLimit[i] = btScalar(0.0);
|
||||
m_upperLimit[i] = btScalar(0.0);
|
||||
m_accumulatedImpulse[i] = btScalar(0.0);
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
|
||||
void btGeneric6DofConstraint::buildJacobian()
|
||||
{
|
||||
btVector3 localNormalInA(0,0,0);
|
||||
|
||||
const btVector3& pivotInA = m_frameInA.getOrigin();
|
||||
const btVector3& pivotInB = m_frameInB.getOrigin();
|
||||
|
||||
btVector3 pivotAInW = m_rbA.getCenterOfMassTransform() * m_frameInA.getOrigin();
|
||||
btVector3 pivotBInW = m_rbB.getCenterOfMassTransform() * m_frameInB.getOrigin();
|
||||
|
||||
btVector3 rel_pos1 = pivotAInW - m_rbA.getCenterOfMassPosition();
|
||||
btVector3 rel_pos2 = pivotBInW - m_rbB.getCenterOfMassPosition();
|
||||
|
||||
int i;
|
||||
//linear part
|
||||
for (i=0;i<3;i++)
|
||||
{
|
||||
if (isLimited(i))
|
||||
{
|
||||
localNormalInA[i] = 1;
|
||||
btVector3 normalWorld = m_rbA.getCenterOfMassTransform().getBasis() * localNormalInA;
|
||||
|
||||
|
||||
// Create linear atom
|
||||
new (&m_jacLinear[i]) btJacobianEntry(
|
||||
m_rbA.getCenterOfMassTransform().getBasis().transpose(),
|
||||
m_rbB.getCenterOfMassTransform().getBasis().transpose(),
|
||||
m_rbA.getCenterOfMassTransform()*pivotInA - m_rbA.getCenterOfMassPosition(),
|
||||
m_rbB.getCenterOfMassTransform()*pivotInB - m_rbB.getCenterOfMassPosition(),
|
||||
normalWorld,
|
||||
m_rbA.getInvInertiaDiagLocal(),
|
||||
m_rbA.getInvMass(),
|
||||
m_rbB.getInvInertiaDiagLocal(),
|
||||
m_rbB.getInvMass());
|
||||
|
||||
//optionally disable warmstarting
|
||||
#ifdef GENERIC_D6_DISABLE_WARMSTARTING
|
||||
m_accumulatedImpulse[i] = btScalar(0.);
|
||||
#endif //GENERIC_D6_DISABLE_WARMSTARTING
|
||||
|
||||
// Apply accumulated impulse
|
||||
btVector3 impulse_vector = m_accumulatedImpulse[i] * normalWorld;
|
||||
|
||||
m_rbA.applyImpulse( impulse_vector, rel_pos1);
|
||||
m_rbB.applyImpulse(-impulse_vector, rel_pos2);
|
||||
|
||||
localNormalInA[i] = 0;
|
||||
}
|
||||
}
|
||||
|
||||
// angular part
|
||||
for (i=0;i<3;i++)
|
||||
{
|
||||
if (isLimited(i+3))
|
||||
{
|
||||
btVector3 axisA = m_rbA.getCenterOfMassTransform().getBasis() * m_frameInA.getBasis().getColumn( kAxisA[i] );
|
||||
btVector3 axisB = m_rbB.getCenterOfMassTransform().getBasis() * m_frameInB.getBasis().getColumn( kAxisB[i] );
|
||||
|
||||
// Dirk: This is IMO mathematically the correct way, but we should consider axisA and axisB being near parallel maybe
|
||||
btVector3 axis = kSign[i] * axisA.cross(axisB);
|
||||
|
||||
// Create angular atom
|
||||
new (&m_jacAng[i]) btJacobianEntry(axis,
|
||||
m_rbA.getCenterOfMassTransform().getBasis().transpose(),
|
||||
m_rbB.getCenterOfMassTransform().getBasis().transpose(),
|
||||
m_rbA.getInvInertiaDiagLocal(),
|
||||
m_rbB.getInvInertiaDiagLocal());
|
||||
|
||||
#ifdef GENERIC_D6_DISABLE_WARMSTARTING
|
||||
m_accumulatedImpulse[i + 3] = btScalar(0.);
|
||||
#endif //GENERIC_D6_DISABLE_WARMSTARTING
|
||||
|
||||
// Apply accumulated impulse
|
||||
btVector3 impulse_vector = m_accumulatedImpulse[i + 3] * axis;
|
||||
|
||||
m_rbA.applyTorqueImpulse( impulse_vector);
|
||||
m_rbB.applyTorqueImpulse(-impulse_vector);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
btScalar getMatrixElem(const btMatrix3x3& mat,int index)
|
||||
{
|
||||
int row = index%3;
|
||||
int col = index / 3;
|
||||
return mat[row][col];
|
||||
}
|
||||
|
||||
///MatrixToEulerXYZ from http://www.geometrictools.com/LibFoundation/Mathematics/Wm4Matrix3.inl.html
|
||||
bool MatrixToEulerXYZ(const btMatrix3x3& mat,btVector3& xyz)
|
||||
{
|
||||
// rot = cy*cz -cy*sz sy
|
||||
// cz*sx*sy+cx*sz cx*cz-sx*sy*sz -cy*sx
|
||||
// -cx*cz*sy+sx*sz cz*sx+cx*sy*sz cx*cy
|
||||
|
||||
/// 0..8
|
||||
|
||||
if (getMatrixElem(mat,2) < btScalar(1.0))
|
||||
{
|
||||
if (getMatrixElem(mat,2) > btScalar(-1.0))
|
||||
{
|
||||
xyz[0] = btAtan2(-getMatrixElem(mat,5),getMatrixElem(mat,8));
|
||||
xyz[1] = btAsin(getMatrixElem(mat,2));
|
||||
xyz[2] = btAtan2(-getMatrixElem(mat,1),getMatrixElem(mat,0));
|
||||
return true;
|
||||
}
|
||||
else
|
||||
{
|
||||
// WARNING. Not unique. XA - ZA = -atan2(r10,r11)
|
||||
xyz[0] = -btAtan2(getMatrixElem(mat,3),getMatrixElem(mat,4));
|
||||
xyz[1] = -SIMD_HALF_PI;
|
||||
xyz[2] = btScalar(0.0);
|
||||
return false;
|
||||
}
|
||||
}
|
||||
else
|
||||
{
|
||||
// WARNING. Not unique. XAngle + ZAngle = atan2(r10,r11)
|
||||
xyz[0] = btAtan2(getMatrixElem(mat,3),getMatrixElem(mat,4));
|
||||
xyz[1] = SIMD_HALF_PI;
|
||||
xyz[2] = 0.0;
|
||||
|
||||
}
|
||||
|
||||
return false;
|
||||
}
|
||||
|
||||
|
||||
void btGeneric6DofConstraint::solveConstraint(btScalar timeStep)
|
||||
{
|
||||
btScalar tau = btScalar(0.1);
|
||||
btScalar damping = btScalar(1.0);
|
||||
|
||||
btVector3 pivotAInW = m_rbA.getCenterOfMassTransform() * m_frameInA.getOrigin();
|
||||
btVector3 pivotBInW = m_rbB.getCenterOfMassTransform() * m_frameInB.getOrigin();
|
||||
|
||||
btVector3 rel_pos1 = pivotAInW - m_rbA.getCenterOfMassPosition();
|
||||
btVector3 rel_pos2 = pivotBInW - m_rbB.getCenterOfMassPosition();
|
||||
|
||||
btVector3 localNormalInA(0,0,0);
|
||||
int i;
|
||||
|
||||
// linear
|
||||
for (i=0;i<3;i++)
|
||||
{
|
||||
if (isLimited(i))
|
||||
{
|
||||
btVector3 angvelA = m_rbA.getCenterOfMassTransform().getBasis().transpose() * m_rbA.getAngularVelocity();
|
||||
btVector3 angvelB = m_rbB.getCenterOfMassTransform().getBasis().transpose() * m_rbB.getAngularVelocity();
|
||||
|
||||
localNormalInA.setValue(0,0,0);
|
||||
localNormalInA[i] = 1;
|
||||
btVector3 normalWorld = m_rbA.getCenterOfMassTransform().getBasis() * localNormalInA;
|
||||
|
||||
btScalar jacDiagABInv = btScalar(1.) / m_jacLinear[i].getDiagonal();
|
||||
|
||||
//velocity error (first order error)
|
||||
btScalar rel_vel = m_jacLinear[i].getRelativeVelocity(m_rbA.getLinearVelocity(),angvelA,
|
||||
m_rbB.getLinearVelocity(),angvelB);
|
||||
|
||||
//positional error (zeroth order error)
|
||||
btScalar depth = -(pivotAInW - pivotBInW).dot(normalWorld);
|
||||
btScalar lo = btScalar(-1e30);
|
||||
btScalar hi = btScalar(1e30);
|
||||
|
||||
//handle the limits
|
||||
if (m_lowerLimit[i] < m_upperLimit[i])
|
||||
{
|
||||
{
|
||||
if (depth > m_upperLimit[i])
|
||||
{
|
||||
depth -= m_upperLimit[i];
|
||||
lo = btScalar(0.);
|
||||
|
||||
} else
|
||||
{
|
||||
if (depth < m_lowerLimit[i])
|
||||
{
|
||||
depth -= m_lowerLimit[i];
|
||||
hi = btScalar(0.);
|
||||
} else
|
||||
{
|
||||
continue;
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
btScalar normalImpulse= (tau*depth/timeStep - damping*rel_vel) * jacDiagABInv;
|
||||
btScalar oldNormalImpulse = m_accumulatedImpulse[i];
|
||||
btScalar sum = oldNormalImpulse + normalImpulse;
|
||||
m_accumulatedImpulse[i] = sum > hi ? btScalar(0.) : sum < lo ? btScalar(0.) : sum;
|
||||
normalImpulse = m_accumulatedImpulse[i] - oldNormalImpulse;
|
||||
|
||||
btVector3 impulse_vector = normalWorld * normalImpulse;
|
||||
m_rbA.applyImpulse( impulse_vector, rel_pos1);
|
||||
m_rbB.applyImpulse(-impulse_vector, rel_pos2);
|
||||
|
||||
localNormalInA[i] = 0;
|
||||
}
|
||||
}
|
||||
|
||||
btVector3 axis;
|
||||
btScalar angle;
|
||||
btTransform frameAWorld = m_rbA.getCenterOfMassTransform() * m_frameInA;
|
||||
btTransform frameBWorld = m_rbB.getCenterOfMassTransform() * m_frameInB;
|
||||
|
||||
btTransformUtil::calculateDiffAxisAngle(frameAWorld,frameBWorld,axis,angle);
|
||||
btQuaternion diff(axis,angle);
|
||||
btMatrix3x3 diffMat (diff);
|
||||
btVector3 xyz;
|
||||
///this is not perfect, we can first check which axis are limited, and choose a more appropriate order
|
||||
MatrixToEulerXYZ(diffMat,xyz);
|
||||
|
||||
// angular
|
||||
for (i=0;i<3;i++)
|
||||
{
|
||||
if (isLimited(i+3))
|
||||
{
|
||||
btVector3 angvelA = m_rbA.getCenterOfMassTransform().getBasis().transpose() * m_rbA.getAngularVelocity();
|
||||
btVector3 angvelB = m_rbB.getCenterOfMassTransform().getBasis().transpose() * m_rbB.getAngularVelocity();
|
||||
|
||||
btScalar jacDiagABInv = btScalar(1.) / m_jacAng[i].getDiagonal();
|
||||
|
||||
//velocity error (first order error)
|
||||
btScalar rel_vel = m_jacAng[i].getRelativeVelocity(m_rbA.getLinearVelocity(),angvelA,
|
||||
m_rbB.getLinearVelocity(),angvelB);
|
||||
|
||||
//positional error (zeroth order error)
|
||||
btVector3 axisA = m_rbA.getCenterOfMassTransform().getBasis() * m_frameInA.getBasis().getColumn( kAxisA[i] );
|
||||
btVector3 axisB = m_rbB.getCenterOfMassTransform().getBasis() * m_frameInB.getBasis().getColumn( kAxisB[i] );
|
||||
|
||||
btScalar rel_pos = kSign[i] * axisA.dot(axisB);
|
||||
|
||||
btScalar lo = btScalar(-1e30);
|
||||
btScalar hi = btScalar(1e30);
|
||||
|
||||
//handle the twist limit
|
||||
if (m_lowerLimit[i+3] < m_upperLimit[i+3])
|
||||
{
|
||||
//clamp the values
|
||||
btScalar loLimit = m_lowerLimit[i+3] > -3.1415 ? m_lowerLimit[i+3] : btScalar(-1e30);
|
||||
btScalar hiLimit = m_upperLimit[i+3] < 3.1415 ? m_upperLimit[i+3] : btScalar(1e30);
|
||||
|
||||
btScalar projAngle = btScalar(-1.)*xyz[i];
|
||||
|
||||
if (projAngle < loLimit)
|
||||
{
|
||||
hi = btScalar(0.);
|
||||
rel_pos = (loLimit - projAngle);
|
||||
} else
|
||||
{
|
||||
if (projAngle > hiLimit)
|
||||
{
|
||||
lo = btScalar(0.);
|
||||
rel_pos = (hiLimit - projAngle);
|
||||
} else
|
||||
{
|
||||
continue;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
//impulse
|
||||
|
||||
btScalar normalImpulse= -(tau*rel_pos/timeStep + damping*rel_vel) * jacDiagABInv;
|
||||
btScalar oldNormalImpulse = m_accumulatedImpulse[i+3];
|
||||
btScalar sum = oldNormalImpulse + normalImpulse;
|
||||
m_accumulatedImpulse[i+3] = sum > hi ? btScalar(0.) : sum < lo ? btScalar(0.) : sum;
|
||||
normalImpulse = m_accumulatedImpulse[i+3] - oldNormalImpulse;
|
||||
|
||||
// Dirk: Not needed - we could actually project onto Jacobian entry here (same as above)
|
||||
btVector3 axis = kSign[i] * axisA.cross(axisB);
|
||||
btVector3 impulse_vector = axis * normalImpulse;
|
||||
|
||||
m_rbA.applyTorqueImpulse( impulse_vector);
|
||||
m_rbB.applyTorqueImpulse(-impulse_vector);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
void btGeneric6DofConstraint::updateRHS(btScalar timeStep)
|
||||
{
|
||||
(void)timeStep;
|
||||
|
||||
}
|
||||
|
||||
btScalar btGeneric6DofConstraint::computeAngle(int axis) const
|
||||
{
|
||||
btScalar angle = btScalar(0.f);
|
||||
|
||||
switch (axis)
|
||||
{
|
||||
case 0:
|
||||
{
|
||||
btVector3 v1 = m_rbA.getCenterOfMassTransform().getBasis() * m_frameInA.getBasis().getColumn(1);
|
||||
btVector3 v2 = m_rbB.getCenterOfMassTransform().getBasis() * m_frameInB.getBasis().getColumn(1);
|
||||
btVector3 w2 = m_rbB.getCenterOfMassTransform().getBasis() * m_frameInB.getBasis().getColumn(2);
|
||||
|
||||
btScalar s = v1.dot(w2);
|
||||
btScalar c = v1.dot(v2);
|
||||
|
||||
angle = btAtan2( s, c );
|
||||
}
|
||||
break;
|
||||
|
||||
case 1:
|
||||
{
|
||||
btVector3 w1 = m_rbA.getCenterOfMassTransform().getBasis() * m_frameInA.getBasis().getColumn(2);
|
||||
btVector3 w2 = m_rbB.getCenterOfMassTransform().getBasis() * m_frameInB.getBasis().getColumn(2);
|
||||
btVector3 u2 = m_rbB.getCenterOfMassTransform().getBasis() * m_frameInB.getBasis().getColumn(0);
|
||||
|
||||
btScalar s = w1.dot(u2);
|
||||
btScalar c = w1.dot(w2);
|
||||
|
||||
angle = btAtan2( s, c );
|
||||
}
|
||||
break;
|
||||
|
||||
case 2:
|
||||
{
|
||||
btVector3 u1 = m_rbA.getCenterOfMassTransform().getBasis() * m_frameInA.getBasis().getColumn(0);
|
||||
btVector3 u2 = m_rbB.getCenterOfMassTransform().getBasis() * m_frameInB.getBasis().getColumn(0);
|
||||
btVector3 v2 = m_rbB.getCenterOfMassTransform().getBasis() * m_frameInB.getBasis().getColumn(1);
|
||||
|
||||
btScalar s = u1.dot(v2);
|
||||
btScalar c = u1.dot(u2);
|
||||
|
||||
angle = btAtan2( s, c );
|
||||
}
|
||||
break;
|
||||
default:
|
||||
btAssert ( 0 ) ;
|
||||
|
||||
break ;
|
||||
}
|
||||
|
||||
return angle;
|
||||
}
|
||||
|
||||
@ -0,0 +1,120 @@
|
||||
/*
|
||||
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 GENERIC_6DOF_CONSTRAINT_H
|
||||
#define GENERIC_6DOF_CONSTRAINT_H
|
||||
|
||||
#include "LinearMath/btVector3.h"
|
||||
#include "btJacobianEntry.h"
|
||||
#include "btTypedConstraint.h"
|
||||
|
||||
class btRigidBody;
|
||||
|
||||
|
||||
|
||||
/// btGeneric6DofConstraint between two rigidbodies each with a pivotpoint that descibes the axis location in local space
|
||||
/// btGeneric6DofConstraint can leave any of the 6 degree of freedom 'free' or 'locked'
|
||||
/// Work in progress (is still a Hinge actually)
|
||||
class btGeneric6DofConstraint : public btTypedConstraint
|
||||
{
|
||||
btJacobianEntry m_jacLinear[3]; // 3 orthogonal linear constraints
|
||||
btJacobianEntry m_jacAng[3]; // 3 orthogonal angular constraints
|
||||
|
||||
btTransform m_frameInA; // the constraint space w.r.t body A
|
||||
btTransform m_frameInB; // the constraint space w.r.t body B
|
||||
|
||||
btScalar m_lowerLimit[6]; // the constraint lower limits
|
||||
btScalar m_upperLimit[6]; // the constraint upper limits
|
||||
|
||||
btScalar m_accumulatedImpulse[6];
|
||||
|
||||
btGeneric6DofConstraint& operator=(btGeneric6DofConstraint& other)
|
||||
{
|
||||
btAssert(0);
|
||||
(void) other;
|
||||
return *this;
|
||||
}
|
||||
|
||||
public:
|
||||
btGeneric6DofConstraint(btRigidBody& rbA, btRigidBody& rbB, const btTransform& frameInA, const btTransform& frameInB );
|
||||
|
||||
btGeneric6DofConstraint();
|
||||
|
||||
|
||||
virtual void buildJacobian();
|
||||
|
||||
virtual void solveConstraint(btScalar timeStep);
|
||||
|
||||
void updateRHS(btScalar timeStep);
|
||||
|
||||
btScalar computeAngle(int axis) const;
|
||||
|
||||
void setLinearLowerLimit(const btVector3& linearLower)
|
||||
{
|
||||
m_lowerLimit[0] = linearLower.getX();
|
||||
m_lowerLimit[1] = linearLower.getY();
|
||||
m_lowerLimit[2] = linearLower.getZ();
|
||||
}
|
||||
|
||||
void setLinearUpperLimit(const btVector3& linearUpper)
|
||||
{
|
||||
m_upperLimit[0] = linearUpper.getX();
|
||||
m_upperLimit[1] = linearUpper.getY();
|
||||
m_upperLimit[2] = linearUpper.getZ();
|
||||
}
|
||||
|
||||
void setAngularLowerLimit(const btVector3& angularLower)
|
||||
{
|
||||
m_lowerLimit[3] = angularLower.getX();
|
||||
m_lowerLimit[4] = angularLower.getY();
|
||||
m_lowerLimit[5] = angularLower.getZ();
|
||||
}
|
||||
|
||||
void setAngularUpperLimit(const btVector3& angularUpper)
|
||||
{
|
||||
m_upperLimit[3] = angularUpper.getX();
|
||||
m_upperLimit[4] = angularUpper.getY();
|
||||
m_upperLimit[5] = angularUpper.getZ();
|
||||
}
|
||||
|
||||
//first 3 are linear, next 3 are angular
|
||||
void SetLimit(int axis, btScalar lo, btScalar hi)
|
||||
{
|
||||
m_lowerLimit[axis] = lo;
|
||||
m_upperLimit[axis] = hi;
|
||||
}
|
||||
|
||||
//free means upper < lower,
|
||||
//locked means upper == lower
|
||||
//limited means upper > lower
|
||||
//limitIndex: first 3 are linear, next 3 are angular
|
||||
bool isLimited(int limitIndex)
|
||||
{
|
||||
return (m_upperLimit[limitIndex] >= m_lowerLimit[limitIndex]);
|
||||
}
|
||||
|
||||
const btRigidBody& getRigidBodyA() const
|
||||
{
|
||||
return m_rbA;
|
||||
}
|
||||
const btRigidBody& getRigidBodyB() const
|
||||
{
|
||||
return m_rbB;
|
||||
}
|
||||
|
||||
|
||||
};
|
||||
|
||||
#endif //GENERIC_6DOF_CONSTRAINT_H
|
||||
398
bullet/src/BulletDynamics/ConstraintSolver/btHingeConstraint.cpp
Normal file
398
bullet/src/BulletDynamics/ConstraintSolver/btHingeConstraint.cpp
Normal file
@ -0,0 +1,398 @@
|
||||
/*
|
||||
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.
|
||||
*/
|
||||
|
||||
|
||||
#include "btHingeConstraint.h"
|
||||
#include "BulletDynamics/Dynamics/btRigidBody.h"
|
||||
#include "LinearMath/btTransformUtil.h"
|
||||
#include "LinearMath/btSimdMinMax.h"
|
||||
#include <new>
|
||||
|
||||
|
||||
btHingeConstraint::btHingeConstraint()
|
||||
: btTypedConstraint (HINGE_CONSTRAINT_TYPE),
|
||||
m_enableAngularMotor(false)
|
||||
{
|
||||
}
|
||||
|
||||
btHingeConstraint::btHingeConstraint(btRigidBody& rbA,btRigidBody& rbB, const btVector3& pivotInA,const btVector3& pivotInB,
|
||||
btVector3& axisInA,btVector3& axisInB)
|
||||
:btTypedConstraint(HINGE_CONSTRAINT_TYPE, rbA,rbB),
|
||||
m_angularOnly(false),
|
||||
m_enableAngularMotor(false)
|
||||
{
|
||||
m_rbAFrame.getOrigin() = pivotInA;
|
||||
|
||||
// since no frame is given, assume this to be zero angle and just pick rb transform axis
|
||||
btVector3 rbAxisA1 = rbA.getCenterOfMassTransform().getBasis().getColumn(0);
|
||||
btScalar projection = rbAxisA1.dot(axisInA);
|
||||
if (projection > SIMD_EPSILON)
|
||||
rbAxisA1 = rbAxisA1*projection - axisInA;
|
||||
else
|
||||
rbAxisA1 = rbA.getCenterOfMassTransform().getBasis().getColumn(1);
|
||||
|
||||
btVector3 rbAxisA2 = rbAxisA1.cross(axisInA);
|
||||
|
||||
m_rbAFrame.getBasis().setValue( rbAxisA1.getX(),rbAxisA2.getX(),axisInA.getX(),
|
||||
rbAxisA1.getY(),rbAxisA2.getY(),axisInA.getY(),
|
||||
rbAxisA1.getZ(),rbAxisA2.getZ(),axisInA.getZ() );
|
||||
|
||||
btQuaternion rotationArc = shortestArcQuat(axisInA,axisInB);
|
||||
btVector3 rbAxisB1 = quatRotate(rotationArc,rbAxisA1);
|
||||
btVector3 rbAxisB2 = rbAxisB1.cross(axisInB);
|
||||
|
||||
|
||||
m_rbBFrame.getOrigin() = pivotInB;
|
||||
m_rbBFrame.getBasis().setValue( rbAxisB1.getX(),rbAxisB2.getX(),-axisInB.getX(),
|
||||
rbAxisB1.getY(),rbAxisB2.getY(),-axisInB.getY(),
|
||||
rbAxisB1.getZ(),rbAxisB2.getZ(),-axisInB.getZ() );
|
||||
|
||||
//start with free
|
||||
m_lowerLimit = btScalar(1e30);
|
||||
m_upperLimit = btScalar(-1e30);
|
||||
m_biasFactor = 0.3f;
|
||||
m_relaxationFactor = 1.0f;
|
||||
m_limitSoftness = 0.9f;
|
||||
m_solveLimit = false;
|
||||
|
||||
}
|
||||
|
||||
|
||||
btHingeConstraint::btHingeConstraint(btRigidBody& rbA,const btVector3& pivotInA,btVector3& axisInA)
|
||||
:btTypedConstraint(HINGE_CONSTRAINT_TYPE, rbA), m_angularOnly(false), m_enableAngularMotor(false)
|
||||
{
|
||||
|
||||
// since no frame is given, assume this to be zero angle and just pick rb transform axis
|
||||
// fixed axis in worldspace
|
||||
btVector3 rbAxisA1 = rbA.getCenterOfMassTransform().getBasis().getColumn(0);
|
||||
btScalar projection = rbAxisA1.dot(axisInA);
|
||||
if (projection > SIMD_EPSILON)
|
||||
rbAxisA1 = rbAxisA1*projection - axisInA;
|
||||
else
|
||||
rbAxisA1 = rbA.getCenterOfMassTransform().getBasis().getColumn(1);
|
||||
|
||||
btVector3 rbAxisA2 = axisInA.cross(rbAxisA1);
|
||||
|
||||
m_rbAFrame.getOrigin() = pivotInA;
|
||||
m_rbAFrame.getBasis().setValue( rbAxisA1.getX(),rbAxisA2.getX(),axisInA.getX(),
|
||||
rbAxisA1.getY(),rbAxisA2.getY(),axisInA.getY(),
|
||||
rbAxisA1.getZ(),rbAxisA2.getZ(),axisInA.getZ() );
|
||||
|
||||
|
||||
btVector3 axisInB = rbA.getCenterOfMassTransform().getBasis() * -axisInA;
|
||||
|
||||
btQuaternion rotationArc = shortestArcQuat(axisInA,axisInB);
|
||||
btVector3 rbAxisB1 = quatRotate(rotationArc,rbAxisA1);
|
||||
btVector3 rbAxisB2 = axisInB.cross(rbAxisB1);
|
||||
|
||||
|
||||
m_rbBFrame.getOrigin() = 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() );
|
||||
|
||||
//start with free
|
||||
m_lowerLimit = btScalar(1e30);
|
||||
m_upperLimit = btScalar(-1e30);
|
||||
m_biasFactor = 0.3f;
|
||||
m_relaxationFactor = 1.0f;
|
||||
m_limitSoftness = 0.9f;
|
||||
m_solveLimit = false;
|
||||
}
|
||||
|
||||
btHingeConstraint::btHingeConstraint(btRigidBody& rbA,btRigidBody& rbB,
|
||||
const btTransform& rbAFrame, const btTransform& rbBFrame)
|
||||
:btTypedConstraint(HINGE_CONSTRAINT_TYPE, rbA,rbB),m_rbAFrame(rbAFrame),m_rbBFrame(rbBFrame),
|
||||
m_angularOnly(false),
|
||||
m_enableAngularMotor(false)
|
||||
{
|
||||
// flip axis
|
||||
m_rbBFrame.getBasis()[0][2] *= btScalar(-1.);
|
||||
m_rbBFrame.getBasis()[1][2] *= btScalar(-1.);
|
||||
m_rbBFrame.getBasis()[2][2] *= btScalar(-1.);
|
||||
|
||||
//start with free
|
||||
m_lowerLimit = btScalar(1e30);
|
||||
m_upperLimit = btScalar(-1e30);
|
||||
m_biasFactor = 0.3f;
|
||||
m_relaxationFactor = 1.0f;
|
||||
m_limitSoftness = 0.9f;
|
||||
m_solveLimit = false;
|
||||
}
|
||||
|
||||
|
||||
|
||||
btHingeConstraint::btHingeConstraint(btRigidBody& rbA, const btTransform& rbAFrame)
|
||||
:btTypedConstraint(HINGE_CONSTRAINT_TYPE, rbA),m_rbAFrame(rbAFrame),m_rbBFrame(rbAFrame),
|
||||
m_angularOnly(false),
|
||||
m_enableAngularMotor(false)
|
||||
{
|
||||
// flip axis
|
||||
m_rbBFrame.getBasis()[0][2] *= btScalar(-1.);
|
||||
m_rbBFrame.getBasis()[1][2] *= btScalar(-1.);
|
||||
m_rbBFrame.getBasis()[2][2] *= btScalar(-1.);
|
||||
|
||||
|
||||
//start with free
|
||||
m_lowerLimit = btScalar(1e30);
|
||||
m_upperLimit = btScalar(-1e30);
|
||||
m_biasFactor = 0.3f;
|
||||
m_relaxationFactor = 1.0f;
|
||||
m_limitSoftness = 0.9f;
|
||||
m_solveLimit = false;
|
||||
}
|
||||
|
||||
void btHingeConstraint::buildJacobian()
|
||||
{
|
||||
m_appliedImpulse = btScalar(0.);
|
||||
|
||||
if (!m_angularOnly)
|
||||
{
|
||||
btVector3 pivotAInW = m_rbA.getCenterOfMassTransform()*m_rbAFrame.getOrigin();
|
||||
btVector3 pivotBInW = m_rbB.getCenterOfMassTransform()*m_rbBFrame.getOrigin();
|
||||
btVector3 relPos = pivotBInW - pivotAInW;
|
||||
|
||||
btVector3 normal[3];
|
||||
if (relPos.length2() > SIMD_EPSILON)
|
||||
{
|
||||
normal[0] = relPos.normalized();
|
||||
}
|
||||
else
|
||||
{
|
||||
normal[0].setValue(btScalar(1.0),0,0);
|
||||
}
|
||||
|
||||
btPlaneSpace1(normal[0], normal[1], normal[2]);
|
||||
|
||||
for (int i=0;i<3;i++)
|
||||
{
|
||||
new (&m_jac[i]) btJacobianEntry(
|
||||
m_rbA.getCenterOfMassTransform().getBasis().transpose(),
|
||||
m_rbB.getCenterOfMassTransform().getBasis().transpose(),
|
||||
pivotAInW - m_rbA.getCenterOfMassPosition(),
|
||||
pivotBInW - m_rbB.getCenterOfMassPosition(),
|
||||
normal[i],
|
||||
m_rbA.getInvInertiaDiagLocal(),
|
||||
m_rbA.getInvMass(),
|
||||
m_rbB.getInvInertiaDiagLocal(),
|
||||
m_rbB.getInvMass());
|
||||
}
|
||||
}
|
||||
|
||||
//calculate two perpendicular jointAxis, orthogonal to hingeAxis
|
||||
//these two jointAxis require equal angular velocities for both bodies
|
||||
|
||||
//this is unused for now, it's a todo
|
||||
btVector3 jointAxis0local;
|
||||
btVector3 jointAxis1local;
|
||||
|
||||
btPlaneSpace1(m_rbAFrame.getBasis().getColumn(2),jointAxis0local,jointAxis1local);
|
||||
|
||||
getRigidBodyA().getCenterOfMassTransform().getBasis() * m_rbAFrame.getBasis().getColumn(2);
|
||||
btVector3 jointAxis0 = getRigidBodyA().getCenterOfMassTransform().getBasis() * jointAxis0local;
|
||||
btVector3 jointAxis1 = getRigidBodyA().getCenterOfMassTransform().getBasis() * jointAxis1local;
|
||||
btVector3 hingeAxisWorld = getRigidBodyA().getCenterOfMassTransform().getBasis() * m_rbAFrame.getBasis().getColumn(2);
|
||||
|
||||
new (&m_jacAng[0]) btJacobianEntry(jointAxis0,
|
||||
m_rbA.getCenterOfMassTransform().getBasis().transpose(),
|
||||
m_rbB.getCenterOfMassTransform().getBasis().transpose(),
|
||||
m_rbA.getInvInertiaDiagLocal(),
|
||||
m_rbB.getInvInertiaDiagLocal());
|
||||
|
||||
new (&m_jacAng[1]) btJacobianEntry(jointAxis1,
|
||||
m_rbA.getCenterOfMassTransform().getBasis().transpose(),
|
||||
m_rbB.getCenterOfMassTransform().getBasis().transpose(),
|
||||
m_rbA.getInvInertiaDiagLocal(),
|
||||
m_rbB.getInvInertiaDiagLocal());
|
||||
|
||||
new (&m_jacAng[2]) btJacobianEntry(hingeAxisWorld,
|
||||
m_rbA.getCenterOfMassTransform().getBasis().transpose(),
|
||||
m_rbB.getCenterOfMassTransform().getBasis().transpose(),
|
||||
m_rbA.getInvInertiaDiagLocal(),
|
||||
m_rbB.getInvInertiaDiagLocal());
|
||||
|
||||
|
||||
// Compute limit information
|
||||
btScalar hingeAngle = getHingeAngle();
|
||||
|
||||
//set bias, sign, clear accumulator
|
||||
m_correction = btScalar(0.);
|
||||
m_limitSign = btScalar(0.);
|
||||
m_solveLimit = false;
|
||||
m_accLimitImpulse = btScalar(0.);
|
||||
|
||||
if (m_lowerLimit < m_upperLimit)
|
||||
{
|
||||
if (hingeAngle <= m_lowerLimit*m_limitSoftness)
|
||||
{
|
||||
m_correction = (m_lowerLimit - hingeAngle);
|
||||
m_limitSign = 1.0f;
|
||||
m_solveLimit = true;
|
||||
}
|
||||
else if (hingeAngle >= m_upperLimit*m_limitSoftness)
|
||||
{
|
||||
m_correction = m_upperLimit - hingeAngle;
|
||||
m_limitSign = -1.0f;
|
||||
m_solveLimit = true;
|
||||
}
|
||||
}
|
||||
|
||||
//Compute K = J*W*J' for hinge axis
|
||||
btVector3 axisA = getRigidBodyA().getCenterOfMassTransform().getBasis() * m_rbAFrame.getBasis().getColumn(2);
|
||||
m_kHinge = 1.0f / (getRigidBodyA().computeAngularImpulseDenominator(axisA) +
|
||||
getRigidBodyB().computeAngularImpulseDenominator(axisA));
|
||||
|
||||
}
|
||||
|
||||
void btHingeConstraint::solveConstraint(btScalar timeStep)
|
||||
{
|
||||
|
||||
btVector3 pivotAInW = m_rbA.getCenterOfMassTransform()*m_rbAFrame.getOrigin();
|
||||
btVector3 pivotBInW = m_rbB.getCenterOfMassTransform()*m_rbBFrame.getOrigin();
|
||||
|
||||
btScalar tau = btScalar(0.3);
|
||||
btScalar damping = btScalar(1.);
|
||||
|
||||
//linear part
|
||||
if (!m_angularOnly)
|
||||
{
|
||||
btVector3 rel_pos1 = pivotAInW - m_rbA.getCenterOfMassPosition();
|
||||
btVector3 rel_pos2 = pivotBInW - m_rbB.getCenterOfMassPosition();
|
||||
|
||||
btVector3 vel1 = m_rbA.getVelocityInLocalPoint(rel_pos1);
|
||||
btVector3 vel2 = m_rbB.getVelocityInLocalPoint(rel_pos2);
|
||||
btVector3 vel = vel1 - vel2;
|
||||
|
||||
for (int i=0;i<3;i++)
|
||||
{
|
||||
const btVector3& normal = m_jac[i].m_linearJointAxis;
|
||||
btScalar jacDiagABInv = btScalar(1.) / m_jac[i].getDiagonal();
|
||||
|
||||
btScalar rel_vel;
|
||||
rel_vel = normal.dot(vel);
|
||||
//positional error (zeroth order error)
|
||||
btScalar depth = -(pivotAInW - pivotBInW).dot(normal); //this is the error projected on the normal
|
||||
btScalar impulse = depth*tau/timeStep * jacDiagABInv - rel_vel * jacDiagABInv;
|
||||
m_appliedImpulse += impulse;
|
||||
btVector3 impulse_vector = normal * impulse;
|
||||
m_rbA.applyImpulse(impulse_vector, pivotAInW - m_rbA.getCenterOfMassPosition());
|
||||
m_rbB.applyImpulse(-impulse_vector, pivotBInW - m_rbB.getCenterOfMassPosition());
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
{
|
||||
///solve angular part
|
||||
|
||||
// get axes in world space
|
||||
btVector3 axisA = getRigidBodyA().getCenterOfMassTransform().getBasis() * m_rbAFrame.getBasis().getColumn(2);
|
||||
btVector3 axisB = getRigidBodyB().getCenterOfMassTransform().getBasis() * m_rbBFrame.getBasis().getColumn(2);
|
||||
|
||||
const btVector3& angVelA = getRigidBodyA().getAngularVelocity();
|
||||
const btVector3& angVelB = getRigidBodyB().getAngularVelocity();
|
||||
|
||||
btVector3 angVelAroundHingeAxisA = axisA * axisA.dot(angVelA);
|
||||
btVector3 angVelAroundHingeAxisB = axisB * axisB.dot(angVelB);
|
||||
|
||||
btVector3 angAorthog = angVelA - angVelAroundHingeAxisA;
|
||||
btVector3 angBorthog = angVelB - angVelAroundHingeAxisB;
|
||||
btVector3 velrelOrthog = angAorthog-angBorthog;
|
||||
{
|
||||
//solve orthogonal angular velocity correction
|
||||
btScalar relaxation = btScalar(1.);
|
||||
btScalar len = velrelOrthog.length();
|
||||
if (len > btScalar(0.00001))
|
||||
{
|
||||
btVector3 normal = velrelOrthog.normalized();
|
||||
btScalar denom = getRigidBodyA().computeAngularImpulseDenominator(normal) +
|
||||
getRigidBodyB().computeAngularImpulseDenominator(normal);
|
||||
// scale for mass and relaxation
|
||||
//todo: expose this 0.9 factor to developer
|
||||
velrelOrthog *= (btScalar(1.)/denom) * m_relaxationFactor;
|
||||
}
|
||||
|
||||
//solve angular positional correction
|
||||
btVector3 angularError = -axisA.cross(axisB) *(btScalar(1.)/timeStep);
|
||||
btScalar len2 = angularError.length();
|
||||
if (len2>btScalar(0.00001))
|
||||
{
|
||||
btVector3 normal2 = angularError.normalized();
|
||||
btScalar denom2 = getRigidBodyA().computeAngularImpulseDenominator(normal2) +
|
||||
getRigidBodyB().computeAngularImpulseDenominator(normal2);
|
||||
angularError *= (btScalar(1.)/denom2) * relaxation;
|
||||
}
|
||||
|
||||
m_rbA.applyTorqueImpulse(-velrelOrthog+angularError);
|
||||
m_rbB.applyTorqueImpulse(velrelOrthog-angularError);
|
||||
|
||||
// solve limit
|
||||
if (m_solveLimit)
|
||||
{
|
||||
btScalar amplitude = ( (angVelB - angVelA).dot( axisA )*m_relaxationFactor + m_correction* (btScalar(1.)/timeStep)*m_biasFactor ) * m_limitSign;
|
||||
|
||||
btScalar impulseMag = amplitude * m_kHinge;
|
||||
|
||||
// Clamp the accumulated impulse
|
||||
btScalar temp = m_accLimitImpulse;
|
||||
m_accLimitImpulse = btMax(m_accLimitImpulse + impulseMag, btScalar(0) );
|
||||
impulseMag = m_accLimitImpulse - temp;
|
||||
|
||||
|
||||
btVector3 impulse = axisA * impulseMag * m_limitSign;
|
||||
m_rbA.applyTorqueImpulse(impulse);
|
||||
m_rbB.applyTorqueImpulse(-impulse);
|
||||
}
|
||||
}
|
||||
|
||||
//apply motor
|
||||
if (m_enableAngularMotor)
|
||||
{
|
||||
//todo: add limits too
|
||||
btVector3 angularLimit(0,0,0);
|
||||
|
||||
btVector3 velrel = angVelAroundHingeAxisA - angVelAroundHingeAxisB;
|
||||
btScalar projRelVel = velrel.dot(axisA);
|
||||
|
||||
btScalar desiredMotorVel = m_motorTargetVelocity;
|
||||
btScalar motor_relvel = desiredMotorVel - projRelVel;
|
||||
|
||||
btScalar unclippedMotorImpulse = m_kHinge * motor_relvel;;
|
||||
//todo: should clip against accumulated impulse
|
||||
btScalar clippedMotorImpulse = unclippedMotorImpulse > m_maxMotorImpulse ? m_maxMotorImpulse : unclippedMotorImpulse;
|
||||
clippedMotorImpulse = clippedMotorImpulse < -m_maxMotorImpulse ? -m_maxMotorImpulse : clippedMotorImpulse;
|
||||
btVector3 motorImp = clippedMotorImpulse * axisA;
|
||||
|
||||
m_rbA.applyTorqueImpulse(motorImp+angularLimit);
|
||||
m_rbB.applyTorqueImpulse(-motorImp-angularLimit);
|
||||
|
||||
}
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
void btHingeConstraint::updateRHS(btScalar timeStep)
|
||||
{
|
||||
(void)timeStep;
|
||||
|
||||
}
|
||||
|
||||
btScalar btHingeConstraint::getHingeAngle()
|
||||
{
|
||||
const btVector3 refAxis0 = getRigidBodyA().getCenterOfMassTransform().getBasis() * m_rbAFrame.getBasis().getColumn(0);
|
||||
const btVector3 refAxis1 = getRigidBodyA().getCenterOfMassTransform().getBasis() * m_rbAFrame.getBasis().getColumn(1);
|
||||
const btVector3 swingAxis = getRigidBodyB().getCenterOfMassTransform().getBasis() * m_rbBFrame.getBasis().getColumn(1);
|
||||
|
||||
return btAtan2Fast( swingAxis.dot(refAxis0), swingAxis.dot(refAxis1) );
|
||||
}
|
||||
130
bullet/src/BulletDynamics/ConstraintSolver/btHingeConstraint.h
Normal file
130
bullet/src/BulletDynamics/ConstraintSolver/btHingeConstraint.h
Normal file
@ -0,0 +1,130 @@
|
||||
/*
|
||||
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.
|
||||
*/
|
||||
|
||||
/* Hinge Constraint by Dirk Gregorius. Limits added by Marcus Hennix at Starbreeze Studios */
|
||||
|
||||
#ifndef HINGECONSTRAINT_H
|
||||
#define HINGECONSTRAINT_H
|
||||
|
||||
#include "LinearMath/btVector3.h"
|
||||
#include "btJacobianEntry.h"
|
||||
#include "btTypedConstraint.h"
|
||||
|
||||
class btRigidBody;
|
||||
|
||||
/// hinge constraint between two rigidbodies each with a pivotpoint that descibes the axis location in local space
|
||||
/// axis defines the orientation of the hinge axis
|
||||
class btHingeConstraint : public btTypedConstraint
|
||||
{
|
||||
#ifdef IN_PARALLELL_SOLVER
|
||||
public:
|
||||
#endif
|
||||
btJacobianEntry m_jac[3]; //3 orthogonal linear constraints
|
||||
btJacobianEntry m_jacAng[3]; //2 orthogonal angular constraints+ 1 for limit/motor
|
||||
|
||||
btTransform m_rbAFrame; // constraint axii. Assumes z is hinge axis.
|
||||
btTransform m_rbBFrame;
|
||||
|
||||
btScalar m_motorTargetVelocity;
|
||||
btScalar m_maxMotorImpulse;
|
||||
|
||||
btScalar m_limitSoftness;
|
||||
btScalar m_biasFactor;
|
||||
btScalar m_relaxationFactor;
|
||||
|
||||
btScalar m_lowerLimit;
|
||||
btScalar m_upperLimit;
|
||||
|
||||
btScalar m_kHinge;
|
||||
|
||||
btScalar m_limitSign;
|
||||
btScalar m_correction;
|
||||
|
||||
btScalar m_accLimitImpulse;
|
||||
|
||||
bool m_angularOnly;
|
||||
bool m_enableAngularMotor;
|
||||
bool m_solveLimit;
|
||||
|
||||
|
||||
public:
|
||||
|
||||
btHingeConstraint(btRigidBody& rbA,btRigidBody& rbB, const btVector3& pivotInA,const btVector3& pivotInB, btVector3& axisInA,btVector3& axisInB);
|
||||
|
||||
btHingeConstraint(btRigidBody& rbA,const btVector3& pivotInA,btVector3& axisInA);
|
||||
|
||||
btHingeConstraint(btRigidBody& rbA,btRigidBody& rbB, const btTransform& rbAFrame, const btTransform& rbBFrame);
|
||||
|
||||
btHingeConstraint(btRigidBody& rbA,const btTransform& rbAFrame);
|
||||
|
||||
btHingeConstraint();
|
||||
|
||||
virtual void buildJacobian();
|
||||
|
||||
virtual void solveConstraint(btScalar timeStep);
|
||||
|
||||
void updateRHS(btScalar timeStep);
|
||||
|
||||
const btRigidBody& getRigidBodyA() const
|
||||
{
|
||||
return m_rbA;
|
||||
}
|
||||
const btRigidBody& getRigidBodyB() const
|
||||
{
|
||||
return m_rbB;
|
||||
}
|
||||
|
||||
void setAngularOnly(bool angularOnly)
|
||||
{
|
||||
m_angularOnly = angularOnly;
|
||||
}
|
||||
|
||||
void enableAngularMotor(bool enableMotor,btScalar targetVelocity,btScalar maxMotorImpulse)
|
||||
{
|
||||
m_enableAngularMotor = enableMotor;
|
||||
m_motorTargetVelocity = targetVelocity;
|
||||
m_maxMotorImpulse = maxMotorImpulse;
|
||||
}
|
||||
|
||||
void setLimit(btScalar low,btScalar high,btScalar _softness = 0.9f, btScalar _biasFactor = 0.3f, btScalar _relaxationFactor = 1.0f)
|
||||
{
|
||||
m_lowerLimit = low;
|
||||
m_upperLimit = high;
|
||||
|
||||
m_limitSoftness = _softness;
|
||||
m_biasFactor = _biasFactor;
|
||||
m_relaxationFactor = _relaxationFactor;
|
||||
|
||||
}
|
||||
|
||||
btScalar getHingeAngle();
|
||||
|
||||
|
||||
const btTransform& getAFrame() { return m_rbAFrame; };
|
||||
const btTransform& getBFrame() { return m_rbBFrame; };
|
||||
|
||||
inline int getSolveLimit()
|
||||
{
|
||||
return m_solveLimit;
|
||||
}
|
||||
|
||||
inline btScalar getLimitSign()
|
||||
{
|
||||
return m_limitSign;
|
||||
}
|
||||
|
||||
};
|
||||
|
||||
#endif //HINGECONSTRAINT_H
|
||||
156
bullet/src/BulletDynamics/ConstraintSolver/btJacobianEntry.h
Normal file
156
bullet/src/BulletDynamics/ConstraintSolver/btJacobianEntry.h
Normal file
@ -0,0 +1,156 @@
|
||||
/*
|
||||
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 JACOBIAN_ENTRY_H
|
||||
#define JACOBIAN_ENTRY_H
|
||||
|
||||
#include "LinearMath/btVector3.h"
|
||||
#include "BulletDynamics/Dynamics/btRigidBody.h"
|
||||
|
||||
|
||||
//notes:
|
||||
// Another memory optimization would be to store m_1MinvJt in the remaining 3 w components
|
||||
// which makes the btJacobianEntry memory layout 16 bytes
|
||||
// if you only are interested in angular part, just feed massInvA and massInvB zero
|
||||
|
||||
/// Jacobian entry is an abstraction that allows to describe constraints
|
||||
/// it can be used in combination with a constraint solver
|
||||
/// Can be used to relate the effect of an impulse to the constraint error
|
||||
class btJacobianEntry
|
||||
{
|
||||
public:
|
||||
btJacobianEntry() {};
|
||||
//constraint between two different rigidbodies
|
||||
btJacobianEntry(
|
||||
const btMatrix3x3& world2A,
|
||||
const btMatrix3x3& world2B,
|
||||
const btVector3& rel_pos1,const btVector3& rel_pos2,
|
||||
const btVector3& jointAxis,
|
||||
const btVector3& inertiaInvA,
|
||||
const btScalar massInvA,
|
||||
const btVector3& inertiaInvB,
|
||||
const btScalar massInvB)
|
||||
:m_linearJointAxis(jointAxis)
|
||||
{
|
||||
m_aJ = world2A*(rel_pos1.cross(m_linearJointAxis));
|
||||
m_bJ = world2B*(rel_pos2.cross(-m_linearJointAxis));
|
||||
m_0MinvJt = inertiaInvA * m_aJ;
|
||||
m_1MinvJt = inertiaInvB * m_bJ;
|
||||
m_Adiag = massInvA + m_0MinvJt.dot(m_aJ) + massInvB + m_1MinvJt.dot(m_bJ);
|
||||
|
||||
btAssert(m_Adiag > btScalar(0.0));
|
||||
}
|
||||
|
||||
//angular constraint between two different rigidbodies
|
||||
btJacobianEntry(const btVector3& jointAxis,
|
||||
const btMatrix3x3& world2A,
|
||||
const btMatrix3x3& world2B,
|
||||
const btVector3& inertiaInvA,
|
||||
const btVector3& inertiaInvB)
|
||||
:m_linearJointAxis(btVector3(btScalar(0.),btScalar(0.),btScalar(0.)))
|
||||
{
|
||||
m_aJ= world2A*jointAxis;
|
||||
m_bJ = world2B*-jointAxis;
|
||||
m_0MinvJt = inertiaInvA * m_aJ;
|
||||
m_1MinvJt = inertiaInvB * m_bJ;
|
||||
m_Adiag = m_0MinvJt.dot(m_aJ) + m_1MinvJt.dot(m_bJ);
|
||||
|
||||
btAssert(m_Adiag > btScalar(0.0));
|
||||
}
|
||||
|
||||
//angular constraint between two different rigidbodies
|
||||
btJacobianEntry(const btVector3& axisInA,
|
||||
const btVector3& axisInB,
|
||||
const btVector3& inertiaInvA,
|
||||
const btVector3& inertiaInvB)
|
||||
: m_linearJointAxis(btVector3(btScalar(0.),btScalar(0.),btScalar(0.)))
|
||||
, m_aJ(axisInA)
|
||||
, m_bJ(-axisInB)
|
||||
{
|
||||
m_0MinvJt = inertiaInvA * m_aJ;
|
||||
m_1MinvJt = inertiaInvB * m_bJ;
|
||||
m_Adiag = m_0MinvJt.dot(m_aJ) + m_1MinvJt.dot(m_bJ);
|
||||
|
||||
btAssert(m_Adiag > btScalar(0.0));
|
||||
}
|
||||
|
||||
//constraint on one rigidbody
|
||||
btJacobianEntry(
|
||||
const btMatrix3x3& world2A,
|
||||
const btVector3& rel_pos1,const btVector3& rel_pos2,
|
||||
const btVector3& jointAxis,
|
||||
const btVector3& inertiaInvA,
|
||||
const btScalar massInvA)
|
||||
:m_linearJointAxis(jointAxis)
|
||||
{
|
||||
m_aJ= world2A*(rel_pos1.cross(jointAxis));
|
||||
m_bJ = world2A*(rel_pos2.cross(-jointAxis));
|
||||
m_0MinvJt = inertiaInvA * m_aJ;
|
||||
m_1MinvJt = btVector3(btScalar(0.),btScalar(0.),btScalar(0.));
|
||||
m_Adiag = massInvA + m_0MinvJt.dot(m_aJ);
|
||||
|
||||
btAssert(m_Adiag > btScalar(0.0));
|
||||
}
|
||||
|
||||
btScalar getDiagonal() const { return m_Adiag; }
|
||||
|
||||
// for two constraints on the same rigidbody (for example vehicle friction)
|
||||
btScalar getNonDiagonal(const btJacobianEntry& jacB, const btScalar massInvA) const
|
||||
{
|
||||
const btJacobianEntry& jacA = *this;
|
||||
btScalar lin = massInvA * jacA.m_linearJointAxis.dot(jacB.m_linearJointAxis);
|
||||
btScalar ang = jacA.m_0MinvJt.dot(jacB.m_aJ);
|
||||
return lin + ang;
|
||||
}
|
||||
|
||||
|
||||
|
||||
// for two constraints on sharing two same rigidbodies (for example two contact points between two rigidbodies)
|
||||
btScalar getNonDiagonal(const btJacobianEntry& jacB,const btScalar massInvA,const btScalar massInvB) const
|
||||
{
|
||||
const btJacobianEntry& jacA = *this;
|
||||
btVector3 lin = jacA.m_linearJointAxis * jacB.m_linearJointAxis;
|
||||
btVector3 ang0 = jacA.m_0MinvJt * jacB.m_aJ;
|
||||
btVector3 ang1 = jacA.m_1MinvJt * jacB.m_bJ;
|
||||
btVector3 lin0 = massInvA * lin ;
|
||||
btVector3 lin1 = massInvB * lin;
|
||||
btVector3 sum = ang0+ang1+lin0+lin1;
|
||||
return sum[0]+sum[1]+sum[2];
|
||||
}
|
||||
|
||||
btScalar getRelativeVelocity(const btVector3& linvelA,const btVector3& angvelA,const btVector3& linvelB,const btVector3& angvelB)
|
||||
{
|
||||
btVector3 linrel = linvelA - linvelB;
|
||||
btVector3 angvela = angvelA * m_aJ;
|
||||
btVector3 angvelb = angvelB * m_bJ;
|
||||
linrel *= m_linearJointAxis;
|
||||
angvela += angvelb;
|
||||
angvela += linrel;
|
||||
btScalar rel_vel2 = angvela[0]+angvela[1]+angvela[2];
|
||||
return rel_vel2 + SIMD_EPSILON;
|
||||
}
|
||||
//private:
|
||||
|
||||
btVector3 m_linearJointAxis;
|
||||
btVector3 m_aJ;
|
||||
btVector3 m_bJ;
|
||||
btVector3 m_0MinvJt;
|
||||
btVector3 m_1MinvJt;
|
||||
//Optimization: can be stored in the w/last component of one of the vectors
|
||||
btScalar m_Adiag;
|
||||
|
||||
};
|
||||
|
||||
#endif //JACOBIAN_ENTRY_H
|
||||
@ -0,0 +1,117 @@
|
||||
/*
|
||||
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.
|
||||
*/
|
||||
|
||||
|
||||
#include "btPoint2PointConstraint.h"
|
||||
#include "BulletDynamics/Dynamics/btRigidBody.h"
|
||||
#include <new>
|
||||
|
||||
|
||||
|
||||
btPoint2PointConstraint::btPoint2PointConstraint()
|
||||
:btTypedConstraint(POINT2POINT_CONSTRAINT_TYPE)
|
||||
{
|
||||
}
|
||||
|
||||
btPoint2PointConstraint::btPoint2PointConstraint(btRigidBody& rbA,btRigidBody& rbB, const btVector3& pivotInA,const btVector3& pivotInB)
|
||||
:btTypedConstraint(POINT2POINT_CONSTRAINT_TYPE,rbA,rbB),m_pivotInA(pivotInA),m_pivotInB(pivotInB)
|
||||
{
|
||||
|
||||
}
|
||||
|
||||
|
||||
btPoint2PointConstraint::btPoint2PointConstraint(btRigidBody& rbA,const btVector3& pivotInA)
|
||||
:btTypedConstraint(POINT2POINT_CONSTRAINT_TYPE,rbA),m_pivotInA(pivotInA),m_pivotInB(rbA.getCenterOfMassTransform()(pivotInA))
|
||||
{
|
||||
|
||||
}
|
||||
|
||||
void btPoint2PointConstraint::buildJacobian()
|
||||
{
|
||||
m_appliedImpulse = btScalar(0.);
|
||||
|
||||
btVector3 normal(0,0,0);
|
||||
|
||||
for (int i=0;i<3;i++)
|
||||
{
|
||||
normal[i] = 1;
|
||||
new (&m_jac[i]) btJacobianEntry(
|
||||
m_rbA.getCenterOfMassTransform().getBasis().transpose(),
|
||||
m_rbB.getCenterOfMassTransform().getBasis().transpose(),
|
||||
m_rbA.getCenterOfMassTransform()*m_pivotInA - m_rbA.getCenterOfMassPosition(),
|
||||
m_rbB.getCenterOfMassTransform()*m_pivotInB - m_rbB.getCenterOfMassPosition(),
|
||||
normal,
|
||||
m_rbA.getInvInertiaDiagLocal(),
|
||||
m_rbA.getInvMass(),
|
||||
m_rbB.getInvInertiaDiagLocal(),
|
||||
m_rbB.getInvMass());
|
||||
normal[i] = 0;
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
void btPoint2PointConstraint::solveConstraint(btScalar timeStep)
|
||||
{
|
||||
btVector3 pivotAInW = m_rbA.getCenterOfMassTransform()*m_pivotInA;
|
||||
btVector3 pivotBInW = m_rbB.getCenterOfMassTransform()*m_pivotInB;
|
||||
|
||||
|
||||
btVector3 normal(0,0,0);
|
||||
|
||||
|
||||
// btVector3 angvelA = m_rbA.getCenterOfMassTransform().getBasis().transpose() * m_rbA.getAngularVelocity();
|
||||
// btVector3 angvelB = m_rbB.getCenterOfMassTransform().getBasis().transpose() * m_rbB.getAngularVelocity();
|
||||
|
||||
for (int i=0;i<3;i++)
|
||||
{
|
||||
normal[i] = 1;
|
||||
btScalar jacDiagABInv = btScalar(1.) / m_jac[i].getDiagonal();
|
||||
|
||||
btVector3 rel_pos1 = pivotAInW - m_rbA.getCenterOfMassPosition();
|
||||
btVector3 rel_pos2 = pivotBInW - m_rbB.getCenterOfMassPosition();
|
||||
//this jacobian entry could be re-used for all iterations
|
||||
|
||||
btVector3 vel1 = m_rbA.getVelocityInLocalPoint(rel_pos1);
|
||||
btVector3 vel2 = m_rbB.getVelocityInLocalPoint(rel_pos2);
|
||||
btVector3 vel = vel1 - vel2;
|
||||
|
||||
btScalar rel_vel;
|
||||
rel_vel = normal.dot(vel);
|
||||
|
||||
/*
|
||||
//velocity error (first order error)
|
||||
btScalar rel_vel = m_jac[i].getRelativeVelocity(m_rbA.getLinearVelocity(),angvelA,
|
||||
m_rbB.getLinearVelocity(),angvelB);
|
||||
*/
|
||||
|
||||
//positional error (zeroth order error)
|
||||
btScalar depth = -(pivotAInW - pivotBInW).dot(normal); //this is the error projected on the normal
|
||||
|
||||
btScalar impulse = depth*m_setting.m_tau/timeStep * jacDiagABInv - m_setting.m_damping * rel_vel * jacDiagABInv;
|
||||
m_appliedImpulse+=impulse;
|
||||
btVector3 impulse_vector = normal * impulse;
|
||||
m_rbA.applyImpulse(impulse_vector, pivotAInW - m_rbA.getCenterOfMassPosition());
|
||||
m_rbB.applyImpulse(-impulse_vector, pivotBInW - m_rbB.getCenterOfMassPosition());
|
||||
|
||||
normal[i] = 0;
|
||||
}
|
||||
}
|
||||
|
||||
void btPoint2PointConstraint::updateRHS(btScalar timeStep)
|
||||
{
|
||||
(void)timeStep;
|
||||
|
||||
}
|
||||
|
||||
@ -0,0 +1,80 @@
|
||||
/*
|
||||
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 POINT2POINTCONSTRAINT_H
|
||||
#define POINT2POINTCONSTRAINT_H
|
||||
|
||||
#include "LinearMath/btVector3.h"
|
||||
#include "btJacobianEntry.h"
|
||||
#include "btTypedConstraint.h"
|
||||
|
||||
class btRigidBody;
|
||||
|
||||
struct btConstraintSetting
|
||||
{
|
||||
btConstraintSetting() :
|
||||
m_tau(btScalar(0.3)),
|
||||
m_damping(btScalar(1.))
|
||||
{
|
||||
}
|
||||
btScalar m_tau;
|
||||
btScalar m_damping;
|
||||
};
|
||||
|
||||
/// point to point constraint between two rigidbodies each with a pivotpoint that descibes the 'ballsocket' location in local space
|
||||
class btPoint2PointConstraint : public btTypedConstraint
|
||||
{
|
||||
#ifdef IN_PARALLELL_SOLVER
|
||||
public:
|
||||
#endif
|
||||
btJacobianEntry m_jac[3]; //3 orthogonal linear constraints
|
||||
|
||||
btVector3 m_pivotInA;
|
||||
btVector3 m_pivotInB;
|
||||
|
||||
|
||||
|
||||
public:
|
||||
|
||||
btConstraintSetting m_setting;
|
||||
|
||||
btPoint2PointConstraint(btRigidBody& rbA,btRigidBody& rbB, const btVector3& pivotInA,const btVector3& pivotInB);
|
||||
|
||||
btPoint2PointConstraint(btRigidBody& rbA,const btVector3& pivotInA);
|
||||
|
||||
btPoint2PointConstraint();
|
||||
|
||||
virtual void buildJacobian();
|
||||
|
||||
|
||||
virtual void solveConstraint(btScalar timeStep);
|
||||
|
||||
void updateRHS(btScalar timeStep);
|
||||
|
||||
void setPivotA(const btVector3& pivotA)
|
||||
{
|
||||
m_pivotInA = pivotA;
|
||||
}
|
||||
|
||||
void setPivotB(const btVector3& pivotB)
|
||||
{
|
||||
m_pivotInB = pivotB;
|
||||
}
|
||||
|
||||
|
||||
|
||||
};
|
||||
|
||||
#endif //POINT2POINTCONSTRAINT_H
|
||||
File diff suppressed because it is too large
Load Diff
@ -0,0 +1,114 @@
|
||||
/*
|
||||
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 SEQUENTIAL_IMPULSE_CONSTRAINT_SOLVER_H
|
||||
#define SEQUENTIAL_IMPULSE_CONSTRAINT_SOLVER_H
|
||||
|
||||
#include "btConstraintSolver.h"
|
||||
class btIDebugDraw;
|
||||
#include "btContactConstraint.h"
|
||||
|
||||
|
||||
|
||||
/// btSequentialImpulseConstraintSolver uses a Propagation Method and Sequentially applies impulses
|
||||
/// The approach is the 3D version of Erin Catto's GDC 2006 tutorial. See http://www.gphysics.com
|
||||
/// Although Sequential Impulse is more intuitive, it is mathematically equivalent to Projected Successive Overrelaxation (iterative LCP)
|
||||
/// Applies impulses for combined restitution and penetration recovery and to simulate friction
|
||||
class btSequentialImpulseConstraintSolver : public btConstraintSolver
|
||||
{
|
||||
|
||||
protected:
|
||||
btScalar solve(btRigidBody* body0,btRigidBody* body1, btManifoldPoint& cp, const btContactSolverInfo& info,int iter,btIDebugDraw* debugDrawer);
|
||||
btScalar solveFriction(btRigidBody* body0,btRigidBody* body1, btManifoldPoint& cp, const btContactSolverInfo& info,int iter,btIDebugDraw* debugDrawer);
|
||||
void prepareConstraints(btPersistentManifold* manifoldPtr, const btContactSolverInfo& info,btIDebugDraw* debugDrawer);
|
||||
|
||||
ContactSolverFunc m_contactDispatch[MAX_CONTACT_SOLVER_TYPES][MAX_CONTACT_SOLVER_TYPES];
|
||||
ContactSolverFunc m_frictionDispatch[MAX_CONTACT_SOLVER_TYPES][MAX_CONTACT_SOLVER_TYPES];
|
||||
|
||||
//choose between several modes, different friction model etc.
|
||||
int m_solverMode;
|
||||
///m_btSeed2 is used for re-arranging the constraint rows. improves convergence/quality of friction
|
||||
unsigned long m_btSeed2;
|
||||
|
||||
public:
|
||||
|
||||
enum eSolverMode
|
||||
{
|
||||
SOLVER_RANDMIZE_ORDER = 1,
|
||||
SOLVER_FRICTION_SEPARATE = 2,
|
||||
SOLVER_USE_WARMSTARTING = 4,
|
||||
SOLVER_CACHE_FRIENDLY = 8
|
||||
};
|
||||
|
||||
btSequentialImpulseConstraintSolver();
|
||||
|
||||
///Advanced: Override the default contact solving function for contacts, for certain types of rigidbody
|
||||
///See btRigidBody::m_contactSolverType and btRigidBody::m_frictionSolverType
|
||||
void setContactSolverFunc(ContactSolverFunc func,int type0,int type1)
|
||||
{
|
||||
m_contactDispatch[type0][type1] = func;
|
||||
}
|
||||
|
||||
///Advanced: Override the default friction solving function for contacts, for certain types of rigidbody
|
||||
///See btRigidBody::m_contactSolverType and btRigidBody::m_frictionSolverType
|
||||
void SetFrictionSolverFunc(ContactSolverFunc func,int type0,int type1)
|
||||
{
|
||||
m_frictionDispatch[type0][type1] = func;
|
||||
}
|
||||
|
||||
virtual ~btSequentialImpulseConstraintSolver() {}
|
||||
|
||||
virtual btScalar solveGroup(btCollisionObject** bodies,int numBodies,btPersistentManifold** manifold,int numManifolds,btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& info, btIDebugDraw* debugDrawer, btStackAlloc* stackAlloc);
|
||||
|
||||
virtual btScalar solveGroupCacheFriendly(btCollisionObject** bodies,int numBodies,btPersistentManifold** manifoldPtr, int numManifolds,btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& infoGlobal,btIDebugDraw* debugDrawer,btStackAlloc* stackAlloc);
|
||||
|
||||
///clear internal cached data and reset random seed
|
||||
virtual void reset();
|
||||
|
||||
btScalar solveCombinedContactFriction(btRigidBody* body0,btRigidBody* body1, btManifoldPoint& cp, const btContactSolverInfo& info,int iter,btIDebugDraw* debugDrawer);
|
||||
|
||||
|
||||
void setSolverMode(int mode)
|
||||
{
|
||||
m_solverMode = mode;
|
||||
}
|
||||
|
||||
int getSolverMode() const
|
||||
{
|
||||
return m_solverMode;
|
||||
}
|
||||
|
||||
unsigned long btRand2();
|
||||
|
||||
int btRandInt2 (int n);
|
||||
|
||||
void setRandSeed(unsigned long seed)
|
||||
{
|
||||
m_btSeed2 = seed;
|
||||
}
|
||||
unsigned long getRandSeed() const
|
||||
{
|
||||
return m_btSeed2;
|
||||
}
|
||||
|
||||
};
|
||||
|
||||
#ifndef BT_PREFER_SIMD
|
||||
typedef btSequentialImpulseConstraintSolver btSequentialImpulseConstraintSolverPrefered;
|
||||
#endif
|
||||
|
||||
|
||||
#endif //SEQUENTIAL_IMPULSE_CONSTRAINT_SOLVER_H
|
||||
|
||||
@ -0,0 +1,255 @@
|
||||
/*
|
||||
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.
|
||||
*/
|
||||
|
||||
|
||||
|
||||
#include "btSolve2LinearConstraint.h"
|
||||
|
||||
#include "BulletDynamics/Dynamics/btRigidBody.h"
|
||||
#include "LinearMath/btVector3.h"
|
||||
#include "btJacobianEntry.h"
|
||||
|
||||
|
||||
void btSolve2LinearConstraint::resolveUnilateralPairConstraint(
|
||||
btRigidBody* body1,
|
||||
btRigidBody* body2,
|
||||
|
||||
const btMatrix3x3& world2A,
|
||||
const btMatrix3x3& world2B,
|
||||
|
||||
const btVector3& invInertiaADiag,
|
||||
const btScalar invMassA,
|
||||
const btVector3& linvelA,const btVector3& angvelA,
|
||||
const btVector3& rel_posA1,
|
||||
const btVector3& invInertiaBDiag,
|
||||
const btScalar invMassB,
|
||||
const btVector3& linvelB,const btVector3& angvelB,
|
||||
const btVector3& rel_posA2,
|
||||
|
||||
btScalar depthA, const btVector3& normalA,
|
||||
const btVector3& rel_posB1,const btVector3& rel_posB2,
|
||||
btScalar depthB, const btVector3& normalB,
|
||||
btScalar& imp0,btScalar& imp1)
|
||||
{
|
||||
(void)linvelA;
|
||||
(void)linvelB;
|
||||
(void)angvelB;
|
||||
(void)angvelA;
|
||||
|
||||
|
||||
|
||||
imp0 = btScalar(0.);
|
||||
imp1 = btScalar(0.);
|
||||
|
||||
btScalar len = btFabs(normalA.length()) - btScalar(1.);
|
||||
if (btFabs(len) >= SIMD_EPSILON)
|
||||
return;
|
||||
|
||||
btAssert(len < SIMD_EPSILON);
|
||||
|
||||
|
||||
//this jacobian entry could be re-used for all iterations
|
||||
btJacobianEntry jacA(world2A,world2B,rel_posA1,rel_posA2,normalA,invInertiaADiag,invMassA,
|
||||
invInertiaBDiag,invMassB);
|
||||
btJacobianEntry jacB(world2A,world2B,rel_posB1,rel_posB2,normalB,invInertiaADiag,invMassA,
|
||||
invInertiaBDiag,invMassB);
|
||||
|
||||
//const btScalar vel0 = jacA.getRelativeVelocity(linvelA,angvelA,linvelB,angvelB);
|
||||
//const btScalar vel1 = jacB.getRelativeVelocity(linvelA,angvelA,linvelB,angvelB);
|
||||
|
||||
const btScalar vel0 = normalA.dot(body1->getVelocityInLocalPoint(rel_posA1)-body2->getVelocityInLocalPoint(rel_posA1));
|
||||
const btScalar vel1 = normalB.dot(body1->getVelocityInLocalPoint(rel_posB1)-body2->getVelocityInLocalPoint(rel_posB1));
|
||||
|
||||
// btScalar penetrationImpulse = (depth*contactTau*timeCorrection) * massTerm;//jacDiagABInv
|
||||
btScalar massTerm = btScalar(1.) / (invMassA + invMassB);
|
||||
|
||||
|
||||
// calculate rhs (or error) terms
|
||||
const btScalar dv0 = depthA * m_tau * massTerm - vel0 * m_damping;
|
||||
const btScalar dv1 = depthB * m_tau * massTerm - vel1 * m_damping;
|
||||
|
||||
|
||||
// dC/dv * dv = -C
|
||||
|
||||
// jacobian * impulse = -error
|
||||
//
|
||||
|
||||
//impulse = jacobianInverse * -error
|
||||
|
||||
// inverting 2x2 symmetric system (offdiagonal are equal!)
|
||||
//
|
||||
|
||||
|
||||
btScalar nonDiag = jacA.getNonDiagonal(jacB,invMassA,invMassB);
|
||||
btScalar invDet = btScalar(1.0) / (jacA.getDiagonal() * jacB.getDiagonal() - nonDiag * nonDiag );
|
||||
|
||||
//imp0 = dv0 * jacA.getDiagonal() * invDet + dv1 * -nonDiag * invDet;
|
||||
//imp1 = dv1 * jacB.getDiagonal() * invDet + dv0 * - nonDiag * invDet;
|
||||
|
||||
imp0 = dv0 * jacA.getDiagonal() * invDet + dv1 * -nonDiag * invDet;
|
||||
imp1 = dv1 * jacB.getDiagonal() * invDet + dv0 * - nonDiag * invDet;
|
||||
|
||||
//[a b] [d -c]
|
||||
//[c d] inverse = (1 / determinant) * [-b a] where determinant is (ad - bc)
|
||||
|
||||
//[jA nD] * [imp0] = [dv0]
|
||||
//[nD jB] [imp1] [dv1]
|
||||
|
||||
}
|
||||
|
||||
|
||||
|
||||
void btSolve2LinearConstraint::resolveBilateralPairConstraint(
|
||||
btRigidBody* body1,
|
||||
btRigidBody* body2,
|
||||
const btMatrix3x3& world2A,
|
||||
const btMatrix3x3& world2B,
|
||||
|
||||
const btVector3& invInertiaADiag,
|
||||
const btScalar invMassA,
|
||||
const btVector3& linvelA,const btVector3& angvelA,
|
||||
const btVector3& rel_posA1,
|
||||
const btVector3& invInertiaBDiag,
|
||||
const btScalar invMassB,
|
||||
const btVector3& linvelB,const btVector3& angvelB,
|
||||
const btVector3& rel_posA2,
|
||||
|
||||
btScalar depthA, const btVector3& normalA,
|
||||
const btVector3& rel_posB1,const btVector3& rel_posB2,
|
||||
btScalar depthB, const btVector3& normalB,
|
||||
btScalar& imp0,btScalar& imp1)
|
||||
{
|
||||
|
||||
(void)linvelA;
|
||||
(void)linvelB;
|
||||
(void)angvelA;
|
||||
(void)angvelB;
|
||||
|
||||
|
||||
|
||||
imp0 = btScalar(0.);
|
||||
imp1 = btScalar(0.);
|
||||
|
||||
btScalar len = btFabs(normalA.length()) - btScalar(1.);
|
||||
if (btFabs(len) >= SIMD_EPSILON)
|
||||
return;
|
||||
|
||||
btAssert(len < SIMD_EPSILON);
|
||||
|
||||
|
||||
//this jacobian entry could be re-used for all iterations
|
||||
btJacobianEntry jacA(world2A,world2B,rel_posA1,rel_posA2,normalA,invInertiaADiag,invMassA,
|
||||
invInertiaBDiag,invMassB);
|
||||
btJacobianEntry jacB(world2A,world2B,rel_posB1,rel_posB2,normalB,invInertiaADiag,invMassA,
|
||||
invInertiaBDiag,invMassB);
|
||||
|
||||
//const btScalar vel0 = jacA.getRelativeVelocity(linvelA,angvelA,linvelB,angvelB);
|
||||
//const btScalar vel1 = jacB.getRelativeVelocity(linvelA,angvelA,linvelB,angvelB);
|
||||
|
||||
const btScalar vel0 = normalA.dot(body1->getVelocityInLocalPoint(rel_posA1)-body2->getVelocityInLocalPoint(rel_posA1));
|
||||
const btScalar vel1 = normalB.dot(body1->getVelocityInLocalPoint(rel_posB1)-body2->getVelocityInLocalPoint(rel_posB1));
|
||||
|
||||
// calculate rhs (or error) terms
|
||||
const btScalar dv0 = depthA * m_tau - vel0 * m_damping;
|
||||
const btScalar dv1 = depthB * m_tau - vel1 * m_damping;
|
||||
|
||||
// dC/dv * dv = -C
|
||||
|
||||
// jacobian * impulse = -error
|
||||
//
|
||||
|
||||
//impulse = jacobianInverse * -error
|
||||
|
||||
// inverting 2x2 symmetric system (offdiagonal are equal!)
|
||||
//
|
||||
|
||||
|
||||
btScalar nonDiag = jacA.getNonDiagonal(jacB,invMassA,invMassB);
|
||||
btScalar invDet = btScalar(1.0) / (jacA.getDiagonal() * jacB.getDiagonal() - nonDiag * nonDiag );
|
||||
|
||||
//imp0 = dv0 * jacA.getDiagonal() * invDet + dv1 * -nonDiag * invDet;
|
||||
//imp1 = dv1 * jacB.getDiagonal() * invDet + dv0 * - nonDiag * invDet;
|
||||
|
||||
imp0 = dv0 * jacA.getDiagonal() * invDet + dv1 * -nonDiag * invDet;
|
||||
imp1 = dv1 * jacB.getDiagonal() * invDet + dv0 * - nonDiag * invDet;
|
||||
|
||||
//[a b] [d -c]
|
||||
//[c d] inverse = (1 / determinant) * [-b a] where determinant is (ad - bc)
|
||||
|
||||
//[jA nD] * [imp0] = [dv0]
|
||||
//[nD jB] [imp1] [dv1]
|
||||
|
||||
if ( imp0 > btScalar(0.0))
|
||||
{
|
||||
if ( imp1 > btScalar(0.0) )
|
||||
{
|
||||
//both positive
|
||||
}
|
||||
else
|
||||
{
|
||||
imp1 = btScalar(0.);
|
||||
|
||||
// now imp0>0 imp1<0
|
||||
imp0 = dv0 / jacA.getDiagonal();
|
||||
if ( imp0 > btScalar(0.0) )
|
||||
{
|
||||
} else
|
||||
{
|
||||
imp0 = btScalar(0.);
|
||||
}
|
||||
}
|
||||
}
|
||||
else
|
||||
{
|
||||
imp0 = btScalar(0.);
|
||||
|
||||
imp1 = dv1 / jacB.getDiagonal();
|
||||
if ( imp1 <= btScalar(0.0) )
|
||||
{
|
||||
imp1 = btScalar(0.);
|
||||
// now imp0>0 imp1<0
|
||||
imp0 = dv0 / jacA.getDiagonal();
|
||||
if ( imp0 > btScalar(0.0) )
|
||||
{
|
||||
} else
|
||||
{
|
||||
imp0 = btScalar(0.);
|
||||
}
|
||||
} else
|
||||
{
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
/*
|
||||
void btSolve2LinearConstraint::resolveAngularConstraint( const btMatrix3x3& invInertiaAWS,
|
||||
const btScalar invMassA,
|
||||
const btVector3& linvelA,const btVector3& angvelA,
|
||||
const btVector3& rel_posA1,
|
||||
const btMatrix3x3& invInertiaBWS,
|
||||
const btScalar invMassB,
|
||||
const btVector3& linvelB,const btVector3& angvelB,
|
||||
const btVector3& rel_posA2,
|
||||
|
||||
btScalar depthA, const btVector3& normalA,
|
||||
const btVector3& rel_posB1,const btVector3& rel_posB2,
|
||||
btScalar depthB, const btVector3& normalB,
|
||||
btScalar& imp0,btScalar& imp1)
|
||||
{
|
||||
|
||||
}
|
||||
*/
|
||||
|
||||
@ -0,0 +1,107 @@
|
||||
/*
|
||||
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 SOLVE_2LINEAR_CONSTRAINT_H
|
||||
#define SOLVE_2LINEAR_CONSTRAINT_H
|
||||
|
||||
#include "LinearMath/btMatrix3x3.h"
|
||||
#include "LinearMath/btVector3.h"
|
||||
|
||||
|
||||
class btRigidBody;
|
||||
|
||||
|
||||
|
||||
/// constraint class used for lateral tyre friction.
|
||||
class btSolve2LinearConstraint
|
||||
{
|
||||
btScalar m_tau;
|
||||
btScalar m_damping;
|
||||
|
||||
public:
|
||||
|
||||
btSolve2LinearConstraint(btScalar tau,btScalar damping)
|
||||
{
|
||||
m_tau = tau;
|
||||
m_damping = damping;
|
||||
}
|
||||
//
|
||||
// solve unilateral constraint (equality, direct method)
|
||||
//
|
||||
void resolveUnilateralPairConstraint(
|
||||
btRigidBody* body0,
|
||||
btRigidBody* body1,
|
||||
|
||||
const btMatrix3x3& world2A,
|
||||
const btMatrix3x3& world2B,
|
||||
|
||||
const btVector3& invInertiaADiag,
|
||||
const btScalar invMassA,
|
||||
const btVector3& linvelA,const btVector3& angvelA,
|
||||
const btVector3& rel_posA1,
|
||||
const btVector3& invInertiaBDiag,
|
||||
const btScalar invMassB,
|
||||
const btVector3& linvelB,const btVector3& angvelB,
|
||||
const btVector3& rel_posA2,
|
||||
|
||||
btScalar depthA, const btVector3& normalA,
|
||||
const btVector3& rel_posB1,const btVector3& rel_posB2,
|
||||
btScalar depthB, const btVector3& normalB,
|
||||
btScalar& imp0,btScalar& imp1);
|
||||
|
||||
|
||||
//
|
||||
// solving 2x2 lcp problem (inequality, direct solution )
|
||||
//
|
||||
void resolveBilateralPairConstraint(
|
||||
btRigidBody* body0,
|
||||
btRigidBody* body1,
|
||||
const btMatrix3x3& world2A,
|
||||
const btMatrix3x3& world2B,
|
||||
|
||||
const btVector3& invInertiaADiag,
|
||||
const btScalar invMassA,
|
||||
const btVector3& linvelA,const btVector3& angvelA,
|
||||
const btVector3& rel_posA1,
|
||||
const btVector3& invInertiaBDiag,
|
||||
const btScalar invMassB,
|
||||
const btVector3& linvelB,const btVector3& angvelB,
|
||||
const btVector3& rel_posA2,
|
||||
|
||||
btScalar depthA, const btVector3& normalA,
|
||||
const btVector3& rel_posB1,const btVector3& rel_posB2,
|
||||
btScalar depthB, const btVector3& normalB,
|
||||
btScalar& imp0,btScalar& imp1);
|
||||
|
||||
/*
|
||||
void resolveAngularConstraint( const btMatrix3x3& invInertiaAWS,
|
||||
const btScalar invMassA,
|
||||
const btVector3& linvelA,const btVector3& angvelA,
|
||||
const btVector3& rel_posA1,
|
||||
const btMatrix3x3& invInertiaBWS,
|
||||
const btScalar invMassB,
|
||||
const btVector3& linvelB,const btVector3& angvelB,
|
||||
const btVector3& rel_posA2,
|
||||
|
||||
btScalar depthA, const btVector3& normalA,
|
||||
const btVector3& rel_posB1,const btVector3& rel_posB2,
|
||||
btScalar depthB, const btVector3& normalB,
|
||||
btScalar& imp0,btScalar& imp1);
|
||||
|
||||
*/
|
||||
|
||||
};
|
||||
|
||||
#endif //SOLVE_2LINEAR_CONSTRAINT_H
|
||||
71
bullet/src/BulletDynamics/ConstraintSolver/btSolverBody.h
Normal file
71
bullet/src/BulletDynamics/ConstraintSolver/btSolverBody.h
Normal file
@ -0,0 +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 BT_SOLVER_BODY_H
|
||||
#define BT_SOLVER_BODY_H
|
||||
|
||||
class btRigidBody;
|
||||
#include "LinearMath/btVector3.h"
|
||||
#include "LinearMath/btMatrix3x3.h"
|
||||
|
||||
|
||||
|
||||
|
||||
ATTRIBUTE_ALIGNED16 (struct) btSolverBody
|
||||
{
|
||||
btVector3 m_centerOfMassPosition;
|
||||
btVector3 m_linearVelocity;
|
||||
btVector3 m_angularVelocity;
|
||||
btRigidBody* m_originalBody;
|
||||
float m_invMass;
|
||||
float m_friction;
|
||||
float m_angularFactor;
|
||||
|
||||
inline void getVelocityInLocalPoint(const btVector3& rel_pos, btVector3& velocity ) const
|
||||
{
|
||||
velocity = m_linearVelocity + m_angularVelocity.cross(rel_pos);
|
||||
}
|
||||
|
||||
//Optimization for the iterative solver: avoid calculating constant terms involving inertia, normal, relative position
|
||||
inline void internalApplyImpulse(const btVector3& linearComponent, const btVector3& angularComponent,btScalar impulseMagnitude)
|
||||
{
|
||||
m_linearVelocity += linearComponent*impulseMagnitude;
|
||||
m_angularVelocity += angularComponent*impulseMagnitude*m_angularFactor;
|
||||
}
|
||||
|
||||
void writebackVelocity()
|
||||
{
|
||||
if (m_invMass)
|
||||
{
|
||||
m_originalBody->setLinearVelocity(m_linearVelocity);
|
||||
m_originalBody->setAngularVelocity(m_angularVelocity);
|
||||
}
|
||||
}
|
||||
|
||||
void readVelocity()
|
||||
{
|
||||
if (m_invMass)
|
||||
{
|
||||
m_linearVelocity = m_originalBody->getLinearVelocity();
|
||||
m_angularVelocity = m_originalBody->getAngularVelocity();
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
|
||||
|
||||
};
|
||||
|
||||
#endif //BT_SOLVER_BODY_H
|
||||
@ -0,0 +1,63 @@
|
||||
|
||||
|
||||
/*
|
||||
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 BT_SOLVER_CONSTRAINT_H
|
||||
#define BT_SOLVER_CONSTRAINT_H
|
||||
|
||||
class btRigidBody;
|
||||
#include "LinearMath/btVector3.h"
|
||||
#include "LinearMath/btMatrix3x3.h"
|
||||
|
||||
//#define NO_FRICTION_TANGENTIALS 1
|
||||
|
||||
///1D constraint along a normal axis between bodyA and bodyB. It can be combined to solve contact and friction constraints.
|
||||
ATTRIBUTE_ALIGNED16 (struct) btSolverConstraint
|
||||
{
|
||||
btVector3 m_relpos1CrossNormal;
|
||||
btVector3 m_relpos2CrossNormal;
|
||||
btVector3 m_contactNormal;
|
||||
btVector3 m_angularComponentA;
|
||||
btVector3 m_angularComponentB;
|
||||
|
||||
btScalar m_appliedVelocityImpulse;
|
||||
int m_solverBodyIdA;
|
||||
int m_solverBodyIdB;
|
||||
btScalar m_friction;
|
||||
btScalar m_restitution;
|
||||
btScalar m_jacDiagABInv;
|
||||
btScalar m_penetration;
|
||||
btScalar m_appliedImpulse;
|
||||
|
||||
int m_constraintType;
|
||||
int m_frictionIndex;
|
||||
int m_unusedPadding[2];
|
||||
|
||||
enum btSolverConstraintType
|
||||
{
|
||||
BT_SOLVER_CONTACT_1D = 0,
|
||||
BT_SOLVER_FRICTION_1D
|
||||
};
|
||||
};
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
#endif //BT_SOLVER_CONSTRAINT_H
|
||||
|
||||
|
||||
@ -0,0 +1,56 @@
|
||||
/*
|
||||
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.
|
||||
*/
|
||||
|
||||
|
||||
#include "btTypedConstraint.h"
|
||||
#include "BulletDynamics/Dynamics/btRigidBody.h"
|
||||
|
||||
static btRigidBody s_fixed(0, 0,0);
|
||||
|
||||
btTypedConstraint::btTypedConstraint(btTypedConstraintType type)
|
||||
: m_constraintType (type),
|
||||
m_userConstraintType(-1),
|
||||
m_userConstraintId(-1),
|
||||
m_rbA(s_fixed),
|
||||
m_rbB(s_fixed),
|
||||
m_appliedImpulse(btScalar(0.))
|
||||
{
|
||||
s_fixed.setMassProps(btScalar(0.),btVector3(btScalar(0.),btScalar(0.),btScalar(0.)));
|
||||
}
|
||||
btTypedConstraint::btTypedConstraint(btTypedConstraintType type, btRigidBody& rbA)
|
||||
: m_constraintType (type),
|
||||
m_userConstraintType(-1),
|
||||
m_userConstraintId(-1),
|
||||
m_rbA(rbA),
|
||||
m_rbB(s_fixed),
|
||||
m_appliedImpulse(btScalar(0.))
|
||||
{
|
||||
s_fixed.setMassProps(btScalar(0.),btVector3(btScalar(0.),btScalar(0.),btScalar(0.)));
|
||||
|
||||
}
|
||||
|
||||
|
||||
btTypedConstraint::btTypedConstraint(btTypedConstraintType type, btRigidBody& rbA,btRigidBody& rbB)
|
||||
: m_constraintType (type),
|
||||
m_userConstraintType(-1),
|
||||
m_userConstraintId(-1),
|
||||
m_rbA(rbA),
|
||||
m_rbB(rbB),
|
||||
m_appliedImpulse(btScalar(0.))
|
||||
{
|
||||
s_fixed.setMassProps(btScalar(0.),btVector3(btScalar(0.),btScalar(0.),btScalar(0.)));
|
||||
|
||||
}
|
||||
|
||||
112
bullet/src/BulletDynamics/ConstraintSolver/btTypedConstraint.h
Normal file
112
bullet/src/BulletDynamics/ConstraintSolver/btTypedConstraint.h
Normal file
@ -0,0 +1,112 @@
|
||||
/*
|
||||
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 TYPED_CONSTRAINT_H
|
||||
#define TYPED_CONSTRAINT_H
|
||||
|
||||
class btRigidBody;
|
||||
#include "LinearMath/btScalar.h"
|
||||
|
||||
enum btTypedConstraintType
|
||||
{
|
||||
POINT2POINT_CONSTRAINT_TYPE,
|
||||
HINGE_CONSTRAINT_TYPE,
|
||||
CONETWIST_CONSTRAINT_TYPE,
|
||||
D6_CONSTRAINT_TYPE,
|
||||
VEHICLE_CONSTRAINT_TYPE
|
||||
};
|
||||
|
||||
///TypedConstraint is the baseclass for Bullet constraints and vehicles
|
||||
class btTypedConstraint
|
||||
{
|
||||
int m_userConstraintType;
|
||||
int m_userConstraintId;
|
||||
|
||||
btTypedConstraintType m_constraintType;
|
||||
|
||||
btTypedConstraint& operator=(btTypedConstraint& other)
|
||||
{
|
||||
btAssert(0);
|
||||
(void) other;
|
||||
return *this;
|
||||
}
|
||||
|
||||
protected:
|
||||
btRigidBody& m_rbA;
|
||||
btRigidBody& m_rbB;
|
||||
btScalar m_appliedImpulse;
|
||||
|
||||
|
||||
public:
|
||||
|
||||
btTypedConstraint(btTypedConstraintType type);
|
||||
virtual ~btTypedConstraint() {};
|
||||
btTypedConstraint(btTypedConstraintType type, btRigidBody& rbA);
|
||||
|
||||
btTypedConstraint(btTypedConstraintType type, btRigidBody& rbA,btRigidBody& rbB);
|
||||
|
||||
virtual void buildJacobian() = 0;
|
||||
|
||||
virtual void solveConstraint(btScalar timeStep) = 0;
|
||||
|
||||
const btRigidBody& getRigidBodyA() const
|
||||
{
|
||||
return m_rbA;
|
||||
}
|
||||
const btRigidBody& getRigidBodyB() const
|
||||
{
|
||||
return m_rbB;
|
||||
}
|
||||
|
||||
btRigidBody& getRigidBodyA()
|
||||
{
|
||||
return m_rbA;
|
||||
}
|
||||
btRigidBody& getRigidBodyB()
|
||||
{
|
||||
return m_rbB;
|
||||
}
|
||||
|
||||
int getUserConstraintType() const
|
||||
{
|
||||
return m_userConstraintType ;
|
||||
}
|
||||
|
||||
void setUserConstraintType(int userConstraintType)
|
||||
{
|
||||
m_userConstraintType = userConstraintType;
|
||||
};
|
||||
|
||||
void setUserConstraintId(int uid)
|
||||
{
|
||||
m_userConstraintId = uid;
|
||||
}
|
||||
|
||||
int getUserConstraintId() const
|
||||
{
|
||||
return m_userConstraintId;
|
||||
}
|
||||
btScalar getAppliedImpulse() const
|
||||
{
|
||||
return m_appliedImpulse;
|
||||
}
|
||||
|
||||
btTypedConstraintType getConstraintType () const
|
||||
{
|
||||
return m_constraintType;
|
||||
}
|
||||
};
|
||||
|
||||
#endif //TYPED_CONSTRAINT_H
|
||||
960
bullet/src/BulletDynamics/Dynamics/btDiscreteDynamicsWorld.cpp
Normal file
960
bullet/src/BulletDynamics/Dynamics/btDiscreteDynamicsWorld.cpp
Normal file
@ -0,0 +1,960 @@
|
||||
/*
|
||||
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.
|
||||
*/
|
||||
|
||||
|
||||
#include "btDiscreteDynamicsWorld.h"
|
||||
|
||||
//collision detection
|
||||
#include "BulletCollision/CollisionDispatch/btCollisionDispatcher.h"
|
||||
#include "BulletCollision/BroadphaseCollision/btSimpleBroadphase.h"
|
||||
#include "BulletCollision/CollisionShapes/btCollisionShape.h"
|
||||
#include "BulletCollision/CollisionDispatch/btSimulationIslandManager.h"
|
||||
#include <LinearMath/btTransformUtil.h>
|
||||
|
||||
//rigidbody & constraints
|
||||
#include "BulletDynamics/Dynamics/btRigidBody.h"
|
||||
#include "BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolver.h"
|
||||
#include "BulletDynamics/ConstraintSolver/btContactSolverInfo.h"
|
||||
#include "BulletDynamics/ConstraintSolver/btTypedConstraint.h"
|
||||
|
||||
//for debug rendering
|
||||
#include "BulletCollision/CollisionShapes/btBoxShape.h"
|
||||
#include "BulletCollision/CollisionShapes/btCapsuleShape.h"
|
||||
#include "BulletCollision/CollisionShapes/btCompoundShape.h"
|
||||
#include "BulletCollision/CollisionShapes/btConeShape.h"
|
||||
#include "BulletCollision/CollisionShapes/btConvexTriangleMeshShape.h"
|
||||
#include "BulletCollision/CollisionShapes/btCylinderShape.h"
|
||||
#include "BulletCollision/CollisionShapes/btMultiSphereShape.h"
|
||||
#include "BulletCollision/CollisionShapes/btPolyhedralConvexShape.h"
|
||||
#include "BulletCollision/CollisionShapes/btSphereShape.h"
|
||||
#include "BulletCollision/CollisionShapes/btTriangleCallback.h"
|
||||
#include "BulletCollision/CollisionShapes/btTriangleMeshShape.h"
|
||||
#include "LinearMath/btIDebugDraw.h"
|
||||
|
||||
|
||||
|
||||
//vehicle
|
||||
#include "BulletDynamics/Vehicle/btRaycastVehicle.h"
|
||||
#include "BulletDynamics/Vehicle/btVehicleRaycaster.h"
|
||||
#include "BulletDynamics/Vehicle/btWheelInfo.h"
|
||||
#include "LinearMath/btIDebugDraw.h"
|
||||
#include "LinearMath/btQuickprof.h"
|
||||
#include "LinearMath/btMotionState.h"
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
btDiscreteDynamicsWorld::btDiscreteDynamicsWorld(btDispatcher* dispatcher,btBroadphaseInterface* pairCache,btConstraintSolver* constraintSolver)
|
||||
:btDynamicsWorld(dispatcher,pairCache),
|
||||
m_constraintSolver(constraintSolver? constraintSolver: new btSequentialImpulseConstraintSolver),
|
||||
m_debugDrawer(0),
|
||||
m_gravity(0,-10,0),
|
||||
m_localTime(btScalar(1.)/btScalar(60.)),
|
||||
m_profileTimings(0)
|
||||
{
|
||||
m_islandManager = new btSimulationIslandManager();
|
||||
m_ownsIslandManager = true;
|
||||
m_ownsConstraintSolver = (constraintSolver==0);
|
||||
}
|
||||
|
||||
|
||||
btDiscreteDynamicsWorld::~btDiscreteDynamicsWorld()
|
||||
{
|
||||
//only delete it when we created it
|
||||
if (m_ownsIslandManager)
|
||||
delete m_islandManager;
|
||||
if (m_ownsConstraintSolver)
|
||||
delete m_constraintSolver;
|
||||
}
|
||||
|
||||
void btDiscreteDynamicsWorld::saveKinematicState(btScalar timeStep)
|
||||
{
|
||||
|
||||
for (int i=0;i<m_collisionObjects.size();i++)
|
||||
{
|
||||
btCollisionObject* colObj = m_collisionObjects[i];
|
||||
btRigidBody* body = btRigidBody::upcast(colObj);
|
||||
if (body)
|
||||
{
|
||||
btTransform predictedTrans;
|
||||
if (body->getActivationState() != ISLAND_SLEEPING)
|
||||
{
|
||||
if (body->isKinematicObject())
|
||||
{
|
||||
//to calculate velocities next frame
|
||||
body->saveKinematicState(timeStep);
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
void btDiscreteDynamicsWorld::synchronizeMotionStates()
|
||||
{
|
||||
//debug vehicle wheels
|
||||
|
||||
|
||||
{
|
||||
//todo: iterate over awake simulation islands!
|
||||
for ( int i=0;i<m_collisionObjects.size();i++)
|
||||
{
|
||||
btCollisionObject* colObj = m_collisionObjects[i];
|
||||
if (getDebugDrawer() && getDebugDrawer()->getDebugMode() & btIDebugDraw::DBG_DrawWireframe)
|
||||
{
|
||||
btVector3 color(btScalar(255.),btScalar(255.),btScalar(255.));
|
||||
switch(colObj->getActivationState())
|
||||
{
|
||||
case ACTIVE_TAG:
|
||||
color = btVector3(btScalar(255.),btScalar(255.),btScalar(255.)); break;
|
||||
case ISLAND_SLEEPING:
|
||||
color = btVector3(btScalar(0.),btScalar(255.),btScalar(0.));break;
|
||||
case WANTS_DEACTIVATION:
|
||||
color = btVector3(btScalar(0.),btScalar(255.),btScalar(255.));break;
|
||||
case DISABLE_DEACTIVATION:
|
||||
color = btVector3(btScalar(255.),btScalar(0.),btScalar(0.));break;
|
||||
case DISABLE_SIMULATION:
|
||||
color = btVector3(btScalar(255.),btScalar(255.),btScalar(0.));break;
|
||||
default:
|
||||
{
|
||||
color = btVector3(btScalar(255.),btScalar(0.),btScalar(0.));
|
||||
}
|
||||
};
|
||||
|
||||
debugDrawObject(colObj->getWorldTransform(),colObj->getCollisionShape(),color);
|
||||
}
|
||||
btRigidBody* body = btRigidBody::upcast(colObj);
|
||||
if (body && body->getMotionState() && !body->isStaticOrKinematicObject())
|
||||
{
|
||||
//we need to call the update at least once, even for sleeping objects
|
||||
//otherwise the 'graphics' transform never updates properly
|
||||
//so todo: add 'dirty' flag
|
||||
//if (body->getActivationState() != ISLAND_SLEEPING)
|
||||
{
|
||||
btTransform interpolatedTransform;
|
||||
btTransformUtil::integrateTransform(body->getInterpolationWorldTransform(),
|
||||
body->getInterpolationLinearVelocity(),body->getInterpolationAngularVelocity(),m_localTime,interpolatedTransform);
|
||||
body->getMotionState()->setWorldTransform(interpolatedTransform);
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
if (getDebugDrawer() && getDebugDrawer()->getDebugMode() & btIDebugDraw::DBG_DrawWireframe)
|
||||
{
|
||||
for ( int i=0;i<this->m_vehicles.size();i++)
|
||||
{
|
||||
for (int v=0;v<m_vehicles[i]->getNumWheels();v++)
|
||||
{
|
||||
btVector3 wheelColor(0,255,255);
|
||||
if (m_vehicles[i]->getWheelInfo(v).m_raycastInfo.m_isInContact)
|
||||
{
|
||||
wheelColor.setValue(0,0,255);
|
||||
} else
|
||||
{
|
||||
wheelColor.setValue(255,0,255);
|
||||
}
|
||||
|
||||
//synchronize the wheels with the (interpolated) chassis worldtransform
|
||||
m_vehicles[i]->updateWheelTransform(v,true);
|
||||
|
||||
btVector3 wheelPosWS = m_vehicles[i]->getWheelInfo(v).m_worldTransform.getOrigin();
|
||||
|
||||
btVector3 axle = btVector3(
|
||||
m_vehicles[i]->getWheelInfo(v).m_worldTransform.getBasis()[0][m_vehicles[i]->getRightAxis()],
|
||||
m_vehicles[i]->getWheelInfo(v).m_worldTransform.getBasis()[1][m_vehicles[i]->getRightAxis()],
|
||||
m_vehicles[i]->getWheelInfo(v).m_worldTransform.getBasis()[2][m_vehicles[i]->getRightAxis()]);
|
||||
|
||||
|
||||
//m_vehicles[i]->getWheelInfo(v).m_raycastInfo.m_wheelAxleWS
|
||||
//debug wheels (cylinders)
|
||||
m_debugDrawer->drawLine(wheelPosWS,wheelPosWS+axle,wheelColor);
|
||||
m_debugDrawer->drawLine(wheelPosWS,m_vehicles[i]->getWheelInfo(v).m_raycastInfo.m_contactPointWS,wheelColor);
|
||||
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
|
||||
int btDiscreteDynamicsWorld::stepSimulation( btScalar timeStep,int maxSubSteps, btScalar fixedTimeStep)
|
||||
{
|
||||
int numSimulationSubSteps = 0;
|
||||
|
||||
if (maxSubSteps)
|
||||
{
|
||||
//fixed timestep with interpolation
|
||||
m_localTime += timeStep;
|
||||
if (m_localTime >= fixedTimeStep)
|
||||
{
|
||||
numSimulationSubSteps = int( m_localTime / fixedTimeStep);
|
||||
m_localTime -= numSimulationSubSteps * fixedTimeStep;
|
||||
}
|
||||
} else
|
||||
{
|
||||
//variable timestep
|
||||
fixedTimeStep = timeStep;
|
||||
m_localTime = timeStep;
|
||||
if (btFuzzyZero(timeStep))
|
||||
{
|
||||
numSimulationSubSteps = 0;
|
||||
maxSubSteps = 0;
|
||||
} else
|
||||
{
|
||||
numSimulationSubSteps = 1;
|
||||
maxSubSteps = 1;
|
||||
}
|
||||
}
|
||||
|
||||
//process some debugging flags
|
||||
if (getDebugDrawer())
|
||||
{
|
||||
gDisableDeactivation = (getDebugDrawer()->getDebugMode() & btIDebugDraw::DBG_NoDeactivation) != 0;
|
||||
}
|
||||
if (numSimulationSubSteps)
|
||||
{
|
||||
|
||||
saveKinematicState(fixedTimeStep);
|
||||
|
||||
//clamp the number of substeps, to prevent simulation grinding spiralling down to a halt
|
||||
int clampedSimulationSteps = (numSimulationSubSteps > maxSubSteps)? maxSubSteps : numSimulationSubSteps;
|
||||
|
||||
for (int i=0;i<clampedSimulationSteps;i++)
|
||||
{
|
||||
internalSingleStepSimulation(fixedTimeStep);
|
||||
synchronizeMotionStates();
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
synchronizeMotionStates();
|
||||
|
||||
return numSimulationSubSteps;
|
||||
}
|
||||
|
||||
void btDiscreteDynamicsWorld::internalSingleStepSimulation(btScalar timeStep)
|
||||
{
|
||||
|
||||
startProfiling(timeStep);
|
||||
|
||||
///update aabbs information
|
||||
updateAabbs();
|
||||
|
||||
///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();
|
||||
|
||||
///integrate transforms
|
||||
integrateTransforms(timeStep);
|
||||
|
||||
///update vehicle simulation
|
||||
updateVehicles(timeStep);
|
||||
|
||||
|
||||
updateActivationState( timeStep );
|
||||
|
||||
|
||||
|
||||
}
|
||||
|
||||
void btDiscreteDynamicsWorld::setGravity(const btVector3& gravity)
|
||||
{
|
||||
m_gravity = gravity;
|
||||
for ( int i=0;i<m_collisionObjects.size();i++)
|
||||
{
|
||||
btCollisionObject* colObj = m_collisionObjects[i];
|
||||
btRigidBody* body = btRigidBody::upcast(colObj);
|
||||
if (body)
|
||||
{
|
||||
body->setGravity(gravity);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
void btDiscreteDynamicsWorld::removeRigidBody(btRigidBody* body)
|
||||
{
|
||||
removeCollisionObject(body);
|
||||
}
|
||||
|
||||
void btDiscreteDynamicsWorld::addRigidBody(btRigidBody* body)
|
||||
{
|
||||
if (!body->isStaticOrKinematicObject())
|
||||
{
|
||||
body->setGravity(m_gravity);
|
||||
}
|
||||
|
||||
if (body->getCollisionShape())
|
||||
{
|
||||
bool isDynamic = !(body->isStaticObject() || body->isKinematicObject());
|
||||
short collisionFilterGroup = isDynamic? short(btBroadphaseProxy::DefaultFilter) : short(btBroadphaseProxy::StaticFilter);
|
||||
short collisionFilterMask = isDynamic? short(btBroadphaseProxy::AllFilter) : short(btBroadphaseProxy::AllFilter ^ btBroadphaseProxy::StaticFilter);
|
||||
|
||||
addCollisionObject(body,collisionFilterGroup,collisionFilterMask);
|
||||
}
|
||||
}
|
||||
|
||||
void btDiscreteDynamicsWorld::addRigidBody(btRigidBody* body, short group, short mask)
|
||||
{
|
||||
if (!body->isStaticOrKinematicObject())
|
||||
{
|
||||
body->setGravity(m_gravity);
|
||||
}
|
||||
|
||||
if (body->getCollisionShape())
|
||||
{
|
||||
addCollisionObject(body,group,mask);
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
void btDiscreteDynamicsWorld::updateVehicles(btScalar timeStep)
|
||||
{
|
||||
BEGIN_PROFILE("updateVehicles");
|
||||
|
||||
for ( int i=0;i<m_vehicles.size();i++)
|
||||
{
|
||||
btRaycastVehicle* vehicle = m_vehicles[i];
|
||||
vehicle->updateVehicle( timeStep);
|
||||
}
|
||||
END_PROFILE("updateVehicles");
|
||||
}
|
||||
|
||||
void btDiscreteDynamicsWorld::updateActivationState(btScalar timeStep)
|
||||
{
|
||||
BEGIN_PROFILE("updateActivationState");
|
||||
|
||||
for ( int i=0;i<m_collisionObjects.size();i++)
|
||||
{
|
||||
btCollisionObject* colObj = m_collisionObjects[i];
|
||||
btRigidBody* body = btRigidBody::upcast(colObj);
|
||||
if (body)
|
||||
{
|
||||
body->updateDeactivation(timeStep);
|
||||
|
||||
if (body->wantsSleeping())
|
||||
{
|
||||
if (body->isStaticOrKinematicObject())
|
||||
{
|
||||
body->setActivationState(ISLAND_SLEEPING);
|
||||
} else
|
||||
{
|
||||
if (body->getActivationState() == ACTIVE_TAG)
|
||||
body->setActivationState( WANTS_DEACTIVATION );
|
||||
}
|
||||
} else
|
||||
{
|
||||
if (body->getActivationState() != DISABLE_DEACTIVATION)
|
||||
body->setActivationState( ACTIVE_TAG );
|
||||
}
|
||||
}
|
||||
}
|
||||
END_PROFILE("updateActivationState");
|
||||
}
|
||||
|
||||
void btDiscreteDynamicsWorld::addConstraint(btTypedConstraint* constraint,bool disableCollisionsBetweenLinkedBodies)
|
||||
{
|
||||
m_constraints.push_back(constraint);
|
||||
if (disableCollisionsBetweenLinkedBodies)
|
||||
{
|
||||
constraint->getRigidBodyA().addConstraintRef(constraint);
|
||||
constraint->getRigidBodyB().addConstraintRef(constraint);
|
||||
}
|
||||
}
|
||||
|
||||
void btDiscreteDynamicsWorld::removeConstraint(btTypedConstraint* constraint)
|
||||
{
|
||||
m_constraints.remove(constraint);
|
||||
constraint->getRigidBodyA().removeConstraintRef(constraint);
|
||||
constraint->getRigidBodyB().removeConstraintRef(constraint);
|
||||
}
|
||||
|
||||
void btDiscreteDynamicsWorld::addVehicle(btRaycastVehicle* vehicle)
|
||||
{
|
||||
m_vehicles.push_back(vehicle);
|
||||
}
|
||||
|
||||
void btDiscreteDynamicsWorld::removeVehicle(btRaycastVehicle* vehicle)
|
||||
{
|
||||
m_vehicles.remove(vehicle);
|
||||
}
|
||||
|
||||
inline int btGetConstraintIslandId(const btTypedConstraint* lhs)
|
||||
{
|
||||
int islandId;
|
||||
|
||||
const btCollisionObject& rcolObj0 = lhs->getRigidBodyA();
|
||||
const btCollisionObject& rcolObj1 = lhs->getRigidBodyB();
|
||||
islandId= rcolObj0.getIslandTag()>=0?rcolObj0.getIslandTag():rcolObj1.getIslandTag();
|
||||
return islandId;
|
||||
|
||||
}
|
||||
|
||||
|
||||
class btSortConstraintOnIslandPredicate
|
||||
{
|
||||
public:
|
||||
|
||||
bool operator() ( const btTypedConstraint* lhs, const btTypedConstraint* rhs )
|
||||
{
|
||||
int rIslandId0,lIslandId0;
|
||||
rIslandId0 = btGetConstraintIslandId(rhs);
|
||||
lIslandId0 = btGetConstraintIslandId(lhs);
|
||||
return lIslandId0 < rIslandId0;
|
||||
}
|
||||
};
|
||||
|
||||
|
||||
|
||||
|
||||
void btDiscreteDynamicsWorld::solveConstraints(btContactSolverInfo& solverInfo)
|
||||
{
|
||||
|
||||
struct InplaceSolverIslandCallback : public btSimulationIslandManager::IslandCallback
|
||||
{
|
||||
|
||||
btContactSolverInfo& m_solverInfo;
|
||||
btConstraintSolver* m_solver;
|
||||
btTypedConstraint** m_sortedConstraints;
|
||||
int m_numConstraints;
|
||||
btIDebugDraw* m_debugDrawer;
|
||||
btStackAlloc* m_stackAlloc;
|
||||
|
||||
|
||||
InplaceSolverIslandCallback(
|
||||
btContactSolverInfo& solverInfo,
|
||||
btConstraintSolver* solver,
|
||||
btTypedConstraint** sortedConstraints,
|
||||
int numConstraints,
|
||||
btIDebugDraw* debugDrawer,
|
||||
btStackAlloc* stackAlloc)
|
||||
:m_solverInfo(solverInfo),
|
||||
m_solver(solver),
|
||||
m_sortedConstraints(sortedConstraints),
|
||||
m_numConstraints(numConstraints),
|
||||
m_debugDrawer(debugDrawer),
|
||||
m_stackAlloc(stackAlloc)
|
||||
{
|
||||
|
||||
}
|
||||
|
||||
InplaceSolverIslandCallback& operator=(InplaceSolverIslandCallback& other)
|
||||
{
|
||||
btAssert(0);
|
||||
(void)other;
|
||||
return *this;
|
||||
}
|
||||
virtual void ProcessIsland(btCollisionObject** bodies,int numBodies,btPersistentManifold** manifolds,int numManifolds, int islandId)
|
||||
{
|
||||
//also add all non-contact constraints/joints for this island
|
||||
btTypedConstraint** startConstraint = 0;
|
||||
int numCurConstraints = 0;
|
||||
int i;
|
||||
|
||||
//find the first constraint for this island
|
||||
for (i=0;i<m_numConstraints;i++)
|
||||
{
|
||||
if (btGetConstraintIslandId(m_sortedConstraints[i]) == islandId)
|
||||
{
|
||||
startConstraint = &m_sortedConstraints[i];
|
||||
break;
|
||||
}
|
||||
}
|
||||
//count the number of constraints in this island
|
||||
for (;i<m_numConstraints;i++)
|
||||
{
|
||||
if (btGetConstraintIslandId(m_sortedConstraints[i]) == islandId)
|
||||
{
|
||||
numCurConstraints++;
|
||||
}
|
||||
}
|
||||
|
||||
m_solver->solveGroup( bodies,numBodies,manifolds, numManifolds,startConstraint,numCurConstraints,m_solverInfo,m_debugDrawer,m_stackAlloc);
|
||||
}
|
||||
|
||||
};
|
||||
|
||||
//sorted version of all btTypedConstraint, based on islandId
|
||||
btAlignedObjectArray<btTypedConstraint*> sortedConstraints;
|
||||
sortedConstraints.resize( m_constraints.size());
|
||||
int i;
|
||||
for (i=0;i<getNumConstraints();i++)
|
||||
{
|
||||
sortedConstraints[i] = m_constraints[i];
|
||||
}
|
||||
|
||||
// assert(0);
|
||||
|
||||
|
||||
|
||||
sortedConstraints.heapSort(btSortConstraintOnIslandPredicate());
|
||||
|
||||
btTypedConstraint** constraintsPtr = getNumConstraints() ? &sortedConstraints[0] : 0;
|
||||
|
||||
InplaceSolverIslandCallback solverCallback( solverInfo, m_constraintSolver, constraintsPtr,sortedConstraints.size(), m_debugDrawer,m_stackAlloc);
|
||||
|
||||
m_constraintSolver->prepareSolve(getCollisionWorld()->getNumCollisionObjects(), getCollisionWorld()->getDispatcher()->getNumManifolds());
|
||||
|
||||
/// solve all the constraints for this island
|
||||
m_islandManager->buildAndProcessIslands(getCollisionWorld()->getDispatcher(),getCollisionWorld()->getCollisionObjectArray(),&solverCallback);
|
||||
|
||||
m_constraintSolver->allSolved(solverInfo, m_debugDrawer, m_stackAlloc);
|
||||
}
|
||||
|
||||
|
||||
|
||||
|
||||
void btDiscreteDynamicsWorld::calculateSimulationIslands()
|
||||
{
|
||||
BEGIN_PROFILE("calculateSimulationIslands");
|
||||
|
||||
getSimulationIslandManager()->updateActivationState(getCollisionWorld(),getCollisionWorld()->getDispatcher());
|
||||
|
||||
{
|
||||
int i;
|
||||
int numConstraints = int(m_constraints.size());
|
||||
for (i=0;i< numConstraints ; i++ )
|
||||
{
|
||||
btTypedConstraint* constraint = m_constraints[i];
|
||||
|
||||
const btRigidBody* colObj0 = &constraint->getRigidBodyA();
|
||||
const btRigidBody* colObj1 = &constraint->getRigidBodyB();
|
||||
|
||||
if (((colObj0) && ((colObj0)->mergesSimulationIslands())) &&
|
||||
((colObj1) && ((colObj1)->mergesSimulationIslands())))
|
||||
{
|
||||
if (colObj0->isActive() || colObj1->isActive())
|
||||
{
|
||||
|
||||
getSimulationIslandManager()->getUnionFind().unite((colObj0)->getIslandTag(),
|
||||
(colObj1)->getIslandTag());
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
//Store the island id in each body
|
||||
getSimulationIslandManager()->storeIslandActivationState(getCollisionWorld());
|
||||
|
||||
END_PROFILE("calculateSimulationIslands");
|
||||
|
||||
}
|
||||
|
||||
|
||||
void btDiscreteDynamicsWorld::updateAabbs()
|
||||
{
|
||||
BEGIN_PROFILE("updateAabbs");
|
||||
|
||||
btVector3 colorvec(1,0,0);
|
||||
btTransform predictedTrans;
|
||||
for ( int i=0;i<m_collisionObjects.size();i++)
|
||||
{
|
||||
btCollisionObject* colObj = m_collisionObjects[i];
|
||||
|
||||
btRigidBody* body = btRigidBody::upcast(colObj);
|
||||
if (body)
|
||||
{
|
||||
// if (body->IsActive() && (!body->IsStatic()))
|
||||
{
|
||||
btPoint3 minAabb,maxAabb;
|
||||
colObj->getCollisionShape()->getAabb(colObj->getWorldTransform(), minAabb,maxAabb);
|
||||
btBroadphaseInterface* bp = (btBroadphaseInterface*)m_broadphasePairCache;
|
||||
|
||||
//moving objects should be moderately sized, probably something wrong if not
|
||||
if ( colObj->isStaticObject() || ((maxAabb-minAabb).length2() < btScalar(1e12)))
|
||||
{
|
||||
bp->setAabb(body->getBroadphaseHandle(),minAabb,maxAabb);
|
||||
} else
|
||||
{
|
||||
//something went wrong, investigate
|
||||
//this assert is unwanted in 3D modelers (danger of loosing work)
|
||||
body->setActivationState(DISABLE_SIMULATION);
|
||||
|
||||
static bool reportMe = true;
|
||||
if (reportMe && m_debugDrawer)
|
||||
{
|
||||
reportMe = false;
|
||||
m_debugDrawer->reportErrorWarning("Overflow in AABB, object removed from simulation");
|
||||
m_debugDrawer->reportErrorWarning("If you can reproduce this, please email bugs@continuousphysics.com\n");
|
||||
m_debugDrawer->reportErrorWarning("Please include above information, your Platform, version of OS.\n");
|
||||
m_debugDrawer->reportErrorWarning("Thanks.\n");
|
||||
}
|
||||
|
||||
|
||||
}
|
||||
if (m_debugDrawer && (m_debugDrawer->getDebugMode() & btIDebugDraw::DBG_DrawAabb))
|
||||
{
|
||||
m_debugDrawer->drawAabb(minAabb,maxAabb,colorvec);
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
END_PROFILE("updateAabbs");
|
||||
}
|
||||
|
||||
void btDiscreteDynamicsWorld::integrateTransforms(btScalar timeStep)
|
||||
{
|
||||
BEGIN_PROFILE("integrateTransforms");
|
||||
btTransform predictedTrans;
|
||||
for ( int i=0;i<m_collisionObjects.size();i++)
|
||||
{
|
||||
btCollisionObject* colObj = m_collisionObjects[i];
|
||||
btRigidBody* body = btRigidBody::upcast(colObj);
|
||||
if (body)
|
||||
{
|
||||
if (body->isActive() && (!body->isStaticOrKinematicObject()))
|
||||
{
|
||||
body->predictIntegratedTransform(timeStep, predictedTrans);
|
||||
body->proceedToTransform( predictedTrans);
|
||||
}
|
||||
}
|
||||
}
|
||||
END_PROFILE("integrateTransforms");
|
||||
}
|
||||
|
||||
|
||||
|
||||
void btDiscreteDynamicsWorld::predictUnconstraintMotion(btScalar timeStep)
|
||||
{
|
||||
BEGIN_PROFILE("predictUnconstraintMotion");
|
||||
for ( int i=0;i<m_collisionObjects.size();i++)
|
||||
{
|
||||
btCollisionObject* colObj = m_collisionObjects[i];
|
||||
btRigidBody* body = btRigidBody::upcast(colObj);
|
||||
if (body)
|
||||
{
|
||||
if (!body->isStaticOrKinematicObject())
|
||||
{
|
||||
if (body->isActive())
|
||||
{
|
||||
body->applyForces( timeStep);
|
||||
body->integrateVelocities( timeStep);
|
||||
body->predictIntegratedTransform(timeStep,body->getInterpolationWorldTransform());
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
END_PROFILE("predictUnconstraintMotion");
|
||||
}
|
||||
|
||||
|
||||
void btDiscreteDynamicsWorld::startProfiling(btScalar timeStep)
|
||||
{
|
||||
(void)timeStep;
|
||||
#ifdef USE_QUICKPROF
|
||||
|
||||
|
||||
//toggle btProfiler
|
||||
if ( m_debugDrawer && m_debugDrawer->getDebugMode() & btIDebugDraw::DBG_ProfileTimings)
|
||||
{
|
||||
if (!m_profileTimings)
|
||||
{
|
||||
m_profileTimings = 1;
|
||||
// To disable profiling, simply comment out the following line.
|
||||
static int counter = 0;
|
||||
|
||||
char filename[128];
|
||||
sprintf(filename,"quickprof_bullet_timings%i.csv",counter++);
|
||||
btProfiler::init(filename, btProfiler::BLOCK_CYCLE_SECONDS);//BLOCK_TOTAL_MICROSECONDS
|
||||
} else
|
||||
{
|
||||
btProfiler::endProfilingCycle();
|
||||
}
|
||||
|
||||
} else
|
||||
{
|
||||
if (m_profileTimings)
|
||||
{
|
||||
btProfiler::endProfilingCycle();
|
||||
|
||||
m_profileTimings = 0;
|
||||
btProfiler::destroy();
|
||||
}
|
||||
}
|
||||
#endif //USE_QUICKPROF
|
||||
}
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
class DebugDrawcallback : public btTriangleCallback, public btInternalTriangleIndexCallback
|
||||
{
|
||||
btIDebugDraw* m_debugDrawer;
|
||||
btVector3 m_color;
|
||||
btTransform m_worldTrans;
|
||||
|
||||
public:
|
||||
|
||||
DebugDrawcallback(btIDebugDraw* debugDrawer,const btTransform& worldTrans,const btVector3& color) :
|
||||
m_debugDrawer(debugDrawer),
|
||||
m_color(color),
|
||||
m_worldTrans(worldTrans)
|
||||
{
|
||||
}
|
||||
|
||||
virtual void internalProcessTriangleIndex(btVector3* triangle,int partId,int triangleIndex)
|
||||
{
|
||||
processTriangle(triangle,partId,triangleIndex);
|
||||
}
|
||||
|
||||
virtual void processTriangle(btVector3* triangle,int partId, int triangleIndex)
|
||||
{
|
||||
(void)partId;
|
||||
(void)triangleIndex;
|
||||
|
||||
btVector3 wv0,wv1,wv2;
|
||||
wv0 = m_worldTrans*triangle[0];
|
||||
wv1 = m_worldTrans*triangle[1];
|
||||
wv2 = m_worldTrans*triangle[2];
|
||||
m_debugDrawer->drawLine(wv0,wv1,m_color);
|
||||
m_debugDrawer->drawLine(wv1,wv2,m_color);
|
||||
m_debugDrawer->drawLine(wv2,wv0,m_color);
|
||||
}
|
||||
};
|
||||
|
||||
void btDiscreteDynamicsWorld::debugDrawSphere(btScalar radius, const btTransform& transform, const btVector3& color)
|
||||
{
|
||||
btVector3 start = transform.getOrigin();
|
||||
|
||||
const btVector3 xoffs = transform.getBasis() * btVector3(radius,0,0);
|
||||
const btVector3 yoffs = transform.getBasis() * btVector3(0,radius,0);
|
||||
const btVector3 zoffs = transform.getBasis() * btVector3(0,0,radius);
|
||||
|
||||
// XY
|
||||
getDebugDrawer()->drawLine(start-xoffs, start+yoffs, color);
|
||||
getDebugDrawer()->drawLine(start+yoffs, start+xoffs, color);
|
||||
getDebugDrawer()->drawLine(start+xoffs, start-yoffs, color);
|
||||
getDebugDrawer()->drawLine(start-yoffs, start-xoffs, color);
|
||||
|
||||
// XZ
|
||||
getDebugDrawer()->drawLine(start-xoffs, start+zoffs, color);
|
||||
getDebugDrawer()->drawLine(start+zoffs, start+xoffs, color);
|
||||
getDebugDrawer()->drawLine(start+xoffs, start-zoffs, color);
|
||||
getDebugDrawer()->drawLine(start-zoffs, start-xoffs, color);
|
||||
|
||||
// YZ
|
||||
getDebugDrawer()->drawLine(start-yoffs, start+zoffs, color);
|
||||
getDebugDrawer()->drawLine(start+zoffs, start+yoffs, color);
|
||||
getDebugDrawer()->drawLine(start+yoffs, start-zoffs, color);
|
||||
getDebugDrawer()->drawLine(start-zoffs, start-yoffs, color);
|
||||
}
|
||||
|
||||
void btDiscreteDynamicsWorld::debugDrawObject(const btTransform& worldTransform, const btCollisionShape* shape, const btVector3& color)
|
||||
{
|
||||
// Draw a small simplex at the center of the object
|
||||
{
|
||||
btVector3 start = worldTransform.getOrigin();
|
||||
getDebugDrawer()->drawLine(start, start+worldTransform.getBasis() * btVector3(1,0,0), btVector3(1,0,0));
|
||||
getDebugDrawer()->drawLine(start, start+worldTransform.getBasis() * btVector3(0,1,0), btVector3(0,1,0));
|
||||
getDebugDrawer()->drawLine(start, start+worldTransform.getBasis() * btVector3(0,0,1), btVector3(0,0,1));
|
||||
}
|
||||
|
||||
if (shape->getShapeType() == COMPOUND_SHAPE_PROXYTYPE)
|
||||
{
|
||||
const btCompoundShape* compoundShape = static_cast<const btCompoundShape*>(shape);
|
||||
for (int i=compoundShape->getNumChildShapes()-1;i>=0;i--)
|
||||
{
|
||||
btTransform childTrans = compoundShape->getChildTransform(i);
|
||||
const btCollisionShape* colShape = compoundShape->getChildShape(i);
|
||||
debugDrawObject(worldTransform*childTrans,colShape,color);
|
||||
}
|
||||
|
||||
} else
|
||||
{
|
||||
switch (shape->getShapeType())
|
||||
{
|
||||
|
||||
case SPHERE_SHAPE_PROXYTYPE:
|
||||
{
|
||||
const btSphereShape* sphereShape = static_cast<const btSphereShape*>(shape);
|
||||
btScalar radius = sphereShape->getMargin();//radius doesn't include the margin, so draw with margin
|
||||
|
||||
debugDrawSphere(radius, worldTransform, color);
|
||||
break;
|
||||
}
|
||||
case MULTI_SPHERE_SHAPE_PROXYTYPE:
|
||||
{
|
||||
const btMultiSphereShape* multiSphereShape = static_cast<const btMultiSphereShape*>(shape);
|
||||
|
||||
for (int i = multiSphereShape->getSphereCount()-1; i>=0;i--)
|
||||
{
|
||||
btTransform childTransform = worldTransform;
|
||||
childTransform.getOrigin() += multiSphereShape->getSpherePosition(i);
|
||||
debugDrawSphere(multiSphereShape->getSphereRadius(i), childTransform, color);
|
||||
}
|
||||
|
||||
break;
|
||||
}
|
||||
case CAPSULE_SHAPE_PROXYTYPE:
|
||||
{
|
||||
const btCapsuleShape* capsuleShape = static_cast<const btCapsuleShape*>(shape);
|
||||
|
||||
btScalar radius = capsuleShape->getRadius();
|
||||
btScalar halfHeight = capsuleShape->getHalfHeight();
|
||||
|
||||
// Draw the ends
|
||||
{
|
||||
btTransform childTransform = worldTransform;
|
||||
childTransform.getOrigin() = worldTransform * btVector3(0,halfHeight,0);
|
||||
debugDrawSphere(radius, childTransform, color);
|
||||
}
|
||||
|
||||
{
|
||||
btTransform childTransform = worldTransform;
|
||||
childTransform.getOrigin() = worldTransform * btVector3(0,-halfHeight,0);
|
||||
debugDrawSphere(radius, childTransform, color);
|
||||
}
|
||||
|
||||
// Draw some additional lines
|
||||
btVector3 start = worldTransform.getOrigin();
|
||||
getDebugDrawer()->drawLine(start+worldTransform.getBasis() * btVector3(-radius,halfHeight,0),start+worldTransform.getBasis() * btVector3(-radius,-halfHeight,0), color);
|
||||
getDebugDrawer()->drawLine(start+worldTransform.getBasis() * btVector3(radius,halfHeight,0),start+worldTransform.getBasis() * btVector3(radius,-halfHeight,0), color);
|
||||
getDebugDrawer()->drawLine(start+worldTransform.getBasis() * btVector3(0,halfHeight,-radius),start+worldTransform.getBasis() * btVector3(0,-halfHeight,-radius), color);
|
||||
getDebugDrawer()->drawLine(start+worldTransform.getBasis() * btVector3(0,halfHeight,radius),start+worldTransform.getBasis() * btVector3(0,-halfHeight,radius), color);
|
||||
|
||||
break;
|
||||
}
|
||||
case CONE_SHAPE_PROXYTYPE:
|
||||
{
|
||||
const btConeShape* coneShape = static_cast<const btConeShape*>(shape);
|
||||
btScalar radius = coneShape->getRadius();//+coneShape->getMargin();
|
||||
btScalar height = coneShape->getHeight();//+coneShape->getMargin();
|
||||
btVector3 start = worldTransform.getOrigin();
|
||||
getDebugDrawer()->drawLine(start+worldTransform.getBasis() * btVector3(btScalar(0.),btScalar(0.),btScalar(0.5)*height),start+worldTransform.getBasis() * btVector3(radius,btScalar(0.),btScalar(-0.5)*height),color);
|
||||
getDebugDrawer()->drawLine(start+worldTransform.getBasis() * btVector3(btScalar(0.),btScalar(0.),btScalar(0.5)*height),start+worldTransform.getBasis() * btVector3(-radius,btScalar(0.),btScalar(-0.5)*height),color);
|
||||
getDebugDrawer()->drawLine(start+worldTransform.getBasis() * btVector3(btScalar(0.),btScalar(0.),btScalar(0.5)*height),start+worldTransform.getBasis() * btVector3(btScalar(0.),radius,btScalar(-0.5)*height),color);
|
||||
getDebugDrawer()->drawLine(start+worldTransform.getBasis() * btVector3(btScalar(0.),btScalar(0.),btScalar(0.5)*height),start+worldTransform.getBasis() * btVector3(btScalar(0.),-radius,btScalar(-0.5)*height),color);
|
||||
break;
|
||||
|
||||
}
|
||||
case CYLINDER_SHAPE_PROXYTYPE:
|
||||
{
|
||||
const btCylinderShape* cylinder = static_cast<const btCylinderShape*>(shape);
|
||||
int upAxis = cylinder->getUpAxis();
|
||||
btScalar radius = cylinder->getRadius();
|
||||
btScalar halfHeight = cylinder->getHalfExtents()[upAxis];
|
||||
btVector3 start = worldTransform.getOrigin();
|
||||
btVector3 offsetHeight(0,0,0);
|
||||
offsetHeight[upAxis] = halfHeight;
|
||||
btVector3 offsetRadius(0,0,0);
|
||||
offsetRadius[(upAxis+1)%3] = radius;
|
||||
getDebugDrawer()->drawLine(start+worldTransform.getBasis() * (offsetHeight+offsetRadius),start+worldTransform.getBasis() * (-offsetHeight+offsetRadius),color);
|
||||
getDebugDrawer()->drawLine(start+worldTransform.getBasis() * (offsetHeight-offsetRadius),start+worldTransform.getBasis() * (-offsetHeight-offsetRadius),color);
|
||||
break;
|
||||
}
|
||||
default:
|
||||
{
|
||||
|
||||
if (shape->isConcave())
|
||||
{
|
||||
btConcaveShape* concaveMesh = (btConcaveShape*) shape;
|
||||
|
||||
//todo pass camera, for some culling
|
||||
btVector3 aabbMax(btScalar(1e30),btScalar(1e30),btScalar(1e30));
|
||||
btVector3 aabbMin(btScalar(-1e30),btScalar(-1e30),btScalar(-1e30));
|
||||
|
||||
DebugDrawcallback drawCallback(getDebugDrawer(),worldTransform,color);
|
||||
concaveMesh->processAllTriangles(&drawCallback,aabbMin,aabbMax);
|
||||
|
||||
}
|
||||
|
||||
if (shape->getShapeType() == CONVEX_TRIANGLEMESH_SHAPE_PROXYTYPE)
|
||||
{
|
||||
btConvexTriangleMeshShape* convexMesh = (btConvexTriangleMeshShape*) shape;
|
||||
//todo: pass camera for some culling
|
||||
btVector3 aabbMax(btScalar(1e30),btScalar(1e30),btScalar(1e30));
|
||||
btVector3 aabbMin(btScalar(-1e30),btScalar(-1e30),btScalar(-1e30));
|
||||
//DebugDrawcallback drawCallback;
|
||||
DebugDrawcallback drawCallback(getDebugDrawer(),worldTransform,color);
|
||||
convexMesh->getStridingMesh()->InternalProcessAllTriangles(&drawCallback,aabbMin,aabbMax);
|
||||
}
|
||||
|
||||
|
||||
/// for polyhedral shapes
|
||||
if (shape->isPolyhedral())
|
||||
{
|
||||
btPolyhedralConvexShape* polyshape = (btPolyhedralConvexShape*) shape;
|
||||
|
||||
int i;
|
||||
for (i=0;i<polyshape->getNumEdges();i++)
|
||||
{
|
||||
btPoint3 a,b;
|
||||
polyshape->getEdge(i,a,b);
|
||||
btVector3 wa = worldTransform * a;
|
||||
btVector3 wb = worldTransform * b;
|
||||
getDebugDrawer()->drawLine(wa,wb,color);
|
||||
|
||||
}
|
||||
|
||||
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
void btDiscreteDynamicsWorld::setConstraintSolver(btConstraintSolver* solver)
|
||||
{
|
||||
if (m_ownsConstraintSolver)
|
||||
{
|
||||
delete m_constraintSolver;
|
||||
}
|
||||
m_ownsConstraintSolver = false;
|
||||
m_constraintSolver = solver;
|
||||
}
|
||||
|
||||
btConstraintSolver* btDiscreteDynamicsWorld::getConstraintSolver()
|
||||
{
|
||||
return m_constraintSolver;
|
||||
}
|
||||
|
||||
|
||||
int btDiscreteDynamicsWorld::getNumConstraints() const
|
||||
{
|
||||
return int(m_constraints.size());
|
||||
}
|
||||
btTypedConstraint* btDiscreteDynamicsWorld::getConstraint(int index)
|
||||
{
|
||||
return m_constraints[index];
|
||||
}
|
||||
const btTypedConstraint* btDiscreteDynamicsWorld::getConstraint(int index) const
|
||||
{
|
||||
return m_constraints[index];
|
||||
}
|
||||
159
bullet/src/BulletDynamics/Dynamics/btDiscreteDynamicsWorld.h
Normal file
159
bullet/src/BulletDynamics/Dynamics/btDiscreteDynamicsWorld.h
Normal file
@ -0,0 +1,159 @@
|
||||
/*
|
||||
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 BT_DISCRETE_DYNAMICS_WORLD_H
|
||||
#define BT_DISCRETE_DYNAMICS_WORLD_H
|
||||
|
||||
#include "btDynamicsWorld.h"
|
||||
|
||||
class btDispatcher;
|
||||
class btOverlappingPairCache;
|
||||
class btConstraintSolver;
|
||||
class btSimulationIslandManager;
|
||||
class btTypedConstraint;
|
||||
#include "BulletDynamics/ConstraintSolver/btContactSolverInfo.h"
|
||||
|
||||
class btRaycastVehicle;
|
||||
class btIDebugDraw;
|
||||
#include "LinearMath/btAlignedObjectArray.h"
|
||||
|
||||
|
||||
///btDiscreteDynamicsWorld provides discrete rigid body simulation
|
||||
///those classes replace the obsolete CcdPhysicsEnvironment/CcdPhysicsController
|
||||
class btDiscreteDynamicsWorld : public btDynamicsWorld
|
||||
{
|
||||
protected:
|
||||
|
||||
btConstraintSolver* m_constraintSolver;
|
||||
|
||||
btSimulationIslandManager* m_islandManager;
|
||||
|
||||
btAlignedObjectArray<btTypedConstraint*> m_constraints;
|
||||
|
||||
btIDebugDraw* m_debugDrawer;
|
||||
|
||||
btVector3 m_gravity;
|
||||
|
||||
//for variable timesteps
|
||||
btScalar m_localTime;
|
||||
//for variable timesteps
|
||||
|
||||
bool m_ownsIslandManager;
|
||||
bool m_ownsConstraintSolver;
|
||||
|
||||
btContactSolverInfo m_solverInfo;
|
||||
|
||||
|
||||
btAlignedObjectArray<btRaycastVehicle*> m_vehicles;
|
||||
|
||||
int m_profileTimings;
|
||||
|
||||
void predictUnconstraintMotion(btScalar timeStep);
|
||||
|
||||
void integrateTransforms(btScalar timeStep);
|
||||
|
||||
void calculateSimulationIslands();
|
||||
|
||||
void solveConstraints(btContactSolverInfo& solverInfo);
|
||||
|
||||
void updateActivationState(btScalar timeStep);
|
||||
|
||||
void updateVehicles(btScalar timeStep);
|
||||
|
||||
void startProfiling(btScalar timeStep);
|
||||
|
||||
virtual void internalSingleStepSimulation( btScalar timeStep);
|
||||
|
||||
void synchronizeMotionStates();
|
||||
|
||||
void saveKinematicState(btScalar timeStep);
|
||||
|
||||
void debugDrawSphere(btScalar radius, const btTransform& transform, const btVector3& color);
|
||||
|
||||
public:
|
||||
|
||||
|
||||
///this btDiscreteDynamicsWorld constructor gets created objects from the user, and will not delete those
|
||||
btDiscreteDynamicsWorld(btDispatcher* dispatcher,btBroadphaseInterface* pairCache,btConstraintSolver* constraintSolver);
|
||||
|
||||
virtual ~btDiscreteDynamicsWorld();
|
||||
|
||||
///if maxSubSteps > 0, it will interpolate motion between fixedTimeStep's
|
||||
virtual int stepSimulation( btScalar timeStep,int maxSubSteps=1, btScalar fixedTimeStep=btScalar(1.)/btScalar(60.));
|
||||
|
||||
virtual void updateAabbs();
|
||||
|
||||
void addConstraint(btTypedConstraint* constraint, bool disableCollisionsBetweenLinkedBodies=false);
|
||||
|
||||
void removeConstraint(btTypedConstraint* constraint);
|
||||
|
||||
void addVehicle(btRaycastVehicle* vehicle);
|
||||
|
||||
void removeVehicle(btRaycastVehicle* vehicle);
|
||||
|
||||
btSimulationIslandManager* getSimulationIslandManager()
|
||||
{
|
||||
return m_islandManager;
|
||||
}
|
||||
|
||||
const btSimulationIslandManager* getSimulationIslandManager() const
|
||||
{
|
||||
return m_islandManager;
|
||||
}
|
||||
|
||||
btCollisionWorld* getCollisionWorld()
|
||||
{
|
||||
return this;
|
||||
}
|
||||
|
||||
virtual void setDebugDrawer(btIDebugDraw* debugDrawer)
|
||||
{
|
||||
m_debugDrawer = debugDrawer;
|
||||
}
|
||||
|
||||
virtual btIDebugDraw* getDebugDrawer()
|
||||
{
|
||||
return m_debugDrawer;
|
||||
}
|
||||
|
||||
virtual void setGravity(const btVector3& gravity);
|
||||
|
||||
virtual void addRigidBody(btRigidBody* body);
|
||||
|
||||
virtual void addRigidBody(btRigidBody* body, short group, short mask);
|
||||
|
||||
virtual void removeRigidBody(btRigidBody* body);
|
||||
|
||||
void debugDrawObject(const btTransform& worldTransform, const btCollisionShape* shape, const btVector3& color);
|
||||
|
||||
virtual void setConstraintSolver(btConstraintSolver* solver);
|
||||
|
||||
virtual btConstraintSolver* getConstraintSolver();
|
||||
|
||||
virtual int getNumConstraints() const;
|
||||
|
||||
virtual btTypedConstraint* getConstraint(int index) ;
|
||||
|
||||
virtual const btTypedConstraint* getConstraint(int index) const;
|
||||
|
||||
btContactSolverInfo& getSolverInfo()
|
||||
{
|
||||
return m_solverInfo;
|
||||
}
|
||||
|
||||
|
||||
};
|
||||
|
||||
#endif //BT_DISCRETE_DYNAMICS_WORLD_H
|
||||
80
bullet/src/BulletDynamics/Dynamics/btDynamicsWorld.h
Normal file
80
bullet/src/BulletDynamics/Dynamics/btDynamicsWorld.h
Normal file
@ -0,0 +1,80 @@
|
||||
/*
|
||||
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 BT_DYNAMICS_WORLD_H
|
||||
#define BT_DYNAMICS_WORLD_H
|
||||
|
||||
#include "BulletCollision/CollisionDispatch/btCollisionWorld.h"
|
||||
class btTypedConstraint;
|
||||
class btRaycastVehicle;
|
||||
class btConstraintSolver;
|
||||
|
||||
|
||||
///btDynamicsWorld is the baseclass for several dynamics implementation, basic, discrete, parallel, and continuous
|
||||
class btDynamicsWorld : public btCollisionWorld
|
||||
{
|
||||
public:
|
||||
|
||||
|
||||
btDynamicsWorld(btDispatcher* dispatcher,btBroadphaseInterface* broadphase)
|
||||
:btCollisionWorld(dispatcher,broadphase)
|
||||
{
|
||||
}
|
||||
|
||||
virtual ~btDynamicsWorld()
|
||||
{
|
||||
}
|
||||
|
||||
///stepSimulation proceeds the simulation over timeStep units
|
||||
///if maxSubSteps > 0, it will interpolate time steps
|
||||
virtual int stepSimulation( btScalar timeStep,int maxSubSteps=1, btScalar fixedTimeStep=btScalar(1.)/btScalar(60.))=0;
|
||||
|
||||
virtual void updateAabbs() = 0;
|
||||
|
||||
virtual void addConstraint(btTypedConstraint* constraint, bool disableCollisionsBetweenLinkedBodies=false) { (void)constraint;};
|
||||
|
||||
virtual void removeConstraint(btTypedConstraint* constraint) {(void)constraint;};
|
||||
|
||||
virtual void addVehicle(btRaycastVehicle* vehicle) {(void)vehicle;};
|
||||
|
||||
virtual void removeVehicle(btRaycastVehicle* vehicle) {(void)vehicle;};
|
||||
|
||||
|
||||
virtual void setDebugDrawer(btIDebugDraw* debugDrawer) = 0;
|
||||
|
||||
virtual btIDebugDraw* getDebugDrawer() = 0;
|
||||
|
||||
//once a rigidbody is added to the dynamics world, it will get this gravity assigned
|
||||
//existing rigidbodies in the world get gravity assigned too, during this method
|
||||
virtual void setGravity(const btVector3& gravity) = 0;
|
||||
|
||||
virtual void addRigidBody(btRigidBody* body) = 0;
|
||||
|
||||
virtual void removeRigidBody(btRigidBody* body) = 0;
|
||||
|
||||
virtual void setConstraintSolver(btConstraintSolver* solver) = 0;
|
||||
|
||||
virtual btConstraintSolver* getConstraintSolver() = 0;
|
||||
|
||||
virtual int getNumConstraints() const { return 0; }
|
||||
|
||||
virtual btTypedConstraint* getConstraint(int index) { (void)index; return 0; }
|
||||
|
||||
virtual const btTypedConstraint* getConstraint(int index) const { (void)index; return 0; }
|
||||
|
||||
};
|
||||
|
||||
#endif //BT_DYNAMICS_WORLD_H
|
||||
|
||||
351
bullet/src/BulletDynamics/Dynamics/btRigidBody.cpp
Normal file
351
bullet/src/BulletDynamics/Dynamics/btRigidBody.cpp
Normal file
@ -0,0 +1,351 @@
|
||||
/*
|
||||
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.
|
||||
*/
|
||||
|
||||
#include "btRigidBody.h"
|
||||
#include "BulletCollision/CollisionShapes/btConvexShape.h"
|
||||
#include "LinearMath/btMinMax.h"
|
||||
#include "LinearMath/btTransformUtil.h"
|
||||
#include "LinearMath/btMotionState.h"
|
||||
#include "BulletDynamics/ConstraintSolver/btTypedConstraint.h"
|
||||
|
||||
btScalar gLinearAirDamping = btScalar(1.);
|
||||
//'temporarily' global variables
|
||||
btScalar gDeactivationTime = btScalar(2.);
|
||||
bool gDisableDeactivation = false;
|
||||
|
||||
btScalar gLinearSleepingThreshold = btScalar(0.8);
|
||||
btScalar gAngularSleepingThreshold = btScalar(1.0);
|
||||
static int uniqueId = 0;
|
||||
|
||||
btRigidBody::btRigidBody(btScalar mass, btMotionState* motionState, btCollisionShape* collisionShape, const btVector3& localInertia,btScalar linearDamping,btScalar angularDamping,btScalar friction,btScalar restitution)
|
||||
:
|
||||
m_linearVelocity(btScalar(0.0), btScalar(0.0), btScalar(0.0)),
|
||||
m_angularVelocity(btScalar(0.),btScalar(0.),btScalar(0.)),
|
||||
m_angularFactor(btScalar(1.)),
|
||||
m_gravity(btScalar(0.0), btScalar(0.0), btScalar(0.0)),
|
||||
m_totalForce(btScalar(0.0), btScalar(0.0), btScalar(0.0)),
|
||||
m_totalTorque(btScalar(0.0), btScalar(0.0), btScalar(0.0)),
|
||||
m_linearDamping(btScalar(0.)),
|
||||
m_angularDamping(btScalar(0.5)),
|
||||
m_linearSleepingThreshold(gLinearSleepingThreshold),
|
||||
m_angularSleepingThreshold(gAngularSleepingThreshold),
|
||||
m_optionalMotionState(motionState),
|
||||
m_contactSolverType(0),
|
||||
m_frictionSolverType(0)
|
||||
{
|
||||
|
||||
if (motionState)
|
||||
{
|
||||
motionState->getWorldTransform(m_worldTransform);
|
||||
} else
|
||||
{
|
||||
m_worldTransform = btTransform::getIdentity();
|
||||
}
|
||||
|
||||
m_interpolationWorldTransform = m_worldTransform;
|
||||
m_interpolationLinearVelocity.setValue(0,0,0);
|
||||
m_interpolationAngularVelocity.setValue(0,0,0);
|
||||
|
||||
//moved to btCollisionObject
|
||||
m_friction = friction;
|
||||
m_restitution = restitution;
|
||||
|
||||
m_collisionShape = collisionShape;
|
||||
m_debugBodyId = uniqueId++;
|
||||
|
||||
//m_internalOwner is to allow upcasting from collision object to rigid body
|
||||
m_internalOwner = this;
|
||||
|
||||
setMassProps(mass, localInertia);
|
||||
setDamping(linearDamping, angularDamping);
|
||||
updateInertiaTensor();
|
||||
|
||||
}
|
||||
|
||||
#ifdef OBSOLETE_MOTIONSTATE_LESS
|
||||
btRigidBody::btRigidBody( btScalar mass,const btTransform& worldTransform,btCollisionShape* collisionShape,const btVector3& localInertia,btScalar linearDamping,btScalar angularDamping,btScalar friction,btScalar restitution)
|
||||
:
|
||||
m_gravity(btScalar(0.0), btScalar(0.0), btScalar(0.0)),
|
||||
m_totalForce(btScalar(0.0), btScalar(0.0), btScalar(0.0)),
|
||||
m_totalTorque(btScalar(0.0), btScalar(0.0), btScalar(0.0)),
|
||||
m_linearVelocity(btScalar(0.0), btScalar(0.0), btScalar(0.0)),
|
||||
m_angularVelocity(btScalar(0.),btScalar(0.),btScalar(0.)),
|
||||
m_linearSleepingThreshold(gLinearSleepingThreshold),
|
||||
m_angularSleepingThreshold(gAngularSleepingThreshold),
|
||||
m_linearDamping(btScalar(0.)),
|
||||
m_angularDamping(btScalar(0.5)),
|
||||
m_optionalMotionState(0),
|
||||
m_contactSolverType(0),
|
||||
m_frictionSolverType(0)
|
||||
|
||||
{
|
||||
|
||||
m_worldTransform = worldTransform;
|
||||
m_interpolationWorldTransform = m_worldTransform;
|
||||
m_interpolationLinearVelocity.setValue(0,0,0);
|
||||
m_interpolationAngularVelocity.setValue(0,0,0);
|
||||
|
||||
//moved to btCollisionObject
|
||||
m_friction = friction;
|
||||
m_restitution = restitution;
|
||||
|
||||
m_collisionShape = collisionShape;
|
||||
m_debugBodyId = uniqueId++;
|
||||
|
||||
//m_internalOwner is to allow upcasting from collision object to rigid body
|
||||
m_internalOwner = this;
|
||||
|
||||
setMassProps(mass, localInertia);
|
||||
setDamping(linearDamping, angularDamping);
|
||||
updateInertiaTensor();
|
||||
|
||||
}
|
||||
|
||||
#endif //OBSOLETE_MOTIONSTATE_LESS
|
||||
|
||||
|
||||
|
||||
|
||||
#define EXPERIMENTAL_JITTER_REMOVAL 1
|
||||
#ifdef EXPERIMENTAL_JITTER_REMOVAL
|
||||
//Bullet 2.20b has experimental damping code to reduce jitter just before objects fall asleep/deactivate
|
||||
//doesn't work very well yet (value 0 disabled this damping)
|
||||
//note there this influences deactivation thresholds!
|
||||
btScalar gClippedAngvelThresholdSqr = btScalar(0.01);
|
||||
btScalar gClippedLinearThresholdSqr = btScalar(0.01);
|
||||
#endif //EXPERIMENTAL_JITTER_REMOVAL
|
||||
|
||||
btScalar gJitterVelocityDampingFactor = btScalar(0.7);
|
||||
|
||||
void btRigidBody::predictIntegratedTransform(btScalar timeStep,btTransform& predictedTransform)
|
||||
{
|
||||
|
||||
#ifdef EXPERIMENTAL_JITTER_REMOVAL
|
||||
//if (wantsSleeping())
|
||||
{
|
||||
//clip to avoid jitter
|
||||
if ((m_angularVelocity.length2() < gClippedAngvelThresholdSqr) &&
|
||||
(m_linearVelocity.length2() < gClippedLinearThresholdSqr))
|
||||
{
|
||||
m_angularVelocity *= gJitterVelocityDampingFactor;
|
||||
m_linearVelocity *= gJitterVelocityDampingFactor;
|
||||
}
|
||||
}
|
||||
|
||||
#endif //EXPERIMENTAL_JITTER_REMOVAL
|
||||
|
||||
btTransformUtil::integrateTransform(m_worldTransform,m_linearVelocity,m_angularVelocity,timeStep,predictedTransform);
|
||||
}
|
||||
|
||||
void btRigidBody::saveKinematicState(btScalar timeStep)
|
||||
{
|
||||
//todo: clamp to some (user definable) safe minimum timestep, to limit maximum angular/linear velocities
|
||||
if (timeStep != btScalar(0.))
|
||||
{
|
||||
//if we use motionstate to synchronize world transforms, get the new kinematic/animated world transform
|
||||
if (getMotionState())
|
||||
getMotionState()->getWorldTransform(m_worldTransform);
|
||||
btVector3 linVel,angVel;
|
||||
|
||||
btTransformUtil::calculateVelocity(m_interpolationWorldTransform,m_worldTransform,timeStep,m_linearVelocity,m_angularVelocity);
|
||||
m_interpolationLinearVelocity = m_linearVelocity;
|
||||
m_interpolationAngularVelocity = m_angularVelocity;
|
||||
m_interpolationWorldTransform = m_worldTransform;
|
||||
//printf("angular = %f %f %f\n",m_angularVelocity.getX(),m_angularVelocity.getY(),m_angularVelocity.getZ());
|
||||
}
|
||||
}
|
||||
|
||||
void btRigidBody::getAabb(btVector3& aabbMin,btVector3& aabbMax) const
|
||||
{
|
||||
getCollisionShape()->getAabb(m_worldTransform,aabbMin,aabbMax);
|
||||
}
|
||||
|
||||
|
||||
|
||||
|
||||
void btRigidBody::setGravity(const btVector3& acceleration)
|
||||
{
|
||||
if (m_inverseMass != btScalar(0.0))
|
||||
{
|
||||
m_gravity = acceleration * (btScalar(1.0) / m_inverseMass);
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
void btRigidBody::setDamping(btScalar lin_damping, btScalar ang_damping)
|
||||
{
|
||||
m_linearDamping = GEN_clamped(lin_damping, (btScalar)btScalar(0.0), (btScalar)btScalar(1.0));
|
||||
m_angularDamping = GEN_clamped(ang_damping, (btScalar)btScalar(0.0), (btScalar)btScalar(1.0));
|
||||
}
|
||||
|
||||
|
||||
|
||||
#include <stdio.h>
|
||||
|
||||
|
||||
void btRigidBody::applyForces(btScalar step)
|
||||
{
|
||||
if (isStaticOrKinematicObject())
|
||||
return;
|
||||
|
||||
applyCentralForce(m_gravity);
|
||||
|
||||
m_linearVelocity *= GEN_clamped((btScalar(1.) - step * gLinearAirDamping * m_linearDamping), (btScalar)btScalar(0.0), (btScalar)btScalar(1.0));
|
||||
m_angularVelocity *= GEN_clamped((btScalar(1.) - step * m_angularDamping), (btScalar)btScalar(0.0), (btScalar)btScalar(1.0));
|
||||
|
||||
#define FORCE_VELOCITY_DAMPING 1
|
||||
#ifdef FORCE_VELOCITY_DAMPING
|
||||
btScalar speed = m_linearVelocity.length();
|
||||
if (speed < m_linearDamping)
|
||||
{
|
||||
btScalar dampVel = btScalar(0.005);
|
||||
if (speed > dampVel)
|
||||
{
|
||||
btVector3 dir = m_linearVelocity.normalized();
|
||||
m_linearVelocity -= dir * dampVel;
|
||||
} else
|
||||
{
|
||||
m_linearVelocity.setValue(btScalar(0.),btScalar(0.),btScalar(0.));
|
||||
}
|
||||
}
|
||||
|
||||
btScalar angSpeed = m_angularVelocity.length();
|
||||
if (angSpeed < m_angularDamping)
|
||||
{
|
||||
btScalar angDampVel = btScalar(0.005);
|
||||
if (angSpeed > angDampVel)
|
||||
{
|
||||
btVector3 dir = m_angularVelocity.normalized();
|
||||
m_angularVelocity -= dir * angDampVel;
|
||||
} else
|
||||
{
|
||||
m_angularVelocity.setValue(btScalar(0.),btScalar(0.),btScalar(0.));
|
||||
}
|
||||
}
|
||||
#endif //FORCE_VELOCITY_DAMPING
|
||||
|
||||
}
|
||||
|
||||
void btRigidBody::proceedToTransform(const btTransform& newTrans)
|
||||
{
|
||||
setCenterOfMassTransform( newTrans );
|
||||
}
|
||||
|
||||
|
||||
void btRigidBody::setMassProps(btScalar mass, const btVector3& inertia)
|
||||
{
|
||||
if (mass == btScalar(0.))
|
||||
{
|
||||
m_collisionFlags |= btCollisionObject::CF_STATIC_OBJECT;
|
||||
m_inverseMass = btScalar(0.);
|
||||
} else
|
||||
{
|
||||
m_collisionFlags &= (~btCollisionObject::CF_STATIC_OBJECT);
|
||||
m_inverseMass = btScalar(1.0) / mass;
|
||||
}
|
||||
|
||||
m_invInertiaLocal.setValue(inertia.x() != btScalar(0.0) ? btScalar(1.0) / inertia.x(): btScalar(0.0),
|
||||
inertia.y() != btScalar(0.0) ? btScalar(1.0) / inertia.y(): btScalar(0.0),
|
||||
inertia.z() != btScalar(0.0) ? btScalar(1.0) / inertia.z(): btScalar(0.0));
|
||||
|
||||
}
|
||||
|
||||
|
||||
|
||||
void btRigidBody::updateInertiaTensor()
|
||||
{
|
||||
m_invInertiaTensorWorld = m_worldTransform.getBasis().scaled(m_invInertiaLocal) * m_worldTransform.getBasis().transpose();
|
||||
}
|
||||
|
||||
|
||||
void btRigidBody::integrateVelocities(btScalar step)
|
||||
{
|
||||
if (isStaticOrKinematicObject())
|
||||
return;
|
||||
|
||||
m_linearVelocity += m_totalForce * (m_inverseMass * step);
|
||||
m_angularVelocity += m_invInertiaTensorWorld * m_totalTorque * step;
|
||||
|
||||
#define MAX_ANGVEL SIMD_HALF_PI
|
||||
/// clamp angular velocity. collision calculations will fail on higher angular velocities
|
||||
btScalar angvel = m_angularVelocity.length();
|
||||
if (angvel*step > MAX_ANGVEL)
|
||||
{
|
||||
m_angularVelocity *= (MAX_ANGVEL/step) /angvel;
|
||||
}
|
||||
|
||||
clearForces();
|
||||
}
|
||||
|
||||
btQuaternion btRigidBody::getOrientation() const
|
||||
{
|
||||
btQuaternion orn;
|
||||
m_worldTransform.getBasis().getRotation(orn);
|
||||
return orn;
|
||||
}
|
||||
|
||||
|
||||
void btRigidBody::setCenterOfMassTransform(const btTransform& xform)
|
||||
{
|
||||
|
||||
if (isStaticOrKinematicObject())
|
||||
{
|
||||
m_interpolationWorldTransform = m_worldTransform;
|
||||
} else
|
||||
{
|
||||
m_interpolationWorldTransform = xform;
|
||||
}
|
||||
m_interpolationLinearVelocity = getLinearVelocity();
|
||||
m_interpolationAngularVelocity = getAngularVelocity();
|
||||
m_worldTransform = xform;
|
||||
updateInertiaTensor();
|
||||
}
|
||||
|
||||
|
||||
bool btRigidBody::checkCollideWithOverride(btCollisionObject* co)
|
||||
{
|
||||
btRigidBody* otherRb = btRigidBody::upcast(co);
|
||||
if (!otherRb)
|
||||
return true;
|
||||
|
||||
for (int i = 0; i < m_constraintRefs.size(); ++i)
|
||||
{
|
||||
btTypedConstraint* c = m_constraintRefs[i];
|
||||
if (&c->getRigidBodyA() == otherRb || &c->getRigidBodyB() == otherRb)
|
||||
return false;
|
||||
}
|
||||
|
||||
return true;
|
||||
}
|
||||
|
||||
void btRigidBody::addConstraintRef(btTypedConstraint* c)
|
||||
{
|
||||
int index = m_constraintRefs.findLinearSearch(c);
|
||||
if (index == m_constraintRefs.size())
|
||||
m_constraintRefs.push_back(c);
|
||||
|
||||
m_checkCollideWith = true;
|
||||
}
|
||||
|
||||
void btRigidBody::removeConstraintRef(btTypedConstraint* c)
|
||||
{
|
||||
m_constraintRefs.remove(c);
|
||||
m_checkCollideWith = m_constraintRefs.size() > 0;
|
||||
}
|
||||
|
||||
385
bullet/src/BulletDynamics/Dynamics/btRigidBody.h
Normal file
385
bullet/src/BulletDynamics/Dynamics/btRigidBody.h
Normal file
@ -0,0 +1,385 @@
|
||||
/*
|
||||
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 RIGIDBODY_H
|
||||
#define RIGIDBODY_H
|
||||
|
||||
#include "LinearMath/btAlignedObjectArray.h"
|
||||
#include "LinearMath/btPoint3.h"
|
||||
#include "LinearMath/btTransform.h"
|
||||
#include "BulletCollision/BroadphaseCollision/btBroadphaseProxy.h"
|
||||
#include "BulletCollision/CollisionDispatch/btCollisionObject.h"
|
||||
|
||||
class btCollisionShape;
|
||||
class btMotionState;
|
||||
class btTypedConstraint;
|
||||
|
||||
|
||||
extern btScalar gLinearAirDamping;
|
||||
|
||||
extern btScalar gDeactivationTime;
|
||||
extern bool gDisableDeactivation;
|
||||
extern btScalar gLinearSleepingThreshold;
|
||||
extern btScalar gAngularSleepingThreshold;
|
||||
|
||||
|
||||
/// btRigidBody class for btRigidBody Dynamics
|
||||
///
|
||||
class btRigidBody : public btCollisionObject
|
||||
{
|
||||
|
||||
btMatrix3x3 m_invInertiaTensorWorld;
|
||||
btVector3 m_linearVelocity;
|
||||
btVector3 m_angularVelocity;
|
||||
btScalar m_inverseMass;
|
||||
btScalar m_angularFactor;
|
||||
|
||||
btVector3 m_gravity;
|
||||
btVector3 m_invInertiaLocal;
|
||||
btVector3 m_totalForce;
|
||||
btVector3 m_totalTorque;
|
||||
|
||||
btScalar m_linearDamping;
|
||||
btScalar m_angularDamping;
|
||||
|
||||
btScalar m_linearSleepingThreshold;
|
||||
btScalar m_angularSleepingThreshold;
|
||||
|
||||
|
||||
//m_optionalMotionState allows to automatic synchronize the world transform for active objects
|
||||
btMotionState* m_optionalMotionState;
|
||||
|
||||
//keep track of typed constraints referencing this rigid body
|
||||
btAlignedObjectArray<btTypedConstraint*> m_constraintRefs;
|
||||
|
||||
public:
|
||||
|
||||
#ifdef OBSOLETE_MOTIONSTATE_LESS
|
||||
//not supported, please use btMotionState
|
||||
btRigidBody(btScalar mass, const btTransform& worldTransform, btCollisionShape* collisionShape, const btVector3& localInertia=btVector3(0,0,0),btScalar linearDamping=btScalar(0.),btScalar angularDamping=btScalar(0.),btScalar friction=btScalar(0.5),btScalar restitution=btScalar(0.));
|
||||
#endif //OBSOLETE_MOTIONSTATE_LESS
|
||||
|
||||
btRigidBody(btScalar mass, btMotionState* motionState, btCollisionShape* collisionShape, const btVector3& localInertia=btVector3(0,0,0),btScalar linearDamping=btScalar(0.),btScalar angularDamping=btScalar(0.),btScalar friction=btScalar(0.5),btScalar restitution=btScalar(0.));
|
||||
|
||||
virtual ~btRigidBody()
|
||||
{
|
||||
//No constraints should point to this rigidbody
|
||||
//Remove constraints from the dynamics world before you delete the related rigidbodies.
|
||||
btAssert(m_constraintRefs.size()==0);
|
||||
}
|
||||
|
||||
|
||||
void proceedToTransform(const btTransform& newTrans);
|
||||
|
||||
///to keep collision detection and dynamics separate we don't store a rigidbody pointer
|
||||
///but a rigidbody is derived from btCollisionObject, so we can safely perform an upcast
|
||||
static const btRigidBody* upcast(const btCollisionObject* colObj)
|
||||
{
|
||||
return (const btRigidBody*)colObj->getInternalOwner();
|
||||
}
|
||||
static btRigidBody* upcast(btCollisionObject* colObj)
|
||||
{
|
||||
return (btRigidBody*)colObj->getInternalOwner();
|
||||
}
|
||||
|
||||
/// continuous collision detection needs prediction
|
||||
void predictIntegratedTransform(btScalar step, btTransform& predictedTransform) ;
|
||||
|
||||
void saveKinematicState(btScalar step);
|
||||
|
||||
|
||||
void applyForces(btScalar step);
|
||||
|
||||
void setGravity(const btVector3& acceleration);
|
||||
|
||||
const btVector3& getGravity() const
|
||||
{
|
||||
return m_gravity;
|
||||
}
|
||||
|
||||
void setDamping(btScalar lin_damping, btScalar ang_damping);
|
||||
|
||||
inline const btCollisionShape* getCollisionShape() const {
|
||||
return m_collisionShape;
|
||||
}
|
||||
|
||||
inline btCollisionShape* getCollisionShape() {
|
||||
return m_collisionShape;
|
||||
}
|
||||
|
||||
void setMassProps(btScalar mass, const btVector3& inertia);
|
||||
|
||||
btScalar getInvMass() const { return m_inverseMass; }
|
||||
const btMatrix3x3& getInvInertiaTensorWorld() const {
|
||||
return m_invInertiaTensorWorld;
|
||||
}
|
||||
|
||||
void integrateVelocities(btScalar step);
|
||||
|
||||
void setCenterOfMassTransform(const btTransform& xform);
|
||||
|
||||
void applyCentralForce(const btVector3& force)
|
||||
{
|
||||
m_totalForce += force;
|
||||
}
|
||||
|
||||
const btVector3& getInvInertiaDiagLocal()
|
||||
{
|
||||
return m_invInertiaLocal;
|
||||
};
|
||||
|
||||
void setInvInertiaDiagLocal(const btVector3& diagInvInertia)
|
||||
{
|
||||
m_invInertiaLocal = diagInvInertia;
|
||||
}
|
||||
|
||||
void setSleepingThresholds(btScalar linear,btScalar angular)
|
||||
{
|
||||
m_linearSleepingThreshold = linear;
|
||||
m_angularSleepingThreshold = angular;
|
||||
}
|
||||
|
||||
void applyTorque(const btVector3& torque)
|
||||
{
|
||||
m_totalTorque += torque;
|
||||
}
|
||||
|
||||
void applyForce(const btVector3& force, const btVector3& rel_pos)
|
||||
{
|
||||
applyCentralForce(force);
|
||||
applyTorque(rel_pos.cross(force));
|
||||
}
|
||||
|
||||
void applyCentralImpulse(const btVector3& impulse)
|
||||
{
|
||||
m_linearVelocity += impulse * m_inverseMass;
|
||||
}
|
||||
|
||||
void applyTorqueImpulse(const btVector3& torque)
|
||||
{
|
||||
m_angularVelocity += m_invInertiaTensorWorld * torque;
|
||||
}
|
||||
|
||||
void applyImpulse(const btVector3& impulse, const btVector3& rel_pos)
|
||||
{
|
||||
if (m_inverseMass != btScalar(0.))
|
||||
{
|
||||
applyCentralImpulse(impulse);
|
||||
if (m_angularFactor)
|
||||
{
|
||||
applyTorqueImpulse(rel_pos.cross(impulse)*m_angularFactor);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
//Optimization for the iterative solver: avoid calculating constant terms involving inertia, normal, relative position
|
||||
inline void internalApplyImpulse(const btVector3& linearComponent, const btVector3& angularComponent,btScalar impulseMagnitude)
|
||||
{
|
||||
if (m_inverseMass != btScalar(0.))
|
||||
{
|
||||
m_linearVelocity += linearComponent*impulseMagnitude;
|
||||
if (m_angularFactor)
|
||||
{
|
||||
m_angularVelocity += angularComponent*impulseMagnitude*m_angularFactor;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
void clearForces()
|
||||
{
|
||||
m_totalForce.setValue(btScalar(0.0), btScalar(0.0), btScalar(0.0));
|
||||
m_totalTorque.setValue(btScalar(0.0), btScalar(0.0), btScalar(0.0));
|
||||
}
|
||||
|
||||
void updateInertiaTensor();
|
||||
|
||||
const btPoint3& getCenterOfMassPosition() const {
|
||||
return m_worldTransform.getOrigin();
|
||||
}
|
||||
btQuaternion getOrientation() const;
|
||||
|
||||
const btTransform& getCenterOfMassTransform() const {
|
||||
return m_worldTransform;
|
||||
}
|
||||
const btVector3& getLinearVelocity() const {
|
||||
return m_linearVelocity;
|
||||
}
|
||||
const btVector3& getAngularVelocity() const {
|
||||
return m_angularVelocity;
|
||||
}
|
||||
|
||||
|
||||
inline void setLinearVelocity(const btVector3& lin_vel)
|
||||
{
|
||||
assert (m_collisionFlags != btCollisionObject::CF_STATIC_OBJECT);
|
||||
m_linearVelocity = lin_vel;
|
||||
}
|
||||
|
||||
inline void setAngularVelocity(const btVector3& ang_vel) {
|
||||
assert (m_collisionFlags != btCollisionObject::CF_STATIC_OBJECT);
|
||||
{
|
||||
m_angularVelocity = ang_vel;
|
||||
}
|
||||
}
|
||||
|
||||
btVector3 getVelocityInLocalPoint(const btVector3& rel_pos) const
|
||||
{
|
||||
//we also calculate lin/ang velocity for kinematic objects
|
||||
return m_linearVelocity + m_angularVelocity.cross(rel_pos);
|
||||
|
||||
//for kinematic objects, we could also use use:
|
||||
// return (m_worldTransform(rel_pos) - m_interpolationWorldTransform(rel_pos)) / m_kinematicTimeStep;
|
||||
}
|
||||
|
||||
void translate(const btVector3& v)
|
||||
{
|
||||
m_worldTransform.getOrigin() += v;
|
||||
}
|
||||
|
||||
|
||||
void getAabb(btVector3& aabbMin,btVector3& aabbMax) const;
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
inline btScalar computeImpulseDenominator(const btPoint3& pos, const btVector3& normal) const
|
||||
{
|
||||
btVector3 r0 = pos - getCenterOfMassPosition();
|
||||
|
||||
btVector3 c0 = (r0).cross(normal);
|
||||
|
||||
btVector3 vec = (c0 * getInvInertiaTensorWorld()).cross(r0);
|
||||
|
||||
return m_inverseMass + normal.dot(vec);
|
||||
|
||||
}
|
||||
|
||||
inline btScalar computeAngularImpulseDenominator(const btVector3& axis) const
|
||||
{
|
||||
btVector3 vec = axis * getInvInertiaTensorWorld();
|
||||
return axis.dot(vec);
|
||||
}
|
||||
|
||||
inline void updateDeactivation(btScalar timeStep)
|
||||
{
|
||||
if ( (getActivationState() == ISLAND_SLEEPING) || (getActivationState() == DISABLE_DEACTIVATION))
|
||||
return;
|
||||
|
||||
if ((getLinearVelocity().length2() < m_linearSleepingThreshold*m_linearSleepingThreshold) &&
|
||||
(getAngularVelocity().length2() < m_angularSleepingThreshold*m_angularSleepingThreshold))
|
||||
{
|
||||
m_deactivationTime += timeStep;
|
||||
} else
|
||||
{
|
||||
m_deactivationTime=btScalar(0.);
|
||||
setActivationState(0);
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
inline bool wantsSleeping()
|
||||
{
|
||||
|
||||
if (getActivationState() == DISABLE_DEACTIVATION)
|
||||
return false;
|
||||
|
||||
//disable deactivation
|
||||
if (gDisableDeactivation || (gDeactivationTime == btScalar(0.)))
|
||||
return false;
|
||||
|
||||
if ( (getActivationState() == ISLAND_SLEEPING) || (getActivationState() == WANTS_DEACTIVATION))
|
||||
return true;
|
||||
|
||||
if (m_deactivationTime> gDeactivationTime)
|
||||
{
|
||||
return true;
|
||||
}
|
||||
return false;
|
||||
}
|
||||
|
||||
|
||||
|
||||
const btBroadphaseProxy* getBroadphaseProxy() const
|
||||
{
|
||||
return m_broadphaseHandle;
|
||||
}
|
||||
btBroadphaseProxy* getBroadphaseProxy()
|
||||
{
|
||||
return m_broadphaseHandle;
|
||||
}
|
||||
void setNewBroadphaseProxy(btBroadphaseProxy* broadphaseProxy)
|
||||
{
|
||||
m_broadphaseHandle = broadphaseProxy;
|
||||
}
|
||||
|
||||
//btMotionState allows to automatic synchronize the world transform for active objects
|
||||
btMotionState* getMotionState()
|
||||
{
|
||||
return m_optionalMotionState;
|
||||
}
|
||||
const btMotionState* getMotionState() const
|
||||
{
|
||||
return m_optionalMotionState;
|
||||
}
|
||||
void setMotionState(btMotionState* motionState)
|
||||
{
|
||||
m_optionalMotionState = motionState;
|
||||
if (m_optionalMotionState)
|
||||
motionState->getWorldTransform(m_worldTransform);
|
||||
}
|
||||
|
||||
//for experimental overriding of friction/contact solver func
|
||||
int m_contactSolverType;
|
||||
int m_frictionSolverType;
|
||||
|
||||
void setAngularFactor(btScalar angFac)
|
||||
{
|
||||
m_angularFactor = angFac;
|
||||
}
|
||||
btScalar getAngularFactor() const
|
||||
{
|
||||
return m_angularFactor;
|
||||
}
|
||||
|
||||
//is this rigidbody added to a btCollisionWorld/btDynamicsWorld/btBroadphase?
|
||||
bool isInWorld() const
|
||||
{
|
||||
return (getBroadphaseProxy() != 0);
|
||||
}
|
||||
|
||||
virtual bool checkCollideWithOverride(btCollisionObject* co);
|
||||
|
||||
void addConstraintRef(btTypedConstraint* c);
|
||||
void removeConstraintRef(btTypedConstraint* c);
|
||||
|
||||
btTypedConstraint* getConstraintRef(int index)
|
||||
{
|
||||
return m_constraintRefs[index];
|
||||
}
|
||||
|
||||
int getNumConstraintRefs()
|
||||
{
|
||||
return m_constraintRefs.size();
|
||||
}
|
||||
|
||||
|
||||
int m_debugBodyId;
|
||||
};
|
||||
|
||||
|
||||
|
||||
#endif
|
||||
|
||||
217
bullet/src/BulletDynamics/Dynamics/btSimpleDynamicsWorld.cpp
Normal file
217
bullet/src/BulletDynamics/Dynamics/btSimpleDynamicsWorld.cpp
Normal file
@ -0,0 +1,217 @@
|
||||
/*
|
||||
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.
|
||||
*/
|
||||
|
||||
#include "btSimpleDynamicsWorld.h"
|
||||
#include "BulletCollision/CollisionDispatch/btCollisionDispatcher.h"
|
||||
#include "BulletCollision/BroadphaseCollision/btSimpleBroadphase.h"
|
||||
#include "BulletCollision/CollisionShapes/btCollisionShape.h"
|
||||
#include "BulletDynamics/Dynamics/btRigidBody.h"
|
||||
#include "BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolver.h"
|
||||
#include "BulletDynamics/ConstraintSolver/btContactSolverInfo.h"
|
||||
|
||||
|
||||
/*
|
||||
Make sure this dummy function never changes so that it
|
||||
can be used by probes that are checking whether the
|
||||
library is actually installed.
|
||||
*/
|
||||
extern "C" void btBulletDynamicsProbe () {}
|
||||
|
||||
|
||||
|
||||
|
||||
btSimpleDynamicsWorld::btSimpleDynamicsWorld(btDispatcher* dispatcher,btBroadphaseInterface* pairCache,btConstraintSolver* constraintSolver)
|
||||
:btDynamicsWorld(dispatcher,pairCache),
|
||||
m_constraintSolver(constraintSolver),
|
||||
m_ownsConstraintSolver(false),
|
||||
m_debugDrawer(0),
|
||||
m_gravity(0,0,-10)
|
||||
{
|
||||
|
||||
}
|
||||
|
||||
|
||||
btSimpleDynamicsWorld::~btSimpleDynamicsWorld()
|
||||
{
|
||||
if (m_ownsConstraintSolver)
|
||||
delete m_constraintSolver;
|
||||
}
|
||||
|
||||
int btSimpleDynamicsWorld::stepSimulation( btScalar timeStep,int maxSubSteps, btScalar fixedTimeStep)
|
||||
{
|
||||
(void)fixedTimeStep;
|
||||
(void)maxSubSteps;
|
||||
|
||||
|
||||
///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();
|
||||
|
||||
///solve contact constraints
|
||||
int numManifolds = m_dispatcher1->getNumManifolds();
|
||||
if (numManifolds)
|
||||
{
|
||||
btPersistentManifold** manifoldPtr = ((btCollisionDispatcher*)m_dispatcher1)->getInternalManifoldPointer();
|
||||
|
||||
btContactSolverInfo infoGlobal;
|
||||
infoGlobal.m_timeStep = timeStep;
|
||||
m_constraintSolver->prepareSolve(0,numManifolds);
|
||||
m_constraintSolver->solveGroup(0,0,manifoldPtr, numManifolds,0,0,infoGlobal,m_debugDrawer, m_stackAlloc);
|
||||
m_constraintSolver->allSolved(infoGlobal,m_debugDrawer, m_stackAlloc);
|
||||
}
|
||||
|
||||
///integrate transforms
|
||||
integrateTransforms(timeStep);
|
||||
|
||||
updateAabbs();
|
||||
|
||||
synchronizeMotionStates();
|
||||
|
||||
return 1;
|
||||
|
||||
}
|
||||
|
||||
|
||||
void btSimpleDynamicsWorld::setGravity(const btVector3& gravity)
|
||||
{
|
||||
m_gravity = gravity;
|
||||
for ( int i=0;i<m_collisionObjects.size();i++)
|
||||
{
|
||||
btCollisionObject* colObj = m_collisionObjects[i];
|
||||
btRigidBody* body = btRigidBody::upcast(colObj);
|
||||
if (body)
|
||||
{
|
||||
body->setGravity(gravity);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
void btSimpleDynamicsWorld::removeRigidBody(btRigidBody* body)
|
||||
{
|
||||
removeCollisionObject(body);
|
||||
}
|
||||
|
||||
void btSimpleDynamicsWorld::addRigidBody(btRigidBody* body)
|
||||
{
|
||||
body->setGravity(m_gravity);
|
||||
|
||||
if (body->getCollisionShape())
|
||||
{
|
||||
addCollisionObject(body);
|
||||
}
|
||||
}
|
||||
|
||||
void btSimpleDynamicsWorld::updateAabbs()
|
||||
{
|
||||
btTransform predictedTrans;
|
||||
for ( int i=0;i<m_collisionObjects.size();i++)
|
||||
{
|
||||
btCollisionObject* colObj = m_collisionObjects[i];
|
||||
btRigidBody* body = btRigidBody::upcast(colObj);
|
||||
if (body)
|
||||
{
|
||||
if (body->isActive() && (!body->isStaticObject()))
|
||||
{
|
||||
btPoint3 minAabb,maxAabb;
|
||||
colObj->getCollisionShape()->getAabb(colObj->getWorldTransform(), minAabb,maxAabb);
|
||||
btBroadphaseInterface* bp = getBroadphase();
|
||||
bp->setAabb(body->getBroadphaseHandle(),minAabb,maxAabb);
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
void btSimpleDynamicsWorld::integrateTransforms(btScalar timeStep)
|
||||
{
|
||||
btTransform predictedTrans;
|
||||
for ( int i=0;i<m_collisionObjects.size();i++)
|
||||
{
|
||||
btCollisionObject* colObj = m_collisionObjects[i];
|
||||
btRigidBody* body = btRigidBody::upcast(colObj);
|
||||
if (body)
|
||||
{
|
||||
if (body->isActive() && (!body->isStaticObject()))
|
||||
{
|
||||
body->predictIntegratedTransform(timeStep, predictedTrans);
|
||||
body->proceedToTransform( predictedTrans);
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
|
||||
void btSimpleDynamicsWorld::predictUnconstraintMotion(btScalar timeStep)
|
||||
{
|
||||
for ( int i=0;i<m_collisionObjects.size();i++)
|
||||
{
|
||||
btCollisionObject* colObj = m_collisionObjects[i];
|
||||
btRigidBody* body = btRigidBody::upcast(colObj);
|
||||
if (body)
|
||||
{
|
||||
if (!body->isStaticObject())
|
||||
{
|
||||
if (body->isActive())
|
||||
{
|
||||
body->applyForces( timeStep);
|
||||
body->integrateVelocities( timeStep);
|
||||
body->predictIntegratedTransform(timeStep,body->getInterpolationWorldTransform());
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
void btSimpleDynamicsWorld::synchronizeMotionStates()
|
||||
{
|
||||
//todo: iterate over awake simulation islands!
|
||||
for ( int i=0;i<m_collisionObjects.size();i++)
|
||||
{
|
||||
btCollisionObject* colObj = m_collisionObjects[i];
|
||||
btRigidBody* body = btRigidBody::upcast(colObj);
|
||||
if (body && body->getMotionState())
|
||||
{
|
||||
if (body->getActivationState() != ISLAND_SLEEPING)
|
||||
{
|
||||
body->getMotionState()->setWorldTransform(body->getWorldTransform());
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
|
||||
void btSimpleDynamicsWorld::setConstraintSolver(btConstraintSolver* solver)
|
||||
{
|
||||
if (m_ownsConstraintSolver)
|
||||
{
|
||||
delete m_constraintSolver;
|
||||
}
|
||||
m_ownsConstraintSolver = false;
|
||||
m_constraintSolver = solver;
|
||||
}
|
||||
|
||||
btConstraintSolver* btSimpleDynamicsWorld::getConstraintSolver()
|
||||
{
|
||||
return m_constraintSolver;
|
||||
}
|
||||
84
bullet/src/BulletDynamics/Dynamics/btSimpleDynamicsWorld.h
Normal file
84
bullet/src/BulletDynamics/Dynamics/btSimpleDynamicsWorld.h
Normal file
@ -0,0 +1,84 @@
|
||||
/*
|
||||
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 BT_SIMPLE_DYNAMICS_WORLD_H
|
||||
#define BT_SIMPLE_DYNAMICS_WORLD_H
|
||||
|
||||
#include "btDynamicsWorld.h"
|
||||
|
||||
class btDispatcher;
|
||||
class btOverlappingPairCache;
|
||||
class btConstraintSolver;
|
||||
|
||||
///btSimpleDynamicsWorld demonstrates very basic usage of Bullet rigid body dynamics
|
||||
///It can be used for basic simulations, and as a starting point for porting Bullet
|
||||
///btSimpleDynamicsWorld lacks object deactivation, island management and other concepts.
|
||||
///For more complicated simulations, btDiscreteDynamicsWorld and btContinuousDynamicsWorld are recommended
|
||||
///those classes replace the obsolete CcdPhysicsEnvironment/CcdPhysicsController
|
||||
class btSimpleDynamicsWorld : public btDynamicsWorld
|
||||
{
|
||||
protected:
|
||||
|
||||
btConstraintSolver* m_constraintSolver;
|
||||
|
||||
bool m_ownsConstraintSolver;
|
||||
|
||||
btIDebugDraw* m_debugDrawer;
|
||||
|
||||
void predictUnconstraintMotion(btScalar timeStep);
|
||||
|
||||
void integrateTransforms(btScalar timeStep);
|
||||
|
||||
btVector3 m_gravity;
|
||||
|
||||
public:
|
||||
|
||||
|
||||
|
||||
///this btSimpleDynamicsWorld constructor creates dispatcher, broadphase pairCache and constraintSolver
|
||||
btSimpleDynamicsWorld(btDispatcher* dispatcher,btBroadphaseInterface* pairCache,btConstraintSolver* constraintSolver);
|
||||
|
||||
virtual ~btSimpleDynamicsWorld();
|
||||
|
||||
///maxSubSteps/fixedTimeStep for interpolation is currently ignored for btSimpleDynamicsWorld, use btDiscreteDynamicsWorld instead
|
||||
virtual int stepSimulation( btScalar timeStep,int maxSubSteps=1, btScalar fixedTimeStep=btScalar(1.)/btScalar(60.));
|
||||
|
||||
virtual void setDebugDrawer(btIDebugDraw* debugDrawer)
|
||||
{
|
||||
m_debugDrawer = debugDrawer;
|
||||
};
|
||||
|
||||
virtual btIDebugDraw* getDebugDrawer()
|
||||
{
|
||||
return m_debugDrawer;
|
||||
}
|
||||
|
||||
virtual void setGravity(const btVector3& gravity);
|
||||
|
||||
virtual void addRigidBody(btRigidBody* body);
|
||||
|
||||
virtual void removeRigidBody(btRigidBody* body);
|
||||
|
||||
virtual void updateAabbs();
|
||||
|
||||
void synchronizeMotionStates();
|
||||
|
||||
virtual void setConstraintSolver(btConstraintSolver* solver);
|
||||
|
||||
virtual btConstraintSolver* getConstraintSolver();
|
||||
|
||||
};
|
||||
|
||||
#endif //BT_SIMPLE_DYNAMICS_WORLD_H
|
||||
734
bullet/src/BulletDynamics/Vehicle/btRaycastVehicle.cpp
Normal file
734
bullet/src/BulletDynamics/Vehicle/btRaycastVehicle.cpp
Normal file
@ -0,0 +1,734 @@
|
||||
/*
|
||||
* Copyright (c) 2005 Erwin Coumans http://continuousphysics.com/Bullet/
|
||||
*
|
||||
* Permission to use, copy, modify, distribute and sell this software
|
||||
* and its documentation for any purpose is hereby granted without fee,
|
||||
* provided that the above copyright notice appear in all copies.
|
||||
* Erwin Coumans makes no representations about the suitability
|
||||
* of this software for any purpose.
|
||||
* It is provided "as is" without express or implied warranty.
|
||||
*/
|
||||
|
||||
#include "LinearMath/btVector3.h"
|
||||
#include "btRaycastVehicle.h"
|
||||
|
||||
#include "BulletDynamics/ConstraintSolver/btSolve2LinearConstraint.h"
|
||||
#include "BulletDynamics/ConstraintSolver/btJacobianEntry.h"
|
||||
#include "LinearMath/btQuaternion.h"
|
||||
#include "BulletDynamics/Dynamics/btDynamicsWorld.h"
|
||||
#include "btVehicleRaycaster.h"
|
||||
#include "btWheelInfo.h"
|
||||
#include "LinearMath/btMinMax.h"
|
||||
|
||||
|
||||
#include "BulletDynamics/ConstraintSolver/btContactConstraint.h"
|
||||
|
||||
|
||||
|
||||
static btRigidBody s_fixedObject( 0,0,0);
|
||||
|
||||
btRaycastVehicle::btRaycastVehicle(const btVehicleTuning& tuning,btRigidBody* chassis, btVehicleRaycaster* raycaster )
|
||||
: btTypedConstraint(VEHICLE_CONSTRAINT_TYPE),
|
||||
m_vehicleRaycaster(raycaster),
|
||||
m_pitchControl(btScalar(0.))
|
||||
{
|
||||
m_chassisBody = chassis;
|
||||
m_indexRightAxis = 0;
|
||||
m_indexUpAxis = 2;
|
||||
m_indexForwardAxis = 1;
|
||||
defaultInit(tuning);
|
||||
}
|
||||
|
||||
|
||||
void btRaycastVehicle::defaultInit(const btVehicleTuning& tuning)
|
||||
{
|
||||
(void)tuning;
|
||||
m_currentVehicleSpeedKmHour = btScalar(0.);
|
||||
m_steeringValue = btScalar(0.);
|
||||
|
||||
}
|
||||
|
||||
|
||||
|
||||
btRaycastVehicle::~btRaycastVehicle()
|
||||
{
|
||||
}
|
||||
|
||||
|
||||
//
|
||||
// basically most of the code is general for 2 or 4 wheel vehicles, but some of it needs to be reviewed
|
||||
//
|
||||
btWheelInfo& btRaycastVehicle::addWheel( const btVector3& connectionPointCS, const btVector3& wheelDirectionCS0,const btVector3& wheelAxleCS, btScalar suspensionRestLength, btScalar wheelRadius,const btVehicleTuning& tuning, bool isFrontWheel)
|
||||
{
|
||||
|
||||
btWheelInfoConstructionInfo ci;
|
||||
|
||||
ci.m_chassisConnectionCS = connectionPointCS;
|
||||
ci.m_wheelDirectionCS = wheelDirectionCS0;
|
||||
ci.m_wheelAxleCS = wheelAxleCS;
|
||||
ci.m_suspensionRestLength = suspensionRestLength;
|
||||
ci.m_wheelRadius = wheelRadius;
|
||||
ci.m_suspensionStiffness = tuning.m_suspensionStiffness;
|
||||
ci.m_wheelsDampingCompression = tuning.m_suspensionCompression;
|
||||
ci.m_wheelsDampingRelaxation = tuning.m_suspensionDamping;
|
||||
ci.m_frictionSlip = tuning.m_frictionSlip;
|
||||
ci.m_bIsFrontWheel = isFrontWheel;
|
||||
ci.m_maxSuspensionTravelCm = tuning.m_maxSuspensionTravelCm;
|
||||
|
||||
m_wheelInfo.push_back( btWheelInfo(ci));
|
||||
|
||||
btWheelInfo& wheel = m_wheelInfo[getNumWheels()-1];
|
||||
|
||||
updateWheelTransformsWS( wheel , false );
|
||||
updateWheelTransform(getNumWheels()-1,false);
|
||||
return wheel;
|
||||
}
|
||||
|
||||
|
||||
|
||||
|
||||
const btTransform& btRaycastVehicle::getWheelTransformWS( int wheelIndex ) const
|
||||
{
|
||||
assert(wheelIndex < getNumWheels());
|
||||
const btWheelInfo& wheel = m_wheelInfo[wheelIndex];
|
||||
return wheel.m_worldTransform;
|
||||
|
||||
}
|
||||
|
||||
void btRaycastVehicle::updateWheelTransform( int wheelIndex , bool interpolatedTransform)
|
||||
{
|
||||
|
||||
btWheelInfo& wheel = m_wheelInfo[ wheelIndex ];
|
||||
updateWheelTransformsWS(wheel,interpolatedTransform);
|
||||
btVector3 up = -wheel.m_raycastInfo.m_wheelDirectionWS;
|
||||
const btVector3& right = wheel.m_raycastInfo.m_wheelAxleWS;
|
||||
btVector3 fwd = up.cross(right);
|
||||
fwd = fwd.normalize();
|
||||
// up = right.cross(fwd);
|
||||
// up.normalize();
|
||||
|
||||
//rotate around steering over de wheelAxleWS
|
||||
btScalar steering = wheel.m_steering;
|
||||
|
||||
btQuaternion steeringOrn(up,steering);//wheel.m_steering);
|
||||
btMatrix3x3 steeringMat(steeringOrn);
|
||||
|
||||
btQuaternion rotatingOrn(right,-wheel.m_rotation);
|
||||
btMatrix3x3 rotatingMat(rotatingOrn);
|
||||
|
||||
btMatrix3x3 basis2(
|
||||
right[0],fwd[0],up[0],
|
||||
right[1],fwd[1],up[1],
|
||||
right[2],fwd[2],up[2]
|
||||
);
|
||||
|
||||
wheel.m_worldTransform.setBasis(steeringMat * rotatingMat * basis2);
|
||||
wheel.m_worldTransform.setOrigin(
|
||||
wheel.m_raycastInfo.m_hardPointWS + wheel.m_raycastInfo.m_wheelDirectionWS * wheel.m_raycastInfo.m_suspensionLength
|
||||
);
|
||||
}
|
||||
|
||||
void btRaycastVehicle::resetSuspension()
|
||||
{
|
||||
|
||||
int i;
|
||||
for (i=0;i<m_wheelInfo.size(); i++)
|
||||
{
|
||||
btWheelInfo& wheel = m_wheelInfo[i];
|
||||
wheel.m_raycastInfo.m_suspensionLength = wheel.getSuspensionRestLength();
|
||||
wheel.m_suspensionRelativeVelocity = btScalar(0.0);
|
||||
|
||||
wheel.m_raycastInfo.m_contactNormalWS = - wheel.m_raycastInfo.m_wheelDirectionWS;
|
||||
//wheel_info.setContactFriction(btScalar(0.0));
|
||||
wheel.m_clippedInvContactDotSuspension = btScalar(1.0);
|
||||
}
|
||||
}
|
||||
|
||||
void btRaycastVehicle::updateWheelTransformsWS(btWheelInfo& wheel , bool interpolatedTransform)
|
||||
{
|
||||
wheel.m_raycastInfo.m_isInContact = false;
|
||||
|
||||
btTransform chassisTrans = getChassisWorldTransform();
|
||||
if (interpolatedTransform && (getRigidBody()->getMotionState()))
|
||||
{
|
||||
getRigidBody()->getMotionState()->getWorldTransform(chassisTrans);
|
||||
}
|
||||
|
||||
wheel.m_raycastInfo.m_hardPointWS = chassisTrans( wheel.m_chassisConnectionPointCS );
|
||||
wheel.m_raycastInfo.m_wheelDirectionWS = chassisTrans.getBasis() * wheel.m_wheelDirectionCS ;
|
||||
wheel.m_raycastInfo.m_wheelAxleWS = chassisTrans.getBasis() * wheel.m_wheelAxleCS;
|
||||
}
|
||||
|
||||
btScalar btRaycastVehicle::rayCast(btWheelInfo& wheel)
|
||||
{
|
||||
updateWheelTransformsWS( wheel,false);
|
||||
|
||||
|
||||
btScalar depth = -1;
|
||||
|
||||
btScalar raylen = wheel.getSuspensionRestLength()+wheel.m_wheelsRadius;
|
||||
|
||||
btVector3 rayvector = wheel.m_raycastInfo.m_wheelDirectionWS * (raylen);
|
||||
const btVector3& source = wheel.m_raycastInfo.m_hardPointWS;
|
||||
wheel.m_raycastInfo.m_contactPointWS = source + rayvector;
|
||||
const btVector3& target = wheel.m_raycastInfo.m_contactPointWS;
|
||||
|
||||
btScalar param = btScalar(0.);
|
||||
|
||||
btVehicleRaycaster::btVehicleRaycasterResult rayResults;
|
||||
|
||||
assert(m_vehicleRaycaster);
|
||||
|
||||
void* object = m_vehicleRaycaster->castRay(source,target,rayResults);
|
||||
|
||||
wheel.m_raycastInfo.m_groundObject = 0;
|
||||
|
||||
if (object)
|
||||
{
|
||||
param = rayResults.m_distFraction;
|
||||
depth = raylen * rayResults.m_distFraction;
|
||||
wheel.m_raycastInfo.m_contactNormalWS = rayResults.m_hitNormalInWorld;
|
||||
wheel.m_raycastInfo.m_isInContact = true;
|
||||
|
||||
wheel.m_raycastInfo.m_groundObject = &s_fixedObject;//todo for driving on dynamic/movable objects!;
|
||||
//wheel.m_raycastInfo.m_groundObject = object;
|
||||
|
||||
|
||||
btScalar hitDistance = param*raylen;
|
||||
wheel.m_raycastInfo.m_suspensionLength = hitDistance - wheel.m_wheelsRadius;
|
||||
//clamp on max suspension travel
|
||||
|
||||
btScalar minSuspensionLength = wheel.getSuspensionRestLength() - wheel.m_maxSuspensionTravelCm*btScalar(0.01);
|
||||
btScalar maxSuspensionLength = wheel.getSuspensionRestLength()+ wheel.m_maxSuspensionTravelCm*btScalar(0.01);
|
||||
if (wheel.m_raycastInfo.m_suspensionLength < minSuspensionLength)
|
||||
{
|
||||
wheel.m_raycastInfo.m_suspensionLength = minSuspensionLength;
|
||||
}
|
||||
if (wheel.m_raycastInfo.m_suspensionLength > maxSuspensionLength)
|
||||
{
|
||||
wheel.m_raycastInfo.m_suspensionLength = maxSuspensionLength;
|
||||
}
|
||||
|
||||
wheel.m_raycastInfo.m_contactPointWS = rayResults.m_hitPointInWorld;
|
||||
|
||||
btScalar denominator= wheel.m_raycastInfo.m_contactNormalWS.dot( wheel.m_raycastInfo.m_wheelDirectionWS );
|
||||
|
||||
btVector3 chassis_velocity_at_contactPoint;
|
||||
btVector3 relpos = wheel.m_raycastInfo.m_contactPointWS-getRigidBody()->getCenterOfMassPosition();
|
||||
|
||||
chassis_velocity_at_contactPoint = getRigidBody()->getVelocityInLocalPoint(relpos);
|
||||
|
||||
btScalar projVel = wheel.m_raycastInfo.m_contactNormalWS.dot( chassis_velocity_at_contactPoint );
|
||||
|
||||
if ( denominator >= btScalar(-0.1))
|
||||
{
|
||||
wheel.m_suspensionRelativeVelocity = btScalar(0.0);
|
||||
wheel.m_clippedInvContactDotSuspension = btScalar(1.0) / btScalar(0.1);
|
||||
}
|
||||
else
|
||||
{
|
||||
btScalar inv = btScalar(-1.) / denominator;
|
||||
wheel.m_suspensionRelativeVelocity = projVel * inv;
|
||||
wheel.m_clippedInvContactDotSuspension = inv;
|
||||
}
|
||||
|
||||
} else
|
||||
{
|
||||
//put wheel info as in rest position
|
||||
wheel.m_raycastInfo.m_suspensionLength = wheel.getSuspensionRestLength();
|
||||
wheel.m_suspensionRelativeVelocity = btScalar(0.0);
|
||||
wheel.m_raycastInfo.m_contactNormalWS = - wheel.m_raycastInfo.m_wheelDirectionWS;
|
||||
wheel.m_clippedInvContactDotSuspension = btScalar(1.0);
|
||||
}
|
||||
|
||||
return depth;
|
||||
}
|
||||
|
||||
|
||||
const btTransform& btRaycastVehicle::getChassisWorldTransform() const
|
||||
{
|
||||
/*if (getRigidBody()->getMotionState())
|
||||
{
|
||||
btTransform chassisWorldTrans;
|
||||
getRigidBody()->getMotionState()->getWorldTransform(chassisWorldTrans);
|
||||
return chassisWorldTrans;
|
||||
}
|
||||
*/
|
||||
|
||||
|
||||
return getRigidBody()->getCenterOfMassTransform();
|
||||
}
|
||||
|
||||
|
||||
void btRaycastVehicle::updateVehicle( btScalar step )
|
||||
{
|
||||
{
|
||||
for (int i=0;i<getNumWheels();i++)
|
||||
{
|
||||
updateWheelTransform(i,false);
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
m_currentVehicleSpeedKmHour = btScalar(3.6) * getRigidBody()->getLinearVelocity().length();
|
||||
|
||||
const btTransform& chassisTrans = getChassisWorldTransform();
|
||||
|
||||
btVector3 forwardW (
|
||||
chassisTrans.getBasis()[0][m_indexForwardAxis],
|
||||
chassisTrans.getBasis()[1][m_indexForwardAxis],
|
||||
chassisTrans.getBasis()[2][m_indexForwardAxis]);
|
||||
|
||||
if (forwardW.dot(getRigidBody()->getLinearVelocity()) < btScalar(0.))
|
||||
{
|
||||
m_currentVehicleSpeedKmHour *= btScalar(-1.);
|
||||
}
|
||||
|
||||
//
|
||||
// simulate suspension
|
||||
//
|
||||
|
||||
int i=0;
|
||||
for (i=0;i<m_wheelInfo.size();i++)
|
||||
{
|
||||
btScalar depth;
|
||||
depth = rayCast( m_wheelInfo[i]);
|
||||
}
|
||||
|
||||
updateSuspension(step);
|
||||
|
||||
|
||||
for (i=0;i<m_wheelInfo.size();i++)
|
||||
{
|
||||
//apply suspension force
|
||||
btWheelInfo& wheel = m_wheelInfo[i];
|
||||
|
||||
btScalar suspensionForce = wheel.m_wheelsSuspensionForce;
|
||||
|
||||
btScalar gMaxSuspensionForce = btScalar(6000.);
|
||||
if (suspensionForce > gMaxSuspensionForce)
|
||||
{
|
||||
suspensionForce = gMaxSuspensionForce;
|
||||
}
|
||||
btVector3 impulse = wheel.m_raycastInfo.m_contactNormalWS * suspensionForce * step;
|
||||
btVector3 relpos = wheel.m_raycastInfo.m_contactPointWS - getRigidBody()->getCenterOfMassPosition();
|
||||
|
||||
getRigidBody()->applyImpulse(impulse, relpos);
|
||||
|
||||
}
|
||||
|
||||
|
||||
|
||||
updateFriction( step);
|
||||
|
||||
|
||||
for (i=0;i<m_wheelInfo.size();i++)
|
||||
{
|
||||
btWheelInfo& wheel = m_wheelInfo[i];
|
||||
btVector3 relpos = wheel.m_raycastInfo.m_hardPointWS - getRigidBody()->getCenterOfMassPosition();
|
||||
btVector3 vel = getRigidBody()->getVelocityInLocalPoint( relpos );
|
||||
|
||||
if (wheel.m_raycastInfo.m_isInContact)
|
||||
{
|
||||
const btTransform& chassisWorldTransform = getChassisWorldTransform();
|
||||
|
||||
btVector3 fwd (
|
||||
chassisWorldTransform.getBasis()[0][m_indexForwardAxis],
|
||||
chassisWorldTransform.getBasis()[1][m_indexForwardAxis],
|
||||
chassisWorldTransform.getBasis()[2][m_indexForwardAxis]);
|
||||
|
||||
btScalar proj = fwd.dot(wheel.m_raycastInfo.m_contactNormalWS);
|
||||
fwd -= wheel.m_raycastInfo.m_contactNormalWS * proj;
|
||||
|
||||
btScalar proj2 = fwd.dot(vel);
|
||||
|
||||
wheel.m_deltaRotation = (proj2 * step) / (wheel.m_wheelsRadius);
|
||||
wheel.m_rotation += wheel.m_deltaRotation;
|
||||
|
||||
} else
|
||||
{
|
||||
wheel.m_rotation += wheel.m_deltaRotation;
|
||||
}
|
||||
|
||||
wheel.m_deltaRotation *= btScalar(0.99);//damping of rotation when not in contact
|
||||
|
||||
}
|
||||
|
||||
|
||||
|
||||
}
|
||||
|
||||
|
||||
void btRaycastVehicle::setSteeringValue(btScalar steering,int wheel)
|
||||
{
|
||||
assert(wheel>=0 && wheel < getNumWheels());
|
||||
|
||||
btWheelInfo& wheelInfo = getWheelInfo(wheel);
|
||||
wheelInfo.m_steering = steering;
|
||||
}
|
||||
|
||||
|
||||
|
||||
btScalar btRaycastVehicle::getSteeringValue(int wheel) const
|
||||
{
|
||||
return getWheelInfo(wheel).m_steering;
|
||||
}
|
||||
|
||||
|
||||
void btRaycastVehicle::applyEngineForce(btScalar force, int wheel)
|
||||
{
|
||||
assert(wheel>=0 && wheel < getNumWheels());
|
||||
btWheelInfo& wheelInfo = getWheelInfo(wheel);
|
||||
wheelInfo.m_engineForce = force;
|
||||
}
|
||||
|
||||
|
||||
const btWheelInfo& btRaycastVehicle::getWheelInfo(int index) const
|
||||
{
|
||||
btAssert((index >= 0) && (index < getNumWheels()));
|
||||
|
||||
return m_wheelInfo[index];
|
||||
}
|
||||
|
||||
btWheelInfo& btRaycastVehicle::getWheelInfo(int index)
|
||||
{
|
||||
btAssert((index >= 0) && (index < getNumWheels()));
|
||||
|
||||
return m_wheelInfo[index];
|
||||
}
|
||||
|
||||
void btRaycastVehicle::setBrake(btScalar brake,int wheelIndex)
|
||||
{
|
||||
btAssert((wheelIndex >= 0) && (wheelIndex < getNumWheels()));
|
||||
getWheelInfo(wheelIndex).m_brake = brake;
|
||||
}
|
||||
|
||||
|
||||
void btRaycastVehicle::updateSuspension(btScalar deltaTime)
|
||||
{
|
||||
(void)deltaTime;
|
||||
|
||||
btScalar chassisMass = btScalar(1.) / m_chassisBody->getInvMass();
|
||||
|
||||
for (int w_it=0; w_it<getNumWheels(); w_it++)
|
||||
{
|
||||
btWheelInfo &wheel_info = m_wheelInfo[w_it];
|
||||
|
||||
if ( wheel_info.m_raycastInfo.m_isInContact )
|
||||
{
|
||||
btScalar force;
|
||||
// Spring
|
||||
{
|
||||
btScalar susp_length = wheel_info.getSuspensionRestLength();
|
||||
btScalar current_length = wheel_info.m_raycastInfo.m_suspensionLength;
|
||||
|
||||
btScalar length_diff = (susp_length - current_length);
|
||||
|
||||
force = wheel_info.m_suspensionStiffness
|
||||
* length_diff * wheel_info.m_clippedInvContactDotSuspension;
|
||||
}
|
||||
|
||||
// Damper
|
||||
{
|
||||
btScalar projected_rel_vel = wheel_info.m_suspensionRelativeVelocity;
|
||||
{
|
||||
btScalar susp_damping;
|
||||
if ( projected_rel_vel < btScalar(0.0) )
|
||||
{
|
||||
susp_damping = wheel_info.m_wheelsDampingCompression;
|
||||
}
|
||||
else
|
||||
{
|
||||
susp_damping = wheel_info.m_wheelsDampingRelaxation;
|
||||
}
|
||||
force -= susp_damping * projected_rel_vel;
|
||||
}
|
||||
}
|
||||
|
||||
// RESULT
|
||||
wheel_info.m_wheelsSuspensionForce = force * chassisMass;
|
||||
if (wheel_info.m_wheelsSuspensionForce < btScalar(0.))
|
||||
{
|
||||
wheel_info.m_wheelsSuspensionForce = btScalar(0.);
|
||||
}
|
||||
}
|
||||
else
|
||||
{
|
||||
wheel_info.m_wheelsSuspensionForce = btScalar(0.0);
|
||||
}
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
|
||||
struct btWheelContactPoint
|
||||
{
|
||||
btRigidBody* m_body0;
|
||||
btRigidBody* m_body1;
|
||||
btVector3 m_frictionPositionWorld;
|
||||
btVector3 m_frictionDirectionWorld;
|
||||
btScalar m_jacDiagABInv;
|
||||
btScalar m_maxImpulse;
|
||||
|
||||
|
||||
btWheelContactPoint(btRigidBody* body0,btRigidBody* body1,const btVector3& frictionPosWorld,const btVector3& frictionDirectionWorld, btScalar maxImpulse)
|
||||
:m_body0(body0),
|
||||
m_body1(body1),
|
||||
m_frictionPositionWorld(frictionPosWorld),
|
||||
m_frictionDirectionWorld(frictionDirectionWorld),
|
||||
m_maxImpulse(maxImpulse)
|
||||
{
|
||||
btScalar denom0 = body0->computeImpulseDenominator(frictionPosWorld,frictionDirectionWorld);
|
||||
btScalar denom1 = body1->computeImpulseDenominator(frictionPosWorld,frictionDirectionWorld);
|
||||
btScalar relaxation = 1.f;
|
||||
m_jacDiagABInv = relaxation/(denom0+denom1);
|
||||
}
|
||||
|
||||
|
||||
|
||||
};
|
||||
|
||||
btScalar calcRollingFriction(btWheelContactPoint& contactPoint)
|
||||
{
|
||||
|
||||
btScalar j1=0.f;
|
||||
|
||||
const btVector3& contactPosWorld = contactPoint.m_frictionPositionWorld;
|
||||
|
||||
btVector3 rel_pos1 = contactPosWorld - contactPoint.m_body0->getCenterOfMassPosition();
|
||||
btVector3 rel_pos2 = contactPosWorld - contactPoint.m_body1->getCenterOfMassPosition();
|
||||
|
||||
btScalar maxImpulse = contactPoint.m_maxImpulse;
|
||||
|
||||
btVector3 vel1 = contactPoint.m_body0->getVelocityInLocalPoint(rel_pos1);
|
||||
btVector3 vel2 = contactPoint.m_body1->getVelocityInLocalPoint(rel_pos2);
|
||||
btVector3 vel = vel1 - vel2;
|
||||
|
||||
btScalar vrel = contactPoint.m_frictionDirectionWorld.dot(vel);
|
||||
|
||||
// calculate j that moves us to zero relative velocity
|
||||
j1 = -vrel * contactPoint.m_jacDiagABInv;
|
||||
GEN_set_min(j1, maxImpulse);
|
||||
GEN_set_max(j1, -maxImpulse);
|
||||
|
||||
return j1;
|
||||
}
|
||||
|
||||
|
||||
|
||||
|
||||
btScalar sideFrictionStiffness2 = btScalar(1.0);
|
||||
void btRaycastVehicle::updateFriction(btScalar timeStep)
|
||||
{
|
||||
|
||||
//calculate the impulse, so that the wheels don't move sidewards
|
||||
int numWheel = getNumWheels();
|
||||
if (!numWheel)
|
||||
return;
|
||||
|
||||
|
||||
btVector3* forwardWS = new btVector3[numWheel];
|
||||
btVector3* axle = new btVector3[numWheel];
|
||||
btScalar* forwardImpulse = new btScalar[numWheel];
|
||||
btScalar* sideImpulse = new btScalar[numWheel];
|
||||
|
||||
int numWheelsOnGround = 0;
|
||||
|
||||
|
||||
//collapse all those loops into one!
|
||||
for (int i=0;i<getNumWheels();i++)
|
||||
{
|
||||
btWheelInfo& wheelInfo = m_wheelInfo[i];
|
||||
class btRigidBody* groundObject = (class btRigidBody*) wheelInfo.m_raycastInfo.m_groundObject;
|
||||
if (groundObject)
|
||||
numWheelsOnGround++;
|
||||
sideImpulse[i] = btScalar(0.);
|
||||
forwardImpulse[i] = btScalar(0.);
|
||||
|
||||
}
|
||||
|
||||
{
|
||||
|
||||
for (int i=0;i<getNumWheels();i++)
|
||||
{
|
||||
|
||||
btWheelInfo& wheelInfo = m_wheelInfo[i];
|
||||
|
||||
class btRigidBody* groundObject = (class btRigidBody*) wheelInfo.m_raycastInfo.m_groundObject;
|
||||
|
||||
if (groundObject)
|
||||
{
|
||||
|
||||
const btTransform& wheelTrans = getWheelTransformWS( i );
|
||||
|
||||
btMatrix3x3 wheelBasis0 = wheelTrans.getBasis();
|
||||
axle[i] = btVector3(
|
||||
wheelBasis0[0][m_indexRightAxis],
|
||||
wheelBasis0[1][m_indexRightAxis],
|
||||
wheelBasis0[2][m_indexRightAxis]);
|
||||
|
||||
const btVector3& surfNormalWS = wheelInfo.m_raycastInfo.m_contactNormalWS;
|
||||
btScalar proj = axle[i].dot(surfNormalWS);
|
||||
axle[i] -= surfNormalWS * proj;
|
||||
axle[i] = axle[i].normalize();
|
||||
|
||||
forwardWS[i] = surfNormalWS.cross(axle[i]);
|
||||
forwardWS[i].normalize();
|
||||
|
||||
|
||||
resolveSingleBilateral(*m_chassisBody, wheelInfo.m_raycastInfo.m_contactPointWS,
|
||||
*groundObject, wheelInfo.m_raycastInfo.m_contactPointWS,
|
||||
btScalar(0.), axle[i],sideImpulse[i],timeStep);
|
||||
|
||||
sideImpulse[i] *= sideFrictionStiffness2;
|
||||
|
||||
}
|
||||
|
||||
|
||||
}
|
||||
}
|
||||
|
||||
btScalar sideFactor = btScalar(1.);
|
||||
btScalar fwdFactor = 0.5;
|
||||
|
||||
bool sliding = false;
|
||||
{
|
||||
for (int wheel =0;wheel <getNumWheels();wheel++)
|
||||
{
|
||||
btWheelInfo& wheelInfo = m_wheelInfo[wheel];
|
||||
class btRigidBody* groundObject = (class btRigidBody*) wheelInfo.m_raycastInfo.m_groundObject;
|
||||
|
||||
btScalar rollingFriction = 0.f;
|
||||
|
||||
if (groundObject)
|
||||
{
|
||||
if (wheelInfo.m_engineForce != 0.f)
|
||||
{
|
||||
rollingFriction = wheelInfo.m_engineForce* timeStep;
|
||||
} else
|
||||
{
|
||||
btScalar defaultRollingFrictionImpulse = 0.f;
|
||||
btScalar maxImpulse = wheelInfo.m_brake ? wheelInfo.m_brake : defaultRollingFrictionImpulse;
|
||||
btWheelContactPoint contactPt(m_chassisBody,groundObject,wheelInfo.m_raycastInfo.m_contactPointWS,forwardWS[wheel],maxImpulse);
|
||||
rollingFriction = calcRollingFriction(contactPt);
|
||||
}
|
||||
}
|
||||
|
||||
//switch between active rolling (throttle), braking and non-active rolling friction (no throttle/break)
|
||||
|
||||
|
||||
|
||||
|
||||
forwardImpulse[wheel] = btScalar(0.);
|
||||
m_wheelInfo[wheel].m_skidInfo= btScalar(1.);
|
||||
|
||||
if (groundObject)
|
||||
{
|
||||
m_wheelInfo[wheel].m_skidInfo= btScalar(1.);
|
||||
|
||||
btScalar maximp = wheelInfo.m_wheelsSuspensionForce * timeStep * wheelInfo.m_frictionSlip;
|
||||
btScalar maximpSide = maximp;
|
||||
|
||||
btScalar maximpSquared = maximp * maximpSide;
|
||||
|
||||
|
||||
forwardImpulse[wheel] = rollingFriction;//wheelInfo.m_engineForce* timeStep;
|
||||
|
||||
btScalar x = (forwardImpulse[wheel] ) * fwdFactor;
|
||||
btScalar y = (sideImpulse[wheel] ) * sideFactor;
|
||||
|
||||
btScalar impulseSquared = (x*x + y*y);
|
||||
|
||||
if (impulseSquared > maximpSquared)
|
||||
{
|
||||
sliding = true;
|
||||
|
||||
btScalar factor = maximp / btSqrt(impulseSquared);
|
||||
|
||||
m_wheelInfo[wheel].m_skidInfo *= factor;
|
||||
}
|
||||
}
|
||||
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
|
||||
|
||||
if (sliding)
|
||||
{
|
||||
for (int wheel = 0;wheel < getNumWheels(); wheel++)
|
||||
{
|
||||
if (sideImpulse[wheel] != btScalar(0.))
|
||||
{
|
||||
if (m_wheelInfo[wheel].m_skidInfo< btScalar(1.))
|
||||
{
|
||||
forwardImpulse[wheel] *= m_wheelInfo[wheel].m_skidInfo;
|
||||
sideImpulse[wheel] *= m_wheelInfo[wheel].m_skidInfo;
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
// apply the impulses
|
||||
{
|
||||
for (int wheel = 0;wheel<getNumWheels() ; wheel++)
|
||||
{
|
||||
btWheelInfo& wheelInfo = m_wheelInfo[wheel];
|
||||
|
||||
btVector3 rel_pos = wheelInfo.m_raycastInfo.m_contactPointWS -
|
||||
m_chassisBody->getCenterOfMassPosition();
|
||||
|
||||
if (forwardImpulse[wheel] != btScalar(0.))
|
||||
{
|
||||
m_chassisBody->applyImpulse(forwardWS[wheel]*(forwardImpulse[wheel]),rel_pos);
|
||||
}
|
||||
if (sideImpulse[wheel] != btScalar(0.))
|
||||
{
|
||||
class btRigidBody* groundObject = (class btRigidBody*) m_wheelInfo[wheel].m_raycastInfo.m_groundObject;
|
||||
|
||||
btVector3 rel_pos2 = wheelInfo.m_raycastInfo.m_contactPointWS -
|
||||
groundObject->getCenterOfMassPosition();
|
||||
|
||||
|
||||
btVector3 sideImp = axle[wheel] * sideImpulse[wheel];
|
||||
|
||||
rel_pos[2] *= wheelInfo.m_rollInfluence;
|
||||
m_chassisBody->applyImpulse(sideImp,rel_pos);
|
||||
|
||||
//apply friction impulse on the ground
|
||||
groundObject->applyImpulse(-sideImp,rel_pos2);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
delete []forwardWS;
|
||||
delete [] axle;
|
||||
delete[]forwardImpulse;
|
||||
delete[] sideImpulse;
|
||||
}
|
||||
|
||||
|
||||
void* btDefaultVehicleRaycaster::castRay(const btVector3& from,const btVector3& to, btVehicleRaycasterResult& result)
|
||||
{
|
||||
// RayResultCallback& resultCallback;
|
||||
|
||||
btCollisionWorld::ClosestRayResultCallback rayCallback(from,to);
|
||||
|
||||
m_dynamicsWorld->rayTest(from, to, rayCallback);
|
||||
|
||||
if (rayCallback.HasHit())
|
||||
{
|
||||
|
||||
btRigidBody* body = btRigidBody::upcast(rayCallback.m_collisionObject);
|
||||
if (body)
|
||||
{
|
||||
result.m_hitPointInWorld = rayCallback.m_hitPointWorld;
|
||||
result.m_hitNormalInWorld = rayCallback.m_hitNormalWorld;
|
||||
result.m_hitNormalInWorld.normalize();
|
||||
result.m_distFraction = rayCallback.m_closestHitFraction;
|
||||
return body;
|
||||
}
|
||||
}
|
||||
return 0;
|
||||
}
|
||||
201
bullet/src/BulletDynamics/Vehicle/btRaycastVehicle.h
Normal file
201
bullet/src/BulletDynamics/Vehicle/btRaycastVehicle.h
Normal file
@ -0,0 +1,201 @@
|
||||
/*
|
||||
* Copyright (c) 2005 Erwin Coumans http://continuousphysics.com/Bullet/
|
||||
*
|
||||
* Permission to use, copy, modify, distribute and sell this software
|
||||
* and its documentation for any purpose is hereby granted without fee,
|
||||
* provided that the above copyright notice appear in all copies.
|
||||
* Erwin Coumans makes no representations about the suitability
|
||||
* of this software for any purpose.
|
||||
* It is provided "as is" without express or implied warranty.
|
||||
*/
|
||||
#ifndef RAYCASTVEHICLE_H
|
||||
#define RAYCASTVEHICLE_H
|
||||
|
||||
#include "BulletDynamics/Dynamics/btRigidBody.h"
|
||||
#include "BulletDynamics/ConstraintSolver/btTypedConstraint.h"
|
||||
#include "btVehicleRaycaster.h"
|
||||
class btDynamicsWorld;
|
||||
#include "LinearMath/btAlignedObjectArray.h"
|
||||
#include "btWheelInfo.h"
|
||||
|
||||
class btVehicleTuning;
|
||||
|
||||
///rayCast vehicle, very special constraint that turn a rigidbody into a vehicle.
|
||||
class btRaycastVehicle : public btTypedConstraint
|
||||
{
|
||||
public:
|
||||
class btVehicleTuning
|
||||
{
|
||||
public:
|
||||
|
||||
btVehicleTuning()
|
||||
:m_suspensionStiffness(btScalar(5.88)),
|
||||
m_suspensionCompression(btScalar(0.83)),
|
||||
m_suspensionDamping(btScalar(0.88)),
|
||||
m_maxSuspensionTravelCm(btScalar(500.)),
|
||||
m_frictionSlip(btScalar(10.5))
|
||||
{
|
||||
}
|
||||
btScalar m_suspensionStiffness;
|
||||
btScalar m_suspensionCompression;
|
||||
btScalar m_suspensionDamping;
|
||||
btScalar m_maxSuspensionTravelCm;
|
||||
btScalar m_frictionSlip;
|
||||
|
||||
};
|
||||
private:
|
||||
|
||||
btScalar m_tau;
|
||||
btScalar m_damping;
|
||||
btVehicleRaycaster* m_vehicleRaycaster;
|
||||
btScalar m_pitchControl;
|
||||
btScalar m_steeringValue;
|
||||
btScalar m_currentVehicleSpeedKmHour;
|
||||
|
||||
btRigidBody* m_chassisBody;
|
||||
|
||||
int m_indexRightAxis;
|
||||
int m_indexUpAxis;
|
||||
int m_indexForwardAxis;
|
||||
|
||||
void defaultInit(const btVehicleTuning& tuning);
|
||||
|
||||
public:
|
||||
|
||||
//constructor to create a car from an existing rigidbody
|
||||
btRaycastVehicle(const btVehicleTuning& tuning,btRigidBody* chassis, btVehicleRaycaster* raycaster );
|
||||
|
||||
virtual ~btRaycastVehicle() ;
|
||||
|
||||
|
||||
const btTransform& getChassisWorldTransform() const;
|
||||
|
||||
btScalar rayCast(btWheelInfo& wheel);
|
||||
|
||||
virtual void updateVehicle(btScalar step);
|
||||
|
||||
void resetSuspension();
|
||||
|
||||
btScalar getSteeringValue(int wheel) const;
|
||||
|
||||
void setSteeringValue(btScalar steering,int wheel);
|
||||
|
||||
|
||||
void applyEngineForce(btScalar force, int wheel);
|
||||
|
||||
const btTransform& getWheelTransformWS( int wheelIndex ) const;
|
||||
|
||||
void updateWheelTransform( int wheelIndex, bool interpolatedTransform = true );
|
||||
|
||||
void setRaycastWheelInfo( int wheelIndex , bool isInContact, const btVector3& hitPoint, const btVector3& hitNormal,btScalar depth);
|
||||
|
||||
btWheelInfo& addWheel( const btVector3& connectionPointCS0, const btVector3& wheelDirectionCS0,const btVector3& wheelAxleCS,btScalar suspensionRestLength,btScalar wheelRadius,const btVehicleTuning& tuning, bool isFrontWheel);
|
||||
|
||||
inline int getNumWheels() const {
|
||||
return int (m_wheelInfo.size());
|
||||
}
|
||||
|
||||
btAlignedObjectArray<btWheelInfo> m_wheelInfo;
|
||||
|
||||
|
||||
const btWheelInfo& getWheelInfo(int index) const;
|
||||
|
||||
btWheelInfo& getWheelInfo(int index);
|
||||
|
||||
void updateWheelTransformsWS(btWheelInfo& wheel , bool interpolatedTransform = true);
|
||||
|
||||
|
||||
void setBrake(btScalar brake,int wheelIndex);
|
||||
|
||||
void setPitchControl(btScalar pitch)
|
||||
{
|
||||
m_pitchControl = pitch;
|
||||
}
|
||||
|
||||
void updateSuspension(btScalar deltaTime);
|
||||
|
||||
void updateFriction(btScalar timeStep);
|
||||
|
||||
|
||||
|
||||
inline btRigidBody* getRigidBody()
|
||||
{
|
||||
return m_chassisBody;
|
||||
}
|
||||
|
||||
const btRigidBody* getRigidBody() const
|
||||
{
|
||||
return m_chassisBody;
|
||||
}
|
||||
|
||||
inline int getRightAxis() const
|
||||
{
|
||||
return m_indexRightAxis;
|
||||
}
|
||||
inline int getUpAxis() const
|
||||
{
|
||||
return m_indexUpAxis;
|
||||
}
|
||||
|
||||
inline int getForwardAxis() const
|
||||
{
|
||||
return m_indexForwardAxis;
|
||||
}
|
||||
|
||||
|
||||
///Worldspace forward vector
|
||||
btVector3 getForwardVector() const
|
||||
{
|
||||
const btTransform& chassisTrans = getChassisWorldTransform();
|
||||
|
||||
btVector3 forwardW (
|
||||
chassisTrans.getBasis()[0][m_indexForwardAxis],
|
||||
chassisTrans.getBasis()[1][m_indexForwardAxis],
|
||||
chassisTrans.getBasis()[2][m_indexForwardAxis]);
|
||||
|
||||
return forwardW;
|
||||
}
|
||||
|
||||
///Velocity of vehicle (positive if velocity vector has same direction as foward vector)
|
||||
btScalar getCurrentSpeedKmHour() const
|
||||
{
|
||||
return m_currentVehicleSpeedKmHour;
|
||||
}
|
||||
|
||||
virtual void setCoordinateSystem(int rightIndex,int upIndex,int forwardIndex)
|
||||
{
|
||||
m_indexRightAxis = rightIndex;
|
||||
m_indexUpAxis = upIndex;
|
||||
m_indexForwardAxis = forwardIndex;
|
||||
}
|
||||
|
||||
virtual void buildJacobian()
|
||||
{
|
||||
//not yet
|
||||
}
|
||||
|
||||
virtual void solveConstraint(btScalar timeStep)
|
||||
{
|
||||
(void)timeStep;
|
||||
//not yet
|
||||
}
|
||||
|
||||
|
||||
};
|
||||
|
||||
class btDefaultVehicleRaycaster : public btVehicleRaycaster
|
||||
{
|
||||
btDynamicsWorld* m_dynamicsWorld;
|
||||
public:
|
||||
btDefaultVehicleRaycaster(btDynamicsWorld* world)
|
||||
:m_dynamicsWorld(world)
|
||||
{
|
||||
}
|
||||
|
||||
virtual void* castRay(const btVector3& from,const btVector3& to, btVehicleRaycasterResult& result);
|
||||
|
||||
};
|
||||
|
||||
|
||||
#endif //RAYCASTVEHICLE_H
|
||||
|
||||
35
bullet/src/BulletDynamics/Vehicle/btVehicleRaycaster.h
Normal file
35
bullet/src/BulletDynamics/Vehicle/btVehicleRaycaster.h
Normal file
@ -0,0 +1,35 @@
|
||||
/*
|
||||
* Copyright (c) 2005 Erwin Coumans http://continuousphysics.com/Bullet/
|
||||
*
|
||||
* Permission to use, copy, modify, distribute and sell this software
|
||||
* and its documentation for any purpose is hereby granted without fee,
|
||||
* provided that the above copyright notice appear in all copies.
|
||||
* Erwin Coumans makes no representations about the suitability
|
||||
* of this software for any purpose.
|
||||
* It is provided "as is" without express or implied warranty.
|
||||
*/
|
||||
#ifndef VEHICLE_RAYCASTER_H
|
||||
#define VEHICLE_RAYCASTER_H
|
||||
|
||||
#include "LinearMath/btVector3.h"
|
||||
|
||||
/// btVehicleRaycaster is provides interface for between vehicle simulation and raycasting
|
||||
struct btVehicleRaycaster
|
||||
{
|
||||
virtual ~btVehicleRaycaster()
|
||||
{
|
||||
}
|
||||
struct btVehicleRaycasterResult
|
||||
{
|
||||
btVehicleRaycasterResult() :m_distFraction(btScalar(-1.)){};
|
||||
btVector3 m_hitPointInWorld;
|
||||
btVector3 m_hitNormalInWorld;
|
||||
btScalar m_distFraction;
|
||||
};
|
||||
|
||||
virtual void* castRay(const btVector3& from,const btVector3& to, btVehicleRaycasterResult& result) = 0;
|
||||
|
||||
};
|
||||
|
||||
#endif //VEHICLE_RAYCASTER_H
|
||||
|
||||
56
bullet/src/BulletDynamics/Vehicle/btWheelInfo.cpp
Normal file
56
bullet/src/BulletDynamics/Vehicle/btWheelInfo.cpp
Normal file
@ -0,0 +1,56 @@
|
||||
/*
|
||||
* Copyright (c) 2005 Erwin Coumans http://continuousphysics.com/Bullet/
|
||||
*
|
||||
* Permission to use, copy, modify, distribute and sell this software
|
||||
* and its documentation for any purpose is hereby granted without fee,
|
||||
* provided that the above copyright notice appear in all copies.
|
||||
* Erwin Coumans makes no representations about the suitability
|
||||
* of this software for any purpose.
|
||||
* It is provided "as is" without express or implied warranty.
|
||||
*/
|
||||
#include "btWheelInfo.h"
|
||||
#include "BulletDynamics/Dynamics/btRigidBody.h" // for pointvelocity
|
||||
|
||||
|
||||
btScalar btWheelInfo::getSuspensionRestLength() const
|
||||
{
|
||||
|
||||
return m_suspensionRestLength1;
|
||||
|
||||
}
|
||||
|
||||
void btWheelInfo::updateWheel(const btRigidBody& chassis,RaycastInfo& raycastInfo)
|
||||
{
|
||||
(void)raycastInfo;
|
||||
|
||||
|
||||
if (m_raycastInfo.m_isInContact)
|
||||
|
||||
{
|
||||
btScalar project= m_raycastInfo.m_contactNormalWS.dot( m_raycastInfo.m_wheelDirectionWS );
|
||||
btVector3 chassis_velocity_at_contactPoint;
|
||||
btVector3 relpos = m_raycastInfo.m_contactPointWS - chassis.getCenterOfMassPosition();
|
||||
chassis_velocity_at_contactPoint = chassis.getVelocityInLocalPoint( relpos );
|
||||
btScalar projVel = m_raycastInfo.m_contactNormalWS.dot( chassis_velocity_at_contactPoint );
|
||||
if ( project >= btScalar(-0.1))
|
||||
{
|
||||
m_suspensionRelativeVelocity = btScalar(0.0);
|
||||
m_clippedInvContactDotSuspension = btScalar(1.0) / btScalar(0.1);
|
||||
}
|
||||
else
|
||||
{
|
||||
btScalar inv = btScalar(-1.) / project;
|
||||
m_suspensionRelativeVelocity = projVel * inv;
|
||||
m_clippedInvContactDotSuspension = inv;
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
else // Not in contact : position wheel in a nice (rest length) position
|
||||
{
|
||||
m_raycastInfo.m_suspensionLength = this->getSuspensionRestLength();
|
||||
m_suspensionRelativeVelocity = btScalar(0.0);
|
||||
m_raycastInfo.m_contactNormalWS = -m_raycastInfo.m_wheelDirectionWS;
|
||||
m_clippedInvContactDotSuspension = btScalar(1.0);
|
||||
}
|
||||
}
|
||||
116
bullet/src/BulletDynamics/Vehicle/btWheelInfo.h
Normal file
116
bullet/src/BulletDynamics/Vehicle/btWheelInfo.h
Normal file
@ -0,0 +1,116 @@
|
||||
/*
|
||||
* Copyright (c) 2005 Erwin Coumans http://continuousphysics.com/Bullet/
|
||||
*
|
||||
* Permission to use, copy, modify, distribute and sell this software
|
||||
* and its documentation for any purpose is hereby granted without fee,
|
||||
* provided that the above copyright notice appear in all copies.
|
||||
* Erwin Coumans makes no representations about the suitability
|
||||
* of this software for any purpose.
|
||||
* It is provided "as is" without express or implied warranty.
|
||||
*/
|
||||
#ifndef WHEEL_INFO_H
|
||||
#define WHEEL_INFO_H
|
||||
|
||||
#include "LinearMath/btVector3.h"
|
||||
#include "LinearMath/btTransform.h"
|
||||
|
||||
class btRigidBody;
|
||||
|
||||
struct btWheelInfoConstructionInfo
|
||||
{
|
||||
btVector3 m_chassisConnectionCS;
|
||||
btVector3 m_wheelDirectionCS;
|
||||
btVector3 m_wheelAxleCS;
|
||||
btScalar m_suspensionRestLength;
|
||||
btScalar m_maxSuspensionTravelCm;
|
||||
btScalar m_wheelRadius;
|
||||
|
||||
btScalar m_suspensionStiffness;
|
||||
btScalar m_wheelsDampingCompression;
|
||||
btScalar m_wheelsDampingRelaxation;
|
||||
btScalar m_frictionSlip;
|
||||
bool m_bIsFrontWheel;
|
||||
|
||||
};
|
||||
|
||||
/// btWheelInfo contains information per wheel about friction and suspension.
|
||||
struct btWheelInfo
|
||||
{
|
||||
struct RaycastInfo
|
||||
{
|
||||
//set by raycaster
|
||||
btVector3 m_contactNormalWS;//contactnormal
|
||||
btVector3 m_contactPointWS;//raycast hitpoint
|
||||
btScalar m_suspensionLength;
|
||||
btVector3 m_hardPointWS;//raycast starting point
|
||||
btVector3 m_wheelDirectionWS; //direction in worldspace
|
||||
btVector3 m_wheelAxleWS; // axle in worldspace
|
||||
bool m_isInContact;
|
||||
void* m_groundObject; //could be general void* ptr
|
||||
};
|
||||
|
||||
RaycastInfo m_raycastInfo;
|
||||
|
||||
btTransform m_worldTransform;
|
||||
|
||||
btVector3 m_chassisConnectionPointCS; //const
|
||||
btVector3 m_wheelDirectionCS;//const
|
||||
btVector3 m_wheelAxleCS; // const or modified by steering
|
||||
btScalar m_suspensionRestLength1;//const
|
||||
btScalar m_maxSuspensionTravelCm;
|
||||
btScalar getSuspensionRestLength() const;
|
||||
btScalar m_wheelsRadius;//const
|
||||
btScalar m_suspensionStiffness;//const
|
||||
btScalar m_wheelsDampingCompression;//const
|
||||
btScalar m_wheelsDampingRelaxation;//const
|
||||
btScalar m_frictionSlip;
|
||||
btScalar m_steering;
|
||||
btScalar m_rotation;
|
||||
btScalar m_deltaRotation;
|
||||
btScalar m_rollInfluence;
|
||||
|
||||
btScalar m_engineForce;
|
||||
|
||||
btScalar m_brake;
|
||||
|
||||
bool m_bIsFrontWheel;
|
||||
|
||||
void* m_clientInfo;//can be used to store pointer to sync transforms...
|
||||
|
||||
btWheelInfo(btWheelInfoConstructionInfo& ci)
|
||||
|
||||
{
|
||||
|
||||
m_suspensionRestLength1 = ci.m_suspensionRestLength;
|
||||
m_maxSuspensionTravelCm = ci.m_maxSuspensionTravelCm;
|
||||
|
||||
m_wheelsRadius = ci.m_wheelRadius;
|
||||
m_suspensionStiffness = ci.m_suspensionStiffness;
|
||||
m_wheelsDampingCompression = ci.m_wheelsDampingCompression;
|
||||
m_wheelsDampingRelaxation = ci.m_wheelsDampingRelaxation;
|
||||
m_chassisConnectionPointCS = ci.m_chassisConnectionCS;
|
||||
m_wheelDirectionCS = ci.m_wheelDirectionCS;
|
||||
m_wheelAxleCS = ci.m_wheelAxleCS;
|
||||
m_frictionSlip = ci.m_frictionSlip;
|
||||
m_steering = btScalar(0.);
|
||||
m_engineForce = btScalar(0.);
|
||||
m_rotation = btScalar(0.);
|
||||
m_deltaRotation = btScalar(0.);
|
||||
m_brake = btScalar(0.);
|
||||
m_rollInfluence = btScalar(0.1);
|
||||
m_bIsFrontWheel = ci.m_bIsFrontWheel;
|
||||
|
||||
}
|
||||
|
||||
void updateWheel(const btRigidBody& chassis,RaycastInfo& raycastInfo);
|
||||
|
||||
btScalar m_clippedInvContactDotSuspension;
|
||||
btScalar m_suspensionRelativeVelocity;
|
||||
//calculated by suspension
|
||||
btScalar m_wheelsSuspensionForce;
|
||||
btScalar m_skidInfo;
|
||||
|
||||
};
|
||||
|
||||
#endif //WHEEL_INFO_H
|
||||
|
||||
Reference in New Issue
Block a user