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

Improved and more stable btConeTwistConstraint (thanks to Edy Boxerman)

Browse Source
This commit is contained in:
rponom
2009-02-04 02:11:45 +00:00
parent daf350168d
commit 2f23237185
5 changed files with 587 additions and 185 deletions
Download Patch File Download Diff File Expand all files Collapse all files

603
src/BulletDynamics/ConstraintSolver/btConeTwistConstraint.cpp
View File

@@ -22,10 +22,15 @@ Written by: Marcus Hennix
#include "LinearMath/btMinMax.h"
#include <new>
//-----------------------------------------------------------------------------
#define CONETWIST_USE_OBSOLETE_SOLVER false
//-----------------------------------------------------------------------------
btConeTwistConstraint::btConeTwistConstraint()
:btTypedConstraint(CONETWIST_CONSTRAINT_TYPE),
m_useSolveConstraintObsolete(false)
//m_useSolveConstraintObsolete(true)
m_useSolveConstraintObsolete(CONETWIST_USE_OBSOLETE_SOLVER)
{
}
@@ -34,39 +39,34 @@ btConeTwistConstraint::btConeTwistConstraint(btRigidBody& rbA,btRigidBody& rbB,
const btTransform& rbAFrame,const btTransform& rbBFrame)
:btTypedConstraint(CONETWIST_CONSTRAINT_TYPE, rbA,rbB),m_rbAFrame(rbAFrame),m_rbBFrame(rbBFrame),
m_angularOnly(false),
m_useSolveConstraintObsolete(false)
// m_useSolveConstraintObsolete(true)
m_useSolveConstraintObsolete(CONETWIST_USE_OBSOLETE_SOLVER)
{
m_swingSpan1 = btScalar(1e30);
m_swingSpan2 = btScalar(1e30);
m_twistSpan = btScalar(1e30);
m_biasFactor = 0.3f;
m_relaxationFactor = 1.0f;
m_solveTwistLimit = false;
m_solveSwingLimit = false;
init();
}
btConeTwistConstraint::btConeTwistConstraint(btRigidBody& rbA,const btTransform& rbAFrame)
:btTypedConstraint(CONETWIST_CONSTRAINT_TYPE,rbA),m_rbAFrame(rbAFrame),
m_angularOnly(false),
m_useSolveConstraintObsolete(false)
// m_useSolveConstraintObsolete(true)
m_useSolveConstraintObsolete(CONETWIST_USE_OBSOLETE_SOLVER)
{
m_rbBFrame = m_rbAFrame;
init();
}
m_swingSpan1 = btScalar(1e30);
m_swingSpan2 = btScalar(1e30);
m_twistSpan = btScalar(1e30);
m_biasFactor = 0.3f;
m_relaxationFactor = 1.0f;
void btConeTwistConstraint::init()
{
m_angularOnly = false;
m_solveTwistLimit = false;
m_solveSwingLimit = false;
m_bMotorEnabled = false;
m_maxMotorImpulse = btScalar(-1);
setLimit(btScalar(1e30), btScalar(1e30), btScalar(1e30));
m_damping = btScalar(0.01);
}
//-----------------------------------------------------------------------------
void btConeTwistConstraint::getInfo1 (btConstraintInfo1* info)
@@ -80,7 +80,8 @@ void btConeTwistConstraint::getInfo1 (btConstraintInfo1* info)
{
info->m_numConstraintRows = 3;
info->nub = 3;
calcAngleInfo();
//calcAngleInfo();
calcAngleInfo2();
if(m_solveSwingLimit)
{
info->m_numConstraintRows++;
@@ -199,12 +200,6 @@ void btConeTwistConstraint::buildJacobian()
if (m_useSolveConstraintObsolete)
{
m_appliedImpulse = btScalar(0.);
//set bias, sign, clear accumulator
m_swingCorrection = btScalar(0.);
m_twistLimitSign = btScalar(0.);
m_solveTwistLimit = false;
m_solveSwingLimit = false;
m_accTwistLimitImpulse = btScalar(0.);
m_accSwingLimitImpulse = btScalar(0.);
@@ -241,101 +236,7 @@ void btConeTwistConstraint::buildJacobian()
}
}
btVector3 b1Axis1,b1Axis2,b1Axis3;
btVector3 b2Axis1,b2Axis2;
b1Axis1 = getRigidBodyA().getCenterOfMassTransform().getBasis() * this->m_rbAFrame.getBasis().getColumn(0);
b2Axis1 = getRigidBodyB().getCenterOfMassTransform().getBasis() * this->m_rbBFrame.getBasis().getColumn(0);
btScalar swing1=btScalar(0.),swing2 = btScalar(0.);
btScalar swx=btScalar(0.),swy = btScalar(0.);
btScalar thresh = btScalar(10.);
btScalar fact;
// Get Frame into world space
if (m_swingSpan1 >= btScalar(0.05f))
{
b1Axis2 = getRigidBodyA().getCenterOfMassTransform().getBasis() * this->m_rbAFrame.getBasis().getColumn(1);
// swing1 = btAtan2Fast( b2Axis1.dot(b1Axis2),b2Axis1.dot(b1Axis1) );
swx = b2Axis1.dot(b1Axis1);
swy = b2Axis1.dot(b1Axis2);
swing1 = btAtan2Fast(swy, swx);
fact = (swy*swy + swx*swx) * thresh * thresh;
fact = fact / (fact + btScalar(1.0));
swing1 *= fact;
}
if (m_swingSpan2 >= btScalar(0.05f))
{
b1Axis3 = getRigidBodyA().getCenterOfMassTransform().getBasis() * this->m_rbAFrame.getBasis().getColumn(2);
// swing2 = btAtan2Fast( b2Axis1.dot(b1Axis3),b2Axis1.dot(b1Axis1) );
swx = b2Axis1.dot(b1Axis1);
swy = b2Axis1.dot(b1Axis3);
swing2 = btAtan2Fast(swy, swx);
fact = (swy*swy + swx*swx) * thresh * thresh;
fact = fact / (fact + btScalar(1.0));
swing2 *= fact;
}
btScalar RMaxAngle1Sq = 1.0f / (m_swingSpan1*m_swingSpan1);
btScalar RMaxAngle2Sq = 1.0f / (m_swingSpan2*m_swingSpan2);
btScalar EllipseAngle = btFabs(swing1*swing1)* RMaxAngle1Sq + btFabs(swing2*swing2) * RMaxAngle2Sq;
if (EllipseAngle > 1.0f)
{
m_swingCorrection = EllipseAngle-1.0f;
m_solveSwingLimit = true;
// Calculate necessary axis & factors
m_swingAxis = b2Axis1.cross(b1Axis2* b2Axis1.dot(b1Axis2) + b1Axis3* b2Axis1.dot(b1Axis3));
m_swingAxis.normalize();
btScalar swingAxisSign = (b2Axis1.dot(b1Axis1) >= 0.0f) ? 1.0f : -1.0f;
m_swingAxis *= swingAxisSign;
m_kSwing = btScalar(1.) / (getRigidBodyA().computeAngularImpulseDenominator(m_swingAxis) +
getRigidBodyB().computeAngularImpulseDenominator(m_swingAxis));
}
// Twist limits
if (m_twistSpan >= btScalar(0.))
{
btVector3 b2Axis2 = getRigidBodyB().getCenterOfMassTransform().getBasis() * this->m_rbBFrame.getBasis().getColumn(1);
btQuaternion rotationArc = shortestArcQuat(b2Axis1,b1Axis1);
btVector3 TwistRef = quatRotate(rotationArc,b2Axis2);
btScalar twist = btAtan2Fast( TwistRef.dot(b1Axis3), TwistRef.dot(b1Axis2) );
m_twistAngle = twist;
btScalar lockedFreeFactor = (m_twistSpan > btScalar(0.05f)) ? m_limitSoftness : btScalar(0.);
if (twist <= -m_twistSpan*lockedFreeFactor)
{
m_twistCorrection = -(twist + m_twistSpan);
m_solveTwistLimit = true;
m_twistAxis = (b2Axis1 + b1Axis1) * 0.5f;
m_twistAxis.normalize();
m_twistAxis *= -1.0f;
m_kTwist = btScalar(1.) / (getRigidBodyA().computeAngularImpulseDenominator(m_twistAxis) +
getRigidBodyB().computeAngularImpulseDenominator(m_twistAxis));
} else
if (twist > m_twistSpan*lockedFreeFactor)
{
m_twistCorrection = (twist - m_twistSpan);
m_solveTwistLimit = true;
m_twistAxis = (b2Axis1 + b1Axis1) * 0.5f;
m_twistAxis.normalize();
m_kTwist = btScalar(1.) / (getRigidBodyA().computeAngularImpulseDenominator(m_twistAxis) +
getRigidBodyB().computeAngularImpulseDenominator(m_twistAxis));
}
}
calcAngleInfo2();
}
}
@@ -382,6 +283,109 @@ void btConeTwistConstraint::solveConstraintObsolete(btSolverBody& bodyA,btSolver
}
}
// apply motor
if (m_bMotorEnabled)
{
// compute current and predicted transforms
btTransform trACur = m_rbA.getCenterOfMassTransform();
btTransform trBCur = m_rbB.getCenterOfMassTransform();
btVector3 omegaA; bodyA.getAngularVelocity(omegaA);
btVector3 omegaB; bodyB.getAngularVelocity(omegaB);
btTransform trAPred; trAPred.setIdentity(); btTransformUtil::integrateTransform(
trACur, btVector3(0,0,0), omegaA, timeStep, trAPred);
btTransform trBPred; trBPred.setIdentity(); btTransformUtil::integrateTransform(
trBCur, btVector3(0,0,0), omegaB, timeStep, trBPred);
// compute desired transforms in world
btTransform trPose(m_qTarget);
btTransform trABDes = m_rbBFrame * trPose * m_rbAFrame.inverse();
btTransform trADes = trBPred * trABDes;
btTransform trBDes = trAPred * trABDes.inverse();
// compute desired omegas in world
btVector3 omegaADes, omegaBDes;
btTransformUtil::calculateVelocity(trACur, trADes, timeStep, btVector3(0,0,0), omegaADes);
btTransformUtil::calculateVelocity(trBCur, trBDes, timeStep, btVector3(0,0,0), omegaBDes);
// compute delta omegas
btVector3 dOmegaA = omegaADes - omegaA;
btVector3 dOmegaB = omegaBDes - omegaB;
// compute weighted avg axis of dOmega (weighting based on inertias)
btVector3 axisA, axisB;
btScalar kAxisAInv = 0, kAxisBInv = 0;
if (dOmegaA.length2() > SIMD_EPSILON)
{
axisA = dOmegaA.normalized();
kAxisAInv = getRigidBodyA().computeAngularImpulseDenominator(axisA);
}
if (dOmegaB.length2() > SIMD_EPSILON)
{
axisB = dOmegaB.normalized();
kAxisBInv = getRigidBodyB().computeAngularImpulseDenominator(axisB);
}
btVector3 avgAxis = kAxisAInv * axisA + kAxisBInv * axisB;
static bool bDoTorque = true;
if (bDoTorque && avgAxis.length2() > SIMD_EPSILON)
{
avgAxis.normalize();
kAxisAInv = getRigidBodyA().computeAngularImpulseDenominator(avgAxis);
kAxisBInv = getRigidBodyB().computeAngularImpulseDenominator(avgAxis);
btScalar kInvCombined = kAxisAInv + kAxisBInv;
btVector3 impulse = (kAxisAInv * dOmegaA - kAxisBInv * dOmegaB) /
(kInvCombined * kInvCombined);
if (m_maxMotorImpulse >= 0)
{
btScalar fMaxImpulse = m_maxMotorImpulse;
if (m_bNormalizedMotorStrength)
fMaxImpulse = fMaxImpulse/kAxisAInv;
btVector3 newUnclampedAccImpulse = m_accMotorImpulse + impulse;
btScalar newUnclampedMag = newUnclampedAccImpulse.length();
if (newUnclampedMag > fMaxImpulse)
{
newUnclampedAccImpulse.normalize();
newUnclampedAccImpulse *= fMaxImpulse;
impulse = newUnclampedAccImpulse - m_accMotorImpulse;
}
m_accMotorImpulse += impulse;
}
btScalar impulseMag = impulse.length();
btVector3 impulseAxis = impulse / impulseMag;
bodyA.applyImpulse(btVector3(0,0,0), m_rbA.getInvInertiaTensorWorld()*impulseAxis, impulseMag);
bodyB.applyImpulse(btVector3(0,0,0), m_rbB.getInvInertiaTensorWorld()*impulseAxis, -impulseMag);
}
}
else // no motor: do a little damping
{
const btVector3& angVelA = getRigidBodyA().getAngularVelocity();
const btVector3& angVelB = getRigidBodyB().getAngularVelocity();
btVector3 relVel = angVelB - angVelA;
if (relVel.length2() > SIMD_EPSILON)
{
btVector3 relVelAxis = relVel.normalized();
btScalar m_kDamping = btScalar(1.) /
(getRigidBodyA().computeAngularImpulseDenominator(relVelAxis) +
getRigidBodyB().computeAngularImpulseDenominator(relVelAxis));
btVector3 impulse = m_damping * m_kDamping * relVel;
btScalar impulseMag = impulse.length();
btVector3 impulseAxis = impulse / impulseMag;
bodyA.applyImpulse(btVector3(0,0,0), m_rbA.getInvInertiaTensorWorld()*impulseAxis, impulseMag);
bodyB.applyImpulse(btVector3(0,0,0), m_rbB.getInvInertiaTensorWorld()*impulseAxis, -impulseMag);
}
}
// joint limits
{
///solve angular part
btVector3 angVelA;
@@ -392,7 +396,10 @@ void btConeTwistConstraint::solveConstraintObsolete(btSolverBody& bodyA,btSolver
// solve swing limit
if (m_solveSwingLimit)
{
btScalar amplitude = ((angVelB - angVelA).dot( m_swingAxis )*m_relaxationFactor*m_relaxationFactor + m_swingCorrection*(btScalar(1.)/timeStep)*m_biasFactor);
btScalar amplitude = m_swingLimitRatio * m_swingCorrection*m_biasFactor/timeStep;
btScalar relSwingVel = (angVelB - angVelA).dot(m_swingAxis);
if (relSwingVel > 0)
amplitude += m_swingLimitRatio * relSwingVel * m_relaxationFactor;
btScalar impulseMag = amplitude * m_kSwing;
// Clamp the accumulated impulse
@@ -402,14 +409,29 @@ void btConeTwistConstraint::solveConstraintObsolete(btSolverBody& bodyA,btSolver
btVector3 impulse = m_swingAxis * impulseMag;
bodyA.applyImpulse(btVector3(0,0,0), m_rbA.getInvInertiaTensorWorld()*m_swingAxis,impulseMag);
bodyB.applyImpulse(btVector3(0,0,0), m_rbB.getInvInertiaTensorWorld()*m_swingAxis,-impulseMag);
// don't let cone response affect twist
// (this can happen since body A's twist doesn't match body B's AND we use an elliptical cone limit)
{
btVector3 impulseTwistCouple = impulse.dot(m_twistAxisA) * m_twistAxisA;
btVector3 impulseNoTwistCouple = impulse - impulseTwistCouple;
impulse = impulseNoTwistCouple;
}
impulseMag = impulse.length();
btVector3 noTwistSwingAxis = impulse / impulseMag;
bodyA.applyImpulse(btVector3(0,0,0), m_rbA.getInvInertiaTensorWorld()*noTwistSwingAxis, impulseMag);
bodyB.applyImpulse(btVector3(0,0,0), m_rbB.getInvInertiaTensorWorld()*noTwistSwingAxis, -impulseMag);
}
// solve twist limit
if (m_solveTwistLimit)
{
btScalar amplitude = ((angVelB - angVelA).dot( m_twistAxis )*m_relaxationFactor*m_relaxationFactor + m_twistCorrection*(btScalar(1.)/timeStep)*m_biasFactor );
btScalar amplitude = m_twistLimitRatio * m_twistCorrection*m_biasFactor/timeStep;
btScalar relTwistVel = (angVelB - angVelA).dot( m_twistAxis );
if (relTwistVel > 0) // only damp when moving towards limit (m_twistAxis flipping is important)
amplitude += m_twistLimitRatio * relTwistVel * m_relaxationFactor;
btScalar impulseMag = amplitude * m_kTwist;
// Clamp the accumulated impulse
@@ -421,9 +443,7 @@ void btConeTwistConstraint::solveConstraintObsolete(btSolverBody& bodyA,btSolver
bodyA.applyImpulse(btVector3(0,0,0), m_rbA.getInvInertiaTensorWorld()*m_twistAxis,impulseMag);
bodyB.applyImpulse(btVector3(0,0,0), m_rbB.getInvInertiaTensorWorld()*m_twistAxis,-impulseMag);
}
}
}
@@ -525,6 +545,333 @@ void btConeTwistConstraint::calcAngleInfo()
}
} // btConeTwistConstraint::calcAngleInfo()
static btVector3 vTwist(1,0,0); // twist axis in constraint's space
//-----------------------------------------------------------------------------
void btConeTwistConstraint::calcAngleInfo2()
{
m_swingCorrection = btScalar(0.);
m_twistLimitSign = btScalar(0.);
m_solveTwistLimit = false;
m_solveSwingLimit = false;
{
// compute rotation of A wrt B (in constraint space)
btQuaternion qA = getRigidBodyA().getCenterOfMassTransform().getRotation() * m_rbAFrame.getRotation();
btQuaternion qB = getRigidBodyB().getCenterOfMassTransform().getRotation() * m_rbBFrame.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();
if (m_swingSpan1 >= btScalar(0.05f) && m_swingSpan2 >= btScalar(0.05f))
{
btScalar swingAngle, swingLimit = 0; btVector3 swingAxis;
computeConeLimitInfo(qABCone, swingAngle, swingAxis, swingLimit);
if (swingAngle > swingLimit * m_limitSoftness)
{
m_solveSwingLimit = true;
// compute limit ratio: 0->1, where
// 0 == beginning of soft limit
// 1 == hard/real limit
m_swingLimitRatio = 1.f;
if (swingAngle < swingLimit && m_limitSoftness < 1.f - SIMD_EPSILON)
{
m_swingLimitRatio = (swingAngle - swingLimit * m_limitSoftness)/
(swingLimit - swingLimit * m_limitSoftness);
}
// swing correction tries to get back to soft limit
m_swingCorrection = swingAngle - (swingLimit * m_limitSoftness);
// adjustment of swing axis (based on ellipse normal)
adjustSwingAxisToUseEllipseNormal(swingAxis);
// Calculate necessary axis & factors
m_swingAxis = quatRotate(qB, -swingAxis);
m_twistAxisA.setValue(0,0,0);
m_kSwing = btScalar(1.) /
(getRigidBodyA().computeAngularImpulseDenominator(m_swingAxis) +
getRigidBodyB().computeAngularImpulseDenominator(m_swingAxis));
}
}
else
{
// you haven't set any limits;
// 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))
{
btVector3 twistAxis;
computeTwistLimitInfo(qABTwist, m_twistAngle, twistAxis);
if (m_twistAngle > m_twistSpan*m_limitSoftness)
{
m_solveTwistLimit = true;
m_twistLimitRatio = 1.f;
if (m_twistAngle < m_twistSpan && m_limitSoftness < 1.f - SIMD_EPSILON)
{
m_twistLimitRatio = (m_twistAngle - m_twistSpan * m_limitSoftness)/
(m_twistSpan - m_twistSpan * m_limitSoftness);
}
// twist correction tries to get back to soft limit
m_twistCorrection = m_twistAngle - (m_twistSpan * m_limitSoftness);
m_twistAxis = quatRotate(qB, -twistAxis);
m_kTwist = btScalar(1.) /
(getRigidBodyA().computeAngularImpulseDenominator(m_twistAxis) +
getRigidBodyB().computeAngularImpulseDenominator(m_twistAxis));
}
if (m_solveSwingLimit)
m_twistAxisA = quatRotate(qA, -twistAxis);
}
else
{
m_twistAngle = btScalar(0.f);
}
}
} // btConeTwistConstraint::calcAngleInfo2()
// given a cone rotation in constraint space, (pre: twist must already be removed)
// this method computes its corresponding swing angle and axis.
// more interestingly, it computes the cone/swing limit (angle) for this cone "pose".
void btConeTwistConstraint::computeConeLimitInfo(const btQuaternion& qCone,
btScalar& swingAngle, // out
btVector3& vSwingAxis, // out
btScalar& swingLimit) // out
{
swingAngle = qCone.getAngle();
if (swingAngle > SIMD_EPSILON)
{
vSwingAxis = btVector3(qCone.x(), qCone.y(), qCone.z());
vSwingAxis.normalize();
if (fabs(vSwingAxis.x()) > SIMD_EPSILON)
{
// non-zero twist?! this should never happen.
int wtf = 0; wtf = wtf;
}
// Compute limit for given swing. tricky:
// Given a swing axis, we're looking for the intersection with the bounding cone ellipse.
// (Since we're dealing with angles, this ellipse is embedded on the surface of a sphere.)
// For starters, compute the direction from center to surface of ellipse.
// This is just the perpendicular (ie. rotate 2D vector by PI/2) of the swing axis.
// (vSwingAxis is the cone rotation (in z,y); change vars and rotate to (x,y) coords.)
btScalar xEllipse = vSwingAxis.y();
btScalar yEllipse = -vSwingAxis.z();
// Now, we use the slope of the vector (using x/yEllipse) and find the length
// of the line that intersects the ellipse:
// x^2 y^2
// --- + --- = 1, where a and b are semi-major axes 2 and 1 respectively (ie. the limits)
// a^2 b^2
// Do the math and it should be clear.
swingLimit = m_swingSpan1; // if xEllipse == 0, we have a pure vSwingAxis.z rotation: just use swingspan1
if (fabs(xEllipse) > SIMD_EPSILON)
{
btScalar surfaceSlope2 = (yEllipse*yEllipse)/(xEllipse*xEllipse);
btScalar norm = 1 / (m_swingSpan2 * m_swingSpan2);
norm += surfaceSlope2 / (m_swingSpan1 * m_swingSpan1);
btScalar swingLimit2 = (1 + surfaceSlope2) / norm;
swingLimit = sqrt(swingLimit2);
}
// test!
/*swingLimit = m_swingSpan2;
if (fabs(vSwingAxis.z()) > SIMD_EPSILON)
{
btScalar mag_2 = m_swingSpan1*m_swingSpan1 + m_swingSpan2*m_swingSpan2;
btScalar sinphi = m_swingSpan2 / sqrt(mag_2);
btScalar phi = asin(sinphi);
btScalar theta = atan2(fabs(vSwingAxis.y()),fabs(vSwingAxis.z()));
btScalar alpha = 3.14159f - theta - phi;
btScalar sinalpha = sin(alpha);
swingLimit = m_swingSpan1 * sinphi/sinalpha;
}*/
}
else if (swingAngle < 0)
{
// this should never happen!
int wtf = 0; wtf = wtf;
}
}
btVector3 btConeTwistConstraint::GetPointForAngle(btScalar fAngleInRadians, btScalar fLength) const
{
// compute x/y in ellipse using cone angle (0 -> 2*PI along surface of cone)
btScalar xEllipse = btCos(fAngleInRadians);
btScalar yEllipse = btSin(fAngleInRadians);
// Use the slope of the vector (using x/yEllipse) and find the length
// of the line that intersects the ellipse:
// x^2 y^2
// --- + --- = 1, where a and b are semi-major axes 2 and 1 respectively (ie. the limits)
// a^2 b^2
// Do the math and it should be clear.
float swingLimit = m_swingSpan1; // if xEllipse == 0, just use axis b (1)
if (fabs(xEllipse) > SIMD_EPSILON)
{
btScalar surfaceSlope2 = (yEllipse*yEllipse)/(xEllipse*xEllipse);
btScalar norm = 1 / (m_swingSpan2 * m_swingSpan2);
norm += surfaceSlope2 / (m_swingSpan1 * m_swingSpan1);
btScalar swingLimit2 = (1 + surfaceSlope2) / norm;
swingLimit = sqrt(swingLimit2);
}
// convert into point in constraint space:
// note: twist is x-axis, swing 1 and 2 are along the z and y axes respectively
btVector3 vSwingAxis(0, xEllipse, -yEllipse);
btQuaternion qSwing(vSwingAxis, swingLimit);
btVector3 vPointInConstraintSpace(fLength,0,0);
return quatRotate(qSwing, vPointInConstraintSpace);
}
// given a twist rotation in constraint space, (pre: cone must already be removed)
// this method computes its corresponding angle and axis.
void btConeTwistConstraint::computeTwistLimitInfo(const btQuaternion& qTwist,
btScalar& twistAngle, // out
btVector3& vTwistAxis) // out
{
btQuaternion qMinTwist = qTwist;
twistAngle = qTwist.getAngle();
if (twistAngle > SIMD_PI) // long way around. flip quat and recalculate.
{
qMinTwist = operator-(qTwist);
twistAngle = qMinTwist.getAngle();
}
if (twistAngle < 0)
{
// this should never happen
int wtf = 0; wtf = wtf;
}
vTwistAxis = btVector3(qMinTwist.x(), qMinTwist.y(), qMinTwist.z());
if (twistAngle > SIMD_EPSILON)
vTwistAxis.normalize();
}
void btConeTwistConstraint::adjustSwingAxisToUseEllipseNormal(btVector3& vSwingAxis) const
{
// the swing axis is computed as the "twist-free" cone rotation,
// but the cone limit is not circular, but elliptical (if swingspan1 != swingspan2).
// so, if we're outside the limits, the closest way back inside the cone isn't
// along the vector back to the center. better (and more stable) to use the ellipse normal.
// convert swing axis to direction from center to surface of ellipse
// (ie. rotate 2D vector by PI/2)
btScalar y = -vSwingAxis.z();
btScalar z = vSwingAxis.y();
// do the math...
if (fabs(z) > SIMD_EPSILON) // avoid division by 0. and we don't need an update if z == 0.
{
// compute gradient/normal of ellipse surface at current "point"
btScalar grad = y/z;
grad *= m_swingSpan2 / m_swingSpan1;
// adjust y/z to represent normal at point (instead of vector to point)
if (y > 0)
y = fabs(grad * z);
else
y = -fabs(grad * z);
// convert ellipse direction back to swing axis
vSwingAxis.setZ(-y);
vSwingAxis.setY( z);
vSwingAxis.normalize();
}
}
void btConeTwistConstraint::setMotorTarget(const btQuaternion &q)
{
btTransform trACur = m_rbA.getCenterOfMassTransform();
btTransform trBCur = m_rbB.getCenterOfMassTransform();
btTransform trABCur = trBCur.inverse() * trACur;
btQuaternion qABCur = trABCur.getRotation();
btTransform trConstraintCur = (trBCur * m_rbBFrame).inverse() * (trACur * m_rbAFrame);
btQuaternion qConstraintCur = trConstraintCur.getRotation();
btQuaternion qConstraint = m_rbBFrame.getRotation().inverse() * q * m_rbAFrame.getRotation();
setMotorTargetInConstraintSpace(qConstraint);
}
void btConeTwistConstraint::setMotorTargetInConstraintSpace(const btQuaternion &q)
{
m_qTarget = q;
// clamp motor target to within limits
{
btScalar softness = 1.f;//m_limitSoftness;
// split into twist and cone
btVector3 vTwisted = quatRotate(m_qTarget, vTwist);
btQuaternion qTargetCone = shortestArcQuat(vTwist, vTwisted); qTargetCone.normalize();
btQuaternion qTargetTwist = qTargetCone.inverse() * m_qTarget; qTargetTwist.normalize();
// clamp cone
if (m_swingSpan1 >= btScalar(0.05f) && m_swingSpan2 >= btScalar(0.05f))
{
btScalar swingAngle, swingLimit; btVector3 swingAxis;
computeConeLimitInfo(qTargetCone, swingAngle, swingAxis, swingLimit);
if (fabs(swingAngle) > SIMD_EPSILON)
{
if (swingAngle > swingLimit*softness)
swingAngle = swingLimit*softness;
else if (swingAngle < -swingLimit*softness)
swingAngle = -swingLimit*softness;
qTargetCone = btQuaternion(swingAxis, swingAngle);
}
}
// clamp twist
if (m_twistSpan >= btScalar(0.05f))
{
btScalar twistAngle; btVector3 twistAxis;
computeTwistLimitInfo(qTargetTwist, twistAngle, twistAxis);
if (fabs(twistAngle) > SIMD_EPSILON)
{
// eddy todo: limitSoftness used here???
if (twistAngle > m_twistSpan*softness)
twistAngle = m_twistSpan*softness;
else if (twistAngle < -m_twistSpan*softness)
twistAngle = -m_twistSpan*softness;
qTargetTwist = btQuaternion(twistAxis, twistAngle);
}
}
m_qTarget = qTargetCone * qTargetTwist;
}
}
//-----------------------------------------------------------------------------
//-----------------------------------------------------------------------------
//-----------------------------------------------------------------------------

45
src/BulletDynamics/ConstraintSolver/btConeTwistConstraint.h
View File

@@ -42,6 +42,8 @@ public:
btScalar m_biasFactor;
btScalar m_relaxationFactor;
btScalar m_damping;
btScalar m_swingSpan1;
btScalar m_swingSpan2;
btScalar m_twistSpan;
@@ -67,6 +69,18 @@ public:
bool m_useSolveConstraintObsolete;
// not yet used...
btScalar m_swingLimitRatio;
btScalar m_twistLimitRatio;
btVector3 m_twistAxisA;
// motor
bool m_bMotorEnabled;
bool m_bNormalizedMotorStrength;
btQuaternion m_qTarget;
btScalar m_maxMotorImpulse;
btVector3 m_accMotorImpulse;
public:
btConeTwistConstraint(btRigidBody& rbA,btRigidBody& rbB,const btTransform& rbAFrame, const btTransform& rbBFrame);
@@ -130,6 +144,7 @@ public:
}
void calcAngleInfo();
void calcAngleInfo2();
inline btScalar getSwingSpan1()
{
@@ -147,8 +162,38 @@ public:
{
return m_twistAngle;
}
bool isPastSwingLimit() { return m_solveSwingLimit; }
void setDamping(btScalar damping) { m_damping = damping; }
void enableMotor(bool b) { m_bMotorEnabled = b; }
void setMaxMotorImpulse(btScalar maxMotorImpulse) { m_maxMotorImpulse = maxMotorImpulse; m_bNormalizedMotorStrength = false; }
void setMaxMotorImpulseNormalized(btScalar maxMotorImpulse) { m_maxMotorImpulse = maxMotorImpulse; m_bNormalizedMotorStrength = true; }
// setMotorTarget:
// q: the desired rotation of bodyA wrt bodyB.
// note: if q violates the joint limits, the internal target is clamped to avoid conflicting impulses (very bad for stability)
// note: don't forget to enableMotor()
void setMotorTarget(const btQuaternion &q);
// same as above, but q is the desired rotation of frameA wrt frameB in constraint space
void setMotorTargetInConstraintSpace(const btQuaternion &q);
btVector3 GetPointForAngle(btScalar fAngleInRadians, btScalar fLength) const;
protected:
void init();
void computeConeLimitInfo(const btQuaternion& qCone, // in
btScalar& swingAngle, btVector3& vSwingAxis, btScalar& swingLimit); // all outs
void computeTwistLimitInfo(const btQuaternion& qTwist, // in
btScalar& twistAngle, btVector3& vTwistAxis); // all outs
void adjustSwingAxisToUseEllipseNormal(btVector3& vSwingAxis) const;
};
#endif //CONETWISTCONSTRAINT_H

