diff --git a/Extras/Serialize/BulletWorldImporter/btWorldImporter.cpp b/Extras/Serialize/BulletWorldImporter/btWorldImporter.cpp index de0b22c8d..a481dbf84 100644 --- a/Extras/Serialize/BulletWorldImporter/btWorldImporter.cpp +++ b/Extras/Serialize/BulletWorldImporter/btWorldImporter.cpp @@ -1011,6 +1011,37 @@ void btWorldImporter::convertConstraintFloat(btTypedConstraintFloatData* constra 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: { printf("unknown constraint type\n"); @@ -1310,6 +1341,38 @@ void btWorldImporter::convertConstraintDouble(btTypedConstraintDoubleData* const 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: { printf("unknown constraint type\n"); diff --git a/examples/OpenGLWindow/MacOpenGLWindow.mm b/examples/OpenGLWindow/MacOpenGLWindow.mm index 544cf2a37..66c672ab1 100644 --- a/examples/OpenGLWindow/MacOpenGLWindow.mm +++ b/examples/OpenGLWindow/MacOpenGLWindow.mm @@ -1054,7 +1054,7 @@ int MacOpenGLWindow::fileOpenDialog(char* filename, int maxNameLength) NSOpenGLContext *foo = [NSOpenGLContext currentContext]; // get the url of a .txt file NSOpenPanel * zOpenPanel = [NSOpenPanel openPanel]; - NSArray * zAryOfExtensions = [NSArray arrayWithObject:@"urdf"]; + NSArray * zAryOfExtensions = [NSArray arrayWithObjects:@"urdf",@"bullet",nil]; [zOpenPanel setAllowedFileTypes:zAryOfExtensions]; NSInteger zIntResult = [zOpenPanel runModal]; diff --git a/src/BulletDynamics/ConstraintSolver/btFixedConstraint.cpp b/src/BulletDynamics/ConstraintSolver/btFixedConstraint.cpp index 894059ad1..75d81cc08 100644 --- a/src/BulletDynamics/ConstraintSolver/btFixedConstraint.cpp +++ b/src/BulletDynamics/ConstraintSolver/btFixedConstraint.cpp @@ -21,166 +21,17 @@ subject to the following restrictions: 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; - m_frameInB = frameInB; - + setAngularLowerLimit(btVector3(0,0,0)); + setAngularUpperLimit(btVector3(0,0,0)); + setLinearLowerLimit(btVector3(0,0,0)); + setLinearUpperLimit(btVector3(0,0,0)); } + + + 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; - } - } -} \ No newline at end of file diff --git a/src/BulletDynamics/ConstraintSolver/btFixedConstraint.h b/src/BulletDynamics/ConstraintSolver/btFixedConstraint.h index e1c9430fd..bff2008b2 100644 --- a/src/BulletDynamics/ConstraintSolver/btFixedConstraint.h +++ b/src/BulletDynamics/ConstraintSolver/btFixedConstraint.h @@ -16,33 +16,18 @@ subject to the following restrictions: #ifndef 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: btFixedConstraint(btRigidBody& rbA,btRigidBody& rbB, const btTransform& frameInA,const btTransform& frameInB); + 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