summaryrefslogtreecommitdiff
path: root/thirdparty/bullet/BulletDynamics/ConstraintSolver/btConeTwistConstraint.cpp
diff options
context:
space:
mode:
Diffstat (limited to 'thirdparty/bullet/BulletDynamics/ConstraintSolver/btConeTwistConstraint.cpp')
-rw-r--r--thirdparty/bullet/BulletDynamics/ConstraintSolver/btConeTwistConstraint.cpp604
1 files changed, 288 insertions, 316 deletions
diff --git a/thirdparty/bullet/BulletDynamics/ConstraintSolver/btConeTwistConstraint.cpp b/thirdparty/bullet/BulletDynamics/ConstraintSolver/btConeTwistConstraint.cpp
index 0572256f74..10678b2a61 100644
--- a/thirdparty/bullet/BulletDynamics/ConstraintSolver/btConeTwistConstraint.cpp
+++ b/thirdparty/bullet/BulletDynamics/ConstraintSolver/btConeTwistConstraint.cpp
@@ -15,49 +15,37 @@ subject to the following restrictions:
Written by: Marcus Hennix
*/
-
#include "btConeTwistConstraint.h"
#include "BulletDynamics/Dynamics/btRigidBody.h"
#include "LinearMath/btTransformUtil.h"
#include "LinearMath/btMinMax.h"
#include <new>
-
-
//#define CONETWIST_USE_OBSOLETE_SOLVER true
#define CONETWIST_USE_OBSOLETE_SOLVER false
#define CONETWIST_DEF_FIX_THRESH btScalar(.05f)
-
SIMD_FORCE_INLINE btScalar computeAngularImpulseDenominator(const btVector3& axis, const btMatrix3x3& invInertiaWorld)
{
btVector3 vec = axis * invInertiaWorld;
return axis.dot(vec);
}
-
-
-
-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(CONETWIST_USE_OBSOLETE_SOLVER)
+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(CONETWIST_USE_OBSOLETE_SOLVER)
{
init();
}
-btConeTwistConstraint::btConeTwistConstraint(btRigidBody& rbA,const btTransform& rbAFrame)
- :btTypedConstraint(CONETWIST_CONSTRAINT_TYPE,rbA),m_rbAFrame(rbAFrame),
- m_angularOnly(false),
- m_useSolveConstraintObsolete(CONETWIST_USE_OBSOLETE_SOLVER)
+btConeTwistConstraint::btConeTwistConstraint(btRigidBody& rbA, const btTransform& rbAFrame)
+ : btTypedConstraint(CONETWIST_CONSTRAINT_TYPE, rbA), m_rbAFrame(rbAFrame), m_angularOnly(false), m_useSolveConstraintObsolete(CONETWIST_USE_OBSOLETE_SOLVER)
{
m_rbBFrame = m_rbAFrame;
m_rbBFrame.setOrigin(btVector3(0., 0., 0.));
- init();
+ init();
}
-
void btConeTwistConstraint::init()
{
m_angularOnly = false;
@@ -75,30 +63,29 @@ void btConeTwistConstraint::init()
m_angCFM = btScalar(0.f);
}
-
-void btConeTwistConstraint::getInfo1 (btConstraintInfo1* info)
+void btConeTwistConstraint::getInfo1(btConstraintInfo1* info)
{
if (m_useSolveConstraintObsolete)
{
info->m_numConstraintRows = 0;
info->nub = 0;
- }
+ }
else
{
info->m_numConstraintRows = 3;
info->nub = 3;
- calcAngleInfo2(m_rbA.getCenterOfMassTransform(),m_rbB.getCenterOfMassTransform(),m_rbA.getInvInertiaTensorWorld(),m_rbB.getInvInertiaTensorWorld());
- if(m_solveSwingLimit)
+ calcAngleInfo2(m_rbA.getCenterOfMassTransform(), m_rbB.getCenterOfMassTransform(), m_rbA.getInvInertiaTensorWorld(), m_rbB.getInvInertiaTensorWorld());
+ if (m_solveSwingLimit)
{
info->m_numConstraintRows++;
info->nub--;
- if((m_swingSpan1 < m_fixThresh) && (m_swingSpan2 < m_fixThresh))
+ if ((m_swingSpan1 < m_fixThresh) && (m_swingSpan2 < m_fixThresh))
{
info->m_numConstraintRows++;
info->nub--;
}
}
- if(m_solveTwistLimit)
+ if (m_solveTwistLimit)
{
info->m_numConstraintRows++;
info->nub--;
@@ -106,90 +93,88 @@ void btConeTwistConstraint::getInfo1 (btConstraintInfo1* info)
}
}
-void btConeTwistConstraint::getInfo1NonVirtual (btConstraintInfo1* info)
+void btConeTwistConstraint::getInfo1NonVirtual(btConstraintInfo1* info)
{
//always reserve 6 rows: object transform is not available on SPU
info->m_numConstraintRows = 6;
info->nub = 0;
-
}
-
-void btConeTwistConstraint::getInfo2 (btConstraintInfo2* info)
+void btConeTwistConstraint::getInfo2(btConstraintInfo2* info)
{
- getInfo2NonVirtual(info,m_rbA.getCenterOfMassTransform(),m_rbB.getCenterOfMassTransform(),m_rbA.getInvInertiaTensorWorld(),m_rbB.getInvInertiaTensorWorld());
+ getInfo2NonVirtual(info, m_rbA.getCenterOfMassTransform(), m_rbB.getCenterOfMassTransform(), m_rbA.getInvInertiaTensorWorld(), m_rbB.getInvInertiaTensorWorld());
}
-void btConeTwistConstraint::getInfo2NonVirtual (btConstraintInfo2* info,const btTransform& transA,const btTransform& transB,const btMatrix3x3& invInertiaWorldA,const btMatrix3x3& invInertiaWorldB)
+void btConeTwistConstraint::getInfo2NonVirtual(btConstraintInfo2* info, const btTransform& transA, const btTransform& transB, const btMatrix3x3& invInertiaWorldA, const btMatrix3x3& invInertiaWorldB)
{
- calcAngleInfo2(transA,transB,invInertiaWorldA,invInertiaWorldB);
-
+ calcAngleInfo2(transA, transB, invInertiaWorldA, invInertiaWorldB);
+
btAssert(!m_useSolveConstraintObsolete);
- // set jacobian
- info->m_J1linearAxis[0] = 1;
- info->m_J1linearAxis[info->rowskip+1] = 1;
- info->m_J1linearAxis[2*info->rowskip+2] = 1;
+ // set jacobian
+ info->m_J1linearAxis[0] = 1;
+ info->m_J1linearAxis[info->rowskip + 1] = 1;
+ info->m_J1linearAxis[2 * info->rowskip + 2] = 1;
btVector3 a1 = transA.getBasis() * m_rbAFrame.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* angular1 = (btVector3*)(info->m_J1angularAxis + info->rowskip);
+ btVector3* angular2 = (btVector3*)(info->m_J1angularAxis + 2 * info->rowskip);
btVector3 a1neg = -a1;
- a1neg.getSkewSymmetricMatrix(angular0,angular1,angular2);
+ a1neg.getSkewSymmetricMatrix(angular0, angular1, angular2);
}
- info->m_J2linearAxis[0] = -1;
- info->m_J2linearAxis[info->rowskip+1] = -1;
- info->m_J2linearAxis[2*info->rowskip+2] = -1;
+ info->m_J2linearAxis[0] = -1;
+ info->m_J2linearAxis[info->rowskip + 1] = -1;
+ info->m_J2linearAxis[2 * info->rowskip + 2] = -1;
btVector3 a2 = transB.getBasis() * m_rbBFrame.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);
+ 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
+ // set right hand side
btScalar linERP = (m_flags & BT_CONETWIST_FLAGS_LIN_ERP) ? m_linERP : info->erp;
- btScalar k = info->fps * linERP;
- int j;
- for (j=0; j<3; j++)
- {
- info->m_constraintError[j*info->rowskip] = k * (a2[j] + transB.getOrigin()[j] - a1[j] - transA.getOrigin()[j]);
- info->m_lowerLimit[j*info->rowskip] = -SIMD_INFINITY;
- info->m_upperLimit[j*info->rowskip] = SIMD_INFINITY;
- if(m_flags & BT_CONETWIST_FLAGS_LIN_CFM)
+ btScalar k = info->fps * linERP;
+ int j;
+ for (j = 0; j < 3; j++)
+ {
+ info->m_constraintError[j * info->rowskip] = k * (a2[j] + transB.getOrigin()[j] - a1[j] - transA.getOrigin()[j]);
+ info->m_lowerLimit[j * info->rowskip] = -SIMD_INFINITY;
+ info->m_upperLimit[j * info->rowskip] = SIMD_INFINITY;
+ if (m_flags & BT_CONETWIST_FLAGS_LIN_CFM)
{
- info->cfm[j*info->rowskip] = m_linCFM;
+ info->cfm[j * info->rowskip] = m_linCFM;
}
- }
+ }
int row = 3;
- int srow = row * info->rowskip;
+ int srow = row * info->rowskip;
btVector3 ax1;
// angular limits
- if(m_solveSwingLimit)
+ if (m_solveSwingLimit)
{
- btScalar *J1 = info->m_J1angularAxis;
- btScalar *J2 = info->m_J2angularAxis;
- if((m_swingSpan1 < m_fixThresh) && (m_swingSpan2 < m_fixThresh))
+ btScalar* J1 = info->m_J1angularAxis;
+ btScalar* J2 = info->m_J2angularAxis;
+ if ((m_swingSpan1 < m_fixThresh) && (m_swingSpan2 < m_fixThresh))
{
- btTransform trA = transA*m_rbAFrame;
+ btTransform trA = transA * m_rbAFrame;
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];
+ 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 * m_relaxationFactor;
- info->m_constraintError[srow] = fact * m_swingAxis.dot(p);
- info->m_constraintError[srow1] = fact * m_swingAxis.dot(q);
+ info->m_constraintError[srow] = fact * m_swingAxis.dot(p);
+ info->m_constraintError[srow1] = fact * m_swingAxis.dot(q);
info->m_lowerLimit[srow] = -SIMD_INFINITY;
info->m_upperLimit[srow] = SIMD_INFINITY;
info->m_lowerLimit[srow1] = -SIMD_INFINITY;
@@ -199,16 +184,16 @@ void btConeTwistConstraint::getInfo2NonVirtual (btConstraintInfo2* info,const bt
else
{
ax1 = m_swingAxis * m_relaxationFactor * m_relaxationFactor;
- 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];
+ 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 * m_biasFactor;
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;
}
@@ -218,36 +203,35 @@ void btConeTwistConstraint::getInfo2NonVirtual (btConstraintInfo2* info,const bt
srow += info->rowskip;
}
}
- if(m_solveTwistLimit)
+ if (m_solveTwistLimit)
{
ax1 = m_twistAxis * m_relaxationFactor * m_relaxationFactor;
- 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* 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 * m_biasFactor;
info->m_constraintError[srow] = k * m_twistCorrection;
- if(m_flags & BT_CONETWIST_FLAGS_ANG_CFM)
+ if (m_flags & BT_CONETWIST_FLAGS_ANG_CFM)
{
info->cfm[srow] = m_angCFM;
}
- if(m_twistSpan > 0.0f)
+ if (m_twistSpan > 0.0f)
{
-
- if(m_twistCorrection > 0.0f)
+ if (m_twistCorrection > 0.0f)
{
info->m_lowerLimit[srow] = 0;
info->m_upperLimit[srow] = SIMD_INFINITY;
- }
+ }
else
{
info->m_lowerLimit[srow] = -SIMD_INFINITY;
info->m_upperLimit[srow] = 0;
- }
+ }
}
else
{
@@ -257,22 +241,20 @@ void btConeTwistConstraint::getInfo2NonVirtual (btConstraintInfo2* info,const bt
srow += info->rowskip;
}
}
-
-
-void btConeTwistConstraint::buildJacobian()
+void btConeTwistConstraint::buildJacobian()
{
if (m_useSolveConstraintObsolete)
{
m_appliedImpulse = btScalar(0.);
m_accTwistLimitImpulse = btScalar(0.);
m_accSwingLimitImpulse = btScalar(0.);
- m_accMotorImpulse = btVector3(0.,0.,0.);
+ m_accMotorImpulse = btVector3(0., 0., 0.);
if (!m_angularOnly)
{
- btVector3 pivotAInW = m_rbA.getCenterOfMassTransform()*m_rbAFrame.getOrigin();
- btVector3 pivotBInW = m_rbB.getCenterOfMassTransform()*m_rbBFrame.getOrigin();
+ btVector3 pivotAInW = m_rbA.getCenterOfMassTransform() * m_rbAFrame.getOrigin();
+ btVector3 pivotBInW = m_rbB.getCenterOfMassTransform() * m_rbBFrame.getOrigin();
btVector3 relPos = pivotBInW - pivotAInW;
btVector3 normal[3];
@@ -282,71 +264,68 @@ void btConeTwistConstraint::buildJacobian()
}
else
{
- normal[0].setValue(btScalar(1.0),0,0);
+ normal[0].setValue(btScalar(1.0), 0, 0);
}
btPlaneSpace1(normal[0], normal[1], normal[2]);
- for (int i=0;i<3;i++)
+ for (int i = 0; i < 3; i++)
{
new (&m_jac[i]) btJacobianEntry(
- m_rbA.getCenterOfMassTransform().getBasis().transpose(),
- m_rbB.getCenterOfMassTransform().getBasis().transpose(),
- pivotAInW - m_rbA.getCenterOfMassPosition(),
- pivotBInW - m_rbB.getCenterOfMassPosition(),
- normal[i],
- m_rbA.getInvInertiaDiagLocal(),
- m_rbA.getInvMass(),
- m_rbB.getInvInertiaDiagLocal(),
- m_rbB.getInvMass());
+ m_rbA.getCenterOfMassTransform().getBasis().transpose(),
+ m_rbB.getCenterOfMassTransform().getBasis().transpose(),
+ pivotAInW - m_rbA.getCenterOfMassPosition(),
+ pivotBInW - m_rbB.getCenterOfMassPosition(),
+ normal[i],
+ m_rbA.getInvInertiaDiagLocal(),
+ m_rbA.getInvMass(),
+ m_rbB.getInvInertiaDiagLocal(),
+ m_rbB.getInvMass());
}
}
- calcAngleInfo2(m_rbA.getCenterOfMassTransform(),m_rbB.getCenterOfMassTransform(),m_rbA.getInvInertiaTensorWorld(),m_rbB.getInvInertiaTensorWorld());
+ calcAngleInfo2(m_rbA.getCenterOfMassTransform(), m_rbB.getCenterOfMassTransform(), m_rbA.getInvInertiaTensorWorld(), m_rbB.getInvInertiaTensorWorld());
}
}
-
-
-void btConeTwistConstraint::solveConstraintObsolete(btSolverBody& bodyA,btSolverBody& bodyB,btScalar timeStep)
+void btConeTwistConstraint::solveConstraintObsolete(btSolverBody& bodyA, btSolverBody& bodyB, btScalar timeStep)
{
- #ifndef __SPU__
+#ifndef __SPU__
if (m_useSolveConstraintObsolete)
{
- btVector3 pivotAInW = m_rbA.getCenterOfMassTransform()*m_rbAFrame.getOrigin();
- btVector3 pivotBInW = m_rbB.getCenterOfMassTransform()*m_rbBFrame.getOrigin();
+ btVector3 pivotAInW = m_rbA.getCenterOfMassTransform() * m_rbAFrame.getOrigin();
+ btVector3 pivotBInW = m_rbB.getCenterOfMassTransform() * m_rbBFrame.getOrigin();
btScalar tau = btScalar(0.3);
//linear part
if (!m_angularOnly)
{
- btVector3 rel_pos1 = pivotAInW - m_rbA.getCenterOfMassPosition();
+ btVector3 rel_pos1 = pivotAInW - m_rbA.getCenterOfMassPosition();
btVector3 rel_pos2 = pivotBInW - m_rbB.getCenterOfMassPosition();
btVector3 vel1;
- bodyA.internalGetVelocityInLocalPointObsolete(rel_pos1,vel1);
+ bodyA.internalGetVelocityInLocalPointObsolete(rel_pos1, vel1);
btVector3 vel2;
- bodyB.internalGetVelocityInLocalPointObsolete(rel_pos2,vel2);
+ bodyB.internalGetVelocityInLocalPointObsolete(rel_pos2, vel2);
btVector3 vel = vel1 - vel2;
- for (int i=0;i<3;i++)
- {
+ for (int i = 0; i < 3; i++)
+ {
const btVector3& normal = m_jac[i].m_linearJointAxis;
btScalar jacDiagABInv = btScalar(1.) / m_jac[i].getDiagonal();
btScalar rel_vel;
rel_vel = normal.dot(vel);
//positional error (zeroth order error)
- btScalar depth = -(pivotAInW - pivotBInW).dot(normal); //this is the error projected on the normal
- btScalar impulse = depth*tau/timeStep * jacDiagABInv - rel_vel * jacDiagABInv;
+ btScalar depth = -(pivotAInW - pivotBInW).dot(normal); //this is the error projected on the normal
+ btScalar impulse = depth * tau / timeStep * jacDiagABInv - rel_vel * jacDiagABInv;
m_appliedImpulse += impulse;
-
+
btVector3 ftorqueAxis1 = rel_pos1.cross(normal);
btVector3 ftorqueAxis2 = rel_pos2.cross(normal);
- bodyA.internalApplyImpulse(normal*m_rbA.getInvMass(), m_rbA.getInvInertiaTensorWorld()*ftorqueAxis1,impulse);
- bodyB.internalApplyImpulse(normal*m_rbB.getInvMass(), m_rbB.getInvInertiaTensorWorld()*ftorqueAxis2,-impulse);
-
+ bodyA.internalApplyImpulse(normal * m_rbA.getInvMass(), m_rbA.getInvInertiaTensorWorld() * ftorqueAxis1, impulse);
+ bodyB.internalApplyImpulse(normal * m_rbB.getInvMass(), m_rbB.getInvInertiaTensorWorld() * ftorqueAxis2, -impulse);
}
}
@@ -356,13 +335,17 @@ void btConeTwistConstraint::solveConstraintObsolete(btSolverBody& bodyA,btSolver
// compute current and predicted transforms
btTransform trACur = m_rbA.getCenterOfMassTransform();
btTransform trBCur = m_rbB.getCenterOfMassTransform();
- btVector3 omegaA; bodyA.internalGetAngularVelocity(omegaA);
- btVector3 omegaB; bodyB.internalGetAngularVelocity(omegaB);
- btTransform trAPred; trAPred.setIdentity();
- btVector3 zerovec(0,0,0);
+ btVector3 omegaA;
+ bodyA.internalGetAngularVelocity(omegaA);
+ btVector3 omegaB;
+ bodyB.internalGetAngularVelocity(omegaB);
+ btTransform trAPred;
+ trAPred.setIdentity();
+ btVector3 zerovec(0, 0, 0);
btTransformUtil::integrateTransform(
trACur, zerovec, omegaA, timeStep, trAPred);
- btTransform trBPred; trBPred.setIdentity();
+ btTransform trBPred;
+ trBPred.setIdentity();
btTransformUtil::integrateTransform(
trBCur, zerovec, omegaB, timeStep, trBPred);
@@ -374,7 +357,7 @@ void btConeTwistConstraint::solveConstraintObsolete(btSolverBody& bodyA,btSolver
// compute desired omegas in world
btVector3 omegaADes, omegaBDes;
-
+
btTransformUtil::calculateVelocity(trACur, trADes, timeStep, zerovec, omegaADes);
btTransformUtil::calculateVelocity(trBCur, trBDes, timeStep, zerovec, omegaBDes);
@@ -415,10 +398,10 @@ void btConeTwistConstraint::solveConstraintObsolete(btSolverBody& bodyA,btSolver
{
btScalar fMaxImpulse = m_maxMotorImpulse;
if (m_bNormalizedMotorStrength)
- fMaxImpulse = fMaxImpulse/kAxisAInv;
+ fMaxImpulse = fMaxImpulse / kAxisAInv;
btVector3 newUnclampedAccImpulse = m_accMotorImpulse + impulse;
- btScalar newUnclampedMag = newUnclampedAccImpulse.length();
+ btScalar newUnclampedMag = newUnclampedAccImpulse.length();
if (newUnclampedMag > fMaxImpulse)
{
newUnclampedAccImpulse.normalize();
@@ -428,31 +411,32 @@ void btConeTwistConstraint::solveConstraintObsolete(btSolverBody& bodyA,btSolver
m_accMotorImpulse += impulse;
}
- btScalar impulseMag = impulse.length();
- btVector3 impulseAxis = impulse / impulseMag;
-
- bodyA.internalApplyImpulse(btVector3(0,0,0), m_rbA.getInvInertiaTensorWorld()*impulseAxis, impulseMag);
- bodyB.internalApplyImpulse(btVector3(0,0,0), m_rbB.getInvInertiaTensorWorld()*impulseAxis, -impulseMag);
+ btScalar impulseMag = impulse.length();
+ btVector3 impulseAxis = impulse / impulseMag;
+ bodyA.internalApplyImpulse(btVector3(0, 0, 0), m_rbA.getInvInertiaTensorWorld() * impulseAxis, impulseMag);
+ bodyB.internalApplyImpulse(btVector3(0, 0, 0), m_rbB.getInvInertiaTensorWorld() * impulseAxis, -impulseMag);
}
}
- else if (m_damping > SIMD_EPSILON) // no motor: do a little damping
+ else if (m_damping > SIMD_EPSILON) // no motor: do a little damping
{
- btVector3 angVelA; bodyA.internalGetAngularVelocity(angVelA);
- btVector3 angVelB; bodyB.internalGetAngularVelocity(angVelB);
+ btVector3 angVelA;
+ bodyA.internalGetAngularVelocity(angVelA);
+ btVector3 angVelB;
+ bodyB.internalGetAngularVelocity(angVelB);
btVector3 relVel = angVelB - angVelA;
if (relVel.length2() > SIMD_EPSILON)
{
btVector3 relVelAxis = relVel.normalized();
- btScalar m_kDamping = btScalar(1.) /
- (getRigidBodyA().computeAngularImpulseDenominator(relVelAxis) +
- getRigidBodyB().computeAngularImpulseDenominator(relVelAxis));
+ btScalar m_kDamping = btScalar(1.) /
+ (getRigidBodyA().computeAngularImpulseDenominator(relVelAxis) +
+ getRigidBodyB().computeAngularImpulseDenominator(relVelAxis));
btVector3 impulse = m_damping * m_kDamping * relVel;
- btScalar impulseMag = impulse.length();
+ btScalar impulseMag = impulse.length();
btVector3 impulseAxis = impulse / impulseMag;
- bodyA.internalApplyImpulse(btVector3(0,0,0), m_rbA.getInvInertiaTensorWorld()*impulseAxis, impulseMag);
- bodyB.internalApplyImpulse(btVector3(0,0,0), m_rbB.getInvInertiaTensorWorld()*impulseAxis, -impulseMag);
+ bodyA.internalApplyImpulse(btVector3(0, 0, 0), m_rbA.getInvInertiaTensorWorld() * impulseAxis, impulseMag);
+ bodyB.internalApplyImpulse(btVector3(0, 0, 0), m_rbB.getInvInertiaTensorWorld() * impulseAxis, -impulseMag);
}
}
@@ -467,7 +451,7 @@ void btConeTwistConstraint::solveConstraintObsolete(btSolverBody& bodyA,btSolver
// solve swing limit
if (m_solveSwingLimit)
{
- btScalar amplitude = m_swingLimitRatio * m_swingCorrection*m_biasFactor/timeStep;
+ 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;
@@ -475,7 +459,7 @@ void btConeTwistConstraint::solveConstraintObsolete(btSolverBody& bodyA,btSolver
// Clamp the accumulated impulse
btScalar temp = m_accSwingLimitImpulse;
- m_accSwingLimitImpulse = btMax(m_accSwingLimitImpulse + impulseMag, btScalar(0.0) );
+ m_accSwingLimitImpulse = btMax(m_accSwingLimitImpulse + impulseMag, btScalar(0.0));
impulseMag = m_accSwingLimitImpulse - temp;
btVector3 impulse = m_swingAxis * impulseMag;
@@ -491,47 +475,41 @@ void btConeTwistConstraint::solveConstraintObsolete(btSolverBody& bodyA,btSolver
impulseMag = impulse.length();
btVector3 noTwistSwingAxis = impulse / impulseMag;
- bodyA.internalApplyImpulse(btVector3(0,0,0), m_rbA.getInvInertiaTensorWorld()*noTwistSwingAxis, impulseMag);
- bodyB.internalApplyImpulse(btVector3(0,0,0), m_rbB.getInvInertiaTensorWorld()*noTwistSwingAxis, -impulseMag);
+ bodyA.internalApplyImpulse(btVector3(0, 0, 0), m_rbA.getInvInertiaTensorWorld() * noTwistSwingAxis, impulseMag);
+ bodyB.internalApplyImpulse(btVector3(0, 0, 0), m_rbB.getInvInertiaTensorWorld() * noTwistSwingAxis, -impulseMag);
}
-
// solve twist limit
if (m_solveTwistLimit)
{
- 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)
+ 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
btScalar temp = m_accTwistLimitImpulse;
- m_accTwistLimitImpulse = btMax(m_accTwistLimitImpulse + impulseMag, btScalar(0.0) );
+ m_accTwistLimitImpulse = btMax(m_accTwistLimitImpulse + impulseMag, btScalar(0.0));
impulseMag = m_accTwistLimitImpulse - temp;
- // btVector3 impulse = m_twistAxis * impulseMag;
+ // btVector3 impulse = m_twistAxis * impulseMag;
- bodyA.internalApplyImpulse(btVector3(0,0,0), m_rbA.getInvInertiaTensorWorld()*m_twistAxis,impulseMag);
- bodyB.internalApplyImpulse(btVector3(0,0,0), m_rbB.getInvInertiaTensorWorld()*m_twistAxis,-impulseMag);
- }
+ bodyA.internalApplyImpulse(btVector3(0, 0, 0), m_rbA.getInvInertiaTensorWorld() * m_twistAxis, impulseMag);
+ bodyB.internalApplyImpulse(btVector3(0, 0, 0), m_rbB.getInvInertiaTensorWorld() * m_twistAxis, -impulseMag);
+ }
}
}
#else
-btAssert(0);
-#endif //__SPU__
+ btAssert(0);
+#endif //__SPU__
}
-
-
-
-void btConeTwistConstraint::updateRHS(btScalar timeStep)
+void btConeTwistConstraint::updateRHS(btScalar timeStep)
{
(void)timeStep;
-
}
-
#ifndef __SPU__
void btConeTwistConstraint::calcAngleInfo()
{
@@ -540,15 +518,15 @@ void btConeTwistConstraint::calcAngleInfo()
m_solveTwistLimit = false;
m_solveSwingLimit = false;
- btVector3 b1Axis1(0,0,0),b1Axis2(0,0,0),b1Axis3(0,0,0);
- btVector3 b2Axis1(0,0,0),b2Axis2(0,0,0);
+ btVector3 b1Axis1(0, 0, 0), b1Axis2(0, 0, 0), b1Axis3(0, 0, 0);
+ btVector3 b2Axis1(0, 0, 0), b2Axis2(0, 0, 0);
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 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 fact;
@@ -558,33 +536,33 @@ void btConeTwistConstraint::calcAngleInfo()
b1Axis2 = getRigidBodyA().getCenterOfMassTransform().getBasis() * this->m_rbAFrame.getBasis().getColumn(1);
swx = b2Axis1.dot(b1Axis1);
swy = b2Axis1.dot(b1Axis2);
- swing1 = btAtan2Fast(swy, swx);
- fact = (swy*swy + swx*swx) * thresh * thresh;
+ swing1 = btAtan2Fast(swy, swx);
+ fact = (swy * swy + swx * swx) * thresh * thresh;
fact = fact / (fact + btScalar(1.0));
- swing1 *= fact;
+ swing1 *= fact;
}
if (m_swingSpan2 >= btScalar(0.05f))
{
- b1Axis3 = getRigidBodyA().getCenterOfMassTransform().getBasis() * this->m_rbAFrame.getBasis().getColumn(2);
+ b1Axis3 = getRigidBodyA().getCenterOfMassTransform().getBasis() * this->m_rbAFrame.getBasis().getColumn(2);
swx = b2Axis1.dot(b1Axis1);
swy = b2Axis1.dot(b1Axis3);
- swing2 = btAtan2Fast(swy, swx);
- fact = (swy*swy + swx*swx) * thresh * thresh;
+ swing2 = btAtan2Fast(swy, swx);
+ fact = (swy * swy + swx * swx) * thresh * thresh;
fact = fact / (fact + btScalar(1.0));
- swing2 *= fact;
+ 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;
+ 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_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 = 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;
@@ -594,14 +572,14 @@ void btConeTwistConstraint::calcAngleInfo()
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) );
+ 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.);
+ // btScalar lockedFreeFactor = (m_twistSpan > btScalar(0.05f)) ? m_limitSoftness : btScalar(0.);
btScalar lockedFreeFactor = (m_twistSpan > btScalar(0.05f)) ? btScalar(1.0f) : btScalar(0.);
- if (twist <= -m_twistSpan*lockedFreeFactor)
+ if (twist <= -m_twistSpan * lockedFreeFactor)
{
m_twistCorrection = -(twist + m_twistSpan);
m_solveTwistLimit = true;
@@ -609,7 +587,7 @@ void btConeTwistConstraint::calcAngleInfo()
m_twistAxis.normalize();
m_twistAxis *= -1.0f;
}
- else if (twist > m_twistSpan*lockedFreeFactor)
+ else if (twist > m_twistSpan * lockedFreeFactor)
{
m_twistCorrection = (twist - m_twistSpan);
m_solveTwistLimit = true;
@@ -618,13 +596,11 @@ void btConeTwistConstraint::calcAngleInfo()
}
}
}
-#endif //__SPU__
-
-static btVector3 vTwist(1,0,0); // twist axis in constraint's space
+#endif //__SPU__
+static btVector3 vTwist(1, 0, 0); // twist axis in constraint's space
-
-void btConeTwistConstraint::calcAngleInfo2(const btTransform& transA, const btTransform& transB, const btMatrix3x3& invInertiaWorldA,const btMatrix3x3& invInertiaWorldB)
+void btConeTwistConstraint::calcAngleInfo2(const btTransform& transA, const btTransform& transB, const btMatrix3x3& invInertiaWorldA, const btMatrix3x3& invInertiaWorldB)
{
m_swingCorrection = btScalar(0.);
m_twistLimitSign = btScalar(0.);
@@ -632,7 +608,7 @@ void btConeTwistConstraint::calcAngleInfo2(const btTransform& transA, const btTr
m_solveSwingLimit = false;
// compute rotation of A wrt B (in constraint space)
if (m_bMotorEnabled && (!m_useSolveConstraintObsolete))
- { // it is assumed that setMotorTarget() was alredy called
+ { // it is assumed that setMotorTarget() was alredy called
// and motor target m_qTarget is within constraint limits
// TODO : split rotation to pure swing and pure twist
// compute desired transforms in world
@@ -641,23 +617,22 @@ void btConeTwistConstraint::calcAngleInfo2(const btTransform& transA, const btTr
btTransform trB = transB * m_rbBFrame;
btTransform trDeltaAB = trB * trPose * trA.inverse();
btQuaternion qDeltaAB = trDeltaAB.getRotation();
- btVector3 swingAxis = btVector3(qDeltaAB.x(), qDeltaAB.y(), qDeltaAB.z());
+ btVector3 swingAxis = btVector3(qDeltaAB.x(), qDeltaAB.y(), qDeltaAB.z());
btScalar swingAxisLen2 = swingAxis.length2();
- if(btFuzzyZero(swingAxisLen2))
+ if (btFuzzyZero(swingAxisLen2))
{
- return;
+ return;
}
m_swingAxis = swingAxis;
m_swingAxis.normalize();
m_swingCorrection = qDeltaAB.getAngle();
- if(!btFuzzyZero(m_swingCorrection))
+ if (!btFuzzyZero(m_swingCorrection))
{
m_solveSwingLimit = true;
}
return;
}
-
{
// compute rotation of A wrt B (in constraint space)
btQuaternion qA = transA.getRotation() * m_rbAFrame.getRotation();
@@ -665,13 +640,17 @@ void btConeTwistConstraint::calcAngleInfo2(const btTransform& transA, const btTr
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();
+ btVector3 vConeNoTwist = quatRotate(qAB, vTwist);
+ vConeNoTwist.normalize();
+ btQuaternion qABCone = shortestArcQuat(vTwist, vConeNoTwist);
+ qABCone.normalize();
+ btQuaternion qABTwist = qABCone.inverse() * qAB;
+ qABTwist.normalize();
if (m_swingSpan1 >= m_fixThresh && m_swingSpan2 >= m_fixThresh)
{
- btScalar swingAngle, swingLimit = 0; btVector3 swingAxis;
+ btScalar swingAngle, swingLimit = 0;
+ btVector3 swingAxis;
computeConeLimitInfo(qABCone, swingAngle, swingAxis, swingLimit);
if (swingAngle > swingLimit * m_limitSoftness)
@@ -684,9 +663,9 @@ void btConeTwistConstraint::calcAngleInfo2(const btTransform& transA, const btTr
m_swingLimitRatio = 1.f;
if (swingAngle < swingLimit && m_limitSoftness < 1.f - SIMD_EPSILON)
{
- m_swingLimitRatio = (swingAngle - swingLimit * m_limitSoftness)/
+ 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);
@@ -694,14 +673,14 @@ void btConeTwistConstraint::calcAngleInfo2(const btTransform& transA, const btTr
// adjustment of swing axis (based on ellipse normal)
adjustSwingAxisToUseEllipseNormal(swingAxis);
- // Calculate necessary axis & factors
+ // Calculate necessary axis & factors
m_swingAxis = quatRotate(qB, -swingAxis);
- m_twistAxisA.setValue(0,0,0);
+ m_twistAxisA.setValue(0, 0, 0);
- m_kSwing = btScalar(1.) /
- (computeAngularImpulseDenominator(m_swingAxis,invInertiaWorldA) +
- computeAngularImpulseDenominator(m_swingAxis,invInertiaWorldB));
+ m_kSwing = btScalar(1.) /
+ (computeAngularImpulseDenominator(m_swingAxis, invInertiaWorldA) +
+ computeAngularImpulseDenominator(m_swingAxis, invInertiaWorldB));
}
}
else
@@ -717,9 +696,9 @@ void btConeTwistConstraint::calcAngleInfo2(const btTransform& transA, const btTr
btScalar x = ivB.dot(ivA);
btScalar y = ivB.dot(jvA);
btScalar z = ivB.dot(kvA);
- if((m_swingSpan1 < m_fixThresh) && (m_swingSpan2 < m_fixThresh))
- { // fixed. We'll need to add one more row to constraint
- if((!btFuzzyZero(y)) || (!(btFuzzyZero(z))))
+ if ((m_swingSpan1 < m_fixThresh) && (m_swingSpan2 < m_fixThresh))
+ { // fixed. We'll need to add one more row to constraint
+ if ((!btFuzzyZero(y)) || (!(btFuzzyZero(z))))
{
m_solveSwingLimit = true;
m_swingAxis = -ivB.cross(ivA);
@@ -727,47 +706,47 @@ void btConeTwistConstraint::calcAngleInfo2(const btTransform& transA, const btTr
}
else
{
- if(m_swingSpan1 < m_fixThresh)
- { // hinge around Y axis
-// if(!(btFuzzyZero(y)))
- if((!(btFuzzyZero(x))) || (!(btFuzzyZero(z))))
+ if (m_swingSpan1 < m_fixThresh)
+ { // hinge around Y axis
+ // if(!(btFuzzyZero(y)))
+ if ((!(btFuzzyZero(x))) || (!(btFuzzyZero(z))))
{
m_solveSwingLimit = true;
- if(m_swingSpan2 >= m_fixThresh)
+ if (m_swingSpan2 >= m_fixThresh)
{
y = btScalar(0.f);
btScalar span2 = btAtan2(z, x);
- if(span2 > m_swingSpan2)
+ if (span2 > m_swingSpan2)
{
x = btCos(m_swingSpan2);
z = btSin(m_swingSpan2);
}
- else if(span2 < -m_swingSpan2)
+ else if (span2 < -m_swingSpan2)
{
- x = btCos(m_swingSpan2);
+ x = btCos(m_swingSpan2);
z = -btSin(m_swingSpan2);
}
}
}
}
else
- { // hinge around Z axis
-// if(!btFuzzyZero(z))
- if((!(btFuzzyZero(x))) || (!(btFuzzyZero(y))))
+ { // hinge around Z axis
+ // if(!btFuzzyZero(z))
+ if ((!(btFuzzyZero(x))) || (!(btFuzzyZero(y))))
{
m_solveSwingLimit = true;
- if(m_swingSpan1 >= m_fixThresh)
+ if (m_swingSpan1 >= m_fixThresh)
{
z = btScalar(0.f);
btScalar span1 = btAtan2(y, x);
- if(span1 > m_swingSpan1)
+ if (span1 > m_swingSpan1)
{
x = btCos(m_swingSpan1);
y = btSin(m_swingSpan1);
}
- else if(span1 < -m_swingSpan1)
+ else if (span1 < -m_swingSpan1)
{
- x = btCos(m_swingSpan1);
+ x = btCos(m_swingSpan1);
y = -btSin(m_swingSpan1);
}
}
@@ -778,10 +757,10 @@ void btConeTwistConstraint::calcAngleInfo2(const btTransform& transA, const btTr
target[2] = x * ivA[2] + y * jvA[2] + z * kvA[2];
target.normalize();
m_swingAxis = -ivB.cross(target);
- m_swingCorrection = m_swingAxis.length();
+ m_swingCorrection = m_swingAxis.length();
- if (!btFuzzyZero(m_swingCorrection))
- m_swingAxis.normalize();
+ if (!btFuzzyZero(m_swingCorrection))
+ m_swingAxis.normalize();
}
}
@@ -790,15 +769,15 @@ void btConeTwistConstraint::calcAngleInfo2(const btTransform& transA, const btTr
btVector3 twistAxis;
computeTwistLimitInfo(qABTwist, m_twistAngle, twistAxis);
- if (m_twistAngle > m_twistSpan*m_limitSoftness)
+ 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);
+ m_twistLimitRatio = (m_twistAngle - m_twistSpan * m_limitSoftness) /
+ (m_twistSpan - m_twistSpan * m_limitSoftness);
}
// twist correction tries to get back to soft limit
@@ -807,8 +786,8 @@ void btConeTwistConstraint::calcAngleInfo2(const btTransform& transA, const btTr
m_twistAxis = quatRotate(qB, -twistAxis);
m_kTwist = btScalar(1.) /
- (computeAngularImpulseDenominator(m_twistAxis,invInertiaWorldA) +
- computeAngularImpulseDenominator(m_twistAxis,invInertiaWorldB));
+ (computeAngularImpulseDenominator(m_twistAxis, invInertiaWorldA) +
+ computeAngularImpulseDenominator(m_twistAxis, invInertiaWorldB));
}
if (m_solveSwingLimit)
@@ -821,15 +800,13 @@ void btConeTwistConstraint::calcAngleInfo2(const btTransform& transA, const btTr
}
}
-
-
// 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
+ btScalar& swingAngle, // out
+ btVector3& vSwingAxis, // out
+ btScalar& swingLimit) // out
{
swingAngle = qCone.getAngle();
if (swingAngle > SIMD_EPSILON)
@@ -840,7 +817,7 @@ void btConeTwistConstraint::computeConeLimitInfo(const btQuaternion& qCone,
// non-zero twist?! this should never happen.
btAssert(fabs(vSwingAxis.x()) <= SIMD_EPSILON));
#endif
-
+
// 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.)
@@ -848,7 +825,7 @@ void btConeTwistConstraint::computeConeLimitInfo(const btQuaternion& qCone,
// 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 xEllipse = vSwingAxis.y();
btScalar yEllipse = -vSwingAxis.z();
// Now, we use the slope of the vector (using x/yEllipse) and find the length
@@ -858,10 +835,10 @@ void btConeTwistConstraint::computeConeLimitInfo(const btQuaternion& qCone,
// 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
+ 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 surfaceSlope2 = (yEllipse * yEllipse) / (xEllipse * xEllipse);
btScalar norm = 1 / (m_swingSpan2 * m_swingSpan2);
norm += surfaceSlope2 / (m_swingSpan1 * m_swingSpan1);
btScalar swingLimit2 = (1 + surfaceSlope2) / norm;
@@ -887,7 +864,7 @@ void btConeTwistConstraint::computeConeLimitInfo(const btQuaternion& qCone,
#if 0
btAssert(0);
#endif
- }
+ }
}
btVector3 btConeTwistConstraint::GetPointForAngle(btScalar fAngleInRadians, btScalar fLength) const
@@ -903,10 +880,10 @@ btVector3 btConeTwistConstraint::GetPointForAngle(btScalar fAngleInRadians, btSc
// a^2 b^2
// Do the math and it should be clear.
- btScalar swingLimit = m_swingSpan1; // if xEllipse == 0, just use axis b (1)
+ btScalar swingLimit = m_swingSpan1; // if xEllipse == 0, just use axis b (1)
if (fabs(xEllipse) > SIMD_EPSILON)
{
- btScalar surfaceSlope2 = (yEllipse*yEllipse)/(xEllipse*xEllipse);
+ btScalar surfaceSlope2 = (yEllipse * yEllipse) / (xEllipse * xEllipse);
btScalar norm = 1 / (m_swingSpan2 * m_swingSpan2);
norm += surfaceSlope2 / (m_swingSpan1 * m_swingSpan1);
btScalar swingLimit2 = (1 + surfaceSlope2) / norm;
@@ -917,20 +894,20 @@ btVector3 btConeTwistConstraint::GetPointForAngle(btScalar fAngleInRadians, btSc
// 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);
+ 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
+ btScalar& twistAngle, // out
+ btVector3& vTwistAxis) // out
{
btQuaternion qMinTwist = qTwist;
twistAngle = qTwist.getAngle();
- if (twistAngle > SIMD_PI) // long way around. flip quat and recalculate.
+ if (twistAngle > SIMD_PI) // long way around. flip quat and recalculate.
{
qMinTwist = -(qTwist);
twistAngle = qMinTwist.getAngle();
@@ -948,80 +925,79 @@ void btConeTwistConstraint::computeTwistLimitInfo(const btQuaternion& qTwist,
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
+ // 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();
+ 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.
+ 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;
+ 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);
+ y = fabs(grad * z);
else
y = -fabs(grad * z);
// convert ellipse direction back to swing axis
vSwingAxis.setZ(-y);
- vSwingAxis.setY( z);
+ vSwingAxis.setY(z);
vSwingAxis.normalize();
}
}
-
-
-void btConeTwistConstraint::setMotorTarget(const btQuaternion &q)
+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);
+ // 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)
+void btConeTwistConstraint::setMotorTargetInConstraintSpace(const btQuaternion& q)
{
m_qTarget = q;
// clamp motor target to within limits
{
- btScalar softness = 1.f;//m_limitSoftness;
+ 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();
+ 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;
+ 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;
+ if (swingAngle > swingLimit * softness)
+ swingAngle = swingLimit * softness;
+ else if (swingAngle < -swingLimit * softness)
+ swingAngle = -swingLimit * softness;
qTargetCone = btQuaternion(swingAxis, swingAngle);
}
}
@@ -1029,16 +1005,17 @@ void btConeTwistConstraint::setMotorTargetInConstraintSpace(const btQuaternion &
// clamp twist
if (m_twistSpan >= btScalar(0.05f))
{
- btScalar twistAngle; btVector3 twistAxis;
+ 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;
+ if (twistAngle > m_twistSpan * softness)
+ twistAngle = m_twistSpan * softness;
+ else if (twistAngle < -m_twistSpan * softness)
+ twistAngle = -m_twistSpan * softness;
qTargetTwist = btQuaternion(twistAxis, twistAngle);
}
}
@@ -1047,15 +1024,15 @@ void btConeTwistConstraint::setMotorTargetInConstraintSpace(const btQuaternion &
}
}
-///override the default global value of a parameter (such as ERP or CFM), optionally provide the axis (0..5).
+///override the default global value of a parameter (such as ERP or CFM), optionally provide the axis (0..5).
///If no axis is provided, it uses the default axis for this constraint.
void btConeTwistConstraint::setParam(int num, btScalar value, int axis)
{
- switch(num)
+ switch (num)
{
- case BT_CONSTRAINT_ERP :
- case BT_CONSTRAINT_STOP_ERP :
- if((axis >= 0) && (axis < 3))
+ case BT_CONSTRAINT_ERP:
+ case BT_CONSTRAINT_STOP_ERP:
+ if ((axis >= 0) && (axis < 3))
{
m_linERP = value;
m_flags |= BT_CONETWIST_FLAGS_LIN_ERP;
@@ -1065,9 +1042,9 @@ void btConeTwistConstraint::setParam(int num, btScalar value, int axis)
m_biasFactor = value;
}
break;
- case BT_CONSTRAINT_CFM :
- case BT_CONSTRAINT_STOP_CFM :
- if((axis >= 0) && (axis < 3))
+ case BT_CONSTRAINT_CFM:
+ case BT_CONSTRAINT_STOP_CFM:
+ if ((axis >= 0) && (axis < 3))
{
m_linCFM = value;
m_flags |= BT_CONETWIST_FLAGS_LIN_CFM;
@@ -1085,19 +1062,19 @@ void btConeTwistConstraint::setParam(int num, btScalar value, int axis)
}
///return the local value of parameter
-btScalar btConeTwistConstraint::getParam(int num, int axis) const
+btScalar btConeTwistConstraint::getParam(int num, int axis) const
{
btScalar retVal = 0;
- switch(num)
+ switch (num)
{
- case BT_CONSTRAINT_ERP :
- case BT_CONSTRAINT_STOP_ERP :
- if((axis >= 0) && (axis < 3))
+ case BT_CONSTRAINT_ERP:
+ case BT_CONSTRAINT_STOP_ERP:
+ if ((axis >= 0) && (axis < 3))
{
btAssertConstrParams(m_flags & BT_CONETWIST_FLAGS_LIN_ERP);
retVal = m_linERP;
}
- else if((axis >= 3) && (axis < 6))
+ else if ((axis >= 3) && (axis < 6))
{
retVal = m_biasFactor;
}
@@ -1106,14 +1083,14 @@ btScalar btConeTwistConstraint::getParam(int num, int axis) const
btAssertConstrParams(0);
}
break;
- case BT_CONSTRAINT_CFM :
- case BT_CONSTRAINT_STOP_CFM :
- if((axis >= 0) && (axis < 3))
+ case BT_CONSTRAINT_CFM:
+ case BT_CONSTRAINT_STOP_CFM:
+ if ((axis >= 0) && (axis < 3))
{
btAssertConstrParams(m_flags & BT_CONETWIST_FLAGS_LIN_CFM);
retVal = m_linCFM;
}
- else if((axis >= 3) && (axis < 6))
+ else if ((axis >= 3) && (axis < 6))
{
btAssertConstrParams(m_flags & BT_CONETWIST_FLAGS_ANG_CFM);
retVal = m_angCFM;
@@ -1123,21 +1100,16 @@ btScalar btConeTwistConstraint::getParam(int num, int axis) const
btAssertConstrParams(0);
}
break;
- default :
+ default:
btAssertConstrParams(0);
}
return retVal;
}
-
-void btConeTwistConstraint::setFrames(const btTransform & frameA, const btTransform & frameB)
+void btConeTwistConstraint::setFrames(const btTransform& frameA, const btTransform& frameB)
{
m_rbAFrame = frameA;
m_rbBFrame = frameB;
buildJacobian();
//calculateTransforms();
}
-
-
-
-
generated by cgit v1.2.3 (git 2.25.1) at 2024-12-17 01:17:00 +0000