Page MenuHome
Differential D1739 Diff 5899 extern/bullet2/src/BulletDynamics/ConstraintSolver/btConeTwistConstraint.cpp

Changeset View

Standalone View

View Options

extern/bullet2/src/BulletDynamics/ConstraintSolver/btConeTwistConstraint.cpp

Show First 20 Lines • Show All 208 Lines • ▼ Show 20 Lines else
info->m_constraintError[srow] = k * m_swingCorrection; info->m_constraintError[srow] = k * m_swingCorrection;
if(m_flags & BT_CONETWIST_FLAGS_ANG_CFM) if(m_flags & BT_CONETWIST_FLAGS_ANG_CFM)
{ {
info->cfm[srow] = m_angCFM; info->cfm[srow] = m_angCFM;
} }
// m_swingCorrection is always positive or 0 // m_swingCorrection is always positive or 0
info->m_lowerLimit[srow] = 0; info->m_lowerLimit[srow] = 0;
info->m_upperLimit[srow] = SIMD_INFINITY; info->m_upperLimit[srow] = (m_bMotorEnabled && m_maxMotorImpulse >= 0.0f) ? m_maxMotorImpulse : SIMD_INFINITY;
srow += info->rowskip; srow += info->rowskip;
} }
} }
if(m_solveTwistLimit) if(m_solveTwistLimit)
{ {
ax1 = m_twistAxis * m_relaxationFactor * m_relaxationFactor; ax1 = m_twistAxis * m_relaxationFactor * m_relaxationFactor;
btScalar *J1 = info->m_J1angularAxis; btScalar *J1 = info->m_J1angularAxis;
btScalar *J2 = info->m_J2angularAxis; btScalar *J2 = info->m_J2angularAxis;
▲ Show 20 Lines • Show All 309 Lines • ▼ Show 20 Lines
#ifndef __SPU__ #ifndef __SPU__
void btConeTwistConstraint::calcAngleInfo() void btConeTwistConstraint::calcAngleInfo()
{ {
m_swingCorrection = btScalar(0.); m_swingCorrection = btScalar(0.);
m_twistLimitSign = btScalar(0.); m_twistLimitSign = btScalar(0.);
m_solveTwistLimit = false; m_solveTwistLimit = false;
m_solveSwingLimit = false; m_solveSwingLimit = false;
btVector3 b1Axis1,b1Axis2,b1Axis3; btVector3 b1Axis1(0,0,0),b1Axis2(0,0,0),b1Axis3(0,0,0);
btVector3 b2Axis1,b2Axis2; btVector3 b2Axis1(0,0,0),b2Axis2(0,0,0);
b1Axis1 = getRigidBodyA().getCenterOfMassTransform().getBasis() * this->m_rbAFrame.getBasis().getColumn(0); b1Axis1 = getRigidBodyA().getCenterOfMassTransform().getBasis() * this->m_rbAFrame.getBasis().getColumn(0);
b2Axis1 = getRigidBodyB().getCenterOfMassTransform().getBasis() * this->m_rbBFrame.getBasis().getColumn(0); b2Axis1 = getRigidBodyB().getCenterOfMassTransform().getBasis() * this->m_rbBFrame.getBasis().getColumn(0);
btScalar swing1=btScalar(0.),swing2 = btScalar(0.); btScalar swing1=btScalar(0.),swing2 = btScalar(0.);
btScalar swx=btScalar(0.),swy = btScalar(0.); btScalar swx=btScalar(0.),swy = btScalar(0.);
btScalar thresh = btScalar(10.); btScalar thresh = btScalar(10.);
▲ Show 20 Lines • Show All 220 Lines • ▼ Show 20 Lines// if(!btFuzzyZero(z))
} }
} }
} }
target[0] = x * ivA[0] + y * jvA[0] + z * kvA[0]; target[0] = x * ivA[0] + y * jvA[0] + z * kvA[0];
target[1] = x * ivA[1] + y * jvA[1] + z * kvA[1]; target[1] = x * ivA[1] + y * jvA[1] + z * kvA[1];
target[2] = x * ivA[2] + y * jvA[2] + z * kvA[2]; target[2] = x * ivA[2] + y * jvA[2] + z * kvA[2];
target.normalize(); target.normalize();
m_swingAxis = -ivB.cross(target); m_swingAxis = -ivB.cross(target);
m_swingCorrection = m_swingAxis.length(); m_swingCorrection = m_swingAxis.length();
if (!btFuzzyZero(m_swingCorrection))
m_swingAxis.normalize(); m_swingAxis.normalize();
} }
} }
if (m_twistSpan >= btScalar(0.f)) if (m_twistSpan >= btScalar(0.f))
{ {
btVector3 twistAxis; btVector3 twistAxis;
computeTwistLimitInfo(qABTwist, m_twistAngle, twistAxis); computeTwistLimitInfo(qABTwist, m_twistAngle, twistAxis);
▲ Show 20 Lines • Show All 187 Lines • ▼ Show 20 Lines if (fabs(z) > SIMD_EPSILON) // avoid division by 0. and we don't need an update if z == 0.
vSwingAxis.normalize(); vSwingAxis.normalize();
} }
} }
void btConeTwistConstraint::setMotorTarget(const btQuaternion &q) void btConeTwistConstraint::setMotorTarget(const btQuaternion &q)
{ {
btTransform trACur = m_rbA.getCenterOfMassTransform(); //btTransform trACur = m_rbA.getCenterOfMassTransform();
btTransform trBCur = m_rbB.getCenterOfMassTransform(); //btTransform trBCur = m_rbB.getCenterOfMassTransform();
// btTransform trABCur = trBCur.inverse() * trACur; // btTransform trABCur = trBCur.inverse() * trACur;
// btQuaternion qABCur = trABCur.getRotation(); // btQuaternion qABCur = trABCur.getRotation();
// btTransform trConstraintCur = (trBCur * m_rbBFrame).inverse() * (trACur * m_rbAFrame); // btTransform trConstraintCur = (trBCur * m_rbBFrame).inverse() * (trACur * m_rbAFrame);
//btQuaternion qConstraintCur = trConstraintCur.getRotation(); //btQuaternion qConstraintCur = trConstraintCur.getRotation();
btQuaternion qConstraint = m_rbBFrame.getRotation().inverse() * q * m_rbAFrame.getRotation(); btQuaternion qConstraint = m_rbBFrame.getRotation().inverse() * q * m_rbAFrame.getRotation();
setMotorTargetInConstraintSpace(qConstraint); setMotorTargetInConstraintSpace(qConstraint);
} }
▲ Show 20 Lines • Show All 146 Lines • Show Last 20 Lines