9
src/BulletDynamics/ConstraintSolver/btTypedConstraint.cpp
View File

@@ -25,7 +25,8 @@ m_userConstraintId(-1),
m_constraintType (type),
m_rbA(s_fixed),
m_rbB(s_fixed),
m_appliedImpulse(btScalar(0.))
m_appliedImpulse(btScalar(0.)),
m_DbgDrawSize(btScalar(-1.f))
{
s_fixed.setMassProps(btScalar(0.),btVector3(btScalar(0.),btScalar(0.),btScalar(0.)));
}
@@ -35,7 +36,8 @@ m_userConstraintId(-1),
m_constraintType (type),
m_rbA(rbA),
m_rbB(s_fixed),
m_appliedImpulse(btScalar(0.))
m_appliedImpulse(btScalar(0.)),
m_DbgDrawSize(btScalar(-1.f))
{
s_fixed.setMassProps(btScalar(0.),btVector3(btScalar(0.),btScalar(0.),btScalar(0.)));
@@ -48,7 +50,8 @@ m_userConstraintId(-1),
m_constraintType (type),
m_rbA(rbA),
m_rbB(rbB),
m_appliedImpulse(btScalar(0.))
m_appliedImpulse(btScalar(0.)),
m_DbgDrawSize(btScalar(-1.f))
{
s_fixed.setMassProps(btScalar(0.),btVector3(btScalar(0.),btScalar(0.),btScalar(0.)));

11
src/BulletDynamics/ConstraintSolver/btTypedConstraint.h
View File

@@ -53,6 +53,7 @@ protected:
btRigidBody& m_rbA;
btRigidBody& m_rbB;
btScalar m_appliedImpulse;
btScalar m_DbgDrawSize;
public:
@@ -95,6 +96,7 @@ public:
int *findex;
};
virtual void buildJacobian() = 0;
virtual void setupSolverConstraint(btConstraintArray& ca, int solverBodyA,int solverBodyB, btScalar timeStep)
@@ -161,6 +163,15 @@ public:
return m_constraintType;
}
void setDbgDrawSize(btScalar dbgDrawSize)
{
m_DbgDrawSize = dbgDrawSize;
}
btScalar getDbgDrawSize()
{
return m_DbgDrawSize;
}
};
#endif //TYPED_CONSTRAINT_H

