Bart/bullet3
1
0
Fork 0
Code Issues Pull Requests Releases Wiki Activity

merged most of the changes from the branch into trunk, except for COLLADA, libxml and glut glitches.

Browse Source 
Still need to verify to make sure no unwanted renaming is introduced.
This commit is contained in:
ejcoumans
2006-09-27 20:43:51 +00:00
parent d1e9a885f3
commit eb23bb5c0c
263 changed files with 7528 additions and 6714 deletions
Download Patch File Download Diff File Expand all files Collapse all files

2
src/BulletDynamics/CMakeLists.txt
View File

@@ -11,6 +11,8 @@ ADD_LIBRARY(LibBulletDynamics
ConstraintSolver/btSequentialImpulseConstraintSolver.cpp
ConstraintSolver/btSolve2LinearConstraint.cpp
ConstraintSolver/btTypedConstraint.cpp
Dynamics/btDiscreteDynamicsWorld.cpp
Dynamics/btSimpleDynamicsWorld.cpp
Dynamics/btRigidBody.cpp
Vehicle/btRaycastVehicle.cpp
Vehicle/btWheelInfo.cpp

18
src/BulletDynamics/ConstraintSolver/btConstraintSolver.h
View File

@@ -16,22 +16,22 @@ subject to the following restrictions:
#ifndef CONSTRAINT_SOLVER_H
#define CONSTRAINT_SOLVER_H
class PersistentManifold;
class RigidBody;
class btPersistentManifold;
class btRigidBody;
struct ContactSolverInfo;
struct BroadphaseProxy;
class IDebugDraw;
struct btContactSolverInfo;
struct btBroadphaseProxy;
class btIDebugDraw;
/// ConstraintSolver provides solver interface
class ConstraintSolver
/// btConstraintSolver provides solver interface
class btConstraintSolver
{
public:
virtual ~ConstraintSolver() {}
virtual ~btConstraintSolver() {}
virtual float SolveGroup(PersistentManifold** manifold,int numManifolds,const ContactSolverInfo& info,class IDebugDraw* debugDrawer = 0) = 0;
virtual float SolveGroup(btPersistentManifold** manifold,int numManifolds,const btContactSolverInfo& info,class btIDebugDraw* debugDrawer = 0) = 0;
};

118
src/BulletDynamics/ConstraintSolver/btContactConstraint.cpp
View File

@@ -16,10 +16,10 @@ subject to the following restrictions:
#include "btContactConstraint.h"
#include "BulletDynamics/Dynamics/btRigidBody.h"
#include "LinearMath/SimdVector3.h"
#include "LinearMath/btVector3.h"
#include "btJacobianEntry.h"
#include "btContactSolverInfo.h"
#include "LinearMath/GenMinMax.h"
#include "LinearMath/btMinMax.h"
#include "BulletCollision/NarrowPhaseCollision/btManifoldPoint.h"
#define ASSERT2 assert
@@ -27,8 +27,8 @@ subject to the following restrictions:
//some values to find stable tresholds
float useGlobalSettingContacts = false;//true;
SimdScalar contactDamping = 0.2f;
SimdScalar contactTau = .02f;//0.02f;//*0.02f;
btScalar contactDamping = 0.2f;
btScalar contactTau = .02f;//0.02f;//*0.02f;
@@ -37,9 +37,9 @@ SimdScalar contactTau = .02f;//0.02f;//*0.02f;
//bilateral constraint between two dynamic objects
void resolveSingleBilateral(RigidBody& body1, const SimdVector3& pos1,
RigidBody& body2, const SimdVector3& pos2,
SimdScalar distance, const SimdVector3& normal,SimdScalar& impulse ,float timeStep)
void resolveSingleBilateral(btRigidBody& body1, const btVector3& pos1,
btRigidBody& body2, const btVector3& pos2,
btScalar distance, const btVector3& normal,btScalar& impulse ,float timeStep)
{
float normalLenSqr = normal.length2();
ASSERT2(fabs(normalLenSqr) < 1.1f);
@@ -48,24 +48,24 @@ void resolveSingleBilateral(RigidBody& body1, const SimdVector3& pos1,
impulse = 0.f;
return;
}
SimdVector3 rel_pos1 = pos1 - body1.getCenterOfMassPosition();
SimdVector3 rel_pos2 = pos2 - body2.getCenterOfMassPosition();
btVector3 rel_pos1 = pos1 - body1.getCenterOfMassPosition();
btVector3 rel_pos2 = pos2 - body2.getCenterOfMassPosition();
//this jacobian entry could be re-used for all iterations
SimdVector3 vel1 = body1.getVelocityInLocalPoint(rel_pos1);
SimdVector3 vel2 = body2.getVelocityInLocalPoint(rel_pos2);
SimdVector3 vel = vel1 - vel2;
btVector3 vel1 = body1.getVelocityInLocalPoint(rel_pos1);
btVector3 vel2 = body2.getVelocityInLocalPoint(rel_pos2);
btVector3 vel = vel1 - vel2;
JacobianEntry jac(body1.getCenterOfMassTransform().getBasis().transpose(),
btJacobianEntry jac(body1.getCenterOfMassTransform().getBasis().transpose(),
body2.getCenterOfMassTransform().getBasis().transpose(),
rel_pos1,rel_pos2,normal,body1.getInvInertiaDiagLocal(),body1.getInvMass(),
body2.getInvInertiaDiagLocal(),body2.getInvMass());
SimdScalar jacDiagAB = jac.getDiagonal();
SimdScalar jacDiagABInv = 1.f / jacDiagAB;
btScalar jacDiagAB = jac.getDiagonal();
btScalar jacDiagABInv = 1.f / jacDiagAB;
SimdScalar rel_vel = jac.getRelativeVelocity(
btScalar rel_vel = jac.getRelativeVelocity(
body1.getLinearVelocity(),
body1.getCenterOfMassTransform().getBasis().transpose() * body1.getAngularVelocity(),
body2.getLinearVelocity(),
@@ -78,10 +78,10 @@ void resolveSingleBilateral(RigidBody& body1, const SimdVector3& pos1,
#ifdef ONLY_USE_LINEAR_MASS
SimdScalar massTerm = 1.f / (body1.getInvMass() + body2.getInvMass());
btScalar massTerm = 1.f / (body1.getInvMass() + body2.getInvMass());
impulse = - contactDamping * rel_vel * massTerm;
#else
SimdScalar velocityImpulse = -contactDamping * rel_vel * jacDiagABInv;
btScalar velocityImpulse = -contactDamping * rel_vel * jacDiagABInv;
impulse = velocityImpulse;
#endif
}
@@ -92,33 +92,33 @@ void resolveSingleBilateral(RigidBody& body1, const SimdVector3& pos1,
//velocity + friction
//response between two dynamic objects with friction
float resolveSingleCollision(
RigidBody& body1,
RigidBody& body2,
ManifoldPoint& contactPoint,
const ContactSolverInfo& solverInfo
btRigidBody& body1,
btRigidBody& body2,
btManifoldPoint& contactPoint,
const btContactSolverInfo& solverInfo
)
{
const SimdVector3& pos1 = contactPoint.GetPositionWorldOnA();
const SimdVector3& pos2 = contactPoint.GetPositionWorldOnB();
const btVector3& pos1 = contactPoint.GetPositionWorldOnA();
const btVector3& pos2 = contactPoint.GetPositionWorldOnB();
// printf("distance=%f\n",distance);
const SimdVector3& normal = contactPoint.m_normalWorldOnB;
const btVector3& normal = contactPoint.m_normalWorldOnB;
SimdVector3 rel_pos1 = pos1 - body1.getCenterOfMassPosition();
SimdVector3 rel_pos2 = pos2 - body2.getCenterOfMassPosition();
btVector3 rel_pos1 = pos1 - body1.getCenterOfMassPosition();
btVector3 rel_pos2 = pos2 - body2.getCenterOfMassPosition();
SimdVector3 vel1 = body1.getVelocityInLocalPoint(rel_pos1);
SimdVector3 vel2 = body2.getVelocityInLocalPoint(rel_pos2);
SimdVector3 vel = vel1 - vel2;
SimdScalar rel_vel;
btVector3 vel1 = body1.getVelocityInLocalPoint(rel_pos1);
btVector3 vel2 = body2.getVelocityInLocalPoint(rel_pos2);
btVector3 vel = vel1 - vel2;
btScalar rel_vel;
rel_vel = normal.dot(vel);
SimdScalar Kfps = 1.f / solverInfo.m_timeStep ;
btScalar Kfps = 1.f / solverInfo.m_timeStep ;
float damping = solverInfo.m_damping ;
float Kerp = solverInfo.m_erp;
@@ -133,23 +133,23 @@ float resolveSingleCollision(
//printf("dist=%f\n",distance);
ConstraintPersistentData* cpd = (ConstraintPersistentData*) contactPoint.m_userPersistentData;
btConstraintPersistentData* cpd = (btConstraintPersistentData*) contactPoint.m_userPersistentData;
assert(cpd);
SimdScalar distance = cpd->m_penetration;//contactPoint.GetDistance();
btScalar distance = cpd->m_penetration;//contactPoint.GetDistance();
//distance = 0.f;
SimdScalar positionalError = Kcor *-distance;
btScalar positionalError = Kcor *-distance;
//jacDiagABInv;
SimdScalar velocityError = cpd->m_restitution - rel_vel;// * damping;
btScalar velocityError = cpd->m_restitution - rel_vel;// * damping;
SimdScalar penetrationImpulse = positionalError * cpd->m_jacDiagABInv;
btScalar penetrationImpulse = positionalError * cpd->m_jacDiagABInv;
SimdScalar velocityImpulse = velocityError * cpd->m_jacDiagABInv;
btScalar velocityImpulse = velocityError * cpd->m_jacDiagABInv;
SimdScalar normalImpulse = penetrationImpulse+velocityImpulse;
btScalar normalImpulse = penetrationImpulse+velocityImpulse;
// See Erin Catto's GDC 2006 paper: Clamp the accumulated impulse
float oldNormalImpulse = cpd->m_appliedImpulse;
@@ -166,26 +166,26 @@ float resolveSingleCollision(
float resolveSingleFriction(
RigidBody& body1,
RigidBody& body2,
ManifoldPoint& contactPoint,
const ContactSolverInfo& solverInfo
btRigidBody& body1,
btRigidBody& body2,
btManifoldPoint& contactPoint,
const btContactSolverInfo& solverInfo
)
{
const SimdVector3& pos1 = contactPoint.GetPositionWorldOnA();
const SimdVector3& pos2 = contactPoint.GetPositionWorldOnB();
const btVector3& pos1 = contactPoint.GetPositionWorldOnA();
const btVector3& pos2 = contactPoint.GetPositionWorldOnB();
SimdVector3 rel_pos1 = pos1 - body1.getCenterOfMassPosition();
SimdVector3 rel_pos2 = pos2 - body2.getCenterOfMassPosition();
btVector3 rel_pos1 = pos1 - body1.getCenterOfMassPosition();
btVector3 rel_pos2 = pos2 - body2.getCenterOfMassPosition();
ConstraintPersistentData* cpd = (ConstraintPersistentData*) contactPoint.m_userPersistentData;
btConstraintPersistentData* cpd = (btConstraintPersistentData*) contactPoint.m_userPersistentData;
assert(cpd);
float combinedFriction = cpd->m_friction;
SimdScalar limit = cpd->m_appliedImpulse * combinedFriction;
btScalar limit = cpd->m_appliedImpulse * combinedFriction;
//if (contactPoint.m_appliedImpulse>0.f)
//friction
{
@@ -193,14 +193,14 @@ float resolveSingleFriction(
{
// 1st tangent
SimdVector3 vel1 = body1.getVelocityInLocalPoint(rel_pos1);
SimdVector3 vel2 = body2.getVelocityInLocalPoint(rel_pos2);
SimdVector3 vel = vel1 - vel2;
btVector3 vel1 = body1.getVelocityInLocalPoint(rel_pos1);
btVector3 vel2 = body2.getVelocityInLocalPoint(rel_pos2);
btVector3 vel = vel1 - vel2;
SimdScalar vrel = cpd->m_frictionWorldTangential0.dot(vel);
btScalar vrel = cpd->m_frictionWorldTangential0.dot(vel);
// calculate j that moves us to zero relative velocity
SimdScalar j = -vrel * cpd->m_jacDiagABInvTangent0;
btScalar j = -vrel * cpd->m_jacDiagABInvTangent0;
float total = cpd->m_accumulatedTangentImpulse0 + j;
GEN_set_min(total, limit);
GEN_set_max(total, -limit);
@@ -213,14 +213,14 @@ float resolveSingleFriction(
{
// 2nd tangent
SimdVector3 vel1 = body1.getVelocityInLocalPoint(rel_pos1);
SimdVector3 vel2 = body2.getVelocityInLocalPoint(rel_pos2);
SimdVector3 vel = vel1 - vel2;
btVector3 vel1 = body1.getVelocityInLocalPoint(rel_pos1);
btVector3 vel2 = body2.getVelocityInLocalPoint(rel_pos2);
btVector3 vel = vel1 - vel2;
SimdScalar vrel = cpd->m_frictionWorldTangential1.dot(vel);
btScalar vrel = cpd->m_frictionWorldTangential1.dot(vel);
// calculate j that moves us to zero relative velocity
SimdScalar j = -vrel * cpd->m_jacDiagABInvTangent1;
btScalar j = -vrel * cpd->m_jacDiagABInvTangent1;
float total = cpd->m_accumulatedTangentImpulse1 + j;
GEN_set_min(total, limit);
GEN_set_max(total, -limit);

48
src/BulletDynamics/ConstraintSolver/btContactConstraint.h
View File

@@ -18,11 +18,11 @@ subject to the following restrictions:
//todo: make into a proper class working with the iterative constraint solver
class RigidBody;
#include "LinearMath/SimdVector3.h"
#include "LinearMath/SimdScalar.h"
struct ContactSolverInfo;
class ManifoldPoint;
class btRigidBody;
#include "LinearMath/btVector3.h"
#include "LinearMath/btScalar.h"
struct btContactSolverInfo;
class btManifoldPoint;
enum {
DEFAULT_CONTACT_SOLVER_TYPE=0,
@@ -33,14 +33,14 @@ enum {
};
typedef float (*ContactSolverFunc)(RigidBody& body1,
RigidBody& body2,
class ManifoldPoint& contactPoint,
const ContactSolverInfo& info);
typedef float (*ContactSolverFunc)(btRigidBody& body1,
btRigidBody& body2,
class btManifoldPoint& contactPoint,
const btContactSolverInfo& info);
struct ConstraintPersistentData
struct btConstraintPersistentData
{
inline ConstraintPersistentData()
inline btConstraintPersistentData()
:m_appliedImpulse(0.f),
m_prevAppliedImpulse(0.f),
m_accumulatedTangentImpulse0(0.f),
@@ -69,8 +69,8 @@ struct ConstraintPersistentData
float m_restitution;
float m_friction;
float m_penetration;
SimdVector3 m_frictionWorldTangential0;
SimdVector3 m_frictionWorldTangential1;
btVector3 m_frictionWorldTangential0;
btVector3 m_frictionWorldTangential1;
ContactSolverFunc m_contactSolverFunc;
ContactSolverFunc m_frictionSolverFunc;
@@ -79,25 +79,25 @@ struct ConstraintPersistentData
///bilateral constraint between two dynamic objects
///positive distance = separation, negative distance = penetration
void resolveSingleBilateral(RigidBody& body1, const SimdVector3& pos1,
RigidBody& body2, const SimdVector3& pos2,
SimdScalar distance, const SimdVector3& normal,SimdScalar& impulse ,float timeStep);
void resolveSingleBilateral(btRigidBody& body1, const btVector3& pos1,
btRigidBody& body2, const btVector3& pos2,
btScalar distance, const btVector3& normal,btScalar& impulse ,float timeStep);
///contact constraint resolution:
///calculate and apply impulse to satisfy non-penetration and non-negative relative velocity constraint
///positive distance = separation, negative distance = penetration
float resolveSingleCollision(
RigidBody& body1,
RigidBody& body2,
ManifoldPoint& contactPoint,
const ContactSolverInfo& info);
btRigidBody& body1,
btRigidBody& body2,
btManifoldPoint& contactPoint,
const btContactSolverInfo& info);
float resolveSingleFriction(
RigidBody& body1,
RigidBody& body2,
ManifoldPoint& contactPoint,
const ContactSolverInfo& solverInfo
btRigidBody& body1,
btRigidBody& body2,
btManifoldPoint& contactPoint,
const btContactSolverInfo& solverInfo
);
#endif //CONTACT_CONSTRAINT_H

4
src/BulletDynamics/ConstraintSolver/btContactSolverInfo.h
View File

@@ -17,10 +17,10 @@ subject to the following restrictions:
#define CONTACT_SOLVER_INFO
struct ContactSolverInfo
struct btContactSolverInfo
{
inline ContactSolverInfo()
inline btContactSolverInfo()
{
m_tau = 0.6f;
m_damping = 1.0f;

132
src/BulletDynamics/ConstraintSolver/btGeneric6DofConstraint.cpp
View File

@@ -17,18 +17,18 @@ subject to the following restrictions:
#include "btGeneric6DofConstraint.h"
#include "BulletDynamics/Dynamics/btRigidBody.h"
#include "BulletDynamics/Dynamics/btMassProps.h"
#include "LinearMath/SimdTransformUtil.h"
#include "LinearMath/btTransformUtil.h"
static const SimdScalar kSign[] = { 1.0f, -1.0f, 1.0f };
static const btScalar kSign[] = { 1.0f, -1.0f, 1.0f };
static const int kAxisA[] = { 1, 0, 0 };
static const int kAxisB[] = { 2, 2, 1 };
Generic6DofConstraint::Generic6DofConstraint()
btGeneric6DofConstraint::btGeneric6DofConstraint()
{
}
Generic6DofConstraint::Generic6DofConstraint(RigidBody& rbA, RigidBody& rbB, const SimdTransform& frameInA, const SimdTransform& frameInB)
: TypedConstraint(rbA, rbB)
btGeneric6DofConstraint::btGeneric6DofConstraint(btRigidBody& rbA, btRigidBody& rbB, const btTransform& frameInA, const btTransform& frameInB)
: btTypedConstraint(rbA, rbB)
, m_frameInA(frameInA)
, m_frameInB(frameInB)
{
@@ -46,18 +46,18 @@ Generic6DofConstraint::Generic6DofConstraint(RigidBody& rbA, RigidBody& rbB, con
}
void Generic6DofConstraint::BuildJacobian()
void btGeneric6DofConstraint::BuildJacobian()
{
SimdVector3 normal(0,0,0);
btVector3 normal(0,0,0);
const SimdVector3& pivotInA = m_frameInA.getOrigin();
const SimdVector3& pivotInB = m_frameInB.getOrigin();
const btVector3& pivotInA = m_frameInA.getOrigin();
const btVector3& pivotInB = m_frameInB.getOrigin();
SimdVector3 pivotAInW = m_rbA.getCenterOfMassTransform() * m_frameInA.getOrigin();
SimdVector3 pivotBInW = m_rbB.getCenterOfMassTransform() * m_frameInB.getOrigin();
btVector3 pivotAInW = m_rbA.getCenterOfMassTransform() * m_frameInA.getOrigin();
btVector3 pivotBInW = m_rbB.getCenterOfMassTransform() * m_frameInB.getOrigin();
SimdVector3 rel_pos1 = pivotAInW - m_rbA.getCenterOfMassPosition();
SimdVector3 rel_pos2 = pivotBInW - m_rbB.getCenterOfMassPosition();
btVector3 rel_pos1 = pivotAInW - m_rbA.getCenterOfMassPosition();
btVector3 rel_pos2 = pivotBInW - m_rbB.getCenterOfMassPosition();
int i;
//linear part
@@ -68,7 +68,7 @@ void Generic6DofConstraint::BuildJacobian()
normal[i] = 1;
// Create linear atom
new (&m_jacLinear[i]) JacobianEntry(
new (&m_jacLinear[i]) btJacobianEntry(
m_rbA.getCenterOfMassTransform().getBasis().transpose(),
m_rbB.getCenterOfMassTransform().getBasis().transpose(),
m_rbA.getCenterOfMassTransform()*pivotInA - m_rbA.getCenterOfMassPosition(),
@@ -80,7 +80,7 @@ void Generic6DofConstraint::BuildJacobian()
m_rbB.getInvMass());
// Apply accumulated impulse
SimdVector3 impulse_vector = m_accumulatedImpulse[i] * normal;
btVector3 impulse_vector = m_accumulatedImpulse[i] * normal;
m_rbA.applyImpulse( impulse_vector, rel_pos1);
m_rbB.applyImpulse(-impulse_vector, rel_pos2);
@@ -94,21 +94,21 @@ void Generic6DofConstraint::BuildJacobian()
{
if (isLimited(i+3))
{
SimdVector3 axisA = m_rbA.getCenterOfMassTransform().getBasis() * m_frameInA.getBasis().getColumn( kAxisA[i] );
SimdVector3 axisB = m_rbB.getCenterOfMassTransform().getBasis() * m_frameInB.getBasis().getColumn( kAxisB[i] );
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
SimdVector3 axis = kSign[i] * axisA.cross(axisB);
btVector3 axis = kSign[i] * axisA.cross(axisB);
// Create angular atom
new (&m_jacAng[i]) JacobianEntry(axis,
new (&m_jacAng[i]) btJacobianEntry(axis,
m_rbA.getCenterOfMassTransform().getBasis().transpose(),
m_rbB.getCenterOfMassTransform().getBasis().transpose(),
m_rbA.getInvInertiaDiagLocal(),
m_rbB.getInvInertiaDiagLocal());
// Apply accumulated impulse
SimdVector3 impulse_vector = m_accumulatedImpulse[i + 3] * axis;
btVector3 impulse_vector = m_accumulatedImpulse[i + 3] * axis;
m_rbA.applyTorqueImpulse( impulse_vector);
m_rbB.applyTorqueImpulse(-impulse_vector);
@@ -116,18 +116,18 @@ void Generic6DofConstraint::BuildJacobian()
}
}
void Generic6DofConstraint::SolveConstraint(SimdScalar timeStep)
void btGeneric6DofConstraint::SolveConstraint(btScalar timeStep)
{
SimdScalar tau = 0.1f;
SimdScalar damping = 1.0f;
btScalar tau = 0.1f;
btScalar damping = 1.0f;
SimdVector3 pivotAInW = m_rbA.getCenterOfMassTransform() * m_frameInA.getOrigin();
SimdVector3 pivotBInW = m_rbB.getCenterOfMassTransform() * m_frameInB.getOrigin();
btVector3 pivotAInW = m_rbA.getCenterOfMassTransform() * m_frameInA.getOrigin();
btVector3 pivotBInW = m_rbB.getCenterOfMassTransform() * m_frameInB.getOrigin();
SimdVector3 rel_pos1 = pivotAInW - m_rbA.getCenterOfMassPosition();
SimdVector3 rel_pos2 = pivotBInW - m_rbB.getCenterOfMassPosition();
btVector3 rel_pos1 = pivotAInW - m_rbA.getCenterOfMassPosition();
btVector3 rel_pos2 = pivotBInW - m_rbB.getCenterOfMassPosition();
SimdVector3 normal(0,0,0);
btVector3 normal(0,0,0);
int i;
// linear
@@ -135,24 +135,24 @@ void Generic6DofConstraint::SolveConstraint(SimdScalar timeStep)
{
if (isLimited(i))
{
SimdVector3 angvelA = m_rbA.getCenterOfMassTransform().getBasis().transpose() * m_rbA.getAngularVelocity();
SimdVector3 angvelB = m_rbB.getCenterOfMassTransform().getBasis().transpose() * m_rbB.getAngularVelocity();
btVector3 angvelA = m_rbA.getCenterOfMassTransform().getBasis().transpose() * m_rbA.getAngularVelocity();
btVector3 angvelB = m_rbB.getCenterOfMassTransform().getBasis().transpose() * m_rbB.getAngularVelocity();
normal[i] = 1;
SimdScalar jacDiagABInv = 1.f / m_jacLinear[i].getDiagonal();
btScalar jacDiagABInv = 1.f / m_jacLinear[i].getDiagonal();
//velocity error (first order error)
SimdScalar rel_vel = m_jacLinear[i].getRelativeVelocity(m_rbA.getLinearVelocity(),angvelA,
btScalar rel_vel = m_jacLinear[i].getRelativeVelocity(m_rbA.getLinearVelocity(),angvelA,
m_rbB.getLinearVelocity(),angvelB);
//positional error (zeroth order error)
SimdScalar depth = -(pivotAInW - pivotBInW).dot(normal);
btScalar depth = -(pivotAInW - pivotBInW).dot(normal);
SimdScalar impulse = (tau*depth/timeStep - damping*rel_vel) * jacDiagABInv;
btScalar impulse = (tau*depth/timeStep - damping*rel_vel) * jacDiagABInv;
m_accumulatedImpulse[i] += impulse;
SimdVector3 impulse_vector = normal * impulse;
btVector3 impulse_vector = normal * impulse;
m_rbA.applyImpulse( impulse_vector, rel_pos1);
m_rbB.applyImpulse(-impulse_vector, rel_pos2);
@@ -165,28 +165,28 @@ void Generic6DofConstraint::SolveConstraint(SimdScalar timeStep)
{
if (isLimited(i+3))
{
SimdVector3 angvelA = m_rbA.getCenterOfMassTransform().getBasis().transpose() * m_rbA.getAngularVelocity();
SimdVector3 angvelB = m_rbB.getCenterOfMassTransform().getBasis().transpose() * m_rbB.getAngularVelocity();
btVector3 angvelA = m_rbA.getCenterOfMassTransform().getBasis().transpose() * m_rbA.getAngularVelocity();
btVector3 angvelB = m_rbB.getCenterOfMassTransform().getBasis().transpose() * m_rbB.getAngularVelocity();
SimdScalar jacDiagABInv = 1.f / m_jacAng[i].getDiagonal();
btScalar jacDiagABInv = 1.f / m_jacAng[i].getDiagonal();
//velocity error (first order error)
SimdScalar rel_vel = m_jacAng[i].getRelativeVelocity(m_rbA.getLinearVelocity(),angvelA,
btScalar rel_vel = m_jacAng[i].getRelativeVelocity(m_rbA.getLinearVelocity(),angvelA,
m_rbB.getLinearVelocity(),angvelB);
//positional error (zeroth order error)
SimdVector3 axisA = m_rbA.getCenterOfMassTransform().getBasis() * m_frameInA.getBasis().getColumn( kAxisA[i] );
SimdVector3 axisB = m_rbB.getCenterOfMassTransform().getBasis() * m_frameInB.getBasis().getColumn( kAxisB[i] );
btVector3 axisA = m_rbA.getCenterOfMassTransform().getBasis() * m_frameInA.getBasis().getColumn( kAxisA[i] );
btVector3 axisB = m_rbB.getCenterOfMassTransform().getBasis() * m_frameInB.getBasis().getColumn( kAxisB[i] );
SimdScalar rel_pos = kSign[i] * axisA.dot(axisB);
btScalar rel_pos = kSign[i] * axisA.dot(axisB);
//impulse
SimdScalar impulse = -(tau*rel_pos/timeStep + damping*rel_vel) * jacDiagABInv;
btScalar impulse = -(tau*rel_pos/timeStep + damping*rel_vel) * jacDiagABInv;
m_accumulatedImpulse[i + 3] += impulse;
// Dirk: Not needed - we could actually project onto Jacobian entry here (same as above)
SimdVector3 axis = kSign[i] * axisA.cross(axisB);
SimdVector3 impulse_vector = axis * impulse;
btVector3 axis = kSign[i] * axisA.cross(axisB);
btVector3 impulse_vector = axis * impulse;
m_rbA.applyTorqueImpulse( impulse_vector);
m_rbB.applyTorqueImpulse(-impulse_vector);
@@ -194,53 +194,53 @@ void Generic6DofConstraint::SolveConstraint(SimdScalar timeStep)
}
}
void Generic6DofConstraint::UpdateRHS(SimdScalar timeStep)
void btGeneric6DofConstraint::UpdateRHS(btScalar timeStep)
{
}
SimdScalar Generic6DofConstraint::ComputeAngle(int axis) const
btScalar btGeneric6DofConstraint::ComputeAngle(int axis) const
{
SimdScalar angle;
btScalar angle;
switch (axis)
{
case 0:
{
SimdVector3 v1 = m_rbA.getCenterOfMassTransform().getBasis() * m_frameInA.getBasis().getColumn(1);
SimdVector3 v2 = m_rbB.getCenterOfMassTransform().getBasis() * m_frameInB.getBasis().getColumn(1);
SimdVector3 w2 = m_rbB.getCenterOfMassTransform().getBasis() * m_frameInB.getBasis().getColumn(2);
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);
SimdScalar s = v1.dot(w2);
SimdScalar c = v1.dot(v2);
btScalar s = v1.dot(w2);
btScalar c = v1.dot(v2);
angle = SimdAtan2( s, c );
angle = btAtan2( s, c );
}
break;
case 1:
{
SimdVector3 w1 = m_rbA.getCenterOfMassTransform().getBasis() * m_frameInA.getBasis().getColumn(2);
SimdVector3 w2 = m_rbB.getCenterOfMassTransform().getBasis() * m_frameInB.getBasis().getColumn(2);
SimdVector3 u2 = m_rbB.getCenterOfMassTransform().getBasis() * m_frameInB.getBasis().getColumn(0);
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);
SimdScalar s = w1.dot(u2);
SimdScalar c = w1.dot(w2);
btScalar s = w1.dot(u2);
btScalar c = w1.dot(w2);
angle = SimdAtan2( s, c );
angle = btAtan2( s, c );
}
break;
case 2:
{
SimdVector3 u1 = m_rbA.getCenterOfMassTransform().getBasis() * m_frameInA.getBasis().getColumn(0);
SimdVector3 u2 = m_rbB.getCenterOfMassTransform().getBasis() * m_frameInB.getBasis().getColumn(0);
SimdVector3 v2 = m_rbB.getCenterOfMassTransform().getBasis() * m_frameInB.getBasis().getColumn(1);
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);
SimdScalar s = u1.dot(v2);
SimdScalar c = u1.dot(u2);
btScalar s = u1.dot(v2);
btScalar c = u1.dot(u2);
angle = SimdAtan2( s, c );
angle = btAtan2( s, c );
}
break;
default: assert ( 0 ) ; break ;

48
src/BulletDynamics/ConstraintSolver/btGeneric6DofConstraint.h
View File

@@ -16,67 +16,67 @@ subject to the following restrictions:
#ifndef GENERIC_6DOF_CONSTRAINT_H
#define GENERIC_6DOF_CONSTRAINT_H
#include "LinearMath/SimdVector3.h"
#include "LinearMath/btVector3.h"
#include "BulletDynamics/ConstraintSolver/btJacobianEntry.h"
#include "btTypedConstraint.h"
class RigidBody;
class btRigidBody;
/// Generic6DofConstraint between two rigidbodies each with a pivotpoint that descibes the axis location in local space
/// Generic6DofConstraint can leave any of the 6 degree of freedom 'free' or 'locked'
/// 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 Generic6DofConstraint : public TypedConstraint
class btGeneric6DofConstraint : public btTypedConstraint
{
JacobianEntry m_jacLinear[3]; // 3 orthogonal linear constraints
JacobianEntry m_jacAng[3]; // 3 orthogonal angular constraints
btJacobianEntry m_jacLinear[3]; // 3 orthogonal linear constraints
btJacobianEntry m_jacAng[3]; // 3 orthogonal angular constraints
SimdTransform m_frameInA; // the constraint space w.r.t body A
SimdTransform m_frameInB; // the constraint space w.r.t body B
btTransform m_frameInA; // the constraint space w.r.t body A
btTransform m_frameInB; // the constraint space w.r.t body B
SimdScalar m_lowerLimit[6]; // the constraint lower limits
SimdScalar m_upperLimit[6]; // the constraint upper limits
btScalar m_lowerLimit[6]; // the constraint lower limits
btScalar m_upperLimit[6]; // the constraint upper limits
SimdScalar m_accumulatedImpulse[6];
btScalar m_accumulatedImpulse[6];
public:
Generic6DofConstraint(RigidBody& rbA, RigidBody& rbB, const SimdTransform& frameInA, const SimdTransform& frameInB );
btGeneric6DofConstraint(btRigidBody& rbA, btRigidBody& rbB, const btTransform& frameInA, const btTransform& frameInB );
Generic6DofConstraint();
btGeneric6DofConstraint();
virtual void BuildJacobian();
virtual void SolveConstraint(SimdScalar timeStep);
virtual void SolveConstraint(btScalar timeStep);
void UpdateRHS(SimdScalar timeStep);
void UpdateRHS(btScalar timeStep);
SimdScalar ComputeAngle(int axis) const;
btScalar ComputeAngle(int axis) const;
void setLinearLowerLimit(const SimdVector3& linearLower)
void setLinearLowerLimit(const btVector3& linearLower)
{
m_lowerLimit[0] = linearLower.getX();
m_lowerLimit[1] = linearLower.getY();
m_lowerLimit[2] = linearLower.getZ();
}
void setLinearUpperLimit(const SimdVector3& linearUpper)
void setLinearUpperLimit(const btVector3& linearUpper)
{
m_upperLimit[0] = linearUpper.getX();
m_upperLimit[1] = linearUpper.getY();
m_upperLimit[2] = linearUpper.getZ();
}
void setAngularLowerLimit(const SimdVector3& angularLower)
void setAngularLowerLimit(const btVector3& angularLower)
{
m_lowerLimit[3] = angularLower.getX();
m_lowerLimit[4] = angularLower.getY();
m_lowerLimit[5] = angularLower.getZ();
}
void setAngularUpperLimit(const SimdVector3& angularUpper)
void setAngularUpperLimit(const btVector3& angularUpper)
{
m_upperLimit[3] = angularUpper.getX();
m_upperLimit[4] = angularUpper.getY();
@@ -84,7 +84,7 @@ public:
}
//first 3 are linear, next 3 are angular
void SetLimit(int axis, SimdScalar lo, SimdScalar hi)
void SetLimit(int axis, btScalar lo, btScalar hi)
{
m_lowerLimit[axis] = lo;
m_upperLimit[axis] = hi;
@@ -99,11 +99,11 @@ public:
return (m_upperLimit[limitIndex] >= m_lowerLimit[limitIndex]);
}
const RigidBody& GetRigidBodyA() const
const btRigidBody& GetRigidBodyA() const
{
return m_rbA;
}
const RigidBody& GetRigidBodyB() const
const btRigidBody& GetRigidBodyB() const
{
return m_rbB;
}

146
src/BulletDynamics/ConstraintSolver/btHingeConstraint.cpp
View File

@@ -17,16 +17,16 @@ subject to the following restrictions:
#include "btHingeConstraint.h"
#include "BulletDynamics/Dynamics/btRigidBody.h"
#include "BulletDynamics/Dynamics/btMassProps.h"
#include "LinearMath/SimdTransformUtil.h"
#include "LinearMath/btTransformUtil.h"
HingeConstraint::HingeConstraint()
btHingeConstraint::btHingeConstraint()
{
}
HingeConstraint::HingeConstraint(RigidBody& rbA,RigidBody& rbB, const SimdVector3& pivotInA,const SimdVector3& pivotInB,
SimdVector3& axisInA,SimdVector3& axisInB)
:TypedConstraint(rbA,rbB),m_pivotInA(pivotInA),m_pivotInB(pivotInB),
btHingeConstraint::btHingeConstraint(btRigidBody& rbA,btRigidBody& rbB, const btVector3& pivotInA,const btVector3& pivotInB,
btVector3& axisInA,btVector3& axisInB)
:btTypedConstraint(rbA,rbB),m_pivotInA(pivotInA),m_pivotInB(pivotInB),
m_axisInA(axisInA),
m_axisInB(-axisInB),
m_angularOnly(false)
@@ -35,8 +35,8 @@ m_angularOnly(false)
}
HingeConstraint::HingeConstraint(RigidBody& rbA,const SimdVector3& pivotInA,SimdVector3& axisInA)
:TypedConstraint(rbA),m_pivotInA(pivotInA),m_pivotInB(rbA.getCenterOfMassTransform()(pivotInA)),
btHingeConstraint::btHingeConstraint(btRigidBody& rbA,const btVector3& pivotInA,btVector3& axisInA)
:btTypedConstraint(rbA),m_pivotInA(pivotInA),m_pivotInB(rbA.getCenterOfMassTransform()(pivotInA)),
m_axisInA(axisInA),
//fixed axis in worldspace
m_axisInB(rbA.getCenterOfMassTransform().getBasis() * -axisInA),
@@ -45,18 +45,18 @@ m_angularOnly(false)
}
void HingeConstraint::BuildJacobian()
void btHingeConstraint::BuildJacobian()
{
m_appliedImpulse = 0.f;
SimdVector3 normal(0,0,0);
btVector3 normal(0,0,0);
if (!m_angularOnly)
{
for (int i=0;i<3;i++)
{
normal[i] = 1;
new (&m_jac[i]) JacobianEntry(
new (&m_jac[i]) btJacobianEntry(
m_rbA.getCenterOfMassTransform().getBasis().transpose(),
m_rbB.getCenterOfMassTransform().getBasis().transpose(),
m_rbA.getCenterOfMassTransform()*m_pivotInA - m_rbA.getCenterOfMassPosition(),
@@ -74,18 +74,18 @@ void HingeConstraint::BuildJacobian()
//these two jointAxis require equal angular velocities for both bodies
//this is unused for now, it's a todo
SimdVector3 axisWorldA = GetRigidBodyA().getCenterOfMassTransform().getBasis() * m_axisInA;
SimdVector3 jointAxis0;
SimdVector3 jointAxis1;
SimdPlaneSpace1(axisWorldA,jointAxis0,jointAxis1);
btVector3 axisWorldA = GetRigidBodyA().getCenterOfMassTransform().getBasis() * m_axisInA;
btVector3 jointAxis0;
btVector3 jointAxis1;
btPlaneSpace1(axisWorldA,jointAxis0,jointAxis1);
new (&m_jacAng[0]) JacobianEntry(jointAxis0,
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]) JacobianEntry(jointAxis1,
new (&m_jacAng[1]) btJacobianEntry(jointAxis1,
m_rbA.getCenterOfMassTransform().getBasis().transpose(),
m_rbB.getCenterOfMassTransform().getBasis().transpose(),
m_rbA.getInvInertiaDiagLocal(),
@@ -94,52 +94,52 @@ void HingeConstraint::BuildJacobian()
}
void HingeConstraint::SolveConstraint(SimdScalar timeStep)
void btHingeConstraint::SolveConstraint(btScalar timeStep)
{
//#define NEW_IMPLEMENTATION
#ifdef NEW_IMPLEMENTATION
SimdScalar tau = 0.3f;
SimdScalar damping = 1.f;
btScalar tau = 0.3f;
btScalar damping = 1.f;
SimdVector3 pivotAInW = m_rbA.getCenterOfMassTransform()*m_pivotInA;
SimdVector3 pivotBInW = m_rbB.getCenterOfMassTransform()*m_pivotInB;
btVector3 pivotAInW = m_rbA.getCenterOfMassTransform()*m_pivotInA;
btVector3 pivotBInW = m_rbB.getCenterOfMassTransform()*m_pivotInB;
// Dirk: Don't we need to update this after each applied impulse
SimdVector3 angvelA; // = m_rbA.getCenterOfMassTransform().getBasis().transpose() * m_rbA.getAngularVelocity();
SimdVector3 angvelB; // = m_rbB.getCenterOfMassTransform().getBasis().transpose() * m_rbB.getAngularVelocity();
btVector3 angvelA; // = m_rbA.getCenterOfMassTransform().getBasis().transpose() * m_rbA.getAngularVelocity();
btVector3 angvelB; // = m_rbB.getCenterOfMassTransform().getBasis().transpose() * m_rbB.getAngularVelocity();
if (!m_angularOnly)
{
SimdVector3 normal(0,0,0);
btVector3 normal(0,0,0);
for (int i=0;i<3;i++)
{
normal[i] = 1;
SimdScalar jacDiagABInv = 1.f / m_jac[i].getDiagonal();
btScalar jacDiagABInv = 1.f / m_jac[i].getDiagonal();
SimdVector3 rel_pos1 = pivotAInW - m_rbA.getCenterOfMassPosition();
SimdVector3 rel_pos2 = pivotBInW - m_rbB.getCenterOfMassPosition();
btVector3 rel_pos1 = pivotAInW - m_rbA.getCenterOfMassPosition();
btVector3 rel_pos2 = pivotBInW - m_rbB.getCenterOfMassPosition();
SimdVector3 vel1 = m_rbA.getVelocityInLocalPoint(rel_pos1);
SimdVector3 vel2 = m_rbB.getVelocityInLocalPoint(rel_pos2);
SimdVector3 vel = vel1 - vel2;
btVector3 vel1 = m_rbA.getVelocityInLocalPoint(rel_pos1);
btVector3 vel2 = m_rbB.getVelocityInLocalPoint(rel_pos2);
btVector3 vel = vel1 - vel2;
// Dirk: Get new angular velocity since it changed after applying an impulse
angvelA = m_rbA.getCenterOfMassTransform().getBasis().transpose() * m_rbA.getAngularVelocity();
angvelB = m_rbB.getCenterOfMassTransform().getBasis().transpose() * m_rbB.getAngularVelocity();
//velocity error (first order error)
SimdScalar rel_vel = m_jac[i].getRelativeVelocity(m_rbA.getLinearVelocity(),angvelA,
btScalar rel_vel = m_jac[i].getRelativeVelocity(m_rbA.getLinearVelocity(),angvelA,
m_rbB.getLinearVelocity(),angvelB);
//positional error (zeroth order error)
SimdScalar depth = -(pivotAInW - pivotBInW).dot(normal);
btScalar depth = -(pivotAInW - pivotBInW).dot(normal);
SimdScalar impulse = tau*depth/timeStep * jacDiagABInv - damping * rel_vel * jacDiagABInv;
btScalar impulse = tau*depth/timeStep * jacDiagABInv - damping * rel_vel * jacDiagABInv;
SimdVector3 impulse_vector = normal * impulse;
btVector3 impulse_vector = normal * impulse;
m_rbA.applyImpulse( impulse_vector, pivotAInW - m_rbA.getCenterOfMassPosition());
m_rbB.applyImpulse(-impulse_vector, pivotBInW - m_rbB.getCenterOfMassPosition());
@@ -150,26 +150,26 @@ void HingeConstraint::SolveConstraint(SimdScalar timeStep)
///solve angular part
// get axes in world space
SimdVector3 axisA = GetRigidBodyA().getCenterOfMassTransform().getBasis() * m_axisInA;
SimdVector3 axisB = GetRigidBodyB().getCenterOfMassTransform().getBasis() * m_axisInB;
btVector3 axisA = GetRigidBodyA().getCenterOfMassTransform().getBasis() * m_axisInA;
btVector3 axisB = GetRigidBodyB().getCenterOfMassTransform().getBasis() * m_axisInB;
// constraint axes in world space
SimdVector3 jointAxis0;
SimdVector3 jointAxis1;
SimdPlaneSpace1(axisA,jointAxis0,jointAxis1);
btVector3 jointAxis0;
btVector3 jointAxis1;
btPlaneSpace1(axisA,jointAxis0,jointAxis1);
// Dirk: Get new angular velocity since it changed after applying an impulse
angvelA = m_rbA.getCenterOfMassTransform().getBasis().transpose() * m_rbA.getAngularVelocity();
angvelB = m_rbB.getCenterOfMassTransform().getBasis().transpose() * m_rbB.getAngularVelocity();
SimdScalar jacDiagABInv0 = 1.f / m_jacAng[0].getDiagonal();
SimdScalar rel_vel0 = m_jacAng[0].getRelativeVelocity(m_rbA.getLinearVelocity(),angvelA,
btScalar jacDiagABInv0 = 1.f / m_jacAng[0].getDiagonal();
btScalar rel_vel0 = m_jacAng[0].getRelativeVelocity(m_rbA.getLinearVelocity(),angvelA,
m_rbB.getLinearVelocity(),angvelB);
float tau1 = tau;//0.f;
SimdScalar impulse0 = (tau1 * axisB.dot(jointAxis1) / timeStep - damping * rel_vel0) * jacDiagABInv0;
SimdVector3 angular_impulse0 = jointAxis0 * impulse0;
btScalar impulse0 = (tau1 * axisB.dot(jointAxis1) / timeStep - damping * rel_vel0) * jacDiagABInv0;
btVector3 angular_impulse0 = jointAxis0 * impulse0;
m_rbA.applyTorqueImpulse( angular_impulse0);
m_rbB.applyTorqueImpulse(-angular_impulse0);
@@ -180,12 +180,12 @@ void HingeConstraint::SolveConstraint(SimdScalar timeStep)
angvelA = m_rbA.getCenterOfMassTransform().getBasis().transpose() * m_rbA.getAngularVelocity();
angvelB = m_rbB.getCenterOfMassTransform().getBasis().transpose() * m_rbB.getAngularVelocity();
SimdScalar jacDiagABInv1 = 1.f / m_jacAng[1].getDiagonal();
SimdScalar rel_vel1 = m_jacAng[1].getRelativeVelocity(m_rbA.getLinearVelocity(),angvelA,
btScalar jacDiagABInv1 = 1.f / m_jacAng[1].getDiagonal();
btScalar rel_vel1 = m_jacAng[1].getRelativeVelocity(m_rbA.getLinearVelocity(),angvelA,
m_rbB.getLinearVelocity(),angvelB);;
SimdScalar impulse1 = -(tau1 * axisB.dot(jointAxis0) / timeStep + damping * rel_vel1) * jacDiagABInv1;
SimdVector3 angular_impulse1 = jointAxis1 * impulse1;
btScalar impulse1 = -(tau1 * axisB.dot(jointAxis0) / timeStep + damping * rel_vel1) * jacDiagABInv1;
btVector3 angular_impulse1 = jointAxis1 * impulse1;
m_rbA.applyTorqueImpulse( angular_impulse1);
m_rbB.applyTorqueImpulse(-angular_impulse1);
@@ -193,33 +193,33 @@ void HingeConstraint::SolveConstraint(SimdScalar timeStep)
#else
SimdVector3 pivotAInW = m_rbA.getCenterOfMassTransform()*m_pivotInA;
SimdVector3 pivotBInW = m_rbB.getCenterOfMassTransform()*m_pivotInB;
btVector3 pivotAInW = m_rbA.getCenterOfMassTransform()*m_pivotInA;
btVector3 pivotBInW = m_rbB.getCenterOfMassTransform()*m_pivotInB;
SimdVector3 normal(0,0,0);
SimdScalar tau = 0.3f;
SimdScalar damping = 1.f;
btVector3 normal(0,0,0);
btScalar tau = 0.3f;
btScalar damping = 1.f;
//linear part
{
for (int i=0;i<3;i++)
{
normal[i] = 1;
SimdScalar jacDiagABInv = 1.f / m_jac[i].getDiagonal();
btScalar jacDiagABInv = 1.f / m_jac[i].getDiagonal();
SimdVector3 rel_pos1 = pivotAInW - m_rbA.getCenterOfMassPosition();
SimdVector3 rel_pos2 = pivotBInW - m_rbB.getCenterOfMassPosition();
btVector3 rel_pos1 = pivotAInW - m_rbA.getCenterOfMassPosition();
btVector3 rel_pos2 = pivotBInW - m_rbB.getCenterOfMassPosition();
SimdVector3 vel1 = m_rbA.getVelocityInLocalPoint(rel_pos1);
SimdVector3 vel2 = m_rbB.getVelocityInLocalPoint(rel_pos2);
SimdVector3 vel = vel1 - vel2;
SimdScalar rel_vel;
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);
//positional error (zeroth order error)
SimdScalar depth = -(pivotAInW - pivotBInW).dot(normal); //this is the error projected on the normal
SimdScalar impulse = depth*tau/timeStep * jacDiagABInv - damping * rel_vel * jacDiagABInv * damping;
btScalar depth = -(pivotAInW - pivotBInW).dot(normal); //this is the error projected on the normal
btScalar impulse = depth*tau/timeStep * jacDiagABInv - damping * rel_vel * jacDiagABInv * damping;
m_appliedImpulse += impulse;
SimdVector3 impulse_vector = normal * impulse;
btVector3 impulse_vector = normal * impulse;
m_rbA.applyImpulse(impulse_vector, pivotAInW - m_rbA.getCenterOfMassPosition());
m_rbB.applyImpulse(-impulse_vector, pivotBInW - m_rbB.getCenterOfMassPosition());
@@ -230,21 +230,21 @@ void HingeConstraint::SolveConstraint(SimdScalar timeStep)
///solve angular part
// get axes in world space
SimdVector3 axisA = GetRigidBodyA().getCenterOfMassTransform().getBasis() * m_axisInA;
SimdVector3 axisB = GetRigidBodyB().getCenterOfMassTransform().getBasis() * m_axisInB;
btVector3 axisA = GetRigidBodyA().getCenterOfMassTransform().getBasis() * m_axisInA;
btVector3 axisB = GetRigidBodyB().getCenterOfMassTransform().getBasis() * m_axisInB;
const SimdVector3& angVelA = GetRigidBodyA().getAngularVelocity();
const SimdVector3& angVelB = GetRigidBodyB().getAngularVelocity();
SimdVector3 angA = angVelA - axisA * axisA.dot(angVelA);
SimdVector3 angB = angVelB - axisB * axisB.dot(angVelB);
SimdVector3 velrel = angA-angB;
const btVector3& angVelA = GetRigidBodyA().getAngularVelocity();
const btVector3& angVelB = GetRigidBodyB().getAngularVelocity();
btVector3 angA = angVelA - axisA * axisA.dot(angVelA);
btVector3 angB = angVelB - axisB * axisB.dot(angVelB);
btVector3 velrel = angA-angB;
//solve angular velocity correction
float relaxation = 1.f;
float len = velrel.length();
if (len > 0.00001f)
{
SimdVector3 normal = velrel.normalized();
btVector3 normal = velrel.normalized();
float denom = GetRigidBodyA().ComputeAngularImpulseDenominator(normal) +
GetRigidBodyB().ComputeAngularImpulseDenominator(normal);
// scale for mass and relaxation
@@ -252,11 +252,11 @@ void HingeConstraint::SolveConstraint(SimdScalar timeStep)
}
//solve angular positional correction
SimdVector3 angularError = -axisA.cross(axisB) *(1.f/timeStep);
btVector3 angularError = -axisA.cross(axisB) *(1.f/timeStep);
float len2 = angularError.length();
if (len2>0.00001f)
{
SimdVector3 normal2 = angularError.normalized();
btVector3 normal2 = angularError.normalized();
float denom2 = GetRigidBodyA().ComputeAngularImpulseDenominator(normal2) +
GetRigidBodyB().ComputeAngularImpulseDenominator(normal2);
angularError *= (1.f/denom2) * relaxation;
@@ -269,7 +269,7 @@ void HingeConstraint::SolveConstraint(SimdScalar timeStep)
}
void HingeConstraint::UpdateRHS(SimdScalar timeStep)
void btHingeConstraint::UpdateRHS(btScalar timeStep)
{
}

32
src/BulletDynamics/ConstraintSolver/btHingeConstraint.h
View File

@@ -16,47 +16,47 @@ subject to the following restrictions:
#ifndef HINGECONSTRAINT_H
#define HINGECONSTRAINT_H
#include "LinearMath/SimdVector3.h"
#include "LinearMath/btVector3.h"
#include "BulletDynamics/ConstraintSolver/btJacobianEntry.h"
#include "btTypedConstraint.h"
class RigidBody;
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 HingeConstraint : public TypedConstraint
class btHingeConstraint : public btTypedConstraint
{
JacobianEntry m_jac[3]; //3 orthogonal linear constraints
JacobianEntry m_jacAng[2]; //2 orthogonal angular constraints
btJacobianEntry m_jac[3]; //3 orthogonal linear constraints
btJacobianEntry m_jacAng[2]; //2 orthogonal angular constraints
SimdVector3 m_pivotInA;
SimdVector3 m_pivotInB;
SimdVector3 m_axisInA;
SimdVector3 m_axisInB;
btVector3 m_pivotInA;
btVector3 m_pivotInB;
btVector3 m_axisInA;
btVector3 m_axisInB;
bool m_angularOnly;
public:
HingeConstraint(RigidBody& rbA,RigidBody& rbB, const SimdVector3& pivotInA,const SimdVector3& pivotInB,SimdVector3& axisInA,SimdVector3& axisInB);
btHingeConstraint(btRigidBody& rbA,btRigidBody& rbB, const btVector3& pivotInA,const btVector3& pivotInB,btVector3& axisInA,btVector3& axisInB);
HingeConstraint(RigidBody& rbA,const SimdVector3& pivotInA,SimdVector3& axisInA);
btHingeConstraint(btRigidBody& rbA,const btVector3& pivotInA,btVector3& axisInA);
HingeConstraint();
btHingeConstraint();
virtual void BuildJacobian();
virtual void SolveConstraint(SimdScalar timeStep);
virtual void SolveConstraint(btScalar timeStep);
void UpdateRHS(SimdScalar timeStep);
void UpdateRHS(btScalar timeStep);
const RigidBody& GetRigidBodyA() const
const btRigidBody& GetRigidBodyA() const
{
return m_rbA;
}
const RigidBody& GetRigidBodyB() const
const btRigidBody& GetRigidBodyB() const
{
return m_rbB;
}

110
src/BulletDynamics/ConstraintSolver/btJacobianEntry.h
View File

@@ -16,32 +16,32 @@ subject to the following restrictions:
#ifndef JACOBIAN_ENTRY_H
#define JACOBIAN_ENTRY_H
#include "LinearMath/SimdVector3.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 JacobianEntry memory layout 16 bytes
// 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 JacobianEntry
class btJacobianEntry
{
public:
JacobianEntry() {};
btJacobianEntry() {};
//constraint between two different rigidbodies
JacobianEntry(
const SimdMatrix3x3& world2A,
const SimdMatrix3x3& world2B,
const SimdVector3& rel_pos1,const SimdVector3& rel_pos2,
const SimdVector3& jointAxis,
const SimdVector3& inertiaInvA,
const SimdScalar massInvA,
const SimdVector3& inertiaInvB,
const SimdScalar massInvB)
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));
@@ -54,12 +54,12 @@ public:
}
//angular constraint between two different rigidbodies
JacobianEntry(const SimdVector3& jointAxis,
const SimdMatrix3x3& world2A,
const SimdMatrix3x3& world2B,
const SimdVector3& inertiaInvA,
const SimdVector3& inertiaInvB)
:m_linearJointAxis(SimdVector3(0.f,0.f,0.f))
btJacobianEntry(const btVector3& jointAxis,
const btMatrix3x3& world2A,
const btMatrix3x3& world2B,
const btVector3& inertiaInvA,
const btVector3& inertiaInvB)
:m_linearJointAxis(btVector3(0.f,0.f,0.f))
{
m_aJ= world2A*jointAxis;
m_bJ = world2B*-jointAxis;
@@ -71,11 +71,11 @@ public:
}
//angular constraint between two different rigidbodies
JacobianEntry(const SimdVector3& axisInA,
const SimdVector3& axisInB,
const SimdVector3& inertiaInvA,
const SimdVector3& inertiaInvB)
: m_linearJointAxis(SimdVector3(0.f,0.f,0.f))
btJacobianEntry(const btVector3& axisInA,
const btVector3& axisInB,
const btVector3& inertiaInvA,
const btVector3& inertiaInvB)
: m_linearJointAxis(btVector3(0.f,0.f,0.f))
, m_aJ(axisInA)
, m_bJ(-axisInB)
{
@@ -87,69 +87,69 @@ public:
}
//constraint on one rigidbody
JacobianEntry(
const SimdMatrix3x3& world2A,
const SimdVector3& rel_pos1,const SimdVector3& rel_pos2,
const SimdVector3& jointAxis,
const SimdVector3& inertiaInvA,
const SimdScalar massInvA)
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 = SimdVector3(0.f,0.f,0.f);
m_1MinvJt = btVector3(0.f,0.f,0.f);
m_Adiag = massInvA + m_0MinvJt.dot(m_aJ);
ASSERT(m_Adiag > 0.0f);
}
SimdScalar getDiagonal() const { return m_Adiag; }
btScalar getDiagonal() const { return m_Adiag; }
// for two constraints on the same rigidbody (for example vehicle friction)
SimdScalar getNonDiagonal(const JacobianEntry& jacB, const SimdScalar massInvA) const
btScalar getNonDiagonal(const btJacobianEntry& jacB, const btScalar massInvA) const
{
const JacobianEntry& jacA = *this;
SimdScalar lin = massInvA * jacA.m_linearJointAxis.dot(jacB.m_linearJointAxis);
SimdScalar ang = jacA.m_0MinvJt.dot(jacB.m_aJ);
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)
SimdScalar getNonDiagonal(const JacobianEntry& jacB,const SimdScalar massInvA,const SimdScalar massInvB) const
btScalar getNonDiagonal(const btJacobianEntry& jacB,const btScalar massInvA,const btScalar massInvB) const
{
const JacobianEntry& jacA = *this;
SimdVector3 lin = jacA.m_linearJointAxis * jacB.m_linearJointAxis;
SimdVector3 ang0 = jacA.m_0MinvJt * jacB.m_aJ;
SimdVector3 ang1 = jacA.m_1MinvJt * jacB.m_bJ;
SimdVector3 lin0 = massInvA * lin ;
SimdVector3 lin1 = massInvB * lin;
SimdVector3 sum = ang0+ang1+lin0+lin1;
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];
}
SimdScalar getRelativeVelocity(const SimdVector3& linvelA,const SimdVector3& angvelA,const SimdVector3& linvelB,const SimdVector3& angvelB)
btScalar getRelativeVelocity(const btVector3& linvelA,const btVector3& angvelA,const btVector3& linvelB,const btVector3& angvelB)
{
SimdVector3 linrel = linvelA - linvelB;
SimdVector3 angvela = angvelA * m_aJ;
SimdVector3 angvelb = angvelB * m_bJ;
btVector3 linrel = linvelA - linvelB;
btVector3 angvela = angvelA * m_aJ;
btVector3 angvelb = angvelB * m_bJ;
linrel *= m_linearJointAxis;
angvela += angvelb;
angvela += linrel;
SimdScalar rel_vel2 = angvela[0]+angvela[1]+angvela[2];
btScalar rel_vel2 = angvela[0]+angvela[1]+angvela[2];
return rel_vel2 + SIMD_EPSILON;
}
//private:
SimdVector3 m_linearJointAxis;
SimdVector3 m_aJ;
SimdVector3 m_bJ;
SimdVector3 m_0MinvJt;
SimdVector3 m_1MinvJt;
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
SimdScalar m_Adiag;
btScalar m_Adiag;
};

52
src/BulletDynamics/ConstraintSolver/btPoint2PointConstraint.cpp
View File

@@ -21,33 +21,33 @@ subject to the following restrictions:
Point2PointConstraint::Point2PointConstraint()
btPoint2PointConstraint::btPoint2PointConstraint()
{
}
Point2PointConstraint::Point2PointConstraint(RigidBody& rbA,RigidBody& rbB, const SimdVector3& pivotInA,const SimdVector3& pivotInB)
:TypedConstraint(rbA,rbB),m_pivotInA(pivotInA),m_pivotInB(pivotInB)
btPoint2PointConstraint::btPoint2PointConstraint(btRigidBody& rbA,btRigidBody& rbB, const btVector3& pivotInA,const btVector3& pivotInB)
:btTypedConstraint(rbA,rbB),m_pivotInA(pivotInA),m_pivotInB(pivotInB)
{
}
Point2PointConstraint::Point2PointConstraint(RigidBody& rbA,const SimdVector3& pivotInA)
:TypedConstraint(rbA),m_pivotInA(pivotInA),m_pivotInB(rbA.getCenterOfMassTransform()(pivotInA))
btPoint2PointConstraint::btPoint2PointConstraint(btRigidBody& rbA,const btVector3& pivotInA)
:btTypedConstraint(rbA),m_pivotInA(pivotInA),m_pivotInB(rbA.getCenterOfMassTransform()(pivotInA))
{
}
void Point2PointConstraint::BuildJacobian()
void btPoint2PointConstraint::BuildJacobian()
{
m_appliedImpulse = 0.f;
SimdVector3 normal(0,0,0);
btVector3 normal(0,0,0);
for (int i=0;i<3;i++)
{
normal[i] = 1;
new (&m_jac[i]) JacobianEntry(
new (&m_jac[i]) btJacobianEntry(
m_rbA.getCenterOfMassTransform().getBasis().transpose(),
m_rbB.getCenterOfMassTransform().getBasis().transpose(),
m_rbA.getCenterOfMassTransform()*m_pivotInA - m_rbA.getCenterOfMassPosition(),
@@ -62,46 +62,46 @@ void Point2PointConstraint::BuildJacobian()
}
void Point2PointConstraint::SolveConstraint(SimdScalar timeStep)
void btPoint2PointConstraint::SolveConstraint(btScalar timeStep)
{
SimdVector3 pivotAInW = m_rbA.getCenterOfMassTransform()*m_pivotInA;
SimdVector3 pivotBInW = m_rbB.getCenterOfMassTransform()*m_pivotInB;
btVector3 pivotAInW = m_rbA.getCenterOfMassTransform()*m_pivotInA;
btVector3 pivotBInW = m_rbB.getCenterOfMassTransform()*m_pivotInB;
SimdVector3 normal(0,0,0);
btVector3 normal(0,0,0);
// SimdVector3 angvelA = m_rbA.getCenterOfMassTransform().getBasis().transpose() * m_rbA.getAngularVelocity();
// SimdVector3 angvelB = m_rbB.getCenterOfMassTransform().getBasis().transpose() * m_rbB.getAngularVelocity();
// 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;
SimdScalar jacDiagABInv = 1.f / m_jac[i].getDiagonal();
btScalar jacDiagABInv = 1.f / m_jac[i].getDiagonal();
SimdVector3 rel_pos1 = pivotAInW - m_rbA.getCenterOfMassPosition();
SimdVector3 rel_pos2 = pivotBInW - m_rbB.getCenterOfMassPosition();
btVector3 rel_pos1 = pivotAInW - m_rbA.getCenterOfMassPosition();
btVector3 rel_pos2 = pivotBInW - m_rbB.getCenterOfMassPosition();
//this jacobian entry could be re-used for all iterations
SimdVector3 vel1 = m_rbA.getVelocityInLocalPoint(rel_pos1);
SimdVector3 vel2 = m_rbB.getVelocityInLocalPoint(rel_pos2);
SimdVector3 vel = vel1 - vel2;
btVector3 vel1 = m_rbA.getVelocityInLocalPoint(rel_pos1);
btVector3 vel2 = m_rbB.getVelocityInLocalPoint(rel_pos2);
btVector3 vel = vel1 - vel2;
SimdScalar rel_vel;
btScalar rel_vel;
rel_vel = normal.dot(vel);
/*
//velocity error (first order error)
SimdScalar rel_vel = m_jac[i].getRelativeVelocity(m_rbA.getLinearVelocity(),angvelA,
btScalar rel_vel = m_jac[i].getRelativeVelocity(m_rbA.getLinearVelocity(),angvelA,
m_rbB.getLinearVelocity(),angvelB);
*/
//positional error (zeroth order error)
SimdScalar depth = -(pivotAInW - pivotBInW).dot(normal); //this is the error projected on the normal
btScalar depth = -(pivotAInW - pivotBInW).dot(normal); //this is the error projected on the normal
SimdScalar impulse = depth*m_setting.m_tau/timeStep * jacDiagABInv - m_setting.m_damping * rel_vel * jacDiagABInv;
btScalar impulse = depth*m_setting.m_tau/timeStep * jacDiagABInv - m_setting.m_damping * rel_vel * jacDiagABInv;
m_appliedImpulse+=impulse;
SimdVector3 impulse_vector = normal * impulse;
btVector3 impulse_vector = normal * impulse;
m_rbA.applyImpulse(impulse_vector, pivotAInW - m_rbA.getCenterOfMassPosition());
m_rbB.applyImpulse(-impulse_vector, pivotBInW - m_rbB.getCenterOfMassPosition());
@@ -109,7 +109,7 @@ void Point2PointConstraint::SolveConstraint(SimdScalar timeStep)
}
}
void Point2PointConstraint::UpdateRHS(SimdScalar timeStep)
void btPoint2PointConstraint::UpdateRHS(btScalar timeStep)
{
}

32
src/BulletDynamics/ConstraintSolver/btPoint2PointConstraint.h
View File

@@ -16,16 +16,16 @@ subject to the following restrictions:
#ifndef POINT2POINTCONSTRAINT_H
#define POINT2POINTCONSTRAINT_H
#include "LinearMath/SimdVector3.h"
#include "LinearMath/btVector3.h"
#include "BulletDynamics/ConstraintSolver/btJacobianEntry.h"
#include "btTypedConstraint.h"
class RigidBody;
class btRigidBody;
struct ConstraintSetting
struct btConstraintSetting
{
ConstraintSetting() :
btConstraintSetting() :
m_tau(0.3f),
m_damping(1.f)
{
@@ -35,38 +35,38 @@ struct ConstraintSetting
};
/// point to point constraint between two rigidbodies each with a pivotpoint that descibes the 'ballsocket' location in local space
class Point2PointConstraint : public TypedConstraint
class btPoint2PointConstraint : public btTypedConstraint
{
JacobianEntry m_jac[3]; //3 orthogonal linear constraints
btJacobianEntry m_jac[3]; //3 orthogonal linear constraints
SimdVector3 m_pivotInA;
SimdVector3 m_pivotInB;
btVector3 m_pivotInA;
btVector3 m_pivotInB;
public:
ConstraintSetting m_setting;
btConstraintSetting m_setting;
Point2PointConstraint(RigidBody& rbA,RigidBody& rbB, const SimdVector3& pivotInA,const SimdVector3& pivotInB);
btPoint2PointConstraint(btRigidBody& rbA,btRigidBody& rbB, const btVector3& pivotInA,const btVector3& pivotInB);
Point2PointConstraint(RigidBody& rbA,const SimdVector3& pivotInA);
btPoint2PointConstraint(btRigidBody& rbA,const btVector3& pivotInA);
Point2PointConstraint();
btPoint2PointConstraint();
virtual void BuildJacobian();
virtual void SolveConstraint(SimdScalar timeStep);
virtual void SolveConstraint(btScalar timeStep);
void UpdateRHS(SimdScalar timeStep);
void UpdateRHS(btScalar timeStep);
void SetPivotA(const SimdVector3& pivotA)
void SetPivotA(const btVector3& pivotA)
{
m_pivotInA = pivotA;
}
void SetPivotB(const SimdVector3& pivotB)
void SetPivotB(const btVector3& pivotB)
{
m_pivotInB = pivotB;
}

94
src/BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolver.cpp
View File

@@ -20,12 +20,12 @@ subject to the following restrictions:
#include "btContactConstraint.h"
#include "btSolve2LinearConstraint.h"
#include "btContactSolverInfo.h"
#include "LinearMath/GenIDebugDraw.h"
#include "LinearMath/btIDebugDraw.h"
#include "btJacobianEntry.h"
#include "LinearMath/GenMinMax.h"
#include "LinearMath/btMinMax.h"
#ifdef USE_PROFILE
#include "LinearMath/GenQuickprof.h"
#include "LinearMath/btQuickprof.h"
#endif //USE_PROFILE
int totalCpd = 0;
@@ -35,7 +35,7 @@ int gTotalContactPoints = 0;
bool MyContactDestroyedCallback(void* userPersistentData)
{
assert (userPersistentData);
ConstraintPersistentData* cpd = (ConstraintPersistentData*)userPersistentData;
btConstraintPersistentData* cpd = (btConstraintPersistentData*)userPersistentData;
delete cpd;
totalCpd--;
//printf("totalCpd = %i. DELETED Ptr %x\n",totalCpd,userPersistentData);
@@ -43,7 +43,7 @@ bool MyContactDestroyedCallback(void* userPersistentData)
}
SequentialImpulseConstraintSolver::SequentialImpulseConstraintSolver()
btSequentialImpulseConstraintSolver::btSequentialImpulseConstraintSolver()
{
gContactDestroyedCallback = &MyContactDestroyedCallback;
@@ -58,15 +58,15 @@ SequentialImpulseConstraintSolver::SequentialImpulseConstraintSolver()
}
}
/// SequentialImpulseConstraintSolver Sequentially applies impulses
float SequentialImpulseConstraintSolver::SolveGroup(PersistentManifold** manifoldPtr, int numManifolds,const ContactSolverInfo& infoGlobal,IDebugDraw* debugDrawer)
/// btSequentialImpulseConstraintSolver Sequentially applies impulses
float btSequentialImpulseConstraintSolver::SolveGroup(btPersistentManifold** manifoldPtr, int numManifolds,const btContactSolverInfo& infoGlobal,btIDebugDraw* debugDrawer)
{
ContactSolverInfo info = infoGlobal;
btContactSolverInfo info = infoGlobal;
int numiter = infoGlobal.m_numIterations;
#ifdef USE_PROFILE
Profiler::beginBlock("Solve");
btProfiler::beginBlock("Solve");
#endif //USE_PROFILE
{
@@ -98,9 +98,9 @@ float SequentialImpulseConstraintSolver::SolveGroup(PersistentManifold** manifol
}
#ifdef USE_PROFILE
Profiler::endBlock("Solve");
btProfiler::endBlock("Solve");
Profiler::beginBlock("SolveFriction");
btProfiler::beginBlock("SolveFriction");
#endif //USE_PROFILE
//now solve the friction
@@ -116,7 +116,7 @@ float SequentialImpulseConstraintSolver::SolveGroup(PersistentManifold** manifol
}
}
#ifdef USE_PROFILE
Profiler::endBlock("SolveFriction");
btProfiler::endBlock("SolveFriction");
#endif //USE_PROFILE
return 0.f;
@@ -124,18 +124,18 @@ float SequentialImpulseConstraintSolver::SolveGroup(PersistentManifold** manifol
float penetrationResolveFactor = 0.9f;
SimdScalar restitutionCurve(SimdScalar rel_vel, SimdScalar restitution)
btScalar restitutionCurve(btScalar rel_vel, btScalar restitution)
{
SimdScalar rest = restitution * -rel_vel;
btScalar rest = restitution * -rel_vel;
return rest;
}
void SequentialImpulseConstraintSolver::PrepareConstraints(PersistentManifold* manifoldPtr, const ContactSolverInfo& info,IDebugDraw* debugDrawer)
void btSequentialImpulseConstraintSolver::PrepareConstraints(btPersistentManifold* manifoldPtr, const btContactSolverInfo& info,btIDebugDraw* debugDrawer)
{
RigidBody* body0 = (RigidBody*)manifoldPtr->GetBody0();
RigidBody* body1 = (RigidBody*)manifoldPtr->GetBody1();
btRigidBody* body0 = (btRigidBody*)manifoldPtr->GetBody0();
btRigidBody* body1 = (btRigidBody*)manifoldPtr->GetBody1();
//only necessary to refresh the manifold once (first iteration). The integration is done outside the loop
@@ -146,29 +146,29 @@ void SequentialImpulseConstraintSolver::PrepareConstraints(PersistentManifold* m
gTotalContactPoints += numpoints;
SimdVector3 color(0,1,0);
btVector3 color(0,1,0);
for (int i=0;i<numpoints ;i++)
{
ManifoldPoint& cp = manifoldPtr->GetContactPoint(i);
btManifoldPoint& cp = manifoldPtr->GetContactPoint(i);
if (cp.GetDistance() <= 0.f)
{
const SimdVector3& pos1 = cp.GetPositionWorldOnA();
const SimdVector3& pos2 = cp.GetPositionWorldOnB();
const btVector3& pos1 = cp.GetPositionWorldOnA();
const btVector3& pos2 = cp.GetPositionWorldOnB();
SimdVector3 rel_pos1 = pos1 - body0->getCenterOfMassPosition();
SimdVector3 rel_pos2 = pos2 - body1->getCenterOfMassPosition();
btVector3 rel_pos1 = pos1 - body0->getCenterOfMassPosition();
btVector3 rel_pos2 = pos2 - body1->getCenterOfMassPosition();
//this jacobian entry is re-used for all iterations
JacobianEntry jac(body0->getCenterOfMassTransform().getBasis().transpose(),
btJacobianEntry jac(body0->getCenterOfMassTransform().getBasis().transpose(),
body1->getCenterOfMassTransform().getBasis().transpose(),
rel_pos1,rel_pos2,cp.m_normalWorldOnB,body0->getInvInertiaDiagLocal(),body0->getInvMass(),
body1->getInvInertiaDiagLocal(),body1->getInvMass());
SimdScalar jacDiagAB = jac.getDiagonal();
btScalar jacDiagAB = jac.getDiagonal();
ConstraintPersistentData* cpd = (ConstraintPersistentData*) cp.m_userPersistentData;
btConstraintPersistentData* cpd = (btConstraintPersistentData*) cp.m_userPersistentData;
if (cpd)
{
//might be invalid
@@ -176,7 +176,7 @@ void SequentialImpulseConstraintSolver::PrepareConstraints(PersistentManifold* m
if (cpd->m_persistentLifeTime != cp.GetLifeTime())
{
//printf("Invalid: cpd->m_persistentLifeTime = %i cp.GetLifeTime() = %i\n",cpd->m_persistentLifeTime,cp.GetLifeTime());
new (cpd) ConstraintPersistentData;
new (cpd) btConstraintPersistentData;
cpd->m_persistentLifeTime = cp.GetLifeTime();
} else
@@ -187,7 +187,7 @@ void SequentialImpulseConstraintSolver::PrepareConstraints(PersistentManifold* m
} else
{
cpd = new ConstraintPersistentData();
cpd = new btConstraintPersistentData();
totalCpd ++;
//printf("totalCpd = %i Created Ptr %x\n",totalCpd,cpd);
cp.m_userPersistentData = cpd;
@@ -205,10 +205,10 @@ void SequentialImpulseConstraintSolver::PrepareConstraints(PersistentManifold* m
cpd->m_frictionSolverFunc = m_frictionDispatch[body0->m_frictionSolverType][body1->m_frictionSolverType];
cpd->m_contactSolverFunc = m_contactDispatch[body0->m_contactSolverType][body1->m_contactSolverType];
SimdVector3 vel1 = body0->getVelocityInLocalPoint(rel_pos1);
SimdVector3 vel2 = body1->getVelocityInLocalPoint(rel_pos2);
SimdVector3 vel = vel1 - vel2;
SimdScalar rel_vel;
btVector3 vel1 = body0->getVelocityInLocalPoint(rel_pos1);
btVector3 vel2 = body1->getVelocityInLocalPoint(rel_pos2);
btVector3 vel = vel1 - vel2;
btScalar rel_vel;
rel_vel = cp.m_normalWorldOnB.dot(vel);
float combinedRestitution = cp.m_combinedRestitution;
@@ -225,7 +225,7 @@ void SequentialImpulseConstraintSolver::PrepareConstraints(PersistentManifold* m
//restitution and penetration work in same direction so
//rel_vel
SimdScalar penVel = -cpd->m_penetration/info.m_timeStep;
btScalar penVel = -cpd->m_penetration/info.m_timeStep;
if (cpd->m_restitution >= penVel)
{
@@ -239,7 +239,7 @@ void SequentialImpulseConstraintSolver::PrepareConstraints(PersistentManifold* m
cpd->m_prevAppliedImpulse = cpd->m_appliedImpulse;
//re-calculate friction direction every frame, todo: check if this is really needed
SimdPlaneSpace1(cp.m_normalWorldOnB,cpd->m_frictionWorldTangential0,cpd->m_frictionWorldTangential1);
btPlaneSpace1(cp.m_normalWorldOnB,cpd->m_frictionWorldTangential0,cpd->m_frictionWorldTangential1);
#define NO_FRICTION_WARMSTART 1
@@ -259,7 +259,7 @@ void SequentialImpulseConstraintSolver::PrepareConstraints(PersistentManifold* m
denom = relaxation/(denom0+denom1);
cpd->m_jacDiagABInvTangent1 = denom;
SimdVector3 totalImpulse =
btVector3 totalImpulse =
#ifndef NO_FRICTION_WARMSTART
cp.m_frictionWorldTangential0*cp.m_accumulatedTangentImpulse0+
cp.m_frictionWorldTangential1*cp.m_accumulatedTangentImpulse1+
@@ -275,18 +275,18 @@ void SequentialImpulseConstraintSolver::PrepareConstraints(PersistentManifold* m
}
}
float SequentialImpulseConstraintSolver::Solve(PersistentManifold* manifoldPtr, const ContactSolverInfo& info,int iter,IDebugDraw* debugDrawer)
float btSequentialImpulseConstraintSolver::Solve(btPersistentManifold* manifoldPtr, const btContactSolverInfo& info,int iter,btIDebugDraw* debugDrawer)
{
RigidBody* body0 = (RigidBody*)manifoldPtr->GetBody0();
RigidBody* body1 = (RigidBody*)manifoldPtr->GetBody1();
btRigidBody* body0 = (btRigidBody*)manifoldPtr->GetBody0();
btRigidBody* body1 = (btRigidBody*)manifoldPtr->GetBody1();
float maxImpulse = 0.f;
{
const int numpoints = manifoldPtr->GetNumContacts();
SimdVector3 color(0,1,0);
btVector3 color(0,1,0);
for (int i=0;i<numpoints ;i++)
{
@@ -296,7 +296,7 @@ float SequentialImpulseConstraintSolver::Solve(PersistentManifold* manifoldPtr,
else
j=i;
ManifoldPoint& cp = manifoldPtr->GetContactPoint(j);
btManifoldPoint& cp = manifoldPtr->GetContactPoint(j);
if (cp.GetDistance() <= 0.f)
{
@@ -308,7 +308,7 @@ float SequentialImpulseConstraintSolver::Solve(PersistentManifold* manifoldPtr,
{
ConstraintPersistentData* cpd = (ConstraintPersistentData*) cp.m_userPersistentData;
btConstraintPersistentData* cpd = (btConstraintPersistentData*) cp.m_userPersistentData;
float impulse = cpd->m_contactSolverFunc(
*body0,*body1,
cp,
@@ -324,16 +324,16 @@ float SequentialImpulseConstraintSolver::Solve(PersistentManifold* manifoldPtr,
return maxImpulse;
}
float SequentialImpulseConstraintSolver::SolveFriction(PersistentManifold* manifoldPtr, const ContactSolverInfo& info,int iter,IDebugDraw* debugDrawer)
float btSequentialImpulseConstraintSolver::SolveFriction(btPersistentManifold* manifoldPtr, const btContactSolverInfo& info,int iter,btIDebugDraw* debugDrawer)
{
RigidBody* body0 = (RigidBody*)manifoldPtr->GetBody0();
RigidBody* body1 = (RigidBody*)manifoldPtr->GetBody1();
btRigidBody* body0 = (btRigidBody*)manifoldPtr->GetBody0();
btRigidBody* body1 = (btRigidBody*)manifoldPtr->GetBody1();
{
const int numpoints = manifoldPtr->GetNumContacts();
SimdVector3 color(0,1,0);
btVector3 color(0,1,0);
for (int i=0;i<numpoints ;i++)
{
@@ -341,11 +341,11 @@ float SequentialImpulseConstraintSolver::SolveFriction(PersistentManifold* manif
//if (iter % 2)
// j = numpoints-1-i;
ManifoldPoint& cp = manifoldPtr->GetContactPoint(j);
btManifoldPoint& cp = manifoldPtr->GetContactPoint(j);
if (cp.GetDistance() <= 0.f)
{
ConstraintPersistentData* cpd = (ConstraintPersistentData*) cp.m_userPersistentData;
btConstraintPersistentData* cpd = (btConstraintPersistentData*) cp.m_userPersistentData;
cpd->m_frictionSolverFunc(
*body0,*body1,
cp,

22
src/BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolver.h
View File

@@ -17,46 +17,46 @@ subject to the following restrictions:
#define SEQUENTIAL_IMPULSE_CONSTRAINT_SOLVER_H
#include "btConstraintSolver.h"
class IDebugDraw;
class btIDebugDraw;
#include "btContactConstraint.h"
/// SequentialImpulseConstraintSolver uses a Propagation Method and Sequentially applies impulses
/// 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 SequentialImpulseConstraintSolver : public ConstraintSolver
class btSequentialImpulseConstraintSolver : public btConstraintSolver
{
float Solve(PersistentManifold* manifold, const ContactSolverInfo& info,int iter,IDebugDraw* debugDrawer);
float SolveFriction(PersistentManifold* manifoldPtr, const ContactSolverInfo& info,int iter,IDebugDraw* debugDrawer);
void PrepareConstraints(PersistentManifold* manifoldPtr, const ContactSolverInfo& info,IDebugDraw* debugDrawer);
float Solve(btPersistentManifold* manifold, const btContactSolverInfo& info,int iter,btIDebugDraw* debugDrawer);
float SolveFriction(btPersistentManifold* manifoldPtr, 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];
public:
SequentialImpulseConstraintSolver();
btSequentialImpulseConstraintSolver();
///Advanced: Override the default contact solving function for contacts, for certain types of rigidbody
///See RigidBody::m_contactSolverType and RigidBody::m_frictionSolverType
///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 RigidBody::m_contactSolverType and RigidBody::m_frictionSolverType
///See btRigidBody::m_contactSolverType and btRigidBody::m_frictionSolverType
void SetFrictionSolverFunc(ContactSolverFunc func,int type0,int type1)
{
m_frictionDispatch[type0][type1] = func;
}
virtual ~SequentialImpulseConstraintSolver() {}
virtual ~btSequentialImpulseConstraintSolver() {}
virtual float SolveGroup(PersistentManifold** manifold,int numManifolds,const ContactSolverInfo& info, IDebugDraw* debugDrawer=0);
virtual float SolveGroup(btPersistentManifold** manifold,int numManifolds,const btContactSolverInfo& info, btIDebugDraw* debugDrawer=0);
};

142
src/BulletDynamics/ConstraintSolver/btSolve2LinearConstraint.cpp
View File

@@ -18,36 +18,36 @@ subject to the following restrictions:
#include "btSolve2LinearConstraint.h"
#include "BulletDynamics/Dynamics/btRigidBody.h"
#include "LinearMath/SimdVector3.h"
#include "LinearMath/btVector3.h"
#include "btJacobianEntry.h"
void Solve2LinearConstraint::resolveUnilateralPairConstraint(
RigidBody* body1,
RigidBody* body2,
void btSolve2LinearConstraint::resolveUnilateralPairConstraint(
btRigidBody* body1,
btRigidBody* body2,
const SimdMatrix3x3& world2A,
const SimdMatrix3x3& world2B,
const btMatrix3x3& world2A,
const btMatrix3x3& world2B,
const SimdVector3& invInertiaADiag,
const SimdScalar invMassA,
const SimdVector3& linvelA,const SimdVector3& angvelA,
const SimdVector3& rel_posA1,
const SimdVector3& invInertiaBDiag,
const SimdScalar invMassB,
const SimdVector3& linvelB,const SimdVector3& angvelB,
const SimdVector3& rel_posA2,
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,
SimdScalar depthA, const SimdVector3& normalA,
const SimdVector3& rel_posB1,const SimdVector3& rel_posB2,
SimdScalar depthB, const SimdVector3& normalB,
SimdScalar& imp0,SimdScalar& imp1)
btScalar depthA, const btVector3& normalA,
const btVector3& rel_posB1,const btVector3& rel_posB2,
btScalar depthB, const btVector3& normalB,
btScalar& imp0,btScalar& imp1)
{
imp0 = 0.f;
imp1 = 0.f;
SimdScalar len = fabs(normalA.length())-1.f;
btScalar len = fabs(normalA.length())-1.f;
if (fabs(len) >= SIMD_EPSILON)
return;
@@ -55,24 +55,24 @@ void Solve2LinearConstraint::resolveUnilateralPairConstraint(
//this jacobian entry could be re-used for all iterations
JacobianEntry jacA(world2A,world2B,rel_posA1,rel_posA2,normalA,invInertiaADiag,invMassA,
btJacobianEntry jacA(world2A,world2B,rel_posA1,rel_posA2,normalA,invInertiaADiag,invMassA,
invInertiaBDiag,invMassB);
JacobianEntry jacB(world2A,world2B,rel_posB1,rel_posB2,normalB,invInertiaADiag,invMassA,
btJacobianEntry jacB(world2A,world2B,rel_posB1,rel_posB2,normalB,invInertiaADiag,invMassA,
invInertiaBDiag,invMassB);
//const SimdScalar vel0 = jacA.getRelativeVelocity(linvelA,angvelA,linvelB,angvelB);
//const SimdScalar vel1 = jacB.getRelativeVelocity(linvelA,angvelA,linvelB,angvelB);
//const btScalar vel0 = jacA.getRelativeVelocity(linvelA,angvelA,linvelB,angvelB);
//const btScalar vel1 = jacB.getRelativeVelocity(linvelA,angvelA,linvelB,angvelB);
const SimdScalar vel0 = normalA.dot(body1->getVelocityInLocalPoint(rel_posA1)-body2->getVelocityInLocalPoint(rel_posA1));
const SimdScalar vel1 = normalB.dot(body1->getVelocityInLocalPoint(rel_posB1)-body2->getVelocityInLocalPoint(rel_posB1));
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));
// SimdScalar penetrationImpulse = (depth*contactTau*timeCorrection) * massTerm;//jacDiagABInv
SimdScalar massTerm = 1.f / (invMassA + invMassB);
// btScalar penetrationImpulse = (depth*contactTau*timeCorrection) * massTerm;//jacDiagABInv
btScalar massTerm = 1.f / (invMassA + invMassB);
// calculate rhs (or error) terms
const SimdScalar dv0 = depthA * m_tau * massTerm - vel0 * m_damping;
const SimdScalar dv1 = depthB * m_tau * massTerm - vel1 * m_damping;
const btScalar dv0 = depthA * m_tau * massTerm - vel0 * m_damping;
const btScalar dv1 = depthB * m_tau * massTerm - vel1 * m_damping;
// dC/dv * dv = -C
@@ -86,8 +86,8 @@ void Solve2LinearConstraint::resolveUnilateralPairConstraint(
//
SimdScalar nonDiag = jacA.getNonDiagonal(jacB,invMassA,invMassB);
SimdScalar invDet = 1.0f / (jacA.getDiagonal() * jacB.getDiagonal() - nonDiag * nonDiag );
btScalar nonDiag = jacA.getNonDiagonal(jacB,invMassA,invMassB);
btScalar invDet = 1.0f / (jacA.getDiagonal() * jacB.getDiagonal() - nonDiag * nonDiag );
//imp0 = dv0 * jacA.getDiagonal() * invDet + dv1 * -nonDiag * invDet;
//imp1 = dv1 * jacB.getDiagonal() * invDet + dv0 * - nonDiag * invDet;
@@ -105,31 +105,31 @@ void Solve2LinearConstraint::resolveUnilateralPairConstraint(
void Solve2LinearConstraint::resolveBilateralPairConstraint(
RigidBody* body1,
RigidBody* body2,
const SimdMatrix3x3& world2A,
const SimdMatrix3x3& world2B,
void btSolve2LinearConstraint::resolveBilateralPairConstraint(
btRigidBody* body1,
btRigidBody* body2,
const btMatrix3x3& world2A,
const btMatrix3x3& world2B,
const SimdVector3& invInertiaADiag,
const SimdScalar invMassA,
const SimdVector3& linvelA,const SimdVector3& angvelA,
const SimdVector3& rel_posA1,
const SimdVector3& invInertiaBDiag,
const SimdScalar invMassB,
const SimdVector3& linvelB,const SimdVector3& angvelB,
const SimdVector3& rel_posA2,
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,
SimdScalar depthA, const SimdVector3& normalA,
const SimdVector3& rel_posB1,const SimdVector3& rel_posB2,
SimdScalar depthB, const SimdVector3& normalB,
SimdScalar& imp0,SimdScalar& imp1)
btScalar depthA, const btVector3& normalA,
const btVector3& rel_posB1,const btVector3& rel_posB2,
btScalar depthB, const btVector3& normalB,
btScalar& imp0,btScalar& imp1)
{
imp0 = 0.f;
imp1 = 0.f;
SimdScalar len = fabs(normalA.length())-1.f;
btScalar len = fabs(normalA.length())-1.f;
if (fabs(len) >= SIMD_EPSILON)
return;
@@ -137,20 +137,20 @@ void Solve2LinearConstraint::resolveBilateralPairConstraint(
//this jacobian entry could be re-used for all iterations
JacobianEntry jacA(world2A,world2B,rel_posA1,rel_posA2,normalA,invInertiaADiag,invMassA,
btJacobianEntry jacA(world2A,world2B,rel_posA1,rel_posA2,normalA,invInertiaADiag,invMassA,
invInertiaBDiag,invMassB);
JacobianEntry jacB(world2A,world2B,rel_posB1,rel_posB2,normalB,invInertiaADiag,invMassA,
btJacobianEntry jacB(world2A,world2B,rel_posB1,rel_posB2,normalB,invInertiaADiag,invMassA,
invInertiaBDiag,invMassB);
//const SimdScalar vel0 = jacA.getRelativeVelocity(linvelA,angvelA,linvelB,angvelB);
//const SimdScalar vel1 = jacB.getRelativeVelocity(linvelA,angvelA,linvelB,angvelB);
//const btScalar vel0 = jacA.getRelativeVelocity(linvelA,angvelA,linvelB,angvelB);
//const btScalar vel1 = jacB.getRelativeVelocity(linvelA,angvelA,linvelB,angvelB);
const SimdScalar vel0 = normalA.dot(body1->getVelocityInLocalPoint(rel_posA1)-body2->getVelocityInLocalPoint(rel_posA1));
const SimdScalar vel1 = normalB.dot(body1->getVelocityInLocalPoint(rel_posB1)-body2->getVelocityInLocalPoint(rel_posB1));
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 SimdScalar dv0 = depthA * m_tau - vel0 * m_damping;
const SimdScalar dv1 = depthB * m_tau - vel1 * m_damping;
const btScalar dv0 = depthA * m_tau - vel0 * m_damping;
const btScalar dv1 = depthB * m_tau - vel1 * m_damping;
// dC/dv * dv = -C
@@ -163,8 +163,8 @@ void Solve2LinearConstraint::resolveBilateralPairConstraint(
//
SimdScalar nonDiag = jacA.getNonDiagonal(jacB,invMassA,invMassB);
SimdScalar invDet = 1.0f / (jacA.getDiagonal() * jacB.getDiagonal() - nonDiag * nonDiag );
btScalar nonDiag = jacA.getNonDiagonal(jacB,invMassA,invMassB);
btScalar invDet = 1.0f / (jacA.getDiagonal() * jacB.getDiagonal() - nonDiag * nonDiag );
//imp0 = dv0 * jacA.getDiagonal() * invDet + dv1 * -nonDiag * invDet;
//imp1 = dv1 * jacB.getDiagonal() * invDet + dv0 * - nonDiag * invDet;
@@ -222,19 +222,19 @@ void Solve2LinearConstraint::resolveBilateralPairConstraint(
void Solve2LinearConstraint::resolveAngularConstraint( const SimdMatrix3x3& invInertiaAWS,
const SimdScalar invMassA,
const SimdVector3& linvelA,const SimdVector3& angvelA,
const SimdVector3& rel_posA1,
const SimdMatrix3x3& invInertiaBWS,
const SimdScalar invMassB,
const SimdVector3& linvelB,const SimdVector3& angvelB,
const SimdVector3& rel_posA2,
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,
SimdScalar depthA, const SimdVector3& normalA,
const SimdVector3& rel_posB1,const SimdVector3& rel_posB2,
SimdScalar depthB, const SimdVector3& normalB,
SimdScalar& imp0,SimdScalar& imp1)
btScalar depthA, const btVector3& normalA,
const btVector3& rel_posB1,const btVector3& rel_posB2,
btScalar depthB, const btVector3& normalB,
btScalar& imp0,btScalar& imp1)
{
}

102
src/BulletDynamics/ConstraintSolver/btSolve2LinearConstraint.h
View File

@@ -16,23 +16,23 @@ subject to the following restrictions:
#ifndef SOLVE_2LINEAR_CONSTRAINT_H
#define SOLVE_2LINEAR_CONSTRAINT_H
#include "LinearMath/SimdMatrix3x3.h"
#include "LinearMath/SimdVector3.h"
#include "LinearMath/btMatrix3x3.h"
#include "LinearMath/btVector3.h"
class RigidBody;
class btRigidBody;
/// constraint class used for lateral tyre friction.
class Solve2LinearConstraint
class btSolve2LinearConstraint
{
SimdScalar m_tau;
SimdScalar m_damping;
btScalar m_tau;
btScalar m_damping;
public:
Solve2LinearConstraint(SimdScalar tau,SimdScalar damping)
btSolve2LinearConstraint(btScalar tau,btScalar damping)
{
m_tau = tau;
m_damping = damping;
@@ -41,64 +41,64 @@ public:
// solve unilateral constraint (equality, direct method)
//
void resolveUnilateralPairConstraint(
RigidBody* body0,
RigidBody* body1,
btRigidBody* body0,
btRigidBody* body1,
const SimdMatrix3x3& world2A,
const SimdMatrix3x3& world2B,
const btMatrix3x3& world2A,
const btMatrix3x3& world2B,
const SimdVector3& invInertiaADiag,
const SimdScalar invMassA,
const SimdVector3& linvelA,const SimdVector3& angvelA,
const SimdVector3& rel_posA1,
const SimdVector3& invInertiaBDiag,
const SimdScalar invMassB,
const SimdVector3& linvelB,const SimdVector3& angvelB,
const SimdVector3& rel_posA2,
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,
SimdScalar depthA, const SimdVector3& normalA,
const SimdVector3& rel_posB1,const SimdVector3& rel_posB2,
SimdScalar depthB, const SimdVector3& normalB,
SimdScalar& imp0,SimdScalar& imp1);
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(
RigidBody* body0,
RigidBody* body1,
const SimdMatrix3x3& world2A,
const SimdMatrix3x3& world2B,
btRigidBody* body0,
btRigidBody* body1,
const btMatrix3x3& world2A,
const btMatrix3x3& world2B,
const SimdVector3& invInertiaADiag,
const SimdScalar invMassA,
const SimdVector3& linvelA,const SimdVector3& angvelA,
const SimdVector3& rel_posA1,
const SimdVector3& invInertiaBDiag,
const SimdScalar invMassB,
const SimdVector3& linvelB,const SimdVector3& angvelB,
const SimdVector3& rel_posA2,
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,
SimdScalar depthA, const SimdVector3& normalA,
const SimdVector3& rel_posB1,const SimdVector3& rel_posB2,
SimdScalar depthB, const SimdVector3& normalB,
SimdScalar& imp0,SimdScalar& imp1);
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 SimdMatrix3x3& invInertiaAWS,
const SimdScalar invMassA,
const SimdVector3& linvelA,const SimdVector3& angvelA,
const SimdVector3& rel_posA1,
const SimdMatrix3x3& invInertiaBWS,
const SimdScalar invMassB,
const SimdVector3& linvelB,const SimdVector3& angvelB,
const SimdVector3& rel_posA2,
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,
SimdScalar depthA, const SimdVector3& normalA,
const SimdVector3& rel_posB1,const SimdVector3& rel_posB2,
SimdScalar depthB, const SimdVector3& normalB,
SimdScalar& imp0,SimdScalar& imp1);
btScalar depthA, const btVector3& normalA,
const btVector3& rel_posB1,const btVector3& rel_posB2,
btScalar depthB, const btVector3& normalB,
btScalar& imp0,btScalar& imp1);
};

14
src/BulletDynamics/ConstraintSolver/btTypedConstraint.cpp
View File

@@ -18,37 +18,37 @@ subject to the following restrictions:
#include "BulletDynamics/Dynamics/btRigidBody.h"
#include "BulletDynamics/Dynamics/btMassProps.h"
static RigidBody s_fixed(MassProps(0,SimdVector3(0.f,0.f,0.f)),0.f,0.f,1.f,1.f);
static btRigidBody s_fixed(btMassProps(0,btVector3(0.f,0.f,0.f)),0.f,0.f,1.f,1.f);
TypedConstraint::TypedConstraint()
btTypedConstraint::btTypedConstraint()
: m_userConstraintType(-1),
m_userConstraintId(-1),
m_rbA(s_fixed),
m_rbB(s_fixed),
m_appliedImpulse(0.f)
{
s_fixed.setMassProps(0.f,SimdVector3(0.f,0.f,0.f));
s_fixed.setMassProps(0.f,btVector3(0.f,0.f,0.f));
}
TypedConstraint::TypedConstraint(RigidBody& rbA)
btTypedConstraint::btTypedConstraint(btRigidBody& rbA)
: m_userConstraintType(-1),
m_userConstraintId(-1),
m_rbA(rbA),
m_rbB(s_fixed),
m_appliedImpulse(0.f)
{
s_fixed.setMassProps(0.f,SimdVector3(0.f,0.f,0.f));
s_fixed.setMassProps(0.f,btVector3(0.f,0.f,0.f));
}
TypedConstraint::TypedConstraint(RigidBody& rbA,RigidBody& rbB)
btTypedConstraint::btTypedConstraint(btRigidBody& rbA,btRigidBody& rbB)
: m_userConstraintType(-1),
m_userConstraintId(-1),
m_rbA(rbA),
m_rbB(rbB),
m_appliedImpulse(0.f)
{
s_fixed.setMassProps(0.f,SimdVector3(0.f,0.f,0.f));
s_fixed.setMassProps(0.f,btVector3(0.f,0.f,0.f));
}

28
src/BulletDynamics/ConstraintSolver/btTypedConstraint.h
View File

@@ -16,48 +16,48 @@ subject to the following restrictions:
#ifndef TYPED_CONSTRAINT_H
#define TYPED_CONSTRAINT_H
class RigidBody;
#include "LinearMath/SimdScalar.h"
class btRigidBody;
#include "LinearMath/btScalar.h"
//TypedConstraint is the baseclass for Bullet constraints and vehicles
class TypedConstraint
class btTypedConstraint
{
int m_userConstraintType;
int m_userConstraintId;
protected:
RigidBody& m_rbA;
RigidBody& m_rbB;
btRigidBody& m_rbA;
btRigidBody& m_rbB;
float m_appliedImpulse;
public:
TypedConstraint();
virtual ~TypedConstraint() {};
TypedConstraint(RigidBody& rbA);
btTypedConstraint();
virtual ~btTypedConstraint() {};
btTypedConstraint(btRigidBody& rbA);
TypedConstraint(RigidBody& rbA,RigidBody& rbB);
btTypedConstraint(btRigidBody& rbA,btRigidBody& rbB);
virtual void BuildJacobian() = 0;
virtual void SolveConstraint(SimdScalar timeStep) = 0;
virtual void SolveConstraint(btScalar timeStep) = 0;
const RigidBody& GetRigidBodyA() const
const btRigidBody& GetRigidBodyA() const
{
return m_rbA;
}
const RigidBody& GetRigidBodyB() const
const btRigidBody& GetRigidBodyB() const
{
return m_rbB;
}
RigidBody& GetRigidBodyA()
btRigidBody& GetRigidBodyA()
{
return m_rbA;
}
RigidBody& GetRigidBodyB()
btRigidBody& GetRigidBodyB()
{
return m_rbB;
}

326
src/BulletDynamics/Dynamics/btDiscreteDynamicsWorld.cpp
View File

@@ -321,4 +321,328 @@ void btDiscreteDynamicsWorld::predictUnconstraintMotion(float timeStep)
}
}
}
}
}=======
#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"
//rigidbody & constraints
#include "BulletDynamics/Dynamics/btRigidBody.h"
#include "BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolver.h"
#include "BulletDynamics/ConstraintSolver/btContactSolverInfo.h"
#include "BulletDynamics/ConstraintSolver/btTypedConstraint.h"
//vehicle
#include "BulletDynamics/Vehicle/btRaycastVehicle.h"
#include "BulletDynamics/Vehicle/btVehicleRaycaster.h"
#include "BulletDynamics/Vehicle/btWheelInfo.h"
#include <algorithm>
btDiscreteDynamicsWorld::btDiscreteDynamicsWorld()
:btDynamicsWorld(new btCollisionDispatcher(),new btSimpleBroadphase()),
m_constraintSolver(new btSequentialImpulseConstraintSolver)
{
m_islandManager = new btSimulationIslandManager();
}
btDiscreteDynamicsWorld::btDiscreteDynamicsWorld(btDispatcher* dispatcher,btOverlappingPairCache* pairCache,btConstraintSolver* constraintSolver)
:btDynamicsWorld(dispatcher,pairCache),
m_constraintSolver(constraintSolver)
{
m_islandManager = new btSimulationIslandManager();
}
btDiscreteDynamicsWorld::~btDiscreteDynamicsWorld()
{
delete m_islandManager ;
delete m_constraintSolver;
//delete the dispatcher and paircache
delete m_dispatcher1;
m_dispatcher1 = 0;
delete m_pairCache;
m_pairCache = 0;
}
void btDiscreteDynamicsWorld::stepSimulation(float timeStep)
{
///update aabbs information
updateAabbs();
///apply gravity, predict motion
predictUnconstraintMotion(timeStep);
///perform collision detection
PerformDiscreteCollisionDetection();
calculateSimulationIslands();
btContactSolverInfo infoGlobal;
infoGlobal.m_timeStep = timeStep;
///solve non-contact constraints
solveNoncontactConstraints(infoGlobal);
///solve contact constraints
solveContactConstraints(infoGlobal);
///update vehicle simulation
updateVehicles(timeStep);
///CallbackTriggers();
///integrate transforms
integrateTransforms(timeStep);
updateActivationState( timeStep );
}
void btDiscreteDynamicsWorld::updateVehicles(float timeStep)
{
for (int i=0;i<m_vehicles.size();i++)
{
btRaycastVehicle* vehicle = m_vehicles[i];
vehicle->UpdateVehicle( timeStep);
}
}
void btDiscreteDynamicsWorld::updateActivationState(float timeStep)
{
for (int i=0;i<m_collisionObjects.size();i++)
{
btCollisionObject* colObj = m_collisionObjects[i];
if (colObj->m_internalOwner)
{
btRigidBody* body = (btRigidBody*)colObj->m_internalOwner;
body->updateDeactivation(timeStep);
if (body->wantsSleeping())
{
if (body->GetActivationState() == ACTIVE_TAG)
body->SetActivationState( WANTS_DEACTIVATION );
} else
{
if (body->GetActivationState() != DISABLE_DEACTIVATION)
body->SetActivationState( ACTIVE_TAG );
}
}
}
}
void btDiscreteDynamicsWorld::addConstraint(btTypedConstraint* constraint)
{
m_constraints.push_back(constraint);
}
void btDiscreteDynamicsWorld::removeConstraint(btTypedConstraint* constraint)
{
std::vector<btTypedConstraint*>::iterator cit = std::find(m_constraints.begin(),m_constraints.end(),constraint);
if (!(cit==m_constraints.end()))
{
m_constraints.erase(cit);
}
}
void btDiscreteDynamicsWorld::addVehicle(btRaycastVehicle* vehicle)
{
m_vehicles.push_back(vehicle);
}
void btDiscreteDynamicsWorld::removeVehicle(btRaycastVehicle* vehicle)
{
std::vector<btRaycastVehicle*>::iterator vit = std::find(m_vehicles.begin(),m_vehicles.end(),vehicle);
if (!(vit==m_vehicles.end()))
{
m_vehicles.erase(vit);
}
}
void btDiscreteDynamicsWorld::solveContactConstraints(btContactSolverInfo& solverInfo)
{
struct InplaceSolverIslandCallback : public btSimulationIslandManager::IslandCallback
{
btContactSolverInfo& m_solverInfo;
btConstraintSolver* m_solver;
btIDebugDraw* m_debugDrawer;
InplaceSolverIslandCallback(
btContactSolverInfo& solverInfo,
btConstraintSolver* solver,
btIDebugDraw* debugDrawer)
:m_solverInfo(solverInfo),
m_solver(solver),
m_debugDrawer(debugDrawer)
{
}
virtual void ProcessIsland(btPersistentManifold** manifolds,int numManifolds)
{
m_solver->SolveGroup( manifolds, numManifolds,m_solverInfo,m_debugDrawer);
}
};
btIDebugDraw* debugDraw = 0;
InplaceSolverIslandCallback solverCallback( solverInfo, m_constraintSolver, debugDraw);
/// solve all the contact points and contact friction
m_islandManager->BuildAndProcessIslands(GetCollisionWorld()->GetDispatcher(),GetCollisionWorld()->GetCollisionObjectArray(),&solverCallback);
}
void btDiscreteDynamicsWorld::solveNoncontactConstraints(btContactSolverInfo& solverInfo)
{
#ifdef USE_QUICKPROF
Profiler::beginBlock("SolveConstraint");
#endif //USE_QUICKPROF
int i;
int numConstraints = m_constraints.size();
///constraint preparation: building jacobians
for (i=0;i< numConstraints ; i++ )
{
btTypedConstraint* constraint = m_constraints[i];
constraint->BuildJacobian();
}
//solve the regular non-contact constraints (point 2 point, hinge, generic d6)
for (int g=0;g<solverInfo.m_numIterations;g++)
{
//
// constraint solving
//
for (i=0;i< numConstraints ; i++ )
{
btTypedConstraint* constraint = m_constraints[i];
constraint->SolveConstraint( solverInfo.m_timeStep );
}
}
#ifdef USE_QUICKPROF
Profiler::endBlock("SolveConstraint");
#endif //USE_QUICKPROF
}
void btDiscreteDynamicsWorld::calculateSimulationIslands()
{
#ifdef USE_QUICKPROF
Profiler::beginBlock("IslandUnionFind");
#endif //USE_QUICKPROF
GetSimulationIslandManager()->UpdateActivationState(GetCollisionWorld(),GetCollisionWorld()->GetDispatcher());
{
int i;
int numConstraints = 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)->m_islandTag1,
(colObj1)->m_islandTag1);
}
}
}
}
//Store the island id in each body
GetSimulationIslandManager()->StoreIslandActivationState(GetCollisionWorld());
#ifdef USE_QUICKPROF
Profiler::endBlock("IslandUnionFind");
#endif //USE_QUICKPROF
}
void btDiscreteDynamicsWorld::updateAabbs()
{
btTransform predictedTrans;
for (int i=0;i<m_collisionObjects.size();i++)
{
btCollisionObject* colObj = m_collisionObjects[i];
if (colObj->m_internalOwner)
{
btRigidBody* body = (btRigidBody*)colObj->m_internalOwner;
if (body->IsActive() && (!body->IsStatic()))
{
btPoint3 minAabb,maxAabb;
colObj->m_collisionShape->GetAabb(colObj->m_worldTransform, minAabb,maxAabb);
btSimpleBroadphase* bp = (btSimpleBroadphase*)m_pairCache;
bp->SetAabb(body->m_broadphaseHandle,minAabb,maxAabb);
}
}
}
}
void btDiscreteDynamicsWorld::integrateTransforms(float timeStep)
{
btTransform predictedTrans;
for (int i=0;i<m_collisionObjects.size();i++)
{
btCollisionObject* colObj = m_collisionObjects[i];
if (colObj->m_internalOwner)
{
btRigidBody* body = (btRigidBody*)colObj->m_internalOwner;
if (body->IsActive() && (!body->IsStatic()))
{
body->predictIntegratedTransform(timeStep, predictedTrans);
body->proceedToTransform( predictedTrans);
}
}
}
}
void btDiscreteDynamicsWorld::predictUnconstraintMotion(float timeStep)
{
for (int i=0;i<m_collisionObjects.size();i++)
{
btCollisionObject* colObj = m_collisionObjects[i];
if (colObj->m_internalOwner)
{
btRigidBody* body = (btRigidBody*)colObj->m_internalOwner;
body->m_cachedInvertedWorldTransform = body->m_worldTransform.inverse();
if (body->IsActive() && (!body->IsStatic()))
{
body->applyForces( timeStep);
body->integrateVelocities( timeStep);
body->predictIntegratedTransform(timeStep,body->m_interpolationWorldTransform);
}
}
}
}

99
src/BulletDynamics/Dynamics/btDiscreteDynamicsWorld.h
View File

@@ -94,4 +94,101 @@ public:
};
#endif //BT_DISCRETE_DYNAMICS_WORLD_H
#endif //BT_DISCRETE_DYNAMICS_WORLD_H=======
/*
Bullet Continuous Collision Detection and Physics Library
Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/
This software is provided 'as-is', without any express or implied warranty.
In no event will the authors be held liable for any damages arising from the use of this software.
Permission is granted to anyone to use this software for any purpose,
including commercial applications, and to alter it and redistribute it freely,
subject to the following restrictions:
1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
3. This notice may not be removed or altered from any source distribution.
*/
#ifndef BT_DISCRETE_DYNAMICS_WORLD_H
#define BT_DISCRETE_DYNAMICS_WORLD_H
#include "btDynamicsWorld.h"
class btDispatcher;
class btOverlappingPairCache;
class btConstraintSolver;
class btSimulationIslandManager;
class btTypedConstraint;
struct btContactSolverInfo;
class btRaycastVehicle;
#include <vector>
///btDiscreteDynamicsWorld provides discrete rigid body simulation
///those classes replace the obsolete CcdPhysicsEnvironment/CcdPhysicsController
class btDiscreteDynamicsWorld : public btDynamicsWorld
{
protected:
btConstraintSolver* m_constraintSolver;
btSimulationIslandManager* m_islandManager;
std::vector<btTypedConstraint*> m_constraints;
std::vector<btRaycastVehicle*> m_vehicles;
void predictUnconstraintMotion(float timeStep);
void integrateTransforms(float timeStep);
void updateAabbs();
void calculateSimulationIslands();
void solveNoncontactConstraints(btContactSolverInfo& solverInfo);
void solveContactConstraints(btContactSolverInfo& solverInfo);
void updateActivationState(float timeStep);
void updateVehicles(float timeStep);
public:
btDiscreteDynamicsWorld(btDispatcher* dispatcher,btOverlappingPairCache* pairCache,btConstraintSolver* constraintSolver);
btDiscreteDynamicsWorld();
virtual ~btDiscreteDynamicsWorld();
virtual void stepSimulation( float timeStep);
void addConstraint(btTypedConstraint* constraint);
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;
}
};
#endif //BT_DISCRETE_DYNAMICS_WORLD_H

49
src/BulletDynamics/Dynamics/btDynamicsWorld.h
View File

@@ -1,3 +1,4 @@
<<<<<<< .working
/*
Bullet Continuous Collision Detection and Physics Library
Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/
@@ -43,4 +44,50 @@ class btDynamicsWorld : public CollisionWorld
};
#endif //BT_DYNAMICS_WORLD_H
#endif //BT_DYNAMICS_WORLD_H=======
/*
Bullet Continuous Collision Detection and Physics Library
Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/
This software is provided 'as-is', without any express or implied warranty.
In no event will the authors be held liable for any damages arising from the use of this software.
Permission is granted to anyone to use this software for any purpose,
including commercial applications, and to alter it and redistribute it freely,
subject to the following restrictions:
1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
3. This notice may not be removed or altered from any source distribution.
*/
#ifndef BT_DYNAMICS_WORLD_H
#define BT_DYNAMICS_WORLD_H
#include "BulletCollision/CollisionDispatch/btCollisionWorld.h"
class btTypedConstraint;
///btDynamicsWorld is the baseclass for several dynamics implementation, basic, discrete, parallel, and continuous
class btDynamicsWorld : public btCollisionWorld
{
public:
btDynamicsWorld(btDispatcher* dispatcher,btOverlappingPairCache* pairCache)
:btCollisionWorld(dispatcher,pairCache)
{
}
virtual ~btDynamicsWorld()
{
}
///stepSimulation proceeds the simulation over timeStep units
virtual void stepSimulation( float timeStep) = 0;
virtual void addConstraint(btTypedConstraint* constraint) {};
virtual void removeConstraint(btTypedConstraint* constraint) {};
};
#endif //BT_DYNAMICS_WORLD_H>>>>>>> .merge-right.r324

8
src/BulletDynamics/Dynamics/btMassProps.h
View File

@@ -16,16 +16,16 @@ subject to the following restrictions:
#ifndef MASS_PROPS_H
#define MASS_PROPS_H
#include <LinearMath/SimdVector3.h>
#include <LinearMath/btVector3.h>
struct MassProps {
MassProps(float mass,const SimdVector3& inertiaLocal):
struct btMassProps {
btMassProps(float mass,const btVector3& inertiaLocal):
m_mass(mass),
m_inertiaLocal(inertiaLocal)
{
}
float m_mass;
SimdVector3 m_inertiaLocal;
btVector3 m_inertiaLocal;
};

48
src/BulletDynamics/Dynamics/btRigidBody.cpp
View File

@@ -16,14 +16,14 @@ subject to the following restrictions:
#include "btRigidBody.h"
#include "btMassProps.h"
#include "BulletCollision/CollisionShapes/btConvexShape.h"
#include "LinearMath/GenMinMax.h"
#include <LinearMath/SimdTransformUtil.h>
#include "LinearMath/btMinMax.h"
#include <LinearMath/btTransformUtil.h>
float gLinearAirDamping = 1.f;
static int uniqueId = 0;
RigidBody::RigidBody( const MassProps& massProps,SimdScalar linearDamping,SimdScalar angularDamping,SimdScalar friction,SimdScalar restitution)
btRigidBody::btRigidBody( const btMassProps& massProps,btScalar linearDamping,btScalar angularDamping,btScalar friction,btScalar restitution)
:
m_gravity(0.0f, 0.0f, 0.0f),
m_totalForce(0.0f, 0.0f, 0.0f),
@@ -37,7 +37,7 @@ RigidBody::RigidBody( const MassProps& massProps,SimdScalar linearDamping,SimdSc
m_frictionSolverType(0)
{
//moved to CollisionObject
//moved to btCollisionObject
m_friction = friction;
m_restitution = restitution;
@@ -51,25 +51,25 @@ RigidBody::RigidBody( const MassProps& massProps,SimdScalar linearDamping,SimdSc
}
void RigidBody::setLinearVelocity(const SimdVector3& lin_vel)
void btRigidBody::setLinearVelocity(const btVector3& lin_vel)
{
m_linearVelocity = lin_vel;
}
void RigidBody::predictIntegratedTransform(SimdScalar timeStep,SimdTransform& predictedTransform) const
void btRigidBody::predictIntegratedTransform(btScalar timeStep,btTransform& predictedTransform) const
{
SimdTransformUtil::IntegrateTransform(m_worldTransform,m_linearVelocity,m_angularVelocity,timeStep,predictedTransform);
btTransformUtil::IntegrateTransform(m_worldTransform,m_linearVelocity,m_angularVelocity,timeStep,predictedTransform);
}
void RigidBody::saveKinematicState(SimdScalar timeStep)
void btRigidBody::saveKinematicState(btScalar timeStep)
{
if (m_kinematicTimeStep)
{
SimdVector3 linVel,angVel;
SimdTransformUtil::CalculateVelocity(m_interpolationWorldTransform,m_worldTransform,m_kinematicTimeStep,m_linearVelocity,m_angularVelocity);
btVector3 linVel,angVel;
btTransformUtil::CalculateVelocity(m_interpolationWorldTransform,m_worldTransform,m_kinematicTimeStep,m_linearVelocity,m_angularVelocity);
//printf("angular = %f %f %f\n",m_angularVelocity.getX(),m_angularVelocity.getY(),m_angularVelocity.getZ());
}
@@ -79,7 +79,7 @@ void RigidBody::saveKinematicState(SimdScalar timeStep)
m_kinematicTimeStep = timeStep;
}
void RigidBody::getAabb(SimdVector3& aabbMin,SimdVector3& aabbMax) const
void btRigidBody::getAabb(btVector3& aabbMin,btVector3& aabbMax) const
{
GetCollisionShape()->GetAabb(m_worldTransform,aabbMin,aabbMax);
}
@@ -87,7 +87,7 @@ void RigidBody::getAabb(SimdVector3& aabbMin,SimdVector3& aabbMax) const
void RigidBody::setGravity(const SimdVector3& acceleration)
void btRigidBody::setGravity(const btVector3& acceleration)
{
if (m_inverseMass != 0.0f)
{
@@ -100,7 +100,7 @@ void RigidBody::setGravity(const SimdVector3& acceleration)
void RigidBody::setDamping(SimdScalar lin_damping, SimdScalar ang_damping)
void btRigidBody::setDamping(btScalar lin_damping, btScalar ang_damping)
{
m_linearDamping = GEN_clamped(lin_damping, 0.0f, 1.0f);
m_angularDamping = GEN_clamped(ang_damping, 0.0f, 1.0f);
@@ -111,7 +111,7 @@ void RigidBody::setDamping(SimdScalar lin_damping, SimdScalar ang_damping)
#include <stdio.h>
void RigidBody::applyForces(SimdScalar step)
void btRigidBody::applyForces(btScalar step)
{
if (IsStatic())
return;
@@ -130,7 +130,7 @@ void RigidBody::applyForces(SimdScalar step)
float dampVel = 0.005f;
if (speed > dampVel)
{
SimdVector3 dir = m_linearVelocity.normalized();
btVector3 dir = m_linearVelocity.normalized();
m_linearVelocity -= dir * dampVel;
} else
{
@@ -144,7 +144,7 @@ void RigidBody::applyForces(SimdScalar step)
float angDampVel = 0.005f;
if (angSpeed > angDampVel)
{
SimdVector3 dir = m_angularVelocity.normalized();
btVector3 dir = m_angularVelocity.normalized();
m_angularVelocity -= dir * angDampVel;
} else
{
@@ -155,17 +155,17 @@ void RigidBody::applyForces(SimdScalar step)
}
void RigidBody::proceedToTransform(const SimdTransform& newTrans)
void btRigidBody::proceedToTransform(const btTransform& newTrans)
{
setCenterOfMassTransform( newTrans );
}
void RigidBody::setMassProps(SimdScalar mass, const SimdVector3& inertia)
void btRigidBody::setMassProps(btScalar mass, const btVector3& inertia)
{
if (mass == 0.f)
{
m_collisionFlags = CollisionObject::isStatic;
m_collisionFlags = btCollisionObject::isStatic;
m_inverseMass = 0.f;
} else
{
@@ -182,13 +182,13 @@ void RigidBody::setMassProps(SimdScalar mass, const SimdVector3& inertia)
void RigidBody::updateInertiaTensor()
void btRigidBody::updateInertiaTensor()
{
m_invInertiaTensorWorld = m_worldTransform.getBasis().scaled(m_invInertiaLocal) * m_worldTransform.getBasis().transpose();
}
void RigidBody::integrateVelocities(SimdScalar step)
void btRigidBody::integrateVelocities(btScalar step)
{
if (IsStatic())
return;
@@ -207,15 +207,15 @@ void RigidBody::integrateVelocities(SimdScalar step)
clearForces();
}
SimdQuaternion RigidBody::getOrientation() const
btQuaternion btRigidBody::getOrientation() const
{
SimdQuaternion orn;
btQuaternion orn;
m_worldTransform.getBasis().getRotation(orn);
return orn;
}
void RigidBody::setCenterOfMassTransform(const SimdTransform& xform)
void btRigidBody::setCenterOfMassTransform(const btTransform& xform)
{
m_worldTransform = xform;
updateInertiaTensor();

150
src/BulletDynamics/Dynamics/btRigidBody.h
View File

@@ -17,25 +17,29 @@ subject to the following restrictions:
#define RIGIDBODY_H
#include <vector>
#include <LinearMath/SimdPoint3.h>
#include <LinearMath/SimdTransform.h>
#include <LinearMath/btPoint3.h>
#include <LinearMath/btTransform.h>
#include "BulletCollision/BroadphaseCollision/btBroadphaseProxy.h"
#include "BulletCollision/CollisionDispatch/btCollisionObject.h"
class CollisionShape;
struct MassProps;
typedef SimdScalar dMatrix3[4*3];
class btCollisionShape;
struct btMassProps;
typedef btScalar dMatrix3[4*3];
extern float gLinearAirDamping;
extern bool gUseEpa;
extern float gDeactivationTime;
extern bool gDisableDeactivation;
extern float gLinearSleepingTreshold;
extern float gAngularSleepingTreshold;
/// RigidBody class for RigidBody Dynamics
/// btRigidBody class for btRigidBody Dynamics
///
class RigidBody : public CollisionObject
class btRigidBody : public btCollisionObject
{
SimdMatrix3x3 m_invInertiaTensorWorld;
@@ -57,81 +61,81 @@ class RigidBody : public CollisionObject
public:
RigidBody(const MassProps& massProps,SimdScalar linearDamping,SimdScalar angularDamping,SimdScalar friction,SimdScalar restitution);
btRigidBody(const btMassProps& massProps,btScalar linearDamping=0.f,btScalar angularDamping=0.f,btScalar friction=0.5f,btScalar restitution=0.f);
void proceedToTransform(const SimdTransform& newTrans);
void proceedToTransform(const btTransform& newTrans);
/// continuous collision detection needs prediction
void predictIntegratedTransform(SimdScalar step, SimdTransform& predictedTransform) const;
void predictIntegratedTransform(btScalar step, btTransform& predictedTransform) const;
void saveKinematicState(SimdScalar step);
void saveKinematicState(btScalar step);
void applyForces(SimdScalar step);
void applyForces(btScalar step);
void setGravity(const SimdVector3& acceleration);
void setGravity(const btVector3& acceleration);
void setDamping(SimdScalar lin_damping, SimdScalar ang_damping);
void setDamping(btScalar lin_damping, btScalar ang_damping);
inline const CollisionShape* GetCollisionShape() const {
inline const btCollisionShape* GetCollisionShape() const {
return m_collisionShape;
}
inline CollisionShape* GetCollisionShape() {
inline btCollisionShape* GetCollisionShape() {
return m_collisionShape;
}
void setMassProps(SimdScalar mass, const SimdVector3& inertia);
void setMassProps(btScalar mass, const btVector3& inertia);
SimdScalar getInvMass() const { return m_inverseMass; }
const SimdMatrix3x3& getInvInertiaTensorWorld() const {
btScalar getInvMass() const { return m_inverseMass; }
const btMatrix3x3& getInvInertiaTensorWorld() const {
return m_invInertiaTensorWorld;
}
void integrateVelocities(SimdScalar step);
void integrateVelocities(btScalar step);
void setCenterOfMassTransform(const SimdTransform& xform);
void setCenterOfMassTransform(const btTransform& xform);
void applyCentralForce(const SimdVector3& force)
void applyCentralForce(const btVector3& force)
{
m_totalForce += force;
}
const SimdVector3& getInvInertiaDiagLocal()
const btVector3& getInvInertiaDiagLocal()
{
return m_invInertiaLocal;
};
void setInvInertiaDiagLocal(const SimdVector3& diagInvInertia)
void setInvInertiaDiagLocal(const btVector3& diagInvInertia)
{
m_invInertiaLocal = diagInvInertia;
}
void applyTorque(const SimdVector3& torque)
void applyTorque(const btVector3& torque)
{
m_totalTorque += torque;
}
void applyForce(const SimdVector3& force, const SimdVector3& rel_pos)
void applyForce(const btVector3& force, const btVector3& rel_pos)
{
applyCentralForce(force);
applyTorque(rel_pos.cross(force));
}
void applyCentralImpulse(const SimdVector3& impulse)
void applyCentralImpulse(const btVector3& impulse)
{
m_linearVelocity += impulse * m_inverseMass;
}
void applyTorqueImpulse(const SimdVector3& torque)
void applyTorqueImpulse(const btVector3& torque)
{
if (!IsStatic())
m_angularVelocity += m_invInertiaTensorWorld * torque;
}
void applyImpulse(const SimdVector3& impulse, const SimdVector3& rel_pos)
void applyImpulse(const btVector3& impulse, const btVector3& rel_pos)
{
if (m_inverseMass != 0.f)
{
@@ -148,31 +152,31 @@ public:
void updateInertiaTensor();
const SimdPoint3& getCenterOfMassPosition() const {
const btPoint3& getCenterOfMassPosition() const {
return m_worldTransform.getOrigin();
}
SimdQuaternion getOrientation() const;
btQuaternion getOrientation() const;
const SimdTransform& getCenterOfMassTransform() const {
const btTransform& getCenterOfMassTransform() const {
return m_worldTransform;
}
const SimdVector3& getLinearVelocity() const {
const btVector3& getLinearVelocity() const {
return m_linearVelocity;
}
const SimdVector3& getAngularVelocity() const {
const btVector3& getAngularVelocity() const {
return m_angularVelocity;
}
void setLinearVelocity(const SimdVector3& lin_vel);
void setAngularVelocity(const SimdVector3& ang_vel) {
void setLinearVelocity(const btVector3& lin_vel);
void setAngularVelocity(const btVector3& ang_vel) {
if (!IsStatic())
{
m_angularVelocity = ang_vel;
}
}
SimdVector3 getVelocityInLocalPoint(const SimdVector3& rel_pos) const
btVector3 getVelocityInLocalPoint(const btVector3& rel_pos) const
{
//we also calculate lin/ang velocity for kinematic objects
return m_linearVelocity + m_angularVelocity.cross(rel_pos);
@@ -181,52 +185,84 @@ public:
// return (m_worldTransform(rel_pos) - m_interpolationWorldTransform(rel_pos)) / m_kinematicTimeStep;
}
void translate(const SimdVector3& v)
void translate(const btVector3& v)
{
m_worldTransform.getOrigin() += v;
}
void getAabb(SimdVector3& aabbMin,SimdVector3& aabbMax) const;
void getAabb(btVector3& aabbMin,btVector3& aabbMax) const;
inline float ComputeImpulseDenominator(const SimdPoint3& pos, const SimdVector3& normal) const
inline float ComputeImpulseDenominator(const btPoint3& pos, const btVector3& normal) const
{
SimdVector3 r0 = pos - getCenterOfMassPosition();
btVector3 r0 = pos - getCenterOfMassPosition();
SimdVector3 c0 = (r0).cross(normal);
btVector3 c0 = (r0).cross(normal);
SimdVector3 vec = (c0 * getInvInertiaTensorWorld()).cross(r0);
btVector3 vec = (c0 * getInvInertiaTensorWorld()).cross(r0);
return m_inverseMass + normal.dot(vec);
}
inline float ComputeAngularImpulseDenominator(const SimdVector3& axis) const
inline float ComputeAngularImpulseDenominator(const btVector3& axis) const
{
SimdVector3 vec = axis * getInvInertiaTensorWorld();
btVector3 vec = axis * getInvInertiaTensorWorld();
return axis.dot(vec);
}
inline void updateDeactivation(float timeStep)
{
if ( (GetActivationState() == ISLAND_SLEEPING) || (GetActivationState() == DISABLE_DEACTIVATION))
return;
if ((getLinearVelocity().length2() < gLinearSleepingTreshold*gLinearSleepingTreshold) &&
(getAngularVelocity().length2() < gAngularSleepingTreshold*gAngularSleepingTreshold))
{
m_deactivationTime += timeStep;
} else
{
m_deactivationTime=0.f;
SetActivationState(0);
}
}
inline bool wantsSleeping()
{
if (GetActivationState() == DISABLE_DEACTIVATION)
return false;
//disable deactivation
if (gDisableDeactivation || (gDeactivationTime == 0.f))
return false;
if ( (GetActivationState() == ISLAND_SLEEPING) || (GetActivationState() == WANTS_DEACTIVATION))
return true;
if (m_deactivationTime> gDeactivationTime)
{
return true;
}
return false;
}
private:
public:
const BroadphaseProxy* GetBroadphaseProxy() const
const btBroadphaseProxy* GetBroadphaseProxy() const
{
return m_broadphaseProxy;
}
BroadphaseProxy* GetBroadphaseProxy()
btBroadphaseProxy* GetBroadphaseProxy()
{
return m_broadphaseProxy;
}
void SetBroadphaseProxy(BroadphaseProxy* broadphaseProxy)
void SetBroadphaseProxy(btBroadphaseProxy* broadphaseProxy)
{
m_broadphaseProxy = broadphaseProxy;
}
@@ -235,18 +271,6 @@ public:
int m_contactSolverType;
int m_frictionSolverType;
/*
//unused
/// for ode solver-binding
dMatrix3 m_R;//temp
dMatrix3 m_I;
dMatrix3 m_invI;
int m_odeTag;
SimdVector3 m_tacc;//temp
SimdVector3 m_facc;
*/
int m_debugBodyId;

122
src/BulletDynamics/Dynamics/btSimpleDynamicsWorld.cpp
View File

@@ -117,4 +117,124 @@ void btSimpleDynamicsWorld::predictUnconstraintMotion(float timeStep)
}
}
}
}
}=======
#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"
btSimpleDynamicsWorld::btSimpleDynamicsWorld()
:btDynamicsWorld(new btCollisionDispatcher(),new btSimpleBroadphase()),
m_constraintSolver(new btSequentialImpulseConstraintSolver)
{
}
btSimpleDynamicsWorld::btSimpleDynamicsWorld(btDispatcher* dispatcher,btOverlappingPairCache* pairCache,btConstraintSolver* constraintSolver)
:btDynamicsWorld(dispatcher,pairCache),
m_constraintSolver(constraintSolver)
{
}
btSimpleDynamicsWorld::~btSimpleDynamicsWorld()
{
delete m_constraintSolver;
//delete the dispatcher and paircache
delete m_dispatcher1;
m_dispatcher1 = 0;
delete m_pairCache;
m_pairCache = 0;
}
void btSimpleDynamicsWorld::stepSimulation(float timeStep)
{
///apply gravity, predict motion
predictUnconstraintMotion(timeStep);
///perform collision detection
PerformDiscreteCollisionDetection();
///solve contact constraints
btPersistentManifold** manifoldPtr = ((btCollisionDispatcher*)m_dispatcher1)->getInternalManifoldPointer();
int numManifolds = m_dispatcher1->GetNumManifolds();
btContactSolverInfo infoGlobal;
infoGlobal.m_timeStep = timeStep;
btIDebugDraw* debugDrawer=0;
m_constraintSolver->SolveGroup(manifoldPtr, numManifolds,infoGlobal,debugDrawer);
///integrate transforms
integrateTransforms(timeStep);
updateAabbs();
}
void btSimpleDynamicsWorld::updateAabbs()
{
btTransform predictedTrans;
for (int i=0;i<m_collisionObjects.size();i++)
{
btCollisionObject* colObj = m_collisionObjects[i];
if (colObj->m_internalOwner)
{
btRigidBody* body = (btRigidBody*)colObj->m_internalOwner;
if (body->IsActive() && (!body->IsStatic()))
{
btPoint3 minAabb,maxAabb;
colObj->m_collisionShape->GetAabb(colObj->m_worldTransform, minAabb,maxAabb);
btSimpleBroadphase* bp = (btSimpleBroadphase*)m_pairCache;
bp->SetAabb(body->m_broadphaseHandle,minAabb,maxAabb);
}
}
}
}
void btSimpleDynamicsWorld::integrateTransforms(float timeStep)
{
btTransform predictedTrans;
for (int i=0;i<m_collisionObjects.size();i++)
{
btCollisionObject* colObj = m_collisionObjects[i];
if (colObj->m_internalOwner)
{
btRigidBody* body = (btRigidBody*)colObj->m_internalOwner;
if (body->IsActive() && (!body->IsStatic()))
{
body->predictIntegratedTransform(timeStep, predictedTrans);
body->proceedToTransform( predictedTrans);
}
}
}
}
void btSimpleDynamicsWorld::predictUnconstraintMotion(float timeStep)
{
for (int i=0;i<m_collisionObjects.size();i++)
{
btCollisionObject* colObj = m_collisionObjects[i];
if (colObj->m_internalOwner)
{
btRigidBody* body = (btRigidBody*)colObj->m_internalOwner;
body->m_cachedInvertedWorldTransform = body->m_worldTransform.inverse();
if (body->IsActive() && (!body->IsStatic()))
{
body->applyForces( timeStep);
body->integrateVelocities( timeStep);
body->predictIntegratedTransform(timeStep,body->m_interpolationWorldTransform);
}
}
}
}

57
src/BulletDynamics/Dynamics/btSimpleDynamicsWorld.h
View File

@@ -52,4 +52,59 @@ public:
};
#endif //BT_SIMPLE_DYNAMICS_WORLD_H
#endif //BT_SIMPLE_DYNAMICS_WORLD_H=======
/*
Bullet Continuous Collision Detection and Physics Library
Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/
This software is provided 'as-is', without any express or implied warranty.
In no event will the authors be held liable for any damages arising from the use of this software.
Permission is granted to anyone to use this software for any purpose,
including commercial applications, and to alter it and redistribute it freely,
subject to the following restrictions:
1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
3. This notice may not be removed or altered from any source distribution.
*/
#ifndef 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;
void predictUnconstraintMotion(float timeStep);
void integrateTransforms(float timeStep);
void updateAabbs();
public:
btSimpleDynamicsWorld(btDispatcher* dispatcher,btOverlappingPairCache* pairCache,btConstraintSolver* constraintSolver);
btSimpleDynamicsWorld();
virtual ~btSimpleDynamicsWorld();
virtual void stepSimulation( float timeStep);
};
#endif //BT_SIMPLE_DYNAMICS_WORLD_H

198
src/BulletDynamics/Vehicle/btRaycastVehicle.cpp
View File

@@ -12,8 +12,8 @@
#include "btRaycastVehicle.h"
#include "BulletDynamics/ConstraintSolver/btSolve2LinearConstraint.h"
#include "BulletDynamics/ConstraintSolver/btJacobianEntry.h"
#include "LinearMath/SimdQuaternion.h"
#include "LinearMath/SimdVector3.h"
#include "LinearMath/btQuaternion.h"
#include "LinearMath/btVector3.h"
#include "btVehicleRaycaster.h"
#include "btWheelInfo.h"
@@ -23,9 +23,9 @@
static RigidBody s_fixedObject( MassProps ( 0.0f, SimdVector3(0,0,0) ),0.f,0.f,0.f,0.f);
static btRigidBody s_fixedObject( btMassProps ( 0.0f, btVector3(0,0,0) ),0.f,0.f,0.f,0.f);
RaycastVehicle::RaycastVehicle(const VehicleTuning& tuning,RigidBody* chassis, VehicleRaycaster* raycaster )
btRaycastVehicle::btRaycastVehicle(const btVehicleTuning& tuning,btRigidBody* chassis, btVehicleRaycaster* raycaster )
:m_vehicleRaycaster(raycaster),
m_pitchControl(0.f)
{
@@ -37,7 +37,7 @@ m_pitchControl(0.f)
}
void RaycastVehicle::DefaultInit(const VehicleTuning& tuning)
void btRaycastVehicle::DefaultInit(const btVehicleTuning& tuning)
{
m_currentVehicleSpeedKmHour = 0.f;
m_steeringValue = 0.f;
@@ -46,7 +46,7 @@ void RaycastVehicle::DefaultInit(const VehicleTuning& tuning)
RaycastVehicle::~RaycastVehicle()
btRaycastVehicle::~btRaycastVehicle()
{
}
@@ -54,10 +54,10 @@ RaycastVehicle::~RaycastVehicle()
//
// basically most of the code is general for 2 or 4 wheel vehicles, but some of it needs to be reviewed
//
WheelInfo& RaycastVehicle::AddWheel( const SimdVector3& connectionPointCS, const SimdVector3& wheelDirectionCS0,const SimdVector3& wheelAxleCS, SimdScalar suspensionRestLength, SimdScalar wheelRadius,const VehicleTuning& tuning, bool isFrontWheel)
btWheelInfo& btRaycastVehicle::AddWheel( const btVector3& connectionPointCS, const btVector3& wheelDirectionCS0,const btVector3& wheelAxleCS, btScalar suspensionRestLength, btScalar wheelRadius,const btVehicleTuning& tuning, bool isFrontWheel)
{
WheelInfoConstructionInfo ci;
btWheelInfoConstructionInfo ci;
ci.m_chassisConnectionCS = connectionPointCS;
ci.m_wheelDirectionCS = wheelDirectionCS0;
@@ -71,9 +71,9 @@ WheelInfo& RaycastVehicle::AddWheel( const SimdVector3& connectionPointCS, const
ci.m_bIsFrontWheel = isFrontWheel;
ci.m_maxSuspensionTravelCm = tuning.m_maxSuspensionTravelCm;
m_wheelInfo.push_back( WheelInfo(ci));
m_wheelInfo.push_back( btWheelInfo(ci));
WheelInfo& wheel = m_wheelInfo[GetNumWheels()-1];
btWheelInfo& wheel = m_wheelInfo[GetNumWheels()-1];
UpdateWheelTransformsWS( wheel );
UpdateWheelTransform(GetNumWheels()-1);
@@ -83,33 +83,33 @@ WheelInfo& RaycastVehicle::AddWheel( const SimdVector3& connectionPointCS, const
const SimdTransform& RaycastVehicle::GetWheelTransformWS( int wheelIndex ) const
const btTransform& btRaycastVehicle::GetWheelTransformWS( int wheelIndex ) const
{
assert(wheelIndex < GetNumWheels());
const WheelInfo& wheel = m_wheelInfo[wheelIndex];
const btWheelInfo& wheel = m_wheelInfo[wheelIndex];
return wheel.m_worldTransform;
}
void RaycastVehicle::UpdateWheelTransform( int wheelIndex )
void btRaycastVehicle::UpdateWheelTransform( int wheelIndex )
{
WheelInfo& wheel = m_wheelInfo[ wheelIndex ];
btWheelInfo& wheel = m_wheelInfo[ wheelIndex ];
UpdateWheelTransformsWS(wheel);
SimdVector3 up = -wheel.m_raycastInfo.m_wheelDirectionWS;
const SimdVector3& right = wheel.m_raycastInfo.m_wheelAxleWS;
SimdVector3 fwd = up.cross(right);
btVector3 up = -wheel.m_raycastInfo.m_wheelDirectionWS;
const btVector3& right = wheel.m_raycastInfo.m_wheelAxleWS;
btVector3 fwd = up.cross(right);
fwd = fwd.normalize();
//rotate around steering over de wheelAxleWS
float steering = wheel.m_steering;
SimdQuaternion steeringOrn(up,steering);//wheel.m_steering);
SimdMatrix3x3 steeringMat(steeringOrn);
btQuaternion steeringOrn(up,steering);//wheel.m_steering);
btMatrix3x3 steeringMat(steeringOrn);
SimdQuaternion rotatingOrn(right,wheel.m_rotation);
SimdMatrix3x3 rotatingMat(rotatingOrn);
btQuaternion rotatingOrn(right,wheel.m_rotation);
btMatrix3x3 rotatingMat(rotatingOrn);
SimdMatrix3x3 basis2(
btMatrix3x3 basis2(
right[0],fwd[0],up[0],
right[1],fwd[1],up[1],
right[2],fwd[2],up[2]
@@ -121,14 +121,14 @@ void RaycastVehicle::UpdateWheelTransform( int wheelIndex )
);
}
void RaycastVehicle::ResetSuspension()
void btRaycastVehicle::ResetSuspension()
{
std::vector<WheelInfo>::iterator wheelIt;
std::vector<btWheelInfo>::iterator wheelIt;
for (wheelIt = m_wheelInfo.begin();
!(wheelIt == m_wheelInfo.end());wheelIt++)
{
WheelInfo& wheel = *wheelIt;
btWheelInfo& wheel = *wheelIt;
wheel.m_raycastInfo.m_suspensionLength = wheel.GetSuspensionRestLength();
wheel.m_suspensionRelativeVelocity = 0.0f;
@@ -138,34 +138,34 @@ void RaycastVehicle::ResetSuspension()
}
}
void RaycastVehicle::UpdateWheelTransformsWS(WheelInfo& wheel )
void btRaycastVehicle::UpdateWheelTransformsWS(btWheelInfo& wheel )
{
wheel.m_raycastInfo.m_isInContact = false;
const SimdTransform& chassisTrans = GetRigidBody()->getCenterOfMassTransform();
const btTransform& chassisTrans = GetRigidBody()->getCenterOfMassTransform();
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;
}
SimdScalar RaycastVehicle::Raycast(WheelInfo& wheel)
btScalar btRaycastVehicle::Raycast(btWheelInfo& wheel)
{
UpdateWheelTransformsWS( wheel );
SimdScalar depth = -1;
btScalar depth = -1;
SimdScalar raylen = wheel.GetSuspensionRestLength()+wheel.m_wheelsRadius;
btScalar raylen = wheel.GetSuspensionRestLength()+wheel.m_wheelsRadius;
SimdVector3 rayvector = wheel.m_raycastInfo.m_wheelDirectionWS * (raylen);
const SimdVector3& source = wheel.m_raycastInfo.m_hardPointWS;
btVector3 rayvector = wheel.m_raycastInfo.m_wheelDirectionWS * (raylen);
const btVector3& source = wheel.m_raycastInfo.m_hardPointWS;
wheel.m_raycastInfo.m_contactPointWS = source + rayvector;
const SimdVector3& target = wheel.m_raycastInfo.m_contactPointWS;
const btVector3& target = wheel.m_raycastInfo.m_contactPointWS;
SimdScalar param = 0.f;
btScalar param = 0.f;
VehicleRaycaster::VehicleRaycasterResult rayResults;
btVehicleRaycaster::btVehicleRaycasterResult rayResults;
void* object = m_vehicleRaycaster->CastRay(source,target,rayResults);
@@ -182,7 +182,7 @@ SimdScalar RaycastVehicle::Raycast(WheelInfo& wheel)
//wheel.m_raycastInfo.m_groundObject = object;
SimdScalar hitDistance = param*raylen;
btScalar hitDistance = param*raylen;
wheel.m_raycastInfo.m_suspensionLength = hitDistance - wheel.m_wheelsRadius;
//clamp on max suspension travel
@@ -199,14 +199,14 @@ SimdScalar RaycastVehicle::Raycast(WheelInfo& wheel)
wheel.m_raycastInfo.m_contactPointWS = rayResults.m_hitPointInWorld;
SimdScalar denominator= wheel.m_raycastInfo.m_contactNormalWS.dot( wheel.m_raycastInfo.m_wheelDirectionWS );
btScalar denominator= wheel.m_raycastInfo.m_contactNormalWS.dot( wheel.m_raycastInfo.m_wheelDirectionWS );
SimdVector3 chassis_velocity_at_contactPoint;
SimdVector3 relpos = wheel.m_raycastInfo.m_contactPointWS-GetRigidBody()->getCenterOfMassPosition();
btVector3 chassis_velocity_at_contactPoint;
btVector3 relpos = wheel.m_raycastInfo.m_contactPointWS-GetRigidBody()->getCenterOfMassPosition();
chassis_velocity_at_contactPoint = GetRigidBody()->getVelocityInLocalPoint(relpos);
SimdScalar projVel = wheel.m_raycastInfo.m_contactNormalWS.dot( chassis_velocity_at_contactPoint );
btScalar projVel = wheel.m_raycastInfo.m_contactNormalWS.dot( chassis_velocity_at_contactPoint );
if ( denominator >= -0.1f)
{
@@ -215,7 +215,7 @@ SimdScalar RaycastVehicle::Raycast(WheelInfo& wheel)
}
else
{
SimdScalar inv = -1.f / denominator;
btScalar inv = -1.f / denominator;
wheel.m_suspensionRelativeVelocity = projVel * inv;
wheel.m_clippedInvContactDotSuspension = inv;
}
@@ -233,13 +233,13 @@ SimdScalar RaycastVehicle::Raycast(WheelInfo& wheel)
}
void RaycastVehicle::UpdateVehicle( SimdScalar step )
void btRaycastVehicle::UpdateVehicle( btScalar step )
{
m_currentVehicleSpeedKmHour = 3.6f * GetRigidBody()->getLinearVelocity().length();
const SimdTransform& chassisTrans = GetRigidBody()->getCenterOfMassTransform();
SimdVector3 forwardW (
const btTransform& chassisTrans = GetRigidBody()->getCenterOfMassTransform();
btVector3 forwardW (
chassisTrans.getBasis()[0][m_indexForwardAxis],
chassisTrans.getBasis()[1][m_indexForwardAxis],
chassisTrans.getBasis()[2][m_indexForwardAxis]);
@@ -252,12 +252,12 @@ void RaycastVehicle::UpdateVehicle( SimdScalar step )
//
// simulate suspension
//
std::vector<WheelInfo>::iterator wheelIt;
std::vector<btWheelInfo>::iterator wheelIt;
int i=0;
for (wheelIt = m_wheelInfo.begin();
!(wheelIt == m_wheelInfo.end());wheelIt++,i++)
{
SimdScalar depth;
btScalar depth;
depth = Raycast( *wheelIt );
}
@@ -268,7 +268,7 @@ void RaycastVehicle::UpdateVehicle( SimdScalar step )
!(wheelIt == m_wheelInfo.end());wheelIt++)
{
//apply suspension force
WheelInfo& wheel = *wheelIt;
btWheelInfo& wheel = *wheelIt;
float suspensionForce = wheel.m_wheelsSuspensionForce;
@@ -277,8 +277,8 @@ void RaycastVehicle::UpdateVehicle( SimdScalar step )
{
suspensionForce = gMaxSuspensionForce;
}
SimdVector3 impulse = wheel.m_raycastInfo.m_contactNormalWS * suspensionForce * step;
SimdVector3 relpos = wheel.m_raycastInfo.m_contactPointWS - GetRigidBody()->getCenterOfMassPosition();
btVector3 impulse = wheel.m_raycastInfo.m_contactNormalWS * suspensionForce * step;
btVector3 relpos = wheel.m_raycastInfo.m_contactPointWS - GetRigidBody()->getCenterOfMassPosition();
GetRigidBody()->applyImpulse(impulse, relpos);
@@ -292,21 +292,21 @@ void RaycastVehicle::UpdateVehicle( SimdScalar step )
for (wheelIt = m_wheelInfo.begin();
!(wheelIt == m_wheelInfo.end());wheelIt++)
{
WheelInfo& wheel = *wheelIt;
SimdVector3 relpos = wheel.m_raycastInfo.m_hardPointWS - GetRigidBody()->getCenterOfMassPosition();
SimdVector3 vel = GetRigidBody()->getVelocityInLocalPoint( relpos );
btWheelInfo& wheel = *wheelIt;
btVector3 relpos = wheel.m_raycastInfo.m_hardPointWS - GetRigidBody()->getCenterOfMassPosition();
btVector3 vel = GetRigidBody()->getVelocityInLocalPoint( relpos );
if (wheel.m_raycastInfo.m_isInContact)
{
SimdVector3 fwd (
btVector3 fwd (
GetRigidBody()->getCenterOfMassTransform().getBasis()[0][m_indexForwardAxis],
GetRigidBody()->getCenterOfMassTransform().getBasis()[1][m_indexForwardAxis],
GetRigidBody()->getCenterOfMassTransform().getBasis()[2][m_indexForwardAxis]);
SimdScalar proj = fwd.dot(wheel.m_raycastInfo.m_contactNormalWS);
btScalar proj = fwd.dot(wheel.m_raycastInfo.m_contactNormalWS);
fwd -= wheel.m_raycastInfo.m_contactNormalWS * proj;
SimdScalar proj2 = fwd.dot(vel);
btScalar proj2 = fwd.dot(vel);
wheel.m_deltaRotation = (proj2 * step) / (wheel.m_wheelsRadius);
wheel.m_rotation += wheel.m_deltaRotation;
@@ -325,69 +325,69 @@ void RaycastVehicle::UpdateVehicle( SimdScalar step )
}
void RaycastVehicle::SetSteeringValue(SimdScalar steering,int wheel)
void btRaycastVehicle::SetSteeringValue(btScalar steering,int wheel)
{
assert(wheel>=0 && wheel < GetNumWheels());
WheelInfo& wheelInfo = GetWheelInfo(wheel);
btWheelInfo& wheelInfo = GetWheelInfo(wheel);
wheelInfo.m_steering = steering;
}
SimdScalar RaycastVehicle::GetSteeringValue(int wheel) const
btScalar btRaycastVehicle::GetSteeringValue(int wheel) const
{
return GetWheelInfo(wheel).m_steering;
}
void RaycastVehicle::ApplyEngineForce(SimdScalar force, int wheel)
void btRaycastVehicle::ApplyEngineForce(btScalar force, int wheel)
{
assert(wheel>=0 && wheel < GetNumWheels());
WheelInfo& wheelInfo = GetWheelInfo(wheel);
btWheelInfo& wheelInfo = GetWheelInfo(wheel);
wheelInfo.m_engineForce = force;
}
const WheelInfo& RaycastVehicle::GetWheelInfo(int index) const
const btWheelInfo& btRaycastVehicle::GetWheelInfo(int index) const
{
ASSERT((index >= 0) && (index < GetNumWheels()));
return m_wheelInfo[index];
}
WheelInfo& RaycastVehicle::GetWheelInfo(int index)
btWheelInfo& btRaycastVehicle::GetWheelInfo(int index)
{
ASSERT((index >= 0) && (index < GetNumWheels()));
return m_wheelInfo[index];
}
void RaycastVehicle::SetBrake(float brake,int wheelIndex)
void btRaycastVehicle::SetBrake(float brake,int wheelIndex)
{
ASSERT((wheelIndex >= 0) && (wheelIndex < GetNumWheels()));
GetWheelInfo(wheelIndex).m_brake;
}
void RaycastVehicle::UpdateSuspension(SimdScalar deltaTime)
void btRaycastVehicle::UpdateSuspension(btScalar deltaTime)
{
SimdScalar chassisMass = 1.f / m_chassisBody->getInvMass();
btScalar chassisMass = 1.f / m_chassisBody->getInvMass();
for (int w_it=0; w_it<GetNumWheels(); w_it++)
{
WheelInfo &wheel_info = m_wheelInfo[w_it];
btWheelInfo &wheel_info = m_wheelInfo[w_it];
if ( wheel_info.m_raycastInfo.m_isInContact )
{
SimdScalar force;
btScalar force;
// Spring
{
SimdScalar susp_length = wheel_info.GetSuspensionRestLength();
SimdScalar current_length = wheel_info.m_raycastInfo.m_suspensionLength;
btScalar susp_length = wheel_info.GetSuspensionRestLength();
btScalar current_length = wheel_info.m_raycastInfo.m_suspensionLength;
SimdScalar length_diff = (susp_length - current_length);
btScalar length_diff = (susp_length - current_length);
force = wheel_info.m_suspensionStiffness
* length_diff * wheel_info.m_clippedInvContactDotSuspension;
@@ -395,9 +395,9 @@ void RaycastVehicle::UpdateSuspension(SimdScalar deltaTime)
// Damper
{
SimdScalar projected_rel_vel = wheel_info.m_suspensionRelativeVelocity;
btScalar projected_rel_vel = wheel_info.m_suspensionRelativeVelocity;
{
SimdScalar susp_damping;
btScalar susp_damping;
if ( projected_rel_vel < 0.0f )
{
susp_damping = wheel_info.m_wheelsDampingCompression;
@@ -426,7 +426,7 @@ void RaycastVehicle::UpdateSuspension(SimdScalar deltaTime)
}
float sideFrictionStiffness2 = 1.0f;
void RaycastVehicle::UpdateFriction(SimdScalar timeStep)
void btRaycastVehicle::UpdateFriction(btScalar timeStep)
{
//calculate the impulse, so that the wheels don't move sidewards
@@ -435,10 +435,10 @@ void RaycastVehicle::UpdateFriction(SimdScalar timeStep)
return;
SimdVector3* forwardWS = new SimdVector3[numWheel];
SimdVector3* axle = new SimdVector3[numWheel];
SimdScalar* forwardImpulse = new SimdScalar[numWheel];
SimdScalar* sideImpulse = new SimdScalar[numWheel];
btVector3* forwardWS = new btVector3[numWheel];
btVector3* axle = new btVector3[numWheel];
btScalar* forwardImpulse = new btScalar[numWheel];
btScalar* sideImpulse = new btScalar[numWheel];
int numWheelsOnGround = 0;
@@ -446,8 +446,8 @@ void RaycastVehicle::UpdateFriction(SimdScalar timeStep)
//collapse all those loops into one!
for (int i=0;i<GetNumWheels();i++)
{
WheelInfo& wheelInfo = m_wheelInfo[i];
class RigidBody* groundObject = (class RigidBody*) wheelInfo.m_raycastInfo.m_groundObject;
btWheelInfo& wheelInfo = m_wheelInfo[i];
class btRigidBody* groundObject = (class btRigidBody*) wheelInfo.m_raycastInfo.m_groundObject;
if (groundObject)
numWheelsOnGround++;
sideImpulse[i] = 0.f;
@@ -460,23 +460,23 @@ void RaycastVehicle::UpdateFriction(SimdScalar timeStep)
for (int i=0;i<GetNumWheels();i++)
{
WheelInfo& wheelInfo = m_wheelInfo[i];
btWheelInfo& wheelInfo = m_wheelInfo[i];
class RigidBody* groundObject = (class RigidBody*) wheelInfo.m_raycastInfo.m_groundObject;
class btRigidBody* groundObject = (class btRigidBody*) wheelInfo.m_raycastInfo.m_groundObject;
if (groundObject)
{
const SimdTransform& wheelTrans = GetWheelTransformWS( i );
const btTransform& wheelTrans = GetWheelTransformWS( i );
SimdMatrix3x3 wheelBasis0 = wheelTrans.getBasis();
axle[i] = SimdVector3(
btMatrix3x3 wheelBasis0 = wheelTrans.getBasis();
axle[i] = btVector3(
wheelBasis0[0][m_indexRightAxis],
wheelBasis0[1][m_indexRightAxis],
wheelBasis0[2][m_indexRightAxis]);
const SimdVector3& surfNormalWS = wheelInfo.m_raycastInfo.m_contactNormalWS;
SimdScalar proj = axle[i].dot(surfNormalWS);
const btVector3& surfNormalWS = wheelInfo.m_raycastInfo.m_contactNormalWS;
btScalar proj = axle[i].dot(surfNormalWS);
axle[i] -= surfNormalWS * proj;
axle[i] = axle[i].normalize();
@@ -496,15 +496,15 @@ void RaycastVehicle::UpdateFriction(SimdScalar timeStep)
}
}
SimdScalar sideFactor = 1.f;
SimdScalar fwdFactor = 0.5;
btScalar sideFactor = 1.f;
btScalar fwdFactor = 0.5;
bool sliding = false;
{
for (int wheel =0;wheel <GetNumWheels();wheel++)
{
WheelInfo& wheelInfo = m_wheelInfo[wheel];
class RigidBody* groundObject = (class RigidBody*) wheelInfo.m_raycastInfo.m_groundObject;
btWheelInfo& wheelInfo = m_wheelInfo[wheel];
class btRigidBody* groundObject = (class btRigidBody*) wheelInfo.m_raycastInfo.m_groundObject;
forwardImpulse[wheel] = 0.f;
@@ -514,10 +514,10 @@ void RaycastVehicle::UpdateFriction(SimdScalar timeStep)
{
m_wheelInfo[wheel].m_skidInfo= 1.f;
SimdScalar maximp = wheelInfo.m_wheelsSuspensionForce * timeStep * wheelInfo.m_frictionSlip;
SimdScalar maximpSide = maximp;
btScalar maximp = wheelInfo.m_wheelsSuspensionForce * timeStep * wheelInfo.m_frictionSlip;
btScalar maximpSide = maximp;
SimdScalar maximpSquared = maximp * maximpSide;
btScalar maximpSquared = maximp * maximpSide;
forwardImpulse[wheel] = wheelInfo.m_engineForce* timeStep;
@@ -530,7 +530,7 @@ void RaycastVehicle::UpdateFriction(SimdScalar timeStep)
{
sliding = true;
SimdScalar factor = maximp / SimdSqrt(impulseSquared);
btScalar factor = maximp / btSqrt(impulseSquared);
m_wheelInfo[wheel].m_skidInfo *= factor;
}
@@ -561,9 +561,9 @@ void RaycastVehicle::UpdateFriction(SimdScalar timeStep)
{
for (int wheel = 0;wheel<GetNumWheels() ; wheel++)
{
WheelInfo& wheelInfo = m_wheelInfo[wheel];
btWheelInfo& wheelInfo = m_wheelInfo[wheel];
SimdVector3 rel_pos = wheelInfo.m_raycastInfo.m_contactPointWS -
btVector3 rel_pos = wheelInfo.m_raycastInfo.m_contactPointWS -
m_chassisBody->getCenterOfMassPosition();
if (forwardImpulse[wheel] != 0.f)
@@ -572,13 +572,13 @@ void RaycastVehicle::UpdateFriction(SimdScalar timeStep)
}
if (sideImpulse[wheel] != 0.f)
{
class RigidBody* groundObject = (class RigidBody*) m_wheelInfo[wheel].m_raycastInfo.m_groundObject;
class btRigidBody* groundObject = (class btRigidBody*) m_wheelInfo[wheel].m_raycastInfo.m_groundObject;
SimdVector3 rel_pos2 = wheelInfo.m_raycastInfo.m_contactPointWS -
btVector3 rel_pos2 = wheelInfo.m_raycastInfo.m_contactPointWS -
groundObject->getCenterOfMassPosition();
SimdVector3 sideImp = axle[wheel] * sideImpulse[wheel];
btVector3 sideImp = axle[wheel] * sideImpulse[wheel];
rel_pos[2] *= wheelInfo.m_rollInfluence;
m_chassisBody->applyImpulse(sideImp,rel_pos);

60
src/BulletDynamics/Vehicle/btRaycastVehicle.h
View File

@@ -14,21 +14,21 @@
#include "BulletDynamics/Dynamics/btRigidBody.h"
#include "BulletDynamics/ConstraintSolver/btTypedConstraint.h"
struct MassProps;
struct btMassProps;
#include "btWheelInfo.h"
struct VehicleRaycaster;
class VehicleTuning;
struct btVehicleRaycaster;
class btVehicleTuning;
///Raycast vehicle, very special constraint that turn a rigidbody into a vehicle.
class RaycastVehicle : public TypedConstraint
class btRaycastVehicle : public btTypedConstraint
{
public:
class VehicleTuning
class btVehicleTuning
{
public:
VehicleTuning()
btVehicleTuning()
:m_suspensionStiffness(5.88f),
m_suspensionCompression(0.83f),
m_suspensionDamping(0.88f),
@@ -45,65 +45,65 @@ public:
};
private:
SimdScalar m_tau;
SimdScalar m_damping;
VehicleRaycaster* m_vehicleRaycaster;
btScalar m_tau;
btScalar m_damping;
btVehicleRaycaster* m_vehicleRaycaster;
float m_pitchControl;
float m_steeringValue;
float m_currentVehicleSpeedKmHour;
RigidBody* m_chassisBody;
btRigidBody* m_chassisBody;
int m_indexRightAxis;
int m_indexUpAxis;
int m_indexForwardAxis;
void DefaultInit(const VehicleTuning& tuning);
void DefaultInit(const btVehicleTuning& tuning);
public:
//constructor to create a car from an existing rigidbody
RaycastVehicle(const VehicleTuning& tuning,RigidBody* chassis, VehicleRaycaster* raycaster );
btRaycastVehicle(const btVehicleTuning& tuning,btRigidBody* chassis, btVehicleRaycaster* raycaster );
virtual ~RaycastVehicle() ;
virtual ~btRaycastVehicle() ;
SimdScalar Raycast(WheelInfo& wheel);
btScalar Raycast(btWheelInfo& wheel);
virtual void UpdateVehicle(SimdScalar step);
virtual void UpdateVehicle(btScalar step);
void ResetSuspension();
SimdScalar GetSteeringValue(int wheel) const;
btScalar GetSteeringValue(int wheel) const;
void SetSteeringValue(SimdScalar steering,int wheel);
void SetSteeringValue(btScalar steering,int wheel);
void ApplyEngineForce(SimdScalar force, int wheel);
void ApplyEngineForce(btScalar force, int wheel);
const SimdTransform& GetWheelTransformWS( int wheelIndex ) const;
const btTransform& GetWheelTransformWS( int wheelIndex ) const;
void UpdateWheelTransform( int wheelIndex );
void SetRaycastWheelInfo( int wheelIndex , bool isInContact, const SimdVector3& hitPoint, const SimdVector3& hitNormal,SimdScalar depth);
void SetRaycastWheelInfo( int wheelIndex , bool isInContact, const btVector3& hitPoint, const btVector3& hitNormal,btScalar depth);
WheelInfo& AddWheel( const SimdVector3& connectionPointCS0, const SimdVector3& wheelDirectionCS0,const SimdVector3& wheelAxleCS,SimdScalar suspensionRestLength,SimdScalar wheelRadius,const VehicleTuning& tuning, bool isFrontWheel);
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 m_wheelInfo.size();
}
std::vector<WheelInfo> m_wheelInfo;
std::vector<btWheelInfo> m_wheelInfo;
const WheelInfo& GetWheelInfo(int index) const;
const btWheelInfo& GetWheelInfo(int index) const;
WheelInfo& GetWheelInfo(int index);
btWheelInfo& GetWheelInfo(int index);
void UpdateWheelTransformsWS(WheelInfo& wheel );
void UpdateWheelTransformsWS(btWheelInfo& wheel );
void SetBrake(float brake,int wheelIndex);
@@ -113,18 +113,18 @@ public:
m_pitchControl = pitch;
}
void UpdateSuspension(SimdScalar deltaTime);
void UpdateSuspension(btScalar deltaTime);
void UpdateFriction(SimdScalar timeStep);
void UpdateFriction(btScalar timeStep);
inline RigidBody* GetRigidBody()
inline btRigidBody* GetRigidBody()
{
return m_chassisBody;
}
const RigidBody* GetRigidBody() const
const btRigidBody* GetRigidBody() const
{
return m_chassisBody;
}
@@ -155,7 +155,7 @@ public:
//not yet
}
virtual void SolveConstraint(SimdScalar timeStep)
virtual void SolveConstraint(btScalar timeStep)
{
//not yet
}

20
src/BulletDynamics/Vehicle/btVehicleRaycaster.h
View File

@@ -11,23 +11,23 @@
#ifndef VEHICLE_RAYCASTER_H
#define VEHICLE_RAYCASTER_H
#include "LinearMath/SimdVector3.h"
#include "LinearMath/btVector3.h"
/// VehicleRaycaster is provides interface for between vehicle simulation and raycasting
struct VehicleRaycaster
/// btVehicleRaycaster is provides interface for between vehicle simulation and raycasting
struct btVehicleRaycaster
{
virtual ~VehicleRaycaster()
virtual ~btVehicleRaycaster()
{
}
struct VehicleRaycasterResult
struct btVehicleRaycasterResult
{
VehicleRaycasterResult() :m_distFraction(-1.f){};
SimdVector3 m_hitPointInWorld;
SimdVector3 m_hitNormalInWorld;
SimdScalar m_distFraction;
btVehicleRaycasterResult() :m_distFraction(-1.f){};
btVector3 m_hitPointInWorld;
btVector3 m_hitNormalInWorld;
btScalar m_distFraction;
};
virtual void* CastRay(const SimdVector3& from,const SimdVector3& to, VehicleRaycasterResult& result) = 0;
virtual void* CastRay(const btVector3& from,const btVector3& to, btVehicleRaycasterResult& result) = 0;
};

14
src/BulletDynamics/Vehicle/btWheelInfo.cpp
View File

@@ -12,25 +12,25 @@
#include "BulletDynamics/Dynamics/btRigidBody.h" // for pointvelocity
SimdScalar WheelInfo::GetSuspensionRestLength() const
btScalar btWheelInfo::GetSuspensionRestLength() const
{
return m_suspensionRestLength1;
}
void WheelInfo::UpdateWheel(const RigidBody& chassis,RaycastInfo& raycastInfo)
void btWheelInfo::UpdateWheel(const btRigidBody& chassis,RaycastInfo& raycastInfo)
{
if (m_raycastInfo.m_isInContact)
{
SimdScalar project= m_raycastInfo.m_contactNormalWS.dot( m_raycastInfo.m_wheelDirectionWS );
SimdVector3 chassis_velocity_at_contactPoint;
SimdVector3 relpos = m_raycastInfo.m_contactPointWS - chassis.getCenterOfMassPosition();
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 );
SimdScalar projVel = m_raycastInfo.m_contactNormalWS.dot( chassis_velocity_at_contactPoint );
btScalar projVel = m_raycastInfo.m_contactNormalWS.dot( chassis_velocity_at_contactPoint );
if ( project >= -0.1f)
{
m_suspensionRelativeVelocity = 0.0f;
@@ -38,7 +38,7 @@ void WheelInfo::UpdateWheel(const RigidBody& chassis,RaycastInfo& raycastInfo)
}
else
{
SimdScalar inv = -1.f / project;
btScalar inv = -1.f / project;
m_suspensionRelativeVelocity = projVel * inv;
m_clippedInvContactDotSuspension = inv;
}

84
src/BulletDynamics/Vehicle/btWheelInfo.h
View File

@@ -11,19 +11,19 @@
#ifndef WHEEL_INFO_H
#define WHEEL_INFO_H
#include "LinearMath/SimdVector3.h"
#include "LinearMath/SimdTransform.h"
#include "LinearMath/btVector3.h"
#include "LinearMath/btTransform.h"
class RigidBody;
class btRigidBody;
struct WheelInfoConstructionInfo
struct btWheelInfoConstructionInfo
{
SimdVector3 m_chassisConnectionCS;
SimdVector3 m_wheelDirectionCS;
SimdVector3 m_wheelAxleCS;
SimdScalar m_suspensionRestLength;
SimdScalar m_maxSuspensionTravelCm;
SimdScalar m_wheelRadius;
btVector3 m_chassisConnectionCS;
btVector3 m_wheelDirectionCS;
btVector3 m_wheelAxleCS;
btScalar m_suspensionRestLength;
btScalar m_maxSuspensionTravelCm;
btScalar m_wheelRadius;
float m_suspensionStiffness;
float m_wheelsDampingCompression;
@@ -33,51 +33,51 @@ struct WheelInfoConstructionInfo
};
/// WheelInfo contains information per wheel about friction and suspension.
struct WheelInfo
/// btWheelInfo contains information per wheel about friction and suspension.
struct btWheelInfo
{
struct RaycastInfo
{
//set by raycaster
SimdVector3 m_contactNormalWS;//contactnormal
SimdVector3 m_contactPointWS;//raycast hitpoint
SimdScalar m_suspensionLength;
SimdVector3 m_hardPointWS;//raycast starting point
SimdVector3 m_wheelDirectionWS; //direction in worldspace
SimdVector3 m_wheelAxleWS; // axle in worldspace
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;
SimdTransform m_worldTransform;
btTransform m_worldTransform;
SimdVector3 m_chassisConnectionPointCS; //const
SimdVector3 m_wheelDirectionCS;//const
SimdVector3 m_wheelAxleCS; // const or modified by steering
SimdScalar m_suspensionRestLength1;//const
SimdScalar m_maxSuspensionTravelCm;
SimdScalar GetSuspensionRestLength() const;
SimdScalar m_wheelsRadius;//const
SimdScalar m_suspensionStiffness;//const
SimdScalar m_wheelsDampingCompression;//const
SimdScalar m_wheelsDampingRelaxation;//const
SimdScalar m_frictionSlip;
SimdScalar m_steering;
SimdScalar m_rotation;
SimdScalar m_deltaRotation;
SimdScalar m_rollInfluence;
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;
SimdScalar m_engineForce;
btScalar m_engineForce;
SimdScalar m_brake;
btScalar m_brake;
bool m_bIsFrontWheel;
void* m_clientInfo;//can be used to store pointer to sync transforms...
WheelInfo(WheelInfoConstructionInfo& ci)
btWheelInfo(btWheelInfoConstructionInfo& ci)
{
@@ -102,13 +102,13 @@ struct WheelInfo
}
void UpdateWheel(const RigidBody& chassis,RaycastInfo& raycastInfo);
void UpdateWheel(const btRigidBody& chassis,RaycastInfo& raycastInfo);
SimdScalar m_clippedInvContactDotSuspension;
SimdScalar m_suspensionRelativeVelocity;
btScalar m_clippedInvContactDotSuspension;
btScalar m_suspensionRelativeVelocity;
//calculated by suspension
SimdScalar m_wheelsSuspensionForce;
SimdScalar m_skidInfo;
btScalar m_wheelsSuspensionForce;
btScalar m_skidInfo;
};