diff --git a/src/BulletDynamics/ConstraintSolver/btConeTwistConstraint.cpp b/src/BulletDynamics/ConstraintSolver/btConeTwistConstraint.cpp index a3d0dcfac..8c73c157f 100644 --- a/src/BulletDynamics/ConstraintSolver/btConeTwistConstraint.cpp +++ b/src/BulletDynamics/ConstraintSolver/btConeTwistConstraint.cpp @@ -610,7 +610,7 @@ void btConeTwistConstraint::calcAngleInfo2() // or you're trying to set at least one of the swing limits too small. (if so, do you really want a conetwist constraint?) } - if (m_twistSpan >= btScalar(0.05f)) + if (m_twistSpan >= btScalar(0.f)) { btVector3 twistAxis; computeTwistLimitInfo(qABTwist, m_twistAngle, twistAxis); diff --git a/src/BulletDynamics/ConstraintSolver/btGeneric6DofConstraint.cpp b/src/BulletDynamics/ConstraintSolver/btGeneric6DofConstraint.cpp index 042bfe5e1..5860d7bb4 100644 --- a/src/BulletDynamics/ConstraintSolver/btGeneric6DofConstraint.cpp +++ b/src/BulletDynamics/ConstraintSolver/btGeneric6DofConstraint.cpp @@ -19,19 +19,19 @@ email: projectileman@yahoo.com http://gimpact.sf.net */ - #include "btGeneric6DofConstraint.h" #include "BulletDynamics/Dynamics/btRigidBody.h" #include "LinearMath/btTransformUtil.h" #include + +#define D6_USE_OBSOLETE_METHOD false //----------------------------------------------------------------------------- btGeneric6DofConstraint::btGeneric6DofConstraint() :btTypedConstraint(D6_CONSTRAINT_TYPE), m_useLinearReferenceFrameA(true), -m_useSolveConstraintObsolete(false) -//m_useSolveConstraintObsolete(true) +m_useSolveConstraintObsolete(D6_USE_OBSOLETE_METHOD) { } @@ -42,8 +42,7 @@ btGeneric6DofConstraint::btGeneric6DofConstraint(btRigidBody& rbA, btRigidBody& , m_frameInA(frameInA) , m_frameInB(frameInB), m_useLinearReferenceFrameA(useLinearReferenceFrameA), -m_useSolveConstraintObsolete(false) -//m_useSolveConstraintObsolete(true) +m_useSolveConstraintObsolete(D6_USE_OBSOLETE_METHOD) { } @@ -537,7 +536,6 @@ int btGeneric6DofConstraint::setLinearLimits(btConstraintInfo2* info) int row = 0; //solve linear limits btRotationalLimitMotor limot; - bool useBasisBodyB = (m_rbB.getInvMass() == btScalar(0.f)); for (int i=0;i<3 ;i++ ) { if(m_linearLimits.needApplyForce(i)) @@ -554,16 +552,8 @@ int btGeneric6DofConstraint::setLinearLimits(btConstraintInfo2* info) limot.m_maxLimitForce = btScalar(0.f); limot.m_maxMotorForce = m_linearLimits.m_maxMotorForce[i]; limot.m_targetVelocity = m_linearLimits.m_targetVelocity[i]; - btVector3 axis; - if(useBasisBodyB) - { - axis = m_calculatedTransformB.getBasis().getColumn(i); - } - else - { - axis = m_calculatedTransformA.getBasis().getColumn(i); - } - row += get_limit_motor_info2(&limot, &m_rbA, &m_rbB, info, row, axis, 0); + btVector3 axis = m_calculatedTransformA.getBasis().getColumn(i); + row += get_limit_motor_info2(&limot, &m_rbA, &m_rbB, info, row, axis, 0); } } return row; diff --git a/src/BulletDynamics/ConstraintSolver/btHingeConstraint.cpp b/src/BulletDynamics/ConstraintSolver/btHingeConstraint.cpp index c2d56e4d9..82eb23241 100644 --- a/src/BulletDynamics/ConstraintSolver/btHingeConstraint.cpp +++ b/src/BulletDynamics/ConstraintSolver/btHingeConstraint.cpp @@ -71,9 +71,9 @@ btHingeConstraint::btHingeConstraint(btRigidBody& rbA,btRigidBody& rbB, const bt btVector3 rbAxisB2 = axisInB.cross(rbAxisB1); m_rbBFrame.getOrigin() = pivotInB; - m_rbBFrame.getBasis().setValue( rbAxisB1.getX(),rbAxisB2.getX(),-axisInB.getX(), - rbAxisB1.getY(),rbAxisB2.getY(),-axisInB.getY(), - rbAxisB1.getZ(),rbAxisB2.getZ(),-axisInB.getZ() ); + m_rbBFrame.getBasis().setValue( rbAxisB1.getX(),rbAxisB2.getX(),axisInB.getX(), + rbAxisB1.getY(),rbAxisB2.getY(),axisInB.getY(), + rbAxisB1.getZ(),rbAxisB2.getZ(),axisInB.getZ() ); //start with free m_lowerLimit = btScalar(1e30); @@ -102,7 +102,7 @@ m_useSolveConstraintObsolete(HINGE_USE_OBSOLETE_SOLVER) rbAxisA1.getY(),rbAxisA2.getY(),axisInA.getY(), rbAxisA1.getZ(),rbAxisA2.getZ(),axisInA.getZ() ); - btVector3 axisInB = rbA.getCenterOfMassTransform().getBasis() * -axisInA; + btVector3 axisInB = rbA.getCenterOfMassTransform().getBasis() * axisInA; btQuaternion rotationArc = shortestArcQuat(axisInA,axisInB); btVector3 rbAxisB1 = quatRotate(rotationArc,rbAxisA1); @@ -132,11 +132,6 @@ m_angularOnly(false), m_enableAngularMotor(false), m_useSolveConstraintObsolete(HINGE_USE_OBSOLETE_SOLVER) { - // flip axis - m_rbBFrame.getBasis()[0][2] *= btScalar(-1.); - m_rbBFrame.getBasis()[1][2] *= btScalar(-1.); - m_rbBFrame.getBasis()[2][2] *= btScalar(-1.); - //start with free m_lowerLimit = btScalar(1e30); m_upperLimit = btScalar(-1e30); @@ -156,11 +151,6 @@ m_useSolveConstraintObsolete(HINGE_USE_OBSOLETE_SOLVER) { ///not providing rigidbody B means implicitly using worldspace for body B - // flip axis - m_rbBFrame.getBasis()[0][2] *= btScalar(-1.); - m_rbBFrame.getBasis()[1][2] *= btScalar(-1.); - m_rbBFrame.getBasis()[2][2] *= btScalar(-1.); - m_rbBFrame.getOrigin() = m_rbA.getCenterOfMassTransform()(m_rbAFrame.getOrigin()); //start with free @@ -364,7 +354,7 @@ void btHingeConstraint::getInfo2 (btConstraintInfo2* info) // angular_velocity = (erp*fps) * (ax1 x ax2) // ax1 x ax2 is in the plane space of ax1, so we project the angular // velocity to p and q to find the right hand side. - btVector3 ax2 = - trB.getBasis().getColumn(2); + btVector3 ax2 = trB.getBasis().getColumn(2); btVector3 u = ax1.cross(ax2); info->m_constraintError[s3] = k * u.dot(p); info->m_constraintError[s4] = k * u.dot(q); @@ -551,7 +541,7 @@ void btHingeConstraint::solveConstraintObsolete(btSolverBody& bodyA,btSolverBody } //solve angular positional correction - btVector3 angularError = -axisA.cross(axisB) *(btScalar(1.)/timeStep); + btVector3 angularError = axisA.cross(axisB) *(btScalar(1.)/timeStep); btScalar len2 = angularError.length(); if (len2>btScalar(0.00001)) { diff --git a/src/BulletDynamics/Dynamics/btDiscreteDynamicsWorld.cpp b/src/BulletDynamics/Dynamics/btDiscreteDynamicsWorld.cpp index 7d0ca8668..4e10fa5cf 100644 --- a/src/BulletDynamics/Dynamics/btDiscreteDynamicsWorld.cpp +++ b/src/BulletDynamics/Dynamics/btDiscreteDynamicsWorld.cpp @@ -1294,12 +1294,18 @@ void btDiscreteDynamicsWorld::debugDrawConstraint(btTypedConstraint* constraint) pPrev = pCur; } - - - btVector3 pivot = pCT->getRigidBodyB().getCenterOfMassTransform() * pCT->getBFrame().getOrigin(); btScalar tws = pCT->getTwistSpan(); btScalar twa = pCT->getTwistAngle(); - tr = pCT->getRigidBodyB().getCenterOfMassTransform() * pCT->getBFrame(); + bool useFrameB = (pCT->getRigidBodyB().getInvMass() > btScalar(0.f)); + if(useFrameB) + { + tr = pCT->getRigidBodyB().getCenterOfMassTransform() * pCT->getBFrame(); + } + else + { + tr = pCT->getRigidBodyA().getCenterOfMassTransform() * pCT->getAFrame(); + } + btVector3 pivot = tr.getOrigin(); btVector3 normal = tr.getBasis().getColumn(0); btVector3 axis1 = tr.getBasis().getColumn(1); getDebugDrawer()->drawArc(pivot, normal, axis1, dbgDrawSize, dbgDrawSize, -twa-tws, -twa+tws, btVector3(0,0,0), true); @@ -1324,7 +1330,7 @@ void btDiscreteDynamicsWorld::debugDrawConstraint(btTypedConstraint* constraint) btScalar maxTh = p6DOF->getRotationalLimitMotor(1)->m_hiLimit; btScalar minPs = p6DOF->getRotationalLimitMotor(2)->m_loLimit; btScalar maxPs = p6DOF->getRotationalLimitMotor(2)->m_hiLimit; - getDebugDrawer()->drawSpherePatch(center, up, axis, dbgDrawSize, minTh, maxTh, minPs, maxPs, btVector3(0,0,0)); + getDebugDrawer()->drawSpherePatch(center, up, axis, dbgDrawSize * btScalar(.9f), minTh, maxTh, minPs, maxPs, btVector3(0,0,0)); axis = tr.getBasis().getColumn(1); btScalar ay = p6DOF->getAngle(1); btScalar az = p6DOF->getAngle(2); @@ -1340,11 +1346,18 @@ void btDiscreteDynamicsWorld::debugDrawConstraint(btTypedConstraint* constraint) btVector3 normal = -tr.getBasis().getColumn(0); btScalar minFi = p6DOF->getRotationalLimitMotor(0)->m_loLimit; btScalar maxFi = p6DOF->getRotationalLimitMotor(0)->m_hiLimit; - getDebugDrawer()->drawArc(center, normal, ref, dbgDrawSize, dbgDrawSize, minFi, maxFi, btVector3(0,0,0), true); + if(minFi > maxFi) + { + getDebugDrawer()->drawArc(center, normal, ref, dbgDrawSize, dbgDrawSize, -SIMD_PI, SIMD_PI, btVector3(0,0,0), false); + } + else if(minFi < maxFi) + { + getDebugDrawer()->drawArc(center, normal, ref, dbgDrawSize, dbgDrawSize, minFi, maxFi, btVector3(0,0,0), true); + } tr = p6DOF->getCalculatedTransformA(); - btVector3 bbMin = tr * p6DOF->getTranslationalLimitMotor()->m_lowerLimit; - btVector3 bbMax = tr * p6DOF->getTranslationalLimitMotor()->m_upperLimit; - getDebugDrawer()->drawBox(bbMin, bbMax, btVector3(0,0,0)); + btVector3 bbMin = p6DOF->getTranslationalLimitMotor()->m_lowerLimit; + btVector3 bbMax = p6DOF->getTranslationalLimitMotor()->m_upperLimit; + getDebugDrawer()->drawBox(bbMin, bbMax, tr, btVector3(0,0,0)); } } break;