92
src/BulletDynamics/Dynamics/btDiscreteDynamicsWorld.cpp
View File

@@ -1210,15 +1210,15 @@ void btDiscreteDynamicsWorld::debugDrawObject(const btTransform& worldTransform,
}
#define DEBUG_DRAW_CONSTR_LEN btScalar(5.f)
void btDiscreteDynamicsWorld::debugDrawConstraint(btTypedConstraint* constraint)
{
bool drawFrames = (getDebugDrawer()->getDebugMode() & btIDebugDraw::DBG_DrawConstraints) != 0;
bool drawLimits = (getDebugDrawer()->getDebugMode() & btIDebugDraw::DBG_DrawConstraintLimits) != 0;
btScalar dbgDrawSize = constraint->getDbgDrawSize();
if(dbgDrawSize <= btScalar(0.f))
{
return;
}
switch(constraint->getConstraintType())
{
@@ -1230,21 +1230,21 @@ void btDiscreteDynamicsWorld::debugDrawConstraint(btTypedConstraint* constraint)
btVector3 pivot = p2pC->getPivotInA();
pivot = p2pC->getRigidBodyA().getCenterOfMassTransform() * pivot;
tr.setOrigin(pivot);
getDebugDrawer()->drawTransform(tr, DEBUG_DRAW_CONSTR_LEN);
getDebugDrawer()->drawTransform(tr, dbgDrawSize);
// that ideally should draw the same frame
pivot = p2pC->getPivotInB();
pivot = p2pC->getRigidBodyB().getCenterOfMassTransform() * pivot;
tr.setOrigin(pivot);
if(drawFrames) getDebugDrawer()->drawTransform(tr, DEBUG_DRAW_CONSTR_LEN);
if(drawFrames) getDebugDrawer()->drawTransform(tr, dbgDrawSize);
}
break;
case HINGE_CONSTRAINT_TYPE:
{
btHingeConstraint* pHinge = (btHingeConstraint*)constraint;
btTransform tr = pHinge->getRigidBodyA().getCenterOfMassTransform() * pHinge->getAFrame();
if(drawFrames) getDebugDrawer()->drawTransform(tr, DEBUG_DRAW_CONSTR_LEN);
if(drawFrames) getDebugDrawer()->drawTransform(tr, dbgDrawSize);
tr = pHinge->getRigidBodyB().getCenterOfMassTransform() * pHinge->getBFrame();
if(drawFrames) getDebugDrawer()->drawTransform(tr, DEBUG_DRAW_CONSTR_LEN);
if(drawFrames) getDebugDrawer()->drawTransform(tr, dbgDrawSize);
btScalar minAng = pHinge->getLowerLimit();
btScalar maxAng = pHinge->getUpperLimit();
if(minAng == maxAng)
@@ -1263,7 +1263,7 @@ void btDiscreteDynamicsWorld::debugDrawConstraint(btTypedConstraint* constraint)
btVector3& center = tr.getOrigin();
btVector3 normal = tr.getBasis().getColumn(2);
btVector3 axis = tr.getBasis().getColumn(0);
getDebugDrawer()->drawArc(center, normal, axis, DEBUG_DRAW_CONSTR_LEN, DEBUG_DRAW_CONSTR_LEN, minAng, maxAng, btVector3(0,0,0), drawSect);
getDebugDrawer()->drawArc(center, normal, axis, dbgDrawSize, dbgDrawSize, minAng, maxAng, btVector3(0,0,0), drawSect);
}
}
break;
@@ -1271,44 +1271,38 @@ void btDiscreteDynamicsWorld::debugDrawConstraint(btTypedConstraint* constraint)
{
btConeTwistConstraint* pCT = (btConeTwistConstraint*)constraint;
btTransform tr = pCT->getRigidBodyA().getCenterOfMassTransform() * pCT->getAFrame();
if(drawFrames) getDebugDrawer()->drawTransform(tr, DEBUG_DRAW_CONSTR_LEN);
if(drawFrames) getDebugDrawer()->drawTransform(tr, dbgDrawSize);
tr = pCT->getRigidBodyB().getCenterOfMassTransform() * pCT->getBFrame();
if(drawFrames) getDebugDrawer()->drawTransform(tr, DEBUG_DRAW_CONSTR_LEN);
if(drawFrames) getDebugDrawer()->drawTransform(tr, dbgDrawSize);
if(drawLimits)
{
btScalar sw1 = pCT->getSwingSpan1();
btScalar sw2 = pCT->getSwingSpan2();
btScalar dist = sw1 > sw2 ? sw1 : sw2;
dist = DEBUG_DRAW_CONSTR_LEN * btCos(dist);
sw1 = DEBUG_DRAW_CONSTR_LEN * btSin(sw1);
sw2 = DEBUG_DRAW_CONSTR_LEN * btSin(sw2);
tr = pCT->getRigidBodyA().getCenterOfMassTransform() * pCT->getAFrame();
tr.setOrigin(btVector3(0,0,0));
btVector3 center = btVector3(dist, 0.f, 0.f);
center = tr * center;
//const btScalar length = btScalar(5);
const btScalar length = dbgDrawSize;
static int nSegments = 8*4;
btScalar fAngleInRadians = btScalar(2.*3.1415926) * (btScalar)(nSegments-1)/btScalar(nSegments);
btVector3 pPrev = pCT->GetPointForAngle(fAngleInRadians, length);
pPrev = tr * pPrev;
for (int i=0; i<nSegments; i++)
{
fAngleInRadians = btScalar(2.*3.1415926) * (btScalar)i/btScalar(nSegments);
btVector3 pCur = pCT->GetPointForAngle(fAngleInRadians, length);
pCur = tr * pCur;
getDebugDrawer()->drawLine(pPrev, pCur, btVector3(0,0,0));
if (i%(nSegments/8) == 0)
getDebugDrawer()->drawLine(tr.getOrigin(), pCur, btVector3(0,0,0));
pPrev = pCur;
}
btVector3 pivot = pCT->getRigidBodyB().getCenterOfMassTransform() * pCT->getBFrame().getOrigin();
center += pivot;
tr = pCT->getRigidBodyA().getCenterOfMassTransform() * pCT->getAFrame();
btVector3 normal = tr.getBasis().getColumn(0);
btVector3 axis1 = tr.getBasis().getColumn(1);
btVector3 axis2 = tr.getBasis().getColumn(2);
btVector3 vert = center + axis1 * sw1;
getDebugDrawer()->drawLine(pivot, vert, btVector3(0,0,0));
vert = center - axis1 * sw1;
getDebugDrawer()->drawLine(pivot, vert, btVector3(0,0,0));
vert = center + axis2 * sw2;
getDebugDrawer()->drawLine(pivot, vert, btVector3(0,0,0));
vert = center - axis2 * sw2;
getDebugDrawer()->drawLine(pivot, vert, btVector3(0,0,0));
btScalar minAng = btScalar(0.f);
btScalar maxAng = SIMD_2_PI;
getDebugDrawer()->drawArc(center, normal, axis1, sw1, sw2, minAng, maxAng, btVector3(0,0,0), false);
btScalar tws = pCT->getTwistSpan();
btScalar twa = pCT->getTwistAngle();
tr = pCT->getRigidBodyB().getCenterOfMassTransform() * pCT->getBFrame();
normal = tr.getBasis().getColumn(0);
axis1 = tr.getBasis().getColumn(1);
getDebugDrawer()->drawArc(pivot, normal, axis1, DEBUG_DRAW_CONSTR_LEN, DEBUG_DRAW_CONSTR_LEN, -twa-tws, -twa+tws, btVector3(0,0,0), true);
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);
}
}
@@ -1317,9 +1311,9 @@ void btDiscreteDynamicsWorld::debugDrawConstraint(btTypedConstraint* constraint)
{
btGeneric6DofConstraint* p6DOF = (btGeneric6DofConstraint*)constraint;
btTransform tr = p6DOF->getCalculatedTransformA();
if(drawFrames) getDebugDrawer()->drawTransform(tr, DEBUG_DRAW_CONSTR_LEN);
if(drawFrames) getDebugDrawer()->drawTransform(tr, dbgDrawSize);
tr = p6DOF->getCalculatedTransformB();
if(drawFrames) getDebugDrawer()->drawTransform(tr, DEBUG_DRAW_CONSTR_LEN);
if(drawFrames) getDebugDrawer()->drawTransform(tr, dbgDrawSize);
if(drawLimits)
{
tr = p6DOF->getCalculatedTransformA();
@@ -1330,7 +1324,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, DEBUG_DRAW_CONSTR_LEN, minTh, maxTh, minPs, maxPs, btVector3(0,0,0));
getDebugDrawer()->drawSpherePatch(center, up, axis, dbgDrawSize, minTh, maxTh, minPs, maxPs, btVector3(0,0,0));
axis = tr.getBasis().getColumn(1);
btScalar ay = p6DOF->getAngle(1);
btScalar az = p6DOF->getAngle(2);
@@ -1346,7 +1340,7 @@ 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, DEBUG_DRAW_CONSTR_LEN, DEBUG_DRAW_CONSTR_LEN, minFi, maxFi, btVector3(0,0,0), true);
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;
@@ -1358,9 +1352,9 @@ void btDiscreteDynamicsWorld::debugDrawConstraint(btTypedConstraint* constraint)
{
btSliderConstraint* pSlider = (btSliderConstraint*)constraint;
btTransform tr = pSlider->getCalculatedTransformA();
if(drawFrames) getDebugDrawer()->drawTransform(tr, DEBUG_DRAW_CONSTR_LEN);
if(drawFrames) getDebugDrawer()->drawTransform(tr, dbgDrawSize);
tr = pSlider->getCalculatedTransformB();
if(drawFrames) getDebugDrawer()->drawTransform(tr, DEBUG_DRAW_CONSTR_LEN);
if(drawFrames) getDebugDrawer()->drawTransform(tr, dbgDrawSize);
if(drawLimits)
{
btTransform tr = pSlider->getCalculatedTransformA();
@@ -1372,7 +1366,7 @@ void btDiscreteDynamicsWorld::debugDrawConstraint(btTypedConstraint* constraint)
btScalar a_min = pSlider->getLowerAngLimit();
btScalar a_max = pSlider->getUpperAngLimit();
const btVector3& center = pSlider->getCalculatedTransformB().getOrigin();
getDebugDrawer()->drawArc(center, normal, axis, DEBUG_DRAW_CONSTR_LEN, DEBUG_DRAW_CONSTR_LEN, a_min, a_max, btVector3(0,0,0), true);
getDebugDrawer()->drawArc(center, normal, axis, dbgDrawSize, dbgDrawSize, a_min, a_max, btVector3(0,0,0), true);
}
}
break;
@@ -1414,3 +1408,5 @@ const btTypedConstraint* btDiscreteDynamicsWorld::getConstraint(int index) const
{
return m_constraints[index];
}