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

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 ;