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

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;
}