Brecht Van Lommel
c8b4cf9206
svn merge https://svn.blender.org/svnroot/bf-blender/trunk/blender -r19820:HEAD Notes: * Game and sequencer RNA, and sequencer header are now out of date a bit after changes in trunk. * I didn't know how to port these bugfixes, most likely they are not needed anymore. * Fix "duplicate strip" always increase the user count for ipo. * IPO pinning on sequencer strips was lost during Undo.
972 lines
26 KiB
C++
972 lines
26 KiB
C++
/*
|
|
Bullet Continuous Collision Detection and Physics Library
|
|
Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/
|
|
|
|
This software is provided 'as-is', without any express or implied warranty.
|
|
In no event will the authors be held liable for any damages arising from the use of this software.
|
|
Permission is granted to anyone to use this software for any purpose,
|
|
including commercial applications, and to alter it and redistribute it freely,
|
|
subject to the following restrictions:
|
|
|
|
1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
|
|
2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
|
|
3. This notice may not be removed or altered from any source distribution.
|
|
*/
|
|
/*
|
|
2007-09-09
|
|
Refactored by Francisco Le?n
|
|
email: projectileman@yahoo.com
|
|
http://gimpact.sf.net
|
|
*/
|
|
|
|
#include "btGeneric6DofConstraint.h"
|
|
#include "BulletDynamics/Dynamics/btRigidBody.h"
|
|
#include "LinearMath/btTransformUtil.h"
|
|
#include "LinearMath/btTransformUtil.h"
|
|
#include <new>
|
|
|
|
|
|
|
|
#define D6_USE_OBSOLETE_METHOD false
|
|
|
|
|
|
btGeneric6DofConstraint::btGeneric6DofConstraint()
|
|
:btTypedConstraint(D6_CONSTRAINT_TYPE),
|
|
m_useLinearReferenceFrameA(true),
|
|
m_useSolveConstraintObsolete(D6_USE_OBSOLETE_METHOD)
|
|
{
|
|
}
|
|
|
|
|
|
|
|
btGeneric6DofConstraint::btGeneric6DofConstraint(btRigidBody& rbA, btRigidBody& rbB, const btTransform& frameInA, const btTransform& frameInB, bool useLinearReferenceFrameA)
|
|
: btTypedConstraint(D6_CONSTRAINT_TYPE, rbA, rbB)
|
|
, m_frameInA(frameInA)
|
|
, m_frameInB(frameInB),
|
|
m_useLinearReferenceFrameA(useLinearReferenceFrameA),
|
|
m_useSolveConstraintObsolete(D6_USE_OBSOLETE_METHOD)
|
|
{
|
|
|
|
}
|
|
|
|
|
|
|
|
#define GENERIC_D6_DISABLE_WARMSTARTING 1
|
|
|
|
|
|
|
|
btScalar btGetMatrixElem(const btMatrix3x3& mat, int index);
|
|
btScalar btGetMatrixElem(const btMatrix3x3& mat, int index)
|
|
{
|
|
int i = index%3;
|
|
int j = index/3;
|
|
return mat[i][j];
|
|
}
|
|
|
|
|
|
|
|
///MatrixToEulerXYZ from http://www.geometrictools.com/LibFoundation/Mathematics/Wm4Matrix3.inl.html
|
|
bool matrixToEulerXYZ(const btMatrix3x3& mat,btVector3& xyz);
|
|
bool matrixToEulerXYZ(const btMatrix3x3& mat,btVector3& xyz)
|
|
{
|
|
// // rot = cy*cz -cy*sz sy
|
|
// // cz*sx*sy+cx*sz cx*cz-sx*sy*sz -cy*sx
|
|
// // -cx*cz*sy+sx*sz cz*sx+cx*sy*sz cx*cy
|
|
//
|
|
|
|
btScalar fi = btGetMatrixElem(mat,2);
|
|
if (fi < btScalar(1.0f))
|
|
{
|
|
if (fi > btScalar(-1.0f))
|
|
{
|
|
xyz[0] = btAtan2(-btGetMatrixElem(mat,5),btGetMatrixElem(mat,8));
|
|
xyz[1] = btAsin(btGetMatrixElem(mat,2));
|
|
xyz[2] = btAtan2(-btGetMatrixElem(mat,1),btGetMatrixElem(mat,0));
|
|
return true;
|
|
}
|
|
else
|
|
{
|
|
// WARNING. Not unique. XA - ZA = -atan2(r10,r11)
|
|
xyz[0] = -btAtan2(btGetMatrixElem(mat,3),btGetMatrixElem(mat,4));
|
|
xyz[1] = -SIMD_HALF_PI;
|
|
xyz[2] = btScalar(0.0);
|
|
return false;
|
|
}
|
|
}
|
|
else
|
|
{
|
|
// WARNING. Not unique. XAngle + ZAngle = atan2(r10,r11)
|
|
xyz[0] = btAtan2(btGetMatrixElem(mat,3),btGetMatrixElem(mat,4));
|
|
xyz[1] = SIMD_HALF_PI;
|
|
xyz[2] = 0.0;
|
|
}
|
|
return false;
|
|
}
|
|
|
|
//////////////////////////// btRotationalLimitMotor ////////////////////////////////////
|
|
|
|
int btRotationalLimitMotor::testLimitValue(btScalar test_value)
|
|
{
|
|
if(m_loLimit>m_hiLimit)
|
|
{
|
|
m_currentLimit = 0;//Free from violation
|
|
return 0;
|
|
}
|
|
|
|
if (test_value < m_loLimit)
|
|
{
|
|
m_currentLimit = 1;//low limit violation
|
|
m_currentLimitError = test_value - m_loLimit;
|
|
return 1;
|
|
}
|
|
else if (test_value> m_hiLimit)
|
|
{
|
|
m_currentLimit = 2;//High limit violation
|
|
m_currentLimitError = test_value - m_hiLimit;
|
|
return 2;
|
|
};
|
|
|
|
m_currentLimit = 0;//Free from violation
|
|
return 0;
|
|
|
|
}
|
|
|
|
|
|
|
|
btScalar btRotationalLimitMotor::solveAngularLimits(
|
|
btScalar timeStep,btVector3& axis,btScalar jacDiagABInv,
|
|
btRigidBody * body0, btSolverBody& bodyA, btRigidBody * body1, btSolverBody& bodyB)
|
|
{
|
|
if (needApplyTorques()==false) return 0.0f;
|
|
|
|
btScalar target_velocity = m_targetVelocity;
|
|
btScalar maxMotorForce = m_maxMotorForce;
|
|
|
|
//current error correction
|
|
if (m_currentLimit!=0)
|
|
{
|
|
target_velocity = -m_ERP*m_currentLimitError/(timeStep);
|
|
maxMotorForce = m_maxLimitForce;
|
|
}
|
|
|
|
maxMotorForce *= timeStep;
|
|
|
|
// current velocity difference
|
|
|
|
btVector3 angVelA;
|
|
bodyA.getAngularVelocity(angVelA);
|
|
btVector3 angVelB;
|
|
bodyB.getAngularVelocity(angVelB);
|
|
|
|
btVector3 vel_diff;
|
|
vel_diff = angVelA-angVelB;
|
|
|
|
|
|
|
|
btScalar rel_vel = axis.dot(vel_diff);
|
|
|
|
// correction velocity
|
|
btScalar motor_relvel = m_limitSoftness*(target_velocity - m_damping*rel_vel);
|
|
|
|
|
|
if ( motor_relvel < SIMD_EPSILON && motor_relvel > -SIMD_EPSILON )
|
|
{
|
|
return 0.0f;//no need for applying force
|
|
}
|
|
|
|
|
|
// correction impulse
|
|
btScalar unclippedMotorImpulse = (1+m_bounce)*motor_relvel*jacDiagABInv;
|
|
|
|
// clip correction impulse
|
|
btScalar clippedMotorImpulse;
|
|
|
|
///@todo: should clip against accumulated impulse
|
|
if (unclippedMotorImpulse>0.0f)
|
|
{
|
|
clippedMotorImpulse = unclippedMotorImpulse > maxMotorForce? maxMotorForce: unclippedMotorImpulse;
|
|
}
|
|
else
|
|
{
|
|
clippedMotorImpulse = unclippedMotorImpulse < -maxMotorForce ? -maxMotorForce: unclippedMotorImpulse;
|
|
}
|
|
|
|
|
|
// sort with accumulated impulses
|
|
btScalar lo = btScalar(-BT_LARGE_FLOAT);
|
|
btScalar hi = btScalar(BT_LARGE_FLOAT);
|
|
|
|
btScalar oldaccumImpulse = m_accumulatedImpulse;
|
|
btScalar sum = oldaccumImpulse + clippedMotorImpulse;
|
|
m_accumulatedImpulse = sum > hi ? btScalar(0.) : sum < lo ? btScalar(0.) : sum;
|
|
|
|
clippedMotorImpulse = m_accumulatedImpulse - oldaccumImpulse;
|
|
|
|
btVector3 motorImp = clippedMotorImpulse * axis;
|
|
|
|
//body0->applyTorqueImpulse(motorImp);
|
|
//body1->applyTorqueImpulse(-motorImp);
|
|
|
|
bodyA.applyImpulse(btVector3(0,0,0), body0->getInvInertiaTensorWorld()*axis,clippedMotorImpulse);
|
|
bodyB.applyImpulse(btVector3(0,0,0), body1->getInvInertiaTensorWorld()*axis,-clippedMotorImpulse);
|
|
|
|
|
|
return clippedMotorImpulse;
|
|
|
|
|
|
}
|
|
|
|
//////////////////////////// End btRotationalLimitMotor ////////////////////////////////////
|
|
|
|
|
|
|
|
|
|
//////////////////////////// btTranslationalLimitMotor ////////////////////////////////////
|
|
|
|
|
|
int btTranslationalLimitMotor::testLimitValue(int limitIndex, btScalar test_value)
|
|
{
|
|
btScalar loLimit = m_lowerLimit[limitIndex];
|
|
btScalar hiLimit = m_upperLimit[limitIndex];
|
|
if(loLimit > hiLimit)
|
|
{
|
|
m_currentLimit[limitIndex] = 0;//Free from violation
|
|
m_currentLimitError[limitIndex] = btScalar(0.f);
|
|
return 0;
|
|
}
|
|
|
|
if (test_value < loLimit)
|
|
{
|
|
m_currentLimit[limitIndex] = 2;//low limit violation
|
|
m_currentLimitError[limitIndex] = test_value - loLimit;
|
|
return 2;
|
|
}
|
|
else if (test_value> hiLimit)
|
|
{
|
|
m_currentLimit[limitIndex] = 1;//High limit violation
|
|
m_currentLimitError[limitIndex] = test_value - hiLimit;
|
|
return 1;
|
|
};
|
|
|
|
m_currentLimit[limitIndex] = 0;//Free from violation
|
|
m_currentLimitError[limitIndex] = btScalar(0.f);
|
|
return 0;
|
|
}
|
|
|
|
|
|
|
|
btScalar btTranslationalLimitMotor::solveLinearAxis(
|
|
btScalar timeStep,
|
|
btScalar jacDiagABInv,
|
|
btRigidBody& body1,btSolverBody& bodyA,const btVector3 &pointInA,
|
|
btRigidBody& body2,btSolverBody& bodyB,const btVector3 &pointInB,
|
|
int limit_index,
|
|
const btVector3 & axis_normal_on_a,
|
|
const btVector3 & anchorPos)
|
|
{
|
|
|
|
///find relative velocity
|
|
// btVector3 rel_pos1 = pointInA - body1.getCenterOfMassPosition();
|
|
// btVector3 rel_pos2 = pointInB - body2.getCenterOfMassPosition();
|
|
btVector3 rel_pos1 = anchorPos - body1.getCenterOfMassPosition();
|
|
btVector3 rel_pos2 = anchorPos - body2.getCenterOfMassPosition();
|
|
|
|
btVector3 vel1;
|
|
bodyA.getVelocityInLocalPointObsolete(rel_pos1,vel1);
|
|
btVector3 vel2;
|
|
bodyB.getVelocityInLocalPointObsolete(rel_pos2,vel2);
|
|
btVector3 vel = vel1 - vel2;
|
|
|
|
btScalar rel_vel = axis_normal_on_a.dot(vel);
|
|
|
|
|
|
|
|
/// apply displacement correction
|
|
|
|
//positional error (zeroth order error)
|
|
btScalar depth = -(pointInA - pointInB).dot(axis_normal_on_a);
|
|
btScalar lo = btScalar(-BT_LARGE_FLOAT);
|
|
btScalar hi = btScalar(BT_LARGE_FLOAT);
|
|
|
|
btScalar minLimit = m_lowerLimit[limit_index];
|
|
btScalar maxLimit = m_upperLimit[limit_index];
|
|
|
|
//handle the limits
|
|
if (minLimit < maxLimit)
|
|
{
|
|
{
|
|
if (depth > maxLimit)
|
|
{
|
|
depth -= maxLimit;
|
|
lo = btScalar(0.);
|
|
|
|
}
|
|
else
|
|
{
|
|
if (depth < minLimit)
|
|
{
|
|
depth -= minLimit;
|
|
hi = btScalar(0.);
|
|
}
|
|
else
|
|
{
|
|
return 0.0f;
|
|
}
|
|
}
|
|
}
|
|
}
|
|
|
|
btScalar normalImpulse= m_limitSoftness*(m_restitution*depth/timeStep - m_damping*rel_vel) * jacDiagABInv;
|
|
|
|
|
|
|
|
|
|
btScalar oldNormalImpulse = m_accumulatedImpulse[limit_index];
|
|
btScalar sum = oldNormalImpulse + normalImpulse;
|
|
m_accumulatedImpulse[limit_index] = sum > hi ? btScalar(0.) : sum < lo ? btScalar(0.) : sum;
|
|
normalImpulse = m_accumulatedImpulse[limit_index] - oldNormalImpulse;
|
|
|
|
btVector3 impulse_vector = axis_normal_on_a * normalImpulse;
|
|
//body1.applyImpulse( impulse_vector, rel_pos1);
|
|
//body2.applyImpulse(-impulse_vector, rel_pos2);
|
|
|
|
btVector3 ftorqueAxis1 = rel_pos1.cross(axis_normal_on_a);
|
|
btVector3 ftorqueAxis2 = rel_pos2.cross(axis_normal_on_a);
|
|
bodyA.applyImpulse(axis_normal_on_a*body1.getInvMass(), body1.getInvInertiaTensorWorld()*ftorqueAxis1,normalImpulse);
|
|
bodyB.applyImpulse(axis_normal_on_a*body2.getInvMass(), body2.getInvInertiaTensorWorld()*ftorqueAxis2,-normalImpulse);
|
|
|
|
|
|
|
|
|
|
return normalImpulse;
|
|
}
|
|
|
|
//////////////////////////// btTranslationalLimitMotor ////////////////////////////////////
|
|
|
|
void btGeneric6DofConstraint::calculateAngleInfo()
|
|
{
|
|
btMatrix3x3 relative_frame = m_calculatedTransformA.getBasis().inverse()*m_calculatedTransformB.getBasis();
|
|
matrixToEulerXYZ(relative_frame,m_calculatedAxisAngleDiff);
|
|
// in euler angle mode we do not actually constrain the angular velocity
|
|
// along the axes axis[0] and axis[2] (although we do use axis[1]) :
|
|
//
|
|
// to get constrain w2-w1 along ...not
|
|
// ------ --------------------- ------
|
|
// d(angle[0])/dt = 0 ax[1] x ax[2] ax[0]
|
|
// d(angle[1])/dt = 0 ax[1]
|
|
// d(angle[2])/dt = 0 ax[0] x ax[1] ax[2]
|
|
//
|
|
// constraining w2-w1 along an axis 'a' means that a'*(w2-w1)=0.
|
|
// to prove the result for angle[0], write the expression for angle[0] from
|
|
// GetInfo1 then take the derivative. to prove this for angle[2] it is
|
|
// easier to take the euler rate expression for d(angle[2])/dt with respect
|
|
// to the components of w and set that to 0.
|
|
btVector3 axis0 = m_calculatedTransformB.getBasis().getColumn(0);
|
|
btVector3 axis2 = m_calculatedTransformA.getBasis().getColumn(2);
|
|
|
|
m_calculatedAxis[1] = axis2.cross(axis0);
|
|
m_calculatedAxis[0] = m_calculatedAxis[1].cross(axis2);
|
|
m_calculatedAxis[2] = axis0.cross(m_calculatedAxis[1]);
|
|
|
|
m_calculatedAxis[0].normalize();
|
|
m_calculatedAxis[1].normalize();
|
|
m_calculatedAxis[2].normalize();
|
|
|
|
}
|
|
|
|
|
|
|
|
void btGeneric6DofConstraint::calculateTransforms()
|
|
{
|
|
m_calculatedTransformA = m_rbA.getCenterOfMassTransform() * m_frameInA;
|
|
m_calculatedTransformB = m_rbB.getCenterOfMassTransform() * m_frameInB;
|
|
calculateLinearInfo();
|
|
calculateAngleInfo();
|
|
}
|
|
|
|
|
|
|
|
void btGeneric6DofConstraint::buildLinearJacobian(
|
|
btJacobianEntry & jacLinear,const btVector3 & normalWorld,
|
|
const btVector3 & pivotAInW,const btVector3 & pivotBInW)
|
|
{
|
|
new (&jacLinear) btJacobianEntry(
|
|
m_rbA.getCenterOfMassTransform().getBasis().transpose(),
|
|
m_rbB.getCenterOfMassTransform().getBasis().transpose(),
|
|
pivotAInW - m_rbA.getCenterOfMassPosition(),
|
|
pivotBInW - m_rbB.getCenterOfMassPosition(),
|
|
normalWorld,
|
|
m_rbA.getInvInertiaDiagLocal(),
|
|
m_rbA.getInvMass(),
|
|
m_rbB.getInvInertiaDiagLocal(),
|
|
m_rbB.getInvMass());
|
|
}
|
|
|
|
|
|
|
|
void btGeneric6DofConstraint::buildAngularJacobian(
|
|
btJacobianEntry & jacAngular,const btVector3 & jointAxisW)
|
|
{
|
|
new (&jacAngular) btJacobianEntry(jointAxisW,
|
|
m_rbA.getCenterOfMassTransform().getBasis().transpose(),
|
|
m_rbB.getCenterOfMassTransform().getBasis().transpose(),
|
|
m_rbA.getInvInertiaDiagLocal(),
|
|
m_rbB.getInvInertiaDiagLocal());
|
|
|
|
}
|
|
|
|
|
|
|
|
bool btGeneric6DofConstraint::testAngularLimitMotor(int axis_index)
|
|
{
|
|
btScalar angle = m_calculatedAxisAngleDiff[axis_index];
|
|
m_angularLimits[axis_index].m_currentPosition = angle;
|
|
//test limits
|
|
m_angularLimits[axis_index].testLimitValue(angle);
|
|
return m_angularLimits[axis_index].needApplyTorques();
|
|
}
|
|
|
|
|
|
|
|
void btGeneric6DofConstraint::buildJacobian()
|
|
{
|
|
if (m_useSolveConstraintObsolete)
|
|
{
|
|
|
|
// Clear accumulated impulses for the next simulation step
|
|
m_linearLimits.m_accumulatedImpulse.setValue(btScalar(0.), btScalar(0.), btScalar(0.));
|
|
int i;
|
|
for(i = 0; i < 3; i++)
|
|
{
|
|
m_angularLimits[i].m_accumulatedImpulse = btScalar(0.);
|
|
}
|
|
//calculates transform
|
|
calculateTransforms();
|
|
|
|
// const btVector3& pivotAInW = m_calculatedTransformA.getOrigin();
|
|
// const btVector3& pivotBInW = m_calculatedTransformB.getOrigin();
|
|
calcAnchorPos();
|
|
btVector3 pivotAInW = m_AnchorPos;
|
|
btVector3 pivotBInW = m_AnchorPos;
|
|
|
|
// not used here
|
|
// btVector3 rel_pos1 = pivotAInW - m_rbA.getCenterOfMassPosition();
|
|
// btVector3 rel_pos2 = pivotBInW - m_rbB.getCenterOfMassPosition();
|
|
|
|
btVector3 normalWorld;
|
|
//linear part
|
|
for (i=0;i<3;i++)
|
|
{
|
|
if (m_linearLimits.isLimited(i))
|
|
{
|
|
if (m_useLinearReferenceFrameA)
|
|
normalWorld = m_calculatedTransformA.getBasis().getColumn(i);
|
|
else
|
|
normalWorld = m_calculatedTransformB.getBasis().getColumn(i);
|
|
|
|
buildLinearJacobian(
|
|
m_jacLinear[i],normalWorld ,
|
|
pivotAInW,pivotBInW);
|
|
|
|
}
|
|
}
|
|
|
|
// angular part
|
|
for (i=0;i<3;i++)
|
|
{
|
|
//calculates error angle
|
|
if (testAngularLimitMotor(i))
|
|
{
|
|
normalWorld = this->getAxis(i);
|
|
// Create angular atom
|
|
buildAngularJacobian(m_jacAng[i],normalWorld);
|
|
}
|
|
}
|
|
|
|
}
|
|
}
|
|
|
|
|
|
|
|
void btGeneric6DofConstraint::getInfo1 (btConstraintInfo1* info)
|
|
{
|
|
if (m_useSolveConstraintObsolete)
|
|
{
|
|
info->m_numConstraintRows = 0;
|
|
info->nub = 0;
|
|
} else
|
|
{
|
|
//prepare constraint
|
|
calculateTransforms();
|
|
info->m_numConstraintRows = 0;
|
|
info->nub = 6;
|
|
int i;
|
|
//test linear limits
|
|
for(i = 0; i < 3; i++)
|
|
{
|
|
if(m_linearLimits.needApplyForce(i))
|
|
{
|
|
info->m_numConstraintRows++;
|
|
info->nub--;
|
|
}
|
|
}
|
|
//test angular limits
|
|
for (i=0;i<3 ;i++ )
|
|
{
|
|
if(testAngularLimitMotor(i))
|
|
{
|
|
info->m_numConstraintRows++;
|
|
info->nub--;
|
|
}
|
|
}
|
|
}
|
|
}
|
|
|
|
|
|
|
|
void btGeneric6DofConstraint::getInfo2 (btConstraintInfo2* info)
|
|
{
|
|
btAssert(!m_useSolveConstraintObsolete);
|
|
int row = setLinearLimits(info);
|
|
setAngularLimits(info, row);
|
|
}
|
|
|
|
|
|
|
|
int btGeneric6DofConstraint::setLinearLimits(btConstraintInfo2* info)
|
|
{
|
|
btGeneric6DofConstraint * d6constraint = this;
|
|
int row = 0;
|
|
//solve linear limits
|
|
btRotationalLimitMotor limot;
|
|
for (int i=0;i<3 ;i++ )
|
|
{
|
|
if(m_linearLimits.needApplyForce(i))
|
|
{ // re-use rotational motor code
|
|
limot.m_bounce = btScalar(0.f);
|
|
limot.m_currentLimit = m_linearLimits.m_currentLimit[i];
|
|
limot.m_currentPosition = m_linearLimits.m_currentLinearDiff[i];
|
|
limot.m_currentLimitError = m_linearLimits.m_currentLimitError[i];
|
|
limot.m_damping = m_linearLimits.m_damping;
|
|
limot.m_enableMotor = m_linearLimits.m_enableMotor[i];
|
|
limot.m_ERP = m_linearLimits.m_restitution;
|
|
limot.m_hiLimit = m_linearLimits.m_upperLimit[i];
|
|
limot.m_limitSoftness = m_linearLimits.m_limitSoftness;
|
|
limot.m_loLimit = m_linearLimits.m_lowerLimit[i];
|
|
limot.m_maxLimitForce = btScalar(0.f);
|
|
limot.m_maxMotorForce = m_linearLimits.m_maxMotorForce[i];
|
|
limot.m_targetVelocity = m_linearLimits.m_targetVelocity[i];
|
|
btVector3 axis = m_calculatedTransformA.getBasis().getColumn(i);
|
|
row += get_limit_motor_info2(&limot, &m_rbA, &m_rbB, info, row, axis, 0);
|
|
}
|
|
}
|
|
return row;
|
|
}
|
|
|
|
|
|
|
|
int btGeneric6DofConstraint::setAngularLimits(btConstraintInfo2 *info, int row_offset)
|
|
{
|
|
btGeneric6DofConstraint * d6constraint = this;
|
|
int row = row_offset;
|
|
//solve angular limits
|
|
for (int i=0;i<3 ;i++ )
|
|
{
|
|
if(d6constraint->getRotationalLimitMotor(i)->needApplyTorques())
|
|
{
|
|
btVector3 axis = d6constraint->getAxis(i);
|
|
row += get_limit_motor_info2(
|
|
d6constraint->getRotationalLimitMotor(i),
|
|
&m_rbA,
|
|
&m_rbB,
|
|
info,row,axis,1);
|
|
}
|
|
}
|
|
|
|
return row;
|
|
}
|
|
|
|
|
|
|
|
void btGeneric6DofConstraint::solveConstraintObsolete(btSolverBody& bodyA,btSolverBody& bodyB,btScalar timeStep)
|
|
{
|
|
if (m_useSolveConstraintObsolete)
|
|
{
|
|
|
|
|
|
m_timeStep = timeStep;
|
|
|
|
//calculateTransforms();
|
|
|
|
int i;
|
|
|
|
// linear
|
|
|
|
btVector3 pointInA = m_calculatedTransformA.getOrigin();
|
|
btVector3 pointInB = m_calculatedTransformB.getOrigin();
|
|
|
|
btScalar jacDiagABInv;
|
|
btVector3 linear_axis;
|
|
for (i=0;i<3;i++)
|
|
{
|
|
if (m_linearLimits.isLimited(i))
|
|
{
|
|
jacDiagABInv = btScalar(1.) / m_jacLinear[i].getDiagonal();
|
|
|
|
if (m_useLinearReferenceFrameA)
|
|
linear_axis = m_calculatedTransformA.getBasis().getColumn(i);
|
|
else
|
|
linear_axis = m_calculatedTransformB.getBasis().getColumn(i);
|
|
|
|
m_linearLimits.solveLinearAxis(
|
|
m_timeStep,
|
|
jacDiagABInv,
|
|
m_rbA,bodyA,pointInA,
|
|
m_rbB,bodyB,pointInB,
|
|
i,linear_axis, m_AnchorPos);
|
|
|
|
}
|
|
}
|
|
|
|
// angular
|
|
btVector3 angular_axis;
|
|
btScalar angularJacDiagABInv;
|
|
for (i=0;i<3;i++)
|
|
{
|
|
if (m_angularLimits[i].needApplyTorques())
|
|
{
|
|
|
|
// get axis
|
|
angular_axis = getAxis(i);
|
|
|
|
angularJacDiagABInv = btScalar(1.) / m_jacAng[i].getDiagonal();
|
|
|
|
m_angularLimits[i].solveAngularLimits(m_timeStep,angular_axis,angularJacDiagABInv, &m_rbA,bodyA,&m_rbB,bodyB);
|
|
}
|
|
}
|
|
}
|
|
}
|
|
|
|
|
|
|
|
void btGeneric6DofConstraint::updateRHS(btScalar timeStep)
|
|
{
|
|
(void)timeStep;
|
|
|
|
}
|
|
|
|
|
|
|
|
btVector3 btGeneric6DofConstraint::getAxis(int axis_index) const
|
|
{
|
|
return m_calculatedAxis[axis_index];
|
|
}
|
|
|
|
|
|
btScalar btGeneric6DofConstraint::getRelativePivotPosition(int axisIndex) const
|
|
{
|
|
return m_calculatedLinearDiff[axisIndex];
|
|
}
|
|
|
|
|
|
btScalar btGeneric6DofConstraint::getAngle(int axisIndex) const
|
|
{
|
|
return m_calculatedAxisAngleDiff[axisIndex];
|
|
}
|
|
|
|
|
|
|
|
void btGeneric6DofConstraint::calcAnchorPos(void)
|
|
{
|
|
btScalar imA = m_rbA.getInvMass();
|
|
btScalar imB = m_rbB.getInvMass();
|
|
btScalar weight;
|
|
if(imB == btScalar(0.0))
|
|
{
|
|
weight = btScalar(1.0);
|
|
}
|
|
else
|
|
{
|
|
weight = imA / (imA + imB);
|
|
}
|
|
const btVector3& pA = m_calculatedTransformA.getOrigin();
|
|
const btVector3& pB = m_calculatedTransformB.getOrigin();
|
|
m_AnchorPos = pA * weight + pB * (btScalar(1.0) - weight);
|
|
return;
|
|
}
|
|
|
|
|
|
|
|
void btGeneric6DofConstraint::calculateLinearInfo()
|
|
{
|
|
m_calculatedLinearDiff = m_calculatedTransformB.getOrigin() - m_calculatedTransformA.getOrigin();
|
|
m_calculatedLinearDiff = m_calculatedTransformA.getBasis().inverse() * m_calculatedLinearDiff;
|
|
for(int i = 0; i < 3; i++)
|
|
{
|
|
m_linearLimits.m_currentLinearDiff[i] = m_calculatedLinearDiff[i];
|
|
m_linearLimits.testLimitValue(i, m_calculatedLinearDiff[i]);
|
|
}
|
|
}
|
|
|
|
|
|
|
|
int btGeneric6DofConstraint::get_limit_motor_info2(
|
|
btRotationalLimitMotor * limot,
|
|
btRigidBody * body0, btRigidBody * body1,
|
|
btConstraintInfo2 *info, int row, btVector3& ax1, int rotational)
|
|
{
|
|
int srow = row * info->rowskip;
|
|
int powered = limot->m_enableMotor;
|
|
int limit = limot->m_currentLimit;
|
|
if (powered || limit)
|
|
{ // if the joint is powered, or has joint limits, add in the extra row
|
|
btScalar *J1 = rotational ? info->m_J1angularAxis : info->m_J1linearAxis;
|
|
btScalar *J2 = rotational ? info->m_J2angularAxis : 0;
|
|
J1[srow+0] = ax1[0];
|
|
J1[srow+1] = ax1[1];
|
|
J1[srow+2] = ax1[2];
|
|
if(rotational)
|
|
{
|
|
J2[srow+0] = -ax1[0];
|
|
J2[srow+1] = -ax1[1];
|
|
J2[srow+2] = -ax1[2];
|
|
}
|
|
if((!rotational))
|
|
{
|
|
btVector3 ltd; // Linear Torque Decoupling vector
|
|
btVector3 c = m_calculatedTransformB.getOrigin() - body0->getCenterOfMassPosition();
|
|
ltd = c.cross(ax1);
|
|
info->m_J1angularAxis[srow+0] = ltd[0];
|
|
info->m_J1angularAxis[srow+1] = ltd[1];
|
|
info->m_J1angularAxis[srow+2] = ltd[2];
|
|
|
|
c = m_calculatedTransformB.getOrigin() - body1->getCenterOfMassPosition();
|
|
ltd = -c.cross(ax1);
|
|
info->m_J2angularAxis[srow+0] = ltd[0];
|
|
info->m_J2angularAxis[srow+1] = ltd[1];
|
|
info->m_J2angularAxis[srow+2] = ltd[2];
|
|
}
|
|
// if we're limited low and high simultaneously, the joint motor is
|
|
// ineffective
|
|
if (limit && (limot->m_loLimit == limot->m_hiLimit)) powered = 0;
|
|
info->m_constraintError[srow] = btScalar(0.f);
|
|
if (powered)
|
|
{
|
|
info->cfm[srow] = 0.0f;
|
|
if(!limit)
|
|
{
|
|
btScalar tag_vel = rotational ? limot->m_targetVelocity : -limot->m_targetVelocity;
|
|
|
|
btScalar mot_fact = getMotorFactor( limot->m_currentPosition,
|
|
limot->m_loLimit,
|
|
limot->m_hiLimit,
|
|
tag_vel,
|
|
info->fps * info->erp);
|
|
info->m_constraintError[srow] += mot_fact * limot->m_targetVelocity;
|
|
info->m_lowerLimit[srow] = -limot->m_maxMotorForce;
|
|
info->m_upperLimit[srow] = limot->m_maxMotorForce;
|
|
}
|
|
}
|
|
if(limit)
|
|
{
|
|
btScalar k = info->fps * limot->m_ERP;
|
|
if(!rotational)
|
|
{
|
|
info->m_constraintError[srow] += k * limot->m_currentLimitError;
|
|
}
|
|
else
|
|
{
|
|
info->m_constraintError[srow] += -k * limot->m_currentLimitError;
|
|
}
|
|
info->cfm[srow] = 0.0f;
|
|
if (limot->m_loLimit == limot->m_hiLimit)
|
|
{ // limited low and high simultaneously
|
|
info->m_lowerLimit[srow] = -SIMD_INFINITY;
|
|
info->m_upperLimit[srow] = SIMD_INFINITY;
|
|
}
|
|
else
|
|
{
|
|
if (limit == 1)
|
|
{
|
|
info->m_lowerLimit[srow] = 0;
|
|
info->m_upperLimit[srow] = SIMD_INFINITY;
|
|
}
|
|
else
|
|
{
|
|
info->m_lowerLimit[srow] = -SIMD_INFINITY;
|
|
info->m_upperLimit[srow] = 0;
|
|
}
|
|
// deal with bounce
|
|
if (limot->m_bounce > 0)
|
|
{
|
|
// calculate joint velocity
|
|
btScalar vel;
|
|
if (rotational)
|
|
{
|
|
vel = body0->getAngularVelocity().dot(ax1);
|
|
if (body1)
|
|
vel -= body1->getAngularVelocity().dot(ax1);
|
|
}
|
|
else
|
|
{
|
|
vel = body0->getLinearVelocity().dot(ax1);
|
|
if (body1)
|
|
vel -= body1->getLinearVelocity().dot(ax1);
|
|
}
|
|
// only apply bounce if the velocity is incoming, and if the
|
|
// resulting c[] exceeds what we already have.
|
|
if (limit == 1)
|
|
{
|
|
if (vel < 0)
|
|
{
|
|
btScalar newc = -limot->m_bounce* vel;
|
|
if (newc > info->m_constraintError[srow])
|
|
info->m_constraintError[srow] = newc;
|
|
}
|
|
}
|
|
else
|
|
{
|
|
if (vel > 0)
|
|
{
|
|
btScalar newc = -limot->m_bounce * vel;
|
|
if (newc < info->m_constraintError[srow])
|
|
info->m_constraintError[srow] = newc;
|
|
}
|
|
}
|
|
}
|
|
}
|
|
}
|
|
return 1;
|
|
}
|
|
else return 0;
|
|
}
|
|
|
|
|
|
|
|
|
|
btGeneric6DofSpringConstraint::btGeneric6DofSpringConstraint(btRigidBody& rbA, btRigidBody& rbB, const btTransform& frameInA, const btTransform& frameInB ,bool useLinearReferenceFrameA)
|
|
: btGeneric6DofConstraint(rbA, rbB, frameInA, frameInB, useLinearReferenceFrameA)
|
|
{
|
|
for(int i = 0; i < 6; i++)
|
|
{
|
|
m_springEnabled[i] = false;
|
|
m_equilibriumPoint[i] = btScalar(0.f);
|
|
m_springStiffness[i] = btScalar(0.f);
|
|
m_springDamping[i] = btScalar(1.f);
|
|
}
|
|
}
|
|
|
|
|
|
void btGeneric6DofSpringConstraint::enableSpring(int index, bool onOff)
|
|
{
|
|
btAssert((index >= 0) && (index < 6));
|
|
m_springEnabled[index] = onOff;
|
|
if(index < 3)
|
|
{
|
|
m_linearLimits.m_enableMotor[index] = onOff;
|
|
}
|
|
else
|
|
{
|
|
m_angularLimits[index - 3].m_enableMotor = onOff;
|
|
}
|
|
}
|
|
|
|
|
|
|
|
void btGeneric6DofSpringConstraint::setStiffness(int index, btScalar stiffness)
|
|
{
|
|
btAssert((index >= 0) && (index < 6));
|
|
m_springStiffness[index] = stiffness;
|
|
}
|
|
|
|
|
|
void btGeneric6DofSpringConstraint::setDamping(int index, btScalar damping)
|
|
{
|
|
btAssert((index >= 0) && (index < 6));
|
|
m_springDamping[index] = damping;
|
|
}
|
|
|
|
|
|
void btGeneric6DofSpringConstraint::setEquilibriumPoint()
|
|
{
|
|
calculateTransforms();
|
|
for(int i = 0; i < 3; i++)
|
|
{
|
|
m_equilibriumPoint[i] = m_calculatedLinearDiff[i];
|
|
}
|
|
for(int i = 0; i < 3; i++)
|
|
{
|
|
m_equilibriumPoint[i + 3] = m_calculatedAxisAngleDiff[i];
|
|
}
|
|
}
|
|
|
|
|
|
|
|
void btGeneric6DofSpringConstraint::setEquilibriumPoint(int index)
|
|
{
|
|
btAssert((index >= 0) && (index < 6));
|
|
calculateTransforms();
|
|
if(index < 3)
|
|
{
|
|
m_equilibriumPoint[index] = m_calculatedLinearDiff[index];
|
|
}
|
|
else
|
|
{
|
|
m_equilibriumPoint[index + 3] = m_calculatedAxisAngleDiff[index];
|
|
}
|
|
}
|
|
|
|
|
|
|
|
void btGeneric6DofSpringConstraint::internalUpdateSprings(btConstraintInfo2* info)
|
|
{
|
|
calculateTransforms();
|
|
// it is assumed that calculateTransforms() have been called before this call
|
|
int i;
|
|
btVector3 relVel = m_rbB.getLinearVelocity() - m_rbA.getLinearVelocity();
|
|
for(i = 0; i < 3; i++)
|
|
{
|
|
if(m_springEnabled[i])
|
|
{
|
|
// get current position of constraint
|
|
btScalar currPos = m_calculatedLinearDiff[i];
|
|
// calculate difference
|
|
btScalar delta = currPos - m_equilibriumPoint[i];
|
|
// spring force is (delta * m_stiffness) according to Hooke's Law
|
|
btScalar force = delta * m_springStiffness[i];
|
|
btScalar velFactor = info->fps * m_springDamping[i];
|
|
m_linearLimits.m_targetVelocity[i] = velFactor * force;
|
|
m_linearLimits.m_maxMotorForce[i] = btFabs(force) / info->fps;
|
|
}
|
|
}
|
|
for(i = 0; i < 3; i++)
|
|
{
|
|
if(m_springEnabled[i + 3])
|
|
{
|
|
// get current position of constraint
|
|
btScalar currPos = m_calculatedAxisAngleDiff[i];
|
|
// calculate difference
|
|
btScalar delta = currPos - m_equilibriumPoint[i+3];
|
|
// spring force is (-delta * m_stiffness) according to Hooke's Law
|
|
btScalar force = -delta * m_springStiffness[i+3];
|
|
btScalar velFactor = info->fps * m_springDamping[i+3];
|
|
m_angularLimits[i].m_targetVelocity = velFactor * force;
|
|
m_angularLimits[i].m_maxMotorForce = btFabs(force) / info->fps;
|
|
}
|
|
}
|
|
}
|
|
|
|
|
|
void btGeneric6DofSpringConstraint::getInfo2(btConstraintInfo2* info)
|
|
{
|
|
// this will be called by constraint solver at the constraint setup stage
|
|
// set current motor parameters
|
|
internalUpdateSprings(info);
|
|
// do the rest of job for constraint setup
|
|
btGeneric6DofConstraint::getInfo2(info);
|
|
}
|
|
|
|
|
|
|
|
|