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

add .bullet extension in File/Open on Mac

Browse Source 
add importer support for FIXED_CONSTRAINT_TYPE, to be replaced by btGeneric6DofSpring2Constraint
btFixedConstraint now derives from btGeneric6DofSpring2Constraint
This commit is contained in:
Erwin Coumans
2015-05-03 09:23:35 -07:00
parent 7a0288bf56
commit 59b511a14e
4 changed files with 76 additions and 177 deletions
Download Patch File Download Diff File Expand all files Collapse all files

63
Extras/Serialize/BulletWorldImporter/btWorldImporter.cpp
View File

@@ -1011,6 +1011,37 @@ void btWorldImporter::convertConstraintFloat(btTypedConstraintFloatData* constra
break; break;
} }
case FIXED_CONSTRAINT_TYPE:
{
btGeneric6DofSpring2Constraint* dof = 0;
if (rbA && rbB)
{
btTransform rbAFrame,rbBFrame;
//compute a shared world frame, and compute frameInA, frameInB relative to this
btTransform sharedFrame;
sharedFrame.setIdentity();
btVector3 centerPos = btScalar(0.5)*(rbA->getWorldTransform().getOrigin()+
rbB->getWorldTransform().getOrigin());
sharedFrame.setOrigin(centerPos);
rbAFrame = rbA->getWorldTransform().inverse()*sharedFrame;
rbBFrame = rbB->getWorldTransform().inverse()*sharedFrame;
dof = createGeneric6DofSpring2Constraint(*rbA,*rbB,rbAFrame,rbBFrame, RO_XYZ);
dof->setLinearUpperLimit(btVector3(0,0,0));
dof->setLinearLowerLimit(btVector3(0,0,0));
dof->setAngularUpperLimit(btVector3(0,0,0));
dof->setAngularLowerLimit(btVector3(0,0,0));
} else
{
printf("Error in btWorldImporter::createGeneric6DofSpring2Constraint: requires rbA && rbB\n");
}
constraint = dof;
break;
}
default: default:
{ {
printf("unknown constraint type\n"); printf("unknown constraint type\n");
@@ -1310,6 +1341,38 @@ void btWorldImporter::convertConstraintDouble(btTypedConstraintDoubleData* const
break; break;
} }
case FIXED_CONSTRAINT_TYPE:
{
btGeneric6DofSpring2Constraint* dof = 0;
if (rbA && rbB)
{
btTransform rbAFrame,rbBFrame;
//compute a shared world frame, and compute frameInA, frameInB relative to this
btTransform sharedFrame;
sharedFrame.setIdentity();
btVector3 centerPos = btScalar(0.5)*(rbA->getWorldTransform().getOrigin()+
rbB->getWorldTransform().getOrigin());
sharedFrame.setOrigin(centerPos);
rbAFrame = rbA->getWorldTransform().inverse()*sharedFrame;
rbBFrame = rbB->getWorldTransform().inverse()*sharedFrame;
dof = createGeneric6DofSpring2Constraint(*rbA,*rbB,rbAFrame,rbBFrame, RO_XYZ);
dof->setLinearUpperLimit(btVector3(0,0,0));
dof->setLinearLowerLimit(btVector3(0,0,0));
dof->setAngularUpperLimit(btVector3(0,0,0));
dof->setAngularLowerLimit(btVector3(0,0,0));
} else
{
printf("Error in btWorldImporter::createGeneric6DofSpring2Constraint: requires rbA && rbB\n");
}
constraint = dof;
break;
}
default: default:
{ {
printf("unknown constraint type\n"); printf("unknown constraint type\n");

2
examples/OpenGLWindow/MacOpenGLWindow.mm
View File

@@ -1054,7 +1054,7 @@ int MacOpenGLWindow::fileOpenDialog(char* filename, int maxNameLength)
NSOpenGLContext *foo = [NSOpenGLContext currentContext]; NSOpenGLContext *foo = [NSOpenGLContext currentContext];
// get the url of a .txt file // get the url of a .txt file
NSOpenPanel * zOpenPanel = [NSOpenPanel openPanel]; NSOpenPanel * zOpenPanel = [NSOpenPanel openPanel];
NSArray * zAryOfExtensions = [NSArray arrayWithObject:@"urdf"]; NSArray * zAryOfExtensions = [NSArray arrayWithObjects:@"urdf",@"bullet",nil];
[zOpenPanel setAllowedFileTypes:zAryOfExtensions]; [zOpenPanel setAllowedFileTypes:zAryOfExtensions];
NSInteger zIntResult = [zOpenPanel runModal]; NSInteger zIntResult = [zOpenPanel runModal];

165
src/BulletDynamics/ConstraintSolver/btFixedConstraint.cpp
View File

@@ -21,166 +21,17 @@ subject to the following restrictions:
btFixedConstraint::btFixedConstraint(btRigidBody& rbA,btRigidBody& rbB, const btTransform& frameInA,const btTransform& frameInB) btFixedConstraint::btFixedConstraint(btRigidBody& rbA,btRigidBody& rbB, const btTransform& frameInA,const btTransform& frameInB)
:btTypedConstraint(FIXED_CONSTRAINT_TYPE,rbA,rbB) :btGeneric6DofSpring2Constraint(rbA,rbB,frameInA,frameInB)
{ {
m_frameInA = frameInA; setAngularLowerLimit(btVector3(0,0,0));
m_frameInB = frameInB; setAngularUpperLimit(btVector3(0,0,0));
setLinearLowerLimit(btVector3(0,0,0));
setLinearUpperLimit(btVector3(0,0,0));
} }
btFixedConstraint::~btFixedConstraint () btFixedConstraint::~btFixedConstraint ()
{ {
} }
void btFixedConstraint::getInfo1 (btConstraintInfo1* info)
{
info->m_numConstraintRows = 6;
info->nub = 0;
}
void btFixedConstraint::getInfo2 (btConstraintInfo2* info)
{
//fix the 3 linear degrees of freedom
const btTransform& transA = m_rbA.getCenterOfMassTransform();
const btTransform& transB = m_rbB.getCenterOfMassTransform();
const btVector3& worldPosA = m_rbA.getCenterOfMassTransform().getOrigin();
const btMatrix3x3& worldOrnA = m_rbA.getCenterOfMassTransform().getBasis();
const btVector3& worldPosB= m_rbB.getCenterOfMassTransform().getOrigin();
const btMatrix3x3& worldOrnB = m_rbB.getCenterOfMassTransform().getBasis();
info->m_J1linearAxis[0] = 1;
info->m_J1linearAxis[info->rowskip+1] = 1;
info->m_J1linearAxis[2*info->rowskip+2] = 1;
btVector3 a1 = worldOrnA * m_frameInA.getOrigin();
{
btVector3* angular0 = (btVector3*)(info->m_J1angularAxis);
btVector3* angular1 = (btVector3*)(info->m_J1angularAxis+info->rowskip);
btVector3* angular2 = (btVector3*)(info->m_J1angularAxis+2*info->rowskip);
btVector3 a1neg = -a1;
a1neg.getSkewSymmetricMatrix(angular0,angular1,angular2);
}
if (info->m_J2linearAxis)
{
info->m_J2linearAxis[0] = -1;
info->m_J2linearAxis[info->rowskip+1] = -1;
info->m_J2linearAxis[2*info->rowskip+2] = -1;
}
btVector3 a2 = worldOrnB*m_frameInB.getOrigin();
{
btVector3* angular0 = (btVector3*)(info->m_J2angularAxis);
btVector3* angular1 = (btVector3*)(info->m_J2angularAxis+info->rowskip);
btVector3* angular2 = (btVector3*)(info->m_J2angularAxis+2*info->rowskip);
a2.getSkewSymmetricMatrix(angular0,angular1,angular2);
}
// set right hand side for the linear dofs
btScalar k = info->fps * info->erp;
btVector3 linearError = k*(a2+worldPosB-a1-worldPosA);
int j;
for (j=0; j<3; j++)
{
info->m_constraintError[j*info->rowskip] = linearError[j];
//printf("info->m_constraintError[%d]=%f\n",j,info->m_constraintError[j]);
}
btVector3 ivA = transA.getBasis() * m_frameInA.getBasis().getColumn(0);
btVector3 jvA = transA.getBasis() * m_frameInA.getBasis().getColumn(1);
btVector3 kvA = transA.getBasis() * m_frameInA.getBasis().getColumn(2);
btVector3 ivB = transB.getBasis() * m_frameInB.getBasis().getColumn(0);
btVector3 target;
//btScalar x = ivB.dot(ivA);//??
btScalar y = ivB.dot(jvA);
btScalar z = ivB.dot(kvA);
btVector3 swingAxis(0,0,0);
{
if((!btFuzzyZero(y)) || (!(btFuzzyZero(z))))
{
swingAxis = -ivB.cross(ivA);
}
}
btVector3 vTwist(1,0,0);
// compute rotation of A wrt B (in constraint space)
btQuaternion qA = transA.getRotation() * m_frameInA.getRotation();
btQuaternion qB = transB.getRotation() * m_frameInB.getRotation();
btQuaternion qAB = qB.inverse() * qA;
// split rotation into cone and twist
// (all this is done from B's perspective. Maybe I should be averaging axes...)
btVector3 vConeNoTwist = quatRotate(qAB, vTwist); vConeNoTwist.normalize();
btQuaternion qABCone = shortestArcQuat(vTwist, vConeNoTwist); qABCone.normalize();
btQuaternion qABTwist = qABCone.inverse() * qAB; qABTwist.normalize();
int row = 3;
int srow = row * info->rowskip;
btVector3 ax1;
// angular limits
{
btScalar *J1 = info->m_J1angularAxis;
btScalar *J2 = info->m_J2angularAxis;
btTransform trA = transA*m_frameInA;
btVector3 twistAxis = trA.getBasis().getColumn(0);
btVector3 p = trA.getBasis().getColumn(1);
btVector3 q = trA.getBasis().getColumn(2);
int srow1 = srow + info->rowskip;
J1[srow+0] = p[0];
J1[srow+1] = p[1];
J1[srow+2] = p[2];
J1[srow1+0] = q[0];
J1[srow1+1] = q[1];
J1[srow1+2] = q[2];
J2[srow+0] = -p[0];
J2[srow+1] = -p[1];
J2[srow+2] = -p[2];
J2[srow1+0] = -q[0];
J2[srow1+1] = -q[1];
J2[srow1+2] = -q[2];
btScalar fact = info->fps;
info->m_constraintError[srow] = fact * swingAxis.dot(p);
info->m_constraintError[srow1] = fact * swingAxis.dot(q);
info->m_lowerLimit[srow] = -SIMD_INFINITY;
info->m_upperLimit[srow] = SIMD_INFINITY;
info->m_lowerLimit[srow1] = -SIMD_INFINITY;
info->m_upperLimit[srow1] = SIMD_INFINITY;
srow = srow1 + info->rowskip;
{
btQuaternion qMinTwist = qABTwist;
btScalar twistAngle = qABTwist.getAngle();
if (twistAngle > SIMD_PI) // long way around. flip quat and recalculate.
{
qMinTwist = -(qABTwist);
twistAngle = qMinTwist.getAngle();
}
if (twistAngle > SIMD_EPSILON)
{
twistAxis = btVector3(qMinTwist.x(), qMinTwist.y(), qMinTwist.z());
twistAxis.normalize();
twistAxis = quatRotate(qB, -twistAxis);
}
ax1 = twistAxis;
btScalar *J1 = info->m_J1angularAxis;
btScalar *J2 = info->m_J2angularAxis;
J1[srow+0] = ax1[0];
J1[srow+1] = ax1[1];
J1[srow+2] = ax1[2];
J2[srow+0] = -ax1[0];
J2[srow+1] = -ax1[1];
J2[srow+2] = -ax1[2];
btScalar k = info->fps;
info->m_constraintError[srow] = k * twistAngle;
info->m_lowerLimit[srow] = -SIMD_INFINITY;
info->m_upperLimit[srow] = SIMD_INFINITY;
}
}
}

23
src/BulletDynamics/ConstraintSolver/btFixedConstraint.h
View File

@@ -16,33 +16,18 @@ subject to the following restrictions:
#ifndef BT_FIXED_CONSTRAINT_H #ifndef BT_FIXED_CONSTRAINT_H
#define BT_FIXED_CONSTRAINT_H #define BT_FIXED_CONSTRAINT_H
#include "btTypedConstraint.h" #include "btGeneric6DofSpring2Constraint.h"
ATTRIBUTE_ALIGNED16(class) btFixedConstraint : public btTypedConstraint
ATTRIBUTE_ALIGNED16(class) btFixedConstraint : public btGeneric6DofSpring2Constraint
{ {
btTransform m_frameInA;
btTransform m_frameInB;
public: public:
btFixedConstraint(btRigidBody& rbA,btRigidBody& rbB, const btTransform& frameInA,const btTransform& frameInB); btFixedConstraint(btRigidBody& rbA,btRigidBody& rbB, const btTransform& frameInA,const btTransform& frameInB);
virtual ~btFixedConstraint(); virtual ~btFixedConstraint();
virtual void getInfo1 (btConstraintInfo1* info);
virtual void getInfo2 (btConstraintInfo2* info);
virtual void setParam(int num, btScalar value, int axis = -1)
{
btAssert(0);
}
virtual btScalar getParam(int num, int axis = -1) const
{
btAssert(0);
return 0.f;
}
}; };
#endif //BT_FIXED_CONSTRAINT_H #endif //BT_FIXED_CONSTRAINT_H